單目機(jī)器人手眼標(biāo)定研究*

鄧仕超，梅 楓，高興宇，楊 龍，梁晨光

(桂林電子科技大學(xué)a.機(jī)電工程學(xué)院;b.廣西制造系統(tǒng)與先進(jìn)制造技術(shù)重點(diǎn)實(shí)驗(yàn)室，廣西 桂林 541004)

0 引言

機(jī)械手的手眼標(biāo)定基本任務(wù)是計(jì)算機(jī)器手末端抓手坐標(biāo)系與視覺傳感器坐標(biāo)系之間的相對齊次變換矩陣，實(shí)現(xiàn)通過視覺傳感器識別目標(biāo)并計(jì)算出目標(biāo)在機(jī)械手基坐標(biāo)系下的三維坐標(biāo)，最終實(shí)現(xiàn)機(jī)械手對目標(biāo)的抓取、焊接等操作。本文利用視覺傳感器與機(jī)器手相結(jié)合，估計(jì)機(jī)器手末端工具坐標(biāo)系到視覺傳感器坐標(biāo)系的轉(zhuǎn)換矩陣。視覺傳感器主要是由相機(jī)構(gòu)成，通過相機(jī)來確定被測物的二維坐標(biāo)，實(shí)現(xiàn)對物體坐標(biāo)的測量。機(jī)械手手眼標(biāo)定通常使用的“AX=XB”模型是Koide K等[1]在1989年首次提出的。在過去的30年里，出現(xiàn)了大量求解手眼標(biāo)定方程AX=XB的方法。一般來說求解方法可以分為兩大類：①分步法，先求解旋轉(zhuǎn)矩陣，再求解平移矩陣。如Wu J等[2]提出的基于四元數(shù)的求解手眼標(biāo)定轉(zhuǎn)換矩陣方法；Daniilidis K[3]提出的對偶四元數(shù)方法；Andreff N等[4]提出的基于希爾維斯特方程的求解方法。②同步法，同時(shí)求解旋轉(zhuǎn)和平移矩陣。如Gwak S等[5]提出的梯度/牛頓法；Wu J等[6]提出的線性矩陣不等式法。

每種算法都有不同的優(yōu)點(diǎn)和缺點(diǎn)，分步法在平移矩陣的精度比旋轉(zhuǎn)矩陣的精度高時(shí)，會把旋轉(zhuǎn)矩陣的誤差代入平移矩陣中，影響整體的手眼標(biāo)定精度。同步法可以得到很好的結(jié)果，但在數(shù)據(jù)迭代上會花費(fèi)大量的時(shí)間。同時(shí)不同的數(shù)學(xué)方法來求解手眼標(biāo)定方程過程中，由于機(jī)器正運(yùn)動(dòng)學(xué)建模、線結(jié)構(gòu)光視覺傳感器標(biāo)定等引入誤差，使得求解機(jī)械手末端坐標(biāo)系到視覺傳感器坐標(biāo)系的轉(zhuǎn)換矩陣較為復(fù)雜。為使手眼標(biāo)定的復(fù)雜度降低，本文基于四元數(shù)法提出了一種單位八元數(shù)方法來估計(jì)視覺傳感器到機(jī)械手的轉(zhuǎn)換矩陣，通過實(shí)驗(yàn)使用該方法使得手眼標(biāo)定的求解過程相對簡單，計(jì)算效率提高。

1 手眼標(biāo)定矩陣方程式的求解

單目機(jī)械手手眼標(biāo)定模型如圖1所示，為了方便手眼標(biāo)定，現(xiàn)建立如下坐標(biāo)系:機(jī)械手基坐標(biāo)和世界坐標(biāo)系重合Ow-xwywzw，機(jī)械手末端坐標(biāo)系Oe-xeyeze，攝像頭坐標(biāo)系Oc-xcyczc，標(biāo)定板坐標(biāo)系OG-xgygzg，手眼標(biāo)定方程式AX=XB化為齊次矩陣[7]為：

(1)

式中，Ra、Rb分別為機(jī)械手末端和相機(jī)坐標(biāo)系的3×3旋轉(zhuǎn)矩陣；Ta、Tb分別為對應(yīng)的3×1平移矩陣；Rx、Ta是代求解的機(jī)械手末端和相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣和平移矩陣。解耦得到旋轉(zhuǎn)公式、平移公式分別為：

Ra×RX=RX×Rb

(2)

Ra×TX+Ta=Rb×Tb+TX

(3)

圖1 手眼標(biāo)定結(jié)構(gòu)簡圖

1.1 基于3-D旋轉(zhuǎn)的最優(yōu)估計(jì)

使用ai、bi來表示機(jī)械手和相機(jī)在僅做旋轉(zhuǎn)運(yùn)動(dòng)時(shí)的一組轉(zhuǎn)換矩陣，由旋轉(zhuǎn)公式(2)，可得：

(4)

其中，

ai=[log(Rai)]^

(5)

bi=[log(Rbi)]^

(6)

通過對式(4)使用SVD(Singular Value Decomposittion)來求解可以得到旋轉(zhuǎn)矩陣RX，再把RX反代入式(3)可以得到平移矩陣TX，但使用分步求解的方法，會把旋轉(zhuǎn)矩陣的誤差代入到平移矩陣中[8]。因此，也可以使用迭代的方法，同時(shí)得到TX、RX。

(7)

但式(6)也不能達(dá)到局部最優(yōu)解[9]。我們使用一種新的視角去對手眼標(biāo)定方程AX=XB進(jìn)行4-D分析。

1.2 基于4-D旋轉(zhuǎn)優(yōu)化估計(jì)

旋轉(zhuǎn)矩陣R可以使用兩個(gè)四元數(shù)qL=(a,b,c,d)T,qR=(p,q,r,s)T來表示[10]。R、qL、qR的關(guān)系可以表示為：

R=RL(qL)×RR(qR)

(8)

式中，RL、RR使用四元數(shù)乘積來表示：

(9)

(10)

旋轉(zhuǎn)矩陣R通過式(8)進(jìn)行參數(shù),σ由單位八元數(shù)統(tǒng)一表示，通過求解其的封閉解來求出旋轉(zhuǎn)矩陣R和平移矩陣T。

(11)

由式子(11)的旋轉(zhuǎn)矩陣R可以表示為：

R=(c1,c2,c3,c4)

(12)

對矩陣R進(jìn)行列c1,c2,c3,c4建立相似的因式分解，則有：

(13)

根據(jù)式(11)、式(13)可以得到P1(σ),P2(σ),P3(σ),P4(σ)是4×8的矩陣，并滿足：

(14)

同理R也可以被分解為行并進(jìn)行因式分解

R=(σTQ1(σ)σTQ2(σ)σTQ3(σ)σTQ4(σ))T。

(15)

為了解決式(7)不能達(dá)到局部最優(yōu)解，把手眼標(biāo)定模型AX=XB從SO(3)轉(zhuǎn)到SO(4)上，進(jìn)行對手眼標(biāo)定方程的求解。由于AX=XB在SO(4)的求解與之前用于求解旋轉(zhuǎn)四元數(shù)分解的四元數(shù)旋轉(zhuǎn)分解QDR (quaternion decomposition from rotation ) 非常相似，所以可以在QDR的基礎(chǔ)上對旋轉(zhuǎn)八元數(shù)分解。

1.3 手眼標(biāo)定方程求解

設(shè)Ei、Zi為機(jī)械手和攝像機(jī)視覺傳感器的坐標(biāo)系在4-D下轉(zhuǎn)換矩陣，由式(2)，可得：

(16)

式中，Ei∈SO(4),Zi∈SO(4)其指數(shù)映射為6×6的斜對稱矩陣。

文中定義Ei、Zi如下所示：

Ei=(ei,1,ei,2,ei,3,ei,4)
Zi=(zi,1,zi,2,zi,3,zi,4)

(17)

將式(17)代入EiRx-RxZi，由矩陣的性質(zhì)可得：

(18)

由式(18)可以得到EiRx-RxZi=(Mi,1σ,Mi,2σ,Mi,3σ,Mi,4σ)，則有：

(19)

式中，每一個(gè)Gjk,i,j,k=1,2,3,4如下：

(20)

其中，參數(shù)矩陣Jjk,i∈R4×8。由式(17) ～ 式(20)可以求解最優(yōu)單位八元數(shù)。

(21)

式中，

(22)

式(21)中的σ是矩陣F的最小特征值對應(yīng)的特征向量，特征值與特征向量的關(guān)系有：

Fσ=λF,minσ

(23)

(24)

(25)

(26)

假設(shè)經(jīng)過測量得到Ei、Zi，構(gòu)建手眼標(biāo)定模型，由式(22)得到特征向量qL和qR，代入到式(8)進(jìn)而可以確定機(jī)械手末端與視覺傳感器坐標(biāo)系之間的轉(zhuǎn)換矩陣R。

1.4 手眼標(biāo)定方程SO(4)參數(shù)化

確定了手眼標(biāo)定模型AX=XB在SO(4)下的求解方法，下面在來考慮得到的坐標(biāo)系的轉(zhuǎn)換矩陣如何從SO(3)轉(zhuǎn)到SO(4)的求解。Thomas F[11]提出了矩陣從SO(3)到SO(4)的轉(zhuǎn)換關(guān)系為：

(27)

式中，ε滿足ε2=0，為了滿足等式，實(shí)際我們設(shè)ε=1/d，當(dāng)d>>1時(shí)，可以滿足等式，經(jīng)過實(shí)際的實(shí)驗(yàn)[12]我們?nèi)=1×107。

2 實(shí)驗(yàn)與結(jié)果

根據(jù)以上算法展開實(shí)驗(yàn)，通過如圖2所示的機(jī)器手進(jìn)行手眼標(biāo)定實(shí)驗(yàn)，六自由度機(jī)械手的重復(fù)定位精度優(yōu)于0.04 mm，相機(jī)的分辨率為2592～1944 ppi，相機(jī)工作時(shí)的焦距為12 mm，按上述方法進(jìn)行手眼標(biāo)定實(shí)驗(yàn)，實(shí)驗(yàn)的步驟為：

(1)通過移動(dòng)機(jī)械手獲取N組機(jī)械手末端和視覺傳感器坐標(biāo)系的坐標(biāo)，六自由度機(jī)械手的重復(fù)定位精度優(yōu)于0.04 mm，為了保證穩(wěn)定性和實(shí)驗(yàn)準(zhǔn)確度，應(yīng)使N>3。

(2)將得到Ai、Bi代入式(27)，將數(shù)據(jù)從SO(3)轉(zhuǎn)到SO(4)上。

(3)通過式(16)～式(26)對AX=XB方程進(jìn)行求解，并將計(jì)算得到R通過式(27)轉(zhuǎn)到SO(3)上。

(4)把計(jì)算得到R進(jìn)行實(shí)際的應(yīng)用，計(jì)算精度。手眼標(biāo)定實(shí)驗(yàn)采用11×8棋盤格作為相機(jī)標(biāo)定和手眼標(biāo)定的固定靶標(biāo)。在實(shí)驗(yàn)過程中，先對攝像頭使用棋盤格標(biāo)定，內(nèi)參和畸變系數(shù)如表1所示，再進(jìn)行手眼標(biāo)定實(shí)驗(yàn)，通過標(biāo)定板固定不動(dòng)，機(jī)器手移動(dòng)，分別記錄相機(jī)、機(jī)械手在不同位置下的外參和基坐標(biāo)到機(jī)械手末端的末端坐標(biāo)，為簡要描述手眼標(biāo)定的過程，本文摘取6個(gè)機(jī)器手位姿和相機(jī)外參數(shù)據(jù)如表2所示。

圖2 AX=XB手眼標(biāo)定實(shí)驗(yàn)場景

表1 相機(jī)內(nèi)參系數(shù)和畸變系數(shù)

表2 手眼標(biāo)定數(shù)據(jù)

從表2可知，任區(qū)2組可構(gòu)成一個(gè)手眼標(biāo)定方程，通過本文算法得到手眼關(guān)系矩陣為：

為了驗(yàn)證手眼關(guān)系矩陣的可靠性，重復(fù)進(jìn)行10組完全獨(dú)立的實(shí)驗(yàn)，通過機(jī)械手末端坐標(biāo)，手眼轉(zhuǎn)換矩陣，相機(jī)位姿公式計(jì)算得到該點(diǎn)在機(jī)械手基坐標(biāo)下的坐標(biāo)，通過與真實(shí)值進(jìn)行比較，其結(jié)果的三維坐標(biāo)對比，如圖3～圖5所示。

圖3 X軸坐標(biāo)比較

圖4 Y軸坐標(biāo)比較

圖5 Z軸坐標(biāo)比較

從圖中觀察根據(jù)手眼關(guān)系轉(zhuǎn)換矩陣計(jì)算的結(jié)果與實(shí)際點(diǎn)的X、Y的坐標(biāo)誤差在視覺傳感器對的工作范圍內(nèi)可以達(dá)到0.5 mm以內(nèi)，基本滿足要求，而Z軸的誤差較大的原因是為了避免機(jī)器手碰到實(shí)驗(yàn)臺，在測量實(shí)際值時(shí)引入一定的誤差導(dǎo)致的。誤差主要是由于實(shí)際點(diǎn)坐標(biāo)在使用機(jī)器手測量時(shí)不能百分百的保證對準(zhǔn)測量點(diǎn)的人為誤差，該誤差不可以避免但可以盡可能減少。另一方面，相機(jī)標(biāo)定的內(nèi)參、畸變系數(shù)、外參求解和手眼標(biāo)定模型的求解，在計(jì)算過程中會產(chǎn)生誤差，這對測量點(diǎn)的坐標(biāo)產(chǎn)生誤差，但標(biāo)定產(chǎn)生的誤差非常小，這樣的計(jì)算誤差是可以接受的。綜合上述實(shí)驗(yàn)分析，本文的算法具有精度高、魯棒性好的能力。

3 結(jié)論

本文構(gòu)建以一種基于單目機(jī)械手手眼關(guān)系標(biāo)定的測量系統(tǒng)，利用棋盤靶標(biāo)作為參考物，使用單位八元數(shù)方法來求解手眼標(biāo)定模型AX=XB，來確定相機(jī)和機(jī)械手末端之間相對姿態(tài)的轉(zhuǎn)換方法。本算法最大限度避免了旋轉(zhuǎn)矩陣誤差代入到平移矩陣中，該算法比手眼標(biāo)定傳統(tǒng)四元數(shù)算法的速度快，且魯棒性好。最后通過測量棋盤格板定板進(jìn)行精度驗(yàn)證，實(shí)驗(yàn)結(jié)果表明系統(tǒng)具有良好的識別能力、較高精度和穩(wěn)定性，滿足實(shí)際的應(yīng)用要求。但單相機(jī)對檢測物體三維空間坐標(biāo)的精度不高，需要配合更多的傳感器，后續(xù)將繼續(xù)開展相關(guān)工作。