水下自主作業機械臂軌跡規劃與抓取仿真

張祥

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

0 引言

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

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

機械臂自主作業的任務是在探測到目標物時,通過控制末端執行器手爪運動到指定位置,確認到達抓取位置后控制末端執行器抓取目標物,然后攜帶目標物進行一定高度的提升。為了完成抓取,首先需要進行機械臂末端的軌跡規劃,生成從起始位置到抓取位置的目標運動點。然后,設計機械臂的運動控制結構,便于在V-REP仿真平臺中進行控制程序的開發。最終,借助機械臂、目標物和環境模型,開展水下作業機械臂自主抓取實驗,驗證方法的有效性。

1 機械臂末端軌跡規劃方法

機械臂在水下作業時,利用其末端搭載的機械手完成夾持等操作。為了保證機械臂末端執行器可以按照預先設定的連續路徑進行作業,需要在笛卡爾空間對機械臂末端進行軌跡規劃,分為全局路徑規劃與局部軌跡插補兩個部分。

1.1 全局路徑規劃

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

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

A*算法搜索時,需要將待搜索的區域簡化為二維圖形空間,如圖1所示,其中灰色區域代表障礙物,黃色區域為搜索路徑。設置兩個表open和closed,搜索步驟如下:

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

(1)計算初始節點和其周圍節點的代價估計F(n)=g(n)+h(n),其中g(n)為實際代價,其值為當前節點到起始點的距離,h(n)為估計代價,其值為當前節點到終點的距離,并將初始節點周圍節點放入到open列表中;

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

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

(4)將節點n添加到表closed中;

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

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

1.2 局部軌跡插補

全局路徑規劃確定了機械臂抓取運動過程中的起始點、過渡點和終點,然而在實際運動控制程序開發中,只有這三個點無法進行完整的位置跟蹤和速度控制,因為缺失中間過程的位置坐標。因此,需要對整個運動軌跡進行插補,按照時間序列生成一系列運動目標點,輸入到運動控制器中進行跟蹤。插補內容包括從起始點到過渡點、從過渡點到終點兩個階段的位置坐標插補。常用的軌跡插補有直線插補和圓弧插補,其他軌跡都可以通過直線或者圓弧進行逼近。為了實現機械臂的快速抓取并避免與目標物的運動干涉,本文選取直線插補進行局部軌跡規劃。

基于機械臂抓取任務的直線插補可以按照如下流程進行:

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

(2)計算機械臂抓取運動過程中的起始點到過渡點的距離:

(3)求起始點到過渡點插補的總時間T:

(4)計算插補的總次數N。當T/Ts的余數為零時,N=ent(T/Ts);當余數不為零時,N=ent(T/Ts)+1,其中ent函數表示取整。

(5)計算插補增量:

(6)計算第i點的插補坐標:

對于機械臂抓取運動過程中的過渡點到終點的插補,參照以上流程進行計算。

2 抓取控制結構

機械臂在水下作業時,主要的工作內容為利用末端機械手,對水下識別到的目標物進行抓取,因此要求末端執行器在整個運動過程中保持平穩。對于水下作業的場景來說,在識別出目標物之后,整個系統的工作場景已經相對固定,且光源也比較固定。因此,本研究中將視覺系統和機械臂分離,把成像元件固定在機械臂本體之外,并且確保目標物在視場范圍內,V-REP中的結構化環境適宜采用上述配置。

圖3 V-REP平臺實驗環境搭建Fig.3 Experimental environment construction of V-REP platform

圖4 機械臂抓取仿真試驗過程Fig.4 The experimental process of manipulator grasping simulation

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

圖2 系統的控制結構Fig.2 Control Structure of the whole system

3 仿真設置與實驗

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

表1 機械臂抓取仿真試驗配置Tab.1 Experiment Configuration for manipulator grasping simulation

其中,UR5六自由度機械臂作為運動機構,負責抓取作業運動過程的執行。在機械臂的末端搭載有抓取機械手,整體結構仿二指手爪型,通過機構的打開和閉合完成對目標物體的抓取操作。本次試驗的目標物選取長方體形狀的實體,表面顏色為橘黃。雙目視覺傳感器組件選取V-REP中的視覺傳感器部件,將兩只傳感器通過一定的間距設置組成雙目視覺系統,并建立浮動窗口將雙目視覺系統的圖像進行實時顯示。基于V-REP搭建的實驗環境如圖3所示。

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

在整個抓取仿真試驗過程中,機械臂末端可以準確運動到指定位置,運動過程中的軌跡平滑,可以正常完成對目標物的抓取,并進行提升。

4 結語

通過在V-REP平臺中的機械臂抓取仿真試驗,對機械臂末端軌跡規劃和抓取方法進行了可行性驗證,試驗結果表明設計的軌跡規劃和抓取算法可行性較高,實用性較強,可以有效地推廣到機械臂的運動控制應用當中。