摘 要：基于ICCP算法的地磁匹配定位可以用于限制慣導(dǎo)系統(tǒng)隨時(shí)間增長的位置誤差。給出ICCP算法的設(shè)計(jì)思想并進(jìn)行推廣，使算法能夠在地磁測(cè)量數(shù)據(jù)存在誤差的情況下，實(shí)現(xiàn)對(duì)慣導(dǎo)的誤差校正。同時(shí)采用滑動(dòng)窗口技術(shù)來快速尋找等值線上的最近點(diǎn)，在每一次收斂到局部最小的過程中都減少了尋找最近點(diǎn)的計(jì)算量，計(jì)算效率得以很大程度的提高。

關(guān)鍵詞：ICCP算法；自主無源導(dǎo)航；地磁匹配；誤差校正

中圖分類號(hào)：TJ765，TN966文獻(xiàn)標(biāo)識(shí)碼：B文章編號(hào)：1004373X(2008)2012203

Geomagnetic Localization Based on ICCP Algorithm

LI Yuze1，SHI Zhiyong1，YANG Yuntao1，F(xiàn)ENG Jun2

(1.Ordnance Engineering College，Shijiazhuang，050003，China;2.Ordnance N.C.O Academy of PLA，Wuhan，430075，China)

Abstract：The geomagnetic localization based on the ICCP algorithm can be used to bind the position errors inherent in Inertial Navigation System (INS)，which increases with time.The central thought of the algorithm is given in this paper and have been improved in some parts.The algorithm can realize the error correction of INS under the noise of geomagnetic measurement data.The second is to reckon the closest points by use of gliding window technique so as to reduce the computation cost in reckoning the closest points in each time of converging to local minimums.The efficiency of computing is improved.

Keywords：ICCP algorithm;autonomous and passive navigation;geomagnetic matching;error correction

1 引 言

實(shí)時(shí)確定載體的位置和速度，是提高武器裝備獨(dú)立作戰(zhàn)能力和整體作戰(zhàn)能力的重要手段，也是提高制導(dǎo)武器性能的基本要求。目前，導(dǎo)航定位的主要方式包括慣性導(dǎo)航和衛(wèi)星導(dǎo)航等。慣性導(dǎo)航，不借助任何外來信息，也不向外輻射任何信息，能夠在全天候條件下，在全球范圍內(nèi)和任何介質(zhì)環(huán)境里自主地、隱蔽地進(jìn)行連續(xù)的三維空間定位和三維空間定向，能夠提供完整的導(dǎo)航參數(shù)，但是它的缺點(diǎn)是導(dǎo)航誤差隨時(shí)間積累，長時(shí)間使用必須校正。雖可采用GPS、無線電和天文導(dǎo)航等信息對(duì)慣性導(dǎo)航系統(tǒng)進(jìn)行校正，但這些方法增加了載體被發(fā)現(xiàn)的危險(xiǎn)性。隨著無源導(dǎo)航技術(shù)的發(fā)展，地磁導(dǎo)航技術(shù)的出現(xiàn)為實(shí)現(xiàn)這一目標(biāo)提供了新的技術(shù)途徑。由于地磁場(chǎng)在全球范圍內(nèi)各點(diǎn)值都不相同，理論上與經(jīng)緯度是一一對(duì)應(yīng)的，同時(shí)某些地區(qū)磁場(chǎng)特征也很明顯，因此地磁導(dǎo)航成目前研究的一個(gè)熱點(diǎn)。地磁導(dǎo)航技術(shù)在獲取地磁信息時(shí)對(duì)外無能量輻射，具有良好的隱蔽性，因此可以實(shí)現(xiàn)載體長期高精度定位。地磁導(dǎo)航就是利用地磁圖特征與當(dāng)?shù)丶磿r(shí)測(cè)量磁特征信息進(jìn)行匹配導(dǎo)航的過程。

所謂地磁匹配，就是把預(yù)先規(guī)劃好的航跡上某些點(diǎn)的地磁場(chǎng)特征量繪制成參考圖(或稱基準(zhǔn)圖)，存貯在載體計(jì)算機(jī)中，當(dāng)載體經(jīng)過這些地區(qū)時(shí)，由地磁匹配測(cè)量儀器實(shí)時(shí)測(cè)量出經(jīng)過這些點(diǎn)地磁場(chǎng)特征量，以構(gòu)成實(shí)時(shí)圖。并在載體上的計(jì)算機(jī)中與參考圖進(jìn)行相關(guān)匹配，計(jì)算出載體的實(shí)時(shí)坐標(biāo)位置，供導(dǎo)航計(jì)算機(jī)解算導(dǎo)航信息。匹配精度受數(shù)字地磁圖精度、地磁匹配測(cè)量儀、匹配算法、匹配特征量等的影響。其中匹配算法是地磁導(dǎo)航的核心技術(shù)，本文主要研究基于ICCP（Iterative Closest Contour Point）算法的地磁匹配定位方法。

2 ICCP算法

ICCP算法最初來源于圖像配準(zhǔn)的ICP（Iterative Closest Point）算法，主要是通過最近點(diǎn)的迭代實(shí)現(xiàn)測(cè)量圖像與模型之間的對(duì)準(zhǔn)和匹配。

由于地磁圖通常以等值線的形式給出，匹配單元選為等值線上與測(cè)量點(diǎn)最近的點(diǎn)而得名。算法采用歐氏距離平方最小為目標(biāo)函數(shù)，求得測(cè)量航跡與真實(shí)航跡之間的最優(yōu)變換，通過該變換求得校正航跡，以實(shí)現(xiàn)對(duì)測(cè)量航跡的校正。

2.1 算法的基本描述

在載體在航行過程中，參考導(dǎo)航系統(tǒng)（一般是慣性導(dǎo)航系統(tǒng)）會(huì)給出一系列的航跡點(diǎn)，記測(cè)量航跡點(diǎn)集合為{xn}［n=1，2，…，N］，真實(shí)航跡點(diǎn)集合為{yn}，地磁測(cè)量值集合為{fn}。由于導(dǎo)航誤差，測(cè)量航跡點(diǎn)坐標(biāo)相對(duì)地磁圖存在誤差，為求得載體的真實(shí)位置，這里將測(cè)得的數(shù)據(jù)點(diǎn)與存儲(chǔ)的地磁圖進(jìn)行匹配。也就是要確定剛性變換T（旋轉(zhuǎn)和平移），該變換使圖上數(shù)據(jù)點(diǎn)和測(cè)量數(shù)據(jù)點(diǎn)間距離最小。點(diǎn)集{yn}正是要確定的。在地磁傳感器沒有測(cè)量誤差的情況下，數(shù)據(jù)點(diǎn)xn一定在地磁值為fn的等值線Cn上，但是不知道是在等值線的哪一點(diǎn)上，于是希望找到剛性變換T使得下式表示的距離最小：

M(C，TX)=∑Nn=1wnd2(Cn，Txn)（1）

其中X＝{xn}是測(cè)量數(shù)據(jù)點(diǎn)集合；C＝{Cn}是地磁測(cè)量值等值線集合；d(Cn，xn)是點(diǎn)xn與等值線Cn之間的距離，在距離度量中引入權(quán)系數(shù)wn以考慮第n個(gè)測(cè)量的相對(duì)重要程度。

用迭代算法求取使距離最小的變換步驟為：

對(duì)每一個(gè)數(shù)據(jù)點(diǎn)xn在其等值線上尋找最近點(diǎn)（依據(jù)歐氏距離最?。涍@些點(diǎn)為yn，假設(shè)yn是xn的相應(yīng)等值線點(diǎn)。

尋找變換T（旋轉(zhuǎn)和平移），使得集合Y＝{yn} 與集合X＝{xn}之間距離最?。?/p>

M(C，TX)=M(Y，TX)=∑Nn=1wn‖yn－Txn‖2(2)

這里先旋轉(zhuǎn)后平移，記對(duì)X旋轉(zhuǎn)的矩陣為R，平移矢量為t，于是有Txn=t+Rxn，這里2個(gè)集合的質(zhì)心分別是：

=1w∑Nn=1wnyn，=1w∑Nn=1wnxn，w=∑Nn=1wn(3)

旋轉(zhuǎn)矩陣可以用單位四元數(shù)q=(q0，q1，q2，q3)T表示，其中∑q2i=1。

四元數(shù)方法最初用于三維情況，其中，旋轉(zhuǎn)角度θ和旋轉(zhuǎn)軸與四元數(shù)的元有如下關(guān)系：q0=cos(θ/2)，［q1，q2，q3］T=sin(θ/2)。

這里處理的問題是二維的，旋轉(zhuǎn)軸為=(0，0，1)T，四元數(shù)簡化為q=［cos(θ/2)，0，0，sin(θ/2)］T，于是有：

R=cos θ－sin θ

sin θcos θ，S=∑Nn=1wn(yn－)(xn－)T(4)

W=S11+S2200S21－S12

0S11－S22S12+S210

0S12－S21S22－S110

S21－S1200－S11－S22(5)

矩陣W的4個(gè)特征值是實(shí)數(shù)，由下式給出：

λ=±［(S11+S22)2+(S21－S12)2］1/2，

±［(S11－S22)2+(S12+S21)2］1/2(6)

記最大的特征值為λm，則特征向量可由式(S11+S22－λm)q0+(S21－S12)q3=0計(jì)算出，由此給出旋轉(zhuǎn)角tan(θ/2)=(S11+S22－λm)/(S12－S21)。旋轉(zhuǎn)矩陣確定后，平移矢量為t=－R。

在上述計(jì)算最優(yōu)變換算法中，首先計(jì)算旋轉(zhuǎn)矩陣，然后計(jì)算平移矢量，也就是說，先旋轉(zhuǎn)集合X使其對(duì)準(zhǔn)集合Y的方向，然后進(jìn)行平移以使集合X的質(zhì)心與集合Y的質(zhì)心重合。

(1) 將集合X變換到集合TX，將新的集合TX作為起始集合進(jìn)行迭代，重復(fù)該過程直至收斂，即T停止顯著的變化。

迭代算法的收斂性非常明顯，從起始集合X0開始算，生成集合序列X1，X2，…，Xk，…，XF，每次迭代中距離減少，即:

M(C，Xk+1)=M(C，TkXk)≤M(C，Xk)(7)

且由于距離有下界（正值，正好是0），因此算法是收斂的。

(2) 經(jīng)過迭代后獲得的集合TX即為校正后的航跡點(diǎn)。

2.2 算法的局限性及改進(jìn)

（1） ICCP算法的假設(shè)前提

第一是地磁傳感器沒有測(cè)量誤差。這在實(shí)際使用過程中是不可能的，因?yàn)闊o論是地磁圖的預(yù)測(cè)量還是航行中的實(shí)際測(cè)量，測(cè)量誤差是不可避免且不可能完全一致。為此文獻(xiàn)［1］提出一種改進(jìn)算法，將測(cè)量誤差特性引入計(jì)算，并用馬氏距離（Mahalanobis distance）代替歐氏距離，取得了一定的效果，但是這也增加了算法的復(fù)雜度。

第二是載體的真實(shí)位置位于測(cè)量地磁值等值線上（或很近的位置）。隨著地磁測(cè)量精度的提高，認(rèn)為這一點(diǎn)基本上是能夠滿足的。

第三是載體的真實(shí)位置位于參考導(dǎo)航系統(tǒng)指示的位置附近。如果參考位置與真實(shí)位置相差很遠(yuǎn)，就很容易導(dǎo)致等值線算法失效，甚至引起誤匹配。為此文獻(xiàn)［2］提出，在應(yīng)用等值線算法進(jìn)行精匹配之前，先進(jìn)行粗匹配，使匹配后的航跡更接近于真實(shí)航跡，再以得到的航跡作為等值線匹配的輸入航跡進(jìn)一步匹配，以提高匹配精度。在粗匹配階段，以參考位置為中心，按照參考導(dǎo)航系統(tǒng)誤差大小確定搜索窗口的大小，在搜索窗口內(nèi)，對(duì)每個(gè)網(wǎng)格點(diǎn)，取與參考航跡形狀相似的地磁序列，作為匹配航跡，采用TERCOM算法中的MSD和MAD準(zhǔn)則匹配，以最小MSD和MAD所對(duì)應(yīng)的地磁序列代替參考航跡。

（2） 快速尋找等值線上的最近點(diǎn)。

根據(jù)迭代算法的第一步可知，ICCP對(duì)準(zhǔn)算法需要大量地計(jì)算測(cè)量點(diǎn)到數(shù)字地圖等值線之間的最近點(diǎn)和最近距離，這些計(jì)算相當(dāng)費(fèi)時(shí)。為了加快它們的計(jì)算，本文給出的快速尋找最近點(diǎn)方法不是在整個(gè)數(shù)字地圖，而是在某一個(gè)滑動(dòng)窗口區(qū)域內(nèi)尋找最近點(diǎn)，窗口的大小和位置根據(jù)距離函數(shù)Mk和xn，k+1來確定，窗口區(qū)域的不斷縮小提高了搜尋速度。

假設(shè)進(jìn)行第k次迭代時(shí)最近點(diǎn)集為Yk={ynk}，采用ICCP算法計(jì)算變換T后，根據(jù)式（2），變換后的點(diǎn)集Xk+1到Y(jié)k={ynk}之間的距離平方和為Mk：

Mk(C，TXk)=M(Yk，TXk)=∑Nn=1wn‖ynk－Txnk‖2

=M(Yk，Xk+1)(8)

進(jìn)入下一次循環(huán)，對(duì)新的點(diǎn)集Xk+1在等值線上尋找新的最近點(diǎn)集設(shè)為Yk+1={yn，k+1}，根據(jù)最近點(diǎn)的操作，應(yīng)該滿足:

‖yn，k+1－xn，k+1‖≤‖ynk－xnk‖≤Mk(9)

其中n=1，2，…，N。

式（9）表明，第k+1次的最近點(diǎn)集Yk+1={yn，k+1}都落在數(shù)據(jù)點(diǎn)集Xk+1相應(yīng)各點(diǎn)的Mk的范圍之內(nèi)。選擇滑動(dòng)窗口大小為Mk就能保證相應(yīng)等值線上的最近點(diǎn)落在這一區(qū)域內(nèi)，現(xiàn)在僅需要在xn，k+1的Mk鄰域內(nèi)搜索。

此方法加快了運(yùn)算速度，尤其當(dāng)?shù)螖?shù)達(dá)到一定次數(shù)后，Mk逐漸收斂，尋找最近點(diǎn)的范圍將只在很小的區(qū)域里進(jìn)行，計(jì)算量減少的程度更是非常明顯。

3 仿 真

3.1 地磁圖的構(gòu)建

利用Talyor多項(xiàng)式模型將實(shí)測(cè)地磁數(shù)據(jù)內(nèi)插到規(guī)則網(wǎng)格上構(gòu)造地磁圖（如圖1所示）。

3.2 算法的仿真

在數(shù)字地磁圖上，載體以某一航跡航行（航跡是任意設(shè)定的），在5個(gè)不同的地方（測(cè)量點(diǎn)數(shù)n≥5，完成匹配的最小點(diǎn)數(shù)為3點(diǎn)）獲取的地磁異常場(chǎng)值如圖1所示（單位為nT），圖中給出載體的真實(shí)航跡和地磁測(cè)量點(diǎn)以及5個(gè)地磁值對(duì)應(yīng)的等值線。預(yù)設(shè)的測(cè)量航跡由真實(shí)航跡加上5度的航向誤差以及某些平移矢量形成。由圖中可以看出，經(jīng)ICCP算法校正后的航跡非常接近真是航跡，證明本文匹配算法的可行性。

圖1 匹配示意圖

4 結(jié) 語

本文給出基于ICCP算法的地磁匹配導(dǎo)航的基本框架，并且針對(duì)地磁傳感器存在測(cè)量誤差，參考導(dǎo)航系統(tǒng)積累誤差過大，以及傳統(tǒng)ICCP算法運(yùn)算量過大的3個(gè)方面的問題提出相應(yīng)的解決方案。仿真實(shí)驗(yàn)表明：

（1） 粗匹配階段的加入，ICCP算法的收斂速度和匹配精度都得到了大幅度的提高。這是由于粗匹配搜索使得局部調(diào)整過程是在真實(shí)位置附近進(jìn)行的，而精匹配可以進(jìn)一步提高匹配精度，因?yàn)榫植科ヅ浜桔E調(diào)整可以使估計(jì)航跡更接近真實(shí)航跡。這一點(diǎn)在在參考航跡與真實(shí)航跡形狀差別較大時(shí)優(yōu)勢(shì)更加明顯。

（2） 采用滑動(dòng)窗口技術(shù)來確定最近點(diǎn)的選擇區(qū)域，縮小了尋找最近點(diǎn)的范圍，計(jì)算效率得以很大程度的提高。

參考文獻(xiàn)

［1］劉繁明，孫嵐，成怡.基于ICCP算法及其推廣的重力定位［J］.中國慣性技術(shù)學(xué)報(bào)，2004，12（5）：36-39.

［2］王可東，陳偲.水下地形匹配等值線算法研究［J］.宇航學(xué)報(bào)，2006，27（5）：995-998.

［3］劉繁明，成怡.重力/慣性匹配導(dǎo)航系統(tǒng)的仿真研究［J］.中國慣性技術(shù)學(xué)報(bào)，2005，13（3）：22-25.

［4］劉承香，劉繁明，劉柱，等.快速ICCP算法實(shí)現(xiàn)地形匹配技術(shù)研究［J］.船舶工程，2003，25（3）：54-56.

［5］劉承香.水下潛器的地形匹配輔助定位技術(shù)研究［D］.哈爾濱：哈爾濱工程大學(xué)，2003.

［6］魏東.重力匹配定位方法研究［D］.哈爾濱：哈爾濱工程大學(xué)，2004.

［7］楊功流，李士心，姜朝宇.地磁輔助慣性導(dǎo)航系統(tǒng)的數(shù)據(jù)融合算法［J］.中國慣性技術(shù)學(xué)報(bào)，2007，15（1）：47-50.

［8］Goldenberg.Felix Geomagnetic Navigation beyond the Magnetic Compass.Record-IEEE PLANS，Position Location and Navigation Symposium，v2006，2006 IEEE/ION Position，Location，and Navigation Symposium，2006：684-694.

［9］Behzad Kamgar-Parsi，Behrooz Kamgar-Parsi.Vehicle Localization on Gravity Maps.Proc.SPIE Conf.on Unmanned Vehicle Technology，1999，3 639:182-191.

［10］Bishop G C.Gravitational Field Maps and Navigation Errors［J］.Oceanic Engineering，2002，27（3）：726-737.

作者簡介 李豫澤 男，1982年出生，湖北洪湖人，碩士研究生。主要從事智能導(dǎo)航定位、地磁測(cè)量方面的研究。

石志勇 男，1965年出生，四川仁壽人，副教授，博士。主要從事現(xiàn)代檢測(cè)、導(dǎo)航定位、地磁測(cè)量方面的研究。

注：本文中所涉及到的圖表、注解、公式等內(nèi)容請(qǐng)以PDF格式閱讀原文