上一篇介紹了D*路徑搜索算法原理解析及Python實現，這一篇緊接着介紹D*算法的改進版。
1.D* Lite算法簡述
D_star Lite算法是Koenig S和Likhachev M基於LPA_star 算法基礎上提出的路徑規劃算法。
D_star Lite 算法之於 LPA_star 算法猶如 D_star 算法之於 A_star 算法。與 LPA_star 採用的正向搜索算法不同，D_star Lite 採用反向搜索方式，效果與D_star 算法相當。無論是前文提到LPA_star 算法還是A_star 算法都不能滿足移動機器人在未知環境中的路徑規劃需求，因爲其在未知地圖中需要不斷的嘗試，與邊走邊找到最優路徑背道而馳。此時反向搜索算法能夠很好的處理這種情況，D_star 算法雖然可以實現未知環境的路徑規劃，但效率較低，基於 LPA_star 的D_star Lite可以很好的應對環境未知的情況，其算法核心在於假設了未知區域都是自由空間，以此爲基礎，增量式地實現路徑規劃，通過最小化rhs值找到目標點到各個節點的最短距離。在移動機器人按照規劃的路徑進行前進時其所到的節點即設置爲起始節點，因此路徑變化或者key值需要更新時，需要更新從目標點到新起點的啓發值以及估計成本。由於移動機器人不斷的靠近目標點，節點的啓發值將不斷減少，且減少至不會超過h（start Org,start New）。由於每次都要減去相同的值，開啓列表的順序並不會改變，因此可以不進行這部分的計算，這樣便避免了每次路徑改變時的隊列遍歷過程。
若前行過程中發現障礙物則將障礙物所對應環境地圖位置設置爲障礙物空間，並再以之爲起點利用“路徑場”信息重新規劃出一條路徑來。此時不僅更新規劃路徑的節點數據，也要更新智能體遍歷過的節點。其關鍵點在於如何在未知的環境中根據傳感器獲取的極少周邊地圖信息來進行最有效的靠近目標點的任務。其實整個靠近的過程一直在擴大已知地圖範圍，儘可能少的嘗試次數來實現完成抵達目標點的任務。下圖爲 D_star Lite 搜索示意圖，黑點是在按照反向搜索的路徑執行時發現的障礙點，到遇到不能通行的障礙點後便更新地圖信息，重新規劃出一條新的路徑繼續前行。
D_star Lite 算法的原理類似 D_star ，起初需要根據已知的環境信息，未知部分視作自由空間，規劃出從目標點到起始點的全局最優路徑，此時即建立了一個“路徑場”信息，爲增量靠近目標點提供擇優依據。D_star Lite算法是反向搜索的，因此LPA_star 裏的g(s)，h(s)有了新的定義，即分別代表從目標點到當前s點的代價值以及當前s點到出發點的啓發值。與LPA_star 中相反，g*(s) 記錄柵格節點的前繼節點，計算式爲
rhs(s)記錄柵格節點的後繼節點的g(s)，有公式：
在評價柵格點的估價值時 D_star Lite 也引入了 k(s) 值進行比較，其中 k(s) 包含兩個值[k(s1); k(s2)] ，分別滿足以下公式：
與 LPA_star算法相對應，可以很容易得出以下公式：
2.D* Lite算法僞代碼
D* Lite 路徑規劃算法描述如下表:
3.D*Lite算法一個簡單的例子
3.1 地圖無變化時
以一個具體例子來講解算法的工作流程：
首先計算出由起始點B1→目標點E3的啓發值h如上圖。h的值近似於從起始網格到當前網格的這兩個網格的x和y座標的絕對差異的最大值。D*Lite算法具體的過程如下圖。
在初始化Initialization階段，將所有可行的網格中的g=rhs=∞，即執行算法2-6步。其中k=[k_1;k_2]按照公式計算即可。
- 在第一步step1中，即要計算E3周圍的點，D2、E2和D3。以D3爲例計算，先按公式計算rhs(s)=min(g(s’)+c(s,s’))=1，再計算k[k_1;k_2]=[3;1]。由此可以計算D2、E2點如圖。比較key值，選擇最小的一個，即爲D2。D2的g值按照公式min_s(c(s’,s)+g(s’)=0+1=1更新爲1，再繼續計算。
- 在第二步step2中，需要以D2爲中心，對C1，C2，C3，D1，D2，D3，E1，E2，E3都進行計算。按照第一步step1的計算方法，各值計算如圖。其中D3的key爲[3;1]最小（最優），故選D3爲進一步擴展點。
- 在第三步step3中，需要以D3爲中心，對C2，C3，D2，D3，E2，E3都進行計算。各值計算如圖，其中C1的key爲[3;2]最小（最優），故選C1爲進一步擴展點。
- 在第四步step4中，需要以C1爲中心，對B1，B2，C2，D1，D2都進行計算。各值計算如圖，其中B1的key爲[3;3]最小（最優），故選B1爲進一步擴展點。
- 在第五步step5中，需要以B1爲中心，對A1，A2，B2，C1，C2都進行計算。各值計算如圖，A1爲起點，算法結束。
因此路徑爲A1→B1→C1→D2→E3。
3.2地圖變化時
當地圖中的一個點D2變爲障礙物時，起始點變爲B1，位置爲C1，目標點仍爲E3，啓發值h變爲如下圖：
此時，k_m=k_m+h(s_last,s_start)=0+1=1,更改各點的啓發值h如上圖（算法28-31行）。D*Lite算法更新如下圖：
首先是計算變化的cost，即Edge Cost Changes步。改變的爲C2。其根據計算式rhs(s)=min_s Succ(s)(c(s’,s)+g(s’)=1+3=4。此時g(s)小於rhs(s)爲欠一致性，即遇到障礙物，g(u)=∞，更新所有的前繼節點（算法19-20行）。因此從step1開始按照之前的路徑重新搜索，但此時不需要再計算太多其他的網格。所以，回到E2開始搜索（step1），進而在D1處搜索（step2），再到C1處搜索，進而達到當前的機器人位置，終止搜索。
因此更新的路徑爲C1→D1→E2→E3。
4.算法總結
D_star Lite結合了D_star 算法動態規劃的特性（由目標位置開始向起始位置進行路徑搜索。當路徑中存在新的障礙時，對於目標位置到新障礙之間的範圍內的路徑節點，新的障礙是不會影響到其到目標的路徑的）和LPA_star 算法的利用增量式搜索特性。
D_star Lite算法能夠很好的適用於未知環境做路經規劃，由於其增量規劃的思想，它可以做到較少重規劃次數以及較少的重規劃影響節點數。但是當狀態空間比較大，也就是環境地圖比較大的時候，採用的 D_star Lite 路徑規劃算法的反向搜索過程需要維護的柵格節點數急劇增加，增加了搜索的時間複雜度。除此之外，D_star Lite 路徑規劃不能應對環境複雜的情況，即局部環境的精細規劃。對於大環境下的路徑規劃，D_star Lite 算法的做法是將環境地圖進行更細粒度的柵格化，雖然在足夠細粒化的環境地圖中可以實現較優的路徑解，但也會帶來更多的規劃序列導致執行次數以及重規劃次數增多，進而路徑規劃執行花費時間也會變得更長。
參考資料
[1]路徑規劃——D* Lite算法
[2]LPA和DLite算法
[3]Koenig S, Likhachev M. Fast replanning for navigation in unknown terrain[J]. IEEE Transactions on Robotics, 2005, 21(3): 354-363.
[4]徐開放. 基於D*Lite算法的移動機器人路徑規劃研究[D]. 哈爾濱工業大學, 2017.
搜索算法其他文章
Field Ｄstar路徑規劃算法
終身規劃Astar算法（LPA*）：Lifelong Planning A*
D*路徑搜索算法原理解析及Python實現