CN102556075B

CN102556075B - Vehicle operating state estimation method based on improved extended Kalman filter

Info

Publication number: CN102556075B
Application number: CN201110419651.0A
Authority: CN
Inventors: 李旭; 陈伟
Original assignee: Southeast University
Current assignee: Southeast University
Priority date: 2011-12-15
Filing date: 2011-12-15
Publication date: 2015-04-01
Anticipated expiration: 2031-12-15
Also published as: CN102556075A

Abstract

A vehicle operating state estimation method based on improved extended Kalman filter includes using the improved extended Kalman filter algorithm for properly modeling to acquire operating state information such as longitudinal forward speed, yaw velocity, lateral speed, side slip angle and the like of a vehicle in a higher maneuvering operating state, wherein the information can be used for relevant control of vehicle active safety. The vehicle operating state estimation method based on improved extended Kalman filter has the advantages of high precision, low cost, high instantaneity and the like.

Description

A Method of Vehicle Operating State Estimation Based on Improved Extended Kalman Filter

技术领域 technical field

本发明涉及一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法，其目的在于利用改进的扩展卡尔曼滤波方法对汽车动力学过程进行适当的建模，获得汽车在较高机动运行状况下的车辆运行状态，这些状态可用于汽车主动安全的相关控制，具有精度高、成本低、实时性好等显著优点，属于汽车主动安全测量及控制领域。The present invention relates to a method for estimating vehicle running state based on improved extended Kalman filter, the purpose of which is to use the improved extended Kalman filter method to properly model the vehicle dynamics process and obtain the vehicle's operating state under relatively high maneuvering conditions. Vehicle running state, these states can be used for the related control of automobile active safety, with significant advantages such as high precision, low cost, and good real-time performance, and belong to the field of automobile active safety measurement and control.

背景技术 Background technique

随着社会经济的发展，道路交通安全问题日益突出，并已成为全球性难题。全世界每年因交通事故都会造成大量的人员伤亡和财产损失，世界各国都在努力降低交通事故的发生。近年来，汽车主动安全技术得到了迅速的发展。汽车主动安全技术能够防患于未然，主动避免事故的发生，已成为现代汽车最主要的发展方向之一。目前常见的主动安全技术主要包括防抱死制动系统(ABS)，车辆电子稳定程序(ESP)，牵引力控制系统(TCS)，电控驱动防滑系统(ASR)，四轮转向稳定控制系统(4WS)等。这些系统通常涉及汽车轮胎的速度、汽车的纵向前进速度、侧向速度、横摆角速度以及质心侧偏角等运行状态的测量或估计，而这些运行状态的测量可用于后续的汽车主动安全控制，因此其精度直接关系汽车的行驶安全性与稳定性，即上述主动安全控制系统能否有效工作在很大程度上依赖于车辆运行状态能否被实时、准确的测量或估计。With the development of society and economy, the problem of road traffic safety has become increasingly prominent and has become a global problem. Traffic accidents all over the world cause a large number of casualties and property losses every year, and all countries in the world are working hard to reduce the occurrence of traffic accidents. In recent years, automotive active safety technology has developed rapidly. Automobile active safety technology can prevent accidents before they happen and actively avoid accidents, which has become one of the most important development directions of modern automobiles. The current common active safety technologies mainly include anti-lock braking system (ABS), vehicle electronic stability program (ESP), traction control system (TCS), electronically controlled anti-skid system (ASR), four-wheel steering stability control system (4WS). )wait. These systems usually involve the measurement or estimation of the speed of the car tires, the longitudinal forward speed of the car, the lateral speed, the yaw rate, and the side slip angle of the center of mass, etc., and the measurement of these running states can be used for subsequent active safety control of the car, Therefore, its accuracy is directly related to the driving safety and stability of the vehicle, that is, whether the above-mentioned active safety control system can work effectively depends to a large extent on whether the vehicle operating state can be measured or estimated in real time and accurately.

目前，在汽车主动安全领域，车辆运动状态主要通过三种方法来测量或估计。一是利用低成本的车载传感器(如惯性传感器和轮速传感器等)，对其测量的信号进行简单的数学推算来获取有关车辆运行状态，这种方法成本低，但由于低成本传感器精度较差且推算处理过于简单而存在较大的测量误差，因而影响了控制效果。二是利用高精度的传感器对有关车辆运行状态进行直接测量(如利用光电五轮仪或高精度的全球导航卫星系统GNSS，尤其是高精度全球定位系统GPS等)，这种方法精度高但价格昂贵，无法大范围推广应用。第三种方法是模型法，即通过对汽车的运行过程进行运动学或动力学建模，同时将有关低成本的车载传感器(如轮速传感器、陀螺仪、加速度计以及GPS等)信息作为观测信息，进而利用适当的滤波估计算法实现对汽车运行状态的估计。第三种方法(即模型法)可实现对难于直测量的估计，扩大状态估计的维数，还可提高有关直测量的精度，同时成本较低。但目前已提出的模型法主要是基于汽车的运动学模型或者对整车或轮胎做了较多线性化假定的动力学模型，这些模型在车辆较平稳运行时能获得较好的估计效果和精度，但在较高机动运行状况下由于难于反映车辆的实际非线性动力学行为导致估计精度较低。At present, in the field of automotive active safety, the vehicle motion state is mainly measured or estimated by three methods. One is to use low-cost on-board sensors (such as inertial sensors and wheel speed sensors, etc.) to perform simple mathematical calculations on the measured signals to obtain relevant vehicle operating conditions. This method is low in cost, but due to poor accuracy of low-cost sensors And the calculation process is too simple and there are large measurement errors, thus affecting the control effect. The second is to use high-precision sensors to directly measure the operating status of the relevant vehicles (such as using a photoelectric five-wheel instrument or a high-precision global navigation satellite system GNSS, especially a high-precision global positioning system GPS, etc.). This method has high precision but is expensive. Expensive and unable to be widely used. The third method is the model method, that is, through kinematics or dynamics modeling of the running process of the car, and information about low-cost on-board sensors (such as wheel speed sensors, gyroscopes, accelerometers, and GPS, etc.) as observations Information, and then use the appropriate filter estimation algorithm to realize the estimation of the running state of the vehicle. The third method (that is, the model method) can realize the estimation of difficult direct measurement, expand the dimension of state estimation, and improve the accuracy of direct measurement, while the cost is low. However, the model methods that have been proposed so far are mainly based on the kinematics model of the car or the dynamic model that makes more linear assumptions on the vehicle or tires. These models can obtain better estimation results and accuracy when the vehicle is running relatively smoothly. , but under high maneuvering conditions, the estimation accuracy is low because it is difficult to reflect the actual nonlinear dynamic behavior of the vehicle.

发明内容 Contents of the invention

为在较高机动工况下实现对车辆运行状态的准确、可靠估计，本发明提出了一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法。本发明提出的方法针对汽车的较高机动运行状况来确定更接近实际的非线性整车动力学模型和轮胎纵向力模型，同时充分利用低成本的车载轮速和方向盘转角传感器信息来建立滤波系统的外部输入量和观测量，进而通过改进的扩展卡尔曼滤波递推算法实现对汽车纵向前进速度、横摆角速度、侧向速度以及质心侧偏角等车辆运行状态的滤波估计，具有精度高、成本低、实时性好等特点。In order to realize accurate and reliable estimation of the running state of the vehicle under relatively high maneuvering conditions, the present invention proposes a method for estimating the running state of the vehicle based on the improved extended Kalman filter. The method proposed by the present invention determines the nonlinear vehicle dynamics model and the tire longitudinal force model that are closer to the reality for the higher maneuvering condition of the automobile, and at the same time makes full use of the low-cost vehicle wheel speed and steering wheel angle sensor information to establish a filtering system The external input and observation quantities, and then through the improved extended Kalman filter recursive algorithm to realize the filter estimation of the vehicle running state such as the longitudinal forward speed, yaw rate, lateral speed and side slip angle of the center of mass, which has high precision, Low cost, good real-time performance and so on.

一种基于改进扩展卡尔曼滤波的车辆运行状态估计方法，本发明针对目前应用较多的前轮转向四轮汽车，为满足汽车主动安全控制对车辆运行状态的测量与估计需要，建立适用于较高机动运行工况的非线性整车动力学模型和轮胎纵向力模型，同时充分利用低成本的车载轮速和方向盘转角传感器信息来确定建立滤波系统的外部输入量和观测量，在此基础上，通过提出的改进扩展卡尔曼滤波递推算法来实现对汽车纵向前进速度、横摆角速度、侧向速度以及质心侧偏角等信息的准确滤波估计；A vehicle running state estimation method based on improved extended Kalman filter. The present invention is aimed at the front-wheel steering four-wheel vehicles that are widely used at present. In order to meet the needs of vehicle active safety control for measuring and estimating the vehicle running state, the invention establishes a method suitable for relatively large Non-linear vehicle dynamics model and tire longitudinal force model under high maneuvering operating conditions, while making full use of low-cost on-board wheel speed and steering wheel angle sensor information to determine the external input and observation of the filter system, on this basis , through the proposed improved extended Kalman filter recursive algorithm to realize the accurate filtering estimation of the vehicle's longitudinal forward speed, yaw rate, lateral speed and side slip angle of the center of mass;

具体步骤包括：Specific steps include:

1)建立扩展卡尔曼滤波的状态方程和观测方程：1) Establish the state equation and observation equation of the extended Kalman filter:

建立三自由度的汽车非线性动力学模型，即建立扩展卡尔曼滤波过程的系统状态方程，离散化后的卡尔曼滤波的状态方程的矩阵形式表示为：Establish a three-degree-of-freedom vehicle nonlinear dynamic model, that is, establish the system state equation of the extended Kalman filter process, and the matrix form of the discretized Kalman filter state equation is expressed as:

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

式中，k表示离散化时刻；In the formula, k represents the discretization time;

系统状态向量为X＝[x₁ x₂ x₃]′且x₁＝v_x，x₂＝ω_z，x₃＝v_y，即X＝[v_x ω_z v_y]′，v_x、v_y及ω_z分别是汽车的纵向前进速度、侧向速度和横摆角速度，本发明中上角标′表示对矩阵转置；The system state vector is X=[x ₁ x ₂ x ₃ ]′ and x ₁ =v _x , x ₂ =ω _z , x ₃ =v _y , that is, X=[v _x ω _z v _y ]′, v _x , v _y and ω _z are respectively the longitudinal forward speed, lateral speed and yaw rate of the automobile, and the superscript ' represents matrix transposition among the present invention;

系统外输入向量为U＝[u₁ u₂ u₃]′且u₁＝δ_f，u₂＝F_tf，u₃＝F_tr，即U＝[δ_f F_tf F_tr]′，δ_f是前轮转向角，F_tf是作用在单个前轮上的纵向力，F_tr是作用在单个后轮上的纵向力；The input vector outside the system is U=[u ₁ u ₂ u ₃ ]′ and u ₁ =δ _f , u ₂ =F _tf , u ₃ =F _tr , namely U=[δ _f F _tf F _tr ]′, δ _f is the front wheel steering angle, F _tf is the longitudinal force acting on a single front wheel, F _tr is the longitudinal force acting on a single rear wheel;

W(k-1)表示零均值的系统高斯白噪声向量且W＝[w₁ w₂ w₃]′，其中w₁、w₂及w₃分别表示三个系统高斯白噪声分量；W(k-1) represents a zero-mean system Gaussian white noise vector and W=[w ₁ w ₂ w ₃ ]′, wherein w ₁ , w ₂ and w ₃ represent three system Gaussian white noise components respectively;

γ(k-1)表示系统外输入对应的零均值高斯白噪声向量且 $γ = {[\begin{matrix} w_{δ_{f}} & w_{F_{tf}} & w_{F_{tr}} \end{matrix}]}^{'},$ 其中及分别表示直测或估计的系统外输入δ_f、F_tf及F_tr对应的零均值高斯白噪声，这些白噪声隐含在系统状态函数f的三个系统外输入里面；非线性的系统状态函数向量为γ(k-1) represents the zero-mean Gaussian white noise vector corresponding to the input outside the system and $γ = {[\begin{matrix} w_{δ_{f}} & w_{f_{tf}} & w_{f_{tr}} \end{matrix}]}^{'},$ in and respectively represent the zero-mean Gaussian white noise corresponding to the directly measured or estimated external input δ _f , F _tf and F _tr , these white noises are implied in the three external inputs of the system state function f; the nonlinear system state function vector is

$f f ((X x,, U u,, W W,, γ γ)) = = [\begin{matrix} {f f}_{11} ((X x ((k k - - 11)),, U u ((k k - - 11)),, W W ((k k - - 11)),, γ γ ((k k - - 11)))) \\ {f f}_{22} ((X x ((k k - - 11)),, U u ((k k - - 11)),, W W ((k k - - 11)),, γ γ ((k k - - 11)))) \\ {f f}_{33} ((X x ((k k - - 11)),, U u ((k k - - 11)),, W W ((k k - - 11)),, γ γ ((k k - - 11)))) \end{matrix}],,$

其中，in,

${f f}_{11} ((X x ((k k - - 11)),, U u ((k k - - 11)),, W W ((k k - - 11)),, γ γ ((k k - - 11)))) = = {v v}_{x x} ((k k - - 11)) + + \frac{T T}{m m} [[m m {v v}_{y the y} ((k k - - 11)) {ω ω}_{z z} ((k k - - 11)) + +$

$22 {C C}_{αf αf} \frac{{v v}_{y the y} ((k k - - 11)) + + a a {ω ω}_{z z} ((k k - - 11))}{{v v}_{x x} ((k k - - 11))} {δ δ}_{f f} ((k k - - 11)) - - \frac{11}{22} {C C}_{d d} {A A}_{f f} {ρ ρ}_{a a} {v v}_{x x}^{22} ((k k - - 11))]] + + \frac{22 T T}{m m} [[{F f}_{tf tf} ((k k - - 11)) + + {F f}_{tr tr} ((k k - - 11))]] + + {w w}_{11}$

${f f}_{22} ((X x ((k k - - 11)),, U u ((k k - - 11)),, W W ((k k - - 11)),, γ γ ((k k - - 11)))) = = {ω ω}_{z z} ((k k - - 11)) + + \frac{T T}{{I I}_{z z}} {{22 a a {C C}_{αf αf} [[{δ δ}_{f f} ((k k - - 11)) - - \frac{(({v v}_{y the y} ((k k - - 11)) + + a a {ω ω}_{z z} ((k k - - 11))))}{{v v}_{x x} ((k k - - 11))}]]$

$- - 22 b b {C C}_{αr αr} \frac{[[b b {ω ω}_{z z} ((k k - - 11)) - - {v v}_{y the y} ((k k - - 11))]]}{{v v}_{x x} ((k k - - 11))}}} + + \frac{22 aT aT}{{I I}_{z z}} {F f}_{tf tf} ((k k - - 11)) {δ δ}_{f f} ((k k - - 11)) + + {w w}_{22}$

${f f}_{33} ((X x ((k k - - 11)),, U u ((k k - - 11)),, W W ((k k - - 11)),, γ γ ((k k - - 11)))) = = {v v}_{y the y} ((k k - - 11)) + + \frac{T T}{m m} {{- - m m {v v}_{x x} ((k k - - 11)) {ω ω}_{z z} ((k k - - 11)) + +$

$22 {C C}_{αf αf} [[{δ δ}_{f f} ((k k - - 11)) + + \frac{- - {v v}_{y the y} ((k k - - 11)) - - a a {ω ω}_{z z} ((k k - - 11))}{{v v}_{x x} ((k k - - 11))}]] + + 22 {C C}_{αr αr} \frac{b b {ω ω}_{z z} ((k k - - 11)) - - {v v}_{y the y} ((k k - - 11))}{{v v}_{x x} ((k k - - 11))}}} + + \frac{22 T T}{m m} {F f}_{tf tf} ((k k - - 11)) {δ δ}_{f f} ((k k - - 11)) + + {w w}_{33}$

在f₁、f₂及f₃的上述表达式中，m和I_z分别是车辆的质量和绕过质心垂向轴的转动惯量，a是汽车前轮轮轴中心到质心的距离，b是汽车后轮轮轴中心到质心的距离，C_αf、C_αr分别表示前、后轮胎的侧偏刚度，C_d表示空气阻力系数，A_f表示车辆前向面积，ρ_a代表空气密度，T表示离散的周期；W对应的系统噪声协方差阵Q(k-1)为：In the above expressions of f ₁ , f ₂ and f ₃ , m and I _z are the mass of the vehicle and the moment of inertia around the vertical axis of the center of mass respectively, a is the distance from the center of the axle of the front wheel of the vehicle to the center of mass, and b is the The distance from the center of the rear wheel axle to the center of mass, C _αf and C _αr represent the cornering stiffness of the front and rear tires respectively, C _d represents the air resistance coefficient, A _f represents the front area of the vehicle, ρ _a represents the air density, and T represents the discrete period; the system noise covariance matrix Q(k-1) corresponding to W is:

$Q (k - 1) = [\begin{matrix} {σ_{w_{1}}}^{2} & 0 & 0 \\ 0 & {σ_{w_{2}}}^{2} & 0 \\ 0 & 0 & {σ_{w_{3}}}^{2} \end{matrix}],$ 其中及分别表示系统高斯白噪声w₁、w₂及w₃对应的方差；γ(k-1)对应的系统外部输入噪声的协方差阵Γ(k-1)为 $Γ (k - 1) = [\begin{matrix} {σ_{δ_{f}}}^{2} & 0 & 0 \\ 0 & {σ_{F_{tf}}}^{2} & 0 \\ 0 & 0 & {σ_{F_{tr}}}^{2} \end{matrix}],$ 其中及分别表示及对应的方差； $Q (k - 1) = [\begin{matrix} {σ_{w_{1}}}^{2} & 0 & 0 \\ 0 & {σ_{w_{2}}}^{2} & 0 \\ 0 & 0 & {σ_{w_{3}}}^{2} \end{matrix}],$ in and represent the variances corresponding to the system Gaussian white noise w ₁ , w ₂ and w ₃ respectively; the covariance matrix Γ(k-1) of the external input noise of the system corresponding to γ(k-1) is $Γ (k - 1) = [\begin{matrix} {σ_{δ_{f}}}^{2} & 0 & 0 \\ 0 & {σ_{f_{tf}}}^{2} & 0 \\ 0 & 0 & {σ_{f_{tr}}}^{2} \end{matrix}],$ in and Respectively and the corresponding variance;

卡尔曼滤波的观测方程的离散化矩阵形式为：The discretization matrix form of the observation equation of the Kalman filter is:

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

式(2)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量，且 $Z (k) = [\begin{matrix} v_{x_m} (k) \\ ω_{z_m} (k) \end{matrix}]$ 、 $H (k) = [\begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \end{matrix}]$ 、 $V = [\begin{matrix} n_{v_{x}} \\ n_{ω_{z}} \end{matrix}],$ 其中v_{x_m}(k)和ω_{z_m}(k)分别为通过轮速传感器测量获得的车辆纵向前进速度和横摆角速度；表示通过轮速传感器测量获得的车辆纵向前进速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过轮速传感器测量获得的横摆角速度的观测噪声且是均值为0、方差为的高斯白噪声；V对应的观测噪声方差阵R可表示为 $R = [\begin{matrix} {σ_{v_{x}}}^{2} & 0 \\ 0 & {σ_{ω_{z}}}^{2} \end{matrix}];$ In formula (2), Z is the observation vector, H is the observation array, V represents the zero-mean observation white noise vector that is uncorrelated with W, and $Z (k) = [\begin{matrix} v_{x_m} (k) \\ ω_{z_m} (k) \end{matrix}]$ , $h (k) = [\begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \end{matrix}]$ , $V = [\begin{matrix} {no}_{v_{x}} \\ {no}_{ω_{z}} \end{matrix}],$ Where v _{x_m} (k) and ω _{z_m} (k) are the longitudinal forward speed and yaw rate of the vehicle measured by the wheel speed sensor; represents the observed noise of the vehicle's longitudinal forward velocity measured by the wheel speed sensor and is a mean of 0 and a variance of Gaussian white noise, represents the observation noise of the yaw rate obtained by the wheel speed sensor measurement and is a mean of 0 and a variance of Gaussian white noise; the observation noise variance matrix R corresponding to V can be expressed as $R = [\begin{matrix} {σ_{v_{x}}}^{2} & 0 \\ 0 & {σ_{ω_{z}}}^{2} \end{matrix}];$

2)进行改进的扩展卡尔曼滤波递推2) Carry out improved extended Kalman filter recursion

对于式(1)和式(2)所描述的系统状态方程和观测方程，运用扩展卡尔曼滤波理论，建立标准滤波递推过程，该递推过程包括时间更新和测量更新：For the system state equation and observation equation described by equations (1) and (2), the standard filter recursion process is established by using the extended Kalman filter theory, which includes time update and measurement update:

时间更新：Time update:

状态一步预测方程： $\hat{X} (k, k - 1) = f (X (k - 1), U (k - 1), 0,0)$ State one-step prediction equation: $\hat{x} (k, k - 1) = f (x (k - 1), u (k - 1), 0,0)$

一步预测误差方差阵P(k，k-1)：One-step prediction error variance matrix 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)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)可分别通过下面的式子求得Among them, A is the Jacobian matrix of the partial derivative of the system state function vector f with respect to the state vector X, and B is the Jacobian matrix of the partial derivative of the system state function vector f with respect to the external input vector U, that is, the i-th row of the matrix A and B The j-th column elements A _{[i, j]} and B _{[i, j]} (i=1, 2, 3 j=1, 2, 3) can be obtained by the following formulas respectively

${A A}_{[[i i,, j j]]} = = \frac{&PartialD; &PartialD; {f f}_{i i}}{&PartialD; &PartialD; {x x}_{j j}} ((\overset{^^}{X x} ((k k,, k k - - 11)),, U u ((k k - - 11)),, 0,0 0,0)) ((i i = = 1,2,3 1,2,3 j j = = 1,2,3 1,2,3))$

${B B}_{[[i i,, j j]]} = = \frac{&PartialD; &PartialD; {f f}_{i i}}{&PartialD; &PartialD; {u u}_{j j}} ((\overset{^^}{X x} ((k k,, k k - - 11)),, U u ((k k - - 11)),, 0,0 0,0)) ((i i = = 1,2,3 1,2,3 j j = = 1,2,3 1,2,3))$

具体而言，各矩阵元素的取值如下：Specifically, the values of each matrix element are as follows:

${A A}_{[[1,1 1,1]]} = = 11 + + T T [[\frac{- - 22 {C C}_{αf αf} (({v v}_{y the y} + + a a {ω ω}_{z z}))}{m m {v v}_{x x}^{22}} {δ δ}_{f f} - - \frac{{C C}_{d d} {A A}_{f f} {ρ ρ}_{a a} {v v}_{x x}}{m m}]] {A A}_{[[1,2 1,2]]} = = T T (({v v}_{y the y} + + \frac{22 {C C}_{αf αf} a a}{m m {v v}_{x x}} {δ δ}_{f f}))$

${A A}_{[[1,3 1,3]]} = = T T (({ω ω}_{z z} + + \frac{22 {C C}_{αf αf}}{m m {v v}_{x x}} {δ δ}_{f f})) {A A}_{[[2,1 2,1]]} = = \frac{22 T T [[a a {C C}_{αf αf} (({v v}_{y the y} + + a a {ω ω}_{z z})) + + b b {C C}_{αr αr} ((b b {ω ω}_{z z} - - {v v}_{y the y}))]]}{{I I}_{z z} {v v}_{x x}^{22}}$

${A A}_{[[2,2 2,2]]} = = 11 - - \frac{22 T T (({a a}^{22} {C C}_{αf αf} + + {b b}^{22} {C C}_{αr αr}))}{{I I}_{z z} {v v}_{x x}} {A A}_{[[2,3 2,3]]} = = \frac{22 T T ((b b {C C}_{αr αr} - - a a {C C}_{αf αf}))}{{I I}_{z z} {v v}_{x x}}$

${A A}_{[[3,1 3,1]]} = = T T [[- - {ω ω}_{z z} - - 22 {C C}_{αr αr} \frac{b b {ω ω}_{z z} - - {v v}_{y the y}}{m m {v v}_{x x}^{22}} + + 22 {C C}_{αf αf} \frac{{v v}_{y the y} + + a a {ω ω}_{z z}}{m m {v v}_{x x}^{22}}]]$

${A A}_{[[3,2 3,2]]} = = T T [[- - {v v}_{x x} + + 22 \frac{((b b {C C}_{αr αr} - - a a {C C}_{αf αf}))}{m m {v v}_{x x}}]]$

${A A}_{[[3,3 3,3]]} = = 11 - - \frac{22 T T (({C C}_{αr αr} + + {C C}_{αf αf}))}{m m {v v}_{x x}}$

${B B}_{[[1,1 1,1]]} = = 22 T T {C C}_{αf αf} \frac{(({v v}_{y the y} + + a a {ω ω}_{z z}))}{m m {v v}_{x x}} {B B}_{[[1,2 1,2]]} = = {B B}_{[[1,3 1,3]]} = = \frac{22 T T}{m m}$

${B B}_{[[2,1 2,1]]} = = \frac{22 Ta Ta}{{I I}_{z z}} {C C}_{αf αf} + + \frac{22 Ta Ta}{{I I}_{z z}} {F f}_{tf tf} {B B}_{[[2,2 2,2]]} = = \frac{22 Ta Ta}{{I I}_{z z}} {δ δ}_{f f}$

${B B}_{[[3,1 3,1]]} = = \frac{22 T T {F f}_{tf tf}}{m m} + + \frac{22 T T {C C}_{αf αf}}{m m} {B B}_{[[3,2 3,2]]} = = \frac{22 T T {δ δ}_{f f}}{m m}$

B_[2，3]＝B_[3，3]＝0B _[2,3] =B _[3,3] =0

测量更新：Measurement update:

滤波增益矩阵k(k)：K(k)＝P(k，k-1)·H′(k)·[H(k)P(k，k-1)H′(k)+R(k)]^-1 Filter gain matrix k(k): K(k)=P(k,k-1) H'(k)[H(k)P(k,k-1)H'(k)+R(k )] ^-1

状态估计： $\hat{X} (k) = \hat{X} (k, k - 1) + K (k) [Z (k) - H (k) \hat{X} (k, k - 1)]$ State estimation: $\hat{x} (k) = \hat{x} (k, k - 1) + K (k) [Z (k) - h (k) \hat{x} (k, k - 1)]$

估计误差方差阵P(k)：P(k)＝[I-K(k)·H(k)]·P(k，k-1)且I为3×3单位阵Estimated error variance matrix P(k): P(k)=[I-K(k) H(k)] P(k, k-1) and I is a 3×3 unit matrix

在实际递推过程中，测量更新采用标量化处理方法。具体而言，时间更新过程可按照上述滤波过程进行，而测量更新按以下改进的递推算法进行：In the actual recursion process, the measurement update adopts the scalarization processing method. Specifically, the time update process can be performed according to the above filtering process, while the measurement update can be performed according to the following improved recursive algorithm:

令P₁＝P(k，k-1)，由于观测向量维数为2，故将H(k)、Z(k)和R(k)阵分成两块，即Let P ₁ =P(k,k-1), Since the dimension of the observation vector is 2, the The H(k), Z(k) and R(k) matrices are divided into two blocks, namely

$H h ((k k)) \overset{^^}{X x} ((k k,, k k - - 11)) = = [\begin{matrix} {h h}_{r r__11} \\ {h h}_{r r__22} \end{matrix}],,$ $H h ((k k)) = = [\begin{matrix} {H h}_{r r__11} \\ {H h}_{r r__22} \end{matrix}],,$ $Z Z ((k k)) = = [\begin{matrix} {Z Z}_{11} \\ {Z Z}_{22} \end{matrix}],,$ $R R ((k k)) = = [\begin{matrix} {R R}_{11} & 00 \\ 00 & {R R}_{22} \end{matrix}]$

对于i从1到2，进行2次递推计算：For i from 1 to 2, perform 2 recursive calculations:

${K K}_{i i} = = \frac{{P P}_{i i} \cdot &Center Dot; {H h}_{r r__i i}^{' '}}{{H h}_{r r__i i} {P P}_{i i} {H h}_{r r__i i}^{' '} + + {R R}_{i i}}$

${\overset{^^}{X x}}_{i i + + 11} = = {\overset{^^}{X x}}_{i i} + + {K K}_{i i} (({Z Z}_{i i} - - {h h}_{r r__i i}))$

P_i+1＝(I-K_i·H_{r_i})·P_i P _i+1 ＝(IK _i ·H _{r_i} )·P _i

最终可得P(k)＝P₃， Finally, P(k)=P ₃ can be obtained,

在上述滤波递推计算过程中，可确定汽车在每个时刻的汽车纵向前进速度v_x(k)、横摆角速度ω_z(k)和侧向速度v_y(k)，进而根据式(3)可确定每个时刻的质心侧偏角：In the above filtering and recursive calculation process, the longitudinal forward velocity v _x (k), yaw rate ω _z (k) and lateral velocity v _y (k) of the vehicle at each moment can be determined, and then according to formula (3 ) can determine the sideslip angle of the center of mass at each moment:

β(k)＝arctan[v_y(k)/v_x(k)] (3)。β(k)=arctan[v _y (k)/v _x (k)] (3).

离散的周期T的典型值为10毫秒、20毫秒、50毫秒或100毫秒。Typical values for the discrete period T are 10 milliseconds, 20 milliseconds, 50 milliseconds or 100 milliseconds.

所述步骤1)中，In the step 1),

式(1)中，卡尔曼滤波的系统外输入的前轮转向角δ_f，是通过方向盘转角传感器测得的方向盘转角δ除以从方向盘到前轮的转向传动比q_t来确定；而轮胎纵向力F_tf和F_tr，是根据Dugoff非线性轮胎模型来确定；In formula (1), the input front wheel steering angle δ _f outside the Kalman filter system is determined by dividing the steering wheel angle δ measured by the steering wheel angle sensor by the steering transmission ratio q _t from the steering wheel to the front wheels; and the tire The longitudinal forces F _tf and F _tr are determined according to the Dugoff nonlinear tire model;

用i_sj(j＝f，r)表示车辆纵向滑移率，即又可分为前轮轴纵向滑移率i_sf和后轮轴纵向滑移率i_sr，下角标j取f或r，f或r分别表示前或后轮轴，i_sj计算方法为：Use i _sj (j=f, r) to represent the longitudinal slip rate of the vehicle, that is, it can be divided into the front axle longitudinal slip rate i _sf and the rear axle longitudinal slip rate i _sr , and the subscript j is f or r, f or r represents the front or rear axle respectively, and the calculation method of i _sj is:

且j＝f，r (4)， and 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)且In formula (4), R represents the radius of the wheel tire; v _tf and v _tr represent the speed along the direction of the tire on the front and rear axles respectively, and v _tf and v _tr can be collectively recorded as v _tj (j=f, r); ω _f represents the rotational angular velocity of the two wheels on the front axle equivalently converted to the rotational angular velocity of the front axle; ω _r represents the rotational angular velocity of the two wheels on the rear axle equivalently converted to the rotational angular velocity of the rear axle, ω _f and ω _r Unified as ω _j (j=f, r) and

${ω ω}_{f f} = = \frac{11}{22} (({ω ω}_{fR f} + + {ω ω}_{fL f}))$

(5)(5)

${ω ω}_{r r} = = \frac{11}{22} (({ω ω}_{rR R} + + {ω ω}_{rL R}))$

式(5)中，ω_fL、ω_fR、ω_rL和ω_rR分别表示左前轮、右前轮、左后轮和右后轮的旋转角速度，通过利用四个轮速传感器测量获得；In formula (5), ω _fL , ω _fR , ω _rL and ω _rR represent the rotational angular velocities of the left front wheel, the right front wheel, the left rear wheel and the right rear wheel respectively, which are obtained by measuring with four wheel speed sensors;

v_tj(j＝f，r)可按式(6)确定：v _tj (j=f, r) can be determined according to formula (6):

v_tf＝v_xcosδ_f+(v_y+aω_z)sinδ_f v _tf ＝v _x cosδ _f +(v _y +aω _z )sinδ _f

(6)(6)

v_tr＝v_x v _tr =v _x

进而，轮胎纵向力F_tf和F_tr可通过式(7)来确定Furthermore, tire longitudinal forces F _tf and F _tr can be determined by formula (7)

${F f}_{tj tj} = = \frac{{C C}_{tj tj} {i i}_{sj sj}}{11 - - {i i}_{sj sj}} {f f}_{t t} (({p p}_{j j})) ((j j = = f f,, r r)) - - - - - - ((77))$

式(7)中，C_tf和C_tr分别表示单个前、后轮胎的纵向刚度，统一记为C_tj(j＝f，r)；变量p_j(j＝f，r)和函数f_t(p_j)(j＝f，r)由以下式子确定：In formula (7), C _tf and C _tr represent the longitudinal stiffness of a single front and rear tire respectively, which are collectively denoted as C _tj (j=f, r); variable p _j (j=f, r) and function f _t ( p _j )(j=f, r) is determined by the following formula:

${p p}_{j j} = = \frac{μ μ {F f}_{zj zj} ((11 - - {ϵ ϵ}_{r r} {v v}_{x x} \sqrt{{i i}_{sj sj}^{22} + + {tan the tan}^{22} {α α}_{j j}})) ((11 - - {i i}_{sj sj}))}{22 \sqrt{{C C}_{tj tj}^{22} {i i}_{sj sj}^{22} + + {C C}_{αj αj}^{22} {tan the tan}^{22} {α α}_{j j}}} j j = = f f,, r r - - - - - - ((88))$

${f f}_{t t} (({p p}_{j j})) = = \{\begin{matrix} {p p}_{j j} ((22 - - {p p}_{j j})) & {p p}_{j j} < < 11 \\ 11 & {p p}_{j j} &GreaterEqual; &Greater Equal; 11 \end{matrix} j j = = f f,, r r - - - - - - ((99))$

式(8)和(9)中，μ表示轮胎和地面间的垂向摩擦系数；ε_r表示道路附着衰减因子；α_f、α_r分别表示前、后轮胎的侧偏角，统一记为α_j(j＝f，r)，可按下式计算In formulas (8) and (9), μ represents the vertical friction coefficient between the tire and the ground; ε _r represents the road adhesion attenuation factor; α _f and α _r represent the side slip angles of the front and rear tires respectively, which are collectively denoted as α _j (j=f, r), can be calculated as follows

${α α}_{f f} = = {δ δ}_{f f} - - \frac{{v v}_{y the y} + + a a {ω ω}_{z z}}{{v v}_{x x}},,$ ${α α}_{r r} = = \frac{b b {ω ω}_{z z} - - {v v}_{y the y}}{{v v}_{x x}} - - - - - - ((1010))$

而F_zj(j＝f，r)表示分配到前或后轮轴上的垂向载荷且可按下式计算And F _zj (j=f, r) represents the vertical load distributed to the front or rear axle and can be calculated as follows

${F f}_{zf zf} = = \frac{mgb mgb}{22 ((a a + + b b))},,$ ${F f}_{zr zr} = = \frac{mga mga}{22 ((a a + + b b))} - - - - - - ((1111))$

式(11)中，g表示重力加速度；In formula (11), g represents the gravitational acceleration;

车辆纵向前进速度和横摆角速度与两个非转向后轮的速度存在以下关系The longitudinal forward speed and yaw rate of the vehicle have the following relationship with the speed of the two non-steering rear wheels

v_x＝(V_RL+V_RR)/2v _x = (V _RL +V _RR )/2

ω_z＝(V_RL-V_RR)/T_W (12)ω _z =(V _RL -V _RR )/T _W (12)

式(12)中，T_W表示后轮轴上两个后轮间的轮距，V_RL和V_RR分别表示左后轮和右后轮的线速度；In formula (12), T _W represents the wheelbase between the two rear wheels on the rear axle, and V _RL and V _RR represent the linear speeds of the left rear wheel and the right rear wheel, respectively;

对于式(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、方差为的高斯白噪声。For the measured values v _{x_m} (k) and ω _{z_m} (k) in formula (2), they are obtained by multiplying the angular velocity measured by the two wheel speed sensors on the rear axle by the tire radius V _{RL_m} = R·ω _rL and V _{RR_m} ＝R·ω _rR , V _{RL_m} and V _{RR_m} represent the noise-containing measurement values of V _RL and _VRR respectively, and then obtained by using formula (12), that is, v _{x_m} and ω _{z_m} represent the noise of v _x and ω _z respectively measured value and represents the observed noise of the longitudinal forward speed obtained by the wheel speed sensor measurement and is a mean of 0 and a variance of Gaussian white noise, represents the observation noise of the yaw rate obtained by the wheel speed sensor measurement and is a mean of 0 and a variance of Gaussian white noise.

有益效果Beneficial effect

1.本发明提出了一种低成本、高精度、实时性好的基于改进扩展卡尔曼滤波的车辆运行状态估计方法，可用于汽车主动安全控制对车辆运行状态的测量与估计需要。1. The present invention proposes a low-cost, high-precision, and real-time vehicle operating state estimation method based on the improved extended Kalman filter, which can be used for vehicle active safety control to measure and estimate the vehicle operating state.

2.本发明的方法是针对汽车较高机动运行工况、在非线性整车动力学模型和轮胎纵向力模型基础上提出的，在较高机动状况下仍可以获得准确的车辆运行状态信息。2. The method of the present invention is proposed on the basis of a nonlinear vehicle dynamics model and a tire longitudinal force model aimed at the relatively high maneuvering operating conditions of automobiles, and accurate vehicle operating state information can still be obtained under relatively high maneuvering conditions.

3.本发明提出的基于改进的扩展卡尔曼滤波的车辆运行状态估计方法不仅可显著提高汽车纵向前进速度和横摆角速度等直测量的精度，而且可实现对质心侧偏角、侧向速度等难于直测量的准确估计。3. The vehicle operating state estimation method based on the improved extended Kalman filter proposed by the present invention can not only significantly improve the accuracy of direct measurements such as the longitudinal forward velocity and the yaw rate of the vehicle, but also realize the measurement of the side slip angle of the center of mass, the lateral velocity, etc. Difficult to measure accurately.

4.本发明提出的方法具有精度高、成本低、实时性好等优点。4. The method proposed by the present invention has the advantages of high precision, low cost, good real-time performance and the like.

附图说明 Description of drawings

图1.车辆动力学模型Figure 1. Vehicle dynamics model

图2.设定的方向盘转角(度)和纵向前进速度(千米/小时-Km/h)随时间变化图Figure 2. Set steering wheel angle (degrees) and longitudinal forward speed (km/h-Km/h) versus time

图3.本发明方法与Carsim输出的质心侧偏角(弧度-rad)随时间的变化曲线及局部放大图Fig. 3. Variation curve and partial enlarged view of the center of mass sideslip angle (radian-rad) of the present invention method and Carsim output with time

图4.本发明方法得到的质心侧偏角相对于Carsim输出的质心侧偏角参考值的误差曲线Fig. 4. The error curve of the sideslip angle of the center of mass obtained by the method of the present invention relative to the reference value of the sideslip angle of the center of mass output by Carsim

具体实施方式 Detailed ways

实施实例1Implementation example 1

随着社会经济的发展，道路交通安全问题日益突出，并已成为全球性难题。全世界每年因交通事故都会造成大量的人员伤亡和财产损失，世界各国都在努力降低交通事故的发生。近年来，汽车主动安全技术得到了迅速的发展。汽车主动安全技术能够防患于未然，主动避免事故的发生，已成为现代汽车最主要的发展方向之一。目前常见的主动安全技术主要包括汽车防抱死制动系统(ABS)，车辆电子稳定程序(ESP)，牵引力控制系统(TCS)，电控驱动防滑系统(ASR)，四轮转向稳定控制系统(4WS)等。这些系统通常涉及汽车轮胎的速度、汽车的纵向前进速度、侧向速度、横摆角速度以及质心侧偏角等运行状态的测量或估计，而这些运行状态的测量可用于后续的汽车主动安全控制，因此其精度直接关系汽车的行驶安全性与稳定性，即上述主动安全控制系统能否有效工作在很大程度上依赖于车辆运行状态能否被实时、准确的测量或估计。With the development of society and economy, the problem of road traffic safety has become increasingly prominent and has become a global problem. Traffic accidents all over the world cause a large number of casualties and property losses every year, and all countries in the world are working hard to reduce the occurrence of traffic accidents. In recent years, automotive active safety technology has developed rapidly. Automobile active safety technology can prevent accidents before they happen and actively avoid accidents, which has become one of the most important development directions of modern automobiles. The current common active safety technologies mainly include automobile anti-lock braking system (ABS), vehicle electronic stability program (ESP), traction control system (TCS), electronically controlled drive anti-skid system (ASR), four-wheel steering stability control system ( 4WS) etc. These systems usually involve the measurement or estimation of the speed of the car tires, the longitudinal forward speed of the car, the lateral speed, the yaw rate, and the side slip angle of the center of mass, etc., and the measurement of these running states can be used for subsequent active safety control of the car, Therefore, its accuracy is directly related to the driving safety and stability of the vehicle, that is, whether the above-mentioned active safety control system can work effectively depends to a large extent on whether the vehicle operating state can be measured or estimated in real time and accurately.

目前，在汽车主动安全领域，车辆运动状态主要通过下述的三种方法来测量或估计：At present, in the field of automotive active safety, the vehicle motion state is mainly measured or estimated by the following three methods:

一是利用低成本的车载传感器(如惯性传感器和轮速传感器等)，对其测量的信号进行简单的数学推算来获取有关车辆运行状态。例如，对于汽车质心侧偏角，可利用纵向和横向加速度计先测得沿两个方向的加速度，然后积分运算分别得到纵向前进速度和侧向速度，进而可求得质心侧偏角。这种方法尽管成本低，但由于低成本传感器精度较差且推算处理过于简单而存在较大的测量误差，因而影响了控制效果。One is to use low-cost on-board sensors (such as inertial sensors and wheel speed sensors, etc.) to perform simple mathematical calculations on the measured signals to obtain relevant vehicle operating conditions. For example, for the side slip angle of the center of mass of the car, the acceleration along the two directions can be measured first by using the longitudinal and lateral accelerometers, and then the longitudinal forward speed and the lateral speed can be obtained by integral operation, respectively, and then the side slip angle of the center of mass can be obtained. Although the cost of this method is low, there is a large measurement error due to the poor accuracy of the low-cost sensor and the simple calculation process, which affects the control effect.

二是利用高精度的传感器对有关车辆运行状态进行直接测量(如利用光电五轮仪或高精度的全球导航卫星系统GNSS，尤其是高精度全球定位系统GPS等)，这种方法精度高但价格昂贵，无法大范围推广应用。The second is to use high-precision sensors to directly measure the operating status of the relevant vehicles (such as using a photoelectric five-wheel instrument or a high-precision global navigation satellite system GNSS, especially a high-precision global positioning system GPS, etc.). This method has high precision but is expensive. Expensive and unable to be widely used.

第三种方法是模型法，即通过对汽车的运行过程进行运动学或动力学建模，同时将有关低成本的车载传感器(如轮速传感器、陀螺仪、加速度计以及GPS等)信息作为观测信息，进而利用适当的滤波估计算法(如龙贝格观测器、非线性观测器或卡尔曼滤波等)来实现对汽车运行状态的估计。第三种方法(即模型法)可实现对有关难于直测量的估计，扩大状态估计的维数，还可提高有关直测量的精度，同时成本较低。但目前已提出的模型法主要是基于汽车的运动学模型或者对整车或轮胎做了较多线性化假定的动力学模型，这些模型在车辆较平稳运行时能获得较好的估计效果和精度，但在较高机动运行状况下由于难于反映车辆的实际非线性动力学行为导致估计精度较低。The third method is the model method, that is, through kinematics or dynamics modeling of the running process of the car, and information about low-cost on-board sensors (such as wheel speed sensors, gyroscopes, accelerometers, and GPS, etc.) as observations Information, and then use the appropriate filter estimation algorithm (such as Romberg observer, nonlinear observer or Kalman filter, etc.) to realize the estimation of the running state of the vehicle. The third method (that is, the model method) can realize the estimation of the difficult direct measurement, expand the dimension of the state estimation, and improve the accuracy of the direct measurement, while the cost is low. However, the model methods that have been proposed so far are mainly based on the kinematics model of the car or the dynamic model that makes more linear assumptions on the vehicle or tires. These models can obtain better estimation results and accuracy when the vehicle is running relatively smoothly. , but under high maneuvering conditions, the estimation accuracy is low because it is difficult to reflect the actual nonlinear dynamic behavior of the vehicle.

为在较高机动运行工况下实现对车辆运行状态的可靠估计，本发明提出了一种基于改进扩展卡尔曼滤波(Extended Kalman Filter，EKF)的车辆运行状态估计方法。本发明提出的滤波估计方法可在汽车较高机动运行下实现对车辆运行状态信号的准确估计，具有精度高、成本低、实时性好等特点。滤波计算出的车辆运行状态信号主要包括汽车前进速度、侧向速度、横摆角速度以及质心侧偏角，这些信息可用于汽车主动安全控制。较高机动运行是指当汽车运行在通常的道路交通环境时，需要较为频繁的转向以及加减速的运行场合(侧向加速度在0.5g之内，g表示重力加速度)。本发明的具体思路如下：In order to realize the reliable estimation of the running state of the vehicle under relatively high maneuvering operating conditions, the present invention proposes a method for estimating the running state of the vehicle based on the improved Extended Kalman Filter (EKF). The filter estimation method proposed by the invention can realize the accurate estimation of the vehicle running state signal under the relatively high maneuvering operation of the vehicle, and has the characteristics of high precision, low cost, good real-time performance and the like. The vehicle running state signal calculated by filtering mainly includes the forward speed, lateral speed, yaw rate and side slip angle of the center of mass of the car, which can be used for active safety control of the car. High maneuvering operation refers to the occasions when the car is running in the usual road traffic environment, which requires more frequent steering and acceleration and deceleration (lateral acceleration is within 0.5g, and g represents the acceleration of gravity). Concrete train of thought of the present invention is as follows:

卡尔曼滤波器是以最小均方差为准则的最优状态估计滤波器，它不需要储存过去的测量值，只根据当前的观测值和前一时刻的估计值，利用计算机进行递推计算，便可实现对实时信号的估计，具有数据存储量小、算法简便的特点。根据卡尔曼滤波理论，车辆运行状态的卡尔曼滤波模型除包括状态方程外，还应包括观测方程。The Kalman filter is an optimal state estimation filter based on the minimum mean square error. It does not need to store the measured values in the past. It only uses the computer to perform recursive calculations based on the current observed values and the estimated values at the previous moment. It can realize the estimation of real-time signals, and has the characteristics of small data storage and simple algorithm. According to the Kalman filter theory, the Kalman filter model of the vehicle running state should include the observation equation in addition to the state equation.

为适应较高机动环境下汽车主动安全控制对车辆运行状态信号的测量与估计要求，首先对汽车进行适当的动力学建模，即建立卡尔曼滤波过程的系统状态方程。针对本发明的应用领域，本发明对于行驶在通常道路交通环境上的前轮转向的四轮车辆(目前应有最广的情况，典型例子如前轮转向的轿车)，可做如下的合理假定：In order to meet the requirements of vehicle active safety control on the measurement and estimation of vehicle running state signals in a high-mobility environment, appropriate dynamic modeling is first performed on the vehicle, that is, the system state equation of the Kalman filter process is established. For the field of application of the present invention, the present invention can make the following reasonable assumptions for the four-wheeled vehicle (there should be the widest situation at present, such as the car with the front wheel steering) of the front wheel steering on the usual road traffic environment :

1)忽略汽车的俯仰、侧倾和上下弹跳运动。1) Ignore the pitch, roll and bouncing motion of the car.

2)忽略汽车悬架对轮胎轴上的影响。2) Ignore the influence of the car suspension on the tire axle.

3)忽略侧倾运动，可认为汽车前轴上左右两个轮胎的转向角、侧偏角、纵向力及侧向力相同；类似地，可假定汽车后轴上左右两个轮胎的侧偏角、纵向力及侧向力相同。3) Neglecting the roll motion, it can be considered that the steering angle, side slip angle, longitudinal force and lateral force of the left and right tires on the front axle of the car are the same; similarly, it can be assumed that the side slip angles of the left and right tires on the rear axle of the car , longitudinal force and lateral force are the same.

根据上述应用要求和假定，本发明针对目前应用较多的前轮转向四轮汽车，采用附图1所示的车辆动力学模型(经等效简化后相当于前、后车轮被分别集中在汽车前、后轴中点而构成的一假想Bicycle模型，如图1右侧所示)。该模型有3个自由度，分别是纵向运动、侧向运动以及横摆转动。图1中定义了车辆载体坐标系，其原点o位于质心处，ox轴沿车辆的纵向轴并与车辆前进方向一致，oz轴垂直于车辆运行平面并指向地面(即向下，绕oz轴的横摆角速度ω_z的正方向定义如图示)，而oy轴按右手螺旋规则可确定。纵向前进速度v_x、侧向速度v_y和横摆角速度ω_z都是指车辆质心的。根据牛顿力学，车辆的动力学模型可描述为According to the above-mentioned application requirements and assumptions, the present invention is aimed at currently using more front-wheel steering four-wheel vehicles, and adopts the vehicle dynamics model shown in accompanying drawing 1 (equivalently after equivalent simplification, the front and rear wheels are respectively concentrated on the four-wheel vehicle) A hypothetical Bicycle model formed by the midpoint of the front and rear axles, as shown on the right side of Figure 1). The model has three degrees of freedom, which are longitudinal motion, lateral motion, and yaw rotation. Figure 1 defines the vehicle carrier coordinate system, its origin o is located at the center of mass, the ox axis is along the longitudinal axis of the vehicle and is consistent with the forward direction of the vehicle, and the oz axis is perpendicular to the vehicle running plane and points to the ground (that is, downward, around the oz axis The positive direction of the yaw rate ω _z is defined as shown in the figure), and the y axis can be determined according to the right-hand spiral rule. The longitudinal forward speed v _x , the lateral speed v _y and the yaw rate ω _z all refer to the center of mass of the vehicle. According to Newtonian mechanics, the dynamic model of the vehicle can be described as

$m m {\overset{\cdot \cdot}{v v}}_{x x} = = m m {v v}_{y the y} {ω ω}_{z z} + + 22 {F f}_{tf tf} cos cos {δ δ}_{f f} - - 22 {F f}_{sf sf} sin sin {δ δ}_{f f} + + 22 {F f}_{tr tr} - - \frac{11}{22} {C C}_{d d} {A A}_{f f} {ρ ρ}_{a a} {v v}_{x x}^{22}$

${I I}_{z z} {\overset{\cdot &Center Dot;}{ω ω}}_{z z} = = 22 a a {F f}_{tf tf} sin sin {δ δ}_{f f} + + 22 a a {F f}_{sf sf} cos cos {δ δ}_{f f} - - 22 b b {F f}_{sr sr} - - - - - - ((11))$

$m m {\overset{\cdot &Center Dot;}{v v}}_{y the y} = = - - m m {v v}_{x x} {ω ω}_{z z} + + 22 {F f}_{tf tf} sin sin {δ δ}_{f f} + + 22 {F f}_{sf sf} cos cos {δ δ}_{f f} + + 22 {F f}_{sr 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是作用在单个后轮上的侧向力。In the formula, v _x , v _y and ω _z are the longitudinal speed, lateral speed and yaw rate of the car respectively, m and I _z are the mass of the car and the moment of inertia around the oz axis respectively, a is the front wheel axle of the car The distance from the center to the center of mass, b is the distance from the center of the rear wheel axle to the center of mass, δ _f is the steering angle of the front wheels, C _d is the air resistance coefficient, A _f is the front area of the vehicle, ρ _a is the air density, and F _tf is The longitudinal force acting on a single front wheel, F _tr is the longitudinal force acting on a single rear wheel, F _sf is the lateral force acting on a single front wheel, F _sr is the lateral force acting on a single rear wheel .

对于行驶在一般道路交通环境的车辆，通常可将作用在各轮上的侧向力表示为For a vehicle running in a general road traffic environment, the lateral force acting on each wheel can usually be expressed as

F_sf＝C_αfα_f，F_sr＝C_αrα_r (2)F _sf =C _αf α _f , F _sr =C _αr α _r (2)

式(2)中，C_αf、C_αr分别表示前、后轮胎的侧偏刚度，α_f、α_r分别表示前、后轮胎的侧偏角且可表示为In formula (2), C _αf and C _αr represent the cornering stiffness of the front and rear tires respectively, and α _f and α _r represent the slip angles of the front and rear tires respectively, which can be expressed as

${α α}_{f f} = = {δ δ}_{f f} - - \frac{{v v}_{y the y} + + a a {ω ω}_{z z}}{{v v}_{x x}},,$ ${α α}_{r r} = = \frac{b b {ω ω}_{z z} - - {v v}_{y the y}}{{v v}_{x x}} - - - - - - ((33))$

将式(2)、(3)代入式(1)，并考虑到δ_f通常是小角度，即sinδ_f≈δ_f、cosδ_f≈1且忽略二阶及以上的高阶微量，经整理后可得Substituting equations (2) and (3) into equation (1), and considering that δ _f is usually a small angle, that is, sinδ _f ≈ δ _f , cosδ _f ≈ 1 and ignoring the second-order and above high-order traces, after sorting out Available

${\overset{\cdot &Center Dot;}{v v}}_{x x} = = \frac{11}{m m} [[m m {v v}_{y the y} {ω ω}_{z z} + + 22 \frac{{v v}_{y the y} + + a a {ω ω}_{z z}}{{v v}_{x x}} {C C}_{αf αf} {δ δ}_{f f} - - \frac{11}{22} {C C}_{d d} {A A}_{f f} {ρ ρ}_{a a} {v v}_{x x}^{22}]] + + \frac{22}{m m} (({F f}_{tf tf} + + {F f}_{tr tr}))$

${\overset{\cdot \cdot}{ω ω}}_{z z} = = \frac{11}{{I I}_{z z}} [[22 a a (({δ δ}_{f f} - - \frac{(({v v}_{y the y} + + a a {ω ω}_{z z}))}{{v v}_{x x}})) {C C}_{αf αf} - - 22 b b {C C}_{αr αr} \frac{((b b {ω ω}_{z z} - - {v v}_{y the y}))}{{v v}_{x x}}]] + + \frac{22 a a}{{I I}_{z z}} {F f}_{tf tf} {δ δ}_{f f} - - - - - - ((44))$

${\overset{\cdot &Center Dot;}{v v}}_{y the y} = = \frac{11}{m m} [[- - m m {v v}_{x x} {ω ω}_{z z} + + 22 (({δ δ}_{f f} - - \frac{(({v v}_{y the y} + + a a {ω ω}_{z z}))}{{v v}_{x x}})) {C C}_{αf αf} + + 22 {C C}_{αr αr} \frac{b b {ω ω}_{z z} - - {v v}_{y the y}}{{v v}_{x x}}]] + + \frac{22}{m m} {F f}_{tf tf} {δ δ}_{f 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 TheirInfluence 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分别表示前或后轮轴)，其计算与车辆的加减速状况密切相关，具体为The front wheel steering angle δ _f in formula (4) can be determined by dividing the steering wheel angle δ measured by the steering wheel angle sensor by the steering transmission ratio q _t from the steering wheel to the front wheels (ie δ _f =δ/q _t ). For the tire longitudinal forces F _tf and F _tr in formula (4), the present invention adopts the Dugoff nonlinear tire model to estimate and determine [references can be made to: Dugoff H., Fancher PS, 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]. For this reason, the longitudinal slip rate i _sj (j=f, r) of the vehicle is introduced (that is, it can be divided into the longitudinal slip rate i _sf of the front axle and the longitudinal slip rate _isr of the rear axle, that is, the subscript j in the present invention is taken as f or r respectively represent the front or rear axle), its calculation is closely related to the acceleration and deceleration of the vehicle, specifically as

且j＝f，r (5) and 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))，其计算公式如下In formula (5), R represents the radius of the tire of the wheel (usually, it can be considered that the tire radii of the four wheels are the same), v _tf and v _tr represent the speed along the direction of the tire on the front and rear axles respectively (for the convenience of marking, v _tf and _vtr can be collectively recorded as v _tj (j=f, r)), ω _f represents the rotational angular velocity of the two wheels on the front axle equivalently converted to the rotational angular velocity on the front axle, and ω _r represents the two wheels on the rear axle The rotational angular velocity of the wheel is equivalently converted to the rotational angular velocity on the rear axle (ω _f and ω _r can be collectively recorded as ω _j (j=f, r)), and its calculation formula is as follows

${ω ω}_{f f} = = \frac{11}{22} (({ω ω}_{fR f} + + {ω ω}_{fL f}))$

(6)(6)

${ω ω}_{r r} = = \frac{11}{22} (({ω ω}_{rR R} + + {ω ω}_{rL R}))$

式(6)中，ω_fL、ω_fR、ω_rL和ω_rR分别表示左前轮、右前轮、左后轮和右后轮的旋转角速度，通过利用四个轮速传感器测量获得。In Equation (6), ω _fL , ω _fR , ω _rL and ω _rR represent the rotational angular velocities of the left front wheel, right front wheel, left rear wheel and right rear wheel, respectively, which are obtained by using four wheel speed sensors.

此外，根据图1所示的运动关系，v_tj(j＝f，r)可按下式确定In addition, according to the kinematic relationship shown in Figure 1, v _tj (j=f, r) can be determined by the following formula

(7)(7)

v_tr＝v_x v _tr =v _x

根据Dugoff轮胎模型[可参考文献：Dugoff H.，Fancher P.S.，Segel L..AnAnalysis of Tire Traction Properties and Their Influence on Vehicle DynamicPerformance.SAE Transactions，79：341-366，1970.SAE Paper No.700377]，轮胎纵向力F_tf和F_tr可通过下式来确定According to the Dugoff tire model [References: Dugoff H., Fancher PS, Segel L..AnAnalysis of Tire Traction Properties and Their Influence on Vehicle DynamicPerformance.SAE Transactions, 79:341-366, 1970.SAE Paper No.700377], The tire longitudinal forces F _tf and F _tr can be determined by the following formula

${F f}_{tj tj} = = \frac{{C C}_{tj tj} {i i}_{sj sj}}{11 - - {i i}_{sj sj}} {f f}_{t t} (({p p}_{j j})) ((j j = = f f,, r r)) - - - - - - ((88))$

式(8)中，C_tf和C_tr分别表示单个前、后轮胎的纵向刚度(可统记为C_tj(j＝f，r))，变量p_j(j＝f，r)和函数f_t(p_j)(j＝f，r)由以下式子确定或定义In formula (8), C _tf and C _tr respectively represent the longitudinal stiffness of a single front and rear tire (which can be collectively denoted as C _tj (j=f, r)), the variable p _j (j=f, r) and the function f _t (p _j )(j=f, r) is determined or defined by the following formula

${p p}_{j j} = = \frac{μ μ {F f}_{zj zj} ((11 - - {ϵ ϵ}_{r r} {v v}_{x x} \sqrt{{i i}_{sj sj}^{22} + + {tan the tan}^{22} {α α}_{j j}})) ((11 - - {i i}_{sj sj}))}{22 \sqrt{{C C}_{tj tj}^{22} {i i}_{sj sj}^{22} + + {C C}_{αj αj}^{22} {tan the tan}^{22} {α α}_{j j}}} j j = = f f,, r r - - - - - - ((99))$

${f f}_{t t} (({p p}_{j j})) = = \{\begin{matrix} {p p}_{j j} ((22 - - {p p}_{j j})) & {p p}_{j j} < < 11 \\ 11 & {p p}_{j j} &GreaterEqual; &Greater Equal; 11 \end{matrix} j j = = f f,, r r - - - - - - ((1010))$

式(9)和(10)中，μ表示轮胎和地面间的垂向摩擦系数，ε_r表示道路附着衰减因子，F_zj(j＝f，r)表示分配到前或后轮轴上的垂向载荷且可按下式计算In formulas (9) and (10), μ represents the vertical friction coefficient between the tire and the ground, ε _r represents the road adhesion attenuation factor, and F _zj (j=f, r) represents the vertical load and can be calculated as follows

式(11)中，g表示重力加速度。In formula (11), g represents the gravitational acceleration.

对于式(4)描述的模型，它是一个具有3自由度的非线性车辆动力学模型，不同于经常所采用的2自由度线性车辆模型。在经常采用的2自由度线性车辆模型中，车辆的纵向前进速度被认为是定常的，车辆模型仅是关于侧向速度和横摆角速度的线性微分方程。因此，2自由度线性车辆模型一般只适合前向速度不变或变化缓慢的运行情况(机动性较低)，而对于较高机动运行情况(即需要频繁转向以及加减速的情形)，该模型存在较大的建模误差。而本发明所采用的3自由度非线性模型对车辆的纵向前进速度并无定常的限定，故即可适应一般机动环境也可适应较高机动环境下车辆运行状态的准确估计。因此，本发明将根据式(4)建立卡尔曼滤波的系统状态方程。For the model described by formula (4), it is a nonlinear vehicle dynamics model with 3 degrees of freedom, which is different from the commonly used 2 degrees of freedom linear vehicle model. In the commonly adopted 2-DOF linear vehicle model, the longitudinal forward velocity of the vehicle is considered constant, and the vehicle model is only a linear differential equation for lateral velocity and yaw rate. Therefore, the 2-degree-of-freedom linear vehicle model is generally only suitable for running situations where the forward speed is constant or changes slowly (low maneuverability). There are large modeling errors. However, the 3-degree-of-freedom nonlinear model adopted by the present invention has no constant limitation on the longitudinal forward speed of the vehicle, so it can be adapted to the general maneuvering environment and also can adapt to the accurate estimation of the vehicle running state in a higher maneuvering environment. Therefore, the present invention will establish the system state equation of the Kalman filter according to formula (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]′，则离散化后的卡尔曼滤波的系统状态方程的矩阵形式可表示为：It should be noted that in the actual Kalman filter recursion process, a discretized Kalman filter model is required. For this purpose, discretize the system of differential equations in formula (4), and define the state vector as X=[x ₁ x ₂ x ₃ ]′ and x ₁ =v _x , x ₂ =ω _z , x ₃ =v _y , that is, X=[v _x ω _z v _y ]′ (the superscript ′ in the present invention represents the transposition of the matrix), the input vector outside the system is defined as U=[u ₁ u ₂ u ₃ ]′ and u ₁ = δ _f , u ₂ ＝F _tf , u ₃ ＝F _tr , that is, U=[δ _f F _tf F _tr ]′, then the matrix form of the discretized Kalman filter system state equation can be expressed as:

X(k)＝f(X(k-1)，U(k-1)，W(k-1)，γ(k-1)) (12)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)表示系统外输入对应的零均值高斯白噪声向量且 $γ = {[\begin{matrix} w_{δ_{f}} & w_{F_{tf}} & w_{F_{tr}} \end{matrix}]}^{'},$ 其中及分别表示直测或估计的系统外输入δ_f、F_tf及F_tr对应的零均值高斯白噪声，这些白噪声隐含在系统状态函数f的三个系统外输入里面(即不显性地出现在f中)；非线性的系统状态函数向量为In the formula, k represents the time of discretization; W(k-1) represents the zero-mean system Gaussian white noise vector and w=[w ₁ w ₂ w ₃ ]′, where w ₁ , w ₂ and w ₃ represent three Gaussian white noise component of the system; γ(k-1) represents the zero-mean Gaussian white noise vector corresponding to the input outside the system and $γ = {[\begin{matrix} w_{δ_{f}} & w_{f_{tf}} & w_{f_{tr}} \end{matrix}]}^{'},$ in and respectively represent the zero-mean Gaussian white noise corresponding to the directly measured or estimated external input δ _f , F _tf and F _tr , these white noises are implied in the three external inputs of the system state function f (that is, do not appear explicitly in f); the nonlinear system state function vector is

其中，in,

且T表示离散的周期(在本发明中，根据测量传感器特性，T的典型值可取为10毫秒、20毫秒、50毫秒、100毫秒等)；W对应的系统噪声协方差阵Q(k-1)为 $Q (k - 1) = [\begin{matrix} {σ_{w_{1}}}^{2} & 0 & 0 \\ 0 & {σ_{w_{2}}}^{2} & 0 \\ 0 & 0 & {σ_{w_{3}}}^{2} \end{matrix}],$ 其中及分别表示系统高斯白噪声w₁、w₂及w₃对应的方差；γ(k-1)对应的系统外部输入噪声的协方差阵Γ(k-1)为 $Γ (k - 1) = [\begin{matrix} {σ_{δ_{f}}}^{2} & 0 & 0 \\ 0 & {σ_{F_{tf}}}^{2} & 0 \\ 0 & 0 & {σ_{F_{tr}}}^{2} \end{matrix}],$ 其中及分别表示及对应的方差。And T represents a discrete period (in the present invention, according to the measurement sensor characteristics, the typical value of T can be taken as 10 milliseconds, 20 milliseconds, 50 milliseconds, 100 milliseconds, etc.); W corresponds to the system noise covariance matrix Q(k-1 )for $Q (k - 1) = [\begin{matrix} {σ_{w_{1}}}^{2} & 0 & 0 \\ 0 & {σ_{w_{2}}}^{2} & 0 \\ 0 & 0 & {σ_{w_{3}}}^{2} \end{matrix}],$ in and represent the variances corresponding to the system Gaussian white noise w ₁ , w ₂ and w ₃ respectively; the covariance matrix Γ(k-1) of the external input noise of the system corresponding to γ(k-1) is $Γ (k - 1) = [\begin{matrix} {σ_{δ_{f}}}^{2} & 0 & 0 \\ 0 & {σ_{f_{tf}}}^{2} & 0 \\ 0 & 0 & {σ_{f_{tr}}}^{2} \end{matrix}],$ in and Respectively and The corresponding variance.

建立车辆运行状态估计的卡尔曼滤波模型的系统状态方程后，下面讨论如何建立其观测方程。从运动学角度，图1所示的车辆运动实际上是一个平面复合运动(纵向运动、侧向运动和横摆转动的复合)，故根据平面复合运动关系，可得After establishing the system state equation of the Kalman filter model for vehicle operating state estimation, how to establish its observation equation is discussed below. From the perspective of kinematics, the vehicle motion shown in Figure 1 is actually a plane compound motion (the compound of longitudinal motion, lateral motion and yaw rotation), so according to the relationship of plane compound motion, we can get

${V V}_{RL RL} = = {v v}_{x x} + + \frac{{T T}_{W W}}{22} {ω ω}_{z z}$

(13)(13)

${V V}_{RR RR} = = {v v}_{x x} - - \frac{{T T}_{W W}}{22} {ω ω}_{z z}$

式中，V_RL和V_RR分别代表左后轮和右后轮(即两个非转向轮)的车轮线速度，T_W是后轮轴上两个后轮间的轮距。In the formula, V _RL and V _RR represent the wheel linear speeds of the left rear wheel and the right rear wheel (ie, two non-steering wheels), respectively, and T _W is the wheelbase between the two rear wheels on the rear axle.

对式(13)重新整理，可以得到Rearranging formula (13), we can get

v_x＝(V_RL+V_RR)/2v _x = (V _RL +V _RR )/2

ω_z＝(V_RL-V_RR)/T_W (14)ω _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的高斯白噪声)。It should be pointed out that the wheel linear velocity of the left rear wheel and the right rear wheel can be obtained through two wheel speed sensors installed on the rear wheel axle, that is, the angular velocity measured by the two wheel speed sensors on the rear wheel axle is multiplied by the tire radius to obtain . Considering the measurement noise of the wheel speed sensor, V _{RL_m} = R · ω _rL and V _{RR_m} = R · ω _rR , where V _{RL_m} and V _{RR_m} represent the noise-containing measurement values of V _RL and V _RR respectively. In addition, V _{RL_m} and V _{RR_m} can also be expressed as in and represent the additive measurement noises of the wheel linear velocities of the left rear wheel and the right rear wheel respectively (both can be modeled as Gaussian white noise with a mean value of 0).

在本发明中，将纵向前进速度和横摆角速度作为卡尔曼滤波模型的观测量。由于纵向前进速度和横摆角速度同时又是上述建立的卡尔曼滤波模型的两个状态，故不难建立滤波系统的观测方程，其离散化后的矩阵形式为In the present invention, the longitudinal forward speed and the yaw rate are taken as the observations of the Kalman filter model. Since the longitudinal forward velocity and the yaw angular velocity are two states of the Kalman filter model established above at the same time, it is not difficult to establish the observation equation of the filter system, and its discretized matrix form is

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

式(15)中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量，且 $Z (k) = [\begin{matrix} v_{x_m} (k) \\ ω_{z_m} (k) \end{matrix}]$ 、 $H (k) = [\begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \end{matrix}]$ 、 $V = [\begin{matrix} n_{v_{x}} \\ n_{ω_{z}} \end{matrix}],$ 而v_{x_m}(k)和ω_{z_m}(k)分别为通过轮速传感器测量获得的纵向前进速度和横摆角速度(即利用后轮轴上两个轮速传感器测得的角速度乘以轮胎半径得到左后轮和右后轮的车轮线速度，进而利用式(14)获得)，即v_{x_m}(k)和ω_{z_m}(k)分别表示v_x和ω_z的含有噪声的测量值且表示通过轮速传感器测量获得的车辆纵向前进速度的观测噪声且是均值为0、方差为的高斯白噪声，表示通过轮速传感器测量获得的横摆角速度的观测噪声且是均值为0、方差为的高斯白噪声；故V对应的观测噪声方差阵R可表示为 $R = [\begin{matrix} {σ_{v_{x}}}^{2} & 0 \\ 0 & {σ_{ω_{z}}}^{2} \end{matrix}] .$ In formula (15), Z is the observation vector, H is the observation array, V represents the zero-mean observation white noise vector uncorrelated with W, and $Z (k) = [\begin{matrix} v_{x_m} (k) \\ ω_{z_m} (k) \end{matrix}]$ , $h (k) = [\begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \end{matrix}]$ , $V = [\begin{matrix} {no}_{v_{x}} \\ {no}_{ω_{z}} \end{matrix}],$ And v _{x_m} (k) and ω _{z_m} (k) are the longitudinal forward velocity and yaw angular velocity measured by the wheel speed sensor respectively (that is, the angular velocity measured by the two wheel speed sensors on the rear axle multiplied by the tire radius to obtain the left rear The wheel linear velocity of the wheel and the right rear wheel, and then use the formula (14) to obtain), that is, v _{x_m} (k) and ω _{z_m} (k) represent the noise-containing measured values of v _x and ω _z respectively and represents the observed noise of the vehicle's longitudinal forward velocity measured by the wheel speed sensor and is a mean of 0 and a variance of Gaussian white noise, represents the observation noise of the yaw rate obtained by the wheel speed sensor measurement and is a mean of 0 and a variance of Gaussian white noise; so the observation noise variance matrix R corresponding to V can be expressed as $R = [\begin{matrix} {σ_{v_{x}}}^{2} & 0 \\ 0 & {σ_{ω_{z}}}^{2} \end{matrix}] .$

对于式(12)描述的系统状态方程和式(15)描述的测量方程，可运用卡尔曼滤波理论，建立起滤波递推估计过程。但注意到式(12)所示的状态方程为非线性方程，在应用卡尔曼滤波计算时，需先进行线性化处理，将系统方程在附近按泰勒级数展开(本发明中用表示状态X的滤波计算值)，保留一阶微量、忽略高阶微量后再进行滤波递推计算，即需按照扩展卡尔曼滤波过程进行滤波递推。根据扩展卡尔曼滤波理论，可建立本发明所涉及的扩展卡尔曼滤波的一般递推过程(该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新)：For the system state equation described by equation (12) and the measurement equation described by equation (15), the Kalman filter theory can be used to establish a filter recursive estimation process. But note that the state equation shown in formula (12) is a nonlinear equation, when applying the Kalman filter calculation, it needs to be linearized first, and the system equation in Nearby is expanded by Taylor series (used in the present invention Indicates the filtering calculation value of state X), retaining the first-order trace and ignoring the higher-order trace before performing the filter recursion calculation, that is, the filter recursion needs to be performed according to the extended Kalman filter process. According to the extended Kalman filter theory, the general recursive process of the extended Kalman filter involved in the present invention can be established (this recursive process includes time update and measurement update, the first two steps of the following recursive process are time update, and the remaining three step for measurement update):

时间更新：Time update:

状态一步预测方程 $\hat{X} (k, k - 1) = f (X (k - 1), U (k - 1), 0,0)$ State one-step prediction equation $\hat{x} (k, k - 1) = f (x (k - 1), u (k - 1), 0,0)$

一步预测误差方差阵P(k，k-1)One-step prediction error variance matrix P(k, 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)可分别通过下面的式子求得Among them, A is the Jacobian matrix (Jacobian) of the partial derivative of the system state function vector f with respect to the state vector X, and B is the Jacobian matrix (Jacobian) of the partial derivative of the system state function vector f with respect to the external input vector U, that is, the matrix A The elements A _{[i, j]} and B _{[i, j] (i=1, 2, 3 j=1, 2, 3) of the i-th row and j-column element A [i, j] of} B and B can be obtained by the following formula respectively

具体而言，根据式(12)，各矩阵元素的取值如下Specifically, according to formula (12), the values of each matrix element are as follows

B_[2，3]＝B_[3，3]＝0B _[2,3] =B _[3,3] =0

测量更新：Measurement update:

滤波增益矩阵k(k) K(k)＝P(k，k-1)·H′(k)·[H(k)P(k，k-1)H′(k)+R(k)]^-1 Filter gain matrix k(k) K(k)=P(k,k-1) H'(k)[H(k)P(k,k-1)H'(k)+R(k) ] ^-1

状态估计 $\hat{X} (k) = \hat{X} (k, k - 1) + K (k) [Z (k) - H (k) \hat{X} (k, k - 1)]$ state estimation $\hat{x} (k) = \hat{x} (k, k - 1) + K (k) [Z (k) - h (k) \hat{x} (k, k - 1)]$

估计误差方差阵P(k) P(k)＝[I-K(k)·H(k)]·P(k，k-1)且I为3×3单位阵Estimated error variance matrix P(k) P(k)=[I-K(k) H(k)] P(k, k-1) and I is a 3×3 unit matrix

注意到上述扩展卡尔曼滤波递推过程在测量更新过程中(即计算k(k)时)存在着矩阵的求逆运算。矩阵求逆时，计算量大且容易造成数值计算的不稳定。对此，本发明在测量更新时不直接采用矩阵求逆的方法，而采用标量化处理(scalar measurement processing)方法。具体而言，时间更新过程可按照上述滤波过程进行，而测量更新按以下改进的递推算法进行：It is noticed that in the recursive process of the above-mentioned extended Kalman filter, there is an inversion operation of the matrix during the measurement update process (that is, when k(k) is calculated). When inverting a matrix, the amount of calculation is large and it is easy to cause the instability of numerical calculation. For this, the present invention does not directly use the matrix inversion method when updating the measurement, but uses the scalar measurement processing method. Specifically, the time update process can be performed according to the above filtering process, while the measurement update can be performed according to the following improved recursive algorithm:

P_i+1＝(I-K_i·H_{r_i})·P_i P _i+1 ＝(IK _i ·H _{r_i} )·P _i

最终可得P(k)＝P₃， Finally, P(k)=P ₃ can be obtained,

在上述滤波递推计算过程中，可确定汽车在每个时刻的汽车纵向前进速度v_x(k)、横摆角速度ω_z(k)和侧向速度v_y(k)，进而根据下式可确定每个时刻的质心侧偏角In the above filter recursive calculation process, the car's longitudinal forward speed v _x (k), yaw rate ω _z (k) and lateral speed v _y (k) at each moment can be determined, and then according to the following formula Determining the sideslip angle of the center of mass at each moment

β(k)＝arctan[v_y(k)/v_x(k)] (16)β(k)=arctan[v _y (k)/v _x (k)] (16)

实施实例2Implementation example 2

为检验本发明提出的基于改进的扩展卡尔曼滤波的车辆运行状态估计方法的实际效果，在专业的汽车动力学仿真软件CarSim上进行了仿真验证实验。In order to test the actual effect of the vehicle running state estimation method based on the improved extended Kalman filter proposed by the present invention, a simulation verification experiment is carried out on the professional vehicle dynamics simulation software CarSim.

CarSim是由美国MSC(Mechanical Simulation Corporation)公司开发的专门针对车辆动力学的仿真软件，目前已被国际上众多的汽车制造商、零部件供应商所采用，被广泛地应用于现代汽车控制系统的商业开发，已成为汽车行业的标准软件，享有很高的声誉。Carsim内的车辆动力学模型是通过分别对汽车的车体、悬架、转向、制动等各子系统以及各个轮胎的高逼真建模来实现的，具有很高的自由度，能够提供非常接近实际的准确的车辆运行状态信息，因此，Carsim输出的车辆运行状态信息可作为车辆的参考输出。CarSim is a simulation software specially designed for vehicle dynamics developed by MSC (Mechanical Simulation Corporation) in the United States. Commercially developed, it has become standard software in the automotive industry and enjoys a high reputation. The vehicle dynamics model in Carsim is realized through the high-fidelity modeling of the car body, suspension, steering, braking and other subsystems, as well as each tire. It has a high degree of freedom and can provide a very close The actual and accurate vehicle running status information, therefore, the vehicle running status information output by Carsim can be used as the reference output of the vehicle.

为检验本发明提出的算法在较高机动环境下的估计效果，仿真实验中设置汽车的方向盘转角δ按幅值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所示。In order to test the estimation effect of the algorithm proposed by the present invention in a relatively high maneuvering environment, the steering wheel angle δ of the automobile is set to change according to the sinusoidal law with an amplitude of 60 ^° in the simulation experiment, and the longitudinal forward speed of the automobile is also constantly accelerating and braking. Changes such as dynamic deceleration and constant speed, the specific change process of the steering wheel angle and longitudinal forward speed over time are shown in Figure 2, and the simulation time is set to 100 seconds (s). The vehicle used is a four-wheel vehicle with front wheel steering, and the main parameters are as follows: m=960 (kg), I _z =1382 (kg·m ² ), a=0.948 (m), b=1.422 (m), C _αf =C _αr =25692 (Newton/rad), T _w =1.390 (meter). Set the linear velocity of the four wheels (obtained by multiplying the angular velocity measured by the wheel speed sensor by the tire radius) and the measurement noise is Gaussian white noise with a mean value of 0 and a standard deviation of 0.04 (m/s). The measurement noise is Gaussian white noise with a mean value of 0 and a standard deviation of 0.0873 (radian). The standard deviations of the zero-mean Gaussian white noise of the Kalman filter system are and The standard deviations of the zero-mean Gaussian white noise of the three outer inputs of the Kalman filter are and The standard deviations of the zero-mean Gaussian white noise of the two observations of the Kalman filter are and The relevant results are shown in Table 1 and Figures 3 to 4 .

表1列出了对于整个过程利用直测法和本发明方法推算车辆运行状态的统计结果对比，表中的误差均是相对于Carsim输出的相应参考值而言的(如直测法的纵向前进速度误差就表示利用直测法推算的纵向前进速度相对于Carsim输出的纵向前进速度参考值的误差)。另外需指出的是，上述两种方法的具体含义如下：直测法是指通过直接测量换算得到的纵向前进速度和横摆角速度，即利用后轮轴上两个轮速传感器测得的角速度乘以轮胎半径得到左后轮和右后轮的车轮线速度，进而利用实施实例1中的式(14)直接推算得到的纵向前进速度和横摆角速度；本发明方法是指利用本发明提出的改进扩展卡尔曼滤波估计方法来推算车辆各运行状态的方法。Table 1 has listed the statistic result comparison that utilizes direct measurement method and the method of the present invention to infer vehicle running state for the whole process, and the error in the table is all relative to the corresponding reference value of Carsim output (as the vertical advance of direct measurement method The speed error means the error of the longitudinal forward speed calculated by the direct measurement method relative to the reference value of the longitudinal forward speed output by Carsim). In addition, it should be pointed out that the specific meanings of the above two methods are as follows: the direct measurement method refers to the longitudinal forward velocity and yaw angular velocity obtained through direct measurement and conversion, that is, the angular velocity measured by the two wheel speed sensors on the rear axle multiplied by The tire radius obtains the wheel linear velocity of the left rear wheel and the right rear wheel, and then utilizes the formula (14) in the implementation example 1 to directly calculate the longitudinal forward velocity and the yaw rate; The Kalman filter estimation method is used to estimate the various operating states of the vehicle.

表1两种方法推算效果的对比表Table 1 Comparison table of the calculation effects of the two methods

表中“--”表示直测法无法推算的项"--" in the table indicates items that cannot be calculated by direct measurement

图3给出了本发明方法估计的质心侧偏角β的结果曲线(图中以EKF点划虚线标示)，以及相应的Carsim的参考输出值(图中以Carsim实黑线标示)。图4则给出了本发明方法估计的β相对于Carsim输出的β参考值的误差曲线。Fig. 3 shows the result curve of the side slip angle β estimated by the method of the present invention (marked by EKF dotted line in the figure), and the corresponding reference output value of Carsim (marked by the solid black line of Carsim in the figure). Fig. 4 shows the error curve of β estimated by the method of the present invention relative to the β reference value output by Carsim.

由表1的对比(尤其是标准差)以及图3～图4，可以看出本发明方法相对于直测法在纵向前进速度和横摆角速度的推算方面精度有了大幅的提高。另外，根据表1及图3～图4，还可以看出本发明方法在侧向速度和质心侧偏角的估计方面也具有很高的精度。From the comparison in Table 1 (especially the standard deviation) and Figures 3 to 4, it can be seen that the accuracy of the method of the present invention has been greatly improved compared with the direct measurement method in the estimation of longitudinal forward velocity and yaw angular velocity. In addition, according to Table 1 and Figures 3 to 4, it can also be seen that the method of the present invention also has high accuracy in estimating the lateral velocity and the sideslip angle of the center of mass.

综上，即使在较高机动运行环境下，本发明提出的基于改进扩展卡尔曼滤波的车辆运行状态估计方法能够准确地估计出车辆纵向前进速度、侧向速度、横摆角速度以及质心侧偏角等信息，这些信息可满足有关汽车主动安全控制的需要。In summary, even in a high maneuvering environment, the method for estimating the vehicle operating state based on the improved extended Kalman filter proposed by the present invention can accurately estimate the vehicle's longitudinal forward velocity, lateral velocity, yaw rate, and side slip angle of the center of mass and other information, which can meet the needs of active safety control of automobiles.

Claims

1. A vehicle running state estimation method based on improved extended Kalman filtering is characterized in that: the method is aimed at a front wheel steering four-wheel automobile, a nonlinear whole automobile dynamic model and a tire longitudinal force model are established, external input quantity and observed quantity of a filter system are determined and established by utilizing automobile wheel speed and steering wheel corner sensor information, and then estimation of automobile longitudinal forward speed, yaw rate, lateral speed and centroid lateral deviation angle information is achieved through the proposed improved extended Kalman filter recursion algorithm; the filtering system is an extended Kalman filtering system, and the following system is the extended Kalman filtering system;

the method comprises the following specific steps:

1) establishing an equation of state and an observation equation of the extended Kalman filtering:

establishing a three-degree-of-freedom automobile nonlinear dynamics model, namely establishing a system state equation of extended Kalman filtering, wherein the matrix form of the discretized state equation of Kalman filtering is expressed as follows:

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

in the formula (1), k represents a discretization time;

the system state vector is X ═ X₁ x₂ x₃]' and x₁＝v_x，x₂＝ω_z，x₃＝v_yI.e. X ═ v_x ω_z v_y]′，v_x、v_yAnd omega_zRespectively the longitudinal forward speed, the lateral speed and the yaw rate of the automobile; in the method for estimating the running state of the vehicle, the upper corner mark' represents the transposition of a pair matrix;

the system external input vector is U ═ U₁ u₂ u₃]' and u₁＝_f，u₂＝F_tf，u₃＝F_trI.e. U ═ 2-_f F_tf F_tr]′，_fIs the front wheel steering angle, F_tfIs a longitudinal force acting on a single front wheel, F_trIs a longitudinal force acting on the single rear wheel;

w (k-1) represents a zero-mean system gaussian white noise vector and W ═ W₁ w₂ w₃]', wherein w₁、w₂And w₃Respectively representing three system Gaussian white noise components;

gamma (k-1) represents the zero mean Gaussian white noise vector corresponding to the external input of the system and

whereinAndrepresenting direct or estimated system inputs, respectively_f、F_tfAnd F_trCorresponding zero-mean Gaussian white noises, which are hidden in three system external inputs of a system state function f; the nonlinear system state function vector is

Wherein,

at f₁、f₂And f₃In the above expression, m and I_zThe mass of the vehicle and the moment of inertia of the vertical axis bypassing the mass center are respectively, a is the distance from the wheel axle center of the front wheel of the vehicle to the mass center, b is the distance from the wheel axle center of the rear wheel of the vehicle to the mass center, and C_αf、C_αrRespectively, the cornering stiffness of the front and rear tires, C_dDenotes the coefficient of air resistance, A_fRepresenting the vehicle forward area, p_aRepresents the air density, T represents the discrete period; the system noise covariance matrix Q (k-1) corresponding to W is:

whereinAndrespectively represent the system Gaussian white noise w₁、w₂And w₃A corresponding variance; gamma (k-1) is the covariance matrix (k-1) of the external input noise of the system

WhereinAndrespectively representAnda corresponding variance;

the discretization matrix form of the observation equation of kalman filtering is:

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

in the formula (2), Z is an observation vector, H is an observation array, V represents a zero-mean observation white noise vector which is uncorrelated with W, and

wherein v is_{x_m}(k) And ω_{z_m}(k) Respectively through wheel speedThe sensor measures the obtained longitudinal forward speed and the yaw angular speed of the vehicle;an observation noise representing a longitudinal forward speed of the vehicle obtained by a wheel speed sensor measurement andis a mean of 0 and a variance ofThe white gaussian noise of (a) is,an observation noise representing a yaw rate obtained by measurement of a wheel speed sensor andis a mean of 0 and a variance ofWhite gaussian noise of (1); the observed noise variance matrix R corresponding to V may be expressed as

2) Performing an improved extended Kalman Filter recursion

For the state equation and the observation equation described by the formula (1) and the formula (2), a standard filtering recursion process is established by using the extended Kalman filtering theory, and the recursion process comprises time updating and measurement updating:

and (3) time updating:

one-step prediction equation of state:

\hat{X} (k, k - 1) = f (X (k - 1), U (k - 1), 0,0)

one-step prediction error variance matrix 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)

wherein, A is the Jacobian matrix of the system state function vector f for calculating the partial derivative of the state vector X, B is the Jacobian matrix of the system state function vector f for calculating the partial derivative of the external input vector U, that is, the ith row and jth column elements A of the matrixes A and B_[i,j]And B_[i,j](i-1, 2,3 j-1, 2,3) can be obtained by the following equation

<math> <mrow> <msub> <mi>A</mi> <mrow> <mo>[</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>]</mo> </mrow> </msub> <mo>=</mo> <mfrac> <msub> <mrow> <mo>&PartialD;</mo> <mi>f</mi> </mrow> <mi>i</mi> </msub> <msub> <mrow> <mo>&PartialD;</mo> <mi>x</mi> </mrow> <mi>j</mi> </msub> </mfrac> <mrow> <mo>(</mo> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>,</mo> <mi>U</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>,</mo> <mn>0,0</mn> <mo>)</mo> </mrow> <mo>,</mo> <mrow> <mo>(</mo> <mi>i</mi> <mo>=</mo> <mn>1,2,3</mn> <mi>j</mi> <mo>=</mo> <mn>1,2,3</mn> <mi></mi> <mo>)</mo> </mrow> </mrow> </math>

<math> <mrow> <msub> <mi>B</mi> <mrow> <mo>[</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>]</mo> </mrow> </msub> <mo>=</mo> <mfrac> <msub> <mrow> <mo>&PartialD;</mo> <mi>f</mi> </mrow> <mi>i</mi> </msub> <msub> <mrow> <mo>&PartialD;</mo> <mi>u</mi> </mrow> <mi>j</mi> </msub> </mfrac> <mrow> <mo>(</mo> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>,</mo> <mi>U</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>,</mo> <mn>0,0</mn> <mo>)</mo> </mrow> <mo>,</mo> <mrow> <mo>(</mo> <mi>i</mi> <mo>=</mo> <mn>1,2,3</mn> <mi>j</mi> <mo>=</mo> <mn>1,2,3</mn> <mi></mi> <mo>)</mo> </mrow> </mrow> </math>

Specifically, the values of the matrix elements are as follows:

B_[2,3]＝B_[3,3]＝0

and (3) measurement updating:

filter gain matrix k (k): k (k) ═ P (k, k-1) · H '(k) ·[ H (k) P (k, k-1) H' (k) + r (k)]^-1

And (3) state estimation:

\hat{X} (k) = \hat{X} (k, k - 1) + K (k) [Z (k) - H (k) \hat{X} (k, k - 1)]

estimation error variance matrix p (k): p (k) ([ I-k (k) · h (k)) ] · P (k, k-1) and I is a 3 × 3 unit array

In the actual recursion process, a scaling processing method is adopted for measurement updating; specifically, the time update process is performed according to the above-described filtering process, and the measurement update is performed according to the following modified recursion algorithm:

let P₁＝P(k,k-1)，Since the observation vector dimension is 2, it will beThe H (k), Z (k) and R (k) arrays are divided into two, i.e.

H (k) \hat{X} (k, k - 1) = [\begin{matrix} h_{r_1} \\ h_{r_2} \end{matrix}], H (k) = [\begin{matrix} H_{r_1} \\ H_{r_2} \end{matrix}], Z (k) = [\begin{matrix} Z_{1} \\ Z_{2} \end{matrix}], R (k) = [\begin{matrix} R_{1} & 0 \\ 0 & R_{2} \end{matrix}]

For i from 1 to 2, 2 recursion calculations were performed:

{\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 is finally obtained₃，

In the filter recursion calculation process, the longitudinal forward speed of the automobile at each moment can be determinedDegree v_x(k) Yaw angular velocity ω_z(k) And lateral velocity v_y(k) The centroid slip angle at each time instant can then be determined according to equation (3):

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

in the step 1) described above, the step of,

in the formula (1), the front wheel steering angle input outside the system of the extended Kalman filter system_fIs the steering wheel angle measured by the steering wheel angle sensor divided by the steering gear ratio q from the steering wheel to the front wheels_tTo determine; while the tire longitudinal force F_tfAnd F_trIs determined according to the Dugoff non-linear tire model;

by i_sj(j ═ f, r) denotes the vehicle longitudinal slip ratio, which can be further divided into the front wheel axle longitudinal slip ratio i_sfAnd longitudinal slip ratio i of rear axle_srThe lower corner mark j is f or r, f or r respectively represents a front or rear axle, i_sjThe calculation method comprises the following steps:

in the formula (4), R represents a radius of a tire for a wheel; v. of_tfAnd v_trRepresenting the speed in the direction of the tyre, v, on the front and rear axles, respectively_tfAnd v_trCan be uniformly denoted as v_tj(j＝f,r)；ω_fRepresenting the equivalent conversion of the rotation angular velocities of the two wheels on the front wheel shaft to the rotation angular velocity on the front wheel shaft; omega_rRepresenting the angular speed of rotation of two wheels on the rear axle, equivalent converted to the angular speed of rotation on the rear axle, omega_fAnd ω_rCan be uniformly recorded as omega_j(j ═ f, r) and

(5)

in the formula (5), ω_fL、ω_fR、ω_rLAnd ω_rRRespectively representing rotation angular velocities of the left front wheel, the right front wheel, the left rear wheel and the right rear wheel, obtained by measurement using four wheel speed sensors;

v_tj(j ═ f, r) can be determined by equation (6):

v_tf＝v_xcos_f+(v_y+aω_z)sin_f (6)

v_tr＝v_x

further, the tire longitudinal force F_tfAnd F_trCan be determined by the formula (7)

F_{tj} = \frac{C_{tj} i_{sj}}{1 - i_{sj}} f_{t} (p_{j}), (j = f, r) - - - (7)

In the formula (7), C_tfAnd C_trRespectively, the longitudinal stiffness of the single front and rear tires, denoted collectively as C_tj(j ═ f, r); variable p_j(j ═ f, r) and function f_t(p_j) (j ═ f, r) is determined by the following equation:

<math> <mrow> <msub> <mi>f</mi> <mi>t</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>p</mi> <mi>j</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>p</mi> <mi>j</mi> </msub> <mrow> <mo>(</mo> <mn>2</mn> <mo>-</mo> <msub> <mi>p</mi> <mi>j</mi> </msub> <mo>)</mo> </mrow> </mtd> <mtd> <msub> <mi>p</mi> <mi>j</mi> </msub> <mo><</mo> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <msub> <mi>p</mi> <mi>j</mi> </msub> <mo>&GreaterEqual;</mo> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <mi>j</mi> <mo>=</mo> <mi>f</mi> <mo>,</mo> <mi>r</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow> </math>

in the formulas (8) and (9), μ represents a vertical friction coefficient between the tire and the ground;_rrepresenting a road adhesion attenuation factor; alpha is alpha_f、α_rRespectively representing the slip angles of the front and rear tires, and collectively denoted as alpha_j(j ═ f, r), can be calculated as follows

And F_zj(j ═ f, r) denotes the vertical load distributed to the front or rear axles and can be calculated as follows

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

In the formula (11), g represents the gravitational acceleration;

the following relationships exist between the longitudinal forward speed and yaw rate of the vehicle and the speeds of the two non-steered rear wheels

v_x＝(V_RL+V_RR)/2

(12)

ω_z＝(V_RL-V_RR)/T_W

In formula (12), T_WIndicating the track width, V, between two rear wheels on the rear axle_RLAnd V_RRRespectively representing the linear speeds of the left rear wheel and the right rear wheel;

for the measured value v in equation (2)_{x_m}(k) And ω_{z_m}(k) V is obtained by multiplying the angular velocity measured by two wheel speed sensors on the rear wheel axle by the radius of the tire_{RL_m}＝R·ω_rLAnd V_{RR_m}＝R·ω_rR，V_{RL_m}And V_{RR_m}Respectively represent V_RLAnd V_RRThe measurement value containing noise, obtained by using the formula (12), i.e. v_{x_m}And ω_{z_m}Respectively represent v_xAnd ω_zContains noisy measured values and an observation noise representing a longitudinal advancing speed obtained by a wheel speed sensor measurement andis a mean of 0 and a variance ofThe white gaussian noise of (a) is,an observation noise representing a yaw rate obtained by measurement of a wheel speed sensor andis a mean of 0 and a variance ofWhite gaussian noise.

2. The method of claim 1 in which the discrete period T is typically 10 milliseconds, 20 milliseconds, 50 milliseconds or 100 milliseconds.