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

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

關鍵詞：ICCP算法；自主無源導航；地磁匹配；誤差校正

中圖分類號：TJ765，TN966文獻標識碼：B文章編號：1004373X(2008)2012203

Geomagnetic Localization Based on ICCP Algorithm

LI Yuze1，SHI Zhiyong1，YANG Yuntao1，FENG 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 引 言

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

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

2 ICCP算法

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

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

2.1 算法的基本描述

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

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

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

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

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

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

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

這里先旋轉后平移，記對X旋轉的矩陣為R，平移矢量為t，于是有Txn=t+Rxn，這里2個集合的質心分別是：

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

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

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

這里處理的問題是二維的，旋轉軸為=(0，0，1)T，四元數簡化為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個特征值是實數，由下式給出：

λ=±［(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計算出，由此給出旋轉角tan(θ/2)=(S11+S22－λm)/(S12－S21)。旋轉矩陣確定后，平移矢量為t=－R。

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

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

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

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

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

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

2.2 算法的局限性及改進

（1） ICCP算法的假設前提

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

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

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

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

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

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

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

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

進入下一次循環，對新的點集Xk+1在等值線上尋找新的最近點集設為Yk+1={yn，k+1}，根據最近點的操作，應該滿足:

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

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

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

此方法加快了運算速度，尤其當迭代次數達到一定次數后，Mk逐漸收斂，尋找最近點的范圍將只在很小的區域里進行，計算量減少的程度更是非常明顯。

3 仿 真

3.1 地磁圖的構建

利用Talyor多項式模型將實測地磁數據內插到規則網格上構造地磁圖（如圖1所示）。

3.2 算法的仿真

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

圖1 匹配示意圖

4 結 語

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

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

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

參考文獻

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

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

［3］劉繁明，成怡.重力/慣性匹配導航系統的仿真研究［J］.中國慣性技術學報，2005，13（3）：22-25.

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

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

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

［7］楊功流，李士心，姜朝宇.地磁輔助慣性導航系統的數據融合算法［J］.中國慣性技術學報，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年出生，湖北洪湖人，碩士研究生。主要從事智能導航定位、地磁測量方面的研究。

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

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