月球車全局路徑規(guī)劃中的A*算法改進

彭 松 賈 陽

(北京空間飛行器總體設計部,北京 100094)

1 引言

月球車是進行月表探測的一種有效工具,在其巡視探測期間,一個重要的工作就是進行路徑規(guī)劃[1]。路徑規(guī)劃分為基于地圖的全局路徑規(guī)劃和基于傳感器的局部路徑規(guī)劃。所謂全局路徑規(guī)劃,是在環(huán)境完全已知的情況下,依據某個或幾個優(yōu)化準則(如路徑最短、時間最少等)來尋找全局最優(yōu)路徑。全局路徑規(guī)劃的算法有很多,如遺傳算法、蟻群算法、神經網絡算法、波形算法、A*算法等。A*算法是一種狀態(tài)空間啟發(fā)式算法,由于其搜索效率相對較高且容易實現, 在智能車輛上得到廣泛的應用[2-5]。美國火星探測漫游車(M ER)采用的Field D*算法就是一種動態(tài)的A*算法,它利用了A*算法的高效性,又采用動態(tài)搜索來解決環(huán)境信息不完整的問題[6]。但是A*算法只是一種“較優(yōu)”的算法,即它一般只能找到較優(yōu)路徑,而非最優(yōu)路徑[7],特別是在存在凹形障礙的情況下,它的搜索路徑更為曲折。

本文是在環(huán)境信息完全已知的情況下對月球車進行全局路徑規(guī)劃,針對傳統(tǒng)A*算法搜索速度慢和返回路徑不夠優(yōu)化的不足,先對算法流程進行改進,提高其搜索效率,優(yōu)化其返回路徑,解決凹形障礙中規(guī)劃失敗的問題,然后使用二次搜索策略對存在凹形障礙情況下的規(guī)劃路徑進行優(yōu)化,達到輸出最短路徑的目的。

2 A*算法改進

2.1 A*算法思想和算法流程

A*算法是在廣度優(yōu)先搜索的基礎上引入了一個估價函數,每次都利用這個估價函數對所有可展點進行評價, 從而找出最優(yōu)的可展點,再從該點進行搜索直至找到目標點[8]。估價函數表達式為:

f(n)=g(n)+h(n), 要求啟發(fā)函數h(n)≤h*(n)

f(n)——從起點經過節(jié)點n 到達目標點的全程路徑代價預測值;

g(n)——從起點到達節(jié)點n 的路徑代價實際值;

h(n)——從節(jié)點n到達目標點的路徑代價估計值;

h*(n)——從節(jié)點n 到達目標點的路徑代價實際值。

f 值最小的可展點視為最優(yōu)。

傳統(tǒng)的A*算法流程[9]如下:

步驟1:初始化, 生成一個OPEN 列表、一個C LOSED 列表。

步驟2:把起點加入OPEN 列表。

步驟3:如果OPEN 列表為空,則失敗退出,否則進行步驟4。

步驟4:遍歷OPEN 列表,查找f 值最小的點,將其移入CLOSED 列表,并把它作為當前點。

步驟5:判斷當前點是否為目標點,若是,則規(guī)劃結束,輸出路徑;否則將當前點的相鄰點投入OPEN 列表,進行步驟3。

步驟6:保存并輸出C LOSED 列表中的路徑節(jié)點。

2.2 改進的A*算法

改進后的A*的算法流程如下:

步驟1:初始化, 生成一個OPEN 列表、一個C LOSED 列表和一個FA THER 列表,初始值都為空;標記所有點的open 值為0。

步驟2:把起始點加入OPEN 列表,標記該點的open 值為1(表示該點進入過OPEN 列表)。

步驟3:如果OPEN 列表為空,記下當前點為壞點,若壞點為起始點,則失敗退出,否則將壞點的父節(jié)點(若B 點最初由A 點展開一次得到,則稱A 是B 的父節(jié)點)置為當前點,跳過步驟4 直接執(zhí)行步驟5。

步驟4:遍歷OPEN 列表,查找f 值最小的節(jié)點,將其移入C LOSED 列表,并把它作為當前點;同時清空OPEN 列表。

步驟5:判斷當前點是否為目標點,若是,則規(guī)劃結束,輸出路徑;否則對于當前點展開的每一個相鄰點進行如下操作:

情況1:它是障礙點或者在CLOSED 列表中,剔除它,查找下一相鄰點;

情況2:它的open 值為1(說明它加入過OPEN列表,其父節(jié)點已經設置),計算該節(jié)點的f ,g 和h值;

情況3:它的open 值為0,把它加入OPEN 列表,同時將其open 值記為1,并且把當前點設置為它的父節(jié)點,計算該節(jié)點的f ,g 和h 值。

然后進行步驟3。

步驟6:保存并輸出路徑,從目標點開始,每個節(jié)點沿著父節(jié)點移動直至起點,即FA THER 列表中的路徑節(jié)點。

2.3 改進說明

比較改進前和改進后的算法流程,這里主要做了5 點改進:

1)為了減少排序節(jié)點的個數,提高算法的執(zhí)行效率,在對所選節(jié)點的相鄰點進行展開時,可首先排除已經在CLOSED 列表中出現的節(jié)點[10]。這在路徑規(guī)劃上的結果就是不允許返回走過的路徑,有效地避免了迂回。有一點值得指出的是,在沒有排除可展節(jié)點中已經在C LOSED 列表中出現的節(jié)點時,經常會出現在某兩個節(jié)點之間來回振蕩的情況,從而使程序陷入死循環(huán),改進后則從根本上避免了該種情況。

2)對所選節(jié)點的相鄰點進行展開后, 先將OPEN 列表中元素清空, 再把這些展開點投入OPEN 列表。這樣做有兩個好處:

一是OPEN 列表中的元素不會因為累加而越來越多,而是有一個上限,這個上限為節(jié)點的所有相鄰節(jié)點數。這樣在挑選OPEN 表中f(n)最小的節(jié)點時,會顯著減少循環(huán)次數,提高執(zhí)行速度。

二是從OPEN 表中投入CLOSED 表的相鄰節(jié)點必是物理上的相鄰,不會出現跳躍,從而保證規(guī)劃路徑的連續(xù)性。

3)關于改進后流程中的步驟5 中情況2 情況,文獻[10]指出:若展開點進入過OPEN 表,檢查這條路徑(即經由當前點到達該展開點)是否更好,用g 值作參考,更小的g 值表示這是更好的路徑。如果g 值更小,把當前點設置為它的父節(jié)點,并重新計算它的g 和f 值;否則g 和f 保持不變。但是實際上不會出現g 值更小的情況,因為后搜索到節(jié)點的g 值總是大于先搜索到節(jié)點的g 值,所以g 和f 值總是保持原來的較小值。由此在選擇OPEN表中最小f 值的節(jié)點時,總是傾向于選擇先搜索到的離起始點近的節(jié)點,造成CLOSED 表中返回的搜索路徑向起始點迂回

本文采用的方法是無論展開點是否進入過OPEN 列表,都重新計算g 值和f 值,然后選擇f值最小的節(jié)點。在路徑規(guī)劃上的體現是,每走一步都是最優(yōu)的,并且此步最優(yōu)是建立在上步最優(yōu)的基礎上,通過步步最優(yōu)來返回CLOSED 列表。

4)返回路徑時,不是將CLOSED 表中的元素逐一列出,而是從目標點開始逐個返回父節(jié)點投入FA THER 列表,直至到達起始點,這樣可顯著減少返回節(jié)點, 縮短了路徑。在編程時, 可以對每個OPEN 表中的節(jié)點建立指針指向它的父節(jié)點,最后通過指針找到它的父節(jié)點。

5)對于步驟3 的改進,主要是針對地圖中存在凹形障礙的情況。當地圖中存在凹形障礙時,搜索路徑往往會掉入凹形陷阱,由于程序中規(guī)定后續(xù)搜索節(jié)點不能是先前進入CLOSED 列表中的點,有時造成搜索路徑卡死在陷阱中,導致規(guī)劃失敗。在這一特殊情況下,采取的策略是路徑從卡住的壞點后退一步到達其父節(jié)點,嘗試其他的可展節(jié)點,如果失敗再后退一步接著嘗試,直至到達目標點。

2.4 仿真算例

2.4.1 路徑規(guī)劃模型的建立

仿真是在假設環(huán)境信息完全已知的情況下做的。仿真輸入是一個二維柵格地圖,用柵格的中心點代表整個柵格,稱為節(jié)點[11]。每個節(jié)點有兩個狀態(tài):可通行點和障礙點。如圖1 所示,“點”表示可通行點,上三角形表示障礙點(下同)。每個節(jié)點可以向四周八個方向的相鄰節(jié)點移動。需要指出的是,從點5 到點7 的連線與障礙點4 所代表的柵格相切,這里認為是可通過的。實際應用中,把真實障礙外擴一定距離,變?yōu)閿U展障礙,以保證點5、7 連線的可通過性。

圖1 地圖中的柵格和節(jié)點Fig.1 Grids and nodes in map

評價函數的選擇:

f(n)=g(n)+h(n),其中

這里取α=1.4,β=1.0,因為相鄰點的對角線方向距離和水平垂直方向的距離比值是2;

這里取的是當前節(jié)點(xn,yn)和目標點(xg,yg)的直線距離。

2.4.2 針對改進的仿真結果

對于改進1)和2),它減少了每次參加排序的節(jié)點數,使得運算加快,顯然提高了搜索效率。

對于改進3)和4),圖2 給出了不失一般性的例子,從圖2(a)和圖2 (b)可以看出,文獻[10]方法返回的路徑向起始點迂回,本文方法表現較好;從圖2(b)和圖2(c)可以看出, FA THER 列表返回的路徑要優(yōu)于CLOSED 列表的返回路徑。

對于改進(5),不失一般性,圖3 給出了一個存在凹形障礙的仿真實例。如圖3 (a)中的A 點所示,由于程序規(guī)定后續(xù)擴展節(jié)點不能是先前進入CLOSED 列表中的點,即不能返回B 點,而與A 點相鄰的其他點又都是障礙點,從而造成路徑卡在A點,無法繼續(xù)擴展,導致規(guī)劃失敗。在這一特殊情況下,采取的策略是從卡住的壞點后退一步到達其父節(jié)點,嘗試其他的可展節(jié)點,如果失敗,再后退一步接著嘗試,直至跳出凹形障礙。如圖3(b)所示,路徑從A 點后退到其父節(jié)點B,B 點依然沒有可擴展點,仍是壞點;路徑再退一步到達B 點的父節(jié)點C,C 點有兩個可展節(jié)點D 和E;下一步選擇D 點,從而跳出了凹洞,繼續(xù)搜索趨向目標點。

圖2 仿真算例比較Fig.2 Com parison of simulation results

圖3 存在凹形障礙物的路徑規(guī)劃Fig.3 Path planning with concavity obstacles

3 雙向二次搜索策略

對于只存在凸形障礙的地圖,改進的A*算法能快速地得到一條最優(yōu)路徑,但是對于存在凹形障礙的地圖,返回的路徑不夠優(yōu)化。

不失一般性,圖4 給出了一個含有凹形障礙的地圖。如圖4(a)所示,CLOSED 列表返回的路徑首先到達凹洞最底部,發(fā)現前方無法通過后,擴展點只能向兩側或后方延伸。由于啟發(fā)信息h(n)是當前點和目標點的直線距離,CLOSED 列表中的擴展節(jié)點連線近似為:以目標點為圓心一層一層地向外畫圓弧直至走出凹形障礙,然后趨向目標點。這種沿圓弧層層外擴的結果使得CLOSED 列表遍歷凹形障礙的內部節(jié)點。

FATHE R 列表返回的路徑比C LOSED 列表的路徑有很大改觀,近似為沿直線進入凹洞底部然后又沿直線跳出,但是依然沒有避免陷入凹洞,路徑不夠優(yōu)化。

采用二次搜索策略優(yōu)化路徑。在第一次搜索的基礎上,將首次搜索CLOSED 列表中返回的節(jié)點看作可通過點,地圖中的其他點都視為障礙點,重新構建地圖,從目標點G 向起始點S 進行搜索,結果如圖4(b)所示(FA THER 列表返回的路徑記為pathnormal,有24 個節(jié)點),從圖中可以看出,返回路徑很好地繞過了凹形障礙。

圖4 正向搜索Fig.4 Normal search

從圖4 還可看出,返回路徑是從右邊繞過障礙的,這是因為目標點G 在起始點S 的右側,導致了搜索點首先向右側進行。若從左側繞過障礙返回路徑,可能會更短,那么如何實現向另一側進行搜索呢?

這里采取的策略是從目標點G 向起始點S 進行逆向搜索。具體操作如下:先從目標點G 向起始點S 搜索一次,返回CLOSED 列表,返回路徑如圖5(a)所示;將CLOSED 列表中返回的節(jié)點看作可通過點,地圖中的其他點都視為障礙點, 重新構建地圖,再從起始點S 向目標點G 進行二次搜索,返回路徑如圖5(b)所示(FA THER 列表返回的路徑記為path-converse,有14 個節(jié)點)。

圖5 逆向搜索Fig.5 Converse search

比較path-normal(25 個節(jié)點)和path-converse(14 個節(jié)點),path-converse 的節(jié)點少,即為最后的優(yōu)化路徑。

總結上述的搜索策略,這里把它稱為雙向二次搜索,步驟如下:

1)正向搜索。從起始點向目標點(S →G)應用A*算法搜索一次,返回C LOSED 列表;將C LOSED表中返回的節(jié)點看作可通過點,地圖中的其他點都視為障礙點,重新構建地圖,從目標點向起始點(G→S)進行二次搜索,返回FA THER 列表,路徑記為path-normal;

2)逆向搜索。從目標點向起始點(G →S)應用A*算法搜索一次,返回CLOSED 列表;將CLOSED表中返回的節(jié)點看作可通過點,地圖中的其他點都視為障礙點,重新構建地圖。從起始點向目標點(S→G)進行二次搜索,返回FA THER 列表,路徑記為path-converse;

3)比較path-normal 和path-converse,擇優(yōu)取之(路徑節(jié)點少的為優(yōu))。

值得說明的是,雙向二次搜索策略是針對存在凹形障礙的地圖提出的,對只有凸形障礙的地圖同樣適應(該種情況下一次搜索結果已經接近最短路徑,二次搜索優(yōu)化效果不明顯)。實際應用中不必判斷障礙物的形狀,直接進行雙向二次搜索即可得到最短路徑。

4 結論

本文針對月球車全局路徑規(guī)劃的應用要求,對傳統(tǒng)的A*算法流程進行了改進,減少了其時間和空間復雜度,使之在路徑規(guī)劃中的應用更加高效。同時對于存在凹形障礙的地圖,給出了成功進行路徑規(guī)劃的處理方法;然后采用雙向二次搜索策略對規(guī)劃路徑進行優(yōu)化,避免路徑掉入凹洞陷阱,達到輸出最短路徑的目的。該方法可以應用于月球車的全局路徑規(guī)劃。

目前的研究都是基于環(huán)境信息完全已知的情況,進一步將基于環(huán)境信息不完整的條件下進行路徑規(guī)劃,使算法應具有接收新的感知信息進行路徑修正的能力。

)

[1]賈陽, 陳建新, 張熇.月球車關鍵技術分析[J].航天器工程, 2006, 3(9):16-22

[2]陳圣群, 滕忠堅, 洪親, 等.A*算法在車輛導航系統(tǒng)中的應用研究[J].微計算機信息, 2008, 24(11-3):269-270, 303

[3]張海濤, 程蔭杭.基于A*算法的全局路徑搜索[J].微計算機信息, 2007, 23(6-2):238-239

[4]岳靚亮.基于Dijkstra、A*算法的汽車導航算路實現[D].長春:吉林大學, 2006, 5

[5]常曉飛, 段麗娟, 符文星,等.基于A*算法的無人機爬升軌跡設計[J].飛行力學, 2008, 26(6):22-25

[6]Ferguson D, Stentz A.The field D*algorithm for improved path planning and replanning in uniform and non-uniform cost environments[R].CM U Technical Report, CM U-TR-RI-05-19, 2005

[7]郝向榮.在智能搜索中A*算法的應用與研究[D].西安:西安建筑科技大學, 2007, 4

[8](美)Nils J.Nilsson 著.人工智能[M].鄭扣根, 莊越挺, 譯.北京:機械工業(yè)出版社, 2007:84-88

[9]姚雪梅.人工智能中A*算法的程序實現——八數碼問題的演示程序[J].電腦與信息技術, 2002(2):1-3

[10]Lester P.A*pathfinding for beginners[EB/OL].[2009-08-31].http://w ww .gamedev.net.(2005-07-20)

[11]Al Hasan S, Vachtsevans G.Intelligent route planning for fast autonomous vehicles operating in a large natural terrain [J].Robotics and Autonomous System s,2002, 40(2):1-24