改進的快速擴展隨機樹路徑規劃算法*

孫豐財， 張亞楠， 史旭華

(寧波大學 信息科學與工程學院,浙江 寧波 315000)

改進的快速擴展隨機樹路徑規劃算法*

孫豐財， 張亞楠， 史旭華

(寧波大學 信息科學與工程學院,浙江 寧波 315000)

針對快速擴展隨機樹(RRT)路徑規劃算法缺乏穩定性和偏離最優解的問題，提出了一基于RRT的偏向性路徑搜索算法(m-RRT)。m-RRT采用生成隨機點向量組的形式對隨機點選取策略進行了優化，改善快速擴展隨機樹的不確定性，減少不必要的擴展，而加快向目標位置搜索的速度，且得到的路徑優于RRT算法的結果。通過其在二維平面路徑規劃和三維機械臂路徑規劃的測試,表明其具有一定的應用價值。

路徑規劃； 機械臂； 快速擴展隨機樹算法； 避障； 機器人操作系統

0 引 言

機器人路徑規劃即通過某些性能(如時間、距離、能量)指標來規劃出一條從初始位置到目標位置的最優或者較優的線路。目前，機械臂路徑規劃方法主要有細胞分解法、人工勢場法、隨機路標法(probabilistic roadmap，PRM)和快速擴展隨機樹(rapidly-exploring random tree，RRT)法等[1～3]。

針對傳統RRT算法的缺陷，Burget F，Bennewitz M等人[4]采用雙向隨機搜索樹(Bi2RRT) 搜索算法，提高了搜索效率，由于該算法較原始RRT算法有更好的收斂性，因此，在目前路徑規劃中很常見。Melchior N A[5]提出的粒子RRT算法考慮了地形的不確定性，保證了在不確定性環境下隨機搜索樹的擴展。Kuffner J J等人[6]提出了RRT2connect算法，使得節點的擴展效率大大提高；同時王道威等人[7]采用動態步長改善了RRT的不確定性，馮林等人[8]通過對比優化的方法使得在復雜環境下的規劃穩定性增強，宋金澤等人[9]通過添加非完整性約束及雙向多步擴展方式提高了搜索效率。

本文針對RRT算法搜索缺乏穩定性和偏離最優解等缺點，對RRT算法進行了相應的改進，通過實驗發現改進算法在二維環境和三維環境均能取得較好的效果。因此,算法對二維、三維環境的路徑規劃，尤其是機械臂的路徑規劃具有一定的參考價值。

1 RRT算法流程

1.1 RRT算法

以節點qstart為初始點，qgoal為目標點，建立搜索樹T，其初始化和擴展過程步驟如下:

1)選擇初始點qstart為根節點。

2)在位形空間中隨機選取采樣點qrand。

3)在已經產生的搜索樹上，搜索一個距離采樣點qrand最近的節點qnearest。

4)在qnearest和qrand的連線上，以一定的步長r從qnearest出發創造一個新的節點qnew。如果從qnearest到qnew的擴展過程中無碰撞發生，則將這個新的節點qnew加入到搜索樹中，并同時將從qnearest到qnew的邊插入到搜索樹中；反之，則無擴展發生。

重復上述步驟，在用戶定義約束的限制下，不斷計算qnew與qgoal的距離，直至其小于給定的距離閾值。

圖1 RRT算法核心步驟

RRT算法的隨機采樣擴張特性，導致其收斂到目標點位置的速度比較慢，耗費的空間資源比較大，同時搜索出的路徑往往偏離最短路徑；為了提高算法的效率和性能，需不斷對該算法進行改進。

1.2 改進后的RRT算法

針對擴展節點的選取，提出了改進RRT算法，即m-RRT算法。

首先，m-RRT算法采用大多數變異RRT算法的通用策略，在標準的RRT算法基礎上加入了偏向策略，即在一定的小概率情況下將隨機采樣節點qrand設置為目標點，以促進搜索因子盡快趨向目標點，以提高搜索效率。

其次，與標準RRT算法步驟(2)不同，m-RRT算法每次隨機采樣m個節點，按照與目標點的距離排序構成隨機序列，將距目標點最近的點作為隨機點qrand開始探索，如果發現發生碰撞導致無法擴展，則需取次距離點進行計算；當隨機序列中首次未碰撞的節點執行完成，或者隨機序列為空之后再次隨機生成m個采樣節點，繼續上述步驟。

算法減少了不必要的探索方向，一定幾率地減少了再次遇到障礙物的可能性，保留了對目標位置的全局趨向性；同時，依次嘗試保證了節點有能力跳出障礙區域，避免陷入局部，有利于快速尋找到目標點。本文通過實驗對比m-RRT算法的搜索時間效率和結果路徑優劣性，選擇每輪隨機采樣個數m=4，此時算法性能表現良好。m-RRT算法主要流程如下：

1)初始化起點qstart和目標點qgoal并在位形空間內建立障礙物模型。

2)在位形空間中隨機選取采樣點qrand,并擴展新生成節點qnew直至到達目標節點qgoal。

a.在空間中隨機采樣4個位置節點，并按照離目標點的距離排序存入qrand_arr隨機序列，選取離目標點距離最近點設為節點qrand。

(1)

機械臂距離公式(各關節角度距離其目標角度之和)

(2)

b.按照標準RRT算法步驟選擇隨機樹T中離隨機采樣點qrand最近的節點為qnearest，按照一定步長進行生成擴展節點qnew，同時檢測從節點qnearest到節點qnew是否與障礙物發生碰撞，如果發生碰撞，則在隨機采樣點qrand_arr隨機序列中選擇次位的節點成為新的隨機節點qrand。

3)如果沒有發生碰撞則在此方向繼續擴展節點并收入隨機樹中，繼續沿此方向擴展，直至發生碰撞，此時，舍棄隨機序列qrand_arr，重新生成4個隨機點構成新的隨機序列。

4)當目標點qgoal與新生成的節點qnew距離小于或等于步長時，將qnew=qgoal，此時如無碰撞則程序結束，若有碰撞則繼續步驟(2)。

5)將得到的路徑點進行保存，得出結果。

改進后算法在二維平面路徑規劃中可以大量減少規劃時間，并且整棵樹的生長偏向于目標點，隨著樹的生長更容易找出更好的路徑；在三維空間中，不需耗費巨大的空間資源，同時，m-RRT算法由于樹的趨向性生長避免了對許多無意義空間的探索，減少了標準RRT算法執行時間，相比于RRT-Start等追求接近最優解的算法，其規劃結果也很理想。

2 實驗對比

2.1 二維有障礙環境下算法效果

2.1.1 簡單環境下算法比較

如圖2所示，實驗分別對piz-RRT算法[10]、標準RRT算法、RRT-star算法[11]、m-RRT算法進行了比較，其中m-RRT算法給出了當隨機序列節點數m取值分別為2，4，6時的運行效果。演示圖均為左側圓形點位置為起始點右側圓形點位置為目標點進行規劃。在簡單環境下，對比4種算法效果發現m-RRT算法擴展節點大量減少，樹的生成有一種趨向性，空間資源浪費很少；同時通過不同隨機序列節點數m的取值，可以明顯發現規劃路徑慢慢趨向于穩定和更優化。

圖2 簡單環境下4種算法比較

2.1.2 復雜環境下算法比較

復雜環境中，不僅障礙物數量增多，起始點與目標點也不易直接搜尋，很大程度提高了路徑規劃的難度。如圖3所示，同樣將以上4種算法進行比較，其中，m-RRT算法給出了當隨機序列節點數m分別取值為2，4，6時的運行效果。演示圖均為左側圓形點位置為起始點，右側圓形點為止為目標點進行規劃。通過對比可以發現即使環境變得復雜，m-RRT算法依然能夠利用很少的擴展到達目標位置，不僅消耗空間資源少，規劃路徑也具有明顯的優勢。

圖3 復雜環境下4種算法比較

通過以上實驗可以發現：對于m-RRT算法，不同m的取值使得規劃路徑慢慢趨向于穩定和更優化，但經過對比發現這種路徑的優化是通過犧牲時間效率達到的，并且當m節點數增加太多也會使m-RRT算法的優勢降低，通過綜合對比最終選用m=4，此時路徑規劃算法無論規劃結果或者規劃耗時均較為滿意，視為最佳方案。

2.1.3 二維路徑規劃效率對比

以上實驗的4種算法中，從耗費空間資源的角度，RRT-star算法和標準RRT算法耗費最多，而m-RRT算法與piz-RRT算法相差不多，相比前兩者，在空間資源的消耗上有明顯的優化，付出的空間代價很小。從規劃結果來看，其他3種算法的隨機性較m-RRT算法強，m-RRT算法很大程度上降低了隨機程度。同時，通過表1中所示的5種不同環境下(從第一組到第五組障礙物依次增多)，4種算法的路徑規劃耗時對比，可以看出：RRT-star算法時間復雜度成倍提升，其次為標準RRT算法，這兩者在時間的消耗上遠大于piz-RRT和m-RRT算法。可見，隨著環境復雜度的增大，m-RRT也同樣獲得了很好的效果。實驗證明了m-RRT算法規劃出的路徑更趨向于平穩，在大多數情況下優于其他3種算法，且規劃路徑較為緊湊，也更接近于理想路徑。

表1 4種改進RRT算法效率對比 s

2.2 三維有障礙環境下機械臂路徑規劃算法效果

2.2.1 機械臂路徑規劃演示

如圖4所示為運用m-RRT算法的路徑規劃運行效果。圖(a)中綠色機械臂為起始位置，橘色機械臂代表目標位置，圖4(b)～圖4(f)依次給出了一次規劃的結果，整體運行路線如圖4(g)和圖4(h)所示。可以看出：在有障礙物的環境下運用m-RRT算法能夠完整地解決此類路徑規劃問題，將機械臂移至側面的行為完全沒有接觸到障礙物，并且機械臂先運動到上方，依次順著箱體表面移動到下方，效果理想，證明了m-RRT算法在三維機械臂的路徑規劃中的有效性。

圖4 三維環境下改進算法路徑規劃結果

2.2.2 機械臂路徑規劃效率對比

圖5所示為2種不同三維環境下標準RRT算法和m-RRT算法的效率比較，分為不同組別，每組取平均值進行比較，可以看出：在障礙物較多的情況下2種RRT算法在解決路徑規劃問題的效率有較大差異，標準RRT算法耗時較多且并不穩定，m-RRT算法相對更加穩定，且不論環境是否復雜，其執行效率相對優越。同時，在三維機械臂路徑規劃實驗中發現RRT-star等算法效率較差，耗時過久且時常無法找到解，并不適用于此類規劃。

圖5 復雜環境和簡單環境情況下效率對比

3 結束語

通過以上二維平面和三維機械臂路徑規劃研究發現，m-RRT算法明顯提高了RRT路徑規劃的穩定性，并且有利于規劃出接近于最優路徑的較優路徑，相對于標準RRT算法，其效率更高，無論在算法效率還是路徑質量，均體現出了很大的優越性。同時m-RRT算法在三維機械臂路徑規劃上也能夠取得較好的效果。實驗表明：m-RRT算法在路徑規劃上具有一定的應用價值。

[1] 王海英,蔡向東,尤 波,等.基于遺傳算法的移動機器人動態路徑規劃研究[J].傳感器與微系統,2007,26(8):32-34.

[2] 王春華,邱立鵬,潘德文.改進蟻群算法的機器人焊接路徑規劃[J].傳感器與微系統,2017,36(2):75-77.

[3] 張云峰,馬振書,孫華剛，等.基于改進快速擴展隨機樹的機械臂路徑規劃[J].火力與指揮控制,2016,41(5):25-30.

[4] Burget F,Bennewitz M,Burgard W.BI^2RRT:An efficient sampling-based path planning framework for task-constrained mobile manipulation[C]∥IEEE/RSJ International Conference on Inteligent Robots & Systems,2016.

[5] Melchior N A,Simmons R.Particle RRT for path planning with uncertainty[C]∥IEEE International Conference on Robotics & Automation,2007:1617-1624.

[6] Kuffner J J,Lavalle S M.RRT-connect:An efficient approach to single-query path planning[C]∥IEEE International Conference on Robotics & Automation,2000:995-1001.

[7] 王道威,朱明富,劉 慧.動態步長的RRT 路徑規劃算法[J].計算機技術與發展,2016,26(3):105-107.

[8] 馮 林,賈菁輝.基于對比優化的RRT路徑規劃改進算法[J].計算機工程與應用, 2011,47(3):210-213.

[9] 宋金澤,戴 斌,單恩忠,等.一種改進的RRT路徑規劃算法[J].電子學報,2010,38(s1):225-228.

[10] 徐 娜,陳 雄,孔慶生,等.非完整約束下的機器人運動規劃算法[J].機器人,2011,33(6):666-672.

[11] Islam F,Nasir J,Malik U,et al.RRT-smart:Rapid convergence implementation of RRT towards optimal solution[C]∥International Conference on Mechatronics and Automation,2012:1651-1656.

史旭華，女，通訊作者，教授，碩士生導師，主要從事模式識別與智能算法的研究工作，E—mail:928111455@qq.com。

Improved rapidly-exploring random tree path planning algorithm*

SUN Feng-cai， ZHANG Ya-nan， SHI Xu-hua

(College of Information Science and Engineering,Ningbo University,Ningbo 315000,China)

Because the rapidly-exploring random tree(RRT)path planning algorithm is unstable and not optimal,propose a biased path search strategy which is calledm-RRT.By generating a random point vectors to optimize the strategy of random points selection,the uncertainty of RRT searching can be improved,meanwhile,the expansion of searching tree can also be reduced.So,it can improve the searching speed to the destination,and the path got bym-RRT is better than that by RRT.Through the two-dimensional path planning and three-dimensional manipulator path planning,the results show that it has certain application value.

path planning;manipulator; rapidly exploring random tree(RRT)algorithm; obstacle avoidance; robot operating system(ROS)

10.13873/J.1000—9787(2017)09—0129—03

2017—07—12

浙江省自然科學基金資助項目 (LY14F030004)；浙江省科技計劃資助項目 (2015C31017)；寧波市自然科學基金資助項目(2016A610092)

TP 24

A

1000—9787(2017)09—0129—03

孫豐財(1992-)，男，碩士研究生，研究方向為模式識別與智能算法。