基于混合稀疏ICCP 的联合抗差重力匹配定位方法

丁继成，杜翔宇，杨崇昭，赵 岩

（哈尔滨工程大学 智能科学与工程学院，哈尔滨 150001）

重力场信息具有良好的时空分布特征，利用重力场信息与重力场数据库进行匹配来获得导航定位信息具有自主性和隐蔽性，且定位误差不随时间而累积，经常用于辅助惯性导航系统（Inertial Navigation System,INS），来实现长时间航行并保持高精度导航定位。使用重力导航系统辅助惯导系统进行误差校正，既能获得高精度导航定位结果，同时也不会破坏导航系统的自主性[1,2]。

匹配算法是重力匹配导航系统的核心部分，经典的算法有地形轮廓匹配（Terrain Contour Matching,TERCOM）算法、最近等值线迭代（Iterative Closest Contour Point,ICCP）算法、桑地亚惯性地形辅助导航（Sandia Inertia Terrain-aided Navigation,SITAN）算法[3,4]。其中TERCOM 算法对于航向误差较为敏感，SITAN 算法对初始位置的精度要求较高。而ICCP 算法最初来源于点云配准的迭代最近点（Iterative Closest Point,ICP）算法，具有精度高，易于实现等特点。其主要思想是通过迭代最近等值线点来实现测量与基准图之间的匹配。该算法先利用惯性导航系统的指示位置，寻找重力图中相应的搜索域，并搜索其最近等值线点，然后以欧氏距离平方和最小的原则不断进行刚性变换，通过重复变换逐步靠近真实航迹，从而对惯导系统的误差进行修正[5]。

ICCP 算法成立是建立在两个假设基础上的：(1)INS 提供的指示航迹接近真实轨迹，且累积误差不大；(2) 重力传感器无测量误差。假设(1)可以通过不断使用ICCP 算法提供充足的外部信息来限制INS 误差的积累，而假设(2)在实际应用中无法实现，不论是重力基准图的预测量还是航行中重力传感器的实际测量，测量误差都不可避免且不可能完全一致，因此会造成最近等值线点不一定存在或者不能确定的情况，导致匹配算法失配或误配[6]。针对于实际测量中重力传感器不可避免存在噪声的问题，国内学者对此进行了深入的研究。从噪声统计特性方面考虑，肖晶等提出了一种基于概率数据关联的地磁ICCP 算法，对某一位置的地磁场进行多次测量，并根据概率数据关联的思想，融合测量值对应的结果，提升了匹配算法的定位精度和鲁棒性[7]；陈卓等提出了一种基于可信点集和轨迹的搜索方法来拒绝标量匹配的不可靠迭代点，提高了匹配的准确性[8]；许宁徽等提出一种基于多属性决策的评价指标，通过熵权法将收敛度指标与轨迹相关性指标融合得到综合评价指标，在噪声干扰的情况下也能很好地收敛[9]。从滤波角度考虑，乔楠等提出了一种基于多尺度特征值经验模态分解的地磁滤波方法，通过滤除干扰信号和重构信号来抑制测量噪声[10]。以上文献对抑制ICCP 算法测量噪声影响方面进行了不同程度的改进，但均未考虑测量误差对ICCP 算法匹配残差的影响。

针对重力传感器存在测量误差导致传统ICCP 算法误配或失配的问题，本文首先对ICCP 算法的匹配残差进行分析，仿真验证匹配残差在高斯噪声影响下不服从高斯分布，进而利用解决稀疏性问题的非凸lp范数优化模型，研究构造增广拉格朗日函数，基于lp范数对ICCP 算法进行改进，使用了加权的混合正则项，以更泛化地表示匹配残差，对最近等值线点进行重调。最后，构建联合匹配算法，使用混合稀疏ICCP（Hybrid Sparse ICCP,HS-ICCP）算法进行粗匹配，提供有效的匹配区域，将匹配后的位置作为新的INS 指示位置提供给经典ICCP 算法进行精匹配，有效地减少了重力传感器测量误差对匹配结果的影响。

1 ICCP 算法分析

1.1 ICCP 算法原理

ICCP 算法是ICP 算法以等值线为匹配单元的特例，它不需要对环境进行事先估计分析，只需要不断重复迭代计算，直至达到迭代的终止条件，将匹配结果作为对真实航迹的估计，从而修正INS 误差[11]。

ICCP 算法原理图如图1 所示，当航行器进入匹配区域后，利用重力传感器测量并记录航行轨迹点的重力异常值。根据INS 系统提供的位置信息，提取对应区域的重力异常数据库，再根据实测重力异常值gi(i=1,2...N)，在重力异常数据库中提取INS 指示航迹 点xi(i=1,2...N)附近的重力异常等值线ci(i=1,2...N)，并在等值线ci上搜索到与点xi距离最近的点yi(i=1,2...N)，然后以INS 指示点集和最近等值线点集间的欧式距离最小为目标函数，求解对应的旋转矩阵和平移矢量，并对INS 指示航迹点集应用求解的刚性变换，不断重复迭代这一过程，直至收敛[12]。这实际上是一种求解最优化问题的过程，最优化目标函数为：

图1 重力ICCP 算法原理图Fig.1 ICCP algorithm implementation

其中，X={xi,i=1,2…N}为INS 指示轨迹点集，Y={yi,i=1,2…N}为最近等值线点集为向量l2范数的平方，R为旋转矩阵，t为平移矢量。

在求解刚性变换的过程中，考虑到旋转和平移变换的不可交换性，需明确对X先做旋转变换R，再做平移变换t，即TX=RX+t。则刚性变换求解数学模型为：

可利用四元数法对旋转矩阵R和平移矢量t进行求解。首先分别以为原点建立新的坐标系，对INS 指示轨迹点xi和最近等值线点yi进行转换：

在求解旋转矩阵R时，可先令平移矢量t′=02×1，将式(6)平方展开可得：

利用四元数法即可求解旋转矩阵R和平移矢量t，并对INS 指示航迹点集应用求解的刚性变换，进行迭代求解。当迭代次数达到预设上限或旋转矩阵和平移矢量增量的绝对值都小于预设阈值时，停止迭代。基于上述原理可以看出，ICCP 算法遵循“初始对准—寻找对应关系（最近点）—求解使得目标函数最小的变换—应用变换”的循环过程[13]，在等值线上寻找全局最优解，该全局最优解就是最终的匹配结果。

1.2 ICCP 算法的局限性

由ICCP 算法原理可知，该算法是基于两个前提假设：（1）载体真实位置距离INS 指示位置不远；（2）重力传感器没有测量误差。该前提假设只是理想状态，实际中，前者可以通过外部校正信息进行修正，但后者在实际测量中不可实现，重力基准图的构建以及航行过程中的重力传感器的实际测量不可避免地存在误差，所以必须考虑测量误差对算法的影响。图2 为在不同噪声情况下的匹配轨迹，分别是：方案1：无测量噪声；方案2：加入方差为3 mGal 的高斯白噪声；方案3：加入方差为5 mGal 的高斯白噪声。图3 为三种方案对应的纬向、经向匹配误差。

图2 不同测量噪声情况下的匹配轨迹Fig.2 Simulation of matching trajectories

图3 不同测量噪声情况下的匹配误差Fig.3 Simulation of matching error

由图2 和图3 可以看出，ICCP 算法在无测量误差条件下对航行误差进行校正，可以得出比较高的匹配精度，且较为稳定，但其抗噪声能力较差；当噪声较大时，容易发生失配或误配的情况。故通常先使用其他匹配算法作为粗匹配，再使用ICCP 算法进行精匹配。

1.3 ICCP 误差分析

针对重力基准图的构建和实时重力异常的测量产生的误差对ICCP 算法的匹配精度会产生影响的问题[14]，对ICCP 算法的匹配残差zi的概率分布进行仿真分析。如图4 所示，重力传感器无测量误差时，匹配残差zi概率分布如图4(a)所示；给重力异常测量值增加方差为5 mGal 的高斯白噪声，匹配残差zi概率分布如图4(b)所示。

图4 匹配残差 zi概率分布图Fig.4 Matching residual ziprobability distribution

由图4(a)可以看出，在无重力测量误差条件下，ICCP 算法匹配残差数据直方图与高斯概率密度拟合较好，此时匹配残差zi服从高斯分布；由图4(b)可以看出，在重力异常测量误差方差为5 mGal 时，ICCP 算法匹配残差数据直方图不符合高斯概率密度分布钟型、对称、均匀变动的特点，不能较好地拟合，此时匹配残差zi不服从高斯分布。

此外，由于惯导系统本身存在器件误差，惯导误差会随着航行时间的增加而累积，如果不对其进行抑制，ICCP 算法可能会因为初始误差较大，而无法找到恰当的最近等值线点，出现误匹配或匹配失败的情况。

2 基于 lp范数的HS-ICCP 算法研究

由于ICCP 算法的实质是一个数学优化问题，其目标是通过最小化INS 指示轨迹点集和最近等值线点集之间的欧氏距离，从而实现高精度匹配。该算法可以看作是一个基于l2范数的最小二乘优化问题。最小二乘法是凸优化的一种特殊形式，但其要求匹配残差服从高斯分布，但在实际情况下，重力异常测量值存在较大误差，导致匹配残差不服从高斯分布。因此，考虑重力异常测量误差会导致匹配点集间的匹配残差不服从高斯分布的问题，本文应用非凸优化模型来解决这个问题，提出了HS-ICCP 算法，以便更好地处理匹配残差非高斯分布的情况，提高匹配的准确性。HS-ICCP 算法方案处理流程设计如图5 所示。

图5 HS-ICCP 算法流程图Fig.5 Flow chart of HS-ICCP algorithm

2.1 lp范数

稀疏性问题[15]在变量选择、图像处理与计算机视觉等领域中一直受到广泛关注，压缩感知理论为解决稀疏性问题提供了理论支撑，许多稀疏性或压缩感知问题可以转化为非凸优化问题，如式(9)所示。

lp范数最优化问题是一个非凸、非光滑、非连续的优化问题，可以将lp范数近似逼近于l0范数：

图6 不同p 取值的范数图Fig.6 Simulation of different norms

传统ICCP 算法采用l2范数来最小化INS 指示点集和等值线点集之间的欧氏距离，如图6 所示，在l2范数的影响下，较大的噪声会由于距离平方的影响而对总误差贡献较大，导致匹配过程可能会被这些异常值所左右；lp范数函数图像在原点附近急剧变尖，意味着在优化过程中，较小的偏差对总范数的贡献减少，而较大的偏差导致贡献加大，有利于剔除掉噪声点、野值点，实现抗差目的。

2.2 混合稀疏ICCP 函数

理论上重力ICCP 匹配算法是建立在重力传感器无测量误差的基础上，以欧式距离作为最优目标函数进行求解。但实际测量中，重力基准图的构建和重力传感器不可避免地存在误差，且匹配残差不服从高斯分布，故使用增广拉格朗日函数建立非凸优化模型，用稀疏诱导lp范数代替l2范数[17]，在保证lp范数的物理意义，同时不影响运算结果的情况下为简化运算，对lp范数进行放大，将lp范数内部的p次幂运算放大为欧氏距离的2 次幂运算，即令定义稀疏ICCP 如下：

其中，xi为INS 指示轨迹点，yi为最近等值线点，R为旋转矩阵，t为平移矢量。求解式(12)可将其分解为查找等值线点和求解刚性变换两个步骤：

对于此公式仍可以使用经典ICCP 算法的最近等值线点搜索方法进行求解。

对于式(14)的求解，则采用增广拉格朗日算法。引入中间变量Z={zi∊R2,i=1…n}，其中，匹配残差zi=Rxi+t-yi，并将其转换成如下形式：

引入增广拉格朗日函数，将式(16)转化为无约束优化模型：

式(13)和式(14)定义了一个优化问题，目的是将匹配残差zi分为非异常值和一小部分异常值其中非异常值来自于正确的对应点，异常值来自于错误或者噪声造成的对应点，故可以通过分类目标找到一个稀疏向量z来实现分类目标的目的。

由于式(16)受到单一的p阶次正则项的限制，泛化性较差。所以在式(16)的基础上，使用加权的混合正则项，可以更泛化地表示匹配残差zi，定义混合稀疏ICCP（HS-ICCP）函数如下：

其中,θi∊ [0,1]为混合正则项的平衡权重。

通过增广拉格朗日方法，将式(18)转化为如下形式：

式(19)中含有四个未知变量，可通过交替方向乘子法（Alternating Direction Method of Multipliers,ADMM）将问题分解为三个步骤进行求解：

Step1：求解匹配残差z。

Step2：利用Step1 得到的匹配残差z，求解旋转矩阵和平移矢量。

Step3：更新惩罚系数μ与拉格朗日乘数λi。

该步骤是对惩罚系数μ进行更新。其中，t指的是第t次迭代，目的是为了增大惩罚系数μ，使Step2 中的(zi+yi-λi/μ)变化尽可能小，防止惩罚系数过小导致最佳位置错过，故取μ=0.5，a=1.1。

2.3 平衡权重的确定

为了高效地求解式(20)，先验地认为INS 指示点集经匹配算法计算后，会收敛于正确的匹配位置。对于欧氏距离较大的最近等值线点，增加平衡权重θi会增加其被分类为噪声点的概率，从而更好地利用p阶正则项的稀疏惩罚能力。而对于欧氏距离较小的最近等值线点，降低平衡权重θi会增加其被分类为正确点的可能性，可以更好地利用l2范数正则项的最小二乘逼近能力。

Sigmoid 是二元分类过程中经常使用的函数，使用Sigmoid 函数[18]构建平衡权重θi和每一个INS 采样点与对应最近等值线点间的欧式距离di的关联函数：

其中，Median 为INS 采样点集与其对应最近等值线点集对之间欧氏距离的中位数。

为了弱化离群点，充分进行惩罚，HS-ICCP 的稀疏性应随着迭代次数的增加而增强，即在匹配过程中，Median 只在外循环搜索最近等值线点结束后计算作为初始值，并在每次外循环迭代过程中逐渐减小为上次迭代的ν倍。随着Median 的减小，平衡权重θi将逐渐趋近于1。

图7 为不同Median 对应的Sigmoid 函数图像，由图可直观看出Median 不断减小为上次迭代的ν倍的变化趋势，但变化幅度不应过大，若变化幅度过大则会出现过渡较快的情况，就失去了平衡权重的意义，故本文将v 取0.9，可以达到均匀过渡的效果，能更好地对等值线点进行分类。

图7 不同Median 对应的Sigmoid 函数Fig.7 Sigmoid function with different median

2.4 标量化误差函数

由于式(20)中的匹配残差zi为向量形式，不易进行求解，故我们使用MM[19（]Majorization-Minimization）算法处理该优化问题。根据式(20)建立其误差方程：

这里z=(zx,zy)，h=(hx,hy)均为矢量，将z=(zx,zy)用h=(hx,hy)线性表示为z=ah。可以将求解矢量函数(20)中z的最优解简化为求解标量函数(26)中a的最优解：

对标量函数求标量参数a的偏导数可得：

对g(ah) 求二阶偏导数，可得：

由于g′(ah) ＞ 0，可知g(ah) 为凸函数，存在全局最小极值。只有当大于g(ah) 的极小值时，ah才有解，否则令a=0。

在此基础上，HS-ICCP 算法可以通过ADMM 双循环的优化架构，使用外循环寻找最近等值线点，计算其对应的欧氏距离，并动态调整平衡权重，利用内循环不断求解其刚性变换矩阵T=(R,t)。最终弱化离群点以及噪声点的影响，并实现匹配。

3 重力联合抗差匹配算法流程

通过上述算法分析可知，HS-ICCP 算法使用lp范数作为距离度量函数，通过ADMM双循环优化架构，对离群点、噪声点进行惩罚，起到了削弱重力传感器测量误差影响的作用，但匹配精度会有所下降。而在理想条件下，经典ICCP 算法具有稳定性好、精度高的特点。基于以上原因，本文提出一种重力联合抗差匹配算法，考虑重力传感器测量误差，先通过HS-ICCP算法粗略匹配，提供有效的匹配区域，将匹配后的大致位置作为新的INS 指示航迹输入经典ICCP 算法中进行精确匹配。

重力联合抗差匹配的具体流程如图8 所示。

图8 重力联合抗差匹配流程图Fig.8 Flow chart of gravity combined robust matching

综上，重力联合抗差匹配导航算法的具体步骤设计如下：

（1）将INS 指示轨迹的累计误差作为阈值，选取重力匹配区域，建立重力异常基准图，以保证该区域能涵盖匹配航迹；

（2）基于INS 指示航迹，使用HS-ICCP 算法进行粗匹配，对含有噪声的重力异常数据进行处理；

（3）将粗匹配的结果作为精匹配的初始INS 指示航迹，使用经典ICCP 算法进行精匹配；

（4）将精匹配的结果作为新的初始INS 指示航迹重复进行精匹配，直至达到收敛条件；

（5）将收敛后的匹配结果输出，作为校正航迹。

4 实验验证与分析

4.1 仿真实验设计

为了验证匹配算法的有效性，选取黄海内一片区域进行测试，通过仿真得到载体的航行路线。表1 给出了仿真线路的起始点坐标、初始误差、惯导系统陀螺仪、加速度计误差、航向角、航行速度等参数，重力异常基准图选用ICGEM 网站提供的SCG-UGM-2模型[21]，分辨率为1′ × 1′，阶数为2190 阶，精度为1～5mGal。

表1 仿真航迹及其参数Tab.1 Simulated path and their parameters

4.2 仿真实验

采用表1 中的参数，参考文献[14][16-18]并经测试验证p取值0.4～0.6 效果较好，权衡算法性能和鲁棒性，本文取p=0.5，ν=0.9，INS 指示航迹采样间隔为10 min，匹配序列长度为10，使用ICCP 算法和联合匹配算法对试验线路进行测试。其中线路所在区域重力基准图的纬度范围为31.5°N～32.5°N，经度范围为121.3°E～122.3°E，重力异常变化范围为-23.37 mGal～14.71 mGal，基准图分辨率为1′ × 1′。

实验一：在INS 运行正常，重力传感器实时测量无误差的情况下，两种算法得到的匹配轨迹如图9 所示。其匹配结果以及惯导航迹的纬向、经向误差如图10所示。

图9 实验一匹配轨迹Fig.9 Matching trajectory of experiment one

图10 实验一匹配误差Fig.10 Matching error of experiment one

表2 为重力传感器无测量误差时，INS 以及两种算法的误差参数对比，其中位置误差表示匹配位置与真实位置之间的直线距离误差。从表2 中的数据以及图9 中等值线分布可以看出，在前10 个采样点进行匹配时，重力异常变化范围在 -10～0 mGal，且变化较为缓慢，致使两种算法匹配精度相当；在后10 个采样点进行匹配时，重力异常变化范围在 -20～5 mGal，且相邻点间变化明显，更有利于重调野值点进行匹配，故联合匹配算法匹配精度略好于ICCP 算法。从各项数值分析来看，联合抗差匹配算法在无重力测量误差的情况下可以正常使用，且导航精度要高于ICCP 算法。

表2 实验一匹配误差参数对比（单位：n mile）Tab.2 Comparison of matching error parameters in experiment one (Unit: n mile)

为了更好地与重力传感器无测量误差时的仿真结果进行对比，给重力传感器持续加入不同均值的噪声进行仿真实验。

实验二：噪声方差为3 mGal 时，两种算法得到的匹配轨迹如图11 所示。其匹配结果以及惯导航迹的纬向、经向误差如图12 所示。

图11 实验二匹配轨迹Fig.11 Matching trajectory of experiment two

图12 实验二匹配误差Fig.12 Matching error of experiment two

表3 为重力传感器噪声方差为3 mGal，INS 以及两种算法的误差参数对比。结合图表信息分析，联合匹配算法相较ICCP 算法更为稳定，纬向误差、经向误差和位置误差最大值均控制在1 n mile 以内，误差的平均值最大为0.562 n mile。采用联合匹配算法计算得到的匹配导航结果精度提升较ICCP 算法提升60%以上。

表3 实验二匹配误差参数对比（单位：n mile）Tab.3 Comparison of matching error parameters in experiment two (Unit: n mile)

实验三：噪声方差为5 mGal 时，两种算法得到的匹配轨迹如图13 所示。其匹配结果以及惯导航迹的纬向、经向误差如图14 所示。

图13 实验三匹配轨迹Fig.13 Matching trajectory of experiment three

图14 实验三匹配误差Fig.14 Matching error of experiment three

表4 为重力传感器噪声方差为5 mGal 时，INS 以及两种算法的误差参数对比。此时重力传感器的测量误差较大，从轨迹图上可以看出，ICCP 算法已经产生较大幅度的偏离，而联合匹配算法仍可以较好地定位到真实航迹，匹配误差平均值和RMS 都控制在1 n mile 以内，且误差的平均值最大为0.571 n mile。采用联合匹配算法计算得到的匹配导航结果精度较ICCP 算法提升75%以上。

对比不同测量误差下，各算法的位置RMS 值，如图15 所示：

图15 各算法位置RMS 直方图Fig.15 Radial RMS histogram of each algorithm

从图15 可以看出，随着测量噪声的不断增加，ICCP 算法的位置RMS 值也随之增加，而联合匹配算法的位置RMS 值维持在稳定范围内，且波动不大。由此可验证本文提出的重力联合抗差匹配算法能够有效抑制重力传感器测量误差对匹配结果的影响，降低匹配误差。

5 结论

针对重力测量误差导致匹配残差不服从高斯分布，使得经典ICCP 算法匹配精度下降甚至失效的问题，提出了一种基于混合稀疏ICCP 的重力联合抗差匹配导航方法。利用lp范数代替l2范数计算匹配残差重调野值点，组成以基于lp范数的混合稀疏ICCP 算法为粗匹配、ICCP 算法为精匹配的重力联合抗差匹配算法。实验结果表明，在无重力异常测量误差的情况下，重力联合抗差匹配算法的匹配精度略优于传统ICCP；在存在重力异常测量误差的情况下，重力联合抗差匹配算法对匹配精度的改善效果明显，其误差最大值稳定在1 n mile 以内，误差均值小于0.572 n mile，误差RMS 小于0.628 n mile，导航精度较传统ICCP 算法提升60%以上，可以达到抗重力异常测量误差的效果，提高了匹配算法的鲁棒性。