Multi-vehicle cooperative trajectory planning method in closed environment

Abstract

The application relates to a closed environment multi-vehicle cooperative trajectory planning method, and belongs to the technical field of automatic driving, in particular to a scheme for centralized trajectory planning of multiple unmanned vehicles. The method solves the problem of low transportation efficiency of multiple vehicles in a closed environment in the intelligent logistics industry. Specifically, the trajectory planning is divided into multiple stages, the first stage is rough path generation, and the second stage is smooth dynamic feasible trajectory generation based on the solving result of the first stage. The method can solve conflict-free trajectories for large-scale vehicle centralized solving and has a relatively short solving time.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of automatic driving, and particularly relates to a multi-vehicle cooperative trajectory planning method in a closed environment. BACKGROUND

[0002] A new round of transformation and upgrading of industrial manufacturing puts forward new performance requirements for unmanned vehicles. The breakthrough of multi-vehicle cooperative planning technology can help solve the efficiency bottleneck of the intelligent logistics industry, reduce the labor intensity of workers, and has important practical significance for improving the production efficiency of the industry, shortening the logistics cycle and improving economic benefits.

[0003] Researching the multi-vehicle cooperative trajectory planning technology of unmanned vehicles helps to solve the problem of low transportation efficiency in a closed environment of the intelligent logistics industry.

[0004] For the optimal trajectory planning problem of multiple nonholonomic constraint unmanned vehicles in a closed transportation environment, the centralized method has the problem of only being applicable to small-scale planning, and under the premise of not having an initial solution as a guide, even the cooperative motion problem of three or four vehicles will often encounter the phenomenon of slow convergence or even convergence failure. The decoupled method has the shortcomings of fast calculation speed and insufficient completeness.

[0005] The application provides a multi-vehicle cooperative trajectory planning method in a closed environment, which adopts two-stage planning and step-by-step implementation. The first stage is rough path generation, and the second stage is smooth dynamic feasible trajectory generation based on the solution results of the first stage. This method can solve large-scale vehicle centralized conflict-free trajectories with shorter solution time. SUMMARY

[0006] The application embodiment provides a multi-vehicle cooperative trajectory planning method in a closed environment, which can enable multiple vehicles to run simultaneously in the same place and reach their respective destinations without conflict, and has higher solving efficiency and a path close to the shortest overall trip.

[0007] The technical solution of the application embodiment is as follows:

[0008] In the case of known environment information of a region where vehicles to be planned can run and specific size of the vehicles, the environment information is abstracted into a two-dimensional map for vehicle driving planning, and the vehicle footprint size is converted into a size projected in proportion to the two-dimensional map;

[0009] In the case of known starting point and ending point of each vehicle, the planning is divided into two stages. First, single-vehicle path search based on a space-time A* graph search algorithm is performed, and finally the trajectory result of each vehicle under uniform speed condition is obtained;

[0010] Then, the positions of the individual vehicle trajectory results that conflict with each other in the same two-dimensional plane and the same time period are subjected to conflict coordination, the positions that conflict with each other are subjected to position constraint in the time period, and then path graph search is performed, and finally, the overall multi-vehicle non-conflict trajectory result is generated.

[0011] Then, the result of the non-conflict graph search is subjected to trajectory smoothing based on an optimization method, so that the driving trajectory is dynamically feasible and the speed change is small.

[0012] Finally, the trajectory result can be directly used as the result of the multi-vehicle cooperative operation, and a speed instruction or signal can be sent to the control end for multi-vehicle direct operation.

[0013] In the example embodiments of the present application, the known environment information of the vehicle operation area to be planned and the specific size of the vehicle: the information obtained through vehicle-vehicle communication, vehicle-road communication and various sensors can be used to express the actual environment of the two-dimensional plane after processing, as the basis for subsequent path planning. The size of the vehicle body includes the size of the vehicle in the two-dimensional plane after the known two-dimensional plane environment information is planned. The specific form can be a two-dimensional grid map, a point cloud map, etc.

[0014] In the example embodiments of the present application, the path search based on the graph search algorithm: first, a kinematic model of the vehicle body is constructed, and then the two-dimensional plane is subjected to appropriate gridding, and the size of the grid is selected according to the kinematic characteristics of the vehicle, and is not fixed. In the sampling stage of the search on the grid map, the expandable sampling range will consider the specific feasible positions of the vehicle kinematics, rather than directly selecting the range of geometric adjacent positions for direct sampling. The step of sampling expansion can be adjusted according to the environment information to obtain the best suboptimal path result.

[0015] In the example embodiments of the present application, the construction of the kinematic model of the vehicle body includes: using L f and L b respectively represent the distance from the rear axle to the front and rear of the vehicle body. W r represents the width of the vehicle body. The state of the vehicle body is represented as (x0, y0, θ0), and the rectangular shape of the vehicle body in the Cartesian coordinate system can be defined as:

[0016]

[0017]

[0018] wherein

[0019]

[0020] In the exemplary embodiments of the present application, the conflict coordination is performed as follows: after each vehicle performs the graph search to generate the path result, a uniform speed is assigned to all single-vehicle paths, assuming that each vehicle moves at a constant speed and that all vehicles start at the same initial time, and the trajectory result of each vehicle is traversed in the same two-dimensional planning plane, and there may be a position where the trajectories of the vehicles conflict. For the conflict position, a method of limiting the presence of a vehicle at the planning position at a certain time is adopted, and the path of the vehicle is re-planned after the limitation, and the search method is the same as the initial single-vehicle graph search method, while considering that the path cannot pass through the limited position when executing at the constant speed, and the other vehicle without limitation does not need to be re-planned. Since the optimal selection object of the limitation is unknown, two allocation situations of the limitation, limitation A or limitation B, will be generated. Both situations are executed, and the process is similar to a multi-branch tree structure, and each branch point is a position discussed in different situations. Here, the limitation A or B only lists the same position at the same time, and the two vehicles will add constraints after the conflict. If more vehicles conflict at the same position at the same time, since the position has been set as a time-space limitation by A or B, the position at the same time in the subsequent time period will also become a path constraint, and the segmented path of other vehicles containing the position at the same time also needs to be re-executed graph search until all vehicles do not conflict in the time-space trajectory.

[0021] In the exemplary embodiments of the present application, the overall multi-vehicle conflict-free trajectory result is generated as follows: after the conflict coordination is performed, a plurality of search results without collision are generated, and the path meeting the optimal criteria is selected as the result of the first-stage solution according to the evaluation criteria.

[0022] In the exemplary embodiments of the present application, the trajectory smoothing based on the optimization method is performed as follows: after the known feasible path result, the result is used as the initial value of the centralized multi-vehicle collaborative trajectory method based on the optimization method or directly as the key optimization variable of the constructed problem. Meanwhile, the running speed, the distance between vehicles, the path smoothness, the degree of obstacle avoidance, the time to the terminal position, etc. are used as variables to construct a convex optimization problem, and a nonlinear optimization method is used to solve the problem.

[0023] The result is used as the key optimization variable of the construction problem of the centralized multi-vehicle collaborative trajectory method based on the optimization method, including: each path segment in the original path is divided into h equal parts, where the time of each path is calculated based on the uniform speed assigned to each path during the conflict coordination, and the time is denoted as ΔT, so the time interval of each small segment is Δt = ΔT / h. The target equation to be minimized is the smoothness of the trajectory, the deviation of the actual h-small-path from the original path, and the running time, and the optimization problem is constructed in the form of quadratic programming. The scope of the limitation conditions is respectively the start point and the end point constraint, the system state transition, the collision constraint, the input quantity speed constraint, and the distance constraint between robots.

[0024] In the exemplary embodiments of the present application, the convex optimization problem is constructed, and the continuity of the variables such as the distance between the vehicle and the contactable object in the environment, the distance between the vehicles, etc. is converted into a convex feasible set. The optimization problem can be in the form of nonlinear least squares, quadratic programming, etc., and is constructed as a nonlinear optimization problem.

[0025] In the exemplary embodiments of the present application, the continuity of the variables is converted into a convex feasible set: the area occupied by the vehicle body is abstracted as a regular graph with continuous edges and closed, and the planning range of the vehicle body is a plurality of mutually intersecting and connected rectangular frame bodies extended from the path result generated by the graph search. In the same period, the frame bodies of each vehicle path result do not intersect, so the planning result of the vehicle body will not collide. The frame bodies do not intersect with the obstacles in the two-dimensional planning environment.

[0026] From the above description of the method, compared with the prior art, the present application has the following advantages:

[0027] (1) Two-stage multi-vehicle path planning is adopted, the graph search method is combined with the optimization-based method, and the quality and solving efficiency of the solution of multiple target planning can be considered.

[0028] (2) The neighborhood expansion range and step length of the graph search consider the kinematics of the vehicle body and environmental factors, which is beneficial to search for a suitable path in a dense obstacle area.

[0029] (3) In the construction of the multi-vehicle trajectory optimization problem, the form of the vehicle body and the collision constraint is converted into a linear form that is beneficial to solving. BRIEF DESCRIPTION OF DRAWINGS

[0030] The present application will be further described below in conjunction with the drawings and embodiments.

[0031] Figure 1 is a schematic diagram of a closed environment multi-vehicle cooperative trajectory planning method implementation process provided by the embodiments of the present application;

[0032] Figure 2 is a schematic diagram of a graph search neighborhood provided by the embodiments of the present application;

[0033] Figure 3 is a schematic diagram of a collision conflict decision multi-way tree provided by the embodiments of the present application;

[0034] Figure 4 is a schematic diagram of a path line segment converted into a rectangular shape provided by the embodiments of the present application; DETAILED DESCRIPTION

[0035] In order to make the technical means, creative features, purposes and effects of the present application easy to understand, the present application is further described below in combination with specific drawings. The specific embodiment of the present application is described below with reference to the drawings. In order to fully understand the present application, many details are described below, but the present application can be implemented without these details for those skilled in the art.

[0036] The embodiment of the present application provides a multi-vehicle cooperative trajectory planning method in a closed environment, which can enable multiple vehicles to simultaneously run in the same venue and conflict-free reach respective destinations, and has higher solving efficiency and a result close to the shortest path of the overall trip.

[0037] The technical scheme of the embodiment of the present application is implemented as follows, as shown in Figure 1 The method can include steps S101-S105:

[0038] S101, in the case of known environment information of a region available for a vehicle to be planned to run and specific size of the vehicle, abstracting the environment information into a two-dimensional map for vehicle driving planning, and converting the size of the vehicle footprint into a size projected to the two-dimensional map in a same proportion;

[0039] S102, in the case of known starting point and ending point of each vehicle, planning in two stages, first, single-vehicle path search based on a space-time A* graph search algorithm, and finally obtaining a trajectory result of each vehicle under uniform speed;

[0040] S103, for positions where trajectory results of each vehicle conflict in the same two-dimensional plane and in the same time period, conflict coordination is performed, the positions where conflicts occur are established as position constraints in the time period, and then path graph search is performed, and finally an overall multi-vehicle conflict-free trajectory result is generated;

[0041] S104, trajectory smoothing based on an optimization method is performed on the result of conflict-free graph search, so that the driving trajectory is dynamically feasible and the speed change is small;

[0042] S105 and S106, simplifying and solving the problem, and finally the trajectory result can be directly used as a result of multi-vehicle cooperative running, and speed instructions or signals can be sent to a control end for direct running of the multi-vehicle.

[0043] In the exemplary embodiment of the present application, the acquisition of the environment information of the region available for the vehicle to be planned to run and the specific size of the vehicle includes information acquired through vehicle-vehicle communication, vehicle-road communication and various sensors, which is processed and can be used to express the actual situation of the two-dimensional plane of the region, as a basis for subsequent path planning. The size of the vehicle body includes the size of the vehicle in the two-dimensional plane after the two-dimensional plane environment information is known.

[0044] In the exemplary embodiments of the present application, the path search based on the graph search algorithm comprises: firstly constructing a kinematic model of the vehicle body, and then performing suitable rasterization on the two-dimensional plane, the size of the raster being selected according to the kinematic characteristics of the vehicle and not being fixed.

[0045] In the exemplary embodiments of the present application, the construction of the kinematic model of the vehicle body comprises: using L f and L b respectively represent the distance from the rear axle to the front of the robot vehicle body and the distance from the rear axle to the rear of the robot vehicle body. W r represents the width of the robot vehicle body. For the state of the robot vehicle body represented as (x0, y0, θ0), the rectangular shape of the robot vehicle body in the Cartesian coordinate system can be defined as:

[0046]

[0047]

[0048] wherein

[0049]

[0050] Here, the path planning is performed for each vehicle one by one using the space-time A* graph search algorithm, because the starting position and the ending position of each vehicle are different. In the planning stage, the kinematic characteristics of the vehicle body are considered, and here the common four-wheel transport vehicle is taken as an example to illustrate how to determine the kinematic model and combine the kinematic model with the search sampling range.

[0051] In implementation, the vehicle is usually a four-wheel vehicle, and the two parallel front wheels assist steering. Here, it is assumed that in the initial search stage of the two-dimensional plane, the vehicle is located at the center E point position of the grid frame as shown in Figure 2 Without combining the kinematic constraints of the vehicle body, the graph search can sample in the directions of B, D, F, H, etc. Here, combined with the front wheel restriction, the vehicle can only reach the forward C, F, I positions or the rearward A, D, G positions in one sampling, and it is impossible to laterally move to the B, H positions. Such search is more in line with the actual running situation of the vehicle body, and lays a foundation for the subsequent trajectory feasibility and optimization.

[0052] In implementation, the size of the raster network of the search is combined with the kinematic characteristics, and here a method is proposed which mainly combines the online speed and the steering angle. Let D represent the graph grid size, and ΔT represents the time required for the robot to cross a side. Due to the physical limitations of each vehicle turning, in order to ensure that the diagonal line in the figure can be crossed in one step, the size of the grid is also related to the control speed of the vehicle body, and the following condition must be met

[0053]

[0054] D and ΔT in the formula are both pre-defined and can be used to inversely calculate the maximum speed permitted and the combined angular velocity after the front wheel turns.

[0055] Here, the grid size sampled in the graph search stage will directly affect the optimality of the final path result. In order to make the result as close to suboptimal as possible, the expansion step size is reduced in relatively narrow environments relative to the size of the vehicle body, and the expansion step size is lengthened in relatively wide scenarios. This ensures faster search speed in relatively open scenarios.

[0056] In the exemplary embodiments of the present application, the conflict coordination includes: after each vehicle executes the graph search to generate a path result, a uniform speed value is assigned to all single-vehicle paths. It is assumed that each vehicle moves at a certain uniform speed, and all vehicles execute the task at the same initial time. The trajectory results of each vehicle are traversed in the same two-dimensional planning plane, and there may be trajectory conflicts at some positions. For the processing method of the conflict position, a certain vehicle is limited to appear at the planning position during a certain period of time, and the path of the vehicle is re-planned after the limitation. The path search method is the same as the single-vehicle graph search method, and it is also necessary to consider that the path cannot pass through the limited position when it is executed at the uniform speed. Another vehicle that is not limited does not need to be re-planned. Since the optimal selection object of the limitation is unknown, two allocation situations of the limitation, limitation A or limitation B, will be generated. Both situations will be executed, and this process is similar to a multi-way tree structure, and each branch point is a position discussed in different situations.

[0057] In implementation, whether each uniform speed vehicle collides can be determined in multiple ways. Here, two implementation methods are listed.

[0058] First, the minimum collision radius between the vehicle bodies is defined. If it is assumed that the projection of the vehicle body on the two-dimensional plane occupies a circular outline shape, the minimum distance of collision between two vehicle bodies is the sum of the radii of the two vehicles. After the collision radius is determined, the grid size can be divided into smaller radii. The grid occupied by the trajectory of the vehicle running at a uniform speed is traversed, and it is detected which grid collides with which vehicle at what time. The area around the collision area can be determined as an obstacle area.

[0059] Second, the distance between the center point of each trajectory segment and the center of the other vehicle body can be directly calculated. If the distance is less than the safety distance, the time period and the area in which a collision is likely to occur are determined as an obstacle area.

[0060] Here, for the case-by-case discussion of restricting car A or restricting car B, the multi-branch tree approach can be as follows: Figure 3 As shown, the root node contains the paths ACE and BCD traversed by the two vehicles. However, during the time period corresponding to the second position, both vehicles have a planned position C, leading to a conflict. Position constraints are imposed on both vehicles at the conflict point for that time period. The left subtree indicates that vehicle 1, after being constrained, will wait at position A for a unit of time. The right subtree indicates that vehicle 2, after being constrained, will wait at position B. The overall cost is recorded for each vehicle. Constraints A or B here only list vehicles at the same position during the same time period; constraints are added after vehicles A and B conflict. If more vehicles conflict at the same position during the same time period, because that position has already been set as a spatiotemporal constraint by A or B, that position will become a path constraint for subsequent vehicles during that time period. If other vehicles' segmented paths contain that position during the same time period, the graph search needs to be re-executed until all vehicles are free of conflict in their spatiotemporal trajectories.

[0061] The result of the wait here is not mandatory. It's simply a matter of adding constraints to the time period and location of the conflict, then performing another graph search, and the result of the wait is the optimal choice under that condition. In other scenarios, the search might reveal a less costly alternative.

[0062] In an exemplary embodiment of this application, generating the overall multi-vehicle collision-free trajectory result includes: after performing the conflict coordination, multiple collision-free search results will be generated, and the path that meets the optimal criteria will be selected according to the evaluation criteria as the result of the first stage solution.

[0063] In an exemplary embodiment of this application, the trajectory smoothing based on the optimization method includes: after obtaining the known feasible path results, using the results as initial values ​​for a centralized multi-vehicle cooperative trajectory method based on the optimization method, or directly using them as key optimization variables to construct the problem. Simultaneously, factors such as running speed, distance between vehicles, path smoothness, degree of obstacle avoidance, and time to the destination can be used as variables to construct a convex optimization problem, which is then solved using a nonlinear optimization method.

[0064] In implementation, the optimal criterion path can have various criteria, including but not limited to the sum of the lengths of all vehicle routes, the total amount of goods transported by all vehicles, the overall handling efficiency, the average energy consumption per vehicle, and the number of coordination conflicts. The specific criterion can be selected based on the desired performance indicators during the actual implementation process.

[0065] In the implementation, the results of the coordinated graph search path are used as the initial value of the multi-vehicle cooperative trajectory method based on the optimization method or directly as the key optimization variables considered. First, a suitable optimization problem needs to be constructed. The construction of the optimization problem can have multiple forms, and here an implementation is adopted. Each path segment in the original path is divided into h equal parts, and the time of each segment is calculated using the uniform speed velocity assigned to each path segment during the collision conflict judgment, denoted as ΔT, so Δt = ΔT / h.

[0066]

[0067]

[0068]

[0069]

[0070]

[0071]

[0072] Here H is the H segment division of the original path, N is the trajectory of all N vehicles, denotes the change value of the vehicle body speed of the front and rear segment divided path, denotes the difference between the current running trajectory and the reference path provided in the graph search stage. k is the weight coefficient of time τ. P and Q are two semi-positive definite weighting matrices. Therefore, the final minimized target equation is: the smoothness of the trajectory, the degree of deviation from the original path, and the running time. The constraint conditions b to f are respectively, b. Start and end point constraints, where and denote the position constraints that the vehicle body must reach at the start and end of the trajectory, c. System state transition, which is in the form of the state transition equation described in the formula, d. Collision constraint, where denotes the free space available for the vehicle body to pass in the road running space, e. Input quantity speed constraint, where denotes the range of the vehicle body running speed u to be limited, f. Distance constraint between robots, where pos(z) denotes the position of the vehicle body trajectory, and R denotes the radius of the circumscribed circle of the vehicle body projected onto the map.

[0073] The final solution result is the feasible trajectory of N vehicles, which is actually the directly executable speed value.

[0074] In the exemplary embodiments of the present application, the constructing convex optimization problem, the constructing process comprises: processing the continuity of variables such as the distance between the vehicle and the contactable object in the environment, the distance between the vehicle and the vehicle into a convex feasible set. The optimization problem can adopt the form of nonlinear least square, quadratic programming, etc., and is constructed into a nonlinear optimization problem.

[0075] In the exemplary embodiments of the present application, the processing of the continuity of the variables into a convex feasible set comprises: taking a two-dimensional planning plane as a base plane, abstracting the area occupied by the vehicle body into a regular graph with edge continuity and closure, and the planning range of the vehicle body is a plurality of mutually intersecting and connected rectangular frame bodies extended by the path result generated by the graph search. In the same period, the frame bodies of each vehicle path result do not intersect, so the planning result of the vehicle body does not collide. The frame bodies do not intersect with the obstacles in the two-dimensional planning environment.

[0076] In implementation, the area occupied by the vehicle body is abstracted and constructed into a regular graph, which can be a rectangle or a circle, etc., to meet the condition of being converted into a convex function or a linear constraint function form.

[0077] In implementation, the rectangular frame body can have various construction forms, which can be as shown in the figure. Figure 4 The driving direction of the vehicle body of the path result (black solid line) planned by the graph search is constructed as the x-axis of the Cartesian coordinate system, and the rectangle will be extended along the x-axis and the y-axis on the rasterized two-dimensional plane until the four sides are close to the contactable obstacles, and then the traversal expansion is stopped. The final safe planning area of the vehicle in this trajectory segment is shown by the dashed line frame. The constraint of whether to collide with the static environment in space is converted into a linear constraint in the two-dimensional plane, and the variable is beneficial to the construction of the convex optimization constraint condition and the calculation.

[0078] The advantage of the execution method is that two-level multi-vehicle path planning is adopted, the graph search method is combined with the method based on optimization, the quality and solving efficiency of the solution of multiple target planning can be considered. The neighborhood expansion range and step length of the graph search consider the kinematics of the vehicle body and environmental factors, which is beneficial to searching for a suitable path in the dense obstacle area. In the construction of the multi-vehicle trajectory optimization problem, the form of the vehicle body and the collision constraint is converted into a linear form beneficial to solving.

Claims

1. A method for multi-vehicle cooperative trajectory planning in a closed environment, characterized in that, The method comprises: Step one, in the case of known environment information of the area where the vehicle to be planned can run and the specific size of the vehicle, abstract the environment information into a two-dimensional map for vehicle driving planning, and convert the size of the vehicle footprint into a size projected in proportion to the two-dimensional map; Step two, in the case of known starting point and ending point of the task of each vehicle, plan in two stages, first separately perform path search based on a graph search algorithm for each vehicle, and finally obtain the trajectory result of each vehicle under uniform speed condition; Step three, for the positions where the trajectory results of the vehicles conflict in the same two-dimensional plane and within the same time period, perform conflict coordination, establish position constraints for the positions where conflicts occur within the time period, and then perform the path search of step two, and finally generate the overall multi-vehicle conflict-free trajectory result; Step four, perform trajectory smoothing based on an optimization method for the conflict-free trajectory result; Step five, directly send the trajectory result obtained as the result of multi-vehicle cooperative operation to the control end to control the multi-vehicle to directly run; The path search based on the graph search algorithm of step two comprises: first, constructing a kinematic model of the vehicle body, then, rasterizing the two-dimensional plane, the size of the grid is selected according to the kinematic characteristics of the vehicle, and is not a fixed size, and the search sampling stage on the grid map is also included, the expandable sampling range will consider the specific feasible positions of the vehicle kinematics, rather than directly selecting the range of geometric adjacent positions for direct sampling, and the step length of the sampling expansion can be adjusted according to the environment information to obtain the best suboptimal path result; The conflict coordination of step three comprises: for each vehicle, limiting its presence at the conflict position within the time period, and re-planning the path of the vehicle after the limitation by using the path search based on the graph search algorithm, without re-planning the paths of the remaining vehicles; The trajectory smoothing based on the optimization method in step four comprises: after knowing the feasible path result, taking the result as the initial value of the centralized multi-vehicle cooperative trajectory method based on the optimization method or directly as the key optimization variable of constructing the problem, simultaneously taking the running speed, the distance between the vehicle and the contactable object in the environment, the distance between the vehicles, the path smoothness, the degree of avoiding obstacles, and the time to the ending position as variables, performing continuous processing on the variables to convert them into a convex feasible set, adopting a nonlinear least square or quadratic programming form for the optimization problem, and solving by using a nonlinear optimization method; The continuous processing on the variables to convert them into a convex feasible set comprises: taking the two-dimensional planning plane as a base plane, abstracting the area occupied by the vehicle body into a regular figure with a continuous closed edge, and the range of the vehicle body planning is a plurality of mutually intersecting and connected rectangular frame bodies extended from the path result; within the same time period, the frame bodies of the path results of each vehicle do not intersect, so the planning result of the vehicle body will not collide; the frame bodies do not intersect with the obstacles in the two-dimensional planning environment.

2. The multi-vehicle cooperative trajectory planning method in a closed environment according to claim 1, characterized in that, The environment information of the area where the vehicle to be planned can run of step one comprises information obtained through vehicle-vehicle communication, vehicle-road communication, and various sensors, and the size of the vehicle body is the size of the vehicle in the two-dimensional plane after the two-dimensional plane environment information is known.

3. The multi-vehicle cooperative trajectory planning method in a closed environment according to claim 1, characterized in that, The kinematic model of the vehicle body includes: using and respectively represent the distance from the rear axle to the front and rear of the vehicle body, represent the width of the vehicle body; for the state of the vehicle body represented as ( , , ), the rectangular shape of the vehicle body in the Cartesian coordinate system is defined as: ， Wherein 。 4. The multi-vehicle cooperative trajectory planning method in a closed environment according to claim 1, characterized in that, After the conflict coordination is performed, a plurality of search results without collision with each other are generated, and a path meeting the optimal criterion is selected according to the evaluation criterion, that is, an overall multi-vehicle non-collision trajectory result is obtained as the result of the first stage solution.

5. The multi-vehicle cooperative trajectory planning method in a closed environment according to claim 1, characterized in that, The results are included as key optimization variables for the construction of a centralized multi-vehicle cooperative trajectory method based on optimization methods: each path segment in the original path is divided into equal parts, with a time interval of , For the execution of conflict coordination, the time calculated by the uniform speed given to each path segment; so the target equation to be minimized is the smoothness of the trajectory, the deviation of the actual equal-part path from the original path, and the running time. The form of the optimization problem is quadratic programming, and the scope of the constraints is as follows: start and end point constraints, system state transition constraints, collision constraints, input quantity speed constraints, and robot distance constraints.

Images