Environment-adaptive MPC path tracking control method based on new heading error definition

Abstract

The present invention provides an environment adaptive MPC path tracking control method based on a new heading error definition. The method establishes a vehicle state space model and constructs a model predictive control MPC path tracking control strategy based on the new heading error definition, and uses a reinforcement learning algorithm DQN to optimize the prediction time domain and control time domain of the model predictive control MPC, thereby reducing the tracking error and improving the path tracking accuracy.

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving path tracking control technology, and in particular to an environment adaptive MPC path tracking control method based on a new heading error definition. Background Art

[0002] With the development of positioning systems, recognition systems and control systems, smart cars have become the main trend of future car development. As one of the core technologies of smart cars, trajectory tracking aims to ensure that the vehicle can stably and accurately follow the target path in different driving environments.

[0003] MPC is a widely used path tracking control method because of its unique advantages in dealing with multi-input multi-output systems and nonlinear systems. It solves the front wheel angle by predicting the vehicle state in the future to minimize the tracking error within the prediction time. The vehicle's heading control should actually be to control the actual speed direction of the vehicle to be consistent with the tangential direction of the target path point, but the heading error definition of the traditional MPC path tracking method is to control the longitudinal axis direction of the vehicle to be consistent with the tangential direction of the target path point, which will make it difficult for the MPC algorithm to ensure that the lateral error and the heading error converge to a smaller value at the same time when solving the front wheel angle. Summary of the invention

[0004] In view of this, the object of the present invention is to provide an environment adaptive MPC path tracking control method based on a new heading error definition, so as to reduce the tracking error and improve the path tracking accuracy.

[0005] To achieve the above object, the present invention adopts the following technical solutions: an environment adaptive MPC path tracking control method based on a new heading error definition, which establishes a vehicle state space model and constructs a model predictive control MPC path tracking control strategy based on the new heading error definition, and uses a reinforcement learning algorithm DQN to optimize the prediction time domain and control time domain of the model predictive control MPC;

[0006] The following steps are involved:

[0007] Step 1: Establish a path tracking control strategy based on the model predictive control MPC algorithm, which includes the following sub-steps;

[0008] Step 2: Use reinforcement learning optimization to obtain the mapping relationship between the prediction time domain and the control time domain and the curvature radius and vehicle speed;

[0009] Step 3: The trained agent selects the corresponding action output according to different state values, and then uses the path tracking control strategy in step 1 to calculate the front wheel angle to act on the wheel to control the vehicle movement.

[0010] In a preferred embodiment, step 1.1: establish a vehicle state space model based on the new heading error definition:

[0011]

[0012] In the formula y=[Y θ] T , B=[2c f / m 0 2c f l f / Iz 0] T ,

[0013]

[0014]

[0015] x is the state matrix of the vehicle, u is the control variable, i.e. the front wheel steering angle δ, y is the output matrix; A, B, C are coefficient matrices; c f and c r is the cornering stiffness of the front and rear wheels of the vehicle, l f and l r are the distances from the front and rear axles to the center of mass, v x and v y are the longitudinal and lateral velocities of the vehicle respectively, θ is the heading angle of the vehicle and satisfies is the vehicle yaw angle, β is the vehicle's center of mass sideslip angle, is the vehicle yaw rate, Y is the vehicle lateral position in the geodetic coordinate system, m is the vehicle mass, I z is the vehicle's moment of inertia;

[0016] Step 1.2: Discretize the continuous state space model of formula (1.1) using the forward Euler method:

[0017]

[0018] Where A k =I m +TA,B k =TB,C k =[C 0 p×q ], q = m + n, m is the number of control quantities, n is the number of state quantities, p is the number of output quantities, and T is the sampling time;

[0019] Step 1.3: Rewrite the discrete state space in formula (1.2) as:

[0020]

[0021] In the formula Δu(k)=u(k)-u(k-1);

[0022] Step 1.4: Set the prediction time domain of the system to N p , the control time domain is N c , the state sequence in the prediction time domain is expressed in the following form:

[0023] ξ n (k+1)=A n ξ n (k)+B n Δu(k)

[0024]

[0025] Step 1.5: The output sequence in the prediction time domain is expressed as:

[0026]

[0027] Step 1.6: Combine the above state sequence and output sequence to obtain a new state space matrix:

[0028]

[0029] Where ξ0(k)=ξ n (k), ΔU0(k)=[Δu(k) Δu(k+1)···Δu(k+N c -1)] T

[0030]

[0031]

[0032] Step 1.7: Establish the objective function:

[0033]

[0034] Where: Δu k (k) = u(k) - u(k-1);

[0035] The first term of the objective function consists of two quantities, the lateral position error and the heading angle error in the global coordinate system; the second term is the increment of the control quantity, that is, the increment of the front wheel steering angle of the car; QQ, RR are weight matrices; constraints are imposed on the state quantity, control quantity and sideslip angle of the system:

[0036] y min ≤y(k)≤y max ,θ min ≤θ(k)≤θ max , Δu min≤Δu(k)≤Δu max ,|β(k)|<0.02μg

[0037] Where μ is the road adhesion coefficient, g is the acceleration of gravity;

[0038] Step 1.8: Rewrite the value function (1.7) as a standard quadratic programming problem:

[0039]

[0040] In the formula, ΔU(k)=[Δu(k), Δu(k+1),..., Δu(k+N c -1)],

[0041]

[0042] Step 1.9: In each optimization cycle, the optimization target is minJ, and the increment of the control variable in the control domain is obtained:

[0043] ΔU(k)=[Δu(k),Δu(k+1),...Δu(k+N c -1)] (1.9)

[0044] Step 1.10: Select the first element of the control sequence (1.9) as the actual control variable to act on the vehicle; the final target turning angle u(k) is:

[0045] u(k)=u(k-1)+Δu(k) (1.10).

[0046] In a preferred embodiment, step 2 specifically includes the following sub-steps:

[0047] Step 2.1: Establish a Simulink simulation model based on the path tracking control strategy described in step 1;

[0048] Step 2.2: Construct driving scenarios with different curvature radii and vehicle speeds as reference inputs for the Simulink simulation model. The mathematical model of the reference path is expressed in the following form:

[0049]

[0050] Where X is the horizontal coordinate in the global coordinate system, and L is the set straight path length;

[0051] Step 2.3: The reward function is defined as the negative of the weighted sum of the squares of the lateral position error and the heading position error within a certain time step ΔT, expressed as:

[0052]

[0053] Where a and b are weight coefficients, e1(t) is the lateral position error, and e2(t) is the heading error;

[0054] Step 2.4: With longitudinal speed v x and the road curvature radius R as the state value, reward as the reward function, and the prediction time domain N p and control time domain N c As the action output, the DQN agent is trained to optimize N p and N c .

[0055] In a preferred embodiment, step 3 specifically includes the following sub-steps:

[0056] Step 3.1: Obtain vehicle status information and upper-level path planning information;

[0057] Step 3.2: Calculate the position information of the preview point according to the preview distance; the preview distance is a variable related to the vehicle speed and satisfies the relationship L d =kv x +d;

[0058] Where L d is the preview distance, k is the preview distance coefficient, v x is the longitudinal speed, d is the initial preview distance;

[0059] Step 3.3: Calculate the minimum curvature radius R of the planned path within the preview distance min ;

[0060] Step 3.4: The reinforcement learning agent selects the corresponding action output according to the state value;

[0061] Step 3.5: Combine the vehicle status information and planned path information obtained in step 3.1 and the N obtained in step 3.4 p and N c Substitute the path tracking control strategy, solve the front wheel steering angle, and control the vehicle motion.

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

[0063] The problem that the lateral position error and heading error are difficult to converge to a smaller value at the same time under the traditional heading error definition is solved, and the path tracking accuracy is improved. In addition, this method can also adjust the prediction time domain and control time domain of the model predictive control algorithm in real time according to road conditions and vehicle speed information, thereby improving the environmental adaptability of intelligent vehicles to different driving scenarios. Compared with the MPC path tracking control method under the traditional heading error definition, this method has higher path tracking accuracy during the steering process; compared with the path tracking control method with fixed prediction time domain and control time domain. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 A simplified schematic diagram of a vehicle model used in a preferred embodiment of the present invention;

[0065] Figure 2 It is a schematic diagram of the unified heading angle and the actual heading angle;

[0066] Figure 3 A flowchart of optimizing the prediction time domain and the control time domain according to a preferred embodiment of the present invention;

[0067] Figure 4 This is a flow chart of the intelligent driving path tracking control method according to a preferred embodiment of the present invention. DETAILED DESCRIPTION

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

[0069] It should be noted that the following detailed descriptions are illustrative and are intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present application belongs.

[0070] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application; as used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or their combinations.

[0071] An environment-adaptive MPC path tracking control method based on a new heading error definition is proposed. This method establishes a vehicle state space model based on the new heading error definition and constructs a model predictive control MPC path tracking control strategy. The reinforcement learning algorithm DQN is used to optimize the prediction time domain and control time domain of the model predictive control MPC. Figures 1 to 4 , including the following steps:

[0072] Step 1: Establish a path tracking control strategy based on the model predictive control MPC algorithm. The steps include:

[0073] Step 1.1: Combine Figure 1 The single track model shown and Figure 2 The actual heading definition shown establishes the vehicle state space model:

[0074] in y=[Y θ] T , B=[2cf / m 0 2c f l f / Iz 0] T ,

[0075]

[0076] x is the vehicle's state matrix, u is the control variable, i.e., the front wheel steering angle δ, and y is the output matrix. A, B, and C are coefficient matrices. f and c r is the cornering stiffness of the front and rear wheels of the vehicle, l f and l r are the distances from the front and rear axles to the center of mass, v x and v y are the longitudinal and lateral velocities of the vehicle respectively, θ is the heading angle of the vehicle and satisfies is the vehicle yaw angle, β is the vehicle's center of mass sideslip angle, is the vehicle yaw rate, Y is the vehicle lateral position in the geodetic coordinate system, m is the vehicle mass, I z is the vehicle's moment of inertia.

[0077] Step 1.2: Discretize the continuous state space model using the forward Euler discretization method:

[0078]

[0079] Among them A k =I m +TA,B k =TB,C k =[C 0 p×q ], q=m+n, m is the number of controlled quantities, n is the number of state quantities, p is the number of output quantities, and T is the sampling time.

[0080] Step 1.3: Rewrite the state space model in step 1.2 as:

[0081]

[0082] in Δu(k)=u(k)-u(k-1), C n =[C k 0].

[0083] Step 1.4: Set the prediction time domain of the system to N p , the control time domain is N c , then the state sequence in the prediction time domain can be expressed in the following form:

[0084] ξ n (k+1)=A nξ n (k)+B n Δu(k)

[0085]

[0086] Step 1.5: The output sequence in the prediction time domain can be expressed as:

[0087]

[0088] Step 1.6: Combine the state sequence and output sequence in step 1.5 to get a new set of state space matrices:

[0089]

[0090] where ξ0(k)=ξ n (k)ΔU0(k)=[Δu(k)Δu(k+1)···Δu(k+N c -1)] T

[0091]

[0092]

[0093] Step 1.7: Establish the objective function:

[0094]

[0095] in Δu k (k) = u(k) - u(k-1);

[0096] Step 1.8: The first term of the objective function consists of two quantities, the lateral position and yaw angle in the geodetic coordinate system. This term mainly reflects the accuracy of the vehicle's trajectory tracking. The second term is the increment of the control quantity, that is, the increment of the front wheel angle of the vehicle. This term reflects the stability of the vehicle's path tracking. QQ and RR are weight matrices. At the same time, considering the actual vehicle characteristics, it is also necessary to impose certain constraints on the system's state quantity and control quantity:

[0097] y min ≤y(k)≤y max ,θ min ≤θ(k)≤θ max , Δu min ≤Δu(k)≤Δu max ,|β(k)|<0.02μg

[0098] Where μ is the road adhesion coefficient and g is the acceleration due to gravity.

[0099] Step 1.9: Rewrite the value function as a standard quadratic programming problem:

[0100]

[0101] Where ΔU=[Δu(k),Δu(k+1),…,Δu(k+N c -1)],

[0102]

[0103] Step 1.10: In each optimization cycle, the optimization target is minJ, and the incremental sequence of the control variables in the control time domain is obtained:

[0104] ΔU=[Δu(k),Δu(k+1),…,Δu(k+N c -1)]

[0105] Step 1.11: Take the first element of the incremental sequence in step 1.10 as the actual control variable to act on the vehicle. The final target turning angle u(k) is: u(k) = u(k-1) + Δu(k).

[0106] Step 2: Use reinforcement learning to establish a mapping relationship between the prediction time domain and the control time domain and the vehicle speed and the road curvature radius to achieve adaptive adjustment of the prediction time domain and the control time domain. The specific steps include:

[0107] Step 2.1: Establish a Simulink model based on the path tracking control strategy described in step 1. In addition, in order to ensure the reliability of the simulation results, the object of the path tracking control strategy adopts a high-precision Carsim vehicle model;

[0108] Step 2.2: According to the national highway design specification JTG D20-2017, construct driving scenarios with different curvature radii and vehicle speeds (see Table 1) as reference inputs for the Simulink simulation model. The mathematical model of the reference path can be expressed as follows:

[0109]

[0110] Where X is the horizontal coordinate in the global coordinate system, and L is the set straight path length;

[0111] The reward function is defined as the negative of the weighted sum of the squares of the lateral position error and the heading position error within a certain time step ΔT, expressed as:

[0112]

[0113] Where a and b are weight coefficients, e1(t) is the lateral position error, and e2(t) is the heading error;

[0114] Step 2.3: Use the actual longitudinal speed V x and the road curvature radius R are state values, the prediction time domain and the control time domain are taken as action outputs, and the DQN agent is trained to optimize the prediction time domain and the control time domain.

[0115] Step 3: The agent adjusts the state value (V x ,R) select the corresponding action output (N p ,N c ), and then use the path tracking control strategy described in step 1 to calculate the front wheel steering angle to act on the wheel to control the vehicle movement. The steps include:

[0116] Step 3.1: Obtain vehicle status information and upper-level path planning information;

[0117] Step 3.2: Calculate the location information of the preview point based on the preview distance. The preview distance is a variable related to the vehicle speed and satisfies the relationship L d =kv+d;

[0118] Step 3.3: Calculate the minimum curvature radius R of the planned path within the preview distance min ;

[0119] Step 3.4: Reinforce the learning agent according to the state quantity (v x ,R min ) Select the corresponding action output (N p ,N c );

[0120] Step 3.5: Combine the vehicle status information, the planned path information and the N obtained in step 3.4 p and N c Substitute the path tracking control strategy, solve the front wheel steering angle, and control the vehicle motion.

[0121] Table 1 National Highway Design Specification JTG D20-2017 Minimum Radius of Circular Curve

[0122]

Claims

1. The environment-adaptive MPC path tracking control method based on the new heading error definition is characterized by: This method establishes a vehicle state space model based on a new heading error definition and constructs a model predictive control (MPC) path tracking control strategy. It also uses the reinforcement learning algorithm DQN to optimize the prediction time domain and control time domain of the model predictive control (MPC). The following steps are involved: Step 1: Establish a path tracking control strategy based on the model predictive control MPC algorithm, which includes the following sub-steps; Step 2: Use reinforcement learning optimization to obtain the mapping relationship between the prediction time domain and the control time domain and the curvature radius and vehicle speed; Step 3: The trained agent selects the corresponding action output according to different state values, and then uses the path tracking control strategy in step 1 to calculate the front wheel angle to act on the wheel to control the vehicle movement; Step 1.1: Establish the vehicle state space model based on the new heading error definition: where y = [Y θ] T , B = [2c f / m 0 2c f l f / Iz 0] T , x is the state matrix of the vehicle, u is the control variable, i.e. the front wheel steering angle δ, y is the output matrix; A, B, C are coefficient matrices; c f and c r is the cornering stiffness of the front and rear wheels of the vehicle, l f and l r are the distances from the front and rear axles to the center of mass, v x and v y are the longitudinal and lateral velocities of the vehicle respectively, θ is the heading angle of the vehicle and satisfies is the vehicle yaw angle, β is the vehicle's center of mass sideslip angle, is the vehicle yaw rate, Y is the vehicle lateral position in the geodetic coordinate system, m is the vehicle mass, I z is the vehicle's moment of inertia; Step 1.2: Discretize the continuous state space model of formula (1.1) using the forward Euler method: Where A k =I m +TA,B k =TB,C k =[C 0 p×q ], q = m'+n, m' is the number of control quantities, n is the number of state quantities, p is the number of output quantities, and T is the sampling time; Step 1.3: Rewrite the discrete state space in formula (1.2) as: In the formula C n =[C k 0], Δu(k)=u(k)-u(k-1); Step 1.4: Set the prediction time domain of the system to N p , the control time domain is N c , the state sequence in the prediction time domain is expressed in the following form: Step 1.5: The output sequence in the prediction time domain is expressed as: Step 1.6: Combine the above state sequence and output sequence to obtain a new state space matrix: where ξ0(k) = ξ n (k), ΔU0(k) = [Δu(k) Δu(k + 1) ··· Δu(k + N c - 1)] T Step 1.7: Establish the objective function: Where: Δu k (k) = u(k) - u(k-1); The first term of the objective function consists of two quantities, the lateral position error and the heading angle error in the global coordinate system; the second term is the increment of the control quantity, that is, the increment of the front wheel steering angle of the car; QQ, RR are weight matrices; constraints are imposed on the state quantity, control quantity and sideslip angle of the system: y min ≤y(k)≤y max ,the min ≤θ(k)≤θ max ,I don't min ≤Δu(k)≤Δu max ,|β(k)|<0.02μg Where μ is the road adhesion coefficient, g is the acceleration of gravity; Step 1.8: Rewrite the value function (1.7) as a standard quadratic programming problem: In the formula, Step 1.9: In each optimization cycle, the optimization target is minJ, and the increment of the control variable in the control domain is obtained: ΔU(k)=[Δu(k),Δu(k+1),...Δu(k+N c -1)] (1.9) Step 1.10: Select the first element of the control sequence (1.9) as the actual control variable to act on the vehicle; the final target turning angle u(k) is: u(k)=u(k-1)+Δu(k) (1.10) The step 2 specifically includes the following sub-steps: Step 2.1: Establish a Simulink simulation model based on the path tracking control strategy described in step 1; Step 2.2: Construct driving scenarios with different curvature radii and vehicle speeds as reference inputs for the Simulink simulation model. The mathematical model of the reference path is expressed in the following form: Where X is the horizontal coordinate in the global coordinate system, and L is the set straight path length; Step 2.3: The reward function is defined as the negative of the weighted sum of the squares of the lateral position error and the heading position error within a certain time step ΔT, expressed as: Where a and b are weight coefficients, e1(t) is the lateral position error, and e2(t) is the heading error; Step 2.4: With longitudinal speed v x and the road curvature radius R as the state value, reward as the reward function, and the prediction time domain N p and control time domain N c As the action output, the DQN agent is trained to optimize N p and N c ; The step 3 specifically includes the following sub-steps: Step 3.1: Obtain vehicle status information and upper-level path planning information; Step 3.2: Calculate the position information of the preview point according to the preview distance; the preview distance is a variable related to the vehicle speed and satisfies the relationship L d =kv x +d; Where L d is the preview distance, k is the preview distance coefficient, v x is the longitudinal speed, d is the initial preview distance; Step 3.3: Calculate the minimum curvature radius R of the planned path within the preview distance min ; Step 3.4: The reinforcement learning agent selects the corresponding action output according to the state value; Step 3.5: Combine the vehicle status information and planned path information obtained in step 3.1 and the N obtained in step 3.4 p and N c Substitute the path tracking control strategy, solve the front wheel steering angle, and control the vehicle motion.

Images