彭 松 賈 陽(yáng)

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

1 引言

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

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

2 A*算法改進(jìn)

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

A*算法是在廣度優(yōu)先搜索的基礎(chǔ)上引入了一個(gè)估價(jià)函數(shù),每次都利用這個(gè)估價(jià)函數(shù)對(duì)所有可展點(diǎn)進(jìn)行評(píng)價(jià), 從而找出最優(yōu)的可展點(diǎn),再?gòu)脑擖c(diǎn)進(jìn)行搜索直至找到目標(biāo)點(diǎn)[8]。估價(jià)函數(shù)表達(dá)式為:

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

f(n)——從起點(diǎn)經(jīng)過(guò)節(jié)點(diǎn)n 到達(dá)目標(biāo)點(diǎn)的全程路徑代價(jià)預(yù)測(cè)值;

g(n)——從起點(diǎn)到達(dá)節(jié)點(diǎn)n 的路徑代價(jià)實(shí)際值;

h(n)——從節(jié)點(diǎn)n到達(dá)目標(biāo)點(diǎn)的路徑代價(jià)估計(jì)值;

h*(n)——從節(jié)點(diǎn)n 到達(dá)目標(biāo)點(diǎn)的路徑代價(jià)實(shí)際值。

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

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

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

步驟2:把起點(diǎn)加入OPEN 列表。

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

步驟4:遍歷OPEN 列表,查找f 值最小的點(diǎn),將其移入CLOSED 列表,并把它作為當(dāng)前點(diǎn)。

步驟5:判斷當(dāng)前點(diǎn)是否為目標(biāo)點(diǎn),若是,則規(guī)劃結(jié)束,輸出路徑;否則將當(dāng)前點(diǎn)的相鄰點(diǎn)投入OPEN 列表,進(jìn)行步驟3。

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

2.2 改進(jìn)的A*算法

改進(jìn)后的A*的算法流程如下:

步驟1:初始化, 生成一個(gè)OPEN 列表、一個(gè)C LOSED 列表和一個(gè)FA THER 列表,初始值都為空;標(biāo)記所有點(diǎn)的open 值為0。

步驟2:把起始點(diǎn)加入OPEN 列表,標(biāo)記該點(diǎn)的open 值為1(表示該點(diǎn)進(jìn)入過(guò)OPEN 列表)。

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

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

步驟5:判斷當(dāng)前點(diǎn)是否為目標(biāo)點(diǎn),若是,則規(guī)劃結(jié)束,輸出路徑;否則對(duì)于當(dāng)前點(diǎn)展開(kāi)的每一個(gè)相鄰點(diǎn)進(jìn)行如下操作:

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

情況2:它的open 值為1(說(shuō)明它加入過(guò)OPEN列表,其父節(jié)點(diǎn)已經(jīng)設(shè)置),計(jì)算該節(jié)點(diǎn)的f ,g 和h值;

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

然后進(jìn)行步驟3。

步驟6:保存并輸出路徑,從目標(biāo)點(diǎn)開(kāi)始,每個(gè)節(jié)點(diǎn)沿著父節(jié)點(diǎn)移動(dòng)直至起點(diǎn),即FA THER 列表中的路徑節(jié)點(diǎn)。

2.3 改進(jìn)說(shuō)明

比較改進(jìn)前和改進(jìn)后的算法流程,這里主要做了5 點(diǎn)改進(jìn):

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

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

一是OPEN 列表中的元素不會(huì)因?yàn)槔奂佣絹?lái)越多,而是有一個(gè)上限,這個(gè)上限為節(jié)點(diǎn)的所有相鄰節(jié)點(diǎn)數(shù)。這樣在挑選OPEN 表中f(n)最小的節(jié)點(diǎn)時(shí),會(huì)顯著減少循環(huán)次數(shù),提高執(zhí)行速度。

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

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

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

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

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

2.4 仿真算例

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

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

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

評(píng)價(jià)函數(shù)的選擇:

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

這里取α=1.4,β=1.0,因?yàn)橄噜忺c(diǎn)的對(duì)角線方向距離和水平垂直方向的距離比值是2;

這里取的是當(dāng)前節(jié)點(diǎn)(xn,yn)和目標(biāo)點(diǎn)(xg,yg)的直線距離。

2.4.2 針對(duì)改進(jìn)的仿真結(jié)果

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

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

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

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

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

3 雙向二次搜索策略

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

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

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

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

圖4 正向搜索Fig.4 Normal search

從圖4 還可看出,返回路徑是從右邊繞過(guò)障礙的,這是因?yàn)槟繕?biāo)點(diǎn)G 在起始點(diǎn)S 的右側(cè),導(dǎo)致了搜索點(diǎn)首先向右側(cè)進(jìn)行。若從左側(cè)繞過(guò)障礙返回路徑,可能會(huì)更短,那么如何實(shí)現(xiàn)向另一側(cè)進(jìn)行搜索呢?

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

圖5 逆向搜索Fig.5 Converse search

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

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

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

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

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

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

4 結(jié)論

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

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

)

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

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

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

[4]岳靚亮.基于Dijkstra、A*算法的汽車導(dǎo)航算路實(shí)現(xiàn)[D].長(zhǎng)春:吉林大學(xué), 2006, 5

[5]常曉飛, 段麗娟, 符文星,等.基于A*算法的無(wú)人機(jī)爬升軌跡設(shè)計(jì)[J].飛行力學(xué), 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*算法的應(yīng)用與研究[D].西安:西安建筑科技大學(xué), 2007, 4

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

[9]姚雪梅.人工智能中A*算法的程序?qū)崿F(xiàn)——八數(shù)碼問(wèn)題的演示程序[J].電腦與信息技術(shù), 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