Scalable Multi-Robot Motion Planning for Congested Environments With Topological Guidance

Multi-robot motion planning (MRMP) is the problem of finding collision-free paths for a set of robots in a continuous state space. The difficulty of MRMP increases with the number of robots and is exacerbated in environments with narrow passages that robots must pass through, like warehouse aisles where coordination between robots is required. In single-robot settings, topology-guided motion planning methods have shown improved performance in these constricted environments. In this work, we extend an existing topology-guided single-robot motion planning method to the multi-robot domain to leverage the improved efficiency provided by topological guidance. We demonstrate our method's ability to efficiently plan paths in complex environments with many narrow passages, scaling to robot teams of size up to 25 times larger than existing methods in this class of problems. By leveraging knowledge of the topology of the environment, we also find higher-quality solutions than other methods.


I. INTRODUCTION
Multi-robot systems have become ubiquitous in many settings such as autonomous factories and warehouses.When the motions of two or more robots are in conflict, highly coordinated multi-robot motion planning (MRMP) is required to avoid collisions with each other and with the environment.
Existing MRMP approaches perform well in open environments but struggle with narrow passages (e.g.warehouses aisles) due to the difficulty of avoiding collisions with obstacles within tight spaces.With multi-robot teams, narrow passages may be introduced or exacerbated by inter-robot collisions, further complicating planning.There are three classes of MRMP approaches: decoupled, which plan a path for each robot separately and offer the least amount of coordination, coupled, which plan in the composite space, which incorporates the degrees of freedom of all robots, and hybrid, which offer a mix of coupled and decoupled planning.Decoupled approaches (e.g., Decoupled PRM [1]) allow for linear scaling with the number of robots; however, they lack the coordination necessary to resolve complex inter-robot collisions.Coupled methods (e.g., Composite RRT [2] and Composite PRM [1]) provide this coordination but consider a large search space that becomes computationally intractable for large teams.Hybrid methods (e.g., CBS-MP [3] and M* [4]) leverage the scalability of decoupled methods along with an increased level of coordination.However, in environments with narrow passages where interrobot collisions are likely, their performance is limited by the time spent resolving conflicts.In this work, we propose a coupled method that exploits topological guidance to more intelligently and efficiently search the composite space.
Guided planning methods leverage external information to efficiently find paths.Topology-guided methods [5]- [7] exploit representations of the environment to direct motion planning through narrow passages.Topological skeletons are embedded graphs encoding the environment.Prior work [5], [6] has explored guiding planning around a skeleton.
In this work, we extend topological skeleton guidance to multi-robot systems to provide the level of coordination required for large teams in environments with narrow passages.We present Composite Dynamic Region-biased Rapidly-exploring Random Trees (CDR-RRT), a coupled MRMP approach that leverages knowledge of the workspace topology, leading to improved planning times while retaining probabilistic completeness.We demonstrate significantly improved scalability compared to existing state-of-the-art methods, successfully finding paths for teams of size up to 25 times larger in complex environments where inter-robot collisions are likely.Our contributions include: • The development of novel composite-space analogs of workspace skeletons and sampling regions.• A scalable, probabilistically complete MRMP method that leverages topological guidance to address problems that require high levels of coordination.• An experimental validation in a variety of congested and open environments with scaling robot team sizes.

II. PRELIMINARIES AND RELATED WORK
In this section, we discuss the motion planning problem and research in both the multi-robot motion planning and guided motion planning domains.

A. Motion Planning Preliminaries
A robot's degrees of freedom (DOFs) include its position and orientation in the workspace, the 2D or 3D space within which the robot physically exists, as well as other configurable values such as joint angles.A configuration is a set of values describing the robot's DOFs.The configuration space (C space ) is the set of all robot configurations [8].The free space (C f ree ) is the subset of C space that only contains valid configurations (e.g.configurations not in collision with obstacles).The obstacle space (C obst ) contains all configurations that are not valid.Given a start configuration, q start , and a goal configuration, q goal , the motion planning problem strives to find a path from q start to q goal through C f ree .
Searching the entire C space is intractable [9], [10], resulting in the emergence of sampling-based motion planning algorithms [2], [11].These algorithms forego completeness guarantees in favor of faster planning and probabilistic completeness, meaning that the probability of finding a solution, if one exists, converges to 1 in the upper limit of planning time.Unfortunately, these randomized sampling techniques suffer in constrained environments [12], where they face the narrow passage problem.This refers to the difficulty of sampling valid configurations within narrow corridors that by volume make up a small proportion of the freespace.
The underlying sampling-based motion planning algorithm that forms the basis of our method is Rapidly-exploring Random Trees (RRT) [2].This method iteratively grows a tree, T , from q start to q goal .During each iteration, a random configuration q rand is sampled.We then find q near , the configuration in T closest to q rand .T is then extended a maximum distance ∆ from q near in the direction of q rand .RRTs exhibit a Voronoi bias that results in the rapid exploration of the C space and makes RRTs an efficient way to handle single-query motion planning problems.RRT variants have been developed to improve performance in the presence of narrow passages [13], [14].

B. Multi-robot Motion Planning
Multi-robot motion planning consists of finding valid paths for a set of robots between their respective starts and goals.The composite space is the Cartesian product of each of the n individual robots' C spaces : where C i represents the C space of robot i.A composite configuration consists of values for each robot's DOFs.The composite free space is made up of all valid configurations such that no robot is in collision with another robot.The MRMP problem can be formulated as finding a continuous path through the composite free space.
Table I compares select MRMP approaches.Decoupled approaches such as Decoupled PRM [1] plan individual robot paths in their own decoupled C spaces and thus do not offer completeness or optimality guarantees.The lack of coordination degrades their performance in narrow passages, where inter-robot collisions may not be possible to avoid along the individual paths only using velocity tuning.
Coupled methods such as Composite PRM [1] and Composite RRT [2] plan directly within the composite C space .

Algorithm
Coordination Scalable Probabilistic Completeness Narrow Passages Decoupled PRM [1] Decoupled X Composite RRT [2] Coupled X Composite PRM [1] Coupled X MRdRRT [15] Coupled X X CBS-MP [3] Hybrid X X MAPF/C [16] Hybrid Other composite methods (e.g., MRdRRT [15]) build individual robot roadmaps, then search an implicit composite roadmap.These methods maintain the probabilistic completeness of the single-robot methods they use.However, due to the decoupled individual roadmap construction, they lack the level of coordination required to efficiently find paths in congested environments with narrow passages.Hybrid methods such as CBS-MP [3], MAPF/C [16], and M* [4] seek to leverage the strengths of both coupled and decoupled methods.For example, CBS-MP [3] plans individual robot paths in their decoupled C spaces and then reconciles the paths in the composite C space .In the worst case, these methods will explore the whole composite C space , but on average, the runtimes are comparable to decoupled methods while providing varying levels of probabilistic completeness and representation optimality guarantees.These methods are generally not well suited for environments with narrow passages due to the computational effort expended transitioning the planner from decoupled to the high level of coordination required.Here, we propose a method for composite-space RRT construction while leveraging topological guidance to improve performance in environments with narrow passages.

C. Guided Motion Planning
Topological guidance has not been well explored in the composite space; however, some hybrid methods have explored using topological information to construct decoupled roadmaps.Ryan [17] decomposes the workspace into halls, represented by singly-linked chains of vertices, and open spaces, represented by fully-connected subgraphs.Yu et al. [18] propose a method for roadmap construction via overlaying a lattice structure onto the workspace.They show that this method leads to efficient path planning for large groups of robots in relatively open environments.
Several single-robot motion planning strategies have been proposed to adapt planning to the workspace.The Feature Sensitive Motion Planning Strategy [19] attempts to subdivide the environment into homogeneous workspace regions that are planned in individually, adapting roadmap construction to local features.The individual roadmaps are merged into a complete roadmap of the planning space.This strategy allows the planner to use resources efficiently.
Workspace Decomposition Strategies [20]- [22] help concentrate planning in narrow areas of the workspace.SyClop [23] uses an RRT to sample frontier decomposition regions.A User-Guided Planning Strategy [24] allows the user to define and manipulate workspace sampling regions that the  REGIONBIASEDRRTGROWTH(T, S, R) 7: return T planner explores in real-time.The planner relies on the user's intuition to identify narrow passages and find paths faster.
Skeleton-based strategies leverage the topology of the workspace using an embedded graph (Fig. 2(a)) that is homotopy equivalent to the workspace.All points in the workspace can be smoothly collapsed to the skeleton [25].Skeleton edges describe contiguous volumes of the free workspace (e.g., tunnels or rooms) and vertices represent connections between these volumes.Given the environment, the skeleton is precomputed and may be used for multiple queries and different types of robots.Examples include medial axis skeletons [26] in 2D and mean curvature skeletons [27] in 3D.Skeletons are generally quick to compute.The medial axis skeleton, for example, can be computed in O(n log n) time where n is the number of obstacle edges [28].
Dynamic Region Sampling with PRM (DR-PRM) [6], initiates local components at the vertices of a skeleton, expands them along adjacent edges, then merges them to form a complete roadmap.Hierarchical Annotated Skeleton Planning [29] extends DR-PRM by relaxing its reliance on skeleton edges over time.We describe the single-query counterpart of DR-PRM, Dynamic Region-biased RRT (DR-RRT), in detail in Section II-D since we extend our method from it.These methods show the advantage of using workspace information to guide planning in C space when they are closely related; however, they are constrained to single-robot settings.

D. Dynamic Region-biased RRT
DR-RRT [5] (Alg. 1) grows an RRT while constraining sampling within regions that advance along a skeleton.
1) Query Skeleton: Algorithm 1 creates two skeletons: one for the entire workspace (line 1), and the query skeleton (Fig. 2(b); line 2), which retains only edges that are along a path from the start to the goal in the workspace.

Algorithm 2 Region-biased RRT Growth
Input: Tree T , Query Skeleton S, Regions R, region radius η, maximum for failed extension attempts τ 1: r ← SELECTREGION(R) 2: q rand ← r.GETRANDOMCFG() 3: qnear ← NEARESTNEIGHBOR(T, q rand ) 4: qnew ← EXTEND(qnear, q rand , ∆) 2) Sampling Regions: Sampling regions will advance along the edges of the query skeleton, using the skeleton, a solution in the workspace, to guide construction of an RRT, a solution in the C space .A region is a bounded volume in the workspace, e.g., a bounding sphere.Construction of the RRT begins by initializing the first region centered on the skeleton vertex closest to q start (line 4).During each iteration, Algorithm 2 selects a region to guide sampling (line 1).The probability of selecting a region is proportional to its extension success rate.To maintain probabilistic completeness, with a small probability, the entire environment is chosen (see Sec. III-E).A random configuration q rand is selected from this region to grow the tree toward (line 3).The algorithm then proceeds as a general RRT by attempting to extend the tree to q new (line 4).The extension success rate is updated based on the outcome of this attempt (lines 7-10).
3) Region Advancement: Once a configuration has been added to the tree, all regions that are in contact with q new are advanced forward along their skeleton edges until they leave q new behind (Alg.2, lines 11-13).Any region that reaches the end of its edge or exceeds the maximum number of extension failures is deleted (lines 14-17).Then, new regions are created on each unexplored skeleton vertex that is within a small distance of q new (lines [18][19][20][21].This cycle of region selection, tree extension, region advancement, and region creation continues until the tree extends to q goal .

III. COMPOSITE DYNAMIC REGION-BIASED RRT
In this paper, we extend DR-RRT to multi-robot systems to propose a new method, Composite Dynamic Region-biased RRT (CDR-RRT).We limit the exploration of the composite space to areas that are likely to yield a solution because of the exponential size of the search space.We do this by developing composite analogs for workspace skeletons and An example of a composite edge and a composite region.Each of the three robots has its own workspace skeleton.Obstacles are gray, and the workspace skeletons are purple.Three skeleton edges that comprise a composite edge are shown in blue.All possible combinations of such edges make up the composite skeleton.Three individual regions that make up a composite region along this composite edge are shown in green.

Algorithm 3 Composite Region-biased RRT Growth
Input: Tree T , Composite Skeleton S, Region r, Failed Vertex and Edge Constraints C, Maximum for Failed Extension Attempts τ 1: Bound ← SELECTREGIONORWHOLEENVIRONMENT() 2: q rand ← r.GETRANDOMCOMPOSITECFG(Bound) 3: qnear ← NEARESTNEIGHBOR(T, q rand ) 4: qnew ← EXTEND(qnear, q rand , ∆)  regions that allow for coupled multi-robot motion planning while leveraging topological guidance as in DR-RRT.We leverage lazy construction of the composite skeleton as we exploit a greedy heuristic to search the composite space.

A. Composite Skeleton
As the composite space is the Cartesian product of each robot's C space , the composite skeleton (Fig. 3) is the Cartesian product of the workspace skeleton for each of the n robots.It consists of composite vertices and edges which respectively represent a set of n vertices or edges in the workspace skeleton where each of the n robots lies.We avoid the exponential expansion associated with the computation of the full composite skeleton graph by using local, on-demand construction.A composite region is made up of n individual sampling regions, one in each robot's workspace.
Computing a composite query skeleton, which is a directed and pruned version of the composite skeleton, requires an explicit computation of the composite skeleton.Instead, we

Algorithm 4 Grow Composite Skeleton
Input: Composite Skeleton S, Composite Source Vertex V , Failed Vertex and Edge Constraints C 1: while V.EXCEEDEDGROWTHATTEMPTS()

2:
V ← V.GETPREDECESSOR() 3: P aths ← MAPFSOLUTION(V, C) 4: E ← EXTRACTFIRSTCOMPOSITEEDGE(P aths) 5: S.ADDEDGE(E) 6: return NEWREGION(E) q new heuristically construct and search the composite skeleton one edge at a time and only consider edges likely to be along a feasible low-cost path from q start to q goal .We discuss our heuristic to capture these edges in Section III-C.

B. Guided Composite RRT Construction
To begin RRT construction, we compute the first composite skeleton edge to explore.Section III-C discusses the construction of composite skeleton edges by growing the composite skeleton from a source composite vertex.CDR-RRT then proceeds as DR-RRT by iteratively performing region-biased sampling, RRT growth, region advancement and deletion, and new region creation until q goal is reached.
During each iteration of CDR-RRT, Algorithm 3 selects a composite region r.After sampling from r and extending the tree, we advance r forward until it leaves q new behind (line 12).In DR-RRT [5], as individual regions advance along skeleton edges, they are centered on intermediate points along the edges.Correspondingly, we create composite intermediates along composite skeleton edges.In composite region advancement, as shown in Fig. 4, all individual regions are advanced forward the minimum amount of intermediates such that the composite region is no longer touching q new .When a composite region reaches the target vertex at the end of its edge, we add an outgoing edge to the composite skeleton from this vertex and spawn a new region (Alg.4).When a region surpasses τ failed extension attempts, it is deleted and replaced with a new region (line 22).

C. Multi-agent Pathfinding Heuristic
We use a multi-agent pathfinding (MAPF) heuristic to identify the next edge to explore given a source composite vertex, V (Alg.4).MAPF is the discrete state space equivalent of the MRMP problem.We use MAPF to generate a path for each robot through the workspace skeleton from V to the vertex closest to each robot's goal (line 3).We ensure that these individual paths are feasible by accounting for potential collisions between robots.We define the capacity of an individual skeleton edge as the minimum width between obstacles along the edge.If the total width of the robots traversing that edge exceeds the capacity, a conflict has occurred.These conflicts are resolved by the MAPF algorithm.
We extract composite skeleton edges from the produced MAPF solution (Alg.4, line 4) and iteratively create a region to traverse each edge.If a region exceeds the maximum number of extension failures traversing an edge, we consider that edge failed and impose a constraint that no further MAPF solutions can contain that composite skeleton edge (Fig. 5(a)).We also increment the number of failed growth attempts that each source composite skeleton vertex has seen.To avoid spending excess computation exploring a region of the composite skeleton that is unlikely to produce a path, if a vertex exceeds the maximum number of growth failures, we backtrack to its predecessor vertex (Fig. 5(b); line 2).

D. Implementation Details
To generate MAPF solutions, we adapt Conflict-Based Search (CBS) [30] and Priority-Based Search (PBS) [31].Both use a hierarchical approach with a low-level search to find individual paths for each agent and a high-level search to resolve conflicts between paths.CBS finds optimal paths with respect to the makespan while PBS has been shown to achieve improved performance in scenarios where the optimal path for one robot blocks the path for other robots.
The size of the full composite skeleton is exponential in the number of robots, so we optimize memory usage by leveraging local construction of the composite skeleton.We can also remove composite vertices and edges when they are no longer useful.A composite edge is no longer useful when the composite region that traverses it has reached the end and been deleted.A composite vertex is no longer useful when all of its incoming and outgoing edges are no longer useful.

E. Theoretical Analysis
Theorem: CDR-RRT is probabilistically complete.Proof: During each iteration of CDR-RRT, there is a probability ϵ > 0 of sampling from the entire environment rather than within a region (Alg.3, line 1).Sampling from the entire environment guarantees probabilistic completeness, ensuring that a valid path from q start to q goal will be found, if one exists, even if all regions are unable to produce valid configurations.As ϵ increases to 1 or as the size of regions is increased to encompass the entire workspace, knowledge of the workspace topology is utilized less and the method eventually reduces to Composite RRT.

IV. VALIDATION
We run scaling MRMP queries in environments designed to highlight the strengths and weaknesses of our approach.We consider both environments with different narrow passage widths and open environments to measure how CDR-RRT compares to other state-of-the-art methods when the workspace is and is not informative.We measure each algorithm's performance in scenarios that require various levels of coordination during planning to demonstrate our improved performance when high coordination is required.

A. Experimental Setup
We compare to several state-of-the-art MRMP methods (Table I).We use CBS-MP [3] with DR-PRM [6] to construct the individual roadmaps as a comparison against a hybrid  method with workspace guidance.We use the implementation of MAPF/C described in [16] with SPARS [32] roadmap generation.Although MRdRRT [15] was designed primarily for manipulators, we compare to it since its use of a tensorproduct roadmap to conduct an RRT-style search of the composite space is similar to our composite skeleton guidance.We pre-compute medial-axis skeletons for 2D environments and mean curvature skeletons for 3D environments.
All methods were implemented in C++ in the Parasol Planning Library.The experiments were run in simulation with holonomic mobile robots using a desktop computer with an Intel Core i9-10900KF CPU at 3.7 GHz and 128 GB of RAM.Each method is given 600 seconds to find a plan or is considered unsuccessful.We report planning times and average path costs given by the makespan.

B. Environments
In this section, we describe our experimental environments and explain why these scenarios highlight the advantages and disadvantages of our approach relative to other methods.
1) Corridors (Fig. 6(a)-6(d)): We consider the Hallway environment featuring a single tunnel within which only one robot may fit vertically, preventing robots from passing each other.We consider two variants of this scenario, one in which Those that were unable to find a solution within the time limit are omitted.We demonstrate improved scalability as compared to the other methods by consistently achieving low planning times even as the number of robots increases.On the larger (4 and 6-robot) scenarios with narrow passages, CDR-RRT achieves the lowest running time.
two groups of robots start on opposite ends of the tunnel and swap places (Cross, Fig. 6(a)) and one in which all robots start on one side and must move to the other (Flow, Fig. 6(b)).In the Inlet scenario (Fig. 6(c)), we show how each method performs when one robot must explicitly move out of the other's way, requiring a high level of coordination.Similarly, in the Track scenario (Fig. 6(d)), we show how each method performs when all robots must move in the same direction around an obstacle, again requiring a high level of coordination, but for larger robot groups.
2) Warehouse (Fig. 6(e)): The Warehouse scenario is designed to imitate the motions required to fetch or place items on shelves in a warehouse.The topology creates several parallel narrow passages, and queries are selected such that conflicting choices of aisles are likely, thus requiring coordination during planning to avoid inter-robot collisions.
It includes a width-wise aisle cutting through the middle of the length-wise aisles creating entry/exit points that can be used to avoid collisions.We scale the number of aisles with the number of robots.We consider three variants with progressively doubled aisle widths.
3) Open Cross (Fig. 7(a)): We evaluate our approach on a classic open MRMP scenario [3] to demonstrate how our method compares when the workspace is not informative, limiting the benefit of topological skeleton guidance.
4) Maze Cross (Fig. 7(b)): In this 3D environment with a narrow maze tunnel, the number of degrees of freedom of each mobile robot is doubled relative to a 2D workspace, resulting in a very large composite space.

C. Narrow Passages
The Hallway, Inlet, and Track scenario results are given in Fig. 8.In the Hallway scenario, CDR-RRT's use of skeleton guidance allows it to find considerably lower cost paths than other methods in both variants of the problem, especially with the larger 4 and 6-robot groups.Composite RRT was able to solve the 6-robot scenarios but with significantly decreased success rates and higher path costs.MRdRRT and MAPF/C were unable to find solutions within the time limit.
Considering the Inlet scenario, runtimes for CDR-RRT, Composite RRT, Composite PRM, and MAPF/C were similar; however, the solution quality varies greatly between the methods.CDR-RRT has an average path cost of 15.35s, while Composite RRT, Composite PRM, and MAPF/C have average path costs of 22.31s, 22.13s, and 25.30s respectively.By sampling along skeleton edges, CDR-RRT finds more direct, lower-cost paths.CBS-MP and MRdRRT were unable to find solutions.We also demonstrate our method on physical robots (Fig. 1).We use Turtlebot3s and integrate ROS with our planning library to follow paths generated by CDR-RRT.
In the Track environment, only CDR-RRT is able to find a solution to the 6-robot scenario due to the difficulty of sampling paths where each robot moves in the same direction.CDR-RRT's use of a MAPF heuristic to find feasible paths over the composite skeleton allows it to efficiently recognize the robots must move either clockwise or counterclockwise.None of the other methods was able to achieve over a 7% success rate on the 4-robot scenario.Decoupled PRM was unable to find a solution for any scenario.
The Warehouse scenarios demonstrate each method's performance in with varying widths of narrow passages (results in Fig. 9).CDR-RRT achieves the lowest planning time on all scenarios and its use of skeleton guidance allows it to scale to the more complex 4 and 6-robot scenarios, where the performance of other methods significantly degrades.Decoupled PRM was unable to complete any scenario.

D. Scalability
We ran up to 100-robot Track scenarios with an 1800second time limit for CDR-RRT to measure its performance on larger problems.Fig. 10 shows that CDR-RRT is able to efficiently find paths for very large robot teams when a high level of coordination is required.We also show that CDR-RRT's MAPF heuristic evaluation maintains scalability.

E. Robot Crossings
The Open Cross and Maze Cross environments evaluate each method's performance in different robot cross scenarios, in which topological guidance provides varying levels of benefit.The Open Cross environment results are in Fig. 11(a).When the topology is not useful, workspace guidance still biases robot paths along skeleton edges, which, in an environment without obstacles, increases the potential for collision relative to Composite RRT and Composite PRM.As a result, CDR-RRT has a higher average planning time than Composite RRT and Composite PRM.This shows that composite skeleton guidance is most effective when there are narrow passages that robots must pass through.
In the Maze Cross scenarios (Fig. 11(b)), CDR-RRT and CBS-MP are the only methods able to plan for 4 robots.Only CDR-RRT is able to plan for 6 robots with 100% success within the time limit (CBS-MP -33%).Decoupled PRM failed to solve the 2-robot scenario.In 3D environments, the size of the composite C space increases significantly, boosting the impact of composite skeleton guidance.

V. CONCLUSION AND FUTURE WORK
We present Composite Dynamic Region-biased Rapidlyexploring Random Trees, a scalable workspace-guided multirobot motion planning approach.We validate our method on a variety of environments, with and without narrow passages, to demonstrate its strengths and weaknesses.We show improved performance in constricted environments.Future work will explore the use of composite skeleton guidance for PRM-based roadmap construction, expanding its utility to multi-query scenarios, as well as extending skeleton guidance to non-holonomic robot teams.

Fig. 2 .Algorithm 1
Fig.2.An example workspace skeleton (a) and query skeleton (b).The query skeleton is a directed and pruned skeleton that only contains edges along a path from qstart to q goal .
Fig.3.An example of a composite edge and a composite region.Each of the three robots has its own workspace skeleton.Obstacles are gray, and the workspace skeletons are purple.Three skeleton edges that comprise a composite edge are shown in blue.All possible combinations of such edges make up the composite skeleton.Three individual regions that make up a composite region along this composite edge are shown in green.

Fig. 4 .
Fig. 4.An example of composite region advancement for two point robots in a 1D environment.The individual robots' skeletons are shown on the axes and the composite skeleton them.Each individual roadmap is shown in blue and the composite roadmap is in purple.Edge intermediates are shown as tick marks along the skeletons.Both individual regions advance at the same rate with respect to their individual intermediates until the composite region leaves qnew behind at the position shown in dark green.

Fig. 5 .
Fig. 5.An example of the MAPF replanning process.(a) If a composite skeleton edge (in red) cannot be traversed, we impose a constraint that future MAPF solutions cannot contain that edge and replan the paths from the last vertex reached.(b) If the maximum number of failed outgoing edges from a vertex is exceeded, a constraint is imposed that future MAPF solutions cannot contain that vertex, and paths are replanned from its predecessor.

Fig. 6 .Fig. 7 .
Fig. 6.The Cross (a) and Flow (b) scenarios feature a single hallway through which all robots must pass.In Cross, the robots on either side must swap places.In Flow, all robots must move from one side of the hallway to the other.In the Inlet (c) scenario, the robots must swap places.One robot must move into the inlet to allow the other to pass.The track environment (d) features a ring through which robots must move.The robots on top and bottom must swap places by moving in the same direction to avoid collision.The Warehouse scenario (e) has three variants of aisle widths (1, 2, and 4 meters; the 1m variant is shown).The robots on top and bottom must swap places with the robot with which they are aligned vertically.

Fig. 8 .
Fig. 8. Running time and path cost results for the Hallway (a), Inlet (b), and Track (c) environments.

Fig. 9 .
Fig. 9. Running time and path cost results for the Warehouse scenarios.

Fig. 10 .
Fig. 10.Running time results for up to 100 robots for CDR-RRT on the Track Environment including heuristic evaluation times.

Fig. 11 .
Fig.11.Results for the Open Cross (a) and Maze Cross (b) environments for all methods.Those that were unable to find a solution within the time limit are omitted.We demonstrate improved scalability as compared to the other methods by consistently achieving low planning times even as the number of robots increases.On the larger (4 and 6-robot) scenarios with narrow passages, CDR-RRT achieves the lowest running time.