工业机器人工具坐标系（TCF）标定的六点法原理

一、基本步骤

（1）在机器人动作范围内找一个非常精确的固定点作为参考点；
（2）在工具上确定一个参考点（最好是工具中心点Tool Center Point, TCP）;
（3）手动操纵机器人的方法移动TCP，以四种不同的工具姿态与固定点刚好碰上。
  前三个点任意姿态，第四点是用工具的参考点垂直于固定点，第五点是工具参考点从固定点向将要设定的TCP的x方向移动，第六点是工具参考点从固定点向将要设定的TCP的在z方向移动，如下图所示：

（4）通过前4个点的位置数据即可计算出TCP的位置，通过后2个点即可确定TCP的姿态

二、标定过程

1、TCP位置标定

  假设取1、2、3、4四个标定点之间相差90°且不在同一平面上，如下图所示：

  给定如下坐标系定义：

【1】基坐标系(0坐标系)：B
【2】末端坐标系：E
【3】工具坐标系：T

  给定如下变换矩阵定义：

【1】末端坐标系 E 相对于基坐标系 B的变换关系 ：${}_E^{B}T$
【2】工具坐标系T 相对于末端坐标系 E的变换关系 ：${}_T^{E}T$
【3】工具坐标系T 相对于基坐标系 B的变换关系 ：${}_T^{B}T$

  显然可以知道：
$${}_E^{B}T{\cdot }_T^{E}T{=}_T^{B}T\text{\hspace{1em}(1)}$$

  对于选定位置点 i = 1、2、3、4，有：

  【1】${}_E^{B}T$不等，设：
$${}_E^{B}T=\left[\begin{array}{cc}{}_E^B{R}_i\text{EBRi}{}_E^B{R}_i& {}^B{P}_{Ei}\text{BPEi}{}^B{P}_{Ei}\\ 0& 1\end{array}\right]\text{\hspace{1em}(2)}$$
  【2】${}_T^{E}T$不等，但其位置${}^E{P}_T$相等，设：
$${}_T^{E}T=\left[\begin{array}{cc}{}_T^E{R}_i& {}^E{P}_T\text{EPT}{}^E{P}_T\\ 0& 1\text{1}1\end{array}\right]\text{\hspace{1em}(3)}$$
  【3】${}_T^{B}T$ 不等，但其位置${}^E{P}_T$相等，设：
$${}_T^{B}T=\left[\begin{array}{cc}{}_T^B{R}_i& {}^B{P}_T\text{BPT}{}^B{P}_T\\ 0& 1\end{array}\right]\text{\hspace{1em}(4)}$$
  关注公式(2)、(3)、(4)中加粗符号，有：
$${}_E^B{R}_i\text{EBRi}{}_E^B{R}_i\cdot {}^E{P}_T\text{EPT}{}^E{P}_T+{}^B{P}_{Ei}\text{BPEi}{}^B{P}_{Ei}={}^B{P}_T\text{BPT}{}^B{P}_T\text{\hspace{1em}(5)}$$
  在实际中，${}_E^{B}T$ 由机器人正解方程可以直接测得，因此，我们直接读取四个位置点的姿态${}_E^B{R}_i$和位置${}^B{P}_{Ei=1,2,3,4}$。假设：
$${}_E^B{R}_1{\cdot }^E{P}_T{+}^B{P}_{E1}{=}^B{P}_T\text{\hspace{1em}(6)}$$
$${}_E^B{R}_2{\cdot }^E{P}_T{+}^B{P}_{E2}{=}^B{P}_T\text{\hspace{1em}(7)}$$
  则，(6) - (7)得：
$${(}_E^B{R}_1{-}_E^B{R}_2){\cdot }^E{P}_T{=}^B{P}_{E2}{-}^B{P}_{E1}\text{\hspace{1em}(8)}$$
  同理可得：
$${(}_E^B{R}_2{-}_E^B{R}_3){\cdot }^E{P}_T{=}^B{P}_{E3}{-}^B{P}_{E2}\text{\hspace{1em}(9)}$$
$${(}_E^B{R}_3{-}_E^B{R}_4){\cdot }^E{P}_T{=}^B{P}_{E4}{-}^B{P}_{E3}\text{\hspace{1em}(10)}$$
  由(8)、(9)、(10)可得：
$$\left[\begin{array}{c}{}_E^B{R}_1{-}_E^B{R}_2\\ {}_E^B{R}_2{-}_E^B{R}_3\\ {}_E^B{R}_3{-}_E^B{R}_4\end{array}\right]{\cdot }^E{P}_T=\left[\begin{array}{c}{}^B{P}_{E2}{-}^B{P}_{E1}\\ {}^B{P}_{E3}{-}^B{P}_{E2}\\ {}^B{P}_{E4}{-}^B{P}_{E3}\end{array}\right]\text{\hspace{1em}(11)}$$
  由于${}^E{P}_T$为 3x1 列向量，而等式右边为 9x3的矩阵，因此方程(11)为不相容方程组，不可直接用非齐次线性方程组求解的方法或者solve求解。采用最小二乘法的矩阵形式，因其系数矩阵不是方阵，不可直接求逆， 因此使用广义逆。采用高斯消元法得到：
$${}^E{P}_T=\left[\begin{array}{c}{}_E^B{R}_1{-}_E^B{R}_2\\ {}_E^B{R}_2{-}_E^B{R}_3\\ {}_E^B{R}_3{-}_E^B{R}_4\end{array}\right]\text{}\left[\begin{array}{c}{}^B{P}_{E2}{-}^B{P}_{E1}\\ {}^B{P}_{E3}{-}^B{P}_{E2}\\ {}^B{P}_{E4}{-}^B{P}_{E3}\end{array}\right]\text{\hspace{1em}(12)}$$
  则式(12)所求得的${}^E{P}_T$即为TCP的位置向量。

2、TCF姿态标定

  在第1部分已经得到工具坐标系(TCF)的位置，而计算TCP姿态采用z/x方向标定。
  此过程中TCF的姿态保持不变(如第一节 – 基本步骤中图所示)。取第一个姿态标定点为位置点4（下图记作标定点1）；机器人从位置点4出发，沿+x方向移动一定距离得到位置点5（下图记作标定点2)；机器人从位置点4出发，沿+z方向移动一定距离得到位置点6(下图记作标定点3）。如下图所示：

  由于3个标定点中的TCF姿态不变，故${}_E^B{R}_{i=4,5,6}$均相等，且由(12)得 ${}^E{P}_T$ 保持不变，故可得到工具坐标系 T 的 x 轴轴向向量 $X$ ，且：
$$X{=}^B{P}_{E5}{-}^B{P}_{E4}\text{\hspace{1em}(13)}$$
  同理，可得到工具坐标系 T 的 z 轴轴向向量 $Z$ ，且：
$$Z{=}^B{P}_{E6}{-}^B{P}_{E4}\text{\hspace{1em}(14)}$$
  进而由右手定则得工具坐标系 T 的 y 轴轴向向量 $Y$ ：
$$Y=Z\times X\text{\hspace{1em}(15)}$$
  为进一步保证坐标系矢量的正交性，重新计算$Z$ ：
$$Z=X\times Y\text{\hspace{1em}(16)}$$
  将(13)、(15)、(16）单位化得到$X^{{}^{\prime }},Y^{{}^{\prime }},Z^{{}^{\prime }}$，得到工具坐标 T 相对于基坐标 B 的姿态${}_T^{B}R$，且
$${}_T^{B}R=\left[\begin{array}{c}{X}^{{}^{\prime }}{Y}^{{}^{\prime }}{Z}^{{}^{\prime }}\end{array}\right]\text{\hspace{1em}(17)}$$
  又末端坐标系 E 旋转矩阵为${}_E^{B}R$，且：
$${}_E^{B}R{}_T^{E}R{=}_T^{B}R\text{\hspace{1em}(18)}$$
  故由(17)、(18)即可得到工具坐标系的旋转矩阵${}_T^{E}R$，即：
$${}_T^{E}R{=}_E^B{R}^{-1}{}_T^{B}R\text{\hspace{1em}(19)}$$

3、TCF标定结果

  由(12)和(19)，可得到工具坐标系 ${}_T^{E}T$ 标定为：
$${}_T^{E}T=\left[\begin{array}{cc}{}_T^{E}R& {}^E{P}_T\\ 0& 1\end{array}\right]\text{\hspace{1em}(20)}$$
  显然，TCF六点法标定的最小条件是能够获取到6个位置点的位姿${}_E^B{T}_{i=1,2,3,4,5,6}$，且为使式(12)能求解，应保证位置点1，2，3，4不在同一平面上。

三、参考文献

【1】康存锋，王红伟，张鹏飞等.焊接机器人工具坐标系标定的研究与实现[j].北京工业大学学报 2016, 42(1).
【2】兰虎等.《工业机器人技术及应用》.