A multi-vehicle collaborative trajectory planning optimization method based on a safety corridor

Abstract

The present invention discloses a multi-vehicle collaborative trajectory planning optimization method based on a safety corridor, belonging to the field of autonomous driving, which includes the following steps: representing a discretized spatio-temporal corridor of driverless vehicles based on the real-time pose information and rectangular contour information of the driverless vehicles in the vehicle group; constructing an objective cost function with the geometric information of the spatio-temporal corridor and the movement time of the driverless vehicles; establishing boundary constraints of the spatio-temporal corridor according to the poses of the driverless vehicles in the vehicle group, and establishing collision avoidance constraint conditions in combination with the position information of the spatio-temporal corridor of the driverless vehicles; establishing a mixed-integer quadratic programming model according to the objective cost function and multiple constraint conditions, and obtaining the global optimal solution through calculation, that is, the position information and movement time information of the spatio-temporal safety corridor; constructing a driverless vehicle trajectory optimization problem according to the obtained safe continuous corridor, and generating a driverless vehicle movement trajectory through calculation and optimization. The present invention can obtain the fastest passing trajectory of driverless vehicles without collision, and improve the overall passing efficiency of multiple vehicle groups.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving, and in particular relates to a multi-vehicle collaborative trajectory planning optimization method based on a safety corridor. Background Art

[0002] With the frequent application of multi-vehicle collaborative technology in transportation scenarios such as traffic, ports, and warehouses, its potential value has attracted increasing attention. In conflict scenarios in structured or unstructured environments, the core of multi-vehicle collaborative control is to make safe collaborative motion decisions and smooth trajectory optimizations, so that each driverless vehicle in the group can reach the target point safely and quickly, and meet the dynamic characteristics of the driverless vehicle during the movement process. Based on this, it is necessary to design corresponding decision-making algorithms for the driverless vehicle group to globally coordinate the behaviors of each driverless vehicle in the group and then send instructions to the corresponding individuals. However, the current research on multi-vehicle collaborative motion planning mainly focuses on specific scenarios such as ramp merging and intersections, and cannot be adapted to general motion scenarios. Therefore, a general method is needed to ensure the motion safety and efficiency of multi-vehicle groups in conflict scenarios.

[0003] Currently, the algorithms for multi-vehicle cooperative motion control are mainly based on game algorithms and sampling algorithms. Some literature has established general decision-making models and trajectory optimization models. For example, the Spielberg game algorithm is used to make decisions on the motion behaviors of each driverless vehicle within the group, and model predictive control and artificial potential field method are used to plan the motion trajectories of driverless vehicles. For details, see Hang, Peng, et al. "Human-like decision making for autonomous driving: A noncooperative game theoretic approach." IEEE Transactions on Intelligent Transportation Systems 22.4 (2020): 2076-2087; or, based on the mixed-integer quadratic programming model and the nonlinear quadratic programming model, a general multi-vehicle motion decision-making and trajectory planning framework is established. For details, see Zhang X, Wang B, Lu Y, et al. A Hierarchical Multi-Vehicle Coordinated Motion Planning Method based on Interactive Spatio-Temporal Corridors[J]. IEEE Transactions on Intelligent Vehicles, 2023. The existing technologies mainly focus on the problem of multi-vehicle conflict-free target position decision-making and safe trajectory planning, assuming that all driverless vehicles reach the target point within the same time. However, in actual situations, in addition to traffic safety, traffic efficiency is also a relatively important issue. The existing research does not consider the problem of multi-vehicle group traffic efficiency in the time domain.

[0004] The present invention proposes a multi-vehicle cooperative trajectory planning and optimization method based on a safety corridor, which can improve traffic efficiency on the basis of ensuring the safe passage of a group of driverless vehicles, and can be applied to tasks such as conflict resolution in road traffic systems, having strong practical application value. Summary of the Invention

[0005] The object of the present invention is to provide a multi-vehicle cooperative trajectory planning and optimization method based on a safety corridor, which solves the problem of multi-vehicle risk conflicts in the spatio-temporal domain in the prior art and can improve traffic efficiency.

[0006] To achieve the above object, the present invention provides a multi-vehicle cooperative trajectory planning and optimization method based on a safety corridor, including the following steps:

[0007] Step 1: Obtain the rough path information to be optimized from the initial position to the target position of each driverless vehicle in the current environment, and divide it into several reference path points according to the path length; among them, the information of each driverless vehicle's reference path point includes the horizontal and vertical coordinate positions.

[0008] Step 2: Based on the reference path points of each driverless vehicle constructed in Step 1, construct the safety corridor cubes of each driverless vehicle. The number of safety corridor cubes of each driverless vehicle is the same as the number of reference path points; among them, the safety corridor cube is represented by a rectangular cube, and the specific parameters include the position of the corridor center point, the corridor length and width, and the passing time of the current corridor is used as the height.

[0009] Step 3: Based on the information of the safety corridor cubes of each driverless vehicle established in Step 2, construct the cost index function to be optimized for the driverless vehicle group, including the tracking deviation cost and the corridor space occupancy cost; among them, the tracking deviation cost is represented by the distance value between the position of the safety corridor cube center point and the reference path point; the corridor space occupancy cost is composed of the corridor length and width, and the passing time cost is composed of the corridor height.

[0010] Step 4: Based on the cost index function to be optimized constructed in Step 3, combine the mixed integer programming algorithm to construct a constrained quadratic programming problem to be optimized; among them, the variables in the cost index function to be optimized include the position coordinates of the safety corridor, the corridor length and width information, and the passing time information representing the corridor height; the constraint conditions in the quadratic programming problem to be optimized include the position constraint of the safety corridor center point, the size constraint of the safety corridor, and the anti-collision constraint between safety corridors.

[0011] Step 5: Based on the quadratic programming problem to be optimized constructed in Step 4, use an optimization solver to calculate the global optimal solution that satisfies the constraint conditions, and construct the safe passing corridors of each driverless vehicle; among them, the global optimal solution includes the position coordinates of the safety corridor, the corridor geometric parameters, and the passing time at each moment.

[0012] Step 6: Based on the safe passing corridors of each driverless vehicle obtained in Step 5, describe the trajectory generation target in the safety corridor as a nonlinear programming problem, and solve the optimal driving trajectory that satisfies the constraint conditions through a solver; the optimal driving trajectory is located inside the safety corridor and satisfies the vehicle dynamics model and other constraint conditions.

[0013] Step 7: Based on the length of the safe corridor of the driverless vehicle constructed in Step 5 and the driving trajectory of the driverless vehicle planned in Step 6, judge whether the length of the driving trajectory located in the safety corridor satisfies the length of the safety corridor. If the planned driving trajectory satisfies the length of the safety corridor, return to Step 1 to start the planning of the next section of the safety corridor; if the planned driving trajectory does not satisfy the length of the safety corridor, return to Step 6 to continue execution.

[0014] Preferably, the specific process of Step 1 is as follows:

[0015] In the inertial coordinate system, for the driverless vehicle group , for the driverless vehicle a set of reference path points is established , where , represents the position coordinates and orientation angle at the reference path point, , .

[0016] Preferably, the specific process of Step 2 is as follows:

[0017] Near the reference path points described in Step 1, safety corridors in the form of cubes are established for each driverless vehicle. Since the corridor geometric structure is a rectangular cube, for the driverless vehicle , at the th reference path point nearby, the corridor cube is represented by the position coordinates of the corridor center point and the corridor geometric dimensions . Therefore, the corridor cube is expressed as shown in the following formula:

[0018] ;

[0019] where the maximum and minimum boundary positions of the safety corridor along the direction and along the direction are expressed as:

[0020] ;

[0021] In the formula, represents the position of the th corridor center point, represents the size of the th corridor, represents the geometric information of the th corridor, represents the abscissa of the position of the th corridor center point, represents the ordinate of the position of the th corridor center point, represents the length of the minimum boundary of the th corridor direction from the center point, represents the length of the maximum boundary of the th corridor direction from the center point, represents the th corridor The length of the maximum boundary in a direction from the central point represents the lower boundary of the th corridor, represents the height of the th corridor, represents the position of the minimum boundary in the th corridor, represents the position of the maximum boundary in the th corridor, represents the position of the minimum boundary in the th corridor, represents the position of the maximum boundary in the

[0022] Preferably, the specific process of step 3 is as follows:

[0023] For each corridor cube established for the driverless vehicle , construct the cost index function to be optimized, which includes the path point tracking deviation cost , the corridor space occupancy cost , and the corridor travel time cost , specifically expressed as shown in the following formula:

[0024] ;

[0025] where respectively represent the weight coefficients of the corresponding cost functions in the index to be optimized.

[0026] Preferably, the specific process of step 4 is as follows:

[0027] Based on the cost index function to be optimized constructed in step 3, combine the mixed integer programming algorithm to construct a quadratic programming problem to be optimized based on constraints, expressed as follows:

[0028] ;

[0029] where represents the travel priority of the driverless vehicle

[0030] Preferably, the position constraint of the safety corridor central point in step 4 is described as that the distance between the corridor central points does not exceed the maximum driving distance, and the specific expression is as follows:

[0031] ;

[0032] where represents the maximum speed of the vehicle moving forward, represents the vehicle at the reference orientation angle at the moment.

[0033] Preferably, the size constraints of the safety corridor in step 4 include the size constraints of the corridor itself and the size constraints of the overlapping area between adjacent corridors. The specific expressions are as follows:

[0034] The size constraint of the corridor itself at any moment is:

[0035] ;

[0036] In the formula, respectively represent the vehicle direction and the vehicle body length in the direction;

[0037] The size of the corridor at any moment needs to satisfy being within the vehicle's motion ability range at the previous moment:

[0038] ;

[0039] Among them:

[0040] ;

[0041] In the formula, represents the maximum speed of the vehicle moving backward, represents the minimum distance that the driverless vehicle moves along the from the moment to the moment in the direction, represents the maximum distance that the driverless vehicle moves along the from the moment to the moment in the direction, represents the minimum distance that the driverless vehicle moves along the from the moment to the moment in the direction, represents the maximum distance that the driverless vehicle moves along the from the moment to the moment in the direction;

[0042] The size constraint of the overlapping area between adjacent corridors is:

[0043] .

[0044] Preferably, the anti-collision constraint between the safety corridors in step 4 is described as different vehicles and The safety corridor cubes at the same moment do not overlap, and the specific expression is as follows:

[0045] ;

[0046] Among them, , is the safe driving distance maintained between vehicles, and the value is a constant, is a binary variable used to represent the th corridor cube corresponding to the driverless vehicle and the th corridor cube corresponding to the driverless vehicle Whether they overlap. If they overlap, the value is 1; if they do not overlap, the value is 0; is a binary variable used to represent the moment value corresponding to the th time period of the driverless vehicle and the moment value corresponding to the th time period of the driverless vehicle Whether they overlap. If they overlap, the value is 0; if they do not overlap, the value is 1, .

[0047] Preferably, in step 5, by calling the Gurobi optimization solver, the value of the variable is obtained when the cost function takes the minimum value under the constraint conditions in step 4, and a safe passage corridor formed by connecting corridor cubes of the driverless vehicle is obtained.

[0048] Preferably, in step 6, the trajectory generation target in the safety corridor is described as a non-linear programming problem including a cost function and constraint conditions, and the optimal driving trajectory satisfying the constraint conditions is solved through a solver. The specific process is as follows:

[0049] Based on the safe passage corridor constructed in step 5, a cost function including trajectory smoothness cost, lateral and longitudinal comfort cost, and spatio-temporal deviation cost from the corridor center point is constructed, and is specifically expressed as:

[0050] Trajectory smoothness cost: , where represents the weight coefficient of the trajectory smoothness cost in the optimization problem, represents the vehicle at the moment The curvature of the trajectory at;

[0051] Lateral and longitudinal comfort cost: , where represents the weight coefficient of the control signal, represents the vehicle at time the front wheel steering angle change rate, represents the vehicle at time the acceleration along the vehicle body's forward direction;

[0052] The spatio-temporal deviation cost from the corridor center point: where represents the error weight coefficient in different directions, represents the vehicle the position coordinates of the center of mass at time ;

[0053] Among them, , represents the value of the running time of the unmanned vehicle's trajectory ;

[0054] Based on the above cost function, the non-linear programming problem is expressed as follows:

[0055] ;

[0056] Among them, the constraint conditions include:

[0057] The dynamic constraints of the unmanned vehicle:

[0058] ;

[0059] where correspond to the position coordinates of the center of mass of the vehicle body and the angle between the vehicle body and the reference coordinate system respectively, respectively represent the vehicle the steering angle of the front wheel, the speed in the vehicle body direction, and the acceleration along the vehicle body direction;

[0060] The unmanned vehicle Control input quantity constraints: ;

[0061] The unmanned vehicle The speed and acceleration in the vehicle body direction need to satisfy the dynamic constraints:

[0062] ;

[0063] The unmanned vehicle The position and the entire vehicle body need to be within the safety corridor constraints:

[0064] ;

[0065] ;

[0066] Among them, , respectively represent the vehicle at the moment coordinate values corresponding to the four corner points of the vehicle body.

[0067] Therefore, the present invention adopts the above-mentioned multi-vehicle collaborative trajectory planning optimization method based on a safety corridor. By optimizing the passing time of the safety corridor of the driverless vehicle, the passing efficiency of the driverless vehicle group can be improved on the basis of safe rendezvous.

[0068] The technical solution of the present invention will be further described in detail below with reference to the drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 is the overall flowchart of a multi-vehicle collaborative trajectory planning optimization method based on a safety corridor according to the present invention;

[0070] Figure 2 is the schematic diagram of the reference path of each driverless vehicle according to the present invention;

[0071] Figure 3 is the schematic diagram of the planned safe space-time corridor of each driverless vehicle according to the present invention;

[0072] Figure 4 is the schematic diagram of the safe space-time corridor planned by the traditional space-time safety corridor planning algorithm;

[0073] Figure 5 is the schematic diagram of the safe driving trajectory of each driverless vehicle according to the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0074] The following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the claimed invention, but merely represents selected embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the scope of protection of the present invention.

[0075] Please refer to Figures 1-5 , a multi-vehicle collaborative trajectory planning optimization method based on a safety corridor, comprising the following steps:

[0076] Step 1: Obtain the rough path information to be optimized between the initial position and the target position of each driverless vehicle in the current environment, and divide it into several reference path points according to the path length; wherein the information of each driverless vehicle reference path point includes the horizontal and vertical coordinate positions; the specific process is as follows:

[0077] In the inertial coordinate system, for the driverless vehicle group , for the driverless vehicle establish a reference path point set , wherein, , represents the position coordinates and orientation angle at the reference path point, , .

[0078] Step 2: Construct the safety corridor cubes for each driverless vehicle based on the reference path points constructed in Step 1; the safety corridor cube is represented by a rectangular cube, and the specific parameters include the position of the corridor center point, the length and width of the corridor, and the passing time of the current corridor is used as the height; the specific process is as follows:

[0079] Near the reference path points described in Step 1, establish the safety corridor cubes for each driverless vehicle. Since the corridor geometric structure is a rectangular cube, for the driverless vehicle , at the th reference path point nearby, the corridor cube is represented by the position coordinates of the corridor center point and the corridor geometric dimensions , so the corridor cube is expressed as shown in the following formula:

[0080] ;

[0081] where the maximum and minimum boundary positions of the safety corridor along the direction and along the direction are expressed as:

[0082] ;

[0083] In the formula, represents the position of the th corridor center point, represents the size of the th corridor, represents the geometric information of the th corridor, represents the abscissa of the position of the th corridor center point, represents the ordinate of the position of the th corridor center point, represents the length of the minimum boundary of the th corridor direction from the center point, represents the length of the maximum boundary of the th corridor direction from the center point, represents the length of the maximum boundary of the th corridor direction from the center point, represents the lower boundary of the th corridor The length of the minimum boundary distance of the direction to the central point Indicates the height of the th corridor, corridor The position of the minimum boundary in the direction Indicates the th corridor The position of the maximum boundary in the direction Indicates the th corridor The position of the minimum boundary in the direction Indicates the th corridor The position of the maximum boundary in the direction

[0084] Step 3: According to the safety corridor cube information established in Step 2, construct the cost index function to be optimized, including tracking deviation cost, corridor space occupancy cost, and corridor passage time cost; among them, the tracking deviation cost is represented by the distance value between the central point position of the safety corridor cube and the reference trajectory position; the corridor space occupancy cost is composed of the corridor length and width; the corridor passage time cost is composed of the corridor height; the specific process is as follows:

[0085] For the corridor cube established for each driverless vehicle , construct the cost index function to be optimized, including the path point tracking deviation cost , corridor space occupancy cost , corridor passage time cost , which is specifically expressed as shown in the following formula:

[0086] ;

[0087] Where respectively represent the weight coefficients of the corresponding cost functions in the index to be optimized

[0088] Step 4: Based on the cost index function to be optimized constructed in Step 3, combine the mixed integer programming algorithm to construct a constrained quadratic programming problem to be optimized; among them, the variables in the cost index function to be optimized include the safety corridor position coordinates, corridor length and width information, and corridor passage time; the constraint conditions in the quadratic programming problem to be optimized include the safety corridor central point position constraint, safety corridor size constraint, and anti-collision constraint between safety corridors; the specific process is as follows:

[0089] Based on the cost index function to be optimized constructed in Step 3, combine the mixed integer programming algorithm to construct a constrained quadratic programming problem to be optimized, which is expressed as follows:

[0090] ;

[0091] Among them, represents the passing priority of the driverless vehicle .

[0092] Among them, the position constraint of the safety corridor central point is described as that the distance of the corridor central point does not exceed the maximum driving distance, and the specific expression is as follows:

[0093] ;

[0094] Among them, represents the maximum speed of the vehicle moving forward, represents the vehicle at the reference orientation angle at the moment.

[0095] The size constraint of the safety corridor includes the size constraint of the corridor itself and the size constraint of the overlapping area of adjacent corridors. The specific expression is as follows:

[0096] The size constraint of the corridor itself at any time is:

[0097] ;

[0098] In the formula, are respectively the body lengths in the directions of the vehicle and ;

[0099] The size of the corridor at any time needs to satisfy being within the vehicle motion ability range at the previous moment:

[0100] ;

[0101] Among them:

[0102] ;

[0103] In the formula, represents the maximum speed of the vehicle moving backward, represents the minimum distance that the driverless vehicle moves along the direction from the moment to the moment, represents the maximum distance that the driverless vehicle moves along the direction from the moment to the moment, represents the minimum distance that the driverless vehicle moves along the direction from the moment to the moment, represents the minimum distance that the driverless vehicle moves along the The maximum distance traveled from the starting moment along the direction at the moment; The size constraint of the overlapping area of adjacent corridors is:

[0104] The anti-collision constraint between safety corridors is described as that the safety corridor cubes of different vehicles

[0105] will not overlap at the same moment. The specific expression is as follows:

[0106] Among them, and is the safe driving distance between vehicles, and its value is a constant.

[0107] ;

[0108] where , is the safe driving distance maintained between vehicles, and its value is a constant. is a binary variable used to represent whether the -th corridor cube corresponding to the driverless vehicle overlaps with the -th corridor cube corresponding to the driverless vehicle . If they overlap, the value is 1; if not, the value is 0. is a binary variable used to represent whether the moment value corresponding to the -th time period of the driverless vehicle overlaps with the moment value corresponding to the -th time period of the driverless vehicle . If they overlap, the value is 0; if not, the value is 1. .

[0109] Step 5. Based on the quadratic programming problem to be optimized constructed in Step 4, use an optimization solver to calculate the global optimal solution that satisfies the constraint conditions, and construct the safe passing corridors for each driverless vehicle. The global optimal solution includes the position coordinates of the safety corridors, corridor geometric parameters, and passing times at each moment. Specifically: By calling the Gurobi optimization solver, find the values of the variables that minimize the cost function under the constraint conditions in Step 4, and obtain the safe passing corridor formed by connecting corridor cubes of the driverless vehicle .

[0110] Step 6: Based on the safe driving corridors of each driverless vehicle obtained in Step 5, describe the trajectory generation objective within the safe corridor as a non-linear programming problem, and solve for the optimal driving trajectory that satisfies the constraint conditions through a solver; the optimal driving trajectory is located inside the safe corridor and satisfies the vehicle dynamics model and other constraint conditions; among them, describing the trajectory generation objective within the safe corridor as a non-linear programming problem including a cost function and constraint conditions, and solving for the optimal driving trajectory that satisfies the constraint conditions through a solver, the specific process is as follows:

[0111] Based on the safe driving corridor constructed in Step 5, construct a cost function including trajectory smoothness cost, longitudinal and lateral comfort cost, and spatio-temporal deviation cost from the corridor central point, which is specifically expressed as:

[0112] Trajectory smoothness cost: , where represents the weight coefficient of the trajectory smoothness cost in the optimization problem, represents the vehicle at time the curvature of the trajectory at;

[0113] Longitudinal and lateral comfort cost: , where represents the weight coefficient of the control signal, represents the vehicle at time the change rate of the front wheel steering angle at, represents the vehicle at time the acceleration along the vehicle's forward direction at;

[0114] Spatio-temporal deviation cost from the corridor central point: , where represents the error weight coefficient in different directions, represents the vehicle the position coordinates of the centroid at time at;

[0115] Among them, , represents the running time value of the driverless vehicle trajectory ;

[0116] Based on the above cost function, the non-linear programming problem is expressed as follows:

[0117] ;

[0118] Among them, the constraint conditions include:

[0119] Driverless vehicle dynamics constraints:

[0120] ;

[0121] wherein correspond to the position coordinates of the vehicle body centroid and the angle between the vehicle body and the reference coordinate system respectively, respectively represent the vehicle front wheel steering angle, the speed in the vehicle body direction, and the acceleration along the vehicle body direction;

[0122] driverless vehicle Control input quantity constraint: ;

[0123] driverless vehicle The speed and acceleration in the vehicle body direction need to satisfy the power constraint:

[0124] ;

[0125] driverless vehicle The position and the entire vehicle body need to be located within the safety corridor constraint:

[0126] ;

[0127] ;

[0128] wherein, , respectively represent the vehicle at time coordinate values corresponding to the four corner points of the vehicle body.

[0129] Step 7, according to the length of the safety corridor of the driverless vehicle constructed in Step 5 and the driving trajectory of the driverless vehicle planned in Step 6, judge whether the length of the driving trajectory located within the safety corridor meets the length of the safety corridor. If the planned driving trajectory meets the length of the safety corridor, return to Step 1 to start the planning of the next section of the safety corridor; if the planned driving trajectory does not meet the length of the safety corridor, return to Step 6 to continue execution.

[0130] Embodiment

[0131] The present invention first constructs the reference path information of each driverless vehicle in a multi-vehicle group, then establishes the variable information describing the spatio-temporal safety corridor of the driverless vehicle. Next, a cost index function to be optimized related to the position and geometric information of the spatio-temporal safety corridor of the driverless vehicle is established, and constraints are constructed for the variables to be optimized. A suitable optimization solver is selected to ensure that the solution obtained by the optimization solver is the global optimal solution of the system. Then, the safety spatio-temporal corridor is constructed using the obtained variable information. Subsequently, each driverless vehicle plans its driving trajectory within its own safety spatio-temporal corridor and satisfies the trajectory smoothness, the dynamic constraints of the driverless vehicle, etc. Finally, it is judged whether the safe driving trajectory planned by the driverless vehicle reaches the target position. If it reaches, the whole process is completed; if not, it is re-planned. It should be noted that before the execution of the whole process, the reference path positions need to be planned for each driverless vehicle.

[0132] The following is the simulation verification of the multi-vehicle cooperative trajectory planning optimization method based on the safety corridor designed by the present invention; the interactive passing tasks of 2 driverless vehicles are simulated. The reference path information of the driverless vehicle group is shown in Table 1:

[0133] Table 1

[0134] ;

[0135] After establishing the optimization cost function and constraints, the parameter values are shown in Table 2:

[0136] Table 2

[0137] ;

[0138] The reference path information of the driverless vehicle is as Figure 2 shown, and there are cross conflicts in the reference paths of driverless vehicle 1 and driverless vehicle 2. The safe passing corridors planned for driverless vehicle 1 and driverless vehicle 2 are as Figure 3 shown. It can be seen from Figure 3 that the spatio-temporal positions of the driverless vehicles do not overlap, and collisions can be avoided. Figure 4 is the safe driving corridor planned for the driverless vehicle by the traditional spatio-temporal safety corridor planning algorithm. Compared with Figure 3 , it can be seen that the planning algorithm proposed by the present invention can significantly reduce the passing time of driverless vehicles under the same operating conditions. In addition, Figure 5 are the driving trajectories planned by each driverless vehicle within its own safety spatio-temporal corridor.

[0139] Therefore, the present invention adopts the above-mentioned multi-vehicle collaborative trajectory planning optimization method based on a safety corridor, and solves the problem of multi-vehicle risk conflicts in the spatio-temporal domain existing in the prior art by optimizing the passing time of the safety corridor of the driverless vehicle, thereby improving the overall passing efficiency of the multi-vehicle group.

[0140] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions of the present invention or make equivalent replacements, and these modifications or equivalent replacements cannot make the modified technical solutions deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A multi-vehicle collaborative trajectory planning optimization method based on a safety corridor, characterized in that: The following steps are involved: Step 1: Obtain the rough path information to be optimized from the initial position to the target position of each unmanned vehicle in the current environment, and divide it into a number of reference path points according to the path length; wherein the reference path point information of each unmanned vehicle includes the horizontal and vertical coordinate positions; Step 2: construct each unmanned vehicle safety corridor cube according to each unmanned vehicle reference path point constructed in step 1, and the number of each unmanned vehicle safety corridor cube is the same as the number of reference path points; wherein the safety corridor cube is represented by a rectangular cube, and the specific parameters include the position of the corridor hub, the length and width of the corridor, and the travel time of the current corridor is used as the height; Step 3: Based on the information of the safety corridor cubes of each driverless vehicle established in step 2, a cost index function to be optimized for the driverless vehicle group is constructed, including tracking deviation cost and corridor space occupancy cost; wherein the tracking deviation cost is represented by the distance value between the central point position of the safety corridor cube and the reference path point; the corridor space occupancy cost is composed of the corridor length and width, and the travel time cost is composed of the corridor height; Step 4: Based on the cost index function to be optimized constructed in step 3, a constraint-based quadratic programming problem to be optimized is constructed in combination with a mixed integer programming algorithm; wherein the variables in the cost index function to be optimized include the position coordinates of the safety corridor, the length and width information of the corridor, and the travel time information representing the height of the corridor; and the constraint conditions in the quadratic programming problem to be optimized include the position constraint of the central point of the safety corridor, the size constraint of the safety corridor, and the anti-collision constraint between the safety corridors; Step 5: Based on the quadratic programming problem to be optimized constructed in step 4, the global optimal solution satisfying the constraint conditions is calculated using the optimization solver to construct a safe passage corridor for each driverless vehicle; wherein the global optimal solution includes the position coordinates, geometric parameters and passage time of the safe corridor at each moment; Step 6: Based on the safe passage corridors for each driverless vehicle obtained in step 5, the trajectory generation target in the safe corridor is described as a nonlinear programming problem, and the optimal driving trajectory that meets the constraints is solved by the solver; the optimal driving trajectory is located inside the safe corridor and meets the vehicle dynamics model and other constraints; Step 7: According to the length of the driverless vehicle safety corridor constructed in step 5 and the driverless vehicle driving trajectory planned in step 6, determine whether the length of the driving trajectory in the safety corridor meets the length of the safety corridor. If the planned driving trajectory meets the length of the safety corridor, return to step 1 to start the next safety corridor planning; if the planned driving trajectory does not meet the length of the safety corridor, return to step 6 to continue execution; In step 6, the trajectory generation target in the safety corridor is described as a nonlinear programming problem including cost function and constraint conditions. The optimal driving trajectory that meets the constraint conditions is solved by the solver. The specific process is as follows: Based on the safe passage corridor constructed in step 5, a cost function including trajectory smoothness cost, lateral and longitudinal comfort cost, and spatiotemporal deviation cost from the corridor hub is constructed, which is specifically expressed as: Trajectory smoothness cost: ,in represents the weight coefficient of trajectory smoothness cost in the optimization problem, Indicates vehicle At the moment The curvature of the trajectory at Horizontal and vertical comfort cost: ,in represents the weight coefficient of the control signal, Indicates vehicle At the moment The rate of change of the front wheel steering angle at Indicates vehicle At the moment The acceleration along the vehicle's forward direction; The cost of time and space deviation from the corridor hub: ,in Represents the error weight coefficient in different directions, Indicates vehicle The center of mass at time The location coordinates of the place; in, , represents the running time value of the unmanned vehicle trajectory ; Based on the above cost function, the nonlinear programming problem is expressed as follows: The constraints include: Unmanned vehicle dynamics constraints: in They correspond to the coordinates of the center of mass of the vehicle body and the angle between the vehicle body and the reference coordinate system, Represents vehicles The turning angle of the front wheels, the speed in the direction of the vehicle body, and the acceleration in the direction of the vehicle body; Self-driving cars Control input constraints: ; Self-driving cars The speed and acceleration in the vehicle body direction need to satisfy the dynamic constraints: Self-driving cars Position and all vehicles need to be within the safety corridor constraints: in, , Represents vehicles At the moment The coordinate values ​​corresponding to the four corner points of the vehicle body.

2. The multi-vehicle collaborative trajectory planning optimization method based on a safety corridor according to claim 1 is characterized in that: The specific process of step 1 is as follows: In the inertial coordinate system, for the driverless car group , for driverless cars Create a reference path point set ,in, , Represents the position coordinates and orientation angle of the reference path point. , .

3. The multi-vehicle collaborative trajectory planning optimization method based on a safety corridor according to claim 2 is characterized in that: The specific process of step 2 is as follows: Near the reference path points described in step 1, a safety corridor cube is established for each driverless car. Since the corridor geometry is a rectangular cube, , which is in Reference waypoints The nearby corridor cubes are located by the coordinates of the corridor hub and corridor geometry So the corridor cube is represented as follows: The safety corridor along Direction and Along The maximum and minimum boundary positions of the direction are expressed as: In the formula, Indicates The location of the corridor hub. Indicates The size of the corridor, Indicates The geometric information of the corridors, Indicates The horizontal coordinate of the central point of the corridor, Indicates The vertical coordinate of the central point of the corridor, Indicates Corridor The length of the minimum boundary from the center point. Indicates Corridor The length of the maximum boundary of the direction from the central point, Indicates Corridor The length of the maximum boundary of the direction from the central point, Indicates Corridor lower boundary The length of the minimum boundary from the center point. Indicates The height of the corridor, Indicates Corridor The position of the minimum boundary of the direction, Indicates Corridor The position of the maximum boundary of the direction, Indicates Corridor The position of the minimum boundary of the direction, Indicates Corridor The location of the maximum boundary of the direction.

4. The multi-vehicle collaborative trajectory planning optimization method based on a safety corridor according to claim 3 is characterized in that: The specific process of step 3 is as follows: Corridor cubes built for each driverless car , construct the cost indicator function to be optimized, including the path point tracking deviation cost , Corridor space occupation cost , Corridor travel time cost , specifically expressed as follows: in They respectively represent the weight coefficients of the corresponding cost functions in the indicators to be optimized.

5. The multi-vehicle collaborative trajectory planning optimization method based on a safety corridor according to claim 4 is characterized in that: The specific process of step 4 is as follows: Based on the cost index function to be optimized constructed in step 3, the constraint-based quadratic programming problem to be optimized is constructed in combination with the mixed integer programming algorithm, which is expressed as follows: in, Represents driverless car The traffic priority.

6. The multi-vehicle collaborative trajectory planning optimization method based on a safety corridor according to claim 5 is characterized in that: The position constraint of the safety corridor hub in step 4 is described as the distance between the corridor hubs does not exceed the maximum driving distance. The specific expression is as follows: in, The maximum forward speed of the vehicle. Indicates vehicle exist The reference heading angle at the moment.

7. The multi-vehicle collaborative trajectory planning optimization method based on a safety corridor according to claim 6 is characterized in that: The size constraints of the safety corridor in step 4 include the size constraints of the corridor itself and the size constraints of the overlapping areas of adjacent corridors. The specific expressions are as follows: The size constraint of the corridor itself at any time is: In the formula, For vehicles Direction and Direction body length; The size of the corridor at any time must be within the range of the vehicle's movement capacity at the previous moment: in: In the formula, represents the maximum speed of the vehicle moving backwards, Indicates that driverless cars Departure time Time along The minimum distance of movement in a direction, Indicates that driverless cars Departure time Time along The maximum distance of movement in the direction, Indicates that driverless cars Departure time Time along The minimum distance of movement in a direction, Indicates that driverless cars Departure time Time along Maximum distance of directional movement; The size constraint of the overlapping area of ​​adjacent corridors is: 。 8. The multi-vehicle collaborative trajectory planning optimization method based on a safety corridor according to claim 7 is characterized in that: The anti-collision constraints between safety corridors in step 4 are described as different vehicles and The safety corridor cubes will not overlap at the same time. The specific expression is as follows: in, , is the safe driving distance between vehicles, which is a constant. is a binary variable used to represent the driverless vehicle The corresponding Corridor cubes and driverless vehicles The corresponding Whether the corridor cubes overlap, if so, the value is 1, if not, the value is 0; is a binary variable used to represent the driverless vehicle No. The time value corresponding to the time period is the same as that of the unmanned vehicle No. Whether the time values ​​corresponding to the time periods overlap, if so, the value is 0, if not, the value is 1. .

9. The multi-vehicle collaborative trajectory planning optimization method based on a safety corridor according to claim 8 is characterized by: Step 5: Call the Gurobi optimization solver to find the variable that satisfies the constraints in step 4 and minimizes the cost function. The value of of A safe passage corridor formed by connecting corridor cubes.

Images