水下目标跟踪粒子滤波算法性能分析*

张程振，丁元明*，杨 阳

（1.大连大学信息工程学院，辽宁 大连 116622；2.大连大学通信与网络重点实验室，辽宁 大连 116622）

0 引言

AUV 在港口安全防御、雷区探测、水下目标打击等有着重要的应用，其中热点之一就是利用AUV 进行水下目标跟踪。经典的卡尔曼滤波器（Kalman filter）在目标跟踪领域得到了广泛的应用，但在实际应用中需要解决系统非线性与非高斯环境问题，为此出现了经典的改进算法，如扩展卡尔曼滤波算法（extended Kalman filter，EKF）、无迹卡尔曼滤波（ynscented Kalman filter，UKF）等。解决非线性滤波问题的方法还有序列蒙特卡罗方法（sequential monte Carlo methods，SMC），即粒子滤波算法（particle filter，PF）。其正式建立应归功于Gordon 等人将重采样技术（resampling）引入蒙特卡罗采样过程，有效解决了样本权值退化问题。

在现代信号处理、人工智能、目标跟踪等领域中，粒子滤波算法的应用不断扩展。Isard和Blake 首先提出Condensation 算法利用粒子滤波解决目标跟踪问题，在该算法中，使用先验概率密度作为重要性采样密度函数，从中抽取粒子，使用得到的样本集合近似表示后验概率密度，但只是适用于简单场景下的目标跟踪。Wang 等人针对纯方位目标跟踪问题，提出了融合当前时刻观测信息的卡尔曼粒子滤波算法，其跟踪效果与传统卡尔曼滤波算法相比具有明显的优势，但是没有考虑目标距离信息。Liu 等人提出一种基于自适应马尔科夫链的粒子滤波跟踪算法，能够以较少的迭代次数使粒子滤波收敛于后验分布，在实时环境中跟踪机动目标获得较高的准确率，但只是二维状态下的目标跟踪。刘畅等针对粒子滤波中传统重采样存在的滤波性能不稳定、有效粒子数波动剧烈、样贫的缺点，提出了一种基于Chopthin 重采样粒子滤波的目标跟踪算法，其能够改善传统重采样存在的样贫，但是没有考虑水下噪声的情况。刘红庆等提出一种基于高斯Monte Carlo 粒子滤波的机动目标跟踪算法，提高了机动目标跟踪精度和跟踪效率，但只考虑了高斯白噪声。

选择适用的算法对目标跟踪具有决定性的作用，但是大多数目标跟踪研究很少考虑噪声类型对算法的影响，以及算法跟踪的时效性问题。PF、EPF和UPF 算法是适用于非高斯噪声条件下的非线性系统滤波，本文研究了EPF和UPF 的关系，建立了水下三维目标跟踪模型，分析比较了3 种滤波算法在高斯噪声和散粒噪声下目标跟踪的能力。

1 数学建模

1.1 目标运动分析

考虑到AUV 最初处于（r，r，r）位置，并且假设目标运动是近似直线运动，使速度保持恒定值。目标在时刻的状态向量表示为：

其中，r（），v（）表示目标在轴方向的位置和速度，轴和轴同理。

基于当前时间状态向量计算下一时间的相对状态向量，如式（2）所示：

式中，Φ 是按照公式计算的状态转移矩阵：

（）是过程噪声，Γ 是过程噪声驱动矩阵：

过程噪声的协方差矩阵按公式计算：

式中，表示过程噪声中的方差，是一个可以调节的参数，且＜＜1。

AUV 观测方程式仅有距离，且观测方程表示为：

式中，

1.2 粒子滤波算法

粒子滤波（PF）的基本思路是选取重要性概率密度并从中随机抽样，取得带有权值的样本后，在状态观测的基础上调节粒子的位置和权重的大小，再使用这些样本近似逼近状态后验分布，最后把样本进行加权求和作为状态的估计值。

PF 算法流程如下：

1.3 扩展粒子滤波算法

扩展粒子滤波（EPF）是一种局部线性化的方法，它通过一阶Taylor 展开式实现，是一种递归的最小均方误差MMSE 估计方法，要求系统是近似高斯后验分布模型。EPF 算法在采样阶段，可以利用EKF 算法为每个粒子计算均值和协方差，然后利用该均值和方差“指导”采样。

EPF 算法流程如下：

1.4 无迹粒子滤波算法

无迹粒子滤波UPF 与EKF 不同的是，它并不是对非线性方程和在估计点处做线性化逼近，而是利用无迹变换（UT）在估计点附近确定采样，用这些样本点表示的高斯密度近似状态的概率密度函数。

UPF 算法流程如下：

2）For=1：

重要性采样阶段：

For=1：

用UKF 算法计算均值和方差。

计算Sigma 点集合。

对2+1 个Sigma 点集做一步预测：

计算系统状态量的一步预测和协方差矩阵：

根据一步预测值，再次使用无迹变换，产生新的Sigma 点集：

由上一步预测的Sigma 点集代入系统观测函数式（6）中，得到预测的观测量，=1，2，…，2+1。

由上一步得到的Sigma 点集的观测预测值，通过加权求和得到系统预测的均值及协方差：

融入最新的观测，并更新。

互相关协方差为：

计算卡尔曼增益矩阵：

计算系统的状态更新和协方差更新：

计算采样更新粒子。

For=1：，为每个粒子重新计算权重。

For=1：，归一化权重。

3）选择阶段（重采样）：

1.5 扩展粒子滤波与无迹粒子滤波的关系

讨论EPF和UPF 的理论关系，其实就是讨论EKF和UKF 的关系。UKF 的交叉协方差矩阵由Sigma 点近似，如式（33）所示。可以在状态空间模型是线性的假设下导出以下方程：

因此，EKF和UKF 具有相同的卡尔曼增益意义，即，

P为噪声的协方差矩阵。从导数和状态估计器中，可以发现非线性模型状态估计中UKF和EKF之间的一些理论关系：

1）UKF和EKF 都在卡尔曼滤波框架内运行。估计状态的准确性取决于预测状态X、预测测量值Z以及卡尔曼增益矩阵。

2）在大多数非线性状态下，UKF 比EKF 具有更高的预测精度。UKF 的状态均值和协方差矩阵是通过非线性函数传播的加权Sigma 点得到。传播过程UT 变换避免了线性化误差的引入，从而降低了滤波器的性能。

3）预测构成新息向量，影响最终估计的准确性。UKF 的预测测量值Z的精度比EKF 的精度高。UKF 先通过系统动力学模型，然后通过观测模型，用Sigma 点逼近预测测量值。第1 次传播接收系统预测状态，如2）中提到的。第2 次传播，即变换的Sigma 点通过非线性观测模型传播，得到预测的测量值。然而，扩展卡尔曼滤波是通过线性化模型、系统模型和观测模型来获取预测值，在预测中存在线性化带来的误差。

4）估计状态X的可靠性依赖于卡尔曼增益矩阵K，EKF 使用预测协方差矩阵P和P以及线性化的设计矩阵H来计算卡尔曼增益。上述3 项均存在线性化误差。然而，UKF 通过加权Sigma 点获得交叉协方差矩阵P和预测测量协方差矩阵P。这些点可以用来准确地计算二阶的均值和协方差。

5）与EKF 相比，UKF 估计过程不需要非线性方程的雅可比矩阵。

6）UKF 通过与系统状态向量维数相关的非线性函数迭代传播Sigma 点，而EKF 只需要计算一次雅可比矩阵。

7）UKF 计算状态协方差矩阵的平方根来采样Sigma 点，目的是匹配当前的统计特性。Cholesky 分解是实现正定矩阵平方根的一种有效方法。然而，分解需要数值的稳定性，因为UKF 的迭代并不总是保证产生正定矩阵。

1.6 高斯噪声和散粒噪声

高斯白噪声是一种简单的随机过程，是由一系列不相关的随机变量组成的理想化随机过程。散粒噪声（SN）通常是由于信号本身和统计交互过程中的统计波动造成的，是有色噪声的一种。在SN 的情况下，噪声幅度突然增大或减小。高斯噪声样本的幅度分布在-1～1 之间，噪声幅度均值为零；SN 样本是振幅大于1，为幅度较高的噪声，并且噪声幅度的平均值也不为零。

2 目标跟踪仿真结果分析

2.1 模型初始化

设定样本的采样间隔是1 s，模拟时间为60 s，粒子数目=50，初始化EPF、UPF 中协方差矩阵P（6），同时设置UT 变换中的相关系数=1、=0、=2，维数=1。针对两种观测噪声，分别用3 种算法做50 次蒙特卡罗仿真，得出某一次实验下的跟踪图像、位置误差和3 种算法的RMSE 曲线图。

2.2 仿真结果

2.2.1 高斯噪声实验

图1 给出了在某一次实验中，PF、EPF和UPF算法的跟踪曲线，可以看出EPF 的跟踪效果最好，PF 次之，EPF 最差且有局部震荡。下页图2 是该次实验对应每一时刻的位置偏差，EPF 算法有随时间增长的局势。

图1 高斯噪声实验下的目标跟踪曲线图

图2 高斯噪声实验下的跟踪误差曲线图

图3 是50 次仿真实验，3 种算法的RMSE 值的分布。对于相同的场景，UPF和EPF 相比，EPF 的跟踪效果是不稳定的，且RMSE 值整体偏大。PF 算法和EPF 算法整体的RMSE 值都保持在较低的水平。图4 给出了3 种算法的运行时间，可以看到PF 算法运行时间最少，UPF和EPF 算法时间相当。

图3 高斯噪声背景下的RMSE

图4 高斯噪声运行时间曲线

2.2.2 散粒噪声实验

SN 的实现可以在MATLAB 中使用“randn”来设计，其中高斯噪声与一些随机的高幅噪声相加，设计的SN 如下所示：

图5～图8 给出了PF、EPF和UPF 算法在散粒噪声下的仿真结果。可以看出，3 种算法在前期保持较低的误差估计，随着时间推移，UPF和PF 算法的误差有上升趋势，EPF 的跟踪误差大，且不稳定，RMSE 值整体偏大。PF和UPF 算法都能较好地实现目标跟踪，且RMSE 值保持在较低的水平。算法的处理时间和高斯情况下基本一致。

图5 散粒噪声实验下的目标跟踪曲线图

图6 散粒噪声实验下的跟踪误差曲线图

图7 散粒噪声背景下的RMSE

图8 散粒噪声运行时间曲线

2.2.3 不同过程噪声实验

给出PF、EPF、UPF 的误差如表1 所示。从表1中可以看出，当系统过程噪声的值较小时，UPF算法的性能是较优越的，然而随之值的增大，UPF 算法的表现低于EPF。3 种算法在应对比较大的过程噪声时的表现，PF 是最差，EPF 最优。

表1 PF、EPF、UPF 误差表

3 结论

本文比较分析了PF、UPF和EPF 算法在高斯噪声和散粒噪声情况下AUV 目标跟踪的性能。用不同的噪声条件验证了3 种滤波器的跟踪精度。在高斯噪声中，UPF和PF 获得了精确的估计，EPF 误差较大；在散粒噪声中，EPF 估计不稳定，PF和UPF的精度都是可以接受的，保持在较低的水平。

当AUV 系统测量噪声不变时，只改变目标过程噪声的值，随着的增大，目标的运动轨迹将不再是直线，而是弯曲的轨迹，EPF 的跟踪性能要明显优于PF，且比UPF 优。UPF 在两种噪声条件下表现出了均衡的性能。在实际中，如果目标的状态噪声较大，可以考虑使用EPF 算法，如果对时效性有需求，且目标状态噪声较小时，可以使用PF 算法，如果没有时效性需求，UPF 算法较优越。