劉逸凡，黃友銳，韓 濤

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

1 引言

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

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

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

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

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

2 相關(guān)工作

2.1 Informed-RRT*算法

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

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

2.2 有向D*算法

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

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

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

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

3.1 關(guān)鍵點的選擇與概率采樣

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

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

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

(1)

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

圖2 圓形采樣子集示意圖

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

3.2 變距離重新布線

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

(2)

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

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

(3)

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

3.3 算法實現(xiàn)流程

算法流程圖如圖3所示。

圖3 本文算法流程圖

融合改進算法實現(xiàn)步驟如下：

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

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

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

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

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

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

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

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

4 仿真與分析

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

4.1 地圖1中的仿真

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

圖4 地圖1的仿真結(jié)果

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

表1 地圖1中的仿真數(shù)據(jù)

五種算法迭代次數(shù)與路徑長度的關(guān)系如圖5所示。

圖5 地圖1中五種算法路徑長度與迭代次數(shù)的關(guān)系

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

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

4.2 地圖2中的仿真

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

圖6 地圖2的仿真結(jié)果

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

表2 地圖2中的仿真數(shù)據(jù)

五種算法迭代次數(shù)與路徑長度的關(guān)系如圖5所示。

圖7 地圖2中五種算法路徑長度與迭代次數(shù)的關(guān)系

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

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

4.3 地圖3中的仿真

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

圖8 地圖3的仿真結(jié)果

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

表3 地圖3中的仿真數(shù)據(jù)

五種算法所生成的最優(yōu)路徑長度與迭代次數(shù)的關(guān)系如圖9所示。

圖9 地圖3中五種算法路徑長度與迭代次數(shù)的關(guān)系

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

5 結(jié)論

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