角速度估计自适应的IMM三维目标跟踪算法

田雨芬， 蒋 宏， 丁全心， 梁国威

(1.北京航空航天大学控制一体化技术国家级科技重点实验室，北京 100191;2.光电控制技术重点实验室，河南 洛阳 471009)

0 引言

无论在国防领域还是民用领域，目标跟踪都占据着重要的位置［1］。精确的目标跟踪是对目标进行成功打击的前提和基础。因此，提高机动目标的跟踪精度是许多科学家、工程师致力研究的课题。但是国内外大量的研究、仿真都是基于二维平面的。而现实中的许多机动往往都是在三维空间内发生的，所以将二维平面的模型和跟踪算法扩展到三维空间进行研究，具有重要的现实意义［2］。

交互式多模型算法(IMM)是将一组模型交互进行跟踪的算法。因IMM算法综合了不同滤波模型对目标状态进行估计，所以从全局角度讲，即便是三维机动目标，其跟踪性能也是比较好的［3］。但是由于目标机动具有不确定性，为了更好地覆盖大范围机动，需要利用参数估计实现模型的自适应调整(AIMM)［4］。本文通过在模型中加入角速度估计，对目标运动角速度进行滤波求精，随着时间的变化，角速度的估计值和真实值之间的差值越来越小。

本文在滤波时选用CV模型和带有角速度估计的CSCT模型进行交互。

1 三维机动模型

常速率协同转弯模型(CSCT模型)是假设目标以一个常速率V(即速度和加速度垂直)在一个平面内做圆周运动，Ω指的是角速度向量。

由于假设的目标轨迹是在同一平面内，则Ω垂直于a和v决定的平面，即Ω⊥v，a·v=0，因此有

其中w为旋转速率，并定义为

若加速度受到噪声干扰，其中ω为白噪声，则有

对应的离散形式为

其中，T为采样周期。

CSCT模型与参数w的取值有很大关系，而w的值则由式(3)得到，这样在已知w的情况下，可以保持滤波方程是线性的。但是模型的应用需要满足条件a·v=0，如果不满足条件时，滤波误差会相对大一些。所以解决办法就是把a·v=0作为运动约束加入滤波迭代过程中［2，5－6］。

2 滤波算法

2.1 IMM 算法

交互多模型算法将目标的机动建模为带马尔可夫系数的多个线性模型，利用模型间数据的交互来提高算法的性能。

IMM算法的一个循环包括4步:交互、n个模型滤波、模型概率更新、组合［7］。只要选取了合适的运动模型，通过IMM算法就可以很好地反映目标实际情况，否则交互多模型算法可能还不如单一算法精确;这样，对目标模型的选取很容易陷入一个误区，就是尽可能选取大的模型集，使模型集可以囊括目标运动的所有可能情况，但是很明显这样会增大算法的计算量，所以迫切需要一种方法，可以通过参数的自适应估计适应目标的不确定机动，即如下所说在滤波算法中加入角速度估计［7］。

2.2 角速度估计算法

角速度估计是实现AIMM算法的基础，通过构造角速度估计的状态方程和伪测量方程，综合考虑角速度的估计值和估计方差，利用卡尔曼滤波得到角速度的估计值。状态方程和伪测量方程为

式中:zw(k)为角速度观测值;w(k)为角速度随机变化量，方差为Qw(k);vw(k)为角速度观测噪声，方差为Rw(k);Qw(k)和Rw(k)相互独立。由于状态转移矩阵和观测矩阵均为1，所以根据类似卡尔曼滤波公式，可得到角速度估计的迭代公式为

其中:w⌒(k)为角速度估计值;Pw(k)为角速度估计方差;Kw(k)为增益。

由于zw(k)的求取公式是强非线性的，所以一般采用Monte Carlo方法求解zw(k)的方差Rw(k)，但计算量太大。本文采用Julier最近提出的一种新的求解方法，随机变量经非线性变换后，可以不需要计算雅可比矩阵，来估计非线性变换后变量的方差。所得的估计方差的精度可达到方差的四阶泰勒展开式以上［8］。方差的近似计算公式为

式中:zi(k)为式(12)中采样点的非线性变换值;wi为每个采样点的权值。采样点按文献［8］选取，此时三维下选取13个采样点。由于在三维空间中共有3个相互呈直角关系的平面。所以求解总的zi(k)时分别是各个平面zi(k)的叠加。

求角速度变化方差时，假设ωw=0的概率为P0，时，ωw在±A之间均匀分布，则有

以上便是角速度估计迭代过程［7－9］。

2.3 IMM算法和角速度估计相结合

IMM算法和角速度估计融合过程的具体结构如图1所示。

图1 IMM算法与角速度估计相融合Fig.1 Combination of IMM algorithm and angular velocity estimation

将CSCT模型在k－1时刻的角速度信息进行估计，得到角速度估计值，再与CV模型交互就会得到k时刻的各个滤波器的信息。

3 算法实现与仿真分析

假设飞行曲线如图2所示，目标从A点(30 km，30 km，30 km)出发，0～50 s做匀速直线运动;50～100 s在B点做顺时针协同转弯，初始加速度为(40，－80，20)(单位为m/s2)，此时目标机动为9.3522g，且加速度向量与速度向量垂直，满足在同一个平面内做圆周运动时速度和加速度垂直的条件。之后100～200 s目标继续做匀速直线运动;200～250 s绕C点作逆时针运动，此时的加速度初始向量为(－50，50，－62)(单位为m/s2)，目标机动为9.59622g，且转动不满足在同一个平面内做圆周运动的条件;之后250～300 s目标继续做匀速直线运动直到终点D。

图2 三维机动目标曲线Fig.2 3D maneuvering target curve

图3 角速度变化曲线Fig.3 Angular velocity curve

图4 滤波曲线和观测曲线的均方根误差对比图Fig.4 RMSE curve of filter and observations

图5 npe曲线图Fig.5 npe curve

由图3可以看出，通过角速度估计以后，相比伪角速度观测值，角速度估计值曲线更加平滑稳定，更加接近于真实值曲线。由图5可以看出，无论有没有经过角速度估计，npe的值基本都在0.6～0.8之间，因此滤波算法大大减小了观测误差。

由图4和图5还可以得出，经过角速度估计之后，均方根误差值和npe值更小，尤其在B弯和C弯处，角速度估计后的曲线具有明显优势。但是由于目标在B弯的转弯角速率是由角速度和速度的估计值计算得到的，满足CSCT模型同一时刻速度和加速度垂直的使用条件，即a·v=0，而目标在C弯的转弯角速率则不满足该条件。所以从图中都可以看出，B弯时的滤波精度比C弯时的滤波精度要高。

4 结论

本文针对三维空间内的目标机动，提出了将角速度估计运用到三维带约束常速率转弯模型中，并且与CV模型进行交互的算法，实现模型的自适应调整。通过对目标角速度进行滤波求精，增加滤波迭代的准确性，随着滤波时间的叠加，角速度滤波值与真实值之间的误差越来越小，说明角速度估计算法的实用性。同时本文直接研究的三维目标机动比二维目标机动更具有现实意义。仿真结果表明，即便目标有较大的机动(本文设计的曲线中机动已经达到了9g以上)，滤波精度依然较高，因此本文提出的算法很有效。

［1］ 孙福明.机动目标跟踪状态估计与数据关联技术的研究［D］.合肥:中国科学技术大学，2007.

［2］ 彭冬亮，郭云飞，薛安克.三维高速机动目标跟踪交互式多模型算法［J］.控制理论与应用，2008，25(5):831－836.

［3］ 周宏仁，敬忠良，王培德.机动目标跟踪［M］.北京:国防工业出版社，1991.

［4］ EFE M，ATHERTON D P.Interacting model maneuvering target tracking using adaptive turn rate models in the algorithm［C］//Proceedings of the 35th Conference on Decision and Control，1996:3151－3156.

［5］ LI X R，JILKOV V P.A survey of maneuvering target tracking:Dynamic models［C］//Proceedings of SPIE Conference on Signal and Data Processing of Small Targets，2000，4048:212－235.

［6］ MATSUZAKI T，KAMEDA H，TSUJIMICHI S，et al.Maneuvering target tracking using constant velocity and constant angular velocity model［C］//IEEE Transactions on Automatic Control，2000，33(8):3230－3234.

［7］ 何衍.机动目标跟踪与传感器网络自组织［D］.杭州:浙江大学，2001.

［8］ JULIER S，UHIMANN J，DURRANT－WHYTE H F.A new method for the nonlinear transformation of means and covariances in filters and estimators［C］//IEEE Transactions on Automatic Control，2000，45:477－482.

［9］ TIAN Ye，JIANG Hong.Turn rate estimation based adaptive IMM algorithm for maneuvering target tracking［C］//2010 International Conference on Electrical Engineering and Automatic Control(ICEEAC)，2010:444－447.