Multi-vehicle cooperative path planning method in unstructured closed environment

Abstract

The application discloses a multi-vehicle cooperative path planning method in an unstructured closed environment. First, the position of the current multi-vehicle in the planning map, the start and end points of the planning task and the current vehicle motion state are determined. Then, the path of each single vehicle is planned, the executable single vehicle path planning is determined according to the current environment information, and the single vehicle executable path result is generated. The multi-vehicle path conflict position is determined, the spatiotemporal position where the multi-vehicles may conflict in the same space-time is determined according to the single vehicle path result. In the single vehicle path result, constraint information is added to the multi-vehicle path conflict position based on the vehicle body position. The multi-vehicle search task is grouped, and the multi-vehicles are grouped differently based on the path information and the collision information. Finally, the multi-vehicle path search is performed on the same group. Based on the collision information and the constraint information, the multi-vehicle path search is performed on the multi-vehicles in the same group, and the collision-free path result of the multi-vehicle path is obtained.

Description

Technical Field

[0001] This invention belongs to the field of path planning technology, specifically relating to a multi-vehicle cooperative path planning method in an unstructured closed environment. Background Technology

[0002] In industrial and commercial scenarios, path and trajectory planning for individual vehicles has been extensively studied. However, an increasing number of applications now require collaborative planning among multiple vehicles. This presents challenges in areas such as task planning, autonomous localization, path planning, trajectory planning, and motion control, leading to problems of excessively large solution scale or rapidly increasing dimensionality, making modeling and solving difficult. Researching collaborative path planning techniques for multiple unmanned transport vehicles can help address the low transportation efficiency in multi-vehicle, closed environments within the intelligent logistics industry.

[0003] Traditional multi-vehicle path planning methods typically employ graph search planning approaches. To avoid collisions between multiple vehicles, different types of conflicts need to be defined, such as vertex conflicts and edge conflicts, and fixed constraint methods are used to avoid conflicts between planning objectives. However, these algorithms do not consider vehicle models with nonholonomic kinematic constraints.

[0004] Considering nonholonomic kinematic constraints reduces the feasible search area, increases node expansion complexity with the number of plans, and reduces solution efficiency, necessitating more efficient methods to address the problem. Simultaneously, a balance needs to be struck between the quality and time cost of path planning.

[0005] Therefore, nonholonomic kinematics-constrained multi-vehicle path planning methods need to adopt reasonable search acceleration strategies to minimize the expansion of collision avoidance nodes and accelerate the solution of multi-vehicle path search problems at the centralized planning level. Summary of the Invention

[0006] To overcome the shortcomings of existing technologies, enable multiple vehicles to operate simultaneously in the same location and reach their respective destinations without conflict, and achieve higher solution efficiency and planning results that approximate the shortest overall travel path, this invention adopts the following technical solution:

[0007] A method for multi-vehicle cooperative path planning in an unstructured closed environment includes the following steps:

[0008] Step S1: Determine the current positions of multiple vehicles on the planning map, the start and end points of the planning task, and the current movement status of the vehicles;

[0009] By employing multiple sensors to acquire environmental information around the vehicle, and utilizing road network information from a prior map, the motion state information of the vehicle is estimated and predicted to determine the positions of multiple vehicles in the planning map and the starting point, ending point, and travel path of the task to be performed. Using multiple sensor technologies can improve the accuracy and robustness of environmental perception, while combining with a prior map can reduce the impact of sensor noise and environmental interference on the planning results, thereby achieving a more accurate, reliable, and practical planning path design.

[0010] Step S2: Plan the route for each bicycle; Based on the current environment information, determine the executable bicycle route plan and generate the executable bicycle route results;

[0011] Step S3: Determine the locations of multi-vehicle path conflicts; based on the single-vehicle path results, determine the spatiotemporal locations where conflicts may occur between multiple vehicles in the same time and space.

[0012] Step S4: In the single-vehicle path results, add constraint information at the locations of multi-vehicle path conflicts based on the vehicle's position;

[0013] Step S5: Group the multi-vehicle search task; group the multiple vehicles differently based on path information and collision information;

[0014] Step S6: Perform multi-vehicle path search for the same group; for multiple vehicles in the same group, perform multi-vehicle path search based on collision information and the constraint information to obtain collision-free path results for the multi-vehicle paths.

[0015] Furthermore, in step S2, taking single-vehicle path planning as an example, it is usually necessary to consider collisions and obstacle avoidance when the vehicle is driving. Therefore, it is necessary to calculate the space occupied by the vehicle body in different directions and positions. In order to avoid the performance loss caused by calculating the space occupied each time and to improve the efficiency and practicality of path planning, in collision detection, an offline vehicle body position table is used to determine the space occupied by the vehicle body when rotating in different directions based on the shape of the vehicle body and the space occupied by the map projection.

[0016] In step S4, constraints are added at conflict locations based on the offline vehicle position table; for two conflicting vehicles, the offline vehicle position table is searched based on the current path point location to determine the planned map space range occupied by the current vehicle; the spatiotemporal position constraint of this time step is added to the single-vehicle path planning of the other vehicle as a sampling exclusion zone.

[0017] In this way, the path conflict points to be resolved can be added as constraints to the path planning process, ensuring that multiple vehicles can avoid potential conflicts when planning their routes, thereby ensuring that multiple vehicles can move in coordination and maintain safety during the journey.

[0018] Furthermore, the vehicle's rotation center is determined, using the midpoint of the line connecting the rear wheels as the rotation center. The vehicle body is rotated according to a certain discrete angular step size, and the spatial coordinates of each discrete position relative to the planned map are recorded in an offline vehicle position table. Based on the offline vehicle position table, the spatial range occupied by the current vehicle body on the planned map can be quickly calculated by looking up the current vehicle body position and orientation angle. In other words, the space occupied by the vehicle body in different directions and positions is calculated in advance and recorded in the offline vehicle position table. Then, by looking up the current vehicle body position and orientation angle in the offline vehicle position table, the space occupied at the current position can be quickly obtained.

[0019] Furthermore, in step S5, the path information is path feature information based on the existing single-vehicle path results of multiple vehicles; the collision information includes feature information such as the number of path conflicts and conflict location information of each vehicle in the aforementioned method flow; multi-vehicle grouping based on path information and collision information includes: obtaining a comprehensive cost based on path information and collision information, evaluating it through a comprehensive cost function, and after evaluation, grouping based on the total number of vehicles, the number of groups, and the cost of each path. The comprehensive cost includes the vehicle's preset traffic priority, the collision frequency of single-vehicle path results, and the distance coupling degree between the vehicle's own path and the paths of other vehicles.

[0020] Furthermore, in step S5, the multiple vehicles are divided into multiple groups according to the degree of conflict, and multi-machine path planning is performed within each group as follows:

[0021] Based on the single-vehicle path results and the corresponding time step, the degree of conflict between each path is evaluated. The evaluation method can be to calculate the Euclidean distance between paths or to determine the degree of conflict based on the inclusion relationship of path points.

[0022] Vehicles with low conflict levels are clustered together. To ensure the priority of clustering, the division can be based on the length of the single vehicle path. If the distance between the start and end points is far or there are many obstacles in the middle, resulting in a long single vehicle planning path, the vehicle with the longest single vehicle path is determined as the highest priority planning. Then, using this high priority path as a reference, the vehicles with the lowest spatial coupling with it are found and grouped into the same group.

[0023] For subsequent paths, select those with longer path lengths and perform the same clustering operation again until all paths are grouped.

[0024] Furthermore, the multi-vehicle path search in step S6 based on collision information and constraint information includes: after each vehicle sets constraints on the path points with collisions, it will execute step S2 single-vehicle path planning under constraints to generate new single-vehicle path results; then it will execute steps S3 and S4 until all single-vehicle path results have no conflict in any location of any vehicle's path in step S3, then the multi-vehicle path search will stop.

[0025] The multi-vehicle path search in step S6 is based on discretizing the space into a grid, then performing a graph search algorithm based on the grid map and the constraints in the search, and finally finding the path group without collision after the search.

[0026] Furthermore, the path planning method in step S2 first discretizes the two-dimensional plane based on the vehicle kinematics model. The degree of discretization is selected according to the vehicle's kinematic characteristics and is not fixed. The path planning is transformed into sampling multiple path points in the graph. During the sampling process, the vehicle's kinematic characteristics and environmental information are considered to avoid areas where the vehicle cannot pass. The sampling expansion step size is adjusted according to the environmental information to obtain the optimal path result. The sampling point selection includes graph search and / or probabilistic pruning. Different priorities are selected and / or pruned for the sampling nodes to obtain the optimal path result under the set path objective. The search strategy will take into account factors such as the vehicle's kinematic characteristics, road grade, and protruding and concave terrain to select the most suitable path.

[0027] Furthermore, step S2 employs a heuristic graph search. First, the discretized space is constructed into a directed graph structure. Then, based on the heuristic algorithm, an evaluation function is introduced during the search process, that is, the estimated path length from the current node to the destination is added to the node evaluation function to achieve the goal of quickly and accurately searching for the optimal solution. Next, a search algorithm is selected for the search, such as the A* algorithm or Dijkstra's algorithm, to obtain the optimal solution for the vehicle's driving path. During the search process, different evaluation functions, such as weight evaluation functions or time evaluation functions, are used to control, optimize, and / or prune the search process. Finally, the path solution obtained by the search is optimized and smoothed to obtain the final executable path.

[0028] The sampling algorithm in step S2 includes the following steps:

[0029] Step S2.1: Initialize a node as the root node;

[0030] Step S2.2: Generate a random point using a sampling function. The random point takes into account the kinematic feasibility or random direction of the vehicle's motion, and has a certain probability of pointing to the target point.

[0031] Step S2.3: Find the node closest to the random point in the tree, and specify the path according to the corresponding kinematic model to obtain a series of path points with a certain step size;

[0032] Step S2.4: For each path point traversed, perform collision detection to determine if there are any obstacles and if the path is valid;

[0033] Step S2.5: If the path points generated in step S2.4 do not have a valid solution, discard the path and return to step S2.2 to resample; otherwise, select the last valid node as the new node.

[0034] Step S2.6: Connect the new node to the parent node, and use the cost function to evaluate and sort the paths. Then select the generated new node and its adjacent points, and try to reconnect their parent nodes.

[0035] Step S2.7: Further optimize the path using optimization methods and generate new nodes; if the termination condition is met, a feasible path is obtained; otherwise, return to step S2.2 to regenerate random points;

[0036] Step S2.8: Optimize the tree structure using a method with pre-estimated information to obtain a better path.

[0037] In the single-vehicle path planning in step S2, a series of path points are generated, and a curve connection method is used. When the distance to the endpoint is a certain distance, the path point is directly connected to the endpoint. The certain distance is set based on satisfying the smoothness and continuity of the path, thereby avoiding the reduction of efficiency and the impact on the completeness of the final result convergence by connecting a series of path points and then solving step by step.

[0038] Further, step S3 includes the following steps:

[0039] Step S3.1: Single-vehicle path planning in multi-vehicle path planning; During the single-vehicle path planning process, the time information corresponding to each sampling step is calculated, and the time for the vehicle to arrive at each sampling point is determined by linear calculation based on the distance between the current sampling point and the previous sampling point and the maximum driving speed set by the vehicle; In this way, detailed time information can be obtained for the path planning results of each vehicle.

[0040] Step S3.2: Determine the location of spatiotemporal conflict; on the same two-dimensional planning plane, traverse the path planning results of each vehicle and check whether there will be path conflicts; for path points that may conflict, record the time step and which two vehicles will collide; these records can be used for subsequent path optimization and obstacle avoidance planning to ensure that multiple vehicles do not conflict during driving.

[0041] Furthermore, in step S3.1, the single-vehicle path planning, sampling, search, and connection curves are all discrete step-size results. Combined with the time under uniform motion, the step size is given a temporal meaning during the search or sampling process, which is used to add spatiotemporal constraints in multi-vehicle search. In single-vehicle path planning, sampling points within the spatiotemporal constraint range can be avoided or removed.

[0042] In step S3.2, the calculation of conflicts is divided into two categories:

[0043] Simultaneous position overlap: If the positions of vehicle waypoints overlap on the two-dimensional planning plane at the same time, it indicates a possible path conflict. To determine whether position overlap has occurred, first find the positions of all vehicles on the planning plane at each time, and then compare whether their position coordinates overlap. If there is an overlap, it indicates that there may be a path conflict point, and corresponding path optimization planning is required.

[0044] Proximity at the next moment: At the current moment, although the locations corresponding to the vehicle's waypoints do not overlap, the vehicle will move to a nearby location at the next moment, which may cause a path conflict. In order to determine whether a conflict will occur in the next step, the location corresponding to the vehicle's waypoint at the next moment can be calculated by predicting the vehicle's trajectory, and it can be checked whether the location overlaps with the location corresponding to the waypoints of other vehicles. If there is an overlap, it means that there is a possible path conflict point, and corresponding path optimization planning is required.

[0045] The advantages and beneficial effects of this invention are as follows:

[0046] This invention addresses the problem that collision detection and multi-machine obstacle avoidance constraints are not applicable to vehicles with nonholonomic kinematic constraints in multi-agent path planning methods, thus improving the applicability and accuracy of the algorithm. It can perform reasonable vehicle grouping when the fleet density is high, significantly improving the success rate, and can reduce the search time compared to traditional methods. The heuristic multi-vehicle path planning based on single-vehicle path feature information and collision frequency information improves the success rate and solution efficiency of the planning. Attached Figure Description

[0047] Figure 1 This is a flowchart of the method provided in the embodiments of this application.

[0048] Figure 2 This is a schematic diagram of a graph search neighborhood provided in an embodiment of this application.

[0049] Figure 3 This is a schematic diagram of a collision conflict decision multi-branch tree provided in an embodiment of this application.

[0050] Figure 4 This application provides a schematic diagram of a path segment being transformed into a rectangular shape. Detailed Implementation

[0051] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are for illustration and explanation only and are not intended to limit the present invention.

[0052] like Figure 1 As shown, a multi-vehicle cooperative path planning method in an unstructured closed environment modifies the definition of conflicts between multiple paths for multiple vehicles after solving the path result for a single vehicle considering nonholonomic kinematic constraints, in order to perform multi-vehicle conflict detection and add obstacle avoidance constraints. Several acceleration methods are employed to improve search efficiency, such as clustering and heuristic search, thereby shortening the search time and improving the efficiency of path planning. This method can comprehensively utilize various information to quickly and efficiently plan collision-free multi-vehicle paths, and it can also cope with different environmental changes and path conflict problems in practical applications. Specifically, it includes the following steps:

[0053] Step S1: By combining information from vehicle sensors and prior maps, determine the current positions of multiple vehicles on the planning map, the start and end points of the planning task, and the current vehicle motion status.

[0054] In this embodiment of the invention, information from vehicle sensors and prior maps is comprehensively considered. By using multiple sensor technologies, such as GPS, LiDAR, and environmental cameras, environmental information surrounding the vehicles is collected and processed. Simultaneously, road network information from the prior map is used to estimate and predict the vehicle's position, orientation, speed, and acceleration, ultimately determining the positions of multiple vehicles on the planning map and the starting and ending points and travel paths for the required tasks. This method improves the accuracy and robustness of environmental perception by employing multiple sensor technologies, while combining prior maps reduces the impact of sensor noise and environmental interference on the planning results, thereby achieving more accurate, reliable, and practical path design.

[0055] Step S2: Plan the route for each bicycle. Based on the current environment information, determine the executable bicycle route planning method and generate the executable bicycle routes.

[0056] In this embodiment of the invention, the single-vehicle path planning method includes: comprehensively considering multiple factors such as vehicle location, environmental map, and driving task requirements, and selecting a suitable executable single-vehicle path planning method for path planning. Specifically, the single-vehicle path planner integrates vehicle location information with road network information in a priori map to generate a feasible vehicle path in the current environment. The path planning method includes, but is not limited to, algorithms such as shortest path, optimal path, and local path, which are automatically selected and configured based on different input information.

[0057] Here, the path planning method can be implemented as follows: First, based on the vehicle's kinematics model, the two-dimensional plane is appropriately discretized. The degree of discretization is selected according to the vehicle's kinematic characteristics and is not fixed. Through discretization, the path planning problem can be transformed into a sampling problem involving multiple path points in the graph. During the sampling process, the vehicle's kinematic characteristics and environmental information are considered to avoid areas where the vehicle cannot pass, such as defined constraint areas, areas with obstacles, and roads that do not meet the vehicle's passage dimensions. The sampling expansion step size is adjusted according to environmental information to obtain the optimal path result.

[0058] During the sampling process, graph search or probabilistic pruning will be used to prioritize or prune sampling nodes to obtain the optimal path for the given path objective. The search strategy will take into account factors such as vehicle kinematics, road grade, and terrain features (both raised and recessed areas) to select the most suitable path.

[0059] For example, heuristic graph search first constructs a directed graph structure from the discretized space. Then, according to the heuristic algorithm, an evaluation function is introduced during the search process—that is, the estimated path length from the current node to the destination is added to the node evaluation function—to achieve the goal of quickly and accurately finding the optimal solution. Next, a suitable search algorithm is selected, such as the A* algorithm or Dijkstra's algorithm, to obtain the optimal solution for the vehicle's travel path. During the search process, different evaluation functions, such as weight evaluation functions or time evaluation functions, can be used to control, optimize, and prune the search process. Finally, the searched path solution is optimized and smoothed to obtain the final executable path.

[0060] For example, the sampling algorithm works as follows: 1. Initialize a node as the root node. 2. The sampling function generates a random point, which considers the kinematic feasibility (or random direction) of vehicle motion. This random point has a certain probability of pointing to the target point. 3. Find the node closest to the random point in the tree and specify the path according to the corresponding kinematic model, obtaining a series of path points with a certain step size. 4. For the traversed path points, perform collision detection to determine if there are obstacles and if the path is valid. 5. If the path point generated in step 4 is found to have no valid solution, discard the path and return to step 2 to resample; otherwise, select the last valid node as the new node. 6. Connect the new node to its parent node and use the cost function to evaluate and sort the paths. Then, select the newly generated node and its adjacent points and try to reconnect their parent nodes. 7. Use optimization methods to further optimize the path and generate new nodes. If the termination condition is met, a feasible path is obtained; otherwise, return to step 2 to regenerate random points. 8. Optimizing the tree structure using methods with pre-estimated information can yield a better path.

[0061] In stepwise single-vehicle path planning algorithms, a series of waypoints are typically generated, and a path is formed by connecting these waypoints with a line. However, stepwise solving can be inefficient and affect the completeness of the final convergence. Therefore, to address these issues, a curved connection method can be used to directly connect waypoints to the destination. Specifically, a curved connection method can be used when the distance to the destination reaches a certain point. This distance can be set according to the application's needs, considering both path smoothness and continuity, as well as practical feasibility.

[0062] Curve generation methods can include RS curves and B-spline curves. These methods can generate geometrically smooth curves based on information such as the distance, direction, and curvature between points, resulting in smoother and more continuous paths. For example, RS curves are a type of smooth highway design curve. Curve parameters are determined according to certain rules, giving the curve smoothness, continuity, and characteristics that meet driver perception requirements. B-spline curves are a popular curve fitting method that generates a smooth curve by interpolating a set of control points. Both methods are widely used in path planning and curve design, effectively meeting the requirements for smoothness and continuity.

[0063] To improve the efficiency and practicality of path planning, an offline vehicle position table method is used in the collision detection approach. First, based on the approximate shape of the vehicle and its projected space on the map, the spatial range occupied by the vehicle in different directions needs to be determined. To this end, the vehicle is rotated around the midpoint of the line connecting the rear wheels, with a certain discrete angular step size, and the spatial coordinates of each discrete position relative to the planned map are recorded. The data in these recorded offline vehicle position tables can be quickly looked up based on the current vehicle position and orientation angle to calculate the spatial range occupied by the vehicle on the planned map at that moment.

[0064] Taking single-vehicle path planning as an example, collision and obstacle avoidance during vehicle movement typically need to be considered. Therefore, it's necessary to calculate the vehicle's occupied space range in different directions and positions. To avoid performance loss from calculating the occupied space each time, an offline vehicle position table method is used. This involves pre-calculating the vehicle's occupied space range in different directions and positions and recording it in an offline vehicle position table. Then, by looking up the current vehicle position and orientation angle in the offline vehicle position table, the occupied space range at the current position can be quickly obtained. Figure 2 As shown, this represents the space occupied by the offline vehicle body position table.

[0065] Step S3: Determine the locations of multi-vehicle path conflicts. Based on the single-vehicle path results, determine the spatiotemporal locations where conflicts may occur between multiple vehicles in the same time and space.

[0066] In multi-vehicle path planning, it is necessary to consider the motion states of multiple vehicles and ensure that they do not collide during their journey. Therefore, determining the spatiotemporal conflict locations of all vehicles requires the following two steps:

[0067] Step S3.1: Single-vehicle path planning in multi-vehicle path planning. During single-vehicle path planning, it is necessary to calculate the time information corresponding to each sampling step. Based on the distance between the current sampling point and the previous sampling point, and the vehicle's maximum speed, the arrival time of the vehicle at each sampling point is determined through linear calculation. In this way, detailed time information can be obtained for the path planning results of each vehicle.

[0068] Taking single-vehicle path planning as an example, the sampling, search, and connection curves are all discrete step-size results. These can be combined with time under uniform motion to assign temporal meaning to the step size during the search or sampling process, adding spatiotemporal constraints in multi-vehicle searches. In single-vehicle path planning, sampling points within the spatiotemporal constraints can be avoided or eliminated.

[0069] Step S3.2: Determine the location of spatiotemporal conflicts. On the same two-dimensional planning plane, iterate through the path planning results of each vehicle to check for path conflicts. For path points where conflicts may occur, record the time step and which two vehicles collide. These records can be used for subsequent path optimization and obstacle avoidance planning to ensure that multiple vehicles do not conflict during their journey.

[0070] Specific conflict calculations can be divided into two categories: Simultaneous positional overlap: At the same moment, the positions corresponding to vehicle path points overlap on the two-dimensional planning plane, indicating a potential path conflict. To determine if positional overlap has occurred, we can first find the positions of all vehicles on the planning plane at each moment, and then compare whether their position coordinates overlap. If overlap exists, it indicates a potential path conflict point, requiring appropriate path optimization planning.

[0071] Proximity at the next moment: Although the locations of the vehicle's waypoints do not overlap at the current moment, the vehicle may move to a nearby location at the next moment, potentially causing a path conflict. To determine whether a conflict will occur in the next step, the vehicle's trajectory can be predicted, the location of the vehicle's waypoint at the next moment can be calculated, and it can be checked whether this location overlaps with the locations of other vehicles' waypoints. If there is an overlap, it indicates a potential path conflict point, requiring appropriate path optimization planning.

[0072] Step S4: In the single-vehicle path results, add constraints at conflict locations based on the vehicle position table and record the constraint information.

[0073] To resolve potential conflicts in multi-vehicle routing, constraints need to be added at the conflict locations. When two vehicles have conflicting routes, this invention takes the following steps to add constraints to the routing plan of the other vehicle:

[0074] For two conflicting vehicles, a lookup of the vehicle position table is performed based on their current path points to determine the planned map space occupied by each vehicle. This vehicle position table can be modeled based on factors such as vehicle model, width, and length to calculate the space occupied by each vehicle at any given time.

[0075] The spatiotemporal position constraint of this time step is added to the single-vehicle path planning of another vehicle as a sampling exclusion zone. That is, in the path planning of another vehicle, this invention marks the space occupied by the current vehicle as a no-sampling exclusion zone, and path points within this exclusion zone are considered impassable.

[0076] In this way, the present invention can incorporate the path conflict points to be resolved as constraints into the path planning process, ensuring that multiple vehicles can avoid potential conflicts when planning routes, thereby guaranteeing coordinated movement among the vehicles and maintaining safety during travel. Figure 3 As shown, spatiotemporal constraints are added based on the offline vehicle body position table.

[0077] Step S5: Group the multi-vehicle search task. Based on path information and collision information, group the multiple vehicles into different groups.

[0078] In this embodiment of the invention, path information and collision information include path feature information of existing single-vehicle path results for multiple vehicles, such as path length, number of turns, degree of coupling with other single-vehicle paths, and path curvature. Collision information includes feature information such as the number of path conflicts and conflict location information for each vehicle in the aforementioned method flow.

[0079] Multi-vehicle grouping based on path and collision information involves a comprehensive cost function evaluation, considering factors such as vehicle preset traffic priority, collision frequency of individual vehicle paths, and the degree of distance coupling between a vehicle's own path and those of other vehicles. After evaluation, grouping is determined based on the total number of vehicles, the number of groups, and the cost of each path.

[0080] In practice, information is used to group multiple vehicles. Here is one implementation method.

[0081] The problem involves dividing multiple vehicles into groups based on the degree of conflict, and then executing a multi-machine path planning algorithm within each group to solve the path planning problem. The main steps include:

[0082] Based on the single-vehicle path results and the corresponding time steps, the degree of conflict between each path is evaluated. The evaluation method can be to calculate the Euclidean distance between paths, or to determine the degree of conflict based on the containment relationship of path points.

[0083] Vehicles with low conflict levels are clustered together. To ensure clustering priority, the length of the individual vehicle's planned path can be used for grouping. If the distance between the start and end points is far or there are many obstacles in between, resulting in a long planned path for each vehicle, the vehicle with the longest path is identified as having the highest priority. Then, using this high-priority path as a reference, vehicles with the lowest spatial coupling are found and grouped into the same group. For example... Figure 4 The groups shown.

[0084] For subsequent paths, select those with longer path lengths and perform the same clustering operation again until all paths are grouped.

[0085] Step S6: Perform multi-vehicle path search for the same group. For multiple vehicles in the same group, perform multi-vehicle path search based on collision and constraint information to obtain the collision-free path results. During the multi-vehicle path search, heuristic search is performed based on constraint and collision information, such as frequency.

[0086] After clustering, multi-vehicle path planning is performed on the vehicles within each group based on the new sequence. The multi-vehicle path planning algorithm takes into account possible collisions between vehicles and designs appropriate heuristic functions and cost functions to ensure the safety and efficiency of multi-vehicle coordinated movement.

[0087] The final multi-vehicle planning scheme should guarantee that multiple vehicles reach the designated target point within the specified time, while avoiding interference from dynamic obstacles, achieving the optimal solution with the shortest time and lowest cost. The specific algorithm pseudocode is as follows:

[0088]

[0089]

[0090] In an exemplary embodiment of this application, step S6, which involves performing multi-vehicle path search based on collision information and constraint information, includes: after each vehicle sets constraints on the path points where a collision occurs, step S2, which involves single-vehicle path planning under constraints, is executed to generate new single-vehicle path results; then steps S3 and S4 are executed until all single-vehicle path results show that no vehicle path has a conflict at any location in step S3, at which point the multi-vehicle path search is stopped.

[0091] In practice, multi-vehicle path search can be implemented in various ways, including but not limited to single-vehicle path search that utilizes collision frequency and suboptimal search, RRT single-vehicle path search with kinematic constraints, and learning-based end-to-end single-vehicle path planning. The specific method chosen depends on the desired target characteristics and functions in the actual implementation process.

[0092] In implementation, an improved multi-agent path algorithm for search execution in a warehousing and transportation environment is presented here. The original algorithm presupposes discretizing the space into a grid, then constructing a graph search algorithm based on the grid map and applying constraints during the search, ultimately finding a collision-free path group. A typical example of a conflict-based planning method is the CBS algorithm (Sharon et al. 2015), a hierarchical two-level search algorithm for optimally solving the MAPF (Mapping Effect Factor).

[0093] In the high-level planning phase, the search algorithm CBS performs a best-first search on the node graph of the constraint tree (CT). Each node N of the CT constraint tree contains a set of constraints N.constraints to coordinate collision relationships between vehicles, adding constraints to search paths to avoid conflicts, and a set of paths N.paths where each vehicle has its own path, all of which satisfy the corresponding N.constraints constraints; let N.path[i] represent vehicle a. i If the cost of path N is the sum of the costs of all vehicle paths, then the cost is expressed as: m represents the number of paths. The root node of the CT constraint tree contains a set of empty set constraints and a set of shortest paths.

[0094] When expanding a CT node, the CBS search algorithm checks for conflicts between its paths. If no conflicts are found, the CT node is the one leading to the destination, and the CBS process terminates. Otherwise, CBS selects one of the conflicts and resolves it by splitting the CT node into two sub-CT nodes. In each sub-CT node, a car at the current conflict position adds spatiotemporal constraints to its own planning space at the conflict point, i.e., adding vertex or edge constraints to limit the car's feasible space at the conflict time step. At this point, the path that should be withdrawn in the low-level planning does not meet the new constraints, so the path needs to be replanned using A* in the low-level planning stage to search for a path that meets the constraints. Finally, the constrained search method in the branch search tree ensures the completeness of the solution and guarantees that the result is optimal.

[0095] ECBS, Focus Search, is a focus search method based on A ε *The bounded suboptimal search algorithm (Pearl and Kim 1982) maintains two lists of search nodes: OPEN and FOCAL. OPEN is a list of nodes to be traversed in the A* algorithm's flow storage queue, where the f-value of node N is calculated based on a user-defined cost function and sorted in a priority queue. Let best... f It is the node with the smallest f value in OPEN, where w is the factor coefficient of the suboptimal solution set by the user. FOCAL contains nodes n in OPEN, for which f(n) ≤ w·f(best) f The nodes are sorted according to the estimated distance function d, which can be expressed as the expected number of steps from the current node n to the final destination node. FOCAL focus search always expands the node with the smallest value of d in the FOCAL list, that is, it expands the node most likely to reach the destination by the shortest path. Since f(best... f The cost C of the optimal solution is... * The lower bound of the focus search ensures that the cost of all returned vehicle paths is at most w·C. * .

[0096] ECBS is a bounded suboptimal variant of CBS, which uses the same suboptimal factor w for focus search at both the high and low levels. The low-level ECBS is for vehicle a. i The search finds a bounded, suboptimal path that satisfies the constraints of CT node N, minimizing the number of conflicts with other vehicle paths. This is achieved through focus search, where f(n) is the cost evaluation function of the A* algorithm, f(n) = g(n) + h(n), and the cost function d(n) is the number of path conflicts between the current vehicle and other vehicles.

[0097] For collision count statistics, the aforementioned collision detection method is required. For each explored node, it's necessary to calculate whether it collides with other path results at that time point within the current time interval, count all collision counts, and record them as collision state heuristics. It's also necessary to calculate whether the time interval from the previous explored node to the current explored node collides with existing path results, count the collision frequency, and record it as a transition process collision heuristic. Both collision calculations use the offline vehicle collision detection method described in step three. The sum of these two calculations yields the estimated cost function d(n).

[0098] When a feasible, conflict-free path is found, it returns not only the path result data, but also the minimum f value f in OPEN. i min (N) represents vehicle a i The lower bound of the shortest path cost value. This invention will use vehicle a... i The cost of the shortest path is represented as f.i opt (N), but this value is usually unknown during the search process. Therefore:

[0099]

[0100] The focus search used at the low level of ECBS is to accelerate the high-level search, rather than the low-level single-machine search path, and to reduce the number of constraint trees that need to be developed for high-level search to resolve conflicts through hierarchical discussions.

[0101] In ECBS, the high-level search is an improved focus search. The OPEN list is the regular openlist of the A* algorithm, and the high-level sorting of CT nodes N is based on... This refers to the minimum cost of the minimum cost path solution at node N in the CT list. From equation (1.1), we can derive that lb(N) ≤ cost(N) ≤ wlb(N). Let the node with the minimum lb in the OPEN list be best. lb The FOCAL list contains nodes at CT node N where cost(N) ≤ w·lb(best). lb The nodes are determined based on the number of collisions h in N.path. c (N) sorting, this number of collisions can be used as a rough estimate of the step size required to reach the destination from the current position in the high-level search phase, that is, how many multi-way trees need to be built to find a collision-free path under CT node N, and further discussion on the number of collision-free path cases. Since lb(best lb The sum of costs for the optimal solution path is denoted by , where the cost of any CT node in the FOCAL list is less than or equal to w times the sum of costs for the optimal solution. Therefore, once a solution is found, its sum of costs will not exceed w times the sum of costs for the optimal solution.

[0102] This multi-vehicle pathfinding algorithm uses a focus search algorithm to accelerate high-level search and employs collision detection from an offline vehicle position table to count path conflicts. The algorithm aims to find collision-free paths for multiple vehicles, ensuring optimal results. The focus search algorithm utilizes collision frequency information and individual vehicle path feature information to accelerate the multi-vehicle pathfinding process.

[0103] The advantages of this execution method are: considering vehicle kinematic constraints, it effectively solves the collision problem in multi-vehicle path planning, ensuring that multiple vehicles can complete their respective tasks without collisions. It uses a grouping method, performing multi-vehicle path search among vehicles within the same group, which speeds up the search and improves search efficiency. Furthermore, combining constraint information and collision frequency information further improves search efficiency and path planning quality.

[0104] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for multi-vehicle cooperative path planning in an unstructured closed environment, characterized in that... Includes the following steps: Step S1: Determine the current positions of multiple vehicles on the planning map, the start and end points of the planning task, and the current movement status of the vehicles; Step S2: Plan the route for each bicycle; Based on the current environment information, determine the executable bicycle route plan and generate the executable bicycle route results; In collision detection, an offline vehicle position table is used to determine the space occupied by the vehicle on the map when it rotates in different directions, based on the shape of the vehicle and the space occupied by the map projection. Step S3: Determine the locations of multi-vehicle path conflicts; based on the single-vehicle path results, determine the spatiotemporal locations where conflicts may occur between multiple vehicles in the same time and space; including the following steps: Step S3.1: Single-vehicle path planning in multi-vehicle path planning; During the single-vehicle path planning process, the time information corresponding to each sampling step is calculated, and the time for the vehicle to arrive at each sampling point is determined by linear calculation based on the distance between the current sampling point and the previous sampling point and the maximum driving speed set by the vehicle. Step S3.2: Determine the location of spatiotemporal conflict; on the same two-dimensional planning plane, traverse the path planning results of each vehicle and check whether there will be path conflicts; for path points that may conflict, record the time step and which two vehicles will collide. Step S4: In the single-vehicle path results, add constraint information at the locations of multi-vehicle path conflicts based on the vehicle's position; Constraints are added at conflict locations based on the offline vehicle position table; for two conflicting vehicles, the offline vehicle position table is searched based on the current path point location to determine the planned map space range occupied by the current vehicle body; The spatiotemporal location constraint of this time step is added to the single-vehicle path planning of another vehicle as a sampling exclusion zone; Step S5: Group the multi-vehicle search task; group the multiple vehicles differently based on path information and collision information; Step S6: Perform multi-vehicle path search for the same group; for multiple vehicles in the same group, perform multi-vehicle path search based on collision information and the constraint information to obtain collision-free path results for the multi-vehicle paths.

2. The multi-vehicle cooperative path planning method in an unstructured closed environment according to claim 1, characterized in that: The vehicle rotation center is determined, and the vehicle body is rotated according to a certain discrete angle step. The spatial coordinate value of each discrete position relative to the planned map is recorded in the offline vehicle position table. Based on the offline vehicle position table, the spatial range of the current vehicle body on the planned map can be calculated quickly according to the current vehicle body position and orientation angle.

3. The multi-vehicle cooperative path planning method in an unstructured closed environment according to claim 1, characterized in that: In step S5, the path information is path feature information based on the existing single-vehicle path results of multiple vehicles; Collision information includes the number of path conflicts and the location of the conflicts for each vehicle; Multi-vehicle grouping based on path and collision information includes: obtaining a comprehensive cost based on path and collision information, evaluating it through a comprehensive cost function, and then grouping the vehicles based on the total number of vehicles, the number of groups, and the cost of each path. The comprehensive cost includes the vehicle's preset traffic priority, the frequency of collisions occurring on a single vehicle's path, and the degree of distance coupling between the vehicle's own path and the paths of other vehicles.

4. The multi-vehicle cooperative path planning method in an unstructured closed environment according to claim 1, characterized in that: The multi-vehicle path search in step S6 based on collision information and constraint information includes: after each vehicle sets constraints on the path points with collisions, it will then execute step S2 single-vehicle path planning under constraints to generate new single-vehicle path results; then steps S3 and S4 will be executed until all single-vehicle path results show that no vehicle path has any conflict at any location in step S3, then the multi-vehicle path search will stop.

5. The multi-vehicle cooperative path planning method in an unstructured closed environment according to claim 1, characterized in that: The path planning method in step S2 first discretizes the two-dimensional plane based on the vehicle kinematics model, transforming path planning into sampling of multiple path points in the graph. During the sampling process, the kinematic characteristics of the vehicle and environmental information are considered to avoid areas where the vehicle cannot pass. Sampling point selection includes graph search and / or probabilistic pruning, which involves selecting and / or pruning sampling nodes with different priorities.

6. The multi-vehicle cooperative path planning method in an unstructured closed environment according to claim 5, characterized in that: In step S2, a heuristic graph search is employed. First, the discretized space is constructed into a directed graph structure. Then, according to the heuristic algorithm, an evaluation function is introduced during the search process, which is the estimated path length from the current node to the destination. Next, a search algorithm is selected to search and obtain the optimal solution for the vehicle's travel path. During the search process, different evaluation functions are used to control, optimize, and / or prune the search process. Finally, the path solution obtained from the search is optimized and smoothed to obtain the final executable path.

7. The multi-vehicle cooperative path planning method in an unstructured closed environment according to claim 1, characterized in that: In step S3.1, the single-vehicle path planning, sampling, search, and connection curves are all discrete step-size results. Combined with the time under uniform motion, the step size is given a temporal meaning during the search or sampling process, which is used to add spatiotemporal constraints in multi-vehicle search. In step S3.2, the calculation of conflicts is divided into two categories: Simultaneous position overlap: If the positions of vehicle waypoints overlap on the two-dimensional planning plane at the same moment, it indicates a possible path conflict. First, find the positions of all vehicles on the two-dimensional planning plane at each moment, and then compare whether their position coordinates overlap. If there is an overlap, it means that there may be a path conflict point, and corresponding path optimization planning is required. Next moment position similar: By predicting the vehicle's trajectory, calculate the position of the vehicle's path point at the next moment, and check whether the position overlaps with the position of the path point of other vehicles. If there is overlap, it indicates that there are potential path conflict points, and corresponding path optimization planning is required.

Images