1.2.3 TCP计算原理及实现

1.2.2节中介绍了RAPID计算TCP的MToolTCPCalib指令，那么TCP数据到底是如何计算得到的？

同一个TCP相对于tool₀的位姿是不变的。假设工具末端移动到固定尖点时法兰盘（tool₀）的位姿是p₁₀（数据类型为pose）、TCP数据是tool₁（数据类型为pose），如图1-26所示。此时工具末端对应的空间绝对位姿p₁₀₀可以用式（1-15）表示：

同理，假设工具末端以第二、第三、第四种姿势接近固定尖点时tool₀的位姿分别是p₂₀、p₃₀、p₄₀，工具末端对应的空间绝对位姿分别是p₂₀₀、p₃₀₀和p₄₀₀，则可以用式（1-16）～式（1-18）表示：

整理式（1-15）中的虚线框部分：

令为R₁，令为tool，令，式（1-15）虚线框部分可以用如下形式表示：

式（1-16）～式（1-18）中的虚线框部分也可用如下形式表示：

由于p₁₀₀、p₂₀₀、p₃₀₀和p₄₀₀四个位姿的空间位置（x、y、z）是一样的，即式（1-15）～式（1-18）中的位置部分（虚线框部分）一致，即式（1-19）～式（1-22）全部相等。整理可得：

对上式再次整理可得：

记R₁-R₂为R₁₂，P₂-P₁为P₂₁，R₂-R₃=R₂₃，P₃-P₂=P₃₂，……可以得到：

上式为标准AX=B形式的超定方程组（约束大于变量数），可以使用最小二乘法进行求解。求解过程如式（1-30）：

根据以上推导，编写RAPID代码实现TCP的位置计算，计算结果与MToolTCPCalib的计算结果比对。式（1-29）为四点法计算表达式，实质对于标准机器人TCP，大于或者等于三点即可计算，点位越多，计算结果越精确（ABB工业机器人示教器定义TCP支持3～10点法）：