0 引　言

近年来，随着人类对海洋探索的深入，水下无人航行器（Unmanned Underwater Vehicle,UUV）迅速发展，UUV分支中的自主水下航行器（Autonomous Underwater Vehicle，AUV）被广泛应用于海洋开发、水下探测等领域。但由于海洋环境的复杂性，单个AUV难以完成困难的水下作业任务，因此，应用多AUV协同系统成为了人们关注的热点，旨在以更高的效率和更低的成本来完成困难任务^[1]。

刘峰等^[2]提出将逆超短基线系统融入AUV集群的协同定位中，通过构建因子图来优化定位过程。但计算复杂度较高，且设置从AUV1和从AUV2的定位误差为固定的2%和1%，未深入考虑真实海洋噪声的时变性会影响AUV的定位精度。李倩等^[3]首次将哺乳动物大脑海马体的空间细胞认知机理应用于AUV的协同定位系统中。通过模仿生物大脑的导航方式，为AUV协同定位提供了新的思路。但算法依赖于高精度的水声测距设备和惯性导航系统，设备成本较高。且观测噪声的统计特性固定，未深入考虑真实海洋噪声的时变性会影响AUV的定位精度。何桂萍等^[4]提出了基于自适应残差加权（Adaptive Residual Weighting-underwater Acoustic Sensor Network，ARW-UWASN）的AUV集群分布式协同定位算法，通过分布式逐级定位，对所有有效的潜在位置利用残差赋以权值，实现对待定位节点位置的自适应加权估计，提高了AUV集群整体位置精度和定位鲁棒性，实现简单、成本低、可靠性好，且不受AUV活动区域限制。但当待定位节点收到的领航和参考节点的信息数量少于3时，无法估算出位置信息，需要等待下一个定位周期，且设置领航 AUV 节点自定位位置误差为 2 m，未深入考虑真实海洋噪声的时变性会影响AUV的定位精度。陈世杰^[5]提出了交互多模型-逆推扩展卡尔曼滤波（Interacting Multiple Model-delay Extended Kalman Filter，IMM-DEKF）算法，进一步提高了协同定位系统的鲁棒性和准确性，但主要研究低频信号下的协同定位问题，未探讨提高通信频率可能引发的信号出序问题和声学传播过程中可能存在的信号丢失问题。且只给出了系统噪声协方差，未给出观测噪声协方差。JIANG等^[6]提出了最大似然估计和高斯置信传播（Maximum Likelihood Estimation and the Gaussian Belief Propagation，ML-GBP）算法，提高了系统的定位精度，减少了计算复杂度并降低了通信开销，且测量噪声是与距离有关的动态值，考虑了真实海洋环境下噪声统计特性的复杂性。JIANG等^[7]提出了一种具有偏差减小和误差补偿的通信时差协同定位（Time Difference of Communication-cooperative localization-bias Reduction and Error Compensation，iTDOC-CL-BREC）方法，消除了时钟异步的影响，在长时间任务下能提供更高的定位精度。且有20%的概率会通信失败（主AUV和从AUV无法通信），考虑了真实海洋环境下噪声干扰对通信的影响。ZHANG等^[8]提出了一种基于一致扩展卡尔曼滤波（Consistent Extended Kalman Filter，CEKF）的多AUV协同定位方法，增加了零空间矢量作为可观测性约束，相较于扩展卡尔曼滤波（Extended Kalman Filter，EKF）算法提高了从AUV的定位精度。但观测噪声统计特性固定，未考虑真实海洋噪声的时变性，且需要在系统稳定的前提下减少通信频率。肖鹏飞等^[9]提出一种自适应无迹卡尔曼滤波（Adaptive Unscented Kalman Filter，AUKF）算法，在融合算法中引入自适应因子，重构系统状态方程中速度项与状态向量的结合方式，解决系统方差不一致问题，提高了AUV的定位精度和滤波的鲁棒性。但是从AUV的定位误差设置为固定的0.2%，未深入考虑真实海洋噪声的时变性会影响AUV的定位精度。周萌萌等^[10]提出一种基于自适应渐消Sage-Husa扩展卡尔曼滤波的多AUV协同定位算法，能够在线调整噪声协方差矩阵，并通过渐消因子提高对新近数据的利用率，在噪声协方差不匹配和噪声协方差时变的情况下表现出比EKF算法更高的定位精度和更好的鲁棒性。但是噪声特性是在某个时刻突然时变的，其余时间都是固定不变的，没有完全考虑海洋环境中噪声的时变性，若在噪声高度时变的环境下可能会降低定位精度。

针对现有研究在处理复杂噪声环境下AUV定位问题的不足，本文创新性地引入了一种统计特性高度时变的高斯噪声模型，该模型可以更精确地模拟真实海洋环境中噪声的时变性，以深入分析在统计特性高度时变且未知的高斯噪声条件下AUV定位精度下降的问题。并针对这一问题提出一种基于自适应阈值判定与改进Sage-HusaUKF（Adaptive Threshold And Improved Sage-Husa Unscented Kalman Filter，AT-ISHUKF）的AUV协同定位算法。该算法在改进Sage-Husa-UKF算法的基础上创新性地引入自适应阈值判定机制，能够对当前环境的噪声统计特性进行分析，判断噪声变化是否剧烈，并通过判断采取不同的噪声更新策略来应对噪声变化。首先，对量测噪声协方差更新进行改进，然后通过无迹卡尔曼滤波对噪声统计特性未知的系统进行分析，得到新息协方差和预测观测协方差，引入自适应阈值判断新息协方差与预测观测协方差之间的差异是否显著，若差异显著则认为噪声特性发生了较大变化，此时采用较小的自适应因子来更快地适应这种变化，若差异不显著则认为噪声特性变化较小，此时采用较大的自适应因子来保持估计的鲁棒性。相较于现有研究，在统计特性高度时变的高斯噪声环境下，本文提出的AT-ISHUKF算法在处理AUV定位问题时具有更高的定位精度和鲁棒性。

1 多AUV协同导航 1.1 多AUV协同导航结构

目前多AUV协同导航主要分为主从式和并行式^[11]。本文基于单领航主从式AUV协同定位展开，单领航主从式AUV协同定位结构如图1所示。

1.2 AUV协同定位运动模型

AUV的二维位置($x,y$)、速度$v$、角度$\theta $可以用状态向量$ X=[x,y,v,\theta]^\mathrm{T} $表示，那么AUV在$k + 1$时刻的二维运动学模型可以表示为：

$ {{X_{k + 1}} = \left[ \begin{gathered} {x_{k + 1}} \\ {y_{k + 1}} \\ {v_{k + 1}} \\ {\theta _{k + 1}} \\ \end{gathered} \right] = f({X_k}) + G({w_k}) = \left[ \begin{gathered} {x_k} + {T}{v_k}\cos ({\theta _k}) \\ {y_k} + {T}{v_k}\sin ({\theta _k}) \\ {v_k} + {T}{u_k} \\ {\theta _k} + {T}{\omega _k} \\ \end{gathered} \right] + G({w_k})。} $

(1)

式中：${X_{k + 1}}$为AUV在$k + 1$时刻的状态向量；${X_k}$为AUV在$k$时刻的状态向量；$f( \cdot )$为状态转移函数；$G( \cdot )$为过程噪声驱动函数；${T}$为采样周期；${u_k}$为AUV在$k$时刻的前向合成加速度；${\omega _k}$为AUV在$k$时刻的航向角速度；${w_k}$为$k$时刻的高斯过程噪声，即：

$ E\left[ {{w_k}} \right] = {q_k},{\rm{cov}} \left[ {{w_k}{w_j}^\mathrm{T}} \right] = {Q_k}。$

(2)

式中：${q_k}$为$k$时刻的${w_k}$的均值；${Q_k}$为$k$时刻的${w_k}$的协方差矩阵。

1.3 AUV协同定位量测模型

在二维空间下，若$k$时刻的主AUV状态向量$ X_k^N= [x_k^N,y_k^N,v_k^N,\theta_k^N]^{\mathrm{T}} $，$k$时刻的从AUV状态向量$ X_k^F=[x_k^F, y_k^F,v_k^F,\theta_k^F]\mathrm{^T} $，协同定位的量测距离是主AUV和从AUV的欧几里得距离，则$k$时刻的量测方程表示为：

$ {Z_k} = h({X_k}) + {v_k} = \sqrt {{{(x_k^N - x_k^F)}^2} + {{(y_k^N - y_k^F)}^2}} + {v_k}。$

(3)

式中：${Z_k}$为AUV在$k$时刻的的量测距离；$h( \cdot )$为量测函数；${v_k}$为$k$时刻的高斯量测噪声，即：

$ E\left[v_k\right]=r_k,{\rm{cov}}\left[v_kv_j^{\mathrm{T}}\right]=R_k 。$

(4)

式中：${r_k}$为$k$时刻的${v_k}$的均值；$ {R_k} $为$k$时刻的${v_k}$的协方差矩阵。

2 算法改进 2.1 Sage-Husa-UKF算法

对式(1)和式(3)建立的多AUV协同定位模型，Sage-Husa-UKF滤波算法步骤如下：

1）初始化

$ \left\{\begin{gathered}\hat{X}_{0|0}=E[X_0]，\\ P_{0|0}=E[(X_0-\hat{X}_{0|0})(X_0-\hat{X}_{0|0})^{\mathrm{T}}]。\\ \end{gathered}\right. $

(5)

2）权重系数计算

$ \left\{ \begin{gathered} w_i^m = w_i^c = 1/(2(n + \lambda )),i = 1, \cdots ,2n ，\\ w_0^m = \lambda /(n + \lambda ) ，\\ w_0^c = \lambda /(n + \lambda ) + 1 - {\alpha ^2} + \beta，\\ \lambda = {\alpha ^2}(n + \kappa ) - n 。\\ \end{gathered} \right. $

(6)

3）构造Sigma点

$ \left\{ \begin{gathered} \chi _{k|k}^{(0)} = {{\hat X}_{k|k}} ，\\ \chi _{k|k}^{(i)} = {{\hat X}_{k|k}} + \sqrt {(n + \lambda ){\boldsymbol{P}}_{k|k}^{(i)}} ,i = 1, \cdots ,n ，\\ \chi _{k|k}^{(i)} = {{\hat X}_{k|k}} - \sqrt {(n + \lambda ){\boldsymbol{P}}_{k|k}^{(i)}} ,i = n + 1, \cdots ,2n 。\\ \end{gathered} \right. $

(7)

4）时间更新

$ \left\{ \begin{gathered} \hat X_{k|k - 1}^{(i)} = f(\chi _{k - 1|k - 1}^{(i)})，\\ {{\hat X}_{k|k - 1}} = \sum\limits_{i = 0}^{2n} {w_i^m} \hat X_{k|k - 1}^{(i)} 。\\ \end{gathered} \right. $

(8)

$ {{\boldsymbol{P}}_{k|k-1}=\displaystyle\sum\limits_{i=0}^{2n}w_i^c(\hat{X}_{k|k-1}^{(i)}-\hat{X}_{k|k-1})(\hat{X}_{k|k-1}^{(i)}-\hat{X}_{k|k-1})^{\mathrm{T}}+{\boldsymbol{\Phi}}_k\hat{{\boldsymbol{Q}}}_{k-1}{\boldsymbol{\Phi}}_k^{\mathrm{T}}。} $

(9)

5）量测更新

$ \left\{ \begin{gathered} \hat Z_{k|k - 1}^{(i)} = h(\chi _{k - 1|k - 1}^{(i)}) ，\\ {{\hat Z}_{k|k - 1}} = \sum\limits_{i = 0}^{2n} {w_i^m} \hat Z_{k|k - 1}^{(i)}。\\ \end{gathered} \right. $

(10)

$ {\boldsymbol{P}}_{ZZ,k} = \sum\limits_{i=0}^{2n}w_i^c(\hat{Z}_{k|k-1}^{(i)} - \hat{Z}_{k|k-1})(\hat{Z}_{k|k-1}^{(i)} - \hat{Z}_{k|k-1})^{\mathrm{T}} + \hat{{\boldsymbol{R}}}_{k-1}，$

(11)

$ {\boldsymbol{P}}_{XZ,k}=\sum\limits_{i=0}^{2n}w_i^c(\hat{X}_{k|k-1}^{(i)}-\hat{X}_{k|k-1})(\hat{Z}_{k|k-1}^{(i)}-\hat{Z}_{k|k-1})\mathrm{^T}。$

(12)

6）滤波结果更新

$ {{\boldsymbol{K}}_k} = {{\boldsymbol{P}}_{XZ,k}}/{{\boldsymbol{P}}_{ZZ,k}}，$

(13)

$ {\hat X_{k|k}} = {\hat X_{k|k - 1}} + {{\boldsymbol{K}}_k}{\varepsilon _k}，$

(14)

$ {\boldsymbol{P}}_{k|k}={\boldsymbol{P}}_{k|k-1}-{\boldsymbol{K}}_k{\boldsymbol{P}}_{ZZ,k}{\boldsymbol{K}}_k^{\mathrm{T}}。$

(15)

7）量测噪声更新

$ {d_k} = (1 - b)/(1 - {b^{k + 1}}),0 < b < 1，$

(16)

$ \hat{{\boldsymbol{R}}}_k=(1-d_k)\hat{{\boldsymbol{R}}}_{k-1}+d_k(\varepsilon_k\varepsilon_k^{\mathrm{T}}-{\boldsymbol{P}}_{ZZ,k})。$

(17)

式中：${X_0}$为AUV初始状态向量（$k = 0$）；${\hat X_{0|0}}$为初始状态后验估计均值；${P_{0|0}}$为初始状态后验估计误差协方差矩阵；$ w_i^m $和$ w_i^c $为权重系数；$n$为状态维度数；$\alpha $为第一刻度因子；$\beta $为第二刻度因子；$\kappa $为第三刻度因子；$\lambda $为符合刻度因子；$ {\hat X_{k|k}} $为状态后验估计均值；$ \chi _{k|k}^{(i)} $为Sigma点第$i$列；$ \boldsymbol{P}_{k|k} $为状态后验估计误差协方差矩阵；$ \sqrt {P_{k|k}^{(i)}} $为${P_{k|k}}$均方根的第$i$列；${\hat X_{k|k - 1}}$为状态先验估计均值；$ \boldsymbol{P}_{k|k-1} $为状态先验估计误差协方差矩阵；$ \boldsymbol{\Phi}_k $为$k$时刻的过程噪声驱动矩阵；$ \hat Z_{k|k - 1}^{(i)} $为$ \chi _{k|k}^{(i)} $的量测先验估计值第$i$列；${\hat Z_{k|k - 1}}$为量测先验估计均值；$ \boldsymbol{P}_{ZZ,k} $为$k$时刻的预测观测协方差矩阵；$ \boldsymbol{P}_{XZ,k} $为$k$时刻的状态预测误差协方差矩阵；$ K_k $为$k$时刻的卡尔曼增益矩阵；${\varepsilon _k} = {Z_k} - {\hat Z_{k|k - 1}}$为$k$时刻的新息协方差；$ {d_k} $为$k$时刻的权重系数（遗忘因子）；$b$为自适应因子；$ \hat{\boldsymbol{Q}}_k $为$k$时刻的过程噪声估计协方差矩阵；$ \hat{\boldsymbol{R}}_k $为$k$时刻的量测噪声估计协方差矩阵。

2.2 改进Sage-Husa自适应算法

观测噪声幅度较大时误差协方差矩阵$ \boldsymbol{P} $容易成为非正定矩阵，针对该问题，提出在更新观测噪声协方差时将动态变化的遗忘因子${d_k}$修改为固定的自适应因子$b$，即将式(17)修改为：

$ \hat{{\boldsymbol{R}}}_k=b\hat{{\boldsymbol{R}}}_{k-1}+(1-b)(\varepsilon_k\varepsilon_k^{\mathrm{T}}-{\boldsymbol{P}}_{ZZ,k})。$

(18)

标准Sage-Husa-UKF算法通过使用遗忘因子$ {d_k} $来更新过程噪声估计值和量测噪声估计值，由式(16)可知，${d_k}$大小取决于自适应因子$b$和AUV的航行时刻$k$，在实际应用中，自适应因子$b$的取值范围通常为[0.85,0.99]，故在AUV航行初期（$k$值较小时），${b^{k + 1}}$接近于1，导致$1 - {b^{k + 1}}$较小，从而${d_k}$较大。

在实际应用中，可能有时观测噪声协方差会较大，即${\hat {\boldsymbol{R}}_{k - 1}}$较大。由式(11)可知，当${\hat {\boldsymbol{R}}_{k - 1}}$增大时，$ {{\boldsymbol{P}}_{ZZ,k}} $也会增大。由式(18)可知，当$ {{\boldsymbol{P}}_{ZZ,k}} $较大时，可能使$ \varepsilon_k\varepsilon_k^{\mathrm{T}} < {\boldsymbol{P}}_{ZZ,k} $，即$ (\varepsilon_k\varepsilon_k^{\mathrm{T}}-{\boldsymbol{P}}_{ZZ,k}) < 0 $，且AUV航行初期${d_k}$较大时，可能导致$ (1-d_k)\hat{R}_{k-1}+d_k(\varepsilon_k\varepsilon_k^{\mathrm{T}}-{\boldsymbol{P}}_{ZZ,k}) < 0 $，即${\hat {\boldsymbol{R}}_k} < 0$，由式(11)可知，当${\hat {\boldsymbol{R}}_k} < 0$时可能会导致${{\boldsymbol{P}}_{ZZ,k + 1}}$接近于0，由式(13)可知，${{\boldsymbol{P}}_{ZZ,k + 1}}$越接近于0，$ {\boldsymbol{K}}_{k+1}{\boldsymbol{K}}_{k+1}^{\mathrm{T}} $越大，由式(15)可知，当$ {\boldsymbol{K}}_{k+1}{\boldsymbol{K}}_{k+1}^{\mathrm{T}} $较大时，有可能导致$ {{\boldsymbol{K}}_{k + 1}}{{\boldsymbol{P}}_{ZZ,k + 1}}{\boldsymbol{K}}_{k + 1}^{\rm{T}} > {{\boldsymbol{P}}_{k + 1|k}} $ ，此时${{\boldsymbol{P}}_{k + 1|k + 1}}$为非正定矩阵。

上述可说明观测噪声幅度较大时，误差协方差矩阵${\boldsymbol{P}}$容易成为非正定矩阵的原因。实际应用中一般b的取值范围为[0.85,0.99]，则$(1 - b)$为[0.01,0.15]，在AUV航行初期（$k = 2$），${d_k}$的取值范围为[0.33,0.39]，不难看出，$ (\varepsilon_k\varepsilon_k^{\mathrm{T}}-{\boldsymbol{P}}_{ZZ,k}) < 0 $时，$ ((1-d_k)\hat{{\boldsymbol{R}}}_{k-1}+ d_k(\varepsilon_k\varepsilon_k^{\mathrm{T}}- {\boldsymbol{P}}_{ZZ,k})) < 0 $的几率远高于$ (b\hat{{\boldsymbol{R}}}_{k-1}+(1-b)(\varepsilon_k\varepsilon_k^{\mathrm{T}}- {\boldsymbol{P}}_{ZZ,k})) < 0 $，故使用自适应因子$b$更新观测噪声估计可以降低因观测噪声协方差过大而导致误差协方差矩阵变为非正定矩阵的风险，提高了滤波器的鲁棒性。

2.3 引入自适应阈值判定机制

当观测噪声的统计特性发生变化时，新息协方差也会相应变化，因此可以通过计算新息协方差与预测观测协方差之间的差值绝对值，并通过与自适应阈值比较大小来判断观测噪声统计特性变化是否剧烈。自适应阈值判定流程如图2所示。

计算新息协方差与预测观测协方差的差值绝对值：

$ {D_k} = \left| {{\varepsilon _k}\varepsilon _k^{\mathrm{T}} - {P_{ZZ,k}}} \right|。$

(19)

计算自适应阈值：

$ \left\{ \begin{gathered} T_k^s = (m/k)\sum\limits_{i = 1}^k {{D_i}} ,k \leqslant {w_s}，\\ T_k^s = (m/{w_s})\sum\limits_{i = k - {w_s} + 1}^k {{D_i}} ,k > {w_s} 。\\ \end{gathered} \right. $

(20)

观测噪声协方差更新：

$ \left\{ \begin{gathered} {{\hat {\boldsymbol{R}}}_k} = {b_1}{{\hat {\boldsymbol{R}}}_{k - 1}} + (1 - {b_1})({\varepsilon _k}{\varepsilon _k}^{\mathrm{T}} - {{\boldsymbol{P}}_{ZZ,k}}),{D_k} > T_k^s，\\ {{\hat {\boldsymbol{R}}}_k} = {b_2}{{\hat {\boldsymbol{R}}}_{k - 1}} + (1 - {b_2})({\varepsilon _k}{\varepsilon _k}^{\mathrm{T}} - {{\boldsymbol{P}}_{ZZ,k}}),{D_k} \leqslant T_k^s。\\ \end{gathered} \right. $

(21)

式中：$ D_k $为$k$时刻的新息协方差与预测观测协方差的差值绝对值；$T_k^S$为$k$时刻的自适应阈值，上标S是为了与采样周期$T$区分；$ w_s $为滑动窗口大小，下标s是为了与过程噪声$w$区分；$m$为放大系数；$b_1$为自适应因子1；$b_2$为自适应因子2。

滑动窗口$ w_s $用于确定计算自适应阈值时所使用的$ D_k $的历史数据序列长度，滑动窗口大小会影响滤波器对观测噪声统计特性变化的响应速度。较大的$ w_s $可以使滤波器更加平滑地适应噪声变化，但可能会降低对突发噪声变化的响应速度；较小的$ w_s $则可以使滤波器更快地响应噪声变化，但可能会增加滤波的波动性。放大系数$m$用于调整自适应阈值对观测噪声统计特性变化的灵敏度。较大的$m$值有助于滤波器保持稳定的输出，增强鲁棒性，但可能降低对噪声剧烈变化的响应速度，进而影响滤波精度。较小的${{m}}$值使滤波器能够更快地响应这些变化，有助于提升滤波的精度，但可能因对微小噪声波动过于敏感而降低鲁棒性。自适应因子$b$用于调整滤波器对历史数据（${\hat {\boldsymbol{R}}_{k - 1}}$）和当前数 据（$ \varepsilon_k\varepsilon_k^{\mathrm{T}}-\boldsymbol{P}_{ZZ,k} $）的信任程度，从而影响滤波器的自适应能力。$b$的值越大，滤波器对历史数据的信任程度越高，自适应速度越慢，因此在观测噪声统计特性变化平稳时，使用较大的$b$值有助于保持滤波鲁棒性。但是在观测噪声统计特性变化剧烈时，使用较大的$b$值会导致自适应速度变慢，从而降低滤波精度；$b$值越小，滤波器对当前数据的信任程度越高，自适应速度越快，因此在观测噪声统计特性变化剧烈时，使用较小的$b$值有助于滤波器快速适应当前噪声统计特性的变化。但是在观测噪声统计特性变化平稳时，使用较小的$b$值会导致自适应速度加快。从而降低滤波鲁棒性。因此，在具体选择滑动窗口${{w_{s}}}$、放大系数$m$和自适应因子$b$的值时，需要通过反复试验来对比不同值下的滤波精度和鲁棒性，以此确定在观测噪声统计特性时变且未知情况下${{w_{s}}}$、${{m}}$和$b$的最佳值。

式(5)～式(15)以及式(19)～式(21)构成了本文所提的基于自适应阈值与改进Sage-HusaUKF（AT-ISHUKF）算法。

3 仿真验证及结果分析 3.1 仿真场景设置

在二维水平面上，领航AUV以初始航向角45°沿直线航行，跟随AUV以初始航向角10°曲线航行，跟随AUV航向角变化如下：

$ \left\{ \begin{gathered} \theta = 10^\circ + 0.33^\circ k，0 < k \leqslant 300，\\ \theta = 0.072^\circ k，300 < k \leqslant 700，\\ \theta = 0.08^\circ k，700 < k \leqslant 1100，\\ \theta = 170^\circ - 0.075^\circ k，1100 < k \leqslant 2000。\\ \end{gathered} \right. $

(22)

根据式(22)，得到跟随AUV的变化航向角，设置采样周期$ {T}=1\; \text{s} $，仿真时长为$2\;000\;{\mathrm{s}}$，领航AUV和跟随AUV的前向合成速度$ u_{k}=1\ \mathrm{m/s} $，领航AUV初始状态向量为$ X_0^N=[400,0,1,45^{\mathrm{o}}]^{\mathrm{T}} $，跟随AUV初始状态向量为$ X_0^F=[0,0,1,10^{\mathrm{o}}]\mathrm{^T} $，根据上述条件即可得到领航AUV和跟随AUV的真实航行轨迹，如图3所示。

3.2 噪声统计特性时变且未知情况下仿真分析

在真实的海洋环境下，由于自然现象（海浪、洋流、风、海洋生物活动等）和人为活动（航运、钻探、声纳等）的共同作用，有可能会产生服从标准正太分布的随机高斯噪声；固定幅值和固定频率且大小随时间变化的正弦噪声和余弦噪声；幅值、频率和大小均随时间变化的正弦噪声和余弦噪声。$N$和$M$是由这些噪声组合而成的时变噪声。

$ N = {\rm{rand}}n(k) + \sin (0.2{\text{π}} k) + \cos (0.4{\text{π}} k)，$

(23)

$ \begin{split} M =& {\rm{rand}}n(k) + {\rm{rand}}_{100}^{[0,1]} \times \sin ({\rm{rand}}_{100}^{[0,1]} \times {\text{π}} k) +\\ &{\rm{rand}}_{100}^{[0,1]} \times \cos ({\rm{rand}}_{100}^{[0,1]} \times {\text{π}} k) 。\end{split}$

(24)

式中：${\rm{rand}}n(k)$为随航行时间$k$变化的服从标准正态分布的随机高斯噪声；$ \sin (0.2{\text{π}} k) $为固定幅值和固定频率且大小随时间变化的正弦噪声；$ {\rm{rand}}_{100}^{[0,1]} \times \sin ({\rm{rand}}_{100}^{[0,1]} \times {\text{π}} k) $表示大小随航行时间$k$变化且航行时间每过100 s幅值（取值范围为[0,1]）和频率（取值范围为[0,1]）随机变化一次的正弦噪声；$ \cos (0.4{\text{π}} k) $表示固定幅值和固定频率且大小随时间变化的余弦噪声；$ {\rm{rand}}_{100}^{[0,1]} \times \cos ({\rm{rand}}_{100}^{[0,1]} \times {\text{π}} k) $表示大小随航行时间$k$变化且航行时间每过100 s幅值（取值范围为[0,1]）和频率（取值范围为[0,1]）随机变化一次的余弦噪声。

设置过程噪声协方差矩阵${{\boldsymbol{Q}}_k} = {[0.1 \times (1 + N)]^2} \times {\rm{diag}}([1,1,0.01,0.01])$，量测噪声协方差矩阵${{\boldsymbol{R}}_k} = [(1 + M)^2]$，过程噪声驱动矩阵${\boldsymbol{\Phi}}_k={\rm{diag}}([0.25{T},0.25{T},0.02{T}, 0.02{T}])$，初始误差协方差矩阵${P_0} = {\rm{diag}}([0.5,0.5,0.1, 0.1])$。第一刻度因子$\alpha $控制Sigma点围绕均值点的分布范围，取值范围通常为[0.001,1]，$\alpha $值越小，数值越稳定，这里设置$\alpha = 0.001$。第二刻度因子$\beta $是用来包含先验知识的参数，对于高斯分布$\beta = 2$是最优选择。第三刻度因子$\kappa $是一个次要参数，用于调整Sigma点的分布，在标准UKF实现中通常设置为$\kappa = 0$。下述各参数是通过多次仿真对比滤波精度得到的最佳取值，放大系数$m = 1.2$，滑动窗口大小$w_s = 10$（若噪声环境与本实验设置的噪声环境区别较大，为了区分量测噪声的变化，滑动窗口和放大系数可能需要进行适当的调整，以达到最佳的鲁棒性和定位精度），自适应因子${b_1} = 0.86$，自适应因子$ {b_2} = 0.956 $。

通过上述设置进行实验仿真，以比较标准UKF算法、Sage-Husa-UKF算法和AT-ISHUKF算法在噪声统计特性时变且未知情况下的性能。

仿真结果如图4～图6所示，在噪声统计特性时变且未知的情况下，航推的定位精度随航行时间增加而逐渐发散，这是因为航推定位误差会随时间逐渐累积。而其他3种算法在航行前1200 s内的定位精度非常高且没有显著差异，在航行1200 s后定位精度开始下降并出现差异，其中AT-ISHUKF算法定位精度最高，Sage-Husa-UKF算法次之，UKF算法最低。这是因为UKF算法具有优良的性能，UKF通过无迹变换（UT变换）生成一系列采样点，能够准确捕捉到系统状态的主要变化特征，即使系统噪声与实际噪声不完全匹配，仍能通过这些采样点来有效地估计系统状态，故在1200 s前3种算法具有相差无几的定位精度。但由于系统噪声与实际噪声自始至终都不匹配，随着航行时间推移，噪声不匹配产生的影响会逐渐累积，达到一定程度后，3种算法的定位精度还是会出现不同程度的下降，因为UKF算法不能动态调整系统的噪声统计特性，因此不能适应噪声的变化，致使定位精度下降最大。而Sage-Husa-UKF算法能够实时估计和更新系统的噪声统计特性，降低了系统噪声与实际噪声的不匹配程度，因此Sage-Husa-UKF算法性能要比UKF算法优越。在实际情况中，有时观测噪声统计特性变化有可能会非常剧烈，若需要保证Sage-Husa-UKF算法的稳定性，自适应因子的取值范围通常为0.95～0.99，这会使Sage-Husa-UKF算法需要更长的时间来适应新的噪声统计特性，导致滤波器收敛速度变慢，从而影响定位精度。而AT-ISHUKF算法可以通过自适应阈值判定机制来判定噪声统计特性变化是否剧烈，若判定噪声统计特性变化剧烈，则采用更小的自适应因子（实验仿真取值为0.86）来快速适应新的噪声统计特性，从而系统噪声与实际噪声的不匹配程度比Sage-Husa-UKF算法更低，因此在噪声统计特性变化剧烈的情况下，AT-ISHUKF算法性能优于Sage-Husa-UKF算法。

表1为3种算法在噪声统计特性时变且未知情况下的性能比较结果，ALE为平均定位误差，RMSE为均方根误差。

可知，在噪声统计特性时变且未知的情况下，AT-ISHUKF算法的平均定位误差比标准UKF算法减少了59.5%，均方根误差减少了72.9%；平均定位误差比Sage-Husa-UKF算法减少了24.9%，均方根误差减少了30.6%。在这种情况下，AT-ISHUKF算法相较于其他2种算法具有更加精确的定位精度和更加优良的鲁棒性。

3.3 噪声统计特性突变情况下仿真分析

在噪声统计特性时变且未知的基础上，过程噪声统计特性和观测噪声统计特性先后突变为原来的5倍，即在1～700 s内，噪声统计特性为3.2节所述的${{\boldsymbol{Q}}_k}$和${{\boldsymbol{R}}_k}$，在701～1400 s内，噪声统计特性为$5{{\boldsymbol{Q}}_k}$和${{\boldsymbol{R}}_k}$，在1401～2000 s内，噪声统计特性为$5{{\boldsymbol{Q}}_k}$和$5{{\boldsymbol{R}}_k}$。

通过上述设置进行实验仿真，以比较标准UKF算法、Sage-Husa-UKF算法和AT-ISHUKF算法在噪声统计特性时变且未知的基础上，噪声统计特性突变为原来5倍时的性能。

仿真结果如图7～图9所示，航行时间在1～1400 s时，3种算法均保持收敛且定位误差波动平稳，说明在过程噪声的统计特性突变为原来的5倍之后，3种算法仍具有较强的稳定性。在航行时间超过1400 s之后，UKF算法定位精度开始趋于发散，而Sage-Husa-UKF算法和AT-ISHUKF算法定位精度下降，其中AT-ISHUKF算法定位精度高于Sage-Husa-UKF算法。说明当观测噪声和过程噪声的统计特性突变为原来的5倍之后，由于UKF算法不能实时估计和更新系统的噪声统计特性，导致系统的噪声统计特性与实际的噪声统计特性严重不匹配，致使定位精度逐渐发散。而Sage-Husa-UKF算法和AT-ISHUKF算法能够实时估计和更新系统的噪声统计特性，降低了系统噪声与实际噪声的不匹配程度，因此只是定位精度下降，而AT-ISHUKF算法的定位精度仍高于Sage-Husa-UKF算法（原因与3.2节所述相同），说明在噪声统计特性突变的情况下，AT-ISHUKF算法的性能优于Sage-Husa-UKF算法。

表2～表5为在噪声统计特性突变情况下3种算法的性能比较结果。

由表2可知，在噪声统计特性未突变时，3种算法在航行初期的性能差异不大。由表3可知，在过程噪声统计特性突变时，3种算法的定位精度均下降，其中UKF算法的定位精度下降最大，Sage-Husa-UKF算法其次，AT-ISHUKF算法最小。由表4可知，在过程噪声和观测噪声的统计特性先后突变时，UKF算法的定位精度发散，AT-ISHUKF算法和Sage-Husa-UKF算法定位精度均下降，但AT-ISHUKF算法比Sage-Husa-UKF算法的定位精度更高，说明在过程噪声和观测噪声的统计特性先后突变时，AT-ISHUKF算法的性能优于Sage-Husa-UKF算法。由表5可知，在噪声统计特性突变情况下，此时UKF算法发散，AT-ISHUKF算法的平均定位误差比Sage-Husa-UKF算法减少了48.9%，均方根误差减少了61.7%。由此可知，在这种情况下，AT-ISHUKF算法相较于其他2种算法同样具有更加精确的定位精度和更加优良的鲁棒性。

4 结　语

本文提出一种基于自适应阈值判定与改进Sage-Husa无迹卡尔曼滤波的AUV协同定位算法。通过引入统计特性高度时变的高斯噪声模型，比固定统计特性的理想噪声模型更加精确地模拟了真实海洋环境中噪声的时变性，并在此高斯噪声模型的基础上对AT-ISHUKF算法进行了仿真验证。仿真结果表明，在噪声统计特性时变且未知的情况下，AT-ISHUKF算法相较于Sage-Husa-UKF算法和UKF算法，具有更高的定位精度和更优的鲁棒性。特别是在噪声统计特性发生突变时，AT-ISHUKF算法仍能保持良好的定位精度，而UKF算法则出现发散现象。综上所述，本研究为AUV在复杂海洋环境中的高精度定位和可靠导航提供了重要技术支撑。