Content uploaded by Hector Garcia de Marina
Author content
All content in this area was uploaded by Hector Garcia de Marina on Feb 04, 2019
Content may be subject to copyright.
IEEE TRANSACTIONS ON ROBOTICS 1
Distributed rotational and translational maneuvering
of rigid formations and their applications.
Hector Garcia de Marina, Member, IEEE, Bayu Jayawardhana, Senior Member, IEEE, and Ming Cao, Senior
Member, IEEE
Abstract—Recently it has been reported that range-
measurement inconsistency, or equivalently mismatches in pre-
scribed inter-agent distances, may prevent the popular gradient
controllers from guiding rigid formations of mobile agents to
converge to their desired shape, and even worse from standing
still at any location. In this paper, instead of treating mismatches
as the source of ill performance, we take them as design param-
eters and show that by introducing such a pair of parameters
per distance constraint, distributed controller achieving simulta-
neously both formation and motion control can be designed that
not only encompasses the popular gradient control, but more
importantly allows us to achieve constant collective translation,
rotation or their combination while guaranteeing asymptotically
no distortion in the formation shape occurs. Such motion control
results are then applied to (a) the alignment of formations’
orientations and (b) enclosing and tracking a moving target.
Besides rigorous mathematical proof, experiments using mobile
robots are demonstrated to show the satisfying performances of
the proposed formation-motion distributed controller.
Index Terms—Formation Control, Rigid Formation, Motion
Control, Target enclosing.
I. INTRODUCTION
MANY coordinated robot tasks, such as, the enclosing of
a target [1], area exploration & surveillance [2] and the
vehicle platooning for energy efficiency [3], can be achieved
by combining two different cooperative controls: multi-agent
formation control and group motion control. The former is to
achieve and maintain a specific desired shape of a multi-agent
formation while the latter to guide the motion of the group
as a whole. For simple formations, such as, line formations in
the vehicle platooning problem, these two control problems
can be solved simultaneously by using the passivity-based
control approach as pursued in [4]. However, for formations
in more complicated shapes, these two control problems are
usually tackled separately, namely using the gradient-based
strategies for formation control and leader-follower coordina-
tion for motion control, where for the latter the leader moves
according to a desired trajectory and the followers simply track
the leader [5]. But when the separately designed formation
controller and motion controller are jointly in force as in
the common scenario in practice, conflict often occurs: the
shape of the formation is distorted when an agent compromises
between the demands from formation and motion controls as
The authors are with the Engineering and Technology Institute of Gronin-
gen, University of Groningen, 9747 AG Groningen, The Netherlands. (e-mail:
{h.j.de.marina, b.jayawardhana, m.cao}@rug.nl). This work was supported
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).
to where and how it needs to move [6], [7]. While efforts
have been made before to solve such conflict by installing
velocity sensors in addition to position sensors on mobile
agents and thus compensating the distortion with the help of
the more sensed information [5], we in this paper take a much
more head-on approach by generalizing the gradient-based
distributed formation control law for both control objectives
simultaneously guaranteeing no shape distortion in the steady-
state desired formation motion even without the knowledge
of any additional sensed information in comparison to the
conventional control. The surprisingly simple structure of our
proposed control law opens possibilities to solve other difficult
problems, such as, collective rotational motion, the enclosing
of a moving target and the formation coordination task for
agents governed by higher-order dynamics.
In gradient-based formation control, stabilizable formations
are identified employing rigidity graph theory where the
vertices of a graph represent the agents and the edges stand
for the inter-agent distance constraints to define the shape of
the formation [8]–[11]. Rigid formations are associated with a
potential function determined by the agents’ relative positions.
The potential has a minimum at the desired distances between
the agents and thus its gradient leads naturally to the formation
controller that stabilizes rigid formations locally. However,
it was recently reported in [12] and later more exhaustively
studied in [13], [14] that for such control laws, if there are
mismatches between the prescribed distances for neighboring
agents, the performance of the controller deteriorates signifi-
cantly since mismatches induce undesired steady-state motion
and distortion in the formation shape. If no estimators, as
proposed and experimented in [15], are installed to improve
the performance of such controllers, the mismatch-induced
motion in steady-states takes interesting forms for almost all
mismatches: in IR2, the formation follows a closed orbit with
a constant angular velocity; in IR3the movement is helical,
determined by a single sinusoidal signal and a constant drift
[16].
Motivated by the mismatch-induced motion reported in [16],
we introduce parameter pairs in the gradient-based formation
control law for steering the whole formation; in particular,
for each prescribed distance constraint, we introduce a pair of
motion parameters, mimicking the mismatches as studied in
[16]. By a one-step (centralized) systematic design of such
motion parameters, we show that we can steer the entire
formation in rotation and/or translation in a distributed manner.
The proposed gradient-based formation-motion joint controller
using the motion parameters has several advantages. First,
arXiv:1604.07849v2 [cs.RO] 25 Aug 2016
IEEE TRANSACTIONS ON ROBOTICS 2
one can achieve precisely the desired steady-state formation
shape and the group motion simultaneously, including rota-
tional motion, in a distributed fashion. To the best of our
knowledge, no such result has been reported in the literature.
For single-integrator agent dynamics, we relate analytically the
magnitude of the motion parameters to the speed of the group
motion. Second, our proposed approach enables us to align the
formation with respect to a global coordinate frame by adding
a simple control term to an arbitrary agent when guiding
the group in motion. In comparison, in the literature such
an alignment is usually obtained by assigning a leader who
knows the global frame and letting the other agents (followers)
estimate the leader’s velocity employing a distributed estimator
[5]. Obviously in our approach, such estimators are not needed
at all. Third, one can easily use our method to address the
target tracking and enclosing problem both in IR2and IR3
where one assigns the formation shape to specify how the
target is enclosed and tracked by the pursuers. In comparison
to the solution of such problems in [17], [18] among other
works, we do not confine our formation to follow the same
circular trajectory nor require all enclosing agents to measure
their relative positions with respect to the target. The rest of
the paper is organized as follows. In Section II we introduce
the notation and background for rigid formations in IR2and
IR3and we briefly review the gradient control in Section
III. The design of the desired steady state motion with the
desired formation shape is described in Section IV, while the
corresponding stability analysis is presented in Section V. We
implement our proposed formation-motion controller in two
different contexts: (i) the design of a simultaneous formation &
translation control in global coordinates in Section VI-A; and
(ii) the formation control design for solving the target tracking
and enclosing problem in Section VI-B. Finally experimental
results using wheeled mobile robots are shown in Section VII.
II. PRELIMINARIES
In this section, we introduce some notations and basic
concepts. For a given matrix A∈IRn×p, define A∆
=A⊗
Im∈IRnm×pm, where the symbol ⊗denotes the Kronecker
product, m= 2 for IR2or otherwise 3for IR3, and Im
is the m-dimensional identity matrix. For a stacked vector
x∆
=xT
1xT
2. . . xT
kTwith xi∈IRn, i ∈ {1, . . . , k},
we define the diagonal matrix Dx
∆
= diag{xi}i∈{1,...,k}∈
IRkn×k. We denote by |X| the cardinality of the set Xand by
||x|| the Euclidean norm of a vector x. We use 1n×mand
0n×mto denote the all-one and all-zero matrix in IRn×m
respectively.
A. Formations and graphs
We consider a formation of n≥2autonomous agents
whose positions are denoted by pi∈IRm. The agents are able
to sense the relative positions of its neighboring agents. The
neighbor relationships are described by an undirected graph
G= (V,E)with the vertex set V={1, . . . , n}and the ordered
edge set E ⊆ V × V. The set Niof the neighbors of agent iis
defined by Ni
∆
={j∈ V : (i, j)∈ E}. We define the elements
of the incidence matrix B∈IR|V|×|E| for Gby
bik
∆
=
+1 if i=Etail
k
−1if i=Ehead
k
0otherwise
,
where Etail
kand Ehead
kdenote the tail and head nodes, respec-
tively, of the edge Ek, i.e. Ek= (Etail
k,Ehead
k). A framework
is defined by the pair (G, p), where p= col{p1, . . . , pn}is
the stacked vector of the agents’ positions pi, i ∈ {1, . . . , n}.
The stacked vector of the sensed relative distances can then
be described by
z=BTp.
Note that each vector zk=pi−pjin zcorresponds to the
relative position associated with the edge Ek= (i, j).
B. Infinitesimally and minimally rigid formations and their
realization
In this section we will briefly review the concept of rigid
formations and how to design them. Most of the concepts
explained here are covered in much more detail in [19] and
[20]. Define the edge function fGby fG(p) = col
kkzkk2
and denote its Jacobian by R(z) = DT
zBT, which is called
in the literature the rigidity matrix. A framework (G, p)is
infinitesimally rigid if rankR(z)=2n−3when embedded
in R2or if rankR(z) = 3n−6when it is embedded in R3.
Additionally, if |E| = 2n−3in the 2D case or |E| = 3n−6
in the 3D case then the framework is called minimally rigid.
Roughly speaking, under the distance contraints the only mo-
tions that one can perform over the agents in an infinitesimally
and minimally rigid framework, while they are already in the
desired shape, are the ones defining translations and rotations
of the whole shape. We illustrate in Figure 1 some examples
in IR2and IR3of rigid and non-rigid frameworks.
For a given stacked vector of desired relative positions
z∗= [ z∗
1
Tz∗
2
T... z∗
|E|
T]T, the resulting set Zof the possible
formations is defined by
Z∆
=I|E| ⊗ Rz∗,
where Ris the set of rotational matrices in IR2or IR3.
Roughly speaking, Zconsists of all formation positions that
are obtained by rotating z∗. If (G, p)is infinitesimally and
minimally rigid, then, similar to the above, we can define the
set of resulting formations Dby
D∆
=nz|||zk|| =dk, k ∈ {1,...,|E|}o,
where dk=||z∗
k||, k ∈ {1,...,|E|}.
Note that in general it holds that Z ⊆ D. For a desired
shape, one can always design Gto make the formation
infinitesimally and minimally rigid. In fact in IR2, an in-
finitesimally and minimally rigid framework with two or more
vertices can always be constructed through the Henneberg
construction [21]. Unfortunately there is no known corre-
sponding systematic operations in IR3yet. Nevertheless in IR3
one can always obtain an infinitesimally and minimally rigid
IEEE TRANSACTIONS ON ROBOTICS 3
(a) (b) (c) (d)
(e) (f) (g)
Fig. 1: a) The square without an inner diagonal is not rigid
since we can smoothly move the top two nodes keeping the
other two fixed without breaking the distance constraints; b)
A rigid but not an infinitesimally rigid framework. If we rotate
the left inner triangle, then the right inner triangle can be
counter-rotated in order to keep the inter-distances constant; c)
A minimally rigid but not an infinitesimally rigid framework
since the nodes’ positions are collinear; d) The triangle is
infinitesimally and minimally rigid; e) The cube formed by
squares without diagonals is not rigid; f) The zero-volume
tetrahedron is rigid but not infinitesimally rigid in IR3, since
all the nodes are co-planar; and g) The tetrahedron in IR3is
infinitesimally and minimally rigid.
framework by the Henneberg-like tetrahedron insertions as it
has been done in [15].
III. GRADIENT CONTROL IN INTER-AGENT DISTANC ES
For a formation of nagents associated with the neighbor
relationship graph G, consider the following system where we
model the agents as kinematical points
˙p=u, (1)
where uis the stacked vector of control inputs ui∈IRmfor
i={1, . . . , n}.
For each edge Ekone can construct a potential function
Vkwith its minimum at the desired distance ||z∗
k|| so that the
gradient of such functions can be used to control inter-agent
distances distributively.
We take as a potential function
VBTp=V(z) = |E|
X
k=1
Vk(zk),
and apply to each agent iin (1) the following gradient descent
control
ui=−∇pi
|E|
X
k=1
Vk(zk).(2)
One can then show straightforwardly that the multi-agent
formation will converge locally to the desired shape employing
the following known results.
Theorem 3.1: [22], [23] If the potential V(z)is real
analytic, i.e. it possesses derivatives of all orders and agrees
with its Taylor series in the neighborhood of every point, then
the formation under the distributed (2) will converge locally
asymptotically to the desired shape Z.
The authors of [22], [23] have shown that for a real analytic
potential function V, system (1) with the control law (2) is
locally asymptotically stable about the point corresponding to
the local minima of V(z). We consider for each edge Ekthe
following family of quadratic1potential functions
Vk(||zk||) = 1
2l(||zk||l−dl
k)2, l ∈N,(3)
with the following gradient along zk
∇zkVk(||zk||)=(∇zk||zk||)∂Vk(||zk||)
∂||zk||
=zk||zk||l−2(||zk||l−dl
k).(4)
One immediate observation is that when l= 1, the right-hand
side of (4) becomes the unit vector zk
||zk|| multiplied by the
distance error (||zk|| − dk). This fact will play an important
role later for the design of the desired steady-state motion
of the formation. It is straightforward to check that for every
edge Ek= (i, j)one has ∇piVk=−∇pjVk=∇zkVk, and
obviously ∇phVk=0m×1for all h6=i, j. Thus the agent
dynamics (1) under (2) can be written in the following compact
form
˙p=−B∇zV(z),(5)
where ∇zV(z)is the stacked vector of ∇zkVk(||zk||)’s. De-
noting the distance error for edge kby
ek=||zk||l−dl
k,(6)
it follows that
∇zkVk(||zk||) = zk||zk||l−2ek.
By substituting it into (5) and noting that
˙ek=l||zk||l−1d
dt ||zk|| =l||zk||l−2zT
k˙zk,
we can write down the closed-loop system dynamics in the
compact form
˙p=−BDzD˜ze=−R(z)TD˜ze(7)
˙z=BT˙p=−BTR(z)TD˜ze(8)
˙e=lD˜zDT
z˙z=−lD˜zR(z)R(z)TD˜ze, (9)
where e∈IR|E| is the stacked vector of ek’s, ˜z∈IR|E| is
the stacked column vector consisting of all the ||zk||l−2’s, and
the matrices Dzand D˜zare the block diagonal matrices of
zand ˜zrespectively, as defined before in Section II-A. In
the following proposition, we establish the local exponential
convergence of the error (6) to zero for arbitrary l∈N. This
fact is a straightforward extension of the result in [13] for
the case when l= 2, and we provide the proof below for
completeness.
Proposition 3.2: Consider the closed-loop system (7)-(9)
with graph Gdriven by the gradient of the potential function
V=PkVkwhere Vkis defined in (3) for a fixed l∈N.
If the formation in the desired shape Dis infinitesimally and
minimally rigid, then the error signal egoes to zero locally
exponentially fast.
1Any analytic potential functions, including the ones for collision avoidance
[24], can be approximated by a quadratic function in a small neighborhood
about their local minima, and for this reason such quadratic potentials are of
special interest of study.
IEEE TRANSACTIONS ON ROBOTICS 4
Proof:First we prove that the error system (9) is au-
tonomous. The elements of R(z)R(z)T=DT
zBTBDzare
products of the form zT
izj, i, j ∈ {1,...,|E|}. It has been
shown in [14] that for any infinitesimally and minimally
rigid framework, there exists a neighborhood Uzabout this
framework such that any zT
izj, zi, zj∈ Uz, can be written as
a smooth function gij (||zk||2), and since ||zk|| = (ek+dl
k)1
l,
we can write
zT
izj=gij (e).(10)
Thus the vector ˜zand the matrix R(z)R(z)Tcan be rewritten
as smooth functions depending locally only on eabout the
origin. This shows that the error system (9) is at least locally
an autonomous system.
Now to prove the stability of e= 0 for (9), we choose a
Lyapunov function V=1
2l||e||2which satisfies
dV
dt=1
leT˙e=−eTD˜zR(z)R(z)TD˜ze
Since R(z)R(z)Tand D˜zare positive definite matrices at the
desired formation shape as R(z)has full row rank at z∈ D,
the matrix
Q(e) = D˜zR(z)R(z)TD˜z(11)
is positive definite at e= 0. Since the eigenvalues of Q(e)are
smooth functions of e, there exists a compact set
Q∆
={e:||e||2≤ρ}(12)
for some positive constant ρwhere Q(e)is positive definite.
Define λmin to be the smallest eigenvalue of Q(e)in Q. Then
dV
dt≤ −λmin||e||2,(13)
which immediately implies the local exponential convergence
of eto zero.
We want to emphasize that e(t)→0as t→ ∞ does not imply
z(t)→ Z but instead z(t)→ D. As pointed out by Theorem
3.1, z(t)is guaranteed to converge to Zonly when the initial
condition z(0) is sufficiently close to Z.
IV. GRADIENT-BASE D FO RM ATIO N-MOTION CONTROL
For the sake of simplicity and clarity in the notation, from
this point onwards we will consider the case for l= 2 and
the main results can be easily extended to any l∈N. Then
˜z=1|E|×1and thus D˜zis the identity matrix. We denote
by Ogthe global frame of coordinates fixed at the origin
of IRmwith some arbitrary fixed orientation. Similarly we
denote by Oithe local frame for agent iwith some fixed
arbitrary orientation independent of Og. Finally, we denote by
Obthe body frame fixed at the centroid pcof the desired rigid
formation. Furthermore, if we rotate the rigid formation with
respect to Og, then Obis also rotated in the same manner.
Let ipjdenote the position of agent jwith respect to Oi. To
simplify notation whenever we represent an agents’ variable
with respect to Og, the superscript is omitted, e.g. pj
∆
=gpj.
We consider a pair of (scaled by a gain c∈IR) motion-
control parameters µk
c∈IR and ˜µk
c∈IR to the term d2
k
for each agent associated with the edge Ek= (i, j). More
precisely, agent iuses a controlled distance d2
k+µk
cand
correspondingly, agent j,d2
k−˜µk
c. For the corresponding edge
Ek= (i, j)we incorporate these parameters to the gradient
descent controller and apply the same gain c, namely
uk
i=−czk(||zk||2−d2
k) + µkzk
uk
j=czk(||zk||2−d2
k) + ˜µkzk,
where uk
iand uk
jare the corresponding control inputs for
agents iand jwith edge Ek. The equations above lead to
the following compact form
˙p=−cR(z)Te+A(µ, ˜µ)z, (14)
where the elements aik of Aare constructed in a very similar
way as in the incidence matrix as follows
aik
∆
=
µkif i=Etail
k
˜µkif i=Ehead
k
0otherwise,
(15)
therefore we define µ, ˜µ∈IR|E| as the stacked vectors of
µkand ˜µkfor all k∈ {1,...,|E|}. Note that if µ=−˜µ,
then d2
k+µk
c=d2
k−˜µk
c=˜
d2
k, which implies that we have a
gradient-based control with ˜
d2
kbeing the new stacked vector of
the prescribed distances. The gain cis a free design parameter
for achieving exponential stability of the formation as we will
see later in the stability analysis.
One important property of (14) is that each agent ican work
with only its own local frame Oi. One can see this more in
detail in the following lemma.
Lemma 4.1: The control law applied in (14) can be imple-
mented for each agent iusing only its local frame Oi.
The proof is omitted due to space limitation but it is
straightforward by following similar arguments as in [5], [11].
In order to induce some desired steady-state motion of the
formation in the desired shape, we can manipulate µand ˜µat
the equilibrium of (14). Then the steady-state motion will be
a function of the desired shape z∗∈ Z and µ, ˜µ:
˙p∗=A(µ, ˜µ)z∗.(16)
Before discussing on how to design the motion parameters µ
and ˜µin order to keep Zinvariant, let us first recall some
facts from rigid body mechanics [25]. As in the case of points
in a rigid body, the steady-state velocity of every agent ˙p∗
iat
the desired rigid formation shape can be decomposed into
˙p∗
i= ˙p∗
c+bω×bp∗
i
| {z }
˙p∗
iω
,(17)
where bωis the angular velocity of the rigid formation (similar
to that for the rigid body). In particular, in view of (14) with
Aas defined in (15), the velocity (17) at the desired shape
z∗∈ Z is given by
˙p∗
c+ ˙p∗
iω=|E|
X
k=1
aikz∗
k.(18)
Let us decompose the motion parameters into µ=µv+µωand
˜µ= ˜µv+ ˜µω, where µv,˜µv∈IR|E| and µω,˜µω∈IR|E| will be
IEEE TRANSACTIONS ON ROBOTICS 5
˙p∗
c
bω×bp∗
i
˙p∗
c
˙p∗
i
(a)
OgOb
bω
b˙p∗
c
(b)
OgOb
bω
b˙p∗
c
(c)
Fig. 2: Illustration of the steady-state motion in different
frames of coordinates: (a) in Og, the steady-state velocity ˙p∗
i
can be decomposed into translational and rotational term given
by (17); (b) in Obthe steady-state velocity vector b˙p∗
ibecomes
constant. For 2D formations the steady-state trajectory in
Ogbecomes a circular closed orbit since bωand b˙p∗
care
perpendicular; and (c) in general for 3D formations bωand
b˙p∗
care not perpendicular, and therefore the resultant trajectory
(shown in red) is helical in Og.
used to assign the desired translational and rotational velocity,
respectively, of the rigid formation. Using this decomposition,
we can rewrite (18) for all the agents into the following
compact form
˙p∗=A(µv,˜µv)z∗
| {z }
1|V|×1⊗˙p∗
c
+A(µω,˜µω)z∗
| {z }
˙p∗
ω
,(19)
where ˙p∗
ω∈IRm|V| is the stacked vector of all the rotational
velocities ˙p∗
iω, i ∈ {1,...,|V|}.
In the desired shape, the stacked vector bz∗of relative
positions bz∗
k, k ∈ {1,...,|E|} in Obbecomes a constant. This
is due to the fact that Obfollows the steady-state motion,
and thus according to (19) the velocities b˙p∗
cand b˙p∗
iωalso
become constant. In particular, if bω= 0, then we will have a
drift of the desired formation in Og; otherwise for a nonzero
constant vector bω, the formation will follow a closed circular
orbit (which is always the case in 2D formations) when bωis
perpendicular to b˙p∗
c, or otherwise a helical trajectory in Og.
Figure 2 shows an illustration of these facts on the steady-state
velocity of the desired formation expressed in either Ogand
Ob.
Now we are ready to show how to design the motion
parameters µand ˜µin (19). Note that the steady-state motion
must not distort the desired shape, i.e. the motion parameters
have to be designed such that Zis an invariant set in (19).
The desired steady-state velocity (16) in Obcan be rewritten
as
b˙p∗=S1Dbz∗µ+S2Dbz∗˜µ=S1Dbz∗S2Dbz∗µ
˜µ
=T(bz∗)µv
˜µv+µω
˜µω,
where S1is constructed from the incidence matrix Bsetting its
−1elements to zero and S2=S1−B. The motion parameters
will be used to design both velocities in (19) with respect to
Obas in Figure 2c since they are constant in such a frame.
Note that the knowledge about Obis only needed during the
design stage for the desired steady-state velocity. During the
implementation of the distributed control law, the agents only
need to know about their own Oi’s as shown in Lemma 4.1.
We now demonstrate how to design the velocity b˙p∗
cemploy-
ing the motion parameters. It is important to note that in an
infinitesimally and minimally rigid framework, the minimum
number of edges associated to a node is two in IR2or three in
IR3. So if bz∗∈ Z then the relative positions bz∗
k’s associated
to agent ican span the whole IR2for the planar formations (or
IR3in the 3D case). This implies that the domain of the desired
velocity b˙p∗
iis the whole space and it can be assigned by an
appropriate choice of µand ˜µ(c.f. (18)). Since the velocity b˙p∗
c
is the same for all the agents, we set the following requirement
for µvand ˜µv
BT(1|V|×1⊗b˙p∗
c) = BTT(bz∗)µv
˜µv=0m|E|×1,(20)
which implies that [µv
˜µv]∈Ker{BTT(bz∗)}. However, since in
general the kernel of T(bz∗)can be non-trivial, there may exist
non-zero [µv
˜µv]such that b˙p∗=0m|V|×1, which is irrelevant for
our motion control. Taking this into account, the set of motion
parameters µvand ˜µvshould satisfy
µv
˜µv∈ U ∆
=PKer{T(bz∗)}⊥nKer{BTT(bz∗)}o,(21)
where PXstands for the projection over the space X. The
lower bound for the degrees of freedom of choosing the
elements of µvand ˜µvfor constructing b˙p∗
ccan obviously be
given by the number of motion parameters of the agent that
has the least number of neighbors. In other words,
dim{U} ≥ min
i{|Ni|},(22)
where Niis defined in Section II-A and Uin (21). Conse-
quently, we propose the following algorithm to compute µv
and ˜µvfor given b˙p∗
cand bz∗.
Algorithm 4.2:
1) Choose an agent iwith the least number of neighbors.
2) Assign µvkand ˜µvkassociated to agent ithat solves
b˙p∗
c=P|E|
k=1 aikbz∗
k.
3) Compute a basis Vthat spans the space defined in (21).
4) Compute a vector v∈IR2|E| employing the basis V
such that the previously computed µvk’s and ˜µvk’s are
the k’th and (|E| +k)’th elements of v. Then, the rest
of elements of vare necessarily the motion parameters
defining b˙p∗
cfor the whole formation, i.e. v= [ µv
˜µv].
IEEE TRANSACTIONS ON ROBOTICS 6
From (15) it is clear that the implementation of the motion
parameters is distributed, while the computation in Algorithm
4.2 is centralized since it requires the knowledge of Oband
Z. On the other hand, the motion parameters only need to
be computed once since the desired speed of the motion can
be modified by just rescaling µvand ˜µv. For a new agent j
joining the formation, its corresponding motion parameters for
the translational motion can be computed in a distributed way.
Consider agent jjoins the formation: if it asks a neighboring
agent iabout i˙p∗
cand Oi, then it becomes straightforward for
agent jto compute the corresponding motion parameters for
the desired j˙p∗
cby only following the second step of Algorithm
4.2.
Now we proceed to show how to design the motion param-
eters for the rotational velocity b˙p∗
ωin (19), i.e. the rotational
motion of the agents around the centroid. Note that this
rotational motion preserves the constant norms ||z∗
k||’s but not
the relative positions z∗
k’s. Since 1
2
d||zk||2
dt=zT
k˙zk, we can
impose the following condition for maintaining constant ||z∗
k||,
DT
z∗˙z∗=DT
z∗BTT(z∗)µω
˜µω=0|E|×1.(23)
As discussed after equation (19) the relative positions bz∗
k
are constant because Obis rotating with the rigid formation.
However, for having such rotation of the formation, the
constant velocities b˙p∗
ωimust be designed in order to keep the
norms ||bzk||’s constant. Therefore, we impose the following
restricting condition for µωand ˜µωin addition to (23)
DT
bz∗BTT(bz∗)µω
˜µω=0|E|×1.(24)
As in the case for calculating the motion parameters for b˙p∗
c,
the set of motion parameters µωand ˜µωare the subset of
motion parameters such that
µω
˜µω∈ W ∆
=PU⊥nKer{DT
bz∗BTT(bz∗)}o,(25)
where Uis as in (21). We remark that the dimension of Win
(25) is at least, one less than the dimension of Uin (21). This
is due to the fact b˙p∗
iωis perpendicular to bp∗
i(as shown in
(17)) and hence, the degree-of-freedom for choosing [µω
˜µω]is
at least one less than that for choosing [µv
˜µv]. The calculation
of the motion parameters µωand ˜µωfor a desired bωcan be
done in a similar way to Algorithm 4.2, where in the second
step we assign the aik’s giving the constant linear velocity
b˙p∗
iωfor making the agent ito rotate with angular velocity bω
around the centroid bp∗
cderived from Z, and the basis Vin
the third step is replaced by Wspanning Was in (25).
As before the implementation of µωand ˜µωis fully dis-
tributed but their computation at the design stage is not. On
the other hand, they only need to be computed once and
the angular speed can be regulated by just rescaling their
magnitude. Note that for designing different trajectories in
Ogas in Figure 2, we only need to change the speed of b˙p∗
c
and bω. Unfortunately, if a new agent jjoins the formation,
then the centroid will change, requiring to recompute in a
centralized way µωand ˜µω. Nevertheless, if the instantaneous
center of rotation [25] pr∈IRmis independent of the new
agent, e,g, as we will see in the target enclosing problem where
it will be fixed at agent 1, then agent jcan still compute its
motions parameters by only asking local information from a
neighboring agent i, i.e. its i˙p∗
iand i˙p∗
calong with Oiand in
addition we also require agent ito know about i(pi−pr).
Example 4.3: We would like to highlight the particular ex-
ample of calculating the bluemotion parameters for triangular
formations and its extension to tetrahedra in 3D. Consider a
generic triangular formation consisting of three agents whose
Ghas the following incidence matrix
B=
1 0 −1
−1 1 0
0−1 1
.(26)
Then it can be checked that the following conditions for µv
and ˜µv(any four implying the remaining one)
µv1+µv2+µv3= 0
˜µv1+ ˜µv2+ ˜µv3= 0
µv2+ ˜µv3= 0
µv3+ ˜µv1= 0
µv1+ ˜µv2= 0
,(27)
satisfy (21) and they have a non-trivial solution with two de-
grees of freedom satisfying (22). We would like to emphasize
that the set of equations in (27) is independent of Z. In general,
the computation of µωand ˜µωdepends on Zas it is shown
in the following proposition
Proposition 4.4: If bz∗∈Z defines an equilateral triangle,
then µω
˜µω=w
√616×1.
If bz∗∈Z defines an isosceles triangle with d1=d2, then
µω
˜µω= wαh3d2
3
2d2
1+d2
3
2−3d2
3
2d2
1+d2
3
1 2−3d2
3
2d2
1+d2
3
3d2
3
2d2
1+d2
3
1iT
,(28)
with α∈IR+being a normalizing factor and w∈IR+defining
the angular speed.
We omit the proof due to space constraint. Another inter-
esting property is that the centroid of the triangular formation
pc(t)is invariant if µ=µωand ˜µ= ˜µωbelong to (25) since
˙pc(t) = 1
3
3
X
i=1
˙pi(t) = 1
3
3
X
k=1
(µωk+ ˜µωk)zk(t)=0.
These results can be extended in a similar way to tetrahedrons.
V. STABILITY AN ALYS IS
After we have the design of the desired steady-state behavior
of the formation as discussed thoroughly in Section IV, we
provide the stability analysis of the closed-loop system in this
section. In particular we show that the control law applied
in (14), with Adesigned as in Section IV, indeed steers the
formation, at least locally, to the desired formation motion.
Similar to (8) and (9), we derive from (14) the following
closed-loop system with the motion parameters
˙z=−cBTR(z)Te+BTAz (29)
˙e=−2cR(z)R(z)Te+ 2R(z)Az. (30)
IEEE TRANSACTIONS ON ROBOTICS 7
Note that as shown in Proposition 3.2, the error system (30) is
autonomous since the elements of R(z)Az ∈IR|E| are linear
combinations of the inner products zT
izj=gij (e). Therefore
we can write (30) as
˙e=−2cQ(e)Te+ 2f(e),(31)
with Q(e)as in (11) with l= 2 and f(e) = R(z)A(µ, ˜µ)z.
Note that f(e)is closely related to the desired shape Zsince
z∈ Z implies f(0) = 0|E|×1. However, in general z∈ D
does not necessarily imply f(0) = 0|E|×1since
R(z)A(µ, ˜µ)z=DT
zBTT(z)µ
˜µ
is designed to be zero only at z∈ Z but not at arbitrary z∈ D.
Theorem 5.1: There exist constants ρ, c∗>0such that for
system (31), e= 0 corresponding to z∈ Z with the motion
parameters [µv
˜µv]and [µω
˜µω]belonging to the spaces (21) and
(25) respectively is locally exponentially stable for all c≥c∗
in the compact set Q∆
={e:||e||2≤ρ}. In particular, the
formation will converge exponentially to the shape defined by
Zand the velocity b˙piwill converge exponentially to b˙p∗
ifor
i∈ {1,...,|V|}.
Proof:We divide the proof in two parts. Firstly we
analyze the inter-agent distance dynamics and show that the
shape formed eventually by the agents is the desired shape.
Secondly we analyze the individual agent dynamics in order
to show that they converge to the steady-state motion defined
by the motion parameters.
Consider the following candidate Lyapunov function
V=1
4||e||2,
with its time derivative given by
dV
dt=1
2eT˙e=−c eTQ(e)e+eTf(e).
As shown in Proposition 3.2, there exists a constant ρ > 0
such that Q(e)is positive definite in the compact set Q∆
={e:
||e||2≤ρ}since Q(0) is positive definite as the framework
is infinitesimally and minimally rigid at z∈ Z. Since f(e)is
real analytic with f(0) = 0|E|×1at z∈ Z because of (20) and
(24), it is also locally Lipschitz in Q. Therefore there exists a
constant q∈IR+such that
dV
dt≤ −cλmin||e||2+eTf(e)
≤(−cλmin +q)||e||2,(32)
where λmin is the minimum eigenvalue of Q(e)in Q. Thus if
one chooses c≥c∗>q
λmin , then the local exponential stability
of the origin of (31) follows, showing that the formation
converges exponentially to the desired shape defined by Z.
For the second part of the proof, we substitute e(t)→
0|E|×1and z(t)→ Z as t→ ∞ into (14), which gives us
˙p(t)−¯
A(µ, ˜µ)z(t)→0,as t→ ∞.
In other words, the velocity of the agents converges exponen-
tially to the desired velocities given by b˙p∗
cand b˙p∗
iωfor all i.
Example 5.2: As before, triangles and tetrahedrons can be
considered as particular cases. For triangles we can write
f(e) = R(z)Az =1
2F(µ, ˜µ)(e+d),
where d=d2
1d2
2d2
3Tand F(µ, ˜µ)∈IR3×3is given by
F(µ, ˜µ) = 2(µ1−˜µ1)+µ2−˜µ3µ2+˜µ3−µ2−˜µ3
−µ3−˜µ12(µ2−˜µ2)+µ3−˜µ1µ3+˜µ1
µ1+˜µ2−µ1−˜µ22(µ3−˜µ3)+µ1−˜µ2.
We would like to pinpoint two special cases. Firstly we notice
that for µω= 0 and ˜µω= 0 we have that BTAz is zero
in (29) since z1+z2+z3= 0. In this case, f(e) = 0 for
all e. Secondly, if Zdefines an equilateral triangle, then Fis
skew symmetric in view of Proposition 4.4. This means that
eTF e = 0 for all ein (32). Therefore both special cases have
q= 0, and hence for any c > 0the origin of (30) is locally
exponentially stable. As before, similar results can be easily
extended to tetrahedrons.
It is important to note that we are assuming that the
measurements done by the agents are not biased with respect to
each other, i.e. we do not consider any undesired mismatches
that will affect the performance of the motion controller. This
is still an open issue that could be addressed in future works
by exploiting the ideas contained in [15].
In the following section, we apply the motion parameters
to a more specific applications of formation motion control.
VI. AP PL IC ATIO NS O F EM PL OYI NG O NLY MOTI ON
PARA ME TE RS
The results from Theorem 5.1 allow one to design the
behaviour of the steady-state motion of the desired formation.
However, these results are restricted to the design of the
velocities b˙p∗
ifor all i, which are obviously defined in Ob.
In particular, for the case of having only translational motion,
i.e. [µω
˜µω] = 02|E|×1, the steady-state orientation of Obwith
respect to Ogis unknown and depends on the initial condition
p(0). Similarly, for the rotational motion around the centroid,
i.e. [µv
˜µv] = 02|E|×1, the steady-state position of the centroid
pcof the formation also depends on p(0).
Throughout the following two subsections, we are going
to extend the results of Theorem 5.1 in order to overcome
the issues mentioned above. The first extension is concerned
with the control of the steady-state orientation of Obwith
respect to Ogin the special case of having only a translational
motion. The second extension deals with the control of the
instantaneous center of rotation for the formation, which is
closely related to the problem of tracking and enclosing a
target as briefly mentioned in the Introduction.
A. Translational motion with controlled heading
Suppose that the motion parameters have been assigned
such that the steady-state motion of the formation defines
a pure translation, i.e. [µω
˜µω] = 02|E|×1. In this special case,
we are interested in controlling the steady-state orientation of
Obwith respect to Og, and consequently the heading of the
translational motion in Og. This task can be accomplished if
we control only one relative position among all the bzk’s with
IEEE TRANSACTIONS ON ROBOTICS 8
respect to Og. Without loss of generality we set the relative
position z1associated to edge E1= (1,2) for achieving this
task. Since z∗∈ Z defines a rigid formation, if we control
the orientation of z1then the orientation of the rest zk’s will
also be controlled. In other words, we are setting the desired
orientation of the formation in Ogwith the following relation
z∗
1=g
bRbz∗
1,(33)
where z∗
1has the orientation with respect to Oggiven by the
desired rotational matrix g
bR∈IRm×mdefining the steady-
state orientation of Obwith respect to Og.
Let us introduce the following potential function in order
to settle the orientation controller for z1implemented by the
agents in E1
V1o=1
2||z1−z∗
1||2,(34)
and define the alignment error
e1o=z1−z∗
1.
The orientation controller derived from the gradient of (34) is
∇p1V1o=−∇p2V1o=e1o.(35)
Note that we require agents 1and 2to know Og, since
they will need iz∗
1for the computation of (35) in Oi. Let
us introduce some definitions before writing in a compact
form the dynamics of the agents including the controller (35).
Define the orientation error vector
eo
∆
= col{e1o, . . . , eko} ∈ IRm|E|,
where eko =0m×1for all k6= 1, and the following augmented
incidence matrix
Ba=Bo02×(|E|−1)
0(|V|−2)×10(|V|−2)×(|E|−1)∈IR|V|×|E|,
where
Bo=1−1T,
describes the neighbor relationships for the alignment task and
Bahas been adjusted to have the same dimensions of B. Now
we can extend the control law in (14) including the controller
(35) in agents 1and 2as follows
u=c(−R(z)Te−Baeo) + Az. (36)
As we have done earlier, we derive the following closed-loop
system by substituting (36) into (1)
˙z=−c(BTR(z)Te−BTBaeo) + BTAz
˙e=−2c(R(z)R(z)Te−R(z)TBaeo)+2DT
zBTAz (37)
˙eo=BT
a˙p=−c(BT
aR(z)Te−BT
aBaeo) + BT
aAz. (38)
Without restricting the value for the desired velocity b˙p∗
c, we
introduce the following assumptions on Gand the construction
of b˙p∗
c.
Assumption 6.1: For 2D (3D) formations the subset of
agents H∆
={1,2,3,(4)}forms a complete subgraph Gh⊆G.
In addition for the desired shape z∗∈ Z the agents in H
form a non-degenerated triangle (tetrahedron). The desired
velocities b˙p∗
i=b˙p∗
cfor i∈ H are constructed employing
the motion parameters derived from only Gh.
Under Assumption 6.1, from Example 5.2 we have that
BT
aAz =0m|E|×1.(39)
The Assumption 6.1 is imposed only on the agents in Hin
order to make easier the following analysis. For the other
agents we construct their motion parameters µvand ˜µvwith
the only condition of satisfying (21).
We are ready now to present and prove the following
convergence result.
Theorem 6.2: Suppose that Aas in (15) is not zero and
its motion parameters [µ
˜µ]define a pure translational motion
and Assumption 6.1 is satisfied. Then, there exists constants
ρ, c∗>0such that the origin of the error systems (37) and (38)
are locally exponentially stable for all c≥c∗in the compact
set Q∆
={e, e1o:|| e
e1o||2≤ρ}. In particular, the formation
will converge exponentially to the shape defined by z∈ Z
with z1=z∗
1, and the velocity of the centroid ˙pcconverges
exponentially to g
bRb˙p∗
c, where g
bRis the desired rotational
matrix between Oband Ogas given in (33).
Proof:First we prove the convergence of the formation
to the desired shape Z. Consider the following candidate
Lyapunov function
V=1
4||e||2+1
2||eo||2,(40)
whose time derivative is
dV
dt=1
2eT˙e+eT
o˙eo=−ceTR(z)R(z)Te+eTR(z)Baeo
+eT
oBT
aR(z)Te+eT
oBT
aBaeo+eTf(e)
=−c||R(z)Te+Baeo||2+eTf(e),(41)
where f(e)is as in (31) and we have employed (39) under
the Assumption 6.1. Because of the zero elements of eoand
Ba, we can further rewrite (41) as
dV
dt=−ceTeT
1oQeTeT
1oT+eTf(e),(42)
where
Q="R(z)R(z)TR2m(z)Bo
BT
oR2m(z)TBT
oBo#,
with the matrix R2m(z)being the first 2mcolumns of R(z),
corresponding to the relative positions involving the agents 1
and 2. According to Theorem 3.1, the first term of (42) coming
from the gradient of a potential function is strictly negative in
the compact set Qfor some positive constant ρexcluding the
origins of eand e1o. Thus Qis positive definite in Q. Since
f(e)is locally Lipschitz with f(0) = 0|E|×1at z∈ Z there
exists a constant q∈IR+such that
dV
dt≤(−cλmin +q)e
e1o
2
,
where λmin is the minimum eigenvalue of Qin Q. Therefore
choosing c≥c∗>q
λmin implies that (40) is not increasing if
e(0), e1o(0) ∈ Q. Hence the origin of the system formed by
(37) and (38) is exponentially stable.
IEEE TRANSACTIONS ON ROBOTICS 9
For the second part of the proof, note that e1o(t)→0as
t→ ∞ implies that z1(t)→z∗
1as t→ ∞, and because
z(t)→ Z as t→ ∞, we have that
˙p(t)−1|V|×1⊗g
bRb˙p∗
c→0,as t→ ∞,
i.e., all agents converge to the same velocity ˙p∗
i=g
bRb˙pc.
Remark 6.3: If one does not want any steady-state move-
ment, i.e. A=0|V|×|E|, then the local exponential stability of
the desired aligned formation is guaranteed for any c > 0.
Remark 6.4: The constant qin the proof of Theorem 6.2 is
the same as that in the proof of Theorem 5.1. For triangular and
tetrahedron formations, the convergence to the desired shape
with the desired heading occurs for any c > 0since q= 0.
It seems reasonable to assign only one agent as a leader for
the orientation of the formation. In such a case the neighbor
relationships for the alignment are described by a directed
graph. In fact this directed graph consists of only two nodes
and therefore it is strongly connected. The stability analysis
can be worked out employing as a potential function the
one shown in [26] for strongly connected directed graphs in
substitution of the potential 1
2||eo||2in (40).
Note that the standard approach for the translational motion
of a rigid formation is to assign a velocity to a leader and
the other agents are equipped with local estimators in order
to track the velocity of the leader [5]. In the special case of
having the formation travelling at a constant speed, we have
shown in Theorem 6.2 that such estimators are not necessary,
reducing the complexity of the implementation of the control
law. In addition in our algorithm the order of the agents in the
formation is preserved with respect to the direction of motion.
Finally, in Theorem 6.2 we have shown that the convergence
to the desired velocity is exponential and not only asymptotic
as in the leader-follower case.
B. Target enclosing problem
The target enclosing problem addresses the scenario of
having a team of enclosing agents with the objective of
surrounding some specific independent target which is either
stationary or moving with an unknown velocity. Furthermore,
in orbiting missions, we not only require the agents to surround
the target but to circumnavigating it. Many work has been done
in this area such as [17], [27] among others. However they
usually do not allow the manipulation of the desired formation
consisting of the enclosing agents together with the target;
all of the agents are usually restricted to follow the same
circular trajectory around the target, which is at the center
of the circle. It is worth noting that in other works, such as
in [18], the complexity of the controller is much higher than
the one proposed in this paper using the motion parameters.
Furthermore, other drawbacks that are usually present in many
works are that all the enclosing agents have to measure their
relative positions or distances to the target, and the proposed
algorithms have only been tested in 2D scenarios.
It has been shown in Section IV that the instantaneous
center of rotation prfor the desired steady-state motion of
the formation is fixed in Og. Note that for ˙p∗
c=0m×1, i.e.
[µv
˜µv] = 02|E|×1, the instantaneous center of rotation pris
coincident with pc. However the steady-state of prin Theorem
5.1 depends on the initial condition p(0). In this section we
are going to associate prto the target to be enclosed, and
without loss of generality we set p1=pr. Furthermore, we
will assume that ˙p1= ˆv1∈IRmis constant and unknown to
the enclosing agents. Let us define
Bd
∆
=˜
IB,
where ˜
I|V|×|V| is obtained from the identity matrix setting its
first element to zero. With this operation we are setting all the
elements of the first row of Bdto zero. The interpretation for
Bdis that some of the enclosing agents are measuring their
relative positions with respect to the target, and that obviously
the target is not interacting with the rest of the formation.
In order to estimate the unknown ˆv1by the enclosing agents,
we will use an estimator ˆvifor each enclosing agent, whose
dynamics are
˙
ˆvi=−κ∇piV, i ∈ {2,...,|V|},(43)
where κ∈IR+is a constant gain and Vthe potential function
as used in the formation control. The estimator (43) can be
written for all the agents in the following compact form
˙
ˆv=−κBdDze, (44)
where ˆv∈IRm|V| is the stacked column vector of ˆvi’s. Note
that ˙
ˆv1=0m×1corresponds to the target, i.e. it does not have
any estimator.
Consider the following control law
u=−cBdDze+A(µ, ˜µ)z+ ˆv, (45)
where c∈IR+is a positive gain. Since all the elements of the
first row of Bdare zero, the first term of the control law in (45)
is not playing a role in the target’s position p1. The motion
parameters of A(µ, ˜µ)have been designed to have bp1=bpr
as the desired steady-state instantaneous center of rotation for
the desired shape z∈ Z. Recall that the desired infinitesimally
and minimally rigid formation includes p1and this is why
we can calculate the needed motion parameters. For example
if by design we set bp1=bpr=bpcthen [µv
˜µv] = 02|E|×1
and [µω
˜µω]is as shown in Section IV. In general, however, the
instantaneous center of rotation can be inside or outside of
the area or volume defined by the enclosing agents. Note that
since p1=pr, the motion parameters of the target are zero,
i.e. the elements of the first row of A(µ, ˜µ)are zero. Therefore
the second term in (45) is not playing any role in the dynamics
of p1. This is an obvious requirement since the target is not
collaborating with the rest of the enclosing agents.
Our approach has two obvious limitations. Firstly we require
to have more than two (three) agents including the target in 2D
(3D) in order to form an infinitesimally and minimally rigid
framework. Secondly, because of the definition of infinitesi-
mally rigid framework, the desired shape including the target
must exclude the ones where all the agents are collinear or
coplanar in 2D or 3D respectively.
Define the error velocity as
ev= ˆv−(1|V|×1⊗ˆv1).
IEEE TRANSACTIONS ON ROBOTICS 10
Note that the first melements of evare zero, therefore
BTev=BT
dev,(46)
and in addition one can check the following identity as well
BTBd=BT˜
IB =BT˜
IT˜
IB =BT
dBd.(47)
Before stating the main result of this section, let us write
down the closed-loop dynamics by substituting (45) in (1)
˙z=BT˙p=−cBT
dBdDze+BTA(µ, ˜µ)z+BT
dev(48)
˙e= 2DT
z˙z=−2cDT
zBT
dBdDze+ 2f(e)+2DT
zBT
dev
(49)
˙ev=˙
ˆv=κBdDze, (50)
where we have employed the identities (46) and (47), f(e)is
the same as in (31), and for the third term of (48) we have
used the following well known fact
BT(1|V|×1⊗ˆv1) = BT
d(1|V|×1⊗ˆv1) = 0m|E|×1.(51)
Theorem 6.5: Suppose that the steady-state motion of the
rigid formation bz∗∈ Z defined by the motion parameters µ
and ˜µhas bp∗
1as the instantaneous center of rotation. Suppose
that agent 1is a free target with constant velocity ˆv1which
is unknown to the rest of the enclosing agents. Then there
exist constants ρ, c∗>0such that the origins of (49) and
(50) are locally asymptotically stable in the compact set Q∆
=
{e, ev:|| e
ev||2≤ρ}. In particular, the formation will converge
asymptotically to the shape defined by Z, and the motion of
the enclosing agents converge to a rotation around the target
with angular speed ||ω|| =||b˙p∗
i||
||bp∗
1−bp∗
i||, i ∈ {2,...,|V|}.
Proof:Consider the following candidate Lyapunov func-
tion
V=κ
4||e||2+1
2||ev||2,
where κis as in (43), with time derivative given by
dV
dt=κ
2eT˙e+eT
v˙ev
=−κceTDT
zBT
dBdDze+κeTf(e) + κeTDT
zBT
dev
−κeT
vBdDze
≤κ(−cλmin +q)||e||2,(52)
where f(e)is as in (31) and qis obtained from f(e)since it
is locally Lipschitz with f(0) = 0|E|×1if z∈ Z. Note that
M(e) = DT
zBT
dBdDzis not defined by the rigidity matrix
R(z)because of Bd. Nevertheless it can be checked that
M(0) is still positive definite if for z∈ Z the framework is
infinitesimally and minimally rigid. Therefore M(e)is positive
definite for some ρ≤ρ1∈R+in the compact set Q, and then
λmin in (52) is the minimum eigenvalue of M(e)in Q. If one
chooses c≥c∗>q
λmin , then the time derivative (52) is not
increasing, and it is zero if and only if e= 0. For such a case,
we can conclude that the shape of the formation converges
to z∗∈ Z, which means that the steady-state motion of the
agents will not distort the desired shape. Thus following a
similar argument as in Section IV, the largest invariant set of
the closed-loop system (48), (49) and (50) with e= 0 and
z∗∈ Z is given by
T∆
=nˆv:DT
z∗BTS1Dz∗µ+S2Dz∗˜µ+ ˆv= 0o.(53)
Since ˆv1in ˆvis fixed, only two steady-state motions for the
agents are possible when ˆv∈ T =Tu∪ Td. In the set Tuall
the agents travel with the same velocity ˙pi= ˆv1and hence
||ev||2=ρ2∈R+. In the set Tdthe enclosing agents rotate
around p1with ||ev|| = 0. Therefore invoking the LaSalle’s
invariance principle, we have that the origins of evand e
defining z∈ Z are locally asymptotically stable in Qwith
ρ < min{ρ1, ρ2}and
˙p(t)−A(µ, ˜µ)z(t)−(1|V|×1⊗ˆv1)→0, t → ∞,(54)
which defines the steady-state rotational movement of the
enclosing agents around the target. The steady-state angular
speed of the enclosing agent iaround the target is its speed
over the radius of the rotation. Since the target is moving with
velocity ˆv1we have to compensate it in the following formula
||ω|| =||˙p∗
i−ˆv1||
||p∗
1−p∗
i|| =||b˙p∗
i||
||bp∗
1−bp∗
i||, i ∈ {2,...,|V|},(55)
where we have employed (54).
Remark 6.6: For the triangular formation consisting of three
agents, one being the target and the other two the enclosing
ones, if Zdefines an equilateral triangle, then we have that
eTf(e) = eTF e = 0 since Fis skew symmetric in view of
Proposition 4.4. Therefore one can choose any c > 0since
q= 0 in (52). Also note that qis the same constant in the
proofs of Theorems 6.5 and 5.1.
Remark 6.7: The minimum number of enclosing agents
sensing their relative positions with respect to the target is
the same as the minimum number of edges needed for a node
belonging to an infinitesimally and minimally rigid framework.
Therefore for planar frameworks the number is two and for
3D frameworks the number is three.
Remark 6.8: The estimation of the constant velocity of the
target is based on the internal model principle as it has been
used in [5]. This implies that the velocity of the target might
be also driven by a superposition of finite sinusoidal signals
with known frequency but unknown phase and amplitude. In
such a case, the dynamics of the estimator (44) have to be
modified in order to include such frequencies as it has been
shown in [5] or [15].
Remark 6.9: The enclosing agents employ their own local
coordinate system in view of Lemma 4.1. Therefore each
enclosing agent iestimates the velocity of the target ˆv1in
Oi.
Since ˆv1is unknown, the conservative region of attraction
Qis difficult to establish or estimate. Nevertheless we can
avoid the undesired equilibrium set Tu⊂ T as in (53) when
ρ2≤ρ≤ρ1. Firstly note that the constants ρ1and ρ2are
mainly determined by p(0) and ˆv(0) respectively. Secondly we
know that Tudetermines a pure translation for all the agents.
Suppose that the enclosing agents can measure the angular
velocity iω, e.g. with a gyroscope. If ˆv(t)converges to Tu,
then iωwill converge to zero as fast as z(t)approaches Z.
IEEE TRANSACTIONS ON ROBOTICS 11
Fig. 3: Four wheeled E-pucks with data-matrices as markers
on their tops. The code in each data-matrix corresponds to the
node number for the robot in the sensing graph.
Furthermore, uifor all iwill converge to ˆv1. Therefore, we can
program the agents such that when iωis almost zero at t=t∗,
their estimators are reset to ˆvi(t∗) = ui(t∗). Since z(t∗)is very
close to Z, then certainly the errors e(t∗),ev(t∗)∈ Q with
ρ<ρ2.
In the case that we do not require the enclosing agents to
rotate around the target, we can combine the results of the
Theorems 6.2 and 6.5. The result of such combination is: the
enclosing agents estimate the unknown velocity of the target;
the enclosing agents form a rigid formation together with the
target, whose orientation with respect to the target can be
determined by a leader. Furthermore in such a special case
we have that A=0|V|×|E|, and therefore q= 0 in Theorems
6.2 and 6.5.
We finish by emphasizing again that the results presented
here do not need measuring any relative velocity, and not all
of the enclosing agents need to measure their relative positions
with respect to the target agent.
VII. EXP ER IM EN TAL RE SU LTS
In this section we validate and test the performances of the
results in Theorems 6.2 and 6.5 using E-puck mobile robots
[28]. The experimental setup consists of four wheeled E-puck
robots in a 2D area of about 2.6×2meters. Each robot is
tagged by a data-matrix on its top as shown in Figure 3. Since
E-pucks are unicycles, we apply feedback linearization about a
reference point to obtain the single-integrator dynamics in (1).
In practice we control the formation of the reference points of
the robots. We set as a reference point to the intersection of
the two solid bars in the data matrix. We simulate the robots’
on-board sensors for the relative positions with a PC. The
computer runs a real time process for the computation of the
relative positions between the robots with a vision algorithm,
and sends the data back to the robots at a frequency of about
20Hz via Bluetooth. The robots then apply the corresponding
distributed algorithm. The whole image of the testing area is
covered by 1280 ×720 pixels, where the distance between
two consecutive horizontal or vertical pixels corresponds to
approximately to 2.0mm. The raw footage of the experiments
reported in this paper can be found in the playlist Robotics at
https://www.youtube.com/c/HectorGarciadeMarina .
A. Translational motion with controlled heading experiment
In this experiment we will validate the results from Theorem
6.2. We consider four robots that have a square with 225 pixels
(45.0mm) for the length of the side as a prescribed shape
1
√2a
a
2
a
√2a
3
a
4
Fig. 4: Desired shape for the three pursuers together with the
target. The target corresponds to the node number 1and ais
a scale factor.
Z. The incidence matrix describing the sensing topology for
maintaining the prescribed shape and the derived A(µ, ˜µ)are
B=
1 0 −1 1 0
−1 1 0 0 0
0−1 1 0 1
0 0 0 −1−1
, A =
µ10 ˜µ3µ40
˜µ1µ20 0 0
0 ˜µ2µ30µ5
0 0 0 ˜µ4˜µ5
.
We designate the robot number 1to be the agent that con-
trols the orientation of z1, which is associated to the edge
E1= (1,2). We take l= 1 for the potential function (3). We
implement the control law (36) but noting that now D˜zis not
constant and equal to the identity matrix anymore. The chosen
speed for the translation is 5pixels/s (1cm/s), and we set the
desired heading of the translational motion to be the same as
the one defined by the direction of bz1. Because of simplicity
and in order to show that Assumption 6.1 is only a sufficient
condition, for achieving the mentioned desired steady-state
velocity we set the motion parameters as
µ= [ −50005]T,˜µ= [ −50005]T.(56)
At the beginning of the experiment robot 1has for the desired
orientation the vector z∗
1=225 0Tpixels. Therefore the
desired heading of the formation is 0radians with respect to
Og. The robots start in random positions close to the origin
of Og. During the experiment, once the agent 1reaches the
imaginary vertical line crossing the position 1100 0Tpix-
els, the value of z∗
1changes to −225 0Tpixels. Therefore
the formation will switch to a heading of πradians with respect
to Og. The gain cin (36) has been set to 0.035. As a remark
the chosen cis smaller than the conservative c∗in Theorem
6.2. The experimental results are discussed in Figure 5.
Remark 7.1: Since all the unicycle robots have the same
reference point for the feedback linearization, once the system
is at e= 0 and eo= 0, i.e. all the robots have the same velocity
˙p∗
c, the headings of the robots achieve consensus, i.e. all the
robots are pointing at the same direction.
B. Enclosing and tracking a target experiment
We consider the scenario of having three pursuers and
one independent target, set as agent 1, with constant velocity
ˆv1=−3 0.35Tpixels/s. The velocity ˆv1is unknown for
the three pursuers. We ask the pursuers to form the shape
in Figure 4 together with the target. The incidence matrix B
corresponding to the sensing topology for the shape in Figure
4 and the derived matrix A(µ, ˜µ)are
IEEE TRANSACTIONS ON ROBOTICS 12
B=
1 0 −1 0 0
−1 1 0 −1 0
0−1 1 0 −1
00011
, A =
˜µ10 ˜µ30 0
µ1µ20 ˜µ40
0 ˜µ2µ30 ˜µ5
0 0 0 µ4µ5
.
We ask the pursuers to maintain the following set of inter-
agent distances
d=√2aaa√2a aT,(57)
where a= 130 pixels. We choose l= 1 for the potential (3)
and implement the controls (44) and (45) to the robots. Note
that again since l= 1, we do not have D˜zbeing constant
and equal to the identity anymore. The motion parameters for
orbiting around the target p1are
µ= [ 0 0 0 −2aγ√2 2aγ ]T,˜µ= [ 0aγ 0−aγ √2 0 ]T,(58)
where γ∈R. Note that µ1and ˜µ3are zero since they
correspond to the target. The agents 2and 3are only using ˜µ4
and ˜µ2respectively since z∗
4and z∗
2are perpendicular to z∗
1.
We set the desired steady-state angular speed to 0.038 rads/s,
therefore from (55) the value of γis
||ω∗|| =||b˙p∗
3||
||bp∗
1−bp∗
3|| =aγ
a=γ= 0.038rads/s.
We have set the control gains κand cto 0.01 and 0.1
respectively in (44) and (45). The gain κhas been chosen
small in order to prevent saturation in the robots. The chosen
cis smaller than c∗in Q, showing the conservative result of c∗
in Theorem 6.5. The initial values ˆvi(0) for i∈ {2,3,4}of the
estimators have been set to zero. The experimental results are
discussed in Figure 6. Note that despiting of the local result in
Theorem 6.5, the initial conditions p(0) and ˆv(0) have been
set relatively far away from the desired equilibrium.
VIII. CONCLUSIONS
In this paper we have presented a distributed motion con-
troller for rigid formations, which is compatible with the
popular gradient-based controllers. The proposed controller
allows us to design the steady-state formation motion in the
desired shape with respect to a frame attached to the rigid
formation. The steady-state velocity vector can be decomposed
into two independent terms: one for the constant translation
of the centroid of the formation and the other for a constant
rotational formation motion around its centroid. Such design
has been achieved by motion parameters in the prescribed dis-
tances between the agents. Two main applications based on the
motion controller have been presented: the first is controlling
the heading of a pure translational formation motion and the
second is addressing the problem of enclosing and tracking
a target. Experiments with mobile robots in IR2have been
presented in order to validate the proposed applications. We
are currently working on extending the mathematical system
model to accommodate agent dynamics modeled by double-
integrators. We are also interested in testing our presented
results in 3D scenarios involving flying drones.
(a) Time t= 0 secs. (b) Time t= 90 secs.
(c) Time t= 195 secs. (d) Time t= 283 secs.
0 50 100 150 200 250 300
−200
−100
0
100
200
300
400
500
Time [s]
Error [pixels]
Orientation Error
Distance Errors
Orientation of edge 1
changed to π rads
(e)
0 50 100 150 200 250 300
0
5
10
15
20
25
Time [s]
Robots’ speed [pixels/s]
Speed 1
Speed 2
Speed 3
Speed 4
Orientation of edge 1
changed to π rads
(f)
Fig. 5: Experimental results of a 2D squared formation travel-
ing with desired heading and speed using our proposed control
law (36) with c= 3.5×10−2. Note that since we have set l= 1
in (3), then D˜zis not constant and equal to the identity matrix
anymore and it should be included as in (7). The designed
motion parameters have been set in (56). The initial positions
of the robots are shown in (a) and the squared configuration
of the formation with 0rads of heading after 90 seconds is
shown in (b). The error and speed plots corresponding to the
experiment are shown in (e) and (f). The desired speed of 5
pixels/s (1cm/s), is achieved in a short time as it is shown
in (f). The orientation leader is marked with a red dot in the
video captures, as well as its trajectory. The relative position
bz1corresponding to the edge E1= (1,2) has been also
marked in red, whereas the rest of sensed relative positions
have been marked in green. The desired orientation for z∗
1
at the beginning is 0rads, so Ogand Obhave the same
orientation. The steady-state velocity b˙p∗
chas been designed
to have the same direction as bz1. Therefore the steady-state
translation of the formation is heading to the right in (b). The
norm of the orientation error eoits shown in (e) in red color.
At the time t= 186 secs, the orientation leader changes the
desired orientation of z∗
1to πrads, i.e. horizontal motion to
the left as it can be noticed in (c) and (d). The video capture
(c) shows the rearranging of the agents for the new desired
orientation. Finally in (d) at time t= 283 secs it is shown
that the formation achieves the new desired heading of πrads
along with the desired speed of 5pixels/s as it is shown in
(f). Note that Obis only defined once the square is formed.
IEEE TRANSACTIONS ON ROBOTICS 13
(a) Time t= 0 secs. (b) Time t= 68 secs.
(c) Time t= 135 secs. (d) Time t= 335 secs.
0 50 100 150 200 250 300 350 400
−6
−4
−2
0
2
4
6
8
Time [s]
Velocity [pixels/s]
Target’s Vx
Pursuers’
estimation
(e)
0 50 100 150 200 250 300 350 400
−3
−2
−1
0
1
2
3
4
Time [s]
Velocity [pixels/s]
Target’s Vy
Pursuers’
estimation
(f)
0 50 100 150 200 250 300 350 400
−200
0
200
400
600
800
1000
Time [s]
Error [pixels]
Distance Errors
(g)
0 50 100 150 200 250 300 350 400
0
0.05
0.1
0.15
0.2
0.25
Time [s]
Angular speed [rad/s]
ω*
ω1
ω2
ω3
(h)
Fig. 6: Experimental results of a team of three pursuers
tracking and enclosing an independent target. The pursuers
start at the left of the scene whereas the target starts at the right
in (a). The pursuers are tagged with blue dots and the target
with a red dot as well as their trajectories. The sensed relative
positions are marked with colors, the pink ones concern the
target and the green ones concern only the pursuers. Note
that there is a pursuer that does not track the target at all.
The pursuers have two tasks. The first one is to form the
prescribed shape with the target as the one shown in Figure 4
with the side lengths defined in (57). The second one is to orbit
around the target with an angular speed ||ω∗|| = 0.0308 rads/s.
The designed motion parameters employed for satisfying such
requirements are in (58). In addition the target travels with
a constant velocity ˆv1= [−3 0.35]Tpixels/sec, which is
unknown to the pursuers. We implement to the pursuers the
control laws (44) and (45) with l= 1 for the potential (3),
therefore D˜zis not constant anymore. We have set the control
gains cand κto 1×10−1and 1×10−2respectively in (45)
and (44). The gain κhas been set low in order to prevent the
E-pucks to do not reach the saturation level in their velocities.
We show the estimation of the two components of ˆv1by the
pursuers in (e) and (f). The asymptotic convergence of the
estimated vis slow due to the low values of κand c. The
convergence of the distance errors to zero in (g) with the
estimation of ˆv1, lead for the convergence of the angular speed
of the pursuers around the target to ||ω∗|| as it is shown in
(h). It is shown from (b) to (d) how the pursuers enclose the
target and circumnavigate it with the prescribed shape shown
in Figure 4.
REFERENCES
[1] Y. Lan, G. Yan, and Z. Lin, “Distributed control of cooperative target
enclosing based on reachability and invariance analysis,” Systems and
Control Letters, vol. 59, no. 7, pp. 381 – 389, 2010.
[2] J. Yuan, Y. Huang, T. Tao, and F. Sun, “A cooperative approach for multi-
robot area exploration,” in Proc. of IEEE International Conference on
Intelligent Robots and Systems (IROS), 2010, pp. 1390–1395.
[3] 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.
[4] E.Vos, J. Scherpen, and A. van der Schaft, “Port-hamiltonian approach
to deployment,” in Proc. of International Symposium on Mathematical
Theory of Networks and Systems, 2012.
[5] H. Bai, M. Arcak, and J. Wen, Cooperative Control Design: A System-
atic, Passivity-Based Approach. New York: Springer, 2011.
[6] R. Skjetne, S. Moi, T. Fossen et al., “Nonlinear formation control of
marine craft,” in Decision and Control, 2002, Proceedings of the 41st
IEEE Conference on, vol. 2. IEEE, 2002, pp. 1699–1704.
[7] G. Antonelli, F. Arrichiello, and S. Chiaverini, “Experiments of for-
mation control with multirobot systems using the null-space-based
behavioral control,” Control Systems Technology, IEEE Transactions on,
vol. 17, no. 5, pp. 1173–1182, 2009.
[8] R. Olfati-Saber and R. M. Murray, “Graph rigidity and distributed
formation stabilization of multi-vehicle systems,” in Proc. of the 41st
IEEE CDC, 2002, pp. 2965–2971.
[9] 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.
[10] 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.
[11] M. Cao, C. Yu, and B. D. O. Anderson, “Formation control using range-
only measurements,” Automatica, vol. 47, pp. 776–781, 2011.
[12] 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.
[13] S. Mou, A. S. Morse, A. Belabbas, Z. Sun, and B. Anderson, “Undi-
rected rigid formations are problematic,” IEEE Transactions on Auto-
matic Control., 2015.
[14] U. Helmke, S. Mou, Z. Sun, and B. D. O. Anderson, “Geometrical
methods for mismatched formation control,” in Proc. of the 53rd
Conference on Decision and Control, 2014, pp. 1341–1346.
[15] H. Garcia de Marina, M. Cao, and B. Jayawardhana, “Controlling
rigid formations of mobile agents under inconsistent measurements,”
Robotics, IEEE Transactions on, vol. 31, no. 1, pp. 31–39, Feb 2015.
[16] Z. Sun, S. Mou, B. Anderson, and A. Morse, “Formation movements in
minimally rigid formation control with mismatched mutual distances,”
in Proc of the 53rd Conference on Decision and Control, 2014.
[17] J. Guo, G. Yan, and Z. Lin, “Local control strategy for moving-target-
enclosing under dynamically changing network topology,” Systems &
Control Letters, vol. 59, no. 10, pp. 654 – 661, 2010.
[18] A. Marasco, S. Givigi, and C. Rabbath, “Model predictive control for
the dynamic encirclement of a target,” in American Control Conference
(ACC), 2012, June 2012, pp. 2004–2009.
[19] L. Asimow and B. Roth, “The rigidity of graphs, ii,” Journal of
Mathematical Analysis and Applications, pp. 171–190, 1979.
[20] 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.
[21] Henneberg, “Die graphische statik der starren systeme,” Leipzig, 1911.
[22] P. Absil and K. Kurdyka, “On the stable equilibrium points of gradient
systems,” Systems & Control Letters, pp. 573–577, 2006.
[23] K. Oh and H. Ahn, “Distance-based undirected formation of single-
integrator and double-integrator modeled agents in n-dimensional space,”
International Journal of Robust and Nonlinear Control, pp. 1809–1820,
2014.
[24] Y. Tian and Q. Wang, “Global stabilization of rigid formations in the
plane,” Automatica, vol. 49, pp. 1436–1441, 2013.
[25] H. Goldstein, Classical Mechanics. Addison-wesley, 1951.
[26] N. Chopra, “Output synchronization on strongly connected graphs,”
Automatic Control, IEEE Transactions on, vol. 57, no. 11, pp. 2896–
2901, Nov 2012.
[27] I. Shames, S. Dasgupta, B. Fidan, and B. Anderson, “Circumnavigation
using distance measurements under slow drift,” Automatic Control, IEEE
Transactions on, vol. 57, no. 4, pp. 889–903, April 2012.
IEEE TRANSACTIONS ON ROBOTICS 14
[28] F. Mondada, M. Bonani, X. Raemy et al., “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.
Hector G. de Marina received the M.Sc. degree in
electronics engineering from Complutense Univer-
sity of Madrid, Madrid, Spain in 2008, the M.Sc.
degree in control engineering from the University
of Alcala de Henares, Alcala de Henares, Spain in
2011, and the Ph.D. degree in applied mathemat-
ics from the University of Groningen, the Nether-
lands. He is currently a postdoctoral researcher in
the drones group at the School of Civil Aviation
(ENAC), Toulouse, France. His research interests
include formation control and navigation for au-
tonomous robots.
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. He 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.
Ming Cao received the PhD degree in electrical
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 inter-
est is in autonomous agents and multi-agent systems,
mobile sensor networks and complex networks. He
is an associate editor for IEEE Transactions on
Circuits and Systems II,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.
