基于自适应正交单纯形CKF 的鲁棒目标跟踪方法*

郝思冲，陈树新，吴 昊，汪家宝，何仁珂

（空军工程大学信息与导航学院，西安 710077）

0 引言

目标跟踪系统在民用和军事领域都有着广泛的应用［1-3］，基于卡尔曼滤波的目标跟踪方法是其中较为成熟的应用方案。根据滤波方法的设计原则，系统模型和测量是滤波方法的两大信息来源，基于滤波方法的目标跟踪问题的本质是利用模型和量测信息估计目标的运动状态。在实际目标跟踪场景中，目标的运动方式与建立的系统模型可能出现不匹配的情况，导致目标跟踪精度的下降，甚至跟踪失效。同时，大部分目标跟踪属于非线性系统状态的最优估计问题。因此，对模型异常情况下的目标跟踪非线性状态估计的研究是提高目标跟踪鲁棒性的关键。

容积卡尔曼滤波（Cubature Kalman Filter，CKF）［4］是解决非线性状态估计问题的有效方法。但在目标跟踪系统中，传统CKF 的滤波精度会随着状态向量维数和测量方程非线性程度的增加而下降。为提高CKF 的滤波性能，文献［5］提出了球面单纯形径向容积卡尔曼滤波算法（Spherical Simplex-radial Cubature Kalman Filter，SSRCKF），在增加少量运算复杂度的情况下，有效提高了滤波估计精度。文献［6］将正交方法和球面单纯形容积准则相结合，提出了正交单纯形容积卡尔曼滤波算法（Orthogonal Simplex Cubature Kalman Filter，OSCKF），有效解决了由系统测量方程的高阶项引起的非局部采样效应。然而，当系统模型状态发生突变或模型不准确时，上述算法会出现滤波估计精度下降，甚至发散的情况，导致系统跟踪性能下降明显［7］。对于模型异常问题，文献［8］通过基于新息向量统计特性条件的卡方检验调节自适应因子的引入时间，用于解决惯性导航中的模型失配问题。文献［9］基于STF 滤波框架，采用无迹变换代替雅可比矩阵计算，在增强系统模型鲁棒性的同时，克服了传统STF 的局限。

针对非线性目标跟踪系统的模型异常问题，需考虑非线性状态估计和模型异常两方面难点。同时，系统的异常也导致滤波计算数值的不稳定，特别是基于采样型滤波方法，会出现协方差数值异常而不能进行三角分解的情况。上述研究并未较好地解决这些问题。本文提出一种自适应正交单纯形容积卡尔曼滤波算法（Adaptive Orthogonal Simplex Cubature Kalman Filter，AOSCKF）。AOSCKF 算法的核心思想是将强跟踪算法（Strong Tracking Filter，STF）［10-11］中的自适应因子与OSCKF 结合，既保留了OSCKF 滤波精度高、非线性适应能力强、可计算性强的优势，又可以解决系统模型异常的问题，使系统具有更强的跟踪能力。本文首先构建目标跟踪模型，在递归贝叶斯滤波框架的基础上，给出正交球面单纯形容积准则，之后在非线性强跟踪滤波的基础上，将自适应因子引入正交单纯形容积卡尔曼滤波中，提出自适应正交单纯形容积卡尔曼滤波算法，最后将其应用于系统状态存在突变的双站纯方位目标跟踪问题中。

1 系统模型

图1 双站纯方位目标跟踪系统示意图

2 正交单纯形容积卡尔曼滤波

由递归贝叶斯滤波框架［12］可知，求解上述系统模型的非线性滤波问题，可以转化为对多维高斯积分的求解。高斯积分在球面径向坐标系下［4］可表示为

三阶球面单纯形容积准则可表示为

正交单纯形容积点可表示为

将式（11）代入式（8），可得三阶正交球面单纯形容积准则如下

由式（9）～式（12）可知，正交球面单纯形容积准则的容积点可离线计算，因此，与球面径向容积准则相比，正交方法的引入并没有降低算法的实时运算效率。

3 自适应正交单纯形容积卡尔曼滤波

OSCKF 具有较高精度的前提是系统状态和噪声分布精确已知。然而，当目标状态出现突变或模型不准确时，OSCKF 的鲁棒性较差。为解决这一问题，本文将强跟踪滤波中的自适应因子与OSCKF 相结合，提出自适应正交单纯形容积卡尔曼滤波算法。

3.1 非线性强跟踪滤波原理

强跟踪滤波［10-11］是一种基于输出残差序列正交性的扩展卡尔曼滤波算法，通过引入自适应因子，在线调整增益矩阵，保证残差序列持续处于彼此正交的状态，从而保证滤波器在模型不确定时，仍具有对系统状态的跟踪能力，有效提高滤波鲁棒性。由文献［9］可得其自适应因子计算方法为

因此，非线性STF 的自适应因子可由式（13）、式（16）～式（19）计算得出。

3.2 自适应正交单纯形容积卡尔曼滤波

根据上述分析，将STF 的自适应因子引入OSCKF，构造AOSCKF。传统CKF 算法中需对协方差矩阵进行Cholesky 分解，在实际运算中，常发生协方差矩阵因非正定而无法正常分解的情况。因此，本文采用QR 分解进行代替，以提高算法的数值稳定性。AOSCKF 的具体算法流程如下：

初始化：

在此后的递推过程中（k=1，2，…），循环进行时间更新和量测更新。

时间更新：

式中，wj为容积点的权值，可表示为

式中，上标l 表示此时刻未引入自适应因子。

量测更新：

4 实验和分析

本文采用双站纯方位目标跟踪系统模型对AOSCKF 算法进行验证分析，并与传统CKF，OSCKF算法进行比较，系统模型如式（1）～式（3）所示。定义位置均方根误差RMSEpos和速度均方根误差RMSEvel为

4.1 匀速直线运动目标机动跟踪

在匀速直线运动中，式（2）中的状态转移矩阵为：

过程噪声的协方差矩阵为：

由于目标状态突变时，系统状态与模型出现不匹配，因此，本文通过状态转移矩阵的变化以模拟系统状态不匹配的场景。式（42）可改写为

式中，μ 和υ 为可调节参数，目标状态的不确定性可通过设定不同的参数进行模拟，当μ=υ=1 时，式（45）退化为式（42），即此时目标状态正常。

假设目标在第60 s 和第120 s 发生状态突变，每次突变持续时间为5 s。第1 次突变中，设置μ=1.1，υ=1.2，第2 次突变中设置μ=1.01，υ=1.05。

假设各算法初始条件相同，经过500 次Monte Carlo 仿真，得到各算法的跟踪轨迹，RMSEpos和RMSEvel，如图2～图4 所示。图中箭头表示目标状态在此处发生突变。

图2 目标跟踪轨迹

图3 位置均方根误差

图4 速度均方根误差

由图2 可知，在目标真实初始状态与系统预测初始状态不同的情况下，CKF，OSCKF 和AOSCKF都能较快收敛，但OSCKF 和AOSCKF 的收敛速度更快，对目标的跟踪误差更小。在没有发生第1 次目标状态突变时，OSCKF 与AOSCKF 的跟踪效果一致。在目标状态突变后，CKF 和OSCKF 的跟踪轨迹与真实轨迹存在较大偏差，而AOSCKF 的跟踪轨迹与真实轨迹的吻合度较高，对突变后状态的跟踪响应更快，跟踪效果更好。其中，第1 次模型突变程度较大，属于较强机动情况，第2 次模型突变程度较小，属于弱机动情况。这说明在较强机动的弱机动场景下，AOSCKF 均具有较好的跟踪能力。

由图3 和图4 可知，在0 s～60 s 内，3 种滤波在位置均方根误差方面相差不大，在速度均方根误差方面，相比CKF，AOSCKF 和OSCKF 误差更小，但三者均能达到收敛状态；在第60 s 和第120 s 目标状态发生突变后，CKF 和OSCKF 的估计误差迅速增加，位置均方根误差最大值分别为14.980 km 和14.230 km，且收敛速度较慢，其原因是目标的状态突变会造成目标实际运动模型与系统状态方程不一致，使得滤波算法中的预测残差增大，而增益无法根据量测数据实时调整，因此，无法保持对目标状态的跟踪。其中，OSCKF 的跟踪效果优于CKF，这是因为正交球面单纯形容积准则的引入使得滤波估计精度得到了有效提高，但面对状态不确定情况OSCKF 的估计精度依旧较低。与OSCKF 相比，AOSCKF 的估计误差更小，位置均方根误差的最大值为7.409 km。目标状态发生突变后，AOSCKF 的位置均方根误差峰值较OSCKF 降低47.9%，速度均方根误差峰值降低27.7%，滤波估计精度更高且收敛速度更快，表明AOSCKF 在应对目标状态突变情况时，具有较强的跟踪能力。这是因为当目标状态突变时，AOSCKF 中的自适应因子能够根据残差的改变对增益矩阵进行在线调节，进而提升估计精度。

4.2 协调转弯运动目标机动跟踪

在协调转弯运动中，式（2）中的状态转移矩阵为：

式中，ω 为目标转弯的角速度。

为模拟目标状态的异常，由4.1 节分析可知，式（46）可改写为

假设运行总时长为380 s，目标在第60 s 和第150 s 发生状态突变，每次突变持续时间为5 s。第1次突变中设置μ=1.01，υ=1.08，第2 次突变中设置μ=1.01，υ=1.05。设定ω=0.015 rad/s，其他参数与上一节相同。假设各算法初始条件一致，经过500 次Monte Carlo 仿真，得到各算法的跟踪轨迹，RMSEpos和RMSEvel，如图5～图7 所示。

图5 目标跟踪轨迹

图6 位置均方根误差

图7 速度均方根误差

由图5 可知，在没有状态突变时，3 种算法均能达到收敛状态，但CKF 收敛速度较慢；当发生第1 次状态突变时，CKF 轨迹与真实轨迹出现较大误差，而当第2 次状态突变发生后，两者的差距进一步增大，直至突变消失，模型回归正常后，CKF 才随着时间的推移逐步恢复对目标的跟踪能力。与CKF相比，OS CKF 对真实轨迹的跟踪误差较小，收敛速度较快，但当目标状态发生突变时，跟踪效果较差；AOSCKF 轨迹与真实轨迹的差距最小，且当目标状态发生突变时，AOSCKF 能迅速响应，持续保持对目标的跟踪。

由图6 和图7 可知，在0 s～60 s 内，3 种滤波算法的位置估计误差相差不大，在速度估计误差方面，AOSCKF 和OSCKF 误差较小；在第60 s 和第150 s目标状态发生突变后，CKF 的误差很大，位置均方根误差的最大值为3.854 km。OSCKF 估计精度相比CKF有一定提升，位置均方根误差的最大值为2.693 km。AOSCKF 估计精度最高，位置均方根误差的最大值为1.914 km，AOSCKF 与OSCKF 相比，位置均方根误差峰值降低28.9%，速度均方根误差降低39.2%，且收敛速度更快，这表明AOSCKF 的鲁棒性更强。

为比较各算法运算复杂度，设CKF 的运行时间为1，上述两种仿真情形中各算法的相对运行时间如表1 所示。由表1 可知，与CKF 相比，OSCKF 的所需运算时间更多，其原因是OSCKF 中正交球面单纯形容积准则的存在使得采样点数比CKF 多2，因此，运算量有所增加；与OSCKF 相比，AOSCKF 的运算量有所提升，这是因为虽然两者采样点数均为2n+2，但自适应因子的引入增加了AOSCKF 的算法复杂度。考虑到AOSCKF 在滤波估计精度和鲁棒性方面的提升，其算法复杂度的增加是可以接受的。

表1 各算法相对运行时间

通过仿真对比可知，协调转弯运动目标跟踪与匀速直线运动目标跟踪的实验结论相似，说明AOSCKF 算法对匀速直线运动协和调转弯运动在目标发生状态突的情况下均有效。

5 结论

在目标跟踪系统中，系统状态模型的不匹配会导致滤波估计精度降低甚至发散。针对该问题，本文提出了AOSCKF 算法，该算法由正交球面单纯形容积准则对多维高斯积分进行近似，可在避免非局部采样效应的情况下有效提高滤波精度。同时，将STF 与OSCKF 相结合，通过引入自适应因子对增益矩阵进行实时调节，从而提高算法的鲁棒性。将AOSCKF 算法应用于存在系统状态模型突变的目标跟踪系统中进行验证，仿真结果表明，与OSCKF 相比，AOSCKF 算法的滤波精度更高，鲁棒性更强，在目标跟踪系统的实际应用中具有良好的前景。