Dynamic teams of robots as ad hoc distributed computers: reducing the complexity of multi-robot motion planning via subspace selection

Abstract

We solve the multi-robot path planning problem using three complimentary techniques: (1) robots that must coordinate to avoid collisions form temporary dynamic teams. (2) Robots in each dynamic team become a distributed computer by pooling their computational resources over ad hoc wireless Ethernet. (3) The computational complexity of each team’s problem is reduced by carefully constraining the environmental subspace in which the problem is considered. An important contribution of this work is a method for quickly choosing the subspace, used for (3), to which each team’s problem is constrained. The heuristic is based on a tile-like pebble motion game, and returns true only if a subset of the environment will permit a solution to be found (otherwise it returns false). We perform experiments with teams of four and six CU Prairiedog robots (built on the iRobot Create platform) deployed in a large residence hall, as well as ten robots in random simulated environments.

Appendices

Appendix A: Description of C-FOREST distributed branch and bound

In this section we describe the C-FOREST algorithm. C-FOREST is a distributed branch and bound technique for parallelizing sampling-based motion planning algorithms across multiple CPUs. C-FOREST is designed for use with asymptotically optimal sampling-based motion planning algorithms like RRT-$\#$ and RRT*.

C-FOREST works by having each CPU build its own random tree. Whenever a more optimal path is discovered by the tree on any CPU, then that path is broadcast to all CPUs so that it can be incorporated into the trees being built on other CPUs.

C-FOREST works for two reasons. (1) It enables “lucky” random exploration (that discovers better homotopy classes or basins of attraction) to be shared across all trees during the planning process so that all trees benefit from the good fortune of any tree. (2) It allows us to ignore portions of the space that cannot possibly lead to better solutions.

(2) is often accomplished with the help of a heuristic. For example, if the length of the geodesic from start to a new point plus the length of the geodesic from the new point to the goal is longer than the best path — then the new point need not be considered because it cannot be part of a path that is shorter than the best one already found. Given the length of the best known path, it is possible to sample directly from the portion of space that yields points shorter than the best solution. The latter heuristic was first used in Any-Time RRT (Ferguson and Stentz 2006), and was previously used in the C-FOREST distributed branch and bound RRT* in our previous work (Otte and Correll 2013b; Otte 2011); more recently, it was used in conjunction with RRT* for the single-robot case in the Bit* algorithm (Gammell et al. 2014).

It is important to note that C-FOREST only provides benefits after the first solution is found (by any CPU). Thus, while C-FOREST can be used with feasible planning algorithms like RRT, it behaves like OR-parallelization for feasible planning.

For ease of presentation, in this section we assume that the robots in the dynamic team $R$ are labeled ${r_1, \ldots , r_{|R|}}$ such that ${R= \{r_1, \ldots , r_{|R|}\}}$. Each robot $r_1$ grows a unique random tree through the team’s combined configuration space $\mathcal {S}_{R}$ from $s_{R}^{\mathrm {start}}$ to $s_{R}^{\mathrm {goal}}$.

The quantity $G(\mathrm {bestLen})$ represents the subset of space in which $\mathbf {geodesic}(s_{R}^{\mathrm {start}}, s_{R}) + \mathbf {geodesic}(s_{R}, s_{R}^{\mathrm {start}}) < \mathrm {bestLen}$, where $\mathbf {geodesic}(A,B)$ returns the length of the geodesic from A to B.

Appendix B: Messages used in low level subroutines

Robots broadcast messages to each other to communicate their intentions, to solve their centralized motion planning problems using C-FOREST distributed computation, and to coordinate their progress along the agreed upon multipaths. Let the overall message be denoted $M$. In our algorithm presentation we use the C/C++ language convention that sub-fields of the message data structure are denoted using the ‘.’ token. Messages from robot $r_i$ contain the following sub-fields, (similar quantities are also tracked locally on each robot and for brevity we omit repeating their definitions, but they appear without the ‘$M.$’ prefix in the presentation of our algorithms):

• $M.\mathrm {epoch}$ is the current planning epoch of $r_i$.

• $M.\mathrm {data}$ contains data about each of the j robots that $r_i$ knows about (either directly or indirectly via other robots).

• $M.\mathrm {path}$ contains $P_{i}$ the path $r_i$ is using to navigate (that is, the subset between its current location and $x_{i}^{\mathrm {goal}}$)—including its time parameterization.

• $M.\mathrm {behindSchedule}$ is the amount of time that the sending robot’s team is behind schedule (with respect to the time parameterization of $P_{i}$).

• $M.\mathrm {data}[k]$ stores data about $r_j$, the k-th robot $r_i$ knows about.

• $M.\mathrm {data}[k].\mathrm {id}$ stores j

• $M.\mathrm {data}[k].\mathrm {start}$ stores $x_{j}^{\mathrm {start}}$.

• $M.\mathrm {data}[k].\mathrm {goal}$ stores $x_{j}^{\mathrm {goal}}$.

• $M.R$ contains a list of all robots in the current team.

• $M.\mathrm {teamData}$ contains additional data about each of the robots in $r_i$’s current team.

• $M.\mathrm {teamData}[k]$ stores data about the k-th robot in $r_i$’s current team. Note that each robot places itself into position $k=0$ when populating message data.

• $M.\mathrm {teamData}[k].\mathrm {id}$ the k-th robot’s id j

• $M.\mathrm {teamData}[k].\mathrm {start}$ stores $s_{j}^{\mathrm {start}}$.

• $M.\mathrm {teamData}[k].\mathrm {goal}$ stores $s_{j}^{\mathrm {goal}}$.

• $M.\mathrm {teamData}[k].\mathrm {epoch}$ stores the k-th robot’s epoch.

• $M.\mathrm {bestSolution}$ is the best solution $\hat{\mathbf {P}}_{R}$ (found so far) currently known to $r_i$ for the sub-problem currently being solved.

• $M.\mathrm {bestLen}$ is the length of the best solution (found so far), $\Vert \mathbf {P}^*_{R}\Vert$.

• $M.\mathrm {bestID}$ is the ID of the robot that generated the best solution (found so far).

• $M.\mathrm {agreeSet}$ is the subset of the current team that believes $\hat{\mathbf {P}}_{R}$ is the best solution (found so far).

• $M.\mathrm {finalSet}$ is the subset of the current team that has submitted a final solution (is done planning for the current sub-problem).

• $M.\mathrm {movingSet}$ is the subset of the current team that has started moving.

Appendix C: Description of lower level subroutines

The initialization routine $\mathbf {Initialize}()$ appears in Algorithm 4. Each robot starts in its own team and initializes its team’s multipath to contain its starting location so that if other robot’s paths conflict with this starting location, they will detect that a team combination should occur.

The reset routine ${\mathbf {Reset}()}$ is described in Algorithm 5 and run before a new motion planning sub-problem is solved; it is responsible for re-initializing all C-FOREST related data (lines 1–4) as well as all data that is used to coordinate movement with a team (lines 5–9).

$\mathbf {waitForDataFromTeammates}()$ appears in algorithm 6 and causes the calling process to sleep until this robot has received data from all members of its current team.

$\mathbf {BroadcastThread}()$ is responsible for sending messages to other robots. It does not appear in pseudocode but, simply packages all data related to movement and planning into a message, and then broadcasts that message on an open channel (to which all robots listen). The broadcast thread is set to run no faster than $\omega$ hz for user-defined $\omega$ so that the communications channel is not saturated. If, in practice, a more sophisticated ad hoc communication protocol is used, then this can be replaced appropriately.

$\mathbf {ListenThread}()$ is responsible for listening for incoming messages from other robots and appears in Algorithm 7. Each time a new message is received (line 2) the data is combined with the local data on the robot to reflect the most up-to-data epoch and path information for nearby robots in general (line 3). The subroutine $\mathbf {mergeAllNewConflicts}$ that appears in the if-statement on Line 4 is responsible for detecting when a team merge operation must take place in response to new message data (and is described in Algorithm 8). The Boolean flag $\mathrm {NewConflictsDetected}$ is set to true if this happens (line 5). If the sending robot is already in the receiving robot’s team (check on line 6), then message passing is used to synchronize the team’s start of movement (lines 7–8 and 13–14), or to compute and improve the C-FOREST solution (lines 9–12). Note that $\mathrm {agreeSet}$ is maintained as the set of robots that agree on the best solution found so far in the C-FOREST algorithm, and $\mathrm {finalSet}$ is similarly maintained as the set of robots that agree on the final solution. These sets are used to ensure the team reaches consensus on a solution before moving along any solution.

$\mathbf {mergeAllNewConflicts}(M, R, \mathrm {teamData})$ appears in Algorithm 8 and is responsible for detecting when a robot’s team must be merged with other robots and teams. The epoch count of a team is always increased to the maximum epoch of any of its robots (line 2–4) to ensure that all robots in a team are computing on the most up-to-date problem faced by that team. Two conditions (both on line 5) can cause a new problem to need to be solved (in which case the epoch number is incremented). The first is if a robot not currently in this robot’s team has a conflicting path; the second is if the sending robot advertises that its own team includes a robot from the receiving robot’s team and it has a higher epoch number (we assume that agents are honest, and so the sending robot must have detected that a robot in the receiving robot’s team is conflicting). In the latter case we must increment the epoch to account for the unlikely event that multiple teams are merging at the same time; doing this ensures that the new problem has an epoch number larger than any problem that may have previously been solved by any of the new team’s members. If the epoch number has been increased, then all relevant new robots are merged into the receiving robot’s team (lines 7–11) and the algorithm returns true. Otherwise the algorithm returns false.

The subroutine $\mathbf {moveAlong}(P_{r})$ is responsible for controlling robot $r$’s movement along $P_{r}$ such that team movement along their shared multipath is coordinated. It appears in Algorithm 9. We note that various coordination mechanisms could be used, the one presented here is simple to implement but may be too naive for many problems (e.g., such as those involving fixed-wing aircraft). This version works well for ground vehicles moving at low speeds. All robots broadcast how far they are behind schedule, and each member of the team waits, when necessary, to match the progress of the robot that is the most behind schedule.

When a team has finished computing a solution to the multi-robot planning problem then ${\mathbf {RobotsAgree}()}$ shown in Algorithm 10, is used to reach consensus on the particular solution that the team uses (since some robots may not yet have received the best solution found by any member of the team). This routine helps with the synchronization of the beginning of movement, which can be initiated by any robot that discovers all team members agree on the final solution (line 1) or by a robot that discovered what it thinks is the best solution—as long as all robots agreed it was the best solution at some previous point. We assume that all robots are honest and seek to use the group optimal solution. However, in rare situations this alternative mechanism for starting movement may cause the team to use a slightly outdated solution; although, it will not cause conflicts/collisions due to the fact that this robot could not possibly have agreed that some other (better) solution was the final solution if it believes its own (worse) solution is still the best. In practice this decreases consensus time by one team-wise message propagation by allowing this relaxed form of consensus to be computed in parallel to the motion planning solution (this is particularly helpful in low-communication environments). Note that if any robot discovers that one of its teammates has started moving, then it also starts moving according to the same solution (even if it previously did not believe it was the best). It is better to agree on a lower quality solution than for two different subsets of the team to move along conflicting solutions.

$\mathbf {processMoveMessage}(M)$ in Algorithm 11 is responsible for updating the robot’s data to reflect the movement agreements of its current team. On lines 1–3 the receiving root checks to make sure that the sending robot believes they are in the same team (line 2) and that they agree on the current epoch (line 3).