技术领域
[0001] 本
发明涉及一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法，其目的在于利用改进的扩展卡尔曼滤波方法对
汽车动
力学过程进行适当的建模，获得汽车在较高机动运行状况下的车辆运行状态，这些状态可用于汽车主动安全的相关控制，具有
精度高、成本低、实时性好等显著优点，属于汽车主动安全测量及控制领域。
背景技术
[0002] 随着社会经济的发展，道路交通安全问题日益突出，并已成为全球性难题。全世界每年因交通事故都会造成大量的人员伤亡和财产损失，世界各国都在努力降低交通事故的发生。近年来，汽车主动安全技术得到了迅速的发展。汽车主动安全技术能够防患于未然，主动避免事故的发生，已成为现代汽车最主要的发展方向之一。目前常见的主动安全技术主要包括防抱死
制动系统(ABS)，车辆
电子稳定程序(ESP)，
牵引力控制系统(TCS)，电控驱动防滑系统(ASR)，四轮转向稳定控制系统(4WS)等。这些系统通常涉及
汽车轮胎的速度、汽车的纵向前进速度、侧向速度、横摆
角速度以及质心
侧偏角等运行状态的测量或估计，而这些运行状态的测量可用于后续的汽车主动安全控制，因此其精度直接关系汽车的行驶安全性与
稳定性，即上述主动安全控制系统能否有效工作在很大程度上依赖于车辆运行状态能否被实时、准确的测量或估计。
[0003] 目前，在汽车主动安全领域，车辆运动状态主要通过三种方法来测量或估计。一是利用低成本的车载
传感器(如惯性传感器和
轮速传感器等)，对其测量的
信号进行简单的数学推算来获取有关车辆运行状态，这种方法成本低，但由于低成本传感器精度较差且推算处理过于简单而存在较大的测量误差，因而影响了控制效果。二是利用高精度的传感器对有关车辆运行状态进行直接测量(如利用光电五轮仪或高精度的全球导航卫
星系统GNSS，尤其是高精度全球
定位系统GPS等)，这种方法精度高但价格昂贵，无法大范围推广应用。第三种方法是模型法，即通过对汽车的运行过程进行运动学或动力学建模，同时将有关低成本的车载传感器(如轮速传感器、
陀螺仪、
加速度计以及GPS等)信息作为观测信息，进而利用适当的滤波估计
算法实现对汽车运行状态的估计。第三种方法(即模型法)可实现对难于直测量的估计，扩大状态估计的维数，还可提高有关直测量的精度，同时成本较低。但目前已提出的模型法主要是基于汽车的
运动学模型或者对整车或轮胎做了较多线性化假定的动力学模型，这些模型在车辆较平稳运行时能获得较好的估计效果和精度，但在较高机动运行状况下由于难于反映车辆的实际非线性动力学行为导致估计精度较低。
发明内容
[0004] 为在较高机动工况下实现对车辆运行状态的准确、可靠估计，本发明提出了一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法。本发明提出的方法针对汽车的较高机动运行状况来确定更接近实际的非线性整车动力学模型和轮胎纵向力模型，同时充分利用低成本的车载轮速和
方向盘转角传感器信息来建立滤波系统的外部输入量和观测量，进而通过改进的扩展卡尔曼滤波递推算法实现对汽车纵向前进速度、
横摆角速度、侧向速度以及质心侧偏角等车辆运行状态的滤波估计，具有精度高、成本低、实时性好等特点。
[0005] 一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法，本发明针对目前应用较多的前轮转向四轮汽车，为满足汽车主动安全控制对车辆运行状态的测量与估计需要，建立适用于较高机动运行工况的非线性整车动力学模型和轮胎纵向力模型，同时充分利用低成本的车载轮速和方向盘转角传感器信息来确定建立滤波系统的外部输入量和观测量，在此
基础上，通过提出的改进扩展卡尔曼滤波递推算法来实现对汽车纵向前进速度、横摆角速度、侧向速度以及质心侧偏角等信息的准确滤波估计；
[0006] 具体步骤包括：
[0007] 1)建立扩展卡尔曼滤波的
状态方程和观测方程：
[0008] 建立三
自由度的汽车非线性动力学模型，即建立扩展卡尔曼滤波过程的系统状态方程，离散化后的卡尔曼滤波的状态方程的矩阵形式表示为：
[0009] X(k)＝f(X(k-1)，U(k-1)，W(k-1)，γ(k-1)) (1)[0010] 式中，k表示离散化时刻；
[0011] 系统状态向量为X＝[x1 x2 x3]′且x1＝vx，x2＝ωz，x3＝vy，即X＝[vx ωz vy]′，vx、vy及ωz分别是汽车的纵向前进速度、侧向速度和横摆角速度，本发明中上角标′表示对矩阵转置；
[0012] 系统外输入向量为U＝[u1 u2 u3]′且u1＝δf，u2＝Ftf，u3＝Ftr，即U＝[δf Ftf Ftr]′，δf是前轮转向角，Ftf是作用在单个前轮上的纵向力，Ftr是作用在单个后轮上的纵向力；
[0013] W(k-1)表示零均值的系统高斯白噪声向量且W＝[w1 w2 w3]′，其中w1、w2及w3分别表示三个系统高斯白噪声分量；
[0014] γ(k-1)表示系统外输入对应的零均值高斯白噪声向量且 其中 及 分别表示直测或估计的系统外输入δf、Ftf及Ftr对应的零均值高斯白噪声，这些白噪声隐含在系统状态函数f的三个系统外输入里面；非线性的系统状态函数向量为[0015]
[0016] 其中，
[0017]
[0018]
[0019]
[0020]
[0021]
[0022]
[0023] 在f1、f2及f3的上述表达式中，m和Iz分别是车辆的
质量和绕过质心垂向轴的
转动惯量，a是汽车前轮轮轴中心到质心的距离，b是汽车后轮轮轴中心到质心的距离，Cαf、Cαr分别表示前、后轮胎的侧偏
刚度，Cd表示
空气阻力系数，Af表示车辆前向面积，ρa代表空气
密度，T表示离散的周期；W对应的系统噪声协方差阵Q(k-1)为：
[0024] 其中 及 分别表示系统高斯白噪声w1、w2及w3对应的方差；γ(k-1)对应的系统外部输入噪声的协方差阵Γ(k-1)为其中 及 分别表示 及 对应的方差；
[0025] 卡尔曼滤波的观测方程的离散化矩阵形式为：
[0026] Z(k)＝H(k)·X(k)+V(k) (2)
[0027] 式(2)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量，且 、 、 其中vx_m(k)和ωz_m(k)分别为通过轮速传感器测量获得的车辆纵向前进速度和横摆角速度；表示通过轮速传感器测量获得的车辆纵向前进速度的观测噪声且 是均值为0、方差为 的高斯白噪声， 表示通过轮速传感器测量获得的横摆角速度的观测噪声且 是均值为0、方差为 的高斯白噪声；V对应的观测噪声方差阵R可表示为
[0028] 2)进行改进的扩展卡尔曼滤波递推
[0029] 对于式(1)和式(2)所描述的系统状态方程和观测方程，运用扩展卡尔曼滤波理论，建立标准滤波递推过程，该递推过程包括时间更新和测量更新：
[0030] 时间更新：
[0031] 状态一步预测方程：
[0032] 一步预测误差方差阵P(k，k-1)：
[0033] 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)[0034] 其中，A是系统状态函数向量f对状态向量X求偏导数的雅可比矩阵，B是系统状态函数向量f对外部输入向量U求偏导数的雅可比矩阵，即矩阵A和B的第i行第j列元素A[i，j]和B[i，j](i＝1，2，3 j＝1，2，3)可分别通过下面的式子求得[0035]
[0036]
[0037] 具体而言，各矩阵元素的取值如下：
[0038]
[0039]
[0040]
[0041]
[0042]
[0043]
[0044]
[0045]
[0046]
[0047] B[2，3]＝B[3，3]＝0
[0048] 测量更新：-1
[0049] 滤波增益矩阵k(k)：K(k)＝P(k，k-1)·H′(k)·[H(k)P(k，k-1)H′(k)+R(k)][0050] 状态估计：
[0051] 估计误差方差阵P(k)：P(k)＝[I-K(k)·H(k)]·P(k，k-1)且I为3×3单位阵[0052] 在实际递推过程中，测量更新采用标量化处理方法。具体而言，时间更新过程可按照上述滤波过程进行，而测量更新按以下改进的递推算法进行：
[0053] 令P1＝P(k，k-1)， 由于观测向量维数为2，故将H(k)、Z(k)和R(k)阵分成两
块，即
[0054]
[0055] 对于i从1到2，进行2次递推计算：
[0056]
[0057]
[0058] Pi+1＝(I-Ki·Hr_i)·Pi
[0059] 最终可得P(k)＝P3，
[0060] 在上述滤波递推计算过程中，可确定汽车在每个时刻的汽车纵向前进速度vx(k)、横摆角速度ωz(k)和侧向速度vy(k)，进而根据式(3)可确定每个时刻的质心侧偏角：
[0061] β(k)＝arctan[vy(k)/vx(k)] (3)。
[0062] 离散的周期T的典型值为10毫秒、20毫秒、50毫秒或100毫秒。
[0063] 所述步骤1)中，
[0064] 式(1)中，卡尔曼滤波的系统外输入的前轮转向角δf，是通过方向盘转角传感器测得的方向盘转角δ除以从方向盘到前轮的转向
传动比qt来确定；而轮胎纵向力Ftf和Ftr，是根据Dugoff非线性轮胎模型来确定；
[0065] 用isj(j＝f，r)表示车辆纵向
滑移率，即又可分为前轮轴纵向滑移率isf和后轮轴纵向滑移率isr，下角标j取f或r，f或r分别表示前或后轮轴，isj计算方法为：
[0066] 且j＝f，r (4)，
[0067] 式(4)中，R表示车轮轮胎半径；vtf和vtr分别表示前、后轮轴上沿轮胎方向的速度，vtf和vtr可统一记为vtj(j＝f，r)；ωf表示前轮轴上两个车轮的旋转角速度等效折算到前轮轴上的旋转角速度；ωr表示后轮轴上两个车轮旋转角速度等效折算到后轮轴上的旋转角速度，ωf和ωr可统一记为ωj(j＝f，r)且
[0068]
[0069] (5)
[0070]
[0071] 式(5)中，ωfL、ωfR、ωrL和ωrR分别表示左前轮、右前轮、左后轮和右后轮的旋转角速度，通过利用四个轮速传感器测量获得；
[0072] vtj(j＝f，r)可按式(6)确定：
[0073] vtf＝vxcosδf+(vy+aωz)sinδf
[0074] (6)
[0075] vtr＝vx
[0076] 进而，轮胎纵向力Ftf和Ftr可通过式(7)来确定
[0077]
[0078] 式(7)中，Ctf和Ctr分别表示单个前、后轮胎的纵向刚度，统一记为Ctj(j＝f，r)；变量pj(j＝f，r)和函数ft(pj)(j＝f，r)由以下式子确定：
[0079]
[0080]
[0081] 式(8)和(9)中，μ表示轮胎和地面间的垂向
摩擦系数；εr表示道路附着衰减因子；αf、αr分别表示前、后轮胎的侧偏角，统一记为αj(j＝f，r)，可按下式计算[0082]
[0083] 而Fzj(j＝f，r)表示分配到前或后轮轴上的垂向
载荷且可按下式计算[0084]
[0086] 车辆纵向前进速度和横摆角速度与两个非转向后轮的速度存在以下关系[0087] vx＝(VRL+VRR)/2
[0088] ωz＝(VRL-VRR)/TW (12)
[0089] 式(12)中，TW表示后轮轴上两个后轮间的
轮距，VRL和VRR分别表示左后轮和右后轮的线速度；
[0090] 对于式(2)中的测量值vx_m(k)和ωz_m(k)，它们是利用后轮轴上两个轮速传感器测得的角速度乘以轮胎半径得到VRL_m＝R·ωrL和VRR_m＝R·ωrR，VRL_m和VRR_m分别表示VRL和VRR含有噪声的测量值，进而利用式(12)获得的，即vx_m和ωz_m分别表示vx和ωz的含有噪声的测量值且 表示通过轮速传感器测量获得的纵向前进速度的观测噪声且 是均值为0、方差为 的高斯白噪声， 表示通过轮速传感器测量获得的横摆角速度的观测噪声且 是均值为0、方差为 的高斯白噪声。
[0091] 有益效果
[0092] 1.本发明提出了一种低成本、高精度、实时性好的基于改进扩展卡尔曼滤波的车辆运行状态估计方法，可用于汽车主动安全控制对车辆运行状态的测量与估计需要。
[0093] 2.本发明的方法是针对汽车较高机动运行工况、在非线性整车动力学模型和轮胎纵向力模型基础上提出的，在较高机动状况下仍可以获得准确的车辆运行状态信息。
[0094] 3.本发明提出的基于改进的扩展卡尔曼滤波的车辆运行状态估计方法不仅可显著提高汽车纵向前进速度和横摆角速度等直测量的精度，而且可实现对质心侧偏角、侧向速度等难于直测量的准确估计。
[0095] 4.本发明提出的方法具有精度高、成本低、实时性好等优点。
附图说明
[0096] 图1.车辆动力学模型
[0097] 图2.设定的方向盘转角(度)和纵向前进速度(千米/小时-Km/h)随时间变化图
[0098] 图3.本发明方法与Carsim输出的质心侧偏角(弧度-rad)随时间的变化曲线及局部放大图
[0099] 图4.本发明方法得到的质心侧偏角相对于Carsim输出的质心侧偏角参考值的误差曲线
具体实施方式
[0100] 实施实例1
[0101] 随着社会经济的发展，道路交通安全问题日益突出，并已成为全球性难题。全世界每年因交通事故都会造成大量的人员伤亡和财产损失，世界各国都在努力降低交通事故的发生。近年来，汽车主动安全技术得到了迅速的发展。汽车主动安全技术能够防患于未然，主动避免事故的发生，已成为现代汽车最主要的发展方向之一。目前常见的主动安全技术主要包括汽车
防抱死制动系统(ABS)，车辆电子稳定程序(ESP)，牵引力控制系统(TCS)，电控驱动防滑系统(ASR)，四轮转向稳定控制系统(4WS)等。这些系统通常涉及
汽车轮胎的速度、汽车的纵向前进速度、侧向速度、横摆角速度以及质心侧偏角等运行状态的测量或估计，而这些运行状态的测量可用于后续的汽车主动安全控制，因此其精度直接关系汽车的行驶安全性与稳定性，即上述主动安全控制系统能否有效工作在很大程度上依赖于车辆运行状态能否被实时、准确的测量或估计。
[0102] 目前，在汽车主动安全领域，车辆运动状态主要通过下述的三种方法来测量或估计：
[0103] 一是利用低成本的车载传感器(如惯性传感器和轮速传感器等)，对其测量的信号进行简单的数学推算来获取有关车辆运行状态。例如，对于汽车质心侧偏角，可利用纵向和横向加速度计先测得沿两个方向的加速度，然后积分运算分别得到纵向前进速度和侧向速度，进而可求得质心侧偏角。这种方法尽管成本低，但由于低成本传感器精度较差且推算处理过于简单而存在较大的测量误差，因而影响了控制效果。
[0104] 二是利用高精度的传感器对有关车辆运行状态进行直接测量(如利用光电五轮仪或高精度的全球导航卫星系统GNSS，尤其是高精度全球定位系统GPS等)，这种方法精度高但价格昂贵，无法大范围推广应用。
[0105] 第三种方法是模型法，即通过对汽车的运行过程进行运动学或动力学建模，同时将有关低成本的车载传感器(如轮速传感器、陀螺仪、加速度计以及GPS等)信息作为观测信息，进而利用适当的滤波估计算法(如龙贝格观测器、非线性观测器或卡尔曼滤波等)来实现对汽车运行状态的估计。第三种方法(即模型法)可实现对有关难于直测量的估计，扩大状态估计的维数，还可提高有关直测量的精度，同时成本较低。但目前已提出的模型法主要是基于汽车的运动学模型或者对整车或轮胎做了较多线性化假定的动力学模型，这些模型在车辆较平稳运行时能获得较好的估计效果和精度，但在较高机动运行状况下由于难于反映车辆的实际非线性动力学行为导致估计精度较低。
[0106] 为在较高机动运行工况下实现对车辆运行状态的可靠估计，本发明提出了一种基于改进扩展卡尔曼滤波(Extended Kalman Filter，EKF)的车辆运行状态估计方法。本发明提出的滤波估计方法可在汽车较高机动运行下实现对车辆运行状态信号的准确估计，具有精度高、成本低、实时性好等特点。滤波计算出的车辆运行状态信号主要包括汽车前进速度、侧向速度、横摆角速度以及质心侧偏角，这些信息可用于汽车主动安全控制。较高机动运行是指当汽车运行在通常的道路交通环境时，需要较为频繁的转向以及加减速的运行场合(侧向加速度在0.5g之内，g表示重力加速度)。本发明的具体思路如下：
[0107] 卡尔曼
滤波器是以最小均方差为准则的最优状态估计滤波器，它不需要储存过去的测量值，只根据当前的观测值和前一时刻的估计值，利用计算机进行递推计算，便可实现对实时信号的估计，具有数据存储量小、算法简便的特点。根据卡尔曼滤波理论，车辆运行状态的卡尔曼滤波模型除包括状态方程外，还应包括观测方程。
[0108] 为适应较高机动环境下汽车主动安全控制对车辆运行状态信号的测量与估计要求，首先对汽车进行适当的动力学建模，即建立卡尔曼滤波过程的系统状态方程。针对本发明的应用领域，本发明对于行驶在通常道路交通环境上的前轮转向的四轮车辆(目前应有最广的情况，典型例子如前轮转向的轿车)，可做如下的合理假定：
[0109] 1)忽略汽车的
俯仰、侧倾和上下弹跳运动。
[0110] 2)忽略汽车悬架对轮胎轴上的影响。
[0111] 3)忽略侧倾运动，可认为汽车前轴上左右两个轮胎的转向角、侧偏角、纵向力及侧向力相同；类似地，可假定汽车后轴上左右两个轮胎的侧偏角、纵向力及侧向力相同。
[0112] 根据上述应用要求和假定，本发明针对目前应用较多的前轮转向四轮汽车，采用附图1所示的车辆动力学模型(经等效简化后相当于前、后车轮被分别集中在汽车前、后轴中点而构成的一假想Bicycle模型，如图1右侧所示)。该模型有3个自由度，分别是纵向运动、侧向运动以及横摆转动。图1中定义了车辆载体
坐标系，其原点o位于质心处，ox轴沿车辆的纵向轴并与车辆前进方向一致，oz轴垂直于车辆运行平面并指向地面(即向下，绕oz轴的横摆角速度ωz的正方向定义如图示)，而oy轴按右手螺旋规则可确定。纵向前进速度vx、侧向速度vy和横摆角速度ωz都是指车辆质心的。根据
牛顿力学，车辆的动力学模型可描述为
[0113]
[0114]
[0115]
[0116] 式中，vx、vy及ωz分别是汽车的纵向前进速度、侧向速度和横摆角速度，m和Iz分别是车辆的质量和绕oz轴的转动惯量，a是汽车前轮轮轴中心到质心的距离，b是汽车后轮轮轴中心到质心的距离，δf是前轮转向角，Cd表示空气阻力系数，Af表示车辆前向面积，ρa代表空气密度，Ftf是作用在单个前轮上的纵向力，Ftr是作用在单个后轮上的纵向力，Fsf是作用在单个前轮上的侧向力，Fsr是作用在单个后轮上的侧向力。
[0117] 对于行驶在一般道路交通环境的车辆，通常可将作用在各轮上的侧向力表示为[0118] Fsf＝Cαfαf，Fsr＝Cαrαr (2)[0119] 式(2)中，Cαf、Cαr分别表示前、后轮胎的侧偏刚度，αf、αr分别表示前、后轮胎的侧偏角且可表示为
[0120]
[0121] 将式(2)、(3)代入式(1)，并考虑到δf通常是小角度，即sinδf≈δf、cosδf≈1且忽略二阶及以上的高阶微量，经
整理后可得
[0122]
[0123]
[0124]
[0125] 对于式(4)中的前轮转向角δf可通过方向盘转角传感器测得的方向盘转角δ除以从方向盘到前轮的转向传动比qt来确定(即δf＝δ/qt)。而对于式(4)中的轮胎纵向力Ftf和Ftr，本发明采用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]。为此，引入车辆纵向滑移率isj(j＝f，r)(即又可分为前轮轴纵向滑移率isf和后轮轴纵向滑移率isr，即本发明中下角标j取f或r分别表示前或后轮轴)，其计算与车辆的加减速状况密切相关，具体为
[0126] 且j＝f，r (5)
[0127] 式(5)中，R表示车轮轮胎半径(通常情况下，可认为四个车轮的轮胎半径相同)，vtf和vtr分别表示前、后轮轴上沿轮胎方向的速度(为标记方便，vtf和vtr可统一记为vtj(j＝f，r))，ωf表示前轮轴上两个车轮的旋转角速度等效折算到前轮轴上的旋转角速度，ωr表示后轮轴上两个车轮旋转角速度等效折算到后轮轴上的旋转角速度(ωf和ωr可统一记为ωj(j＝f，r))，其计算公式如下
[0128]
[0129] (6)
[0130]
[0131] 式(6)中，ωfL、ωfR、ωrL和ωrR分别表示左前轮、右前轮、左后轮和右后轮的旋转角速度，通过利用四个轮速传感器测量获得。
[0132] 此外，根据图1所示的运动关系，vtj(j＝f，r)可按下式确定
[0133] vtf＝vxcosδf+(vy+aωz)sinδf
[0134] (7)
[0135] vtr＝vx
[0136] 根据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]，轮胎纵向力Ftf和Ftr可通过下式来确定
[0137]
[0138] 式(8)中，Ctf和Ctr分别表示单个前、后轮胎的纵向刚度(可统记为Ctj(j＝f，r))，变量pj(j＝f，r)和函数ft(pj)(j＝f，r)由以下式子确定或定义
[0139]
[0140]
[0141] 式(9)和(10)中，μ表示轮胎和地面间的垂向摩擦系数，εr表示道路附着衰减因子，Fzj(j＝f，r)表示分配到前或后轮轴上的垂向载荷且可按下式计算
[0142]
[0143] 式(11)中，g表示重力加速度。
[0144] 对于式(4)描述的模型，它是一个具有3自由度的非线性车辆动力学模型，不同于经常所采用的2自由度线性车辆模型。在经常采用的2自由度线性车辆模型中，车辆的纵向前进速度被认为是定常的，车辆模型仅是关于侧向速度和横摆角速度的线性微分方程。因此，2自由度线性车辆模型一般只适合前向速度不变或变化缓慢的运行情况(机动性较低)，而对于较高机动运行情况(即需要频繁转向以及加减速的情形)，该模型存在较大的建模误差。而本发明所采用的3自由度非线性模型对车辆的纵向前进速度并无定常的限定，故即可适应一般机动环境也可适应较高机动环境下车辆运行状态的准确估计。因此，本发明将根据式(4)建立卡尔曼滤波的系统状态方程。
[0145] 应注意的是，在实际的卡尔曼滤波递推过程中，需采用离散化的卡尔曼滤波模型。为此，对式(4)的微分方程组进行离散化处理，且定义状态向量为X＝[x1 x2 x3]′且x1＝vx，x2＝ωz，x3＝vy，即X＝[vx ωz vy]′(本发明中上角标′表示对矩阵转置)，系统外输入向量定义为U＝[u1 u2 u3]′且u1＝δf，u2＝Ftf，u3＝Ftr，即U＝[δf Ftf Ftr]′，则离散化后的卡尔曼滤波的系统状态方程的矩阵形式可表示为：
[0146] X(k)＝f(X(k-1)，U(k-1)，W(k-1)，γ(k-1)) (12)[0147] 式中，k表示离散化时刻；W(k-1)表示零均值的系统高斯白噪声向量且w＝[w1 w2 w3]′，其中w1、w2及w3分别表示三个系统高斯白噪声分量；γ(k-1)表示系统外输入对应的零均值高斯白噪声向量且 其中 及 分别表示直测或估计的系统外输入δf、Ftf及Ftr对应的零均值高斯白噪声，这些白噪声隐含在系统状态函数f的三个系统外输入里面(即不显性地出现在f中)；非线性的系统状态函数向量为[0148]
[0149] 其中，
[0150]
[0151]
[0152]
[0153]
[0154]
[0155]
[0156] 且T表示离散的周期(在本发明中，根据
测量传感器特性，T的典型值可取为10毫秒、20毫秒、50毫秒、100毫秒等)；W对应的系统噪声协方差阵Q(k-1)为其中 及 分别表示系统高斯白噪声w1、w2及w3对应的方差；γ(k-1)对应的系统外部输入噪声的协方差阵Γ(k-1)为其中 及 分别表示 及 对应的方差。
[0157] 建立车辆运行状态估计的卡尔曼滤波模型的系统状态方程后，下面讨论如何建立其观测方程。从运动学角度，图1所示的车辆运动实际上是一个平面复合运动(纵向运动、侧向运动和横摆转动的复合)，故根据平面复合运动关系，可得
[0158]
[0159] (13)
[0160]
[0161] 式中，VRL和VRR分别代表左后轮和右后轮(即两个非转向轮)的车轮线速度，TW是后轮轴上两个后轮间的轮距。
[0162] 对式(13)重新整理，可以得到
[0163] vx＝(VRL+VRR)/2
[0164] ωz＝(VRL-VRR)/TW (14)
[0165] 需要指出的是，左后轮和右后轮的车轮线速度可通过安装在后轮轴上的两个轮速传感器获得，即利用后轮轴上两个轮速传感器测得的角速度乘以轮胎半径得到。考虑到轮速传感器的测量噪声，VRL_m＝R·ωrL与VRR_m＝R·ωrR，其中VRL_m和VRR_m分别表示VRL和VRR含有噪声的测量值。另外，VRL_m和VRR_m还可分别表示为其中 和 分别表示左后轮和右后轮的车轮线速度的加性测量噪声(均可建模为均值为
0的高斯白噪声)。
[0166] 在本发明中，将纵向前进速度和横摆角速度作为卡尔曼滤波模型的观测量。由于纵向前进速度和横摆角速度同时又是上述建立的卡尔曼滤波模型的两个状态，故不难建立滤波系统的观测方程，其离散化后的矩阵形式为
[0167] Z(k)＝H(k)·X(k)+V(k) (15)[0168] 式(15)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量，且 、 、 而vx_m(k)和ωz_m(k)分别为通过轮速传感器测量获得的纵向前进速度和横摆角速度(即利用后轮轴上两个轮速传感器测得的角速度乘以轮胎半径得到左后轮和右后轮的车轮线速度，进而利用式(14)获得)，即vx_m(k)和ωz_m(k)分别表示vx和ωz的含有噪声的测量值且
表示通过轮速传感器测量获得的车辆纵向前进速度的观测噪声且 是均值为0、方差为的高斯白噪声， 表示通过轮速传感器测量获得的横摆角速度的观测噪声且 是均值为0、方差为 的高斯白噪声；故V对应的观测噪声方差阵R可表示为
[0169] 对于式(12)描述的系统状态方程和式(15)描述的测量方程，可运用卡尔曼滤波理论，建立起滤波递推估计过程。但注意到式(12)所示的状态方程为非线性方程，在应用卡尔曼滤波计算时，需先进行线性化处理，将系统方程在 附近按泰勒级数展开(本发明中用 表示状态X的滤波计算值)，保留一阶微量、忽略高阶微量后再进行滤波递推计算，即需按照扩展卡尔曼滤波过程进行滤波递推。根据扩展卡尔曼滤波理论，可建立本发明所涉及的扩展卡尔曼滤波的一般递推过程(该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新)：
[0170] 时间更新：
[0171] 状态一步预测方程
[0172] 一步预测误差方差阵P(k，k-1)
[0173] 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)[0174] 其中，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)可分别通过下面的式子求得
[0175]
[0176]
[0177] 具体而言，根据式(12)，各矩阵元素的取值如下
[0178]
[0179]
[0180]
[0181]
[0182]
[0183]
[0184]
[0185]
[0186]
[0187] B[2，3]＝B[3，3]＝0
[0188] 测量更新：
[0189] 滤波增益矩阵k(k) K(k)＝P(k，k-1)·H′(k)·[H(k)P(k，k-1)H′(k)+R(k)]-1[0190] 状态估计
[0191] 估计误差方差阵P(k) P(k)＝[I-K(k)·H(k)]·P(k，k-1)且I为3×3单位阵[0192] 注意到上述扩展卡尔曼滤波递推过程在测量更新过程中(即计算k(k)时)存在着矩阵的求逆运算。矩阵求逆时，计算量大且容易造成数值计算的不稳定。对此，本发明在测量更新时不直接采用矩阵求逆的方法，而采用标量化处理(scalar measurement processing)方法。具体而言，时间更新过程可按照上述滤波过程进行，而测量更新按以下改进的递推算法进行：
[0193] 令P1＝P(k，k-1)， 由于观测向量维数为2，故将H(k)、Z(k)和R(k)阵分成两块，即
[0194]
[0195] 对于i从1到2，进行2次递推计算：
[0196]
[0197]
[0198] Pi+1＝(I-Ki·Hr_i)·Pi
[0199] 最终可得P(k)＝P3，
[0200] 在上述滤波递推计算过程中，可确定汽车在每个时刻的汽车纵向前进速度vx(k)、横摆角速度ωz(k)和侧向速度vy(k)，进而根据下式可确定每个时刻的质心侧偏角[0201] β(k)＝arctan[vy(k)/vx(k)] (16)[0202] 实施实例2
[0203] 为检验本发明提出的基于改进的扩展卡尔曼滤波的车辆运行状态估计方法的实际效果，在专业的汽车动力学仿真
软件CarSim上进行了仿真验证实验。
[0204] CarSim是由美国MSC(Mechanical Simulation Corporation)公司开发的专
门针对车辆动力学的仿真软件，目前已被国际上众多的
汽车制造商、零部件供应商所采用，被广泛地应用于现代汽车控制系统的商业开发，已成为汽车行业的标准软件，享有很高的声誉。Carsim内的车辆动力学模型是通过分别对汽车的车体、悬架、转向、制动等各子系统以及各个轮胎的高逼真建模来实现的，具有很高的自由度，能够提供非常接近实际的准确的车辆运行状态信息，因此，Carsim输出的车辆运行状态信息可作为车辆的参考输出。
[0205] 为检验本发明提出的算法在较高机动环境下的估计效果，仿真实验中设置汽车0
的方向盘转角δ按幅值60的正弦规律变化，同时汽车的纵向前进速度也在不断地做加速、制动减速和匀速等变化，方向盘转角和纵向前进速度具体随时间的变化过程如附图2所示，仿真时长设置为100秒(s)。所用车辆是一个前轮转向的四轮车，主要参数如下：m
2
＝960(千克)、Iz＝1382(千克·米 )、a＝0.948(米)、b＝1.422(米)、Cαf＝Cαr＝
25692(牛顿/弧度)、Tw＝1.390(米)。设定四个车轮的线速度(通过轮速传感器测得的角速度乘以轮胎半径得到)的测量噪声均为均值是0、标准差是0.04(米/秒)的高斯白噪声，方向盘转角传感器的测量噪声为均值是0、标准差是0.0873(弧度)的高斯白噪声。卡尔曼滤波的系统零均值高斯白噪声的标准差分别为
及 卡尔曼滤波的三个外输入的零均值高斯白噪声的标准差分别为
及 卡尔曼滤波的两个观测量的零
均值高斯白噪声的标准差分别为 及 有关结果如表
1以及图3～图4所示。
[0206] 表1列出了对于整个过程利用直测法和本发明方法推算车辆运行状态的统计结果对比，表中的误差均是相对于Carsim输出的相应参考值而言的(如直测法的纵向前进速度误差就表示利用直测法推算的纵向前进速度相对于Carsim输出的纵向前进速度参考值的误差)。另外需指出的是，上述两种方法的具体含义如下：直测法是指通过直接测量换算得到的纵向前进速度和横摆角速度，即利用后轮轴上两个轮速传感器测得的角速度乘以轮胎半径得到左后轮和右后轮的车轮线速度，进而利用实施实例1中的式(14)直接推算得到的纵向前进速度和横摆角速度；本发明方法是指利用本发明提出的改进扩展卡尔曼滤波估计方法来推算车辆各运行状态的方法。
[0207] 表1两种方法推算效果的对比表
[0208] 表中“--”表示直测法无法推算的项
[0209]
[0210] 图3给出了本发明方法估计的质心侧偏角β的结果曲线(图中以EKF点划虚线标示)，以及相应的Carsim的参考输出值(图中以Carsim实黑线标示)。图4则给出了本发明方法估计的β相对于Carsim输出的β参考值的误差曲线。
[0211] 由表1的对比(尤其是标准差)以及图3～图4，可以看出本发明方法相对于直测法在纵向前进速度和横摆角速度的推算方面精度有了大幅的提高。另外，根据表1及图3～图4，还可以看出本发明方法在侧向速度和质心侧偏角的估计方面也具有很高的精度。
[0212] 综上，即使在较高机动运行环境下，本发明提出的基于改进扩展卡尔曼滤波的车辆运行状态估计方法能够准确地估计出车辆纵向前进速度、侧向速度、横摆角速度以及质心侧偏角等信息，这些信息可满足有关汽车主动安全控制的需要。
