機(jī)器人手眼標(biāo)定的推導(dǎo)過程

本文解決的問題：

機(jī)械手搭載雙目相機(jī)，手眼標(biāo)定。

本文有細(xì)致的推導(dǎo)過程，非常全面。

什么是手眼標(biāo)定

確定像素坐標(biāo)系和空間機(jī)械手坐標(biāo)系的坐標(biāo)轉(zhuǎn)化關(guān)系；

為什么會(huì)存在這個(gè)？

舉一個(gè)生活中常見的例子——用手移動(dòng)物體：

第一步：眼睛觀察到三維世界，并將其轉(zhuǎn)換到視網(wǎng)膜平面（三維空間轉(zhuǎn)換到二維平面）傳送信息給大腦；

第二步：大腦想要移動(dòng)某個(gè)物體，假設(shè)想要將物體從A點(diǎn)移動(dòng)B點(diǎn)（二維坐標(biāo)），但是物體是三維空間中的物體，是三維坐標(biāo)，需要將二維坐標(biāo)換算成三維坐標(biāo)；

第三步：大腦已經(jīng)獲得A點(diǎn)和B點(diǎn)的三維坐標(biāo)，大腦給手（執(zhí)行機(jī)構(gòu)）發(fā)出指令去完成這個(gè)任務(wù)；

其中第二步就是手眼標(biāo)定，得到二維坐標(biāo)（像素坐標(biāo)）到三維坐標(biāo)的轉(zhuǎn)換矩陣

在實(shí)際控制中，相機(jī)檢測(cè)到目標(biāo)在圖像中的像素位置后，通過標(biāo)定好的坐標(biāo)轉(zhuǎn)換矩陣將相機(jī)的像素坐標(biāo)變換到機(jī)械手的空間坐標(biāo)系中，然后根據(jù)機(jī)械手坐標(biāo)系計(jì)算出各個(gè)電機(jī)該如何運(yùn)動(dòng)，從而控制機(jī)械手到達(dá)指定位置。這個(gè)過程中涉及到了圖像標(biāo)定，圖像處理，運(yùn)動(dòng)學(xué)正逆解，手眼標(biāo)定等。

相機(jī)的裝載位置

不在手上(eye-to-hand)

相機(jī)固定在一個(gè)地方，機(jī)械手的運(yùn)動(dòng)不會(huì)帶著相機(jī)一起移動(dòng)。

在手上(eye-in-hand)

相機(jī)安裝在機(jī)械手上，隨著機(jī)械手一起移動(dòng)。較為常用。這個(gè)實(shí)際上和eye-to-hand類似。

可以快速有效地標(biāo)定被測(cè)物體的坐標(biāo)。

這種情況的標(biāo)定過程實(shí)際上和相機(jī)和機(jī)械手分離的標(biāo)定方法是一樣的，因?yàn)橄鄼C(jī)拍照時(shí)，機(jī)械手會(huì)運(yùn)動(dòng)到相機(jī)標(biāo)定的時(shí)候的位置，然后相機(jī)拍照，得到目標(biāo)的坐標(biāo)，再控制機(jī)械手，所以簡單的相機(jī)固定在末端的手眼系統(tǒng)很多都是采用這種方法，標(biāo)定的過程和手眼分離系統(tǒng)的標(biāo)定是可以相同對(duì)待的。

●基于圖像的視覺控制

●基于位置的視覺控制

●結(jié)合兩者的混合視覺控制

在正式開始講解之前，可以看一下：深入淺出地理解機(jī)器人手眼標(biāo)定

對(duì)手眼標(biāo)定有一個(gè)直觀的認(rèn)識(shí)。

正式開始

本文的相機(jī)搭載方案是，hand-in-eye。移動(dòng)相機(jī)，標(biāo)定求解過程

在推導(dǎo)過程中，我們會(huì)用到四個(gè)坐標(biāo)系，分別是：

●基礎(chǔ)坐標(biāo)系（用base表示）

●機(jī)械手坐標(biāo)系（用tool表示）

●相機(jī)坐標(biāo)系（用cam表示）

●標(biāo)定物坐標(biāo)系（用cal表示）

下面先給出示意圖：

坐標(biāo)系之間的轉(zhuǎn)換關(guān)系說明：

●baseHtool：表示機(jī)械手坐標(biāo)系到基礎(chǔ)坐標(biāo)系的轉(zhuǎn)換關(guān)系，可以由機(jī)器人系統(tǒng)中得出。（已知）

●toolHcam：表示相機(jī)坐標(biāo)系到機(jī)械手坐標(biāo)系的轉(zhuǎn)換關(guān)系；這個(gè)轉(zhuǎn)化關(guān)系在機(jī)械手移動(dòng)過程中是不變的；（未知，待求）

●calHcam：表示相機(jī)坐標(biāo)系到標(biāo)定板坐標(biāo)系的轉(zhuǎn)換關(guān)系（相機(jī)外參），可以由相機(jī)標(biāo)定求出；（相當(dāng)于已知）

●baseHcal：表示標(biāo)定板坐標(biāo)系到基礎(chǔ)坐標(biāo)系的變換，這個(gè)是最終想要得到的結(jié)果；只要機(jī)械手和標(biāo)定板的相對(duì)位置不變，這個(gè)變換矩陣不發(fā)生變化。

所以：其中的A已知，X待求，B需要通過相機(jī)標(biāo)定得知（張正友標(biāo)定法可以求得）。

驗(yàn)證結(jié)果

1. 基礎(chǔ)坐標(biāo)系(求解baseHtool)

符合右手定則的XYZ三個(gè)坐標(biāo)軸

●原點(diǎn)：機(jī)器人底座的中心點(diǎn)

●X軸正向：指向機(jī)器人的正前方

●Z軸正向：指向機(jī)器人的正上方

●Y軸正向：由右手定則確定

六個(gè)自由度

●三個(gè)位置：x、y、z（第六軸法蘭盤圓心相對(duì)于原點(diǎn)的偏移量）

●三個(gè)角：Rx、Ry、Rz（第六軸法蘭盤的軸線角度，由初始姿態(tài)即豎直向上繞x軸旋轉(zhuǎn)Rx度，再繞Y軸旋轉(zhuǎn)Ry度，再繞Z軸旋轉(zhuǎn)Rz度得到）

●旋轉(zhuǎn)方式（機(jī)器人RPY角和Euler角 – 基本公式）（機(jī)器人學(xué)-熊有倫36-40頁）

●繞定軸X-Y-Z旋轉(zhuǎn)(判斷機(jī)械臂輸出四元數(shù)與代碼得到的四元數(shù)是否相等得到)

一定要注意歐拉角和李代數(shù)不一樣，非常容易搞混，因?yàn)樗麄兌际?個(gè)量

歐拉角：分別繞x、y、z軸旋轉(zhuǎn)的角度，不一樣的旋轉(zhuǎn)次序，得到的R不一樣；

李代數(shù)：維度是3，是繞一個(gè)軸轉(zhuǎn)動(dòng)一定的角度。歐拉角可以理解成李代數(shù)在x、y、z軸上的分解旋轉(zhuǎn)。（不一定正確，不過比較形象）

注：不同機(jī)械臂示教器顯示的法蘭盤的數(shù)據(jù)格式不一樣，有的是用歐拉角顯示的，有的是用角軸顯示的。

2. camHcal相機(jī)到標(biāo)定板

●注意：標(biāo)定板坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換到相機(jī)坐標(biāo)系下

思路大致如下：

●已知雙目相機(jī)的內(nèi)參、畸變系數(shù)、外參（Pr=R?Pl+t P_r=R*P_l+tP

r=R?P l +t）,

●對(duì)左右相機(jī)的兩張圖片調(diào)用OpenCV中的findChessboardCorners函數(shù)，找到內(nèi)角點(diǎn)（如果結(jié)果不好，繼續(xù)提取亞像素點(diǎn)）；

●將左右相機(jī)的像素點(diǎn)對(duì)應(yīng)起來，得到匹配的2d點(diǎn)；

●使用空間異面直線的方法，用對(duì)應(yīng)的2d點(diǎn)計(jì)算出以右相機(jī)為世界坐標(biāo)系的3維坐標(biāo)Pcam P_{cam}P cam ；(立體視覺匹配)

●計(jì)算出每個(gè)角點(diǎn)以棋盤格為世界坐標(biāo)的3維坐標(biāo)Pcal P_{cal}P cal；

●通過解方程Pcam=camHcal?Pcal P_{cam}=camHcal*P_{cal}P cam=camHcal?P cal 求解出外參(3d-3d：ICP，SVD奇異值分解（十四講173頁）)

張正友相機(jī)標(biāo)定Opencv實(shí)現(xiàn)

參數(shù)解釋：

●第一個(gè)參數(shù)Image，傳入拍攝的棋盤圖Mat圖像，必須是8位的灰度或者彩色圖像；

●第二個(gè)參數(shù)patternSize，每個(gè)棋盤圖上內(nèi)角點(diǎn)的行列數(shù)，一般情況下，行列數(shù)不要相同，便于后續(xù)標(biāo)定程序識(shí)別標(biāo)定板的方向；

●第三個(gè)參數(shù)corners，用于存儲(chǔ)檢測(cè)到的內(nèi)角點(diǎn)圖像坐標(biāo)位置，一般用元素是Point2f的向量來表示：vector image_points_buf;

●第四個(gè)參數(shù)flage：用于定義棋盤圖上內(nèi)角點(diǎn)查找的不同處理方式，有默認(rèn)值。

3. 求解AX=XB

以下四篇論文對(duì)應(yīng)著四種求解方法

Tsai, Roger Y., and Reimar K. Lenz. “A new technique for fully autonomous and efficient 3D robotics hand/eye calibration.” IEEE Transactions on robotics and automation 5.3 (1989): 345-358.(博客：Tsai-Lenz的OpenCV實(shí)現(xiàn))

Horaud, Radu, and Fadi Dornaika. “Hand-eye calibration.” The international journal of robotics research 14.3 (1995): 195-210.

Park, Frank C., and Bryan J. Martin. “Robot sensor calibration: solving AX= XB on the Euclidean group.” IEEE Transactions on Robotics and Automation10.5 (1994): 717-721.(博客：Navy的OpenCV實(shí)現(xiàn))

Daniilidis, Konstantinos. “Hand-eye calibration using dual quaternions.” The International Journal of Robotics Research 18.3 (1999): 286-298.

網(wǎng)上有源代碼可以下載：經(jīng)典手眼標(biāo)定算法C++代碼

文獻(xiàn)3采用的是李群的理論，將AX=XB轉(zhuǎn)化成最小二乘問題；

文獻(xiàn)4采用的時(shí)對(duì)偶四元數(shù)的知識(shí)，用對(duì)偶四元數(shù)表達(dá)旋轉(zhuǎn)和平移，從而進(jìn)行統(tǒng)一計(jì)算；

著四種算法精度差不多，不過文獻(xiàn)4的效果要更好點(diǎn)。

具體實(shí)現(xiàn)文獻(xiàn)3的算法，下面具體介紹

對(duì)數(shù)：乘法變加法

李群李代數(shù)

利用李群知識(shí)求解AX=XB

采用“兩步法”求解上述方程，先解算旋轉(zhuǎn)矩陣，再求得平移向量。

求解旋轉(zhuǎn)矩陣

代碼：用兩組數(shù)據(jù)求解方程AX=XB

審核編輯：湯梓紅