# Multiple Path Coordination for Mobile Robots A Geometric Algorithm

Multiple path coordination for mobile robots: a geometric algorithm

S. Leroy, J.P. Laumond, T. Simeon

LAAS-CNRS 7, avenue du Colonel Roche 31077 Toulouse Cedex 4 - France fsleroy,jpl,nicg@laas.fr

This paper presents a geometric based approach for multiple mobile robot motion coordination. All the robot paths being computed independently, we address the problem of coordinating the motion of the robots along their own path in such a way they do not collide each other. The proposed algorithm is based on a bounding box representation of the obstacles in the so-called coordination diagram. The algorithm is resolution-complete. Its e ciency is illustrated by examples involving more than 100 robots.

Abstract

been introduced in Kant and Zucker, 1986]: the method rst plans the paths of the robots independently and then computes the velocity pro les 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]. The path coordination problem as such has been addressed in O'Donnell and Lozano-Perez, 1989] where the notion of coordination diagram has been rst introduced. It dealt with two robots, a case which has been also addressed 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 address problems involving more than two robots.

1 Introduction: Path coordination

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

Objective, approach and contribution We want

robot path coordination and path planning are two related issues in robot motion planning. In multiple robot path planning the robot paths are not a priori computed. A solution to the multiple robot path planning problem is a collision-free path in the cartesian product of the con guration spaces of all the robots. A solution to the problem exists i the start and goal con gurations belong to a same connected component of the global collision-free con guration space. Searching such a space is a highly combinatorial problem Hopcroft et al., 1984]. To face this complexity several authors have investigated decoupled schemes1 . The decoupled approach has

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-

Path coordination versus Path planning Multiple

to solve problems involving more than 100 robots in realistic situations. The algorithm consists in searching a n-dimensional coordination diagram. The main contribution 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 e ciency inherits from the e ciency 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).

Paths SA The geometric tools we use are based on

2 Paths SA and geometric tools

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 supported 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 methods): 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 admissible path exists i 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 e cient when dealing with arcs of circle and straight line segments.

R 1

R2

s2

a1 b1

(a)

(b)

s1

b 2 a2

(a)

(b)

(c)

(d)

Figure 1: Two intersecting robot traces

Traces A mobile robot path being given, a trace is the

Figure 2: Two SA paths (a), the coordination diagram (b), the partition of the diagram induced by the path decomposition (c), the bounding box representation of the obstacles and a solution path (d). vary from 0 to 1. Figure 2(b) shows the corresponding coordination diagram (s1 ; s2 ): the black domains represent the set of con guration pairs (s1 ; s2) such that the robots collide when they are respectively at con gurations 1 (s1 ) and 2 (s2 ). Black domains are obstacles to avoid. A coordinated motion exists i 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).

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 e ciently (Figure 1(a)). tions of two robots along their own path, it is necessary to compute the intersection of their trace. Figure 1 shows two traces. The bold sub-path a1; b1] (resp. a2; b2]) gathers the con gurations at which the rst (resp. second) robot intersects the trace of the second (resp. rst) one. The endpoints of such sub-paths are called coordination con gurations. 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.

Coordination con gurations To coordinate the mo-

A bounding box representation Our contribution

in a coordination diagram is a rectangle whose endpoint coordinates are the coordination con gurations 3. Let

is to propose an algorithm to explore the diagram without computing the exact shape of the obstacles2 . We use a bounding box representation based on the following property: the (minimal) box bounding an obstacle us consider the case in Figure 1. The coordinates of four points de ning the rectangle in the coordination diagram are respectively (a1 ; b1), (a1 ; b2), (a2 ; b1) and

2

3 Coordination for two robots

Coordination diagram Coordinating the motion of two robots along two given paths is a classical problem. Its solution consists in exploring the so-called coordination diagram O'Donnell and Lozano-Perez, 1989]. Let us consider the two paths 1 (s1 ) and 2 (s2 ) in Figure 2(a). Both coordinates s1 and s2 are assumed to

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 con guration on a path is its curvilinear abscissa s on .

(a2 ; b2). The computation of the boxes is then done by computing the coordination con gurations (see above). box representation directly in the coordination diagram of 1 and 2 , we rst 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 ) and ( 2 ) the pieces sequences of 1 and 2 respectively. The coordination diagram for 1 and 2 then appear as the union of the coordination diagrams of the various pairs ( 1 ; 2 ). For instance, the two paths in Figure 2(a) both consist of 4 arcs of a circle. Therefore the coordination diagram appears as the union of 16 elementary coordination diagrams (Figure 2(c)). Then, for each elementary coordination diagram, we compute a bounding box representation of the obstacles. Figure 2(d) shows the bounding box representation of the diagram in Figure 2(b).

;i ;j ;i ;j

Path decomposition Let us now consider two SA paths and . Instead of applying the bounding

1 2

robot remaining at a xed position. Then the bounding box approximation does not a ect the completeness of the algorithm for these rst two cases.

Figure 4: AjjA special case: bounding boxes would ll the space. Completeness is not necessarily guaranteed in the third case AjjA: we may nd 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 processed with the bounding box approach. Moreover such cases are easily identi ed in the path decomposition step above. This means that, according to the inputs, the algorithm may or not activate the recursive subdivision. The activation condition is a function dedicated to the case AjjA and checking the existence of a collision-free vertical or horizontal line in the diagram. The activation cases are seldom seen. For instance they do not appear in the examples displayed in Figures 5, 7 and 8.

Search Such a representation induces a cell decomposition 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 rst two pieces of its path.

Figure 3: This case cannot appear when at least one robot moves along a straight line segment.

Completeness The algorithm is complete i it is complete when applied to the elementary diagrams corresponding respectively to three cases: SjjS, SjjA, AjjA.

For the rst two cases the algorithm is complete. The only way for the bounding box approach to loose a solution is that there exist two vertical and horizontal lines intersecting two obstacles (Figure 3). This is however 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)

Generalized coordination diagram Let us now consider n robot paths . The cartesian product of all the ( 2?1) elementary ( ; ) coordination diagrams is a n-dimensional cube called generalized coordination diagram. A point in the n-cube belongs to an obstacle i at least two robots collide. Therefore, the obstacles in the generalized coordination diagram have a cylindrical shape4 . As a consequence the topology of the generalized coordination diagram is fully characterized by the topology of the elementary 2-dimensional diagrams. Figure 5(b) shows the 10 elementary diagrams for the path coordination problem of Figure 5(a). A solution to the coordination problem is a collisionfree path between (0; : : : 0) to (1; : : : 1).

i n n i j

4 Coordination for n robots

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

4

putes coordination paths which are Manhattan paths: only one robot moves at once. If needed, we may overcome this fact by \smoothing" the computed path with the help of optimization techniques as in Svestka and Overmars, 1995]. (a) (b)

Figure 5: The 10 elementary diagrams (b) of the generalized coordination diagram of 5 paths (a).

s 3

s3

s

1

s

1

(a)

s 2

s 2

Figure 6: The cell decomposition of a diagram re nes the cell decomposition of other diagrams.

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 1 , 2 and 3 . The cell decomposition of ( 1 ; 2 ) coordination diagram induces a partition of the axis s2 . Then the cell decomposition of the ( 2 ; 3) diagram is re ned according to this partition. More generally, the cell decomposition of a ( ; ) diagram induces a re nement of the cell decompositions of the 2(n ? 1) diagrams ( ; ) and ( ; ) (see Figure 6). We denote by (i; j )-cell a cell of the ( ; ) diagram after re nement. The 2-dimensional (i; j )-cells of all the ( ; ) diagrams induce a cell decomposition of the ncube. 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 moving only one robot at once at each step. To do that the algorithm generates the 2n cells adjacent to the ncell through a (n ? 1)-dimensional hyper-plane. Let us consider a n-cell cell, adjacent to the current collisionfree 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 ( ; ) 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 comi j i k k j i j i j i :

(b) Figure 7: A case with 32 robots: the robots traces (a) and the 496 elementary diagrams. The partition into the 8 robot subgroups is illustrated by the 8 bold triangles. stacles in the generalized coordination diagram, the algorithm above inherits from the completeness property of the coordination procedure for two robots presented in Section 3.

Completeness Due to the cylindrical shape of the ob-

Interaction graph The nal extension we propose is

supported by a practical assumption. When a high num-

ber of robots plan their paths independently the path coordination problems are in general localized in di erent domains of the environment and only concern robot subsets. To reduce the combinatorial complexity of the global problem in practice we rst identify which robot traces intersect another trace. We then build an interaction graph whose nodes are the robots; two robot-nodes are adjacent i both corresponding traces intersect. A simple decomposition of the graph into connected components identi es automatically the various subgroups of robots requiring motion coordination. Then the algorithm above is applied to each subgroup.

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

References

Alami et al., 1995] R. Alami, F. Robert, F. F. Ingrand, and S. Suzuki. Multi-robot cooperation through incremental plan-merging. In IEEE International Conference on Robotics and Automation, Nagoya, Aichi (Japan), pages 2573{2578, 1995.

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 computed automatically. The global coordination diagram appears in Figure 7(b) showing clearly the structure induced 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 Sparc Ultra-1. The following table presents the computation times of each step of the algorithm for the examples 5in the gure 7 and the gure 8 that involves 150 robots . A more complete analysis appears in Leroy, 1998]. 32 rob. 150 rob. Interaction graph computation and bounding box representation 30s 240s of the diagrams Diagram re nement 6s 13s Search 3:7s 1:5s

Bien and Lee, 1992] Z. Bien and J. Lee. A minimumtime trajectory planning method for two robots. IEEE Transactions on Robotics and Automation, 8(3):414{ 418, June 1992. Buckley, 1989] S.J. Buckley. Fast motion planning for multiple moving robots. In IEEE International Kahng. Cooperative mobile robotics: Antecedents and directions. In Autonomous robots, volume 4, pages 7{ 27, 1997. Chang et al., 1994] C. Chang, M.J. Chung, and B.H. Lee. Collision avoidance of two robot manipulators by minimum delay time. IEEE Transaction on Systems, Man and Cybernetics, 24(3):517{522, 1994. Erdmann and Lozano-Perez, 1986] M. Erdmann and T. Lozano-Perez. On multiple moving objects. In

Conference on Robotics and Automation, Scottsdale (USA), pages 322{326, May 1989. Cao et al., 1997] Y. Cao, A.S. Fukunaga, and A.B.

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 e ciency of computation of the coordination con gurations and the bounding box representation of the obstacles in the elementary coordination 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 ncubes. In practice the algorithm may explore e ciently n-cubes of dimension up to ten (i.e., involving 10 robots).

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

1986. Hopcroft et al., 1984] J. E. Hopcroft, J. T. Schwartz, and M. Sharir. On the complexity of motion planning for multiple independant objects: Pspace-hardness of the warehouseman's problem. The International Journal of Robotics Research, 3:76{88, 1984. Kant and Zucker, 1986] K. Kant and S. W. Zucker. Toward e cient trajectory planning: The path-velocity decomposition. The International Journal of Robotics Research, 5(3):72{89, 1986. La Valle and Hutchinson, 1996] S. La Valle and S. Hutchinson. Optimal motion planning for multiple robots having independants goals. In

IEEE International Conference on Robotics and Automation, Minneapolis (USA), pages 1847{1852, May

IEEE International Conference on Robotics and Automation, San Francisco (USA), pages 1419{1424,

1996. Latombe, 1991a] J.C. Latombe. A fast path planner for a car-like indoor mobile robot. In 9th Nat. Conf. on Arti cial Intelligence, AAAI, Anaheim, CA, pages 659{665, July 1991. Latombe, 1991b] J.C. Latombe. Robot Motion Planning. Kluwer Academic Publishers, 1991. Laumond et al., 1994] J.P. Laumond, P. Jacobs, M. Ta x, and R. Murray. A motion planner

Figure 8: A case with 150 robots for non holonomic mobile robots. IEEE Transactions on Robotics and Automation, 10(5):577{593, 1994. Laumond, 1986] J.P Laumond. Feasible trajectories for mobile robots with kinematic and environment constraints. In International Conference on Intelligent Autonomous Systems, Amsterdam, 1986. Leroy, 1998] S. Leroy. Outils geometriques pour la plani cation de chemins de robots mobiles non holonomes. PhD thesis, Universite Paul Sabatier, Toulouse (France), Decembre 1998. Mirtich and Canny, 1992] B. Mirtich and J. Canny. Using skeletons for nonholonomic motion planning among obstacles. In IEEE International Conference on Robotics and Automation, Nice (France), pages 2533{2540, 1992. O'Donnell and Lozano-Perez, 1989] P. O'Donnell and T. Lozano-Perez. Deadlock-free and collision-free coordination of two robot manipulators. In IEEE International Conference on Robotics and Automation, Scottsdale (USA), pages 484{489, 1989.

Reif and Wang, 1995] J. Reif and H. Wang. Social potential elds : a distributed behavioral control for autonomous robots. In K. Goldberg et al, editor, Algorithmic Foundations of Robotics, pages 331{345. A. K. Peters, 1995. Simeon et al., 1998] T. Simeon, S. Leroy, and J.P. Laumond. A collision checker for car-like robots coordination. In IEEE International Conference on Robotics and Automation, Leuven (Belgium), volume 1, pages 46{51, 1998. Svestka and Overmars, 1995] P. Svestka and M. Overmars. Coordinated motion planning for multiple carlike robots using probabilistic roadmaps. In IEEE InWarren, 1990] C.W. Warren. Multiple robot path coordination using arti cial potential elds. In IEEE In-

ternational Conference on Robotics and Automation, Nagoya, Aichi (Japan), pages 1631{1636, 1995.

ternational Conference on Robotics and Automation, Cincinnati (USA), volume 1, pages 500{505, 1990.