Feedback correction method and device for model predictive control of autonomous driving vehicles

Abstract

The present invention discloses a feedback correction method and device for model predictive control of an autonomous driving vehicle, and relates to the field of computer technology. A specific implementation of the method includes: linearizing the kinematic model of the vehicle to obtain an equivalent interference signal; determining the statistical eigenvalues ​​of the equivalent interference signal, and setting the initial value of the state covariance matrix of the extended Kalman filter; and determining the feedback correction result of the state value of the current control cycle of the vehicle according to the state value of the current control cycle of the vehicle, the control instructions of the previous control cycle, and the statistical eigenvalues ​​of the equivalent interference signal. This implementation enables the model predictive control algorithm to make relatively accurate predictions and controls on the future state and dynamic behavior of the vehicle, greatly improving the accuracy and robustness of the control system.

Description

Technical Field

[0001] The present invention relates to the field of computer technology, and in particular to a feedback correction method and device for model predictive control of an autonomous driving vehicle. Background Art

[0002] For the controller of an autonomous vehicle, in addition to being stable, accurate, and fast in following the desired trajectory, it also needs to be robust, that is, the ability to resist interference from uncertain factors. During the driving process of an autonomous vehicle, there are various uncertainties, such as external factors such as rough roads, running over stones, occasional inaccuracies in positioning data or high-precision maps, and internal factors such as model mismatch and modeling errors after linearization. These factors will cause inaccurate or jumpy vehicle states, which will be coupled to the control system and cause the vehicle to oscillate in the lateral and longitudinal directions or even continue to diverge.

[0003] The traditional feedback correction method used in the model predictive control of autonomous driving vehicles is to directly compare the actual state of the autonomous driving vehicle with the predicted state, and then use the difference as the compensation amount for predictive control, or directly use the noise signal as the equivalent of the white noise signal in the Kalman filter. However, when the interference factor is too large or the model mismatch is serious, the traditional feedback correction method cannot accurately compensate the compensation amount, which will eventually lead to excessive steady-state deviation or divergence of the control system, resulting in low robustness and control accuracy of the controller. Summary of the invention

[0004] In view of this, an embodiment of the present invention provides a feedback correction method and device for model predictive control of an autonomous driving vehicle, which enables the model predictive control algorithm to make relatively accurate predictions and controls on the future state and dynamic behavior of the vehicle, greatly improving the accuracy and robustness of the control system.

[0005] To achieve the above objective, according to one aspect of an embodiment of the present invention, a feedback correction method for model predictive control of an autonomous driving vehicle is provided.

[0006] A feedback correction method for model predictive control of an autonomous driving vehicle, comprising:

[0007] The kinematic model of the vehicle is linearized to obtain an equivalent interference signal;

[0008] Determine the statistical eigenvalue of the equivalent interference signal, and set the initial value of the state covariance matrix of the extended Kalman filter;

[0009] The feedback correction result of the state value of the current control cycle of the vehicle is determined according to the state value of the current control cycle of the vehicle, the control instruction of the previous control cycle, and the statistical characteristic value of the equivalent interference signal.

[0010] Optionally, linearizing the kinematic model of the vehicle to obtain an equivalent interference signal includes:

[0011] The kinematic model of the vehicle is linearized by performing a first-order Taylor expansion;

[0012] An equivalent interference signal is obtained according to the kinematic model after linearization processing, and the equivalent interference signal includes process noise and model mismatch equivalent interference signal.

[0013] Optionally, the kinematic model of the vehicle is:

[0014]

[0015]

[0016]

[0017]

[0018] Among them, x k ,y k 、v k ,θ k represents the state variable, a k ,δ k represents the control variable, represents the process noise of each state variable, T represents the control period, and L is the vehicle wheelbase;

[0019] The state transfer matrix φ obtained by performing a first-order Taylor expansion on the vehicle's kinematic model is k , control matrix Γ k and model mismatch equivalent interference signal matrix They are:

[0020]

[0021]

[0022]

[0023] Then, the equivalent interference signal △W k for:

[0024]

[0025] Optionally, determining the statistical characteristic value of the equivalent interference signal includes:

[0026] Determine the mean and variance of the equivalent interference signal, wherein the mean E(ΔW k )for:

[0027]

[0028] W k Assuming it is a noise signal that conforms to the Gaussian distribution, its mean and variance are:

[0029] E(W k )=[μ x μ y μ v μ θ ] T ;

[0030]

[0031] Among them, μ x , μ y , μ v , μ θ are the means of variables x, y, v, and θ respectively, are the process noise variances of variables x, y, v, and θ respectively;

[0032] Then, the equivalent interference signal △W k The variance of △Q k for:

[0033] △Q k =E[(△W k -E(△W k ))(△W k -E(△W k )) T ]+Q+E[2(△W k -E(△W k ))(W k -E(W k )) T ].

[0034] Optionally, if the equivalent interference signal ΔW k Transformed into white noise with zero mean but:

[0035]

[0036] in, The mean of is 0 and the variance is:

[0037] Optionally, the feedback correction result of the state value of the current control cycle of the vehicle is calculated by the following formula:

[0038] X k+1 / k =φ kX k +Γ k U k-1 +E(△W k );

[0039] Among them, X k is the state variable value after the state is updated [x k y k v k θ k ] T , U k-1 is the control instruction of the previous control cycle [a k-1 δ k-1 ] T .X k+1 / k is the predicted state variable, ie, the feedback correction result.

[0040] Optionally, it also includes:

[0041] The output model of the extended Kalman filter is linearized to obtain the observation noise;

[0042] Determining a statistical characteristic value of the observation noise;

[0043] The extended Kalman filter is updated according to the state value of the current control cycle of the vehicle, the statistical characteristic value of the equivalent interference signal, and the statistical characteristic value of the observation noise.

[0044] Optionally, the output model is:

[0045] Y k =HX k +V k ;

[0046] Among them, the H matrix is ​​a four-dimensional unit matrix: H = I4;

[0047] above are the observation noises of each vehicle state;

[0048] The statistical eigenvalues ​​of the observation noise include the variance matrix R of the observation noise, and:

[0049]

[0050] in, are the observation noise variances of variables x, y, v, and θ, respectively.

[0051] Optionally, updating of the extended Kalman filter includes:

[0052] The Kalman filter includes covariance prediction, Kalman filter gain calculation, state variable estimation value update, and covariance matrix update.

[0053] Optionally, the covariance prediction of the Kalman filter is calculated by the following formula:

[0054] P k+1 / k =φ k P k φ k T +△Q k ;

[0055] Among them, P k represents the prior estimate of the covariance, P k+1 / k is the posterior estimate of the covariance, φ k is the state transfer matrix;

[0056] The Kalman filter gain K is calculated by the following formula:

[0057] K k+1 =P k+1 / k (P k+1 / k +R) -1 ;

[0058] The state variable estimate update is calculated using the following formula:

[0059] X k+1 / k+1 =X k+1 / k +K k+1 (Y k -X k+1 / k ),

[0060] Among them, Y k is the state variable value observed by the vehicle in real time [x k y k v k θ k ];

[0061] The covariance matrix update is calculated using the following formula:

[0062] P k+1 / k+1 =P k+1 / k -K k+1 P k+1 / k ).

[0063] According to another aspect of an embodiment of the present invention, a feedback correction device for model predictive control of an autonomous driving vehicle is provided.

[0064] A feedback correction device for model predictive control of an autonomous driving vehicle, comprising:

[0065] A model processing module is used to linearize the kinematic model of the vehicle to obtain an equivalent interference signal;

[0066] A signal processing module, used to determine the statistical eigenvalues ​​of the equivalent interference signal and set the initial value of the state covariance matrix of the extended Kalman filter;

[0067] The state correction module is used to determine the feedback correction result of the state value of the current control cycle of the vehicle according to the state value of the current control cycle of the vehicle, the control instruction of the previous control cycle, and the statistical characteristic value of the equivalent interference signal.

[0068] Optionally, the model processing module is further used for:

[0069] The kinematic model of the vehicle is linearized by performing a first-order Taylor expansion;

[0070] An equivalent interference signal is obtained according to the kinematic model after linearization processing, and the equivalent interference signal includes process noise and model mismatch equivalent interference signal.

[0071] Optionally, the kinematic model of the vehicle is:

[0072]

[0073]

[0074]

[0075]

[0076] Among them, x k ,y k 、v k ,θ k represents the state variable, a k ,δ k represents the control variable, represents the process noise of each state variable, T represents the control period, and L is the vehicle wheelbase;

[0077] The state transfer matrix φ obtained by performing a first-order Taylor expansion on the vehicle's kinematic model is k , control matrix Γ k and model mismatch equivalent interference signal matrix They are:

[0078]

[0079]

[0080]

[0081] Then, the equivalent interference signal △W k for:

[0082]

[0083] Optionally, the signal processing module is further used for:

[0084] Determine the mean and variance of the equivalent interference signal, wherein the mean E(ΔW k )for:

[0085]

[0086] W k Assuming it is a noise signal that conforms to the Gaussian distribution, its mean and variance are:

[0087] E(W k )=[μ x μ y μ v μ θ ] T ;

[0088]

[0089] Among them, μ x , μ y , μ v , μ θ are the means of variables x, y, v, and θ respectively, are the process noise variances of variables x, y, v, and θ respectively;

[0090] Then, the equivalent interference signal △W k The variance of △Q k for:

[0091] △Q k =E[(△W k -E(△W k ))(△W k -E(△W k )) T ]+Q+E[2(△W k -E(△W k ))(W k -E(W k )) T ].

[0092] Optionally, if the equivalent interference signal ΔW k Transformed into white noise with zero mean but:

[0093]

[0094] in, The mean of is 0 and the variance is:

[0095] Optionally, the feedback correction result of the state value of the current control cycle of the vehicle is calculated by the following formula:

[0096] X k+1 / k =φ k X k +Γ k U k-1 +E(△W k );

[0097] Among them, X k is the state variable value after the state is updated [x k y k v k θ k ] T , U k-1 is the control instruction of the previous control cycle [a k-1 δ k-1 ] T .X k+1 / k is the predicted state variable, ie, the feedback correction result.

[0098] Optionally, the model processing module is further used to: perform linearization processing on the output model of the extended Kalman filter to obtain observation noise;

[0099] The signal processing module is also used to: determine the statistical characteristic value of the observation noise;

[0100] The device also includes a filter updating module, which is used to update the extended Kalman filter according to the state value of the current control cycle of the vehicle, the statistical characteristic value of the equivalent interference signal, and the statistical characteristic value of the observation noise.

[0101] Optionally, the output model is:

[0102] Y k =HX k +V k ;

[0103] Among them, the H matrix is ​​a four-dimensional unit matrix: H = I4;

[0104] above are the observation noises of each vehicle state;

[0105] The statistical eigenvalues ​​of the observation noise include the variance matrix R of the observation noise, and:

[0106]

[0107] in, are the observation noise variances of variables x, y, v, and θ, respectively.

[0108] Optionally, updating of the extended Kalman filter includes:

[0109] The Kalman filter includes covariance prediction, Kalman filter gain calculation, state variable estimation value update, and covariance matrix update.

[0110] Optionally, the covariance prediction of the Kalman filter is calculated by the following formula:

[0111] P k+1 / k =φ k P k φ k T +△Q k ;

[0112] Among them, P k represents the prior estimate of the covariance, P k+1 / k is the posterior estimate of the covariance, φ k is the state transfer matrix;

[0113] The Kalman filter gain K is calculated by the following formula:

[0114] K k+1 =P k+1 / k (P k+1 / k +R) -1 ;

[0115] The state variable estimate update is calculated using the following formula:

[0116] X k+1 / k+1 =X k+1 / k +K k+1 (Y k -X k+1 / k ),

[0117] Among them, Y k is the state variable value observed by the vehicle in real time [x k y k v k θ k ];

[0118] The covariance matrix update is calculated using the following formula:

[0119] P k+1 / k+1 =Pk+1 / k -K k+1 P k+1 / k ).

[0120] According to yet another aspect of an embodiment of the present invention, an electronic device for feedback correction of model predictive control of an autonomous driving vehicle is provided.

[0121] An electronic device for feedback correction of model predictive control of an autonomous driving vehicle, comprising: one or more processors; a storage device for storing one or more programs, wherein when the one or more programs are executed by the one or more processors, the one or more processors implement the feedback correction method for model predictive control of an autonomous driving vehicle provided in an embodiment of the present invention.

[0122] According to still another aspect of the embodiments of the present invention, a computer readable medium is provided.

[0123] A computer-readable medium having a computer program stored thereon, wherein the program, when executed by a processor, implements a feedback correction method for model predictive control of an autonomous driving vehicle provided by an embodiment of the present invention.

[0124] An embodiment of the above invention has the following advantages or beneficial effects: an equivalent interference signal is obtained by linearizing the kinematic model of the vehicle; the statistical eigenvalues ​​of the equivalent interference signal are determined, and the initial value of the state covariance matrix of the extended Kalman filter is set; a technical means for determining the feedback correction result of the state value of the current control cycle of the vehicle according to the state value of the current control cycle of the vehicle, the control instruction of the previous control cycle, and the statistical eigenvalues ​​of the equivalent interference signal can obtain the equivalent interference signal by linearizing the kinematic model of the vehicle, comprehensively consider the instability caused by various uncertain factors, unify various noise signals and errors into interference signals and estimate their statistical characteristics, and perform feedback correction in combination with the actual state value of the current control cycle of the vehicle, in an effort to enable the model predictive control algorithm to make relatively accurate predictions and controls on the future state and dynamic behavior of the vehicle, greatly improving the accuracy and robustness of the control system.

[0125] The further effects of the above-mentioned non-conventional optional manner will be described below in conjunction with specific implementation examples. BRIEF DESCRIPTION OF THE DRAWINGS

[0126] The accompanying drawings are used to better understand the present invention and do not constitute an improper limitation of the present invention.

[0127] Figure 1 This is a block diagram of the implementation principle of model predictive control for autonomous vehicles;

[0128] Figure 2is a schematic diagram of the main steps of a feedback correction method for model predictive control of an autonomous driving vehicle according to an embodiment of the present invention;

[0129] Figure 3 1 is a schematic diagram of an implementation flow of an extended Kalman filter according to an embodiment of the present invention;

[0130] Figure 4 is a schematic diagram of main modules of a feedback correction device for model predictive control of an autonomous driving vehicle according to an embodiment of the present invention;

[0131] Figure 5 is an exemplary system architecture diagram to which embodiments of the present invention may be applied;

[0132] Figure 6 It is a schematic diagram of the structure of a computer system of a terminal device or a server suitable for implementing an embodiment of the present invention. DETAILED DESCRIPTION

[0133] The following is a description of exemplary embodiments of the present invention in conjunction with the accompanying drawings, including various details of the embodiments of the present invention to facilitate understanding, which should be considered as merely exemplary. Therefore, it should be recognized by those of ordinary skill in the art that various changes and modifications may be made to the embodiments described herein without departing from the scope and spirit of the present invention. Similarly, for clarity and conciseness, the description of well-known functions and structures is omitted in the following description.

[0134] In order to solve the shortcomings of traditional feedback correction methods in the prior art, the present invention estimates and corrects the actual state of the vehicle by estimating the statistical characteristics of the disturbance signal and applying the extended Kalman filter (hereinafter referred to as EKF) technology. The specific variables include the current position, heading angle, and speed of the vehicle. The corrected state is then fed back to the model predictive controller, thereby improving the robustness and control accuracy of the controller.

[0135] Predictive control is a specific type of control method. It uses a prediction model and the system's historical data and future inputs to predict the system's future output. Feedback correction control is obtained by optimizing a certain performance indicator within a rolling finite time interval. The model predictive control of the present invention includes three links: model prediction, rolling optimization, and feedback correction. The implementation principle block diagram is shown below: Figure 1As shown. Among them, the control model predicted in the embodiment of the present invention can be a vehicle kinematic model or a dynamic model, or it can be a model generated by least squares method or neural network method, but it is conceivable that no matter how the model is generated, it is not an accurate mathematical model. For uncertain factors such as model mismatch, nonlinearity, time variation and disturbance in the system, the prediction based on the above model cannot be consistent with the actual situation, such as the existence of constant steady-state deviation, slow response time, etc. The rolling optimization process refers to determining the future control output in an optimal way through a certain performance indicator. The performance indicator involves the future behavior of the system. This link is a rolling optimization based on the prediction model. For various uncertain interference factors, the control output obtained by rolling optimization will deviate from the expected control effect.

[0136] Based on the above situation, the feedback correction link is a supplement and online correction of various uncertainties existing in the above two links, which is also the main design link of the present invention. The present invention comprehensively considers the instability caused by the above uncertainties, unifies various noise signals and errors into disturbance signals and estimates their statistical characteristics, uses EKF technology combined with the actual state of the vehicle for estimation, and strives to make the model predictive control algorithm make more accurate predictions and controls on the future state and dynamic behavior of the vehicle, thereby improving the robustness and control accuracy of the vehicle controller.

[0137] According to one aspect of the present invention, a feedback correction method for model predictive control of an autonomous driving vehicle is provided. Figure 2 Schematic diagram of the main steps of the feedback correction method for model predictive control of an autonomous driving vehicle according to an embodiment of the present invention. Figure 2 As shown, the feedback correction method for model predictive control of an autonomous driving vehicle according to an embodiment of the present invention mainly includes the following steps S201 to S203.

[0138] Step S201: linearizing the kinematic model of the vehicle to obtain an equivalent interference signal;

[0139] Step S202: determining the statistical eigenvalue of the equivalent interference signal, and setting the initial value of the state covariance matrix of the extended Kalman filter;

[0140] Step S203: determining a feedback correction result of the state value of the vehicle in the current control cycle according to the state value of the vehicle in the current control cycle, the control instruction of the previous control cycle, and the statistical characteristic value of the equivalent interference signal.

[0141] According to the above-mentioned steps S201 to S203, the kinematic model of the vehicle can be linearized to obtain an equivalent interference signal, and the instability caused by various uncertain factors is comprehensively considered. Various noise signals and errors are unified as equivalent to interference signals and their statistical characteristics are estimated. Feedback correction is performed in combination with the actual state value of the vehicle's current control cycle, in an effort to enable the model predictive control algorithm to make more accurate predictions and controls on the future state and dynamic behavior of the vehicle, thereby greatly improving the accuracy and robustness of the control system.

[0142] According to an embodiment of the present invention, when the kinematic model of the vehicle is linearized to obtain an equivalent interference signal, the following steps may be specifically performed:

[0143] The kinematic model of the vehicle is linearized by performing a first-order Taylor expansion;

[0144] An equivalent interference signal is obtained according to the kinematic model after linearization processing, and the equivalent interference signal includes process noise and model mismatch equivalent interference signal.

[0145] For nonlinear models, the general model is as follows:

[0146] X k+1 =f k (X k ,U k )+W k ;

[0147] Y k =h k (X k )+V k ;

[0148] Among them, X k represents the state variable, U k represents the control variable, W k represents process noise interference, Y k Indicates output, V k represents the observation noise, f k 、h k represents functions where at least one of the terms is nonlinear.

[0149] In order to estimate the state of a nonlinear system, the error caused by the linearization process and W k 、V k First, the nonlinear model of the variable to be linearized is expanded by the first-order Taylor at the working point of the variable to be linearized, as shown below:

[0150]

[0151]

[0152] Here, P and P0 are the variables to be linearized and the operating point, φ k is the state transfer matrix, Γ k is the control matrix, H k is the observation matrix. Then, we can get:

[0153]

[0154]

[0155] Above, △W k , △V k It is the equivalent interference signal after combining model mismatch and noise interference.

[0156] In the present invention, the vehicle model is a kinematic model:

[0157]

[0158]

[0159]

[0160]

[0161] Among them, x k ,y k 、v k ,θ k represents the state variable, a k ,δ k represents the control variable, represents the process noise of each state variable, T represents the control period, and L is the vehicle wheelbase.

[0162] Combined with the above-mentioned related formulas for linearizing the general nonlinear model, the state transfer matrix φ is obtained by performing a first-order Taylor expansion on the vehicle's kinematic model k , control matrix Γ k and model mismatch equivalent interference signal matrix They are:

[0163]

[0164]

[0165]

[0166] Here, is the model mismatch equivalent interference signal, then the total equivalent interference signal △Wk for:

[0167]

[0168] In the present invention, the output model:

[0169] Y k =HX k +V k ;

[0170] Among them, the H matrix is ​​a four-dimensional unit matrix: H = I4;

[0171]

[0172] above, are the observation noise of each vehicle state.

[0173] According to the above introduction, the state model and output model of the extended Kalman filter can be linearized to obtain the equivalent interference signal △W k and observation noise V k Afterwards, in the specific implementation process, in order to correct the acquired state variables of the vehicle in the current control cycle (including the vehicle's position coordinates, speed and heading angle), it is necessary to correct the equivalent interference signal △W k Further processing is performed, specifically, firstly, the statistical characteristic value of the equivalent interference signal needs to be determined.

[0174] According to one embodiment of the present invention, determining the statistical characteristic value of the equivalent interference signal includes:

[0175] Determine the mean and variance of the equivalent interference signal, wherein the mean E(ΔW k )for:

[0176]

[0177] In general, W k Assuming it is a noise signal that conforms to the Gaussian distribution, its mean and variance are:

[0178] E(W k )=[μ x μ y μ v μ θ ] T ;

[0179]

[0180] Among them, μ x , μ y , μ v , μθ are the means of variables x, y, v, and θ respectively, are the process noise variances of variables x, y, v, and θ respectively;

[0181] Then, the equivalent interference signal △W k The variance of △Q k for:

[0182] △Q k =E[(△W k -E(△W k ))(△W k -E(△W k )) T ]+Q+E[2(△W k -E(△W k ))(W k -E(W k )) T ];

[0183] Among them, the equivalent interference signal △W k The variance of △Q k is △W k and E(△W k ) is the variance of the combined disturbances of the two.

[0184] According to an embodiment of the present invention, in order to design an extended Kalman filter EKF, ΔW is k Transformed into white noise with zero mean If the equivalent interference signal △W k Transformed into white noise with zero mean but:

[0185]

[0186] Then the linearized system state equation is equivalent to:

[0187]

[0188] in, The mean of is 0 and the variance is:

[0189] At the same time, the observation noise variance matrix can also be set:

[0190]

[0191] in, are the observation noise variances of variables x, y, v, and θ respectively;

[0192] Set the initial value of the state covariance matrix P estimated by the EKF filter:

[0193]

[0194] Set the vehicle's starting position X0 = [x0 y0 v0 θ0] T In the present invention, the initial state is the vehicle state obtained by integrating the inertial measurement unit IMU, the laser radar and the wheel speed meter.

[0195] After that, the state value of the vehicle's current control cycle can be corrected. Specifically, the feedback correction result of the state value of the vehicle's current control cycle is calculated by the following formula:

[0196] X k+1 / k =φ k X k +Γ k U k-1 +E(△W k );

[0197] Among them, X k is the state variable value after the state is updated [x k y k v k θ k ] T , U k-1 is the control instruction of the previous control cycle [a k-1 δ k-1 ] T .X k+1 / k is the predicted state variable, ie, the feedback correction result.

[0198] According to the technical solution of the present invention, after feedback correction is performed on the state variables of the vehicle, the output model of the extended Kalman filter can also be linearized to obtain observation noise; the statistical characteristic value of the observation noise is determined; and the extended Kalman filter is updated according to the state value of the current control cycle of the vehicle, the statistical characteristic value of the equivalent interference signal, and the statistical characteristic value of the observation noise.

[0199] Specifically, as mentioned above, the output model in the embodiment of the present invention is:

[0200] Y k =HX k +V k ;

[0201] Among them, the H matrix is ​​a four-dimensional unit matrix: H = I4;

[0202] above are the observation noises of each vehicle state;

[0203] Furthermore, the statistical eigenvalues ​​of the observation noise are assumed to include the variance matrix R of the observation noise, and:

[0204]

[0205] in, are the observation noise variances of variables x, y, v, and θ, respectively.

[0206] Then, when updating the extended Kalman filter, the following aspects can be mainly included:

[0207] The Kalman filter includes covariance prediction, Kalman filter gain calculation, state variable estimation value update, and covariance matrix update.

[0208] Specifically, the covariance prediction of the Kalman filter is calculated by the following formula:

[0209] P k+1 / k =φ k P k φ k T +△Q k ;

[0210] Among them, P k represents the prior estimate of the covariance, P k+1 / k is the posterior estimate of the covariance, φ k is the state transfer matrix;

[0211] The Kalman filter gain K is calculated by the following formula:

[0212] K k+1 =P k+1 / k (P k+1 / k +R) -1 ;

[0213] The state variable estimate update is calculated using the following formula:

[0214] X k+1 / k+1 =X k+1 / k +K k+1 (Y k -X k+1 / k ),

[0215] Among them, Y k is the state variable value observed by the vehicle in real time [x k y k v k θ k ];

[0216] The covariance matrix update is calculated using the following formula:

[0217] P k+1 / k+1 =P k+1 / k -K k+1 P k+1 / k ).

[0218] The present invention applies the extended Kalman filter technology to the feedback correction link of the control system, which comprehensively considers the instability caused by various uncertain factors, equates them to disturbance signals and estimates their statistical characteristics, greatly improving the accuracy and robustness of the control system.

[0219] Figure 3 : is a schematic diagram of the implementation process of the extended Kalman filter of the embodiment of the present invention. In the embodiment of the present invention, the extended Kalman filter technology is applied to perform feedback correction of the model predictive control of the autonomous driving vehicle. It mainly includes several parts such as model linearization processing, estimation of statistical characteristics of interference signals, and extended Kalman filter. Figure 3 As shown, the implementation process of the extended Kalman filter in the embodiment of the present invention is mainly as follows:

[0220] 1. Obtain the current state of the vehicle, including the vehicle's position, speed, heading angle, etc., to build the state model and output model of the extended Kalman filter;

[0221] 2. Linearize the nonlinear vehicle kinematic model, and then obtain the linearized key matrix, such as the equivalent interference signal in the embodiment of the present invention;

[0222] 3. Estimate the statistical characteristics of the equivalent interference signal, such as mean and variance;

[0223] 4. Initialize the variance matrix R of the observation noise and the initial value of the state covariance matrix P of the filter estimate;

[0224] 5. Carry out vehicle state prediction to obtain a prediction value corresponding to the current state of the vehicle as a feedback correction result, and then output the feedback correction result to the controller for predictive control of the vehicle;

[0225] 6. Perform covariance prediction and Kalman filter gain calculation;

[0226] 7. Update vehicle status and covariance matrix.

[0227] Figure 4 Schematic diagram of the main modules of the feedback correction device for model predictive control of an autonomous driving vehicle according to an embodiment of the present invention. Figure 4 As shown, the feedback correction device 400 for model predictive control of an autonomous driving vehicle according to an embodiment of the present invention mainly includes a model processing module 401, a signal processing module 402 and a state correction module 403.

[0228] The model processing module 401 is used to perform linearization processing on the kinematic model of the vehicle to obtain an equivalent interference signal;

[0229] A signal processing module 402 is used to determine the statistical eigenvalues ​​of the equivalent interference signal and set the initial value of the state covariance matrix of the extended Kalman filter;

[0230] The state correction module 403 is used to determine the feedback correction result of the state value of the current control cycle of the vehicle according to the state value of the current control cycle of the vehicle, the control instruction of the previous control cycle, and the statistical characteristic value of the equivalent interference signal.

[0231] According to one embodiment of the present invention, the model processing module 401 may also be used to:

[0232] The kinematic model of the vehicle is linearized by performing a first-order Taylor expansion;

[0233] An equivalent interference signal is obtained according to the kinematic model after linearization processing, and the equivalent interference signal includes process noise and model mismatch equivalent interference signal.

[0234] According to one embodiment of the present invention, the kinematic model of the vehicle is:

[0235]

[0236]

[0237]

[0238]

[0239] Among them, x k ,y k 、v k ,θ k represents the state variable, a k ,δ k represents the control variable, represents the process noise of each state variable, T represents the control period, and L is the vehicle wheelbase;

[0240] The state transfer matrix φ obtained by performing a first-order Taylor expansion on the vehicle's kinematic model is k , control matrix Γ k and model mismatch equivalent interference signal matrix They are:

[0241]

[0242]

[0243]

[0244] Then, the equivalent interference signal △W k for:

[0245]

[0246] According to one embodiment of the present invention, the signal processing module is further used for:

[0247] Determine the mean and variance of the equivalent interference signal, wherein the mean E(ΔW k )for:

[0248]

[0249] W k Assuming it is a noise signal that conforms to the Gaussian distribution, its mean and variance are:

[0250] E(W k )=[μ x μ y μ v μ θ ] T ;

[0251]

[0252] Among them, μ x , μ y , μ v , μ θ are the means of variables x, y, v, and θ respectively, are the process noise variances of variables x, y, v, and θ respectively;

[0253] Then, the equivalent interference signal △W k The variance of △Q k for:

[0254] △Q k =E[(△W k -E(△W k ))(△W k -E(△W k )) T ]+Q+E[2(△W k -E(△W k ))(W k -E(W k )) T ].

[0255] According to an embodiment of the present invention, if the equivalent interference signal ΔW k Transformed into white noise with zero mean but:

[0256]

[0257] in, The mean of is 0 and the variance is:

[0258] According to one embodiment of the present invention, the feedback correction result of the state value of the current control cycle of the vehicle is calculated by the following formula:

[0259] X k+1 / k =φ k X k +Γ k U k-1 +E(△W k );

[0260] Among them, X k is the state variable value after the state is updated [x k y k v k θ k ] T , U k-1 is the control instruction of the previous control cycle [a k-1 δ k-1 ] T .X k+1 / k is the predicted state variable, ie, the feedback correction result.

[0261] According to another embodiment of the present invention, the model processing module 401 may also be used to: perform linearization processing on the output model of the extended Kalman filter to obtain observation noise;

[0262] The signal processing module 402 may also be used to: determine a statistical characteristic value of the observation noise;

[0263] The feedback correction device 400 for model predictive control of autonomous driving vehicles may also include a filter update module (not shown in the figure), which is used to update the extended Kalman filter according to the state value of the vehicle's current control cycle, the statistical characteristic value of the equivalent interference signal, and the statistical characteristic value of the observation noise.

[0264] According to an embodiment of the present invention, the output model is:

[0265] Y k =HX k +V k ;

[0266] Among them, the H matrix is ​​a four-dimensional unit matrix: H = I4;

[0267] above are the observation noises of each vehicle state;

[0268] The statistical eigenvalues ​​of the observation noise include the variance matrix R of the observation noise, and:

[0269]

[0270] in, are the observation noise variances of variables x, y, v, and θ, respectively.

[0271] In an embodiment of the present invention, the updating of the extended Kalman filter includes:

[0272] The Kalman filter includes covariance prediction, Kalman filter gain calculation, state variable estimation value update, and covariance matrix update.

[0273] In one of the optional embodiments of the present invention, the covariance prediction of the Kalman filter is calculated by the following formula:

[0274] P k+1 / k =φ k P k φ k T +△Q k ;

[0275] Among them, P k represents the prior estimate of the covariance, P k+1 / k is the posterior estimate of the covariance, φ k is the state transfer matrix;

[0276] The Kalman filter gain K is calculated by the following formula:

[0277] K k+1 =P k+1 / k (P k+1 / k +R) -1 ;

[0278] The state variable estimate update is calculated using the following formula:

[0279] X k+1 / k+1 =X k+1 / k +K k+1 (Y k -X k+1 / k ),

[0280] Among them, Y k is the state variable value observed by the vehicle in real time [x k yk v k θ k ];

[0281] The covariance matrix update is calculated using the following formula:

[0282] P k+1 / k+1 =P k+1 / k -K k+1 P k+1 / k ).

[0283] According to the technical solution of the embodiment of the present invention, an equivalent interference signal is obtained by linearizing the kinematic model of the vehicle; the statistical eigenvalues ​​of the equivalent interference signal are determined, and the initial value of the state covariance matrix of the extended Kalman filter is set; and the technical means for determining the feedback correction result of the state value of the current control cycle of the vehicle according to the state value of the current control cycle of the vehicle, the control instruction of the previous control cycle, and the statistical eigenvalues ​​of the equivalent interference signal can obtain the equivalent interference signal by linearizing the kinematic model of the vehicle, comprehensively consider the instability caused by various uncertain factors, unify various noise signals and errors into interference signals and estimate their statistical characteristics, and perform feedback correction in combination with the actual state value of the current control cycle of the vehicle, in an effort to enable the model predictive control algorithm to make relatively accurate predictions and controls on the future state and dynamic behavior of the vehicle, thereby greatly improving the accuracy and robustness of the control system.

[0284] Figure 5 An exemplary system architecture 500 is shown to which a feedback correction method for model predictive control of an autonomous driving vehicle or a feedback correction device for model predictive control of an autonomous driving vehicle according to an embodiment of the present invention can be applied.

[0285] like Figure 5 As shown, system architecture 500 may include terminal devices 501, 502, 503, a network 504 and a server 505. Network 504 is used to provide a medium for communication links between terminal devices 501, 502, 503 and server 505. Network 504 may include various connection types, such as wired, wireless communication links or optical fiber cables, etc.

[0286] Users can use terminal devices 501, 502, 503 to interact with server 505 through network 504 to receive or send messages, etc. Various applications can be installed on terminal devices 501, 502, 503, such as navigation applications, location information collection applications, positioning applications, etc. (only examples).

[0287] The terminal devices 501 , 502 , and 503 may be various electronic devices having a display screen and supporting web browsing, including but not limited to smart phones, tablet computers, laptop computers, desktop computers, and the like.

[0288] The server 505 may be a server that provides various services, such as a background management server that provides estimation and prediction of the current state parameters of the vehicle collected by the user using the terminal devices 501, 502, and 503 (for example only). The background management server may linearize the kinematic model of the vehicle to obtain an equivalent interference signal; determine the statistical eigenvalues ​​of the equivalent interference signal, and set the initial value of the state covariance matrix of the extended Kalman filter; determine the feedback correction result of the state value of the current control cycle of the vehicle according to the state value of the current control cycle of the vehicle, the control instructions of the previous control cycle, and the statistical eigenvalues ​​of the equivalent interference signal, and feed back the processing result (for example, the feedback correction result - for example only) to the terminal device.

[0289] It should be noted that the feedback correction method for model predictive control of an autonomous driving vehicle provided in an embodiment of the present invention is generally executed by the server 505 , and accordingly, the feedback correction device for model predictive control of an autonomous driving vehicle is generally arranged in the server 505 .

[0290] It should be understood that Figure 5 The number of terminal devices, networks and servers in the embodiment is only for illustration. Any number of terminal devices, networks and servers may be provided according to the implementation requirements.

[0291] Reference below Figure 6 , which shows a schematic diagram of the structure of a computer system 600 of a terminal device or a server suitable for implementing an embodiment of the present invention. Figure 6 The terminal device or server shown is only an example and should not bring any limitation to the functions and scope of use of the embodiments of the present invention.

[0292] like Figure 6 As shown, the computer system 600 includes a central processing unit (CPU) 601, which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM) 602 or a program loaded from a storage part 608 into a random access memory (RAM) 603. In the RAM 603, various programs and data required for the operation of the system 600 are also stored. The CPU 601, the ROM 602, and the RAM 603 are connected to each other via a bus 604. An input / output (I / O) interface 605 is also connected to the bus 604.

[0293] The following components are connected to the I / O interface 605: an input section 606 including a keyboard, a mouse, etc.; an output section 607 including a cathode ray tube (CRT), a liquid crystal display (LCD), etc., and a speaker, etc.; a storage section 608 including a hard disk, etc.; and a communication section 609 including a network interface card such as a LAN card, a modem, etc. The communication section 609 performs communication processing via a network such as the Internet. A drive 610 is also connected to the I / O interface 605 as needed. A removable medium 611, such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, etc., is installed on the drive 610 as needed, so that a computer program read therefrom is installed into the storage section 608 as needed.

[0294] In particular, according to the embodiments disclosed in the present invention, the process described above with reference to the flowchart can be implemented as a computer software program. For example, the embodiments disclosed in the present invention include a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes a program code for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from the network through the communication part 609, and / or installed from the removable medium 611. When the computer program is executed by the central processing unit (CPU) 601, the above-mentioned functions defined in the system of the present invention are executed.

[0295] It should be noted that the computer-readable medium shown in the present invention may be a computer-readable signal medium or a computer-readable storage medium or any combination of the above two. The computer-readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or device, or any combination of the above. More specific examples of computer-readable storage media may include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present invention, a computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in combination with an instruction execution system, device or device. In the present invention, a computer-readable signal medium may include a data signal propagated in a baseband or as part of a carrier wave, which carries a computer-readable program code. This propagated data signal may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. Computer-readable signal media may also be any computer-readable medium other than computer-readable storage media, which may send, propagate or transmit a program for use by or in conjunction with an instruction execution system, apparatus or device. The program code contained on the computer-readable medium may be transmitted using any appropriate medium, including but not limited to: wireless, wire, optical cable, RF, etc., or any suitable combination of the above.

[0296] The flow chart and block diagram in the accompanying drawings illustrate the possible architecture, function and operation of the system, method and computer program product according to various embodiments of the present invention. In this regard, each box in the flow chart or block diagram can represent a module, a program segment, or a part of a code, and the above-mentioned module, program segment, or a part of a code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram or flow chart, and the combination of the boxes in the block diagram or flow chart can be implemented with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0297] The units or modules involved in the embodiments of the present invention may be implemented in software or hardware. The units or modules described may also be arranged in a processor. For example, they may be described as: a processor includes a model processing module, a signal processing module, and a state correction module. The names of these units or modules do not, in some cases, constitute limitations on the units or modules themselves. For example, the model processing module may also be described as "a module for linearizing the kinematic model of the vehicle to obtain an equivalent interference signal".

[0298] As another aspect, the present invention also provides a computer-readable medium, which may be included in the device described in the above embodiment; or it may exist independently without being assembled into the device. The above computer-readable medium carries one or more programs, and when the above one or more programs are executed by a device, the device includes: linearizing the kinematic model of the vehicle to obtain an equivalent interference signal; determining the statistical characteristic value of the equivalent interference signal, and setting the initial value of the state covariance matrix of the extended Kalman filter; determining the feedback correction result of the state value of the current control cycle of the vehicle according to the state value of the current control cycle of the vehicle, the control instruction of the previous control cycle, and the statistical characteristic value of the equivalent interference signal.

[0299] According to the technical solution of the embodiment of the present invention, an equivalent interference signal is obtained by linearizing the kinematic model of the vehicle; the statistical eigenvalues ​​of the equivalent interference signal are determined, and the initial value of the state covariance matrix of the extended Kalman filter is set; and the technical means for determining the feedback correction result of the state value of the current control cycle of the vehicle according to the state value of the current control cycle of the vehicle, the control instruction of the previous control cycle, and the statistical eigenvalues ​​of the equivalent interference signal can obtain the equivalent interference signal by linearizing the kinematic model of the vehicle, comprehensively consider the instability caused by various uncertain factors, unify various noise signals and errors into interference signals and estimate their statistical characteristics, and perform feedback correction in combination with the actual state value of the current control cycle of the vehicle, in an effort to enable the model predictive control algorithm to make relatively accurate predictions and controls on the future state and dynamic behavior of the vehicle, thereby greatly improving the accuracy and robustness of the control system.

[0300] The above specific implementations do not constitute a limitation on the protection scope of the present invention. It should be understood by those skilled in the art that various modifications, combinations, sub-combinations and substitutions may occur depending on design requirements and other factors. Any modification, equivalent substitution and improvement made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A feedback correction method for model predictive control of an autonomous driving vehicle, characterized in that: include: The kinematic model of the vehicle is linearized to obtain an equivalent interference signal; Determine the statistical eigenvalue of the equivalent interference signal, and set the initial value of the state covariance matrix of the extended Kalman filter; Determine a feedback correction result of the state value of the current control cycle of the vehicle according to the state value of the current control cycle of the vehicle, the control instruction of the previous control cycle, and the statistical characteristic value of the equivalent interference signal; Determining the statistical characteristic value of the equivalent interference signal includes: determining the mean and variance of the equivalent interference signal, wherein the mean E(ΔW k )for: in, is the model mismatch equivalent interference signal matrix obtained by performing the first-order Taylor expansion on the vehicle's kinematic model, and k is the serial number of the control cycle; W k Assuming it is a noise signal that conforms to the Gaussian distribution, its mean and variance are: E(W k )=[μ x m y m v m θ ] T ; Among them, μ x , μ y , μ v , μ θ are the means of variables x, y, v, and θ respectively, are the process noise variances of variables x, y, v, and θ respectively; Then, the equivalent interference signal ΔW k The variance of ΔQ k for: ΔQ k =E[(ΔW k -E(ΔW k ))(ΔW k -E(ΔW k )) T ]+Q+E[2(ΔW k -E(ΔW k ))(W k -E(W k )) T ]; The feedback correction result of the state value of the vehicle in the current control cycle is calculated by the following formula: X k+1 / k =φ k X k +Γ k U k-1 +E(ΔW k ); Among them, φ k is the state transfer matrix obtained by first-order Taylor expansion of the vehicle's kinematic model, Γ k is the control matrix obtained by first-order Taylor expansion of the vehicle's kinematic model, X k is the state variable value after the state is updated [x k y k v k θ k ] T , U k-1 is the control instruction of the previous control cycle [a k-1 δ k-1 ] T , X k+1 / k is the predicted state variable, ie, the feedback correction result.

2. The method according to claim 1, characterized in that: The equivalent interference signals obtained by linearizing the kinematic model of the vehicle include: The kinematic model of the vehicle is linearized by performing a first-order Taylor expansion; An equivalent interference signal is obtained according to the kinematic model after linearization processing, and the equivalent interference signal includes process noise and model mismatch equivalent interference signal.

3. The method according to claim 2, characterized in that The kinematic model of the vehicle is: Among them, x k ,y k 、v k ,θ k represents the state variable, a k , δ k represents the control variable, represents the process noise of each state variable, T represents the control period, and L is the vehicle wheelbase; The state transfer matrix φ obtained by performing a first-order Taylor expansion on the vehicle's kinematic model is k , control matrix Γ k and model mismatch equivalent interference signal matrix They are: Then, the equivalent interference signal ΔW k for:

4. The method according to claim 1, characterized in that: If the equivalent interference signal ΔW k Transformed into white noise with zero mean but: in, The mean of is 0 and the variance is:

5. The method according to claim 1, characterized in that Also includes: The output model of the extended Kalman filter is linearized to obtain the observation noise; Determining a statistical characteristic value of the observation noise; The extended Kalman filter is updated according to the state value of the current control cycle of the vehicle, the statistical characteristic value of the equivalent interference signal, and the statistical characteristic value of the observation noise.

6. The method according to claim 5, characterized in that The output model is: Y k =HX k +V k ; Among them, the H matrix is ​​a four-dimensional unit matrix: above are the observation noises of each vehicle state; The statistical eigenvalues ​​of the observation noise include the variance matrix R of the observation noise, and: in, are the observation noise variances of variables x, y, v, and θ, respectively.

7. The method according to claim 5, characterized in that The updating of the extended Kalman filter includes: The Kalman filter includes covariance prediction, Kalman filter gain calculation, state variable estimation value update, and covariance matrix update.

8. The method according to claim 7, characterized in that The covariance prediction of the Kalman filter is calculated by the following formula: P k+1 / k =φ k P k φ k T +ΔQ k ; Among them, P k represents the prior estimate of the covariance, P k+1 / k is the posterior estimate of the covariance, φ k is the state transfer matrix; The Kalman filter gain K is calculated by the following formula: K k+1 =P k+1 / k (P k+1 / k +R) -1 ; The state variable estimate update is calculated using the following formula: X k+1 / k+1 =X k+1 / k +K k+1 (Y k -X k+1 / k ), Among them, Y k is the state variable value observed by the vehicle in real time [x k y k v k θ k ]; The covariance matrix update is calculated using the following formula: P k+1 / k+1 =P k+1 / k -K k+1 P k+1 / k )。 9. A feedback correction device for model predictive control of an autonomous driving vehicle, characterized in that: include: A model processing module is used to linearize the kinematic model of the vehicle to obtain an equivalent interference signal; A signal processing module, used to determine the statistical eigenvalues ​​of the equivalent interference signal and set the initial value of the state covariance matrix of the extended Kalman filter; A state correction module, used to determine a feedback correction result of the state value of the vehicle in the current control cycle according to the state value of the vehicle in the current control cycle, the control instruction of the previous control cycle, and the statistical characteristic value of the equivalent interference signal; The signal processing module is further used to determine the mean and variance of the equivalent interference signal, wherein the mean E(ΔW k )for: in, is the model mismatch equivalent interference signal matrix obtained by performing the first-order Taylor expansion on the vehicle's kinematic model, and k is the serial number of the control cycle; W k Assuming it is a noise signal that conforms to the Gaussian distribution, its mean and variance are: E(W k )=[μ x m y m v m θ ] T ; Among them, μ x , μ y , μ v , μ θ are the means of variables x, y, v, and θ respectively, are the process noise variances of variables x, y, v, and θ respectively; Then, the equivalent interference signal ΔW k The variance of ΔQ k for: ΔQ k =E[(ΔW k -E(ΔW k ))(ΔW k -E(ΔW k )) T ]+Q+E[2(ΔW k -E(ΔW k ))(W k -E(W k )) T ]; The feedback correction result of the state value of the vehicle in the current control cycle is calculated by the following formula: X k+1 / k =φ k X k +Γ k U k-1 +E(ΔW k ); Among them, φ k is the state transfer matrix obtained by first-order Taylor expansion of the vehicle's kinematic model, Γ k is the control matrix obtained by first-order Taylor expansion of the vehicle's kinematic model, X k is the state variable value after the state is updated [x k y k v k θ k ] T , U k-1 is the control instruction of the previous control cycle [a k-1 δ k-1 ] T , X k+1 / k is the predicted state variable, ie, the feedback correction result.

10. An electronic device for feedback correction of model predictive control of an autonomous driving vehicle, characterized in that: include: one or more processors; a storage device for storing one or more programs, When the one or more programs are executed by the one or more processors, the one or more processors implement the method according to any one of claims 1 to 8.

11. A computer readable medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the method according to any one of claims 1 to 8 is implemented.

Images