Goal assignment and trajectory planning for large teams of interchangeable robots

Abstract

This paper presents Goal Assignment and Planning: a computationally tractable, complete algorithm for generating dynamically feasible trajectories for $N$ interchangeable (identical) robots navigating through known cluttered environments to $M$ goal states. This is achieved by assigning goal states to robots to minimize the maximum cost over all robot trajectories. The computational complexity of this algorithm is shown to be polynomial in the number of robots in contrast to the expected exponential complexity associated with planning in the joint state space. This algorithm can be used to plan trajectories for dozens of robots, each in a potentially high dimensional state space. A series of planar case studies are presented and finally, experimental trials are conducted with a team of six quadrotor robots navigating in a constrained three-dimensional environment.

Appendix: Extended proofs

Lemma 2

The conditions in (7), (8) satisfy the antisymmetry property $i \prec j \implies i \nsucc j$

Proof

Assume $i \prec j$ and $i \succ j$. Without loss of generality, this requires one of the following cases:

$$\begin{aligned} \begin{aligned} \text {Case 1:}&\quad { {s}}_{i} \in P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}}), { {s}}_{j} \in P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _i}})\\ \text {Case 2:}&\quad { {g}}_{{\phi ^\star _j}} \in P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _i}}),{ {g}}_{{\phi ^\star _i}} \in P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}})\\ \text {Case 3:}&\quad { {s}}_{i} \in P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}}), { {g}}_{{\phi ^\star _i}} \in P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}}) \end{aligned} \end{aligned}$$

First, consider Case 1. This implies both of the following equalities:

$$\begin{aligned} ||P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}})||&= ||P^\star ({ {s}}_{j},{ {s}}_{i})||+||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _j}})||\\ ||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _i}})||&= ||P^\star ({ {s}}_{i},{ {s}}_{j})||+||P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _i}})|| \end{aligned}$$

Applying these to Eq. (5) yields:

$$\begin{aligned} \begin{aligned}&\max \Bigg (||P^\star ({ {s}}_{i},{ {s}}_{j})||+||P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _i}})||, ||P^\star ({ {s}}_{j},{ {s}}_{i})||\\ {}&\quad +\,||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _j}})|| \Bigg ) \\&\le \max \left( ||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _j}})||,||P^\star ({ {s}}_{j}, { {g}}_{{\phi ^\star _i}})||\right) \end{aligned} \end{aligned}$$

This is clearly a contradiction and therefore Case 1 is not possible.

It can be similarly shown that Case 2 is also impossible using a change of variables.

Next, consider Case 3. Due to the optimal graph search, either

$$\begin{aligned}&P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _i}}) \subset P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}}) \quad \text { OR } \quad || P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}}) ||\\&\quad = || P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _i}}) || + || P^\star ({ {g}}_{{\phi ^\star _i}},{ {s}}_{i}) ||+ || P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _j}})|| \end{aligned}$$

Regardless of which of these conditions is present, it is relatively straightforward to show that Eq. (5) is violated in much the same manner as Case 1 was handled.

As there are no conditions which allow for both $i \prec j$ and $i \succ j$, it is clear that $i \prec j \implies i \nsucc j$ $\square$

Lemma 3

The conditions in (7), (8) satisfy the transitivity property $i \prec j$ AND $j \prec k \implies i \prec k$

Proof

Assume $i \prec j$ AND $j \prec k$ AND $k \prec i$

Without loss of generality, there are the following conditions which result in this:

$$\begin{aligned} \begin{aligned} \text {Case 1:}&\!\!\quad { {s}}_{i} \!\in \! P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}}), { {s}}_{j} \!\in \! P^\star ({ {s}}_{k},{ {g}}_{{\phi ^\star _k}}), { {s}}_{k} \!\in \! P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _i}}) \\ \text {Case 2:}&\!\!\quad { {s}}_{i} \!\in \! P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}}), { {s}}_{j} \!\in \! P^\star ({ {s}}_{k},{ {g}}_{{\phi ^\star _k}}), { {g}}_{i} \!\in \! P^\star ({ {s}}_{k},{ {g}}_{{\phi ^\star _k}}) \\ \text {Case 3:}&\!\!\quad { {s}}_{i} \!\in \! P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}}), { {g}}_{k} \!\in \! P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}}), { {g}}_{i} \!\in \! P^\star ({ {s}}_{k},{ {g}}_{{\phi ^\star _k}}) \\ \text {Case 4:}&\!\!\quad { {g}}_{j} \!\in \! P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _i}}), { {g}}_{k} \!\in \! P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}}), { {g}}_{i} \!\in \! P^\star ({ {s}}_{k},{ {g}}_{{\phi ^\star _k}}) \end{aligned} \end{aligned}$$

First, consider Case 1. The following conditions result:

$$\begin{aligned} \begin{aligned} ||P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}})||&= ||P^\star ({ {s}}_{j},{ {s}}_{i})||+||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _j}})|| \\ ||P^\star ({ {s}}_{k},{ {g}}_{{\phi ^\star _k}})||&= ||P^\star ({ {s}}_{k},{ {s}}_{j})||+||P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _k}})|| \\ ||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _i}})||&= ||P^\star ({ {s}}_{i},{ {s}}_{k})||+||P^\star ({ {s}}_{k},{ {g}}_{{\phi ^\star _i}})|| \end{aligned} \end{aligned}$$

(10)

it can be shown that the equalities (10) cannot be a solution to the LexBAP and:

$$\begin{aligned}&\left( \sum _{l \in \{ i,j,k \} } ||P^\star ({ {s}}_{l},{ {g}}_{\phi ^\star _{l})}||^p \right) ^{\frac{1}{p}}\\&\quad >\!\left( ||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _j}})||^p \!+\! ||P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _k}})||^p \!+\!||P^\star ({ {s}}_{k},{ {g}}_{{\phi ^\star _i}})||^p \right) ^{\frac{1}{p}} \end{aligned}$$

Therefore, Case 1 is not a valid set of conditions.

Next, consider Case 2:

$$\begin{aligned} \begin{aligned} ||P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _j}})||&= ||P^\star ({ {s}}_{j},{ {s}}_{i})||+||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _j}})||\\ ||P^\star ({ {s}}_{k},{ {g}}_{{\phi ^\star _k}})||&= ||P^\star ({ {s}}_{k},{ {s}}_{j})||+ ||P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _i}})||\\&\quad +||P^\star ({ {g}}_{{\phi ^\star _i}},{ {g}}_{{\phi ^\star _k}})|| \end{aligned} \end{aligned}$$

(11)

Applying Eq. (5) for robots $k$ and $j$ while utilizing Eq. (11) yields:

$$\begin{aligned}&\text {max}(||P^\star ({ {s}}_{k},{ {s}}_{j})||+||P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _i}})||\nonumber \\&\quad +||P^\star ({ {g}}_{{\phi ^\star _i}},{ {g}}_{{\phi ^\star _k}})||,||P^\star ({ {s}}_{j},{ {s}}_{i})||+||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _j}})||) \nonumber \\&\le \text {max}(||P^\star ({ {s}}_{k},{ {s}}_{j})||+||P^\star ({ {s}}_{j},{ {s}}_{i})||\nonumber \\&\quad +||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _j}})||,||P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _i}})||+||P^\star ({ {g}}_{{\phi ^\star _i}},{ {g}}_{{\phi ^\star _k}})||) \end{aligned}$$

(12)

Make the following definitions:

$$\begin{aligned} \begin{aligned}&a \!=\! ||P^\star ({ {s}}_{k},{ {s}}_{j})||&b \!=\! ||P^\star ({ {s}}_{j},{ {g}}_{{\phi ^\star _i}})||&c \!=\! ||P^\star ({ {g}}_{{\phi ^\star _i}},{ {g}}_{{\phi ^\star _k}})|| \\&d \!=\! ||P^\star ({ {s}}_{j},{ {s}}_{i})||&e \!=\! ||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _i}})||&f \!=\! ||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _j}})|| \end{aligned} \end{aligned}$$

(13)

Then, rewrite Eq. (12) using these definitions:

$$\begin{aligned} \text {max}\left( (a+b+c),(d+f)\right) \le \text {max}\left( (a+d+f),(b+c) \right) \end{aligned}$$

If $b+c \ge a+d+f$, it is clear from the previous equation that $b+c \ge a+b+c$, which implies $c\le 0$. As $c$ is defined as $||P^\star ({ {g}}_{{\phi ^\star _i}},{ {g}}_{{\phi ^\star _k}})||$, this value must be strictly greater than zero. Therefore, the fact that $a+d+f\ge b+c$ is used:

$$\begin{aligned} \text {max}\left( (a+b+c),(d+f)\right) \le a+d+f \end{aligned}$$

Clearly $a+d+f\ge d+f$, so this can be simplified to:

$$\begin{aligned} a+b+c\le a+d+f \end{aligned}$$

and further to:

$$\begin{aligned} d+f \ge b+c \end{aligned}$$

(14)

Applying Eq. (5) for robots $i$ and $j$ and using Eq. (11):

$$\begin{aligned}&\max \left( ||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _i}})||,||P^\star ({ {s}}_{j},{ {s}}_{i})||+||P^\star ({ {s}}_{i}, { {g}}_{{\phi ^\star _j}})||\right) \\&\quad \le \max \left( ||P^\star ({ {s}}_{i},{ {g}}_{{\phi ^\star _j}})||,||P^\star ({ {s}}_{j}, { {g}}_{{\phi ^\star _i}})||\right) \end{aligned}$$

Using the definitions in (13), this can be rewritten as:

$$\begin{aligned} \text {max}\left( e,(d+f)\right) \le \text {max}\left( b,f \right) \end{aligned}$$

It can be shown through simple contradiction that $b\ge f$ to simplify this equation to:

$$\begin{aligned} \text {max}\left( e,(d+f)\right) \le b \end{aligned}$$

Which means that $b \ge d+f$. Incorporating the knowledge from (14), this implies that:

$$\begin{aligned} b \ge d+f \ge b+c \implies 0 \ge c \end{aligned}$$

However, $c$ is defined to be strictly greater than zero and therefore it is impossible for Eq. (A) to be satisfied and therefore Case 2 is impossible.

Case 3 is equivalent to Case 2 with a change of variables and is not possible.

Case 4 is equivalent to Case 1 with a change of variables and is not possible.

As there are no conditions which allow $i \prec j$ AND $j \prec k$ AND $k \prec i$, then it is clear that $i \prec j$ AND $j \prec k \implies i \prec k$. Therefore the conditions in Eqs. (7) and (8) satisfy the transitivity property.$\square$