基于改进TERCOM的地形辅助导航算法

王 丹，刘利强，奔粤阳，戴平安，王健成

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

自主水下航行器（Autonomous Underwater Vehicle,AUV）是海洋资源开发和探测的重要工具。随着技术的发展对AUV 自主性、隐蔽性和长时性的要求越来越高，但惯性导航系统（Inertial Navigation System,INS）误差会随着时间增加而发散，影响AUV 隐蔽性。为降低INS 位置误差可通过地形辅助导航方法进行修正[1]，地形辅助导航系统具有很好的隐蔽性和抗干扰能力，能很好地帮助INS 解决位置发散问题，匹配算法是实现该技术的关键。地形轮廓匹配（Terrain Contour Matching,TERCOM）算法是地形辅助导航算法中最经典的算法之一，该算法是根据INS 提供航迹进行平移得到最终匹配序列，对INS 提供的航迹在角度上有较高要求，具有易实现、实用性强、隐蔽性高、性能可靠等优点，但实时性差且对水下航行器的机动性和INS 航向误差较敏感[2]。

许多学者对TERCOM 算法进行了研究，韩等人[3]采用最短路径算法改进TERCOM 算法，将基于受限空间顺序约束算法的误匹配判断方法与TERCOM 算法结合，提高了算法的实时性和定位精度。程等人[4]提出采用两阶段联合算法，提高TERCOM 的匹配精度，但匹配时间变长。为提高TERCOM 算法的性能，赵等人[5]和王等人[6]提出通过降低航向角度误差提高TERCOM 算法的匹配精度，但匹配速度较慢。袁等人[7]提出将TERCOM 与ICCP 算法联合使用，利用不同算法的性能差异实现互补，改善了TERCOM 算法的航向问题，提高了匹配精度，但由于两种算法都属于批处理算法，匹配时间变长。因粒子群算法具有结构简单、运行速度快等优点，许等人[8]提出采用粒子群优化（Particle Swarm Optimization,PSO）算法提高地磁匹配成功率。季等人[9]提出基于模拟退火优化PSO（Particle Swarm Optimization based on Simulated Annealing,SAPSO）算法提高匹配精度。

TERCOM 算法可与PSO 算法结合来提高地形匹配效果。本文提出在传统TERCOM 算法基础上增加旋转角度机制，首先依据INS 航迹进行两次旋转平移并采用TERCOM 算法进行粗匹配来降低航向误差，其次采用改进PSO 算法进行精匹配，从而得到最优匹配序列。匹配过程由粗到精，提高了传统TERCOM算法的匹配效果。

1 TERCOM 算法

TERCOM 算法是序列相关匹配方法之一，匹配时需采集多个点数，匹配周期较长。TERCOM 算法工作原理为：首先以INS 位置为中心，按一定搜索范围将数据格网化，其次遍历搜索范围内每个格网，得到多组与INS 航迹平行的序列，最后将每组序列中各点深度值与实测深度值进行相关分析[10]。通常采用均方差算法（Mean Square Difference,MSD）作为匹配方法的判定标准[11]：

式中，n为实测匹配序列长度；HA(i)为实测第i个采样点水深值(i= 1,2…n)；HS为数字地图指示水深值；(x,y)为惯导参考点位置；(τx,τy)为航行器在数字地图两个坐标轴方向的恒定距离分量。JMSD取值越小，匹配精度越高。

根据TERCOM 算法的原理分析，寻找最优匹配序列的核心是INS 提供的航迹位置和航向误差。如图1所示，当INS 没有航向偏差时TERCOM 算法的匹配航迹是与INS 航迹相平行的一组高程剖面。TERCOM算法在实际应用中隐含了一个条件，即假设匹配时INS 的航向偏差为0°或很小的角度（＜0.5°）。但在实际航行中INS 的航向偏差不可能为0°，当航向偏差较大时（＞0.5°），INS 航迹偏离了真实航迹，与INS 航迹平行的最优匹配序列也偏离了真实航迹。为更好地接近真实航迹，本文提出在传统TERCOM 算法基础上增加旋转角度机制，降低航向偏差，提高TERCOM算法的匹配效果。

图1 TERCOM 算法匹配示意图Fig.1 TERCOM algorithm matching diagram

2 改进粒子群算法

PSO 算法是一种模拟鸟群觅食过程的典型群智能优化算法，在PSO 算法中粒子位置是根据自身经验和整个群体经验进行更新。算法简单速度快，但易陷入局部最优。在D维解空间中，初始种群数量为N，用表示第i个粒子位置，表示第i个粒子在第j维空间的位置；表示粒子搜索个体最优解，表示第i个粒子在第j维空间的个体最优解；表示整个种群搜索的全局最优解，表示在第i维空间的全局最优解。标准粒子群速度和位置更新公式如下[12-14]：

式中，ω为惯性权重；为第i个粒子在t时刻的飞行速度，i= 1,2…N；t为当前迭代次数；为第i个粒子在t时刻的位置；和为学习因子；和为在[0,1]间随机数。

PSO 算法的速度由三部分组成，分别对应式(2)的惯性部分、认知部分和社会部分。认知和社会部分表示粒子种群中的信息交换,使粒子能够通过优良信息不断修正前进的速度和方向，进而使算法找到最优解。惯性部分可扩大搜索范围，提高全局搜索能力，从而使粒子跳出局部最优。其中惯性权重ω具备调节PSO 算法全局与局部寻优能力，在迭代初期使全局搜索能力变强，不断搜索未知区域，在迭代后期局部开发能力变强，在最优解范围内进行开发，所以参数ω随收敛过程逐渐减小[15-19]。惯性部分对算法在地形匹配的影响较大。

参数ω的选择直接影响粒子群算法的性能，本文基于ω参数的特点设计四种不同模型。模型1 将ω设为线性形式，如式(4)所示，ω在迭代初期具有最大值，全局搜索能力较强，但是在迭代后期ω变化较快，说明在迭代后期寻找局部最优能力较差；为弥补线性形式的不足，模型2 将ω设为指数形式，如式(5)所示，ω值缓慢降低提高了迭代后期局部搜索能力，但与线性形式相比全局搜索能力稍有降低；为提高迭代前期的全局搜索能力，在模型2 基础上增加一个参数m构造成模型3，如式(6)所示，其中m是正整数，τ是小于等于1 的数，τ的m次幂可有效提高ω的值，从而提高算法的全局寻优能力。但在PSO 算法中需要更好地协调全局寻优能力和局部寻优能力，所以在模型3基础上增加一个正弦函数构造成模型4，如式(7)所示，正弦函数是一个周期函数，能够更好地协调全局搜索和局部搜索，而且因正弦函数图像的特点，可以很好地保持模型4 的优势。

式中，ωmax为惯性权重最大值；τ=t/T，t为当前迭代次数，T为最大迭代次数；m为正整数；λ和k为非负整数。

为验证ω参数四种模型的特点，设当ωmax=1、λ=10、m=3、k=6、T=3000 次时，四种模型曲线如图2所示。模型1 线性形式的收敛曲线在迭代前期具有很高的ω值，说明迭代前期全局搜索能力强，但在迭代后期下降很快，导致算法在迭代后期局部搜索能力变差；模型2 指数形式与模型1 相比，在迭代后期ω值变化缓慢，说明局部搜索能力变强，但在迭代初期ω值较小，说明全局搜索能力相对减弱；与模型2 相比，模型3 不仅保留了局部寻优的能力，同时增加的m参数提高了全局搜索能力；模型4 中ω值呈周期性变化，在迭代初期很快取得最大ω值，若能在迭代初期找到全局最优解，可更快进行收敛，且在迭代后期ω值逐渐变小，说明局部搜索能力较强。因τ是离散值，且(k+0.5)(1 -τ)不是纯整数，所以模型4 曲线是无限趋近与0。对比其他三种模型，模型4 能更好地协调全局性和局部性，应用到地形匹配中可在全局搜索和局部搜索中不断寻找最优匹配序列，最终找到最优解，并且模型4 是周期函数，若在前期找到匹配最优解即可进行收敛，提高了匹配速度，缩短了匹配时间。

图2 四种模型惯性权重曲线图Fig.2 Inertial weight curves of the four models

将模型4 的惯性权重改变形式引入到式(2)的标准PSO 算法中，改进后的PSO 算法记作改进惯性权重粒子群算法（Weight Particle Swarm Optimization,WPSO），WPSO 流程图如图3所示。

图3 WPSO 算法流程图Fig.3 Flow chart of WPSO algorithm

3 增加旋转角度机制TERCOM 算法

增加旋转角度机制TERCOM 算法的设计思路是先通过TERCOM 算法粗匹配后，用WPSO 算法进行精匹配。首先根据INS 提供的航迹，确定整个匹配航迹的重心坐标：

式中，(xi,yi)为INS 提供的航迹位置坐标；D为实测深度值序列与INS 航迹序列间距离，采用马氏距离进行计算，马氏距离可有效降低噪声的影响[20]；为对应点在数字地图中的数值与实际测量深度值的差；n为匹配序列长度；C为协方差矩阵；为第i个实测采样点水深值；为第i个点在数字地图中的数值；为平均值。

其次以匹配序列重心为原点，对INS 提供航迹增加旋转角度公式如下：

式中，(xp,yp)为增加旋转角α后的航迹坐标；f(α)为由α组成的转向矩阵。

最后将[xp,yp]作为初始匹配序列进行TERCOM匹配，实现匹配定位。

增加旋转角度机制TERCOM 算法过程为：

（1）根据INS 的航迹位置，确定航迹的重心坐标(xh,yh)。

（2）初始匹配时，旋转角α从0°开始，以s=0.2°为步长对原始INS 航迹进行旋转。每旋转一次，遍历搜索区内每一个格网，依据旋转后的航迹平移并进行传统TERCOM 匹配，直到旋转角大于2 倍INS 角度误差时停止旋转，k表示2 倍INS 角度误差。

（3）根据MSD 准则，通过TERCOM 匹配后，找到最小MSD 值及对应的粗旋转最优角αi。

（4）基于粗旋转结果，以αi为中心，在[αi-2s,αi+2s]范围内以s/10为步长进行精旋转，并采用TERCOM 平移匹配，重复步骤（3）。当相邻两次匹配结果差值满足限差时（限差为50 m），则停止迭代，从而得到精旋转角度αj及两次旋转后的匹配序列Pj。

（5）在两次旋转后的匹配序列Pj基础上采用WPSO 算法进行精匹配。精匹配的过程：①初始化粒子群各参数；②评价每个粒子适应度值，适应度函数采用MSD 算法；③更新Pbest和Gbest；④判定是否找到全局最优解；⑤判断是否满足终止条件。

（6）当满足终止条件时，停止迭代，得到最优匹配序列。若不满足终止条件返回步骤（5）。终止条件是WPSO 算法达到最大迭代次数。

增加旋转角度机制TERCOM 算法的匹配流程图如图4所示。

图4 增加旋转角度机制TERCOM 算法流程图Fig.4 Flow chart of adding rotation angle mechanism TERCOM algorithm

增加旋转角度机制TERCOM 算法可更好地向真实航迹靠拢。由粗旋转到精旋转的过程可有效降低航向偏差，采用WPSO 算法进一步优化旋转后的匹配序列，从而提高匹配精度。增加旋转角度机制的TERCOM 算法记作TERCOM-A。

4 仿真及分析

海底深度值位于东经122.0568 °～122.1263 °、北纬37.5704 °～37.6078 °，插值后数据有1391×750 个网格点。多波束测深系统观测误差为5 m，惯性导航系统陀螺漂移为0.01 °/h，加速度计常值偏置0.01 mg。水下航行器采样间隔1 s，采样周期30 s，以8 kn 速度匀速航行，速度误差为 0.1 m/s，水深测量范围-65 m-0 m。通过设置初始位置误差作为水下航行器进入匹配区域时INS 已累积的位置误差。

仿真实验中，WPSO算法参数设置：粒子数为200，最大迭代次数为3000 次，学习因子c1=c2=1.35，惯性权重ω在(0,1]随着迭代次数的增加而逐渐递减。

为验证算法的有效性，分别在2 次不同初始误差情况下，比较基于 MSD 传统 TERCOM 算法和TERCOM-A 算法的匹配效果。为保证数据准确性，算法进行30 次后求平均值。当AUV 在水下做转弯运动时，水下航行器初始位置均在（122.086°E,37.577°N），初始航向均为北偏东150 °，初始航向误差均设置为3 °，匹配序列均为14，搜索范围是以INS 指示位置为中心，以6σ为边长的矩形区域。其中，σ为INS 位置误差的标准差，INS 在东向和北向误差的标准差均为30 m。地形熵为0.267，水深标准差为7.73 m。当INS初始位置误差较小时，仿真设置为经度误差0.3′，纬度误差0.3′；当INS 初始位置误差较大时，仿真设置为经度误差1′，纬度误差1′。两种匹配算法的轨迹如图5-6所示。

图5 INS 初始位置误差较小时两种算法匹配图Fig.5 Matching graph of two algorithms under the condition of INS small initial position errors

从图5 和图6 可以看出，TERCOM-A 算法的匹配精度明显优于TERCOM 算法。为进一步对比两种算法的匹配性能，分别针对上述两种情况，计算匹配航迹上各水深测量点位置的匹配时间、匹配误差、匹配误差标准差、经纬度误差和航向误差，两种匹配算法的地形匹配结果如表1所示。

图6 INS 初始位置误差较大时两种算法匹配图Fig.6 Matching graph of two algorithms under the condition of INS large initial position errors

表1 两种匹配算法的地形匹配结果Tab.1 Terrain matching results of two matching algorithms

从表1 可知，当初始位置误差较小时，与传统TERCOM 算法相比，TERCOM-A 算法的匹配误差可降低4 倍，经度误差可降低近5 倍，纬度误差可降低3 倍多，航向误差降低了89.3%；当初始位置误差较大时，与传统 TERCOM 算法相比，TERCOM-A 算法的匹配误差可降低6 倍多，经度误差可降低10 倍多，纬度误差可降低近2 倍，航向误差降低了84%。且在两种不同状态下，TERCOM-A算法的匹配误差标准差较小，说明匹配误差变化小且稳定。

改进后的TERCOM 算法的匹配精度明显优于传统TERCOM算法，主要是因为改进后的TERCOM算法，经过由粗匹配到精匹配的过程，有效降低了INS 航迹与真实航迹的航向角度偏差，提高了传统TERCOM 算法的匹配精度。同时在匹配时间方面，增加旋转角度机制的TERCOM 算法并没有增加太多时间，也充分说明了在精匹配时采用改进粒子群算法在快速寻优方面的优势。因此改进的TERCOM算法有效解决了传统TERCOM 算法的航向敏感问题，提高了匹配效果。

4 结论

增加旋转角度机制TERCOM 算法实现了由粗匹配到精匹配的过程，匹配精度更高，更好地抑制航向误差，限制累积误差增长，从而提高了匹配效果。同时改进的PSO 算法可有效提高PSO 的全局寻优能力，经WPSO 精匹配后的TERCOM 算法可实现快速、精准定位，提高匹配效率。改进的TERCOM算法可抑制INS 位置误差发散，提高AUV 长时水下自主航行隐蔽性，具有一定工程应用价值。