0 引　言

随着海洋资源开发、环境监测及军事应用等领域的快速发展，无人水面艇（Unmanned Surface Vehicle，USV）凭借其灵活性与自主性，已成为海洋探测与任务执行的核心平台之一。在多USV协同作业场景中，精准的位置感知是确保任务成功率的基础。然而，复杂海洋环境下传感器噪声的不确定性（尤其是水声测距噪声的时变特性），严重制约了协同定位系统的可靠性。传统的协同定位算法通常假设噪声统计特性已知且服从固定分布，这一理想化条件在实际应用中难以满足，导致定位误差累积甚至系统发散。

USV协同定位依赖水声设备进行测距和通信，增强了系统的抗干扰能力，但同时也引入了不可避免的水声噪声的影响。水声信道是一种典型的时变多径通道，测距和通信信号可能包含直接路径、多路径和异常值3种情况^{[1 − 4]}。直接路径是正常测量与通信的理想状态，多路径和异常值的影响会导致水声测距时呈现非高斯分布特性，出现概率分布的尾部比高斯分布重的重尾噪声。这一现象在浅海或障碍物密集区域尤为显著，导致传统基于高斯假设的滤波算法产生系统性偏差。传统的非线性协同定位研究多采用扩展卡尔曼滤波器（EKF）或无迹卡尔曼滤波器（UKF）进行数据融合，但在重尾非高斯观测噪声的情况下，这些方法的估计性能显著下降，甚至可能导致滤波器的不稳定^{[5 − 6]}。因此，USV的协同定位算法需要增强其鲁棒性，以降低未知重尾噪声对定位精度的影响，从而提升系统的整体性能与稳定性^{[7 − 9]}。

当前，国内外在协同定位研究中大多假设测量噪声为高斯分布，而针对未知重尾分布情况下的USV协同定位算法还有待深入探究^[10]。例如Webster^[11]研究了基于扩展卡尔曼滤波（Extended Kalman Filter，EKF）的协同定位算法，研究结果表明EKF算法能够有效实现多艇的协同定位。但是该算法面对未知重尾噪声时性能大大降低，严重影响了协同定位精度。值得注意的是，现有文献中基于变分推断的滤波方法（如变分贝叶斯自适应卡尔曼滤波）虽在机器人SLAM领域展现了噪声参数联合估计的优势，但其在USV动态协同定位中的适用性尚未得到充分验证。

本文以未知重尾噪声为前提，提出一种基于变分贝叶斯卡尔曼滤波（Variational Bayesian Kalman Filter，VBKF）的自适应协同定位框架。利用变分贝叶斯卡尔曼滤波进行协同定位，通过迭代方式计算得到USV的后验位置估计，从而抑制异常值对系统的影响，进而提高USV的协同定位精度。与现有方法相比，本文的创新点在于：摒弃传统固定噪声协方差的强假设，通过变分贝叶斯实现噪声参数与状态量的同步在线估计；通过实船试验验证算法对时变重尾噪声的鲁棒性，为复杂海洋环境下的USV协同定位提供了新的技术路径。

1 协同定位系统数学模型 1.1 USV运动学模型

由于无人水面艇（USV）的运动特性受水面约束，其动力学行为通常可简化为二维平面内的刚体运动。基于此物理特性，本研究忽略高度维度的动态耦合效应，将多USV协同定位问题聚焦于二维平面分析。通过定义平面坐标系下的状态向量，系统模型可表述为以下一般形式：

$ \left\{ {\begin{split} &\dot x = v\cos \theta ，\\ &\dot y = v\sin \theta 。\end{split}} \right. $

(1)

式中：(x, y)为导航坐标系内USV的位置坐标；v为USV在航行时的合速度；$\theta $为USV的航向。

在k时刻USV测量到的速度表示为${v_k}$，此时USV航向表示为${\theta _k}$，USV在导航坐标系内的位置表示为$({x_k}, {y_k})$，根据式(1)可以得到USV定位问题的离散形式：

$ \left\{ {\begin{split} & {x_k} = {x_{k - 1}} + {v_k}\Delta t\cos {\theta _k}，\\ & {y_k} = {y_{k - 1}} + {v_k}\Delta t\sin {\theta _k} 。\end{split}} \right. $

(2)

式中：$\Delta t$为定位过程中的采样周期。

1.2 基于水声测距的量测模型 1.2.1 水声测距原理

水声测距是基于水下声波传播特性实现空间距离测量的核心技术，其原理可概括为 “时延-声速”耦合模型：通过测量声波信号在水介质中传播的时间延迟（Time of Flight，ToF），结合声速剖面（Sound Velocity Profile，SVP）构建的距离解算方程，获取两点间的相对空间距离。具体数学表征如下：

当水声测距的发射端与接收端间距为$ d $，声波传播时间为$ \Delta t $，则理想条件下距离可表示为：

$ d = c \cdot \Delta t 。$

(3)

式中：$ c $为声波在介质中的传播速度，m/s。然而，实际水下环境中声速受温度$ T $、盐度$ S $与压力$ P $，即深度）共同影响，需采用经验公式动态修正，水声测距的主要误差项包括：声速剖面空间非均匀性（温盐跃层效应）导致的$ c $建模误差；多径传播引起的时延估计模糊（尤其浅水或复杂地形）；收发换能器指向性偏差与平台姿态扰动；环境噪声（生物声源、船舶噪声）对信号检测概率的影响。

1.2.2 量测模型

本文围绕主从式USV协同定位系统展开研究，其架构设计遵循以下原则：主艇配备高精度GNSS/INS组合导航系统，从艇搭载低成本MEMS-IMU导航模块。基于异构平台的导航精度差异，在协同定位过程中，主艇位置误差可视为小量予以忽略。通过主从式拓扑结构，系统利用水声测距信息建立相对观测约束，具体表示为：

$ z_k^{} = \sqrt {{{\left( {{x_k} - x_k^m} \right)}^2} + {{\left( {{y_k} - y_k^m} \right)}^2}} 。$

(4)

式中：$(x_k^m,y_k^m)$为k时刻主艇的位置坐标；${z_k}$为主从艇间的相对距离。

2 基于变分贝叶斯卡尔曼滤波的协同定位算法

经典卡尔曼滤波器（Kalman Filter，KF）基于最小均方误差准则，通过预设过程噪声协方差矩阵Q和量测噪声协方差矩阵R实现状态最优估计，其理论有效性严格依赖于噪声统计特性（即Q、R）的精确先验建模。然而，在USV协同定位系统中，水声测距噪声受时变多径效应与突发环境干扰影响，呈现显著的非高斯重尾特性，导致传统KF的固定噪声参数假设失效。为此，自适应卡尔曼滤波（Adaptive Kalman Filter，AKF）框架被提出，其核心思想是通过在线噪声估计动态调整Q与R。早期AKF方法（如Sage-Husa算法）采用滑动窗口（窗口长度N=10～50）或指数遗忘因子更新噪声协方差，但这些方法仍在面对重尾分布时会产生协方差膨胀现象。为提升非高斯噪声下的鲁棒性，研究者融合鲁棒统计理论改进AKF：Huber-based AKF（HAKF）利用广义最大似然估计（M-estimation）构造抗差代价函数，通过Huber阈值γ（通常经验设置为γ=1.345）抑制粗差影响，但其性能对γ的敏感性较高；变分贝叶斯卡尔曼滤波通过引入逆Wishart先验分布对Q、R进行概率建模，并利用变分下界最大化实现状态与噪声参数的联合推理，理论上可兼容任意形式的噪声分布^[12]。

2.1 自适应卡尔曼滤波器

卡尔曼滤波器是从线性高斯状态空间模型（Linear Gaussian State-Space Model，LG-SSM）中的噪声测量值估计动态状态的经典算法。从概率的角度来看，该模型可以表示为：

$ {{\boldsymbol{x}}_k}\sim\mathbb{N}\left( {{{\boldsymbol{A}}_{k - 1}}{{\boldsymbol{x}}_{k - 1}},{{\boldsymbol{Q}}_{k - 1}}} \right) ，$

(5)

$ {{\boldsymbol{y}}_k}\sim \mathbb{N}\left( {{{\boldsymbol{H}}_k}{{\boldsymbol{x}}_k},{{\bf{\varSigma}} _k}} \right)。$

(6)

式中：$\mathbb{N}( \cdot )$为多元高斯分布；${{\boldsymbol{x}}_k} \in {\mathbb{R}^n}$为动态为${{\boldsymbol{A}}_{k - 1}}$为动态模型矩阵为${{\boldsymbol{Q}}_{k - 1}}$为过程噪声协方差为${{\boldsymbol{y}}_k} \in {\mathbb{R}^d}$为测量值为${{\boldsymbol{H}}_k}$为测量矩阵；${{\boldsymbol{\varSigma}} _k}$是测量噪声协方差矩阵。这里，${{\boldsymbol{x}}_k}$是未知变量，${{\boldsymbol{y}}_k}$是观测变量，而矩阵${{\boldsymbol{A}}_{k - 1}}、{{\boldsymbol{Q}}_{k - 1}}、{{\boldsymbol{H}}_k}$和${\Sigma _k}$已知。并进一步假设${{\boldsymbol{x}}_0}\sim \mathbb{N}\left( {{{\boldsymbol{m}}_0},{{\boldsymbol{P}}_0}} \right)$，其中${{\boldsymbol{m}}_0}$和${{\boldsymbol{P}}_0}$是已知的先验均值和协方差。使用2个卡尔曼滤波器步骤（时间更新与量测更新）递归执行状态估计，该算法递归地解决了一般的多元线性估计（回归）问题。当不假设状态是静态的（如RLS）时，矩阵${{\boldsymbol{A}}_{k - 1}}$和${{\boldsymbol{Q}}_{k - 1}}$可用于对状态的动态进行建模。

2.2 变分贝叶斯协同定位算法

在上述卡尔曼滤波器中，假设状态空间模型中的模型矩阵已知。在噪声协方差${{\boldsymbol{\varSigma}} _k}$未知的情况下，考虑变分贝叶斯自适应卡尔曼滤波器。假设模型具有以下形式^[13]：

$ {{\boldsymbol{x}}_k}\sim \mathbb{N}\left( {{{\boldsymbol{A}}_{k - 1}}{{\boldsymbol{x}}_{k - 1}},{{\boldsymbol{Q}}_{k - 1}}} \right)，$

(7)

$ {{\boldsymbol{y}}_k}\sim \mathbb{N}\left( {{{\boldsymbol{H}}_k}{{\boldsymbol{x}}_k},{{\boldsymbol{\varSigma}} _k}} \right)，$

(8)

$ {{\boldsymbol{\varSigma}} _k}\sim p\left( {{{\boldsymbol{\varSigma}} _k}|{{\boldsymbol{\varSigma}} _{k - 1}}} \right)。$

(9)

式中：$ {{\boldsymbol{\varSigma}} _k}\sim p\left( {{{\boldsymbol{\varSigma}} _k}|{{\boldsymbol{\varSigma}} _{k - 1}}} \right) $，定义了未知测量噪声协方差的马尔可夫动态模型先验。如果能够为该模型实现最优（非高斯）贝叶斯滤波器，将有计算式为：

$ p\left( {{{\boldsymbol{x}}_k},{{\boldsymbol{\varSigma}} _k}|{{\boldsymbol{y}}_{1:k}}} \right)。$

(10)

从优化理论视角，卡尔曼滤波器可被理解为动态系统下的递归最小二乘（RLS）估计器的广义形式。具体而言，若将状态空间模型中的过程噪声协方差矩阵${{\boldsymbol{Q}}_k}$设为零矩阵，且观测噪声协方差${{\boldsymbol{R}}_k}$退化为标量常数，则卡尔曼滤波的更新方程将等价于带遗忘因子的RLS算法。在贝叶斯滤波框架下，协方差矩阵的自适应估计可被重新参数化为时变噪声方差的线性回归问题。这一形式化关联表明，传统RLS可视为卡尔曼滤波在静态系统与各向同性噪声假设下的特例。

尽管上述简化模型为协方差估计提供了递归闭式解，但其局限于静态或准静态场景，无法充分描述USV协同定位系统的动态特性（如运动耦合与非平稳噪声）。因此，本研究利用全维状态空间模型构建自适应协同定位算法。

然而上述模型的精确贝叶斯滤波器在计算上很难处理。因此状态和协方差矩阵的联合滤波分布可以用自由形式的变分贝叶斯近似来近似，计算式如下：

$ p\left( {{{\boldsymbol{x}}_k},{{\boldsymbol{\varSigma}} _k}|{{\boldsymbol{y}}_{1:k}}} \right) \approx {Q_{\boldsymbol{x}}}\left( {{{\boldsymbol{x}}_k}} \right){Q_\Sigma }\left( {{{\boldsymbol{\varSigma}} _k}} \right)。$

(11)

式中：${Q_{\boldsymbol{x}}}\left( {{{\boldsymbol{x}}_k}} \right)$和${Q_{\mathbf{\Sigma }}}\left( {{\Sigma _k}} \right)$是未知的近似密度，通过最小化真实分布和近似值之间的Kullback-Leibler（KL）散度而形成：

$ \begin{split} &\text{KL}\left[{Q}_{x}\left({x}_{k}\right){Q}_{\Sigma }\left({{\boldsymbol{\varSigma}} }_{k}\right)\Vert p\left({x}_{k},{{\boldsymbol{\varSigma}} }_{k}|{y}_{1:k}\right)\right]=\\ &\int {{Q_{\boldsymbol{x}}}} \left( {{{\boldsymbol{x}}_k}} \right){Q_\Sigma }\left( {{{\boldsymbol{\varSigma}} _k}} \right)\log \left( {\frac{{{Q_{\boldsymbol{x}}}\left( {{{\boldsymbol{x}}_k}} \right){Q_{\mathbf{\Sigma }}}\left( {{\Sigma _k}} \right)}}{{p\left( {{{\boldsymbol{x}}_k},{{\boldsymbol{\varSigma}} _k}|{{\boldsymbol{y}}_{1:k}}} \right)}}} \right){\text{d}}{\boldsymbol{x}}{\text{d}}{{\boldsymbol{\varSigma}} _k}。\end{split} $

(12)

使用变分法的方法，可以将上述KL散度相对于$ {Q_{\mathbf{x}}}\left( {{{\boldsymbol{x}}_k}} \right) $和$ {Q_{\mathbf{\Sigma }}}\left( {{{\boldsymbol{\varSigma}} _k}} \right) $最小化，从而得到以下近似值：

$ {Q_{\boldsymbol{x}}}\left( {{{\boldsymbol{x}}_k}} \right) \propto \exp \left( {\int {\log } p\left( {{{\boldsymbol{x}}_k},{{\boldsymbol{\varSigma}} _k}|{{\boldsymbol{y}}_{1:k}}} \right){Q_{\mathbf{\Sigma }}}\left( {{{\boldsymbol{\varSigma}} _k}} \right){\text{d}}{{\boldsymbol{\varSigma}} _k}} \right)，$

(13)

$ {Q_\Sigma }\left( {{{\boldsymbol{\varSigma}} _k}} \right) \propto \exp \left( {\int {\log } p\left( {{{\boldsymbol{x}}_k},{{\boldsymbol{\varSigma}} _k}|{{\boldsymbol{y}}_{1:k}}} \right){Q_{\boldsymbol{x}}}\left( {{{\boldsymbol{x}}_k}} \right){\text{d}}{{\boldsymbol{x}}_k}} \right)。$

(14)

求解上述方程得到以下近似值：

$ p\left( {{{\boldsymbol{x}}_k},{{\boldsymbol{\varSigma}} _k}|{{\boldsymbol{y}}_{1:k - 1}}} \right) \approx \mathbb{N}\left( {{{\boldsymbol{x}}_k}|, {{\boldsymbol{m}}_k},{{\boldsymbol{P}}_k}} \right)\mathbb{I}W\left( {{{\boldsymbol{\varSigma}} _k}|{v_k},{{\boldsymbol{V}}_k}} \right)。$

(15)

式中：${{\mathbf{m}}_k}$和${{\mathbf{P}}_k}$为标准卡尔曼滤波器给出；${v_k}$和${{\mathbf{V}}_k}$为逆Wishart分布的参数。例如，建议协方差可以计算为逆Wishart分布的平均值：

$ {{\boldsymbol{\varSigma}} _k} = \frac{1}{{{v_k} - d - 1}}{{\boldsymbol{V}}_k}。$

(16)

需要选择动态模型$p\left( {{{\boldsymbol{\varSigma}} _k}|{{\boldsymbol{\varSigma}} _{k - 1}}} \right)$，使其在贝叶斯滤波器预测步骤上产生逆Wishart分布，这种动态模型很难明确构建，因此进行了扩展。得到以下动态模型：

$ v_k^ - = \rho \left( {{v_{k - 1}} - d - 1} \right) + d + 1，$

(17)

$ \Sigma_k^-={B{\boldsymbol{}}}\Sigma_{k-1}{\boldsymbol{B}}^{\mathrm{T}}。$

(18)

式中：$v_k^ - $和$\Sigma _k^ - $为先验参数；$\rho $为实数$0 < \rho \leqslant 1$；${\boldsymbol{B}}$为矩阵$0 < |{\boldsymbol{B}}| \leqslant 1$。这里，参数$\rho $通过指数降低自由度来控制协方差矩阵先前估计的遗忘。矩阵${\boldsymbol{B}}$可用于对协方差矩阵的确定性动态进行建模。

由上述算法反复迭代计算，可得到k时刻USV位置的最优估计结果。

3 船载实验验证

为验证本文提出的未知噪声环境下USV自适应协同定位算法的有效性，设计并实施了基于实船验证试验的离线仿真分析。试验平台由2艘主艇和1艘从艇构成三节点协同定位网络，具体配置如下：

1）主艇导航基准：主艇搭载光纤惯性组合导航系统（GNSS定位精度：水平位置RMS ≤ 0.15 m，航向角精度 ≤ 0.1°），通过差分GPS提供厘米级参考轨迹；

2）从艇自主定位：从艇配备低成本磁罗经与多普勒计程仪（速度精度±0.05 m/s），构成航位推算系统（从艇配备差分GPS作为位置基准，便于离线分析）；

3）相对距离量测：主从艇间通过德国Evologics S2CR7/17水声通信机实现双向时延测距。

试验数据采集于太湖动态环境，截取连续1800 s的有效航行段进行算法验证。图1为试验期间各艇的航行轨迹。

试验过程中的水声测距误差如图2所示。可以看出，测距时经常会出现比较大的异常值，导致噪声分布呈现为未知特性。

通过对实验数据的离线处理，本文的VBKF自适应协同定位算法与传统的UKF、HIDDF、FGMC协同定位算法的误差对比如图3所示。

比较图2和图3 可以发现，在测距误差增大时，定位误差会受到较大影响，而本文所提的FGMC算法与传统算法相比定位误差明显减小。

各个算法的均方根误差如表1所示。

可以看出，本文所提的USV协同定位算法定位误差为15.95 m，均小于传统的UKF、HIDDF、FGMC算法。均方根误差与UKF、HIDDF、AKF、FGMC算法相比分别减少了58%、14%、6%、15%。

4 结　语

本文研究了未知重尾噪声下的USV协同定位算法，建立了USV协同定位系统的运动学模型，并设计了适合处理未知重尾噪声的变分贝叶斯卡尔曼滤波协同定位方法，船载试验结果验证了本文所提算法的有效性，在实验中均方根误差与UKF、HIDDF、FGMC算法相比分别减少了58%、14%、15%。

本文提出的算法具有较大的适用性，呈现重尾分布的测距误差对传统算法影响很大，而对本文所提算法影响较小，有利于提高USV协同定位能力。