CN110307841A

CN110307841A - 一种基于信息量测不完全的车辆运动参数估计方法

Info

Publication number: CN110307841A
Application number: CN201910514101.3A
Authority: CN
Inventors: 陈素娟
Original assignee: Nanjing Institute of Technology
Current assignee: Nanjing Institute of Technology
Priority date: 2019-06-14
Filing date: 2019-06-14
Publication date: 2019-10-08
Anticipated expiration: 2039-06-14
Also published as: CN110307841B

Abstract

本发明公开了一种基于信息量测不完全的车辆运动参数估计方法，本方法根据车辆相对运动工况建立了多个不同的卡尔曼滤波系统状态模型；以全球定位系统与车辆通信信息结合毫米波雷达信息作为观测量，同时结合视频跟踪系统，在运行过程中考虑到实际环境中可能出现的过程干扰和量测数据干扰，利用交互多模型算法，获取同一车道车辆的速度、加速度等相关参数；同时结合车载视频跟踪，可为视频图像干扰较大的情况下，实现同一车道上的车辆目标定位。本发明方法具有高定位精度，高可靠性和适应性好的优点。

Description

一种基于信息量测不完全的车辆运动参数估计方法

技术领域

本发明公开了一种基于信息量测不完全的车辆运动参数估计方法，属于车辆跟踪监测技术领域。

背景技术

随着社会经济的发展，车辆保有量迅速增长，道路交通安全问题日益突出，已成为全球性难题。全世界每年因交通事故造成了大量的人员伤亡和财产损失，车辆事故统计表明，汽车追尾事故是公路交通中最大多也是车辆运行过程中最危险的事故之一，其中91％的追尾事故是由于驾驶员注意力不集中引起的，如果提前0.5s提醒驾驶员，则追尾事故避免率可达90％。司机面对的主要威胁主要是道路上的其他车辆。汽车辅助驾驶的目的就是让司机警惕驾驶环境以及同其他车辆之间可能发生的碰撞。对路面运动车辆进行实时、精确的跟踪预测首先需要从公路交通场景中提取动态目标信息，然后再结合相关算法对目标车辆进行跟踪，得到目标的运行参数。这些信息都可以通过跟踪车辆的雷达测量得到的，但是在复杂的交通场景下，干扰增多，在动态目标信息提取方面，单一的信息采集方法难以满足跟踪要求。GPS定位精度高，但受天气和位置的影响较大。当遇到天气不佳的时候、或者处于高架桥/树荫的下面，或者在高楼的旁边角落、地下车库或露天的下层车库，GPS的定位就会受到相当大的影响，甚至无法进行定位服务。毫米波雷达性能优良，但所测信息噪声较大，无法有效区分干扰目标，也会受恶劣天气影响和周围车辆、障碍物的电磁波干扰。车载设备得到的视频图像的背景比较复杂并且是动态变化的，易受环境影响，如天气、光照等干扰影响。

在智能交通系统中，随着车载设备的普及以及计算机视觉领域中目标检测与跟踪算法的深入研究，基于车载设备视频图像的目标检测和跟踪结果将能为辅助驾驶提供可靠的参考信息，从而进一步实现安全驾驶。

发明内容

为了实现对目标车辆运动参数的准确获取、可靠估计，本发明提出的方法针对公路交通车辆不同的运动典型工况建立多个卡尔曼滤波模型，同时采用GPS、加速度计和雷达毫米波对目标数据进行采集，结合车载视频，并考虑到量测信息获取不完全的情况。考虑到实际车辆运行的几种典型工况，为滤除雷达传感器观测噪声，获得较为准确的与前车间的距离、车速等信息，本发明采用基于交互式多模型(IMM，Interacting Multiple Model)的Kalman滤波算法来跟踪预测同一车道车辆相对运行参数。

为达到上述目的，本发明采用的技术方案如下：

一种基于信息量测不完全的车辆运动参数估计方法，包括以下步骤：

(1)建立同一车道车辆相对运动的卡尔曼滤波模型；

所述车辆相对运动分为匀速运动，匀加速运动和匀减速运动；

所述卡尔曼滤波模型为：

其中，j＝1表示匀速运动的卡尔曼滤波模型，j＝2表示匀加速运动的卡尔曼滤波模型，j＝3表示匀减速运动的卡尔曼滤波模型；X_j(k)为第j个卡尔曼滤波模型k时刻的目标运动状态向量，A_j为适维状态转移矩阵，w_j(k)是均值为零、方差为Q_j的高斯白噪声，Z_j(k)表示第j个卡尔曼滤波模型k时刻的观测数据，C_j为适维量测矩阵，d_j(k)表示第j个卡尔曼滤波模型k时刻车辆运动数据的探测概率，

v_j(k)满足：

其中，分别为距离测量方差在直角坐标系下的x轴和y轴分量，分别为速度测量方差在直角坐标系下的x轴和y轴分量，分别为加速度测量方差在直角坐标系下的x轴和y轴分量，α为任意一极小实数；

(2)考虑卡尔曼滤波模型的概率，确定卡尔曼滤波模型的输入以及误差方差阵；

(3)对每个卡尔曼滤波模型进行卡尔曼滤波递推，确定车辆的距离、速度和加速度。

前述的适维状态转移矩阵如下：

其中，T表示离散的周期。

前述的方差Q_j表示如下：

其中，T表示离散的周期。

前述的适维量测矩阵表示如下：

其中，T表示离散的周期。

前述的d_j(k)取值如下：

(a)某一时刻，车辆的GPS、加速度计、毫米波雷达均工作正常且车车通信正常，则d_j(k)＝I_6×6；

(b)某一时刻，车辆的车车通信发生故障，毫米波雷达工作正常，则

(c)某一时刻，车辆的车车通信正常，毫米波雷达，GPS工作正常，加速度计故障，则

(d)某一时刻，车辆的车车通信正常，毫米波雷达、加速度计工作正常，GPS信号丢失，则d_j(k)＝I_6×6；

(e)某一时刻，只有车辆的车载视频正常工作，且能够通过跟踪获得目标运动参数，采用车载视频跟踪；

(f)某一时刻，车车通信失效、毫米波雷达传感器无效且车载视频无法跟踪，则d_j(k)＝0_6×6。

前述的卡尔曼滤波模型的输入表示为：

其中，X_j(k-1)为第j个卡尔曼滤波模型k时刻的输入，同时也是第j个卡尔曼滤波模型k-1时刻的状态估计值，ρ_ij(k-1)为卡尔曼滤波模型的混合概率，

ρ_ij(k-1)计算如下：

ρ_ij(k-1)＝p_ij(k-1)ρ_i(k-1)/ρ_j(k,k-1)

其中，p_ij(k-1)为k-1时刻第j个卡尔曼滤波模型和第i个卡尔曼滤波模型之间的转移概率，ρ_i(k-1)为k-1时刻第i个卡尔曼滤波模型的模型概率估计值，ρ_j(k,k-1)为k时刻第j个卡尔曼滤波模型的模型概率预测值；

所述卡尔曼滤波模型的误差方差阵表示为：

其中，P_j(k-1)表示第j个卡尔曼滤波模型在k-1时刻的误差方差阵估计值。

前述的对每个卡尔曼滤波模型进行卡尔曼滤波递推，过程如下：

(31)对车辆的运动状态进行预测如下：

X_j(k,k-1)＝A_j(k-1)X_j(k-1)

其中，X_j(k,k-1)为第j个卡尔曼滤波模型在k时刻的状态预测值；

(32)对车辆运动状态的误差方差阵预测如下：

P_j(k,k-1)＝A_j(k-1)P_j(k-1)A_j ^T(k-1)+Q_j(k-1)

其中，P_j(k,k-1)为第j个卡尔曼滤波模型在k时刻的误差方差阵预测值；

(33)对车辆的运动状态进行估计如下：

X_j(k)＝X_j(k,k-1)+K_j(k)(Z_j(k)-C_jX_j(k,k-1))

其中，X_j(k)为第j个卡尔曼滤波模型在k时刻的状态估计值，K_j(k)表示第j个卡尔曼滤波器在k时刻的增益，

K_j(k)表示如下：

K_j(k)＝P_j(k,k-1)C_j ^T(k-1)S_j ^-1(k)

S_j(k)＝C_jP_j(k,k-1)C_j ^T+R_j(k-1)：

(34)车辆运动状态的误差方差阵估计如下：

P_j(k)＝P_j(k,k-1)-K_j(k)S_j(k)K_j ^T(k)：

其中，P_j(k)表示第j个卡尔曼滤波模型在k时刻的误差方差阵估计值；

(35)计算第j个卡尔曼滤波模型在k时刻的模型概率：

其中，Γ_j(k)为最大似然函数；

(36)对所有卡尔曼滤波模型的运动状态估计进行概率加权并求和，得到车辆运动的状态估计为：

X(k)中包含目标车辆x轴和y轴的位置、速度和加速度。

前述的最大似然函数；Γ_j(k)表示如下：

本发明所达到的有益效果为：

(1)本发明考虑到实际环境中可能出现的过程干扰和量测数据干扰，利用交互式多模型实现多个运动工况下的卡尔曼滤波模型之间的交互，即使在某些量测数据丢失的情况下，也能实时、准确、可靠的获取同一车道前后车辆的速度、加速度等参数，实现对同一车道车辆的实时、准确、可靠的运动参数估计。

(2)本发明也可以结合车载视频跟踪，在视频图像干扰较大的情况下，提供准确、可靠的获取车辆位置信息。

附图说明

图1为本发明观测数据确定流程图；

图2为本发明基于Kalman滤波的交互式多模型滤波流程图；

图3为本发明中Kalman滤波与车载视频相结合流程图。

具体实施方式

下面对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

本发明提供一种基于信息量测不完全的车辆运动参数估计方法，具体步骤包括：

1、建立车辆的典型运动工况模型

工况1：前后车相对速度保持匀速，建立模型为匀速运动模型；

工况2：前后车相对匀加速行驶，建立模型为匀加速运动模型；

工况3：前后车相对匀减速行驶，建立模型为匀减速运动模型；

由于3种不同的卡尔曼滤波模型具有相同的状态模型形式，区别在于状态转移矩阵不同，因此可将状态方程统一表示。

第j(j＝1,2,3)个典型运动工况模型离散化后的卡尔曼滤波的状态方程可表示为：

X_j(k+1)＝A_jX_j(k)+w_j(k) (1)

其中，X_j(k)∈Rⁿ为第j个典型运动工况下k时刻的目标运动状态向量，A_j为适维状态转移矩阵，w_j(k)∈Rⁿ是均值为零、方差为Q_j(Q_j>0)的高斯白噪声。

目标运动状态向量X_j(k)通常包括目标位置、目标速度和加速度等信息，记为：

考虑到车辆运动在二维平面，这里x_j(k),y_k,j,是二维平面坐标系下目标真实的位置、速度和加速度，单位分别为m(米)、m/s(米/秒)和m/s²(米/(秒×秒)。

第j个典型运动工况模型的初始状态为X_j(0)，其均值为μ_j(0)，协方差为P_j(0)＞0。本发明中矩阵的右上角标T表示矩阵的转置。

对于第1个典型运动工况下，状态转移矩阵A₁为：

对于第2个工况下，状态转移矩阵A₂为：

对于第3个工况下，状态转移矩阵A₃为：

其中，T表示离散的周期，在本发明中，根据测量传感器特性，T的典型值可取为0.2s，0.5s和1s等。

对于不同的工况，Q_j分别为：

2、建立卡尔曼滤波模型的观测方程

第j个卡尔曼滤波器的观测方程为：

Z_j(k)＝d_j(k)C_jX_j(k)+v_j(k),(j＝1,2,3) (8)

其中，Z_j(k)表示第j个卡尔曼滤波器k时刻的观测数据，C_j为适维量测矩阵，矩阵如式(10)～(12)所示。

v_j(k)∈R^m满足：

式中，d_j(k)为离散随机变量，服从Bernoulli分布。d_j(k)＝1时，表示k时刻第j个模型的量测数据正常，d_j(k)＝0时表示k时刻第j个模型量测数据丢失，α为任意一极小实数。

w_j(k)和v_j(k)与初始状态X_j(0)互不相关。

d_j(k)＝0对应的α取极限形式：α→∞。d_j(k)＝1时对应的R_j＞0，R_j如式(13)所示。

其中，分别为距离测量方差在直角坐标系下的分量；分别为速度测量方差在直角坐标系下的分量；分别为加速度测量方差在直角坐标系下的分量。这些测量方差是由实际测量工具的精度决定的，为已知量。

本发明采用GPS、加速度计、毫米波雷达测距测速以提供观测信息，结合车载视频对同一车道车辆运动参数进行跟踪和预测。其中，GPS与加速度计提供自身车速与加速度信息，可通过车车通信，实现数据共享；毫米波雷达安装在车的前方和后方，用于测量前后车辆相对距离与速度信息，提供同一车道前后车辆的位置及速度。GPS、加速度计所提供的车速与加速度信息由于需要车车通信，故可能发生信息丢失的情况，即发生量测信息不完全现象。另外一种是由于传感器可能存在的失效引起量测数据不完全。此外在复杂交通场景下，车载视频安装在车的前方或后方，对同一车道的前后车辆进行实时图像采集。由于可能干扰情况，跟踪目标会发生丢失。基于以上三种信息可能存在的丢失情况，当所有传感器都正常工作时，车载视频跟踪正常，由于信息全面而准确，可以将GPS、加速度计以及毫米波测量得到的相关信息，作为观测量；当车车通信故障而失效或者GPS、加速度计发生故障时，以毫米波雷达的数据作为系统观测量；当毫米波雷达传感器本身故障而造成的量测信息不完全时，而车车通信正常时，可将GPS、加速度计的相关测量数据作为观测量；当毫米波雷达传感器本身故障而造成的量测信息不完全时，而车车通信正常时，若GPS发生故障，以加速度计的数据为观测数据；当传感器本身故障而造成的量测信息不完全时，而车车通信正常时，若加速度发生故障，以GPS的数据为观测数据；若以上量测数据均不能作为观测数据时，可利用车载视频进行目标检测跟踪；若发生车车通信失效、毫米波雷达传感器无效且车载视频无法跟踪式，可利用kalman滤波进行一步预测，实现车辆运行参数的估计。参见图1，以上几种情况可分别表示成以下形式：

(a)某一时刻，所有观测数据均有效，即GPS、加速度计、毫米波雷达均工作正常且车车通信正常。此时d_j(k)＝I_6×6；

(b)某一时刻，通信发生故障，毫米波雷达工作正常。此时以毫米波雷达数据为观测量，无加速度信息，故

(c)某一时刻，车车通信正常，毫米波雷达工作正常，GPS信号正常，加速度计出现故障。此时以毫米波雷达、GPS测得数据为观测量，无加速度信息，故

(d)某一时刻，车车通信正常，毫米波雷达工作正常，GPS信号丢失，加速度计正常。此时以毫米波雷达、加速度计测得数据为观测量，故d_j(k)＝I_6×6；

(e)某一时刻，只有车载视频正常工作，且可通过跟踪获得目标运动参数，采用车载视频跟踪；

(f)某一时刻，所有观测数据均无效，此时d_j(k)＝0_6×6。

3、基于Kalman滤波估计车辆的距离、速度和加速度

具体步骤参见图2，包括：

(31)建立同一车道车辆相对运动的状态模型；根据式(1)、(8)，分别建立3个不同的Kalman滤波模型，分别匀速运动模型，匀加速运动模型和匀减速运动模型：

其中，j＝1表示匀速运动模型，j＝2表示匀加速运动模型，j＝3表示匀减速运动模型。

模型之间的转移概率为p_ij，(i＝1,2,3,j＝1,2,3)表示从工况i转移到工况j。

模型之间的转移概率初始取值可以根据实际情况取，如：

p₁₁＝0.9,p₁₂＝0.05,p₁₃＝0.05

p₂₁＝0.05,p₂₂＝0.9,p₂₃＝0.05

p₃₁＝0.05,p₃₂＝0.05,p₃₃＝0.9。

(32)交互估计计算

1)预测第j个模型的概率为ρ_j(k,k-1)：

其中，ρ_i(k-1)为k-1时刻第i个模型的概率，p_ij(k-1)为k-1时刻模型之间的转移概率。

2)预测混合概率ρ_ij(k-1)：

ρ_ij(k-1)＝p_ij(k-1)ρ_i(k-1)/ρ_j(k,k-1) (15)

3)交互估计后第j个滤波器在k时刻的输入为：

其中，P_j(k-1)表示第j个模型在k-1时刻的误差方差阵估计值。

(33)每个卡尔曼滤波器各自进行kalman滤波递推，第j个模型的滤波器过程如下：

时间更新：

1)状态预测方程：

X_j(k,k-1)＝A_j(k-1)X_j(k-1) (18)

X_j(k,k-1)为第j个模型在k时刻的状态预测值，X_j(k-1)为第j个模型在k-1时刻的状态估计值。

2)预测误差方差：

P_j(k,k-1)＝A_j(k-1)P_j(k-1)A_j ^T(k-1)+Q_j(k-1) (19)

其中，P_j(k,k-1)为第j个模型在k时刻的误差方差阵预测值。

测量更新：

3)滤波增益：

K_j(k)＝P_j(k,k-1)C_j ^T(k-1)S_j ^-1(k) (20)

其中，K_j(k)表示第j个模型在k时刻的增益，

S_j(k)＝C_jP_j(k,k-1)C_j ^T+R_j(k-1) (21)

状态估计：

X_j(k)＝X_j(k,k-1)+K_j(k)(Z_j(k)-C_jX_j(k,k-1)) (22)

其中，X_j(k)为第j个模型在k时刻的状态估计值。

估计误差方差阵：

P_j(k)＝P_j(k,k-1)-K_j(k)S_j(k)K_j ^T(k) (23)

(34)模型概率更新

在每个模型完成时间更新后，利用最大似然函数Γ_j(k)计算新的模型概率ρ_j(k)：

因此，第j个模型在k时刻的模型概率由贝叶斯定理给出：

计算出各模型为正确的后验概率之后，对所有滤波器的状态估计进行概率加权并求和，权系数为模型正确的后验概率，得到估计组合后的状态估计为：

此时得到的X(k)＝[x y v_x v_y a_x a_y]^T内各个状态量分别为目标车辆目前的运动状态参数。通过合成计算可以得到目标车辆的运动位置、速度和加速度参数。

图3为基于Kalman滤波与车载视频相结合估计车辆的距离、速度和加速度的流程图，当车载视频正常工作且可跟踪获得目标参数时，采用车载视频进行车辆跟踪；当所有观测数据均无效，且车载视频无法获得目标参数时，采用基于Kalman滤波估计车辆的距离、速度和加速度。

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

Claims

1.一种基于信息量测不完全的车辆运动参数估计方法，其特征在于，包括以下步骤：

(1)建立同一车道车辆相对运动的卡尔曼滤波模型；

所述卡尔曼滤波模型为：

v_j(k)满足：

2.根据权利要求1所述的一种基于信息量测不完全的车辆运动参数估计方法，其特征在于，所述适维状态转移矩阵如下：

其中，T表示离散的周期。

3.根据权利要求1所述的一种基于信息量测不完全的车辆运动参数估计方法，其特征在于，所述方差Q_j表示如下：

其中，T表示离散的周期。

4.根据权利要求1所述的一种基于信息量测不完全的车辆运动参数估计方法，其特征在于，所述适维量测矩阵表示如下：

其中，T表示离散的周期。

5.根据权利要求1所述的一种基于信息量测不完全的车辆运动参数估计方法，其特征在于，所述d_j(k)取值如下：

6.根据权利要求1所述的一种基于信息量测不完全的车辆运动参数估计方法，其特征在于，所述卡尔曼滤波模型的输入表示为：

其中，X_j(k-1)为第j个卡尔曼滤波模型k时刻的输入，同时也是第j个卡尔曼滤波模型k-1时刻的状态估计值，ρ_i|j(k-1)为卡尔曼滤波模型的混合概率，

ρ_i|j(k-1)计算如下：

ρ_i|j(k-1)＝p_ij(k-1)ρ_i(k-1)/ρ_j(k,k-1)

所述卡尔曼滤波模型的误差方差阵表示为：

7.根据权利要求6所述的一种基于信息量测不完全的车辆运动参数估计方法，其特征在于，所述对每个卡尔曼滤波模型进行卡尔曼滤波递推，过程如下：

(31)对车辆的运动状态进行预测如下：

X_j(k,k-1)＝A_j(k-1)X_j(k-1)

(32)对车辆运动状态的误差方差阵预测如下：

P_j(k,k-1)＝A_j(k-1)P_j(k-1)A_j ^T(k-1)+Q_j(k-1)

(33)对车辆的运动状态进行估计如下：

X_j(k)＝X_j(k,k-1)+K_j(k)(Z_j(k)-C_jX_j(k,k-1))

K_j(k)表示如下：

K_j(k)＝P_j(k,k-1)C_j ^T(k-1)S_j ^-1(k)

S_j(k)＝C_jP_j(k,k-1)C_j ^T+R_j(k-1)：

(34)车辆运动状态的误差方差阵估计如下：

P_j(k)＝P_j(k,k-1)-K_j(k)S_j(k)K_j ^T(k)：

(35)计算第j个卡尔曼滤波模型在k时刻的模型概率：

其中，Γ_j(k)为最大似然函数；

X(k)中包含目标车辆x轴和y轴的位置、速度和加速度。

8.根据权利要求7所述的一种基于信息量测不完全的车辆运动参数估计方法，其特征在于，所述最大似然函数；Γ_j(k)表示如下：