Feedforward and predictive lqr-based autonomous vehicle path tracking method and experimental device

Abstract

The present application relates to a kind of based on feedforward and prediction LQR automatic driving vehicle path tracking method and experimental device, belong to automatic driving technical field;Method, comprising: establishing model: establishing the two-degree-of-freedom lateral dynamics model of automatic driving vehicle;Feedforward LQR controller is constructed: according to two-degree-of-freedom lateral dynamics model construction feedforward LQR controller;Establishment of predictive controller: according to fuzzy control algorithm establishes fuzzy adaptive prediction time based on real-time speed-curvature, and prediction controller is designed based on constant turning rate and speed amplitude model;Feedforward+prediction LQR controller is established: according to feedforward LQR and prediction controller establishes lateral feedforward+prediction LQR path tracking controller;Experimental device, according to the above based on feedforward and prediction LQR automatic driving vehicle path tracking method, based on Labview and Simulink CAN communication is built;Solve the problem that automatic driving vehicle is difficult to consider in complex working condition tracking accuracy and stability.

Description

Technical Field

[0001] This invention relates to a path tracking method and experimental apparatus for autonomous vehicles based on feedforward and prediction LQR, belonging to the field of autonomous driving technology. Background Technology

[0002] Autonomous vehicles refer to a new generation of automobiles that, by equipping themselves with advanced sensors and other devices and utilizing new technologies such as artificial intelligence, possess autonomous driving capabilities and are gradually becoming intelligent mobile spaces and application terminals. Autonomous vehicles can provide safer, more energy-efficient, more environmentally friendly, and more convenient modes of transportation and comprehensive solutions, and are internationally recognized as a future development direction and a hot topic of interest.

[0003] With the rapid development of autonomous driving technology, self-driving vehicles have been gradually deployed in specific fields such as sanitation, transportation, and industrial parks, greatly changing people's lifestyles and quality of life. Higher levels of autonomous driving hold a very broad prospect for the future. An autonomous driving system consists of four parts: environmental perception, task decision-making, motion planning, and execution control. Path tracking, as the main task of execution control, is crucial for autonomous driving systems. It must not only ensure tracking accuracy but also guarantee the vehicle's safety and handling stability.

[0004] For example, Chinese Patent Publication No. CN113978547A discloses an autonomous driving steering control method and system. The method includes: acquiring planning trajectory information sent by a planning module, vehicle chassis information sent by a chassis module, and vehicle position information sent by a positioning module. The planning trajectory information includes a planned driving trajectory composed of a series of trajectory points; finding the trajectory point closest to the vehicle in the planning trajectory information based on the vehicle position information, and recording it as a matching point; determining the remaining driving distance of the vehicle based on the matching point and the planning trajectory information; determining whether the remaining driving distance is greater than a preset aiming distance in a kinematics-based tracking algorithm; if so, calculating the vehicle's front wheel deflection angle using a kinematics-based tracking algorithm for steering control; if not, calculating the vehicle's front wheel deflection angle using a dynamics-based LQR algorithm for steering control. This invention can achieve accurate steering control in low-speed scenarios while meeting vehicle stability control requirements.

[0005] However, the kinematics-based tracking algorithm it uses is an existing algorithm, and no adaptive improvements have been made to the specific kinematics-based tracking algorithm. Therefore, it cannot simultaneously guarantee tracking accuracy, vehicle safety, and handling stability.

[0006] Therefore, it is particularly important to develop high-level path tracking algorithms, especially to address the challenge of balancing tracking accuracy and stability when autonomous vehicles are turning. Summary of the Invention

[0007] The technical problem to be solved by this invention is to provide a path tracking method and experimental device for autonomous vehicles based on feedforward and prediction LQR, so as to solve the problem that it is difficult to balance the tracking accuracy and stability of autonomous vehicles under complex working conditions.

[0008] The present invention provides a path tracking method for autonomous vehicles based on feedforward and prediction LQR, comprising:

[0009] Model establishment: Establish a two-degree-of-freedom lateral dynamics model for autonomous vehicles;

[0010] Constructing a feedforward LQR controller: Construct a feedforward LQR controller based on a two-degree-of-freedom transverse dynamics model;

[0011] Establish a predictive controller: Based on the fuzzy control algorithm, establish a fuzzy adaptive prediction time based on real-time vehicle speed-curvature, and design a predictive controller based on a constant turning rate and speed amplitude model;

[0012] Establish a feedforward + predictive LQR controller: Establish a lateral feedforward + predictive LQR path tracking controller based on the feedforward LQR and predictive controller.

[0013] First, a two-degree-of-freedom lateral dynamics model of the autonomous vehicle is established, and a feedforward LQR controller is constructed based on the two-degree-of-freedom lateral dynamics model. However, the feedforward LQR controller has a certain lag. Therefore, a fuzzy adaptive prediction time based on real-time vehicle speed-curvature is established based on the fuzzy control algorithm. Based on the constant turning rate and speed amplitude model (CTRV) designed in this paper, a predictive controller is designed. A lateral feedforward + predictive LQR path tracking controller is established based on the feedforward LQR and the predictive controller to predict the vehicle's position in the future, thereby improving the vehicle's stability while ensuring tracking accuracy.

[0014] Preferably, the two-degree-of-freedom lateral dynamics model includes the vehicle's own lateral and yaw motions, as well as the lateral and heading errors with respect to the reference path.

[0015] The state-space representation of the two-degree-of-freedom transverse dynamics model is as follows:

[0016]

[0017] Where δ is the front wheel steering angle, O is the vehicle's center of gravity, and l f l r e represents the distance from the front and rear axles to the center of mass. d θ represents the lateral error between the vehicle and the desired trajectory. r Let the heading angle be the desired trajectory. C is the yaw angle of the vehicle. f and C r These are the total lateral stiffness of the front and rear wheels, respectively.z Let v be the moment of inertia about the Z-axis. x This refers to the vehicle's longitudinal speed.

[0018] Preferably, the input to the feedforward LQR controller is the vehicle body status and road information collected and processed by the on-board system, and the output of the feedforward LQR controller is the front wheel steering angle.

[0019] Preferably, the steps for establishing the predictive controller specifically include:

[0020] Based on the influence of road curvature and vehicle speed on prediction time, and according to fuzzy control theory, road curvature and vehicle speed are used as inputs to the fuzzy controller, and prediction time is used as the output of the fuzzy controller, so as to adjust the prediction time online in real time.

[0021] The road curvature, vehicle speed, and prediction time of the autonomous vehicle are classified into accuracy levels based on fuzzy control.

[0022] Membership functions are applied to the input and output variables of the predicted time of autonomous vehicles. The Mamdani algorithm and the average value method of the maximum membership degree are used to defuzzify the predicted time of the output variable of autonomous vehicles.

[0023] The vehicle's position over a certain period of time is predicted based on a constant turning rate and speed amplitude model. The two-degree-of-freedom lateral dynamics model of the current vehicle position and the current reference path point is updated using the predicted vehicle position and a reference path point at a certain prediction time.

[0024] Preferably, the road curvature of the autonomous vehicle is divided into seven accuracy levels based on fuzzy control, the vehicle speed of the autonomous vehicle is divided into seven accuracy levels based on fuzzy control, and the prediction time of the autonomous vehicle is divided into eleven accuracy levels based on fuzzy control.

[0025] Preferably, the adoption of membership functions specifically involves: adopting a Gauss2mf type membership function for the input variables, the road curvature and the vehicle speed of the autonomous vehicle, and adopting a Trimf type membership function for the output variable, the prediction time of the autonomous vehicle.

[0026] Preferably, the operation method of the feedforward + predictive LQR controller specifically includes:

[0027] Based on the vehicle status and path information collected and processed by the in-vehicle system, the prediction time required by the predictive controller is calculated through fuzzy control.

[0028] The predictive controller based on prediction time updates the two-degree-of-freedom lateral dynamics model of the vehicle and a reference path point at a certain prediction time in real time.

[0029] Based on a real-time updated two-degree-of-freedom lateral dynamics model, the front wheel steering angle output by the control system to the autonomous vehicle is calculated by a feedforward LQR controller.

[0030] The present invention discloses an experimental device for path tracking of autonomous vehicles based on feedforward and prediction LQR, which is built based on CAN communication between LabVIEW and Simulink according to the above-mentioned path tracking method for autonomous vehicles based on feedforward and prediction LQR.

[0031] A path tracking experimental setup for autonomous vehicles based on feedforward and prediction LQR was used to achieve closed-loop control.

[0032] Preferably, the control algorithm for the above-mentioned path tracking method for autonomous vehicles based on feedforward and prediction LQR is built in Simulink, and the physical signals in Simulink are converted into CAN signals and transmitted to the motor controller to control the operation of the steering motor, thereby realizing closed-loop control.

[0033] Preferably, the steering motor is powered by a storage battery.

[0034] Compared with the prior art, the present invention has the following beneficial effects:

[0035] (1) This invention proposes a path tracking method and experimental device for autonomous vehicles based on feedforward + prediction LQR, which can take into account the tracking accuracy and stability of autonomous vehicles under complex working conditions and improve the path tracking performance of autonomous vehicles.

[0036] (2) This invention establishes a fuzzy adaptive prediction time based on real-time vehicle speed-curvature, and establishes a prediction controller based on real-time adjusted prediction time, which integrates vehicle speed and road information into steering control, significantly improving the stability of autonomous vehicles when steering. Attached Figure Description

[0037] Figure 1 This is a flowchart of the path tracking method for autonomous vehicles based on feedforward and prediction LQR as described in this invention;

[0038] Figure 2 This invention establishes a two-degree-of-freedom lateral dynamics model for autonomous vehicles.

[0039] Figure 3 This is a schematic diagram of the autonomous vehicle driving scenario established by the present invention. a is a schematic diagram of the autonomous vehicle driving scenario when the current driving path of the vehicle is not on the reference path, and b is a schematic diagram of the autonomous vehicle driving scenario when the current driving path of the vehicle is on the reference path.

[0040] Figure 4This is a schematic diagram illustrating the effect of the predictive controller established in this invention;

[0041] Figure 5 This invention establishes a fuzzy input quantity vehicle speed membership function graph;

[0042] Figure 6 This invention establishes a fuzzy input quantity road curvature membership function graph.

[0043] Figure 7 This is the fuzzy output prediction time membership function graph established by this invention;

[0044] Figure 8 This is a schematic diagram of the fuzzy control inference results established by the present invention;

[0045] Figure 9 This is the schematic diagram of the feedforward + predictive LQR controller established in this invention;

[0046] Figure 10 This is a flowchart of the experimental device for path tracking of autonomous vehicles based on feedforward and prediction LQR as described in this invention. Detailed Implementation

[0047] To more clearly illustrate the embodiments or technical solutions of the present invention, the embodiments or technical solutions will be briefly described below. Obviously, the following description is only some embodiments of the present invention. For those skilled in the art, other embodiments can be obtained based on these embodiments without creative effort.

[0048] Example 1

[0049] To improve the path tracking performance of autonomous vehicles during complex path tracking, and to meet the requirements of vehicle tracking accuracy and stability, this embodiment proposes a path tracking method for autonomous vehicles based on feedforward and predictive LQR. First, a two-degree-of-freedom lateral dynamics model of the autonomous vehicle is established, and a feedforward LQR controller is constructed based on this model. A fuzzy adaptive prediction time based on real-time vehicle speed-curvature is established using a fuzzy control algorithm, and a predictive controller is designed based on the constant turning rate and speed amplitude model (CTRV) presented in this paper. Second, a lateral feedforward + predictive LQR path tracking controller is established based on the feedforward LQR and the predictive controller. The technical route of this invention is as follows: Figure 1 As shown, the specific steps are as follows:

[0050] Step 1: Establish a two-degree-of-freedom lateral dynamics model for autonomous vehicles

[0051] like Figure 2As shown, the model includes the vehicle's own lateral and yaw motions, as well as the lateral and heading errors relative to the reference path. The differential equations of the vehicle's two-degree-of-freedom dynamics model are:

[0052]

[0053]

[0054] During lateral tracking, autonomous vehicles generate two types of errors with the reference path: the error between the vehicle's center of gravity and the desired trajectory point P. re-t0 Distance error (lateral error) e d And the vehicle and the desired trajectory point P re-t0 The direction error (heading error) at the location e θ The design goal of this embodiment is to eliminate these two types of errors, thereby achieving path tracking.

[0055] The differential equation for the lateral error between the vehicle and the tracking path is:

[0056]

[0057]

[0058]

[0059]

[0060] The state-space representation of the two-degree-of-freedom lateral dynamic model is obtained by simultaneously solving the two-degree-of-freedom dynamic differential equation and the lateral error differential equation:

[0061]

[0062]

[0063] U = δ

[0064] Among them, F yf F yr α is the total lateral force exerted on the front and rear axle tires. f α r Let δ be the slip angle of the front and rear wheels, δ be the front wheel steering angle, O be the vehicle's center of gravity, and v be the slip angle of the front and rear wheels. y Let β be the speed of the center of gravity, and l be the sideslip angle of the center of gravity. f l r e represents the distance from the front and rear axles to the center of mass. d Let θ be the lateral error between the vehicle and the desired trajectory, and θ be the vehicle's heading angle. r Let the heading angle be the desired trajectory. C is the yaw angle of the vehicle. f and Cr These are the total lateral stiffness of the front and rear wheels, respectively. z Let v be the moment of inertia about the Z-axis. x This refers to the vehicle's longitudinal speed.

[0065] Step 2: Establish a feedforward LQR controller based on the two-degree-of-freedom lateral dynamics model of the autonomous vehicle.

[0066] For the sake of convenience in designing LQR controllers, the state-space expression is temporarily ignored. The evaluation function and constraint function of LQR control are designed as follows:

[0067]

[0068] In the formula, Q = diag[q1,q2,q3,q4], and each parameter in the matrix represents the sum of e. d (lateral error) (Rate of change of lateral error) (heading error) The degree of importance attached to the (rate of change of heading error), R = [r], represents the degree of importance attached to the front wheel steering angle of the control variable. Based on the mean value theorem of integrals, a method combining the midpoint Euler method and the forward Euler method is used to... Discretize the data:

[0069]

[0070] In the formula, I is the identity matrix, and dt is the sampling time. The evaluation function J is under the constraints. The expression for the minimum value is:

[0071]

[0072] In the formula, Differentiate using the vector derivative rule, let γ k =2P k X k The Riccati equation can be derived as follows:

[0073]

[0074] By solving the Riccati equation, we can obtain:

[0075]

[0076] make The original expression can be simplified to:

[0077] U k =-KX k

[0078] In the formula, K = [K1, K2, K3, K4] are the feedback coefficients of LQR. Feedforward control is designed to eliminate the neglect of... The resulting steady-state error, expressed by feedforward control, is:

[0079]

[0080] In the formula, δ f is the front wheel steering angle output by the feedforward control, and k is the curvature of the reference path.

[0081] Therefore, the output expression of the feedforward LQR controller is: U = -KX + δ f .

[0082] Step 3: Adjust the prediction time online in real time based on fuzzy control to establish a prediction controller.

[0083] like Figure 3 As shown in (a), when the vehicle's current travel path is not on the reference path, the distance from the reference path point P is... re-to If the lateral error is large, the feedforward LQR control system will output a large front wheel steering angle to make the vehicle steer towards the reference path point P. re-t1 Movement. Although it can quickly follow the reference path, vehicle stability and comfort are poor. The vehicle should move towards the reference path point P. re-t2 sports.

[0084] like Figure 3 As shown in (b), when the vehicle's current travel path is on the reference path, the distance between the reference path point P and the reference path point is... re-to The lateral error is very small, and the front wheel steering angle output by the feedforward LQR control system is approximately 0. In the next moment, the vehicle continues driving in its current state until it deviates from the reference path. Only after the system determines the existence of a lateral error will it output a certain front wheel steering angle. At this point, the control system exhibits a certain degree of lag, and the vehicle should move towards the desired trajectory point P. re-t2 sports.

[0085] To address the potential issues with the aforementioned feedforward LQR controller, a predictive controller is designed based on a constant turning rate and speed amplitude model (CTRV) to predict the vehicle's position over a future period, thereby improving vehicle stability while maintaining tracking accuracy.

[0086] The constant turning rate and speed amplitude model (CTRV) is shown below:

[0087]

[0088] in:

[0089]

[0090]

[0091] In the formula, τ p The value of this parameter is crucial for predicting time. If the value is too small, the prediction range is small, and the distance between the current vehicle position and the predicted vehicle position is too close, resulting in a weak prediction. If the value is too large, the predicted vehicle position will deviate significantly from the current vehicle position, worsening the prediction performance. pre y pre v pre These represent the predicted vehicle's x-coordinate, y-coordinate, yaw angle, vehicle speed, and yaw rate (which can also be represented as the predicted vehicle's position and state information), respectively. T is the transpose symbol, and t is time.

[0092] like Figure 4 As shown, the reference path point P is used to predict the vehicle's position and a certain prediction time. re-ti The lateral error model updates the current vehicle position and the current reference path point P. re-t0 The lateral error model.

[0093] To select the optimal prediction time to adapt to various road conditions, this invention designs an adaptive prediction controller based on vehicle speed and road curvature using a fuzzy control algorithm. The two input variables v and kr of its fuzzification interface are fuzzified, with a precision level of 7 selected; the output variable τ of the defuzzification interface... p Select 11 precision levels for classification:

[0094] v = {NB NM NS ZO PS PM PB};

[0095] kr={NB NM NS ZO PS PM PB};

[0096] τ p ={NA NL NB NM NS ZO PS PM PB PL PA};

[0097] like Figure 5 and Figure 6 As shown, Gauss2mf-type membership functions are applied to the input variables v and kr; for example... Figure 7 As shown, for the output variable τ p Use Trimf type membership functions. For example... Figure 8 As shown, the Mamdani algorithm and the average value method based on the maximum membership degree are used to analyze the output variable τ. p Defuzzification (unfuzzing) is performed to output the accurate τ required by the predictive controller. p .

[0098] Step 4: As Figure 9As shown, a feedforward + predictive LQR controller is established based on feedforward LQR and a predictive controller. According to the feedforward + predictive LQR path tracking method and experimental setup for autonomous vehicles, based on the vehicle state information and path information collected and processed by the onboard system, the prediction time required by the predictive controller is calculated through fuzzy control. Based on the prediction time, the predictive controller updates the two-degree-of-freedom lateral dynamics model of the vehicle and a reference path point at a certain prediction time in real time. Based on the real-time updated two-degree-of-freedom lateral dynamics model, the feedforward LQR controller calculates the front wheel steering angle output by the control system to the autonomous vehicle.

[0099] Example 2

[0100] This embodiment discloses an experimental device for path tracking of autonomous vehicles based on feedforward and prediction LQR. The device is built based on CAN communication between LabVIEW and Simulink, according to the above-mentioned path tracking method for autonomous vehicles based on feedforward and prediction LQR.

[0101] like Figure 10 As shown, a CAN communication experimental device based on LabVIEW and Simulink was built. Based on the feedforward + prediction LQR path tracking method and experimental device for autonomous vehicles, a 12V battery was selected to power the steering motor. A control algorithm was built based on Simulink, and LabVIEW was used to convert the Simulink physical signals into CAN signals and transmit them to the controller to control the steering motor (converting CAN signals into physical signals and transmitting them to Simulink serves as system feedback), thus achieving closed-loop control.

Claims

1. A path tracking method for autonomous vehicles based on feedforward and prediction LQR, characterized in that, include: Model establishment: Establish a two-degree-of-freedom lateral dynamics model for autonomous vehicles; Constructing a feedforward LQR controller: Construct a feedforward LQR controller based on a two-degree-of-freedom transverse dynamics model; Establish a predictive controller: Based on the fuzzy control algorithm, establish a fuzzy adaptive prediction time based on real-time vehicle speed-curvature, and design a predictive controller based on a constant turning rate and speed amplitude model; The steps for establishing the predictive controller specifically include: Based on the influence of road curvature and vehicle speed on prediction time, and according to fuzzy control theory, road curvature and vehicle speed are used as inputs to the fuzzy controller, and prediction time is used as the output of the fuzzy controller, so as to adjust the prediction time online in real time. The road curvature, vehicle speed, and prediction time of the autonomous vehicle are classified into accuracy levels based on fuzzy control. Membership functions are applied to the input and output variables of the autonomous vehicle prediction time. The Mamdani algorithm and the average value method of the maximum membership degree are used to defuzzify the output variable of the autonomous vehicle prediction time. The vehicle's position over a certain period of time is predicted based on a constant turning rate and speed amplitude model. The two-degree-of-freedom lateral dynamics model of the vehicle's position and a reference path point over a certain prediction time is then used to update the two-degree-of-freedom lateral dynamics model of the current vehicle position and the current reference path point. Establish a feedforward + predictive LQR controller: Based on the feedforward LQR and predictive controller, establish a lateral feedforward + predictive LQR path tracking controller.

2. The path tracking method for autonomous vehicles based on feedforward and prediction LQR according to claim 1, characterized in that, The two-degree-of-freedom lateral dynamics model includes the vehicle's own lateral and yaw motions, as well as the lateral and heading errors relative to the reference path.

3. The path tracking method for autonomous vehicles based on feedforward and prediction LQR according to claim 1, characterized in that, The input to the feedforward LQR controller is the vehicle body status and road information collected and processed by the on-board system, and the output of the feedforward LQR controller is the front wheel steering angle.

4. The path tracking method for autonomous vehicles based on feedforward and prediction LQR according to claim 1, characterized in that, The road curvature of the autonomous vehicle is divided into seven accuracy levels based on fuzzy control, the vehicle speed of the autonomous vehicle is divided into seven accuracy levels based on fuzzy control, and the prediction time of the autonomous vehicle is divided into eleven accuracy levels based on fuzzy control.

5. The path tracking method for autonomous vehicles based on feedforward and prediction LQR according to claim 1, characterized in that, The adoption of membership functions specifically involves adopting a Gauss2mf-type membership function for the input variables of the autonomous vehicle's road curvature and vehicle speed, and a Trimf-type membership function for the output variable of the autonomous vehicle's prediction time.

6. The path tracking method for autonomous vehicles based on feedforward and prediction LQR according to claim 1, characterized in that, The specific operation method of the feedforward + prediction LQR controller includes: Based on the vehicle status and path information collected and processed by the in-vehicle system, the prediction time required by the predictive controller is calculated through fuzzy control. The predictive controller based on prediction time updates the two-degree-of-freedom lateral dynamics model of the vehicle and a reference path point at a certain prediction time in real time. Based on a real-time updated two-degree-of-freedom lateral dynamics model, the front wheel steering angle output by the control system to the autonomous vehicle is calculated by a feedforward LQR controller.

7. An experimental device for path tracking of autonomous vehicles based on feedforward and prediction LQR, characterized in that, The path tracking method for autonomous vehicles based on feedforward and prediction LQR as described in any one of claims 1-6 is built based on CAN communication between LabVIEW and Simulink.

8. The experimental apparatus for path tracking of autonomous vehicles based on feedforward and prediction LQR according to claim 7, characterized in that, The control algorithm for the path tracking method of autonomous vehicles based on feedforward and prediction LQR as described in any one of claims 1-6 is built based on Simulink. The physical signals of Simulink are converted into CAN signals and transmitted to the motor controller to control the operation of the steering motor, thereby realizing closed-loop control.

9. The experimental apparatus for path tracking of autonomous vehicles based on feedforward and prediction LQR according to claim 8, characterized in that, The steering motor is powered by a storage battery.

Images