手眼標定

轉載請註明出處 https://blog.csdn.net/wanggao_1990/article/details/81435660

機器人視覺系統中要實現像素座標與實際座標的轉換，首先要進行標定。這裏標定不僅包括攝像頭標定，也包括機器人系統的手眼標定。這裏的構型爲Eye-in-hand。

1、攝像機標定

（1）理論

張正友棋盤標定法對攝像機進行標定。
由於攝像機標定結果要用到後面的手眼標定中，棋盤圖片拍攝時需要遵守：標定板固定位置不動，手眼組合體變換姿態拍攝圖片。
目的：兩組座標系的兩兩轉化矩陣：$T_1$和$T_2$
1） 攝像機座標系C轉化爲圖片像素座標系P的轉換矩陣T1

$P=T_1*C$

T1在攝像機標定中內參矩陣3*3
2） 棋盤世界座標系G轉化爲攝像機座標系C的轉換矩陣T2

$P=T_2*C$

T2在攝像機標定中外參矩陣4*4，由旋轉矩陣r33和平移向量t31構成[ t r; 0 0 0 1]

(2) 方法

OpenCv 或者 Matlab

2、手眼標定

（1）理論部分

手眼標定目的：得到機器手座標系H轉化爲攝像機座標系$C$的轉化矩陣$T_3$。可表示爲：

$C=T_3*H$

T3需要根據公式$AX=XB$得到；實際中，分別知道$A$、$B$求出來的X有無窮多個解。所以爲了實現唯一解至少需要兩組$A$和$B$，即至少需要3個位置的攝像機標定結果。
下圖爲$AX=XB$圖解，棋盤相對機器人基座標系固定，因此有

$B_1*X*A_1 = B_2*X*A_2$

轉換$A_2*A_1^{-1}*X = X*B_2^{-1}*B_1$

可令 $A= A_2*A_1^{-1}$，$B=B_2^{-1}*B_1$。

###1) $A$的求法
$A$是兩個攝像機座標系之間的變換矩陣。假設上述攝像機標定中有3張標定圖片的外參標定結果分別是$H_{c1}$、$H_{c2}$、$H_{c3}$，那麼可以得到兩個$A$矩陣：

$A_1=H_{c2}*H_{c1}^{-1}$

$A_2=H_{c3}*H_{c2}^{-1}$

2) $B$的求法

B是兩個機器手座標系之間的變換矩陣。假設上述攝像機標定中的3張標定圖片所一一對應的機器手座標系在基座標系（也可以是工件座標系或者其他固定的參考座標系）中的座標系描述矩陣結果分別是$H_{g1}$、$H_{g2}$、$H_{g3}$（需要從機器人控制器或示教器中讀取或轉換），那麼可以得到兩個$B$矩陣：

$B_1=H_{c2}^{-1}*H_{c1}$

$B_2=H_{c3}^{-1}*H_{c2}$

由以上兩組$A$和$B$，代入$AX=XB$就可以得到唯一解X，從而$T_3=X$。
至少2組數據，可以進行求解，參考Navy_HandEye或Tsai_HandEye等方法。

（2）計算過程

1. 獲取攝像機標定已知攝像機外參矩陣$H_{c1}$、$H_{c2}$、$H_{c3}$（攝像機標定時已有）

2. 從機器人控制器中讀取對應的機械手座標末端姿態描述數據
   機器人系統的座標系描述數據可能有2種形式：
   - 平移向量+歐拉角: $ x, y, z, r_z, r_y, r_z $ ， 需要知道轉換關係.
   - 平移向量+四元數模式： $ x, y, z, q_w, q_x, q_y, q_z$，唯一轉換。
   推薦選擇四元數模式，轉換成旋轉矩陣部分唯一，如下

   $R = \begin{bmatrix} 2(q_w^2+q_x^2)-1 & 2(q_xq_y - q_wq_z) & 2(q_xq_z+q_wq_x) \\ 2(q_xq_y+q_wq_z) & 2(q_w^2 + q_y^2)-1 & 2(q_yq_z-q_wq_x) \\ 2(q_xq_z+q_wq_x) & 2(q_yq_z + q_wq_x) & 2(q_w^2 + q_z^2)-1\\ \end{bmatrix}$

3. 求解攝像機座標系到機械手座標系的變換矩陣X
   例如採集圖像25幅，進行相機標定，可得到25個外參矩陣，同時對應可以從控制器界面讀取或者轉換得到25個姿態矩陣。接着可以利用 1） 2）得到$C_{25}^2$組$A$、$B$，再利用$AX=XB$計算得到$X$。

標題3、根據標定結果對目標定位

##（1）理論部分
由上述1、2標定得到：

矩陣	含義	說明
$H_{pc}$	內參矩陣	攝像機座標系C -> 像素座標系P
$H_{cg}$	手眼矩陣	機械手座標系H -> 攝像機座標系C
$H_g$	從控制器讀取的末端姿態矩陣	機械手座標系H -> 基座標系B

基座標系轉化爲像素座標系的變換矩陣，可由下面兩式求解（注意使用齊次座標系）。

$z_c* [u,v,1]^{T} = H_{pc} * [x_c,y_c,z_c]^{T}$

$[x,y,z]^{T} = H_g * H_{cg}^{-1}*[x_c,y_c,z_c]^{T}$

---

未完 …