【ABB】四点计算工具TCP实现代码

1.TCP（toolcenter point）机器人工具中心点。该坐标系非常重要，ABB机器人通常走点位都以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