Edge-weighted consensus-based formation control strategy with collision avoidance

SUMMARY In this paper, a consensus-based control strategy is presented to gather formation for a group of differential-wheeled robots. The formation shape and the avoidance of collisions between robots are obtained by exploiting the properties of weighted graphs. Since mobile robots are supposed to move in unknown environments, the presented approach to multi-robot coordination has been extended in order to include obstacle avoidance. The effectiveness of the proposed control strategy has been demonstrated by means of analytical proofs. Moreover, results of simulations and experiments on real robots are provided for validation purposes.

Since in this paper only undirected graphs will be considered, it follows that the elements of E are unordered pairs of elements, namely: The Neighbors subset of the ith agent is defined as Each agent is supposed to be able to exchange data with its neighbors, that is, with all the agents that are in its Neighbors subset.
Given a graph G = (V , E) and an orientation map defined over the edge set, the Incidence matrix, I ∈ R N×M , can be defined as: where M is the cardinality of the edge set and ε k is the kth edge of G. When the orientation map is not defined, a random orientation can be chosen. From the definition of the Incidence matrix, a definition of the Laplacian matrix follows, which highlights the edges present in a graph: The Laplacian matrix has some following remarkable properties: 22 1. L is positive semi-definite.
2. The eigenvalues of the Laplacian matrix are always non-negative. Moreover, they can always be ordered as follows: For further details on algebraic graph theory, the reader is referred to Royle and Godsil. 22

Background on consensus problem
The consensus problem 23 is a well-known and widely studied problem in the field of decentralized control. It starts from considering all the agents as holonomic kinematic models: where x i ∈ R is the state of the ith agent. The consensus problem for N agents, whose goal is to drive the whole system to a final common state, can be solved, in a completely decentralized way, with the Laplacian-based feedback method. The feedback control is in the forṁ where x = [x 1 , . . . , x N ] T , and w ij (x) are positive edge weight functions. It is worth noting that the edge weights w ij used in this approach are only function of x i and x j , thus implementing a fully decentralized algorithm. However, the edge weight function will be referred to as w ij (x) for ease of notation. Let the Weight matrix W (z) be defined as follows: Furthermore, let the weighted Laplacian matrix be defined as follows: The control law in Eq. (4) can be rewritten in the following matrix form: 16 For further details of consensus problem, the reader is referred to ref. [23]. So far, only scalar states have been taken into account. Consider now the position of each agent as its own state. More specifically, if the position of the ith agent is n-dimensional, the ith agent's state is given by x i = [x i,1 , . . . , x i,n ] T . Considering N agents, the state of a multi-agent system may be defined as However, for the sake of clarity, the dynamics of the system will be hereafter considered in one dimension only, without loss of generality. All the results presented, in fact, may be easily extended to the n-dimensional case.

Control Law for Formation Achieving
The consensus-based formation control algorithm introduced in ref. [8] exploits the following control law:ẋ where b is defined as a bias vector. Letx be the vector that contains the desired positions for the agents with respect to the centroid of the formation: the bias vector is defined as b = −Lx. One of the main drawbacks of the control law in Eq. (8) is that the definition of the formation by means of the desired positions described by the vectorx does not allow the formation to rotate and adapt its shape to avoid collisions while moving in unknown environments. In fact, the vectorx defines the positions that each agent have to match in each dimension with respect to the centroid of the group, 8 and thus it must be recalculated for each of these with respect to a world fixed frame. Alternatively, it will be shown that given an appropriate choice of edge-weights w ij (x), ∀ (v i , v j ) ∈ E, it is possible to obtain the desired formation without using a bias in the control law. In fact, the weight functions may be opportunely defined in order to make the inter-robot distances converge to a desired values, thus creating the desired formation. Furthermore, in the control law presented in this paper, the weight functions can be chosen such that collisions among the agents are always avoided. More specifically, it will be proven that, given a safety distance δ, if the initial configuration of the system is such that all the inter-agent distances are strictly greater than δ, then they will never go below this value. In ref. [16] edge-weight functions w ij (x) were designed for ensuring the preservation of the connectivity of the graph, defining the control law that prevented edges of the communication graphs from being disconnected. In this paper, inspired by Ji and Egerstedt, 16 the edge-weight functions are designed with the purpose of obtaining the desired formation while avoiding inter-robot collisions. This objective is obtained exploiting the control law introduced in Eq. (4).
For this purpose, let l ij be the edge vector between agents i and j , i.e. l ij = x i − x j . Furthermore, define a collision-free realization of graph G as for some positive , where D M is the maximum allowed distance that guarantees connectivity between the agents, and δ is the safety distance, that is, the minimum distance that guarantees collision avoidance.
Then an edge-tension function V ij is defined as follows ( Fig. 1(a)): where K ij > 0 is a constant, α ij > 0 is a value used to define the intensity of the inter-robot influence, 20 and V min ij > 0 is defined such that It is worth noting that, according to its definition in Eq. (10), V ij is not a function of the entire state x.
In fact, since l ij = x i − x j , V ij is a function of only x i and x j . However, it will be hereafter referred to as V ij (δ, x) for ease of notation. This function is non-negative, and has a strict minimum in l ij = D ij , with where acsch (·) is the inverse function of csch (·). The choice of the value of the constant K ij > 0 is related to the position of the minimum of the edge-tension function, i.e. the desired distance for each couple of agents. From Eq. (10), it follows that The total tension energy of graph G can then be defined as follows: Moreover, the edge-weights are defined as follows ( Fig. 1(b)): For ease of notation, hereafter α ij will be assumed to be equal to 1, ∀i, j . Nevertheless, all the following proofs still hold for arbitrary values of α ij > 0.

Proposition 1.
Given an initial position x 0 ∈ D G,δ , for some > 0, if the system is driven by the control law in Eq. (4), with the edge-weights defined in Eq. (15), the total tension energy of graph G defined in Eq. (14) does not increase.
Proof. From Eqs. (4), (13), (15), the control law of the system can be rewritten as follows: Assume that, at time τ , x (τ ) ∈ D G,δ for some > 0. The total tension energy of the graph V (δ, x), defined in Eq. (14), is a positive function, and is zero only in the desired configuration, i.e. l ij = D ij ∀l ij ∈ E. The time derivative of the total tension energy function is defined as follows: Thus, for any x (τ ) ∈ D G,δ ,V (δ, x (τ )) is non-positive, which proves the statement.
The total tension energy function V (δ, x) has been defined in Eq. (14) as the sum of non-negative functions V ij (δ, x) described in Eq. (10). Since these functions are clearly equal to zero only when the agents are in the desired configuration, it is possible to conclude that the desired formation is a global minimum for the total tension energy V (δ, x). However, it is possible for the system to evolve to some local minima of the total tension energy. In order to avoid local minima, the Virtual Relabeling algorithm, that will be described in Section 5, may be exploited.
In order to ensure that the presented control algorithm avoids collisions between agents achieving formation, the following proposition is provided.

Proposition 2.
Given an initial position x 0 ∈ D G,δ , for some > 0, under the control law in Eq. (4), with the edge-weights defined in Eq. (15), collisions among the agents are always avoided.
Proof. Let δ be the safety distance for the agents, i.e. if the distance between each couple of agents is greater than δ, collisions are avoided. To prove the statement, it will be shown that as V (δ, x (τ )) decreases (or at least does not increase), no edge-length will approach δ. To this purpose, definê Since the function V (δ, x) is bounded inside the set D G,δ , this maximum exists. Let M 1 ≥ 0 be the number of edges whose length is less than D ij , and let M 2 ≥ 0 be the number of edges whose length is greater than or equal to D ij . Thus, the maximumV is obtained when M 1 edge-lengths are equal to the minimum allowed length (i.e. l ij = (δ + )) and M 2 edge-lengths are equal to the maximum allowed length (i.e. l ij = D M ). Hence: where K * and V min * are the mean values of K ij and V min ij . A bound will now be defined for the minimal edge-length that can generate this value for the total tension energy. Consider this total tension energy as if it were generated from one single edge, whose edge-length isl ≤ (δ + ), while all the other edge-lengths are equal to D ij , i.e. their contribution to the total tension energy is zero. The edge-lengthl is defined such that where V min > 0. To prove the statement, it is necessary to prove thatl > δ. Substituting Eq. (20) into Eq. (19): From the definition of the edge-tension function in Eq. (10), it follows: Since Hence, it is possible to conclude that (l − δ) > 0, which impliesl > δ and, sincel is bounded by δ, as V decreases (or at least does not increase), no edge-length tends to δ.
It is important to stress out that Proposition 2 holds for arbitrarily values of D M > δ > 0. Hence, the value of D M may be chosen bigger than the radius of the environment where the agents move. To deal with limited communication ranges, this control strategy can be extended, modifying the edge-weight function in Eq. (15) as described in ref. [16].

Obstacle Avoidance
One of the main issues that arises when robots have to be coordinated in an unstructured or unknown environment is that they have to take into account the presence of obstacles. The problem of obstacle avoidance has been widely investigated in the literature and typical solutions rely on the possibility of avoiding collisions with environmental obstacles of unknown position by exploiting many different approaches, such as the bug algorithm, 24, 25 artificial potential fields, 26 or vector field histogram. 27,28 Even though such approaches have demonstrated their reliability, the purpose of the paper is to define an obstacle avoidance algorithm that can be included in the consensus-based framework presented so far. For this purpose, consider, without loss of generality, the case where a robot detects an obstacle at a distance d o ≤ d sens , where d sens represents the effective range of on-board sensors able to detect obstacles all around the robot. In that case, as described in Olfati-Saber, 18 a virtual agent is projected on the obstacle by the robot that detects it (see Fig. 2). While in Olfati-Saber 18 artificial potential fields are used for collision avoidance purposes, in this paper the virtual agents (i.e. the obstacles) are included into the previously described graph-based algorithm. In particular, the Laplacian-based algorithm used to achieve formation can be extended simply by introducing a new virtual edge to the connectivity graph to represent the link between the real robot and the virtual agent.
For the sake of clarity, suppose that the number of virtual agents detected by the robots of the swarm is N o . The new graph G = (V , E ) can be considered an extended version of the original one, thus all the sets used to describe graph G must be redefined in order to take into account the virtual edges. The vertex set and the edge set can be redefined as V := V V o and E := E E o respectively, where: is the set of the edges connecting a real agent with a virtual one. As a consequence, also the incidence and weight matrices are modified to include virtual agents. More specifically: It is worth noting that, as shown in Fig. 2, the number of virtual agents can be different with respect to the number of detected obstacles. In fact, as long as robots are moving in an unknown environment, they cannot distinguish an obstacle from another one. This means that different robots detecting the same object will project on it different virtual agents.

Assumption 1. The distance between each couple of obstacles is supposed to be greater then the size of a robot.
This assumption is necessary to ensure that the obstacles can be overcome by the robots. Otherwise, the swarm can get stuck due to narrow gates. Path planning, which can solve such problems, is beyond the focus of this paper.
Without loss of generality, consider the case where, while N agents are moving in the environment, the pth one senses an obstacle. In this case, the dynamics of the agents that do not sense the obstacle are not influenced directly by its presence. On the contrary, the pth agent defines a virtual agent, whose position corresponds to the position of the obstacle. The dynamics of the virtual agent can be generically described asẋ However, as the virtual agent slides on the surface of an obstacle whose shape is not known, it is not possible to define the functions f o (·) in advance. Furthermore, if the obstacle is not static (e.g. the obstacle is a noncooperative vehicle), its law of motion is supposed to be unknown. As a consequence, the only way to ensure that collisions with obstacles are avoided is to exploit the following proposition. Proof. To include the virtual agent defined once an obstacle is sensed, the total tension energy of the graph G defined in Eq. (14) can be modified as follows: where V pv (δ o , x) is the edge-tension function related to the edge between the pth real agent and the vth virtual agent, and δ o is the safety distance required between robots and obstacles. Namely: where l pv is the edge vector between the pth real agent and the vth virtual agent, i.e. l pv = x p − x v , and α pv > 0 is a constant value that can be used to modulate the intensity of the interactions between real and virtual agents. Thus, to prove the avoidance of collisions, Proposition 2 can be applied with this modified total tension energy function.
Using the same approach, obstacle avoidance can be ensured even in the presence of more than one obstacle sensed by more than one agent (i.e. many virtual agents are added to the graph). Similar to what has been described in Section 3, the choice of the value of the constant K v > 0 is related to the length of the edge between the real and virtual agents for which the edge-weight function is equal to zero. A good choice is to set this distance to the value of the sensing range. In other words, when an agent senses an obstacle and defines a virtual agent, the corresponding edge-weight is always negative, thus always introducing a repulsive action.
It is worth noting that, in the presence of virtual agents, since their movement is not influenced by the position of the real robots, the graph becomes directed. This causes that Proposition 1 does not hold when virtual agents are added to the graph and, as a consequence, this implies that the shape of the formation is not preserved in the presence of obstacles. However, the multi-robot system is not supposed to embed virtual agents inside the formation: virtual agents are introduced only for collision avoidance purposes. Hence, robots will overcome the obstacles without maintaining the shape of the formation, i.e. by performing split-and-rejoin maneuvers, or by reducing the inter-robot distances. Examples of these maneuvers will be shown in the simulations described in Section 6.

Local Minima Avoidance
The following section describes an algorithm to let the robots autonomously escape from local minima of the total tension energy function V (δ, x), introduced in Eq. (14).

Virtual relabeling
As demonstrated in Proposition 1, the desired formation configuration is the global minimum of the total tension energy function V (δ, x) introduced in Eq. (6). However, it is possible for the system to evolve to some local minimum configuration. To make the robots escape from local minima, the virtual relabeling algorithm may be exploited.
As long as the communication graph is connected, all the robots can calculate the position of the centroid of the group. Then, by computing their own position with respect to the centroid of the group, they can find an agreement on the position they should occupy in the final formation. In order to do this, each robot needs to acquire information from all the other robots of the group. For this purpose, the data broadcasting algorithm, which will be introduced in Section 5.2, may be exploited. Therefore, the virtual relabeling algorithm, defined as in Table I, may implemented on each robot. More specifically: To better clarify the relabeling algorithm, in Fig. 3 a typical example of local minimum for a system involving four robots achieving a square formation is depicted. In order to show how the virtual relabeling algorithm can assure the convergence of the swarm of robots to the desired final configuration even in the case of local minima, consider the example reported in Figs. 4(a) and (b).
In the first case, the robots reach a local minimum configuration, i.e. according to Proposition 1, the robots reach a configuration where the total tension energy of graph G does not increase, but, due to the wrong order to the robots, is not null. In this case, in fact, the tension energy between each robot and its neighbors results to be null, thus the control law in Eq. (4) is null. Conversely, in the second case, once the virtual relabeling algorithm has been applied, the robots calculate the tension energy corresponding to the desired final formation, i.e. the total tension energy of the group is null if and only if the group reaches the desired formation. As reported, each robot calculates its own v1 vector depending on its position with respect to the centroid and teammates. Then, starting from v1, vector v2 is calculated to redefine the actual label that each robot should use to achieve the right configuration. In the depicted example, the robots of the group reach an agreement on v2 and each of them is able to detect that robots R 3 and R 4 are in the wrong position with respect to their teammates.

Data broadcasting
One of the main problems related with robots with a limited communication range is that they cannot always acquire information about the whole swarm, that is, some robots can not see each other. Hence, the virtual relabeling algorithm introduced in Section 5.1 cannot be implemented. However, as long as the communication graph is connected, this problem may be overcome by introducing data broadcasting between teammates, i.e. by allowing each robot to transmit information about the relative position of connected teammates with respect to itself, thus transforming de facto a connected communication graph into a complete graph. As an example, consider the three robots depicted in Fig. 5. The corresponding Neighbors subsets are defined as Generally speaking, given a couple of communicating robots, it is possible to define x ij = (x i − x j ) and y ij = (y i − y j ) as the relative distance between robots i and j on the x-axis and y-axis respectively. Thus, the ith robot can estimate the relative position of the hth robot by    exploiting the following equations: hj .
In order to reduce the measurement errors, the data broadcast by each robot is grouped into a data packet containing the sender ID and the relative position of their neighbors with respect to itself. Each of the transmitted data are associated with a hop count c ij (∀i = 1 . . . N, j ∈ N i ) that is incremented each time a robot broadcasts position data that are not directly measured, thus allowing the receiver to chose the data with the lowest hop count. The general scheme of the string transmitted by the generic j th robot of a swarm is the 4 N j + 2 elements vector defined as: Vector element: Vector index: where N j is the cardinality of the neighbors subset and terms 3-6 are repeated ∀i ∈ N j . As an example, by considering the system depicted in Fig. 5, the data transmitted by R j are stored in the following vector: It is worth noting that the data broadcasting algorithm may be exploited to improve the obstacle avoidance ability of the system as well. In fact, each robot may also broadcast the position of the virtual agents defined when an obstacle is detected, as described in Section 4.
The data broadcasting algorithm requires the communication graph to be connected. In order to ensure connectivity of the communication graph, several strategies may be exploited (see e.g refs. [16,[29][30][31][32][33], and references therein).

Simulations and Experiments
To validate the control strategy presented so far, several simulations and experiments have been implemented on differential-drive robots, described by the following kinematic model To deal with the fact that this model represents a non-holonomic system, the feedback linearization technique presented in ref. [34] has been considered. Moreover, to make the formation move in a desired direction, a common offset has been added to the control law in Eq. (4) to describe the desired speed of the barycenter of the formation. A video-clip that describes both simulations and experiments can be viewed on the web. 1

Simulations
Simulations are performed using Matlab/Simulink. As an example, six robots are simulated while achieving a formation with 1.5-m radius, moving in an environment where three round obstacles are placed on their trajectory. To emphasize the different behaviors that come out by changing the value of α ij , two different simulations are reported in Figs. 6(a) and (b): In the first one, where the inter-robot action is modulated by α ij = 1, robots manifest a flexible behavior when obstacles are encountered. In the second one, where α ij = 10, the formation is too rigid to be able to split in order to overcome the obstacles, thus the formation preserves its shape while sliding over them. Figure 6(c) shows the mean square error (MSE) between the actual and desired distances for each pair of robots. It is worth noting that, for time 2.5 s ≤ t ≤ 6 s, i.e. when obstacles are encountered, the MSE value is high if α ij = 1 and is approximatively 0 when α ij = 10.     7. Error between the actual and the desired distances between each robot and the centroid of the formation exploiting potentials introduced in ref. [8] and the consensus-based algorithm introduced in this paper, considering a communication delay of 0.5 s.

Communication delay
To better enlighten the behavior of the system in the presence of communication delay, some other simulations involving only three robots achieving regular formation have been performed.
More specifically, simulations show three robots moving in an obstacle-free environment. To compare the algorithm with a potential field-based algorithm, the control law introduced in ref. [8] has been implemented first, adding an artificial potential field for collision avoidance, as described in ref. [9]. Simulations show that, as proven in ref. [15], artificial potential fields are not robust with respect to communication delays. In fact, Fig. 6.1 shows the error between the actual and desired distances between each robot and the centroid of the formation, with a communication delay of 0.5 s. As expected, the system does not converge to the desired formation.
Conversely, simulations show that the control law introduced in this paper is robust with respect to communication delays, as expected for consensus-based control laws. 9 Figure 7(b) shows the error between the actual and desired distances between each robot and the centroid of the formation, with a communication delay of 0.5 s. As expected, the error quickly converges to zero.

Experimental results
Experiments are performed within the software platform described in ref. [35]. A group of four E-Puck robots 36 are moving in a 2.0 × 1.5 m arena. The position of each robot is monitored with a webcam positioned over the arena, and the robots are tracked using colored markers. Hence, position data are collected in a centralized manner. However, the algorithm has been implemented in a decentralized way, simulating the presence of limited range communication. More specifically, the communication range was set to R = 0.3 m: each robot was then allowed to exchange data only with its neighbors, that is, robots whose distance was less than R.
The size and position of the obstacles was known in advance to the central computation unit. However, the presence of exteroceptive sensors for obstacle detection was emulated: the position of each obstacle was available to each robot only when the distance between an obstacle and the robot itself was less than the sensing range, R = 0.3 m.
Two different experimental setups have been used to test the algorithm. In the first setup, four robots starting from random positions converge to the desired square formation while avoiding collisions, and after 20 s they start moving along the x-axis with a constant speed.
In the second setup, an obstacle is placed in the middle of the arena in a position unknown to the system, thus robots have to overcome it while moving in formation along the x-axis. This setup has been used also to test different behaviors of the system in case of α ij = 1 and 10.
For each setup, data have been collected over 10 experimental runs. The average MSE between the actual and desired distances for each pair of robots are represented in Figs. 8(a) and (b) respectively. In particular, in the second setup, it can be seen that at time ≈35 s the formation detects the obstacle, thus the variance increases.

Conclusions
This paper described a consensus-based algorithm for formation control of groups of mobile robots. By means of the definition of appropriate edge-weight functions, formation control is achieved, as well as inter-robot collision avoidance. The convergence of the system to the desired configuration has been analytically proven, exploiting tools from the graph theory. Moreover, by defining virtual agents projected on the detected unknown objects, the same framework has been exploited to perform obstacle avoidance. The proposed control algorithm has been validated by means of extensive Matlab/Simulink simulations and experiments conducted on real robots moving in an unknown environment.
Future works will aim at explicitly including connectivity maintenance strategies in order to relax requirement on the connectedness of communication graph.