CN105549003A - 一种汽车雷达目标跟踪方法 - Google Patents

Abstract

本发明涉及一种汽车雷达目标跟踪方法，其包括：步骤S1，设置过程噪声协方差矩阵、测量噪声协方差矩阵R以及采样权重参数；步骤S2，初始化运动状态向量；对状态误差协方差矩阵进行乔利斯基分解以获得分解矩阵，并初始化该分解矩阵；步骤S3，计算获得西格玛点及其均值权重参数和方差权重参数；步骤S4，计算获得运动状态向量预测值和状态预测分解矩阵；步骤S5，计算获得测量值向量预测值和测量预测分解矩阵；步骤S6，计算获得卡尔曼增益；步骤S7，计算获得运动状态向量估计值和分解矩阵估计值；步骤S8，返回执行所述步骤S2。本发明一方面减少了计算量，另一方面基本消除了状态误差协方差矩阵非正定导致算法停滞的隐患。

Description

一种汽车雷达目标跟踪方法

技术领域

本发明涉及目标跟踪领域，尤其涉及一种基于平方根不敏卡尔曼滤波(SRUKF)的汽车雷达目标跟踪方法。

背景技术

卡尔曼滤波(KF)是目标跟踪领域应用广泛的一种跟踪算法。卡尔曼滤波器的作用在于，通过对运动目标运动过程和传感器测量过程建模，利用间接、含噪声的测量值，尽量准确地估计目标的运动状态。运动状态包括目标的位置、速度、加速度信息。传感器测量模型中包含有噪声(称为测量噪声)，这代表着对目标的测量含有一定的不准确性；目标运动过程模型中也包含有噪声(称为过程噪声)，这代表运动目标的运动过程并不完全符合运动模型的假设。虽然真实世界的很多动态系统都并不能确切地符合所假设的运动模型，但是由于卡尔曼滤波器被设计在有噪声的情况下工作，因此一个近似的符合已经可以使这个滤波器非常有用了。滤波的意义在于，通过假设运动模型和测量模型，减少测量噪声和过程噪声对目标跟踪效果的影响，从而更精确的估计目标车辆的运动状态。

传统的卡尔曼滤波器只适用于线性系统，即运动模型和测量模型必须是线性系统。而在车辆目标跟踪领域，这常常是难以达到的。多数情况下，目标车辆运动状态可以用一个简单的八维状态空间向量X表示：其中包含在车体坐标系下的目标的横向距离x、横向速度横向加速度横向加加速度纵向距离y、纵向速度纵向加速度以及纵向加加速度而车载传感器(以毫米波雷达为例)，测得的目标信息则用测量值空间向量Y表示：Y＝[r,a,v]^T，其中包含目标距离r、目标角度a以及目标径向速度v。由于状态空间向量X和测量值空间向量Y之间不满足线性关系，状态空间向量X到测量值空间向量Y的映射由测量模型h表示：因此，该测量模型h也不是线性系统，从而使得传统的卡尔曼滤波器无法使用。

为此，需要对传统的卡尔曼滤波器进行改进，由此产生了扩展卡尔曼滤波器(EKF)，这是为了解决非线性问题而提出的一种滤波器。然而，EKF应对非线性问题是通过将非线性系统进行一阶线性化来进行滤波的。这种方式虽然能得到结果，但是在后验状态均值和后验状态协方差上往往会出现较大误差，甚至出现滤波器发散的情况。

鉴于上述情况，现有技术中进一步开发了不敏卡尔曼滤波器(UKF)，这是另一种适用于非线性系统的卡尔曼滤波器。UKF相比于EKF在后验状态均值和后验状态协方差上有了很大的提高，而且和EKF有同阶的计算复杂度。UKF并不对非线性系统进行线性化，而是在状态空间向量和测量值空间向量进入非线性系统之前，用一组特殊的权值进行采样，这组采样点称为西格玛点，这组西格玛点能准确的反映原分布的均值和协方差。让这组采样点通过非线性系统后，再对输出值加权求和，这样得到的后验均值和后验协方差能达到三阶泰勒展开式的精度。这一整个过程称为UT变换。然而在实际使用UKF进行滤波时，仍会遇到一些困难。具体来说，在UT变换中，需要对协方差矩阵求乔利斯基分解，因此，一方面，这要求运算过程中协方差矩阵始终保持正定，另一方面，这带来的计算量也较大。由于目标跟踪领域对实时性的要求较高，传感器频率比较高，对算法迭代速度的要求也比较高，因此UT变换的高复杂度和运算过程中可能导致的不正定问题就成了限制UKF算法应用的瓶颈。

发明内容

为了解决上述现有技术存在的问题，本发明旨在提供一种汽车雷达目标跟踪方法，以克服现有的汽车雷达目标跟踪方法中UKF滤波器运算性能上的不足，提高该跟踪方法的实时性和稳定性。

本发明所述的一种汽车雷达目标跟踪方法，其包括：步骤S0，构建目标车辆运动模型以及汽车雷达测量模型，并将目标车辆的运动状态标记为L维的运动状态向量X，将汽车雷达测得的目标车辆信息标记为测量值向量Y；根据所述目标车辆运动模型获得状态误差协方差矩阵P和过程噪声协方差矩阵Q，根据所述汽车雷达测量模型获得测量噪声协方差矩阵R，其特征在于，该方法还包括以下步骤：

步骤S1，将所述过程噪声协方差矩阵Q和所述测量噪声协方差矩阵R分别设置为常数矩阵，并设置采样权重参数α、β和θ；

步骤S2，初始化第k个时刻的运动状态向量X_k；对第k个时刻的状态误差协方差矩阵P_k进行乔利斯基分解以获得分解矩阵S_k，并初始化该分解矩阵S_k；

步骤S3，计算获得初始化后的运动状态向量X_k所对应的2L+1个西格玛点σ_x,k及其均值权重参数W_m和方差权重参数W_c；

步骤S4，计算获得目标车辆在第k+1个时刻的运动状态向量预测值并根据该运动状态向量预测值所述过程噪声协方差矩阵Q、所述西格玛点σ_x,k以及所述方差权重参数W_c，计算获得第k+1个时刻的状态预测分解矩阵

步骤S5，计算获得第k+1个时刻的测量值向量预测值并根据该测量值向量预测值所述测量噪声协方差矩阵R、所述西格玛点σ_x,k以及所述方差权重参数W_c，计算获得第k+1个时刻的测量预测分解矩阵

步骤S6，根据所述西格玛点σ_x,k、所述方差权重参数W_c、所述运动状态向量预测值所述测量值向量预测值以及所述测量预测分解矩阵计算获得卡尔曼增益K_k+1；

步骤S7，计算获得目标车辆在第k+1个时刻的运动状态向量估计值根据所述状态预测分解矩阵测量预测分解矩阵以及卡尔曼增益K_k+1，计算获得第k+1个时刻的分解矩阵估计值

步骤S8，返回执行所述步骤S2，并使k＝k+1，直至目标车辆停止运动。

在上述汽车雷达目标跟踪方法中，所述步骤S2包括：当k＝0时，采用通过所述汽车雷达获得的测量值向量Y₀初始化所述运动状态向量X_k，且将所述分解矩阵S_k初始化为L维的单位矩阵；当k≠0时，将所述运动状态向量X_k和分解矩阵S_k分别设置为：其中，表示目标车辆在第k个时刻的运动状态向量估计值，表示第k个时刻的分解矩阵估计值。

在上述汽车雷达目标跟踪方法中，所述步骤S3包括：根据以下公式可获得2L+1个西格玛点σ_x,k及其均值权重参数W_m和方差权重参数W_c：

${\mathrm{\σ}}_{x,k}^{\left(0\right)}=X_k;$

${\mathrm{\σ}}_{x,k}^{\left(i\right)}=X_k+{\mathrm{\ηS}}_k^{\left(i\right)},i=1,...L;$

${\mathrm{\σ}}_{x,k}^{\left(i\right)}=X_k-{\mathrm{\ηS}}_k^{\left(i\right)},i=L,...2L;$

$W_m^{\left(0\right)}=\frac{\mathrm{\λ}}{L+\mathrm{\λ}};$

$W_c^{\left(0\right)}=\frac{\mathrm{\λ}}{L+\mathrm{\λ}}+(1-{\mathrm{\α}}^2+\mathrm{\β});$

$W_c^{\left(i\right)}=W_m^{\left(i\right)}=\frac{1}{2(L+\mathrm{\λ})},i=1,...,2L;$

其中，λ＝α²(L+θ)-L，L为所述运动状态向量X的维度，α,β,θ为所述采样权重参数；和分别对应第i个西格玛点的均值权重参数和方差权重参数。

在上述汽车雷达目标跟踪方法中，所述步骤S4包括：首先，将所述西格玛点σ_x,k代入所述目标车辆运动模型的状态转移函数f(·)中，以获得第k+1个时刻的预测采样点集σ_x,k+1，其中，所述状态转移函数f(·)为：

$f(\·)=\left[\begin{array}{cccccccc}1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3& 0& 0& 0& 0\\ 0& 1& dt& 0.5*{\mathrm{dt}}^2& 0& 0& 0& 0\\ 0& 0& 1& dt& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3\\ 0& 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2\\ 0& 0& 0& 0& 0& 0& 1& dt\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right];$

其中，dt表示所述汽车雷达的采样周期；

然后，根据所述预测采样点集σ_x,k+1以及所述均值权重参数W_m，并通过以下公式计算获得所述目标车辆在第k+1个时刻的运动状态向量预测值

${\stackrel{\‾}{X}}_{k+1}={\Σ}_{i=0}^{2L}{W}_m^{\left(i\right)}{\mathrm{\σ}}_{x,k+1}^{\left(i\right)}.$

在上述汽车雷达目标跟踪方法中，所述步骤S4包括：将所述西格玛点σ_x,k代入所述目标车辆运动模型的状态转移函数f(·)中，以获得第k+1个时刻的预测采样点集σ_x,k+1，其中，所述状态转移函数f(·)为：

$f(\·)=\left[\begin{array}{cccccccc}1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3& 0& 0& 0& 0\\ 0& 1& dt& 0.5*{\mathrm{dt}}^2& 0& 0& 0& 0\\ 0& 0& 1& dt& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3\\ 0& 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2\\ 0& 0& 0& 0& 0& 0& 1& dt\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right];$

其中，dt表示所述汽车雷达的采样周期；

然后，根据所述预测采样点集σ_x,k+1、所述运动状态向量预测值所述过程噪声协方差矩阵Q以及所述方差权重参数W_c，并通过以下公式计算获得所述第k+1个时刻的状态预测分解矩阵

${\stackrel{\‾}{s}}_{x,k+1}=qr\left\{\[\sqrt{W_c^{\left(1\right)}}\left({\mathrm{\σ}}_{x,k+1}^{(1:2L)}-{\stackrel{\‾}{X}}_{k+1}\right)\sqrt{Q}\]\right\};$

${\stackrel{\‾}{S}}_{x,k+1}=cholupdate({\stackrel{\‾}{s}}_{x,k+1},{\mathrm{\σ}}_{x,k+1}^{\left(0\right)}-{\stackrel{\‾}{X}}_{k+1},W_c^{\left(0\right)}).$

在上述汽车雷达目标跟踪方法中，所述步骤S5包括：首先，将所述西格玛点σ_x,k代入所述目标车辆运动模型的状态转移函数f(·)中，以获得第k+1个时刻的预测采样点集σ_x,k+1，其中，所述状态转移函数f(·)为：

$f(\·)=\left[\begin{array}{cccccccc}1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3& 0& 0& 0& 0\\ 0& 1& dt& 0.5*{\mathrm{dt}}^2& 0& 0& 0& 0\\ 0& 0& 1& dt& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3\\ 0& 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2\\ 0& 0& 0& 0& 0& 0& 1& dt\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right];$

其中，dt表示所述汽车雷达的采样周期；

然后，将所述预测采样点集σ_x,k+1代入所述汽车雷达测量模型的测量函数h(·)中，以获得第k+1个时刻的预测测量值点集σ_y,k+1，其中，所述测量函数h(·)为：

$h(\·)=\left[\begin{array}{c}\sqrt{x^2+y^2}\\ \arctan (y/x)\\ (x\stackrel{\·}{x}+y\stackrel{\·}{y})/\sqrt{x^2+y^2}\end{array}\right];$

最后，根据所述预测测量值点集σ_y,k+1以及所述均值权重参数W_m，并通过以下公式计算获得所述第k+1个时刻的测量值向量预测值

${\stackrel{\‾}{Y}}_{k+1}={\Σ}_{i=0}^{2L}{W}_m^{\left(i\right)}{\mathrm{\σ}}_{y,k+1}^{\left(i\right)}.$

在上述汽车雷达目标跟踪方法中，所述步骤S5包括：首先，将所述西格玛点σ_x,k代入所述目标车辆运动模型的状态转移函数f(·)中，以获得第k+1个时刻的预测采样点集σ_x,k+1，其中，所述状态转移函数f(·)为：

$f(\·)=\left[\begin{array}{cccccccc}1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3& 0& 0& 0& 0\\ 0& 1& dt& 0.5*{\mathrm{dt}}^2& 0& 0& 0& 0\\ 0& 0& 1& dt& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3\\ 0& 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2\\ 0& 0& 0& 0& 0& 0& 1& dt\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right];$

其中，dt表示所述汽车雷达的采样周期；

然后，将所述预测采样点集σ_x,k+1代入所述汽车雷达测量模型的测量函数h(·)中，以获得第k+1个时刻的预测测量值点集σ_y,k+1，其中，所述测量函数h(·)为：

$h(\·)=\left[\begin{array}{c}\sqrt{x^2+y^2}\\ \arctan (y/x)\\ (x\stackrel{\·}{x}+y\stackrel{\·}{y})/\sqrt{x^2+y^2}\end{array}\right];$

最后，根据所述预测测量值点集σ_y,k+1、所述测量值向量预测值所述测量噪声协方差矩阵R以及所述方差权重参数W_c，并通过以下公式计算获得所述第k+1个时刻的测量预测分解矩阵

${\stackrel{\‾}{s}}_{y,k+1}=qr\left\{\[\sqrt{W_c^{\left(1\right)}}\left({\mathrm{\σ}}_{y,k+1}^{(1:2L)}-{\stackrel{\‾}{Y}}_{k+1}\right)\sqrt{R}\]\right\};$

${\stackrel{\‾}{S}}_{y,k+1}=cholupdate({\stackrel{\‾}{s}}_{y,k+1},{\mathrm{\σ}}_{y,k+1}^{\left(0\right)}-{\stackrel{\‾}{Y}}_{k+1},W_c^{\left(0\right)}).$

在上述汽车雷达目标跟踪方法中，所述步骤S6包括：首先，首先，将所述西格玛点σ_x,k代入所述目标车辆运动模型的状态转移函数f(·)中，以获得第k+1个时刻的预测采样点集σ_x,k+1，其中，所述状态转移函数f(·)为：

$f(\·)=\left[\begin{array}{cccccccc}1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3& 0& 0& 0& 0\\ 0& 1& dt& 0.5*{\mathrm{dt}}^2& 0& 0& 0& 0\\ 0& 0& 1& dt& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3\\ 0& 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2\\ 0& 0& 0& 0& 0& 0& 1& dt\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right];$

其中，dt表示所述汽车雷达的采样周期；

其次，将所述预测采样点集σ_x,k+1代入所述汽车雷达测量模型的测量函数h(·)中，以获得第k+1个时刻的预测测量值点集σ_y,k+1，其中，所述测量函数h(·)为：

$h(\·)=\left[\begin{array}{c}\sqrt{x^2+y^2}\\ \arctan (y/x)\\ (x\stackrel{\·}{x}+y\stackrel{\·}{y})/\sqrt{x^2+y^2}\end{array}\right];$

然后，根据所述预测采样点集σ_x,k+1、所述预测测量值点集σ_y,k+1、所述方差权重参数W_c、所述运动状态向量预测值以及所述测量值向量预测值并通过以下公式计算获得运动状态向量与测量值向量的互协方差函数P_xy：

$P_{xy}={\Σ}_{i=0}^{2L}{W}_c^{\left(i\right)}({\mathrm{\σ}}_{x,k+1}-{\stackrel{\‾}{X}}_{k+1})({\mathrm{\σ}}_{y,k+1}-{\stackrel{\‾}{Y}}_{k+1});$

最后，根据所述互协方差函数P_xy以及测量预测分解矩阵通过以下公式计算获得所述卡尔曼增益K_k+1：

在上述汽车雷达目标跟踪方法中，所述步骤S7包括：根据所述运动状态向量预测值所述测量值向量预测值所述卡尔曼增益K_k+1以及所述测量值向量Y_k+1，并通过以下公式计算获得所述目标车辆在第k+1个时刻的运动状态向量估计值

${\hat{X}}_{k+1}={\stackrel{\‾}{X}}_{k+1}+K_{k+1}(y_{k+1}-{\stackrel{\‾}{Y}}_{k+1}).$

在上述汽车雷达目标跟踪方法中，所述步骤S7包括：根据所述状态预测分解矩阵所述测量预测分解矩阵以及所述卡尔曼增益K_k+1，并通过以下公式计算获得所述第k+1个时刻的分解矩阵估计值

${\hat{S}}_{k+1}=cholupdate({\stackrel{\‾}{S}}_{x,k+1},K_{k+1}{\stackrel{\‾}{S}}_{y,k+1},-1).$

由于采用了上述的技术解决方案，本发明通过将状态误差协方差矩阵P_k在算法迭代执行之前进行乔利斯基分解，以获得分解矩阵，进而在算法迭代过程中，将该分解矩阵进行非线性迭代，自动更新，而不需要在每一次步骤执行过程中都去计算状态误差协方差矩阵的乔利斯基分解，从而避免了状态误差协方差矩阵非正定所导致的分解失败、算法停滞的问题，综上，本发明一方面减少了计算量，提高了汽车雷达目标跟踪的实时性，另一方面基本消除了矩阵非正定导致算法停滞的隐患，提高了汽车雷达目标跟踪的稳定性。

附图说明

图1是采用本发明仿真单目标车辆运动过程的滤波效果图；

图2是在采用本发明仿真单目标车辆运动过程中X方向距离的滤波误差与测量误差对比图；

图3是在采用本发明仿真单目标车辆运动过程中Y方向距离的滤波误差与测量误差对比图；

图4是在真实车辆环境下，使用毫米波雷达测量单目标车辆运动并通过本发明得到的滤波效果示意图。

具体实施方式

下面结合附图，给出本发明的较佳实施例，并予以详细描述。

首先，对本发明中涉及到的一些概念进行说明：

1、系统运动模型：其描述了动态系统的运动规律性，具体来说，就是使用系统状态转移函数f(·)或者状态转移矩阵F描述系统由第k个时刻的状态到第k+1个时刻的状态的过渡关系：X_k+1＝f(X_k)或者X_k+1＝F·X_k。

在汽车行驶中，由于加速、减速、变道、超车等驾驶动作的频繁发生，目标车辆相对于自车的运动过程是变化的(自车：安装雷达传感器的车辆，目标车辆：被雷达传感器探测的车辆)。但是，一般认为，除个别例外，并且在高采样数据的情况下，目标车辆相对于自车的加加速度(加速度的变化率)可以在较长的时间内保持为定值。因此，考虑车辆相对加加速度恒定并受到白噪声干扰，可以构造一个变加速度的四阶运动模型，又称CJ模型(ConstJerkModel，常加加速度模型，简称CJ模型)，其表示如下：

X_k+1＝f(X_k)+w_k

其中，系统在第k个时刻的状态向量为：

$X_k={\[x_k,{\stackrel{\·}{x}}_k,\stackrel{\·\·}{x_k},\stackrel{\·\·\·}{x_k},y_k,{\stackrel{\·}{y}}_k,\stackrel{\·\·}{y_k},\stackrel{\·\·\·}{y_k}\]}^T$

该状态向量的维度L＝8，其中，在车体坐标系下，分别表示目标车辆相对自车在x方向上的横向距离、横向速度、横向加速度和加加速度；分别表示目标车辆相对自车在y方向上的纵向距离、纵向速度、纵向加速度和加加速度。

根据系统运动模型，状态转移函数f(·)可定义为：

$f(\·)=\left[\begin{array}{cccccccc}1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3& 0& 0& 0& 0\\ 0& 1& dt& 0.5*{\mathrm{dt}}^2& 0& 0& 0& 0\\ 0& 0& 1& dt& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3\\ 0& 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2\\ 0& 0& 0& 0& 0& 0& 1& dt\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right]$

其中，dt表示雷达传感器的采样周期。

在CJ模型中，向量 $w_k={\[{\mathrm{\ζ}}_x,{\mathrm{\ζ}}_{\stackrel{\·}{x}},{\mathrm{\ζ}}_{\stackrel{\·\·}{x}},{\mathrm{\ζ}}_{\stackrel{\·\·\·}{x}},{\mathrm{\ζ}}_y,{\mathrm{\ζ}}_{\stackrel{\·}{y}},{\mathrm{\ζ}}_{\stackrel{\·\·}{y}},{\mathrm{\ζ}}_{\stackrel{\·\·\·}{y}}\]}^T,$ 其中，分别表示横向距离、速度、加速度和加加速度受到的高斯白噪声的方差；分别表示纵向距离、速度、加速度和加加速度受到的高斯白噪声的方差。

假设状态向量各维度受到的高斯白噪声相互独立，则过程噪声协方差矩阵Q(其表征了实际运动过程与所选择的运动模型之间的不符合程度)表示为：

$Q=\left[\begin{array}{cccccccc}{\mathrm{\ζ}}_x& 0& 0& 0& 0& 0& 0& 0\\ 0& {\mathrm{\ζ}}_{\stackrel{\·}{x}}& 0& 0& 0& 0& 0& 0\\ 0& 0& {\mathrm{\ζ}}_{\stackrel{\·\·}{x}}& 0& 0& 0& 0& 0\\ 0& 0& 0& {\mathrm{\ζ}}_{\stackrel{\·\·\·}{x}}& 0& 0& 0& 0\\ 0& 0& 0& 0& {\mathrm{\ζ}}_y& 0& 0& 0\\ 0& 0& 0& 0& 0& {\mathrm{\ζ}}_{\stackrel{\·}{y}}& 0& 0\\ 0& 0& 0& 0& 0& 0& {\mathrm{\ζ}}_{\stackrel{\·\·}{y}}& 0\\ 0& 0& 0& 0& 0& 0& 0& {\mathrm{\ζ}}_{\stackrel{\·\·\·}{y}}\end{array}\right]$

2、系统测量模型：其指系统状态向量到测量值向量的映射，具体来说，其使用系统测量函数h(·)或者系统测量矩阵H描述系统状态到测量值的映射关系：Y_k＝h(X_k)或者Y_k＝H·X_k。

自车的雷达传感器可以测得目标车辆的距离、角度和径向速度信息，即系统在第k个时刻的测量值向量可表示为：

Y_k＝[r_k,a_k,v_k]^T

该测量值向量的维度O＝3，其中，r表示目标车辆的距离，a表示目标车辆的角度，v表示目标车辆的径向速度。

因此，系统测量模型可表示如下：

其中， $\begin{array}{c}{Y}_k=h\left(X_k\right)+u_k\\ h\left(X_k\right)=\left[\begin{array}{c}\sqrt{{x_k}^2+{y_k}^2}\\ \arctan (y_k/x_k)\\ (x_k{\stackrel{\·}{x}}_{k,}+y_k{\stackrel{\·}{y}}_k)/\sqrt{{x_k}^2+{y_k}^2}\end{array}\right];u_k=\left[\begin{array}{c}{\mathrm{\ξ}}_r\\ {\mathrm{\ξ}}_a\\ {\mathrm{\ξ}}_v\end{array}\right],\end{array}$ 其中ξ_r、ξ_a、ξ_v为距离、角度、径向速度的测量误差，分别服从相互独立的N(0,ξ_r)，N(0,ξ_a)，N(0,ξ_v)高斯白噪声，因此，测量噪声协方差矩阵R(表征了雷达传感器实际测量值与所选择的测量模型之间的不符合程度)表示为：

$R=\left[\begin{array}{ccc}{\mathrm{\ξ}}_r& 0& 0\\ 0& {\mathrm{\ξ}}_a& 0\\ 0& 0& {\mathrm{\ξ}}_v\end{array}\right]$

下面对本发明的具体步骤进行详细说明。

本发明，即一种汽车雷达目标跟踪方法，包括以下步骤：

步骤S0，构建目标车辆运动模型以及汽车雷达测量模型，并将目标车辆的运动状态标记为L维的运动状态向量X，将汽车雷达测得的目标车辆信息标记为测量值向量Y；根据目标车辆运动模型获得状态误差协方差矩阵P和过程噪声协方差矩阵Q，根据汽车雷达测量模型获得测量噪声协方差矩阵R(该获得过程为如上所述的现有技术，此处不再赘述)；

具体来说，构建目标车辆运动模型为：X_k+1＝f(X_k)+w_k，其中，X_k+1为第k+1个时刻的运动状态向量，X_k为第k个时刻的运动状态向量，f(·)为状态转移函数，w_k为第k个时刻目标车辆的运动状态受到的高斯白噪声的方差矩阵；构建汽车雷达测量模型为：Y_k＝h(X_k)+u_k，其中，Y_k为第k个时刻的测量值向量，h(·)为测量函数，u_k为测量误差矩阵；

将所述运动状态向量X表示为： $X={\[x,\stackrel{\·}{x},\stackrel{\·\·}{x},\stackrel{\·\·\·}{x},y,\stackrel{\·}{y},\stackrel{\·\·}{y},\stackrel{\·\·\·}{y}\]}^T,$ 其中，分别表示目标车辆相对于安装有汽车雷达的自车在x方向上的横向距离、横向速度、横向加速度和加加速度；分别表示目标车辆相对于自车在y方向上的纵向距离、纵向速度、纵向加速度和加加速度；

将所述测量值向量Y表示为：Y＝[r,a,v]^T，其中，r表示目标车辆的距离，a表示目标车辆的角度，v表示目标车辆的径向速度。

步骤S1，将过程噪声协方差矩阵Q和测量噪声协方差矩阵R分别设置为常数矩阵，并设置采样权重参数α、β和θ(该采样权重参数即为不敏卡尔曼滤波器的权重参数)；

具体来说，将过程噪声协方差矩阵Q和测量噪声协方差矩阵R设置为如下所示的常数矩阵(该常数矩阵在迭代过程中不被更新)：

$Q=\left[\begin{array}{cccccccc}0.2& 0& 0& 0& 0& 0& 0& 0\\ 0& 0.02& 0& 0& 0& 0& 0& 0\\ 0& 0& 0.002& 0& 0& 0& 0& 0\\ 0& 0& 0& 0.0002& 0& 0& 0& 0\\ 0& 0& 0& 0& 0.2& 0& 0& 0\\ 0& 0& 0& 0& 0& 0.02& 0& 0\\ 0& 0& 0& 0& 0& 0& 0.002& 0\\ 0& 0& 0& 0& 0& 0& 0& 0.0002\end{array}\right]$

$R=\left[\begin{array}{ccc}0.5& 0& 0\\ 0& 0.001& 0\\ 0& 0& 0.5\end{array}\right]$

上述过程噪声协方差矩阵Q中各状态分量的高斯白噪声方差与构建的目标车辆运动模型有关，测量噪声协方差矩阵R中测量分量的高斯白噪声方差的选择与汽车雷达的测量精度有关，两个常数矩阵均属经验值。

另外，采样权重参数α决定了西格玛点(西格玛点的定义将在以下步骤中说明)在均值周围分散程度的大小，通常设定为一个较小的值(10^-4<α<1)；采样权重参数β用于整合状态向量分布的先验信息，在假设状态向量符合标准正态分布时，β＝2时最优；采样权重参数θ为二次缩放因子，一般设为0。因此，在此设{α,β,θ}＝{0.01,2,0}。

步骤S2，初始化第k个时刻的运动状态向量X_k；对第k个时刻的状态误差协方差矩阵P_k进行乔利斯基(Cholesky)分解以获得相应的分解矩阵S_k 并初始化该第k个时刻的分解矩阵S_k；

具体来说，步骤S2中的对运动状态向量X_k以及分解矩阵S_k的初始化可根据时刻k的取值，分为两种情况：

1)当k＝0时，目标车辆处于初始状态(即，没有第k-1时刻的状态作为迭代)，因此，采用通过汽车雷达获得的第一次测量值向量Y₀＝[r₀,a₀,v₀]来初始化运动状态向量X₀：

$X_0=\left[\begin{array}{c}{r}_{0}cos\left(a_0\right),\\ v_{0}cos\left(a_0\right)\\ 0\\ 0\\ r_{0}sin\left(a_0\right)\\ v_{0}sin\left(a_0\right)\\ 0\\ 0\end{array}\right]$

此时，分解矩阵S₀初始化为L维的单位矩阵。

2)当k≠0时，系统处于迭代过程(即，有第k-1时刻的状态作为迭代)，此时，其中，表示目标车辆在第k个时刻的运动状态向量估计值，表示第k个时刻的分解矩阵估计值。

步骤S3，根据步骤S2中获得初始化后的运动状态向量X_k以及步骤S1中获得的采样权重参数α、β和θ，计算获得该运动状态向量X_k所对应的2L+1个西格玛点σ_x,k及其均值权重参数W_m和方差权重参数W_c；

具体来说，西格玛点是进行UT变换时，对运动状态向量这一随机变量进行特殊采样所获得的状态采样点。经证实，西格玛点经历非线性变换后取得的均值和协方差可以达到泰勒展开式三阶的精度。均值权重参数W_m为求均值时的西格玛点权重，方差权重参数W_c为求方差时的西格玛点权重。

根据以下公式可获得2L+1个西格玛点σ_x,k及其均值权重参数W_m和方差权重参数W_c：

${\mathrm{\&sigma;}}_{x,k}^{\left(0\right)}=X_k$

${\mathrm{\&sigma;}}_{x,k}^{\left(i\right)}=X_k+{\mathrm{\&eta;S}}_k^{\left(i\right)},i=1,...L$

${\mathrm{\&sigma;}}_{x,k}^{\left(i\right)}=X_k-{\mathrm{\&eta;S}}_k^{\left(i\right)},i=L,...2L$

$W_m^{\left(0\right)}=\frac{\mathrm{\&lambda;}}{L+\mathrm{\&lambda;}}$

$W_c^{\left(0\right)}=\frac{\mathrm{\&lambda;}}{L+\mathrm{\&lambda;}}+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})$

$W_c^{\left(i\right)}=W_m^{\left(i\right)}=\frac{1}{2(L+\mathrm{\&lambda;})},i=1,...,2L$

其中，λ＝α²(L+θ)-L，(L为运动状态向量X的维度，α,β,θ为步骤S1中设置的采样权重参数)；和分别对应第i个西格玛点的均值权重参数和方差权重参数。

步骤S4，根据步骤S3中获得的西格玛点σ_x,k以及均值权重参数W_m，计算获得目标车辆在第k+1个时刻的运动状态向量预测值并根据该运动状态向量预测值步骤S1中获得的过程噪声协方差矩阵Q以及步骤S3中获得的西格玛点σ_x,k以及方差权重参数W_c，计算获得第k+1个时刻的状态预测分解矩阵

具体来说，首先将步骤S3中获得的西格玛点σ_x,k代入目标车辆运动模型的状态转移函数f(·)(如上所述)中，从而获得第k+1个时刻的预测采样点集σ_x,k+1：

σ_x,k+1＝f(σ_x,k)

然后，根据该预测采样点集σ_x,k+1以及均值权重参数W_m，通过以下公式计算获得目标车辆在第k+1个时刻的运动状态向量预测值

${\stackrel{\&OverBar;}{X}}_{k+1}=\underset{i=0}{\overset{2L}{\&Sigma;}}{W}_m^{\left(i\right)}{\mathrm{\&sigma;}}_{x,k+1}^{\left(i\right)}$

最后，根据上述运动状态向量预测值过程噪声协方差矩阵Q以及预测采样点集σ_x,k+1及方差权重参数W_c，通过以下公式计算获得第k+1个时刻的状态预测分解矩阵

${\stackrel{\&OverBar;}{S}}_{x,k+1}=qr\left\{\&lsqb;\sqrt{W_c^{\left(1\right)}}\left({\mathrm{\&sigma;}}_{x,k+1}^{(1:2L)}-{\stackrel{\&OverBar;}{X}}_{k+1}\right)\sqrt{Q}\&rsqb;\right\}$

${\stackrel{\&OverBar;}{S}}_{x,k+1}=cholupdate({\stackrel{\&OverBar;}{s}}_{x,k+1},{\mathrm{\&sigma;}}_{x,k+1}^{\left(0\right)}-{\stackrel{\&OverBar;}{X}}_{k+1},W_c^{\left(0\right)})$

步骤S5，根据步骤S3中获得的西格玛点σ_x,k以及均值权重参数W_m，计算获得第k+1个时刻的测量值向量预测值并根据该测量值向量预测值步骤S1中获得的测量噪声协方差矩阵R以及步骤S3中获得的西格玛点σ_x,k以及方差权重参数W_c，计算获得第k+1个时刻的测量预测分解矩阵

具体来说，首先按步骤S4获得第k+1个时刻的预测采样点集σ_x,k+1，并将该预测采样点集σ_x,k+1代入汽车雷达测量模型的测量函数h(·)(如上所述)中，从而获得第k+1个时刻的预测测量值点集σ_y,k+1：

σ_y,k+1＝h(σ_x,k+1)

然后，根据该预测测量值点集σ_y,k+1以及均值权重参数W_m，通过以下公式计算获得第k+1个时刻的测量值向量预测值

${\stackrel{\&OverBar;}{Y}}_{k+1}=\underset{i=0}{\overset{2L}{\&Sigma;}}{W}_m^{\left(i\right)}{\mathrm{\&sigma;}}_{y,k+1}^{\left(i\right)}$

最后，根据上述测量值向量预测值测量噪声协方差矩阵R、预测测量值点集σ_y,k+1以及方差权重参数W_c，通过以下公式计算获得第k+1个时刻的测量预测分解矩阵

${\stackrel{\&OverBar;}{s}}_{y,k+1}=qr\left\{\&lsqb;\sqrt{W_c^{\left(1\right)}}\left({\mathrm{\&sigma;}}_{y,k+1}^{(1:2L)}-{\stackrel{\&OverBar;}{Y}}_{k+1}\right)\sqrt{R}\&rsqb;\right\}$

${\stackrel{\&OverBar;}{S}}_{y,k+1}=cholupdate({\stackrel{\&OverBar;}{S}}_{y,k+1},{\mathrm{\&sigma;}}_{y,k+1}^{\left(0\right)}-{\stackrel{\&OverBar;}{Y}}_{k+1},W_c^{\left(0\right)})$

步骤S6，根据步骤S3中获得的西格玛点σ_x,k、方差权重参数W_c、步骤S4中获得的运动状态向量预测值步骤S5中获得的测量值向量预测值以及测量预测分解矩阵计算获得卡尔曼增益K_k+1；

具体来说，首先根据步骤S4获得预测采样点集σ_x,k+1，并根据步骤S5获得预测测量值点集σ_y,k+1；

然后，根据上述预测采样点集σ_x,k+1、预测测量值点集σ_y,k+1、方差权重参数W_c、运动状态向量预测值以及测量值向量预测值通过以下公式计算获得运动状态向量与测量值向量的互协方差函数P_xy：

$P_{xy}=\underset{i=0}{\overset{2L}{\&Sigma;}}{W}_c^{\left(i\right)}({\mathrm{\&sigma;}}_{x,k+1}-{\stackrel{\&OverBar;}{X}}_{k+1})({\mathrm{\&sigma;}}_{y,k+1}-{\stackrel{\&OverBar;}{Y}}_{k+1})$

最后，根据上述互协方差函数P_xy以及测量预测分解矩阵通过以下公式计算获得卡尔曼增益K_k+1：

$K_{k+1}=(P_{xy}/{\stackrel{\&OverBar;}{S}}_{y,k+1}^T)/{\stackrel{\&OverBar;}{S}}_{y,k+1}$

步骤S7，根据步骤S4中获得的运动状态向量预测值步骤S5中获得的测量值向量预测值步骤S6中获得的卡尔曼增益K_k+1以及通过汽车雷达获得的第k+1个时刻的测量值向量Y_k+1，计算获得目标车辆在第k+1个时刻的运动状态向量估计值根据步骤S4中获得的状态预测分解矩阵步骤S5中获得的测量预测分解矩阵以及步骤S6中获得的卡尔曼增益K_k+1，计算获得第k+1个时刻的分解矩阵估计值

具体来说，首先根据上述运动状态向量预测值测量值向量预测值卡尔曼增益K_k+1以及测量值向量Y_k+1，通过以下公式计算获得目标车辆在第k+1个时刻的运动状态向量估计值

${\hat{X}}_{k+1}={\stackrel{\&OverBar;}{X}}_{k+1}+K_{k+1}(Y_{k+1}-{\stackrel{\&OverBar;}{Y}}_{k+1})$

然后，根据上述状态预测分解矩阵测量预测分解矩阵以及卡尔曼增益K_k+1，通过以下公式计算获得第k+1个时刻的分解矩阵估计值

${\hat{S}}_{k+1}=cholupdate({\stackrel{\&OverBar;}{S}}_{x,k+1},K_{k+1}{\stackrel{\&OverBar;}{S}}_{y,k+1},-1)$

步骤S8，返回执行步骤S2，并使k＝k+1，直至目标车辆停止运动。

具体来说，下一个测量周期开始，本方法将跳转至步骤S2开始迭代，直到没有测量值向量输入为止，最后一个输出的将是滤波器对目标车辆运动最后一点的运动状态的状态估计值。

下面以采用本实例方法对模拟单个运动车辆目标进行跟踪为例：车辆初始状态向量设定为： ${\&lsqb;x_k,{\stackrel{\&CenterDot;}{x}}_k,{\stackrel{\&CenterDot;\&CenterDot;}{x}}_k,{\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{x}}_k{y}_k,{\stackrel{\&CenterDot;}{y}}_k,{\stackrel{\&CenterDot;\&CenterDot;}{y}}_k,{\stackrel{\&CenterDot;\&CenterDot;\&CenterDot;}{y}}_k\&rsqb;}^T=\&lsqb;10,10,-2,0,1,0.1,2,0\&rsqb;.$ 其中距离单位为m，速度单位为m/s，加速度单位为m/s²，加加速度单位为m/s³。时间间隔dt为0.1s，仿真170个点，即目标车辆运动17秒的过程。跟踪效果图如图1所示，其中：“·”表示目标车辆真实的运动轨迹，“-”表示采用本发明滤波的目标运动轨迹。可以看出，经过滤波的轨迹和目标真实运动轨迹拟合效果很好，准确反映了目标真实运动过程。滤波误差和测量值误差的对比如图2、图3所示，其中“·”表示测量值误差，“+”表示滤波误差。滤波误差是指滤波结果的x和y坐标与真实运动轨迹的x和y坐标间的差值。测量值误差是指将测量值转化到x和y坐标系下，得到的测量值x和y坐标与真实运动轨迹的x和y坐标间的差值。表1对测量值误差和滤波值误差做了统计比较。

表1

	X方向距离误差均方根值	Y方向距离误差均方根值
			测量值误差	1.6877	1.4649
滤波值误差	0.4180	0.1168
			滤波值误差/测量值误差	0.2476	0.0797

由表1可以看出，采用本发明获得的滤波误差比测量误差小得多，因此有效地减少了由于传感器测量误差带来的跟踪效果下降的情况。图4中展示的是在十字路口的真实道路环境下，用毫米波雷达测量一个运动车辆目标，并通过本发明得到的滤波轨迹。

以上所述的，仅为本发明的较佳实施例，并非用以限定本发明的范围，本发明的上述实施例还可以做出各种变化。即凡是依据本发明申请的权利要求书及说明书内容所作的简单、等效变化与修饰，皆落入本发明专利的权利要求保护范围。本发明未详尽描述的均为常规技术内容。

Claims (10)

1.一种汽车雷达目标跟踪方法，其包括：步骤S0，构建目标车辆运动模型以及汽车雷达测量模型，并将目标车辆的运动状态标记为L维的运动状态向量X，将汽车雷达测得的目标车辆信息标记为测量值向量Y；根据所述目标车辆运动模型获得状态误差协方差矩阵P和过程噪声协方差矩阵Q，根据所述汽车雷达测量模型获得测量噪声协方差矩阵R，其特征在于，该方法还包括以下步骤：

步骤S1，将所述过程噪声协方差矩阵Q和所述测量噪声协方差矩阵R分别设置为常数矩阵，并设置采样权重参数α、β和θ；

步骤S2，初始化第k个时刻的运动状态向量X_k；对第k个时刻的状态误差协方差矩阵P_k进行乔利斯基分解以获得分解矩阵S_k，并初始化该分解矩阵S_k；

步骤S3，计算获得初始化后的运动状态向量X_k所对应的2L+1个西格玛点σ_x,k及其均值权重参数W_m和方差权重参数W_c；

步骤S4，计算获得目标车辆在第k+1个时刻的运动状态向量预测值并根据该运动状态向量预测值所述过程噪声协方差矩阵Q、所述西格玛点σ_x,k以及所述方差权重参数W_c，计算获得第k+1个时刻的状态预测分解矩阵

步骤S5，计算获得第k+1个时刻的测量值向量预测值并根据该测量值向量预测值所述测量噪声协方差矩阵R、所述西格玛点σ_x,k以及所述方差权重参数W_c，计算获得第k+1个时刻的测量预测分解矩阵

步骤S6，根据所述西格玛点σ_x,k、所述方差权重参数W_c、所述运动状态向量预测值所述测量值向量预测值以及所述测量预测分解矩阵计算获得卡尔曼增益K_k+1；

步骤S7，计算获得目标车辆在第k+1个时刻的运动状态向量估计值根据所述状态预测分解矩阵测量预测分解矩阵以及卡尔曼增益K_k+1，计算获得第k+1个时刻的分解矩阵估计值

步骤S8，返回执行所述步骤S2，并使k＝k+1，直至目标车辆停止运动。

2.根据权利要求1所述的汽车雷达目标跟踪方法，其特征在于，所述步骤S2包括：当k＝0时，采用通过所述汽车雷达获得的测量值向量Y₀初始化所述运动状态向量X_k，且将所述分解矩阵S_k初始化为L维的单位矩阵；当k≠0时，将所述运动状态向量X_k和分解矩阵S_k分别设置为：其中，表示目标车辆在第k个时刻的运动状态向量估计值，表示第k个时刻的分解矩阵估计值。

3.根据权利要求1所述的汽车雷达目标跟踪方法，其特征在于，所述步骤S3包括：根据以下公式可获得2L+1个西格玛点σ_x,k及其均值权重参数W_m和方差权重参数W_c：

${\mathrm{\&sigma;}}_{x,k}^{\left(0\right)}=X_k;$

${\mathrm{\&sigma;}}_{x,k}^{\left(i\right)}=X_k+{\mathrm{\&eta;S}}_k^{\left(i\right)},i=1,...L;$

${\mathrm{\&sigma;}}_{x,k}^{\left(i\right)}=X_k-{\mathrm{\&eta;S}}_k^{\left(i\right)},i=L,...2L;$

$W_m^{\left(0\right)}=\frac{\mathrm{\&lambda;}}{L+\mathrm{\&lambda;}};$

$W_c^{\left(0\right)}=\frac{\mathrm{\&lambda;}}{L+\mathrm{\&lambda;}}+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;});$

$W_c^{\left(i\right)}=W_m^{\left(i\right)}=\frac{1}{2(L+\mathrm{\&lambda;})},i=1,...,2L;$

其中，λ＝α²(L+θ)-L，L为所述运动状态向量X的维度，α,β,θ为所述采样权重参数；和分别对应第i个西格玛点的均值权重参数和方差权重参数。

4.根据权利要求1所述的汽车雷达目标跟踪方法，其特征在于，所述步骤S4包括：首先，将所述西格玛点σ_x,k代入所述目标车辆运动模型的状态转移函数f(·)中，以获得第k+1个时刻的预测采样点集σ_x,k+1，其中，所述状态转移函数f(·)为：

$f(\&CenterDot;)=\left[\begin{array}{cccccccc}1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3& 0& 0& 0& 0\\ 0& 1& dt& 0.5*{\mathrm{dt}}^2& 0& 0& 0& 0\\ 0& 0& 1& dt& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3\\ 0& 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2\\ 0& 0& 0& 0& 0& 0& 1& dt\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right];$

其中，dt表示所述汽车雷达的采样周期；

然后，根据所述预测采样点集σ_x,k+1以及所述均值权重参数W_m，并通过以下公式计算获得所述目标车辆在第k+1个时刻的运动状态向量预测值

${\stackrel{\&OverBar;}{X}}_{k+1}={\&Sigma;}_{i=0}^{2L}{W}_m^{\left(i\right)}{\mathrm{\&sigma;}}_{x,k+1}^{\left(i\right)}.$

5.根据权利要求1所述的汽车雷达目标跟踪方法，其特征在于，所述步骤S4包括：将所述西格玛点σ_x,k代入所述目标车辆运动模型的状态转移函数f(·)中，以获得第k+1个时刻的预测采样点集σ_x,k+1，其中，所述状态转移函数f(·)为：

$f(\&CenterDot;)=\left[\begin{array}{cccccccc}1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3& 0& 0& 0& 0\\ 0& 1& dt& 0.5*{\mathrm{dt}}^2& 0& 0& 0& 0\\ 0& 0& 1& dt& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3\\ 0& 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2\\ 0& 0& 0& 0& 0& 0& 1& dt\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right];$

其中，dt表示所述汽车雷达的采样周期；

然后，根据所述预测采样点集σ_x,k+1、所述运动状态向量预测值所述过程噪声协方差矩阵Q以及所述方差权重参数W_c，并通过以下公式计算获得所述第k+1个时刻的状态预测分解矩阵

${\stackrel{\&OverBar;}{s}}_{x,k+1}=qr\{\&lsqb;\sqrt{W_c^{\left(1\right)}}({\mathrm{\&sigma;}}_{x,k+1}^{(1:2L)}-{\stackrel{\&OverBar;}{X}}_{k+1})\sqrt{Q}\&rsqb;\};$

${\stackrel{\&OverBar;}{S}}_{x,k+1}=cholupdate({\stackrel{\&OverBar;}{s}}_{x,k+1},{\mathrm{\&sigma;}}_{x,k+1}^{\left(0\right)}-{\stackrel{\&OverBar;}{X}}_{k+1},W_c^{\left(0\right)}).$

6.根据权利要求1所述的汽车雷达目标跟踪方法，其特征在于，所述步骤S5包括：首先，将所述西格玛点σ_x,k代入所述目标车辆运动模型的状态转移函数f(·)中，以获得第k+1个时刻的预测采样点集σ_x,k+1，其中，所述状态转移函数f(·)为：

$f(\&CenterDot;)=\left[\begin{array}{cccccccc}1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3& 0& 0& 0& 0\\ 0& 1& dt& 0.5*{\mathrm{dt}}^2& 0& 0& 0& 0\\ 0& 0& 1& dt& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3\\ 0& 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2\\ 0& 0& 0& 0& 0& 0& 1& dt\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right];$

其中，dt表示所述汽车雷达的采样周期；

然后，将所述预测采样点集σ_x,k+1代入所述汽车雷达测量模型的测量函数h(·)中，以获得第k+1个时刻的预测测量值点集σ_y,k+1，其中，所述测量函数h(·)为：

$h(\&CenterDot;)=\left[\begin{array}{c}\sqrt{x^2+y^2}\\ \arctan \left(y/x\right)\\ (x\stackrel{\&CenterDot;}{x}+y\stackrel{\&CenterDot;}{y})/\sqrt{x^2+y^2}\end{array}\right];$

最后，根据所述预测测量值点集σ_y,k+1以及所述均值权重参数W_m，并通过以下公式计算获得所述第k+1个时刻的测量值向量预测值

${\stackrel{\&OverBar;}{Y}}_{k+1}={\&Sigma;}_{i=0}^{2L}{W}_m^{\left(i\right)}{\mathrm{\&sigma;}}_{y,k+1}^{\left(i\right)}.$

7.根据权利要求1所述的汽车雷达目标跟踪方法，其特征在于，所述步骤S5包括：首先，将所述西格玛点σ_x,k代入所述目标车辆运动模型的状态转移函数f(·)中，以获得第k+1个时刻的预测采样点集σ_x,k+1，其中，所述状态转移函数f(·)为：

$f(\&CenterDot;)=\left[\begin{array}{cccccccc}1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3& 0& 0& 0& 0\\ 0& 1& dt& 0.5*{\mathrm{dt}}^2& 0& 0& 0& 0\\ 0& 0& 1& dt& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3\\ 0& 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2\\ 0& 0& 0& 0& 0& 0& 1& dt\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right];$

其中，dt表示所述汽车雷达的采样周期；

然后，将所述预测采样点集σ_x,k+1代入所述汽车雷达测量模型的测量函数h(·)中，以获得第k+1个时刻的预测测量值点集σ_y,k+1，其中，所述测量函数h(·)为：

$h(\&CenterDot;)=\left[\begin{array}{c}\sqrt{x^2+y^2}\\ \arctan \left(y/x\right)\\ (x\stackrel{\&CenterDot;}{x}+y\stackrel{\&CenterDot;}{y})/\sqrt{x^2+y^2}\end{array}\right];$

最后，根据所述预测测量值点集σ_y,k+1、所述测量值向量预测值所述测量噪声协方差矩阵R以及所述方差权重参数W_c，并通过以下公式计算获得所述第k+1个时刻的测量预测分解矩阵

${\stackrel{\&OverBar;}{s}}_{y,k+1}=qr\{\&lsqb;\sqrt{W_c^{\left(1\right)}}({\mathrm{\&sigma;}}_{y,k+1}^{(1:2L)}-{\stackrel{\&OverBar;}{Y}}_{k+1})\sqrt{R}\&rsqb;\};$

${\stackrel{\&OverBar;}{S}}_{y,k+1}=cholupdate({\stackrel{\&OverBar;}{s}}_{y,k+1},{\mathrm{\&sigma;}}_{y,k+1}^{\left(0\right)}-{\stackrel{\&OverBar;}{Y}}_{k+1},W_c^{\left(0\right)}).$

8.根据权利要求1所述的汽车雷达目标跟踪方法，其特征在于，所述步骤S6包括：首先，首先，将所述西格玛点σ_x,k代入所述目标车辆运动模型的状态转移函数f(·)中，以获得第k+1个时刻的预测采样点集σ_x,k+1，其中，所述状态转移函数f(·)为：

$f(\&CenterDot;)=\left[\begin{array}{cccccccc}1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3& 0& 0& 0& 0\\ 0& 1& dt& 0.5*{\mathrm{dt}}^2& 0& 0& 0& 0\\ 0& 0& 1& dt& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2& \left(\frac{1}{6}\right)*{\mathrm{dt}}^3\\ 0& 0& 0& 0& 0& 1& dt& 0.5*{\mathrm{dt}}^2\\ 0& 0& 0& 0& 0& 0& 1& dt\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right];$

其中，dt表示所述汽车雷达的采样周期；

其次，将所述预测采样点集σ_x,k+1代入所述汽车雷达测量模型的测量函数h(·)中，以获得第k+1个时刻的预测测量值点集σ_y,k+1，其中，所述测量函数h(·)为：

$h(\&CenterDot;)=\left[\begin{array}{c}\sqrt{x^2+y^2}\\ \arctan \left(y/x\right)\\ (x\stackrel{\&CenterDot;}{x}+y\stackrel{\&CenterDot;}{y})/\sqrt{x^2+y^2}\end{array}\right];$

然后，根据所述预测采样点集σ_x,k+1、所述预测测量值点集σ_y,k+1、所述方差权重参数W_c、所述运动状态向量预测值以及所述测量值向量预测值并通过以下公式计算获得运动状态向量与测量值向量的互协方差函数P_xy：

$P_{xy}={\mathrm{\&Sigma;}}_{i=0}^{2L}{W}_c^{\left(i\right)}({\mathrm{\&sigma;}}_{x,k+1}-{\stackrel{\&OverBar;}{X}}_{k+1})({\mathrm{\&sigma;}}_{y,k+1}-{\stackrel{\&OverBar;}{Y}}_{k+1});$

最后，根据所述互协方差函数P_xy以及测量预测分解矩阵通过以下公式计算获得所述卡尔曼增益K_k+1：

$K_{k+1}=(P_{xy}/{\stackrel{\&OverBar;}{S}}_{y,k+1}^T)/{\stackrel{\&OverBar;}{S}}_{y,k+1}.$

9.根据权利要求1所述的汽车雷达目标跟踪方法，其特征在于，所述步骤S7包括：根据所述运动状态向量预测值所述测量值向量预测值所述卡尔曼增益K_k+1以及所述测量值向量Y_k+1，并通过以下公式计算获得所述目标车辆在第k+1个时刻的运动状态向量估计值

${\hat{X}}_{k+1}={\stackrel{\&OverBar;}{X}}_{k+1}+K_{k+1}(Y_{k+1}-{\stackrel{\&OverBar;}{Y}}_{k+1}).$

10.根据权利要求1所述的汽车雷达目标跟踪方法，其特征在于，所述步骤S7包括：根据所述状态预测分解矩阵所述测量预测分解矩阵以及所述卡尔曼增益K_k+1，并通过以下公式计算获得所述第k+1个时刻的分解矩阵估计值

${\hat{S}}_{k+1}=cholupdate({\stackrel{\&OverBar;}{S}}_{x,k+1},K_{k+1}{\stackrel{\&OverBar;}{S}}_{y,k+1},-1).$