An automatic driving vehicle trajectory tracking control method, system and application

Abstract

This invention proposes a trajectory tracking control method, system, and application for autonomous vehicles, belonging to the field of autonomous driving and intelligent vehicle control technology. The method includes: S1, information acquisition; S2, dynamic adjustment of the prediction time domain; S3, constructing a nonlinear model predictive control optimization problem; S4, solving for the optimal control sequence; and S5, execution feedback. This invention introduces a dual-input fuzzy inference system based on road curvature and vehicle speed, enabling the prediction time domain to adaptively extend according to the steepness of curves, effectively solving the problem of insufficient curve look-ahead in existing single-input fuzzy methods due to neglecting curvature.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving and intelligent vehicle control technology, specifically to an autonomous vehicle trajectory tracking control method, system, and application. Background Technology

[0002] Trajectory tracking control, as a core technology of autonomous driving systems, directly determines the vehicle's accuracy in following the reference path and its driving safety. Nonlinear model predictive control (NMPC) has become a research hotspot in the field of lateral control of intelligent vehicles because it can handle the nonlinear characteristics of the system, multiple constraints, and multi-objective optimization requirements.

[0003] However, traditional NMPC methods generally employ a fixed prediction time domain for optimization, a mechanism that struggles to adapt to the dynamic trade-offs between tracking accuracy and algorithm real-time performance under complex driving conditions. Specifically, while an excessively long prediction time domain can improve foresight and control performance, it drastically increases the computational load of the optimization problem, severely limiting the algorithm's real-time performance. Conversely, while an excessively short prediction time domain can reduce the time required for a single step solution, it leads to insufficient trajectory foresight, resulting in a significant deterioration in tracking accuracy under conditions of curvature changes, such as curves.For example, in the literature [Xiong, L., Qi, Y., Leng, B., Yang, X., Liu, M., & Luo, Y. (2023, October). Nonlinear Model Predictive Path-Following Control with Steering Lag Compensation for Autonomous Vehicles. In 2023 7th CAA International Conference on Vehicular Control and Intelligence (CVCI), 2023: 1-6. IEEE.] (Translated by Xiong Lu, Qi Yunhai, Leng Bo, Yang Xiang, Liu Mu, Luo Yugong. Nonlinear Model Predictive Path-Following Control with Steering Lag Compensation for Autonomous Vehicles. 2023 7th CAA International Conference on Vehicular Control and Intelligence (CVCI). IEEE, 2023: 1-6.) and [Zhao, K., Wang, C., Xiao, G., Li, H., Ye, J., & Liu, Y. (2020). Research for Nonlinear Model Predictive Controls to Laterally [Translation: Zhao Kai, Wang Chun, Xiao Guo, Li Hua, Ye Jin, Liu Yang. Research on nonlinear model predictive control for lateral control of unmanned vehicle trajectory tracking. Applied Sciences, 2020, 10(17): 6034.] and the literature [Boggio, M., Novara, C., & Taragna, M. (2023). Trajectory Planning and Control for Autonomous Vehicles: A “Fast” Data-Aided NMPC Approach. European Journal of Control, 74, 100857.] (Translation: Boggio, M., Novara, C., Taragna, M. Trajectory Planning and Control for Autonomous Vehicles: A “Fast” Data-Aided NMPC Approach. European Journal of Control, 2023, 74: 100857.) etc. Therefore, how to reconcile the inherent conflict between predictive capability and computational efficiency is the core challenge currently facing NMPC trajectory tracking control.

[0004] To address the aforementioned contradictions, existing research mainly focuses on model simplification and algorithm acceleration, but neither approach fundamentally resolves the conflict between predictive capability and computational efficiency. Regarding model simplification, Linear Time-Varying MPC (LTV-MPC) uses a linear time-varying model to approximate the nonlinear dynamic model, reducing the complexity of single-step solutions. However, linearization errors are significantly amplified under extreme conditions. Another strategy is an adaptive switching method between kinematic and dynamic models based on vehicle speed; however, accurately defining the model switching boundary is difficult, easily leading to control jitter and affecting system smoothness. As disclosed in the literature [Rokonuzzaman, M., Mohajer, N., & Nahavandi, S. (2023). Effective Adoption of Vehicle Models for Autonomous Vehicle Path Tracking: A Switched MPC Approach. Vehicle System Dynamics, 61(5), 1236–1259.] Regarding algorithm acceleration, some studies have used neural networks to approximate MPC control laws offline, achieving millisecond-level responses. However, their generalization ability is limited by the distribution range of training data, and their reliability is insufficient under extreme conditions; as recorded in the literature [Rokonuzzaman, M., Mohajer, N., & Nahavandi, S. (2023). Effective Adoption of Vehicle Models for Autonomous Vehicle PathTracking: A Switched MPC Approach. Vehicle System Dynamics, 61(5), 1236–1259.]. Other studies have adopted event-triggered mechanisms to reduce the frequency of optimization solutions, but at the cost of sacrificing the closed-loop stability of the control system.As described in the literature [Chen, J., Xu, X., & Yang, J. (2025). Adaptive Model Predictive Control for Autonomous Vehicle Trajectory Tracking. Vehicles, 7(4), 114.] (Translation: Chen Jie, Xu Xiao, Yang Jian. Adaptive Model Predictive Control for Autonomous Vehicle Trajectory Tracking. Vehicles, 2025, 7(4): 114.). None of the above methods directly perform adaptive optimization on the core parameter of prediction time domain, and therefore cannot fundamentally solve the inherent contradiction between NMPC tracking accuracy and real-time performance.

[0005] Against this backdrop, fuzzy control, with its characteristics of not relying on a precise mathematical model of the controlled object, easily incorporating expert experience, and possessing robust nonlinear mapping capabilities, provides a feasible technical path for adaptive adjustment in the prediction time domain. Existing research has used vehicle speed as a single fuzzy input to achieve prediction time domain adjustment, initially verifying the effectiveness of fuzzy logic in parameter adaptation. However, this method does not consider the differentiated impact of geometric features such as road curvature on the controller's look-ahead requirements, leading to a mismatch between the prediction time domain and road complexity under curve conditions, thus limiting the improvement of control performance. As described in the literature [Rokonuzzaman, M., Mohajer, N., & Nahavandi, S. (2023). Effective Adoption of Vehicle Models for Autonomous Vehicle Path Tracking: A Switched MPC Approach. Vehicle System Dynamics, 61(5), 1236–1259.] (Translation: Rokonuzzaman, M., Mohajer, N., Nahavandi, S. Effective Adoption of Vehicle Models for Autonomous Vehicle Path Tracking: A Switched MPC Approach. Vehicle System Dynamics, 2023, 61(5): 1236–1259.). Other studies have introduced curvature information to adjust control weights to improve steering response performance, but these still use a fixed prediction time domain, failing to fully unleash the potential of parameter adaptation, and still exhibiting control response lag in road sections with abrupt curvature changes. As described in the literature [Zhang,P.,Xia,X.,Fu,Y.,&Sun,J.(2025).FVSMPC:Fuzzy Adaptive Virtual Steering Coefficient Model Predictive Control for Differential Tracked RobotTrajectory Tracking.Actuators,14(12),493.] (Translation: Zhang Peng, Xia Xin, Fu Yong, Sun Jie.FVSMPC:Fuzzy Adaptive Virtual Steering Coefficient Model Predictive Control for Differential Tracked RobotTrajectory Tracking.Actuators,2025,14(12):493.).

[0006] Furthermore, existing NMPC studies based on kinematic models often implicitly assume low-speed conditions, neglecting the essential constraint of tire slip characteristics on model effectiveness at high speeds. Specifically, kinematic models assume that the tire slip angle is negligible and that the vehicle's velocity direction is always consistent with its heading. This assumption holds true at low speeds, but at high speeds (typically >15 m / s), significant tire lateral forces lead to an increase in the slip angle, causing a significant deviation between the kinematic model's predicted trajectory and the vehicle's actual response. Ultimately, this results in a sharp deterioration in control performance, failing to meet the stability requirements of high-speed scenarios. As described in the literature [Yan, R., Li, P., Qin, D., & Liu, S. (2024, May). Trajectory Tracking Control for Autonomous Vehicle Based on Curvature Feedforward MPC. In Proceedings of the 2024 5th International Conference on Artificial Intelligence and Electromechanical Automation (AIEA) (pp. 1018–1024). Guangzhou, China: IEEE.] (Translation: Yan Rui, Li Peng, Qin Dong, Liu Shuang. Trajectory Tracking Control for Autonomous Vehicle Based on Curvature Feedforward MPC. Proceedings of the 5th International Conference on Artificial Intelligence and Electromechanical Automation (AIEA) 2024. Guangzhou, China, May 2024: 1018-1024.)

[0007] In summary, existing NMPC trajectory tracking control methods for autonomous vehicles suffer from technical shortcomings such as static configuration in the prediction time domain, insufficient adaptability to operating conditions, poor high-speed stability, and difficulty in balancing tracking accuracy and real-time performance. There is an urgent need for an NMPC trajectory tracking control method that can dynamically adjust the prediction time domain by combining vehicle motion state and road geometry features, while simultaneously considering control accuracy, smoothness, and algorithm real-time performance, to address the deficiencies of existing technologies. Summary of the Invention

[0008] The technical problem this invention aims to solve is that existing single-input fuzzy adaptive NMPC trajectory tracking control methods suffer from insufficient cornering tracking accuracy, poor high-speed stability, and inadequate control smoothness due to neglecting road curvature, lacking dynamic stability constraints, and lacking control increment constraints. This invention provides a trajectory tracking control method, system, and application based on fuzzy adaptive prediction time-domain nonlinear model predictive control that can achieve predictive time-domain adaptive adjustment while balancing high-speed stability and smoothness.

[0009] The technical solution of this invention: An autonomous vehicle trajectory tracking control method, which differs in that it includes the following steps: S1. Information Acquisition: Obtain the geometric information and desired motion state of the reference path, and acquire the real-time operating status of the vehicle; S2. Dynamically adjust the prediction time domain: The road curvature is extracted from the geometric information of the reference path, the longitudinal speed is extracted from the real-time vehicle operating status, and the road curvature and the real-time longitudinal speed of the vehicle are used as dual input variables to dynamically adjust the prediction time domain of the nonlinear model predictive control through a fuzzy inference system. S3. Constructing a nonlinear model for predictive control optimization: Based on the prediction time domain, the tracking error is calculated according to the real-time operating state of the vehicle and the desired motion state, and a nonlinear model predictive control optimization problem is constructed based on the nonlinear kinematic error model; the objective function of the optimization problem includes weighted penalties for the tracking error, control quantity and control increment, and the constraints include at least actuator amplitude constraints, control increment constraints and vehicle lateral stability constraints for limiting the maximum safe front wheel steering angle under high-speed conditions. S4. Solve for the optimal control sequence: The optimization problem is solved using a sequential quadratic programming algorithm to obtain the optimal control sequence. S5. Execution Feedback: The first control quantity of the optimal control sequence is output to the vehicle actuator, and feedback correction is performed based on the real-time operating status of the vehicle to form a closed-loop control.

[0010] Furthermore, the nonlinear kinematic error model uses the longitudinal displacement error, lateral displacement error, and heading angle error of the vehicle relative to the reference path as state variables, and the vehicle speed and front wheel steering angle as control variables. Its continuous-time vehicle kinematic model is as follows: Among them, state variables Control quantity ; Longitudinal displacement, unit: m; Lateral displacement, unit: m; and Represents the longitudinal and lateral velocities in the inertial coordinate system, in m / s; Heading angle, unit: rad; The vehicle's yaw rate, in rad / s; Rear axle speed, unit: m / s; Front wheel steering angle, unit: rad; This refers to the vehicle's wheelbase, in meters (m).

[0011] Furthermore, the specific process of dynamically adjusting the prediction time domain of the nonlinear model predictive control through the fuzzy inference system in step S2 includes: Vehicle speed is divided into six fuzzy subsets, corresponding to extremely low speed, very low speed, low speed, medium speed, high speed, and extremely high speed, respectively, with the universe of discourse limited to 0 km / h to 100 km / h; a triangular membership function is used, and the center values ​​of the membership degree of each subset are set to 0, 20, 40, 60, 80, and 100 km / h respectively. The road curvature is divided into five fuzzy subsets, corresponding to minimum curvature, very small curvature, moderate curvature, large curvature, and maximum curvature, respectively, with the universe of discourse limited to 0m. - ¹ to 0.014m - ¹; A non-uniform triangular membership function is used, and the center values ​​of each subset with a membership degree of 1 are set to approximately 0, 0.0028, 0.007, 0.011, and 0.014m, respectively. - ¹; The prediction time domain is divided into six fuzzy subsets, corresponding to the very short time domain, very short time domain, short time domain, medium time domain, long time domain, and very long time domain, respectively. The universe of discourse is limited to integers between 10 and 40. The triangular membership function is used, and the center values ​​of the membership degree of each subset are set to 10, 16, 22, 28, 34, and 40 respectively. The prediction time domain is output according to the fuzzy configuration rules. The fuzzy configuration rules take into account both the real-time performance and predictability of the calculation. The configuration principle is: when the vehicle speed is higher and / or the curvature is greater, the prediction time domain is smaller to reduce the solution load of rolling optimization; conversely, the prediction time domain is larger to improve the smoothness of trajectory tracking.

[0012] Furthermore, in step S3, the objective function is expressed as: in, To predict the time domain, To control the time domain, Here is the error weight matrix. To control the weight matrix, To control the incremental weight matrix, For state prediction based on time k The system state error at time 1. For state prediction based on time k Control parameters at any given time (including speed v and front wheel steering angle) ), For state prediction based on time k The control increment at any given time is the difference between two consecutive control commands, or the difference between commands in two adjacent control cycles. The reference feedforward control quantity at time k.

[0013] Furthermore, the actuator amplitude constraint includes vehicle speed constraint and front wheel steering angle constraint; the specific constraint formula is as follows: in, For state prediction based on time k Longitudinal vehicle speed at any given time, in m / s; and These are the minimum and maximum longitudinal vehicle speeds allowed by the vehicle's actuators, respectively, in m / s; For state prediction based on time k Front wheel steering angle at any given moment, unit: rad; and These are the minimum and maximum front wheel steering angles allowed by the vehicle's actuators, respectively, in rad.

[0014] Furthermore, the control increment constraint is used to limit the rate of change of the control quantity in adjacent control cycles, and the constraint is as follows: in, Indicates the longitudinal speed increment between adjacent control cycles, in m / s; and These are the minimum and maximum permissible longitudinal speed increments, respectively, in m / s; The front wheel steering angle increment between adjacent control cycles is expressed in rad. and These are the minimum and maximum permissible front wheel steering angle increments, respectively, in rad.

[0015] Furthermore, the vehicle lateral stability constraint is as follows: Where L is the vehicle wheelbase, in meters (m); The maximum permissible lateral acceleration to ensure the lateral stability of a vehicle, in m / s². 2 ; For state prediction based on time k Front wheel steering angle at a given moment, unit: rad; when vehicle speed Permissible front wheel steering angle when raised It automatically contracts, thus forcing the vehicle to maintain a smaller steering angle at high speeds.

[0016] Furthermore, the sequential quadratic programming algorithm transforms the restricted nonlinear programming problem into a series of quadratic programming subproblems, and solves one quadratic programming subproblem in each iteration to update the control sequence.

[0017] An autonomous vehicle trajectory tracking and control system includes: The upper-level trajectory planning information module is used to provide geometric information of the reference path and the desired motion state; The fuzzy adaptive decision module is used to extract road curvature from the geometric information of the reference path, extract real-time longitudinal vehicle speed from the real-time vehicle operating status, and dynamically adjust the prediction time domain of the nonlinear model predictive control through the fuzzy inference system using the road curvature and the real-time longitudinal vehicle speed as dual input variables. The nonlinear rolling optimization module is used to calculate the tracking error based on the predicted time domain, the real-time operating state of the vehicle, and the desired motion state. It constructs an optimization problem based on a nonlinear kinematic error model and solves the optimization problem using a sequential quadratic programming algorithm to obtain the optimal control sequence. The objective function of the optimization problem includes weighted penalties for tracking error, control quantity, and control increment. The constraints include at least actuator amplitude constraints, control increment constraints, and vehicle lateral stability constraints for limiting the maximum safe front wheel steering angle under high-speed conditions. The vehicle status feedback module is used to output the first control quantity of the optimal control sequence to the vehicle actuator, and perform feedback correction based on the real-time operating status of the vehicle to form a closed-loop control.

[0018] A vehicle controller includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps of the above-described autonomous vehicle trajectory tracking control method.

[0019] Compared with the prior art, the present invention has the following beneficial effects: 1. Significantly Improved Tracking Accuracy: This invention introduces a dual-input fuzzy inference system based on road curvature and vehicle speed, enabling the prediction time domain to adaptively extend according to the steepness of curves. This effectively solves the problem of insufficient curve look-ahead in existing single-input fuzzy methods due to neglecting curvature. Simulation results show that under low-speed (30km / h) double lane-change conditions, this invention improves lateral tracking accuracy by 23.4% compared to the fixed prediction time domain nonlinear model predictive control method; under high-speed (70km / h) conditions, the lateral tracking accuracy is improved by 8.01%. Compared to the traditional linear model predictive control method, the accuracy improvement is even more significant (42.4% at low speeds and 77.91% at high speeds).

[0020] This invention forms a closed-loop control through vehicle state feedback, continuously corrects model prediction deviations, and enables the system to have good robustness to complex working conditions such as sudden changes in road curvature and vehicle speed changes.

[0021] 2. Strong Real-Time Performance: This invention employs an adaptive prediction time-domain strategy, automatically shortening the prediction time domain on straight or low-curvature road sections to avoid computational redundancy caused by a fixed long time domain. Simultaneously, it utilizes nonlinear optimization and sequential quadratic programming algorithms based on kinematic error models, significantly reducing the single-step solution complexity compared to nonlinear model predictive control based on dynamic models. Simulation data shows that under low-speed conditions, the average solution time of this invention is reduced by 11.72% compared to nonlinear model predictive control with a fixed prediction time domain; under high-speed conditions, it is reduced by 16.04%, effectively eliminating control lag and meeting the stringent real-time requirements of autonomous driving.

[0022] 3. Excellent control smoothness and ride comfort: This invention introduces a control increment penalty term into the objective function and explicitly applies control increment constraints, limiting the rate of change of the front wheel angle within adjacent control cycles, thereby suppressing drastic jumps in the steering wheel angle. This allows for smooth transitions in steering commands, avoiding vehicle yaw and vibration caused by sudden changes in control quantities, significantly improving ride comfort and the lifespan of the actuators.

[0023] 4. High stability and low safety risk at high speeds: Addressing the instability risk caused by neglecting tire side-slip characteristics in existing kinematic models under high-speed conditions, this invention introduces a dynamic stability constraint based on the road adhesion limit. This constraint enforces a non-linear inverse relationship between the maximum safe front wheel steering angle and the square of the vehicle speed—the higher the vehicle speed, the smaller the allowable front wheel steering angle, fundamentally avoiding the risks of sideslip and rollover during high-speed cornering. Compared to traditional methods that only consider actuator amplitude constraints, this invention significantly expands the safe speed range of the kinematic model controller without switching to a complex dynamic model, achieving a balance between computational efficiency and driving safety. Attached Figure Description

[0024] Figure 1 This is a schematic diagram of the vehicle in a coordinate system. Figure 2 This is a diagram of the overall architecture of the control system. Figure 3 For vehicle speed membership graph; Figure 4 This is a double-line curvature diagram; Figure 5 For curvature membership graph; Figure 6 To predict the time-domain membership graph; Figure 7 For fuzzy control output surface; Figure 8 A comparison of performance indicators under a 30km / h operating condition; Figure 9 A comparison of performance indicators under the 70km / h operating condition. Detailed Implementation

[0025] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments. It should be noted that these embodiments are for illustrative purposes only and are not intended to limit the scope of protection of the invention.

[0026] 1. Vehicle kinematic modeling In the inertial coordinate system (XOY), assuming the vehicle undergoes planar motion and the body and suspension system are rigid, lateral forces on the tires and load transfer between the front and rear axles are ignored. Kinematic analysis of the vehicle is performed with the rear axle as the center. For a small car with front-wheel steering and rear-wheel drive, it can be abstracted as a bicycle model, specifically as follows: Figure 1 As shown.

[0027] Figure 1 The key parameters are described in Table 1: Table 1 Description of Key Parameters By analyzing the vehicle's motion state, a continuous-time model of the vehicle can be obtained: (1) Among them, the state variables are The control quantity is , and These represent the longitudinal and lateral velocities in the inertial coordinate system, respectively, in m / s. Let be the vehicle's yaw rate, in rad / s. This can be abstracted into a nonlinear state equation form: (2) Define the reference trajectory state as Define the system state error vector as .in These represent the ideal longitudinal position, lateral position, and heading angle of the reference trajectory, respectively, in meters (m), meters (m), and rad (rad). These represent longitudinal position error, lateral position error, and heading angle error, respectively, with units of m, m, and rad.

[0028] The system state error can then be expressed as: (3) By differentiating the error vector and combining it with the kinematic model, we can obtain the error dynamic equation: (4) Wherein, the reference heading angular velocity satisfies , This represents the corresponding rate of change of error; The longitudinal position error rate is expressed in m / s. The longitudinal position error rate is expressed in m / s. v is the rate of change of longitudinal position error, in m / s; v is the actual longitudinal speed of the vehicle, in m / s; v r The actual longitudinal speed of the vehicle, in m / s; The actual heading angle of the vehicle, in rad; Reference heading angle, unit: rad; Reference yaw rate, unit: rad / s; The curvature of the road.

[0029] 2. Design of trajectory tracking control system 2.1 Overall Architecture like Figure 2 As shown, the Fuzzy Adaptive Nonlinear Model Predictive Control (FANMPC) constructed in this paper is mainly composed of four core components coupled together: an upper-level trajectory planning information module, a fuzzy adaptive decision-making module, a nonlinear rolling optimization module, and a vehicle state feedback module.

[0030] The upper-level trajectory planning information module first provides the geometric information of the reference path and the desired motion state; the fuzzy adaptive decision module is used to extract the road curvature from the geometric information of the reference path, extract the real-time longitudinal vehicle speed from the real-time vehicle operation state, and use the road curvature and the real-time longitudinal vehicle speed as dual input variables to dynamically adjust the prediction time domain Np of the nonlinear model predictive control through the fuzzy inference system to achieve an adaptive balance between computational efficiency and predictive performance; the nonlinear rolling optimization module is the nonlinear model predictive control (NMPC) controller implemented in this invention, which infers the future state evolution based on the kinematic error model and solves the optimal control sequence within the feasible domain that satisfies multiple constraints. The NMPC controller kernel extrapolates future state evolution based on a nonlinear kinematic error model. It uses the Sequence Quadratic Programming (SQP) algorithm to solve for the optimal control sequence that minimizes the weighted cost of tracking error and control energy consumption within a feasible region that strictly satisfies the constraints of actuator physical amplitude, control increment, and vehicle lateral stability. Finally, the vehicle state feedback module applies the first term of the sequence to the controlled vehicle and continuously corrects the prediction deviation through state feedback, thereby forming a highly robust closed-loop path tracking system.

[0031] 2.2 NMPC Controller (i.e., Nonlinear Rolling Optimization Module) Design To facilitate optimization, the forward Euler method is used to discretize the continuous model described above. The sampling period is set to... The discretized state transition equations are obtained as follows: (5) Where e(k) and e(k+1) represent the system state error at the k-th discrete time and the (k+1)-th discrete time, respectively; It is a continuous nonlinear kinematic state transition function; The discrete sampling period of the control system, in seconds; specifically: (6) in, , These are the longitudinal position error, lateral position error, and yaw angle error predicted for the next moment, respectively, with units of m, m, and rad. , and These are the actual longitudinal vehicle speed, front wheel steering angle, and yaw angle at time k, respectively, in m / s, rad, and rad. and Here, represents the longitudinal vehicle speed, yaw angle, and yaw rate on the reference trajectory at time k, respectively, in m / s, rad, and rad / s. Within each control cycle, NMPC predicts the future trajectory of the system in the prediction time domain Np.

[0032] Assuming the control time domain is Nc, when k > Nc, the control quantity remains unchanged, i.e. To balance real-time computation with tracking accuracy, a fuzzy logic controller is introduced. This is based on road curvature. and vehicle speed As input, dynamically adjust the prediction time domain Np: (7) Formula (7) is the pseudocode form of fuzzy control, here This represents the fuzzy control module, and the detailed design of the fuzzy control strategy will be described later.

[0033] Within each control cycle, the controller needs to base its actions on the current time. status Predict the state in the next Np steps. Let the control sequence be... The predicted state sequence is then obtained through recursive calculation: (8) in, This represents the prediction of the k-th future time from the current time k. The system state at each discrete time step; This represents the prediction of the future at time k. Step control input; This represents the nonlinear vehicle kinematics state transition function after forward Euler discretization. When hour, That is, controlling the constraints of the time domain on computational complexity.

[0034] To suppress abrupt changes in steering wheel angle and vehicle speed, control increments were introduced. At each sampling time Construct the following restricted nonlinear optimization problem: (9) To predict the time domain, To control the time domain, Here is the error weight matrix. To control the weight matrix, To control the incremental weight matrix, For state prediction based on time k System state variables at time 1. For state prediction based on time k The amount of control at any given moment For state prediction based on time k Control increment at any time, The reference feedforward control quantity at time k.

[0035] Because vehicle chassis actuators (such as electronic throttle valves and steering motors) have physical travel limits, the control input must be adjusted. Apply hard constraints. Where u(k) is the control input at time k (including...). ); This represents the vertical input at time k, in m / s. The front wheel angle at time k is expressed in rad.

[0036] (10) The unfolded form is as follows: (11) For state prediction based on time k Longitudinal vehicle speed at any given time, in m / s; and These are the minimum and maximum longitudinal vehicle speeds allowed by the vehicle's actuators, respectively, in m / s; For state prediction based on time k Front wheel steering angle at any given moment, unit: rad; and These are the minimum and maximum front wheel steering angles allowed by the vehicle's actuators, respectively, in rad.

[0037] At the same time, we also added constraints to the introduced control increment, that is, we limited the vehicle's movement rate in the discrete domain, in the following form: (12) The unfolded form is as follows: (13) In addition, to prevent the risk of sideslip or rollover during high-speed cornering, nonlinear constraints based on the Dynamics Envelope are introduced. According to vehicle dynamics principles, the lateral acceleration of a vehicle while cornering... The road surface adhesion limit must be met: (14) in, The instantaneous lateral acceleration of a vehicle during motion, measured in m / s². 2 ; This represents the current road surface adhesion coefficient. Acceleration due to gravity, unit: m / s² 2 .

[0038] Under the kinematic model, the lateral acceleration can be approximated as: (15) R is the instantaneous turning radius of the vehicle's current trajectory, in meters (m). Therefore, the maximum safe front wheel steering angle... With vehicle speed It exhibits a non-linear inverse proportional relationship: (16) The maximum permissible safe lateral acceleration of a vehicle, determined by the road adhesion limit, in m / s². 2 Rearranging formula (16), we obtain the state dependency constraints regarding the rotation angle: (17) When the vehicle speed Permissible front wheel steering angle when raised It automatically contracts, thus forcing the vehicle to maintain a small steering angle at high speeds, fundamentally avoiding the risk of instability.

[0039] Define the above constraint set as The cost function in formula (9) ,exist At time 1, the problem becomes a standard constrained nonlinear programming (NLP) problem, which is solved using the sequential quadratic programming (SQP) algorithm from the built-in fmincon function in MATLAB. The core idea is to transform the complex nonlinear problem into an iterative solution of a series of quadratic programming (QP) subproblems. With this, the design of the NMPC controller is complete.

[0040] 2.3 Design of Fuzzy Adaptive Decision Module Within the framework of the aforementioned nonlinear model predictive control (NMPC), the prediction time domain... The selection of the parameter not only defines the controller's "foreseeability distance" to future trajectories but also directly constrains the computational dimension and solution timeliness of the Sequential Quadratic Programming (SQP) problem. Traditional NMPC often uses a fixed constant as the prediction time domain. However, in the actual working conditions of autonomous vehicles, this static configuration often leads to overshooting due to insufficient foreseeability at high speeds or reduced real-time performance due to computational redundancy on low-speed, complex curvature road sections. To resolve this contradiction, a fuzzy adaptive decision-making strategy coupling vehicle state and environmental geometric features is designed, which dynamically adjusts... The value of is chosen to achieve a Pareto optimal balance between control performance and computational cost. The core logic of this decision-making mechanism lies in establishing a nonlinear mapping from the perception parameter space to the control parameter space. In the fuzzification stage, the real-time longitudinal speed of the vehicle is selected. Road curvature relative to the current reference trajectory It is a two-input variable.

[0041] Speed The universe of discourse is divided into six fuzzy subsets, [TS,VS,NS,MS,HS,LS], representing extremely low speed, very low speed, low speed, medium speed, high speed, and extremely high speed, respectively, to characterize the intensity of vehicle motion over time. Considering the use of a vehicle kinematic model, which has some error compared to the nonlinearity of a real vehicle, the designed speed range is [0,100]. Furthermore, because real-time performance is required for vehicle control, a trigonometric membership function is chosen, with the specific division as follows: Figure 3 As shown.

[0042] curvature The domain of discourse is divided according to the hyperbola path of the experiment, and the hyperbola reference trajectory model is shown in formula (18).

[0043] (18) in, For reference longitudinal position, unit: m; , These are the reference lateral position and reference heading angle at x, respectively, in meters (m) and rad (rad). , , , , and These are all preset constant parameters that determine the geometry of the double-lane-switching road. The parameters in the trajectory model are respectively... , , , , , The curvature of the trajectory is calculated according to formula (19). (For lateral displacement), we can obtain as follows: Figure 4 The trajectory curvature diagram.

[0044] (19) in, and These are the first and second derivatives of the horizontal coordinate with respect to the vertical coordinate of the reference trajectory, respectively. The curvature of the reference experimental trajectory is 0.013m. -1 Within this range, to allow for some space, the design universe of discourse is set to [0, 0.014]. Simultaneously, to avoid control oscillations caused by overly fine universe of discourse division, the curvature is divided into five fuzzy subsets, denoted by [TK, VK, MK, HK, LK], representing minimum curvature, very small curvature, moderate curvature, large curvature, and maximum curvature, respectively, reflecting the spatial constraints imposed by the environment on steering control accuracy. The membership relationships are as follows: Figure 5 As shown, the universe of discourse in the prediction time domain is limited to an integer between 10 and 40 to achieve an adaptive balance between computational efficiency and prediction performance.

[0045] Considering the real-time performance issues of nonlinear model predictive control, the control time domain is fixed, while the prediction time domain is limited to [10, 40], and its universe of discourse is designed as [10, 40]. This universe is then divided into six subsets, with [TN, VN, NN, MN, HN, LN] representing extremely short, very short, short, medium, long, and very long prediction time domains, respectively. The membership relationships are as follows: Figure 6 As shown.

[0046] The prediction time domain is output according to fuzzy configuration rules. These rules comprehensively consider both real-time performance and predictability, and are configured as follows: the higher the vehicle speed and / or the greater the curvature, the smaller the prediction time domain is output to reduce the solution load of rolling optimization; conversely, the larger the prediction time domain is output to improve the smoothness of trajectory tracking. A set of fuzzy control rules was designed based on the relationship between vehicle speed, curvature, and prediction time domain, as shown in Table 2.

[0047] Table 2 Fuzzy Control Rules To verify the feasibility of the design rules, the output surface of the fuzzy rules was generated, such as... Figure 7 As shown, the output surface of the designed fuzzy control rule is relatively smooth, with no obvious irregular shape or obvious bumps, indicating that the designed fuzzy rule is usable.

[0048] 2.4 Execution Feedback: The first control quantity of the optimal control sequence is output to the vehicle actuator, and feedback correction is performed based on the real-time operating status of the vehicle to form a closed-loop control.

[0049] The present invention provides a method for trajectory tracking and control of autonomous vehicles, which specifically includes: S1. Information Acquisition: Acquire the geometric information and desired motion state of the reference path, and acquire the real-time operating status of the vehicle; S2. Dynamically adjust the prediction time domain: extract the road curvature from the geometric information of the reference path, extract the longitudinal speed from the real-time vehicle operating status, and use the road curvature and the real-time longitudinal speed of the vehicle as dual input variables to dynamically adjust the prediction time domain of the nonlinear model prediction control through the fuzzy inference system. S3. Constructing a nonlinear model for predictive control optimization: Based on the prediction time domain, the tracking error is calculated according to the real-time operating state of the vehicle and the desired motion state, and a nonlinear model predictive control optimization problem is constructed based on the nonlinear kinematic error model; the objective function of the optimization problem includes weighted penalties for the tracking error, control quantity and control increment, and the constraints include at least actuator amplitude constraints, control increment constraints and vehicle lateral stability constraints for limiting the maximum safe front wheel steering angle under high-speed conditions. S4. Solve for the optimal control sequence: The optimization problem is solved using a sequential quadratic programming algorithm to obtain the optimal control sequence.

[0050] 3 Simulation Results 3.1 Under the condition of vehicle speed of 30km / h To verify the performance of the fuzzy adaptive prediction time-domain-based NMPC controller, a comparative analysis was conducted with the traditional fixed prediction time-domain NMPC controller and the kinematics-based model predictive controller. The experiments used Matlab / Simulink 2024b and Carsim 2019, and the vehicle was a C-class hatchback sedan. Specific vehicle parameters are shown in Table 3. Table 3 Vehicle Specific Parameters The experimental trajectory was generated in Matlab / Simulink using formula (18), and the same experimental trajectory was configured in Carsim. The lateral control accuracy and real-time calculation performance of the three controllers were compared and analyzed.

[0051] Depend on Figure 8 As can be seen in (a), at a vehicle speed of 30 km / h, the FANMPC, traditional NMPC, and MPC controllers can all effectively track the trajectory with an accuracy within 10 cm, but the tracking accuracy differs slightly at turns. Figure 8 As can be more clearly seen in (b), the control accuracy of the traditional NMPC controller is higher than that of the MPC controller, which is also due to the higher accuracy of the NMPC model. Furthermore, the traditional NMPC controller has poor real-time performance, resulting in control lag, while the adaptive predictive time domain of FANMPC gives it better real-time control performance. Therefore, the control accuracy of FANMPC is higher than that of the traditional NMPC. Figure 8 (c) and Figure 8 As can be seen from (d) in the figure, FANMPC can effectively achieve adaptive predictive time-domain control, and at the same time, it has a lower solution time compared to the traditional NMPC.

[0052] 3.2 Under the condition of vehicle speed of 70km / h At a vehicle speed of 70 km / h, the specific performance indicators of the controller are as follows: Figure 9 As shown.

[0053] in accordance with Figure 9 From (a) we can conclude that at a vehicle speed of 70 km / h, all three controllers can effectively track the trajectory, but their tracking performance is poor at curves. Combined with... Figure 9 In data (b), both FANMPC and the traditional NMPC controller outperform MPC in lateral error, with the most significant difference observed at curves. Regarding real-time performance, through... Figure 9 (c) and Figure 9 As can be seen from (d) in the figure, FANMPC’s adaptive prediction time domain is effective and its solution time is better than that of traditional NMPC.

[0054] 3.3 Quantitative Analysis For the two operating conditions described above, quantitative analysis was performed on the data. For the lateral error, its root mean square was used as the standard to analyze the overall fluctuation level of vehicle deviation from the trajectory. For the solution time, its average value was used as the standard to analyze the overall average level and central trend of the solution time for each controller. Specific values ​​are shown in Table 4.

[0055] Table 4. Lateral error and solution time data for each controller. The tracking performance of the MPC controller is worse than that of the NMPC controller in both operating conditions. Furthermore, the higher the speed and the stronger the vehicle's nonlinearity, the worse the control effect of the MPC becomes. At low speeds, the root mean square error of the lateral tracking error is 0.0250 meters, while at high speeds it reaches 0.3740 meters. In contrast, the traditional fixed prediction time-domain NMPC outperforms the MPC in tracking error, with an error of only 0.0188 meters at low speeds and only 0.0898 meters at high speeds. Although the trajectory tracking effect is better, its nonlinear quadratic programming solution time is longer. FANMPC, based on the traditional NMPC, improves the accuracy of lateral tracking error while also improving the real-time performance of the computation.

[0056] See Figure 2 The present invention also proposes an autonomous vehicle trajectory tracking control system, which corresponds to the aforementioned autonomous vehicle trajectory tracking control method, and specifically includes: The upper-level trajectory planning information module is used to provide geometric information of the reference path and the desired motion state; The fuzzy adaptive decision module is used to extract road curvature from the geometric information of the reference path, extract real-time longitudinal vehicle speed from the real-time vehicle operating status, and dynamically adjust the prediction time domain of the nonlinear model predictive control through the fuzzy inference system using the road curvature and the real-time longitudinal vehicle speed as dual input variables. The nonlinear rolling optimization module is used to calculate the tracking error based on the predicted time domain, the real-time operating state of the vehicle, and the desired motion state. It constructs an optimization problem based on a nonlinear kinematic error model and solves the optimization problem using a sequential quadratic programming algorithm to obtain the optimal control sequence. The objective function of the optimization problem includes weighted penalties for tracking error, control quantity, and control increment. The constraints include at least actuator amplitude constraints, control increment constraints, and vehicle lateral stability constraints for limiting the maximum safe front wheel steering angle under high-speed conditions. The vehicle status feedback module is used to output the first control quantity of the optimal control sequence to the vehicle actuator, and perform feedback correction based on the real-time operating status of the vehicle to form a closed-loop control.

[0057] The present invention also proposes a vehicle controller, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the above-described autonomous vehicle trajectory tracking control method.

Claims

1. A trajectory tracking control method for an autonomous vehicle, characterized in that, Includes the following steps: S1. Information Acquisition: Obtain the geometric information and desired motion state of the reference path, and acquire the real-time operating status of the vehicle; S2. Dynamically adjust the prediction time domain: The road curvature is extracted from the geometric information of the reference path, the longitudinal speed is extracted from the real-time vehicle operating status, and the road curvature and the real-time longitudinal speed of the vehicle are used as dual input variables to dynamically adjust the prediction time domain of the nonlinear model predictive control through a fuzzy inference system. S3. Constructing a nonlinear model predictive control optimization problem: Based on the prediction time domain, the tracking error is calculated according to the real-time operating state of the vehicle and the desired motion state, and a nonlinear model predictive control optimization problem is constructed based on the nonlinear kinematic error model; the objective function of the optimization problem includes weighted penalties for the tracking error, control quantity and control increment, and the constraints include at least actuator amplitude constraints, control increment constraints and vehicle lateral stability constraints for limiting the maximum safe front wheel steering angle under high-speed conditions. S4. Solve for the optimal control sequence: The optimization problem is solved using a sequential quadratic programming algorithm to obtain the optimal control sequence. S5. Execution Feedback: The first control quantity of the optimal control sequence is output to the vehicle actuator, and feedback correction is performed based on the real-time operating status of the vehicle to form a closed-loop control.

2. The autonomous vehicle trajectory tracking control method according to claim 1, characterized in that, The nonlinear kinematic error model uses the longitudinal displacement error, lateral displacement error, and heading angle error of the vehicle relative to the reference path as state variables, and vehicle speed and front wheel steering angle as control variables. Its continuous-time vehicle kinematic model is as follows: Among them, state variables Control quantity ; Longitudinal displacement, unit: m; This refers to lateral displacement, in meters (m). and These represent the longitudinal and lateral velocities in the inertial coordinate system, respectively, in m / s. Heading angle, unit: rad; The vehicle's yaw rate, in rad / s; Rear axle speed, unit: m / s; Front wheel steering angle, unit: rad; This refers to the vehicle's wheelbase, in meters (m).

3. The autonomous vehicle trajectory tracking control method according to claim 2, characterized in that, The specific process of dynamically adjusting the prediction time domain of the nonlinear model prediction control through the fuzzy inference system in step S2 includes: Vehicle speed is divided into six fuzzy subsets, corresponding to extremely low speed, very low speed, low speed, medium speed, high speed, and extremely high speed, respectively, with the universe of discourse limited to 0 km / h to 100 km / h; a triangular membership function is used, and the center values ​​of the membership degree of each subset are set to 0, 20, 40, 60, 80, and 100 km / h respectively. The road curvature is divided into five fuzzy subsets, corresponding to minimum curvature, very small curvature, moderate curvature, large curvature, and maximum curvature, respectively, with the universe of discourse limited to 0m. - ¹ to 0.014m - ¹; A non-uniform triangular membership function is used, and the center values ​​of each subset with a membership degree of 1 are set to approximately 0, 0.0028, 0.007, 0.011, and 0.014m, respectively. - ¹; The prediction time domain is divided into six fuzzy subsets, corresponding to the very short time domain, very short time domain, short time domain, medium time domain, long time domain, and very long time domain, respectively. The universe of discourse is limited to integers between 10 and 40. The triangular membership function is used, and the center values ​​of the membership degree of each subset are set to 10, 16, 22, 28, 34, and 40 respectively. The prediction time domain is output according to the fuzzy configuration rules. The fuzzy configuration rules take into account both the real-time performance and predictability of the calculation. The configuration principle is: when the vehicle speed is higher and / or the curvature is greater, the prediction time domain is smaller to reduce the solution load of rolling optimization; conversely, the prediction time domain is larger to improve the smoothness of trajectory tracking.

4. The autonomous vehicle trajectory tracking control method according to claim 1, characterized in that, In step S3, the objective function is expressed as: in, To predict the time domain, To control the time domain, Here is the error weight matrix. To control the weight matrix, To control the incremental weight matrix, For state prediction based on time k The system state error at time 1. For state prediction based on time k Control parameters at any given time (including speed v and front wheel steering angle) ), For state prediction based on time k The control increment at any given time is the difference between the commands in two adjacent control cycles. The reference feedforward control quantity at time k.

5. The autonomous vehicle trajectory tracking control method according to claim 4, characterized in that, The actuator amplitude constraint includes vehicle speed constraint and front wheel steering angle constraint; the specific constraint formula is as follows: in, For state prediction based on time k Longitudinal vehicle speed at any given time, in m / s; and These are the minimum and maximum longitudinal vehicle speeds allowed by the vehicle's actuators, respectively, in m / s; For state prediction based on time k Front wheel steering angle at any given moment, unit: rad; and These are the minimum and maximum front wheel steering angles allowed by the vehicle's actuators, respectively, in rad.

6. The autonomous vehicle trajectory tracking control method according to claim 5, characterized in that: The control increment constraint is used to limit the rate of change of the control quantity between adjacent control cycles, and the constraint is as follows: in, Indicates the longitudinal speed increment between adjacent control cycles, in m / s; and These are the minimum and maximum permissible longitudinal speed increments, respectively, in m / s; The front wheel steering angle increment between adjacent control cycles is expressed in rad. and These are the minimum and maximum permissible front wheel steering angle increments, respectively, in rad.

7. The autonomous vehicle trajectory tracking control method according to claim 6, characterized in that, The vehicle lateral stability constraint is: Where L is the vehicle wheelbase, in meters (m); The maximum permissible lateral acceleration to ensure the lateral stability of a vehicle, in m / s². 2 ; For state prediction based on time k Front wheel steering angle at a given moment, unit: rad; when vehicle speed Permissible front wheel steering angle when raised It automatically contracts, thus forcing the vehicle to maintain a smaller steering angle at high speeds.

8. The autonomous vehicle trajectory tracking control method according to claim 1, characterized in that, In step S4, the sequential quadratic programming algorithm transforms the restricted nonlinear programming problem into a series of quadratic programming subproblems. Each iteration solves one quadratic programming subproblem to update the control sequence.

9. A trajectory tracking control system for an autonomous vehicle, characterized in that, include: The upper-level trajectory planning information module is used to provide geometric information of the reference path and the desired motion state; The fuzzy adaptive decision module is used to extract road curvature from the geometric information of the reference path, extract real-time longitudinal vehicle speed from the real-time vehicle operating status, and dynamically adjust the prediction time domain of the nonlinear model predictive control through the fuzzy inference system using the road curvature and the real-time longitudinal vehicle speed as dual input variables. The nonlinear rolling optimization module is used to calculate the tracking error based on the predicted time domain, the real-time operating state of the vehicle, and the desired motion state. It constructs an optimization problem based on a nonlinear kinematic error model and solves the optimization problem using a sequential quadratic programming algorithm to obtain the optimal control sequence. The objective function of the optimization problem includes weighted penalties for tracking error, control quantity, and control increment. The constraints include at least actuator amplitude constraints, control increment constraints, and vehicle lateral stability constraints for limiting the maximum safe front wheel steering angle under high-speed conditions. The vehicle status feedback module is used to output the first control quantity of the optimal control sequence to the vehicle actuator, and perform feedback correction based on the real-time operating status of the vehicle to form a closed-loop control.

10. A vehicle controller, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the autonomous vehicle trajectory tracking control method according to any one of claims 1 to 8.

Images