1、 一種改進(jìn)的RRT機(jī)器人路徑規(guī)劃算法研究 胡兵 向鳳紅 毛劍琳摘 要：路徑規(guī)劃技術(shù)是目前機(jī)器人領(lǐng)域研究熱點(diǎn)，而路徑規(guī)劃算法是其核心內(nèi)容。可變步長的快速隨機(jī)搜索樹（Rapidly-exploring Random Tree，RRT）算法在機(jī)器人路徑規(guī)劃算法中復(fù)雜度高、效率較低，針對(duì)這一問題，提出一種改進(jìn)的RRT算法。在可變步長的隨機(jī)樹生長過程中，引入雙向生長策略，利用雙向生長特性，提高路徑搜索效率，解決了最優(yōu)路徑與低效率間的矛盾。實(shí)驗(yàn)仿真數(shù)據(jù)表明，改進(jìn)后的RRT算法在路徑規(guī)劃中不僅算法復(fù)雜度低，且搜索效率提高了約一倍。Key：快速隨機(jī)搜索樹；可變步長；雙向生長；路徑規(guī)劃；搜索效率DOI：10.1

2、1907/rjdk.172931：TP312：A ：1672-7800（2018）006-0013-04Abstract：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 a

3、lgorithm， 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

4、 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； Bidir

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

6、問題7。經(jīng)典RRT算法具有隨機(jī)性大、避障能力強(qiáng)、實(shí)時(shí)性弱與搜索效率低等特點(diǎn)8。為了解決搜索效率低的問題，研究人員在經(jīng)典RRT算法基礎(chǔ)上提出了很多改進(jìn)方法，如偏向RRT算法9。偏向RRT算法在一定程度上解決了經(jīng)典RRT算法效率低的問題，卻遺留了隨機(jī)性大的缺點(diǎn)。本文在加入目標(biāo)引力思想的經(jīng)典RRT算法基礎(chǔ)上首先引入可變步長思想，借助可變步長的魯棒性，讓隨機(jī)搜索樹朝著目標(biāo)點(diǎn)方向擴(kuò)展生長，無需對(duì)全局空間進(jìn)行隨機(jī)采樣，解決了隨機(jī)性大的問題，但搜索效率較低10-11。為解決經(jīng)典RRT算法效率較低、復(fù)雜度高的問題，本文在改進(jìn)的RRT算法基礎(chǔ)上引入雙向生長策略。改進(jìn)后的RRT算法不僅避障能力強(qiáng)，而且路徑搜索效率

7、高，很好地解決了獲取最優(yōu)路徑與搜索效率低之間的矛盾。1 經(jīng)典RRT算法經(jīng)典RRT算法是從狀態(tài)空間一初始點(diǎn)出發(fā)，通過隨機(jī)采樣擴(kuò)展增加新節(jié)點(diǎn)，生成一個(gè)隨機(jī)擴(kuò)展樹，當(dāng)隨機(jī)樹中的新節(jié)點(diǎn)包含目標(biāo)點(diǎn)或進(jìn)入目標(biāo)區(qū)域時(shí)，初始點(diǎn)到目標(biāo)點(diǎn)間至少形成一條以隨機(jī)樹新節(jié)點(diǎn)組成的路徑12。假設(shè)在二維工作空間內(nèi)，C為系統(tǒng)的狀態(tài)空間。從初始點(diǎn)X-init出發(fā)搜索擴(kuò)展隨機(jī)樹T，對(duì)隨機(jī)樹T逐步構(gòu)建，構(gòu)建過程如圖1所示。在狀態(tài)空間C中，T為擴(kuò)展樹，X-init為初始狀態(tài)點(diǎn)，X-goal為目標(biāo)狀態(tài)點(diǎn)。狀態(tài)空間C中路徑不通區(qū)域稱為障礙區(qū)X-obs（X-obsX），路徑可通區(qū)域稱為自由區(qū)X-free（X-freeX）。X-rand（X-

8、randX-free）為每次擴(kuò)展隨機(jī)樹時(shí)在C空間的自由區(qū)域中隨機(jī)取的點(diǎn)，X-near為每次擴(kuò)展隨機(jī)樹時(shí)隨機(jī)樹中與X-rand最近的點(diǎn)，在X-near和X-rand的連線上求X-new（X-newX-free）。一個(gè)隨機(jī)擴(kuò)展樹T從初始點(diǎn)X-init出發(fā)，生成 K個(gè)頂點(diǎn)的算法基本結(jié)構(gòu)如下：RRT_BCV（X-init，X-goal）Ti（X-init）；for k=1 to K doX-rand=random_state（）；X-near=nearest_neighbor（X-rand，T）；u=select_input（X-rand，X-near）；X-new=new_state（X-near，

9、u，t）；if collision（X-new）continue；T.add.vertex（X-new）；if |X-near-X-good|return Reached；Return T隨機(jī)樹T生長過程中，函數(shù)random_state（）在狀態(tài)空間中隨機(jī)選取一點(diǎn)X-rand；函數(shù)nearest_neighbor（）在當(dāng)前搜索樹上選取離X-rand距離最近的節(jié)點(diǎn)X-near加以擴(kuò)展；函數(shù)select_input（）在X-rand與X-near結(jié)合下得到輸入U(xiǎn)。根據(jù)給定標(biāo)準(zhǔn)，在輸入U(xiǎn)中選擇一個(gè)輸入，使得X-near盡可能接近X-rand，生成一個(gè)節(jié)點(diǎn)X-near，函數(shù)new_state（）得到新

10、節(jié)點(diǎn)X-new。每次擴(kuò)展得到新節(jié)點(diǎn)后均要判斷其是否在障礙區(qū)域內(nèi)，若是，則返回到for循環(huán)重新選取新的隨機(jī)點(diǎn)；若否，則直接將其加入當(dāng)前樹中。當(dāng)加入的新節(jié)點(diǎn)與目標(biāo)點(diǎn)間的距離足夠小時(shí)，路徑搜索結(jié)束，生成規(guī)劃后的路徑。RRT算法擴(kuò)展生長時(shí)的最小單位為步長。經(jīng)典RRT算法在狀態(tài)空間C中隨機(jī)擴(kuò)展新節(jié)點(diǎn)X-new時(shí)，以為固定步長計(jì)算新節(jié)點(diǎn)X-new位置時(shí)的方法如式（1）所示。2 改進(jìn)后的經(jīng)典RRT算法2.1 引入目標(biāo)引力思想的經(jīng)典RRT算法針對(duì)經(jīng)典RRT算法隨機(jī)性大的問題，許多學(xué)者提出了解決方法。本文將人工勢(shì)場法中的目標(biāo)引力思想引入到經(jīng)典RRT算法，確定目標(biāo)后使隨機(jī)樹朝著目標(biāo)點(diǎn)或目標(biāo)區(qū)域方向擴(kuò)展生長。改進(jìn)后

11、的RRT算法只需局部隨機(jī)采樣，故在減小經(jīng)典RRT算法隨機(jī)性的同時(shí)改善了路徑的光滑性13。引入目標(biāo)引力思想的經(jīng)典RRT算法在規(guī)劃路徑時(shí)的關(guān)鍵，是在路徑從初始點(diǎn)隨機(jī)擴(kuò)展后的每一個(gè)節(jié)點(diǎn)處都引入一個(gè)目標(biāo)引力函數(shù)，新節(jié)點(diǎn)計(jì)算方法如式（2）所示。引入目標(biāo)引力思想后，經(jīng)典的RRT算法有效減小了路徑規(guī)劃時(shí)的隨機(jī)性，與經(jīng)典RRT算法相比，不僅具有隨機(jī)方向的隨機(jī)點(diǎn)x-rand，還增加了目標(biāo)區(qū)域方向的目標(biāo)點(diǎn)x-goal。目標(biāo)點(diǎn)x-goal是新節(jié)點(diǎn)x-new生成的主要影響因素，新節(jié)點(diǎn)x-new的位置會(huì)隨著步長的變化而變化。當(dāng)大于k-時(shí)，新節(jié)點(diǎn)將朝著隨機(jī)點(diǎn)x-rand方向生長，此時(shí)x-rand跟經(jīng)典RRT算法的隨機(jī)點(diǎn)選

12、取很接近，具有大隨機(jī)性和強(qiáng)避障能力；當(dāng)小于k-時(shí)，新節(jié)點(diǎn)x-new朝向目標(biāo)點(diǎn)x-goal生長，隨機(jī)樹將朝著目標(biāo)點(diǎn)或目標(biāo)區(qū)域擴(kuò)展。機(jī)器人應(yīng)用所處的工作環(huán)境都較為復(fù)雜14。從初始點(diǎn)到目標(biāo)點(diǎn)路徑規(guī)劃時(shí)，障礙物是不可避免的。引入目標(biāo)引力思想的經(jīng)典RRT算法適用于無障礙的理想環(huán)境與少障礙物環(huán)境，當(dāng)遇到多障礙物的復(fù)雜工作環(huán)境時(shí)，因不具有經(jīng)典RRT算法的隨機(jī)性，不能順利繞開障礙物，導(dǎo)致最終無法規(guī)劃出有效路徑。2.2 引入可變步長的經(jīng)典RRT算法本文在引入目標(biāo)引力思想的經(jīng)典RRT算法基礎(chǔ)上引入可變步長策略，實(shí)現(xiàn)RRT動(dòng)態(tài)隨機(jī)與強(qiáng)避障能力優(yōu)點(diǎn)。改進(jìn)后的算法在隨機(jī)樹擴(kuò)展新節(jié)點(diǎn)x-new時(shí)起著關(guān)鍵作用，即在x-ra

13、nd與x-near的連線上以一個(gè)步長為距離確定生長新節(jié)點(diǎn)x-new。在障礙物環(huán)境下，可變步長可以有效利用經(jīng)典RRT算法的隨機(jī)性。當(dāng)遇到障礙物時(shí)，可取大于k-，此時(shí)隨機(jī)樹將具有經(jīng)典RRT算法的隨機(jī)性，朝著隨機(jī)點(diǎn)方向擴(kuò)展，不會(huì)碰到障礙物；當(dāng)沒有遇到障礙物時(shí)，可取引入可變步長的經(jīng)典RRT算法保證了隨機(jī)樹從初始點(diǎn)朝著目標(biāo)點(diǎn)方向生長，同時(shí)保證了RRT算法的強(qiáng)避障能力與良好的路徑光滑性。路徑規(guī)劃中的時(shí)間復(fù)雜度是算法的重要參數(shù)之一15。經(jīng)典RRT算法因需對(duì)全局空間進(jìn)行擴(kuò)展，生長的節(jié)點(diǎn)較多，因此時(shí)間復(fù)雜度是RRT算法需要考慮的因素。許多改進(jìn)后的RRT算法在路徑規(guī)劃上可以接近最優(yōu)解，但時(shí)間復(fù)雜度較高，引入可變步

14、長的經(jīng)典RRT算法在擴(kuò)展新節(jié)點(diǎn)時(shí)，無需通過計(jì)算和比較多個(gè)擴(kuò)展隨機(jī)點(diǎn)的值來確定新節(jié)點(diǎn)。2.3 引入雙向生長策略的經(jīng)典RRT算法經(jīng)典RRT算法從初始點(diǎn)到目標(biāo)點(diǎn)隨機(jī)擴(kuò)展生長時(shí)隨機(jī)性很大，搜索效率低；而引入目標(biāo)引力思想與可變步長策略的經(jīng)典RRT算法有效解決了隨機(jī)性大和避障能力低的問題，但路徑搜索效率較低。本文在此改進(jìn)算法基礎(chǔ)上引入雙向生長策略，以期解決效率低的問題。雙向生長策略指從初始點(diǎn)和目標(biāo)點(diǎn)同時(shí)生成兩棵RRT隨機(jī)樹，兩棵隨機(jī)樹同時(shí)擴(kuò)展生長后于狀態(tài)空間某一點(diǎn)相遇時(shí)，生成一條有效路徑。算法在初始點(diǎn)x-init和x-goal同時(shí)開始構(gòu)造RRT樹T-i和T-j，從任意一個(gè)RRT樹中選取與x-rand距離最

15、近的節(jié)點(diǎn)x-near，在x-rand與x-near連線上找到一個(gè)距離x-rand最近的點(diǎn)x-new，將其加入RRT樹中。同時(shí)再尋找另一個(gè)RRT樹中距離x-new最近的點(diǎn)，在擴(kuò)展過程中試圖用相同的算法將兩樹連接起來。若兩樹中的兩節(jié)點(diǎn)距離足夠小，則可確定T-i與T-j連通，基本算法如下：RRT_BCV（X-init，X-goal）Ti（X-init），Tj（X-goal）for k=1 to K doX-rand=random_state（）；if not（BCV_CONNECT（Ti， X-rand）=trapped） thenif（BCV_CONNECT（Tj，X-new）=reached）

16、thenReturn PATH（Ti，Tj）；swap（Ti，Tj）；Return（Ti，Tj）；BCV_CONNECT（T，X）RepartX-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）；改進(jìn)后的RRT算法（下文簡稱為可變步長的雙向RRT算法）中的兩棵隨機(jī)樹在同時(shí)擴(kuò)展生長時(shí)，不僅具有目標(biāo)引力產(chǎn)生的朝目標(biāo)點(diǎn)方向生

17、長特性，還具有可變步長產(chǎn)生的高避障能力。兩隨機(jī)樹各自朝著自己的目標(biāo)點(diǎn)方向生長，當(dāng)它們產(chǎn)生的新節(jié)點(diǎn)X-new在空間中某點(diǎn)相遇時(shí)，將形成一條有效路徑，最終規(guī)劃出的路徑將接近最優(yōu)解。3 實(shí)驗(yàn)結(jié)果與分析設(shè)機(jī)器人為圓點(diǎn)狀，將可變步長的雙向RRT算法實(shí)驗(yàn)仿真數(shù)據(jù)與可變步長的RRT算法實(shí)驗(yàn)仿真數(shù)據(jù)進(jìn)行比較，驗(yàn)證算法的正確性與高效率。實(shí)驗(yàn)環(huán)境為Windows 2007，Intel（R） Core（TM）2.3GHz、2G內(nèi)存，編譯工具為MATLAB 2 012b。圖2中左下角的機(jī)器人為圓點(diǎn)狀，即為根節(jié)點(diǎn)；狀態(tài)空間1 0001 000，X軸與Y軸坐標(biāo)范圍均為0，1 000，原點(diǎn)為0，0，兩點(diǎn)間的直線距離約為1

18、414m；實(shí)驗(yàn)環(huán)境中障礙物為黑色斑塊，大小形狀不一，隨機(jī)設(shè)置。實(shí)驗(yàn)1：對(duì)可變步長的RRT算法進(jìn)行仿真實(shí)驗(yàn)，如圖2所示。從左下角初始點(diǎn)出發(fā)的隨機(jī)樹在狀態(tài)空間C中擴(kuò)展搜索，生成的新節(jié)點(diǎn)用小黑點(diǎn)表示。從左下角初始點(diǎn)出發(fā)的隨機(jī)樹生長線用黑點(diǎn)和細(xì)線相間表示，規(guī)劃出的路徑用粗線表示。圖3是路徑規(guī)劃仿真效果。實(shí)驗(yàn)2：對(duì)可變步長的雙向RRT算法進(jìn)行仿真實(shí)驗(yàn)，圖4是實(shí)驗(yàn)仿真結(jié)果，從初始點(diǎn)出發(fā)的隨機(jī)樹與從目標(biāo)點(diǎn)出發(fā)的隨機(jī)樹在狀態(tài)空間C中同時(shí)擴(kuò)展搜索，生成新節(jié)點(diǎn)，在某點(diǎn)相遇后生成一條路徑。由于目標(biāo)引力的作用，新節(jié)點(diǎn)生成位置比較集中。從左下角初始點(diǎn)出發(fā)的隨機(jī)樹生長線用星點(diǎn)與細(xì)線相間表示，從右上角目標(biāo)點(diǎn)出發(fā)的隨機(jī)樹生長

19、線用黑點(diǎn)和細(xì)線相間表示，規(guī)劃出的路徑用粗線表示。圖5是路徑規(guī)劃仿真效果。表1是兩組實(shí)驗(yàn)20次仿真數(shù)據(jù)的平均值。通過實(shí)驗(yàn)數(shù)據(jù)對(duì)比可知，在實(shí)驗(yàn)所設(shè)定的已知靜態(tài)障礙物環(huán)境下，可變步長的雙向RRT算法路徑搜索成功率高；平均新節(jié)點(diǎn)個(gè)數(shù)生成量減少近一半，算法復(fù)雜度降低；平均路徑長度為1 556，規(guī)劃出的路徑也相對(duì)改進(jìn)前的算法趨于平緩；路徑搜索所需平均時(shí)間顯著減少，搜索效率提高近一倍?？勺儾介L的雙向RRT算法解決了最優(yōu)路徑與效率低間的矛盾，最終規(guī)劃出的路徑更接近最優(yōu)解。4 結(jié)語本文針對(duì)可變步長的經(jīng)典RRT算法在路徑搜索時(shí)效率較低與算法復(fù)雜度高的問題，在算法中引入雙向生長策略，兩隨機(jī)樹從初始點(diǎn)和目標(biāo)點(diǎn)同時(shí)搜索

20、擴(kuò)展，生成的新節(jié)點(diǎn)在狀態(tài)空間中的某點(diǎn)相遇后形成一條有效路徑，進(jìn)而完成對(duì)機(jī)器人的路徑規(guī)劃實(shí)驗(yàn)。實(shí)驗(yàn)仿真結(jié)果表明，可變步長的雙向RRT算法在路徑搜索時(shí)除具有避障能力強(qiáng)、隨機(jī)性小等特點(diǎn)外，還具有生成新節(jié)點(diǎn)少、算法復(fù)雜度低和路徑搜索效率高的優(yōu)點(diǎn)，最終生成的路徑更接近最優(yōu)解，具有一定的實(shí)用價(jià)值。Reference：1 徐飛.基于改進(jìn)人工勢(shì)場法的機(jī)器人避障及路徑規(guī)劃研究J.計(jì)算機(jī)科學(xué)，2016，43（12）：293-296.2 朱愛斌，何大勇，羅文成，等.基于雙目視覺方法的可穿戴式導(dǎo)盲機(jī)器人研究J.機(jī)械設(shè)計(jì)與研究，2016，32（5）：31-34.3 萬善余，范迪.基于遺傳算法的信號(hào)燈配時(shí)J.電子科技，2

21、017，30（3）：45-52.4 LAVALLE S M. Planning algorithms.IllinoisM.USA：University of Illinois Press，2004.5 劉成菊，韓俊強(qiáng)，安康.基于改進(jìn)RRT算法的RoboCup機(jī)器人動(dòng)態(tài)路徑規(guī)劃J.機(jī)器人，2017，39（1）：8-15.6 LAVALLE S M，KUFFNER J. Rapidly random-exploring trees：progress and prospectsC.Proc of the International Workshop on Algorithmic Foundations of Robotics. Hanover，USA，2000：54-59.7 劉佳順，劉檢華，張之敬，等.基于任意時(shí)間RRT算法的三維自動(dòng)布線技術(shù)J.機(jī)械工程學(xué)報(bào)，2016，52（13）：156-165.8 王全