CN103448730B - 一种高速公路汽车追尾碰撞预警关键参数估计方法 - Google Patents

Abstract

本发明公开了一种高速公路汽车追尾碰撞预警关键参数估计方法。本方法根据高速公路车辆不同的相对运动的典型工况建立多个不同的卡尔曼滤波系统状态模型，以全球定位系统与车车通信信息结合雷达信息作为观测量，并在运行过程中检测、容忍传感器信息的不准确甚至失效，利用交互多模型算法，即使在某些传感器失效时，也能实时、准确、可靠的获取两车间相对距离、速度与加速度等高速公路追尾碰撞预警关键参数，同时结合相对加速度信息，来改进传统距离碰撞时间<i>TTC</i>估计方法，从而准确、可靠地估计出<i>TTC</i>这个高速公路追尾碰撞预警关键参数，具有精度高、可靠性好、实时性好、适应性好的优点。

Description

一种高速公路汽车追尾碰撞预警关键参数估计方法

技术领域

本发明涉及一种高速公路汽车追尾碰撞预警关键参数估计方法，属于汽车主动安全测量及控制领域。

背景技术

随着社会经济的发展，伴随着车辆保有量的迅速增长，道路交通安全问题日益突出，并已成为全球性难题。全世界每年因交通事故都会造成大量的人员伤亡和财产损失，世界各国都在努力降低交通事故的发生。统计资料表明，车辆的追尾碰撞事故是高速公路上最主要的事故形态，其预警系统的研究也因此引起了世界各国的重视。追尾碰撞预警系统是通过感知前方车辆与自车相对运动信息，利用某种算法判定是否存在潜在的追尾碰撞危险。因此，环境感知与预警算法是追尾碰撞预警的两项关键技术。

在环境感知方面，雷达、红外、机器视觉等技术都被广泛研究和应用，用以测量两车间的相对距离和相对车速。其中，毫米波雷达最为常用，性能也最为优良，但所测信息噪声较大，无法有效区分旁车道干扰目标，且会受恶劣天气影响和周围车辆、障碍物的电磁波干扰，特别在高速路上，隔离带和路两边的金属极大的限制了雷达性能；且无论是雷达、红外，还是机器视觉，都无法获取相对加速度这个信息，近年来，国外很多学者对基于全球定位系统与车车通信（GPS-V2V）的防追尾碰撞技术进行了研究，该所获取的信息较为全面、准确，并且不受天气、周围障碍物、旁车道干扰目标等影响，但其有效性取决于GPS和车车通信所传递信息的准确可靠，若GPS受到遮挡，或因车车通信受到干扰而被屏蔽或延时，则会导致信息的不准确甚至无法获取，且车车通信并未普及，很多车辆并不具备该功能；同时，无论是雷达还是GPS-V2V，都不可避免存在因传感器故障和失效引起的测量信息产生无法确定的变化甚至错误，影响防追尾碰撞预警系统的的可靠性。

国内外的追尾碰撞预警算法主要包括安全距离逻辑算法与安全时间逻辑算法两类，TTC（Time to collision，距离碰撞时间）是算法中的一个关键参数，其估计的准确性和可靠性直接关系着预警算法是否准确有效。国内外的TTC值计算都是以当前两车相对距离除以相对速度，这种方法忽略了相对加速度这个参数，导致计算结果误差较大，易产生虚警和漏警，影响算法的准确性。同时，为滤除雷达等传感器观测噪声，获取较为准确的与前方车辆的车间距离、速度等信息，卡尔曼滤波、粒子滤波等滤波方法被广泛应用，近年来，交互多模型（IMM）机动目标跟踪算法也被应用于此，进一步减小了测量误差，但所采用的模型未考虑高速公路追尾碰撞典型工况，准确性不足，也无法对故障或失效传感器信息有效处理和容忍，影响算法的可靠性。这些都限制了其在汽车追尾碰撞预警系统中的使用。

发明内容

为实现在对于车辆间相对距离、速度、加速度与距离碰撞时间TTC（Time tocollision）这些高速公路追尾碰撞预警关键参数的准确、可靠估计，本发明提出了一种高速公路汽车追尾碰撞预警关键参数估计方法。本发明提出的方法针对高速公路车辆不同的相对运动典型工况，建立多个卡尔曼滤波模型，同时以全球定位系统与车车通信信息结合雷达信息来建立滤波系统的观测量，并在运行过程中检测、容忍传感器信息的不准确甚至失效，即使在某些传感器失效时，依然能够取得较为准确的获取相关参数，进而通过交互多模型-卡尔曼滤波算法实现对两车相对距离、速度与加速度等高速公路追尾碰撞预警关键参数的估计，同时结合相对加速度信息，来改进传统TTC估计方法，以准确、可靠地估计出TTC这个高速公路追尾碰撞预警关键参数，具有精度高、可靠性好、实时性好、适应性好等特点。

一种高速公路汽车追尾碰撞预警关键参数估计方法，其特征在于：本方法根据高速公路车辆不同的相对运动典型工况建立多个不同的卡尔曼滤波系统状态模型，以全球定位系统与车车通信信息结合雷达信息作为观测量，并在运行过程中检测、容忍传感器信息的不准确甚至失效，利用交互多模型算法，实现不同典型运动工况下的卡尔曼滤波模型之间的交互，即使在某些传感器失效时，也能实时、准确、可靠的获取两车相对距离、速度与加速度等高速公路追尾碰撞预警关键参数，同时结合相对加速度信息，来改进传统TTC（Time to collision，距离碰撞时间）估计方法，从而准确、可靠地估计出TTC这个高速公路追尾碰撞预警关键参数；

具体步骤包括：

1）建立高速公路汽车相对运动的状态模型

针对高速公路汽车行驶工况，建立7个不同的卡尔曼滤波模型，这7个模型具有相同的状态模型形式，其区别仅在于状态转移矩阵的不同，其状态方程可统一表示，第j(j=1,2,3...7)个模型离散化后的卡尔曼滤波的状态方程的矩阵形式表示为：

X_j(k)＝A_j(k,k-1)X_j(k-1)+W_j(k-1)(j＝1,2...7)     （1）

式中，k表示离散化时刻，下标j（j=1，2...7）表示第j个模型，本发明的多模型设置为7个模型，这7个模型有相同的状态向量，该状态向量X_j＝[d  v_L  a_L  v_F  a_F]′，本发明中上角标'表示对矩阵转置，d表示前车距自车的距离[米]，V_L表示前车速度[米/秒]，a_L表示前车加速度[米/(秒×秒)]，V_F表示自车速度[米/秒]，a_F表示自车加速度[米/(秒×秒)]；W_j表示零均值的系统高斯白噪声向量且W_j＝[w₁  w₂  w₃  w₄  w₅]′，其中w₁、w₂、w₃、w₄及w₅分别表示五个系统高斯白噪声分量，其对应的系统噪声协方差阵Q_j为：

$Q_j=\left[\begin{array}{ccccc}{{\mathrm{\&sigma;}}_{w_1}}^2& 0& 0& 0& 0\\ 0& {{\mathrm{\&sigma;}}_{w_2}}^2& 0& 0& 0\\ 0& 0& {{\mathrm{\&sigma;}}_{w_3}}^2& 0& 0\\ 0& 0& 0& {{\mathrm{\&sigma;}}_{w_4}}^2& 0\\ 0& 0& 0& 0& {{\mathrm{\&sigma;}}_{w_5}}^2\end{array}\right],$ 其中及分别表示系统高斯白噪声w₁、w₂、w₃、w₄及w₅对应的方差；A_j表示状态转移矩阵，这7个模型具有不同的状态转移矩阵，其具体设置如下：

针对高速公路车辆运行及发生追尾碰撞的典型场景和工况进行分析，为简化模型，假定自车与前车行驶在同一车道上，定义工况1为安全工况：自车以等于前车的速度匀速行驶，工况2-7分别为常见的危险工况：

工况2：前车静止，自车匀速行驶；

工况3：前车静止，自车加速行驶；

工况4：前车匀速行驶，自车以大于前车速度的车速匀速行驶；

工况5：前车匀速行驶，自车加速行驶；

工况6：前车减速行驶，自车匀速行驶；

工况7：前车减速行驶，自车加速行驶；

这7种典型工况其所对应的状态转移矩阵A₁-A₇分别为：

$A_1=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0\end{array}\right],A_2=\left[\begin{array}{ccccc}1& 0& 0& -T& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0\end{array}\right],$

$A_3=\left[\begin{array}{ccccc}1& 0& 0& -T& {0.5T}^2\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& T\\ 0& 0& 0& 0& 1\end{array}\right],A_4=\left[\begin{array}{ccccc}1& T& 0& -T& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0\end{array}\right],$

$A_5=\left[\begin{array}{ccccc}1& T& 0& -T& {-0.5T}^2\\ 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& T\\ 0& 0& 0& 0& 1\end{array}\right],A_6=\left[\begin{array}{ccccc}1& T& {0.5T}^2& -T& 0\\ 0& 1& T& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0\end{array}\right],$

$A_7=\left[\begin{array}{ccccc}1& T& {0.5T}^2& -T& {-0.5T}^2\\ 0& 1& T& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& T\\ 0& 0& 0& 0& 1\end{array}\right]$

T表示离散的周期，在本发明中，根据测量传感器特性，T的典型值可取为50毫秒、100毫秒、200毫秒等；

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

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

Z_j(k)＝H_j(k)X_j(k)+V_j(k)(j＝1,2...7)      （2）

其中Z_j为观测向量，H_j为观测矩阵，V_j是与W_j互不相关的零均值观测噪声向量，其协方差矩阵为R_j；

本发明采用毫米波雷达、GPS（Global Positioning System，全球定位系统）、加速度计，结合V2V（vehicle to vehicle，车车通信）用以提供观测信息。其中，毫米波雷达用以测量两车相对距离与相对速度；GPS与加速度计用以提供自车位置、车速与加速度信息，V2V作为虚拟传感器，提供前车的位置、车速与加速度信息，GPS-V2V信息较为全面准确，但GPS信号受到遮挡或V2V因受到干扰而屏蔽而无法传输信息时，甚至可能前车并未安装V2V，都会导致无法提供有效的信息；

V2V发生故障或失效的分为两类，一类是通信故障，表现为无任何前车信息信号，另一类是由于前车的GPS传感器失效所引起的，表现为所传送的前车信息不完整，不包含前车的GPS传感器信息；自车GPS是否发生故障或失效则可利用HDOP（Horizontal Dilution Of Precision，水平精度因子）值结合卫星数来判别，在HDOP<5.0时且卫星数大于等于4个，判定为自车GPS能够正常工作，在HDOP>5.0时或卫星数小于4个时，判定为自车GPS失效；

基于这样的考虑，结合GPS-V2V与雷达的优缺点，当所有传感器都工作正常时，由于GPS-V2V信息全面而准确，以GPS、加速度计及V2V信息作为观测量；当车车通信由于通信故障而失效从而无法感知前车信息，但自车GPS能正常工作时，以GPS、加速度计及雷达信息作为观测量；当车车通信由于前车GPS失效从而无法获取前车GPS信息，但自车GPS能正常工作时，以GPS、加速度计、雷达信息以及前车加速度作为观测量；当自车GPS失效，但车车通信有效且能获取全面的前车信息时，以加速度计、雷达及车车通信信息作为观测量；当自车及前车GPS失效但车车通信能够获取前车加速度计信息时，以自车与前车加速度计信息和雷达信息作为观测量；当自车GPS失效时且车车通信无法获取前车信息时，以加速度计及雷达信息作为观测量；

即，Z_j、H_j与R_j都根据传感器是否失效进行切换：

①自车GPS有效且存在全面的车车通信信息，

$Z_j=\left[\begin{array}{c}{d}_{\mathrm{GPS}-V2V}\\ v_{L-V2V}\\ v_{F-\mathrm{GPS}}\\ a_{L-V2V}\\ a_{F-\mathrm{ACC}}\end{array}\right],H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\end{array}\right],$ d_GPS-V2V为由GPS及V2V所观测的自车与前车位置信息所计算出的车间距离[米]，v_L-V2V为V2V所传输的前车的速度[米/秒]，a_L-V2V为V2V所传输的前车加速度[米/(秒×秒)]，v_F-GPS为GPS所观测的自车速度[米/秒]，a_F-ACC为加速度计所观测的自车加速度[米/(秒×秒)]，此时，

$V_j=\left[\begin{array}{c}{n}_{d_{\mathrm{GPS}-V2V}}\\ {n_v}_{L-V2V}\\ {n_v}_{F-\mathrm{GPS}}\\ {n_a}_{L-V2V}\\ {n_a}_{F-\mathrm{ACC}}\end{array}\right],$ 表示通过GPS及V2V所观测的自车与前车位置信息所计算出的车间距离的观测噪声且是均值为0、方差为的高斯白噪声，表示通过V2V所传输的前车的速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过GPS所观测的自车速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过V2V所传输的前车加速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过加速度计所观测的自车加速度的观测噪声且是均值为0、方差为的高斯白噪声；V_j对应的观测噪声方差阵R_j可表示为

$R_j=\left[\begin{array}{ccccc}{\mathrm{\&sigma;}}_{d_{\mathrm{GPS}-V2V}}^2& 0& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{L-V2V}}^2& 0& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{v_{F-\mathrm{GPS}}}^2& 0& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{a_{L-V2V}}^2& 0\\ 0& 0& 0& 0& {\mathrm{\&sigma;}}_{a_{F-\mathrm{ACC}}}^2\end{array}\right];$

②自车GPS有效但不存在车车通信信息，

$Z_j=\left[\begin{array}{c}{d}_{\mathrm{radar}}\\ v_{F-\mathrm{GPS}}\\ a_{F-\mathrm{ACC}}\\ v_{r-\mathrm{radar}}\end{array}\right],H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\\ 0& 1& 0& -1& 0\end{array}\right],$ d_radar为由雷达所观测的车间距离[米]，v_r-radar为雷达所观测的前车相对于自车的速度[米/秒]， $V_j=\left[\begin{array}{c}{n}_{d_{\mathrm{radar}}}\\ n_{v_{F-\mathrm{GPS}}}\\ n_{a_{F-\mathrm{ACC}}}\\ n_{v_{r-\mathrm{radar}}}\end{array}\right],$ 表示通过雷达传感器测量获得的车间距离的观测噪声且是均值为0、方差为的高斯白噪声，表示通过雷达所观测的前车相对于自车的速度的观测噪声且是均值为0、方差为的高斯白噪声，V_j对应的观测噪声方差阵R_j可表示为 $R_h=\left[\begin{array}{cccc}{\mathrm{\&sigma;}}_{d_{\mathrm{radar}}}^2& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{F-\mathrm{GPS}}}^2& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{a_{F-\mathrm{ACC}}}^2& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{v_{r-\mathrm{radar}}}^2\end{array}\right];$

③自车GPS有效且存在车车通信信息，但因前车GPS失效，车车通信信息里不包括前车GPS信息，

$Z_j=\left[\begin{array}{c}{d}_{\mathrm{radar}}\\ v_{F-\mathrm{GPS}}\\ a_{F-\mathrm{ACC}}\\ v_{r-\mathrm{radar}}\\ a_{L-V2V}\end{array}\right],H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\\ 0& 1& 0& -1& 0\\ 0& 0& 1& 0& 0\end{array}\right],$ V_j对应的观测噪声方差阵R_j可表示为

$R_j=\left[\begin{array}{ccccc}{\mathrm{\&sigma;}}_{d_{\mathrm{radar}}}^2& 0& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{F-\mathrm{GPS}}}^2& 0& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{a_{F-\mathrm{ACC}}}^2& 0& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{v_{r-\mathrm{radar}}}^2& 0\\ 0& 0& 0& 0& {\mathrm{\&sigma;}}_{a_{L-V2V}}^2\end{array}\right];$

④自车GPS失效但存在全面的车车通信信息

$Z_j=\left[\begin{array}{c}{d}_{\mathrm{radar}}\\ v_{r-\mathrm{radar}}\\ v_{L-V2V}\\ a_{L-V2V}\\ a_{F-\mathrm{ACC}}\end{array}\right],H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& -1& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\end{array}\right],$ V_j对应的观测噪声方差阵R_j可表示

为 $R_j=\left[\begin{array}{ccccc}{\mathrm{\&sigma;}}_{d_{\mathrm{radar}}}^2& 0& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{r-\mathrm{radar}}}^2& 0& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{v_{L-V2V}}^2& 0& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{a_{L-V2V}}^2& 0\\ 0& 0& 0& 0& {\mathrm{\&sigma;}}_{a_{F-\mathrm{ACC}}}^2\end{array}\right];$

⑤自车与前车GPS都失效，但存在车车通信信息，能够接收到前车的加速度信息

$Z_j=\left[\begin{array}{c}{d}_{\mathrm{radar}}\\ a_{L-V2V}\\ a_{F-\mathrm{ACC}}\\ v_{r-\mathrm{radar}}\end{array}\right],H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\\ 0& 1& 0& -1& 0\end{array}\right],$ V_j对应的观测噪声方差阵R_j可表示

为 $R_j=\left[\begin{array}{cccc}{\mathrm{\&sigma;}}_{d_{\mathrm{radar}}}^2& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{a_{L-V2V}}^2& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{a_{F-\mathrm{ACC}}}^2& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{v_{r-\mathrm{radar}}}^2\end{array}\right];$

⑥自车GPS失效且不存在车车通信信息，

$Z_j=\left[\begin{array}{c}{d}_{\mathrm{radar}}\\ v_{r-\mathrm{radar}}\\ a_{F-\mathrm{ACC}}\end{array}\right],H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& -1& 0\\ 0& 0& 0& 0& 1\end{array}\right]$ ，V_j对应的观测噪声方差阵R_j

可表示为 $R_j=\left[\begin{array}{ccc}{\mathrm{\&sigma;}}_{d_{\mathrm{radar}}}^2& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{r-\mathrm{radar}}}^2& 0\\ 0& 0& {\mathrm{\&sigma;}}_{a_{F-\mathrm{ACC}}}^2\end{array}\right];$

3）交互多模型估计车辆间相对距离、车速与加速度

根据式（1）和式（2），分别建立7个不同的卡尔曼滤波模型，系统模型之间的转移概率为p_ij，下标i、j（i=1,2,3...7,j=1,2,3...7）表示从状态i转移到状态j的概率；

①交互估计计算

预测第j(j=1,2,3...7)个模型的模型概率ρ_j(k,k-1):

${\mathrm{\&rho;}}_j(k,k-1)=\underset{i=1}{\overset{7}{\mathrm{\&Sigma;}}}{p}_{\mathrm{ij}}{\mathrm{\&rho;}}_i(k-1)$

预测混合概率ρ_i|j(k-1):

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

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

$X_{0j}(k-1)=\underset{i=1}{\overset{7}{\mathrm{\&Sigma;}}}{X}_i(k-1){\mathrm{\&rho;}}_{i|j}(k-1)$

$P_{0j}(k-1)=\underset{i=1}{\overset{7}{\mathrm{\&Sigma;}}}{\mathrm{\&rho;}}_{i|j}(k-1)\{P_i(k-1)+[X_i(k-1)-X_{0j}(k-1)\left]{X_i(k-1)-X_{0j}(k-1)]}^{\&prime;}\right\}$

其中P_i(k-1)表示第i个模型在k-1时刻的估计误差方差阵；

②每个卡尔曼滤波器各自进行卡尔曼滤波递推，第j(j=1,2,3...7)个模型的滤波过程如下：

时间更新：

状态一步预测方程:X_j(k,k-1)＝A_jX_0j(k-1)

一步预测误差方差阵：P_j(k,k-1)＝A_j(k-1)P_0j(k-1)(A_j(k-1))′+Q_j(k-1)

测量更新：

滤波增益矩阵：K_j(k)＝P_j(k,k-1)(H_j(k-1))′(S_j(k))^-1

S_j(k)＝H_j(k-1)P_j(k,k-1)(H_j(k-1))′+R_j(k-1)

状态估计：X_j(k)＝X_j(k,k-1)+K_j(k)(Z_j(k)-H_jX_j(k,k-1))

估计误差方差阵P_j(k)：P_j(k)＝P_j(k,k-1)-K_j(k)S_j(k)(K_j(k))′

③模型概率更新

在每个模型完成上一步的更新之后，利用最大似然函数Λ_j(k)计算新的模型概率ρ_j(k)；最大似然函数计算如下：

${\mathrm{\&Lambda;}}_j\left(k\right)=\frac{\exp \{-\frac{1}{2}{(Z_j\left(k\right)-H_j{X}_j(k,k-1))}^{\&prime;}{\left(S_j\left(k\right)\right)}^{-1}(Z_j\left(k\right)-H_j{X}_j(k,k-1))\}}{{\left|2\mathrm{\&pi;}{S}_j\left(k\right)\right|}^{-\frac{1}{2}}}$

因此，模型j(j=1,2,3...7)在k时刻的模型概率由贝叶斯定理给出：

${\mathrm{\&rho;}}_j\left(k\right)=\frac{{\mathrm{\&Lambda;}}_j\left(k\right){\mathrm{\&rho;}}_j(k,k-1)}{\underset{i=1}{\overset{7}{\mathrm{\&Sigma;}}}{\mathrm{\&Lambda;}}_i\left(k\right){\mathrm{\&rho;}}_i(k,k-1)}$

④估计组合

在计算出各模型为正确的后验概率之后，对所有滤波器的状态估计进行概率加权并求和，权系数为模型正确的后验概率，得到估计组合后的状态估计为： $X\left(k\right)=\underset{j=1}{\overset{7}{\mathrm{\&Sigma;}}}{X}_j\left(k\right){\mathrm{\&rho;}}_j\left(k\right),$ 其中， $X\left(k\right)={\left[\begin{array}{ccccc}\stackrel{\&OverBar;}{d}& {\stackrel{\&OverBar;}{v}}_L& {\stackrel{\&OverBar;}{a}}_L& {\stackrel{\&OverBar;}{v}}_F& {\stackrel{\&OverBar;}{a}}_F\end{array}\right]}^{\&prime;},$ 各状态变量的上标“-”表示各状态量的最终滤波估计值，即X(k)内各状态变量依次分别表示估计组合后的车距、前车速度、前车加速度、自车速度、自车加速度，据此即可推算出估计组合后的两车相对距离、速度和加速度，即估计组合后的车辆间相对距离[米]可直接估计得出，估计组合后前车相对于自车的速度[米/秒]估计组合后前车相对于自车的加速度

4）改进TTC估计

传统的TTC计算方法如式（3）所示：

$\mathrm{TTC}=-\frac{\stackrel{\&OverBar;}{d}}{{\stackrel{\&OverBar;}{v}}_r}---\left(3\right)$

式（3）中，为估计组合后前车相对于自车的速度[米/秒]，且该方法假设车辆间相对加速度为零，存在较大的误差；本发明在此基础上对其进行改进，考虑车辆间相对加速度信息，有

$\stackrel{\&OverBar;}{d}+{\stackrel{\&OverBar;}{v}}_r\&times;\mathrm{TTC}+0.5{\stackrel{\&OverBar;}{a}}_r\&times;{\mathrm{TTC}}^2=0---\left(4\right)$

式（4）中，为估计组合后前车相对于自车的加速度，且解式（4）可得：

$\mathrm{TTC}=\left\{\begin{array}{cc}-\frac{\stackrel{\&OverBar;}{d}}{{\stackrel{\&OverBar;}{v}}_r}& {\stackrel{\&OverBar;}{v}}_r<0,{\stackrel{\&OverBar;}{a}}_r=0\\ -\frac{{\stackrel{\&OverBar;}{v}}_r}{{\stackrel{\&OverBar;}{a}}_r}-\frac{\sqrt{{\stackrel{\&OverBar;}{v}}_r^2-2\stackrel{\&OverBar;}{d}\&times;{\stackrel{\&OverBar;}{a}}_r}}{a_r}& {\stackrel{\&OverBar;}{v}}_r<0,{\stackrel{\&OverBar;}{a}}_r\&NotEqual;0\\ -\frac{{\stackrel{\&OverBar;}{v}}_r}{{\stackrel{\&OverBar;}{a}}_r}+\frac{\sqrt{{\stackrel{\&OverBar;}{v}}_r^2-2\stackrel{\&OverBar;}{d}\&times;{\stackrel{\&OverBar;}{a}}_r}}{{\stackrel{\&OverBar;}{a}}_r}& {\stackrel{\&OverBar;}{v}}_r\&GreaterEqual;0,{\stackrel{\&OverBar;}{a}}_r<0\end{array}\right.---\left(5\right)$

将利用交互多模型方法所估计出的值代入（5）即可实时计算出TTC值，且当或（且）时式（5）无解，此时前车车速与加速度都大于自车，显见车辆处于安全状态，因此此时无需计算TTC值。

本发明的优点及显著效果：

1.本发明提出了一种高速公路汽车追尾碰撞预警关键参数估计方法，可满足高速公路汽车追尾碰撞预警及控制的需要，具有精度高、可靠性好、实时性好、适应性好等优点。

2.本发明的方法是针对高速公路车辆不同的相对运动典型工况提出的，并利用交互多模型算法实现不同工况模型间的交互，并充分利用车车通信技术能够全面可靠获取信息的优点，将其结合全球定位系统与雷达信息来建立滤波系统的观测量，保证在不同的工况下对于两车相对距离、速度与加速度等高速公路追尾碰撞预警关键参数估计的准确性。

3.本发明采用多传感器融合的方法，在运行过程中检测、容忍传感器信息的不准确甚至失效，保证对于两车相对距离、速度与加速度等高速公路追尾碰撞预警关键参数估计的可靠性和鲁棒性，即使某些传感器失效，依然能取得较好的估计效果。

4.本发明结合相对加速度信息，对传统TTC（Time to collision，距离碰撞时间）估计方法进行改进，能够准确、可靠地估计出TTC这个高速公路追尾碰撞预警关键参数。

附图说明

图1.本发明所提出方法流程框图；

图2.观测方程切换框图；

具体实施方式

实施实例

在我国，随着汽车工业蓬勃发展和汽车保有量迅速增长，道路交通事故频繁发生，由此导致的人员伤亡和财产损失数目惊人，给国家的人民生命财产和国民经济造成了巨大的损失。因此，如何采取合理的手段与措施，减少交通事故的发生成为一个亟待解决的问题，这不仅是政府和人民普遍关注的社会问题，同时也是科学技术进步所面临的重要课题之一。近年来，尽管采用越来越多的被动安全技术减轻了事故的伤害程度，但引发交通事故产生的根本原因仍未得到有效解决。在《中国汽车工业中长期科技发展战略研究》（2004-2020年）中，也提出了“我国汽车被动安全技术的发展相对较快，以电子技术为基础的大量主动安全技术还没有开展或只是刚刚起步，这将是我国今后安全技术发展工作中需要重视的问题”。基于这样的考虑，智能交通系统ITS(Intelligent Transport System)应运而生。其中的先进汽车控制与安全系统AVCSS(Advanced Vehicle Control andSafety System)是智能交通系统中以车辆为研究对象的子系统，提高汽车主动安全性是该系统要实现的目标。

交通事故的主要原因是日益突出的人、车、路之间的矛盾，包括超载、超速、酒后驾驶、疲劳驾驶或驾驶员注意力不集中等原因，其主要表现形式是碰撞事故，包括车与车的碰撞、车与人的碰撞、车与固定物的碰撞等。对交通事故的统计分析表明，车与车的碰撞在交通事故中占到了60%-70%，而所造成的财产损失和人员伤亡也占到了60-70%。而在不同类型的碰撞事故中，追尾碰撞所占比例最大，所造成的危害也是最大，特别是在高速公路上，追尾碰撞更是最主要的事故形式。因此，高速公路追尾碰撞预警系统的研究引起了世界各国的重视。追尾碰撞预警系统是通过感知前方车辆与自车相对运动信息，利用某种算法判定是否存在潜在的追尾碰撞危险，环境感知与预警算法是其中的两项关键技术。

在环境感知方面，国内和国外的防碰撞预警系统都是通过在本车上安装雷达、红外或视觉传感器来获取周围车辆信息，但无论是哪种传感器，都具有很大的局限性，其中，毫米波雷达最为常用，性能也最为优良，但所测信息噪声较大，无法有效区分旁车道干扰目标，且会受恶劣天气影响和周围车辆、障碍物的电磁波干扰。特别在高速路上，隔离带和路两边的金属极大的限制了雷达性能；机器视觉技术对环境、天气变化的适应性不高，深度信息探测准确性差；且无论是雷达、红外，还是机器视觉，都无法获取相对加速度这个信息。近年来，国外很多学者对基于全球定位系统与车车通信（GPS-V2V，Global Positioning System-vehicle to vehicle）的防追尾碰撞技术进行了研究，其中，美国的加州大学伯克利分校是全球定位系统与车车通信应用于防碰撞预警系统等主动安全领域的先驱者，其PATH项目组对于基于车车通讯的各种主动安全技术如车道偏离预警、追尾碰撞预警等进行了一系列的研究和实验验证工作，充分验证了基于全球定位系统与车车通信的汽车主动安全的技术可行性和优势，该技术所获取的信息的较为全面和准确，并且不受天气、周围障碍物、旁车道干扰目标等影响，但其有效性取决于GPS和车车通信所传递信息的准确可靠，若GPS受到遮挡，或因车车通信受到干扰而被屏蔽或延时，则会导致信息的不准确甚至无法获取，且车车通信并未普及，很多车辆并不具备该功能。同时，无论是雷达还是GPS-V2V，都不可避免存在因传感器故障和失效引起的测量信息产生无法确定的变化甚至错误，影响防追尾碰撞预警系统的的可靠性。

国内外的追尾碰撞预警算法主要包括安全距离逻辑算法与安全时间逻辑算法两类，TTC（Time to collision，距离碰撞时间）是算法中的一个关键参数，其估计的准确性和可靠性直接关系着预警算法是否准确有效。国内外的TTC值计算都是以当前两车相对距离除以相对速度，这种方法忽略了相对加速度这个参数，导致计算结果误差较大，易产生虚警和漏警，影响算法的准确性。同时，为滤除雷达等传感器观测噪声，获取较为准确的与前方车辆的车间距离、速度等信息，卡尔曼滤波、粒子滤波等滤波方法被广泛应用，近年来，交互多模型（IMM）机动目标跟踪算法也被应用于此，进一步减小了测量误差，但所采用的模型未考虑高速公路追尾碰撞典型工况，准确性不足，也无法对故障或失效传感器信息有效处理和容忍，影响方法的可靠性。这些都限制了其在汽车追尾碰撞预警系统中的使用。

为在不同汽车相对运动工况下实现在对于车辆间相对距离、速度、加速度与TTC（Time to collision，距离碰撞时间）这些高速公路追尾碰撞预警关键参数的准确、可靠估计，本发明提出了一种高速公路汽车追尾碰撞预警关键参数估计方法。本发明提出的方法针对高速公路车辆不同的相对运动典型工况，建立多个卡尔曼滤波模型，同时以全球定位系统与车车通信信息结合雷达信息来建立滤波系统的观测量，并在运行过程中检测、容忍传感器信息的不准确甚至失效，实现多传感器的有效互补，即使在某些传感器失效时依然能够获取较为准确的相关参数，进而通过交互多模型-卡尔曼滤波算法实现对两车相对距离、速度与加速度等高速公路追尾碰撞预警关键参数的估计，同时结合相对加速度信息，来改进传统TTC（Time to collision，距离碰撞时间）估计方法，以准确、可靠地估计出TTC这个高速公路追尾碰撞预警关键参数，具有精度高、可靠性好、实时性好、适应性好等特点，本发明的具体思路如下：

交互多模型算法具有自适应的特点，通过建立不同的多个模型滤波器，各模型滤波器通过估计状态的组合实现交互，模型之间基于马尔可夫链进行切换，能够有效地对各个模型的概率进行调整。本发明的交互多模型算法中通过卡尔曼滤波（Extended Kalman Filter，EKF）方法建立模型滤波器，卡尔曼滤波器是以最小均方差为准则的最优状态估计滤波器，它不需要储存过去的测量值，只根据当前的观测值和前一时刻的估计值，利用计算机进行递推计算，便可实现对实时信号的估计。

为实现不同高速公路汽车相对运动典型工况下追尾碰撞预警关键参数的估计，首先对汽车相对运动进行适当的运动学建模，即建立卡尔曼滤波过程的系统状态方程。针对本发明的应用领域，本发明对于行驶在高速公路上的四轮车辆，可做自车与前车行驶在同一车道上的合理假定。

针对高速公路汽车行驶工况，建立7个不同的卡尔曼滤波模型，这7个模型具有相同的状态模型形式，其区别仅在于状态转移矩阵的不同，第j个模型离散化后的卡尔曼滤波的状态方程的矩阵形式表示为：

X_j(k)＝A_j(k,k-1)X_j(k-1)+W_j(k-1)(j＝1,2...7)    （1）

式中，k表示离散化时刻，下标j（j=1，2...7）表示第j个模型，本发明的多模型设置为7个模型，这7个模型有相同的状态向量，该状态向量X_j＝[d  v_L  a_L  v_F  a_F]′，本发明中上角标'表示对矩阵转置，d表示前车距自车的距离[米]，V_L表示前车速度[米/秒]，a_L表示前车加速度[米/(秒×秒)]，V_F表示自车速度[米/秒]，a_F表示自车加速度[米/(秒×秒)]；W_j表示零均值的系统高斯白噪声向量且W_j＝[w₁  w₂  w₃  w₄  w₅]′，其中w₁、w₂、w₃、w₄及w₅分别表示五个系统高斯白噪声分量，其对应的系统噪声协方差阵Q_j为：

$Q_j=\left[\begin{array}{ccccc}{{\mathrm{\&sigma;}}_{w_1}}^2& 0& 0& 0& 0\\ 0& {{\mathrm{\&sigma;}}_{w_2}}^2& 0& 0& 0\\ 0& 0& {{\mathrm{\&sigma;}}_{w_3}}^2& 0& 0\\ 0& 0& 0& {{\mathrm{\&sigma;}}_{w_4}}^2& 0\\ 0& 0& 0& 0& {{\mathrm{\&sigma;}}_{w_5}}^2\end{array}\right],$ 其中及分别表示系统高斯白噪声w₁、w₂、w₃、w₄及w₅对应的方差；A_j表示状态转移矩阵，这7个模型具有不同的状态转移矩阵，其具体设置如下：

针对高速公路车辆运行及发生追尾碰撞的典型场景和工况进行分析，为简化模型，假定自车与前车行驶在同一车道上，定义如下6种较为普遍的追尾碰撞危险工况：

工况2：前车静止，自车匀速行驶；

工况3：前车静止，自车加速行驶；

工况4：前车匀速行驶，自车以大于前车速度的车速匀速行驶；

工况5：前车匀速行驶，自车加速行驶；

工况6：前车减速行驶，自车匀速行驶；

工况7：前车减速行驶，自车加速行驶；

除了所定义的六种典型危险工况之外，多模型中还应考虑高速公路上常见安全状态：

工况1：自车以等于前车的速度匀速行驶；

工况8：自车以小于前车的速度匀速行驶；

工况9：前车加速行驶，自车匀速行驶；

工况10：前车匀速行驶，自车减速行驶；

工况11：前车加速行驶，自车减速行驶；

可见工况8,9,10,11的状态转移矩阵分别等同于工况4,6,5,7因此仅对工况1-7求取状态转移矩阵即可。

这7种典型工况其所对应的状态转移矩阵A₁-A₇分别为：

$A_1=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0\end{array}\right],A_2=\left[\begin{array}{ccccc}1& 0& 0& -T& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0\end{array}\right],$

$A_3=\left[\begin{array}{ccccc}1& 0& 0& -T& -0.5T^2\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& T\\ 0& 0& 0& 0& 1\end{array}\right],A_4=\left[\begin{array}{ccccc}1& T& 0& -T& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0\end{array}\right],$

$A_5=\left[\begin{array}{ccccc}1& T& 0& -T& -0.5T^2\\ 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& T\\ 0& 0& 0& 0& 1\end{array}\right],A_6=\left[\begin{array}{ccccc}1& T& 0.5T^2& -T& 0\\ 0& 1& T& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0\end{array}\right],$

$A_7=\left[\begin{array}{ccccc}1& T& 0.5T^2& -T& -0.5T^2\\ 0& 1& T& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& T\\ 0& 0& 0& 0& 1\end{array}\right]$

T表示离散的周期（在本发明中，根据测量传感器特性，T的典型值可取为50毫秒、100毫秒、200毫秒等）；

在本发明中，卡尔曼滤波模型的观测量根据传感器是否失效进行实时切换，以保证在运行过程中检测、容忍传感器信息的不准确甚至失效，本发明采用毫米波雷达、GPS、加速度计，结合V2V用以提供观测信息。毫米波雷达用以测量两车相对距离与相对速度，测量信息所含噪声较大，受恶劣天气、旁边车辆影响会导致信息误差加大，且无法提供加速度信息，但优点在于其失效形式表现为软失效，即测量误差增大，而并非完全失去作用的硬失效。GPS与加速度计用以提供自车位置、车速与加速度信息，V2V作为虚拟传感器，提供前车的位置、车速与加速度信息，GPS-V2V信息较为全面准确，但GPS信号受到遮挡或V2V因受到干扰而屏蔽而无法传输信息时，甚至可能前车并未安装V2V，都会导致无法提供有效的前车信息，同时若前车GPS传感器发生故障，车车通信所传输的信息也会发生错误。在此假定前车也安装GPS传感器与加速度传感器。

V2V发生故障或失效的分为两类，一类是通信故障，表现为无前车信息信号，很易判别，另一类是由于前车的GPS传感器失效所引起的，表现为所传送的前车信息不完整，不包含前车的GPS传感器信息；自车GPS是否发生故障或失效则可利用HDOP（Horizontal Dilution Of Precision，水平精度因子）值结合卫星数来判别，在HDOP<5.0时且卫星数大于等于4个，判定为自车GPS能够正常工作，在HDOP>5.0时或卫星数小于4个时，判定为自车GPS失效；在此假定前车也具备GPS的故障检测功能，当前车GPS被判定发生故障时，则车车通信不向自车发送前车的GPS信息。

基于这样的考虑，结合GPS-V2V与雷达的优缺点，当所有传感器都工作正常时，由于GPS-V2V信息全面而准确，以GPS、加速度计及V2V信息作为观测量；当车车通信由于通信故障而失效从而无法感知前车信息，但自车GPS能正常工作时，以GPS、加速度计及雷达信息作为观测量；当车车通信由于前车GPS失效从而无法获取前车GPS信息，但自车GPS能正常工作时，以GPS、加速度计、雷达信息以及前车加速度作为观测量；当自车GPS失效但车车通信有效能获取全面地前车信息时，以加速度计、雷达及车车通信信息作为观测量；当自车及前车GPS失效但车车通信能够获取前车加速度计信息时，以自车与前车加速度计信息和雷达信息作为观测量；当自车GPS失效时且车车通信无法获取前车信息时，以加速度计及雷达信息作为观测量；

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

Z_j(k)＝H_j(k)X_j(k)+V_j(k)(j＝1,2...7)    （2）

式（2）中其中Z_j为观测向量，H_j为观测矩阵，V_j是与W_j互不相关的零均值观测噪声向量，其协方差矩阵为R_j，Z_j、H_j与R_j都根据传感器是否失效进行切换，如附图2所示：

①自车GPS有效且存在全面的车车通信信息，

$Z_j=\left[\begin{array}{c}{d}_{\mathrm{GPS}-V2V}\\ v_{L-V2V}\\ v_{F-\mathrm{GPS}}\\ a_{L-V2V}\\ a_{F-\mathrm{ACC}}\end{array}\right],H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\end{array}\right],$ d_GPS-V2V为由GPS及V2V所观测的自车与前车位置信息所计算出的车间距离[米]，v_L-V2V为V2V所传输的前车的速度[米/秒]，a_L-V2V为V2V所传输的前车加速度[米/(秒×秒)]，v_F-GPS为GPS所观测的自车速度[米/秒]，a_F-ACC为加速度计所观测的自车加速度[米/(秒×秒)]，此时，

$V_j=\left[\begin{array}{c}{n}_{d_{\mathrm{GPS}-V2V}}\\ n_{v_{L-V2V}}\\ n_{v_{F-\mathrm{GPS}}}\\ n_{a_{L-V2V}}\\ n_{a_{F-\mathrm{ACC}}}\end{array}\right],$ 表示通过GPS及V2V所观测的自车与前车位置信息所计算出的车间距离的观测噪声且是均值为0、方差为的高斯白噪声，表示通过V2V所传输的前车的速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过GPS所观测的自车速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过V2V所传输的前车加速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过加速度计所观测的自车加速度的观测噪声且是均值为0、方差为的高斯白噪声；V_j对应的观测噪声方差阵R_j可表示为

$R_j=\left[\begin{array}{ccccc}{\mathrm{\&sigma;}}_{d_{\mathrm{GPS}-V2V}}^2& 0& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{L-V2V}}^2& 0& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{v_{F-\mathrm{GPS}}}^2& 0& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{a_{L-V2V}}^2& 0\\ 0& 0& 0& 0& {\mathrm{\&sigma;}}_{a_{F-\mathrm{ACC}}}^2\end{array}\right];$

②自车GPS有效但不存在车车通信信息，

$Z_j=\left[\begin{array}{c}{d}_{\mathrm{radar}}\\ v_{F-\mathrm{GPS}}\\ a_{F-\mathrm{ACC}}\\ v_{r-\mathrm{radar}}\end{array}\right],H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\\ 0& 1& 0& -1& 0\end{array}\right],$ d_radar为由雷达所观测的车间距离[米]，v_r-radar为雷达所观测的前车相对于自车的速度[米/秒]， $V_j=\left[\begin{array}{c}{n}_{d_{\mathrm{radar}}}\\ n_{v_{F-\mathrm{GPS}}}\\ n_{a_{F-\mathrm{ACC}}}\\ n_{v_{r-\mathrm{radar}}}\end{array}\right],$ 表示通过雷达传感器测量获得的车间距离的观测噪声且是均值为0、方差为的高斯白噪声，表示通过雷达所观测的前车相对于自车的速度的观测噪声且是均值为0、方差为的高斯白噪声，V_j对应的观测噪声方差阵R_j可表示为 $R_j=\left[\begin{array}{cccc}{\mathrm{\&sigma;}}_{d_{\mathrm{radar}}}^2& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{F-\mathrm{GPS}}}^2& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{a_{F-\mathrm{ACC}}}^2& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{v_{r-\mathrm{radar}}}^2\end{array}\right];$

③自车GPS有效且存在车车通信信息，但因前车GPS失效，车车通信信息里不包括前车GPS信息，

$Z_j=\left[\begin{array}{c}{d}_{\mathrm{radar}}\\ v_{F-\mathrm{GPS}}\\ v_{F-\mathrm{ACC}}\\ a_{r-\mathrm{radar}}\\ a_{L-V2V}\end{array}\right],H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\\ 0& 1& 0& -1& 0\\ 0& 0& 1& 0& 0\end{array}\right],$ V_j对应的观测噪声方差阵R_j可表示为

$R_j=\left[\begin{array}{ccccc}{\mathrm{\&sigma;}}_{d_{\mathrm{radar}}}^2& 0& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{F-\mathrm{GPS}}}^2& 0& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{a_{F-\mathrm{ACC}}}^2& 0& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{v_{r-\mathrm{radar}}}^2& 0\\ 0& 0& 0& 0& {\mathrm{\&sigma;}}_{a_{L-V2V}}^2\end{array}\right];$

④自车GPS失效但存在全面的车车通信信息

$Z_j=\left[\begin{array}{c}{d}_{\mathrm{radar}}\\ v_{r-\mathrm{radar}}\\ v_{L-V2V}\\ a_{L-V2V}\\ a_{F-\mathrm{ACC}}\end{array}\right],H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& -1& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\end{array}\right],$ V_j对应的观测噪声方差阵R_j可表示

为 $R_j=\left[\begin{array}{ccccc}{\mathrm{\&sigma;}}_{d_{\mathrm{radar}}}^2& 0& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{r-\mathrm{radar}}}^2& 0& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{v_{L-V2V}}^2& 0& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{a_{L-V2V}}^2& 0\\ 0& 0& 0& 0& {\mathrm{\&sigma;}}_{a_{F-\mathrm{ACC}}}^2\end{array}\right];$

⑤自车与前车GPS都失效，但存在车车通信信息，能够接收到前车的加速度信息

$Z_j=\left[\begin{array}{c}{d}_{\mathrm{radar}}\\ a_{L-V2V}\\ a_{F-\mathrm{ACC}}\\ v_{r-\mathrm{radar}}\end{array}\right],H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\\ 0& 1& 0& -1& 0\end{array}\right],$ V_j对应的观测噪声方差阵R_j可表示

为 $R_j=\left[\begin{array}{cccc}{\mathrm{\&sigma;}}_{d_{\mathrm{radar}}}^2& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{a_{L-V2V}}^2& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{a_{F-\mathrm{ACC}}}^2& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{v_{r-\mathrm{radar}}}^2\end{array}\right]\text{;}$

⑥自车GPS失效且不存在车车通信信息，

$Z_j=\left[\begin{array}{c}{d}_{\mathrm{radar}}\\ v_{r-\mathrm{radar}}\\ a_{F-\mathrm{ACC}}\end{array}\right],H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& -1& 0\\ 0& 0& 0& 0& 1\end{array}\right],$ V_j对应的观测噪声方差阵Rj

可表示为 $R_j=\left[\begin{array}{ccc}{\mathrm{\&sigma;}}_{d_{\mathrm{radar}}}^2& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{r-\mathrm{radar}}}^2& 0\\ 0& 0& {\mathrm{\&sigma;}}_{a_{F-\mathrm{ACC}}}^2\end{array}\right];$

针对上述高速公路行驶车辆典型工况，建立七个不同的卡尔曼滤波模型，系统模型之间的转移概率为p_ij，下标i、j（i=1,2,3...7,j=1,2,3...7）表示从状态i转移到状态j的概率；

1）交互估计计算

预测第j(j=1,2,3...7)个模型的模型概率ρ_j(k,k-1):

${\mathrm{\&rho;}}_j(k,k-1)=\underset{i=1}{\overset{7}{\mathrm{\&Sigma;}}}{p}_{\mathrm{ij}}{\mathrm{\&rho;}}_i(k-1)$

预测混合概率ρ_i|j(k-1):

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

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

$X_{0j}=(k-1)=\underset{i=1}{\overset{7}{\mathrm{\&Sigma;}}}{X}_j(k-1){\mathrm{\&rho;}}_{i|j}(k-1)$

$P_{0j}(k-1)=\underset{i=1}{\overset{7}{\mathrm{\&Sigma;}}}{\mathrm{\&rho;}}_{i|j}(k-1)\{P_i(k-1)+[X_i(k-1)-X_{0j}(k-1)\left]{[X_i(k-1)-X_{0j}(k-1)]}^{\&prime;}\right\}$

其中P_i(k-1)表示第i个模型在k-1时刻的估计误差方差阵；

2）每个卡尔曼滤波器各自进行卡尔曼滤波递推，第j(j=1,2,3...7)个模型的滤波过程如下：

时间更新：

状态一步预测方程:X_j(k,k-1)＝A_jX_0j(k-1)

一步预测误差方差阵：P_j(k,k-1)＝A_j(k-1)P_0j(k-1)(A_j(k-1))′+Q_j(k-1)测量更新：

滤波增益矩阵：K_j(k)＝P_j(k,k-1)(H_j(k-1))′(S_j(k))^-1

S_j(k)＝H_j(k-1)P_j(k,k-1)(H_j(k-1))′+R_j(k-1)

状态估计：X_j(k)＝X_j(k,k-1)+K_j(k)(Z_j(k)-H_jX_j(k,k-1))

估计误差方差阵P_j(k)：P_j(k)＝P_j(k,k-1)-K_j(k)S_j(k)(K_j(k))′

3）模型概率更新

在每个模型完成上一步的更新之后，利用最大似然函数Λ_j(k)计算新的模型概率ρ_j(k)；最大似然函数计算如下：

${\mathrm{\&Lambda;}}_j\left(k\right)=\frac{\exp \{-\frac{1}{2}{(Z_j\left(k\right)-H_j{X}_j(k,k-1))}^{\&prime;}{\left(S_j\left(k\right)\right)}^{-1}(Z_j\left(k\right)-H_j{X}_j(k,k-1))\}}{{\left|2\mathrm{\&pi;}{S}_j\left(k\right)\right|}^{-\frac{1}{2}}}$

因此，模型j(j=1,2,3...7)在k时刻的模型概率由贝叶斯定理给出：

${\mathrm{\&rho;}}_j\left(k\right)=\frac{{\mathrm{\&Lambda;}}_j\left(k\right){\mathrm{\&rho;}}_j(k,k-1)}{\underset{i=1}{\overset{7}{\mathrm{\&Sigma;}}}{\mathrm{\&Lambda;}}_i\left(k\right){\mathrm{\&rho;}}_i(k,k-1)}$

4）估计组合

在计算出各模型为正确的后验概率之后，对所有滤波器的状态估计进行概率加权并求和，权系数为模型正确的后验概率，得到估计组合后的状态估计为： $\left(k\right)=\underset{j=1}{\overset{7}{\mathrm{\&Sigma;}}}{X}_j\left(k\right){\mathrm{\&rho;}}_j\left(k\right),$ 其中， $X\left(k\right)={\left[\begin{array}{ccccc}\stackrel{\&OverBar;}{d}& {\stackrel{\&OverBar;}{v}}_L& {\stackrel{\&OverBar;}{a}}_L& {\stackrel{\&OverBar;}{v}}_F& {\stackrel{\&OverBar;}{a}}_F\end{array}\right]}^{\&prime;},$ 各状态变量的上标“-”表示各状态量的最终滤波估计值，即X(k)内各状态变量依次分别表示估计组合后的车距、前车速度、前车加速度、自车速度、自车加速度，据此即可推算出估计组合后的两车相对距离、速度和加速度，即估计组合后的车辆间相对距离[米]可直接估计得出，估计组合后前车相对于自车的速度[米/秒]估计组合后前车相对于自车的加速度

在交互多模型估计的基础上，对传统的TTC计算方法进行改进，传统的TTC计算方法如式（3）所示：

$\mathrm{TTC}=-\frac{\stackrel{\&OverBar;}{d}}{{\stackrel{\&OverBar;}{v}}_r}---\left(3\right)$

式（3）中，为估计组合后前车相对于自车的速度[米/秒]，且该方法假设车辆间相对加速度为零，存在较大的误差；本发明在此基础上对其进行改进，考虑车辆间相对加速度信息，有

$\stackrel{\&OverBar;}{d}+{\stackrel{\&OverBar;}{v}}_r\&times;\mathrm{TTC}+0.5{\stackrel{\&OverBar;}{a}}_r\&times;{\mathrm{TTC}}^2=0---\left(4\right)$

式（4）中，为估计组合后前车相对于自车的加速度，且解式（4）可得：

$\mathrm{TTC}=\left\{\begin{array}{cc}-\frac{\stackrel{\&OverBar;}{d}}{{\stackrel{\&OverBar;}{v}}_r}& {\stackrel{\&OverBar;}{v}}_r<0,{\stackrel{\&OverBar;}{a}}_r=0\\ -\frac{{\stackrel{\&OverBar;}{v}}_r}{{\stackrel{\&OverBar;}{a}}_r}-\frac{\sqrt{{\stackrel{\&OverBar;}{v}}_r^2-2\stackrel{\&OverBar;}{d}\&times;{\stackrel{\&OverBar;}{a}}_r}}{a_r}& {\stackrel{\&OverBar;}{v}}_r<0,{\stackrel{\&OverBar;}{a}}_r\&NotEqual;0\\ -\frac{{\stackrel{\&OverBar;}{v}}_r}{{\stackrel{\&OverBar;}{a}}_r}+\frac{\sqrt{{\stackrel{\&OverBar;}{v}}_r^2-2\stackrel{\&OverBar;}{d}\&times;{\stackrel{\&OverBar;}{a}}_r}}{{\stackrel{\&OverBar;}{a}}_r}& {\stackrel{\&OverBar;}{v}}_r\&GreaterEqual;0,{\stackrel{\&OverBar;}{a}}_r<0\end{array}\right.---\left(5\right)$

将利用交互多模型方法所估计出的值代入（5）即可实时计算出TTC值，且当或(且)时式（5）无解，此时前车车速与加速度都大于自车，显见车辆处于安全状态，因此此时无需计算TTC值。

Claims (1)

1.一种高速公路汽车追尾碰撞预警关键参数估计方法，其特征在于：本方法根据高速公路车辆不同的相对运动典型工况建立多个不同的卡尔曼滤波系统状态模型，以全球定位系统与车车通信信息结合雷达信息作为观测量，并在运行过程中检测、容忍传感器信息的不准确以及失效，利用交互多模型算法，实现不同典型运动工况下的卡尔曼滤波模型之间的交互，即使在某些传感器失效时，也能实时、准确、可靠的获取两车相对距离、速度与加速度高速公路追尾碰撞预警关键参数，同时结合相对加速度信息，来改进传统距离碰撞时间TTC估计方法，从而准确、可靠地估计出TTC这个高速公路追尾碰撞预警关键参数；

具体步骤包括：

1)建立高速公路汽车相对运动的状态模型

针对高速公路汽车行驶工况，建立7个不同的卡尔曼滤波模型，这7个模型具有相同的状态模型形式，其区别仅在于状态转移矩阵的不同，其状态方程可统一表示，第j(j＝1,2,3…7)个模型离散化后的卡尔曼滤波的状态方程的矩阵形式表示为：

X_j(k)＝A_j(k,k-1)X_j(k-1)+W_j(k-1)(j＝1,2...7)   (1)

式中，k表示离散化时刻，下标j(j＝1，2…7)表示第j个模型，多模型设置为7个模型，这7个模型有相同的状态向量，该状态向量X_j＝[d v_L a_L v_F a_F]′，上角标'表示对矩阵转置，d表示前车距自车的距离[米]，V_L表示前车速度[米/秒]，a_L表示前车加速度[米/(秒×秒)]，V_F表示自车速度[米/秒]，a_F表示自车加速度[米/(秒×秒)]；W_j表示零均值的系统高斯白噪声向量且W_j＝[w₁ w₂ w₃ w₄ w₅]′，其中w₁、w₂、w₃、w₄及w₅分别表示五个系统高斯白噪声分量，其对应的系统噪声协方差阵Q_j为：

$Q_j=\left[\begin{array}{ccccc}{{\mathrm{\&sigma;}}_{w_1}}^2& 0& 0& 0& 0\\ 0& {{\mathrm{\&sigma;}}_{w_2}}^2& 0& 0& 0\\ 0& 0& {{\mathrm{\&sigma;}}_{w_3}}^2& 0& 0\\ 0& 0& 0& {{\mathrm{\&sigma;}}_{w_4}}^2& 0\\ 0& 0& 0& 0& {{\mathrm{\&sigma;}}_{w_5}}^2\end{array}\right],$ 其中及分别表示系统高斯白噪声w₁、w₂、w₃、w₄及w₅对应的方差；A_j表示状态转移矩阵，这7个模型具有不同的状态转移矩阵，其具体设置如下：

针对高速公路车辆运行及发生追尾碰撞的典型场景和工况进行分析，为简化模型，假定自车与前车行驶在同一车道上，定义工况1为安全工况：自车以等于前车的速度匀速行驶，工况2-7分别为常见的危险工况，

工况2：前车静止，自车匀速行驶；

工况3：前车静止，自车加速行驶；

工况4：前车匀速行驶，自车以大于前车速度的车速匀速行驶；

工况5：前车匀速行驶，自车加速行驶；

工况6：前车减速行驶，自车匀速行驶；

工况7：前车减速行驶，自车加速行驶；

这7种典型工况其所对应的状态转移矩阵A₁-A₇分别为：

$A_1=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0\end{array}\right],$ $A_2=\left[\begin{array}{ccccc}1& 0& 0& -T& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0\end{array}\right],$

$A_3=\left[\begin{array}{ccccc}1& 0& 0& -T& -0.5T^2\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& T\\ 0& 0& 0& 0& 1\end{array}\right],$ $A_4=\left[\begin{array}{ccccc}1& T& 0& -T& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0\end{array}\right],$ $A_5=\left[\begin{array}{ccccc}1& T& 0& -T& -0.5T^2\\ 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 1& T\\ 0& 0& 0& 0& 1\end{array}\right],$

$A_6=\left[\begin{array}{ccccc}1& T& 0.5T^2& -T& 0\\ 0& 1& T& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0\end{array}\right],$ $A_7=\left[\begin{array}{ccccc}1& T& 0.5T^2& -T& -0.5T^2\\ 0& 1& T& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 1& T\\ 0& 0& 0& 0& 1\end{array}\right]$

T表示离散的周期，根据测量传感器特性，T的典型值可取为50毫秒、100毫秒、200毫秒；

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

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

Z_j(k)＝H_j(k)X_j(k)+V_j(k)(j＝1,2...7)   (2)

其中Z_j为观测向量，H_j为观测矩阵，V_j是与W_j互不相关的零均值观测噪声向量，其协方差矩阵为R_j；

采用毫米波雷达、GPS(Global Positioning System，全球定位系统)、加速度计，结合V2V(vehicle to vehicle，车车通信)用以提供观测信息；其中，毫米波雷达用以测量两车相对距离与相对速度；GPS与加速度计用以提供自车位置、车速与加速度信息，V2V作为虚拟传感器，提供前车的位置、车速与加速度信息，GPS-V2V信息较为全面准确，但GPS信号受到遮挡或V2V因受到干扰而屏蔽而无法传输信息时，甚至可能前车并未安装V2V，都会导致无法提供有效的信息；

V2V发生故障或失效的分为两类，一类是通信故障，表现为无任何前车信息信号，另一类是由于前车的GPS传感器失效所引起的，表现为所传送的前车信息不完整，不包含前车的GPS传感器信息；自车GPS是否发生故障或失效则可利用HDOP(HorizontalDilution Of Precision，水平精度因子)值结合卫星数来判别，在HDOP<5.0时且卫星数大于等于4个，判定为自车GPS能够正常工作，在HDOP>5.0时或卫星数小于4个时，判定为自车GPS失效；

基于这样的考虑，结合GPS-V2V与雷达的优缺点，当所有传感器都工作正常时，由于GPS-V2V信息全面而准确，以GPS、加速度计及V2V信息作为观测量；当车车通信由于通信故障而失效从而无法感知前车信息，但自车GPS能正常工作时，以GPS、加速度计及雷达信息作为观测量；当车车通信由于前车GPS失效从而无法获取前车GPS信息，但自车GPS能正常工作时，以GPS、加速度计、雷达信息以及前车加速度作为观测量；当自车GPS失效，但车车通信有效且能获取全面的前车信息时，以加速度计、雷达及车车通信信息作为观测量；当自车及前车GPS失效但车车通信能够获取前车加速度计信息时，以自车与前车加速度计信息和雷达信息作为观测量；当自车GPS失效时且车车通信无法获取前车信息时，以加速度计及雷达信息作为观测量；

即，Z_j、H_j与R_j都根据传感器是否失效进行切换：

①自车GPS有效且存在全面的车车通信信息，

$Z_j=\left[\begin{array}{c}{d}_{GPS-V2V}\\ v_{L-V2V}\\ v_{F-GPS}\\ a_{L-V2V}\\ a_{F-ACC}\end{array}\right],$ $H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\end{array}\right],$ d_GPS-V2V为由GPS及V2V所观测的自车与前车位置信息所计算出的车间距离[米]，v_L-V2V为V2V所传输的前车的速度[米/秒]，a_L-V2V为V2V所传输的前车加速度[米/(秒×秒)]，v_F-GPS为GPS所观测的自车速度[米/秒]，a_F-ACC为加速度计所观测的自车加速度[米/(秒×秒)]，此时，

$V_j=\left[\begin{array}{c}{n}_{d_{GPS-V2V}}\\ n_{v_{L-V2V}}\\ n_{v_{F-GPS}}\\ n_{a_{L-V2V}}\\ n_{a_{F-ACC}}\end{array}\right],$ 表示通过GPS及V2V所观测的自车与前车位置信息所计算出的车间距离的观测噪声且是均值为0、方差为的高斯白噪声，表示通过V2V所传输的前车的速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过GPS所观测的自车速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过V2V所传输的前车加速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过加速度计所观测的自车加速度的观测噪声且是均值为0、方差为的高斯白噪声；V_j对应的观测噪声方差阵R_j可表示为

$R_j=\left[\begin{array}{ccccc}{\mathrm{\&sigma;}}_{d_{GPS-V2V}}^2& 0& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{L-V2V}}^2& 0& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{v_{F-GPS}}^2& 0& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{a_{L-V2V}}^2& 0\\ 0& 0& 0& 0& {\mathrm{\&sigma;}}_{a_{F-ACC}}^2\end{array}\right];$

②自车GPS有效但不存在车车通信信息，

$Z_j=\left[\begin{array}{c}{d}_{radar}\\ v_{F-GPS}\\ a_{F-ACC}\\ v_{r-radar}\end{array}\right],$ $H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\\ 0& 1& 0& -1& 0\end{array}\right],$ d_radar为由雷达所观测的车间距离[米]，v_r-radar为雷达所观测的前车相对于自车的速度[米/秒]， $V_j=\left[\begin{array}{c}{n}_{d_{radar}}\\ n_{v_{F-GPS}}\\ n_{a_{F-ACC}}\\ n_{v_{r-radar}}\end{array}\right],$ 表示通过雷达传感器测量获得的车间距离的观测噪声且是均值为0、方差为的高斯白噪声，表示通过雷达所观测的前车相对于自车的速度的观测噪声且是均值为0、方差为的高斯白噪声，V_j对应的观测噪声方差阵R_j可表示为 $R_j=\left[\begin{array}{cccc}{\mathrm{\&sigma;}}_{d_{radar}}^2& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{F-GPS}}^2& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{a_{F-ACC}}^2& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{v_{r-radar}}^2\end{array}\right];$

③自车GPS有效且存在车车通信信息，但因前车GPS失效，车车通信信息里不包括前车GPS信息，

$Z_j=\left[\begin{array}{c}{d}_{radar}\\ v_{F-GPS}\\ a_{F-ACC}\\ v_{r-radar}\\ a_{L-V2V}\end{array}\right],$ $H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\\ 0& 1& 0& -1& 0\\ 0& 0& 1& 0& 0\end{array}\right],$ V_j对应的观测噪声方差阵R_j可表示为

$R_j=\left[\begin{array}{ccccc}{\mathrm{\&sigma;}}_{d_{radar}}^2& 0& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{F-GPS}}^2& 0& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{a_{F-ACC}}^2& 0& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{v_{r-radar}}^2& 0\\ 0& 0& 0& 0& {\mathrm{\&sigma;}}_{a_{L-V2V}}^2\end{array}\right];$

④自车GPS失效但存在全面的车车通信信息

$Z_j=\left[\begin{array}{c}{d}_{radar}\\ v_{r-radar}\\ v_{L-V2V}\\ a_{L-V2V}\\ a_{F-ACC}\end{array}\right],$ $H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& -1& 0\\ 0& 1& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\end{array}\right],$ V_j对应的观测噪声方差阵R_j可表示为

$R_j=\left[\begin{array}{ccccc}{\mathrm{\&sigma;}}_{d_{radar}}^2& 0& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{r-radar}}^2& 0& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{v_{L-V2V}}^2& 0& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{a_{L-V2V}}^2& 0\\ 0& 0& 0& 0& {\mathrm{\&sigma;}}_{a_{F-ACC}}^2\end{array}\right];$

⑤自车与前车GPS都失效，但存在车车通信信息， 能够接收到前车的加速度信息

$Z_j=\left[\begin{array}{c}{d}_{radar}\\ a_{L-V2V}\\ a_{F-ACC}\\ v_{r-radar}\end{array}\right],$ $H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1\\ 0& 1& 0& -1& 0\end{array}\right],$ V_j对应的观测噪声方差阵R_j可表示为

$R_j=\left[\begin{array}{cccc}{\mathrm{\&sigma;}}_{d_{radar}}^2& 0& 0& 0\\ 0& {\mathrm{\&sigma;}}_{a_{L-V2V}}^2& 0& 0\\ 0& 0& {\mathrm{\&sigma;}}_{a_{F-ACC}}^2& 0\\ 0& 0& 0& {\mathrm{\&sigma;}}_{v_{r-radar}}^2\end{array}\right];$

⑥自车GPS失效且不存在车车通信信息，

$Z_j=\left[\begin{array}{c}{d}_{radar}\\ v_{r-radar}\\ a_{F-ACC}\end{array}\right],$ $H_j=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 0& -1& 0\\ 0& 0& 0& 0& 1\end{array}\right],$ V_j对应的观测噪声方差阵R_j可表示为

$R_j=\left[\begin{array}{ccc}{\mathrm{\&sigma;}}_{d_{radar}}^2& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_{r-radar}}^2& 0\\ 0& 0& {\mathrm{\&sigma;}}_{a_{F-ACC}}^2\end{array}\right];$

3)交互多模型估计车辆间相对距离、车速与加速度

根据式(1)和式(2)，分别建立7个不同的卡尔曼滤波模型，系统模型之间的转移概率为p_ij，下标i、j(i＝1,2,3…7,j＝1,2,3…7)表示从状态i转移到状态j的概率；

①交互估计计算

预测第j(j＝1,2,3…7)个模型的模型概率ρ_j(k,k-1):

${\mathrm{\&rho;}}_j(k,k-1)=\underset{i=1}{\overset{7}{\&Sigma;}}{p}_{ij}{\mathrm{\&rho;}}_i(k-1)$

预测混合概率ρ_i|j(k-1):

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

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

$X_{0j}(k-1)=\underset{i=1}{\overset{7}{\&Sigma;}}{X}_i\left(k-1\right){\mathrm{\&rho;}}_{i|j}\left(k-1\right)$

$P_{0j}(k-1)=\underset{i=1}{\overset{7}{\&Sigma;}}{\mathrm{\&rho;}}_{i|j}(k-1)\left\{P_i\right(k-1)+\&lsqb;X_i(k-1)-X_{0j}(k-1)\&rsqb;{\&lsqb;X_i\left(k-1\right)-X_{0j}\left(k-1\right)\&rsqb;}^{\&prime;}\}$

其中P_i(k-1)表示第i个模型在k-1时刻的估计误差方差阵；

②每个卡尔曼滤波器各自进行卡尔曼滤波递推，第j(j＝1,2,3…7)个模型的滤波过程如下：

时间更新：

状态一步预测方程:X_j(k,k-1)＝A_jX_0j(k-1)

一步预测误差方差阵：P_j(k,k-1)＝A_j(k-1)P_0j(k-1)(A_j(k-1))′+Q_j(k-1)

测量更新：

滤波增益矩阵：K_j(k)＝P_j(k,k-1)(H_j(k-1))′(S_j(k))^-1

S_j(k)＝H_j(k-1)P_j(k,k-1)(H_j(k-1))′+R_j(k-1)

状态估计：X_j(k)＝X_j(k,k-1)+K_j(k)(Z_j(k)-H_jX_j(k,k-1))

估计误差方差阵P_j(k)：P_j(k)＝P_j(k,k-1)-K_j(k)S_j(k)(K_j(k))′

③模型概率更新

在每个模型完成上一步的更新之后，利用最大似然函数Λ_j(k)计算新的模型概率ρ_j(k)；最大似然函数计算如下：

${\mathrm{\&Lambda;}}_j\left(k\right)=\frac{\exp \{-\frac{1}{2}{\left(Z_j\left(k\right)-H_j{X}_j\left(k,k-1\right)\right)}^{\&prime;}{\left(S_j\left(k\right)\right)}^{-1}\left(Z_j\left(k\right)-H_j{X}_j\left(k,k-1\right)\right)\}}{|2{\mathrm{\&pi;S}}_j\left(k\right){|}^{-\frac{1}{2}}}$

因此，模型j(j＝1,2,3…7)在k时刻的模型概率由贝叶斯定理给出：

${\mathrm{\&rho;}}_j\left(k\right)=\frac{{\mathrm{\&Lambda;}}_j\left(k\right){\mathrm{\&rho;}}_j(k,k-1)}{\underset{i=1}{\overset{7}{\&Sigma;}}{\mathrm{\&Lambda;}}_i\left(k\right){\mathrm{\&rho;}}_i(k,k-1)}$

④估计组合

在计算出各模型为正确的后验概率之后，对所有滤波器的状态估计进行概率加权并求和，权系数为模型正确的后验概率，得到估计组合后的状态估计为： $X\left(k\right)=\underset{j=1}{\overset{7}{\&Sigma;}}{X}_j\left(k\right){\mathrm{\&rho;}}_j\left(k\right),$ 其中， $X\left(k\right)={\left[\begin{array}{ccccc}\stackrel{\&OverBar;}{d}& {\stackrel{\&OverBar;}{v}}_L& {\stackrel{\&OverBar;}{a}}_L& {\stackrel{\&OverBar;}{v}}_F& {\stackrel{\&OverBar;}{a}}_F\end{array}\right]}^{\&prime;},$ 各状态变量的上标“-”表示各状态量的最终滤波估计值，即X(k)内各状态变量依次分别表示估计组合后的车距、前车速度、前车加速度、自车速度、自车加速度，据此即可推算出估计组合后的两车相对距离、速度和加速度，即估计组合后的车辆间相对距离[米]可直接估计得出，估计组合后前车相对于自车的速度[米/秒]估计组合后前车相对于自车的加速度 ${\stackrel{\&OverBar;}{a}}_r={\stackrel{\&OverBar;}{a}}_L-{\stackrel{\&OverBar;}{a}}_F;$

4)改进TTC估计

传统的TTC计算方法如式(3)所示：

$TTC=-\frac{\stackrel{\&OverBar;}{d}}{{\stackrel{\&OverBar;}{v}}_r}---\left(3\right)$

式(3)中，为估计组合后前车相对于自车的速度[米/秒]，且该方法假设车辆间相对加速度为零，存在较大的误差；在此基础上对其进行改进，考虑车辆间相对加速度信息，有

$\stackrel{\&OverBar;}{d}+{\stackrel{\&OverBar;}{v}}_r\&times;TTC+0.5{\stackrel{\&OverBar;}{a}}_r\&times;{\mathrm{TTC}}^2=0---\left(4\right)$

式(4)中，为估计组合后前车相对于自车的加速度，且解式(4)可得：

$TTC=\left\{\begin{array}{cc}-\frac{\stackrel{\&OverBar;}{d}}{{\stackrel{\&OverBar;}{v}}_r}& {\stackrel{\&OverBar;}{v}}_r<0,{\stackrel{\&OverBar;}{a}}_r=0\\ -\frac{{\stackrel{\&OverBar;}{v}}_r}{{\stackrel{\&OverBar;}{a}}_r}-\frac{\sqrt{{\stackrel{\&OverBar;}{v}}_r^2-2\stackrel{\&OverBar;}{d}\&times;{\stackrel{\&OverBar;}{a}}_r}}{a_r}& {\stackrel{\&OverBar;}{v}}_r<0,{\stackrel{\&OverBar;}{a}}_r\&NotEqual;0\\ -\frac{{\stackrel{\&OverBar;}{v}}_r}{{\stackrel{\&OverBar;}{a}}_r}+\frac{\sqrt{{\stackrel{\&OverBar;}{v}}_r^2-2\stackrel{\&OverBar;}{d}\&times;{\stackrel{\&OverBar;}{a}}_r}}{{\stackrel{\&OverBar;}{a}}_r}& {\stackrel{\&OverBar;}{v}}_r\&GreaterEqual;0,{\stackrel{\&OverBar;}{a}}_r<0\end{array}\right.---\left(5\right)$

将利用交互多模型方法所估计出的值代入(5)即可实时计算出TTC值，且当或(且)时式(5)无解，此时前车车速与加速度都大于自车，显然车辆处于安全状态，因此此时无需计算TTC值。