具有目標導向性的RRT路徑規劃研究

郭孝坤 吳玉秀 李景 程瑞嘉

摘? 要：在機器人全局路徑規劃中，首先對全局路徑規劃的各種探索路徑的算法做了總結，對RRT、A*和D*算法做了對比，分析了這些算法的優缺點。鑒于RRT具有概率完備性，A*和D*具有深度優先性，也就是目標導向性，文章使用一種算法更改了原RRT算法中隨機點的選取方式，使RRT具有概率完備性的同時也擁有了目標導向性，實驗證明，改進后的RRT算法能夠更快探索出到達目標點的路徑。

關鍵詞：全局路徑規劃;RRT;A*;深度優先性;概率完備性

中國分類號：TP242? ? ? ? ? ?文獻標識碼：A文章編號：2096-4706（2022）01-0069-05

Abstract： In the global path planning of robots， we first summarize the various path exploration algorithms of the global path planning， compare the RRT， A* and D* algorithms， and analyze their advantages and disadvantages. Given that RRT has probability completeness， A* and D* has depth priority， that is target orientation， this paper uses an algorithm to change the selection method of random points in the original RRT algorithm and make RRT have probability completeness and target orientation at the same time. The experiment proves that improved RRT algorithm can faster explore the path to reach the target point.

Keywords： global path planning; RRT; A*; depth priority; probability completeness

0? 引? 言

機器人應用中，路徑規劃是必須且首先需要的核心技術[1]，很多后續任務也都是從路徑規劃開始。目前路徑規劃可分為局部路徑規劃和全局路徑規劃，本文主要研究了全局路徑規劃下的路徑探索問題。全局路徑規劃意思是機器人已知全局環境信息，通過拓撲法、可視圖法、柵格法將全局信息轉化為機器人易處理的信息，并在此基礎上做路徑探索。常見的路徑探索方法主要有A*、D*、RRT法等。

A*算法在保證避障的前提下總是期望下一步盡可能接近目標點[2]，這種路徑規劃方式有著深度優先的特點，他能快速接近目標點。但是缺點也很明顯，它容易陷入局部最優，如遇上凹型障礙物規劃的路徑容易一直轉圈。D*算法[3]類似A*，不同的是D*算法從目標點開始向起始點搜索，在動態環境中有不俗的表現，他可以在環境發生變化時不斷遍歷節點從而找出新的最短路徑，但是這種算法是以犧牲效率為代價的。RRT算法的核心思路是以起始點開始不斷在地圖上擴展探尋，直至找到目標點為止，其作出的探索路徑像是一顆不斷生長的樹[4]。RRT是一種具有概率完備性的路徑探索算法，也就是RRT算法理論上一定可以找到通往目標點的路徑，并且依靠隨機性，RRT算法搜索目標點的速度要遠遠高于完整搜索算法，且不會陷入局部最優。RRT算法缺點是搜索全局路徑效率沒有A*和D*高，并且搜索出來的路徑也不是最優最短。

本文基于上述全局路徑規劃方法，分析了各種路徑規劃算法的優缺點，最終研究并改進了RRT算法，使得RRT算法在擁有概率完備性的同時又能擁有A*和D*深度優先的優點。

1? 傳統RRT算法概述

RRT全稱快速擴展隨機樹，其算法的核心理論是以機器人初始起點作為樹的根節點，在向著地圖的任意方向隨機采樣，以采樣點連接樹中已存在且最靠近的節點向著采樣點延伸新節點的方式生長樹的新節點，當樹的新節點成功探尋到目標點的時候停止。從該新節點開始找他的父節點，直到找到根節點。這樣就找到一條通往目標點的可行路徑[5]。

1.1? 傳統RRT算法步驟

對地圖進行SLAM建模，不考慮智能體所處環境出現上坡下坡等特殊環境，假定處于二維平面工作空間W（WorkSapce）中，W∈R2。對傳統RRT路徑算法步驟描述如下：將智能體的起點xinit作為隨機樹的根節點;在Wfree中隨機選擇一個坐標記做xrand;使用Nearest_Neihbor（）函數尋找隨機樹上所有節點，距離xrand最近的節點向著xrand的方向生長出新的節點，記做xnear;反復調用EXTEND（）函數使xnear盡可能接近xrand，直到xnear完成到達xrand點、靠近障礙物、到達限制長度和接近目標點四個目標中的一個為止，將這個xnear添加到隨機樹中，此時隨機樹成功產生一個新的節點xnew。重復選取xrand，重復添加新的xnear，直到上述算法成功找到使xnew到達目標點區域范圍內，程序結束，具體算法描述為：

Function：RRT（n∈Wfree，Δt∈R）

1：T.init（xinit）

2：for a=0; a<n;a++

3：xran←RANDOM_NODE（Wfree）

4：EXTEND（T， xrand）

5：end for

6：return T

Function：EXTEND（T， xrand）

1：xnear←Nearet_Neighbor （T， xrand）

2：u←Select_Input （xrand， xnew）

3：xnew←New_State（xnear， u， Δt）

4：if NoCollision Path（xnear， xnew） then

5：T. Add_Node（xnew）

6：T.Add_Edge（xnear， xnew， u）

7：end if

8：return T

1.2? 傳統RRT算法圖解

RRT大致過程如圖1所示，圖中綠色光點代表起始點，黃色光點代表終點，經過一段時間的搜尋，RRT算法最終找到了目標點，圖中粉紅色線段代表RRT算法找出的路徑?？梢钥闯鰝鹘yRRT路徑在地圖的每一個角落都進行了搜索，他探索的路徑不一定是最優的，但一定可以找到目標點。

2? 改進的RRT算法

2.1? 改進的RRT算法介紹

通過圖1可以看出傳統RRT算法雖然通過概率完備性成功在地圖上找到了通往目標點的路徑，但是RRT做了很多無用功，有些不必要搜索的地方RRT也做了探尋。如果給RRT算法提供一個朝向目標的大體方向，使RRT向著目標點的方向探尋，那么探尋目標點的效率就會提高很多。因此本文對傳統RRT算法做了改進，使之添加了一種目標導向性的特性。

根據上述算法描述，RRT算法每次下一節點xnear的確定都是從最靠近xrand的樹節點向xrand方向延伸。而xrand是通過概率函數隨機在地圖上撒點得到，所以改進RRT算法的核心就是控制隨機點xrand在地圖上的落點。如果能設計一種機制，使得xrand的隨機性可控，即期望它下一次落點能夠幫助RRT算法更快朝著目標探索，那么就可以有效提高RRT算法的效率。當然，這種控制xrand隨機落點的行為會導致RRT算法丟失隨機性的特點，所以改進機制需要在幫助RRT提高探索效率的同時，還要盡可能保留其隨機性的特點，也就是改進RRT算法可以選擇犧牲少部分隨機性特點，來換取其具有一部分目標導向性的特點。

改進RRT的地圖探索機制如圖2所示，綠色五角星表示RRT節點樹中距離目標點最近的節點，紅色為目標點。以兩點為中線（θ1=θ2=45°），按圖示意將地圖分為OAB、OBC、OCD、ODA四個區域，其中，∠AOB=∠BOC=∠COD=∠DOA=π/2。OAB為目標域，OBC和ODA為鄰域，OCD為目標反向域。

設總概率不變，改進后的RRT算法將通過概率函數增大在目標區域選擇xrand坐標的概率，減小在目標反向域選擇坐標的概率，而xrand在兩個鄰域落點的概率保持不變。這樣改進后的RRT算法，犧牲了一定的隨機性，添加了一定的目標導向性，即樹的生長被指定了大致的方向。

2.2? 改進RRT的xrand節點的選取

改進的RRT算法與未改進的RRT算法比較，其他算法步驟不變，只更改隨機點xrand選取的方法[6]。在原始RRT算法中，xrand的選取機制是以地圖的長和寬為基礎各乘以一個隨機的百分比得到地圖上的隨機點，改進后的RRT算法在為xrand選取隨機點始終是以O點建立的極坐標選取的，原始的方法并不適用。因此，改進的方法是先以O點建立一個極坐標系并求出角度和距離，隨后將極坐標值轉換成原全局地圖上的坐標值，隨后再加上O點的坐標值即可表示出xrand在全局地圖下的坐標點。

首先是xrand角度的選取，角度選取機制如下：

（1）遍歷當前已存在的樹的節點，找出距離目標點最近的節點，記為O點;

（2）O點水平向右延伸，至地圖邊緣形成OE線段，以OE線段為極軸建立極坐標系;

（3）根據圖2展示的改進RRT探索機制，利用概率函數分配xrand在對應區域出現的概率;

（4）概率函數執行，得到xrand的角度值。

在選取了角度之后需要對xrand節點選取長度。為了讓xrand能夠覆蓋地圖所有區域，需要對不同角度xrand長度選取做計算，xrand的長度根據可表示為：

式中rand（）為隨機百分比函數，OVn他表示了O點到四個頂點其中一個的距離，若長度超出地圖邊界時，算法重新執行式（1），直至長度不會超出地圖。OVn為xrand取不同角度時應取得的最長模長，如圖3所示改進RRT中xrand長度選取機制，圖中OE、OF、OG和OH將地圖分為四個部分，每個部分最長的模長即為O點至其斜邊的距離。設式（1）中xrand取得的角度為θ，若θ∈[0，π/2]，則n為2;θ∈[π/2，π]，n為3;θ∈[π，3/2π]，n為4;θ∈[3/2π，2π]，n為1。

在xrand以OE為極坐標系時的角度和距離都確定情況下，設O點在地圖上的坐標為（xo，yo），則xrand在地圖上的坐標可表示為：

式（2）表示在全局地圖坐標下xrand的坐標，改進RRT算法相對于原始RRT算法需要對表1算法第三點xrand←RANDOM_NODE（Wfree）修改為本節所描述算法，其他步驟不變。

3? 實驗仿真

在一個20×20的方形地圖內，左上角坐標為（0，0），右下角坐標為（20，20），地圖中有障礙物若干，其中還包含一個凹型障礙物陷阱。RRT算法探索的起始點是（5，0），在地圖中以綠色光點標明，目標點是（5，20），在地圖中以黃色光點標明。在matlab上基于上述地圖對原始RRT算法和改進后RRT算法進行實驗仿真。

如圖4所示進行了九次原始RRT算法仿真，如圖5所示進行了九次改進后RRT算法仿真。表2為前后兩次仿真實驗相關數據。

從圖4和圖5可以看出，圖4中的實驗每次總是試圖探尋整個地圖以發掘找到目標點的路徑，圖5中的實驗明顯減少了對不必要搜索區域的探索。但是這種對比不是絕對的，原始RRT算法也可能比改進后RRT更先更快探索到目標點，如圖4（a）對比圖5（a），圖4（g）對比圖5（g），但是大概率對比，改進后的RRT是優于原始RRT算法的，如其余七幅圖的對比。

如表2所示，原始RRT算法平均每次的總時長為5.27 s，平均每次步數為259步，改進后RRT算法平均每次總時長為3.34 s，平均每次為步數189步。表2數據表明改進后的RRT算法在平均步數和平均用時上大大少于原始RRT算法，并且他繼承了原始RRT算法隨機性的同時也吸收了A*和D*算法深度優先性的特點。

4? 結? 論

路徑規劃是機器人應用中非常重要的一環，本文對全局路徑規劃中的幾種常見的路徑探索算法的優缺點進行了歸納總結，最后用RRT算法結合A*和D*算法的優點進行了改進，讓RRT算法既有隨機性又有目標導向性，實驗證明這種改進方式能利用隨機性幫助智能體逃離局部最優的同時，也能更快速探索出通往目標點的路徑。

改進后的RRT算法依然存在不足：

其一，對于目標區域、鄰域和目標反向域的xrand落點概率值并沒有嚴格的算法支持，在本文中依靠的是多次試驗獲得的相對較優的經驗值，后續的改進中可以利用自適應等算法鎖定最優概率值。

其二，如圖5改進的RRT部分實驗展示（圖5（d），圖5（g），圖5（h），圖5（i）），在犧牲了一部分的隨機性之后，這可能會導致算法在大概率情況下會嘗試目標方向的路徑探索，即使目標方向會有障礙，這將導致一些不必要的探索出現，此時路徑探索會變得集中，后續的改進中仍需要減少這種不必要的路徑探索。

其三，改進后RRT算法中xrand的最長模長選取機制不是很合理，有時隨機長度可能會超出地圖邊緣，這時xrand的長度需要重新選擇，這使得改進后RRT算法的總用時可能會加長，后續改進中可以選擇新的模長選取機制，以減少總時長。

參考文獻：

[1] 張捍東，鄭睿，岑豫皖.移動機器人路徑規劃技術的現狀與展望 [J].系統仿真學報，2005（2）：439-443.

[2] 張海濤，程蔭杭.基于A*算法的全局路徑搜索 [J].微計算機信息，2007（17）：238-239+308.

[3] 谷潤平，崔朋，唐建勛，等.基于D*算法的場面滑行動態規劃研究 [J].科學技術與工程，2015，15（1）：315-319+328.

[4] DU M B，MEI T，CHEN J J，et al.RRT-based Motion Planning Algorithm for Intelligent Vehicle in Complex Environments [J].Robot，2015，37（4）：443-450.

[5] 張捍東，陳陽，吳玉秀.未知環境下移動機器人實時路徑規劃 [J].計算機工程與應用，2018，54（19）：140-146.

[6] 劉永紅，劉明雍，謝波.航位推算組合導航系統在線標定技術 [J].中國慣性技術學報，2015，23（4）：434-437.

作者簡介：郭孝坤（1997—），男，安徽馬鞍山人，碩士在讀，研究方向：路徑規劃;吳玉秀（1982—），男，漢族，河南安陽人，講師，博士，研究方向：機器視覺與圖像檢測;李景（1997—），男，安徽六安人，碩士在讀，研究方向：雙目視覺;程瑞嘉（1997—），男，山東濱州人，碩士在讀，研究方向：深度學習。