基于自适应渐消Sage-Husa 扩展卡尔曼滤波的协同定位算法

周萌萌，张冰，赵强，潘梦婷，左思雨

江苏科技大学 电子信息学院，江苏 镇江 212100

0 引 言

近年来，随着人类对海洋开发的日渐深入，自主水下航行器（autonomous underwater vehicle，AUV）被广泛应用于海洋资源开发、水下探测等领域，多AUV 组网协同作业逐渐成为人们关注的焦点[1]。AUV 位置估计在协同导航中十分重要，但由于复杂水下环境带来的不确定性，多AUV 协同系统的定位精度受限。采用基于状态空间模型的滤波方法，能够得到具有统计意义的最优估计，位置信息精度较高。文献[2]提出了一种基于迭代差值滤波（IDDF）的多AUV 协同导航算法，该算法将迭代滤波技术与插值滤波（DDF）算法相结合，但是无法解决量测异常时的鲁棒性抑制问题。文献[3]将凸优化算法应用于多AUV 协同导航系统中，然而仅仅适用于少量AUV，在AUV 数量增多的情况下，其有效性有待商榷。文献[4]提出了一种基于双移动信标的定位方法，通过该算法论证了分离角度和分离间距对定位性能的影响，但所用方法复杂，在AUV 受到外界干扰时，定位性能无法确保最优。文献[5]提出了一种基于交互式模型的多AUV 协同导航定位算法，但未考虑该算法在复杂噪声背景下的有效性。

AUV 协同导航是一个典型的非线性位置估计问题，众多学者提出了针对AUV 协同导航的定位算法。例如扩展卡尔曼滤波算法[6]（extended Kalman filter, EKF）、无迹卡尔曼滤波算法[7]（unscented Kalman filter, UKF）。但是，这两种算法都未考虑外部干扰带来的问题，模型的不确定性导致卡尔曼滤波难以精确估计系统噪声和量测噪声。文献[8]提出了自适应扩展卡尔曼滤波（AEKF）算法，该算法可以对噪声协方差矩阵进行自适应调整，实时估计噪声，但也存在诸多缺点，例如滤波易发散，影响定位精度；计算量过大，对操作系统硬件要求过高；过程噪声矩阵的正定性[9-11]难以得到保证。

本文拟提出一种基于自适应渐消Sage-Husa扩展卡尔曼滤波算法。首先，对Sage-Husa 量测噪声协方差矩阵进行改进；其次，再通过渐消记忆指数加权得到新息的协方差估计值，以提高新近数据的利用率；然后，引入渐消因子来调整误差协方差矩阵，以提高滤波器的精确性和自适应性；最后，针对噪声协方差不匹配以及噪声协方差时变的情况，分别采用所提算法与EKF 算法、期望最大化扩展卡尔曼滤波算法[12]（EM-EKF）进行仿真对比。

1 多AUV 协同导航系统问题

多AUV 协同导航定位是基于水声通信网络的合作定位方法，通常是少量AUV 配备高精度系统，充当领航AUV，跟随AUV 配备低精度系统。在运行过程中，各AUV 利用所携带的导航传感器，通过水声通信技术获取领航AUV 的位置信息以及二者之间的距离信息后，结合信息融合技术来提高自身定位精度，从而提高集群协同作业过程中整体定位精度[13-14]。

1.1 从AUV 的系统模型

从航行模式来看，协同导航系统结构可分为主从式和并行式结构，在此给出的是多领航者主从式AUV 协同定位模型。由于AUV 的深度位置信息可以由深度计精确测量，俯仰角在平稳运动时的变化幅度较小，因此可将AUV 水下三维定位问题转化为二维空间的定位问题。

式中：Vk为前向合成速度； ωk为偏航角速度；φk为航向角；t为采样周期。由式（1）可知，协同导航系统为非线性结构。其中，Vk和 ωk可由AUV 的本体传感器测量，为系统的控制输入：

式中：uk为 控制输入模型；Vmk和 ωmk分 别 为AUV的速度以及偏航角速度的量测值；wvk和wωk均为相互独立的零均值高斯白噪声。再令系统噪声协方差矩阵为Qk=diag[]， 其中和为高斯白噪声wvk和wωk的方差矩阵。

从而，可将跟随AUV 的二维运动学方程由式（1）简写为

式中：f(·)为 非线性模型的转移函数；wk为过程噪声。

1.2 协同定位量测模型

协同定位的量测模型是对二维平面上的领航AUV 和跟随AUV 之间的位置关系建模。假设两领航AUV 与跟随AUV 的水平距离为R1k和R2k，则可分别表示为

将R1k和R2k作为量测值，得到量测方程：

式中，vk为量测噪声。假设vk为零均值的相互独立高斯白噪声，则噪声协方差矩阵表示为

2 Sage-Husa 自适应扩展卡尔曼滤波算法

由式（3）和式（5）建立多AUV 协同定位运动模型和量测模型，用Sage-Husa 自适应扩展卡尔曼滤波（Sage-Husa AEKF）算法滤波，具体原理如下。

非线性化系统方程为

式中，h(·)为非线性系统的量测函数。

残差为

卡尔曼测量更新为

自适应估计器为

由式（15）可知，前半项 (1-dk)利用遗忘因子b与前一时刻的相乘，初始值设为正定，因而相乘结果也为正定。后半项用来更新所要计算的，当滤波过程中出现收敛趋势时，误差协方差Pk会 减小，且Hk为 一定值，则HkPk/k-1会逐渐减小，并趋于0，对量测噪声协方差估计的影响可忽略不计。因此，将式（15）的量测噪声协方差矩阵改为

3 渐消因子的选取

由式（10）可知，新息的理论协方差Ck为

式中，若Pk-1增 加，则Ck也会增加。同样，在未知信息的影响下，Ck也会增加。在此情况下，Rk的增加亦会导致Ck的增加。因此，由开窗法[15]得到Ck的估计值为

由式（9）～式（13）和式（16）～式（18）可求得渐消因子 λk。

其中，tr(.)为 求迹运算。为 保证 λk≥1， 当≤Nk时， λk=1。当新息协方差的理论值偏离估计值，通过调整 λk来补偿不断增加的估计误差。

由式（20）可知， λk的 求取需要依靠准确的，但由式（19）计算得到的仅是对滑动窗口内的数据进行算术平均，没有利用新近数据。因此，接下来将通过一种渐消记忆指数加权滤波估计器，加大对新近量测数据的利用权重，突出新近数据的作用。以下按照负指数函数规律对加权系数βi赋值，则有

为满足式（21）对加权系数序列的要求，令

则，此时

将式（24）代入式（19），替代原来的权系数1/(k-1) ， 得到的为

另，由式（23）又可知

将式（27）代入式（20），则可求出 λk。此算法简单且易实现，也即由式（8）～式（16）、式（17）～式（20）以及式（27）构成了本文所提自适应渐消Sage-Husa扩展卡尔曼滤波(Saga-Husa AFEKF)算法。

4 仿真验证

为验证本文算法的有效性，依据文献[16]设定相关条件数据（数据来源完整可靠），本节将分别在噪声协方差不匹配时及噪声协方差时变时，将本文算法与EKF，EM-EKF 算法进行仿真对比。

4.1 仿真条件

领航AUV 和跟随AUV 的实际运行轨迹如图1 所示，采样周期t=1 s，仿真总时长为1 500 s。状态值为=[0 0 60π/180]，量测噪声协方差矩阵值为=diag([1]2)，系统噪声协方差矩阵值为Qk=diag([0.1 0.1]2)， 误 差 协 方 差 值 为Pk=diag([1 1 0.1]2)，=zeros(1,1)，遗 忘 因 子b=0.995。领航AUV1 起点（-500 m，0 m），领航AUV2 起点（500 m，0 m），均以初始航向角60°沿直线航行；跟随AUV 起点（0，0），以初始航向角60°进行S 形运动。为验证本文所提方法的有效性，采用定位误差（LE）、均方根误差（RMSE）和平均定位误差（ALE）作为多AUV 协同定位的性能指标：

图1 领航AUV 和跟随AUV 真实轨迹Fig. 1 True trajectory of leader AUV and follower AUV

4.2 噪声协方差不匹配情况下仿真分析

在AUV 协同定位中，量测噪声易受外界干扰，不稳定性较大。过程噪声主要由元器件的误差确定，而元器件又时常受温度、湿度等外部因素的影响发生改变。设噪声协方差在800～1 000 s发生改变，即=diag([10]2)，Qk=diag([1 1]2)，以检验3 种算法在噪声协方差不匹配时的自适应能力。仿真结果如图2 和图3 所示（图例中的DR代表航位推算）。表1 为3 种算法协同定位性能的比较结果。

表1 噪声协方差不匹配时3 种算法的协同定位性能比较Table 1 Cooperative localization performance comparison of three algorithms with unmatched noise covariance

图2 噪声协方差不匹配时跟随AUV 定位误差比较Fig. 2 Comparison of localization errors of follower AUV with unmatched noise covariance

图3 噪声协方差不匹配时跟随AUV 航行轨迹比较Fig. 3 Comparison of navigation trajectory of follower AUV with unmatched noise covariance

如图2 和图3 所示，DR 航推位置逐渐偏离真实轨迹，定位精度逐渐发散，原因是DR 航推信息仅由DVL 和罗经提供相应的速度和位置信息，定位误差会逐渐累积。当噪声协方差矩阵严重不匹配时，3 种算法均趋于收敛。其中EKF 滤波波动较大，定位精度较低。EM-EKF 算法滤波结果优于EKF 算法，是因为前者可以在线调整量测噪声协方差矩阵。本文算法的定位精度优于EMEKF 和EKF 算法，因为本文算法能够自适应调整噪声协方差矩阵的统计特性，由更准确的新息协方差估计值引入渐消因子修正预测误差协方差矩阵，从而调节滤波增益，获取更准确的状态向量值。本文算法具有更好的自适应性和滤波精度。

由表1 可知，在量测噪声协方差矩阵严重不匹配时，与EM-EKF 算法相比，本文算法的均方根误差减少17.82%，平均误差减少17.87%；与EKF算法相比，本文算法的均方根误差减少24.48%，平均误差减少22.54%。由此可知，在此情况下，本文算法具有更好的定位精度。

4.3 噪声协方差时变情况下仿真分析

在AUV 协同定位中，噪声受到外部环境的干扰后，其统计特性通常随时间发生变化。量测噪声和过程噪声协方差不准确时会降低滤波的鲁棒性，导致定位精度降低。本文设定噪声协方差的值在1 000～1 200 s 发生跳变，其统计特性变为4.1 节所述值的5 倍，即 5和 5Qk，以检验算法在噪声协方差矩阵时变情况下的性能。图4 和图5 为仿真结果，表2 为3 种算法协同定位性能的比较结果。

图4 噪声协方差时变情况下跟随AUV 定位误差比较图Fig. 4 Comparison on localization errors of follower AUV with the time-varying noise covariance

图5 噪声协方差时变情况下跟随AUV 航行轨迹比较图Fig. 5 Comparison on navigation trajectory of follower AUV with the time-varying noise covariance

表2 噪声协方差时变情况下3 种算法的协同定位性能比较Table 2 Cooperative localization performance comparison of three algorithms with time-varying noise covariance

如图4 和图5 所示，DR 航推位置逐渐偏离真实轨迹，定位误差随着时间的变化不断累积。当噪声协方差矩阵处于时变情况时， EKF 算法的定位误差在1 000 s 左右出现剧烈波动，定位误差逐渐增大，1 200 s 之后定位误差开始降低，波动幅度较小； EM-EKF 算法的定位精度优于EKF 算法，是因为该算法可以自适应估计量测噪声协方差矩阵。与另2 种算法相比，本文算法结果波动较为平缓，定位精度更高；原因是噪声协方差处于时变状态下时，本文算法通过自适应估计器对量测噪声协方差矩阵进行自适应调整，实时调整增益矩阵，补偿噪声协方差矩阵新息的影响，提高了滤波算法的抗干扰性和定位精度。

由表2 可知，当噪声协方差处于时变情况下时，与EM-EKF 算法相比，本文算法的均方根误差减少42.11%，平均误差减少34.87%；与EKF算法相比，本文算法的均方根误差减少51.23%，平均误差减少46.90%。在此情况下，本文算法具有更好的定位精度。

5 结 语

由于水下环境复杂多变，易导致AUV 的噪声统计特性发生改变，在此情况下，EKF 及UKF 等常规卡尔曼滤波算法难以解决此类问题，无法准确估算AUV 在水下的位置状态，不能有效提升AUV 定位精度。基于此，本文提出了一种自适应渐消Sage-Husa 扩展卡尔曼滤波方法，对量测噪声协方差矩阵进行改进并引入渐消因子。仿真结果表明，当噪声协方差不匹配或者噪声协方差处于时变情况下，本文所提算法可有效提高AUV定位精度，具有一定的自适应性及抗干扰性。此外，本文只考虑了白噪声对算法的影响，后续可进一步研究其他非高斯噪声，如有色噪声等因素对协同定位导航算法的影响。