steering laws and continuum models for planar formations

Boston University--Harvard University--University of Illinois--University of Maryland

Steering Laws and Continuum Models for Planar Formations

Eric W. Justh, P.S. Krishnaprasad

Institute for Systems Research& ECE Department

University of MarylandCollege Park, MD 20742

42nd IEEE Conference on Decision and Control, Dec 9-12, 2003


Acknowledgements

Collaborators: Jeffrey Heyer, Larry Schuette, David Tremper atNaval Research Laboratory, Washington D.C., and Fumin Zhang

at the University of Maryland, College Park.

• NRL: “Motion Planning and Control of Small Agile Formations”

• AFOSR: “Dynamics and Control of Agile Formations”

• ARO: “Communicating Networked Control Systems”


References1. E.W. Justh and P.S. Krishnaprasad, “A simple control law for UAV formation flying,” Institute for Systems Research Technical Report TR 2002-38, 2002 (see http://www.isr.umd.edu).

2. E.W. Justh and P.S. Krishnaprasad, “Steering laws and continuum models for planar formations,” Proc. 42nd IEEE Conf. Decision and Control, in press, 2003.

3. E.W. Justh and P.S. Krishnaprasad, “Equilibria and steering laws for planar formations,” Systems and Control Letters, in press, 2003.

4. E.W. Justh and P.S. Krishnaprasad, “Steering laws and convergence for planar formations,” Proc. Block Island Workshop on Cooperative Control, to appear, 2003.


Outline

• Motivation: UAV formation control

• Planar model based on unit-speed motion with steering control

- Equilibrium formations

- Two-vehicle laws and Lyapunov functions

- Connection to gyroscopic systems

• Implementation considerations

• Further developments


UAV Modeling• Features of UAV model:

- High speed sluggish maneuvering.- Turning significant energy penalty. - Autopilot takes into account detailed vehicle kinematics.

• Vehicles modeled as point particles moving at unit speed and subject to steering control.

• A formation control law is a feedback law which specifies these steering controls.

• Modeling may be appropriate in other settings with high speeds and penalties associated with turning (e.g., loss of dynamic stability).

Dragon Runner

(Photo from U.S. Marine Corps website)

Dragon Eye(Photo credit: Jonathan Finer, The Washington Post)


Planar Model (Frenet-Serret Equations)

x1

y1

r1

111

111

11

uuxy

yxxr

u1, u2,..., un are curvature (i.e., steering) control inputs.

x2

y2

r2222

222

22

uu

xyyxxr

xn

yn

rn

nnnnnn

nn

uu

xyyxxr

•••

Unit speed assumption

Specifying u1, u2,..., un as feedback functions of (r1, x1, y1), (r2, x2, y2),..., (rn, xn, yn) defines a control law.


Characterization of Equilibrium ShapesProposition (Justh, Krishnaprasad): For equilibrium shapes (i.e., relative equilibria of the dynamics on configuration space), u1 = u2 = ... = un, and there are only two possibilities:

(a) u1 = u2 = ... = un = 0: all vehicles head in the same direction (with arbitrary relative positions), or

(b) u1 = u2 = ... = un 0: all vehicles move on the same circular orbit (with arbitrary chordal distances between them).

g1g5

g4

g2

g3

g1

g3g5

g2

g4

(a)(b)


Equilibrium Formations of Two Vehicles

Rectilinear formation (motion perpendicular to the baseline)

Collinear formation Circling formation (vehicle separation equals the diameter of the orbit)


Planar Formation Laws for Two Vehicles

111

111

11

uuxy

yxxr

222

222

22

uu

xyyxxr

122111 ||

|)(|),,,,( yrrryxyxr fFu

12 rrr

211222 ||

|)(|),,,,( yrrryxyxr fFu

x1y1

r1

x2

y2r2

kj

j kjk

F

yrrx

rrr

yxyxr

|||||)(|

),,,,(2

1

2

1

1122

2

||1|)(|

rr orf

|r|

f(|r|)

0 ro


Shape Variables for Two Vehicles

= |r| 1

21111 cos

||sin

|| y

rrx

rr

2222 cos||

sin||

yrrx

rr

12 sinsin

)cos)(cos/1(cos)()cos,sin,cos,sin,(

12122111

f

F

)cos)(cos/1(cos)()cos,sin,cos,sin,(

12211222

fF

System after change of variables:

Dot products can be expressed as sines and cosines in the new variables:

)sin()sin(

2121

1212

yxyx


• Rectilinear formation (perpendicular to baseline) or collinear formation:

Lyapunov Functions for Two Vehicles

)())cos(1ln( 12 hVpair

)())cos(1ln( 12 hVcirc

0,)()()()( 22112112 0,0)()()()( 22211211

or

)(h

)(f

• Circling formation or collinear formation:

0,)()()()( 22112112 0,0)()()()( 22211211

• Impose further conditions on the jk to stabilize specific formations while destabilizing others.


Biological Analogy

Align each vehicle perpendicular to the baseline between the vehicles.

Steer toward or away from the other vehicle to maintain appropriate separation.

Align with the other vehicle’s heading.

D. Grünbaum, “Schooling as a strategy for taxis in a noisy environment,” in Animal Groups in Three Dimensions, J.K. Parrish and W.M. Hamner, eds., Cambridge University Press, 1997.

• Biological analogy (swarming, schooling): - Decreasing responsiveness at large separation distances. - Switch from attraction to repulsion based on separation distance or density. - Mechanism for alignment of headings.

Steering control: 212222 |||)(|

||||yxy

rrry

rrx

rr

fu

22211211 ,,,0Choice of coefficients:


Gyroscopically Interacting Particles• Note: Vpair and Vcir are not to be thought of as synthetic potentials (commonly used in robotics for directing motion toward a target or away from obstacles).

• Vpair and Vcir are Lyapunov functions for the shape dynamics.

• The kinetic energy of each particle is conserved (because they interact via gyroscopic forces), and initial conditions are such that they all move at unit speed.

• There is an analogy with the Lorentz force law for charged particles in a magnetic field.

• In mechanics, gyroscopic forces are associated with vector potentials.

• References: - L.-S. Wang and P.S. Krishnaprasad, J. Nonlin. Sci., 1992. - J.E. Marsden and T.S. Ratiu, Introduction to Mechanics and Symmetry, Springer, 1999, (2nd edition)


Lie Group Setting

....,,2,1 nj

u

u

jjj

jjj

jj

xy

yx

xr

....,,2,1

100

nj

g

jjj

j

ryx

....,,2,1),2(000

001

010

000

000

100

, njseg

ugg

jjj

jjj

DynamicsGroup variablesFrenet-Serret Equations

g1, g2, ..., gn G = SE(2), the group of rigid motions in the plane.

copies n

GGGS

Configuration space

.,...,2,~ 11 njggg jj

Shape variables Shape space

copies 1

n

GGGR

Assume the controls u1, u2, ..., un are functions of shape variables only.Shape variables

capture relative vehicle positions and orientations.


Formation Control for n vehiclesGeneralization of the two-vehicle formation control law to n vehicles:

At present, it is conjectured (based on simulation results) that such control laws stabilize certain formations. However, analytical work is ongoing.

jkj

kj

kjkjkkjjkjj fF

nu y

rrrr

rryxyxrr||

|)(|),,,,(1

jjj

jjj

jj

u

u

xy

yx

xr

nj ,,2,1


Rectilinear Control Law Simulations


Rectilinear Control Law Simulations

Simulations with 10 vehicles (for different random initial conditions).

Leader-following behavior: the red vehicle follows a prescribed path (dashed line).

Normalized Separation Parameter vs. Time3

1time

roOn-the-fly modification of the separation parameter.


Circling Control Law Simulations


Circling Control Law

On-the-fly modification of the separation parameter.

Normalized Separation Parameter vs. Time3

1time

ro

Simulations with 10 vehicles (for different random initial conditions).

“Beacon-circling” behavior: the vehicles respond to a beacon, as well as to each other.

jkj

kj

kjkjj

kj

kjj f

nu y

rrrr

rrxrrrr

|||)(|

||1


Convergence Result for n > 2

• Convergence Result (Justh, Krishnaprasad): There exists a sublevel set of V and a control law (depending only on shape variables) such that on .

• With this Lyapunov function, we cannot prove global convergence for n > 2.

• Although we obtain an explicit formula for the controls uj, j=1,...,n, there is no guarantee that this particular choice of controls will result in convergence to a particular desired equilibrium shape in .

0V

n

j jkkjkj hV

1|)(|)cos(1ln rr

• We consider rectilinear relative equilibria, and the Lyapunov function


Performance Criteria

Waypoints

time

Steering controlsumax

-umax

0

Steering “Energy”

time

time

Intervehicle distances

• Faithful following of waypoint-specified trajectories

• Sufficient separation between vehicles (to avoid collisions)

• Minimize steering: for UAVs, turning requires considerably more energy than straight, level flight. Maneuverability is also limited.


• Basic parameters:

- rsep = separation while circling.

- n = number of vehicles.

• Derived parameters:

- Rectilinear law:

- Circling law:

• For the circling law, we precisely control the equilibrium formation.

• For the rectilinear law, we only approximately achieve the desired equilibrium vehicle separations.

• The steady-state vehicle separation for the rectilinear law is chosen to be half that of the circling law, although other choices are possible.

Choice of Parameters

1,22 nro

sepr

nrsepno orr 2

2 cos12,

1

.5

02 50 100

n

n


Motion Description Language Approach• Each vehicle simulates the evolution of the entire formation in real time; i.e., the vehicles all run the same motion plan.

- Disturbances (e.g., wind for UAVs) lead to estimation errors.

- GPS and communication used to reinitialize the estimators.

• The motion plan can be changed on the fly.

- Interrupts, due to the environment or human intervention, can change the motion plan (e.g., dynamical system parameters).

- The communication protocol must ensure that all vehicles update their motion plans simultaneously.

• This approach is consistent with motion description language formalism: V. Manikonda, P.S. Krishnaprasad, and J. Hendler, “Languages, Behaviors, Hybrid Architectures, and Motion Control,” in Mathematical Control Theory, J. Baillieul and J.C. Willems, eds., Springer, pp. 199-226, 1999.


Time Discretization• Control laws specify u1(t), u2(t), ..., un(t) at each time instant t.

• Instead, compute u1(tm), u2(tm), ..., un(tm), where tm=mT for m=1, 2, ..., and let

• Maximum value of T is determined by the control law.

T = ½ seems to be a reasonable choice (for , , 1).

• Piecewise constant controls allow the vehicle positions to be computed using simple formulas:

).,[),()( 1 mmmjj ttttutu

)(

)(cos1

)(sin)()(

)(1)( 1 mj

mj

mjmjmj

mjmj t

Ttu

Ttutt

tut ryxr

TtuTtu

TtuTtutttt

mjmj

mjmjmjmjmjmj

)(cos)(sin

)(sin)(cos)()()()( 11 yxyx


Limited Steering Authority

minimum radius of curvature

steering rate

• umax = maximum (absolute) value the steering control is permitted to take.

• umax is determined either by the minimum radius of curvature or by the steering rate.

....,,2,1

if , if , if ,

nj

uuuuuuu

uuuu

maxidealjmax

maxidealjmax

idealj

maxidealjmax

j


• Why steering rate matters:

• The transition time should be a small fraction of the interval T.

• If the transition times are not trivial, they can be taken into account by using Simpson’s Rule in the numerical integration.

Finite Steering Rate Effects

umax

-umax

uj

t

transition governed by steering rate limitation

T 2T 3T 4T 5T


Sensor-Based Implementationtransmit antenna

receive antennas

• One pair of antennas gives a sinusoidal function of angle of arrival.

0 -

• Range is inversely related to received power./4s1(t) s2(t)

• Antenna separation and transmission frequency are related to UAV dimensions.

• Two pairs of antennas, used for both transmitting and receiving, can provide all the terms in the control law.

• GPS is not required.


3-Dimensional Frenet-Serret Equations

xzy

r

r - position vectorx - tangent y - normal z - binormal wv

wuvu

yxzzxy

zyxxr

u, v, w are control inputs (two of which uniquely specify the trajectory)

Frenet-Serret: v = 0 u = curvature w = torsion

Note: the Frenet-Serret frame applies to the trajectory, and is not a body-fixed frame for the UAV

unit speed assumption


Continuum Model

• Continuity equation (Liouville equation):

.sin

cos

ru

t

• Vector field (in polar coordinates):

.sincos

/

/

udtd

dtd

r

• Conservation of matter:

.,1,, tddtG

rr

• Energy functional:

.~~)~,~,(),,(|)~(|)~cos(1ln21)( ddddtthtV

G Gc rrrrrr

• This continuum formulation only involves two scalar fields: the density (t,r,) and the steering control u(t,r,).

• However, the underlying space is 3-dimensional (for planar formations).

• Incorporating time and/or spatial derivatives in the equation for u yields a coupled system of PDEs for and u.


References1. E.W. Justh and P.S. Krishnaprasad, “A simple control law for UAV formation flying,” Institute for Systems Research Technical Report TR 2002-38, 2002 (see http://www.isr.umd.edu).

2. E.W. Justh and P.S. Krishnaprasad, “Steering laws and continuum models for planar formations,” Proc. 42nd IEEE Conf. Decision and Control, in press, 2003.

3. E.W. Justh and P.S. Krishnaprasad, “Equilibria and steering laws for planar formations,” Systems and Control Letters, in press, 2003.

4. E.W. Justh and P.S. Krishnaprasad, “Steering laws and convergence for planar formations,” Proc. Block Island Workshop on Cooperative Control, to appear, 2003.

steering laws and continuum models for planar formations

Documents