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

　　關鍵詞： 動態(tài)路徑規(guī)劃；A*,；D*,；LPA*；D*Lite

0 引言

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

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

1 A*算法

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

　　f（n）=g（n）+h（n）（1）

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

　　同Dijkstra算法類似,，A*算法也維持一個Open表,。Open表中節(jié)點的優(yōu)先級是依據(jù)f（n）的大小排列的,，   f（n）值越小,，被搜索到的優(yōu)先級越高,。為保證能搜索到最優(yōu)解，啟發(fā)函數(shù)h（n）不能太大,，不能大于節(jié)點n到目標節(jié)點的實際代價,；但如果h（n）=0，則A*算法退化為Dijkstra算法,，雖能保證得到最優(yōu)路徑,，但算法效率低；如果h（n）恰好等于節(jié)點n到目標節(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*算法的基礎上提出的。LPA*算法,，即Lifelong Planning A*算法,，基于A*算法和Dynamic SWSF-FP算法[12]的思想，可以在環(huán)境變化時快速求得最優(yōu)路徑,。但LPA*算法是為求解定起點和定終點之間最優(yōu)路徑問題而設計,，不適用于像車輛導航這種車輛位置變化的情景。為此,，Koenig S和Likhachev M通過對LPA*算法改造,，使LPA*算法的思想能應用到諸如車輛動態(tài)導航這樣的問題。

　　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）,。當所有節(jié)點連續(xù)時,，說明g（v）真實代表節(jié)點v到起始節(jié)點的代價。

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

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

3 D*Lite Label算法

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

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

　?。?）在判斷某節(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）當路徑規(guī)劃結束后,，機器人或車輛要向下一個節(jié)點運動時,，D*Lite算法采用貪婪搜索的方式尋找下一個節(jié)點。令u表示當前節(jié)點v的一個后繼節(jié)點,，如果g（u）+c（v，u）最小,，則該后繼節(jié)點就是下一步要移向的節(jié)點,。該策略仍然需要探索當前節(jié)點的所有后繼節(jié)點。

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

　　定義有向圖G=（V，E）,，其中V表示節(jié)點集合,，E表示邊的集合，e（u,，v）∈E表示有向邊u→v,，c（u，v）表示e（u,，v）的權值,，限定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設置父節(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設置標號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）,。

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

　　Initial（）{

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

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

　　L3\}

　　程序運行中,，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表示車輛當前位置節(jié)點。Stentz在參考文獻[7]描述D*算法時將節(jié)點狀態(tài)分為兩類,，一類處于“下降狀態(tài)”（LOWER state）,，一類處于“上升狀態(tài)”（RAISE state）。針對兩種狀態(tài)節(jié)點,，本文創(chuàng)新性地采用兩種更新策略,。當TopV.g>TopV.rhs時，節(jié)點處于下降狀態(tài),，調用Update_Lower（u,，SourceV）函數(shù)對TopV的前續(xù)節(jié)點進行更新，u表示待更新節(jié)點,，SourceV表示觸發(fā)u被更新的源節(jié)點,；當TopV.g<TopV.rhs時，節(jié)點處于上升狀態(tài)，調用Update_Raise（u）對TopV的前續(xù)節(jié)點進行更新,。

　　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） {

　　L1\ switch （u.Tag）{

　　L2/ case NEW：

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

　　L4\   u.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）；

　　L8\    u.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） {

　　L1\ if（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/ } }

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

4 實驗結果

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

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

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

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

5 結論

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