隨裕猛，陳賢富，劉斌

（中國科學(xué)技術(shù)大學(xué)信息科學(xué)技術(shù)學(xué)院，安徽合肥230027）

D-star Lite算法及其動態(tài)路徑規(guī)劃實驗研究

隨裕猛，陳賢富，劉斌

（中國科學(xué)技術(shù)大學(xué)信息科學(xué)技術(shù)學(xué)院，安徽合肥230027）

車輛導(dǎo)航系統(tǒng)的核心是路徑規(guī)劃算法，路徑規(guī)劃算法分靜態(tài)路徑規(guī)劃（Static Path Planning，SPP）算法和動態(tài)路徑規(guī)劃（Dynamic Path Planning，DPP）算法，SPP的不足是不能對實時變化交通信息做出快速響應(yīng)，而DPP則可以利用路網(wǎng)中實時更新的交通信息及時地為駕駛者提供更佳的導(dǎo)航路線。本文在研究了靜態(tài)路徑規(guī)劃中用到的一些算法后，如A*算法，繼而分析動態(tài)路徑規(guī)劃的一些思想，在此基礎(chǔ)上分析D*Lite算法可以改進(jìn)的地方，并給出優(yōu)化后的算法程序。利用10×10、50×50、100×100三種規(guī)模的模擬路網(wǎng)做對比實驗，實驗表明優(yōu)化后的D*Lite算法在速度上有了較大提高。

動態(tài)路徑規(guī)劃；A*；D*；LPA*；D*Lite

0 引言

生成導(dǎo)航路徑的算法有多種，其中經(jīng)典算法之一便是Dijkstra算法[1]，該算法也是其他眾多算法的基礎(chǔ)。為提高求解最優(yōu)路徑的效率，研究者們相繼提出了多種快速算法，其中A*算法[2-3]是其中較重要的一種算法，它采用啟發(fā)式搜索的方式，不再像Dijkstra算法盲目式搜索，可使搜索范圍明顯縮小，也使導(dǎo)航效率得到提高；雙向搜索（Bidirectional Search）[4]也是一種快速搜索方式，它采用從起點和目標(biāo)點同時開始搜索的策略，理想情況下會在路徑的中點處相遇，從而終止搜索過程；分層技術(shù)（Hierarchical Methods）[5-6]采用預(yù)處理的辦法使待搜索的路網(wǎng)維度降低，從而達(dá)到快速搜索的目的。

交通信息是動態(tài)變化的，如某個路段此時擁堵，或暢通，或限行等，在下一時刻此路段信息可能又發(fā)生了變化，為應(yīng)對這種情況，當(dāng)需要為駕駛者及時更新導(dǎo)航路徑時，簡單重復(fù)調(diào)用靜態(tài)導(dǎo)航算法并不是最優(yōu)的選擇。Anthony Stentz在1994年提出了D*（Dynamic A*）[7-8]算法，即動態(tài)A*算法，該算法的最初目的是解決機(jī)器人在不確定環(huán)境下的尋路問題。Koenig和Likhachev在2004年提出LPA*算法[9]，該算法受人工智能領(lǐng)域的“增量搜索”（Incremental Search）思想啟發(fā)，通過復(fù)用先前搜索產(chǎn)生的信息，從而達(dá)到可以快速重新規(guī)劃最優(yōu)路徑的目的。LPA*算法解決的是定起點、定目標(biāo)點的尋路問題，為應(yīng)對變起點、定目標(biāo)點問題，Koenig和Likhachev在LPA*算法的基礎(chǔ)上又提出了D*Lite[10]算法。2011年K Al-Mutib等人又將D*Lite算法應(yīng)用于多機(jī)器（Multi-Agent）的實時動態(tài)路徑規(guī)劃[11]。本文通過對D*Lite算法分析，發(fā)現(xiàn)該算法在執(zhí)行過程中有些計算是可以避免的，從而可以使算法效率更高。

1 A＊算法

A*算法的核心在于估價函數(shù)的設(shè)計上，如式（1）所示：

f（n）=g（n）+h（n）（1）其中g(shù)（n）稱為耗散函數(shù)，表示從起始節(jié)點nstart到節(jié)點n的實際代價；h（n）稱為啟發(fā)函數(shù)，表示節(jié)點n到目標(biāo)節(jié)點ngoal的估計代價；f（n）表示從起始節(jié)點經(jīng)由節(jié)點n到目標(biāo)節(jié)點的估計代價。

同Dijkstra算法類似，A*算法也維持一個Open表。Open表中節(jié)點的優(yōu)先級是依據(jù)f（n）的大小排列的，f（n）值越小，被搜索到的優(yōu)先級越高。為保證能搜索到最優(yōu)解，啟發(fā)函數(shù)h（n）不能太大，不能大于節(jié)點n到目標(biāo)節(jié)點的實際代價；但如果h（n）=0，則A*算法退化為Dijkstra算法，雖能保證得到最優(yōu)路徑，但算法效率低；如果h（n）恰好等于節(jié)點n到目標(biāo)節(jié)點的實際代價，則A*算法探索的節(jié)點恰好就是最優(yōu)路徑上的節(jié)點。所以h（n）的取值直接影響算法的速度和精確度，常見的h（n）的取值有兩點之間的歐幾里得距離（Euclidean Distance）和曼哈頓距離（Manhattan Distance）等。

圖1所示為h（n）的大小對搜索空間的影響對比圖。

2 D＊Lite算法

D*Lite算法是Koenig S和Likhachev M在LPA*算法的基礎(chǔ)上提出的。LPA*算法，即Lifelong Planning A*算法，基于A*算法和Dynamic SWSF-FP算法[12]的思想，可以在環(huán)境變化時快速求得最優(yōu)路徑。但LPA*算法是為求解定起點和定終點之間最優(yōu)路徑問題而設(shè)計，不適用于像車輛導(dǎo)航這種車輛位置變化的情景。為此，Koenig S和Likhachev M通過對LPA*算法改造，使LPA*算法的思想能應(yīng)用到諸如車輛動態(tài)導(dǎo)航這樣的問題。

LPA*算法區(qū)別于其他算法的一個重要特點是rhs（v）的定義，如式（2）：

其中pred（v）表示節(jié)點v的前繼節(jié)點，g（u）是節(jié)點u到起始節(jié)點vstart的代價，類似于A*算法中的g（n），c（u，v）表示從節(jié)點u到節(jié)點v的代價。對于節(jié)點v，如果g（v）=rhs（v），則稱該節(jié)點“連續(xù)”（Consistent），否則稱“不連續(xù)”（Nonconsistent）。當(dāng)所有節(jié)點連續(xù)時，說明g（v）真實代表節(jié)點v到起始節(jié)點的代價。

D*Lite算法繼承了rhs（v）的概念，但D*Lite算法是從目標(biāo)節(jié)點向起始節(jié)點搜索，這一點和D*算法相同，和LPA*、A*算法相反，此時rhs（v）的定義如式（3）：

succ（v）表示節(jié)點v的后續(xù)節(jié)點，此時g（u）表示節(jié)點u到目標(biāo)節(jié)點到代價。D*Lite和LPA*算法的不同之處還在于當(dāng)環(huán)境變化后，節(jié)點的啟發(fā)函數(shù)值的處理。如前所述，LPA*算法解決的是起點固定、目標(biāo)點固定的最優(yōu)路徑搜索問題，節(jié)點v的啟發(fā)函數(shù)值不是動態(tài)變化的；然而，D*Lite算法面向的是起點（如車輛位置）隨時間變化、目標(biāo)點固定的最優(yōu)路徑搜索問題，節(jié)點v的啟發(fā)函數(shù)值是隨著起點位置的變化而變化的。為此，Koenig S和Likhachev M在參考文獻(xiàn)[11]中給出了兩種解決方法：一是，根據(jù)新的起點位置，將優(yōu)先隊列（Priority queue）中所有節(jié)點的啟發(fā)函數(shù)值重新計算；二是，并不重新計算隊列中的啟發(fā)函數(shù)值，而是在計算新添加到優(yōu)先隊列中的節(jié)點的啟發(fā)函數(shù)值時，加上一個修飾符km，km表示車輛或機(jī)器人移動距離的疊加。

3 D＊Lite Label算法

通過分析D*Lite算法，發(fā)現(xiàn)該算法仍然存在一些可以改進(jìn)的地方。

（1）初始化時無需對網(wǎng)絡(luò)中所有節(jié)點都進(jìn)行初始化，因為采用啟發(fā)式搜索，有些節(jié)點根本就不會被搜索到。

（2）在判斷某節(jié)點是否存在于優(yōu)先列表中時，如果遍歷整個表，則效率并不是最優(yōu)的。

（3）在更新節(jié)點v的rhs（v）時，在某些情況下并不需要探索它的所有后繼節(jié)點。如果v是連續(xù)節(jié)點，它的某個后繼節(jié)點u觸發(fā)了v的更新程序，此時只需比較rhs（v）和g（u）+c（v，u）的大小。

（4）當(dāng)路徑規(guī)劃結(jié)束后，機(jī)器人或車輛要向下一個節(jié)點運(yùn)動時，D*Lite算法采用貪婪搜索的方式尋找下一個節(jié)點。令u表示當(dāng)前節(jié)點v的一個后繼節(jié)點，如果g（u）+c（v，u）最小，則該后繼節(jié)點就是下一步要移向的節(jié)點。該策略仍然需要探索當(dāng)前節(jié)點的所有后繼節(jié)點。

針對上述問題，參考D*算法的設(shè)計，本文采用為節(jié)點設(shè)置標(biāo)號v.Tag的方式和父節(jié)點屬性v.Father的方式進(jìn)行優(yōu)化。為區(qū)別經(jīng)典D*Lite算法，本文將下述算法定義為D*Lite Label算法。

圖1 h（n）對搜索空間的影響對比

定義有向圖G=（V，E），其中V表示節(jié)點集合，E表示邊的集合，e（u，v）∈E表示有向邊u→v，c（u，v）表示e（u，v）的權(quán)值，限定c（u，v）≥0。Succ（v）表示節(jié)點v的后繼節(jié)點集合，u∈Succ（v）表示存在有向邊e（v，u）；Pred（v）表示節(jié)點v的前續(xù)節(jié)點集合，u∈Pred（v）表示存在有向邊e（u，v）。為節(jié)點v設(shè)置父節(jié)點屬性v.Father，如果v.Father=u，則表示在最優(yōu)路徑上v的下一節(jié)點是u。類似于A*算法中的Open表和Close表，D*Lite算法用一個優(yōu)先隊列Queue來保存等待更新的節(jié)點，本文仍然沿用優(yōu)先隊列Queue這個概念。另外，本文還為每個節(jié)點v設(shè)置標(biāo)號v.Tag屬性，如果v.Tag=NEW，則表示該節(jié)點還未曾被搜索到；如果v.Tag=OPEN，則表示該節(jié)點等待更新且已存入Queue隊列中；如果v.Tag= CLOSED，則表示該節(jié)點已經(jīng)從Queue中移除。用v.g、v. rhs、v.h分別代表D*Lite算法中的g（v）、rhs（v）、h（vstart，v）。

先對程序進(jìn)行初始化，StartV表示車輛起始位置節(jié)點，GoalV表示目標(biāo)節(jié)點。

Initial（）{

L1/StartV.rhs=StartV.g=∞；GoalV.rhs=0；

L2/GoalV.Tag=OPEN；Queue.Add（GoalV）；

L3}

程序運(yùn)行中，Queue.Top（）函數(shù)返回Queue中Key值最小的節(jié)點，Key的取值與D*Lite算法一致，Key=[min（v.g，v.rhs）+v.h+km，min（v.g，v.rhs）]，函數(shù)Cal_Key（v）用于計算節(jié)點v的Key值。CurrV表示車輛當(dāng)前位置節(jié)點。Stentz在參考文獻(xiàn)[7]描述D*算法時將節(jié)點狀態(tài)分為兩類，一類處于“下降狀態(tài)”（LOWER state），一類處于“上升狀態(tài)”（RAISE state）。針對兩種狀態(tài)節(jié)點，本文創(chuàng)新性地采用兩種更新策略。當(dāng)TopV.g>TopV.rhs時，節(jié)點處于下降狀態(tài)，調(diào)用Update_Lower（u，SourceV）函數(shù)對TopV的前續(xù)節(jié)點進(jìn)行更新，u表示待更新節(jié)點，SourceV表示觸發(fā)u被更新的源節(jié)點；當(dāng)TopV.g＜TopV.rhs時，節(jié)點處于上升狀態(tài)，調(diào)用Update_Raise（u）對TopV的前續(xù)節(jié)點進(jìn)行更新。

ComputeShortestPath（CurrV）{

L1/TopV=Queue.Top（）；

L2/while（TopV.Key＜CurrV.Key

L3/||CurrV.rhs!=CurrV.g）{

L4 if（TopV.g>TopV.rhs）{

L5/TopV.g=TopV.rhs；

L6/TopV.Tag=CLOSED；Queue.Remove（TopV）；

L7/for all u∈Pred（TopV）

L8/Update_Lower（u，TopV）；}

L9/else{

L10/TopV.g=∞；

L11/for all u∈Pred（TopV）

L12/Update_Raise（u）；}

L13/TopV=Queue.Top（）；

L14}}

Update_Lower（u，SourceV）{

L1switch（u.Tag）{

L2/case NEW：

L3/u.rhs=SouceV.g+c（u，SourceV）；Cal_Key（u）；

L4u.Father=SouceV；u.Tag=OPEN；Queue.Add（u）；

L5/case OPEN：

L6/if（u.rhs>SourceV.g+c（u，SourceV））{

L7/u.rhs=SourceV.g+c（u，SourceV）；

L8u.Father=SouceV；Cal_Key（u）；}

L9/case CLOSED：

L10/if（u.rhs>SourceV.g+c（u，SourceV）

L11/||u.Father=SourceV）{

L12/u.rhs=SourceV.g+c（u，SourceV）；Cal_Key（u）；

L13/u.Father=SouceV；u.Tag=OPEN；Queue.Add（u）；}

L14}}

Update_Raise（u）{

L1if（u!=GoalV）{

L2/for all v∈Succ（u）{

L3/if（v.Tag==CLOSED&&u.rhs>v.g+c（u，v））{

L4/u.rhs=v.g+c（u，v）；u.Father=v；}

L5/}

L6/if（u.rhs!=u.g&&u.Tag!=OPEN）{

L7/u.Tag=OPEN；Cal_Key（u）；Queue.Add（u）；}

L8/if（u.rhs==u.g&&u.Tag==OPEN）{

L9/u.Tag=CLOSED；Queue.Remove（u）；}

L10/}}

程序運(yùn)行的主程序同D*Lite算法基本一致，稍微不同的一點是，當(dāng)最后更新節(jié)點時需判斷該節(jié)點是處于上升狀態(tài)還是下降狀態(tài)，然后采用相應(yīng)的更新函數(shù)，主程序其余部分此處不再贅述，請見參考文獻(xiàn)[11]。

4 實驗結(jié)果

本文分別采用10×10、50×50、100×100的方陣圖模擬路網(wǎng)，每條邊代表一條路，每條邊的權(quán)值為1～5之間的均勻隨機(jī)整數(shù)，起始點和目標(biāo)點為網(wǎng)絡(luò)中的交叉點，位置隨機(jī)決定。啟發(fā)函數(shù)采用兩點之間的曼哈頓距離。當(dāng)起始點和目標(biāo)點的位置確定后，分別用A*算法、D*Lite、D*Lite Label三種算法規(guī)劃最優(yōu)路徑。

（1）為模擬車輛位置的動態(tài)變化，本文在先前規(guī)劃好的路徑上，產(chǎn)生一個隨機(jī)位置作為車輛當(dāng)前位置。

（2）為模擬路網(wǎng)環(huán)境的變化，本文在車輛當(dāng)前位置和目標(biāo)節(jié)點之間的路徑上產(chǎn)生一個隨機(jī)“阻塞”，置該條邊的權(quán)值為無窮大。

當(dāng)阻塞發(fā)生后，分別采用A*、D*Lite、D*Lite Label三種算法對路徑重新規(guī)劃，統(tǒng)計每種算法所探索的節(jié)點數(shù)、所用時間。本文的A*算法同樣采用了標(biāo)號的方式。在三種規(guī)模的路網(wǎng)下做1 000次實驗，統(tǒng)計其平均值，實驗環(huán)境為Intel i5 CPU，主頻2.6 GHz，8 GB內(nèi)存，仿真平臺為Visual Studio 2010，得到的實驗結(jié)果如表1、表2、表3所示。

5 結(jié)論

實驗結(jié)果顯示，隨著路網(wǎng)規(guī)模的增大，動態(tài)路徑規(guī)劃算法與靜態(tài)路徑規(guī)劃算法的重復(fù)調(diào)用相比，其優(yōu)勢更加突出。D*Lite Label算法基于D*Lite算法的思想，在所探索的節(jié)點數(shù)方面，兩種算法基本一致。由于D*Lite Label算法為每個節(jié)點增加了一些屬性，避免了某些節(jié)點被反復(fù)更新，且同時使更新過程更加快速，使得該算法在時間效率上更優(yōu)。

[1]DIJKSTRA E W.A note on two problems in connexion with graphs[J].Numerische Mathematik，1959，1（1）：269-271.

[2]NILSSON N J.Principles of artificial intelligence[M].Berlin：Springer：1982.

[3]HART P E，NILSSON N J，RAPHAEL B.A formal basis for the heuristic determination of minimum cost paths[J]. Systems Science and Cybernetics，IEEE Transactions on，1968，4（2）：100-107.

[4]LUBY M，RAGDE P.A bidirectional shortest-path algorithm with good average-case behavior[J].Algorithmica，1989，4（1-4）：551-567.

[5]SCHULZ M H F，WAGNERT D.Engineering multi-level overlay graphs for shortest-path queries′[C].Proceedings of the Eighth Workshop on Algorithm Engineering and Experiments and the Third Workshop on Analytic Algorithmics and Combinatorics，SIAM，2006：123-156.

[6]SANDERS P，SCHULTES D.Algorithms-ESA 2006[M]. Berlin Heidelberg Springer，2006.

[7]STENTZ A.Optimal and efficient path planning for partially-known environments[C].Robotics and Automation，1994. Proceedings of 1994 IEEE International Conference on. IEEE，1994：3310-3317.

[8]STENTZ A.The focussed D^*algorithm for real-time replanning[C].IJCAI.1995，95：1652-1659.

[9]KOENIG S，LIKHACHEV M，F(xiàn)URCY D.Lifelong planning A*[J].Artificial Intelligence，2004，155（1）：93-146.

[10]KOENIG S，LIKHACHEV M.Fast replanning for navigation in unknown terrain[J].Robotics，IEEE Transactions on，2005，21（3）：354-363.

[11]AL-MUTIB K，ALSULAIMAN M，EMADUDDIN M，et al. D*Lite based real-time multi-agent path planning in dynamic environments[C].Computational Intelligence，Modelling and Simulation（CIMSiM），2011 Third International Conference on.IEEE，2011：170-174.

[12]RAMALINGAM G，REPS T.An incremental algorithm for a generalization of the shortest-path problem[J].Journal of Algorithms，1996，21（2）：267-305.

表1 10×10方陣圖下的實驗結(jié)果對比

[3]宋牟平，陳翔.基于實時小波變換信號處理的相干檢測布里淵光時域反射計[J].光學(xué)學(xué)報，2009，29（10）：2818-2821.

[4]FARAHANI M A，WYLIE M T V，CASTILLO-GUERRA E，et al.Reduction in the number of averages required in BOTDA sensors using wavelet denoising techniques[J].Journal of Lightwave Technology，2012，30（8）：1134-1142.

[5]李星容，李永倩，張碩.同步疊加平均算法抑制噪聲的Labview實現(xiàn)[J].華北電力大學(xué)學(xué)報，2009，36（4）：74-76.

[6]周倩婷，危峻，徐志鵬.噪聲特性對多次采集累加平均技術(shù)的影響[J].紅外與激光工程，2010，39（5）：959-962.

[7]楊鶯，韋育森.提取周期微弱信號的幾種軟件實現(xiàn)方法[J].測控技術(shù)，2008，27（9）：22-24.

[8]張弦，王宏力.進(jìn)化小波消噪方法及其在滾動軸承故障診斷中的應(yīng)用[J].機(jī)械工程學(xué)報，2010，46（15）：76-81.

[9]田玉靜，左紅偉.小波消噪閾值算法優(yōu)化[J].聲學(xué)技術(shù)，2009，28（4）：503-506.

[10]宋牟平，趙斌.希爾伯特變換處理的布里淵散DOFS的研究[J].光子學(xué)報，2005，34（9）：1328-1331.

（收稿日期：2014-12-09）

作者簡介：

呂媛（1989-），通信作者，女，碩士研究生，主要研究方向：光纖傳感技術(shù)。E-mail：15577330762@163.com。

秦祖軍（1978-），男，博士，副教授，主要研究方向：光纖傳感、光纖技術(shù)與光纖通信。

D-star Lite algorithm and its experimental study on dynamic path planning

Sui Yumeng，Chen Xianfu，Liu Bin
（School of Information Science and Technology，University of Science and Technology of China，Hefei 230027，China）

The core of vehicle navigation system is the path planning algorithm，and the path planning algorithm is divided into two types，the SPP（Static Path Planning）and the DPP（Dynamic Path Planning）.The shortage of SPP is that it cannot make quick response to the real-time changing traffic information.And the DPP can provide better route timely for the drivers when the traffic information has been updated.This paper studies some classic algorithms for SPP，like A*，then analyzes the thoughts of dynamic path planning algorithms.Based on those，this paper analyzes the D*Lite algorithm and indicates the places that can be optimized，then introduces the procedure of the optimized D*Lite.This paper performs three sets of experiments on the simulate traffic network of size 10×10，50×50，100×100，and the experiments results show that the optimized D*Lite algorithm gets greatly improved in terms of speed.

dynamic path planning；A*；D*；LPA*；D*Lite

TP391；U491

：A

：1674-7720（2015）07-0016-04

2014-12-04）

隨裕猛（1988-），男，碩士研究生，主要研究方向：智能信息處理。

陳賢富（1963-），男，博士，副教授，主要研究方向：復(fù)雜系統(tǒng)與計算智能。

劉斌（1992-），男，碩士研究生，主要研究方向：智能信息處理。反射儀[J].中國激光，2012，39（8）：1-5.