CN103853908A - 一种自适应交互式多模型的机动目标跟踪方法 - Google Patents

Abstract

本发明涉及在交通运输领域中车辆目标的机动跟踪。建立各模型混合初始化输入包括各模型的混合初始条件和混合初始状态的协方差矩阵；建立匀速（CV）和匀加速（CA）运动模型；更新是指根据Kalman滤波方程计算误差的协方差矩阵和新息；利用Kalman滤波结果中的新息构造目标运动模型的似然函数，计算马尔可夫状态转移概率矩阵；利用马尔可夫状态转移概率矩阵作为各运动模型之间切换的加权进行融合后的估计输出。本发明解决了传统的交互式多模型算法中存在的由于目标发生机动导致滤波模型与目标运动模型不匹配造成的误差增大或者失跟的问题，具有计算复杂度小，跟踪效果好的优点，可用于交通运输领域机动车的目标跟踪。

Description

一种自适应交互式多模型的机动目标跟踪方法

技术领域

本发明属于机动目标跟踪领域，具体涉及在交通运输领域中，当行驶中的汽车出现突然转弯、加速、减速等运动时，提出一种自适应交互式多模型的机动目标跟踪方法。

背景技术

目标跟踪是将探测传感器收到的目标移动数据结合各种不确定信息源所产生的不同观测集合进行滤波，并估计出运动目标的状态参数，比如目标的距离、方位、速度、加速度等。

单模型机动目标跟踪算法，只对非机动目标跟踪性能良好。当目标机动时，系统的跟踪性能下降，甚至导致目标丢失。为了实现对机动目标跟踪，需要针对机动目标的运动特点建立合理的运动模型。典型的运动模型有Singer模型、“当前”统计模型、交互式多模型（interacting multiple model,IMM）等。在传统的IMM模型中，假设目标在不同的运动模型之间的转移概率是固定的，这种假设并没有充分的考虑到运动模型的选择性，而是利用类似“硬判决”的思想将模型之间的转移概率固定在某个数值。事实上，当目标的运动模型具有某种趋向性，传统的IMM算法只是通过调解不同观测向量条件下运动模型的后验概率加权实现对运动模型之间的“综合”，而并没有考虑到马尔可夫转移概率矩阵设计的不合理性。

交通运输领域中，当行驶中的汽车出现突然转弯、加速、减速等运动也属于目标的机动，尤其当车辆目标突然进入弯道时，车辆运动状态较之前匀速或匀加速直线运动将发生较大的变化，如果不能有效的适应目标的机动状态，很容易造成目标的丢失，而当目标发生机动时，恰是最容易发生交通事故的时刻。交通运输领域车辆机动目标跟踪方法的主要目的是满足汽车运行中能可靠而精确地跟踪并判定目标，增强防撞探测系统运行的稳定性和可靠性，有效提高驾驶员的行车安全，保障生命财产安全。

因此，针对传统IMM跟踪算法中采用固定马尔可夫矩阵的缺点，本发明提出一种交通运输领域自适应交互式多模型的机动目标跟踪方法，提高复杂环境性下车辆机动目标跟踪系统的性能，具有重大理论意义及工程应用价值。

发明内容

针对现有技术中存在的上述不足之处，本发明要解决的技术问题是提供一种稳定、可靠、环境适应性强、算法实时性高的车辆机动目标跟踪方法，提高复杂环境性下车辆机动目标跟踪系统的性能。

本发明为实现上述目的所采用的技术方案是：一种自适应交互式多模型的机动目标跟踪方法，包括以下步骤：

步骤1：建立IMM跟踪模型中的各种模型混合初始化输入，包括各模型的混合初始条件和混合初始状态的协方差矩阵；

步骤2：利用每个模型的混合输入，分别计算匀速运动模型和匀加速运动模型初始值

步骤3：根据Kalman滤波方程计算误差的协方差矩阵和新息计算估计更新；

步骤4：利用基于新息的运动模型似然比函数，建立马尔可夫状态转移概率矩阵；

步骤5：利用当前时刻获得的状态状态转移矩阵对模型条件估计进行融合，计算总体状态估计输出。

所述匀速运动模型为：

状态矢量表示为：

X=[x(k)  v_x(k)  y(k)  v_y(k)]^T

式中，x(k)、y(k)分别表示k时刻目标在x、y方向上的位移，v_x(k)、v_y(k)分别表示k时刻目标在x、y方向上的速度；

状态转移矩阵表示为：

$\mathrm{\Φ}=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]$

式中，T为相邻两个时刻的时间间隔；

系统控制量表示为：

$\mathrm{\Γ}=\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right]$

系统过程噪声表示为：

$W\left(k\right)=\left[\begin{array}{c}{u}_x\left(k\right)\\ u_y\left(k\right)\end{array}\right]$

式中，u_x(k)、u_y(k)描述了x、y方向上的过程噪声，假设为相互独立的、零均值、高斯白噪声，方差分别为

和

系统观测矩阵为：

$C=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]$

观测噪声为：

$V\left(k\right)=\left[\begin{array}{c}{v}_x\left(k\right)\\ v_y\left(k\right)\end{array}\right]$

其中，v_x(k)、v_y(k)为x、y方向上的观测噪声，假设为相互独立的、零均值、高斯白噪声，方差为σ²。

所述匀加速运动模型为：

状态矢量表示为：

X=[x(k)  v_x(k)  a_x(k)  y(k)  v_y(k)  a_y(k)]^T

式中，x(k)、y(k)分别表示k时刻目标在x、y方向上的位移，v_x(k)、v_y(k)分别表示k时刻目标在x、y方向上的速度；

状态转移矩阵表示为：

$\mathrm{\Φ}=\left[\begin{array}{cccccc}1& T& 0& 0& T^2/2& 0\\ 0& 1& 0& 0& T& 0\\ 0& 0& 1& T& 0& T^2/2\\ 0& 0& 0& 1& 0& T\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

式中，T为相邻两个时刻的时间间隔；

系统控制量表示为：

$\mathrm{\Γ}=\left[\begin{array}{cc}{T}^2/4& 0\\ T/2& 0\\ 0& T^2/4\\ 0& T/2\\ 1& 0\\ 0& 1\end{array}\right]$

系统过程噪声表示为：

$W\left(k\right)=\left[\begin{array}{c}{u}_x\left(k\right)\\ u_y\left(k\right)\end{array}\right]$

式中，u_x(k)、u_y(k)描述了x、y方向上的过程噪声，假设为相互独立的、零均值、高斯白噪声，方差分别为和

系统观测矩阵为：

$C=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]$

观测噪声为：

$V\left(k\right)=\left[\begin{array}{c}{v}_x\left(k\right)\\ v_y\left(k\right)\end{array}\right]$

其中，v_x(k)、v_y(k)描述了x、y方向上的观测噪声，假设为相互独立的、零均值、高斯白噪声，方差为σ²。

所述各种模型M_j(k)的混合初始条件为：

${\hat{X}}^{\mathrm{oj}}(k-1|k-1)=\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\hat{X}}^i(k-1|k-1){\mathrm{\μ}}_{\mathrm{ij}}(k-1|k-1)$

式中，k为目标运动的时刻，为交互后各滤波器在k时刻的输入，μ_ij(k-1|k-1)为混合概率，计算为

${\mathrm{\μ}}_{\mathrm{ij}}(k-1|k-1)=P\left\{M_i(k-1)\right|M_i\left(k\right),Z(k-1)\}$

$=\frac{1}{{\stackrel{\‾}{c}}_j}{p}_{\mathrm{ij}}{\mathrm{\μ}}_i(k-1)$

式中， ${\stackrel{\‾}{c}}_j=\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{p}_{\mathrm{ij}}{\mathrm{\μ}}_i(k-1)$ 为归一化常数。

所述混合初始状态的协方差矩阵为：

$P^{\mathrm{oj}}(k-1|k-1)$

$=\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{\mathrm{\μ}}_{\mathrm{ij}}(k-1|k-1)\{P^i(k-1|k-1)+$

$[{\hat{X}}^i(k-1|k-1)-{\hat{X}}^{\mathrm{oj}}(k-1|k-1)]$

$\×{[{\hat{X}}^i(k-1|k-1)-{\hat{X}}^{\mathrm{oj}}(k-1|k-1)]}^T\}$

其中，μ_ij(k-1|k-1)为为混合概率，Pⁱ(k-1|k-1)为k-1时刻的协方差，

为k-1时刻第i个滤波器的输出，

为交互后各滤波器在k时刻的输入。

所述步骤3具体步骤如下：

对应于模型M_j(k)，

P^oj(k-1/k-1)及Z(k)作为输入进行卡尔曼滤波处理，预测误差协方差矩阵表示为：

$P_{\tilde{X}}^j\left(k\right|k-1)={\mathrm{\Φ}}_j\left(k\right|k-1)P_{\tilde{X}}^{0j}(k-1|k-1){\mathrm{\Φ}}_j^T\left(k\right|k-1)$

$+{\mathrm{\Γ}}_j(k-1)Q(k-1){\mathrm{\Γ}}_j^T(k-1)$

卡尔曼滤波增益计算公式：

$K_j\left(k\right)=P_{\tilde{X}}^j\left(k\right|k-1)C^T\left(k\right){[C\left(k\right)P_{\tilde{X}}^j\left(k\right|k-1)C^T\left(k\right)+R\left(k\right)]}^{-1}$

卡尔曼滤波误差协方差矩阵计算公式：

$P_{\tilde{X}}^j\left(k\right|k)=[I-K_j\left(k\right)C\left(k\right)]P_{\tilde{X}}^j\left(k\right|k-1)$

所述步骤4具体步骤如下：

利用给定观测向量Z(k)的条件下模型M_j(k)的后验概率，计算为：

${\mathrm{\μ}}_j\left(k\right)=P\left\{M_j\left(k\right)\right|Z\left(k\right)\}$

$=P\left\{Z\left(k\right)\right|M_j\left(k\right),Z(k-1)\left\}P\right\{M_j\left(k\right)\left|Z(k-1)\right\}$

$=\frac{1}{c}{\mathrm{\Λ}}_j\left(k\right)\underset{i=1}{\overset{r}{\mathrm{\Σ}}}{p}_{\mathrm{ij}}{\mathrm{\μ}}_i(k-1)=\frac{1}{c}{\mathrm{\Λ}}_j\left(k\right){\stackrel{\‾}{c}}_j$

其中，

为归一化常数，Λ_j(k)为观测Z(k)的似然函数，计算为：

${\mathrm{\Λ}}_j\left(k\right)=P\left\{Z\left(k\right)\right|M_j\left(k\right),Z^{k-1}\}$

$=\frac{1}{{\left(2\mathrm{\π}\right)}^{n/2}{\left|S_j\left(k\right)\right|}^{1/2}}\exp \{-\frac{1}{2}{\mathrm{\υ}}_j^T{S}_j^{-1}\left(k\right){\mathrm{\υ}}_j\}$

式中， ${\mathrm{\υ}}_j\left(k\right)=Z\left(k\right)-C\left(k\right){\hat{X}}^j\left(k\right|k-1)$ 为模型M_j(k)的新息；

$S_j\left(k\right)=C\left(k\right)P_{\tilde{X}}^j\left(k\right|k-1)C^T\left(k\right)+R\left(k\right)$ 为模型M_j(k)的协方差矩阵；

以匀速和匀加速两种状态模型，建立对应的马尔可夫状态转移矩阵：

$P=\left[\begin{array}{cc}{p}_{11}& p_{12}\\ p_{21}& p_{22}\end{array}\right]$

式中p_ij满足，

基于新息，可以构造目标运动模型似然函数，似然函数为：

$p_{M_i}\left[{\mathrm{\υ}}_i\left(k\right)\right]=\frac{1}{{\left(2\mathrm{\π}\right)}^{\frac{N}{2}}{\left|S_i\left(k\right)\right|}^{\frac{1}{2}}}{e}^{-\frac{{\mathrm{\υ}}_i^H\left(k\right)S_i^{-1}\left(k\right){\mathrm{\υ}}_i\left(k\right)}{2}}$

式中，N为新息向量的维数，υ_j(k)为模型M_j(k)的新息，S_j(k)为模型M_j(k)的协方差矩阵。

基于匀速和匀加速运动模型似然函数构造似然比函数：

$\frac{p_{M_1}\left[{\mathrm{\&upsi;}}_1\left(k\right)\right]}{p_{M_2}\left[{\mathrm{\&upsi;}}_2\left(k\right)\right]}\underset{<}{\overset{>}{}}T\begin{array}{c}{M}_1\left(k\right)\\ M_2\left(k\right)\end{array}$

根据似然比函数计算马尔可夫转移矩阵中的各个变量，表达式如下：

$p_{12}\left(k\right)=f\left(\frac{p_{M_1}\left[{\mathrm{\&upsi;}}_1\left(k\right)\right]}{p_{M_2}\left[{\mathrm{\&upsi;}}_2\left(k\right)\right]}\right)$

所述总体状态估计输出为：

$\hat{X}\left(k\right|k)=\underset{j=1}{\overset{r}{\mathrm{\&Sigma;}}}{\hat{X}}^j\left(k\right|k){\mathrm{\&mu;}}_j\left(k\right)$

对应的协方差矩阵为：

$P\left(k\right|k)=\underset{i=1}{\overset{r}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}_j\left(k\right)\{P^j\left(k\right|k)+[{\hat{X}}^j\left(k\right|k)-\hat{X}\left(k\right|k)\left]{[{\hat{X}}^j\left(k\right|k)-\hat{X}\left(k\right|k)]}^T\right\}$

其中，μ_j(k)为观测向量Z(k)的条件下模型M_j(k)的后验概率，为K时刻目标的状态估计输出，P^j(k|k)为K时刻第j个滤波器协方差矩阵。

本发明具有以下优点：

1.本发明针对交通运输领域汽车运动状态特征，建立基于匀速（CV）和匀加速（CA）运动模型的模型集，不仅降低了模型与运动模式匹配的复杂度，同时提高了系统运行的实时性；

2.本发明采用基于新息构造的运动模型似然比函数，进行在线自适应调整模型的噪声方差和马尔可夫矩阵，实现机动目标运动模式与模型集合的自适应调整，提高了IMM算法的跟踪精度；

3.本发明通过构造若干典型取值的马尔可夫转移概率矩阵，利用似然比函数的取值在不同的矩阵之间进行选择，降低了马尔可夫状态转移概率矩阵的计算复杂度，有效提高了算法的实时性；

4.仿真结果表明，本发明跟踪结果优于传统的IMM方法。

附图说明

图1为本发明的模型估计流程示意图；

图2为本发明机动目标真实运动轨迹和跟踪轨迹示意图；

图3为本发明与传统的交互式多模型估计方法对机动目标跟踪位置误差示意图；

图4为本发明与基于匀速（CV）模型和加速（CA）模型的Kalman滤波跟踪位置误差示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

如图1所示，为本发明的模型估计流程示意图。

步骤1，建立IMM跟踪模型中的各种模型混合初始化输入。

1a)目标的运动状态方程和观测方程，假设有r个运动模型，每种运动模型对应的状态转移矩阵为Φ_j（1≤j≤r），方程可以表示为：

X(k+1)=Φ_j(k|k-1)X(k)+Г_jW_j(k),j=1，...,r

Z(k)=C_j(k)X_j(k)+V_j(k)

式中，X(k)为k时刻的系统状态矢量，Z(k)为k时刻的系统观测（测量）矢量，Φ_j为第j种运动模型所对应的状态转移矩阵，Г_j为第j种运动模型所对应的系统控制量，W_j(k)表示过程噪声，为零均值、协方差矩阵为Q_j(k)的白噪声序列,C_j为第j种运动模型所对应的观测矩阵，V_j(k)表示观测噪声，为零均值、协方差矩阵为R_j(k)的白噪声序列。

在IMM模型中，用马尔可夫状态转移概率矩阵来描述不同模型之间的转换，马尔可夫状态转移概率矩阵表示为：

式中，第i行、第j列元素为p_ij（1≤i,j≤r），表示从运动模型i转移到运动模型j的概率，即

p_ij=p{M_j(k)|M_i(k)}

式中，M_j(k)表示k时刻处于运动模型j，且

$\underset{j=1}{\overset{r}{\mathrm{\&Sigma;}}}{p}_{\mathrm{ij}}=1$

1b)利用每个滤波器的混合输入，分别计算匀速运动模型和匀加速运动模型初始值。汽车类目标的机动特征具有比较明显的特点，通常是进行直线运动（匀速直线运动、匀加速直线运动）或者转弯运动。对于这些运动特征，可以利用简单、实用的信号模型进行建模描述，且目标的运动特征能够很好地与运动模型吻合。

当汽车进行匀速运动时，状态矢量表示为：

X＝[x(k)  v_x(k)  y(k)  v_y(k)]^T

式中，x(k)、y(k)分别表示k时刻目标在x、y方向上的位移，v_x(k)、v_y(k)分别表示k时刻目标在x、y方向上的速度；

状态转移矩阵表示为：

$\mathrm{\&Phi;}=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]$

式中，T为相邻两个时刻的时间间隔系统控制量表示为：

$\mathrm{\&Gamma;}=\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right]$

系统过程噪声表示为：

$W\left(k\right)=\left[\begin{array}{c}{u}_x\left(k\right)\\ u_y\left(k\right)\end{array}\right]$

式中，u_x(k)、u_y(k)描述了x、y方向上的过程噪声，假设为相互独立的、零均值、高斯白噪声，方差分别为

和

系统观测矩阵为：

$C=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]$

观测噪声为：

$V\left(k\right)=\left[\begin{array}{c}{v}_x\left(k\right)\\ v_y\left(k\right)\end{array}\right]$

其中，v_x(k)、v_y(k)描述了x、y方向上的观测噪声，假设为相互独立的、零均值、高斯白噪声，方差为σ²。

当汽车进行匀加速运动时，状态矢量表示为：

X=[x(k)  v_x(k)  a_x(k)  y(k)  v_y(k)  a_y(k)]^T

式中，x(k)、y(k)分别表示k时刻目标在x、y方向上的位移，v_x(k)、v_y(k)分别表示k时刻目标在x、y方向上的速度；

状态转移矩阵表示为：

$\mathrm{\&Phi;}=\left[\begin{array}{cccccc}1& T& 0& 0& T^2/2& 0\\ 0& 1& 0& 0& T& 0\\ 0& 0& 1& T& 0& T^2/2\\ 0& 0& 0& 1& 0& T\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

式中，T为相邻两个时刻的时间间隔

系统控制量表示为：

$\mathrm{\&Gamma;}=\left[\begin{array}{cc}{T}^2/4& 0\\ T/2& 0\\ 0& T^2/4\\ 0& T/2\\ 1& 0\\ 0& 1\end{array}\right]$

系统过程噪声表示为：

$W\left(k\right)=\left[\begin{array}{c}{u}_x\left(k\right)\\ u_y\left(k\right)\end{array}\right]$

式中，u_x(k)、u_y(k)描述了x、y方向上的过程噪声，假设为相互独立的、零均值、高斯白噪声，方差分别为

和

系统观测矩阵为：

$C=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]$

观测噪声为：

$V\left(k\right)=\left[\begin{array}{c}{v}_x\left(k\right)\\ v_y\left(k\right)\end{array}\right]$

其中，v_x(k)、v_y(k)描述了x、y方向上的观测噪声，假设为相互独立的、零均值、高斯白噪声，方差为σ²。

1c)交互式多模型跟踪方法中的各种模型M_j(k)的混合初始条件为：

${\hat{X}}^{\mathrm{oj}}(k-1|k-1)=\underset{i=1}{\overset{r}{\mathrm{\&Sigma;}}}{\hat{X}}^i(k-1|k-1){\mathrm{\&mu;}}_{\mathrm{ij}}(k-1|k-1)$

式中，μ_ij(k-1|k-1)为混合概率，计算为

${\mathrm{\&mu;}}_{\mathrm{ij}}(k-1|k-1)=P\left\{M_i(k-1)\right|M_i\left(k\right),Z(k-1)\}$

$=\frac{1}{{\stackrel{\&OverBar;}{c}}_j}{p}_{\mathrm{ij}}{\mathrm{\&mu;}}_i(k-1)$

式中，

为归一化常数，计算为

${\stackrel{\&OverBar;}{c}}_j=\underset{i=1}{\overset{r}{\mathrm{\&Sigma;}}}{p}_{\mathrm{ij}}{\mathrm{\&mu;}}_i(k-1)$

混合初始状态协方差矩阵为：

$P^{\mathrm{oj}}(k-1|k-1)$

$=\underset{i=1}{\overset{r}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}_{\mathrm{ij}}(k-1|k-1)\{P^i(k-1|k-1)+$

$[{\hat{X}}^i(k-1|k-1)-{\hat{X}}^{\mathrm{oj}}(k-1|k-1)]$

$\&times;{[{\hat{X}}^i(k-1|k-1)-{\hat{X}}^{\mathrm{oj}}(k-1|k-1)]}^T\}$

步骤2：利用滤波技术更新当前时刻模型条件估计

对应于模型M_j(k)，

P^oj(k-1/k-1)及Z(k)作为输入进行卡尔曼滤波处理，预测误差协方差矩阵表示为：

$P_{\tilde{X}}^j\left(k\right|k-1)={\mathrm{\&Phi;}}_j\left(k\right|k-1)P_{\tilde{X}}^{0j}(k-1|k-1){\mathrm{\&Phi;}}_j^T\left(k\right|k-1)$

$+{\mathrm{\&Gamma;}}_j(k-1)Q(k-1){\mathrm{\&Gamma;}}_j^T(k-1)$

卡尔曼滤波增益计算公式：

$K_j\left(k\right)=P_{\tilde{X}}^j\left(k\right|k-1)C^T\left(k\right){[C\left(k\right)P_{\tilde{X}}^j\left(k\right|k-1)C^T\left(k\right)+R\left(k\right)]}^{-1}$

卡尔曼滤波误差协方差矩阵计算公式：

$P_{\tilde{X}}^j\left(k\right|k)=[I-K_j\left(k\right)C\left(k\right)]P_{\tilde{X}}^j\left(k\right|k-1)$

步骤3：利用基于新息的运动模型似然比函数，建立马尔可夫状态转移概率矩阵

3a)模型概率是给定观测向量Z(k)的条件下模型M_j(k)的后验概率，计算为

${\mathrm{\&mu;}}_j\left(k\right)=P\left\{M_j\left(k\right)\right|Z\left(k\right)\}$

$=P\left\{Z\left(k\right)\right|M_j\left(k\right),Z(k-1)\left\}P\right\{M_j\left(k\right)\left|Z(k-1)\right\}$

$=\frac{1}{c}{\mathrm{\&Lambda;}}_j\left(k\right)\underset{i=1}{\overset{r}{\mathrm{\&Sigma;}}}{p}_{\mathrm{ij}}{\mathrm{\&mu;}}_i(k-1)=\frac{1}{c}{\mathrm{\&Lambda;}}_j\left(k\right){\stackrel{\&OverBar;}{c}}_j$

其中，c为归一化常数，计算为

$c=\underset{j=1}{\overset{r}{\mathrm{\&Sigma;}}}{\mathrm{\&Lambda;}}_j\left(k\right){\stackrel{\&OverBar;}{c}}_j$

Λ_j(k)为观测Z(k)的似然函数，计算为

${\mathrm{\&Lambda;}}_j\left(k\right)=P\left\{Z\left(k\right)\right|M_j\left(k\right),Z^{k-1}\}$

$=\frac{1}{{\left(2\mathrm{\&pi;}\right)}^{n/2}{\left|S_j\left(k\right)\right|}^{1/2}}\exp \{-\frac{1}{2}{\mathrm{\&upsi;}}_j^T{S}_j^{-1}\left(k\right){\mathrm{\&upsi;}}_j\}$

式中， ${\mathrm{\&upsi;}}_j\left(k\right)=Z\left(k\right)-C\left(k\right){\hat{X}}^j\left(k\right|k-1)$ 为模型M_j(k)的新息 $S_j\left(k\right)=C\left(k\right)P_{\tilde{X}}^j\left(k\right|k-1)C^T\left(k\right)+R\left(k\right)$ 为模型M_j(k)的协方差矩阵

3b)在IMM运动模型中还需要对各种模型之间的状态转移概率进行建模，状态转移概率描述了某个时刻目标在多个运动模型之间相互切换的概率，以匀速和匀加速两种状态模型，建立对应的马尔可夫状态转移矩阵

$P=\left[\begin{array}{cc}{p}_{11}& p_{12}\\ p_{21}& p_{22}\end{array}\right]$

式中p_ij满足，

基于新息，可以构造目标运动模型似然函数，定义为

$p_{M_i}\left[{\mathrm{\&upsi;}}_i\left(k\right)\right]=\frac{1}{{\left(2\mathrm{\&pi;}\right)}^{\frac{N}{2}}{\left|S_i\left(k\right)\right|}^{\frac{1}{2}}}{e}^{-\frac{{\mathrm{\&upsi;}}_i^H\left(k\right)S_i^{-1}\left(k\right){\mathrm{\&upsi;}}_i\left(k\right)}{2}}$

式中，N为新息向量的维数，υ_j(k)为模型M_j(k)的新息，S_j(k)为模型M_j(k)的协方差矩阵。

基于匀速和匀加速运动模型似然函数构造似然比函数

$\frac{p_{M_1}\left[{\mathrm{\&upsi;}}_1\left(k\right)\right]}{p_{M_2}\left[{\mathrm{\&upsi;}}_2\left(k\right)\right]}\underset{<}{\overset{>}{}}T\begin{array}{c}{M}_1\left(k\right)\\ M_2\left(k\right)\end{array}$

根据似然比函数计算马尔可夫转移矩阵中的各个变量，表达式如下：

$p_{12}\left(k\right)=f\left(\frac{p_{M_1}\left[{\mathrm{\&upsi;}}_1\left(k\right)\right]}{p_{M_2}\left[{\mathrm{\&upsi;}}_2\left(k\right)\right]}\right)$

步骤4：利用当前时刻获得的状态状态转移矩阵对模型条件估计进行融合，计算总体状态估计输出，IMM算法的输出公式表示为：

$\hat{X}\left(k\right|k)=\underset{j=1}{\overset{r}{\mathrm{\&Sigma;}}}{\hat{X}}^j\left(k\right|k){\mathrm{\&mu;}}_j\left(k\right)$

对应的协方差矩阵为：

$P\left(k\right|k)=\underset{i=1}{\overset{r}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}_j\left(k\right)\{P^j\left(k\right|k)+[{\hat{X}}^j\left(k\right|k)-\hat{X}\left(k\right|k)\left]{[{\hat{X}}^j\left(k\right|k)-\hat{X}\left(k\right|k)]}^T\right\}$

本发明的效果可以通过以下仿真进一步说明

仿真内容：假设一个目标以72km/h的速度进行匀速直线运动（模拟汽车在直道的情况），然后再进入一个半径为80m的弯道。观测噪声的均方根为1m。驱动噪声的均方根误差为1m。分别选取自适应IMM跟踪方法（包含匀速运动模型M₁(k)、加速运动模型M₂(k)）、传统IMM跟踪方法、基于匀速运动模型M₁(k)的Kalman滤波跟踪、基于加速运动模型M₂(k)的Kalman滤波跟踪四种方法进行跟踪处理。

为了衡量不同跟踪算法的效果，这里以跟踪误差的均方值作为评价指标，定义为滤波结果与真实结果之间误差的均方值。利用1000次Monte Carlo仿真实验的结果进行平均处理，以减小某次实验的随机误差影响。

仿真结果与分析：

图2为本发明机动目标真实运动轨迹示意图，图3为本发明与传统的交互式多模型估计方法对机动目标跟踪位置误差示意图,从结果中可以看出,无论是在目标直线运动阶段还是在弯道运动阶段，自适应IMM算法都可以精确、稳定的跟踪上机动目标，且自适应IMM算法的跟踪结果要优于传统IMM方法。

图4为本发明与基于匀速（CV）模型和加速（CA）模型的Kalman滤波跟踪位置误差示意图，基于匀速运动模型的Kalman滤波跟踪处理只能在目标匀速直线运动阶段跟踪上目标；当目标进入转弯运动时，基于匀速运动模型的Kalman滤波处理无法有效的跟踪上目标，造成较大的跟踪误差。而基于加速运动模型的Kalman滤波处理虽然在弯道运动阶段有较好的跟踪结果，但是在直线运动阶段的跟踪误差较大。自适应IMM算法性能基本上和运动模型匹配的Kalman滤波算法相当，但是能较好的跟随目标的运动模型变化做出选择，得到满意的机动目标跟踪结果。

针对交通运输中车辆目标的跟踪问题，本发明提出基于自适应马尔可夫转移概率矩阵的IMM跟踪算法。传统的IMM算法中假设马尔可夫转移概率矩阵是固定的，限制了对机动目标运动模型的选择性。本发明构造了运动模型似然比函数，提出了基于运动模型似然比函数的自适应马尔可夫转移概率计算方法。自适应马尔可夫转移概率矩阵的IMM跟踪算法可以很好的跟踪机动目标，得到和运动模型匹配跟踪滤波相近的效果。实际测试结果表明，跟踪算法可对运动中车辆进行有效跟踪，方法具备很好的自适应能力。

Claims (8)

1.一种自适应交互式多模型的机动目标跟踪方法，其特征在于，包括以下步骤：

步骤1：建立IMM跟踪模型中的各种模型混合初始化输入，包括各模型的混合初始条件和混合初始状态的协方差矩阵；

步骤2：利用每个模型的混合输入，分别计算匀速运动模型和匀加速运动模型初始值

步骤3：根据Kalman滤波方程计算误差的协方差矩阵和新息计算估计更新；

步骤4：利用基于新息的运动模型似然比函数，建立马尔可夫状态转移概率矩阵；

步骤5：利用当前时刻获得的状态状态转移矩阵对模型条件估计进行融合，计算总体状态估计输出。

2.根据权利要求1所述的一种自适应交互式多模型的机动目标跟踪方法，其特征在于，所述匀速运动模型为：

状态矢量表示为：

X＝[x(k)  v_x(k)  y(k)  v_y(k)]^T

式中，x(k)、y(k)分别表示k时刻目标在x、y方向上的位移，v_x(k)、v_y(k)分别表示k时刻目标在x、y方向上的速度；

状态转移矩阵表示为：

$\mathrm{\&Phi;}=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]$

式中，T为相邻两个时刻的时间间隔；

系统控制量表示为：

$\mathrm{\&Gamma;}=\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right]$

系统过程噪声表示为：

$W\left(k\right)=\left[\begin{array}{c}{u}_x\left(k\right)\\ u_y\left(k\right)\end{array}\right]$

式中，u_x(k)、u_y(k)描述了x、y方向上的过程噪声，假设为相互独立的、零均值、高斯白噪声，方差分别为

和

系统观测矩阵为：

$C=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]$

观测噪声为：

$V\left(k\right)=\left[\begin{array}{c}{v}_x\left(k\right)\\ v_y\left(k\right)\end{array}\right]$

其中，v_x(k)、v_y(k)为x、y方向上的观测噪声，假设为相互独立的、零均值、高斯白噪声，方差为σ²。

3.根据权利要求1所述的一种自适应交互式多模型的机动目标跟踪方法，其特征在于，所述匀加速运动模型为：

状态矢量表示为：

X=[x(k)  v_x(k)  a_x(k)  y(k)  v_y(k)  a_y(k)]^T

式中，x(k)、y(k)分别表示k时刻目标在x、y方向上的位移，v_x(k)、v_y(k)分别表示k时刻目标在x、y方向上的速度；

状态转移矩阵表示为：

$\mathrm{\&Phi;}=\left[\begin{array}{cccccc}1& T& 0& 0& T^2/2& 0\\ 0& 1& 0& 0& T& 0\\ 0& 0& 1& T& 0& T^2/2\\ 0& 0& 0& 1& 0& T\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

式中，T为相邻两个时刻的时间间隔；

系统控制量表示为：

$\mathrm{\&Gamma;}=\left[\begin{array}{cc}{T}^2/4& 0\\ T/2& 0\\ 0& T^2/4\\ 0& T/2\\ 1& 0\\ 0& 1\end{array}\right]$

系统过程噪声表示为：

$W\left(k\right)=\left[\begin{array}{c}{u}_x\left(k\right)\\ u_y\left(k\right)\end{array}\right]$

式中，u_x(k)、u_y(k)描述了x、y方向上的过程噪声，假设为相互独立的、零均值、高斯白噪声，方差分别为和

系统观测矩阵为：

$C=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]$

观测噪声为：

$V\left(k\right)=\left[\begin{array}{c}{v}_x\left(k\right)\\ v_y\left(k\right)\end{array}\right]$

其中，v_x(k)、v_y(k)描述了x、y方向上的观测噪声，假设为相互独立的、零均值、高斯白噪声，方差为σ²。

4.根据权利要求1所述的一种自适应交互式多模型的机动目标跟踪方法，其特征在于，所述各种模型M_j(k)的混合初始条件为：

${\hat{X}}^{\mathrm{oj}}(k-1|k-1)=\underset{i=1}{\overset{r}{\mathrm{\&Sigma;}}}{\hat{X}}^i(k-1|k-1){\mathrm{\&mu;}}_{\mathrm{ij}}(k-1|k-1)$

式中，k为目标运动的时刻，

为交互后各滤波器在k时刻的输入，μ_ij(k-1|k-1)为混合概率，计算为

${\mathrm{\&mu;}}_{\mathrm{ij}}(k-1|k-1)=P\left\{M_i(k-1)\right|M_i\left(k\right),Z(k-1)\}$

$=\frac{1}{{\stackrel{\&OverBar;}{c}}_j}{p}_{\mathrm{ij}}{\mathrm{\&mu;}}_i(k-1)$

式中， ${\stackrel{\&OverBar;}{c}}_j=\underset{i=1}{\overset{r}{\mathrm{\&Sigma;}}}{p}_{\mathrm{ij}}{\mathrm{\&mu;}}_i(k-1)$ 为归一化常数。

5.根据权利要求1所述的一种自适应交互式多模型的机动目标跟踪方法，其特征在于，所述混合初始状态的协方差矩阵为：

$P^{\mathrm{oj}}(k-1|k-1)$

$=\underset{i=1}{\overset{r}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}_{\mathrm{ij}}(k-1|k-1)\{P^i(k-1|k-1)+$

$[{\hat{X}}^i(k-1|k-1)-{\hat{X}}^{\mathrm{oj}}(k-1|k-1)]$

$\&times;{[{\hat{X}}^i(k-1|k-1)-{\hat{X}}^{\mathrm{oj}}(k-1|k-1)]}^T\}$

其中，μ_ij(k-1|k-1)为为混合概率，Pⁱ(k-1|k-1)为k-1时刻的协方差，

为k-1时刻第i个滤波器的输出，

为交互后各滤波器在k时刻的输入。

6.根据权利要求1所述的一种自适应交互式多模型的机动目标跟踪方法，其特征在于，所述步骤3具体步骤如下：

对应于模型M_j(k)，以P^oj(k-1/k-1)及Z(k)作为输入进行卡尔曼滤波处理，预测误差协方差矩阵表示为：

$P_{\tilde{X}}^j\left(k\right|k-1)={\mathrm{\&Phi;}}_j\left(k\right|k-1)P_{\tilde{X}}^{0j}(k-1|k-1){\mathrm{\&Phi;}}_j^T\left(k\right|k-1)$

$+{\mathrm{\&Gamma;}}_j(k-1)Q(k-1){\mathrm{\&Gamma;}}_j^T(k-1)$

卡尔曼滤波增益计算公式：

$K_j\left(k\right)=P_{\tilde{X}}^j\left(k\right|k-1)C^T\left(k\right){[C\left(k\right)P_{\tilde{X}}^j\left(k\right|k-1)C^T\left(k\right)+R\left(k\right)]}^{-1}$

卡尔曼滤波误差协方差矩阵计算公式：

$P_{\tilde{X}}^j\left(k\right|k)=[I-K_j\left(k\right)C\left(k\right)]P_{\tilde{X}}^j\left(k\right|k-1)$

7.根据权利要求1所述的一种自适应交互式多模型的机动目标跟踪方法，其特征在于，所述步骤4具体步骤如下：

利用给定观测向量Z(k)的条件下模型M_j(k)的后验概率，计算为：

${\mathrm{\&mu;}}_j\left(k\right)=P\left\{M_j\left(k\right)\right|Z\left(k\right)\}$

$=P\left\{Z\left(k\right)\right|M_j\left(k\right),Z(k-1)\left\}P\right\{M_j\left(k\right)\left|Z(k-1)\right\}$

$=\frac{1}{c}{\mathrm{\&Lambda;}}_j\left(k\right)\underset{i=1}{\overset{r}{\mathrm{\&Sigma;}}}{p}_{\mathrm{ij}}{\mathrm{\&mu;}}_i(k-1)=\frac{1}{c}{\mathrm{\&Lambda;}}_j\left(k\right){\stackrel{\&OverBar;}{c}}_j$

其中，

为归一化常数，Λ_j(k)为观测Z(k)的似然函数，计算为：

${\mathrm{\&Lambda;}}_j\left(k\right)=P\left\{Z\left(k\right)\right|M_j\left(k\right),Z^{k-1}\}$

$=\frac{1}{{\left(2\mathrm{\&pi;}\right)}^{n/2}{\left|S_j\left(k\right)\right|}^{1/2}}\exp \{-\frac{1}{2}{\mathrm{\&upsi;}}_j^T{S}_j^{-1}\left(k\right){\mathrm{\&upsi;}}_j\}$

式中， ${\mathrm{\&upsi;}}_j\left(k\right)=Z\left(k\right)-C\left(k\right){\hat{X}}^j\left(k\right|k-1)$ 为模型M_j(k)的新息； $S_j\left(k\right)=C\left(k\right)P_{\tilde{X}}^j\left(k\right|k-1)C^T\left(k\right)+R\left(k\right)$ 为模型M_j(k)的协方差矩阵；

以匀速和匀加速两种状态模型，建立对应的马尔可夫状态转移矩阵：

$P=\left[\begin{array}{cc}{p}_{11}& p_{12}\\ p_{21}& p_{22}\end{array}\right]$

式中p_ij满足，

基于新息，可以构造目标运动模型似然函数，似然函数为：

$p_{M_i}\left[{\mathrm{\&upsi;}}_i\left(k\right)\right]=\frac{1}{{\left(2\mathrm{\&pi;}\right)}^{\frac{N}{2}}{\left|S_i\left(k\right)\right|}^{\frac{1}{2}}}{e}^{-\frac{{\mathrm{\&upsi;}}_i^H\left(k\right)S_i^{-1}\left(k\right){\mathrm{\&upsi;}}_i\left(k\right)}{2}}$

式中，N为新息向量的维数，υ_j(k)为模型M_j(k)的新息，S_j(k)为模型M_j(k)的协方差矩阵。

基于匀速和匀加速运动模型似然函数构造似然比函数：

$\frac{p_{M_1}\left[{\mathrm{\&upsi;}}_1\left(k\right)\right]}{p_{M_2}\left[{\mathrm{\&upsi;}}_2\left(k\right)\right]}\underset{<}{\overset{>}{}}T\begin{array}{c}{M}_1\left(k\right)\\ M_2\left(k\right)\end{array}$

根据似然比函数计算马尔可夫转移矩阵中的各个变量，表达式如下：

$p_{12}\left(k\right)=f\left(\frac{p_{M_1}\left[{\mathrm{\&upsi;}}_1\left(k\right)\right]}{p_{M_2}\left[{\mathrm{\&upsi;}}_2\left(k\right)\right]}\right)$

8.根据权利要求1所述的一种自适应交互式多模型的机动目标跟踪方法，其特征在于，所述总体状态估计输出为：

$\hat{X}\left(k\right|k)=\underset{j=1}{\overset{r}{\mathrm{\&Sigma;}}}{\hat{X}}^j\left(k\right|k){\mathrm{\&mu;}}_j\left(k\right)$

对应的协方差矩阵为：

$P\left(k\right|k)=\underset{i=1}{\overset{r}{\mathrm{\&Sigma;}}}{\mathrm{\&mu;}}_j\left(k\right)\{P^j\left(k\right|k)+[{\hat{X}}^j\left(k\right|k)-\hat{X}\left(k\right|k)\left]{[{\hat{X}}^j\left(k\right|k)-\hat{X}\left(k\right|k)]}^T\right\}$

其中，μ_j(k)为观测向量Z(k)的条件下模型M_j(k)的后验概率，

为K时刻目标的状态估计输出，P^j(k|k)为K时刻第j个滤波器协方差矩阵。

Images