Model predictive control trajectory tracking control system and method based on reinforcement learning

Abstract

The invention relates to a model predictive control trajectory tracking control system based on reinforcement learning, comprising a linear time-varying model predictive controller, a planning module, a state estimation module and a reinforcement learning module, wherein the linear time-varying model predictive controller comprises a prediction model, an optimization solution module and an objective function + constraint condition module; the prediction trajectory of the prediction model is combined with the reference trajectory of the planning module and input to the optimization solution module, the objective function and the constraint condition are solved, the optimal control quantity is calculated and sent to a controlled vehicle for execution; after the vehicle executes a command, the state quantity is obtained and fed back to the state estimation module, the planning module and the reinforcement learning module; the state estimation module corrects the model error according to the feedback state quantity; the planning module performs local path planning; the reinforcement learning module adjusts the vehicle MPC control error weight parameter according to the difference between the reference trajectory and the actual vehicle position, adapts to the driving road and trajectory, and improves the control accuracy.

Description

Technical Field

[0001] The present invention belongs to the technical field of vehicle automatic driving control, and specifically relates to a model predictive control trajectory tracking control system and method based on reinforcement learning. Background Art

[0002] The control area of ​​autonomous driving can be divided into lateral control and longitudinal control according to the driving direction. Longitudinal control mainly controls the vehicle's throttle and brakes, and the technology is relatively mature; while lateral control is to give a control amount to control the vehicle's steering wheel to approach the reference trajectory to achieve the purpose of trajectory tracking.

[0003] Whether it is the mature longitudinal control or the hot lateral control, the core of control is still to design a controller that meets the requirements and its code implementation. A survey of the field of autonomous driving control at home and abroad found that the commonly used controllers include PID controller, fuzzy PID, Stanley controller, sliding mode controller (SMC), model predictive control (MPC), pure tracking controller, linear quadratic regulator (LQR), etc. Among them, the idea of ​​​​the MPC controller is to combine the measurement value of the current system and the prediction model to predict the control output in the future, and by solving various constrained optimization problems, the system error is minimized in the future. Based on this, the MPC controller is more robust than other control methods, with higher accuracy and more common applications.

[0004] However, even with a relatively excellent controller, the environmental variables of daily driving and the vehicle's own factors will cause a significant decline in control accuracy: for the vehicle itself, first of all, the measurement error of the sensor and the aging of the equipment, the change of vehicle load, the four-wheel alignment and other factors will cause the control accuracy deviation; for environmental factors, including the terrain factors of the road, the adhesion coefficient, etc. Although the vehicle's own factors and sensor accuracy deviation are unavoidable, they can be compensated by adjusting the control parameters of the control algorithm. However, for the factors of driving road conditions, due to the changes in the terrain of the road, the friction coefficient is not a constant value, and the road curvature is also time-varying. Therefore, when the controller can achieve high-precision control in a certain section of the road, it does not mean that it is suitable for all other sections. When the driving road changes, some parameters of the controller, including the lateral error weight, the yaw angle error weight, the vehicle side slip coefficient, etc., will change. For fixed controller parameters, the lateral control of the vehicle at this time will definitely produce a certain deviation, thereby reducing the vehicle's driving stability and safety.

[0005] The search found that CN110221611A discloses a trajectory tracking control method, device and unmanned vehicle, and CN110568760A discloses a parameterized learning decision control system and method suitable for lane changing and lane keeping. Among them, the optimization method disclosed in CN110221611A is to use the reinforcement learning method to fit and train these cumbersome processes with neural networks. As we all know, replacing the solvable physical model with a parameter-fitted neural network has the advantage of omitting the complex calculation process, but the disadvantage is that it is equivalent to turning the original clear internal logical structure into a black box, and the internal operating mechanism cannot be understood and errors are prone to occur. Therefore, it is a very bold and dangerous approach for vehicle control. At the same time, the optimization method disclosed in CN110221611A uses the training A3C model. The state quantity of the neural network input of the optimization method disclosed in CN110221611A is the lateral deviation of the trajectory, and the output is directly the control quantity. The patent disclosed in CN110568760A describes a decision-making behavior module based on reinforcement learning. When the perception signal is input to the decision-making module, the trajectory route of the vehicle in the future time domain can be autonomously decided based on the reinforcement learning training, where the route includes the lateral deviation T of the vehicle. y , behavior time Tf, and acceleration and deceleration behavior. These decision parameters will be sent to the trajectory planning module to generate a trajectory input to the MPC controller to solve the optimal control quantity. Therefore, this patent focuses on using reinforcement learning to learn decision-making behaviors, and the MPC controller has not been improved. Summary of the invention

[0006] The purpose of the present invention is to propose a model predictive control trajectory tracking control system and method based on reinforcement learning in response to the defects of the prior art. The method adopts the DDPG algorithm to train and learn the error parameters in order to solve the problem that the fixed parameters of the model predictive controller are not suitable for the trajectory tracking control error caused by multiple road conditions, so that the controller can adjust the control parameters in time according to the actual state information and the reference trajectory when controlling the vehicle driving, thereby reducing the control error and improving the control accuracy.

[0007] In order to achieve the above objectives, the present invention provides a model predictive control trajectory tracking control system based on reinforcement learning, including a linear time-varying model predictive controller, a planning module, a state estimation module and a reinforcement learning module, wherein the linear time-varying model predictive controller includes a prediction model, an optimization solution module and an objective function + constraint condition module; the prediction module is used to characterize a vehicle and predict its driving trajectory in a future prediction time domain;

[0008] The optimization solution module is used to solve the objective function + constraint condition module, calculate the optimal control quantity, and send it to the controlled vehicle for execution;

[0009] The objective function + constraint condition module determines the numerical range of the optimal control amount and the incremental size of the control amount in the control time domain when the MPC controller controls the vehicle to track the trajectory;

[0010] The planning module is used to timely publish the local planning path trajectory to the linear time-varying model predictive controller according to the state quantity fed back by the vehicle;

[0011] The state estimation module is used to correct the model error between the theoretically established vehicle dynamics model and the actual vehicle model, so that the theoretical model is close to the dynamic characteristics of the actual vehicle;

[0012] The reinforcement learning module is used to timely adjust the error weight parameters of the linear time-varying model predictive controller according to the control error between the vehicle and the reference trajectory, adapt the driving road and trajectory, and improve the control accuracy.

[0013] The present invention discloses a model predictive control (MPC) trajectory tracking control system based on reinforcement learning, which is suitable for lateral trajectory tracking control during vehicle automatic driving. The system mainly includes two parts: an MPC controller and a reinforcement learning module. The MPC controller includes a prediction model, an optimization solution module, and a constraint cost function; the reinforcement learning module is an Actor and Critic neural network. By adopting this system, the mechanical characteristics of the vehicle can be analyzed, and a whole vehicle dynamics model can be established; the nonlinear model is linearized by Taylor series and discretized by Euler to obtain a prediction model about the vehicle state space; the optimization solution module solves the MPC cost function based on the constraint conditions and calculates the optimal control quantity; for the error weight matrix fixed by the MPC controller, the Deep Deterministic Policy Gradient (DDPG) algorithm of reinforcement learning is used to learn and train it. Combined with the idea of ​​reinforcement learning, this system greatly improves the control performance of the MPC controller, improves the vehicle trajectory tracking ability and robustness to the driving environment.

[0014] The present invention also provides a model predictive control trajectory tracking control method based on reinforcement learning, comprising the following steps:

[0015] Step 1: Use the positioning module to obtain the initial position information of the vehicle, the planning module plans the local reference trajectory, and sends the absolute coordinate value data of the trajectory in the ground coordinate system to the linear time-varying model predictive controller;

[0016] Step 2: The optimization solution module of the linear time-varying model predictive controller combines the objective function + constraint condition module to solve the optimal vehicle front wheel steering angle control value according to the objective function and the constraint condition;

[0017] Step 3: The optimization solution module sends the optimal vehicle front wheel angle control amount to the actuator of the controlled vehicle. After the vehicle executes, the state information of the next moment is fed back to the state estimation module, the reinforcement learning module and the planning module.

[0018] Step 4: The state estimation module calculates the deviation between the system reference model and the actual model of the vehicle, and inputs the calculation result to the linear time-varying model predictive controller to correct the error of its prediction model;

[0019] Step 5: The planning module replans the vehicle reference trajectory according to the state information at the next moment, and the reinforcement learning module adjusts the error weight parameter in the linear time-varying model predictive controller according to the difference between the actual vehicle trajectory state and the reference trajectory replanned by the planning module.

[0020] The present invention further adopts the following technical solution:

[0021] In step 2, the objective function is defined as follows:

[0022]

[0023] In the formula, Q represents the positive definite matrix of output weights, R represents the positive definite matrix of control weights, ρ is the relaxation factor weight, ε is the relaxation factor, represents the state quantity at time t, ΔU(t) represents the control increment at time t, N p represents the prediction time domain, N c Represents the control time domain, η represents the system output sequence between t and t+i predicted at time t, and Δu represents the control increment sequence between t and t+i predicted at time t; the above formula consists of three parts, which respectively represent the output state quantity, control quantity and relaxation term of the vehicle. The former reflects the error degree of the control algorithm in controlling the vehicle trajectory tracking according to the system output quantity in the prediction time domain; the control term reflects the speed of change of the control quantity per unit time according to the control input quantity increment in the control time domain, so as to ensure the smoothness and curvature of the steering; the addition of the relaxation term is to prevent the objective function from having no optimal solution due to the uncertain changes caused by the actual vehicle driving process;

[0024] The control weight matrix R in the objective function is a one-dimensional matrix, and the output weight matrix Q is a two-dimensional matrix, which is in the following form:

[0025]

[0026] In the formula, q lat represents the lateral error weight, q ψ Represents the yaw angle error weight.

[0027] In step 2, the constraints are defined as follows:

[0028] (1) Driving stability constraints

[0029]

[0030]

[0031] In the formula, δ represents the front wheel steering angle variable, δ H is the limit value of the front wheel turning angle under good adhesion coefficient, δ L , is the front wheel turning angle limit under low adhesion coefficient, l f Represents the distance between the front axle of the vehicle and the center of mass of the vehicle, l r Represents the distance between the rear axle of the vehicle and the center of mass of the vehicle, β H represents the sideslip angle of the center of mass under the condition of good adhesion coefficient, β L It represents the sideslip angle of the center of mass under low adhesion coefficient conditions. It is the angle formed by the center of mass speed and the longitudinal axis x of the vehicle;

[0032] (2) Front wheel swing restraint

[0033]

[0034] In the formula, δ min is the minimum turning angle of the front wheels of the vehicle, δ max is the maximum turning angle of the front wheels of the vehicle, Δδ min is the minimum increment of the front wheel per unit time, Δδ max is the maximum increment of the front wheel of the vehicle per unit time, Δδ represents the increment of the front wheel of the vehicle per unit time;

[0035] (3) Output state constraints

[0036]

[0037] In the formula, e lat min is the minimum lateral trajectory error of the vehicle, e lat max is the maximum lateral trajectory error of the vehicle, e ψ min is the minimum yaw angle error of the vehicle, e ψ max is the maximum yaw angle error of the vehicle, e lat represents the lateral trajectory error of the vehicle, e ψ is the yaw angle error of the vehicle.

[0038] In step 2, the process of the optimization solution module solving the optimal vehicle front wheel steering angle control amount is as follows:

[0039] Step 2-1, define the matrix Q e With R e :

[0040]

[0041]

[0042] In the formula, is the Krone product operator, I Np The dimension is N p The identity matrix, I Nc The dimension is N c The identity matrix of

[0043] Step 2-2, rewrite the objective function expression as:

[0044]

[0045]

[0046] In the formula, Y represents the system output sequence matrix between t and t+i predicted at time t, ΔU represents the control increment sequence matrix between t and t+i predicted at time t, are coefficient matrices, H t To characterize Q e The process matrix, To characterize R e The process matrix, P t It is a constant term that does not affect the solution and can be ignored;

[0047] Step 2-3: Convert the MPC control problem into a quadratic programming problem and rewrite the objective function and constraints as follows:

[0048]

[0049] st.ΔU min ≤ΔU(t)≤ΔU max

[0050] U mid ≤U(t)≤U max

[0051]

[0052]

[0053] Among them, ΔU min Indicates the minimum value of the vehicle control amount change, ΔU max Indicates the maximum value of the vehicle control amount change, ΔU(t) is the vehicle control amount change value; U min is the minimum value of vehicle control quantity, U maxis the maximum value of the vehicle control amount, U(t) is the vehicle control amount; Y c min is the minimum hard constraint of the system state quantity, Y c max is the maximum hard constraint of the system state quantity; Y sc min is the minimum soft constraint of the system state quantity, Y sc max is the maximum soft constraint on the system state quantity.

[0054] In step 3, the controller control quantity is the vehicle front wheel steering angle, and the state quantity includes the vehicle coordinate XY value, the vehicle lateral speed, the vehicle longitudinal speed, the vehicle yaw angle, the vehicle yaw angular velocity, and the prediction model outputs the vehicle lateral error and yaw angle error.

[0055] In step 5, the design of the reinforcement learning module is as follows:

[0056] The observation states of the reinforcement learning module are defined as yaw angle deviation and lateral deviation, and the action set is defined as lateral error weight and yaw angle error weight.

[0057] Build an Actor network. The Actor network includes a three-dimensional state input layer, two fully connected layers as hidden layers, and a two-dimensional action output layer. The first hidden layer is after the state input layer, which is a fully connected layer consisting of 200 neurons. The second hidden layer is after the first layer, which is also a fully connected layer consisting of 128 neurons. The second hidden layer is connected to the two-dimensional action output layer. The output layer activation function of the Actor network action is the hyperbolic tangent function (Tanh).

[0058] Build a Critic network, which consists of two input layers, two fully connected layers and an output layer. After the two-dimensional state input layer is the first hidden layer, which consists of 200 neurons and is a fully connected layer. The two-dimensional action input layer and the first hidden layer are connected to the second hidden layer, which consists of 128 neurons and is also a fully connected layer. The second hidden layer is one-dimensionally connected to the output layer, and the output layer outputs the value function of the action. The activation function of the action value function output layer uses identity transformation.

[0059] The Actor network update strategy is deterministic policy gradient plus Omstein-Uhlenbeck (1930) process noise to increase random exploration; the Critic network is based on the value function and updates the Critic network by minimizing the loss function;

[0060] The reward function setting is divided into lateral error range and yaw angle error range. The lateral error range is divided into less than 0.05m, less than 0.10m, less than 0.15m, less than 0.20m, less than 0.30m, greater than 0.40m, greater than 1.00m, and greater than 3.00m; the yaw angle error range is divided into less than 0.05rad, less than 0.10rad, less than 0.20rad, less than 0.25rad, greater than 0.30rad, greater than 0.40rad, and greater than 0.50rad.

[0061] In step 5, the updating steps of the neural network of the reinforcement learning module are as follows:

[0062] Step 5-1: In the observation state s t Next, we select action a by following the strategy of adding noise t , enter the state s at the next moment t+1 , and get the reward value R t+1 ;

[0063] Step 5-2: Sequence (s t , a t , R t+1 ,s t+1 ) is stored in the experience replay pool, and n groups of sequences (s t , a t , R t+1 ,s t+1 );

[0064] Step 5-3. Set the target value y of the value function t = r(s t , a t )+γQ′(s t+1 ,μ′(s t+1 )|ω′), where r represents the reward at time t, γ represents the discount factor at time t, Q′ represents the Q value at time t+1, μ′ represents the target actor network at time t+1, and ω′ represents the target critic network at time t+1;

[0065] Step 5-4: Update the Critic network by minimizing the loss function. The expression is as follows:

[0066]

[0067] Step 5-5, update the Actor network with sampled gradients. The expression is as follows:

[0068]

[0069] Step 5-6: Update the target Actor network and Critic network: θ Q′←τθ Q +(1-τ)θ Q′ ,θ μ′ ←τθ μ +(1-τ)θ μ′ ,τ<<1

[0070] in, represents the expected value of the loss function between the current Critic network and another Critic network β, ω represents the network parameters of the current Critic network, y t Represents the computational value of another Critic network β, represents the expected value of the loss function of the current actor network, Represents the gradient of the current value, a=μ(s t ) indicates that based on the current s t The action value a calculated by the actor network, represents the policy gradient of the current actor network selection action, θ Q′ represents the target Critic network, θ μ represents the target Actor network, θ Q represents the current Critic network, θ μ Represents the current Actor network, and τ is the update rate.

[0071] The present invention adopts the design of MPC controller, including dynamic modeling, model expression solution, MPC prediction model establishment, objective function and constraint condition setting, and objective function solution optimization within the constraint condition. Based on the traditional MPC controller design idea, the present invention simplifies the derivation of the formula, so that the control of the vehicle is supported by the theoretical model basis. The present invention uses the reinforcement learning method to train the fixed objective function error weight parameter, so that when the vehicle is driving in different environments, the controller can match the optimal error weight, thereby reducing the control error. The reinforcement learning training method is adopted on the basis of ensuring the correctness of the theoretical logic of vehicle control, which is a stable and precise approach. At the same time, although MPC is a classic controller, the focus is different according to its design ideas, selected variables and other factors. According to the lateral control of the vehicle, the present invention selects the state quantity as the absolute coordinates to the ground, the lateral and longitudinal speeds, etc., the control quantity is the front wheel angle, and the setting of the constraint condition is mainly carried out around the restriction of the front wheel angle. In addition, the state quantity input to the DDPG neural network of the present invention is the lateral deviation and yaw angle deviation of the trajectory and the curvature of the road, and the output is the adjustment of the lateral error weight and the yaw angle error weight value. Compared with the traditional method, the DDPG method adopted in the present invention has a small action space variance and a higher algorithm convergence speed than A3C; the reward function, state selection, action definition and neural network structure of reinforcement learning are all different from those of the traditional method. In short, the present invention is designed to optimize the error weight parameters of the MPC controller using the reinforcement learning method, so as to achieve the purpose of reducing control error and improving robustness.

[0072] In addition, since the direct factor of control error is the control accuracy of the controller, even if the decision parameters are accurate, the inaccurate control amount calculated by the controller is still the key to the control error. Compared with the prior art, the present invention achieves the purpose of optimizing the MPC controller by training the controller error weight parameters through reinforcement learning, thereby reducing the control error and improving the robustness of the controller to the environment.

[0073] The advantages of the present invention are as follows:

[0074] (1) Based on the idea of ​​reinforcement learning, the controller parameters are changed according to different road conditions, and the control accuracy is significantly improved;

[0075] (2) The controller with variable parameters can show better robustness to the driving environment;

[0076] (3) The controller's single control instruction calculation to release test cycle is less than 0.0 ls, and the control real-time performance is good. BRIEF DESCRIPTION OF THE DRAWINGS

[0077] The present invention will be further described below in conjunction with the accompanying drawings.

[0078] Figure 1 It is a control framework diagram of the present invention.

[0079] Figure 2 This is the Actor neural network structure diagram of the DDPG algorithm of the present invention.

[0080] Figure 3 This is the DDPG algorithm Critic neural network structure diagram of the present invention. DETAILED DESCRIPTION

[0081] Embodiment 1

[0082] like Figure 1 As shown, a model predictive control trajectory tracking control system based on reinforcement learning includes a linear time-varying model predictive controller, a planning module, a state estimation module and a reinforcement learning module, wherein the linear time-varying model predictive controller includes a prediction model, an optimization solution module and an objective function + constraint condition module. The prediction module is used to characterize the vehicle and predict its driving trajectory in the future prediction time domain; the optimization solution module is used to solve the objective function + constraint condition module, calculate the optimal control quantity, and send it to the controlled vehicle for execution; the objective function + constraint condition module determines the numerical range of the optimal control quantity calculated and issued when the MPC controller controls the vehicle to track the trajectory and the incremental size of the control quantity in the control time domain; the planning module is used to timely publish the local planning path trajectory to the linear time-varying model predictive controller according to the state quantity fed back by the vehicle; the state estimation module is used to correct the model error between the theoretically established vehicle dynamics model and the actual vehicle model, so that the theoretical model is close to the dynamic characteristics of the actual vehicle; the reinforcement learning module is used to timely adjust the error weight parameter of the linear time-varying model predictive controller according to the control error between the vehicle and the reference trajectory, adapt to the driving road and trajectory, and improve the control accuracy.

[0083] A model predictive control trajectory tracking control method based on reinforcement learning, comprising the following steps:

[0084] Step 1: Use the positioning module to obtain the initial position information of the vehicle. The planning module uses the A* algorithm to plan the local reference trajectory based on the initial position information of the vehicle, and sends the absolute coordinate value data of the trajectory in the ground coordinate system to the optimization solution module of the linear time-varying model predictive controller to calculate and solve the optimal control quantity. At the same time, the prediction module predicts the driving trajectory in the future prediction time domain based on the initial position information of the vehicle through its internal prediction model as a feedback link and sends the predicted trajectory to the optimization solution module.

[0085] Step 2: The optimization solution module of the linear time-varying model predictive controller receives the reference trajectory of the planning module and the predicted trajectory of the prediction module, and then combines the objective function + constraint condition module to solve the optimal vehicle front wheel steering angle control value according to the objective function and the constraint conditions.

[0086] The objective function is defined as follows:

[0087]

[0088] Where Q and R represent the positive definite matrices of output weight and control weight respectively, ρ is the relaxation factor weight, ε is the relaxation factor, represents the state quantity at time t, ΔU(t) represents the control increment at time t, N p 、N c Represent the prediction time domain and the control time domain respectively, η and Δu represent the system output sequence and control increment sequence between time t and time t+i predicted at time t respectively. The above formula consists of three parts, which represent the output state quantity, control quantity and relaxation term of the vehicle respectively. The former reflects the error degree of the control algorithm in controlling the vehicle trajectory tracking according to the system output quantity in the prediction time domain; the control term reflects the change speed of the control quantity per unit time according to the control input quantity increment in the control time domain to ensure the stability and curvature of the steering; the relaxation term is added to prevent the objective function from having no optimal solution due to the uncertain changes caused by the actual vehicle driving process.

[0089] The control weight matrix R in the objective function is a one-dimensional matrix, and the output weight matrix Q is a two-dimensional matrix, which is in the following form:

[0090]

[0091] In the formula, q lat represents the lateral error weight, q ψ Represents the yaw angle error weight.

[0092] The constraints are divided into three parts: driving stability constraints, front wheel swing angle constraints, and output state constraints. The first two restrict the front wheel swing angle and angular velocity of the vehicle under terrain factors and the vehicle's own physical conditions, and the last one restricts the error term in the vehicle trajectory tracking process. Then, the constraints are defined as follows:

[0093] (1) Driving stability constraints

[0094]

[0095]

[0096] In the formula, δ represents the front wheel steering angle variable, δ His the limit value of the front wheel turning angle under good adhesion coefficient, δ L is the front wheel turning angle limit under low adhesion coefficient, l f Represents the distance between the front axle of the vehicle and the center of mass of the vehicle, l r Represents the distance between the rear axle of the vehicle and the center of mass of the vehicle, β H represents the sideslip angle of the center of mass under the condition of good adhesion coefficient, β L It represents the sideslip angle of the center of mass under low adhesion coefficient conditions. It is the angle formed by the center of mass speed and the longitudinal axis x of the vehicle;

[0097] (2) Front wheel swing restraint

[0098]

[0099] In the formula, δ min is the minimum turning angle of the front wheels of the vehicle, δ max is the maximum turning angle of the front wheels of the vehicle, Δδ min is the minimum increment of the front wheel per unit time, Δδ max is the maximum increment of the front wheel of the vehicle per unit time, Δδ represents the increment of the front wheel of the vehicle per unit time;

[0100] (3) Output state constraints

[0101]

[0102] In the formula, e lat min is the minimum lateral trajectory error of the vehicle, e lat max is the maximum lateral trajectory error of the vehicle, e ψ min is the minimum yaw angle error of the vehicle, e ψ max is the maximum yaw angle error of the vehicle, e lat represents the lateral trajectory error of the vehicle, e ψ is the yaw angle error of the vehicle.

[0103] The optimization solution module converts the MPC control problem into a quadratic programming problem. The specific steps for solving the optimal vehicle front wheel steering angle control amount are as follows:

[0104] Step 2-1, define the matrix Q e With R e :

[0105]

[0106]

[0107] In the formula, is the Krone product operator, I Np with I Nc The dimensions are N pWith N c The identity matrix of

[0108] Step 2-2, rewrite the objective function expression as:

[0109]

[0110]

[0111] In the formula, Y and ΔU represent the system output sequence matrix and control increment sequence matrix between time t and time t+i predicted at time t, respectively. are coefficient matrices, H t , To characterize Q e With R e The process matrix, P t It is a constant term that does not affect the solution and can be ignored;

[0112] Step 2-3: Convert the MPC control problem into a quadratic programming problem and rewrite the objective function and constraints as follows:

[0113]

[0114] st.ΔU min ≤ΔU(t)≤ΔU max

[0115] U min ≤U(t)≤U max

[0116]

[0117]

[0118] Among them, ΔU min , ΔU max They represent the minimum and maximum values ​​of the vehicle control amount change, ΔU(t) is the vehicle control amount change value; U min , U max is the minimum and maximum value of the vehicle control quantity, U(t) is the vehicle control quantity; Y c min , Y c max are the minimum hard constraint and the maximum hard constraint of the system state quantity respectively; Y sc min , Y sc max They are the minimum soft constraint and the maximum soft constraint of the system state quantity respectively.

[0119] Step 3: The optimization solution module sends the optimal front wheel steering angle control value obtained to the actuator of the controlled vehicle for execution. The state quantity obtained after the vehicle executes the operation is It will be fed back to the state estimation module, reinforcement learning module and planning module.

[0120] The front wheel steering angle control quantity U(t) obtained by the controller is the front wheel steering angle of the vehicle, and the state quantity Including vehicle coordinate XY values, vehicle lateral speed, vehicle longitudinal speed, vehicle yaw angle, vehicle yaw angular velocity, and prediction model output vehicle lateral error and yaw angle error.

[0121] Step 4: The state estimation module receives the vehicle state after the vehicle executes the control amount, and compares it with the dynamic reference model obtained by theoretical modeling to obtain the model error for correcting the Paralympic model. The state estimation module calculates the deviation between the system reference model data and the actual vehicle model data through parameter fitting, and inputs the fitting result to the linear time-varying model predictive controller in the form of dynamic model parameter deviation to correct the parameter error of its prediction model. After the prediction model corrects the error, it predicts the vehicle's future driving trajectory in the prediction time domain again and inputs the predicted trajectory to the optimization solution module.

[0122] Step 5: The planning module replans the vehicle reference trajectory according to the state information at the next moment and inputs the reference trajectory into the reinforcement learning module and the optimization solution module. The reinforcement learning module adjusts the error weight parameter in the linear time-varying model predictive controller according to the difference between the actual vehicle trajectory state and the reference trajectory replanned by the planning module, so as to change the error weight parameter according to different driving road conditions, so that it can adapt to more complex roads and reduce the trajectory tracking error caused by fixed parameters.

[0123] In the above steps 4 and 5, after the predicted trajectory of the prediction module and the reference trajectory of the planning module are input into the optimization solution module, the operation is returned to step 2, that is, the optimization solution module solves the objective function + constraint condition module and sends the solved optimal vehicle front wheel steering angle control amount to the vehicle for execution.

[0124] In step 5, the reinforcement learning module includes the state set, action set, Actor-Critic network, network update strategy and reward function. The design of the reinforcement learning module is as follows:

[0125] The observation states of the reinforcement learning module are defined as yaw angle deviation and lateral deviation, and the action set is defined as lateral error weight and yaw angle error weight.

[0126] like Figure 2As shown, build an Actor network. The Actor network consists of a three-dimensional state input layer, two fully connected layers as hidden layers, and a two-dimensional action output layer. The state input layer is followed by the first hidden layer, which consists of 200 neurons and is a fully connected layer; the first layer is followed by the second hidden layer, which consists of 128 neurons and is also a fully connected layer; the second hidden layer is followed by a two-dimensional action output layer. The output layer activation function of the Actor network action is the hyperbolic tangent function (Tanh).

[0127] like Figure 3 As shown, the Critic network is built. The Critic network consists of two input layers, two fully connected layers and an output layer. After the two-dimensional state input layer is the first hidden layer, which consists of 200 neurons and is a fully connected layer; the two-dimensional action input layer and the first hidden layer are connected to the second hidden layer, which consists of 128 neurons and is also a fully connected layer; the second hidden layer is one-dimensionally connected to the output layer, and the output layer outputs the value function of the action. The activation function of the action value function output layer adopts identity transformation.

[0128] The Actor network update strategy is deterministic policy gradient plus Omstein-Uhlenbeck (1930) process noise to increase random exploration; the Critic network is based on the value function and updates the Critic network by minimizing the loss function.

[0129] The reward function is divided into lateral error range and yaw angle error range. The error ranges and reward values ​​can be seen in Table 1. The lateral error range is less than 0.05m, less than 0.10m, less than 0.15m, less than 0.20m, less than 0.30m, greater than 0.40m, greater than 1.00m, and greater than 3.00m; the yaw angle error range is less than 0.05rad, less than 0.10rad, less than 0.20rad, less than 0.25rad, greater than 0.30rad, greater than 0.40rad, and greater than 0.50rad.

[0130] Table 1 Reward Distribution Table

[0131]

[0132] The update steps of the neural network of the reinforcement learning module are as follows:

[0133] Step 5-1: In the observation state s t Next, we select action a by following the strategy of adding noise t , enter the state s at the next moment t+1 , and get the reward value R t+1 ;

[0134] Step 5-2: Sequence (s t , a t , R t+1 ,s t+1 ) is stored in the experience replay pool, and n groups of sequences (s t , a t , R t+1 ,s t+1 );

[0135] Step 5-3. Set the target value y of the value function t = r(s t , a t )+γQ′(s t+1 ,μ′(s t+1 )|ω′), where r, γ, Q′, μ′, and ω′ represent the reward at time t, the discount factor, the Q value at time t+1, the target actor network, and the target critic network, respectively;

[0136] Step 5-4: Update the Critic network by minimizing the loss function. The expression is as follows:

[0137]

[0138] Step 5-5, update the Actor network with sampled gradients. The expression is as follows:

[0139]

[0140] Step 5-6: Update the target Actor network and Critic network: θ Q′ ←τθ Q +(1-τ)θ Q′ ,θ μ′ ←τθ μ +(1-τ)θ μ′ ,τ<<1

[0141] in, ω、y t They represent the expected value of the loss function between the current Critic network and another Critic network β, the network parameters of the current Critic network, and the computational value of another Critic network β. a=μ(s t ), They represent the expected value of the loss function of the current actor network, the gradient of the current value, and the gradient of the current value based on the current s. t The action value a calculated by the actor network, the policy gradient of the current actor network to select the action, θ Q′ ,θ μ′Represent the target Critic network and the target Actor network respectively, θ Q represents the current Critic network, θ μ Represents the current Actor network, and τ is the update rate.

[0142] The present invention adopts a planning module to plan a local reference trajectory, a prediction model of a controller predicts a driving trajectory in a future prediction time domain, the predicted trajectory is input into an optimization solution module in combination with a reference trajectory of the planning module, the optimization solution module solves an objective function + constraint condition module, calculates an optimal control quantity, and sends it to a controlled vehicle for execution; a state quantity obtained after the vehicle executes a control command is fed back to a state estimation module, a planning module, and a reinforcement learning module; the state estimation module corrects the prediction model error according to the feedback state quantity, and after the prediction model corrects the error, it predicts the driving trajectory in the future prediction time domain again and inputs the predicted trajectory into the optimization solution module; the planning module performs local path planning again and inputs the re-planned reference trajectory into the optimization solution module and the reinforcement learning module (the optimization solution module can solve the objective function + constraint condition module again according to the re-input prediction trajectory and reference trajectory); the reinforcement learning module adjusts the vehicle MPC control error weight parameter according to the difference between the reference trajectory and the actual vehicle position, adapts to the driving road and trajectory, and improves the control accuracy.

[0143] In addition to the above embodiments, the present invention may also have other implementation modes. Any technical solution formed by equivalent replacement or equivalent transformation falls within the protection scope required by the present invention.

Claims

1. A model predictive control trajectory tracking control system based on reinforcement learning, characterized by: It includes a linear time-varying model predictive controller, a planning module, a state estimation module, and a reinforcement learning module. The linear time-varying model predictive controller includes a prediction model, an optimization solution module, and an objective function + constraint condition module. The prediction model is used to characterize the vehicle and predict its driving trajectory in the future prediction time domain. The optimization solution module is used to solve the objective function + constraint condition module, calculate the optimal control quantity, and send it to the controlled vehicle for execution; The objective function + constraint condition module is used to determine the numerical range of the optimal control quantity and the incremental size of the control quantity within the control time domain when the MPC controller controls the vehicle to track the trajectory. The objective function is defined as follows: Where Q represents the positive definite matrix of output weights, R represents the positive definite matrix of control weights, ρ is the relaxation factor weight, ε is the relaxation factor, represents the state quantity at time t, ΔU(t) represents the control increment at time t, N p represents the prediction time domain, N c represents the control time domain, η represents the system output sequence between t and t+i predicted at time t, and Δu represents the control increment sequence between t and t+i predicted at time t; The control weight matrix R in the objective function is a one-dimensional matrix, and the output weight matrix Q is a two-dimensional matrix, which is in the following form: Where q lat represents the lateral error weight, q ψ represents the yaw angle error weight; The constraints are defined as follows: (1) Driving stability constraints Where δ represents the front wheel angle variable, δ H is the front wheel turning angle limit under good adhesion coefficient, δ L is the front wheel turning angle limit under low adhesion coefficient, l f Represents the distance between the front axle of the vehicle and the center of mass of the vehicle, l r Represents the distance between the rear axle of the vehicle and the center of mass of the vehicle, β H Indicates the sideslip angle of the center of mass under good adhesion coefficient, β L It represents the sideslip angle of the center of mass under low adhesion coefficient conditions; (2) Front wheel swing restraint Where, δ min is the minimum turning angle of the vehicle’s front wheels, δ max is the maximum turning angle of the vehicle’s front wheels, Δδ min is the minimum increment of the front wheel per unit time, Δδ max is the maximum increment of the front wheel of the vehicle per unit time, and Δδ represents the increment of the front wheel of the vehicle per unit time; (3) Output state constraints Where, e latmin is the minimum lateral trajectory error of the vehicle, e latmax is the maximum lateral trajectory error of the vehicle, e ψmin is the minimum yaw angle error of the vehicle, e ψmax is the maximum yaw angle error of the vehicle, e lat represents the lateral trajectory error of the vehicle, e ψ is the vehicle's yaw angle error; The planning module is used to timely publish the local planned path trajectory to the linear time-varying model predictive controller based on the state quantity fed back by the vehicle; The state estimation module is used to correct the model error between the theoretically established vehicle dynamics model and the actual vehicle model, so that the theoretical model is closer to the dynamic characteristics of the actual vehicle; The reinforcement learning module is used to timely adjust the error weight parameters of the linear time-varying model predictive controller based on the control error between the vehicle and the reference trajectory, adapt to the driving road and trajectory, and improve control accuracy; The design of the reinforcement learning module is as follows: The observation states of the reinforcement learning module are defined as yaw angle deviation and lateral deviation, and the action set is defined as lateral error weight and yaw angle error weight. Build an Actor network, which consists of a 3D state input layer, two fully connected layers, and a 2D action output layer. The first hidden layer after the state input layer consists of 200 neurons and is a fully connected layer. After the first layer is the second hidden layer, which consists of 128 neurons and is also a fully connected layer; after the second hidden layer is connected to the two-dimensional action output layer; Build a critic network, which consists of two input layers, two fully connected layers, and an output layer. After the two-dimensional state input layer is the first hidden layer, which consists of 200 neurons and is a fully connected layer. The two-dimensional action input layer and the first hidden layer are connected to the second hidden layer, which consists of 128 neurons and is also a fully connected layer. The second hidden layer is one-dimensionally connected to the output layer, which outputs the value function of the action. The actor network update strategy is a deterministic policy gradient plus Ornstein-Uhlenbeck process noise to increase random exploration. The critic network is based on the value function and updates the critic network by minimizing the loss function. The reward function is set up as a lateral error range and a yaw angle error range. The lateral error range is divided into less than 0.05m, less than 0.10m, less than 0.15m, less than 0.20m, less than 0.30m, greater than 0.40m, greater than 1.00m, and greater than 3.00m; the yaw angle error range is divided into less than 0.05rad, less than 0.10rad, less than 0.20rad, less than 0.25rad, greater than 0.30rad, greater than 0.40rad, and greater than 0.50rad.

2. The method of a model predictive control trajectory tracking control system based on reinforcement learning according to claim 1, characterized in that: The following steps are involved: Step 1: Obtain the initial position information of the vehicle, and the planning module plans a local reference trajectory and sends the absolute coordinate value data of the trajectory in the ground coordinate system to the linear time-varying model predictive controller; Step 2: The optimization solution module of the linear time-varying model predictive controller solves the optimal vehicle front wheel angle control value according to the objective function and the constraint conditions; Step 3: Send the optimal front wheel steering angle control value to the actuator of the controlled vehicle. After the vehicle executes the control, the state information of the next moment is fed back to the state estimation module, reinforcement learning module and planning module. Step 4: The state estimation module calculates the deviation between the system reference model and the actual vehicle model, and inputs the calculation result to the linear time-varying model predictive controller to correct the error of its prediction model; Step 5: The planning module replans the vehicle reference trajectory based on the state information at the next moment, and the reinforcement learning module adjusts the error weight parameter in the linear time-varying model predictive controller based on the difference between the actual vehicle trajectory state and the reference trajectory replanned by the planning module.

3. The model predictive control trajectory tracking control method based on reinforcement learning according to claim 2, characterized in that: In step 2, the objective function is defined as follows: Where Q represents the positive definite matrix of output weights, R represents the positive definite matrix of control weights, ρ is the relaxation factor weight, ε is the relaxation factor, represents the state quantity at time t, ΔU(t) represents the control increment at time t, N p represents the prediction time domain, N c represents the control time domain, η represents the system output sequence between t and t+i predicted at time t, and Δu represents the control increment sequence between t and t+i predicted at time t; The control weight matrix R in the objective function is a one-dimensional matrix, and the output weight matrix Q is a two-dimensional matrix, which is in the following form: Where q lat represents the lateral error weight, q ψ represents the yaw angle error weight.

4. The model predictive control trajectory tracking control method based on reinforcement learning according to claim 2, characterized in that: In step 2, the constraints are defined as follows: (1) Driving stability constraints Where δ represents the front wheel angle variable, δ H is the front wheel turning angle limit under good adhesion coefficient, δ L is the front wheel turning angle limit under low adhesion coefficient, l f Represents the distance between the front axle of the vehicle and the center of mass of the vehicle, l r Represents the distance between the rear axle of the vehicle and the center of mass of the vehicle, β H Indicates the sideslip angle of the center of mass under good adhesion coefficient, β L It represents the sideslip angle of the center of mass under low adhesion coefficient conditions; (2) Front wheel swing restraint Where, δ min is the minimum turning angle of the vehicle’s front wheels, δ max is the maximum turning angle of the vehicle’s front wheels, Δδ min is the minimum increment of the front wheel per unit time, Δδ max is the maximum increment of the front wheel of the vehicle per unit time, and Δδ represents the increment of the front wheel of the vehicle per unit time; (3) Output state constraints Where, e lat min is the minimum lateral trajectory error of the vehicle, e lat max is the maximum lateral trajectory error of the vehicle, e ψmin is the minimum yaw angle error of the vehicle, e ψmax is the maximum yaw angle error of the vehicle, e lat represents the lateral trajectory error of the vehicle, e ψ is the vehicle's yaw angle error.

5. The model predictive control trajectory tracking control method based on reinforcement learning according to claim 2, characterized in that: In step 2, the process of the optimization solution module solving the optimal vehicle front wheel angle control value is as follows: Step 2-1. Define the matrix Q e With R e : Where, is the Krone product operator, N Np The dimension is N p The identity matrix, I Nc The dimension is N c The identity matrix of Step 2-2, rewrite the objective function expression as: Where Y represents the system output sequence matrix between time t and time t+i predicted at time t, ΔU represents the control increment sequence matrix between time t and time t+i predicted at time t, are coefficient matrices, H t To characterize Q e The process matrix, To characterize R e The process matrix, P t is a constant term; Step 2-3: Convert the MPC control problem into a quadratic programming problem and rewrite the objective function and constraints as follows: st.ΔU min ≤ΔU(t)≤ΔU max U min ≤U(t)≤U max Among them, ΔU min Indicates the minimum value of the vehicle control amount change, ΔU max Indicates the maximum value of the vehicle control amount change, ΔU(t) is the vehicle control amount change value; U min is the minimum value of the vehicle control quantity, U max is the maximum value of the vehicle control amount, U(t) is the vehicle control amount; Y c min is the minimum hard constraint of the system state quantity, Y c max is the maximum hard constraint of the system state quantity; Y sc min is the minimum soft constraint of the system state quantity, Y sc max is the maximum soft constraint of the system state quantity.

6. The model predictive control trajectory tracking control method based on reinforcement learning according to claim 2, characterized in that: In step 3, the controller control quantity U(t) is the vehicle front wheel angle δ, and the state quantity Including vehicle coordinate XY values, vehicle lateral speed, vehicle longitudinal speed, vehicle yaw angle, vehicle yaw angular velocity, and prediction model output vehicle lateral error and yaw angle error.

7. The model predictive control trajectory tracking control method based on reinforcement learning according to claim 2, characterized in that: In step 5, the design of the reinforcement learning module is as follows: The observation states of the reinforcement learning module are defined as yaw angle deviation and lateral deviation, and the action set is defined as lateral error weight and yaw angle error weight. Build an Actor network, which consists of a 3D state input layer, two fully connected layers, and a 2D action output layer. The first hidden layer after the state input layer consists of 200 neurons and is a fully connected layer. After the first layer is the second hidden layer, which consists of 128 neurons and is also a fully connected layer; after the second hidden layer is connected to the two-dimensional action output layer; Build a critic network, which consists of two input layers, two fully connected layers, and an output layer. After the two-dimensional state input layer is the first hidden layer, which consists of 200 neurons and is a fully connected layer. The two-dimensional action input layer and the first hidden layer are connected to the second hidden layer, which consists of 128 neurons and is also a fully connected layer. The second hidden layer is one-dimensionally connected to the output layer, which outputs the value function of the action. The actor network update strategy is a deterministic policy gradient plus Ornstein-Uhlenbeck process noise to increase random exploration. The critic network is based on the value function and updates the critic network by minimizing the loss function. The reward function is set up as a lateral error range and a yaw angle error range. The lateral error range is divided into less than 0.05m, less than 0.10m, less than 0.15m, less than 0.20m, less than 0.30m, greater than 0.40m, greater than 1.00m, and greater than 3.00m; the yaw angle error range is divided into less than 0.05rad, less than 0.10rad, less than 0.20rad, less than 0.25rad, greater than 0.30rad, greater than 0.40rad, and greater than 0.50rad.

8. The model predictive control trajectory tracking control method based on reinforcement learning according to claim 2, characterized in that: In step 5, the updating steps of the reinforcement learning module neural network are as follows: Step 5-1, in the observation state s t Next, follow the strategy of adding noise process to select action a t , enter the state s at the next moment t+1 , and get the reward value R t+1 ; Step 5-2, the sequence (s t ,a t ,R t+1 ,s t+1 ) is stored in the experience replay pool, and n groups of sequences (s t ,a t ,R t+1 ,s t+1 ); Step 5-3. Set the target value y of the value function t =r(s t ,a t )+γQ′(s t+1 ,μ′(s t+1 )|ω′), where r represents the reward at time t, γ represents the discount factor at time t, Q′ represents the Q value at time t+1, μ′ represents the target actor network at time t+1, and ω′ represents the target critic network at time t+1; Step 5-4: Update the Critic network by minimizing the loss function. The expression is as follows: Step 5-5, update the Actor network with the sampled gradient. The expression is as follows: Steps 5-6: Update the target actor network and the critic network: θ Q′ ←τθ Q +(1-τ)θ Q′ ,θ μ′ ←τθ μ +(1-τ)θ μ′ ,τ<<1 in, represents the expected value of the loss function between the current Critic network and another Critic network β, ω represents the network parameters of the current Critic network, y t Represents the computational value of another Critic network β, Represents the expected value of the loss function of the current actor network, Represents the gradient of the current value, a=μ(s t ) indicates that based on the current s t The action value a calculated by the actor network, represents the policy gradient of the current actor network's action selection, θ Q′ represents the target Critic network, θ μ′ represents the target Actor network, θ Q Represents the current Critic network, θ μ Represents the current Actor network, and τ is the update rate.

Images