Cross-Product Steering for Distributed Velocity Alignment in Multi-Agent Systems

Transcription

1 SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS Cross-Product Steering for Distributed Velocity Alignment in Multi-Agent Systems Nima Moshtagh, Student Member, IEEE, Ali Jadbabaie,, Member, IEEE, and Kostas Daniilidis, Senior Member, IEEE Abstract We study the problem of distributed velocity alignment among a group of nonholonomic agents in 2 and 3 dimensions. Inspired by social aggregation phenomena such as flocking and schooling in birds and fish, we develop vision based control laws for velocity alignment and motion coordination. The proposed control laws are distributed, in the sense that only information from neighboring agents are included. Furthermore, the control laws are coordinate-free and do not rely on measurement or communication of heading information among neighbors, but instead requires measurements of bearing, optical flow and timeto-collision, all of which can be measured using vision. Index Terms Distributed coordination, cooperative control, vision-based control. I. INTRODUCTION Cooperative control of multiple autonomous agents has become a vibrant part of robotics and control theory research. The main underlying theme of this line of research is to analyze and/or synthesize spatially distributed control architectures that can be used for motion coordination of large groups of autonomous vehicles. Inspired by social aggregation phenomena in birds and fish [24], [2], researchers in robotics and control theory have been developing tools, methods, and algorithms for distributed motion coordination of multi-vehicle systems. Some of this type of research is focused on flocking, synchronization and and formation control [2], [4], [9], [26], [32], [25], [3], [28], [2], [9], [23], while others focus on rendezvous, distributed coverage and deployment [6], []. A key assumption implicit in all of the above references is that each vehicle or robot (hereafter called an agent) communicates its position and or velocity information to its neighbors. Two agents are denoted as neighbors if the distance between them is less than a pre-specified communication range. Other researchers use vision as the primary sensing modality and they often assume that agents are capable of communicating an estimate of their position to their neighbors [27], [34], [35]. In [7] the formation control problem is considered as a visual servoing task. For a pair of mobile robots denoted as leader and follower, it is assumed that the follower can measure a pair of features on the leader. The relative kinematics of the leader and the follower in the image plane is derived, The authors are with the GRASP Laboratory, University of Pennsylvania, Philadelphia, PA 94. {nima,jadbabai,kostas}@grasp.upenn.edu Ali Jadbabaie s research is supported in parts by the following grants: ARO-MURI W9NF-5--38, ONR/YIP-54237, ONR N46436, and NSF-ECS Kostas Daniilidis research is supported in part by: NSF-IIS-8329, NSF-IIS-2293, NSF-EIA , and ARO/MURI DAAD and by using feedback linearization string stability and leaderto-formation stability is achieved. In [8], the authors propose a framework for vision-based formation control for a group of nonholonomic robots. Omnidirectional cameras are used as the only sensor for robots. Also, a decentralized controller that can tolerate changes in the formation is proposed. The images from the omnidirectional cameras are used to estimate the relative angle and distances between agents. By applying input-output feedback linearization, the authors design control laws for leader-following and obstacle avoidance. The goal of this paper is to propose a series of distributed control laws that will result in global velocity alignment among a set of nonholonomic agents in the plane and 3 dimensional space. A key feature of our proposed distributed controller is that no distance or heading measurement is required. First, we propose a coordinate-free control law for flocking and velocity alignment among a set of kinematic agents using purely local interactions. The key idea is to design the angular velocity of each agent based on the cross product of the velocity of the agent and its neighbors so that it turns every agent towards the average direction of its nearest neighbors. The designed control laws are similar to geodesic control laws that was introduced recently in [2]. The key difference is that the new controllers do not allow rotation about the velocity vector. It is worthwhile to note that the planar version of the proposed controller reduces to the wellknown Kuramoto model of coupled nonlinear oscillators which has been extensively studied in mathematical physics as well as control and dynamics communities [29], [3], [7]. The same model has also been used for phase regulation of cyclic robotic systems [5]. The main result of the paper is that these controllers can be recast in terms of quantities that do not require communication among nearest neighbors but only require visual sensing. Contrary to [2] where it is assumed that each agent has access to the exact values of its neighbors velocities, here we design a distributed control law that uses only visual clues from nearest neighbors to achieve distributed velocity alignment and flocking, even when the topology of the underlying graph representing sensing among neighbors changes with time. Each agent is assumed to have only monocular vision and capable of measuring basic visual capabilities such as bearing angle, optical flow (bearing derivative) and time-tocollision. Incidentally, experimental evidence suggests that several animal species, including pigeons and flies, are capable

2 SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS 2 of estimating time-to-collision [8], [33], []. Computationally, time-to-collision can be estimated from the ratio of area change to area or from the divergence of the optical flow [4], [6]. Regarding optical flow, we refer the reader to the survey [3]. One important feature of our proposed control law is that we are not assuming that the relative distance measurements are available. As a result, we are able to extend our results to motion in three dimensional space. From the vision point of view, our approach is similar to visual servoing methods used in [7], [8], [3]. However, these approaches assume that a specific vertical pose of an omnidirectional camera allows the computation of both bearing and distance, while in our approach no distance measurement is required. This paper is organized as follows: In section II we explain the problem statement and express the main results. We also prove that the state where all the agents have the same velocity is attractive under our control law. The proof of convergence for the case where the neighboring relations are time-dependant is provided in section III. In section IV the equation that describes the relative motion of neighboring agents is derived. The results of sections II, III and IV are used in sections V and VI to derive vision-based control inputs for velocity alignment in 2 and 3 dimensions, respectively. Our simulations are given in section VII. Finally, the conclusions are states in section VIII. II. THE CROSS-PRODUCT INPUT Consider a group of N agents in the 2 or 3 dimensional space. Each agent is capable of sensing information from its neighbors. The neighborhood set of agent i, N i, is a set of agents that can be seen by agent i. The precise meaning of seeing will be cleared later in section V. The size of the neighborhood depends on the characteristics of the sensors. The neighboring relationship between agents can be conveniently described by a proximity graph. Definition 2.: The proximity graph G = {V, E, W} is a weighted graph consisting of: a set of vertices V indexed by the set of mobile agents; a set of edges E = {(v i, v j ) v i, v j V, and i j}; a set of weights W, over the set of edges E. If x, y V, and (x, y) E, then x and y are said to be adjacent, or neighbors and we denote this by writing x y. The number of neighbors of each vertex is its valence. A path of length r from vertex x to vertex y is a sequence of r + distinct vertices starting with x and ending with y such that consecutive vertices are adjacent. If there is a path between any two vertices of a graph G, then G is said to be connected. Our goal in this section is to design a control law for each agent so that the headings of the mobile agents reach agreement and velocity vectors are aligned, resulting in a swarm-like pattern. We therefore define the consensus set as follows: Definition 2.2: The set of states where the headings of all agents are the same and velocity vectors are aligned is called the consensus set. Note that in the above definition, we do not care about the value of the agreed upon velocity, just the fact that agreement has been reached. All agents are assumed to be kinematic moving with a constant unit speed i.e v i =, i {,..., N}. Let the rotation matrix R i = [R ix, R iy, R iz ] represent the attitude of agent i in the fixed world frame (R ik is the k-th column of matrix R i ). The full kinematic equation describing the rotation of each agent is Ṙ i = R i ω i, i =,..., N where ω i = [ω ix, ω iy, ω iz ] T is the body angular velocity expressed in the body frame of agent i and ω i = ω iz ω iy ω iz ω ix ω iy ω ix is the skew symmetric matrix of ω i. We have assumed the velocity vector v i is along the third column of the attitude matrix (or along the z-axis of the body frame), so we have v i = R i e 3 where e 3 = [,, ] T. Therefore, the reduced kinematics of each agent becomes v i = Ṙie 3 = R i ω i e 3 which can be simplified to v i = ω ix R iy + ω iy R ix, i =,..., N. () System () is underactuated with control inputs ω ix and ω iy. Note that ω iz is free, which implies that we do not directly control the torsion or rotation around v i. We choose the control input ω i of the form ω i = v i v j (2) where N i. = {j i j} {,..., N}\{i}. (3) Input (2) is a coordinate-free control input, because it is in terms of v i and v j, both expressed the body-frame of agent i. With the assumption that each agent knows the velocities of all its neighbors within a prespecified radius, here is an informal description of how the velocity alignment is done. Each agent performs the following tasks: ) it detects its neighbors according to the proximity graph G; 2) it computes the average of the velocity vectors of its neighbors, and 3) it rotates in a direction that aligns its velocity vector with the average of its neighbors. Our controller does not guarantee that the proximity graph of the multi-agent system remains connected, rather we claim that if graph G remains connected, then all the velocity vectors converge to the consensus set. Now we have the following theorem: Theorem 2.3: Consider N agents with dynamics given by (). If the proximity graph of the agents is fixed and connected, and all initial velocity vectors are in the same hemisphere, by applying the input (2) all trajectories converge to the set of

3 z z SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS 3 V center 2.8 V sum.5.6 y i x x.5.5 y Fig.. The volume of the intersection of the smallest cone containing the velocity vectors and the unit sphere is considered as the Lyapunov function. equilibrium points given by ω i =. Furthermore, the velocity consensus set is locally asymptotically attractive. Proof: Since the velocity of agent i in its own frame is v i = [,, ] T, we have ω i = ω ix ω iy = v jx v jy = v jy v jx. ω iz v jz Thus we have the following expressions for ω ix, ω iy and ω iz : y ω ix = v jx = Riyv T j (4) ω iy = v jy = Rixv T j (5) ω iz =. (6) The x and y components of the angular velocity as given by (4) and (5) reveal that the cross-product input is a special case of the geodesic control law that was introduced in [2]. Given that the components of the angular velocity represent the yaw, pitch and roll of the rigid body, from (6) we see that, with our choice of control input, roll or rotation about the velocity axis is zero. The proof is completed in three steps. It is shown in Step A that control input (2) minimizes the total energy of the system, and drives the system to the equilibria given by ω i =, i {,..., N}. ) Step A: Let v = (v,..., v N ) and consider the following positive definite function W (v) = v i v j 2 = vi 2 j i( T v j ) (7) j i which is a measure of misalignment among the velocity vectors, and the summation is over all the neighboring agents Fig. 2. The versor vector is pointing towards the inside of the cone. i j. The time derivative of W is given by Ẇ (v) = N v i j i( T v j + v j T v i ) = v i T v j i= N [ = ωix ( Riyv T j ) + ω iy ( R T ] ixv j ) i= N = ωix 2 + ωiy 2 = i= N ωi T ω i (8) i= Consider the positively invariant set Ω c = {v W (v) c}. By LaSalle s invariance principle, all the trajectories starting in Ω c converge to the largest invariant set contained in the set Γ = {v Ẇ = } = {v ω = (ω,..., ω N ) = }. The solution of ω = is the equilibrium of v i v j =, i {,..., N}. (9) In Steps B and C of the proof we show that that if all the velocities start in the same hemisphere, the only equilibrium of ω = is where all the velocities are equal, resulting. As a result, the velocities vectors will be asymptotically aligned. 2) Step B: It can be shown that with the control input (2) any hemisphere in the world frame is invariant, meaning that if all the velocity vectors start in the same hemisphere at time t =, they will not leave that hemisphere. Consider the smallest cone that contains all the velocity vectors. This cone is defined by a unit vector through the center of the cone v center, and an angle φ m, as shown in Figure. The intersection of this cone with the unit sphere is a minimum volume cone, C m, which contains all the velocity vectors. Therefore all the vectors have a geodesic distance of at most φ m from the central vector, v center, meaning: φ j φ m, j {,..., N}. Let agent i be the vector on the cone with the maximum radian distance from v center, i.e. φ i = φ m. Therefore, for all v j in the neighborhood of agent i, we have φ j φ i =

4 SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS 4 φ m, ( j N i ). Let us define v i = v j with φ i as its angular distance from v center. Since all the vectors v j are inside the cone C m, their sum is also a vector inside C m (i.e. φi φ m ). The input of agent i can be written in terms of v i : ω i = v i v j = v i v j = v i v i. Let y i be the vector that is pointing from the extreme vector v i to v i as shown in Figure (2). This unit-length vector - also known as versor - is defined as: y i. = (v i v i ) v i (v i v i ) v i = v i < v i, v i > v i v i sin α i () where α i is the radial distance between vectors v i and v i, and <, > represents the inner product. Then ω i can be written as ω i = < v i, R iy > = v i sin α i < y i, R iy > < v i, R ix > v i sin α i < y i, R ix > This control input is a geodesic control input [2] that moves v i along the geodesic versor y i. Since v i C m then vector y i is pointing inside the cone C m. Hence the extreme vector v i will always rotate towards the inside of the cone. As long as φ m < π/2 the geodesic direction will be unique and the cone C m will be invariant. Hence, the interior of a hemisphere (defined by a cone with φ m = π/2 ɛ, ɛ > ) is invariant. 3) Step C: Here we show that if all the velocity vectors start in the same hemisphere, the only stable equilibrium in the hemisphere is the velocity consensus set where v i = v j, for all i j. It was proved in Step A that the system converges to the equilibrium of ω = (ω,..., ω N ) =, which is the solution of (9). To show that v i = v j for all i and j, we use a contradiction argument. Suppose condition (9) holds, but not all the vectors are equal. Let v i be one of the vectors that defines the v i X i Z i v j v perp j Fig. 3. Consider the orthogonal projection of the velocity vectors v j onto the xy-plane of agent i. Y i z y Fig. 4. The cross product vectors lie on the xy-plane of the body frame of agent i. minimum volume cone C m with φ i = φ m. Since the proximity graph is assumed to be connected, there exist an index set M N i such that v i v j = () j M while v i v j, j M. All the vectors v j, j M are in the cone C m and all the cross product vectors u ij = v i v j are consequently perpendicular to v i and lie in the xy-plane of the body frame of agent i. It can be shown that all the vectors u ij are actually in one half-plane. To show that consider all the vectors expressed in the body frame of agent i, in which v i is along the body z-axis: v i = [,, ] T. Let us decompose the vectors v j N i into two components: v parallel j which is parallel to v i, and v perp j that is perpendicular to v i. Thus, v j = v parallel j.5 + v perp j. Therefore v i v j = v i v perp j. Since v i is chosen such that it is on the cone with angle φ m, and v j C m, the projected vectors v perp j all fall into a half-plane (see Figure 3): v perp j {(x i, y i, z i ) y i >, z i = }. The heading of any vector in this half-plane can be parameterized by an angle θ (, π). Therefore, the cross product vectors u ij = v i v j will fall into the half-plane parameterized by θ (π/2, 3π/2) (see Figure 4): u ij {(x i, y i, z i ) x i <, z i = }. Consequently, u ij are given by u ij = v i v perp j = u ij cos θ j sin θ j, θ j (π/2, 3π/2). Condition () implies that u ij sin θ j =, j M u ij cos θ j = j M x

5 SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS 5 which does not have a solution in the half-plane where θ j (π/2, 3π/2), because the second summation is negative. This is in contradiction with (). Therefore, v i = v j, j M N i. If the proximity graph G were a complete graph (equivalent to an all-to-all neighboring relationship), then the proof would be complete. Otherwise, it needs to be shown that at the equilibrium all the vectors must be equal. Define I = {, 2,..., N}. Since the proximity graph of the agents is assumed to be connected but not complete, there is an agent k N i such that it s neighborhood includes some j I \ N i. Because k N i we have v k = v i and φ k = φ m φ j, j N k. Similarly, all the vectors u kj = v k v j lie in the xy-plane of the body frame of agent k. Noting that all the vectors u kj are actually in a half-plane, similar to above argument it implies that v k = v j, j N k is the only possible solution. Now the remaining vectors are in the set I \ {N i N k } which loses cardinality as more neighbors are considered. In the end, it can be shown that all the vectors must be equal (v i = v j, i, j I) given that they all start in the same hemisphere and their connectivity graph remains fixed and connected. This means that the velocity consensus set is locally attractive. III. SWITCHING TOPOLOGY When the neighborhood relations changes over time, the underlying proximity graph will be time varying and we will have a switching topology. Let P denote a set that indexes the class of all simple graphs defined on N vertices; so if p P then G p is the corresponding proximity graph on N vertices. Let σ(t) be a switching signal whose value at time t is the index of the graph representing the proximity graph of agent i. The switching time t k is when signal σ is discontinuous; hence for t [t k, t k+ ) we have σ(t) = p k, and G pk represents the proximity graph of the N-agent system. Let the piecewise constant signal σ : [, ) P be the switching signal with consecutive switching times separated by a non vanishing dwell time, µ D. This means that the time between two consecutive switchings satisfies t k+ t k µ D, all k. A piecewise constant is a signal that exhibits a finite number of discontinuities in any finite time interval and that is constant between consecutive discontinuities. The set of all switching signals σ with this property is denoted by S dwell []. It is assumed that the agents are synchronized in the sense that t ik = t jk = t k for all i, j {, 2,..., N} and all k. What this means in the present context is that each agent is constrained to change its control law only at discrete time instances. Each agent i would use control laws similar to (2). The control input is now given by ω i (t) = v i (t) v j (t), t [t k, t k+ ) (2) (t) which is hybrid, since the set of neighbors N i changes discontinuously. The hybrid controller may result in change of the proximity graph as the switching occurs. Our algorithm is very similar in nature to the Circumcenter Algorithm for rendezvous problem of mobile agents that was introduced by Cortes et. al [5]. In [5] the goal is to control the position vectors of a number of mobile agents to reach a common value, and each agent moves towards the circumcenter, center of the minimum radius d-sphere enclosing all the agents. But here we control the velocity vectors of agents to drive them to the consensus, and each agent rotates to align its velocity vector with the average velocity of its neighbors. The circumcenter algorithm drives the agents towards circumcenter that is a always inside the convex hull of the positions; while the cross-product control law drives the velocity vectors towards the average velocity vector, which (with the assumption that they are all in the same hemisphere) is also a vector inside the convex cone containing the vectors. Therefore, both algorithm are similar in nature, but one acts on position vectors and the other on velocity vectors. Hence, for the proof of the convergence of our controller for the switching topology, we rely on the results developed in [5]. First, we review some basic concepts regarding the stability of discrete-time dynamical systems and set-valued maps The following definitions are used in the proof of the convergence of the velocities when switching occurs and can be found in [5]. Definition 3.: An algorithm is a nondeterministic discretetime dynamical system, and is given by a set-valued map T : R d 2 (Rd) with the property that T (p) for all p R d. A trajectory of an algorithm T is a sequence {p m } m N {} R d with the property that p m+ T (p m ). An algorithm is closed at p R d if for all pairs of convergent sequences p k p and p k p such that p k T (p k), one has that p T (p). An algorithm is closed on W R d if it is closed at all p W. A set C is weakly positively invariant with respect to algorithm T if p C, p T (p ) such that p C. The function V : R d R is non-increasing along T on W R d if V (p ) V (p), p W and p T (p). Now we can state the equivalent of LaSalle s Invariance Principle for closed algorithms. Theorem 3.2: (LaSalle s Invariance Principle for Closed Algorithms) Let T be a closed algorithm on W R d and let V : R d R be a continuous function non-increasing along T on W. Consider the set Ω = {p W p T (p) such that V (p ) = V (p)}. Assume the trajectory {p m } m N {} of T takes values in W and is bounded. Then there exist c R such that p m M V (c), where M is the largest weakly positively invariant set contained in Ω. Now we have the following theorem for velocity alignment of a group of kinematics agents with a switching topology: Theorem 3.3: Let σ : [, ) P be a piecewise constant switching signal corresponding to all graphs over N vertices whose switching times t, t 2,... satisfy (t k+ t k ) µ D, k. Consider a system of N agents with reduced kinematics given by v i = ω ix R iy +ω iy R ix, i =,..., N. Let the initial

6 SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS 6 velocity vectors start in the same hemisphere. If the proximity graph of the agents remains connected, then by applying the steering law (2) all trajectories converge to the equilibria of ω i (t) =. Furthermore, the velocity consensus set is locally attractive. Proof: According to the Definition 3., the dynamic system under consideration with switching topology is an algorithm, denoted by T Gp. Because the proximity graph G p is assumed to be connected, by Proposition 4.3 in [5], the algorithm is proved to be closed on (, π/2) N. In our system, the largest latitude angle φ m (t) W = (, π/2), corresponding to a an extreme velocity vector that falls on the boundary of the minimum volume cone C m, defines a trajectory for our dynamic system. Consider the minimum volume cone C m with volume V c. For a cone with radian angle φ m (t) this volume is given by Fig. 5. Y world j r ij r j Y i β ij Y j θ j X j q ij i r i Configuration of two agents. X i θi X world V c (t) = V s 2 ( cos φ m(t)). (3) where V s = 4π/3 is the volume of the unit sphere. It was shown in the proof of Theorem (2.3) that the cone C m is invariant and the cross-product input rotates a vector on the boundary of the cone towards the inside of the cone, hence, shrinking the volume over the time interval [t k, t k+ ). At the switching time t k+ the proximity graph changes. Suppose v i is the vector on the boundary of C m and has the largest latitude angle at the switching time t k+, i.e. φ i (t k+ ) = φ m (t k+ ). Any vector inside the cone has a latitude angle smaller than φ m (t k+ ). Therefore by rotating v i towards the average of its neighbors, it will move towards the inside of the cone, and the radian angle of the cone decreases: φ m (t k+ ) φ m (t k ). From (3) it can be seen that the volume of the cone decreases: V c (t k+ ) V c (t k ). Also the volume of the cone depends continuously on the value of φ m. Thus V c is a continuous and non-increasing function along the trajectory of T Gp on W N. Now Theorem 3.2, the LaSalle s invariant principle for closed algorithms, is used to deduce that the trajectory φ m (t) converges to the largest weakly positively invariant set contained in Γ = {(v,..., v N ) V c (φ(t k+ )) = V c (φ(t k ))} which corresponds to the case that the volume of the cone C m does not change anymore. Since V c (t k ) is a non-increasing positive sequence, it is lower-bounded and it converges to zero, V c (t k )., i.e. the only fixed-point of the system corresponds to the zero-volume cone. This is true because if at the fixed-point the volume is fixed and nonzero, then there exists an agent i such that v i is not equal to other velocity vectors. Agent i will have a nonzero control input ω i that will consequently decrease the volume of the cone C m. This is contradicting the fact that on the largest positively invariant set Γ the volume is fixed. Therefore, volume V c (t k ) must converge to zero. Since φ i (t) φ m (t), it is seen from (3) that as volume V c (t) and φ m (t) converge to zero, all the radian distances φ i (t) must also converge to zero. Therefore, the trajectory φ m (t) and all the velocity vectors asymptotically converge to the consensus set {(v,..., v N ) v i = v j, i, j {,..., N}}. Thus, the velocity consensus set is locally asymptotically attractive. Now that we have proved the convergence of the velocity vectors to the consensus set using the cross-product control input (2), and under fixed and switching topology conditions, we can apply this control law for vision-based velocity alignment. In the next section we derive the equation of motion that describe the relative motion of two rigid body agents in the workspace. IV. EQUATION OF MOTION Let b ( ) denote a vector in the body frame of agent i and f ( ) a vector in the fixed world frame. Let j N i be any neighbor of agent i. The relative position of agents i and j can be represented by a vector b r ij R 3 : b r ij = ( b R f )( f r ij ) where b R f is the rotation matrix from the fixed frame to the body frame. Since ṙ ij = ṙ j ṙ i = v j v i, by differentiating r ij with respect to time, we get: b r ij = ( b R f )( f r ij ) + ( b R f )( f v j f v i ) = bˆω i ( b R f )( f r ij ) + ( b v j b v i ) where b ω i is the body angular velocity vector of agent i and to get the last equality we have used ˆω i = ( b R f )( f R b ). In the end we get b r ij = b ω i b r ij + ( b v j b v i ). (4) From now on we drop the superscript b for notational simplicity. It is understood that all the vectors are given in the body-frame of agent i, unless otherwise specified. Let denote the distance between agents i and j. We normalize the optical flow equation (4) by dividing it by to get ṙ ij = ω i q ij + (v j v i ), j N i (5)

7 SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS 7 where q ij = r ij / is the unit-length bearing vector. Equation (5) holds for all the agents that are in N i. Thus, we sum (5) over all j N i to get: ṙ ij = ω i q ij + (v j v i ) (6) The goal is to solve (6) for input ω i so that it is only a function of some measurable quantities such as bearing and time-to-collision. Since the equation of the relative motion (6) is normalized by the relative distance, we need to use a modified version of the control input (2) that is also normalized by the relative distance, : ω i = (v i v j ). (7) Since the factor of / is a positive weight, its affect is only on the rate of convergence, and the controller (7) will drive the system to the consensus set as well. V. VISION-BASED PLANAR FORMATION When nonholonomic robots move on the plane, we can model their motion with unicycle kinematic agents. It is assumed that all agents move with constant unit speed. Thus, for each agent the kinematic model becomes: ẋ i = cos θ i ẏ i = sin θ i θ i = ω i i =,..., N. (8) When we restrict the motion of agents to plane z =, the cross-product control law (7) will reduce to ω ix = ω iy =, ω iz = sin(θ i θ j ) (9) which is the same input used for synchronization in the Kuramoto model of coupled nonlinear oscillators [3]. In this section we use this control law to solve the optical flow equation (6) for ω i so that it is only in terms of the measurable quantities. For planar agents, the horizontal bearing β ij is defined by the angle that the projection of agent j makes with the bodyframe of agent i, and is given by: β ij = θ i atan2 ( ) y j y i, x j x i (2) and its rate of change β ij defines the optical flow. It can be shown that the time-to-collision between agents i and j can be measured as the rate of growth of the image area [8], i.e. the relative change in the area A ij of projection of agent j on the image plane of agent i. In other words τ ij = 2A ij A ij =. (2) To formally define the sensing, we assume that each agent i can measure: β ij or the relative bearing in agent i s reference frame βij or optical flow : the rate of change of bearing τ ij or time-to-collision Fig. 6. X i Z i ω i v j v i θ i θ j Y i Vectors v i, v j and ω i in the special case of planar motion. for any agent j in the set of its neighbors N i. For robots moving on the ground, the solution of the optical flow equation (6) can be obtained by substituting values for q ij, ω i, v i and v j restricted to the planar motion (corresponding to plane z = ). The body-frame of agent i is such that the velocity vector v i is along the y-axis, and the angular velocity ω i is orthogonal to the (x, y)-plane (see Figure 6). Thus v i = [,, ] T ω i = [,, ω iz ] T. The unit bearing vector q ij and the unit velocity vector v j of a neighboring agent in the body-frame of agent i become q ij = cos β ij sin β ij, v j = sin(θ i θ j ) cos(θ i θ j ) where β ij is the bearing angle. A simple differentiation of r ij = q ij reveals that we have ṙ ij = M ij r ij where M ij is the measurement matrix, and is given by: M ij = /τ ij β ij β ij /τ ij. /τ ij By substituting for v i, v j, ω i and q ij in (6) we get: [ ] M ij q ij = [ sin(θi ] θj ) q ω ij + cos(θ i θ j ). iz Out of the three linear equations we obtained only the first one is used to solve for ω iz, and by using (9) we get ( ) j N βij i sin β ij τ ω iz = ij cos β ij. (22) sin β ij This is the vision-based control law that is equivalent to (9) and gives us velocity alignment for a group of kinematic agents. See section VII for simulations that show the effectiveness of this vision-based control input. VI. VISION-BASED FORMATION CONTROL IN 3D Consider a group of N agents in the 3 dimensional space. Each agent is capable of sensing some information from its neighbors as defined by (3). Similar assumptions on the neighborhood relationships hold as explained in section II.

8 SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS 8 Z i ψ ij q ij shows that ṙ ij = M ij r ij where the measurement matrix M ij is given by /τ ij β ij ψ ij cos β ij M ij = β ij /τ ij ψ ij sin β ij. ψ ij cos β ij ψ ij sin β ij /τ ij X i β ij Y i Thus, we have the left-hand-side of (6) in terms of the measurements: ṙ ij / = M ij q ij (26) Fig. 7. The polar angle β ij and the azimuth ψ ij in 3D. In three dimensions, the location of agent i in the fixed world coordinates is given by (x i, y i, z i ) and its velocity is v i = (ẋ i, ẏ i, ż i ) T. Let q ij be the unit bearing vector starting from agent i and pointing towards agent j, as shown in Figure 7. The bearing of agent j with respect to agent i is the projection of agent j and is represented by a pair of angles (β ij, ψ ij ). The azimuth angle β ij is given by the angle between projection of q ij and the x-axis of agent i s body frame and the latitude angle ψ ij is given by the angle between the vector q ij and the z-axis of agent i s body frame. Given q ij = [q, q 2, q 3 ] T and q ij = sin ψ ij cos β ij sin ψ ij sin β ij (23) cos ψ ij we can define the bearing angles as β ij = atan2 ( q 2, q ) (24) ψ ij = arccos(q 3 ). (25) The speed of projection is the optical flow and is given by the rate of change of bearing ( β ij, ψ ij ), and time-tocollision between agents i and j can be measured as in (2). In summary, each agent i can measure: (β ij, ψ ij ) as the bearing angles ( β ij, ψ ij ) as the optical flow τ ij or time-to-collision for any agent j in the set of neighbors N i. For a group of agents moving in the 3-dimensional space, a controller that results in velocity alignment is the one that steers the headings and attitudes of all the agents to a common value. In what follows, we use the control law (7) to solve the optical flow equation (6). By solving the above equation we mean finding an expression for vector ω i that is only a function of the measurable quantities. Next, we show how we can write the terms of optical flow equation in terms of either inputs or measurable quantities. A. Position Vector r ij The position vector r ij R 3 is defined by r ij = q ij where q ij is given by (23). A simple differentiation of r ij The optical flow vector is q ij. However, as the only other variables are the measurable bearing angles β ij and ψ i j we will use the term flow for the bearing derivatives ( β ij, ψ ij ). B. Relative velocity (v i v j ) Given v i = [,, ] T and v j = [v jx, v jy, v jz ] T the control input (7) becomes ω i = (v i v j ) = v jy v jx = ω ix ω iy Now, we write the relative velocities in terms of the inputs ω iy and ω ix. (v j v i ) = v jx v jy = ω iy ω ix (27) v jz a where a = j (v jz ). Thus, the first two elements of the relative velocities can be written in terms of the inputs ω ix and ω iy, and as we will see, the third element has no effect on our solution, so we leave it as it is. C. Solving The Optical Flow Equation Now, we have every term in (6) in terms of either the input ω i or the measurable quantities. We plug in (26) and (27) into optical flow equation, which we repeat here: ṙ ij / = ω i q ij + (v j v i ). After substitution we have M ij q ij = ω ix ω iy q ij + ω iy ω ix. a The above gives us 3 linear equations in terms of the input and our measurements, but only the first two are used to solve for the control inputs. By solving the first two equations for ω ix and ω iy we get ω ix = ( j M ijq ij ) 2 cos ψ ij (28) ω iy = ( j M ijq ij ) cos ψ ij (29) where the numerators are ( ) M ij q ij = sin ψ ij cos β ij τ β ij sin ψ ij sin β ij ij + ψ ij cos ψ ij cos β ij ( M ij q ij = )2 sin ψ ij sin β ij + τ β ij sin ψ ij cos β ij ij + ψ ij cos ψ ij sin β ij.

9 SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS 9 Therefore, equations (28) and (29) are the vision-based control inputs. They are only in terms of the measured quantities of bearing, optical flow and time-to-collision, as desired. In section VII we numerically show the vision-based control laws asymptotically drive the system to the consensus set. VII. SIMULATIONS In this section we numerically verify the effectiveness of the cross-product control law and our theory of vision-based velocity alignment. In Figure 8.a the initial positions and headings of kinematic agents in the 3 dimensional space are shown. By applying the control input 2 all the agents reach the consensus state as shown in Figure 8.b. Even though the topology of the underlying proximity graph is changed (compare Figures 8.a and 8.c), the group reached the consensus set because the graph remained connected. Figures 9.a and 9.b demonstrate the initial and final configurations of five kinematic agents that are governed by the vision-based control laws (28) and (29). For each agent the required measurements are numerically computed. From (24) and (25) the bearing β ij and attitude ψ ij were computed and by numerically differentiating them we obtained the optical flow. Also, equation (2) was used to compute time-tocollision τ ij between two agents. We can designate one of the agents to be the leader of the group. It was proved in [22] that in the presence of a leader all other agents (followers) are forced to align their velocities with that of the leader. Figure shows an example of a leader-following using vision-based control laws with a dynamic leader. All agents follow the leader moving in a sinusoidal path. The initial positions and headings of five kinematic agents in a plane is shown in Figure.a. Figure.b shows that in a planar motion all agents can follow the leader by using the vision-based control law (22). VIII. CONCLUSIONS We have developed control laws for nonholonomic robots that only use visual measurements from cameras for distributed velocity alignment and flocking. The control laws are based on measurement of bearing, optical flow and time-tocollision, all of which can be measured from images. The proposed control laws steer each agent towards the average direction of its neighbors. We show that if the proximity graph of agents stays connected, all headings will align and agents will flock as a group, even when interconnection is time varying. We are currently working on experimental verification on real robots. A series of Evolution Robots (ER) robots are being used to verify the theory. Each robot is equipped with a camera that has a fish-eye lens capable of seeing the entire surrounding of the robot (with a field of view of 36 degrees). The challenging part of the experiment is making accurate measurements of optical flow and time-to-collision in the presence of noise in the images. Both quantities require differentiating noisy measurements that will amplify the noise and increase the error. REFERENCES [] M. Batalin, G.S. Sukhatme, and M. Hattig. Mobile robot navigation using a sensor network. In IEEE International Conference on Robotics and Automation, pages , 24. [2] R. W. Beard and V. Stepanyan. Synchronization of information in distributed multiple vehicle coordinated control. In Proceedings of IEEE Conference on Decision and Control, December 23. [3] S.S. Beauchemin and J.L. Barron. The computation of optical flow. ACM Computing Surveys, 27: , 995. [4] R. Cipolla and A. Blake. Surface orientation and time to contact from image divergence and deformation. In Proc. Second European Conference on Computer Vision, pages 87 22, 992. [5] J. Cortes, S. Martinez, and F. Bullo. robust rendezvous for mobile autonomous agents via proximity graphs in d dimensions. Submitted to IEEE Transactions on Automatic Control, 26. [6] J. Cortes, S. Martinez, T. Karatas, and F. Bullo. Coverage control for mobile sensing networks. IEEE Transactions on Robotics and Automation, 2(2): , February 24. [7] N. Cowan, O. Shakernia, R. Vidal, and S. Sastry. Formation control of nonholonomic mobile robots with omnidirectional visual servoing and motion segmentation. In IEEE Conference on Intelligent Robotic Systems, 23. [8] A. Das, R. Fierro, V. Kumarand J. Ostrowski, J. Spletzer, and C. J. Taylor. Vision based formation control of multiple robots. IEEE Trans. Robotics and Automation, 9:83 825, 22. [9] J. Fredslund and M.J. Mataric. A general algorithm for robot formations using local sensing and minimal communication. IEEE Trans. Robotics and Automation, pages , 22. [] S. N. Fry, R. Sayaman, and M. H. Dickinson. The aerodynamics of free-flight maneuvers in drosophila. Science, 3: , 23. [] J. P. Hespanha. Uniform stability of switched linear systems: Extensions of lasalle s invariance principle. IEEE Transactions on Automatic Control, 49(4), April 24. [2] A. Jadbabaie, J. Lin, and A. S. Morse. Coordination of groups of mobile autonomous agents using nearest neighbor rules. IEEE Transactions on Automatic Control, 48(6):988, June 23. [3] A. Jadbabaie, N. Motee, and M Barahona. On the stability of Kuramoto model of coupled nonlinear oscillators. In Proceedings of the American Control Conference, June 24. [4] E.W. Justh and P.S. Krishnaprasad. Equilibria and steering laws for planar formations. Systems and Control letters, 52():25 38, May 24. [5] E. Klavins and D. E. Koditschek. Phase regulation of decentralized cyclic robotic systems. International Journal of Robotics and Automation, (3): , 22. [6] J. J. Koenderink and A. J. van Doorn. How an ambulant observer can construct a model of the enviornment from the geometrical structure of the visual inflow. Kybernetik, 977. [7] Y. Kuramoto. Cooperative Dynamics in Complex Physical Systems. Springer, Berlin, 989. [8] D. N. Lee and P. E. Reddish. Plummeting gannets - a paradigm of ecological optics. Nature, 583: , 98. [9] Z. Lin, M. Brouke, and B. Francis. Local control strategies for groups of mobile autonomous agents. IEEE Transactions on Automatic Control, 49(4): , April 24. [2] D. J. Low. Following the crowd. Nature, 47: , 2. [2] N. Moshtagh and A. Jadbabaie. Distributed geodesic control laws for flocking of nonholonomic agents. IEEE Transactions on Automatic Control, 26. [22] N. Moshtagh, A. Jadbabaie, and K. Daniilidis. Vision-based distributed coordination and flocking of multi-agent systems. In Proceedings of Robotics: Science and Systems, Cambridge, USA, June 25. [23] D.J. Naffin, G.S. Sukhatme, and M. Akar. Lateral and longitudinal stability for decentralized formation control. In Proceedings of the International Symposium on Distributed Autonomous Robotic Systems, 24. [24] A Okubo. Dynamical aspects of animal grouping: swarms, schools, flocks, and herds. Advances in Biophysics, 22: 94, 986. [25] R. Olfati-Saber and R. M. Murray. Flocking for multiagent dynamical systems: algorithms and theory. IEEE Transactions on Automatic Control, 5(3):4 42, 26. [26] N.E. Leonard P. Ogren, E. Fiorelli. Cooperative control of mobile sensing networks: Adaptive gradient climbing in a distributed environment. IEEE Transaction on Automatic Control, 49(8):292 32, August 24.

10 SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS [27] P.E. Rybski, S.A. Stoeter, M. Gini, D.F. Hougen, and N.P. Papanikolopoulos. Performance of a distributed robotic system using shared communications channels. IEEE Trans. Robotics and Automation, 9: , 22. [28] R. Sepulchre, D. Paley, and N Leonard. Collective motion and oscillator synchronization. In V. Kumar, N. Leonard, and A. S. Morse, editors, Cooperative Control, volume 39 of Lecture Notes in Control and Information Science. Springer, 24. [29] S. H. Strogatz. From Kuramoto to Crawford: exploring the onset of synchronization in population sof coupled nonlinear oscillators. Physica D, 43: 2, 2. [3] H. Tanner, A. Jadbabaie, and G Pappas. Flocking in teams of nonholonomic agents. In V. Kumar, A. S. Morse, and N. E. Leonard, editors, Proceedings of the Block Island Workshop on Cooperative Control, Springer Series in Control and Informations Science, to appear. 24. [3] R. Vidal, O. Shakernia, and S. Sastry. Formation control of nonholonomic mobile robots with omnidirectional visual servoing and motion segmentation. In IEEE International Conference on Robotics and Automation, 23. [32] W. Wang and J. J. E. Slotine. On partial contraction analysis for couplled nonlinear oscillators. technical Report, Nonlinear Systems Laboratory, MIT, 23. [33] Y. Wang and B. J. Frost. Time to collision is signalled by neurons in the nucleus rotundus of pigeons. Nature, 356(6366): , 992. [34] T. Weigel, J.-S. Gutmann, M. Dietl, A. Kleiner, and B. Nebel. Cs freiburg: coordinating robots for successful soccer playing. IEEE Trans. Robotics and Automation, 9: , 22. [35] Z. Zhu, K. Rajasekar, E. Riseman, and A. Hanson. Panoramic virtual stereo vision of cooperative mobile robots for localizing 3d moving objects. In IEEE Workshop on Omnidirectional Vision, pages 29 36, 2.

11 SUBMITTED TO IEEE TRANSACTIONS ON ROBOTICS position [z] position [z] 2 position [z] (a) (b) (c) Fig. 8. a) The initial configuration of kinematic agents in 3D. b) By using the cross-product input (2) agents align their velocities in 3D. c) The topology of the proximity graph has changed, but because the graph remained connected, all the velocities converged to the consensus set position [z] position [z] (a) (b) Fig. 9. vectors. a) The initial configuration of 5 kinematic agents in 3D. b) The vision-based control inputs (28) and (29) result in the alignment of the velocity position[y] position[x] (a) (b) Fig.. a) The initial position and headings of 5 agents in 2D. b) The agents follow a dynamic leader in a planar motion.