融合有向D*與RRT *的移動機器人路徑規劃算法

劉逸凡，黃友銳，韓 濤

(安徽理工大學電氣與信息工程學院，安徽 淮南 232001)

1 引言

近年來，移動機器人在自動巡檢和物流運輸領域的作用越發重要，是當前的一個研究熱點。隨著應用場景的不斷增多，對機器人路徑規劃能力的需求也在不斷提高[1]-[3]。

目前，路徑規劃算法主要分為兩種，基于圖的方法和基于采樣的方法。而基于采樣的路徑規劃算法中，應用范圍最廣的是快速隨機樹[4]-[6](Rapidly-exploring Random Tree， RRT)法，它是使用隨機采樣來避免構建狀態空間。

文獻[4]提出了RRT算法，它以起點為根進行生長，當樹枝生長到目標點后，形成最終路徑。文獻[7]提出RRT-Connect算法，以雙樹形式形成路徑。RRT算法對于解決單查詢規劃問題有著特有的優勢，特別是，需要避過大量障礙物后才能到達目標地時。但由于沒有考慮最優性問題，所以只能產生可行路徑，卻無法得到漸進最優路徑。

文獻[8]提出的RRT*算法，它以RRT方法找到第一個可行路徑之后，會通過重新布線來優化樹的結構，來返回一個更優的路徑。文獻[9]將RRT-Connect與RRT*相結合，提出了RRT*-Connect算法，它是能返回漸進最優解的雙樹算法。算法會在整個狀態空間持續采樣，以便返返回一個漸進最優解，但是在整個狀態空間隨機采樣，依然不能有效的降低尋找最優路徑的成本。文獻[10]提出了Informed-RRT*算法，該算法在找到第一條可行路徑之后，在橢圓集中進行采樣來優化路徑。文獻[11]提出了Batch Informed Trees (BIT*)算法，通過采樣和試探法交替進行來解決路徑規劃問題。文獻[12]提出了改進RRT*FN算法，使用啟發式采樣方法，保留樹中的高性能節點，提高了算法性能。但是這些算法在通過狹窄通道或通過連續的小洞時，仍需要大量迭代才能到達目標點。而且上述算法，只能在固定的半徑范圍內重新布線。當重新布線半徑較小時，生成的路徑中會存在大量的冗余拐點，無法滿足機器人或智能車輛的運動需求；當重布線的半徑較大時，采樣點的替代父節點過多，會嚴重拖累運算速度。

針對上述問題，本文提出了融合有向D*[13]與RRT*的路徑規劃算法。本文將RRT*算法的重現布線部分進行優化，在Informed橢圓集的基礎上，根據有向D*的關鍵點思想提出了一種關鍵點采樣集合的方法。在融合改進算法找到第一條可行路徑之后，通過判斷可行路徑與障礙物頂點之間的距離，將部分障礙物頂點作為關鍵點，并以關鍵點為圓心，建立圓形子集。把重現布線的采樣點按概率分配在圓形子集和整個狀態空間中，優化轉彎處的路徑。再引入變距離重新布線方法，經過少量的大半徑重新布線后減少冗余節點，然后利用小半徑重新布線多次優化轉彎處的路徑，在不影響運算速度的前提下，得到一條更為平滑的漸進最優路徑。本文算法在路徑節點數量、路徑長度等方面的有效性。

2 相關工作

2.1 Informed-RRT*算法

RRT*算法是通過在自由空間中隨機采樣，構成一個從起點xstart開始尋找可行路徑的過程。在迭代過程中，以固定步長選取采樣點方向上的新節點，直到到達目標點xgoal。

Informed-RRT*開始時與RRT*算法一致，但是在找到首條可行路徑之后，會在橢球子集上繼續采樣，根據新添加到樹上的狀態進行重新布線。新添加的狀態將樹中附近現有節點視為替代父級，將代價最小的節點作為新的父節點，在樹上形成新的樹枝。

2.2 有向D*算法

有向D*算法采用反向搜索方式。從終點開始，對于關鍵節點采用由父節點逐級確定子節點的方式，再通過方向函數將兩個節點連接成直線，判斷直線與障礙物是否發生碰撞，決定該節點的去留，實現對關鍵節點的篩選。有向D*算法使用關鍵點搜索替代了傳統D*算法遍歷柵格狀態，形成從起點指向終點的指針的搜索方式。

如圖1，是有向D*算法的搜索方式，圖中點S(黑色)、G(紅色)分別是起點和終點，10個紅色菱形是在障礙物頂點中選取的關鍵點。有向D*算法，對于存在障礙物的地圖進行最優路徑搜索時，只需要考慮對10個關鍵點進行訪問，有效提高搜索效率。

圖1 有向D*算法搜索方式

3 融合有向D*與RRT*的路徑規劃算法

3.1 關鍵點的選擇與概率采樣

融合改進算法在搜索路徑之前，首先將障礙物膨化γ單位長度，防止生成的路徑與障礙物之間空隙過小，導致機器人無法順利通過。然后使用與Informed-RRT*相同的方式，在無障礙空間內進行隨機采樣，以搜索節點。

在形成初始路徑之后，結合有向D*算法的思想確定采樣空間。將障礙物的部分頂點確認為關鍵點，再根據關鍵點確定合適的采樣空間。算法從終點開始對所形成的初始路徑集合S={S1，S2，…，Sn-1，Sn}進行直線化處理，其中S1為起點Xstart，Sn為起點Xgoal。首先以S1為父節點，Sn-1為子節點，兩點相連，判斷SnSn-1是否與障礙物發生碰撞；若沒有碰撞，則將Sn-2設為子節點，再次判斷SnSn-2是否與障礙物碰撞；進行順序比較，直到SnSk與障礙物發生碰撞，此時把父節點Sn的位置信息存入OPEN列表中，再把Sk設為新的父節點，Sk-1設為子節點，判斷SkSk-1是否與障礙物發生碰撞，按順序將發生碰撞的父節點存入OPEN列表中，直到將S1選為子節點，停止判斷，將S1存入OPEN列表。將OPEN列表中的所有節點依次連接，形成優化后的最初路徑。若障礙物頂點與優化路徑距離小于3個單位長度，就將其選擇關鍵點并形成關鍵點集合X={X1，X2，…，Xn}。

最后以關鍵點X為圓形，δ為半徑，形成數個圓形子集作為采樣空間。每次采樣情況可用式(1)表示，其中s為調用函數生成的浮點數，sr為目標采樣概率，sample_c和sample_o分別表示算法在圓形采樣空間內采樣和在整個無障礙空間內采樣。

(1)

本文算法的圓形采樣子集與優化后初始路徑如圖2所示。

圖2 圓形采樣子集示意圖

圖中，Xstart和Xgoal分別表示起點和終點。因為黑色障礙物頂點v與初始路徑之間的距離小于D，所以將膨脹處理后的新障礙物中對應的頂點v’設為圓心，δ為半徑形成圓形采樣子集(藍色虛線)。

3.2 變距離重新布線

采樣后產生一個隨機點Xrand，以Xrand為圓形，r為半徑，在樹上搜索節點，將搜索到的相鄰節點形成集合Xnear。cost()是指從樹的起始節點到本節點的路徑代價。cost(L)是指從相鄰節點到到新產生的節點Xrand的路徑代價。算法沒有將距離采樣點最近的節點作為父節點，而是選擇到采樣點Xrand路徑代價最小的節點，判斷公式如下：

(2)

將每個相鄰節點進行比較，選擇路徑長度最優的父節點。如果在無碰撞情況下，存在路徑代價更小的節點，就將其定義為Xrand新的父節點，當在搜索范圍內得到路徑代價最小的父節點時，便將這條路徑作為新生成的樹枝添加到樹中。

在進行半徑為r的搜索時，r為一個變量，它會隨迭代次數的增大而不斷減小，表示為

(3)

式中，n為當前的迭代次數，a是根據地圖大小而確定的常量。

3.3 算法實現流程

算法流程圖如圖3所示。

圖3 本文算法流程圖

融合改進算法實現步驟如下：

步驟1：初始化參數，將路徑規劃起點Xstart作為隨機樹T的根節點。迭代開始，使用RRT算法在無碰撞障礙空間進行均勻采樣，直到生成第一條可用的初始路徑；

步驟2：將形成的初始路徑節點存入集合S={S1，S2，…，Sn-1，Sn}，以Sn為初始節點的父節點Sp，Sn-1為初始子節點Sc。

步驟3：將父子節點相連，判斷兩點連線是否與障礙物發生碰撞，若SpSc與障礙物未發生碰撞，就將Sc-1作為新的子節點，重復步驟3，若SpSc與障礙物發生碰撞，就將Sp存入OPEN列表中，把子節點Sc設為新的父節點Sp，將Sc-1設為新子節點Sc；

步驟4：判斷子節點Sc是否為路徑起點S1，如果不是路徑起點，就重復步驟3，若Sc是路徑起點S1，就將OPEN列表中的點依次相連，形成優化后的初始路徑；

步驟5：將所有障礙物膨化γ單位長度，原障礙物的頂點為v，膨化后的障礙物對應頂點為v’，若頂點v和優化后的初始路徑之間的距離小于單位長度a，就把頂點v對應的v’設為關鍵點，再以關鍵點為圓心，δ為半徑形成圓形采樣區域；

步驟6：隨機生成一個0-1的浮點數，判斷是否大于目標采樣概率sr，當大于sr時，本文算法在圓心區域內進行均勻采樣，若小于sr，則在所有的無碰撞區域內均勻采樣；

步驟7：以采樣點為圓形，r為半徑，搜索相鄰節點，計算從路徑起點到相鄰節點再到采樣點的路徑代價，通過式(2)篩選出采樣點的父節點再將兩者相連，將新點和生成的樹枝加入到樹中；

步驟8：判斷迭代次數n是否達到預設值，若小于預設值，則重復步驟6，若迭代次數到達上限，就將Xgoal加入到樹中，從Xgoal回溯到起點Xstart，得到最終路徑。

4 仿真與分析

為了驗證融合有向D*與RRT*的路徑規劃算法(DDS-RRT*)的有效性，分別在不同環境中，將RRT*算法、Informed-RRT*算法、RRT*-Connect算法、BIT(batch_informed_trees)算法以及本文提出的DDS-RRT*算法在Python3.7中進行對比。給出DDS-RRT*算法的生長結果和五種算法的最優路徑結果，并且對機器人行駛的路程、時間和總步數進行比較和分析。在仿真中對障礙物進行膨化處理，并將機器人視為一個點。由于RRT類算法具有隨機性，所以在每張地圖中都進行40次獨立實驗，最大步長為2，最大迭代次數為8000。

4.1 地圖1中的仿真

地圖1(50×30)中起點和目標點設置為(5，5)，(45，25)，用來模擬存在隨機稠密障礙物的環境。在地圖1中進行的仿真，如圖4所示。

圖4 地圖1的仿真結果

圖4(b)中綠色點虛線、黑色長虛線、青色長虛線、藍色點劃線和紅色實線分別對應RRT*、Informed-RRT*、RRT*-Connect、BIT和本文算法，將五種RRT類算法進行40次重復實驗，去除兩次最優和最次結果后，五種算法的路徑長度、搜索時間和總步數統計見表1。

表1 地圖1中的仿真數據

五種算法迭代次數與路徑長度的關系如圖5所示。

圖5 地圖1中五種算法路徑長度與迭代次數的關系

在復雜障礙物環境中BIT是對比算法中效果最好的算法。而Informed-RRT*算法搜索時間較長，RRT*-Connect是RRT*算法的雙樹版本，僅在搜索時間上有較大優化，其搜索時間雖優于RRT*算法，但任弱于BIT算法。上述兩種算法在地圖1中的路徑長度與BIT算法任差距較大。將本文算法與BIT算法比較可知，本文算法搜索時間比BIT算法減少14%，路徑長度縮短了3.82%，總步數減少了58.82%。

與本文算法相比，四種對比算法不但路徑收斂性較差，而且無效轉彎較多，不能很好的滿足移動機器人的運動需求。四種對比算法更傾向于從中間障礙物較多的區域進行繞行而不是穿過，這也增加了路徑的長度。本文算法通過關鍵點采樣方法，在復雜障礙物環境中得到的路徑長度是五種算法中最優的。又因為本文算法使用了變距離重新布線策略，不但在搜索時間上比BIT算法略少，而且少量大半徑重新布線，刪除了樹枝上大量的冗余節點。保留的高效節點使生成的最終路徑上無效彎曲更少，減少了總步數，提高了路徑的平滑度。五種算法生成的路徑長度都隨迭代次數的增大而減小并最終趨于穩定，但本文算法隨著迭代次數的變化，總能提供相比其它四種算法更短的路徑，并能以較小的迭代次數得到比其它算法更短的路徑。

4.2 地圖2中的仿真

地圖2(50×30)中起點和目標點設置為(5，5)，(45，25)。與地圖1相比，地圖2主要是模擬迷宮環境，用來驗證算法通過連續小洞的能力。在地圖2中進行的仿真如圖6所示。

圖6 地圖2的仿真結果

圖6(b)中綠色點虛線、黑色長虛線、青色長虛線、藍色點劃線和紅色實線分別對應RRT*、Informed-RRT*、RRT*-Connect、BIT和本文算法，將五種RRT類算法進行40次重復實驗，去除兩次最優和最次結果后，五種算法的路徑長度、搜索時間和總步數統計見表2。

表2 地圖2中的仿真數據

五種算法迭代次數與路徑長度的關系如圖5所示。

圖7 地圖2中五種算法路徑長度與迭代次數的關系

在地圖2這種連續鉆小洞的情況下，對比算法中BIT的路徑成本最大，Informed-RRT*算法搜索用時最長。RRT*和RRT*-Connect算法在路徑長度上較優，而且RRT*-Connect是對比算法中搜索時間最短的。將本文算法與RRT*-Connect算法進行比較可知，本文算法搜索時間減少了4.2%，路徑長度縮短了5.26%，總步長減少了48.48%。

本文算法在路徑長度方面是最優的，而在地圖1中表現較好的BIT算法，在地圖2中路徑收斂性最差。五種算法相比，本文算法平均搜索時間優于RRT*-Connect和BIT算法，路徑的總步長得到了大幅度的減少，路徑長度也是最優的。對比五種算法在地圖2中生成的路徑，可以看出本文算法相比其余四種算法在障礙物頂點處的轉彎更為平滑，沒有出現其余四種算法中90°急轉彎的情況，更加符合移動機器人的運動學規律。雖然地圖2中起點和終點之間的歐幾里得距離較近，但因為存在連續的狹窄小洞，增加了狀態空間中無效的采樣點數量，使最終的路徑長度增大。本文算法與其它四種RRT類算法相比，將采樣點按概率分配在障礙物頂點的圓形采樣區域和整個狀態空間，克服了采樣點過于分散的問題，使得采樣效率大大增加。變距離布線策略刪除路徑上的大量的無效節點，在減小總步數的同時提高了路徑的平滑度。而且本文算法相比其它四種算法收斂速度更快，可以用更小的迭代次數得到更短的路徑。

4.3 地圖3中的仿真

地圖3(35×10)中起點和目標點設置為(2，2)，(30，8)，用來模擬連續狹窄通道，驗證算法在狹窄通道環境下的規劃能力。在地圖3中進行的仿真如圖8所示。

圖8 地圖3的仿真結果

圖8(b)中綠色點虛線、黑色長虛線、青色長虛線、藍色點劃線和紅色實線分別對應RRT*、Informed-RRT*、RRT*-Connect、BIT和本文算法，將五種RRT類算法進行40次重復實驗，去除兩次最優和最次結果后，五種算法的路徑長度、搜索時間和總步數統計見表3。

表3 地圖3中的仿真數據

五種算法所生成的最優路徑長度與迭代次數的關系如圖9所示。

圖9 地圖3中五種算法路徑長度與迭代次數的關系

在地圖3這種連續狹小通道情況下，Informed-RRT*算法是四種對比算法中路徑收斂性最優的，但Informed-RRT*算法搜索時間較長。本文算法比Informed-RRT*算法路徑長度縮短了3.83%，搜索時間減少了59.54%，總步數減少了43.47%。對比五種算法生成的路徑可知，本文算法的路徑收斂性最優，且路徑上的冗余拐點也更少，在通過連續狹窄通道之后，其余四種算法都發生不同程度的無效轉彎，而本文算法可以用較為平滑的路徑直接與目標點相連，減少了冗余路徑。該算法用更少的迭代次數實現了與其它算法相同的效果，減少了搜索時間。雖然地圖3中起點與終點的歐幾里得距離較近，但是連續的狹窄通道放大了起點與終點之間的距離，導致Informed-RRT*算法的橢圓子集變大，影響采樣效率，使搜索時間變長。本文算法的關鍵點采樣方法在連續狹小通道內效果明顯，能克服橢圓子集過大的問題，生成代價更小、總步數更少的最優路徑。

5 結論

現有的RRT類算法在復雜環境中存在路徑收斂性差和總步數多的問題，提出了一種融合有向D*與RRT*的路徑規劃算法。該算法通過關鍵點采樣和變距離重新布線對路徑規劃的采樣區域進行優化，減少了無用采樣，提高路徑規劃效率。實驗結果表明，本文算法在相同環境下，比四種RRT類算法的路徑長度縮短了4.30%，搜索時間減少了25.91%，路徑總步數減少了50.26%，且可以適應存在連續小洞和狹窄通道的特殊環境。本文算法在路徑規劃的收斂性和平滑性方面都有較好的效果，可以適應各種復雜環境，希望后續的研究可以將算法應用到無人機等高維度路徑規劃領域。