基于ICCP算法的地磁匹配定位方法

摘 要：基于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=1wnyn－Txn2(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=1wnynk－Txnk2

=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格式阅读原文