基于D*Lite算法路徑規劃的改進方法

戴年慧，趙江銘，王傲杰，胡鋇

(鄭州大學機械與動力工程學院，河南鄭州 450001)

0 前言

早在20世紀就開始研究路徑規劃算法，比如基于圖搜索的全局路徑規劃Dijsktra算法、A*算法[1]、D*算法、Dynamic SWSF-FP等；比如基于采樣的全局規劃算法PRM算法、RRT算法；比如局部規劃算法人工勢場法[2]、DWA算法、遺傳算法[3]等。

D*Lite算法是在LPA*[4]的基礎上提出的一種新的重規劃方法。D*Lite算法[5]是一種高效的動態路徑規劃方法[6]。當然，D*Lite算法也存在著不足，此前有不少人對其進行了改進。隨裕猛等[6]進行了D-star Lite算法及其動態路徑規劃實驗研究，使算法在速度上有了較大的提高；張曉冉、居鶴華[7]采用改進D*Lite算法進行自主移動機器人路徑規劃，引入Bresenham畫線算法，并建立分辨率高于全局障礙圖的局部障礙圖，提高算法對動態環境的適應性；連云霞等[8]將改進D*Lite算法應用在虛擬士兵路徑規劃中，加入煙花算法減少不必要的轉彎，并解決過于靠近障礙物的問題；周燕萍、劉以安[9]研究一種移動機器人的路徑規劃算法，提出一種結合入侵雜草算法和NURBS算法的混合路徑規劃方法。在以上基于D*Lite算法的改進方法中，沒有針對啟發值的改進。另外，余文凱等[10]基于地圖預處理及改進A*算法的路徑規劃，優化子節點的選擇，使規劃后的路徑不再穿過障礙物的頂點，避免機器人與障礙物過于接近甚至碰撞，但由于刪除了障礙物周圍的危險節點，使得獲得路徑的概率降低(如終點位于地圖角落，其上面節點和右邊節點為障礙點時，算法易陷入死循環，而搜索不到一條可行的路徑)。

針對以上問題，本文作者提出一種改進方法來提高算法啟發值的準確性并優化路徑，并通過仿真實驗對比，對算法進行驗證。

1 D*Lite算法

D*Lite算法是KOENIG S和LIKHACHEV M在LPA*算法的基礎上改進并提出的。LPA*算法，即Liflong Planning A*算法，基于A*算法和Dynamic SWSF-FP算法[11]的思想，用來解決動態環境下定起點到定目標點的路徑規劃問題，采用正向搜索方式。不論是A*算法還是LPA*算法都不適用于車輛(起點)位置變化的情況。基于LPA*算法的D*Lite算法可以很好地應對環境未知的情況，其算法核心是通過最小化rhs值找到目標節點到各個節點的最短距離，所到的節點即設置為起始節點，因此當路徑變化和key值需要更新時，更新從目標點到新起始點的啟發值和估計成本。

1.1 D*Lite符號說明

D*Lite算法是在Lifelong Planning A*算法的基礎上改進的，區別于LPA*算法的部分在于g(s)和rhs(s)的定義，g(s)表示當前節點s到目標節點goal的距離，如下所示：

(1)

(2)

式中：succ(s)∈S，表示節點s∈S的子節點的集合；同樣地，pred(s)∈S，表示節點s∈S的父節點的集合,S表示圖的節點的有限集。c(s′，s)∈(0，∞)表示從節點s移動到s′∈succ(s)的代價。

起點start∈S到節點s的估計值即為啟發值h(start，s)，起點start到節點s的距離為兩者坐標數值差的最大值。

優先隊列U中存放待擴展節點，節點按照key值排序，key定義如式(3)和式(4)所示：

key=[key1，key2]

(3)

其中:

key1=min[g(s)，rhs(s)+h(s)]

key2=min[g(s)，rhs(s)]

(4)

key中第一個分量直接對應于A*使用的f值，f=g(s)+h(s)。排序規則按照單調非遞減目標距離的順序展開具有相同key值的頂點，例如，key(s1)≤key(s2)，表示key1(s1)

1.2 其他概念

局部一致：g(s)=rhs(s)。當所有節點均為一致狀態時，即g(s)值等于s到目標點goal的最短距離，可以找到任意一點s到目標點goal的最短路徑。比如，當前節點為s，其子節點s′(向著目標點goal前進的小一個節點通過最小的g(s′)+c(s′，s)獲得)不斷重復直到到達目標點goal。

局部過一致：g(s)>rhs(s)。表示當前節點s可以通過子節點s′使自己到目標點的路徑更短，即s′到s的邊緣代價函數c(s，s′)值變低，網格上障礙物被清除(c(s，s′)的值從∞變為B)或搜到一條更短的路徑。此時，將g(s)設置為rhs(s)，節點狀態變為局部一致。

局部欠一致：g(s)

1.3 算法流程

D*Lite算法流程如圖1所示，具體步驟如下：

(1)初始化

優先隊列U為空集，將所有柵格rhs(s)和g(s)置為無窮大，終點goal的rhs(goal)設置為0，計算終點goal的key值并插入優先隊列U中。

(2)計算預設路徑

對當前節點的擴展節點計算key值并插入優先隊列U中，key值最小的彈出優先隊列并作為當前節點，重復這一過程，直到搜索到起始點為止，生成一條可行的路徑。

(3)機器人運動

機器人根據預規劃的路徑運動到下一個點，同時檢查環境是否改變。

(4)環境改變

如果環境改變即發現下一個點為障礙點，更新該節點和受影響的節點的參數，回到步驟(2)重新計算路徑，直到機器人運動到終點。

2 改進

前文中介紹的D*Lite算法的改進大多針對算法效率的改進，作者研究了A*算法的啟發值和D*Lite算法在預規劃路徑的表現之后，發現以下一些可以進行改進的地方：

(1)啟發值對增量式搜索算法很重要。啟發值始終是小于或者等于實際路徑長度，構造一個較為接近實際長度的啟發值對算法性能很有意義。采用一個比切比雪夫距離更接近實際路徑長度的啟發值。

(2)引入安全系數。采用原始D*Lite算法對路徑進行規劃可發現斜穿過障礙物柵格的頂點現象。因此，對擴展節點進行分類，對危險節點引入安全系數。

2.1 構造啟發值h

在優先隊列U中，根據key的值進行排序。由第1.1節中符號說明中可知，排序取決于第一個分量key1，即(rhs(s)+h(s))(當預規劃時)，rhs(s)表示當前節點s到目標節點goal的步數。根據A*算法可知，h(s)越精確，擴展次數越少，尋路越快且路徑越短；h(s)越小于實際路徑，擴展點越多，速度越慢；反之，不能找到最短路徑，但是速度很快。一開始使用的距離是切比雪夫距離，如式(5)所示：

h(s)=D·max(|sx-startx|,|sy-starty|)

(5)

可是這樣的啟發式不夠準確，因為運動中包括斜線運動。所以構造一個更加準確的啟發值，

dx=|sx-startx|

(6)

dy=|sy-starty|

(7)

h(s)=D2·min(dx，dy)+D·(dx+dy-2·

min(dx，dy))

(8)

式(8)中：D2表示穿過斜線的代價系數；D表示直穿過的代價系數。第一項表示斜穿過格子的代價值，第二項表示直穿過格子的代價值。對比可知，構造的啟發值比式(5)表示的切比雪夫距離更加接近實際的距離。

黑色為障礙物節點，紅色為起點，綠色為終點，起點到終點的連線為規劃的路徑。

優先隊列U是closelist擴展的節點的集合，是圖中的淺灰色;closelist是優先隊列U彈出的節點的集合，是圖中的深灰色柵格，表示搜索的次數。由兩張圖對比可知：圖2(a)中closelist中的節點數量為8個，圖2(b)中closelist數量為6個，圖2(a)比圖2(b)多出節點(4，5)和節點(2，2)，故而圖2(a)的擴展次數比圖2(b)的多。由此可知，改進的啟發值比切比雪夫距離的啟發值更準確，擴展次數減少，性能更好。

2.2 引入安全系數

D*Lite算法當前節點s的擴展點遵循8個方向進行擴展，如圖3所示；當前節點到下一子結點可能會出現碰撞障礙物或者離障礙物過近的現象。針對在這一表現，受文獻[10]啟發，對子節點加入狀態值，降低路徑斜穿過障礙物頂點的概率。

圖3 父節點與子節點的坐標關系

由圖3所示，根據當前父節點與子節點的坐標關系，將父節點周圍的子節點進行分類。為了區分子節點，改變危險子節點和已搜索的節點的狀態值，具體操作如下：

(1)判斷危險子節點。如果當前父節點上面的子節點(子節點2)為障礙點，那該子節點的左右兩個節點(子節點1、子節點3)標為危險子節點；如果當前父節點右邊子節點(子節點4)為障礙點，那么該子節點的上下兩個節點(子節點3、子節點5)標為危險子節點；同理推之，當前父節點下面的子節點(子節點6)為障礙點，那么標記該節點的左右兩個節點(子節點7、子節點5)為危險子節點；當前父節點左邊的子節點(子節點8)為障礙點，標記該節點的上下兩個節點(子節點1、子節點7)為危險子節點。

(2)標記危險節點狀態值。判斷為危險子節點的節點狀態值標記為1。

(3)對于狀態值state為1的節點，在啟發值h中增加一個安全系數cost(oi)。

(4)對于所有搜索過的非危險節點，節點狀態值置為-1。

父節點與其擴展子節點的連線到障礙點的距離為D(oi)，其代價cost(oi)為

(9)

式(9)中：a表示一個柵格的長度；t表示安全代價值。

由圖4可以發現:圖4(a)的路徑出現了連線斜穿過障礙點的節點，而圖4(b)中路徑與障礙柵格保持了半個柵格長度的距離，比如，圖4(a)中節點(3，4)到節點(2，3)的連線經過障礙點(3，3)的頂點，過于靠近障礙物；在圖4(b)中節點(2，3)不作為優先選擇，節點(3，4)下一個節點選擇了(2，4)。改進后規劃的路徑沒有斜穿過障礙物柵格頂點的現象，而是與障礙物保持了一定距離。

圖4 引入安全系數的仿真對比

3 仿真結果與分析

在MATLAB中進行仿真，分別是5×5、10×10和50×50的柵格，障礙點數量分別是5、14和98，設定障礙點在3種規模的柵格中隨機分布，每個網格的大小為一個像素。算法中改變了啟發值和加入了安全系數，所以重規劃和原算法中重規劃原理一樣，只不過重規劃的路徑也體現了預規劃路徑的性能：擴展節點減少和遠離障礙點不是文中的重點，故這里就不做重規劃路徑的仿真。

由圖5可知：改進前的路徑都有經過障礙物的頂點，而加入安全系數后規劃的路徑都與障礙物保持了半個像素的距離。對比文獻[10]將危險點都去除的方法，如起始點的上方柵格和右方柵格為障礙點的情況就搜索不到路徑，提高了保證路徑生成的概率。

圖5 三種規模地圖的路徑規劃的仿真對比

表1是3種隨機障礙點地圖路徑規劃的結果對比，可知:在搜索次數上，3種規模的地圖擴展次數分別減少38.5%、21.4%、50%，；在規劃的路徑長度上，3種規模的地圖路徑長度分別增加8.5%、4.7%和減少0.69%。由此可以看出：改進后的算法搜索次數減少，算法搜索效率提高，性能更好；在5×5和10×10的地圖中，路徑長度是增加的，且長度增加率是減小的;在50×50的地圖中，路徑長度是減小的，由此可見，該改進算法運用于大地圖中的效果更好。

表1 3種規模的柵格路徑規劃的結果

4 結論

文章改進了D*Lite算法的啟發值，在擴展結點中引入安全系數，對于危險節點將不作為優先選擇的節點。通過仿真結果可以看出，改進后的D*Lite算法啟發值更精確、更接近實際距離，減少了搜索擴展的次數，并且解決了路徑穿過障礙物柵格頂點的問題，提高了算法的搜索效率，能夠規劃出一條十分安全的路徑。