基于改進RRT算法的機械臂路徑規劃*

蔡文濤， 鄧 屹， 張 靜,2， 張永波， 饒 爽， 陽 康

(1.西南科技大學 信息工程學院，四川 綿陽 621010；2.中國科學技術大學 信息科學技術學院，安徽 合肥 230026)

0 引 言

多關節串聯機械臂由于運動空間大、作業靈活，被廣泛應用在工業制造、軍事醫療等領域。而機械臂規劃算法是實現機械臂高效、靈巧、柔順作業的技術關鍵，一直是國內外相關專家和技術人員的研究焦點。

快速探索隨機樹(rapidly-exploring random tree，RRT)算法因能夠有效地搜索高維空間,解決機械臂非完整約束限制而被廣泛采用。但由于RRT算法導向性差，規劃空間增大時，存在所規劃的路徑平滑度差,且算法時間復雜度高的問題[1]。近年來，研究者們不斷地改進RRT算法力求解決這些問題，Burget F等人提出雙向隨機搜索樹搜索算法提高搜索效率，提高了其收斂速率[2]；孫豐財等人提出采用生成隨機點向量組的形式對隨機點選取策略進行了優化，改善RRT的不確定性，加快搜索速度[1～5]。

本文引入目標概率偏置策略，結合步長控制方法改進傳統RRT算法，并在MATLAB和機器人操作系統(robot operation system,ROS)平臺上搭建實驗，通過實驗驗證了算法的有效性。

1 碰撞檢測與機械臂建模

1.1 機械臂模型與D-H參數

本文研究對象為6自由度串聯機械臂，其D-H坐標建模如圖1所示，相關D-H參數見表1。

圖1 6自由機械臂模型

θi/(°)αi-1/(°)ai-1/cmdi/cm關節角/(°)θ1000-360～360θ2907.90-105～90 θ308.10-100～70 θ4905.35.3-360～360θ5905.00-80～70θ6901.21.2-360～360

1.2 機械臂運動學

根據D-H建模方法，串聯機械臂相鄰兩關節間的齊次變換矩陣

(1)

(2)

式中n,o,a3個列向量的前3行表示機械臂的姿態[4]；p向量的前3行代表末端執行器在基坐標系下的三維空間坐標。

1.3 碰撞檢測法

本文采用幾何包絡法簡化機械臂與障礙物的碰撞檢測模型，如圖2所示。

圖2 碰撞檢測模型示意

機械臂連桿簡化為圓柱體模型，不規則障礙物簡化為球體模型，b0(x,y,z)為球心世界坐標，球半徑為R0，機械臂連桿i的截面半徑為r0。簡化后碰撞檢測問題轉換為障礙物球心到連桿軸心到距離d與r0+R0的數量關系：當d≥r0+R0時，連桿i與障礙物未發生碰撞，否則，當d

2 基于RRT的改進路徑規劃算法

2.1 RRT算法基本原理

RRT算法是從空間中1個起始節點Xs出發，通過隨機采樣，不斷增加新節點Xnw，生成1個隨機擴展樹。當增加的新節點中有目標節點Xt時，Xs到Xt就會至少有1條路徑[6]。首先，以Xs作為樹根，隨機采樣得到Xr；然后，找到隨機樹中離Xr最近的節點Xnr，在Xr指向Xn方向上以一定步長生成Xnw，若Xnw沒碰到障礙物，則加入隨機樹，若碰到障礙物，則重新采樣；最后，若Xnw等于Xt，則算法結束，否則繼續生長。RRT樹生長過程如圖3所示，Xs到Xt間加粗線條為路徑，空白區域為Cfree空間[6,9]。

圖3 RRT樹生長的過程

由于RRT算法采樣的隨機性，新生成的節點通常遠離目標節點，隨著規劃空間變大，隨機樹盲目生長和低效擴展的弊端會更明顯。

2.2 目標概率偏置與步長優化策略

基于隨機采樣的RRT算法缺乏導向性，借鑒啟發式算法的思想，在隨機樹的生長過程中設置一個偏置概率參數，可使隨機采樣點更導向于目標點，提高RRT算法的導向性[3]。若不受障礙物干涉，節點樹將繼續向目標點方向擴展，應用此方法可在很大程度上縮短RRT路徑規劃的時間。但當障礙物較多時，RRT算法擴展樹則無法繞過障礙物到達目標點，此時需要使用歩長控制策略[5]，控制目標點方向的步長，改善隨機樹的避障能力。

RRT的原始新節點位置的計算

(3)

式中p為RRT的生長最小單位，稱為步長。

步長控制RRT算法(S-RRT)新節點位置的計算

(4)

新節點位置公式通過添加目標點Xt方向上的分量p2，使不同步長對新節點的位置會有影響。當p2>p1時，新節點偏向于Xt，擴展樹向目標點方向擴展。當p2p1，使擴展樹盡可能向目標點方向擴展。若遇到障礙取p2

2.3 改進RRT路徑規劃算法描述

結合目標概率偏置與步長優化策略，使隨機樹生長時以P概率偏置目標節點采樣，以1-P概率隨機采樣，并進行碰撞檢測，若檢p2>p1測到碰撞則減小目標節點方向的步長否則增加目標節點方向步長。改進RRT(I-RRT)基本算法流程如下：

1)初始化Xs，Xt，p2，p2，n；

2)若節點樹上新生節點Xnw等于Xt，則結束生長，否則進行下一步；

3)以概率P使Xr=Xt，1-P概率隨機采樣生成隨機節點Xr；

4)進行碰撞檢測，若連線(Xnr,Xr)與空間障礙物碰撞則回到步驟(2)，且變量n的值累加，p2=p2+n，p1=p1-n，必須限定n值大小，若無碰撞則下一步；

6)從目標節點到起始節點搜索一條無碰撞路徑。

3 實驗仿真

3.1 MATLAB實驗搭建與結果分析

本文構建基于MATLAB平臺的實驗，比較本文I-RRT算法、目標偏置P-RRT、步長控制S-RRT與RRT的路徑規劃過程，驗證本文改進方法的有效性。仿真環境硬件信息：Intel Core i5—6400 CPU，主頻2.7 GHz，內存4 G。實驗各項參數單位均為米(m)，初始設定如：偏置概率P=0.2，初始步長p1=p2=20，變量n=10，起始點為(0,0)，目標點為(850,300)。

實驗結果圖范圍為900×900，圓形區域為障礙物，方點為起始點，圓點為目標點，窄線條為擴展節點樹，寬線條為路徑。對比圖4(a)，(c)，(d)可得：I-RRT與P-RRT比RRT擴展節點數少，導向性好，但P-RRT的避障能力不強；比較圖4(a)，(b)，(d)可知：I-RRT與S-RRT比RRT的節點樹簡單，路徑更平滑。由實驗結果得：I-RRT算法導向性好，隨機樹更簡單，路徑平滑度也更好。

圖4 改進的RRT與RRT比較

表2為以上仿真實驗的1 000次實驗統計值結果記錄。由表2知，I-RRT算法，較P-RRT,S-RRT,RRT等算法平均耗時大大降低，其中I-RRT算法較RRT算法時間復雜度優化至30 %；同時I-RRT算法導向性增強，在迭代次數與擴展節點數上較前3種算法也都有明顯減少，路徑規劃成功率明顯提高。

表2 仿真實驗環境下的數據比較

3.2 ROS機械臂避障規劃實驗

本文采用I-RRT末端所規劃的路徑反解到關節空間下的方法[7]實現機械臂控制，基于公式(2)，機械臂逆運動學求解使用KDL(The Kinematics and Dynamics Library)求解器完成[8,9]。其中，機械臂避障實驗任務描述如圖5所示。仿真場景包含1個機械臂，1個工作臺，1個球形包絡的障礙物。實驗要求機械臂從初始位姿完成任務1，并從任務1狀態避開障礙完成任務2。

圖5 機械臂避障任務描述

如圖6所示，機械臂完成任務1，并順利地越過避障完成任務2。

圖6 改進RRT算法避障規劃仿真過程

由實驗可知，利用本文的改進算法實現了機械臂避障規劃，保證機械臂順利地避開障礙物并完成任務位姿，證明了算法的有效性。另外，本文使用I-RRT算法進行連續500次運行，機械臂490次成功完成避障任務，結果表明了I-RRT算法有效且可靠。

4 結 論

仿真實驗表明：改進后的算法(I-RRT)，導向性明顯增強，較RRT時間復雜度降低至30 %，且低于S-RRT和P-RRT等算法的時間復雜度，該算法的路徑規劃成功率也有明顯提升，達99.65 %。進一步提升該算法在動態障礙物環境中的避障能力，將是后續研究工作的重點。