Driving track planning method for predictive control by using nonlinear model

Abstract

The invention discloses a driving track planning method for predictive control by using a nonlinear model, and the method comprises the steps: designing and solving a nonlinear model predictive objective function, and obtaining an initial solution of nonlinear model predictive control; a plurality of candidate solutions are obtained according to the preliminary solution of nonlinear model prediction control, sampling points of all the candidate solutions are connected through a Bezier curve, and preliminary trajectories of all the candidate solutions are obtained; and constructing an initial population of the genetic algorithm based on the initial solution of the nonlinear model predictive control, evaluating the fitness of each candidate solution in the initial population, selecting the candidate solution with the highest fitness as an optimal solution, and outputting a corresponding control sequence and trajectory. According to the method, nonlinear model predictive control is utilized, good balance between vehicle driving scene parameters such as obstacle avoidance performance and path length is found, and the feasibility and comfort of a track are improved; and meanwhile, the trajectory is obtained by utilizing Bezier curve connection, so that the smoothness of the trajectory is improved, and rapid turning and discontinuity of the path are reduced.

Description

Technical Field

[0001] The present invention relates to the field of vehicle automatic driving, and in particular to a driving trajectory planning method using a nonlinear model for predictive control. Background Art

[0002] With the rapid development of autonomous driving technology, autonomous vehicles have gradually become an important part of the future intelligent transportation system. The autonomous driving system aims to enable vehicles to drive autonomously in various complex environments through advanced sensors, control algorithms and artificial intelligence technologies, reduce human operations, and improve traffic safety and transportation efficiency. However, there are still many challenges in achieving efficient, safe and smooth autonomous driving, especially in the key link of trajectory planning. Trajectory planning is one of the core modules of the autonomous driving system. Its main task is to generate a feasible driving path based on the current state of the vehicle, the surrounding environment and the target location. This path not only needs to consider the dynamic constraints of the vehicle, but also needs to avoid obstacles and collisions, and have a certain degree of smoothness and comfort.

[0003] There are many types of trajectory planning algorithms. Although these algorithms have their own advantages, there are still some problems in practical applications:

[0004] 1. In the prior art, a driving risk field optimization algorithm based on Gaussian distribution is used to establish the driving risk field of obstacles by introducing the two-dimensional Gaussian distribution functions of static and dynamic obstacles. The risk field is used to quantify the driving risk of each point on the road and generate the optimal trajectory by optimizing the target points in the sampling area. However, the modeling of obstacles is too idealistic and it is easy to ignore other important factors in complex scenes. In addition, the generated trajectory has poor smoothness and insufficient real-time performance.

[0005] 2. In the prior art, the artificial potential field method is used for trajectory planning. By placing the vehicle in a virtual potential field composed of a target point and obstacles, the vehicle automatically moves toward the target point under the action of the potential field while avoiding obstacles. The target point generates an attractive force on the vehicle, while the obstacle generates a repulsive force on the vehicle, and finally the vehicle moves along the direction of the combined force of the potential field. However, the trajectory generated by this method is not smooth enough and has a high computational complexity, especially when dealing with dynamic obstacles. Summary of the invention

[0006] In order to improve the comfort and smoothness of the trajectory, the present invention provides a driving trajectory planning method using a nonlinear model for predictive control.

[0007] To this end, the technical solution adopted in the present invention is:

[0008] A driving trajectory planning method using a nonlinear model for predictive control, the method comprising:

[0009] Obtain the cost of each driving stage of the vehicle, design and solve the nonlinear model prediction objective function, and obtain the preliminary solution of the nonlinear model predictive control;

[0010] Introduce different disturbances and randomness into the preliminary solution predicted by the nonlinear model to obtain multiple candidate solutions, and use Bezier curves to connect the sampling points of each candidate solution to obtain the preliminary trajectory of each candidate solution;

[0011] Based on the preliminary solution of nonlinear model predictive control, the initial population of the genetic algorithm is constructed, the fitness of each candidate solution in the initial population is evaluated, the candidate solution with the highest fitness is selected as the optimal solution, the control sequence corresponding to the optimal solution is output, and the preliminary trajectory corresponding to the optimal solution is output as the vehicle driving trajectory.

[0012] According to the above scheme, the nonlinear model prediction objective function is specifically the sum of the costs of each driving stage of the vehicle; the costs include: position error, heading angle error, control input norm and artificial potential field potential energy.

[0013] According to the above scheme, nonlinear model predictive control constrains the vehicle's control input, speed, acceleration, and obstacle avoidance through the vehicle dynamics model.

[0014] According to the above scheme, before connecting the sampling points using the Bezier curve, the risk field model is used to evaluate the risk level of each sampling point, and sampling points in areas with lower risk values ​​are selected for connection.

[0015] According to the above solution, the risk field model is specifically a two-dimensional Gaussian distribution function determined by the risk intensity coefficient, the obstacle position, and the risk field influence range.

[0016] According to the above scheme, the obstacle position includes a static obstacle position and a dynamic obstacle position; the future position of the dynamic obstacle position is predicted by a constant speed model.

[0017] According to the above scheme, when the sampling points are connected by the Bezier curve, the curvature change of the Bezier curve is minimized by optimizing the coordinates of the control points of the Bezier curve; the curvature is the second-order derivative of the Bezier curve.

[0018] According to the above scheme, the fitness of each candidate solution in the initial population is evaluated, and the candidate solution with the highest fitness is selected by the following method:

[0019] The fitness of each candidate solution is scored based on the smoothness of its trajectory, obstacle avoidance performance, and path length;

[0020] Based on the probability selection of fitness value, the candidate solutions with higher fitness are retained for the next genetic operation;

[0021] The fitness of the candidate solutions of the new generation population obtained by genetic operation is re-scored, selected and iterated until the stopping condition is reached, and the candidate solution with the highest fitness in the final generation is selected.

[0022] According to the above solution, the stopping condition is specifically: reaching a preset number of iterations, or the fitness change range of the candidate solutions in the population is small.

[0023] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the driving trajectory planning method using a nonlinear model for predictive control as described above is implemented.

[0024] The beneficial effects produced by the present invention are:

[0025] 1. The present invention uses nonlinear model predictive control to solve the preliminary solution of the objective function, and evaluates the fitness of each group of candidate solutions generated by the preliminary solution to select the optimal solution, finds a good balance between vehicle driving scene parameters such as obstacle avoidance performance and path length, and improves the feasibility and comfort of the generated trajectory.

[0026] 2. Before connecting the trajectories, the present invention uses the risk field model to evaluate and predict the risk level of the sampling points, thereby preventing vehicles from entering high-risk areas and reducing the probability of vehicle damage and personal injury.

[0027] 3. The present invention uses Bezier curves to connect and obtain the trajectory, and minimizes the curvature by optimizing the coordinates of the curve control points, thereby improving the smoothness of the trajectory and reducing the possibility of sharp turns and discontinuity of the path. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 It is a schematic flow chart of the method of the present invention;

[0029] Figure 2 is a relationship diagram of nonlinear model predictive control, objective function and constraint model in one embodiment of the present invention;

[0030] Figure 3 is a schematic diagram of the S3 sub-step flow chart of an embodiment of the present invention;

[0031] Figure 4 It is a structural diagram of a computer device provided by another embodiment of the present invention. DETAILED DESCRIPTION

[0032] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0033] Embodiment 1

[0034] The present invention provides a driving trajectory planning method using a nonlinear model for predictive control, such as Figure 1 As shown, the method specifically includes:

[0035] S1. Obtain the cost of each driving stage of the vehicle, design and solve the nonlinear model prediction objective function, and obtain the preliminary solution of the nonlinear model predictive control.

[0036] The sum of the predicted costs of each driving stage of the vehicle is taken as the stage cost J, which includes position error, heading angle error, norm of control input and potential energy of artificial potential field. J is used as the nonlinear model prediction objective function, and the objective function is solved online to obtain the optimal control input sequence, so that the vehicle can drive along a smooth trajectory in the best way while avoiding obstacles.

[0037] In a preferred embodiment of the present invention, the stage cost J can be expressed as:

[0038]

[0039] Among them, w 1 、w 2 、w 3 、w 4 is the weight coefficient, which is used to adjust the importance of each cost; pos，k is the position error, i.e., the distance between the current position of the vehicle and the reference trajectory point; e θ，k is the error of the heading angle; ||u k || 2 is the norm of the control input, which is used to punish drastic changes in the control input and ensure smoothness; U APF，k is the potential energy of the artificial potential field; N p is the phase step length of each driving phase.

[0040] Among them, the position error e pos，k :

[0041]

[0042] X k and Y k Indicates the current position coordinates of the vehicle; X ref，k and Y ref，k Represents the position coordinates of the reference trajectory point.

[0043] Heading angle error e θ，k :

[0044] e θ，k =θ k -θref，k

[0045] θ k Indicates the current heading angle of the vehicle; θ ref，k Indicates the reference heading angle.

[0046] Artificial potential field potential U APF，k Including the attractive potential field U att and the repulsive potential field U rep ,in:

[0047]

[0048] Among them, k att is the attraction coefficient, d goal The distance between the current vehicle position and the target point,

[0049]

[0050] Among them, k rep is the repulsive force coefficient, d obs The distance between the current vehicle position and the nearest obstacle position, d 0 is the impact range threshold. When the distance is greater than d 0 , the repulsive force is zero.

[0051] Among them, the obstacle position is divided into a static obstacle position and a dynamic obstacle position. In this embodiment, the position information thereof can be respectively expressed as:

[0052] Mark the static and dynamic obstacle positions as 0 respectively s and 0 d ;

[0053] Static obstacles:

[0054] O s ={(X s，i , Y s，i )}

[0055] Among them, X s，i , Y s，i Respectively represent the coordinate points of static obstacles in different directions;

[0056] O d ={(X d，i , Y d，i , v x，i , v y，i )}

[0057] Among them, X d，i , Y d，i , v x，i , v y，iThey respectively represent the coordinate points of the dynamic obstacles in different directions and the component velocities of the dynamic obstacles in different directions.

[0058] In a preferred embodiment of the present invention, a constant velocity model is used to predict the future position of a dynamic obstacle:

[0059] X d，i，k =X d，i，0 +v x，i ·t k

[0060] Y d，i，k =Y d，i，0 +v y，i ·t k

[0061] Solve the objective function J to obtain the control input sequence corresponding to the preliminary solution, including the initial state of the vehicle, including the initial position, speed, heading angle and other state information of the vehicle; the target path generated by the Bezier curve; dynamic and static obstacle information, including the position, speed and movement direction of the obstacle, and generate the control input sequence and the corresponding state sequence:

[0062] x k+1 =f(x k ,u k )

[0063] Among them, x k is the state vector at the kth time step, u k is the control input at the kth time step.

[0064] By controlling the input sequence U input N p Control future time point N p Vehicle output at the time.

[0065] In addition, certain constraints need to be added when using nonlinear model predictive control. In a preferred embodiment of the present invention, a Kinematic Bicycle Model is used as a vehicle dynamics model to constrain the nonlinear model.

[0066] Among them, the state vector:

[0067] x = [X, Y, θ, v] T

[0068] Where X, Y are the vehicle's position in the global coordinate system, θ is the vehicle's heading angle, and v is the vehicle's velocity.

[0069] The control input u is expressed as:

[0070] μ=[a,δ] T

[0071] Among them, a represents acceleration and δ represents the steering angle.

[0072] The vehicle state is represented as:

[0073]

[0074] Wherein, L represents the wheelbase of the vehicle.

[0075] Constraints are imposed on the vehicle's control input, speed, acceleration, and obstacle avoidance, wherein each constraint threshold can be set in advance or obtained through experimental data.

[0076] A: Control input constraints

[0077]

[0078] B: Obstacle avoidance constraints

[0079] d obs，k ≥d safe

[0080] d safe It is a defined safety distance that is set or obtained through experimental data.

[0081] C: Velocity and acceleration constraints

[0082]

[0083] like Figure 2 Shown is a relationship diagram of nonlinear model predictive control, objective function and constraint model in step S1.

[0084] S2. Introduce different disturbances and randomness into the preliminary solution predicted by the nonlinear model to obtain multiple candidate solutions, and use Bezier curves to connect the sampling points of each candidate solution to obtain the preliminary trajectory of each candidate solution.

[0085] A set of candidate solutions is generated based on the preliminary solution predicted by the nonlinear model. Given the starting point ps and target point pg of the candidate solution, as well as several navigation points between the starting point ps and the target point pg, a series of sampling points are obtained. The set of sampling points is represented as S:

[0086] S=[P 0 , P 1 , P 2 , P 3 ...P n ]

[0087] Among them, P 0 =P s , P n =Pg , P i ∈R 2 ,i=0,1,2,3...(n-1),n,R 2 Represents the entire plane.

[0088] At the same time, the risk field model is used to evaluate the risk of each sampling point to avoid high-risk areas. The risk field model is specifically a two-dimensional Gaussian distribution function determined by the risk intensity coefficient, obstacle location, and risk field influence range.

[0089] In a preferred embodiment of the present invention, the two-dimensional Gaussian distribution function can be specifically expressed as:

[0090]

[0091] Among them, α i is the risk intensity coefficient, (X o，i , Y o，i ) is the position of the ith obstacle, Indicates the scope of influence of the control risk field;

[0092] According to the risk field function R(X, Y), the area with lower risk value is selected for path sampling;

[0093] (X s , Y s )∈{(X, Y)|R(X, Y)≤R threshold}

[0094] Among them, R threshold is a set threshold.

[0095] The sampling points in the low-risk area are smoothly connected using an n-order Bezier curve, which is defined as:

[0096]

[0097] Among them C i is the coordinate of the control point, C i =(x i ,y i ), which can be coincident with the sampling point or obtained through optimization; b i，n (t) is the Bernstein basis function.

[0098] At the same time, the sampling points are constrained. To ensure the feasibility of the sampling points.

[0099] B″(t) is the second-order derivative of the nth-order Bezier curve, which represents the curvature of the connected path. In this embodiment, by optimizing the coordinates of the control point C i Minimize the curvature, that is To improve the smoothness of the curve.

[0100] In addition, in order to generate diverse candidate solutions, different disturbances and randomness are introduced into the preliminary solution of the nonlinear model predictive control, and the preliminary trajectory is slightly changed to generate diverse candidate solutions, usually dozens to hundreds of candidate solutions, and the sampling points are connected using Bezier curves to obtain the preliminary trajectory of each candidate solution.

[0101] S3. Construct an initial population of the genetic algorithm based on the preliminary solution of the nonlinear model predictive control, evaluate the fitness of each candidate solution in the initial population, select the candidate solution with the highest fitness as the optimal solution, output the control sequence corresponding to the optimal solution, and output the preliminary trajectory corresponding to the optimal solution as the vehicle driving trajectory.

[0102] like Figure 3 As shown, the specific steps of S3 can be divided into:

[0103] S31. Score the fitness of each candidate solution according to the smoothness, obstacle avoidance performance, and path length of the trajectory of each candidate solution.

[0104] Evaluate the fitness of each candidate solution and select a solution X for each candidate solution based on comprehensive indicators such as trajectory smoothness, obstacle avoidance performance, and path length. i Rate.

[0105] In a preferred embodiment of the present invention, the score of each candidate solution is calculated using the applicable function F(X i )express;

[0106] F(X i )=w 5 S smooth (X i )+w 6 S obstacle (X i )+w 7 S length (X i )

[0107] Among them, w 5 、w 6 and w 7 is the weight coefficient, S smooth (X i ) is the smoothness score of the trajectory, which is the sum of the curvature changes of the preliminary trajectory obtained based on S2; S obstacle (X i ) is the obstacle avoidance performance score, which is obtained based on the minimum distance between the trajectory and the obstacle; S length (X i ) is the path length score, obtained based on the total path length of the preliminary trajectory.

[0108]

[0109] Among them, κ k is the curvature at the kth time step, N p is the population size.

[0110]

[0111] Among them, d obs (x k ) is the distance between the vehicle and the nearest obstacle at the kth time step.

[0112]

[0113] Where ΔX k and ΔY k is the trajectory increment at the kth time step.

[0114] S32: Based on the probability selection of fitness values, the candidate solutions with higher fitness are retained for the next genetic operation.

[0115] For each candidate solution X i Calculate the fitness F(X i ), select the individuals with better performance according to the fitness value for the next genetic operation, and retain the individuals with higher fitness based on the probability selection of the fitness value, and continue the genetic operation.

[0116] S33. Re-score, select and iterate the fitness of the candidate solutions of the new generation population obtained by genetic operation until the stopping condition is reached, and select the candidate solution with the highest fitness in the final generation.

[0117] The fitness of the new generation population generated by genetic operations is evaluated, and the fitness value of each individual is updated. When the preset number of iterations is reached, or the fitness of individuals in the population does not change much, the iteration is stopped, and finally the individual with the highest fitness is selected as the optimal solution X°, and the control sequence corresponding to the optimal solution is output, and the preliminary trajectory corresponding to the optimal solution is output as the vehicle driving trajectory.

[0118] Embodiment 2

[0119] like Figure 4 The figure is a schematic diagram of the structure of a computer device provided by an embodiment of the present invention, such as a smart phone, tablet computer, laptop computer, desktop computer, rack server, blade server, tower server or cabinet server (including an independent server, or a server cluster composed of multiple servers) that can execute programs. The computer device 20 of this embodiment includes at least but is not limited to: a memory 21 and a processor 22 that can communicate with each other via a system bus, such as Figure 4 It should be pointed out that Figure 4 Only computer device 20 is shown with components 21 - 22 , but it should be understood that implementing all of the components shown is not a requirement, and more or fewer components may alternatively be implemented.

[0120] In this embodiment, the memory 21 (i.e., readable storage medium) includes flash memory, hard disk, multimedia card, card-type memory (e.g., SD or DX memory, etc.), random access memory (RAM), static random access memory (SRAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), programmable read-only memory (PROM), and the memory 21 can also be an external storage device of the computer device 20, such as a plug-in hard disk equipped on the computer device 20, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (FlashCard), etc. Of course, the memory 21 can also include both the internal storage unit of the computer device 20 and its external storage device. In this embodiment, the memory 21 is generally used to store the operating system and various application software installed in the computer device 20, such as the program code in the embodiment. In addition, the memory 21 can also be used to temporarily store various data that have been output or are to be output.

[0121] The processor 22 may be a central processing unit (CPU), a controller, a microcontroller, a microprocessor, or other data processing chips in some embodiments. The processor 22 is generally used to control the overall operation of the computer device 20. In this embodiment, the processor 22 is used to run the program code stored in the memory 21 or process data to implement a driving trajectory planning method for predictive control using a nonlinear model in the method embodiment.

[0122] The present invention utilizes nonlinear model predictive control to solve a preliminary solution to the objective function, and evaluates the fitness of each group of candidate solutions generated by the preliminary solution to select the optimal solution, thereby finding a good balance between vehicle driving scene parameters such as obstacle avoidance performance and path length, and improving the feasibility and comfort of the generated trajectory; at the same time, before connecting the trajectory, the risk field model is used to evaluate and predict the risk level of the sampling point, thereby avoiding the vehicle from entering a high-risk area and reducing the probability of vehicle damage and personal injury; in addition, the present invention utilizes Bezier curves to connect the trajectory, and minimizes the curvature by optimizing the coordinates of the curve control points, thereby improving the smoothness of the trajectory and reducing the possibility of sharp turns and discontinuity of the path.

[0123] It should be pointed out that, according to the needs of implementation, the various steps / components described in this application can be split into more steps / components, and two or more steps / components or partial operations of steps / components can be combined into new steps / components to achieve the purpose of the present invention.

[0124] The order of execution of each step in the above embodiment does not mean the order of execution. The order of execution of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the present application.

[0125] It should be understood that those skilled in the art can make improvements or changes based on the above description, and all these improvements and changes should fall within the scope of protection of the appended claims of the present invention.

Claims

1. A driving trajectory planning method using a nonlinear model for predictive control, characterized in that: The method comprises: Obtain the cost of each driving stage of the vehicle, design and solve the nonlinear model prediction objective function, and obtain the preliminary solution of the nonlinear model predictive control; Introduce different disturbances and randomness into the preliminary solution predicted by the nonlinear model to obtain multiple candidate solutions, and use Bezier curves to connect the sampling points of each candidate solution to obtain the preliminary trajectory of each candidate solution; Based on the preliminary solution of nonlinear model predictive control, the initial population of the genetic algorithm is constructed, the fitness of each candidate solution in the initial population is evaluated, the candidate solution with the highest fitness is selected as the optimal solution, the control sequence corresponding to the optimal solution is output, and the preliminary trajectory corresponding to the optimal solution is output as the vehicle driving trajectory.

2. The driving trajectory planning method using a nonlinear model for predictive control according to claim 1, characterized in that: The nonlinear model prediction objective function is specifically the sum of the costs of each driving phase of the vehicle; The cost includes: position error, heading angle error, control input norm and artificial potential field potential energy.

3. The driving trajectory planning method using a nonlinear model for predictive control according to claim 1, characterized in that: Nonlinear model predictive control constrains the vehicle's control input, speed, acceleration, and obstacle avoidance through the vehicle dynamics model.

4. The driving trajectory planning method using a nonlinear model for predictive control according to claim 1, characterized in that: Before connecting the sampling points using Bezier curves, the risk field model is used to evaluate the risk level of each sampling point, and sampling points in areas with lower risk values ​​are selected for connection.

5. The driving trajectory planning method using a nonlinear model for predictive control according to claim 4, characterized in that: The risk field model is specifically a two-dimensional Gaussian distribution function determined by the risk intensity coefficient, the obstacle position, and the risk field influence range.

6. The driving trajectory planning method using a nonlinear model for predictive control according to claim 5, characterized in that: The obstacle position includes a static obstacle position and a dynamic obstacle position; The future position of the dynamic obstacle is predicted by a constant speed model.

7. The driving trajectory planning method using a nonlinear model for predictive control according to claim 1, characterized in that: When the sampling points are connected by the Bezier curve, the curvature change of the Bezier curve is minimized by optimizing the coordinates of the control points of the Bezier curve; the curvature is the second-order derivative of the Bezier curve.

8. The driving trajectory planning method using a nonlinear model for predictive control according to claim 1, characterized in that: Evaluate the fitness of each candidate solution in the initial population and select the candidate solution with the highest fitness. The specific method is as follows: The fitness of each candidate solution is scored based on the smoothness of its trajectory, obstacle avoidance performance, and path length; Based on the probability selection of fitness value, the candidate solutions with higher fitness are retained for the next genetic operation; The fitness of the candidate solutions of the new generation population obtained by genetic operation is re-scored, selected and iterated until the stopping condition is reached, and the candidate solution with the highest fitness in the final generation is selected.

9. The driving trajectory planning method using a nonlinear model for predictive control according to claim 8, characterized in that: The stopping condition is specifically: reaching a preset number of iterations, or a small change in the fitness of candidate solutions in the population.

10. A computer device, characterized in that: The invention comprises a memory, a processor and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, a driving trajectory planning method for predictive control using a nonlinear model as described in claims 1-9 is implemented.

Images