付東昊, 趙鐵軍

（沈陽工業(yè)大學(xué) 機(jī)械工程學(xué)院，沈陽 110000）

0 引言

機(jī)械臂的路徑規(guī)劃問題一直是機(jī)械臂領(lǐng)域的研究熱點(diǎn), 機(jī)械臂路徑規(guī)劃問題的本質(zhì)，即在規(guī)定的范圍內(nèi)，從路徑起始點(diǎn)到路徑終止點(diǎn)，為機(jī)械臂在運(yùn)動(dòng)的過程中尋找到一條最優(yōu)路徑[1]。現(xiàn)階段機(jī)械臂主要的路徑規(guī)劃方法有[2]：人工勢(shì)場(chǎng)法、搜索算法及隨機(jī)采樣法。人工勢(shì)場(chǎng)法是在機(jī)械臂規(guī)劃過程中，在目標(biāo)點(diǎn)與障礙物分別設(shè)置引力函數(shù)與斥力函數(shù)，從而得到一條可行路徑[3]。搜索算法解決問題的方法是通過在環(huán)境里列舉出問題的所有可能結(jié)果，并通過高性能的計(jì)算機(jī)來找出最優(yōu)解的一種方法，現(xiàn)階段一般有枚舉算法、A*算法等算法。隨機(jī)采樣法有隨機(jī)路圖法（Probabilistic roadmap method, PRM）[4]和快速搜索樹（Rapidly-exploring random trees, RRT）[5]。在使用隨機(jī)采樣法進(jìn)行路徑規(guī)劃時(shí)，向外隨機(jī)采樣來規(guī)劃出一條合適的路徑?？焖偎阉鳂渌惴ㄟm用于解決處理高維空間中路徑規(guī)劃問題，而隨機(jī)路圖法并不適用，因此，快速隨機(jī)樹算法通常應(yīng)用在機(jī)械臂的路徑規(guī)劃問題上。

近年來，國內(nèi)外專家學(xué)者一直將基于RRT算法的機(jī)械臂路徑規(guī)劃作為研究的重點(diǎn)。RRT算法以固定步長(zhǎng)向外搜索，具有搜索能力強(qiáng)、適應(yīng)性高等特點(diǎn)，但是會(huì)缺乏方向性，產(chǎn)生過多的無用節(jié)點(diǎn)等問題。在此基礎(chǔ)上，許多學(xué)者對(duì)此進(jìn)行了改進(jìn)。在2000年Kuffner等[6]提出了從起始點(diǎn)和終止點(diǎn)同時(shí)擴(kuò)展兩棵隨機(jī)樹從而實(shí)現(xiàn)對(duì)工作空間的快速采樣，即RRTConnect算法。在2011年Karaman等[7]提出了重選擇父節(jié)點(diǎn)的方法, 并提出RRT*算法。Gammel等[8]將RRT*算法中引入了橢圓約束采樣空間，提出了Informed-RRT*大大縮短了搜索時(shí)間，但不適用與動(dòng)態(tài)環(huán)境下。Jordan等[9]將雙向隨機(jī)擴(kuò)展樹引入RRT*算法提出了B-RRT*算法。

本文的研究以Sawyer冗余機(jī)械臂為例，針對(duì)RRT算法在路徑規(guī)化過程中缺乏方向性，成功率較低等問題，提出了改進(jìn)的RRT算法，利用改進(jìn)的算法減少了路徑生成時(shí)間和無用節(jié)點(diǎn)，提高了路徑規(guī)劃的成功率。

1 冗余機(jī)械臂分析

在機(jī)械臂的工作空間內(nèi)，非冗余機(jī)械臂存在關(guān)節(jié)限位會(huì)導(dǎo)致機(jī)械臂的末端執(zhí)行器在工作空間中的某些點(diǎn)位不可達(dá)。為了解決此類問題，冗余機(jī)械臂增加了一個(gè)自由度[10]。本文所使用的為Rethink公司生產(chǎn)的七自由度機(jī)械臂Sawyer，為了在避奇異與空間避障的方面具有優(yōu)勢(shì)，增加了關(guān)節(jié)個(gè)數(shù)和旋轉(zhuǎn)角度。Sawyer 機(jī)械臂如圖1所示，D-H參數(shù)如表1所示。

2 基本RRT算法

RRT 算法是在工作空間內(nèi)，以固定步長(zhǎng)向全局進(jìn)行隨機(jī)采樣的全局路徑規(guī)劃方法。其基本原理是在工作空間內(nèi)，已知起始點(diǎn)Xinit和終止點(diǎn)Xgoal，以初始點(diǎn)Xinit建立樹結(jié)構(gòu)并開始向外進(jìn)行擴(kuò)展搜索，并向外隨機(jī)采樣生成產(chǎn)生隨機(jī)點(diǎn)Xrand，遍歷隨機(jī)樹找到距Xrand最近的新節(jié)點(diǎn)Xnear，由Xnear擴(kuò)展步長(zhǎng)step，得到新節(jié)點(diǎn)Xnew。并通過使用碰撞檢測(cè)函數(shù)來判斷Xnew是否與障礙物發(fā)生碰撞。若Xnew通過碰撞檢測(cè)則將其納入樹中，否則放棄Xnew。在尋找到目標(biāo)點(diǎn)之前反復(fù)進(jìn)行以上步驟，并在目標(biāo)點(diǎn)處設(shè)置閾值p，若新節(jié)點(diǎn)與終止點(diǎn)的距離小于這個(gè)閾值，則判斷到達(dá)終止點(diǎn)。并在起始點(diǎn)與終止點(diǎn)之間生成一條連續(xù)無規(guī)則的路徑，傳統(tǒng)的RRT算法原理如圖2所示。

圖2 RRT算法原理

3 改進(jìn)RRT算法

3.1 引入目標(biāo)偏置策略

根據(jù)RRT算法可知，若不限制RRT算法的迭代次數(shù)，隨機(jī)樹產(chǎn)生的無用支點(diǎn)會(huì)導(dǎo)致隨機(jī)擴(kuò)展樹產(chǎn)生在整個(gè)空間內(nèi)。只要時(shí)間足夠，RRT算法總能找到一條有效路徑。為解決RRT算法在路徑規(guī)劃過程中，進(jìn)行無方向性的隨機(jī)采樣，提高選擇Xrand的有效性，加快路徑的搜索速度。將目標(biāo)偏置策略引入隨機(jī)采樣的函數(shù)中[11]。使在每次采樣的過程中，Xrand以一定概率選擇Xgoal做為采樣點(diǎn)，即Xrand=Xgoal。采樣公式如（1）所示：

具體方法為設(shè)置參數(shù)β為目標(biāo)偏置概率，并在采樣過程中隨機(jī)產(chǎn)生概率βrand（0＜βrand＜1）。目標(biāo)偏置策略原理如圖3所示。若βrand＜β，則判斷Xrand=Xgoal，由點(diǎn)劃線連接。在其他情況下則判斷Xrand為在空間中隨機(jī)選擇的一點(diǎn)。

圖3 目標(biāo)偏置策略

3.2 雙向隨機(jī)擴(kuò)展

在RRT算法中，從起始點(diǎn)生成一棵樹向終止點(diǎn)進(jìn)行隨機(jī)擴(kuò)展搜索。若工作空間較小，障礙物較少則路徑規(guī)劃較容易，產(chǎn)生的無用支點(diǎn)較少。若障礙物較多，工作空間較大，RRT算法會(huì)在空環(huán)境中產(chǎn)生過多的無用節(jié)點(diǎn)，增加路徑規(guī)劃的時(shí)間，降低了算法的搜索效率。

針對(duì)上述問題，本文提出在起始點(diǎn)與終止點(diǎn)中隨機(jī)生成一點(diǎn)Xmid，并從起始點(diǎn)與終止點(diǎn)分別同時(shí)向Xmid生成隨機(jī)搜索樹，進(jìn)行隨機(jī)采樣，此時(shí)Xmid將作為此次路徑的Xgoal，而原本的Xgoal將作為Xinit2。在起始點(diǎn)與終止點(diǎn)結(jié)束路徑搜索后將兩條路徑整合，得到一條最終路徑。改進(jìn)算法的原理如圖4所示。

圖4 改進(jìn)RRT算法原理

4 仿真驗(yàn)證

為了驗(yàn)證改進(jìn)的RRT算法可以在工作空間中具有更好的搜索效率，本文分別在Matlab與ROS中進(jìn)行三維路徑規(guī)劃與冗余機(jī)械臂路徑規(guī)劃的仿真實(shí)驗(yàn)。

4.1 三維路徑規(guī)劃仿真

使用Matlab建立路徑規(guī)劃的三維仿真環(huán)境。設(shè)置仿真環(huán)境的采樣空間為500 mm×500 mm×500 mm，起始目標(biāo)點(diǎn)坐標(biāo)為（0，0，0），終止目標(biāo)點(diǎn)為（500，500，500）。隨機(jī)設(shè)置黑色障礙物的形狀與位置。RRT算法搜索路徑的擴(kuò)展步長(zhǎng)step為30 mm，閾值p為20。改進(jìn)的RRT算法中目標(biāo)偏置概率為0.5，設(shè)置算法迭代次數(shù)為10 000，若在迭代次數(shù)內(nèi)無法到達(dá)目標(biāo)點(diǎn)，則判定路徑搜索失敗。RRT算法與改進(jìn)的RRT算法路徑規(guī)劃結(jié)果如圖5和圖6所示。

圖5 RRT算法生成路徑

圖6 改進(jìn)RRT算法生成路徑

由圖2與圖3可知，RRT算法在路徑規(guī)劃過程中在無障礙物區(qū)域產(chǎn)生過多的無用節(jié)點(diǎn)，而改進(jìn)的RRT算法明顯減少了無用節(jié)點(diǎn)的產(chǎn)生。通過30次的仿真實(shí)驗(yàn)，RRT算法與改進(jìn)算法的各項(xiàng)數(shù)據(jù)如表2所示。

表2 實(shí)驗(yàn)結(jié)果平均值

由表2中數(shù)據(jù)可知，改進(jìn)的RRT算法對(duì)比RRT算法在三維環(huán)境中，各項(xiàng)數(shù)據(jù)均少于RRT算法。生成節(jié)點(diǎn)數(shù)減少了約89.3%，平均路徑規(guī)劃時(shí)間減少了約91.1%，成功率提高了約31.6%。改進(jìn)的RRT算法有效提高了路徑的搜索效率。

4.2 冗余機(jī)械臂路徑規(guī)劃仿真

本文通過使用機(jī)器人操作系統(tǒng)ROS進(jìn)行機(jī)械臂可視化的仿真實(shí)驗(yàn)，并通過可視化平臺(tái)Rviz中設(shè)置仿真環(huán)境，通過Moveit！對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)規(guī)劃。通過搭建障礙物，利用冗余機(jī)械臂來對(duì)比在使用RRT算法與改進(jìn)RRT算法時(shí)機(jī)械臂的運(yùn)動(dòng)情況。為模擬真實(shí)機(jī)械臂的運(yùn)動(dòng)情況，在機(jī)械臂的末端附加一個(gè)立方體，以表示機(jī)械臂完成抓取后，進(jìn)行避障。

實(shí)驗(yàn)一：簡(jiǎn)單環(huán)境冗余機(jī)械臂避障。每種算法在相同環(huán)境下各進(jìn)行20次實(shí)驗(yàn)。在簡(jiǎn)單環(huán)境中機(jī)械臂使用RRT算法與改進(jìn)的RRT算法路徑規(guī)劃分別如圖7、圖8所示。平均時(shí)間如表3所示。

表3 簡(jiǎn)單環(huán)境平均時(shí)間

圖7 簡(jiǎn)單環(huán)境下RRT算法機(jī)械臂路徑

圖8 簡(jiǎn)單環(huán)境下改進(jìn)RRT算法機(jī)械臂路徑

通過20次簡(jiǎn)單環(huán)境的實(shí)驗(yàn)，隨機(jī)選擇冗余機(jī)械臂避障路徑的仿真結(jié)果，可以看出改進(jìn)算法所規(guī)劃的路徑要優(yōu)于RRT算法規(guī)劃的路徑，平均的路徑規(guī)劃時(shí)間也減少了大約84%。驗(yàn)證了改進(jìn)算法應(yīng)用在機(jī)械臂簡(jiǎn)單環(huán)境避障的有效性。

實(shí)驗(yàn)二：狹窄環(huán)境避障。為了驗(yàn)證冗余機(jī)械臂在狹窄環(huán)境的避障能力，將增加障礙物。與簡(jiǎn)單環(huán)境相同，各進(jìn)行20次實(shí)驗(yàn)。在狹窄環(huán)境中機(jī)械臂使用RRT算法與改進(jìn)的RRT算法路徑規(guī)劃分別如圖9、圖10所示。平均規(guī)劃時(shí)間如表4所示。

表4 狹窄環(huán)境平均時(shí)間

圖9 狹窄環(huán)境下RRT算法機(jī)械臂路徑

圖10 狹窄環(huán)境下改進(jìn)RRT算法機(jī)械臂路徑

由仿真結(jié)果可知，復(fù)雜環(huán)境由于障礙物較多，而且在設(shè)置障礙物之間的距離較為狹窄的情況下，路徑長(zhǎng)度與時(shí)間要多于簡(jiǎn)單環(huán)境。冗余機(jī)械臂在狹窄環(huán)境中使用改進(jìn)RRT算法的所生成的路徑要明顯優(yōu)于RRT算法生成的路徑。而在平均規(guī)劃時(shí)間上，改進(jìn)的RRT算法減少了約85%。

綜合兩次實(shí)驗(yàn)，證明了改進(jìn)的RRT算法在路徑規(guī)劃上提高了路徑搜索效率，減少了算法的計(jì)算量。

5 結(jié)論

本文以冗余機(jī)械臂Sawyer為研究對(duì)象，在簡(jiǎn)單環(huán)境與狹窄環(huán)境下對(duì)冗余機(jī)械臂進(jìn)行多次仿真實(shí)驗(yàn)。通過實(shí)驗(yàn)結(jié)果可以看出，改進(jìn)的RRT算法在兩種環(huán)境下的規(guī)劃時(shí)間均減少了約85%，在路徑的選擇上也有明顯的改善。并且機(jī)械臂在狹窄環(huán)境下也可以有效地避免障礙物發(fā)生碰撞。改進(jìn)的RRT算法在機(jī)械臂的路徑規(guī)劃問題上具有一定的參考價(jià)值。