基于仿射修正技术的水下地形ICCP匹配算法

徐晓苏，吴剑飞，徐胜保，王立辉，李佩娟

（1.微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2.东南大学 仪器科学与工程学院，南京 210096）

基于仿射修正技术的水下地形ICCP匹配算法

徐晓苏，吴剑飞，徐胜保，王立辉，李佩娟

（1.微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2.东南大学 仪器科学与工程学院，南京 210096）

ICCP匹配算法是水下组合导航系统中最为重要的匹配算法。针对传统ICCP匹配算法存在仅对水下航行器惯性导航系统指示航迹作旋转和平移的刚性变换的局限性问题，为提高水下航行器地形辅助导航系统中匹配算法的精度，分析了水下航行器惯性导航系统误差特性，建立了误差模型，提出了基于仿射修正技术的水下地形ICCP匹配算法。首先利用ICCP匹配算法对惯性导航系统指示航迹进行刚性变换，再利用最小二乘法求解仿射参数，进而对ICCP匹配航迹进行仿射修正。仿真研究表明，基于仿射修正技术的ICCP匹配算法能较好地解决传统ICCP匹配算法刚性变换的局限性，匹配精度优于传统ICCP算法，匹配误差小于数字地图网格间距的50%，同时仿射修正所耗费时间极少，所增加的时间仅为传统ICCP匹配算法匹配时间的千分之一。

水下航行器；惯性导航；地形匹配；ICCP；仿射变换

水下航行器因其在未来的军用和民用领域都具有重要的战略意义而成为了世界上军事强国重点发展的领域。水下航行器有高度隐蔽性和保持长时间精确航行的要求，而实现这一要求的关键技术之一是其导航技术。水下的导航方式有别于陆路导航，GPS、无线电等导航方式因水中信号迅速衰减而不适用于水下航行器。惯性导航因其具有自主性、隐蔽性等优点，成为了水下航行器导航手段的首选。但惯性导航有固有的缺陷，其误差会随着时间的积累逐渐增大，因而在惯性导航定位系统运行一段时间后必须采用外来信息对惯性导航系统进行修正。随着导航技术特别是组合导航技术的发展，应用地形、重力、地磁等信息来辅助惯性导航[1]为水下航行器导航研究提供了一种新思路。由于地形高程信息测量较为方便，且陆路上的地形匹配技术已应用于实际，可为水下地形匹配导航提供借鉴，因而应用地形高程信息来辅助水下航行器的导航具备高度可行性。

ICCP（Iterative Closest Contour Point）算法是目前水下地形匹配导航技术最为重要的匹配算法之一，其本质为相关极值匹配算法[2]。但是ICCP算法对惯导航迹所作的修正仅仅是旋转和平移，没有对航迹进行缩放，因而不能修正辅助定位过程中惯导指示航迹相对于实际航迹发生的形变所造成的误差，因此本文引入了仿射修正技术，以解决 ICCP算法的缺陷，提高匹配精度。

1 ICCP算法

ICCP算法最初由图像配准算法 ICP（Iterative Closest Point）发展而来的[2]。国内外许多专家、学者对ICCP算法进行了系统研究，Behzad K.P.首次将该算法引入重力匹配辅助导航，并对其实现形式进行了较为详细的阐述。Bishop通过大量仿真试验系统地分析了该算法的可行性及误差影响[3]，刘承香详细分析了 ICCP算法在地形匹配辅助导航的适用性，并对ICCP关键技术展开了研究[4]。

1.1 ICCP算法描述

ICCP算法匹配[4-7]示意图如图1所示。水下航行器沿着实际航迹序列(N为采样点个数)航行过程中，由于惯性导航系统存在误差，因此主惯导系统会给出一个偏离实际航迹序列的指示航迹序列，与此同时，测深装置也记录测得的水深序列。在测深装置没有误差的情况下，可以确定实际航迹序列一定位于根据实测水深信息序列生成的水深等值线上。通常认为，主惯导系统给出的位置信息在一定误差范围内是可信的，因此可以将主惯导系统的指示航迹序列作为待匹配航迹序列，根据水深信息序列在数字地图中生成等值线并在等值线上的提取最近点序列作为匹配目标，迭代地寻找刚性变换T，T包括一个旋转量R和一个平移量t，使以下目标函数最小：

其中，wi代表其中各序列点的权值，k代表第k次迭代过程，N代表航迹序列点个数。

图1 ICCP算法匹配示意图Fig.1 Description of ICCP algorithm

利用四元数法[4]找到刚性变换T后，对待匹配航迹序列作刚性变换T·Si：

1.2 ICCP算法的缺陷

主惯导系统的定位误差有如下特性：

根据加速度计的偏移、陀螺仪的漂移以及惯性导航系统的辅助定位周期可以得出[8]：

●水下航行器运行一段时间后，主惯导定位系统给出的位置存在一定的误差，计为，其中Xδ为纬度误差，Yδ为经度误差。

●通常地形辅助定位的时间比较短，因而可得出该段时间内，惯导解算的速度v为：其中，vr为水下航行器的真实速度，vδ为辅助定位前航行器速度积累误差，vε为辅助定位过程中速度误差的变化量。速度误差主要是由加速度计零偏和重力加速度在水平姿态误差上的投影所造成，由于地形辅助定位时间内引起的速度误差远小于航行器辅助定位之前引起的误差，故式(3)可以简化为：

●同理于速度解算的分析，可得惯导解算的航向d：

其中，dr为水下航行器的真实航向，dδ为航行器在辅助定位前航向积累误差，dε为辅助定位过程中因误差引起的航向变化量。同理于速度解算，计算航向的式(5)也可以简化成：

由前面的分析可得，在惯导修正的这段时间，惯导解算航迹和航行器实际航迹之间的关系为：

通过上述对主惯导系统误差特性分析可得，由于加速度计的偏移、陀螺仪的漂移，主惯导系统的指示航迹和水下航行器实际航迹之间的关系不仅存在旋转和平移，而且也存在缩放，因而不是一种仅包旋转和平移的刚性变换关系。ICCP算法作了实际航迹是水下航行器惯性导航系统的测量航迹的平移和旋转的刚性变换的假设，因而匹配的误差较大。

针对这一缺陷，可引入仿射模型[8]来对ICCP匹配后的航迹进行再次修正，以提高匹配精度。通过模拟退火等算法来搜索仿射变换因子的最优解以实现航迹匹配的计算量较大，难以满足水下航行器地形匹配的实时性要求，因此本文综合考虑了匹配精度和效率，采用最小二乘法来求解仿射参数，提高算法实时性，再对 ICCP匹配算法的匹配结果进行仿射变换，以解决ICCP匹配算法所存在的缺陷，提高匹配精度。

2 仿射变换算法

2.1 仿射变换数学描述

通过对惯导系统的误差模型的分析，可以建立一个简单仿射变化模型[9-10]，包含平移、旋转、缩放三种变换，分别对应于水下航行器的初始位置误差、航向角误差、速率误差。其数学描述可表示为：

其中，POS为变换前的位置信息，POS’为变换后的位置信息，θ为旋转角，对应于航向角误差；α为缩放量，对应于速率误差；tx、ty为平移量，对应于初始位置误差。

图2 仿射模型示意图Fig.2 Description of affine model

2.2 仿射变换参数求解

仿射变换参数求解具体方法叙述如下：

将求得ESD(tx,ty,α,θ)最小时候的g记为，对ESD求导可得：

由于ESD对g的二阶导数

3 仿射修正ICCP匹配算法

基于仿射修正的ICCP匹配算法具体叙述如下：

第一步：在匹配区建立的规则网格数字地图上提取等深线。

第二步：由主惯性导航系统的位置信息序列和航行过程中实际测量的水深信息，得到一个预匹配的初始对准航迹。

第三步：应用ICCP算法，对初始对准航迹进行刚性变换，寻找全局最优的航迹。

第四步：应用仿射变换，对由ICCP算法得到的最优航迹进行缩放和旋转变换。

第五步：对最后得到的航迹进行精度评估，利用成功匹配的结果修正主惯导系统。

图3 射修正ICCP算法流程图Fig.3 Flow chart of ICCP algorithm based on affine correction

4 仿真实验

为了验证本文所述匹配方法在不同特征地形[11]上的有效性，本文在选取了两块特征不同的地形，其中地形a水深标准差为5.7029，地形b水深标准差为7.8779，如图4(a)、图4(b)所示。在每块地形上规划三条不同的航迹，再利用 ICCP算法和仿射修正 ICCP算法进行匹配，比较匹配误差以验证仿射修正 ICCP算法在不同地形特征，不同航迹上提高匹配精度的有效性，航迹匹配结果如图5(a)、图5(b)所示。

实验中的主惯导系统的惯性器件参数和采样参数如表1。两块地形匹配结果统计如表2(a)、表2(b)。

表1 仿真参数Tab.1 Simulation parameters

图4 (a) 仿真实验所用的地形图(a)Fig.4(a) Topographic map (a) used in the simulation

图4 (b) 仿真实验所用的地形图(b)Fig.4(b) Topographic map (b) used in the simulation

图5 (a) 水下航行器航迹匹配图（a）Fig.5(a) Topographic map (a) used in the simulation

图5 (b) 水下航行器航迹匹配图（b）Fig.5(b) Topographic map (b) used in the simulation

表2 (a) 地形a匹配结果Tab.2(a) Matching results of topographic map a

表2 (b) 地形b匹配结果Tab.2(b) Matching results of topographic map b

从地形匹配的结果可以看出：在表1所设置的仿真参数条件下，在地形特征不同的两个区域，沿三种不同航向的航迹，利用本文所述的仿射修正ICCP匹配方法都能解决ICCP算法仅做旋转和平移的刚性变换的缺陷。相比于ICCP算法的匹配方法，仿射修正ICCP匹配方法在匹配精度上有很大的提高，同时，由于ICCP匹配航迹已经位于真实航迹附近，仿射修正时无需进行全局最优收敛的判断，因而运算量较小，一般只需ICCP所耗时间的千分之一。

5 结 论

本文分析了ICCP算法刚性变换的局限性，并针对ICCP算法的局限性，提出了基于仿射修正技术水下地形ICCP匹配导航算法，给出了仿射变换的参数求解方法。通过仿真实验的验证了算法在不同地形不同航迹上匹配的有效性，结果表明：基于仿射修正技术的水下地形ICCP匹配算法可以以极小的时间代价来弥补ICCP算法刚性变换的局限性，匹配精度优于ICCP算法，匹配误差小于数字地图格网间距的50%。

（References）：

[1]Deng Z,Ge Y,Guan W,et al.Underwater map-matching aided inertial navigation system based on multigeophysical information[J].Frontiers of Electrical and Electronic Engineering in China,2010,5(4):496-500.

[2]童余德，边少锋，蒋东方，等.实时ICCP算法重力匹配仿真[J].中国惯性技术学报，2011，19(3)：340-343.TONG Yu-de,BIAN Shao-feng,JLANG Dong-fang,XIAO Sheng-hong.Gravity matching simulation of realtime ICCP algorithm[J].Journal of Chinese Inertial Technology,2011,19(3):340-343.

[3]Bishop G C.Gravitational field maps and navigational errors[J].Oceanic Engineering,2002,27(3):726-737.

[4]刘承香.水下潜器的地形匹配辅助定位技术研究[D].哈尔滨：哈尔滨工程大学，2003.LIU Cheng-xiang.Research on the terrain matching system of the underwater vehicle[D].Harbin:Harbin Engineering University,2003.

[5]Zhou Yingying,Zhang Yanshun,Guo Lei.Simulation of geomagnetic/inertial integrated navigation system[C]//2011 Third International Conference on Measuring Technology and Mechatronics Automation.2011,vol.3:253-256.

[6]Zhang Y,Kang C,Li H,et al.Geomagnetism-aided navigation based on matching algorithm for underwater vehicles[M]//Mechanical Engineering and Technology.Springer Berlin Heidelberg,2012:59-65.

[7]张涛，徐晓苏，李佩娟.混沌优化水下地形匹配算法研究[J].中国惯性技术学报，2009，17(2)：156-158，164.ZHANG Tao,XU Xiao-su,LI Pei-juan.Underwater terrain matching algorithm based on chaotic optimization[J].Journal of Chinese Inertial Technology,2009,17(2):156-158,164.

[8]罗诗途，任治新.基于仿射模型变换的地磁匹配导航算法[J].中国惯性技术学报，2010，18(4)：462-465.LUO Shi-tu,REN Zhi-xin.Geomagnetic matching algorithms based on affine model[J].Journal of Chinese Inertial Technology,2010,18(4):426-465.

[9]刘颖，吴美平，谢红卫.地磁匹配算法研究框架和组合匹配策略[J].国防科技大学学报，2010，32(06)：172-176.LIU Ying,WU Mei-ping,XIE Hong-wei.A framework for magnetic matching study and an integrated matching method[J].Journal of National University of Defense Technology,2010,32(6):172-176.

[10]Jiang Z,Chen Z,Zheng B,et al.A kind of 3D reconstructtion method based on affine transformation[C]//2009 3rd International Conference on Genetic and Evolu- tionary Computing.2009:358-361.

[11]Wang K,Li Y,Rizos C.Research on terrain suitability of iterative closest contour point algorithm for underwater navigation[C]//Proceedings of the 22nd International Technical Meeting of the Satellite Division of The Institute of Navigation.2009:866-870.

ICCP algorithm for underwater terrain matching navigation based on affine correction

XU Xiao-su,WU Jian-fei,XU Sheng-bao,WANG Li-hui,LI Pei-juan
(1.Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology,Ministry of Education,Southeast University,Nanjing 210096,China;2.School of Instrument Science &Engineering,Southeast University,Nanjing 210096,China)

ICCP is the most important matching algorithm used in underwater integrated navigation system.Traditional ICCP algorithm can only do rigid transformation (rotation and translation) for the indicated track of underwater vehicle’s INS.In order to overcome this problem and improve the accuracy of track matching,the error characteristics of INS are analyzed,and the error model of INS is established.Then a new ICCP algorithm based on affine correction is put forward.The track indicated by INS is transformed according to the ICCP algorithm.The affine parameters are solved with the least squares method.The ICCP matching track is modified by affine transformation.The simulations show that the defects of traditional ICCP algorithm can be avoided by affine correction,and the matching result is better than those of traditional ICCP algorithm,in which the matching error is less than 50% of digital map grid spacing.Meanwhile,the additional time consumed in affine correction is just about one thousandth of that in traditional ICCP algorithm.

underwater vehicle;inertial navigation;terrain matching;ICCP;affine transformation

U666.1

A

1005-6734(2014)03-0362-06

10.13695/j.cnki.12-1222/o3.2014.03.016

2014-01-13；

2014-04-15

国家自然科学基金项目（51175082，61203192）

徐晓苏（1961—），男，博士生导师，从事测控技术与导航定位领域的研究。E-mail：xxs@seu.edu.cn