第３Ｏ卷第６期　计算机仿真　２０１３年６月　

文章编号：１００６—９３４８（２０１３）０６—００１５—０５　

ＩＣＣＰ重力匹配辅助导航算法研究及改进　

白文平　，王志刚　

（１．中国人民解放军９１５５０部队，辽宁大连１１６０２３；　２．海军工程大学兵器工程系，湖北武汉４３００３３）　

摘要：迭代最近等值线算法（ＩＣＣＰ）是一种重要的匹配辅助导航算法，ＩＣＣＰ虽稳定性高，却具有实时性差的缺陷。通过研究　与分析可知，采样结构是导致算法实时性差的关键因素，并且改变算法的采样结构可有效解决算法实时性差的问题。在改　进ＩＣＣＰ的基础上，又通过“二次迭代”技术，将匹配位置用于更新测量和指示重力，并重新进行匹配解算，可进一步提高算　法的匹配精度。仿真结果表明，以上工作对算法实时性和精度的改进是有效的。　关键词：迭代最近等值线算法；重力匹配辅助惯性导航；匹配；卡尔曼滤波　

中图分类号：ＴＪＯ１　文献标识码：Ｂ　

Ｒｅｓｅａｒｃｈｉｎｇ　ａｎｄ　Ｉｍｐｒｏｖｉｎｇ　ｏｆ　ＩＣＣＰ　Ａｌｇｏｒｉｔｈｍ　

ｆｏｒ　Ｇｒａｖｉｔｙ　Ａｉｄｅｄ　Ｉｎｅｒｔｉａｌ　Ｎａｖｉｇａｔｉｏｎ　

ＢＡＩ　Ｗｅｎ—ｐｉｎｇ　．ＷＡＮＧ　Ｚｈｉ—ｇａｎｇ　’　

（１．Ｔｈｅ　９１５５０ｔｈ　Ｔｒｏｏｐ　ｏｆ　ＰＬＡ，Ｄａｌｉａｎ　Ｌｉａｏｎｉｎｇ　１　１６０２３，Ｃｈｉｎａ；　

２．Ｄｅｐａｒｔｍｅｎｔ　ｏｆ　Ｎａｖｉｇａｔｉｏｎ，Ｎａｖａｌ　Ｕｎｉｖｅｒｓｉｔｙ　ｏｆ　Ｅｎｇｉｎｅｅｒｉｎｇ，Ｗｕｈａｎ　Ｈｕｂｅｉ　４３００３３，Ｃｈｉｎａ）　

ＡＢＳＴＲＡＣＴ：Ｔｈｅ　ｉｎｔｅｒｖａｌ　ｃｌｏｓｅｓｔ　ｃｏｎｔｏｕｒ　ｐｏｉｎｔｓ　ａｌｇｏｒｉｔｈｍ　ｉｓ　ａｎ　ｉｍｐｏｒｔａｎｔ　ｎａｖｉｇａｔｉｏｎ　ａｌｇｏｒｉｔｈｍ　ｆｏｒ　ｍａｔｃｈｉｎｇ．Ａ１一　

ｔｈｏｕｇｈ　ｔｈｅ　ＩＣＣＰ　ａｌｇｏｒｉｔｈｍ　ｈａｓ　ｍｅｒｉｔ　ｏｆ　ｈｉｇｈ　ｓｔａｂｉｌｉｔｙ，ｉｔ　ａｌｓｏ　ｈａｓ　ｂａｄ　ｑｕａｌｉｔｙ　ｏｆ　ｒｅａｌ—ｔｉｍｅ．Ａｃｃｏｒｄｉｎｇ　ｔｏ　ｔｈｅ　ｒｅｓｅａｒｃｈ　

ａｎｄ　ａｎａｌｙｓｉｓ　ｉｎ　ｔｈｉｓ　ｐａｐｅｒ，ｗｅ　ｃａｎ　ｆｉｎｄ　ｔｈａｔ　ｔｈｅ　ｓａｍｐｌｉｎｇ　ｓｔｒｕｃｔｕｒｅ　ｌｅａｄｓ　ｔｏ　ａｌｇｏｒｉｔｈｍ’Ｓ　ｂａｄ　ｑｕａｌｉｔｙ　ｏｆ　ｒｅａｌ—ｔｉｍｅ，ａｎｄ　

ｔｈｉｓ　Ｃａｎ　ｂｅ　ｉｍｐｒｏｖｅｄ　ｂｙ　ｃｈａｎｇｉｎｇ　ｉｔｓ　ｓａｍｐｌｉｎｇ　ｓｔｒｕｃｔｕｒｅ．Ｔｈｉｓ　ｐａｐｅｒ　ａｌｓｏ　ｐｒｅｓｅｎｔｅｄ　ａ　ｒｅｉｔｅｒａｔｉｖｅ　ｔｅｃｈｎｉｑｕｅ，ｗｈｉｃｈ　ｉｓ　

ｂａｓｅｄ　ｏｎ　ｒｅｍａｔｃｈｉｎｇ　ｔｈｅ　ｐｏｓｉｔｉｏｎ　ｗｉｔｈ　ｔｈｅ　ｒｅｆｒｅｓｈｅｄ　ａｎｄ　ｉｎｄｉｃａｔｅｄ　ｇｒａｖｉｔｙ　ｔｏ　ｆｕｒｔｈｅｒ　ｉｍｐｒｏｖｅ　ｔｈｅ　ｍａｔｃｈｉｎｇ　ａｃｃｕｒａｃｙ．　

Ｓｉｍｕｌａｔｉｏｎ　ｒｅｓｕｌｔｓ　ｓｈｏｗ　ｔｈａｔ　ｔｈｅ　ｉｍｐｒｏｖｅｍｅｎｔｓ　ｏｆ　ＩＣＣＰ　ｍａｔｃｈｉｎｇ　ａｌｇｏｒｉｔｈｍ　ａｒｅ　ｅｆｆｅｃｔｉｖｅ．　

ＫＥＹＷＯＲＤＳ：Ｉｎｔｅｒｖａｌ　ｃｌｏｓｅｓｔ　ｃｏｎｔｏｕｒ　ｐｏｉｎｔ（ＩＣＣＰ）ａｌｇｏｒｉｔｈｍ；Ｇｒａｖｉｔｙ　ａｉｄｅｄ　ｉｎｅｒｔｉａｌ　ｎａｖｉｇａｔｉｏｎ；Ｍａｔｃｈｉｎｇ；Ｋａｌｍａｎ　ｆｉｌｔｅｒ　

１　引言　

惯性导航系统（Ｉｎｅｒｔｉａｌ　Ｎａｖｉｇａｔｉｏｎ　Ｓｙｓｔｅｍ，ＩＮＳ）的误差随　

时间积累发散，无法长时间保持高精度，因此，必须要通过其　

它导航方式实时或定期修正ＩＮＳ。出于军事上的隐蔽性要　求，水下军用潜器很难利用卫星导航或无线电导航信息，而　

利用水下地理特征辅助导航却是很好的选择。　

利用不同的地理特征可构成相应的匹配辅助惯性导航　系统，这其中重力匹配辅助惯性导航系统…由于具有完全自　

主性和高隐蔽性因而获得各国的广泛关注，其中具有代表性　

的产品是美国Ｌｏｃｋｈｅｅｄ　Ｍａｒｔｉｎ公司研制的通用重力模块　

（Ｕｎｉｖｅｒｓａｌ　Ｇｒａｖｉｔｙ　Ｍｏｄｕｌｅ，ＵＧＭ）所组成的重力导航系统　

（Ｎａｖｉｇａｔｉｏｎ　ａｎｄ　Ｇｒａｖｉｔｙ　Ｓｙｓｔｅｍ，ＮＧＳ）和贝尔公司研制的重力　

收稿日期：２０１２—０８—０３修回日期：２０１２—０８—３０　辅助导航系统　（Ｇｒａｖｉｔｙ　Ａｉｄｅｄ　Ｉｎｅｒｔｉａｌ　Ｎａｖｉｇａｔｉｏｎ　Ｓｙｓｔｅｍ，　

ＧＡＩＮＳ）。在对重力匹配辅助惯性导航系统的研究中，合适　

的匹配算法是其中的一个核心技术。ＩＣＣＰ算法　是近年　

来使用较多的一类匹配算法，最初它是用于图像配准中，Ｂｅ．　

ｈｚａｄ　Ｋ．Ｐ首次将其引入到水下辅助导航，并给出了其具体的　

实现形式，Ｂｉｓｈｏｐ　Ｇ．Ｃ则通过仿真，系统地验证了该算法的　

可行性及各种误差影响。　

ＩＣＣＰ算法有其独特的优点，它将参考航迹作为点集来　

匹配的设计原理，可有效提高算法的容错能力，使其具有较　

好的稳定性，目前，ＩＣＣＰ算法已成为水下辅助导航领一种重　

要的相关匹配算法，针对算法研究的工作日益增多，如文献　

［６］（刘承香，２００３）曾对其容错能力以及可靠性做过详尽的　

研究分析，文献［７］（郑彤，２００８）针对其计算量大的问题做　

过专门的改进。尽管如此，对于如何改正算法实时性差的缺　

陷却鲜有研究，而这正是在原算法设计结构的基础上所无法　

一

１５—　完成。要想解决算法实时性差的问题，必须改变算法的设计　

结构（如采样结构）。针对算法缺陷，本文在接下来的第四节　

中将予以讨论，研究增强算法实时眭的方法。　

２重力匹配辅助惯性导航原理　

重力匹配辅助惯性导航系统是利用高分辨率的重力异　

常图组成的基本特征信息数据库，结合惯性导航系统给出的　

位置信息和海洋重力仪器　提供的实测值，按照一定的匹配　

算法对惯导系统的位置进行校正。系统原理结构图如图１　

所示。　

图１　重力匹配辅助惯性导航系统原理示意图　

为了确定实测重力的具体位置信息，通常是以惯性导航　

的近似位置为基准，根据实际观测精度确定置信区间范围，　

在已知地理特征信息数据库中提取重力信息，并根据潜器下　

潜深度通过计算重力异常垂直梯度　及地形对其影响　叫以　

对实测重力数据进行归算　，以便将海洋重力实测的重力　

值和重力数据库提取的重力值归算到同一个计算面上，然后　

按照一定的算法估算出最佳匹配位置，最后再通过卡尔曼滤　

波将位置误差作为观测量，对惯导系统的平台误差角和位置　

误差等进行估计，从而完成对惯导系统的误差修正。　

３　ＩＣＣＰ匹配辅助导航算法分析　

目前，常用的匹配辅助导航算法主要有单点迭代算法和　

序列迭代算法两大类。单点迭代算法是对每一个采样点都　

进行匹配，并将修正信息提供给ＩＮＳ，以抑制惯导误差增长，　

该类算法以ＳＩＴＡＮ（Ｓａｎｄｉａ　Ｉｎｅｒｔｉａｌ　Ｔｅｒｒａｉｎ—Ａｉｄｅｄ　Ｎａｖｉｇａｔｉｏｎ）　算法”　为典型代表。序列相关匹配算法是当匹配序列采样　

长度达到设定的点数之后，才进行一次匹配，匹配完成之后，　

将修正信息提供给ＩＮＳ，本文将要研究的ＩＣＣＰ算法就属于此　

类算法。　

３．１　ＩＣＣＰ算法原理　

ＩＣＣＰ算法最初是用于图像配准，主要是通过最近点的　

迭代实现测量图像与模型之间的对准和匹配。由于重力图　

通常以等值线的形式给出，匹配位置选为等值线上与测量点　

最近的点，算法因而得名。ＩＣＣＰ算法原理如图２所示。　

其中由Ｐ　（ｉ＝１，２，…，Ⅳ）点构成的直实线是“ＩＮＳ指示　

航迹”，Ⅳ为航迹的长度。弯曲实线称为“真实航迹”，由置（ｉ　

＝１，２，…，Ⅳ）构成，ｃ　（ｉ＝１，２，…，Ⅳ）表示的是当地实测重力　

一】６一　Ｃ３　

图２　ＩＣＣＰ算法原理示意图　

异常等值线。　

由于惯导误差的存在，｛Ｐ　：ｉ＝１，…，Ｎ｝和｛Ｘｉ：ｉ＝１，…，　

Ⅳ｝之间必定存在误差。ＩＣＣＰ算法的基本思想就是：　必定　

位于Ｃ　的等值线上，那么按照一定的准则，使Ｐ　靠拢到Ｃ　

上，从而找到最优估计点　和对应的“匹配航迹”（如图２中　

虚线所示），实现对真实航迹的最优估计。这实际上是一种　

求最优解的过程，文献［４］给出的最优化目标函数如下：　

Ｎ　Ｎ　Ｅ＝∑ｄ（　。　。　。）＋　∑ｄ（　，ｃ　）　（１）　

为所求的估计点Ｉ１／的位置，ａ　为惯导指示位置Ｐ　点的位　

置，ｃ　为　在Ｃ　等值线上的投影。ｄ（　，ａ）为　，ａ间的欧几　

里得距离，Ｋ为刚度系数　Ｊ。在已知载体指示位置和求得等　

值线上的投影位置，并选择好刚性系数后，那么运用相应的　

优化算法就可就可实现对（１）式的求解，从而得到最优解，即　

真实航迹。　

３．２　ＩＣＣＰ算法步骤　

对于ＩＣＣＰ算法的匹配过程可解释为，首先在最优化目　

标函数的基础上求得一个刚性变换（包括旋转和平移），之后　

通过迭代反复求解刚性变换，直至刚性变换不再有明显变　

化，最后对惯导指示航迹使用刚性变换从而获得最终的匹配　

航迹。算法的具体步骤如下：　

１）对每一个指示航迹点Ｐ　在测量重力值对应的重力等　

值线ｃ　上寻找最近点，记为　；　２）求解刚性变换　，使得集合Ｙ＝｛Ｙｉ：ｉ＝１，…，Ｎ｝和Ｐ　

＝｛Ｐ　：　１，…，Ⅳ｝之间欧氏距离最小，即Ｍ（Ｙ，ＴＰ）＝　

∑Ｉｌ　ｙｌ　ＴＰ　【ｌ；　‘　ｌ　３）将集合Ｐ变换到ＴＰ，将新的集合ＴＰ作为起始集合进　

行下一步迭代，直至收敛即Ｔ停止显著变化；　

４）经过收敛迭代后获得的集合ＴＰ即为最终的匹配　

航迹。　

３．３　ＩＣＣＰ算法特点分析　

由３．１节、３．２节的原理分析和计算步骤可以看出，ＩＣＣＰ　

重力相关匹配算法具有如下的特点：　

１）ＩＣＣＰ算法是为了寻找全局意义下的最优对准方法，　

由于是在整个地图上搜寻等值线上的最近点，因此理论上该　算法在任何初始位置误差的情况下都能工作；也就是说，只　

要没有超出数字地图范围的情况下，如果重力特征独特，无　

论参考导航系统的积累误差有多大，匹配算法都能进行匹配　

对准。　

２）由算法流程图可以看出，它是一种序列迭代算法，只　

有在采样到足够多的航迹点之后才能进行事后的批处理，无　

法连续的给出匹配位置，所以实时性比较差；　

３）区别于ＳＩＴＡＮ算法对每个航迹点分别计算匹配点，　

ＩＣＣＰ算法是对特定时间段内的航迹点集操作，因此得出的　

匹配航迹稳定性较好，不易出现误匹配，算法定位过程中潜　

器可以机动航行，但因为其是作刚性变换，故对航向误差比　

较敏感；　４）ＩＣＣＰ算法在等值线上寻找最近点的计算量非常大，　

因为它是在整个重力背景场图上搜寻匹配位置，从而可能在　

实时应用环境中受到限制。　

４　ＩＣＣＰ匹配辅助导航算法改进　

由以上分析：ＩＣＣＰ算法有其独特的优点，它将参考航迹　

作为点集来匹配的设计原理，可有效提高算法的容错能力，　

使其具有较好的稳定性，尽管如此，算法却存在实时性差的　

问题，针对算法缺陷，本文在接下来的小节中将予以讨论，研　

究增强算法实时性的方法。　

４．１　固定采样点数改善ＩＣＣＰ算法实时性　

本文３．３节对ＩＣＣＰ算法的特点进行了分析，指出了其　

实时性差的主要缺陷。ＩＣＣＰ算法之所以实时性差，在于它　

的匹配原理是建立在事后的批处理上，算法完成一次匹配必　

须在采样到足够的采样点和重力测量值之后才能进行。由　

匹配辅助导航的原理可以知道，辅助导航是建立在提取重力　

变化特征的基础上进行的，即载体所经过航迹上的重力值必　

须有明显的变化特征。由于海底地形在很小的距离范围内　

就能发生很大的变化，因此水下地形辅助导航相应的采样周　

期可以很短，而重力场是地球的基本物理场，它的变化相对　

海底地形没有那么剧烈，只有在很大的范围上才能看出它的　

变化特征，另外再考虑到水下潜器运动速度相对较低，此时　

相应的采样时间势必要大一些。以１２节的船速、一次匹配　

需要２０个采样点为例，再考虑到各种重力测量误差，若要重　力发生明显变化，相邻采样点间的距离至少要在１海里以　

上，此时的采样周期则需５分钟，相邻两次匹配的时间间隔　

即为１００分钟，算法的实时性差的缺陷由此可见。　

由以上分析可知，采样时间最为耗时，算法采样过程如　

图３所示，算法最初设计时是将参考航迹点Ｐ＝｛Ｐ　：ｉ＝１，　

…，Ⅳ｝作为整体进行批处理，因此每次匹配需要多个采样　

点。匹配计算后，最新的匹配航迹点：　与之前的匹配点：　

Ｍ＝｛Ｍ　：ｉ＝１，…，Ｎ一１｝同时给出，这一设计就是导致算法　

实时性不高的根本原因。　

下面阐述如何解决算法实时性差的方法，这一方法借鉴　图３　ＩＣＣＰ算法采样示意图　

了计算机编程思想中的移位寄存器原理，改进后的算法采样　

示意图如图４所示。　

图４改进的ＩＣＣＰ算法采样示意图　

即在获得最新的采样点Ｐ　＋。时，用其更新之前采样点集　

的第一个采样值Ｐ＿，更新点集经移位处理后，再以新的采样　

点集Ｐ＝｛Ｐ　：ｉ＝２，…，Ⅳ＋１｝进行匹配计算，可获得匹配航　

迹点集Ｍ＝｛Ｍ　：ｉ＝２，…，Ⅳ＋１｝，其中对应最新采样点Ｐ　＋　

的匹配值　＋　即为当前时刻的匹配点。　

改进后的ＩＣＣＰ算法与原算法的最大不同在于：只需一　

次采样到Ｎ个值，之后的匹配位置即可实时给出。除此之外　

还有以下两个优点：首先，由于前后两次匹配所用的采样点　

集只有一个值不同，因此点集Ｐ：｛Ｐ　：ｉ＝２，…，Ⅳ｝对应的等　

值线以及权值无需重新提取及计算，从而降低了算法的复杂　

度，也为算法的实现提供了方便；其次，由于匹配位置可实时　

给出，在此基础上就可将其用于重力测量值的改正，进而提　

高重力测量的精度。　

由以上介绍的算法改进原理可以知道，每个匹配位置将　

被重复计算Ⅳ次（初始采样点集除外），考虑到每次计算的　

随机误差，改进算法在完成Ⅳ匹配计算之后，将对计算值做　

一次重调，即对参考航迹点为Ｐ　．　对应的匹配点肘　进行加　

权计算，从而得到一个最优匹配位置，计算方法如下：　

其中ｋ为常数，（　，　）为匹配点肘　对应的经、纬度坐　

标，　为权值，可由计算航迹点在相应点集中的权值Ｗ　来计　

算，具体形式为：　

一

１７一