一種改進的雙向快速搜索隨機樹算法

徐秉超， 嚴 華

(四川大學電子信息學院，成都 610065)

隨著機器人控制及智能化領域研究的不斷發展，作為基礎的路徑規劃研究也日趨深化。路徑規劃指在障礙物環境下，尋找從初始點至目的點的運行路徑，同時保證路徑可跟蹤，安全無碰撞[1]。常用的路徑規劃算法分為不同類型，如Dijkstra、A*、D*等組成的前向圖搜索算法，以柵格法和可視圖法為代表的幾何構造規劃算法以及在智能控制理論下出現的基于遺傳算法[2]和神經網絡[3]的規劃算法。

快速搜索隨機樹(RRT)算法由Lavall在1998年提出。相比全局搜索和啟發式算法，RRT算法[4]能在保證概率完備性的同時極大降低時間成本。在高維空間里的優秀表現也令它倍受關注。RRT優化性能強大，將機械對象的實際動力學約束加入規劃，可以使生成路徑可跟蹤[5]。該算法無需對地圖進行幾何劃分，可以在動態規劃中對局部地圖進行再規劃以生成局部路徑[6],也廣泛應用于多目標點規劃任務[7]。然而，由于使用全局隨機采樣，RRT算法存在隨機性大、效率低的問題。為此，目標導向RRT[8]和雙向RRT[9]等改進方向被接連提出，基于概率的回歸機制[10]被用于緩解重復生長問題。文獻[11]的變步長雙向RRT算法通過目標引力和雙向搜索方式，跟蹤目標點并擴大連接面積，但同時降低了全局避障能力，額外耗費大量時間。

在變步長雙向RRT算法基礎上引入預生長、重要障礙斥力和隨機點篩選機制策略以縮小問題規模并提高避障過程中的隨機性及有效性，優化路徑規劃的效率。

1 相關工作

1.1 快速搜索隨機樹

傳統 RRT算法主要包括兩個階段：構造隨機生長樹和反向搜索可行的軌跡[12]。首先以起始點作為樹的根節點，在全狀態空間中隨機采樣以生長無碰撞的葉節點，直到有節點進入目標區域或達到目標點。隨后以距離目標點最近的點開始反向搜索父節點，生成一條從起始位置到目標位置的路徑[13]。

RRT算法中的單次生長如圖1所示。首先以起始點Xinit為根節點構造一棵隨機搜索樹。然后在狀態空間中隨機取一自由點Xrand，找出隨機搜索樹中距離Xrand最近的樹節點Xnear。接著，從Xnear向Xrand擴展出基礎步長為ρ的新節點Xnew。若Xnew與Xnear間的路徑沒有發生碰撞，則將Xnew加入搜索樹。重復上述過程直到節點進入目標區域或到達設置的最大迭代次數。一次完整的規劃結果如圖2所示。

圖1 RRT算法節點生長示意圖Fig.1 Schematic diagram of node growth RRT algorithm

圖2 RRT尋徑示意圖Fig.2 Diagram of RRT path searching

由圖2可知，傳統RRT算法雖然避免了局部最優的陷阱，但因小部分的局部最優陷阱而失去全部導向性，致使在大部分環境中，性能得不償失。為了解決此問題，需在傳統算法中加入適當的導向功能。路徑規劃有兩個要求，一是到達目標點，另一個是避開障礙，需要在目標跟蹤過程中加入啟發式降低隨機性，在避障過程中則需要完全隨機從而保證避障的效果。傳統RRT沒有對生長區域的有效性進行評估，例如在本文研究環境中，一方面為空間形成的區域區分度，即非主路徑區域擴展的可能性應稍低，在圖2環境中為右上和左下的部分區域；另一方面為時間形成的區域區分度，即已生長區域應有更低的可能性進行擴展，在圖2環境中為部分左上區域。

1.2 變步長雙向RRT

考慮到RRT算法的諸多不足，研究人員提出了雙向生長樹和目標引力機制。

雙向生長樹更多地在實際物理系統限制上進行優化，它從起點和目標點分別擴展生長樹，緩和了目標點所在位置對規劃的影響，不再需要對鄰近邊界的目標點進行特殊處理，并且將目標點擴展為點集，提高了搜索樹和目標連接的效率，從而降低了形成路徑的時間和復雜度。

在人工勢場法[14]中，路徑前進的方向取決于目標點和所有障礙，給規劃提供有效導向，因而實時性較好。在借鑒人工勢場法而加入目標引力的RRT算法中，新節點Xnew將使用式(1)得出：

(1)

式(1)中：ρ為基礎步長；k為引力系數，即在每一次生長過程中，Xnew相對于Xnear向隨機點前進ρ的同時，向目標點前進kρ長度。

目標引力在一定程度上解決了空間形成的區域區分度和到達目標過程中的導向，提高了規劃效率，但因為只取用了目標引力，受其影響的避障過程無法實現完全隨機，從而降低全局避障能力，對效率造成極大損失。

2 算法的改進

基于變步長雙向RRT算法，改進算法首先進行預生長，即先在直線方向進行生長的嘗試，以接近人類思維的方式，降低前期生長的代價；接著，加入隨機點篩選機制，對部分時間形成的區域區分度進行處理；最后，引入重要障礙物斥力，消除引力在避障過程中的影響，降低不必要的效率損失。

2.1 預生長

人類思維下，在無障礙的環境中，使用直線連接起點和目標點具有最高的效率。如圖3所示，預生長分別從兩棵樹的根節點開始，沿著起始點到目標點的直線方向，以2倍基礎步長2ρ檢測碰撞，以基礎步長進行生長直到發生碰撞，并刪除最后一個生長的節點。刪除操作用于保證搜索樹節點與障礙物存在適當距離。與障礙物距離過近的節點在后續生長中將有更大可能性發生碰撞，因而生長失敗的概率更大，效率降低。

圖3 預生長示意圖Fig.3 Diagram of pre-extended

預生長過程模擬了人類的直觀思維。在該過程中，除了進行少量碰撞檢測外無需其他計算，生長效率大于正常生長流程，在低消耗下快速通過初期的自由空間，減少在前期無障礙區域的時間花費，縮小了需進行規劃計算的區域，一定程度上縮小了問題規模。

2.2 隨機點篩選機制

目標引力解決了一部分區域區分度問題，卻依舊未能對時間形成的區域區分度進行處理。為此，一個基于metropolis接受準則的隨機點篩選機制將被用來嘗試緩解該問題。metropolis接受準則被用于模擬退火過程，它在一個已有解的基礎上，接受優化解的同時以一定概率接受惡化解。隨著逐步降低接受惡化解的概率，最終取得近似最優解。該準則可以跳出局部最優，同時保證概率完備性。

具體的，每一次隨機生長過程中，分別計算最近鄰點Xnear和生長樹中的最新節點Xend到目標點的距離。按照式(2)判斷隨機點是否被接受。與原準則不同，為了提高效率，篩選機制不再使用概率P間接判斷解的接受與否，而將趨近近似最優解的過程直接量化在對距離的判斷中，原概率P可由式(2)表示。

(2)

(3)

式中：d為計算兩個節點間歐式距離的函數。通過比例a，完成metropolis接受準則中概率的功能，實現在生長節點離目標區域較遠的時候，改進算法能接受更壞的情況，即距離目標區域比最新生成節點遠得多的情況。隨著生長節點逐漸接近目標區域，接受壞點的范圍逐漸縮小，最終趨近最優解。

通過基于metropolis接受準則的隨機點篩選機制，有效對因時間形成的不同生長可能性區域進行劃分，一定程度上減少隨機搜索樹的重復生長，提高生長效率。

2.3 重要障礙物斥力

目標引力在路徑規劃中的首要任務即尋找目標的過程中加入了導向。但同時，規劃過程中占比更高的避障過程也受此影響而無法使用完全隨機過程進行處理，從而削弱了避障性能。修改的障礙物斥力將被引入以緩解該問題。

圖4對比了僅有目標引力和有較完善的勢場兩種情況下的理論生長過程。在僅有目標引力的情況下，搜索樹生長到節點X3才發現障礙。在經歷了數次失敗或無效的反向生長后，最終才能到達一個較好的節點X2。然而存在一個更好更快的路徑完成這個子任務，即從X1經虛曲線直接到達X2，這就需要一個適當的障礙物斥力機制。

圖4 重要障礙物斥力示意圖Fig.4 Diagram for repulsion of obstacle

傳統人工勢場的斥力場計算因考慮所有障礙導致復雜度激增[15]。針對該問題，劃分障礙物的權重對于引入斥力極為重要。

在單步生長過程中，最近鄰點Xnear到目標點的第一個障礙物是一個典型而優秀的選擇，借鑒人工勢場法的斥力部分，改進算法斥力體系中的斥力系數krep使用式(4)計算：

(4)

式(4)中：C1為障礙物斥力調整系數；l為起始點到目標點的距離；d為障礙物邊緣到最近鄰點的距離；C0根據基礎步長決定，目的是在障礙距離過遠時，精簡計算步驟；C2為可接受的反向生長的比例，建議取值在1.0～1.1。設置斥力最大值可以防止在特殊情況下，擴展出過度反向生長的無效節點。

在使用時，將引力系數與計算得到的斥力系數直接相減，可得到合力系數k*，新節點生成公式為

k*=k-krep

(5)

引入重要障礙物斥力，可以緩解路徑規劃中避障過程因目標引力而產生的效率損失，甚至在避障過程中實現完全隨機。雖然在障礙環境極度簡單的情況下將增加部分時間成本，但在大部分環境中，尋徑效率有較好的提升效果。

3 實驗結果與分析

3.1 實驗環境

假設目標機器人為理想圓點，使用仿真軟件及數據證明改進算法的可行性與搜索效率。實驗使用Windows 10，Intel Core 2.5 GHz，8 G內存，編譯工具為MATLAB R2018a。

隨機地圖仿真實驗中，系統隨機生成規格為[1 000,1 000]的地圖，原點[0,0]為左上角，如圖5所示。黑色圓形為設置的障礙物，圓心及半徑在設定的一定范圍內隨機，障礙數量為100。將[25,25]設置為起始點，目標點為[975,975]。

圖5 隨機地圖Fig.5 Random map

在本組實驗中，設置基本步長ρ=20，引力系數k=2，C0=2，C1=1/8 000，C2=1.01。

在矩形障礙地圖實驗及特殊陷阱地圖實驗中，使用確定的地圖進行實驗，地圖的規格為[500,500]的矩陣，設置[25,25]為起點，右下角[475,475]為目標點。基礎步長設置為5，其余參數相同。

3.2 實驗結果與分析

首先對改進算法的可行性進行實驗，驗證算法是否能在給定環境中找到一條可行路徑。然后在相同實驗條件下對比不同算法的性能，得到實驗數據并分析。

3.2.1 可行性實驗

圖6為一次實驗的結果。兩棵搜索樹分別由左上角的起始點和右下角的目標點出發，圖6中展示了尋徑過程中產生的節點及連線和最終路徑。實驗證明了在設定條件下，本文改進算法可以完成路徑搜索，輸出一條無碰撞的可行路徑。

圖6 算法可行性驗證Fig.6 The verification on the feasibility of algorithm

3.2.2 效率驗證

實驗將比較改進算法、目標偏向RRT、可變步長的雙向RRT算法、人工勢場法和A*算法。其中基礎的A*算法用于獲得最佳路徑。其余不同算法使用相對相似的參數在相同環境和地圖下進行仿真實驗。共進行三組實驗，分別使用隨機生成環境、矩形障礙環境和一個特殊陷阱地圖。設定單次規劃時間超過30 s或連續擴展失敗100次記作失敗。對于存在隨機性的RRT系列算法，每組進行50次尋徑，統計成功尋徑的平均時間、路徑長度以及生成的節點數量。

實驗首先在隨機生成地圖中進行，表1為實驗結果。人工勢場法面對此類環境極易陷入局部最優，無法完成規劃任務。RRT系列算法表現較優。改進算法和變步長雙向RRT路徑規劃成功率均為100%，且在各類指標上均優于目標偏向RRT。其中，改進算法相對變步長雙向RRT在節點數量和平均規劃時間指標上都優化了30%左右。

表1 隨機地圖仿真實驗數據

圖7對比了改進算法與變步長雙向RRT算法的結果，可變雙向RRT在避障過程中，存在不必要的導向性，進行了許多方向明確而重復的生長，降低了規劃效率。而改進算法在面對障礙時隨機性更充分，從而并沒有展現出過強的方向性，避障效率更高。

圖7 隨機環境實驗結果Fig.7 Results of random map experiment

簡單矩形障礙環境的實驗結果在表2中列出，數據表明人工勢場法的路徑因為障礙環境不會突變而更為平滑，但由于計算量大，時間方面并沒有優勢。圖8展示了一次路徑規劃的結果，在該組實驗中，預生長的作用尤為凸顯。由于預生長的作

圖8 矩陣障礙地圖實驗結果Fig.8 Results of rectangle obstacle map experiment

表2 矩形障礙仿真實驗數據

用，實際進行計算規劃的區域不到原計算規模的一半，并且降低了節點數目，提高了效率，相比可變步長的雙向RRT的規劃時間優化了50%左右。

第三組實驗使用如圖9所示地圖，其包含一個典型的陷阱，相比于全局搜索，許多啟發式算法面對該類陷阱的表現都較差。從表3的實驗數據可看出，基礎的人工勢場法無法解決此類路徑規劃問題，而RRT算法由于其概率完備性，存在一定概率完成任務。其中改進算法由于引入障礙斥力和隨機點篩選機制，在陷阱中的表現略優于其余比較算法，在成功率和時間指標上都有所提升，但還未達到解決此類問題的程度。其根本原因是對生長可能性的區域區分還不夠完善，改進算法中的篩選機制只能劃分遠離目標點的已生長的區域，而不能劃分所有已生長區域，從而較好地避免重復生長，仍有提升空間。

表3 特殊陷阱仿真實驗數據

圖9 特殊陷阱地圖Fig.9 Special trap environment

4 結論

針對RRT算法在規劃過程中表現出的部分問題，對可變步長雙向RRT算法進行改進，采用預生長、重要障礙斥力和隨機點篩選機制三種機制，得到以下結論。

(1)預生長機制可以實現對路徑規劃的直觀處理，縮小實際規劃問題規模，提高路徑規劃速度。

(2)引入的障礙物斥力在不明顯提高計算成本的同時，有效解決了目標引力對于避障過程的誤導問題。

(3)隨機點篩選機制有效減少了重復生長的出現，對于結點數量的指標有較大改善，通過提高生長的成功率改善規劃效率。

(4)對于特殊陷阱環境，相比傳統方法，改進算法有一定進步，但還無法完全解決重復生長問題，有待深入研究。