Controlling Rigid Formations of Mobile Agents Under Inconsistent Measurements

Abstract

Despite the great success of using gradient-based controllers to stabilize rigid formations of autonomous agents in the past years, surprising yet intriguing undesirable collective motions have been reported recently, when inconsistent measurements are used in the agents' local controllers. To make the existing gradient control robust against such measurement inconsistency, we exploit local estimators following the well-known internal model principle for robust output regulation control. The new estimator-based gradient control is still distributed in nature, and can be constructed systematically even when the number of agents in a rigid formation grows. We prove rigorously that the proposed control is able to guarantee exponential convergence, and then demonstrate through robotic experiments and computer simulations that the reported inconsistency-induced orbits of collective movements are effectively eliminated.

Full text

IEEE TRANSACTIONS ON ROBOTICS 1
Controlling rigid formations of mobile agents under
inconsistent measurements
Hector Garcia de Marina, Student Member, IEEE, Ming Cao, Member, IEEE, and Bayu Jayawardhana, Senior
Member, IEEE
Abstract—Despite the great success of using gradient-based
controllers to stabilize rigid formations of autonomous agents in
the past years, surprising yet intriguing undesirable collective
motions have been reported recently when inconsistent measure-
ments are used in the agents’ local controllers. To make the
existing gradient control robust against such measurement in-
consistency, we exploit local estimators following the well known
internal model principle for robust output regulation control. The
new estimator-based gradient control is still distributed in nature
and can be constructed systematically even when the number of
agents in a rigid formation grows. We prove rigorously that the
proposed control is able to guarantee exponential convergence
and then demonstrate through robotic experiments and computer
simulations that the reported inconsistency-induced orbits of
collective movements are effectively eliminated.
Index Terms—Formation Control, Distributed Control, Dis-
tributed Calibration
I. INTRODUCTION
TEAMS of autonomous robots that work cooperatively
are used more and more widely for a range of robotic
tasks [1], [2]. Robots have been deployed in formations with
different shapes in order to facilitate the adaptive sampling of
an unknown environment [3] or to achieve better cooperation
efficiency [4]. As a result, considerable research efforts have
been made in the past few years on designing distributed
control laws to stabilize the shapes of formations of au-
tonomous agents [5]–[8]. In particular, within the research
area of developing cooperative control theory for multi-agent
systems, a sequence of theoretical investigations have been
made to design formation control laws using the notion of
graph rigidity [9]–[12], and such control laws are usually
based on the gradients of the potential functions closely related
to the graphs describing the distance constraints between the
neighboring agents.
However, it has been recently reported in [13], [14] that
for such gradient control laws, if agents disagree with their
neighboring peers on the measured or prescribed distances
between them, undesirable formation motion might appear.
Surprisingly, such inconsistency induced motions take peculiar
forms: in IR2, the agents move collectively in a distorted but
rigid formation following a closed orbit that is determined by
a single sinusoidal signal; in IR3, the orbit becomes helical
The authors are with the Institute of Technology, Engineering and Man-
agement, University of Groningen, 9747 AG Groningen, The Netherlands.
(e-mail: {h.j.de.marina, m.cao, b.jayawardhana}.rug.nl). This work was sup-
ported by the the EU INTERREG program under the auspices of the
SMARTBOT project and the work of Cao was also supported by the European
Research Council (ERC-StG-307207).
that is determined by a single sinusoidal signal and a constant
drift. This is rather unexpected especially when knowing the
robustness as a consequence of the exponential convergence
of gradient control; after all, exponential convergence of a
dynamical system usually implies its robustness against small
disturbances. With the hindsight gained from [13], [14], one
realizes that the exponential convergence takes place for the
error signals determined by the differences of the real and
prescribed distances between neighboring agents, but this does
not prevent the ill behavior of the position or velocity signals
of the agents when measurement inconsistency exists. Such
an observation is by no means trivial, but may affect the
application of robotic formations because robustness issue is
particularly relevant in practice, where distance disagreements
may arise for several reasons. Firstly, robots may have different
guidance systems, which may differ in their setting points;
secondly, sensors equipped on robots may not return the
same reading even if they are measuring the same distance
due to heterogeneity in manufacturing processes; and thirdly,
the same sensor can produce different readings for the same
distance in face of random measurement noises.
In this paper, we focus on dealing with this tricky robustness
issue by proposing to use an estimator-based gradient control.
We are able to show that under mild assumptions, the pro-
posed control strategy stabilizes formations in the presence
of measurement inconsistency eliminating all the reported
undesirable steady-state collective motions and distortion in
the formations’ final shapes. It takes full advantage of the
strength of the existing gradient control, especially the expo-
nential convergence speed, and at the same time preserves the
distributed nature of the local cooperative control laws. We
have discussed similar ideas in [15] to install simple local
estimators at the chosen estimating agents. We study more
advanced estimator-based control in this paper that avoids the
possible high gains in control and handles a much broader
class of measurement inconsistency. This inconsistency is in
the form of a combination of a constant bias and a finite
number of sinusoidal noise, which arises often in marine
robotic tasks when sea waves perturb sensing [16] [17].
The rest of the paper is organized as follows. In Section II
we describe the formation control problem for rigid formations
in IR2and IR3and the robustness issues associated with
gradient formation control. The estimator-based gradient con-
trol is proposed in Section III following the well established
internal model principle rooted in robust control. In Section
IV we carry out stability analysis and discuss in detail how to
choose estimating agents systematically in Section V. Finally,
arXiv:1609.06435v1 [cs.RO] 21 Sep 2016

IEEE TRANSACTIONS ON ROBOTICS 2
in Section VI experimental results are demonstrated using
wheeled mobile robots moving in the plane and simulation
results are discussed for mobile agents maneuvering in the
three dimensional space.
II. RIGID FORMATIONS
We consider a formation in IRm,m= 2 or 3, consisting of
n≥2autonomous agents labeled by 1, . . . , n, whose neighbor
relationships are described by an undirected graph Gwith the
vertex set V={1, . . . , n}and the edge set E ⊆ V × V. We
use |E| to denote the number of edges of G. Let kij denote
the ordered pair (i, j)to label the edge between vertices iand
j, and thus kij 6=kji . Let Eidenote the set of the labels in the
form of kij of all the edges associated with vertex i. To keep a
desired shape of the formation, each agent iis assigned with
the task of keeping some prescribed distance dkij to every
neighbor j. We assume that such distance constraints dkij are
realizable in IRm.
Corresponding to the formation, Gis embedded in IRmby
assigning to each vertex ia Cartesian coordinate xi∈IRm.
Aframework is a pair (G, x), where x= (xT
1, . . . , xT
n)Tis a
multi-point in IRmn. For every framework (G, x), we define
the edge function fG: IRmn →IR|E| by
fG(x) = col
∀kij ∈E(||zkij ||2),
where col(·)defines the column vector by collecting all its
arguments as the vector’s components, zkij =xi−xjis the
relative position vector between vertices iand jfor the edge
kij in the framework and || · || denotes the Euclidean norm.
In order to define rigidity formations, we first review some
basic notions on rigidity.
Definition 2.1: [18] A framework (G, x)is locally rigid if
for every x∈IRmn there exists a neighborhood Xof xsuch
that f−1
G(fG(x)) ∩ X =f−1
K(fK(x)) ∩ X, where Kis the
complete graph with the same vertex set Vof G.
Definition 2.2: [18] A framework (G, x)is globally rigid if
f−1
G(fG(x)) = f−1
K(fK(x)).
Roughly speaking a framework (G, x)is rigid if it is not
possible to smoothly move some vertices of the framework
without moving the rest while maintaining the edge lengths
specified by fG(x). If this property holds only locally in the
neighborhood Xof x, then the framework is only locally rigid;
otherwise, if the property holds for the whole space, then the
framework is globally rigid. Most of the existing literature has
focused on a special class of rigid frameworks. We need some
more definitions to introduce such frameworks.
Let us take the following approximation of fG(x)
fG(x+δx) = fG(x) + dfG(x)δx +O(δx2),
where dfG(x)denotes the Jacobian matrix of fG(x)and δx is
an infinitesimal displacement of x. The matrix dfG(x)is then
called the rigidity matrix of the framework (G, x).
Definition 2.3: [18] A framework (G, x)is infinitesimally
rigid if rank dfG(x)=2n−3in IR2or rank dfG(x)=3n−6
in IR3.
Roughly speaking, an infinitesimally rigid framework (G, x)
only admits rotations and translations of the whole framework
in order to satisfy fG(x+δx) = fG(x). The edge function
remains constant up to the first order when δx belongs to the
kernel of dfG(x).
Note that an infinitesimally rigid framework is also rigid,
but in general the converse is not true. In order to state whether
both frameworks are equivalent, we need to introduce the
concept of regular points.
Definition 2.4: [18] A multi-point xis a regular point of
(G, x)if
rank dfG(x) = max{rank dfG(x)|x∈IRmn}.
Theorem 2.5: [18] A framework (G, x)is infinitesimally
rigid if and only if (G, x)is rigid and xis a regular point.
For an infinitesimally rigid framework (G, x)that is em-
bedded in IR2, it has at least 2n−3edges. If it has exactly
2n−3edges, then the framework is called minimally rigid. For
an infinitesimally rigid framework (G, x)that is embedded in
IR3, if it has exactly 3n−6edges then the framework is also
called minimally rigid.
It is shown by Henneberg [9] that the 2D minimally rigid
graphs on two or more vertices are exactly the graphs that
can be obtained, starting from a single edge, by a sequence of
operations of the following two types:
1) Add a new vertex to the graph, together with edges
connecting it to two previously existing vertices.
2) Subdivide an edge of the graph, and add an edge
connecting the newly formed vertex to a third previously
existing vertex.
The first operation is referred to as the Henneberg insertion
operation.
In the next section, we discuss gradient control for rigid
formations.
III. GRA DI EN T CO NT ROL A ND I TS ROBU ST NE SS I SS UE
Assume that agent i’s motion is described by a first-order
kinematic point model
˙xi=ui, i = 1, . . . , n, (1)
where ui∈IRmis the control input for the agent i.
In [10], an elegant distributed control law has been pre-
sented utilizing zkij
ui=−X
kij ∈Ei
zkij ekij ,(2)
where ekij is the error between the square of the real distance
¯
dkij and the square of the prescribed distances dkij between
the two agents iand jassociated with edge kij
ekij =||¯
dkij ||2−d2
kij .(3)
It has been shown in [10] that when ¯
dkij =¯
dkji and dkij =
dkji , control law (2) causes the solution of the closed-loop
n-agent system to follow the direction of the gradient of the
system’s potential function 1
4Pkij ∈E e2
kij . Consequently, it is
convenient to show that the errors ekij converge exponentially
to zero when the formation is minimally rigid. For this reason,
a number of research groups have applied this gradient-based

IEEE TRANSACTIONS ON ROBOTICS 3
control law to a range of formation control problems under
different settings [11], [12], [19], [20].
However, more recently, intriguing robustness issues of the
gradient formation control have been reported in [13], [14]. For
two neighboring agents iand j, if there is some inconsistency
in their measured or prescribed distance between them, namely
¯
dkij 6=¯
dkji or dkij 6=dkji , and thus ekij 6=ekj i , the control
law (2) does not correspond to the gradient of the potential
function constructed in [10] anymore. Indeed, constant in-
consistency leads to two highly undesirable behaviors of the
formation [13], [14]:
1) Unknown distorted final shape. When the inconsistency
is small, the errors ekij converge to some unknown small
but non-zero values, and thus the shape of the formation
becomes distorted even as tgoes to infinity.
2) Steady-state collective motion induced by inconsistency.
In IR2, the agents move collectively in formation fol-
lowing a closed orbit that is determined by a single
sinusoidal signal; in IR3, the orbit becomes helical that is
determined by a single sinusoidal signal and a constant
drift.
Since measurement errors are ubiquitous in real robotic
applications, this robustness issue inherent to the structure of
gradient control poses urgent demand on designing new robust
control strategies which preserves the exponential convergence
property of gradient control and at the same time is robust
against measurement discrepancies. One can show that the
effects of ¯
dkij 6=¯
dkji and dkij 6=dkji are equivalent in causing
the undesired behavior just described. In this paper, to empha-
size the possible measurement errors, we focus on deriving our
system models for the case when ¯
dkij 6=¯
dkji while similar
analysis carries over to the case when dkij 6=dkji .
IV. EST IM ATOR -BASED GRADIENT CONTRO L
In this section, we present in detail how local estimators
can be designed for chosen agents, called estimating agents,
such that measurement inconsistencies can be compensated
distributively. Three main challenges are worth pointing out.
First, the estimators’ dynamics should not, if possible, affect
the exponential convergence that is associated with the gradi-
ent control. Second, compensation should be done locally and
different estimating agents should not give rise to conflicting
compensation goals. Third, the class of discrepancies should
be broad enough to contain at least the constant signals
discussed in [13], [14]. In view of these challenges, one soon
realizes that the design task is not easy at all. We have made
some preliminary effort along this line in [15], where the
estimator deals with only constant inconsistencies, and may
run into high control gains. In what follows, we propose
a novel estimator-based gradient control based on the well-
known internal model principle that has been used for solving
tracking and disturbance rejection problems [21], [22], and
more recently for cooperative control of multi-agents systems
[23].
A. Estimating agents
As we have discussed in the previous section, when there
are distance measurement discrepancies, we have ¯
dkij 6=¯
dkji
and thus ekij 6=ekji . We introduce the new variables µkfor
each edge k= 1,...,|E| such that
ekij =ekji −µk.(4)
Obviously, the definition of µkdistinguishes the two asso-
ciated agents iand jsince the indices iand jare not
exchangeable in (4). We call agent i, whose label is the leading
subscript for edge kon the left-hand side of (4), the estimating
agent for edge ksince we will design an estimator for agent
ito estimate µklater. Then for each edge k, there is only one
estimating agent associated with it. We will discuss in Section
VI how one chooses the estimating agents systematically. For
each agent i, we use Kito denote the set of the labels of the
edges for which agent iis chosen to be the estimating agent,
and then Ki⊂ Ei.
B. Modeling measurement inconsistency
We assume that the discrepancies µkare in the form of the
superposition of a constant signal and psinusoidal signals with
known frequencies {ω1, ω2, . . . , ωp}, namely
µk(t) = αk+
p
X
i=1
βisin(ωit+φi),(5)
where αk,βiand φiare fixed but unknown offset, amplitude
and phase respectively. This noise model is widely used for
formation control when the robots are known to work in the
environment with periodic background noises. For example,
short-term sea waves can be described by a superposition of
periodic waves whose frequencies can be accurately estimated
[17], and thus the measurement noise for underwater marine
vehicles using floating buoys [16] can be treated as the
superposition of a finite number of sinusoidal signals with
known frequencies.
C. Estimator-based control
We first propose the estimator-based gradient control. To
explain the reasoning of the construction of the specific form
of the estimator, we have to wait until we build up the state-
space model for the overall closed-loop system.
We propose to use the following distributed, estimator-
based, dynamic, gradient control
ui=−X
kij ∈Ei
zkij ekij +X
kij ∈Ki
zkij ˆµk,(6)
where the first term is the same as the gradient control in (2)
and the second term uses ˆµk∈IR which is agent i’s estimate
of the discrepancy µk. This estimator’s dynamics are described
by
˙
ξk= Λξk+κBk(ekij +µk−ˆµk)(7)
ˆµk=BT
kξk(8)
where ξk∈IR2p+1 is the state of estimator kand it can be
initialized arbitrarily,
Λ = 

0 0 0
0 0 −Ω
0 Ω 0

, Bk=b1
b2,(9)

IEEE TRANSACTIONS ON ROBOTICS 4
Ω = diag{ω1, ω2, . . . , ωp}, the constants b1∈IR and b2∈
IR2pare such that the pair (Bk,Λ) is observable, and κ > 0
is the gain to be designed.
Now the first-order agent dynamics (1), the estimator-based
gradient control (6) and the estimator dynamics (7) and (8)
define precisely the closed-loop dynamics of each agent. To
analyze the collective motion of the n-agent system, we need
to build the state-space model.
D. State-space model of the closed-loop system
Consider all the kij ∈ ∪Ki. Since for each edge in E,
there is only one estimating agent, we know that there are
exactly |E| such kij . We stack all the corresponding zkij ,
ekij ,µkand ˆµktogether into column vectors to obtain the
relative position, error, inconsistency and estimation vectors
z= col(zT
kij )∈IRm|E| ,e= col(ekij )∈IR|E| ,µ= col(µk)∈
IR|E| and ˆµ= col( ˆµk)∈IR|E|. Define the system’s state
x=xT
1·· · xT
nT∈IRmn. Then the n-agent system
dynamics derived from (1) and (6) are
˙x=−R(z)Te−ST
1(z)(µ−ˆµ),(10)
where R(z)∆
=dfG(x)is the rigidity matrix of graph G,S1=
ZTJ,Z=diag{zkij },Jis obtained by replacing all the −1
in H⊗Imby zero, being Hthe transpose of the incidence
matrix of G[24].
Now we are ready to present the state-space model for the
closed-loop n-agent system derived from, (7), (8) and (10).
Note that the error system can be easily computed from (10) as
˙e= 2R(z) ˙xas discussed in [15]. More precisely, the closed-
loop system can be written in the following compact form
P:˙e=−2R(z)R(z)Te−2R(z)S1(z)T(µ−ˆµ)
y=e
(11)
C:˙
ξ=Mξ +κB(y+µ−ˆµ)
ˆµ=BTξ(12)
W:˙w=Mw
µ=BTw,(13)
where ξ= col{ξk}is the state of P,M= diag{Λ,...,Λ},
B= diag{Bk}, and w= col{wk}is the state of the
exosystem Wwhose output is the discrepancy signal given
in (5). Despite the fact that the variable zappearing in (11) is
a function of x(which is not part of the state equation), it is
worth to mention that the terms R(z)R(z)Tand R(z)S1(z)T
can be expressed solely as a function of eas discussed in [25].
Hence the state equations (11)-(13) defines an autonomous
system. The initial estimates of the offset, phase and amplitude
of µkare encoded in the initial condition w(0). Note that the
estimating agents are measuring e+µas a whole, while the
unknown µappears in (12). The signal flow of the closed-loop
system is shown in the block diagram in Figure 1.
Using standard framework in robust output regulation prob-
lem, one can take the inconsistency µto be the disturbances
that directly influence the input and the output signals of the
plant P, and the controller Cmust contain internal models
that are copies of the exosystem W. One can check that in
W P
C
µe
−
ˆµ
Fig. 1: The plant Pcorresponds to the common error system
e, where its input and output are perturbed by the discrepancy
µgenerated by the exosystem Was in (13). The disturbance
rejection is achieved by using the internal controller Cthat is
a copy of the exosystem.
this case the Byrnes-Isidori regulator equation [26] is solvable
with the trivial solution ξ=wand e= 0.
After setting up the mathematical descriptions of the
estimator-based control and the corresponding state-space
system model, we are ready to show in the next section
that the n-agent system under measurement inconsistency is
exponentially stabilized by our proposed control.
V. STABILITY AN ALYS IS
In the previous section, we have designed a distributed
controller using the key idea of compensating the discrepancy
locally using internal-model-based estimators. Now we present
our main result showing the performances of the proposed
controller.
Theorem 5.1: For the closed-loop n-agent formation (11)
and (12) with the measurement inconsistency vector µdriven
by the exosystem (13) and unknown initial condition w(0) ∈
IR(1+2p)|E| , if the matrix
Z∆
=−R(z∗)R(z∗)T+R(z∗)S1(z∗)T(15)
is Hurwitz at the desired relative position z∗corresponding
to e= 0, then there exist κ∗>0and b∗>0such
that for any κ||Bk||2> κ∗and ||b2|| < b∗, the system
admits a locally exponentially attractive invariant manifold
Q∆
={(w, x, ξ)|e= 0, ξ =w}, and thus the shape of the
formation converges exponentially to the desired shape defined
by e= 0, the estimation ˆµconverges exponentially to µ
and the velocity ˙xconverges exponentially to zero (i.e. the
formation eventually stops).
Proof: We take the coordinate transformation α=e+
µ−BTξand θ=w−ξ, and then the equations (11)-(13)
can be rewritten into (14), where S2(z) = R(z)−S1(z). We
then calculate its Jacobian matrix Lat the equilibrium point
e= 0,α= 0 and θ= 0. Although the system matrix of (14)
is state dependent, several of its components are functions of
the inner products zT
izjand thus their partial derivatives can
be computed straightforwardly. In fact, the Jacobian matrix is
L=L1L2
L3L4,(16)

IEEE TRANSACTIONS ON ROBOTICS 5


˙e
˙α
˙
θ

=

−2R(z)S2(z)T−2R(z)S1(z)T0
−2R(z)S2(z)T−2R(z)S1(z)T−κBTB BTM
−κB 0M−κBBT


e
α
θ

(14)
where
L1=−2R(z∗)S2(z∗)T−2R(z∗)S1(z∗)T
−2R(z∗)S2(z∗)T−2R(z∗)S1(z∗)T−κBTB
L2=0
BTM, L3=−κB 0, L4=M−κBBT.
We now prove that L1is Hurwitz, and this is equivalent to
prove the system
˙e=−2R(z)S2(z)Te−2R(z)S1(z)Tα(17)
˙α=−2R(z)S2(z)Te−2R(z)S1(z)Tα−κBTBα (18)
is asymptotically stable at the origin e= 0 and α= 0 in which
case ˆµ=µ. Let f(e, α)∆
=−2R(z)S2(z)Te−2R(z)S1(z)Tα,
and we compute
F0
∆
=∂f (e, 0)
∂e |e=0 = 2Z, (19)
which is Hurwitz since Zis in view of the condition in the
theorem. Therefore, there exists a positive definite matrix P=
PTsuch that
FT
0P+P F0=−2I. (20)
Then for system (17)-(18) consider the candidate Lyapunov
function
V(e, α) = eTP e +1
2αTα, (21)
whose time derivative along the system’s solution is
˙
V(e, α)=2eTP f (e, α) + αTf(e, α)−κ||B||2||α||2
= 2eTP f (e, 0) + 2eTP(f(e, α)−f(e, 0)) +
+αTf(e, α)−κ||B||2||α||2
= 2eTP(F0e+g(e) + ¯
f(e)α) + αTf(e, α)
−κ||B||2||α||2,(22)
where g(e)is the Taylor-series residue that satisfies
lim
||e||→0||g(e)||
||e|| = 0,(23)
and ¯
f(e) = −2R(z)S1(z)T. In particular, (23) implies that for
any  > 0, there exists a δsuch that ||e|| ≤ δ=⇒ ||g(e)|| ≤
||e||. Taking =1
2||P|| with the corresponding δ, since ¯
f(e)
is locally Lipschitz, we know that for all ||e|| ≤ δ,||α|| ≤ δ,
there exist p, q > 0such that
˙
V(e, α)≤ −2||e||2+ 2||e||||P|| 1
2||P||||e|| +p||e||||α||+
+q||α||2−κ||Bk||2||α||2,(24)
where the third and fourth terms on the right-hand side are
due to the boundedness of ¯
f(e)in an open ball. Hence, by
choosing κ||B||2>0such that
q+p2
2−κ||B||2≤ −1
2,(25)
we have, in view of Young’s inequality, that
˙
V(e, α)≤ −1
2(||e||2+||α||2),(26)
and thus system (17)-(18) converges exponentially to the origin
for all the initial conditions e(0), α(0) starting in the set C:=
{x, ˆµ, µ |λmin(P)||e||2+1
2||e+µ−ˆµ||2≤δ2}. So we have
proved that L1is Hurwitz.
We further observe that L4is Hurwitz for any κ > 0, which
follows from the asymptotic stability of ˙
θ=L4θsince L4+
LT
4=−2κBBTand the pair (B, M )is observable. Thus, if
L2is zero, i.e. b2= 0 since BT
k=b10is a left eigenvector
for the single zero eigenvalue of Λ, then the eigenvalues of L
are the eigenvalues of L1and L4. Therefore, for a sufficiently
small Bsuch that 0<||b2|| < b∗,Lis still Hurwitz. Hence,
system (14) is locally exponentially stable, which implies that
(w, x, ξ)locally exponentially converges to Q. Since ˙x= 0
in the invariant manifold Q, we conclude also that ˙x(t)→0
exponentially as t→ ∞, i.e. the formation eventually stops.
Remark 5.2: For the sake of clarity, we have assumed that
Mis the same for all the inconsistencies µk. It can be checked
that the result in Theorem 5.1 still holds for having different
sets of frequencies for each inconsistency µk. Note that we
have not only removed all undesired effects induced by the
presence of inconsistency, but with the estimation of µ(t),
Theorem 5.1 provides a systematic method to calibrate the
offset of the sensors in the estimating agents with respect to
the sensors in the non-estimating agents.
In the next section, we explain how to choose the estimating
agents systematically to guarantee the conditions in Theorem
5.1 to hold.
VI. SE LE CT IN G TH E ES TIMATI NG AG EN TS
The condition of Zbeing Hurwitz in Theorem 5.1 is a
sufficient condition for the local exponential stability of system
(11)-(12). To check this condition, one needs to calculate the
eigenvalues of an |E| × |E| square matrix. Such computations
can be burdensome and in this section we are going to
show that for a large class of infinitesimally minimally rigid
formations one can still guarantee the admissibility of the
condition by choosing smartly the estimating agents and thus
avoid computing the eigenvalues.
A. Stabilizing a large class of infinitesimally minimally rigid
formations in IR2
In this subsection we study a class of infinitesimally mini-
mally rigid formations in IR2that are generated by a sequence
of Henneberg insertion operations starting from triangular
formations, for which we present two ways of picking the
estimating agents. Then we introduce a systematic way of

IEEE TRANSACTIONS ON ROBOTICS 6
Fig. 2: A triangular formation associated with a cyclic
estimating-agent graph where the tails of the arrows indicate
the corresponding estimating agents.
Fig. 3: A triangular formation associated with an acyclic
estimating-agent graph where the tails of the arrows indicate
the corresponding estimating agents.
choosing the estimating agents based on the Henneberg in-
sertion described at the end of Section II. We remark that a
range of minimally rigid formations can be generated through
the Henneberg insertion operation [27].
Proposition 6.1: For any undirected triangular formation,
where each agent acts as an estimating agent for only one
edge, then its associated Zmatrix is Hurwitz.
Proof: One can check that in this case R(z)S2(z)T+
S2(z)R(z)T=R(z)R(z)T. In addition, R(z∗)R(z∗)Tis
positive definite matrix since undirected triangular formations
are minimally rigid. So −R(z)S2(z)Tis Hurwitz at z=z∗
or equivalently e= 0, and this in turn is equivalent to Zis
Hurwitz.
Proposition 6.2: For any undirected triangular formation,
where one agent is the estimating agent for both of the two
edges that it is associated with and exactly one other agent is
the estimating agent for the remaining edge, then its Zmatrix
is Hurwitz.
Proof: In this case, we have
R(z)ST
2(z) = 

||z112 ||20 0
−zT
223 z112 ||z223 ||2−zT
223 z331
0−zT
331 z223 ||z331 ||2
,(27)
which can be rewritten into the block lower-triangular form
R(z)ST
2(z) = A11 0
A12 A22.(28)
Here, A11 =||z112 ||2is always positive; the characteristic
polynomial of A22 is quadratic and thus it is easy to compute
that both of its two eigenvalues live in the open left half-plane
when z223 and z331 are not parallel, which has to be true since
the formation is rigid. So the Zmatrix itself is Hurwitz.
The two situations of choosing estimating agents for trian-
gular formations are illustrated in Fig. 2 and Fig. 3 respec-
tively.
Fig. 4: Tetrahedron formation where the tails of the arrows
indicate the corresponding estimating agents.
Proposition 6.3: For any infinitesimally minimally rigid
formations in IR2that is generated by the Henneberg insertion
operation, its associated Zmatrix is Hurwitz if one chooses
the estimating agents following exactly the sequence of the
Henneberg insertions and in addition: (i) for the first three
agents, pick the estimating agents as in Proposition 6.1 or
Proposition 6.2; (ii) for any new insertion operation that has
just added two edges from a new agent to two existing agents,
pick those two existing agents to be the estimating agents for
the newly added two edges. Note that only those two chosen
estimating agents are involved and the other agents are not
affected at all.
Proof: It suffices to prove that for an n-agent, n≥3,
minimally rigid formation in IR2whose Zmatrix is Hurwitz
with its chosen estimating agents, the new (n+ 1)-agent
formation obtained from the n-agent formation by the Hen-
neberg insertion operation still has a Hurwitz Zmatrix if its
estimating agents are chosen according to the rule stipulated
in the proposition.
Let the n-agent formation’s Zmatrix be R(z)ST
2(z)and
the Zmatrix for the (n+ 1)-agent formation be ¯
R(z)¯
ST
2(z).
Then
¯
R(z)¯
ST
2(z) = R(z)ST
2(z) 0
>C(z),(29)
where “>” denotes the submatrix of less importance and
C(z) = ||zl||2−zT
lzl+1
−zT
l+1zl||zl+1 ||2,(30)
where zland zl+1 are the two vectors pointing from the
two chosen estimating agents’ positions to the (n+ 1)th
agent’s position. Then using the similar argument as proving
Proposition 6.2, one can show that ¯
R(z)¯
ST
2(z)is also Hurwitz.
B. Stabilizing a special class of infinitesimally minimally rigid
formations in IR3
Now we look at undirected rigid formations in IR3. We start
with simple tetrahedron formations.
Proposition 6.4: For an undirected tetrahedron formation in
IR3, if its estimating agents are chosen as shown in Figure 4,
then its Zmatrix is Hurwitz.
Proof: The Zmatrix for the tetrahedron formation with
the estimating agents chosen as shown in Figure 4 is
R(z)ST
2(z) = A(z) 0
>G(z),(31)

IEEE TRANSACTIONS ON ROBOTICS 7
Fig. 5: Three wheeled E-pucks with data-matrices as markers
on their tops that are used in the experimental setup.
where A(z)is the same matrix as in (27) and G(z)is the
Grammian matrix ¯zT¯zand ¯zis the stacked column vector of
z4,z5, and z6. Since the tetrahedron formation is minimally
rigid at z∗, all the vectors in ¯zare linearly independent.
Therefore, G(¯z)is positive definite at z∗and thus the Zmatrix
is Hurwitz.
Since there is no necessary and sufficient combinatorial
conditions for formations’ rigidity properties in IR3yet, we
can only look at a special class of rigid formations in IR3.
Proposition 6.5: Consider the class of infinitesimally mini-
mally rigid formation in IR3that can be generated by adding
in sequence new agents to a tetrahedron formation such that
every time the new agent is connected to three existing agents
that form a triangular formation. If in each insertion operation,
the three estimating agents for the three newly added edges
are exactly the three associated existing agents, then the Z
matrix for the overall formation is Hurwitz.
The proof for this proposition is similar to that of Proposition
6.3 and we omit it here.
VII. EXP ER IM EN TAL AN D SI MU LATION R ES ULT S
A. Formation experiments in IR2
We first test the result in Theorem 5.1 using the E-puck
mobile robotic platform [28]. The experimental setup consists
of four wheeled E-puck robots in a planar area of 2.6×2
meters. Each robot is identified by a data-matrix marker on
its top as shown in Figure 5. Each robot’s reference point is
the intersection of the two solid bars of the marker and the
orientation of the marker is recognized by a vision algorithm
running at a PC connected to an overhead camera. Since E-
pucks are usually modeled by unicycles, we apply feedback
linearization about their reference points to obtain single-
integrator dynamics for simpler controller implementation. In
essence, we control the formation of the reference points of
the robots. The whole image of the testing area is covered by
1600 ×1200 pixels, where the distance between two consec-
utive horizontal or vertical pixels corresponds approximately
to 1.6mm. The PC runs a real time process computing the
relative vectors between the robots and computes the control
inputs for the robots. The communication takes place when
sending the commands from the PC to the E-pucks in order to
move their wheels, which gives the required linear and angular
velocities to the robots after being translated into common
(linear velocity) and differential (angular velocity) commands
to the wheels of the robots. The communication is done via
Bluetooth at the fixed frequency of 20Hz.
(a) (b)
800 1000 1200 1400
200
300
400
500
600
700
800
X [pixels]
Y [pixels]
(c)
0 20 40 60 80
−30
−20
−10
0
10
Time [s]
Error [Pixels]
e1e2e3e4e5
(d)
0 20 40 60 80
0
10
20
30
40
50
60
Time [s]
Speed [Pixels/s]
Speed1
Speed2
Speed3
Speed4
(e)
Fig. 6: Experimental results of a 2D formation with incon-
sistent measurements where a standard gradient-based control
is used (i.e. without the use of our proposed estimator); (a)
initial configuration; (b) final configuration after 100 seconds;
(c) the robot’s trajectories; (d) the plot of errors ek, where
k= 1,...,5; (e) speeds of the four robots which show that
the robots never stop.
We consider the following fixed measurement inconsistency
that is unknown to the robots
µ=19 16 19.5 10 16Tpixels2.(32)
The magnitude of such inconsistency is carefully chosen to
reflect the possible bias of about √16 ×1.6=6.4mm, which
is quite common among acoustic or infrared sensors for this
kind of robots.
When the robots use directly the standard gradient control
strategy, the measurement inconsistency induces the closed
orbit as shown in Figure 6 and the shape of the formation
is distorted. In comparison, for the same setup, we also
apply the estimator-based gradient control (6)-(8). We pick the
estimating agents following the rule specified by Proposition
5.3 and as a result the transpose of the incidence matrix of
the associated estimating-agent graph is
H=






1−1 0 0
0 1 −1 0
1 0 −1 0
−1 0 0 1
001−1






.(33)
We choose κ= 1 and BT
k=110.
In Figure 7 we show the experimental result of the formation
under the estimator-based gradient control. It is clear that
the robots do not exhibit any undesirable motion induced by
measurement inconsistency. The errors and the robots’ speeds
converge to zero as soon as the inconsistency is effectively
estimated by the estimating agents. In experiments, the errors
and the speeds do not converge to zero precisely since once the
discrepancies are approaching being effectively estimated, the

IEEE TRANSACTIONS ON ROBOTICS 8
(a) (b)
1200 1300 1400
600
650
700
750
800
X [pixels]
Y [pixels]
(c)
0 10 20
−20
−15
−10
−5
0
5
10
Time [s]
Error [Pixels]
e1e2e3e4e5
(d)
0 5 10 15 20
0
10
20
30
40
50
60
Time [s]
Speed [Pixels/s]
Speed1
Speed2
Speed3
Speed4
(e)
0 5 10 15 20
0
5
10
15
20
25
Time [s]
Measurement inconsistency
and estimation [pixels2]
ˆµ1
µ1
ˆµ2
µ2
ˆµ3
µ3
(f)
Fig. 7: Experimental results of a 2D formation with inconsis-
tent measurements using our proposed estimator-based gradi-
ent control. The initial configuration in (a) and the final con-
figuration after 20 seconds in (b) correspond to the trajectory
plot in (c) using dashed-lines and dotted-lines respectively; (d)
the plot of errors ek,k= 1,...,5; (e) the plots of the robots’
speeds which show that the four agents eventually stop; (f)
the plot of the estimated measurement inconsistencies for the
first three edges (shown in solid-line) which asymptotically
converge to the actual ones (shown in dashed-line).
control inputs become small and can be dominated by friction
forces.
B. Formation simulations in IR3
In this numerical setup, we consider a formation of five
agents in IR3. The measurement inconsistency takes the form
of the superposition of a constant random offset and a sine
wave with a known frequency. Each inconsistency µkhas
different frequencies and offsets. We implement control (6)-
(8) and choose the estimating agents according to Proposition
5.5. The five agents are prescribed to maintain two regular
tetrahedrons sharing the same base where all the edge lengths
are d= 5. The five agents are placed randomly within a
volume of 50 cubic units. We choose κ= 1,BT
k=110,
and the estimators ξkare initialized to be zero. In Figure 8
it is clear that the agents’ velocities converge to zero and the
formation shape converges to the desired one. Moreover, ˆµ(t)
converges to the periodic inconsistency µ(t).
VIII. CONCLUSIONS
In this paper we have presented an estimator-based gradient
control for stabilizing rigid formations using the internal model
principle. We have effectively dealt with measurement incon-
sistency in the form of the combination of periodic signals with
known frequencies but unknown amplitudes, phases and off-
sets. The proposed distributed control removes the surprising
undesirable steady-state movement reported by some recent
papers. To carry out a key step of choosing estimating agents
in our proposed approach, we have discussed a systematic
way to guarantee the performance of our controller for classes
of infinitesimally minimally rigid formations in IR2and IR3.
Experimental results for four mobile robots have been per-
formed for formations in IR2and numerical simulations have
been done for formations in IR3. We are currently working on
extending our control design to more detailed robot models,
such as higher-order integrator and unicycle models. We are
also interested in testing the performances of the controllers
using outdoor robotic setups.
REFERENCES
[1] M. Egerstedt and X. Hu, “Formation constrained multi-agent control,”
IEEE Transactions on Robotics and Automation, vol. 17, pp. 947–951,
2001.
[2] F. Bullo, J. Cortes, and S. Martinez, Distributed Control of Robotic
Networks. Princeton: Princeton University Press, 2009.
[3] N. E. Leonard, D. A. Paley, F. Lekien, R. Sepulchre, D. M. Fratan-
toni, and R. E. Davis, “Collective motion, sensor networks and ocean
sampling,” Proceedings of IEEE, vol. 95, pp. 48–74, 2007.
[4] D. M. Stipanovic, C. R. Graunke, K. A. Intlekofer, and M. W.
Spong, “Formation control and collision avoidance for multi-agent non-
holonomic systems: Theory and experiments,” International Journal of
Robotics Research, vol. 27, pp. 107–126, 2008.
[5] I. Suzuki and M. Yamashita, “Distributed anonymous mobile robots:
Formation of geometric patterns,” SIAM J. Comput., vol. 28, pp. 1347–
1363, 1999.
[6] J. Fredslund and M. J. Mataric, “A general algorithm for robot forma-
tions using local sensing and minimal communication,” IEEE Transac-
tions on Robotics and Automation, vol. 18, pp. 837–846, 2002.
[7] J. R. T. Lawton and R. W. Beard, “A decentralized approach to formation
maneuvers,” IEEE Transactions on Robotics and Automation, vol. 19,
pp. 933–941, 2003.

IEEE TRANSACTIONS ON ROBOTICS 9
Z [pixels]
X [pixels]
Y [pixels]
(a)
5 10 15 20 25
−0.4
−0.2
0
0.2
0.4
0.6
Time [secs]
Error [pixels]
e1
e2
e3
(b)
10 20 30
0
2
4
6
8
Time [s]
Speed [Pixels/s]
speed1
speed2
speed3
speed4
speed5
(c)
0 2 4 6 8 10 12 14 16 18 20
−0.5
0
0.5
1
1.5
Time [s]
Measurement Inconsistency
and estimation [Pixels2]
ˆµ1
µ1
ˆµ2
µ2
ˆµ3
µ3
(d)
Fig. 8: Simulation results of a 3D formation with inconsistent
measurements using our proposed estimator-based gradient
control where the inconsistencies are biased sinusoidal signals;
(a) the plot of the trajectories where the initial positions are
shown in circles and the final positions are shown in crosses;
(b) the error ek,k= 1,2,3; (c) the plot of the agents’ speeds
which shows that the five agents eventually stop; (d) the plot of
the estimated measurements inconsistencies for the first three
edges (shown in solid-line) which asymptotically converge to
the actual ones (shown in dashed-line).
[8] J.-W. Kwon and D. Chwa, “Hierarchical formation control based on a
vector field method for wheeled mobile robots,” IEEE Transactions on
Robotics, vol. 28, pp. 1335–1345, 2012.
[9] B. D. O. Anderson, C. Yu, B. Fidan, and J. Hendrickx, “Rigid graph
control architectures for autonomous formations,” IEEE Control Systems
Magazine, vol. 28, pp. 48–63, 2008.
[10] L. Krick, M. E. Broucke, and B. A. Francis, “Stabilization of infinites-
imally rigid formations of multi-robot networks,” International Journal
of Control, vol. 82, pp. 423–439, 2009.
[11] C. Yu, B. D. O. Anderson, S. Dasgupta, and B. Fidan, “Control of
minimally persistent formations in the plane,” SIAM Journal on Control
and Optimization, vol. 48, pp. 206–233, 2009.
[12] M. Cao, C. Yu, and B. D. O. Anderson, “Formation control using range-
only measurements,” Automatica, vol. 47, pp. 776–781, 2011.
[13] A. Belabbas, S. Mou, A. Morse, and B. Anderson, “Robustness issues
with undirected formations,” in Proc. of the 51st IEEE Conference on
Decision and Control, Hawaii, USA, 2012, pp. 1445–1450.
[14] Z. Sun, S. Mou, B. D. Anderson, and A. S. Morse, “Non-robustness of
gradient control for 3-d undirected formations with distance mismatch,”
in Proc. of the 2013 IEEE Australian Control Conference, 2013, pp.
369–374.
[15] H. Marina, M. Cao, and B. Jayawardhana, “Controlling formations of
autonomous agents with distance disagreements,” in Proc. of the 4th
IFAC Workshop on Distributed Estimation and Control in Networked
Systems, 2013.
[16] A. Caiti, A. Garulli, F. Livide, and D. Prattichizzo, “Localization of
autonomous underwater vehicles by floating acoustic buoys: A set-
membership approach,” IEEE Journal of oceanic engineering, vol. 30,
pp. 140–152, 2005.
[17] M. Belmont, J. Baker, and J. Horwood, “Acoidance of phase shift errors
in short term deterministic sea wave prediction,” Journal of Marine
Engineering and Technology, vol. 2, pp. 21–26, 2003.
[18] L. Asimow and B. Roth, “The rigidity of graphs, ii,” Journal of
Mathematical Analysis and Applications, pp. 171–190, 1979.
[19] M. Cao, A. S. Morse, C. Yu, B. D. O. Anderson, and S. Dasgupta,
“Maintaining a directed, triangular formation of mobile autonomous
agents,” Communications in Information and Systems, vol. 11, pp. 1–16,
2011.
[20] K.-K. Oh and H.-S. Ahn, “Formation control of mobile agents based
on inter-agent distance dynamics,” Automatica, vol. 47, pp. 2306–2312,
2011.
[21] C. de Persis and B. Jayawardhana, “On the internal model principle in
the coordination of nonlinear systems,” IEEE Trans. Contr. Netw. Syst.,
vol. 1, no. 3, pp. 272–282, 2014.
[22] C. Byrnes, F. D. Priscoli, and A. Isidori, Output Regulation of Uncertain
Nonlinear Systems. Boston: Birkhauser, 1997.
[23] P. Wieland, R. Sepulchre, and F. Allgower, “An internal model principle
is necessary and sufficient for linear output synchronization,” Automat-
ica, vol. 47, pp. 1068–1074, 2011.
[24] R. Diestel, Graph Theory. Springer-Verlag New York, Inc., 1997.
[25] S. Mou, A. S. Morse, A. Belabbas, Z. Sun, and B. Anderson, “Undi-
rected rigid formations are problematic,” to appear in the Proc. 53rd
IEEE Conf. Dec. Contr., Los Angeles, 2014.
[26] Byrnes and A. Isidori, “Asymptotic stabilization of minimum phase
nonlinear systems,” IEEE Transactions on Automatic Control, vol. 36,
pp. 1122–1137, 1991.
[27] R. Haas, D. Orden, G. Rote, F. Santos, B. Servatius, H. Servatius,
D. Souvaine, I. Streinu, and W. Whiteley, “Planar minimally rigid graphs
and pseudo-triangulations,” Computational Geometry, vol. 31, no. 1-2,
pp. 31 – 61, 2005.
[28] F. Mondada, M. Bonani, X. Raemy, J. Pugh, C. Cianci, A. Kalptocz,
S. Magnenat, J.-C. Zufferey, D. Floreano, and A. Martinoli, “The e-
puck, a robot designed for education in engineering,” in Proc. of the 9th
Conference on Autonomous Robot Systems and Competitions, 2009, pp.
59–65.

IEEE TRANSACTIONS ON ROBOTICS 10
Hector G. de Marina received the M.Sc. degree in
electronics engineering from Complutense Univer-
sity of Madrid, Madrid, Spain in 2008 and the M.Sc.
degree in control engineering from the University
of Alcala de Henares, Alcala de Henares, Spain
in 2011. He is currently a PhD student in the
Faculty of Mathematics and Natural Sciences, Uni-
versity of Groningen, Groningen, The Netherlands.
His research interests include formation control and
navigation for autonomous robots.
Ming Cao received the PhD degree in electri-
cal engineering from Yale University, New Haven,
CT, USA in 2007. He is an associate professor
with tenure responsible for the research direction
of network analysis and control at the University
of Groningen, the Netherlands. His main research
interest is in autonomous agents and multi-agent
systems, mobile sensor networks and complex net-
works. He is an associate editor for Systems and
Control Letters, and for the Conference Editorial
Board of the IEEE Control Systems Society. He is
also a member of the IFAC Technical Committee on Networked Systems.
Bayu Jayawardhana (SM13) received the B.Sc.
degree in electrical and electronics engineering from
the Institut Teknologi Bandung, Bandung, Indonesia,
in 2000, the M.Eng. degree in electrical and elec-
tronics engineering from the Nanyang Technological
University, Singapore, in 2003, and the Ph.D. de-
gree in electrical and electronics engineering from
Imperial College London, London, U.K., in 2006.
Currently, he is an associate professor in the Faculty
of Mathematics and Natural Sciences, University of
Groningen, Groningen, The Netherlands. He was
with Bath University, Bath, U.K., and with Manchester Interdisciplinary Bio-
centre, University of Manchester, Manchester, U.K. His research interests are
on the analysis of nonlinear systems, systems with hysteresis, mechatronics,
systems and synthetic biology. Prof. Jayawardhana is a Subject Editor of the
International Journal of Robust and Nonlinear Control, an associate editor of
the European Journal of Control, and a member of the Conference Editorial
Board of the IEEE Control Systems Society.