Automatic driving automobile track planning method based on improved model predictive control

Abstract

An improved model predictive control-based automatic driving vehicle trajectory planning method is characterized by comprising a point quality model, a Sigmoid function, a finite state machine and a simplified dual neural network. The point mass model ignores the automobile size information, only considers the automobile position and speed and describes the automobile motion by a point with mass, so that the calculated amount of trajectory planning is effectively reduced; the shape of the Sigmoid function can be changed by parameters to form a safety barrier suitable for vehicle overtaking, and the barrier can serve as a constraint to be merged into MPC for optimization; the finite-state machine adjusts the speed or the driving lane of the vehicle according to the use condition of adjacent lanes and the distance and the speed of the front vehicle, so that the driving safety of the vehicle is ensured, and a generated activation function decides whether the vehicle changes lanes or decelerates; the simplified dual neural network optimizes a target function in the MPC rolling optimization process, reduces the calculation complexity, does not involve matrix decomposition and inversion operation in the solving process, occupies less resources compared with a traditional method, feeds the solved transverse and longitudinal acceleration back to a vehicle control system, and improves the accuracy of the vehicle control system. Therefore, the vehicle executes lane changing or deceleration behaviors, and the driving safety of the vehicle under the same-direction double lanes is ensured.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving vehicle trajectory planning, and mainly to an autonomous driving vehicle trajectory planning method based on improved model predictive control. Background Art

[0002] In recent years, with the rapid development of vehicle technology and artificial intelligence, various autonomous driving technologies have been applied to actual vehicles, improving the safety of vehicle driving, the comfort of passengers and reducing the driving workload of drivers. At present, vehicle trajectory planning is a hot topic in the research of autonomous vehicles, but trajectory planning still has problems with real-time and safety. The trajectory planning of the vehicle needs to be updated and adjusted in real time according to the driving environment, so it is necessary to further optimize the rapidity of the algorithm used. The safety of the vehicle during driving cannot be ignored, and generating a reasonable and safe driving trajectory is the prerequisite for ensuring the safety of trajectory planning.

[0003] When performing obstacle avoidance and overtaking tasks, autonomous vehicles can plan a path based on the road conditions and surrounding vehicle information collected by sensors, and track the planned path according to changes in the surrounding environment. The current trajectory planning idea commonly used in academia is to pre-set the initial position and destination, and generate a global trajectory plan based on the target position and the current environment system. Common algorithms include graph search algorithms (A* algorithm, Dijkstra algorithm) and sampling methods (fast search random tree). On the basis of global trajectory planning, the vehicle plans a local trajectory according to the driving environment around the vehicle (surrounding vehicles and road obstacles) during the process of tracking the trajectory. Common local trajectory planning methods include dynamic window method and artificial potential field method. However, traditional trajectory planning requires a lot of calculations when solving the optimal trajectory, especially in complex environments and when there are too many obstacles. It has poor adaptability to dynamic environments. When the driving environment changes, traditional methods may not be able to adjust the trajectory in real time. Even if the optimal trajectory is found, it is easy to fall into a local optimal solution instead of a global optimal solution. Model predictive control (MPC) can optimize the control strategy in each control cycle to adapt to the changes in the dynamic environment, and can handle the dynamic constraints of the system state and control input (the present invention uses the Sigmoid function as the dynamic constraint of vehicle driving), making the algorithm more flexible and robust in complex environments. Compared with global and local planning methods, model predictive control can consider the control strategy of the next time domain at each moment, and can take into account both global and local optimality. The Sigmoid function is an S-shaped curve function, which is usually used as an activation function in a neural network. In trajectory planning, the Sigmoid function can use the vehicle as a constraint barrier to regulate the driving range of the autonomous driving vehicle, and can be used as a dynamic constraint to optimize the overtaking trajectory. The simplified dual neural network (SDNN) is a design that combines duality and simplification techniques to optimize the neural network, which can improve the efficiency and performance of neural network calculations, and is suitable for various application scenarios that need to improve computing efficiency and reduce complexity. The present invention introduces a discrete dual neural network to solve the QP matrix in the model predictive control, which can effectively reduce the MPC solution time and thus improve the reaction speed of the autonomous driving vehicle. Summary of the invention

[0004] In order to solve the problem of fast and safe overtaking of vehicles with obstacles in the same lane and adjacent lanes when an autonomous driving car performs obstacle avoidance trajectory planning in the same direction two lanes, the present invention takes Sigmoid function as constraint, uses SDNN to simplify the QP matrix, and proposes an autonomous driving car trajectory planning based on improved model predictive control.

[0005] The technical solution adopted by the present invention to solve the technical problem is as follows:

[0006] The trajectory planning method of an autonomous driving vehicle based on improved model predictive control includes the following steps:

[0007] Step 1: Ignore the size information of the vehicle and the load transfer caused by lateral and longitudinal acceleration, use a point with mass to describe the vehicle motion, and establish a point mass model of the vehicle.

[0008] Step 2: Use the CarSim car model to obtain the real-time parameters of the vehicle: vehicle position, heading angle, lateral speed and acceleration, longitudinal speed and acceleration, position and speed of other vehicles, etc.

[0009] Step 3: Based on the driving environment information obtained by CarSim, use the Sigmoid function to build a safety barrier for all vehicles in the environment. Take safety and comfort as optimization indicators, optimize the curvature ε of the Sigmoid function, and obtain the upper and lower limits of the curvature.

[0010] Step 4: According to the usage of adjacent lanes and the distance and speed of the vehicle ahead, the finite state machine (FSM) decision module adjusts the speed of the vehicle or changes the lane of the vehicle to ensure the safety of the vehicle. According to different situations, the corresponding activation functions δ and γ are generated and sent to the model predictive control module to execute appropriate driving behavior.

[0011] Step 5. The model prediction control module receives the prediction model constructed by the point mass model, the upper and lower limits of curvature obtained by the Sigmoid function construction and optimization module, the decision parameters of the FSM decision module, and other information, and constructs a suitable objective function J in combination with the constraints. In the solution process, a discrete SDNN is introduced to construct a simplified dual neural network model to quickly solve the QP matrix, obtain the lateral and longitudinal accelerations, and send the lateral and longitudinal accelerations to the Carsim vehicle model.

[0012] The beneficial effects of the present invention are as follows:

[0013] 1. The present invention proposes an improved model predictive control (IMPC) method that comprehensively considers the vehicle state and driving environment and implements appropriate driving behavior, which can solve the lane changing problem under different driving environments.

[0014] 2. The present invention proposes a trajectory planning method, which uses the Sigmoid function as a constraint barrier for trajectory planning to achieve smooth path control and obstacle avoidance overtaking. The Sigmoid function can ensure the smoothness of the trajectory, avoid sharp acceleration or deceleration, and provide a more natural motion trajectory for obstacle avoidance overtaking. The constraint barrier can change the shape of the constraint by adjusting the curvature ε of the Sigmoid function, and the pre-optimization of the curvature can be incorporated into the MPC to reduce the amount of calculation and improve the real-time performance of the optimization algorithm.

[0015] 3. The present invention introduces FSM as a decision-making algorithm. When there is an obstacle vehicle ahead, two driving behaviors can be adopted: lane change and overtaking and braking. The FSM algorithm has the advantages of being easy to build and strong in real time. It can generate different activation signals for different driving environments and generate different constraints to act on the MPC to execute overtaking or braking.

[0016] 4. The present invention adopts SDNN to construct the quadratic programming problem into a simplified dual neural network model, which reduces the computational complexity in the MPC rolling optimization process, improves the MPC solution speed, and ensures the real-time performance of trajectory planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 It is the overall structural block diagram of the present invention.

[0018] Figure 2 The present invention is based on a finite state machine decision flow chart. Specific implementation plan

[0019] The present invention is further described in detail below with reference to the accompanying drawings and specific implementations:

[0020] like Figure 1 As shown, the trajectory planning method of the automatic driving vehicle based on the improved model predictive control of the present invention includes: step 1, ignoring the size information of the vehicle and the load transfer caused by the lateral and longitudinal acceleration, using a point with mass to describe the vehicle movement, and establishing a point mass model of the vehicle. Step 2, using the CarSim car model to obtain the real-time parameters of the vehicle: vehicle position, heading angle, lateral speed and acceleration, longitudinal speed and acceleration, position and speed of other vehicles, etc. Step 3, according to the driving environment information obtained by CarSim, using the Sigmoid function to build a safety barrier for all vehicles in the environment, taking safety and comfort as optimization indicators, optimizing the curvature ε of the Sigmoid function, and obtaining the upper and lower limits of the curvature. Step 4, according to the use of the adjacent lanes and the distance and speed of the vehicle in front, the finite state machine (FSM) decision module adjusts the speed of the vehicle or changes the driving lane of the vehicle to ensure the safety of the vehicle. Generate corresponding activation functions δ and γ according to different situations, and send them to the model predictive control module to execute appropriate driving behaviors. Step 5. The model prediction control module receives the prediction model constructed by the point mass model, the upper and lower limits of curvature obtained by the Sigmoid function construction and optimization module, the decision parameters of the FSM decision module, and other information, and constructs a suitable objective function J in combination with the constraints. In the solution process, a discrete SDNN is introduced to construct a simplified dual neural network model to quickly solve the QP matrix, obtain the lateral and longitudinal accelerations, and send the lateral and longitudinal accelerations to the Carsim vehicle model.

[0021] like Figure 2As shown, in the trajectory planning method of the automatic driving vehicle based on the improved model predictive control of the present invention, the finite state machine decision determines whether the vehicle brakes or changes lanes. The FSM algorithm adjusts the speed of the vehicle or the driving lane of the vehicle to ensure driving safety according to the usage of the adjacent lanes and the distance and speed of the vehicle in front. First, determine whether the distance to the vehicle in front is safe. If there is not enough safe distance, brake. If there is enough safe distance, take the next step of decision based on whether the adjacent lane is free. If there is a car in the adjacent lane, continue to follow the vehicle in front. If the adjacent lane is free, change lanes. If there is enough distance from the vehicle being overtaken in the original lane, change lanes back to the original lane to complete the overtaking. If there is not enough distance, continue to drive and wait for the opportunity to return to the original lane.

[0022] The present invention is based on an improved model predictive control automatic driving vehicle trajectory planning method, and the specific implementation steps are as follows:

[0023] Step 1: Ignore the size information of the vehicle and the load transfer caused by lateral and longitudinal acceleration, use a point with mass to describe the vehicle motion, and establish a point mass model of the vehicle.

[0024] The kinematic equations for the point mass model are as follows:

[0025]

[0026] Where x, y are the vehicle's forward direction and yaw direction in the vehicle coordinate system, respectively, and X, Y are the earth coordinate system. is the vehicle heading angle, a x ,a y is the lateral acceleration and longitudinal acceleration. From the above formula, the system can be regarded as an input u=[a x a y ] T , the state quantity is The general form of the control system can be written as:

[0027] χ=f(χ,u) (2)

[0028] Simply expressed as:

[0029]

[0030] Perform forward Euler discretization to obtain:

[0031]

[0032] in Transform the above formula as follows:

[0033] The new state space expression is as follows:

[0034]

[0035] in n is the state quantity dimension, and m is the control quantity dimension.

[0036] The prediction output expression is:

[0037] Y(k)=φ k ξ(k)+θ k ΔU(k) ​​(6)

[0038] in:

[0039]

[0040] After discretizing the kinematic equations of the point mass model horizontally and vertically:

[0041]

[0042] where v x and v y is the longitudinal and lateral speed of the ego vehicle, x and y are the longitudinal and lateral positions of the ego vehicle on the simulated road, a ex and a ey is the longitudinal and lateral acceleration of the vehicle, T s is the sampling period. The speed and position of the front vehicle and the left vehicle are as follows:

[0043]

[0044] Where Δv xi and Δv yi is the longitudinal and lateral speed difference between the i-th vehicle and the ego vehicle, Δy i and Δx i is the distance between the i-th vehicle and the ego vehicle, a xi and a yi is the acceleration of the i-th vehicle on the longitudinal and transverse axes.

[0045] Step 2: Use the CarSim car model to obtain the real-time parameters of the vehicle: vehicle position, heading angle, lateral speed and acceleration, longitudinal speed and acceleration, position and speed of other vehicles, etc.

[0046] Step 3: Based on the driving environment information obtained by CarSim, use the Sigmoid function to build a safety barrier for all vehicles in the environment. Take safety and comfort as optimization indicators, optimize the curvature ε of the Sigmoid function, and obtain the upper and lower limits of the curvature.

[0047] The Sigmoid function is bounded, continuous, differentiable, and strictly monotonic, and is widely used in function-based trajectory planning algorithms. This paper uses the Sigmoid function as an overtaking barrier for trajectory planning. The Sigmoid function is expressed as follows:

[0048]

[0049] where y lat is the lateral offset of the vehicle, s / 2 is the distance to the symmetrical path point (s / 2, y lat / 2) The longitudinal distance required (the symmetrical path points refer to the points in symmetrical positions selected on the path in path planning in order to improve the smoothness and stability of the path). The formal parameter ζ determines the curvature of the function. The larger its value, the more drastic the turn.

[0050] The curvature form parameters are optimized as follows:

[0051] The curvature calculation formula is:

[0052]

[0053] in:

[0054]

[0055] When dx changes slightly, we can approximate that ds(x)≈d(x) and

[0056] In summary, the curvature calculation formula can be rewritten as:

[0057]

[0058] After calculation, the heading angle and curvature are:

[0059]

[0060] From the above formula, we can see that the curvature is related to the curvature parameter ζ. The shape of the Sigmoid constraint barrier can be changed by changing the curvature parameter. There are many ways to change lanes with different curvatures, but the physical conditions and road driving safety constraints must be met during vehicle driving. Therefore, it is necessary to constrain the lane change curvature and incorporate it into the MPC as a set of constraints. The curvature ρ is constrained to be at the minimum curvature ρ. min With the maximum curvature ρ max between.

[0061] The maximum curvature is obtained at the inflection point of the Sigmoid function, that is, dρ(x) / d(x)=0. Substituting the x value at the inflection point into ρ(x) gives the maximum curvature ρ. max , the curvature parameter ζ is also the maximum value at this time:

[0062]

[0063] where γ = e 1.3170 To calculate ζ max The maximum curvature of the safe trajectory needs to meet the vehicle dynamics constraints. When turning, the maximum lateral acceleration cannot be exceeded and the vehicle dynamics constraints are met. The lateral acceleration is affected by the friction factor between the car and the road and the front wheel angle. The upper limit of the lateral acceleration is given by Decision, among which Determining the yaw rate The upper limit of V x is the longitudinal velocity of the vehicle, μ is the friction coefficient between the vehicle and the ground, and g is the acceleration of gravity. To ensure the stability of the turn, the maximum curvature of the lane change trajectory is linked to the maximum value of the yaw rate:

[0064]

[0065] With the maximum curvature ρ max The difference is that the minimum curvature ρ min The geometric constraints are considered, that is, vehicles cannot collide when overtaking.

[0066]

[0067] where y c is to the symmetrical target point (s,y lat ) is the tolerable error. The optimization of ζ can be incorporated into MPC, and [ζ min ,ζ max ] are regarded as constraints to form a standardized overtaking trajectory.

[0068] Step 4: According to the usage of adjacent lanes and the distance and speed of the vehicle ahead, the finite state machine (FSM) decision module adjusts the speed of the vehicle or changes the lane of the vehicle to ensure the safety of the vehicle. According to different situations, the corresponding activation functions δ and γ are generated and sent to the model predictive control module to execute appropriate driving behavior.

[0069] After obtaining surrounding information, the ego vehicle will brake or overtake, which is determined by the FSM algorithm. The FSM algorithm adjusts the ego vehicle's speed or lane to ensure driving safety based on the usage of adjacent lanes and the distance and speed of the vehicle in front. The corresponding activation function is generated according to different situations and sent to the MPC to execute appropriate driving behavior.

[0070] The FSM parameters are obtained as follows:

[0071]

[0072] where δ k is the lane change activation function, when δ k When it is 1, the vehicle changes lanes, γ k is the braking activation function, when γ k When it is 1, the vehicle brakes. f For the safe distance of the workshop, is the lateral and longitudinal distance between the ego vehicle and other vehicles. The activation function is used in the subsequent Sigmoid constraint barrier, input into the MPC to generate the trajectory, and the output is the lateral and longitudinal acceleration of the ego vehicle.

[0073] Step 4: The model prediction control module receives the prediction model constructed by the point quality model, the upper and lower limits of curvature obtained by the Sigmoid function construction and optimization module, the decision parameters of the FSM decision module, and other information, and constructs a suitable objective function J in combination with the constraints. In the solution process, a discrete SDNN is introduced to construct a simplified dual neural network model to quickly solve the QP matrix, obtain the lateral and longitudinal accelerations, and send the lateral and longitudinal accelerations to the Carsim vehicle model.

[0074] (1) Establishment of objective function

[0075] Driving on the center line of the lane at an appropriate speed can be written as a cost function:

[0076]

[0077] in and Input is represents the lateral and longitudinal acceleration changes of the vehicle, J2 is the cost function for optimizing the relaxation variables, N c To control the time domain, N p is the prediction time domain, n v =1 / 2 is the number of surrounding vehicles, χ is the weight variable, is the relaxation factor.

[0078] In order to meet the driving comfort and physical conditions, the hard constraints are:

[0079]

[0080] There is a certain coupling relationship between the lateral and longitudinal speeds of the vehicle, and the slip angle β constrains the relationship between the two, which can be expressed as:

[0081]

[0082] According to the small angle approximation theorem, it can be regarded as β = v y / v x To ensure driving safety, the vehicle slip angle must meet the following constraints:

[0083] β<β max (twenty one)

[0084] In summary, the longitudinal velocity v y Can be constrained to:

[0085]

[0086] The road driving constraints are:

[0087]

[0088] Where L is the width of the car, y min and max The upper and lower boundaries of the road.

[0089] Sigmoid constraint barrier: When the ego vehicle approaches a dangerous area, it can change lanes to a collision-free area. This barrier is incorporated into the MPC as a constraint and activated by the activation signal provided by the decision module.

[0090]

[0091] where y lat is the lateral distance that the ego vehicle should keep from the front vehicle, including the width of the vehicle, ζ is the pre-optimized curvature geometry parameter, S f is the longitudinal safety distance of the workshop, S f It can be defined as:

[0092]

[0093] where v x is the vehicle speed, a emax is the vehicle acceleration, v xi is the speed of the obstacle vehicle. imax is the acceleration of the obstacle vehicle, t is the reaction time of surrounding information transmission, and d0 is the minimum distance between vehicles. It should be noted that The positive or negative value of depends on the lane the vehicle is in. If the vehicle is in the right lane, changing lanes to the left is positive, and if the vehicle is in the left lane, changing lanes to the right is negative. After the vehicle reaches the overtaking lane, it can return to the original lane after reaching a safe distance from the vehicle behind, completing the lane change.

[0094] (2) Rapid solution of model predictive control

[0095] In the rolling optimization process of MPC, the model prediction output in analytical expression can be solved by quadratic programming to obtain the optimal control sequence. Let E(k) = φξ and Equation (18) can be rewritten as:

[0096]

[0097] Where Q is the output weight matrix, R is the control increment weight matrix, ρ is the weight factor weight, E = Y(k)-ψ(k), Np is the model prediction time domain, θ = Y-Yref,

[0098] To find the optimal solution for the above formula, we can get:

[0099]

[0100] Written in quadratic programming form:

[0101]

[0102] in, f T =[2E T Qθ0],K∈R p*n ; l,h∈R p , if l i = -∞ or h i =∞ The corresponding inequality constraint is one-sided; if l i = -∞ and h i =∞, the corresponding inequality constraint does not exist.

[0103] Based on the duality principle, the dual problem of the above equation can be written as:

[0104]

[0105] where λ i ,λ h is the Lagrange multiplier, let v = λ i -λ h , substituting into the above formula, we can get:

[0106]

[0107] For the quadratic programming problem, the Karush-Kuhn-Tucker Conditions (KKT) are:

[0108]

[0109] It can be shortened to:

[0110]

[0111] where g(z) = [g(z1)g(z2)…g(z p )] T The piecewise linear vector function is defined as follows:

[0112]

[0113] According to formula (31), we can get:

[0114]

[0115] Where O=H -1 E T ,p=H -1 f.

[0116] In summary, the following simplified dual neural network model (SSDN) can be constructed:

[0117] Equation of state:

[0118]

[0119] Output equation:

[0120]

[0121] Among them, v is the state variable; κ>0 is the scaling factor for adjusting the convergence speed of the simplified dual neural network model.

[0122] make Substituting into the state equation we get:

[0123]

[0124] Where z is a neuron, g(z) is a nonlinear activation function, and KO-I is a symmetric weight matrix. At this point, SSDN can be viewed as a periodic neural network model.

[0125] The above is a continuous SDNN model. The discrete SDNN model using the Runge-Kutta method is as follows:

[0126] Equation of state:

[0127]

[0128] The output equation is:

[0129] U QR =Ov i -p (36)

[0130] Among them, T p represents the step size, and the other symbols are as follows:

[0131]

[0132] This discrete SDNN can be applied to the rolling optimization of MPC. For the quadratic programming of J, the corresponding relationship between the discrete SDNN parameters and the quadratic programming parameters is:

[0133]

[0134] SDNN solves quadratic programming, and the number of neurons is equal to the number of bilateral constraints of the quadratic programming. Online calculation does not involve matrix decomposition and inversion, which is faster than traditional solution methods.

Claims

1. A trajectory planning method for an autonomous driving vehicle based on improved model predictive control, characterized in that: The method comprises the following steps: Step 1: Ignore the size information of the vehicle and the load transfer caused by lateral and longitudinal acceleration, use a point with mass to describe the vehicle motion, and establish a point mass model of the vehicle. Step 2: Use the CarSim car model to obtain the real-time parameters of the vehicle: vehicle position, heading angle, lateral speed and acceleration, longitudinal speed and acceleration, position and speed of other vehicles, etc. Step 3: Based on the driving environment information obtained by CarSim, use the Sigmoid function to build a safety barrier for all vehicles in the environment. Take safety and comfort as optimization indicators, optimize the curvature ε of the Sigmoid function, and obtain the upper and lower limits of the curvature. Step 4: According to the usage of adjacent lanes and the distance and speed of the vehicle ahead, the finite state machine (FSM) decision module adjusts the speed of the vehicle or changes the lane of the vehicle to ensure the safety of the vehicle. According to different situations, the corresponding activation functions δ and γ are generated and sent to the model predictive control module to execute appropriate driving behavior. Step 5. The model prediction control module receives the prediction model constructed by the point mass model, the upper and lower limits of curvature obtained by the Sigmoid function construction and optimization module, the decision parameters of the FSM decision module, and other information, and constructs a suitable objective function J in combination with the constraints. In the solution process, a discrete SDNN is introduced to construct a simplified dual neural network model to quickly solve the QP matrix, obtain the lateral and longitudinal accelerations, and send the lateral and longitudinal accelerations to the Carsim vehicle model.

2. The automatic driving vehicle trajectory planning method based on improved model predictive control according to claim 1, characterized in that: In step 1, the size information of the vehicle and the load transfer caused by the lateral and longitudinal accelerations are ignored, and a point with mass is used to describe the vehicle motion to establish the vehicle point mass model. The process is as follows: (1) The kinematic equation of the point mass model is as follows: Where x, y are the vehicle's forward direction and yaw direction in the vehicle coordinate system, respectively, and X, Y are the earth coordinate system. is the vehicle heading angle. From the above formula, the system can be regarded as an input u=[a x a y ] T , the state quantity is The general form of the control system can be written as: χ=f(χ,u) (2) Simply expressed as: Perform forward Euler discretization to obtain: in Change the above formula as follows: The new state space expression is as follows: in n is the state quantity dimension, and m is the control quantity dimension. The prediction output expression is: Y(k)=φ k ξ(k)+θ k ΔU(k) ​​(6) in After discretizing the kinematic equations of the point mass model horizontally and vertically: where v x and v y is the longitudinal and lateral speed of the ego vehicle, x and y are the longitudinal and lateral positions of the ego vehicle on the simulated road, and a ex and a ey is the longitudinal and lateral acceleration of the vehicle, T s is the sampling period. The speed and position of the front vehicle and the left vehicle are as follows: where Δv xi and Δv yi is the longitudinal and lateral speed difference between the i-th vehicle and the ego vehicle, Δy i and Δx i is the distance between the i-th vehicle and the ego vehicle, a xi and a yi is the acceleration of the i-th vehicle on the longitudinal and transverse axes.

3. The automatic driving vehicle trajectory planning method based on improved model predictive control according to claim 1, characterized in that: Step 3: Based on the driving environment information obtained by CarSim, use the Sigmoid function to build a safety barrier for all vehicles in the environment. Take safety and comfort as optimization indicators, optimize the curvature ε of the Sigmoid function, and obtain the upper and lower limits of the curvature. The specific pre-optimization process is as follows: The Sigmoid function is bounded, continuous, differentiable, and strictly monotonic, and is widely used in function-based trajectory planning algorithms. This paper uses the Sigmoid function as an overtaking barrier for trajectory planning. The Sigmoid function is expressed as follows: where y lat is the lateral offset of the vehicle, s / 2 is the distance to the symmetrical path point (s / 2, y lat / 2) The longitudinal distance required (the symmetrical path points refer to the points in symmetrical positions selected on the path in path planning in order to improve the smoothness and stability of the path). The formal parameter ζ determines the curvature of the function. The larger its value, the more drastic the turn. The curvature form parameters are optimized as follows: The curvature calculation formula is: in: When dx changes slightly, we can approximate that ds(x)≈d(x) and In summary, the curvature calculation formula can be rewritten as: After calculation, the heading angle and curvature are: From the above formula, we can see that the curvature is related to the curvature parameter ζ. The shape of the Sigmoid constraint barrier can be changed by changing the curvature parameter. There are many ways to change lanes with different curvatures, but the physical conditions and road driving safety constraints must be met during vehicle driving. Therefore, it is necessary to constrain the lane change curvature and incorporate it into the MPC as a set of constraints. The curvature ρ is constrained to be at the minimum curvature ρ. min With the maximum curvature ρ max between. The maximum curvature is obtained at the inflection point of the Sigmoid function, that is, dρ(x) / d(x)=0. Substituting the x value at the inflection point into ρ(x) gives the maximum curvature ρ. max , the curvature parameter ζ is also the maximum value at this time: where γ = e 1.3170 To calculate ζ max The maximum curvature of the safe trajectory needs to meet the vehicle dynamics constraints. When turning, the maximum lateral acceleration cannot be exceeded and the vehicle dynamics constraints are met. The lateral acceleration is affected by the friction factor between the car and the road and the front wheel angle. The upper limit of the lateral acceleration is given by Decision, among which Determining the yaw rate The upper limit of V x is the longitudinal velocity of the vehicle, μ is the friction coefficient between the vehicle and the ground, and g is the acceleration of gravity. To ensure the stability of the turn, the maximum curvature of the lane change trajectory is linked to the maximum value of the yaw rate: With the maximum curvature ρ max The difference is that the minimum curvature ρ min The geometric constraints are considered, that is, vehicles cannot collide when overtaking. where y c is to the symmetrical target point (s,y lat ) is the tolerable error. The optimization of ζ can be incorporated into MPC, and [ζ min ,ζ max ] are regarded as constraints to form a standardized overtaking trajectory.

4. The automatic driving vehicle trajectory planning method based on improved model predictive control according to claim 1, characterized in that: Step 4: According to the usage of adjacent lanes and the distance and speed of the vehicle ahead, the finite state machine (FSM) decision module adjusts the speed of the vehicle or changes the lane of the vehicle to ensure the safety of the vehicle. According to different situations, the corresponding activation functions δ and γ are generated and sent to the model predictive control module to execute appropriate driving behavior. The specific FSM decision process is as follows: After obtaining surrounding information, the ego vehicle will brake or overtake, which is determined by the FSM algorithm. The FSM algorithm adjusts the ego vehicle's speed or lane to ensure driving safety based on the usage of adjacent lanes and the distance and speed of the vehicle in front. The corresponding activation function is generated according to different situations and sent to the MPC to execute appropriate driving behavior. The FSM parameters are obtained as follows: where δ k is the lane change activation function, when δ k When it is 1, the vehicle changes lanes, γ k is the braking activation function, when γ k When it is 1, the vehicle brakes. f For the safe distance of the workshop, is the lateral and longitudinal distance between the ego vehicle and other vehicles. The activation function is used in the subsequent Sigmoid constraint barrier, input into the MPC to generate the trajectory, and the output is the lateral and longitudinal acceleration of the ego vehicle.

5. The automatic driving vehicle trajectory planning method based on improved model predictive control according to claim 1, characterized in that: As described in step 5, the model prediction control module receives the prediction model constructed by the point quality model, the upper and lower limits of curvature obtained by the Sigmoid function construction and optimization module, the decision parameters of the FSM decision module, and other information, and constructs a suitable objective function J in combination with the constraints. In the solution process, a discrete SDNN is introduced, and a simplified dual neural network model is constructed to quickly solve the QP matrix to obtain the lateral and longitudinal accelerations, which are then sent to the Carsim vehicle model. (1) Establishment of objective function Driving on the center line of the lane at an appropriate speed can be written as a cost function: in and Input is represents the lateral and longitudinal acceleration changes of the vehicle, J2 is the cost function for optimizing the relaxation variables, N c To control the time domain, N p is the prediction time domain, n v =1 / 2 is the number of surrounding vehicles, χ is the weight variable, and θ is the relaxation factor. In order to meet the driving comfort and physical conditions, the hard constraints are: There is a certain coupling relationship between the lateral and longitudinal speeds of the vehicle, and the slip angle β constrains the relationship between the two, which can be expressed as: According to the small angle approximation theorem, it can be regarded as β = v y / v x To ensure driving safety, the vehicle slip angle must meet the following constraints: |β|＜β max (21) In summary, the longitudinal velocity v y Can be constrained to: The road driving constraints are: Where L is the width of the car, y min and max The upper and lower boundaries of the road. Sigmoid constraint barrier: When the ego vehicle approaches a dangerous area, it can change lanes to a collision-free area. This barrier is incorporated into the MPC as a constraint and activated by the activation signal provided by the decision module. where y lat is the lateral distance that the ego vehicle should keep from the front vehicle, including the width of the vehicle, ζ is the pre-optimized curvature geometry parameter, S f is the longitudinal safety distance of the workshop, S f It can be defined as: where v x is the vehicle speed, a emax is the vehicle acceleration, v xi is the speed of the obstacle vehicle. imax is the acceleration of the obstacle vehicle, t is the reaction time of surrounding information transmission, and d0 is the minimum distance between vehicles. It should be noted that The positive or negative value of depends on the lane the vehicle is in. If the vehicle is in the right lane, changing lanes to the left is positive, and if the vehicle is in the left lane, changing lanes to the right is negative. After the vehicle reaches the overtaking lane, it can return to the original lane after reaching a safe distance from the vehicle behind, completing the lane change. (2) Rapid solution of model predictive control In the rolling optimization process of MPC, the model prediction output in analytical expression can be solved by quadratic programming to obtain the optimal control sequence. Let E(k) = φξ and Equation (19) can be rewritten as: Where Q is the output weight matrix, R is the control increment weight matrix, ρ is the weight factor weight, E = Y(k)-ψ(k), Np is the model prediction time domain, θ = Y-Yref, To find the optimal solution for the above formula, we can get: Written in quadratic programming form: in, f T =[2E T Qθ0],K∈R p*n ; l,h∈R p , if l i = -∞ or h i =∞ The corresponding inequality constraint is one-sided; if l i = -∞ and h i =∞, the corresponding inequality constraint does not exist. Based on the duality principle, the dual problem of the above equation is: where λ i ,λ h is the Lagrange multiplier, let v = λ i -λ h , substituting into the above constraint, we can get: For the quadratic programming problem, the Karush-Kuhn-Tucker Conditions (KKT) are: It can be written as: where g(z) = [g(z1) g(z2) … g(z p )] T The piecewise linear vector function is defined as follows: According to (31), we can get: Where O=H -1 E T ,p=H -1 f. In summary, the following simplified dual neural network model (SSDN) can be constructed: Equation of state: Output equation: in, v is a state variable; κ>0 is a scaling factor for adjusting the convergence speed of the simplified dual neural network model. make Substituting into the state equation we get: Where z is a neuron, g(z) is a nonlinear activation function, and KO-I is a symmetric weight matrix. At this point, SSDN can be viewed as a periodic neural network model. The above is a continuous SDNN model. The discrete SDNN model using the Runge-Kutta method is as follows: Equation of state: The output equation is: OR QR =Ov i -p (36) Among them, T p represents the step size, and the other symbols are as follows: This discrete SDNN can be applied to the rolling optimization of MPC. For the quadratic programming of J, the corresponding relationship between the discrete SDNN parameters and the quadratic programming parameters is: SDNN solves quadratic programming, and the number of neurons is equal to the number of bilateral constraints of the quadratic programming. Online calculation does not involve matrix decomposition and inversion, which is faster than traditional solution methods.

Images