基于航跡規(guī)劃的無人機(jī)地形輔助導(dǎo)航

張睿, 李萬睿, 肖勇, 楊林, 許斌

(1.東南大學(xué) 自動化學(xué)院, 江蘇 南京 210096; 2.西北工業(yè)大學(xué) 自動化學(xué)院, 陜西 西安 710072; 3.成都飛機(jī)設(shè)計研究所, 四川 成都 610041)

地形輔助導(dǎo)航(terrain-aided navigation, TAN)是近年來受到廣泛重視并已成功使用的輔助導(dǎo)航技術(shù)[1]。地形輪廓匹配(terrain contour matching, TERCOM)具有完全自主、導(dǎo)航誤差不隨時間積累的優(yōu)點,它利用飛行路徑正下方的地形高度和存儲的參考高程地圖進(jìn)行比較,進(jìn)而得出飛行器的位置信息。目前TERCOM可以達(dá)到百米以內(nèi)的定位精度,使用TERCOM技術(shù)可以提高無人機(jī)(unmanned aerial vehicle,UAV)組合導(dǎo)航系統(tǒng)的系性能和可靠性,提高戰(zhàn)機(jī)低空突防能力。

隨著國內(nèi)外學(xué)者的廣泛研究,在傳統(tǒng)TERCOM的基礎(chǔ)上已演變出了基于最近等值點迭代法[2]和基于濾波技術(shù)的桑迪亞慣性地形輔助導(dǎo)航[3]等多種匹配方法。但是基于高程的地形匹配普遍面臨著受匹配精度受地形起伏程度影響較大的問題。針對上述問題,文獻(xiàn)[4-5]提出了二維圖像與地形高程相結(jié)合的匹配方式,以減弱地形輔助導(dǎo)航對地形高程變化的依賴。文獻(xiàn)[6]提出了一種基于粒子濾波和長短時記憶網(wǎng)絡(luò)相結(jié)合的TAN算法,來降低飛行器在平坦地形上的導(dǎo)航誤差。采用傳統(tǒng)平均平方差算法(mean square difference, MSD)、平均絕對差算法(mean absolute difference, MAD)和交叉相關(guān)算法(cross correlation, COR)匹配算子的TERCOM算法,存在耗時長實時性差的問題[7]。經(jīng)大量研究證實[8-10],采用智能優(yōu)化算法代替?zhèn)鹘y(tǒng)地形匹配中的遍歷算法能夠保證搜索最佳匹配位置,并且提高搜索匹配的效率。粒子群優(yōu)化算法(particle swarm optimization, PSO)作為智能群優(yōu)化方法的典型代表[11],具有結(jié)構(gòu)簡單、狀態(tài)更新容易實現(xiàn)且沒有許多需要調(diào)節(jié)的參數(shù)的優(yōu)點,廣泛應(yīng)用于各類優(yōu)化問題中,近年來也應(yīng)用于地形匹配中[12]。但是在實際應(yīng)用過程中,粒子群優(yōu)化算法存在早熟收斂的問題,容易陷入局部極值點,從而導(dǎo)致不能收斂到全局極值點,為了提升粒子群算法的尋優(yōu)性能,本文在PSO基礎(chǔ)上引入分布估計(estimation of distribution algorithm, EDA)的思想,加強(qiáng)PSO算法的全局尋優(yōu)能力,形成了分布估計思想的優(yōu)化PSO算法。

本文研究基于航跡規(guī)劃的地形輔助導(dǎo)航算法。首先采用A*算法進(jìn)行航跡規(guī)劃,為飛行器規(guī)劃出途經(jīng)地形起伏較大區(qū)域的航路,其次采用優(yōu)化PSO算法進(jìn)行地形匹配,改善傳統(tǒng)TERCOM實時性差的問題,最后進(jìn)行慣性導(dǎo)航/地形匹配組合導(dǎo)航。

1 基于A*算法的地形輔助導(dǎo)航航跡規(guī)劃

TERCOM算法對地形的依賴性較強(qiáng),所以匹配區(qū)域的地形適配性是否滿足算法的要求就顯得至關(guān)重要。本文采用航跡規(guī)劃選出始終在地形起伏明顯區(qū)域的飛行航跡,再利用地形輔助導(dǎo)航算法提升導(dǎo)航精度,其工作流程如圖1所示。

圖1 基于航跡規(guī)劃的地形輔助導(dǎo)航算法原理Fig.1 Principle of terrain aided navigation algorithm Based on track planning

基于A*算法的地形輔助導(dǎo)航航跡規(guī)劃的原理為:首先對無人機(jī)需要途徑的區(qū)域進(jìn)行分塊并計算各塊地形標(biāo)準(zhǔn)差,根據(jù)地形標(biāo)準(zhǔn)差選出地形平坦區(qū)域和地形起伏區(qū)域,將地形平坦區(qū)域視為航跡規(guī)劃中的威脅區(qū)域,避免駛?cè)?其次,在航跡規(guī)劃時引入地形信息,利用A*算法在地形特征約束下進(jìn)行離線航跡規(guī)劃。

基于A*算法的地形輔助導(dǎo)航航跡規(guī)劃步驟為:

1)獲取無人機(jī)任務(wù)區(qū)間數(shù)字地圖,如圖2。

圖2 任務(wù)區(qū)間三維地形圖Fig.2 3D topographic map of mission area

2)計算地形標(biāo)準(zhǔn)差。地形標(biāo)準(zhǔn)差通常用來描述地形的總體起伏程度,為保證地形標(biāo)準(zhǔn)差對地形有較強(qiáng)的分級能力,將數(shù)字地圖分為1 km×1 km的網(wǎng)格,并計算各分塊區(qū)域內(nèi)的地形標(biāo)準(zhǔn)差,地形標(biāo)準(zhǔn)差表示為[13]:

(1)

3)劃分地形適配區(qū)。當(dāng)?shù)匦螛?biāo)準(zhǔn)差值大于18時,該區(qū)域地形起伏較大[14],可進(jìn)行地形匹配,故將地形標(biāo)準(zhǔn)差值大于18的區(qū)域劃分為地形適配區(qū),如圖3所示。

圖3 地形平坦區(qū)域劃分Fig.3 Division of flat terrain

4)采用A*算法進(jìn)行航跡規(guī)劃,使無人機(jī)盡可能地飛行在地形適配區(qū)內(nèi)。A*算法的航跡代價函數(shù)為[15]:

f(x)=g(x)+u(x)

(2)

式中:x為當(dāng)前節(jié)點;g(x)為當(dāng)前節(jié)點到起點的真實路徑長度;u(x)為當(dāng)前節(jié)點到目標(biāo)位置的路徑長度估值;f(x)代表從航跡規(guī)劃初始點經(jīng)過點x到達(dá)目標(biāo)點的最小航跡長度估計值。

2 基于優(yōu)化PSO算法的TERCOM方法

傳統(tǒng)TERCOM算法采用遍歷搜索方式進(jìn)行實時高程與基準(zhǔn)子圖間的匹配,當(dāng)捷聯(lián)慣導(dǎo)系統(tǒng)(strapdown inertial navigation system,SINS)位置誤差積累較大時,基準(zhǔn)子圖的搜索范圍增大,此時的搜索效率較低。本文采用優(yōu)化粒子群算法代替?zhèn)鹘y(tǒng)TERCOM中的遍歷方法,以提升TERCOM的搜索效率。

在優(yōu)化粒子群算法中,粒子通過PSO算法與EDA算法交替更新,由于EDA中的種群是根據(jù)基準(zhǔn)子圖的適應(yīng)度值由最優(yōu)到最劣排序的,所以PSO-EDA組合方法可以避免搜索方向向局部最優(yōu)快速收斂。優(yōu)化PSO算法可以替代傳統(tǒng)TERCOM中遍歷搜索部分,采用優(yōu)化PSO算法TERCOM的步驟為:

1)實時高程采集。在匹配時,為了避免速度變化引起實測高程序列與數(shù)字地圖網(wǎng)格不匹配,通常將SINS的軌跡進(jìn)行等間距采樣,即當(dāng)UAV飛過一個地圖網(wǎng)格的間距時,保存高程值為一個采樣點[16]。

2)初始化粒子。在以慣性導(dǎo)航指示位置為中心,±3σ的范圍構(gòu)建搜索區(qū)域,在搜索區(qū)域內(nèi)以實測地形高程序列為模板所形成的地形高程序列即為一條基準(zhǔn)子圖,在使用優(yōu)化PSO算法搜索時,每條基準(zhǔn)子圖即為一個粒子,如圖4所示。

圖4 粒子的表示方法Fig.4 Representation of particles

3)計算粒子適應(yīng)度值。采用平均方差算法(mean square difference, MSD)計算粒子的適應(yīng)度值,MSD的計算為[7]:

(3)

式中:N為高度序列的個數(shù);xi為地形剖面中的第i個高程值;yi為實測高程序列中第i個高程值。TERCOM匹配的過程就是找到JMSD最小值的過程。

4)采用EDA算法更新種群。在TERCOM中,基準(zhǔn)子圖的位置分布服從二維高斯分布[17],即:

(4)

式中xture、yture為飛行器真實位置的經(jīng)度和緯度。

用適應(yīng)度最優(yōu)粒子的位置分布更新二維高斯分布模型中的均值與方差,并根據(jù)更新后的模型采樣生成新的種群。

5)重新計算粒子適應(yīng)度值。計算新種群內(nèi)每個粒子的適應(yīng)度值。

6)采用PSO算法個體最優(yōu)值與全局最優(yōu)值。對每個粒子,將它當(dāng)前適應(yīng)度值分別和個體最優(yōu)適應(yīng)度值、全局最優(yōu)適應(yīng)度值相比較,如果適應(yīng)度值較小,則對當(dāng)前個體最優(yōu)位置和全局最優(yōu)位置進(jìn)行更新,粒子的速度和位置更新公式為[11]:

(5)

7)判斷終止條件。若未到達(dá)最大迭代次數(shù),根據(jù)式(2)進(jìn)行粒子的位置和速度更新,返回步驟2)進(jìn)行下一次迭代。

8)輸出定位結(jié)果。輸出全局最優(yōu)值粒子所對應(yīng)的最佳匹配位置。

綜上所述,基于改進(jìn)粒子群的地形匹配方法搜索流程如圖5所示。

圖5 優(yōu)化PSO更新步驟Fig.5 Optimize PSO update steps

3 慣性/地形組合導(dǎo)航

UAV的慣性導(dǎo)航系統(tǒng)與地形匹配導(dǎo)航系統(tǒng)具有優(yōu)勢互補(bǔ)的特點,將兩者組合可以克服各自的缺點,取長補(bǔ)短。組合系統(tǒng)結(jié)構(gòu)框圖如圖6所示。

圖6 TAN松組合系統(tǒng)結(jié)構(gòu)Fig.6 Structure block diagram of TAN loose combination system

TERCOM誤差受數(shù)字地圖分辨率、高度表測量誤差等綜合影響,沒有明確的數(shù)學(xué)模型,故選取捷聯(lián)慣性導(dǎo)航系統(tǒng)的狀態(tài)量為組合導(dǎo)航系統(tǒng)的狀態(tài)變量。

選擇捷聯(lián)慣導(dǎo)系統(tǒng)的位置誤差、速度誤差、姿態(tài)誤差、陀螺漂移和加速度計零偏作為狀態(tài)量[17]:

(6)

根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)的誤差方程,可得地形輔助導(dǎo)航系統(tǒng)的狀態(tài)方程為[18]:

(7)

式中:A為狀態(tài)矩陣;G為狀態(tài)噪聲矩陣;W為噪聲矢量。

選擇地形匹配位置與捷聯(lián)慣導(dǎo)系統(tǒng)解算的位置作為地形輔助組合導(dǎo)航系統(tǒng)的量測量:

(8)

根據(jù)式(6)、(7)建立的狀態(tài)模型與量測模型,便可使用Kalman濾波算法對SINS進(jìn)行位置校正。

4 仿真及分析

4.1 基于優(yōu)化PSO算法的TERCOM方法仿真驗證

本文仿真中基于優(yōu)化PSO算法的TERCOM方法與基于MSD算子的傳統(tǒng)TERCOM方法的匹配成功率與單次匹配時間進(jìn)行對比,除匹配算法外,其余仿真參數(shù)保持一致。實驗區(qū)域選為四川甘孜州雅江縣,地圖范圍為北緯30°～30.15°,東經(jīng)100°～100.15°。

設(shè)UAV以30 m/s的速度勻速航行,高程的采樣間隔為1 s,航行時間為400 s,整段航跡共有500個采樣點,為保證第1次和最后一次匹配時有足夠的實測序列,采樣至第30個采樣點位置后開始匹配,整段航跡分為44個等長的匹配序列。實測高程序列由真實高程值加誤差形成,搜索區(qū)域大小由3.2節(jié)的搜索區(qū)域確定方法給出。改進(jìn)PSO算法參數(shù)設(shè)置如表1所示。

表1 優(yōu)化PSO算法主要參數(shù)Table 1 The main parameters of optimize PSO algorithm

基于優(yōu)化PSO算法的TERCOM方法與基于遍歷算法的傳統(tǒng)TERCOM方法的匹配結(jié)果如圖7所示,匹配誤差如圖8所示。設(shè)進(jìn)入匹配區(qū)時SINS位置誤差為300 m,每次匹配的搜索范圍為1 200 m×1 200 m的正方形區(qū)域,共1 600個基準(zhǔn)子圖。

圖7 優(yōu)化PSO算法/遍歷算法匹配結(jié)果Fig.7 Optimize PSO algorithm/traversal algorithm matching results

圖8 優(yōu)化PSO算法/遍歷算法匹配誤差Fig.8 Optimize PSO algorithm/traversal algorithm matching error

基于優(yōu)化PSO算法的TERCOM方法與傳統(tǒng)TERCOM方法的匹配成功率與單次匹配時間對比如表2所示。

表2 2種匹配算法的地形匹配結(jié)果比較Table 2 Comparison of terrain matching results of two matching algorithms

由表2可直觀看出:基于遍歷的傳統(tǒng)TERCOM算法的平均每次匹配時長為0.001 1 s,基于優(yōu)化PSO算法的TERCOM方法平均每次匹配時長為0.000 3 s,僅約為傳統(tǒng)TERCOM算法的1/3,并且匹配成功率更高。因此基于優(yōu)化PSO算法的TERCOM是有效的,在實際應(yīng)用中更具優(yōu)勢。

4.2 基于航跡規(guī)劃的無人機(jī)地形輔助導(dǎo)航仿真

通過比較未進(jìn)行航跡規(guī)劃的地形輔助導(dǎo)航與引入航跡規(guī)劃的地形輔助導(dǎo)航的導(dǎo)航結(jié)果,驗證在無人機(jī)任務(wù)區(qū)間包含非匹配區(qū)時,基于航跡規(guī)劃的地形輔助導(dǎo)航具有更高的導(dǎo)航精度。地形匹配算法為本文提出的基于改進(jìn)PSO算法的TERCOM方法。仿真主要參數(shù)如表3所示。

表3 TAN主要參數(shù)Table 3 Main parameters of TAN

設(shè)UAV的航行起點為東經(jīng)107.1°,北緯33.2°,終點為東經(jīng)107.7°,北緯33.7°,進(jìn)行2次仿真實驗,實驗1為未進(jìn)行航跡規(guī)劃的地形輔助導(dǎo)航,為對照組,實驗2為航跡規(guī)劃后的地形輔助導(dǎo)航。為保證采集到足夠的實測高程序列,從100 s開始進(jìn)行地形輪廓匹配。如圖9所示,2條軌跡的起點與終點相同,航行區(qū)間內(nèi)有大量的地形平坦區(qū)域,即圖中地形等高線稀疏的區(qū)域。

圖9 航跡規(guī)劃前后地形輔助導(dǎo)航軌跡對比Fig.9 Comparison of terrain-aided navigation trajectories before and after track planning

航跡規(guī)劃前后地形輔助導(dǎo)航軌跡對比如圖9所示,如果UAV從任務(wù)起點直線飛行至目標(biāo)點,則途中經(jīng)過大量地形平坦區(qū)域,未進(jìn)行航跡規(guī)劃時的地形輔助導(dǎo)航位置誤差如圖10所示。經(jīng)過航跡規(guī)劃后,UAV繞過地形平坦區(qū)域航行至目標(biāo)點,位置誤差如圖11所示。

圖10 未航跡規(guī)劃的地形輔助導(dǎo)航位置誤差Fig.10 Terrain aided navigation position error without track planning

圖11 航跡規(guī)劃后的地形輔助導(dǎo)航位置誤差Fig.11 Terrain-aided navigation position error after track planning

圖10中,未加航跡規(guī)劃的地形輔助導(dǎo)航的最大位置誤差為496 m,而在飛行前進(jìn)行航跡規(guī)劃,使UAV的航線避開地形平坦區(qū)域后,最大定位誤差僅為71 m,航跡規(guī)劃路徑上的定位誤差僅為未規(guī)劃航跡路徑上的1/7。

5 結(jié)論

1)本文針對傳統(tǒng)地形輔助導(dǎo)航受地形影響大和實時性差的問題,提出基于航跡規(guī)劃的無人機(jī)地形輔助導(dǎo)航方法。

2)在飛行前首先結(jié)合地形特征進(jìn)行航跡規(guī)劃,在搜索匹配時充分利用粒子群優(yōu)化算法的快速尋優(yōu)能力,并采用分布估計算法輔助更新粒子群算法中種群的分布,達(dá)到快速匹配和定位,提高了導(dǎo)航系統(tǒng)的綜合能力。

3)仿真分析表明,基于優(yōu)化PSO算法的地形匹配算法的單次匹配用時明顯低于傳統(tǒng)TERCOM算法,且經(jīng)過預(yù)先航跡規(guī)劃的地形輔助導(dǎo)航算法的匹配精度明顯優(yōu)于未經(jīng)過航跡規(guī)劃的地形輔助導(dǎo)航算法。