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

---

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

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

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

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

---

李群基本知識

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

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

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

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

指數(shù)映射

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

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

對數(shù)映射

? ? ? ?李群到李代數(shù)的轉(zhuǎn)換滿足對數(shù)映射關(guān)系，假設(shè)θ∈SO(3)，則其對數(shù)映射為：?

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

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

---

利用李群知識求解AX=XB

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

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

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

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

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

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

將上式展開可以得到?

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

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

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

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

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

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

? ? ? ?當(dāng)只有兩組A、B時，即有A1,A2,B1,B2，?

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

旋轉(zhuǎn)矩陣的解為：?

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

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

---

算法源代碼

? ? ? ?根據(jù)上述基本計算步驟，在利用OpenCV 2.0開源庫的基礎(chǔ)上，編寫Tsai手眼標(biāo)定方法的c++程序，其實(shí)現(xiàn)函數(shù)代碼如下：

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;}

?

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。