基于ＩＣＣＰ算法的地磁匹配定位方法

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

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

中圖分類號：TJ765，TN966文獻標(biāo)識碼：B文章編號：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 引 言

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

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

2 ICCP算法

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

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

2.1 算法的基本描述

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

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

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

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

對每一個數(shù)據(jù)點xn在其等值線上尋找最近點（依據(jù)歐氏距離最小），記這些點為yn，假設(shè)yn是xn的相應(yīng)等值線點。

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

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

這里先旋轉(zhuǎn)后平移，記對X旋轉(zhuǎn)的矩陣為R，平移矢量為t，于是有Txn=t+Rxn，這里2個集合的質(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個特征值是實數(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計算出，由此給出旋轉(zhuǎn)角tan(θ/2)=(S11+S22－λm)/(S12－S21)。旋轉(zhuǎn)矩陣確定后，平移矢量為t=－R。

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

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

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

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

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

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

2.2 算法的局限性及改進

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

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

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

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

（2） 快速尋找等值線上的最近點。

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

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

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

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

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

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

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

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

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

3 仿 真

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

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

3.2 算法的仿真

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

圖1 匹配示意圖

4 結(jié) 語

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

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

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

參考文獻

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

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

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

［4］劉承香，劉繁明，劉柱，等.快速ICCP算法實現(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é)報，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)航定位、地磁測量方面的研究。

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

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