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

权利要求书

1.一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法，其特征在于：本方法是针对前轮转向四轮汽车，建立非线性整车动力学模型和轮胎纵向力模型，同时利用车载轮速和方向盘转角传感器信息来确定建立滤波系统的外部输入量和观测量，然后通过提出的改进扩展卡尔曼滤波递推算法来实现对汽车纵向前进速度、横摆角速度、侧向速度和质心侧偏角信息的估计；所述滤波系统是扩展卡尔曼滤波系统，下述系统是指扩展卡尔曼滤波系统；

具体步骤包括：

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

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

X(k)＝f(X(k-1)，U(k-1)，W(k-1)，γ(k-1))              (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)表示系统外输入对应的零均值高斯白噪声向量且γ=wδfwFtfwFtr′,]]>其中及分别表示直测或估计的系统外输入δ_f、F_tf及F_tr对应的零均值高斯白噪声，这些白噪声隐含在系统状态函数f的三个系统外输入里面；非线性的系统状态函数向量为

f(X,U,W,γ)=f1(X(k-1),U(k-1),W(k-1),γ(k-1))f2(X(k-1),U(k-1),W(k-1),γ(k-1))f3(X(k-1),U(k-1),W(k-1),γ(k-1)),]]>

其中，

f1(X(k-1),U(k-1),W(k-1),γ(k-1))=vx(k-1)+Tm[mvy(k-1)ωz(k-1)+]]>

2Cαfvy(k-1)+aωz(k-1)vx(k-1)δf(k-1)-12CdAfρavx2(k-1)]+2Tm[Ftf(k-1)+Ftr(k-1)]+w1]]>

f2(X(k-1),U(k-1),W(k-1),γ(k-1))=ωz(k-1)+TIz{2aCαf[δf(k-1)-(vy(k-1)+aωz(k-1))vx(k-1)]]]>

-2bCαr[bωz(k-1)-vy(k-1)]vx(k-1)}+2aTIzFtf(k-1)δf(k-1)+w2]]>

f3(X(k-1),U(k-1),W(k-1),γ(k-1))=vy(k-1)+Tm{-mvx(k-1)ωz(k-1)+]]>

2Cαf[δf(k-1)+-vy(k-1)-aωz(k-1)vx(k-1)]+2Cαrbωz(k-1)-vy(k-1)vx(k-1)}+2TmFtf(k-1)δf(k-1)+w3]]>

在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)=σw12000σw22000σw32,]]>其中及分别表示系统高斯白噪声w₁、w₂及w₃对应的方差；γ(k-1)对应的系统外部输入噪声的协方差阵Γ(k-1)为Γ(k-1)=σδf2000σFtf2000σFtr2,]]>其中及分别表示及对应的方差；

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

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

式(2)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量，且Z(k)=vx_m(k)ωz_m(k)]]>、H(k)=100010]]>、V=nvxnωz,]]>其中v_{x_m}(k)和ω_{z_m}(k)分别为通过轮速传感器测量获得的车辆纵向前进速度和横摆角速度；表示通过轮速传感器测量获得的车辆纵向前进速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过轮速传感器测量获得的横摆角速度的观测噪声且是均值为0、方差为的高斯白噪声；V对应的观测噪声方差阵R可表示为R=σvx200σωz2;]]>

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

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

时间更新：

状态一步预测方程：X^(k,k-1)=f(X(k-1),U(k-1),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]=∂fi∂xj(X^(k,k-1),U(k-1),0,0)(i=1,2,3j=1,2,3)]]>

B[i,j]=∂fi∂uj(X^(k,k-1),U(k-1),0,0)(i=1,2,3j=1,2,3)]]>

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

A[1,1]=1+T[-2Cαf(vy+aωz)mvx2δf-CdAfρavxm]A[1,2]=T(vy+2Cαfamvxδf)]]>

A[1,3]=T(ωz+2Cαfmvxδf)A[2,1]=2T[aCαf(vy+aωz)+bCαr(bωz-vy)]Izvx2]]>

A[2,2]=1-2T(a2Cαf+b2Cαr)IzvxA[2,3]=2T(bCαr-aCαf)Izvx]]>

A[3,1]=T[-ωz-2Cαrbωz-vymvx2+2Cαfvy+aωzmvx2]]]>

A[3,2]=T[-vx+2(bCαr-aCαf)mvx]]]>

A[3,3]=1-2T(Cαr+Cαf)mvx]]>

B[1,1]=2TCαf(vy+aωz)mvxB[1,2]=B[1,3]=2Tm]]>

B[2,1]=2TaIzCαf+2TaIzFtfB[2,2]=2TaIzδf]]>

B[3,1]=2TFtfm+2TCαfmB[3,2]=2Tδfm]]>

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

状态估计：X^(k)=X^(k,k-1)+K(k)[Z(k)-H(k)X^(k,k-1)]]]>

估计误差方差阵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(k)X^(k,k-1)=hr_1hr_2,]]>H(k)=Hr_1Hr_2,]]>Z(k)=Z1Z2,]]>R(k)=R100R2]]>

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

Ki=Pi·Hr_i′Hr_iPiHr_i′+Ri]]>

X^i+1=X^i+Ki(Zi-hr_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)。