Multi-Robot Path Planning in Complex Environments via Graph Embedding

As illustrated in Figure 2 (a), we consider connected graphs consisting of simple loops with 3 vertices that are connected by additional inter-cell edges. We can show that MPP instances restricted to such graphs are always feasible:

We only give a sketch of proof and refer readers to [17] for a complete proof. Any permutation $\sigma$ of robot positions can be decomposed into a set of pairwise swaps. To swap the location of two robots, $\mathbf{x}_{i}$ and $\mathbf{x}_{j}$, we first find a path in $\mathcal{G}$ connecting $\mathbf{x}_{i}$ and $\mathbf{x}_{j}$, which is always possible as $\mathcal{G}$ is connected, denoted as $\mathbf{x}_{i},\mathbf{x}_{1},\mathbf{x}_{2},\cdots,\mathbf{x}_{K},\mathbf{x}_{j}$. We can further decompose the swap into a series of sub-swaps:

where each pair of positions in a sub-swap are connected directly by some $\mathbf{e}\in\mathcal{E}$, where $\mathbf{e}$ is either a loop edge or an inter-cell edge. In either case, the sub-swap can be performed with the help of a nearby vacant vertex as illustrated in Figure 2. Finally, such vacant vertex must exist because our number of robots is strictly less than the number of vertices and the vacant position can be moved anywhere via vacant moves.

In the worst case, the makespan of above mentioned MPP solutions is $\mathcal{O}(|\mathcal{X}|^{2})$. This is because we have only one vacant vertex, which is needed to perform every sub-swap. When more vacant vertices are available, we can parallelize the sub-swaps and improve the makespan. We present a space-time search algorithm to find parallel sub-swaps, which is similar to [15] in principle but adapted to graphs of more general topology. As illustrated in the inset, we first use the CLINK algorithm [32] to cluster the loops into a binary tree. This algorithm starts by treating each loop as a separate cluster, and then iteratively merge two smallest clusters (with the smallest number of vertices) until only one cluster is left. We then iteratively merge small leaf nodes until all leaf nodes have more than $K>1$ loops ($K=4$ in the inset), where $K$ is a user specified parameter determining the congestion-level. In the worst case, this method requires $\lceil|\mathcal{X}|/(3K)\rceil$ vertices to be left vacant. Next, we introduce a vacant vertex for each leaf node. As a result, cyclic moves and vacant moves can be performed parallelly within a leaf node and two pairs of connected leaf nodes can perform two robot position swaps in parallel. Finally, we adopt the divide-and-conquer algorithm proposed in [15]. For every internal binary tree node, we consider their two children, which are two connected sub-graph. We swap robots between the two sub-graphs until $\mathbf{x}_{i}$ and $\mathbf{x}_{\sigma(i)}$ belongs to the same sub-graph for all $i=1,\cdots,N$. This procedure is performed recursively from the root node to all the leaves, where the swaps within different sub-trees are performed in parallel.

The major challenging in the above algorithm lies in the scheduling of parallel robot position swaps between sub-graphs, for which we use a space-time data structure. We denote $\mathcal{G}_{i}$ as the $i$th leaf node and we assume there are $N_{1}$ leaf nodes in one of the sub-graph and $N_{2}$ in the other ($N_{1}+N_{2}\leq\lceil|\mathcal{X}|/(3K)\rceil$). The inter-sub-graph position swaps are decomposed into several rounds, where each round consists of several parallel position swaps between two connected leaf nodes. We denote as $\mathcal{G}_{i,t}$ as the state of $\mathcal{G}_{i}$ at the beginning of $t$th round. We further connect $\mathcal{G}_{i,t}$ and $\mathcal{G}_{j,t+1}$ using a space-time edge $\mathbf{e}_{ij,t}$ if two conditions hold: 1) $\mathcal{G}_{i}$ and $\mathcal{G}_{j}$ are connected by some inter-loop edge $\mathbf{e}\in\mathcal{E}$; 2) $\mathcal{G}_{i}$ and $\mathcal{G}_{j}$ belongs to the same sub-graph, i.e., $i,j\leq N_{1}$ or $N_{1}<i,j\leq N_{1}+N_{2}$. Our space-time data structure is defined as the space-time graph $<\{\mathcal{G}_{i,t}|t\leq t_{0}+1\},\{\mathbf{e}_{ij,t}|t\leq t_{0}\}>$, where $t_{0}$ is the maximal allowed number of rounds. Note that the space-time graph is directed with each $\mathbf{e}_{ij,t}$ directing from $\mathbf{e}_{j,t+1}$ to $\mathbf{e}_{i,t}$. The inter-sub-graph position swaps can be accomplished by multi-rounds of swaps between leaf nodes, which in turn corresponds to selecting a set of space-time edges $\mathbf{e}_{ij,t}$. Our algorithm greedily select a set of space-time edges leading to an inter-sub-graph position swap in the earliest round. After each selection, we remove the conflicting space-time edges before selecting the next set of edges, until all the robots belong to the correct sub-graph. When no position swaps can be found for the given $t_{0}$, we introduce a new round and increase $t_{0}$ by one. This procedure is guaranteed to terminate.

Our method is outlined in Algorithm 1, which is composed of two main steps. First, we consider all pairs of connected leave nodes $<\mathcal{G}_{a,t},\mathcal{G}_{b,t}>$ belonging to different sub-graphs. We find the two shortest paths connecting from $\mathcal{G}_{a,t}$, $\mathcal{G}_{b,t}$ to some leaf node containing a robot that belongs to a different sub-graph. Specifically, we run Dijkstra’s algorithm with all-one edge weights from both $\mathcal{G}_{a,t}$ and $\mathcal{G}_{b,t}$. We terminate the Dijkstra’s algorithm at any $\mathcal{G}_{i,t}$ satisfying the following condition:

where we define $\#\mathcal{G}_{i,t}$ as the number of robots within $\mathcal{G}_{i,t}$ that belongs to a different sub-graph from $\mathcal{G}_{i,t}$ at the beginning of the $t$th round. Intuitively, Equation 1 ensures that swapping one more robot away from $\mathcal{G}_{i}$ during the $t$th round will not cause conflict with the number of to-be-swapped robots required by other swapping operations in the future rounds. Finally, note that we consider nodes pairs in a time-ascending order. The first pair of leaf nodes, for which such paths can be found, are selected, which corresponds to the position swap in the earliest round.

Our second step would remove three types of conflicting edges: 1) If an edge $\mathbf{e}_{ij,t}$ is selected, then all the edges $\{\mathbf{e}_{kj,t},\mathbf{e}_{ik,t}|k=1,\cdots,N_{1}+N_{2}\}$ are removed because they conflict with the edges along the two paths; 2) all the edges $\{\mathbf{e}_{ak,t},\mathbf{e}_{bk,t}|k=1,\cdots,N_{1}+N_{2}\}$ are removed because they conflict with the inter-sub-graph swap between $\mathcal{G}_{a,t}$, $\mathcal{G}_{b,t}$; 3) Let’s define $\#_{/}\mathcal{G}_{j,t}$ as the number of robots belonging to the same sub-graph as $\mathcal{G}_{j}$ at the beginning of $t$th round ($\#\mathcal{G}_{j,t}+\#_{/}\mathcal{G}_{j,t}+1$ equals to the number of vertices in $\mathcal{G}_{j}$), then all the edges:

should be removed. This is because selecting edge $\mathbf{e}_{kj,t}$ implies that a robot $\mathbf{x}_{a}\in\mathcal{G}_{k,t}$ must be swapped with another robot $\mathbf{x}_{b}\in\mathcal{G}_{j,t}$ at $t$th round. We must have $\mathbf{x}_{b}$ belongs to the same sub-graph as $\mathcal{G}_{j}$ at the beginning of $t$th round, because otherwise the shortest path would stop at $\mathbf{x}_{b}$ instead of moving on to $\mathbf{x}_{a}$. However, $\underset{t^{\prime}=t,\cdots,t_{0}}{\min}\#_{/}\mathcal{G}_{j,t^{\prime}}=0$ implies that, if $\mathbf{x}_{b}$ was swapped out of $\mathcal{G}_{j}$ during $t$th round, there will not be enough to-be-swapped agents (that belongs to the same sub-graph as $\mathcal{G}_{j}$) during some future rounds.

Since $\bar{\mathcal{G}}$ is a simplicial complex, each cell is a triangle and we denote the 3 vertices of this triangle as: $\bar{\mathbf{x}}_{1,2,3}$. The distance-$r$ corner points can be computed using the following formula, as illustrated in Figure 4 (a):

We only show formula for $\mathbf{x}_{2}$ and the formulas for $\mathbf{x}_{1,3}$ are symmetric. We will mark the cell as invalid and remove the 3 vertices from $\mathcal{G}$ if there are overlappings between $C(\mathbf{x}_{1,2,3})$.

Even when the 3 corner points are not overlapping, robots can still collide when they perform cyclic moves along the 3 loop edges with constant speed, as illustrated in Figure 4 (b). To derive a condition for collision-free cyclic moves, we assume that the 3 robots trace out a trajectory $\tau_{1,2,3}(t)$ where $t\in[0,1]$. At time $t$, the 3 robots are at positions:

and we have collision-free cyclic moves if:

Here we use cyclic subscripts to denote 3 symmetric conditions. The left-hand side of Equation 3 is quadratic when squared and determining the smallest value of a quadratic equation in $[0,1]$ has closed-form solution, which can be used for determining whether a cell is invalid in Algorithm 2.

In addition to determining the validity of a cell, our mesh optimization algorithm (Section VI) requires an operator that can modify an invalid cell’s geometric shape to achieve validity using numerical optimization. To this end, we derive a condition equivalent to Equation 3 but does not contain continuous time variable $t$, because the variable $t$ can take infinitely many values from $[0,1]$ leading to a difficult semi-infinite programming problem [33]. Taking one of the equations $\|\mathbf{x}_{1}(t)-\mathbf{x}_{2}(t)\|\geq 2r$ in Equation 3 for example (the other 2 cases are symmetric), its left-hand-side is a polynomial of a single time variable $t$. To eliminate $t$, the Fekete, Markov-Lukaćz theorem [34] can be applied to show that, if Equation 3 holds, then we have:

where $\alpha_{1,2,3,4}$ are four unknown variables to be fitted. The $2\times 2$ PSD-cone constraint is equivalent to two quadratic constraints: $\alpha_{1}\alpha_{3}\geq 0$ and $\alpha_{1}\alpha_{3}\geq\alpha_{2}^{2}$. By equating coefficients in the quadratic constraints, we can express $\alpha_{1,2,3,4}$ in terms of $\mathbf{x}_{1,2,3}$ to get the following equivalent form:

where $\alpha_{4}$ is an additional decision variable that cannot be eliminated. But unlike $t$, we only need to satisfy Equation LABEL:eq:equi_cond2 for a single $\alpha_{4}$. We summarize this result below:

We show that, as long as condition 2 is satisfied, condition 3 must also be satisfied, so $\Phi(\bar{\mathcal{G}})$ always satisfy condition 3. Condition 3 requires that a robot can move to a vacant position along any $\mathbf{e}\in\mathcal{E}$. We analyze two cases: $\mathbf{e}$ being a loop edge or $\mathbf{e}$ being an inter-cell edge.

Loop edge: If one of the 3 corner points $\mathbf{x}_{i}$ in a cell is vacant and another robot $\mathbf{x}_{j}$ is moving along a loop edge to $\mathbf{x}_{i}$, we show that this move is always collision-free. We prove by contradiction as illustrated in Figure 5. If this move is not collision-free, then the third corner point $\mathbf{x}_{k}$ must be in the way between $\mathbf{x}_{i}$ and $\mathbf{x}_{j}$. However, in this case, cyclic moves in the cell is not collision-free, which contradicts condition 2.

Inter-cell edge: If two neighboring triangles satisfy condition 2 and share an edge $\bar{\mathbf{e}}\in\bar{\mathcal{E}}$, then cyclic moves inside each triangle are collision-free. As illustrated in Figure 6, if robot $\mathbf{x}_{i}$ is to be moved to a vacant position $\mathbf{x}_{j}$ along an inter-cell edge, then we can rotate $\mathbf{x}_{i}$ (along with the two other robots) clockwise and $\mathbf{x}_{j}$ counterclockwise until the line-segment $\mathbf{x}_{i}-\mathbf{x}_{j}$ is orthogonal to $\bar{\mathbf{e}}$ and the convex hull of $C(\mathbf{x}_{i})$ and $C(\mathbf{x}_{j})$ does not contain other agents so the vacant move is collision-free. We prove in Appendix IX that such convex hull always exists between any pair of valid triangles. Finally, we rotate $\mathbf{x}_{i,j}$ reversely to undo extra changes.

Our metric consists of 4 terms $\mathcal{M}_{\#r},\mathcal{M}_{\#rc},\mathcal{M}_{sd},\mathcal{M}_{g}$ that can either be a function of the mesh $\bar{\mathcal{G}}$ or the graph $\mathcal{G}$. These four terms measure the robot packing density as well as the area covered by the robots in $\mathcal{W}$. Our first term, $\mathcal{M}_{\#r}$ is defined as: $\mathcal{M}_{\#r}(\mathcal{G})\triangleq|\mathcal{X}|$, which is the number of vertices contained in $\mathcal{G}$. This term reflects the number of robots that $\mathcal{G}$ can hold, but MPP problems might not be feasible for all these robots because $\mathcal{G}$ might not be connected. $\mathcal{M}_{\#r}$ is a heuristic that guides our algorithm to find more valid cells at an early stage and then try to connect them. Our second term, $\mathcal{M}_{\#rc}$, is the number of robots in the largest connected component of $\mathcal{G}$:

which is the maximal number of robots for which MPP problems are feasible. Note that $\mathcal{M}_{\#r,\#rc}$ are discrete, non-differentiable terms that are related to both the topology and geometry of $\bar{\mathcal{G}}$. To increase $\mathcal{M}_{\#r,\#rc}$, we need to update the shape of triangles in $\bar{\mathcal{G}}$ and also update their connectivity so that they are valid. Our third term is the agent packing density averaged over the valid cells:

$\mathcal{M}_{sd}$ is a heuristic guidance term that biases our algorithm towards denser robot packing, when only a subset of the workspace is covered by the robot. Our last metric term $\mathcal{M}_{g}$ is the 2D AMIPS energy [36] using the smallest regular triangle satisfying condition 2 as the target shape. $\mathcal{M}_{g}$ is differentiable, purely geometric, and related to $\bar{\mathcal{G}}$ only.

The greedy algorithm is summarized in Algorithm 3, which exhaustively tries one of the re-meshing operators: edge-flip, edge-split, edge-collapse, vertex-smoothing, local-optimization, and global-optimization. Our re-meshing operators, except for global-optimization, are all local and only affect a small neighborhood of cells. The first 4 re-meshing operators are inherited from [19, 35], as illustrated in Figure 7. Edge-flip is guided by $\mathcal{M}_{g}$ and is mostly applied to obtuse triangles and turns them into acute triangles. Edge-split and edge-collapse are used to remove tiny edges and split long edges in $\bar{\mathcal{G}}$, respectively. Finally, vertex-smoothing locally optimizes $\mathcal{M}_{g}$ with respect to the one-ring neighborhood of a single vertex. The four operators combined can turn a low-quality mesh into one with nice element shapes.

Our algorithm uses two passes. In the first pass, we allow the algorithm to explore a larger search space by allowing both edge-collapse and edge-split. To ensure the convergence of the first pass, we avoid the cases where consecutive edge-flip and edge-split operators happen for a same edge repeatedly. Allowing edge-split will create more triangles and potentially lead to denser packing of robots. In practice, we split a $\bar{\mathbf{e}}$ when its length is larger than $1.3\times$ of the optimal length $\bar{\mathbf{e}}^{*}$. Here the optimal length is the edge length of the smallest regular triangles satisfying condition 2, which equals to: $|\bar{\mathbf{e}}^{*}|=(2\sqrt{3}+4)r$. In the second pass, we are more conservative and disallow edge-split. This pass can be considered as post-processing, merging too small triangles and simplifying the robot layout. Within each pass, we check every possible operator and only accept operators when the weighted metric $\mathcal{M}_{\#r}+w\mathcal{M}_{\#rc}$ monotonically increases and we set $w=10$. We also reject operators that violate our assumption on $\bar{\mathcal{G}}$ being a cell decomposition, i.e. introducing non-manifold connection and flipped cells. The non-manifoldness of $\bar{\mathcal{G}}$ can happen only in edge-collapse and can be avoided by the link condition check [37]. A flipped cell has a negative area which can be checked by the following constraint:

Aside from the four remeshing operators inherited from [19, 35], we introduce one local- and one global-optimization operators. These operators use the primal-dual interior point method to optimize the geometric shape of the cells, taking condition 2 as hard constraints. At the same time, we maximize $\mathcal{M}_{sd}$ by minimizing the area of valid cells, leading to a denser robot packing. Specifically, for a cell in $\bar{\mathcal{G}}$ with vertices $\bar{\mathbf{x}}_{1,2,3}$, we solve the following problem in the local-optimization operator:

where we require that the entire mesh to be a cell decomposition, so we add Equation 5 for all cells. We also require that those originally valid cells stay valid, so we add Equation LABEL:eq:equi_cond2 for all valid cells. This optimization can be solved efficiently because it is local. Since we only treat the 3 vertices of a single cell in $\bar{\mathcal{G}}$ as decision variables, most of the terms in the objective function and constraints are outside the 1-ring neighborhood of $\bar{\mathbf{x}}_{1,2,3}$ and not influenced by the decision variables, which can be omitted. Note that Equation LABEL:eq:equi_cond2 is expressed in terms of the corner points $\mathbf{x}_{1,2,3}$, instead of mesh vertices $\bar{\mathbf{x}}_{1,2,3}$. When the optimizer requires partial derivatives of Equation LABEL:eq:equi_cond2 against $\bar{\mathbf{x}}_{1,2,3}$, we use the chain rule on the relationship Equation 2.

The global-optimization operator is very similar to the local-optimization operator, which also takes the form of Equation 6, but we set all the variables $\bar{\mathbf{x}}$ as decision variables. The global-optimization is more expensive than all other operators, but we found that this last operator can significantly improve robot packing density and it is more efficient than applying the local-optimization operator to each of the cell.