1

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 1

Cooperative Controland

Mobile Sensor Networks

Cooperative Control, Part I, D-F

Naomi Ehrich Leonard

Mechanical and Aerospace EngineeringPrinceton University

and Electrical Systems and AutomationUniversity of Pisa

naomi@princeton.edu,www.princeton.edu/~naomi

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 2

Outline and Key References

A. Artificial Potentials and Projected Gradients:

R. Bachmayer and N.E. Leonard. Vehicle networks for gradient descent in asampled environment. In Proc. 41st IEEE CDC, 2002.

B. Artificial Potentials and Virtual Beacons:

N.E. Leonard and E. Fiorelli. Virtual leaders, artificial potentials and coordinatedcontrol of groups. In Proc. 40th IEEE CDC, pages 2968-2973, 2001.

C. Artificial Potentials and Virtual Bodies with Feedback Dynamics:

P. Ogren, E. Fiorelli and N.E. Leonard. Cooperative control of mobile sensornetworks: Adaptive gradient climbing in a distributed environment. IEEETransactions on Automatic Control, 49:8, 2004.

2

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 3

Outline and Key References

D. Virtual Tensegrity Structures:

B. Nabet and N.E. Leonard. Shape control of a multi-agent system using tensegritystructures. In Proc. 3rd IFAC Wkshp on Lagrangian and Hamiltonian Methods forNonlinear Control, 2006.

E. Networks of Mechanical Systems and Rigid Bodies:

S. Nair, N.E. Leonard and L. Moreau. Coordinated control of networked mechanicalsystems with unstable dynamics. In Proc. 42nd IEEE CDC, 2003.

T.R. Smith, H. Hanssmann and N.E. Leonard. Orientation control of multipleunderwater vehicles. In Proc. 40th IEEE CDC, pages 4598-4603, 2001.

S. Nair and N.E. Leonard. Stabilization of a coordinated network of rotating rigidbodies. In Proc. 43rd IEEE CDC, pages 4690-4695, 2004.

F. Curvature Control and Level Set Tracking:

F. Zhang and N.E. Leonard. Generating contour plots using multiple sensorplatforms. In Proc. IEEE Swarm Intelligence Symposium, 2005.

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 4

D. Virtual Tensegrity Structures

with Ben Nabet

3

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 5

Real cables do notincrease in length andreal struts do notdecrease in length.

(see papers by R. Connelly)

Linear Model

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 6

Potential

4

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 7

This fixes the shape of theequilibria but not the size.

Equilibria

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 8

Nonlinear Model

5

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 9

Potential

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 10

Equilibria

6

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 11

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 12

Shape Change

Choose a path from initial to final configuration that consists of a path of stabletensegrity structures.

Can then prove boundedness of transient and convergence to final structure.

7

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 13

Cable

Strut

Initial shape

Finalshape

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 14

Multi-Scale Shape Change

8

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 15

E. Networks of Mechanical Systems/Rigid Bodies

• Geometric framework: Method of Controlled Lagrangians with A.M. Bloch and J.E. Marsden

- Energy shaping for stabilization of (otherwise unstable) underactuated mechanical systems.

- Restrict to control dynamics that derive from a Lagrangian.

- Theory is constructive for certain classes: Synthesis!

also D.E. Chang and C.A. Woolsey, P.S. Krishnaprasad, G. Sanchez de Alvarez,see also IDA-PBC method – Blankenstein, Ortega, Spong, van der Schaft et al

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 16

Method of Controlled Lagrangians

• Given a mechanical system, possibly underactuated and possibly with unstable dynamics.

• Design Lc so corresponding Euler-Lagrange equations match original equations with control law.

• Matching conditions are PDE’s.

• For certain classes of systems, use structured modification Lc of L

- Q=S x G. L invariant to G. Shape kinetic energy metric. - Modify potential energy to break symmetry (if desired).

• Yields parametrized family of Lc that satisfy matching conditions.

• Theory provides conditions on (control) parameters for stability:

s

φg m

M

l

u

- Construct energy function.- Consider dissipation and asymptotic stability

Bloch, Leonard, Marsden, IEEE TAC, 2000, 2001

9

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 17

Coordination

• Design artificial potentials to couple N individual systems. - Relative position/orientation of vehicle pairs. - Potential well = desired group configuration.

• Treat coupled multi-body system with same approach as for individual. - Symmetry group G for Hamiltonian + potentials. - Reduce action of G on phase space. - Construct energy function to prove:

Individual dynamics are stabilized and group is stably coordinated.

Nair and Leonard; Smith, Hanssmann and Leonard

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 18

Role of Symmetry

• Potentials will break symmetry:

E.g., consider N vehicles and Q = SE(3) x . . . x SE(3)

Suppose Q is original symmetry group.

- Break N-1 copies of SO(3) to align orientations. - Break N-1 copies of SE(3) to align and distribute. - Break N copies of SO(3) to align and to orient whole group, etc.

• Break symmetry for coordination and group cohesion.

• Preserve symmetries when control authority is limited.

• Discrete symmetries in homogeneous group with no ordering.

N times

10

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 19

Same Features as in Particle Systems

• Distributed control.

• Neighborhood of each vehicle can be prescribed. (Global info not required)

• No ordering of vehicles is necessary. Provides robustness to failure.

• Vehicles are interchangeable.

Illustrations:

A. Two (underwater) vehicles in SE(3)

B. N inverted-pendulum-on-cart systems.

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 20

A: Coordinated Orientation of 2 Vehicles in SE(3)

A

B

A

B

with Troy Smith and Heinz Hanssmann

11

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 21

Introduce Artificial Potential

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 22

Reduced System

12

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 23

Underwater Vehicles

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 24

B: Coordination of Mechanical Systems with Unstable Dynamicswith Sujit Nair

Extend controlled Lagrangians to collection of unstablemechanical systems with controlled coupling.

• Class of systems includes inverted pendulum on a cart.

• Goal: Stabilize each pendulum in the upright position while synchronizing the motion of the carts.

13

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 25

Extension to Network of Systems

s

φg m

Mlu

c sin φ

y

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 26

Exp

lori

ng S

cala

r Fi

elds

Generating a contour plot with three clusters:

Fumin Zhang and N.E. Leonard, Proc. IEEE Swarm Symposium, 2005

Curvature Control and Level Set Tracking

14

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 27

Filte

r D

esig

n

2r

3r

4r

cr

1r

Four moving sensor platforms, each takes one measurement atime:

Taylor Series:

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 28

Filte

r D

esig

n

Filtering problem:

From a series of measurements

find and at the center.

Step k-1: Step k:

15

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 29

Filte

r D

esig

n

Step k-1: Step k:

Prediction:

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 30

Update:

Filte

r D

esig

n

Find that minimizes

We get:

error covariance of predictionerror covariance of measurements

16

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 31

Estimate:Fi

lter

Des

ign

2r

3r

4r

cr

cD

cxD

cyD

a

b

1r

y

A special arrangement to simplify the estimators

x

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 32

Estimate:

Filte

r D

esig

n

How to estimate the Hessian

We have a prediction

17

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 33

Estimate:Fi

lter

Des

ign

2r

3r

4r

cr

Er

Jr

cD

ED

KD

1r

Fr

Kr

JD

Assuming formation is small enough

FD

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 34

Filte

r D

esig

n

We now know the Hessian:

18

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 35

Find and that minimize the mean squareerror L.

Form

atio

n D

esig

nEstimation Error:

Error in estimate of field value at center.

Error in estimate of gradient at center.

Optimization:

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 36

Form

atio

n D

esig

n

Optimization:

General solutions are numerical.

We found analytical solutions when B is diagonal.

[Ögren, Fiorelli and Leonard 04],[FZ, Leonard SIS05][FZ, Leonard CDC06].

Covariance matrix of updated measurements Error in estimate of first diag. el. of Hessian

19

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 37

1. Achieve the cross formation with optimal shape and.

2. Align the horizontal axis of the formation with thetangent vector to the level curve at the center.

3. Control the motion of the center to go along the desiredlevel curve.

*b

*a

We get a contour plot with gradient estimates alongthe level curve.

Coo

pera

tive

Con

trol

Goals for cooperative controllers:

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 38

Coo

pera

tive

Con

trol

1r 2

r

4r

3r

1q

2q

3q

Jacobi Vectors:

20

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 39

Coo

pera

tive

Con

trol

Decoupled Dynamics:

where and

FormationCenter

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 40

Coo

pera

tive

Con

trol

2r

3r

4r

*a

*b

1r

1x1

y

Formation Control:

21

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 41

Tra

ckin

g L

evel

Cur

ves

Reduced center dynamics:

)(sr

1x

1y

Boundary tracking is a special case.

N.E. Leonard – U. Pisa – 18-20 April 2007Slide 42

Convergence proved using LaSalle’s Invariance Principle.

Tra

ckin

g L

evel

Cur

ves

Control Lyapunov Function:

Steering Control:

which achieves

[FZ, Leonard CDC06, SIS05]