Stable Flocking Algorithm for Multi-Robot Systems Formation Control

Bin Lei, Wenfeng Li, Fan Zhang

AbstractThe problem of multiple robots system formation using a distributed control method is studied in this paper. The main idea of this paper is that uses swarm flocking control algorithm to implement the biods model of Reynolds among multi-robots. With the help of graph theory, we propose a provably-stable flocking control law, which ensures that the internal group formation is stabilized to a desired shape, while all the robots velocities and directions converge to the same. Player/Stage simulation results show that the proposed method can be efficiently applied to multiple robots formation control. With the characteristic of Player/Stage, the algorithm in this paper can be implemented on the real robots with so few or no changes.

I. INTRODUCTION

HERE has been a significant interest in the control of multiple robots moving in a formation or performing a coordinated task for the Multi-Robot Systems (MRS) researchers over resent years [1-5]. This is because there are many potential benefits of such systems performing tasks which are difficult for a single robot, including overall system robustness, intelligence, enhanced performance, and flexibility. Solutions for formation control problem of MRS are currently widely applied in search and rescue operations, landmine removal, remote space exploration and mapping, and also the control of satellites. Fundamentally, there are two groups of approaches in controlling multiple mobile robots formation, namely: centralized control and decentralized control. The first group use a centralized unit that supervises the whole group and commands the individual robots based on the global information [2, 3]. And the second group use distributed methods for achieving the coordination on the basis of local information and decisions [4, 5]. Those systems that require global information or broadcast communication may have a lack of scalability or high costs of the physical setup but allow more accuracy in forming a large range of robotic formations. On the contrary, systems using only local communication and sensor data, while limited in variety and precision of formations, tend to be more scalable, more robust, and easier

Manuscript received December 10, 2007. This work was supported in part by the National Natural Science Foundation of China under Grant 60475031. Bin Lei is with the School of Logistics Engineering, Wuhan University of Technology, Wuhan, 430063, P.R.China ( phone:86-027-86535227; e-mai: lbhaha@whut.edu.cn). Wenfeng Li is with the School of Logistics Engineering, Wuhan University of Technology, Wuhan, 430063, P.R.China (e-mail: liwf@whut.edu.cn). Fan Zhang is with the School of Logistics Engineering, Wuhan University of Technology, Wuhan, 430063, P.R.China (e-mail: ailexy@126.com).

to build [1]. In nature, flocking is a kind of ubiquitous self-organized phenomenon, such as flock of birds, school of fish, and crowds of people. And it is a form of collective behaviour of large number of interacting agents with a common group objective. The study of flocks/schools and swarms has attracted a lot of researchers in different fields such as biology, physics, robotics and control engineering [6-14]. Understanding the mechanisms and operational principles in them can provide useful ideas for developing formation control, distributed cooperative control and coordination of multiple mobile autonomous agents/robots. In 1987, Reynolds introduced a computer model called boids that simulates the motion of bird flocks or fish schools. Each bird has a local control strategy, but the desirable group behaviour is achieved. This model suggests that flocking is the combined result of three simple steering rules, which each agent independently follows [7]: Separation: steer to avoid crowding local flockmates. Alignment: steer towards the average heading of local flockmates. Cohesion: steer to move toward the average position of local flockmates. A similar model of Reynolds was proposed in 1995 by Vicsek et al. [8]. In this model, headings of each agent are updated as the average of the headings of agent itself with its nearest neighbours plus some additive noise. Numerical simulations indicate the spontaneous emergence of coherent collective motion, resulting in the headings of all agents to converge to a common value. A rigorous proof of convergence for Vicseks model was given by Jadbabaie et al. [9] in 2003. They proved that the alignment strategy leads to the result that all the agents headings converge to a common heading. The important contribution of the work in [9] is that connectivity in all times is not needed and connectivity of the network on average is sufficient for alignment of the agents. Following the works above, Tanner et al. [10, 11] considered a group of mobile agents moving in the plane with double-integrator dynamics. They introduced a set of control laws that enable the group to generate stable flocking motion, but these control laws cannot regulate the final speed and heading of the group. R. Olfati-Saber [12, 13] presented a theoretical framework for design and analysis of distributed flocking algorithms. They introduced three kinds of agents: -agents, -agents and -agents, and gave a universal definition of flocking for particle systems with similarities to Lyapunov stability analysis. The models of agent in most of present flocking researches are really simple: an agent is a point in the complex plane with

1544 978-1-4244-1823-7/08/$25.00 c 2008 IEEE

no kinematics constraints of motion. But in our work, the agents are simulated wheeled mobile robots. In this paper, we use virtual forces to propose a distributed controller for MRS formation control. It is shown that in a connected graph (time invariant or not), asymptotically, all velocities are the same, collisions are avoided, all potentials are minimized. One advantage of this controller is that it needs only the smallest local information to achieve the steady state for the whole group. This is similar with the behaviours of agents found in nature which may not be capable of performing the calculations demanded by the general rule in real time, and retains a straightforward implementation. II. MODEL OF FLOCK MEMBER

Ft Fn

y v l

A. Model of Mobile Robot.

In this paper, we built a model of Pioneer-2dx like mobile robot which is shown in Fig.1. The reference point with coordinates (x, y) is the midpoint of the rear wheels. We assume that the distance between both rear and front axles is l=1, and denote w as the speed of the front wheel of the robot and as the angle between the front wheel and the main direction of the robot. Simple computation shows that the model of the robot is:
x y w 0 w cos cos 0 0 w cos sin 0 + 0 u + 0 u w sin = 1 0 0 1 1 0 0

Fig.1. The robot model.

B. Problem Description.
Consider a group of N mobile robots moving on the plane, with dynamics expressed by double integrators:
ri = vi

(1)
2

v i = u i = u it + u in , i = 1, . . . ,N
T

(5)
T

where ri = (xi, yi) is the position of robot i, vi = ( x i , yi) its velocity, and ui = ( ut, un )T its acceleration inputs, see Fig.2. Let the relative position vector between robot i and j be denoted rij = ri rj. Robot i is steered via its acceleration input ui. Let R be the communication radius of robot i. Robots beyond this range are assumed not to affect robot i. In addition, we give the following assumptions: y Robots are of the same model and satisfy non-slipping and pure-rolling constraints. y The workspace is flat and without obstacles. y Each robot can extract necessary information via its communication equipment. y A collision is assumed to have occurred when the coordinates of two robots coincide. The problem can be described as: giving initial positions and orientations of a group of robots, the objective is to design the control input (5) so that if connectivity is maintained in the group, y formation is established, y an overall motion that satisfies the limitation of communication range R, y no collision among each robot, y velocity of each robot is synchronized, y and pair-wise distances between robots are stabilized to steady state values within a given range. This is being understood technically as a convergence property on the robot velocity vectors and their relative distances. We will present a realization of the control law (5) that achieves the control objective in the following section.

Let us do not care about the direction of the front wheels. We may still simplify the model and get a kinematic function which is expressed as the following control system:

x cos sin y = 0

0 v + 0 1

(2)

where the control values of each robot are its translational velocity v and angular velocity . The simplified dynamics equations of the robot are given by,
m v = mu t = Ft u t = Ft / m
I = Iu n = l Fn u n = l Fn / I

(3a) (3b)

where m is the mass of the robot, I is its moment of inertia, Fn and Ft are the normal component and tangential component of resultant force acting on the robot respectively and l denotes the moment arm of the forces. And from (3) we can get the following equations v(t+t) = v(t) + ut y t (t+t) =(t) + u y t ,
n

(4a) (4b)

these two values will be as the control input values for each robot.

2008 IEEE Congress on Evolutionary Computation (CEC 2008)

1545

j vi Qi i i y ri Fi x i R

set of neighbours of robot i, Ni = { j | ||rij|| R } {1, . . . , N }.

Definition 2 (Smooth function): The smooth function k(s) is a scalar function that smoothly varies between 0 and 1. It is used to construct a smooth potential force function. A possible choice is the following bump function which was introduced in [13]:

1, k (s) = 0.5[1+ cos((s k ) /(1 k ))], 0

0s<k k s 1 (6) s > 1.

Fig.2. Control forces acting on robot i and the neighbours of it.

III. FLOCKING ALGORITHM At the first time, we briefly present the main graph theoretic [14] terminology used in this paper. An (undirected) graph G consists of a vertex set, V, and an edge set E, where an edge is an unordered pair of distinct vertices in G. If x, y V, and (x, y) E, then x and y are said to be adjacent, with adjacency matrix A = [aij]. The degree matrix of G is a diagonal matrix (A) with diagonal elements which are row-sums of A. The symmetric matrix defined as
L = (A ) A

where k (0,1), and we choose k = 0.8 throughout the paper. Since the robots are in motion, their relative distances can change with time, affecting their neighbouring sets. The time dependence of the neighbouring relations gives rise to a switching graph. For each edge incident to robot i, we define an internal potential force function, fij which should satisfy: Definition 3 (Potential force function): The potential force function fij is a smooth function of the distance ||rij|| between robots i and j, such that i) the potential force fij = 0 when robots i and j are located at a desired distance D, ii) fij is increasing near ||rij|| = R. To construct a smooth collective potential force of a flock, function fij should be smoothed by the smooth function k(s) at distance ||rij|| = R, and constant fij = 0 for ||rij||> R, to capture the fact that beyond this distance there is no robot interaction. Define the following function as

is called the Laplacian matrix of G. It is known that the Laplacian matrix captures many topological properties of the graph. Among those, is the fact that L is always positive semidefinite, it has zero as a single eigenvalue whenever the graph is connected and the associated eigenvector is the n-dimensional vector of ones, 1n. The second largest eigenvalue, 2 is known to convey a lot of information about the structure of the graph and its connectivity. Lemma 1: Let G be a graph on n vertices with Laplacian L. Then for any vector x, i) L satisfies the following sum-of-squares (SOS) property: x T Lx = 1 a ij ( x j x i ) 2 , 2 ( i , j )E ii) Let G be a connected graph, then
2 ( L ) = min
x T Lx > 0, || x || 2

f (|| rij ||) = h(|| rij || D)k (|| rij || / R)

(7a)

h(s) =

a + b becs a a b ab(ecs 1) , ( )+ = 2 becs + a 2 becs + a

(7b)

where h(s) is an uneven sigmoid function with parameters that satisfy 0 < a < b ,c>1 and guarantee h(0) = 0. The collective potential function V(r) of a group of robots is a nonnegative attractive/repulsive pair-wise potential function with a global minimum at ||rij|| = D and a finite cut-off at R . Then, define the following potential energy function as

V (r) =

as x ranges over all nonzero vectors orthogonal to 1n. Proof: All these results are well-known in the field of algebraic graph theory and can be found in [14]. Definition 1 (Neighbouring graph): [10] The neighbouring graph, G = {V, E}, is an undirected graph consisting of a set of vertices (nodes), V = {r1, . . . ,rN }, indexed by the agents in the group, and a set of edges, E = {(ri, rj) V V | ||ri -rj||<R }, containing unordered pairs of nodes that represent neighbouring relations. Let Ni denote the index

1 F (|| rij ||) 2 i ji,Ni

(8a)

1 = H (|| rij || D)k (|| rij || / R) 2 i ji,Ni

(a + b ) b ln( e cs + 1) bs . c a

H (s) =

h ( s ) ds

(8b)

The functions H(s) and h(s) are depicted in Fig.3.

1546

2008 IEEE Congress on Evolutionary Computation (CEC 2008)

where denotes the Kronecker product. This m-dimensional Laplacian satisfies the following SOS property: x = 1 a || x x || 2 . (13) xT L ij j i 2 ( i , j )E Before presenting the stability analysis of flocking behavior under the control law (11), we need to define the following structural Hamilton function:
H(r, v) = V(r) + T(v)
2 i i

(14)

where T ( v ) = (1 / 2 ) m v is the kinetic energy of the multi-robot systems in the moving frame.
Fig.3. The attractive/repulsive function H(s) and h(s) with parameters a=1, b=5, c=3 and D=2.

From function (7), the virtual forces acting on the agent i (see Fig.2) are defined as
Fi = Qi =

f (|| rij ||)( r j ri ) / || r j ri || k (|| rij || / R )( v j v i ) .

j N i

(9)

Theorem 1: Consider a group of robots applying the control law (11) with structural dynamics (12). Let c = {(r,v) : H(r,v) c} be a level-set of the Hamilton function, such that for any solution starting in c the robots form a flock t 0 . Then, the following statements hold i) all robots asymptotically move with the same velocity, ii) given c < c* = F (0), no robot internal collisions occur for all t 0. Proof: Taking the derivative of H(r, v), we have

j N i

Then, the control law in (5) can be written as

u it = F i cos( i i ) / m u
n i

H ( r , v ) = V ( r ) + T ( v ) = v iT V i + v iT m i v i
(12 )

(10)

= ( Q i + F i sin( i i )) l / I .

v ) = v iT V i + v iT ( V i L i
( i , j ) E

(15)

Thus, (4) becomes

(13 ) v = 1 = v iT L i 2

ij

|| v j v i || 2 0

v i (t + t ) = v i (t ) + Fi cos( i i ) t / m

i (t + t ) = i (t ) + (Qi + Fi sin( i i )) l t / I .
IV. STABILITY ANALYSIS

(11)

Stability analysis of small swarm or flock agents were considered in [12-18]. In [16], Beni et al. consider a synchronous distributed control method for discrete one and two dimensional swarm structures and prove stability in the presence of disturbances using Lyapunov methods. Swarm stability under total asynchronism (i.e., asynchronism with time delays) was first considered in [17]. A one dimensional discrete time totally asynchronous swarm model is proposed and stability (swarm cohesion) is proved. The authors prove asymptotic convergence under total asynchronism conditions and finite time convergence under partial asynchronism conditions. In this section, the stability analysis method is mainly according to reference [13]. A group of robots using the control law (11) obtain the following structural dynamics:

which means the structural energy H(r, v) is monotonically decreasing for all t 0 . In addition, H(r, v) c for all t 0 that implies c is an invariant set. From LaSalles invariance principle, all the solutions of (12) starting in c converge to the largest invariant set in E = {(r,v) c : H = 0}. However, since the group of robots constitutes a dynamic flock for all t 0, G is a connected graph for all t 0. Thus, based on (15), we conclude that the velocities of all robots match to the same value (v1 = = vn), which proves part i). Moreover, the configuration r asymptotically converges to a fixed r* = D. Assume two distinct robots m, n collision at time t1, for all t 0, we have 1 V ( r (t )) = F (|| rm (t ) rn (t ) || + F (|| rij ||) 2 i \{ m ,n } j \{i ,m ,m },N i

F (|| rm (t ) rn (t ) || = F ( 0 ) = c * .
Hence, V(r (t1)) c*. But from (14) we have
V(r (t)) = H(r (t), v (t)) - T(v (t)) H(r (t), v (t)) c < c*

r = v m v = V ( r ) L ( r ) v

(12)

for all t 0. This is in contradiction with the above inequality. Therefore, no two robots collide at any time t 0, which proves part ii). Remark 1: The assumption in Theorem 1 rarely hold for a universal set of initial states and thus fragmentation emerges instead of flocking. We will demonstrate these phenomena in the next sections.

is the m-dimensional graph Laplacian matrix, where L defined by = L I L m

2008 IEEE Congress on Evolutionary Computation (CEC 2008)

1547

V. SIMULATIONS In order to verify the validity and the performance of the algorithm presented in the above sections, we have implemented it written in Java and simulated it in a robot simulation software Player/Stage. Player [19] provides a network interface to a variety of robots and sensor hardware and supports multiple concurrent client connections to devices, creating new possibilities for distributed and collaborative sensing and control. Stage [20] simulates a population of mobile robots moving in and sensing a two-dimensional bitmapped environment. Stage devices present a standard Player interface so few or no changes are required to move between simulation and hardware. Several controllers designed in Stage have been demonstrated to work on real robots. In this section, we present several simulation results of 2-D flocking. The following parameters remain fixed throughout all simulations: D = 2m, R = 4m, a = 1, b = 5 and c = 3 for h(s). The max speed of each robot is 0.6m/s, and m = 1kg, l=1m, I =1 for each robot. With a random position within a ball R = 5m, initial velocities were also selected randomly with arbitrary directions, fragmentation will be created, see Fig. 4. This is because of lacking of existence of a group objective for all of the robots. Fragmentation is a generic form of collective behavior of robots applying control law (11). This behavior is insensitive to the type of probability distribution of the initial position of the robots. Apparently, for the case of a highly dense initial connected graph with small initial velocity mismatch, one might expect that the group of robots form a flock. Fragmentation phenomenon can be viewed as lack of cohesion in a group of robots [13]. A special initial state of 5 robots is demonstrated in Fig. 5(a). We use the nearest neighbor principle which means we ignore all neighbours but the nearest one within the neighbour set (Ni =1). In matrix terms, this modification is intended to take the adjacency matrix of the connectivity graph in the original problem and reduce it to a minimal connected form (modulo row / column swaps). Thus, converge speed is slower than the following case, but decreasing the computing demand of each robot. See fig. 5(b) (c) (d) (e). Through the simulation, we find that the control law (11) only guarantees the creation of flocking in some special initial states. It is not surprising that a large number of robots with a random set of initial states, flocking behavior is not created. To create flocking for a generic set of initial states, we should give each robot a group objective. Virtual leader [21] [22] or -agent [13] may be two good choices and this will be a future work for us. VI. CONCLUSIONS In this paper, we presented a distributed control algorithm based on virtual force and nearest neighbour principle for multi-robots formation control and implemented it for the double integrator model in MRS. Simulation results show that the proposed method can be efficiently applied to multiple robots formation control.

Fig. 4. Fragmentation for 5 robots applying control law (11).

(a) t = 0s

(b) t = 25s

(c) t = 50s

(d) t = 100s

-5

-6

Position Y (m)

-7

-8

-9 6 7 8 9 Position X (m) 10 11 12

(e) Steady state. Fig. 5. Flocking for 5 robots (nearest neighbour Ni=1).

REFERENCES
[1] C. A. Nguyen, Q. P. Ha, S. Huang, and H. Trinh. Observer-based decentralized approach to robotic formation control. In Proceedings of the 2004 Australian Conference of Robotics and Automation, pages 18, Canberra, Australia, December 2004. [2] C. Belta and V. Kumar, "Trajectory design for formations of robots by kinematic energy shaping", Proc. IEEE International Conference on Robotics & Automation, Washington DC, pp.2593-2598, 2002.

1548

2008 IEEE Congress on Evolutionary Computation (CEC 2008)

[3] M. Egerstedt and X. Hu, "Formation Constrained Multi-Agent Control", Proc. IEEE Int. Conf. Robotics & Automation, Seoul, Korea, pp.3961-3966, 2001. [4] H. Yamaguchi, T. Arai, and G. Beni, "A distributed control scheme for multiple robotic vehicles to make group formations", Robotics and Autonomous Systems, vol. 36, no. 4, pp. 125-147, 2001. [5] S. Carpin and L.E. Parker, "Cooperative Leader Following in a Distributed Multi- Robot System", Proc. IEEE International Conference on Robotics & Automation, pp. 2994-3001, 2002. [6] A. Okubo, Dynamical aspects of animal grouping: swarms, schools, flocks, and herds, Adv. Biophysics., vol. 22, 1986, pp. 194. [7] Reynolds, Flocks, birds, and schools: a distributed behavioral model, Computer Graphics, vol. 21, 1987, pp. 25-34. [8] T. Vicsek et al. Novel type of phase transitions in a system of self-driven particles. Physical Review Letters, 75:12261229, 1995. [9] A. Jadbabaie, J. Lin, and A.S. Morse, Coordination of groups of mobile autonomous agents using nearest neighbor rules, IEEE Trans. Auto. Control, vol. 48, 2003, pp. 988-1001. [10] Tanner H G, Jadbabaie A, Pappas GJ. Stable flocking of mobile agents, Part I: Fixed Topology. Proc. Of 42nd IEEE Conference on Decision and Control, 2003:2010 - 2015. [11] Tanner H G, Jadbabaie A, Pappas GJ. Stable flocking of mobile agents, Part II: dynamic topology. Proc. of 42nd IEEE Conference on Decision and Control, 2003:2016 - 2021. [12] R. Olfati-Saber and R. M. Murray, Flocking with obstacle avoidance: Cooperation with limited communication in mobile networks, in Proc. 42nd IEEE Conf. Decision and Control, vol. 2, Dec. 2003, pp: 20222028. [13] R. Olfati-Saber, Flocking for multi-agent dynamic systems: algorithms and theory, IEEE Trans. Automatic Control, vol. 51, pp. 401420, 2006. [14] C. Godsil and G. Royle, Algebraic Graph Theory, Vol. 207 of Graduate Texts in Mathematics. New York: Springer-Verlag, 2001. [15] Veysel Gazi and Kevin M. Passino, Stability Analysis of Swarms, IEEE Trans. Auto. Control, vol. 48, 2003, pp. 692-697 [16] G. Beni and P. Liang, Pattern reconfiguration in swarmsconvergence of a distributed asynchronous and bounded iterative algorithm, IEEE Trans. Robot. Automat, vol. 12, pp. 485490, June 1996. [17] Y. Liu, K. M. Passino, and M. M. Polycarpou, Stability analysis of M-dimensional asynchronous swarms with a fixed communication topology, IEEE Trans. Auto. Control, vol. 48, no. 1, pp. 7695, Jan. 2003. [18] E. W. Justh and P. S. Krishnaprasad, A simple control law for UAV formation flying, Inst. Sys. Res., Univ. Maryland, College Park, MD, Tech. Rep. 2002-38, 2002. [19] Brian P. Gerkey, Richard T. Vaughan, Most Valuable Player: A Robot Device Server for Distributed Control, Proc. IEEE/RSJ Intl. Conf. on Intelligent Robots and System, 2001, pp. 1226. [20] Richard T. Vaughan. Stage: a multiple robot simulator, Institute for Robotics and Intelligent Systems Technical Report (IRIS-00-393), University of Southern California, 2000. [21] H. Shi, et al, Virtual Leader Approach to Coordinated Control of Multiple Mobile Agents with Asymmetric Interactions, Proceedings of the 44th IEEE Conf. on Decision and Control, and the European Control Conference, 2005, pp.6250-6255. [22] H. Shi, et al., Flocking of Multi-Agent Systems with a Virtual Leader, Proc. Of the 2007 IEEE Symposium on Artificial Life, 2007, pp.287-294.

2008 IEEE Congress on Evolutionary Computation (CEC 2008)

1549