一種改進(jìn)的RRT路徑規(guī)劃算法*

王稷堯，袁鋒偉

（南華大學(xué)機(jī)械工程學(xué)院，衡陽 421001）

0 引言

隨著智能機(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)路徑[1]。根據(jù)其原理不同，路徑規(guī)劃算法有人工勢(shì)場(chǎng)法[2]、神經(jīng)網(wǎng)絡(luò)法[3]、遺傳算法[4]等。其中，以RRT算法為代表的采樣規(guī)劃算法，通過一個(gè)初始點(diǎn)作為根節(jié)點(diǎn)，采用隨機(jī)采樣的方式增加樹結(jié)構(gòu)的分支，當(dāng)分支節(jié)點(diǎn)包含目標(biāo)節(jié)點(diǎn)或進(jìn)入目標(biāo)區(qū)域，即可快速生成一條由樹節(jié)點(diǎn)生成的從初始點(diǎn)到目標(biāo)點(diǎn)的路徑，具有概率完備性，在多障礙物的復(fù)雜場(chǎng)景應(yīng)用較多[5]。

北京工業(yè)大學(xué)的阮曉鋼等[6]，提出了一種基于信息增益與RRT思想相結(jié)合的機(jī)器人環(huán)境探索策略，增強(qiáng)機(jī)器人對(duì)未知環(huán)境的探索降，低傳統(tǒng)RRT算法固有的盲目性。昆明理工大學(xué)的胡兵等[1]，在可變步長(zhǎng)的隨機(jī)樹生長(zhǎng)過程中，引入雙向生長(zhǎng)策略，利用雙向生長(zhǎng)特性，提高路徑搜索效率，解決了最優(yōu)路徑與低效率間的矛盾。林依凡等[7]提出了一種無碰撞檢測(cè)RRT*運(yùn)動(dòng)規(guī)劃方法，增強(qiáng)了在復(fù)雜環(huán)境中的路徑規(guī)劃能力。四川大學(xué)的仲健寧等[8]提出一種基于多種啟發(fā)式策略和強(qiáng)化節(jié)點(diǎn)機(jī)制改進(jìn)的高效RRT*路徑規(guī)劃算法，該算法提出強(qiáng)化節(jié)點(diǎn)機(jī)制，擴(kuò)大節(jié)點(diǎn)蘊(yùn)涵的信息，提高節(jié)點(diǎn)利用率。廣東工業(yè)大學(xué)的何兆楚等[9]利用人工勢(shì)場(chǎng)法進(jìn)行局部路徑規(guī)劃，當(dāng)陷入局部極小值時(shí)，使用改進(jìn)的快速擴(kuò)展隨機(jī)算法自適應(yīng)地選擇臨時(shí)目標(biāo)點(diǎn)，使搜索過程跳出局部極小值點(diǎn)；當(dāng)機(jī)械臂逃離局部極小值點(diǎn)時(shí)，切換回人工勢(shì)場(chǎng)法進(jìn)行規(guī)劃。

基于以上的研究分析，根據(jù)RRT算法隨機(jī)性、無序性的特點(diǎn)，對(duì)RRT算法進(jìn)行選點(diǎn)方式、算法優(yōu)化、碰撞檢測(cè)等方面的改進(jìn)，提出一種新的路徑規(guī)劃算法。本研究以傳統(tǒng)RRT算法與勢(shì)場(chǎng)法相結(jié)合，對(duì)RRT算法加入勢(shì)場(chǎng)法、新節(jié)點(diǎn)生成策略、狹窄空間算法等，對(duì)RRT算法進(jìn)行進(jìn)一步優(yōu)化。由于勢(shì)場(chǎng)的影響，檢測(cè)到的障礙物會(huì)產(chǎn)生斥力場(chǎng)，因此節(jié)點(diǎn)生成時(shí)函數(shù)值過大，會(huì)直接將其剔除，因此改進(jìn)后的RRT算法也不再需要重復(fù)進(jìn)行碰撞檢測(cè)，縮短了整體的運(yùn)算時(shí)間。

1 問題分析

1.1 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)組成的路徑[10]。

以二維空間中的RRT算法為例，假設(shè)在二維狀態(tài)空間C中，引入初始點(diǎn)xinit和目標(biāo)點(diǎn)xgoal，使得xinit根據(jù)快速拓展隨機(jī)樹T進(jìn)行隨機(jī)樹的逐步擴(kuò)建，最終到達(dá)xgoal，其過程如圖1所示。

圖1 節(jié)點(diǎn)生成策略

狀態(tài)空間C中，障礙物所在的區(qū)域?yàn)檎系K區(qū)Xob(XobX)，其他可通行的區(qū)域?yàn)橥ㄐ袇^(qū)Xpa(XpaX)。在工作過程中，RRT算法會(huì)從初始點(diǎn)xinit開始逐步通過隨機(jī)采樣擴(kuò)展結(jié)點(diǎn)，首先它會(huì)選定任意一個(gè)父節(jié)點(diǎn)，之后根據(jù)一個(gè)隨機(jī)的方向和規(guī)定的步長(zhǎng)生成一個(gè)最近的臨時(shí)節(jié)點(diǎn)xrand，根據(jù)xrand所在位置判定其在Xob(XobX)還是Xpa(XpaX)。若其在障礙區(qū)，則該xrand無效：若其在通行區(qū)，則可生成一新的父節(jié)點(diǎn)xnew。這一過程會(huì)一直重復(fù)，直到xnew進(jìn)入目標(biāo)點(diǎn)xgoal的鄰域范圍Xgoal(XgoalX)，表明已成功找到目標(biāo)點(diǎn)xgoal，進(jìn)行逆追隨找到連接xinit與xgoal的最短路徑，完成路徑規(guī)劃。其偽代碼如表1所示。

表1 RRT algorithm

隨機(jī)樹T生長(zhǎng)過程中，函數(shù)RANDOM_STATE（）會(huì)從空間中選取一點(diǎn)；根據(jù)函數(shù)NEAREST_COMPARE（），會(huì)在空間中拓展出一點(diǎn)；經(jīng)過函數(shù)STEER（），SELECT_INPUT（），NEW_STATE（）判定后得到隨機(jī)樹上的新節(jié)點(diǎn)；之后collisionchecking（）函數(shù)會(huì)對(duì)其進(jìn)行碰撞檢測(cè)，若滿足條件則生成新節(jié)點(diǎn)；最后進(jìn)行鄰域檢測(cè)findnear_neighbor（），若滿足則路徑規(guī)劃成功；經(jīng)過路徑比較函數(shù)MINPATH（）輸出最優(yōu)路線。

1.2 實(shí)際問題分析

經(jīng)過實(shí)驗(yàn)測(cè)試，傳統(tǒng)的RRT算法依然存在著很多問題。

（1）規(guī)劃路徑成功率較低，在測(cè)試實(shí)驗(yàn)中，設(shè)定最大節(jié)點(diǎn)生成個(gè)數(shù)為1 000個(gè)，在圖2實(shí)驗(yàn)中，成功率為70%；在圖3實(shí)驗(yàn)中，成功率僅僅為50%。若想增高成功率，可以設(shè)定更多的節(jié)點(diǎn)數(shù)和減小節(jié)點(diǎn)步長(zhǎng)，但無疑會(huì)加大工作量，延長(zhǎng)整體的工作時(shí)間。

圖2 路徑規(guī)劃1

圖3 路徑規(guī)劃2

（2）所規(guī)劃路徑質(zhì)量較低，在兩種地圖中，RRT算法雖然都成功完成了路徑規(guī)劃任務(wù)，但所得到的路徑是非常不理想的，與最優(yōu)路徑之間還存在著一定的距離，同時(shí)所規(guī)劃路徑存在過大的轉(zhuǎn)角，在實(shí)際工作過程中，會(huì)產(chǎn)生一定程度的震蕩，降低整個(gè)工作過程的穩(wěn)定性。

（3）在狹窄空間中，RRT算法容易陷入無解情況，如圖3所示，由于算法生成策略的隨機(jī)性，在狹窄的通路中，當(dāng)父節(jié)點(diǎn)離障礙物較近時(shí)，新生成的節(jié)點(diǎn)很大概率落入障礙區(qū)中，導(dǎo)致選點(diǎn)失敗陷入死循環(huán)。

針對(duì)于以上的問題，本文提出了一種RRT結(jié)合人工勢(shì)場(chǎng)軌跡規(guī)劃的算法。改進(jìn)RANDOM_STATE（）選點(diǎn)函數(shù)，加入引力場(chǎng)和斥力場(chǎng)綜合作用影響和狹窄概率算法，使得生成受到勢(shì)場(chǎng)和概率算法的綜合影響。由于勢(shì)場(chǎng)的加入，障礙物生成的斥力場(chǎng)會(huì)影響到隨機(jī)點(diǎn)生成，使得隨機(jī)點(diǎn)不會(huì)生成在障礙區(qū)中，因此改進(jìn)后的算法不再需要進(jìn)行碰撞檢測(cè)函數(shù)collisionchecking（），提高了工作效率。引力場(chǎng)與斥力場(chǎng)的相互作用，使得整個(gè)空間中的勢(shì)場(chǎng)連續(xù)性變化，加之勢(shì)場(chǎng)法中對(duì)于機(jī)器人轉(zhuǎn)角因素的考慮，減小了路徑規(guī)劃中的過大轉(zhuǎn)角，增強(qiáng)了工作過程的平穩(wěn)性。在狹窄空間中，狹窄概率算法的加入使得路徑搜索的成功率增大，在面臨復(fù)雜地形時(shí)整個(gè)算法的表現(xiàn)更加出色。

2 算法的改進(jìn)

2.1 勢(shì)場(chǎng)法的引入

針對(duì)傳統(tǒng)RRT算法隨機(jī)性過大、規(guī)劃路徑質(zhì)量較低的問題，本文提出一種基于傳統(tǒng)RANDOM_STATE（）選點(diǎn)函數(shù)的增益求解函數(shù)。

在傳統(tǒng)的勢(shì)場(chǎng)法中，針對(duì)于機(jī)器人中的路徑規(guī)劃問題，經(jīng)常使用如下的引力函數(shù)和斥力函數(shù)：

式中：Ua(Pn)為機(jī)器人在位置Pn時(shí)所受到的引力；Ka為引力系數(shù)；Pg為目標(biāo)點(diǎn)所在位置。

式中：Ur為斥力函數(shù)；Kr為斥力系數(shù)；ln為機(jī)器人各個(gè)連桿與障礙物之間的距離；l0為整個(gè)斥力場(chǎng)的作用范圍。

根據(jù)機(jī)器人各個(gè)連桿與障礙物之間的距離，就可以很輕松的知道機(jī)器人與障礙物是否發(fā)生了碰撞，因此在優(yōu)化選點(diǎn)函數(shù)時(shí)，可以將兩者的距離差作為條件編入函數(shù)，以此省略了之后的collisionchecking（）碰撞檢測(cè)函數(shù)。

對(duì)于任意時(shí)刻，機(jī)器人受到的總勢(shì)能為引力勢(shì)能與斥力勢(shì)能的總和，即：

2.2 選點(diǎn)函數(shù)的改進(jìn)

在RRT算法中，傳統(tǒng)的選點(diǎn)函數(shù)如圖4所示，以xnew父節(jié)點(diǎn)為中心，初始設(shè)定的步長(zhǎng)為半徑，得到子節(jié)點(diǎn)的出生圓。之后根據(jù)隨機(jī)方向rand，得到一個(gè)新的子節(jié)點(diǎn)xrand，再進(jìn)行碰撞檢測(cè)，若滿足工作要求則子節(jié)點(diǎn)生成成功。

圖4 傳統(tǒng)原理

從其工作原理中可看出，方向的確定是完全隨機(jī)的，而且每次生成新的子節(jié)點(diǎn)之后都要進(jìn)行碰撞檢測(cè)，雖然拓展性較好，但針對(duì)于機(jī)器人的復(fù)雜工作環(huán)境來看，其工作效率很低，選點(diǎn)方式也并不科學(xué)。

結(jié)合勢(shì)場(chǎng)法的思想，如圖5所示，根據(jù)實(shí)際工作環(huán)境設(shè)定工作區(qū)域，將其分為若干個(gè)區(qū)域，計(jì)算每個(gè)區(qū)域中的綜合增益，根據(jù)增益得到每個(gè)區(qū)域方向的生成概率，完成最終的子節(jié)點(diǎn)生成。綜合增益可表示為：

圖5 改進(jìn)原理

式中：k0為初始增益系數(shù)；X0為初始增益，其計(jì)算已在經(jīng)典算法中得出；ku為勢(shì)場(chǎng)增益系數(shù)；Xui為勢(shì)場(chǎng)增益。k0、ku可以根據(jù)實(shí)際的工作過程進(jìn)行取值。Xui可表示為：

式中：k?為條件系數(shù)，若步長(zhǎng)圓內(nèi)存在障礙物，則取值為0，否則為1；Di為各個(gè)區(qū)域范圍；U(θ,r)為總勢(shì)能函數(shù)，其中θ、r可根據(jù)實(shí)際工作過程進(jìn)行取值。

根據(jù)每部分的增益結(jié)果，可得到各區(qū)域隨機(jī)方向生成的概率為：

改進(jìn)后的選點(diǎn)函數(shù)由于勢(shì)場(chǎng)的引導(dǎo)作用，省去了很多無效點(diǎn)的生成和碰撞檢測(cè)，使得路徑規(guī)劃效率更高，節(jié)約了時(shí)間。新生成父、子節(jié)點(diǎn)之間的轉(zhuǎn)角不會(huì)過大，使得整個(gè)路徑更光滑，也更接近理想路徑。

2.3 加入狹窄概率函數(shù)

針對(duì)于RRT算法在狹窄環(huán)境中易陷入無解的情況，有針對(duì)性的加入一種狹窄概率函數(shù)，其偽代碼如表2所示。

表2 狹義算法

隨機(jī)選點(diǎn)函數(shù)COLLISIONCHECKING_NEW在工作過程中，狹窄概率函數(shù)會(huì)同時(shí)使用監(jiān)測(cè)函數(shù)MONITOR監(jiān)測(cè)新生成的xnew節(jié)點(diǎn)，當(dāng)xnew與D中已經(jīng)生成的節(jié)點(diǎn)之間的距離和位置條件，滿足judge函數(shù)時(shí)，則會(huì)將工作區(qū)域個(gè)數(shù)進(jìn)行更細(xì)劃分，以此找到可能存在的狹窄通路。

RRT算法方向選擇的隨機(jī)性和勢(shì)場(chǎng)法容易陷入局部死循環(huán)的特性，都決定了其對(duì)于狹窄路徑規(guī)劃存在著弊端。狹窄概率算法很好地解決了狹窄通路難以尋找的問題，使得算法的整體魯棒性增強(qiáng)，優(yōu)化了路徑規(guī)劃過程。

3 實(shí)驗(yàn)驗(yàn)證

3.1 實(shí)驗(yàn)一

將RRT算法與RRT結(jié)合人工勢(shì)場(chǎng)軌跡規(guī)劃的算法進(jìn)行仿真實(shí)驗(yàn)，驗(yàn)證算法的實(shí)際工作情況。如圖6~7所示，本次實(shí)驗(yàn)采用Matlab R2016b數(shù)學(xué)仿真軟件進(jìn)行，設(shè)置多組地圖進(jìn)行測(cè)試。實(shí)驗(yàn)一的部分參數(shù)如表3所示。

表3 實(shí)驗(yàn)一參數(shù)

圖6 傳統(tǒng)算法

圖7 改進(jìn)算法

實(shí)驗(yàn)過程中，黑色圖形為隨機(jī)設(shè)置的障礙物，紅色圓點(diǎn)為初始點(diǎn)xinit，綠色圓點(diǎn)為目標(biāo)點(diǎn)xgoal，隨機(jī)樹生成的節(jié)點(diǎn)為藍(lán)色圓點(diǎn)，每步之間的距離用藍(lán)色線段相連，其中找到的最優(yōu)路徑用紅色路線標(biāo)出。

實(shí)驗(yàn)數(shù)據(jù)對(duì)比如表4所示。實(shí)驗(yàn)中的部分?jǐn)?shù)據(jù)如表中所示，通過實(shí)驗(yàn)數(shù)據(jù)可明顯看出，改進(jìn)后的算法成功率有所提高，在200次實(shí)驗(yàn)中僅有6次沒有成功找到目標(biāo)點(diǎn)，成功率提高了13%。同時(shí)，所產(chǎn)生的節(jié)點(diǎn)個(gè)數(shù)大大減少，改進(jìn)后算法生成的節(jié)點(diǎn)數(shù)減少了大約86.22%，因此整體工作效率也得到了很大提高，平均工作時(shí)間減少了大約67.6%，同時(shí)改進(jìn)后的算法所找到的路徑質(zhì)量較高，接近理想路徑的數(shù)量更多。

表4 算法比較

綜上所述，改進(jìn)后的算法在普通環(huán)境中能更好地實(shí)現(xiàn)路徑規(guī)劃，無論是從成功率、節(jié)點(diǎn)數(shù)還是工作時(shí)間上都得到了一定改善，接下來仍要繼續(xù)測(cè)試在一些特殊條件中算法的表現(xiàn)如何。

3.2 實(shí)驗(yàn)二

在本組實(shí)驗(yàn)中，針對(duì)于算法的狹窄空間路徑規(guī)劃能力進(jìn)行測(cè)試，如圖8~9所示。采用Matlab R2016b數(shù)學(xué)仿真軟件進(jìn)行，建立了整體地形較為狹窄的空間，設(shè)置多個(gè)復(fù)雜、易誘導(dǎo)路徑，模擬實(shí)際的工作過程。部分實(shí)驗(yàn)參數(shù)如表5所示。

表5 實(shí)驗(yàn)二參數(shù)

圖8 傳統(tǒng)算法

在對(duì)算法進(jìn)行了200次模擬后，結(jié)果如表6所示。普通RRT算法成功率僅為47%，且所用平均節(jié)點(diǎn)數(shù)約為909個(gè)，實(shí)驗(yàn)時(shí)間較長(zhǎng)。而改進(jìn)后的算法成功率為63.5%，增加了16.5%，平均節(jié)點(diǎn)數(shù)減少了大約13.3%，平均時(shí)間減少了大約22.1%。

圖9 改進(jìn)算法

表6 實(shí)驗(yàn)二結(jié)果

因?yàn)樵O(shè)置地圖環(huán)境較為復(fù)雜，且設(shè)定步數(shù)也僅為1 000步，所以兩種算法成功率均較低，但是也可明顯看出改進(jìn)后的算法對(duì)于路徑的規(guī)劃表現(xiàn)更佳，從成功率和規(guī)劃效率上有了一定的優(yōu)化，尤其在地圖的狹窄部分，由于選點(diǎn)函數(shù)和狹窄概率函數(shù)的影響，使得選點(diǎn)判斷更合理，所選擇路徑更優(yōu)。

4 結(jié)束語

設(shè)計(jì)一種新的選點(diǎn)函數(shù)，通過區(qū)域化增益的方式，隨機(jī)生成新的子節(jié)點(diǎn)，通過節(jié)點(diǎn)選擇判定后產(chǎn)生，可以有效減少無效點(diǎn)，提高選點(diǎn)效率。

針對(duì)特殊狹窄空間，彈性地改變選點(diǎn)判定范圍，更有利于尋找正確路徑，提高整體路徑規(guī)劃效率。

本文研究提出的RRT結(jié)合人工勢(shì)場(chǎng)軌跡規(guī)劃的算法，經(jīng)過模擬實(shí)驗(yàn)測(cè)試，相對(duì)于傳統(tǒng)的RRT算法，改進(jìn)后的算法擁有更好的路徑規(guī)劃能力，能有效改善路徑規(guī)劃中路徑的選擇問題。