Scalable Asymptotically-Optimal Multi-Robot Motion Planning

More Experimental Data

This appendix presents additional experimental data omitted from Section [sec:experiments].

2-Robot Benchmark

For the two-robot benchmark, additional data is presented in Figure 1. Here, the data presented in Figure [fig:polygonal_benchmark] is shown again over time instead of over iterations.

R-Robot Benchmark

For the $`R`$-robot benchmark, additional data is presented in Figure 2, showing query resolution times for the various methods.

To emphasize the lack of scalability for alternate methods, additional experiments were run in this setup using a minimal roadmap. The tests use a $`9`$-node roadmap for each robot as illustrated in Figure [fig:nine_grid]. Each roadmap is constructed with slight perturbations to the nodes within the shaded regions indicated in the figure.

The data for this modified benchmark (shown in Figure 3) indicate that even using a very small roadmap does not allow alternate methods to scale. While the methods scale better, Implict $`\tt A^{\text *}`$ does time out for $`R \geq 5`$, and $`\tt RRT^{\text *}`$ times out for $`R \geq 6`$.

Manipulator Benchmark

For the dual-arm manipulator benchmark, additional data is presented in Figure 4. Here, the data of Figure [fig:motoman_convergence] is shown over iterations instead of over time.

For a fixed $`n \in \mathbb{N}_+`$, define for every robot $`i`$ the PRM roadmap $`\ensuremath{\mathbb{G}}_i = (\mathbb{V}_i, \mathbb{E}_i)`$ constructed over $`\ensuremath{\ensuremath{\mathbb{C}}^{\textup{f}}}_i`$, such that $`|\mathbb{V}_i|=n`$ with connection radius $`r(n)`$. Then, $`\hat\ensuremath{\mathbb{G}} = (\mathbb{\hat V}, \mathbb{\hat E}) = \ensuremath{\mathbb{G}}_1\times \ldots \times \ensuremath{\mathbb{G}}_R`$ is the tensor product roadmap in space $`\ensuremath{\mathbb{C}}`$ (for an illustration, see Figure 5). Formally, $`\mathbb{\hat V} = \{ ( v_1, v_2, \dots, v_R ), \forall i, v_i \in \mathbb{V}_i\}`$ is the Cartesian product of the nodes from each roadmap $`\ensuremath{\mathbb{G}}_i`$. For two vertices $`V =(v_1,\ldots,v_m) \in \mathbb{\hat V}, V'=(v'_1,\ldots,v'_m) \in \mathbb{\hat V}`$ the edge set $`\mathbb{\hat E}`$ contains edge $`(V,V')`$ if for every $`i`$ it is that $`v_i=v'_i`$ or $`(v_i,v'_i)\in \mathbb{E}_i`$.¹

As shown in Algorithm [algo:drrtstar], $`\ensuremath{{\tt dRRT^*}}`$ grows a tree $`\ensuremath{\mathbb{T}}`$ over $`\hat\ensuremath{\mathbb{G}}`$, rooted at the start configuration $`S`$ and initializes path $`\pi_{\textup{best}}`$ (line 1). The method stores the node added each iteration $`V`$ (Line 2), as part of an informed process to guide the expansion of $`\mathbb{T}`$ towards the goal. The method iteratively expands $`\ensuremath{\mathbb{T}}`$ given a time budget (Line 3), as detailed by Algorithm [algo:drrtstar_expand], storing the newly added node $`V`$ (Line 4). After expansion, the method traces the path which connects the source $`S`$ with the target $`T`$ (Line 5). If such a path is found, it is stored in $`\pi_{\textup{best}}`$ if it improves upon the cost of the previous solution (Lines 6, 7). Finally, the best path found $`\pi_{\textup{best}}`$ is returned (Line 8).

The expansion step is given in Alg. [algo:drrtstar_expand]. The default initial step of the method is given in Lines 1-4, i.e., when no $`\ensuremath{V^{\textup{last}}}`$ is passed (Line 1), which corresponds to an exploration step similar to RRT: a random sample $`Q^{\textup{rand}}`$ is generated in $`\mathbb{C}`$ (Line 2), its nearest neighbor $`V^{\textup{near}}`$ in $`\ensuremath{\mathbb{T}}`$ is found (Line 3) and the oracle function $`\mathbb{O}_d(\cdot,\cdot)`$ returns the implicit graph node $`V^{\textup{new}}`$ that is a neighbor of $`V^{\textup{near}}`$on the implicit graph in the direction of $`\ensuremath{Q^{\textup{rand}}}`$ (Line 4). If a $`\ensuremath{V^{\textup{last}}}`$, however, is provided (Line 5)—which happens when the last iteration managed to generate a node closer to the goal relative to its parent—then the $`\ensuremath{V^{\textup{new}}}`$ is greedily generated so as to be a neighbor of $`\ensuremath{V^{\textup{last}}}`$ in the direction of the goal $`T`$ (Line 6).

In either case, the method next finds neighbors $`N`$, which are adjacent to $`V^{\textup{new}}`$ in $`\hat\ensuremath{\mathbb{G}}`$ and have also been added to $`\ensuremath{\mathbb{T}}`$ (Line 7). Among $`N`$, the best node $`V^{\textup{best}}`$is chosen, for which the local path $`\mathbb{L}(\ensuremath{V^{\textup{best}}}, \ensuremath{V^{\textup{new}}})`$ is collision-free and that the total path cost to $`\ensuremath{V^{\textup{new}}}`$ is minimized (Line 8). If no such parent can be found (Line 9), the expansion fails and no node is returned (Line 10). Then, if $`\ensuremath{V^{\textup{new}}}`$ is not in $`\ensuremath{\mathbb{T}}`$, it is added (Lines 11-13). Otherwise, if it exists, the tree is rewired so as to contain edge $`(\ensuremath{V^{\textup{best}}}, \ensuremath{V^{\textup{new}}})`$, and the cost of the $`\ensuremath{V^{\textup{new}}}`$’s sub-tree (if any) is updated (Lines 14, 15). Then, for all nodes in $`N`$ (Line 16), the method tests $`\ensuremath{\mathbb{T}}`$ should be rewired through $`\ensuremath{V^{\textup{new}}}`$ to reach this neighbor. Given that $`\mathbb{L}(\ensuremath{V^{\textup{new}}}, v)`$ is collision-free and is of lower cost than the existing path to $`v`$ (Line 17), the tree is rewired to make $`\ensuremath{V^{\textup{new}}}`$ be the parent of $`v`$ (line 18).

Finally, if in this iteration the heuristic value of $`\ensuremath{V^{\textup{new}}}`$ is lower than its parent node $`\ensuremath{V^{\textup{best}}}`$ (line 19), the method returns $`\ensuremath{V^{\textup{new}}}`$ (Line 20), causing the next iteration to greedily expand $`\ensuremath{V^{\textup{new}}}`$. Otherwise, $`NULL`$ is returned so as to do an exploration step. Note that the approach is implemented with helpful branch-and-bound pruning after an initial solution is found, though this is not reflected in the algorithmics.

$`V^{\textup{new}}`$ is determined via an oracle function. Using this oracle function and a simple rewiring scheme is sufficient for showing asymptotic optimality for $`\ensuremath{{\tt dRRT^*}}`$ (see Section [sec:analysis]). The oracle function $`\mathbb{O}_d`$ for a two-robot case is illustrated in Figure 6. First, let $`\rho(Q,Q')`$ be the ray from configuration $`Q`$ terminating at $`Q'`$. Then, denote $`\angle_{Q} (Q',Q'')`$ as the minimum angle between $`\rho(Q,Q')`$ and $`\rho(Q,Q'')`$. When $`Q^{\textup{rand}}`$ is drawn in $`\ensuremath{\mathbb{C}}`$, its nearest neighbor $`V^{\textup{near}}`$ in $`\ensuremath{\mathbb{T}}`$ is found. Then, project the points $`Q^{\textup{rand}}`$ and $`V^{\textup{near}}`$ into each robot space $`\ensuremath{\mathbb{C}}_i`$, i.e., ignore the configurations of other robots.

The method separately searches the single-robot roadmaps to discover $`V^{\textup{new}}`$. Denote $`\ensuremath{V^{\textup{near}}}= (v_1,\ldots,v_R), \ensuremath{Q^{\textup{rand}}}= (\tilde{q}_1,\ldots, \tilde{q}_R)`$. For every robot $`i`$, let $`N_i\subset \mathbb{V}_i`$ be the neighborhood of $`v_i \in \mathbb{V}_i`$, and identify $`v'_i = \argmin_{v \in N_i} \angle_{v_i} (q^{rand}_i, v)`$. The oracle function returns node $`\ensuremath{V^{\textup{new}}}= (v'_1,\ldots,v'_R)`$.

As in the standard $`\ensuremath{{\tt RRT}}`$ as well as in $`\ensuremath{{\tt dRRT}}`$, the $`\ensuremath{{\tt dRRT^*}}`$ approach has a Voronoi-bias property  . It is, however, slightly more involved to observe as shown in Figure 7. To generate an edge $`(V, V')`$, random sample $`Q^{\textup{rand}}`$ must be drawn within the Voronoi cell of $`V`$, denoted $`\textup{Vor}(V)`$ (A) and in the general direction of $`V'`$, denoted $`\textup{Vor}'(V)`$ (B). The intersection of these two volumes $`\textup{Vol}(V) = \textup{Vor}(V) \cap \textup{Vor}'(V)`$ is the volume to be sampled generate $`V^{\textup{new}}`$ via $`V^{\textup{near}}`$.

Consider a shared workspace with $`R \geq 2`$ robots, each operating in configuration space $`\ensuremath{\mathbb{C}}_i`$ for $`1\leq i\leq R`$. Let $`\ensuremath{\ensuremath{\mathbb{C}}^{\textup{f}}}_i \subset \ensuremath{\mathbb{C}}_i`$ be each robot’s free space, where it is free from collision with the static scene, and $`\ensuremath{\mathbb{C}}^{\textup{o}}_i=\ensuremath{\mathbb{C}}_i\setminus \ensuremath{\ensuremath{\mathbb{C}}^{\textup{f}}}_i`$ is the forbidden space for robot $`i`$. The composite $`\ensuremath{\mathbb{C}}`$-space $`\ensuremath{\mathbb{C}}= \prod^R_{i=1} \ensuremath{\mathbb{C}}_i`$ is the Cartesian product of each robot’s $`\ensuremath{\mathbb{C}}`$-space. A composite configuration $`Q = (q_1,\ldots,q_R) \in \ensuremath{\mathbb{C}}`$ is an $`R`$-tuple of robot configurations. For two distinct robots $`i, j`$, denote by $`I_i^j(q_j)\subset \ensuremath{\mathbb{C}}_i`$ the set of configurations where $`i`$ and $`j`$ collide. Then, the composite free space $`\ensuremath{\ensuremath{\mathbb{C}}^{\textup{f}}}\subset \ensuremath{\mathbb{C}}`$ consists of configurations $`Q=(q_1,\ldots,q_R)`$ subject to:

Each $`Q \in \ensuremath{\ensuremath{\mathbb{C}}^{\textup{f}}}`$ requires robots to not collide with obstacles, and each pair to not collide with each other. The composite forbidden space is defined as $`\ensuremath{\mathbb{C}}^{\textup{o}}= \ensuremath{\mathbb{C}}\setminus \ensuremath{\ensuremath{\mathbb{C}}^{\textup{f}}}`$.

Given $`S,T \in \ensuremath{\ensuremath{\mathbb{C}}^{\textup{f}}}`$, where $`S=(s_1,\ldots,s_R),T=(t_1,\ldots,t_R)`$, a trajectory $`\Sigma:[0,1]\rightarrow \ensuremath{\ensuremath{\mathbb{C}}^{\textup{f}}}`$ is a continuous curve in $`\ensuremath{\ensuremath{\mathbb{C}}^{\textup{f}}}`$, such that $`\Sigma(0)=S,\Sigma(1)=T`$, where the $`R`$ robots move simultaneously. $`\Sigma`$ is an $`R`$-tuple $`(\sigma_1,\ldots,\sigma_R)`$ of robot paths such that $`\sigma_i:[0,1]\rightarrow \ensuremath{\ensuremath{\mathbb{C}}^{\textup{f}}}_i`$.

The objective is to find a trajectory which minimizes a cost function $`c(\cdot)`$. The analysis assumes the cost is the sum of robot path lengths, i.e., $`c(\Sigma)= \sum_{i=1}^R |\sigma_i|`$, where $`|\sigma_i|`$ denotes the standard arc length of $`\sigma_i`$. The arguments also work for $`\max_{i=1:R} |\sigma_i|`$.² Section [sec:analysis] shows sufficient conditions for $`{\tt dRRT^*}`$ to converge to optimal trajectories over the cost function $`c`$.

This work studies the asymptotic optimality of sampling-based multi-robot planning over implicit structures. The objective is to efficiently search the composite space of multi-robot systems while also achieving formal guarantees. This requires construction of asymptotically optimal individual robot roadmaps and appropriately searching their tensor product. Performance is further improved by informed search in the composite space.

These results may extend to more complex settings involving kinodynamic constraints by relying on recent work . Furthermore, the analysis may be valid for different cost functions other than total distance. The tools presented here can prove useful in the context of simultaneous task and motion planning for multiple robots, especially manipulators .