CN105699964A - 一种基于汽车防撞雷达的道路多目标跟踪方法 - Google Patents

Abstract

本发明公开一种基于汽车防撞雷达的道路多目标跟踪方法，通过有向图切换的无迹卡尔曼概率密度假设(DS-UKGMPHD)方法实现。主要包括获取先验信息、建立观测与运动模型、建立有向图Φ、建立自适应有向图切换准则、初始化及计算混合参数、各目标模型的UKGMPHD滤波、估计融合以及输出估计轨迹等步骤。本发明不仅适用于非线性、复杂运动状态的多个目标，而且兼顾机动目标跟踪实时性和可实现性，进一步提升汽车防撞雷达系统的实时性和可靠性，保障道路安全，在汽车防撞雷达信号处理领域具有广阔的应用前景。

Description

一种基于汽车防撞雷达的道路多目标跟踪方法

技术领域

本发明涉及汽车防撞雷达信号处理领域，尤其涉及一种基于汽车防撞雷达的道路多目标跟踪的方法。

背景技术

随着我国汽车的保有量的增加、汽车的行驶速度的提高，汽车的碰撞事故越来越多。近年来，汽车预警技术成为研究开发的热点，汽车防撞雷达作为一项主动防撞措施，能够追踪汽车前方一定距离内的单个或多个目标，获取其与本车的距离、速度和方位角等信息，并可根据预警系统在危险情况下发出报警信号，以提示驾驶员采取措施，避免由于外后视镜视野不足或驾驶员注意力不集中发生的撞车事故。

针对汽车防撞雷达道路多目标跟踪问题，现有技术的解决方法可归纳为两类。第一类是经典方法，主要基于滤波、数据关联及航迹管理；中国专利CN105000019A公开了一种用于检测、追踪和估计静止的路边物体的方法和系统，将来自几个前视雷达传感器的数据点用来确定是否存在静止的路边物体之前合并、归类和预分类数据点来减少数据量，通过数据拟合算法以估计物体的参数，但该专利仅针对路边静止的多目标进行跟踪与估计，未涉及机动目标的追踪问题；此外，随着道路车辆数目的增加，数据关联算法计算量将呈“组合爆炸”式增长，导致防撞系统实时性、准确度欠佳。第二类方法是基于随机有限集(RFS)理论，包括概率密度假设(PHD)方法及势概率假设密度(CPHD)方法；上述理论均可基于高斯假设(GM)和基于非高斯假设的贯序蒙特卡罗(SMC)方法实现；将汽车防撞雷达道路目标跟踪置于目标检测步骤，如恒虚警检测、快速傅里叶变换等之后，虚警概率已得到一定程度的降低，故从实时性和实用性角度出发，可使用高斯混合概率假设密度(GMPHD)方法作为一种基于高斯假设的跟踪滤波方法。张昱、宋骊平、虎小龙在“基于概率假设密度的汽车防撞雷达多目标跟踪”(现代雷达,2014,36(6):82-87.)一文中提出使用多模型GMPHD方法进行道路目标跟踪，然而该法存在以下缺陷：首先，GMPHD法仅适用于线性条件，而雷达极坐标形式的量测方程导致直角坐标下的跟踪问题为非线性；其次，基于少量模型的跟踪方法，难以适应实际情况中复杂的、运动的、多个道路目标。

针对上述问题，非汽车防撞雷达的多目标跟踪文献给出了解决方案，但并未全面解决这些问题。郝燕玲,孟凡彬,孙枫,沈锋等在“基于IMM的UK-GMPHDF算法在多机动目标跟踪中的应用”(系统工程理论与实践,2011,11:2225-2233)一文中，针对三维空中目标，基于交互多模型(IMM)，在高斯混合概率假设密度滤波器(GMPHDF)中加入无迹卡尔曼滤波(UKF)解决非线性多目标跟踪问题，但仿真使用模型仅为匀速模型、“当前”模型，且在三维空间实施；中国专利CN104020466A公开了一种基于变结构多模型的机动目标跟踪方法，利用现有的模型扩展方法BMA的候选模型集与现有的期望模式扩展EMA中求期望模型的方法构成一种新的模型集合自适应策略，从而利用通用的变结构交互多模型算法实现单目标的多运动状态跟踪，但无多目标估计能力。中国专利105261036A公开了一种基于匹配的目标跟踪方法，利用FPGA电路进行去均值归一化积运算，从而实现对目标的跟踪，但该专利涉及到跟踪图像，且主要依赖硬件实施，无法解决实际情况中复杂的多个运动目标跟踪问题。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于汽车防撞雷达的道路多目标跟踪方法，实现防撞雷达对道路多目标的数目、位置的实时可靠估计与准确跟踪。

本发明采用的技术方案是：以汽车防撞雷达对多目标探测后得到的数据为基础，基于变结构交互式多模型(IMM)方法，采用有向图切换(DS)法，在多个运动模型间实时选择切换合适的模型子集，并进行基于无迹变换的卡尔曼概率密度假设(UKGMPHD)滤波。

本发明具体技术方案为：

一种基于汽车防撞雷达的道路多目标跟踪方法，其步骤如下：

步骤1、获取先验信息，建立第一观测与运动模型。所述先验信息为通过汽车防撞雷达获取的目标状态信息。

确定状态方程量测方程及其道路目标的多种模型。依据实际情况选择不同模型，状态方程建立为：

其中，是目标模型的状态转移矩阵，下标m代表不同目标模型，下标k表示时刻k，下标i表示不同目标。假设噪声服从高斯分布，有

汽车防撞雷达观测向量θ_k为观测方位角，r_k为目标与观测点间距离。

步骤2、建立第二观测与运动模型，初始化有向图模型集，建立有向图Φ。

2(a)汽车防撞雷达车辆目标限于二维平面运动，目标i在k时刻的非线性量测模型如下：

其中，为高斯分布的量测噪声，x_s,k，y_s,k为自车观测位置坐标。

2(b)建立有向图Φ，及其有向子图Φ_(l){β_l-1,β_l,β_l+1}。

步骤3、建立自适应有向图切换准则。设k-1时刻子图为Φ^(k-1)，切换规则如下：

式中，τ_DS是模型转换的阈值概率，表示k-1时刻概率β^(i±1)；

步骤4、初始化及计算混合参数。

4(a)时刻k＝0时，每一向量子图D_n中含M个模型。对于当前模型q(q＝1,...,M)，初始化目标状态目标协方差矩阵模型概率

4(b)设当前时刻k-1,下一时刻为k。则对于k＞0时刻的第i个高斯分量，有

其中，为k-1时刻的高斯分量i模型p的出现概率，

进一步地，当前有向量子图内模型数M，交互初始PHD函数为：

混合权值、状态、协方差分别为：

上式中，

步骤5、各目标模型的UKGMPHD滤波。

以各高斯分量i在k-1时刻所得混合参数作为输入进行UKGMPHD滤波,得k时刻跟踪权值，状态及协方差矩阵

进一步地对步骤5描述，包括以下过程：

5(a)对k＝0时刻，初始化目标概率密度，有：

5(b)预测。对k＞0时刻，k-1时刻，高斯分量i总数J_k-1，概率密度假设函数D_k-1(x)为：

进一步地，所述预测概率密度假设函数D_k|k-1(x)依据对象的不同分为存活目标、新生目标及衍生目标概率密度假设函数。本发明道路多目标跟踪通常不涉及衍生目标，故概率密度假设函数为：D_k|k-1(x)＝D_S,k|k-1(x)+γ_k(x)

存活目标、新生目标概率密度假设函数分别如下：

使用无迹卡尔曼滤波的思想滤波。依据状态方程，一步预测均值方差如下：

式中，l＝0,……,2n为无迹滤波的Sigma点，L＝2n+1为Sigma点总数。则k时刻预测概率密度假设函数为：

5(c)更新。

对于雷达获取的非线性量测方程，一步预测均值、方差分别为：

结合步骤5(b)结果D_k|k-1(x)及检测概率P_D,k，k时刻的更新概率密度假设函数D_k(x)为：

D_D,k(x；z)、估计均值方差可表示为：

其中，

高斯分量i的估计均值和方差如下：

上式中，状态增益矩阵、协方差分别为：

5(d)剪枝。

对于更新得到的高斯分布进行剪枝，若高斯分布小于修剪阈值τ，直接予以删除。公式为：

5(e)合并。

对于分布接近的若干高斯分布，合并为一个新的分布，即集合：

合并后，新的高斯分布为：

5(f)目标数目与状态估计。

目标数目估计为融合估计的权值，即：

取整可得目标数目。按照上述步骤输出合并后状态记作

进一步地，在步骤5完成后，存在模型概率更新步骤；完成模型概率更新后，返回步骤4。所述模型概率更新步骤如下：

k时刻模型(q＝1,...,M)下高斯分量i的似然函数为模型q更新概率为：

步骤6、估计融合。估计融合PHD函数：

其中，J_k为高斯分量个数，高斯分量i的权值，均值，协方差分别为：

上式中，

步骤7、输出多目标的跟踪位置估计轨迹。

本发明方法可以应用于具有自动驾驶模式的车辆驾驶系统，本发明方法将关于行驶路段的多目标跟踪信息输出形成多目标跟踪位置估计轨迹，所述多目标跟踪位置估计轨迹信息或数据提供到一个或多个车辆驾驶系统，所述车辆驾驶系统基于本发明提供的信息或数据发出自动驾驶模式的命令。更进一步地，所述多目标跟踪位置估计轨迹信息或数据与来自车辆驾驶系统或其它终端的车辆导航系统的地图数据相结合，提升自动驾驶的用户体验与安全性。

本发明涉及的一种基于汽车防撞雷达的道路多目标跟踪方法，采用有向图切换的无迹卡尔曼概率密度假设(DS-UKGMPHD)方法，与现有技术相比，本发明达到的技术效果为：

1、本发明毋需数据关联、易于实施，适用于非线性、复杂运动状态下的多目标跟踪，与雷达极坐标形式的量测方程相匹配，保证了汽车防撞雷达多目标跟踪的实时性。

2、在道路车辆数目与模型覆盖较多的情况下，本发明实时剔除不必要模型，同时兼顾机动目标跟踪实时性和可实现性，提高了跟踪准确度。

3、本发明在杂波环境中实时估计多个目标的数目与位置等信息，进一步提高防撞雷达的可靠性，降低虚警率。

4、针对实际情况中复杂的、运动的、多个道路目标，本发明实现防撞雷达对道路多目标的数目、位置的实时可靠估计与准确跟踪，进一步提升汽车防撞雷达系统的实时性和可靠性，保障道路安全。

附图说明

图1为本发明有向图模型，其中l为模型序号，Φ₍₀₎为默认有向子图。

图2为本发明的实现流程图。

图3为本发明实施例2中杂波湮没的道路多机环境下各目标真实轨迹及汽车防撞雷达对各目标的量测点迹。

图4为本发明实施例2中使用DS-UKGMPHD方法对汽车防撞雷达多目标跟踪图。其中，“○”表示目标运动轨迹的起始端点，“☆”表示载有汽车防撞雷达的自车位置。

图5为本发明实施例2中使用IMM-UKGMPHD方法对汽车防撞雷达多目标跟踪图。

图6为本发明实施例2中使用DS-UKGMPHD方法，跟踪各目标在x方向随时间的目标位置-时间图。

图7为本发明实施例2中使用DS-UKGMPHD方法，跟踪各目标在y方向随时间的目标位置-时间图。

图8为本发明实施例2中使用IMM-UKGMPHD方法，跟踪各目标在x方向随时间的目标位置-时间图。

图9为本发明实施例2中使用IMM-UKGMPHD方法，跟踪各目标在y方向随时间的目标位置-时间图。

图10为本发明实施例2中使用DS-UKGMPHD方法与IMM-UKGMPHD方法目标个数跟踪对比图。

图11为本发明实施例2中使用DS-UKGMPHD方法与IMM-UKGMPHD方法跟踪多目标位置-时间对比图。

具体实施方式

下面通过具体实施例，进一步对本发明的技术方案进行具体说明。应该理解，下面的实施例只是作为具体说明，而不限制本发明的范围，同时本领域的技术人员根据本发明所做的显而易见的改变和修饰也包含在本发明范围之内。

实施例1

一种基于汽车防撞雷达的道路多目标跟踪方法，具体步骤如下：

步骤1、获取先验信息，建立第一观测与运动模型。所述先验信息为通过汽车防撞雷达获取的目标状态信息。

确定状态方程量测方程及其道路目标的多种模型。依据实际情况选择不同模型，状态方程建立为：

其中，是目标模型的状态转移矩阵，下标m代表不同目标模型，下标k表示时刻k，下标i表示不同目标。假设噪声服从高斯分布，有

汽车防撞雷达观测向量θ_k为观测方位角，r_k为目标与观测点间距离。

步骤2、建立第二观测与运动模型，初始化有向图模型集，建立有向图Φ。

2(a)汽车防撞雷达车辆目标限于二维平面运动，目标i在k时刻的非线性量测模型如下：

其中，为高斯分布的量测噪声，x_s,k，y_s,k为自车观测位置坐标。

2(b)建立有向图Φ，及其有向子图Φ_(l){β_l-1,β_l,β_l+1}。

步骤3、建立自适应有向图切换准则。设k-1时刻子图为Φ^(k-1)，切换规则如下：

式中，τ_DS是模型转换的阈值概率，表示k-1时刻概率β^(i±1)；

步骤4、初始化及计算混合参数。

4(a)时刻k＝0时，每一向量子图D_n中含M个模型。对于当前模型q(q＝1,...,M)，初始化目标状态目标协方差矩阵模型概率

4(b)设当前时刻k-1,下一时刻为k。则对于k＞0时刻的第i个高斯分量，有

其中，为k-1时刻的高斯分量k＞0模型p的出现概率，

当前有向量子图内模型数M，交互初始PHD函数为：

混合权值、状态、协方差分别为：

上式中，

步骤5、各目标模型的UKGMPHD滤波。

以各高斯分量i在k-1时刻所得混合参数作为输入进行UKGMPHD滤波,得k时刻跟踪权值，状态及协方差矩阵包括以下过程：

5(a)对k＝0时刻，初始化目标概率密度，有：

5(b)预测。对k＞0时刻，k-1时刻，高斯分量i总数J_k-1，概率密度假设函数D_k-1(x)为：

所述预测概率密度假设函数D_k|k-1(x)依据对象的不同分为存活目标、新生目标及衍生目标概率密度假设函数。本发明道路多目标跟踪通常不涉及衍生目标，故概率密度假设函数为：D_k|k-1(x)＝D_S,k|k-1(x)+γ_k(x)

存活目标、新生目标概率密度假设函数分别如下：

使用无迹卡尔曼滤波的思想滤波。依据状态方程，一步预测均值方差如下：

式中，l＝0,……,2n为无迹滤波的Sigma点，L＝2n+1为Sigma点总数。则k时刻预测概率密度假设函数为：

5(c)更新。

对于雷达获取的非线性量测方程，一步预测均值、方差分别为：

结合步骤5(b)结果D_k|k-1(x)及检测概率P_D,k，k时刻的更新概率密度假设函数D_k(x)为：

D_D,k(x；z)、估计均值方差可表示为：

其中，

高斯分量i的估计均值和方差如下：

上式中，状态增益矩阵、协方差分别为：

5(d)剪枝。

对于更新得到的高斯分布进行剪枝，若高斯分布小于修剪阈值τ，直接予以删除。公式为：

5(e)合并。

对于分布接近的若干高斯分布，合并为一个新的分布，即集合：

合并后，新的高斯分布为：

${\widetilde{\mathrm{\ω}}}_k^{\left(l\right)}=\frac{1}{{\stackrel{\‾}{\mathrm{\ω}}}_k^{\left(l\right)}}\underset{i\∈L}{\mathrm{\Σ}}{\mathrm{\ω}}_k^{\left(i\right)}{x}_k^{\left(i\right)}$

5(f)目标数目与状态估计。

目标数目估计为融合估计的权值，即：

取整可得目标数目。按照上述步骤输出合并后状态记作

在步骤5完成后，存在模型概率更新步骤；完成模型概率更新后，返回步骤4。所述模型概率更新步骤如下：

k时刻模型(q＝1,...,M)下高斯分量i的似然函数为模型q更新概率为：

步骤6、估计融合。估计融合PHD函数：

其中，J_k为高斯分量个数，高斯分量i的权值，均值，协方差分别为：

上式中，

步骤7、输出多目标的跟踪位置估计轨迹。

实施例2

本实施例基于计算机仿真对实施例1进行验证，验证环境为MatlabR2014a。具体实施方案如下：

1.仿真场景设置：

在二维平面区域[150m，150m]²内，模拟双向四车道道路环境，自左向右为第一车道至第四车道，以y轴正方向为第一、二车道流向。描述过程中，速度、加速度方向默认与流向一致。假设汽车防撞雷达探测范围在120m内，载有汽车防撞雷达的自车位置(40,0)m，并以25m/s的速度向y轴正方向做匀速运动。x坐标为各目标所处车道位置，y坐标为防撞雷达以自身为静止参考点探测到的目标相对位置。模拟道路目标运动情况如下：防撞雷达在0.1s于(62,120)m处探测到车辆目标1以24.8m/s的初速度与自车相向而行，做加速度0.21m/s²的匀减速运动，1.2s起以加速度0.08m/s²做匀加速运动，至4s恢复匀速运动；在2.1s时探测到车辆目标2在(70,106)m处以初速度22.3m/s从第四车道变道至第三车道，角速度为-0.1rad/s，3.6s起以角速度-0.04rad/s左转，驶回第四车道；探测到车辆目标3出现于3.1s在(22,3)m处，以初速度26.7m/s做加速度0.5m/s²的加速运动，在4.5s后做加速度为0.02m/s²的减速运动；车辆目标4在6s时在(18,3)m处被探测到以27.5m/s做匀速运动，6.8s起变道至第二条车道，该过程角速度分别为0.05rad/s，-0.05rad/s，9.3s时驶于自车前方以27.5m/s匀速行驶。

2.实施例验证

对于状态模型，取目标状态向量为各分量分别代表道路目标在x轴方向的坐标、速度，y轴方向的坐标、速度，以及x轴方向、y轴方向加速度。

使用五种模型进行跟踪，选取状态模型如下：

取有向子图切换概率阈值为0.65，马尔科夫转移概率矩阵为：

设噪声、杂波服从高斯分布，过程噪声协方差矩阵Q＝diag([5，2，5，2，0.5,0.5])，量测噪声协方差矩阵R＝diag([0.01，0.01])。目标存活概率P_S＝0.95，检测概率P_D＝0.99,合并门限为4，允许最大高斯分量个数J_max为100，剪枝阈值τ为10^-5，每次扫描平均杂波数为4。选取最优次模式分配(OSPA)距离为评价指标。状态模型中，取对应的角速度ω分别为-1rad/s,-0.05rad/s,0.05rad/s,1rad/s。仿真时长10s，采样间隔0.1s。

由仿真结果，防撞雷达在附图3的杂波道路及非线性条件下进行量测与跟踪。由附图4、附图6、附图7，DS-UKGMPHD算法跟踪位置与真实轨迹基本一致，而附图8、附图9表明IMM-UKGMPHD算法对目标的跟踪存在明显误差。由附图10，在使用5个跟踪模型的情况下，DS-UKGMPHD算法的目标误跟点个数较IMM-UKGMPHD算法更少。由附图11，在目标数目误跟时刻，两算法均出现峰值，而在其余时刻，DS-UKGMPHD算法的OSPA距离误差相对较小。

Claims (7)

1.一种基于汽车防撞雷达的道路多目标跟踪方法，具体步骤如下：

步骤1、获取先验信息，建立第一观测与运动模型；所述先验信息为通过汽车防撞雷达获取的目标状态信息；

确定状态方程量测方程 $z_k={\left[\begin{array}{cc}{\mathrm{\θ}}_k^i& r_k^i\end{array}\right]}^T$ 及其道路目标的多种模型；状态方程建立为： $x_k^i=F_{k,m}^i\left(x_{k-1}^i\right)+{\mathrm{\ω}}_{k,m}^i$

其中，是目标模型的状态转移矩阵，下标m代表不同目标模型，下标k表示时刻k，下标i表示不同目标；假设噪声服从高斯分布，有

汽车防撞雷达观测向量其中，θ_k为观测方位角，r_k为目标与观测点间距离；

步骤2、建立第二观测与运动模型，初始化有向图模型集，建立有向图Φ；

2(a)、汽车防撞雷达车辆目标限于二维平面运动，目标i在k时刻的非线性量测模型如下：

$z_k^i=\left[\begin{array}{c}{\mathrm{\θ}}_k^i\\ r_k^i\end{array}\right]=\left[\begin{array}{c}arctan\frac{y_k^i-y_{s,k}}{x_k^i-x_{s,k}}\\ \sqrt{{(x_k^i-x_{s,k})}^2+{(y_k^i-y_{s,k})}^2}\end{array}\right]+\left[\begin{array}{c}{v}_{\mathrm{\θ},k}^i\\ v_{r,k}^i\end{array}\right]$

其中，为高斯分布的量测噪声，x_s,k，y_s,k为自车观测位置坐标；

2(b)、建立有向图Φ，及其有向子图Φ_(l){β_l-1,β_l,β_l+1}；

步骤3、建立自适应有向图切换准则；设k-1时刻子图为Φ^(k-1)，切换规则如下：

式中，τ_DS是模型转换的阈值概率，表示k-1时刻概率β^(i±1)；

步骤4、初始化及计算混合参数；

4(a)、时刻k＝0时，每一向量子图D_n中含M个模型；对于当前模型q(q＝1,...,M)，初始化目标状态目标协方差矩阵模型概率

4(b)设当前时刻k-1,下一时刻为k；则对于k＞0时刻的第i个高斯分量，有：

${\mathrm{\μ}}_{k-1,pq}^{\left(i\right)}=\frac{1}{{\stackrel{\‾}{c}}_q}={\mathrm{\π}}_{pq}{\mathrm{\μ}}_{k-1,p}^i$

其中，为k-1时刻的高斯分量i模型p的出现概率；

步骤5、各目标模型的UKGMPHD滤波；

以各高斯分量i在k-1时刻所得混合参数作为输入进行UKGMPHD滤波,得k时刻跟踪权值，状态及协方差矩阵

步骤6、估计融合；估计融合PHD函数：

$D_k\left(x\right)=\underset{q=1}{\overset{M}{\Σ}}\underset{i=1}{\overset{J_k}{\Σ}}{\mathrm{\ω}}_{k,q}^{\left(i\right)}N(x,m_{k,q}^{\left(i\right)},P_{k,q}^{\left(i\right)})$

其中，J_k为高斯分量个数，高斯分量i的权值，均值，协方差分别为：

${\mathrm{\ω}}_k^{\left(i\right)}=\underset{q=1}{\overset{M}{\Σ}}{\mathrm{\ω}}_{k,q}^{\left(i\right)}{\mathrm{\μ}}_{k,q}^{\left(i\right)}$

$m_k^{\left(i\right)}=\underset{q=1}{\overset{M}{\Σ}}{m}_{k,q}^{\left(i\right)}{\mathrm{\μ}}_{k,q}^{\left(i\right)}$

上式中，

步骤7、输出多目标的跟踪位置估计轨迹。

2.根据权利要求1所述的一种基于汽车防撞雷达的道路多目标跟踪方法，其特征在于：步骤4还包括，当前有向量子图内模型数M，交互初始PHD函数为：

$D_{k-1,q}\left(x\right)=\underset{p=1}{\overset{M}{\Σ}}{D}_{k-1,p}\left(x\right){\mathrm{\μ}}_{k-1,pq}^{\left(i\right)}$

混合权值、状态、协方差分别为：

${\mathrm{\ω}}_{k-1,q}^{\left(i\right)}=\underset{p=1}{\overset{M}{\Σ}}{\mathrm{\ω}}_{k-1,p}^{\left(i\right)}{\mathrm{\μ}}_{k-1,pq}^{\left(i\right)}$

$m_{k-1,q}^{\left(i\right)}=\underset{p=1}{\overset{M}{\Σ}}{m}_{k-1,p}^{\left(i\right)}{\mathrm{\μ}}_{k-1,pq}^{\left(i\right)}$

上式中，

3.根据权利要求1所述的一种基于汽车防撞雷达的道路多目标跟踪方法，其特征在于：所述步骤5还包括以下过程：

5(a)、对k＝0时刻，初始化目标概率密度，有：

$D_0\left(x\right)=\underset{i=1}{\overset{J_0}{\Σ}}{\mathrm{\ω}}_0^{\left(i\right)}N(x;m_0^{\left(i\right)},P_0^{\left(i\right)})$

5(b)、预测；

对k＞0时刻，k-1时刻，高斯分量i总数J_k-1，概率密度假设函数D_k-1(x)为： $D_{k-1}\left(x\right)=\underset{i=1}{\overset{J_{k-1}}{\mathrm{\Σ}}}{\mathrm{\ω}}_{k-1}^{\left(i\right)}N(x;m_{k-1}^{\left(i\right)},P_{k-1}^{\left(i\right)})$

依据状态方程，一步预测均值方差如下：

$m_{k|k-1}^{\left(i\right)}=\underset{l=0}{\overset{2n}{\Σ}}{u}^{(i,l)}{x}_{k|k-1}^{(i,l)}$

$P_{k|k-1}^{\left(i\right)}=\underset{l=0}{\overset{L}{\Σ}}{u}^{(i,l)}(x_{k|k-1}^{(i,l)}-m_{k|k-1}^{\left(i\right)}){(x_{k|k-1}^{(i,l)}-m_{k|k-1}^{\left(i\right)})}^T+Q_{k-1}$

式中，l＝0,……,2n为无迹滤波的Sigma点，L＝2n+1为Sigma点总数；则k时刻预测概率密度假设函数为： $D_{k|k-1}\left(x\right)=\underset{i=1}{\overset{J_{k|k-1}}{\Σ}}{\mathrm{\ω}}_{k|k-1}^{\left(i\right)}N(x;m_{k|k-1}^{\left(i\right)},P_{k|k-1}^{\left(i\right)})$

5(c)更新；

对于雷达获取的非线性量测方程，一步预测均值、方差分别为：

${\mathrm{\η}}_{k|k-1}^{\left(i\right)}=\underset{l=0}{\overset{2n}{\Σ}}{u}^{(i,l)}{z}_{k|k-1}^{(i,l)}$

$S_k^{\left(i\right)}=\underset{l=0}{\overset{2n}{\Σ}}{u}^{(i,l)}(z_{k|k-1}^{(i,l)}-{\mathrm{\η}}_{k|k-1}^{\left(i\right)}){(z_{k|k-1}^{(i,l)}-{\mathrm{\η}}_{k|k-1}^{\left(i\right)})}^T+R_k$

结合步骤5(b)结果D_k|k-1(x)及检测概率P_D,k，k时刻的更新概率密度假设函数D_k(x)为：

$D_k\left(x\right)=(1-P_{D,k})D_{k|k-1}\left(x\right)+\underset{z\∈Z_k}{\Σ}{D}_{D,k}(x;z)$

D_D,k(x；z)、估计均值方差可表示为：

$D_{D,k}(x;z)=\underset{i=1}{\overset{J_{k|k-1}}{\Σ}}\frac{P_{D,k}{\mathrm{\ω}}_{k|k-1}^{\left(i\right)}{q}_k^{\left(i\right)}\left(z\right)N(x;m_{k|k}^{\left(i\right)}(z),P_k^{\left(i\right)})}{{\mathrm{\κ}}_k\left(z\right)+P_{D,k}{\mathrm{\Σ}}_{j=1}^{J_{k|k-1}}{\mathrm{\ω}}_{k|k-1}^{\left(j\right)}{q}_k^{\left(j\right)}\left(z\right)}$

其中， $q_k^{\left(i\right)}\left(z\right)=N(z;{\mathrm{\η}}_{k|k-1}^{\left(i\right)},S_k^{\left(i\right)});$

高斯分量i的估计均值和方差如下：

$m_k^{\left(i\right)}\left(z\right)=m_{k|k-1}^{\left(i\right)}+K_k^{\left(i\right)}(z-{\mathrm{\η}}_{k|k-1}^{\left(i\right)})$

$P_k^{\left(i\right)}=P_{k|k-1}^{\left(i\right)}-G_k^{\left(i\right)}{\[S_k^{\left(i\right)}\]}^{-1}{\[G_k^{\left(i\right)}\]}^T$

上式中，状态增益矩阵、协方差分别为：

$K_k^{\left(i\right)}=G_k^{\left(i\right)}{\[S_k^{\left(i\right)}\]}^{-1}$

$G_k^{\left(i\right)}=\underset{l=0}{\overset{2n}{\Σ}}{u}^{\left(l\right)}(x_{k|k-1}^{\left(l\right)}-m_{k|k-1}^{\left(l\right)}){(x_{k|k-1}^{\left(l\right)}-m_{k|k-1}^{\left(l\right)})}^T$

5(d)剪枝；

对于更新得到的高斯分布进行剪枝，若高斯分布小于修剪阈值τ，直接予以删除；公式为： $I=\{i=1,...,J_k|{\mathrm{\ω}}_k^{\left(i\right)}>\mathrm{\τ}\}$

5(e)合并；

对于分布接近的若干高斯分布，合并为一个新的分布，即集合：

$L=\{i\∈I|(m_k^{\left(i\right)}-m_k^{\left(j\right)}){\left(P_k^{\left(i\right)}\right)}^{-1}{(m_k^{\left(i\right)}-m_k^{\left(j\right)})}^T\≤U\}$

合并后，新的高斯分布为：

${\widetilde{\mathrm{\ω}}}_k^{\left(l\right)}=\frac{1}{{\stackrel{\‾}{\mathrm{\ω}}}_k^{\left(l\right)}}\underset{i\∈L}{\Σ}{\mathrm{\ω}}_k^{\left(i\right)}{x}_k^{\left(i\right)}$

${\tilde{P}}_k^{\left(i\right)}=\frac{1}{{\stackrel{\‾}{\mathrm{\ω}}}_k^{\left(l\right)}}\underset{i\∈L}{\Σ}{\mathrm{\ω}}_k^{\left(i\right)}(P_k^{\left(i\right)}+({\tilde{m}}_k^{\left(l\right)}-m_k^{\left(i\right)}\left){\left({\tilde{m}}_k^{\left(l\right)}-m_k^{\left(i\right)}\right)}^T\right)$

5(f)目标数目与状态估计；

目标数目估计为融合估计的权值，即：取整得到目标数目；

按照上述步骤输出合并后状态记作

4.根据权利要求3所述的一种基于汽车防撞雷达的道路多目标跟踪方法，其特征在于：所述步骤5(b)中，所述预测概率密度假设函数D_k|k-1(x)依据对象的不同分为存活目标和新生目标概率密度假设函数；

所述概率密度假设函数为：D_k|k-1(x)＝D_S,k|k-1(x)+γ_k(x)

所述存活目标、新生目标概率密度假设函数分别如下：

$D_{S,k|k-1}\left(x\right)=P_{S,k}\underset{i=1}{\overset{J_{k-1}}{\Σ}}{\mathrm{\ω}}_{S,k-1}^{\left(i\right)}N(x;m_{S,k-1}^{\left(i\right)},P_{S,k-1}^{\left(i\right)})$

${\mathrm{\γ}}_k\left(x\right)=\underset{i=1}{\overset{J_{\mathrm{\γ},k}}{\Σ}}{\mathrm{\ω}}_{\mathrm{\γ},k}^{\left(i\right)}N(x;m_{\mathrm{\γ},k}^{\left(i\right)},P_{\mathrm{\γ},k}^{\left(i\right)}).$

5.根据权利要求1～4所述的任一种基于汽车防撞雷达的道路多目标跟踪方法，其特征在于：在步骤5完成后，还存在模型概率更新步骤；在所述模型概率更新步骤完成后，返回步骤4；

所述模型概率更新步骤如下：

k时刻模型(q＝1,...,M)下高斯分量i的似然函数为模型q更新概率为：

${\mathrm{\Λ}}_{k,q}^{\left(i\right)}=N\left(Z_k\right|q,m_{k-1,q}^{\left(i\right)},{\tilde{P}}_{k-1,q}^{\left(i\right)})$

${\mathrm{\μ}}_{k,q}^{\left(i\right)}=\frac{1}{c^{\left(i\right)}}{\mathrm{\Λ}}_{k,q}^i\underset{p=1}{\overset{M}{\Σ}}{\mathrm{\π}}_{\mathrm{\ρ}q}{\mathrm{\μ}}_{k-1,p}^{\left(i\right)}.$

6.一种具有自动驾驶模式的车辆驾驶系统的方法，其特征在于：基于权利要求1～5所述的任一种基于汽车防撞雷达的道路多目标跟踪方法将关于相关行驶路段的多目标跟踪信息输出形成多目标跟踪位置估计轨迹，所述多目标跟踪位置估计轨迹信息或数据提供到一个或多个车辆驾驶系统，所述车辆驾驶系统基于所述多目标跟踪位置估计轨迹信息或数据发出自动驾驶模式的命令。

7.根据权利要求6所述一种具有自动驾驶模式的车辆驾驶系统的方法，其特征在于：所述多目标跟踪位置估计轨迹信息或数据与车辆导航系统的地图数据相结合；所述车辆导航系统来自于车辆驾驶系统或其它终端。