基于迭代計算的地磁輪廓線匹配算法

解偉男，李清華，奚伯齊，黃黎平，王常虹

（哈爾濱工業大學 空間控制與慣性技術研究中心，哈爾濱 150001）

基于迭代計算的地磁輪廓線匹配算法

解偉男，李清華，奚伯齊，黃黎平，王常虹

（哈爾濱工業大學 空間控制與慣性技術研究中心，哈爾濱 150001）

針對慣性/地磁匹配組合導航系統，提出了一種基于迭代計算的地磁輪廓線匹配新算法，可以有效地修正慣導系統的初始位置誤差和初始航向誤差，并具有較高的實時性。首先以均方差準則建立匹配軌跡和實測地磁特征量的相關性約束，引入匹配曲線的參數化模型，通過泰勒展開并忽略高階小項，將相關性約束簡化為曲線平移位移和航向誤差角的多變量表達式。然后依據相關性準則，將地磁匹配問題轉化為以曲線平移位移和航向誤差角為變量的非線性方程組的求解問題。采用牛頓迭代求解非線性方程組，實現地磁輪廓線匹配。最后仿真結果表明，基于迭代計算的地磁輪廓線匹配方法的最大匹配誤差為傳統輪廓線匹配方法的18.2%，為等值線約束迭代最近點匹配方法的7.8%，并且所提出的新方法耗時僅為5 ms，滿足實時匹配要求。

地磁匹配；輪廓線匹配；迭代；均方差準則

地磁匹配導航以地球的基本物理場為基礎，通過線圖匹配實現導航定位，具有無源、無輻射、隱蔽性強、誤差不隨時間累積等眾多優點，因此作為近年來新生的輔助慣性導航的手段，在確保武器系統導航信息的安全性、可靠性和自主性方面展現出了巨大的軍事潛力[1-3]。

目前，地磁匹配算法主要分為兩大類：第一類是輪廓線匹配算法，該算法具有原理簡單，適用范圍廣，對初始誤差要求低等優點。然而該方法要求根據慣導系統輸出的參考軌跡形狀，遍歷有效范圍內所有平行于參考軌跡的序列，并通過相關性準則確定最優的匹配結果，算法的實時性受到搜索范圍和搜索步長的制約，而且該算法無法修正慣導系統的初始航向誤差，匹配精度較差[4-5]。第二類是ICCP匹配算法，ICCP匹配算法最初由圖像配準算法ICP（Iterative Closest Point）發展而來[6]，ICCP匹配算法求取測量軌跡與最近等值點軌跡之間的最優剛性變換，通過該變換校正測量軌跡來實現匹配的目的，ICCP匹配可以同時修正慣導系統的初始位置誤差和初始航向誤差。然而該算法以磁場測量等值線上的最近點序列作為匹配目標，因此算法收斂于該最近點序列而不收斂于真實軌跡序列，因此算法精度無法得到保證；此外ICCP算法在尋找最近點時需要耗費較多時間，這也影響了算法的實時性[7-8]。也有學者針對上述問題進行改進，比如將兩類匹配算法融合[9]以及采用仿生學匹配算法[10-11]等，然而匹配算法的精度、實時性等問題還有待進一步研究。

本文提出了一種基于迭代計算的地磁輪廓線匹配新算法。該算法引入匹配曲線的參數化模型，并通過泰勒展開將 MSD相關性約束轉化為曲線平移位移和航向誤差角的多變量表達式，從而將匹配問題轉化為非線性方程組的求解問題，最終通過迭代計算實現地磁匹配。該算法可以有效地消除慣導系統的初始位置誤差和初始航向誤差，并具有較高的實時性。

1 迭代匹配原理

1.1 問題描述

如圖1所示的地磁匹配示意圖。曲線Rf為慣導系統輸出的軌跡曲線，即參考軌跡；曲線M為匹配算法求得的匹配軌跡曲線，即匹配軌跡，其位于真實軌跡曲線附近；曲線Rf ′與匹配軌跡平行，參考軌跡與匹配軌跡之間的夾角為α，即航向誤差角。從圖中可以看出參考軌跡與匹配軌跡間存在著平移和旋轉，對應著慣導系統的初始位置誤差和初始航向誤差。令點(a, b)T為參考軌跡上的任意一點，其對應的匹配軌跡點為(u, v)T，其中(a, u)表示點的經度，(b, v)表示點的緯度。

圖1 地磁匹配示意圖Fig.1 Illustration of geomagnetic matching

1.2 性能指標

分別用I( a, b)、I( u, v)表示點(a, b)T和點(u, v)T所在位置對應的地磁圖庫中地磁場特征值。考慮到參考軌跡在匹配軌跡附近，即可將I( u, v)泰勒展開：

式中，?I( a, b)/?x表示地磁特征值對經度方向的梯度在點(a, b)T上的取值，?I( a, b)/?y 表示地磁特征值對緯度方向的梯度在點(a, b)T上的取值，O2為高階小項。

采用均方差準則計算曲線的相關性，即計算匹配軌跡曲線上各點所對應的地球磁場特征值與真實磁場測量值之差的平方和的均值，如式(2)所示：

式中，Ir(a, b)為載體在參考點(a, b)T時磁傳感器實時測量得到的地磁特征值，L為參考曲線的長度。

將泰勒展開式(1)帶入式(2)，并忽略高階小項O2，可以得到如式(3)的相關性約束：

在不引起混淆的情況下，可將相關性約束(3)寫成如下簡化形式：

這里存在u和v兩組位置變量而只有一個約束，顯然不能唯一的確定匹配結果。傳統的輪廓線匹配方法是遍歷所有平行于參考軌跡的曲線，使相關性約束(4)取最小值的曲線即為所求。首先這種方法的實時性受到搜索范圍和搜索步長的制約，匹配速度慢；其次，該方法只能修正慣導系統的初始位置誤差，匹配精度差。若加入航向等誤差進行搜索，計算量會大幅度升高，難以在線計算。

本文將建立匹配軌跡的參數化模型，并將該模型引入相關性約束，從而將遍歷搜索方法轉化為快速迭代算法。設參考軌跡Rf的描述方程為

式中，t為參考軌跡的參變量。

如圖1所示，當慣導系統存在初始位置誤差和初始航向誤差時，匹配曲線M的描述方程為

將參考軌跡Rf離散化成n( n＞2)個點，其對應離散點的坐標為可以得到離散化的均方差約束指標函數：

因此地磁匹配問題可以轉化為尋求Δx、Δy和α，使指標函數(8)取極小值。

1.3 匹配求解

為使指標函數(8)取極小值，可分別將指標函數對自變量Δx、Δy和α求一階偏導數，并令其為零，即

將均方差約束指標函數(8)帶入式(9)可得：

因此地磁匹配可以轉化為由Δx、Δy和α組成的非線性方程組(13)的求解問題，該非線性方程組可通過Newton迭代法求解。匹配結果可以通過式(6)計算得到。

2 迭代匹配算法

基于迭代計算的地磁輪廓線匹配算法的實現步驟如下：

第二步：根據參考軌跡離散化的n個位置，從預先存儲的地磁數據庫中讀取該位置的地磁特征值并根據地磁數據庫信息計算該位置的地磁特征值梯度信息

第三步：初始化迭代變量

第五步：求解線性方程組

第六步：更新迭代變量

第七步：判斷是否滿足終止迭代條件，若滿足則停止迭代并跳到第八步，否則跳到第四步。

終止迭代的條件有兩個，滿足任意一個即終止迭代：① 迭代次數達到預設的最大迭代次數；② 迭代變量增量Y的2范數小于設定值，即

式中，ε＞0為預先給定的迭代最小誤差。第八步：根據迭代變量計算匹配結果

根據上述匹配步驟可知，迭代計算過程中地磁特征值梯度信息Ix,i和Iy,i并不改變，因此可以將匹配區域地磁特征值梯度信息事先存入載體計算機中，匹配計算前直接在載體計算機中檢索得到，也可以通過檢索載體計算機中的地磁特征值信息后計算得到。

需要指出的是，基于迭代計算的地磁輪廓線匹配算法假設參考軌跡位于匹配軌跡附近，在忽略高階小項O2的基礎上，以一階差分的形式近似相關性約束。然而當慣導系統的初始誤差較大時，忽略的高階小項O2會嚴重影響算法的精度。為了克服上述問題，可以采用多次迭代的方式提高匹配算法的精度。在多次迭代時，可以將當前的迭代結果作為下一次迭代的參考軌跡。

3 仿真實驗

為了考察上述算法的有效性，本節對算法進行仿真驗證。根據地球磁場異常網格(EMAG2)繪制中國東北某位置的地磁異常圖作為匹配背景磁圖，仿真過程中，選取地磁匹配區域內的某條直線航跡作為真實軌跡，對真實航跡疊加一定的旋轉和平移誤差作為慣導系統輸出的參考軌跡，如圖2所示，其中初始位置誤差為(0.04°, -0.04°)，初始航向誤差為0.8°。

仿真時首先假設磁傳感器沒有測量噪聲，圖3和圖4分別給出了經度誤差曲線和緯度誤差曲線，其中算法1為傳統的輪廓匹配算法[4]，算法2為ICCP匹配算法[7]，算法 3為基于迭代計算的地磁輪廓線匹配算法，算法4為采用二次迭代的基于迭代計算的地磁輪廓線匹配算法。仿真結果表明，所提出算法的最大誤差為傳統輪廓線匹配最大誤差的11.0%，為ICCP匹配最大誤差的3.6%。

圖2 真實軌跡和參考軌跡Fig.2 The real track and the reference track

若在地磁數據中加入幅值為2 nT的隨機噪聲，匹配結果如圖5和圖6所示。仿真結果表明，所提出算法的最大誤差為傳統輪廓線匹配最大誤差的 18.2%，為ICCP匹配最大誤差的7.8%。

圖3 忽略傳感器噪聲時匹配經度誤差曲線Fig.3 Longitude errors of matching results without the magnetic sensor noise

圖4 忽略傳感器噪聲時匹配緯度誤差曲線Fig.4 Latitude errors of matching results without the magnetic sensor noise

根據匹配結果可以看出，基于迭代計算的地磁輪廓線匹配算法具有較高的匹配精度，可以修正慣導系統的初始位置誤差和初始航向誤差。此外，若采用多次迭代，匹配精度可以進一步提高。

利用Matlab中的tic和toc語句計算匹配算法運行時間，傳統的輪廓線匹配算法在仿真計算機中總耗時為6.1 s（搜索步長為0.001°）；ICCP匹配算法在仿真計算機中總耗時為1.4 s；采用二次迭代的基于迭代計算的地磁輪廓線匹配算法在仿真計算機中總耗時約為5 ms。因此所提出的算法具有較高的實時性，可以保證地磁導航的在線計算。

圖5 考慮噪聲時匹配經度誤差曲線Fig.5 Longitude errors of matching results with random noise

圖6 考慮噪聲時匹配緯度誤差曲線Fig.6 Latitude errors of matching results with random noise

4 結 論

本文提出了一種基于迭代計算的地磁輪廓線匹配新方法。該方法將匹配問題轉化為以匹配曲線參數為變量的非線性方程組，通過迭代計算求解非線性方程組從而確定匹配結果。仿真結果表明，所提出算法的最大匹配誤差為傳統輪廓線匹配最大誤差的 18.2%，為ICCP匹配最大誤差的7.8%。此外，所提出的算法在仿真計算機中的總耗時約為5 ms。因此提出的基于迭代計算的地磁輪廓線匹配算法可以有效的提高匹配導航的定位精度，改善匹配導航的實時性。

（References）：

[1] Batista P, Petit N, Silvestre C, Oliveira P. Further results on the observability in magneto-inertial navigation[C]// 2013 American Control Conference. 2013: 2503-2508.

[2] Wang Peng, Wu Yuan-xin, Hu Xiao-ping, Ruan Qing. Geomagnetic aided navigation suitability evaluation based on principal component analysis[C]//2012 International Conference on Industrial Control and Electronics Engineering. 2012: 324-329.

[3] Guo Cai-fa, Cai Hong, G H M van der Heijden. Feature extraction and geomagnetic matching[J]. The Journal of Navigation, 2013, 66(6): 799-811.

[4] 陳勵華, 王仕成, 孫淵, 等. 地磁緩變區域的多維特征量匹配方法[J]. 中國慣性技術學報, 2011, 19(6): 720-724. Chen Li-hua, Wang Shi-cheng, Sun Yuan, et al. Matching of multi-dimensional feature elements in areas with smooth magnetic fields[J]. Journal of Chinese Inertial Technology, 2011, 19(6): 720-724.

[5] Xie Wei-nan, Qu Zhen-shen, Li Qing-hua. A fast algorithm of the geomagnetic correlation matching based on MSD[C]//The 3rd International Conference on Control, Automation and Systems Engineering. 2013: 59-62.

[6] Besl P J, Mckay N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14(2): 239-256.

[7] 康崇, 張曉峻, 樊黎明. 基于 ICCP算法的地磁匹配輔助導航[J]. 應用基礎與工程科學學報, 2014, 22(3): 598-605. Kang Chong, Zhang Xiao-jun, Fan Li-ming. Geomagnetic-matching aided navigation based on ICCP algorithm [J]. Journal of Basic Science and Engineering, 2014, 22(3): 598-605.

[8] Zhang Yan-shun, Li Chun-yu, Liu Shan-shan, Jiang Ming, Guo Lei. Locating method of geomagnetic/inertial integrated navigation system by forecasting the geomagnetic matching initial value[C]//2014 IEEE Chinese Guidance, Navigation and Control Conference. 2014: 235-239.

[9] 王勝平, 張紅梅, 趙建虎, 等. 利用TERCOM與ICCP進行聯合地磁匹配導航[J]. 武漢大學學報(信息科學版), 2011, 36(10): 1209-1212. Wang Sheng-ping, Zhang Hong-mei, Zhao Jian-hu, et al. Marine geomagnetic navigation technology based on Integration of TERCOM and ICCP[J]. Geomatics and Information Science of Wuhan University, 2011, 36(10): 1209-1212.

[10] 王躍鋼, 賈磊, 單斌, 等. 自適應SA-ACO地磁匹配導航算法[J]. 中國慣性技術學報, 2014, 22(1): 89-93. Wang Yue-gang, Jia Lei, Shan Bin, et al. Adaptive SA-ACO geomagnetic matching navigation algorithm[J]. Journal of Chinese Inertial Technology, 2014, 22(1): 89-93.

[11] Liu Ming-yong, Li Hong, Liu Kun. Geomagnetic navigation of AUV without a priori magnetic map[C]//2014 OCEANS. 2014: 1-5.

Geomagnetic contour matching algorithm based on iterative method

XIE Wei-nan, LI Qing-hua, XI Bo-qi, HUANG Li-ping, WANG Chang-hong
(Space Control and Inertial Technology Research Center, Harbin Institute of Technology, Harbin 150001, China)

A novel geomagnetic contour matching algorithm based on iterative method is proposed for inertial/geomagnetic integrated navigation systems, which can correct the initial position and heading errors of INS (Inertial Navigation Systems), and lead to high real-time performance. First, the correlation constraint of the matching track and geomagnetic characteristic value is built according to MSD (Mean Square Deviation) criterion. By introducing the parameterized model of the matching track and neglecting the higher-order expanded terms of the first order Taylor expansion, the correlation constraint is translated into a mathematical expression of the displacement and initial heading error. Then the geomagnetic matching problem is converted to the solutions of nonlinear equations according to the correlation criterion. Newton iteration is applied to implement the geomagnetic matching algorithm. Simulation results indicate that the matching error of the contour matching algorithm based on iterative method is decreased to 18.2% compared to conventional contour matching method, and is decreased to 7.8% compared to ICCP (Iterated Closest Contour Point) matching method. Meanwhile, the time consumed in the proposed algorithm is only 5 ms, meeting the real-time requirement.

geomagnetic matching; contour matching; iteration; mean square deviation

V249.3

：A

2015-05-15；

：2015-09-21

國家自然科學基金（61403095，61375046）；總裝預研項目（51309040401）

解偉男（1979—），男，講師，從事地磁導航技術研究。E-mail：xieweinan@hit.edu.cn

聯 系 人：李清華（1979—），男，副研究員。E-mail：huahit@hit.edu.cn

1005-6734(2015)05-0631-05

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