在自主車(chē)輛路徑規(guī)劃中逆向D*算法的應(yīng)用

雷雨能，賴(lài)文娟，曾 刊

(中國(guó)兵器工業(yè)第五八研究所 軍品部，四川 綿陽(yáng) 621000)

自主車(chē)輛研究是當(dāng)今的1 個(gè)熱點(diǎn)，路徑規(guī)劃作為自主車(chē)輛完成行駛?cè)蝿?wù)的安全保障，是其智能化程度的重要標(biāo)志。所謂路徑規(guī)劃，是指在具有障礙物的環(huán)境中，機(jī)器人按照某一特定性能指標(biāo)(如距離、時(shí)間等)搜索1 條從起始狀態(tài)到目標(biāo)狀態(tài)的最優(yōu)或次優(yōu)路徑的過(guò)程。由于自主車(chē)輛運(yùn)行在戶外復(fù)雜環(huán)境中，因此對(duì)路徑規(guī)劃的仿真研究非常必要。傳統(tǒng)的路徑規(guī)劃算法主要有人工勢(shì)場(chǎng)法、可視圖法，而目前比較流行的路徑規(guī)劃算法主要有A*、D*、神經(jīng)網(wǎng)絡(luò)、遺傳以及蟻群等等算法［1］。其中，文獻(xiàn)［2］采用的D*算法比較適合在小規(guī)模、部分已知的環(huán)境中進(jìn)行路徑規(guī)劃，而在較大規(guī)模復(fù)雜環(huán)境中，直接規(guī)劃起點(diǎn)到終點(diǎn)的計(jì)算代價(jià)往往很大。

針對(duì)上述問(wèn)題，本文對(duì)逆向D*算法進(jìn)行了分析和驗(yàn)證。對(duì)自主車(chē)輛裝備的各種傳感器所探測(cè)到的前方路面環(huán)境建立局部柵格地圖。在局部柵格地圖中采用A*方法［3］搜索中間節(jié)點(diǎn)，并規(guī)劃得出車(chē)輛當(dāng)前位置到中間節(jié)點(diǎn)的局部路徑。在行駛到中間節(jié)點(diǎn)后，自主車(chē)輛再重新搜索下1 個(gè)中間節(jié)點(diǎn)，并規(guī)劃下1 條局部路徑，自主車(chē)輛沿著下1 條局部路徑行駛。依此循環(huán)后，直到到達(dá)最終目的地。通過(guò)實(shí)驗(yàn)驗(yàn)證，該算法克服了D*算法在大規(guī)模環(huán)境下計(jì)算量較大的缺點(diǎn)，提高了路徑規(guī)劃算法的效率。

1 逆向D* 算法

1.1 基本思想

在部分信息未知的復(fù)雜環(huán)境中，機(jī)器人路徑規(guī)劃有以下3 個(gè)特點(diǎn)［4］:①環(huán)境信息的改變大多是在機(jī)器人周?chē)喈?dāng)近的1 個(gè)區(qū)域當(dāng)中進(jìn)行的，這主要是因?yàn)闄C(jī)器人一般裝備的各種傳感器都只具有一定的觀察范圍。這意味著路徑規(guī)劃過(guò)程不一定非要在整個(gè)全局環(huán)境內(nèi)進(jìn)行。②機(jī)器人的行進(jìn)過(guò)程中，如果大多數(shù)的障礙物很小，那么簡(jiǎn)單的路徑改變就足以表達(dá)，這樣就可以避免很高的計(jì)算代價(jià)。③在機(jī)器人行進(jìn)過(guò)程中的1 個(gè)節(jié)點(diǎn)上，在大多數(shù)情況之下只有剩下的一部分路徑需要重新規(guī)劃。那么，根據(jù)第2 個(gè)特點(diǎn)，重新規(guī)劃的這部分路徑就可以變得更短，D*算法正是針對(duì)部分信息已知的環(huán)境中路徑規(guī)劃問(wèn)題提出的。

D*算法在小規(guī)模環(huán)境區(qū)域規(guī)劃中比較有效，但在較大規(guī)模環(huán)境下直接規(guī)劃起點(diǎn)到終點(diǎn)的路徑，往往造成很大的計(jì)算代價(jià)。為了不再計(jì)算初始的目標(biāo)距離勢(shì)場(chǎng)，通過(guò)定義“機(jī)器人距離”的方法來(lái)建立機(jī)器人當(dāng)前位置中心的局部區(qū)域勢(shì)場(chǎng)，并且通過(guò)搜索距離目標(biāo)估計(jì)代價(jià)最小的中間目標(biāo)節(jié)點(diǎn)，來(lái)滿足機(jī)器人滾動(dòng)優(yōu)化［5］的需要，提出了逆向D*算法。

1.2 設(shè)計(jì)流程

逆向D*算法的設(shè)計(jì)流程如下:

1)采用柵格環(huán)境建模法對(duì)機(jī)器人行進(jìn)中傳感器所感知到的前方信息建立局部環(huán)境地圖，對(duì)障礙物柵格進(jìn)行預(yù)膨脹，以保證機(jī)器人和障礙物有一定的安全膨脹距離。通過(guò)膨脹后，機(jī)器人可以被看成是以其參考點(diǎn)為中心的“點(diǎn)”機(jī)器人。

2)設(shè)置機(jī)器人從起點(diǎn)S(xs，ys)到目標(biāo)點(diǎn)G(xg，yg)運(yùn)動(dòng)過(guò)程中所經(jīng)歷的目標(biāo)勢(shì)能最小值Emin。假設(shè)自由柵格坐標(biāo)點(diǎn)I(xi，yi)的勢(shì)能為，則在滾動(dòng)優(yōu)化過(guò)程中，機(jī)器人要以其當(dāng)前位置R 為中心，向外尋找目標(biāo)勢(shì)能小于Emin-ε(ε ＞0，為常數(shù)項(xiàng))的中間柵格節(jié)點(diǎn)I。而此柵格節(jié)點(diǎn)I 應(yīng)該滿足

3)在搜索中間柵格節(jié)點(diǎn)I 過(guò)程中，以A*方法建立確定搜索優(yōu)先度的估計(jì)函數(shù)f(i)。其中，被搜索點(diǎn)I 的估價(jià)函數(shù)f(i)=g(i)+h(i)。而與D*算法不同的是，g(i)是從機(jī)器人當(dāng)前位置R 到搜索柵格節(jié)點(diǎn)I 的實(shí)際代價(jià)，即機(jī)器人距離，h(i)是搜索柵格節(jié)點(diǎn)I 到目標(biāo)節(jié)點(diǎn)G 的估計(jì)代價(jià)。

4)在被搜索的區(qū)域里，機(jī)器人距離g(i)被記錄在柵格位圖中，當(dāng)搜索到滿足上述條件的I 節(jié)點(diǎn)后，從節(jié)點(diǎn)I 出發(fā)，按照機(jī)器人距離g(i)減少的梯度方向，回溯到當(dāng)前機(jī)器人位置R，而該回溯的逆向過(guò)程就是從機(jī)器人當(dāng)前位置R 到達(dá)中間目標(biāo)I 的路徑。

5)機(jī)器人從起點(diǎn)出發(fā)，通過(guò)不斷地采用A*方法搜索中間目標(biāo)I 來(lái)實(shí)現(xiàn)滾動(dòng)的動(dòng)態(tài)路徑規(guī)劃。

2 環(huán)境地圖的表示

無(wú)論采用哪種路徑規(guī)劃算法，都需要知道機(jī)器人所在的自身環(huán)境，包括機(jī)器人自身的位置、障礙物的位置、路標(biāo)以及目標(biāo)的位置等，這樣機(jī)器人才能搜索到達(dá)目的地位置的行走路徑，以便更好地完成任務(wù)。而如何對(duì)環(huán)境建立模型，就成了路徑規(guī)劃的基本前提。目前，建立環(huán)境模型的方法主要有幾何建模法、拓?fù)浣７ㄒ约皷鸥竦貓D建模法［1］。針對(duì)不同的路徑規(guī)劃需要不同的建模方法，由于柵格地圖很容易創(chuàng)建和維護(hù)，故本文所述的逆向D*算法所采用的環(huán)境表示方法是柵格地圖環(huán)境建模法。

柵格地圖就是將整個(gè)工作環(huán)境分為若干相同大小的柵格，對(duì)于每個(gè)柵格指出其是否存在障礙物。機(jī)器人所了解的每個(gè)柵格的信息直接與環(huán)境中某區(qū)域?qū)?yīng)，環(huán)境被表示成離散柵格，柵格“0”表示為可通行的柵格，柵格“1”表示為障礙物柵格。本文所建立的環(huán)境地圖如圖1 所示。柵格范圍為20 ×20 方格，柵格單元尺寸為0.5 m，黑色方格為膨脹后的障礙物柵格，空白柵格為可通行柵格。

圖1 環(huán)境柵格地圖

3 算法流程

本文所設(shè)計(jì)的自主車(chē)輛路徑規(guī)劃仿真流程如圖2 所示。

4 實(shí)驗(yàn)結(jié)果與結(jié)論

為了說(shuō)明逆向D*算法在自主車(chē)輛路徑規(guī)劃中應(yīng)用的可行性及有效性，采用圖1 所示的環(huán)境柵格地圖，以最短距離和最短計(jì)算量作為路徑規(guī)劃的優(yōu)化性能指標(biāo)，利用matlab 進(jìn)行自主車(chē)輛逆向D*算法的實(shí)驗(yàn)仿真。為了說(shuō)明逆向D*算法的優(yōu)越性，還與D*算法的仿真結(jié)果進(jìn)行了比較和分析。實(shí)驗(yàn)的硬件配置:處理器為Pentium(R)Dual-Core CPU E5500 2.8 GHz;內(nèi)存為2 GB。

本實(shí)驗(yàn)中，自主車(chē)輛從起始點(diǎn)S(3，3)出發(fā)，預(yù)計(jì)行駛到目標(biāo)點(diǎn)G(18，18)，以完成路徑規(guī)劃任務(wù)。起始點(diǎn)和目標(biāo)點(diǎn)在柵格地圖中的位置如圖3 所示。實(shí)驗(yàn)中還設(shè)定自主車(chē)輛傳感器所能探測(cè)到的局部環(huán)境柵格地圖范圍為6 ×6 方格，而估價(jià)函數(shù)中的機(jī)器人距離g(i)為歐幾里德距離。

圖2 算法流程

圖3 起始點(diǎn)和目標(biāo)點(diǎn)位置示意圖

逆向D*算法的仿真結(jié)果如圖4 ～圖6 所示。自主車(chē)輛在行進(jìn)過(guò)程中，總共搜索到2 個(gè)中間節(jié)點(diǎn)，分別是L1和L2，圖4 ～圖6 分別表示起始點(diǎn)S 到中間節(jié)點(diǎn)L1、中間節(jié)點(diǎn)L1到中間節(jié)點(diǎn)L2、中間節(jié)點(diǎn)L2到目標(biāo)點(diǎn)G 的規(guī)劃結(jié)果，柵格位圖中的數(shù)字表示機(jī)器人距離g(i)。同等實(shí)驗(yàn)條件下，D*算法的仿真結(jié)果如圖5 所示。

圖4 逆向D* 算法路徑規(guī)劃(1)

圖5 逆向D* 算法路徑規(guī)劃(2)

圖6 逆向D* 算法路徑規(guī)劃(3)

圖7 D* 算法路徑規(guī)劃

以上實(shí)驗(yàn)結(jié)果說(shuō)明，逆向D*算法與D*算法規(guī)劃出的路徑一樣，都是最短的安全行駛路徑，但逆向D*算法在路徑規(guī)劃過(guò)程中，3 次累計(jì)需要計(jì)算的機(jī)器人距離g(i)次數(shù)比D*算法需要計(jì)算的機(jī)器人距離g(i)次數(shù)要少3 倍以上，這樣大大減少了計(jì)算量，提高路徑規(guī)劃執(zhí)行的效率。

5 結(jié)束語(yǔ)

本文討論了自主車(chē)輛的路徑規(guī)劃，分析了逆向D*路徑規(guī)劃算法的原理以及基本流程，利用仿真實(shí)驗(yàn)驗(yàn)證了其有效性，并與D*路徑規(guī)劃算法做分析比較，驗(yàn)證了該算法能大大提高路徑規(guī)劃的執(zhí)行效率。

［1］朱大奇，顏明重. 移動(dòng)機(jī)器人路徑規(guī)劃技術(shù)研究概述［J］.控制與決策，2010，25(7):962-967.

［2］劉亮.自主移動(dòng)機(jī)器人的路徑規(guī)劃與避障研究［D］.武漢:武漢京理工大學(xué)，2007.

［3］劉進(jìn). 自主式移動(dòng)機(jī)器人控制系統(tǒng)與路徑規(guī)劃研究［D］.青島:青島大學(xué)，2009.

［4］張毅，羅元，鄭太雄.移動(dòng)機(jī)器人技術(shù)及其應(yīng)用［M］.北京:電子工業(yè)出版社，2007.

［5］張純剛，席裕庚.全局環(huán)境未知時(shí)基于滾動(dòng)窗口的機(jī)器人路徑規(guī)劃［J］.中國(guó)科學(xué)(E 輯)，2001，31(2):51-57.

［6］黃鶴.部分環(huán)境信息已知的智能機(jī)器人路徑規(guī)劃方法研究［D］.南京:南京理工大學(xué)，2005.

［7］張亮，周繼寶. 基于遺傳算法的后勤運(yùn)輸車(chē)路徑規(guī)劃［J］.四川兵工學(xué)報(bào)，2012(2):81-83.