Automatic driving transverse and longitudinal trajectory tracking control method based on LQR and double PID

Through the trajectory tracking control method combined with LQR and dual PID, the vehicle position is estimated in real time and the front wheel angle and vehicle acceleration are optimized, which solves the steady-state error and insufficient steering problems of autonomous vehicles in trajectory tracking control, achieving higher trajectory tracking accuracy and real-time performance.

CN120559997APending Publication Date: 2025-08-29JIANGSU UNIV OF TECH
View PDF 2 Cites 0 Cited by

Patent Information

Application Number
CN202510649554.2
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2025-05-20
Publication Date
2025-08-29

AI Technical Summary

Technical Problem

In the prior art, there are problems such as steady-state errors in trajectory tracking control, insufficient steering caused by the steering characteristics of the vehicle due to its own mechanical structure, and inaccurate calculation of the vehicle throttle opening and tire output torque, resulting in poor real-time and dynamic response efficiency.

Method used

The trajectory tracking control method based on LQR and dual PID is adopted, and the vehicle posture parameters are estimated in real time through the SCKF algorithm, combined with five-order polynomial fitting and lane line constraints, a dual PID regulator is designed to realize lateral and longitudinal control, and the front wheel angle and vehicle acceleration are optimized by feedforward control and PID steering compensator to improve the trajectory tracking accuracy and safety.

Benefits of technology

It effectively solves the problems of steady-state error and insufficient steering, improves the trajectory tracking accuracy and real-time performance, and ensures the safe and stable driving of the vehicle in complex environments.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN120559997A_ABST
    Figure CN120559997A_ABST
Patent Text Reader

Abstract

The invention belongs to the technical field of automatic driving, and particularly relates to an automatic driving transverse and longitudinal trajectory tracking control method based on LQR and double PID, and the method comprises the steps: firstly estimating the pose parameter of a current vehicle in real time through an SCKF algorithm in a trajectory planner, setting a lane line constraint, and determining the planning trajectory information through the estimation parameter in combination with quintic polynomial fitting; then, an LQR-double-PID transverse and longitudinal trajectory tracking algorithm is adopted, a double-PID regulator is designed, an LQR controller is combined, the front wheel turning angle of the vehicle is calculated from the aspect of transverse control, and the driving torque / braking force of the vehicle is calculated from the aspect of longitudinal control; trajectory tracking control of the vehicle is achieved through the combined effect of the transverse controller and the longitudinal controller. Feedforward control and steering compensation are introduced from the outside, the steady-state error of the system is reduced, the corresponding movement torque / braking force of the tire is rapidly output, and the real-time performance of trajectory tracking is improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving, and specifically relates to an autonomous driving lateral and longitudinal trajectory tracking control method based on LQR and dual PID. Background Art

[0002] With the development of autonomous vehicles (intelligent electric vehicles, IEVs), trajectory tracking control technology has become a key indicator of autonomous driving performance. It enables the vehicle to follow a planned path without human control, minimizing the deviation between the actual path and the target path to ensure stable and safe driving. This technology primarily involves lateral and longitudinal control. Lateral control precisely tracks the target trajectory by controlling the steering wheel angle or steering wheel angle. Longitudinal control adjusts the driving and braking mechanisms to achieve target values ​​for parameters such as the controlled vehicle's speed and acceleration. Therefore, maximizing trajectory tracking accuracy during intelligent driving is a key goal of this technology.

[0003] To further improve the trajectory tracking accuracy of autonomous vehicles, Chinese patent document CN114384915A provides an "Improved Path Tracking Control Method Based on LQR." This invention discloses an improved path tracking control method based on LQR. This method uses a fuzzy controller to dynamically adjust the Q and R matrices in the LQR controller based on lateral and heading deviation values. By solving the optimization problem, the required front wheel steering angle is obtained, improving the vehicle's trajectory tracking accuracy in practical applications. However, in terms of lateral control of the vehicle, this invention does not consider the steady-state error caused by the fact that the motion error and the rate of change of the error cannot be simultaneously zero when the control system is operating smoothly. Chinese patent document CN115857511A provides "A wheeled mobile robot trajectory tracking considering slippage". The invention discloses an LQR control method for wheeled mobile robot trajectory tracking considering slippage. By considering tire slippage, a reference model is established on the basis of the LQR algorithm, and a fixed-time convergence interference observer is designed to estimate the slippage interference, thereby improving the anti-interference performance of the control system. The control method of the invention can ensure high-precision control of the system with a smaller amount of calculation and ensure that the vehicle can be controlled under slippage conditions. However, in terms of longitudinal control of the vehicle, the invention does not consider the vehicle's own rotation as a mechanical structure connection, resulting in an understeering problem caused by the mismatch between the actual vehicle angle value and the system output.

[0004] In the above invention, in terms of vehicle trajectory tracking control, the steady-state error in the control system, the understeering caused by the steering characteristics of the vehicle's own mechanical structure, and the poor real-time performance caused by the calculation of the vehicle's throttle opening and tire output torque are not taken into consideration. Therefore, there are defects in the efficiency and accuracy of dynamic response. Summary of the Invention

[0005] The purpose of the present invention is to provide an automatic driving lateral and longitudinal trajectory tracking control method based on LQR and dual PID, which is used to solve the technical problems existing in the prior art control system, such as steady-state error, understeering caused by the steering characteristics of the vehicle's own mechanical structure, inaccurate calculation of the vehicle's throttle opening and tire output torque, resulting in poor real-time performance and defects in the efficiency and accuracy of dynamic response.

[0006] The described LQR and dual PID-based lateral and longitudinal trajectory tracking control method for autonomous driving first uses the SCKF algorithm in the trajectory planner to estimate the current vehicle's posture parameters in real time, sets lane line constraints, and determines the planned trajectory information by combining the estimated parameters with quintic polynomial fitting; then adopts an LQR-dual PID lateral and longitudinal trajectory tracking algorithm, designs a dual PID regulator, and combines it with the LQR controller to calculate the vehicle's front wheel angle from the lateral control aspect and the vehicle's driving torque / braking force from the longitudinal control aspect; and realizes the vehicle's trajectory tracking control through the combined action of the lateral and longitudinal controllers.

[0007] Preferably, a trajectory planner is designed to first construct a vehicle dynamics model; in the trajectory planner, a state estimator is designed using the SCKF algorithm to perform real-time estimation of the vehicle's posture parameters; based on the estimated parameters obtained by the SCKF state estimation, the future trajectory is fitted with a fifth-order polynomial equation, and lane line constraints are set; thereafter, the posture parameters obtained by the SCKF state estimation and the planned trajectory state of the fifth-order polynomial fitting equation are used as the input of the trajectory tracking error controller, and the error model is combined with the trajectory tracking matrix relationship to obtain the vehicle's real-time lateral tracking error and heading angle error.

[0008] Preferably, the future trajectory is fitted, and the fifth-order polynomial to be fitted is expressed as follows:

[0009] y=a0+a1x+a2x 2 +...+a5x 5 ,

[0010] Where a t (t=0,1,...5) is the coefficient of the fifth-order polynomial equation to be solved; the vehicle passes through multiple discrete trajectory points (x i ,y i )(i=1,2,3...), so any point x i The slope at is w i , that is, The slope relationship is as follows:

[0011]

[0012] Add constraints to the controller as follows:

[0013] stx i -x z ≥0y(x i )≤y z

[0014] x i -x j ≤0y(x i )≥y j

[0015] In the formula, point (x z ,y z )(z=1,2,3...) and (x j ,y j )(j=1,2,3...) represent points on the left and right road boundary lines respectively; according to the principle of minimizing the sum of squares of position deviation and slope deviation, the quintic polynomial fitting problem is transformed into an optimization problem as shown in the following formula:

[0016]

[0017] Preferably, the fitting steps for performing fifth-order polynomial fitting are as follows:

[0018] Step 1: Take the current vehicle position as the origin, establish a Cartesian coordinate system, and determine the current position coordinate of the vehicle as A(x a ,y a ) means x a =0,y a =0, the velocity of the position point is v a =v, with a slope of w a =0;

[0019] Step 2: Determine the target point location, that is, point B (x b ,y b ), and then further determine the speed v at the target point B b and slope w b ;

[0020] Step 3: Directly bring the pose parameters of points A and B into the optimization problem and set the derivative to 0, that is, Is 0, solve the optimization problem to get the coefficient a t (t=0,1,...5), the complete trajectory equation after fitting is obtained through the above steps.

[0021] Preferably, the trajectory tracking error model is:

[0022]

[0023] Where: v y Vehicle longitudinal speed, vx Indicates the lateral speed, e y is the lateral error, is the heading angle error, represents the yaw angle, θ r represents the target yaw angle, k r Represents curvature; Combine the error model with the trajectory tracking matrix relationship to calculate the real-time lateral tracking error e of the vehicle y and heading angle error

[0024] Preferably, in terms of lateral control, the feedback control law u(t) and the feedback matrix k are calculated by the LQR controller, and the feedforward angle δ is obtained by the feedforward controller. f , secondly, e y 、 As the input of the PID steering compensator, the front wheel steering compensation value Δδ is calculated, and u(t) and δ f The complete front wheel steering angle of the control system is obtained by combining Δδ and Δδ. At the same time, the front wheel steering angle constraint is set under the vehicle driving safety conditions to ensure the safety of the trajectory tracking process.

[0025] Preferably, the cost function of the LQR controller is:

[0026]

[0027] Where: Q and R are the weight matrices of the controller, where Q represents the weight matrix of the performance index function for the state quantity, which is usually a symmetric positive definite or semi-positive definite matrix, and R represents the weight of the control quantity, which is a symmetric positive definite matrix. According to the standard Riccati equation, the LQR control law is: u(t) = -ke rr (t), where u(t) is the optimal control law of the system and k = [k1, k2, k3, k4]; by introducing feedforward control, the final control law is u(t) = -ke rr (t)+δ f ; When the control system is stable, it will make The error-feedforward matrix relationship is obtained:

[0028]

[0029] Where: k1 and k3 are the first and third terms of the feedback matrix k calculated in the first part respectively; by feedforward control of the steering angle to reduce the steady-state error of the controller, let the first term in the above formula be 0, so that e y =0, with e rr The first row of the (t) matrix is ​​used as the design basis for the feedforward control law, from which the feedforward angle δ is calculated by the following formula f :

[0030]

[0031] Preferably, e y 、 As input, the compensation angle Δδ is obtained by using the PID steering compensator. The front wheel steering angle is compensated and Δδ is added to u(t) to obtain the accurate control value. The compensation angle can be expressed as follows:

[0032]

[0033] Where: K P0 , K I0 , K D0 are the proportional factor, integral factor, and differential factor respectively; t represents time; the feedforward control and PID steering compensator are combined to form a "feedforward + steering compensator", and the accurate front wheel steering angle δ of the car is obtained by combining Δδ with the complete control law u(t) of the system.

[0034] Preferably, in terms of longitudinal control, the throttle-brake converter based on dual PID is divided into a dual PID controller, an offline throttle-brake calibration table, and a throttle-brake controller; in order to output the vehicle acceleration that meets the normal control conditions, the a of the vehicle target trajectory point is used. p 、v p 、e x 、 The regulated acceleration a is input to the dual PID regulator and acts together with the vehicle speed v on the offline throttle and brake calibration table to derive the throttle opening thr corresponding to the system actuator at each trajectory point. Finally, combined with rpm, it is converted into the power torque / brake force required by the vehicle tire through the throttle and brake controller.

[0035] Preferably, in the dual PID regulator, the first-layer PID controller is used to control the influence of the vehicle longitudinal error on the system, and the expression is as follows:

[0036]

[0037] Where: K P1 is the proportional coefficient; K I1 is the integral coefficient; K D1 is the differential coefficient, and its control target is e x ;

[0038] The second layer PID controller is used to control v p The impact on a is expressed as:

[0039]

[0040] Where: K P2 is the proportional coefficient; K I2is the integral coefficient; K D2 is the differential coefficient; K_e x Indicates the influence of the first-layer PID controller on the second-layer PID controller.

[0041] The present invention has the following advantages:

[0042] 1. Trajectory tracking matrix relationship The problem of steady-state error caused by the vehicle error and error change rate cannot be 0 at the same time is solved. The present invention is based on the LQR feedback control law. rr The first row of the (t) matrix designs the feedforward control law δ f , combining the feedforward control with the LQR algorithm to obtain the complete feedback relationship u(t)=-ke rr (t)+δ f , solve the system steady-state error problem.

[0043] 2. In order to solve the problem of vehicle understeering, the present invention uses the vehicle's real-time e y 、 The compensation value Δδ is used as the input of the PID steering compensator to compensate the vehicle's front wheel angle and is combined with the LQR controller considering feedforward to further reduce the vehicle tracking error.

[0044] 3. When the vehicle is driving, in order to avoid surrounding obstacles and take into account the safety of vehicle movement, the present invention adopts the method of fitting a fifth-order polynomial equation to obtain a planned trajectory to achieve the effect of avoiding obstacles, and sets lane line constraints using points on the left and right lane lines to keep the trajectory within the lane lines, thereby ensuring the safety of the vehicle.

[0045] 4. In terms of longitudinal control, the present invention prepares an offline throttle and brake calibration table in advance, uses a dual PID regulator, introduces vehicle posture parameters and uses a table lookup to quickly obtain the throttle opening required at this time, obtains the motion torque / brake force according to the motor model, and the actuator acts on the tire according to the corresponding torque, thereby improving the real-time performance of the controller and completing trajectory tracking through the combination of lateral and longitudinal controllers. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 This is a system principle diagram of an automatic driving lateral and longitudinal trajectory tracking control method based on LQR and dual PID in the present invention.

[0047] Figure 2 Schematic diagram of the two-degree-of-freedom dynamic model in the present invention.

[0048] Figure 3 Schematic diagram of the trajectory tracking error model in the present invention.

[0049] Figure 4This is a schematic diagram of the automatic driving fitting principle of the present invention.

[0050] Figure 5 Schematic diagram of the LQR-dual PID horizontal and vertical trajectory tracking algorithm in the present invention.

[0051] Figure 6 This is a flow chart of an automatic driving lateral and longitudinal trajectory tracking control method based on LQR and dual PID of the present invention.

[0052] Figure 7 This is the throttle-brake relationship diagram used in the present invention.

[0053] Figure 8 Schematic diagram of the autonomous parking scenario.

[0054] Figure 9 Schematic diagram of the continuous lane-changing scenario.

[0055] Figure 10 The figure compares the effects of the present invention and other existing technologies in autonomous parking scenarios.

[0056] Figure 11 The figure is a comparison of the effects of the present invention and other existing technologies in a continuous lane changing scenario. DETAILED DESCRIPTION

[0057] The specific implementation methods of the present invention will be further explained in detail below through the description of embodiments with reference to the accompanying drawings, so as to help those skilled in the art to have a more complete, accurate and in-depth understanding of the inventive concept and technical solution of the present invention.

[0058] like Figure 1-Figure 7 As shown, the present invention provides an automatic driving lateral and longitudinal trajectory tracking control method based on LQR and dual PID, comprising the following steps.

[0059] First, the SCKF algorithm in the trajectory planner is used to estimate the current vehicle's posture parameters in real time, and lane line constraints are set. The planned trajectory information is determined by combining the estimated parameters with quintic polynomial fitting.

[0060] Since the vehicle is a nonlinear, complex, time-varying system during driving, establishing a model that accurately describes the vehicle's own dynamic characteristics is a prerequisite for achieving autonomous driving trajectory tracking. To design a trajectory planner, first build a vehicle dynamics model. Figure 2 As shown. Under the condition of ensuring the stability and safety of vehicle tracking control, the present invention derives the vehicle two-degree-of-freedom dynamic differential equation from the torque balance equation as follows:

[0061]

[0062] Where: (x, y) represents the current position coordinates of the vehicle; is the yaw angle; I represents the moment of inertia around the z axis; m represents the vehicle body mass; l r , l f Indicates the distance from the vehicle's center of mass to the front and rear axles; a y represents lateral acceleration; M represents yaw torque; represents the yaw angular velocity; Indicates the vehicle's yaw angular acceleration.

[0063] Considering that the front wheel turning angle in the model is small, that is, cosδ≈1, the obtained dynamic matrix is ​​expressed as:

[0064]

[0065] In the formula represents the lateral acceleration, i.e.

[0066] In the trajectory planner, the state estimator is designed based on the SCKF (Square-root Cubature Kalman Filter, SCKF) algorithm to perform real-time estimation of the vehicle's posture parameters.

[0067] In order to accurately obtain the state information of each point in the vehicle, the present invention adopts the SCKF state estimator. This method considers the following discrete-time nonlinear system:

[0068]

[0069] Where f(·) is the state transfer equation of the algorithm; h(·) is the measurement equation; x k is the state variable at time k; z k is the measured variable; u k is the input variable; w k-1 is the process noise; v k To measure noise.

[0070] In order to make full use of the measurement parameters, the measurement value is taken as part of the state variable. Therefore, the state variable is defined as:

[0071]

[0072] In the above formula, ω ij is the angular velocity of each wheel; a x is the longitudinal acceleration of the vehicle. Therefore, according to the standard Kalman filter principle, at the sampling time T, the state transfer equation and measurement equation of the SCKF state estimator are:

[0073]

[0074] Based on the estimated parameters obtained by SCKF state estimation, the future trajectory is fitted with a fifth-order polynomial equation and lane line constraints are set.

[0075] To address the problem of obstacles around vehicles when performing actions such as parking and lane changing, the present invention designs a trajectory planner that fits the future trajectory using a quintic polynomial equation and sets lane line constraints to ensure the rationality and safety of the trajectory designed by the controller. The quintic polynomial to be fitted is expressed as follows:

[0076] y=a0+a1x+a2x 2 +...+a5x 5 ,

[0077] Where a t (t=0,1,...5) are the coefficients of the quintic polynomial equation to be solved.

[0078] For vehicle fitting scenarios, it is usually considered that the vehicle passes through multiple discrete trajectory points (x i ,y i )(i=1,2,3...), so any point x i The slope at is w i , that is, The slope relationship is as follows:

[0079]

[0080] In trajectory planning, to ensure that the planned trajectory always stays within the lane and that the vehicle travels safely from the starting point to the destination without colliding with other vehicles, the controller needs to be constrained as follows:

[0081] stx i -x z ≥0y(x i )≤y z

[0082] x i -x j ≤0y(x i )≥y j ,

[0083] In the formula, point (x z ,y z )(z=1,2,3...) and (x j ,y j )(j=1,2,3...) represent points on the left and right road boundary lines respectively.

[0084] According to the principle of minimizing the sum of the squares of position deviation and slope deviation, the present invention transforms the quintic polynomial fitting problem into an optimization problem as shown in the following formula:

[0085]

[0086] Then, a fifth-order polynomial fitting is performed. The automatic driving fitting principle diagram of the present invention is shown as follows: Figure 4 The fitting steps are as follows:

[0087] Step 1: Take the current vehicle position as the origin, establish a Cartesian coordinate system, and determine the current position coordinate of the vehicle as A(x a ,y a ) means x a =0,y a =0, the velocity of the position point is v a =v, with a slope of w a =0.

[0088] Step 2: Determine the target point location, that is, point B (x b ,y b ), and then further determine the speed v at the target point B b and slope w b .

[0089] Step 3: Directly bring the pose parameters of points A and B into the optimization problem and set the derivative to 0, that is, is 0, solve the optimization problem to get the coefficient a t (t=0,1,...5), the complete trajectory equation after fitting is obtained through the above steps, and the target trajectory is determined as follows Figure 4 As shown by the red dotted line in the middle, the heading angle, planned speed, planned acceleration and θ at each point of the fitting trajectory are r 、v p 、a p 、k r Expressed as:

[0090]

[0091] Afterwards, the pose parameters estimated by the SCKF state and the planned trajectory state of the fifth-order polynomial fitting equation are used as the input of the trajectory tracking error controller. The error model is combined with the trajectory tracking matrix relationship to obtain the vehicle's real-time lateral tracking error and heading angle error.

[0092] In order to further characterize the dynamic behavior of the autonomous driving vehicle during the trajectory tracking process, the present invention establishes a vehicle trajectory tracking model based on the Cartesian coordinate system XOY, such as Figure 3As shown in Figure 2, the black solid line is used as the target trajectory of the vehicle, and the red dotted line is used as the actual trajectory of the vehicle. When the vehicle is in motion, there is always a certain degree of deviation between the actual position of the vehicle and the target trajectory. Therefore, with the center of mass of the vehicle as the origin, point G is set as the actual position of the vehicle, point H is set as the target position, s(t) represents the real-time longitudinal displacement of the vehicle, and θ g Indicates the angle between the real-time vehicle body longitudinal axis and the horizontal axis, θ r The angle between the longitudinal axis and the transverse axis of the vehicle at the target trajectory point H is the heading angle of the target trajectory point.

[0093] According to the vehicle motion geometry rules, during driving And it is a small value, so the vehicle heading angle error is And for θ r Derivative existence Therefore, the trajectory tracking error model is:

[0094]

[0095] Where: v y Vehicle longitudinal speed; represents the derivative of the lateral error; further we get:

[0096]

[0097] Substituting the above formula into the dynamic matrix formula, the trajectory tracking matrix relationship is obtained as follows:

[0098]

[0099] In the above formula, represents the second derivative of the lateral error; represents the first derivative of the lateral error; Represents the second-order derivative of the lateral error. Rewrite the above formula as:

[0100]

[0101] In the above formula Represents the error matrix e rr (t) first-order derivative; A, B, C are e rr (t), control quantity u, target heading angle derivative The coefficient of ; According to the relationship between variables:

[0102]

[0103] Afterwards, this method adopts an LQR-dual PID lateral and longitudinal trajectory tracking algorithm, designs a dual PID regulator, and combines it with an LQR controller to calculate the vehicle front wheel angle from the lateral control aspect and calculate the vehicle driving torque / brake force from the longitudinal control aspect. Figure 5 As shown in Figure 2, the vehicle's real-time posture parameters and the target trajectory information obtained by the trajectory planner are used as state inputs.

[0104] In terms of lateral control, the feedback control law u(t) and the feedback matrix k are calculated by the LQR controller, and the feedforward angle δ is obtained by the feedforward controller. f , secondly, e y 、 As the input of the PID steering compensator, the front wheel steering compensation value Δδ is calculated, and u(t) and δ f The complete front wheel steering angle of the control system is obtained by combining Δδ and Δδ. At the same time, the front wheel steering angle constraint is set under the vehicle driving safety conditions to ensure the safety of the trajectory tracking process.

[0105] By taking the obtained vehicle speed and incorporating the inherent properties of the vehicle, the optimization problem of the LQR controller is calculated, and the control law and feedback matrix of the LQR controller are derived. The cost function of the LQR controller is:

[0106]

[0107] Where: Q and R are the weight matrices of the controller, where Q represents the weight matrix of the performance index function for the state quantity, which is usually a symmetric positive definite or semi-positive definite matrix, and R represents the weight of the control quantity, which is a symmetric positive definite matrix.

[0108] In the present invention, the values ​​of Q and R are verified by simulation as follows:

[0109]

[0110] According to the standard Riccati equation, the LQR control law is: u(t) = -ke rr (t), where u(t) is the optimal control law of the system and k = [k1, k2, k3, k4].

[0111] According to the road curvature and vehicle posture information, the feedforward angle is introduced and combined with the control law to obtain the complete control law equation. Based on the system error equation, the error-feedforward matrix relationship is calculated and the basic design equation of feedforward control is determined.

[0112] From the trajectory tracking matrix relationship, we can see that the system always has Therefore, no matter what the value of the feedback matrix k is, the derivative of the trajectory tracking lateral deviation and lateral deviation e rr(t) cannot be zero at the same time. It is known that there is a certain degree of steady-state error when the control system is stable. Therefore, this method introduces feedforward control, that is, adding the feedforward angle δ on the basis of the LQR control law u(t) f , the final control law is u(t)=-ke rr (t)+δ f .

[0113] Substituting this formula into the trajectory tracking matrix relationship, we can obtain: When the control system is stable, The error-feedforward matrix relationship is obtained:

[0114]

[0115] Where: k1 and k3 are the first and third terms of the feedback matrix k calculated in the first part.

[0116] The present invention reduces the steady-state error of the controller by feedforward control of the rotation angle. Let the first term in the above formula be 0, that is, e y =0, yes Therefore, e rr The first row of the (t) matrix is ​​used as the design basis for the feedforward control law, from which the feedforward angle δ is calculated by the following formula f :

[0117]

[0118] The feedforward angle δ f The calculation method is different from the traditional vehicle kinematics model in which the turning radius R and the turning characteristic K are used. v The impact on the vehicle is based on the vehicle's two-degree-of-freedom dynamic model, and the longitudinal speed v is introduced. x , rate of change of heading angle And the LQR feedback matrix k = [k1, k2, k3, k4] is used as an influencing factor, which enables the algorithm to make feedback adjustments based on the vehicle's current speed, heading angle and other state quantities, further improving the dynamic response and accuracy of the feedforward collision angle.

[0119] When a vehicle is driving normally on the road, due to the defects of the vehicle's mechanical steering system, it often has the problem of understeering, which makes the output front wheel angle inconsistent with the required control amount, resulting in a decrease in trajectory tracking accuracy. In order to solve the impact of this problem on vehicle control, e y 、 As input, the compensation angle Δδ is obtained by using the PID steering compensator, the front wheel angle is compensated and Δδ is added to u(t) to obtain the accurate control value. The compensation angle can be expressed as follows:

[0120]

[0121] Where: K P0 , K I0 , K D0 where Δδ is the proportional factor, Δδ is the integral factor, and Δδ is the differential factor; t represents time. Feedforward control and the PID steering compensator combine to form a "feedforward + steering compensator." This combines Δδ with the system's complete control law u(t) to determine the vehicle's accurate front wheel steering angle δ.

[0122] In terms of longitudinal control, in order to output the vehicle acceleration that meets the normal control conditions, the a of the vehicle target trajectory point is used. p 、v p 、e x 、 The regulated acceleration a is input to the dual PID regulator and acts together with the vehicle speed v on the offline throttle and brake calibration table to derive the throttle opening thr corresponding to the system actuator action at each trajectory point. Finally, combined with rpm, it is converted into the power torque / brake force required by the vehicle tires through the throttle and brake controller, improving the real-time performance of the system. The trajectory tracking process of autonomous driving is realized through the combined action of the lateral and longitudinal controllers.

[0123] The dual-PID-based throttle-brake converter consists of a dual-PID controller, an offline throttle-brake calibration table, and a throttle-brake controller. While the vehicle is in motion, the required tire torque / braking force can be quickly determined by looking up the offline calibration table based on the vehicle's control variables and parameters, reducing calculation complexity and improving the real-time performance of the control system.

[0124] The purpose of the dual PID controller is to solve the problem that the actual vehicle posture parameters cannot quickly reach the planned state. The first layer PID controller is used to control the impact of the vehicle longitudinal error on the system. The expression is as follows:

[0125]

[0126] Where: K P1 is the proportional coefficient; K I1 is the integral coefficient; K D1 is the differential coefficient, and its control target is e x .

[0127] The second layer PID controller is used to control v p The impact on a is expressed as:

[0128]

[0129] Where: K P2 is the proportional coefficient; K I2 is the integral coefficient; K D2is the differential coefficient; K_e x Indicates the influence of the first-layer PID controller on the second-layer PID controller.

[0130] The throttle-brake calibration converter consists of an offline throttle-brake calibration table and a throttle-brake controller. The throttle-brake calibration table is formed by testing the relationship data of vehicle speed, acceleration, and throttle opening under different conditions, and is stored in the control system in an offline form, as shown in Table 1. The throttle-brake relationship diagram is obtained by sorting Table 1, as shown in Figure 7 shown.

[0131] Table 1: Offline throttle and brake calibration table

[0132]

[0133] When a and v are input, the throttle opening thr is automatically found and input together with the speed rpm to the accelerator and brake controller. The motor model formula is used to calculate the motion torque / braking force required by the actuator of the vehicle to cope with the current situation, and input it to the vehicle tires to achieve the purpose of controlling the vehicle's turning, acceleration, and deceleration. The motor model used in the present invention can be described as:

[0134] P=ξrpm,

[0135] Where: P is the power required by the motor, and ξ is the torque required by the motor.

[0136] The method used in this paper is based on experimental calibration and is universal for various types of motors. Assuming that ξ and rpm are approximately linearly related, we have:

[0137]

[0138] Where: max Indicates the maximum torque of the motor and has ξ max =380thr,n b Indicates the ratio of maximum power to maximum torque. The vehicle's tire torque / brake force is calculated based on the relationship between the throttle opening and the motor model.

[0139] The lateral and longitudinal controllers are combined with the trajectory planner and trajectory tracking error control. The vehicle's trajectory tracking control is achieved by transmitting the motion torque / braking force and the front wheel steering angle δ to the vehicle model. At the same time, the posture parameters of the vehicle model are fed back to each module to further improve the control effect.

[0140] In order to fully verify the effectiveness of the present invention, the present invention uses Carsim to realize the vehicle dynamics model and virtual scene setting, Prescan to carry the sensor model, Matlab / Simulink algorithm module to design the control center and select the following Figure 8The autonomous parking scenario shown and Figure 9 The continuous lane-changing scenario shown in the figure is simulated. By comparing the trajectory tracking results of three controllers: an LQR+feed-forward+steering compensation controller, an LQR+feedforward controller, and an LQR controller, the changes in lateral error, heading angle error, front wheel angle, and yaw rate of each controller are observed. These two simulation scenarios provide a comprehensive test of the autonomous vehicle's tracking capabilities during autonomous parking and continuous lane changes.

[0141] exist Figure 8 The experimental scenario shown in Figure 1 describes the basic principle of vehicle autonomous parking. The moving vehicle obtains a parking trajectory through the trajectory planning controller, as shown in Figure 1. Figure 8 As shown by the white dotted line in the middle, point P1 is the starting point of parking, point P2 is the middle point of the parking trajectory, and point P3 is the final point of parking. During this process, the vehicle speed gradually changes from v1 to v2 and v3, and v3 = 0. This shows the whole process of the vehicle from normal driving to autonomous parking. Figure 10 (a) Figure 10 (b) It can be seen that the LQR+feed-forward+steering compensation controller using this method is closest to the target trajectory, and the maximum curvature of the trajectory reaches 0.11m -1 ;Depend on Figure 10 (c) Figure 10 (d) It can be seen that the maximum lateral tracking error of the controller using this method is 0.004m, while the maximum error of other controllers is 0.03m. The tracking error is much smaller than that of other controllers, and the tracking accuracy is improved by 86%. The maximum heading angle error of the controller using this method is 0.18rad, while the maximum error of other controllers is 0.21rad. It can be seen that the controller using this method is better than other controllers. Therefore, the superiority of the LQR+feed-forward+steering compensation controller in terms of real-time performance is further verified. Figure 10 (e) Figure 10 (f) It can be seen that the vehicle's yaw rate and front wheel angle are kept small during tracking, meeting the requirements of system stability and safety.

[0142] exist Figure 9In the experimental scenario shown, the vehicle is in a high-speed driving state and performs continuous lane changing. By setting Vehicle1, Vehicle2, Vehicle3, and Vehicle4 as obstacle vehicles on the highway, the autonomous vehicle obtains a continuous lane changing trajectory through the trajectory planning controller. Point D1 is used as the lane changing starting point, Path1 is used as the trajectory of continuous lane changing Phase1 (first phase), D2 is used as the lane changing starting point, Path2 is used as the trajectory of continuous lane changing Phase2 (second phase), and D3 is used as the lane changing starting point. The entire continuous lane changing process is completed to achieve autonomous lane changing. Figure 11 (a) Figure 11 (b) It can be seen that in the continuous lane change scenario, the vehicle changes lanes twice to keep going straight, completing the planned action, and the tracking result of the LQR+feed-forward+steering compensation controller is closer to the target trajectory, with a trajectory curvature of 0-0.11m -1 There are drastic changes between Figure 11 (c) and (d) show that the lateral error of the LQR+feed-forward+steering compensation controller varies between -0.003 and 0.005 m, with a small fluctuation range, and the maximum error is much smaller than that of other controllers. When t = 21 s, the lateral error of the controller using this method has converged to 0, while the other controllers are close to 0 at t = 26 s. Therefore, the controller using this method improves the tracking real-time performance by 19%. Figure 11 (e) Figure 11 (f) While keeping the yaw rate and front wheel angle small, the heading angle error of the controller is also smaller than that of other controllers, further demonstrating the superiority of the designed controller.

[0143] In the present invention, Figure 1 The principle of the automatic driving control system is given. In the figure: (x, y) represents the current position coordinates of the vehicle; (x r ,y r ) is the position of each point on the planned trajectory; v y Indicates the longitudinal speed of the vehicle, v x Indicates the lateral speed; Indicates the yaw angle. In this model, the yaw angle is the heading angle, θ r represents the target yaw angle, k r represents curvature; I represents the moment of inertia around the z axis; m represents the mass of the vehicle body; C f ,C r Indicates the lateral stiffness of the front and rear wheels of the vehicle; l r , l f Indicates the distance from the vehicle's center of mass to the front and rear axles; e y is the lateral error, is the heading angle error, e x is the longitudinal error, is the rate of change of longitudinal error, e rr (t) is the error matrix; u(t) represents the feedback control law; k represents the feedback matrix; δ represents the vehicle front wheel angle; v p represents the planning speed, a p Indicates the planned acceleration; rpm indicates the current speed of the vehicle engine, v is the vehicle speed, torque indicates the vehicle driving torque, and brake indicates the braking force. Figure 2 The two-degree-of-freedom dynamic model of the vehicle is given; in the figure: point O is the center of mass of the vehicle; F yf , F yr Indicates the lateral force of the front and rear tires of the vehicle; α f ,α r represents the side slip angle of the front and rear wheels of the vehicle; β represents the side slip angle of the vehicle's center of mass; v f 、v r Indicates the speed of the vehicle's front and rear wheels.

[0144] The present invention is described above by way of example in conjunction with the accompanying drawings. It is obvious that the specific implementation of the present invention is not limited to the above-mentioned method. As long as various non-substantial improvements are made using the inventive concept and technical solution of the present invention, or the inventive concept and technical solution are directly applied to other occasions without improvement, they are all within the scope of protection of the present invention.

Claims

1. A method for lateral and longitudinal trajectory tracking control of an autonomous driving system based on LQR and dual PID, characterized by: First, the SCKF algorithm in the trajectory planner estimates the current vehicle's pose parameters in real time and sets lane line constraints. The planned trajectory information is determined by combining the estimated parameters with a quintic polynomial fit. Then, an LQR-dual PID lateral and longitudinal trajectory tracking algorithm was adopted, and a dual PID regulator was designed. Combined with the LQR controller, the vehicle's front wheel angle was calculated from the lateral control aspect, and the vehicle's driving torque / braking force was calculated from the longitudinal control aspect; the vehicle's trajectory tracking control was achieved through the combined action of the lateral and longitudinal controllers.

2. The LQR and dual-PID based automatic driving lateral and longitudinal trajectory tracking control method according to claim 1, characterized in that: To design a trajectory planner, first build a vehicle dynamics model. Within the trajectory planner, use the SCKF algorithm to design a state estimator to estimate the vehicle's pose parameters in real time. Based on the estimated parameters obtained from the SCKF state estimation, a quintic polynomial equation is used to fit the future trajectory and set lane constraints. Afterwards, the pose parameters estimated by the SCKF state and the planned trajectory state of the fifth-order polynomial fitting equation are used as the input of the trajectory tracking error controller. The error model is combined with the trajectory tracking matrix relationship to obtain the vehicle's real-time lateral tracking error and heading angle error.

3. The LQR and dual PID based automatic driving lateral and longitudinal trajectory tracking control method according to claim 2, characterized in that: Fit the future trajectory, and the fifth-order polynomial to be fitted is expressed as follows: y=a0+a1x+a2x 2 +...+a5x 5 , Where a t (t=0,1,...5) is the coefficient of the fifth-order polynomial equation to be solved; the vehicle passes through multiple discrete trajectory points (x i ,y i )(i=1,2,3...), so any point x i The slope at is w i , that is, The slope relationship is as follows: Add constraints to the controller as follows: s.t.x i -x z ≥0y(x i )≤y z x i -x j ≤0y(x i )≥y j In the formula, point (x z ,y z )(z=1,2,3...) and (x j ,y j )(j=1,2,3...) represent points on the left and right road boundary lines respectively; according to the principle of minimizing the sum of squares of position deviation and slope deviation, the quintic polynomial fitting problem is transformed into an optimization problem as shown in the following formula:

4. The LQR and dual-PID based automatic driving lateral and longitudinal trajectory tracking control method according to claim 3, characterized in that: The fitting steps for a fifth-order polynomial fit are as follows: Step 1: Take the current vehicle position as the origin, establish a Cartesian coordinate system, and determine the current position coordinate of the vehicle as A(x a ,y a ) means x a =0,y a =0, the velocity of the position point is v a =v, with a slope of w a =0; Step 2: Determine the target point location, that is, point B (x b ,y b ), and then further determine the speed v at the target point B b and slope w b ; Step 3: Directly bring the pose parameters of points A and B into the optimization problem and set the derivative to 0, that is, is 0, solve the optimization problem to get the coefficient a t (t=0,1,...5), the complete trajectory equation after fitting is obtained through the above steps.

5. The LQR and dual PID based automatic driving lateral and longitudinal trajectory tracking control method according to claim 4, characterized in that: The trajectory tracking error model is: Where: v y Vehicle longitudinal speed, v x Indicates the lateral speed, e y is the lateral error, is the heading angle error, represents the yaw angle, θ r represents the target yaw angle, k r Represents curvature; Combine the error model with the trajectory tracking matrix relationship to calculate the real-time lateral tracking error e of the vehicle y and heading angle error 6. The LQR and dual PID based automatic driving lateral and longitudinal trajectory tracking control method according to claim 1, characterized in that: In terms of lateral control, the feedback control law u(t) and the feedback matrix k are calculated by the LQR controller, and the feedforward angle δ is obtained by the feedforward controller. f , secondly, e y 、 As the input of the PID steering compensator, the front wheel steering compensation value Δδ is calculated, and u(t) and δ f The complete front wheel steering angle of the control system is obtained by combining Δδ and Δδ. At the same time, the front wheel steering angle constraint is set under the vehicle driving safety conditions to ensure the safety of the trajectory tracking process.

7. The LQR and dual PID based automatic driving lateral and longitudinal trajectory tracking control method according to claim 6, characterized in that: The cost function of the LQR controller is: Where: Q and R are the weight matrices of the controller, where Q represents the weight matrix of the performance index function for the state quantity, which is usually a symmetric positive definite or semi-positive definite matrix, and R represents the weight of the control quantity, which is a symmetric positive definite matrix. According to the standard Riccati equation, the LQR control law is: u(t) = -ke rr (t), where u(t) is the optimal control law of the system and k = [k1, k2, k3, k4]; by introducing feedforward control, the final control law is u(t) = -ke rr (t)+δ f ; When the control system is stable, The error-feedforward matrix relationship is obtained: Where: k1 and k3 are the first and third terms of the feedback matrix k calculated in the first part respectively; by feedforward control of the steering angle to reduce the steady-state error of the controller, let the first term in the above formula be 0, so that e y =0, with e rr The first row of the (t) matrix is ​​used as the design basis for the feedforward control law, from which the feedforward angle δ is calculated by the following formula f :

8. The LQR and dual PID based automatic driving lateral and longitudinal trajectory tracking control method according to claim 7, characterized in that: e y 、 As input, the compensation angle Δδ is obtained by using the PID steering compensator. The front wheel steering angle is compensated and Δδ is added to u(t) to obtain the accurate control value. The compensation angle can be expressed as follows: Where: K P0 , K I0 , K D0 are the proportional factor, integral factor, and differential factor respectively; t represents time; the feedforward control and PID steering compensator are combined to form a "feedforward + steering compensator", which combines Δδ with the system's complete control law u(t) to obtain the accurate front wheel steering angle δ of the car.

9. The LQR and dual PID based automatic driving lateral and longitudinal trajectory tracking control method according to claim 1, characterized in that: In terms of longitudinal control, the throttle-brake converter based on dual PID is divided into dual PID controller, offline throttle-brake calibration table, and throttle-brake controller; in order to output the vehicle acceleration that meets the normal control conditions, the a of the vehicle target trajectory point is used as the reference. p 、v p 、e x 、 The regulated acceleration a is input to the dual PID regulator and acts together with the vehicle speed v on the offline throttle and brake calibration table to derive the throttle opening thr corresponding to the system actuator at each trajectory point. Finally, combined with rpm, it is converted into the power torque / brake force required by the vehicle tire through the throttle and brake controller.

10. The LQR and dual PID based automatic driving lateral and longitudinal trajectory tracking control method according to claim 9, characterized in that: In the dual PID regulator, the first-layer PID controller is used to control the impact of the vehicle longitudinal error on the system. The expression is as follows: Where: K P1 is the proportional coefficient; K I1 is the integral coefficient; K D1 is the differential coefficient, and its control target is e x ; The second layer PID controller is used to control v p The impact on a is expressed as: Where: K P2 is the proportional coefficient; K I2 is the integral coefficient; K D2 is the differential coefficient; K_e x Indicates the influence of the first-layer PID controller on the second-layer PID controller.

Citation Information

Patent Citations

  • Improved path tracking control method based on LQR

    CN114384915A

  • Wheel type mobile robot trajectory tracking LQR control method considering slipping

    CN115857511A