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

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 kind of travel condition of vehicle method of estimation based on improving EKF

Technical field

The present invention relates to a kind of travel condition of vehicle method of estimation based on improving EKF, its object is to utilize the EKF method improved to carry out suitable modeling to automobile dynamic quality process, obtain the travel condition of vehicle of automobile under higher motor-driven operation conditions, these states can be used for the relevant control of automobile active safety, there is the remarkable advantages such as precision is high, cost is low, real-time is good, belong to automobile active safety and measure and control field.

Background technology

Along with socioeconomic development, traffic safety problem becomes increasingly conspicuous, and becomes global problem.The whole world all can cause a large amount of personal casualty and property damage because of traffic accident every year, and the generation reducing traffic accident is all being made great efforts in countries in the world.In recent years, automobile active safety technology obtains and develops rapidly.Automobile active safety technology can prevent trouble before it happens, and initiatively avoids the generation of accident, has become one of topmost developing direction of modern automobile.Active safety technologies common at present mainly comprises anti-skid brake system (ABS), vehicle electric stability program (ESP), anti-slip regulation (TCS), automatically controlled Anti-slip regulation system (ASR), four-wheel steering stabilizing control system (4WS) etc.These systems are usually directed to the speed of motor tire, longitudinal speed of advance of automobile, side velocity, the measurement of the running state such as yaw velocity and side slip angle or estimation, and the measurement of these running statees can be used for follow-up automobile active safety control, therefore the driving safety of its precision direct relation automobile and stability, namely can above-mentioned active safety control system effectively be operated in and depend on travel condition of vehicle to a great extent and measure by real time, accurately or estimate.

At present, in field of automotive active safety, state of motion of vehicle is measured mainly through three kinds of methods or estimates.One is the onboard sensor (as inertial sensor and wheel speed sensors etc.) utilizing low cost, the reckoning of simple mathematics is carried out to its signal measured and obtains associated vehicle running state, this method cost is low, but because low cost sensor accuracy is poor and calculate process too simple and there is larger measured error, thus have impact on control effects.Two is utilize high-precision sensor directly to measure (as utilized photoelectricity fifth wheel gauge or high-precision global navigation satellite system GNSS to associated vehicle running state, especially high precision global position system GPS etc.), this method precision is high but expensive, cannot apply on a large scale.The third method is modelling tool, namely by carrying out kinematics or Dynamic Modeling to the operational process of automobile, simultaneously using onboard sensor (as wheel speed sensors, gyroscope, accelerometer and the GPS etc.) information about low cost as observation information, and then the estimation utilizing suitable filtering algorithm for estimating to realize motoring condition.The third method (i.e. modelling tool) can realize being difficult to straight estimation of measuring, the dimension that expanded state is estimated, also can improve about the straight precision measured, cost is lower simultaneously.But the modelling tool proposed at present is mainly based on the kinematics model of automobile or done the kinetic model compared with polytenization supposition to car load or tire, these models vehicle comparatively smooth running time can obtain good estimation effect and precision, but owing to being difficult to reflect that the actual Nonlinear dynamic behaviors of vehicle causes estimated accuracy lower under higher motor-driven operation conditions.

Summary of the invention

For realize under higher motor-driven operating mode to travel condition of vehicle accurate, reliably estimate, the present invention proposes a kind of based on improving the travel condition of vehicle method of estimation of EKF.The method that the present invention proposes determines Nonlinear Integer vehicle dynamics model closer to reality and longitudinal force of tire model for the higher motor-driven operation conditions of automobile, make full use of the vehicle-mounted wheel speed of low cost and steering wheel angle sensor information to set up outside input and the observed quantity of filtering system simultaneously, and then realize estimating that there is the features such as precision is high, cost is low, real-time is good to the filtering of the travel condition of vehicle such as automobile longitudinal speed of advance, yaw velocity, side velocity and side slip angle by the EKF recursive algorithm improved.

A kind of travel condition of vehicle method of estimation based on improving EKF, the present invention is directed to the front-wheel steering four-wheel automobile that application is more at present, for meeting automobile active safety control to the measurement of travel condition of vehicle and estimation needs, set up the Nonlinear Integer vehicle dynamics model and the longitudinal force of tire model that are applicable to higher motor-driven operating condition, make full use of outside input and observed quantity that the vehicle-mounted wheel speed of low cost and steering wheel angle sensor information determine to set up filtering system simultaneously, on this basis, realize automobile longitudinal speed of advance by the improvement EKF recursive algorithm proposed, yaw velocity, the accurate filtering of the information such as side velocity and side slip angle is estimated,

Concrete steps comprise:

1) equation of state and the observational equation of EKF is set up:

Set up the Vehicle Nonlinear kinetic model of three degree of freedom, namely set up the system state equation of EKF process, the matrix representation of the equation of state of the Kalman filtering after discretization is:

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

In formula, k represents the discretization moment;

State of the system vector is X=[x ₁x ₂x ₃] ' and x ₁=v _x, x ₂=ω _z, x ₃=v _y, i.e. X=[v _xω _zv _y] ', v _x, v _yand ω _zlongitudinal speed of advance of automobile, side velocity and yaw velocity respectively, superscript in the present invention ' represent matrix transpose;

The outer input vector of system is U=[u ₁u ₂u ₃] ' and u ₁=δ _f, u ₂=F _tf, u ₃=F _tr, i.e. U=[δ _ff _tff _tr] ', δ _ffront wheel steering angle, F _tfact on the longitudinal force on single front-wheel, F _tract on the longitudinal force on single trailing wheel;

W (k-1) represent zero-mean system Gaussian white noise vector and W=[w ₁w ₂w ₃] ', be w wherein ₁, w ₂and w ₃represent three system Gaussian white noise components respectively;

γ (k-1) represent system input corresponding zero mean Gaussian white noise vector outward and $\mathrm{\γ}={\left[\begin{array}{ccc}{w}_{{\mathrm{\δ}}_f}& w_{F_{\mathrm{tf}}}& w_{F_{\mathrm{tr}}}\end{array}\right]}^{\′},$ Wherein and represent that the straight system surveyed or estimate inputs δ outward respectively _f, F _tfand F _trcorresponding zero mean Gaussian white noise, three systems that these white noises lie in state of the system function f input the inside outward; Nonlinear state of the system functional vector is

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

Wherein,

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

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

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

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

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

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

At f ₁, f ₂and f ₃above-mentioned expression formula in, m and I _zbe the quality of vehicle and the rotor inertia walking around the vertical axle of barycenter respectively, a is the distance of vehicle front wheel shaft center to barycenter, and b is the distance of automobile back wheel wheel shaft center to barycenter, C _{α f}, C _{α r}represent the cornering stiffness of forward and backward tire respectively, C _drepresent aerodynamic drag factor, A _frepresent vehicle forward direction area, ρ _arepresent density of air, T represents the discrete cycle; The system noise covariance battle array Q (k-1) that W is corresponding is:

$Q(k-1)=\left[\begin{array}{ccc}{{\mathrm{\σ}}_{w_1}}^2& 0& 0\\ 0& {{\mathrm{\σ}}_{w_2}}^2& 0\\ 0& 0& {{\mathrm{\σ}}_{w_3}}^2\end{array}\right],$ Wherein and represent system Gaussian white noise w respectively ₁, w ₂and w ₃corresponding variance; The covariance matrix Γ (k-1) of its exterior input noise that γ (k-1) is corresponding is $\mathrm{\Γ}(k-1)=\left[\begin{array}{ccc}{{\mathrm{\σ}}_{{\mathrm{\δ}}_f}}^2& 0& 0\\ 0& {{\mathrm{\σ}}_{F_{\mathrm{tf}}}}^2& 0\\ 0& 0& {{\mathrm{\σ}}_{F_{\mathrm{tr}}}}^2\end{array}\right],$ Wherein and represent respectively and corresponding variance;

The discretization matrix form of the observational equation of Kalman filtering is:

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

In formula (2), Z is observation vector, and H is observation battle array, and V represents that white noise vector observed by mutual incoherent zero-mean with W, and $Z\left(k\right)=\left[\begin{array}{c}{v}_{x\_m}\left(k\right)\\ {\mathrm{\ω}}_{z\_m}\left(k\right)\end{array}\right]$ 、 $H\left(k\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]$ 、 $V=\left[\begin{array}{c}{n}_{v_x}\\ n_{{\mathrm{\ω}}_z}\end{array}\right],$ Wherein v _{x_m}(k) and ω _{z_m}k () is respectively and measures by wheel speed sensors the longitudinal direction of car speed of advance and yaw velocity that obtain; represent by wheel speed sensors measure obtain longitudinal direction of car speed of advance observation noise and be average be 0, variance is gaussian white noise, represent by wheel speed sensors measure obtain yaw velocity observation noise and be average be 0, variance is gaussian white noise; The observation noise variance matrix R that V is corresponding can be expressed as $R=\left[\begin{array}{cc}{{\mathrm{\σ}}_{v_x}}^2& 0\\ 0& {{\mathrm{\σ}}_{{\mathrm{\ω}}_z}}^2\end{array}\right];$

2) the EKF recursion improved is carried out

For the system state equation described by formula (1) and formula (2) and observational equation, use EKF theoretical, Criterion filtering recursive process, this recursive process comprises time renewal and measurement updaue:

Time upgrades:

State one-step prediction equation: $\hat{X}(k,k-1)=f(X(k-1),U(k-1),\mathrm{0,0})$

One-step prediction error covariance 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 state of the system functional vector f asks partial derivative Jacobi matrix to state vector X, and B is that state of the system functional vector f asks the Jacobi matrix of partial derivative to outside input vector U, i.e. the i-th row jth column element A of matrix A and B _{[i, j]}and B _{[i, j]}(i=1,2,3 j=1,2,3) can try to achieve respectively by formula below

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

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

Specifically, the value of each element of a matrix is as follows:

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

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

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

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

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

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

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

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

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

B _[2，3]＝B _[3，3]＝0

Measurement updaue:

Filtering gain matrix k (k): K (k)=P (k, k-1) H ' (k) [H (k) P (k, k-1) H ' (k)+R (k)] ^-1

State estimation: $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\hat{X}(k,k-1)\text{]}$

Estimation error variance battle array P (k): P (k)=[I-K (k) H (k)] P (k, k-1) and I is 3 × 3 unit matrix

In actual recursive process, measurement updaue adopts scalarization processing method.Specifically, time renewal process can be carried out according to above-mentioned filtering, and measurement updaue is undertaken by the recursive algorithm of following improvement:

Make P ₁=P (k, k-1), because observation vector dimension is 2, therefore will h (k), Z (k) and R (k) battle array are divided into two pieces, namely

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

For i from 1 to 2, carry out 2 recurrence calculation:

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

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

P _i+1＝(I-K _i·H _{r_i})·P _i

Finally can obtain P (k)=P ₃,

In above-mentioned filtering recurrence calculation process, the automobile longitudinal speed of advance v of automobile in each moment can be determined _x(k), yaw velocity ω _z(k) and side velocity v _y(k), and then the side slip angle that each moment can be determined according to formula (3):

β(k)＝arctan[v _y(k)/v _x(k)] (3)。

The representative value of discrete cycle T is 10 milliseconds, 20 milliseconds, 50 milliseconds or 100 milliseconds.

Described step 1) in,

In formula (1), the front wheel steering angle δ that the system of Kalman filtering inputs outward _f, be that the steering wheel angle δ that recorded by steering wheel angle sensor is divided by the steering gear ratio q from bearing circle to front-wheel _tdetermine; And longitudinal force of tire F _tfand F _tr, be determine according to the non-linear tire model of Dugoff;

Use i _sjnot only but also can be divided into front wheel spindle straight skidding rate i (j=f, r) represents longitudinal direction of car slip rate, _sfwith hind axle straight skidding rate i _sr, subscript j gets f or r, f or r represents front or rear wheel shaft respectively, i _sjmethod of calculating is:

and j=f, r (4),

In formula (4), R represents wheel tyre radius; v _tfand v _trrepresent the speed along tire direction on forward and backward wheel shaft respectively, v _tfand v _trcan unify to be designated as v _tj(j=f, r); ω _frepresent that the spin velocity equivalence conversion of two wheels on front wheel spindle is to the spin velocity on front wheel spindle; ω _rrepresent that on hind axle, two rotation of wheel cireular frequency equivalence conversions are to the spin velocity on hind axle, ω _fand ω _rcan unify to be designated as ω _j(j=f, r) and

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

(5)

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

In formula (5), ω _fL, ω _fR, ω _rLand ω _rRrepresent the spin velocity of the near front wheel, off front wheel, left rear wheel and off hind wheel respectively, measure by utilizing four wheel speed sensors and obtain;

V _tj(j=f, r) can determine by formula (6):

v _tf＝v _xcosδ _f+(v _y+aω _z)sinδ _f

(6)

v _tr＝v _x

And then, longitudinal force of tire F _tfand F _trdetermine by formula (7)

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

In formula (7), C _tfand C _trrepresent the longitudinal rigidity of single forward and backward tire respectively, unification is designated as C _tj(j=f, r); Variable p _j(j=f, r) sum functions f _t(p _j) (j=f, r) determined by following formula:

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

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

In formula (8) and (9), μ represents the vertical friction coefficient between tire and ground; ε _rrepresent road attachment decay factor; α _f, α _rrepresent the sideslip angle of forward and backward tire respectively, unification is designated as α _j(j=f, r), can be calculated as follows

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

And F _zj(j=f, r) represents the vertical load that is assigned on front or rear wheel shaft and can be calculated as follows

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

In formula (11), g represents acceleration due to gravity;

There is following relation in longitudinal direction of car speed of advance and yaw velocity and two non-speed of trailing wheel that turn to

v _x＝(V _RL+V _RR)/2

ω _z＝(V _RL-V _RR)/T _W(12)

In formula (12), T _wrepresent the wheelspan between two trailing wheels on hind axle, V _rLand V _rRrepresent the linear velocity of left rear wheel and off hind wheel respectively;

For the observed reading v in formula (2) _{x_m}(k) and ω _{z_m}k (), they are that the cireular frequency utilizing on hind axle two wheel speed sensors to record is multiplied by tire radius and obtains V _{rL_m}=R ω _rLand V _{rR_m}=R ω _rR, V _{rL_m}and V _{rR_m}represent V respectively _rLand V _rRcontaining noisy observed reading, and then formula (12) is utilized to obtain, i.e. v _{x_m}and ω _{z_m}represent v respectively _xand ω _zcontaining noisy observed reading and represent by wheel speed sensors measure obtain longitudinal speed of advance observation noise and be average be 0, variance is gaussian white noise, represent by wheel speed sensors measure obtain yaw velocity observation noise and be average be 0, variance is gaussian white noise.

Beneficial effect

1. the present invention proposes a kind of low cost, high precision, real-time good based on the travel condition of vehicle method of estimation improving EKF, can be used for automobile active safety control to the measurement of travel condition of vehicle with estimate needs.

2. method of the present invention for the higher motor-driven operating condition of automobile, propose on Nonlinear Integer vehicle dynamics model and longitudinal force of tire model basis, under higher motor-driven situation, still can obtain travel condition of vehicle information accurately.

3. the travel condition of vehicle method of estimation based on the EKF improved that the present invention proposes not only can significantly improve the precision that automobile longitudinal speed of advance and yaw velocity etc. are directly measured, and can realize being difficult to straight accurate estimation of measuring to side slip angle, side velocity etc.

4. the method that the present invention proposes has the advantages such as precision is high, cost is low, real-time is good.

Accompanying drawing explanation

Fig. 1. vehicle dynamic model

Fig. 2. the steering wheel angle (degree) of setting and longitudinal speed of advance (thousand ms/h of-Km/h) variation diagram in time

Fig. 3. side slip angle (radian-rad) curve and the partial enlarged drawing over time that the inventive method and Carsim export

Fig. 4. the curve of error of the side slip angle reference value that the side slip angle that the inventive method obtains exports relative to Carsim

Detailed description of the invention

Embodiment 1

Along with socioeconomic development, traffic safety problem becomes increasingly conspicuous, and becomes global problem.The whole world all can cause a large amount of personal casualty and property damage because of traffic accident every year, and the generation reducing traffic accident is all being made great efforts in countries in the world.In recent years, automobile active safety technology obtains and develops rapidly.Automobile active safety technology can prevent trouble before it happens, and initiatively avoids the generation of accident, has become one of topmost developing direction of modern automobile.Active safety technologies common at present mainly comprises anti-lock braking system in automobiles (ABS), vehicle electric stability program (ESP), anti-slip regulation (TCS), automatically controlled Anti-slip regulation system (ASR), four-wheel steering stabilizing control system (4WS) etc.These systems are usually directed to the speed of motor tire, longitudinal speed of advance of automobile, side velocity, the measurement of the running state such as yaw velocity and side slip angle or estimation, and the measurement of these running statees can be used for follow-up automobile active safety control, therefore the driving safety of its precision direct relation automobile and stability, namely can above-mentioned active safety control system effectively be operated in and depend on travel condition of vehicle to a great extent and measure by real time, accurately or estimate.

At present, in field of automotive active safety, state of motion of vehicle is measured mainly through following three kinds of methods or estimates:

One is the onboard sensor (as inertial sensor and wheel speed sensors etc.) utilizing low cost, carries out the reckoning of simple mathematics obtain associated vehicle running state to its signal measured.Such as, for automobile side slip angle, vertical and horizontal accelerometer can be utilized first to record acceleration/accel along both direction, and then integral operation obtains longitudinal speed of advance and side velocity respectively, and then can try to achieve side slip angle.Although this method cost is low, because low cost sensor accuracy is poor and calculate process too simple and there is larger measured error, thus have impact on control effects.

Two is utilize high-precision sensor directly to measure (as utilized photoelectricity fifth wheel gauge or high-precision global navigation satellite system GNSS to associated vehicle running state, especially high precision global position system GPS etc.), this method precision is high but expensive, cannot apply on a large scale.

The third method is modelling tool, namely by carrying out kinematics or Dynamic Modeling to the operational process of automobile, simultaneously using onboard sensor (as wheel speed sensors, gyroscope, accelerometer and the GPS etc.) information about low cost as observation information, and then the estimation utilizing suitable filtering algorithm for estimating (as Luenberger observer, nonlinear observer or Kalman filtering etc.) to realize motoring condition.The third method (i.e. modelling tool) can realize about being difficult to straight estimation of measuring, the dimension that expanded state is estimated, also can improve the precision about directly measuring, cost is lower simultaneously.But the modelling tool proposed at present is mainly based on the kinematics model of automobile or done the kinetic model compared with polytenization supposition to car load or tire, these models vehicle comparatively smooth running time can obtain good estimation effect and precision, but owing to being difficult to reflect that the actual Nonlinear dynamic behaviors of vehicle causes estimated accuracy lower under higher motor-driven operation conditions.

For realizing the reliable estimation to travel condition of vehicle under higher motor-driven operating condition, the present invention proposes a kind of travel condition of vehicle method of estimation based on improving EKF (Extended Kalman Filter, EKF).The filtering method of estimation that the present invention proposes can realize the accurate estimation to travel condition of vehicle signal under the higher motor-driven operation of automobile, has the features such as precision is high, cost is low, real-time is good.The travel condition of vehicle signal that filtering calculates mainly comprises automobile speed of advance, side velocity, yaw velocity and side slip angle, and these Information Availabilities control in automobile active safety.Higher motor-driven operation refers to when automotive operation is at common road traffic environment, needs to turn to comparatively frequently and the operation occasion (within 0.5g, g represents acceleration due to gravity to lateral acceleration) of acceleration and deceleration.Concrete thought of the present invention is as follows:

Kalman filter take Minimum Mean Square Error as the optimal State Estimation filter of criterion, it does not need to store observed reading in the past, only according to the estimated valve of current observed value and previous moment, computing machine is utilized to carry out recurrence calculation, just can realize the estimation to live signal, have the advantages that memory data output is little, algorithm is easy.According to kalman filtering theory, the Kalman filter model of travel condition of vehicle, except comprising equation of state, also should comprise observational equation.

For automobile active safety control under the higher motor-driven environment of adaptation is to the measurement of travel condition of vehicle signal and estimation requirement, first suitable Dynamic Modeling is carried out to automobile, namely set up the system state equation of Kalman filtering process.For application of the present invention, the present invention, for the four wheeler (should have the widest situation at present, exemplary is as the car of front-wheel steering) of the front-wheel steering travelled on usual road traffic environment, can do following reasonable assumption:

1) pitching of automobile, inclination and upper and lower bounce motion is ignored.

2) automotive suspension is ignored on the impact on tire axle.

3) ignore roll motion, can think that the deflection angle of two tires in left and right on automobile front axle, sideslip angle, longitudinal force and side force are identical; Similarly, can on assumed vehicle rear axle the sideslip angle of two tires in left and right, longitudinal force and side force identical.

Require and supposition according to above-mentioned application, the present invention is directed to the front-wheel steering four-wheel automobile that application is more at present, adopt the vehicle dynamic model shown in accompanying drawing 1 (being equivalent to the imaginary Bicycle model that forward and backward wheel is formed by concentrating on automobile axle mid point respectively after equivalent-simplification, as shown on the right side of Fig. 1).This model has 3 degree of freedom, is that longitudinal movement, sideway movement and yaw rotate respectively.Define vehicle carrier coordinate system in Fig. 1, its initial point o is positioned at barycenter place, and ox axle is consistent with vehicle forward direction along the longitudinal axis of vehicle, oz axle perpendicular to vehicle operating plane and point to ground (namely downward, around the yaw velocity ω of oz axle _zpositive dirction definition as diagram), and oy axle can be determined by right-handed helix rule.Longitudinal speed of advance v _x, side velocity v _ywith yaw velocity ω _zall to refer to vehicle centroid.According to Newtonian mechanics, the kinetic model of vehicle can be described as

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

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

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

In formula, v _x, v _yand ω _zlongitudinal speed of advance of automobile, side velocity and yaw velocity respectively, m and I _zbe the quality of vehicle and the rotor inertia around oz axle respectively, a is the distance of vehicle front wheel shaft center to barycenter, and b is the distance of automobile back wheel wheel shaft center to barycenter, δ _ffront wheel steering angle, C _drepresent aerodynamic drag factor, A _frepresent vehicle forward direction area, ρ _arepresent density of air, F _tfact on the longitudinal force on single front-wheel, F _tract on the longitudinal force on single trailing wheel, F _sfact on the side force on single front-wheel, F _sract on the side force on single trailing wheel.

For the vehicle travelled at Ordinary road traffic environment, usually the side force acted on each wheel can be expressed as

F _sf＝C _αfα _f，F _sr＝C _αrα _r(2)

In formula (2), C _{α f}, C _{α r}represent the cornering stiffness of forward and backward tire respectively, α _f, α _rrepresent the sideslip angle of forward and backward tire respectively and can be expressed as

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

Formula (2), (3) are substituted into formula (1), and consider δ _fnormally low-angle, i.e. sin δ _f≈ δ _f, cos δ _f≈ 1 and ignore second order and above high-order trace, through arrange after can obtain

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

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

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

For the front wheel steering angle δ in formula (4) _fthe steering wheel angle δ recorded by steering wheel angle sensor is divided by the steering gear ratio q from bearing circle to front-wheel _tdetermine (i.e. δ _f=δ/q _t).And for the longitudinal force of tire F in formula (4) _tfand F _trthe present invention adopts the non-linear tire model of Dugoff to estimate to determine [can bibliography: 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].For this reason, longitudinal direction of car slip rate i is introduced _sj(j=f, r) (not only but also can be divided into front wheel spindle straight skidding rate i _sfwith hind axle straight skidding rate i _sr, namely in the present invention, subscript j gets f or r and represents front or rear wheel shaft respectively), it calculates with the acceleration and deceleration situation of vehicle closely related, is specially

and j=f, r (5)

In formula (5), R represents wheel tyre radius (under normal circumstances, can think that the tire radius of four wheels is identical), v _tfand v _trrepresent that the speed along tire direction on forward and backward wheel shaft (is mark convenience, v respectively _tfand v _trcan unify to be designated as v _tj(j=f, r)), ω _frepresent that the spin velocity equivalence conversion of two wheels on front wheel spindle is to the spin velocity on front wheel spindle, ω _rrepresent that on hind axle, two rotation of wheel cireular frequency equivalence conversions are to the spin velocity (ω on hind axle _fand ω _rcan unify to be designated as ω _j(j=f, r)), its computing formula is as follows

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

(6)

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

In formula (6), ω _fL, ω _fR, ω _rLand ω _rRrepresent the spin velocity of the near front wheel, off front wheel, left rear wheel and off hind wheel respectively, measure by utilizing four wheel speed sensors and obtain.

In addition, the movement relation according to Fig. 1, v _tj(j=f, r) can determine by following formula

v _tf＝v _xcosδ _f+(v _y+aω _z)sinδ _f

(7)

v _tr＝v _x

[can bibliography: Dugoff H. according to Dugoff tire model, 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], longitudinal force of tire F _tfand F _trdetermine by following formula

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

In formula (8), C _tfand C _trrepresent that the longitudinal rigidity of single forward and backward tire (can be united and be designated as C respectively _tj(j=f, r)), variable p _j(j=f, r) sum functions f _t(p _j) (j=f, r) determined by following formula or define

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

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

In formula (9) and (10), μ represents the vertical friction coefficient between tire and ground, ε _rrepresent road attachment decay factor, F _zj(j=f, r) represents the vertical load that is assigned on front or rear wheel shaft and can be calculated as follows

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

In formula (11), g represents acceleration due to gravity.

For the model that formula (4) describes, it is a non-linear vehicle dynamic model with 3DOF, is different from the 2DOF linear vehicle model often adopted.In the 2DOF linear vehicle model often adopted, longitudinal speed of advance of vehicle is considered to permanent, and auto model is only the linear differential equation about side velocity and yaw velocity.Therefore, it is constant or change running condition (manoevreability is lower) slowly that 2DOF linear vehicle model is generally only suitable for forward speed, and for the higher motor-driven running condition situation of acceleration and deceleration (namely need frequently to turn to and), there is larger modeling error in this model.And the longitudinal speed of advance of 3DOF nonlinear model of the present invention to vehicle there is no permanent restriction, therefore the accurate estimation that general motor-driven environment also can adapt to travel condition of vehicle under higher motor-driven environment can be adapted to.Therefore, the present invention will set up the system state equation of Kalman filtering according to formula (4).

It should be noted, in the Kalman filtering recursive process of reality, the Kalman filter model of discretization need be adopted.For this reason, sliding-model control is carried out to the simultaneous differential equation of formula (4), and definition status vector is X=[x ₁x ₂x ₃] ' and x ₁=v _x, x ₂=ω _z, x ₃=v _y, i.e. X=[v _xω _zv _ysuperscript in] ' (the present invention ' represent matrix transpose), the outer input vector of system is defined as U=[u ₁u ₂u ₃] ' and u ₁=δ _f, u ₂=F _tf, u ₃=F _tr, i.e. U=[δ _ff _tff _trthe matrix form of the system state equation of the Kalman filtering of] ', then after discretization can be expressed as:

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

In formula, k represents the discretization moment; W (k-1) represent zero-mean system Gaussian white noise vector and w=[w ₁w ₂w ₃] ', be w wherein ₁, w ₂and w ₃represent three system Gaussian white noise components respectively; γ (k-1) represent system input corresponding zero mean Gaussian white noise vector outward and $\mathrm{\&gamma;}={\left[\begin{array}{ccc}{w}_{{\mathrm{\&delta;}}_f}& w_{F_{\mathrm{tf}}}& w_{F_{\mathrm{tr}}}\end{array}\right]}^{\&prime;},$ Wherein and represent that the straight system surveyed or estimate inputs δ outward respectively _f, F _tfand F _trcorresponding zero mean Gaussian white noise, three systems that these white noises lie in state of the system function f input the inside (namely having to explicitly not appearing in f) outward; Nonlinear state of the system functional vector is

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

Wherein,

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

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

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

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

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

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

And T represents discrete cycle (in the present invention, according to survey sensor characteristic, the representative value of T can be taken as 10 milliseconds, 20 milliseconds, 50 milliseconds, 100 milliseconds etc.); The system noise covariance battle array Q (k-1) that W is corresponding is $Q(k-1)=\left[\begin{array}{ccc}{{\mathrm{\&sigma;}}_{w_1}}^2& 0& 0\\ 0& {{\mathrm{\&sigma;}}_{w_2}}^2& 0\\ 0& 0& {{\mathrm{\&sigma;}}_{w_3}}^2\end{array}\right],$ Wherein and represent system Gaussian white noise w respectively ₁, w ₂and w ₃corresponding variance; The covariance matrix Γ (k-1) of its exterior input noise that γ (k-1) is corresponding is $\mathrm{\&Gamma;}(k-1)=\left[\begin{array}{ccc}{{\mathrm{\&sigma;}}_{{\mathrm{\&delta;}}_f}}^2& 0& 0\\ 0& {{\mathrm{\&sigma;}}_{F_{\mathrm{tf}}}}^2& 0\\ 0& 0& {{\mathrm{\&sigma;}}_{F_{\mathrm{tr}}}}^2\end{array}\right],$ Wherein and represent respectively and corresponding variance.

After setting up the system state equation of the Kalman filter model that travel condition of vehicle is estimated, discuss below and how to set up its observational equation.From kinematics angle, the vehicle movement shown in Fig. 1 is actually a Planar Compound motion (compound that longitudinal movement, sideway movement and yaw rotate), therefore according to Planar Compound movement relation, can obtain

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

(13)

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

In formula, V _rLand V _rRrepresent the wheel line speed of left rear wheel and off hind wheel (i.e. two non-wheel flutters) respectively, T _wit is the wheelspan on hind axle between two trailing wheels.

Formula (13) is rearranged, can obtain

v _x＝(V _RL+V _RR)/2

ω _z＝(V _RL-V _RR)/T _W(14)

It is pointed out that the wheel line speed of left rear wheel and off hind wheel obtains by two wheel speed sensors be arranged on hind axle, the cireular frequency namely utilizing on hind axle two wheel speed sensors to record is multiplied by tire radius and obtains.Consider the measurement noises of wheel speed sensors, V _{rL_m}=R ω _rLwith V _{rR_m}=R ω _rR, wherein V _{rL_m}and V _{rR_m}represent V respectively _rLand V _rRcontaining noisy observed reading.In addition, V _{rL_m}and V _{rR_m}also can be expressed as wherein with represent the additivity measurement noises (all can be modeled as the Gaussian white noise that average is 0) of the wheel line speed of left rear wheel and off hind wheel respectively.

In the present invention, using the observed quantity as Kalman filter model of longitudinal speed of advance and yaw velocity.Due to two states that longitudinal speed of advance and yaw velocity are again the Kalman filter models of above-mentioned foundation simultaneously, therefore be not difficult to set up the observational equation of filtering system, the matrix form after its discretization is

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

In formula (15), Z is observation vector, and H is observation battle array, and V represents that white noise vector observed by mutual incoherent zero-mean with W, and $Z\left(k\right)=\left[\begin{array}{c}{v}_{x\_m}\left(k\right)\\ {\mathrm{\&omega;}}_{z\_m}\left(k\right)\end{array}\right]$ 、 $H\left(k\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]$ 、 $V=\left[\begin{array}{c}{n}_{v_x}\\ n_{{\mathrm{\&omega;}}_z}\end{array}\right],$ And v _{x_m}(k) and ω _{z_m}k () is respectively and measures longitudinal speed of advance of obtaining and yaw velocity by wheel speed sensors (cireular frequency namely utilizing on hind axle two wheel speed sensors to record is multiplied by the wheel line speed that tire radius obtains left rear wheel and off hind wheel, and then utilize formula (14) to obtain), i.e. v _{x_m}(k) and ω _{z_m}k () represents v respectively _xand ω _zcontaining noisy observed reading and represent by wheel speed sensors measure obtain longitudinal direction of car speed of advance observation noise and be average be 0, variance is gaussian white noise, represent by wheel speed sensors measure obtain yaw velocity observation noise and be average be 0, variance is gaussian white noise; Therefore observation noise variance matrix R corresponding to V can be expressed as $R=\left[\begin{array}{cc}{{\mathrm{\&sigma;}}_{v_x}}^2& 0\\ 0& {{\mathrm{\&sigma;}}_{{\mathrm{\&omega;}}_z}}^2\end{array}\right].$

The measurement equation that the system state equation described for formula (12) and formula (15) describe, can use kalman filtering theory, set up filtering recurrence estimation process.But notice that the equation of state shown in formula (12) is nonlinear equation, when application card Kalman Filtering calculates, need advanced line linearity process, system equation is existed near (use in the present invention by Taylor series expansion the filtering computing value of expression state X), retain single order trace, ignore high-order trace after carry out filtering recurrence calculation again, namely need carry out filtering recursion according to EKF process.Theoretical according to EKF, can set up EKF involved in the present invention general recursive process (this recursive process comprise the time upgrade and measurement updaue, the first two steps of recursive process are upgrade the time below, and remaining three steps are measurement updaue):

Time upgrades:

State one-step prediction equation $\hat{X}(k,k-1)=f(X(k-1),U(k-1),\mathrm{0,0})$

One-step prediction error covariance 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 that state of the system functional vector f asks the Jacobi matrix (Jacobian) of partial derivative to state vector X, B is that state of the system functional vector f asks the Jacobi matrix (Jacobian) of partial derivative to outside input vector U, i.e. the i-th row jth column element A of matrix A and B _{[i, j]}and B _{[i, j]}(i=1,2,3 j=1,2,3) can try to achieve respectively by formula below

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

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

Specifically, according to formula (12), the value of each element of a matrix is as follows

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

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

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

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

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

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

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

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

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

B _[2，3]＝B _[3，3]＝0

Measurement updaue:

Filtering gain matrix k (k) K (k)=P (k, k-1) H ' (k) [H (k) P (k, k-1) H ' (k)+R (k)] ^-1

State estimation $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\hat{X}(k,k-1)]$

Estimation error variance battle array P (k) P (k)=[I-K (k) H (k)] P (k, k-1) and I is 3 × 3 unit matrix

Notice that above-mentioned EKF recursive process (when namely calculating k (k)) in measurement updaue process also exists the inversion operation of matrix.During matrix inversion, calculated amount greatly and easily cause the instability of numerical calculation.To this, the present invention does not directly adopt the method for matrix inversion when measurement updaue, and adopts scalarization process (scalar measurement processing) method.Specifically, time renewal process can be carried out according to above-mentioned filtering, and measurement updaue is undertaken by the recursive algorithm of following improvement:

Make P ₁=P (k, k-1), because observation vector dimension is 2, therefore will h (k), Z (k) and R (k) battle array are divided into two pieces, namely

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

For i from 1 to 2, carry out 2 recurrence calculation:

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

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

P _i+1＝(I-K _i·H _{r_i})·P _i

Finally can obtain P (k)=P ₃,

In above-mentioned filtering recurrence calculation process, the automobile longitudinal speed of advance v of automobile in each moment can be determined _x(k), yaw velocity ω _z(k) and side velocity v _y(k), and then the side slip angle that each moment can be determined according to following formula

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

Embodiment 2

For the actual effect of the travel condition of vehicle method of estimation based on the EKF improved that inspection the present invention proposes, the vehicle dynamics simulation software CarSim of specialty carries out simulating, verifying experiment.

CarSim is the special simulation software for vehicle dynamics developed by U.S. MSC (Mechanical Simulation Corporation) company, at present by automakers numerous in the world, components supplying business adopt, be widely used in the business development of modern automobile control system, become the standard software of auto trade, enjoyed a very good reputation.Vehicle dynamic model in Carsim be by respectively to the car body of automobile, suspension, turn to, the height modeling true to nature of each subsystem such as braking and each tire realizes, there is very high degree of freedom, the closely actual information of travel condition of vehicle accurately can be provided, therefore, the reference that the travel condition of vehicle information that Carsim exports can be used as vehicle exports.

For the estimation effect of algorithm under higher motor-driven environment that inspection the present invention proposes, the steering wheel angle δ of automobile is set in emulation experiment by amplitude 60 ⁰the change of sinusoidal rule, simultaneously longitudinal speed of advance of automobile also constantly accelerating, braking deceleration and the change such as at the uniform velocity, as shown in Figure 2, emulation duration is set to 100 seconds (s) for steering wheel angle and the concrete process over time of longitudinal speed of advance.Vehicle used is the four-wheeled of a front-wheel steering, and principal parameter is as follows: m=960 (kilogram), I _z=1382 (kilogram-meters ²), a=0.948 (rice), b=1.422 (rice), C _{α f}=C _{α r}=25692 (newton/radians), T _w=1.390 (rice).The measurement noises of the linear velocity (cireular frequency recorded by wheel speed sensors is multiplied by tire radius and obtains) of setting four wheels is the Gaussian white noise that average is 0, standard deviation is 0.04 (meter per second), and the measurement noises of steering wheel angle sensor is the Gaussian white noise that average is 0, standard deviation is 0.0873 (radian).The standard deviation of the system zero mean Gaussian white noise of Kalman filtering is respectively and the standard deviation of three zero mean Gaussian white noise inputted outward of Kalman filtering is respectively and the standard deviation of the zero mean Gaussian white noise of two observed quantities of Kalman filtering is respectively and related results is as shown in table 1 and Fig. 3 ~ Fig. 4.

Table 1 lists and utilizes straight survey method and the inventive method to calculate the statistics contrast of travel condition of vehicle for whole process, and the error in table is all (the longitudinal speed of advance error as directly surveyed method just represents the error of longitudinal speed of advance reference value that the longitudinal speed of advance utilizing straight survey method to calculate exports relative to Carsim) for the corresponding reference value that Carsim exports.Be pointed out that in addition, the concrete meaning of above-mentioned two kinds of methods is as follows: straight survey method refers to by directly measuring the longitudinal speed of advance and yaw velocity that convert and obtain, namely the cireular frequency utilizing on hind axle two wheel speed sensors to record is multiplied by the wheel line speed that tire radius obtains left rear wheel and off hind wheel, and then the longitudinal speed of advance and yaw velocity that utilize the formula in embodiment 1 (14) directly to calculate to obtain; The inventive method refers to that the improvement EKF method of estimation utilizing the present invention to propose is to calculate the method for each running state of vehicle.

Table 1 two kinds of methods calculate the contrast table of effect

In table, "--" represents the item that straight survey method cannot calculate

Fig. 3 gives the result curve (indicating with EKF dot-dash dotted line in figure) of the side slip angle β that the inventive method is estimated, and the reference output valve of corresponding Carsim (indicating with the real black line of Carsim in figure).The curve of error of the β reference value that the β that Fig. 4 then gives the inventive method estimation exports relative to Carsim.

By contrast (especially standard deviation) and Fig. 3 ~ Fig. 4 of table 1, can find out that the inventive method has had relative to straight survey method precision in the reckoning of longitudinal speed of advance and yaw velocity and significantly improve.In addition, according to table 1 and Fig. 3 ~ Fig. 4, it can also be seen that the inventive method also has very high precision in the estimation of side velocity and side slip angle.

To sum up, even if under higher motor-driven running environment, what the present invention proposed can estimate the information such as longitudinal direction of car speed of advance, side velocity, yaw velocity and side slip angle exactly based on the travel condition of vehicle method of estimation improving EKF, and these information can meet the needs of regarding Car active safety control.

Claims (2)

1. the travel condition of vehicle method of estimation based on improvement EKF, it is characterized in that: this method is for front-wheel steering four-wheel automobile, set up Nonlinear Integer vehicle dynamics model and longitudinal force of tire model, utilize vehicle-mounted wheel speed and steering wheel angle sensor information to determine to set up outside input and the observed quantity of filtering system, the improvement EKF recursive algorithm then by proposing realizes the estimation to automobile longitudinal speed of advance, yaw velocity, side velocity and side slip angle information simultaneously; Described filtering system is EKF system, and following system refers to EKF system;

Concrete steps comprise:

1) equation of state and the observational equation of EKF is set up:

Set up the Vehicle Nonlinear kinetic model of three degree of freedom, namely set up the system state equation of EKF, the matrix representation of the equation of state of the Kalman filtering after discretization is:

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

In formula (1), k represents the discretization moment;

State of the system vector is X=[x ₁x ₂x ₃] ' and x ₁=v _x, x ₂=ω _z, x ₃=v _y, i.e. X=[v _xω _zv _y] ', v _x, v _yand ω _zlongitudinal speed of advance of automobile, side velocity and yaw velocity respectively; Superscript in this travel condition of vehicle method of estimation ' represent matrix transpose;

The outer input vector of system is U=[u ₁u ₂u ₃] ' and u ₁=δ _f, u ₂=F _tf, u ₃=F _tr, i.e. U=[δ _ff _tff _tr] ', δ _ffront wheel steering angle, F _tfact on the longitudinal force on single front-wheel, F _tract on the longitudinal force on single trailing wheel;

W (k-1) represent zero-mean system Gaussian white noise vector and W=[w ₁w ₂w ₃] ', be w wherein ₁, w ₂and w ₃represent three system Gaussian white noise components respectively;

γ (k-1) represent system input corresponding zero mean Gaussian white noise vector outward and $\mathrm{\&gamma;}={\left[\begin{array}{ccc}{w}_{{\mathrm{\&delta;}}_f}& w_{F_{\mathrm{tf}}}& w_{F_{\mathrm{tr}}}\end{array}\right]}^{\&prime;},$ Wherein and represent that the straight system surveyed or estimate inputs δ outward respectively _f, F _tfand F _trcorresponding zero mean Gaussian white noise, three systems that these white noises lie in state of the system function f input the inside outward; Nonlinear state of the system functional vector is

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

Wherein,

$\begin{array}{c}{f}_1(X(k-1),U(k-1),W(k-1),\mathrm{\&gamma;}(k-1))=v_x(k-1)+\frac{T}{m}[{\mathrm{mv}}_y(k-1){\mathrm{\&omega;}}_z(k-1)+\\ {2C}_{\mathrm{\&alpha;f}}\frac{v_y(k-1)+{\mathrm{a\&omega;}}_z(k-1)}{v_x(k-1)}{\mathrm{\&delta;}}_f(k-1)-\frac{1}{2}{C}_d{A}_f{\mathrm{\&rho;}}_a{v}_x^2(k-1)]+\frac{2T}{m}[F_{\mathrm{tf}}(k-1)+F_{\mathrm{tr}}(k-1)]+w_1\end{array}$

$\begin{array}{c}{f}_2(X(k-1),U(k-1),W(k-1),\mathrm{\&gamma;}(k-1))={\mathrm{\&omega;}}_z(k-1)+\frac{T}{I_z}\left\{2a{C}_{\mathrm{\&alpha;f}}\right[{\mathrm{\&delta;}}_f(k-1)-\frac{(v_y(k-1)+a{\mathrm{\&omega;}}_z(k-1))}{v_x(k-1)}\\ -2b{C}_{\mathrm{\&alpha;r}}\frac{[{\mathrm{b\&omega;}}_z(k-1)-v_y(k-1)]}{v_x(k-1)}\}+\frac{2\mathrm{aT}}{I_z}{F}_{\mathrm{tf}}(k-1){\mathrm{\&delta;}}_f(k-1)+w_2\end{array}$

$\begin{array}{c}{f}_3(X(k-1),U(k-1),W(k-1),\mathrm{\&gamma;}(k-1))=v_y(k-1)+\frac{T}{m}\{-{\mathrm{mv}}_x(k-1){\mathrm{\&omega;}}_z(k-1)+\\ {2C}_{\mathrm{\&alpha;f}}[{\mathrm{\&delta;}}_f(k-1)+\frac{-v_y(k-1)-{\mathrm{a\&omega;}}_z(k-1)}{v_x(k-1)}]+2C_{\mathrm{\&alpha;r}}\frac{b{\mathrm{\&omega;}}_z(k-1)-v_y(k-1)}{v_x(k-1)}+\frac{2T}{m}{F}_{\mathrm{tf}}(k-1){\mathrm{\&delta;}}_f(k-1)+w_3\end{array}$

At f ₁, f ₂and f ₃above-mentioned expression formula in, m and I _zbe the quality of vehicle and the rotor inertia walking around the vertical axle of barycenter respectively, a is the distance of vehicle front wheel shaft center to barycenter, and b is the distance of automobile back wheel wheel shaft center to barycenter, C _{α f}, C _{α r}represent the cornering stiffness of forward and backward tire respectively, C _drepresent aerodynamic drag factor, A _frepresent vehicle forward direction area, ρ _arepresent density of air, T represents the discrete cycle; The system noise covariance battle array Q (k-1) that W is corresponding is:

$Q(k-1)=\left[\begin{array}{ccc}{{\mathrm{\&sigma;}}_{w_1}}^2& 0& 0\\ 0& {{\mathrm{\&sigma;}}_{w_2}}^2& 0\\ 0& 0& {{\mathrm{\&sigma;}}_{w_3}}^2\end{array}\right],$ Wherein and represent system Gaussian white noise w respectively ₁, w ₂and w ₃corresponding variance; The covariance matrix Γ (k-1) of its exterior input noise that γ (k-1) is corresponding is $\mathrm{\&Gamma;}(k-1)=\left[\begin{array}{ccc}{{\mathrm{\&sigma;}}_{{\mathrm{\&delta;}}_f}}^2& 0& 0\\ 0& {{\mathrm{\&sigma;}}_{F_{\mathrm{tf}}}}^2& 0\\ 0& 0& {{\mathrm{\&sigma;}}_{F_{\mathrm{tr}}}}^2\end{array}\right],$ Wherein and represent respectively and corresponding variance;

The discretization matrix form of the observational equation of Kalman filtering is:

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

In formula (2), Z is observation vector, and H is observation battle array, and V represents that white noise vector observed by mutual incoherent zero-mean with W, and $Z\left(k\right)=\left[\begin{array}{c}{v}_{x\_m}\left(k\right)\\ {\mathrm{\&omega;}}_{z\_m}\left(k\right)\end{array}\right],H\left(k\right)=\begin{array}{c}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right],V=\left[\begin{array}{c}{n}_{v_x}\\ n_{{\mathrm{\&omega;}}_z}\end{array}\right]\end{array},$ Wherein v _{x_m}(k) and ω _{z_m}k () is respectively and measures by wheel speed sensors the longitudinal direction of car speed of advance and yaw velocity that obtain; represent by wheel speed sensors measure obtain longitudinal direction of car speed of advance observation noise and be average be 0, variance is gaussian white noise, represent by wheel speed sensors measure obtain yaw velocity observation noise and be average be 0, variance is gaussian white noise; The observation noise variance matrix R that V is corresponding can be expressed as $R=\left[\begin{array}{cc}{{\mathrm{\&sigma;}}_{v_x}}^2& 0\\ 0& {{\mathrm{\&sigma;}}_{{\mathrm{\&omega;}}_z}}^2\end{array}\right];$

2) the EKF recursion improved is carried out

For the equation of state described by formula (1) and formula (2) and observational equation, use EKF theoretical, Criterion filtering recursive process, this recursive process comprises time renewal and measurement updaue:

Time upgrades:

State one-step prediction equation: $\hat{X}(k,k-1)=f(X(k-1),U(k-1),\mathrm{0,0})$

One-step prediction error covariance 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 state of the system functional vector f asks partial derivative Jacobi matrix to state vector X, and B is that state of the system functional vector f asks the Jacobi matrix of partial derivative to outside input vector U, i.e. the i-th row jth column element A of matrix A and B _{[i, j]}and B _{[i, j]}(i=1,2,3 j=1,2,3) can try to achieve respectively by formula below

$A_{[i,j]}=\frac{{\&PartialD;f}_i}{{\&PartialD;x}_j}(\hat{X}(k,k-1),U(k-1),\mathrm{0,0}),(i=\mathrm{1,2,3}j=\mathrm{1,2,3})$

$B_{[i,j]}=\frac{{\&PartialD;f}_i}{{\&PartialD;u}_j}(\hat{X}(k,k-1),U(k-1),\mathrm{0,0}),(i=\mathrm{1,2,3}j=\mathrm{1,2,3})$

Specifically, the value of each element of a matrix is as follows:

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

$\begin{array}{cc}{A}_{\left[\mathrm{1,3}\right]}=T({\mathrm{\&omega;}}_z+\frac{{2C}_{\mathrm{\&alpha;f}}}{{\mathrm{mv}}_x}{\mathrm{\&delta;}}_f)& A_{\left[\mathrm{2,1}\right]}=\frac{2T[a{C}_{\mathrm{\&alpha;f}}(v_y+{\mathrm{a\&omega;}}_z)+{\mathrm{bC}}_{\mathrm{\&alpha;r}}({\mathrm{b\&omega;}}_z-v_y)]}{I_z{v_x}^2}\end{array}$

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

$A_{\left[\mathrm{3,1}\right]}=T[-{\mathrm{\&omega;}}_z-{2C}_{\mathrm{\&alpha;r}}\frac{{\mathrm{b\&omega;}}_z-v_y}{{{\mathrm{mv}}_x}^2}+{2C}_{\mathrm{\&alpha;f}}\frac{v_y+{\mathrm{a\&omega;}}_z}{{{\mathrm{mv}}_x}^2}]$

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

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

$\begin{array}{cc}{B}_{\left[\mathrm{1,1}\right]}=2T{C}_{\mathrm{\&alpha;f}}\frac{(v_y+{\mathrm{a\&omega;}}_z)}{{\mathrm{mv}}_x}& B_{\left[\mathrm{1,2}\right]}=B_{\left[\mathrm{1,3}\right]}=\frac{2T}{m}\end{array}$

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

$\begin{array}{cc}{B}_{\left[\mathrm{3,1}\right]}=\frac{{2\mathrm{TF}}_{\mathrm{tf}}}{m}+\frac{2T{C}_{\mathrm{\&alpha;f}}}{m}& B_{\left[\mathrm{3,2}\right]}=\frac{{2\mathrm{T\&delta;}}_f}{m}\end{array}$

B _[2,3]＝B _[3,3]＝0

Measurement updaue:

Filtering gain matrix k (k): K (k)=P (k, k-1) H ' (k) [H (k) P (k, k-1) H ' (k)+R (k)] ^-1

State estimation: $\hat{X}\left(k\right)=\hat{X}(k,k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\hat{X}(k,k-1)]$

Estimation error variance battle array P (k): P (k)=[I-K (k) H (k)] P (k, k-1) and I is 3 × 3 unit matrix

In actual recursive process, measurement updaue adopts scalarization processing method; Specifically, time renewal process is carried out according to above-mentioned filtering, and measurement updaue is undertaken by the recursive algorithm of following improvement:

Make P ₁=P (k, k-1), because observation vector dimension is 2, therefore will h (k), Z (k) and R (k) battle array are divided into two pieces, namely

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

For i from 1 to 2, carry out 2 recurrence calculation:

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

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

P _i+1＝(I-K _i·H _{r_i})·P _i

Finally can obtain P (k)=P ₃,

In above-mentioned filtering recurrence calculation process, the automobile longitudinal speed of advance v of automobile in each moment can be determined _x(k), yaw velocity ω _z(k) and side velocity v _y(k), and then the side slip angle that each moment can be determined according to formula (3):

β(k)＝arctan[v _y(k)/v _x(k)] (3)

Described step 1) in,

In formula (1), the front wheel steering angle δ that the system of EKF system inputs outward _f, be that the steering wheel angle δ that recorded by steering wheel angle sensor is divided by the steering gear ratio q from bearing circle to front-wheel _tdetermine; And longitudinal force of tire F _tfand F _tr, be determine according to the non-linear tire model of Dugoff;

Use i _sjnot only but also can be divided into front wheel spindle straight skidding rate i (j=f, r) represents longitudinal direction of car slip rate, _sfwith hind axle straight skidding rate i _sr, subscript j gets f or r, f or r represents front or rear wheel shaft respectively, i _sjmethod of calculating is:

In formula (4), R represents wheel tyre radius; v _tfand v _trrepresent the speed along tire direction on forward and backward wheel shaft respectively, v _tfand v _trcan unify to be designated as v _tj(j=f, r); ω _frepresent that the spin velocity equivalence conversion of two wheels on front wheel spindle is to the spin velocity on front wheel spindle; ω _rrepresent that on hind axle, two rotation of wheel cireular frequency equivalence conversions are to the spin velocity on hind axle, ω _fand ω _rcan unify to be designated as ω _j(j=f, r) and

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

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

In formula (5), ω _fL, ω _fR, ω _rLand ω _rRrepresent the spin velocity of the near front wheel, off front wheel, left rear wheel and off hind wheel respectively, measure by utilizing four wheel speed sensors and obtain;

V _tj(j=f, r) can determine by formula (6):

v _tf＝v _xcosδ _f+(v _y+aω _z)sinδ _f(6)

v _tr＝v _x

And then, longitudinal force of tire F _tfand F _trdetermine by formula (7)

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

In formula (7), C _tfand C _trrepresent the longitudinal rigidity of single forward and backward tire respectively, unification is designated as C _tj(j=f, r); Variable p _j(j=f, r) sum functions f _t(p _j) (j=f, r) determined by following formula:

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

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

In formula (8) and (9), μ represents the vertical friction coefficient between tire and ground; ε _rrepresent road attachment decay factor; α _f, α _rrepresent the sideslip angle of forward and backward tire respectively, unification is designated as α _j(j=f, r), can be calculated as follows

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

And F _zj(j=f, r) represents the vertical load that is assigned on front or rear wheel shaft and can be calculated as follows

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

In formula (11), g represents acceleration due to gravity;

There is following relation in longitudinal direction of car speed of advance and yaw velocity and two non-speed of trailing wheel that turn to

v _x＝(V _RL+V _RR)/2

(12)

ω _z＝(V _RL-V _RR)/T _W

In formula (12), T _wrepresent the wheelspan between two trailing wheels on hind axle, V _rLand V _rRrepresent the linear velocity of left rear wheel and off hind wheel respectively;

For the observed reading v in formula (2) _{x_m}(k) and ω _{z_m}k (), they are that the cireular frequency utilizing on hind axle two wheel speed sensors to record is multiplied by tire radius and obtains V _{rL_m}=R ω _rLand V _{rR_m}=R ω _rR, V _{rL_m}and V _{rR_m}represent V respectively _rLand V _rRcontaining noisy observed reading, and then formula (12) is utilized to obtain, i.e. v _{x_m}and ω _{z_m}represent v respectively _xand ω _zcontaining noisy observed reading and represent by wheel speed sensors measure obtain longitudinal speed of advance observation noise and be average be 0, variance is gaussian white noise, represent by wheel speed sensors measure obtain yaw velocity observation noise and be average be 0, variance is gaussian white noise.

2. the travel condition of vehicle method of estimation based on improving EKF according to claim 1, is characterized in that the representative value of discrete cycle T is 10 milliseconds, 20 milliseconds, 50 milliseconds or 100 milliseconds.