工業機器人工具坐標系（TCF）標定的六點法原理

目錄

• 一、基本步驟
• 二、標定過程
  • 1、TCP位置標定
  • 2、TCF姿態標定
  • 3、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的變換關系 ：\(^{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】蘭虎等.《工業機器人技術及應用》.