Controlling formations of multiple mobile robots. Jaydev P. Desai Jim Ostrowski Vijay Kumar

Transcription

1 Controlling formations of multiple mobile robots Jayev P. Desai Jim Ostrowski Vijay Kumar General Robotics an Active Sensory Perception (GRASP) Laboratory, University of Pennsylvania 340 Walnut Street, Room 30C, Philaelphia, PA Abstract In this paper we investigate feeback laws use to control multiple robots moving together in a formation. We propose a metho for controlling formations that uses only local sensor-base information, in a leaer-follower motion. We use methos of feeback linearization to exponentially stabilize the relative istance an orientation of the follower, an show that the zero ynamics of the system are also (asymptotically) stable. We emonstrate in simulation the use of these algorithms to control six robots moving aroun an obstacle. These types of control laws can be use to control arbitrarily large numbers of robots moving in very general types of formations. Keywors: Nonholonomic motion planning, Control theory an Formations of robots. Introuction This paper aresses issues of control an coorination for many robots moving in formation using ecentralize controllers. The research on control an motion planning for mobile robots is both extensive an iverse. In the area of mobile robots, optimal motion plans for a single car-like robot have been thoroughly stuie, incluing results of Rees an Shepp [], who prove that optimal paths in a free environment consist of straight lines an circular arcs. Other researchers [] have stuie the evelopment of motion plans for mobile robots in the presence of obstacles, an in particular for systems using only local, or sensor-base, planning. One avenue of past research that we have pursue is to use variational methos to obtain optimal motion plans in the presence of obstacles by consiering the ynamics of the mobile manipulators [3]. However, the search for optimal motion plans can be computationally very expensive, particularly as the number of robots or egrees of freeom of the system gets large. For this reason, we pursue in the current work easily computable feeback laws that can be use in conjunction with a higher level (but lower complexity) motion planner. This material is base upon work supporte by the U.S. Army Research Oce uner grant DAAH , the Defense Avance Research Projects Agency uner grant N J-647, an the National Science Founation uner grants BES9-669, BES9-796 an CMS The rst author is also supporte by a Fellowship (NSF grant SHR89-030) from the Institute for Research in Cognitive Science at the University of Pennsylvania. In the area of nonholonomic systems (which inclues most wheele mobile robots), a great eal of research has been one, particularly in the area of nonlinear control. For example, several researchers have stuie planning algorithms for mobile robots, with more recent eorts focuse on using time-varying [4] an non-smooth feeback laws to generate exponentially stabilizing controllers [5]. In our current stuy, we show that for a restricte class of tracking motions (line segments an circular arcs), the smooth state feeback laws evelope here also provie asymptotic convergence to a nominal steay-state trajectory. On the other han, many researchers have recently investigate controlling multiple (i.e., many) cooperative robots [6, 7]. This research has primarily focuse on generating emergent or complex behaviors from organizations of many small, iniviual, an often heterogeneous robots. Generally, these approaches either require some type of supervisory (centralize) control an process planning, or rely on more evolutionary approaches to generate a pre-specie system behavior. The goal we pursue here is ierent than these approaches in that we target a specic goal an attempt to generate rigorously provable measures/bouns on the performance of our system. We formulate the problem of control an coorination for many robots moving in formation using ecentralize controllers as one in which a motion plan for the overall formation is given, for example, using methos from optimal control or some other external source such as human operator. This motion plan is then use to control a single lea robot. We assume that each robot has the ability to measure the relative position of other robots that are immeiately ajacent to it. Once the motion for the lea robot is given, the remainer of the formation is governe by local control laws base on the relative ynamics of each of the follower robots an the relative positions of the robots in the formation. These control laws have the avantage of proviing easily computable, real-time feeback control, with provable performance for the entire system. In this paper we escribe two scenarios for feeback control within a formation. In the rst scenario, one robot follows another by controlling the relative istance an orientation between the two. This situation is applicable to all formations in which each robot has one leaer except for the lea robot. Thus it can be use for robots marching in a single le. In the secon scenario, a robot maintains its position in the formation by maintaining a specie istance

2 from two robots, or from one robot an an obstacle in the environment. This behavior is useful for robots that are constraine by more than one robot (or obstacle) in the formation. This is true, for example, for robots that are internal to (or insie) a formation of robots marching in a rectangular formation with a rows an b columns, or for a robot in a single le formation marching very near an obstacle where the robot must maintain a specie separation from the leaer as well as a specie separation from the obstacle. We emonstrate the use of these techniques in several simulations, incluing motions in formation with up to six robots an obstacles. Extensions to many more robots is quite straightforwar, an not computationally burensome. In Section, we iscuss two types of feeback control laws for maintaining the formation. We then present several numerical simulations to illustrate the proceure in Section 3 an nally make some concluing remarks in Section 4. Control of formations We assume that the motion of the lea robot is planne, an focus our attention on controlling the internal geometries of the formation. For our purposes, we use tools from optimal control theory evelope in [3] to specify the motion of the lea robot which can be compute by minimizing a suitable cost function. Thus the trajectory an velocity compute by the metho of [3] is transmitte to the rst generation of robots in the chain which act as leaers for the other follower robots in the formation.. Feeback control In this section we evelop two types of feeback controllers for maintaining formations of multiple mobile robots. (x, y ) θ ROBOT l ψ (x, y ) ROBOT Figure : Notation for l? control l? control: In Fig., we show a system of two nonholonomic mobile robots separate by a istance of l between the center of the rst robot an the front castor of the secon robot. The istance between the castor an the center of the axis of the wheels of each robot is enote by. Each robot has two actuate egrees-of-freeom. The two robots are not physically θ couple in any way an the aim of the feeback controller is to maintain the esire formation. The state of the system is given by: [l ; ; ] T In the l? control of the two mobile robots, the aim is to maintain a esire length, l an a esire relative angle between the two robots. The kinematic equations for the system of two mobile robots shown in Fig. is given by: for the rst robot an: _x i = v i cos i _y i = v i sin i () _ i =! i _l = v cos? v cos +! sin _ = l fv sin? v sin +! cos? l! g _ =! () for the secon robot where, = +? an v i ;! i (i = ; ), are the linear an angular velocities at the center of the axle of each robot. In orer to avoi collisions between robots, we will require that l >. We use stanar techniques of I/O linearization [8] to generate a control law that gives exponentially convergent solutions in the internal shape variables l an. The control law is given by:! = cos l (? )? v sin + l! + sin g (3) v =?! tan where = (l? l ) + v cos cos : This leas to ynamics in the l? variables of the form: _l = (l? l ) _ = (? ): Proposition Assume the system of two mobile robots shown in Fig. an the associate control law in (3). For the motion of the lea robot following a circular path, v = K,! = K, is locally asymptotically convergent to (t)+??arccos(k = ),, where = r Kl +K sin = arctan K cos K l +K sin. + K cos Proof: Since l an are exponentially stabilize, we focus our attention on the zero ynamics of the system, which for this case is given in terms of. We seek to show that the zero ynamics of the system are asymptotically stable, i.e., the system is asymptotically minimum phase [8].

3 The ierential equation for is given by: _ = K l + K sin cos(k t? ) + K cos sin(k t? ) The above equation can be re-written as: _ = cos(k t? + 0 +? ) where an are as given in the proposition. We make a change of variables to =? = K t + 0?, which gives the following ierential equation: _ = K? cos( +? ): (4) The equilibrium point, e =? + + arccos(k = ), is easily shown to be asymptotically stable, using a linearization of (4). Thus, the motion of the robots will locally converge to the equilibrium trajectory, e =? e = (t) +?? arccos(k = ). Corollary For the case that the lea robot follows a straight line (! = K = 0), converges exponentially to 0. Proof: For v = K an! = 0, we obtain the following expression for! after some algebraic manipulations:! = _ = K sin( 0? ) (5) where, 0 is the initial orientation of the rst robot at the beginning of the motion (an maintaine as a constant throughout the motion since! = 0 ) (t) = 0 ). Integrating the expression an assuming (0) = 0, we get: tan? 0 = tan 0? 0 exp? K t As t!,! 0 an exponentially converges to 0. Remarks:. Note that we can solve explicitly for the motion of as tan +? + tan +?? q?k +K q = C 0 exp??k +K p?k t where = K t + 0?, an are as ene in the proposition an > K (since l > ). C 0 is the constant of integration.. Base on the results of Proposition, it is clear that there is a constant oset, e =?, in steay state. If we assume that the lea robot is moving in a straight line, then we arrive at the result of Corollary. 3. The optimal point-to-point paths of a nonholonomic car with constraints on the turning raius were shown by Rees an Shepp [] to be compose of straight lines an circular arcs. This is seen to be generally true even for more complicate systems from the numerical results presente in [9]. Hence, in this section, we only investigate motions involving straight lines an circular arcs. (x 3, y 3 ) ROBOT 3 l 3 θ 3 ψ 3 (x, y ) θ l 3 ROBOT ψ 3 (x, y ) ROBOT Figure : Notation for l? l control l? l control: In Fig., we show a system of three nonholonomic mobile robots. The task involves stabilizing the istance of the thir robot from the other two robots. Thus the state of the thir robot is speci- e by (l 3 ; l 3 ; 3 ) T. Distances are measure from the center of the axle of the rst two robots to the castor of the thir robot which is oset by from its axle. Each robot has two actuate egrees-of-freeom. The three robots are not physically couple in any way an the aim of the feeback controller is to maintain the esire formation. In the l?l control, the aim is to maintain the esire lengths, l an 3 l 3 of the thir robot from its two leaers. Again, we assume that both l 3 an l 3 >. Aitionally, we require that the follower robot never lies on the line connecting the two lea robots. In particular, this requires that the two lea robots never separate by a istance greater than l 3 + l3. The kinematic equations for the system of three mobile robots shown in Fig. is given by () for i = ; an _l 3 = v 3 cos? v cos 3 +! 3 sin _l 3 = v 3 cos? v cos 3 +! 3 sin _ 3 =! 3 (6) for the thir robot where, i = i + i3? 3 (i = ; ). Again, we use I/O linearization to generate a feeback control law:! 3 = (l 3? l 3 ) cos + v cos 3 cos sin(? )? (l 3? l 3 ) cos? v cos 3 cos v 3 = (l 3? l 3 ) + v cos 3?! 3 sin cos : (7) θ

4 This gives exponential convergence for the controlle variables: _l 3 = (l 3? l 3 ) _l 3 = (l 3? l 3 ): (8) Proposition 3 Assume a system of three mobile robots as in Fig. an the associate control law given by (7). For a straight line parallel motion of the rst two robots (constant velocity v = v = K,! =! = 0 an 0 = 0 = 0 ), 3 locally converges exponentially to e 3 = 0 an 3 (t) = 3 (0) an 3 (t) = 3 (0). Proof: The proofs are similar in nature to those presente for l? feeback control. Restricting our attention to the zero ynamics, the above l? l control law gives tan 3? 0 _ 3 = K sin( 0? 3 ) _ 3 = 0 (9) _ 3 = 0 Integrating the expression an assuming 3 (0) = 30, we get: exp? K t : = tan 30? 0 As t!, 3! 0 an 3 exponentially converges to 0. Hence, the solution e 3 = 0 is a stable equilibrium point. Similarly the other two ierential equations for 3 an 3 gives constant value for all time, t, namely, 3 (0) an 3 (0) respectively. Thus, the choice of conguration variables is epenent on the type of control law employe to control the motion of a robot. For example in the case of l? control, the conguration variables of the controlle robot are given by (l ik ; ik ; k ) an in the case of l? l control, it is given by (l ik ; l jk ; k ), where i; j refers to the subscript for the lea robot/robots an k refers to the subscript for the controlle robot. For a long chain of robots moving with a particular formation, the rst robot in the chain plans an optimal path, avoiing any obstacles. Its velocity an trajectory information is then conveye to the rst generation of robots in the chain an they in turn act as the lea robots for subsequent generations own the chain.. Graphs of formations We can think of a formation as being mae up of two components: a) a irecte relational graph structure, or iagraph, that represents the internal topology of the formation, an b) the shape variables that escribe its internal state. Graphs in this paper are \irecte graphs" or iagraphs [0], as the lea robot is use to control the other follower robots in the formation. The arrows are thus irecte from the lea robot to the follower robot in the formation. In this context, specic formations lea to the evelopment of unique iagraphs, an a particular set of shape variables associate with each graph structure. The types of control investigate here, such as the l? control law, lea us naturally to the stuy of trees. A tree is a connecte acyclic graph. Sometimes in the presence of obstacles it is necessary to switch from one iagraph to another an this leas to internal ynamics of graphs an hence the resulting graph is non-isomorphic to the original graph. Two graphs, G an H are sai to be isomorphic if there exists a one-to-one corresponence between their point sets which preserve ajacency. Consier for example the two graphs, G an H shown in the gure below. The graph H is the e- G R R R H R R Figure 3: Isomorphic an Non-isomorphic iagraphs sire graph when there is a very narrow constriction an hence the robots must change to a single le formation. In that situation the original graph, G nees to transform to H an hence the various controller interepenencies must also change. Shown in the same gure is another formation, J, which is isomorphic to G. The change in the formations shoul be such that the subsequent generations of robots can obtain sensory ata from the robot immeiately ahea of them an hence can autonomously carry out their motions. One of the possible avenues of future research is to stuy the transitions from one graph to another which are non-isomorphic in orer to overcome obstacles in the environment. In the following section, we investigate the changes in formation between isomorphic graphs an emonstrate when the controller interepenencies change in the presence of obstacles. Once a specic graph has been chosen to represent a formation, the shape variables can be associate to the graph, an the ynamics governing them written own as in Eqs. an 6. In choosing the two pairings of variables for the internal ynamics, we have attempte to formulate the problem in a manner similar to previous work on geometric formulations of locomotion systems []. In the geometric setting, one can use the natural symmetries of the problem to ecouple the stuy of locomotion into two basic constituents: motion of the boy as a whole, generally associate to motion within a Lie group, G, an motion of the internal shape of the boy, escribe by some shape manifol, M. In this setting, the eect of shape changes on locomotion can be written succinctly as g? _g =?A(r) _r (0) _r = u; () J R

5 where g G, r M, an u represents the control inputs, an the map A is calle a connection. We have attempte to use similar techniques in this paper, where one can think of changes in formation as changes in the internal shape of the boy (or alternatively, as changes in the graph). One can then raw the analogy with biological systems, for example, paramecia or snakes, which can eform their internal shape in orer to move through narrow passageways. This woul parallel our use of changes in formation (iscusse below) from a triangular formation to a march-abreast formation in orer to move through a narrow corrior. For the present case, the internal symmetries of the problem are reecte in the position an orientation of the invariance of the lea robot. This allows us to write the shape ynamics in a form similar to () above, in which the group variables have eectively been remove from the equations. This is a point that requires further stuy, in orer to utilize fully the geometry of these types of systems. 3 Results Next, we show the implementation of these control laws in simulation using several examples. switching that occurs in the controllers in the presence of obstacles. In Fig. 6, R executes a straight 0 R R : Lea Robot : with R, R : with R, R O R : l-psi with R / with O, R : l-psi with R / with O, R : with, Figure 5: Controller schematic Obstacle Figure 6: Obstacle : Example R R 4 R R Figure 4: Parallel parking Moving in constant formation: We rst consier an example of parallel parking without any obstacles in the environment. The lea robot enote R plans an optimal path as shown in Fig. 4. In this example we only show two follower robots an they are both controlle by an l? control law. Using the notation of Fig., the conguration of the system of three robots is given by [x ; y ; ; l ; ; ; l 3 ; 3 ; 3 ] T. In this example, the lea robot's (R) optimal path minimizes the istance travele in the parallel isplacement of 4m an its path is as shown in Fig. 4. Also shown is the snapshot at various instants of time of the three robots moving in formation. In the secon example, we emonstrate the case of 6 robots moving aroun an obstacle. The initial formation of these robots with their controller interepenencies is as shown in Fig. 5. In both the examples shown in Fig. 6 an Fig. 7, we emonstrate the Actually, the invariance in orientation requires that all orientation variables, i, be shifte by the same amount. 0 Obstacle R Figure 7: Obstacle : Example line path from (0; 0; 0) to (0; 0; 0). As prove in Section., we woul expect the orientations of the followers to exponentially stabilize to that of the lea robot, R, after they overcome the obstacle. This is observe in the plot shown in Fig. 6. Initially when the lea robot follows the straight line, the follower robots, R an R 4 are controlle by the l? feeback law while the other robots are controlle by the l? l feeback law as shown in Fig. 5. However, when R an R 4 move within a preene istance of the obstacle, they switch from the l? controller to an l? l controller, where one of the istances is the istance to the obstacle. This guarantees that the robots are a preene istance away from the obstacle. After the obstacle is overcome an the istance of R an R 4 excees the threshol, it switches back to the l? controller. This is observe in the paths shown in Fig. 6. In Fig. 7, the optimal path for R is compute by minimizing a suitable norm which coul be the norm of velocities or the actuator forces. The initial an nal conguration of R is from (0; 0; 0) to (8; 0; 0). The trajectories for all the robots are as shown in Fig. 7. The transition in the controller interepenencies is R

6 very similar to that illustrate in Fig. 6. Although it is icult to see this from the gure, all of the robots move without contacting each other (or the obstacle). Change in formation : Finally, we very briey escribe the use of these methoologies to change formation. In this section we illustrate with an example the change in formation of a formation of robots as might be one in orer to squeeze through a constriction. Shown in Fig. 8 is a formation of six robots arrange in the triangular formation an the task is to change the formation from a triangular one to a rectangular one. Shown in the same gure is the initial an nal conguration of these robots an the paths of each of them. We note that there is no collision among the robots uring this transition phase from the triangular formation to the rectangular one. This is a formation change that is performe by changing only the shape variables an not the graph of the formation Conclusions Obstacle R R Obstacle Figure 8: Change in formation In this paper we have stuie strategies for controlling formations of mobile robots using methos from nonlinear control theory. For the current work, we have focuse on ecomposing the problem of controlling formations into one of controlling a single robot's motion in following one or two lea robots. We use the terms l? an l? l control to reect whether the control laws are base on tracking the position an orientation of the robot relative to a lea robot, or the position relative to two lea robots, respectively. It is assume that there is a single lea robot for the overall formation whose motion plans are generate externally. The results presente in this paper apply to general formations moving through an environment that may inclue obstacles. The two controllers propose here are use to achieve this objective. The l? an the l? l controller guarantee the correct formation, while the moie l? l controller can also be use to change the shape of our formation in the presence of obstacles. We contrast this approach to our former work using optimal control methos [3, 9]. These tools provie excellent open loop plans, but ten to incur much higher computational costs as the number of robots in the formation increases. For this reason, we use these methos to plan an optimal collision free path for the lea robot an use that information to control the eet of follower robots. This metho lens itself to a great avantage for on-line motion planning. The computation time typically for three robots avoiing a circular obstacle requires at least min. for a specically tailore compile coe an the formation may also not be necessarily maintaine. In contrast, the motion for the formation of six robots aroun an obstacle of higher complexity or the one shown in Fig. 7 has a run time of about 5 s. in Matlab. We also envision that the results from graph theory an the geometric techniques relate to robotic locomotion will ultimately enable us to construct explicit maps going from one formation to another. In other wors, we woul like to have the ability to generate smooth transitions between non-isomorphic graphs. This might be particularly useful in moving through narrow passageways, or past other obstructions in the environment. The use of shape changes to eect these types of transitions will play a key role in these types of transformations. Another critical issue is the propagation of the eects of suen changes in the motion of the lea robot in the presence of uncertainities through the formation. These topics are the subject of our future work in this area. References [] J. A. Rees an L. A. Shepp, \Optimal paths for a car that goes both forwars an backwars," Pacic. Journal of Math., vol. 45, no., pp. 367{393, 990. [] L. Kavraki, P. Svestka, J. C. Latombe, an M. H. Overmars, \Probabilistic roamaps for path planning in highimensional conguration space.," IEEE Transactions on Robotics an Automation, vol., no. 4, pp. 566{580, 996. [3] J. P. Desai an V. Kumar, \Nonholonomic motion planning for multiple mobile manipulators," in Proc. of 997 IEEE Int. Conf. on Robotics an Automation, (Albuquerque, NM), April 997. [4] C. Samson, \Time-varying feeback stabilization of carlike wheele mobile robots," The International Journal of Robotics Research, vol., no., pp. 55{64, 993. [5] O. Egelan, E. Berglun, an O. J. Soralen, \Exponential stabilization of a nonholonomic unerwater vehicle with constant esire conguration," in Proc. of 994 IEEE Int. Conf. on Robotics an Automation, (San Diego, CA), pp. 0{5, May 994. [6] T. Balch an R. Arkin, \Communication in reactive multiagent robotic systems," Autonomous Robots, vol. (), pp. 7{5, 994. [7] X. Yun, E. Paljug, an R. Bajcsy, \Tracs: An experimental multiagent robotic system," in DARPA Workshop on Software Tools for Distribute Intelligent Control Systems, (Pacica, CA), July 990. Also available as Tech. Report MS-CIS-90-60, University of Pennsylvania. [8] H. Asaa an J.-J. E. Slotine, Robot Analysis an Control. New York: John Wiley & Sons, 986. [9] J. Desai, C. C. Wang, M. Zefran, an V. Kumar, \Motion planning for multiple mobile manipulators," in Proc. of 996 IEEE Int. Conf. on Robotics an Automation, (Minneapolis, MN), 996. [0] F. Harary, Graph Theory. New York: Aison-Wesley Publishing Company, 97. [] J. P. Ostrowski an J. W. Burick, \The geometric mechanics of unulatory robotic locomotion." To appear in the International Journal of Robotics Research, July 997.