基于動(dòng)態(tài)稀疏A*算法的廣域無人機(jī)航路規(guī)劃

文/黃文剛 陳奭

無人機(jī)（UAV）由于其人員零傷亡、高機(jī)動(dòng)性、費(fèi)用低等一系列優(yōu)點(diǎn)，在軍事和民事領(lǐng)域受到廣泛關(guān)注。對(duì)于UAV的航路規(guī)劃，國內(nèi)外學(xué)者提出了很多方法，如A*算法、遺傳算法、蟻群算法等。在眾多算法中，A*算法由于其簡單高效而得到廣泛應(yīng)用。但A*算法要收斂到最優(yōu)解需要很長的時(shí)間和較大的運(yùn)算資源，且生成的航跡并不一定滿足UAV的機(jī)動(dòng)約束條件。Szczerba等人于2000年提出在搜索算法中結(jié)合約束條件的稀疏A*算法，有效地縮短了計(jì)算時(shí)間。然而稀疏A*算法在復(fù)雜廣域環(huán)境規(guī)劃高質(zhì)量的航路仍較為耗時(shí)，而現(xiàn)代戰(zhàn)爭，分秒必爭，基于此，本文提出了針對(duì)復(fù)雜廣域環(huán)境下的分層動(dòng)態(tài)稀疏A*航路規(guī)劃算法。

1 基于稀疏A*算法的航路規(guī)劃

首先，航跡代價(jià)的計(jì)算公式描述為：

圖1：當(dāng)前節(jié)點(diǎn)的二維搜索區(qū)域

圖2：分層算法架構(gòu)

圖3：廣域動(dòng)態(tài)稀疏A*算法航路規(guī)劃

圖4：分層方法粗規(guī)劃航路

其中，J是航路規(guī)劃的代價(jià)函數(shù)；n為總航跡段數(shù)；li表示第i段航跡的長度，通過約束航跡的總長度可縮短UAV的飛行時(shí)間，既節(jié)省了油耗，又降低了UAV的危險(xiǎn)系數(shù)；fi,threat為第i段航跡的威脅指數(shù)，它限制UAV不要與已知的威脅障礙距離太近；w1、w2為權(quán)系數(shù)。

稀疏A*算法是標(biāo)準(zhǔn)的啟發(fā)式搜索算法A*的改進(jìn)。傳統(tǒng)的A*算法進(jìn)行航跡搜索時(shí)，通常將規(guī)劃環(huán)境網(wǎng)格化，通過預(yù)先確定的代價(jià)函數(shù)（式（2））尋找最小代價(jià)航跡。

圖5：分段小區(qū)域航路規(guī)劃圖

圖6：分層航路規(guī)劃全局航路圖

其中，g(n)為從起始點(diǎn)到當(dāng)前節(jié)點(diǎn)n的實(shí)際代價(jià)，啟發(fā)函數(shù)h(n)表示從當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)的估值，a、b代表真實(shí)代價(jià)和預(yù)計(jì)代價(jià)的加權(quán)。A*算法每一步的擴(kuò)展中，具有最小f(n)值的節(jié)點(diǎn)被選擇并插入可能路徑OPEN鏈表中。

稀疏A*算法結(jié)合的約束條件主要包括最小搜索步長、最大轉(zhuǎn)角、航跡距離約束和固定目標(biāo)進(jìn)入方向等。最小步長是無人機(jī)在改變姿態(tài)時(shí)必須直飛的距離，也是每次搜索的最小距離；最大轉(zhuǎn)角約束生成航路的最大拐彎角。稀疏A*算法以扇形節(jié)點(diǎn)作為擴(kuò)展節(jié)點(diǎn)，縮減了搜索區(qū)域和搜索時(shí)間。關(guān)于稀疏A*算法的詳細(xì)表述可以參考文獻(xiàn)[2]。

2 基于動(dòng)態(tài)稀疏A*算法的航路規(guī)劃

在無人機(jī)航路規(guī)劃系統(tǒng)中，無人機(jī)安全有效地規(guī)避障礙物直至到達(dá)目標(biāo)點(diǎn)，規(guī)劃航路的搜索擴(kuò)展方法是關(guān)鍵。在稀疏A*算法的基礎(chǔ)上，結(jié)合飛行環(huán)境，通過定長的OPEN表、改進(jìn)搜索擴(kuò)展方法、二次優(yōu)化等思想，給出了一種基于可行優(yōu)先準(zhǔn)則的航路規(guī)劃方法，即動(dòng)態(tài)稀疏A*算法，該算法極大地縮短空間搜索時(shí)間并減小搜索空間，同時(shí)保持較高航路飛行品質(zhì)。

2.1 改進(jìn)擴(kuò)展規(guī)劃區(qū)域

稀疏A*算法在節(jié)點(diǎn)擴(kuò)展過程中，結(jié)合航路約束條件有效地減小了搜索空間，縮短了搜索時(shí)間。動(dòng)態(tài)稀疏A*算法借用稀疏A*算法中的“稀疏化”思想，采用定長的OPEN表，減少長距離時(shí)間的時(shí)間消耗，但與此同時(shí)又借鑒變步長思想通過離散化其局部規(guī)劃區(qū)域而進(jìn)行了改進(jìn)。給定約束條件θ（最大轉(zhuǎn)角）和最大搜索步長Lmax和最小搜索步長Lmin，則離散化的當(dāng)前搜索區(qū)域如圖1所示。若由節(jié)點(diǎn)Pk-1擴(kuò)展到當(dāng)前擴(kuò)展到節(jié)點(diǎn)Pk，以節(jié)點(diǎn)Pk點(diǎn)為圓點(diǎn)建立搜索區(qū)域?yàn)椴介LLmax為半徑，夾角大小為2θ的扇形區(qū)域，動(dòng)態(tài)稀疏A*算法將扇形區(qū)域進(jìn)行點(diǎn)陣式離散化，n等分2θ的扇面，m等分Lmax與Lmin之間的距離，可以建立起(n+1)?m的待擴(kuò)展點(diǎn)陣B。一般應(yīng)根據(jù)規(guī)劃環(huán)境選取合適的Lmax與Lmin。而m，n的值越大，找到滿足要求的航路的概率就越大，但同時(shí)內(nèi)存要求和收斂時(shí)間也相應(yīng)增加。

搜索過程中并不需要遍歷扇形區(qū)域內(nèi)的每一個(gè)位置，而只需考慮其中的離散化點(diǎn)陣節(jié)點(diǎn)，依據(jù)上圖中給出的搜索方法，由Pk點(diǎn)至第Pk+1點(diǎn)可擴(kuò)展點(diǎn)Bi,j的坐標(biāo)(xk+1,yk+1)計(jì)算如下：

其中，：

相比稀疏A*算法的搜索區(qū)域，動(dòng)態(tài)稀疏A*算法因其離散化處理故可以選取較大的局部搜索區(qū)域，克服了搜索區(qū)域大小和地圖分辨精度之間的矛盾，且規(guī)劃時(shí)間短。

2.2 基于二次優(yōu)化的航跡優(yōu)化

初始航路點(diǎn)生成之后，在滿足飛機(jī)機(jī)動(dòng)性能下，采取二次優(yōu)化算法進(jìn)行航路優(yōu)化，優(yōu)化的航路可有效縮短曲折路徑，減少了飛機(jī)動(dòng)力消耗，縮短了飛行時(shí)間；該二次優(yōu)化算法也可用于地面無人車、無人機(jī)船等規(guī)劃路徑的優(yōu)化。

3 分層動(dòng)態(tài)稀疏A*算法

分層動(dòng)態(tài)稀疏A*算法采用分層思想處理不同環(huán)境約束，分割規(guī)劃環(huán)境，先后對(duì)分層區(qū)域進(jìn)行航路規(guī)劃，相比單一的動(dòng)態(tài)稀疏A*算法優(yōu)勢在于面對(duì)較大的較為復(fù)雜的規(guī)劃環(huán)境時(shí)，分層稀疏A*算法能夠更加快速規(guī)劃出高質(zhì)量的飛行航路。分層算法框架結(jié)構(gòu)如圖2所示，首先分析規(guī)劃環(huán)境，明確無人機(jī)任務(wù)的起始點(diǎn)、目標(biāo)點(diǎn)、任務(wù)引導(dǎo)點(diǎn)、安全區(qū)、匹配區(qū)，以及規(guī)劃空間中的地形、敵方雷達(dá)、禁飛區(qū)等威脅；然后通過計(jì)算將相交連的中小威脅合并為一個(gè)整體，確定整體的威脅范圍；設(shè)置合理的威脅范圍門限，忽略作用范圍小于門限的威脅，結(jié)合搜索環(huán)境確定合適的步長，在含有大范圍威脅的環(huán)境中進(jìn)行粗航路規(guī)劃；然后選取中繼航路點(diǎn)，分割規(guī)劃環(huán)境，在小范圍內(nèi)加載所有威脅障礙信息后進(jìn)行精細(xì)航路規(guī)劃，最后順序連接每段小范圍內(nèi)規(guī)劃的航路得到整個(gè)大范圍的完整航路。分層規(guī)劃是一種處理大范圍任務(wù)區(qū)域的思想，具體的粗細(xì)規(guī)劃也可采用其他算法。

中繼航路點(diǎn)可以通過算法自動(dòng)選取，將第一層粗規(guī)劃的航路按一定航跡長度劃分，劃分間隔位置選為中繼航路點(diǎn)。簡單方便起見也可以選取一些距理想航路較遠(yuǎn)的粗規(guī)劃航路點(diǎn)作為中繼航路點(diǎn)。

4 仿真結(jié)果

分層稀疏A*算法在Pentium(R) Dual-Core 2.6GHz PC機(jī)上進(jìn)行了仿真實(shí)驗(yàn)，運(yùn)行環(huán)境為Windows XP，編程環(huán)境為matlabR2011b。廣域任務(wù)規(guī)劃區(qū)域見圖3?；痉抡鎱?shù)設(shè)定為：任務(wù)區(qū)域?yàn)?000km×1000km的正方形區(qū)域；航路起點(diǎn)start=[5,5]，目標(biāo)點(diǎn)target=[950,950]；威脅源用圖中紅色圓形區(qū)域表示，圓形區(qū)域之內(nèi)為不可飛行區(qū)域；代價(jià)函數(shù)中系數(shù)，w1=w2=1/2；在進(jìn)行節(jié)點(diǎn)擴(kuò)展時(shí)，N=4；最大轉(zhuǎn)角θ=45°；

在仿真中，設(shè)計(jì)了采用同分層算法的精細(xì)規(guī)劃中步長相同的動(dòng)態(tài)稀疏A*算法進(jìn)行航路規(guī)劃，作為對(duì)比。如圖3所示，算法中選取最小步長L0=2km，大步長L=2L0，算法規(guī)劃初始航跡如圖中青色線，優(yōu)化后如藍(lán)色實(shí)線。算法規(guī)劃的航跡長1396.3403km，耗時(shí)6.3715s。

圖4為分層算法中基于動(dòng)態(tài)稀疏A*算法的粗規(guī)劃航路，算法中選取最小步長L0=10km，大步長L=2L0，選取威脅門限為45km，即在粗規(guī)劃中整體威脅半徑小于45km的威脅被忽略。粗規(guī)劃稀疏A*算法規(guī)劃初始航跡如圖中青色虛線，優(yōu)化后如藍(lán)色實(shí)線，規(guī)劃耗時(shí)0.6819s。結(jié)合粗規(guī)劃航路和全局環(huán)境選取中繼航路點(diǎn)(a)(148.4605,216.3442)、(b)(286.1584,424.8660)、(c)(647.9430,651.3517)、(d)(766.0722,796.5406)，從起點(diǎn)向終點(diǎn)方向依次如圖中綠色星號(hào)。

圖5中（a）～（e）為按起點(diǎn)到終點(diǎn)方向次序的小區(qū)域的航路規(guī)劃圖。算法中選取最小步長L0=2km，大步長L=2L0，精細(xì)規(guī)劃稀疏A*算法規(guī)劃初始航跡如圖中青色虛線，優(yōu)化后如藍(lán)色實(shí)線，優(yōu)化后航跡長度，圖（a）256.9741km，圖（b）250.2602km，圖（c）428.3721km，圖（d）187.2797km，圖（e）241.4205km，耗時(shí)分別為0.6685s、0.6612s、0.9106s、0.5614s、0.6521s。

圖6為采用分層規(guī)劃算法規(guī)劃的全局航路圖，圖中藍(lán)色航路為圖5中5個(gè)子圖各自規(guī)劃后的優(yōu)化航路依次銜接而成，全長1364.3066km。分層算法耗時(shí)為粗規(guī)劃時(shí)間加所有子圖的規(guī)劃時(shí)間，即3.2251s。如果幾個(gè)子圖采取并行規(guī)劃，則分層算法耗時(shí) 粗規(guī)劃時(shí)間+max（子圖（a）～子圖（e）規(guī)劃時(shí)間）。

5 結(jié)束語

通過仿真圖與數(shù)據(jù)可以看出，相比直接進(jìn)行廣域規(guī)劃，基于分層動(dòng)態(tài)稀疏A*算法的航路規(guī)劃既保持規(guī)劃精度又縮短了規(guī)劃時(shí)間，精細(xì)規(guī)劃中每個(gè)小區(qū)域相對(duì)獨(dú)立，可以結(jié)合各自環(huán)境選取合適的算法進(jìn)行規(guī)劃，且可以借助多處理器進(jìn)行并行規(guī)劃。分層動(dòng)態(tài)稀疏A*算法耗時(shí)更少，規(guī)劃航跡更短，飛行品質(zhì)更優(yōu)。