A multi-robot cooperative path planning method for complex environment

Abstract

The application belongs to path planning method, and particularly relates to a multi-robot cooperative path planning method for complex environment. It comprises a preparation step and subsequent execution steps, step one: individually planning shortest path for each robot; step two: performing global priority distribution for the robot; step three: introducing robot and conflict resolution; step four: judging conflict and classifying conflict resolution; step five: priority distribution in key conflict; step six: solving key conflict; and step seven: saving and displaying path. The application has the following beneficial effects relative to prior art: based on CA* algorithm, a conflict resolution method based on conflict classification and priority game is proposed, which can effectively solve problems such as road occupation, blockage and head-on collision in multi-robot path planning, and effectively improve the efficiency of the algorithm on the basis of solving these problems.

Description

Technical Field

[0001] This invention pertains to path planning methods, specifically a multi-robot collaborative path planning method for complex environments. Background Technology

[0002] With the continuous advancement of technology and the rapid development of artificial intelligence, robots are being used more and more widely in various industries. For single or multiple robots working together, rational path planning is becoming increasingly important, and many scholars at home and abroad have already conducted various studies on path planning.

[0003] In recent years, the swarm collaboration technology of intelligent robots has received increasing attention from scholars both domestically and internationally. Multi-agent path finding (MAPF), a key problem in swarm collaboration, aims to find a conflict-free path from the starting point to the destination for each robot in the swarm. MAPF has demonstrated significant research potential and value in various fields such as warehousing systems, traffic control, airport operations, and military logistics. When multiple mobile robots jointly perform tasks, conflicts may arise between them. To resolve these conflicts and ensure that all robots reach their designated destination, an algorithm capable of effectively resolving conflicts needs to be developed.

[0004] Currently popular path planning algorithms include intelligent algorithms, deep reinforcement learning algorithms, and traditional algorithms. Intelligent algorithms include ant colony optimization, neural network algorithms, and particle swarm optimization. Deep reinforcement learning algorithms combine deep learning and reinforcement learning. Traditional algorithms include Dijkstra's algorithm, A* algorithm, and artificial potential field methods.

[0005] The A* algorithm is a direct search method for finding the shortest path in static road networks. It searches for the shortest path by associating the actual distance from the initial node to the current node with the estimated distance from the current node to the target node. Compared with other algorithms, it has advantages such as flexibility, speed, and high solution efficiency. In multi-robot cooperative path planning, the A* algorithm cannot handle the coordinated path processing of multiple robots. This led to the development of algorithms such as CA* and HCA* to address the MAPF problem. The CA* algorithm plans a path for each robot sequentially, incorporating the path constraints of previous robots during the planning process. However, these algorithms have significant limitations, easily encountering bottlenecks that are difficult to solve or falling into infinite loops, leading to algorithmic errors. Summary of the Invention

[0006] The purpose of this invention is to address the shortcomings of existing technologies by providing a multi-robot cooperative path planning method for complex environments.

[0007] The specific technical solution adopted in this invention is as follows: a multi-robot cooperative path planning method for complex environments, comprising the following steps:

[0008] Preparation steps, and subsequent execution steps.

[0009] Step 1: Plan the shortest path for each robot individually.

[0010] The rules stipulate that the paths of robots other than itself are not considered, and only the obstacles in the map are considered as zero constraints. The improved A* algorithm is used to try to plan a path under zero constraints for each robot, and save whether there is a solution under zero constraints, and the shortest path when there is a solution.

[0011] Step 2: Assign global priority to the robot

[0012] Priority determination for global collaborative path planning of robots

[0013] Step 3: Introducing robots and conflict resolution

[0014] Robots are processed sequentially according to priority to resolve conflicts.

[0015] Step 4: Identify and classify conflicts to resolve them.

[0016] The severity of conflicts is divided into two categories: general conflicts and critical conflicts.

[0017] Step 5: Prioritization during critical conflicts

[0018] Upon identifying a critical conflict, the process moves to local solution. This step prioritizes local solutions to facilitate conflict resolution.

[0019] Step Six: Resolve Key Conflicts

[0020] To resolve key local conflicts and engage in potential priority game theory;

[0021] Step 7: Save and display the path

[0022] Save the final path and display it on the map according to the path information.

[0023] The multi-robot cooperative path planning method for complex environments described above includes the following preparation steps:

[0024] Given a square grid map M with obstacles, its side length is N, the number of robots is R, and the starting point of each robot is S. r The target point is G. r , where r = 1, 2, ..., R.

[0025] The multi-robot cooperative path planning method for complex environments described above includes, in step one:

[0026] Sub-step 1.1: Initialization of the robot

[0027] The robot is initialized to facilitate path planning. The `open` table for the robot under the A* algorithm is initialized to store the search area, containing the robot's starting position, evaluation function f(n), and the previous movement direction. The `close` table for the robot under the A* algorithm is initialized to store the already searched area, containing the positions of points searched and the previous movement direction.

[0028] f(n)=g(n)+w·h(n)+t p (1)

[0029] In formula (1), f(n) is the evaluation function in the traditional A* algorithm, representing the evaluation function from the starting point S. r The robot runs from state node n (the current node of the robot) to the target point G. r The total estimated cost,

[0030] In formula (1), g(n) is the actual cost from the starting point to the state node, that is, how many distances (grids) are actually traveled from the starting point to the state node.

[0031] In formula (1), h(n) is the estimated cost from the state node to the target point, that is, how far it is expected to walk from the state node to the target point. h(n) uses the Manhattan distance.

[0032] h(n) = |x S -x n |+|y S -y n | (2)

[0033] x in formula (2) S ,y S Represents the target point coordinates, x n ,y n Represents the coordinates of the state node.

[0034] In formula (1), w is a dynamic parameter used to measure the goal-orientation of the algorithm. When the value of w is equal to 0, the A* algorithm evolves into the Dijkstra algorithm, which performs a blind, aimless search outward from the starting point. When the value of w is less than 1, it means that w × h(n) is less than the actual cost of moving from state node n to the target. The larger w is, the fewer nodes the A* algorithm expands, and the slower the running speed will be. When the value of w is equal to 1, the algorithm will find the best path without expanding other nodes. It should be noted that the A* algorithm can only guarantee finding the shortest path when the value of w is less than or equal to 1. When the value of w is greater than 1, the algorithm will focus more on the distance to the destination when searching the surrounding area, and the search area will usually be greatly reduced.

[0035] In formula (1) t p As a turning penalty, since the algorithm stipulates that the robot cannot backtrack, it can only turn left or right and move straight. When the robot turns left or right, it will be penalized. Given the same g(n) and h(n), the robot will tend to move straight rather than turn frequently. Typically, t... p Set it to slightly greater than 0; the goal is simply to differentiate between going straight and turning.

[0036] Sub-step 1.2: Expand the open and close tables

[0037] The `open` table is traversed and searched. If the `open` table is empty, it means that no zero-constraint path can be found for the robot, so it is marked as having no solution, the `close` table is saved, and the process jumps to sub-step 1.3. If the `open` table is not empty, the point with the smallest f(n) value in the `open` table is selected as the state node n and added to the `close` table. It is then determined whether point n is the target point. If point n is the target point, it means that the robot has found a zero-constraint path, the `close` table is saved, and the process jumps to sub-step 1.4. If point n is not the target point, it means that no path has been found yet, so a search of the surrounding neighborhood of the point is performed, the robot's `open` table is updated, and sub-step 1.2 is repeated until the current sub-step is exited. During the repeated execution, the `open` and `close` tables will be continuously updated and iterated.

[0038] The "search of the surrounding neighborhood" is defined as follows: Within the four neighboring neighborhoods of state node n (i.e., the nodes adjacent to it in the top, bottom, left, and right directions), detect nodes that meet the requirements and add them to the open list. Calculate and store f(n), h(n), g(n), and t. p If the node already exists in the open table, compare the f(n) values ​​of the two tables and keep the smaller one.

[0039] A "qualified node" is defined as: the node is located within the map area, there are no obstacles at that point, and it is not in the close table.

[0040] Sub-step 1.3: Processing the unsolvable robot

[0041] Sub-step 1.2 will jump to this sub-step if it determines that the robot has no solution. In this sub-step, some processing is performed on the robot with no solution, such as storing the robot's starting position in the path table and specifying the path length L of the robot with no solution. path If the value is 1, the robot will remain in its initial position and proceed to sub-step 1.5.

[0042] Sub-step 1.4: Perform path backtracking on the robot with a solution and provide the path.

[0043] If sub-step 1.2 determines that the robot has a solution, it jumps to this sub-step and performs path backtracking. Using the robot's target point as the search node, it searches backwards from the end of the `close` table to find which node it originated from, and uses this node as the new search node, repeating this search process. Each time a node is searched, it is stored in the `path` table from back to front. When the starting point is found, the path backtracking is considered complete, and its path length L is defined. path Given the row number in the path table, proceed to step seven.

[0044] Sub-step 1.5: Determine whether all robots have planned their paths.

[0045] Determine whether all robots have completed zero-constraint path planning, i.e., whether they are marked as having a solution or not. If all robots have completed path planning, proceed to step two. If there are still robots that have not planned a path, perform zero-constraint path planning for the next robot in sequence, starting from step one.

[0046] As described above, a multi-robot cooperative path planning method for complex environments includes step two.

[0047] Sub-step 2.1: Give priority indicators

[0048] For a robot with a solution, let N be the number of rows in the open table when it finds the target point during step one. open The length of the close table in the same state is N. close The priority evaluation indicators are specified as follows:

[0049]

[0050] The metric I reflects the difficulty of the robot's path search. A larger I indicates that the robot finds a path more easily, meaning it is more likely to find another feasible path. To improve the success rate of path planning, robots with smaller I values ​​are given higher priority. This means the algorithm will prioritize ensuring the passage of robots that have a harder time finding a path before coordinating with robots that are more likely to find another path. If the metrics are exactly the same, the priority is randomly assigned.

[0051] Sub-step 2.2: Handling special cases

[0052] Set all robots with no solution to the lowest priority, and randomly assign priorities among the robots with no solution.

[0053] As described above, a multi-robot cooperative path planning method for complex environments includes step three.

[0054] Sub-step 3.1: Initialize the order table

[0055] Initialize an empty `order` table. This table is created based on the longest time step of all robots. Each time step stores the current position of each robot. The rule is that all robots start simultaneously at time step 1, and all robots reach the target position at the longest time step.

[0056] The stipulation "planning a path under the constraint of the order table" means that the robot will plan its path under the combined constraints of the map's built-in obstacles and the order table. The introduction of the order table constraint may lead to two types of conflicts: placeholder conflicts and opposing conflicts. Placeholder conflicts occur when two robots reach the same node at the same time step. The method for determining this is to check if the next node that robot n will reach already exists in the order table for that time step. Opposing conflicts occur when two robots swap positions at two adjacent time steps. The method for determining this is to check if the node reached by robot n at this time step exists in the order table for the next time step, and whether the position of robot m at the previous time step is the same as the position of robot n at the next time step.

[0057] Sub-step 3.2: Introduce robots sequentially and resolve conflicts.

[0058] Introduce robots in order of priority and guide them to follow the planned paths. If a conflict occurs, proceed to step four. If all newly introduced robots do not conflict with the order table, populate the order table with the path table of that robot and continue introducing new robots in order of priority until all robots have been planned. Once the path planning is complete, proceed to step seven to save and display the results.

[0059] As described above, a multi-robot cooperative path planning method for complex environments includes step four.

[0060] A general conflict refers to a conflict that will not cause robot n to take a long detour; that is, the robot can find another short, conflict-free path to avoid the conflict at that node. The solution to this type of conflict is to directly find a new, conflict-free path.

[0061] Critical conflicts are divided into critical detour conflicts and critical blocking conflicts. A critical detour conflict is one that causes robot n to take a long detour, but a solution still exists. A critical blocking conflict is one that directly renders robot n without a solution; this conflict must be resolved for robot n to have a solution. The approach to resolving critical conflicts is to eliminate local conflicts.

[0062] The steps to determine the criticality of a conflict are as follows:

[0063] Sub-step 4.1: Replan the route to prepare for determining the type of conflict.

[0064] Given the constraints of the current order table, reapply the improved A* algorithm to plan the path, and extract the length of the replanned path as L. pathN Make the following judgment:

[0065] Sub-step 4.2: Identify and resolve key conflicts

[0066] If L pathN If the value is 1, it means the robot has no solution after the conflict is introduced. This conflict is marked as a critical conflict, and the process proceeds to step five. If the value is greater than 1, it is determined whether the conflict leads to a long detour. A long detour is defined as being marked when the following conditions are met.

[0067]

[0068] In formula (4), N represents the side length of the map. When the length of the newly planned route is greater than the original route length by the square root of the side length and then rounded down, it is considered a long detour.

[0069] If a long detour occurs, mark the conflict as a critical conflict and proceed to step five. If no such conflict occurs, proceed to sub-step 4.3.

[0070] Sub-step 4.3: No critical conflict

[0071] If the conflict can be resolved by taking a short detour, save the planned path to the corresponding path table, modify the order table according to the path table, and return to sub-step 3.2 to continue the conflict judgment and resolution for the next robot.

[0072] As described above, a multi-robot cooperative path planning method for complex environments includes step five.

[0073] The specific steps for priority planning are as follows: Identify the location of the critical conflict and the time step (time) at which the conflict occurs.

[0074] Sub-step 5.1: Prerequisites

[0075] Ignore the bots that haven't joined the discussion yet.

[0076] Sub-step 5.2: Handling special cases

[0077] Check if any robots stop in place at time steps 'time' and 'time-1'. If so, place them at the end of the local priority list. Check which robots have not entered the 5×5 area centered on the conflict point at time step 'time', and place these robots at the end of the local priority list according to their priority order during global solution.

[0078] Sub-step 5.3: Local priority evaluation index

[0079] The remaining robots are analyzed, and a local priority evaluation index is defined as follows:

[0080]

[0081] In formula (5) I p This is a priority evaluation index for local planning; the smaller the value, the higher the priority.

[0082] In formula (5) L pathP The length of the robot's original planned path within a 5x5 area centered on the point of conflict.

[0083] N in formula (5) path The extended neighborhood surrounding the aforementioned path.

[0084] Calculate I for the remaining robot p The lower priority of this metric will be increased; if they are the same, a random metric will be assigned.

[0085] Based on the above rules, complete the priority planning for local conflict optimization, and proceed to step six.

[0086] As described above, a multi-robot cooperative path planning method for complex environments includes step six.

[0087] In this step, only the robots discussed in sub-step 3.2 are considered. The robots are numbered according to their priority order in step five, as n1, n2…n mThere are m robots in the discussion. The temporary reservation table is defined as `orderT`, initially empty. The specific operation steps are as follows:

[0088] Sub-step 6.1: Define the start and end points for handling critical conflicts.

[0089] The original planned positions of each robot at the time step preceding the conflict are set as their local starting positions. The original planned positions of each robot at the time step following the conflict in the existing robot paths are set as their local target positions. Local conflict resolution ends when all robots have reached their local target positions (not necessarily at the same time step). The initial value of robot index i is defined as 1.

[0090] Sub-step 6.2: Introduce the robot sequentially.

[0091] Let robot n i Move to the local target location according to the path table, and check if it conflicts with the orderT table. If a conflict occurs, jump to sub-step 6.4; otherwise, save the path table. If a solution exists after a conflict occurs, continue to sub-step 6.3; otherwise, mark it.

[0092] Sub-step 6.3: Determine if all robots involved in critical conflict handling have completed their processes. Check if i equals m; if not, increment i and repeat sub-step 6.2.

[0093] Sub-step 6.4: Path replanning

[0094] Perform a local search on the robot. Under the constraints of the orderT list, expand the robot's current position and surrounding positions with time steps. That is, the robot can perform five movements: up, down, left, right, and stationary. Add points that meet the expansion requirements to the open list with time steps, and add points in the open list to the close list with time steps. Check if there is a local target position in the close list. If there is, backtrack the path according to the path backtracking rules. If not, repeat this step until the number of repetitions exceeds the search circuit breaker limit. If the search circuit breaker limit is exceeded, mark it as having no solution and jump to sub-step 6.6.

[0095] Set the number of search circuit breakers to the number of map edges, which is N.

[0096] After the local search is complete, proceed to substep 6.5.

[0097] Sub-step 6.5: Process the path after reaching the local target point.

[0098] Using the method in step one, and under the constraints of the introduced order table, plan the robot's path from the local target point to its final target point. If a conflict occurs, plan the path along with the already introduced robots using the method in step two and related steps. Finally, modify the path table of the already introduced robots, and modify the orderT table based on the robot's path table as a reference for the subsequent local path planning of the robot, and return to sub-step 6.2.

[0099] Sub-step 6.6: Priority game when there is no solution

[0100] This sub-step involves prioritizing robots in local conflicts. The robot whose conflict occurred in sub-step 6.4 is prioritized to the highest level, and the conflict resolution in step six is ​​performed again. If an infinite loop occurs, i.e., two robots always prioritize to the highest level in turn, then the local conflict resolution is unsolvable. The robot is marked as unsolvable, and the process returns to step two to redo priority planning and conflict resolution.

[0101] This invention offers the following advantages over existing technologies: it optimizes the traditional A* algorithm, improving its search efficiency for solving single-robot paths. Based on the CA* algorithm, it applies this method to multi-robot path planning in complex environments, proposing a conflict resolution method based on conflict classification and priority game theory. This method effectively addresses issues such as path occupation, blocking, and head-on collisions in multi-robot path planning, while significantly improving the algorithm's efficiency. Attached Figure Description

[0102] Figure 1 Main flowchart;

[0103] Figure 2 Flowchart for step three;

[0104] Figure 3 Flowchart for step four;

[0105] Figure 4 Flowchart for steps five and six;

[0106] Figure 5 Initial map and robot starting target point;

[0107] Figure 6 A conflict is imminent at time step 8;

[0108] Figure 7 At 9:00 AM, robot number 2 took the avoidance maneuver.

[0109] Figure 8 The key conflict was successfully resolved;

[0110] Figure 9 The result of the CA* algorithm. Detailed Implementation

[0111] The present invention will be further described and illustrated below with reference to the accompanying drawings and specific embodiments.

[0112] A multi-robot cooperative path planning method for complex environments includes the following steps:

[0113] Preparation: Given a square grid map M with obstacles, its side length is N, the number of robots is R, and the starting point of each robot is S. r The target point is G. r , where r = 1, 2, ..., R.

[0114] Step 1: Plan the shortest path for each robot individually.

[0115] The rules stipulate that paths of robots other than themselves are not considered, and only the obstacles inherent in the map are considered as a zero-constraint condition. An improved A* algorithm is used to attempt to plan a path for each robot under zero constraints, and the existence of a solution under zero constraints and the shortest path when a solution exists are recorded. The sub-steps for path planning for a single robot using the improved A* algorithm are as follows:

[0116] Sub-step 1: Initializing the robot

[0117] The robot is initialized to facilitate path planning. The `open` table for the robot under the A* algorithm is initialized to store the area to be searched, containing the robot's starting position, evaluation function f(n), and the direction of the previous move. The `close` table for the robot under the A* algorithm is initialized to store the areas already searched, containing the positions of points searched and the direction of the previous move.

[0118] f(n)=g(n)+w·h(n)+t p (1)

[0119] In formula (1), f(n) is the evaluation function in the traditional A* algorithm, representing the process from the starting point Sr to the state node n (the current node of the robot) and then to the target point G. r The total estimated cost.

[0120] In formula (1), g(n) is the actual cost from the starting point to the state node, that is, how far (grids) is actually traveled from the starting point to the state node.

[0121] In formula (1), h(n) is the estimated cost from the state node to the target point, i.e., the estimated distance from the state node to the target point. h(n) uses the Manhattan distance.

[0122] h(n) = |x S-x n |+|y S -y n | (2)

[0123] x in formula (2) S ,y S Represents the target point coordinates, x n ,y n Represents the coordinates of the state node.

[0124] In formula (1), w is a dynamic parameter used to measure the goal-orientation of the algorithm. When the value of w is equal to 0, the A* algorithm evolves into the Dijkstra algorithm, which will blindly expand outward from the starting point without a goal. When the value of w is less than 1, it means that w×h(n) is less than the actual cost of moving from the state node n to the target. The larger w is, the fewer nodes the A* algorithm expands, and the slower the running speed will be. When the value of w is equal to 1, the algorithm will find the best path without expanding other nodes. It should be noted that the A* algorithm can only guarantee that it can find the shortest path when the value of w is less than or equal to 1. When the value of w is greater than 1, the algorithm will focus more on the distance to the destination when searching the surrounding area, and the search area will usually be greatly reduced.

[0125] In formula (1) t p As a turning penalty, since the algorithm stipulates that the robot cannot backtrack, it can only turn left or right and move straight. When the robot turns left or right, it will be penalized. Given the same g(n) and h(n), the robot will tend to move straight rather than frequently turn. Typically, t... p Set it to slightly greater than 0; this is simply to differentiate between going straight and turning.

[0126] Sub-step 2: Expand the open and close tables

[0127] The process iterates through the `open` table, checking if it's empty. If empty, it means no zero-constraint path can be found for the robot; mark it as unsolvable, save the `close` table, and proceed to sub-step three. If not empty, select the point with the smallest `f(n)` value from the `open` table as state node `n` and add it to the `close` table. Check if point `n` is the target point. If it is, the robot has found a zero-constraint path; save the `close` table and proceed to sub-step four. If point `n` is not the target point, no path has been found yet; search the surrounding neighborhood of the point, update the robot's `open` table, and repeat sub-step two until exiting this sub-step. During this repeated execution, the `open` and `close` tables are continuously updated and iterated.

[0128] The "search of the surrounding neighborhood" is defined as follows: Within the four neighboring neighborhoods of state node n (i.e., the nodes adjacent to it in the top, bottom, left, and right directions), detect nodes that meet the requirements and add them to the open list. Calculate and store f(n), h(n), g(n), and t. p If the node already exists in the open table, compare the f(n) values ​​of the two and keep the smaller one.

[0129] A “meeting node” is defined as: the node is located within the map area, there are no obstacles at the point, and it is not in the close table.

[0130] Sub-step 3: Processing the unsolvable robot

[0131] If substep two determines that the robot has no solution, it will jump to this substep, where some processing is performed on the unsolvable robot. The robot's starting position is stored in the path table, and the path length L of the unsolvable robot is defined. path If the value is 1, the robot will remain in its starting position and proceed to sub-step five.

[0132] Sub-step four: Perform path backtracking on the robot with a solution and provide the path.

[0133] If the second sub-step determines that the robot has a solution, it jumps to this sub-step and performs path backtracking. Using the robot's target point as the search node, it searches backwards from the end of the `close` table to find which node it originated from, and uses this node as a new search node, repeating this search process. Each time a node is searched, it is stored in the `path` table from back to front. When the starting point is found, the path backtracking is considered complete, and its path length L is defined. path Given the row number in the path table, proceed to sub-step seven.

[0134] Sub-step 5: Determine whether all robots have planned their paths.

[0135] Determine whether all robots have completed zero-constraint path planning, i.e., whether they are marked as having a solution or not. If all robots have completed path planning, proceed to step two. If there are still robots that have not planned a path, perform zero-constraint path planning for the next robot in sequence, starting from step one.

[0136] Step 2: Assign global priority to the robot

[0137] Priority determination for global collaborative path planning of robots is defined by the following steps:

[0138] Sub-step 1: Specify priority indicators

[0139] For a robot with a solution, let N be the number of rows in the open table when it finds the target point during step one. open The length of the close table in the same state is N. close The priority evaluation indicators are specified as follows:

[0140]

[0141] The metric I reflects the difficulty of the robot's path search. A larger I indicates that the robot finds a path more easily, meaning it is more likely to find another feasible path. To improve the success rate of path planning, robots with smaller I values ​​are given higher priority. This means the algorithm will prioritize ensuring the passage of robots that have a harder time finding a path before coordinating with robots that are more likely to find another path. If the metrics are completely identical, the priority is randomly assigned.

[0142] Sub-step two: Handling special cases

[0143] Set all robots with no solution to the lowest priority, and randomly assign priorities among the robots with no solution.

[0144] Step 3: Introducing robots and conflict resolution

[0145] Robots are processed sequentially according to priority to resolve conflicts:

[0146] Sub-step 1: Initialize the order table

[0147] Initialize an empty `order` table. This table is created based on the longest time step of all robots, and each time step stores the current position of each robot. It is defined that all robots start simultaneously at time step 1, and all robots reach the target position at the longest time step.

[0148] The stipulation "planning a path under the constraint of the order table" means that the robot will plan a path under the combined constraints of the map's built-in obstacles and the order table. Here, the introduction of the order table constraint may lead to two types of conflicts: placeholder conflict and opposing conflict. Placeholder conflict occurs when two robots arrive at the same node at the same time step. The method to determine this is to check whether the next node that robot n will arrive at already exists in the order table for the corresponding time step. Opposing conflict occurs when two robots swap positions in two adjacent time steps. The method to determine this is to check whether the node that robot n arrives at in this time step exists in the reservation table for the next time step, and whether the position of robot m in the previous time step is the same as the position of robot n in the next time step.

[0149] Sub-step 2: Introduce robots sequentially and resolve conflicts.

[0150] Introduce robots in order of priority and guide them along the planned paths. If a conflict occurs, proceed to step four. If all newly introduced robots do not conflict with the order table, populate the order table with the path table of that robot, and then introduce new robots in order of priority until all robots have been planned. Once path planning is complete, proceed to step seven to save and display the results.

[0151] Step 4: Identify and classify conflicts to resolve them.

[0152] Conflicts are categorized into two types based on their severity: general conflicts and critical conflicts. The definitions and resolution strategies for these two types of conflicts are as follows.

[0153] A general conflict refers to a conflict that does not cause robot n to take a long detour; that is, the robot can find another short, conflict-free path to avoid the conflict at that node. The solution to this conflict is to directly find a new, conflict-free path.

[0154] Critical conflicts are divided into critical detour conflicts and critical blocking conflicts. A critical detour conflict is one that causes robot n to take a long detour, but a solution still exists. A critical blocking conflict is one that directly renders robot n without a solution; this conflict must be resolved for robot n to have a solution. The approach to resolving critical conflicts is to eliminate local conflicts.

[0155] The steps to determine the criticality of a conflict are as follows:

[0156] Sub-step 1: Replanning the route to prepare for determining the type of conflict

[0157] Given the constraints of the current order table, reapply the improved A* algorithm to plan the path, and extract the length of the replanned path as L. pathN Make the following judgment:

[0158] Sub-step 2: Identify and resolve key conflicts

[0159] If L pathN If the value is 1, it means the robot has no solution after the conflict is introduced. This conflict is marked as a critical conflict, and the process proceeds to step five. If the value is greater than 1, it is determined whether the conflict leads to a long detour. A long detour is defined as being marked when the following conditions are met.

[0160]

[0161] In formula (4), N represents the side length of the map. When the length of the newly planned path is greater than the length of the original path by the square root of the side length and then rounded down, it is considered a long detour.

[0162] If a long detour occurs, mark the conflict as a critical conflict and proceed to step five. If no such conflict occurs, proceed to sub-step three.

[0163] Sub-step 3: No critical conflicts

[0164] If the conflict can be resolved by taking a short detour, save the planned path to the corresponding path table, modify the order table according to the path table, and return to step three sub-step two to continue the conflict judgment and resolution for the next robot.

[0165] Step 5: Prioritization during critical conflicts

[0166] Upon identifying a critical conflict, the process proceeds to local solution planning. This step prioritizes local solutions to facilitate conflict resolution. The location of the critical conflict and the time step (time) at which the conflict occurs are determined. The specific steps for prioritization are as follows:

[0167] Sub-step 1: Prerequisites

[0168] Ignore bots that haven't joined the discussion yet.

[0169] Sub-step two: Handling special cases

[0170] Check if any robots stop in place at time steps 'time' and 'time-1'. If so, place them at the end of the local priority list. Check which robots have not entered the 5×5 area centered on the conflict point at time step 'time', and place these robots at the end of the local priority list according to their priority order during global solution.

[0171] Sub-step 3: Local priority evaluation index

[0172] The remaining robots are analyzed, and a local priority evaluation index is defined as follows:

[0173]

[0174] In formula (5) I p This is a priority evaluation index for local planning; the smaller the value, the higher the priority.

[0175] In formula (5) L pathP The length of the robot's original planned path within a 5×5 area centered on the conflict point.

[0176] N in formula (5) path This refers to the expandable neighborhood surrounding the aforementioned path.

[0177] Calculate I for the remaining robot p The lower priority of the metric will be increased, and if they are the same, a random one will be assigned.

[0178] Based on the above rules, complete the priority planning for local conflict optimization, and proceed to step six.

[0179] Step Six: Resolve Key Conflicts

[0180] This step involves resolving key local conflicts and conducting potential priority games. Only robots added to the discussion in sub-step two of step three are considered in this step. The robots are numbered according to their priority order of completion in step five, as n1, n2…n m There are m robots participating in the discussion. The temporary reservation table is defined as `orderT`, initially empty. The specific operation steps are as follows:

[0181] Sub-step 1: Define the start and end points for handling critical conflicts.

[0182] The original planned positions of each robot at the time step preceding the conflict are set as their local starting positions. The original planned positions of each robot at the time step following the conflict in the existing robot paths are set as their local target positions. Local conflict resolution ends when all robots have reached their local target positions (not necessarily at the same time step). The initial value of robot index i is defined as 1.

[0183] Sub-step 2: Introduce the robots sequentially

[0184] Let robot n i Move to the local target location according to the path table, and check if it conflicts with the orderT table. If a conflict occurs, proceed to sub-step four. If not, save the path table. If a solution exists after a conflict occurs, continue to sub-step three; otherwise, mark it.

[0185] Sub-step 3: Determine if all robots that have entered the critical conflict handling stage have completed their processing. Determine if i is equal to m. If not, increment i and repeat sub-step 2.

[0186] Sub-step 4: Path replanning

[0187] Perform a local search on the robot. Under the constraints of the orderT table, expand the robot's current position and surrounding positions with time steps. This means the robot can perform five movements: up, down, left, right, and stationary. Add points that meet the expansion requirements to the open list with time steps. Add points from the open list to the close list with time steps. Check if there is a local target position in the close list. If so, backtrack the path according to the path backtracking rules. If not, repeat this step until the number of repetitions exceeds the search circuit breaker limit. If the search circuit breaker limit is exceeded, mark the solution as unsolvable and proceed to sub-step six.

[0188] Set the number of search circuit breakers to the number of map edges, which is N.

[0189] After the local search is completed, proceed to sub-step five.

[0190] Sub-step 5: Process the path after reaching the local target point

[0191] Using the method in step one, and under the constraints of the introduced order table, plan the robot's path from the local target point to its final target point. If a conflict occurs, plan the path along with the already introduced robots using the method in step two and related steps. Finally, modify the path table of the already introduced robots, and modify the orderT table based on the robot's path table as a reference for the subsequent local path planning of the robot, and return to sub-step two.

[0192] Sub-step six: Priority game when there is no solution

[0193] This sub-step involves prioritizing robots in local conflicts, raising the priority of the robot involved in the conflict in sub-step four to the highest level, and then proceeding to step six to resolve the conflict. If an infinite loop occurs, meaning that two robots always raise their priority to the highest level in turn, then the local conflict resolution is unsolvable. In this case, the robot is marked as having no solution, and the process returns to step two to redo the priority planning and conflict resolution.

[0194] Step 7: Save and display the path

[0195] Save the final path and display it on the map according to the path information.

[0196] Here is a specific example.

[0197] by Figure 4 Let's take the environment as an example to illustrate.

[0198] Preparation: A square grid map M containing obstacles is known, such as... Figure 4 As shown, the side length is 10, there are 3 robots, and the starting row and column indices of the robots are [6,9], [8,2], [1,3], and the target row and column indices are [9,7], [1,2], [9,1]. The robot colors are red, cyan, and green. The parameters w = 2 and t... p =0.3. All coordinates in the following text are defined as row and column indices.

[0199] Step 1: Plan the path for each robot using the improved A* algorithm under zero-constraint conditions. This example uses the first robot; the same applies to the second and third robots. The specific sub-steps are as follows:

[0200] Sub-step 1: Let the initial open table be [6,9,0,5,10,5,1], which means [initial row index, initial column index, actual distance traveled, g(n), Manhattan distance from the target point, h(n), the first two are calculated using the formula f(n), the relative position of the previous point, and the time step].

[0201] Sub-step 2: Expand the open table. Select the point with the smallest f(n) value from the open table and expand it. Taking the first time step of the first robot as an example, expand the four positions of up, down, left and right, namely [5,9], [7,9], [6,8], [6,10]. Only [7,9] meets the requirements because there is no obstacle at this point. Add the points that meet the requirements to the robot's open table according to the open table format.

[0202] Sub-step 3: Add the point with the smallest f(n) value in the open list to the close list. At the first time step, add the starting point to the close list. Repeat sub-steps 2 and 3 until the target point [9,7] is added to the close list. At this point, the number of rows in the open list is the path length L of the robot. path .

[0203] Sub-step four: Perform path backtracking using the close table to obtain the robot's path. In this example, the result of the first robot's path backtracking is shown in Table 1. Perform the above steps for all robots in sequence to obtain the path table for each robot.

[0204]

[0205] Table 1 shows the path obtained by Robot No. 1.

[0206] Step 2: Specify the robot's global conflict resolution priority. Calculate the indicators according to the formula, with the number of rows in the open table being [6, 9, 8] and the number of rows in the close table being [7, 29, 33], resulting in a priority order of [3, 2, 1].

[0207] Step 3: Perform improved path planning (A*) on the robots according to priority, and add the robots to their order table after planning is completed. If a conflict occurs, proceed to Step 4 for critical conflict judgment and resolution.

[0208] In this example, robot #3 (green) is introduced first. After introducing robot #3, there are no constraints in the order table, so robot #3 retains its original path. Next, robot #2 (cyan) is introduced. When moving along its original path, it will encounter a placeholder conflict with robot #3 at point [6,6] at time step 9. This leads to step four, where the conflict type is determined and resolved. After resolving the conflict, the paths of both robots are saved. Then, robot #1 (red) is introduced. When this robot moves along its planned path, it will not conflict with the order tables of other robots, so it is saved directly. At this point, the path planning for all robots is complete.

[0209] Step 4: A typical placeholder conflict occurs in this specific implementation scheme. The node it occupies affects whether the robot can solve the problem successfully, and it cannot be solved by simply adjusting the robot priority. For the conflict point [6,6], and the robot number [3,2] which has already been introduced for discussion, the conflict type is determined and resolved.

[0210] After introducing robot 3, a conflict occurred at [6,6]. Under the constraints of the order table, we performed improved path planning for A* and found that there was no solution for robot 3. This means that the conflict [6,6] is a critical conflict for robot 3. We then proceeded to step five to try to resolve this critical conflict.

[0211] Step 5: First, plan the priority for local solution. Based on the local priority rules mentioned earlier, set the priority to [3,2]. Define each robot's starting point as its position at the time step before the conflict occurs, and its target point as the time step after the conflict occurs on the original path. In this example, set robot 3's local starting point to [6,5] and its local target point to [7,6], and robot 2's local starting point to [7,6] and its local target point to [6,5]. Proceed to step 6 for path planning. The robot positions at time step 8 before the conflict occur are as follows: Figure 5 As shown, a collision will occur if the original path is followed.

[0212] Step Six: Introduce robots sequentially according to priority. When robot 3 is introduced, the `order` table is empty, so robot 3 can reach the local target point along its original path. Next, robot 2 is introduced. At this point, robot 2 cannot reach the local target point under the constraints of the `order` table. Therefore, path planning is performed on robot 2. The idea behind local path planning is to introduce robots that wait in place but are allowed to reach previously visited locations. The basic implementation method is still to expand the `open` and `close` tables until the target location is reached.

[0213] Sub-step 1: Expand the initial open table, which is initially [7,6,6,8], representing [the row index, column index, position information of the previous point (6 represents the starting position), and time step information of the point], respectively. Under the constraints of the order table, add the following positions that meet the requirements to the open table: [7,6,5,9], [8,6,2,9], [7,7,4,9], which respectively stop at time step 9 for one time step, move down, and move right.

[0214] Sub-step 2: Add all newly added nodes to the open table in the previous step to the close table. If a local target node appears in the close table, the local solution is considered successful, and proceed to sub-step 3. If not, repeat sub-step 1 for the newly added nodes in the close table, and continue until a path is found. To prevent unlimited searching due to a lack of solutions, the number of loop breaks is set to equal the number of edges N.

[0215] Sub-step 3: Backtrack the path based on the previous position information stored in the close table and save it. After local conflict resolution, add the local path to the original path, and plan the position after the conflict to the final target position according to the principle of step 3.

[0216] Figure 6 The demonstration shows that at time step 9, robot 2 performed an avoidance maneuver to ensure the passage of robot 3. Figure 7 The diagram shows the resolution of the local conflict between the two robots. Table 2 illustrates the paths of robots 2 and 3 after the local conflict was resolved. It can be seen that robot 2 performed active avoidance and stopped, successfully allowing robot 3 to pass through the obstacle, resolving the conflict, and making the problem solvable.

[0217] time step Path of Robot No. 2 Path of Robot No. 3 8 [7,6] [6,5] 9 [7,7] [6,6] 10 [7,7] [7,6] 11 [7,6] [8,6] 12 [6,6] [8,5] 13 [6,5] [8,4]

[0218] Table 2 Local Conflict Resolution Scheme

[0219] Step 7: Finally, all the robots introduced in Step 3 were successfully introduced, and three conflict-free paths were generated for each robot. The path planning is now complete.

[0220] Furthermore, the performance of this invention is compared with that of the classic CA* algorithm on the same map to verify its effectiveness:

[0221] The CA* algorithm prioritizes robots and introduces them sequentially, solving the problem with constraints defined by an order list. In this approach, robot 1 is solved first, successfully finding a path. Then, robot 2 is solved under the same order constraints. Since robot 3's initial position is on a path that robot 2 must traverse, robot 2 cannot provide a path. Next, robot 3 is introduced, and it can find a conflict-free path. The final path obtained by the CA* algorithm is as follows: Figure 8 As shown.

[0222] The algorithm proposed in this invention can resolve the critical conflict at point [6,6], allowing robot 2 to avoid the obstacle, so that robot 2 can reach its destination smoothly while robot 3 can pass through.

[0223] The above comparison clearly shows that the improved conflict resolution method can effectively increase the success rate of solving critical blockages.

Claims

1. A multi-robot cooperative path planning method for complex environments, characterized in that, Includes the following steps: Preparation steps, and subsequent execution steps. Step 1: Plan the shortest path for each robot individually. The rules stipulate that the paths of robots other than itself are not considered, and only the obstacles in the map are considered as zero constraints. The improved A* algorithm is used to try to plan a path under zero constraints for each robot, and save whether there is a solution under zero constraints, and the shortest path when there is a solution. Step 2: Assign global priority to the robot Priority determination for global collaborative path planning of robots Step 3: Introducing robots and conflict resolution Robots are processed sequentially according to priority to resolve conflicts. Step 4: Identify and classify conflicts to resolve them. The severity of conflicts is divided into two categories: general conflicts and critical conflicts. Step 5: Prioritization during critical conflicts Upon identifying a critical conflict, the process moves to local solution. This step prioritizes local solutions to facilitate conflict resolution. Step Six: Resolve Key Conflicts To resolve key local conflicts and engage in potential priority game theory; Step 7: Save and display the path Save the final path and display it on the map according to the path information; Step one includes Sub-step 1.1: Initialization of the robot The robot is initialized to facilitate path planning. The robot's open table under the A* algorithm is initialized to store the search area. This table stores the robot's starting position and evaluation function. f(n) The previous movement direction is initialized, and the robot's close table under the A* algorithm is used to store the areas that have been searched. This table stores the positions of the points searched by the robot and the previous movement direction. (1) In formula (1) f(n) It is the evaluation function in the traditional A* algorithm, representing the evaluation function from the starting point. S r Run to state node n That is, from the robot's current node to the target point G r The total estimated cost, In formula (1) g(n) It represents the actual cost from the starting point to the state node, that is, how far the distance actually traveled from the starting point to the state node. In formula (1) h(n) It represents the estimated cost from the state node to the target point, i.e., how far is expected to be from the state node to the target point. h(n) Using Manhattan distance (2) In formula (2) x S ,y S Indicates the coordinates of the target point. x n ,y n Represents the coordinates of the state node. In formula (1) w These are dynamic parameters used to measure the objective nature of the algorithm. w When the value of is equal to 0, the A* algorithm evolves into Dijkstra's algorithm, which performs a blind, untargeted search outwards from the starting point; when w When the value is less than 1, it means w × h (n) Comparison from state node n The smaller the actual cost of moving to the target, the larger w is, the fewer nodes the A* algorithm expands, resulting in a slower running speed. When the value of w equals 1, the algorithm searches for the optimal path without expanding other nodes. It's important to note that the A* algorithm only guarantees finding the shortest path when the value of w is less than or equal to 1. w When the value is greater than 1, the algorithm will place more emphasis on the distance to the endpoint when searching the surrounding area, which usually significantly reduces the search area. In formula (1) t p As a turning penalty, since the algorithm stipulates that the robot cannot backtrack, it can only turn left or right and move straight. When the robot turns left or right, it will be penalized. g(n) and h(n) Under the same conditions, robots tend to travel in straight lines rather than frequently turning; typically, t p Set it to slightly greater than 0; the goal is simply to differentiate between going straight and turning. Sub-step 1.2: Expand the open and close tables Traverse and search the `open` table, checking if it is empty. If the `open` table is empty, it means that no zero-constraint path can be found for the robot, mark it as having no solution, save the `close` table, and jump to sub-step 1.

3. If the `open` table is not empty, select a path from the `open` table. f(n) The point with the smallest value is designated as the state node. n Add the point to the `close` table, and check if point n is the target point. If it is, the robot has found a zero-constraint path, saves the `close` table, and jumps to sub-step 1.

4. If point n is not the target point, it means no path has been found yet, so a neighborhood search is performed on the point, the robot's `open` table is updated, and sub-step 1.2 is repeated until the current sub-step is exited. During the repeated execution, the `open` and `close` tables will be continuously updated and iterated. "Neighborhood search" is defined as: in the state node n The four neighboring nodes (top, bottom, left, and right) of the element are checked, and nodes that meet the requirements are added to the open list. The calculation is then performed and the list is saved. f(n),h(n),g(n),t p If the node already exists in the open table, then compare the two. f(n) Values, keep the smaller one. A "qualified node" is defined as: the node is located within the map area, there are no obstacles at that point, and it is not in the close table. Sub-step 1.3: Processing the unsolvable robot Sub-step 1.2 will jump to this sub-step if it determines that the robot has no solution. In this sub-step, some processing is performed on the robot with no solution, such as storing the robot's starting position in the path table and specifying the path length of the robot with no solution. L path If the value is 1, the robot will remain in its initial position and proceed to sub-step 1.

5. Sub-step 1.4: Perform path backtracking on the robot with a solution and provide the path. If substep 1.2 determines that the robot has a solution, jump to this substep. In this substep, backtrack the path, using the robot's target point as the search node. Search backward from the end of the close table to find which node the target point is derived from, and use this node as the new search node. Repeat this search process continuously. After each node is searched, store it in the path table from back to front. When the starting point is found, the path backtracking is considered complete, and the path length is defined. L path Given the row number in the path table, proceed to step seven. Sub-step 1.5: Determine whether all robots have planned their paths. Determine whether all robots have completed zero-constraint path planning, i.e., all robots are marked as having a solution or not having a solution. If all robots have completed path planning, proceed to step two. If there are still robots that have not planned a path, perform zero-constraint path planning for the next robot in sequence, starting from step one. Step four includes A general conflict refers to a conflict that will not cause robot n to take a long detour; that is, the robot can find another short, conflict-free path to avoid the conflict at that node. The solution to this type of conflict is to directly find a new, conflict-free path. Critical conflicts are divided into critical detour conflicts and critical blocking conflicts. A critical detour conflict is one that causes robot n to take a long detour, but a solution still exists. A critical blocking conflict is one that directly renders robot n without a solution; this conflict must be resolved for robot n to have a solution. The approach to resolving critical conflicts is to eliminate local conflicts. The steps to determine the criticality of a conflict are as follows: Sub-step 4.1: Replan the route to prepare for determining the type of conflict. Under the constraints of the current order table, the improved A* algorithm is used again to plan the path, and the length of the replanned path is extracted as follows. L pathN Make the following judgment: Sub-step 4.2: Identify and resolve key conflicts like L pathN If the value is 1, it means the robot has no solution after the conflict is introduced. This conflict is marked as a critical conflict, and the process proceeds to step five. If the value is greater than 1, it is determined whether the conflict leads to a long detour. A long detour is defined as being marked when the following conditions are met. (3) In formula (4), N represents the side length of the map. When the length of the newly planned route is greater than the original route length by the square root of the side length and then rounded down, it is considered a long detour. If a long detour occurs, mark the conflict as a critical conflict and proceed to step five. If no such conflict occurs, proceed to sub-step 4.

3. Sub-step 4.3: No critical conflict If the conflict can be resolved by taking a short detour, save the planned path to the corresponding path table, modify the order table according to the path table, and return to sub-step 3.2 to continue the conflict judgment and resolution for the next robot. Step five includes The specific steps for priority planning are as follows: Identify the location of the critical conflict and the time step (time) at which the conflict occurs. Sub-step 5.1: Prerequisites Ignore the bots that haven't joined the discussion yet. Sub-step 5.2: Handling special cases Check if any robots stop in place at time steps 'time' and 'time-1'. If so, place them at the end of the local priority list. Check which robots have not entered the 5×5 area centered on the conflict point at time step 'time', and place these robots at the end of the local priority list according to their priority order during global solution. Sub-step 5.3: Local priority evaluation index The remaining robots are analyzed, and a local priority evaluation index is defined as follows: (4) In formula (5) I p This is a priority evaluation index for local planning; the smaller the value, the higher the priority. In formula (5) L pathP The length of the robot's original planned path within a 5x5 area centered on the point of conflict. In formula (5) N path The extended neighborhood surrounding the aforementioned path. Calculation of the remaining robots I p The lower priority of this metric will be increased; if they are the same, a random metric will be assigned. Based on the above rules, complete the priority planning for local conflict optimization, and proceed to step six.

2. The multi-robot cooperative path planning method for complex environments as described in claim 1, characterized in that: Preparation steps include A square grid map with obstacles is known. M Its side length is N The number of robots is R The starting points of each robot are respectively S r The target point is G r ,in r =1,2,…, R .

3. The multi-robot cooperative path planning method for complex environments as described in claim 2, characterized in that: Step two includes Sub-step 2.1: Give priority indicators For a robot with a solution, the number of rows in the open table is defined as follows when it finds the target point during step one. N open The length of the close table in the same state is N close The priority evaluation indicators are specified as follows: (5) This indicator I This reflects the difficulty level of the robot's path search. I A larger value indicates that the robot finds a path more easily, meaning it's more likely to find another feasible path. To improve the success rate of path planning, let... I Smaller robots have higher priority, meaning the algorithm will prioritize ensuring the passage of robots that are more difficult to find a path before coordinating with robots that are more likely to find another path. If the metrics are exactly the same, the priority is randomly assigned. Sub-step 2.2: Handling special cases Set all robots with no solution to the lowest priority, and randomly assign priorities among the robots with no solution.

4. The multi-robot cooperative path planning method for complex environments as described in claim 3, characterized in that: Step three includes Sub-step 3.1: Initialize the order table Initialize an empty `order` table. This table is created based on the longest time step of all robots. Each time step stores the current position of each robot. The rule is that all robots start simultaneously at time step 1, and all robots reach the target position at the longest time step. The stipulation "planning a path under the constraint of an order table" means that the robot will plan its path under the combined constraints of the map's built-in obstacles and the order table. The introduction of the order table constraint may lead to two types of conflicts: placeholder conflicts and opposing conflicts. Placeholder conflicts occur when two robots reach the same node at the same time step. The method for determining this is to check if the next node that robot n will reach already exists in the order table for that time step. Opposing conflicts occur when two robots swap positions in two adjacent time steps. The method for determining this is to check if the node reached by robot n at this time step exists in the order table for the next time step, and whether the position of robot m at the previous time step is the same as the position of robot n at the next time step. Sub-step 3.2: Introduce robots sequentially and resolve conflicts. Introduce robots in order of priority and guide them to follow the planned paths. If a conflict occurs, proceed to step four. If all newly introduced robots do not conflict with the order table, populate the order table with the path table of that robot and continue introducing new robots in order of priority until all robots have been planned. Once the path planning is complete, proceed to step seven to save and display the results.

5. The multi-robot cooperative path planning method for complex environments as described in claim 4, characterized in that: Step six includes In this step, only the robots discussed in sub-step 3.2 are considered. The robots are numbered according to their priority order in step five, as n1, n2…n m There are m robots in the discussion. The temporary reservation table is defined as `orderT`, initially empty. The specific operation steps are as follows: Sub-step 6.1: Define the start and end points for handling critical conflicts. The original planned positions of each robot at the time step preceding the conflict are set as their local starting positions. The original planned positions of each robot at the time step following the conflict in the existing robot paths are set as their local target positions. Local conflict resolution ends when all robots have reached their local target positions. The initial value of robot index i is defined as 1. Sub-step 6.2: Introduce the robot sequentially. Let robot n i Move to the local target location according to the path table, and check if it conflicts with the orderT table. If a conflict occurs, jump to sub-step 6.4; otherwise, save the path table. If a solution exists after a conflict occurs, continue to sub-step 6.3; otherwise, mark it. Sub-step 6.3: Determine if all robots involved in critical conflict handling have completed their processes. Check if i equals m. If not, increment i and repeat sub-step 6.

2. Sub-step 6.4: Path replanning Perform a local search on the robot. Under the constraints of the orderT list, expand the robot's current position and surrounding positions with time steps. That is, the robot can perform five movements: up, down, left, right, and stationary. Add points that meet the expansion requirements to the open list with time steps, and add points in the open list to the close list with time steps. Check if there is a local target position in the close list. If there is, backtrack the path according to the path backtracking rules. If not, repeat this step until the number of repetitions exceeds the search circuit breaker limit. If the search circuit breaker limit is exceeded, mark it as having no solution and jump to sub-step 6.

6. Set the number of search circuit breakers to the number of map edges. After the local search is complete, proceed to substep 6.

5. Sub-step 6.5: Process the path after reaching the local target point. Using the method in step one, and under the constraints of the introduced order table, plan the robot's path from the local target point to its final target point. If a conflict occurs, plan the path along with the already introduced robots using the method in step two and related steps. Finally, modify the path table of the already introduced robots, and modify the orderT table based on the robot's path table as a reference for the subsequent local path planning of the robot, and return to sub-step 6.

2. Sub-step 6.6: Priority game when there is no solution This sub-step involves prioritizing robots in local conflicts. The robot whose conflict occurred in sub-step 6.4 is prioritized to the highest level, and the conflict resolution in step six is ​​performed again. If an infinite loop occurs, i.e., two robots always prioritize to the highest level in turn, then the local conflict resolution is unsolvable. The robot is marked as unsolvable, and the process returns to step two to redo priority planning and conflict resolution.

Images