Automatica 45 (2009) 2754–2762

Contents lists available at ScienceDirect

Automatica

journal homepage: www.elsevier.com/locate/automatica

Global and robust formation-shape stabilization of relative sensing networksI

Jorge Cortés ∗Department of Mechanical and Aerospace Engineering, University of California, 9500 Gilman Drive, San Diego, United States

a r t i c l e i n f o

Article history:Received 9 November 2008Received in revised form8 June 2009Accepted 17 August 2009Available online 7 October 2009

Keywords:Multi-agent systemsDistributed algorithmsFormation controlMultidimensional scalingLinear iterations

a b s t r a c t

This paper proposes a simple, distributed algorithm that achieves global stabilization of formations forrelative sensing networks in arbitrary dimensions with fixed topology. Assuming the network runs aninitialization procedure to equally orient all agent reference frames, convergence to the desired formationshape is guaranteed even in partially asynchronous settings. We characterize the algorithm robustnessagainst several sources of errors: link failures, measurement errors, and frame initialization errors. Thetechnical approach combines algebraic graph theory, multidimensional scaling, and distributed lineariterations.

© 2009 Elsevier Ltd. All rights reserved.

1. Introduction

This paper proposes a distributed algorithm for relative sens-ing networks to achieve formation shape stabilization. A relativesensing network consists of a group of agents, each with its ownreference frame, that can sense the relative position of their neigh-bors. The algorithm assumes that the interaction topology remainsfixed and guarantees that the network shape converges to the de-sired formation, starting from any initial configuration.

1.1. Literature review

There is a large body of work on formation control in the multi-agent systems literature. A wide range of issues have been ad-dressed, including pattern formation, stability, and merging, seee.g., Egerstedt and Hu (2001), Justh and Krishnaprasad (2004),Smith and Hadaegh (2007) and Tanner, Pappas, and Kumar (2004)for a very small sample of works. Numerous continuous-time for-mation control strategies employ algebraic graph-theoretic tools,see e.g., Fax and Murray (2004), Lafferriere, Williams, Caughman,and Veerman (2005) and Ren (2007). The work by Ji and Egerst-edt (2007) proposes hybrid rendezvous-to-formation control

I Research supported by NSF CAREER ECS-0546871 and NSF CMMI-0908508awards. The material in this paper was partially presented at the AmericanControl Conference, St. Louis, Missouri, USA, June 10-12, 2009. This paper wasrecommended for publication in revised form by Associate Editor Zhihua Qu, underthe direction of Editor Andrew R. Teel.∗ Tel.: +1 858 822 7930; fax: +1 858 822 3107.E-mail address: cortes@ucsd.edu.

0005-1098/$ – see front matter© 2009 Elsevier Ltd. All rights reserved.doi:10.1016/j.automatica.2009.09.019

strategies under state-dependent interaction topologies. Theworks by Dörfler (2008), Krick, Broucke, and Francis (2009), Olfati-Saber and Murray (2002) and Yu, Anderson, Dasgupta, and Fidan(2009) use graph rigidity ideas to achieve formation shape stabi-lization on the plane. However, in the rigidity approach, the desiredformation shape is in general only locally stable (e.g., collinear net-work configurations are invariant and additional undesired locallystable equilibria exist). Another source of inspiration for this workis the literature on graph drawing (Kamada & Kawai, 1989), mul-tidimensional scaling (Borg & Groenen, 1997), and iterative ma-jorization (Gotsman & Koren, 2005), where the design of globaloptimization algorithms that overcome the local stability proper-ties of the desired configurations is a topic of vigorous research.Finally, groups of agents with only relative information about eachother’s state are considered by Lin, Francis, and Maggiore (2005),Bullo, Cortés, Martínez, and Chung (2009) and Piovan, Shames, Fi-dan, Bullo, and Anderson (2008).

1.2. Statement of contributions

The main contribution of the paper is a simple, distributed co-ordination algorithm that stabilizes the shape of a relative sensingnetwork to a desired formation. In contrast to previous work, thedesired formation is not encoded using inter-agent distances andassuming that the interaction topology is rigid. Instead, in d ∈ Z>0dimensions, and assuming that the interaction topologyhas at leasta globally reachable vertex,we encode the desired formation by as-signing to each agent a vector in Rd. For each agent, this vector isa weighted sum of the neighbors’ relative positions in the desiredformation. The proposed strategy runs in discrete time and works

J. Cortés / Automatica 45 (2009) 2754–2762 2755

in arbitrary dimensions. The formation control objective can be en-coded by means of the global minimization of the stress functionassociated to the network. Our algorithmic design builds on addi-tional contributions regarding the majorization of the stress func-tion and its critical pointswhen the interaction graph is directed. Inparticular, we show how the critical points can be characterized asthe solutions to a sparse linear equation whose elements are com-putable in a distributedway over the interaction graph, in both theundirected and the directed cases. The coordination strategy thenresults from thedesign of a Jacobi overrelaxation algorithm to solvethe linear equation. We characterize the global convergence prop-erties of the algorithmaswell as its descent propertieswith regardsto the stressmajorization function.We also establish the algorithmrobustness against several sources of error: when agents fail to ac-quire the relative position of neighboring agents, when there aremeasurement errors in the relative position information of neigh-boring agents, andwhen there are errors in the initial computationof the common reference frame of the individual agents.

1.3. Organization

Section 2 introduces basic notions from graph theory and dis-tributed linear iterations. Section 3 states the formation controlproblem and the relative sensing network model. Section 4 devel-ops several results on the stress function from scaling theory, withspecial emphasis on the directed graph case. Section 5 presents thecoordination algorithm design and analyzes its convergence prop-erties. Section 6 characterizes the algorithm robustness againstseveral sources of error. Section 7 gathers our conclusions andideas for future work. The appendix presents the proof of someauxiliary results.

1.4. Notation

We letR,R>0, andR≥0 denote the set of reals, positive reals, andnonnegative reals, respectively. We denote by ‖ · ‖∞ the∞-normin the Euclidean space, and we denote the ∞-distance betweena point p ∈ Rd and a set S ⊂ Rd by dist∞(p, S). With a slightabuse of notation, we do not distinguish between a vector P =(p1, . . . , pn) ∈ (Rd)n and the matrix P ∈ Rn×d whose ith row ispi. We let Id ∈ Rd×d denote the identity matrix and 1d ∈ Rd denotethe vector whose entries are all 1. For A1, . . . , An ∈ Rd×d, we letdiag (A1, . . . , An) ∈ Rdn×dn denote the block-diagonal matrix thathas A1, . . . , An in the diagonal. Given A ∈ Rd1×d2 and B ∈ Re1×e2 ,we let A⊗ B ∈ Rd1e1×d2e2 denote its Kronecker product. For squarematrices A ∈ Rd×d, B ∈ Re×e, the eigenvalues of A ⊗ B are theproduct of the eigenvalues of A and B. The Cartesian product ofmaps f1 : X1 → Y1 and f2 : X2 → Y2 is f1× f2 : X1× X2 → Y1× Y2,(f1×f2)(x1, x2) = (f1(x1), f2(x2)). For brevity, the Cartesianproductof f1, . . . , fm is denoted Πmk=1fk. Finally, we let πα : Rd → R,α ∈ {1, . . . , d}, denote the projection of a vector onto its αth-component.

2. Preliminary developments

Here we introduce basic notions on kinematic motions,algebraic graph theory, and linear iterations. For further details onthese topics, we refer to Bertsekas and Tsitsiklis (1997), Bullo et al.(2009) and Cormen, Leiserson, Rivest and Stein (2001).

2.1. Fixed and body reference frames

Let Σfixed = (pfixed, {x1fixed, . . . , xdfixed}) be a fixed reference

frame in Rd and let Σb = (pb, {x1b, . . . , xnb}) be a reference frame

fixed with a moving body. A point q and a vector v expressed withrespect to the framesΣfixed andΣb are denoted by qfixed and qb, andvfixed and vb, respectively. The origin ofΣb is the point pb, denotedby pfixedb when expressed with respect to Σfixed. The orientation of

Σb is characterized by the rotation matrix Rfixedb ∈ SO(d), whosecolumns are the frame vectors {x1b, . . . , x

db} of Σb expressed with

respect toΣfixed. With this notation, changes of frames read

qfixed = Rfixedb qb + pfixedb , (1a)

vfixed = Rfixedb vb. (1b)

2.2. Graph-theoretic notions

A directed graph (or digraph) G = (V, E) of order n consistsof a vertex set V with n elements, and an edge set E ⊂ V × V .For simplicity, we take V = {1, . . . , n}. A digraph is undirected if(j, i) ∈ E anytime (i, j) ∈ E . In a digraph Gwith an edge (i, j) ∈ E ,i is called an in-neighbor of j, and j is called an out-neighbor of i. Adirected path in a digraph is an ordered sequence of vertices suchthat any two consecutive vertices in the sequence are an edge ofthe digraph. A vertex of a digraph is globally reachable if it can bereached from any other vertex by traversing a directed path. Anundirected graph is connected if there exists a path between anytwo vertices. For an undirected graph, this notion is equivalent tothe graph having a globally reachable vertex.A weighted digraph is a triplet G = (V, E,A) where (V, E) is a

digraph and where A is an n × n weighted adjacency matrix withthe following properties: for i, j ∈ {1, . . . , n}, the entry aij > 0 if(i, j) is an edge of G, and aij = 0 otherwise. A weighted digraph isundirected if aij = aji for all i, j ∈ {1, . . . , n}. When convenient, wewriteA(G) tomake clear the explicit graph dependence. A digraphG = (V, E) can be naturally thought of as a weighted digraph bydefining entries aij = 1 if (i, j) is an edge ofG, and aij = 0otherwise.Reciprocally, one can define the unweighted version of a weighteddigraph (V, E,A) by simply considering the digraph (V, E).The weighted out- and in-degree matrices are the diagonal

matrices Dout(G) = diag (A1n) and Din(G) = diag(AT1n

). A

weighted digraph is regular if there exists a ∈ R>0 such thatDout(G) = Din(G) = a In. If G is undirected, we denote D(G) =Dout(G) = Din(G). The graph Laplacian of the weighted digraph G is

L(G) = Dout(G)−A(G).

Note that L(G)1n = 0, and thatG is undirected iff L(G) is symmetric.For undirected graphs, the Laplacian is a symmetric, positivesemidefinite matrix. The Laplacian also captures the connectivityproperties of the graph: L(G) has rank n − 1 if and only if G hasa globally reachable vertex. The following result states a usefulproperty.

Lemma 2.1. For any connected undirected weighted graph G, thematrix 2D(G)−1 − hD(G)−1L(G)D(G)−1 is positive definite for h ∈(0, 1).

The proof of this result is given in the Appendix. Next, we definereverse and mirror digraphs. Let E be the set of edges obtained byreversing the order of all pairs in E . The reverse digraph G of G is(V, E). Observe

A(G) = A(G)T,L(G) = Dout(G)−A(G) = Din(G)−A(G)T.

In general, L(G) 6= L(G)T. The mirror digraph G of G is (V, E ∪ E)with

A(G) =12(A(G)+A(G)T) = Sym(A(G)),

L(G) =12(L(G)+ L(G)).

The mirror graph is undirected.

2756 J. Cortés / Automatica 45 (2009) 2754–2762

Lemma 2.2. Given a weighted digraph G and x, y ∈ Rn,

12

∑(i,j)∈E

aij(xi − xj)(yi − yj) = yTL(G)x.

The proof of this result is given in the Appendix.

2.3. Jacobi overrelaxation iteration

Given an invertible matrix A ∈ Rn×n and a vector b ∈ Rn,consider the linear system Ax = b. The Jacobi overrelaxation (JOR)algorithm is an iterative procedure to compute the unique solutionx = A−1b ∈ Rn. It is formulated as the discrete-time dynamicalsystem

xi(`+ 1) = (1− h)xi(`)− h1aii

(∑j6=i

aijxj(`)− bi),

with ` ∈ Z≥0, i ∈ {1, . . . , n}, x(0) ∈ Rn, and h ∈ (0, 1).The convergence properties of the JOR algorithm can be fullycharacterized in terms of the eigenvalues of the matrix describingthe linear iteration, see Bertsekas and Tsitsiklis (1997).Given a digraph G, as long as (i) agent i has access to bi and aii,

and (ii) if aij 6= 0, then (i, j) ∈ E , the JOR algorithm is amenableto distributed implementation in the following sense: agent i cancompute the ith component xi of the solution x = A−1b withinformation gathered from its out-neighbors in G.

3. Problem statement

The objective of this paper is to synthesize a discrete-time dis-tributed coordination algorithm that achieves global stabilizationof the desired formation shape. Here we describe the capabilitiesof the robotic network and formally state the control objective.

3.1. Relative sensing network

Consider a group of n agents in Rd. We assume that each agenthas its own reference frameΣi. Expressedwith respect to the fixedframe Σfixed, the ith frame Σi is described by a position pfixedi ∈

Rd and an orientation Rfixedi ∈ SO(d). The dynamical model ofeach agent is as follows. With its sensed information, agent i ∈{1, . . . , n} computes its own control input, expressed in its localframeΣi as uii. In the local frame, each agent moves according to

pii(`+ 1) = uii, ` ∈ Z≥0.

According to (1), in the global frameΣfixed,

pfixedi (`+ 1) = pfixedi (`)+ Rfixedi uii, ` ∈ Z≥0.

The sensing interactions between agents are encoded by a digraphG = ({1, . . . , n}, E). An edge (i, j)means that agent i can sense therelative position of agent j in its own local frame, pij. There is noexplicit communication among agents. We refer to this networkby SrsG .A coordination algorithm is a specification of a control input uii

for each agent i ∈ {1, . . . , n}. The algorithm is distributed over SrsG ifagent i can compute its control input with the sensing informationcollected on the relative position of its neighbors in the graph G.

3.2. The control objective

Our objective is to stabilize the group configuration to a desiredformation. The desired formation is encoded as follows. Given Z∗ =(z∗1 , . . . , z

∗n ) ∈ (R

d)n, let Rgd(Z∗) be the set of configurations in

(Rd)n which are related to Z∗ by a translation and a rotation in Rd,i.e.,

Rgd(Z∗) = {(w1, . . . , wn) ∈ (Rd)n | there is (q, R) ∈

Rd × SO(d) such thatwi = Rz∗i + q, i ∈ {1, . . . , n}},

obviously, Z∗ ∈ Rgd(Z∗). Note that any two configurations inRgd(Z∗) have the same inter-agent distances, i.e., kij = ‖wi −wj‖ ∈ R>0, i 6= j ∈ {1, . . . , n} are the same for anyW ∈ Rgd(Z∗).The control objective is then to stabilize the group of agents to aconfiguration that belongs to Rgd(Z∗).

4. Scaling theory and stress majorization

In this section, we introduce the notion of stress function frommultidimensional scaling theory (Borg & Groenen, 1997; Kruskal,1964) and explain its relationship with the formation controlproblem. We also prove various results that will be instrumentalin the algorithm design, paying attention to the case of directedgraphs.

4.1. The stress function

The raw Stress function StressG : (Rd)n → R is

StressG (p1, . . . , pn) =12

∑(i,j)∈E

(‖pi − pj‖ − kij)2. (2)

For an undirected graph G, this definition coincides with theclassical one used in the multidimensional scaling. Note that thedesired formation configurations are global minimizers of StressG .Under additional assumptions on the rigidity of the graph, onecan guarantee that they are the only ones. Alternatively, one mayconsider the S-Stress (Takane, Young, & De Leeuw, 1977) function

S-StressG (p1, . . . , pn) =12

∑(i,j)∈E

(‖pi − pj‖2 − k2ij)2,

which has the same global minimizers. The S-Stress function isthe Lyapunov function considered in Krick et al. (2009); Olfati-Saber andMurray (2002) in the context of formation control. Here,instead, we focus on the raw Stress, although the developmentsbelow apply equally with the appropriate modifications. Consider

pi = 2∑j:(i,j)∈E

(‖pi − pj‖ − kij)pi − pj‖pi − pj‖

, (3)

for i ∈ {1, . . . , n}. Note that agent i can execute this algorithmwithlocal information. When G is undirected, this system correspondsto the gradient descent of StressG . Under suitable assumptions onthe rigidity ofG, one can show (Dörfler, 2008; Krick et al., 2009) thatthe desired equilibria of the system are locally stable. However, thesystem has other undesired equilibria (other local minimizers ofStressG ), which turn out to be also locally stable (Krick et al., 2009).In addition, it is not difficult to establish that the set of collinearnetwork configurations is invariant under (3).

4.2. Stress majorization

In general, the direct optimization of the stress function is proneto local minima. An alternative route involves the construction ofmajorization functions that are easier to optimize. This is what wediscuss next.Let us start with some notation. Given Z = (z1, . . . , zn) ∈ (Rd)n

and an undirected graph G, let GZ be the weighted graph withadjacency matrixA(GZ )with entries

aij ={kij inv(‖zi − zj‖), (i, j) ∈ E,0, (i, j) 6∈ E,

J. Cortés / Automatica 45 (2009) 2754–2762 2757

where inv(x) = 1/x if x 6= 0, and inv(0) = 0. Note that, for Z ∈Rgd(Z∗), the graphs GZ and G are the same. The stress majorizationfunction F Z

G : (Rd)n → R is

F ZG (P) = tr (P

TL(G)P)− 2 tr (PTL(GZ )Z)+12

∑(i,j)∈E

k2ij.

The name of the function is justified as follows.

Proposition 4.1 (Borg & Groenen, 1997). Given an undirected graphG, for any P = (p1, . . . , pn), Z = (z1, . . . , zn) ∈ (Rd)n,

StressG (p1, . . . , pn) ≤ F ZG (P).

Moreover, if P = Z, then F PG (P) = StressG (P).

This result can be generalized to the case of digraphs.

Proposition 4.2. Given a digraph G, for any P = (p1, . . . , pn), Z =(z1, . . . , zn) ∈ (Rd)n,

StressG (p1, . . . , pn) ≤ F ZG (P).

Moreover, if P = Z, then F PG(P) = StressG (P).

Proof. We expand the expression of StressG as

StressG (p1, . . . , pn) =12

∑(i,j)∈E

‖pi − pj‖2

−

∑(i,j)∈E

‖pi − pj‖kij +12

∑(i,j)∈E

k2ij.

Using Lemma 2.2, the first summand can be rewritten astr (PTL(G)P). Using the Laplacian of the graph GZ , the second sum-mand can be rewritten as

−

∑(i,j)∈E

‖pi − pj‖kij

= −

∑(i,j)∈E

kij inv(‖zi − zj‖)‖pi − pj‖ · ‖zi − zj‖.

Using the Cauchy–Schwarz inequality and an argument similar tothe one used in the proof of Lemma 2.2, we deduce that this sum-mand is upper bounded 2 tr (PTL(GZ )Z), with equality if P = Z , andthis concludes the proof. �

Alternatively, the stressmajorization function can be expressedusing the Kronecker product as

F ZG (P) = P

T(L(G)⊗ Id)P − 2PT(L(GZ )⊗ Id)Z +12

∑(i,j)∈E

k2ij. (4)

This expression is useful in establishing the following keyproperties of the stress majorization function.

Proposition 4.3. Given Z = (z1, . . . , zn) ∈ (Rd)n and an undirectedgraph G, the following holds:

(i) The gradient and Hessian of F ZG are, respectively,

∇F ZG = 2(L(G)⊗ Id)P − 2(L(G

Z )⊗ Id)Z,

∇2(F Z

G ) = 2L(G)⊗ Id.

In particular, both are distributed over the graph G;(ii) The function F Z

G is globally convex;(iii) P ∈ (Rd)n is a global minimizer of F Z

G if and only if

(L(G)⊗ Id)P = (L(GZ )⊗ Id)Z . (5)

In particular, if G is connected, any two minima of F ZG are equal

up to a translation in Rd.

Proof. Fact (i) readily follows from expression (4). Fact (ii) followsfrom noting that the eigenvalues of ∇2(F Z

G ) are the same asthose of L(G) with their multiplicity doubled. Since L(G) ispositive semidefinite, so is ∇2(F Z

G ). Finally, fact (iii) follows fromfact (i). �

The next result will be important later for our distributedalgorithm design in the case of directed graphs.

Lemma 4.4. Given Z ∈ Rgd (Z∗) and a digraph G with a globallyreachable vertex, P is a global minimizer of F Z

Gif and only if

(L(G)⊗ Id)P = (L(G)⊗ Id)Z . (6)

Proof. Note that G = GZ because Z ∈ Rgd(Z∗). According toProposition 4.3(iii), P is a global minimizer of F Z

Gif and only if

(L(G)⊗ Id)P = (L(G)⊗ Id)Z . (7)

Since G has a globally reachable vertex, the mirror graph G isconnected, and therefore, (7) is equivalent to P = Z + 1n ⊗ q, forsome q ∈ Rd, which implies (6). The reverse implication follows bynoting that, since G has a globally reachable vertex, its Laplacianhas rank n − 1, and therefore (L(G) ⊗ Id)v = 0 if and only ifv ∈ 1n ⊗ Rd. Consequently, (6) implies that P = Z + 1n ⊗ q,for some q ∈ Rd, which is equivalent to (7). �

The importance of Lemma 4.4 stems from the following obser-vation: the critical points of F Z

Gcan be characterized by a linear

equation (6) defined by the Laplacian matrix of G, which is dis-tributed over the digraph G. Note that the original characteriza-tion (7) is defined by the Laplacian matrix of the mirror graph,which is not distributed over G.

5. Coordination algorithm for global stabilization of forma-tions

In this section, we propose a discrete-time distributed coor-dination algorithm that achieves global stabilization of the de-sired formation. We begin by discussing the problem of finding anetwork-wide reference frame and then design the motion coor-dination algorithm.

5.1. Common orientation of local reference frames

The reference frames of the individual agents in SrsG might havedifferent orientations with respect to the global reference frame.However, the network can execute some suitable initializationalgorithm to equally orient all agent reference frames. Here wedescribe one simple procedure based on a distributed implementa-tion of the flooding algorithm (Peleg, 2000) on the relative sensingnetwork. Other solutions to the common reference frame problemare explored in Piovan et al. (2008).Assume G has a globally reachable vertex. For simplicity, we

describe the strategy first inR2. At the first time step, a preselectedglobally reachable vertexmoves a unit in the direction of its x-axis.All other agents that can sense the position of this agent measurethe relative displacement in their local frames and figure out thex-axis direction of the agent. They rotate their frames to alignthem with the direction of the relative displacement. The processis repeated until all agents have aligned their frames with the oneof the globally reachable vertex.In R3, it takes two time steps for each agent to figure out the

orientation of the reference frame of the globally reachable vertex.This vertex firstmoves in the direction of its x-axis, and thenmovesin the direction of its y-axis. Then the process is repeated until allagents have equally-aligned reference frames.Note that this algorithm does not provide the network with a

common origin or reference point, and therefore agents cannotcompute or move to positions in the environment specified in aglobal reference frame.

2758 J. Cortés / Automatica 45 (2009) 2754–2762

5.2. Motion coordination via Jacobi iteration

Here, we assume that all agent reference frames have the sameorientation, i.e., Rfixedi = R, for i ∈ {1, . . . , n}, for some R ∈SO(d), unknown to the individual agents. Given the discussion inSection 4, our strategy to make the network achieve the desiredformation is to globally optimize F Z∗

G . From Proposition 4.3 andLemma 4.4, this can be achieved by solving the sparse linearequation

(L(G)⊗ Id)P = (L(G)⊗ Id)Z∗.

To solve this equation, we propose to use a Jacobi overrelaxationiteration (JOR), cf. Section 2.3. Let

b = (b1, . . . , bn) = (L(G)⊗ Id)Z∗, (8)

with bi ∈ Rd, i ∈ {1, . . . , n}. In Cartesian coordinates, the JORalgorithm for each agent i is, for ` ∈ Z≥0,

pi(`+ 1) = (1− h)pi(`)+ h1di

(∑j6=i

aijpj(`)+ bi),

where (d1, . . . , dn) is the diagonal of Dout(G). If di = 0, then we setpi(` + 1) = pi(`). In the local frame of agent i, this is written, for` ∈ Z≥0,

pii(`+ 1) = h1di

(∑j6=i

aijpij(`)+ (Rfixedi )Tbi

), (9)

if di 6= 0, and pii(` + 1) = 0 otherwise. The individual agent doesnot know the rotation matrix Rfixedi = R. Therefore, instead of (9),agent i runs, for ` ∈ Z≥0,

pii(`+ 1) = h1di

(∑j6=i

aijpij(`)+ bi), (10)

if di 6= 0, and pii(`+1) = 0 otherwise. In the global frame, using (1),the algorithm (10) reads, for ` ∈ Z≥0,

pi(`+ 1) = (1− h)pi(`)+ h1di

(∑j6=i

aijpj(`)+ Rbi), (11)

if di 6= 0, and pi(`+ 1) = pi(`) otherwise. This corresponds to theJOR algorithm to solve the linear equation

(L(G)⊗ Id)P = (In ⊗ R)b. (12)

Note that all the solutions of this equation correspond to transla-tions of a rotated configuration of Z∗, and therefore, all belong toRgd(Z∗), as desired.Next, we characterize the distributed character of this algo-

rithm as well as its convergence properties.

Proposition 5.1. Consider the relative sensing network SrsG , where Ghas a globally reachable vertex. Let h ∈ (0, 1) and assume all agentreference frames are equally oriented. Then, the following holds

(i) the coordination algorithm (10) is distributed over SrsG . Moreover,as initial information, each agent only needs to store a vectorin Rd;

(ii) the coordination algorithm (10) converges to a configuration Win Rgd (Z∗);

(iii) if G is undirected, the stress majorization function F WG is

monotonically decreasing along (10).

Proof. Fact (i) follows from directly from (10). The only initialinformation that agent i ∈ {1, . . . , n} requires is the vector bi ∈ Rd.To show fact (ii), we reason on the expression (11) of the algorithmin the global frame. Some basic manipulations yield

(In ⊗ R)b = (In ⊗ R)(L(G)⊗ Id)Z∗

= (L(G)⊗ Id)(In ⊗ R)Z∗.

Consider the change of coordinates Y = (y1, . . . , yn) = P − (In ⊗R)Z∗. Then, the linear equation (12) reads as

(L(G)⊗ Id)Y = 0. (13)

For α ∈ {1, . . . , d}, let

yα = Πni=1πα(Y ) = (πα(y1), . . . , πα(yn)) ∈ Rn.

Then the JOR algorithm (11) reads, for ` ∈ Z≥0,

(yα)i(`+ 1) = (yα)i(`)+ h1di

∑j6=i

aij((yα)j(`)− (yα)i(`)),

if di 6= 0, and (yα)i(`+1) = (yα)i(`) otherwise, for α ∈ {1, . . . , d}.Therefore, we have d copies of the same linear dynamical system,and we just need to analyze the convergence properties of

z(`+ 1) = (In − hD−1out(G)L(G))z(`), (14)

with z(0) ∈ Rn and ` ∈ Z≥0, where for convenience setD−1out(G)ii =0 if di = 0. It is easy to see that, for h ∈ (0, 1), the matrixF = In−hD−1out(G)L(G) is stochastic. Using Bullo et al. (2009, Propo-sition 1.65(ii)), we deduce that the trajectory of (14) starting atz(0) converges to (vTz(0)/vT1n)1n, where v denotes a left eigen-vector of F with eigenvalue 1. Therefore, for each α ∈ {1, . . . , d},there exists sα ∈ R such that yα(`) → sα1n as ` → +∞.Denoting s = (s1, . . . , sd) ∈ Rd, we conclude that yi(`) → sas ` → +∞, for i ∈ {1, . . . , n}, or equivalently, that the co-ordination algorithm (11) converges to the configuration W =

(RZ∗1 + s, . . . , RZ∗n + s) = (In ⊗ R)Z

∗+ 1n ⊗ s ∈ Rgd(Z∗).

To show fact (iii), we examine the differences F WG (P(`+ 1))−

F WG (P(`)), where {P(`) ∈ (R

d)n | ` ∈ Z≥0} is the sequence ofnetwork configurations generated by the JOR algorithm (11). Aftersome manipulations, this algorithm can be written in closed formas

P(`+ 1) = P(`)− h(D(G)−1L(G)⊗ Id)(P(`)−W ),

for ` ∈ Z≥0. Next,we compute the evolution of the stressmajoriza-tion function as

F WG (P(`+ 1))− F W

G (P(`)) = P(`+ 1)T(L(G)⊗ Id)P(`+ 1)

− P(`)T(L(G)⊗ Id)P(`)− 2(P(`+ 1)− P(`))T(L(G)⊗ Id)W .

Using the identity (A1 ⊗ B1)(A2 ⊗ B2) = (A1A2) ⊗ (B1B2), we canrewrite the evolution of F W

G as

F WG (P(`+ 1))− F W

G (P(`))

= −2hzT(D(G)−1 ⊗ Id)(L(G)⊗ Id)P(`)+ h2zT(D(G)−1L(G)D(G)−1 ⊗ Id)z+ 2hzT(D(G)−1 ⊗ Id)(L(G)⊗ Id)W ,

where, for brevity, we use the shorthand notation z = (L(G) ⊗Id)(P(`)−W ). A final simplification yields

F WG (P(`+ 1))− F W

G (P(`))

= −hzT((2D(G)−1 − hD(G)−1L(G)D(G)−1)⊗ Id)z.

Using Lemma 2.1, we can guarantee that for h ∈ (0, 1), the ma-trix (2D(G)−1 − hD(G)−1L(G)D(G)−1)⊗ Id is positive definite, andtherefore

F WG (P(`+ 1))− F W

G (P(`)) ≤ 0,

J. Cortés / Automatica 45 (2009) 2754–2762 2759

Fig. 1. Counter example to Proposition 5.1(iii) in the directed case. For this digraph,there exist initial network configurations for which F W

G is not monotonicallydecreasing along the execution of (10).

with equality holding if and only if z = (L(G)⊗ Id)(P(`)−W ) = 0,i.e., P(`) ∈ Rgd(Z∗). �

Remark 5.2. Proposition 5.1(iii) does not hold in general if G is di-rected. A counter example is given by the digraph plotted in Fig. 1.For this digraph, the matrix Sym(L(G)D(G)−1L(G)) has a negativeeigenvalue, and therefore there exist initial network configurationsfor which F W

G is not monotonically decreasing along (10). •

Fig. 2 shows an execution of (10) over a relative sensing networkin R3 composed of 60 agents, with interaction topology given bythe Buckminster Fuller geodesic dome (Aldersey-Williams, 1995).The desired formation, shaped as a soccer ball, is encoded via (8).

6. Robustness against link failures, measurement errors, andframe orientation errors

In this section, we examine the robustness properties of thecoordination algorithm (10) against several sources of error. First,we consider the situationwhere one ormore of agents fail to sensethe relative position of other neighboring agents. We refer to suchoccurrences as link failures. Second, we consider measurementerrors in the acquisition of the relative position of neighboringagents. Finally, we study the algorithm robustness to errors in theinitialization of the orientation of the agents’ frames.

6.1. Robustness against link failures and convergence of asynchronousexecutions

Consider the scenario where an agent fails to acquire the rela-tive position of a neighbor. In such case, it seems logical for thatagent to use the last recorded information about the relative posi-tion of the neighbor. Interestingly, this situation can be interpretedas an asynchronous execution of (10), where, at each time step,the information that an agent has about other neighboring agentsmight be outdated. In our case, the outdatedness is due to a sensorfailure, but it could also correspond to other reasons, such as delaysin processing information.

The convergence of the resulting algorithm can be studied usingwell-established results on partially asynchronous algorithms,see Bertsekas and Tsitsiklis (1997, Chapter 7). Roughly speaking,at time `, agent i does not necessarily use the most recent value,pij(`), about another agent, but rather an outdated one,

pij(τij (`)), 0 ≤ τ ij (`) ≤ `. (15)

The quantity ` − τ ij (`) represents the delay. Therefore, the JORalgorithm (10) then reads, for ` ∈ Z≥0,

pii(`+ 1) = h1di

(∑j6=i

aijpij(τij (`))+ bi

). (16)

The next result states that the convergence of (16) is stillguaranteed if the frequency of the failures is bounded.

Proposition 6.1. Consider the relative sensing network SrsG operatingasynchronously, where G has a globally reachable vertex. Let h ∈(0, 1) and assume all agent reference frames are equally oriented.Assume there exists B > 0 such that ` − B + 1 ≤ τ ij (`) ≤ `, forall ` ∈ Z≥0, and all (i, j) ∈ E . Then, the algorithm (16) converges toa configuration W in Rgd (Z∗).

Proposition 6.1 is established by rewriting the coordinationalgorithm in a manner analogous to the expressions obtainedin the proof of Proposition 5.1(ii), and then applying the resultsin Blondel, Hendrickx, Olshevsky and Tsitsiklis (2005) and Moreau(2005). We omit it for space reasons.Fig. 3 shows an execution of (16) over a relative sensing net-

work composed of 20 agents, with interaction topology given by adirected version of the Desargues graph (Harary, 1994). The max-imum delay is B = 10 steps, i.e., no agent has relative positioninformation on its neighbors that is more than 10 steps outdated.

6.2. Robustness against measurement errors

Herewe study the convergence properties of the algorithm (10)under measurement errors. Assume that, for each edge (i, j) of G,agent i senses the position of agent j in its own local frame with anerror bounded by σ ∈ R≥0, i.e., agent i uses the erroneous relativeposition

pij + eij, (17)

of agent j, with eij ∈ Rd, ‖eij‖∞ ≤ σ , instead of the correct relativeposition pij. Under suchmeasurement errors, the JOR algorithm (10)then reads, for ` ∈ Z≥0,

pii(`+ 1) = h1di

(∑j6=i

aij(pij(`)+ eij(`)

)+ bi

), (18)

with ‖eij(`)‖∞ ≤ σ for each edge (i, j) of G. The following resultcharacterizes the convergence of (18).

Fig. 2. Execution of the algorithm (10) with h = 0.25 over a relative sensing network in R3 composed of 60 agents. (a) initial configuration, (b) evolution, and (c) finalformation.

2760 J. Cortés / Automatica 45 (2009) 2754–2762

Fig. 3. Execution of the algorithm (16) with h = 0.2 over a relative sensing network in R2 composed of 20 agents. (a) initial configuration, (b) evolution, (c) final formation,and (d) evolution of (10) from the same initial condition for comparison.

Fig. 4. Execution of the algorithm (18) with h = 0.25 and σ = 0.2 over a relative sensing network in R2 composed of 20 agents. (a) initial configuration, (b) evolution, and(c) formation after 80 iterations.

Proposition 6.2. Consider the relative sensing network SrsG , whereG has a globally reachable vertex. Let h ∈ (0, 1) and assume allagent reference frames are equally oriented. Assume the networkagents acquire erroneous relative position information according tothemodel (17)withmismatch bounded byσ ∈ R≥0. Then, there existsK ∈ R>0 such that the algorithm (18) converges to

{Z ∈ (Rd)n | there exists W ∈ Rgd (Z∗)such that ‖Z −W‖∞ ≤ Khσ }.

Proof. As in the proof of Proposition 5.1, define the change ofcoordinates Y = (y1, . . . , yn) = P − (In ⊗ R)Z∗, and, for eachα ∈ {1, . . . , d}, let

yα = Πni=1πα(Y ) = (πα(y1), . . . , πα(yn)) ∈ Rn.

Then, the algorithm (18) can be rewritten, for ` ∈ Z≥0,

(yα)i(`+ 1) = (yα)i(`)

+ h1di

∑j6=i

aij((yα)j(`)− (yα)i(`)+ πα(eij(`))),

if di 6= 0, and (yα)i(`+1) = (yα)i(`) otherwise, for α ∈ {1, . . . , d}.Note that∣∣∣∣∣∑j6=i

aijπα(eij(`))

∣∣∣∣∣ ≤∑j6=i

aij|πα(eij(`))|

≤

∑j6=i

aij‖eij(`)‖∞ ≤ diσ . (19)

Hence, we have d instances of a linear dynamical system withbounded disturbances, and we just need to analyze

z(`+ 1) = (In − hD−1out(G)L(G))z(`)+ hv(`), (20)

with z(0) ∈ Rn and ` ∈ Z≥0. From (19),we deduce that ‖v(`)‖∞ ≤σ . Note that F = In − hD−1out(G)L(G) is stochastic. Moreover,since G is globally reachable, F has 1 as the unique eigenvalueof maximum magnitude and with multiplicity 1. Therefore, thelinear system (20) is input-to-state stable (Jiang & Wang, 2001)with respect to the agreement configurations {s1n | s ∈ R}.The convergence result then follows from this observation. Theconstant K is obtained by observing that F − 1

vT1n1nvT, where

v denotes a left eigenvector of F with eigenvalue 1, has all itseigenvalues in the unit complex disk centered at 0, i.e., the matrixis Schur stable. Hence, from LaSalle (1986, Chapter 5), there existc > 1 and b ∈ (0, 1) such that ‖(F − 1

vT1n1nvT)k‖∞ ≤ c bk. Upper-

bounding the evolution in (20), we get K = c1−b . �

Note that Proposition 6.2 only guarantees convergence to aset. According to the statement, the configurations in this set cor-respond to translations and rotations of the desired formationslightly deformed by the effect of the erroneous relative positionreadings of the sensors.Fig. 4 shows an execution of (18) over the relative sensing net-

work of Fig. 3 under measurement errors. The norm of the mea-surement errors is upper bounded by σ = 0.2. As forecasted byProposition 6.2, the network shape converges to a formation closeto the desired one, while the group of agents moves in the plane.

6.3. Robustness against frame orientation errors

Here we explore the robustness properties of the coordinationalgorithm (10) against errors in the computation of the commonorientation of the reference frame, cf. Section 5.1. Suppose thatthe algorithm selected by the network to orient all agent frames

J. Cortés / Automatica 45 (2009) 2754–2762 2761

Fig. 5. Execution of the algorithm (10) with h = 0.25 over a relative sensing network in R2 composed of 20 agents under errors in the orientation of the agent frames. (a)initial configuration, (b) evolution, and (c) final formation shape.

equally is not executed perfectly and consequently the finalorientation of the frame of agent i ∈ {1, . . . , n} is of the form

Rfixedi = R+ Ξi, (21)

where R ∈ SO(d) and the error matrix Ξi satisfies ‖Ξi‖∞ ≤ ε.Given the difference in the orientation of the frames, (10) readsnow in the global frame as

pi(`+ 1) = (1− h)pi(`)+ h1di

(∑j6=i

aijpj(`)+ Rfixedi bi), (22)

for ` ∈ Z≥0, which corresponds to the JOR algorithm to solve thelinear algebraic equation

(L(G)⊗ Id)P = diag(Rfixed1 , . . . , Rfixedn

)b. (23)

Observe that the mismatch in the orientation of the framesmakes this linear equation ill-posed. In other words, the vectordiag

(Rfixed1 , . . . , Rfixedn

)b does not belong to the range of thematrix

L(G) ⊗ Id, and therefore, there does not exist a solution P of (23).This observation is consistent with the fact that the algorithm de-sign assumes all frames are equally oriented. Even though (23) hasno solution, the question about the convergence of (22) remains.The next result answers it.

Proposition 6.3. Consider the relative sensing network SrsG , where Ghas a globally reachable vertex. Let h ∈ (0, 1). Let the orientation ofthe frame of agent i ∈ {1, . . . , n} be given by (21). Then, there existsK ∈ R>0 such that the algorithm (10) converges to

{Z ∈ (Rd)n | there exists W ∈ Rgd (Z∗)

such that ‖Z −W‖∞ ≤ Khε}.

The proof of this result is analogous to the proof of Proposi-tion 6.2, and we omit it in the interest of space. After some ma-nipulations, one can show that

K = Kmax{‖bi‖∞ | i ∈ {1, . . . , n}}min{di | di 6= 0, i ∈ {1, . . . , n}}

.

Note that Proposition 6.3 only guarantees convergence to aset. According to the statement, the configurations in this setcorrespond to translations and rotations of the desired formationslightly deformed by the effect of the mismatch in the orientationof the agent frames.Fig. 5 shows an execution of (10) over the relative sensing

network of Fig. 3 under errors in the computation of the commonorientation of the agent frames. Each agent orients its own framewith an angle of 90 degrees with an error whose absolute value isbounded by nine degrees. The desired formation is the one givenby Fig. 3(c). As forecast by Proposition 6.3, the network shape

converges to a formation close to the desired one, while the groupof agents moves in the plane.Under additional conditions on the interaction topology, it is

possible to state a slightly stronger result.

Corollary 6.4. Under the assumptions of Proposition 6.3, furtherassume that G is regular. Then, for each initial condition, there existss ∈ Rd and W ∈ (Rd)n with dist∞(W , Rgd (Z∗)) ≤ Kε (where Konly depends on the graph and the desired formation) such that theevolution of (10) converges to the line {Z + a(1n ⊗ s) | a ∈ R}.

7. Conclusions and future work

We have proposed a distributed formation control strategy forrelative sensing networks, and analyzed its convergence proper-ties, showing that, for any interaction topology with a globallyreachable vertex, it globally stabilizes the desired formation shape.We have established the algorithm robustness against link fail-ures, measurement errors, and errors in the initial computation ofthe common reference frame. Future work will include the designof error-correcting algorithms that completely eliminate any mis-match in the orientation of the agent frames and the extension ofthe results to consider switching interaction topologies and indi-vidual agent dynamics.

Acknowledgments

The author thanks Profs. Brian D. O. Anderson, Baris Fidan, andSonia Martínez for insightful conversations.

Appendix

Proof of Lemma 2.1. Since the graph is connected, every vertexhas a positive degree, and thematrixD(G) is positive definite. Now,note that

2D(G)−1 − hD(G)−1L(G)D(G)−1

= D(G)−1/2(2In − hD(G)−1/2L(G)D(G)−1/2)D(G)−1/2.

It suffices to show that the symmetric matrix 2In − hD(G)−1/2L(G)D(G)−1/2 is positive definite. The matrix D(G)−1/2L(G)D(G)−1/2 isthe normalized Laplacian considered in Chung (1997). Accordingto Chung (1997, Lemma 1.7), the eigenvalues of the normalizedLaplacian all belong to [0, 2], and the result follows. �

Proof of Lemma 2.2. Note that, for x ∈ Rn, the ith coordinate ofthe vectors L(G)x, L(G)x ∈ Rn are

(L(G)x)i =n∑j=1

aij(xi − xj), (L(G)x)i =n∑j=1

aji(xi − xj).

2762 J. Cortés / Automatica 45 (2009) 2754–2762

We use these expressions in the following manipulations,∑(i,j)∈E

aij(xi − xj)(yi − yj) =n∑i,j=1

aij(xi − xj)(yi − yj)

=

n∑i,j=1

aij(xi − xj)yi −n∑i,j=1

aij(xi − xj)yj

=

n∑i=1

yi(L(G)x)i +n∑i,j=1

aji(xi − xj)yi

= yT(L(G)+ L(G))x,

as claimed. �

References

Aldersey-Williams, H. (1995). The most beautiful molecule: The discovery of thebuckyball. New York: John Wiley.

Bertsekas, D. P., & Tsitsiklis, J. N. (1997). Parallel and distributed computation:Numerical methods. Athena Scientific.

Blondel, V. D., Hendrickx, J. M., Olshevsky, A., & Tsitsiklis, J. N. (2005). Convergenceinmultiagent coordination, consensus, and flocking. In IEEE conf. on decision andcontrol and European control conference, Seville, Spain (pp. 2996–3000).

Borg, I., & Groenen, P. (1997). Modern multidimensional scaling: Theory andapplications. New York: Springer.

Bullo, F., Cortés, J., & Martínez, S. (2009). Applied Mathematics Series, Distributedcontrol of robotic networks. Princeton University Press, Electronically availableat http://coordinationbook.info.

Chung, F. R. K. (1997). CBMS regional conference series in mathematics: Vol. 92.Spectral graph theory. Providence, RI: American Mathematical Society.

Cormen, T. H., Leiserson, C. E., Rivest, R. L., & Stein, C. (2001). Introduction toalgorithms (2nd ed.). MIT Press.

Dörfler, F. (2008). Geometric analysis of the formation control problem for autonomousrobots. Diploma thesis.

Egerstedt, M., & Hu, X. (2001). Formation constrained multi-agent control. IEEETransactions on Robotics and Automation, 17(6), 947–951.

Fax, J. A., &Murray, R.M. (2004). Information flow and cooperative control of vehicleformations. IEEE Transactions on Automatic Control, 49(9), 1465–1476.

Gotsman, C., & Koren, Y. (2005). Distributed graph layout for sensor networks.Journal of Graph Algorithms and Applications, 9(3), 327–346.

Harary, F. (1994). Graph theory. Westview Press.Ji, M., & Egerstedt, M. (2007). Distributed control of multiagent systems whilepreserving connectedness. IEEE Transactions on Robotics, 23(4), 693–703.

Jiang, Z.-P., & Wang, Y. (2001). Input-to-state stability for discrete-time nonlinearsystems. Automatica, 37(6), 857–869.

Justh, E. W., & Krishnaprasad, P. S. (2004). Equilibria and steering laws for planarformations. Systems & Control Letters, 52(1), 25–38.

Kamada, T., & Kawai, S. (1989). An algorithm for drawing general undirected graphs.Information Processing Letters, 31, 7–15.

Krick, L., Broucke, M. E., & Francis, B. (2009). Stabilization of infinitesimally rigidformations of multi-robot networks. International Journal of Control, 82(3),423–439.

Kruskal, J. B. (1964). Nonmetric multidimensional scaling: A numerical method.Psychometrika, 29, 115–129.

Lafferriere, G., Williams, A., Caughman, J., & Veerman, J. J. P. (2005). Decentralizedcontrol of vehicle formations. Systems & Control Letters, 54(9), 899–910.

LaSalle, J. P. (1986). Applied mathematical sciences: Vol. 62. The stability and controlof discrete processes. Springer.

Lin, Z., Francis, B., & Maggiore, M. (2005). Necessary and sufficient graphicalconditions for formation control of unicycles. IEEE Transactions on AutomaticControl, 50(1), 121–127.

Moreau, L. (2005). Stability of multiagent systems with time-dependent communi-cation links. IEEE Transactions on Automatic Control, 50(2), 169–182.

Olfati-Saber, R., & Murray, R. M. (2002). Distributed cooperative control of multiplevehicle formations using structural potential functions. In IFAC world congress,Spain.

Peleg, D. (2000). Monographs on discrete mathematics and applications, Distributedcomputing. A locality-sensitive approach. SIAM.

Piovan, G., Shames, I., Fidan, B., Bullo, F., & Anderson, B. D. O. (2008). On frame andorientation localization for relative sensing networks. In IEEE conf. on decisionand control, Cancún, México (pp. 2326–2331).

Ren, W. (2007). Consensus strategies for cooperative control of vehicle formations.IET Control Theory & Applications, 1(2), 505–512.

Smith, R. S., & Hadaegh, F. Y. (2007). Closed-loop dynamics of cooperative vehicleformations with parallel estimators and communication. IEEE Transactions onAutomatic Control, 52(8), 1404–1414.

Takane, Y., Young, F. W., & De Leeuw, J. (1977). Nonmetric individual differencesmultidimensional scaling: An alternating least-squares method with optimalscaling features. Psychometrika, 42, 7–67.

Tanner, H. G., Pappas, G. J., & Kumar, V. (2004). Leader-to-formation stability. IEEETransactions on Robotics and Automation, 20(3), 443–455.

Yu, C., Anderson, B. D. O., Dasgupta, S., & Fidan, B. (2009). Control of minimallypersistent formations in the plane. SIAM Journal on Control and Optimization,48(1), 206–233.

Jorge Cortés received the Licenciatura degree in mathe-matics from the Universidad de Zaragoza, Zaragoza, Spain,in 1997, and the Ph.D. degree in engineering mathematicsfrom the Universidad Carlos III de Madrid, Madrid, Spain,in 2001. He was a Postdoctoral Research Associate at theSystems, Signals and Control Department, University ofTwente, Enschede, The Netherlands (from January to June2002), and at the Coordinated Science Laboratory, Univer-sity of Illinois at Urbana-Champaign (from August 2002to September 2004). From 2004 to 2007, he was an assis-tant professor with the Department of AppliedMathemat-

ics and Statistics, University of California, Santa Cruz. He is currently an associateprofessor in the Department of Mechanical and Aerospace Engineering, Universityof California, San Diego. His research interests focus on mathematical control the-ory, distributedmotion coordination for groups of autonomous agents, and geomet-ric mechanics and geometric integration. He is the author of Geometric, Control andNumerical Aspects of Nonholonomic Systems (Springer Verlag, 2002) and co-author(together with F. Bullo and S. Martínez) of Distributed Control of Robotic Networks(Princeton University Press, 2009). He is the recipient of the 2006 Spanish Societyof Applied Mathematics Young Researcher Prize and co-recipient (together with F.Bullo and S. Martínez) of the 2008 IEEE Control Systems Magazine Outstanding Pa-per Award. He is currently an associate editor for the IEEE Transactions on AutomaticControl, Systems and Control Letters, and European Journal of Control.