一种两步式相关极值重力匹配算法

蔡体菁，魏琪鹭，王新宇

(1. 东南大学 仪器科学与工程学院，江苏 南京 210096；2. 中国电子科技集团公司第二十六研究所，重庆 400060)

0 引言

重力辅助惯性导航系统(GAINS)使用重力异常或重力梯度值作为辅助信息来校正惯性导航系统(INS)随时间累积的定位误差，并补偿陀螺仪和加速度的漂移误差。GAINS具有自主性高，隐蔽性好，定位精度高，抗干扰能力强等优点。因此，在海洋和航空航天领域GAINS应用较广[1-2]。

重力匹配算法是GAINS的关键技术，关键在于结合重力和INS指示航迹两方面的信息得到精确的位置结果。通过比较重力测量值序列和重力异常图中序列的相似性，得到最优匹配结果。其中针对重力数据的处理方法主要有相关极值分析[3-4]、图像配准、扩展卡尔曼滤波[5-7]、粒子滤波[8-10]、地形轮廓匹配(TERCOM)[11-14]、迭代最近等值点(ICCP)[15-17]和直线段匹配[18]等。

本文提出了一种基于相关极值分析的两步式重力匹配算法。匹配算法的核心是基于相关极值算法的差分目标函数。首先通过均方差相关算法(MSD)快速粗匹配得到刚性变换的目标位置，并通过刚性变换将INS轨迹变换到真实轨迹附近；然后精确MSD匹配，通过筛选重力等值点和根据匹配约束条件搜索匹配线段，依据贪心算法得到最佳匹配轨迹，即为两步式匹配算法的最终结果。

1 基于MSD的差分相关目标函数模型

常用相关分析匹配算法包括交叉相关(COR)算法、平均绝对离差相关(MAD)算法和均方差相关(MSD)算法[4,19]。一般MSD算法性能最优，因此,重力观测序列与重力异常图提取序列间基于MSD构建目标函数J(λk,φk)为

(1)

运载体上测得的重力为地球引力与在惯性系下运载体随地球自转时产生离心力的合力，受运载体速度及其所在纬度的影响很大，即厄特弗斯效应。设vEi和vNi为i时刻载体的东向和北向速度，φi为载体当地纬度，R为地球半径，载体在该点的厄特弗斯改正计算式为

(2)

由于INS误差随时间增加而积累，运载体速度和纬度误差会随之增大，重力异常改正结果的误差也会随之增大，最终产生重力匹配的虚假定位。针对减小厄特弗斯效应的影响，对式(1)采用差分思想改进，以前、后2个连续的重力异常测量值之差为新的观测量[19-20]，可在消除厄特弗斯效应影响的同时，消除重力测量系统误差的影响，该差分匹配目标函数为

(3)

2 快速MSD匹配方法和刚性变换

2.1 快速MSD匹配方法

为了缩小搜索范围，各INS航迹点的重力等值点搜索区域由INS漂移范围(一般由瑞利分布确定)确定。为减少计算量，基于INS漂移范围，各点均选取相同且合适的固定搜索区域∑，使真实航迹点包含在∑中。为简化搜索过程和适用于网格化的重力异常图，假设∑是该INS指示位置所在网格上、下、左、右M个网格的范围内，共计S=(2M+1)2个网格区域。

快速MSD匹配方法以N个重力异常测量点为一组，构成一个匹配序列，从第N个点开始匹配。把匹配序列各时刻对应的INS指示位置的搜索区域∑中第i个网格点提取出一条匹配曲线Bi。依据式(3)可从S条匹配曲线中得到一个最优的匹配结果Bk。由于INS在短时间内具有较高的精度，可用Bk近似对应的真实轨迹，因此,选取Bk的终点作为一个匹配结果[21]。

由于一次匹配只比较有限的S个序列的目标函数，该方法可快速得到一个粗略的匹配结果，但该方法受限于INS误差累积不能过多，否则在一定时间积累后，INS轨迹和真实轨迹形状和方向差异过大，会使匹配出现很大误差。因此，从该方法得到的结果中提取合理的匹配结果，可作为接下来精确匹配的基准位置。

2.2 提取合适匹配点

快速MSD匹配结果的单点误差可以分为两种情况，一种是匹配结果误差小于一定值，如在实验中设定误差小于0.05°，另一种是匹配结果与真实结果相差远。考虑到INS在较短时间内相对漂移很小，相邻连续2个真实位置的间距和角度与相应惯性导航输出点距离和角度应该近似相等或相差很小[4]，引入约束条件：

(4)

(5)

式中:(xt,yt)、(xt+1,yt+1)分别为前、后连续2个相邻时刻的坐标，均在相应置信区间内；ε1、ε2为接近于0的误差；αINS为INS输出的位移角度；LINS为INS输出的位移距离。

由于快速MSD匹配方法是粗匹配，故将ε1和ε2约束放宽。当遇到不满足约束条件的匹配点时，将该点记为不满足点，否则为满足点，当用约束条件判断下一个点时，不使用它前一个不满足点，而使用最近的一个满足点进行判断。这种方法提取的满足点是快速MSD匹配的合理匹配结果。

2.3 刚性变换

利用刚性变换将INS指示航迹变换到真实航迹处，则INS指示航迹形状与真实航迹形状相同，故INS相对误差都保留。

(6)

图1 一般刚性变换

如图2所示，将快速MSD匹配方法提取的合格匹配点拟合得到线段l1，相对应的INS轨迹点也拟合得到线段l2，可计算出两条直线的位置和角度关系，由此确定刚性变化中整体平移(Δx,Δy)和旋转Δθ。然后利用式(6)的刚性变化，将INS轨迹变换到MSD轨迹指示的新待匹配位置，此时该刚性变换后的INS轨迹距离真实轨迹较近，成为新的待匹配轨迹，为下面的精确匹配方法缩小了搜索等值点的范围，减少了搜索时间。

图2 基于快速MSD方法的航迹刚性变换

3 精确匹配算法

当航行时间较短或初始位置误差较小时，搜索区域较小，可较快地通过等值点搜索到合适的匹配线段。而当航行时间较长或初始位置误差较大时，搜索区域过大，直接通过等值点搜索匹配线段搜索到的等值点过多，匹配耗费时间过长，影响算法效率和实时性。因此，将快速MSD结果作刚性变换基准得到的线段作为新的待匹配航迹，根据之前时刻的位置误差和INS漂移误差得到新的置信区间，然后重新搜索等值点和匹配线段。因为刚性变换产生的新航迹与原始INS指示航迹相同，所以使用新航迹作为等值点匹配方法的被匹配航迹并无影响。同时由于新置信区间比旧搜索区域小，搜索到的等值点数少很多，可避免大量误匹配等值点和误匹配等值线段，提高算法效率和实时性。

3.1 搜索等值点

3.2 搜索匹配线段

只对待匹配轨迹的第1个位置的搜索区域∑2遍历搜索，搜索重力等值点作为匹配线段搜索起点，其他位置通过搜索线段的方式搜索。由于第2节中刚性变换缩小了搜索范围，新搜索区域∑2远小于第2节的搜索区域∑。

搜索线段的方法是，在已有第L层线段起点Pf的情况下，利用待匹配轨迹对应的相邻点间距离关系，可得线段的终点Pb，又基于式(4)、(5)，将Pb附近一定范围内的点都视为线段的可能终点。

从多个可能终点中筛选出匹配线段终点的方法是，先检查各可能终点是否为重力等值点，然后计算线段前、后两点的重力差分误差平方值，作为该匹配线段的代价。该差分误差为式(3)差分误差平方和的一个组成部分。因此，通过下式设定的阈值判断是否保留该可能终点，舍弃差分误差过大的终点。

(7)

其中阈值δ2由重力传感器系统误差和厄特弗斯效应决定

3.3 连接等值线段和搜索最小路径

连接相邻层间的匹配线段，并基于贪心算法思想，通过记录和更新当前路径代价和，使每个节点仅记录从第一层节点到该节点最小路径中的该节点前一节点位置，舍弃其他非最小路径。

当线段连接完成后，在最后一层寻找最小的当前路径代价和，然后利用该节点存储的该路径前一节点，逐层寻找直到第一层，最后可得精确匹配结果。

4 两步式匹配方法效果

将已有重力图利用克里金插值方法细化为0.001°×0.001°的网格。运动轨迹和INS导航轨迹均由仿真生成。INS参数设定为加速度计逐次启动漂移,即5×10-5g(g=9.780 369 8 m/s2)，激光陀螺随机漂移为0.01 (°)/h，初始位置误差大于0.5°，初始角度误差为5°。运载体航行速度为15 m/s，每120 s选取1个重力采样点，采集20个采样点进行匹配，选择3条航向角ψ分别为0°、90°、-45°，经过重力适配区的真实航迹如图3～5所示。

图3 航向角为0°的匹配结果

图4 航向角为90°的匹配结果

图5 航向角为45°的匹配结果

快速MSD匹配算法匹配10个点耗时15～20 s，基于快速MSD匹配结果的刚性变换耗时0.01～0.02 s，且刚性变换结果的误差小于0.05°，故在此基础上，设定0.1°×0.1°等值点搜索范围，使用合适的搜索范围，可在地图适合匹配的情况下较快地得到精确匹配结果，匹配结果如图6～8和表1所示，可精确到500 m内，且比单纯快速MSD匹配结果更优。而如果地图不适合匹配，花费较多时间也能得到理想结果。

图6 航向角为0°的单点匹配误差

图7 航向角为90°的单点匹配误差

图8 航向角为-45°的单点匹配误差

匹配误差/m航向角为0°航向角为90°航向角为-45°快速MSD均值926.820 6813.245 9758.311 1MSD284.249 7313.232 9284.628 2两步式均值324.778 3372.047 6386.564 7MSD258.479 9158.810 5300.422 6

5 结论

1) 快速MSD法可快速得到匹配结果，但当匹配条件不理想时，如INS轨迹误差稍大或导航角度误差存在或航行区域粗糙度不足，快速MSD结果可能有很大的误差。

2) 精确匹配算法需选择一个合理搜索范围，搜索区域小时搜索时间短，如果搜索区域过大，搜索时间爆发式增长，还可能造成误匹配的情况。

3) 使用刚性变换将两种方法结合在一起，由于刚性变换法保留原始INS轨道的形状特征，不影响精确匹配的约束条件，匹配结果的精度无改变。在这种情况下，较小的搜索区域会减少大量误匹配的等值点和等值线段。两步式匹配法可在保证匹配精度的情况下用相对较小的时间完成大范围搜索，且能适应INS初始位置误差较大和导航角度误差存在的情况。

4) 实际应用中，只有航迹在一个粗糙度足够的重力适配区内时，才可以使用基于相关极值的重力匹配算法，否则有可能产生误匹配的情况。

5) 两步式算法也存在局限性，此方法更适用于大初始位置误差的情况，如果已知初始位置误差较小，直接使用精确匹配算法速度更快，更具实时性。