1.TCP(工具中心点)机器人工具中心点。坐标系是非常重要的。ABB机器人通常在TCP的基础上行走。
 2. ABB可以通过四点法TCP（缺省方向）进行计算，使用该方法如下所示。

 三。ABB机器人提供MoToCTCPCALIP1 P1、P2、P3、P4、Toe1、马克斯Err、平均Err；计算TCP的命令，其中P1、P2、P3、P4是机器人教学中四个点的共同目标（六轴坐标值），并将计算结果分配给工具1的坐标系。
 4．计算TCP，通常使用矩阵运算，并结合*小二乘法得到TCP的XYZ和相应的误差。
 文章描述了通过四点法自动计算TCP。

 6。如上图所示，四点教学结束后，四点形成球面，球面中心为当前TCP所在的wobj0下的坐标值XYZ。
 7.因此，TCP按四点计算，即四点对应的球心由四点计算。
 8.创建了四点球中心计算功能，返回值POS类型（即，球体XYZ的中心）
 Func POS cal ball center (robot target P1, robot target P2, robot target P3, robot target 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. Transmit X;
 X4 / p4.trans.x;
 Y1: = p1.trans.y;
 Y2: = P2. Transaction y;
 Y3 / p3.trans.y;
 Y4: = p4.trans.y;
 Z1: = P1. Transmission Z;
 Z2 / p2.trans.z;
 Z3: = p3.trans.z;
 Z4: = P4. Transmit 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 y _ 2 / y _ 1 / y _ 1, z _ 2 / z _ 1 / z _ 1, z _ 2 / z _ 2 / z _ 1, z _ 2 / z _ 2 / z _ 1;
 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*b3a12*b2*a31b1*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： 机器人工具0的当前位置（XYZ，Q1-Q4）
 X：工具TCP姿态(xyz，q1-q4)
 B：在机器人工具的当前姿态的当前位置表示（XYZ，Q1-Q4）
 则X=A-1 B
 根据上述公式，创建代码如下：
 PROCCAL_TCP_POS (RoboTargetP1,RoboTargetP2,RoboTargetP3,RoboTargetP4,INOUT tool data tool 1)
 VAR POS tool_pos;
 Variable pose pose1: = [[0,0,0], [1,0,0,0]];
 Tool _ location: = CAL_Ball_Center (P1, P2, P3, P4);
 pose1: = [tool_pos, [1,0,0,0]];
 pose1：=posemult（poseinv（[p4.trans，p4.rot]），pose1）；
 Tool 1.tframe:=Position 1;
 tool1.tframe.rot: = [1,0,0,0];
 ENDPROC