Quick Multi-Robot Motion Planning by Combining Sampling and Search

1 Introduction

Solving a multi-robot motion planning (MRMP) problem within a realistic timeframe plays a crucial role in the modern and coming automation era, including fleet operations in warehouses [Wurman et al., 2008] as well as collaborative robotic manipulation [Feng et al., 2020]. Nevertheless, the problem is known to be tremendously challenging even with simple settings [Spirakis and Yap, 1984; Hopcroft et al., 1984; Hearn and Demaine, 2005]. Filling this gap is a key milestone in automation.

Informally, MRMP aims at finding a collection of paths for multiple robots. Those paths must be obstacle-free, and also, inter-robot collision-free. Single-robot motion planning itself is intractable in general [Reif, 1979], furthermore, the specific difficulty of MRMP comes from the second condition. As a result, most studies on MRMP decouple the problem into (i) how to find obstacle-free paths and (ii) how to manage inter-robot collisions on those paths. This decoupling takes the form of two-phase planning that first constructs roadmaps and then performs multi-agent search, finding collision-free paths on those roadmaps. Here, a roadmap is a graph that approximates the workspace for one robot and is carefully constructed to include obstacle-free paths from initial to goal states. We can see two-phase planning examples in [ˇSvestka and Overmars, 1998; Solovey et al., 2016; H¨onig et al., 2018; Solis et al., 2021], to name just a few.

∗Most work done when Keisuke Okumura was a Ph.D. student at Tokyo Institute of Technology.

Contribution. In contrast to approaches based on twophase planning, we propose a novel MRMP algorithm that simultaneously performs roadmap construction and multiagent search. The crux is developing robot-wise roadmaps as necessary according to the multi-agent search progress. Doing so keeps the search space small, leading to quick MRMP solving. The proposed algorithm, called Simultaneous Sampling-and-Search Planning (SSSP), guarantees to eventually find a solution for solvable instances. Although SSSP outputs sequential solutions such that at most one robot moves at a time, it is possible to post-process known solutions to remove redundant motion and waiting time, as presented in this paper. Our extensive evaluations on various scenarios with diverse degrees of freedom and kinematic constraints demonstrate that SSSP significantly outperforms standard approaches to MRMP, i.e., solving more problem instances much faster. We also provide a planning demo with 32 ground robots (64-DOFs in total) in a dense situation.

1.1 RelatedWork

The (single-robot) motion planning problem, a fundamental problem in robotics, aims at finding an obstacle-free path in cluttered environments. An established approach is sampling-based motion planning (SBMP) [Elbanhawi and Simic, 2014]. SBMP iteratively and randomly samples state points from the space and then constructs a roadmap; a solution is derived by pathfinding on the roadmap. Numerous SBMP algorithms have been developed so far, such as [Kavraki et al., 1996; LaValle, 1998; Karaman and Frazzoli, 2011]. SBMP can solve problems even with many degrees of freedom and has achieved successful results not limited to robotics [LaValle, 2006].

In principle, SBMP is applicable to MRMP by considering one composite robot consisting of all robots [Choset et al., 2005], as seen in [S´anchez and Latombe, 2002; Le and Plaku, 2018]. However, such strategies require sampling from the high-dimensional space linear to the number of robots, being a bottleneck even for SBMP [LaValle, 2006]. Consequently, recent studies mostly take the aforementioned two-phase planning. In those studies, as the first phase, roadmaps are explicitly prepared via conventional SBMP [ˇSvestka and Overmars, 1998; Wagner et al., 2012; Solovey et al., 2016; Solis et al., 2021; Dayan et al., 2021] or implicitly embedded as lattice grids [Han et al., 2018; H¨onig et al., 2018; Cohen et al., 2019]. Depending on the heterogeneity of robots, a roadmap is shared among robots, or, robot-wise roadmaps are constructed. The lattice grids are available when the configuration space of each robot is not high-dimensional or state transitions of robots are restricted to a limited number; otherwise, the search space dramatically grows. This study does not assume such limitations. The second phase often uses multi-agent pathfinding (MAPF) algorithms such as [Sharon et al., 2015; Barer et al., 2014; Wagner and Choset, 2015], including prioritized planning [Erdmann and Lozano-Perez, 1987; Silver, 2005], and sometimes discretized versions of SBMP [C´ap et al., 2013; Solovey et al., 2016]. MAPF [Stern et al., 2019] is a problem of finding a set of collision-free paths on a graph and collects extensive attention since the 2010s.

SSSP is directly inspired by two algorithms, respectively for SBMP and MAPF: (i) Expansive Space Trees (EST) [Hsu et al., 1997] is an example of SBMP, which performs planning by constructing a query tree growing with random walks. (ii) A∗ with operator decomposition [Standley, 2010] solves MAPF efficiently by decomposing successors in the search tree such that at most one agent takes an action, rather than all agents take actions simultaneously. We realize rapid MRMP by combining these techniques.

1.2 Paper Organization

Section 2 describes the problem formulation and assumptions for MRMP. Section 3 describes SSSP. Section 4 presents the empirical results. Section 5 provides discussions. Throughout the paper, we present a sub-optimal algorithm and focus on the decision problem because solving MRMP itself is challenging. Optimal SSSP is discussed at the end. The appendix, code, and video are available at https://kei18.github.io/sssp/.

2 Preliminaries

2.1 ProblemDefinition of MRMP

We consider a problem of motion planning for a team of n robots A = {1, 2, . . . , n} in the 3D closed workspace W ⊂ R3 . Each robot i is operated in its own configuration space Ci ⊂ RdCi , where dCi ∈ N>0. A set of points occupied by robot i at a configuration q ∈ Ci is denoted as Ri(q) ⊂ W. The space W may contain obstacles O ⊂ W. A free space for robot i is then C free i := {q ∈ Ci | Ri(q) ∩ O = ∅}. A trajectory for robot i is defined by a continuous mapping σi : R≥0 ↦→ Ci.

Definition 1. An MRMP instance is defined by a tuple (W, A, C, O, R, Qinit , Qgoal ), where C := (Ci)i∈A, R := (Ri)i∈A, Qinit := (qinit i ∈ Ci )i∈A, and Qgoal := (Qgoal i ⊆ Ci)i∈A.

Definition 2. Given an MRMP instance, the MRMP problem is to find a tuple of n trajectories (σi)i∈A (i.e., solution) and tend ∈ R≥0, satisfying the following conditions:

• endpoint: σi(0) = qinit i ∧ σi(tend) ∈ Qgoal i
• obstacle-free: σi(τ) ∈ C free i , 0 ≤ τ ≤ tend
• inter-robot collision-free: Ri (σi(τ)) ∩ Rj (σj(τ)) = ∅, i ̸= j ∈ A, 0 ≤ τ ≤ tend

2.2 Constraints of Robot Motions

Definition 2 assumes that a robot can go in any direction in the configuration space unless it encounters obstacles. We call it geometric MRMP. Meanwhile, robots are often subject to kinematic and dynamics constraints. Kinematic constraints restrict the local directions of motion available to a robot from a given configuration. For instance, wheeled robots cannot translate sideways. Dynamics constraints are governed by the time derivatives, such as velocity and acceleration. For instance, cars cannot stop instantly. Kinematic and dynamics constraints are collectively called kinodynamic constraints. Motion planning under kinodynamic constraints is called kinodynamic planning [Donald et al., 1993]. Since this study is an early-stage attempt at combining sampling and search, we consider only kinematic constraints and ignore dynamics constraints for simplicity.

2.3 Planning with Kinematic Constraints

Consider a control space Ui ∈ RdUi for robot i, where dUi ∈ N>0. For instance, the control space of a wheeled robot is defined by motor controls of its wheels. Then, transitions of configurations under kinematic constraints are governed by ˙ q = fi(q, u), where q ∈ Ci and u ∈ Ui. A kinematic MRMP instance is defined by a composition of an MRMP instance in Def. 1, control spaces (Ui)i∈A, and transition functions (fi)i∈A. Given a kinematic MRMP instance, the kinematic MRMP problem asks a sequence of control inputs for each robot i, that is, ξi : R≥0 ↦→ Ui. A trajectory σi of configurations is successively defined as σi(0) := qinit i and σi(t) := ∫ t 0 fi (σi(τ), ξi(τ)) dτ + σi(0). ξi constitutes a solution when σi satisfies the conditions in Def. 2 for all robots.

2.4 Discretized Time and Local Planner

Both geometric and kinematic MRMPs are defined in continuous time. However, it is realistic for planning to discretize the time. That is, by introducing ∆ ∈ R>0 as a small amount of time, we aim at finding a path of configurations for each robot, such that any consecutive two points are travelable in ∆, without encountering obstacles, without inter-robot collisions, and following kinematic constraints. For instance, given a sequence of control inputs ξ 0 , ξ 1 , . . ., a path of configurations in kinematic MRMP is successively defined as σ0 i := qinit i and σk i := σk-1 i + fi (σk-1 i , ξ k-1 )∆. Herein, δ can be regarded as a problem input shared between robots.

For this discretization, as often assumed in motion planning studies [Choset et al., 2005; LaValle, 2006], we assume that each robot i has a local planner denoted as connecti. Given two configurations qfrom, qto ∈ Ci, this function returns a unique trajectory σ of the duration ∆ that satisfies: (i) σ(0) = qfrom ∧ σ(∆) = qto , (ii) σ(τ) ∈ C free i for 0 ≤ τ ≤

∆, and (iii) σ follows fi of kinematic MRMP. If no such σ is found, connecti returns ⊥. For instance, connecti(qfrom, qto ) may output (∆-τ)qfrom +τqto for geometric MRMP, or Dubins paths [Dubins, 1957] for car-like robots. Since kinematic MRMP excludes dynamics constraints, this paper assumes that each robot can always remain in its current configuration, i.e., connecti(q, q) ̸= ⊥ for any q ∈ C free i .

Observe that the local planners hide control inputs of kinematic MRMP; as long as they are definable, we can directly consider planning in configuration spaces. Therefore, we collectively call geometric and kinematic MRMPs the MRMP problem and do not distinguish the two explicitly. With the local planners, a solution of MRMP is a tuple of paths (Πi)i∈A, where Πi = (q0, q1, . . . , qk | qt ∈ Ci) and k is common between robots, satisfying the following conditions:

• endpoint: Πi[0] = qinit i ∧Πi[k] ∈ Qgoal i
• consistent path: connecti(Πi[t],Πi[t +1]) ̸= ⊥
• inter-robot collision-free:

The domain of t is {0, 1, . . . , k - 1}.

2.5 Roadmap

A roadmap for robot i is a directed graph Gi = (Vi,Ei) which approximates C free i with a finite set of vertices. Each vertex q ∈ Vi corresponds to a configuration of Ci, thus simply denoted as q ∈ Ci. The roadmap Gi must satisfy q ∈ C free i for all q ∈ Vi and connecti(q, q′ ) ̸= ⊥ for all (q, q′ ) ∈ Ei.

2.6 Blackbox Utility Functions

To solve MRMP, we introduce four functions. The first three are common in motion planning studies [Choset et al., 2005; LaValle, 2006], whereas the last one is specific to MRMP.

Sampling. The function samplei randomly samples a configuration q ∈ Ci, where q may not be in C free i .

Distance. The function disti : Ci × Ci ↦→ R≥0 defines a distance of two configurations of robot i. It is not necessary for this function to consider obstacles or inter-robot collisions, however, we assume that p = q ⇔ disti(p, q) = 0, dist(p, q) = dist(q, p), and disti satisfies the triangle inequality, e.g., the Euclidean distance.

Steering. The function steeri takes two configurations qfrom , qto ∈ C free i and then returns a "closer" configuration q ∈ C free i to qto . With a prespecified parameter ϵ > 0, formally:

In practice, steer can be approximately implemented by binary search, e.g., starting from qto , repeatedly sampling q ∈ Ci while halving the distance from qfrom until q is feasible.

Collision. For two robots i, j, given four configurations qfrom {i,j} , qto {i,j} ∈ C free {i,j} such that connect{i,j}(qfrom {i,j} , qto {i,j} ) ̸= ⊥, the function collide(i,j)(qfrom i , qto i , qfrom j , qto j ) returns TRUE when there is a collision if two robots simultaneously change their configurations from qfrom {i,j} to qto {i,j} . Here, a collision is defined similarly to Eq. (1). The function otherwise returns FALSE. For convenience, we use collide(Qfrom,Qto ), where Q{from, to} = (q{from, to} i ∈ C free i )i∈A. This shorthand notation returns TRUE if and only if there is a pair (i, j) for which collide(i,j)(qfrom i , qto i , qfrom j , qto j ) returns TRUE. As for implementation, since collision detection is important in singlerobot motion planning and has been studied for a long time, well-known open-source libraries are available, e.g., [Pan et al., 2012].

Domain Independence. To sumup, we solveMRMP using only five blackbox functions: connect, sample, dist, steer, and collide. Doing so makes our approach non-restrictive to specific robotic systems, rather it is applicable to many planning domains as we will see in the experiments.

3 AlgorithmDescription

In a nutshell, SSSP performs a best-first search using operator decomposition [Standley, 2010] while simultaneously growing robot-wise roadmaps via random walks [Hsu et al., 1997]. This section first explains the core idea, followed by the pseudocode, theoretical analysis of completeness, and postprocessing to obtain better solutions.

3.1 Core Idea

As a high-level description, SSSP constructs a search tree while expanding robot-wise roadmaps. Each search node in the tree contains a tuple of configurations Q = (qi ∈ C free i )i∈A and robot i that will take the next action. When this node is selected during the search process, the algorithm does vertex expansion and search node expansion in order. The former expands the roadmap Gi for robot i by randomwalks from Q[i]. The latter creates the successors of the node by transiting the configuration of robot i from Q[i] to its neighboring configurations on Gi, and then passing the turn for i to i + 1. Figure 1 illustrates this procedure with initial roadmap construction explained later, while Fig. 2 shows an example of constructed roadmaps.

3.2 Details

Algorithm 1 presents the pseudocode of SSSP. Some artifacts are explained as follows.

Initial Roadmap Construction (Line 1) is done by conventional single-agent SBMP such as RRT-Connect [Kuffner and LaValle, 2000]. The objective is to secure at least one valid path from initial to goal configurations for each robot.

Best-first Search (Lines 3-22) realizes the core idea by maintaining a priority queue Open that stores generated nodes and a set Explored that stores already generated search situations. For each iteration, SSSP checks whether the popped node from Open satisfies the goal condition (Line 8). If so, it returns a solution by backtracking the node.

Vertex Expansion (Lines 11-15) is implemented by steering from the target configuration to newly sampled ones, up to a fixed number m ∈ N>0. To guarantee completeness, SSSP also uses vanilla random sampling with a small probability λ (0.01 in our experiments). Each new vertex must satisfy a constraint of distance threshold, which suppresses roadmaps being too dense; otherwise, the search space dramatically increases, making it difficult to find a solution. Similar techniques are seen in SBMP studies [Kala, 2013; Dobson and Bekris, 2014]. Each new vertex qnew is followed by edge updates, that is, connecting qnew to all vertices that connect returns trajectories. Note that, in practice, many sampling trials (i.e., large m) may compromise computation time.

Search Node Expansion (Lines 16-22) adds successors that (i) have not appeared yet in the search process and (ii) do not collide with other robots.

Search Iteration (Lines 2-23). The search iterates until a solution is found while decreasing the distance thresholds for vertex expansion by multiplying 0 < γ < 1 by current ones.

Search Node Scoring. The heart of the best-first search is how to score each node that determines which node is popped from Open (Line 7). Given a tuple of configurations Q, SSSP scores the node by summation over each robot i's shortest path distance from Q[i] to one of the configu-

Algorithm 1 SSSP; input: instance I; output: solution

params: #sampling m ∈ N>0 random sampling prob. λ ∈ (0, 1] threshold distances θi ∈ R>0, decay rate γ ∈ (0, 1) 1: Gi = (Vi, Ei) ← init roadmap(I, i); for each i ∈ A 2: while TRUE do 3: initialize Open, Explored ▷ priority queue, set 4: Open.push (〈Q : Qinit , next : 1, parent : ⊥〉) 5: Explored.append ((Qinit , 1)) 6: while Open ̸= ∅ do 7: N ←Open.pop() 8: if ∀i ∈ A, N .Q[i] ∈ Qgoal i then 9: return backtrack(N ) 10: i ← N .next ; qfrom ← N .Qfrom[i] 11: for 1, 2, . . . ,m do ▷ vertex expansion via sampling 12: qnew ← samplei() 13: with prob. (1 - λ): qnew ← steeri(qfrom, qnew) 14: if minq∈Vi disti (q, qnew) > θi then 15: Vi ← Vi ∪ {qnew}; update Ei 16: j ← i + 1 if i ̸= |A| else 1 ▷ node expansion 17: for qto ∈ {q ∈ Vi | (qfrom, q) ∈ Ei} do 18: Qnew ← copy(N .Q); Qnew[i] ← qto 19: if (Qnew, j) ∈ Explored then continue 20: if collide (N.Q, Qnew) then continue 21: Open.push (⟨Q : Qnew, next : j, parent : N⟩) 22: Explored .append ((Qnew, j)) 23: θi ← γθi; for each i ∈ A

rations in Qgoal i on the roadmap Gi, while weighting each edge (qfrom, qto ) ∈ Ei by dist(qfrom, qto ). The path distance is calculated by ignoring inter-robot collisions, as common in heuristics for MAPF studies [Silver, 2005].

3.3 Properties

Let k ∈ N>0 be the number of search iterations of Lines 223. Moreover, let Gki be the roadmap Gi at the beginning of k-th iteration (i.e., Gi at Line 3) and Gk := (Gki )i∈A. A solution is called sequential when at most one robot transits configurations simultaneously at any time.

Lemma 3. SSSP finds a solution in the k-th search iteration if Gk contains a sequential solution.

Proof. For each k-th iteration, Gi is not infinitely increasing due to the distance threshold θi. Thus, the search space is finite: O(|V1| · . . . · |Vn| · |A|). Thanks to brute-force search in finite space, SSSP finds a solution if Gk contains one.

Theorem 4. For the geometric MRMP problem (Def. 2), the probability that SSSP finds a solution approaches one as k approaches ∞, provided the instance is solvable.

Proof. We limit the discussion to geometric MRMP wherein each robot can move in arbitrary directions in its configuration space. The proof is based on analysis in [ˇSvestka and Overmars, 1998], which claims if an instance is solvable, (i) there is a sequential solution and (ii) sufficiently dense robot-wise roadmaps constructed uniformly at random sampling (e.g., PRM [Kavraki et al., 1996]) contain a sequential solution. According to Lemma 3, SSSP finds a solution once roadmaps holding claim-(ii) are obtained. SSSP eventually constructs such roadmaps for the following reasons.

For each search iteration, each robot i tries to develop its roadmap with at least m ≥ 1 new samples. With the probability 1 - λ > 0, some of them are outcomes of uniformly at random sampling. Each iteration terminates in finite time (see Lemma 3), therefore, each robot does not stop attempts of uniformly at random sampling until finding solutions. Moreover, each iteration decreases the distance threshold, enabling robots to construct denser roadmaps.

In short, Thm. 4 states that SSSP eventually finds a solution. This corresponds to probabilistic completeness in SBMP [Elbanhawi and Simic, 2014], defined by the probability of finding solutions bounded by the number of sampling.

3.4 Postprocessing

SSSP returns only sequential solutions and compromises solution quality such as the maximum traveling time. Thus, we briefly discuss how to realize parallel execution that enables two or more robots to move simultaneously. Specifically, we consider postprocessing to refine solutions. Smoothing solution trajectories by postprocessing is common in single-robot SBMP [Geraerts and Overmars, 2007]. However, MRMP additionally takes care of inter-robot collisions.

We describe our method with Fig. 3a. Suppose that SSSP outputs a sequential solution of Πi = (0, 1, 1, 2, 2, 3) (blue) and Πj = (0, 0, 1, 1, 2, 2) (red). The method repeats the next two steps until a given solution metric has not improved.

1. Construct a temporal plan graph (TPG) [H¨onig et al., 2016] of the solution. TPG is a directed acyclic graph that records temporal dependencies of each robot's motions. We also attach possible "shortcut" motions to TPG. Figure 3b shows an example. There is an arc between the motions (0, 1) of robots {i, j}; these motions must happen in order due to collision avoidance. Several shortcut arcs exist, for example, i can skip using vertex-2 by directly going from vertex-1 to vertex-3.
2. Remove redundant motions in TPG while keeping the dependencies between robots. Figure 3c shows an example. For j, the motions (1, 1) and (2, 2) are removed but (0, 0) survives to keep the dependency with i.

In the example, we finally obtain a refined solution Πi = (0, 1, 3, 3) and Πj = (0, 0, 1, 2).

The above refinement is applicable to any MRMP solutions not limited to those from SSSP. Indeed, the experiments applied the refinement for solutions obtained by all methods.

4 Evaluation

This section extensively evaluates SSSP on a variety of MRMP problems and demonstrates that it can solve various MRMP rapidly compared to other standard approaches. We further assess solution quality, scalability about the number of robots |A|, and which components are essential for SSSP, followed by a ground-robot demo in a dense situation.

4.1 Experimental Setups

Benchmarks. As illustrated in Fig. 4, we prepared various scenarios with diverse degrees of freedom and kinematic constraints, in closed workspaces W ∈ [0, 1]{2,3} . To focus on characteristics specific to MRMP, we modeled these scenarios with simple geometric patterns (e.g., spheres or lines) and reduced the effort of the connect and collide functions; these functions were performed with simple geometry calculations. For each scenario, we prepared 100 instances by randomly generating initial/goal configurations and obstacle layouts. For each instance, the number of robots |A| was chosen from the interval {2, 3, . . . , 10}. Robots' body parameters (e.g., radius and arm length) were also generated randomly and differed between robots. These parameters were adjusted so that robots are sufficiently congested, otherwise, the instances become easy to solve. Note that unsolvable instances may be included, though we excluded obviously unsolvable instances such as initial configurations with inter-robot collisions. In summary, each instance consists of a team of heterogeneous robots and each robot has a different configuration space; a shared roadmap is unavailable.

Baselines. Our goal is to develop planning algorithms that can be applied to various domains without the use of external knowledge other than five black-box functions. To this end, we carefully selected the following well-known baseline methods that are applicable to MRMP defined in Sec. 2. 1

• Probabilistic roadmap (PRM) [Kavraki et al., 1996] is a celebrated SBMP. For MRMP, PRM samples a composite configuration of |A| robots directly from O(|A|) dimensional spaces and constructs a single roadmap, and then derives a solution by pathfinding on it.
• Rapidly-exploring Random Tree (RRT) [LaValle, 1998] is another popular SBMP, focusing on singlequery situations. Similar to PRM, RRT for MRMP samples a composite state and constructs a tree roadmap rooted in a composite one of initial configurations for all robots.
• RRT-Connect (RRT-C) [Kuffner and LaValle, 2000] is a popular extension of RRT, which accelerates finding a solution by bi-directional search from both initial and goal configurations.

1 dRRT(∗) [Solovey et al., 2016; Shome et al., 2020] was not included due to requiring an additional oracle.

• Prioritized Planning (PP) [Erdmann and LozanoPerez, 1987; Silver, 2005; Van Den Berg and Overmars, 2005] is a standard approach to MAPF such that robots sequentially plan paths while avoiding collisions with already planned paths. We applied PP to roadmaps constructed by robot-wise PRMs, as taken in [Le and Plaku, 2018]. PP was repeated with random priorities until the problem is solved.
• Conflict-Based Search (CBS) [Sharon et al., 2015] is another popular MAPF algorithm. CBS is applicable to MRMP when roadmaps are given [Solis et al., 2021]. We run CBS on robot-wise roadmaps constructed by PRM. Moreover, we manipulated the heuristic of CBS to avoid collisions as much as possible during the search. Doing so loses the optimality of CBS but speeds up finding solutions significantly [Barer et al., 2014].

All hyperparameters of each method including SSSP were adjusted prior to the experiments (see the appendix). SSSP used RRT-Connect [Kuffner and LaValle, 2000] to obtain initial robot-wise roadmaps (Line 1). Note that this is irrelevant to RRT-C in the baselines. PP/CBS were tested with PRM rather than RRT-Connect because otherwise constructed roadmaps do not include detours, which is essential for solving MAPF in the second phase of two-phase planning. Since all methods rely on non-determinism, we tested each method with 10 different random seeds for each instance (1,000 trials in total).

Metrics. The objective is to find solutions as quickly as possible. Therefore, we rate how many instances are solved within given time limits (maximum: 5min).

Evaluation Environment. The simulator and all methods were coded in Julia. The experiments were run on a desktop PC with Intel Core i9-7960X 2.8 GHz CPU and 64 GB RAM. A maximum of 32 different instances were run in parallel using multi-threading. All methods used exactly the same implementations of connect, collide, sample, and dist.

4.2 Results of Various MRMP Problems

Figure 4 summarizes the results. In short, SSSP outperforms the other baselines in all tested scenarios, i.e., solving more instances much faster. We acknowledge that runtime performance heavily relies on implementations; however, these results indicate that SSSP is very promising. The results of SSSP in Arm22 and Dubins2d are relatively non-remarkable but we guess this is due to many unsolvable instances, which could be easily generated in these scenarios. We later discuss why SSSP is quick.

Solution Quality. As reference records of solution quality, Table 1 shows the expected total traveling time of all robots (aka. sum-of-costs), after applying the postprocessing introduced in Sec. 3.4 to all methods. Compared to the other baselines, the total traveling time of SSSP is not the best but comparable. We will discuss optimality at the end of the paper.

4.3 Scalability Test

We next assess the scalability of SSSP about the number of robots |A|, varied by 10 increments. For each |A|, 100 Point2d instances were prepared with smaller robots' radius (see Fig. 5). PP with tuned parameters (see the appendix) was also tested as a baseline, which relatively scored high among the other baselines in the scenario with many robots. Figure 5 shows that, with larger |A|, SSSP takes longer but still acceptable time for planning, compared to the baseline.

4.4 Which Elements are Essential?

We next address another question that asks which technical components are essential to SSSP. Specifically, we evaluated degraded versions that omit the following components: (i) initial roadmap constructions (Line 1), (ii) search node scoring (replaced by random values), (iii) vertex expansion (Lines 11-15) (iv) distance thresholds check (Line 14), (v) steering (by setting λ = 1), and (vi) integrated sampling and search. The last one rated SSSP without vertex expansion (m = 0) on robot-wise PRMs.

Table 2 reveals that all these components contribute to the performance of SSSP. Among them, involving the appropriate node scoring is particularly critical to achieving high success rates within a limited time, as well as vertex expansion. The initial roadmap construction is effective when single-robot motion planning itself is difficult (Snake2d). The steering effect was non-dramatic because the workspace is small relative to robots; we discuss this in the appendix, together with scores of total traveling time.

4.5 Robot Demo

We applied SSSP to 32 robots (https://toio.io/) modeled as Point2d in a dense situation (Fig. 6). The robots evolve on a specific playmat and are controllable by instructions of absolute coordinates. Even though an experimenter randomly placed the robots as the initial states (see the movie), the planning was done in about 30 s, and then all robots eventually reached their goal. Importantly, this demo was based on only five utility functions of connect, sample, dist, steer, and collide, without any prepared environmental representations.

5 Conclusion and Discussion

This paper introduced the SSSP algorithm that rapidly solves MRMP. The main idea behind the algorithm was to unite techniques developed for SBMP and search techniques for MAPF. The former had been studied mainly in the robotics community, while the latter in the AI community. Bringing them together brings promising results, as extensively demonstrated in our experiments. In the remainder, we provide further discussions and future directions.

Why is SSSP Quick? We provide two qualitative explanations: one from the MAPF side, and another from the SBMP side. The quickness of SSSP relies on both factors.

Branching Factor: During the search, SSSP decomposes successors into search nodes corresponding to at most one robot taking a motion. Compared to search styles allowing all robots to move simultaneously, the decomposition significantly reduces branching factor, i.e., the number of successors at each node. In general, the average branching factor b largely determines the search effort [Edelkamp and Schrodl, 2011]. Assume that each robot has k possible motions from each configuration on average. Coupled with perfect heuristics and a perfect tie-breaking strategy, allowing all robots to move simultaneously results in b = k|A| and generates (k|A| ) · l nodes, where l is the depth of the search. In contrast, SSSP results in b = k and enables searching the equivalent node with only (k|A|) · l nodes generation. This is a key trick of A∗ with operator decomposition [Standley, 2010] for MAPF; we based this idea to develop SSSP.

Imbalanced Roadmaps: PRM-based methods (i.e., PRM,

PP, and CBS in our experiments) have no choice other than to construct roadmaps uniformly spread in each configuration space, making search spaces huge. Such drawbacks might be relieved with biased sampling but representing good bias for MRMP is not trivial; indeed, existing studies use machine learning [Arias et al., 2021; Okumura et al., 2022]. In contrast, owing to carefully-designed components, SSSP naturally constructs sparse roadmaps in important regions for each robot as seen in Fig. 2. Consequently, the search space for SSSP is kept small, which also contributes to quick MRMP.

KinodynamicMRMP. We untreated dynamics constraints, therefore, kinodynamic MRMP is an interesting direction. Similarly to RRT [LaValle and Kuffner Jr, 2001] or EST [Hsu et al., 1997] that are applicable to kinodynamic planning, we consider that SSSP is also applicable to such planning. The adaptation is by considering planning with a state x instead of a configuration q ∈ Ci for robot i, which comprises a configuration q and its derivative ˙ q (i.e., x := (q, ˙ q)). Some parts require care; in usual kinodynamic MRMP, connecti(x, x) = ⊥ as we see in cars that cannot stop instantly. This means that sequential solutions are not allowed. In this case, it is necessary to regard |A| successive search nodes in SSSP as "one block" to enable concurrent motions of multiple robots.

Optimal MRMP. SSSP prioritizes solving MRMP itself, rather than solution quality. However, it is possible to reflect quality by modifying node scoring, which is currently designed as a greedy search. With terminologies of A∗ search [Hart et al., 1968], SSSP only uses h-value (i.e., estimation of cost-to-go). A promising direction is to incorporate g-value (i.e., cost-to-come), letting SSSP asymptotically optimal without degrading the performance of solvability. We also point out that, to be optimal, rewiring the search tree is mandatory as seen in SBMP or MAPF studies [Karaman and Frazzoli, 2011; Okumura, 2023a]. Another possibility is to incorporate iterative refinement schemes [Okumura et al., 2021; Li et al., 2021] developed for MAPF, although there is no theoretical guarantee.

Further Integration of SBMP and MAPF. The concept behind the paper was developing robot-wise roadmaps according to the multi-agent search progress, turning out to be promising. We consider this direction should be further investigated. A very-recent study [Kottinger et al., 2022] explores this direction for CBS. Many powerful MAPF algorithms exist not limited to A∗ adaptation or CBS, such as [Li et al., 2022; Okumura, 2023b]. Therefore, integrating them with SBMP may fruit practical methodologies for MRMP.

Acknowledgments

We thank Franc¸ois Bonnet and the anonymous reviewers for their many insightful comments on the manuscript. This work was partly supported by JSPS KAKENHI Grant Number 20J23011, 21K11748, 21H03423, 20K11685 and JST ACTX Grant Number JPMJAX22A1. Keisuke Okumura thanks the support of the Yoshida Scholarship Foundation when he was a Ph.D. student.