基于迭代計(jì)算的地磁輪廓線匹配算法

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

（哈爾濱工業(yè)大學(xué) 空間控制與慣性技術(shù)研究中心，哈爾濱 150001）

基于迭代計(jì)算的地磁輪廓線匹配算法

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

（哈爾濱工業(yè)大學(xué) 空間控制與慣性技術(shù)研究中心，哈爾濱 150001）

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

地磁匹配；輪廓線匹配；迭代；均方差準(zhǔn)則

地磁匹配導(dǎo)航以地球的基本物理場(chǎng)為基礎(chǔ)，通過(guò)線圖匹配實(shí)現(xiàn)導(dǎo)航定位，具有無(wú)源、無(wú)輻射、隱蔽性強(qiáng)、誤差不隨時(shí)間累積等眾多優(yōu)點(diǎn)，因此作為近年來(lái)新生的輔助慣性導(dǎo)航的手段，在確保武器系統(tǒng)導(dǎo)航信息的安全性、可靠性和自主性方面展現(xiàn)出了巨大的軍事潛力[1-3]。

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

本文提出了一種基于迭代計(jì)算的地磁輪廓線匹配新算法。該算法引入匹配曲線的參數(shù)化模型，并通過(guò)泰勒展開(kāi)將 MSD相關(guān)性約束轉(zhuǎn)化為曲線平移位移和航向誤差角的多變量表達(dá)式，從而將匹配問(wèn)題轉(zhuǎn)化為非線性方程組的求解問(wèn)題，最終通過(guò)迭代計(jì)算實(shí)現(xiàn)地磁匹配。該算法可以有效地消除慣導(dǎo)系統(tǒng)的初始位置誤差和初始航向誤差，并具有較高的實(shí)時(shí)性。

1 迭代匹配原理

1.1 問(wèn)題描述

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

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

1.2 性能指標(biāo)

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

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

采用均方差準(zhǔn)則計(jì)算曲線的相關(guān)性，即計(jì)算匹配軌跡曲線上各點(diǎn)所對(duì)應(yīng)的地球磁場(chǎng)特征值與真實(shí)磁場(chǎng)測(cè)量值之差的平方和的均值，如式(2)所示：

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

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

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

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

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

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

如圖1所示，當(dāng)慣導(dǎo)系統(tǒng)存在初始位置誤差和初始航向誤差時(shí)，匹配曲線M的描述方程為

將參考軌跡Rf離散化成n( n＞2)個(gè)點(diǎn)，其對(duì)應(yīng)離散點(diǎn)的坐標(biāo)為可以得到離散化的均方差約束指標(biāo)函數(shù)：

因此地磁匹配問(wèn)題可以轉(zhuǎn)化為尋求Δx、Δy和α，使指標(biāo)函數(shù)(8)取極小值。

1.3 匹配求解

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

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

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

2 迭代匹配算法

基于迭代計(jì)算的地磁輪廓線匹配算法的實(shí)現(xiàn)步驟如下：

第二步：根據(jù)參考軌跡離散化的n個(gè)位置，從預(yù)先存儲(chǔ)的地磁數(shù)據(jù)庫(kù)中讀取該位置的地磁特征值并根據(jù)地磁數(shù)據(jù)庫(kù)信息計(jì)算該位置的地磁特征值梯度信息

第三步：初始化迭代變量

第五步：求解線性方程組

第六步：更新迭代變量

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

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

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

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

需要指出的是，基于迭代計(jì)算的地磁輪廓線匹配算法假設(shè)參考軌跡位于匹配軌跡附近，在忽略高階小項(xiàng)O2的基礎(chǔ)上，以一階差分的形式近似相關(guān)性約束。然而當(dāng)慣導(dǎo)系統(tǒng)的初始誤差較大時(shí)，忽略的高階小項(xiàng)O2會(huì)嚴(yán)重影響算法的精度。為了克服上述問(wèn)題，可以采用多次迭代的方式提高匹配算法的精度。在多次迭代時(shí)，可以將當(dāng)前的迭代結(jié)果作為下一次迭代的參考軌跡。

3 仿真實(shí)驗(yàn)

為了考察上述算法的有效性，本節(jié)對(duì)算法進(jìn)行仿真驗(yàn)證。根據(jù)地球磁場(chǎng)異常網(wǎng)格(EMAG2)繪制中國(guó)東北某位置的地磁異常圖作為匹配背景磁圖，仿真過(guò)程中，選取地磁匹配區(qū)域內(nèi)的某條直線航跡作為真實(shí)軌跡，對(duì)真實(shí)航跡疊加一定的旋轉(zhuǎn)和平移誤差作為慣導(dǎo)系統(tǒng)輸出的參考軌跡，如圖2所示，其中初始位置誤差為(0.04°, -0.04°)，初始航向誤差為0.8°。

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

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

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

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

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

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

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

圖5 考慮噪聲時(shí)匹配經(jīng)度誤差曲線Fig.5 Longitude errors of matching results with random noise

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

4 結(jié) 論

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

（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] 陳勵(lì)華, 王仕成, 孫淵, 等. 地磁緩變區(qū)域的多維特征量匹配方法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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算法的地磁匹配輔助導(dǎo)航[J]. 應(yīng)用基礎(chǔ)與工程科學(xué)學(xué)報(bào), 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ìn)行聯(lián)合地磁匹配導(dǎo)航[J]. 武漢大學(xué)學(xué)報(bào)(信息科學(xué)版), 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] 王躍鋼, 賈磊, 單斌, 等. 自適應(yīng)SA-ACO地磁匹配導(dǎo)航算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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

國(guó)家自然科學(xué)基金（61403095，61375046）；總裝預(yù)研項(xiàng)目（51309040401）

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

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

1005-6734(2015)05-0631-05

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