胡兵 向鳳紅 毛劍琳

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

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

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 引言

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

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

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

1 經(jīng)典RRT算法

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

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

在狀態(tài)空間C中，T為擴展樹，X-init為初始狀態(tài)點，X-goal為目標狀態(tài)點。狀態(tài)空間C中路徑不通區(qū)域稱為障礙區(qū)X-obs（X-obsX），路徑可通區(qū)域稱為自由區(qū)X-free（X-freeX）。X-rand（X-randX-free）為每次擴展隨機樹時在C空間的自由區(qū)域中隨機取的點，X-near為每次擴展隨機樹時隨機樹中與X-rand最近的點，在X-near和X-rand的連線上求X-new（X-newX-free）。一個隨機擴展樹T從初始點X-init出發(fā)，生成 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生長過程中，函數(shù)random_state（）在狀態(tài)空間中隨機選取一點X-rand；函數(shù)nearest_neighbor（）在當前搜索樹上選取離X-rand距離最近的節(jié)點X-near加以擴展；函數(shù)select_input（）在X-rand與X-near結合下得到輸入U。根據(jù)給定標準，在輸入U中選擇一個輸入，使得X-near盡可能接近X-rand，生成一個節(jié)點X-near，函數(shù)new_state（）得到新節(jié)點X-new。每次擴展得到新節(jié)點后均要判斷其是否在障礙區(qū)域內，若是，則返回到for循環(huán)重新選取新的隨機點；若否，則直接將其加入當前樹中。當加入的新節(jié)點與目標點間的距離足夠小時，路徑搜索結束，生成規(guī)劃后的路徑。

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

2 改進后的經(jīng)典RRT算法

2.1 引入目標引力思想的經(jīng)典RRT算法

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

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

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

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

2.2 引入可變步長的經(jīng)典RRT算法

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

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

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

2.3 引入雙向生長策略的經(jīng)典RRT算法

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

雙向生長策略指從初始點和目標點同時生成兩棵RRT隨機樹，兩棵隨機樹同時擴展生長后于狀態(tài)空間某一點相遇時，生成一條有效路徑。算法在初始點x-init和x-goal同時開始構造RRT樹T-i和T-j，從任意一個RRT樹中選取與x-rand距離最近的節(jié)點x-near，在x-rand與x-near連線上找到一個距離x-rand最近的點x-new，將其加入RRT樹中。同時再尋找另一個RRT樹中距離x-new最近的點，在擴展過程中試圖用相同的算法將兩樹連接起來。若兩樹中的兩節(jié)點距離足夠小，則可確定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算法）中的兩棵隨機樹在同時擴展生長時，不僅具有目標引力產(chǎn)生的朝目標點方向生長特性，還具有可變步長產(chǎn)生的高避障能力。兩隨機樹各自朝著自己的目標點方向生長，當它們產(chǎn)生的新節(jié)點X-new在空間中某點相遇時，將形成一條有效路徑，最終規(guī)劃出的路徑將接近最優(yōu)解。

3 實驗結果與分析

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

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

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

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

4 結語

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

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

參考文獻：

[1] 徐飛.基于改進人工勢場法的機器人避障及路徑規(guī)劃研究[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機器人動態(tài)路徑規(guī)劃[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] 王全，王維，李焱，等.基于混合策略的輪式機器人路徑規(guī)劃方法[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在汽車避障局部路徑規(guī)劃中的應用[J].湖南大學學報：自然科學版，2017，44（4）：30-37.

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

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

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

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

（責任編輯：杜能鋼）