Path Planning for Permutation-Invariant Multi-Robot Formations

Proceedings of the 2005 IEEE International Conference on Robotics and Automation Barcelona, Spain, April 2005 Path Planning for Permutation-Invariant Multi-Robot Formations Stephen Kloder Seth Hutchinson The Beckman Institute for Advanced Science and Technology University of Illinois at Urbana-Champaign Urbana, IL 61801 Email: {kloder, seth}@uiuc.edu Abstract In this paper we demonstrate path planning for our formation space that represents permutation-invariant multirobot formations. Earlier methods generally pre-assign roles for each individual robot, rely on local planning and behaviors to build emergent behaviors, or give robots implicit constraints to meet. Our method first directly plans the formation as a set, and only afterwards determines which robot takes which role. To build our representation of this formation space, we make use of a property of complex polynomials: they are unchanged by permutations of their roots. Thus we build a characteristic polynomial whose roots are the robot locations, and use its coefficients as a representation of the formation. Mappings between work spaces and formation spaces amount to building and solving polynomials. In this paper, we construct an efficient obstacle collision detector, and use it in a local planner. From this we construct a basic roadmap planner. We thus demonstrate that our polynomialbased representation can be used for effective permutationinvariant formation planning. I. INTRODUCTION Multi-robot planning is a useful and developing subfield of robot planning. Multiple coordinated robots have been used successfully in a variety of applications including surveillance and monitoring [1], localization and exploration [2], satellite arrangement [3], search and rescue [4], and object manipulation and transportation [5], [6], [7]. Multi-robot motion planning approaches are distinguished by their levels of centralization. In fully centralized methods (e.g. [8], [9], [10]), a single planner plans all robots motions simultaneously. This often makes use of a joint configuration space that represents the configurations of all robots [11]. Slightly less centralized is the decoupled approach [12], [13], [14], where each robot plans its own path, but a single centralized planner coordinates the robots by dictating how each robot follows its path. Significantly less centralized are emergent behaviors methods [15], [7], where each robot follows local behaviors that depend on relative locations of nearby robots. In emergent behavior methods, a centralized planner determines the general motion of the robots, and the structure that they maintain. Fully decentralized methods are usually coverage methods [16], [17], [18], [19], with goals of maximizing the total region visible by robots sensors, or of minimizing the probability of passing undetected through a region. In coverage methods, all robots plan independently, but consider each others locations when moving. In more centralized methods, robot roles and relationships are usually predefined. The methods that do not predefine roles and relationships are found on the decentralized end of the spectrum, e.g. blanket coverage methods, and a few emergent behavior methods like [20]. However, these methods do not control their formations directly, but set constraints and move until they are met. This usually limits feasible goals to symmetric formations. Our new method plans formations in a direct fashion like the more centralized methods, while still being flexible enough to allow changing roles and relationships like less centralized methods. We introduced our representation for permutation-invariant formations and a simple planner in [21] for planar robots. Our method uses complex polynomials to represent multi-robot formations. We demonstrated basic planning in obstacle-free environments, and showed some of its properties. In this paper we demonstrate collision detection, and show how it can be used to implement roadmap planning in the formation space. This paper is laid out as follows: Section II describes the representation and basic planning, Section III describes collision detection, Section IV describes roadmap results, and Section V describes proposed future work. II. REPRESENTATION AND BASIC PLANNING A. A Permutation-invariant representation of formations The configuration space of a labeled multirobot formation is generally an ordered list of configurations, one for each robot. However, we are building a configuration space for unlabeled formations, where exchanging two robots does not change the configuration. We call this a formation space, and write FS[X] n = X n /S n,wheren is the number of robots, X is the configuration space of one robot, and S n is the symmetric group of permutations of n elements. In this quotient space, (x 1,x 2,...,x n ) and (x 1,x 2,...,x n ) are identified iff they are permutations of each other. We shall sometimes refer to FS[X] n as FS n for short if X is known. In this paper we focus on translating planar robots, whose configurations can be represented as (x, y) R 2.Werepresent the configuration of each robot as z = x + yi C, so we can construct FS[C] n with complex polynomials. Therefore the robots workspace W, the space in which the individual robots move, is represented by the complex plane C. Forn robots at locations z 1,...,z n, we define the polynomial 0-7803-8914-X/05/$20.00 2005 IEEE. 1797

P (λ) = (λ z 1 )(λ z 2 )...(λ z n ) = λ n + a 1 λ n 1 + + a n 1 λ + a n (1) The complex coefficients a 1,...,a n can be used to define a permutation invariant formation, or simply a formation. For any configuration Z = {z 1,...,z n } of the multirobot system, (1) defines a unique formation a =(a 1,...,a n ). Throughout this paper, we will refer to sets of robot configurations, like Z, as multirobot configurations, while the n-entry vectors like a that specify these multirobot configurations will be referred to as formations. Equation (1) defines the mapping f : C n /S n C n to be Z a. Since the roots of a polynomial vary continuously as a function of the coefficients, f 1,whichisa Z, is a continuous mapping from a formation to a multirobot configuration. Since f is a bijection (Fundamental Theorem of Algebra), and both f and f 1 are continuous (see [22]), f is a homeomorphism, so FS[C] n = C n. We thus define our formation space as FS[C] n = C n, the set of all possible n-tuples of complex polynomial coefficients. In this paper, we will use a and b to represent formations; each formation is a vector of complex coefficients of a complex polynomial P (λ), asin(1). B. Straight-Line Planning For local planning, we use a straight-line planner in the formation space. To plan from a = (a 1,...,a n ) to b = (b 1,...,b n ), follow the path l (t) in the formation space, where l (t) =(1 t)(a 1,...,a n )+t (b 1,...,b n ) (2) in which t [0, 1] parameterizes the path. Therefore, at time t, the robots will be at the roots of the polynomial λ n + n [(1 t) a k + tb k ] λ n k =0. (3) k=1 In [21], we showed that if both the initial and goal configurations translate, scale, rotate, and reflect by the same amount, the resulting path will be transformed accordingly. This means that the path, relative to its endpoints, is independent of scale, orientation, origin location, or handedness. For example, Figure 1 illustrates two straight-line paths in the formation space The figures show the workspace paths of the robots. The paths on the right were generated by taking the start and goal points from the figure on the left; translating, scaling, rotating, and reflecting the endpoints; and moving from the new start to the new goal using straight-line planning. The resulting paths are the same paths as if the original paths were also translated, scaled, rotated, and reflected the same way as the endpoints. III. OBSTACLES For permutation-invariant motion planning to be useful, it must be able to test for collisions with obstacles. To do this, we will define the swept volume of a path V (a, b) to be the locus of points in the workspace traversed by the robots while following the formation space path l (t) from equation (2). Formally: V (a, b) = { z C z f 1 ((1 t) a + tb),t [0, 1] }, i.e. V (a, b) is the set of all roots of l (t) for all values of t [0, 1]. Given polynomial configurations a and b, andan obstacle region O W, the straight-line path in the formation space from formation a to formation b generates a collision iff V (a, b) O. In this section, we will first show how to check collisions for point obstacles. We will then extend this to line segment obstacles. Once we can check for collisions with line segments, we can check for collisions with any polygonal obstacle. A. Point Obstacles Consider a point obstacle with coordinates z c.foragiven formation a, a collision occurs if z c is a root of (1), i.e. if some robot is coincident with the point obstacle. This can be determined simply by evaluating P (z c ). There is a collision iff P (z c )=0.Ifwedefinethe ˆ operator such that: ẑ = ( z n 1,z n 2,...,z,1 ), (4) we can write P (z c )=0concisely as a ẑ c = z n c, (5) Since (5) is linear in the formation coefficients, the obstacle point (or more particularly, the locus of formations that correspond to at least one robot at z c ) corresponds to a 2n 2 dimension hyperplane in the formation space. Testing a single formation for collision is straightforward; testing for a path in the formation space requires a little more work. To check V (a, b) for collision with z c,wemust essentially evaluate (5) at all formations along the path. Theorem 1: For any a, b C n, define function τ (z c )= zn c + a ẑ c. (6) (a b) ẑ c V (a, b) collides with a point obstacle at z c C iff τ (z c ) R and τ (z c ) [0, 1]. Proof: Using (2) to give formations as a function of t, we write (5) as l (t) ẑ c = zc n (7) (1 t)a ẑ c + tb ẑ c = zc n. (8) If (8) is satisfied for any t [0, 1], then at least one robot collides with the obstacle at z c. Solving for t we obtain t = zn c + a ẑ c (a b) ẑ c = τ (z c ). (9) In order for there to be a collision with z c,thet given by (9) must be real with 0 t 1. 1798

Fig. 1. Before and after: translation, scaling, rotation, and reflection B. Horizontal Segment Obstacles We showed in [21] that if the start and goal are translated and rotated, the paths transform accordingly. Since any edge can be rotated and translated onto the real axis, we can rotate and translate a and b thesameway.ifv (a, b) collides with an edge, then the transformed path will collide with a subset of the real axis. This means we can test collision with any line segment if we can test for collision with the real axis. Theorem 2: For any p, q, r, s R n, V (p + qi, r + si) collides with the real axis iff there exists an x R such that x is a root of the polynomial Ω(x) = 2n ω m x 2n m,where m=1 ω m is given in Figure 2, and τ (x) [0, 1]. Proof: Set a = p + qi and b = r + si. Weusethe results from the previous section to test for collision with the real axis. With point obstacles, z c was fixed, so we tested z c in (9) and determined whether t was real and in [0, 1]. Here, however, z c can be any point on the edge, so we search for a z c that makes t real and in [0, 1]. Substituting x R for z c in (9), we obtain t = τ (x) = xn + a ˆx (a b) ˆx. (10) If there is some x such that τ (x) is real and between 0 and 1, then V (a, b) intersects the real axis at x. To make this determination, first find the set of values for x such that τ (x) is real. Since (10) is a ratio of two complex numbers, we make the denominator real, and focus on the numerator. With z defined as the complex conjugate of z, we transform (10) as ( x n ) ( ( ) ) + a ˆx a b ˆx τ (x) = ( ) (11) (a b) ˆx a b ˆx = (xn + a ˆx) [( a b ) ˆx ] (a b) ˆx 2 (12) ( ) x n + n n ( ) a k x n k aj b j x n j k=1 = (a b) ˆx 2 (13) = n ( ) aj b j x 2n j + (a b) ˆx 2 n n ( ) a k aj b j x 2n j k k=1 (a b) ˆx 2. (14) Since the denominator is real, τ (x) is real iff the imaginary component of the numerator is 0. We rearrange terms on the right half of (14) by substituting m = j + k and removing k: n n ( ) a k aj b j x 2n j k k=1 x 2n m n m=2 = 2n a m j ( aj b j ). (15) However, the terms on the RHS are only valid if 1 m j n, i.e.m n j m 1, while still satisfying 1 j n. Therefore, we write (15) as n n ( ) a k aj b j x 2n j k k=1 x 2n m min(n,m 1) m=2 j=max(1,m n) = 2n a m j ( aj b j ). Using this, we can rewrite the numerator of (14) as a polynomial w m x 2n m 2n,where m=1 a m b m m =1 a m b m + m 1 ( ) a m j aj b j 2 m n w m = n ( ) a m j aj b j n +1 m, j=m n m 2n (16) To determine I (w m ), the imaginary component of w m,we use the fact that a = p + qi, b = r + si, andp, q, r, s R n. Therefore a j b j = (p j r j ) i (s j q j ) (17) ( ) a j ak b k = (p j + iq j )((p k r k ) i (s k q k )) = (p j (p k r k )+q j (q k s k )) +i (p j (s k q k )+q j (p k r k )) (18) 1799

ω m = s m q m m =1 s m q m + m 1 p m j (s j q j )+q m j (p j r j ) 2 m n n j=m n p m j (s j q j )+q m j (p j r j ) n +1 m 2n Fig. 2. Formulae for coefficients of Ω(x) Any line segment can be rotated, scaled, and translated to the real axis via T u and SR v u : u = SR v u (T u (u)) = 0 (21) v = SR v u (T u (v)) = v u 2 (22) Fig. 3. Collision with real axis We substitute (17) and (18) into (16) to construct ω m = I (w m ), producing the coefficients in Figure 2. The real zeroes of the degree 2n 1 real polynomial Ω(x) = 2n ω m x 2n m are the values for x such that m=1 τ (x) R. If0 < τ (x j ) < 1, thenv (a, b) intersects the real axis at x j. This method was applied to collisions with the real axis. But it can also be used to test for collisions with a segment of the real axis, e.g. a line segment on the real axis from x min to x max (where x min <x max ). This segment can be checked for collision by rejecting any x j / [x min,x max ] without calculating τ (x j ). Fig. 3 shows an example of collisions with the real axis. The robots are moving from the X s towards the O s, and the diamonds mark the points of collision with the real axis, as returned by the method described here. The robot paths are given up to the earliest collision, i.e. the x j among the roots of Ω(x) with minimum τ (x j ). C. Arbitrary Segment Obstacles Now that we can test for collision with segments on the real axis, we can test for collisions with any segment from u C to v C. To do so we make use of the transformations in [21]. Translation: In the complex plane, translation is the same as addition. Apply this addition to all items uniformly. T c ({z 1,...,z n })={z 1 + c, z 2 + c,...,z n + c} (19) Scale & Rotation: In the complex plane, scaling and rotation are actually the same operation: multiplication. Every c C can be written as c = m d, wherem is an integer and d has magnitude 1 (set m = c, andd = c/m). Multiplying by m is a scale operation, and multiplying by d is a rotation. Therefore multiplying by c does both. SR c ({z 1,...,z n })={z 1 c, z 2 c,...,z n c} (20) In [21], we also defined lifted operators Tc and SR c, which show how the transformational operators affect the polynomials. Tc = ft c f 1, which means that for any Z C and any c C, ifa = f (Z), Z = T c (Z), anda = f (Z ), then a = T c (a). Similarly, SRc = fsr c f 1. T c and SR c can be calculated explicitly, without f or f 1 : where and a k = T c (a 1,...,a n )=(a 1,...,a n ), (23) ( ) n ( c) k + k k ( ) n j a j ( c) k j, (24) k j SR c (a 1,a 2,...,a n )= ( a 1 c, a 2 c 2,...,a n c n). (25) See [21] for derivations of these expressions. Now, we can take the transformational operators we applied to u and v, and apply the corresponding lifted transformation to the polynomials: a = SR ( ) v u T u (a) (26) b = SR ( ) v u T u (b). (27) If V (a, b) collide with uv, thenv (a, b ) will also collide with the real axis between u and v. We can therefore apply the method in III-B to a, b, u,andv to determine if the original path collides with uv. Fig. 4 shows an example of collisions with arbitrary segments. The robots and edge in this example can be rotated and scaled to match Fig. 3 exactly, and the resulting collision information transforms accordingly. Therefore we can use the collision information from Fig. 3 to find the collisions in Fig. 4. D. General Obstacle Collisions We can use the edge collision checker to check collision with any obstacle region. A polygonal region can be checked by checking for collision with each edge, as a robot cannot enter the polygon without crossing an edge. Any non-polygonal region can be approximated by polygons. Therefore we can check for collision with any obstacle region. 1800

Fig. 4. Collision with segment IV. ROADMAP PLANNING The straight-line planner, combined with the obstacle collision checker, form an effective local planner. We use it to apply simple Probabilistic Roadmap (PRM) [23] methods to permutation-invariant formations. To generate each node in the roadmap, we generated n random samples z 1,...,z n from the collision-free workspace, and constructed a node at a = f ({z 1,...,z n }). Configuration nodes a and b were connected iff V (a, b) did not collide with any edges. To determine routes, we used A* with a metric of n d (a, b) = a j b j n j. (28) The exponents applied to the distances are to offset the tendencies of the later exponents to grow much faster than the earlier ones. Compare to (25) above. Figures 6 and 9 show typical results. In both cases, the border is treated as an obstacle; robots are forbidden from leaving the visible region. Figure 6 uses 50 nodes for 4 robots, while figure 9 uses 200 nodes for 8 robots. Both show PRM planning is feasible for FS n with obstacles. We can smooth the paths by iteratively averaging the points along the path, while avoiding robot-robot and robot-obstacle collisions. Applying this path smoothing to Figures 6 and 9 produces paths in Figures 7 and 10 respectively. By contrast, figures 5 and 8 show the same start and goal configurations as 6 and 9 respectively, but planned without any obstacle checking or PRM. V. CONCLUSION In this paper we demonstrated obstacle collision detection for permutation-invariant formations, and used it to implement effective roadmap planning that avoids obstacles. The collision checking is efficient as it can be executed in polynomial time and is not dependent on resolution or path deconstruction. There remains much more to explore on this subject. Robotto-robot collisions, while rare in experiments, are still possible, and remain unchecked. The sampling method is based on selecting individual robot configurations; we would like to find a good formation sampling scheme. The roadmap method used is not optimal; we do not know which metrics, connecting methods, and enhancing mechanisms are most appropriate for this representation. We would also like to look at other types of robots, other levels of centralization and uncertainty, and other types of paths. Fig. 5. Fig. 6. Fig. 7. Four robots, without obstacle checking Four robots, with obstacle checking Four robots, with path smoothing REFERENCES [1] J. Feddema and D. Schoenwald, Decentralized control of cooperative robotic vehicles, IEEE Trans. Robot. Automat., vol. 18, no. 5, pp. 852 864, Oct. 2002. [2] D. Fox, W. Burgard, H. Kruppa, and S. Thrun, A probabilistic approach to collaborative multi-robot localization, Autonomous Robots, vol. 8, no. 3, pp. 325 344, June 2000. [Online]. Available: citeseer.nj.nec.com/fox00probabilistic.html [3] W. Kang, N. Xi, and A. Sparks, Formation control of autonomous agents in 3d workspace, in Proc. IEEE ICRA 00. Int. Conf. Robotics and Automation 2000, vol. 2, San Francisco, CA, Apr. 2000, pp. 1755 1760. [4] J. S. Jennings, G. Whelan, and W. F. Evans, Cooperative search and rescue with a team of mobile robots, in Proc. IEEE ICAR Int. Conf. Advanced Robotics, 1997, July7-9 1997, pp. 193 200. [5] D. Rus, B. Donald, and J. Jennings, Moving furniture with teams 1801

Fig. 8. Fig. 9. Fig. 10. Eight robots, without obstacle checking Eight robots, with obstacle checking Eight robots, with path smoothing of autonomous robots, in Proc. IEEE IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 1995, Pittsburgh, PA, Aug. 1995, pp. 235 242. [6] M. Mataric, M. Nilsson, and K. Simsarian, Cooperative multi-robot box pushing, in Proc. IEEE IEEE/RSJ Int. Conf. Intelligent Robots and Systems, 1995, Pittsburgh, PA, Aug. 1995, pp. 556 561. [7] A.Das,R.Fierro,V.Kumar,J.Ostrowski,J.Spletzer,andC.Taylor, A vision-based formation control framework, IEEE Trans. Robot. Automat., vol. 18, no. 5, pp. 813 825, Oct. 2002. [8] J. Schwartz and M. Sharir, On the piano movers problem: III. coordinating the motion of several independent bodies: The special case of circular bodies moving amidst polygonal barriers, Robotics Research, vol. 2, no. 3, pp. 46 75, 1983. [9] P. Svestka and M. Overmars, Coordinated path planning for multiple robots, Robotics and Autonomous Systems, vol. 23, no. 4, pp. 125 152, 1998. [10] G. Sanchez and J. Latombe, Using a prm planner to compare centralized and decoupled planning for multi-robot systems, in Proc. IEEE ICRA 02. Int. Conf. Robotics and Automation 2002, vol. 2, May 2002, pp. 2112 2119. [11] J. Latombe, Robot Motion Planning. Norwell, Massachusetts: Kluwer Academic Publishers, 1991. [12] Y. Guo and L. E. Parker, A distributed and optimal motion planning approach for multiple mobile robots, in Proc. IEEE ICRA 02. Int. Conf. Robotics and Automation 2002, Washington, DC, May 2002, pp. 2612 2619. [13] T. Simeon, S. Leroy, and J.-P. Laumond, Path coordination for multiple mobile robots: a resolution complete algorithm, IEEE Transaction on Robotics and Automation, vol. 18, no. 1, pp. 42 49, Feb. 2002. [14] S. M. LaValle and S. A. Hutchinson, Optimal motion planning for multiple robots having independent goals, IEEE Trans. on Robotics and Automation, vol. 14, no. 6, pp. 912 925, Dec. 1998. [15] J. Fredslund and M. Mataric, A general algorithm for robot formations using local sensing and minimal communication, IEEE Trans. Robot. Automat., vol. 18, no. 5, pp. 837 846, Oct. 2002. [16] D. W. Gage, Command control for many-robot systems, in AUVS92, the Nineteenth Annual AUVS Technical Symposium, Hunstville Alabama, USA, June 1992, pp. 22 24. [17] Q. Du, V. Faber, and M. Gunzburger, Centroidal voronoi tessellations: Applications and algorithms, SIAM Rev., vol. 41, no. 4, pp. 637 676, 1999. [18] D. Popa, H. Stephanou, C. Helm, and A. Sanderson, Robotic deployment of sensor networks using potential fields, in Proc. IEEE ICRA 04. Int. Conf. Robotics and Automation 2004, vol.1,neworleans, LA, Apr. 2004, pp. 642 647. [19] A. Howard, M. Mataric, and G. Sukhatme, An incremental deployment algorithm for mobile robot teams, in IEEE/RSJ International Conference on Intelligent Robots and System, vol. 3, Oct. 2002, pp. 2849 2854. [20] P. Ogren, E. Fiorelli, and N. Leonard, Formations with a mission: Stable coordination of vehicle group maneuvers, in Proceedings of the 15 International Symposium on Mathematical Theory of Networks and Systems, Notre Dame, IN, Aug. 2002. [Online]. Available: citeseer.nj.nec.com/ogren02formations.html [21] S. Kloder, S. Bhattacharya, and S. Hutchinson, A configuration space for permutation-invariant multi-robot formations, in Proc. IEEE ICRA 04. Int. Conf. Robotics and Automation 2004, vol.3,neworleans,la, Apr. 2004, pp. 2746 2751. [22] M. Marden, The Geometry of the Zeros of A Polynomial in a Complex Variable. New York: American Mathematics Society, 1949. [23] L. Kavraki, P. Svestka, J. C. Latombe, and M. Overmars, Probabilistic roadmaps for path planning in high-dimensional configuration spaces, IEEE Trans. Robot. Automat., vol. 12, pp. 556 580, Aug. 1996. 1802