基于視覺引導(dǎo)的機(jī)械臂對接規(guī)劃

顏小豪, 劉 楠, 王 毅, 陶 佳, 楊長輝

(1.重慶理工大學(xué) 機(jī)械工程學(xué)院,重慶 400054,E-mail:a15310882290@163.com;2.軍事科學(xué)院 系統(tǒng)工程研究院,北京 100071)

隨著勞動力成本的不斷提升,各行業(yè)對于無人化、智能化的需求越來越多,機(jī)器人的應(yīng)用也越來越廣泛,一些需要機(jī)械臂完成對接操作的工作任務(wù)也不斷涌現(xiàn)。如在無人加油站中,通過機(jī)器人給機(jī)動車自動加油就需要機(jī)械臂驅(qū)動油槍與機(jī)動車油箱實(shí)現(xiàn)對接[1]。普通設(shè)備都是采用標(biāo)準(zhǔn)化的結(jié)構(gòu),而針對一些非標(biāo)準(zhǔn)化的結(jié)構(gòu)就需要設(shè)計(jì)針對性的識別及對接方法。

劉崇偉等針對大型運(yùn)輸設(shè)備上的電池充電補(bǔ)給任務(wù),設(shè)計(jì)了一款剛?cè)峤Y(jié)合充電機(jī)械臂,實(shí)現(xiàn)充電補(bǔ)給時安全柔順的對接[1]。劉金國等人為在軌機(jī)器人設(shè)計(jì)了一種新型末端執(zhí)行器用于空間站在軌加油對接作業(yè),該末端執(zhí)行器可實(shí)現(xiàn)快速隔離開關(guān)在管道中的自動對接[2]。王旭峰等為準(zhǔn)確獲取無人機(jī)自主空中加油對接時受油插頭與加油錐套的相對位姿信息,提出一種機(jī)器視覺輔助的插頭錐套式無人機(jī)自主空中加油方案[3]。Gregory P. Scott等人設(shè)計(jì)了一種機(jī)器人加油系統(tǒng)用于在海上為海軍軍艦等特殊設(shè)備加油[4]。Tommaso Gasparetto等人提出了一個浮動機(jī)器人仿真平臺,用于模擬演示衛(wèi)星在軌移動性。并且為機(jī)器人設(shè)計(jì)和開發(fā)了對接機(jī)構(gòu),以實(shí)現(xiàn)對接過程自動化等[5]。目前針對機(jī)械臂作業(yè)過程中的對接操作,大都采用設(shè)計(jì)對應(yīng)的對接機(jī)構(gòu)或輔助對接方法,而對視覺引導(dǎo)以及機(jī)械臂對接姿態(tài)規(guī)劃較少,本文針對一種特定環(huán)境下的對接作業(yè),采用視覺技術(shù)提高機(jī)器人的識別和定位精度,設(shè)計(jì)相應(yīng)的識別方法,對機(jī)械臂完成對接作業(yè)時的姿態(tài)進(jìn)行研究,并規(guī)劃其對接姿態(tài)。

本文針對的目標(biāo)是針對某特種車輛的油箱進(jìn)行自動加油對接作業(yè)的環(huán)境,這種環(huán)境下,由于待加油車輛每次停放位置都存在差別,因此每次加油對接任務(wù)前,都需要對特種車輛油箱加油口進(jìn)行識別和定位,在此基礎(chǔ)上驅(qū)動機(jī)械臂完成對接動作,同時機(jī)械臂對接作業(yè)時需要保證末端執(zhí)行器與加油口平面法向向量方向一致。為了解決這類問題,本文提出了一種基于RGB-D視覺引導(dǎo)的六自由度加油機(jī)械臂對接方法,通過RGB-D相機(jī)識別對接口卡槽,再通過第五關(guān)節(jié)分離法規(guī)劃機(jī)械臂對接,實(shí)現(xiàn)對某特種車輛油箱加油口的順利對接,并通過實(shí)驗(yàn)驗(yàn)證視覺識別的準(zhǔn)確性和基于該方法規(guī)劃的機(jī)械臂對接作業(yè)姿態(tài)的適用性。

1 材料與方法

1.1 加油過程與對接口

本文針對某特種車輛油箱進(jìn)行自動加油對接的作業(yè)環(huán)境,某特種車輛油箱示意如圖1。在此環(huán)境下加油機(jī)械臂執(zhí)行對接作業(yè)時需要末端執(zhí)行器與加油口平面法向向量方向一致才能順利完成對接動作,如圖2所示。

圖1 油箱示意圖

圖2 作業(yè)姿態(tài)示意圖

本文所使用的某特種車輛油箱對接口如圖3所示,對接時需由陽端卡槽對準(zhǔn)陰端卡齒,陰端前進(jìn)的同時勻速旋轉(zhuǎn)120°完成對接,如圖4 對接頭對接示意圖。該對接頭陽端最顯著的特征為多個同心圓組成的對接口表面和3個與對接口表面形成鮮明對比的卡槽。

圖3 對接口示意圖

圖4 對接頭對接示意圖

1.2 基于RGB-D相機(jī)的對接口識別方法

加油機(jī)械臂對接時需要提前獲取目標(biāo)的位姿信息,為了能適用該機(jī)械臂的工作環(huán)境,本文采用了RealSense D435i型RGB-D相機(jī)及環(huán)形光源搭建了機(jī)械臂的視覺模塊,環(huán)形光源中心嵌入相機(jī),并將其搭載在末端執(zhí)行器上,如圖5。

圖5 末端執(zhí)行器安裝光源、相機(jī)、輸油管

根據(jù)對接口的特征,本文首先通過識別三個卡槽,再利用卡槽計(jì)算油口旋轉(zhuǎn)角度,從而得到目標(biāo)對接口的位置。

由于對接口所處環(huán)境較為復(fù)雜,為了能準(zhǔn)確檢測出對接口的角點(diǎn)信息,首先需要對對接口進(jìn)行初步定位,確定其感興趣區(qū)域ROI(region of interest),采用MeanShift均值遷移模糊算法進(jìn)行邊緣平滑濾波,中和色彩分布相近的顏色,侵蝕掉圖像中面積較小的顏色區(qū)域。對處理之后的圖像采用霍夫梯度法,設(shè)置合適的迭代半徑,即得到圓盤的大致圓形輪廓如圖6所示。

圖6 對接口的初步定位

得到對接口的初步信息后采用基于灰度圖像的Shi-Tomasi角點(diǎn)檢測算法[17],采用亞像素角點(diǎn)處理,得到如圖7所示。通過對檢測出的角點(diǎn)位置信息進(jìn)行計(jì)算,就可以得到相應(yīng)的對接口中心位置信息。

圖7 對接口中心點(diǎn)

相機(jī)下獲取到的對接口中心位置信息還需轉(zhuǎn)換成機(jī)械臂基座標(biāo)系下的位置信息。首先采用張正友標(biāo)定法[18]對相機(jī)進(jìn)行內(nèi)參標(biāo)定,得到該相機(jī)的內(nèi)參矩陣為:

再使用相機(jī)自帶的SDK對該相機(jī)的兩個紅外攝像頭之間的參數(shù)進(jìn)行標(biāo)定,得到旋轉(zhuǎn)矩陣和平移矩陣。最后將彩色圖像與深度圖像進(jìn)行映射,就可以得到對接中心的深度信息,進(jìn)而得到油口中心(Xcenter,Ycenter)在相機(jī)坐標(biāo)系下的位置(Xco,Yco,Zco)。根據(jù)第五關(guān)節(jié)分離法可知對接中心的姿態(tài)信息,從而得到相機(jī)坐標(biāo)系下的對接口中心的位姿。

最后采用Tsai-Lenz眼在手上的手眼標(biāo)定算法[19],對相機(jī)與機(jī)械臂之間的轉(zhuǎn)換關(guān)系進(jìn)行標(biāo)定,從而得到機(jī)械臂基座標(biāo)系下的目標(biāo)位置信息。

1.3 基于第五關(guān)節(jié)分離法的加油機(jī)械臂對接姿態(tài)規(guī)劃

為保證機(jī)械臂帶動末端執(zhí)行器進(jìn)行加油對接時的姿態(tài)滿足加油口對接要求,本文通過第五關(guān)節(jié)分離法對加油機(jī)械臂的對接姿態(tài)進(jìn)行規(guī)劃。第五關(guān)節(jié)分離法即根據(jù)機(jī)械臂的結(jié)構(gòu)形式,在機(jī)械臂的第五關(guān)節(jié)處把機(jī)械臂分成機(jī)械臂本體與末端執(zhí)行器兩部分,通過分別對這兩部分進(jìn)行分析求得機(jī)械臂對接姿態(tài)。第五關(guān)節(jié)分離法大都應(yīng)用于腕關(guān)節(jié)處三個軸線交于一點(diǎn)的六自由度機(jī)械臂,并且在使用時需要對末端執(zhí)行器的姿態(tài)進(jìn)行規(guī)劃,而本文基于第五關(guān)節(jié)分離法提出一種腕關(guān)節(jié)處三個軸線相互垂直的機(jī)械臂進(jìn)行姿態(tài)規(guī)劃。

使用第五關(guān)節(jié)分離法對機(jī)械臂進(jìn)行姿態(tài)規(guī)劃就可以簡化對于末端執(zhí)行器姿態(tài)分析,進(jìn)而減少機(jī)械臂對接所需時間、優(yōu)化對接作業(yè)流程?；诘谖尻P(guān)節(jié)分離法規(guī)劃對接姿態(tài)時,在機(jī)械臂的第五關(guān)節(jié)處把機(jī)械臂分成機(jī)械臂本體與末端執(zhí)行器兩部分,如圖8所示。

圖8 基于第五關(guān)節(jié)法規(guī)劃機(jī)械臂對接姿態(tài)

首先根據(jù)相機(jī)識別到的目標(biāo)姿態(tài)逆解第五關(guān)節(jié)的信息,得到末端執(zhí)行器部分的位姿信息,之后固定末端執(zhí)行器部分的姿態(tài),再對機(jī)械臂本體部分正運(yùn)動學(xué)求解,從而求出整個機(jī)械臂對接時的姿態(tài)信息,最后驅(qū)動機(jī)械臂完成對接。完成一次對接后,后續(xù)針對同類車輛,由于其目標(biāo)姿態(tài)固定,再次對接時只需規(guī)劃機(jī)械臂本體部分姿態(tài)即可。

1.3.1 運(yùn)動學(xué)分析

通過對機(jī)械臂進(jìn)行正運(yùn)動學(xué)分析就可以知道機(jī)械臂末端到基座坐標(biāo)系的變換矩陣,該機(jī)械臂的D-H參數(shù)表如表1所示。依照該加機(jī)械臂結(jié)構(gòu)以及D-H參數(shù)表建立機(jī)械臂連桿坐標(biāo)系,如圖9。在標(biāo)準(zhǔn)D-H法中相鄰連桿坐標(biāo)系的齊次變換關(guān)系如式(1):

表1 機(jī)械臂D-H參數(shù)表

圖9 機(jī)械臂坐標(biāo)系

(1)

根據(jù)標(biāo)準(zhǔn)D-H法中相鄰連桿坐標(biāo)系的齊次變換關(guān)系式(1)以及該加油機(jī)械臂的D-H參數(shù)表可以算出該加油機(jī)械臂任意兩相鄰關(guān)節(jié)的轉(zhuǎn)換矩陣如式(2)所示為1和2關(guān)節(jié)的轉(zhuǎn)換矩陣。

(2)

(3)

由式(3)可得:

聯(lián)立以上兩式可得:

d4=(d6r23-py)cosθ1+(px-d6r13)sinθ1

令A(yù)=d6r23-py,B=px-d6r13,C=d4,則

(4)

其中:

D=a2cosθ2+a3cos (θ2-θ3)

E=-a2sinθ2-a3sin (θ2-θ3)

F=a2sin2θ2+a3sinθ2sin (θ2-θ3)+a2cos2θ2+

a3cosθ2cos (θ2-θ3)

G=sin (θ2-θ2+θ2)

H=cos (θ2-θ2+θ2)

1.3.2 仿真驗(yàn)證

使用MATLAB下的Robtics Toolbox工具箱對該機(jī)械臂進(jìn)行運(yùn)動學(xué)分析仿真驗(yàn)證。如圖10所示,為該機(jī)械臂處于初始狀態(tài),即機(jī)械臂關(guān)節(jié)角度為(0,0,0,0,0,0)時的機(jī)械臂模型。從圖10可以看出當(dāng)機(jī)械臂的關(guān)節(jié)角度為(0,0,0,0,0,0)時末端執(zhí)行器的位置為(0,-0.295,1.513)。當(dāng)任意選取一組機(jī)械臂關(guān)節(jié)數(shù)據(jù),例如(20°,30°,40°,60°,50°,60°)使用公式(3)可以計(jì)算出末端執(zhí)行器的位置為(-0.147,-0.332,1.436)與圖11仿真結(jié)果一致。

圖10 機(jī)械臂初始位置

圖11 任取關(guān)節(jié)角度的機(jī)械臂位置

假定圖9的初始位置為機(jī)械臂的初始狀態(tài),模擬三次不同位置使用第五關(guān)節(jié)分離法規(guī)劃機(jī)械臂對接姿態(tài)如圖12。仿真結(jié)果表明,本文對機(jī)械臂的運(yùn)動學(xué)分析正確,規(guī)劃機(jī)械臂對接姿態(tài)有較好的適用性。

圖12 模擬對接作業(yè)

2 結(jié)果與討論

2.1 實(shí)驗(yàn)平臺搭建與實(shí)驗(yàn)流程

為了驗(yàn)證對接口識別與定位的準(zhǔn)確性和第五關(guān)節(jié)分離法在此類情況下規(guī)劃機(jī)械臂對接姿態(tài)的適用性,本文搭建了機(jī)械臂對接作業(yè)實(shí)驗(yàn)平臺,并分別進(jìn)行是否采用第五關(guān)節(jié)分離法規(guī)劃對接姿態(tài)的實(shí)驗(yàn)。

本文使用的是Aubo-i10協(xié)作機(jī)械臂,搭載了特制的末端執(zhí)行器用于夾持對接裝置以及安裝相機(jī)。實(shí)驗(yàn)其它采用的設(shè)備有模擬目標(biāo)平臺、RealSense D435i型的RGB-D相機(jī)、處理器為R7-5800H的電腦等。該機(jī)械臂結(jié)構(gòu)以及搭建的實(shí)驗(yàn)環(huán)境如圖13所示。

圖13 機(jī)械臂實(shí)驗(yàn)平臺

整個機(jī)械臂對接作業(yè)實(shí)驗(yàn)流程如圖14所示。

圖14 機(jī)械臂對接作業(yè)實(shí)驗(yàn)流程圖

首先通過相機(jī)識別到目標(biāo)的位置信息,使用第五關(guān)節(jié)分離法規(guī)劃此類情況下機(jī)械臂姿態(tài)時,根據(jù)目標(biāo)對接口的固定姿態(tài)固定末端執(zhí)行器部分的姿態(tài),對機(jī)械臂本體部分即前五個關(guān)節(jié)規(guī)劃姿態(tài);而不使用第五關(guān)節(jié)法規(guī)劃對接姿態(tài)時,在獲取到目標(biāo)位置后,直接規(guī)劃整個機(jī)械臂的姿態(tài)。在第一次結(jié)束對接作業(yè)后,由于目標(biāo)的姿態(tài)不變,所以后續(xù)使用第五關(guān)節(jié)分離法時也只需獲取目標(biāo)位置后直接規(guī)劃機(jī)械臂本體即可。而不使用第五關(guān)節(jié)分離法則需循環(huán)整個流程。

2.2 對接口識別以及加油機(jī)械臂對接

假定從相機(jī)獲取目標(biāo)位置到末端執(zhí)行器到達(dá)目標(biāo)點(diǎn)為完成一次對接作業(yè)。如圖15實(shí)驗(yàn)過程所示:(a)相機(jī)獲取目標(biāo)位置;(b)機(jī)械臂帶動末端執(zhí)行器運(yùn)動到目標(biāo)位置;(c)機(jī)械臂帶動末端執(zhí)行器轉(zhuǎn)動到達(dá)目標(biāo)點(diǎn)。

圖15 實(shí)驗(yàn)過程示意圖

隨機(jī)設(shè)置十組不同的目標(biāo)對接口位置,在不同目標(biāo)對接口位置下,目標(biāo)對接口的姿態(tài)相同而位置不相同。相機(jī)分別識別十組目標(biāo)位置,并與示教器手動對接的位置進(jìn)行比較,xyz三個方向中任意一個的位置信息相機(jī)識別與示教的誤差在2 mm內(nèi)則滿足對接要求,測試結(jié)果如表2 視覺識別情況統(tǒng)計(jì)。在相機(jī)識別結(jié)果滿足對接要求的情況下,分別測試使用第五關(guān)節(jié)分離法完成一次對接作業(yè)時間和不使用第五關(guān)節(jié)分離法完成一次對接作業(yè)時間。實(shí)驗(yàn)結(jié)果如表3所示。

表2 視覺識別情況統(tǒng)計(jì)

表3 完成對接作業(yè)情況統(tǒng)計(jì)

2.3 實(shí)驗(yàn)結(jié)果討論

十組實(shí)驗(yàn)對于目標(biāo)位置的識別成功率為100%,十組相機(jī)識別目標(biāo)位置與示教目標(biāo)位置在xyz三個方向的比較誤差均在2 mm內(nèi),即滿足對接的要求。其中誤差達(dá)到2 mm的為兩組,四組誤差為1 mm。四組誤差小于1 mm。使用第五關(guān)節(jié)分離法規(guī)劃機(jī)械臂對接作業(yè)姿態(tài)的平均完成對接作業(yè)時間為9.95 s,而不使用第五關(guān)節(jié)分離法規(guī)劃機(jī)械臂對接作業(yè)姿態(tài)的平均完成對接作業(yè)時間為10.29 s。使用第五關(guān)節(jié)分離法規(guī)劃機(jī)械臂對接姿態(tài)與不使用第五關(guān)節(jié)分離法規(guī)劃機(jī)械臂對接姿態(tài)相比。使用第五關(guān)節(jié)分離法規(guī)劃對接姿態(tài)時,減少了3.3%完成對接作業(yè)所需時間。10組實(shí)驗(yàn)中,有8組減少了完成對接作業(yè)所需時間,1組與不使用第五關(guān)節(jié)分離法規(guī)劃機(jī)械臂對接姿態(tài)完成對接作業(yè)所需的時間持平,1組完成對接作業(yè)所需時間多出0.1 s。實(shí)驗(yàn)結(jié)果表明:在這種環(huán)境下該視覺識別方法對于該對接口識別效果良好,滿足對接的位置精度要求。使用第五關(guān)節(jié)分離法規(guī)劃該機(jī)械臂的對接姿態(tài)有較好的適用性,并且能有效減少機(jī)械臂完成對接作業(yè)的時間。

3 結(jié)論

本文針對一些特殊環(huán)境下的機(jī)械臂對接情況,此類環(huán)境下要求機(jī)械臂對接時末端執(zhí)行器姿態(tài)與目標(biāo)平面姿態(tài)一致。針對其對接口的特征設(shè)計(jì)了相對應(yīng)的識別方法,使用第五關(guān)節(jié)分離法規(guī)劃其對接姿態(tài)。分析了該機(jī)器人的正逆運(yùn)動學(xué)并使用MATLAB進(jìn)行了仿真驗(yàn)證。在實(shí)驗(yàn)室搭建了實(shí)驗(yàn)平臺,進(jìn)行視覺識別以及是否使用第五關(guān)節(jié)分離法的機(jī)械臂對接作業(yè)對比實(shí)驗(yàn),實(shí)驗(yàn)數(shù)據(jù)表明:十組實(shí)驗(yàn)識別成功率為100%,十組相機(jī)識別目標(biāo)位置與示教目標(biāo)位置在xyz三個方向的比較誤差均在2 mm內(nèi),即滿足對接的要求。使用第五關(guān)節(jié)分離法規(guī)劃機(jī)械臂姿態(tài)時機(jī)械臂完成對接作業(yè)平均完成時間為9.95 s,而不使用時完成對接作業(yè)的平均時間為10.29 s。使用第五關(guān)節(jié)分離法時,平均減少了3.3%的對接作業(yè)時間。結(jié)果表明:在這種環(huán)境下,針對該對接口的識別方法的識別準(zhǔn)確度達(dá)到對接要求,使用第五關(guān)節(jié)分離法規(guī)劃該機(jī)械臂的對接姿態(tài)有較好的適用性,并且能有效減少機(jī)械臂完成對接作業(yè)的時間。