Path tracking model prediction control method based on preview curvature

Abstract

The invention relates to a path tracking model prediction control method based on preview curvature, and belongs to the technical field of automatic driving vehicle motion control, and the method comprises the following steps: S1, building a vehicle dynamics model; s2, establishing a path tracking model; s3, the road curvature sequence is augmented as a control quantity extension item, the control quantity extension item and an original control input front wheel steering angle are combined into an augmented control vector, and the path tracking model is reconstructed to obtain a standard state equation; and S4, designing a cost function and a constraint condition, taking a curvature item as a control quantity extension item, and performing optimization solution on the augmented path tracking model. According to the method, the tracking precision and the steering smoothness in the curve scene are remarkably improved, the calculation dimension of the matrix is remarkably reduced, and the calculation real-time performance is remarkably improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of motion control of autonomous vehicles, and relates to a path tracking model predictive control method based on preview curvature. Background Art

[0002] With the rapid development of autonomous driving technology, path tracking, as the core link of vehicle motion control, directly determines driving safety and occupant comfort. Currently, the main autonomous driving path tracking methods include geometric control based on vehicle kinematics, feedback control based on dynamic models, and model predictive control (MPC), etc. Each method has its own advantages and disadvantages in dynamic disturbance suppression and real-time performance. Geometric control methods (such as Pure Pursuit, Stanley) generate steering commands through simplified kinematic models and geometric relationships. Although they have high computational efficiency, they ignore dynamic characteristics such as tire side slip and yaw inertia. In high-speed or large-curvature scenarios, the tracking error increases significantly, and they cannot fuse road curvature information for feedforward compensation. They only rely on the current path geometric parameters, and the dynamic disturbance suppression ability is weak. Traditional feedback control methods (such as Linear Quadratic Regulator, LQR) design state feedback controllers based on dynamic models. Although they can partially improve dynamic performance, they still regard road curvature as an external disturbance and do not explicitly incorporate it into the control law design, resulting in a lag in the controller's response to changes in bend curvature. Especially in continuous bend scenarios, overshoot and oscillation are likely to occur due to the lack of preview ability. In addition, although advanced methods such as adaptive control and sliding mode control improve the tolerance to model uncertainties through parameter adaptation or robustness design, they still mainly rely on feedback compensation and lack a feedforward fusion mechanism for known road curvature, making it difficult to actively suppress dynamic disturbances.

[0003] As the current mainstream method, MPC can explicitly handle future disturbances and multi-objective constraints through rolling horizon optimization, and can fuse the road curvature sequence to achieve precise feedforward compensation, significantly improving the bend tracking accuracy. However, its real-time bottleneck severely restricts engineering applications: MPC needs to iteratively solve optimization problems (such as quadratic programming) online, and the computational complexity increases exponentially with the prediction horizon and model degrees of freedom. The single-step solution time often exceeds the response requirements of in-vehicle controllers. To meet real-time requirements, it often relies on high-performance computing units (such as GPU / FPGA) for acceleration, resulting in a significant increase in system cost and power consumption, and it is difficult to adapt to in-vehicle computing platforms with limited computing power. Although other methods try to reduce the computational load by simplifying the model or sacrificing prediction ability, it is difficult to balance the control accuracy and real-time requirements. In summary, the existing technologies have not effectively solved the dual contradictions of insufficient utilization of road curvature information and poor real-time performance of predictive control, restricting the large-scale implementation of autonomous driving systems. Summary of the Invention

[0004] In view of this, the purpose of the present invention is to provide a path tracking model predictive control method based on preview curvature under the MPC framework. Under the MPC framework, the future road curvature information is used as an extended term of the control quantity rather than an augmented term of the state quantity, and the computational complexity is significantly reduced by reconstructing the optimization problem.

[0005] To achieve the above object, the present invention provides the following technical solutions:

[0006] A path tracking model predictive control method based on preview curvature includes the following steps:

[0007] S1: Establish a vehicle dynamics model;

[0008] S2: Establish a path tracking model;

[0009] S3: Augment the road curvature sequence as an extended term of the control quantity, merge it with the original control input front wheel steering angle to form an augmented control vector, and reconstruct the path tracking model to obtain a standard state equation;

[0010] S4: Design a cost function and constraint conditions, use the curvature term as an extended term of the control quantity, and optimize and solve the augmented path tracking model.

[0011] Further, the vehicle dynamics model described in step S1 is as follows:

[0012]

[0013] In the formula, m represents the vehicle mass, v y , v x represent the lateral speed and longitudinal speed respectively, represents the vehicle yaw angle, I z represents the moment of inertia of the vehicle about the Z axis, l f , l r represent the distances from the front and rear axles to the vehicle center of mass respectively, F yf , F yr represent the lateral forces of the front and rear wheels respectively.

[0014] Further, the path tracking model described in step S2 is:

[0015] x(k + 1) = Ax(k) + Bu(k) + Dc R (k)

[0016] In the formula, k represents the current time, represents the state quantity, e y represents the lateral displacement error, is the first derivative of e y , represents the yaw angle error, is The first derivative; u = δ represents the control variable, δ represents the front wheel steering angle of the vehicle, and c R represents the road curvature; A = ΔT·A c +I, B = ΔT·B c , D = ΔT·D c ; ΔT represents the sampling time, and I represents the identity matrix;

[0017]

[0018]

[0019] C f represents the front wheel cornering stiffness, and C r represents the rear wheel cornering stiffness, m represents the vehicle mass, and l f represents the distance from the vehicle's center of mass to the front axle, and l r represents the distance from the vehicle's center of mass to the rear axle.

[0020] Furthermore, in step S3, the known road curvature sequence c R (k) is used as the control variable extension term and merged with the original control input, the front wheel steering angle δ, into an augmented control vector:

[0021]

[0022] Furthermore, the cost function described in step S4 includes two parts: the tracking error and the steering input:

[0023] J = X T QX + U T RU

[0024] where J represents the cost function, X represents the state quantity matrix, X T represents the transpose of the state quantity matrix, Q represents the state weight matrix, R represents the control weight matrix, U represents the control sequence, and U T represents the transpose of the control sequence;

[0025] X = [x(1), x(2), …, x(N p )] T

[0026] U = [u(0), u(1), …, u(N c -1)] T

[0027] Furthermore, the constraint conditions described in step S4 include: the following constraints are imposed on the steering angle:

[0028] u min (k + i) ≤ u(k + i) ≤ u max(k + i), where i = 1, 2..., N - 1

[0029] Δu min (k + i) ≤ Δu(k + i) ≤ Δu max (k + i), where i = 1, 2..., N - 1

[0030] u min (k + i) represents the minimum value of the control quantity at time k + i, u(k + i) represents the control quantity at time k + i, and u max (k + i) represents the maximum value of the control quantity at time k + i, Δu min (k + i) represents the minimum value of the control increment at time k + i, Δu(k + i) represents the control increment at time k + i, and Δu max (k + i) represents the maximum value of the control increment at time k + i.

[0031] The beneficial effects of the present invention are as follows: Sufficient utilization of curvature information: By utilizing the rolling prediction characteristic of MPC, the known future road curvature sequence is directly embedded into the system model as a control parameter, breaking through the limitation of traditional methods that ignore curvature information or can only utilize single - moment curvature information, and significantly improving the tracking accuracy and steering smoothness in curve scenarios. Control problem dimension reconstruction strategy: Different from traditional state augmentation methods, in this patent, the curvature sequence is used as an extended item of the control quantity and merged with the original steering control input to form an augmented control vector. While maintaining the simplicity of the state model, the complexity of the optimization problem is reduced to the same order as that of a single control quantity in the MPC framework, significantly reducing the computational dimension of the matrix and significantly improving the computational real - time performance.

[0032] Other advantages, objectives, and features of the present invention will be described to some extent in the subsequent description, and to some extent, will be obvious to those skilled in the art based on the study of the following text, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be achieved and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] In order to make the objectives, technical solutions, and advantages of the present invention clearer, the present invention will be described in detail preferably with reference to the accompanying drawings, where:

[0034] Figure 1 is a vehicle dynamics model diagram;

[0035] Figure 2 is a tracking error model diagram;

[0036] Figure 3 is a flowchart of the path - tracking model predictive control method based on preview curvature according to the present invention;

[0037] Figure 4This is a simulation result diagram of the path tracking controller of the present invention;

[0038] Figure 5 , 6 This is a comparison chart of the calculation efficiency of the present invention and the state quantity augmentation calculation efficiency. DETAILED DESCRIPTION

[0039] The following describes the embodiments of the present invention by 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 only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0040] 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 thus the drawings only show components related to the present invention rather than being drawn according to the number, shape and size of components in actual implementation. In actual implementation, the type, quantity and proportion of each component may be changed arbitrarily, and the component layout may also be more complicated.

[0041] In the following description, numerous details are discussed to provide a more thorough explanation of the embodiments of the present invention. However, it is obvious to those skilled in the art that the embodiments of the present invention can be implemented without these specific details. In other embodiments, well-known structures and devices are shown in the form of block diagrams rather than in detail to avoid making the embodiments of the present invention difficult to understand.

[0042] Embodiment 1:

[0043] The general idea of ​​the present invention is to add the road curvature to the control quantity under the path tracking control algorithm using the MPC framework, so that the known road curvature information can be used and the matrix dimension can be reduced, and finally the path tracking accuracy and calculation real-time performance can be taken into account. Figure 2 The algorithm system flow chart, the specific implementation steps of this algorithm are as follows:

[0044] (1) Establishing a vehicle dynamics model. The tracking error model is based on the commonly used vehicle two-degree-of-freedom dynamics model. The vehicle dynamics model is as follows: Figure 1 As shown:

[0045]

[0046] Where m is the vehicle mass, v y 、vx They are the lateral velocity and the longitudinal velocity, respectively, is the vehicle yaw angle, I z is the moment of inertia of the vehicle about the Z-axis, l f 、l r are the distances from the front and rear axles to the vehicle's center of mass, F yf 、F yr are the lateral forces of the front and rear wheels, respectively.

[0047] (2) Establish a path tracking model. The schematic diagram of the vehicle path tracking model is as Figure 2 shown. The yaw angle error in path tracking is defined as the difference between the vehicle yaw angle and the desired yaw angle:

[0048]

[0049] In the formula, is the vehicle yaw angle, is the desired yaw angle.

[0050] Taking the derivative of Equation (2) gives

[0051]

[0052] Considering the road curvature c R , the ideal yaw angular velocity can be obtained from the following formula:

[0053]

[0054] The lateral displacement error e y is defined as the distance from the vehicle's center of mass to the center line of the preset trajectory. According to the kinematic relationship, we can get

[0055]

[0056] Linearizing Equation (5) at e y = 0 gives:

[0057]

[0058] The second derivatives of the lateral displacement error and the yaw angle error are:

[0059]

[0060] Combining Equations (1) to (7) and rewriting them in the form of a state-space equation, selecting the state variables The control variable u = δ, where δ is the vehicle's front wheel steering angle, we can get:

[0061]

[0062] In the formula,

[0063]

[0064] C f represents the cornering stiffness of the front wheels, and C r represents the cornering stiffness of the rear wheels, m represents the vehicle mass, and l f represents the distance from the vehicle's center of mass to the front axle, and l r represents the distance from the vehicle's center of mass to the rear axle.

[0065] The first-order Euler method is used to discretize Equation (8):

[0066] x(k + 1) = Ax(k) + Bu(k) + Dc R (k) (9)

[0067] In the formula, A = ΔT·A c + I 4×4 , B = ΔT·B c , D = ΔT·D c ; ΔT represents the sampling time, and I 4×4 represents the 4×4 identity matrix.

[0068] Equation (8) is the system model equation in model predictive control. Observing Equation (8), it can be found that it has an additional curvature term c R (k) compared to the standard state-space equation x(k + 1) = Ax(k) + Bu(k). Modern control theories are all based on the standard state-space equation. To apply these theories, it is necessary to process the curvature term and transform Equation (8) into the standard state-space equation form.

[0069] (3) System model reconstruction.

[0070] In the traditional state model, the vehicle dynamics state equation only includes vehicle states (such as lateral error, heading angle), and the road curvature is used as a state augmentation term. Let:

[0071]

[0072] Then the augmented standard state equation can be obtained:

[0073]

[0074]

[0075] However, in the present invention, the road curvature is augmented into the control quantity, and then the above tracking error model can be expressed as:

[0076]

[0077] In the formula, Using this method to reconstruct the system can not only utilize the known road curvature information but also reduce the matrix dimension.

[0078] (4) Cost function design. The goal of the controller is to minimize the cost function, which includes minimizing the tracking error and the steering input in two parts:

[0079] J = X T QX + U T RU (13)

[0080] Where, J represents the cost function, X represents the state quantity matrix, X T represents the transpose of the state quantity matrix, Q represents the state weight matrix, R represents the control weight matrix, U represents the control sequence, and U T represents the transpose of the control sequence.

[0081] X = [x(1), x(2), …, x(N p )] T

[0082] U = [u(0), u(1), …, u(N c -1)] T

[0083] (5) Constraint design. To meet the physical constraints and make the vehicle's corner change as smooth as possible during the tracking process, the following constraints are imposed on the corner:

[0084]

[0085] u min (k + i) represents the minimum value of the control quantity at the k + i moment, u(k + i) represents the control quantity at the k + i moment, and u max (k + i) represents the maximum value of the control quantity at the k + i moment, Δu min (k + i) represents the minimum value of the control increment at the k + i moment, Δu(k + i) represents the control increment at the k + i moment, and Δu max (k + i) represents the maximum value of the control increment at the k + i moment.

[0086] (6) Optimization and solution. According to the above steps, the MPC controller design is completed. When solving, the augmented optimization problem is transformed into a quadratic programming problem for solution, and the curvature term is used as the control quantity expansion term for optimization and solution to achieve the collaborative optimization of "prediction performance" and "computational efficiency".

[0087] As Figure 3 shown is the overall flowchart of the present invention.

[0088] Example 2:

[0089] To further illustrate this patent, under the MPC framework, the controller is designed by augmenting the state variables and the control variables respectively, and the changes in the matrix dimensions are observed as follows:

[0090] (1) By the method of augmenting the state variables:

[0091]

[0092] In the formula,

[0093]

[0094] Thus, the cost function can be transformed into the objective function of a quadratic programming problem:

[0095]

[0096] (2) By the method of augmenting the control variables:

[0097]

[0098] In the formula,

[0099]

[0100] Thus, the cost function can be transformed into the objective function of a quadratic programming problem:

[0101]

[0102] By comparing the matrix dimensions in formula (16) and formula (18), it can be found that the matrix dimension in formula (16) increases significantly more than that in formula (18), which in turn leads to an increase in the computational amount. Thus, the superiority of this patent is verified theoretically. In addition to theoretical derivation, a co-simulation platform of CarSim / Simulink is used for simulation verification. With the same controller parameters set, the simulation results are as shown in Figure 4 、 Figure 5 、 Figure 6 . The tracking accuracy effects of the two different augmentation methods are almost the same, but after using the control variable augmentation, the CPU calculation time can be shortened to 25% of that of the state variable augmentation, which further shows that the control variable augmentation method proposed in this patent can significantly improve the calculation efficiency and achieve a balance between path tracking accuracy and calculation real-time performance.

[0103] In the above embodiments, the reference to "this embodiment" in the specification means that the specific features, structures or characteristics described in combination with the embodiments are included in at least some embodiments, but not necessarily all embodiments. The multiple occurrences of "this embodiment" do not necessarily refer to the same embodiment.

[0104] In the above embodiments, although the present invention has been described in conjunction with specific embodiments of the present invention, many alternatives, modifications, and variations of these embodiments will be apparent to those of ordinary skill in the art based on the foregoing description. For example, other storage structures (e.g., dynamic RAM (DRAM)) may be used with the embodiments discussed. Embodiments of the present invention are intended to cover all such alternatives, modifications, and variations that fall within the broad scope of the appended claims.

[0105] This embodiment also provides a computer-readable storage medium having stored thereon a computer program, which when executed by a processor implements any one of the methods in this embodiment.

[0106] This embodiment also provides an electronic terminal, comprising: a processor and a memory;

[0107] The memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory so that the terminal executes any one of the methods in this embodiment.

[0108] For the computer-readable storage medium in this embodiment, those of ordinary skill in the art can understand that all or part of the steps for implementing the above method embodiments can be completed by hardware related to the computer program. The foregoing computer program can be stored in a computer-readable storage medium. When the program is executed, it executes the steps including the above method embodiments; and the foregoing storage medium includes: various media such as ROM, RAM, magnetic disk, or optical disk that can store program codes.

[0109] The electronic terminal provided in this embodiment includes a processor, a memory, a transceiver, and a communication interface. The memory and the communication interface are connected to the processor and the transceiver and complete communication therebetween. The memory is used to store a computer program, the communication interface is used for communication, and the processor and the transceiver are used to run the computer program so that the electronic terminal executes each step of the method as described above.

[0110] In this embodiment, the memory may include a random access memory (Random Access Memory, abbreviated as RAM), and may also include a non-volatile memory, such as at least one disk memory.

[0111] The above-mentioned processor may be a general-purpose processor, including a Central Processing Unit (CPU for short), a Network Processor (NP for short), etc.; it may also be a Digital Signal Processor (DSP for short), an Application Specific Integrated Circuit (ASIC for short), a Field-Programmable Gate Array (FPGA for short), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.

[0112] The present invention can be used in numerous general-purpose or special-purpose computing system environments or configurations. For example: personal computers, server computers, handheld or portable devices, tablet devices, multi-processor systems, microprocessor-based systems, set-top boxes, programmable consumer electronic devices, network PCs, minicomputers, mainframe computers, distributed computing environments including any of the above systems or devices, and so on.

[0113] The present invention can be described in the general context of computer-executable instructions executed by a computer, such as program modules. Generally, program modules include routines, programs, objects, components, data structures, etc. that perform specific tasks or implement specific abstract data types. The present invention can also be practiced in a distributed computing environment, where tasks are performed by remote processing devices connected through a communication network. In a distributed computing environment, program modules can be located in local and remote computer storage media including storage devices.

[0114] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical solutions of the present invention can be modified or equivalently replaced without departing from the spirit and scope of the present technical solution, and they should all be covered by the scope of the claims of the present invention.

Claims

1. A path tracking model predictive control method based on preview curvature, characterized in that: The following steps are involved: S1: Establish vehicle dynamics model; S2: Establish path tracking model; S3: The road curvature sequence is augmented as the control quantity extension term, and combined with the original control input front wheel steering angle to form an augmented control vector, and the path tracking model is reconstructed to obtain the standard state equation; S4: Design the cost function and constraints, use the curvature term as the control quantity extension term, and optimize the augmented path tracking model.

2. The path tracking model predictive control method based on preview curvature according to claim 1, characterized in that: The vehicle dynamics model in step S1 is as follows: In the formula, m represents the vehicle mass, v y 、v x denote the lateral velocity and longitudinal velocity respectively, represents the vehicle yaw angle, I s represents the moment of inertia of the vehicle around the Z axis, l f , l r Respectively represent the distance from the front and rear axles to the center of mass of the vehicle, F yf 、F yr Represent the lateral forces of the front and rear wheels respectively.

3. The path tracking model predictive control method based on preview curvature according to claim 1, characterized in that: The path tracking model in step S2 is: x(k+1)=Ax(k)+Bu(k)+Dc R (k) In the formula, k represents the current time, Represents the state quantity, e y represents the lateral displacement error, for e y The first derivative of represents the yaw angle error, for The first-order derivative of; u = δ represents the control amount, δ represents the front wheel steering angle of the vehicle, c R Represents the road curvature; A = ΔT·A c +I,B=ΔT·B c , D = ΔT·D c ; ΔT represents the sampling time, I 4×4 represents the identity matrix; C f represents the front wheel cornering stiffness, C r represents the rear wheel cornering stiffness, m represents the vehicle mass, l f Represents the distance from the vehicle's center of mass to the front axle, l r Indicates the distance from the vehicle's center of mass to the rear axle.

4. The path tracking model predictive control method based on preview curvature according to claim 1, characterized in that: In step S3, the known road curvature sequence c R (k) is used as an extended control term and is combined with the original control input front wheel steering angle δ to form an augmented control vector:

5. The path tracking model predictive control method based on preview curvature according to claim 1, characterized in that: The cost function in step S4 includes two parts: tracking error and steering input: J=X T QX+U T RU Where J represents the cost function, X represents the state sequence, and X T represents the transpose of the state quantity sequence, Q represents the state weight matrix, R represents the control weight matrix, U represents the control sequence, and U T Indicates control sequence transposition; X=[x(1),x(2),…,x(N p )] T U=[u(0),u(1),…,u(N c -1)] T 。 6. The path tracking model predictive control method based on preview curvature according to claim 1, characterized in that: The constraint conditions in step S4 include: applying the following constraints to the corners: u min (k+i)≤u(k+i)≤u max (k+i),i=1,2...,N-1 Δu min (k+i)≤Δu(k+i)≤Δu max (k+i),i=1,2...,N-1 u min (k+i) represents the minimum value of the control amount at time k+i, u(k+i) represents the control amount at time k+i, and u max (k+i) represents the maximum value of the control amount at time k+i, Δu min (k+i) represents the minimum value of the control increment at time k+i, Δu(k+i) represents the control increment at time k+i, Δu max (k+i) represents the maximum value of the control increment at time k+i.

7. An electronic device, characterized in that: including memory and processor; The memory is used to store computer programs; The processor is used to implement the preview curvature-based path tracking model predictive control method as described in any one of claims 1 to 6 when executing the computer program.

8. A computer-readable storage medium, characterized in that: The storage medium stores a computer program, and when the computer program is executed by a processor, the path tracking model predictive control method based on preview curvature as described in any one of claims 1 to 6 is implemented.

9. A computer program product, characterized in that: It comprises a computer program, which, when executed by a processor, implements the path tracking model predictive control method based on preview curvature as described in any one of claims 1 to 6.

Images