Intelligent vehicle state estimation method

Abstract

The invention provides an intelligent vehicle state estimation method, which comprises the following steps of: firstly, establishing an intelligent vehicle dynamic model comprising three degrees of freedom of longitudinal, transverse and yawing; secondly, according to the established intelligent vehicle dynamic model, selecting a state quantity, an observed quantity and system input of an intelligent vehicle state estimation system, and establishing a state equation and an observation equation of the intelligent vehicle state estimation system; and finally, combining unscented Kalman filtering and strong tracking filtering to design an intelligent vehicle state estimation algorithm with high estimation precision and strong robustness. According to the method, high-precision robust estimation of the state of the intelligent vehicle can be realized, and an accurate vehicle state signal is provided for motion control of maneuverability, stability and the like of the intelligent vehicle.

Description

Technical Field

[0001] The present invention relates to the field of automobile assisted driving, and in particular to a method for estimating the state of an intelligent vehicle. Background Art

[0002] The acquisition of vehicle status signals is an important prerequisite for vehicle motion control, and the accuracy of the acquired vehicle status signals is closely related to the vehicle control effect. However, some vehicle status signals need to be collected using very expensive sensors, which greatly increases the production and manufacturing cost of the entire vehicle. In addition, some important vehicle status signals such as the vehicle's center of mass sideslip angle, body roll angle and other signals are difficult to measure directly using sensors. Therefore, it is necessary to design a corresponding vehicle status signal estimation method to indirectly obtain the vehicle's status information through analytical methods, thereby providing accurate and reliable signals for the vehicle's motion planning and dynamic control.

[0003] Among the existing vehicle state signal estimation methods, the extended Kalman filter algorithm is simple and easy to operate, and is widely used in the field of nonlinear system filtering. However, the extended Kalman filter requires the linearization of nonlinear functions, which leads to linearization errors and subsequent reductions in filtering accuracy. The unscented Kalman filter uses unscented transformation to improve the estimation accuracy of the system, but its robustness to model uncertainty is poor. The strong tracking filter has good robustness to the uncertainty of the system model due to the introduction of the fading factor, but it inevitably has the shortcomings of the extended Kalman filter.

[0004] Since accurate acquisition of vehicle state is a prerequisite for motion control such as maneuverability and stability, it is necessary to develop high-precision and high-robustness nonlinear vehicle state estimation methods to achieve high-precision and robust estimation of vehicle state. Summary of the invention

[0005] The purpose of the present invention is to propose a smart car state estimation method to achieve high-precision robust estimation of the smart car state and provide accurate vehicle state signals for motion control such as maneuverability and stability of the smart car.

[0006] To achieve the above purpose, the technical solution of the present invention is: a method for estimating the state of an intelligent vehicle, the steps are as follows:

[0007] S1. Establish a dynamic model of the intelligent vehicle including three degrees of freedom: longitudinal, lateral and yaw;

[0008] S2. According to the established intelligent vehicle dynamics model, select the state quantity, observation quantity and system input of the intelligent vehicle state estimation system, and establish the state equation and observation equation of the intelligent vehicle state estimation system;

[0009] S3. Combining unscented Kalman filtering and strong tracking filtering, a high-precision and robust estimation algorithm for the state of the smart car is designed to estimate the state of the smart car.

[0010] Preferably, in S1, the longitudinal dynamics model of the smart car is:

[0011]

[0012] In the formula, v x is the longitudinal velocity of the vehicle; v y is the lateral velocity of the vehicle; γ is the yaw rate of the vehicle; m is the mass of the vehicle; ∑F x is the total tire longitudinal force; F w F is the air resistance of the vehicle; f is the rolling resistance of the vehicle tire; a x is the longitudinal acceleration of the vehicle; C d is the air resistance coefficient; A is the frontal area of ​​the vehicle; ρ is the air density; g is the acceleration of gravity, and f is the rolling resistance coefficient.

[0013] Preferably, in S1, the lateral dynamics model of the smart car is:

[0014]

[0015] In the formula, ∑F y is the total tire lateral force; a y is the lateral acceleration of the vehicle; F xi 、F yi are the longitudinal force and lateral force of each tire respectively, and the subscript i = {fl, fr, rl, rr} represents the left front wheel, right front wheel, left rear wheel and right rear wheel respectively; δ f is the front wheel turning angle.

[0016] Preferably, in S1, the yaw dynamics model of the smart car is:

[0017]

[0018] In the formula, I z is the moment of inertia of the vehicle around the Z axis; ∑M z is the total vehicle yaw moment; a is the distance from the vehicle center of mass to the front axle; b is the distance from the vehicle center of mass to the rear axle; c is the vehicle wheelbase.

[0019] Preferably, in S2, the state quantity of the intelligent vehicle state estimation system is selected as x=[v x v y γ] T , the observed quantity is z = [a x a y ] T, the input of the system is u=[δ f F xi F yi ] T , where i = {fl, fr, rl, rr}.

[0020] Preferably, the state equation of the smart car state estimation system is:

[0021]

[0022] Preferably, the observation equation of the smart car state estimation system is:

[0023]

[0024] Preferably, in S3, the smart car state high-precision robust estimation algorithm specifically includes the following steps:

[0025] S3.1, Initialization:

[0026]

[0027] In the formula, x 0 and are the initial value and mean value of the state quantity respectively; P 0 is the initial covariance of the state quantity;

[0028] S3.2. According to the mean of the state quantity at time k and covariance P k A symmetric sampling strategy is used to select 2n+1 Sigma points and their weights:

[0029]

[0030] In the formula, χ i,k is the sampling value of the i-th Sigma point at time k; λ = α 2 (n+κ)-n is a scaling parameter used to reduce the total prediction error; α is an adjustment parameter used to control the distribution of Sigma points around the mean; κ is an adjustable parameter used to improve the accuracy of the approximation; n is a natural constant; is the weight for calculating the mean of the sampling points; The weight for calculating the variance of sampling points;

[0031] S3.3. According to the state equation of the intelligent vehicle state estimation system, the state of the Sigma point is predicted, and the state prediction covariance when the fading factor is not introduced is calculated.

[0032] ξ i,k+1|k =f(χ i,k ,u k) (9)

[0033]

[0034] In the formula, u k is the input of the system at time k; ξ i,k+1|k is the predicted value of the Sigma point state when the fading factor is not introduced; f(χ i,k ,u k ) is the state equation; Q k is the process noise; is the state prediction mean when the fading factor is not introduced;

[0035] S3.4. Predict the mean value based on the state without the introduction of the fading factor and the state prediction covariance without the introduction of the fading factor Sample again to obtain a new set of Sigma points:

[0036]

[0037] In the formula, is the state prediction mean based on the absence of the fading factor and covariance The obtained Sigma point;

[0038] S3.5. Substitute the newly obtained Sigma point set into the observation equation of the intelligent vehicle state estimation system and calculate the predicted value of the observation quantity when the fading factor is not introduced. And calculate the predicted quantity covariance when the fading factor is not introduced and mutual covariance Prepare for the subsequent calculation of the fading factor:

[0039]

[0040] In the formula, is the observation equation; is the predicted value of the Sigma point observation when the fading factor is not introduced; R k+1 is the observation noise;

[0041] S3.6. The state prediction covariance calculated without the introduction of the fading factor The covariance of the observations without the introduction of the fading factor and mutual covariance Calculate the fading factor λ k+1 ;

[0042] S3.7, Introducing the fading factor λ k+1 , recalculate the state prediction covariance P after introducing the fading factor k+1|k :

[0043]

[0044] Where P k+1|k The state prediction covariance after the introduction of the fading factor;

[0045] S3.8. Predict the mean value based on the state without the introduction of the fading factor And the state prediction covariance P after the introduction of the fading factor k+1|k , sample again to obtain a new set of Sigma points:

[0046]

[0047] In the formula, χ i,k+1|k is the state prediction mean based on the absence of the fading factor And the state prediction covariance P after the introduction of the fading factor k+1|k The obtained Sigma point;

[0048] S3.9. Substitute the Sigma point into the observation equation of the smart car state estimation system and calculate the predicted value of the observation after the fading factor is introduced. And calculate the observation covariance P after introducing the fading factor zz and the cross-covariance P xz :

[0049] γ i,k+1|k =h(χ i,k+1|k ) (twenty two)

[0050]

[0051] In the formula, γ i,k+1|k is the predicted value of the Sigma point observation after the fading factor is introduced;

[0052] S3.10, obtain new measurement value z k+1 Then, calculate the Kalman gain K k+1 And the mean of the state and variance P k+1 to update.

[0053] Preferably, the state prediction covariance obtained by calculation without introducing the fading factor is The covariance of the observations without the introduction of the fading factor and mutual covariance Calculate the fading factor λ k+1 , specifically:

[0054]

[0055] In the formula, in the formula, is the residual sequence; τ is the forgetting factor; λ 0 is the original calculated value of the fading factor; tr(·) represents the trace of the matrix; V k+1 is the covariance matrix of the residual sequence.

[0056] Preferably, the new measurement value z is obtained k+1 Then, calculate the Kalman gain K k+1 And the mean of the state and variance P k+1 Update, specifically:

[0057]

[0058] In the formula, z k+1 is the measured value; K k+1 is the Kalman gain; is the state mean; P k+1 is the variance.

[0059] Compared with the prior art, the present invention has the following beneficial effects:

[0060] It combines the advantages of the unscented change in the unscented Kalman filter algorithm for high approximation accuracy of nonlinear systems and the advantage of the fading factor in the strong tracking filter algorithm for strong robustness to system model uncertainty, and further adopts the fading factor equivalent calculation method to avoid solving the Jacobian matrix of the nonlinear system. The designed state estimation method can perform high-precision and robust estimation of the state of the intelligent vehicle, providing accurate vehicle state signals for motion control such as the maneuverability and stability of the intelligent vehicle, and therefore has broad market application prospects. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 Schematic diagram of the smart car state estimation method of the present invention. DETAILED DESCRIPTION

[0062] The technical solution of the present invention is described in detail below in conjunction with the accompanying drawings.

[0063] The present invention may be implemented in many different forms and should not be considered limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the invention to those skilled in the art.

[0064] The present invention discloses a method for estimating the state of an intelligent vehicle. The steps of the method for estimating the state of an intelligent vehicle are as follows:

[0065] S1. Establish a dynamic model of the intelligent vehicle including three degrees of freedom: longitudinal, lateral and yaw.

[0066] The intelligent vehicle dynamics model including longitudinal, lateral and yaw degrees of freedom is as follows:

[0067] The longitudinal dynamics model of the smart car is:

[0068]

[0069] The lateral dynamics model of the smart car is:

[0070]

[0071] The yaw dynamics model of the smart car is:

[0072]

[0073] In the formula, v x is the longitudinal velocity of the vehicle; v y is the lateral velocity of the vehicle; γ is the yaw rate of the vehicle; m is the mass of the vehicle; I z is the moment of inertia of the vehicle around the Z axis; ∑F x is the total tire longitudinal force; ∑F y is the total tire lateral force; ∑M z is the total vehicle yaw moment; F w F is the air resistance of the vehicle; f F is the rolling resistance of the vehicle tire; xi 、F yi are the longitudinal force and lateral force of each tire, respectively. The subscript i = {fl, fr, rl, rr} represents the left front wheel, right front wheel, left rear wheel and right rear wheel respectively. a x is the longitudinal acceleration of the vehicle; a y is the lateral acceleration of the vehicle; δ f is the front wheel turning angle; a is the distance from the vehicle's center of mass to the front axle; b is the distance from the vehicle's center of mass to the rear axle; c is the vehicle's wheelbase; C d is the air resistance coefficient; A is the frontal area of ​​the vehicle; ρ is the air density; g is the acceleration of gravity; and f is the rolling resistance coefficient.

[0074] S2. According to the established intelligent vehicle dynamics model, select the state quantity, observation quantity, and system input of the intelligent vehicle state estimation system, and establish the state equation and observation equation of the intelligent vehicle state estimation system.

[0075] The state quantity of the selected intelligent vehicle state estimation system is x = [v x v y γ] T , the observed quantity is z = [a x a y ] T , the input of the system is u=[δ f Fxi F yi ] T , where i = {fl, fr, rl, rr}, the state equation and observation equation of the smart car state estimation system are as follows:

[0076] The state equation of the smart car state estimation system is:

[0077]

[0078] The observation equation of the smart car state estimation system is:

[0079]

[0080] S3. Design a high-precision and robust estimation algorithm for the state of an intelligent vehicle that has high estimation accuracy for nonlinear systems and strong robustness to system model uncertainties.

[0081] like Figure 1 As shown, the design of the intelligent vehicle state high-precision robust estimation algorithm includes the following steps:

[0082] S3.1, Initialization:

[0083]

[0084] In the formula, x 0 and are the initial value and mean value of the state quantity respectively; P 0 is the initial covariance of the state quantity.

[0085] S3.2. According to the mean of the state quantity at time k and covariance P k A symmetric sampling strategy is used to select 2n+1 Sigma points and their weights:

[0086]

[0087]

[0088] In the formula, χ i,k is the sampling value of the i-th Sigma point at time k; λ = α 2 (n+κ)-n is a scaling parameter used to reduce the total prediction error; α is an adjustment parameter used to control the distribution of Sigma points around the mean; κ is an adjustable parameter that can improve the accuracy of the approximation; n is a natural constant; is the weight for calculating the mean of the sampling points; The weight for calculating the variance of sampling points; is the mean value of the state quantity at time k; P k is the covariance of the state quantity at time k.

[0089] S3.3. One-step prediction of the Sigma point is performed based on the state equation of the nonlinear system, and the state prediction covariance is calculated without introducing the fading factor.

[0090] ξ i,k+1|k =f(χ i,k ,u k ) (9)

[0091]

[0092] In the formula, u k is the input of the system at time k; ξ i,k+1|k is the predicted value of the Sigma point state when the fading factor is not introduced; f(χ i,k ,u k ) is the state equation; Q k is the process noise; is the state prediction mean when the fading factor is not introduced; Predict the covariance of the state without the introduction of the fading factor.

[0093] S3.4. Predict the mean value based on the state without the introduction of the fading factor and the state prediction covariance without the introduction of the fading factor Sample again to obtain a new set of Sigma points:

[0094]

[0095] In the formula, is the state prediction mean based on the absence of the fading factor and covariance The obtained Sigma points.

[0096] S3.5. Substitute the newly obtained Sigma point set into the observation equation of the system and calculate the predicted value of the observation when the fading factor is not introduced. And further calculate the predicted quantity covariance when the fading factor is not introduced and mutual covariance Prepare for the subsequent calculation of the fading factor:

[0097]

[0098] In the formula, is the observation equation; is the predicted value of the Sigma point observation when the fading factor is not introduced; R k+1 is the observation noise; The predicted value of the observed quantity when the fading factor is not introduced; The covariance of the observations when the fading factor is not introduced; The cross-covariance of the observations when no fading factor is introduced.

[0099] S3.6. The state prediction covariance without the introduction of the fading factor calculated previously The covariance of the observations without the introduction of the fading factor and mutual covariance Calculate the fading factor λ k+1 :

[0100]

[0101] In the formula, in the formula, is the residual sequence; τ is the forgetting factor; λ 0 is the original calculated value of the fading factor; tr(·) represents the trace of the matrix; V k+1 is the covariance matrix of the residual sequence.

[0102] S3.7, Introducing the fading factor λ k+1 , recalculate the state prediction covariance P after introducing the fading factor k+1|k :

[0103]

[0104] Where P k+1|k The state prediction covariance after the introduction of the fading factor;

[0105] S3.8. Predict the mean value based on the state without the introduction of the fading factor And the state prediction covariance P after the introduction of the fading factor k+1|k , sample again to obtain a new set of Sigma points:

[0106]

[0107] In the formula, χ i,k+1|k is the state prediction mean based on the absence of the fading factor And the state prediction covariance P after the introduction of the fading factor k+1|k The obtained Sigma points.

[0108] S3.9. Substitute the Sigma point into the observation equation of the system and calculate the predicted value of the observation after the fading factor is introduced. And further calculate the observation covariance P after introducing the fading factor zz and the cross-covariance P xz :

[0109] γ i,k+1|k =h(χ i,k+1|k ) (twenty two)

[0110]

[0111] In the formula, γ i,k+1|k is the predicted value of the Sigma point observation after the fading factor is introduced; is the predicted value of the observed value after the fading factor is introduced; P zz is the observed covariance after the fading factor is introduced; P xz is the cross-covariance of the observations after the fading factor is introduced.

[0112] S3.10, obtain new measurement value z k+1 Then, calculate the Kalman gain K k+1 And the mean of the state and variance P k+1 To update:

[0113]

[0114] In the formula, z k+1 is the measured value; K k+1 is the Kalman gain; is the state mean; P k+1 is the variance.

[0115] It will be understood by those skilled in the art that, unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as those generally understood by those skilled in the art in the art to which the present invention belongs. It should also be understood that terms such as those defined in common dictionaries should be understood to have meanings consistent with the meanings in the context of the prior art, and will not be interpreted with idealized or overly formal meanings unless defined as herein.

[0116] The specific implementation methods described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific implementation method of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for estimating the state of an intelligent vehicle, characterized in that: Here are the steps: S1. Establish a dynamic model of the intelligent vehicle including three degrees of freedom: longitudinal, lateral and yaw; S2. According to the established intelligent vehicle dynamics model, select the state quantity, observation quantity and system input of the intelligent vehicle state estimation system, and establish the state equation and observation equation of the intelligent vehicle state estimation system; S3. Combining unscented Kalman filtering and strong tracking filtering, a high-precision and robust estimation algorithm for the state of the smart car is designed to estimate the state of the smart car.

2. The method for estimating the state of an intelligent vehicle according to claim 1, characterized in that: In S1, the longitudinal dynamics model of the smart car is: In the formula, v x is the longitudinal velocity of the vehicle; v y is the lateral velocity of the vehicle; γ is the yaw rate of the vehicle; m is the mass of the vehicle; ∑F x is the total tire longitudinal force; F w F is the air resistance of the vehicle; f is the rolling resistance of the vehicle tire; a x is the longitudinal acceleration of the vehicle; C d is the air resistance coefficient; A is the frontal area of ​​the vehicle; ρ is the air density; g is the acceleration of gravity, and f is the rolling resistance coefficient.

3. The method for estimating the state of an intelligent vehicle according to claim 1, characterized in that: In S1, the lateral dynamics model of the smart car is: In the formula, ∑F y is the total tire lateral force; a y is the lateral acceleration of the vehicle; F xi 、F yi are the longitudinal force and lateral force of each tire respectively, and the subscript i = {fl, fr, rl, rr} represents the left front wheel, right front wheel, left rear wheel and right rear wheel respectively; δ f is the front wheel turning angle.

4. The method for estimating the state of an intelligent vehicle according to claim 1, characterized in that: In S1, the yaw dynamics model of the smart car is: In the formula, I z is the moment of inertia of the vehicle around the Z axis; ∑M z is the total vehicle yaw moment; a is the distance from the vehicle center of mass to the front axle; b is the distance from the vehicle center of mass to the rear axle; c is the vehicle wheelbase.

5. The method for estimating the state of an intelligent vehicle according to claim 1, characterized in that: In S2, the state quantity of the intelligent vehicle state estimation system is selected as x=[v x v y γ] T , the observed quantity is z = [a x a y ] T , the input of the system is u=[δ f F xi F yi ] T , where i = {fl, fr, rl, rr}.

6. The method for estimating the state of an intelligent vehicle according to claim 5, characterized in that: The state equation of the smart car state estimation system is:

7. The method for estimating the state of an intelligent vehicle according to claim 5, characterized in that: The observation equation of the smart car state estimation system is:

8. The method for estimating the state of an intelligent vehicle according to claim 1, characterized in that: In S3, the smart car state high-precision robust estimation algorithm specifically includes the following steps: S3.1, Initialization: In the formula, x0 and are the initial value and mean of the state quantity respectively; P0 is the initial covariance of the state quantity; S3.

2. According to the mean of the state quantity at time k and covariance P k A symmetric sampling strategy is used to select 2n+1 Sigma points and their weights: In the formula, χ i,k is the sampling value of the i-th Sigma point at time k; λ = α 2 (n+κ)-n is a scaling parameter used to reduce the total prediction error; α is an adjustment parameter used to control the distribution of Sigma points around the mean; κ is an adjustable parameter used to improve the accuracy of the approximation; n is a natural constant; is the weight for calculating the mean of the sampling points; The weight for calculating the variance of sampling points; S3.

3. According to the state equation of the intelligent vehicle state estimation system, the state of the Sigma point is predicted, and the state prediction covariance when the fading factor is not introduced is calculated. x i,k+1|k =f(x i,k ,u k ) (9) In the formula, u k is the input of the system at time k; ξ i,k+1|k is the predicted value of the Sigma point state when the fading factor is not introduced; f(χ i,k ,u k ) is the state equation; Q k is the process noise; is the state prediction mean when the fading factor is not introduced; S3.

4. Predict the mean value based on the state without the introduction of the fading factor and the state prediction covariance without the introduction of the fading factor Sample again to obtain a new set of Sigma points: In the formula, is the state prediction mean based on the absence of the fading factor and covariance The obtained Sigma point; S3.

5. Substitute the newly obtained Sigma point set into the observation equation of the intelligent vehicle state estimation system and calculate the predicted value of the observation quantity when the fading factor is not introduced. And calculate the predicted quantity covariance when the fading factor is not introduced and mutual covariance Prepare for the subsequent calculation of the fading factor: In the formula, is the observation equation; is the predicted value of the Sigma point observation when the fading factor is not introduced; R k+1 is the observation noise; S3.

6. The state prediction covariance calculated without the introduction of the fading factor The covariance of the observations without the introduction of the fading factor and mutual covariance Calculate the fading factor λ k+1 ; S3.7, Introducing the fading factor λ k+1 , recalculate the state prediction covariance P after introducing the fading factor k+1|k : Where P k+1|k The state prediction covariance after the introduction of the fading factor; S3.

8. Predict the mean value based on the state without the introduction of the fading factor And the state prediction covariance P after the introduction of the fading factor k+1|k , sample again to obtain a new set of Sigma points: In the formula, χ i,k+1|k is the state prediction mean based on the absence of the fading factor And the state prediction covariance P after the introduction of the fading factor k+1|k The obtained Sigma point; S3.

9. Substitute the Sigma point into the observation equation of the smart car state estimation system and calculate the predicted value of the observation after the fading factor is introduced. And calculate the observation covariance P after introducing the fading factor zz and the cross-covariance P xz : c i,k+1|k =h(χ i,k+1|k ) (22) In the formula, γ i,k+1|k is the predicted value of the Sigma point observation after the fading factor is introduced; S3.10, obtain new measurement value z k+1 Then, calculate the Kalman gain K k+1 And the mean of the state and variance P k+1 to update.

9. The method for estimating the state of an intelligent vehicle according to claim 8, characterized in that: The state prediction covariance obtained according to the calculation without introducing the fading factor The covariance of the observations without the introduction of the fading factor and mutual covariance Calculate the fading factor λ k+1 , specifically: In the formula, is the residual sequence; τ is the forgetting factor; λ0 is the original calculated value of the fading factor; tr(·) represents the trace of the matrix; V k+1 is the covariance matrix of the residual sequence.

10. The method for estimating the state of an intelligent vehicle according to claim 8, characterized in that: The new measurement value z is obtained k+1 Then, calculate the Kalman gain K k+1 And the mean of the state and variance P k+1 Update, specifically: In the formula, z k+1 is the measured value; K k+1 is the Kalman gain; is the state mean; P k+1 is the variance.

Images