Multi-Robot Path Planning Using Medial-Axis-Based Pebble-Graph Embedding

We propose a centralized approach for Multi-Robot Path Planning (MPP) for planar workspaces with polygonal obstacles. This problem has a wide range of applications such as warehouse management [1], computer games [2], and crowd modeling [3], which require the coordination of a large swarm of robots within a limited computational budget. MPP plans must be computed within a couple of milliseconds for an interactive game and an automatic warehouse needs to answer thousands of queries on a daily basis. Unfortunately, solving general MPP problems is NP-hard [4, 5, 6] and practical algorithms based on sampled roadmaps [7] and conflict-based search [8] quickly become intractable for more than a few robots. Follow-up research relies on additional assumptions on environment shapes and/or robot arrangements to attain practical performance.

We formalize 2D labeled MPP problems. We define $\mathcal{W}\subseteq\mathbb{R}^{2}$ as the 2D workspace and assum that the boundary of workspace, $\partial\mathcal{W}$, is piecewise linear. We have a set of $N$ disk-shaped robots initially centered at $\mathbf{s}^{1},\ldots,\mathbf{s}^{N}$ with identical unit radii, and we denote $B(\bullet)$ as a unit circle centered at $\bullet$. The robots have distinct goal positions $\mathbf{t}^{1,\cdots,N}$. Our method aims at finding a continuous path for each robot connecting $\mathbf{s}^{i}$ and $\mathbf{t}^{i}$, where the disk-shaped regions of any two robots do not overlap for any given time instance and no robot collides with obstacles.

Having defined the problem, we provide an overview of our approach. The first step of our algorithm is pebble-graph embedding (Section III). The pebble-graph is embedded into $\mathcal{W}$ with the help of Blum’s medial axis analysis, which can be performed through robust algorithms such as [31]. The medial axis analysis extracts two crucial pieces of information from a continuous workspace: skeleton lines and inscribed circles, as illustrated in Figure 1 (c). An inscribed circle, denoted as $\mathcal{C}$, is defined as a circular subset of the workspace that touches the boundary of the workspace at least two points, and skeleton lines are derived by connecting centers of inscribed circles. These two pieces of information, skeleton lines and inscribed circles, are crucial because they allow the structure in $\mathcal{W}$ to move and accommodate robots: robots can reside in inscribed circles and move along sub-paths of skeleton lines. As illustrated in Figure 1 (d), we design an algorithm to select a subset of inscribed circles where robots can be arranged into connected loops such that they can move to nearby vacant positions or rotate along loops in a collision-free manner as illustrated in Figure 1 (e). By construction, our pebble-graph is topologically identified with the setting considered in [11]. Our second step is motion planning (Section IV), where we compute a series of motions to answer arbitrary MPP queries. In other words, robots can perform arbitrary position permutations restricted to the graph vertices. Finally, start and goal positions might not coincide with graph vertices, and we use unlabeled, local navigations to move robots between their start/goal positions and graph vertices (Section V).

The first step of our method finds $M>N$ graph vertex positions using the greedy Algorithm 1. These vertices should be close to robots’ start positions, so that robots can be moved to vertices with a high success rate via local navigation algorithms such as [29]. We further assume each robots’ goal position set is close to its start position set, so goal positions can be ignore in the graph construct step. Our method maintains: 1) a set of considered robot start positions $\mathbb{X}$ initialized to be empty; 2) a set of remaining positions initialized to be $\{\mathbf{s}^{1},\cdots,\mathbf{s}^{N}\}$; 3) a set of considered inscribed circles $\mathbb{C}$ initialized to be empty. During each iteration, an arbitrary position in the remaining set is picked, e.g. $\mathbf{s}^{i}$. By the definition of inscribed circles, $B(\mathbf{s}^{i})$ must be contained in at least one inscribed circle, and we select the circle whose center is closest to $\mathbf{s}^{i}$, denoted as $\mathcal{C}(\mathbf{s}^{i})$. (To find $\mathcal{C}(\mathbf{s}^{i})$, we sample the skeleton lines at regular intervals and extract inscribed circles centered at sample points, giving a discrete circle set $\bar{\mathbb{C}}$.) We then move $\mathbf{s}^{i}$ from the remaining set to the considered set $\mathbb{X}$ and move $\mathcal{C}(\mathbf{s}^{i})$ into the circle set $\mathbb{C}$. Further, if there is another $\mathbf{s}^{j}$ in the remaining set such that $B(\mathbf{s}^{j})\subset\mathcal{C}(\mathbf{s}^{i})$, we also move $\mathbf{s}^{j}$ into the considered set. We terminate when the remaining set is empty. Note the success of our method depends on the order of choosing positions in the remaining set. To increase the success rate, we propose executing the algorithm multiple times using randomly shuffled start positions and return the first successful result. The resulting circle set would be used to construct our discrete loop graph using a BuildGraph function.

In the following, we analyze the relationship between loops and derive geometric conditions of a graph embedding, such that robot motions are collision-free. Since two loops can belong to different inscribed circles or different layers of the same inscribed circle, we use the subscript to index circles and superscript to index loops inside a circle. For example, $\mathcal{C}_{a}^{i}$ indicates the $i$th loop (starting from the innermost one) of the $a$th circle.

We allow robots to perform two types of movements on the pebble graph: 1) cyclic permutation of robots in a single loop; 2) movement of robot to an adjacent vacant position (either in the same loop or in an adjacent loop). This setting is similar to prior work in [17], but that method focuses on checking the feasibility, while we ensure feasibility of arbitrary robot configuration change on the graph under the assumption that the graph vertices are not fully occupied. Our pebbling motion planner does not differentiate between loops of the same circle or loops of different circles (because such differences have been taken care of in Section III). Therefore, we only use a global superscript to index loops. For the $i$th loop, the set of vertices is denoted as $\mathcal{V}_{\mathcal{L}}^{i}$. These vertices are connected by a set of edges denoted as $\mathcal{E}_{\mathcal{L}}^{i}$.

Formally, the topology of a pebble-graph $\mathcal{G}=<\mathcal{V},\mathcal{E}>$ successfully constructed from Algorithm 1 is simple, undirected, and connected. Different loops can be interpreted as a partition of $\mathcal{V}$ as follows:

The partition of vertices induces a partition of edge set $\mathcal{E}$ as follows:

Specifically, we assume that the vertices can be partitioned into $K>1$ loops, where $K$ is the number of loops in the graph. We also require that $\mathcal{E}_{\mathcal{L}}^{i}$ is the unique loop connecting $\mathcal{V}_{\mathcal{L}}^{i}$ and that $\mathcal{E}_{\mathcal{L}}^{i}$ is a simple loop (with no repeated vertices). Note that, under these definitions, each loop must have at least $3$ vertices, i.e. $|\mathcal{V}_{\mathcal{L}}^{i}|\geq 3$. This is necessary because our motion planner moves robots by swapping the positions of two neighbors connected by a graph edge, where we need a third position as a buffer to accommodate temporary robots (see our extended version [32] for more details). Finally, we allow two loops to overlap; however, if the $i$th loop and the $j$th loop overlap, we require that they share exactly $2$ vertices. As a result, two loops can also share a common edge (this is why we have $|\mathcal{V}_{\mathcal{L}}^{i}\cap\mathcal{V}_{\mathcal{L}}^{j}|\in\{0,2\}$ and $|\mathcal{E}_{\mathcal{L}}^{i}\cap\mathcal{E}_{\mathcal{L}}^{j}|\in\{0,1\}$). This corresponds to Case III of Section III-B. In summary, a successful graph returned by Algorithm 1 satisfies the following conditions:

On such a graph, we could follow similar reasoning as [9, 11] to verify feasibility for all the MPP instances and construct the corresponding motion plans, as summarized in the following result:

We prove Theorem 1 constructively in our extended version [32] by converting the permutation into a sequence of pairwise position swaps between two neighboring vertices, and we use the proof to construct a planning algorithm to solve MPP instances in our extended version [32].We then show that each swap can be accomplished by a $\mathcal{O}(|\mathcal{V}|)$-sequence of motions. We further show that the amortized length of motion sequence for permuting the position of two arbitrarily distant vertices is also $\mathcal{O}(|\mathcal{V}|)$. Therefore, the total length of motion sequence to solve the pebbling problem is $\mathcal{O}(|\mathcal{V}|^{2})$. To accomplish each position swap, we need to move the vacant vertex, which is not occupied by any robot, near the to-be-swapped vertices. We could optimize the motion plan by moving the vacant vertex along the shortest path. It is convenient to pre-compute the all-pair shortest paths, which costs $\mathcal{O}(|\mathcal{V}|^{3})$. This step dominates the complexity of computing the motion sequence.

In general, robot start/goal positions do not coincide with the graph vertices and we use local navigations to move robots to/from graph vertices. To this end, we propose a heuristic method based on RVO [29] and CAPT [14]. The RVO algorithm resolves local collisions between robots, and CAPT further uses the Hungarian algorithm to solve unlabeled navigation problems by assigning robots to goal positions. Since our graph vertices do not coincide with robot start/goal positions, we use a similar technique to assign each $\mathbf{s}^{i}/\mathbf{t}^{i}$ to some graph vertex. This assignment can be arbitrary in our method because robots can be moved to any graph vertices and permuted later, while we propose computing an as-close-as-possible assignment via optimal transport by solving the following mixed integer linear programming:

where $z_{\mathbf{s}}^{ij}=1$ implies assigning $\mathbf{s}^{i}$ to $\mathbf{v}^{j}$. After the assignment is computed, we can move each $\mathbf{s}^{i}$ to $\mathbf{v}^{j}$ using RVO. An identical procedure is used to assign $\mathbf{t}^{i}$ to $\mathbf{v}^{j}$ with decision variables denoted as $z_{\mathbf{t}}^{ij}$. Finally, the graph vertex permutation can be determined from $z_{\mathbf{s}}^{ij}$ and $z_{\mathbf{t}}^{ij}$. We set $\sigma(j)=j^{\prime}$ if $z_{\mathbf{s}}^{ij}=1$ and $z_{\mathbf{t}}^{ij^{\prime}}=1$ for some $i$.

We evaluate the performance of our method on a set of $5$ benchmarks. The algorithm is implemented in C++ and tested on a desktop machine with an Intel Core i7 CPU running at 3.30GHz with 16GB of RAM. We have also compared our algorithm with [16, 25], which are recently proposed methods for centralized motion planning in continuous workspaces. Our method can compute motion plans for up to $200$ robots in less than $10$ seconds shown in Figure 7 (a) and (b), where the cost of pebble-graph embedding (Algorithm 1) is marginal and the majority of computation is spent on scheduling pebble motions on the graph. The detailed timing and trajectory qualities of the three methods are summarized in Table I. These results are derived by performing 50 random trials and taking the average. For each random trial, the robots’ start positions are randomly sampled.

Our first benchmark is illustrated in Figure 7 (a), where we compare our method and prior work [16] under different robot densities. The robots’ start positions are shown in red and their goal positions are derived by randomly permuting the starts. For $100$ robots, our method takes $8$ seconds to compute the motion plan (including pebble-graph construction and pebble motion planning, but not local navigation), while it takes $10$ seconds to compute the motion plan using [16]. We then increase the number of robots to $200$ in Figure 7 (a), where our algorithm still takes $8$ seconds to find a motion plan, while the computational cost of [16] is $18$ seconds using a rectangular grid as the graph. We can further increase the number of robots up to $600$, resulting in robots occupying $47\%$ of $|\mathcal{W}|$, and the motion plan can be computed within $313$ seconds. We have tried a similar benchmark (Figure 7 (b)) with a different obstacle setup, where the overall computation time of our method is $2$ seconds for $70$ robots. We can increase the number of robots up to $710$, occupying $55\%$ of $|\mathcal{W}|$, for which our method computes a motion plan within $453$ seconds. Our third benchmark in Figure 7 (c) involves a maple-shaped boundary with a narrow passage and our forth benchmark contains irregular obstacles. Both [16, 25] fail for these irregular cases. This is because the regular grid used by [16] does not fit into the narrow space and the well-separated assumption of [25] does not hold, In contrast, our method succeeds in computing a motion plan within $220$ seconds for $170$ robots in Figure 7 (c) and $130$ robots in Figure 7 (d), with a success rate of $100\%$ over all $20$ executions.

Furthermore, we implement a similar scenario to [25], but unlike their experiments, the robots in our experiments are randomly placed. As a results, some robots do not have sufficient free space to accomplish a revolving motion, which is needed in [25] to find feasible motion plans. However, our method successfully moves robots into inscribed circles via local navigation, and revolving motions can still be performed. These behaviors are illustrated in the video and the overall computation time is less than $3$ seconds.

In Figure 9, we have profiled the success rate of our method and [16] under different scenarios and robot densities. Note that our method relies on a randomized, greedy Algorithm 1 to construct the pebble graph, so it is possible to improve our success rate by running Algorithm 1 multiple times using different random seeds until a solution is found, as in Line 15 to Line 17 of Algorithm 1.

We presented a new method to bridge the gap between continuous MPP planning and discrete pebble-graph motion. We first use medial axis analysis to extract critical information from the workspace, i.e. skeleton lines and inscribed circles. Using this information, we convert the free space into a pebble-graph via embedding.

We show that motion planning on the pebble-graph is always feasible under mild assumptions and general MPP instances can be reduced to graph pebbling problems via local navigation. We conduct experiments using a set of $5$ challenging benchmarks and achieve a $100\%$ success rate under robot densities less than $37\%$. In Figure 10, the robots take up $68\%$ of the free space, which implies that our method can work under extreme robot densities. The major limitation of our method lies in the overly conservative conditions derived in Section III which can leave some gap regions between robots. In large open areas, regular grids can have better space coverage [16]. Our future work would consider hybrid graph embedding techniques that combine multiple space tiling patterns, and we plan to derive improved boundary conditions for neighboring inscribed circles to accommodate more robots. Another limitation is that, our Algorithm 1 only considers robots’ start positions, and ignores their goals. This works well if the goal set is derived by permuting the start positions (as in Figure 7 (ab)), but the local navigation can fail if goal sets are far from the start positions (as in Figure 7 (cd)). \AtNextBibliography