Cooperative Control and Mobile Sensor Networks

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

Cooperative Control and Mobile Sensor Networks

Cooperative Control, Part I, D-F

Naomi Ehrich Leonard

Mechanical and Aerospace EngineeringPrinceton University

and Electrical Systems and Automation University of Pisa

[email protected], 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 a sampled environment. In Proc. 41st IEEE CDC, 2002.

B. Artificial Potentials and Virtual Beacons:

N.E. Leonard and E. Fiorelli. Virtual leaders, artificial potentials and coordinated control 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 sensor networks: Adaptive gradient climbing in a distributed environment. IEEE Transactions on Automatic Control, 49:8, 2004.

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

Outline and Key ReferencesD. Virtual Tensegrity Structures:

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

E. Networks of Mechanical Systems and Rigid Bodies:

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

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

S. Nair and N.E. Leonard. Stabilization of a coordinated network of rotating rigid bodies. 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 sensor platforms. In Proc. IEEE Swarm Intelligence Symposium, 2005.

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

D. Virtual Tensegrity Structureswith Ben Nabet

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

Real cables do not increase in length and real struts do not decrease in length.

(see papers by R. Connelly)

Linear Model

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

Potential

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

This fixes the shape of the equilibria but not the size.

Equilibria

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

Nonlinear Model

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

Potential

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

Equilibria

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 stable tensegrity structures.

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

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

CableStrut

Initial shape

Finalshape

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

Multi-Scale Shape Change

QuickTime™ and aCinepak decompressor

are needed to see this picture.

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

Ml

u- Construct energy function.- Consider dissipation and asymptotic stability

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

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

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

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

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

Underwater Vehicles

QuickTime™ and aYUV420 codec decompressor

are needed to see this picture.

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.

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

Expl

orin

g Sc

alar

Fie

lds

Generating a contour plot with three clusters:

QuickTime™ and aCinepak decompressor

are needed to see this picture.

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

Curvature Control and Level Set Tracking

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

Filte

r Des

ign

2r

3r

4r

cr1r

Four moving sensor platforms, each takes one measurement a time:

Taylor Series:

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

Filte

r Des

ign

Filtering problem:

From a series of measurements

find and at the center.

Step k-1: Step k:

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

Filte

r Des

ign

Step k-1: Step k:

Prediction:

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

Update:

Filte

r Des

ign

Find that minimizes

We get:

error covariance of predictionerror covariance of measurements

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

Estimate:

Filte

r Des

ign

2r

3r

4r

cr

cD

cxD

cyDa

b

1r

y

A special arrangement to simplify the estimators

x

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

Estimate:

Filte

r Des

ign

How to estimate the Hessian

We have a prediction

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

Estimate:

Filte

r Des

ign

2r

3r

4r

cr

Er

Jr

cDED

KD

1r

FrKr

JD

Assuming formation is small enough

FD

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

Filte

r Des

ign

We now know the Hessian:

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

Find and that minimize the mean square error L.

Form

atio

n D

esig

n

Estimation 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

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 the tangent vector to the level curve at the center.

3. Control the motion of the center to go along the desired level curve.

*b*a

We get a contour plot with gradient estimates along the 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 2r4r

3r

1q2q

3q

Jacobi Vectors:

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

*b1r 1x1y

Formation Control:

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

Trac

king

Lev

el C

urve

s

Reduced center dynamics:

)(sr

1x1y

Boundary tracking is a special case.

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

Convergence proved using LaSalle’s Invariance Principle.

Trac

king

Lev

el C

urve

s

Control Lyapunov Function:

Steering Control:

which achieves

[FZ, Leonard CDC06, SIS05]