A method for target point allocation and cooperative path planning for multiple unmanned vehicles

By combining the Hungarian-CBS search forest module and the spatiotemporal Hybrid A* module, the problem of efficient combination of target point allocation and path planning in multi-autonomous vehicle scenarios is solved, realizing the shortest path and collision-free planning of multi-autonomous vehicle systems, and improving algorithm efficiency and success rate.

CN117420838BActive Publication Date: 2026-11-10SOUTHEAST UNIV
View PDF 2 Cites 0 Cited by

Patent Information

Application Number
CN202311189660.4
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2023-09-15
Publication Date
2026-11-10
Estimated Expiration
2043-09-15

AI Technical Summary

Technical Problem

Existing multi-agent path planning algorithms cannot achieve an efficient combination of target point allocation and path planning in multi-autonomous vehicle scenarios, resulting in unreasonable target point allocation, lengthy paths, and a low success rate in constructing collision-free trajectories.

Method used

A Hungarian-CBS search forest module and a spatiotemporal Hybrid A*-based underlying path search module are adopted. The Hungarian algorithm and CBS are combined to construct a target point allocation search forest. The initial target point allocation scheme is calculated by the Hungarian algorithm, and the spatiotemporal Hybrid A* multi-agent joint path planning algorithm is used to construct a collision-free path, detect and adjust conflicts, and ensure the shortest path and collision-free operation of the multi-autonomous vehicle system.

Benefits of technology

It improves the success rate and algorithm efficiency of multi-vehicle collaborative path planning, reduces path length, and lowers the average running time and collision probability of the intelligent agent.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN117420838B_ABST
    Figure CN117420838B_ABST
Patent Text Reader

Abstract

This invention proposes a target point allocation and cooperative path planning method for multiple autonomous vehicles, comprising: a Hungarian-CBS search forest algorithm to find the optimal target point allocation scheme by comparing the cost of cooperative path planning under different target point allocation schemes; a low-level path search module based on spatiotemporal Hybrid A* to obtain a collision-free path that conforms to the kinematic model of the autonomous vehicle; and an iterative method for solving target allocation and path search to obtain the optimal allocation scheme and the optimal vehicle path. This invention addresses the shortcomings of multi-agent path planning algorithms in the cooperative path planning problem of unlabeled underactuated robots, such as unreasonable target point allocation, lengthy paths, and low success rate of collision-free trajectory construction, thereby improving path solving efficiency, reducing path length, and enhancing path safety.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] This invention relates to the field of multi-agent path search, specifically a method for target point allocation and collaborative path planning for multiple unmanned vehicles. Background Technology

[0002] Multi-agent path finding (MAPF) is a research hotspot in artificial intelligence and robotics, aiming to find collision-free paths for multiple robots simultaneously, ensuring the efficient operation of multi-robot collaborative systems. It has been widely applied in logistics robots, drone swarms, and robotic arm collaboration. To further enhance the autonomy of multi-agent systems, unlabeled multi-agent path planning has gradually gained attention. This requires the system to comprehensively analyze the distribution of agents and target points without manually specifying the goals for each agent, rationally assigning navigation goals to each agent, and constructing collision-free paths to complete the task. Most existing algorithms are loosely coupled, consisting of two stages: first, solving for target point allocation based on heuristic costs, and then solving for multi-agent path planning based on the target points assigned to each agent. While this approach yields feasible solutions to the unlabeled multi-agent path planning problem, it fails to achieve an efficient combination of multi-agent path planning and target point allocation algorithms, leading to shortcomings such as unreasonable target point allocation, lengthy paths, and low success rates in constructing collision-free trajectories.

[0003] Technical Comparison with Patent CN 111623780A "A Multi-Unmanned Vessel Cooperative Path Planning Method Based on Priority Target Allocation"

[0004] Patent CN 111623780A addresses multi-unmanned surface vessel (USV) applications, while this patent targets multi-unmanned vehicle (UAV) applications. The difference lies in the fact that in multi-UAV applications, the environmental space is much narrower, requiring consideration of interference between the UAVs. To address this issue, this patent employs the CBS algorithm to collaboratively construct paths for multiple UAVs, ensuring collision-free operation. In contrast, patent CN111623780A combines elliptical motion constraints with FMM to form a single USV path planning algorithm, but it does not consider conflicts between USVs during path planning and does not discuss mechanisms to avoid such conflicts.

[0005] Patent CN 111623780A divides task allocation and path planning into two stages. First, it assigns target points to multiple unmanned surface vessels (USVs) based on a GA priority target allocation method, generating a preliminary navigation route for each USV. Then, based on motion constraints, it plans the actual path for each USV to travel along the preliminary navigation route to the next target point within the navigable area. This patent, however, couples task allocation and path planning into a single algorithm framework (Hungarian-CBS) for searching. Experiments show that, compared to the two-stage algorithm, the coupled algorithm can reduce potential conflicts between multiple USVs during the task allocation stage, thereby improving the success rate and efficiency of collaborative path planning.

[0006] Patent CN 111623780A employs a genetic algorithm to allocate target points to unmanned surface vessels (USVs) based on the path length obtained from single-USV path planning. This patent, however, uses a Hungarian algorithm to construct task allocations based on paths planned through CBS collaborative path planning, and then checks the rationality of the proposed solutions. For unreasonable target point allocation schemes, the path is reconstructed within a new scheme, thus forming an iterative solution mechanism for task allocation and path planning, ensuring optimal task allocation and efficient path planning. Summary of the Invention

[0007] To address the aforementioned technical problems, this invention proposes a target point allocation and cooperative path planning method for multiple autonomous vehicles. Based on the Hungarian algorithm and CBS, a target point allocation search forest is constructed to find the optimal target allocation scheme and complete conflict detection and constraint construction, minimizing the travel path of the multi-autonomous vehicle system. According to the constructed target points and constraints, paths conforming to the kinematic model of underactuated multi-autonomous vehicles are found, and the conflict search method is determined to ensure collision-free travel between autonomous vehicles and the environment, and between autonomous vehicles themselves, allowing them to safely reach their respective target points. This invention improves the solution speed of unlabeled multi-autonomous vehicle cooperative path planning algorithms, reduces path length, and increases the planning success rate.

[0008] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0009] A method and algorithm for target point allocation and cooperative path planning for multiple autonomous vehicles are presented, comprising: a Hungarian-CBS search forest module and a low-level path search module based on spatiotemporal Hybrid A*.

[0010] The specific steps of the Hungarian-CBS forest search module are as follows:

[0011] S1: Input the grid map, the initial pose of each unmanned vehicle and the position of each target point, and complete the algorithm initialization of the Hungarian-CBS module and the spatiotemporal Hybrid A* multi-agent joint path planning module. This step requires that each unmanned vehicle has the same number of target points.

[0012] S2: Based on the obstacle distribution in the obtained map, the initial pose of each unmanned vehicle and the position of each target point, calculate the Euclidean distance of each unmanned vehicle relative to each target point at the initial time, as an estimate of the path cost, construct the cost adjacency matrix, and calculate the initial target point allocation scheme based on the Hungarian algorithm.

[0013] S3: Based on the spatiotemporal Hybrid A* multi-agent joint path planning algorithm, construct the paths of each unmanned vehicle under this scheme, save the solution results as scheme nodes, and put them into the open set of the Hungarian-CBS algorithm, i.e. the upper-level open set.

[0014] S4: Using the total path length in the nodes as the node cost, find the node with the minimum cost in the upper open set as the node currently under study, and remove it from the open set. To prevent subsequent visits to this node, put it into the closed set.

[0015] S5: Perform a rationality check on the scheme. The rationality check determines whether to construct a new target point allocation scheme. Update the adjacency matrix with the path length between each agent and the target point in the existing scheme, and reconstruct the target point allocation scheme. If the new target point allocation scheme is inconsistent with the existing one, and the new scheme does not have a corresponding search tree, it indicates that the original scheme may be unreasonable. Then accept the reconstructed target point allocation scheme and its corresponding search tree, and return to S3 to perform path search. If the new scheme already exists in the search forest, or the scheme is verified to be reasonable, then there is no need to generate a new search tree, and proceed directly to S6.

[0016] S6: Conflict detection is responsible for checking whether there is a conflict in the multi-vehicle path of the node, and splitting the conflicting node into two new nodes. If no conflict is found, the current node is a feasible solution, and the algorithm stops and the result is output. If a conflict is found, the current node is split into two child nodes, and constraints are added to the conflicting agents. Then, return to S3 to build the respective paths of the two extended nodes.

[0017] The specific working steps of the underlying path search module based on spatiotemporal Hybrid A* are as follows:

[0018] S1: Initialize the algorithm and clear the open and closed set lists;

[0019] S2: Place the initial position of the first driverless car into the open set of the lower-level algorithm as the first node to be expanded;

[0020] S3: Select the node with the minimum path cost from the open set as the research node and discard it from the open set;

[0021] S4: Find all Reeds-shape curves connecting the current node to the target point and check whether the curves meet the constraints and are collision-free with the environment. Although multiple Reeds-shape curves are constructed for the same set of start and end points, the algorithm only retains the collision-free and constraint-compliant solutions. At this time, the algorithm sorts the curves according to their lengths, selects the shortest path as the research object, divides the curve into equal lengths using the vehicle constant speed motion model, constructs key nodes of the curves, and assigns timestamps so that the upper-level algorithm can complete the collision search and constraint construction on the RS curves and jumps to step S7. If all curves cannot simultaneously meet the collision-free and constraint conditions, proceed to step S5.

[0022] S5: When no feasible RS curve exists, incremental path search is completed by using node expansion. New nodes are searched around the current node using the defined motion primitives to ensure that the constructed trajectory conforms to kinematic constraints. It is determined whether the grid where the expanded node is located already exists in the open set. For nodes that already exist in the open set, if their path length is reduced, the corresponding node in the original open set is replaced; if they do not exist in the open set, they are added to the open set.

[0023] S6: Return to S3 and repeat until a feasible RS curve appears;

[0024] S7: Return to S2 and perform path search for the next driverless car until all driverless car paths have been constructed.

[0025] As a further improvement of the present invention, the cost adjacency matrix in step S2 of the Hungarian-CBS search forest module is maintained globally for the search forest. At the initial moment, the adjacency matrix is ​​solved by the Euclidean distance between the initial position of the unmanned vehicle and the target point. Then, the path construction under a certain target point allocation scheme is completed based on the spatiotemporal Hybrid A* multi-agent joint path planning algorithm. The adjacency matrix is ​​then updated with the path length of each unmanned vehicle to its respective target point.

[0026] As a further improvement of the present invention, the underlying path planning in step S3 of the Hungarian-CBS search forest module is used for Hungarian-CBS underlying path search. This algorithm constructs a path expansion method using an underactuated autonomous vehicle kinematic model, determines the conflict judgment conditions, path constraints and path expansion methods, and obtains a collision-free multi-autonomous vehicle spatiotemporal path, ensuring the success rate of constructing a collision-free path.

[0027] As a further improvement of the present invention, in the working steps of the Hungarian-CBS search forest module, the upper-level open set in S3 is the sum of all nodes in each search tree that have not undergone scheme rationality detection and conflict detection. The algorithm only detects the schemes in the open set and constructs new schemes and paths. In S4, the different search trees are uniformly sorted using the total path length in the nodes as the cost index, and the node with the smallest cost is used as the current expansion node.

[0028] As a further improvement of the present invention, in the working steps of the Hungarian-CBS search forest module, the search forest in S5 consists of three levels: nodes, binary search trees, and the search forest itself. It records the constraints of each agent, the path, and the total path cost in the current solution. Each binary search tree represents a target point allocation scheme and is expanded from its root node. In S6, when an agent conflict is found in a node, it is split into two child nodes, and constraints are constructed for the two conflicting agents. In S5, when an unreasonable allocation scheme of a node is found, the target point allocation scheme and its corresponding root node are reconstructed to form another search tree. The search forest records all current allocation schemes and sorts the open nodes in all allocation schemes uniformly. The node with the lowest cost is taken as the next expansion node. If there is no conflict in a node, it means that it is the current optimal feasible solution and is output by the algorithm.

[0029] As a further improvement of the present invention, the conflict search verification described in S6 involves performing a spatiotemporal search on the paths of each unmanned vehicle to the target point in the current scheme to determine whether a conflict occurs between two or more agents. By calculating the distance between vehicles at a certain moment, if the distance is less than a threshold, it is determined that the two vehicles collided at time t, and constraints are then constructed accordingly.

[0030] Constraint{(t,a1,p1)}

[0031] and

[0032] Constraint{(t,a2,p2)}

[0033] Where t represents the time when the conflict occurs, and a i This represents the agent number. According to this constraint, during the path search process using the S5 spatiotemporal Hybrid A* multi-agent joint path planning algorithm, the agent number is a. i The intelligent agent cannot reach position p at time t. i This avoids collisions between intelligent agents.

[0034] Compared with the prior art, the advantages of this invention are:

[0035] Existing unlabeled multi-agent path planning algorithms are mostly two-stage algorithms, performing multi-agent path planning based on the result of target point allocation, which cannot achieve joint optimization of target point allocation and path. The proposed algorithm is based on two modules: an upper-layer search forest of Hungarian-CBS and a lower-layer path planning of HybridA*. It comprehensively compares the results of multi-vehicle cooperative path planning under different task allocation schemes, ensuring optimal target point allocation while guaranteeing collision-free operation between vehicles. This improves the solution efficiency of MAPF, reduces the makespan of the unlabeled multi-agent path planning problem, the average running time of agents, and the collision probability. Attached Figure Description

[0036] Figure 1 This is a diagram of the proposed algorithm framework;

[0037] Figure 2 This is a block diagram of the Hungarian-CBS module;

[0038] Figure 3 This is the flowchart of the Hungarian-CBS algorithm;

[0039] Figure 4 This is a flowchart of a multi-agent joint path planning algorithm based on spatiotemporal HybridA*.

[0040] Figure 5 This is the kinematic model of the wheeled robot studied in this invention;

[0041] Figure 6 This is the path planning effect generated by the algorithm of this invention. Detailed Implementation

[0042] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0043] Figure 1 The algorithm framework diagram of this invention is shown, consisting of two modules: a CBS search forest based on Hungarian goal point allocation and a multi-agent joint path planning based on spatiotemporal HybridA*. The upper layer constructs a goal point allocation search forest based on the Hungarian algorithm and CBS, finding the optimal goal allocation scheme and completing conflict detection and constraint construction to minimize the travel path of the multi-autonomous vehicle system. The lower layer is invoked during the node expansion process of the upper layer. Based on the goals specified by the upper layer and the constructed constraints, it searches for paths conforming to the kinematic model of the underactuated multi-autonomous vehicle system and determines the conflict search method to ensure collision-free travel between autonomous vehicles and the environment, and between autonomous vehicles themselves, allowing them to safely reach their respective goal points.

[0044] Among them, the Hungarian-CBS algorithm framework is as follows: Figure 2As shown, it consists of three levels: nodes, binary search trees, and a search forest. The definition of a node is similar to the standard CBS algorithm, representing a multi-agent path planning scheme. It records the constraints of each agent, the path, and the total cost of the current path. Each binary search tree represents a target point allocation scheme, expanded from its root node. When agent conflict occurs within a node, it splits into two child nodes, constructing constraints for the two conflicting agents. When an allocation scheme for a node is found to be unreasonable, the target point allocation scheme and its corresponding root node are reconstructed, forming another search tree, as shown. Figure 2 The dark nodes in the tree on the left are shown. The search forest records all current allocation schemes and sorts the open nodes in all schemes uniformly, selecting the node with the lowest cost as the next expansion node. If there are no conflicts in a node, it indicates that it is the current optimal feasible solution and can be used as the algorithm output, such as... Figure 2 The dark nodes in the middle tree are shown.

[0045] The specific implementation of the Hungarian-CBS algorithm described in this invention consists of the following steps, and its flowchart is as follows: Figure 3 As shown:

[0046] S1: Input a grid map, the initial poses of each autonomous vehicle, and the positions of each target point. Complete the algorithm initialization of the Hungarian-CBS module and the spatiotemporal Hybrid A* multi-agent joint path planning module. This step requires that each autonomous vehicle has the same number of target points.

[0047] S2: Based on the obstacle distribution in the obtained map, the initial pose of each unmanned vehicle and the position of each target point, calculate the Euclidean distance of each unmanned vehicle relative to each target point at the initial time, as an estimate of the path cost, construct the cost adjacency matrix, and calculate the initial target point allocation scheme based on the Hungarian algorithm.

[0048] S3: Based on the spatiotemporal Hybrid A* multi-agent joint path planning algorithm, construct the paths of each unmanned vehicle under this scheme, save the solution results as scheme nodes, and put them into the open set (i.e., the upper-level open set) of the Hungarian-CBS algorithm.

[0049] S4: Using the total path lengths of all nodes as the node cost, find the node with the minimum cost in the upper-level open set and remove it from the open set. To prevent subsequent visits to this node, it will be placed in the closed set.

[0050] S5: Perform a rationality check on the proposed solution. The rationality check determines whether to construct a new target point allocation scheme. Update the adjacency matrix using the path lengths between each agent and the target point in the existing scheme, and reconstruct the target point allocation scheme. If the new target point allocation scheme is inconsistent with the existing one, and the new scheme does not have a corresponding search tree, it indicates that the original scheme may be unreasonable. In this case, accept the reconstructed target point allocation scheme and its corresponding search tree, and return to S3 for path searching. If the new scheme already exists in the search forest, or the scheme is verified as reasonable, there is no need to generate a new search tree; proceed directly to S6.

[0051] S6: Conflict detection checks for conflicts in the multi-driver paths of a node and splits conflicting nodes into two new nodes. If no conflict is found, the current node is a feasible solution. The algorithm stops at this point and outputs the result. If a conflict is found, the current node is split into two child nodes, and constraints are added to the conflicting agents. The algorithm then returns to S3 to construct paths for the two resulting extended nodes.

[0052] In S2, the cost adjacency matrix E is maintained globally in the search forest. Initially, the adjacency matrix is ​​calculated using the Euclidean distance between the initial position of the autonomous vehicle and the target point. Then, based on the spatiotemporal Hybrid A* multi-agent joint path planning algorithm, a path is constructed under a certain target point allocation scheme. Finally, the adjacency matrix is ​​updated using the path lengths from each autonomous vehicle to its respective target point.

[0053] The underlying path planning described in S3 is a spatiotemporal Hybrid A* multi-agent joint path planning algorithm used for Hungarian-CBS underlying path search. This algorithm constructs path expansion methods using the kinematic model of underactuated autonomous vehicles, determines conflict judgment conditions, path constraints, and path expansion methods, and obtains collision-free multi-autonomous vehicle spatiotemporal paths, ensuring a high success rate in constructing collision-free paths.

[0054] The upper-level open set mentioned in S3 is the sum of all nodes in each search tree that have not undergone scheme rationality detection and conflict detection. The algorithm only detects schemes in the open set and constructs new schemes and paths.

[0055]

[0056] Here, Open_Treei represents all nodes in each binary search tree that have not been split. Ranking nodes under different constraints in different objective allocation schemes ensures the optimality of the search. In S4, different search trees are uniformly sorted using the total path length of nodes as the cost metric, and the node with the lowest cost is used as the current expansion node.

[0057] The rationality check of the scheme described in S5 involves recalculating the Hungarian task allocation using the updated adjacency matrix. Since both the number of target points and agents are N, this constitutes a balanced assignment problem.

[0058]

[0059]

[0060]

[0061] X ij =1,0

[0062] Where E is the weighted adjacency matrix of a bipartite graph consisting of the set of agents and the set of target points. ij Let represent the path length from agent i to target point j. When the path from agent i to target point j is uncertain, the Euclidean distance between the agent's starting point and the target point is used as the path length estimate. X is the decision variable for the assignment problem. ij When the value is 1, it indicates that agent i is matched with target point j. The first two equations in the constraint respectively indicate that a target point is assigned to only a single agent and an agent is assigned to only one target point. Therefore, the balanced assignment problem aims to determine an assignment scheme X that minimizes the sum of costs J of the scheme while ensuring a one-to-one correspondence between agents and target points, thereby obtaining the optimal task assignment scheme.

[0063] The conflict search verification described in S6 involves performing a spatiotemporal search on the paths of each autonomous vehicle to the target point in the current scheme to determine whether a conflict occurs between two or more agents. By calculating the distance between vehicles at a certain moment, if the distance is less than a threshold, it is determined that the two vehicles collided at time t, and constraints are then constructed accordingly.

[0064] Constraint{(t,a1,p1)}

[0065] and

[0066] Constraint{(t,a2,p2)}

[0067] Where t represents the time when the conflict occurs, and a i This represents the agent's ID. According to this constraint, during the path search process using the S5 spatiotemporal Hybrid A* multi-agent joint path planning algorithm, the agent numbered a... i The intelligent agent cannot reach position p at time t. i This avoids collisions between intelligent agents.

[0068] The spatiotemporal Hybrid A*-based multi-agent joint path planning algorithm takes the initial positions of each autonomous vehicle, the target points specified by the upper layer for each vehicle, and the constructed constraints as inputs to sequentially construct N autonomous vehicle paths. Its algorithm flow is as follows: Figure 4 As shown, the description is as follows:

[0069] S1: Initialize the algorithm and clear the open and closed set lists.

[0070] S2: Place the initial position of the first driverless car into the open set of the lower-level algorithm as the first node to be expanded.

[0071] S3: Select the node with the minimum path cost from the open set as the research node and remove it from the open set.

[0072] S4: Find all Reeds-shape curves connecting the current node to the target point and check whether the curves satisfy the constraints and are collision-free with the environment. Although multiple Reeds-shape curves can be constructed for the same set of start and end points, the algorithm only retains the collision-free and constraint-compliant solutions. At this point, the algorithm sorts the curves by length, selects the shortest path as the research object, divides the curve into equal-length segments using a constant-speed vehicle motion model, constructs key nodes for the curve, and assigns timestamps so that the upper-level algorithm can complete the conflict search and constraint construction on the RS curve, and then jumps to step S7. If all curves cannot simultaneously satisfy both collision-free and constraint conditions, proceed to S5.

[0073] S5: When no feasible RS curve exists, incremental path search is performed using node expansion. New nodes are searched around the current node using the defined motion primitives to ensure the constructed trajectory conforms to kinematic constraints. It is determined whether the grid containing the expanded node already exists in the open set. For nodes already in the open set, if their path length is reduced, the corresponding node in the original open set is replaced; otherwise, it is added to the open set.

[0074] S6: Return to S3 and repeat until a feasible RS curve appears.

[0075] S7: Return to S2 and perform path search for the next driverless car until all driverless car paths have been constructed.

[0076] For the open set of the lower-level algorithm described in S2, all nodes that failed to pass S4-S5 are recorded. They are sorted according to the cost of the motion infinitesimal elements, and the node with the lowest cost is selected as the current node under study in S3 each time. The cost is...

[0077] f = h + g

[0078] Where h is the Euclidean distance heuristic cost between the node and the current agent's target, and g is the path length from the starting point to the current node.

[0079] The vehicle kinematics model described in this invention is defined as follows: Figure 5 As shown. XOY represents the map coordinate system, the vehicle center coordinates are (x, y), the angle between the vehicle's heading and the positive X-axis is θ, the vehicle's linear velocity is v, and its angular velocity is ω. Taking a sampling interval of T, the discrete kinematic model can be constructed as follows:

[0080]

[0081] The robot's velocity satisfies v∈[-v] max ,v max ], v max Let v be the robot's maximum linear velocity. It moves forward when v > 0 and backward otherwise. The angular velocity satisfies ω ∈ [-ω]. max ,ω max When ω > 0, the robot rotates counterclockwise; otherwise, it rotates clockwise.

[0082] The node expansion described in S5 involves constructing motion elements for vehicle forward movement based on motion primitives. These motion primitives use Δv and Δω as the sampling interval for two control variables v and ω, constructing a control variable sampling set Y,Ω. The motion primitive U then uses the Cartesian product of Y,Ω as the control input to the kinematic equations, generating a set of motion elements.

[0083] U = f(Υ × Ω)

[0084] Υ={-v max ,-v max +Δv,···,0,···,v max}

[0085] Ω={-ω max ,-ω max +Δω,···,0,···,ω max}

[0086] Where f(·) represents the input matrix in the kinematic equations. Unlike the Ackermann kinematic model, wheeled robots allow the vehicle to rotate in place, i.e.

[0087]

[0088] In addition, allowing robots to remain stationary to complete collision avoidance between agents, i.e.

[0089] f(0,0)∈U

[0090] For the node expansion described in S5, it is necessary to determine whether it meets the constraints specified by the upper-layer algorithm. Define expansion conflict: construct a circular region with radius R based on the dimensions of each intelligent vehicle, and determine whether a collision occurs.

[0091] when

[0092] ||p1-p2||2≤R

[0093] When it is determined that a collision has occurred, p i Let i = 1, 2 be the coordinates of the two agents. In S5, the current node is discarded by comparing whether there is a point in the constraints that will collide with the current expanded node.

[0094] Simulation results:

[0095] The algorithm described in this invention was verified by selecting a 32×32 room map from the MAPF standard dataset, and the results are as follows. Figure 6 As shown in the figure. The experiment included 10 wheeled unmanned vehicles and 10 target points. The initial positions of each unmanned vehicle were set in the areas marked on the left and bottom sides of the map, while the target points were set in the areas marked on the right and top sides of the map. Using the algorithm described in this invention, the trajectories of each unmanned vehicle shown in the figure were obtained.

[0096] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any other way. Any modifications or equivalent changes made based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A method for target point allocation and cooperative path planning for multiple unmanned vehicles, and an algorithm for target point allocation and cooperative path planning for multiple unmanned vehicles, comprising: The Hungarian-CBS search forest module and the spatiotemporal Hybrid A*-based underlying path search module are characterized by: The specific steps of the Hungarian-CBS forest search module are as follows: S1: Input the grid map, the initial pose of each unmanned vehicle and the position of each target point, and complete the algorithm initialization of the Hungarian-CBS module and the spatiotemporal Hybrid A* multi-agent joint path planning module. This step requires that each unmanned vehicle has the same number of target points. S2: Based on the obstacle distribution in the obtained map, the initial pose of each unmanned vehicle and the position of each target point, calculate the Euclidean distance of each unmanned vehicle relative to each target point at the initial time, as an estimate of the path cost, construct the cost adjacency matrix, and calculate the initial target point allocation scheme based on the Hungarian algorithm. S3: Based on the spatiotemporal Hybrid A* multi-agent joint path planning algorithm, construct the paths of each unmanned vehicle under this scheme, save the solution results as scheme nodes, and put them into the open set of the Hungarian-CBS algorithm, i.e. the upper-level open set. S4: Using the total path length in the nodes as the node cost, find the node with the minimum cost in the upper open set as the node to be studied, and remove it from the open set. To prevent the node from being visited again in the future, put it into the closed set. S5: Perform a rationality check on the scheme. The rationality check determines whether to construct a new target point allocation scheme. Update the adjacency matrix with the path length between each agent and the target point in the existing scheme, and reconstruct the target point allocation scheme. If the new target point allocation scheme is inconsistent with the existing one, and the new scheme does not have a corresponding search tree, it indicates that the original scheme may be unreasonable. Then accept the reconstructed target point allocation scheme and its corresponding search tree, and return to S3 to perform path search. If the new scheme already exists in the search forest, or the scheme is verified to be reasonable, then there is no need to generate a new search tree, and proceed directly to S6. S6: Conflict detection is responsible for checking whether there is a conflict in the multi-vehicle path of the node, and splitting the conflicting node into two new nodes. If no conflict is found, the current node is a feasible solution, and the algorithm stops and the result is output. If a conflict is found, the current node is split into two child nodes, and constraints are added to the conflicting agents. Then, return to S3 to build the respective paths of the two extended nodes. The specific working steps of the underlying path search module based on spatiotemporal Hybrid A* are as follows: T1: Initialize the algorithm and clear the open and closed set lists; T2: Place the initial position of the first driverless car into the open set of the lower-level algorithm as the first node to be expanded; T3: Select the node with the minimum path cost from the open set as the research node and discard it from the open set; T4: Find all Reeds-shape curves connecting the current node to the target point and check whether the curves meet the constraints and are collision-free with the environment. Although multiple Reeds-shape curves are constructed for the same set of start and end points, the algorithm only retains the collision-free and constraint-compliant solutions. At this time, the algorithm sorts the curves according to their lengths, selects the shortest path as the research object, divides the curve into equal lengths using the vehicle constant speed motion model, constructs key nodes of the curves, and assigns timestamps so that the upper-level algorithm can complete the conflict search and constraint construction on the RS curves, and jumps to step T7. If all curves cannot simultaneously meet the collision-free and constraint conditions, proceed to T5. T5: When no feasible RS curve exists, incremental path search is completed by using node expansion. New nodes are searched around the current node using the defined motion primitives to ensure that the constructed trajectory conforms to kinematic constraints. It is determined whether the grid where the expanded node is located already exists in the open set. For nodes that already exist in the open set, if their path length is reduced, the corresponding node in the original open set is replaced; if they do not exist in the open set, they are added to the open set. T6: Return to T3 and repeat until a feasible RS curve appears; T7: Return to T2 and perform path search for the next driverless car until all driverless car paths have been constructed.

2. The target point allocation and cooperative path planning method for multiple unmanned vehicles according to claim 1, characterized in that: In the working steps of the Hungarian-CBS search forest module, the cost adjacency matrix mentioned in S2 is maintained globally for the search forest. At the initial moment, the adjacency matrix is ​​solved using the Euclidean distance between the initial position of the unmanned vehicle and the target point. Then, the path construction under a certain target point allocation scheme is completed based on the spatiotemporal Hybrid A* multi-agent joint path planning algorithm. The adjacency matrix is ​​then updated with the path length of each unmanned vehicle to its respective target point.

3. The method for target point allocation and collaborative path planning for multiple unmanned vehicles according to claim 1, characterized in that: The underlying path planning described in step S3 of the Hungarian-CBS search forest module is used for Hungarian-CBS underlying path search. This algorithm constructs a path expansion method using an underactuated autonomous vehicle kinematic model, determines the conflict judgment conditions, path constraints, and path expansion methods, and obtains collision-free multi-autonomous vehicle spatiotemporal paths, ensuring the success rate of constructing collision-free paths.

4. The target point allocation and cooperative path planning method for multiple unmanned vehicles according to claim 1, characterized in that: In the working steps of the Hungarian-CBS search forest module, the upper-level open set mentioned in S3 is the sum of all nodes in each search tree that have not undergone scheme rationality detection and conflict detection. The algorithm only detects the schemes in the open set and constructs new schemes and paths. In S4, the different search trees are uniformly sorted using the total path length in the nodes as the cost index, and the node with the smallest cost is used as the current expansion node.

5. The target point allocation and cooperative path planning method for multiple unmanned vehicles according to claim 1, characterized in that: In the working steps of the Hungarian-CBS search forest module, the search forest in S5 consists of three levels: nodes, binary search trees, and the search forest itself. It records the constraints of each agent, the path, and the total path cost in the current solution. Each binary search tree represents a target point allocation scheme and is expanded from its root node. In S6, when an agent conflict is found in a node, it splits and expands into two child nodes, and constraints are constructed for the two conflicting agents. If the allocation scheme of a node is found to be unreasonable in S5, the target point allocation scheme and its corresponding root node are reconstructed to form another search tree. The search forest records all current allocation schemes and sorts the open nodes in all allocation schemes uniformly. The node with the lowest cost is taken as the next expansion node. If there is no conflict in a node, it means that it is the current optimal feasible solution and is output by the algorithm.

6. The method for target point allocation and cooperative path planning for multiple unmanned vehicles according to claim 1, characterized in that: In the Hungarian-CBS search forest module's working steps, S6, the conflict search verification involves performing a spatiotemporal search on the paths of each autonomous vehicle to the target point in the current scheme to determine whether conflicts occur between two or more agents. This is done by calculating the distance between vehicles at a given time; if the distance is less than a threshold, a collision is determined to have occurred between the two vehicles at time t, and constraints are then constructed accordingly. and ; Where t represents the time when the conflict occurs. This represents the agent number. According to this constraint, in step T5 of the underlying path search module based on spatiotemporal Hybrid A, the agent number is... The agent cannot reach the position at time t. This avoids collisions between intelligent agents.

Citation Information

Patent Citations

  • Hop searching based conflict-avoiding path planning method for multiple robots

    CN109115226A

  • Primary-secondary multi-unmanned-platform task allocation and collaborative planning method based on region segmentation

    CN116627128A