雙目視覺(jué)手眼標(biāo)定的精度優(yōu)化*

岳有軍,李勇飛,趙 輝,王紅君

(天津理工大學(xué) a.天津市復(fù)雜系統(tǒng)控制理論與應(yīng)用重點(diǎn)實(shí)驗(yàn)室;b.電氣工程與自動(dòng)化學(xué)院,天津 300384)

0 引言

聚晶金剛石復(fù)合片鉆頭(polycrystalline diamond compact bit,PDC)是一種強(qiáng)度、耐磨性、韌性、各向同性都極佳并且導(dǎo)熱率也相對(duì)較好的一種鉆井工具,適用于軟地層和中硬地層,被用來(lái)作為石油行業(yè)的鉆井工具,極大地提高了鉆井速度和破巖效率,使得鉆井周期大大減小。但PDC鉆頭在復(fù)底層鉆井中經(jīng)常有磨損嚴(yán)重、掉齒、崩刃、復(fù)合片脫落等現(xiàn)象發(fā)生,這造成了鉆頭壽命降低甚至失效的結(jié)果[1]。PDC鉆頭造價(jià)昂貴,需要采取保護(hù)措施,延長(zhǎng)鉆頭的壽命。目前的采用的方式是在PDC鉆頭受力和齒磨損最大的胎體冠狀部堆焊一層碳化鎢,提升其耐磨性[2]。為了確保鋼體鉆頭的敷焊成品質(zhì)量,使用雙目視覺(jué)焊接機(jī)器人對(duì)其進(jìn)行表面敷焊,敷焊對(duì)機(jī)械臂定位精度要求極高,為了提高定位精度,對(duì)Eye-To-Hand型手眼標(biāo)定方程的求解進(jìn)行了算法優(yōu)化研究。

費(fèi)致根等[3]采用了非線性優(yōu)化算法Adam優(yōu)化手眼標(biāo)定矩陣,其非線性算法的精確度優(yōu)于線性算法,但并未解決奇異解問(wèn)題且容易陷入局部最優(yōu)解;李巍等[4]用四元數(shù)參數(shù)化旋轉(zhuǎn)矩陣,LMI凸松弛化全局的方法解決了非線性算法容易陷入局部最優(yōu)解的問(wèn)題,但精度并沒(méi)有明顯的提升;馬清華等[5]用矩陣直積法計(jì)算出了手眼標(biāo)定矩陣,以最小化重投影誤差為代價(jià)函數(shù)優(yōu)化了手眼標(biāo)定矩陣,該方式具有良好的穩(wěn)定性和準(zhǔn)確性,但以復(fù)雜繁瑣的重投影誤差函數(shù)作為代價(jià)函數(shù),迭代速度慢,標(biāo)定過(guò)程耗時(shí)過(guò)長(zhǎng);TSAI等[6]使用一種線性算法,將旋轉(zhuǎn)和平移分開計(jì)算極大地提高了計(jì)算效率,標(biāo)定速度極快,缺點(diǎn)是只有一組數(shù)據(jù)參與,容易被偶然誤差影響,導(dǎo)致精度不高。王龍等[7]提出一種基于LMI優(yōu)化的對(duì)偶四元數(shù)手眼標(biāo)定算法,用線性矩陣不等式代替了奇異值分解法,最后通過(guò)實(shí)驗(yàn)證明了精度有所提升,同樣計(jì)算過(guò)程復(fù)雜,迭代效率低。吳慶華等[8]基于矩陣奇異值分解求取手眼標(biāo)定矩陣,但在數(shù)據(jù)測(cè)量時(shí)需要先變換θ角,判斷測(cè)量數(shù)據(jù)是否可以用于標(biāo)定,提高了標(biāo)定精度,但需要先判斷數(shù)據(jù)是否可用。

針對(duì)目前線性算法容易受到偶然誤差干擾,非線性算法容易陷入局部最優(yōu)解以及奇異解的問(wèn)題,提出了一種全新的方法,以眼在手外作為試驗(yàn)平臺(tái),解決了手眼標(biāo)定矩陣AX=XB在使用Ada-delta過(guò)程中的梯度的求取問(wèn)題,在線性算法求解AX=XB的基礎(chǔ)上引入非線性算法,利用線性算法確定性的結(jié)果解決和改善了非線性算法奇異解和局部最優(yōu)解的問(wèn)題,利用非線性算法多數(shù)據(jù)的優(yōu)勢(shì)避免了線性算法容易被偶然誤差影響的問(wèn)題。Ada-delta具有自適應(yīng)的學(xué)習(xí)率,不易陷入局部最優(yōu)解,用于求AX=XB具有穩(wěn)定性較強(qiáng),抗干擾能力好,精確度更高的特點(diǎn)。

1 雙目視覺(jué)空間定位原理

雙目視覺(jué)定位本質(zhì)上是利用視差獲取距離信息,利用相平面坐標(biāo)獲取位置信息。如圖1雙目相機(jī)是利用左右相機(jī)與標(biāo)定點(diǎn)構(gòu)成兩個(gè)底邊共線的三角形,通過(guò)這兩個(gè)三角形之間幾何變換關(guān)系求取標(biāo)定點(diǎn)相對(duì)于雙目相機(jī)的位置,獲得標(biāo)定點(diǎn)在相機(jī)坐標(biāo)系的三維坐標(biāo)[9]。

圖1 雙目成像原理

根據(jù)三邊平行,兩三角形相似,可以由相似三角形的性質(zhì)得到點(diǎn)在相機(jī)坐標(biāo)系的三維坐標(biāo)與其在相平面成像點(diǎn)坐標(biāo)的關(guān)系式:

(1)

式中:(xL,z)和(xR,z)分別為點(diǎn)在左相平面和右相平面的成像點(diǎn)坐標(biāo),f為相機(jī)的焦距,B為基線,(X,Y,Z)為點(diǎn)在相機(jī)坐標(biāo)系中的三維坐標(biāo)[10]。

可以看出計(jì)算是默認(rèn)雙目相機(jī)X軸共軸,Y、Z軸平行,但在實(shí)際裝配中幾乎不可能做到這點(diǎn),同時(shí)相機(jī)鏡頭在制造過(guò)程也會(huì)不可避免的產(chǎn)生誤差,鏡頭誤差產(chǎn)生的徑向和切向畸變會(huì)使理論成像點(diǎn)與實(shí)際成像點(diǎn)落點(diǎn)不同。所以通過(guò)拍攝多張標(biāo)定板圖片,對(duì)左右相機(jī)在同一時(shí)刻拍攝的圖片進(jìn)行縱橫向?qū)Ρ?可以得到左右相機(jī)坐標(biāo)系之間的相對(duì)位姿關(guān)系,以及切徑向畸變數(shù)據(jù),即可修正左右相機(jī)之間的裝配誤差以及鏡頭本身的誤差[11]。

2 手眼標(biāo)定模型

圖2 手眼標(biāo)定模型

(2)

(3)

(4)

(5)

利用這一點(diǎn)聯(lián)立式(4)、式(5)可得:

(6)

(7)

令:

則式(7)就是AX=XB,只需求A、B的值就可得到機(jī)械臂基坐標(biāo)系相對(duì)于相機(jī)坐標(biāo)系的位姿轉(zhuǎn)換及矩陣X[13]。

可以分別轉(zhuǎn)化為:

(8)

(9)

由式(8)、式(9)可以看出A、B分別為標(biāo)定板坐標(biāo)系在相機(jī)坐標(biāo)系、機(jī)械臂末端坐標(biāo)系在基坐標(biāo)系第i到i+1次的位姿轉(zhuǎn)換矩陣,所以A可以從相機(jī)外參獲取,B可以直接讀取機(jī)械臂末端位姿獲得。

3 優(yōu)化算法

Ada-delta是非線性算法,非線性算法的優(yōu)點(diǎn)是可以通過(guò)大量的樣本數(shù)據(jù)計(jì)算獲得高精度的結(jié)果,其本質(zhì)上是通過(guò)求變量的梯度,然后反向傳播來(lái)向最低點(diǎn)前進(jìn),通過(guò)多次迭代最終到達(dá)最低點(diǎn)[14]。而Ada-delta算法在這個(gè)基礎(chǔ)上增設(shè)了自適應(yīng)學(xué)習(xí)率,能夠根據(jù)損失函數(shù)的梯度比值變化而自主調(diào)節(jié)學(xué)習(xí)率,確保了在開始迭代時(shí)的學(xué)習(xí)率穩(wěn)定變化,以及迭代末期學(xué)習(xí)率不會(huì)太小導(dǎo)致迭代效果差[15]。在迭代計(jì)算中,每一步迭代的內(nèi)容是最耗費(fèi)時(shí)間的,所以為了提高整體的效率,將4×4的位姿轉(zhuǎn)換矩陣分成旋轉(zhuǎn)矩陣和位姿矩陣兩部分進(jìn)行計(jì)算,先利用Ada-delta計(jì)算3×3的旋轉(zhuǎn)矩陣,然后利用已知的旋轉(zhuǎn)矩陣帶入到Ada-delta中求位移矩陣。使用Ada-delta求解AX=XB的最大問(wèn)題是全矩陣運(yùn)算式是無(wú)法求梯度的,針對(duì)這一點(diǎn)提出了一個(gè)解決方案,那就是將矩陣的元素分開求取,令,

HR(X)=ARXR-XRBR

式中:HR為姿態(tài)轉(zhuǎn)換矩陣,AR為標(biāo)定板兩次姿態(tài)之間的變換矩陣,BR機(jī)械臂兩次姿態(tài)之間的變換矩陣。

則

(10)

式中:HR(ω)全為0,可以得到:

(11)

由式(11)可看出只需要計(jì)算hR11、hR21、hR31就可以計(jì)算出所有的ω值,從而求出XR。

求平移矩陣需要將已知的旋轉(zhuǎn)矩陣帶入到位姿轉(zhuǎn)換矩陣的方程AX=XB中,得到同式(10)的方程組,由于旋轉(zhuǎn)矩陣已知,只需要求其中一個(gè)式子h14就可以得到平移矩陣的值。

h14=a11ω14+a12ω24+a13ω34+a14ω44-
b14ω11-b24ω12-b34ω13-b44ω14

(12)

式中:ω14,ω24,ω34組成分別為平移矩陣的XYZ值,根據(jù)位姿矩陣的組成X可知ω44的值為1。

(13)

將hR式帶入到Ada-delta中去迭代時(shí),方程中沒(méi)有非零的常數(shù)項(xiàng),顯然解不是唯一的,會(huì)產(chǎn)生通解和奇異解。將線性算法求出ω的初始值ω11,ω22,ω33代入到式(11)中,保證每個(gè)h都有兩個(gè)常數(shù)項(xiàng),以避免通解和奇異解的出現(xiàn),并且不會(huì)因?yàn)轭~外引入常數(shù)項(xiàng)而引起誤差。

Ada-delta算法是在梯度下降法的基礎(chǔ)上進(jìn)行了大量的優(yōu)化變動(dòng)而生成的一種能自動(dòng)生成并更新學(xué)習(xí)率的優(yōu)良非線性算法,適合于處理大量數(shù)據(jù)、計(jì)算梯度可求的方程。

一是全面強(qiáng)化頂層設(shè)計(jì)，重大研究取得明顯進(jìn)展。組織調(diào)研論證，梳理出七大流域40個(gè)重大水利科技問(wèn)題。新增“東北四省區(qū)節(jié)水增糧高效灌溉技術(shù)研究與規(guī)?；痉丁钡?項(xiàng)國(guó)家科技支撐計(jì)劃項(xiàng)目?！熬扌蜋C(jī)組水電站建筑結(jié)構(gòu)關(guān)鍵技術(shù)”等4項(xiàng)科技成果獲國(guó)家科技進(jìn)步二等獎(jiǎng)，“強(qiáng)涌潮河口曹娥江大閘工程建設(shè)關(guān)鍵技術(shù)研究與實(shí)踐”等49項(xiàng)科技成果獲大禹水利科學(xué)技術(shù)獎(jiǎng)。

Ada-delta算法主要內(nèi)容是:

建立目標(biāo)函數(shù)h(ω),損失函數(shù)loss(x)為預(yù)測(cè)值減真實(shí)值的方差,設(shè)置學(xué)習(xí)率lr的更新參數(shù)S,delta的更新權(quán)重;

設(shè)置S、delte的初始值,通過(guò)對(duì)loss函數(shù)求的梯度的初始值分配權(quán)重對(duì)S、delta更新,利用更新的S、delta求得合適的學(xué)習(xí)率lr;

利用當(dāng)前x對(duì)應(yīng)的梯度與學(xué)習(xí)率求得ω的參數(shù)更新量,對(duì)ω進(jìn)行更新,一直迭代,直到loss函數(shù)小于設(shè)定值或到達(dá)迭代次數(shù)后停止。

過(guò)程代碼:

(1)設(shè)置目標(biāo)函數(shù):h(x)=x1ω1+x2ω2+x3ω3-x4ω4-x5ω5-x6ω6;預(yù)測(cè)函數(shù):pred(x);損失函數(shù):loss(x)=(pred-y)2;

(2)eps=0.001,更新ω=ω+eps,利用更新后的ω求loss函數(shù),聯(lián)立更新前后的loss函數(shù)計(jì)算梯度:gradient=(loss_after-loss_before)/eps;

(3)令S、delta的初始值為0,更新權(quán)重設(shè)置為0.2,0.8,S=0.2*S+0.8*gradient2;lr=(delta+1×10-6)/(S+1×10-6);分子分母添加1e-6是防止初始的學(xué)習(xí)率為0;

(4)參數(shù)更新量:gradient_ω=lr1/2*gradient;ω=ω-gradient_ω;

(5)用更新后的ω更新delta放到下一次迭代中使用delta=0.2×delta+0.8×gradient2;

(6)迭代5000次之后,得到ω的值,就可以進(jìn)行下一個(gè)h式的計(jì)算。

4 仿真實(shí)驗(yàn)驗(yàn)證

4.1 雙目相機(jī)參數(shù)標(biāo)定

本次實(shí)驗(yàn)使用的雙目相機(jī)模組尺寸為38×38 mm,可輸出圖片像素最大為1924×1080,焦距6 mm,基線80 mm。手眼標(biāo)定之前需要先對(duì)相機(jī)進(jìn)行參數(shù)標(biāo)定和修正。使用OpenCV的16×16 mm棋盤格標(biāo)定板,角點(diǎn)為11×8,拍攝約50組標(biāo)定板圖片。

(a) 左相機(jī)圖片 (b) 右相機(jī)圖片圖3 標(biāo)定圖像

標(biāo)定結(jié)果如表1和表2所示。

表1 雙目相機(jī)內(nèi)部參數(shù) (mm)

表2 雙目相機(jī)外部參數(shù)

使用MATLAB內(nèi)置的stereo Camera Calibrator計(jì)算得到相機(jī)標(biāo)定誤差為0.11 pixel。

圖4 標(biāo)定板角點(diǎn)檢測(cè)

圖5 雙目相機(jī)標(biāo)定誤差

4.2 手眼標(biāo)定

本次實(shí)驗(yàn)使用的是雙目相機(jī)與機(jī)械臂分離且相對(duì)位姿確定的眼在手外型布置,實(shí)驗(yàn)平臺(tái)如圖6所示。

圖6 手眼標(biāo)定平臺(tái)

非線性算法需要大量的數(shù)據(jù)支撐計(jì)算,因此測(cè)取了59組手眼位姿的數(shù)據(jù),對(duì)其用排列組合的方式擴(kuò)充,共有58×59=3244組基礎(chǔ)數(shù)據(jù)。

4.3 數(shù)據(jù)結(jié)果比較

將數(shù)據(jù)代入到Tsai中,得到位姿轉(zhuǎn)換矩陣:

最終得ω11=-0.512 2,ω22=0.942 9,ω33=-0.525 9,將ω11,ω22,ω33代入非線性方法Ada-delta中,迭代5000次,隨著迭代次數(shù)的遞增,3個(gè)方程的損失函數(shù)及原函數(shù)最終都趨向于0,得到旋轉(zhuǎn)矩陣:

將旋轉(zhuǎn)矩陣帶入到位姿矩陣的求解方程中,迭代5000次,得到位姿矩陣:

由式(2)變換得:

(14)

將優(yōu)化前后的X代入,再代入手眼對(duì)應(yīng)的一組數(shù)據(jù)可以得到優(yōu)化前后的標(biāo)定板和機(jī)械臂末端的位姿關(guān)系。

式中:T1、T2分別為通過(guò)優(yōu)化前和優(yōu)化后的X解得的標(biāo)定板和機(jī)械臂末端的位姿轉(zhuǎn)換關(guān)系矩陣。

將優(yōu)化前后的X與獲得的T1、T2聯(lián)合代入相機(jī)與標(biāo)定板的位姿關(guān)系求出機(jī)械臂末端在機(jī)械臂基坐標(biāo)系中的理論位姿,與機(jī)械臂系統(tǒng)讀取的真實(shí)位姿作差得到一個(gè)差值矩陣,然后對(duì)差值矩陣的每個(gè)元素取絕對(duì)值求和,選取了15組數(shù)據(jù)進(jìn)行比較,比較結(jié)果如圖7所示。

圖7 優(yōu)化前后的Frobenius范數(shù)

優(yōu)化前的差值矩陣平均值為0.305 65,優(yōu)化后的平均值為0.520 717。

5 結(jié)論

在傳統(tǒng)手眼標(biāo)定算法中,線性算法由于只需一組圖像數(shù)據(jù),計(jì)算速度快,但同時(shí)容易受到偶然誤差的影響。非線性算法有大量的基礎(chǔ)數(shù)據(jù)做支撐,具有高精準(zhǔn)度、抗干擾能力強(qiáng)的優(yōu)點(diǎn),但容易陷入局部最優(yōu)解,并且由于方程AX=XB沒(méi)有常數(shù)項(xiàng),會(huì)面對(duì)奇異解的問(wèn)題。

將線性算法Tsai與非線性算法Ada-delta結(jié)合,先將一組數(shù)據(jù)帶入到Tsai中求解AX=XB,得到優(yōu)化前的手眼標(biāo)定矩陣X,然后將X中的3個(gè)對(duì)角線元素ω11,ω22,ω33帶入到Ada-delta算法中,求解出X其余的元素,得到優(yōu)化后的X。首先,Ada-delta算法的自適應(yīng)學(xué)習(xí)率在很大程度上避免了局部最優(yōu)解問(wèn)題;其次,在非線性算法中引入了常數(shù)項(xiàng),避免了產(chǎn)生奇異解的問(wèn)題;最后,大量圖像數(shù)據(jù)作為運(yùn)算數(shù)據(jù)的支撐,增強(qiáng)了算法的抗干擾能力和魯棒性,避免了偶然性誤差的影響。

實(shí)測(cè)結(jié)果也表明,基于Ada-delta的優(yōu)化算法得到的手眼標(biāo)定矩陣的精度比傳統(tǒng)線性算法得到的精度更高,且一致性較好。