張 寧，張彩霞，高 萌，林 壯,，郭 靜,3，余 偉,3

(1.佛山科學(xué)技術(shù)學(xué)院機(jī)電工程與自動化學(xué)院，佛山 528000；2.佛山智能裝備技術(shù)研究院，佛山 528000；3.廣東省智慧城市基礎(chǔ)設(shè)施健康監(jiān)測與評估工程技術(shù)研究中心，佛山 528000)

0 引言

近年來，隨著機(jī)器人行業(yè)的不斷發(fā)展，雙機(jī)械臂因其能搬運大負(fù)載、靈巧、敏捷、適應(yīng)更加復(fù)雜的工作環(huán)境的優(yōu)勢逐漸受到人們的關(guān)注。而路徑規(guī)劃即是雙機(jī)械臂協(xié)同的基礎(chǔ)，又是雙機(jī)械臂運動控制的前提。所以路徑規(guī)劃問題被大量專家學(xué)者探索改進(jìn)，并產(chǎn)生了許多優(yōu)秀的算法。

基于采樣的路徑規(guī)劃算法[1]在機(jī)械臂運動規(guī)劃中應(yīng)用最為廣泛。此類型算法在工作空間任意采樣，經(jīng)過某種要求，將采樣點通過局部規(guī)劃器添加到已建立的搜索樹中，并判斷是否發(fā)生碰撞。若不產(chǎn)生碰撞則將采樣點加入搜索范圍中，然后通過搜索算法在工作空間中選擇無碰撞路徑。此類型的算法主要包括RRT[2]算法和PRM[3]算法。通過上述兩種算法得到的路徑是不確定的且不是最優(yōu)的。為了改善采樣法路徑規(guī)劃存在的路徑非最優(yōu)的缺點，RRT在原來的基礎(chǔ)上衍生出了RRT*[4]、RRT-Connect[5]、RRG[6]和S-RRT[7]等改進(jìn)算法。而PRM算法也衍生出一些性能良好的算法，如LD-PRM[8]、CPRM[9]和PRM*[10]等改進(jìn)算法。

本文在上述文獻(xiàn)的基礎(chǔ)上，結(jié)合RRT算法和PRM算法的優(yōu)點，提出一種混合的改進(jìn)算法，并將改進(jìn)后的算法應(yīng)用到雙機(jī)械臂運動規(guī)劃中。最終經(jīng)仿真驗證和實際雙機(jī)械臂搬運測試，本文所提方案效果更高。

1 結(jié)構(gòu)與模型

在協(xié)同作業(yè)過程中，兩臺機(jī)械臂為各自獨立的系統(tǒng)，且研究的雙機(jī)械臂協(xié)同需要兩臺機(jī)械臂在同一空間坐標(biāo)系下完成協(xié)調(diào)作業(yè)。雙機(jī)械臂協(xié)同搬運模型如圖1所示。其中將左機(jī)械臂看作主機(jī)械臂，右機(jī)械臂看作從機(jī)械臂。

圖1 雙機(jī)械臂協(xié)同搬運模型

1.1 約束關(guān)系建立

根據(jù)王美玲[11]所提出的3種操作模式，本文雙臂協(xié)同搬運主要運用獨立模式和全約束模式，而在獨立模式中，首要任務(wù)是避免雙機(jī)械臂之間，機(jī)械臂與障礙物之間出現(xiàn)碰撞，而全約束模式下兩臺機(jī)械臂需滿足位姿，速度和加速度之間的約束關(guān)系。

下面給出雙臂間的位姿約束關(guān)系：

(1)

(2)

所以：

(3)

故由式(3)可推導(dǎo)出雙機(jī)械臂之間的位姿約束關(guān)系：

(4)

下面給出雙機(jī)械臂之間的速度約束關(guān)系：

被搬運工件的運動速度與第m(m=L或R)個機(jī)械臂末端的運行速度滿足如下關(guān)系：

(5)

當(dāng)被搬運工件與雙機(jī)械臂形成閉鏈時，它們之間無相對運動，因此滿足下式：

(6)

結(jié)合式(6)、式(7)可以得到：

(7)

根據(jù)式(7)可以進(jìn)一步推導(dǎo)出雙臂間的速度與加速度約束關(guān)系：

(8)

在雙臂自身的參考坐標(biāo)系下，滿足如下的微分運動關(guān)系：

(9)

結(jié)合式(8)、式(9)可以推導(dǎo)得到雙臂的關(guān)節(jié)速度約束關(guān)系：

(10)

1.2 雙機(jī)械臂基座標(biāo)標(biāo)定

為了得到精確的雙機(jī)械臂基座標(biāo)齊次變換關(guān)系，本文提出一種簡單精確的標(biāo)定方法。一般情況下，將左機(jī)械臂的坐標(biāo)系和世界坐標(biāo)系重合，故本文只需要進(jìn)行兩機(jī)械臂基座之間的標(biāo)定。使用機(jī)械臂自帶的示教器在雙機(jī)械臂可達(dá)空間中標(biāo)定一個坐標(biāo)系相對世界坐標(biāo)系方向相同且固定不變的工件。示教器進(jìn)行工件標(biāo)定后分別獲得工件相對于雙機(jī)械臂基座系的笛卡爾空間坐標(biāo)，通過坐標(biāo)轉(zhuǎn)換獲得雙機(jī)械臂基座標(biāo)位姿關(guān)系。具體步驟如下：

步驟1：固定工件位置，在標(biāo)定期間保持位置不變。對機(jī)械臂分別安裝標(biāo)定針，具體如圖2所示；

圖2 標(biāo)定針

步驟2：使用主機(jī)械臂的示教器進(jìn)行工件標(biāo)定，通過三點法獲得標(biāo)定結(jié)果并記錄如下：

M1=(x1,y1,z1,R1,P1,Y1)

(11)

步驟3：同理按照上述步驟操作，從機(jī)械臂獲得標(biāo)定結(jié)果并記錄如下：

M2=(x2,y2,z2,R2,P2,Y2)

(12)

步驟4：將(R1,P1,Y1)和(R2,P2,Y2)根據(jù)外旋方式，推導(dǎo)出旋轉(zhuǎn)矩陣：

R=Rz(γ)*Ry(β)*Rx(α)

(13)

最終得到主從機(jī)械臂基座標(biāo)相對于虛擬工件基座標(biāo)的齊次變換矩陣：

(14)

(15)

由式(14)、式(15)最終得出雙機(jī)械臂基坐標(biāo)之間的齊次變換矩陣：

(16)

通常情況下在實際系統(tǒng)中很難避免，這也將導(dǎo)致機(jī)械臂軌跡存在偏差。

2 路徑規(guī)劃算法改進(jìn)

2.1 傳統(tǒng)RRT算法

RRT算法描述如下：隨機(jī)樹T的根節(jié)點為xinit，集合V存儲樹的節(jié)點xi，邊集E存儲節(jié)點與節(jié)點的連接，且每個節(jié)點存在于Xfree中，在運行該算法時，通過選取節(jié)點來確定樹的擴(kuò)散方向。當(dāng)節(jié)點到達(dá)目標(biāo)節(jié)點后，停止運行，返回起始點到目標(biāo)點的樹干路徑連線。其偽代碼如表1所示。

表1 RRT算法偽代碼

2.2 改進(jìn)RRT算法

經(jīng)過對RRT算法的分析，本文對該算法進(jìn)行改進(jìn)，其主要思想：在RRT算法基礎(chǔ)上加入偏置采樣策略后，產(chǎn)生搜索樹并將葉節(jié)點存入到程序數(shù)組中，獲取規(guī)劃的路徑后融合PRM算法，將得到的路徑節(jié)點代替PRM算法的采樣點，以此來減小傳統(tǒng)PRM算法隨機(jī)采樣的分散性，最后利用A*[12]算法得到最優(yōu)路徑。

2.2.1 目標(biāo)偏向采樣策略

RRT算法采用均勻采樣的方式滿足概率的完備性，但同時也增加了數(shù)據(jù)的冗余性，其結(jié)果將導(dǎo)致算法規(guī)劃時間較長，不能快速到達(dá)目標(biāo)節(jié)點qgoal。針對這一問題，本文在傳統(tǒng)RRT算法的基礎(chǔ)上加入了偏置采樣，步驟為：先設(shè)定目標(biāo)偏向閾值pgoal，在采樣的過程中，由均勻分布得到一個概率值p。若p

2.2.2 融合PRM算法

PRM算法由離線學(xué)習(xí)和在線查詢兩部分組成。離線學(xué)習(xí)部分主要是在工作空間中采取一定量的坐標(biāo)點。在線查詢部分是在已知起始點和終點的前提下，由離線部分構(gòu)建的地圖，發(fā)現(xiàn)一條連接起始點到終點的無碰撞軌跡。

所以對目前所獲得的路徑進(jìn)行處理，獲取每個路徑點坐標(biāo)，此時將獲取的坐標(biāo)充當(dāng)PRM算法中的隨機(jī)采樣點，然后基于所獲得的軌跡點建立可行區(qū)域。算法偽代碼如表2所示。

通過上述方式減少了采樣點在工作空間中的分散性，提高了路徑搜索的明確性，完成PRM算法中的離線學(xué)習(xí)階段，然后再利用A*算法找到最佳的路徑，使得改進(jìn)后的算法能夠減小規(guī)劃時間并獲得最優(yōu)路徑。

3 方案驗證

為了驗證所提方案效果更好，本文進(jìn)行了實驗仿真和物理測試。實驗仿真主要有兩部分組成，第一部分先是在MATLAB環(huán)境下，驗證改進(jìn)算法的性能，主要對傳統(tǒng)RRT算法，RRT-Connect算法、PRM算法和改進(jìn)后的算法在時間和路徑上的表現(xiàn)進(jìn)行對比，然后再ROS上模擬雙機(jī)械臂獨立操作模式下的運動規(guī)劃。第二部分驗證雙機(jī)械臂基座標(biāo)關(guān)系的精確度。經(jīng)過實驗仿真驗證之后，對本文所提的方案在實際機(jī)器人上進(jìn)行物理驗證。

3.1 數(shù)值仿真

為了證明改進(jìn)算法的的效率更高，首先在MATLAB上測試算法的可行性，然后在ROS上驗證機(jī)械臂路徑規(guī)劃的效果。

3.1.1 MATLAB仿真驗證

設(shè)置[10,10]為起始點坐標(biāo)，[490,490]為目標(biāo)點坐標(biāo)，0.6為目標(biāo)偏置概率，1000為迭代次數(shù)，20為擴(kuò)散步長。經(jīng)實驗操作我們獲得改進(jìn)后的算法圖像，具體如圖3所示。

圖3 二維平面改進(jìn)前后的RRT算法對比圖

表3 RRT算法改進(jìn)前后對比

起點和目標(biāo)點分別在圖中的左上角和右下角，標(biāo)有“-”的細(xì)曲線表示隨機(jī)樹，圖3f中的紅色線條為經(jīng)改進(jìn)后的最終規(guī)劃路徑。

為了客觀驗證改進(jìn)算法的效果，重復(fù)進(jìn)行50次路徑規(guī)劃。引入PRM算法后，相比傳統(tǒng)RRT算法時間縮短了60.6%，路徑縮短了20.3%相比于上一步雖然犧牲了一些時間，但仍遠(yuǎn)遠(yuǎn)優(yōu)于傳統(tǒng)RRT算法。

3.1.2 在ROS平臺仿真

ROS操作系統(tǒng)作為機(jī)械臂的控制軟件，它提供了操作系統(tǒng)應(yīng)有的服務(wù)，包括硬件抽象，底層設(shè)備控制，常用函數(shù)的實現(xiàn)，進(jìn)程間消息傳遞，以及功能包管理等服務(wù)。

具體的驗證雙機(jī)械臂路徑規(guī)劃算法的操作如下：首先從配置Movelt生成的config文件里啟動Rviz。其次將改進(jìn)的路徑算法當(dāng)作一個可執(zhí)行函數(shù)模塊，當(dāng)執(zhí)行該函數(shù)后獲得一條無碰撞的軌跡點并存儲在軌跡下發(fā)接口中。最后將Rviz和軌跡下發(fā)接口通信連接，軌跡下發(fā)接口以設(shè)定的頻率將軌跡點發(fā)送給Rviz節(jié)點，Rviz中的機(jī)械臂在獲得軌跡下發(fā)接口發(fā)送的軌跡點后立即做出響應(yīng)。

設(shè)置主機(jī)械臂的目標(biāo)點位置為(0.37,-0.41,1.36)、姿態(tài)四元數(shù)為(0.99,0,0,-0.11)，從機(jī)械臂的目標(biāo)點位置為(0.77,-0.405,1.36)、姿態(tài)四元數(shù)為(0,0.99,0.11,0)。最終在ROS上雙機(jī)械臂的運行路徑如圖4所示。

(a) RRT算法(b) 改進(jìn)算法

由圖4可知，機(jī)械臂之間的木板為障礙物，藍(lán)色線段為機(jī)械臂末端運行軌跡，經(jīng)實驗對比，所改進(jìn)的算法效果更好。

3.2 軌跡誤差驗證

通過雙機(jī)械臂示教器在雙機(jī)械臂可達(dá)空間按照上述第一部分的標(biāo)定步驟進(jìn)行工件標(biāo)定，標(biāo)定后雙機(jī)械臂的數(shù)值分別為(0.65,0,0.37,0.18,0,0)和(0.65,0,0.43,0,0,0)，帶入式(16)，最終得到雙機(jī)械臂基座標(biāo)的齊次矩陣。

(17)

由上述位姿關(guān)系可得，雙機(jī)械臂基座標(biāo)之間在x方向距離為1.28 m，y、z保持在同一平面上。

根據(jù)所求得的基座標(biāo)齊次變換關(guān)系，更改ROS環(huán)境下雙機(jī)械臂URDF模型下base_link的參數(shù)，調(diào)整雙機(jī)械臂的仿真位姿和實際機(jī)器人位姿完全一致。

為了驗證上述所求雙機(jī)械臂基座標(biāo)的正確性，在Rviz界面讀取主機(jī)械臂末端安裝的夾爪的位置為(0.4,0,1.35)，姿態(tài)為(0.08,0.7,-0.08,0.70)，根據(jù)式(3)求得從機(jī)械臂末端夾爪的位姿。并將求得的位姿和從機(jī)械臂示教器所顯示的位姿數(shù)據(jù)進(jìn)行對比，為了實驗的客觀性，不斷變換雙機(jī)械臂的位姿，經(jīng)過20次實驗，最終得出推導(dǎo)的從機(jī)械臂位姿和示教器顯示的位姿在各方向的誤差小于1 mm，故可證明所提雙機(jī)械臂標(biāo)定算法的正確性。

3.3 物理環(huán)境驗證

本研究基于華數(shù)機(jī)器人HSR_CO605本體搭建系統(tǒng)平臺，在主機(jī)械臂末端安裝有六維力矩傳感器Onrobot-hex，并對兩臺機(jī)械臂安裝氣動夾爪。ROS下發(fā)機(jī)械臂軌跡點周期設(shè)置為40 ms，具體實物系統(tǒng)如圖5所示。

圖5 物理環(huán)境

由圖5所示，以3個剛體框架為障礙物，在Rviz環(huán)境下對障礙物三維建模。在獨立操作模式下，對機(jī)械臂進(jìn)行運動規(guī)劃，設(shè)置主機(jī)械臂的抓取點位置為(0.55,-0.405,1.36)、姿態(tài)四元數(shù)為(0.99,0,0,-0.11)，從機(jī)械臂的抓取點位置為(0.65,-0.405,1.36)、姿態(tài)四元數(shù)為(0,0.99,0.11,0)。當(dāng)通過改進(jìn)算法對機(jī)械臂進(jìn)行運動規(guī)劃之后對雙機(jī)械臂進(jìn)行五次多項式軌跡優(yōu)化，實時訂閱/joint_state話題獲得機(jī)械臂軌跡值。最終獲得優(yōu)化后軌跡如圖6和圖7所示。

圖6 主機(jī)械臂位置、速度、加速度數(shù)據(jù)

在全約束模式下，對主機(jī)械臂進(jìn)行笛卡爾空間運動規(guī)劃，設(shè)置起始點為主機(jī)械臂的抓取點，實時采集關(guān)節(jié)軸位置、速度、加速度數(shù)據(jù)。然后設(shè)機(jī)械臂末端間距相差0.1 m，對所獲得的主機(jī)械臂軌跡點代入式(3)推導(dǎo)出對應(yīng)從機(jī)械臂的軌跡點。然后通過主從機(jī)械臂之間的笛卡爾空間中的位置誤差來驗證約束關(guān)系的正確性，設(shè)計誤差評判公式為：

ess=XFollow-XMain-0.1

(18)

主從機(jī)械臂x軸、y軸、z軸方向誤差大小如圖8所示。

圖8 軌跡誤差

經(jīng)過上述軌跡點的誤差對比，x、y、z三個方向的誤差均在±0.003 m內(nèi)，這相對于機(jī)器人本體結(jié)構(gòu)來說，誤差可忽略不計，故可說明約束關(guān)系推理完全正確。

總之，由于各種校準(zhǔn)誤差和外部干擾，雙臂協(xié)作系統(tǒng)中總是存在未知的軌跡偏差。這些軌跡偏差是非線性的，具有不確定性，易導(dǎo)致系統(tǒng)損壞和執(zhí)行器過載。

4 結(jié)論

為了使雙機(jī)械臂能夠在復(fù)雜的環(huán)境中完成協(xié)同運動任務(wù)，提出了一種改進(jìn)的路徑規(guī)劃算法。通過改進(jìn)的雙機(jī)械臂基座標(biāo)標(biāo)定算法，獲得精確的雙機(jī)械臂模型的前提下，對傳統(tǒng)的RRT算法偏置采樣，然后結(jié)合PRM算法對路徑二次優(yōu)化，最后經(jīng)過五次多項式優(yōu)化路徑以獲得最優(yōu)軌跡。經(jīng)過實驗仿真，改進(jìn)后的路徑規(guī)劃算法比傳統(tǒng)的RRT算法在路徑上縮短了20.3%，時間上減小了60.6%。最后通過ROS控制實際雙機(jī)械臂，進(jìn)一步驗證了改進(jìn)后的算法效率更高。