1.3.4 工件坐标系计算原理及实现

1.3.3节介绍了如何使用DefFrame函数计算获得坐标系。在ABB工业机器人的示教器中定义坐标系时，默认使用的是“DefFrame（p₁，p₂，p₃\Origin：=3）”的函数，即坐标系原点在点p₃到p₁p₂连线的垂足处。

DefFrame函数返回的是pose类型的数据（位姿数据），即在计算时，要计算坐标系的原点位置（x、y、z）和坐标系姿态。

假设空间3点为p₁（x₁，y₁，z₁）、p₂（x₂，y₂，z₂）和p₃（x₃，y₃，z₃）。由点p₃向p₁p₂连线做垂线，垂足为点p₄（x，y，z），p₄即为需要求解的坐标系原点（见图1-45）。

把经过点p₁和点p₂的空间直线称为L₁，可以用式（1-31）表示：

由于向量垂直于，根据向量点积规则，。

为方便书写，记：

将式（1-31）和式（1-32）整理为式（1-33）：

计算式（1-33）即可得到由点p₁、p₂和p₃构成的坐标系原点。RAPID提供求解AX=B的指令“MatrixSolve A，B，X”，计算结果存储于X数组中。MatrixSolve中使用的数据类型为dnum，在调用时可以使用NumToDnum和DnumToNum函数进行转化。

对于坐标系原点姿态，则可以通过向量的叉乘得到，如图1-46及图1-47所示。

计算过程如下：

（1）向量单位化，得到单位向量；

（2）向量单位化，得到单位向量；

（3）向量叉乘向量，得到单位向量；

（4）单位向量等于单位向量叉乘单位向量；

（5）得到单位向量、单位向量、单位向量；

（6）将旋转矩阵转化为欧拉角；

（7）利用OrientZYX函数将欧拉角转化为四元数。

综合以上计算坐标系原点与姿态的介绍，可以编写RAPID代码如下，运算结果与DefFrame（p1，p2，p3\Origin：=3）相同：