一種雙向目標RRT路徑規劃算法研究

符 強，藍星輝，任風華，伍建輝

(桂林電子科技大學，廣西 桂林 541004)

1 引言

隨著自動駕駛技術的高速發展，無人機和機器人等智能移動設備的應用越來越廣泛，此類智能設備自主探索未知環境的技術正在不斷擴展到越來越多的應用領域。進行自主探索的首要任務就是進行路徑規劃，路徑規劃的目標是在有大量障礙物的復雜地理環境中，以最快的速度規劃出一條連接起點位置和目標點位置的路徑，并且該路徑能夠很好的滿足移動設備的運動學約束，使得移動設備能夠有效的規避障礙物進行自主探索。目前許多學者們熱衷于研究的路徑規劃算法主要A*算法[1]、D*算法[2]、人工勢場法[3]等啟發式搜索算法，此類算法在復雜的環境信息下路徑規劃的效率較高，但是規劃出來的路徑效果并不理想，路徑不能夠很好的滿足設備的運動學約束，不利于智能設備的行駛；蟻群算法[4]、遺傳算法[5]等智能仿生算法，在搜索環境大且復雜的情況下能夠較好的規劃出合適的路徑，但由于需要不斷的迭代計算，時效性不佳；迪杰斯特拉(Dijkstra)[6]算法能夠很好的規劃出一條最短路徑，但搜索效率會隨著地圖環境的增大而有所降低。

快速搜索隨機樹算法(RRT)是一種傳統的路徑規劃算法，它是基于一種樹形結構的典型算法，許多學者將其用于路徑規劃方面。RRT算法具有靈活強大的搜索能力，能夠用于許多復雜大環境下的路徑規劃，但RRT算法也存在一些缺陷，比如采樣效率較低、規劃所得的最終路徑并不是最優、路徑存在很多鋸齒狀的折線路徑等問題。針對RRT算法的不足，不少學者也提出了不同的方法對其進行改進。文獻[7]提出了基于改進RRT算法的三維路徑規劃算法，該算法基于重選原有父節點的方式，使得規劃的路徑能夠有一定的縮短，并且引入歐幾里得最短距離的思想和啟發式搜索函數，提高了算法的效率，但規劃的路徑曲折；文獻[8]提出了RRT-Connect算法，該算法引入自適應步長調整策略，根據障礙物的密集程度動態調整隨機樹的探測速度，從而提高搜索效率。文獻[9]提出了基于雙采樣點的B-RRT路徑規劃算法，該算法改變原有RRT算法單向采樣的策略，采用雙向采樣點的方法提高其搜索速度，并選取各自目標樹的節點作為目標點，提高搜索效率，但規劃的路徑不是最優。文獻[10]提出一種專門針對復雜環境下的IB-RRT算法，該算法引入智能樣本插入啟發式搜索策略提高其搜索速度。文獻[11]提出一種改進的B-RRT算法，該算法采用預生長機制提高路徑規劃的避障效率，但在復雜環境下計算的節點量較大。

上述方法的研究都已經取得一定的成果，但雙向RRT算法存在的采樣點隨機性大、路徑曲折、路徑過長等問題依然存在，基于上述問題，本文針對原有的B-RRT算法進行改進，提出了融合Dijkstra算法的雙向目標RRT算法(goal fusion Dijkstra bi-directional RRT，GDB-RRT)。首先，提出了一種基于目標約束采樣和目標偏置擴展的策略改進原始的雙向RRT算法，提高路徑規劃的效率；其次，融合Dijkstra算法來尋找雙向RRT樹中完整路徑節點之間的最短路徑，由于Dijkstra算法只需要對雙向目標RRT算法搜索得到的整條路徑的節點進行運算，所以能夠極大的減小其運算的節點數量，因此運算的效率有很大的提升；最后采用B樣條算法對最短路徑進行平滑處理，使得曲線光滑連續且無過大折角，以保證智能移動設備能夠按照規劃路徑前行。

2 雙向快速搜索隨機樹算法原理

雙向快速搜索隨機樹(B-RRT)算法的原理是把起始點和目標點均作為隨機樹的根節點，分別從兩個指定的根節點開始，同時產生2棵各自的隨機樹Ta、Tb。兩棵隨機樹分別朝著相向的目標樹節點進行搜索，每輪通過2次隨機采樣，每輪產生2個隨機采樣點，分別進行比較2個候選采樣點與各自目標隨機樹上最近節點的距離，并且判斷新生成的隨機采樣點與最近的節點之間是否存在障礙物，最終選取距離較小的、并且路徑之間無障礙物碰撞的點作為最終可行采樣點，從而在兩棵隨機樹上分別生成新的枝葉。

雙采樣點示意圖如圖1所示，起點和目標點分別生成各自的隨機樹，兩棵隨機樹相向搜索，同時兩棵隨機樹分別生成2個候選采樣點qrand1，qrand2，判斷隨機點和新的節點qnew之間是否有障礙物存在，如果沒有障礙物存在則新的節點添加成功，將qrand更新為qnew，原有的qnew更新為qnear，每次更新之后都會進行一次判斷，判斷一下起點樹的節點坐標是否有等于目標樹的節點坐標，即nodestart=nodegoal，當二者節點坐標存在相等情況說明已經搜索到一條完整的路徑，立即退出當前搜索。此傳統的B-RRT算法相比于原有的單向RRT效率有所提高，但是此搜索策略仍未擺脫原有算法的隨機采樣，即傳統B-RRT算法隨機采樣隨機性太大，不具有目標導向性，存在一些不必要的隨機采樣點，在原有B-RRT算法基礎上還有待改進。

算法的一般步驟如下：

圖1 B-RRT算法原理圖

1)首先通過MapDate()函數來建立環境數據模型，初始化各項參數并啟動秒表計時器；

2)通過while循環來判斷兩棵隨機樹的節點坐標是否存在契合點，若不存在，則進入循環，兩棵隨機樹繼續相向生長，直到搜索到一條完整的路徑，則退出while循環；

3)最后通過ConnectAllPath ()函數獲取整條完整路徑。

原算法主體程序如下：

RRT＿pathplanning()

1) MapDate()

2) Tree(start，goal)

3) While (pathfound())

4) NODESrand ←SampleRand()

5) NODESstart←Extend(Ta，NODESrand)

6) NODESstart←Extend(Tb，NODESrand)

7) Tree←Add＿tree()

8) endlwhile

9) ConnectAllPath()

3 雙向目標RRT算法

3.1 路徑規劃問題的特征描述

在大量的復雜地形環境中進行路徑規劃時，使用原有的B-RRT算法進行搜索時存在采樣點隨機性大、路徑過于冗長、最終規劃所得路徑呈折線折形的問題。由于采樣點的生成隨機性較大，導致隨機樹在搜索空間中有過多不必要的搜索，降低了搜索效率；智能移動設備的能源消耗也是有限的，規劃的路徑應最大程度達到最短，以減少移動設備的能源消耗；規劃的路徑呈現出小范圍內的折角轉彎路徑，根據無人機和機器人等智能移動終端的運動學約束，智能移動設備不能夠很好的按照規劃的路徑前行。基于上述問題以及相關算法的研究，本文提出了一種目標偏置及約束采樣的雙向RRT算法，具體思路為：為提高采樣的有效性，在采樣的過程中以合適的偏置概率來平衡目標采樣和隨機采樣的權重比，從而提高隨機采樣的有效性；為了提高目標采樣的目標導向性，在每生成一個新節點的同時由目標點和采樣點共同決定，從而提高采樣的目標導向性，以提高搜索的效率。

3.2 目標約束采樣

本文為提高隨機采樣點的目標導向性，結合目標偏置和位置約束兩個策略，同時借鑒啟發式搜索[12]和人工勢場法[13]的思想以提高采樣點的目標導向性。在算法中先設置兩個目標偏置概率，其中一個為起點隨機樹向目標點隨機樹生長的目標偏置概率Pstart-bias，另一個為目標點隨機樹向起點隨機樹生長的偏置概率Pgoal-bias。當兩個目標偏置概率設定好后，雙方的隨機樹開始朝著各自的目標樹進行搜索，然后雙方按照均勻概率隨機產生一個起點樹的概率值和目標點樹的概率值分別為Prand-start、Prand-goal。從起點隨機樹端朝著目標點隨機樹端開始進行搜索，若起點樹的隨機概率值Prand-start小于對應的起點隨機樹的目標偏置概率值Pstart-goal，則將對方目標點隨機樹的節點坐標Nodesgoal作為隨機采樣點Nodesrand。如果條件不滿足，就在自由空間中隨機產生一個采樣點，起點樹的目標偏置采樣公式如式(1)所示

(1)

從目標點隨機樹端朝著起點隨機樹端開始搜索，若該目標點隨機樹的隨機概率Prand-goal小于對應的目標點隨機樹的偏置概率Pgoal-bias，則將起始點隨機樹的節點坐標Nodesstart作為采樣點Nodesrand。如果條件不滿足，就在自由空間中隨機產生一個采樣點。目標點樹的目標偏置采樣公式如式(2)所示

(2)

兩式中RandSimple()為隨機采樣函數。

采樣點的生成是隨機的，在進行路徑搜索的過程中需要對生成的隨機點進行篩選，即需對生成的隨機采樣點進行位置約束，去掉一些不滿足條件的隨機點。約束思想是每隨機生成一組隨機采樣點，即對其各隨機點的位置進行判斷。判斷該采樣點在x方向或者在y方向上比前一個采樣點更加接近目標樹的節點，若滿足上述條件則選取該組采樣點中最靠近目標樹的節點作為新節點，否則繼續循環采樣，直到滿足上述條件為止。將兩棵隨機樹的起點分別設置為第一個參考采樣點，之后不斷更新，約束采樣的示意圖如圖2所示。

圖2 采樣約束示意圖

3.3 目標偏置擴展

本文為使得隨機點不斷朝著目標樹節點的方向擴展，借鑒粒子群算法[14]的思想，通過給采樣點方向和目標樹節點方向分配不同的權重，在采樣點和目標樹節點之間生成一個新的節點，該節點按照一定的權重偏向于目標點，使得新點的擴展方向不再單純由隨機采樣點決定，而是由采樣點和各自目標樹的節點來共同決定。通過設置合適的權重就能夠使得新的隨機點不斷朝著目標點的方向擴展，使得每一次擴展都能夠有效的接近目標點，從而加快路徑的搜索速度，目標偏置擴展的示意圖如圖3所示。

新節點偏向目標的生成公式為：

圖3 目標偏置擴展示意圖

Nodesnew=Nodesnearest+xstep·vj·Ngoal+(1-vj)·Nrand

(3)

式中：xstep為擴展步長，vj為目標點方向權重，取值范圍為[0，1]；Ngoal為目標點方向單位矢量，Nrand為采樣點方向單位矢量。

當目標點方向權重為1時，采樣點徑直的朝著目標隨機樹的方向擴散，當目標點方向權重為0時，采樣點不具有目標導向性。

4 Dijkstra策略下雙向目標RRT

雙向目標RRT算法結合Dijkstra策略是在雙向目標RRT算法搜索到完整路徑后進行最短路徑優化的一個處理過程。在雙向目標RRT搜索完成后，將多余的不必要枝葉節點進行修剪，保留整條完整的主干路徑節點，通過Dijkstra算法對整條完整路徑中Nodes節點坐標進行迭代運算找到一條最短路徑。

4.1 Dijkstra算法原理

Dijkstra算法是經典的最短路徑搜索算法。它的主要的思路是，從起點坐標開始通過不同的路徑逐漸地向外層擴展搜索，搜索到終點坐標則停止搜索，最后得到一條最短路徑。Dijkstra算法求解最短路徑問題的基本步驟如下：

1)首先設立一個節點數組PathFound，通過該數組保存所有待訪問的節點坐標，其次設立另一個節點數組PathThrough記錄已經訪問過的所有節點坐標的集合，數組PathFound初始化為只有起始點節點坐標，數組PathThrough初始化為空集合。

2)從節點數組PathFound中的起點坐標開始，尋找出距離起始節點最近的節點坐標，將尋找到的節點坐標放入數組PathThrough中進行存儲，兩節點之間的距離求解公式為：

(4)

3)將起點到所有可行路徑的節點距離進行迭代運算，尋找出起點到路徑中各個節點的最短路徑，這一步操作稱為松弛操作，同時把這些最短路徑的相鄰節點坐標加入集合PathFound中等待下一次的運算。

4)判斷節點數組PathThrough中的所有節點坐標是否已經全部訪問，如果沒有則返回步驟2)直到PathThrough數組中的點坐標均已經訪問完成，此時，節點數組PathThrough中包含了網絡中從起始節點出發可以到達的所有節點，通過上一個步驟的松弛操作可以最終尋得一條最短路徑。

4.2 松弛操作

松弛操作屬于一種遍歷的方法，其主要思想是在所有的可行的節點空間樹中，從起點出發探索整個空間樹的節點，比較相鄰節點的路徑消耗來確定其子節點，如此不斷探索下去，直到找到最終節點。具體算法的實現流程如下圖所示。

圖4 松弛操作流程圖

圖5中的模型表示的是從起點到路徑中其余各個可行節點的最短路徑，起始節點為1號點，目標節點為6號點，現在需要求的是從起始節點到目標節點的最短路徑；首先將模型中的頂點和邊長的數值在二維數組N中進行存儲，初始值如表1所示。

圖5 Dijkstra算法路徑搜索模型

表1 二維數組N存儲的節點之間的距離關系

還需要用一個一維數組dis來存儲1號頂點到其余各個頂點的初始路程，可以稱dis數組為“距離表”，如下：

123456dis0112∞∞∞

此時的dis數組中的值稱為最短路徑的估計值。

要得到從隨機樹1號起點到隨機樹其余各可行路徑節點的最短路程，就得通過松弛得方式進行遍歷計算，通過遍歷計算，最終得到一條到達終點的最短路徑，下面就從1號點開始模擬一下松弛的過程：首先從根節點開始。找到一個距離1號起點最近的可行節點，通過觀察數組dis可知當前離隨機樹根節點的1號節點最近的是2號節點，當選擇了2號節點后，dis[2]的距離值就已經從估計值變為了一個確定值，即隨機樹起點1號節點到2號節點的最短路程就是當前的dis[2]值。當2號點已經確定，接下來尋找2號頂點的可行的路徑，從模型圖中可以看出有2→3和2→4這兩可行路徑。首先討論通過2→3這條可行路徑，其路徑長度為N[2][3]=9，然而到隨機樹節點3的路徑還有1→3這可行路徑，經運算其路徑長度dis[3]=12，接下來比對比兩種行走方式那條路徑更短，通過對比發現dis[3]>dis[2]+N[2][3]，因此將dis[3]更新為10，并確定其路徑為1→2→3這樣的一個過程稱之為松弛。類似的討論2→4可知dis[4]>dis[2]+N[2][4]，因此dis[4]更新為4。以上就是對2號頂點所有的可行路徑進行了松弛，松弛完畢之后的dis數組為

123456dis0110 4∞∞

完整的松弛過程如下：

123456dis0112∞ ∞∞↓123456dis0110 4∞∞↓123456dis01 8 41719↓123456dis01 8 41319↓123456dis01 8 41317↓123456dis01 8 41317

本文為獲得最短路徑，充分利用Dijkstra算法搜索最短路徑的優勢，將雙向目標RRT算法搜索得到的完整路徑的節點進行多路徑連接操作，即將相間隔的可行節點進行連接，并且連接路徑中無障礙物產生碰撞，由此產生多路徑通道。當多路徑通道模型建立，采用 Dijkstra算法進行松弛運算，得到一條最短路徑。

5 路徑平滑處理

1946年Schoenberg首次提出樣條理論，1972年Gordon等從外形設計的需求出發，對Bezier曲線進行發展，提出B樣條曲線[15]。B樣條曲線是一種變化靈活的曲線，曲線的局部形狀受相應頂點的控制，如果在曲線的轉折點一定范圍內選擇合適的控制頂點，可以在該段折線范圍內得到滿足一定要求的光滑曲線，其數學表達式為

(5)

式中，0≤t≤1；i=1，2，…，m，分別表示第i段B樣條曲線；n表示樣條曲線為n次參數曲線；Pi+k表示第i段B樣條曲線的第k個控制點，Fk，n為n次B樣條的基函數，其表達式為

(6)

式中，0≤μ≤1，k=0，1，2…，n。

由式(7)可知，當曲線的階次確定下來后，在本文中的階次采用3階曲線的基函數也隨之得到確定，可通過改變曲線路徑中轉折點附近控制點的選取來控制曲線的形狀。因此采用B樣條理論通過改變折線路徑中轉折點附近的采樣點的選取，進而通過這些控制點將轉折點兩端的線段進行平滑處理，最終得到平滑的可飛行路徑。

為驗證路徑平滑后的效果，選取經過最短路徑優化后的路徑作比較，其對比結果如圖6所，圖中藍色點為起點，紅色點為終點，紅色的線條為經過Dijkstra算法優化后的最短路徑，綠色線條為經過3次B樣條優化后的路徑。從圖中可以看出，最短路徑在經過3次B樣條處理后路徑變得光滑，折角被平滑處理，整體路徑的走勢沒有被改變。

GDB-RRT算法的詳細流程圖如圖7所示。雙向目標RRT算法能夠提高隨機采樣點的目標導向性，擴展的路徑也朝著目標點進行生長，加快搜索的速度，提高其搜索效率。融合Dijkstra算法縮短路徑的長度，通過樣條曲線理論改善了路徑曲折的問題，最終保證路徑的可行性。GDB-RRT算法的完整代碼結構主要包括：目標約束采樣函數、目標偏置擴展函數、路徑縮短函數、平滑路徑函數四部分。

圖6 路徑平滑前后對比

圖7 GDB-RRT算法整體實現流程圖

6 實驗驗證

為驗證GDB-RRT算法的效果，本文針對傳統B-RRT算法和GDB-RRT算法進行仿真分析。選取同一尺寸不同復雜環境下的實驗結果作對比。仿真平臺及配置為：matlab R2020a，64位Windows10操作系統，處理Intel(R) Core(TM) i5-9500，CPU主頻3GHz，內存8GB。

6.1 仿真研究

為了驗證本文所提出的GDD-RRT算法具有更好的性能，將B-RRT和GDB-RRT算法在3種二維仿真環境下進行仿真對比實驗，環境地圖尺寸大小均為[500，500]。仿真中的所有起點均標注為紅色點，坐標為(10，490)，終點均標注為藍色點，坐標為(490，10)。由于RRT算法類均具有隨機性，因此在每個環境中，對B-RRT和GDB-RRT算法分別執行100次重復實驗，最后給出平滑后的可飛行路徑，并統計

圖8 B-RRT和雙向目標RRT搜索結果對比

各個指標的平均值。圖8中的(a)圖(b)圖分別為B-RRT和雙向目標RRT的搜索結果對比，圖9為雙向目標RRT路徑經過最短路徑優化前后的對比。圖10～12為不同環境下B-RRT算法和GDB-RRT算法路徑規劃效果對比。

圖8中(a)圖為原始B-RRT搜索過程，(b)圖為雙向目標RRT搜索過程，圖中從藍色起點出發的線條是起點隨機樹，從紅色起點出發的線條是目標隨機樹，從兩圖的對比中可以明顯看出，由于原始B-RRT采樣的隨機性，隨機樹在沒有障礙物的空間隨機擴展，生長較為發散。由于雙向目標RRT算法引入了約束擴展和目標偏置策略，使得隨機樹不再單純的朝著無碰撞空間進行擴展，而是朝著目標樹有方向性的生長，能夠快速的通過狹窄區域進行搜索，減少其在不必要的空間中進行探索，相比于原有B-RRT算法，雙向目標RRT算法采樣的效率有所提升，搜索的時間也變得更短。圖9中的藍色線條為雙向目標RRT搜索得到的路徑，紅色線條為經過Dijkstra算法優化后得到的最短路徑。從圖中可以看出經過優化后的路徑明顯變短。

圖9 最短路徑優化前后對比

三種不同環境下的路徑規劃結果如圖10～12所示，圖中，藍色實線代表初始搜索路徑，紅色實線代表簡化縮短的路徑，綠色實線代表經過平滑處理后的最優路徑。圖10的環境1中為存在大量不規則隨機障礙物的環境，圖11中的環境2為狹窄通道較多的復雜環境，圖12中的環境3為非常具有挑戰性的單通道迷宮環境。

綜合以上三種復雜環境的規劃結果可知：相比于原有的B-RRT算法，GDB-RRT算法的路徑搜索得到的最優路徑更短且平滑，此算法搜索得到的最優路徑更加符合智能移動設備的運動平穩性要求，且為設備的運動節省更多的能源消耗。

圖10 環境1規劃結果

6.2 占用內存分析

對于B-RRT而言，有效節點數越多，隨機節點數越少，節點的利用率就越高，算法消耗的內存就越少，因此以路徑搜索后的節點利用率來衡量算法所占用內存的多少。2種算法在不同環境下路徑搜索的節點平均使用情況如表2所示。

表2 不同環境下路徑搜索的節點使用情況

圖11 環境2規劃結果

圖12 環境3規劃結果

由表2可知：三種環境下B-RRT算法的平均擴展節點有259個，GDB-RRT算法的平均擴展節點有137個，相比之下減少了47%的擴展節點。B-RRT算法的平均有效節點有27個，平均有效節點的利用率為10%，GDB-RRT算法的平均有效節點有32個，平均有效節點利用率為20%。綜上數據，GDB-RRT的節點利用率是B-RRT節點利用率的2倍左右，節點利用率有所提高，占用的內存減少近一半。

6.3 路徑規劃效率分析

一般而言，算法運行的時間越短，那么路徑規劃的效率也就越高，因此通過算法的運行時間來衡量規劃效率的高低。3種環境下算法運行的平均時間如表3所示。

表3 不同環境下算法運行時間

由表3可知：B-RRT算法在三種環境下的平均運行時間為3.672s，而GDB-RRT算法的平均運行時間為2.461s，平均運行時間縮短了33%。因此，相比B-RRT算法，GDB-RRT算法的規劃效率更高，能夠更快的搜索出完整的優化路徑。

6.4 路徑優化效果分析

在路徑規劃中，規劃所得的路徑長度也是用來衡量算法對路徑優化效果的一個重要指標，其次路徑平滑的程度也能夠作為一項評價指標，由于平滑程度無法量化，所以只對路徑長度進行量化分析，2種算法在不同環境下所規劃路徑的平均長度對比如表4所示。

表4 不同環境下算法規劃的路徑長度

由表4可知：B-RRT算法在3種環境下所得路徑的平均長度為1066m，而GDB-RRT算法所得路徑的平均長度為981m，相比較之下GDB-RRT的算法比原算法規劃的路徑縮短了85m，平均縮短7.9%。同時，結合圖7～9可知：GDB-RRT算法得到的路徑總體上比B-RRT更加平滑。綜上數據對比可知GDB-RRT算法的路徑規劃效率有所提高，最終得到的路徑更短而且更加平滑。

7 結束語

本文提出的GDB-RRT算法的路徑規劃算法在以下三方面有所提高：首先采用目標采樣和目標偏置策略使得采樣點具有目標導向性，提高采樣點的利用率，使得搜索效率有所提高；其次融合Dijkstra算法使得規劃的路徑變得更短，減少移動設備的能源消耗；最后采用樣條優化策略對算法得到的最短路徑進行優化，使得路徑變得更加平滑。

通過在三種不同環境下與B-RRT算法的對比仿真，結果表明GDB-RRT算法在節點利用率方面提高了1倍，擴展節點減少了49%，內存占用更少，搜索時間縮短33%，路徑縮短7.9%，最終的路徑也更加平滑，優化效果更好。證明本文所提算法具有一定的優越性和有效性。