CN110675435A - 基于卡尔曼滤波和χ2检测平滑处理的车辆轨迹跟踪方法 - Google Patents

Abstract

本发明公开一种基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法，目的在于寻找更为有效的车辆跟踪检测的方式，其包括如下步骤：构造目标车辆行驶过程的运动学模型和测量模型；获取目标车辆历史运动轨迹并将运动学模型和测量模型代入卡尔曼滤波器中的时间更新方程和状态更新方程对目标车辆运动状态进行迭代更新，预测下一时刻的运动状态；构造χ²故障检测器，对预测结果进行故障检测。本发明针对现有技术跟踪时可能出现的识别错误或丢失等问题应用了χ²故障检测器，可以有效的减小轨迹预测的偏差，提高轨迹识别正确率。

Description

基于卡尔曼滤波和χ2检测平滑处理的车辆轨迹跟踪方法

技术领域

本发明涉及自动驾驶领域，具体而言，本发明涉及一种基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法。

背景技术

随着科学技术的不断发展与进步，无人驾驶车辆作为现代车辆发展的主要趋势，各大车企与高校围绕其各种技术和功能开展了各方面的的研究应用与实验测试。轨迹跟踪控制是实现无人驾驶的基础，也作为智能车车辆控制的一项重要技术，要获取车辆在当前场景中的行驶轨迹，就需要对车辆进行长时间的目标跟踪。

对于通过视频图像进行车辆跟踪检测方式，其获取的原始车辆轨迹具有检测借点框不稳定、轨迹存在诸多跳跃、平滑性差等缺点，同时，在于跟踪过程中存在被大面积遮挡、光照影响、背景杂乱以及阴暗变化等噪声问题，应用传统跟踪算法得到的处理结果不精确，难以准确获取目标车辆的位置信息与速度信息。

发明内容

为了寻找更为有效的车辆跟踪检测的方式，本发明提供了一种基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法，该卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法包括如下步骤：

步骤S1:将获取的目标车辆运动视频图像视为二维平面坐标系，并以目标车辆识别矩形框的中心点作为分析对象构造目标车辆行驶过程的运动学模型和测量模型：

步骤S2:获取目标车辆历史运动轨迹并将所述运动学模型和所述测量模型代入卡尔曼滤波器中的时间更新方程和状态更新方程对目标车辆运动状态进行迭代更新，预测下一时刻的运动状态；

步骤S3:构造χ²故障检测器，并根据所述卡尔曼滤波器构造所述χ²故障检测器的检测统计量；比较所述检测统计量与预设阈值，当所述检测统计量大于所述阈值时，则选用k时刻的先验状态估计值作为k时刻的后验状态估计值参与执行步骤S2，以动态实时地获取目标车辆的运动轨迹，其中k表示时刻，为大于0的整数。

优选地，所述目标车辆行驶过程的运动学模型为：

X_k+1＝AX_k+BW_k

式中，X_k为k时刻的目标车辆真实运动状态；

A∈R^4×4表示作用在X_k上的状态转移矩阵；

B∈R⁴表示作用在过程噪声W_k上的系数矩阵；

W_k∈R⁴表示k时刻服从W_k～N(0,Q)分布的过程噪声；

Q∈R^4×4表示过程噪声协方差矩阵；

所述测量模型为：

Z_k＝HX_k+V_k

式中，H∈R^2×4表示测量模型矩阵，它把真实状态空间映射成观测空间；

V_k∈R²表示服从V_k～N(0,R)分布的测量噪声；

R∈R^2×2表示测量噪声协方差矩阵。

优选地，所述根据所述卡尔曼滤波算法构造所述χ²故障检测器的检测统计量包括如下步骤：

将卡尔曼滤波器稳态状态下的卡尔曼增益代入所述状态更新方程得到k时刻的状态估计值；

根据k时刻的状态估计值和所述测量模型得到残余偏差L_k；

根据所述残余偏差L_k构造检测统计量g_k，其中，所述检测统计量g_k为：

g_k＝L_k ^TB_kL_k

B_k＝E(L_kL_k ^T)＝HPH^T+R

式中，L_k服从高斯分布，B_k为残余偏差L_k的协方差矩阵；

H表示测量模型矩阵；

P为卡尔曼滤波器中的估计误差协方差矩阵；

R为测量噪声V_k的协方差矩阵；

g^k服从m自由度的卡方分布，且不远离0，m为自然数。

优选地，所述预设阈值基于所述检测统计量自由度值与预设的概率置信范围查阅卡方分布表确认。

优选地，所述检测统计量自由度值为1，所述预设的概率置信范围为95％。

优选地，所述步骤S2之前包括如下步骤：

根据预设条件初始化所述运动学模型和所述测量模型，其中，所述预设条件为过程噪声、测量噪声和状态初值X₀的期望满足以下条件：

优选地，根据预设条件初始化所述运动学模型和所述测量模型包括如下步骤：

确认采样周期，并根据所述采样周期和所述预设条件初始化所述运动学模型和所述测量模型。

与现有技术相比，本发明一种基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法具有如下技术效果：

本发明一种基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法针对现有技术跟踪时可能出现的识别错误或丢失等问题应用了χ²故障检测器，可以有效的减小轨迹预测的偏差，提高轨迹识别正确率，同时，该χ²故障检测器是一种通常与卡尔曼滤波其配合使用的检测器，检验计算复杂性较低，便于行业应用。

本发明附加的方面和优点将在下面的描述中部分给出，这些将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1为本发明一个实施例提供一种基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法的流程示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

请参阅图1，本发明实施例一种基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法包括如下步骤：

步骤S1:将获取的目标车辆运动视频图像视为二维平面坐标系，并以目标车辆识别矩形框的中心点作为分析对象构造目标车辆行驶过程的运动学模型和测量模型。

在一些实施方式中，目标车辆运动视频图像优选通过车载视觉***获取。

具体地址，目标车辆行驶过程的运动学模型为：

X_k+1＝AX_k+BW_k

式中，X_k为k时刻的目标车辆真实运动状态；

A∈R^4×4表示作用在X_k上的状态转移矩阵；

B∈R⁴表示作用在过程噪声W_k上的系数矩阵；

W_k∈R⁴表示k时刻服从W_k～N(0,Q)分布的过程噪声；

Q∈R^4×4表示过程噪声协方差矩阵；

测量模型为：

Z_k＝HX_k+V_k

式中，H∈R^2×4表示测量模型矩阵，它把真实状态空间映射成观测空间；

V_k∈R²表示服从V_k～N(0,R)分布的测量噪声；

R∈R^2×2表示测量噪声协方差矩阵。

步骤S2:获取目标车辆历史运动轨迹并将所述运动学模型和所述测量模型代入卡尔曼滤波器中的时间更新方程和状态更新方程对目标车辆运动状态进行迭代更新，预测下一时刻的运动状态；

在一些实施方式中，在步骤S2之后包括如下步骤：

根据预设条件初始化运动学模型和测量模型，其中，预设条件为过程噪声、测量噪声和状态初值X₀的期望满足以下条件：

优选地，过程噪声的协方差矩阵可以根据实际的运行过程估计得到，这里记作：

E(W_kW_k ^T)＝Q

测量噪声的协方差矩阵一般可以根据相关测量传感器如摄像头厂家的数据和实际测量得到，这里记作：

E(V_kV_k ^T)＝R

***状态初始的协方差为：

E(X₀X₀ ^T)＝∑_0|0。

优选地，根据预设条件初始化运动学模型和测量模型包括如下步骤：确认采样周期，并根据采样周期和预设条件初始化运动学模型和测量模型。

示例地，假设在跟踪过程中采样周期为Δt，则运动学模型和测量模型参数矩阵定义如下：

运动学模型和测量模型的状态定义如下：

其中，X_k∈R⁴表示k时刻目标车辆真实运动状态；

x(k)和v_x(k)分别表示的是k时刻时，视频图像中目标车辆在x轴方向上的坐标和速度的值；

y(k)和v_y(k)分别表示的是k时刻时，目标车辆在y轴方向上的坐标和速度的值；

a_x(k)表示目标车辆在x轴方向上的加速度，a_y(k)表示目标车辆在y轴方向上的加速度；

Z_k∈R²表示观测向量，即摄像机或视频中所检测到的车辆位置，因在建立观测向量时，由于只能检测到运动车辆的中心位置，以及只能观测到目标车辆识别矩形框中心点的位置，观测向量只包括目标车辆位置的中心点坐标，并没有速度信息，其中，x_z(k)和y_z(k)分别表示在k时刻图像序列中检测到的目标车辆的矩形框中心点所对应的x轴与y轴坐标值。

卡尔曼滤波器包括两个主要过程：预估与校正，其中，预估过程主要是利用时间更新方程建立对当前状态的先验估计，及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计值；校正过程负责反馈，利用测量更新方程在预估过程的先验估计值及当前测量变量的基础上建立起对当前状态的改进的后验估计。以下给出离散卡尔曼滤波的时间更新方程和状态更新方程。

时间更新方程：

P_k ^-＝AP_k-1A^T+Q

P_k-1(0)＝P₀

P₀＝diag(1,1,1,1)

状态更新方程：

K_k＝P_k ^-H^T(HP_k ^-H^T+R)^-1

P_k＝(I-K_kH)P_k ^-

式中，表示k时刻的卡尔曼滤波器对目标车辆运动状态的先验估计值；

表示k-1时刻的相应的卡尔曼滤波器对目标车辆运动状态的后验估计值；

表示k时刻的后验状态估计值；

P_k ^-表示n×n先验估计误差协方差矩阵；

P_k表示n×n后验估计误差协方差矩阵；

I表示n×n阶的单位矩阵；

K_k表示n×m阶矩阵，称为卡尔曼增益或混合因数，作用是使后验估计误差协方差最小。

在本发明实施例中，将步骤S1所构造的运动学模型和测量模型代入时间更新方程与状态更新方程，即可得到目标车辆的估计预测运动轨迹。

步骤S3:构造χ²故障检测器，并根据所述卡尔曼滤波器构造所述χ²故障检测器的检测统计量；比较所述检测统计量与预设阈值，当所述检测统计量大于所述阈值时，则选用k时刻的先验状态估计值作为k时刻的后验状态估计值参与执行步骤S2，以动态实时地获取目标车辆的运动轨迹，其中k表示时刻，为大于0的整数。

具体地，根据卡尔曼滤波算法构造χ²故障检测器的检测统计量包括如下步骤：

将卡尔曼滤波器稳态状态下的卡尔曼增益代入状态更新方程得到k时刻的状态估计值；

根据k时刻的状态估计值和测量模型得到残余偏差L_k；

根据残余偏差L_k构造检测统计量g_k，其中，检测统计量g_k为：

g_k＝L_k ^TB_kL_k

B_k＝E(L_kL_k ^T)＝HPH^T+R

式中，L_k服从高斯分布，B_k为残余偏差L_k的协方差矩阵；

H表示测量模型矩阵；

P为卡尔曼滤波器中的估计误差协方差矩阵；

R为测量噪声V_k的协方差矩阵；

g_k服从m自由度的卡方分布，且不远离0，m为自然数。

在一些实施方式中，预设阈值基于检测统计量自由度值与预设的概率置信范围查阅卡方分布表确认。

示例地，当检测统计量自由度值为1，预设的概率置信范围为95％时，查阅卡方分布表即可得到相应的预设阈值为3.84。这样，当g_k小于预设阈值时，即g_k≤3.84时，χ²故障检测器不会启动警报，反之当g_k大于预设阈值时，χ²故障检测器会发出警报，残余偏差值过大，超过置信范围，同时对于卡尔曼滤波的状态迭代过程进行以下的调整：

即舍弃k时刻的测量值Z_k，直接选用k时刻的先验状态估计值

作为k时刻的后验状态估计值，并参与进行下一步的预测与迭代过程。通过以上步骤S2与步骤S3的不断重复进行，可以动态实时地获取目标车辆的运动轨迹，

值得注意的是，本发明考虑卡尔曼滤波器稳态工作状态时的卡方检测，卡尔曼滤波器稳态状态下的参数如下所示：

K@PH^T(HPH^T+R)^-1

式中，P_k ^-为k时刻的先验估计误差协方差矩阵，P为卡尔曼滤波器达到稳态状态时的估计误差协方差矩阵，此时的先验估计与后验估计误差协方差矩阵P_k ^-、P_k均等于P，将卡尔曼滤波器稳态状态下的估计误差协方差矩阵代入式状态更新方程中，即可得到卡尔曼滤波器达到稳态状态时的卡尔曼增益K。

卡方检测将k时刻的残余偏差L_k定义为：

由测量模型与残余偏差L_k联立可得残余偏差L_k另外一种表达形式：

式中，X_k为***状态真实值，V_k为测量噪声。

与现有技术相比，本发明实施例一种基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法具有如下技术效果：

本发明实施例一种基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法针对现有技术跟踪时可能出现的识别错误或丢失等问题应用了χ²故障检测器，可以有效的减小轨迹预测的偏差，提高轨迹识别正确率，同时，该χ²故障检测器是一种通常与卡尔曼滤波其配合使用的检测器，检验计算复杂性较低，便于行业应用。

以上所述仅是本发明的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法，其特征在于，所述基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法包括如下步骤：

步骤S1:将获取的目标车辆运动视频图像视为二维平面坐标系，并以目标车辆识别矩形框的中心点作为分析对象构造目标车辆行驶过程的运动学模型和测量模型；

步骤S2:获取目标车辆历史运动轨迹并将所述运动学模型和所述测量模型代入卡尔曼滤波器中的时间更新方程和状态更新方程对目标车辆运动状态进行迭代更新，预测下一时刻的运动状态；

步骤S3:构造χ²故障检测器，并根据所述卡尔曼滤波器构造所述χ²故障检测器的检测统计量；比较所述检测统计量与预设阈值，当所述检测统计量大于所述预设阈值时，则选用k时刻的先验状态估计值作为k时刻的后验状态估计值参与执行步骤S2，以动态实时地获取目标车辆的运动轨迹，其中k表示时刻，为大于0的整数。

2.如权利要求1所述的基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法，其特征在于，所述目标车辆行驶过程的运动学模型为：

X_k+1＝AX_k+BW_k

式中，X_k为k时刻的目标车辆真实运动状态；

A∈R^4×4表示作用在X_k上的状态转移矩阵；

B∈R⁴表示作用在过程噪声W_k上的系数矩阵；

W_k∈R⁴表示k时刻服从W_k～N(0,Q)分布的过程噪声；

Q∈R^4×4表示过程噪声协方差矩阵；

所述测量模型为：

Z_k＝HX_k+V_k

式中，H∈R^2×4表示测量模型矩阵，它把真实状态空间映射成观测空间；

V_k∈R²表示服从V_k～N(0,R)分布的测量噪声；

R∈R^2×2表示测量噪声协方差矩阵。

3.如权利要求2所述的基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法，其特征在于，所述根据所述卡尔曼滤波算法构造所述χ²故障检测器的检测统计量包括如下步骤：

将卡尔曼滤波器稳态状态下的卡尔曼增益代入所述状态更新方程得到k时刻的状态估计值；

根据k时刻的状态估计值和所述测量模型得到残余偏差L_k；

根据所述残余偏差L_k构造检测统计量g_k，其中，所述检测统计量g_k为：

g_k＝L_k ^TB_kL_k

B_k＝E(L_kL_k ^T)＝HPH^T+R

式中，L_k服从高斯分布，B_k为残余偏差L_k的协方差矩阵；

H表示测量模型矩阵；

P为卡尔曼滤波器中的估计误差协方差矩阵；

R为测量噪声V_k的协方差矩阵；

g^k服从m自由度的卡方分布，且不远离0，m为自然数。

4.如权利要求3所述的基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法，其特征在于，所述预设阈值基于所述检测统计量自由度值与预设的概率置信范围查阅卡方分布表确认。

5.如权利要求4所述的基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法，其特征在于，所述检测统计量自由度值为1，所述预设的概率置信范围为95％。

6.如权利要求1所述的基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法，其特征在于，所述步骤S2之前包括如下步骤：

根据预设条件初始化所述运动学模型和所述测量模型，其中，所述预设条件为过程噪声、测量噪声和状态初值X₀的期望满足以下条件：

7.如权利要求6所述的基于卡尔曼滤波和χ²检测平滑处理的车辆轨迹跟踪方法，其特征在于，根据预设条件初始化所述运动学模型和所述测量模型包括如下步骤：

确认采样周期，并根据所述采样周期和所述预设条件初始化所述运动学模型和所述测量模型。