水下自主作業(yè)機(jī)械臂軌跡規(guī)劃與抓取仿真

張祥

(中國船舶集團(tuán)有限公司第七一三研究所,河南鄭州 450015)

0 引言

水下自主作業(yè)[1]技術(shù)主要涉及智能裝備在水中通信與目標(biāo)探測識別、水下釋放、回收及動平衡作業(yè)等關(guān)鍵技術(shù),探索復(fù)雜流場對智能裝備運(yùn)動規(guī)律的影響,突破微光環(huán)境中目標(biāo)識別、基于智能控制算法的水下對接回收、智能規(guī)劃作業(yè)等關(guān)鍵技術(shù),為安全高效作業(yè)提供技術(shù)支撐。水下作業(yè)機(jī)械臂作為水下無人系統(tǒng)的重要組成部分,是執(zhí)行水下作業(yè)任務(wù)的關(guān)鍵執(zhí)行機(jī)構(gòu),是最終實(shí)現(xiàn)抓取任務(wù)的執(zhí)行機(jī)構(gòu)。本研究以水下無人系統(tǒng)中的水下作業(yè)機(jī)械臂為研究對象,設(shè)計(jì)機(jī)械臂的軌跡規(guī)劃與抓取控制方法,基于V-REP平臺開展機(jī)械臂自主作業(yè)試驗(yàn)進(jìn)行驗(yàn)證。

機(jī)械臂的水下作業(yè)是一個(gè)較為復(fù)雜的工作過程,其中涉及了大量控制算法的應(yīng)用,現(xiàn)有的算法開發(fā)與驗(yàn)證過程較繁瑣,開發(fā)周期長且研發(fā)成本高。V-REP是一個(gè)具有集成開發(fā)環(huán)境的機(jī)器人3D仿真平臺,用于機(jī)器人模型及其控制系統(tǒng)的快速算法開發(fā)、快速原型驗(yàn)證等[2]。V-REP提供了許多功能模塊,可以通過API接口和腳本功能集合和組合以滿足使用要求,可支持的編程語言包括Python、C/C++、Lua、MATLAB和Octave等。張世超等[3]利用VREP與MATLAB完成機(jī)器人加工仿真,在運(yùn)動仿真中進(jìn)行碰撞檢測和仿真軌跡點(diǎn)的誤差分析。任建新[4]等提出了一種基于Visual Studio與V-REP的聯(lián)合仿真方法,可以驗(yàn)證了機(jī)器人運(yùn)動學(xué)模型以及正逆解的準(zhǔn)確性。鄭怡廷[5]等以UR10工業(yè)機(jī)械臂為模型,通過Python語言編寫的操控軟件將UR10機(jī)械臂和V-REP平臺結(jié)合起來,以實(shí)現(xiàn)UR10可視化仿真,并進(jìn)行了仿真交互驗(yàn)證。

機(jī)械臂自主作業(yè)的任務(wù)是在探測到目標(biāo)物時(shí),通過控制末端執(zhí)行器手爪運(yùn)動到指定位置,確認(rèn)到達(dá)抓取位置后控制末端執(zhí)行器抓取目標(biāo)物,然后攜帶目標(biāo)物進(jìn)行一定高度的提升。為了完成抓取,首先需要進(jìn)行機(jī)械臂末端的軌跡規(guī)劃,生成從起始位置到抓取位置的目標(biāo)運(yùn)動點(diǎn)。然后,設(shè)計(jì)機(jī)械臂的運(yùn)動控制結(jié)構(gòu),便于在V-REP仿真平臺中進(jìn)行控制程序的開發(fā)。最終,借助機(jī)械臂、目標(biāo)物和環(huán)境模型,開展水下作業(yè)機(jī)械臂自主抓取實(shí)驗(yàn),驗(yàn)證方法的有效性。

1 機(jī)械臂末端軌跡規(guī)劃方法

機(jī)械臂在水下作業(yè)時(shí),利用其末端搭載的機(jī)械手完成夾持等操作。為了保證機(jī)械臂末端執(zhí)行器可以按照預(yù)先設(shè)定的連續(xù)路徑進(jìn)行作業(yè),需要在笛卡爾空間對機(jī)械臂末端進(jìn)行軌跡規(guī)劃,分為全局路徑規(guī)劃與局部軌跡插補(bǔ)兩個(gè)部分。

1.1 全局路徑規(guī)劃

由于機(jī)械臂末端執(zhí)行器和目標(biāo)物均有一定的形狀,在運(yùn)動過程中為了避免干涉,需要在路徑規(guī)劃時(shí)設(shè)計(jì)機(jī)械臂末端的運(yùn)動軌跡規(guī)避目標(biāo)物的外包絡(luò)。較為便捷的方法是在機(jī)械臂末端的運(yùn)動起始點(diǎn)和抓取位置終點(diǎn)之間,設(shè)置運(yùn)動過渡點(diǎn),其主要目的就是將機(jī)械臂抓取的運(yùn)動過程分為如下兩個(gè)階段。第一階段是由起始點(diǎn)運(yùn)動到過渡點(diǎn),這兩點(diǎn)間的運(yùn)動軌跡上沒有任何障礙物,因?yàn)榇诉\(yùn)動過程中距目標(biāo)物較遠(yuǎn),也不會與目標(biāo)物產(chǎn)生運(yùn)動干涉。第二階段是由過渡點(diǎn)運(yùn)動到抓取終點(diǎn),這時(shí)機(jī)械臂末端執(zhí)行器距離目標(biāo)物會越來越近,需要結(jié)合末端執(zhí)行器的形狀與目標(biāo)物的三維特征進(jìn)行局部規(guī)劃插補(bǔ),避免產(chǎn)生抓取時(shí)的碰撞。此外,考慮到機(jī)械臂在水下運(yùn)動時(shí)水動力特性的干擾,在設(shè)計(jì)期望軌跡時(shí)需要盡可能保證連續(xù)與平滑。綜上,運(yùn)動過渡點(diǎn)的設(shè)計(jì)需要符合下列約束:起始點(diǎn)到過渡點(diǎn)沒有任何障礙物;過渡點(diǎn)到終點(diǎn)避免與目標(biāo)物運(yùn)動干涉;期望的運(yùn)動軌跡平滑。

通過在V-REP仿真環(huán)境中搭建雙目視覺系統(tǒng),可以實(shí)時(shí)探測到目標(biāo)物的中心位置坐標(biāo)、外部形狀和尺寸。根據(jù)目標(biāo)物的測量信息,并且在運(yùn)動過渡點(diǎn)的設(shè)計(jì)約束下,通過A*算法搜索全局運(yùn)動路徑。A*算法是一種在已知地圖信息的條件下求解運(yùn)動路徑的搜索方法,其通常在二維空間進(jìn)行路徑搜索,而機(jī)械臂抓取目標(biāo)物的操作是在三維空間進(jìn)行的。因此,在搜索時(shí)以V-REP俯視圖作為搜索地圖。

A*算法搜索時(shí),需要將待搜索的區(qū)域簡化為二維圖形空間,如圖1所示,其中灰色區(qū)域代表障礙物,黃色區(qū)域?yàn)樗阉髀窂?。設(shè)置兩個(gè)表open和closed,搜索步驟如下:

圖1 A＊全局搜索路徑Fig.1 Global search path of A＊ algorithm

(1)計(jì)算初始節(jié)點(diǎn)和其周圍節(jié)點(diǎn)的代價(jià)估計(jì)F(n)=g(n)+h(n),其中g(shù)(n)為實(shí)際代價(jià),其值為當(dāng)前節(jié)點(diǎn)到起始點(diǎn)的距離,h(n)為估計(jì)代價(jià),其值為當(dāng)前節(jié)點(diǎn)到終點(diǎn)的距離,并將初始節(jié)點(diǎn)周圍節(jié)點(diǎn)放入到open列表中;

(2)選取open列表中F(n)值最小的點(diǎn)n,計(jì)算其周圍節(jié)點(diǎn)x的F值;

(3)如果節(jié)點(diǎn)x已經(jīng)在表open中,并且重新計(jì)算的F值小于原值,則將節(jié)點(diǎn)n設(shè)置為節(jié)點(diǎn)x的父節(jié)點(diǎn),并且更新表open中節(jié)點(diǎn)x的F值;如果節(jié)點(diǎn)x不在表open中,則將節(jié)點(diǎn)n設(shè)置為節(jié)點(diǎn)x的父節(jié)點(diǎn),并求解節(jié)點(diǎn)x的F值添加到表open中;

(4)將節(jié)點(diǎn)n添加到表closed中;

(5)判斷終點(diǎn)是否已經(jīng)在表open中,如果沒有則返回步驟b繼續(xù)搜索,如果存在則搜索結(jié)束,其路徑為從終點(diǎn)開始,每個(gè)節(jié)點(diǎn)沿著父節(jié)點(diǎn)移動至起點(diǎn)。

從過渡點(diǎn)到抓取終點(diǎn)的運(yùn)動路徑需要保證二維特性,即過渡點(diǎn)與抓取終點(diǎn)在Z軸方向上的坐標(biāo)值需保持一致,由此過渡點(diǎn)在Z軸方向上的位置可以確定。對于過渡點(diǎn)的X軸、Y軸坐標(biāo)值,可以選取A*搜索路徑上距目標(biāo)物外圍最近一點(diǎn)的值。

1.2 局部軌跡插補(bǔ)

全局路徑規(guī)劃確定了機(jī)械臂抓取運(yùn)動過程中的起始點(diǎn)、過渡點(diǎn)和終點(diǎn),然而在實(shí)際運(yùn)動控制程序開發(fā)中,只有這三個(gè)點(diǎn)無法進(jìn)行完整的位置跟蹤和速度控制,因?yàn)槿笔е虚g過程的位置坐標(biāo)。因此,需要對整個(gè)運(yùn)動軌跡進(jìn)行插補(bǔ),按照時(shí)間序列生成一系列運(yùn)動目標(biāo)點(diǎn),輸入到運(yùn)動控制器中進(jìn)行跟蹤。插補(bǔ)內(nèi)容包括從起始點(diǎn)到過渡點(diǎn)、從過渡點(diǎn)到終點(diǎn)兩個(gè)階段的位置坐標(biāo)插補(bǔ)。常用的軌跡插補(bǔ)有直線插補(bǔ)和圓弧插補(bǔ),其他軌跡都可以通過直線或者圓弧進(jìn)行逼近。為了實(shí)現(xiàn)機(jī)械臂的快速抓取并避免與目標(biāo)物的運(yùn)動干涉,本文選取直線插補(bǔ)進(jìn)行局部軌跡規(guī)劃。

基于機(jī)械臂抓取任務(wù)的直線插補(bǔ)可以按照如下流程進(jìn)行:

(1)設(shè)定機(jī)械臂末端期望的運(yùn)動速度v和控制程序單次運(yùn)行的周期Ts;

(2)計(jì)算機(jī)械臂抓取運(yùn)動過程中的起始點(diǎn)到過渡點(diǎn)的距離:

(3)求起始點(diǎn)到過渡點(diǎn)插補(bǔ)的總時(shí)間T:

(4)計(jì)算插補(bǔ)的總次數(shù)N。當(dāng)T/Ts的余數(shù)為零時(shí),N=ent(T/Ts);當(dāng)余數(shù)不為零時(shí),N=ent(T/Ts)+1,其中ent函數(shù)表示取整。

(5)計(jì)算插補(bǔ)增量:

(6)計(jì)算第i點(diǎn)的插補(bǔ)坐標(biāo):

對于機(jī)械臂抓取運(yùn)動過程中的過渡點(diǎn)到終點(diǎn)的插補(bǔ),參照以上流程進(jìn)行計(jì)算。

2 抓取控制結(jié)構(gòu)

機(jī)械臂在水下作業(yè)時(shí),主要的工作內(nèi)容為利用末端機(jī)械手,對水下識別到的目標(biāo)物進(jìn)行抓取,因此要求末端執(zhí)行器在整個(gè)運(yùn)動過程中保持平穩(wěn)。對于水下作業(yè)的場景來說,在識別出目標(biāo)物之后,整個(gè)系統(tǒng)的工作場景已經(jīng)相對固定,且光源也比較固定。因此,本研究中將視覺系統(tǒng)和機(jī)械臂分離,把成像元件固定在機(jī)械臂本體之外,并且確保目標(biāo)物在視場范圍內(nèi),V-REP中的結(jié)構(gòu)化環(huán)境適宜采用上述配置。

圖3 V-REP平臺實(shí)驗(yàn)環(huán)境搭建Fig.3 Experimental environment construction of V-REP platform

圖4 機(jī)械臂抓取仿真試驗(yàn)過程Fig.4 The experimental process of manipulator grasping simulation

整個(gè)系統(tǒng)的控制結(jié)構(gòu)如圖2所示。視覺定位系統(tǒng)識別出目標(biāo)物后,對其進(jìn)行三維坐標(biāo)測量。通過機(jī)械臂末端軌跡規(guī)劃,對測量數(shù)據(jù)經(jīng)過分析后,求解出機(jī)械臂末端執(zhí)行器運(yùn)動路徑,即機(jī)械臂末端軌跡的一系列位置坐標(biāo)。這些位置坐標(biāo)作為機(jī)械臂最終期望的運(yùn)動位置。通過上述流程得到的位置坐標(biāo)作為機(jī)械臂運(yùn)動的輸入數(shù)據(jù),與實(shí)時(shí)讀取的末端執(zhí)行器位置數(shù)據(jù)比較后,將兩者的差值輸入到偏差控制算法當(dāng)中。然后通過機(jī)械臂的逆雅克比矩陣,將操作空間的數(shù)據(jù)變換到關(guān)節(jié)空間當(dāng)中,從而驅(qū)動機(jī)械臂各個(gè)關(guān)節(jié)完成相應(yīng)動作。

圖2 系統(tǒng)的控制結(jié)構(gòu)Fig.2 Control Structure of the whole system

3 仿真設(shè)置與實(shí)驗(yàn)

為了對機(jī)械臂的抓取過程進(jìn)行仿真驗(yàn)證,利用機(jī)器人仿真軟件V-REP搭建機(jī)械臂抓取試驗(yàn)環(huán)境,主要內(nèi)容包括六自由度機(jī)械臂UR5、末端抓取機(jī)械手、目標(biāo)物和雙目視覺傳感器組件,抓取仿真試驗(yàn)的設(shè)備配置如表1所示。

表1 機(jī)械臂抓取仿真試驗(yàn)配置Tab.1 Experiment Configuration for manipulator grasping simulation

其中,UR5六自由度機(jī)械臂作為運(yùn)動機(jī)構(gòu),負(fù)責(zé)抓取作業(yè)運(yùn)動過程的執(zhí)行。在機(jī)械臂的末端搭載有抓取機(jī)械手,整體結(jié)構(gòu)仿二指手爪型,通過機(jī)構(gòu)的打開和閉合完成對目標(biāo)物體的抓取操作。本次試驗(yàn)的目標(biāo)物選取長方體形狀的實(shí)體,表面顏色為橘黃。雙目視覺傳感器組件選取V-REP中的視覺傳感器部件,將兩只傳感器通過一定的間距設(shè)置組成雙目視覺系統(tǒng),并建立浮動窗口將雙目視覺系統(tǒng)的圖像進(jìn)行實(shí)時(shí)顯示?；赩-REP搭建的實(shí)驗(yàn)環(huán)境如圖3所示。

試驗(yàn)中,將機(jī)械臂抓取的運(yùn)動控制算法通過Lua編程寫入到UR5機(jī)械臂的線程腳本中。通過腳本函數(shù),將機(jī)械臂末端的軌跡進(jìn)行輸入,主要包括起始點(diǎn)、過渡點(diǎn)和抓取終點(diǎn)間的軌跡規(guī)劃點(diǎn)。整個(gè)抓取試驗(yàn)的過程為,首先根據(jù)目標(biāo)物外形尺寸,計(jì)算出抓取點(diǎn)和過渡點(diǎn)的位姿數(shù)據(jù),然后結(jié)合起始點(diǎn)的位姿進(jìn)行兩段軌跡規(guī)劃,第一段運(yùn)動為機(jī)械臂從起始點(diǎn)運(yùn)動到過渡點(diǎn),第二段為從過渡點(diǎn)運(yùn)動到抓取終點(diǎn)。在到達(dá)過渡點(diǎn)時(shí),設(shè)定機(jī)械臂停留1秒鐘,給末端機(jī)械手留有充足的時(shí)間打開手爪,隨后運(yùn)動到抓取的終點(diǎn)位置,停留1秒鐘,閉合手爪,完成對目標(biāo)物的抓取。整個(gè)試驗(yàn)的過程及結(jié)果如圖4所示。

在整個(gè)抓取仿真試驗(yàn)過程中,機(jī)械臂末端可以準(zhǔn)確運(yùn)動到指定位置,運(yùn)動過程中的軌跡平滑,可以正常完成對目標(biāo)物的抓取,并進(jìn)行提升。

4 結(jié)語

通過在V-REP平臺中的機(jī)械臂抓取仿真試驗(yàn),對機(jī)械臂末端軌跡規(guī)劃和抓取方法進(jìn)行了可行性驗(yàn)證,試驗(yàn)結(jié)果表明設(shè)計(jì)的軌跡規(guī)劃和抓取算法可行性較高,實(shí)用性較強(qiáng),可以有效地推廣到機(jī)械臂的運(yùn)動控制應(yīng)用當(dāng)中。