本文解決的問題： 機械手搭載雙目相機，手眼標(biāo)定。 本文有細(xì)致的推導(dǎo)過程，非常全面。 什么是手眼標(biāo)定 確定像素坐標(biāo)系和空間機械手坐標(biāo)系的坐標(biāo)轉(zhuǎn)化關(guān)系； 為什么會存在這個？ 舉一個生活中常見的例子——用手移動物體： 第一步：眼睛觀察到三維世界，并將其轉(zhuǎn)換到視網(wǎng)膜平面（三維空間轉(zhuǎn)換到二維平面）傳送信息給大腦； 第二步：大腦想要移動某個物體，假設(shè)想要將物體從A點移動B點（二維坐標(biāo)），但是物體是三維空間中的物體，是三維坐標(biāo)，需要將二維坐標(biāo)換算成三維坐標(biāo)； 第三步：大腦已經(jīng)獲得A點和B點的三維坐標(biāo)，大腦給手（執(zhí)行機構(gòu)）發(fā)出指令去完成這個任務(wù)； 其中第二步就是手眼標(biāo)定，得到二維坐標(biāo)（像素坐標(biāo)）到三維坐標(biāo)的轉(zhuǎn)換矩陣 在實際控制中，相機檢測到目標(biāo)在圖像中的像素位置后，通過標(biāo)定好的坐標(biāo)轉(zhuǎn)換矩陣將相機的像素坐標(biāo)變換到機械手的空間坐標(biāo)系中，然后根據(jù)機械手坐標(biāo)系計算出各個電機該如何運動，從而控制機械手到達(dá)指定位置。這個過程中涉及到了圖像標(biāo)定，圖像處理，運動學(xué)正逆解，手眼標(biāo)定等。 相機的裝載位置 不在手上(eye-to-hand) 相機固定在一個地方，機械手的運動不會帶著相機一起移動。 在手上(eye-in-hand) 相機安裝在機械手上，隨著機械手一起移動。較為常用。這個實際上和eye-to-hand類似。 可以快速有效地標(biāo)定被測物體的坐標(biāo)。 這種情況的標(biāo)定過程實際上和相機和機械手分離的標(biāo)定方法是一樣的，因為相機拍照時，機械手會運動到相機標(biāo)定的時候的位置，然后相機拍照，得到目標(biāo)的坐標(biāo)，再控制機械手，所以簡單的相機固定在末端的手眼系統(tǒng)很多都是采用這種方法，標(biāo)定的過程和手眼分離系統(tǒng)的標(biāo)定是可以相同對待的。 ●基于圖像的視覺控制 ●基于位置的視覺控制 ●結(jié)合兩者的混合視覺控制 在正式開始講解之前，可以看一下：深入淺出地理解機器人手眼標(biāo)定 對手眼標(biāo)定有一個直觀的認(rèn)識。 正式開始 本文的相機搭載方案是，hand-in-eye。移動相機，標(biāo)定求解過程 在推導(dǎo)過程中，我們會用到四個坐標(biāo)系，分別是： ●基礎(chǔ)坐標(biāo)系（用base表示） ●機械手坐標(biāo)系（用tool表示） ●相機坐標(biāo)系（用cam表示） ●標(biāo)定物坐標(biāo)系（用cal表示） 下面先給出示意圖：

坐標(biāo)系之間的轉(zhuǎn)換關(guān)系說明： ●baseHtool：表示機械手坐標(biāo)系到基礎(chǔ)坐標(biāo)系的轉(zhuǎn)換關(guān)系，可以由機器人系統(tǒng)中得出。（已知） ●toolHcam：表示相機坐標(biāo)系到機械手坐標(biāo)系的轉(zhuǎn)換關(guān)系；這個轉(zhuǎn)化關(guān)系在機械手移動過程中是不變的；（未知，待求） ●calHcam：表示相機坐標(biāo)系到標(biāo)定板坐標(biāo)系的轉(zhuǎn)換關(guān)系（相機外參），可以由相機標(biāo)定求出；（相當(dāng)于已知） ●baseHcal：表示標(biāo)定板坐標(biāo)系到基礎(chǔ)坐標(biāo)系的變換，這個是最終想要得到的結(jié)果；只要機械手和標(biāo)定板的相對位置不變，這個變換矩陣不發(fā)生變化。

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

1. 基礎(chǔ)坐標(biāo)系(求解baseHtool) 符合右手定則的XYZ三個坐標(biāo)軸 ●原點：機器人底座的中心點 ●X軸正向：指向機器人的正前方 ●Z軸正向：指向機器人的正上方 ●Y軸正向：由右手定則確定

六個自由度 ●三個位置：x、y、z（第六軸法蘭盤圓心相對于原點的偏移量） ●三個角：Rx、Ry、Rz（第六軸法蘭盤的軸線角度，由初始姿態(tài)即豎直向上繞x軸旋轉(zhuǎn)Rx度，再繞Y軸旋轉(zhuǎn)Ry度，再繞Z軸旋轉(zhuǎn)Rz度得到） ●旋轉(zhuǎn)方式（機器人RPY角和Euler角 – 基本公式）（機器人學(xué)-熊有倫36-40頁） ●繞定軸X-Y-Z旋轉(zhuǎn)(判斷機械臂輸出四元數(shù)與代碼得到的四元數(shù)是否相等得到)

一定要注意歐拉角和李代數(shù)不一樣，非常容易搞混，因為他們都是3個量 歐拉角：分別繞x、y、z軸旋轉(zhuǎn)的角度，不一樣的旋轉(zhuǎn)次序，得到的R不一樣； 李代數(shù)：維度是3，是繞一個軸轉(zhuǎn)動一定的角度。歐拉角可以理解成李代數(shù)在x、y、z軸上的分解旋轉(zhuǎn)。（不一定正確，不過比較形象） 注：不同機械臂示教器顯示的法蘭盤的數(shù)據(jù)格式不一樣，有的是用歐拉角顯示的，有的是用角軸顯示的。 2. camHcal相機到標(biāo)定板 ●注意：標(biāo)定板坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換到相機坐標(biāo)系下

思路大致如下： ●已知雙目相機的內(nèi)參、畸變系數(shù)、外參（Pr=R?Pl+t P_r=R*P_l+tP r=R?P l +t）, ●對左右相機的兩張圖片調(diào)用OpenCV中的findChessboardCorners函數(shù)，找到內(nèi)角點（如果結(jié)果不好，繼續(xù)提取亞像素點）； ●將左右相機的像素點對應(yīng)起來，得到匹配的2d點； ●使用空間異面直線的方法，用對應(yīng)的2d點計算出以右相機為世界坐標(biāo)系的3維坐標(biāo)Pcam P_{cam}P cam ；(立體視覺匹配) ●計算出每個角點以棋盤格為世界坐標(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頁）) 張正友相機標(biāo)定Opencv實現(xiàn)

參數(shù)解釋： ●第一個參數(shù)Image，傳入拍攝的棋盤圖Mat圖像，必須是8位的灰度或者彩色圖像； ●第二個參數(shù)patternSize，每個棋盤圖上內(nèi)角點的行列數(shù)，一般情況下，行列數(shù)不要相同，便于后續(xù)標(biāo)定程序識別標(biāo)定板的方向； ●第三個參數(shù)corners，用于存儲檢測到的內(nèi)角點圖像坐標(biāo)位置，一般用元素是Point2f的向量來表示：vector image_points_buf; ●第四個參數(shù)flage：用于定義棋盤圖上內(nèi)角點查找的不同處理方式，有默認(rèn)值。 3. 求解AX=XB 以下四篇論文對應(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實現(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實現(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ù)的知識，用對偶四元數(shù)表達(dá)旋轉(zhuǎn)和平移，從而進(jìn)行統(tǒng)一計算； 著四種算法精度差不多，不過文獻(xiàn)4的效果要更好點。 具體實現(xiàn)文獻(xiàn)3的算法，下面具體介紹 對數(shù)：乘法變加法 李群李代數(shù)

利用李群知識求解AX=XB

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

求解旋轉(zhuǎn)矩陣

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