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

时间:2024-03-08 19:10:53

一、基本步骤

(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的变换关系 :\(^{B}_ {E}T\)
【2】工具坐标系T 相对于末端坐标系 E的变换关系 :\(^{E}_ {T}T\)
【3】工具坐标系T 相对于基坐标系 B的变换关系 :\(^{B}_ {T}T\)

  显然可以知道:
$$^{B}_ {E}T · ^{E}_ {T}T = ^{B}_ {T}T \tag{1}$$

  对于选定位置点 i = 1、2、3、4,有:

  【1】\(^{B}_ {E}T\)不等,设:

\[^{B}_ {E}T = \begin{bmatrix} \pmb{^{B}_ {E}R_{i}} & \pmb{^{B}P_ {Ei}}\\ 0 & 1 \\ \end{bmatrix} \tag{2} \]

  【2】\(^{E}_ {T}T\)不等,但其位置\(^{E}P_ {T}\)相等,设:

\[^{E}_ {T}T = \begin{bmatrix} ^{E}_ {T}R_ {i} & \pmb{^{E}P_ {T}} \\ 0 & \pmb{1} \\ \end{bmatrix} \tag{3} \]

  【3】\(^{B}_ {T}T\) 不等,但其位置\(^{E}P_ {T}\)相等,设:

\[ ^{B}_ {T}T = \begin{bmatrix} ^{B}_ {T}R_ {i} & \pmb{^{B}P_ {T}}\\ 0 & 1 \\ \end{bmatrix} \tag{4} \]

  关注公式(2)、(3)、(4)中加粗符号,有:

\[ \pmb{^{B}_ {E}R_ {i}} · \pmb{^{E}P_ {T}} + \pmb{^{B}P_ {Ei}} = \pmb{^{B}P_ {T}} \tag{5} \]

  在实际中,\(^{B}_ {E}T\) 由机器人正解方程可以直接测得,因此,我们直接读取四个位置点的姿态\(^{B}_ {E}R_ {i}\)和位置\(^{B}P_ {Ei = 1, 2, 3, 4}\)。假设:

\[^{B}_ {E}R_ {1}· ^{E}P_ {T} + ^{B}P_ {E1} = ^{B}P_ {T} \tag{6} \]

\[^{B}_ {E}R_ {2}· ^{E}P_ {T} + ^{B}P_ {E2} = ^{B}P_ {T} \tag{7} \]

  则,(6) - (7)得:

\[(^{B}_ {E}R_ {1} - ^{B}_ {E}R_ {2} )· ^{E}P_ {T} = ^{B}P_ {E2} - ^{B}P_ {E1} \tag{8} \]

  同理可得:

\[(^{B}_ {E}R_ {2} - ^{B}_ {E}R_ {3} )· ^{E}P_ {T} = ^{B}P_ {E3} - ^{B}P_ {E2} \tag{9} \]

\[(^{B}_ {E}R_ {3} - ^{B}_ {E}R_ {4} )· ^{E}P_ {T} = ^{B}P_ {E4} - ^{B}P_ {E3} \tag{10} \]

  由(8)、(9)、(10)可得:

\[ \begin{bmatrix} ^{B}_ {E}R_ {1} - ^{B}_ {E}R_ {2}\\ ^{B}_ {E}R_ {2} - ^{B}_ {E}R_ {3}\\ ^{B}_ {E}R_ {3} - ^{B}_ {E}R_ {4} \end{bmatrix}· ^{E}P_ {T} = \begin{bmatrix} ^{B}P_ {E2} - ^{B}P_ {E1}\\ ^{B}P_ {E3} - ^{B}P_ {E2}\\ ^{B}P_ {E4} - ^{B}P_ {E3} \end{bmatrix} \tag{11} \]

  由于\(^{E}P_ {T}\)为 3x1 列向量,而等式右边为 9x3的矩阵,因此方程(11)为不相容方程组,不可直接用非齐次线性方程组求解的方法或者solve求解。采用最小二乘法的矩阵形式,因其系数矩阵不是方阵,不可直接求逆, 因此使用广义逆。采用高斯消元法得到:

\[^{E}P_ {T} = \begin{bmatrix} ^{B}_ {E}R_ {1} - ^{B}_ {E}R_ {2}\\ ^{B}_ {E}R_ {2} - ^{B}_ {E}R_ {3}\\ ^{B}_ {E}R_ {3} - ^{B}_ {E}R_ {4} \end{bmatrix}\verb|\| \begin{bmatrix} ^{B}P_ {E2} - ^{B}P_ {E1}\\ ^{B}P_ {E3} - ^{B}P_ {E2}\\ ^{B}P_ {E4} - ^{B}P_ {E3} \end{bmatrix} \tag{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姿态不变,故\(^{B}_ {E}R_{i = 4,5,6}\)均相等,且由(12)得 \(^{E}P_ {T}\) 保持不变,故可得到工具坐标系 T 的 x 轴轴向向量 \(X\) ,且:

\[X = ^{B}P_ {E5} - ^{B}P_ {E4} \tag{13} \]

  同理,可得到工具坐标系 T 的 z 轴轴向向量 \(Z\) ,且:

\[Z = ^{B}P_ {E6} - ^{B}P_ {E4} \tag{14} \]

  进而由右手定则得工具坐标系 T 的 y 轴轴向向量 \(Y\)

\[Y = Z \times X\tag{15} \]

  为进一步保证坐标系矢量的正交性,重新计算\(Z\)

\[Z = X \times Y \tag{16} \]

  将(13)、(15)、(16)单位化得到\(X^{\'}, Y^{\'}, Z^{\'}\),得到工具坐标 T 相对于基坐标 B 的姿态\(^{B}_ {T}R\),且

\[^{B}_ {T}R = \begin{bmatrix}X^{\'} Y^{\'} Z^{\'} \end{bmatrix} \tag{17} \]

  又末端坐标系 E 旋转矩阵为\(^{B}_ {E}R\),且:

\[^{B}_ {E}R\ ^{E}_ {T}R=^{B}_ {T}R \tag{18} \]

  故由(17)、(18)即可得到工具坐标系的旋转矩阵\(^{E}_ {T}R\),即:

\[^{E}_ {T}R = ^{B}_ {E}R^{-1}\ ^{B}_ {T}R \tag{19} \]

3、TCF标定结果

  由(12)和(19),可得到工具坐标系 \(^{E}_ {T}T\) 标定为:

\[^{E}_ {T}T = \begin{bmatrix} ^{E}_ {T}R & ^{E}P_ {T} \\ 0 & 1 \end{bmatrix} \tag{20} \]

  显然,TCF六点法标定的最小条件是能够获取到6个位置点的位姿\(^{B}_ {E}T_{i=1,2,3,4,5,6}\),且为使式(12)能求解,应保证位置点1,2,3,4不在同一平面上。

三、参考文献

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