Eye-in-hand和Eye-to-hand問題求解和實驗

• gc?Hj?，因此代入以上兩個公式時需要進行求逆計算。

齊次變換矩陣求逆方法
對于原齊次矩陣 ABT=[ABR0ApB01]

AB?T=[BA?RT0??BA?RT?ApB0??1?]

聯立上述的兩個公式，

weHi?ecH?(gcHi)?1=wgH

ew?Hi??ce?H?(cg?Hi?)?1=gw?H

weHj?ecH?(gcHj)?1=wgH

ew?Hj??ce?H?(cg?Hj?)?1=gw?H

消除常量矩陣 wgH

gw?H，

weHi?ecH?(gcHi)?1=weHj?ecH?(gcHj)?1

ew?Hi??ce?H?(cg?Hi?)?1=ew?Hj??ce?H?(cg?Hj?)?1

上式兩邊同時左乘 (weHj)?1

(ew?Hj?)?1 得，

(weHj)?1?weHi?ecH?(gcHi)?1=ecH?(gcHj)?1

(ew?Hj?)?1?ew?Hi??ce?H?(cg?Hi?)?1=ce?H?(cg?Hj?)?1

上式兩邊同時右乘 gcHi

cg?Hi? 得

(weHj)?1?weHi?ecH=ecH?(gcHj)?1?gcHi

(ew?Hj?)?1?ew?Hi??ce?H=ce?H?(cg?Hj?)?1?cg?Hi?

考慮到(gcHj)?1=(cgHj)

(cg?Hi?)=(gc?Hi?)?1，因此上式可以轉換為，

(weHj)?1?weHi?ecH=ecH?cgHj?(cgHi)?1

(ew?Hj?)?1?ew?Hi??ce?H=ce?H?gc?Hj??(gc?Hi?)?1

令X=ecH

B=gc?Hj??(gc?Hi?)?1，則上式可以化簡為，

AX=XB

AX=XB

以上方程為手眼標定的最終求解方程，具體求解方式可以參考Tasi and Lenz，1989的論文A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration。

以下描述通過仿真實驗的方式進行手眼標定測試和驗證的方法。大致流程如下：

1. 手眼系統仿真場景搭建。
2. 標定樣本采集。包括攝像機圖像采集，以及對應的機器人關節構型采集。
3. 圖像處理提取棋盤圖角點并計算棋盤相對于攝像機坐標系的位姿。
4. 采用基于Tasi-lenz的公式進行計算。

仿真實驗的主要環境配置和使用到的工具有：

• 操作系統：Windows 7 64bit
• 圖像處理工具：Matlab，OpenCV
• 機器人和攝像機仿真環境：RobWork （需先安裝QT、Boost等依賴庫）
• 虛擬棋盤圖編輯：AC3D，用于生成帶貼圖的棋盤標定板，加載到手眼仿真場景中

clc; close all;GridSize = 30;  %length of the square
m =7;      %number of row
n = 10;       % number of col
margin = 10; % white boarder size
I = ones(m*GridSize+2*margin,n*GridSize+2*margin)*255;%the first grid is black
for i = 1:mif mod(i,2)==1for j = 1:nif mod(j,2)==1I(1+(i-1)*GridSize+margin:i*GridSize+margin,...1+(j-1)*GridSize+margin:j*GridSize+margin) = 0;endendelsefor j = 1:nif mod(j,2)==0I(1+(i-1)*GridSize+margin:i*GridSize+margin,...1+(j-1)*GridSize+margin:j*GridSize+margin) = 0;endendend
end
imshow(I);imwrite(I,'chessboard.ppm');

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29

最終生成棋盤圖如下所示。

將上一步生成的chessboard.ac文件添加到場景XML文件中，并且在UR機器人末端設置一個虛擬攝像機，最終得到的機器人運行場景如下圖。

安裝在UR末端的攝像機的定義方式可以參考RobWork官網上的教程，本例中攝像機的定義方式如下如下：

<Frame name="Hand-Eye" refframe="UR.TCP"><RPY>0 -15 0</RPY><Pos>0.1 0 0</Pos>
</Frame><Frame name="Hand-Eye-Sim" refframe="Hand-Eye" type="Movable"><RPY>0 0 180</RPY><Pos>0 0 0</Pos><Property name="CameraName">Hand-Eye</Property><Property name="Camera">31.0482 960 720</Property>
</Frame>	

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11

需編寫一個RobWorkStudio的插件，用于采集攝像機圖片，以及與圖像對應的機器人TCP相對于機器人基坐標系的齊次變換矩陣，并保存到本地硬盤上。

源碼的運行要求安裝RobWork，安裝方法參考官網鏈接。

編譯本教程提供源碼時，需注意修改加載的機器人場景文件的路徑。

cv::Mat skew(cv::Mat m)
{Mat t(3, 3, CV_64FC1);t.at<double>(0, 0) = 0;t.at<double>(0, 1) = - m.at<double>(2, 0);t.at<double>(0, 2) = m.at<double>(1, 0);t.at<double>(1, 0) = m.at<double>(2, 0);t.at<double>(1, 1) = 0;t.at<double>(1, 2) = - m.at<double>(0, 0);t.at<double>(2, 0) = - m.at<double>(1, 0);t.at<double>(2, 1) = m.at<double>(0, 0);t.at<double>(2, 2) = 0;return t;
}void Tsai_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 rgij(3, 1, CV_64FC1);Mat rcij(3, 1, CV_64FC1);double theta_gij;double theta_cij;Mat rngij(3, 1, CV_64FC1);Mat rncij(3, 1, CV_64FC1);Mat Pgij(3, 1, CV_64FC1);Mat Pcij(3, 1, CV_64FC1);Mat tempA(3, 3, CV_64FC1);Mat tempb(3, 1, CV_64FC1);Mat A;Mat b;Mat pinA;Mat Pcg_prime(3, 1, CV_64FC1);Mat Pcg(3, 1, CV_64FC1);Mat PcgTrs(1, 3, CV_64FC1);Mat Rcg(3, 3, CV_64FC1);Mat eyeM = Mat::eye(3, 3, CV_64FC1);Mat Tgij(3, 1, CV_64FC1);Mat Tcij(3, 1, CV_64FC1);Mat tempAA(3, 3, CV_64FC1);Mat tempbb(3, 1, CV_64FC1);Mat AA;Mat bb;Mat pinAA;Mat Tcg(3, 1, CV_64FC1);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, rgij);Rodrigues(Rcij, rcij);theta_gij = norm(rgij);theta_cij = norm(rcij);rngij = rgij / theta_gij;rncij = rcij / theta_cij;Pgij = 2 * sin(theta_gij / 2)*rngij;Pcij = 2 * sin(theta_cij / 2)*rncij;tempA = skew(Pgij + Pcij);tempb = Pcij - Pgij;A.push_back(tempA);b.push_back(tempb);}//Compute rotationinvert(A, pinA, DECOMP_SVD);Pcg_prime = pinA * b;Pcg = 2 * Pcg_prime / sqrt(1 + norm(Pcg_prime) * norm(Pcg_prime));PcgTrs = Pcg.t();Rcg = (1 - norm(Pcg) * norm(Pcg) / 2) * eyeM + 0.5 * (Pcg * PcgTrs + sqrt(4 - norm(Pcg)*norm(Pcg))*skew(Pcg));//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);tempAA = Rgij - eyeM;tempbb = Rcg * Tcij - Tgij;AA.push_back(tempAA);bb.push_back(tempbb);}invert(AA, pinAA, DECOMP_SVD);Tcg = pinAA * bb;Rcg.copyTo(Hcg(Rect(0, 0, 3, 3)));Tcg.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;
}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56
• 57
• 58
• 59
• 60
• 61
• 62
• 63
• 64
• 65
• 66
• 67
• 68
• 69
• 70
• 71
• 72
• 73
• 74
• 75
• 76
• 77
• 78
• 79
• 80
• 81
• 82
• 83
• 84
• 85
• 86
• 87
• 88
• 89
• 90
• 91
• 92
• 93
• 94
• 95
• 96
• 97
• 98
• 99
• 100
• 101
• 102
• 103
• 104
• 105
• 106
• 107
• 108
• 109
• 110
• 111
• 112
• 113
• 114
• 115
• 116
• 117
• 118
• 119
• 120
• 121

在自定義的手眼標定場景文件中，攝像機相對于機器人TCP的關系如下：

<Frame name="Hand-Eye" refframe="UR.TCP"><RPY>0 -15 0</RPY><Pos>0.1 0 0</Pos>
</Frame>

• 1
• 2
• 3
• 4

轉換成4 ×

× 4的齊次變換矩陣如下：

ecH=??????0.96592600.25881900100?0.25881900.96592600.1001??????

ce?H=?????0.9659861360843613?0.00031871107048437940.25859366449327270?0.00038437325583683220.9999999054504882?0.00020336227483621180??0.25859357522955960.00029584162685441490.96598616725485550?0.10012491158556857.92132967571962e?053.021520822331239e?051??????
通過對比以上兩組齊次變換矩陣結果，可以兩者是十分接近的，證明手眼標定結果是正確的。

wcH?cgHi=weHi?egH

cw?H?gc?Hj?=ew?Hj??ge?H
其中：

• wcH

• ge?H 是一個常量矩陣。

改寫以上公式：
(weHi)?1?wcH?cgHi=egH

AX=XB
以上方程是Eye-to-Hand標定的最終求解方程，具體求解方式可以參考Tasi and Lenz，1989的論文A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration。

具體的eye-to-hand求解仿真可以參考eye-in-hand標定求解仿真進行，這里不再贅述