Intelligent vehicle state estimation method

Abstract

The application provides a smart vehicle state estimation method, first, a smart vehicle dynamics model including three degrees of freedom of longitudinal, lateral and yaw is established; second, according to the established smart vehicle dynamics model, state variables, observation variables and inputs of the smart vehicle state estimation system are selected, and state equations and observation equations of the smart vehicle state estimation system are established; finally, a smart vehicle state estimation algorithm with high estimation accuracy and strong robustness is designed by combining the unscented Kalman filter and the strong tracking filter. The method can realize high-precision robust estimation of the smart vehicle state, and provides accurate vehicle state signals for motion control such as maneuverability and stability of the smart vehicle.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of auxiliary driving of an automobile, and in particular to a method for estimating the state of an intelligent vehicle. BACKGROUND

[0002] The acquisition of vehicle state signals is an important prerequisite for the motion control of a vehicle, and the accuracy of the acquired vehicle state signals is closely related to the vehicle control effect. However, some vehicle state signals need to be collected by using very expensive sensors, which greatly increases the production cost of the vehicle. In addition, some important vehicle state signals, such as the vehicle mass center side slip angle and the vehicle body roll angle, are difficult to directly measure by using sensors. Therefore, it is necessary to design a corresponding vehicle state signal estimation method to indirectly acquire the state information of the vehicle by an analytical method, so as to provide accurate and reliable signals for the motion planning and dynamics control of the vehicle.

[0003] In the existing vehicle state signal estimation method, 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 needs to linearize the nonlinear function, which leads to linearization error and subsequent decline 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 system model uncertainty due to the introduction of a fading factor, but it inevitably has the shortcomings of the extended Kalman filter.

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

[0005] The purpose of the present application is to provide a method for estimating the state of an intelligent vehicle to achieve high-precision and robust estimation of the state of the intelligent vehicle and provide accurate vehicle state signals for the maneuverability, stability and other motion control of the intelligent vehicle.

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

[0007] S1, establishing an intelligent vehicle dynamics model including three degrees of freedom of longitudinal, lateral and yaw;

[0008] S2, selecting the state quantity, observation quantity and input of the intelligent vehicle state estimation system according to the established intelligent vehicle dynamics model, and establishing the state equation and observation equation of the intelligent vehicle state estimation system;

[0009] S3. A high-precision and robust state estimation algorithm for intelligent vehicles is designed by combining unscented Kalman filtering and strong tracking filtering, which is used to estimate the state of intelligent vehicles.

[0010] Preferably, in S1, the longitudinal dynamics model of the intelligent vehicle is:

[0011]

[0012] In the formula, v x v is the longitudinal velocity of the vehicle. y Let be the lateral velocity of the vehicle; γ be the yaw rate of the vehicle; m be the mass of the vehicle; ∑F x F represents the total longitudinal force of the tire. w F represents the air resistance experienced by the vehicle. f The rolling resistance of the vehicle tires; a x C is the longitudinal acceleration of the vehicle. d ρ is the air resistance coefficient; A is the vehicle's frontal area; ρ is the air density; g is the acceleration due to gravity; and f is the rolling resistance coefficient.

[0013] Preferably, in S1, the lateral dynamics model of the intelligent vehicle is:

[0014]

[0015] In the formula, ∑F y The total lateral force of the tire; a y F is the lateral acceleration of the vehicle. xi F yi These represent the longitudinal and lateral forces of each tire, respectively, with subscripts i = {fl, fr, rl, rr} indicating the left front tire, right front tire, left rear tire, and right rear tire, respectively; δ f This refers to the steering angle of the front wheels.

[0016] Preferably, in S1, the yaw dynamics model of the intelligent vehicle is as follows:

[0017]

[0018] In the formula, I z Let M be the moment of inertia of the vehicle about the Z-axis; z denoted as yaw moment; a is the distance from the vehicle's center of gravity to the front axle; b is the distance from the vehicle's center of gravity to the rear axle; c is the vehicle's track width.

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

[0020] Preferably, the state equation of the intelligent vehicle state estimation system is:

[0021]

[0022] Preferably, the observation equation of the intelligent vehicle state estimation system is:

[0023]

[0024] Preferably, in step S3, the high-precision robust estimation algorithm for the intelligent vehicle state specifically includes the following steps:

[0025] S3.1 Initialization:

[0026]

[0027] In the formula, x0 and These are the initial values ​​and mean values ​​of the state variables, respectively; P0 is the initial covariance of the state variables.

[0028] S3.2, Based on the mean of the state variables at time k 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 Let λ be the sampled value of the i-th Sigma point at time k; λ = α 2 (n+κ)-n is the 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. The weights are used to calculate the mean of the sampling points; Weights for calculating the variance of sampling points;

[0031] S3.3. Based on the state equation of the intelligent vehicle state estimation system, predict the state of the Sigma point and calculate the state prediction covariance without introducing the fading factor.

[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 f(χ) represents the predicted state of the Sigma point without the introduction of a fading factor. i,k ,u k ) represents the state equation; Q k This is process noise; The mean state prediction without the introduction of the fading factor;

[0035] S3.4 Predict the mean based on the state without introducing the fading factor. State prediction covariance without introducing the fading factor Resampling yields a new set of Sigma points:

[0036]

[0037] In the formula, The mean of the state predictions without the introduction of the fading factor. Covariance The obtained Sigma points;

[0038] S3.5. Substitute the newly obtained Sigma point set into the observation equation of the intelligent vehicle state estimation system to calculate the predicted values ​​of the observations without introducing the fading factor. And calculate the covariance of the predictors without introducing the fading factor. and mutual covariance This is to prepare for the subsequent calculation of the fading factor.

[0039]

[0040] In the formula, The observation equation; R represents the predicted value of the Sigma point observation without introducing the fading factor. k+1 To observe noise;

[0041] S3.6 Predict the covariance based on the calculated state without introducing the fading factor. Observation covariance without introducing a fading factor and mutual covariance Calculate the fading factor λ k+1 ;

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

[0043]

[0044] In the formula, P k+1|k To predict the covariance of the state after introducing the fading factor;

[0045] S3.8 Predict the mean based on the state without introducing the fading factor. and the state prediction covariance P after introducing the fading factor k+1|k A new set of Sigma points is obtained by resampling:

[0046]

[0047] In the formula, χ i,k+1|k The mean of the state predictions without the introduction of the fading factor. and the state prediction covariance P after introducing the fading factor k+1|k The obtained Sigma points;

[0048] S3.9 Substitute the Sigma point into the observation equation of the intelligent vehicle state estimation system, and calculate the predicted value of the observation after introducing the fading factor. And calculate the observation covariance P after introducing the fading factor. zz and 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 The predicted values ​​of Sigma point observations after introducing a fading factor;

[0052] S3.10 Obtain the 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 Update.

[0053] Preferably, the state prediction covariance calculated based on the state without introducing the fading factor... Observation covariance without introducing a fading factor and mutual covariance Calculate the fading factor λ k+1 Specifically:

[0054]

[0055] In the formula, in the formula, V 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; Vk+1 Let be 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 The update will be performed as follows:

[0057]

[0058] In the formula, z k+1 For measurement; K k+1 Kalman gain; P is the state mean; k+1 Let Variance be the variance.

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

[0060] This method integrates the advantages of unscented Kalman filtering algorithm, which has high approximation accuracy for nonlinear systems, and strong tracking filtering algorithm, which has strong robustness of fading factors to system model uncertainties. Furthermore, it adopts an equivalent calculation method for fading factors to avoid solving the Jacobian matrix of nonlinear systems. As a result, the designed state estimation method can provide high-precision and robust estimation of the state of intelligent vehicles, providing accurate vehicle state signals for motion control such as handling and stability of intelligent vehicles. Therefore, it has broad market application prospects. Attached Figure Description

[0061] Figure 1 This is a schematic diagram of the intelligent vehicle state estimation method of the present invention. Detailed Implementation

[0062] The technical solution of the present invention will now be described in detail with reference to the accompanying drawings.

[0063] This invention can be implemented in many different forms and should not be considered as limited to the embodiments described herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully express the scope of the invention to those skilled in the art.

[0064] This invention discloses a method for estimating the state of an intelligent vehicle, the steps of which are as follows:

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

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

[0067] The longitudinal dynamics model of the intelligent vehicle is as follows:

[0068]

[0069] The lateral dynamics model of the intelligent vehicle is as follows:

[0070]

[0071] The yaw dynamics model of the intelligent vehicle is as follows:

[0072]

[0073] In the formula, v x v is the longitudinal velocity of the vehicle. y γ is the lateral velocity of the vehicle; γ is the yaw rate of the vehicle; m is the mass of the vehicle; I z Let F be the moment of inertia of the vehicle about the Z-axis; x The total longitudinal force of the tire; ∑F y The total tire lateral force; ∑M z F represents the total vehicle yaw moment. w F represents the air resistance experienced by the vehicle. f F is the rolling resistance of the vehicle tires. xi F yi These represent the longitudinal and lateral forces of each tire, respectively, with subscripts i = {fl, fr, rl, rr} indicating the left front tire, right front tire, left rear tire, and right rear tire, respectively; a x The longitudinal acceleration of the vehicle; a y For the lateral acceleration of the vehicle; δ f 1 is the front wheel steering angle; a is the distance from the vehicle's center of gravity to the front axle; b is the distance from the vehicle's center of gravity to the rear axle; c is the vehicle's track width; C d ρ is the air resistance coefficient; A is the vehicle's frontal area; ρ is the air density; g is the acceleration due to gravity; f is the rolling resistance coefficient.

[0074] S2. Based on the established intelligent vehicle dynamics model, select the state variables, observations, and system inputs of the intelligent vehicle state estimation system, and establish the state equations and observation equations of the intelligent vehicle state estimation system.

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

[0076] The state equation of the intelligent vehicle state estimation system is:

[0077]

[0078] The observation equation for the intelligent vehicle state estimation system is:

[0079]

[0080] S3. Design a high-precision 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, x0 and Let P1 and P2 be the initial values ​​and mean values ​​of the state variables, respectively; P0 is the initial covariance of the state variables.

[0085] S3.2, Based on the mean of the state variables at time k 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 Let λ be the sampled value of the i-th Sigma point at time k; λ = α 2 (n+κ)-n is the 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. The weights are used to calculate the mean of the sampling points; Weights for calculating the variance of sampling points; P is the mean of the state variables at time k; k Let be the covariance of the state variables at time k.

[0089] S3.3. Perform a one-step prediction of the Sigma point based on the state equation of the nonlinear system, and calculate the state prediction covariance 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 f(χ) represents the predicted state of the Sigma point without the introduction of a fading factor. i,k ,u k ) represents the state equation; Q k This is process noise; The mean state prediction without the introduction of the fading factor; This represents the state prediction covariance without the introduction of a fading factor.

[0093] S3.4 Predict the mean based on the state without introducing the fading factor. State prediction covariance without introducing the fading factor Resampling yields a new set of Sigma points:

[0094]

[0095] In the formula, The mean of the state predictions without the introduction of the fading factor. Covariance The obtained Sigma point.

[0096] S3.5. Substitute the newly obtained Sigma point set into the system's observation equations to calculate the predicted values ​​of the observations without introducing the fading factor. Furthermore, the covariance of the predictor was calculated without the introduction of the fading factor. and mutual covariance This is to prepare for the subsequent calculation of the fading factor.

[0097]

[0098] In the formula, The observation equation; R represents the predicted value of the Sigma point observation without introducing the fading factor. k+1 To observe noise; Predicted values ​​of observations without introducing a fading factor; The covariance of observations without introducing a fading factor; Cross-covariance of observations without introducing a fading factor.

[0099] S3.6 Predict the covariance based on the previously calculated state without introducing the fading factor. Observation covariance without introducing a fading factor and mutual covariance Calculate the fading factor λ k+1 :

[0100]

[0101] In the formula, in the formula, V 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 Let be the covariance matrix of the residual sequence.

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

[0103]

[0104] In the formula, P k+1|k To predict the covariance of the state after introducing the fading factor;

[0105] S3.8 Predict the mean based on the state without introducing the fading factor. and the state prediction covariance P after introducing the fading factor k+1|k A new set of Sigma points is obtained by resampling:

[0106]

[0107] In the formula, χ i,k+1|k The mean of the state predictions without the introduction of the fading factor. and the state prediction covariance P after introducing the fading factor k+1|k The obtained Sigma point.

[0108] S3.9 Substitute the Sigma point into the system's observation equation and calculate the predicted value of the observation after introducing the fading factor. Furthermore, the covariance P of the observed measurements after introducing the fading factor was calculated. zz and 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 The predicted values ​​of Sigma point observations after introducing a fading factor; P represents the predicted value of the observed values ​​after introducing the fading factor. zz The covariance of the observations after introducing the fading factor; P xz The cross-covariance of observations after introducing the fading factor.

[0112] S3.10 Obtain the 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 Update:

[0113]

[0114] In the formula, z k+1 For measurement; K k+1 Kalman gain; P is the state mean; k+1 Let Variance be the variance.

[0115] It will be understood by those skilled in the art that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. It should also be understood that terms such as those defined in general dictionaries should be understood to have the same meaning as in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless defined as herein.

[0116] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above description is only a specific embodiment 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 within the protection scope of the present invention.

Claims

1. A method of intelligent vehicle state estimation, characterized by, The steps are as follows: S1, a smart car dynamics model including three degrees of freedom of longitudinal, lateral and yaw is established; S2, according to the established smart car dynamics model, the state quantity, observation quantity and system input of the smart car state estimation system are selected, and the state equation and observation equation of the smart car state estimation system are established; S3, the smart car state high-precision robust estimation algorithm is designed by combining the unscented Kalman filter and the strong tracking filter, so as to estimate the state of the smart car; In the S1, the smart car longitudinal dynamics model is: where v x is the longitudinal speed of the vehicle; v y is the lateral speed of the vehicle; γ is the yaw rate of the vehicle; m is the mass of the vehicle; ∑F x is the total longitudinal tire force; F w is the air resistance experienced by the vehicle; F f is the tire rolling resistance of the vehicle; a x is the longitudinal acceleration of the vehicle; C d is the air resistance coefficient; A is the vehicle frontal area; p is the air density; g is the gravitational acceleration, and f is the rolling resistance coefficient; In the S1, the smart car lateral dynamics model is: where ∑F y is the total tire lateral force; a y is the vehicle lateral acceleration; F xi , F yi are the longitudinal and lateral forces of each tire, respectively, with subscript i = {fl, fr, rl, rr} representing the front left, front right, rear left, and rear right wheels, respectively; δ f is the front wheel steering angle.

2. The intelligent vehicle state estimation method of claim 1, wherein In the S1, the smart car yaw dynamics model is: where I z is the moment of inertia of the vehicle about 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; and c is the vehicle track.

3. The intelligent vehicle state estimation method of claim 1, wherein, In S2, the state variable of the intelligent vehicle state estimation system is selected as x = [v x v y ] T , the observation variable is z = [a x a y ] T , and the input of the system is u = [δ f F xi F yi ] T , wherein i = {fl, fr, rl, rr}.

4. The intelligent vehicle state estimation method of claim 3, wherein, The state equation of the smart car state estimation system is:

5. The intelligent vehicle state estimation method of claim 3, wherein, The observation equation of the smart car state estimation system is:

6. The intelligent vehicle state estimation method of claim 1, wherein, In the S3, the smart car state high-precision robust estimation algorithm specifically includes the following steps: S3.1, initialization: where x0and are the initial value and mean value of the state quantity, respectively; P0is the initial covariance of the state quantity; S3.2, mean of state quantities at time k and covariance P k 2n+1 Sigma points and their weights are selected using a symmetric sampling strategy: where χ i,k Xi(k) is the sample value of the ith Sigma point at time k; λ = α 2 (n + κ) - n is a scaling parameter for reducing the total prediction error; α is an adjustment parameter for controlling the distribution of Sigma points around the mean; κ is an adjustable parameter for improving the approximation accuracy; n is a natural constant; is the weight for calculating the sample point mean; is the weight for calculating the sample point variance; S3.3, state prediction of the Sigma points according to the state equation of the intelligent vehicle state estimation system, and calculation of the state prediction covariance without introducing the fading factor ξ i,k+1|k = f(x i,k , u k ) (9) where u k is the input of the system at time k; ξ i,k+1|k is the predicted state of the Sigma point without fading factor; f(x i,k , u k ) is the state equation; Q k is the process noise; is the predicted mean of the state without fading factor; S3.4, State prediction mean without fading factor and state prediction covariance without fading factor Resample to obtain a new set of Sigma points: wherein is the state prediction mean based on no fading factor introduced and covariance resulting Sigma points; S3.5, bring the new set of Sigma points into the observation equation of the intelligent vehicle state estimation system, calculate the observation prediction value without introducing the fading factor and the prediction covariance and the cross covariance Prepare for subsequent calculation of fading factor: wherein is the observation equation; is the Sigma point observation prediction without fading factor; R k+1 is the observation noise; S3.6, the state prediction covariance calculated without introducing the fading factor the observation covariance without introducing the fading factor and the cross covariance calculating the fading factor λ k+1 ; S3.7, Introduce fading factor λ k+1 , Recompute state prediction covariance P after introducing fading factor λ k+1|k : In the formula, P k+1|k is the state prediction covariance after the fading factor is introduced; S3.8, State prediction mean without fading factor and state prediction covariance P with fading factor k+1|k A new set of Sigma points is obtained by resampling: where χ i,k+1|k is the state prediction mean based on the state prediction without the fading factor and the state prediction covariance P with the fading factor k+1|k the resulting Sigma points; S3.9, bring the sigma point into the observation equation of the intelligent vehicle state estimation system, calculate the observation prediction value after introducing the fading factor and calculate the observation covariance P zz and cross-covariance P xz : γ i,k+1|k = h(x 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 z k+1 Then, compute Kalman gain K k+1 and update the mean and variance P k+1 of the state.

7. The intelligent vehicle state estimation method of claim 6, wherein, the state prediction covariance calculated without introducing the fading factor the observation covariance calculated without introducing the fading factor and the cross covariance calculating the fading factor λ k+1 , specifically: wherein is the residual sequence; τ is a forgetting factor; λ0is the original calculated value of the fading factor; tr(·) denotes the trace of a matrix; V k+1 is the covariance matrix of the residual sequence.

8. The intelligent vehicle state estimation method of claim 6, wherein, 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 The update will be performed as follows: where z k+1 is a measurement value; K k+1 is a Kalman gain; is a state mean; P k+1 is a variance.

Images