魯棒性地形匹配/慣性組合導航算法

程傳奇，郝向陽，張振杰，馬智剛

（信息工程大學 導航與空天目標工程學院，鄭州 450001）

魯棒性地形匹配/慣性組合導航算法

程傳奇，郝向陽，張振杰，馬智剛

（信息工程大學 導航與空天目標工程學院，鄭州 450001）

基于傳統平均絕對差算法引入了選取搜索區域和匹配運算的改進策略，使得算法在匹配定位時允許載體靈活機動，提高了實時性；針對桑迪亞算法易發散的問題，分析了組合濾波算法的特點，建立了地形匹配/慣性組合導航的數學模型；針對地形匹配“虛定位”影響濾波結果的問題，引入了一種基于新息向量的魯棒檢測算法，采用最小二乘對誤匹配點進行擬合改正。仿真研究表明：與傳統算法相比，組合濾波算法精度更高、穩定性更強，能高頻率（200 Hz）輸出高精度導航參數，誤差保持在一個格網間距（90 m）以內；魯棒檢測算法能有效抵御粗差影響，在經緯度方向各加入0.05°粗差的情況下，仍可保持較高精度。

地形匹配；組合導航；擴展卡爾曼濾波；新息向量；魯棒性

地形輔助導航（Terrain Aided Navigation，TAN）是一種自主、隱蔽、全天候的無源導航方式，在軍事導航領域及水下潛器自主導航中應用廣泛[1]。相關學者針對地形輔助導航算法開展了大量的研究工作，歸結起來，地形輔助導航算法可分為批處理和遞推濾波兩大類[2]。經典的批處理算法有地形等高線匹配（Terrain Contour Matching，TERCOM）算法和等值線最近點迭代（Iterative Closest Contour Point，ICCP）算法，遞推濾波算法有桑迪亞地形輔助導航（Sandia Inertial Terrain Aided Navigation，SITAN）算法和粒子濾波（Particle Filtering，PF）算法。批處理算法實時性差且TERCOM對航向誤差敏感、ICCP對初始誤差敏感，易陷入局部最優。批處理改進算法中，通過粒子群優化[2]、仿射修正[3]、二分搜索算法[4]、多波束聲吶探測[5]、加速匹配策略[6]等可改善算法實時性及局部收斂性。遞推濾波算法適用于初始誤差較小的情況下，且SITAN算法對地形要求較高，易發散；PF算法計算量較大，實時性差；其改進算法中，通過TERCOM確定初始誤差[7]、GPU加速[8]、網格自適應約束[9]、穩健估計理論[10]、自適應濾波[11]等可改善算法實時性及穩定性。這些算法大都著眼于提高單一算法的可靠性，而未考慮地形匹配/慣導的組合模式來提高地形輔助導航算法的穩定性。文獻[12]在景象匹配輔助導航系統對慣導修正過程中引入了卡爾曼濾波技術，并設計了相應的景象匹配/慣性組合導航算法，取得了良好效果；文獻[13]將ICCP匹配位置誤差作為觀測量，設計了重力匹配/慣導組合導航算法，用卡爾曼濾波對慣導系統誤差進行最優估計，驗證了匹配/慣導組合模式的可行性及可靠性。這種組合模式中，匹配定位屬于批處理過程，難以實現連續定位，在組合濾波工程實現中存在問題，且誤匹配帶來的異常值會影響組合導航精度，嚴重時會造成濾波發散。

本文提出了一種魯棒性地形匹配/慣性組合導航算法，采用改進型MAD算法配合基于新息向量的檢測算法，以期提高匹配算法的實時性同時增強組合濾波算法的魯棒性。

1 地形匹配模型

1.1 傳統MAD地形匹配模型

獲取實測地形剖面后，以慣導指示位置為中心，以系統的±3σ誤差為幅度，從地形基準庫中提取導航位置周圍一定區域內與導航指示路徑平行的地形高程區域，即匹配搜索區域，見圖1。

圖1 選取搜索區域示意圖Fig.1 Sketch of method to obtain searching area

將實測地形剖面與提取的地形剖面通過一定準則作相關分析，所得相關極值點的位置即為最佳估計位置。平均絕對差算法的相關分析準則定義如下：

分析可知，傳統MAD算法有以下缺點：

1）在數據帶內對航向誤差較敏感，數據采集定位過程中要求載體不能機動；

2）屬于后驗批處理方式，必須飛行一段時間累積到一定長度的地形高程數據序列后才能進行相關匹配運算，不能實時修正慣導。

為解決傳統MAD算法存在的問題，提出了改進策略。

1.2 改進型MAD地形匹配模型

1.2.1 選取搜索區域的改進策略

載體運動時，慣導可實時給出載體位置，以慣導指示位置為中心，以慣導±3σ誤差為幅度確定一個沿航向方向的正方形，所有正方形組合成為一個不規則區域。該區域即為匹配搜索區域，見圖 2，這樣，進行數據采集時載體可以機動。

圖2 新方法選取搜索區域示意圖Fig.2 Sketch of new method for obtaining search area

1.2.2 匹配算法的改進策略

MAD加速算法描述如下：

① 實時獲取高程序列

慣導的短時精度較高，因此地形匹配系統開機時利用慣導在各采樣時刻輸出的位置信息，結合地形高程數據庫可獲得N-1個候選匹配序列，即：

第一次采樣過程中選擇與當前時刻相鄰的N-1個地形高，第二次采樣中選擇與前一時刻相鄰的個地形高，依此類推。這樣，在每個采樣時刻均有實時采樣點及式(4)推算的高程點構成的N個匹配序列參與地形匹配運算，保證在任意時刻均可獲得當前的地形匹配位置。

② 消除冗余計算，提高匹配計算效率

傳統MAD算法存在冗余計算，為提高匹配計算速度，應盡量減少或避免冗余計算。

在k+1時刻進行搜索，MAD算法滿足：

從式(5)的遞推形式可以看出，采用上述優化算法可以大大減少計算量。

2 組合導航模型

MAD算法本質上屬于開環反饋系統，由于僅使用地形匹配位置信息修正慣性導航定位誤差，而沒有補償慣性器件誤差，使得慣導誤差傳播較快；SITAN算法由于僅使用高程差作為量測值，使得算法易發散，為增強其魯棒性，建立了地形匹配/慣性組合導航模型。

2.1 狀態模型

系統的狀態模型采用東北天坐標系下的慣導 15維誤差方程，如下所示：

2.2 量測模型

地形匹配/慣導組合的觀測量為匹配位置和量測高程，因此在當地導航坐標系中，觀測方程可表示為

式中：φINS和λINS表示慣導解算的水平位置和表示地形匹配位置；和分別為實時測量系統獲取的地形高和根據慣導位置在DTED中檢索到的地形高；為地形斜率，采用九點擬合法進行地形線性化獲取地形斜率。

3 魯棒檢測算法

相關度量值的統計特性決定了相關匹配算法產生“虛定位”（即誤匹配）的問題是不可避免的，導致組合導航中的觀測值出現粗差。這些異常觀測值勢必會影響地形匹配/慣性組合導航中卡爾曼濾波器的穩定性，嚴重時會導致濾波發散。因此改進思路中存在兩個關鍵問題：1）如何檢測異常值？2）如何處理異常值？

3.1 新息 2χ檢測算法

卡爾曼濾波模型里，把當前觀測信息與狀態預報信息的差值定義為新息向量，即：

式(9)為新息向量的協方差陣。一般情況下，新息向量服從零均值正態分布，即：

由式(8)可以看出，新息向量包含系統狀態信息和量測信息兩部分。當系統無異常時，新息向量為零均值白噪聲；當系統出現異常時，新息向量不再是白噪聲序列。在地形匹配/慣性組合模型中，量測粗差意味著出現了誤匹配。這樣，把粗差檢測轉變為一般的假設檢驗問題：

式中：H0是原假設，表示量測值正常；H1是備擇假設，表示量測值出現粗差。構造檢驗統計量：

式中，Tk服從自由度是m（代表量測值維數）的2χ分布。那么：

3.2 最小二乘擬合改正值

出現誤匹配時，這里采用最小二乘擬合改正值。通常載體短時運動軌跡為平滑曲線，因此可由前k個正確匹配序列點確定一條曲線。設正確匹配點為由最小二乘準則易得：

通過式(16)計算第k個點（誤匹配點）的改正值：

具體實施流程如下：

Step 1：獲取k時刻的地形匹配位置，即量測值，根據式(8)和式(9)計算新息向量及其協方差；

Step 2：根據式(12)計算檢驗統計量Tk；

Step 3：比較Tk與閾值TD，若無異常，則返回濾波過程；若出現異常，則轉入Step 4；

Step 4：根據3.2用最小二乘擬合改正值作為新的量測值；

重復以上步驟，直到通過判斷準則為止，返回濾波過程進行導航計算。

上述過程為迭代實現，但實施過程中一般一次改正即可通過判斷準則，因此運行效率較高。

4 仿真實驗分析

采用SRTM數據（格網間距90 m × 90 m）進行仿真分析，參數設置見表1和表2。航跡1時長730 s，航跡2時長780 s，都包含了加減速、俯仰、轉彎等運動狀態。

為了驗證本文算法的性能，設計以下4種方案解算導航參數進行對比：

方案1：傳統MAD算法；

方案2：SITAN算法；

方案3：地形匹配/慣性組合算法；

方案4：魯棒性地形匹配/慣性組合算法。

方案1～3的解算結果見圖3和圖4；方案1和方案3的誤差統計結果見表3和表4。

通過對以上圖表進行分析可得出以下結論：

1）方案3可以有效解決慣導單獨導航時誤差迅速累積的問題，長航時導航仍能保持一個格網間距的精度；

圖3 航跡1解算結果Fig.3 Computed results of trajectory 1

圖4 航跡2解算結果Fig.4 Computed results of trajectory 2

表1 仿真參數設置Tab.1 Simulation parameters

表2 IMU仿真參數設置Tab.2 Simulation parameters

表3 方案1與方案3誤差統計對比（航跡1）Tab.3 Statistics of position errors of scheme 1 and scheme 3 (trajectory 1)

表4 方案1與方案3誤差統計對比（航跡2）Tab.4 Statistics of position errors of scheme 1 and scheme 3 (trajectory 2)

2）與方案1相比，方案3解算結果精度更高，整體誤差曲線更平滑，可有效削弱相關匹配算法中的某些跳變點；

3）方案3的導航參數輸出頻率與慣導一致，數據更新速率快。由于時間更新，在方案1未能匹配定位的時刻，方案3仍可以連續輸出導航信息；

4）航跡2使用方案2解算的結果發散，主要是因為地形變化較劇烈，使得地形線性化過程中帶來的誤差太大；而方案3仍然保持較高的精度，充分說明了組合導航算法用于地形輔助導航的可行性。

為驗證方案4的抗粗差性能，在MAD匹配定位結果中加入粗差，從第100個匹配點開始，每隔20個點東向和北向各加入0.05°的粗差，方案3和方案4的解算結果見圖5和圖6。

分析圖5和圖6可知：

1）方案3使用常規卡爾曼濾波進行解算，由于粗差的影響，使得導航誤差較大；但出現粗差后，仍能夠迅速收斂，佐證了地形匹配/慣導組合系統的穩定性。

2）方案4的解算精度依然較高，說明論文的魯棒檢測算法能夠有效提高地形輔助導航系統的抗粗差能力，驗證了魯棒檢測算法的穩健性。

圖5 方案3、方案4導航誤差對比（航跡1）Fig.5 Position errors of scheme 3 and scheme 4 (trajectory 1)

圖6 方案3、4導航誤差對比（航跡2）Fig.6 Position errors of scheme 3 and scheme 4 (trajectory 2)

5 結 論

本文提出了一種魯棒性地形匹配/慣性組合導航算法，分析了傳統MAD算法的特點，給出了選取搜索區域和加速匹配的改進策略，建立了地形匹配/慣性組合導航數學模型，引入了基于新息向量的魯棒檢測算法。通過仿真實驗驗證了算法的性能，結果表明：魯棒性地形匹配/慣性組合導航算法提高了地形輔助導航算法的精度和可靠性。組合濾波中，誤差統計特性對于濾波結果影響較大，下一步將研究地形匹配/慣性組合系統中的精確誤差模型來提高導航精度。

參考文獻（References）：

[1] Paull L, Saeedi S, Seto M. AUV navigation and localization: a review[J]. IEEE Journal of Oceanic Engineering, 2014, 39(4): 131-149.

[2] 周玲, 程向紅. 基于約束粒子群優化的海底地形輔助慣性導航定位方法[J]. 中國慣性技術學報, 2014, 23(3): 369-372, 384. Zhou Ling, Cheng Xiang-hong. Seabed terrain-aided SINS location based on constrained particle swarm optimization method[J]. Journal of Chinese Inertial Technology, 2014, 23(3): 369-372, 384.

[3] 徐曉蘇, 吳劍飛, 徐勝保, 等. 基于仿射修正技術的水下地形 ICCP匹配算法[J]. 中國慣性技術學報, 2014, 22(3): 362-367. Xu Xiao-su, Wu Jian-fei, Xu Sheng-bao, et al. ICCP algorithm for underwater terrain matching navigation based on affine correction[J]. Journal of Chinese Inertial Technology, 2014, 22(3): 362-367.

[4] Eroglu O, Yilmaz G. A terrain referenced UAV localization algorithm using binary search method[J]. Journal of Intelligent & Robotic Systems, 2014, 73(4): 309-323.

[5] Song Zi-qi, Bian Hong-yu, Zielinski A. Underwater terrainaided navigation based on multibeam bathymetric sonar images[J]. Journal of Marine Science and Application, 2015, 14: 1-9.

[6] 程傳奇, 郝向陽, 劉松林. 基于改進型MAD算法的地形輔助慣性導航[J]. 電子測量技術, 2015, 38(4): 6-9. Cheng Chuan-qi, Hao Xiang-yang, Liu Song-lin. Improved MAD algorithm for terrain aided inertial navigation[J]. Electrical measurement technology, 2015, 38(4): 6-9.

[7] Zhao L, Gao N, Huang B Q, et al. A novel terrain aided navigation algorithm combined with the TERCOM algorithm and particle filter[J]. IEEE Sensors Journal, 2014, 15(2): 1124-1131.

[8] Yigit H, Yilmaz G. Development of a GPU accelerated terrain referenced UAV localization and navigation algorithm[J]. Journal of Intelligent & Robotic Systems, 2013, 7(1-4): 477-489.

[9] Young M Y, Chan G P. Improvement of terrain referenced navigation using a Point Mass Filter with grid adaptation [J]. International Journal of Control, Automation, and Systems, 2015, 13(5): 1173-1181.

[10] Zhang K, Li Y, Zhao J H. A study of underwater terrain navigation based on the robust matching method[J]. Journal of Navigation, 2014, 67(4): 569-578.

[11] Yun S H, Lee W, Park C G. Covariance calculation for batch processing terrain referenced navigation[C]//Proceeding of Position Location and Navigation Symposium. Monterey, USA, 2014: 701-706.

[12] 熊智, 李明星, 冷雪飛, 等. 景象匹配/慣性組合導航精確修正算法[J]. 中國慣性技術學報, 2007, 15(5): 564-567, 614. Xiong Zhi, Li Ming-xing, Leng Xue-fei, et al. Precision amendment algorithm for scene matching/INS integrated navigation[J]. Journal of Chinese Inertial Technology, 2007, 15(5): 564-567, 614.

[13] 王志剛, 邊少鋒. 基于ICCP算法的重力輔助慣性導航[J]. 測繪學報, 2008, 37(2): 147-151, 157. Wang Zhi-gang, Bian Shao-feng. ICCP algorithm for gravity aided inertial navigation[J]. Acta Geodaetica et Cartographica Sinica, 2008, 37(2): 147-151, 157.

[14] 劉帥. GPS/INS組合導航算法研究與實現[D]. 鄭州:信息工程大學, 2012. Liu Shuai. Research and implementation of GPS/INS integrated navigation algorithms[D]. Zhengzhou: Information Engineering University, 2012.

Robust integrated navigation algorithm of terrain aided navigation/INS

CHENG Chuan-qi, HAO Xiang-yang, ZHANG Zhen-jie, MA Zhi-gang

(School of Navigation and Aerospace Engineering, Information Engineering University, Zhengzhou 450001, China)

Based on traditional mean absolute difference (MAD) algorithm, an improved selection strategy for search area and match operation is put forward, which makes carrier flexible when positioning and improves the real-time performance. The characteristics of integrated filtering algorithm are analyzed, and the mathematical model of TAN(terrain matching navigation)/INS integrated navigation is presented. To solve the problem of mismatching error, a robust detection algorithm based on innovation vector is introduced, and the mismatching points are modified by the least-squares curve fitting method. Simulation experiments show that, compared with traditional MAD and SITAN algorithm, the proposed integrated navigation algorithm has higher precision and reliability, and can output navigation information with high frequency (200 Hz), with the errors being less than a grid space (90 m) of digital map. The robust detection algorithm can effectively weaken the effect of gross error, and can still performs well even when 0.05° gross errors are added in latitude and longitude directions, respectively.

terrain matching; integrated navigation; extended Kalman filter; innovation vector; robust

U666.1

A

1005-6734(2016)02-0202-06

10.13695/j.cnki.12-1222/o3.2016.02.012

2015-11-25；

2016-02-29

國家自然科學基金項目（61173077）；信息工程大學研究生基金項目（510087）

程傳奇（1989—），男，博士生，主要研究導航定位與位置服務。E-mail: legend3q@163.com