魯棒性地形匹配/慣性組合導(dǎo)航算法

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

（信息工程大學(xué) 導(dǎo)航與空天目標(biāo)工程學(xué)院，鄭州 450001）

魯棒性地形匹配/慣性組合導(dǎo)航算法

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

（信息工程大學(xué) 導(dǎo)航與空天目標(biāo)工程學(xué)院，鄭州 450001）

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

地形匹配；組合導(dǎo)航；擴(kuò)展卡爾曼濾波；新息向量；魯棒性

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

本文提出了一種魯棒性地形匹配/慣性組合導(dǎo)航算法，采用改進(jìn)型MAD算法配合基于新息向量的檢測(cè)算法，以期提高匹配算法的實(shí)時(shí)性同時(shí)增強(qiáng)組合濾波算法的魯棒性。

1 地形匹配模型

1.1 傳統(tǒng)MAD地形匹配模型

獲取實(shí)測(cè)地形剖面后，以慣導(dǎo)指示位置為中心，以系統(tǒng)的±3σ誤差為幅度，從地形基準(zhǔn)庫中提取導(dǎo)航位置周圍一定區(qū)域內(nèi)與導(dǎo)航指示路徑平行的地形高程區(qū)域，即匹配搜索區(qū)域，見圖1。

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

將實(shí)測(cè)地形剖面與提取的地形剖面通過一定準(zhǔn)則作相關(guān)分析，所得相關(guān)極值點(diǎn)的位置即為最佳估計(jì)位置。平均絕對(duì)差算法的相關(guān)分析準(zhǔn)則定義如下：

分析可知，傳統(tǒng)MAD算法有以下缺點(diǎn)：

1）在數(shù)據(jù)帶內(nèi)對(duì)航向誤差較敏感，數(shù)據(jù)采集定位過程中要求載體不能機(jī)動(dòng)；

2）屬于后驗(yàn)批處理方式，必須飛行一段時(shí)間累積到一定長(zhǎng)度的地形高程數(shù)據(jù)序列后才能進(jìn)行相關(guān)匹配運(yùn)算，不能實(shí)時(shí)修正慣導(dǎo)。

為解決傳統(tǒng)MAD算法存在的問題，提出了改進(jìn)策略。

1.2 改進(jìn)型MAD地形匹配模型

1.2.1 選取搜索區(qū)域的改進(jìn)策略

載體運(yùn)動(dòng)時(shí)，慣導(dǎo)可實(shí)時(shí)給出載體位置，以慣導(dǎo)指示位置為中心，以慣導(dǎo)±3σ誤差為幅度確定一個(gè)沿航向方向的正方形，所有正方形組合成為一個(gè)不規(guī)則區(qū)域。該區(qū)域即為匹配搜索區(qū)域，見圖 2，這樣，進(jìn)行數(shù)據(jù)采集時(shí)載體可以機(jī)動(dòng)。

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

1.2.2 匹配算法的改進(jìn)策略

MAD加速算法描述如下：

① 實(shí)時(shí)獲取高程序列

慣導(dǎo)的短時(shí)精度較高，因此地形匹配系統(tǒng)開機(jī)時(shí)利用慣導(dǎo)在各采樣時(shí)刻輸出的位置信息，結(jié)合地形高程數(shù)據(jù)庫可獲得N-1個(gè)候選匹配序列，即：

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

② 消除冗余計(jì)算，提高匹配計(jì)算效率

傳統(tǒng)MAD算法存在冗余計(jì)算，為提高匹配計(jì)算速度，應(yīng)盡量減少或避免冗余計(jì)算。

在k+1時(shí)刻進(jìn)行搜索，MAD算法滿足：

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

2 組合導(dǎo)航模型

MAD算法本質(zhì)上屬于開環(huán)反饋系統(tǒng)，由于僅使用地形匹配位置信息修正慣性導(dǎo)航定位誤差，而沒有補(bǔ)償慣性器件誤差，使得慣導(dǎo)誤差傳播較快；SITAN算法由于僅使用高程差作為量測(cè)值，使得算法易發(fā)散，為增強(qiáng)其魯棒性，建立了地形匹配/慣性組合導(dǎo)航模型。

2.1 狀態(tài)模型

系統(tǒng)的狀態(tài)模型采用東北天坐標(biāo)系下的慣導(dǎo) 15維誤差方程，如下所示：

2.2 量測(cè)模型

地形匹配/慣導(dǎo)組合的觀測(cè)量為匹配位置和量測(cè)高程，因此在當(dāng)?shù)貙?dǎo)航坐標(biāo)系中，觀測(cè)方程可表示為

式中：φINS和λINS表示慣導(dǎo)解算的水平位置和表示地形匹配位置；和分別為實(shí)時(shí)測(cè)量系統(tǒng)獲取的地形高和根據(jù)慣導(dǎo)位置在DTED中檢索到的地形高；為地形斜率，采用九點(diǎn)擬合法進(jìn)行地形線性化獲取地形斜率。

3 魯棒檢測(cè)算法

相關(guān)度量值的統(tǒng)計(jì)特性決定了相關(guān)匹配算法產(chǎn)生“虛定位”（即誤匹配）的問題是不可避免的，導(dǎo)致組合導(dǎo)航中的觀測(cè)值出現(xiàn)粗差。這些異常觀測(cè)值勢(shì)必會(huì)影響地形匹配/慣性組合導(dǎo)航中卡爾曼濾波器的穩(wěn)定性，嚴(yán)重時(shí)會(huì)導(dǎo)致濾波發(fā)散。因此改進(jìn)思路中存在兩個(gè)關(guān)鍵問題：1）如何檢測(cè)異常值？2）如何處理異常值？

3.1 新息 2χ檢測(cè)算法

卡爾曼濾波模型里，把當(dāng)前觀測(cè)信息與狀態(tài)預(yù)報(bào)信息的差值定義為新息向量，即：

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

由式(8)可以看出，新息向量包含系統(tǒng)狀態(tài)信息和量測(cè)信息兩部分。當(dāng)系統(tǒng)無異常時(shí)，新息向量為零均值白噪聲；當(dāng)系統(tǒng)出現(xiàn)異常時(shí)，新息向量不再是白噪聲序列。在地形匹配/慣性組合模型中，量測(cè)粗差意味著出現(xiàn)了誤匹配。這樣，把粗差檢測(cè)轉(zhuǎn)變?yōu)橐话愕募僭O(shè)檢驗(yàn)問題：

式中：H0是原假設(shè)，表示量測(cè)值正常；H1是備擇假設(shè)，表示量測(cè)值出現(xiàn)粗差。構(gòu)造檢驗(yàn)統(tǒng)計(jì)量：

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

3.2 最小二乘擬合改正值

出現(xiàn)誤匹配時(shí)，這里采用最小二乘擬合改正值。通常載體短時(shí)運(yùn)動(dòng)軌跡為平滑曲線，因此可由前k個(gè)正確匹配序列點(diǎn)確定一條曲線。設(shè)正確匹配點(diǎn)為由最小二乘準(zhǔn)則易得：

通過式(16)計(jì)算第k個(gè)點(diǎn)（誤匹配點(diǎn)）的改正值：

具體實(shí)施流程如下：

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

Step 2：根據(jù)式(12)計(jì)算檢驗(yàn)統(tǒng)計(jì)量Tk；

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

Step 4：根據(jù)3.2用最小二乘擬合改正值作為新的量測(cè)值；

重復(fù)以上步驟，直到通過判斷準(zhǔn)則為止，返回濾波過程進(jìn)行導(dǎo)航計(jì)算。

上述過程為迭代實(shí)現(xiàn)，但實(shí)施過程中一般一次改正即可通過判斷準(zhǔn)則，因此運(yùn)行效率較高。

4 仿真實(shí)驗(yàn)分析

采用SRTM數(shù)據(jù)（格網(wǎng)間距90 m × 90 m）進(jìn)行仿真分析，參數(shù)設(shè)置見表1和表2。航跡1時(shí)長(zhǎng)730 s，航跡2時(shí)長(zhǎng)780 s，都包含了加減速、俯仰、轉(zhuǎn)彎等運(yùn)動(dòng)狀態(tài)。

為了驗(yàn)證本文算法的性能，設(shè)計(jì)以下4種方案解算導(dǎo)航參數(shù)進(jìn)行對(duì)比：

方案1：傳統(tǒng)MAD算法；

方案2：SITAN算法；

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

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

方案1～3的解算結(jié)果見圖3和圖4；方案1和方案3的誤差統(tǒng)計(jì)結(jié)果見表3和表4。

通過對(duì)以上圖表進(jìn)行分析可得出以下結(jié)論：

1）方案3可以有效解決慣導(dǎo)單獨(dú)導(dǎo)航時(shí)誤差迅速累積的問題，長(zhǎng)航時(shí)導(dǎo)航仍能保持一個(gè)格網(wǎng)間距的精度；

圖3 航跡1解算結(jié)果Fig.3 Computed results of trajectory 1

圖4 航跡2解算結(jié)果Fig.4 Computed results of trajectory 2

表1 仿真參數(shù)設(shè)置Tab.1 Simulation parameters

表2 IMU仿真參數(shù)設(shè)置Tab.2 Simulation parameters

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

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

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

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

4）航跡2使用方案2解算的結(jié)果發(fā)散，主要是因?yàn)榈匦巫兓^劇烈，使得地形線性化過程中帶來的誤差太大；而方案3仍然保持較高的精度，充分說明了組合導(dǎo)航算法用于地形輔助導(dǎo)航的可行性。

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

分析圖5和圖6可知：

1）方案3使用常規(guī)卡爾曼濾波進(jìn)行解算，由于粗差的影響，使得導(dǎo)航誤差較大；但出現(xiàn)粗差后，仍能夠迅速收斂，佐證了地形匹配/慣導(dǎo)組合系統(tǒng)的穩(wěn)定性。

2）方案4的解算精度依然較高，說明論文的魯棒檢測(cè)算法能夠有效提高地形輔助導(dǎo)航系統(tǒng)的抗粗差能力，驗(yàn)證了魯棒檢測(cè)算法的穩(wěn)健性。

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

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

5 結(jié) 論

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

參考文獻(xiàn)（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] 周玲, 程向紅. 基于約束粒子群優(yōu)化的海底地形輔助慣性導(dǎo)航定位方法[J]. 中國慣性技術(shù)學(xué)報(bào), 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] 徐曉蘇, 吳劍飛, 徐勝保, 等. 基于仿射修正技術(shù)的水下地形 ICCP匹配算法[J]. 中國慣性技術(shù)學(xué)報(bào), 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] 程傳奇, 郝向陽, 劉松林. 基于改進(jìn)型MAD算法的地形輔助慣性導(dǎo)航[J]. 電子測(cè)量技術(shù), 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] 熊智, 李明星, 冷雪飛, 等. 景象匹配/慣性組合導(dǎo)航精確修正算法[J]. 中國慣性技術(shù)學(xué)報(bào), 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算法的重力輔助慣性導(dǎo)航[J]. 測(cè)繪學(xué)報(bào), 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ǎo)航算法研究與實(shí)現(xiàn)[D]. 鄭州:信息工程大學(xué), 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

國家自然科學(xué)基金項(xiàng)目（61173077）；信息工程大學(xué)研究生基金項(xiàng)目（510087）

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