一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法

摘要

一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法，利用改进的扩展卡尔曼滤波算法对汽车动力学过程进行适当的建模，获得汽车在较高机动运行状况下的纵向前进速度、横摆角速度、侧向速度以及质心侧偏角等运行状态信息，这些信息可用于汽车主动安全的相关控制，本发明方法具有精度高、成本低、实时性好等特点。

说明书

技术领域

本发明涉及一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法，其目的 在于利用改进的扩展卡尔曼滤波方法对汽车动力学过程进行适当的建模，获得汽 车在较高机动运行状况下的车辆运行状态，这些状态可用于汽车主动安全的相关 控制，具有精度高、成本低、实时性好等显著优点，属于汽车主动安全测量及控 制领域。

背景技术

随着社会经济的发展，道路交通安全问题日益突出，并已成为全球性难题。 全世界每年因交通事故都会造成大量的人员伤亡和财产损失，世界各国都在努力 降低交通事故的发生。近年来，汽车主动安全技术得到了迅速的发展。汽车主动 安全技术能够防患于未然，主动避免事故的发生，已成为现代汽车最主要的发展 方向之一。目前常见的主动安全技术主要包括防抱死制动系统(ABS)，车辆电 子稳定程序(ESP)，牵引力控制系统(TCS)，电控驱动防滑系统(ASR)，四轮 转向稳定控制系统(4WS)等。这些系统通常涉及汽车轮胎的速度、汽车的纵向 前进速度、侧向速度、横摆角速度以及质心侧偏角等运行状态的测量或估计，而 这些运行状态的测量可用于后续的汽车主动安全控制，因此其精度直接关系汽车 的行驶安全性与稳定性，即上述主动安全控制系统能否有效工作在很大程度上依 赖于车辆运行状态能否被实时、准确的测量或估计。

目前，在汽车主动安全领域，车辆运动状态主要通过三种方法来测量或估计。 一是利用低成本的车载传感器(如惯性传感器和轮速传感器等)，对其测量的信 号进行简单的数学推算来获取有关车辆运行状态，这种方法成本低，但由于低成 本传感器精度较差且推算处理过于简单而存在较大的测量误差，因而影响了控制 效果。二是利用高精度的传感器对有关车辆运行状态进行直接测量(如利用光电 五轮仪或高精度的全球导航卫星系统GNSS，尤其是高精度全球定位系统GPS 等)，这种方法精度高但价格昂贵，无法大范围推广应用。第三种方法是模型法， 即通过对汽车的运行过程进行运动学或动力学建模，同时将有关低成本的车载传 感器(如轮速传感器、陀螺仪、加速度计以及GPS等)信息作为观测信息，进 而利用适当的滤波估计算法实现对汽车运行状态的估计。第三种方法(即模型法) 可实现对难于直测量的估计，扩大状态估计的维数，还可提高有关直测量的精度， 同时成本较低。但目前已提出的模型法主要是基于汽车的运动学模型或者对整车 或轮胎做了较多线性化假定的动力学模型，这些模型在车辆较平稳运行时能获得 较好的估计效果和精度，但在较高机动运行状况下由于难于反映车辆的实际非线 性动力学行为导致估计精度较低。

发明内容

为在较高机动工况下实现对车辆运行状态的准确、可靠估计，本发明提出了 一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法。本发明提出的方法针对 汽车的较高机动运行状况来确定更接近实际的非线性整车动力学模型和轮胎纵 向力模型，同时充分利用低成本的车载轮速和方向盘转角传感器信息来建立滤波 系统的外部输入量和观测量，进而通过改进的扩展卡尔曼滤波递推算法实现对汽 车纵向前进速度、横摆角速度、侧向速度以及质心侧偏角等车辆运行状态的滤波 估计，具有精度高、成本低、实时性好等特点。

一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法，本发明针对目前应 用较多的前轮转向四轮汽车，为满足汽车主动安全控制对车辆运行状态的测量与 估计需要，建立适用于较高机动运行工况的非线性整车动力学模型和轮胎纵向力 模型，同时充分利用低成本的车载轮速和方向盘转角传感器信息来确定建立滤波 系统的外部输入量和观测量，在此基础上，通过提出的改进扩展卡尔曼滤波递推 算法来实现对汽车纵向前进速度、横摆角速度、侧向速度以及质心侧偏角等信息 的准确滤波估计；

具体步骤包括：

1)建立扩展卡尔曼滤波的状态方程和观测方程：

建立三自由度的汽车非线性动力学模型，即建立扩展卡尔曼滤波过程的系统 状态方程，离散化后的卡尔曼滤波的状态方程的矩阵形式表示为：

X(k)＝f(X(k-1)，U(k-1)，W(k-1)，γ(k-1))                    (1)

式中，k表示离散化时刻；

系统状态向量为X＝[x₁ x₂ x₃]′且x₁＝v_x，x₂＝ω_z，x₃＝v_y，即 X＝[v_x ω_z v_y]′，v_x、v_y及ω_z分别是汽车的纵向前进速度、侧向速度和横摆角 速度，本发明中上角标′表示对矩阵转置；

系统外输入向量为U＝[u₁ u₂ u₃]′且u₁＝δ_f，u₂＝F_tf，u₃＝F_tr，即 U＝[δ_f  F_tf F_tr]′，δ_f是前轮转向角，F_tf是作用在单个前轮上的纵向力，F_tr是作 用在单个后轮上的纵向力；

W(k-1)表示零均值的系统高斯白噪声向量且W＝[w₁ w₂ w₃]′，其中w₁、 w₂及w₃分别表示三个系统高斯白噪声分量；

γ(k-1)表示系统外输入对应的零均值高斯白噪声向量且 $\gamma ={\left(\begin{array}{ccc}{w}_{{\delta }_f}& w_{F_{\mathrm{tf}}}& w_{F_{\mathrm{tr}}}\end{array}\right)}^{\prime },$其中及分别表示直测或估计的系统外输入δ_f、 F_tf及F_tr对应的零均值高斯白噪声，这些白噪声隐含在系统状态函数f的三个系 统外输入里面；非线性的系统状态函数向量为

$f(X,U,W,\gamma )=\left(\begin{array}{c}{f}_1(X(k-1),U(k-1),W(k-1),\gamma (k-1))\\ f_2(X(k-1),U(k-1),W(k-1),\gamma (k-1))\\ f_3(X(k-1),U(k-1),W(k-1),\gamma (k-1))\end{array}\right),$

其中，

$f_1(X(k-1),U(k-1),W(k-1),\gamma (k-1))=v_x(k-1)+\frac{T}{m}[m{v}_y(k-1){\omega }_z(k-1)+$

$2C_{\mathrm{\alpha f}}\frac{v_y(k-1)+a{\omega }_z(k-1)}{v_x(k-1)}{\delta }_f(k-1)-\frac{1}{2}{C}_d{A}_f{\rho }_a{v}_x^2(k-1)]+\frac{2T}{m}[F_{\mathrm{tf}}(k-1)+F_{\mathrm{tr}}(k-1)]+w_1$

$f_2(X(k-1),U(k-1),W(k-1),\gamma (k-1))={\omega }_z(k-1)+\frac{T}{I_z}\left\{2a{C}_{\mathrm{\alpha f}}\right[{\delta }_f(k-1)-\frac{(v_y(k-1)+a{\omega }_z(k-1))}{v_x(k-1)}]$

$-2b{C}_{\mathrm{\alpha r}}\frac{[b{\omega }_z(k-1)-v_y(k-1)]}{v_x(k-1)}\}+\frac{2\mathrm{aT}}{I_z}{F}_{\mathrm{tf}}(k-1){\delta }_f(k-1)+w_2$

$f_3(X(k-1),U(k-1),W(k-1),\gamma (k-1))=v_y(k-1)+\frac{T}{m}\{-m{v}_x(k-1){\omega }_z(k-1)+$

$2C_{\mathrm{\alpha f}}[{\delta }_f(k-1)+\frac{-v_y(k-1)-a{\omega }_z(k-1)}{v_x(k-1)}]+2C_{\mathrm{\alpha r}}\frac{b{\omega }_z(k-1)-v_y(k-1)}{v_x(k-1)}\}+\frac{2T}{m}{F}_{\mathrm{tf}}(k-1){\delta }_f(k-1)+w_3$

在f₁、f₂及f₃的上述表达式中，m和I_z分别是车辆的质量和绕过质心垂向轴的转动 惯量，a是汽车前轮轮轴中心到质心的距离，b是汽车后轮轮轴中心到质心的距离， C_αf、C_αr分别表示前、后轮胎的侧偏刚度，C_d表示空气阻力系数，A_f表示车辆前 向面积，ρ_a代表空气密度，T表示离散的周期；W对应的系统噪声协方差阵Q(k-1) 为：

$Q(k-1)=\left(\begin{array}{ccc}{{\sigma }_{w_1}}^2& 0& 0\\ 0& {{\sigma }_{w_2}}^2& 0\\ 0& 0& {{\sigma }_{w_3}}^2\end{array}\right),$其中及分别表示系统高斯白噪 声w₁、w₂及w₃对应的方差；γ(k-1)对应的系统外部输入噪声的协方差阵Γ(k-1)为 $\Gamma (k-1)=\left(\begin{array}{ccc}{{\sigma }_{{\delta }_f}}^2& 0& 0\\ 0& {{\sigma }_{F_{\mathrm{tf}}}}^2& 0\\ 0& 0& {{\sigma }_{F_{\mathrm{tr}}}}^2\end{array}\right),$其中及分别表示及对 应的方差；

卡尔曼滤波的观测方程的离散化矩阵形式为：

Z(k)＝H(k)·X(k)+V(k)                                (2)

式(2)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观 测白噪声向量，且$Z\left(k\right)=\left(\begin{array}{c}{v}_{x\_m}\left(k\right)\\ {\omega }_{z\_m}\left(k\right)\end{array}\right)$、$H\left(k\right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right)$、$V=\left(\begin{array}{c}{n}_{v_x}\\ n_{{\omega }_z}\end{array}\right),$其中v_{x_m}(k) 和ω_{z_m}(k)分别为通过轮速传感器测量获得的车辆纵向前进速度和横摆角速度； 表示通过轮速传感器测量获得的车辆纵向前进速度的观测噪声且是均值为 0、方差为的高斯白噪声，表示通过轮速传感器测量获得的横摆角速度的 观测噪声且是均值为0、方差为的高斯白噪声；V对应的观测噪声方差阵 R可表示为$R=\left(\begin{array}{cc}{{\sigma }_{v_x}}^2& 0\\ 0& {{\sigma }_{{\omega }_z}}^2\end{array}\right);$

2)进行改进的扩展卡尔曼滤波递推

对于式(1)和式(2)所描述的系统状态方程和观测方程，运用扩展卡尔曼 滤波理论，建立标准滤波递推过程，该递推过程包括时间更新和测量更新：

时间更新：

状态一步预测方程：$\hat{X}(k,k-1)=f(X(k-1),U(k-1),\mathrm{0,0})$

一步预测误差方差阵P(k，k-1)：

P(k，k-1)＝A(k，k-1)P(k-1)A′(k，k-1)+B(k，k-1)Γ(k-1)B′(k，k-1)+Q(k-1)

其中，A是系统状态函数向量f对状态向量X求偏导数的雅可比矩阵，B是系统状 态函数向量f对外部输入向量U求偏导数的雅可比矩阵，即矩阵A和B的第i行第j 列元素A_[i，j]和B_[i，j](i＝1，2，3  j＝1，2，3)可分别通过下面的式子求得

$A_{[i,j]}=\frac{\partial f_i}{\partial x_j}(\hat{X}(k,k-1),U(k-1),\mathrm{0,0})(i=\mathrm{1,2,3}j=\mathrm{1,2,3})$

$B_{[i,j]}=\frac{\partial f_i}{\partial u_j}(\hat{X}(k,k-1),U(k-1),\mathrm{0,0})(i=\mathrm{1,2,3}j=\mathrm{1,2,3})$

具体而言，各矩阵元素的取值如下：

$A_{\left[\mathrm{1,1}\right]}=1+T[\frac{-2C_{\mathrm{\alpha f}}(v_y+a{\omega }_z)}{m{v_x}^2}{\delta }_f-\frac{C_d{A}_f{\rho }_a{v}_x}{m}]A_{\left[\mathrm{1,2}\right]}=T(v_y+\frac{2C_{\mathrm{\alpha f}}a}{m{v}_x}{\delta }_f)$

$A_{\left[\mathrm{1,3}\right]}=T({\omega }_z+\frac{2C_{\mathrm{\alpha f}}}{m{v}_x}{\delta }_f)A_{\left[\mathrm{2,1}\right]}=\frac{2T[a{C}_{\mathrm{\alpha f}}(v_y+a{\omega }_z)+b{C}_{\mathrm{\alpha r}}(b{\omega }_z-v_y)]}{I_z{v_x}^2}$

$A_{\left[\mathrm{2,2}\right]}=1-\frac{2T(a^2{C}_{\mathrm{\alpha f}}+b^2{C}_{\mathrm{\alpha r}})}{I_z{v}_x}{A}_{\left[\mathrm{2,3}\right]}=\frac{2T(b{C}_{\mathrm{\alpha r}}-a{C}_{\mathrm{\alpha f}})}{I_z{v}_x}$

$A_{\left[\mathrm{3,1}\right]}=T[-{\omega }_z-2C_{\mathrm{\alpha r}}\frac{b{\omega }_z-v_y}{m{v_x}^2}+2C_{\mathrm{\alpha f}}\frac{v_y+a{\omega }_z}{m{v_x}^2}]$

$A_{\left[\mathrm{3,2}\right]}=T[-v_x+2\frac{(b{C}_{\mathrm{\alpha r}}-a{C}_{\mathrm{\alpha f}})}{m{v}_x}]$

$A_{\left[\mathrm{3,3}\right]}=1-\frac{2T(C_{\mathrm{\alpha r}}+C_{\mathrm{\alpha f}})}{m{v}_x}$

$B_{\left[\mathrm{1,1}\right]}=2T{C}_{\mathrm{\alpha f}}\frac{(v_y+a{\omega }_z)}{m{v}_x}{B}_{\left[\mathrm{1,2}\right]}=B_{\left[\mathrm{1,3}\right]}=\frac{2T}{m}$

$B_{\left[\mathrm{2,1}\right]}=\frac{2\mathrm{Ta}}{I_z}{C}_{\mathrm{\alpha f}}+\frac{2\mathrm{Ta}}{I_z}{F}_{\mathrm{tf}}{B}_{\left[\mathrm{2,2}\right]}=\frac{2\mathrm{Ta}}{I_z}{\delta }_f$

$B_{\left[\mathrm{3,1}\right]}=\frac{2T{F}_{\mathrm{tf}}}{m}+\frac{2T{C}_{\mathrm{\alpha f}}}{m}{B}_{\left[\mathrm{3,2}\right]}=\frac{2T{\delta }_f}{m}$

B_[2，3]＝B_[3，3]＝0

测量更新：

滤波增益矩阵k(k)：K(k)＝P(k，k-1)·H′(k)·[H(k)P(k，k-1)H′(k)+R(k)]^-1

状态估计：$\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\hat{X}(k,k-1)\text{]}$

估计误差方差阵P(k)：P(k)＝[I-K(k)·H(k)]·P(k，k-1)且I为3×3单位阵

在实际递推过程中，测量更新采用标量化处理方法。具体而言，时间更新过 程可按照上述滤波过程进行，而测量更新按以下改进的递推算法进行：

令P₁＝P(k，k-1)，由于观测向量维数为2，故将 H(k)、Z(k)和R(k)阵分成两块，即

$H\left(k\right)\hat{X}(k,k-1)=\left(\begin{array}{c}{h}_{r\_1}\\ h_{r\_2}\end{array}\right),$$H\left(k\right)=\left(\begin{array}{c}{H}_{r\_1}\\ H_{r\_2}\end{array}\right),$$Z\left(k\right)=\left(\begin{array}{c}{Z}_1\\ Z_2\end{array}\right),$$R\left(k\right)=\left(\begin{array}{cc}{R}_1& 0\\ 0& R_2\end{array}\right)$

对于i从1到2，进行2次递推计算：

$K_i=\frac{P_i·H_{r\_i}^{\prime }}{H_{r\_i}{P}_i{H}_{r\_i}^{\prime }+R_i}$

${\hat{X}}_{i+1}={\hat{X}}_i+K_i(Z_i-h_{r\_i})$

P_i+1＝(I-K_i·H_{r_i})·P_i

最终可得P(k)＝P₃，

在上述滤波递推计算过程中，可确定汽车在每个时刻的汽车纵向前进速度 v_x(k)、横摆角速度ω_z(k)和侧向速度v_y(k)，进而根据式(3)可确定每个时刻的质 心侧偏角：

β(k)＝arctan[v_y(k)/v_x(k)]                        (3)。

离散的周期T的典型值为10毫秒、20毫秒、50毫秒或100毫秒。

所述步骤1)中，

式(1)中，卡尔曼滤波的系统外输入的前轮转向角δ_f，是通过方向盘转角 传感器测得的方向盘转角δ除以从方向盘到前轮的转向传动比q_t来确定；而轮胎 纵向力F_tf和F_tr，是根据Dugoff非线性轮胎模型来确定；

用i_sj(j＝f，r)表示车辆纵向滑移率，即又可分为前轮轴纵向滑移率i_sf和后轮轴 纵向滑移率i_sr，下角标j取f或r，f或r分别表示前或后轮轴，i_sj计算方法为：

且j＝f，r  (4)，

式(4)中，R表示车轮轮胎半径；v_tf和v_tr分别表示前、后轮轴上沿轮胎方向 的速度，v_tf和v_tr可统一记为v_tj(j＝f，r)；ω_f表示前轮轴上两个车轮的旋转角速度等 效折算到前轮轴上的旋转角速度；ω_r表示后轮轴上两个车轮旋转角速度等效折算 到后轮轴上的旋转角速度，ω_f和ω_r可统一记为ω_j(j＝f，r)且

${\omega }_f=\frac{1}{2}({\omega }_{\mathrm{fR}}+{\omega }_{\mathrm{fL}})$

(5)

${\omega }_r=\frac{1}{2}({\omega }_{\mathrm{rR}}+{\omega }_{\mathrm{rL}})$

式(5)中，ω_fL、ω_fR、ω_rL和ω_rR分别表示左前轮、右前轮、左后轮和右后轮 的旋转角速度，通过利用四个轮速传感器测量获得；

v_tj(j＝f，r)可按式(6)确定：

v_tf＝v_xcosδ_f+(v_y+aω_z)sinδ_f

                                            (6)

v_tr＝v_x

进而，轮胎纵向力F_tf和F_tr可通过式(7)来确定

$F_{\mathrm{tj}}=\frac{C_{\mathrm{tj}}{i}_{\mathrm{sj}}}{1-i_{\mathrm{sj}}}{f}_t\left(p_j\right)(j=f,r)---\left(7\right)$

式(7)中，C_tf和C_tr分别表示单个前、后轮胎的纵向刚度，统一记为C_tj(j＝f，r)； 变量p_j(j＝f，r)和函数f_t(p_j)(j＝f，r)由以下式子确定：

$p_j=\frac{\mu F_{\mathrm{zj}}(1-{ϵ}_r{v}_x\sqrt{{i_{\mathrm{sj}}}^2+{\tan }^2{\alpha }_j})(1-i_{\mathrm{sj}})}{2\sqrt{{C_{\mathrm{tj}}}^2{i_{\mathrm{sj}}}^2+{C_{\mathrm{\alpha j}}}^2{\tan }^2{\alpha }_j}}j=f,r---\left(8\right)$

$f_t\left(p_j\right)=\left(\begin{array}{cc}{p}_j(2-p_j)& p_j<1\\ 1& p_j\ge 1\end{array}\right)j=f,r---\left(9\right)$

式(8)和(9)中，μ表示轮胎和地面间的垂向摩擦系数；ε_r表示道路附着 衰减因子；α_f、α_r分别表示前、后轮胎的侧偏角，统一记为α_j(j＝f，r)，可按下式计 算

${\alpha }_f={\delta }_f-\frac{v_y+a{\omega }_z}{v_x},$${\alpha }_r=\frac{b{\omega }_z-v_y}{v_x}---\left(10\right)$

而F_zj(j＝f，r)表示分配到前或后轮轴上的垂向载荷且可按下式计算

$F_{\mathrm{zf}}=\frac{\mathrm{mgb}}{2(a+b)},$$F_{\mathrm{zr}}=\frac{\mathrm{mga}}{2(a+b)}---\left(11\right)$

式(11)中，g表示重力加速度；

车辆纵向前进速度和横摆角速度与两个非转向后轮的速度存在以下关系

v_x＝(V_RL+V_RR)/2

ω_z＝(V_RL-V_RR)/T_W                                (12)

式(12)中，T_W表示后轮轴上两个后轮间的轮距，V_RL和V_RR分别表示左后 轮和右后轮的线速度；

对于式(2)中的测量值v_{x_m}(k)和ω_{z_m}(k)，它们是利用后轮轴上两个轮速传 感器测得的角速度乘以轮胎半径得到V_{RL_m}＝R·ω_rL和V_{RR_m}＝R·ω_rR，V_{RL_m}和 V_{RR_m}分别表示V_RL和V_RR含有噪声的测量值，进而利用式(12)获得的，即v_{x_m}和 ω_{z_m}分别表示v_x和ω_z的含有噪声的测量值且表 示通过轮速传感器测量获得的纵向前进速度的观测噪声且是均值为0、方差为 的高斯白噪声，表示通过轮速传感器测量获得的横摆角速度的观测噪声且 是均值为0、方差为的高斯白噪声。

有益效果

1.本发明提出了一种低成本、高精度、实时性好的基于改进扩展卡尔曼滤波 的车辆运行状态估计方法，可用于汽车主动安全控制对车辆运行状态的测量与估 计需要。

2.本发明的方法是针对汽车较高机动运行工况、在非线性整车动力学模型 和轮胎纵向力模型基础上提出的，在较高机动状况下仍可以获得准确的车辆运行 状态信息。

3.本发明提出的基于改进的扩展卡尔曼滤波的车辆运行状态估计方法不仅 可显著提高汽车纵向前进速度和横摆角速度等直测量的精度，而且可实现对质心 侧偏角、侧向速度等难于直测量的准确估计。

4.本发明提出的方法具有精度高、成本低、实时性好等优点。

附图说明

图1.车辆动力学模型

图2.设定的方向盘转角(度)和纵向前进速度(千米/小时-Km/h)随时间变化图

图3.本发明方法与Carsim输出的质心侧偏角(弧度-rad)随时间的变化曲线及局 部放大图

图4.本发明方法得到的质心侧偏角相对于Carsim输出的质心侧偏角参考值的误 差曲线

具体实施方式

实施实例1

随着社会经济的发展，道路交通安全问题日益突出，并已成为全球性难题。 全世界每年因交通事故都会造成大量的人员伤亡和财产损失，世界各国都在努力 降低交通事故的发生。近年来，汽车主动安全技术得到了迅速的发展。汽车主动 安全技术能够防患于未然，主动避免事故的发生，已成为现代汽车最主要的发展 方向之一。目前常见的主动安全技术主要包括汽车防抱死制动系统(ABS)，车 辆电子稳定程序(ESP)，牵引力控制系统(TCS)，电控驱动防滑系统(ASR)， 四轮转向稳定控制系统(4WS)等。这些系统通常涉及汽车轮胎的速度、汽车的 纵向前进速度、侧向速度、横摆角速度以及质心侧偏角等运行状态的测量或估计， 而这些运行状态的测量可用于后续的汽车主动安全控制，因此其精度直接关系汽 车的行驶安全性与稳定性，即上述主动安全控制系统能否有效工作在很大程度上 依赖于车辆运行状态能否被实时、准确的测量或估计。

目前，在汽车主动安全领域，车辆运动状态主要通过下述的三种方法来测量 或估计：

一是利用低成本的车载传感器(如惯性传感器和轮速传感器等)，对其测量 的信号进行简单的数学推算来获取有关车辆运行状态。例如，对于汽车质心侧偏 角，可利用纵向和横向加速度计先测得沿两个方向的加速度，然后积分运算分别 得到纵向前进速度和侧向速度，进而可求得质心侧偏角。这种方法尽管成本低， 但由于低成本传感器精度较差且推算处理过于简单而存在较大的测量误差，因而 影响了控制效果。

二是利用高精度的传感器对有关车辆运行状态进行直接测量(如利用光电五 轮仪或高精度的全球导航卫星系统GNSS，尤其是高精度全球定位系统GPS等)， 这种方法精度高但价格昂贵，无法大范围推广应用。

第三种方法是模型法，即通过对汽车的运行过程进行运动学或动力学建模， 同时将有关低成本的车载传感器(如轮速传感器、陀螺仪、加速度计以及GPS 等)信息作为观测信息，进而利用适当的滤波估计算法(如龙贝格观测器、非线 性观测器或卡尔曼滤波等)来实现对汽车运行状态的估计。第三种方法(即模型 法)可实现对有关难于直测量的估计，扩大状态估计的维数，还可提高有关直测 量的精度，同时成本较低。但目前已提出的模型法主要是基于汽车的运动学模型 或者对整车或轮胎做了较多线性化假定的动力学模型，这些模型在车辆较平稳运 行时能获得较好的估计效果和精度，但在较高机动运行状况下由于难于反映车辆 的实际非线性动力学行为导致估计精度较低。

为在较高机动运行工况下实现对车辆运行状态的可靠估计，本发明提出了一 种基于改进扩展卡尔曼滤波(Extended Kalman Filter，EKF)的车辆运行状态估 计方法。本发明提出的滤波估计方法可在汽车较高机动运行下实现对车辆运行状 态信号的准确估计，具有精度高、成本低、实时性好等特点。滤波计算出的车辆 运行状态信号主要包括汽车前进速度、侧向速度、横摆角速度以及质心侧偏角， 这些信息可用于汽车主动安全控制。较高机动运行是指当汽车运行在通常的道路 交通环境时，需要较为频繁的转向以及加减速的运行场合(侧向加速度在0.5g 之内，g表示重力加速度)。本发明的具体思路如下：

卡尔曼滤波器是以最小均方差为准则的最优状态估计滤波器，它不需要储存 过去的测量值，只根据当前的观测值和前一时刻的估计值，利用计算机进行递推 计算，便可实现对实时信号的估计，具有数据存储量小、算法简便的特点。根据 卡尔曼滤波理论，车辆运行状态的卡尔曼滤波模型除包括状态方程外，还应包括 观测方程。

为适应较高机动环境下汽车主动安全控制对车辆运行状态信号的测量与估 计要求，首先对汽车进行适当的动力学建模，即建立卡尔曼滤波过程的系统状态 方程。针对本发明的应用领域，本发明对于行驶在通常道路交通环境上的前轮转 向的四轮车辆(目前应有最广的情况，典型例子如前轮转向的轿车)，可做如下 的合理假定：

1)忽略汽车的俯仰、侧倾和上下弹跳运动。

2)忽略汽车悬架对轮胎轴上的影响。

3)忽略侧倾运动，可认为汽车前轴上左右两个轮胎的转向角、侧偏角、纵 向力及侧向力相同；类似地，可假定汽车后轴上左右两个轮胎的侧偏角、纵向力 及侧向力相同。

根据上述应用要求和假定，本发明针对目前应用较多的前轮转向四轮汽车， 采用附图1所示的车辆动力学模型(经等效简化后相当于前、后车轮被分别集中 在汽车前、后轴中点而构成的一假想Bicycle模型，如图1右侧所示)。该模型有3 个自由度，分别是纵向运动、侧向运动以及横摆转动。图1中定义了车辆载体坐 标系，其原点o位于质心处，ox轴沿车辆的纵向轴并与车辆前进方向一致，oz轴 垂直于车辆运行平面并指向地面(即向下，绕oz轴的横摆角速度ω_z的正方向定义 如图示)，而oy轴按右手螺旋规则可确定。纵向前进速度v_x、侧向速度v_y和横摆角 速度ω_z都是指车辆质心的。根据牛顿力学，车辆的动力学模型可描述为

$m{\stackrel{·}{v}}_x=m{v}_y{\omega }_z+2F_{\mathrm{tf}}\cos {\delta }_f-2F_{\mathrm{sf}}\sin {\delta }_f+2F_{\mathrm{tr}}-\frac{1}{2}{C}_d{A}_f{\rho }_a{v}_x^2$

$I_z{\stackrel{·}{\omega }}_z=2a{F}_{\mathrm{tf}}\sin {\delta }_f+2a{F}_{\mathrm{sf}}\cos {\delta }_f-2b{F}_{\mathrm{sr}}---\left(1\right)$

$m{\stackrel{·}{v}}_y=-m{v}_x{\omega }_z+2F_{\mathrm{tf}}\sin {\delta }_f+2F_{\mathrm{sf}}\cos {\delta }_f+2F_{\mathrm{sr}}$

式中，v_x、v_y及ω_z分别是汽车的纵向前进速度、侧向速度和横摆角速度，m和I_z分 别是车辆的质量和绕oz轴的转动惯量，a是汽车前轮轮轴中心到质心的距离，b 是汽车后轮轮轴中心到质心的距离，δ_f是前轮转向角，C_d表示空气阻力系数，A_f表 示车辆前向面积，ρ_a代表空气密度，F_tf是作用在单个前轮上的纵向力，F_tr是作用 在单个后轮上的纵向力，F_sf是作用在单个前轮上的侧向力，F_sr是作用在单个后 轮上的侧向力。

对于行驶在一般道路交通环境的车辆，通常可将作用在各轮上的侧向力表示 为

F_sf＝C_αfα_f，F_sr＝C_αrα_r                                      (2)

式(2)中，C_αf、C_αr分别表示前、后轮胎的侧偏刚度，α_f、α_r分别表示前、后轮 胎的侧偏角且可表示为

${\alpha }_f={\delta }_f-\frac{v_y+a{\omega }_z}{v_x},$${\alpha }_r=\frac{b{\omega }_z-v_y}{v_x}---\left(3\right)$

将式(2)、(3)代入式(1)，并考虑到δ_f通常是小角度，即sinδ_f≈δ_f、cosδ_f≈1且 忽略二阶及以上的高阶微量，经整理后可得

${\stackrel{·}{v}}_x=\frac{1}{m}[m{v}_y{\omega }_z+2\frac{v_y+a{\omega }_z}{v_x}{C}_{\mathrm{\alpha f}}{\delta }_f-\frac{1}{2}{C}_d{A}_f{\rho }_a{v}_x^2]+\frac{2}{m}(F_{\mathrm{tf}}+F_{\mathrm{tr}})$

${\stackrel{·}{\omega }}_z=\frac{1}{I_z}[2a({\delta }_f-\frac{(v_y+a{\omega }_z)}{v_x})C_{\mathrm{\alpha f}}-2b{C}_{\mathrm{\alpha r}}\frac{(b{\omega }_z-v_y)}{v_x}]+\frac{2a}{I_z}{F}_{\mathrm{tf}}{\delta }_f---\left(4\right)$

${\stackrel{·}{v}}_y=\frac{1}{m}[-m{v}_x{\omega }_z+2({\delta }_f-\frac{(v_y+a{\omega }_z)}{v_x})C_{\mathrm{\alpha f}}+2C_{\mathrm{\alpha r}}\frac{b{\omega }_z-v_y}{v_x}]+\frac{2}{m}{F}_{\mathrm{tf}}{\delta }_f$

对于式(4)中的前轮转向角δ_f可通过方向盘转角传感器测得的方向盘转角δ 除以从方向盘到前轮的转向传动比q_t来确定(即δ_f＝δ/q_t)。而对于式(4)中的轮 胎纵向力F_tf和F_tr，本发明采用Dugoff非线性轮胎模型来估计确定[可参考文献： Dugoff H.，Fancher P.S.，Segel L..An Analysis of Tire Traction Properties and Their  Influence on Vehicle Dynamic Performance.SAE Transactions，79：341-366，1970. SAE Paper No.700377]。为此，引入车辆纵向滑移率i_sj(j＝f，r)(即又可分为前轮 轴纵向滑移率i_sf和后轮轴纵向滑移率i_sr，即本发明中下角标j取f或r分别表示前或 后轮轴)，其计算与车辆的加减速状况密切相关，具体为

且j＝f，r  (5)

式(5)中，R表示车轮轮胎半径(通常情况下，可认为四个车轮的轮胎半径相 同)，v_tf和v_tr分别表示前、后轮轴上沿轮胎方向的速度(为标记方便，v_tf和v_tr可统 一记为v_tj(j＝f，r))，ω_f表示前轮轴上两个车轮的旋转角速度等效折算到前轮轴上的 旋转角速度，ω_r表示后轮轴上两个车轮旋转角速度等效折算到后轮轴上的旋转角 速度(ω_f和ω_r可统一记为ω_j(j＝f，r))，其计算公式如下

${\omega }_f=\frac{1}{2}({\omega }_{\mathrm{fR}}+{\omega }_{\mathrm{fL}})$

(6)

${\omega }_r=\frac{1}{2}({\omega }_{\mathrm{rR}}+{\omega }_{\mathrm{rL}})$

式(6)中，ω_fL、ω_fR、ω_rL和ω_rR分别表示左前轮、右前轮、左后轮和右后轮的旋 转角速度，通过利用四个轮速传感器测量获得。

此外，根据图1所示的运动关系，v_tj(j＝f，r)可按下式确定

v_tf＝v_xcosδ_f+(v_y+aω_z)sinδ_f

                                                (7)

v_tr＝v_x

根据Dugoff轮胎模型[可参考文献：Dugoff H.，Fancher P.S.，Segel L..An  Analysis of Tire Traction Properties and Their Influence on Vehicle Dynamic  Performance.SAE Transactions，79：341-366，1970.SAE Paper No.700377]，轮胎纵 向力F_tf和F_tr可通过下式来确定

$F_{\mathrm{tj}}=\frac{C_{\mathrm{tj}}{i}_{\mathrm{sj}}}{1-i_{\mathrm{sj}}}{f}_t\left(p_j\right)(j=f,r)---\left(8\right)$

式(8)中，C_tf和C_tr分别表示单个前、后轮胎的纵向刚度(可统记为C_tj(j＝f，r))， 变量p_j(j＝f，r)和函数f_t(p_j)(j＝f，r)由以下式子确定或定义

$p_j=\frac{\mu F_{\mathrm{zj}}(1-{ϵ}_r{v}_x\sqrt{{i_{\mathrm{sj}}}^2+{\tan }^2{\alpha }_j})(1-i_{\mathrm{sj}})}{2\sqrt{{C_{\mathrm{tj}}}^2{i_{\mathrm{sj}}}^2+{C_{\mathrm{\alpha j}}}^2{\tan }^2{\alpha }_j}}j=f,r---\left(9\right)$

$f_t\left(p_j\right)=\left(\begin{array}{cc}{p}_j(2-p_j)& p_j<1\\ 1& p_j\ge 1\end{array}\right)j=f,r---\left(10\right)$

式(9)和(10)中，μ表示轮胎和地面间的垂向摩擦系数，ε_r表示道路附着衰减 因子，F_zj(j＝f，r)表示分配到前或后轮轴上的垂向载荷且可按下式计算

$F_{\mathrm{zf}}=\frac{\mathrm{mgb}}{2(a+b)},$$F_{\mathrm{zr}}=\frac{\mathrm{mga}}{2(a+b)}---\left(11\right)$

式(11)中，g表示重力加速度。

对于式(4)描述的模型，它是一个具有3自由度的非线性车辆动力学模型， 不同于经常所采用的2自由度线性车辆模型。在经常采用的2自由度线性车辆模型 中，车辆的纵向前进速度被认为是定常的，车辆模型仅是关于侧向速度和横摆角 速度的线性微分方程。因此，2自由度线性车辆模型一般只适合前向速度不变或 变化缓慢的运行情况(机动性较低)，而对于较高机动运行情况(即需要频繁转 向以及加减速的情形)，该模型存在较大的建模误差。而本发明所采用的3自由度 非线性模型对车辆的纵向前进速度并无定常的限定，故即可适应一般机动环境也 可适应较高机动环境下车辆运行状态的准确估计。因此，本发明将根据式(4) 建立卡尔曼滤波的系统状态方程。

应注意的是，在实际的卡尔曼滤波递推过程中，需采用离散化的卡尔曼滤波 模型。为此，对式(4)的微分方程组进行离散化处理，且定义状态向量为 X＝[x₁ x₂ x₃]′且x₁＝v_x，x₂＝ω_z，x₃＝v_y，即X＝[v_x ω_z v_y]′(本发明中上 角标′表示对矩阵转置)，系统外输入向量定义为U＝[u₁ u₂ u₃]′且u₁＝δ_f， u₂＝F_tf，u₃＝F_tr，即U＝[δ_f F_tf F_tr]′，则离散化后的卡尔曼滤波的系统状态 方程的矩阵形式可表示为：

X(k)＝f(X(k-1)，U(k-1)，W(k-1)，γ(k-1))                 (12)

式中，k表示离散化时刻；W(k-1)表示零均值的系统高斯白噪声向量且 w＝[w₁ w₂ w₃]′，其中w₁、w₂及w₃分别表示三个系统高斯白噪声分量；γ(k-1) 表示系统外输入对应的零均值高斯白噪声向量且$\gamma ={\left(\begin{array}{ccc}{w}_{{\delta }_f}& w_{F_{\mathrm{tf}}}& w_{F_{\mathrm{tr}}}\end{array}\right)}^{\prime },$其中及分别表示直测或估计的系统外输入δ_f、F_tf及F_tr对应的零均值高斯白 噪声，这些白噪声隐含在系统状态函数f的三个系统外输入里面(即不显性地出 现在f中)；非线性的系统状态函数向量为

$f(X,U,W,\gamma )=\left(\begin{array}{c}{f}_1(X(k-1),U(k-1),W(k-1),\gamma (k-1))\\ f_2(X(k-1),U(k-1),W(k-1),\gamma (k-1))\\ f_3(X(k-1),U(k-1),W(k-1),\gamma (k-1))\end{array}\right),$

其中，

$f_1(X(k-1),U(k-1),W(k-1),\gamma (k-1))=v_x(k-1)+\frac{T}{m}[m{v}_y(k-1){\omega }_z(k-1)+$

$2C_{\mathrm{\alpha f}}\frac{v_y(k-1)+a{\omega }_z(k-1)}{v_x(k-1)}{\delta }_f(k-1)-\frac{1}{2}{C}_d{A}_f{\rho }_a{v}_x^2(k-1)]+\frac{2T}{m}[F_{\mathrm{tf}}(k-1)+F_{\mathrm{tr}}(k-1)]+w_1$

$f_2(X(k-1),U(k-1),W(k-1),\gamma (k-1))={\omega }_z(k-1)+\frac{T}{I_z}\left\{2a{C}_{\mathrm{\alpha f}}\right[{\delta }_f(k-1)-\frac{(v_y(k-1)+a{\omega }_z(k-1))}{v_x(k-1)}]$

$-2b{C}_{\mathrm{\alpha r}}\frac{[b{\omega }_z(k-1)-v_y(k-1)]}{v_x(k-1)}\}+\frac{2\mathrm{aT}}{I_z}{F}_{\mathrm{tf}}(k-1){\delta }_f(k-1)+w_2$

$f_3(X(k-1),U(k-1),W(k-1),\gamma (k-1))=v_y(k-1)+\frac{T}{m}\{-m{v}_x(k-1){\omega }_z(k-1)+$

$2C_{\mathrm{\alpha f}}[{\delta }_f(k-1)+\frac{-v_y(k-1)-a{\omega }_z(k-1)}{v_x(k-1)}]+2C_{\mathrm{\alpha r}}\frac{b{\omega }_z(k-1)-v_y(k-1)}{v_x(k-1)}\}+\frac{2T}{m}{F}_{\mathrm{tf}}(k-1){\delta }_f(k-1)+w_3$

且T表示离散的周期(在本发明中，根据测量传感器特性，T的典型值可取为10 毫秒、20毫秒、50毫秒、100毫秒等)；W对应的系统噪声协方差阵Q(k-1)为 $Q(k-1)=\left(\begin{array}{ccc}{{\sigma }_{w_1}}^2& 0& 0\\ 0& {{\sigma }_{w_2}}^2& 0\\ 0& 0& {{\sigma }_{w_3}}^2\end{array}\right),$其中及分别表示系统高斯白噪声 w₁、w₂及w₃对应的方差；γ(k-1)对应的系统外部输入噪声的协方差阵Γ(k-1)为 $\Gamma (k-1)=\left(\begin{array}{ccc}{{\sigma }_{{\delta }_f}}^2& 0& 0\\ 0& {{\sigma }_{F_{\mathrm{tf}}}}^2& 0\\ 0& 0& {{\sigma }_{F_{\mathrm{tr}}}}^2\end{array}\right),$其中及分别表示及对 应的方差。

建立车辆运行状态估计的卡尔曼滤波模型的系统状态方程后，下面讨论如何 建立其观测方程。从运动学角度，图1所示的车辆运动实际上是一个平面复合运 动(纵向运动、侧向运动和横摆转动的复合)，故根据平面复合运动关系，可得

$V_{\mathrm{RL}}=v_x+\frac{T_W}{2}{\omega }_z$

(13)

$V_{\mathrm{RR}}=v_x-\frac{T_W}{2}{\omega }_z$

式中，V_RL和V_RR分别代表左后轮和右后轮(即两个非转向轮)的车轮线速度，T_W是后轮轴上两个后轮间的轮距。

对式(13)重新整理，可以得到

v_x＝(V_RL+V_RR)/2

ω_z＝(V_RL-V_RR)/T_W                                (14)

需要指出的是，左后轮和右后轮的车轮线速度可通过安装在后轮轴上的两个 轮速传感器获得，即利用后轮轴上两个轮速传感器测得的角速度乘以轮胎半径得 到。考虑到轮速传感器的测量噪声，V_{RL_m}＝R·ω_rL与V_{RR_m}＝R·ω_rR，其中V_{RL_m}和 V_{RR_m}分别表示V_RL和V_RR含有噪声的测量值。另外，V_{RL_m}和V_{RR_m}还可分别表示 为其中和分别表示左后轮和右后 轮的车轮线速度的加性测量噪声(均可建模为均值为0的高斯白噪声)。

在本发明中，将纵向前进速度和横摆角速度作为卡尔曼滤波模型的观测量。 由于纵向前进速度和横摆角速度同时又是上述建立的卡尔曼滤波模型的两个状 态，故不难建立滤波系统的观测方程，其离散化后的矩阵形式为

Z(k)＝H(k)·X(k)+V(k)                                    (15)

式(15)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白 噪声向量，且$Z\left(k\right)=\left(\begin{array}{c}{v}_{x\_m}\left(k\right)\\ {\omega }_{z\_m}\left(k\right)\end{array}\right)$、$H\left(k\right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right)$、$V=\left(\begin{array}{c}{n}_{v_x}\\ n_{{\omega }_z}\end{array}\right),$而v_{x_m}(k)和 ω_{z_m}(k)分别为通过轮速传感器测量获得的纵向前进速度和横摆角速度(即利用 后轮轴上两个轮速传感器测得的角速度乘以轮胎半径得到左后轮和右后轮的车 轮线速度，进而利用式(14)获得)，即v_{x_m}(k)和ω_{z_m}(k)分别表示v_x和ω_z的含有 噪声的测量值且表示通过轮速传感器测量获 得的车辆纵向前进速度的观测噪声且是均值为0、方差为的高斯白噪声， 表示通过轮速传感器测量获得的横摆角速度的观测噪声且是均值为0、方 差为的高斯白噪声；故V对应的观测噪声方差阵R可表示为$R=\left(\begin{array}{cc}{{\sigma }_{v_x}}^2& 0\\ 0& {{\sigma }_{{\omega }_z}}^2\end{array}\right).$

对于式(12)描述的系统状态方程和式(15)描述的测量方程，可运用卡尔 曼滤波理论，建立起滤波递推估计过程。但注意到式(12)所示的状态方程为非 线性方程，在应用卡尔曼滤波计算时，需先进行线性化处理，将系统方程在 附近按泰勒级数展开(本发明中用表示状态X的滤波计算值)，保 留一阶微量、忽略高阶微量后再进行滤波递推计算，即需按照扩展卡尔曼滤波过 程进行滤波递推。根据扩展卡尔曼滤波理论，可建立本发明所涉及的扩展卡尔曼 滤波的一般递推过程(该递推过程包括时间更新和测量更新，下面递推过程的前 两步为时间更新，剩余的三步为测量更新)：

时间更新：

状态一步预测方程$\hat{X}(k,k-1)=f(X(k-1),U(k-1),\mathrm{0,0})$

一步预测误差方差阵P(k，k-1)

P(k，k-1)＝A(k，k-1)P(k-1)A′(k，k-1)+B(k，k-1)Γ(k-1)B′(k，k-1)+Q(k-1)

其中，A是系统状态函数向量f对状态向量X求偏导数的雅可比矩阵(Jacobian)， B是系统状态函数向量f对外部输入向量U求偏导数的雅可比矩阵(Jacobian)，即 矩阵A和B的第i行第j列元素A_[i，j]和B_[i，j](i＝1，2，3  j＝1，2，3)可分别通过下面的式子 求得

$A_{[i,j]}=\frac{\partial f_i}{\partial x_j}(\hat{X}(k,k-1),U(k-1),\mathrm{0,0})(i=\mathrm{1,2,3}j=\mathrm{1,2,3})$

$B_{[i,j]}=\frac{\partial f_i}{\partial u_j}(\hat{X}(k,k-1),U(k-1),\mathrm{0,0})(i=\mathrm{1,2,3}j=\mathrm{1,2,3})$

具体而言，根据式(12)，各矩阵元素的取值如下

$A_{\left[\mathrm{1,1}\right]}=1+T[\frac{-2C_{\mathrm{\alpha f}}(v_y+a{\omega }_z)}{m{v_x}^2}{\delta }_f-\frac{C_d{A}_f{\rho }_a{v}_x}{m}]A_{\left[\mathrm{1,2}\right]}=T(v_y+\frac{2C_{\mathrm{\alpha f}}a}{m{v}_x}{\delta }_f)$

$A_{\left[\mathrm{1,3}\right]}=T({\omega }_z+\frac{2C_{\mathrm{\alpha f}}}{m{v}_x}{\delta }_f)A_{\left[\mathrm{2,1}\right]}=\frac{2T[a{C}_{\mathrm{\alpha f}}(v_y+a{\omega }_z)+b{C}_{\mathrm{\alpha r}}(b{\omega }_z-v_y)]}{I_z{v_x}^2}$

$A_{\left[\mathrm{2,2}\right]}=1-\frac{2T(a^2{C}_{\mathrm{\alpha f}}+b^2{C}_{\mathrm{\alpha r}})}{I_z{v}_x}{A}_{\left[\mathrm{2,3}\right]}=\frac{2T(b{C}_{\mathrm{\alpha r}}-a{C}_{\mathrm{\alpha f}})}{I_z{v}_x}$

$A_{\left[\mathrm{3,1}\right]}=T[-{\omega }_z-2C_{\mathrm{\alpha r}}\frac{b{\omega }_z-v_y}{m{v_x}^2}+2C_{\mathrm{\alpha f}}\frac{v_y+a{\omega }_z}{m{v_x}^2}]$

$A_{\left[\mathrm{3,2}\right]}=T[-v_x+2\frac{(b{C}_{\mathrm{\alpha r}}-a{C}_{\mathrm{\alpha f}})}{m{v}_x}]$

$A_{\left[\mathrm{3,3}\right]}=1-\frac{2T(C_{\mathrm{\alpha r}}+C_{\mathrm{\alpha f}})}{m{v}_x}$

$B_{\left[\mathrm{1,1}\right]}=2T{C}_{\mathrm{\alpha f}}\frac{(v_y+a{\omega }_z)}{m{v}_x}{B}_{\left[\mathrm{1,2}\right]}=B_{\left[\mathrm{1,3}\right]}=\frac{2T}{m}$

$B_{\left[\mathrm{2,1}\right]}=\frac{2\mathrm{Ta}}{I_z}{C}_{\mathrm{\alpha f}}+\frac{2\mathrm{Ta}}{I_z}{F}_{\mathrm{tf}}{B}_{\left[\mathrm{2,2}\right]}=\frac{2\mathrm{Ta}}{I_z}{\delta }_f$

$B_{\left[\mathrm{3,1}\right]}=\frac{2T{F}_{\mathrm{tf}}}{m}+\frac{2T{C}_{\mathrm{\alpha f}}}{m}{B}_{\left[\mathrm{3,2}\right]}=\frac{2T{\delta }_f}{m}$

B_[2，3]＝B_[3，3]＝0

测量更新：

滤波增益矩阵k(k) K(k)＝P(k，k-1)·H′(k)·[H(k)P(k，k-1)H′(k)+R(k)]^-1

状态估计$\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\hat{X}(k,k-1)]$

估计误差方差阵P(k) P(k)＝[I-K(k)·H(k)]·P(k，k-1)且I为3×3单位阵

注意到上述扩展卡尔曼滤波递推过程在测量更新过程中(即计算k(k)时) 存在着矩阵的求逆运算。矩阵求逆时，计算量大且容易造成数值计算的不稳定。 对此，本发明在测量更新时不直接采用矩阵求逆的方法，而采用标量化处理 (scalar measurement processing)方法。具体而言，时间更新过程可按照上述滤 波过程进行，而测量更新按以下改进的递推算法进行：

令P₁＝P(k，k-1)，由于观测向量维数为2，故将 H(k)、Z(k)和R(k)阵分成两块，即

$H\left(k\right)\hat{X}(k,k-1)=\left(\begin{array}{c}{h}_{r\_1}\\ h_{r\_2}\end{array}\right),$$H\left(k\right)=\left(\begin{array}{c}{H}_{r\_1}\\ H_{r\_2}\end{array}\right),$$Z\left(k\right)=\left(\begin{array}{c}{Z}_1\\ Z_2\end{array}\right),$$R\left(k\right)=\left(\begin{array}{cc}{R}_1& 0\\ 0& R_2\end{array}\right)$

对于i从1到2，进行2次递推计算：

$K_i=\frac{P_i·H_{r\_i}^{\prime }}{H_{r\_i}{P}_i{H}_{r\_i}^{\prime }+R_i}$

${\hat{X}}_{i+1}={\hat{X}}_i+K_i(Z_i-h_{r\_i})$

P_i+1＝(I-K_i·H_{r_i})·P_i

最终可得P(k)＝P₃，

在上述滤波递推计算过程中，可确定汽车在每个时刻的汽车纵向前进速度 v_x(k)、横摆角速度ω_z(k)和侧向速度v_y(k)，进而根据下式可确定每个时刻的质心侧 偏角

β(k)＝arctan[v_y(k)/v_x(k)]                                    (16)

实施实例2

为检验本发明提出的基于改进的扩展卡尔曼滤波的车辆运行状态估计方法 的实际效果，在专业的汽车动力学仿真软件CarSim上进行了仿真验证实验。

CarSim是由美国MSC(Mechanical Simulation Corporation)公司开发的专门针 对车辆动力学的仿真软件，目前已被国际上众多的汽车制造商、零部件供应商所 采用，被广泛地应用于现代汽车控制系统的商业开发，已成为汽车行业的标准软 件，享有很高的声誉。Carsim内的车辆动力学模型是通过分别对汽车的车体、 悬架、转向、制动等各子系统以及各个轮胎的高逼真建模来实现的，具有很高的 自由度，能够提供非常接近实际的准确的车辆运行状态信息，因此，Carsim输 出的车辆运行状态信息可作为车辆的参考输出。

为检验本发明提出的算法在较高机动环境下的估计效果，仿真实验中设置汽 车的方向盘转角δ按幅值60⁰的正弦规律变化，同时汽车的纵向前进速度也在不断 地做加速、制动减速和匀速等变化，方向盘转角和纵向前进速度具体随时间的变 化过程如附图2所示，仿真时长设置为100秒(s)。所用车辆是一个前轮转向的四 轮车，主要参数如下：m＝960(千克)、I_z＝1382(千克·米²)、a＝0.948(米)、b＝1.422 (米)、C_αf＝C_αr＝25692(牛顿/弧度)、T_w＝1.390(米)。设定四个车轮的线速度 (通过轮速传感器测得的角速度乘以轮胎半径得到)的测量噪声均为均值是0、 标准差是0.04(米/秒)的高斯白噪声，方向盘转角传感器的测量噪声为均值是0、 标准差是0.0873(弧度)的高斯白噪声。卡尔曼滤波的系统零均值高斯白噪声的 标准差分别为及卡尔曼滤波的三个外输入的零均值高斯白噪声的标准差分别为及卡尔曼滤波的两个观测量的零均值 高斯白噪声的标准差分别为及有关结 果如表1以及图3～图4所示。

表1列出了对于整个过程利用直测法和本发明方法推算车辆运行状态的统 计结果对比，表中的误差均是相对于Carsim输出的相应参考值而言的(如直测 法的纵向前进速度误差就表示利用直测法推算的纵向前进速度相对于Carsim输 出的纵向前进速度参考值的误差)。另外需指出的是，上述两种方法的具体含义 如下：直测法是指通过直接测量换算得到的纵向前进速度和横摆角速度，即利用 后轮轴上两个轮速传感器测得的角速度乘以轮胎半径得到左后轮和右后轮的车 轮线速度，进而利用实施实例1中的式(14)直接推算得到的纵向前进速度和横 摆角速度；本发明方法是指利用本发明提出的改进扩展卡尔曼滤波估计方法来推 算车辆各运行状态的方法。

表1两种方法推算效果的对比表

表中“--”表示直测法无法推算的项

图3给出了本发明方法估计的质心侧偏角β的结果曲线(图中以EKF点划 虚线标示)，以及相应的Carsim的参考输出值(图中以Carsim实黑线标示)。图 4则给出了本发明方法估计的β相对于Carsim输出的β参考值的误差曲线。

由表1的对比(尤其是标准差)以及图3～图4，可以看出本发明方法相对于 直测法在纵向前进速度和横摆角速度的推算方面精度有了大幅的提高。另外，根 据表1及图3～图4，还可以看出本发明方法在侧向速度和质心侧偏角的估计方面 也具有很高的精度。

综上，即使在较高机动运行环境下，本发明提出的基于改进扩展卡尔曼滤波 的车辆运行状态估计方法能够准确地估计出车辆纵向前进速度、侧向速度、横摆 角速度以及质心侧偏角等信息，这些信息可满足有关汽车主动安全控制的需要。