Multiple Path Coordination for Mobile Robots: A Geometric Algorithm

1 Introduction: Path coordination

This paper addresses the following problem: consider n mobile robots sharing the same workspace and plan­ ning their paths independently; n such paths being given we want to devise an algorithm deciding whether coordi­ nated motions exist for the mobile robots along their own paths, so that each robot can reach its own goal with­ out colliding the other ones. The problem is known as the multiple robot path coordination problem [Latombe, 1991b].

Path coordination versus Path planning Multiple robot path coordination and path planning are two re­ lated issues in robot motion planning. In multiple robot path planning the robot paths are not a priori com­ puted. A solution to the multiple robot path planning problem is a collision-free path in the cartesian product of the configuration spaces of all the robots. A solu­ tion to the problem exists iff the start and goal config­ urations belong to a same connected component of the global collision-free configuration space. Searching such a space is a highly combinatorial problem [Hopcroft et al., 1984].

To face this complexity several authors have investi­ gated decoupled schemes 1 . The decoupled approach has been introduced in [Kant and Zucker, 1986]: the method first plans the paths of the robots independently and then computes the velocity profiles so that the robots do not collide. The approach has been further revisited in [Erdmann and Lozano-Perez, 1986; Buckley, 1989; Warren, 1990; Alami et al., 1995].

1 Other schemes for multiple robot path planning have been proposed. For instance some centralized approaches aim at facing the problem complexity with probabilistic al-

The path coordination problem as such has been ad­ dressed in [O'Donnell and Lozano-Perez, 1989] where the notion of coordination diagram has been first introduced. It dealt with two robots, a case which has been also ad­ dressed in [Bien and Lee, 1992; Chang et al., 1994]. A strategy based on dynamic. programming was proposed more recently in [La Valle and Hutchinson, 1996] to ad­ dress problems involving more than two robots.

Objective, approach and contribution We want to solve problems involving more than 100 robots in re­ alistic situations. The algorithm consists in searching a n-dimensional coordination diagram. The main contri­ bution is to propose a bounding box representation of the diagram obstacles. With respect to the previous works above we do not use any regular grid representation. The algorithm is resolution complete and it is complete for a large class of inputs. Its efficiency inherits from the efficiency of simple geometric operations giving rise to a collision-checker dedicated to mobile robot coordination and summarized in Section 2. After having introduced a cell decomposition of the coordination diagram for the case of two robots (Section 3), we extend the algorithm to the general case (Section 4).

2 Paths SA and geometric tools

Paths SA The geometric tools we use are based on the following assumption: the robot paths are sequences of straight line segments (S) and arcs of a circle (A). Such sequences are denoted by SA . This assumption is sup­ ported by both theoretical and practical considerations.

gorithms (see [Svestka and Overmars, 1995) and references therein). From another point of view, cooperation-oriented approaches are based on local informations (potential meth­ ods): see for instance [Reif and Wang, 1995) and [Cao et al., 1997) for a recent overview. Techniques for path coordination are out of the scope of all these methods.

First of all, it has been proved that a collision-free admis­ sible path exists iff there exists a collision-free admissible path of type SA [Laumond, 1986]. Moreover, most of the existing complete motion planners for mobile robots provide solution paths of the type SA (e.g., [Laumond et al., 1994; Latombe, 1991a; Svestka and Overmars, 1995; Mirtich and Canny, 1992]). Finally geometric algorithms like boolean operations or swept volume computations are simple and computationally efficient when dealing with arcs of circle and straight line segments.

Traces A mobile robot path being given, a trace is the volume swept by the robot when moving along the path. Assuming that the robot is a polygon, the trace of a path of type SA is a generalized polygon whose boundary is a sequence of straight line segments and arcs of a circle. [Simeon et al., 1998] have shown how to compute such traces efficiently (Figure 1(a)) .

Coordination configurations To coordinate the mo­ tions of two robots along their own path, it is neces­ sary to compute the intersection of their trace. Fig­ ure 1 shows two traces. The bold sub-path [a1 , bl] (resp. [ a2, b2]) gathers the configurations at which the first (resp. second) robot intersects the trace of the second (resp. first) one. The endpoints of such sub-paths are called coordination configurations. [Simeon et al., 1998] have proposed a geometric algorithm to compute them when the robots are convex polygons and move along SA paths. In this paper we keep the same assumptions.

3 Coordination for two robots

Coordination diagram Coordinating the motion of two robots along two given paths is a classical prob­ lem. Its solution consists in exploring the so-called co­ ordination diagram [O'Donnell and Lozano-Perez, 1989]. Let us consider the two paths "Yl(si) and -y2 (s2 ) in Fig­ ure 2(a). Both coordinates s1 and s2 are assumed to vary from 0 to 1. Figure 2(b) shows the corresponding coordination diagram ( s1 , s2 ): the black domains rep­ resent the set of configuration pairs ( s1 , s2 ) such that the robots collide when they are respectively at config­ urations 'Y1(s1) and 'Y2(s2). Black domains are obsta­ cles to avoid. A coordinated motion exists iff there is a collision-free path in the diagram linking the point (0,0) (the robots are both at the beginning of their own path) to the point (1,1) (the robots are both at the end of their path).

A bounding box representation Our contribution is to propose an algorithm to explore the diagram with­ out computing the exact shape of the obstacles 2 . We use a bounding box representation based on the follow­ ing property: the (minimal} box bounding an obstacle in a coordination diagram is a rectangle whose endpoint coordinates are the coordination configurations 3 . Let us consider the case in Figure 1. The coordinates of four points defining the rectangle in the coordination diagram are respectively (a1,b1), (a1,b2), (a2,b1) and Path decomposition Let us now consider two SA paths 11 and 12 . Instead of applying the bounding box representation directly in the coordination diagram of 11 and 12 , we first apply a path decomposition. Each path is decomposed into its elementary pieces consisting of either straight line segments, or arcs of a circle. Let (/1,i) and (/2 , 1) the pieces sequences of 1 1 and 12 re­ spectively. The coordination diagram for 11 and 12 then appear as the union of the coordination diagrams of the various pairs (11,; , { 2, 1) . For instance, the two paths in Figure 2(a) both consist of 4 arcs of a circle. Therefore the coordinat ion diagram appears as the union of 16 el­ ementary coordination diagrams (Figure 2(c)). Then, for each elementary coordination diagram, we compute a bounding box representation of the obstacles. Fig­ ure 2(d) shows the bounding box representation of the diagram in Figure 2(b) .

2 The obstacles in Figure 2(b) have been computed with a brute force discretization approach used only for display purpose.

3 In our context the coordinate of a configuration on a path 1 is its curvilinear abscissa s on f.

(a2 , b2 ). The computation of the boxes is then done by computing the coordination configurations (see above) .

Search Such a representation induces a cell decompo­ sition of the coordination diagram into rectangles. Any classical search algorithm may be used to compute a collision-free path from the origin (0,0) to the goal (1,1). Figure 2(d) shows a solution path. For this example, note that the widthest robot R2 (corresponding to the vertical coordinate in the diagram) should necessarily move forward , backward and then forward along the first two pieces of its path.

Completeness The algorithm is complete iff it is com­ plete when applied to the elementary diagrams corre­ sponding respectively to three cases: 5\\5, 5\\A, A\\A.

For the first two cases the algorithm is complete. The only way for the bounding box approach to loose a so­ lution is that there exist two vertical and horizontal lines intersecting two obstacles (Figure 3) . This is how­ ever not possible since at least one robot moves along a straight line segment: indeed, the robot moving along the straight line cannot intersect twice the other (convex)

robot remaining at a fixed position. Then the bounding box approximation does not affect the completeness of the algorithm for these first two cases.

Completeness is not necessarily guaranteed in the third case A\\A: we may find counterexamples where the bounding box approximation of the obstacles may split the free space into two connected components. Figure 4 shows an example where the bounding box transforms the full space into an obstacle. However such cases can be solved by the following resolution complete procedure: both arcs of a circle are recursively split into smaller arcs and each pair of the new elementary pieces is pro­ cessed with the bounding box approach. Moreover such cases are easily identified in the path decomposition step above. This means that, according to the inputs, the al­ gorithm may or not activate the recursive subdivision. The activation condition is a function dedicated to the case A\\A and checking the existence of a collision-free vertical or horizontal line in the diagram. The activa­ tion cases are seldom seen. For instance they do not appear in the examples displayed in Figures 5, 7 and 8.

4 Coordination for n robots

Generalized coordination diagram Let us now consider n robot paths li· The cartesian product of all the n(n 2 - 1 ) elementary (1;,/j) coordination diagrams is a n-dimensional cube called generalized coordination di­ agram. A point in the n-cube belongs to an obstacle iff at least two robots collide. Therefore, the obstacles in the generalized coordination diagram have a cylindrical shape 4 . As a consequence the topology of the general­ ized coordination diagram is fully characterized by the topology of the elementary 2-dimensional diagrams. Fig­ ure 5(b) shows the 10 elementary diagrams for the path coordination problem of Figure 5(a).

A solution to the coordination problem is a collision­ free path between (0, . .. 0) to (1, ... 1).

4 This property has been already noticed in [La Valle and Hutchinson, 1996]

. ,

.,

Generalized coordination diagram modeling and searching We have seen that the bounding box rep­ resentation of the coordination diagram for two robots induces a decomposition of the diagram into rectangles. Let us consider three paths /I , 12 and 13 . The cell de­ composition of b1, 12) coordination diagram induces a partition of the axis s2. Then the cell decomposition of the (r2 , 13 ) diagram is refined according to this partition. More generally, the cell decomposition of a ( ri, 'Yi) dia­ gram induces a refinement of the cell decompositions of the 2(n-l) diagrams (r;,/k) and bk>rj) (see Figure 6). We denote by (i,j)-cell a cell of the (r;,/j) diagram af­ ter refinement. The 2-dimensional ( i, j)-cells of all the ( ri , rj) diagrams induce a cell decomposition of the n­ cube. The cells of the n-cube are denoted by n-cells. The main advantage of the following search is that it does not require an explicit representation of the n-cube.

Let us consider a (collision-free) n-cell reached at a current step of the search. The strategy consists in mov­ ing only one robot at once at each step. To do that the algorithm generates the 2n cells adjacent to the n­ cell through a (n- !)-dimensional hyper-plane. Let us consider a n-cell cell, adjacent to the current collision­ free n-cell and corresponding to an elementary motion of robot i. Due to the cylindrical shape of the obstacles, testing if cell is collision-free is easily performed: each of the ( n - 1) projections of cell onto the elementary (ri, 1.) diagrams should be a collision-free (i, .)-cell.

The search is performed by an A · algorithm whose heuristic function is the shortest Euclidean path to the goal point (1, ... 1) of the n-cube. Our algorithm computes coordination paths which are Manhattan paths: only one robot moves at once. If needed, we may over­ come this fact by "smoothing" the computed path with the help of optimization techniques as in [Svestka and Overmars, 1995] .

Completeness Due to the cylindrical shape of the ob­ stacles in the generalized coordination diagram, the al­ gorithm above inherits from the completeness property of the coordination procedure for two robots presented in Section 3.

Interaction graph The final extension we propose is supported by a practical assumption. When a high number of robots plan their paths independently the path coordination problems are in general localized in differ­ ent domains of the environment and only concern robot subsets. To reduce the combinatorial complexity of the global problem in practice we first identify which robot traces intersect another trace. We then build an interac­ tion graph whose nodes are the robots; two robot-nodes are adjacent iff both corresponding traces intersect. A simple decomposition of the graph into connected com­ ponents identifies automatically the various subgroups of robots requiring motion coordination. Then the algo­ rithm above is applied to each subgroup.

Results Figure 7(a) shows an example of 32 mobile robots paths (including the traces). The 8 connected components of the interaction graph have been com­ puted automatically. The global coordination diagram appears in Figure 7(b) showing clearly the structure in­ duced by the 8 connected components. A detailed view of the coordination diagram involving a subgroup of 5 robots appears; it includes a display of the computed solution path for this group.

All the steps of the algorithm have been implemented in C++ and run on Spare Ultra-1. The following table presents the computation times of each step of the algo­ rithm for the examples in the figure 7 and the figure 8 that involves 150 robots 5 . A more complete analysis ap­ pears in [Leroy, 1998].

5 Conclusion

The proposed approach permits to solve problems for more than 100 robots in a reasonable time. The key points of the method are the efficiency of computation of the coordination configurations and the bounding box representation of the obstacles in the elementary coordi­ nation diagrams.

Nevertheless we should notice that the performance depends on the decomposition of the interaction graph into connected components. The worst case appears when the interaction graph has only one component (e.g., when the trace of some robot intersects all the other traces) . In fact, the complexity of the approach is dominated by the highest dimension of the considered n­ cubes. In practice the algorithm may explore efficiently n-cubes of dimension up to ten (i.e., involving 10 robots).

5 The motion planner computing an admissible collision­ free path for each robot is based on the algorithm presented in [Laumond et al., 1994] . It is not possible to display the "effective" motions on pictures; animations related to this work may be seen at http:/ /www . laas .fr/~sleroy .

We just argue that this limitation is not critical in prac­ tice. Moreover we do not know any alternative approach allowing to solve the case of Figure 8.