交互多模自适应容积Kalman 滤波算法设计及履带车辆跟踪应用*

于镇滔，李若霆，王忠庆，刘 鹏，*，卢志刚

（1.中北大学，太原 030051；2.北方自动控制技术研究所，太原 030006）

0 引言

在野外环境中，复杂道路条件给轮式车辆行驶造成了困难，因此，常使用履带车辆执行各项军事任务。直线运动叠加履带侧滑提升了履带车辆轨迹的复杂性，且测量距离的增加降低了测量值的准确性。因此，亟待研究户外环境下履带车辆的跟踪算法。

由于履带车辆结构较为复杂，受力分析具有一定的复杂性，研究者对履带车辆运动模型的构建通常利用轮式车辆模型，或通过动力学平衡方程建立滑动操纵模型［1-4］。为简化迭代过程，本文采用轮式车辆模型描述运动轨迹。Jerk 模型对目标加速度变化率进行实时估计，常被用于车辆轨迹描述，但在实际过程中预先定义的参数是时变的，为此，提出了改进型Jerk 模型。文献［5］在对4 种α-Jerk 模型从理论分析和跟踪仿真中，发现所有模型在高机动阶段都会出现误差跳跃。文献［6］采用变维滤波算法跟踪运动目标，当检测到目标发生机动时变维滤波器采用机动模型，其他情况为非机动模型，但在模型切换过程中，初始状态值的重新确定会导致跟踪的中断。为克服单模型滤波算法不足，研究者们开始采用多模型方案。交互式多模型算法（interacting multiple model，IMM）依概率将多个运动状态加权融合获得状态值，广泛应用于对机动目标的跟踪。在最近的研究中，为减小跟踪误差，文献［7-8］从模型的种类和数量出发，采用包含自适应当前统计模型的交互多模自适应模型，以提高机动目标定位精度。此外，考虑到状态转移矩阵对切换速度的影响，文献［9］利用子模型在连续时间点对应概率值之差调整状态转移矩阵，改善了模型的切换速度和分配的合理性，对状态转移矩阵的改善有效提升了定位的精确度，但受限于滤波算法的性能。本文通过对子滤波算法进行改进，以此提升状态估计精度，从而减小跟踪误差。

在非线性系统中，容积Kalman 滤波（cubature Kalman filter，CKF）算法采用三阶球面-径向容积规则计算高斯积分，有效提升了状态估计精度，被广泛应用于目标跟踪中。文献［10］对目标状态观测的过程进行分析，发现了实验数据中存在乘性噪声，其导致测量值倍增或减小，产生异常数据。基于无线传感器网络的目标跟踪的研究中，对车辆进行观测所获得的测量值，会受到测量距离地增加或测量平台振动的影响，研究者将其归为乘性噪声［11］。为减小乘性噪声的影响，文献［12-13］分别针对测量值中含有有色乘性噪声或白色乘性噪声对CKF 算法进行改进，且均认为过程噪声和测量噪声不相关。文献［1，14］考虑到加性噪声和乘性噪声具有相关性，提出了一种最小均方差意义下的最优递推算法。

户外环境中，搭载传感器的车载网对履带车辆进行远距离观测，测量值不可避免地含有乘性噪声，与机动轨迹共同造成传统CKF 算法跟踪误差增加。本文通过分析和比较传统滤波算法误差协方差，依据噪声相关系数已知或未知对传统CKF 算法进行改进，与IMM 算法相融合后提出一种交互式自适应CKF（IMM-ACKF）算法，应用于户外环境下履带式车辆跟踪。

1 模型的建立和问题的提出

1.1 运动模型和测量模型的建立

基于容积Kalman 滤波算法的履带车辆定位需要有已知车辆运动模型和观测模型。履带车辆行驶在户外环境中，变量v 代表车辆移动速度，水平方向夹角θ 为车辆移动方向。对于履带车辆，其运动状态方程表示如下：

其中，Ai表示不同运动模型的状态转移矩阵；xk表示在k 时刻包含变量v 和θ 的状态量；ωk为期望为零且协方差为Qk的过程噪声。在户外环境中，车辆以匀速直线运动在预设轨迹上行驶，机动躲避各型障碍物。本文采用Jerk 模型描述这一机动轨迹，状态转移矩阵和过程噪声的构建具体可参考文献［15］。

考虑到户外环境对测量值的影响，无线传感器网络的测量模型不仅要考虑加性噪声对测量值的影响，还需要考虑由测量距离产生的乘性噪声对其的影响，因此，测量方程表示如下：

1.2 问题的提出

在使用Kalman 滤波进行状态估计时，使用误差协方差Pk判断滤波器性能。在只考虑加性噪声而不考虑相关乘性噪声的情况下，误差协方差可以表示如下：

其中，Hk为虚拟测量噪声；I 为单位矩阵；（·）T表示矩阵的转置。对于本文所使用的CKF 算法，虚拟测量矩阵Hk可以表示如下：

当系统中既有加性噪声又有乘性噪声时，式（3）所表示的误差协方差矩阵已经不能作为滤波器的性能指标。所以需要重新计算新环境下的估计误差，由此计算出真实的误差协方差矩阵。

将观测方程改写如下

其中，（·）m表示真实值。

对比式（3）和式（7），发现当系统含有乘性噪声后，传统的容积卡尔曼滤波算法的精度将难以得到保证。

此外，考虑到履带与地面的摩擦系数、电机控制不对称等原因会造成车辆平移，与正常的直线运动相叠加构成一种复合运动，并且非匀速机动轨迹对使用单模型进行描述的滤波算法会造成较大估计误差，故本文采用IMM 描述履带车辆运动模型。

2 自适应容积Kalman 滤波器

传统的目标跟踪通常不考虑传感器的测量距离对测量值的影响，为解决传统CKF 算法无法处理乘性噪声的问题，本文利用新息分析法提出了一种自适应CKF（adaptive CKF，ACKF）算法。本章分为两大部分，第1 部分为CKF 的核心基础；第2 部分提出在加性噪声和乘性噪声的相关系数已知和未知情况下的ACKF 算法。

2.1 容积Kalman 滤波

在非线性高斯滤波的测量更新过程中，变量的求解都会遇到“非线性函数高斯密度函数”形式的高斯积分，具体如下：

其中，u（x）包含了被积的非线性函数和部分高斯概率密度函数；Rn为积分域。令x=ry，且，式（8）通过坐标变换得：

其中，Un表示n 维单位球面；σ（·）表示n 维单位球面上的元素。

将式（9）拆分为球面积分和径向积分后，通过等式变换将径向积分转化为Gauss-Lacquerer 积分。分别计算球面积分和径向积分，将两部分结果相乘得到三次球面- 径向容积准则，最终式（8）表示如下：

基于三次球面- 径向容积准则，Ienkaran Arasaratnam 和Simon Haykin 提出了容积Kalman 滤波算法［16］。

2.2 自适应容积Kalman 滤波

2.2.1 已知参数dk的测量更新过程

则测量估计值表示如下：

含有加性噪声和相关乘性噪声的系统中，理论上自协方差矩阵可以由新息协方差矩阵表示：

其中

由此可得互协方差矩阵

进而状态更新过程如下：

2.2.2 参数dk未知的测量更新过程

其中，M 是滑动窗口的长度。通过k-1 时刻的新息协方差可得到迭代等式：

当乘性噪声的相关系数dk已知时，自协方差矩阵可由式（17）求得：

但在实际中噪声相关系数是未知或不确定。为了利用式（17）求解自协方差矩阵，重新确定相关系数。对式（25）两边同时求迹得：

其中，tr（·）表示矩阵的迹。

其余步骤沿用式（20）和式（21）。

3 交互式多模态自适应容积Kalman滤波

针对户外机动轨迹，使用单模滤波器进行目标跟踪会造成误差发散，本章采用IMM 提高运动模型匹配度。将ACKF 算法作为IMM 算法的子滤波算法，提出交互多模型自适应容积Kalman 滤波算法（IMM-ACKF）。

设计的IMM 算法包含3 个子滤波器，每个滤波器对应一个运动模型，其中一个使用匀速直线模型，状态转移矩阵为AV；剩余两个为匀加速模型，状态转移矩阵为AC，两者不同之处是过程误差协方差不同。当履带车辆发生机动时对应轨迹假设为曲线，采用双匀加速模型以提高IMM 算法对机动轨迹的匹配度［17］。相较于传统的IMM-CKF 算法，可有效地降低乘性噪声对车辆状态估计的影响。IMMACKF 算法流程图如图1 所示。

图1 交互式多模态自适应容积Kalman 滤波算法Fig.1 Interactive multimode adaptive cubature Kalman filtering algorithm

4 数值仿真

为了测试IMM-ACKF 算法在复杂路面环境中跟踪效果和对观测值中含有相关乘性噪声的有效处理，本章开展了两组对比。第1 组：对比在有无相关乘性噪声情况下，传统CKF 算法对目标跟踪信号的处理，以及展示ACKF 算法处理乘性噪声效果。第2 组：对比分别利用IMM-ACKF 算法和ACKF 算法对机动目标的跟踪效果。

4.1 参数设置

为了描述车辆在运行过程中的机动轨迹，在“几”字形轨迹的4 个拐弯处分别使用Jerk 模型产生圆弧轨迹代替原有的直线轨迹，以此验证IMM-ACKF 算法的定位性能。本文使用均方根误差作为跟踪效果评判标准，表示如下：

4.2 仿真结果讨论

通过仿真得到了不同滤波算法对应的跟踪轨迹，如图2 所示，其中，CKF-C 表示在相关乘性噪声下基于CKF 算法的跟踪结果。按照式（30）计算各参数跟踪轨迹的误差值，如图3 和表1 所示。

表1 跟踪误差统计Table 1 Tracking error statistics

图2 基于CKF 算法的车辆跟踪轨迹Fig.2 Vehicle tracking trajectory based on CKF algorithm

图3 车辆轨迹误差Fig.3 Vehicle track errors

通过对比两种不同行驶路径下的跟踪结果，由图2（a）可知，当履带车辆只进行直线运动，受到乘性噪声的影响，CKF 算法对应的跟踪轨迹极大地偏离预设轨迹，而ACKF 算法的跟踪轨迹与预设轨迹基本吻合。图3 中的误差值也佐证了这一点。由此可知，ACKF 算法有效地处理了测量值中的相关乘性噪声。但当车辆行驶在机动轨迹上时，ACKF 算法的跟踪轨迹偏离预设轨迹严重，ACKF 轨迹误差范围在0～20 m。

相比单模型ACKF 算法，IMM-ACKF 算法的跟踪误差大幅度降低，在机动和非机动轨迹交叉处的误差波动得到明显减弱，如表1 所示。

对比ACKF 算法和IMM-ACKF 算法的速度和方向角误差统计，在移动速度方面，IMM-ACKF 的估计较为准确且波动较小，ACKF 算法对应的估计值在较大范围内波动。而在方向角方面，相比ACKF 算法估计值，IMM-ACKF 的方向角误差值有较大波动。

5 结论

本文研究了履带车辆在复杂环境中的目标跟踪问题，由于传感器在测量距离内获得的测量值含有乘性噪声，跟踪误差增幅较大。采用新息分析法和滑动窗口法对带有乘性噪声的Kalman 增益求解过程进行改进，提出了一种ACKF 算法。通过对比匀速直线运动下的车辆跟踪结果，可知传统CKF 算法无法处理该乘性噪声，而ACKF 算法可以有效处理乘性噪声对目标跟踪带来的影响。进一步有效提出了IMM-ACKF 来处理履带车辆在室外的运动，通过对比可知，所提出的IMM-ACKF 算法有效解决了复杂环境下车辆跟踪模型不匹配问题，以及降低了乘性噪声对测量值的影响。