Path tracking model predictive control method for controlling weighted summation of sequences

Abstract

The invention relates to a path tracking model predictive control method for control sequence weighted summation, and belongs to the field of automatic driving vehicle control. Aiming at the problems that future dynamic information of a vehicle is not fully utilized and the control precision is limited due to the fact that only a first element of a control sequence is adopted in traditional model predictive control, the method comprises the steps that a vehicle path tracking dynamics model is established and discretized; designing a cost function and constraint to construct a quadratic programming problem, and solving an optimal control sequence; a weight distribution mechanism is designed to carry out weighted summation on the control sequence to serve as actual control input, wherein the weight is attenuated according to time so as to strengthen the control quantity weight at the current moment; according to the method, a traditional rolling optimization framework is broken through, the control precision and the system robustness are remarkably improved by fusing multi-step prediction information, meanwhile, the algorithm compatibility is kept, and the stability and safety problems of path tracking under complex working conditions are effectively solved.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous driving vehicle control and relates to a path tracking model predictive control method based on weighted summation of control sequences. Background Art

[0002] Autonomous driving technology has become a core research area in academia and industry due to its significant potential to reduce traffic accidents, ease road congestion, and promote economic development. Path tracking, a key functional module of autonomous driving systems, is relied upon by both traditional modular architectures and end-to-end solutions to achieve vehicle trajectory control. To achieve high-level fully autonomous driving, vehicles must maintain high-precision path tracking capabilities in a variety of complex scenarios. Therefore, developing high-performance path tracking control algorithms is crucial to promoting the implementation of autonomous driving technology.

[0003] Current path tracking control methods mainly include geometric control, PID control, sliding mode control, H ∞ Methods include control, linear quadratic regulator (LQR), and model predictive control (MPC). MPC can use vehicle dynamics models to predict future dynamics and improve control accuracy by leveraging spatiotemporal information. Furthermore, MPC has the unique advantages of handling multiple inputs and multiple outputs and considering system constraints, making it the most effective and commonly used method in path tracking.

[0004] The basic principles of MPC include three parts: prediction model, rolling optimization, and feedback correction. Among them, rolling optimization is what distinguishes MPC from other control methods. Rolling optimization means that at each sampling moment, MPC constructs an optimization problem based on current information and solves the control sequence from the current moment to a certain moment in the future. Only the first variable in the control sequence is input into the controlled object. At the next moment, a new control sequence is solved and the first variable in the control sequence is input again, thus achieving online optimization control. In path tracking applications, due to the complex and changing environment, the predicted dynamic information of the vehicle is critical for timely adjustment of the planned path and control objectives to ensure driving safety. However, the traditional MPC design only uses the first variable in the control sequence at each sampling moment, so that other control input information obtained based on the future predicted vehicle dynamic information is completely discarded, resulting in insufficient utilization of the model prediction information and a decrease in control accuracy.

[0005] In summary, existing model predictive control methods are limited by the traditional rolling optimization framework and cannot fully utilize the predicted information of the vehicle's future dynamics, which restricts their performance breakthroughs in autonomous driving path tracking scenarios. Summary of the Invention

[0006] In view of this, the purpose of the present invention is to provide a path tracking model predictive control method based on weighted summation of control sequences. After solving the control sequence, the obtained control sequences are weighted and added up as the control input at the current moment, thereby improving the traditional rolling optimization framework that only uses the first control quantity, so that the vehicle model prediction information can be fully utilized.

[0007] In order to achieve the above object, the present invention provides the following technical solutions:

[0008] A path tracking model predictive control method based on weighted summation of control sequences comprises the following steps:

[0009] S1: Establish path-following dynamics model;

[0010] S2: Add curvature steady-state error compensation and establish the standard state space equations;

[0011] S3: Design the cost function and system constraints and formulate the quadratic programming problem;

[0012] S4: Design a control sequence weight distribution mechanism;

[0013] S5: Perform weighted summation on the control sequence obtained at the current sampling moment and use it as the control input.

[0014] Furthermore, the path tracking dynamics model described in S1 is as follows:

[0015]

[0016] Where, e y is the lateral position error, is the heading angle error, u c =δ, δ is the front wheel steering angle, κ is the road curvature,

[0017]

[0018] Where m is the vehicle mass, v x is the vehicle longitudinal velocity, I z is the vehicle's moment of inertia around the Z axis, C f 、C r are the front and rear vehicle cornering stiffness, l f 、l r are the distances from the vehicle's center of mass to the front and rear axles, respectively.

[0019] Furthermore, in S2, due to the existence of the curvature term D in formula (1) cκ will make the control strategy design process very complicated. In order to eliminate the influence of the curvature term and ensure the simplicity of the system model, the curvature steady-state error compensation method is used to eliminate the curvature term. According to the limit value theorem, the steady-state error value that needs to be compensated is:

[0020]

[0021] Where G s is the understeering gradient:

[0022]

[0023] By adding the compensation of formula (2) to formula (1), the curvature term can be eliminated:

[0024]

[0025] Where x = x c -x ss , u=u c -u ss .

[0026] By discretizing Equation (3) using the first-order forward Euler method, we can obtain the standard discrete state space equation:

[0027] x(k+1)=Ax(k)+Bu(k) (4)

[0028] Where A = T s ·A c +I 4×4 , B=T s ·B c , T s is the sampling time, I 4×4 is the identity matrix of dimension 4×4.

[0029] Furthermore, the cost function in S3 is:

[0030]

[0031] Where Np is the prediction step size, Nc is the control step size, Q is the state weight matrix, R is the control weight matrix, and x r is the expected path. In the present invention, since the state quantity is selected as the tracking error related quantity, x r =[0 0 0 0] T , x(i|k) represents the state value predicted i steps ahead at the current moment k, and u(i-1|k) represents the control value predicted i-1 steps ahead at the current moment k.

[0032] In order to make the derivation of the formula more concise, transform (5) into augmented vector form:

[0033] J=X T QX+U T RU (6)

[0034] Where,

[0035] X=[x(1|k) x(2|k) ... x(Np|k)] T

[0036] U=[u(0|k) u(1|k) ... u(Nc-1|k)] T

[0037]

[0038] In vehicle path tracking, the actuator is the front wheel angle, so the actual physical constraints of the steering actuator need to be considered:

[0039]

[0040] Where, u is the lower limit of the actuator, is the upper limit of the actuator.

[0041] Combining equations (4) to (7), the quadratic programming optimization problem of the system can be obtained as follows:

[0042]

[0043] Solving equation (8) can obtain the required optimal control sequence U*.

[0044] U*=[u*(0)u*(1)...u*(Nc-1)] T

[0045] Furthermore, in S4, weights are allocated for the Nc control variables in the obtained optimal control sequence U*, so as to more fully utilize the prediction information of the vehicle dynamics model.

[0046] In path tracking applications, safety is the primary goal of the system. Therefore, the control value at the current moment is the most important to the system, while the influence of subsequent control values ​​decreases over time. Based on this principle, the following weight distribution mechanism is designed:

[0047] For the control quantity u*(i) in the control sequence, its corresponding weight is:

[0048]

[0049] By using formula (9) to assign weights to the control quantities in the control sequence, it is possible to achieve the effect that the closer to the current moment, the greater the influence of the control quantity.

[0050] Furthermore, in S5, weighted summation is performed based on the assigned weights to finally obtain the control amount at the current moment:

[0051] u*=w0u*(0)+w1u*(1)+…+w Nc-1 u*(Nc-1) (10)

[0052] The beneficial effects of the present invention are:

[0053] (1) By designing a weighted summation mechanism for control sequences, the traditional model predictive control, which only uses the first element of the control sequence, is completely changed. The predicted control quantity for the future period is integrated into the current moment input, achieving a step-change improvement in the utilization of the prediction information of the vehicle dynamics model and overcoming the technical defect of limited control accuracy due to information discarding.

[0054] (2) Based on the core safety requirements of the path tracking scenario, a distribution mechanism is established in which the current control value has the highest weight and the weight decreases in the future time periods. While improving the utilization rate of prediction information, it ensures that the temporal importance distribution of control decisions meets the safety boundary requirements of the autonomous driving system.

[0055] (3) The weighted summation structure enables the control variable to include multi-step predicted dynamic information, which is equivalent to injecting forward compensation capability into the control system. Under non-steady-state conditions such as sudden changes in road curvature and changes in adhesion coefficient, it can significantly suppress overshoot and oscillation of trajectory tracking, thereby improving the overall robustness of the system.

[0056] (4) The innovative solution is implemented on the basis of retaining the three core modules of traditional model predictive control: predictive modeling, constraint processing, and rolling optimization. It not only achieves a breakthrough in control performance, but also ensures seamless compatibility with the existing autonomous driving system architecture, reducing the cost of project implementation.

[0057] (5) Through curvature steady-state error compensation and discretization modeling technology, road curvature interference is eliminated and the controller is digitally deployed. The theoretical optimal control is converted into an executable control strategy to resolve the contradiction between the strong nonlinear characteristics of vehicle dynamics and the real-time control requirements.

[0058] Other advantages, objects, and features of the present invention will be described in part in the following description and, in part, will be apparent to those skilled in the art upon examination of the following description or may be learned from practice of the present invention. The objects and other advantages of the present invention may be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below with reference to the accompanying drawings, in which:

[0060] Figure 1 It is a two-degree-of-freedom vehicle dynamics model diagram;

[0061] Figure 2 This is the path tracking kinematic model diagram;

[0062] Figure 3 Flowchart for the implementation of the algorithm of the present invention;

[0063] Figure 4 This is a global coordinate result diagram of the simulation of the path tracking control method of the present invention;

[0064] Figure 5 This is a diagram showing the lateral position error simulation results of the path tracking control method of the present invention. DETAILED DESCRIPTION

[0065] The following describes the embodiments of the present invention by means of specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic illustrations of the basic concept of the present invention, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0066] Among them, the accompanying drawings are only for illustrative purposes and represent only schematic diagrams rather than actual pictures, and should not be understood as limiting the present invention. In order to better illustrate the embodiments of the present invention, some parts of the accompanying drawings may be omitted, enlarged or reduced, and do not represent the dimensions of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions may be omitted in the accompanying drawings.

[0067] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if there are terms such as "upper", "lower", "left", "right", "front", "back", etc. indicating directions or positional relationships, they are based on the directions or positional relationships shown in the drawings. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operate in a specific direction. Therefore, the terms describing the positional relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0068] See also Figures 1 to 5The overall idea of ​​the present invention is: under the MPC framework, after constructing the system's quadratic programming problem based on the dynamic model, weights are assigned to the solved control sequence, and the control quantity obtained after weighted summation is used as the control input, so as to make full use of the model prediction information and improve the control accuracy.

[0069] according to Figure 3 The specific steps of this algorithm are as follows:

[0070] (1) Establishment of system dynamics model. First, establish the vehicle two-degree-of-freedom dynamics model, such as Figure 1 As shown, according to Newton's second law:

[0071]

[0072] Where m is the vehicle mass, v y 、v x are the vehicle's lateral and longitudinal speeds, I z is the vehicle's moment of inertia around the Z axis, is the vehicle yaw angle, l f 、l r are the distances from the vehicle's center of mass to the front and rear axles, F yf 、F yr are the lateral forces on the front and rear tires of the vehicle, respectively.

[0073] In the vehicle two-degree-of-freedom dynamics model, the tire lateral force is expressed in a linear form:

[0074]

[0075] Where C f 、C r are the front and rear tire cornering stiffness, α f , α r They are the front and rear tire slip angles, respectively. The tire slip angles are calculated using the following formula:

[0076]

[0077] Path tracking kinematic model such as Figure 2 As shown. The vehicle's heading angle error Defined as the difference between the vehicle's yaw angle and the desired yaw angle:

[0078]

[0079] Where, is the desired yaw angle.

[0080] First-order derivative of heading angle error for:

[0081]

[0082] Where κ is the road curvature.

[0083] Lateral displacement error e y It is defined as the distance from the center of mass of the vehicle to the nearest point on the preset path, which can be obtained from the kinematic relationship:

[0084]

[0085] For formula (16) in e y = 0, we can obtain the following linearization:

[0086]

[0087] Calculate the second-order derivative of the heading angle error and the yaw angle error:

[0088]

[0089] (2) Establishment of standard state space equations.

[0090] Combining equations (11) to (18), the system differential equation is expressed as a state space equation, and the state quantity is selected as Control quantity u=δ, and the equation can be obtained:

[0091]

[0092] Where,

[0093]

[0094] The curvature term D in formula (19) c κ will make the controller design process very complicated. To ensure the simplicity of the system state space equation and to ensure that the curvature information is not lost, the present invention adopts the steady-state error compensation method to eliminate the curvature term. The compensation value obtained by the limit value theorem is:

[0095]

[0096] Where G s is the understeering gradient,

[0097]

[0098] The curvature term can be eliminated by adding compensation as shown in formula (20) to the state quantity and control quantity respectively:

[0099]

[0100] Where x = x c -x ss , u=uc -u ss .

[0101] The first-order forward Euler method is used to discretize Equation (21), and the standard state space equation is obtained as follows:

[0102] x(k+1)=Ax(k)+Bu(k) (22)

[0103] Where A = A c ΔT+I 4×4 , B=B c ΔT, ΔT is the sampling time.

[0104] (3) Construction of cost function and constraints

[0105] The present invention adopts the classical cost function construction, whose goal is to minimize the tracking error and control input:

[0106]

[0107] To ensure the simplicity of subsequent derivation, Equation (23) is rewritten into augmented vector form:

[0108] J=X T QX+U T RU (24)

[0109] Where,

[0110] X=[x(1|k) x(2|k) ... x(Np|k)] T

[0111] U=[u(0|k) u(1|k) ... u(Nc-1|k)] T

[0112]

[0113] Considering the physical limitations of the steering actuator in the actual vehicle system, the constraints of the control quantity can be obtained as follows:

[0114]

[0115] Where u is the physical lower limit of the actual steering actuator, It is the physical upper limit of the actual steering actuator.

[0116] (4) Establishing the Quadratic Programming Problem

[0117] Combining equations (22) to (25), the system quadratic programming optimization problem can be constructed as follows:

[0118]

[0119] Solving equation (26) can obtain the optimal control sequence U* at the current sampling time.

[0120] U*=[u*(0)u*(1)...u*(Nc-1)] T

[0121] (5) Weight allocation algorithm design

[0122] In order to make full use of the optimal control sequence obtained based on the model prediction information, the control quantities in the optimal control sequence are weighted.

[0123] Safety is the primary goal of path tracking, so vehicle tracking accuracy at the current moment is more important than tracking accuracy at future moments. Based on this principle, the current control variable is given the highest weight, while the influence of subsequent control variables decreases over time. (Subsequent control variables are calculated to achieve global optimality, which may sacrifice local performance at the current moment, so their influence needs to be weakened compared to the current control variable.)

[0124] In summary, the following weight distribution mechanism is designed for the control quantity u*(i) in the control sequence U*:

[0125]

[0126] (6) Weighted summation of control quantities

[0127] Obtain the optimal control sequence U* and the weight w corresponding to the control amount in the optimal control sequence i Then perform weighted summation and use it as the control input at the current moment.

[0128] u*=w1u*(1)+w2u*(2)+…+w Nc-1 u*(Nc-1) (28)

[0129] To more effectively illustrate the effectiveness of the present invention, we compare it with the traditional rolling optimization MPC method. In the traditional MPC method, after obtaining the optimal control sequence U*, the control input at the current moment is:

[0130] u*=u*(0) (29)

[0131] The two methods were simulated on the CarSim-Simulink joint simulation platform, with the simulation conditions set to 72km / h, road adhesion coefficient 0.75, and double lane shift. The global coordinate diagram and lateral position error diagram obtained by simulation are shown in the figure below. Figure 4 and Figure 5As shown in the figure, it can be seen from the data in the figure that the MPC method proposed in the present invention can improve the vehicle tracking accuracy based on the full use of the model prediction information.

[0132] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solutions, which should all be included in the scope of the claims of the present invention.

Claims

1. A path tracking model predictive control method based on weighted summation of control sequences, characterized by: The following steps are involved: S1: Establish a dynamic model for vehicle path tracking; S2: constructing discrete state space equations based on the dynamic model; S3: Design a cost function that includes tracking error and control input, and construct a quadratic programming problem based on system constraints to solve the optimal control sequence U*=[u*(0)u*(1)...u*(Nc-1)] at the current sampling time. T , where Nc is the control step size; S4: Assign weight w to each control variable u*(i) in the optimal control sequence U* i , the weight distribution satisfies that the control amount weight is the highest at the current moment and the weight decreases over time; S5: The weighted summation result As the actual control input at the current moment.

2. The path tracking model predictive control method based on weighted summation of control sequences according to claim 1, characterized in that: In S1, the kinetic model is: in, e y is the lateral position error, is the heading angle error, the control quantity u c =δ, δ is the front wheel steering angle, κ is the road curvature, matrix A c 、B c 、D c Determined by vehicle parameters, including vehicle mass m, longitudinal velocity v x , moment of inertia I z , front wheel cornering stiffness C f , rear wheel cornering stiffness C r , distance from center of mass to front axle l f , distance from center of mass to rear axle l r .

3. The path tracking model predictive control method based on weighted summation of control sequences according to claim 2, characterized in that: The S2 includes curvature steady-state error compensation: Compensation The insufficient turning gradient The state space equation after eliminating the curvature term is where x = x c -x ss , u=u c -u ss ; Using the first-order forward Euler method to discretize, we get x(k+1)=Ax(k)+Bu(k), where A=T s ·A c +I 4×4 , B=T s ·B c , T s is the sampling time.

4. The path tracking model predictive control method based on weighted summation of control sequences according to claim 1, characterized in that: The weight distribution formula in S4 is:

5. The path tracking model predictive control method based on weighted summation of control sequences according to claim 1, characterized in that: In S3, the system constraint is the physical limit of the front wheel angle: Among them, u, They are the lower and upper limits of the steering actuator respectively.

6. The path tracking model predictive control method based on weighted summation of control sequences according to claim 2, characterized in that: The matrix expression of the kinetic model is:

7. The path tracking model predictive control method based on weighted summation of control sequences according to claim 3, characterized in that: The identity matrix I in the discretization 4×4 It is 4×4 dimension.

8. The path tracking model predictive control method based on weighted summation of control sequences according to claim 1, characterized in that: In S3, the cost function is: Among them, Np is the prediction step size, Q is the state quantity weight matrix, R is the control quantity weight matrix, x r is the expected path, x r =[00 0 0] T .

9. A path tracking control system, characterized in that: The method comprises a processor and a memory, wherein the memory stores a computer program, and when the program is executed, the steps of the method according to any one of claims 1 to 8 are implemented.

Images