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

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

（新疆大學機械工程學院，新疆 烏魯木齊 830047）

1 引言

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

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

2 算法原理

2.1 機械臂運動學模型建立

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

圖1 機械臂模型Fig.1 Manipulator Model

2.2 障礙物碰撞檢測

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

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

把半徑為r的機械臂連桿包絡盒簡化成線段AB，機械臂連桿包絡盒和障礙物包絡盒之間的碰撞關系可轉換為線段AB和障礙物包絡盒六個平面之間的碰撞關系。為便于計算把障礙物進行膨化處理，膨脹尺寸為r，建立障礙物包絡盒坐標關系。

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

3 RRT算法研究

3.1 傳統(tǒng)RRT算法

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

3.2 改進RRT算法

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

3.2.1 設置虛擬目標點

為了提高算法的搜索效率，在雙向RRT算法擴展時設置虛擬目標點，使隨機樹在目標偏置過程中把虛擬目標點當作目標點進行擴展。虛擬目標點產生的步驟如下：

（1）當初始點和目標點的連線和障礙物沒有發(fā)生碰撞，則取初始點和目標點連線的中點作為虛擬目標點。

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

過初始點和目標點的中點A(xm，ym，zm) 作直線L1的垂線L2和L3，直線L2和L3分別平行于水平面和鉛錘面。設直線L2和L3的方向向量分別為(n1，m1，p1)和(n2，m2，p2)，L2和L3的方程分別為：

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

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

3.2.2 改變步長

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

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

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

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

式中：p1—步長增加系數。

3.3 縮短路徑

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

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

其詳細步驟如下：

（1）通過改進RRT 算法得到從初始點到目標點的一系列無碰撞路徑點集M1。

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

（3）從目標點開始連接前一個節(jié)點，重復步驟（2），直到到達初始點。

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

3.4 路徑平滑處理

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

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

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

k次B樣條基函數的遞推公式為：

式中：k—B樣條函數的階數；i—B樣條函數的序列號；記0/0=0。

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

4 實驗結果與討論

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

4.1 MATLAB仿真實驗

在MATLAB 仿真實驗中，機械臂的工作空間為（100×100×100）cm，初始點坐標為（3，34，40），目標點坐標為（74，34，40），路徑的搜索步長為1cm，設定隨機樹產生隨機點最大個數為2500，如果在設定的產生最大隨機點數范圍內找到路徑則認為路徑搜索成功，反之則認為路徑搜索失敗。

在同等參數設置下分別對雙向RRT算法，目標偏置RRT算法以及改進的RRT算法進行20次實驗，雙向RRT算法機械臂末端路徑搜索圖，如圖5（a）所示。偏置RRT算法機械臂末端路徑搜索圖，如圖5（b）所示。改進RRT算法機械臂末端路徑搜索，如圖5（c）所示。

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

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

表1 三種算法實驗數據對比Tab.1 Comparison of Experimental Data of Three Algorithms

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

4.2 ROS機械臂路徑規(guī)劃實驗

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

圖7 機械臂運動狀態(tài)Fig.7 Manipulator Movement State

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

5 結論

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