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

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

1 引言

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

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

2 算法原理

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

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

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

2.2 障礙物碰撞檢測

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

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

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

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

3 RRT算法研究

3.1 傳統(tǒng)RRT算法

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

3.2 改進(jìn)RRT算法

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

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

為了提高算法的搜索效率，在雙向RRT算法擴(kuò)展時設(shè)置虛擬目標(biāo)點(diǎn)，使隨機(jī)樹在目標(biāo)偏置過程中把虛擬目標(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)的連線和障礙物沒有發(fā)生碰撞，則取初始點(diǎn)和目標(biāo)點(diǎn)連線的中點(diǎn)作為虛擬目標(biāo)點(diǎn)。

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

過初始點(diǎn)和目標(biāo)點(diǎn)的中點(diǎn)A(xm，ym，zm) 作直線L1的垂線L2和L3，直線L2和L3分別平行于水平面和鉛錘面。設(shè)直線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和直線L2、L3的交點(diǎn)，若球O和直線L2、L3的交點(diǎn)都在障礙物內(nèi)，則以球半徑增大球的半徑，再進(jìn)行計算，直到球和直線L2、L3至少有一個交點(diǎn)不在障礙物內(nèi)為止。若只有一個交點(diǎn)在障礙物外時，則把該點(diǎn)作為虛擬目標(biāo)點(diǎn)，若有多個交點(diǎn)都在障礙物外時，則把距離障礙物遠(yuǎn)的點(diǎn)作為虛擬目標(biāo)點(diǎn)。

3.2.2 改變步長

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

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

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

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

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

3.3 縮短路徑

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

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

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

（1）通過改進(jìn)RRT 算法得到從初始點(diǎn)到目標(biāo)點(diǎn)的一系列無碰撞路徑點(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)的前一個點(diǎn)保留，并且以該節(jié)點(diǎn)為父節(jié)點(diǎn)再次執(zhí)行上述操作，直到到達(dá)目標(biāo)點(diǎn)。

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

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

3.4 路徑平滑處理

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

式中：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ù)的序列號；記0/0=0。

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

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

為了驗(yàn)證改進(jìn)算法和對改進(jìn)算法搜索到的避障路徑優(yōu)化的有效性，分別在MATLAB平臺和ROS平臺上進(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），路徑的搜索步長為1cm，設(shè)定隨機(jī)樹產(chǎn)生隨機(jī)點(diǎn)最大個數(shù)為2500，如果在設(shè)定的產(chǎn)生最大隨機(jī)點(diǎn)數(shù)范圍內(nèi)找到路徑則認(rèn)為路徑搜索成功，反之則認(rèn)為路徑搜索失敗。

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

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

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

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

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

5 結(jié)論

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