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

孫豐財， 張亞楠， 史旭華

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

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

孫豐財， 張亞楠， 史旭華

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

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

路徑規(guī)劃； 機械臂； 快速擴展隨機樹算法； 避障； 機器人操作系統(tǒng)

0 引 言

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

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

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

1 RRT算法流程

1.1 RRT算法

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

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

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

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

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

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

圖1 RRT算法核心步驟

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

1.2 改進后的RRT算法

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

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

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

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

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

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

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

(1)

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

(2)

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

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

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

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

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

2 實驗對比

2.1 二維有障礙環(huán)境下算法效果

2.1.1 簡單環(huán)境下算法比較

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

圖2 簡單環(huán)境下4種算法比較

2.1.2 復雜環(huán)境下算法比較

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

圖3 復雜環(huán)境下4種算法比較

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

2.1.3 二維路徑規(guī)劃效率對比

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

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

2.2 三維有障礙環(huán)境下機械臂路徑規(guī)劃算法效果

2.2.1 機械臂路徑規(guī)劃演示

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

圖4 三維環(huán)境下改進算法路徑規(guī)劃結果

2.2.2 機械臂路徑規(guī)劃效率對比

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

圖5 復雜環(huán)境和簡單環(huán)境情況下效率對比

3 結束語

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

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

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

[3] 張云峰,馬振書,孫華剛，等.基于改進快速擴展隨機樹的機械臂路徑規(guī)劃[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] 王道威,朱明富,劉 慧.動態(tài)步長的RRT 路徑規(guī)劃算法[J].計算機技術與發(fā)展,2016,26(3):105-107.

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

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

[10] 徐 娜,陳 雄,孔慶生,等.非完整約束下的機器人運動規(guī)劃算法[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-)，男，碩士研究生，研究方向為模式識別與智能算法。