Adaptive trajectory tracking control method for autonomous vehicle

By employing a high-order strong tracking capacitive Kalman filter algorithm and singular value decomposition, the tire lateral force is estimated in real time and the lateral stiffness is corrected. This solves the problem that tire nonlinear characteristics cannot be updated in real time in existing technologies, and enables high-precision trajectory tracking control of autonomous vehicles under complex working conditions.

CN121995922APending Publication Date: 2026-05-08NORTHEAST GASOLINEEUM UNIV
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202610203193.3
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2026-02-12
Publication Date
2026-05-08

AI Technical Summary

Technical Problem

Existing model predictive control methods based on vehicle monorail models cannot update tire nonlinear characteristics in real time under large sideslip angle conditions, resulting in increased lateral displacement and heading angle errors, making it difficult to guarantee trajectory tracking and stability.

Method used

A high-order strong tracking capacitive Kalman filter algorithm combined with singular value decomposition and fading factor is used to estimate tire lateral force in real time and calculate lateral stiffness correction factor. An adaptive model predictive controller is constructed to dynamically adjust tire lateral stiffness parameters.

Benefits of technology

It improves the path tracking performance of autonomous vehicles under uncertain conditions, significantly reduces lateral displacement and heading angle errors, and enhances the accuracy and stability of trajectory tracking control.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN121995922A_ABST
    Figure CN121995922A_ABST
Patent Text Reader

Abstract

The invention relates to the technical field of automatic driving, in particular to a self-adaptive trajectory tracking control method for an automatic driving vehicle, and the method comprises the following steps: building a vehicle dynamics trajectory tracking model; based on the vehicle dynamics trajectory tracking model, a high-order strong tracking volume Kalman filtering algorithm is adopted to estimate the lateral force of the tire, and in the high-order strong tracking volume Kalman filtering algorithm, singular value decomposition is adopted to replace Cholesky decomposition, and a fading factor is introduced to dynamically adjust the filtering covariance; according to the tire lateral force obtained through estimation and the tire lateral force obtained through calculation based on a linear tire model, a cornering stiffness correction factor is calculated; and correcting a tire cornering stiffness parameter in a model prediction controller in real time by using the obtained cornering stiffness correction factor, and constructing a self-adaptive model prediction controller. According to the invention, the path tracking performance of the autonomous vehicle under the uncertain working condition can be improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and in particular to an adaptive trajectory tracking control method for autonomous vehicles. Background Technology

[0002] Lateral motion control of autonomous vehicles is one of the key technologies for achieving vehicle trajectory tracking and driving stability. In existing technologies, publicly available literature and patents generally employ Model Predictive Control (MPC) methods based on a single-track vehicle model for lateral control in addressing the trajectory tracking problem of autonomous vehicles. For example, in the MPC path tracking method published by Domina et al. in 2023, a brushed tire model was used to calculate lateral forces, and the effectiveness was verified in simulation and real-vehicle tests. This method determines the vehicle mass m and yaw inertia Iz by measurement, while the front and rear wheel lateral stiffness Cf,Cr are identified offline through hill-start assist tests. The paper points out that linear tire characteristics only hold true within a small sideslip angle range of 0–3.5° for the front wheels and 0–2° for the rear wheels. Beyond this range, the predictive model will produce significant errors due to neglecting tire nonlinearity. This indicates that this type of MPC controller fails to update the sideslip stiffness parameters in real time, limiting its applicability to large sideslip angle conditions.

[0003] Traditional MPC controllers based on fixed lateral stiffness have limitations in model accuracy and trajectory tracking performance: most methods rely on offline experimental calibration or assume constant lateral stiffness, failing to reflect tire nonlinear characteristics in real time; the few strategies employing online estimation are mostly based on linear models and do not adequately consider estimation uncertainties. Therefore, under complex or extreme conditions, the difference between the predicted model and the vehicle's actual dynamics increases significantly, leading to increased lateral displacement and heading angle errors, making trajectory tracking and stability difficult to guarantee. This background underscores the necessity of developing model predictive controllers with adaptive lateral stiffness capabilities. Summary of the Invention

[0004] This invention aims to overcome the shortcomings of existing technologies and provide an adaptive trajectory tracking control method for autonomous vehicles. It can improve the path tracking performance of autonomous vehicles under uncertain operating conditions.

[0005] The present invention also provides an apparatus and a storage medium for implementing an adaptive trajectory tracking control method for autonomous vehicles.

[0006] To solve the above-mentioned technical problems, the present invention is implemented as follows:

[0007] An adaptive trajectory tracking control method for autonomous vehicles includes the following steps:

[0008] (1) Establish a vehicle dynamic trajectory tracking model;

[0009] (2) Based on the vehicle dynamic trajectory tracking model, the lateral force of the tire is estimated by a high-order strong tracking capacitive Kalman filter algorithm. In the high-order strong tracking capacitive Kalman filter algorithm, singular value decomposition is used instead of Cholesky decomposition, and a fading factor is introduced to dynamically adjust the filter covariance.

[0010] (3) Calculate the lateral stiffness correction factor based on the tire lateral force estimated in step (2) and the tire lateral force calculated based on the linear tire model;

[0011] (4) Using the lateral stiffness correction factor obtained in step (3), the lateral stiffness parameter of the tire in the model predictive controller is corrected in real time to construct an adaptive model predictive controller in order to realize trajectory tracking control of autonomous vehicles.

[0012] Furthermore, the introduction of the fading factor in step (2) specifically includes:

[0013] Calculate the residual sequence based on the predicted system state, observed values, and system noise;

[0014] Based on the residual sequence, the residual covariance matrix is ​​calculated using a recursive algorithm with a forgetting factor;

[0015] The fading factor is calculated based on the residual covariance matrix, the system noise covariance matrix, the observation noise covariance matrix, and the state prediction covariance matrix, wherein the fading factor λ k+1 The calculation formula is: λ k+1 = max(1,tr(N k+1 ) / tr(M k+1 ), where N k+1 and M k+1 This is the intermediate calculation matrix.

[0016] Further, the calculation of the lateral stiffness correction factor in step (3) specifically involves: the lateral stiffness correction factor λ cf and λ cr Calculated using the following formula:

[0017]

[0018] F cf and F cr The lateral forces of the front and rear wheels are calculated based on a linear tire model.

[0019] Furthermore, the real-time correction value for the tire's lateral stiffness in step (4) is:

[0020] Among them, C cf and C cr The nominal front and rear wheel lateral stiffness.

[0021] Furthermore, the vehicle dynamic trajectory tracking model in step (1) is a three-degree-of-freedom vehicle dynamic model. The state variables of the model include the vehicle's longitudinal velocity, lateral velocity, yaw angle, yaw rate, longitudinal position and lateral position in the geodetic coordinate system.

[0022] Furthermore, the dynamic constraints of the model predictive controller in step (4) are set as follows:

[0023]

[0024] The constraint condition for the front wheel angle increment can be expressed as:

[0025]

[0026] The vehicle's front wheel steering angle constraint and front wheel steering angle increment constraint are set as follows:

[0027] .

[0028] An electronic device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the program, performs the steps of the method described above.

[0029] A computer-readable storage medium having a computer program stored thereon that, when executed by a processor, performs the steps of the method described above.

[0030] This invention proposes an adaptive model predictive control (AMPC) strategy to improve the path tracking capability of autonomous vehicles under uncertain conditions. Specifically, a high-order strong tracking capacitive Kalman filter algorithm based on singular value decomposition is used to estimate tire lateral forces. By calculating the difference between the linear tire force and the estimated tire force, a lateral stiffness correction coefficient is derived, thereby constructing an AMPC control strategy that can adapt to changes in lateral stiffness. To verify the effectiveness of the proposed scheme, CARSIM and MATLAB co-simulation was used. The parameters for the comparative experiments were kept consistent, and the vehicle parameters used were those of a Class C vehicle in CARSIM. Double lane change and single lane change scenarios were selected for verification. The final results showed that the proposed scheme was optimal in both cases. Attached Figure Description

[0031] Figure 1 This is a comparison chart of simulation results under the dual line shifting (DLC) working condition of the present invention.

[0032] Figure 2This is a comparison chart of simulation results under the single line shifting (SLC) condition of this invention.

[0033] Figure 3 This is a flowchart of the HCKF algorithm of the present invention.

[0034] Figure 4 This is a flowchart of the SVD-HSTCKF algorithm of the present invention. Detailed Implementation

[0035] The present invention will now be described in detail through specific embodiments. These embodiments are provided to enable a more thorough understanding of the invention and to fully convey the scope of the invention to those skilled in the art. As used throughout the specification and claims, the terms "comprising" or "including" are open-ended and are interpreted as "comprising but not limited to". The following description is a preferred embodiment for carrying out the invention; however, this description is intended to illustrate the general principles of the specification and is not intended to limit the scope of the invention. The scope of protection of the present invention is determined by the appended claims.

[0036] A vehicle dynamics trajectory tracking model is established, and a lateral force estimator is designed. The estimator is based on the High-Order Capable Kalman Filter (HCKF) algorithm, with improvements made as follows: 1. Singular value decomposition is introduced instead of Cholesky decomposition to enhance numerical stability and positive definiteness. 2. A fading factor is introduced in conjunction with strong tracking filter theory to dynamically adjust the filter's covariance, enabling the filter to force the tracking of rapid changes in the system state. This forms a High-Order Strong Tracking Capable Kalman Filter (SVD-HSTCKF) based on singular value decomposition. By calculating the difference between the linear tire force and the estimated tire force, a lateral stiffness correction factor is obtained, thus proposing an AMPC control strategy based on adaptive lateral stiffness.

[0037] In an autonomous vehicle trajectory tracking control system, the method of the present invention is employed, and the control method includes the following steps:

[0038] Step 1: Establish a vehicle dynamics trajectory tracking model;

[0039] Step 2: Improve the HCFK algorithm to form a lateral force estimator for SVD-HSTCKF;

[0040] Step 3: Calculate the lateral stiffness correction factor;

[0041] Step 4: AMPC controller;

[0042] Step 5: Joint simulation verification;

[0043] Step 1: Establish a vehicle dynamics trajectory tracking model.

[0044] This invention employs a three-degree-of-freedom vehicle dynamics model and combines it with geodetic coordinate system transformation to form a trajectory tracking dynamics model, as follows:

[0045]

[0046] in, Indicates vehicle mass. and These represent the distances from the vehicle's center of gravity to the front and rear axles, respectively. , Its derivatives represent the vehicle's longitudinal and lateral states and speed in its coordinate system. , and These represent the vehicle's yaw angle, yaw rate, and yaw acceleration, respectively. This represents the vehicle's moment of inertia. Indicates the front wheel steering angle. , These represent the longitudinal forces acting on the front and rear wheels, respectively. and These represent the lateral forces acting on the front and rear wheels, respectively.

[0047] The lateral and longitudinal forces of a tire can be simplified into a linear relationship, as shown in the following equation:

[0048]

[0049] in, For tire longitudinal stiffness. For slip ratio, These are tire lateral stiffness, It is the sideslip angle.

[0050] The lateral and longitudinal forces acting on a tire can be simplified to the following linear relationship:

[0051]

[0052] in and These represent the lateral stiffness of the tire. , Indicates the lateral angle.

[0053] The front and rear wheel slip angles are expressed as follows:

[0054]

[0055] The longitudinal slip ratio of the front and rear tires can be expressed as:

[0056]

[0057] in, The radius of the wheel's rolling motion. This represents the angular velocity of the wheel.

[0058] Furthermore, equation (1) can be expressed as:

[0059]

[0060] In the formula: Represents a state variable. Represents a state variable.

[0061] To meet the computational requirements, the above model is linearized. Assume the current state variable is... The control quantity is Then there is a state space:

[0062]

[0063] exist Performing a Taylor expansion on equation (1), we get:

[0064]

[0065] make The above formula can be written as:

[0066]

[0067]

[0068]

[0069]

[0070] in, , , .

[0071] There are some physical quantities in CarSim that provide physical values ​​for the simulation.

[0072] Step 2: This patent introduces singular value decomposition instead of Cholesky decomposition based on the high-order capacitive Kalman algorithm, enhancing numerical stability and positive definiteness. Furthermore, it incorporates a fading factor based on strong tracking filter theory, allowing dynamic adjustment of the filter's covariance to force the filter to track rapid changes in the system state. See also Figure 3 As shown, Figure 3 This shows the calculation process of HCKF.

[0073] First, it is assumed that the changes in the lateral forces of each tire during vehicle operation conform to a Markov process, and a second-order random walk model is used to describe these changes.

[0074]

[0075] In the formula: To estimate the lateral tire force, for The first derivative, It is random noise.

[0076] To simplify the dynamic equations in the lateral and yaw directions, we assume that the lateral forces of the front and rear tires, yaw rate, and lateral acceleration satisfy the following relationships:

[0077]

[0078] A nonlinear discrete system with added noise can be represented by the following equation:

[0079]

[0080] In the formula, and They represent The system state vector and measurement vector at time t, and the state vector is set as... Measurement vector , For system process noise, To measure the noise of the system, and , System initial state ,and , , They are unrelated. Assume it is known that... The posterior probability density of the system at any given time Therefore, the lateral force can be estimated according to the HCKF algorithm. The improvements are as follows:

[0081] The SVD decomposition is shown below:

[0082]

[0083] In the formula, , The column vectors are the variance matrices. The left and right singular vectors; The variance matrix The singular value matrix of . For ease of subsequent derivation, let . .

[0084] After introducing SVD decomposition, update the state integration points: The calculation method is as follows:

[0085]

[0086] in = , , superscript Indicates that it does not contain . Find the integration point Calculation method:

[0087]

[0088] In the formula, express dimensional unit vector and the th Each element is 1; .

[0089] and The calculation method is shown in equation (19), and the rules for taking the value of j are shown in equation (20).

[0090]

[0091] Introducing a fading factor The calculation process is as follows:

[0092]

[0093] In the formula, , This is system noise; The system noise was not considered because a fading factor was not introduced. The state error covariance matrix; , The state error covariance matrix and cross-covariance matrix are given without introducing a fading factor but considering system noise. , , , All are calculation parameter matrices of the fading factor; For residuals; This is the predicted state value; This is the forgetting factor, with a value of 1.

[0094] Calculate the state error covariance matrix including the decay factor:

[0095]

[0096] Singular value decomposition (SVD) is performed on the state error covariance matrix calculated using the attenuation factor:

[0097]

[0098] make The updated state volume point after the new measurement is calculated as follows:

[0099]

[0100] Next, based on the HCKF calculation process as follows: Figure 1 HCKF algorithm flowchart, we introduce : Calculate the integral points passed through the measurement equations , Measured predicted values , covariance matrix Measurement error covariance , Gain matrix K, weight w(i).

[0101]

[0102] Update the state estimate and the state error covariance matrix.

[0103]

[0104] Step 3: Calculate the lateral skewing stiffness correction factor

[0105] The lateral force of the tire can be estimated using the previous step. and This patent proposes an adaptive adjustment method based on the difference between the linear lateral force and the estimated value, wherein the lateral stiffness correction coefficient is defined as:

[0106]

[0107] Therefore, the correction value for tire lateral stiffness can be expressed as:

[0108]

[0109] C cf and C cr The nominal front and rear wheel lateral stiffness.

[0110] Step 4: AMPC Controller

[0111] When designing the AMPC controller, the equation (2) will be used. and Replace with and This allows us to obtain the continuous state equation for real-time correction of lateral stiffness. In practical applications, discretization is required; assuming the discretization step length is... Discretization using the forward Euler method yields:

[0112]

[0113] in , and . For simulating step size, .

[0114] By using discrete state vectors With control vector Combine and expand the state vector It can be represented as:

[0115]

[0116] Based on formulas (12) and (13), a new state-space expression can be derived:

[0117]

[0118] in, , ,and .

[0119] The predicted output of the iterative calculation system can be expressed as:

[0120]

[0121] To avoid getting stuck in local optima or becoming unsolvable, a relaxation factor is introduced, and the objective function can be expressed as:

[0122]

[0123] The constraints to be considered during the optimization process mainly include control input increment constraints, control quantity constraints, output constraints, and state constraints. The constraint on the front wheel angle control quantity can be expressed as:

[0124]

[0125] Step 5: Joint simulation verification

[0126] To verify the effectiveness of the proposed scheme, CARSIM and MATLAB co-simulation was used. The parameters for the comparative experiments were kept consistent, and the vehicle parameters were those of a Class C vehicle in CARSIM. Double lane change and single lane change scenarios were selected for verification. The final results showed that the proposed scheme was optimal in both cases.

[0127] See Figure 1 As shown, Figure 1 This is a simulation test of the double lane change (DLC) condition, with the road surface set as high-adhesion (DLC). =0.85), the vehicle speed is set to a constant speed of Vx=72km / h. For example Figure 1 As shown in (a), in a two-lane change scenario, all three control strategies can complete the path tracking task, while lateral displacement error (LDE) and heading angle error (HAE) are important indicators for evaluating tracking performance. Figure 1 (b) As shown in Table 1, the maximum lateral displacement error of the vehicle controlled by AMPC is 0.0616 m, which is 47% and 69% higher than that of MPC and LQR control, respectively. Under AMPC control, the mean absolute lateral error is 0.0112 m, which is 45% and 72% higher than that of MPC and LQR, respectively. Figure 1 (e) shows that the HAE eventually converges to zero for all control strategies. However, AMPC exhibits a smaller HAE and a faster convergence speed. The maximum heading angle error of the vehicle controlled by AMPC is 1.1008 degrees, representing a 61% and 63% improvement in control accuracy compared to MPC and LQR controls, respectively. This indicates that the AMPC controller has better tracking performance than the MPC and LQR controllers because the AMPC controller employs a lateral stiffness adaptive control strategy, which improves the model's accuracy. Figure 1 Tables (c) and (d) demonstrate the effectiveness of the SVD-HSTCKF estimator in estimating the lateral tire forces on the front and rear axles. Table 1 shows that the SVD-HSTCKF estimator exhibits high accuracy when faced with abrupt changes.

[0128] Table 1

[0129] Double line shift LDE Max (m) LDEAverage(m) LDERmse(m) HAE Max(deg) HAEAverage(deg) HAERmse(m) AMPC 0.0616 0.0112 0.0205 1.1008 0.1923 0.3263 MPC 0.1155 0.0202 0.0381 2.8166 0.4357 0.7830 LQR 0.1662 0.0404 0.0588 3.0049 0.436 0.8623

[0130] Figure 2 This is a simulation test of the single lane change (SLC) condition, with the road surface set as high-adhesion (…). =0.85), the vehicle speed is set to a constant speed of Vx=72km / h. For example Figure 2 As shown in (a), under the single-track path tracking condition, all three control strategies can complete the tracking task of the reference trajectory, while the lateral displacement error (LDE) and heading angle error (HAE) are the key indicators for evaluating tracking performance. Figure 2(b) and Table 2 show that the maximum lateral displacement error of the vehicle controlled by AMPC is 0.0945 m, which is about 22% and 53% higher than that of MPC (0.1213 m) and LQR (0.1995 m), respectively. In terms of average lateral error, the average absolute lateral error of AMPC is only 0.0105 m, which is about 56% and 80% lower than that of MPC (0.0237 m) and LQR (0.0535 m), respectively. The root mean square error of LDE is also about 33% and 51% lower than that of MPC and LQR, respectively, indicating that AMPC has a significant advantage in lateral tracking accuracy. Figure 2 (e) shows that HAE eventually converges to zero for all control strategies. Compared to MPC, AMPC reduces the maximum heading angle error from 7.2016° to 4.1904°, with a peak error reduction of approximately 42%, and the average and root mean square heading angle errors also decreasing by approximately 31%. Although LQR is slightly better than AMPC in terms of heading angle error, its lateral error is significantly increased. Overall, AMPC significantly reduces lateral displacement error while maintaining a smaller heading angle error, and its overall tracking performance is better than both MPC and LQR. This is attributed to the adaptive yaw stiffness update strategy introduced in AMPC, which improves model matching and control accuracy. Figure 2 (c) and (d) further present the estimation results of the SVD-HSTCKF estimator for the lateral tire forces of the front and rear axles. It can be seen that the estimated curves almost completely coincide with the true values, indicating that the estimator still has high estimation accuracy under sudden operating conditions, and provides reliable state information for the AMPC controller.

[0131] Table 2

[0132] Single line shift LDEMax(m) LDEAverage(m) LDERmse(m) HAE Max(deg) HAEAverage(deg) HAERmse(m) AMPC 0.0945 0.0105 0.0332 4.1904 0.5157 0.6462 MPC 0.1213 0.0237 0.0497 7.2016 0.7453 0.9344 LQR 0.1995 0.0535 0.0671 3.0056 0.441 0.5536

[0133] Table 3

[0134] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. An adaptive trajectory tracking control method for autonomous vehicles, characterized in that, Includes the following steps: (1) Establish a vehicle dynamic trajectory tracking model; (2) Based on the vehicle dynamic trajectory tracking model described in step (1), the tire lateral force is estimated by a high-order strong tracking capacitive Kalman filter algorithm. In the high-order strong tracking capacitive Kalman filter algorithm, singular value decomposition is used instead of Cholesky decomposition, and a fading factor is introduced to dynamically adjust the filter covariance. (3) Calculate the lateral stiffness correction factor based on the tire lateral force estimated in step (2) and the tire lateral force calculated based on the linear tire model; (4) Using the lateral stiffness correction factor obtained in step (3), the lateral stiffness parameter of the tire in the model predictive controller is corrected in real time to construct an adaptive model predictive controller in order to realize trajectory tracking control of autonomous vehicles.

2. The adaptive trajectory tracking control method for autonomous vehicles according to claim 1, characterized in that, The introduction of the fading factor in step (2) specifically includes: Calculate the residual sequence based on the predicted system state, observed values, and system noise; Based on the residual sequence, the residual covariance matrix is ​​calculated using a recursive algorithm with a forgetting factor; The fading factor is calculated based on the residual covariance matrix, the system noise covariance matrix, the observation noise covariance matrix, and the state prediction covariance matrix, wherein the fading factor λ k+1 The calculation formula is: λ k+1 = max(1, tr(N k+1 ) / tr(M k+1 ), where N k+1 and M k+1 This is the intermediate calculation matrix.

3. The adaptive trajectory tracking control method for autonomous vehicles according to claim 1 or 2, characterized in that, The calculation of the lateral stiffness correction factor in step (3) specifically involves: the lateral stiffness correction factor λ cf and λ cr Calculated using the following formula: ; F cf and F cr The lateral forces of the front and rear wheels are calculated based on a linear tire model.

4. The adaptive trajectory tracking control method for autonomous vehicles according to claim 3, characterized in that, The correction value for the real-time tire lateral stiffness parameter mentioned in step (4) is: ; Among them, C cf and C cr The nominal front and rear wheel lateral stiffness.

5. The adaptive trajectory tracking control method for autonomous vehicles according to claim 4, characterized in that, The vehicle dynamics trajectory tracking model mentioned in step (1) is a three-degree-of-freedom vehicle dynamics model. The state variables of the model include the vehicle's longitudinal velocity, lateral velocity, yaw angle, yaw rate, longitudinal position and lateral position in the geodetic coordinate system.

6. The adaptive trajectory tracking control method for autonomous vehicles according to claim 5, characterized in that, The constraint on the front wheel angle control quantity of the model predictive controller dynamic constraint in step (4) can be expressed as: 。 7. An electronic device 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 program, it implements the steps of the method as described in any one of claims 1 to 6.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps of the method as described in any one of claims 1 to 6.