錢燮暉,何秀鳳,郭俊文,王 礫

(1.河海大學(xué)地球科學(xué)與工程學(xué)院,江蘇 南京 211100; 2.北方信息控制研究院集團(tuán)有限公司,江蘇 南京 211100)

車輛在野外環(huán)境的路徑規(guī)劃是部隊(duì)作戰(zhàn)和演習(xí)的關(guān)鍵問(wèn)題,一直受到廣泛關(guān)注。車輛路徑規(guī)劃指的是從起始位置(起始點(diǎn))到終止位置(目標(biāo)點(diǎn))的空間內(nèi),根據(jù)研究人員對(duì)其所設(shè)定的某種具有可選擇最優(yōu)路徑規(guī)劃法則,來(lái)輸出一條能夠精確到達(dá)目標(biāo)的最優(yōu)或次優(yōu)的安全無(wú)碰撞路徑[1]。由于受野外環(huán)境復(fù)雜且信息量大、障礙物多等因素的影響,路徑規(guī)劃問(wèn)題已被確定為非確定性多項(xiàng)式困難問(wèn)題[2]。

目前有很多智能算法應(yīng)用于路徑規(guī)劃問(wèn)題中,各有其優(yōu)缺點(diǎn)。遺傳算法具有較強(qiáng)的魯棒性,能自適應(yīng)地調(diào)整搜索方向,但在求解過(guò)程中常常伴隨著大量的冗余迭代,降低了搜索效率;蟻群算法具有良好地正反饋和自適應(yīng)能力,適合全局搜索,但其收斂性對(duì)初始化參數(shù)較為敏感,需要較長(zhǎng)的搜索時(shí)間,易出現(xiàn)停滯現(xiàn)象;模擬退火算法的計(jì)算過(guò)程簡(jiǎn)單,魯棒性強(qiáng),適用于并行處理,但其收斂速度慢,收斂時(shí)間長(zhǎng),算法性能受初始值和有關(guān)參數(shù)的影響很大[3];A*算法適合全局最優(yōu)路徑規(guī)劃,有較強(qiáng)的實(shí)時(shí)性,但隨著搜索空間和存儲(chǔ)數(shù)據(jù)量的增大,導(dǎo)致計(jì)算量增大和搜索時(shí)間變長(zhǎng),其實(shí)時(shí)性無(wú)法獲得保證[4]。

考慮到車輛在野外的路徑規(guī)劃對(duì)實(shí)時(shí)性有較高的要求,選擇實(shí)時(shí)性更高的A*算法進(jìn)行路徑規(guī)劃。許多研究學(xué)者針對(duì)A*算法在相應(yīng)場(chǎng)景下做了很多研究,如文獻(xiàn)[5]中通過(guò)改進(jìn)障礙搜索方式和啟發(fā)函數(shù),在犧牲了路徑長(zhǎng)度的情況下,提高了算法的性能和路徑的安全性,但這是建立在室內(nèi)仿真環(huán)境下,沒(méi)有考慮復(fù)雜的野外環(huán)境以及遭遇未知障礙物的情況;文獻(xiàn)[6]中通過(guò)采用優(yōu)先子節(jié)點(diǎn)的生成策略來(lái)避免生成穿過(guò)障礙物柵格節(jié)點(diǎn)的路徑,但僅僅通過(guò)一個(gè)簡(jiǎn)單的仿真實(shí)驗(yàn)并不足以驗(yàn)證該算法在實(shí)際環(huán)境下的可靠性;文獻(xiàn)[7]中提出了一種雙向時(shí)效A*算法,并采用多近鄰柵格距離計(jì)算方案,在提高算法搜索效率的同時(shí)平滑路徑,但不適合大尺寸復(fù)雜地圖?？偟膩?lái)說(shuō),雖然A*算法有所改進(jìn),但仍有些問(wèn)題沒(méi)有得到很好的解決:沒(méi)有考慮在復(fù)雜的野外環(huán)境下算法的可靠性;僅僅考慮優(yōu)化搜索效率,卻忽略了算法對(duì)車輛機(jī)動(dòng)性的提高。

為了考慮在實(shí)際的野外環(huán)境,給車輛規(guī)劃出一條快速到達(dá)目的地的最優(yōu)路徑,研究借鑒文獻(xiàn)[6]中提出的優(yōu)先子節(jié)點(diǎn)生成策略,結(jié)合真實(shí)的野外背景,計(jì)算得到了問(wèn)題的解決方案。通過(guò)改進(jìn)傳統(tǒng)A*算法的搜索策略,提升車輛在野外環(huán)境下的機(jī)動(dòng)性,同時(shí)提出了一種加入預(yù)處理的二次A*算法使車輛能夠?qū)崟r(shí)避障。首先,對(duì)目標(biāo)區(qū)域進(jìn)行預(yù)處理,得到不同地物的環(huán)境信息并加以區(qū)分;然后,為驗(yàn)證改正后算法的可靠性將其與A*算法進(jìn)行實(shí)驗(yàn)對(duì)比。當(dāng)車輛沿著規(guī)劃好的全局最優(yōu)路徑行進(jìn)中遇到未知障礙物時(shí),實(shí)時(shí)更新環(huán)境信息,確定局部規(guī)劃區(qū)域。再次,應(yīng)用A*算法進(jìn)行局部路徑規(guī)劃,將全局路徑規(guī)劃和局部路徑規(guī)劃相結(jié)合,從而規(guī)劃出一條安全無(wú)碰撞的最優(yōu)路徑。

1 基于二次A*算法的路徑規(guī)劃

1.1 A*算法原理

A*算法綜合分析BFS算法和Dijkstra算法各自的特點(diǎn),取長(zhǎng)補(bǔ)短,通過(guò)選擇合適的估價(jià)函數(shù)來(lái)引導(dǎo)搜索方向,減小搜索范圍,在保證搜索效率的情況下找到最優(yōu)路徑,是一種典型的啟發(fā)式搜索算法[8-10]。由于A*算法在搜索過(guò)程中都是選擇代價(jià)最低的節(jié)點(diǎn),因此最終得到的路徑也是代價(jià)最低的。在A*算法中最關(guān)鍵的就是估價(jià)函數(shù)[11],其公式為

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

(1)

其中:f(n)表示從起始節(jié)點(diǎn)經(jīng)由任意節(jié)點(diǎn)n到目標(biāo)點(diǎn)的估價(jià)函數(shù);g(n)表示從起始節(jié)點(diǎn)到任意節(jié)點(diǎn)n的實(shí)際代價(jià);h(n)表示從節(jié)點(diǎn)n到目標(biāo)點(diǎn)的啟發(fā)式評(píng)估代價(jià)。啟發(fā)式函數(shù)h(n)表示任意節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的最小代價(jià)評(píng)估值。

通過(guò)實(shí)驗(yàn)發(fā)現(xiàn)A*算法在進(jìn)行復(fù)雜環(huán)境路徑規(guī)劃時(shí)存在一個(gè)明顯的缺點(diǎn):無(wú)法在動(dòng)態(tài)環(huán)境下進(jìn)行路徑規(guī)劃。從A*算法的原理進(jìn)行分析:估價(jià)函數(shù)值f(n)由g(n)和h(n)2個(gè)部分組成,而g(n)和h(n)的取值都和環(huán)境信息有著密切的關(guān)系。因此只有在全局環(huán)境信息已知的情況下,A*算法才能規(guī)劃得到最優(yōu)路徑[12]。如果已知環(huán)境中突然出現(xiàn)未知障礙物或不可通行區(qū)域,A*算法就不能完成預(yù)設(shè)定的路徑。

運(yùn)動(dòng)過(guò)程中突然出現(xiàn)障礙物情形如圖1所示,其中黑色方格表示障礙物,其他白色方格為可通行區(qū)域,紅色三角形表示起點(diǎn)P,紅色圓圈表示目的地Q,虛線為規(guī)劃的路徑。當(dāng)按照規(guī)劃好的路徑移動(dòng),運(yùn)動(dòng)到M點(diǎn)時(shí)突然遇到障礙物(灰色方格所示),此時(shí)就不能繼續(xù)前進(jìn),此次路徑規(guī)劃失敗。

圖1 運(yùn)動(dòng)過(guò)程中突然出現(xiàn)障礙物Fig.1 Sudden obstructions during motion

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

考慮到車輛在野地中的行駛速度會(huì)比在道路上慢得多,因此將道路設(shè)置為最高優(yōu)先級(jí),即道路凌駕于障礙區(qū)之上,在居民區(qū)等不可通行區(qū)域中假如存在道路,那么車輛依然可以在這些障礙區(qū)內(nèi)的道路中行進(jìn)。由于傳統(tǒng)A*算法在生成8鄰域的子節(jié)點(diǎn)時(shí),沒(méi)有優(yōu)先順序。研究借鑒文獻(xiàn)[6]中的優(yōu)先級(jí)子節(jié)點(diǎn)生成策略提出一種改進(jìn)搜索策略。

節(jié)點(diǎn)O的子節(jié)點(diǎn)生成策略如圖2所示。圖2中把與當(dāng)前節(jié)點(diǎn)O的8個(gè)方向上的節(jié)點(diǎn)(A、B、C、D、P、Q、M、N)分成2組,將水平和垂直方向上的節(jié)點(diǎn)(A、B、C、D)設(shè)為優(yōu)先級(jí),將對(duì)角線方向上的節(jié)點(diǎn)(P、Q、M、N)設(shè)為次優(yōu)先級(jí)。在生成子節(jié)點(diǎn)的過(guò)程中,優(yōu)先生成節(jié)點(diǎn)A、B、C、D,至于剩下的次優(yōu)先級(jí)節(jié)點(diǎn)根據(jù)如下規(guī)則生成:

(1) 當(dāng)相鄰優(yōu)先節(jié)點(diǎn)A、B或B、C或C、D或D、A同是在道路上的節(jié)點(diǎn),則優(yōu)先生成此方向與其對(duì)角線方向的子節(jié)點(diǎn),比如節(jié)點(diǎn)A、B都在道路上,則優(yōu)先生成子節(jié)點(diǎn)P,并且路徑搜索方向偏向于OA、OB和OP;

圖2 節(jié)點(diǎn)O的子節(jié)點(diǎn)生成策略Fig.2 Node O’s child node generation policy

(2) 當(dāng)A、B、C、D同時(shí)都是道路或野地上的節(jié)點(diǎn),則生成的子節(jié)點(diǎn)沒(méi)有優(yōu)先順序;

(3) 當(dāng)優(yōu)先節(jié)點(diǎn)中包含有野地上的節(jié)點(diǎn),則不生成與其相鄰的子節(jié)點(diǎn),比如節(jié)點(diǎn)A在野地上,則不生成子節(jié)點(diǎn)P、N,也就不會(huì)生成路徑OA、OP、ON。

此方法優(yōu)先選取在道路上的節(jié)點(diǎn),從而在搜索路徑時(shí)優(yōu)先生成道路的路徑。為了區(qū)分道路速度和野地速度,通過(guò)MapX工具分別計(jì)算出所規(guī)劃路徑在道路和野地的長(zhǎng)度,計(jì)算方法是:首先判斷間隔足夠小的2個(gè)特征點(diǎn)是否同時(shí)在道路或野地上,然后通過(guò)迭代將每2個(gè)點(diǎn)間的距離分別相加得到道路和野地的長(zhǎng)度。同時(shí)給道路行駛速度和野地行駛速度取一個(gè)適當(dāng)?shù)谋壤?這里設(shè)定車輛在道路的速度為57 km/h,在野地的速度為26 km/h。

1.3 基于二次A*算法的路徑規(guī)劃原理

由上所述,A*算法的估價(jià)函數(shù)與環(huán)境信息有著密切的關(guān)系。在環(huán)境信息未知的情況下,A*算法規(guī)劃路徑存在很大的不足,且無(wú)法動(dòng)態(tài)規(guī)劃路徑。研究提出了一種預(yù)處理的算法,在進(jìn)行路徑規(guī)劃前先將所選區(qū)域進(jìn)行預(yù)處理,這樣就可以充分掌握該地區(qū)的環(huán)境信息,進(jìn)一步我們可以將該區(qū)域內(nèi)的可通行區(qū)域和障礙物進(jìn)行區(qū)分。同時(shí),加入預(yù)處理算法能夠有效彌補(bǔ)A*算法只能靜態(tài)規(guī)劃路徑的缺陷。在復(fù)雜的野外環(huán)境下,當(dāng)車輛由于遭遇極端天氣等不可抗力因素導(dǎo)致原先可通行區(qū)域變得不可通行,這時(shí)就需要實(shí)時(shí)更新局部環(huán)境信息從而實(shí)現(xiàn)動(dòng)態(tài)路徑規(guī)劃。

二次A*算法實(shí)質(zhì)上就是已規(guī)劃好的路徑在運(yùn)動(dòng)過(guò)程中突然出現(xiàn)未知障礙物時(shí),再次采用A*算法進(jìn)行障礙區(qū)的局部規(guī)劃而不需要重新運(yùn)用A*算法進(jìn)行全局規(guī)劃,局部規(guī)劃區(qū)域明顯小于全局規(guī)劃區(qū)域,因而局部規(guī)劃的時(shí)間比全局規(guī)劃少,提高了車輛運(yùn)動(dòng)的實(shí)時(shí)性。

基于二次A*算法的路徑規(guī)劃步驟如下:

(1) 地圖預(yù)處理,明確全局環(huán)境信息后選取起始點(diǎn)和終點(diǎn)進(jìn)行全局最優(yōu)路徑規(guī)劃;

(2) 運(yùn)動(dòng)過(guò)程中突然出現(xiàn)未知障礙物,那么已規(guī)劃好的路徑就無(wú)法到達(dá)目的地,此時(shí)更新局部環(huán)境信息,確定局部規(guī)劃區(qū)域,運(yùn)用A*算法進(jìn)行局部路徑規(guī)劃;

(3) 結(jié)合全局規(guī)劃和局部規(guī)劃得到最優(yōu)路徑。路徑規(guī)劃流程如圖3所示。

圖3 基于二次A*算法的路徑規(guī)劃流程Fig.3 Path planning flowchart based on the quatratic A* algorithm

1.4 確定局部規(guī)劃區(qū)域

車輛運(yùn)動(dòng)過(guò)程中突然遇到障礙物時(shí)需要進(jìn)行局部路徑規(guī)劃,當(dāng)運(yùn)動(dòng)的下一步有障礙物時(shí),記下當(dāng)前點(diǎn)的位置,并檢測(cè)前進(jìn)路徑中障礙物所占有的格子數(shù),記為n,設(shè)s=n+5,當(dāng)s為奇數(shù),則s=n+6。局部規(guī)劃區(qū)域如圖4所示。①當(dāng)下一步障礙物所占格子(圖4中黑色格子)在當(dāng)前格子(圖4(a)中灰色格子)的正上方(或正下方),則局部規(guī)劃區(qū)域?yàn)?s+1)×s,如圖4(a)中第2行到第8行的方框區(qū)域(圖4(a)中線條突出區(qū)域);②當(dāng)下一步障礙物所占格子在當(dāng)前格子的左側(cè)(或右側(cè)),則局部規(guī)劃區(qū)域?yàn)閟×(s+1),如圖4(b)中第2行到第7行的方框區(qū)域(圖4(b)中線條突出區(qū)域);③當(dāng)下一步障礙物所占格子在當(dāng)前格子的斜45°方向,則局部規(guī)劃區(qū)域?yàn)閟×(s+1),如圖4(c)中第2行到第9行的方框區(qū)域(圖4(c)中線條突出區(qū)域)。

圖4 局部規(guī)劃區(qū)域Fig.4 Local planning area

1.5 預(yù)處理算法分析

(1) 地圖環(huán)境構(gòu)建 真實(shí)地圖柵格化如圖5所示。圖5(a)表示的是真實(shí)地圖,其中綠色的部分和白色的部分分別表示障礙物區(qū)域和可通行區(qū)域。圖5(b)表示對(duì)真實(shí)地圖環(huán)境加以柵格化,黑色表示障礙區(qū),白色表示可通行區(qū)域。在將真實(shí)地圖柵格化的過(guò)程中,把障礙區(qū)適當(dāng)?shù)財(cái)U(kuò)大以達(dá)到車輛安全無(wú)碰撞通行的目的。

圖5 真實(shí)地圖柵格化Fig.5 Real map rasterization

(2) 地圖要素的提取與表示 地圖要素可以分為障礙要素和可通行要素。障礙要素包括居民區(qū)、湖泊、河流等,可通行要素一般是指野外環(huán)境下可以通行的道路。各個(gè)地圖要素的類型和處理方式如表1所列。

由于不可通行的障礙都是有特征的(線狀要素或面狀要素)而且相對(duì)于可通行區(qū)域要小的多,為了更方便路徑規(guī)劃,將不可通行區(qū)域提取出來(lái),把表示障礙的區(qū)域或者曲線用紅色繪制出來(lái),表示不可通行區(qū)域;同時(shí)將可通行的道路用綠色繪制出來(lái),表示可通行的道路。

障礙區(qū)域的提取與繪制見圖6。圖6(a)表示河流、湖泊以及居民區(qū)等障礙區(qū)域,白色條紋區(qū)域表示居民區(qū),淺藍(lán)色線條和區(qū)域分別表示河流和湖泊。經(jīng)過(guò)地圖要素的提取,將這些障礙區(qū)域用紅色標(biāo)出,對(duì)比圖6(a)和圖6(b),可以看出,在繪制障礙區(qū)域時(shí),將表示線狀要素的障礙區(qū)(河流)以及面狀要素的障礙區(qū)域(居民區(qū)和湖泊)的邊界適當(dāng)擴(kuò)大,來(lái)達(dá)到使車輛安全通行的目的。

道路及等距線繪制見圖7。圖7(a)中的黑色線條表示可通行的道路。經(jīng)過(guò)地圖要素的提取,將可通行的道路用綠色標(biāo)出(見圖7(b))。圍繞在綠色道路周圍的藍(lán)色線條則是道路等距線,在進(jìn)行路徑規(guī)劃時(shí)用來(lái)分析離道路的距離,從而達(dá)到快速接近道路的目的。

表1 地圖要素及其處理方式

圖6 障礙區(qū)域的提取與繪制Fig.6 Extraction and plotting of obstacle areas

圖7 道路及等距線繪制Fig.7 Road and equidistant line plotting

2 實(shí)驗(yàn)驗(yàn)證

為了驗(yàn)證提出方法的可行性,選擇沈陽(yáng)某地區(qū)的dem地圖進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)環(huán)境:Windows 10,Visual Studio 2010,MapX 5.0,以C++為實(shí)驗(yàn)平臺(tái)。由于dem地圖的區(qū)域過(guò)大,因此實(shí)驗(yàn)時(shí)選取其中具有代表性的一塊區(qū)域進(jìn)行分析。

研究將所選區(qū)域柵格化,具備居民區(qū)、湖泊、河流、道路等地圖要素,可以滿足實(shí)驗(yàn)的要求,如圖8所示。將所選區(qū)域進(jìn)行地圖預(yù)處理,明確全局環(huán)境信息,結(jié)果如圖9所示,圖9中的柵格表示采樣密度為8 000的實(shí)際大小。

圖8 地圖柵格化Fig.8 Map rasterization

圖9 地圖預(yù)處理結(jié)果Fig.9 Map preprocessing results

為了提升實(shí)驗(yàn)的可靠性,根據(jù)不同的地形條件選取4組不同的起點(diǎn)和終點(diǎn)進(jìn)行對(duì)比實(shí)驗(yàn),如圖10所示。圖10中黃線表示傳統(tǒng)A*算法規(guī)劃的路徑,藍(lán)線表示改進(jìn)A*算法規(guī)劃的路徑,圖10(a)～(d)對(duì)應(yīng)的地形條件越來(lái)越復(fù)雜,相應(yīng)的實(shí)驗(yàn)結(jié)果見表2和表3。當(dāng)遇到未知障礙物時(shí),二次A*算法動(dòng)態(tài)規(guī)劃的路徑結(jié)果如圖11所示,圖11(a)是沒(méi)有障礙物時(shí)的路徑規(guī)劃;圖11(b)為突然遇到障礙物,紅色網(wǎng)格區(qū)域?yàn)槲粗系K區(qū)域;圖11(c)為局部規(guī)劃后,結(jié)合全局規(guī)劃和局部規(guī)劃得到的運(yùn)動(dòng)路徑。

實(shí)際規(guī)劃路徑長(zhǎng)度對(duì)比見表2,相應(yīng)的搜索時(shí)間與行駛時(shí)間對(duì)比見表3。從實(shí)驗(yàn)結(jié)果可知:①預(yù)處理使全局環(huán)境信息更加明了,便于之后的路徑規(guī)劃。②改進(jìn)搜索策略的算法雖然在實(shí)際規(guī)劃的路徑長(zhǎng)度和搜索時(shí)間有所犧牲,但減少了行駛時(shí)間,提高了車輛的機(jī)動(dòng)性。從實(shí)驗(yàn)結(jié)果來(lái)看,隨著地形條件的不斷復(fù)雜,行駛時(shí)間分別減少了26.6%、29.5%、32.4%和35.2%,說(shuō)明改進(jìn)后的算法適用于復(fù)雜的環(huán)境。③局部規(guī)劃的區(qū)域比全局區(qū)域小,減少了總體規(guī)劃時(shí)間,提高了車輛運(yùn)動(dòng)的實(shí)時(shí)性。

3 結(jié)論

針對(duì)車輛無(wú)法在環(huán)境信息未知的復(fù)雜野外環(huán)境下快速到達(dá)目的地的問(wèn)題,提出了一種加入預(yù)處理算法的基于二次A*算法的車輛路徑規(guī)劃方法。通過(guò)地圖預(yù)處理明確全局環(huán)境信息,提取障礙物信息和可通行區(qū)域,通過(guò)改進(jìn)搜索策略的A*算法規(guī)劃出一條快速到達(dá)目的地的路徑,當(dāng)出現(xiàn)未知障礙物時(shí)確定局部規(guī)劃區(qū)域,結(jié)合全局規(guī)劃和局部規(guī)劃,得到最優(yōu)路徑。實(shí)驗(yàn)結(jié)果表明:改進(jìn)算法規(guī)劃的路徑雖然犧牲了部分搜索時(shí)間和路徑長(zhǎng)度,但對(duì)于車輛的機(jī)動(dòng)性有了極大的提高,有利于車輛在作戰(zhàn)或演習(xí)中快速到達(dá)指定位置,同時(shí)地形越復(fù)雜,算法的優(yōu)化程度越明顯,適用于復(fù)雜環(huán)境的路徑規(guī)劃。并且在遇到障礙物時(shí),由于局部規(guī)劃區(qū)域比全局區(qū)域小,因而減少了總體的規(guī)劃時(shí)間,提高了車輛的實(shí)時(shí)性。

圖10 2種算法對(duì)比Fig.10 Comparison of two kinds of algorithms

圖11 動(dòng)態(tài)路徑規(guī)劃Fig.11 Dynamic path planning

表2 實(shí)際規(guī)劃路徑長(zhǎng)度對(duì)比

表3 搜索時(shí)間與行駛時(shí)間對(duì)比