QQ
ABB机器人之四点计算工具TCP实现代码
声明：本媒体部分图片、文章来源于网络
版权归原作者所有，如有侵权，请与我联系删除。
1. TCP（toolcenter point）机器人工具中心点。该坐标系非常重要，机器人通常走点位都以TCP为基准。
2. ABB可以通过四点法计算TCP（默认方向），使用方法如下图。
3. ABB机器人提供MToolTCPCalib p1,p2,p3,p4,tool1,max_err,mean_err;指令计算tcp，其中p1,p2,p3,p4为机器人示教时四个点的jointtarget（6个轴坐标值），计算结果赋值到tool1的坐标系
4. 计算TCP，通常使用矩阵运算，并结合*小二乘法得到TCP的xyz，以及对应误差。
5. 本文介绍通过四点自动计算TCP方法。
6. 如上图所示，在完成四点示教后，四点构成一个球面，球心即为当前TCP所在wobj0下的坐标值xyz。
7. 故通过四点计算tcp，即通过四点计算该四点对应的球心。
8. 创建四点计算球心函数，返回值为pos类型（即球心的xyz）
FUNC pos cal_ball_center(robtarget p1,robtarget p2,robtarget p3,robtarget p4) VAR pos pos1; var num x1; var num x2; var num x3; var num x4; var num y1; var num y2; var num y3; var num y4; var num z1; var num z2; var num z3; var num z4; var num a11; var num a12; var num a13; var num a21; var num a22; var num a23; var num a31; var num a32; var num a33; var num b1; var num b2; var num b3; var num d; var num d1; var num d2; var num d3; x1:=p1.trans.x; x2:=p2.trans.x; x3:=p3.trans.x; x4:=p4.trans.x;
y1:=p1.trans.y; y2:=p2.trans.y; y3:=p3.trans.y; y4:=p4.trans.y;
z1:=p1.trans.z; z2:=p2.trans.z; z3:=p3.trans.z; z4:=p4.trans.z;
a11:=2*(x2-x1); a12:=2*(y2-y1); a13:=2*(z2-z1); a21:=2*(x3-x2); a22:=2*(y3-y2); a23:=2*(z3-z2); a31:=2*(x4-x3); a32:=2*(y4-y3); a33:=2*(z4-z3); b1:=x2*x2-x1*x1+y2*y2-y1*y1+z2*z2-z1*z1; b2:=x3*x3-x2*x2+y3*y3-y2*y2+z3*z3-z2*z2; b3:=x4*x4-x3*x3+y4*y4-y3*y3+z4*z4-z3*z3; d:=a11*a22*a33+a12*a23*a31+a13*a21*a32-a11*a23*a32-a12*a21*a33-a13*a22*a31; d1:=b1*a22*a33+a12*a23*b3+a13*b2*a32-b1*a23*a32-a12*b2*a33-a13*a22*b3; d2:=a11*b2*a33+b1*a23*a31+a13*a21*b3-a11*a23*b3-b1*a21*a33-a13*b2*a31; d3:=a11*a22*b3+a12*b2*a31+b1*a21*a32-a11*b2*a32-a12*a21*b3-b1*a22*a31; pos1.x:=d1/d; pos1.y:=d2/d; pos1.z:=d3/d; RETURN pos1;ENDFUNC
9. 创建通过获取球心坐标，计算TCP程序如下
B:=AX；
A：机器人tool0表示的当前位置pose（xyz，q1-q4）
X: 工具tcp pose（xyz，q1-q4）
B：机器人当前工具表示的当前位置pose（xyz，q1-q4）
则X=A-1 B
根据如上公式，创建代码如下：
PROC cal_tcp_pos(robtarget p1,robtarget p2,robtarget p3,robtarget p4,inout tooldata tool1) var pos tool_pos; VAR pose pose1:=[[0,0,0],[1,0,0,0]];
tool_pos:=cal_ball_center(p1,p2,p3,p4); pose1:=[tool_pos,[1,0,0,0]]; pose1:=posemult(poseinv([p4.trans,p4.rot]),pose1); tool1.tframe :=pose1; tool1.tframe .rot:=[1,0,0,0]; ENDPROC 更多资讯:ABB机器人配件