Model Predictive Trajectory Tracking Control Method for Intelligent Vehicles Based on Feedback Control Compensation

Abstract

The present invention belongs to the field of intelligent vehicle trajectory tracking control, and specifically relates to an intelligent vehicle model prediction trajectory tracking control method based on feedback control compensation, comprising: step 1, establishing a 2-degree-of-freedom vehicle kinematic model to simulate the motion state of the intelligent vehicle; step 2, constructing a model prediction feedforward control system MPC, including a linear vehicle kinematic model of the vehicle's future T-time driving state, an objective function in the optimization process, and constraints that meet traffic regulations and vehicle structure; step 3, after the MPC feedforward control, a PID feedback control is connected in series to compensate the feedforward control output instruction according to the historical driving deviation, so that the vehicle's actual driving trajectory is closer to the preview trajectory; step 4, outputting the calculation result, and checking again whether the traffic regulations and vehicle structure constraints are met; step 5, adjusting the throttle or brake opening according to the acceleration error through PID. The method can improve the robustness of intelligent vehicle trajectory tracking control.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent vehicle trajectory tracking control, and in particular relates to an intelligent vehicle model prediction trajectory tracking control method based on feedback control compensation. Background Art

[0002] Controlling the vehicle to complete accurate trajectory tracking is the key technology for the vehicle to achieve autonomous driving. Improving the tracking accuracy of intelligent driving vehicles and controlling the vehicle to travel according to the planned trajectory can effectively alleviate traffic congestion and improve traffic efficiency. On the premise that the vehicle control accuracy meets the road traffic safety requirements, improving the robustness of the vehicle controller enables intelligent driving vehicles to meet the control requirements in different scenarios and enhance the key technology of the market competitiveness of intelligent vehicles.

[0003] Model predictive control (MPC) can receive control instructions through the built-in model to predict the driving state of the vehicle in the future, and iteratively optimize according to the objective function set by the scenario requirements to obtain the optimal control sequence, which effectively ensures the driving safety of autonomous vehicles and is increasingly used in the field of autonomous driving trajectory tracking control. For example, patent: CN202110888969.7 discloses a variable prediction time domain MPC path tracking method based on fuzzy control, which can adapt to the curvature change, speed change and change rate of the reference path, and improve the algorithm adaptability and stability of MPC path tracking for different reference paths. However, this study does not consider longitudinal control and is not suitable for scenarios such as expressway merging that require strict longitudinal control. Yang Zekun et al. (Distributed Drive Intelligent Vehicle Trajectory Tracking Control Based on Adaptive Variable Parameter MPC [J / OL]. Journal of Mechanical Engineering, 1-15 [2024-05-22]) estimated the tire lateral force based on the square root volume Kalman filter, corrected the tire cornering stiffness, and implemented MPC trajectory tracking control based on TS fuzzy variable weights, and the robustness of the controller was improved. However, the goal of their longitudinal control is only speed tracking, ignoring the problem that the position deviation will gradually increase with the accumulation of speed deviation over time. Summary of the invention

[0004] In order to eliminate the longitudinal position deviation caused by the accumulation of speed deviation and improve the accuracy and robustness of trajectory tracking control of intelligent driving vehicles, the present invention proposes a model predictive control trajectory tracking control method based on feedback control compensation, which can combine the MPC feedforward controller and the PID feedback controller, and minimize the position deviation, speed deviation and heading angle deviation. The intelligent vehicle is controlled laterally and longitudinally, so that it can complete trajectory tracking control under different working conditions in the merging area of ​​urban expressways, thereby improving the robustness of vehicle control.

[0005] To achieve the above objectives, the present invention adopts the following technical solutions:

[0006] The intelligent vehicle model prediction trajectory tracking control method based on feedback control compensation includes the following steps:

[0007] Step 1: Establish a 2-DOF vehicle kinematic model to simulate the motion state of the intelligent vehicle.

[0008] The vehicle kinematic model is:

[0009]

[0010] Where, φ is the vehicle heading angle; ν is the vehicle longitudinal speed; is the component velocity of the vehicle in the direction of the x-axis; is the component velocity of the vehicle in the y-axis direction of the coordinate axis; is the longitudinal acceleration of the vehicle, denoted by a; is the yaw rate of the vehicle; δ is the front wheel turning angle of the vehicle; L is the wheelbase of the vehicle.

[0011] Step 2: Construct a model predictive feedforward control system (MPC), including a linear vehicle kinematic model that predicts the vehicle's future driving state T time, the objective function in the optimization process, and the constraints that meet traffic regulations and vehicle structure.

[0012] The differential form of the vehicle kinematics model is discretized and linearized to obtain the linear vehicle kinematics model: t+1 =Az t +Bu+C

[0013] The matrices A, B, and C are:

[0014]

[0015] Set the objective function to:

[0016]

[0017] in, is the expected driving state of the vehicle at time t in the planned path, z t is the actual driving state of the vehicle at time t; Q, R and R d is the weight matrix, Q adjusts the influence of position deviation, speed deviation and heading angle deviation on the objective function, R adjusts the influence of control output, and R d Adjust the influence of the change in control output, u t represents the front wheel steering angle and acceleration command acting at time t, u t+1 Represents the front wheel steering angle and acceleration command acting at time t+1.

[0018] Read the vehicle's driving status through the sensor t=[t,x t ,y t ,v t ,φ t ] T , where z t represents the vehicle driving state, t represents the time index corresponding to the vehicle driving state, x t ,y t represents the vehicle position coordinates returned from the sensor, v t Indicates the current speed of the vehicle, φ t Indicates the angle of the vehicle's heading x-axis. Read the planned driving state of the vehicle at time t in the planned trajectory

[0019] The vehicle driving state z t Input linear error model z t+1 =Az t +Bu+C, calculate the optimal control output instruction sequence u=[u t ,u t+1 ,…,u T-1 ] and vehicle driving state sequence Indicates the preview state at time t+1, corresponding to the preview state at time t

[0020]

[0021] The optimal control sequence solution process needs to meet the following traffic regulations and vehicle configuration constraints:

[0022]

[0023] In the formula, Δu max is the maximum value of the control instruction increment; v min and v max are the minimum and maximum values ​​of the speed respectively; u min and u max They are the minimum and maximum values ​​of the control output respectively.

[0024] Step 3: Set up the PID feedback control system.

[0025] The feedforward control calculates the optimal control output sequence and preview trajectory. However, due to the difference between the actual working conditions and the theoretical working conditions, the vehicle cannot completely follow the planned trajectory. Therefore, after the MPC feedforward control, the PID feedback control is connected in series to compensate the feedforward control output command according to the historical driving deviation, so that the actual driving trajectory of the vehicle is closer to the preview trajectory.

[0026] The calculation method for compensating the rotation angle output and acceleration output through the PID control algorithm is as follows:

[0027]

[0028] Where T is the control interval, K P , K I , K D is the coefficient, Δδ is the compensation value of the feedback control to the front wheel steering angle of the feedforward control, Δa is the compensation value of the feedback control to the acceleration of the feedforward control, and is the preview state of the vehicle at time t and the actual state z t Compared with the actual state z at time t-1 t-1 The position angle deviation and distance deviation are calculated as follows:

[0029]

[0030] in, Indicates z t-1 to The position changes, Indicates z t-1 To z t The position changes, degreeA and degreeB represent and The angle of .

[0031] Step 4: After feedforward control and feedback control, the control output calculation result is u cal =[δ+Δδ,a+Δa], and check again whether the constraints of traffic regulations and vehicle structure are met.

[0032] When encountering unsatisfied constraints, if v t >v max or v t <v m i n , then a t =a t-1 , where a t represents the calculated acceleration at time t, a t-1 Indicates the vehicle acceleration output at time t-1; if δ t >δ max or δ t <δ min , then δ t =δ max or δ t =δ min , where δ t represents the calculated front wheel turning angle at time t; if |Δδ t |>Δδ max , then δ t =δt-1 ,in represents the actual front wheel turning angle of the vehicle at time t-1, δ t-1 Indicates the actual front wheel turning angle of the vehicle at time t-1; if a t >a max or a t <a min , then a t =a max or a t =a min ; if |Δa t >Δa max , a t =a t-1 ,in represents the actual acceleration of the vehicle at time t-1. After adjustment, the control variable at time t is recorded as

[0033] Step 5: Use PID to adjust the throttle or brake opening according to the acceleration error.

[0034]

[0035] in, For, among them is the real acceleration at time t, and Δdegree is the increment of the accelerator or brake opening.

[0036] The command that the intelligent vehicle finally receives and executes at time t is Where degree is the throttle or brake opening in the previous time step.

[0037] Beneficial effects of the present invention:

[0038] This control method uses the timestamp in the planned trajectory as the unique index, and performs tracking control with the goal of minimizing the position deviation, velocity deviation and heading angle deviation at the control moment, effectively improving the longitudinal control accuracy without reducing the lateral control accuracy. This method can improve the robustness of intelligent vehicle trajectory tracking control.

[0039] Since there is a deviation between the actual driving state and the theoretical state due to the incomplete consideration of variables and inaccurate parameters in MPC feedforward control, this method uses PID feedback control to compensate the feedforward control output, which enhances the robustness and reduces the model uncertainty compared with the traditional model predictive control method. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 The trajectory tracking control process of the KMPC-PID controlled intelligent vehicle;

[0041] Figure 2 A local planning trajectory for an embodiment;

[0042] Figure 3 The planning speed of the embodiment varies with the x-coordinate;

[0043] Figure 4 This is the schematic diagram of the 2-DOF vehicle kinematic model;

[0044] Figure 5 It is the maximum absolute lateral deviation thermal diagram of KMPC-PID, MPC-t, and PID control algorithms under various working condition combinations through the Carla platform;

[0045] Figure 6 The average absolute lateral deviation thermodynamic diagram of KMPC-PID, MPC-t, and PID control algorithms under various operating condition combinations through the Carla platform;

[0046] Figure 7 It is the maximum absolute longitudinal deviation heat map of KMPC-PID, MPC-t and PID control algorithms under various working condition combinations through the Carla platform;

[0047] Figure 8 The average absolute longitudinal deviation thermodynamic diagram of KMPC-PID, MPC-t, and PID control algorithms under various operating condition combinations through the Carla platform;

[0048] Fig. 9 It is the maximum absolute lateral deviation thermal diagram of KMPC-PID, MPC-t, and PID control algorithms under various working condition combinations through the HIL platform;

[0049] Fig.10 The average absolute lateral deviation thermodynamic diagram of KMPC-PID, MPC-t, and PID control algorithms under various working condition combinations through the HIL platform;

[0050] Fig.11 It is the maximum absolute longitudinal deviation heat map of KMPC-PID, MPC-t and PID control algorithms under various working condition combinations through the HIL platform;

[0051] Fig.12 The average absolute longitudinal deviation heat map of KMPC-PID, MPC-t, and PID control algorithms under various operating condition combinations through the HIL platform; DETAILED DESCRIPTION

[0052] The specific implementation methods of the present invention are further described below in conjunction with the accompanying drawings and technical solutions.

[0053] The intelligent vehicle model prediction trajectory tracking control method based on control compensation (KMPC-PID, K represents the method based on the vehicle kinematic model) of the present invention performs the intelligent vehicle trajectory tracking control process as follows Figure 1 shown.

[0054] The planned local trajectory of this embodiment is as follows Figure 2 As shown, the velocity changes with the X coordinate as Figure 3 shown.

[0055] The specific steps are as follows:

[0056] Step 1: Figure 4 As shown, a 2-DOF vehicle kinematic model is established to obtain the relationship between wheelbase L, driving speed v, heading angle φ, front wheel turning angle δ and acceleration a, as follows:

[0057]

[0058] Step 2: Construct a model predictive feedforward control system based on the vehicle kinematic model, linear error model and objective function.

[0059] Step 21: Linearize the vehicle kinematic model to reduce computational complexity:

[0060] z t+1 =Az t +Bu+C

[0061]

[0062] Step 22: Establish the objective function to minimize the position deviation, speed deviation, heading angle deviation, control output and control output change rate. The objective function is as follows:

[0063]

[0064] The Q matrix controls the influence weights of the deviations of the state variables. The values ​​in this embodiment are as follows:

[0065] Q = [1 1 1 0.5]

[0066] The R matrix controls the output influence weights. The values ​​in this embodiment are as follows:

[0067] R = [0.01 0.01]

[0068] R d The matrix controls the influence weight of the output change size. The values ​​in this embodiment are as follows:

[0069] R d =[0.01 1]

[0070] Step 23: Set the constraints as follows:

[0071]

[0072] Among them, δ represents the front wheel turning angle, and Δδ represents the front wheel turning angular velocity.

[0073] Step 24: Record the feedforward model predictive control calculation result as u1 = [δ, a]

[0074] Step 3: Establish a PID feedback control model. The calculation method is as follows:

[0075]

[0076] Among them, Δδ and Δa represent the angular deviation compensation and the driving distance deviation compensation, represents the distance deviation at time t, Represents the rotation angle deviation at time t.

[0077] Step 31: Read the current driving status of the vehicle t and the previous state z t-1 , and calculate the position deviation between the current state and the actual state of the vehicle in the preview trajectory output by the feedforward model predictive control at the previous moment, and calculate the driving distance deviation based on the relative positions of the three points and angular deviation

[0078] Step 32: Calculate the steering angle control compensation amount Δδ and the throttle or brake opening control compensation amount Δa according to the feedback control model.

[0079] Step 33: Combine the feedforward control output and the feedback control compensation output according to the following formula:

[0080] u=[δ+Δδ,a+Δa]

[0081] Step 34: Check whether the control command calculation result meets the traffic regulations and vehicle structure constraints. If it meets the requirements, If it does not meet the requirement, the adjusted control output is recorded as

[0082] Step 35: Use PID to adjust the acceleration It is converted into the throttle or brake opening degree + Δdegree, and the converted degree + Δdegree is the longitudinal operation instruction finally executed by the intelligent vehicle.

[0083] Step 4: Conduct experiments and analyze the results.

[0084] Table 1 Experimental conditions division

[0085]

[0086] Step 41, establish the experimental working condition, set the initial speed and the final speed to 46.8 km / h, and divide the experiment into 2×3=6 working condition combinations according to the intermediate speed and the curvature at the time of merging according to Table 1.

[0087] Step 42: Carla software and the HIL hardware-in-the-loop simulation platform are used to simulate and control the intelligent vehicle equipped with the KMPC-PID controller. The maximum absolute lateral deviation (lateral.max), the maximum absolute longitudinal deviation (longitudinal.max), the average absolute lateral deviation (lateral.ave), and the average absolute longitudinal deviation (longitudinal.ave) are shown in Tables 2 and 3.

[0088] Table 2 Control effects of KMPC-PID control method in different scenarios on Carla platform

[0089]

[0090] Table 3 Control effects of different scenarios of KMPC-PID control method on HIL platform

[0091]

[0092] It can be seen from Tables 2 and 3 that the maximum lateral deviation of the KMPC-PID control method under the Carla and HIL simulation platforms and 6 experimental conditions is 0.046m, the maximum longitudinal deviation is 2.399m, the maximum average lateral deviation is 0.017m, and the maximum average longitudinal deviation is 1.383m, which can meet the control needs of intelligent driving vehicles with a speed of 36 to 57.6km / h and meet the requirements for road safety.

[0093] Step 43, the traditional controller (MPC-lat) performs tracking control with the minimum speed deviation as the goal. In order to evaluate the effect of the KMPC-PID controller in tracking control with the minimum longitudinal deviation as the goal, the Carla simulation platform is used to simulate the control of the intelligent vehicle equipped with the MPC-lat controller and the KMPC-PID controller. The experimental results are shown in Table 4.

[0094] Table 4 Control effects of MPC-lat controller and KMPC-PID controller on intelligent vehicles under Carla simulation platform

[0095]

[0096] It can be seen from Table 4 that the KMPC-PID control algorithm is superior to the MPC-lat control algorithm in terms of maximum longitudinal deviation and average longitudinal deviation, indicating that relying solely on speed tracking cannot eliminate the longitudinal position deviation; except for the "period speed-incoming curvature: 57.6-0.02" scenario, the maximum lateral deviation and average lateral deviation of the KMPC-PID control algorithm are smaller than those of the MPC-lat control algorithm, indicating that the KMPC-PID control algorithm ensures the lateral control accuracy while effectively improving the longitudinal control capability.

[0097] Table 5 Simulation results of KMPC-PID, MPC-t, and PID control algorithms in various scenarios through the Carla platform

[0098]

[0099]

[0100] The control effect thermal diagram of KMPC-PID, MPC-t and PID control algorithms under various working conditions through Carla platform is as follows Figure 5-Figure 8 shown.

[0101] Combining Table 5 and Figure 5-Figure 8 It can be seen that although the PID control algorithm has a relatively good longitudinal control effect, its lateral control effect is poorer and its robustness is weaker than that of the KMPC-PID and MPC-t control algorithms. Compared with the MPC-t control algorithm, the lateral control accuracy of KMPC-PID can meet the control requirements under different working conditions, and its longitudinal control accuracy is higher and its robustness is stronger.

[0102] Table 6 Simulation experimental results of KMPC-PID, MPC-t, and PID control algorithms in various scenarios through the HIL platform

[0103]

[0104]

[0105] The control effect thermal diagram of KMPC-PID, MPC-t and PID control algorithms under various working conditions through the HIL platform is as follows Figure 9-12 shown.

[0106] Combining Table 6 and Figure 9-12 It can be seen that in terms of lateral control and longitudinal control, the control effect of the control algorithm based on model predictive control (MPC) is significantly better than that of the PID control algorithm; except for the "speed of 57.6km / h, 0.005m -1In addition to the "merging into curvature" scenario, in terms of lateral control and longitudinal control, the KMPC-PID control algorithm that provides control compensation through feedback control shows better control effects than the MPC-t control algorithm.

[0107] By comparing PID, MPC-lat, MPC-t and KMPC-PID in different scenarios, it is found that the KMPC-PID control algorithm after the MPC-t series feedback compensation control system has the best control effect in most scenarios and its robustness is enhanced.

Claims

1. An intelligent vehicle model prediction trajectory tracking control method based on feedback control compensation, characterized in that: The following steps are involved: Step 1: Establish a 2-DOF vehicle kinematics model to simulate the motion state of the intelligent vehicle; The vehicle kinematic model is: Where, φ is the vehicle heading angle; ν is the vehicle longitudinal speed; is the component velocity of the vehicle in the direction of the x-axis; is the component velocity of the vehicle in the y-axis direction of the coordinate axis; is the longitudinal acceleration of the vehicle, denoted by a; is the yaw rate of the vehicle; δ is the front wheel turning angle of the vehicle; L is the wheelbase of the vehicle; Step 2: construct a model predictive feedforward control system MPC, including a linear vehicle kinematic model that predicts the vehicle's future driving state at time T, an objective function in the optimization process, and constraints that meet traffic regulations and vehicle structure; The differential form of the vehicle kinematics model is discretized and linearized to obtain the linear vehicle kinematics model: t+1 =Az t +Bu+C The matrices A, B, and C are: Set the objective function to: in, is the expected driving state of the vehicle at time t in the planned path, z t is the actual driving state of the vehicle at time t; Q, R and R d is the weight matrix, Q adjusts the influence of position deviation, speed deviation and heading angle deviation on the objective function, R adjusts the influence of control output, and R d Adjust the influence of the change in control output, u t represents the front wheel steering angle and acceleration command acting at time t, u t+1 Represents the front wheel steering angle and acceleration command acting at time t+1; Read the vehicle's driving status through the sensor t =[t,x t ,y t ,v t ,φ t ] T , where z t represents the vehicle driving state, t represents the time index corresponding to the vehicle driving state, x t ,y t represents the vehicle position coordinates returned from the sensor, v t Indicates the current speed of the vehicle, φ t Indicates the angle of the vehicle's heading x-axis; reads the planned driving state of the vehicle at time t in the planned trajectory The vehicle driving state z t Input linear error model z t+1 =Az t +Bu+C, calculate the optimal control output instruction sequence u=[u t ,u t+1 ,…,u T-1 ] and vehicle driving state sequence Indicates the preview state at time t+1, corresponding to the preview state at time t The optimal control sequence solution process needs to meet the following traffic regulations and vehicle configuration constraints: In the formula, Δu max is the maximum value of the control instruction increment; v min and v max are the minimum and maximum values ​​of the speed respectively; u min and u max They are the minimum and maximum values ​​of the control output respectively; Step 3: Set up a PID feedback control system; The PID feedback control is connected in series after the MPC feedforward control to compensate the feedforward control output command according to the historical driving deviation, so that the actual driving trajectory of the vehicle is closer to the preview trajectory; The calculation method for compensating the rotation angle output and acceleration output through the PID control algorithm is as follows: Where T is the control interval, K P , K I , K D is the coefficient, Δδ is the compensation value of the feedback control to the front wheel steering angle of the feedforward control, Δa is the compensation value of the feedback control to the acceleration of the feedforward control, and is the preview state of the vehicle at time t and the actual state z t Compared with the actual state z at time t-1 t-1 The position angle deviation and distance deviation are calculated as follows: in, Indicates z t-1 to The position changes, Indicates z t-1 To z t The position changes, degreeA and degreeB represent and The angle of Step 4: After feedforward control and feedback control, the control output calculation result is u cal =[δ+Δδ,a+Δa], check again whether the constraints of traffic regulations and vehicle structure are met; When encountering unsatisfied constraints, if v t >v max or v t <v min , then a t =a t-1 , where a t represents the calculated acceleration at time t, a t-1 Indicates the vehicle acceleration output at time t-1; if δ t >δ max or δ t <δ min , then δ t =δ xam or δ t =δ min , where δ t represents the calculated front wheel turning angle at time t; if |Δδ t |>Δδ max , then δ t =δ t-1 ,in represents the actual front wheel turning angle of the vehicle at time t-1, δ t-1 Indicates the actual front wheel turning angle of the vehicle at time t-1; if a t >a max or a t <a min , then a t =a max or a t =a min ; if |Δa t >Δa max , a t =a t-1 ,in represents the actual acceleration of the vehicle at time t-1; after adjustment, the control variable at time t is recorded as Step 5: Use PID to adjust the throttle or brake opening according to the acceleration error; in, For, among them is the real acceleration at time t, Δdegree is the increment of throttle or brake opening; The command that the intelligent vehicle finally receives and executes at time t is Where degree is the throttle or brake opening in the previous time step.

Images