一種改進的RRT機器人路徑規劃算法研究

胡兵 向鳳紅 毛劍琳

摘 要：路徑規劃技術是目前機器人領域研究熱點，而路徑規劃算法是其核心內容。可變步長的快速隨機搜索樹（Rapidly-exploring Random Tree，RRT）算法在機器人路徑規劃算法中復雜度高、效率較低，針對這一問題，提出一種改進的RRT算法。在可變步長的隨機樹生長過程中，引入雙向生長策略，利用雙向生長特性，提高路徑搜索效率，解決了最優路徑與低效率間的矛盾。實驗仿真數據表明，改進后的RRT算法在路徑規劃中不僅算法復雜度低，且搜索效率提高了約一倍。

關鍵詞：快速隨機搜索樹；可變步長；雙向生長；路徑規劃；搜索效率

DOI：10.11907/rjdk.172931

中圖分類號：TP312

文獻標識碼：A 文章編號：1672-7800（2018）006-0013-04

Abstract：Path planning technology is currently a hot research in the field of robotics， and the path planning algorithm is the core content. Aiming at high complexity and low efficiency of RRT（Rapidly-exploring Random Tree） in robot path planning algorithm， this paper proposes an improved RRT algorithm on the basis of in the process of RRT. In the variable step size RRT random tree growth， the bidirectional growth strategy is introduced， and the efficiency of path search is improved by using the characteristic of bidirectional growth， and the contradiction between the optimal path and the low efficiency are solved. The results of simulation data show that the improved RRT algorithm not only has low complexity， good smoothness， but also improves the search efficiency twice as much in path planning.

Key Words：RRT； variable step-size； Bidirectional growth； path planning； search efficiency

0 引言

隨著智能機器人不斷發展，路徑規劃問題研究越來越深入。路徑規劃指機器人在當前環境中按照一定的標準，搜索出一條從起始狀態點到目標狀態點，能夠繞開障礙物的最優或次優路徑。傳統的人工勢場法[1]、神經網絡法[2]和遺傳算法[3]在解決機器人路徑規劃問題時，需在一確定空間內對障礙物建模，在實際應用中路徑搜索效率較低。RRT（Rapidly Random-exploring Trees）算法[4-6]因快速隨機搜索和無需建模等特點，無需預處理，因而在路徑規劃問題上得到廣泛運用，能有效解決高維空間和復雜環境下的路徑規劃問題[7]。

經典RRT算法具有隨機性大、避障能力強、實時性弱與搜索效率低等特點[8]。為了解決搜索效率低的問題，研究人員在經典RRT算法基礎上提出了很多改進方法，如偏向RRT算法[9]。偏向RRT算法在一定程度上解決了經典RRT算法效率低的問題，卻遺留了隨機性大的缺點。本文在加入目標引力思想的經典RRT算法基礎上首先引入可變步長思想，借助可變步長的魯棒性，讓隨機搜索樹朝著目標點方向擴展生長，無需對全局空間進行隨機采樣，解決了隨機性大的問題，但搜索效率較低[10-11]。

為解決經典RRT算法效率較低、復雜度高的問題，本文在改進的RRT算法基礎上引入雙向生長策略。改進后的RRT算法不僅避障能力強，而且路徑搜索效率高，很好地解決了獲取最優路徑與搜索效率低之間的矛盾。

1 經典RRT算法

經典RRT算法是從狀態空間一初始點出發，通過隨機采樣擴展增加新節點，生成一個隨機擴展樹，當隨機樹中的新節點包含目標點或進入目標區域時，初始點到目標點間至少形成一條以隨機樹新節點組成的路徑[12]。

假設在二維工作空間內，C為系統的狀態空間。從初始點X-init出發搜索擴展隨機樹T，對隨機樹T逐步構建，構建過程如圖1所示。

在狀態空間C中，T為擴展樹，X-init為初始狀態點，X-goal為目標狀態點。狀態空間C中路徑不通區域稱為障礙區X-obs（X-obsX），路徑可通區域稱為自由區X-free（X-freeX）。X-rand（X-randX-free）為每次擴展隨機樹時在C空間的自由區域中隨機取的點，X-near為每次擴展隨機樹時隨機樹中與X-rand最近的點，在X-near和X-rand的連線上求X-new（X-newX-free）。一個隨機擴展樹T從初始點X-init出發，生成 K個頂點的算法基本結構如下：

RRT_BCV（X-init，X-goal）

Ti（X-init）；

for k=1 to K do

X-rand=random_state（）；

X-near=nearest_neighbor（X-rand，T）；

u=select_input（X-rand，X-near）；

X-new=new_state（X-near，u，t）；

if collision（X-new）

continue；

T.add.vertex（X-new）；

if ||X-near-X-good||

return Reached；

Return T

隨機樹T生長過程中，函數random_state（）在狀態空間中隨機選取一點X-rand；函數nearest_neighbor（）在當前搜索樹上選取離X-rand距離最近的節點X-near加以擴展；函數select_input（）在X-rand與X-near結合下得到輸入U。根據給定標準，在輸入U中選擇一個輸入，使得X-near盡可能接近X-rand，生成一個節點X-near，函數new_state（）得到新節點X-new。每次擴展得到新節點后均要判斷其是否在障礙區域內，若是，則返回到for循環重新選取新的隨機點；若否，則直接將其加入當前樹中。當加入的新節點與目標點間的距離足夠小時，路徑搜索結束，生成規劃后的路徑。

RRT算法擴展生長時的最小單位為步長ρ。經典RRT算法在狀態空間C中隨機擴展新節點X-new時，以ρ為固定步長計算新節點X-new位置時的方法如式（1）所示。

2 改進后的經典RRT算法

2.1 引入目標引力思想的經典RRT算法

針對經典RRT算法隨機性大的問題，許多學者提出了解決方法。本文將人工勢場法中的目標引力思想引入到經典RRT算法，確定目標后使隨機樹朝著目標點或目標區域方向擴展生長。改進后的RRT算法只需局部隨機采樣，故在減小經典RRT算法隨機性的同時改善了路徑的光滑性[13]。

引入目標引力思想的經典RRT算法在規劃路徑時的關鍵，是在路徑從初始點隨機擴展后的每一個節點處都引入一個目標引力函數，新節點計算方法如式（2）所示。

引入目標引力思想后，經典的RRT算法有效減小了路徑規劃時的隨機性，與經典RRT算法相比，不僅具有隨機方向的隨機點x-rand，還增加了目標區域方向的目標點x-goal。目標點x-goal是新節點x-new生成的主要影響因素，新節點x-new的位置會隨著步長的變化而變化。當ρ大于k-ρ時，新節點將朝著隨機點x-rand方向生長，此時x-rand跟經典RRT算法的隨機點選取很接近，具有大隨機性和強避障能力；當ρ小于k-ρ時，新節點x-new朝向目標點x-goal生長，隨機樹將朝著目標點或目標區域擴展。

機器人應用所處的工作環境都較為復雜[14]。從初始點到目標點路徑規劃時，障礙物是不可避免的。引入目標引力思想的經典RRT算法適用于無障礙的理想環境與少障礙物環境，當遇到多障礙物的復雜工作環境時，因不具有經典RRT算法的隨機性，不能順利繞開障礙物，導致最終無法規劃出有效路徑。

2.2 引入可變步長的經典RRT算法

本文在引入目標引力思想的經典RRT算法基礎上引入可變步長策略，實現RRT動態隨機與強避障能力優點。改進后的算法在隨機樹擴展新節點x-new時起著關鍵作用，即在x-rand與x-near的連線上以一個步長為距離確定生長新節點x-new。在障礙物環境下，可變步長可以有效利用經典RRT算法的隨機性。當遇到障礙物時，可取ρ大于k-ρ，此時隨機樹將具有經典RRT算法的隨機性，朝著隨機點方向擴展，不會碰到障礙物；當沒有遇到障礙物時，可取ρ

引入可變步長的經典RRT算法保證了隨機樹從初始點朝著目標點方向生長，同時保證了RRT算法的強避障能力與良好的路徑光滑性。

路徑規劃中的時間復雜度是算法的重要參數之一[15]。經典RRT算法因需對全局空間進行擴展，生長的節點較多，因此時間復雜度是RRT算法需要考慮的因素。許多改進后的RRT算法在路徑規劃上可以接近最優解，但時間復雜度較高，引入可變步長的經典RRT算法在擴展新節點時，無需通過計算和比較多個擴展隨機點的值來確定新節點。

2.3 引入雙向生長策略的經典RRT算法

經典RRT算法從初始點到目標點隨機擴展生長時隨機性很大，搜索效率低；而引入目標引力思想與可變步長策略的經典RRT算法有效解決了隨機性大和避障能力低的問題，但路徑搜索效率較低。本文在此改進算法基礎上引入雙向生長策略，以期解決效率低的問題。

雙向生長策略指從初始點和目標點同時生成兩棵RRT隨機樹，兩棵隨機樹同時擴展生長后于狀態空間某一點相遇時，生成一條有效路徑。算法在初始點x-init和x-goal同時開始構造RRT樹T-i和T-j，從任意一個RRT樹中選取與x-rand距離最近的節點x-near，在x-rand與x-near連線上找到一個距離x-rand最近的點x-new，將其加入RRT樹中。同時再尋找另一個RRT樹中距離x-new最近的點，在擴展過程中試圖用相同的算法將兩樹連接起來。若兩樹中的兩節點距離足夠小，則可確定T-i與T-j連通，基本算法如下：

RRT_BCV（X-init，X-goal）

Ti（X-init），Tj（X-goal）

for k=1 to K do

X-rand=random_state（）；

if not（BCV_CONNECT（Ti， X-rand）=trapped） then

if（BCV_CONNECT（Tj，X-new）=reached） then

Return PATH（Ti，Tj）；

swap（Ti，Tj）；

Return（Ti，Tj）；

BCV_CONNECT（T，X）

Repart

X-near=nearest_neighbor（ X-rand，T）；

u=select_input（ X-rand，X-near）；

X-new=new_state（X-near，u，t）；

if collision（X-new）

continue；

T.add.vertex（X-new）；

T.add.edge（X-near，X-new）；

改進后的RRT算法（下文簡稱為可變步長的雙向RRT算法）中的兩棵隨機樹在同時擴展生長時，不僅具有目標引力產生的朝目標點方向生長特性，還具有可變步長產生的高避障能力。兩隨機樹各自朝著自己的目標點方向生長，當它們產生的新節點X-new在空間中某點相遇時，將形成一條有效路徑，最終規劃出的路徑將接近最優解。

3 實驗結果與分析

設機器人為圓點狀，將可變步長的雙向RRT算法實驗仿真數據與可變步長的RRT算法實驗仿真數據進行比較，驗證算法的正確性與高效率。實驗環境為Windows 2007，Intel（R） Core（TM）2.3GHz、2G內存，編譯工具為MATLAB 2 012b。圖2中左下角的機器人為圓點狀，即為根節點；狀態空間1 000×1 000，X軸與Y軸坐標范圍均為[0，1 000]，原點為[0，0]，兩點間的直線距離約為1 414m；實驗環境中障礙物為黑色斑塊，大小形狀不一，隨機設置。

實驗1：對可變步長的RRT算法進行仿真實驗，如圖2所示。從左下角初始點出發的隨機樹在狀態空間C中擴展搜索，生成的新節點用小黑點表示。從左下角初始點出發的隨機樹生長線用黑點和細線相間表示，規劃出的路徑用粗線表示。圖3是路徑規劃仿真效果。

實驗2：對可變步長的雙向RRT算法進行仿真實驗，圖4是實驗仿真結果，從初始點出發的隨機樹與從目標點出發的隨機樹在狀態空間C中同時擴展搜索，生成新節點，在某點相遇后生成一條路徑。由于目標引力的作用，新節點生成位置比較集中。從左下角初始點出發的隨機樹生長線用星點與細線相間表示，從右上角目標點出發的隨機樹生長線用黑點和細線相間表示，規劃出的路徑用粗線表示。圖5是路徑規劃仿真效果。

表1是兩組實驗20次仿真數據的平均值。通過實驗數據對比可知，在實驗所設定的已知靜態障礙物環境下，可變步長的雙向RRT算法路徑搜索成功率高；平均新節點個數生成量減少近一半，算法復雜度降低；平均路徑長度為1 556，規劃出的路徑也相對改進前的算法趨于平緩；路徑搜索所需平均時間顯著減少，搜索效率提高近一倍。可變步長的雙向RRT算法解決了最優路徑與效率低間的矛盾，最終規劃出的路徑更接近最優解。

4 結語

本文針對可變步長的經典RRT算法在路徑搜索時效率較低與算法復雜度高的問題，在算法中引入雙向生長策略，兩隨機樹從初始點和目標點同時搜索擴展，生成的新節點在狀態空間中的某點相遇后形成一條有效路徑，進而完成對機器人的路徑規劃實驗。

實驗仿真結果表明，可變步長的雙向RRT算法在路徑搜索時除具有避障能力強、隨機性小等特點外，還具有生成新節點少、算法復雜度低和路徑搜索效率高的優點，最終生成的路徑更接近最優解，具有一定的實用價值。

參考文獻：

[1] 徐飛.基于改進人工勢場法的機器人避障及路徑規劃研究[J].計算機科學，2016，43（12）：293-296.

[2] 朱愛斌，何大勇，羅文成，等.基于雙目視覺方法的可穿戴式導盲機器人研究[J].機械設計與研究，2016，32（5）：31-34.

[3] 萬善余，范迪.基于遺傳算法的信號燈配時[J].電子科技，2017，30（3）：45-52.

[4] LAVALLE S M. Planning algorithms.Illinois[M].USA：University of Illinois Press，2004.

[5] 劉成菊，韓俊強，安康.基于改進RRT算法的RoboCup機器人動態路徑規劃[J].機器人，2017，39（1）：8-15.

[6] LAVALLE S M，KUFFNER J. Rapidly random-exploring trees：progress and prospects[C].Proc of the International Workshop on Algorithmic Foundations of Robotics. Hanover，USA，2000：54-59.

[7] 劉佳順，劉檢華，張之敬，等.基于任意時間RRT算法的三維自動布線技術[J].機械工程學報，2016，52（13）：156-165.

[8] 王全，王維，李焱，等.基于混合策略的輪式機器人路徑規劃方法[J].計算機工程與應用，2014，54（4）：45-49.

[9] LAVALLE S M，KUFFNER J. Rapidly random-exploring trees：progress and prospects[C]. Proceedings of Algorithmic and Computational Robotics：New Directions，2001：293-308.

[10] AGIRREBEITIA J，AVILES R，DE BUSTOS I F，et al. A new APF strategy for path planning in environments with obstacles[J]. Mechanism and Machine Theory，2005，40（6）：645-658.

[11] 宋曉琳，周南，黃正瑜，等.改進RRT在汽車避障局部路徑規劃中的應用[J].湖南大學學報：自然科學版，2017，44（4）：30-37.

[12] 馮林，賈菁輝.基于對比優化的RRT路徑規劃改進算法[J].計算機工程與應用，2011，47（3）：210-213.

[13] 段鎖林，王琳琳.智能輪椅路徑規劃優化問題的研究[J].計算機仿真，2015，32（3）：412-416.

[14] 徐雪松，楊勝杰，陳榮元.復雜環境移動群機器人最優路徑規劃方法[J].電子測量與儀器學報，2016，21（2）：274-282.

[15] 高慶吉，王磊，牛國臣，等.基于目標導向的連續型機器人路徑規劃[J].北京航空航天大學學報，2013，39（11）：1486-1490.

（責任編輯：杜能鋼）