Trajectory tracking control method, device and equipment for autonomous vehicle and storage medium

Abstract

The invention discloses an automatic driving vehicle trajectory tracking control method, device and equipment and a storage medium, and relates to the technical field of automatic driving vehicle control, and the method comprises the steps: obtaining environment information and vehicle state information, and determining a reference trajectory according to the environment information and the vehicle state information; calculating a tracking error according to the reference trajectory and the actual state of the vehicle; according to the tracking error, PID control is adopted to calculate longitudinal expected driving torque, the longitudinal expected driving torque is distributed to corresponding driving wheels of the vehicle, and four-wheel driving torque is obtained; a gain scheduling PD controller is adopted according to the tracking error, and an expected front wheel turning angle is obtained; and controlling the vehicle according to the four-wheel drive torque and the expected front wheel steering angle. The problem that parameter perturbation, time-varying characteristics and state quantity of the vehicle in a complex environment are difficult to obtain is solved, the adaptability of the automatic driving vehicle in a complex working condition is enhanced, and the implementation cost of the controller is reduced.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving car control, and particularly relates to an automatic driving car trajectory tracking control method and device, equipment and a storage medium. BACKGROUND

[0002] Trajectory tracking is one of the core tasks of automatic driving technology, and must meet the requirements of high precision and driving stability. The main task of trajectory tracking is to track the expected trajectory within a specified time according to the reference position information and speed information of the decision planning layer, and to minimize the lateral error and heading error, including the longitudinal vehicle speed following and the lateral path tracking two sub-tasks. Considering the complexity of longitudinal and lateral centralized control, most current researches decouple it, while meeting the lateral control target, taking into account the longitudinal control.

[0003] In actual vehicle driving, the dynamics of the vehicle itself has uncertainty, and the tire cornering stiffness will be perturbed. At the same time, the disturbance of the external environment is very significant, which will change the friction between the tire and the ground in a short time. In addition, the longitudinal vehicle speed is in a dynamic state of change at all times. This leads to a decrease in the accuracy of vehicle control and the driving stability of the vehicle. SUMMARY

[0004] The main purpose of the present application is to provide an automatic driving car trajectory tracking control method, device, equipment and storage medium, which aims to solve the technical problem of poor controller robustness caused by parameter uncertainty when the vehicle is driving.

[0005] To achieve the above purpose, the present application provides an automatic driving car trajectory tracking control method, which comprises: Obtaining environment information and vehicle state information, and determining a reference trajectory according to the environment information and the vehicle state information; Calculating a tracking error according to the reference trajectory and the actual state of the vehicle; Calculating a longitudinal expected driving torque by PID control according to the tracking error, and distributing the longitudinal expected driving torque to the corresponding driving wheels of the vehicle to obtain four-wheel driving torque; Obtaining an expected front wheel steering angle by gain scheduling PD controller according to the tracking error; Controlling the vehicle according to the four-wheel driving torque and the expected front wheel steering angle.

[0006] In an embodiment, the step of calculating a longitudinal expected driving torque by PID control according to the tracking error, and distributing the longitudinal expected driving torque to the corresponding driving wheels of the vehicle to obtain four-wheel driving torque comprises: obtaining a longitudinal vehicle speed error at a current time and a historical longitudinal vehicle speed error at a previous time according to the tracking error; obtaining a PID control coefficient, a first distance from a gravity center of the vehicle to a front axle of the vehicle, and a second distance from the gravity center of the vehicle to a rear axle of the vehicle; calculating a longitudinal desired drive torque according to the PID control coefficient, the longitudinal vehicle speed error, and the historical longitudinal vehicle speed error; distributing the longitudinal desired drive torque to corresponding drive wheels of the vehicle according to the first distance and the second distance to obtain four-wheel drive torques.

[0007] In an embodiment, the step of obtaining the desired front wheel angle according to the tracking error using a gain scheduling PD controller comprises: obtaining a lateral error and a yaw angle error according to the tracking error, and obtaining a yaw angular velocity; establishing a trajectory tracking dynamics model based on the lateral error, the yaw angle error, and the yaw angular velocity; transforming the trajectory tracking dynamics model based on a linear parameter variation strategy to obtain a trajectory tracking dynamics polytopic model; designing a gain scheduling PD controller of lateral output feedback based on the trajectory tracking dynamics polytopic model; solving the gain scheduling PD controller to obtain a target control law, and obtaining a desired front wheel angle according to the target control law and the tracking error.

[0008] In an embodiment, the step of establishing the trajectory tracking dynamics model based on the lateral error, the yaw angle error, and the yaw angular velocity comprises: obtaining a total vehicle mass, longitudinal forces and lateral forces on four wheels of the vehicle, a longitudinal speed, a lateral speed, a front wheel angle of the vehicle, and a yaw moment of inertia; establishing a dynamics balance equation according to the total vehicle mass, the longitudinal forces, the lateral forces, the longitudinal speed, the lateral speed, the front wheel angle, and the yaw moment of inertia; obtaining lateral disturbance forces, yaw disturbance forces, a path projection error, a reference path curvature, front and rear wheel effective cornering stiffnesses, front and rear wheel cornering angles, front and rear lateral tire forces, and a mass center cornering angle; simplifying the dynamics balance equation according to the lateral disturbance forces and the yaw disturbance forces to obtain a simplified dynamics balance equation; establishing a trajectory tracking model according to the yaw angular velocity, the lateral error, the yaw angle error, the longitudinal speed, the lateral speed, the path projection error, and the reference path curvature; A trajectory tracking dynamics model is obtained according to the relationship between the centric side slip angle and the front wheel steering angle, the relationship between the front and rear lateral tire forces and the front and rear wheel side slip angles, the simplified dynamics equilibrium equation, and the trajectory tracking model.

[0009] In an embodiment, the step of transforming the trajectory tracking dynamics model based on the linear parameter varying strategy to obtain a trajectory tracking dynamics polytopic model comprises: defining a time-varying parameter based on the linear parameter varying strategy and the lateral velocity; transforming the trajectory tracking dynamics model by the time-varying parameter to obtain a linear parameter varying state model; defining a range of the time-varying parameter according to a preset triangular enveloping curve; transforming the linear parameter varying state model based on the range to obtain a transformed linear parameter varying state model; transforming a coefficient matrix in the transformed linear parameter varying state model according to a polytopic vertex feature of tire side stiffness to obtain a transformed coefficient matrix; adjusting the transformed linear parameter varying state model according to the transformed coefficient matrix to obtain an adjusted linear parameter varying state model; constructing an evaluation index equation according to the lateral error, the yaw angle error, and a front wheel steering angle signal; obtaining a trajectory tracking dynamics polytopic model according to the adjusted linear parameter varying state model and the evaluation index equation.

[0010] In an embodiment, the step of designing a gain scheduling PD controller of lateral output feedback based on the trajectory tracking dynamics polytopic model comprises: transforming the trajectory tracking dynamics polytopic model to obtain a transformed trajectory tracking dynamics polytopic model; setting a control law of the gain scheduling PD controller of lateral output feedback according to a coefficient matrix and a filter constant; constructing a controller transfer function according to the control law, and converting the controller transfer function into a discrete state space model of the controller; defining an extended state variable; obtaining a closed loop system state equation based on the extended state variable, the discrete state space model of the controller, and the transformed trajectory tracking dynamics polytopic model; obtaining a closed loop system model according to the closed loop system state equation; designing a gain scheduling static output feedback controller based on time domain and frequency domain performance; constructing a gain scheduling PD controller of lateral output feedback according to the gain scheduling static output feedback controller and the closed loop system model.

[0011] In an embodiment, the step of solving the gain-scheduled PD controller to obtain a target control law and obtaining a desired front wheel angle from the target control law and the tracking error comprises: obtaining preset filter constants and stable region parameters; solving a coefficient matrix of a linear parameter varying system at each subsystem according to the preset filter constants and the stable region parameters; solving an initial static output feedback controller that makes each subsystem satisfy a stable region constraint condition according to the coefficient matrix; solving a linear matrix according to the initial static output feedback controller to obtain an initial coordinate transformation matrix; iteratively solving a linear matrix according to the initial coordinate transformation matrix to obtain a gain matrix; obtaining gain-scheduled PD controller parameters according to the gain matrix; solving the gain-scheduled PD controller by the gain-scheduled PD controller parameters to obtain a target control law and obtaining a desired front wheel angle from the target control law and the tracking error.

[0012] In addition, to achieve the above object, the present application further provides an automatic driving vehicle trajectory tracking control device, which comprises: an obtaining module, configured to obtain environment information and vehicle state information, and determine a reference trajectory according to the environment information and the vehicle state information; a calculating module, configured to calculate a tracking error according to the reference trajectory and an actual vehicle state; the calculating module is further configured to calculate a longitudinal expected driving torque by PID control according to the tracking error, and distribute the longitudinal expected driving torque to corresponding driving wheels of the vehicle to obtain four-wheel driving torques; the obtaining module is further configured to obtain a desired front wheel angle by a gain-scheduled PD controller according to the tracking error; a control module, configured to control the vehicle according to the four-wheel driving torques and the desired front wheel angle.

[0013] In addition, to achieve the above object, the present application further provides an automatic driving vehicle trajectory tracking control device, which comprises: a memory, a processor, and a computer program stored in the memory and capable of running on the processor, the computer program being configured to implement the steps of the automatic driving vehicle trajectory tracking control method as described above.

[0014] In addition, to achieve the above object, the application further provides a storage medium, which is a computer readable storage medium, and a computer program is stored on the storage medium, and the computer program is executed by a processor to implement the steps of the automatic driving vehicle trajectory tracking control method.

[0015] In addition, to achieve the above object, the application further provides a computer program product, which comprises a computer program, and the computer program is executed by a processor to implement the steps of the automatic driving vehicle trajectory tracking control method.

[0016] The one or more technical solutions provided by the application have at least the following technical effects: 1) By acquiring the environment and vehicle state information to determine the reference trajectory, and calculating the tracking error, and then using PID control and gain scheduling PD controller to calculate the longitudinal expected driving torque and expected front wheel angle, the precise control of the vehicle is finally realized. Effectively solve the problem of reducing the steering precision and driving stability caused by the uncertainty of vehicle dynamics characteristics, significant external environment interference and dynamic change of longitudinal vehicle speed, etc. Significantly improve the precision and stability of the automatic driving vehicle trajectory tracking. By adjusting the front wheel angle through the gain scheduling PD controller, real-time adjustment can be made according to the error and dynamic environment during vehicle driving, so that the steering is more accurate. Gain scheduling control can dynamically adjust the parameters of the controller according to different driving conditions and environmental conditions, so that the control system is more flexible and adaptable. This adaptability can achieve better control effect under different driving environments, especially in the actual driving conditions that change frequently, to ensure the stability and safety of the vehicle behavior.

[0017] 2) By using a model based on lateral error, yaw angle error and yaw rate, the controller can adaptively adjust the gain in different vehicle states, improving the adaptability of the vehicle to different road conditions and vehicle driving states. The trajectory tracking dynamics model is converted into a trajectory tracking dynamics polytope model through a linear parameter variation strategy, which can more accurately reflect the behavior of the vehicle in different states. The design of the polytope model makes the control strategy more detailed and accurate, effectively improving the performance of the controller under various driving conditions. By considering the nonlinear characteristics of the vehicle in different states, the trajectory tracking is more stable and reliable. By establishing a gain scheduling PD controller for lateral output feedback, the vehicle can adjust the front wheel angle in real time according to the current driving state to more accurately respond to the instantaneous changes in the environment. This process not only improves the handling performance of the vehicle on curves or complex road conditions, but also enhances the robustness of the control system, allowing the vehicle to drive stably under dynamic road conditions. Using the gain scheduling PD controller ensures that the control system can respond promptly and reasonably when facing unexpected situations, avoiding traffic accidents caused by inaccurate steering or slow response. This real-time control capability is particularly important for autonomous driving or driving assistance systems, significantly improving the safety of vehicles in complex environments. BRIEF DESCRIPTION OF DRAWINGS

[0018] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and serve to explain the principles of the present application together with the specification.

[0019] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the accompanying drawings needed to be used in the embodiments or prior art description will be briefly introduced. Obviously, for those skilled in the art, other drawings can also be obtained based on these drawings without creative labor.

[0020] Figure 1 Flowchart provided for Embodiment One of the trajectory tracking control method of the autonomous vehicle of the present application; Figure 2 Flowchart provided for Embodiment Two of the trajectory tracking control method of the autonomous vehicle of the present application; Figure 3 Three-degree-of-freedom vehicle model provided for Embodiment One of the trajectory tracking control method of the autonomous vehicle of the present application; Figure 4 Trajectory tracking model provided for Embodiment One of the trajectory tracking control method of the autonomous vehicle of the present application; Figure 5 Vehicle speed range provided for Embodiment One of the trajectory tracking control method of the autonomous vehicle of the present application; Figure 6A side slip stiffness range diagram provided for an embodiment of the trajectory tracking control method for an autonomous vehicle of the present application; Figure 7 A weight change trend diagram with vehicle speed provided for an embodiment of the trajectory tracking control method for an autonomous vehicle of the present application; Figure 8 An algorithm flow diagram for solving a gain scheduling PD controller provided for an embodiment of the trajectory tracking control method for an autonomous vehicle of the present application; Figure 9 A trajectory tracking control architecture diagram provided for an embodiment of the trajectory tracking control method for an autonomous vehicle of the present application.

[0021] The object implementation, functional features and advantages of the present application will be further described with reference to the embodiments and the accompanying drawings. DETAILED DESCRIPTION

[0022] It should be understood that the specific embodiments described herein are merely intended to explain the technical solutions of the present application, and are not intended to limit the present application.

[0023] In order to better understand the technical solutions of the present application, the specific embodiments will be described in detail below with reference to the drawings and the accompanying drawings.

[0024] The main solution of the embodiment of the present application is: obtaining environment information and vehicle state information, and determining a reference trajectory according to the environment information and the vehicle state information; calculating a tracking error according to the reference trajectory and the actual state of the vehicle; calculating a longitudinal desired driving torque using PID control according to the tracking error, and distributing the longitudinal desired driving torque to the corresponding drive wheels of the vehicle to obtain four-wheel drive torque; obtaining a desired front wheel steering angle using a gain scheduling PD controller according to the tracking error; and controlling the vehicle according to the four-wheel drive torque and the desired front wheel steering angle.

[0025] Since the related research on trajectory tracking of autonomous vehicles in the prior art mainly focuses on conventional scenarios such as medium-low speed and good road surface, the vehicle and tire dynamics models used are simplified, and there are parameter uncertainties in the actual driving process of the vehicle, which will cause obvious model errors and bring challenges to the accuracy and stability of vehicle trajectory tracking. In terms of lateral control strategy selection, most controllers use state feedback, but the vehicle's center of mass side slip angle and lateral speed are difficult to obtain through simple and low-cost methods, and the use of multi-sensor information fusion estimation is costly, and the tracking performance depends on the accuracy of state estimation.

[0026] The present application provides a solution to solve the problems of parameter perturbation, time-varying and difficult to obtain state quantity of the vehicle in complex environment, enhance the adaptability of autonomous vehicles in complex working conditions, and reduce the implementation cost of the controller.

[0027] It should be noted that the execution subject of the embodiment can be a computing service device with data processing, network communication and program running functions, such as a tablet computer, a personal computer, a mobile phone, etc., or an electronic device capable of realizing the above functions, an automatic driving car trajectory tracking control device, etc. The following takes the automatic driving car trajectory tracking control device as an example to describe the embodiment and the following embodiments.

[0028] Based on this, the application provides an automatic driving car trajectory tracking control method, which refers to Figure 1 , Figure 1 The flowchart of the first embodiment of the automatic driving car trajectory tracking control method of the application is shown in the figure.

[0029] In the embodiment, the automatic driving car trajectory tracking control method includes steps S10-S50: Step S10: Obtain environment information and vehicle state information, and determine a reference trajectory according to the environment information and the vehicle state information.

[0030] It should be noted that the environment information is the external environment information of the current driving of the automatic driving vehicle, and the vehicle state information can include vehicle speed, acceleration, steering angle, yaw rate and other information reflecting the current motion state of the vehicle. In actual application, these environment information and vehicle state information can be obtained through various sensors mounted on the vehicle, such as camera, radar, gyroscope, accelerometer, etc. After obtaining these information, the reference trajectory suitable for the current environment and vehicle state is determined by using the pre-set algorithm and model, such as path planning algorithm based on map data, trajectory generation model combined with vehicle dynamics characteristics, etc. The reference trajectory is the ideal path that the vehicle expects to drive.

[0031] Step S20: Calculate the tracking error according to the reference trajectory and the actual state of the vehicle.

[0032] After determining the reference trajectory, the actual driving state of the vehicle is compared with the reference trajectory. By measuring the position deviation of the current actual position of the vehicle and the corresponding point on the reference trajectory, the lateral error is obtained; by measuring the deviation of the actual yaw angle of the vehicle and the expected yaw angle at the corresponding point on the reference trajectory, the yaw angle error is obtained; at the same time, the actual yaw rate of the vehicle can also be obtained. The tracking error includes vehicle speed error, lateral error and yaw angle error, which reflects the deviation degree between the actual driving of the vehicle and the expected trajectory, and is an important basis for subsequent control adjustment.

[0033] Step S30: Calculate the longitudinal expected driving torque by PID control according to the tracking error, and distribute the longitudinal expected driving torque to the corresponding driving wheels of the vehicle to obtain four-wheel driving torque.

[0034] PID control is a widely used and mature control algorithm, which calculates the control amount according to the proportional, integral and differential of the tracking error. In this application, for longitudinal control, the longitudinal expected driving torque is calculated by the PID control algorithm according to the calculated tracking error such as lateral error and yaw angle error. The longitudinal expected driving torque is the expected driving force size of the vehicle in the longitudinal direction. After obtaining the longitudinal expected driving torque, considering that the vehicle is usually four-wheel drive, the longitudinal expected driving torque needs to be reasonably distributed to the four drive wheels according to the driving characteristics, load distribution and other factors of the vehicle, so as to obtain the four-wheel drive torque of each drive wheel, so as to realize the accurate control of the longitudinal power of the vehicle.

[0035] In a possible implementation, step S30 can include steps A11-A14: Step A11: obtaining the longitudinal vehicle speed error at the current time and the historical longitudinal vehicle speed error at the last time according to the tracking error; It should be noted that the tracking error includes the vehicle speed error, specifically including the longitudinal vehicle speed error at the current time T and the historical longitudinal vehicle speed error at the last time .

[0036] It can be understood that the longitudinal vehicle speed error can be calculated from the current longitudinal vehicle speed and the reference longitudinal vehicle speed corresponding to the reference trajectory, and the longitudinal vehicle speed error is calculated as follows: - Formula 1 In the above formula 1, is the longitudinal vehicle speed error, is the current longitudinal vehicle speed, is the reference longitudinal vehicle speed.

[0037] Step A12: obtaining the PID control coefficient, the first distance from the center of gravity of the vehicle to the front axle of the vehicle and the second distance from the center of gravity of the vehicle to the rear axle of the vehicle; In a specific implementation, the PID control coefficient includes , and . The first distance from the center of gravity of the vehicle to the front axle of the vehicle is l f , and the second distance from the center of gravity of the vehicle to the rear axle of the vehicle is l r .

[0038] Step A13: calculating the longitudinal expected driving torque according to the PID control coefficient, the longitudinal vehicle speed error and the historical longitudinal vehicle speed error; In a specific implementation, the longitudinal PID controller has a small number of parameters, and the trial-and-error method is directly used to set the parameters to obtain the longitudinal desired driving torque T v , which is calculated as shown in Equation 2 below: + Equation 2 In Equation 2, is the longitudinal desired driving torque, T is the sampling time, 、 and are PID control coefficients.

[0039] Step A14: distributing the longitudinal desired driving torque to the corresponding driving wheels of the vehicle according to the first distance and the second distance to obtain four-wheel driving torques.

[0040] In a specific implementation, the longitudinal desired driving torque calculated in Equation 2 above can be distributed to the four driving wheels according to the front and rear axle loads to obtain four-wheel driving torques, which are calculated as shown in Equation 3 below: Equation 3 In Equation 3, T i is the driving torque of each wheel, T fl is the driving torque of the left front wheel, T fr is the driving torque of the right front wheel, T rl is the driving torque of the left rear wheel, T rr is the driving torque of the right rear wheel.

[0041] Step S40: obtaining a desired front wheel steering angle by using a gain scheduling PD controller according to the tracking error.

[0042] For lateral control of the vehicle, a gain scheduling PD controller is used to calculate a desired front wheel steering angle according to the lateral error and the yaw angle error in the tracking error calculated above. The gain scheduling PD controller can dynamically adjust the gain parameters of the controller according to different driving conditions and error conditions. Through this dynamic adjustment mechanism, the controller can more accurately calculate the desired front wheel steering angle according to the current driving state and error size of the vehicle. This desired front wheel steering angle is the front wheel steering angle required for the vehicle to accurately track the reference trajectory and achieve stable lateral driving.

[0043] Step S50: controlling the vehicle according to the four-wheel driving torques and the desired front wheel steering angle.

[0044] After obtaining the four-wheel drive torque and the desired front wheel angle, the automatic driving vehicle trajectory tracking control device sends these control instructions to the actuators of the vehicle, such as the motor control system and the steering control system, etc. The actuators of the vehicle accurately adjust the longitudinal driving force and the front wheel steering angle of the vehicle according to these instructions, so that the vehicle can accurately track the reference trajectory in the actual driving process, realize stable and accurate automatic driving trajectory tracking control, effectively cope with various working conditions under complex environment, and improve the safety and reliability of the automatic driving vehicle.

[0045] The embodiment provides an automatic driving vehicle trajectory tracking control method, which determines a reference trajectory by obtaining environment and vehicle state information, calculates a tracking error, and then calculates a longitudinal desired driving torque and a desired front wheel angle by using a PID control and a gain scheduling PD controller respectively, so as to finally realize accurate control of the vehicle. The embodiment effectively solves the problem of reduced steering accuracy and driving stability caused by vehicle dynamics uncertainty, significant external environment interference and dynamic changes in longitudinal vehicle speed, and significantly improves the accuracy and stability of automatic driving vehicle trajectory tracking. By adjusting the front wheel angle through the gain scheduling PD controller, the steering can be more accurate in real time according to the error and dynamic environment during vehicle driving. Gain scheduling control can dynamically adjust the parameters of the controller according to different driving conditions and environmental conditions, so that the control system is more flexible and adaptable. This adaptability can achieve better control effect under different driving environments, especially in actual driving conditions that change frequently, to ensure the stability and safety of vehicle behavior.

[0046] Based on the first embodiment of the present application, in the second embodiment of the present application, the same or similar contents as the above-mentioned first embodiment can be referred to the above introduction, and will not be described in detail. On this basis, please refer to Figure 2 , step S40 includes steps S401-S405: Step S401: obtaining a lateral error and a yaw angle error according to the tracking error, and obtaining a yaw angular velocity.

[0047] It should be noted that the tracking error also includes a lateral error and a yaw angle error, so the lateral error and the yaw angle error can be obtained according to the tracking error, and the yaw angular velocity can also be obtained through the gyro sensor and other sensors mounted on the vehicle. The lateral error reflects the deviation of the actual position of the vehicle from the reference trajectory in the lateral direction, the yaw angle error reflects the deviation of the actual yaw angle of the vehicle from the desired yaw angle at the corresponding point on the reference trajectory, and the yaw angular velocity reflects the angular velocity of the vehicle rotating around the vertical axis. These parameters are crucial for accurately calculating the desired front wheel angle.

[0048] Step S402: establishing a trajectory tracking dynamics model based on the lateral error, the yaw angle error and the yaw angular velocity.

[0049] In a specific implementation, a vehicle trajectory tracking dynamics model can be established based on the lateral error, yaw angle error and yaw angular velocity, which can describe the dynamic characteristics of the vehicle during lateral movement, and the establishment process fully considers the dynamic characteristics of the vehicle and various influencing factors in the actual driving process. Through this model, the movement trend of the vehicle under different error conditions can be more accurately analyzed, providing a reliable basis for the subsequent development of control strategies. For example, the model involves the influence of factors such as the steering system characteristics of the vehicle, the friction between the tire and the ground, and other factors on lateral movement, so that the model can more truly reflect the lateral movement state of the vehicle.

[0050] In a feasible implementation, step S402 can include steps B11-B16: Step B11: obtaining the vehicle mass, the longitudinal force and lateral force acting on the four wheels of the vehicle, the longitudinal velocity, the lateral velocity, the front wheel angle of the vehicle, and the yaw moment of inertia; The vehicle mass, the longitudinal force and lateral force acting on the four wheels of the vehicle, the longitudinal velocity, the lateral velocity, the front wheel angle of the vehicle, and the yaw moment of inertia are basic data for establishing the trajectory tracking dynamics model. The vehicle mass affects the inertia characteristics of the vehicle, the longitudinal force and lateral force reflect the interaction between the vehicle and the ground, the longitudinal velocity and lateral velocity describe the movement state of the vehicle in the plane, the front wheel angle is a direct control quantity of the vehicle steering, and the yaw moment of inertia is related to the rotational stability of the vehicle. These parameters can be obtained through various sensors mounted on the vehicle, such as force sensors, speed sensors, angle sensors, etc.

[0051] Step B12: establishing a dynamics balance equation based on the vehicle mass, the longitudinal force, the lateral force, the longitudinal velocity, the lateral velocity, the front wheel angle, and the yaw moment of inertia; For the trajectory tracking problem of an autonomous vehicle, in order to facilitate the design of the controller, the model needs to be simplified: (1) ignore the influence of suspension vertical action; (2) ignore the vehicle vertical movement, pitch movement and roll movement; (3) do not consider air resistance; (4) only consider the longitudinal, lateral and yaw movement of the vehicle. A three-degree-of-freedom vehicle model is shown in Figure 3 The following dynamics balance equation is established:

[0052] Formula 4 In formula 4 above, m is the vehicle mass; F yi and F xi are the lateral forces and longitudinal forces acting on the four wheels, respectively, ); is the yaw rate; v x is the longitudinal velocity, v y is the lateral velocity; δ is the front wheel steering angle; is the yaw moment of inertia.

[0053] Step B13: Obtain the lateral disturbance force, the yaw disturbance force, the path projection error, the reference path curvature, the front and rear wheel effective cornering stiffness, the front and rear wheel cornering angle, the front and rear lateral tire force, and the mass center cornering angle; It should be noted that the lateral disturbance force d F is the lateral force caused by external disturbances such as road unevenness and crosswind, and the yaw disturbance force d M is the yaw moment caused by external disturbances, and the path projection error d S is the arc length direction projection error from the current position of the vehicle to the nearest point on the reference trajectory, i.e., the longitudinal deviation; the reference path curvature ρ(d) is the curvature of the reference trajectory at the current lateral position d, and the front and rear wheel effective cornering stiffness k f and k r are the elastic stiffness of the front and rear wheel tire contact areas when deformed in the lateral direction, and the front and rear wheel cornering angle α f and α r is the angle between the actual driving direction of the wheel and the wheel plane, and the front and rear lateral tire force F yf and F yr is the force received by the front and rear wheels in the lateral direction, and the mass center cornering angle β is the angle between the mass center velocity direction of the vehicle and the longitudinal axis of the vehicle. These parameters are crucial for accurately describing the force and motion state of the vehicle during lateral motion, and can provide key data support for further improvement of the trajectory tracking dynamics model.

[0054] Step B14: Simplify the dynamics balance equation according to the lateral disturbance force and the yaw disturbance force to obtain a simplified dynamics balance equation; When the current wheel steering angle is small, formula 4 can be simplified as follows: Formula 5 In formula 5, m is the mass of the vehicle, d F is the lateral disturbance force, is the yaw rate; v x is the longitudinal velocity,v y is the lateral velocity, is the yaw moment of inertia, F yi and F xi are the lateral and longitudinal forces on the four wheels, respectively, l f is the first distance from the center of gravity of the vehicle to the front axle, d M is the yaw disturbance force.

[0055] Step B15: establishing a trajectory tracking model according to the yaw angular velocity, the lateral error, the yaw angular error, the longitudinal velocity, the lateral velocity, the path projection error, and the reference path curvature; assuming that the heading error is small during trajectory tracking, defining e y is the lateral deviation, e φ is the yaw angular deviation, as shown in Figure 4 Figure 4 is a schematic diagram of the trajectory tracking model, and the trajectory tracking model is established as follows: Equation 6 In equation 6, is the lateral error rate of change, is the yaw angular error, is the actual yaw angular velocity, is the heading angular error rate of change, is the path projection error, is the reference path curvature, is the tangent angle rate of change of the reference path.

[0056] Step B16: obtaining a trajectory tracking dynamics model according to the relationship between the mass center side slip angle and the front wheel steering angle, the relationship between the front and rear lateral tire forces and the front and rear wheel side slip angles, the simplified dynamics balance equation, and the trajectory tracking model.

[0057] When the vehicle front wheel steering angle δ is small and the tire is in a linear working area, the side slip angles of the left and right wheels can be approximately considered to be equal, at this time, the relationship between the front and rear lateral tire forces and the front and rear wheel side slip angles is represented as follows: Equation 7 In equation 7, and are the front and rear wheel effective side slip stiffnesses, respectively, and are the front and rear wheel side slip angles, respectively, and ​respectively, and have and = The center of mass side slip angle may be expressed as equation 8: Equation 8 Considering the small front wheel angle assumption and the left and right wheel side slip angle equal assumption, the relationship between the front and rear lateral tire forces and the front and rear wheel side slip angles can be expressed as: Equation 9 Selecting , combined with the above equation 7, equation 9, equation 5 and equation 6, we get: Equation 10 In equation 10, is a trajectory tracking dynamics model, is a state vector, is a disturbance vector, containing all external disturbance terms, is a control input vector, containing only front wheel angle, A is a system matrix, is a disturbance matrix, is a control matrix. In equation 10, A, and are expressed as equation 11: Equation 11 Step S403: transforming the trajectory tracking dynamics model based on the linear parameter variation strategy to obtain a trajectory tracking dynamics polytopic model.

[0058] In actual driving scenarios, the longitudinal vehicle speed will change, and and Therefore, the present scheme adopts a linear parameter varying (LPV) strategy to handle the time-varying nature of vehicle speed, i.e., the trajectory tracking dynamics model is transformed by the LPV method, thereby obtaining a polytopic model of trajectory tracking dynamics. Specifically, the LPV strategy regards the vehicle speed as a time-varying parameter, and decomposes the nonlinear system into multiple linear subsystems around this parameter, each of which corresponds to the approximate linear behavior of the vehicle speed within a certain range. These linear subsystems are fused in the form of parameter-dependent convex combination to form a polytopic model covering the entire range of vehicle speed changes. The model takes the vehicle speed as the scheduling variable, and the system matrix, input matrix and disturbance matrix in the state space expression are all expressed as affine functions of the vehicle speed, thereby significantly reducing the complexity of controller design while ensuring model accuracy. Through LPV transformation, the polytopic model of trajectory tracking dynamics can reflect the influence of vehicle speed changes on vehicle lateral motion in real time, providing an accurate dynamic description for subsequent robust control strategy design based on the polytopic model.

[0059] In a feasible implementation, step S403 can include steps B21-B28: Step B21: defining a time-varying parameter based on a linear parameter varying strategy and the lateral velocity; It should be noted that the time-varying parameter can be defined according to the linear parameter varying strategy and the lateral velocity, as shown in the following formula 12: Formula 12 In formula 12, the longitudinal vehicle speed and its reciprocal are defined as two time-varying parameters to form a vector In the state equation, appears in the denominator, causing the system to be nonlinear. By introducing , the nonlinear term can be "linearized" into a parameter-dependent form; the coefficients in the subsequent model will be expressed as affine functions of , thereby forming an LPV model.

[0060] Step B22: transforming the trajectory tracking dynamics model by the time-varying parameter to obtain a linear parameter varying state model; It can be understood that the trajectory tracking dynamics model can be transformed by the time-varying parameter to obtain a linear parameter varying state model, as shown in the following formula 13: Formula 13 In formula 13, is a state vector including lateral error, heading error and yaw rate, is a control input, i.e., front wheel steering angle,​ External disturbances For parameter-dependent The system matrix, and The input matrix is ​​usually a constant. This can be expressed as Equation 14: Formula 14 In Equation 14, all adoption numbers are 1 and The affine function of 2, therefore the entire system satisfies the LPV structure, when v x When changing, It also changes accordingly, but always remains in a state of flux. Within the determined parameter space.

[0061] Step B23: Define the range of the time-varying parameters according to the preset triangle enclosing curve; parameter The value changes with vehicle speed on the curve l : The above changes, in order to reduce the conservatism of the controller design, such as Figure 5 As shown, Figure 5 This is a diagram illustrating the vehicle speed range. Figure 5 The region enclosed by the triangle PRM in the diagram represents the range of values ​​for the time-varying parameter.

[0062] Step B24: Transform the linear parameter change state model based on the range to obtain the transformed linear parameter change state model; In the diagram, the line connecting point R (x2, y2) and point P (x1, y1) is tangent to curve l at point P, and the line connecting point R and point M (x3, y3) is tangent to curve l at point M. Therefore, we can obtain: Formula 15 Curve l describes 1 and The relationship between 2 and 3, in actual operation, v x ∈[vmin, vmax], therefore p The trajectory lies on this hyperbola. In order to construct a multicellular model, the continuous parameter space needs to be discretized into a finite number of vertices. In this embodiment, a triangular PRM region is used to surround this curve segment, thereby avoiding the direct use of a non-dimensional set.

[0063] Any point Q on curve l enclosed by a triangle can be uniquely determined by a linear combination of vertices P, R, and M, as shown in Equation 16: Formula 16 Point P is at the minimum vehicle speed Value, point R is the harmonic mean point, located at the midpoint of the curve, and point M is the value at maximum speed. Value. Triangle PRM is a convex hull containing the curve segment, and the entire LPV model can be represented as a convex combination of these three vertex systems. In Equation 16, it is represented that any point Q = ( p 1, p 2) The triangle PRM can be uniquely represented as the weighted sum of its three vertices P, R, and M. η i ( p ) is the weighting coefficient, each η i yes p The function.

[0064] According to the area method, the weight of each point can be calculated as follows: Formula 17 Equation 17 is used to calculate the barycentric coordinates of a point in the plane relative to the triangle. Each weight is equal to the area of ​​the sub-triangle formed by the target point and its two vertices, divided by the area of ​​the entire triangle, where represents S. QRM Let S represent the area of ​​the triangle formed by point Q, R, and M, corresponding to η1, S QPM Let S represent the area of ​​the triangle formed by points Q, P, and M, corresponding to η², S QPR Let S represent the area of ​​the triangle formed by points Q, P, and R, corresponding to η3. PRM Let represent the area of ​​the entire triangle PRM, which is a constant.

[0065] The linear parameter change state model can then be transformed into a transformed linear parameter change state model, as shown in Equation 18: Formula 18 For any point Q on the plane and the vertices P, R, and M of the triangle, we have: Q = η1P + η2R + η3M. Therefore, we can obtain the final transformation linear parameter change state model.

[0066] A in Equation 18 i Equation 19 is expressed as follows: Formula 19 Step B25: Transform the coefficient matrix in the transformed linear parameter change state model according to the multi-cell vertex characteristics of the tire lateral stiffness to obtain the transformed coefficient matrix; Unlike longitudinal vehicle speed, which can be measured, tire lateral stiffness cannot be measured in real time, resulting in significant uncertainty. This is addressed by analyzing the vertex characteristics of the multicellular matrix to cover the entire parameter variation range. Assuming the left and right wheels have the same lateral stiffness, the coefficient matrix in Equation 18 can be transformed into: Equation 20 Equation 20 is a further decomposition of the linear parameter-varying state model coefficient matrix Equation 18 by expressing the system matrix as a linear combination of three parts, E oi is the base term without uncertain parameters, E ai is the coefficient matrix related to the front cornering stiffness k f , E bi is the coefficient matrix related to the rear cornering stiffness k r , k f and k r are unknown but bounded parameters, which can be treated as uncertain parameters and handled by polytopic method.

[0067] In Equation 20, E oi , E ai and E bi is expressed as Equation 21 as follows: Equation 21 wherein, x i is the value of the 1st vertex p 1, y i is the value of the 1st vertex p 2, k f is the front cornering stiffness, k r is the rear cornering stiffness.

[0068] Step B26: adjusting the transformed linear parameter-varying state model according to the transformed coefficient matrix to obtain an adjusted linear parameter-varying state model; As shown in Figure 6 , Figure 6 is a cornering stiffness range diagram, the cornering stiffness varies in the range of Figure 6 the area enclosed by the rectangle INFT in the middle, then the matrix expression at each vertex is: Equation 22 In Equation 22, the vertices I, N, F, T correspond to the serial numbers 1, 2, 3, and 4, respectively. Equation 22 is the final vertex system of the polytopic model, which has four vertices (I, N, F, T) corresponding to four extreme combinations of the tire cornering stiffness.

[0069] Considering the tire cornering stiffness uncertainty, the system state equation is a polytopic model, and the subsystem matrix at each speed point is represented by the four vertices as follows:

[0070] Equation 23 Equation 23 is an adjusted linear parameter variation state model, wherein Co{.} represents a convex hull, i.e., a linear combination of all vertex systems; for any actual k f , k r combination, there is a set of weights ≥0, such that: Therefore, the entire system can be regarded as a convex combination of the four vertex systems, and each is k f , k r a function of the actual stiffness variation.

[0071] Step B27: constructing an evaluation index equation according to the lateral error, the yaw angle error, and a front wheel steering angle signal; In specific implementation, a key evaluation index can be constructed, which is expressed as follows: Equation 24 In Equation 24, z is a controlled output, which is used to measure the performance of the controller, and is an output matrix of the evaluation index, which is expressed as follows: Equation 25 There are many difficulties in measuring the lateral velocity. Therefore, the lateral deviation, the yaw angle deviation, and the yaw angle velocity are selected as the measurement outputs, which are expressed as follows: Equation 26 In Equation 26, .

[0072] Step B28: obtaining a trajectory tracking dynamics polytopic model according to the adjusted linear parameter variation state model and the evaluation index equation.

[0073] In specific implementation, an LPV polytopic dynamics model considering the longitudinal vehicle speed time-varying property and the tire cornering stiffness uncertainty can be obtained according to the adjusted linear parameter variation state model, the evaluation index equation, and the measurement outputs, which is expressed as: Equation 27 The model can be discretized by using a zero-order holder method, and a discrete model can be obtained, which is shown in the following formula 28. Formula 28 Formula 28 is a discrete-time system model, which is suitable for digital controller implementation, and the state variable x ( k )= , the control input u ( k )= δ ( k ), that is, the front wheel steering angle, the output y ( k ) is a measurable signal, z ( k ) is a performance index.

[0074] Step S404: Design a gain-scheduled PD controller of lateral output feedback based on the trajectory tracking dynamics polytopic model.

[0075] In a specific implementation, the gain-scheduled PD controller of lateral output feedback can be designed according to the trajectory tracking dynamics polytopic model considering multiple performance constraints. The controller can adjust the control parameters in real time according to the change of vehicle speed, ensuring accurate trajectory tracking of the vehicle at different speeds. Specifically, the gain-scheduled PD controller monitors the vehicle speed signal online, dynamically adjusts the proportional and differential control parameters using a preset scheduling function, so that the controller focuses on stability at low speed and response speed at high speed. At the same time, the controller also introduces an output feedback mechanism, which measures the actual output signals such as lateral deviation and yaw angle deviation of the vehicle, and adjusts the control input in real time, thereby effectively overcoming the influence of model uncertainty, external disturbance and other factors on the trajectory tracking accuracy. In addition, in order to further improve the robustness of the controller, multiple performance constraints can be integrated into the controller design, such as limiting the amplitude and rate of change of the control input, optimizing the dynamic response characteristics of the system, etc., to ensure that the controller can operate stably and reliably under various complex working conditions.

[0076] In a feasible implementation, step S404 can include steps B31-B38: Step B31: Transform the trajectory tracking dynamics polytopic model to obtain a transformed trajectory tracking dynamics polytopic model; It can be understood that, for the convenience of formula derivation, the following can be allowed: Formula 29 Then the trajectory tracking dynamics polytopic model, that is, formula 28, can be transformed into the following formula 30: Formula 30 Step B32: Construct the control law of the gain-scheduled PD controller according to the coefficient matrix and the filter constant, and convert the control law into a discrete state space model of the controller; It can be understood that the coefficient matrix is and , The control law of the gain-scheduled PD controller is:

[0077] Formula 31 In formula 31, is a first-order filter used to smooth the differential term and prevent noise amplification, when → 0, it tends to be a pure differential term.

[0078] Step B33: Construct a controller transfer function according to the control law, and convert the controller transfer function into a discrete state space model of the controller; Considering that the vehicle speed is easy to measure and the tire cornering stiffness is not easy to measure, the parameters of the gain-scheduled PD controller only depend on the vehicle speed. If the vehicle speed is constant and the cornering stiffness parameter is perturbed, the same controller is used within the perturbation range, and the following formula is obtained: Formula 32 In formula 32, each , is a fixed gain corresponding to the i-th speed point (P, R, M), and the weight is calculated by the area method η i ( p ) to realize gain scheduling, when the vehicle speed changes, the controller automatically adjusts the gain to adapt to different working conditions.

[0079] Then, a controller transfer function is constructed according to the control law, and is converted into a discrete state space model of the controller, which is represented as follows: Formula 33 In formula 33, , is the internal state of the controller, is the measured output, is the control output, the parameters of the gain-scheduled PD controller only depend on the measurable longitudinal vehicle speed, gain scheduling is realized by using the vertex gain and the weight formula online, real-time estimation of the tire cornering stiffness is avoided, the practicability and robustness of the system are significantly improved, and the following formula is obtained: Formula 34 Formula 34 is a specific expression of the state space matrix of the controller, is the controller output feedback matrix, is the direct feedforward gain, controller input gain, controller state transition matrix.

[0080] Step B34: define the augmented state variable; In implementation, the augmented state variable can be defined as the following equation 35: Equation 35 Step B35: obtain the closed-loop system state equation based on the augmented state variable, the discrete state space model of the controller, and the transformed trajectory tracking dynamics polytopic model; Based on equation 35, equation 33 is brought into equation 30, and the following equation 36 is obtained: Equation 36 Through equation 36, the controlled object state x(k) and the controller state x c (k) are merged into a new state vector, , is the coefficient matrix of the closed-loop system, which is determined by the original system and the controller.

[0081] wherein, , indicates the standard structure of the closed-loop system matrix, based on the state feedback connection.

[0082] Step B36: obtain the closed-loop system model according to the closed-loop system state equation; In implementation, the coefficient matrix in the closed-loop system state equation of equation 36 can be rewritten as: Equation 37 Equation 37 is an affine decomposition of the closed-loop system matrix, which is used to support LPV analysis, wherein, , , , is a fixed matrix determined by the system and the controller structure, =[ ] is the controller gain matrix.

[0083] Thus, the design problem of the gain-scheduled PD controller can be converted into the system: Equation 38 Equation 38 is the final closed-loop system model, and the control input u ( k ) is still reserved as an independent input.

[0084] Step B37: design the gain-scheduled static output feedback controller based on time domain and frequency domain performance; Equation 39 In equation 39, This indicates control input, such as the front wheel steering angle. Depends on vehicle speed ρ=v x The feedback gain matrix, for each K i It is a fixed gain matrix corresponding to the i-th velocity point (P, R, M). η i ( p The weights are calculated using the area method and vary with the current vehicle speed.

[0085] The design problem of a Static Output Feedback (SOF) controller using Equation 39, where:

[0086]

[0087]

[0088]

[0089] Formula 40 The above is a further reconstruction of Equation 36, with the aim of transforming it into standard SOF form. For the performance output matrix, This is for measuring the output matrix.

[0090] From equations 40, 39, 34, and 32, we can see that: Formula 41 like Figure 7 As shown, Figure 7 This is a diagram illustrating the trend of weight changes with vehicle speed. The value of varies with vehicle speed, and Equation 41 is the gain matrix. K ( p ) explicit decomposition, each K i The fixed gain corresponding to the i-th velocity point, η i ( p The weight is determined by the current vehicle speed; therefore, K ( p ) is a convex combination of three fixed gain matrices.

[0091] Taking into account both time-domain and frequency-domain performance, the application of time-domain pole placement and frequency-domain... Control and design a gain-scheduled SOF controller.

[0092] Step B38: Construct a gain-scheduled PD controller of lateral output feedback according to the gain-scheduled static output feedback controller and the closed-loop system model.

[0093] From the time domain, the eigenvalues of the coefficient matrix of the closed-loop system formula 36 A cl The sufficient and necessary condition that all the eigenvalues of the coefficient matrix of the closed-loop system formula 36 are located in the specific region D(a, b) is that there exists a positive definite matrix which satisfies the following inequality: Formula 42 Formula 42 is an LMI condition for region pole placement, which is used to ensure that the closed-loop system is stable and has good performance. D(a, b) is a circular region with (a, 0) as the center and b as the radius, and it also satisfies .

[0094] Define the transfer function from the disturbance input to the control output in the closed-loop system formula 36 as , then its norm is represented as . The norm represents the system's ability to suppress disturbances.

[0095] From the frequency domain, for any positive real number > 0, the closed-loop system is stable and . The sufficient and necessary condition for is that there exists a positive definite matrix which satisfies the following inequality: Formula 43 Formula 43 is derived from the expression of , Formula 37. It can be seen that both formula 42 and formula 43 inequalities are bilinear matrix inequalities (BMI), and their solutions are non-convex problems. The coordinate transformation matrix method is used to solve them. Q > 0 is a positive definite matrix, and Z ∈ is the variable to be solved, which is related to the controller gain.

[0096] If the system measurement output matrix C y is a row full-rank matrix, then there exists a non-singular matrix ∈ which satisfies the following equation: Formula 44 That is, through coordinate transformation, the output matrix is changed into a combination of unit matrix and zero matrix.

[0097] Then we have the coordinate transformation matrix: Formula 45 in, yes Moore-Penrose reverse, yes Orthogonal basis of orthogonal complement space, T c1 ∈ It is a constant matrix, T c2 ∈ It is a non-singular outlier matrix used to construct transformations; therefore, It is a matrix consisting of the output matrix and its orthogonal complement. Combining quadratic stability theory, the state equations of the closed-loop system satisfy the time-domain D-stability constraints and the frequency-domain... Sufficient condition for performance: for any positive real number If the value is greater than 0, the closed-loop system's state equations are quadratic stable, and all its coefficient matrices are... The eigenvalues ​​are located in the region D(a, b) and simultaneously satisfy the following conditions: < A sufficient condition is that there exists a positive definite matrix. ,matrix k=1,2, and Let i = 1, 2, 3, j = 1, 2, 3, 4 satisfy the following inequality: Formula 46 Formula 47 Equation 46 is the LMI condition for the stability of region D, ensuring the closed-loop system matrix. A cl The eigenvalues ​​are located in ( Within a circular region with center (a, 0) and radius (b),... Let be the feedback gain at the i-th velocity point. Equation 47 is... The LMI condition for performance restricts the transfer function from the disturbance input ω(k) to the control input z(k). T zω The infinite norm, It is a positive definite matrix, usually taken as the identity matrix.

[0098] in: Formula 48 Equation 48 represents the coordinate transformation of the system matrix, the purpose of which is to convert the system to its standard form, facilitating the application of classical control theory. T yk The coordinate transformation matrix is ​​derived from the output matrix. C yThe gain-scheduled SOF controller vertex parameter expression is therefore constructed as: Equation 49 Equation 49 is the explicit expression of the gain-scheduled SOF controller vertex parameter, each K i is the fixed gain matrix corresponding to the ith speed point (P, R, M), is an intermediate variable obtained by solving the LMI, from the sub-block in equation 46, thus, K i is directly calculated by linear algebra operations. The gain-scheduled PD controller parameters at the vertex can be further obtained by equation 49 and equation 41, thereby obtaining the gain-scheduled PD controller of lateral output feedback.

[0099] Step S405: Solving the gain-scheduled PD controller to obtain the target control law, and obtaining the expected front wheel steering angle according to the target control law and the tracking error.

[0100] In a specific implementation, the gain-scheduled PD controller of lateral output feedback designed above is applied to the trajectory tracking control of an autonomous vehicle. According to the current vehicle speed p , the gain matrix K ( p ) is calculated by equation 41, which is a convex combination of three fixed gain matrices K i , each K i corresponding to a specific speed point (P, R, M), and the weight ηi ( p ) varies with the vehicle speed. Then, through equation 49 and related derivation, the parameters of the gain-scheduled PD controller at the vertex are determined, and the controller model is completely constructed. In each control period, according to the vehicle state information such as position, speed, heading angle, etc. measured by the sensor, as well as the expected trajectory information, the trajectory tracking error is calculated. These error signals are taken as the input of the controller, and after the operation of the gain-scheduled PD controller, the expected front wheel steering angle control command is output. The control command acts on the steering actuator of the vehicle, so that the vehicle travels along the expected trajectory, and at the same time, the controller dynamically adjusts the gain matrix according to the real-time change of the vehicle speed, to ensure good trajectory tracking performance at different vehicle speeds, and to realize stable and accurate trajectory tracking control of the autonomous vehicle under complex working conditions.

[0101] In a feasible implementation, step S405 can include steps B41-B46: Step B41: Obtain the preset filter constants and stable region parameters; It should be noted that using sufficient conditions to solve the problem will introduce conservatism into the controller. Under the condition of satisfying the D stability constraint, its conservatism can be reduced by solving the following problem.

[0102] That is, find the coordinate transformation matrix. , This makes the solution to the following convex optimization problem... The minimum is 50, as shown in the following formula:

[0103] Formula 50 Therefore, preset filter constants can be selected. And the parameters of the stable region, that is, the parameters of the stable region D(a,b).

[0104] Step B42: Solve for the coefficient matrix of the linear parameter variation system at each subsystem based on the preset filter constants and the stable region parameters; In practical implementation, the coefficient matrix of the LPV system with parameter uncertainty at each subsystem can be calculated according to Equation 38 and Equation 40 above.

[0105] Step B43: Solve for the initial static output feedback controller that satisfies the stability region constraints of each subsystem based on the coefficient matrix; The NM-HS algorithm can be used to obtain the initial static output feedback controller that satisfies the D stability constraint for each subsystem. That is, satisfying , .

[0106] Step B44: Solve for the linear matrix based on the initial static output feedback controller to obtain the initial coordinate transformation matrix; In practical implementation, an initial static output feedback controller can be used. ,by Let k be variables, k=1,2, solve the following LMIs problems:

[0107] Formula 51 Through iterative optimization of the original LMI problem, Equation 42 represents the regional D stability, and Equation 43 represents the H∞ performance. By introducing initial values, the convergence speed can be accelerated, and the local optimal solution under these initial values ​​can be obtained. Let Equation 45... The initial coordinate transformation matrix can be obtained. T y .

[0108] Step B45: Iteratively solve the initial coordinate transformation matrix for the linear matrix to obtain the gain matrix; The gain matrix can be obtained by iteratively solving the linear matrix using the initial coordinate transformation matrix. Specifically, the calculation... ,make ,as well as , T y It is a non-singular matrix constructed from the output matrix.

[0109] make ,by Given variables, solve the following LMIs problems:

[0110] Formula 52 Equation 52 is the nth LMI solution in the iterative optimization process, and each iteration is based on the current coordinate transformation matrix T. cl,k The system matrix is ​​updated with the goal of minimizing γ∞ while satisfying the regional stability of D and the performance constraints of H∞. This is achieved by continuously adjusting T. cl,k It can gradually approach the optimal solution.

[0111] calculate .if or ,calculate Proceed to the next step. Otherwise, let Then return to equation 52 to calculate the final gain matrix.

[0112] The iterative coordinate transformation matrix is ​​represented as: . N k Used for updating the coordinate transformation matrix during the iteration process. To allow a maximum number of iterations, Specify the tolerance. For example... Figure 8 As shown, Figure 8 To obtain the algorithm flowchart for solving the gain-scheduled PD controller problem, the coefficient matrix of the LPV system at each subsystem is determined. The gain-scheduled PD controller problem is transformed into a SOF design problem. The initial controller is obtained using the NM-HS hybrid search. Based on the initial controller Solving the LMIs problem yields Q k , Z k Let n=0, using the initial... T y calculate ;based on solving the problem LMIs to obtain , , Y i , calculating N k , judging whether or , if yes, calculating , Y i K i , further calculating K pi , K di , if no, setting , and returning to the step of solving the problem LMIs to obtain , , , Y i .

[0113] Step B46: obtaining gain scheduling PD controller parameters according to the gain matrix; In a specific implementation, the gain scheduling PD controller parameters can be calculated according to the gain matrix and formula 41, that is, K pi and K di .

[0114] Step B47: solving the gain scheduling PD controller by using the gain scheduling PD controller parameters to obtain a target control law, and obtaining an expected front wheel steering angle according to the target control law and the tracking error.

[0115] It can be understood that after the gain scheduling PD controller parameters are obtained, the target control law can be solved according to in formula 31, and the final expected front wheel steering angle can be calculated based on the target control law and the tracking error.

[0116] As shown in formula 32, Figure 9 Figure 9 ​​​The trajectory tracking control architecture for this embodiment includes a trajectory tracking controller module, a vehicle state perception module, a desired trajectory planning module, and a steering execution module. The trajectory tracking controller module serves as the core processing unit, responsible for receiving real-time data from the vehicle state perception module, such as the current position, speed, and heading angle of the vehicle, and simultaneously obtaining the desired trajectory information provided by the desired trajectory planning module. Based on this information, the trajectory tracking controller module uses the gain-scheduled PD controller parameters obtained in the previous step to perform real-time operation and processing, generating the desired front wheel steering angle control command. The vehicle state perception module continuously monitors the vehicle state through a high-precision sensor network, ensuring the accuracy and real-time nature of the data, and providing reliable input for the trajectory tracking controller. The desired trajectory planning module plans the optimal driving trajectory based on the pre-set route information, traffic conditions, and vehicle dynamics, providing a target guide for trajectory tracking control. The steering execution module receives the front wheel steering angle control command from the trajectory tracking controller module and converts it into actual vehicle steering actions through precise steering mechanisms, enabling the vehicle to travel stably and accurately along the desired trajectory. Through the close cooperation between the modules, the entire trajectory tracking control architecture achieves efficient and safe trajectory tracking control of the autonomous vehicle under complex working conditions.

[0117] This embodiment uses a model based on lateral error, yaw angle error, and yaw rate to control the gain adaptively under different vehicle dynamics, improving the vehicle's adaptability to different road conditions and driving styles. The trajectory tracking dynamics model is converted into a trajectory tracking dynamics polytope model through a linear parameter variation strategy, which can more accurately reflect the behavior of the vehicle under different states. The design of the polytope model makes the control strategy more detailed and accurate, effectively improving the performance of the controller under various driving conditions. By considering the nonlinear characteristics of the vehicle under different states, the trajectory tracking becomes more stable and reliable. By establishing a gain-scheduled PD controller with lateral output feedback, the vehicle can adjust the front wheel steering angle in real time according to the current driving state to more accurately respond to instantaneous changes in the environment. This process not only improves the handling performance of the vehicle on curves or complex road conditions, but also enhances the robustness of the control system, enabling the vehicle to travel stably under dynamically changing road conditions. Using a gain-scheduled PD controller ensures that the control system can respond promptly and reasonably when facing unexpected situations, avoiding traffic accidents caused by inaccurate steering or slow response. This real-time control capability is particularly important for autonomous driving or driving assistance systems, significantly improving the safety of vehicles in complex environments.

[0118] It should be noted that the above examples are only used for understanding the present application and do not constitute a limitation on the automatic driving vehicle trajectory tracking control method of the present application, and more forms of simple transformation based on the technical concept are within the protection scope of the present application.

[0119] The above is only some embodiments of the present application, and does not limit the patent scope of the present application, and any equivalent structural transformation made by using the contents of the present application specification and drawings, or direct / indirect application in other related technical fields are included in the patent protection scope of the present application.

Claims

1. A trajectory tracking control method for an autonomous vehicle, characterized in that, The autonomous vehicle trajectory tracking and control method includes: Acquire environmental information and vehicle status information, and determine a reference trajectory based on the environmental information and vehicle status information; The tracking error is calculated based on the reference trajectory and the actual state of the vehicle. The longitudinal desired driving torque is calculated using PID control based on the tracking error, and then the longitudinal desired driving torque is distributed to the corresponding drive wheels of the vehicle to obtain the four-wheel drive torque. The desired front wheel steering angle is obtained by using a gain-scheduled PD controller based on the tracking error. The vehicle is controlled based on the four-wheel drive torque and the desired front wheel steering angle.

2. The method as described in claim 1, characterized in that, The step of calculating the longitudinal desired driving torque using PID control based on the tracking error, and distributing the longitudinal desired driving torque to the corresponding drive wheels of the vehicle to obtain the four-wheel drive torque includes: Based on the tracking error, the longitudinal vehicle speed error at the current moment and the historical longitudinal vehicle speed error at the previous moment are obtained; Obtain the PID control coefficients, the first distance from the vehicle's center of gravity to the front axle, and the second distance from the vehicle's center of gravity to the rear axle; The desired longitudinal driving torque is calculated based on the PID control coefficients, the longitudinal vehicle speed error, and the historical longitudinal vehicle speed error. Based on the first distance and the second distance, the longitudinal desired driving torque is distributed to the corresponding drive wheels of the vehicle to obtain the four-wheel drive torque.

3. The method as described in claim 1, characterized in that, The step of obtaining the desired front wheel steering angle by using a gain-scheduled PD controller based on the tracking error includes: Based on the tracking error, the lateral error and yaw angle error are obtained, and the yaw rate is acquired. A trajectory tracking dynamics model is established based on the lateral error, the yaw angle error, and the yaw angular velocity. The trajectory tracking dynamics model is transformed based on a linear parameter variation strategy to obtain a multi-cell model of trajectory tracking dynamics. Based on the aforementioned trajectory tracking dynamics multicellular model, a gain-scheduled PD controller with lateral output feedback is designed. The gain-scheduled PD controller is solved to obtain the target control law, and the desired front wheel steering angle is obtained based on the target control law and the tracking error.

4. The method as described in claim 3, characterized in that, The steps for establishing a trajectory tracking dynamics model based on the lateral error, the yaw angle error, and the yaw angular velocity include: The system obtains the vehicle's mass, longitudinal and lateral forces on the four wheels, longitudinal velocity, lateral velocity, front wheel steering angle, and yaw moment of inertia. A dynamic equilibrium equation is established based on the vehicle mass, longitudinal force, lateral force, longitudinal velocity, lateral velocity, front wheel steering angle, and yaw moment of inertia. Acquire lateral disturbance force, yaw disturbance force, path projection error, reference path curvature, effective lateral stiffness of front and rear wheels, lateral slip angle of front and rear wheels, lateral tire force of front and rear wheels, and lateral slip angle of the center of gravity; The kinetic equilibrium equations are simplified based on the lateral disturbance force and the yaw disturbance force to obtain simplified kinetic equilibrium equations. A trajectory tracking model is established based on the yaw rate, the lateral error, the yaw rate error, the longitudinal velocity, the lateral velocity, the path projection error, and the reference path curvature. Based on the relationship between the center of gravity sideslip angle and the front wheel steering angle, the relationship between the front and rear lateral tire forces and the front and rear wheel sideslip angles, the simplified dynamic balance equation, and the trajectory tracking model, a trajectory tracking dynamic model is obtained.

5. The method as described in claim 3, characterized in that, The step of transforming the trajectory tracking dynamics model based on a linear parameter variation strategy to obtain a multicellular trajectory tracking dynamics model includes: Based on the linear parameter variation strategy and the time-varying parameters defined by the lateral velocity; The trajectory tracking dynamics model is transformed by the time-varying parameters to obtain a linear parameter change state model. The range of the time-varying parameters is defined according to the preset triangle enclosing curve; Based on the range, the linear parameter change state model is transformed to obtain the transformed linear parameter change state model. The coefficient matrix in the transformed linear parameter change state model is transformed based on the multi-cell vertex characteristics of tire lateral stiffness to obtain the transformed coefficient matrix. The transformation linear parameter change state model is adjusted according to the transformation coefficient matrix to obtain the adjusted linear parameter change state model; An evaluation index equation is constructed based on the lateral error, the yaw angle error, and the front wheel steering angle signal. Based on the adjusted linear parameter change state model and the evaluation index equation, a trajectory tracking dynamics multicellular model is obtained.

6. The method as described in claim 3, characterized in that, The steps for designing a gain-scheduled PD controller with lateral output feedback based on the trajectory tracking dynamics multicell model include: The trajectory tracking dynamics multicell model is transformed to obtain the transformed trajectory tracking dynamics multicell model; The control law of the PD controller with output feedback is set according to the coefficient matrix and filter constants. Construct the controller transfer function based on the control law, and convert the controller transfer function into a discrete state-space model of the controller; Define extended state variables; The closed-loop system state equation is obtained based on the extended state variables, the discrete state-space model of the controller, and the multi-cell model of the transformation trajectory tracking dynamics. The closed-loop system model is obtained based on the state equations of the closed-loop system. Design a gain-scheduled static output feedback controller based on time-domain and frequency-domain performance; A gain-scheduled PD controller with lateral output feedback is constructed based on the gain-scheduled static output feedback controller and the closed-loop system model.

7. The method according to any one of claims 3 to 6, characterized in that, The steps of solving the gain-scheduled PD controller to obtain the target control law, and obtaining the desired front wheel steering angle based on the target control law and the tracking error, include: Obtain the preset filter constants and stable region parameters; Solve the coefficient matrix of the linear parameter variation system at each subsystem based on the preset filter constants and the stable region parameters; The initial static output feedback controller that satisfies the stability region constraints of each subsystem is obtained by solving the coefficient matrix. The initial coordinate transformation matrix is ​​obtained by solving the linear matrix based on the initial static output feedback controller. The initial coordinate transformation matrix is ​​iteratively solved against the linear matrix to obtain the gain matrix; The gain-scheduling PD controller parameters are obtained based on the gain matrix. The gain scheduling PD controller is solved by the parameters of the gain scheduling PD controller to obtain the target control law, and the desired front wheel steering angle is obtained according to the target control law and the tracking error.

8. An autonomous vehicle trajectory tracking and control device, characterized in that, The device includes: The acquisition module is used to acquire environmental information and vehicle status information, and determine a reference trajectory based on the environmental information and vehicle status information; The calculation module is used to calculate the tracking error based on the reference trajectory and the actual state of the vehicle; The calculation module is also used to calculate the desired driving torque based on the tracking error using PID control, and to distribute the longitudinal desired driving torque to the corresponding drive wheels to obtain the four-wheel drive torque; The acquisition module is further configured to use a gain-scheduled PD controller to obtain the desired front wheel steering angle based on the tracking error; A control module is used to control the vehicle based on the four-wheel drive torque and the desired front wheel steering angle.

9. An autonomous vehicle trajectory tracking and control device, characterized in that, The device includes: a memory, a processor, and a computer program stored in the memory and executable on the processor, the computer program being configured to implement the steps of the autonomous vehicle trajectory tracking control method as described in any one of claims 1 to 7.

10. A storage medium, characterized in that, The storage medium is a computer-readable storage medium, and a computer program is stored on the storage medium. When the computer program is executed by a processor, it implements the steps of the autonomous vehicle trajectory tracking control method as described in any one of claims 1 to 7.

Images