當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

机器人手眼标定详解

發布時間：2023/12/31 编程问答 29 豆豆

生活随笔收集整理的這篇文章主要介紹了机器人手眼标定详解小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

手眼標定詳解

研究現狀

所謂手眼標定是統一視覺系統和機器人的坐標系，從而可以使視覺系統所確定的物體位姿可以轉換到機器人坐標系下，由機械臂完成對物體的作業。
最常見的手眼系統包括Eye-to-Hand和Eye-in-Hand兩種。在Eye-to-Hand手眼系統中，攝像機與機器人基座的位置是相對固定的，手眼關系式求解攝像機坐標系與機器人基座坐標系之間的轉換關系。在Eye-in-Hand手眼系統中，攝像機由于固定在機械臂末端，手眼關系求解的是攝像機坐標系與機械臂末端坐標系之間的轉換關系。在機器人處于不同的位置和姿態的情況下，獲取“眼”相對于標定物的關系。然后結合機器人的位姿（可以從機器人控制器上讀出），就能建立標定方程 $A X = X B$ ，求解方程就能得到手眼關系矩陣。在求解標定方程 $A X = X B$ 時，由于方程求解的非線性和不穩定，如何求得一個誤差小而且有意義的解是手眼標定的重要問題。關于如何求解的問題，就有了許多不同的手眼標定方法。對手眼換矩陣的求解主要有兩類方法。

一類方法利用旋轉矩陣的性質把非線性化的問題線性化。這類方法均包含兩個階段：首先通過線性化的方法求解手眼變換矩陣的旋轉部分，然后將旋轉部分的求解結果代入手眼方程，求解手眼矩降的平移部分，這類方法可以推導出簡單且求解方便的線性求解方程，但是由于是分兩步束解，旋轉矩陣參數的估計誤差會傳遞到平移向量中，因此對觀測數據中的噪聲敏感。這類方法可以得出唯一解的充分條件是至少有兩次機械臂運動的旋轉軸是非平行的。這類方法中Shui的方法是通過兩次移動機械臂來計算手眼變換矩陣。Tsai將問題分解成兩部分，即將旋轉平移矩陣分解成旋轉和平移兩部分進行求解，先求解旋轉矩陣，再將旋轉矩陣代入求解平移向量。在后來的研究中lee給出了簡化求解方程的方法，Strobl對此問題做了優化。Daniilidis采用對偶四元數來表示空間的運動。Chou和Kamel提出的方法基于奇異值分解SVD。對于在線手眼標定，Andreff提出的方法給出了一種新的手眼關系的線性化方程，以實現數學上的高效計算，滿足在線標定對實時性的需求。

另一類是采用優化的方法求解非線性問題。這類方法需要束解復雜的非線性優化問題，通常計算量相對較大，求解耗時，不能滿足在線實時計算的需求。而且非線性優化的結果依賴于初值的選擇，選擇不當的初值，會陷入局部最優，無法得到全局最優解。在這類方法中，zhuang將機器人手眼系統與機器人執行器作為一個整體進行建模，并應用非線性優化的方法求解旋轉矩陣和平移向量,以最小化手眼變換矩陣誤差的Frobenius范數作為優化目標。wei提出了一種方法，這種方法無需借助標定工具，且可以同時完成手眼標定和攝像機標定。Ilonen采用了標記點定位的方法，但需要已知攝像機內參。

通過調研，決定使用Chou和Kamel方法，該方法能實現所需功能，且實現過程相對簡潔，方便程序實現和維護。

問題描述

問題描述：本題目中的手眼系統為Eye-to-Hand。在eye to hand手眼系統中，采集了機器人N組位姿下, 末端坐標系{tool}在基坐標系{base}下位置及姿態T_b_t, 標定板坐標系{cal}在相機坐標系{cam}下的位置及姿態T_c_cal。求基坐標系的到相機坐標系齊次坐標變換 $^{bas}H_{cam}$ 。

求解說明

任意運動中,機器人末端到標定板的位姿關系 $^{tool}H_{cal}$ 保持不變，基坐標系到相機坐標系的位姿關系保持不變 $^{base}H_{cam}$ ，則有
$toolHcal=toolHbasei?baseHcami?camHcali=toolHbasej?baseHcamj?camHcalj,i≠j^{tool}H_{cal}=^{tool}H_{base}^i*^{base}H_{cam}^i*^{cam}H_{cal}^i=^{tool}H_{base}^j*^{base}H_{cam}^j*^{cam}H_{cal}^j,i\neq j$

其中 $i$ , $j$ 分別表示第 $i$ ， $j$ 次運動。由于 $^{base}H_{cam}^i=^{base}H_{cam}^j$ ，且始終不變，上式整理可得
$(toolHbasej)?1?toolHbasei?baseHcam=baseHcam?camHcalj?(camHcali)?1,i≠j(^{tool}H_{base}^j)^{-1}*^{tool}H_{base}^i*^{base}H_{cam}=^{base}H_{cam}*^{cam}H_{cal}^j*(^{cam}H_{cal}^i)^{-1},i\neq j$
因此有
$A X = X B$

$A=^{tool}H_{base}^i*(^{tool}H_{base}^j)^{-1}, \\ B=^{cam}H_{cal}^j*(^{cam}H_{cal}^i)^{-1}, \\ X=^{base}H_{cam}$

因此問題變為求解關于X的矩陣方程AX=XB。

求解AX=XB

求解AX=XB該問題使用Chou和Kamel方法提出的方法[1,2]。旋轉部分使用四元數表示
$RARX=RXRB?qA?qX=qX?qBR_AR_X=R_XR_B\Leftrightarrow q_A*q_X=q_X*q_B$

使用四元數乘法表示旋轉變換，重新構造線性系統
$qA?qX?qX?qB=qA?qX?qB￣?qX=(qA?qB￣)?qXq_A*q_X-q_X*q_B=q_A*q_X-\overline{q_B}*q_X=(q_A-\overline{q_B})*q_X$
因此有
$qX?qB=(x0?xTX(x0I+Sk(x))b)(b0b)q_X*q_B=\left(\begin{matrix} x_0 & -\mathbf{x}^T \\ \mathbf{X} & ( x_0\mathbf{I}+\text{S} k(\mathbf{x}) )\mathbf{b} \end{matrix}\right) \left(\begin{matrix} b0\\ \mathbf{b} \end{matrix}\right) \\$

$=(x0b0?xTbXb0+(x0I+Sk(x))b)=\left(\begin{matrix} x_0b_0 -\mathbf{x}^T\mathbf{b} \\ \mathbf{X}b_0 + ( x_0\mathbf{I}+\text{S} k(\mathbf{x}) )\mathbf{b} \end{matrix}\right)\\$

$=(b0x0?bTxbx0+(b0I?Sk(b))x)=\left(\begin{matrix} b_0x_0 -\mathbf{b}^T\mathbf{x} \\ \mathbf{b}x_0 + ( b_0\mathbf{I}-\text{S} k(\mathbf{b}) )\mathbf{x} \end{matrix}\right)\\$

$=(b0?bTb(b0I?Sk(b)))(x0x)=qB￣?qX=\left(\begin{matrix} b_0 & -\mathbf{b}^T \\ \mathbf{b} & ( b_0\mathbf{I}-\text{S} k(\mathbf{b})) \end{matrix}\right) \left(\begin{matrix} x_0\\ \mathbf{x} \end{matrix}\right)\\ =\overline{q_B}*q_X$
然后使用奇異值分解求解線性方程組,SVD分解實現參考第三版《Numerical Recipes》。

構建A，B矩陣時，任意兩個不同位置姿矩陣，可以構造一個方程，因此輸入數據的數量n與可構建方程數m的關系可以通過排列組合確定 $m=Cn2=n(n?1)/2m=C^2_n=n(n-1)/2$ 。可根據此關系及標定系統能處理的最大矩陣維數，設計標定功能需要考慮系統能處理的最大輸入數據量。

假設系統處理的最大矩陣維數為l,則有
$l=4?m=4?Cn2l=4*m=4*C^2_n$

可得
$n=(2+(4+8l))/4n=(2+\sqrt{(4+8l)})/4$

若輸入數據量過大，可以考慮另一種方法構造A,B矩陣，使用相鄰的兩個位姿構建A,B矩陣。則方程數 $m = n ? 1$ ,A,B矩陣的較大的維數是 $4 ? m = 4 n ? 4$ 。

仿真驗證

進行C++算法開發前，首先使用matlab可以較快實現，驗證選定方法及推導過程的正確性。使用matlab建立隨意建立一個機器人，設定好相機坐標系和標定板坐標系。然后采集不同位姿下，機器人末端在基坐標的位姿T_b_t，標定板坐標系在相機坐標系下的位姿T_c_cal。主程序如下，完整程序還包括solve_hand_eye_equation()子程序，該子程序是Chou和Kamel的方法的實現。除此外還有tsai， shiu, park, park, lu8, liang等人的方法對應的子程序tsai()， shiu(), park(), park(), lu8(), liang()，可以到這里下載【下載鏈接】。

%% %eye to hand 手眼標定驗證程序，以仿真驗證代替物理機器驗證。 %libing,2020.4.23 %% %建立機器人模型，獲得試驗數據，%驗證算法理論的正確性，再用于驗證C++程序的正確性。 clc clear clc; close('all'); %隨意建立一個機器人 PM_PI=3.1415926; deg=PM_PI/180; % theta d a alpha sigma ML1 = Link([ 0, 0.4967, 0, 0, 0], 'modified'); ML2 = Link([ 0, -0.18804, 0.2, 3*PM_PI/2, 0], 'modified'); ML3 = Link([ 0, 0.17248, 0.79876, 0 , 0], 'modified'); ML4 = Link([ 0, 0.98557, 0.25126, 3*PM_PI/2, 0], 'modified'); ML5 = Link([ 0, 0, 0, PM_PI/2 , 0], 'modified'); ML6 = Link([ 0, 0, 0, PM_PI/2 , 0], 'modified'); robot = SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','test\_robot'); robot.teach(); %可以自由拖動的關節角度 hold on; %基坐標系 H_base=transl(0,0,0); trplot(H_base,'frame','b','color','b'); hold on; %axis([-2,2,-2,2,-1,3]); %末端坐標系 T_b_t=zeros(4,4); theta=[0,0,0,0,0,0]; T_b_t(:,1:4)=robot.fkine(theta); fig_tool=trplot(T_b_t,'frame','t','color','b'); %設置相機坐標系（隨意設置，與標定結果對比） H_b_c=zeros(4,4); t=transl(1,1.5,2); H_b_c=t*trotx(15)*troty(30)*trotz(45); fprintf("設置相機位姿為\n"); disp(H_b_c); trplot(H_b_c,'frame','cam','color','g'); %設置機器人末端坐標系到標定板坐標系的變換（隨意設置） H_t_cal=zeros(4,4); t=transl(0.1,0.2,0.1); H_t_cal=t*trotx(15)*troty(30)*trotz(45); T_b_cal=T_b_t*H_t_cal; fig_cal=trplot(T_b_cal,'frame','cal','color','g');%采集手眼標定所需的數據數據(10個位姿) fp1=fopen('testq_T_b_t.txt','w'); fp2=fopen('testq_T_c_cal.txt','w'); for i=1:10J1=-6*i*deg;J2=-4.5*i*deg;J3=-4.5*i*deg;J4=3.0*i*deg;J5=4.5*i*deg;J6=3.0*i*deg;theta=[J1,J2,J3,J4,J5,J6];%采集手眼標定數據數據T_b_t(:,4*i-3:4*i)=robot.fkine(theta); %基座坐標系到機器人末端坐標系的變換T_b_cal=T_b_t(:,4*i-3:4*i)*H_t_cal; %由T_b_cal=T_b_c*T_c_cal導出T_c_calT_c_cal(:,4*i-3:4*i)=inv(H_b_c)*T_b_cal;%相機坐標系到標定板坐標系的變換 q1(1:3)= T_b_t(1:3,4*i);%寫入，用于C++程序測試q1(4:7)=rot2q(T_b_t(1:3,4*i-3:4*i-1));q2(1:3)= T_c_cal(1:3,4*i);%寫入，用于C++程序測試q2(4:7)=rot2q(T_c_cal(1:3,4*i-3:4*i-1));fprintf(fp1,'%.6f %.6f %.6f %.6f %.6f %.6f %.6f\n',...q1(1),q1(2),q1(3),q1(4),q1(5),q1(6),q1(7));fprintf(fp2,'%.6f %.6f %.6f %.6f %.6f %.6f %.6f\n',...q2(1),q2(2),q2(3),q2(4),q2(5),q2(6),q2(7));pause(0.2);delete(fig_tool);delete(fig_cal);fig_tool=trplot(T_b_t(:,4*i-3:4*i),'frame','t','color','r');%繪制末端坐標系fig_cal=trplot(T_b_cal,'frame','cal','color','g');%繪制標定板坐標系robot.plot(theta); end fclose(fp1); fclose(fp2); %利用采集的數據構造方程的系數矩陣A,B H_b_t=T_b_t; H_c_cal=T_c_cal; [m,n]=size(H_b_t); m=n/4; fp1=fopen('testA.txt','w'); fp2=fopen('testB.txt','w'); n=1; for i=1:m-1 j=i+1;A(:,4*i-3:4*i)=H_b_t(:,4*j-3:4*j)*inv(H_b_t(:,4*i-3:4*i));B(:,4*i-3:4*i)=H_c_cal(:,4*j-3:4*j)*inv(H_c_cal(:,4*i-3:4*i));testA(4*i-3:4*i,:)=A(:,4*i-3:4*i);%用于C++程序測試testB(4*i-3:4*i,:)=B(:,4*i-3:4*i);%用于C++程序測試for k=1:4fprintf(fp1,'%.6f %.6f %.6f %.6f\n',...testA(4*i-4+k,1),testA(4*i-4+k,2),...testA(4*i-4+k,3),testA(4*i-4+k,4));fprintf(fp2,'%.6f %.6f %.6f %.6f\n',...testB(4*i-4+k,1),testB(4*i-4+k,2),...testB(4*i-4+k,3),testB(4*i-4+k,4)); end end fclose(fp1); fclose(fp2); %求AX=XB解方程 fprintf("標定結果\n"); X1=solve_hand_eye_equation(A,B); %文獻的其他方法 X2=tsai(A,B) X3=shiu(A,B) X4=park(A,B) X5=lu8(A,B) X6=liang(A,B)

建立的機器人如下圖所示

matlab設置相機坐標系的相對于基坐標系位姿為

H_b_c=zeros(4,4); t=transl(1,1.5,2); H_b_c=t*trotx(15)*troty(30)*trotz(45);

計算得到對應的位姿為如下，這將與算法計算的結果進行對比。

H_b_c = 0.6124 -0.6124 0.5000 1.0000 0.7745 0.5915 -0.2241 1.5000 -0.1585 0.5245 0.8365 2.0000 0 0 0 1.0000

采集10組位姿數據，測試算法。計算結果如下，與設置的數據一致，驗證了算法的正確性。

設置相機位姿為0.6124 -0.6124 0.5000 1.00000.7745 0.5915 -0.2241 1.5000-0.1585 0.5245 0.8365 2.00000 0 0 1.0000 標定結果 X2 =0.6124 -0.6124 0.5000 1.00000.7745 0.5915 -0.2241 1.5000-0.1585 0.5245 0.8365 2.00000 0 0 1.0000 X3 =0.6124 -0.6124 0.5000 1.00000.7745 0.5915 -0.2241 1.5000-0.1585 0.5245 0.8365 2.00000 0 0 1.0000 X4 =0.6124 -0.6124 0.5000 1.00000.7745 0.5915 -0.2241 1.5000-0.1585 0.5245 0.8365 2.00000 0 0 1.0000 X5 =0.6124 -0.6124 0.5000 1.00000.7745 0.5915 -0.2241 1.5000-0.1585 0.5245 0.8365 2.00000 0 0 1.0000 X6 =0.6124 -0.6124 0.5000 1.00000.7745 0.5915 -0.2241 1.5000-0.1585 0.5245 0.8365 2.00000 0 0 1.0000

同時采集T_b_t，T_c_cal，testA，testB的數據，用于C++程序調試和驗證。
matlab程序參見solve_hand_eye_equation.m,test_robot.m。

C++程序開發

首先實現公用接口solve_hand_eye_equation(),求解AX=XB，該接口與機器人無關，作為一個通用函數。
使用matlab采集testA,testB的數據驗證。運行該功能的測試用例函數test_solve_hand_eye_equation(),結果如下

其次構造一個robot類，僅包含手眼標定功能，獲取相機坐標系功能，以及相機坐標系參數。

class robot { public:robot() = default;//默認構造函數robot(const robot& newRobot);//復制構造函數，方便創建新機器人int hand_eye_calibarate(int num, double T_b_t[][7], double T_c_cal[][7]);void get_hand_eye_param(double T[4][4]); private:double H_b_c[4][4] = { {0} };//基坐標系到相機固定坐標系的齊次變換//double DHparam[6][5];機器人DH參數 protected: };

最后實現手眼標定接口，接口主要實現把輸入數據的四元數形式轉換為旋轉矩陣，構造A,B矩陣，最后調用solve_hand_eye_equation()求解方程AX=XB。運行該功能的測試用例函數hand_eye_calibarate() 結果如下,與matlab仿真驗證的結果一致(使用matlab機器人模型采集的數據)，算法驗證完成。

T_b_t: 1.389353 -0.161673 -0.374760 -0.000012 -0.039367 -0.052134 0.997864 1.495724 -0.333834 -0.238036 -0.000190 -0.079298 -0.102923 0.991523 1.562537 -0.524060 -0.080913 -0.000948 -0.120277 -0.151058 0.981180 1.585289 -0.722849 0.093874 -0.002944 -0.162627 -0.195301 0.967161 1.561916 -0.919740 0.283137 -0.007023 -0.206449 -0.234523 0.949910 1.492980 -1.103947 0.483300 -0.014154 -0.251564 -0.267729 0.929966 1.381702 -1.265028 0.690486 -0.025352 -0.297486 -0.294087 0.907949 1.233814 -1.393543 0.900604 -0.041587 -0.343405 -0.312952 0.884537 1.057254 -1.481657 1.109443 -0.063695 -0.388197 -0.323880 0.860435 0.861714 -1.523652 1.312769 -0.092296 -0.430459 -0.326641 0.836356 T_c_cal: -0.474389 -2.366986 -1.315044 -0.022235 -0.050624 -0.034793 0.997864 -0.569053 -2.470300 -1.093639 -0.045221 -0.100774 -0.068418 0.991523 -0.707189 -2.547185 -0.871851 -0.069796 -0.149797 -0.099877 0.981180 -0.883184 -2.590179 -0.657082 -0.096807 -0.196832 -0.128393 0.967161 -1.088846 -2.593220 -0.456094 -0.127029 -0.240827 -0.153420 0.949910 -1.313870 -2.552169 -0.274656 -0.161076 -0.280563 -0.174650 0.929966 -1.546457 -2.465225 -0.117259 -0.199323 -0.314694 -0.192005 0.907949 -1.774056 -2.333193 0.013063 -0.241839 -0.341809 -0.205610 0.884537 -1.984178 -2.159575 0.114821 -0.288338 -0.360497 -0.215766 0.860435 -2.165222 -1.950474 0.188031 -0.338148 -0.369429 -0.222904 0.836356 ------------------標定前Tcam:---------------- 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 ------------------標定后Tcam:---------------- 0.612370 -0.612373 0.500003 0.999996 0.774521 0.591504 -0.224143 1.499992 -0.158495 0.524521 0.836515 1.999987 0.000000 0.000000 0.000000 1.000000

小結

（1）不管是eye-to-hand系統，還是eye-in-hand系統，問題的關鍵都是轉化為求解方程AX=XB。
（2）本文的所述方法不適用于scara機器人，這類方法可以得出唯一解的充分條件是至少有兩次機械臂運動的旋轉軸是非平行的。
（3）這里的仿真數據沒有加入噪聲，所以各種方法的計算結果都是一樣的。實際應用中各種方法的結果應該是有一定差異的。

參考資料

[1]Shah, R. D. Eastman, T. Hong, An Overview of Robot-Sensor Calibration Methods for Evaluation of Perception Systems, Performance Metrics for Intelligent Systems, (2012).

[2]J. C. K. Chou, M. Kamel Finding the Position and Orientation of a Sensor on a Robot Manipulator Using Quaternions. In The International Journal of Robotics Research, 10(3): 240-254, 1991.

[3]R. Tsai, R. Lenz A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration. In IEEE Transactions on Robotics and Automation, 5(3):345-358, 1989.

[4]程玉立. 面向工業應用的機器人手眼標定與物體定位[D].浙江大學,2016.

[5]張云珠. 工業機器人手眼標定技術研究[D].哈爾濱工程大學,2010.

r on a Robot Manipulator Using Quaternions. In The International Journal of Robotics Research, 10(3): 240-254, 1991.

[3]R. Tsai, R. Lenz A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration. In IEEE Transactions on Robotics and Automation, 5(3):345-358, 1989.

[4]程玉立. 面向工業應用的機器人手眼標定與物體定位[D].浙江大學,2016.

[5]張云珠. 工業機器人手眼標定技術研究[D].哈爾濱工程大學,2010.

[6]William H. Press, Saul A. Teukolsky. Numerical Recipes: The Art of Scientific Computing (3rd Edition)

總結

以上是生活随笔為你收集整理的机器人手眼标定详解的全部內容，希望文章能夠幫你解決所遇到的問題。

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

上一篇： PHP实现国密SM3算法
下一篇：《人工智能：一种现代方法(AIMA)》绪