基于引導擴展的快速隨機搜索樹算法

楊馨韻，嚴華

（四川大學電子信息學院，成都610065）

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

移動機器人；路徑規劃；快速擴展隨機樹算法；目標偏向；有效障礙物頂點

0 引言

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

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

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

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

1 相關工作

1.1 問題定義

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

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

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

1.2 基本RRT算法

快速隨機搜索樹算法的關鍵思想是利用狀態空間中的采樣點來引導隨機樹的擴展，從而使探索偏向于空間中未探索的區域。在每次迭代中，隨機樹都向隨機方向擴展，只要有足夠的時間和足夠的迭代次數，就沒有不被探索的區域，所以該算法總是可以得到一條路徑。

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

圖1 RRT擴展示意圖

1.3 RRT-Connect算法

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

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

圖2 RRT-Connect擴展示意圖

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

2 改進算法

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

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

圖3 障礙物頂點引導擴展

2.1 障礙物有效頂點

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

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

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

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

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

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

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

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

2.2 臨時目標點選取策略

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

圖5 膨脹障礙物有效頂點

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

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

（2）不在已擴展的隨機樹上。

（3）與當前節點連線無碰撞。

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

2.3 算法實現

改進算法具體實現步驟如下：

Step1設置兩棵樹分別從xinit和xgoal出發；

Step2引入目標偏向機制，每次迭代中，以非擴展樹最新節點xnew為目標點擴展；

Step3調用NearestNeighbor函數，找到擴展樹中距離目標點最近的節點xnear作為當前節點，如果當前節點到目標點連線中存在障礙物執行Step4，反之跳轉

Step5；

Step4調用FindTempNode函數，首先找到當前節點和目標點連線中距離當前結點最近的障礙物；用臨時目標點選擇機制選取合適的臨時目標點，執行Step5，若沒有找到合適的臨時目標點，跳轉Step6；

Step5從當前節點以步長step擴展節點，直到到達（臨時）目標點，跳轉Step7；

Step6交換隨機樹，如果此時仍然沒有找到合適的目標點引導隨機樹擴展，選取節點較少的隨機樹，使用隨機方式擴展該隨機樹；

Step7調用Connect函數，判斷兩棵樹是否連接。如果連接，返回路徑；反之，執行Step2。

3 實驗與分析

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

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

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

圖6 地圖1情況下路徑圖

圖7 地圖2情況下路徑圖

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

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

表1 地圖1仿真實驗數據對比

表2 地圖2仿真實驗數據對比

4 結語

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