摘 要： 車輛導(dǎo)航系統(tǒng)的核心是路徑規(guī)劃算法，路徑規(guī)劃算法分靜態(tài)路徑規(guī)劃（Static Path Planning， SPP）算法和動態(tài)路徑規(guī)劃（Dynamic Path Planning， DPP）算法，SPP的不足是不能對實(shí)時變化交通信息做出快速響應(yīng)，而DPP則可以利用路網(wǎng)中實(shí)時更新的交通信息及時地為駕駛者提供更佳的導(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)做對比實(shí)驗(yàn)，實(shí)驗(yàn)表明優(yōu)化后的D*Lite算法在速度上有了較大提高。
關(guān)鍵詞： 動態(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]也是一種快速搜索方式，它采用從起點(diǎn)和目標(biāo)點(diǎn)同時開始搜索的策略，理想情況下會在路徑的中點(diǎn)處相遇，從而終止搜索過程；分層技術(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*算法解決的是定起點(diǎn)、定目標(biāo)點(diǎn)的尋路問題，為應(yīng)對變起點(diǎn)、定目標(biāo)點(diǎn)問題，Koenig和Likhachev在LPA*算法的基礎(chǔ)上又提出了D*Lite[10]算法。2011年K Al-Mutib等人又將D*Lite算法應(yīng)用于多機(jī)器（Multi-Agent）的實(shí)時動態(tài)路徑規(guī)劃[11]。本文通過對D*Lite算法分析，發(fā)現(xiàn)該算法在執(zhí)行過程中有些計(jì)算是可以避免的，從而可以使算法效率更高。
1 A*算法
A*算法的核心在于估價(jià)函數(shù)的設(shè)計(jì)上，如式（1）所示：
f（n）=g（n）+h（n）（1）
其中g(shù)（n）稱為耗散函數(shù)，表示從起始節(jié)點(diǎn)nstart到節(jié)點(diǎn)n的實(shí)際代價(jià)；h（n）稱為啟發(fā)函數(shù)，表示節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)ngoal的估計(jì)代價(jià)；f（n）表示從起始節(jié)點(diǎn)經(jīng)由節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)。
同Dijkstra算法類似，A*算法也維持一個Open表。Open表中節(jié)點(diǎn)的優(yōu)先級是依據(jù)f（n）的大小排列的， f（n）值越小，被搜索到的優(yōu)先級越高。為保證能搜索到最優(yōu)解，啟發(fā)函數(shù)h（n）不能太大，不能大于節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的實(shí)際代價(jià)；但如果h（n）=0，則A*算法退化為Dijkstra算法，雖能保證得到最優(yōu)路徑，但算法效率低；如果h（n）恰好等于節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的實(shí)際代價(jià)，則A*算法探索的節(jié)點(diǎn)恰好就是最優(yōu)路徑上的節(jié)點(diǎn)。所以h（n）的取值直接影響算法的速度和精確度，常見的 h（n）的取值有兩點(diǎ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*算法是為求解定起點(diǎn)和定終點(diǎn)之間最優(yōu)路徑問題而設(shè)計(jì)，不適用于像車輛導(dǎo)航這種車輛位置變化的情景。為此，Koenig S和Likhachev M通過對LPA*算法改造，使LPA*算法的思想能應(yīng)用到諸如車輛動態(tài)導(dǎo)航這樣的問題。
LPA*算法區(qū)別于其他算法的一個重要特點(diǎn)是rhs（v）的定義，如式（2）：
其中pred（v）表示節(jié)點(diǎn)v的前繼節(jié)點(diǎn)，g（u）是節(jié)點(diǎn)u到起始節(jié)點(diǎn)vstart的代價(jià)，類似于A*算法中的g（n），c（u，v）表示從節(jié)點(diǎn)u到節(jié)點(diǎn)v的代價(jià)。對于節(jié)點(diǎn)v，如果 g（v）=rhs（v），則稱該節(jié)點(diǎn)“連續(xù)”（Consistent），否則稱“不連續(xù)”（Nonconsistent）。當(dāng)所有節(jié)點(diǎn)連續(xù)時，說明g（v）真實(shí)代表節(jié)點(diǎn)v到起始節(jié)點(diǎn)的代價(jià)。
D*Lite算法繼承了rhs（v）的概念，但D*Lite算法是從目標(biāo)節(jié)點(diǎn)向起始節(jié)點(diǎn)搜索，這一點(diǎn)和D*算法相同，和LPA*、A*算法相反，此時rhs（v）的定義如式（3）：
succ（v）表示節(jié)點(diǎn)v的后續(xù)節(jié)點(diǎn)，此時g（u）表示節(jié)點(diǎn)u到目標(biāo)節(jié)點(diǎn)到代價(jià)。D*Lite和LPA*算法的不同之處還在于當(dāng)環(huán)境變化后，節(jié)點(diǎn)的啟發(fā)函數(shù)值的處理。如前所述，LPA*算法解決的是起點(diǎn)固定、目標(biāo)點(diǎn)固定的最優(yōu)路徑搜索問題，節(jié)點(diǎn)v的啟發(fā)函數(shù)值不是動態(tài)變化的；然而，D*Lite算法面向的是起點(diǎn)（如車輛位置）隨時間變化、目標(biāo)點(diǎn)固定的最優(yōu)路徑搜索問題，節(jié)點(diǎn)v的啟發(fā)函數(shù)值是隨著起點(diǎn)位置的變化而變化的。為此，Koenig S和Likhachev M在參考文獻(xiàn)[11]中給出了兩種解決方法：一是，根據(jù)新的起點(diǎn)位置，將優(yōu)先隊(duì)列（Priority queue）中所有節(jié)點(diǎn)的啟發(fā)函數(shù)值重新計(jì)算；二是，并不重新計(jì)算隊(duì)列中的啟發(fā)函數(shù)值，而是在計(jì)算新添加到優(yōu)先隊(duì)列中的節(jié)點(diǎn)的啟發(fā)函數(shù)值時，加上一個修飾符km，km表示車輛或機(jī)器人移動距離的疊加。
3 D*Lite Label算法
通過分析D*Lite算法，發(fā)現(xiàn)該算法仍然存在一些可以改進(jìn)的地方。
（1）初始化時無需對網(wǎng)絡(luò)中所有節(jié)點(diǎn)都進(jìn)行初始化，因?yàn)椴捎脝l(fā)式搜索，有些節(jié)點(diǎn)根本就不會被搜索到。
（2）在判斷某節(jié)點(diǎn)是否存在于優(yōu)先列表中時，如果遍歷整個表，則效率并不是最優(yōu)的。
?。?）在更新節(jié)點(diǎn)v的rhs（v）時，在某些情況下并不需要探索它的所有后繼節(jié)點(diǎn)。如果v是連續(xù)節(jié)點(diǎn)，它的某個后繼節(jié)點(diǎn)u觸發(fā)了v的更新程序，此時只需比較rhs（v）和g（u）+c（v，u）的大小。
（4）當(dāng)路徑規(guī)劃結(jié)束后，機(jī)器人或車輛要向下一個節(jié)點(diǎn)運(yùn)動時，D*Lite算法采用貪婪搜索的方式尋找下一個節(jié)點(diǎn)。令u表示當(dāng)前節(jié)點(diǎn)v的一個后繼節(jié)點(diǎn)，如果g（u）+c（v，u）最小，則該后繼節(jié)點(diǎn)就是下一步要移向的節(jié)點(diǎn)。該策略仍然需要探索當(dāng)前節(jié)點(diǎn)的所有后繼節(jié)點(diǎn)。
針對上述問題，參考D*算法的設(shè)計(jì)，本文采用為節(jié)點(diǎn)設(shè)置標(biāo)號v.Tag的方式和父節(jié)點(diǎn)屬性v.Father的方式進(jìn)行優(yōu)化。為區(qū)別經(jīng)典D*Lite算法，本文將下述算法定義為D*Lite Label算法。
定義有向圖G=（V，E），其中V表示節(jié)點(diǎn)集合，E表示邊的集合，e（u，v）∈E表示有向邊u→v，c（u，v）表示e（u，v）的權(quán)值，限定c（u，v）≥0。Succ（v）表示節(jié)點(diǎn)v的后繼節(jié)點(diǎn)集合，u∈Succ（v）表示存在有向邊e（v，u）；Pred（v）表示節(jié)點(diǎn)v的前續(xù)節(jié)點(diǎn)集合，u∈Pred（v）表示存在有向邊e（u，v）。為節(jié)點(diǎn)v設(shè)置父節(jié)點(diǎn)屬性v.Father，如果v.Father=u，則表示在最優(yōu)路徑上v的下一節(jié)點(diǎn)是u。類似于A*算法中的Open表和Close表，D*Lite算法用一個優(yōu)先隊(duì)列Queue來保存等待更新的節(jié)點(diǎn)，本文仍然沿用優(yōu)先隊(duì)列Queue這個概念。另外，本文還為每個節(jié)點(diǎn)v設(shè)置標(biāo)號v.Tag屬性，如果v.Tag=NEW，則表示該節(jié)點(diǎn)還未曾被搜索到；如果v.Tag=OPEN，則表示該節(jié)點(diǎn)等待更新且已存入Queue隊(duì)列中；如果v.Tag=CLOSED，則表示該節(jié)點(diǎn)已經(jīng)從Queue中移除。用v.g、v.rhs、v.h分別代表D*Lite算法中的g（v）、rhs（v）、h（vstart，v）。
先對程序進(jìn)行初始化，StartV表示車輛起始位置節(jié)點(diǎn)，GoalV表示目標(biāo)節(jié)點(diǎn)。
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é)點(diǎn)，Key的取值與D*Lite算法一致，Key=[min（v.g，v.rhs）+v.h+km，min（v.g，v.rhs）]，函數(shù)Cal_Key（v）用于計(jì)算節(jié)點(diǎn)v的Key值。CurrV表示車輛當(dāng)前位置節(jié)點(diǎn)。Stentz在參考文獻(xiàn)[7]描述D*算法時將節(jié)點(diǎn)狀態(tài)分為兩類，一類處于“下降狀態(tài)”（LOWER state），一類處于“上升狀態(tài)”（RAISE state）。針對兩種狀態(tài)節(jié)點(diǎn)，本文創(chuàng)新性地采用兩種更新策略。當(dāng)TopV.g>TopV.rhs時，節(jié)點(diǎn)處于下降狀態(tài)，調(diào)用Update_Lower（u，SourceV）函數(shù)對TopV的前續(xù)節(jié)點(diǎn)進(jìn)行更新，u表示待更新節(jié)點(diǎn)，SourceV表示觸發(fā)u被更新的源節(jié)點(diǎn)；當(dāng)TopV.g<TopV.rhs時，節(jié)點(diǎn)處于上升狀態(tài)，調(diào)用Update_Raise（u）對TopV的前續(xù)節(jié)點(diǎn)進(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） {
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/ } }
程序運(yùn)行的主程序同D*Lite算法基本一致，稍微不同的一點(diǎn)是，當(dāng)最后更新節(jié)點(diǎn)時需判斷該節(jié)點(diǎn)是處于上升狀態(tài)還是下降狀態(tài)，然后采用相應(yīng)的更新函數(shù)，主程序其余部分此處不再贅述，請見參考文獻(xiàn)[11]。
4 實(shí)驗(yàn)結(jié)果
本文分別采用10×10、50×50、100×100的方陣圖模擬路網(wǎng)，每條邊代表一條路，每條邊的權(quán)值為1~5之間的均勻隨機(jī)整數(shù)，起始點(diǎn)和目標(biāo)點(diǎn)為網(wǎng)絡(luò)中的交叉點(diǎn)，位置隨機(jī)決定。啟發(fā)函數(shù)采用兩點(diǎn)之間的曼哈頓距離。當(dāng)起始點(diǎn)和目標(biāo)點(diǎn)的位置確定后，分別用A*算法、D*Lite、D*Lite Label三種算法規(guī)劃最優(yōu)路徑。
?。?）為模擬車輛位置的動態(tài)變化，本文在先前規(guī)劃好的路徑上，產(chǎn)生一個隨機(jī)位置作為車輛當(dāng)前位置。
?。?）為模擬路網(wǎng)環(huán)境的變化，本文在車輛當(dāng)前位置和目標(biāo)節(jié)點(diǎn)之間的路徑上產(chǎn)生一個隨機(jī)“阻塞”，置該條邊的權(quán)值為無窮大。
當(dāng)阻塞發(fā)生后，分別采用A*、D*Lite、D*Lite Label三種算法對路徑重新規(guī)劃，統(tǒng)計(jì)每種算法所探索的節(jié)點(diǎn)數(shù)、所用時間。本文的A*算法同樣采用了標(biāo)號的方式。在三種規(guī)模的路網(wǎng)下做1 000次實(shí)驗(yàn)，統(tǒng)計(jì)其平均值，實(shí)驗(yàn)環(huán)境為Intel i5 CPU，主頻2.6 GHz，8 GB內(nèi)存，仿真平臺為Visual Studio 2010，得到的實(shí)驗(yàn)結(jié)果如表1、表2、表3所示。
5 結(jié)論
實(shí)驗(yàn)結(jié)果顯示，隨著路網(wǎng)規(guī)模的增大，動態(tài)路徑規(guī)劃算法與靜態(tài)路徑規(guī)劃算法的重復(fù)調(diào)用相比，其優(yōu)勢更加突出。D*Lite Label算法基于D*Lite算法的思想，在所探索的節(jié)點(diǎn)數(shù)方面，兩種算法基本一致。由于D*Lite Label算法為每個節(jié)點(diǎn)增加了一些屬性，避免了某些節(jié)點(diǎn)被反復(fù)更新，且同時使更新過程更加快速，使得該算法在時間效率上更優(yōu)。
參考文獻(xiàn)
[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.