Vehicle state estimation method considering noise disturbance and quality parameter mismatch

Abstract

The invention provides a vehicle state estimation method considering noise disturbance and quality parameter mismatch. The method comprises the steps that the longitudinal acceleration, the front wheel steering angle and the transverse acceleration of a vehicle are obtained through a vehicle-mounted sensor; based on the obtained longitudinal acceleration, the front wheel steering angle and the transverse acceleration, a nonlinear three-degree-of-freedom vehicle dynamics model of the longitudinal direction, the transverse direction and the yawing direction of the vehicle is established, discretization is conducted on the established model, and unscented transformation is adopted to process the nonlinear transmission problem of the mean value and the covariance; an unscented Kalman filter is constructed based on the model to carry out state prediction and prediction correction on a vehicle system, and a strong tracking thought and adaptive filtering are introduced to deal with a model parameter mismatch problem and noise disturbance uncertainty. According to the method, environmental interference with uncertain noise statistical characteristics and complex conditions with uncertain vehicle model parameters are considered, vehicle-mounted sensor information and measurement information can be fully utilized, and accurate estimation of vehicle state parameters under uncertain external disturbance is achieved.

Description

Technical Field

[0001] The present invention relates to a vehicle state estimation method for model mismatch and insufficient observer performance, and in particular to a vehicle state estimation method considering noise disturbance and quality parameter mismatch, which is an essential link in unmanned driving and active safety control. Background Art

[0002] With the continuous advancement of intelligent networking of automobiles and the continuous development of artificial intelligence, the intelligent driving of automobiles has received widespread attention. In order to reduce the occurrence of traffic accidents and promote the development of driverless vehicles, the application of ADAS (Advanced Driver Assistance System) is becoming more and more common. The accurate acquisition of vehicle status is not only a prerequisite for the realization of ADAS, but also an important guarantee for the effective operation of active safety systems. Nowadays, key vehicle status information such as the sideslip angle of the center of mass and the yaw rate are difficult to be directly measured by on-board sensors, and the use of expensive sensors to obtain them faces the problem of being unable to be mass-produced. Therefore, many vehicle state estimation methods that combine low-cost on-board sensors with advanced algorithms have emerged. Most vehicle state estimation algorithms usually require an accurate vehicle dynamics model to ensure the estimation accuracy, but due to the attributes of the vehicle as a means of transportation, the mass of the vehicle often changes. At present, there are few state estimation algorithms that consider the disturbance caused by the mismatch of the mass parameters of the vehicle. Therefore, it is necessary to propose a vehicle state estimation method that considers noise disturbance and mass parameter mismatch to accurately estimate the state of the transport vehicle. Summary of the invention

[0003] The technical problem to be solved by the present invention is to provide a vehicle state estimation method that takes into account noise disturbance and quality parameter mismatch, which suppresses uncertain noise interference and improves the robustness of the algorithm when facing model mismatch problems.

[0004] The present invention adopts the following technical solutions to solve the above problems:

[0005] The present invention first provides a vehicle state estimation method considering noise disturbance and quality parameter mismatch, comprising:

[0006] The vehicle's longitudinal acceleration, front wheel steering angle and lateral acceleration are obtained using on-board sensors;

[0007] Based on the acquired longitudinal acceleration, front wheel steering angle and lateral acceleration of the vehicle, a nonlinear three-degree-of-freedom vehicle dynamics model of the longitudinal, lateral and yaw of the vehicle is established;

[0008] The established nonlinear three-degree-of-freedom vehicle dynamics model is discretized, and the traceless transformation is used to deal with the nonlinear transfer problem of mean and covariance.

[0009] Adaptive filtering is introduced into the unscented Kalman filter to deal with the uncertainty of noise disturbance, and an adaptive unscented Kalman filter is constructed based on the maximum a posteriori estimation criterion;

[0010] In the prediction step, the strong tracking idea is integrated into the unscented Kalman filter by introducing the fading factor, and the state error covariance matrix is ​​dynamically adjusted to construct a strong tracking unscented Kalman filter.

[0011] A filter divergence criterion is introduced, and the adaptive unscented Kalman filter and the strong tracking unscented Kalman filter are switched according to whether the criterion is met or not. The discretized vehicle system model is predicted and corrected based on the constructed strong tracking and adaptive unscented Kalman joint estimation algorithm.

[0012] Aiming at the demand of obtaining high-precision key state parameters of vehicles in uncertain environments with vehicle model mismatch and noise disturbance, a single vehicle key state parameter estimation method is provided. Under non-ideal conditions, the longitudinal and lateral motions of the vehicle can be described as nonlinear systems. Therefore, a state estimation algorithm framework is built based on the Unscented Kalman Filter (UKF). In harsh environments such as humidity and low temperature, the vehicle sensors will have low estimation accuracy due to the influence of observation noise with uncertain statistical characteristics. Therefore, an adaptive Kalman filter is constructed based on the maximum a posteriori estimation criterion (MAP) to estimate and correct the measurement noise variance matrix in real time online to suppress the interference of uncertain noise. As a vehicle is a transport equipment, the fluctuation of model parameters such as mass is often not negligible, which will also lead to reduced estimation accuracy or even filter divergence. Therefore, the strong tracking idea is integrated into the Unscented Kalman Filter by introducing a fading factor to deal with the model parameter perturbation problem and improve the robustness of the estimation algorithm.

[0013] Compared with the existing methods, the present invention has the following advantages:

[0014] 1. The present invention takes into account the environmental interference of uncertain noise and the characteristics of vehicle model parameter perturbations when the vehicle is moving, can make full use of the on-board sensor information and measurement information, solves the problem of reduced estimation accuracy and even filtering divergence of the vehicle under the above conditions, and realizes accurate estimation of the vehicle state under non-ideal conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 It is the nonlinear three-degree-of-freedom vehicle dynamics model on which the state estimation algorithm described in the present invention is based.

[0016] Figure 2 It is a block diagram of the unscented Kalman filter algorithm described in the present invention.

[0017] Figure 3It is a block diagram of the Strong Tracking and Adaptive Unscented Kalman Joint Estimation (STSHAUKF) algorithm described in the present invention. DETAILED DESCRIPTION

[0018] In order to explain the content of the present invention in more detail, the structural features, technical content and realization purpose of the present invention are now fully described in combination with the accompanying drawings and formulas. The accompanying drawings are only simplified schematic diagrams, so the basic structure of the present invention is only illustrated in a schematic manner.

[0019] The present invention first provides a vehicle state estimation method considering noise disturbance and quality parameter mismatch, which specifically includes the following steps:

[0020] Step 1: Use vehicle-mounted sensors to obtain vehicle motion state information, where the vehicle motion state information includes the vehicle's longitudinal acceleration, front wheel steering angle, and lateral acceleration.

[0021] Step 2:

[0022] (1) Establishment of three-degree-of-freedom nonlinear vehicle dynamics model

[0023] A three-degree-of-freedom dynamic model considering the longitudinal, lateral and yaw of the vehicle is adopted, such as Figure 1 As shown, the state space equation is as follows:

[0024]

[0025] The measurement equation is as follows:

[0026]

[0027] Where m represents the vehicle mass, I z represents the moment of inertia of the vehicle around the z-axis, a and b represent the distance from the center of mass of the vehicle to the front and rear axles, respectively, and k f ,k r Denote the equivalent cornering stiffness of the front and rear wheels, γ, β, v x Represent the vehicle’s yaw rate, sideslip angle and longitudinal velocity, respectively. Respectively represent the time derivatives of the vehicle's yaw rate, center of mass sideslip angle and longitudinal velocity; δ, a x ,a y They represent the vehicle's front wheel steering angle, longitudinal acceleration and lateral acceleration respectively, which are obtained by on-board sensors.

[0028] The state variable is defined as x = [γβv x ] T , the observed variable is defined as z = a y , the input variable is defined as u=[δa x ] T, and construct a discretized three-degree-of-freedom dynamic model of the vehicle system based on the state space equation and measurement equation of the vehicle system:

[0029]

[0030] Among them, x k ,x k-1 are the vehicle system state variables at time k-1 and k respectively; u k-1 ,u k are the system input at time k-1 and k respectively; f(.) is the nonlinear state transfer function of the vehicle model; h(.) is the nonlinear observation equation function, w k-1 and v k are the process noise of the vehicle model at time k-1 and the measurement noise of the model at time k, respectively. k-1 The mean value of k-1 , the covariance matrix is ​​Q k ,v k The mean value is r k , the covariance matrix is ​​R k .

[0031] (2) Construction of the Unscented Kalman Filter Algorithm Framework

[0032] Using unscented transformation, the state variable x at time k-1 is k-1 and the covariance matrix P k-1 We get 2n+1 Sigma points, namely:

[0033]

[0034] Where λ = α 2 (n+κ)-n is the composite scale factor, κ is the parameter to be selected in the traceless transformation, which is a fixed value, usually taken as 2, and α is a fixed parameter that controls the distribution of Sigma points, and its value range is usually 10 -4 <α<1, and Represents the i-th column of the square root of the matrix.

[0035] The weight coefficient of each Sigma point is:

[0036]

[0037] Where: and are the initial values ​​of the state quantity weight and covariance weight respectively; and are the i-th parameters of the state quantity weight and covariance weight respectively; b w is the regularization coefficient, which is greater than 0 and usually takes a value of 2.

[0038] The above continuous nonlinear vehicle model is discretized, and the state variable x is obtained at each moment through unscented transformation. k 2n+1 Sigma points, the framework of the unscented Kalman filter algorithm can be constructed based on the Kalman filter algorithm, such as Figure 2 The algorithm framework is mainly divided into prediction step and correction step, which are described as follows:

[0039] ① Prediction step

[0040] Calculate the one-step prediction of 2n+1 points and take the weighted average to get the one-step prediction of the system state quantity:

[0041] X i,k|k-1 =f(X i，k-1 ,u k-1 ,k-1)+q k-1

[0042]

[0043] According to the one-step prediction value point set, we substitute it into the observation equation to obtain the predicted observation value, i=0,2,...,2n.

[0044] Y (i) (k+1|k)=h(X i，k+1|k ,u k , k+1)+r k

[0045]

[0046] in, is the prior estimate at time k, is the posterior estimate at time k-1, P k|k-1 is the prior error covariance matrix at time k-1, P k-1 is the posterior error covariance matrix at time k-1.

[0047] ②Calibration step

[0048] The prediction covariance matrix is:

[0049]

[0050] The cross-correlation matrix is:

[0051]

[0052] Compute the Kalman gain matrix, compute the system state update, and update the covariance matrix:

[0053]

[0054] in, is the estimated value of the state at time k, P k is the posterior error covariance matrix at time k, R k is the covariance matrix of the system measurement noise at time k.

[0055] Step 3:

[0056] (1) Constructing an adaptive unscented Kalman filter based on the maximum a posteriori estimation criterion (MAP)

[0057] Defining ε k is the observed value z at time k k and the predicted value at time k The difference between the two is used to update the measurement noise covariance matrix in real time, so as to estimate and correct the measurement noise in real time to suppress the influence of uncertain noise on the estimation accuracy. The process of noise variance matrix processing is as follows:

[0058]

[0059] λ k =(1-d) / (1-d k+1 )

[0060]

[0061] Y (i) (k+1|k)=h(X i，k+1|k ,u k , k+1)+r k

[0062] Among them, λ k is the forgetting iteration factor, d is the Sage-Husa coefficient, and its value range is generally [0.9, 1].

[0063] (2) Constructing a strong tracking unscented Kalman filter

[0064] As mentioned above, the MAP-based adaptive filtering algorithm can estimate and correct the measurement noise in real time, which can effectively suppress the interference of uncertain measurement noise. In practical applications, the parameters of the vehicle model are usually time-varying, and the model mismatch problem is also an unavoidable non-ideal condition. The problem of reduced estimation accuracy caused by model mismatch also needs to be solved.

[0065] When the model parameters such as vehicle mass are time-varying, the weights of new measurements and old data always remain unchanged when estimating the vehicle state, resulting in poor estimation performance. The accumulation of old data will reduce the impact of new measurements on the predicted value. In order to solve this problem, the present invention introduces a fading factor into the prediction step in the unscented Kalman filter framework to integrate the strong tracking idea into the unscented Kalman filter, dynamically adjust the state error covariance matrix, and define is the new information, whose value is the observation value z at time k+1 k+1 and the predicted value at time k The difference. Introducing the fading factor matrix Dynamically adjust the state error covariance matrix, and then adjust the gain matrix K of the Kalman filter k Based on the orthogonality principle, the new information sequence is orthogonal at different times, and the fading factor matrix is ​​selected based on this principle. The algorithm processing flow is as follows.

[0066] Adjust the covariance matrix one-step forecast to:

[0067]

[0068] The selected gain matrix needs to ensure that the algorithm can achieve the optimal estimate and that the innovation vectors are orthogonal at different times, so the following constraints must be met:

[0069]

[0070] Under the above constraints, the calculation process of the fading factor matrix is ​​as follows:

[0071]

[0072] where α i It is a constant determined based on the prior information of the system state, reflecting the system state attenuation rate. Its size is positively correlated with the speed of system state change, and is generally taken as α i ≥1, ρ is the forgetting factor, ranging from [0,1], h(k+1) is λ i The standard coefficient of (k+1).

[0073] Step 4:

[0074] The strong tracking unscented Kalman filter can deal with the problems of system model parameter perturbation and inaccurate initial state, and can improve the robustness of the estimation method. However, when the filter convergence is good, the filtering accuracy will decrease due to the miscorrection of the error covariance matrix. The adaptive unscented Kalman filter can deal with the problem of decreased estimation accuracy caused by the unknown statistical characteristics of the observation noise. However, it is easy to misadjust the measurement noise under the condition of model parameter perturbation, resulting in decreased accuracy. It cannot solve the problem of filtering divergence caused by mismatch of model parameters such as quality.

[0075] Therefore, the present invention introduces a filter divergence criterion for complex working conditions with model parameter perturbations and uncertain noise disturbances, and switches between the use of an adaptive unscented Kalman filter and a strong tracking unscented Kalman filter according to whether the criterion is met. Based on the constructed strong tracking and adaptive unscented Kalman joint estimation algorithm, the obtained discretized vehicle system model is predicted and corrected.

[0076] The basic idea of ​​the filter divergence criterion is that the square of the new information sequence should be consistent with its theoretical statistical characteristics, such as:

[0077]

[0078]

[0079] Among them, γ is the reserve coefficient, whose value is not less than 1. The left side of the inequality is the sum of squares of the new information sequence, and the right side is the theoretical value of the covariance of the new information sequence. When the criterion is met, it is considered that the filter converges, and an adaptive unscented Kalman filter is used to improve the filtering accuracy; when the criterion is not met, it is considered that the filter is abnormal, and a strong tracking unscented Kalman filter is used to enhance the robustness and tracking of the state estimation.

[0080] The basic flow chart of the strong tracking and adaptive unscented Kalman joint estimation algorithm (STSHAUKF) proposed in this invention is as follows: Figure 3 Shown

[0081] Step 5:

[0082] The present invention combines unscented Kalman filtering, maximum a posteriori estimation criterion, and strong tracking filter to design the STSHAUKF algorithm, and the specific steps are as follows:

[0083] ①Using UKF as the framework of the algorithm;

[0084] ② Based on the maximum a posteriori estimation criterion, an adaptive unscented Kalman filter is constructed to adjust the noise variance matrix in real time;

[0085] ③Use strong tracking filter to dynamically adjust the error covariance matrix;

[0086] ④ Adopt adaptive unscented Kalman filter or strong tracking filter according to the filter divergence criterion

[0087] The framework flow chart of the STSHAUKF algorithm is shown in Figure x. The framework flow chart is further explained as follows:

[0088] ① Preset fixed parameters κ, α, b in the traceless transformation w , fixed parameter α in strong tracking filter i , ρ and filter divergence criterion reserve coefficient γ

[0089] ② Initialize the state vector and covariance matrix:

[0090]

[0091] ③Use untraceable transformation to obtain Sigma points and calculate new information based on the observed values Observation covariance matrix P yy,k With the cross-correlation matrix P xy,k , bring in the filter divergence criterion to determine whether the filter diverges. If it diverges, go to step 4. If it converges,

[0092] Go to step 5;

[0093] ④ Calculate the fading factor matrix Adjust the covariance matrix P k+1|k , P yy,k , P xy,k ;

[0094] ⑤ Preset the Sage-Husa coefficient d and calculate λ k , adjust the measurement noise covariance matrix R k ;

[0095] ⑥ Calculate the modified Kalman filter gain K k , iteratively update the state vector estimate and the error covariance matrix

[0096] ⑦ Let k=k+1 and return to step 3.

[0097] The above description is only an exemplary 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 principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A vehicle state estimation method considering noise disturbance and quality parameter mismatch, characterized in that: Includes steps: Obtain the vehicle's longitudinal acceleration, front wheel steering angle, and lateral acceleration; Based on the acquired longitudinal acceleration, front wheel steering angle and lateral acceleration of the vehicle, a nonlinear three-degree-of-freedom vehicle dynamics model of the longitudinal, lateral and yaw of the vehicle is established; The established nonlinear three-degree-of-freedom vehicle dynamics model is discretized, and the traceless transformation is used to deal with the nonlinear transfer problem of mean and covariance. Adaptive filtering is introduced into the unscented Kalman filter to deal with the uncertainty of noise disturbance, and an adaptive unscented Kalman filter is constructed based on the maximum a posteriori estimation criterion; in the prediction step, the strong tracking idea is integrated into the unscented Kalman filter by introducing a fading factor, and the state error covariance matrix is ​​dynamically adjusted to construct a strong tracking unscented Kalman filter; A filter divergence criterion is introduced, and the adaptive unscented Kalman filter and the strong tracking unscented Kalman filter are switched according to whether the criterion is met or not; the obtained discretized vehicle system model is predicted and corrected based on the constructed strong tracking and adaptive unscented Kalman joint estimation algorithm.

2. The vehicle state estimation method considering noise disturbance and quality parameter mismatch according to claim 1, characterized in that: Based on the acquired longitudinal acceleration, front wheel steering angle and lateral acceleration of the vehicle, a nonlinear three-degree-of-freedom vehicle dynamics model of longitudinal, lateral and yaw of the vehicle is established. The established nonlinear three-degree-of-freedom vehicle dynamics model is: Where m represents the vehicle mass, I z represents the moment of inertia of the vehicle around the z-axis, a and b represent the distance from the center of mass of the vehicle to the front and rear axles, respectively, and k f , k r Denote the equivalent cornering stiffness of the front and rear wheels, γ, β, v x Represent the vehicle’s yaw rate, sideslip angle and longitudinal velocity, respectively. Respectively represent the time derivatives of the vehicle's yaw rate, center of mass sideslip angle and longitudinal velocity; δ, a x , a y represent the front wheel steering angle, longitudinal acceleration and lateral acceleration of the vehicle respectively, which are obtained by the on-board sensor. The state variable is defined as x = [γ β v x ] T , the observed variable is defined as z = a y , the input variable is defined as u=[δ a x ] T .

3. The vehicle state estimation method considering noise disturbance and quality parameter mismatch according to claim 2, characterized in that: The established nonlinear three-degree-of-freedom vehicle dynamics model is discretized, and the unscented transformation is used to deal with the nonlinear transfer problem of mean and covariance, including: Discretize the established nonlinear three-degree-of-freedom vehicle dynamics model: Among them, x k , x k-1 are the vehicle system state variables at time k and k-1 respectively; u k-1 ,u k are k-1, k-time system inputs respectively; f(.) is the nonlinear state transfer function of the vehicle model; h(.) is the nonlinear observation equation function, w k-1 and v k are the process noise of the vehicle model at time k-1 and the measurement noise of the model at time k, respectively, k-1 The mean value of k-1 , the variance is Q k , v k The mean value is r k , the variance is R k ; Using unscented transformation, the state variable x at time k-1 is k-1 and the covariance matrix P k-1 We get 2n+1 Sigma points, that is Where λ = α 2 (n+κ)-n is the composite scale factor, κ is the parameter to be selected in the traceless transformation, which is a fixed value, and α is a fixed parameter that controls the distribution of Sigma points, with a value range of 10 -4 <α<1, and represents the i-th column of the square root of the matrix; The weight coefficient of each Sigma point is: Where: and are the initial values ​​of the state quantity weight and covariance weight respectively; and are the i-th parameters of the state quantity weight and covariance weight respectively; b w is the regularization coefficient, which is greater than 0; calculate the one-step prediction of 2n+1 points and take the weighted average to obtain the one-step prediction of the system state quantity X i，k|k-1 =f( Xi，k-1 ,u k-1 ,k-1)+q k-1 According to the one-step prediction value point set, the predicted observation value is obtained by substituting it into the observation equation, i = 0, 2, ..., 2n; Y (i) (k+1|k)=h(X i，k+1|k ,u k ,k+1)+r k The prediction covariance matrix is: The cross-correlation matrix is: Compute the Kalman gain matrix, compute the system state update, and update the covariance matrix:

4. The vehicle state estimation method considering noise disturbance and quality parameter mismatch according to claim 3, characterized in that: Adaptive filtering is introduced into the unscented Kalman filter to deal with the uncertainty of noise disturbance. In the step of constructing an adaptive unscented Kalman filter based on the maximum a posteriori estimation criterion, the measurement noise covariance matrix is ​​updated in real time to estimate and correct the measurement noise in real time. The process of processing the noise variance matrix is ​​as follows: l k =(1-d) / (1-d k+1 ) Y (i) (k+1|k)=h(X i，k+1|k ,u k ,k+1)+r k Among them, λ k is the forgetting iteration factor, and d is the Sage-Husa coefficient.

5. The vehicle state estimation method considering noise disturbance and quality parameter mismatch according to claim 4, characterized in that: In the prediction step, the strong tracking idea is integrated into the unscented Kalman filter by introducing the fading factor, and the state error covariance matrix is ​​dynamically adjusted. In the step of constructing the strong tracking unscented Kalman filter, the definition is the new information, whose value is the observation value z at time k+1 k+1 and the predicted value at time k The difference; introduce the vanishing factor matrix Dynamically adjust the state error covariance matrix, and then adjust the gain matrix K of the Kalman filter k Make real-time adjustments; based on the orthogonal principle, ensure that the new information sequence is orthogonal at different times, and select the fading factor matrix based on this principle The algorithm processing flow is as follows Adjust the covariance matrix one-step forecast to: in: where α i It is a constant determined based on the prior information of the system state, reflecting the system state decay rate. Its size is positively correlated with the speed of system state change. ρ is the forgetting factor, ranging from [0, 1], and h(k+1) is λ i The standard coefficient of (k+1).

6. The vehicle state estimation method considering noise disturbance and quality parameter mismatch according to claim 5, characterized in that: The filter divergence criterion is introduced. According to whether the criterion is satisfied or not, the adaptive unscented Kalman filter and the strong tracking unscented Kalman filter are switched. The discretized vehicle system model is predicted and corrected based on the constructed strong tracking and adaptive unscented Kalman joint estimation algorithm. The filter divergence criterion is: the square of the new information sequence and its theoretical statistical characteristics are consistent: Where γ is the reserve coefficient, whose value is not less than 1. The left side of the inequality is the sum of squares of the new information sequence, and the right side is the theoretical value of the covariance of the new information sequence. When the criterion is met, it is considered that the filter converges, and an adaptive unscented Kalman filter is used to improve the filtering accuracy. When the criterion is not met, it is considered that the filter is abnormal. At this time, a strong tracking unscented Kalman filter is used to enhance the robustness and tracking of the state estimation.

Images