TCP对准功能的实现

显示全部楼层 · 2024-9-25 13:52:03

大家一起支持原创，推动机器人使用和发展

本公众号对各类ABB机器人应用，仿真，毕业设计提供技术支持，详细后台留言

本公众号诚挚希望与各机器人培训机构，机器人使用单元合作，提供技术支持，详细后台留言

在机器人示教器手动操纵界面，有一个“对准”按钮（所有版本RW均有）。其功能是，可以让机器人当前的TCP可以快速/就近和一个坐标系对齐（通常就是TCP的z和坐标系的z平行），这样能快速调整姿态。

在3D视觉引导等项目中，目标点为3D相机给出。为避免路径上奇异或者机器人旋转过大导致线缆缠绕，放置/抓取产品目标点的前几个准备点，姿态的z最好能先和坐标系的z对齐，减少过程中姿态的调整（前置点如果姿态和目标点完全一致，有时候会导致不可达）。

示教器只提供了手动模式“对准”的功能，未提供“对准”函数让用户使用。

本文研究并实现了“对准”功能，测试结果与示教器的“对准”功能一致（本文仅实现了目标点的z方向就近对准参考坐标系的z+/z-负方向，实际示教器的“对准”还有目标点的z就近对准xy平面等，留待读者自行研究）

“对准”功能，本质就是将上左图的坐标系A的z方向，要和坐标系B的z方向一致（或者相反），最终坐标系A的xy平面与坐标系B的xy平面平行（坐标系A的x无需和坐标系B的x一致）。

坐标系A的z方向（可以将坐标系A的姿态数据转化为旋转矩阵，最后一列即为z方向的单位向量）实质为一个单位向量，坐标系B的z方向也为一个单位向量。可以利用向量的点乘，得到单位向量Az和单位向量Bz的夹角。

利用单位向量Az 叉乘单位向量Bz，可以得到垂直于平面AzOBz的向量cross，也就是向量Az绕着cross旋转了Θ°后，到达了Bz。

以上这段也就是轴角（旋转矢量）的定义。关于旋转矢量，可以参考以下链接：

空间姿态四元数几何意义及与旋转向量转换

也就是坐标系A经过变换，坐标系A的z与坐标系B的z对齐。

CONSTrobtarget p105:=[[825.0002,1.074095E-06,987.0695],[0.06004369,0.4615303,-0.8591881,-0.2125564],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];! p105为示教点CONST robtarget p106:=[[825.0001,3.335122E-30,987.0701],[2.208061E-08,-0.4732175,0.8809456,-1.186104E-08],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];! p106为使用示教器，将p105对准后记录的点PERS robtarget pnew:=[[824.773,-0.0204436,986.792],[0.637121,-0.306924,0.636816,-0.307147],[-1,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];! pnew为计算出的对准点
PROC test90()    pnew:=AlignZ(p105,[[0,0,0],[1,0,0,0]]);    ! 此处测试对准base坐标系，四元数为[1,0,0,0]，若参考坐标系的z朝下，可以是[0,0,1,0]    MoveJ p105,v100,fine,tool0\WObj:=wobj0;    MoveJ p106,v100,fine,tool0\WObj:=wobj0;    ! 移动到示教器"对准"功能后的点    MoveJ p105,v100,fine,tool0\WObj:=wobj0;    WaitTime 1;    MoveJ pnew,v100,fine,tool0\WObj:=wobj0;    ! 移动到计算后的对准点，结果与p106一致  ENDPROC
  FUNC robtarget AlignZ(robtarget p,pose frame)      ! 将传入的p的z方向和frame的z方向对齐      ! 先计算获得p的旋转矩阵，得到姿态中的z方向的向量（旋转矩阵最后一列）      ! 再计算frame的旋转矩阵，得到姿态中的z方向的向量（旋转矩阵最后一列）      VAR num rotmat_p{3,3};      !p的旋转矩阵      VAR num rotmat_frame{3,3};      !frame的旋转矩阵
      VAR pos v_p_z;      !p点的旋转矩阵的z向量      VAR pos v_frame_z;      !要对齐的frame坐标系的旋转矩阵的z向量      VAR pos v_cross_vec;      ! v_p_z叉乘 v_frame_z 得到的向量，即向量v_p_z绕着v_cross_vec旋转theta，到达v_frame_z向量      VAR num mag;      VAR num pdot;      VAR num theta;      VAR orient o_delta;      VAR robtarget pout;      pout:=p;      QuatToRotMatrix p.rot,rotmat_p;      QuatToRotMatrix frame.rot,rotmat_frame;
      v_p_z:=[rotmat_p{1,3},rotmat_p{2,3},rotmat_p{3,3}];      !p点的旋转矩阵的z向量      v_frame_z:=[rotmat_frame{1,3},rotmat_frame{2,3},rotmat_frame{3,3}];      !要对齐的frame坐标系的旋转矩阵的z向量
      pdot:=DotProd(v_p_z,v_frame_z);      ! a*b = |a|*|b|*cos(theta)      ! 由于是两个单位向量相乘，|a|,|b|都等于1      theta:=ACos(pdot);      ! 计算得到v_p_z 旋转到 v_frame_z的角度      IF theta>90 THEN          theta:=theta-180;          !就近对齐，即如果要旋转大于90后和目标z方向一致，则就和目标z的负方向对齐      ENDIF
      v_cross_vec:=CrossProd(v_p_z,v_frame_z);      ! 计算v_p_z 叉乘v_frame_z,得到向量v_cross_vec，      ! 是v_p_z旋转到v_frame_z时参考（绕）的向量方向      mag:=VectMagn(v_cross_vec);      v_cross_vec.x:=v_cross_vec.x/mag;      v_cross_vec.y:=v_cross_vec.y/mag;      v_cross_vec.z:=v_cross_vec.z/mag;      ! 单位化
      ! 以上步骤，计算得到v_p_z，绕着v_cross_vec向量，旋转theta角度后，和v_frame_z向量一致或者方向相反      ! 根据轴角（旋转矢量定义），得到对应的四元数      o_delta.q1:=Cos(theta/2);      o_delta.q2:=Sin(theta/2)*v_cross_vec.x;      o_delta.q3:=Sin(theta/2)*v_cross_vec.y;      o_delta.q4:=Sin(theta/2)*v_cross_vec.z;      pout.rot:=o_delta*pout.rot;      ! 得到p的旋转矩阵中的z方向和目标z方向对齐后的姿态数据      RETURN pout;  ENDFUNC    ! 将四元数转换为旋转矩阵的函数  PROC QuatToRotMatrix(orient o,INOUT num m{*,*})      VAR num x2;      VAR num y2;      VAR num z2;      VAR num xy;      VAR num xz;      VAR num yz;      VAR num wx;      VAR num wy;      VAR num wz;
      ! 计算四元数的平方项和乘积项      x2:=o.q2*o.q2;      y2:=o.q3*o.q3;      z2:=o.q4*o.q4;      xy:=o.q2*o.q3;      xz:=o.q2*o.q4;      yz:=o.q3*o.q4;      wx:=o.q1*o.q2;      wy:=o.q1*o.q3;      wz:=o.q1*o.q4;
      ! 填充旋转矩阵      m{1,1}:=1.0-2.0*(y2+z2);      m{1,2}:=2.0*(xy-wz);      m{1,3}:=2.0*(xz+wy);      m{2,1}:=2.0*(xy+wz);      m{2,2}:=1.0-2.0*(x2+z2);      m{2,3}:=2.0*(yz-wx);      m{3,1}:=2.0*(xz-wy);      m{3,2}:=2.0*(yz+wx);      m{3,3}:=1.0-2.0*(x2+y2);  ENDPROC

********************************

如何获取更多经典文章？

关注公众号 ABB机器人实战技巧，点击页面底部的

TCP对准功能的实现

本帖子中包含更多资源