CN112034445B - 基于毫米波雷达的车辆运动轨迹跟踪方法和系统 - Google Patents

Abstract

本发明公开了一种基于毫米波雷达的车辆运动轨迹跟踪方法和系统，毫米波雷达实时获取车辆与雷达的距离、车辆相对于雷达的方位角和径向速度，其中跟踪方法包括：1、建立车辆状态转移方程和状态观测方程；2、获取车辆初始状态；设置粒子群并初始化每个粒子的状态；3、根据当前时刻毫米波雷达获取到的车辆观测量对粒子群进行第一次更新；4、对粒子群进行重新采样做第二次更新；5、根据第二次更新后的粒子权重和状态，计算当前时刻k车辆的状态估计，得到当前时刻车辆的位置和速度；6、跳转至步骤3进行下一时刻的车辆跟踪。该方法可以显著降低车辆观测数据中具有随机性和间歇性野值对车辆状态估计的影响，具有较强的抗奇异值能力。

Description

基于毫米波雷达的车辆运动轨迹跟踪方法和系统

技术领域

本发明属于目标跟踪定位技术领域，具体涉及一种车辆行驶中的跟踪方法和系统。

背景技术

随着汽车工业的飞速发展和人民生活水平的提高，汽车已然成为生活中必不可少的一部分，因此日益增加的汽车数量所带来的安全问题就越来越受到人们的关注。为了帮助人们判断行车情况，同时也为自动驾驶护航，高级驾驶辅助系统(Advanced DrivingAssistant System，ADAS)被引入大众的视野。ADAS利用安装在车上各式各样的传感器(如：毫米波雷达、激光雷达、单\双目摄像头以及卫星导航)收集环境信息以分析实时环境，其中车载毫米波雷达不易受到目标表面形状和颜色的影响，也不受天时天候的阻碍，具有环境适应性强，探测性能稳定的特点，是汽车安全技术的研究热点。在车辆A上安装车载毫米波雷达，对毫米波雷达发射波及回波的处理可以获取车辆A周围其他车辆相对于车辆A的大致距离、径向速度和方位角，在复杂的环境中如何利用这三个观测量准确跟踪目标车辆的行进轨迹是一个极富挑战性的研究课题。

考虑到现有毫米波器件、集成技术和相关的信号处理和解决方案尚且没有完全成熟，现有目标跟踪相关技术存在以下问题：传统的目标跟踪一般用卡尔曼滤波实现，这一滤波算法只有针对线性模型且在观测噪声和状态转移噪声均服从高斯分布的时候才是最优的。

大多数应用中针对非线性模型采用扩展卡尔曼滤波算法实现目标跟踪，即在短时间内将模型视为线性模型，然后利用线性卡尔曼滤波算法解决。该方法在做局部线性化时损失了一定的精度，尤其对于非线性度很强的模型，这样的算法往往跟踪效果不佳。

而且，传感器的观测噪声往往不服从高斯分布，其中一种典型的情况就是观测量中存在奇异值(或者叫野值)，其分布不满足高斯分布，这样的奇异值可能来源于实时情景中的目标遮挡或车辆突发抖动，针对这种情况，传统滤波算法的跟踪效果较差。

以上所述非线性模型的处理和非高斯观测噪声的存在都对目标跟踪算法提出了巨大的挑战。

发明内容

发明目的：针对现有技术中存在的问题，本发明提供了一种基于毫米波雷达的车辆运动轨迹跟踪方法，该方法可以显著降低车辆观测数据中具有随机性和间歇性野值对车辆状态估计的影响，具有较强的抗奇异值能力。

技术方案：本发明一方面公开了一种基于毫米波雷达的车辆运动轨迹跟踪方法，所述毫米波雷达用于实时获取车辆与雷达的距离、车辆相对于雷达的方位角和径向速度；包括：

(1)建立车辆状态转移方程：

其中

为k时刻车辆的状态量，x_k、y_k表示k时刻车辆相对于雷达的横、纵坐标，

表示k时刻车辆相对于雷达的横、纵向速度；f(·)为状态转移函数，A为状态转移矩阵，△t表示毫米波雷达的观测时间间隔，s_k为状态转移噪声，服从零均值、协方差为Q的高斯分布，即

建立车辆状态观测方程：

其中r_k、a_k、v_k分别为k时刻毫米波雷达获取的车辆与雷达的距离、车辆相对于雷达的方位角和径向速度；h(·)为观测函数；o_k为观测噪声，服从零均值、协方差为R/W_k的高斯分布，即

R为观测噪声协方差常数矩阵；W_k为观测噪声协方差权重矩阵，W_k＝diag([w_k,1,w_k,2,w_k,3])，每一权值w_k,m均服从Gamma分布，即

(2)给定车辆初始状态的期望<Φ₀>；产生包含S个粒子的集合，所述粒子集中第s个粒子在第k时刻的状态用

表示；以<Φ₀>为均值，Q为协方差的高斯分布随机初始化粒子群中每一个粒子的状态为

及其权重为

初始时刻表示为k＝0；

(3)令k＝k+1，根据k时刻毫米波雷达获取到的观测量z_k，第一次更新粒子群，更新后第s个粒子的状态

是一个以

为均值、Q为协方差的高斯变量，其权重

满足：

其中

为第s个粒子在(k-1)时刻完成第二次更新后的权重，

为观测量z_k的概率密度函数；

(4)对粒子群进行重新采样完成第二次更新，更新后粒子的状态为

权重为

(5)根据第二次更新后的粒子权重和状态，计算当前k时刻车辆的状态估计<Φ_k>，得到当前时刻k车辆相对于毫米波雷达的位置和速度；

(6)跳转至步骤(3)进行下一时刻的车辆跟踪。

将k时刻观测噪声协方差权重矩阵W_k视为时变参数矩阵是，所述步骤(3)可以采用如下步骤对粒子权重做第一次更新：

(3.1)将k时刻观测噪声协方差权重矩阵W_k视为时变参数矩阵，用EM算法估算W_k的估计值<W_k>：

计算w_k,m的估算值：

其中δ_k,m表示向量

的第m个元素，R_m,m代表矩阵R对角线上的第m个元素；则有：

<W_k>＝diag([<w_k,1>,<w_k,2>,<w_k,3>])；

(3.2)观测量z_k的概率密度函数为：

(3.3)粒子权重第一次更新的递推公式为：

将W_k视为与

相互独立的变量时，所述步骤(3)可以采用如下步骤对粒子权重做第一次更新：

(3.1')将W_k视为变量，与

相互独立，则观测量z_k的概率密度函数

的计算式为：

其中λ_m为矩阵R^-1对角线上的第m个元素，

等于向量

的第m个元素，z_k,m为观测量z_k的第m个元素；m＝1,2,3；

(3.2')粒子权重第一次更新的递推公式为：

所述步骤(4)具体包括：

(4.1)对粒子权重

做累积和：

构建累积和序列ψ_k，ψ_k＝[ψ_k,0,ψ_k,1,...,ψ_k,S]，其中ψ_k,0＝0，ψ_k,S＝1；ψ_k中相邻两个元素构成一个区间，总共有S个区间；

(4.2)在0到1之间产生随机数r，得到参考量

(4.3)构建等差数列D＝[d₁,d₂,…,d_S]，其中的第s个元素可以表示为：

统计数列D中的每一个元素d_s，位于ψ_k所构成的S个区间中的区间序号数，记为I_s，s＝1,...,S，构成序列I＝[I₁,I₂,…,I_S]；

(4.4)根据序列I对粒子群进行重采样，并更新粒子权重；所述第s个粒子重采样后的状态为：

权重为：

所述步骤(5)采用如下方式计算当前时刻k车辆相对于毫米波雷达的状态估计<Φ_k>：

<Φ_k>中的元素依次为当前时刻k车辆相对于毫米波雷达的横、纵坐标和横、纵向速度。

本发明还公开了实现上述车辆运动轨迹跟踪方法的系统，包括：

车辆跟踪模型建立模块，用于建立车辆状态转移方程和状态观测方法；

初始化模块，用于获取车辆的初始状态，设置粒子群中粒子的初始状态；

粒子群第一次更新模块，用于根据当前时刻k毫米波雷达获取到的车辆观测量z_k对粒子群进行第一次更新；

粒子群第二次更新模块，用于对粒子群进行重新采样做第二次更新；

车辆状态估计模块，用于根据第二次更新后的粒子权重和状态，计算当前时刻k车辆的状态估计，得到当前时刻k车辆的位置和速度。

有益效果：与现有技术相比，本发明公开的车辆运动轨迹跟踪方法具有以下优点：1、本发明通过对车辆状态观测方程中的观测噪声所服从的高斯分布的协方差引入重权值W_k，根据W_k的变化推导出实时观测结果的后验分布，从而引导粒子权重向更稳健的趋势更新，奇异值出现时观测结果对粒子权重计算的贡献会降低，因此估计系统状态时受奇异值影响较小，从而提高了粒子滤波的跟踪性能；2、由于引入权值向量而非单一权值，使得同一时刻毫米波雷达获得的不同维度观测量不会互相影响，某一维度出现的奇异值不会使得其他维度的观测对粒子权重计算的贡献同时受到抑制，进而更进一步地提高了算法的跟踪性能。

附图说明

图1为本发明公开的车辆运动轨迹跟踪方法的流程图；

图2为实施例1中车辆运动轨迹和雷达的理论观测结果图；

图3为实施例1中车辆运动轨迹和观测噪声服从高斯分布时的观测结果图；

图4为实施例1中跟踪轨迹与实际轨迹的对比图；

图5为实施例2中车辆运动轨迹和观测噪声服从t分布时的观测结果图；

图6为实施例2中跟踪轨迹与实际轨迹的对比图；

图7为本发明公开的车辆运动轨迹跟踪系统的组成示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明的具体实施案例做说明。

实施例1：

本发明公开了一种基于毫米波雷达的车辆运动轨迹跟踪方法，所述毫米波雷达用于实时获取车辆与雷达的距离、车辆相对于雷达的方位角和径向速度；如图1所示，包括如下步骤：

步骤1、建立车辆状态转移方程：

其中

为k时刻车辆的状态量，其维度N＝4，即Φ_k∈R⁴，x_k、y_k表示k时刻车辆相对于雷达的横、纵坐标，

表示k时刻车辆相对于雷达的横、纵向速度；f(·)为状态转移函数，A为状态转移矩阵，△t表示毫米波雷达的观测时间间隔，本实施例中本例中△t＝0.005s，s_k为状态转移噪声，s_k∈R⁴，服从零均值、协方差为Q的高斯分布，即

建立车辆状态观测方程：

其中r_k、a_k、v_k分别为k时刻毫米波雷达获取的车辆与雷达的距离、车辆相对于雷达的方位角和径向速度，即观测量的维度M＝3，z_k∈R³；h(·)为观测函数；o_k为观测噪声，服从零均值、协方差为R/W_k的高斯分布，即

o_k∈R³，R为观测噪声协方差常数矩阵；W_k为观测噪声协方差权重矩阵，R和Q的值可以根据实际情况调整，本实施例中取值为：Q＝diag([0.001,0.001,0.1,0.1])，R＝diag([10,0.001,2])。

W_k为对角阵，W_k＝diag([w_k,1,w_k,2,w_k,3])，每一权值w_k,m均服从Gamma分布，即

本实施例中v_m统一取值2，可以很好地处理经常出现奇异值的情形，如果奇异值出现的概率较小，则可以适当增大v_m的值。

步骤2、给定车辆初始状态的期望<Φ₀>；设置包括S个粒子的粒子群，所述粒子群中第s个粒子在k时刻的状态为

以<Φ₀>为均值，Q为协方差的高斯分布随机初始化粒子群中粒子群中每一个粒子的状态为

及其权重为

为车辆初始状态的期望；初始时刻表示为k＝0；

本实施例中，粒子群规模为1000，即S＝1000。粒子数量S越大，算法的精度越高，但同时计算量也会相应的增大，影响实时应用，反之粒子数越少则运算量也越少，对硬件计算速度的要求越低。因此得根据实际应用需求权衡S的取值，从而获得计算精度和速度上的最佳平衡。如图2所示，图2-(a)为本实施例中车辆的实际运动轨迹，图2-(b)、图2-(c)、图2-(d)分别为雷达对距离、方位角、径向速度的理想观测结果。

步骤3、令k＝k+1，根据当前k时刻毫米波雷达获取到的车辆观测量z_k对粒子群进行第一次更新；

根据贝叶斯方法，有如下后验期望计算：

其中Φ_0:k表示从初始时刻到第k时刻的车辆状态，q为重要性采样函数。

用ω_k表示

即为前文所述的粒子权重，再利用蒙特卡洛方法，就能获得下式：

其中

表示第s个粒子在k时刻的权重。

考虑给定观测z_1:k的条件下截止到k时刻的系统状态Φ_0:k的完整后验分布，利用模型的Markov性质，可以得到如下后验分布的递推公式：

因此，粒子的重要性权重可以用下式计算：

根据以上后验期望计算过程，应选取合适的重要性采样分布，如果构造的重要性采样满足下列关系：

那么可以获得以下粒子权重的递推关系式：

本发明中选取重要性采样如下：

那么粒子权重的递推关系式可简化为：

其中

为第s个粒子在(k-1)时刻的权重，

为观测量z_k的概率密度函数。因此，k时刻粒子的更新需要首先选取重要性采样分布，本发明中选取系统的状态转移模型，即：

作为重要性采样分布，这可以大大简化粒子权重的计算过程。以此更新粒子集，即对于每一个粒子

其更新后的状态是以

为均值，以Q为协方差的高斯变量，对粒子群中的所有粒子都做更新。

的计算，有以下两种方式：

方法A、将W_k视为时变参数矩阵，然后用EM算法估算，似然函数为：

其中δ_i,m表示向量

的第m个元素，计算时用

代替，而R_m,m代表矩阵R对角线上的第m个元素。另外，<Φ₀>表示给定的车辆的初始状态期望，Φ₀为车辆的实际初始状态，const表示常数。令lnp(Φ_0:k,z_1:k,W_1:k)>对w_k,m的偏导数为零，则得到w_k,m的估算公式如下式，本实施例中v_m统一取值为2：

以上<·>表示估计值，而参数矩阵W_k的估计可以由<w_k,m>构成：

<W_k>＝diag([<w_k,1>,<w_k,2>,<w_k,3>])

用<W_k>代替W_k，则观测概率模型p(z_k|Φ_k)|_Wk＝Wk>为高斯分布，因此可以得到粒子权重的计算公式为：

方法B、将W_k视为变量，与

相互独立，则有观测概率密度函数：

其中λ_m等于矩阵R^-1对角线上的第m个元素，

等于向量

z_k,m为z_k的第m个元素。另外St(·)表示Student-t分布，Gam(·)表示Gamma分布；Γ(·)是Gamma计算函数的表示。这样便获得了粒子权重的递推公式：

因为在每一个时间步做完第二次粒子更新时，计算完所有粒子的权重

(s＝1,...,1000)后需要做归一化，因此可以省略每一个粒子权重递推公式中同乘的系数；

步骤4、对粒子群进行重新采样做第二次更新，具体步骤包括；

(4.1)对粒子权重

做累积和：

构建累积和序列ψ_k，ψ_k＝[ψ_k,0,ψ_k,1,...,ψ_k,S]，其中ψ_k,0＝0，ψ_k,S＝1；ψ_k中相邻两个元素构成一个区间，总共有S个区间；

(4.2)在0到1之间产生随机数r，得到参考量

(4.3)构建等差数列D＝[d₁,d₂,…,d_S]，其中的第s个元素可以表示为：

统计数列D中的每一个元素d_s，位于ψ_k所构成的S个区间中的区间序号数，记为I_s，构成序列I＝[I₁,I₂,…,I_S]；

(4.4)根据序列I对粒子群进行重采样，并更新粒子权重；所述第s个粒子重采样后的状态为：

权重为：

以上过程可以解释为：如果重采样前某一个粒子

所对应的权重

很大，那么它所形成区间[ψ_k,s-1,ψ_k,s]相对于其他的区间就更大，因此，等差数列D中元素处于该区间的概率就更大，个数也就更多，那么导致序列I中会有很多元素，其值恰好对应这个大权重粒子，即序列I中等于I_s的元素较多，那么重新采样更新粒子的时候，就会有出现更多的粒子等于

例如

换句话说，就是将这一个大权重粒子

复制成多个，相反的，权重小的粒子，就更有可能会被剔除。

步骤5、根据第二次更新后的粒子

及对应的权重

估计当前时刻k车辆的状态，用<Φ_k>表示，即为当前时刻k车辆相对于毫米波雷达的位置和速度；

采用如下方式计算当前时刻k车辆的状态估计<Φ_k>：

<Φ_k>中的元素依次为当前时刻k车辆相对于毫米波雷达的横、纵坐标和横、纵向速度；

步骤6、跳转至步骤3进行下一时刻的车辆跟踪。

本实施例中毫米波雷达的观测量加高斯分布噪声，其能量与协方差矩阵R相对应，雷达的观测结果如图3所示，其中图3-(a)为车辆运动轨迹，图3-(b)、图3-(c)、图3-(d)分别为雷达对距离、方位角、径向速度的实际测量结果。将本发明的跟踪轨迹结果与实际的车辆轨迹进行对比，利用定位的均方误差MSE来评估效果，得到的结果为：

其中Num表示雷达连续观测的次数，<x_k>、<y_k>表示k时刻算法估计得到的横、纵坐标，x_k、y_k表示k时刻车辆实际的横、纵坐标。其跟踪轨迹与实际轨迹的对比如图4所示，(a)-(f)分别为车辆实际轨迹与跟踪轨迹的对比、横向距离对比、纵向距离对比、本发明的均方误差、横向速度对比、纵向速度对比。可以看出：本发明公开的方法在观测噪声服从高斯分布的条件下具有很好的跟踪性能。

实施例2：

为了检验本发明公开的方法对奇异值的鲁棒性，在实施例1的基础上将观测噪声由高斯分布换成student-t分布进行仿真实验，具体步骤如下：

首先根据车辆的运动轨迹，计算雷达的理论观测值z；然后利用MATLAB产生自由度为2的t分布随机数r₂，并用相应的噪声标准差(即协方差矩阵R对角线上的各个元素)乘以该随机数，叠加到z上作为实际观测结果，如图5所示。最后利用本发明公开的方法跟踪车辆的轨迹，对比结果如图6所示。

将本发明方法的跟踪轨迹结果与实际的车辆轨迹进行对比，利用定位的均方误差MSE来评估效果，得到的结果为：

其中Num表示雷达连续观测的次数，<x_k>、<y_k>表示k时刻算法估计得到的横、纵坐标，x_k、y_k表示k时刻车辆实际的横、纵坐标。可以看出：本发明对观测数据中的奇异值具有很强的鲁棒性，可以有效提高系统状态的估计精度，抑制观测数据中的奇异值对系统状态估计的影响。

实施例3：

实现上述车辆运动轨迹跟踪方法的系统如图7所示，包括：

车辆跟踪模型建立模块1，用于建立车辆状态转移方程和状态观测方程；

初始化模块2，用于获取车辆的初始状态，设置粒子群中粒子的初始状态；

粒子群第一次更新模块3，用于根据当前时刻k毫米波雷达获取到的车辆观测量z_k对粒子群进行第一次更新；

粒子群第二次更新模块4，用于对粒子群进行重新采样做第二次更新；

车辆状态估计模块5，用于根据第二次更新后的粒子权重和状态，计算当前时刻k车辆的状态估计，得到当前时刻k车辆相对于毫米波雷达的位置和速度。

Claims (8)

1.基于毫米波雷达的车辆运动轨迹跟踪方法，所述毫米波雷达用于实时获取车辆与雷达的距离、车辆相对于雷达的方位角和径向速度；其特征在于，包括如下步骤：

(1)建立车辆状态转移方程：

其中

为k时刻车辆的状态量，x_k、y_k表示k时刻车辆相对于雷达的横、纵坐标，

表示k时刻车辆相对于雷达的横、纵向速度；f(·)为状态转移函数，A为状态转移矩阵，△t表示毫米波雷达的观测时间间隔，s_k为状态转移噪声，服从零均值、协方差为Q的高斯分布，即

建立车辆状态观测方程：

其中r_k、a_k、v_k分别为k时刻毫米波雷达获取的车辆与雷达的距离、车辆相对于雷达的方位角和径向速度；h(·)为观测函数；o_k为观测噪声，服从零均值、协方差为R/W_k的高斯分布，即

R为观测噪声协方差常数矩阵；W_k为观测噪声协方差权重矩阵，W_k＝diag([w_k,1,w_k,2,w_k,3])，每一权值w_k,m均服从Gamma分布，即

(2)给定车辆初始状态的期望<Φ₀>；产生包含S个粒子的集合，所述粒子集中第s个粒子在第k时刻的状态用

表示；以<Φ₀>为均值，Q为协方差的高斯分布随机初始化粒子群中每一个粒子的状态

及其权重为

初始时刻表示为k＝0；

(3)令k＝k+1，根据k时刻毫米波雷达获取到的观测量z_k，第一次更新粒子群，更新后第s个粒子的状态

是一个以

为均值、Q为协方差的高斯变量，相应的权重

满足：

其中

为第s个粒子在(k-1)时刻完成第二次更新后的权重，

为观测量z_k的概率密度函数；

(4)对粒子群进行重新采样完成第二次更新，更新后粒子的状态为

权重为

所述步骤(4)具体包括：

(4.1)对粒子权重

做累积和：

构建累积和序列ψ_k，ψ_k＝[ψ_k,0,ψ_k,1,...,ψ_k,S]，其中ψ_k,0＝0，ψ_k,S＝1；ψ_k中相邻两个元素构成一个区间，总共有S个区间；

(4.2)在0到1之间产生随机数r，得到参考量

(4.3)构建等差数列D＝[d₁,d₂,…,d_S]，其中的第s个元素可以表示为：

统计数列D中的每一个元素d_s，位于ψ_k所构成的S个区间中的区间序号数，记为I_s，s＝1,...,S，构成序列I＝[I₁,I₂,…,I_S]；

(4.4)根据序列I对粒子群进行重采样，并更新粒子权重；所述第s个粒子重采样后的状态为：

权重为：

(5)根据第二次更新后的粒子权重和状态，计算当前时刻k车辆的状态估计<Φ_k>，得到当前时刻k车辆相对于毫米波雷达的位置和速度；

(6)跳转至步骤(3)进行下一时刻的车辆跟踪。

2.根据权利要求1所述的车辆运动轨迹跟踪方法，其特征在于，所述步骤(3)的步骤为：

(3.1)将k时刻观测噪声协方差权重矩阵W_k视为时变参数矩阵，用EM算法估算W_k的估计值<W_k>：

计算w_k,m的估算值：

其中δ_k,m表示向量

的第m个元素，R_m,m代表矩阵R对角线上的第m个元素；则有：

<W_k>＝diag([<w_k,1>,<w_k,2>,<w_k,3>])；

(3.2)观测量z_k的概率密度函数为：

(3.3)粒子权重第一次更新的递推公式为：

3.根据权利要求1所述的车辆运动轨迹跟踪方法，其特征在于，所述步骤(3)的步骤为：

(3.1')将W_k视为变量，与

相互独立，则观测量z_k的概率密度函数

的计算式为：

其中λ_m为矩阵R^-1对角线上的第m个元素，

等于向量

的第m个元素，z_k,m为观测量z_k的第m个元素；m＝1,2,3；

(3.2')粒子权重第一次更新的递推公式为：

4.根据权利要求1所述的车辆运动轨迹跟踪方法，其特征在于，所述步骤(5)采用以下方式计算当前时刻k车辆相对于毫米波雷达的状态估计<Φ_k>：

<Φ_k>中的元素依次为当前时刻k车辆相对于毫米波雷达的横、纵坐标和横、纵向速度。

5.基于毫米波雷达的车辆运动轨迹跟踪系统，所述毫米波雷达用于实时获取车辆与雷达的距离、车辆相对于雷达的方位角和径向速度；其特征在于，包括：

车辆跟踪模型建立模块，用于建立车辆状态转移方程和状态观测方程；所述车辆状态转移方程为：

其中

为k时刻车辆的状态量，x_k、y_k表示k时刻车辆相对于雷达的横、纵坐标，

表示k时刻车辆相对于雷达的横、纵向速度；f(·)为状态转移函数，A为状态转移矩阵，△t表示毫米波雷达的观测时间间隔，s_k为状态转移噪声，服从零均值、协方差为Q的高斯分布，即

所述车辆状态观测方程为：

其中r_k、a_k、v_k分别为k时刻毫米波雷达获取的车辆与雷达的距离、车辆相对于雷达的方位角和径向速度；h(·)为观测函数；o_k为观测噪声，服从零均值、协方差为R/W_k的高斯分布，即

R为观测噪声协方差常数矩阵；W_k为观测噪声协方差权重矩阵，W_k＝diag([w_k,1,w_k,2,w_k,3])，每一权值w_k,m均服从Gamma分布，即

初始化模块，用于获取车辆的初始状态，设置粒子群中粒子的初始状态；所述粒子群为包含S个粒子的集合，所述粒子群中第s个粒子在第k时刻的状态用

s＝1,2,…,S表示；以车辆初始状态的期望<Φ₀>为均值，Q为协方差的高斯分布随机初始化粒子群中每一个粒子的状态

及其权重为

初始时刻表示为k＝0；

粒子群第一次更新模块，用于根据当前时刻k毫米波雷达获取到的车辆观测量z_k对粒子群进行第一次更新，更新后第s个粒子的状态

是一个以

为均值、Q为协方差的高斯变量，相应的权重

满足：

其中

为第s个粒子在(k-1)时刻完成第二次更新后的权重，

为观测量z_k的概率密度函数；

粒子群第二次更新模块，用于对粒子群进行重新采样做第二次更新；所述粒子群第二次更新模块采用如下步骤对粒子进行第二次更新：

(4.1)对粒子权重

做累积和：

构建累积和序列ψ_k，ψ_k＝[ψ_k,0,ψ_k,1,...,ψ_k,S]，其中ψ_k,0＝0，ψ_k,S＝1；ψ_k中相邻两个元素构成一个区间，总共有S个区间；

(4.2)在0到1之间产生随机数r，得到参考量

(4.3)构建等差数列D＝[d₁,d₂,…,d_S]，其中的第s个元素可以表示为：

统计数列D中的每一个元素d_s，位于ψ_k所构成的S个区间中的区间序号数，记为I_s，s＝1,...,S，构成序列I＝[I₁,I₂,…,I_S]；

(4.4)根据序列I对粒子群进行重采样，并更新粒子权重；所述第s个粒子重采样后的状态为：

权重为：

车辆状态估计模块，用于根据第二次更新后的粒子权重和状态，计算当前时刻k车辆的状态估计，得到当前时刻k车辆相对于毫米波雷达的位置和速度。

6.根据权利要求5所述的车辆运动轨迹跟踪系统，其特征在于，所述粒子群第一次更新模块将k时刻观测噪声协方差权重矩阵W_k视为时变参数矩阵，采用如下步骤更新粒子的权重：

(3.1)用EM算法估算W_k的估计值<W_k>：

计算w_k,m的估算值：

其中δ_k,m表示向量

的第m个元素，R_m,m代表矩阵R对角线上的第m个元素；则有：

<W_k>＝diag([<w_k,1>,<w_k,2>,<w_k,3>])；

(3.2)观测量z_k的概率密度函数为：

(3.3)粒子权重第一次更新的递推公式为：

7.根据权利要求5所述的车辆运动轨迹跟踪系统，其特征在于，所述粒子群第一次更新模块将W_k视为与

相互独立的变量，采用如下步骤更新粒子的权重：

(3.1')计算观测量z_k的概率密度函数

其中λ_m为矩阵R^-1对角线上的第m个元素，

等于向量

的第m个元素，z_k,m为观测量z_k的第m个元素；m＝1,2,3；

(3.2')粒子权重第一次更新的递推公式为：

8.根据权利要求5所述的车辆运动轨迹跟踪系统，其特征在于，所述车辆状态估计模块采用以下方式计算当前时刻k车辆的状态估计<Φ_k>：

<Φ_k>中的元素依次为当前时刻k车辆相对于毫米波雷达的横、纵坐标和横、纵向速度；

为粒子群第二次更新模块更新后的粒子状态。

Images