本文主要介紹Frank C. Park and Bryan J. Martin在文獻Robot sensor calibration: solving AX=XB on the Euclidean group中提出的手眼標定算法，該算法也被稱為Navy手眼標定算法，該算法的主要創新點為利用李群理論的知識來求解手眼標定經典方程。該算法基于OpenCV的C++版本程序可去CSDN資源下載，MATLAB版本作者為蘇黎世理工的Christian Wengert，也可在此處下載。轉自：https://blog.csdn.net/YunlinWang/article/details/51871520

---

? ? ? ?正如Tsai等在文獻中指出的，手眼標定問題其實就是求解AX=XB方程問題。其中AA為機器人末端連桿坐標架在機器人-攝像機系統移動前后的轉換關系，B為攝像機坐標架在移動前后的相對關系。Tsai指出要唯一確定手眼矩陣的各分量，至少需要旋轉軸不平行的兩組運動。由于在觀測中一般存在噪聲，因此在實際測量中一般需要多組運動來求解該方程。

? ? ? ?假設有多組觀測值{(A1,B1),(A2,B2),?,(Ak,Bk)}，求解AX=XB方程可以轉化為如下最小化問題?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??

其中，d表示在歐式群上的距離測度。利用李群理論知識可以將上述最小化問題轉化為最小二乘擬合問題，從而可以得到簡單而又明確的解。下面介紹李群中的基本知識。

---

李群基本知識

? ? ? ?描述剛體運動可以用歐式群表示，由如下形式的矩陣表示：

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

其中，θ∈SO(3),b∈?3，SO(3)表示旋轉矩陣組成的群。

? ? ? ?每個李群都有其對應的李代數，李群與李代數之間的轉換可以由指數映射和對數映射完成。由于本人數學知識比較單薄，如果讀者想要繼續深入理解李群和李代數知識，可以自行查閱相關文獻或者書籍。在這里我就不詳細介紹，只簡單介紹Park文獻中出現的知識。

指數映射

? ? ? ?李代數到李群的轉換滿足指數映射關系，假設[w]∈so(3)，而exp?[w]∈SO(3)，則其指數映射滿足羅德里格斯公式：

? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

對數映射

? ? ? ?李群到李代數的轉換滿足對數映射關系，假設θ∈SO(3)，則其對數映射為：?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

其中，1+2cos??=tr(θ)。

---

利用李群知識求解AX=XB

? ? ? ?我們知道，手眼標定問題其實就是求解方程AX=XB，將X移到方程的右邊，可以得到，根據上文介紹的對數映射關系，在方程的兩邊取對數，則可以得到?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??

令?logA=[α],logB=[β]，則上式可以化為[α]=X[β]XT=[Xβ]，從而?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?α=Xβ

? ? ? ?AX=XB寫成矩陣形式為?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??

將上式展開可以得到?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??

? ? ? ?與Tsai手眼標定類似，同樣采用“兩步法”求解上述方程，先解算旋轉矩陣，再求得平移向量。由上面推導 α=Xβ，因此 θAθX=θXθB可以寫成?α=θXβ。當存在多組觀測值時，求解該方程可以轉化為下面最小二乘擬合問題：?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??

? ? ? ?很顯然，上述問題是典型的絕對定向問題，因而求解上式與絕對定向相同，其解為?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?
其中，。

然而，上式的求解是有條件的，即MM不奇異，因而當AA、BB只有兩組時，不能用上述方法求解。

? ? ? ?當只有兩組A、B時，即有A1,A2,B1,B2，?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

旋轉矩陣的解為：?

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?
其中，，×表示叉乘。

? ? ? ?在求得旋轉矩陣后，求解平移向量的步驟與Tsai手眼標定相同，讀者可以查看上篇文獻，這里就不在贅述。

---

算法源代碼

? ? ? ?根據上述基本計算步驟，在利用OpenCV 2.0開源庫的基礎上，編寫Tsai手眼標定方法的c++程序，其實現函數代碼如下：

void Navy_HandEye(Mat Hcg, vector<Mat> Hgij, vector<Mat> Hcij) {CV_Assert(Hgij.size() == Hcij.size());int nStatus = Hgij.size();Mat Rgij(3, 3, CV_64FC1);Mat Rcij(3, 3, CV_64FC1);Mat alpha1(3, 1, CV_64FC1);Mat beta1(3, 1, CV_64FC1);Mat alpha2(3, 1, CV_64FC1);Mat beta2(3, 1, CV_64FC1);Mat A(3, 3, CV_64FC1);Mat B(3, 3, CV_64FC1);Mat alpha(3, 1, CV_64FC1);Mat beta(3, 1, CV_64FC1);Mat M(3, 3, CV_64FC1, Scalar(0));Mat MtM(3, 3, CV_64FC1);Mat veMtM(3, 3, CV_64FC1);Mat vaMtM(3, 1, CV_64FC1);Mat pvaM(3, 3, CV_64FC1, Scalar(0));Mat Rx(3, 3, CV_64FC1);Mat Tgij(3, 1, CV_64FC1);Mat Tcij(3, 1, CV_64FC1);Mat eyeM = Mat::eye(3, 3, CV_64FC1);Mat tempCC(3, 3, CV_64FC1);Mat tempdd(3, 1, CV_64FC1);Mat C;Mat d;Mat Tx(3, 1, CV_64FC1);//Compute rotationif (Hgij.size() == 2) // Two (Ai,Bi) pairs{Rodrigues(Hgij[0](Rect(0, 0, 3, 3)), alpha1);Rodrigues(Hgij[1](Rect(0, 0, 3, 3)), alpha2);Rodrigues(Hcij[0](Rect(0, 0, 3, 3)), beta1);Rodrigues(Hcij[1](Rect(0, 0, 3, 3)), beta2);alpha1.copyTo(A.col(0));alpha2.copyTo(A.col(1));(alpha1.cross(alpha2)).copyTo(A.col(2));beta1.copyTo(B.col(0));beta2.copyTo(B.col(1));(beta1.cross(beta2)).copyTo(B.col(2));Rx = A*B.inv();}else // More than two (Ai,Bi) pairs{for (int i = 0; i < nStatus; i++){Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);Rodrigues(Rgij, alpha);Rodrigues(Rcij, beta);M = M + beta*alpha.t();}MtM = M.t()*M;eigen(MtM, vaMtM, veMtM);pvaM.at<double>(0, 0) = 1 / sqrt(vaMtM.at<double>(0, 0));pvaM.at<double>(1, 1) = 1 / sqrt(vaMtM.at<double>(1, 0));pvaM.at<double>(2, 2) = 1 / sqrt(vaMtM.at<double>(2, 0));Rx = veMtM*pvaM*veMtM.inv()*M.t();}//Computer Translation for (int i = 0; i < nStatus; i++){Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);Hgij[i](Rect(3, 0, 1, 3)).copyTo(Tgij);Hcij[i](Rect(3, 0, 1, 3)).copyTo(Tcij);tempCC = eyeM - Rgij;tempdd = Tgij - Rx * Tcij;C.push_back(tempCC);d.push_back(tempdd);}Tx = (C.t()*C).inv()*(C.t()*d);Rx.copyTo(Hcg(Rect(0, 0, 3, 3)));Tx.copyTo(Hcg(Rect(3, 0, 1, 3)));Hcg.at<double>(3, 0) = 0.0;Hcg.at<double>(3, 1) = 0.0;Hcg.at<double>(3, 2) = 0.0;Hcg.at<double>(3, 3) = 1.0;}

?

總結

以上是生活随笔為你收集整理的手眼标定算法---Navy算法（Robot sensor calibration: solving AX=XB on the Euclidean group）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。