一种仿射变换两步式TERCOM 重力匹配算法

吴睿盈, 王 燕, 孙 勇

（北京航天控制仪器研究所, 北京 100039）

0 引言

水下航行器长航时、 高精度的自主导航不仅是执行水下任务、 发展海洋经济及维护领海安全的前提基础, 也是建设 “海洋强国” 的重要保障［1-2］。 惯 性 导 航 系 统（Inertial Navigation System,INS）因其同时具备自主性、 隐蔽性和抗干扰性等诸多优点被广泛应用于各类自主导航平台［3］。 但是因为惯性器件的误差随时间累积, 现阶段水下航行器自主导航多采用以惯性导航为主, 以基于重力、地形、 地磁等地球物理特征的匹配导航为辅的组合导航方式。 其中, 重力匹配导航获取重力信息时不会对外辐射能量, 可以保证航行器的隐蔽性,且海洋重力场特征较为稳定, 匹配定位精度高,利用重力匹配辅助惯性导航对于提高水下航行器的自主导航能力具有重要意义和军事价值［4-5］。

惯性/重力匹配导航系统由惯性导航系统、 海洋重力场基准图、 重力测量系统和重力匹配导航系统四部分构成, 由航行器自身搭载的重力仪实时测量所处位置的重力场特征信息, 再将实时测量值与预先构建好的海洋重力场背景图进行匹配,通过重力匹配算法估计航行器的位置信息。 惯性/重力匹配导航系统基本工作原理如图1 所示。 重力匹配算法是惯性/重力匹配导航的核心技术, 由目前发展更为成熟的地形匹配算法发展而来, 主要分为单点迭代和序列迭代算法。 单点迭代算法以桑迪亚惯性地形辅助导航（Sandia Inertial Terrain Aided Navigation, SITAN）算法为代表, SITAN 算法借助卡尔曼滤波技术可以进行实时重力匹配。 序列迭代算法主要包括地形轮廓匹配（Terrain Contour Matching, TERCOM）算法和最近等值线迭代（Iterated Closest Contour Point, ICCP）算法, 对达到一定长度的观测采样序列进行匹配。 相比于TERCOM算法, ICCP 算法对初始误差和重力观测误差较为敏感, 对重力背景图的分辨率依赖性更强［6-7］。TERCOM 算法是目前应用最广泛的算法, 但只利用平移变换修正导航误差, 精度和稳定性较差［8］。

图1 惯性/重力匹配导航系统基本工作原理Fig.1 Basic working principle of INS/gravity matching navigation system

为了提高TERCOM 算法的精度, 针对TERCOM 算法仅依靠平移不能完全修正INS 产生误差的问题, Han 等［9］提出了一种基于最短 路 径的TERCOM 算法, 在进行相关分析时, 额外引入了线段的角度和距离作为最短路径权重计算的参数,减小了较大的航向误差。 赵建虎等［10］提出了一种基于自适应旋转变换的TERCOM 算法, 在TERCOM 算法的基础上增加了旋转变换并能够自适应地确定最佳旋转角。 魏二虎等［11］提出了一种带有旋转和尺度变换功能的改进TERCOM 算法, 提高了定位精度。 蔡体菁等［12］提出了一种两步式改进TERCOM 算法, 首先使用TERCOM 算法进行刚性变换粗匹配, 然后进行精匹配, 在粗匹配轨迹附近筛选重力等值点组成精匹配重力序列, 利用贪心算法解算出最佳匹配轨迹, 既提高了匹配精度也保证了匹配效率。

为了更好解决TERCOM 算法仅依靠平移修正位置误差、 精度和稳定性较低的缺陷, 本文设计了一种基于仿射变换的两步式TERCOM 算法, 意图在提高匹配精度和稳定性的同时缩短匹配时间。考虑对待匹配轨迹实施仿射变换寻找最佳匹配轨迹, 给出了仿射变换模型及仿射参数的确定方法,并介绍了实施改进算法的具体流程。 通过仿真实验验证, 改进的TERCOM 算法精度和稳定性明显优于传统TERCOM 算法。

1 TERCOM 算法的工作原理及分析

由于地球重力场具有连续、 随机、 等值（多个地理点的重力异常值相等）的特性, 基于单个重力异常观测量无法唯一确定水下航行器的位置, 因而要求重力异常采样序列达到一定长度再进行匹配。 TERCOM 算法的实施过程是在一定搜索范围内按等间隔平移惯导指示轨迹, 并按照平移后的位置在重力基准图中提取对应的重力异常, 生成待匹配轨迹的重力序列, 遍历搜索范围内的格网,得到多组待匹配轨迹的重力序列, 通过相关分析算法计算选取满足最优化准则的待匹配重力序列,将该最优序列的坐标轨迹当做载体真实轨迹, 惯导指示轨迹与该最优序列的平移量即为惯性导航的修正量［13-15］, 如图2 所示。 搜索范围可设为正方形, 由于惯导误差有累积特性, 搜索范围边长由重力异常观测序列中最后一点的漂移范围a确定,惯导指示轨迹在（INSx±a,INSy±a）范围内平移,形成一个以惯导指示轨迹为中心轴边长为2a的菱形柱体, 如图3 所示。 常用的相关分析算法有三种, 包括交叉相关（Cross Correlation, COR） 算法、平均绝对差相关（Mean Absolute Deviation, MAD）算法以及平均平方差相关（Mean Square Deviation,MSD）算法, 假定在时间序列ti、ti+1、 …、ti+N时刻共采样得到N个重力异常观测量, 记为, 平移后得到的第j条待匹配轨迹的重力序列为, 则三种算法的计算模型如下所示:

图2 TERCOM 算法的工作原理示意图Fig.2 Working principle of TERCOM algorithm

图3 搜索范围示意图Fig.3 Diagram of search scope

交叉相关算法

平均绝对差相关算法

平均平方差相关算法

最优化的匹配准则就是选取满足JCOR取最大值或者JMAD、JMSD取最小值的待匹配重力序列, 文中的重力匹配使用的相关分析算法均为MSD 算法。

传统TERCOM 算法通过平移惯导轨迹在重力基准图中搜索最佳匹配序列, 原理简单, 对初始误差不敏感, 应用最为广泛。 但实际应用中, 惯导在不同方向上具有不同的误差特性, 即使是同一方向的误差, 因为各种因素的影响, 也在不断的变化［11］, 仅依靠平移修正轨迹并不能完全消除位置误差, 必然导致匹配结果的精度与稳定性均比较差［10］。

2 基于自适应仿射修正的改进TERCOM 算法

根据惯性器件误差特性, 陀螺和加速度计的常值漂移、 随机游走以及零偏会激励导航定位参数产生舒勒和地球两种周期振荡, 并产生速度、位置和航向上的常值误差分量, 且在经度上产生随时间增长积累的误差, 所以考虑惯性导航会产生的三种误差: 航向误差、 速度误差和位置误差。

航向误差使惯性导航指示轨迹与真实轨迹之间存在旋转变换关系, 速度误差使惯性导航指示轨迹与真实轨迹之间存在伸缩变换关系, 位置误差则对应着平移变换关系。 由上述分析可以看出,惯性导航指示轨迹与真实轨迹之间的关系可以利用仿射变换描述, 仿射关系如图4 所示, 曲线Ri为惯性导航指示轨迹, 曲线Rt为真实轨迹, 曲线R′i与Ri平行, 曲线R′i旋转θ角度得到R″i, 同Rt航向一致。

图4 仿射变换示意图Fig.4 Schematic diagram of affine transformation

建立惯性导航指示轨迹与真实轨迹之间的仿射变换模型

式（4）中,xt、yt分别为载体的真实经度和纬度坐标位置;k1、k2分别为经度和纬度方向上伸缩变换的伸缩系数;θ为旋转变换的旋转角度参数;x、y分别为惯性导航指示轨迹的经度和纬度坐标位置;x0、y0分别为重力匹配导航开始时惯性导航指示轨迹的经纬度坐标, 即重力匹配导航初始坐标; ΔX、 ΔY为平移变换参数, 由载体进行重力匹配导航前累积的初始位置误差δx、δy和重力匹配导航过程中累计的惯导位置误差Δx、 Δy组成

伸缩系数、 旋转系数的确定方式如图5 所示,P1为重力匹配导航第一个匹配点的惯性指示坐标,Pn为重力匹配导航最后一个匹配点的惯性指示坐标。

图5 仿射变换参数确定方式示意图Fig.5 Schematic diagram of affine transformation parameters determination method

图5 中标识出的角度存在以下关系

式（6）中, Δφ、 Δλ为行驶到Pn时累积的纬度误差和经度误差, 一般通过惯性器件的参数进行估算, 数值与式（5）中的ΔX、 ΔY相同, 旋转系数。 将Δφ、 Δλ造成的位置误差投影到P1Pn延长线上得到ΔL, 即

由ΔL可以确定伸缩系数k1、k2的范围, 一般来说, 惯性器件的漂移会造成匹配点之间的间距变大, 所以伸缩系数的上限设为1。

本文提出的改进TERCOM 算法分为粗匹配和精匹配两部分: 首先估计载体在经度和纬度方向上的位置漂移, 利用传统TERCOM 算法在漂移范围内寻找到一条粗匹配轨迹, 精匹配在粗匹配轨迹附近的小范围区域内进行, 即缩小平移变换参数取值范围, 以此减小精匹配的搜索范围, 减少精匹配时间; 精匹配过程是一个循环迭代的过程, 循环包括自适应仿射参数修正及重力匹配两个步骤, 每次循环的结果相互比较、 印证, 以此逐步缩小重力序列搜索范围, 得到准确且稳定匹配结果的同时, 减少多次匹配造成的计算量增加。 算法流程如图6 所示, 具体实施步骤如下:

图6 改进算法流程图Fig.6 Flowchart of improved algorithm

1）粗匹配: 根据式（5）计算位置误差ΔX、 ΔY,根据位置误差设置重力序列搜索范围, 依据MSD匹配准则, 利用传统TERCOM 算法在搜索范围内寻找到一条粗匹配轨迹L1。

2）仿射参数初始设置: 精匹配以粗匹配轨迹L1为中心, 按照式（4）给出的仿射模型进行仿射变换, 在搜索范围内按照搜索间隔提取待匹配重力序列: 利用式（6）计算旋转系数, 搜索间隔设为; 利用式（8）计算伸缩系数k1,k2∈, 搜索间隔设为, 因为伸缩系数范围通常很小, 所以搜索间隔设置大一些, 对于匹配精度影响不大; 由于经度误差具有累计特性, 数值较大, 精匹配时, 经度方向平移变换参数的初始范围设为, 搜索间隔为, 纬度方向平移变换参数的初始范围设为, 搜索间隔为, 可取ΔXJ=2ΔYJ。

3）重力匹配: 依据MSD 匹配准则, 对步骤2提取出的待匹配重力序列进行计算, 得到一条最优轨迹作为精匹配轨迹L2。 计算L1和L2的平均距离误差, 公式如下

式（9）中,ed为平均距离误差,di为L1中第i点与L2中对应点之间的距离,n为重力序列中采样点总数。 同时计算L1和L2的平均经度误差edx和平均纬度误差edy, 单位均为m。

4）自适应仿射参数修正: 当ed＞300 时, 说明粗匹配结果不稳定, 需重复进行精匹配过程直到结果稳定。 重复精匹配过程中, 需重新确定仿射变换系数:, 搜索间隔设为,θ′为上一次匹配所得轨迹对应的旋转系数; 伸缩系数计算公式更改为k1∈［k′1-kr,k′1+kr］,k2∈［k′2-kr,k′2+kr］,k′1、k′2为上一次匹配所得轨迹的伸缩系数,kr为设定的搜索范围, 搜索间隔为, 每次循环kr都减小为上次循环的一半; 同时, 若edx＜500, 经度方向平移变换参数ΔXJ减小为原先的一半, 若edy＜300, 纬度方向平移变换参数ΔYJ减小为原先的一半, 搜索间隔均不改变。

5）再次重力匹配: 将上一次循环得到的精匹配轨迹L2重新记为L1, 再次进行步骤3, 得到一条新的精匹配轨迹L2, 并计算L1和L2的平均距离误差ed、 平均经度误差edx和平均纬度误差edy, 若ed＞300, 则转到步骤4。

6）迭代终止条件: 重复进行重力匹配及仿射参数修正过程, 直到ed≤300, 循环结束, 最后一次匹配结果即为算法最终匹配结果。

3 仿真实验分析

3.1 仿真条件

重力基准图由圣地亚哥大学发布的卫星测高全球重力异常数据制作, 实验所选取的导航区域为: 北纬4.7° ～6.4°, 东经120° ～120.5°, 重力异常单位为mGal, 分辨率为1′ ×1′, 重力基准图如图7 所示。

图7 仿真实验用重力基准图Fig.7 Gravity reference diagram for simulation experiment

仿真所用的真实轨迹由惯导轨迹生成器解算,惯导轨迹生成器的作用为设置陀螺仪和加速度计的常值零偏、 随机游走值和采样频率以及载体的航向角和航行速度, 输出陀螺仪和加速度计的对应角增量和速度增量, 再利用惯性导航原理计算仿真轨迹。 本实验设定陀螺仪的常值零偏为0.01（°） /h,随机游走为0.005（°） /h1/2; 加速度计零偏为5μg, 随机游走为2.5μg/Hz1/2。 设载体行驶3h 后, 开始进行重力匹配导航。 仿真得到的惯导轨迹与真实轨迹误差如图8 所示, 表1 给出了惯导轨迹误差的具体数值。

图8 惯导轨迹误差图Fig.8 Diagram of inertial navigation trajectory error

表1 惯导轨迹误差数据Table 1 Data of inertial navigation trajectory error

本文在实验区选择了两条航线: 一条载体初始位置为（5.2°, 120.2°）, 其航线位于重力信息较为丰富的适配区; 另一条载体初始位置为（5.6°,120.1°）, 其航线位于重力信息稀疏的非适配区。每条航线沿经线和纬线的航速均为5nmile/h（约2.6m/s）, 惯导轨迹初始航向误差约为3°, 重力仪观测值的仿真采用基准图重力异常数据加入测量噪声的方法。 现今重力仪的动态精度已经达到1mGal 量级, 因此测量噪声取方差为1mGal 的白噪声。 重力匹配时长为1h, 6min 采样1 次, 每次匹配共取10 个采样点。 根据所用惯导器件误差特性,估算粗匹配开始时纬度漂移范围为1.4nmile（约2593m）, 经度漂移范围为3nmile（约5679m）。

3.2 适配区实验结果

在（5.2°, 120.2°）位置附近区域, 重力信息较为丰富, 重力匹配导航的效果较好, 传统TERCOM 算法也基本可以保证全程定位精度在千米以内, 平均定位精度小于800m。 由图9（a）和图9（b）对比可知, 改进TERCOM 算法的匹配效果明显优于传统算法; 由图9（c）和图9（d）对比可知, 改进算法X方向、Y方向和位置的平均误差均降低到了传统算法的一半, 其中位置平均误差为355.5m。表2 对比了传统TERCOM 算法和改进TERCOM 算法的匹配误差, 可以看出改进算法的每一项均优于传统算法。

表2 适配区两种算法匹配位置误差统计Table 2 Matching position error statistics of two algorithms in adaptation area

图9 适配区匹配效果Fig.9 Matching results in adaptation area

3.3 非适配区实验结果

非适配区利用TERCOM 算法进行粗匹配时存在匹配成功但匹配效果不佳和匹配失败两种情况。

（1）粗匹配成功

在（5.6°, 120.1°）位置处, 重力信息稀疏, 匹配效果不佳。 由图10（a）可知, 传统重力匹配轨迹和真实轨迹相差很远; 由图10（c）可知, 传统算法的位置误差全部在1500m 以上, 误差最大点处甚至达到了3000m; 而由图10（b）可知, 改进算法的匹配轨迹则明显更加贴近真实轨迹; 由图10（d）可知, 改进算法的定位精度均保持在千米以内, 计算得到匹配轨迹的平均位置精度小于800m,X方向、Y方向以及总位置的定位误差约为传统算法的1/4, 改进算法在传统算法匹配不佳的条件下依然可以取得良好的匹配效果。 表3 列出了两种算法的匹配结果数据。

图10 非适配区粗匹配成功时的匹配效果Fig.10 Matching results when coarse matching succeeds in non-adaptive area

表3 非适配区粗匹配成功时两种算法匹配位置误差统计Table 3 Matching position error statistics of two algorithms when coarse matching succeeds in non-adaptive area

（2）粗匹配失败

在（5.6°, 120.1°） 位置处重复进行匹配实验,传统TERCOM 算法会有小概率出现误匹配情况。如图11（a）所示, 匹配轨迹偏离真实轨迹的距离很远, 位置误差甚至远超纯惯性导航的误差。 因为改进算法的粗匹配过程使用传统TERCOM 算法,且精匹配过程是在粗匹配的基础上进行的, 所以粗匹配的结果对于改进算法的匹配效果影响很大。由图11（d）可知, 改进算法的匹配效果不佳, 多个匹配点的位置误差超过了2000m。 但由图11（a）和图11（b）对比可知, 改进算法的匹配轨迹比惯性指示轨迹接近真实轨迹, 大大减少了匹配位置误差,很大程度上提升了重力匹配算法的可靠性。 表4 列出了两种算法的匹配结果数据。 虽然改进算法的匹配精度并不高, 平均误差约在2000m 左右, 但粗匹配和精匹配后的匹配结果可以相互印证, 如果相差很大, 可以起到警示匹配有误或不稳定的作用。 精匹配不断缩小收缩范围, 且设置迭代终止条件为与上一次精匹配结果位置误差小于阈值,就是为了追求较为稳定的结果。 相比于传统算法只对匹配轨迹进行一次搜索, 改进算法提高了其可靠性。

图11 非适配区粗匹配失败时的匹配效果Fig.11 Matching results when coarse matching fails in non-adaptive area

表4 非适配区粗匹配失败时两种算法匹配位置误差统计Table 4 Matching position error statistics of two algorithms when coarse matching fails in non-adaptive area

4 结论

传统TERCOM 算法仅依靠平移修正惯导轨迹,忽略了由于惯性器件的误差特性对轨迹造成的旋转变换和伸缩变换, 精度和稳定性较低。 针对此问题, 本文提出了一种基于仿射变换的两步式改进TERCOM 重力匹配算法。 通过仿射变换修正惯导轨迹, 提高了匹配精度和稳定性; 通过粗匹配、精匹配结合减少待匹配轨迹数量, 保证了匹配速度。 仿真实验表明: 在传统TERCOM 算法的重力适配区域, 改进的TERCOM 算法可以明显提高导航精度, 精度提升约50%; 在传统TERCOM 算法匹配效果并不十分理想时, 改进的TERCOM 算法仍具有一定适应性, 能够大幅提高导航精度, 使得重力匹配导航更具有稳定性; 在传统TERCOM算法出现误匹配、 失去导航能力时, 说明匹配结果不稳定, 会使得改进的TERCOM 算法的粗匹配、精匹配结果相差巨大, 起到对误匹配情况的警示作用。