改進(jìn)RRT算法的機(jī)械臂路徑規(guī)劃

江 韓，晁永生，周江林，李純艷

（新疆大學(xué)機(jī)械工程學(xué)院，新疆 烏魯木齊 830047）

1 引言

近年來(lái)，隨著科學(xué)技術(shù)的發(fā)展，機(jī)械臂應(yīng)用的范圍越來(lái)越廣泛，如何讓機(jī)械臂在工作環(huán)境中快速規(guī)劃出一條無(wú)碰撞路徑則是機(jī)械臂路徑規(guī)劃研究的重點(diǎn)。常用的路徑搜索算法有A*算法［1］，人工勢(shì)場(chǎng)法［2］，RRT算法［3］等，A*算法在不規(guī)則的障礙環(huán)境（如凹字形或工字形）中難以找到路徑，人工勢(shì)場(chǎng)法在路徑搜索中容易陷入局部極小值從而導(dǎo)致路徑規(guī)劃失敗，RRT算法能夠有效地對(duì)高維空間進(jìn)行搜索，并且無(wú)需對(duì)構(gòu)型空間障礙物進(jìn)行準(zhǔn)確的描述，因此RRT算法被廣泛應(yīng)用于機(jī)械臂的路徑規(guī)劃中。針對(duì)傳統(tǒng)RRT算法搜索路徑隨機(jī)性大、效率低的問(wèn)題，許多學(xué)者對(duì)傳統(tǒng)RRT算法進(jìn)行了改進(jìn)，文獻(xiàn)［4］提出一種低振蕩人工勢(shì)場(chǎng)-自適應(yīng)擴(kuò)展隨機(jī)樹(shù)混合算法。文獻(xiàn)［5］提出了一種改進(jìn)的快速隨機(jī)搜索樹(shù)算法，該算法通過(guò)標(biāo)準(zhǔn)RRT算法獲得可行路徑，然后根據(jù)路徑安全性和路徑長(zhǎng)度來(lái)計(jì)算路徑代價(jià)。由文獻(xiàn)［6］提出了一種改進(jìn)RRT算法，并且采用雙向同時(shí)剪枝取最優(yōu)的策略來(lái)刪除不必要的節(jié)點(diǎn)。文獻(xiàn)［7］在仿人機(jī)器人中利用雙向RRT算法進(jìn)行抓取操作。

針對(duì)傳統(tǒng)的RRT算法搜索效率低，隨機(jī)性大的特點(diǎn)提出一種改進(jìn)RRT算法。該算法通過(guò)雙向擴(kuò)展隨機(jī)樹(shù)、在隨機(jī)采樣中加入目標(biāo)偏置、設(shè)置虛擬目標(biāo)點(diǎn)和改變步長(zhǎng)來(lái)提高算法的搜索效率，利用雙向刪除取最短路徑的方法刪除路徑中的冗余節(jié)點(diǎn)，并使用三次B樣條曲線(xiàn)對(duì)刪除冗余節(jié)點(diǎn)后的無(wú)碰撞路徑進(jìn)行平滑處理。

2 算法原理

2.1 機(jī)械臂運(yùn)動(dòng)學(xué)模型建立

由于機(jī)械臂的每個(gè)關(guān)節(jié)都有自由旋轉(zhuǎn)度與獨(dú)立位置，因此需要借助數(shù)學(xué)模型來(lái)建立機(jī)械臂末端在世界坐標(biāo)系中坐標(biāo)與機(jī)械臂關(guān)節(jié)角度的聯(lián)系［8］。以丹麥優(yōu)傲公司的UR5機(jī)械臂作為研究對(duì)象，該機(jī)械臂具有六個(gè)自由度，機(jī)械臂的六個(gè)關(guān)節(jié)都為旋轉(zhuǎn)關(guān)節(jié)。機(jī)械臂的實(shí)物，如圖1（a）所示。運(yùn)用D-H參數(shù)法建立機(jī)械臂運(yùn)動(dòng)模型，如圖1（b）所示。

圖1 機(jī)械臂模型Fig.1 Manipulator Model

2.2 障礙物碰撞檢測(cè)

在實(shí)際的工作環(huán)境中障礙物是不規(guī)則的，機(jī)械臂與障礙物之間的關(guān)系判斷非常復(fù)雜需要耗費(fèi)大量的時(shí)間，也很難通過(guò)準(zhǔn)確的數(shù)學(xué)公式進(jìn)行表達(dá)。因此為了提高機(jī)械臂在工作空間中碰撞檢測(cè)效率，對(duì)障礙物采用長(zhǎng)方體包絡(luò)，對(duì)機(jī)械臂連桿采用圓柱體包絡(luò)，此方法雖然增加了障礙物和機(jī)械臂模型區(qū)域，但是提高了機(jī)械臂在工作環(huán)境中規(guī)劃路徑的效率，同時(shí)增加了機(jī)械臂在實(shí)際運(yùn)行過(guò)程中的安全性。機(jī)械臂某個(gè)連桿和障礙物包絡(luò)盒，如圖2所示。

圖2 機(jī)械臂某個(gè)連桿和障礙物包絡(luò)盒示意圖Fig.2 Schematic Diagram of a Simplified Link and Obstacle Envelope Box for Manipulator

把半徑為r的機(jī)械臂連桿包絡(luò)盒簡(jiǎn)化成線(xiàn)段AB，機(jī)械臂連桿包絡(luò)盒和障礙物包絡(luò)盒之間的碰撞關(guān)系可轉(zhuǎn)換為線(xiàn)段AB和障礙物包絡(luò)盒六個(gè)平面之間的碰撞關(guān)系。為便于計(jì)算把障礙物進(jìn)行膨化處理，膨脹尺寸為r，建立障礙物包絡(luò)盒坐標(biāo)關(guān)系。

設(shè)線(xiàn)段AB的方程為f(x，y，z)，若方程f(x，y，z)=0的解集與上式（1）有交集則機(jī)械臂與障礙物產(chǎn)生碰撞，反之機(jī)械臂和障礙物不發(fā)生碰撞，對(duì)機(jī)械臂另外五個(gè)連桿的判斷也采用同樣的方法進(jìn)行求解。

3 RRT算法研究

3.1 傳統(tǒng)RRT算法

RRT算法是一種基于隨機(jī)采樣的搜索算法，由英國(guó)教授Steven M.La Valle在1998年首次提出來(lái)。該算法首先在機(jī)器人狀態(tài)空間中從初始點(diǎn)開(kāi)始構(gòu)建隨機(jī)樹(shù)，通過(guò)隨機(jī)采樣的方法生成隨機(jī)節(jié)點(diǎn)，然后從離該節(jié)點(diǎn)最近的隨機(jī)樹(shù)節(jié)點(diǎn)按照步長(zhǎng)生成新節(jié)點(diǎn)，在生成新節(jié)點(diǎn)過(guò)程中，若發(fā)生碰撞，則舍去新節(jié)點(diǎn)，否則添加到隨機(jī)樹(shù)中。重復(fù)以上過(guò)程，直到生成的新節(jié)點(diǎn)為目標(biāo)點(diǎn)。傳統(tǒng)RRT算法的偽代碼如下所示。

3.2 改進(jìn)RRT算法

針對(duì)傳統(tǒng)RRT 算法搜索效率低的問(wèn)題，文獻(xiàn)［9］提出了雙向RRT算法，該算法從初始點(diǎn)和目標(biāo)點(diǎn)同時(shí)生成兩棵隨機(jī)樹(shù)，這樣雖然能夠提高隨機(jī)樹(shù)搜索的效率，但還是存在搜索隨機(jī)性大的問(wèn)題。文獻(xiàn)［10］在隨機(jī)樹(shù)搜索過(guò)程中引入了目標(biāo)偏置的思想，減小了在全局狀態(tài)空間均勻采樣耗費(fèi)的代價(jià)，但避免不了算法的低效擴(kuò)展。在上述算法的基礎(chǔ)上通過(guò)設(shè)置虛擬目標(biāo)點(diǎn)和變步長(zhǎng)來(lái)對(duì)算法進(jìn)行改進(jìn)。

3.2.1 設(shè)置虛擬目標(biāo)點(diǎn)

為了提高算法的搜索效率，在雙向RRT算法擴(kuò)展時(shí)設(shè)置虛擬目標(biāo)點(diǎn)，使隨機(jī)樹(shù)在目標(biāo)偏置過(guò)程中把虛擬目標(biāo)點(diǎn)當(dāng)作目標(biāo)點(diǎn)進(jìn)行擴(kuò)展。虛擬目標(biāo)點(diǎn)產(chǎn)生的步驟如下：

（1）當(dāng)初始點(diǎn)和目標(biāo)點(diǎn)的連線(xiàn)和障礙物沒(méi)有發(fā)生碰撞，則取初始點(diǎn)和目標(biāo)點(diǎn)連線(xiàn)的中點(diǎn)作為虛擬目標(biāo)點(diǎn)。

（2）當(dāng)初始點(diǎn)和目標(biāo)的之間的連線(xiàn)和障礙物發(fā)生碰撞時(shí)，設(shè)初始點(diǎn)的坐標(biāo)為(x1，y1，z1)，目標(biāo)點(diǎn)的坐標(biāo)為(x2，y2，z2)，直線(xiàn)L1的方程為：

過(guò)初始點(diǎn)和目標(biāo)點(diǎn)的中點(diǎn)A(xm，ym，zm) 作直線(xiàn)L1的垂線(xiàn)L2和L3，直線(xiàn)L2和L3分別平行于水平面和鉛錘面。設(shè)直線(xiàn)L2和L3的方向向量分別為(n1，m1，p1)和(n2，m2，p2)，L2和L3的方程分別為：

以中點(diǎn)A(xm/2，ym/2，zm/2)為球心，以r為半徑，則球的方程為：

聯(lián)立式（3）～式（5）可求得球O和直線(xiàn)L2、L3的交點(diǎn)，若球O和直線(xiàn)L2、L3的交點(diǎn)都在障礙物內(nèi)，則以球半徑增大球的半徑，再進(jìn)行計(jì)算，直到球和直線(xiàn)L2、L3至少有一個(gè)交點(diǎn)不在障礙物內(nèi)為止。若只有一個(gè)交點(diǎn)在障礙物外時(shí)，則把該點(diǎn)作為虛擬目標(biāo)點(diǎn)，若有多個(gè)交點(diǎn)都在障礙物外時(shí)，則把距離障礙物遠(yuǎn)的點(diǎn)作為虛擬目標(biāo)點(diǎn)。

3.2.2 改變步長(zhǎng)

為了進(jìn)一步提高算法的搜索效率，在障礙物周?chē)O(shè)置一個(gè)半徑為R的球形安全區(qū)域，在新節(jié)點(diǎn)生成時(shí)，先判斷該節(jié)點(diǎn)是否在安全區(qū)域內(nèi)，若在安全區(qū)域內(nèi)時(shí)，則改變步長(zhǎng)生成新的節(jié)點(diǎn)且不進(jìn)行碰撞檢測(cè)。若不在安全區(qū)域內(nèi)時(shí)，則以原來(lái)的步長(zhǎng)生成新節(jié)點(diǎn)并進(jìn)行碰撞檢測(cè)。變步長(zhǎng)生成新節(jié)點(diǎn)的過(guò)程，如圖3所示。

圖3 變步長(zhǎng)生成新節(jié)點(diǎn)過(guò)程Fig.3 Variable Step Size to Generate New Node Process

生成新節(jié)點(diǎn)的具體過(guò)程如下。

以Xnearst為父節(jié)點(diǎn)生成的新節(jié)點(diǎn)Xnew1在安全區(qū)域內(nèi)，則改變步長(zhǎng)重新生成新節(jié)點(diǎn)Xnew2，保留Xnew2，舍去Xnew1。生成的新節(jié)點(diǎn)Xnew3在非安全區(qū)域時(shí)，則連接X(jué)new3Xnearst進(jìn)行碰撞檢測(cè)，若發(fā)生碰撞，則舍去Xnew3，否則保留Xnew3。Xnew重新生成，如式（6）所示。

式中：p1—步長(zhǎng)增加系數(shù)。

3.3 縮短路徑

雖然在改進(jìn)RRT算法中加入添加了目標(biāo)偏置點(diǎn)和變步長(zhǎng)，但搜索到的避障路徑還是存在著冗余節(jié)點(diǎn)多的問(wèn)題。針對(duì)這一問(wèn)題，對(duì)避障路徑中的冗余節(jié)點(diǎn)進(jìn)行雙向刪除取最優(yōu)的處理，分別從初始點(diǎn)和目標(biāo)點(diǎn)對(duì)路徑中的冗余節(jié)點(diǎn)進(jìn)行刪除，比較兩次刪除冗余節(jié)點(diǎn)后路徑的長(zhǎng)度，保留較短路徑的節(jié)點(diǎn)。這樣雖然不能保證路徑最優(yōu)，但還是能夠有效的縮短路徑。刪除冗余節(jié)點(diǎn)原理，如圖4所示。

圖4 刪除冗余節(jié)點(diǎn)原理Fig.4 Principle of Deleting Redundant Nodes

其詳細(xì)步驟如下：

（1）通過(guò)改進(jìn)RRT 算法得到從初始點(diǎn)到目標(biāo)點(diǎn)的一系列無(wú)碰撞路徑點(diǎn)集M1。

（2）首先從初始點(diǎn)連接路徑點(diǎn)1，如果初始點(diǎn)和路徑點(diǎn)1之間未發(fā)生碰撞，則初始點(diǎn)再連接路徑點(diǎn)2，依次下去，直到兩點(diǎn)之間發(fā)生碰撞，則將發(fā)生碰撞的路徑點(diǎn)的前一個(gè)點(diǎn)保留，并且以該節(jié)點(diǎn)為父節(jié)點(diǎn)再次執(zhí)行上述操作，直到到達(dá)目標(biāo)點(diǎn)。

（3）從目標(biāo)點(diǎn)開(kāi)始連接前一個(gè)節(jié)點(diǎn)，重復(fù)步驟（2），直到到達(dá)初始點(diǎn)。

（4）分別比較從初始點(diǎn)和目標(biāo)點(diǎn)刪除冗余節(jié)點(diǎn)后路徑的長(zhǎng)度，保留較短路徑的節(jié)點(diǎn)，連接保留的路徑節(jié)點(diǎn)。

3.4 路徑平滑處理

刪除避障路徑中冗余節(jié)點(diǎn)雖然縮短了路徑的長(zhǎng)度，但路徑在轉(zhuǎn)折點(diǎn)處存在著不平滑問(wèn)題，針對(duì)這一問(wèn)題，采用三次B樣條曲線(xiàn)對(duì)避障路徑進(jìn)行平滑處理。B樣條曲線(xiàn)的方程如下：

式中：Ni，k(u)(i=0，1，…，n)—k次B樣條基函數(shù)；

Pi(i=0，1，…，n)—控制頂點(diǎn)，決定B樣條的控制多邊形。

k次B樣條基函數(shù)的遞推公式為：

式中：k—B樣條函數(shù)的階數(shù)；i—B樣條函數(shù)的序列號(hào)；記0/0=0。

通過(guò)關(guān)節(jié)空間中對(duì)應(yīng)的位置—時(shí)間序列點(diǎn)，將其作為型值點(diǎn)反算控制頂點(diǎn)，再利用三次B樣條插值法進(jìn)行軌跡的擬合，保證了運(yùn)動(dòng)軌跡經(jīng)過(guò)所對(duì)應(yīng)的工作空間位置，其第i段曲線(xiàn)的函數(shù)表達(dá)式為：

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

為了驗(yàn)證改進(jìn)算法和對(duì)改進(jìn)算法搜索到的避障路徑優(yōu)化的有效性，分別在MATLAB平臺(tái)和ROS平臺(tái)上進(jìn)行仿真驗(yàn)證。

4.1 MATLAB仿真實(shí)驗(yàn)

在MATLAB 仿真實(shí)驗(yàn)中，機(jī)械臂的工作空間為（100×100×100）cm，初始點(diǎn)坐標(biāo)為（3，34，40），目標(biāo)點(diǎn)坐標(biāo)為（74，34，40），路徑的搜索步長(zhǎng)為1cm，設(shè)定隨機(jī)樹(shù)產(chǎn)生隨機(jī)點(diǎn)最大個(gè)數(shù)為2500，如果在設(shè)定的產(chǎn)生最大隨機(jī)點(diǎn)數(shù)范圍內(nèi)找到路徑則認(rèn)為路徑搜索成功，反之則認(rèn)為路徑搜索失敗。

在同等參數(shù)設(shè)置下分別對(duì)雙向RRT算法，目標(biāo)偏置RRT算法以及改進(jìn)的RRT算法進(jìn)行20次實(shí)驗(yàn)，雙向RRT算法機(jī)械臂末端路徑搜索圖，如圖5（a）所示。偏置RRT算法機(jī)械臂末端路徑搜索圖，如圖5（b）所示。改進(jìn)RRT算法機(jī)械臂末端路徑搜索，如圖5（c）所示。

圖5 三種算法機(jī)械臂末端路徑搜索圖Fig.5 Three Algorithm Manipulator End Path Search Diagram

取這20次實(shí)驗(yàn)的平均值，三種算法的實(shí)驗(yàn)對(duì)比數(shù)據(jù)，如表1所示。改進(jìn)RRT 算法相對(duì)比雙向RRT 算法和目標(biāo)偏置RRT 算法，路徑長(zhǎng)度分別縮短了約31%和12%，路徑搜索時(shí)間和采樣點(diǎn)個(gè)數(shù)分別為雙向RRT 算法的5%和15%，目標(biāo)偏置RRT 算法的47%和33%。改進(jìn)RRT算法相對(duì)比雙向RRT算法在路徑搜索成功率上也有了提高。為了進(jìn)一步優(yōu)化改進(jìn)RRT算法搜索到的避障路徑，對(duì)避障路徑進(jìn)行雙向刪除取最短路徑的處理，最后利用三次B樣條曲線(xiàn)對(duì)刪除冗余節(jié)點(diǎn)后的避障路徑平滑處理。刪除冗余節(jié)點(diǎn)和用三次B樣條曲線(xiàn)優(yōu)化路徑，如圖6所示。圖6中折線(xiàn)為雙向刪除冗余節(jié)點(diǎn)取最優(yōu)得到的路徑圖，曲線(xiàn)為三次B樣條曲線(xiàn)對(duì)刪除冗余節(jié)點(diǎn)后避障路徑平滑處理圖。

表1 三種算法實(shí)驗(yàn)數(shù)據(jù)對(duì)比Tab.1 Comparison of Experimental Data of Three Algorithms

圖6 刪除冗余節(jié)點(diǎn)和三次B樣條曲線(xiàn)優(yōu)化路徑圖Fig.6 Delete Redundant Nodes and Cubic B-Spline Curve to Optimize the Path Diagram

4.2 ROS機(jī)械臂路徑規(guī)劃實(shí)驗(yàn)

在ROS仿真平臺(tái)上，對(duì)UR5機(jī)械臂進(jìn)行路徑規(guī)劃實(shí)驗(yàn)，把三次B樣條曲線(xiàn)優(yōu)化后的路徑節(jié)點(diǎn)通過(guò)KDL（The Kinematic and Dynamics Library）求解器逆解到關(guān)節(jié)空間，來(lái)實(shí)現(xiàn)對(duì)機(jī)械臂的控制。機(jī)械臂初始狀態(tài)，如圖7（a）所示。機(jī)械臂運(yùn)動(dòng)軌跡，如圖7（b）所示。機(jī)械臂到達(dá)目標(biāo)點(diǎn)，如圖7（c）所示。

圖7 機(jī)械臂運(yùn)動(dòng)狀態(tài)Fig.7 Manipulator Movement State

機(jī)械臂通過(guò)反解三次B樣條曲線(xiàn)優(yōu)化后的路徑節(jié)點(diǎn)順利地避開(kāi)障礙物且到達(dá)目標(biāo)點(diǎn)。

5 結(jié)論

結(jié)合雙向RRT算法雙樹(shù)擴(kuò)展和目標(biāo)偏置RRT算法目標(biāo)偏置的優(yōu)點(diǎn)，并在其中設(shè)置虛擬目標(biāo)點(diǎn)和變步長(zhǎng)來(lái)對(duì)RRT算法進(jìn)行改進(jìn)。MATLAB仿真實(shí)驗(yàn)表明，改進(jìn)RRT算法在保證路徑搜索成功率的同時(shí)不僅對(duì)路徑搜索效率有了較大的提升，路徑長(zhǎng)度也有明顯的縮短。考慮到機(jī)械臂實(shí)際運(yùn)行情況，采用雙向刪除取最優(yōu)的策略刪除避障路徑中的冗余節(jié)點(diǎn)，并利用三次B樣條曲線(xiàn)對(duì)刪除冗余節(jié)點(diǎn)后的避障路徑進(jìn)行平滑處理。ROS平臺(tái)仿真實(shí)驗(yàn)表明，機(jī)械臂反解優(yōu)化后的路徑節(jié)點(diǎn)能順利的避開(kāi)障礙物到達(dá)目標(biāo)點(diǎn)。