observer-based feedback control for stabilization of collective motion

1846 IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 21, NO. 5, SEPTEMBER 2013

Observer-Based Feedback Control forStabilization of Collective Motion

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

Abstract— Multivehicle control for collective motion has appli-cations in environmental sampling in the atmosphere and oceans.Previous works in this field have 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 controlalgorithm for the stabilization of parallel and circular motionusing measurements of the 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 3-D model of a miniature submarine. Thealgorithm has also been implemented on a laboratory-scale multi-vehicle underwater testbed. We describe the results of experi-mental validation using motion-capture-based feedback control inthe University of Maryland’s Neutral Buoyancy Research Facility.

Index Terms— Distributed control, marine technology, mobilerobots, ocean sampling, sensor networks, underwater vehicles.

I. INTRODUCTION

THE MOTIVATION for pursuing coordinated collectivemotion of autonomous vehicles comes from the desire

to estimate rapidly evolving spatiotemporal processes usingmobile sensor networks. A collection of vehicles may bebetter suited to sample an environmental phenomenon thanan individual platform because the collection can gathermultiple simultaneous measurements over a larger area. Forexample, multiple unmanned aerial vehicles performing envi-ronmental sampling can further the understanding of the rapidintensification of tropical cyclones [1] and transmission ofairborne pathogens [2], [3]. Similarly, sampling of oceanicprocesses for greater sonar performance prediction can benefitfrom multi-vehicle cooperation [4], [5]. Other applicationsinclude underwater minesweeping [6], diffusion mapping andspraying [7], and boundary tracking [8] for oil spills andalgae growth.

Prior work in the field of collective motion has producedmany control algorithms for vehicles modeled as self-propelled

Manuscript received August 8, 2011; revised March 27, 2012; acceptedJune 9, 2012. Manuscript received in final form June 16, 2012. Date ofpublication July 24, 2012; date of current version August 12, 2013. Thiswork was supported in part by the National Science Foundation underGrant CMMI0928416 and Grant CMMI0954361 and by the Office of NavalResearch under Grant N00014-09-1-1058. Recommended by Associate EditorN. K. Kazantzis.

S. Napora is with ATK Space Systems Inc., Beltsville, MD 20705 USA(e-mail: [email protected]).

D. Paley is with the Department of Aerospace Engineering and the Institutefor Systems Research, University of Maryland, College Park, MD 20742 USA(e-mail: [email protected]).

Digital Object Identifier 10.1109/TCST.2012.2205252

particles. In [9], theoretically justified control laws for thismodel are provided to stabilize synchronized, balanced, andcircular formations. The authors in [10] and [11] build uponthese control laws and adapt them to function in the pres-ence of a spatially and temporally varying flowfield. Theauthors in [12] provide a second-order steering control fora self-propelled vehicle model using backstepping as an alter-native to proportional control. The authors in [13] examinecollective motion via pursuit dynamics, where a leader vehicleperformed a behavior and the other vehicles pursued theleader. Taking another approach, [14] examined the effects oflong-range connections on collective behaviors.

A challenge to achieving collective motion is the stabiliza-tion of moving formations with limited information. In [15],flocking behavior of agents is described whereby only acertain number of agents are informed of the desired behavior.This restriction is also described in [16], in the contextof a self-propelled particle system with limited communi-cation between agents. Information can also be limited bysensing capabilities. In this case, other approaches such asestimation must be taken into consideration to determine themissing information. In [17], limited sensing is overcomeusing sliding-mode estimators to achieve formation tracking.

Additional research into cooperative control involves theexperimental validation of the proposed control algorithm.Validation can be achieved through a variety of plat-forms ranging from aircraft to submersibles. The researchersin [18] designed a cost-effective ground platform capableof self-assembly. The authors of [19] utilized a fin-actuatedplatform to stabilize parallel and balanced formations ofunderwater vehicles. The authors in [3] and [4] utilized vehi-cles capable of waypoint navigation to perform the desiredbehavior.

In this paper, parallel and circular formations are studiedusing 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 asenvironmental sampling [4]. Previous works on these collec-tive behaviors indicate that each vehicle requires knowledgeof the relative position and relative velocity orientation of theother vehicles in the group. Here, we assume that each vehicleis capable of sensing only the relative position of other vehiclesas well as its own turning rate. The main contributions of thispaper are to present theoretically justified methods for: 1) esti-mating the velocity of one vehicle relative to another vehicleand 2) utilizing that estimate in an observer-based feedbackcontrol to stabilize parallel and circular formations of multiple

1063-6536 © 2012 IEEE

NAPORA AND PALEY: OBSERVER-BASED FEEDBACK CONTROL FOR STABILIZATION OF COLLECTIVE MOTION 1847

self-propelled vehicles with second-order rotational dynamics.The cooperative control algorithms are implemented on a 3-Dsubmarine model to simulate a more realistic performance, andare experimentally validated using a laboratory-scale testbedof multiple underwater vehicles.

The outline for this paper is as follows. Section II presentskinematic and dynamic models of self-propelled vehiclemotion, including control laws that stabilize parallel andcircular 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 3-Drigid-body submarine model and results from simulating thecorresponding control implementation. Section V discussesresults from experimental validation using an underwatervehicle testbed. Section VI summarizes the results andongoing work.

II. PARTICLE DYNAMICS AND

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 [9]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 [9] 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 1. The inertial position ofvehicle k is denoted rk = [xk yk]T

I , and the orientation of its(planar, unit) velocity by θk . The steering control νk is appliedto the heading rate allowing the vehicle to change course asindicated by the following equations of motion:

xk = cos θk

yk = 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 [9].

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

νk = − K

N

N∑

j=1

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

where θk = [θ1 − θk, . . . , θN − θk]T . Note that the absoluteorientations of the other vehicles’ velocities are not requiredfor control νk ; only the relative orientations suffice. The choiceof control gain K influences the convergence speed of the

formation as well as the formation type. Choosing K<0 in (2)produces straight-line motion where all the vehicle trajectoriesare parallel [9]. Choosing K >0 yields balanced motion; thisbehavior occurs when the sum of all vehicles’ velocities isequal to zero. These motions are illustrated in Fig. 1.

A circular formation is achieved when each vehicle’s turningrate and center of rotation is identical to the rest of the group.The center of rotation ck is defined in Cartesian notation withrespect to an inertial frame I as

ck = rk + ω−10

[− sin θk

cos θ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 [9]

ν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 −(1/N)11T , where 1 = [1, . . . , 1]T ∈ R

N . This formation isalso illustrated 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 θk

yk = 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 = K p(αk(θ k)− ωk) (6)

where αk(θ k) is defined in (2) and K p > 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 formationsin which θk = θ j for all pairs k, j and ωk = 0 for all k.

Proof: Begin by examining the second-order rotationaldynamics of a single vehicle implementing the parallelcontrol law

θk = ωk

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


(a) (b) (c)

Fig. 1. Collective motions of the self-propelled vehicle model. (a) Parallelformation. (b) Balanced formation. (c) Circular formation.

where K<0, K p>0, and pθ = (1/N)∑N

k=1 rk . Thesedynamics can be expanded to the entire system of N vehiclesusing vector notation as

θ = ω

ω = K p

(K (∇U)T − ω

)(8)

where U(θ) = (1/2)‖pθ‖2, θ = [θ1, . . . , θN ]T , and ω =[ω1, . . . , ωN ]T . Choosing the Lyapunov function

V (θ ,ω) = 1

2ωTω − K p K U(θ) ≥ 0 (9)

yields the following derivative with respect to time:V = ωTω − K p K∇U θ

= K p

(K∇U − ωT

)ω

= −K p K∇Uω − K pω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 [9].

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

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

where γk(Rk, θ k) is defined in (4) and K p > 0. The collectivebehaviors produced by the first-order model are also exhibitedin this extended model. A five-vehicle simulation of thiscontrol law is illustrated in Fig. 3.

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 = c j for all pairs k, j and ωk = ω0for all k.

Proof: Consider the following composite Lyapunovfunction:

V = K p Kω20

2trace

(cTPc

)+ 1

2

N∑

k=1

(ωk − ω0)2 (12)

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

V =N∑

k=1

K p Kω20Pkcrk(1 − ω−1

0 ωk)+ (ωk − ω0)ωk

= −K p

N∑

k=1

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

According to the invariance principle, the solutions convergeto the largest invariant set in which V = 0, i.e., the set� = {ωk ≡ ω0,∀ k}. In �, ωk = ω0 and ωk = 0 for allk, which implies that each particle is constantly rotating atω0. Based on (4), this constant rotational control occurs onlywhen Pkc[1 1]T = 0 for all k, i.e., each particle is travelingabout the same circle.

Theorems 1 and 2 ensure that the proportional controllerstabilizes both parallel and circular formations with thesecond-order vehicle model. With the assurance thatour control design is stable, the next step is to addressthe limitations in the sensory information required to performthe 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 r j and rk , respectively, while thevector between the particles is represented by r j/k = r j − rk .

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 that xk

is aligned with rk as shown in Fig. 4 and yk = zk ×xk , wherezk is out of the plane. We express r j/k as components in frameBk as r j/k = x j/kxk + y j/kyk .

Consider the inertial kinematics of j relative to k. Takingthe derivative of r j/k with respect to the inertial frame andexpressing the result in matrix notation with respect to frameI yields

[Iv j/k

]

I =[Id

dtr j/k

]

I= [r j − rk]I

=[

cos θ j − cos θk

sin θ j − sin θk

]

I. (14)

In this equation, Iv j/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,

[Iv j/k]I means that the inertial velocity of particle

j with respect to particle k is expressed as vector componentsin the inertial frame I.

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 ofBk with respect to I is IωBk = ωkzk . The velocity in theinertial frame can be expressed as components in frame Bk


−25 −20 −15 −10 −5 0

−18

−16

−14

−12

−10

−8

−6

−4

−2

0

2

x

y

(a)

0 5 10 15 20 25 302

2.5

3

3.5

4

4.5

5

5.5

t(s)

θ k(r

ad)

(b)

0 5 10 15 20 25 30

−1

−0.5

0

0.5

1

t(s)

ωk(r

ad/s )

(c)

Fig. 2. Simulation of five self-propelled vehicles performing the parallel control law using a proportional turning rate controller (6); K = −1 and K p = 1.(a) Trajectories. (b) Velocity orientations. (c) Turning rates.

−6 −4 −2 0 2 4 6

−4

−3

−2

−1

0

1

2

3

4

5

x

y

(a)

0 20 40 60 80 100−5

0

5

10

15

20

25

30

35

t(s)

θ k(r

ad)

(b)

0 20 40 60 80 100

−1

−0.5

0

0.5

1

t(s)

ωk(r

ad/s)

(c)

Fig. 3. Simulation of five self-propelled vehicles performing the circular control law using a proportional turning rate controller (11) and ω0 = 0.25; K = 1and K p = 1. (a) Trajectories. (b) Velocity orientations. (c) Turning rates.

x

y

k

j

rj = xj

yj

rk = xk

ykrk

rj

rj/k

IBj

Bk

Fig. 4. Vectors utilized in the dynamic model.

using a 2 × 2 rotation matrix to rotate by −θk[Iv j/k

]

Bk=

[cos(θk) sin(θk)

− sin(θk) cos(θk)

] [cos θ j − cos θk

sin θ j − sin θk

]

I

=[

cos(θ j − θk)− 1sin(θ j − θk)

]

Bk

. (15)

Although the resulting matrix contains the desired relativeorientation, the term on the left is not directly measurable fromthe path frame. It can be related to the path frame velocityBk v j/k using the transport equation [20]

I d

dt

(r j/k

) =Bk d

dt

(r j/k

) + IωBk × r j/k . (16)

In matrix notation[Iv j/k

]

Bk=

[Bk v j/k

]

Bk+ [ωkzk × r j/k

]Bk. (17)

Using r j/k = x j/kxk + y j/kyk andBk(d/dt)(r j/k) = s j/kxk +

v j/kyk yields[

cos(θ j − θk)− 1sin(θ j − θk)

]

Bk

=[

s j/k

v j/k

]

Bk

+ ωk

[−y j/k

x j/k

]

Bk

. (18)

Solving for θ j − θk yields

θ j − θk = arctan

(v j/k + ωk x j/k

1 + s j/k − ωk y j/k

). (19)

Using (19), calculating particle j ’s velocity orientationrelative to k requires knowledge of k’s turning rate as wellas the position and velocity of particle j with respect to k.Assuming that the turning rate ωk and relative position r j/k

are measured, each particle can estimate the relative velocityBk v j/k in the path frame Bk using the estimator described next.

B. Estimation of Relative Velocity Orientation

Consider the case where particle k is estimating the relativevelocity of particle j in frame Bk . In this case, let r j/k =x j/kxk + y j/kyk and Bk v j/k = s j/kxk + v j/kyk be the positionand velocity estimates, respectively. Also, let r j/k � r j/k −r j/k and Bk v j/k � Bk v j/k − Bk v j/k represent the estimationerrors for position and velocity. Note that we estimate the


velocity of particle j with respect to particle k in frame Bk .Choosing the estimator dynamics

Bk d

dt

(r j/k

) = −K1r j/k + Bk v j/k

Bk d

dt

(v j/k

) = −K2r j/k (20)

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

dt

[ r j/k

Bk v j/k

]

Bk︸︷︷︸�e j/k

=[−K1 1−K2 0

]

︸︷︷︸�A

[ r j/k

Bk v j/k

]

Bk︸︷︷︸�e j/k

+[

0−Bk a j/k

]

Bk︸︷︷︸�g j/k(t)

.

(21)

Observe that the estimator is a linear system of the forme j/k = Ae j/k + g j/k(t), where g j/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-order(5) and relative-orientation relationship (19) utilize Cartesiancoordinates with respect to the frame Bk . To be consistent, werewrite (20) as

˙x j/k = −K1x j/k + s j/k

˙y j/k = −K1y j/k + v j/k

˙s j/k = −K2x j/k

˙v j/k = −K2y j/k (22)

where x j/k � x j/k − x j/k and y j/k � y j/k − y j/k. Notex j/k and y j/k represent the position estimates, and s j/k andv j/k represent the relative velocity estimates in frame Bk .

For the estimator defined in (21), the perturbation g j/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 g j/k(t) defined in (21) is proportional to thepositive quantity

ε �K 2

1 + K2 + 1

K1 K2. (23)

Proof: Consider the Lyapunov function

V = eTj/k Pe j/k (24)

where e j/k � [x j/kBk(d/dt)x j/k y j/k

Bk(d/dt)y j/k]T.

The matrix P is chosen by solving the Lyapunov equation

P A + AT P = −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/k Qe j/k + Bk aT

j/k

(r j/k − εBk v j/k

). (27)

The estimator assumes that the relative position is known;therefore, the error in the position estimate is negligible.

As a result, (27) ensures V ≤ 0 for ||e j/k|| ≥ b, where bis proportional to ε||g j/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 Control

Let us now consider an N-particle system that obeys thesecond-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 = K p(νk − ωk)

˙x j/k = −K1x j/k + s j/k

˙y j/k = −K1y j/k + v j/k

˙s j/k = −K2x j/k

˙v j/k = −K2y j/k (28)

where k, j = 1, . . . , N and νk represents the desiredcontrol law.

Let

θ j − θk = arctan

(v j/k + ωk x j/k

1 + s j/k − ωk y j/k

)(29)

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

For a parallel formation, νk in (28) is given by αk(θ k) givenin (2). Noting that the parallel control law is a summation ofsine terms and that the relative orientation calculation usesan inverse tangent, the parallel control law can be simplifiedusing trigonometric identities to

αk

(θ k

)= − K

N

N∑j=1

v j/k+ωk x j/k√(v j/k+ωk x j/k)

2+(1+s j/k−ωk y j/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 composite Lyapunov function

V = 1

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

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


errors given by

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

]T (32)

where e j/k � [x j/kBk(d/dt)x j/k y j/k

Bk(d/dt)y j/k].

The matrix P is chosen by solving the Lyapunov equationP A + AT P = −Q, where Q ∈ R

4×4 is the identity matrix.For this system

P = I2×2 ⊗[

K2+12K1

− 12

− 12

ε2

](33)

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

V = −K pω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 ∈ R

4N2

B = I2×2 ⊗[−1 0

0 ε

](35)

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

Bk a1/1Bk a1/2

Bk a1/2 . . .

Bk a1/NBk a1/N . . .Bk aN/N

Bk aN/N

]T. (36)

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

z =[ω

e

], z = zT

[K p(IN×N ) 0

0 I4N2×4N2

]

︸︷︷︸�D

z (37)

which yields

V = −zT Dz − 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

Bk aTj/k


)(39)

where r j/k is the position error and Bk v j/k is the velocityerror. In the context of the problem, we assume that the relativeposition is measured. Therefore, in the steady state, r j/k isproportional to the measurement noise, which we ignore. Thissimplification allows the function of gains to be pulled outsideof the double summation and used to scale this term in theLyapunov derivative. Under this simplification, V ≤ 0 when

zT Dz > ε

N∑

k=1

N∑

j=1

∥∥∥Bk aTj/kBk v j/k

∥∥∥. (40)

Hence, solutions that lie outside the bound zT Dz =ε∑N

k=1∑N

j=1 ||Bk aTj/kBk v j/k|| will approach this boundary.

Once on the boundary or inside, the solutions will remain therebecause 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 bound

on z. (Were we to enforce physical limits on turning rate andseparation distance between vehicles, we could use Theorem 4to establish that z is uniformly bounded.) Simulated results ofthe 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 boundedby a term proportional to ε. Implementation of the circularcontrol law is achieved using νk = γk(Rk, θ k). Note thatthe relative orientation is used to calculate the centers ofrotation (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 −ω01T ) eT ]T is bounded by a quantity proportional to ε givenin (23).

Proof: Consider the composite Lyapunov function

V = eT (IN2×N2 ⊗ P

)e + K p Kω2

0

2trace

(cT Pc

)(41)

+1

2

N∑

k=1

(ωk − ω0)2 (42)

where c = [c1, . . . , cN ]T , K p > 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

−K p (ω − ω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 coordi-nates is used to simplify (43) by letting

z =[ω − ω01

e

], z = zT Dz (44)

where D is defined in (37), which yields

V = −zT Dz − 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

Bk a j/k


)(46)

where r j/k is the negligible position error and Bk v j/k

is the velocity error. Under the assumption of noise-freemeasurements, V ≤ 0 when

zT Dz > ε

N∑

k=1

N∑

j=1

∥∥∥Bk aTj/kBk v j/k

∥∥∥. (47)

Hence, solutions that lie outside the bound zT Dz =ε∑N

k=1∑N

j=1 ||Bk aTj/kBk v j/k|| will approach this boundary.

Once on the boundary or inside, solutions will remain therebecause 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.


Fig. 5. Simulation of five self-propelled vehicles performing the observer-based parallel control law (28) with a proportional turning rate controller; K = −1,K p = 1, K1 = 10, and K2 = 100. (a) Trajectories. (b) Position estimation errors. (c) Velocity estimation errors.

Fig. 6. Simulation of five self-propelled vehicles performing the observer-based circular control law (28) with a proportional turning rate controller andω0 = 0.25; K = 1, K p = 1, K1 = 10, and K2 = 100. (a) Trajectories. (b) Position estimation errors. (c) Velocity estimation errors.

Note that the error in the estimates approaches zero, whichimplies that each vehicle determines the relative position andrelative velocity of the other vehicles in steady state.

IV. SIMULATED SUBMARINE DYNAMICS AND 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 devel-oped a higher fidelity model of a miniature submarine tovalidate the algorithm. The submarine model has six degreesof freedom and obeys Euler’s equations of motion. Fig. 7displays a free-body diagram of the six forces acting on thevehicle: buoyancy Fb, gravity Fg , drag Fd , thrust Ft , rudderFr , and dive planes Fe. A proportional-integral controller isimplemented to regulate depth by deflecting the dive planes.(The integral term is required to find the elevator deflectionthat counteracts the buoyancy force.)

For the collective control law implementation, knowledge ofthe relative horizontal velocity orientation θk −θ j between thevehicles is required. Note that the extension to a 3-D rigidbody requires that we further define the relative velocityorientation as the difference in orientation of each vehicle’svelocity projected onto the horizontal plane. We approximatethis quantity by assuming that each vehicle’s planar velocityheading is aligned with its yaw angle ψk . Although thisassumption is not true in general, due to the vehicle’s sideslip

Fig. 7. Free-body diagram of the submarine model. (a) Front view. (b) Sideview.

velocity, the sideslip velocity is zero when traveling in aparallel formation [22]. The parallel control law is

uk = K p(φ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 [22]. The circular control law (4)becomes

νk = ω0

(sk + KPkc [cos (ψk −βk) sin (ψk − βk)]T

)(49)


−20 −10 0 10 20 30 40

0

5

10

15

20

25

30

35

40

45

50

x(m)

y(m

)

(a)

0 10 20 30 40 50 600

0.5

1

1.5

2

2.5

t(s)

ψk(r

ad)

(b)

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(r

ad/s)

(c)

Fig. 8. Simulation of five model submarines performing the parallel control law (48); K = −1 and K p = 1. (a) Trajectories. (b) Yaw angle. (c) Yaw rate.

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

−6

−4

−2

0

2

4

6

x(m)

y(m

)

(a)

0 50 100 150

5

10

15

20

25

30

35

40

t(s)

ψk(r

ad)

(b)

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(r

ad�s)

sω0

(c)

Fig. 9. Simulation of five model submarines performing the circular control law (49) with ω0 = 0.25; K = 1 and K p = 1. (a) Trajectories. (b) Yaw angle.(c) Yaw rate.

Fig. 10. Simulation of five model submarines performing the parallel control law (48); K = 9.99||pθ ||−10, K p = 1, K1 = 10, and K2 = 100. (a) Trajectories.(b) Position estimation errors. (c) Velocity estimation errors.

where c = [c1, . . . , cN ]T , sk is submarine ks speed, and ck =rk + ω−1

0 [− sin(ψk − βk) cos(ψk − βk)]TI . 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 vehicles’ 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.

Figs. 10 and 11 display the parallel and circular forma-tions using the estimated quantities. Note that, in the subma-rine model, the estimates do not converge to zero for the


Fig. 11. Simulation of five model submarines performing the circular control law (49) with ω0 = 0.25; K = 0.1, K p = 1, K1 = 10, and K2 = 100.(a) Trajectories. (b) Position estimation errors. (c) Velocity estimation errors.

(a) (b) (c)

Fig. 12. Underwater vehicle testbed consisting of six submarines that operate in the Neutral Buoyancy Research Facility at the University of Maryland and12 underwater cameras used for motion tracking. (a) Miniature submarines. (b) Underwater testing facility. (c) Underwater cameras.

circular case. This observation is attributed to the high ruddercontrol effort when approaching the desired formation. Insteadof settling 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. 11(a).

V. EXPERIMENTAL RESULTS WITH

LABORATORY TESTBED

The control laws and estimator described in Sections IIIand IV are designed using an idealized modeling framework.This technique allows high-level control laws to be evaluatedfor stability and convergence without the need for a specificsystem model to be utilized. In order to demonstrate theusefulness of the control laws, we have implemented themon an underwater-vehicle testbed (see Fig. 12). The testbedis operated in the University of Maryland’s Neutral BuoyancyResearch Facility, which is 25 ft deep and 50 ft across, andoutfitted with 12 Qualisys underwater motion-capture cameras.Each submarine is a radio-controlled 1:60 scale model ofthe U.S.S. Albacore and uses an onboard microprocessor andsensors to steer. (Additional details about the testbed areavailable in [23].)

The multivehicle control system functions using inner andouter control loops. Fig. 13 shows a block diagram of thecontrol architecture. The inner loop, which runs at 50 Hz on

Fig. 13. Multivehicle control architecture for the underwater vehicle testbed.

the microprocessor onboard the submarine, is used to stabilizethe desired turning rate using state feedback. The micro-processor serves two separate functions. First, the submarine’syaw rate is stabilized to a desired rate via state-feedbackcontrol provided by a gyroscope fixed to the vehicle. Themicroprocessor also serves as an analyzer for the desired yawrate which is passed from the transmitter to the receiver.The outer loop runs at 20 Hz from the motion-capturecomputer. The loop receives each submarine’s position andorientation from the cameras and computes the desired turningrate and dive plane deflections. The data is transmitted wire-lessly to the submarine via a radio-frequency transmitter.

Experimental validation of the control laws was firstperformed using a single submarine and a virtual vehicle. Fora parallel formation, the virtual vehicle provides a reference


Fig. 14. Experimental run of a submarine performing the parallel (48) and circular (49) multivehicle control laws with a virtual vehicle. (a) Parallel formation.(b) Circular formation.

Fig. 15. Experimental run of two submarines performing the parallel control law. (a) Coordinated trajectories. (b) Error in relative horizontal orientation.

Fig. 16. Experimental run of two submarines performing the circular control law. (a) Coordinated trajectories. (b) Error in centroid positions.

horizontal velocity orientation [16]. Fig. 14 shows a testrun in which the virtual vehicle travels along the positivex-axis and the submarine changes course to align with thisheading. For test runs of the circular formation, the virtualvehicle travels around the center of the tank with a radiusof 2.5 m. The submarine consistently circles the center ofthe tank at the desired radius. At the end of the test run,the desired center of rotation and the submarine’s center of

rotation are approximately 0.1 m apart. These experimentalresults demonstrate that the control laws and the onboardturning-rate controller are performing as expected.

The next set of experimental runs used two submarines toperform 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, the


Fig. 17. Experimental test run of two submarines performing the parallel control law (48) using the observer-based feedback control algorithm (28).(a) Coordinated trajectories. (b) Error in relative horizontal orientation.

Fig. 18. Experimental test run of two submarines performing the circular control law (49) using the observer-based feedback control algorithm (28).(a) Coordinated trajectories. (b) Error in centroid positions.

submarine’s yaw orientation started with approximately 60°error, which by the end of the run was less than 3°.

The circular control was also tested with two submarinesand illustrated in Fig. 16. In these tests, the submarinesattempt to converge to an arbitrary circular formation witha 3-m radius. Though not perfect, the results are promisingbecause the vehicles are rotating in the correct directionand the circle-center difference is not increasing. Fig. 17shows the results of implementing the observer-based parallelcontrol algorithm. The addition of the observer appears todegrade the performance compared to the two submarinecase, though the final error is less than 20°. Fig. 18 shows theresults from implementing the observer-based method withthe circular control law. The performance is similar to thetwo-submarine test of the circular control law. We observeeach vehicle turning in the correct direction, but the error inthe center agreement varies in time. Possible error sourcesinclude measurement noise, model approximation error, andtracking accuracy. We also use time differencing to extractthe inertial velocity from the motion capture’s position data,which may introduce additional error in the definition of eachsubmarine’s path frame.

Although the algorithm may contribute to the observedperformance, the testbed itself may also introduce additional

error sources. For example, the submarines must operate ina band of water that is deep enough for the motion-capturesystem to see but not so deep that radio transmission iscompromised. Under the loss of radio transmission, the subma-rine will enter a safe mode which stops the propeller untila connection is re-established. Similarly, when tracking islost, the submarine will enter a hold mode where the lastknown settings are utilized. Both these modes deviate fromthe desired behavior, but are necessary to ensure the safety ofthe submarine and the experimental facility.

VI. CONCLUSION

This paper described an observer-based decentralizedfeedback control algorithm to stabilize parallel and circularformations using an all-to-all communication topology.The extension to a limited communication topology ismore than likely possible based on previous work with afirst-order particle model [16]. The proposed algorithm wastheoretically justified for a second-order vehicle model, andsimulations illustrated convergence to the desired formation.In addition, the cooperative control algorithms were extendedto a 3-D rigid-body submarine model with appropriaterotational dynamics. Simulations using this model reinforced


the theoretical results obtained by the idealized version.A laboratory-scale testbed of underwater vehicles was alsodescribed along with corresponding experimental results.Test runs using a virtual vehicle validated the parallel andcircular formation controllers. Results from tests of thecooperative control algorithms with multiple vehicles werealso presented. The parallel formation was achieved with andwithout the observer. For circular formations, further researchis necessary to understand how the combination of vehiclesensing, dynamics, and control impacts performance. Forexample, we are examining the stability of the formation inthe presence of sensor noise. A stochastic formulation of thisproblem would provide insight into the performance of thesubmarine testbed. Additionally, while beyond the scope ofthis paper, the impact of external disturbances such as watercurrents is the topic of ongoing research [10], [11].

ACKNOWLEDGMENT

The authors would like to specifically recognize N. Sydneyand L. DeVries for their contributions to the testing andmaintenance of the submarine fleet. They would also liketo acknowledge the support of many other members ofthe Collective Dynamics and Control Laboratory and SpaceSystems Laboratory, University of Maryland, College Park.

REFERENCES

[1] P.-H. Lin, “The first successful typhoon eyewall-penetration recon-naissance flight mission conducted by the unmanned aerial vehicle,aerosonde,” Bulletin Amer. Meteorol. Soc., vol. 87, no. 11, pp.1481–1483, 2006.

[2] L. Techy, D. A. Paley, and C. A. Woolsey, “UAV coordination on convexcurves in wind: An environmental sampling application,” in Proc. Eur.Control Conf., Budapest, Hungary, 2009, pp. 4967–4972.

[3] L. Techy, C. A. Woolsey, and D. G. Schmale, “Path planning forefficient UAV coordination in aerobiological sampling missions,” inProc. 47th IEEE Conf. Decision Control, Cancun, Mexico, Dec. 2008,pp. 2814–2819.

[4] N. E. Leonard, D. A. Paley, F. Lekien, R. Sepulchre, D. M. Fratantoni,and R. E. Davis, “Collective motion, sensor networks, and oceansampling,” Proc. IEEE, vol. 95, no. 1, pp. 48–74, Jan. 2007.

[5] D. O. Popa, A. C. Sanderson, R. J. Komerska, S. S. Mupparapu,D. R. Blidberg, and S. G. Chappel, “Adaptive sampling algorithms formultiple autonomous underwater vehicles,” in Proc. Auton. UnderwaterVeh., Jun. 2004, pp. 108–118.

[6] A. J. Healey, “Application of formation control for multi-vehicle roboticminesweeping,” in Proc. 40th IEEE Conf. Decision Control, vol. 2.Orlando, FL, Dec. 2001, pp. 1497–1502.

[7] H. Chao and Y.-Q. Chen, “Cooperative sensing and distributed controlof a diffusion process using centroidal voronoi tessellations,” Numer.,Math. Theor. Methods Appl., vol. 3, no. 2, pp. 162–177, May 2010.

[8] C. H. Hsieh, D. Marthaler, B. Q. Nguyen, D. J. Tung, A. L. Bertozzi,and R. M. Murray, “Experimental validation of an algorithm forcooperative boundary tracking,” in Proc. Amer. Control Conf., vol. 2.2005, pp. 1078–1083.

[9] R. Sepulchre, D. A. Paley, and N. Leonard, “Stabilization of planarcollective motion: All-to-all communication,” IEEE Trans. Autom.Control, vol. 52, no. 5, pp. 811–824, May 2007.

[10] D. A. Paley and C. Peterson, “Stabilization of collective motion in atime-invariant flowfield,” AIAA J. Guid., Control, Dynamics, vol. 32,no. 3, pp. 771–779, 2009.

[11] C. Peterson and D. A. Paley, “Multi-vehicle coordination in anestimated time-varying flowfield,” AIAA J. Guid., Control, Dynamics,vol. 34, no. 1, pp. 177–191, 2011.

[12] R. Mellish, S. Napora, and D. A. Paley, “Backstepping control designfor motion coordination of self-propelled vehicles in a flowfield,” Int.J. Robust Nonlinear Control, vol. 21, no. 12, pp. 1452–1466, 2011.

[13] W. Ding, G. Yan, and Z. Lin, “Collective motions and formations underpursuit strategies on directed acyclic graphs,” Automatica, vol. 46,no. 1, pp. 174–181, 2010.

[14] A. Buscarino, L. Fortuna, M. Frasca, and A. Rizzo, “Distributed controlof collective motion of robots through mainly local interactions,” inProc. Eur. Conf. Complex Syst., Oxford, U.K., Sep. 2006, pp. 1–6.

[15] H. Su, X. Wang, and Z. Lin, “Flocking of multi-agents with a virtualleader,” IEEE Trans. Autom. Control, vol. 54, no. 2, pp. 293–307, Feb.2009.

[16] R. Sepulchre, D. A. Paley, and N. E. Leonard, “Stabilization of planarcollective motion with limited communication,” IEEE Trans. Autom.Control, vol. 53, no. 3, pp. 706–719, Apr. 2008.

[17] Y. Cao, W. Ren, and Z. Meng, “Decentralized finite-time sliding modeestimators and their applications in decentralized finite-time formationtracking,” Syst. Control Lett., vol. 59, no. 9, pp. 522–529, 2010.

[18] M. Rubenstein and R. Nagpal, “Kilobot: A robotic module fordemonstrating collective behaviors,” in Proc. IEEE Conf. Robot.Autom., Anchorage, AK, May 2010, pp. 47–51.

[19] D. J. Klein, P. K. Bettale, B. I. Triplett, and K. A.Morgansen, “Autonomous underwater multivehicle control with limitedcommunication: Theory and experiment,” in Proc. 2nd IFAC WorkshopNavigat., Guide. Control Underwater Veh., Apr. 2008, pp. 1–6.

[20] N. J. Kasdin and D. A. Paley, Engineering Dynamics: A ComprehensiveIntroduction, 1st ed. Princeton, NJ: Princeton Univ. Press, 2011.

[21] H. Khalil, Nonlinear Systems, 3rd ed. Upper Saddle River, NJ:Prentice-Hall, 2002.

[22] R. Mellish and D. A. Paley, “Motion coordination of planar rigidbodies,” in Proc. IEEE Conf. Decision Control, Dec. 2011, pp.4897–4902.

[23] N. Sydney, S. Napora, and D. A. Paley, “A multi-vehicle testbed forunderwater motion coordination,” in Proc. Workshop Perform. Eval.Intell. Syst., Baltimore, MD, 2010, pp. 1–5.

Seth Napora received the B.S. degree in aerospaceengineering and the M.S. degree in aerospace engi-neering with a specialization in flight dynamics andcontrol from the University of Maryland, CollegePark, in 2009 and 2011, respectively.

He is a Guidance, Navigation, and Control Engi-neer with ATK Space Systems Inc., Beltsville, MD.

Derek A. Paley (SM’02) received the B.S.degree in applied physics from Yale University,New Haven, CT, in 1997, and the Ph.D. degreein mechanical and aerospace engineering fromPrinceton University, Princeton, NJ, in 2007.

He is an Associate Professor with theDepartment of Aerospace Engineering andthe Institute for Systems Research, University ofMaryland, College Park. He is the co-authorof Engineering Dynamics: A ComprehensiveIntroduction. His current research interests include

dynamics and control, including cooperative control of autonomous vehicles,adaptive sampling with mobile networks, and spatial modeling of biologicalgroups.

Dr. Paley was the recipient of the National Science Foundation CAREERAward in 2010. He is the Founding Director of the Collective Dynamics andControl Laboratory and a member of the Vertical Lift Rotorcraft Center ofExcellence, the Maryland Robotics Center, and the Program in Neuroscienceand Cognitive Science.

observer-based feedback control for stabilization of collective motion

Documents