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

摘要

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

主权项

一种高速公路汽车追尾碰撞预警关键参数估计方法，其特征在于：本方法根据高速公路车辆不同的相对运动典型工况建立多个不同的卡尔曼滤波系统状态模型，以全球定位系统与车车通信信息结合雷达信息作为观测量，并在运行过程中检测、容忍传感器信息的不准确以及失效，利用交互多模型算法，实现不同典型运动工况下的卡尔曼滤波模型之间的交互，即使在某些传感器失效时，也能实时、准确、可靠的获取两车相对距离、速度与加速度高速公路追尾碰撞预警关键参数，同时结合相对加速度信息，来改进传统距离碰撞时间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}{{\sigma }_{w_1}}^2& 0& 0& 0& 0\\ 0& {{\sigma }_{w_2}}^2& 0& 0& 0\\ 0& 0& {{\sigma }_{w_3}}^2& 0& 0\\ 0& 0& 0& {{\sigma }_{w_4}}^2& 0\\ 0& 0& 0& 0& {{\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}_{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}{\sigma }_{d_{GPS-V2V}}^2& 0& 0& 0& 0\\ 0& {\sigma }_{v_{L-V2V}}^2& 0& 0& 0\\ 0& 0& {\sigma }_{v_{F-GPS}}^2& 0& 0\\ 0& 0& 0& {\sigma }_{a_{L-V2V}}^2& 0\\ 0& 0& 0& 0& {\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}{\sigma }_{d_{radar}}^2& 0& 0& 0\\ 0& {\sigma }_{v_{F-GPS}}^2& 0& 0\\ 0& 0& {\sigma }_{a_{F-ACC}}^2& 0\\ 0& 0& 0& {\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}{\sigma }_{d_{radar}}^2& 0& 0& 0& 0\\ 0& {\sigma }_{v_{F-GPS}}^2& 0& 0& 0\\ 0& 0& {\sigma }_{a_{F-ACC}}^2& 0& 0\\ 0& 0& 0& {\sigma }_{v_{r-radar}}^2& 0\\ 0& 0& 0& 0& {\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}{\sigma }_{d_{radar}}^2& 0& 0& 0& 0\\ 0& {\sigma }_{v_{r-radar}}^2& 0& 0& 0\\ 0& 0& {\sigma }_{v_{L-V2V}}^2& 0& 0\\ 0& 0& 0& {\sigma }_{a_{L-V2V}}^2& 0\\ 0& 0& 0& 0& {\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}{\sigma }_{d_{radar}}^2& 0& 0& 0\\ 0& {\sigma }_{a_{L-V2V}}^2& 0& 0\\ 0& 0& {\sigma }_{a_{F-ACC}}^2& 0\\ 0& 0& 0& {\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}{\sigma }_{d_{radar}}^2& 0& 0\\ 0& {\sigma }_{v_{r-radar}}^2& 0\\ 0& 0& {\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):${\rho }_j(k,k-1)=\underset{i=1}{\overset{7}{\Sigma }}{p}_{ij}{\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){\rho }_{i|j}\left(k-1\right)$$P_{0j}(k-1)=\underset{i=1}{\overset{7}{\Sigma }}{\rho }_{i|j}(k-1)\left\{P_i\right(k-1)+[X_i\left(k-1\right)-X_{0j}\left(k-1\right)\left]{[X_i\left(k-1\right)-X_{0j}\left(k-1\right)]}^{\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))^‑1S_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)；最大似然函数计算如下：${\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时刻的模型概率由贝叶斯定理给出：${\rho }_j\left(k\right)=\frac{{\Lambda }_j\left(k\right){\rho }_j(k,k-1)}{\underset{i=1}{\overset{7}{\Sigma }}{\Lambda }_i\left(k\right){\rho }_i(k,k-1)}$④估计组合在计算出各模型为正确的后验概率之后，对所有滤波器的状态估计进行概率加权并求和，权系数为模型正确的后验概率，得到估计组合后的状态估计为：$X\left(k\right)=\underset{j=1}{\overset{7}{\Sigma }}{X}_j\left(k\right){\rho }_j\left(k\right),$其中，$X\left(k\right)={\left[\begin{array}{ccccc}\bar{d}& {\bar{v}}_L& {\bar{a}}_L& {\bar{v}}_F& {\bar{a}}_F\end{array}\right]}^{\prime },$各状态变量的上标“‑”表示各状态量的最终滤波估计值，即X(k)内各状态变量依次分别表示估计组合后的车距、前车速度、前车加速度、自车速度、自车加速度，据此即可推算出估计组合后的两车相对距离、速度和加速度，即估计组合后的车辆间相对距离[米]可直接估计得出，估计组合后前车相对于自车的速度[米/秒]估计组合后前车相对于自车的加速度${\bar{a}}_r={\bar{a}}_L-{\bar{a}}_F;$4)改进TTC估计传统的TTC计算方法如式(3)所示：$TTC=-\frac{\bar{d}}{{\bar{v}}_r}---\left(3\right)$式(3)中，为估计组合后前车相对于自车的速度[米/秒]，且该方法假设车辆间相对加速度为零，存在较大的误差；在此基础上对其进行改进，考虑车辆间相对加速度信息，有$\bar{d}+{\bar{v}}_r\times TTC+0.5{\bar{a}}_r\times {\mathrm{TTC}}^2=0---\left(4\right)$式(4)中，为估计组合后前车相对于自车的加速度，且解式(4)可得：$TTC=\left\{\begin{array}{cc}-\frac{\bar{d}}{{\bar{v}}_r}& {\bar{v}}_r<0,{\bar{a}}_r=0\\ -\frac{{\bar{v}}_r}{{\bar{a}}_r}-\frac{\sqrt{{\bar{v}}_r^2-2\bar{d}\times {\bar{a}}_r}}{a_r}& {\bar{v}}_r<0,{\bar{a}}_r\ne 0\\ -\frac{{\bar{v}}_r}{{\bar{a}}_r}+\frac{\sqrt{{\bar{v}}_r^2-2\bar{d}\times {\bar{a}}_r}}{{\bar{a}}_r}& {\bar{v}}_r\ge 0,{\bar{a}}_r<0\end{array}\right.---\left(5\right)$将利用交互多模型方法所估计出的值代入(5)即可实时计算出TTC值，且当或(且)时式(5)无解，此时前车车速与加速度都大于自车，显然车辆处于安全状态，因此此时无需计算TTC值。