Formation and Flocking Control Algorithms for Robot Networks with Double Integrator Dynamics and Time-Varying Formations

In this work, we study the problem of designing control laws that achieve time-varying formation and flocking behaviors in robot networks where each agent or robot presents double integrator dynamics. To design the control laws, we adopt a hierarchical control approach. First, we introduce a virtual velocity, which is used as a virtual control input for the position subsystem (outer loop). The objective of the virtual velocity is to achieve collective behaviors. Then, we design a velocity tracking control law for the velocity subsystem (inner loop). An advantage of the proposed approach is that the robots do not require the velocity of their neighbors. Additionally, we address the case in which the second state of the system is not available for feedback. We include a set of simulation results to show the performance of the proposed control laws.


Introduction
Collective behaviors in nature are interesting phenomena that have attracted the attention of scientists from different disciplines. Interesting examples include flocks of birds and schools of fish, social networks, vehicular traffic, etc. In these examples, collective behaviors emerge due to local interactions between the members of the swarm. Such collective behaviors can be used to perform complex tasks; thus, the objective is to emulate them in groups of robots (in this work, the words agent and robot are used interchangeably), such as wheeled mobile robots, aerial and underwater vehicles, or robot manipulators. Controlling multiple agents presents some advantages, such as time reductions and energy savings when performing complex tasks. On the other hand, some of the applications that can be performed when replicating the collective behaviors in robot networks are search and rescue missions, object transportation, agricultural irrigation, and area mapping.
Examples of collective behaviors are consensus, rendezvous, synchronization, formation, and flocking. In this work, we only focus on formation and flocking behaviors in robot networks with double integrator dynamics. Currently, we can find a great variety of works that address these behaviors. A survey on formation control algorithms can be found in [1]. In this work, the authors classify the cases of study in formation control based on position, displacement, and distance measurements. In [2,3], the leader-follower approach is used to carry out the desired formation in multi-agent systems with double integrator dynamics. In [4,5], the leader-follower approach is also used, but these works consider time-varying formation problems. The problem of time-varying formation is more challenging than fixed formation since the geometric pattern of the formation changes over time. The problem of achieving time-varying formation with bounded control inputs is observer for the problem of flocking with partial information. Extensive simulations are reported in Section 6. Finally, the paper ends with some conclusions given in Section 7.

Problem Statement
Consider a robot network with N identical elements whose dynamics are governed by double integrator dynamicsṗ where the states of the i-th robot are the position p i ∈ n and velocity v i ∈ n ; u i ∈ n is the control input and i ∈ N = {1, 2, . . . , N}. Next, we describe the collective behaviors under study.

Formation.
The time-varying formation problem consists of steering all the robots of the team to a certain position to form a desired time-varying geometric shape or pattern while the velocity of each robot converges to the rate of change of the formation. The formation control objective can be defined as where p ij (t) = p i (t) − p j (t) ∈ n is the error of position between the agent i and its neighbor denoted by j. δ ij (t) = δ i (t) − δ j (t) ∈ n describes the geometry of the formation and δ i (t) ∈ n , δ j (t) ∈ n are time-varying offset vectors that are at least twice differentiable.
Flocking. In this collective behavior, all the robots follow a common reference velocity while maintaining a desired formation; thus, the flocking control objective is to achieve where v d (t) ∈ n is the desired velocity for the team.
From the above definitions, the time-varying formation is a special case of flocking when v d (t) = 0.

Graph Theory
We consider that the communication topology is fixed and the communication channels can be either unidirectional or bidirectional. Such topologies can be modeled by undirected and directed graphs. A graph G = (N , E ) consists of a set of nodes N (each node represents a robot in the network) and a set of edges E ∈ N × N . If the set of edges are unordered pairs of N , the graph is called undirected. For an undirected graph, the edge (i, j) ∈ E denotes that robots i and j can obtain information from each other. On the other hand, if the edges are ordered pairs of N , the graph is called a directed or digraph, and information only flows in one direction. The adjacency matrix A = [a ij ] ∈ N×N for an undirected graph is defined as For a directed graph, we have a ij = 1 j ∈ N i 0 otherwise (5) where N i is the set of neighbors transmitting information to agent i. The Laplacian matrix For an undirected graph, the Laplacian matrix is symmetric and positive semidefinite. However, for a directed graph, L is not necessarily symmetric. By construction, the Laplacian matrix satisfies L1 N = 0, meaning that the vector 1 N = 1 . . . 1 ∈ N is an eigenvector of L and it is associated with the zero eigenvalue λ 1 = 0. For an undirected graph, all of the nonzero eigenvalues of L are real and positive, whereas, for a directed graph, the rest of the spectrum of L have positive real parts. If the eigenvalue λ 1 = 0 has algebraic multiplicity of one, then the graph is connected [24][25][26]. If there exists a sequence of edges (undirected path) that joins any pair of nodes, we say that the graph is connected [27]. A digraph is strongly connected if there is a directed path that connects every pair of nodes.

Formation and Flocking Controllers
In order to design the formation and flocking control laws, we exploit the cascade structure (chain of integrators) of the robot's dynamics given in (1). To this end, we introduce the virtual input ϑ i ∈ n and rewrite (1) as follows: The open-loop system (7) can be analyzed as an interconnected system with states (p i ,ṽ i ) and inputs (ϑ i , u i ) coupled by the termṽ i . Similar to the backstepping approach, the control problem is divided into two particular control problems: the first step consists of designing the control ϑ i that achieves timevarying formation or flocking for the subsystem (7a), and as a second step, we design the control u i that steers the velocity errorṽ i asymptotically to zero. The hierarchical control approach simplifies the control design; nevertheless, it imposes the restriction that the virtual input ϑ i must be at least once differentiable; see (7b). Furthermore, it is desirable that the time derivative of control input ϑ i for the i-th robot does not depend on the velocity of its neighbors. To fulfill the control objectives mentioned above, we propose the following dynamic control law: where v d (t) ∈ n is the desired flocking velocity that the robot network must follow, ϕ i ∈ n is an additional state, k i , γ i ∈ are positive gains, c > 0 is the coupling strength, a ij is the ij-th element of the adjacency matrix and ϕ ij = ϕ i − ϕ j . In Figure 1, we present in a visual manner the proposed control law, so that readers have a better interpretation. Now, we are in a position to state the first result of this paper, which is summarized in the following proposition. In the green block, the external controller is included, while the orange block contains the internal controller; the values of the terms a i1 , a ij and a iN will change depending on the used graph.

Proposition 1.
Assume that the graph G is connected and consider the robot network (1) in a closed loop with (8). Then, the dynamic control law (8) guarantees (ii) flocking behavior in the sense of (3).
To proceed with the stability analysis, we use the following change in coordinates [26]: where the matrix Q ∈ N−1×N is defined as .
By construction, the matrix Q satisfies [28] Using these properties, it can be shown that Finally, by taking into account (12), (14) and where ξ = ϕ q ṽ ∈ 3nN−n is the extended state and where L r = QLQ ∈ N−1×N−1 is the reduced Laplacian matrix; O nN×nN ∈ nN×nN and O n(N−1)×nN ∈ nN×nN denote zero matrices. Since G is a connected graph by assumption, the real parts of the eigenvalues of L r are positive [26]. Moreover, the eigenvalues of L r are the same as the Laplacian matrix L except for λ 1 = 0. This implies that the matrix −cL r is Hurwitz. Since each matrix on the diagonal of A ξ is a Hurwitz matrix and A ξ is a block upper triangular matrix, it follows that A ξ is also Hurwitz. Therefore, the origin of (16) (ξ = 0) is a globally exponentially stable equilibrium point. From Equation (15), the exponential convergence of q(t) to zero implies r(t) → 1 N ⊗ r as t → ∞ and hence r i (t) → r , which in turn implies r ij (t) → 0. Combining the previous result with the exponential convergence of ϕ(t) to zero yields This implies that the robot network achieves the desired geometry formation. The last part of the proof consists of showing that the robots' velocities satisfy lim t→∞ v i (t) =δ i (t) (∀i ∈ N ) for time-varying formation and lim t→∞ v i (t) = v d (t) +δ i (t) for the flocking behavior. This can be done by noticing that lim meaning that the flocking behavior is achieved. Finally, for time-varying formation (v d (t) = 0), which implies that the time-varying formation is achieved.

Time-Varying Formation and Flocking Controllers without Velocity Measurements
Motivated by the fact that many commercial robots are only equipped with position sensors, in this section, we combine the dynamic control law (8) with a linear observer that estimates the second state for the robot i. Consider the robot network where y i ∈ n is the output of the system. To address the lack of velocity measurements, we propose the observer˙p wherep i ∈ n andv i ∈ n denote, respectively, the estimated position and estimated velocity;ỹ i ∈ 2 is the output error and Ξ 1i ∈ n×n , Ξ 2i ∈ n×n are the observer gains.
In Figure 2, we show a block diagram that depicts the controller when including the Luenberger observer.
where v d (t) ∈ n is the desired flocking velocity, k i , γ i ∈ are positive gains, c > 0 is the coupling strength, andv i ∈ n is the estimated velocity. Assume that the communication graph G is connected and the observer gain Ξ i = Ξ 1i Ξ 2i ∈ 2n×n is chosen such that the matrix A i − Ξ i C i is Hurwitz where Then, the controller (19) in combination with the observer (18) guarantees (ii) flocking as defined in (3); Proof. First, we define the observer By taking into account (17) and (18), the time derivative ofx i is given bẏx where A oi = A i − Ξ i C i ∈ 2n×2n . By taking into account the observer error dynamics (20), the robot network (17) and the control law (19), the overall closed-loop dynamics are given bẏ where E i = O n γ i I n ∈ n×2n and r i is defined in (9). Notice that the addition of the observer error dynamics does not destroy the cascade structure of the closed-loop dynamics. Therefore, we can follow the steps of the proof of Proposition 1 to obtaiṅ where η = ϕ q ṽ x ∈ 5nN−n is the extended state vector, q ∈ n(N−1) is defined in (12),x = x 1 · · ·x N ∈ 2nN , and A η is a block upper triangular matrix given by with A o = blockdiag{A o1 , · · · , A oN } ∈ 2nN×2nN and Since the observer gain Ξ i guarantees that A oi = A i − Ξ i C i is Hurwitz, each matrix on the diagonal of A η is Hurwitz; thus, the equilibrium point η = 0 is exponentially stable. The previous result directly implies that lim t→∞v i (t) = v i (t), and hence item (iii) is proven. Due to the fact that ϕ(t), q(t) andṽ(t) converge exponentially to zero, we can follow the steps of the proof of Proposition 1 to prove items (i) and (ii).

Flocking Control with Partial Information
In order to design the controllers (8) and (19), it is assumed that all the robots have access to the desired flocking velocity v d (t). In this section, we study the scenario in which only a portion of the robots have access to the flocking velocity. Let N d ⊂ N be the subset that contains all the robots that have access to v d (t). Since at least one robot knows v d (t), it is possible to estimate the flocking velocity for the robot i / ∈ N d by using information about its neighbors.

Proposition 3.
Assume that G is undirected (directed) and connected (strongly connected). Moreover, assume that the desired flocking velocity v d (t) ∈ n is a bounded continuous function and satisfies thatv d (t) → 0 as t → ∞. Then, the distributed observeṙv where µ > 0 is the observer gain (24), its time derivative is given by˙v

Proof. The flocking estimation error is defined asv
where we use the fact thatv d Using again the Kronecker product, (26) becomes˙v Since G is undirected (directed) and connected (strongly connected) by assumption, the eigenvalues of F are strictly positive (see [29] for further details). This implies that the matrix −µ(F ⊗ I n ) is Hurwitz. Thus, ifv d (t) = 0, the system (27) has an exponentially stable equilibrium point at the origin. The differential Equation (27) can be analyzed as a stable linear system with a continuous bounded inputv d (t) that converges asymptotically to zero [30]. Therefore, we can conclude that lim t→∞v d (t) = 0.
The results of Propositions 2 and 3 can be combined to solve the problem of flocking without velocity measurements and with partial information.

Proposition 4. Consider the robot network (1) in a closed loop with the flocking control law
wherev i ∈ n andv d i ∈ n are obtained from the observers (18) and (24), respectively. Assume that G is undirected (directed) and connected (strongly connected). Moreover, assume that the desired flocking velocity v d (t) ∈ n is a bounded continuous function and satisfies thatv d (t) → 0 as t → ∞. Then, the controller (28) in combination with the observers (18) and (24) achieves flocking in the sense of (3).
Proof. By taking into account (17), (7), (18), (24) and (28), the closed-loop dynamics reaḋ Using (12) and (14), the closed-loop dynamics can be written aṡ It can be verified that A σ is Hurwitz and hence σ = 0 is a globally exponentially stable equilibrium point ifv d (t) = 0. Therefore, the closed-loop dynamics (30) are a stable linear system with inputv d (t) that converges to zero. Thus, we conclude that σ(t) → 0 as t → ∞. This in turn implies thatv Following the steps of Proposition 1, it can be shown that

Numerical Results
In this section, we present numerical results to show the performance of the proposed control laws and observers. To carry out the simulation, we consider a robot network with six agents represented by (1). The simulations are carried out on Matlab software. In each simulation, it is assumed that only the position of each robot is available from sensor measurements; thus, the observer (18) is used in combination with the formation and flocking controllers.

Time-Varying Formation Control
First, we validate the performance of the time-varying formation that is obtained from (19) by setting v d (t) = 0. All the robots start at rest, i.e., v i (0) = 0, and the initial position of each agent is chosen as The initial conditions for the Luenberger observer states were set asp i (0) =v i (0) = 0 for all i ∈ N . On the other hand, the initial condition for the auxiliary state ϕ i was set as ϕ i (0) = 0 for all i ∈ N . The control and observer gains are, respectively, chosen as K = Γ = 5I ∈ 12×12 and Ξ i = [ 6I 5I ] ∈ 4×2 , while the coupling strength is set as c = 2.
To achieve the desired time-varying formation, we use the following time In this simulation, the communication graph is a directed ring and its corresponding Laplacian matrix is given by To obtain a better insight into the performance of the Luenberger observer, we compute the norm of the velocity errors given The time evolution of ||e v x || and ||e v y || is shown in Figure 3. Clearly, both quantities converge to zero exponentially; therefore, the estimated velocity converges to the actual velocity of the robots.
In Figure 4, we show that the the time-varying formation is achieved, due to the fact that p x ij (t) − δ x ij (t) and p y ij (t) − δ y ij (t) converge exponentially to zero after the transient response.
The estimated velocity is shown in Figure 5. In the figure, all robots' estimated velocities converge to the rate of change of the formation given byδ i (t) after a few seconds. From the results of Figures 4 and 5, we can conclude that the formation objective is achieved.   Figure 6 shows the robots' trajectory in the x − y plane. As can be seen for t < 6, the agents move from the initial position to achieve the formation in blue dotted lines; subsequently, for t > 6, the agents move to a line formation, in which they remain for the rest of the time.

Flocking Control
The objective of the second simulation is to validate the performance of the flocking controller (19) together with the observer (18). The desired velocity profile is proposed as It is important to point out that the first derivative of the desired velocity profilev d (t) → 0 as t → ∞; hence, the condition in Proposition 3 is fulfilled. On the other hand, the initial conditions, the control and observer gains, the time-varying formation δ i (t) and the communication graph are the same as in the first simulation. Before performing the tests to validate the flocking control, we carried out a simulation to assess the performance of the distributed observer (24). Note that the distributed observer (24) does not depend on the states of the robot network. We consider that only robots 1 and 5 know the desired velocity profile; thus, the parameter d i is set as d 1 , d 5 = 1 and d 2,3,4,6 = 0. This means that the rest of the robots will have to recover the information through the distributed observer. The initial condition and observer gain for the distributed observer were chosen asv d i (0) = 0 for all i ∈ N and µ = 10. Figure 7 shows the trajectories obtained with the distributed observer (24). As can be observed, all the estimated states converge to the desired velocity profile in both coordinates; see Figure 7a,b. With this result, we can say that the estimator is working well; therefore, it can be used in combination with the observer-based flocking approach described in Section 4.
According to the definition of flocking given in Section 2, this behavior is achieved if all the robots' velocities follow the desired flocking velocity plus the rate of change of the formation and the robots maintain a safe distance from one another and form a desired geometric pattern. The velocity matching condition is shown in Figure 8, where all the estimated velocities of the robots converge to v d (t) +δ i (t). The second flocking condition is depicted in Figure 9, where clearly the distance p ij (t) − δ ij (t) converges to zero, meaning that the robots achieve the desired formation. It is important to highlight the good performance of the observer-based flocking algorithm considering that only the position of the robots is obtained from sensor measurements and only two robots know the the desired flocking velocity profile.   Figure 10 shows the trajectory of the robot network in the plane, which is similar to the flocking behavior observed in birds-that is, while maintaining a time-varying formation, all the robots move with the same speed. With this result and the ones presented in Figures 8 and 9, we can conclude that the proposed flocking control law is able to emulate the flocking behavior.

Conclusions
In this work, we study the problem of designing control laws that achieve timevarying formation and flocking in robot networks where each robot is modeled as a double integrator system. To solve this problem, we adopt a hierarchical control strategy that allows us to design a decentralized dynamic controller able to achieve both collective behaviors. For instance, if time-varying formation is pursued, we only need to set the desired flocking velocity to zero. An important advantage of the proposed controller is that for the robot i, the control input u i does not require the linear velocities of its neighbors; it only requires the relative position of the neighbors and its own velocity.
Since, in practice, it is not always possible to measure all the states of the system, a Luenberger observer is proposed to estimate the robot's velocity. The hierarchical control approach allows us to easily combine the formation and flocking control law with the observer, and the stability analysis is very straightforward. In addition to the velocity observer, we design a distributed observer that estimates the desired flocking velocity for those robots that do not have access to the desired velocity profile.
To model the communication between agents, the theory of graphs is employed. The proposed controller works for either undirected or directed graphs. As a result, it is possible to avoid the use of the leader-follower approach, which is most used in these types of works; the only condition is that the graph must be connected and strongly connected for the distributed observer. To validate the presented theory, numerical results are shown that allow us to conclude to that the proposed control law is able to emulate the collective behavior under study; in the case of the distributed observer, the obtained results were good, considering that only two agents knew the desired velocity profile. Finally, as future research, we will study the case of collision avoidance, so as to later implement the method presented in this work in some unmanned vehicles.