基于改進D*Lite的二維路徑連續動態規劃算法

傅 亮,劉 峰,劉書勇*,韓新宇

(1.中國航空無線電電子研究所 民機航電系統部,上海 200241;2.哈爾濱工程大學 計算機科學與技術學院,黑龍江 哈爾濱 150001)

0 引言

路徑規劃[1]是讓目標對象基于環境模型下尋找一條經過起止節點的無碰撞障礙物的安全路線優化問題,它廣泛應用于移動機器人、空中無人機、自動駕駛車輛、水面無人艇等領域[2-8]。基于規劃模式可劃分為靜態路徑規劃與動態路徑規劃兩大類。靜態路徑規劃常用算法有圖搜索算法[9-11]和智能優化算法。圖搜索算法如Dijkstra、A*、RRT等,智能優化算法如蟻群算法、遺傳算法、粒子群算法、蝙蝠算法等[12-15]。靜態路徑規劃算法實施要求之一是需要全區域的地形信息,對未知地形則不具備規劃能力。為解決在未知領域進行路線搜索的問題,提出了動態路徑規劃算法,如D*、LPA*、D*Lite等[16-18]。

D*Lite算法是融合D*和LPA*提出的一種反向搜索的動態路徑規劃算法,應用在未知環境下進行快速路線搜索,可以對未知突變障礙進行重規劃,是一種高效的規劃算法,但是它依然存在易受運動物體影響及路徑貼近障礙物等問題,因此眾多研究學者對其進行了改進。吳濤等人[19]引入跳點搜索概念,僅對跳點進行訪問,能夠有效優化路徑長度。杜軒等人[20]結合人工勢場(Artificial Potential Field,APF)法對D*Lite規劃路線進行重規劃,使得目標物體在移動中能夠與障礙物保持有效的安全距離,但本身并未對D*Lite算法進行改進。張毅等人[21]針對原有Boustrophedon單元分解法進行改進重構,提取關鍵單元,引導正確搜索方向,有效提高規劃速度。戴年慧等人[22]進入安全系數阻止危險節點作為路徑優先選擇節點以避免斜穿過障礙,導致冗余點過多。以上改進算法僅對預規劃路徑進行單次動態重構,并沒有進行多次重構優化,沒有體現動態規劃的多樣性。

針對以上問題,本文提出了一種改進D*Lite的二維路徑連續動態規劃算法(Continuous D*Lite,CD*Lite)。本文算法基于柵格法進行環境建模,記n·m矩陣。優化原始D*Lite算法的切比雪夫啟發估計函數,使其在估計數值上更加貼近實際路線規劃。引入人工勢場的引力場概念,作為補充啟發估計函數,并提出了一種全域啟發估計算子。引入人工勢場的斥力場概念,提出了一種方向禁忌矩陣,能夠避免斜穿柵格障礙節點。優化重規劃模式,使其能夠在連續產生動態障礙的條件下對路徑進行持續重構。最后引入冗余點刪除機制,使得規劃路線更加平滑。

1 相關工作

1.1 D*Lite算法

D*Lite[18]是由Sven Koenig和Maxim Likhachev于2002年提出的一種基于LPA*和D*的動態路徑規劃算法。LPA*是一種基于A*和Dynamic SWSF-FP思想的正向傳播增量式動態路徑規劃算法,但正向搜索會因為很多分支導致效率下降,因此D*Lite融入D*的逆向搜索思想,從目標節點反向搜索起始節點,能夠有效地減少節點訪問次數,減少搜索時間。D*Lite是一種起始節點可變化的反向規劃算法,因此更適合應用于未知環境或預規劃路線突變障礙的情況。

D*Lite算法通過不斷更新節點值執行路徑搜索過程,其定義如式(1)所示:

(1)

式中:keys是由k1和k2組成的二維矩陣,表示為s節點的鍵值;sstart為當前開始節點,sgoal為目標節點,slast為重構路徑之前的sstart節點,g(s)為sgoal節點到s節點的歷史最小實際代價值;rhs(s)為sgoal節點到s節點的啟發估計值;km為鍵值修飾器,是slast節點到sstart節點的實際移動距離。

D*Lite與LPA*最大的不同在于key值定義,通過引入km因子修飾key,使得啟發估計值的sstart節點是可變化的,可以減少不必要節點的計算操作,從而大大提高算法效率,也使得key值比較對于實際而言更有意義。由式(1)可知k1由最小實際代價值、啟發估計值、鍵值修飾器累和求得,其具體定義如式(2)～(5)所示:

(2)

(3)

h(s,sstart)=λ·max(|xs-xsstart|,|ys-ysstart|),

(4)

(5)

式中:Succ(s)為s節點的后繼節點集合;λ為單步移動代價;c(s,s′)為邊緣實際代價函數,即s節點到邊緣s′節點的歐氏距離,xsysxs′ys′為s,s′的橫縱坐標值;h(s,sstart)為s,sstart的切比雪夫距離,xsysxsstartysstart為s,sstart的橫縱坐標;

算法核心在于不斷遍歷節點并計算keys放入待檢測隊列(Priority Queue)。同時由式(2)～(3)可知,g(s)和rhs(s)定義相同,但區別為g(s)為歷史最小值、rhs(s)為當前計算值,取二者最小值用于更新keys,共計3種不確定狀態如下所示:

① 若rhs(s)=g(s),局部一致狀態。此狀態表示結點s處于預估與實際基本沒有差錯的穩定狀態。如無外界影響,將一直保持此狀態。

② 若rhs(s)

③ 若rhs(s)>g(s),則處于局部欠一致狀態。此狀態表示節點s周圍節點發生障礙物突變導致舊有解失效,已經收到了附近新障礙物直接或間接的影響,因此需要將其放入優先隊列中進行進一步判斷。

D*Lite算法主要功能包括預規劃路徑和動態規劃路徑,具體如下所示:

① 預規劃路徑:算法初期會通過更新全局地圖信息,根據式(1)計算相應擴散節點的keys,并將其置于優先隊列中,然后計算優先隊列最小keys,計算規則為先按照k1升序排序,再按照k2升序排序,最后優先隊列中首項即為所求,更新該s節點的最小實際代價值,根據3種局部狀態執行相應操作,遍歷邊緣節點尋找最優keys作為其前繼節點,重復以上操作,直到遍歷到sstart節點,功能結束。

② 動態規劃路徑:當預規劃路徑的節點突變為障礙物,會重新計算突變節點的keys,將受到突變節點影響的所有邊緣節點全部置于優先隊列中,再執行預規劃路徑功能,完成單次動態路徑規劃。

1.2 人工勢場

人工勢場算法是路徑規劃和局部避障運動中較為常用的方法,該算法的基本思想是在移動點和障礙物周圍定義勢場,移動點的運動依靠勢場力進行驅動。人工勢場[23]是由Khatib于1986年提出的一種虛擬力場,它將目標與障礙物看作產生引力與斥力的物體,對其投入場內物體使其沿著場內合力方向進行移動。主要方法為建立引力場Uatt(q)和斥力場Urep(q),如式(6)～(7)所示:

(6)

(7)

式中:ξ為引力尺度因子,η為斥力尺度因子;ρ(q,qgoal)為物體與目標的歐氏距離,ρ(q,qobs)為物體與障礙物的歐氏距離,ρ0為障礙物的影響半徑。

Uatt(q)和Urep(q)的負梯度方向即為引力和斥力的方向,負梯度值即為物體受到引力和斥力的值,如式(8)～(9)所示:

(8)

(9)

式中:ξ為引力尺度因子,η為斥力尺度因子,ρ(q,qgoal)為物體與目標的歐氏距離,ρ(q,qobs)為物體與障礙物的歐氏距離,ρ0為障礙物的影響半徑。

2 CD*Lite改進算法

2.1 改進啟發估計算子

在進行動態路徑規劃時,針對移動點當前位置附近的節點,使用傳統的D*Lite算法來估計啟發式值。具體而言,使用切比雪夫距離作為計算方式,該距離是在向量空間中測量兩個點之間的一種方法,它定義為兩個點在各個坐標上數值差的絕對值中的最大值。切比雪夫距離也被稱為棋盤距離,因為它類似于在棋盤上移動棋子的最短步數。在設定的場景中,目標節點周圍的鄰接節點與目標節點的切比雪夫距離都是1,這意味著在橫向、縱向和斜向移動時,到達鄰接節點的代價是相同的。然而,在實際情況中,當目標節點向預定節點移動時,“斜走”“先直走再橫走”到達預定節點的代價是不同的。因此,需要改進傳統啟發估計算子以匹配實際路徑規劃的路徑代價,基于式(10)～(12)對其進行改進。

h′(s,sstart)=λ1·diffmin+λ2·(diffmax-diffmin),

(10)

式中:

diffmin=min(|xs-xsstart|,|ys-ysstart|),

(11)

diffmax=max(|xs-xsstart|,|ys-ysstart|),

(12)

2.2 改進Key鍵值算子

2.2.1 引入人工勢場引力算子

人工勢場法定義目標點引力場如式(6)所示,引力場引導移動點向目標點運動,在移動點和目標點間無障礙的理想條件下,移動點和目標點坐標間連線距離始終為最短距離;算法不需要對全局軌跡進行搜索,規劃時間短、效率高,滿足實時和軌跡生成安全性的要求。定義hatt(s)為移動點與目標點的啟發估計代價如式(13)所示,即引入人工勢場引力算子作為補充啟發代價算子。

(13)

式中:ξ為引力尺度因子,xsysxsgoalysgoal為s,sgoal的橫縱坐標。

2.2.2 全域啟發估計算子

D*Lite算法的啟發估計算子如式(1)所示,通過計算起始點到目標點的代價值標識移動點的啟發效果,在局部規劃中是一種盲目性的試錯路徑規劃,在復雜環境中帶來更多的時間效率損耗。為改進keys鍵值算子,本文提出全域啟發估計算子hwhole替換h(s,sstart),如式(14)～(15)所示:

(14)

其中:

hwhole=h′(s,sstart)+hatt(s),

(15)

式中:g(s)和rhs(s)定義如式(2)～(3)所示,h′(s,sstart)和hatt(s)定義如式(10)和式(13)所示。

式(1)中h(s,sstart)為s,sstart的切比雪夫距離,由2.1節可知hwhole內h′(s,sstart)算子改進h(s,sstart)算子的實際路徑規劃路徑代價,提升規劃路徑精度;由2.2.1節可知,hwhole內新增的hatt(s)算子引導移動點始終向目標點方向移動,不必通過分別計算鄰域8個方向的結果后做方向選擇,加快算法收斂速度。

2.3 引入方向禁忌矩陣

D*Lite算法在規劃路徑時會出現斜穿障礙物頂點的危險行為[22],在實際路徑規劃當中這種路線會對移動物體產生威脅,需要將其修正為安全行為,如圖1所示。

圖1 修正危險路線Fig.1 Correct the dangerous route

修正規劃路徑中的危險路徑需要識別危險節點,即障礙物節點。遍歷環境內每一格柵格,判定識別其是否為障礙物;同時對其鄰接8個方向柵格節點的路線選擇和變更情況按照圖1的禁忌準則進行確認,并構建鄰域方向禁忌矩陣d用于存儲危險路線修正變更結果;移動點根據所在節點8鄰域的規劃路線修正結果,減少重復計算,避免迂回搜索。

鄰域方向禁忌矩陣d定義如式(16)所示,在對目標節點進行鄰域搜索時,基于其過濾危險節點。

(16)

(17)

(18)

i=x×m+y,

(19)

式中:move(i)為i節點能夠經過的安全鄰域節點;d為p+1行8列的二維矩陣,p對應n行m列柵格模型環境矩陣c內最后一個序號節點;方向禁忌矩陣d的行序號i與n×m柵格模型環境矩陣c內(x,y)節點對應關系如式(19)所示。禁忌矩陣d有8列,即8個分量對應節點8個鄰域方向移動有效位,對應鄰域節點的標識如圖2所示。

圖2 鄰域節點的標識Fig.2 Identification of neighborhood node

在算法進行鄰域節點搜索或動態路徑重規劃尋找周圍影響節點后,基于d進行安全行為判斷。從i～j節點尋找到一條路線,如果dij=-1說明j節點不存在,該路線無效;dij=1說明i節點可以移動到j節點,是一種安全行為;dij=0說明i節點不可以移動到j節點,是一種危險行為。dij初始化策略偽代碼如算法1所示。

算法1 UpdateD算法1 Input:S2Output:d3BEGIN:4 for eachs∈S5 if s is not existent ∥節點s為無效節點6 dij=-17 ∥ i～j節點的路線為安全路線8 else ifi do not cross j obliquely9 dij=110 elsedij=011 end if12 end for13 returnd ∥更新方向禁忌矩陣14END

2.4 連續動態規劃模式

D*Lite算法由一次全局預規劃和一次動態路徑規劃組成。算法首先執行預規劃機制,生成全局初始路線;接著進行動態路徑規劃,如果全局初始路線上出現突變障礙,會重新設定sstart起點直到新規劃路線再次經過sgoal節點,完成一次動態重構路徑,目標點會按照更新路線進行移動。如果目標點在完成D*Lite算法更新后的路線上移動時,在snow識別出新的突變障礙,重新使用D*Lite算法規劃路線,會導致運算效率降低。本文改進D*Lite算法的動態路徑規劃過程,提出連續障礙突變動態路徑規劃算法,即在一次動態路徑規劃完成后,若識別出新的突變障礙,可繼續開展動態路徑規劃,省略全局預規劃過程,大幅提升運算效率。算法輸入包括柵格節點集合以及經UpdateD算法輸出的方向禁忌矩陣d。輸出一個滿足上述約束符合既定需求的動態規劃局部路線節點集合E,偽代碼如算法2所示。

算法2 連續障礙突變動態路徑規劃算法1 Input:S,d2Output:動態規劃局部路線節點集合E3BEGIN:4 ∥既定路線有突變障礙5 If any obstacle changed near s do6 Update sstart and sgoal7 ∥更新全域8 Scan Global Grid and Update snow9 ∥動態規劃路線10 Compute shortest path and Update E11 end if12 returnE13END

2.5 冗余點刪除機制

經過動態規劃后,路徑從起始節點sstart開始順序連接鄰域節點sgoal,直至到達終止節點,如圖3所示的“規劃路線”。規劃路徑中的節點與其相鄰節點符合鄰域8方向機制,即只能與其周圍8個方向中的某一個節點相連。然而,這樣的連接方式可能會導致節點冗余的問題。為解決這一問題,本文提出了冗余點刪除機制。具體操作是從起始節點開始按順序遍歷路徑上的節點,構建一個“過渡路線”,如圖3所示。如果這個“過渡路線”不被認為是“危險路線”,則刪除當前節點之前的節點。如果被認為是“危險路線”,則返回前一個節點,將其所在的“過渡路線”定義為“最終路線”,并將該節點作為新的起始節點,繼續遍歷和重復進行冗余點刪除,直到達到終止節點。這樣構建出的“最終路線”即為經過冗余點刪除后優化的規劃路徑,實現了路徑的平滑化,并且目標點移動的方向不再局限于鄰域8方向。

圖3 冗余點刪除流程Fig.3 Redundant point deletion process

3 CD*Lite算法實現

本文CD*Lite算法具體實現步驟如下:

步驟1:初始化參數包括rows、cols、snow、sstart、slast、sgoal、km、U、d;

步驟2:執行預規劃路徑功能,令rhs(sgoal)=0,根據式(14)計算keysgoal,將sgoal和keysgoal作為子項插入U;

步驟3:反向搜索,遍歷s節點,根據g(s)和rhs(s)三種不一致狀態計算keys并對g(s)和rhs(s)進行同步更新,找到相應代價最小的鄰域節點,基于方向禁忌矩陣d篩除危險節點,直到遍歷經過sstart且U中子項最小的keys不小于keysstart時規劃結束,此階段延續采用D*Lite算法構建路徑方式。

步驟4:執行本文提出的連續動態規劃模式,默認目標單次移動單位柵格距離,在移動過程中實時檢測突變障礙,若有異常則及時更新方向禁忌矩陣d,更新所有因突變障礙而受到影響的所有節點rhs(s)并執行步驟2和步驟3同類操作,至此完成單次動態路徑重構。之后繼續實時檢測全域既定路線是否有突變障礙,重復以上流程以實現連續動態路徑規劃,直至目標移動到sgoal,返回全局規劃路線與突變障礙節點。

步驟5:最后采用冗余點刪除機制對CD*Lite規劃的路徑進行優化,保留最優路線,算法結束。

基于以上步驟,本文CD*Lite算法偽代碼如算法3所示。

算法3 CD*Lite算法1 Input:S,d2Output:全局規劃路線節點集合Ewhole3BEGIN:4procedure Initialize()5 initialize s∈S,snow=sstart=slast,U=?,km=0,d=0,rhs(s)=g(s)=∞,rhs(sgoal)=0;6 Calculate sgoals key and insert into U(sgoal,key)7procedure Calculate Key(s)8 return[min(g(s),rhs(s))+hwhole(s)+km, min(g(s),rhs(s))]9procedure Compute Shortest Path()10 while U.TopKeyg(sstart)11 u=U.Top();12 Calculate Key(u) and Update us near Vertex13 Update U and related g(u) & rhs(u)14procedure Continuous Dynamic Program (u)15 Move and Scan abrupt obstacles on preplanning path;16 If any obstacle changed:17 Update D and km=h'(slast,sstart)18 While True19 whilesstart≠sgoal20 Update sstart,slast21 Scan directed influenced s by abrupt obstacles22 rhs(s) = ∞ and insert into U(s,Calculate Key(s))23 rhs(obstacles)=g(obstacles)=∞24 Compute Shortest Path()25 If get valid path26 Update Ewhole27 Else28 spre-aiming=spre-aiming.Prec29procedure Main()30 Initialize()31 Update D(d)32 Compute Shortest Path()33 whilesnow≠sgoal34 Continuous Dynamic Programming (snow)35 Update Ewhole36 returnEwhole37END

4 實驗分析

4.1 實驗環境與參數設定

本實驗的硬件環境:CPU為Intel(R) Core(TM) i7-9750H CPU @ 2.60 GHz 12線程;軟件環境:Windows 10平臺PyCharm 2021.3.3。CD*Lite算法在柵格地圖上進行動態路徑規劃結果驗證。實驗采用20×20、50×50、100×100三種規模柵格地圖,地圖內黑色方塊為障礙地圖塊,白色方塊為目標可行動區域圖塊,黃色方塊為目標行動終止節點。為保證柵格地圖內容隨機性,使用隨機算法對應生成三種地圖節點集,如式 (20)～(21)所示:

ob=random.sample(range(0,a·b),c),

(20)

(21)

式中:ob為障礙節點集,a和b為生成柵格地圖的行高和列寬,c為障礙節點數目總和,fieldij為柵格地圖節點集。按照實驗環境要求生成三種規模地圖節點集,為確保生成地圖普適性,設置障礙占比分別為 25.5%、40.16%、39.71%,如圖4所示。

(a) 20×20柵

(b) 50×50柵

(c) 100×100柵圖4 三種規模柵格地圖模型Fig.4 Three scale grid map models

在預規劃模式中,本文CD*Lite算法將與傳統路徑規劃算法LPA*、D*Lite進行實驗對比,參數設定單步移動代價如表1所示。在動態規劃模式中,首先基于單次動態規劃,本文改進的CD*Lite算法將與文獻[22]提出的改進D*Lite算法從路線節點個數、路線長度、運行時間進行對比。然后,進行連續動態規劃以驗證本文改進的CD*Lite算法。

表1 單步移動代價Tab.1 Single step moving cost

4.2 預規劃實驗結果分析

預規劃實驗結果如圖5～圖7所示,分別為20×20、50×50、100×100三種規模下各算法所規劃的路徑,障礙占比分別為 25.5%、40.16%、39.71%,改進效果對比如表2所示。

(a) LPA*20×20

(b) D*Lite20×20

(c) CD*Lite20×20圖5 三種算法在20×20規模下的預規劃路徑結果Fig.5 Three algorithms pre planning path results under 20×20 scales

(a) LPA*50×50

(b) D*Lite50×50

(c) CD*Lite50×50圖6 三種算法在50×50規模下的預規劃路徑結果Fig.6 Three algorithms pre planning path results under 50×50 scales

(a) LPA*100×100

(b) D*Lite100×100

(c) CD*Lite100×100圖7 三種算法在100×100規模下的預規劃路徑結果Fig.7 Three algorithms pre planning path results under 100×100 scales

表2 CD*Lite與LPA*、D*Lite實驗結果數據對比Tab.2 Comparison of experimental results of CD*Lite in this paper,LPA* and D*Lite presented

CD*Lite算法在預規劃模式下不執行冗余點刪除機制,在20×20、50×50和100×100的地圖中都能夠消除斜穿障礙邊界點的危險行為。而傳統算法LPA*和D*Lite在路徑規劃時以最短距離為前提,不考慮實際障礙物的影響,因此會產生較多的危險節點。本文算法通過引入方向緊急矩陣實現了目前的效果。

相比于LPA*和D*Lite算法,改進的CD*Lite算法計算節點代價的次數減少約50%和30%,計算時間減少約50%和35%。這是因為本文提出的全域啟發算子能夠更好地估計當前節點的代價值,從而更有效地指導算法進行全局路徑規劃。

改進后的CD*Lite算法考慮到實際應用中需要保持距離,因此在危險節點處需要增加拐點。即使在20×20和50×50的地圖中,改進后的算法仍能夠保持距離不增加。隨著地圖規模的增大,與LPA*和D*Lite算法相比,距離僅增加約10%。

動態規劃定義為在預規劃路徑基礎上出現突變障礙之后繼續路徑規劃操作,根據實現模式可以分為單次動態規劃與連續動態規劃。單次動態規劃如D*Lite,而本文所提出的CD*Lite可以連續進行單次動態規劃即連續動態規劃。理論上可以連續進行無窮次動態規劃,為便于實驗結果解析,本文設計的CD*Lite算法基于上述中的20×20規模柵圖選取連續3次動態規劃結果如圖8所示。圖中黃色為移動目標搜索起點,紅色為突變障礙節點。移動目標不斷更新snow,同時偵查全域既定路線上是否有突變障礙,當運動到圖8(b)黃色節點時偵測到異常變化,初始化sstart=snow,slast=sstart并對既定路線進行重構,目標沿新路線移動并更新snow,重復以上步驟的具體路線重構結果如圖8(c)～(d)所示。

(a) 真實預規劃路線

(b) 突變1

(c) 突變2

(d) 突變3圖8 CD*Lite算法連續3次對突變障礙動態路徑規劃Fig.8 CD*Lite algorithm performs dynamic path planning for abrupt obstacle nodes for three consecutive times

文獻[22]提出一種改進D*Lite算法,通過引入安全系數,使得危險節點將不作為路徑的優先選擇,能夠有效地避免斜穿過障礙物柵格頂點。本文將從路徑長度、規劃曲線拐點個數、運行時間三方面進行對比以驗證CD*Lite的連續動態規劃模式的可行性,其具體實驗結果如表3所示。

表3 CD*Lite與LPA*、D*Lite實驗結果數據對比Tab.3 Comparison of experimental results and data between CD*Lite and LPA*,D*Lite

CD*Lite算法和文獻[22]在進行首次動態路徑規劃時的操作相同,都需要對所有相關節點keys進行初始賦值。然而,CD*Lite算法與文獻[22]的不同之處在于CD*Lite會保留節點信息并進行持續更新。

在持續既定路線節點變異3次后,文獻[22]算法沒有記憶能力,而CD*Lite算法相比文獻[22]算法的節點代價計算次數減少約80%,運算時間減少約70%。這表明本文CD*Lite算法在面對連續既定路線突變障礙情況下能夠高效地動態重構路徑,展現了其可行性和高效性。

5 結束語

本文融入人工勢場的引力場思想提出了一種適用于連續既定路線突變障礙情況下的連續動態路徑規劃CD*Lite算法,對原始切比雪夫代價估計函數進行改進區分行走代價,使其更貼近真實仿真。引入引力算子提出了一種全域代價估計算子并改進鍵值算子,能夠有效地對當前規劃路線的節點進行有效評估,提高了算法規劃效率。提出鄰域方向禁忌矩陣,有效避免產生危險路線行為;引入冗余點刪除機制,使得規劃路徑更加平滑,使目標移動更加靈活;分別基于預規劃和連續動態規劃模式下,對模擬柵格地圖進行路徑規劃進行仿真驗證,成功驗證了本文CD*Lite算法連續動態路徑規劃模式的優勢。