Automatic driving vehicle local path planning and trajectory tracking integrated control method

Abstract

The invention relates to the technical field of local path planning and trajectory tracking of an automatic driving vehicle, in particular to a local path planning and LQR trajectory tracking integrated control research method based on a long short-term memory neural network, and the method comprises the steps: obtaining a state quantity under a Frenet coordinate system at any moment through the mutual conversion of a geodetic coordinate system and the Frenet coordinate system; acquiring a vehicle guide driving reference line based on the global path; predicting a dynamic obstacle driving track based on a long short-term memory neural network; trajectory planning and speed planning are carried out based on a dynamic planning algorithm and a quadratic planning algorithm; splicing the trajectory of the current frame and the planned frame based on a polynomial, and issuing to a trajectory tracker; and designing an LQR trajectory tracking controller and a PID speed tracker based on the vehicle two-degree-of-freedom error model. And finally, safe obstacle avoidance of the automatic driving vehicle to the dynamic obstacle is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of local path planning and trajectory tracking of autonomous driving vehicles, and in particular to a research method for local path planning and LQR trajectory tracking integrated control based on long short-term memory neural network. Background Art

[0002] As the automotive industry gradually moves towards intelligent networking and unmanned driving, the advantages of autonomous driving technology have become increasingly prominent, making it an indispensable part of the intelligent transportation system. Path planning and trajectory tracking, as important technologies for the development of autonomous driving, play a vital role in the guidance and safety of vehicles, and have gradually become hot topics in the field of autonomous driving research. At present, how to accurately predict the vehicle's behavior trajectory, speed, etc., and how to reasonably plan the vehicle's driving path based on this information and stably track the path is one of the important challenges in the field of autonomous driving, and it has important research significance for improving the driving safety and stability of autonomous driving vehicles.

[0003] In local path planning, accurate trajectory prediction of obstacles, reasonable constraints on the lateral and longitudinal kinematic parameters of the vehicle, ensuring accurate tracking of the vehicle's trajectory, and safe obstacle avoidance on roads with changing curvature have become difficulties that need to be solved in the field of autonomous driving path planning and trajectory tracking. Local path planning relies on the dynamic environment information generated by sensors to plan a local obstacle avoidance curve under the constraints of vehicle motion parameters to ensure safe driving of the vehicle. At present, the main local path planning methods include Bezier curves, B-splines and other spline curve methods; dynamic window approach (DWA), time elastic band algorithm (TEB), and optimization algorithms. Although the paths generated by spline curve methods such as Bezier curves and B-splines are very smooth and suitable for actual vehicle driving, the path shape can be fine-tuned by adjusting the control points, but because it may be difficult to find suitable control points in complex environments, they are mostly used in static environments. Based on the dynamic window method, feasible paths can be quickly calculated in dynamic environments, and paths that meet vehicle dynamics constraints can be generated. It is suitable for high-speed path solving, but it may fall into local optimal solutions rather than global optimal solutions, and it is more dependent on the accuracy of grid maps. The time elastic band algorithm performs well in real-time, but its optimization process involves more calculations, especially when dealing with high-density obstacles and complex environments. The calculation complexity and time overhead are high, it is more dependent on the quality of the initial path, parameter tuning is difficult and it is easy to fall into the local optimal solution. Compared with the above algorithms, the dynamic programming algorithm can decompose complex problems into sub-problems to solve and obtain the optimal solution, and can make effective obstacle avoidance decisions. After the quadratic programming algorithm constrains the dynamic parameters of the autonomous driving vehicle, it can better obtain a local path that meets the vehicle parameters. Secondly, with the increasing number of intelligent systems in the human environment, the ability of the system to perceive, understand and predict human behavior has become increasingly important. Accurately predicting the trajectory of dynamic obstacles is particularly important for the obstacle avoidance planning of autonomous driving vehicles, and it is the guarantee of autonomous driving driving safety. Currently, the commonly used trajectory prediction methods include Markov decision model, Bayesian model, Gaussian mixture model, MPC prediction model, Kalman prediction model, and long short-term neural network memory model (LSTM). The prediction method based on the MPC model cannot predict long trajectory information due to the limitation of the vehicle dynamics model; the Kalman model and Gaussian mixture model are greatly affected by noise and lack precision, and cannot accurately meet the requirements of local planning of autonomous driving vehicles. The trajectory predicted based on the Markov decision model and Bayesian algorithm is easily affected by the fluctuation of input data, resulting in inaccurate prediction of the trajectory.Compared with the prediction algorithms mentioned above, the long short-term memory neural network can more accurately store the historical trajectory information with a longer memory to predict the trajectory of the next frame. It can use very long sequence information to build a learning model, so it is very advantageous to use it for time series prediction. Finally, in order to dynamically verify the accuracy and effectiveness of the path planning results, it is also particularly important to accurately track the trajectory output by the upstream controller. At present, the commonly used trajectory tracking algorithms include proportional-integral-derivative controller (PID), linear quadratic regulator (LQR), model predictive control (MPC), Stanley tracking algorithm, etc. The PID control algorithm can follow the desired trajectory well, but the parameters are difficult to adjust, and when dealing with nonlinear systems, complex dynamic systems or system parameters that change over time, the performance may be poor and there may be steady-state errors. The MPC-based trajectory tracking algorithm can handle multi-constraint problems and can control multiple variables well, but the computational cost is too high. The Stanley-based trajectory tracking algorithm is robust, simple and efficient, but when encountering sharp curves or extremely high speeds, the control performance may be affected. As one of the earliest and most mature state-space design methods developed in modern control theory, the LQR theory is based on the vehicle dynamics model. The LQR algorithm has a low computational cost, can respond quickly at low and high speeds, has good real-time performance, and is widely used in high-level autonomous driving technology. Summary of the invention

[0004] The purpose of the present invention is to provide a research method for local path planning and LQR trajectory tracking integrated control based on long short-term memory neural network, so as to improve the safety and tracking accuracy of local path planning results of autonomous driving vehicles when facing dynamic obstacles and environmental complexity, and ensure the safe driving of autonomous driving vehicles.

[0005] To achieve the above object, the present invention provides a local path planning and LQR trajectory tracking integrated control method based on long short-term memory neural network, comprising the following steps:

[0006] In a first aspect, the present invention provides an obstacle trajectory prediction method, the method comprising:

[0007] By converting between the geodetic coordinate system and the Frenet coordinate system, the state quantity in the Frenet coordinate system at any time can be obtained;

[0008] Obtaining a vehicle driving guidance reference line based on the global path;

[0009] Predicting dynamic obstacle trajectories based on long short-term memory neural networks;

[0010] Combined with the first aspect, in the Cartesian coordinate system, the lateral trajectory and longitudinal trajectory of the vehicle are coupled to each other, while in the Frenet coordinate system, the distance between the vehicle position and the reference line can be represented by s and l respectively, and the longitudinal and lateral trajectory speeds can be obtained by taking the derivative with respect to time. Finally, the two-dimensional trajectory problem of the vehicle is decoupled into two one-dimensional trajectory problems in the Frenet coordinate system, reducing the complexity of the calculation, specifically:

[0011]

[0012] in The actual state of the Cartesian coordinate system, is the state parameter of the Frenet coordinate system, s is the longitudinal displacement, is the longitudinal velocity, is the longitudinal acceleration, l is the lateral displacement, is the lateral velocity, is the lateral acceleration, l' is the first derivative with respect to the arc length, and l" is the second derivative with respect to the arc length.

[0013] Furthermore, the global path obtained by the global planning path algorithm is too long and tortuous, which is not conducive to accurate tracking by the downstream trajectory tracker. In order to ensure the smoothness of the trajectory and the stability of vehicle driving, the method of reducing the number of reference trajectory points and shortening the length of the reference path is adopted, and the global planning path is optimized by secondary planning to obtain a smooth global reference line with an appropriate number of trajectory points to guide the vehicle to drive along the reference line.

[0014] The process of obtaining the vehicle guidance driving reference line based on the global path is as follows:

[0015] Reference line optimization problem definition:

[0016] J=C smooth +C ref +C length

[0017] Define the smoothing cost:

[0018]

[0019] Among them, w smooth is the weight coefficient, P i is the current point coordinate (x i ,y i ),P i-1 is the coordinate of the previous discrete point (x i-1 ,y i-1 ), P i+1 is the coordinate of the next discrete point (xi+1 ,y i+1 ).

[0020] Define the length cost:

[0021]

[0022] Define the offset cost:

[0023]

[0024] Among them, (x ir ,y ir ) are the discrete points of the original reference line path.

[0025] Furthermore, the dynamic obstacle trajectory prediction process is based on the long short-term memory neural network, specifically:

[0026] Forget Gate:

[0027] f t =σ(W f' ·[h t-1 ,x t ])

[0028] Among them, h t-1 =[x t-1 ,y t-1 ] T represents the input vector at the previous moment, x t =[x,y,kappa,θ,v x ,v y ] T is the current input vector, W' f is the forget gate weight matrix, b f is the bias term of the forget gate, σ is the activation function of each gate, and f t is the output of the forget gate. W' f It can be seen as W f and U f The two matrices are concatenated, corresponding to h t-1 and x t .

[0029] Input Gate:

[0030] i t =σ(W i ·[h t-1 ,x t ]+b i )

[0031] Where W i is the weight matrix, b i is the bias term, i t is the output vector.

[0032] According to h t-1 and x t Get an update on the cell state at the current moment:

[0033]

[0034] The cell state equation at time T is:

[0035]

[0036] Output Gate:

[0037] o t =σ(W o ·[h t-1 ,x t ]+b o )

[0038] h t =o t ⊙tanh(C t )

[0039] A second aspect: The present invention provides a method for trajectory planning and trajectory tracking of an autonomous driving vehicle, the method comprising:

[0040] Trajectory planning and speed planning based on dynamic programming algorithm and quadratic programming algorithm;

[0041] The trajectory of the current frame and the planned frame are concatenated based on polynomials and sent to the trajectory tracker;

[0042] The LQR trajectory tracking controller and PID speed tracker are designed based on the vehicle's two-degree-of-freedom error model.

[0043] Combined with the first aspect, the trajectory planning cost function in dynamic programming is defined as:

[0044] Cost tra =Cost l +Cost dl +Cost ddl +Cost obs

[0045] Among them, l is the lateral displacement, l' is the lateral displacement velocity, and l" is the lateral displacement acceleration. w l is the lateral displacement cost weight coefficient, w dl is the lateral velocity cost weight coefficient, w ddl is the lateral acceleration cost weight coefficient,

[0046] (1) The obstacle cost is:

[0047]

[0048] Among them, l min , l max is the maximum and minimum lateral distance between obstacle boundary points, s min 、s max is the maximum and minimum longitudinal distance between the obstacle boundary points, and sigmod is the activation function that can map real numbers to the interval (0,1).

[0049] In dynamic programming, the speed planning cost function is defined as:

[0050] cost=cost_speed+cost_acc+cost_jerk+cost_obs

[0051] (1) Obstacle collision cost:

[0052]

[0053] Among them, s min is the minimum longitudinal distance of the vehicles at time t, s max is the corresponding maximum longitudinal distance, s is the longitudinal distance of the sampling point, buffer is the margin, and w obs is the obstacle cost coefficient.

[0054] (2) Definition of speed cost

[0055]

[0056] Among them, w speed is the speed cost coefficient, v is the actual vehicle speed, v ref The reference speed.

[0057] (3) Definition of acceleration cost

[0058]

[0059] Among them, s(t+1,s+k) is the state quantity at the next moment, s(t,s+k) is the state quantity at the current moment, and w acc is the speed cost coefficient, and a is the actual acceleration.

[0060] (4) Jerk cost

[0061] Jerk is characterized by an impact amount, which is mainly used to reduce the discomfort caused by sudden acceleration of the vehicle and ensure driving comfort.

[0062]

[0063] In quadratic programming, the trajectory planning cost function is defined as:

[0064]

[0065] Among them, l ref The lateral displacement obtained for the dynamic programming.

[0066] In quadratic programming, the speed planning cost function is defined as:

[0067]

[0068] where s ref(i) Get the longitudinal displacement of the path for dynamic programming, Get the longitudinal velocity of the path for dynamic planning, is the longitudinal acceleration, is the longitudinal acceleration, w s is the reference longitudinal distance cost coefficient, w vel is the reference longitudinal velocity. Cost coefficient, w acc is the reference longitudinal acceleration. Cost coefficient, w jerk is the reference longitudinal acceleration cost coefficient.

[0069] Furthermore, the trajectory of the current frame and the planned frame is concatenated based on the polynomial and sent to the trajectory tracker, specifically:

[0070] l(s)=a 0 +a 1 ·s+a 2 ·s 2 +a 3 ·s 3 +a 4 ·s 4 +a 5 ·s 5 dl(s)=a 1 +2a 2 ·s+3a 3 ·s 2 +4a 4 ·s 3 +5a 5 ·s 4

[0071] ddl(s)=2a 2 +6a 3 ·s+12a 4 ·s 2 +20a 5 ·s 3 dddl(s)=6a 3 ·s+24a 4 ·s+60a 5 ·s 2

[0072] Furthermore, the LQR trajectory tracking controller and PID speed tracker process are designed based on the vehicle two-degree-of-freedom error model, specifically:

[0073] The tracking error state space equation of the LQR vehicle lateral dynamics model is:

[0074]

[0075] in, is the lateral error, lateral error change rate, yaw angle error, and yaw angle error change rate of the vehicle, l f is the distance from the front axle to the center of mass, l r is the distance from the rear axle to the center of mass, C αf is the front tire cornering stiffness, C αr is the cornering stiffness of the rear tire, δ is the front wheel steering angle, v x is the longitudinal velocity of the vehicle, I z is the moment of inertia of the vehicle around the z axis, m is the total mass of the vehicle, is the desired yaw rate of the vehicle, is the expected yaw angular acceleration.

[0076] Feedforward Controller Design:

[0077]

[0078] Where κ is the curvature, k 3 is the gain factor.

[0079] Longitudinal PID speed tracker design:

[0080]

[0081] Among them, e s is the longitudinal error, e v is the longitudinal velocity error, k p ,k i ,k d ,K p ,K i ,K d is the PID control gain coefficient.

[0082] The present invention relates to the technical field of local path planning and trajectory tracking of autonomous driving vehicles, and specifically to a local path planning and LQR trajectory tracking integrated control research method based on long short-term memory neural network. By converting between the earth coordinate system and the Frenet coordinate system, the state quantity in the Frenet coordinate system at any time is obtained; the vehicle guidance driving reference line is obtained based on the global path; the dynamic obstacle driving trajectory is predicted based on the long short-term memory neural network; trajectory planning and speed planning are performed based on the dynamic programming algorithm and the quadratic programming algorithm; the current frame and the planned frame trajectory are concatenated based on polynomials and sent to the trajectory tracker; and the LQR trajectory tracking controller and PID speed tracker are designed based on the vehicle two-degree-of-freedom error model. Finally, the autonomous driving vehicle can avoid dynamic obstacles safely. The present invention provides an effective solution for avoiding dynamic obstacles of autonomous driving vehicles, and can plan a safer obstacle avoidance path and control the vehicle to drive stably along the safe path. The safety and driving performance of autonomous driving vehicles are improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0083] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0084] Figure 1 The invention discloses a local path planning and LQR trajectory tracking integrated control research method based on long short-term memory neural network.

[0085] Figure 2 It is a schematic diagram of mutual conversion between the geodetic coordinate system and the Frenet coordinate system of the present invention;

[0086] Figure 3 The present invention is a schematic diagram of a vehicle guiding driving reference line obtained based on a global path;

[0087] Figure 4 It is a schematic diagram of the prediction principle based on long short-term memory neural network of the present invention;

[0088] Figure 5 It is a schematic diagram of the dynamic programming algorithm in the trajectory planning of the present invention;

[0089] Figure 6 It is a schematic diagram of the secondary planning in the trajectory planning of the present invention.

[0090] Figure 7 It is a schematic diagram of the dynamic programming algorithm in the speed planning of the present invention.

[0091] Figure 8It is a schematic diagram of secondary planning in speed planning of the present invention.

[0092] Fig. 9 This is a schematic diagram of the polynomial trajectory splicing of the present invention.

[0093] Fig.10 It is a schematic diagram of the LQR trajectory tracking algorithm based on the vehicle dynamics model of the present invention. DETAILED DESCRIPTION

[0094] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and should not be construed as limiting the present invention.

[0095] The following are some nouns and terminology descriptions of the present invention:

[0096] Long Short-Term Memory Network (LSTM)

[0097] Dynamic Programming (DP)

[0098] Quadratic Programming (QP)

[0099] Linear Quadratic Regulator (LQR)

[0100] See also Figure 1 , a local path planning and trajectory tracking integrated control method process for an autonomous driving vehicle, comprising the following steps:

[0101] S1: Obtain the state quantity in the Frenet coordinate system at any time by converting between the geodetic coordinate system and the Frenet coordinate system;

[0102] For details, please refer to Figure 2 The diagram of the conversion between the geodetic coordinate system and the Frenet coordinate system is provided. In the Cartesian coordinate system, the lateral trajectory and longitudinal trajectory of the vehicle are coupled to each other, while in the Frenet coordinate system, the distance between the vehicle position and the reference line can be expressed as s and l respectively. The longitudinal and lateral trajectory speeds can be obtained by taking the derivative of time respectively. Finally, the two-dimensional trajectory problem of the vehicle is decoupled into two one-dimensional trajectory problems in the Frenet coordinate system, reducing the complexity of the calculation, specifically:

[0103]

[0104] in The actual state of the Cartesian coordinate system, is the state parameter of the Frenet coordinate system, s is the longitudinal displacement, is the longitudinal velocity, is the longitudinal acceleration, l is the lateral displacement, is the lateral velocity, is the lateral acceleration, l' is the first derivative with respect to the arc length, and l" is the second derivative with respect to the arc length.

[0105] S2: The process of obtaining the vehicle driving guidance reference line based on the global path;

[0106] For details, please refer to Figure 3 The provided schematic diagram of the vehicle guidance driving reference line based on the global path. The global path obtained by the global planning path algorithm is too long and tortuous, which is not conducive to the accurate tracking of the downstream trajectory tracker. In order to ensure the smoothness of the trajectory and the stability of vehicle driving, the method of reducing the number of reference trajectory points and shortening the reference path length is adopted, and the global planning path is optimized by secondary planning to obtain a smooth global reference line with an appropriate number of trajectory points to guide the vehicle to drive along the reference line.

[0107] The process of obtaining the vehicle guidance driving reference line based on the global path is as follows:

[0108] Reference line optimization problem definition:

[0109] J=C smooth +C ref +C length

[0110] Define the smoothing cost:

[0111]

[0112] Among them, w smooth is the weight coefficient, P i is the current point coordinate (x i ,y i ),P i-1 is the coordinate of the previous discrete point (x i-1 ,y i-1 ), P i+1 is the coordinate of the next discrete point (x i+1 ,y i+1 ).

[0113] Define the length cost:

[0114]

[0115] Define the offset cost:

[0116]

[0117] Among them, (x ir ,y ir ) are the discrete points of the original reference line path.

[0118] S3: Predicting dynamic obstacle trajectory based on long short-term memory neural network;

[0119] For details, please refer to Figure 4 The provided diagram is based on the prediction principle of the long short-term memory neural network. The fixed-length short-term memory neural network can store the historical trajectory information with a longer memory more accurately to predict the trajectory of the next frame. It can use very long sequence information to establish a learning model, so it is very advantageous to use it for time series prediction.

[0120] Forget Gate:

[0121] f t =σ(W f' ·[h t-1 ,x t ])

[0122] Among them, h t-1 =[x t-1 ,y t-1 ] T represents the input vector at the previous moment, x t =[x,y,kappa,θ,v x ,v y ] T is the current input vector, W' f is the forget gate weight matrix, b f is the bias term of the forget gate, σ is the activation function of each gate, and f t is the output of the forget gate. W' f It can be seen as W f and U f The two matrices are concatenated, corresponding to h t-1 and x t .

[0123] Input Gate:

[0124] i t =σ(W i ·[h t-1 ,x t ]+b i )

[0125] Where W i is the weight matrix, b i is the bias term, i tis the output vector.

[0126] According to h t-1 and x t Get an update on the cell state at the current moment:

[0127]

[0128] The cell state equation at time T is:

[0129]

[0130] Output Gate:

[0131] o t =σ(W o ·[h t-1 ,x t ]+b o )

[0132] h t =o t ⊙tanh(C t )

[0133] S4: Trajectory planning and speed planning process based on dynamic programming algorithm and quadratic programming algorithm;

[0134] For details, please refer to Figure 5 Schematic diagram of dynamic programming algorithm in trajectory planning. In dynamic programming, the trajectory planning cost function is defined as:

[0135] In dynamic programming, the trajectory planning cost function is defined as:

[0136] Cost tra =Cost l +Cost dl +Cost ddl +Cost obs

[0137] Among them, l is the lateral displacement, l is the lateral displacement velocity, l” is the lateral displacement acceleration, w l is the lateral displacement cost weight coefficient, w dl is the lateral velocity cost weight coefficient, w ddl is the lateral acceleration cost weight coefficient,

[0138] (1) The obstacle cost is:

[0139]

[0140] Among them, l min , l max is the maximum and minimum lateral distance between obstacle boundary points, smin 、s max is the maximum and minimum longitudinal distance between the obstacle boundary points, and sigmod is the activation function that can map real numbers to the interval (0,1).

[0141] For details, please refer to Figure 6 Schematic diagram of the quadratic programming algorithm in trajectory planning. In quadratic programming, the trajectory planning cost function is defined as:

[0142]

[0143] Among them, l ref The lateral displacement obtained for the dynamic programming.

[0144] For details, please refer to Figure 7 Schematic diagram of the dynamic programming algorithm in speed planning. In dynamic programming, the speed planning cost function is defined as:

[0145] cost=cost_speed+cost_acc+cost_jerk+cost_obs

[0146] (1) Obstacle collision cost:

[0147]

[0148] Among them, s min is the minimum longitudinal distance of the vehicles at time t, s max is the corresponding maximum longitudinal distance, s is the longitudinal distance of the sampling point, buffer is the margin, and w obs is the obstacle cost coefficient.

[0149] (2) Definition of speed cost

[0150]

[0151] Among them, w speed is the speed cost coefficient, v is the actual vehicle speed, v ref The reference speed.

[0152] (3) Definition of acceleration cost

[0153]

[0154] Among them, s(t+1,s+k) is the state quantity at the next moment, s(t,s+k) is the state quantity at the current moment, and w acc is the speed cost coefficient, and a is the actual acceleration.

[0155] (4) Jerk cost

[0156] Jerk is characterized by an impact amount, which is mainly used to reduce the discomfort caused by sudden acceleration of the vehicle and ensure driving comfort.

[0157]

[0158]

[0159] For details, please refer to Figure 8 Schematic diagram of the quadratic programming algorithm in speed planning. In quadratic programming, the speed planning cost function is defined as:

[0160]

[0161] where s ref(i) Get the longitudinal displacement of the path for dynamic programming, Get the longitudinal velocity of the path for dynamic planning, is the longitudinal acceleration, is the longitudinal acceleration, w s is the reference longitudinal distance cost coefficient, w vel is the reference longitudinal velocity. Cost coefficient, w acc is the reference longitudinal acceleration. Cost coefficient, w jerk is the reference longitudinal jerk penalty coefficient.

[0162] S5: The process of splicing the current frame and the planned frame trajectory based on the polynomial and sending it to the trajectory tracker;

[0163] For details, please refer to Fig. 9 Schematic diagram of polynomial trajectory splicing. In the planning algorithm, if the trajectory is replanned for each frame, it will cause additional time overhead, and the discontinuous trajectory sequence will easily cause the vehicle to jitter. The purpose of trajectory splicing is to enable the trajectory tracking control algorithm to better track the trajectory and ensure the continuity of the tracking trajectory.

[0164] l(s)=a 0 +a 1 ·s+a 2 ·s 2 +a 3 ·s 3 +a 4 ·s 4 +a 5 ·s 5 dl(s)=a 1 +2a 2 ·s+3a 3 ·s 2 +4a 4 ·s 3 +5a 5 ·s 4

[0165] ddl(s)=2a 2 +6a 3 ·s+12a 4 ·s 2 +20a 5 ·s 3 dddl(s)=6a 3 ·s+24a 4 ·s+60a 5 ·s 2

[0166] S6: Design the LQR trajectory tracking controller and PID speed tracker process based on the vehicle two-degree-of-freedom error model.

[0167] For details, please refer to Fig.10 Schematic diagram of the LQR trajectory tracking algorithm based on the vehicle dynamics model. First, a single-track two-degree-of-freedom dynamics model of the vehicle is established. Secondly, the state equation of the vehicle lateral dynamics tracking error model is built, and the vehicle tracking error is calculated. Then, the PID speed tracking controller is designed based on the vehicle tracking error and speed error to achieve vehicle speed tracking. Finally, the linear quadratic solver is used to solve the optimal feedback control quantity to control the vehicle to travel along the desired trajectory planning path.

[0168] The tracking error state space equation of the LQR vehicle lateral dynamics model is:

[0169]

[0170]

[0171] in, is the lateral error, lateral error change rate, yaw angle error, and yaw angle error change rate of the vehicle, l f is the distance from the front axle to the center of mass, l r is the distance from the rear axle to the center of mass, C αf is the front tire cornering stiffness, C αr is the cornering stiffness of the rear tire, δ is the front wheel steering angle, v x is the longitudinal velocity of the vehicle, I z is the moment of inertia of the vehicle around the z axis, m is the total mass of the vehicle, is the desired yaw rate of the vehicle, is the expected yaw angular acceleration.

[0172] Feedforward Controller Design:

[0173]

[0174] Where κ is the curvature, k 3is the gain factor.

[0175] Longitudinal PID speed tracker design:

[0176]

[0177] Among them, e s is the longitudinal error, e v is the longitudinal velocity error, k p ,k i ,k d ,K p ,K i ,K d is the PID control gain coefficient.

[0178] What is disclosed above is only a preferred embodiment of the present invention, and it certainly cannot be used to limit the scope of rights of the present invention. Ordinary technicians in this field can understand that all or part of the processes of the above embodiment and equivalent changes made according to the claims of the present invention still fall within the scope of the invention.

Claims

1. A local path planning and trajectory tracking integrated control method for an autonomous driving vehicle, characterized in that: The following steps are involved: By converting between the geodetic coordinate system and the Frenet coordinate system, the state quantity in the Frenet coordinate system at any time can be obtained; Obtaining a vehicle driving guidance reference line based on the global path; Predicting dynamic obstacle trajectories based on long short-term memory neural networks; Trajectory planning and speed planning based on dynamic programming algorithm and quadratic programming algorithm; The trajectory of the current frame and the planned frame are concatenated based on polynomials and sent to the trajectory tracker; The LQR trajectory tracking controller and PID speed tracker are designed based on the vehicle's two-degree-of-freedom error model.

2. The local path planning and trajectory tracking integrated control method for an autonomous driving vehicle according to claim 1, characterized in that: The process of converting the geodetic coordinate system into the Frenet coordinate system comprises the following steps: In the Cartesian coordinate system, the lateral trajectory and longitudinal trajectory of the vehicle are coupled to each other, while in the Frenet coordinate system, the distance between the vehicle position and the reference line can be expressed by s and l respectively. The longitudinal and lateral trajectory velocities can be obtained by taking the derivative of time respectively. Finally, the two-dimensional trajectory problem of the vehicle is decoupled into two one-dimensional trajectory problems in the Frenet coordinate system, which reduces the complexity of the calculation. Specifically: in is the actual state of the Cartesian coordinate system, is the state parameter of the Frenet coordinate system, s is the longitudinal displacement, is the longitudinal velocity, is the longitudinal acceleration, l is the lateral displacement, is the lateral velocity, is the lateral acceleration, l' is the first derivative with respect to the arc length, and l" is the second derivative with respect to the arc length.

3. The integrated control method for local path planning and trajectory tracking of an autonomous driving vehicle according to claim 1, characterized in that: The method of optimizing the initial global path by using a quadratic programming method to obtain a reference line includes the following steps: The global path obtained by the global path planning algorithm is too long and tortuous, which is not conducive to accurate tracking by the downstream trajectory tracker. In order to ensure the smoothness of the trajectory and the stability of vehicle driving, the method of reducing the number of reference trajectory points and shortening the length of the reference path is adopted, and the global planning path is optimized by secondary planning to obtain a smooth global reference line with an appropriate number of trajectory points to guide the vehicle to drive along the reference line. The process of obtaining the vehicle guidance driving reference line based on the global path is as follows: Reference line optimization problem definition: J=C smooth +C ref +C length Define the smoothing cost: Among them, w smooth is the weight coefficient, P i is the current point coordinate (x i ,y i ),P i-1 is the coordinate of the previous discrete point (x i-1 ,y i-1 ), P i+1 is the coordinate of the next discrete point (x i+1 ,y i+1 ). Define the length cost: Define the offset cost: Among them, (x ir ,y ir ) are the discrete points of the original reference line path.

4. The integrated control method for local path planning and trajectory tracking of an autonomous driving vehicle according to claim 1, characterized in that: The method of predicting the next frame trajectory of an obstacle based on a long short-term memory neural network includes the following steps: Long short-term memory neural networks can store historical trajectory information with relatively high accuracy to predict the trajectory of the next frame. They can use very long sequence information to build learning models, so using them to predict time series is very advantageous. The process of predicting the trajectory of dynamic obstacles based on long short-term memory neural network is as follows: Forget Gate: f t =σ(W f' ·[h t-1 ,x t ]) Among them, h t-1 =[x t-1 ,y t-1 ] T represents the input vector at the previous moment, x t =[x,y,kappa,θ,v x ,v y ] T is the current input vector, W′ f is the forget gate weight matrix, b f is the bias term of the forget gate, σ is the activation function of each gate, and f t is the output of the forget gate. f It can be seen as W f and U f The two matrices are concatenated, corresponding to h t-1 and x t . Input Gate: i t =σ(W i ·[h t-1 ,x t ]+b i ) Where W i is the weight matrix, b i is the bias term, i t is the output vector. According to h t-1 and x t Get an update on the cell state at the current moment: The cell state equation at time T is: Output Gate: the t =σ(W o ·[h t-1 ,x t ]+b o ) h t =o t ⊙tanh(C t ) 5. The integrated control method for local path planning and trajectory tracking of an autonomous driving vehicle according to claim 1, characterized in that: The dynamic programming algorithm and the quadratic programming algorithm perform trajectory planning and speed planning, including the following steps: In local path planning, accurate trajectory prediction of obstacles is performed, and the vehicle's lateral and longitudinal kinematic parameters are reasonably constrained to achieve dynamic and safe obstacle avoidance of the vehicle. In dynamic programming, the trajectory planning cost function is defined as: Cost tra =Cost l +Cost dl +Cost ddl +Cost obs Where, l is the lateral displacement, l' is the lateral displacement velocity, and l" is the lateral displacement acceleration. w l is the lateral displacement cost weight coefficient, w dl is the lateral velocity cost weight coefficient, w ddl is the lateral acceleration cost weight coefficient, (1) The obstacle cost is: Among them, l min , l max is the maximum and minimum lateral distance between obstacle boundary points, s min 、s max is the maximum and minimum longitudinal distance between the obstacle boundary points, and sigmod is the activation function that can map real numbers to the interval (0,1). In dynamic programming, the speed planning cost function is defined as: cost=cost_speed+cost_acc+cost_jerk+cost_obs (1) Obstacle collision cost: Among them, s min is the minimum longitudinal distance of the vehicles at time t, s max is the corresponding maximum longitudinal distance, s is the longitudinal distance of the sampling point, buffer is the margin, and w obs is the obstacle cost coefficient. (2) Definition of speed cost Among them, w speed is the speed cost coefficient, v is the actual vehicle speed, v ref The reference speed. (3) Definition of acceleration cost Among them, s(t+1,s+k) is the state quantity at the next moment, s(t,s+k) is the state quantity at the current moment, and w acc is the speed cost coefficient, and a is the actual acceleration. (4) Jerk cost Jerk is characterized by an impact amount, which is mainly used to reduce the discomfort caused by sudden acceleration of the vehicle and ensure driving comfort. In quadratic programming, the trajectory planning cost function is defined as: Among them, l ref The lateral displacement obtained for the dynamic programming. In quadratic programming, the speed planning cost function is defined as: where s ref(i) Get the longitudinal displacement of the path for dynamic programming, Get the longitudinal velocity of the path for dynamic planning, is the longitudinal acceleration, is the longitudinal acceleration, w s is the reference longitudinal distance cost coefficient, w vel is the reference longitudinal velocity. Cost coefficient, w acc is the reference longitudinal acceleration cost coefficient, w jerk is the reference longitudinal jerk penalty coefficient.

6. The integrated control method for local path planning and trajectory tracking of an autonomous driving vehicle according to claim 1, characterized in that: The step of splicing the trajectories and sending them to the trajectory tracker includes the following steps: In the planning algorithm, if the trajectory is replanned for each frame, it will cause additional time overhead, and the discontinuous trajectory sequence will easily cause the vehicle to jitter. The purpose of trajectory splicing is to enable the trajectory tracking control algorithm to better track the trajectory and ensure the continuity of the tracking trajectory. The trajectory of the current frame and the planned frame is concatenated based on the polynomial and sent to the trajectory tracker. Specifically: <h2 style=";text-align:left;direction:ltr">l(s) = a0 + a1 s + a2 s<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> +a3·s<h2 style=";text-align:left;direction:ltr"> 3 <h2 style=";text-align:left;direction:ltr"> +a4·s<h2 style=";text-align:left;direction:ltr"> 4 <h2 style=";text-align:left;direction:ltr"> +a5·s<h2 style=";text-align:left;direction:ltr"> 5 <h2 style=";text-align:left;direction:ltr"> dl(s) = a1+2a2·s+3a3·s<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> +4a4·s<h2 style=";text-align:left;direction:ltr"> 3 <h2 style=";text-align:left;direction:ltr"> +5a5·s<h2 style=";text-align:left;direction:ltr"> 4 ddl(s)=2a2+6a3·s+12a4·s 2 +20a5·s 3 dddl(s)=6a3·s+24a4·s+60a5·s 2 7. The integrated control method for local path planning and trajectory tracking of an autonomous driving vehicle according to claim 1, characterized in that: The design of the LQR trajectory tracking controller and the PID speed tracker based on the vehicle two-degree-of-freedom error model includes the following steps: Firstly, a two-degree-of-freedom dynamic model of a single track vehicle is established. Secondly, the state equation of the vehicle lateral dynamic tracking error model is constructed, and the vehicle tracking error is calculated. Then, a PID speed tracking controller is designed based on the vehicle tracking error and speed error to achieve vehicle speed tracking. Finally, the optimal feedback control quantity is calculated using a linear quadratic solver to control the vehicle to travel along the desired trajectory planning path. The tracking error state space equation of the LQR vehicle lateral dynamics model is: in, is the lateral error, lateral error change rate, yaw angle error, and yaw angle error change rate of the vehicle, l f is the distance from the front axle to the center of mass, l r is the distance from the rear axle to the center of mass, C αf is the front tire cornering stiffness, C αr is the cornering stiffness of the rear tire, δ is the front wheel steering angle, v x is the vehicle longitudinal velocity, I z is the moment of inertia of the vehicle around the z axis, m is the total mass of the vehicle, is the desired yaw rate of the vehicle, is the expected yaw angular acceleration. Feedforward Controller Design: Among them, κ is the curvature and k3 is the gain coefficient. Longitudinal PID speed tracker design: Among them, e s is the longitudinal error, e v is the longitudinal velocity error, k p ,k i ,k d ,K p ,K i ,K d is the PID control gain coefficient.

Images