[发明专利]一种汽车运动状态的纵/横向多模型估计方法

权利要求书

1.一种汽车运动状态的纵/横向多模型估计方法，其特征在于：本方法针对汽车运行过程纵、横向运动状态耦合的特点，根据汽车典型行驶工况的运动学特征，将汽车运行状态解耦为3个纵向运动学模型和2个横向运动学模型，形成5个扩展卡尔曼滤波系统状态模型，以北斗卫星定位系统、电子罗盘等车载传感器结合增强型电子地图等获取的信息作为观测量，利用交互多模型算法，准确判别当前汽车的行驶工况，进而实时、准确、可靠的获取汽车位置、速度、航向角等运动状态信息；具体步骤包括：

1)建立汽车纵横向运动的模型集

针对汽车典型行驶工况的运动学特征，建立3个纵向运动学模型和2个横向运动学模型，这5个模型具有相同的状态模型形式，其区别仅仅在于系统状态函数的不同，其状态方程可统一表示，第j(j＝1,2,3,4,5)个模型离散化后的扩展卡尔曼滤波(Extended KalmanFilter，EKF)的状态方程的矩阵形式表示为：

X_j(k)＝f_j(X_j(k-1),W_j(k-1)) (1)

这5个模型有相同的状态向量，该状态向量X_j＝[x₁ x₂ x₃ x₄ x₅ x₆ x₇ x₈]′，且x₁＝x，x₂＝y，x₃＝φ，x₄＝v，x₅＝ω，x₆＝a，x₇＝c₀，x₈＝c₁，即X_j＝[x y φ v ω a c₀ c₁]′，本发明中上角标'表示对矩阵转置，k表示离散化时刻；x，y分别表示车辆的东向和北向位置，φ表示车辆航向角(即车辆速度与车辆行进方向的夹角)，v表示车体坐标系下车辆的速度，ω表示车体坐标系下车辆的横摆角速度，a表示车体坐标系下车辆的纵向加速度，c₀和c₁分别表示利用单回旋模型表示的道路曲率参数，其中，c₀为车辆当前所在点的道路曲率，c₁为道路曲率的变化系数；W_j表示零均值的系统高斯白噪声向量且W_j＝[w₁ w₂ w₃ w₄ w₅ w₆ w₇ w₈]′，其中w₁、w₂、w₃、w₄、w₅、w₆、w₇及w₈分别表示八个系统高斯白噪声分量，其对应的系统噪声协方差阵Q_j为：

其中分别表示系统高斯白噪声w₁、w₂、w₃、w₄、w₅、w₆、w₇及w₈对应的方差；f_j表示系统状态函数，这5个模型具有不同的系统状态函数向量，其具体设置如下：

工况1：纵向模型1，汽车加/减速行驶；

工况2：纵向模型2，汽车匀速行驶；

工况3：纵向模型3，汽车静止；

工况4：横向模型1，汽车变道；

工况5：横向模型2，汽车保持车道；

这5种典型工况其所对应的非线性系统状态函数f_j(j＝1,2,3,4,5)分别为：

其中，

f₁³(X₁(k-1),W₁(k-1))＝φ(k-1)+ω(k-1)T，

f₁⁴(X₁(k-1),W₁(k-1))＝v(k-1)+a(k-1)T，

f₁⁵(X₁(k-1),W₁(k-1))＝ω(k-1)，f₁⁶(X₁(k-1),W₁(k-1))＝a(k-1)，

f₁⁷(X₁(k-1),W₁(k-1))＝0，f₁⁸(X₁(k-1),W₁(k-1))＝0；

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

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

第j(j＝1,2,3,4,5)个滤波器的观测方程为：

Z_j(k)＝H_j(k)X_j(k)+V_j(k) (2)

式(2)中，Z_j为观测向量，H_j为观测阵，V_j表示与W_j互不相关的零均值观测白噪声向量，且

其中，x_m(k)，y_m(k)分别表示由北斗卫星定位系统获取的车辆东向和北向位置，φ_m(k)表示由电子罗盘获取的车辆航向角，v_m(k)表示由里程计获取的车辆纵向车速，ω_m(k)表示由横摆角速度陀螺仪获取的车辆横摆角速度，a_m(k)表示由纵向加速度计获取的车辆纵向加速度，c_{0_m}(k)和c_{1_m}(k)分别表示由增强型电子地图获取的车辆当前所在点的道路曲率和道路曲率的变化系数，表示通过北斗卫星定位系统获取的车辆东向位置的观测噪声且是均值为0、方差为的高斯白噪声，表示通过北斗卫星定位系统获取的车辆北向位置的观测噪声且是均值为0、方差为的高斯白噪声，表示通过电子罗盘获取的车辆航向角的观测噪声且是均值为0、方差为的高斯白噪声，表示通过里程计获取的车辆纵向车速的观测噪声且是均值为0、方差为的高斯白噪声，表示通过横摆角速度陀螺仪获取的车辆横摆角速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过纵向加速度计获取的车辆纵向加速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过增强型电子地图获取的车辆当前所在点的道路曲率的观测噪声且是均值为0、方差为的高斯白噪声，表示通过增强型电子地图获取的道路曲率的变化系数的观测噪声且是均值为0、方差为的高斯白噪声；V_j对应的观测噪声方差阵R_j可表示为

3)交互多模型估计方法

①交互估计计算

上述五个扩展卡尔曼系统模型之间的转移概率为p_ij，下标i、j(i＝1,2…5,j＝1,2,…5)表示从状态i转移到状态j的概率；

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

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

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

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

②模型条件滤波

每个模型滤波器对于式(1)和式(2)所描述的状态方程和观测方程，运用扩展卡尔曼滤波理论，各自进行标准扩展卡尔曼滤波递推，该递推过程包括时间更新和测量更新，第j个模型的滤波过程如下：

时间更新：

状态一步预测方程X_j(k,k-1)＝f_j(X_0j(k-1),0)

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

其中，A是系统状态函数向量f_j对状态向量X_j求偏导数的雅可比矩阵，即矩阵A_j的第s行第t列元素可通过下式求得：

测量更新：

滤波增益矩阵：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,k-1)-K_j(k)S_j(k)(K_j(k))′

③模型概率更新

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

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

④估计组合

在计算出各模型为正确的后验概率之后，对所有滤波器的状态估计进行概率加权并求和，权系数为模型正确的后验概率，得到最终的状态估计为：其中，各状态变量的上标“-”表示各状态量的最终滤波估计值，即X(k)内各状态变量依次分别表示估计组合后的东向位置、北向位置、航向角、纵向车速、横摆角速度、纵向加速度、车辆当前所在点的道路曲率、道路曲率的变化系数。