基于航跡規劃的無人機地形輔助導航

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

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

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

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

本文研究基于航跡規劃的地形輔助導航算法。首先采用A*算法進行航跡規劃,為飛行器規劃出途經地形起伏較大區域的航路,其次采用優化PSO算法進行地形匹配,改善傳統TERCOM實時性差的問題,最后進行慣性導航/地形匹配組合導航。

1 基于A*算法的地形輔助導航航跡規劃

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

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

基于A*算法的地形輔助導航航跡規劃的原理為:首先對無人機需要途徑的區域進行分塊并計算各塊地形標準差,根據地形標準差選出地形平坦區域和地形起伏區域,將地形平坦區域視為航跡規劃中的威脅區域,避免駛入;其次,在航跡規劃時引入地形信息,利用A*算法在地形特征約束下進行離線航跡規劃。

基于A*算法的地形輔助導航航跡規劃步驟為:

1)獲取無人機任務區間數字地圖,如圖2。

圖2 任務區間三維地形圖Fig.2 3D topographic map of mission area

2)計算地形標準差。地形標準差通常用來描述地形的總體起伏程度,為保證地形標準差對地形有較強的分級能力,將數字地圖分為1 km×1 km的網格,并計算各分塊區域內的地形標準差,地形標準差表示為[13]:

(1)

3)劃分地形適配區。當地形標準差值大于18時,該區域地形起伏較大[14],可進行地形匹配,故將地形標準差值大于18的區域劃分為地形適配區,如圖3所示。

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

4)采用A*算法進行航跡規劃,使無人機盡可能地飛行在地形適配區內。A*算法的航跡代價函數為[15]:

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

(2)

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

2 基于優化PSO算法的TERCOM方法

傳統TERCOM算法采用遍歷搜索方式進行實時高程與基準子圖間的匹配,當捷聯慣導系統(strapdown inertial navigation system,SINS)位置誤差積累較大時,基準子圖的搜索范圍增大,此時的搜索效率較低。本文采用優化粒子群算法代替傳統TERCOM中的遍歷方法,以提升TERCOM的搜索效率。

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

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

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

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

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

(3)

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

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

(4)

式中xture、yture為飛行器真實位置的經度和緯度。

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

5)重新計算粒子適應度值。計算新種群內每個粒子的適應度值。

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

(5)

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

8)輸出定位結果。輸出全局最優值粒子所對應的最佳匹配位置。

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

圖5 優化PSO更新步驟Fig.5 Optimize PSO update steps

3 慣性/地形組合導航

UAV的慣性導航系統與地形匹配導航系統具有優勢互補的特點,將兩者組合可以克服各自的缺點,取長補短。組合系統結構框圖如圖6所示。

圖6 TAN松組合系統結構Fig.6 Structure block diagram of TAN loose combination system

TERCOM誤差受數字地圖分辨率、高度表測量誤差等綜合影響,沒有明確的數學模型,故選取捷聯慣性導航系統的狀態量為組合導航系統的狀態變量。

選擇捷聯慣導系統的位置誤差、速度誤差、姿態誤差、陀螺漂移和加速度計零偏作為狀態量[17]:

(6)

根據捷聯慣導系統的誤差方程,可得地形輔助導航系統的狀態方程為[18]:

(7)

式中:A為狀態矩陣;G為狀態噪聲矩陣;W為噪聲矢量。

選擇地形匹配位置與捷聯慣導系統解算的位置作為地形輔助組合導航系統的量測量:

(8)

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

4 仿真及分析

4.1 基于優化PSO算法的TERCOM方法仿真驗證

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

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

表1 優化PSO算法主要參數Table 1 The main parameters of optimize PSO algorithm

基于優化PSO算法的TERCOM方法與基于遍歷算法的傳統TERCOM方法的匹配結果如圖7所示,匹配誤差如圖8所示。設進入匹配區時SINS位置誤差為300 m,每次匹配的搜索范圍為1 200 m×1 200 m的正方形區域,共1 600個基準子圖。

圖7 優化PSO算法/遍歷算法匹配結果Fig.7 Optimize PSO algorithm/traversal algorithm matching results

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

基于優化PSO算法的TERCOM方法與傳統TERCOM方法的匹配成功率與單次匹配時間對比如表2所示。

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

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

4.2 基于航跡規劃的無人機地形輔助導航仿真

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

表3 TAN主要參數Table 3 Main parameters of TAN

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

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

航跡規劃前后地形輔助導航軌跡對比如圖9所示,如果UAV從任務起點直線飛行至目標點,則途中經過大量地形平坦區域,未進行航跡規劃時的地形輔助導航位置誤差如圖10所示。經過航跡規劃后,UAV繞過地形平坦區域航行至目標點,位置誤差如圖11所示。

圖10 未航跡規劃的地形輔助導航位置誤差Fig.10 Terrain aided navigation position error without track planning

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

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

5 結論

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

2)在飛行前首先結合地形特征進行航跡規劃,在搜索匹配時充分利用粒子群優化算法的快速尋優能力,并采用分布估計算法輔助更新粒子群算法中種群的分布,達到快速匹配和定位,提高了導航系統的綜合能力。

3)仿真分析表明,基于優化PSO算法的地形匹配算法的單次匹配用時明顯低于傳統TERCOM算法,且經過預先航跡規劃的地形輔助導航算法的匹配精度明顯優于未經過航跡規劃的地形輔助導航算法。