JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007 1

Observer-based Feedback Control for Stabilizationof Collective Motion

Seth Napora and Derek A. Paley, Senior Member, IEEE

Abstract—Multi-vehicle control for collective motion has appli-cations in environmental sampling in the atmosphere and ocean.Previous work in this field has produced theoretically justifieddecentralized algorithms for stabilization of motion primitivessuch as parallel and circular motion of self-propelled vehiclesusing measurements of relative position and relative velocity.This paper describes an observer-based distributed control al-gorithm for the stabilization of parallel and circular motionusing measurements of relative position only. The algorithmenables each vehicle to utilize information about vehicle dynamicsand turning rates to estimate the relative velocity of othervehicles. Theoretical justification is provided for the closed-loopperformance, and numerical simulations illustrate the extensionof the algorithm to a three-dimensional model of a miniaturesubmarine. The algorithm has also been implemented on alaboratory-scale multi-vehicle underwater testbed; we describethe results of experimental validation using motion-capture-based feedback control in the University of Maryland’s NeutralBuoyancy Research Facility.

I. INTRODUCTION

Motivation for pursuing coordinated, collective motion ofautonomous vehicles comes from the desire to estimate rapidlyevolving spatiotemporal processes using mobile sensor net-works. A collection of vehicles may be better suited to samplean environmental phenomenon than an individual platformbecause the collection can gather multiple, simultaneous mea-surements over a larger area. For example, multiple UAVsperforming environmental sampling can further the under-standing of the rapid intensification of tropical cyclones [11]and transmission of airborne pathogens [22], [23]. Similarly,sampling of oceanic processes for greater sonar performanceprediction can benefit from multi-vehicle cooperation [10],[16]. Other applications include underwater minesweeping [5],diffusion mapping and spraying [3], and boundary tracking [6]for oil spills and algae growth.

Prior work in the field of collective motion has producedmany control algorithms for vehicles modeled as self-propelledparticles. In [18], theoretically justified control laws for thismodel are provided to stabilize synchronized, balanced, andcircular formations. The authors in [14] and [15] build uponthese control laws and adapt them to function in the presenceof a spatially and temporally varying flowfield. The authors

This material is based upon work supported by the National ScienceFoundation under Grant Nos. CMMI0928416 and CMMI0954361 and theOffice of Naval Research under Grant No. N00014-09-1-1058.

S. Napora is a guidance, navigation, and control engineer with ATK SpaceSystems Inc., Beltsville, MD 20705. setna05@gmail.com

D. Paley is an associate professor in the Department of AerospaceEngineering, University of Maryland, College Park, MD 20742, USAdpaley@umd.edu

in [12] provide a second-order steering control for a self-propelled vehicle model using backstepping as an alternateto proportional control. The authors in [4] examine collectivemotion via pursuit dynamics where a leader vehicle performeda behavior and the other vehicles pursued the leader. Takinganother approach, [1] examined the effects of long-rangeconnections on collective behaviors.

A challenge to achieving collective motion is the stabiliza-tion of moving formations with limited information. In [20],flocking behavior of agents is described whereby only a certainnumber of agents are informed of the desired behavior. Thisrestriction is also described in [19], in the context of a self-propelled particle system with limited communication betweenagents. Information can also be limited by sensing capabilities.In this case, other approaches such as estimation must be takeninto consideration to determine the missing information. In [2],limited sensing is overcome using sliding-mode estimators toachieve formation tracking.

Additional research into cooperative control involves theexperimental validation of the proposed control algorithm.Validation can be achieved through a variety of platformsranging from aircraft to submersibles. The researchers in [17]designed a cost-effective ground platform capable of self-assembly. The authors of [9] utilized a fin-actuated platformto stabilize parallel and balanced formations of underwatervehicles. The authors in [10] and [23] utilized vehicles capableof waypoint navigation to perform the desired behavior.

In this paper, parallel and circular formations are stud-ied using a self-propelled vehicle model with second-ordersteering control. These particular formations represent basicmotion primitives from which more complex trajectories canbe designed to meet application specific demands, such as en-vironmental sampling [10]. Previous work on these collectivebehaviors indicate that each vehicle requires knowledge of therelative position and relative velocity orientation of the othervehicles in the group. Here, we assume that each vehicle iscapable of sensing only the relative position of other vehiclesas well as its own turning rate. The main contributions ofthis paper are to present theoretically justified methods for(1) estimating the velocity of one vehicle relative to anothervehicle and (2) utilizing that estimate in an observer-basedfeedback control to stabilize parallel and circular formations ofmultiple, self-propelled vehicles with second-order rotationaldynamics. The cooperative control algorithms are implementedon a three-dimensional submarine model to simulate a morerealistic performance, and are experimentally validated usinga laboratory-scale testbed of multiple underwater vehicles.

The outline for the paper is as follows. Section II presents

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007 2

kinematic and dynamic models of self-propelled vehicle mo-tion, including control laws that stabilize parallel and circu-lar formations using relative position and relative velocity.Section III derives an observer-based feedback control toestimate relative velocity using noise-free measurements ofrelative position and turning rate. Section IV describes a three-dimensional rigid-body submarine model and results fromsimulating the corresponding control implementation. SectionV discusses results from experimental validation using anunderwater vehicle testbed. Section VI summarizes the resultsand ongoing work.

II. PARTICLE DYNAMICS & STATE-FEEDBACK CONTROL

In our study of collective motion, we consider paralleland circular formations as building blocks for more complexmotion. These cooperative motions have been achieved in [18]using a particle model to represent each vehicle in a group.We describe that model here, along with a vehicle model withsecond-order rotational dynamics. For each model, we includea description of control algorithms for stabilizing parallel andcircular formations.

A. Self-Propelled Vehicle Model with First-Order Steering

A dynamic model that has been used to design collectivemotion [18] is a constant-speed vehicle model with first-ordersteering control also known as a self-propelled particle model.This model assumes that each agent moves in the plane at aconstant speed, often assumed to be one. The inertial positionof vehicle k is denoted rk = [xk yk]T

I, and the orientation

of its (planar, unit) velocity by θk. The steering control νkis applied to the heading rate allowing the vehicle to changecourse as indicated by the following equations of motion:

xk = cos θkyk = sin θkθk = νk,

(1)

where k = 1, ..., N represents the kth vehicle in a group ofsize N . Collective control laws have been designed for thismodel resulting in parallel and circular formations [18].

A parallel formation is achieved when each vehicle obtainsthe same velocity orientation. The following gradient controlachieves this motion with all-to-all communication [18]:

νk = −KN

N∑j=1

sin(θj − θk) , αk(θk), (2)

where θk = [θ1 − θk, ..., θN − θk]. Note that the absoluteorientations of the other vehicles’ velocities are not requiredfor control νk, only the relative orientations. The choiceof control gain K influences the convergence speed of theformation as well as the formation type. Choosing K < 0in (2) produces straight-line motion where all the vehicletrajectories are parallel [18]. Choosing K > 0 yields balancedmotion; this behavior occurs when the sum of all vehicles’velocities is equal to zero. These motions are illustrated inFig. 1.

A circular formation is achieved when each vehicle’s turningrate and center of rotation is identical to the rest of the group.

(a) Parallel formation (b) Balanced formation (c) Circular formation

Fig. 1: Collective motions of the self-propelled vehicle model.

The center of rotation ck is defined in Cartesian notation withrespect to an inertial frame I as

ck = rk + ω−10

[− sin θkcos θk

]I, (3)

where |ω0|−1 is the circle’s radius. Using the center of rotation,the following control expressed in matrix notation produces acircular formation with all-to-all communication [18]

νk = ω0 (1 +KPkcrk) , γk(Rk,θk), (4)

where c = [c1, ..., cN ]T , Rk = [r1 − rk, ..., rN − rk]T , andK > 0. Pk is the kth row of the projector matrix P = IN×N−1N 11T , where 1 = [1, ..., 1]T ∈ RN . This formation is alsoillustrated in Fig. 1.

Note that the circular control law for vehicle k can berepresented in terms of relative velocity orientations, θk, andrelative positions, Rk, expressed as components in a pathreference frame (see Section III-A).

B. Self-Propelled Vehicle Model with Second-Order Steering

Although the first-order vehicle model is useful for studyingvarious group behaviors, it may not adequately representthe rotational dynamics of an actual vehicle. A conventionalvehicle applies a moment to control the rotational accelerationinstead of controlling the heading rate to change direction.Incorporating this observation yields the following dynamics:

xk = cos θkyk = sin θkθk = ωkωk = uk.

(5)

The control laws (2) and (4) derived for the vehicle modelwith first-order steering control can be extended to the vehiclemodel with second-order steering control via a proportionalcontroller that drives the desired turning rate to that of thefirst-order model’s control law. The parallel formation for thismodel becomes [12],

uk = Kp(αk(θk)− ωk), (6)

where αk(θk) is defined in (2) and Kp > 0. A five-vehiclesimulation of this control law is illustrated in Fig. 2.

Theorem 1: The vehicle model (5) with control (6), whereαk(θk) is defined in (2), stabilizes the set of parallel forma-tions in which θk = θj for all pairs k, j and ωk = 0 for allk.

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007 3

Proof: Begin by examining the second-order rotationaldynamics of a single vehicle implementing the parallel controllaw

θk = ωkωk = Kp(K[− sin θk cos θk]pθ − ωk)

(7)

where K < 0, Kp > 0, and pθ = 1N

∑Nk=1 rk. These

dynamics can be expanded to the entire system of N vehiclesusing vector notation as

θ = ωω = Kp(K(∇U)T − ω)

(8)

where U(θ) = 12‖pθ‖2, θ = [θ1, ..., θN ]T , and ω =

[ω1, ..., ωN ]T . Choosing the Lyapunov function

V (θ,ω) = 12ω

Tω −KpKU(θ) ≥ 0, (9)

yields the following derivative with respect to time

V = ωTω −KpK∇U θ= Kp(K∇U − ωT )ω −KpK∇Uω= −Kpω

Tω.(10)

According to the invariance principle, solutions converge tothe largest invariant set in which V = 0, i.e., the set Λ ={ωk ≡ 0,∀ k}. In Λ, ω = ω = 0, which implies ∇U = 0.Therefore, Λ contains the critical points of U(θ) which includeparallel, balanced, and unbalanced configurations. Only the setof parallel formations is stable for K < 0 [18].

Similarly, circular motion can be achieved with modelsusing the following control law [12]

uk = Kp(γk(Rk,θk)− ωk), (11)

where γk(Rk,θk) is defined in (4) and Kp > 0. The collectivebehaviors produced by the first-order model are also exhibitedin this extended model.

Theorem 2: The vehicle model (5) with control (11), whereγk(Rk,θk) is defined in (4), stabilizes the set of circularformations in which ck = cj for all pairs k, j and ωk = ω0

for all k.Proof: Consider the following composite Lyapunov func-

tion

V =KpKω

20

2trace(cTPc) +

1

2

N∑k=1

(ωk − ω0)2 (12)

where c = [c1, ..., cN ]T , Kp > 0, K > 0, and P = IN×N −1N 11T . Taking the derivative with respect to time yields

V =

N∑k=1

KpKω20Pkcrk(1− ω−10 ωk) + (ωk − ω0)ωk

= −Kp

N∑k=1

(ωk − ω0)2 ≤ 0. (13)

According to the invariance principle, solutions converge to thelargest invariant set in which V = 0, i.e., the set Λ = {ωk ≡ω0,∀ k}. In Λ, ωk = ω0 and ωk = 0 for all k, which impliesthat each particle is constantly rotating at ω0. Based on (4), thisconstant rotational control occurs only when Pkc[1 1]T = 0for all k, i.e., each particle is traveling about the same circle.

Theorems 1 and 2 ensure that the proportional controller sta-bilizes both parallel and circular formations with the second-order vehicle model. With assurance that our control designis stable, the next step is to address limitations in the sensoryinformation required to perform the collective behaviors.

III. OBSERVER-BASED FEEDBACK CONTROL DESIGN

The parallel- and circular-formation controls in the previoussection require that each vehicle is aware of the relativevelocity orientation of other vehicles in the group. Here, weassume knowledge of relative position only, and design anobserver to estimate the relative velocity which requires thateach vehicle knows its own turning rate.

A. Dynamic Model of Relative Orientation

Without loss of generality, we begin by examining a pairof particles j and k. Fig. 4 shows particles j and k in aninertial frame, I. Each particle’s position relative to the originis represented by the vectors rj and rk, respectively, while thevector between the particles is represented by rj/k = rj − rk.

x

y

k

j

rj = xj

yj

rk = xk

ykrk

rj

rj/k

IBj

Bk

Fig. 4: Vectors utilized in dynamic model.

An inertial-frame representation is not necessarily knownto each particle. Particle k views the world from path frameBk = (k,xk,yk, zk), which moves with the particle so thatxk is aligned with rk as shown in Fig. 4 and yk = zk × xk,where zk is out of the plane. We express rj/k as componentsin frame Bk as rj/k = xj/kxk + yj/kyk.

Consider the inertial kinematics of j relative to k. Takingthe derivative of rj/k with respect to the inertial frame andexpressing the result in matrix notation with respect to frameI yields [Ivj/k]I =

[Iddt rj/k

]I

= [rj − rk]I

=

[cos θj − cos θksin θj − sin θk

]I.

(14)

In this equation, Ivj/k represents the velocity of particle jwith respect to k in the inertial frame. The subscript I refers tothe coordinate system in which this quantity is expressed. Forexample,

[Ivj/k]I means that the inertial velocity of particlej with respect to particle k is expressed as vector componentsin the inertial frame I.

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007 4

−25 −20 −15 −10 −5 0

−18

−16

−14

−12

−10

−8

−6

−4

−2

0

2

x

y

(a) Trajectories

0 5 10 15 20 25 302

2.5

3

3.5

4

4.5

5

5.5

t(s)

θ k(rad)

(b) Velocity orientations

0 5 10 15 20 25 30

−1

−0.5

0

0.5

1

t(s)

ωk(rad/s)

(c) Turning rates

Fig. 2: A simulation of five self-propelled vehicles performing the parallel control law using a proportional turning rate controller(6); K = −1 and Kp = 1.

−6 −4 −2 0 2 4 6

−4

−3

−2

−1

0

1

2

3

4

5

x

y

(a) Trajectories

0 20 40 60 80 100−5

0

5

10

15

20

25

30

35

t(s)

θ k(rad)

(b) Velocity orientations

0 20 40 60 80 100

−1

−0.5

0

0.5

1

t(s)

ωk(rad/s)

(c) Turning rates

Fig. 3: A simulation of five self-propelled vehicles performing the circular control law using a proportional turning ratecontroller (11) and ω0 = 0.25; K = 1 and Kp = 1.

The inertial kinematics do not contain the relative orienta-tion, θj−θk, which is needed to implement controllers (2) and(4). To obtain the relative orientation, we rewrite the inertialvelocity in particle k’s path frame. The angular velocity of Bkwith respect to I is IωBk = ωkzk. The velocity in the inertialframe can be expressed as components in frame Bk using a 2× 2 rotation matrix to rotate by −θk:[Ivj/k]Bk

=

[cos(θk) sin(θk)− sin(θk) cos(θk)

] [cos θj − cos θksin θj − sin θk

]I

=

[cos(θj − θk)− 1

sin(θj − θk)

]Bk

.

(15)Although the resulting matrix contains the desired relative

orientation, the term on the left is not directly measurable fromthe path frame. It can be related to the path frame velocity,Bkvj/k, using the transport equation [7]:

I ddt

(rj/k) =Bk d

dt(rj/k) + IωBk × rj/k. (16)

In matrix notation,[Ivj/k]Bk=[Bkvj/k

]Bk

+[ωkzk × rj/k

]Bk. (17)

Using rj/k = xj/kxk + yj/kyk andBk ddt (rj/k) = sj/kxk +

vj/kyk yields[cos(θj − θk)− 1

sin(θj − θk)

]Bk

=

[sj/kvj/k

]Bk

+ ωk

[−yj/kxj/k

]Bk

. (18)

Solving for θj − θk yields

θj − θk = arctan

(vj/k + ωkxj/k

1 + sj/k − ωkyj/k

). (19)

Using relationship (19), calculating particle j’s velocityorientation relative to k requires knowledge of k’s turning rateas well as the position and velocity of particle j with respectto k. Assuming that the turning rate, ωk, and relative position,rj/k, are measured, each particle can estimate the relativevelocity, Bkvj/k, in the path frame, Bk, using the estimatordescribed next.

B. Estimation of Relative Velocity Orientation

Consider the case where particle k is estimating the rel-ative velocity of particle j in frame Bk. In this case, letrj/k = xj/kxk + yj/kyk and Bk vj/k = sj/kxk + vj/kykbe the position and velocity estimates, respectively. Also, let4rj/k , rj/k − rj/k and 4Bkvj/k , Bk vj/k − Bkvj/krepresent the estimation errors for position and velocity. Note

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007 5

that we estimate the velocity of particle j with respect toparticle k in frame Bk. Choosing the estimator dynamics

Bk ddt (rj/k) = −K14rj/k + Bk vj/k

Bk ddt (vj/k) = −K24rj/k,

(20)

where K1 > 0 and K2 > 0, yields the following errordynamics:Bk d

dt

[4rj/k4Bkvj/k

]Bk︸ ︷︷ ︸

,ej/k

=

[−K1 1−K2 0

]︸ ︷︷ ︸

,A

[4rj/k4Bkvj/k

]Bk︸ ︷︷ ︸

,ej/k

+

[0

−Bkaj/k

]Bk︸ ︷︷ ︸

,gj/k(t)

.

(21)Observe that the estimator is a linear system of the formej/k = Aej/k + gj/k(t), where gj/k(t) is a time-varyingperturbation equal to the relative acceleration of j with respectto k in frame Bk.

Representing the equations in vector notation is useful forstudying the stability of the system, but the second-ordermodel (5) and relative-orientation relationship (19) utilizeCartesian coordinates with respect to the frame Bk. To beconsistent, we rewrite (20) as

˙xj/k = −K14xj/k + sj/k˙yj/k = −K14yj/k + vj/k˙sj/k = −K24xj/k˙vj/k = −K24yj/k,

(22)

where 4xj/k , xj/k − xj/k and 4yj/k , yj/k − yj/k. Notexj/k and yj/k represent the position estimates, and sj/k andvj/k represent the relative velocity estimates in frame Bk.

For the estimator defined in (21), the perturbation gj/k(t)is not bounded, but can be made arbitrarily small using anappropriate choice of gains as described next.

Lemma 3: The error in the velocity estimation due to theperturbation gj/k(t) defined in (21) is proportional to thepositive quantity

ε ,K2

1 +K2 + 1

K1K2. (23)

Proof: Consider the following Lyapunov function

V = eTj/kPej/k (24)

where ej/k , [4xj/kBk ddt4xj/k 4yj/k

Bk ddt4yj/k]T . The

matrix P is chosen by solving the Lyapunov equation

PA+ATP = −Q (25)

where Q ∈ R4x4 is the identity matrix. For this system,

P = I2×2 ⊗[K2+12K1

− 12

− 12

ε2

]. (26)

Taking the derivative with respect to time yields

V = −eTj/kQej/k + BkaTj/k(4rj/k − ε4Bkvj/k

).(27)

The estimator assumes that the relative position is known;therefore, the error in the position estimate is negligible. Asa result, (27) ensures V ≤ 0 for ||ej/k|| ≥ b, where b isproportional to ε||gj/k(t)||L.

We have not identified an analytic method for optimallychoosing gains K1 and K2, however, the quantity ε defined in(23) can be minimized by choosing K2 � K1 � 1.

C. Observer-Based Feedback ControlLet’s now consider an N -particle system that obeys the

second-order model (5). Each particle utilizes the estimator(22) to determine the relative velocities of the other parti-cles. These estimates are then used to calculate the relativeorientations of the particles using (19). Finally, each particleimplements the desired control using the estimated relativeorientations. The state-space representation of the combinedsystem is

xk = cos(θk)yk = sin(θk)

θk = ωkωk = Kp(νk − ωk)

˙xj/k = −K14xj/k + sj/k˙yj/k = −K14yj/k + vj/k˙sj/k = −K24xj/k˙vj/k = −K24yj/k,

(28)

where k, j = 1, ..., N and νk represents the desired controllaw.

Let

θj − θk = arctan

(vj/k + ωkxj/k

1 + sj/k − ωkyj/k

)(29)

and θk = [θ1 − θk, ..., θN − θk]. Note that the combination ofthe control law and estimator establish the perturbation in (21)as vanishing [8] because vehicles in the desired formation donot move relative to the body frame, Bk. If a vehicle remainsstationary in frame Bk, then Bkvj/k = Bkaj/k = 0.

For a parallel formation, νk in (28) is given by αk(θk) givenin (2). Noting that the parallel control law is a summationof sine terms and the relative orientation calculation uses aninverse tangent, the parallel control law can be simplified usingtrigonometric identities to

αk(θk) = −KN∑Nj=1

vj/k+ωkxj/k√(vj/k+ωkxj/k)2+(1+sj/k−ωkyj/k)2

.

(30)The following result is the product of Lyapunov analysis ofthe combined observer and control dynamics (28).

Theorem 4: Choosing the control νk = αk(θk) defined in(2) ensures that, along solutions of (28), z = [ωT eT ]T isbounded by a quantity proportional to ε given in (23).

Proof: Consider the following composite Lyapunov func-tion

V =1

2ωTω −KpKU(θ) + eT (IN2×N2 ⊗ P )e ≥ 0 (31)

where K < 0, Kp > 0, ω = [ω1, ..., ωN ]T , and N is thenumber of vehicles. Let e be a 4N2 × 1 matrix of estimatorerrors given by

e ,[e1/1 e1/2 . . . e1/N e2/1 . . . eN/N

]T, (32)

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007 6

where ej/k , [4xj/kBk ddt4xj/k 4yj/k

Bk ddt4yj/k]. The

matrix P is chosen by solving the Lyapunov equation, PA+ATP = −Q where Q ∈ R4×4 is the identity matrix. For thissystem,

P = I2×2 ⊗[K2+12K1

− 12

− 12

ε2

], (33)

where ε is defined in (23). Taking the derivative with respectto time yields

V = −KpωTω − eT (IN2×N2 ⊗Q)e−

1T (IN2×N2 ⊗B)(I2×2 ⊗ C)e,(34)

where I is the identity matrix with dimensions given by thesubscript, 1 = [1, ..., 1]T ∈ R4N2

,

B = I2×2 ⊗[−1 00 ε

], (35)

and C is a 2N2 × 2N2 diagonal matrix with diagonal[Bka1/1Bka1/1

Bka1/2Bka1/2 . . .

Bka1/NBka1/N . . . BkaN/N

BkaN/N]T.

(36)

A change of coordinates is used to simplify (34) by letting

z =

[ωe

], z = zT

[Kp(IN×N ) 0

0 I4N2×4N2

]︸ ︷︷ ︸

,D

z, (37)

which yields

V = −zTDz− 1T (IN2×N2 ⊗B)(I2×2 ⊗ C)e. (38)

Note that the second term can be rewritten as the followingdouble summation

N∑k=1

N∑j=1

BkaTj/k(4rj/k − ε4Bkvj/k

), (39)

where 4rj/k is the position error and 4Bkvj/k is the velocityerror. In the context of the problem, we assume that therelative position is measured. Therefore, in steady-state,4rj/kis proportional to the measurement noise, which we ignore.This simplification allows the function of gains to be pulledoutside of the double summation and used to scale this termin the Lyapunov derivative. Under this simplification, V ≤ 0when

zTDz > ε

N∑k=1

N∑j=1

||BkaTj/k4Bkvj/k||. (40)

Hence, solutions that lie outside the bound zTDz =ε∑Nk=1

∑Nj=1 ||BkaTj/k4Bkvj/k|| will approach this bound-

ary. Once on the boundary or inside, solutions will remainthere because V < 0 in the region outside of the boundary.

In Theorem 4, we have some authority over ε through ourchoice of estimator gains. Making ε small reduces the boundon z. (Were we to enforce physical limits on turning rate andseparation distance between vehicles, we could use Theorem4 to establish that z is uniformly bounded.) Simulated resultsof the parallel formation are shown in Fig. 5.

Using a similar formulation described next, the error termsin the observer-based circular control law can also be bounded

by a term proportional to ε. Implementation of the circularcontrol law is achieved using νk = γk(Rk, θk). Note that therelative orientation is used to calculate the centers of rotation(3) in particle k’s path frame.

Theorem 5: Choosing the control νk = γk(θk,Rk) definedin (4) guarantees that along solutions of (28), z = [(ωT −ω01

T ) eT ]T is bounded by a quantity proportional to ε givenin (23).

Proof: Consider the following composite Lyapunov func-tion

V = eT (IN2×N2 ⊗ P )e +KpKω

20

2trace(cTPc) + (41)

1

2

N∑k=1

(ωk − ω0)2, (42)

where c = [c1, ..., cN ]T , Kp > 0, K > 0, and P is theprojector matrix. The vector e and matrix P are defined in(32) and (33), respectively. Taking the derivative with respectto time yields

V = eT (IN2×N2 ⊗Q)e− 1T (IN2×N2 ⊗B)(I2×2 ⊗ C)e

−Kp(ω − ω01)T (ω − ω01), (43)

where ω = [ω1, ..., ωN ]T and Q ∈ R4×4 is the identity matrix.The matrix B is defined in (35). A change of coordinates isused to simplify (43) by letting

z =

[ω − ω01

e

], z = zTDz, (44)

where D is defined in (37), which yields

V = −zTDz− 1T (IN2×N2 ⊗B)(I2×2 ⊗ C)e. (45)

Note that the second term can be rewritten as the followingdouble summation

N∑k=1

N∑j=1

Bkaj/k(4rj/k − ε4Bkvj/k

), (46)

where 4rj/k is the negligible position error and 4Bkvj/kis the velocity error. Under the assumption of noise-freemeasurements, V ≤ 0 when

zTDz > ε

N∑k=1

N∑j=1

||BkaTj/k4Bkvj/k||. (47)

Hence, solutions that lie outside the bound zTDz =ε∑Nk=1

∑Nj=1 ||BkaTj/k4Bkvj/k|| will approach this bound-

ary. Once on the boundary or inside, solutions will remainthere because V < 0 in the region outside the boundary.

We choose K2 � K1 � 1 so that ε is small, allowing thevehicles to approach arbitrarily close to the circular forma-tion. Simulation results in Fig. 6 illustrate the observer-basedfeedback control algorithm converging to a circular formation.Note that the error in the estimates approach zero whichimplies that each vehicle determines the relative position andrelative velocity of the other vehicles in steady-state.

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007 7

−30 −25 −20 −15 −10 −5 0

−15

−10

−5

0

5

10

x

y

(a) Trajectories

0 5 10 15 20 25 300

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

t(s)

||4r j

/k||

(b) Position Estimation Errors

0 5 10 15 20 25 300

2

4

6

8

10

12

14

16

18

20

t(s)

||4B k

vj/k||

(c) Velocity Estimation Errors

Fig. 5: A simulation of five self-propelled vehicles performing the observer-based parallel control law (28) with a proportionalturning rate controller; K = −1, Kp = 1, K1 = 10, and K2 = 100.

−4 −2 0 2 4 6

0

1

2

3

4

5

6

7

8

x

y

(a) Trajectories

0 20 40 60 80 1000

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

t(s)

||4r j

/k||

(b) Position estimation errors

0 20 40 60 80 1000

2

4

6

8

10

12

14

16

18

20

t(s)

||4B k

vj/k||

(c) Velocity estimation errors

Fig. 6: A simulation of five self-propelled vehicles performing the observer-based circular control law (28) with a proportionalturning rate controller and ω0 = .25; K = 1, Kp = 1, K1 = 10, and K2 = 100.

IV. SIMULATED SUBMARINE DYNAMICS & CONTROL

The vehicle model used above is useful in developingcontrol laws, but does not take into account the translationaldynamics of an actual vehicle. Therefore, we have developed ahigher fidelity model of a miniature submarine to validate thealgorithm. The submarine model has six-degrees of freedomand obeys Euler’s equations of motion. Fig. 7 displays a free-body diagram of the six forces acting on the vehicle: buoyancy,Fb, gravity, Fg , drag, Fd, thrust, Ft, rudder, Fr, and diveplanes, Fe. A proportional-integral controller is implementedto regulate depth by deflecting the dive planes. (The integralterm is required to find the elevator deflection that counteractsthe buoyancy force.)

Fb

Fg

Fr

(a) Front view

Fb

Fg

Fe

Ft

Fd

(b) Side view

Fig. 7: Free-body diagram of submarine model.

For the collective control law implementation, knowledge ofthe relative horizontal velocity orientation, θk − θj , betweenthe vehicles is required. Note that the extension to a three-dimensional rigid-body requires that we further define therelative velocity orientation as the difference in orientation ofeach vehicle’s velocity projected onto the horizontal plane.We approximate this quantity by assuming that each vehicle’splanar velocity heading is aligned with its yaw angle ψk.Although this assumption is not true in general due to thevehicle’s sideslip velocity, the sideslip velocity is zero whentraveling in a parallel formation [13]. The parallel control lawis

uk = Kp(φk(ψ)− ωk), (48)

where ψk = [ψ1 − ψk, ..., ψN − ψk]T . A five-submarinesimulation of this control is shown in Fig. 8 from an overheadview. Note that just like the vehicle model, each submarineconverges to the desired formation.

The assumption that the submarine’s velocity heading iswell approximated by its yaw angle does not hold for circularmotion. While traveling in a circle, the submarine modelexhibits a sideslip angle βk [13]. The circular control law (4)becomes

νk = ω0(sk +KPkc[cos(ψk − βk) sin(ψk − βk)]T ), (49)

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007 8

−20 −10 0 10 20 30 40

0

5

10

15

20

25

30

35

40

45

50

x(m)

y(m

)

(a) Trajectories

0 10 20 30 40 50 600

0.5

1

1.5

2

2.5

t(s)

ψk(rad)

(b) Yaw angle

0 10 20 30 40 50 60−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

0.5

t(s)

ψk(rad/s)

(c) Yaw rate

Fig. 8: A simulation of five model submarines performing the parallel control law (48); K = −1 and Kp = 1.

where c = [c1, ..., cN ]T , sk is submarine k’s speed, and ck =rk + ω−10 [− sin(ψk − βk) cos(ψk − βk)]T

I. A five vehicle

simulation of this implementation is shown in Fig. 9 from anoverhead view. Note that each vehicle’s turning rate convergesto skω0 and βk converges to a constant.

The relative velocity estimator was adapted for the subma-rine model. Based on how the estimator was derived, eachsubmarine’s estimates take place in a path frame aligned withits horizontal velocity. By utilizing the estimator onboard themodel, there is no need to know the other vehicle’s sideslipangle because the estimates will contain that information.Therefore, each submarine only needs to know its own sideslipangle to perform the desired control.

The addition of the estimator with the parallel formationcaused oscillations about the desired heading due to estimationerror. Understanding that the estimator error was induced bythe rotational movement of the body, the K gain was scheduledaccording to K = 9.99||pθ|| − 10, where pθ is the averageof the vehicles’ unit velocities. This choice slows down thespeed of convergence as well as the rotational movement of themodel. The reduction in turning rate and turning accelerationreduces the perturbation and allows the estimates to converge;ultimately, the formation converges as well.

Fig. 10 and Fig. 11 display the parallel and circular forma-tions using the estimated quantities. Note that in the submarinemodel, the estimates do not converge to zero for the circularcase. This observation is attributed to the high rudder controleffort when approaching the desired formation. Instead ofsettling to a constant offset, the rudder oscillates around thatoffset because the estimates have not completely converged.The rudder actuation amplifies the time-varying perturbationthat induces error in the estimates as well. Nonetheless theestimation error remains bounded, and the circular formationconverges as shown in Fig. 11a.

V. EXPERIMENTAL RESULTS WITH LABORATORY TESTBED

The control laws and estimator described in Section IIIand IV are designed using an idealized modeling framework.This technique allows higher level control laws to be eval-uated for stability and convergence without the need for aspecific system model to be utilized. In order to demonstrate

the usefulness of the control laws, we have implementedthem on an underwater-vehicle testbed (see Fig. 12). Thetestbed is operated in the University of Maryland’s NeutralBuoyancy Research Facility, which is 25 feet deep, 50 feetacross, and outfitted with twelve Qualisys underwater motion-capture cameras. Each submarine is a radio-controlled, 1:60scale model of the U.S.S. Albacore and uses an onboardmicroprocessor and sensors to steer. (Additional details aboutthe testbed are available in [21].)

Qualisys

Yaw Rate Gyroscope

Turning Rate Controller

Depth Controller

Cooperative Control Law

Onboard Control Loop: 50Hz

Topside Control Loop: 20Hz

+

+

-

-

Submarine

Fig. 13: The multi-vehicle control architecture for the under-water vehicle testbed.

The multi-vehicle control system functions using inner andouter control loops. Fig. 13 shows a block diagram of thecontrol architecture. The inner loop, which runs at 50Hz on themicroprocessor onboard the submarine, is used to stabilize thedesired turning rate using state feedback. The microprocessorserves two separate functions. First, the submarine’s yaw rateis stabilized to a desired rate via state-feedback control pro-vided by a gyroscope fixed to the vehicle. The microprocessoralso serves as an analyzer for the desired yaw-rate value thatis passed from the transmitter to the receiver. The outer loopruns at 20 Hz from the motion capture computer. The loopreceives each submarine’s position and orientation from thecameras and computes the desired turning rate and dive planedeflections. The data is transmitted wirelessly to the submarinevia a radio-frequency transmitter.

Experimental validation of the control laws was first per-formed using a single submarine and a virtual vehicle. For

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007 9

−6 −4 −2 0 2 4 6 8 10

−6

−4

−2

0

2

4

6

x(m)

y(m

)

(a) Trajectories

0 50 100 150

5

10

15

20

25

30

35

40

t(s)

ψk(rad)

(b) Yaw angle

0 50 100 150−0.5

−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

0.5

t(s)

ψk(rad/s)

sω0

(c) Yaw rate

Fig. 9: A simulation of five model submarines performing the circular control law (49) with ω0 = 0.25; K = 1 and Kp = 1.

0 20 40 60 80 100 120

−10

0

10

20

30

40

50

60

70

80

x(m)

y(m

)

(a) Trajectories

0 50 100 1500

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

t(s)

||4r j

/k||

(b) Position Estimation Errors

0 50 100 1500

5

10

15

t(s)

||4B k

vj/k||

(c) Velocity Estimation Errors

Fig. 10: A simulation of five model submarines performing the parallel control law (48); K = 9.99||pθ||−10 Kp = 1, K1 = 10and K2 = 100.

a parallel formation, the virtual vehicle provides a referencehorizontal velocity orientation [19]. Fig. 14 shows a test run inwhich the virtual vehicle travels along the positive x axis andthe submarine changes course to align with this heading. Fortest runs of the circular formation, the virtual vehicle travelsaround the center of the tank with a radius of 2.5 meters.The submarine consistently circles the center of the tank atthe desired radius. At the end of the test run, the desiredcenter of rotation and the submarine’s center of rotation areapproximately 0.1 meters apart. These experimental resultsdemonstrate that the control laws and onboard turning-ratecontroller are performing as expected.

The next set of experimental runs uses two submarinesperforming the desired control law. Fig. 15 shows a test runof two submarines performing the parallel control law. Theinitial conditions were chosen such that the final orientationshould utilize the maximum area of the tank. In this case, thesubmarine’s yaw orientation started with approximately 60◦

error and by the end of the run had less than 3◦ error.

The circular control was also tested with two submarines. Inthese tests, the submarines attempt to converge to an arbitrarycircular formation with a 3 meter radius. Though not perfect,the results are promising because the vehicles are rotatingin the correct direction and the centroid difference is notincreasing. Fig. 17 shows the results of implementing the

observer-based feedback control algorithm. The addition ofthe observer appears to degrade the performance comparedto the two submarine case, though the final error is lessthan 20◦. Fig. 18 shows the results from implementing theobserver-based method with the circular control law. Theperformance is similar to the two-submarine test of the circularcontrol law. We observe each vehicle turning in the correctdirection, but the error in the centroid agreement varies intime. Possible error sources include measurement noise, modelapproximation error, and tracking accuracy. We also use time-differencing to extract the inertial velocity from the motioncapture’s position data, which may introduce additional errorin the definition of each submarine’s path frame.

Although the algorithm may contribute to the observedperformance, the testbed itself may also introduce additionalerror sources. For example, the submarines must operatein a band of water that is deep enough for the motion-capture system to see, but not so deep that radio transmissionis compromised. Under the loss of radio transmission, thesubmarine will enter a safe mode which stops the propelleruntil a connection is re-established. Similarly, when trackingis lost, the submarine will enter a hold mode where the lastknown settings are utilized. Both of these modes deviate fromthe desired behavior, but are necessary to ensure safety of thesubmarine and the experimental facility.

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007 10

−10 −5 0 5

−8

−6

−4

−2

0

2

4

6

x(m)

y(m

)

(a) Trajectories

0 20 40 60 80 100 1200

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

t(s)

||4r j

/k||

(b) Position Estimation Errors

0 20 40 60 80 100 1200

5

10

15

t(s)

||4B k

vj/k||

(c) Velocity Estimation Errors

Fig. 11: A simulation of five model submarines performing the circular control law (49) with ω0 = 0.25; K = .1, Kp = 1,K1 = 10, and K2 = 100.

(a) Miniature submarines (b) Underwater testing facility (c) Underwater cameras

Fig. 12: The underwater vehicle testbed consists of six submarines that operate in the Neutral Buoyancy Research Facility atthe University of Maryland and twelve underwater cameras used for motion tracking.

−5 0 5−8

−6

−4

−2

0

2

4

6

8

Dep

th(m

)

x(m)

y(m

)

0

0.5

1

1.5

2

2.5

3

3.5

4

(a) Parallel formation

−5 0 5−8

−6

−4

−2

0

2

4

6

8

Dep

th(m

)

x(m)

y(m

)

0

0.5

1

1.5

2

2.5

3

3.5

4

(b) Circular formation

Fig. 14: An experimental run of a submarine performing the parallel (48) and circular (49) multi-vehicle control laws with avirtual vehicle.

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007 11

−5 0 5−8

−6

−4

−2

0

2

4

6

8

Dep

th(m

)

x(m)

y(m

)

0

0.5

1

1.5

2

2.5

3

3.5

4

(a) Coordinated trajectories

0 2 4 6 8 10 120

10

20

30

40

50

60

70

80

90

t(s)

|θ 2−θ 1|(d

egrees)

Run 1Run 2Run 3

(b) Error in relative horizontal orientation

Fig. 15: An experimental run of two submarines performing the parallel control law.

−5 0 5−8

−6

−4

−2

0

2

4

6

8

Dep

th(m

)

x(m)

y(m

)

0

0.5

1

1.5

2

2.5

3

3.5

4

(a) Coordinated trajectories

0 5 10 15 20 250

5

10

15

t(s)

||c2−c 1||(m)

Run 1Run 2Run 3

(b) Error in centroid positions

Fig. 16: An experimental run of two submarines performing the circular control law.

−5 0 5−8

−6

−4

−2

0

2

4

6

8

Dep

th(m

)

x(m)

y(m

)

0

0.5

1

1.5

2

2.5

3

3.5

4

(a) Coordinated trajectories

0 2 4 6 8 100

10

20

30

40

50

60

70

80

90

t(s)

|θ 2−

θ 1|(d

egrees)

(b) Error in relative horizontal orientation

Fig. 17: Experimental test run of two submarines performing the parallel control law (48) using the observer-based feedbackcontrol algorithm (28).

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007 12

−5 0 5−8

−6

−4

−2

0

2

4

6

8

Dep

th(m

)

x(m)

y(m

)

0

0.5

1

1.5

2

2.5

3

3.5

4

(a) Coordinated trajectories

0 10 20 30 400

5

10

15

t(s)

||c2−

c1||(m)

(b) Error in centroid positions

Fig. 18: Experimental test run of two submarines performing the circular control law (49) using the observer-based feedbackcontrol algorithm (28).

VI. CONCLUSION

This paper describes an observer-based decentralized feed-back control algorithm to stabilize parallel and circular forma-tions using an all-to-all communication topology. The exten-sion to a limited communication topology is more than likelypossible based on previous work with a first-order particlemodel [19]. The proposed algorithm is theoretically justifiedfor a second-order vehicle model, and simulations illustrateconvergence to the desired formation. In addition, the coop-erative control algorithms are extended to a three-dimensionalrigid-body submarine model with appropriate rotational dy-namics. Simulations using this model reinforce the theoreticalresults obtained by the idealized version. A laboratory-scaletestbed of underwater vehicles is also described along withcorresponding experimental results. Test runs using a virtualvehicle validate the parallel and circular formation controllers.Results from tests of the cooperative control algorithms withmultiple vehicles are also presented. The parallel formationwas achieved with and without the observer. For circularformations, further research is necessary to understand howthe combination of vehicle sensing, dynamics, and controlimpacts performance. For example, we are examining thestability of the formation in the presence of sensor noise. Astochastic formulation of this problem would provide insightinto the performance of the submarine testbed. Additionally,while beyond the scope of this paper, the impact of externaldisturbances such as water currents is the topic of ongoingresearch [15], [14].

ACKNOWLEDGMENT

The authors would like to specifically recognize NitinSydney and Levi DeVries for their contributions to the testingand maintenance of the submarine fleet. We would alsolike to acknowledge the support of many other members ofthe Collective Dynamics and Control Laboratory and SpaceSystems Laboratory.

REFERENCES

[1] Arturo Buscarino, Luigi Fortuna, Mattia Frasca, and Alessandro Rizzo.Distributed control of collective motion of robots through mainly localinteractions. Structure, pages 1–6, 2006.

[2] Yongcan Cao, Wei Ren, and Ziyang Meng. Decentralized finite-timesliding mode estimators and their applications in decentralized finite-time formation tracking. Systems and Control Letters, 59(9):522–529,2010.

[3] Haiyang Chao and Yang-Quan Chen. Cooperative sensing and dis-tributed control of a diffusion process using centroidal voronoi tessella-tions. Numer: Math. Theor. Meth. Appl., 3(2):162–177, May 2010.

[4] Wei Ding, Gangfeng Yan, and Zhiyun Lin. Collective motions and for-mations under pursuit strategies on directed acyclic graphs. Automatica,46(1):174–181, 2010.

[5] A. J. Healey. Application of formation control for multi-vehicle roboticminesweeping. Proceedings of the 40th IEEE Conference on Decisionand Control, 2:1497–1502, Orlando, Florida, 2001.

[6] C H Hsieh, D Marthaler, B Q Nguyen, D J Tung, A L Bertozzi, andR M Murray. Experimental validation of an algorithm for cooperativeboundary tracking. Proceedings of the American Control Conference,2:1078–1083, 2005.

[7] N. J. Kasdin and D. A. Paley. Engineering Dynamics: A ComprehensiveIntroduction. Princeton University Press, 1st. edition, 2011.

[8] H. Khalil. Nonlinear Systems. Prentice Hall, Upper Saddle River, NJ07458, 3rd. edition, 2002.

[9] Daniel J. Klein, Patrick K. Bettale, Benjamin I. Triplett, and Kristi A.Morgansen. Autonomous underwater multivehicle control with limitedcommunication: Theory and experiment. 2008.

[10] Naomi Ehrich Leonard, Derek A. Paley, Francois Lekien, RodolpheSepulchre, David M. Fratantoni, and Russ E. Davis. Collective motion,sensor networks, and ocean sampling. Proceedings of the IEEE,95(1):48–74, 2007.

[11] Po-Hsiung Lin. The first successful typhoon eyewall-penetration re-connaissance flight mission conducted by the unmanned aerial ve-hicle, aerosonde. Bulletin of the American Meteorological Society,87(11):1481–1483, 2006.

[12] R. Mellish, S. Napora, and D. A. Paley. Backstepping control designfor motion coordination of self-propelled vehicles in a flowfield. Inter-national Journal of Robust and Nonlinear Control, 21(12):1452–1466,2011.

[13] R. Mellish and D. A. Paley. Motion coordination of planar rigid bodies.Proceedings of the IEEE Conference on Decision and Control, pages4897–4902, December 2011.

[14] D. A. Paley and C. Peterson. Stabilization of collective motion in a time-invariant flowfield. AIAA Journal of Guidance, Control, and Dynamics,32(3):771–779, 2009.

[15] C. Peterson and D. A. Paley. Multi-vehicle coordination in an esti-mated time-varying flowfield. AIAA Journal of Guidance, Control, andDynamics, 34(1):177–191, 2011.

JOURNAL OF LATEX CLASS FILES, VOL. 6, NO. 1, JANUARY 2007 13

[16] D. O. Popa, A. C. Sanderson, R. J. Komerska, S. S. Mupparapu, D. R.Blidberg, and S. G. Chappel. Adaptive sampling algorithms for multipleautonomous underwater vehicles. Autonomous Underwater Vehicles,pages 108–118, 2004.

[17] M. Rubenstein and R. Nagpal. Kilobot: A robotic module for demon-strating collective behaviors. Proceeding of the IEEE Conference onRobotics and Automation, Anchorage, Alaska, 2010.

[18] R. Sepulchre, D. A. Paley, and N. Leonard. Stabilization of planarcollective motion: All-to-all communication. IEEE Transactions onAutomatic Control, 52(5):811–824, 2007.

[19] R. Sepulchre, D. A. Paley, and N. E. Leonard. Stabilization of planarcollective motion with limited communication. IEEE Transactions onAutomatic Control, 53(3):706–719, 2008.

[20] Housheng Su, Xiaofan Wang, and Zongli Lin. Flocking of multi-agents with a virtual leader. IEEE Transactions on Automatic Control,54(2):293–307, 2009.

[21] N. Sydney, S. Napora, and D. A. Paley. A multi-vehicle testbed forunderwater motion coordination. Proceeding of the 2010 Workshopon the Performance Evaluation for Intelligent Systems (electronic),Baltimore, Maryland, 2010.

[22] L. Techy, D. A. Paley, and C. A. Woolsey. UAV coordination on convexcurves in wind: An environmental sampling application. Proceedingsof the European Control Conference, pages 4967–4972, Budapest,Hungary, 2009.

[23] L. Techy, C. A. Woolsey, and D. G. Schmale. Path planning for efficientUAV coordination in aerobiological sampling missions. Proceedings ofthe 47th IEEE Conference on Decision and Control, pages 2814–2819,Cancun, Mexico, 2008.

Seth Napora is a guidance, navigation, and controlengineer with ATK Space Systems Inc. He receivedthe B.S. degree in aerospace engineering from theUniversity of Maryland in 2009 and the M.S. degreein aerospace engineering with a specialization inflight dynamics and control from the University ofMaryland in 2011.

Derek A. Paley is an associate professor in theDepartment of Aerospace Engineering at the Uni-versity of Maryland. He is the founding directorof the Collective Dynamics and Control Laboratoryand a member of the Vertical Lift Rotorcraft Cen-ter of Excellence, the Maryland Robotics Center,and the Program in Neuroscience and CognitiveScience; he is an affiliated member of the Insti-tute for Systems Research. Paley received the B.S.degree in Applied Physics from Yale Universityin 1997 and the Ph.D. degree in Mechanical and

Aerospace Engineering from Princeton University in 2007. He received theNational Science Foundation CAREER award in 2010 and is co-authorof Engineering Dynamics: A Comprehensive Introduction. His research inter-ests are in the area of dynamics and control, including cooperative control ofautonomous vehicles, adaptive sampling with mobile networks, and spatialmodeling of biological groups.