基于引導(dǎo)擴(kuò)展的快速隨機(jī)搜索樹算法

楊馨韻，嚴(yán)華

（四川大學(xué)電子信息學(xué)院，成都610065）

針對移動機(jī)器人在路徑規(guī)劃過程中，快速擴(kuò)展隨機(jī)樹（RRT）算法隨機(jī)性強(qiáng)、搜索無偏向性以及在較窄出口環(huán)境下搜索效率明顯下降等問題，提出一種基于障礙物有效頂點引導(dǎo)擴(kuò)展的改進(jìn)算法。該算法通過對碰撞障礙物分析，選取障礙物有效頂點引導(dǎo)隨機(jī)樹擴(kuò)展，從而提高隨機(jī)樹的搜索效率。最后，在機(jī)器人操作系統(tǒng)（ROS）上進(jìn)行仿真實驗，使用ROS可視化工具（RVIZ）顯示規(guī)劃結(jié)果，結(jié)果顯示基于障礙物有效頂點引導(dǎo)擴(kuò)展的算法性能更優(yōu)。

移動機(jī)器人；路徑規(guī)劃；快速擴(kuò)展隨機(jī)樹算法；目標(biāo)偏向；有效障礙物頂點

0 引言

近年來，路徑規(guī)劃廣泛應(yīng)用在產(chǎn)品組裝、無人水面艇路徑規(guī)劃、無人機(jī)航跡規(guī)劃等領(lǐng)域，引起了研究者的高度關(guān)注和重視[1-3]。路徑規(guī)劃問題描述為根據(jù)最短距離、最短規(guī)劃時間等性能指標(biāo)搜索出一條從起始點到目標(biāo)點的最優(yōu)路徑或次優(yōu)路徑[4]。路徑規(guī)劃的方法有很多，大致可以分為兩類：傳統(tǒng)的路徑規(guī)劃算法，包括可視圖法、柵格法、人工勢場法等；智能路徑規(guī)劃算法，如遺傳算法、神經(jīng)網(wǎng)絡(luò)算法等[5]。傳統(tǒng)的路徑規(guī)劃算法結(jié)構(gòu)簡單，但在障礙物較多的復(fù)雜環(huán)境中，算法實現(xiàn)困難；遺傳算法實現(xiàn)簡單，但容易陷入局部最小值，效率不高；神經(jīng)網(wǎng)絡(luò)算法學(xué)習(xí)能力優(yōu)秀，但是泛化能力差。

快速擴(kuò)展隨機(jī)樹[6]（Rapidly-exploring Random Tree，RRT）算法是由LaValle等人提出的一種基于采樣的路徑規(guī)劃算法。它提出了一種啟發(fā)式、隨機(jī)化的動態(tài)規(guī)劃方法，能快速地探索狀態(tài)空間，并能很好地解決具有高自由度和復(fù)雜系統(tǒng)動力學(xué)的問題。因不需要對空間建模，能夠有效地解決高維空間中機(jī)器人路徑規(guī)劃問題，在路徑規(guī)劃領(lǐng)域得到了廣泛應(yīng)用。但RRT算法也存在以下缺陷：①隨機(jī)樹在自由空間中隨機(jī)采樣，搜索無導(dǎo)向性；②收斂速度遲緩、搜索效率低等。

針對上述不足，考慮收斂速度、搜索效率的改進(jìn)RRT算法被大量提出。其中被廣泛提及的要數(shù)LaValle等人提出的雙向多步RRT（RRT-Connect）[7]，它分別在起始點和目標(biāo)點構(gòu)建隨機(jī)樹，使用貪婪的啟發(fā)式方法來連接兩棵隨機(jī)樹，以改善規(guī)劃時間。時至今日，仍有很多研究者在不斷研究提升隨機(jī)樹搜索效率的改進(jìn)算法，同時將改進(jìn)算法應(yīng)用到更多場景[8-11]。

RRT的改進(jìn)算法很多，但算法本身具有的隨機(jī)性依然存在，這使得RRT算法在出口較窄的復(fù)雜環(huán)境中算法效率明顯下降。為此，本文基于RRT-Connect算法，提出一種使用障礙物有效頂點引導(dǎo)隨機(jī)樹擴(kuò)展的改進(jìn)思想，以提升算法效率。最后，在具有較窄出口的環(huán)境中與RRT、RRT-Connect和DRRT-Connect[12]進(jìn)行對比實驗，證明改進(jìn)算法的有效性。

1 相關(guān)工作

1.1 問題定義

為方便敘述，本文借鑒Karama和Frazzoli提出的路徑規(guī)劃問題公式[13]。

定義1（自由/障礙物空間）給定一個環(huán)境空間X?Rd，用Xfree?X表示自由空間，Xobs?X表示障礙物空間。

定義2（無碰撞路徑）設(shè)（xinit,xgoal,Xfree）為路徑規(guī)劃問題，其中：xinit∈Xfree為起始點，xgoal∈Xfree為目標(biāo)點，Xgoal?Xfree為目標(biāo)點區(qū)域。當(dāng)且僅當(dāng)σ(0)=xinit，σ()1?Xgoal時，自由空間的連續(xù)映射σ:[0,1]→Xfree是無碰撞路徑。

1.2 基本RRT算法

快速隨機(jī)搜索樹算法的關(guān)鍵思想是利用狀態(tài)空間中的采樣點來引導(dǎo)隨機(jī)樹的擴(kuò)展，從而使探索偏向于空間中未探索的區(qū)域。在每次迭代中，隨機(jī)樹都向隨機(jī)方向擴(kuò)展，只要有足夠的時間和足夠的迭代次數(shù)，就沒有不被探索的區(qū)域，所以該算法總是可以得到一條路徑。

RRT算法的擴(kuò)展過程如圖1所示，首先在起始點xinit構(gòu)建隨機(jī)樹，在每次迭代中，使用函數(shù)Random在狀態(tài)空間中隨機(jī)采樣一個節(jié)點xrand，并調(diào)用Extend函數(shù)來增加葉子節(jié)點，當(dāng)新增節(jié)點xnew到達(dá)xgoal或者進(jìn)入xgoal區(qū)域，回溯該節(jié)點以獲得一條由初始點到目標(biāo)點的路徑。Extend函數(shù)用于擴(kuò)展節(jié)點，首先在隨機(jī)樹中找到距離xrand最近的節(jié)點xnear，然后從xnear向xrand擴(kuò)展一步得到一個新節(jié)點xnew。此時會出現(xiàn)三種情況：如果xnew和xnear的連線與障礙物碰撞，放棄此次擴(kuò)展返回Trapped；如果到達(dá)xrand，將xrand加入隨機(jī)樹返回Reached；否則將xnew添加到隨機(jī)樹返回Advanced。

圖1 RRT擴(kuò)展示意圖

1.3 RRT-Connect算法

基于RRT算法搜索空間的盲目性和節(jié)點擴(kuò)展缺乏記憶性，為提高空間搜索效率，Kuffner和LaValle提出RRT-Connect算法。

RRT-Connect算法的擴(kuò)展過程如圖2所示。不同于基本RRT算法，RRT-Connect算法分別在起始點xinit和目標(biāo)點xgoal構(gòu)建兩棵并行的隨機(jī)樹；在每次迭代過程中，一棵樹以隨機(jī)采樣方式擴(kuò)展一個步長的距離得到xnew，另一棵樹使用Connect函數(shù)嘗試連接xnew。同時，在每次迭代結(jié)束后選擇節(jié)點數(shù)較少的樹擴(kuò)展。Connect是一個貪婪的函數(shù)，通過在內(nèi)部迭代調(diào)用Extend函數(shù)擴(kuò)展節(jié)點，直到碰到障礙物或者到達(dá)xnew停止擴(kuò)展。

圖2 RRT-Connect擴(kuò)展示意圖

雖然RRT-Connect采用connect策略可以降低算法的隨機(jī)性，但是由于在每次迭代中使用了隨機(jī)方式擴(kuò)展節(jié)點，依然存在效率低下的情況。為此，王坤等人提出DRRT-connect算法，該算法在RRT-Connect的基礎(chǔ)上增加第三節(jié)點，生成四棵隨機(jī)樹，極大地提高了算法的收斂速度；同時引入目標(biāo)偏向策略，使隨機(jī)樹能更好地向目標(biāo)擴(kuò)展，提高了無障礙條件下的收斂速度。然而，DRRT-connect仍然存在缺點。例如，在一些復(fù)雜的環(huán)境下，選擇第三節(jié)點比較困難。還有，當(dāng)目標(biāo)連線上存在障礙物且環(huán)境出口較小時，隨機(jī)樹往往被困在出口周圍。為了找到出口，需要進(jìn)行大量的迭代，因此會消耗大量的時間。此時，使用有效的方法引導(dǎo)隨機(jī)樹擴(kuò)展是改進(jìn)算法的關(guān)鍵。

2 改進(jìn)算法

根據(jù)上節(jié)討論，將隨機(jī)樹擴(kuò)展分為以下兩種情況分析。首先，考慮簡單無障礙環(huán)境。如圖3（a）所示，隨機(jī)樹已擴(kuò)展到xnow，采用目標(biāo)偏向策略可以快速擴(kuò)展隨機(jī)樹。其次，當(dāng)從xnow到目標(biāo)節(jié)點xgoal連線上存在障礙物時，若采用目標(biāo)偏向策略引導(dǎo)隨機(jī)樹擴(kuò)展，會使擴(kuò)展停在障礙物附近；此時，若沿著障礙物邊界擴(kuò)展，繞開障礙物找到路徑是可行的。已知障礙物邊界是由一個個障礙物頂點連接而成，若直接在障礙物邊界中找到合適、有效的頂點作為臨時目標(biāo)點，并引導(dǎo)隨機(jī)樹擴(kuò)展，則尋路時間和路徑長度會大大縮短（如圖3（b）所示）。

因此，本文改進(jìn)算法策略如下：在目標(biāo)偏向策略的基礎(chǔ)上引入障礙物頂點引導(dǎo)策略。在每次迭代中，判斷當(dāng)前節(jié)點到目標(biāo)點之間是否存在障礙物。在無障礙物情況下，采用目標(biāo)偏向策略引導(dǎo)隨機(jī)樹快速擴(kuò)展。反之，找到引發(fā)碰撞的障礙物，即最接近當(dāng)前節(jié)點的障礙物，并在障礙物頂點中找到合適的頂點作為臨時目標(biāo)點引導(dǎo)擴(kuò)展。

圖3 障礙物頂點引導(dǎo)擴(kuò)展

2.1 障礙物有效頂點

為方便描述，首先提出以下概念。

定義3（凸多邊形）如果一個多邊形的任意一條邊向兩方無限延長成為一直線時，其他各邊都在此直線的同旁，那么這個多邊形就叫做凸多邊形。

圖4 凹凸點（實心圓為凸點，空心圓為凹點）

考慮以下情況，將隨機(jī)樹擴(kuò)展到xnow，目標(biāo)點為xgoal（如圖4所示），分析三種引導(dǎo)方式：（1）使用目標(biāo)偏向策略，路徑會與障礙物發(fā)生碰撞；（2）利用多邊形障礙物的凹點A或B引導(dǎo)擴(kuò)展，隨機(jī)樹將進(jìn)入障礙物凹處，這與我們逃離障礙物的初衷相違背；（3）利用多邊形障礙物的凸點C引導(dǎo)擴(kuò)展，當(dāng)隨機(jī)樹到達(dá)頂點C的位置時，可以直接與xgoal相連。其次，由凹凸點的定義可以得出，相對于凹點，凸點的內(nèi)角小于等于180度，可以在更大的范圍內(nèi)擴(kuò)展?？傊裹c的性質(zhì)可以使其作為一個合適的臨時目標(biāo)點，引導(dǎo)隨機(jī)樹逃離障礙物而不會進(jìn)入障礙物死區(qū)。

根據(jù)定義3、4可知，凸多邊形的每一個頂點就是一個凸點，計算多邊形的凸點，就是求出多邊形的最大凸多邊形。計算方法如下：

Step1按順（逆）時針順序給定多邊形頂點坐標(biāo)P1( x1,y1),P2( x2,y2),…,Pn(xn,yn)，記錄頂點個數(shù)為n。

Step2利用定義2判斷多邊形的每個頂點，記錄凸點。

Step3若記錄的凸點個數(shù)等于多邊形頂點個數(shù)n，結(jié)束算法；否則將所有凸點順序排列，更新n值，跳轉(zhuǎn)Step2。

2.2 臨時目標(biāo)點選取策略

考慮到機(jī)器人實際尺寸，為避免碰撞，需要對障礙物有效頂點進(jìn)行膨脹處理。首先將障礙物中所有凸點Pl(xl,yl)按照順時針順序連接得到一個凸多邊形；在凸多邊形的外部做平行于的兩條平行線，距離為σ，兩條線的交點Ql即為膨脹后的Pl，如圖5所示。計算公式如下：

圖5 膨脹障礙物有效頂點

臨時目標(biāo)點選擇步驟如下：首先，找到當(dāng)前節(jié)點和目標(biāo)節(jié)點連線中距離當(dāng)前節(jié)點最近的障礙物；其次，將障礙物有效頂點Pl經(jīng)過膨脹處理后得到的Ql作為可選對象；最后，按照以下規(guī)則選取臨時目標(biāo)點。

（1）該點在自由空間中。

（2）不在已擴(kuò)展的隨機(jī)樹上。

（3）與當(dāng)前節(jié)點連線無碰撞。

（4）滿足以上3點條件且起始點到該點、該點到目標(biāo)點距離之和最短。

2.3 算法實現(xiàn)

改進(jìn)算法具體實現(xiàn)步驟如下：

Step1設(shè)置兩棵樹分別從xinit和xgoal出發(fā)；

Step2引入目標(biāo)偏向機(jī)制，每次迭代中，以非擴(kuò)展樹最新節(jié)點xnew為目標(biāo)點擴(kuò)展；

Step3調(diào)用NearestNeighbor函數(shù)，找到擴(kuò)展樹中距離目標(biāo)點最近的節(jié)點xnear作為當(dāng)前節(jié)點，如果當(dāng)前節(jié)點到目標(biāo)點連線中存在障礙物執(zhí)行Step4，反之跳轉(zhuǎn)

Step5；

Step4調(diào)用FindTempNode函數(shù)，首先找到當(dāng)前節(jié)點和目標(biāo)點連線中距離當(dāng)前結(jié)點最近的障礙物；用臨時目標(biāo)點選擇機(jī)制選取合適的臨時目標(biāo)點，執(zhí)行Step5，若沒有找到合適的臨時目標(biāo)點，跳轉(zhuǎn)Step6；

Step5從當(dāng)前節(jié)點以步長step擴(kuò)展節(jié)點，直到到達(dá)（臨時）目標(biāo)點，跳轉(zhuǎn)Step7；

Step6交換隨機(jī)樹，如果此時仍然沒有找到合適的目標(biāo)點引導(dǎo)隨機(jī)樹擴(kuò)展，選取節(jié)點較少的隨機(jī)樹，使用隨機(jī)方式擴(kuò)展該隨機(jī)樹；

Step7調(diào)用Connect函數(shù)，判斷兩棵樹是否連接。如果連接，返回路徑；反之，執(zhí)行Step2。

3 實驗與分析

根據(jù)上述改進(jìn)設(shè)計算法，本文在搭載了機(jī)器人操作系統(tǒng)（Robots On System，ROS）的Ubuntu 14.04上進(jìn)行算法仿真測試，并在ROS可視化工具（3D visualization tool for ROS，RVIZ）上展示完整的路徑規(guī)劃結(jié)果。

為證明改進(jìn)算法的性能，使用基本RRT、RRTConnect和DRRT-Connect算法進(jìn)行比較。實驗在兩種出口較窄的環(huán)境中進(jìn)行，地圖大小為200×200，起始點坐標(biāo)設(shè)置為（3,3），位于地圖左上角；目標(biāo)點坐標(biāo)設(shè)置為（197,197），位于地圖右下角。為方便對比，設(shè)置機(jī)器人半徑為3，步長為3。考慮到機(jī)器人半徑，設(shè)置障礙物有效頂點向外膨脹長度為5。

圖6和圖7分別是地圖1、2的實驗結(jié)果，從左到右從上到下依次是RRT、RRT-Connect、DRRT-Connect和改進(jìn)算法，組實線是規(guī)劃后的路徑，細(xì)實線是隨機(jī)樹的分支。

圖6 地圖1情況下路徑圖

圖7 地圖2情況下路徑圖

從結(jié)果可以看出，基本RRT算法的規(guī)劃結(jié)果分支較多，探索區(qū)域覆蓋了整個空閑空間。由于RRT-Connect采用兩棵樹交替擴(kuò)展，并且引入具有貪婪特性的連接函數(shù)，與RRT算法的規(guī)劃結(jié)果相比，減少了分支，但是仍然存在探索無效區(qū)域的情況。DRRT-Connect在RRT-Connect的基礎(chǔ)上提出第三節(jié)點的概念，增加了隨機(jī)樹的數(shù)量，并且引入目標(biāo)偏向策略使隨機(jī)樹擴(kuò)展更具方向性，規(guī)劃結(jié)果表明，隨機(jī)樹的分支更少。另外，三種算法在較窄出口處都存在盲目采樣的情況。相反，改進(jìn)算法利用狹窄出口處的障礙物頂點引導(dǎo)隨機(jī)樹擴(kuò)展，降低了探索的盲目性，使隨機(jī)樹能夠更快地找到路徑出口，進(jìn)而規(guī)劃結(jié)果具有更少的分支和更少的無效探索。

為避免實驗偶然性，在兩個地圖上分別對每種算法進(jìn)行了50次仿真實驗，實驗結(jié)果如表1、2所示。從數(shù)據(jù)中可以更為直觀的看出，相對于RRT、RRT-Connect和DRRT-Connect，改進(jìn)算法在迭代次數(shù)和平均規(guī)劃時間上都有明顯的優(yōu)勢。此外，改進(jìn)算法還縮短了路徑長度法，主要是因為隨機(jī)樹向目標(biāo)點擴(kuò)展過程中，在發(fā)現(xiàn)障礙物時就以障礙物有效頂點引導(dǎo)擴(kuò)展，而不是接近障礙物后才選擇繞過它。

表1 地圖1仿真實驗數(shù)據(jù)對比

表2 地圖2仿真實驗數(shù)據(jù)對比

4 結(jié)語

針對RRT算法隨機(jī)性強(qiáng)、搜索無偏向性以及在較窄出口環(huán)境中搜索效率明顯下降等問題，本文提出了一種目標(biāo)偏向策略和障礙物有效頂點引導(dǎo)策略相結(jié)合的改進(jìn)算法解決該問題，并通過仿真實驗證明了改進(jìn)策略的有效性。