Geometry of Steering Laws in
Cooperative Control
Eric W. Justh, P.S. Krishnaprasad, F. Zhang
Institute for Systems Research
& ECE Department
University of Maryland
College Park, MD 20742
Workshop on Lie Group Methods and Control Theory
International Centre for Mathematical Sciences
14, India Street
Edinburgh, Scotland, June 29, 2004
Boston University--Harvard University--University of Illinois--University of Maryland
James Clerk Maxwell (1831-1879)
Boston University--Harvard University--University of Illinois--University of Maryland
Maxwell and Control
Maxwell’s (1857) essay on Saturn’s rings contained the
first use of the characteristic polynomial to assess
stability. This later led to his use of the same method
in designing a speed governor – a physical feedback
control device – in a problem of accurate electrical
measurements.
Maxwell studied the problem of Saturn’s rings under
Newtonian attraction. Coherence of a ring of
artificial earth satellites can be achieved by synthetic
interactions (realized via feedback control laws).
Boston University--Harvard University--University of Illinois--University of Maryland
Maxwell and Control
It is extraordinary that, as we are gathered here to
discuss geometric integration and control, the
spacecraft Cassini-Huygens is taking the closest look
ever at Saturn’s rings (and shepherd moons), a
triumph of action at a distance via precise control.
New data on bending waves, density waves and
spokes in the ring system may lead to further
theoretical work on the problem of Saturn’s rings.
Boston University--Harvard University--University of Illinois--University of Maryland
Maxwell and Gyroscopic Interaction
Maxwell’s equations for the electromagnetic field are
complemented by the equation of Lorentz for the force on a
charged particle in an electromagnetic field.
In a region where the electric field vanishes, the particle
motion is governed by a purely gyroscopic Lagrangian
given in terms of the magnetic vector potential. Gyroscopic
forces leave the kinetic energy invariant.
Boston University--Harvard University--University of Illinois--University of Maryland
Our subject
This talk is about the exploitation of purely
gyroscopic interactions to achieve coherent patterns
in the motion of particles. We do this in the language
most natural to the problem, the language of moving
frames. The next few slides constitute a brief review
of some aspects of moving frames.
Boston University--Harvard University--University of Illinois--University of Maryland
Frenet-Serret Frame
Regular Curve
s
γ ( s)
N
T
C3
B
and
γ ′′ ≠ 0
Frenet-Serret
γ ′(s) = T (s)
equations
T ′(s) =
κ (s)N(s)
N′(s) = −κ (s)T (s)
+τ (s)B(s)
B′(s) =
−τ (s)N(s)
Boston University--Harvard University--University of Illinois--University of Maryland
Natural Frenet or
Relatively Parallel Adapted Frame
M2
Regular Curve
s
T
M1
s
γ ( s)
C2
T
M1
Equations for relatively parallel adapted
frame
γ ′( s )
M2
= T (s)
T ′( s ) =
k1 ( s ) M 1 ( s ) + k2 ( s ) M 2 ( s )
M 1′( s ) = − k1 ( s )T ( s )
M 2′ ( s ) = − k2 ( s )T ( s )
s=0
R. L. Bishop, Amer. Math. Month. (1975), 82(3):246-251
Boston University--Harvard University--University of Illinois--University of Maryland
Control System on SE(3)
⎛ g γ ⎞′
⎜ 0 1⎟
⎝
⎠
where
and
and
[TNB ]
g
=
ξ
⎡ 0 −κ
= ⎢κ 0
⎢
⎢⎣ 0 τ
e1
⎛1⎞
= ⎜0⎟
⎜ ⎟
⎜0⎟
⎝ ⎠
⎛ g γ ⎞⎛ ξ e1 ⎞
= ⎜
⎟⎜ 0 0 ⎟
0
1
⎝
⎠⎝
⎠
[T M 1M 2 ]
or
0⎤
−τ ⎥
⎥
0 ⎥⎦
or
⎡0
⎢k
⎢ 1
⎢⎣ k2
− k1
0
0
− k2 ⎤
0 ⎥
⎥
0 ⎥⎦
Boston University--Harvard University--University of Illinois--University of Maryland
Inversion
κ (s) =
τ ( s)
=
γ ′′( s )
(FS)
γ ′( s ) ⋅ ( γ ′′( s ) × γ ′′′( s ) )
(κ ( s ) )
2
s
ki ( s ) = γ ′′( s ) ⋅ M i (0) − ∫ ki (σ )γ ′′( s ) ⋅ γ ′(σ ) dσ
0
i = 1, 2
κ (s) =
(RPAF)
k12 ( s ) + k22 ( s )
θ ′( s ) = τ
(where
θ = arg ( k )
Boston University--Harvard University--University of Illinois--University of Maryland
Normal (k-plane) Development of
γ
x
Frame interaction
Boston University--Harvard University--University of Illinois--University of Maryland
Isokinetic Motion
F
= q (Ε + υ × Β)
υ = x
Ε = 0
L =
B =
1
2
υ ,υ + q υ , Α #
×Α
Boston University--Harvard University--University of Illinois--University of Maryland
Outline
Motivation: Flying in spatial patterns (formations)
Model: Interaction laws for unit-speed particles
• Formations as shape equilibria
• Convergence to specific formations
- Analysis for two particles (vehicles)
- Simulations for n vehicles
• Interaction with surfaces - obstacle-avoidance and boundaryfollowing
Boston University--Harvard University--University of Illinois--University of Maryland
Acknowledgements
Collaborators:
Jeff Heyer, Larry Schuette, David Tremper
Naval Research Laboratory
Funding:
• NRL: “Motion Planning and Control of Small Agile Formations”
• AFOSR: “Dynamics and Control of Agile Formations”
• ARO: “Communicating Networked Control Systems”
Boston University--Harvard University--University of Illinois--University of Maryland
Red Arrows
Boston University--Harvard University--University of Illinois--University of Maryland
Planar Model (Natural Frenet Frames)
Unit speed
assumption
x2
x1
xn
y1
y2
yn
r2
rn
r1
r1 = x 1
x 1 = y 1u1
y 1 = − x 1u1
r2 = x 2
x 2 = y 2u 2
y 2 = − x 2u 2
•
•
•
rn = x n
x n = y nun
y n = − x nun
u1, u2,..., un are curvature
(i.e., steering) control inputs.
Specifying u1, u2,..., un as feedback functions of (r1, x1, y1),
(r2, x2, y2),..., (rn, xn, yn) defines a control law.
Boston University--Harvard University--University of Illinois--University of Maryland
3-D Model (Natural Frenet Frames)
Unit speed
assumption
z2
y2
x2
z1
r2
xn
yn
y1
x1 = y 1u1 + z 1v1
y 1 = − x1u1
z 1 = − x1v1
x1
r1
zn
r1 = x1
rn
r2 = x 2
x 2 = y 2 u 2 + z 2 v2
y 2 = − x 2u2
z 2 = − x 2 v2
rn = x n
x n = y n u n + z n vn
The natural curvatures (u1,v1),
(u2,v2),...,(un,vn) are controls.
y n = − x nun
z n = − x n vn
Relatively parallel adapted frames in the sense of
R. L. Bishop, Amer. Math. Month. (1975), 82(3):246-251
Boston University--Harvard University--University of Illinois--University of Maryland
Characterization of Equilibrium Shapes
Proposition (Justh, Krishnaprasad): Suppose the controls u1, u2, …, un are invariant
under rigid motions in the plane. For equilibrium shapes (i.e. relative equilibria of the
unreduced dynamics), 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).
g5
(b)
g
3
(a)
g4
g2
g1
g1
g5
g2
g3
g4
Boston University--Harvard University--University of Illinois--University of Maryland
3-D Equilibrium Shapes
• The control laws are assumed to be invariant under rigid motions in threedimensional space.
• Shape variables capture relative distances and angles between vehicles.
• Shape equilibria correspond to steady-state formations.
g1
g4
g2
g5
g4
g1
g2
g3
rectilinear formation
g2
g4
g5
circling formation
g3
g5
g1
g3
helical formation
Boston University--Harvard University--University of Illinois--University of Maryland
Pair Equilibria
Rectilinear formation
(motion perpendicular
to the baseline)
Collinear formation
Circling formation
(separation equals the
diameter of the orbit)
Boston University--Harvard University--University of Illinois--University of Maryland
Planar Control Law for 2 particles
r1 = x 1
x 1 = y 1u1
y 1 = − x 1u1
r2 = x 2
x 2 = y 2u 2
y 2 = − x 2u 2
r = r2 − r1
⎛ r
⎞⎛ r
⎞
⎛ r
⎞
⎜
⎟
⎜
⎟
⎜
=
−
−
⋅
−
⋅
−
−
⋅
x
y
r
u1
η⎜
y1 ⎟⎟ + µx 2 ⋅ y1
1 ⎟⎜
1 ⎟ f (| |)⎜
⎝ |r|
⎠⎝ | r |
⎠
⎝ |r|
⎠
⎛ r
⎞⎛ r
⎞
⎛ r
⎞
⎜
⎟
⎜
⎟
⎜
=
−
⋅
⋅
−
⋅
x
y
r
u2
η⎜
y 2 ⎟⎟ + µx1 ⋅ y 2
2 ⎟⎜
2 ⎟ f (| |)⎜
⎝|r |
⎠⎝ | r |
⎠
⎝|r |
⎠
x2
x1
y1
y2
µ>
η
2
>0
f(|r|)
r2
r1
0
|r|
Boston University--Harvard University--University of Illinois--University of Maryland
Change of variables
Dot products can be expressed as sines and cosines in the
new variables:
r
r
⋅ x1 = sin φ1
⋅ y1 = cos φ1
|r|
|r|
r
r
⋅ x 2 = sin φ 2
⋅ y 2 = cos φ 2
|r|
|r|
φ2
x 2 ⋅ y1 = sin(φ 2 − φ1 )
ρ = |r|
φ1
x1 ⋅ y 2 = sin(φ1 − φ 2 )
System after change of variables:
ρ = sin φ 2 − sin φ1
φ1 = −η sin φ1 cos φ1 + f ( ρ ) cos φ1 + µ sin(φ 2 − φ1 )
+ (1 / ρ )(cos φ 2 − cos φ1 )
φ 2 = −η sin φ 2 cos φ 2 − f ( ρ ) cos φ 2 + µ sin(φ1 − φ 2 )
+ (1 / ρ )(cos φ 2 − cos φ1 )
Boston University--Harvard University--University of Illinois--University of Maryland
Lyapunov Function
• A Lyapunov function is
V pair = − ln(cos(φ 2 − φ1 ) + 1) + h( ρ )
φ2
where f (ρ) = dh/dρ.
• The derivative of Vpair with respect to time
along trajectories of the system is
ρ = |r|
φ1
V pair =
∂V pair
∂φ1
φ1 +
∂V pair
∂ϕ 2
φ2 +
∂V pair
∂ρ
ρ
≤ 0.
• This Lyapunov function is the key to proving
a convergence result for the two-particle
system.
Boston University--Harvard University--University of Illinois--University of Maryland
Convergence result
• Our Lyapunov function must be “radially unbounded,” meaning Vpair → ∞ as
ρ → 0 and as ρ → ∞. (Some minor technical assumptions are also needed.)
Proposition (Justh, Krishnaprasad): For any initial condition satisfying
|φ2 - φ1| ≠ π and ρ > 0, the system converges to the set of equilibria, which has
the form
{(ρ , π2 , π2 ), ∀ρ > 0 } ∪ {(ρ ,− π2 ,− π2 ), ∀ρ > 0 } ∪ {(ρe ,0,0) f ( ρe ) = 0 }
Proof: Uses LaSalle’s Invariance Principle.
• Examples of (relative) equilibria:
ρe
ρe
Boston University--Harvard University--University of Illinois--University of Maryland
Intuition
Steering control equation for vehicle #2:
⎛ r
⎞
⎞
⎛ r
⎞⎛ r
u2 = −η ⎜⎜ ⋅ x 2 ⎟⎟⎜⎜ ⋅ y 2 ⎟⎟ − f (| r |)⎜⎜ ⋅ y 2 ⎟⎟ + µx1 ⋅ y 2
⎝|r |
⎠
⎠
⎝|r |
⎠⎝ | r |
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.
• 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.
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.
Boston University--Harvard University--University of Illinois--University of Maryland
Key ideas for two-vehicle problem
• Unit-speed motion with steering control.
- Gyroscopic forces preserve kinetic energy of each particle.
- In mechanics, gyroscopic forces are associated with vector potentials.
• Shape variables: relative distances and angles.
• Lyapunov function ⇒ convergence result for the shape dynamics.
• Equilibria of the shape dynamics = relative equilibria of the vehicle dynamics.
• Particle re-labeling symmetry.
• Lie group formulation:
- The dynamics of each particle can be expressed as a left-invariant system
evolving on SE(2), the group of rigid motions in the plane.
- G=SE(2) is a symmetry group for the dynamics: the control law is invariant under
rigid motions of the entire formation.
-Vpair is also invariant under G.
Therefore, we can consider the reduced system
evolving on shape space = (G×G)/G = G.
Boston University--Harvard University--University of Illinois--University of Maryland
Gyroscopic Forces and Vector Potentials
Kinetic energy
Scalar potential
Consider the Lagrangian:
L=
1
2
x ⋅ M x + y ( x ) ⋅ x − x ⋅ Kx ,
Vector potential
(linear-in-velocity
term)
x ∈ R n , M = M T > 0, K = K T .
Euler-Lagrange equations:
T
∂
d ∂L ∂L
d
y
(M x + y ( x ) ) − ⎛⎜ ⎞⎟ x + Kx
−
=
dt ∂ x ∂ x dt
⎝ ∂x ⎠
T
⎛ ∂y ⎞
⎛ ∂y ⎞
= Mx + ⎜
⎟x − ⎜
⎟ x + Kx
⎝ ∂x ⎠
⎝ ∂x ⎠
= M x + Q ( x ) x + Kx
= 0,
T
⎛ ∂y ⎞ ⎛ ∂y ⎞
T
(∗) Q ( x ) = ⎜
⎟−⎜
⎟ ⇒ Q ( x ) = − Q ( x ).
⎝ ∂x ⎠ ⎝ ∂x ⎠
Note: Lagrangian with linear-in-velocity term ⇒ skew term in the dynamics,
but the converse only holds if Q(x) can be expressed as in (∗) for some y(x).
Boston University--Harvard University--University of Illinois--University of Maryland
Gyroscopically interacting particles
For a single particle:
r = position, v = r , a = r ,
m = mass = 1,
2
H = kinetic energy = 12 v ,
⎡ 0 −u ⎤
ma = F ⇔ r = ⎢
⎥ r.
⎢⎣ u
0 ⎥⎦
Note: F is a gyroscopic force
(Recall Lorentz force law for a
charged particle in a magnetic field)
H = 0,
⎛ 2 H cos θ ⎞
⎟,
r=⎜
⎟
⎜
H
2
sin
θ
⎠
⎝
θ = u.
Restrict to the
level-set of H
given by H=1/2.
r=x
Note that u may be a complicated
x = y u Frenet-Serret
function of time, and may involve
y = − x u equations
feedback.
For multiple particles, the kinetic energy of each particle is conserved, and the
particles interact via gyroscopic forces.
Boston University--Harvard University--University of Illinois--University of Maryland
Shape space for n particles
Frenet-Serret
Equations
rj = x j
x j = y ju j
y j = −x ju j
j = 1, 2 , ..., n .
Shape variables
capture relative
positions and
orientations.
Group variables
Dynamics
⎛ ⎡0 0 1 ⎤ ⎡0 − 1 0 ⎤ ⎞
⎜⎢
⎡x j y j r j ⎤
⎥ ⎢
⎥ ⎟
⎢
⎥
g j = g j ⎜ ⎢0 0 0 ⎥ + ⎢1 0 0 ⎥ u j ⎟
⎜⎢
⎥
gj = ⎢
⎥ ⎢
⎥ ⎟
⎜ ⎢0 0 0 ⎥ ⎢0 0 0 ⎥ ⎟
⎢
⎥
⎦ ⎣
⎦ ⎠
⎝⎣
⎢⎣ 0
0
1 ⎥⎦
= g j ξ j , ξ j ∈ se ( 2 ), j = 1, 2 , ..., n .
j = 1, 2 , ..., n .
Configuration space
g1, g2, ..., gn ∈ G = SE(2),
n copies
the group of rigid motions
S = G×G× ×G
in the plane.
Shape variables
g~j = g1−1g j , j = 2,...,n.
Assume the controls u1, u2, ..., un are
functions of shape variables only.
Shape space
n −1 copies
R = G×G ×
×G
Boston University--Harvard University--University of Illinois--University of Maryland
Two-particle law: Lie group setting
• Dynamics on configuration space S=G×G, where G=SE(2):
⎡ x1
g1 = ⎢
⎢
⎢⎣ 0
⎡x2
g2 = ⎢
⎢
⎢⎣ 0
y1
0
y2
0
r1 ⎤
⎡ cos θ1
⎥ = ⎢ sin θ
1
⎥ ⎢
1⎥
⎦ ⎢⎣ 0
r2 ⎤
− sin θ1
⎥ , g 1 = g 1ξ 1 = g 1 ( A0 + A1u1 ).
⎥
0
1⎥
⎦
ξ 1 , ξ 2 ∈ se ( 2 )
− sin θ 2 r2 ⎤
⎥ , g 2 = g 2ξ 2 = g 2 ( A0 + A1u 2 ).
cos θ 2
⎥
0
1⎥
⎦
⎡0
⎡0 0 1⎤
cos θ1
⎡ cos θ 2
⎥ = ⎢ sin θ
2
⎥ ⎢
1⎥
⎦ ⎢⎣ 0
• Shape variable: g =
r1 ⎤
g 1− 1 g 2
• Dynamics on shape space R=G: g = g ξ ,
A0 = ⎢ 0
⎢
⎢⎣ 0
0
0
0⎥,
⎥
0⎥
⎦
A1 = ⎢ 1
⎢
⎢⎣ 0
−1
0
0
0⎤
0 ⎥.
⎥
0⎥
⎦
ξ = ξ 2 − g −1ξ 1 g = ξ 2 − Ad g −1ξ 1 ∈ se ( 2 ).
• Controls as functions of the shape variable g:
( )
g g
g
u1 ( g ) = −η ⎛⎜ 13 2 23 ⎞⎟ + f ( r ) 23 + µ g 21 , g = [ g ij ], r =
r
⎝ r
⎠
⎛ g 13 g 23 ⎞
⎛ g 23 ⎞
21
−1
ij
u 2 ( g ) = −η ⎜
+
f
(
r
)
+
µ
g
,
g
=
[
g
].
⎟
⎜
⎟
2
r
r
⎝
⎠
⎝
⎠
2
2
g 13
+ g 23
,
V pair = V pair ( g ).
Boston University--Harvard University--University of Illinois--University of Maryland
Lyapunov Function
V = − ln (1 + x 2 ⋅ x1 ) + h (| r2 − r1 |)
Penalize heading-direction
misalignment
h( ρ )
Penalize inter-vehicle distances
which are too large or small
• V depends only on shape variables.
f (ρ)
ro
ρ
• Idea: show that for suitable choice of
control law, dV/dt - 0.
• For two vehicles, global convergence
results are obtained (Justh and
Krishnaprasad, TR 2002, CDC 2003,
SCL 2004, CDC 2004)
Boston University--Harvard University--University of Illinois--University of Maryland
Generalization to 3-D Law
Natural curvatures
for vehicle #1:
Natural curvatures
for vehicle #2:
⎛ r
⎞⎛ r
⎞
⎛ r
⎞
x
u1 = −η ⎜
⋅ 1 ⎟⎜
⋅ y1 ⎟ + f (| r |) ⎜
⋅ y1 ⎟ + µ x 2 ⋅ y1
⎝ | r | ⎠⎝ | r | ⎠
⎝|r| ⎠
⎛ r
⎞⎛ r
⎞
⎛ r
⎞
v1 = −η ⎜
⋅ x1 ⎟⎜
⋅ z1 ⎟ + f (| r |) ⎜
⋅ z1 ⎟ + µ x 2 ⋅ z1
⎝ | r | ⎠⎝ | r | ⎠
⎝|r| ⎠
⎛ r
⎞
⎞
⎛ r
⎞⎛ r
⎜
⎜
⎟
⎜
⎟
x
y
r
u2 = −η ⎜ ⋅ 2 ⎟⎜ ⋅ 2 ⎟ − f (| |)⎜ ⋅ y 2 ⎟⎟ + µx1 ⋅ y 2
⎝|r |
⎠
⎠
⎝|r |
⎠⎝ | r |
⎛ r
⎞⎛ r
⎞
⎛ r
⎞
v2 = −η ⎜
⋅ x 2 ⎟⎜
⋅ z 2 ⎟ − f (| r |) ⎜
⋅ z 2 ⎟ + µ x1 ⋅ z 2
⎝|r|
⎠⎝ | r |
⎠
⎝|r|
⎠
Baseline
alignment
Collision
avoidance
Heading
alignment
Boston University--Harvard University--University of Illinois--University of Maryland
Lie Group Setting
Frenet-Serret
Equations
rj = x j
x j = y ju j
y j = −x ju j
Group variables
⎡x j y j
⎢
gj = ⎢
⎢
⎢⎣ 0
0
j = 1, 2 , ..., n .
rj⎤
⎥
⎥
⎥
1 ⎥⎦
Dynamics
⎛ ⎡0 0 1 ⎤ ⎡0 − 1 0 ⎤ ⎞
⎜⎢
⎥ ⎢
⎥ ⎟
g j = g j ⎜ ⎢0 0 0 ⎥ + ⎢1 0 0 ⎥ u j ⎟
⎜⎢
⎥ ⎢
⎥ ⎟
⎜ ⎢0 0 0 ⎥ ⎢0 0 0 ⎥ ⎟
⎦ ⎣
⎦ ⎠
⎝⎣
= g j ξ j , ξ j ∈ se ( 2 ), j = 1, 2 , ..., n .
j = 1, 2 , ..., n . g , g , ..., g ∈ G = SE(2),
1
2
n
the group of rigid motions
in the plane.
Shape variables
capture relative
vehicle positions
and orientations.
Configuration space
n copies
S = G×G×
×G
Assume the controls u1, u2, ..., un are
functions of shape variables only.
Shape variables
g~ j = g 1 − 1 g j ,
j = 2 ,..., n .
Shape space
n −1 copies
R = G×G ×
×G
Boston University--Harvard University--University of Illinois--University of Maryland
Lie Group Setting (3-D Trajectories)
• Represent each vehicle trajectory as a function on the Lie group SE(3) of rigid motions:
⎡
⎢x
g1 = ⎢ 1
⎢
⎢
⎣0
y1
z1
0
0
⎤
r1 ⎥⎥
,
⎥
⎥
1⎦
⎡
⎢x
g2 = ⎢ 2
⎢
⎢
⎣0
y2
z2
0
0
⎤
r2 ⎥⎥
∈ S E (3)
⎥
⎥
1⎦
• Define the shape variable:
⎡ x1 ⋅ x 2
⎢y ⋅ x
−1
g = g1 g 2 = ⎢ 1 2
⎢ z1 ⋅ x 2
⎢
⎣ 0
x1 ⋅ y 2
x1 ⋅ z 2
y1 ⋅ y 2
z1 ⋅ y 2
y1 ⋅ z 2
z1 ⋅ z 2
0
0
( r2 − r1 ) ⋅ x 1 ⎤
( r2 − r1 ) ⋅ y 1 ⎥⎥
( r2 − r1 ) ⋅ z 1 ⎥
⎥
1
⎦
• Left-invariant systems on SE(3):
g 1 = g 1ξ 1 = g 1 ( A0 + A1u 1 + A2 v1 ), ξ 1 ∈ se (3)
g 2 = g 2 ξ 2 = g 2 ( A0 + A1u 2 + A2 v 2 ), ξ 2 ∈ se (3)
g = g ξ = g (ξ 2 − A d g -1 ξ 1 ) = g (ξ 2 − g -1ξ 1 g ), ξ ∈ se (3)
⎡0
⎢0
A0 = ⎢
⎢0
⎢
⎣0
0
0
0
0
0
0
0
0
1⎤
⎡0
⎢1
0 ⎥⎥
, A1 = ⎢
⎢0
0⎥
⎥
⎢
0⎦
⎣0
−1
0
0
0
0
0
0
0
0⎤
⎡0
⎢0
0 ⎥⎥
, A2 = ⎢
⎢1
0⎥
⎥
⎢
0⎦
⎣0
0
−1
0
0
0
0
0
0
0⎤
0 ⎥⎥
0⎥
⎥
0⎦
Boston University--Harvard University--University of Illinois--University of Maryland
Formation Control for n vehicles
Generalization of the two-vehicle formation control law to n vehicles:
rj = x j
x j = y ju j
y j = −x ju j
⎡
1
u j = ∑ ⎢ F (r j − rk , x j , y j , x k , y k ) − f (| r j − rk
n k≠ j⎢
⎣
⎛ r j − rk
⎞⎤
|)⎜
⋅ y j ⎟⎥
⎜ | r j − rk |
⎟⎥
⎝
⎠⎦
j = 1, 2, …, n
At present, it is conjectured (based on simulation results) that such control
laws stabilize certain formations. However, analytical work is ongoing.
Boston University--Harvard University--University of Illinois--University of Maryland
Rectilinear Control Law Simulation
Boston University--Harvard University--University of Illinois--University of Maryland
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. Time
3
On-the-fly modification of
the separation parameter.
ro
1
time
Boston University--Harvard University--University of Illinois--University of Maryland
Circling Control Law Simulation
Boston University--Harvard University--University of Illinois--University of Maryland
Circling Control Law
⎞
1 ⎡ ⎛ r j − rk
u j = ∑ ⎢ ±η ⎜
⋅ x j ⎟ − f (| r j − rk
⎜
⎟
n k ≠ j ⎢⎣ ⎝ | r j − rk |
⎠
“Beacon-circling” behavior: the vehicles respond
to a beacon, as well as to each other.
Simulations with 10 vehicles (for
different random initial conditions).
On-the-fly
modification of the
separation parameter.
⎛ r j − rk
⎞⎤
|) ⎜
⋅y
⎜ | r − r | j ⎟⎟ ⎥⎥
⎝ j k
⎠⎦
Normalized Separation Parameter vs. Time
3
ro
1
time
Boston University--Harvard University--University of Illinois--University of Maryland
3-D Simulations
Rectilinear Law:
Circling Law:
uj =
⎞ ⎛ r jk
⎞
1 ⎡ ⎛ r jk
−
⋅
⋅
− f (| r jk
η
y
x
⎢
∑ ⎜
j ⎟
⎟ ⎜⎜ | r | j ⎟⎟
n k ≠ j ⎢⎣ ⎜⎝ | r jk |
⎠ ⎝ jk
⎠
⎤
⎛ r
⎞
|) ⎜ jk ⋅ y j ⎟ + µ x k ⋅ y j ⎥
⎜|r |
⎟
⎥⎦
⎝ jk
⎠
⎧⎪ ⎛ r jk
⎞ ⎛ r jk
⎞
⎛ r jk
⎞
1
⋅x j ⎟⎜
⋅ y j ⎟ − f (| r jk |) ⎜
⋅y j ⎟
u j = ∑ ⎨ −η ⎜
⎟⎜ | r |
⎟
⎜|r |
⎟
n k ≠ j ⎪⎩ ⎜⎝ | r jk |
⎠ ⎝ jk
⎠
⎝ jk
⎠
⎡
⎛ r jk
⎞ ⎛ r jk
⎞ ⎤ ⎪⎫
+ µ ⎢−xk ⋅ y j + 2 ⎜
⋅ xk ⎟ ⎜
⋅ y j ⎟⎥ ⎬
⎜|r |
⎟⎜ | r |
⎟⎥
⎢⎣
⎝ jk
⎠ ⎝ jk
⎠ ⎦ ⎪⎭
where r jk = r j − rk , µ >
⎡ ⎛ r
f (| r jk |) = α ⎢1 − ⎜ o
⎢ ⎜⎝ | r jk
⎣
⎞
⎟
| ⎟⎠
2
η
2
> 0,
⎤
⎥, α > 0
⎥
⎦
Boston University--Harvard University--University of Illinois--University of Maryland
Obstacle Avoidance
• Idea: control inputs for the moving vehicle are determined by the trajectory of
the closest point on the obstacle surface.
• Goals: boundary following and non-collision.
z2
y2
y2
x2
x2
z1 x
1
x1
y1
r2
y1
r2
r1
r1
F. Zhang, E.W. Justh, and P.S. Krishnaprasad, CDC 2004.
Boston University--Harvard University--University of Illinois--University of Maryland
Boundary-Following Simulation
Boston University--Harvard University--University of Illinois--University of Maryland
Performance Criteria
• Faithful following of waypointspecified trajectories
• Sufficient separation between
vehicles (to avoid collisions)
Intervehicle distances
Waypoints
time
• Minimize steering:
for UAVs, turning
requires considerably
more energy than
straight, level flight.
Maneuverability is
also limited.
umax
Steering controls
Steering “Energy”
time
0
time
-umax
Boston University--Harvard University--University of Illinois--University of Maryland
Toward Practical Missions
Boston University--Harvard University--University of Illinois--University of Maryland
Convergence Result for n > 2
• We consider rectilinear relative equilibria, and the Lyapunov
function
n
[ (
)
V = ∑ ∑ − ln 1 + cos(θ j − θ k ) + h(| r j − rk |)
]
j =1 k < j
• Convergence Result (Justh, Krishnaprasad): There exists a sublevel
set Ω of V and a control law (depending only on shape variables) such
that V ≤ 0 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 Ω.
Boston University--Harvard University--University of Illinois--University of Maryland
Continuum Model
• Vector field (in polar coordinates):
⎛ dr / dt ⎞ ⎛ cosθ ⎞
⎜
⎟ ⎜
⎟
⎜
⎟ = ⎜ sin θ ⎟.
⎜ dθ / dt ⎟ ⎜ u ⎟
⎝
⎠ ⎝
⎠
• This continuum formulation
only involves two scalar fields:
the density ρ(t,r,θ) and the
steering control u(t,r,θ).
• Continuity equation (Liouville equation):
⎡ ∂ (uρ ) ⎛ cosθ ⎞
⎤
∂ρ
⎜
⎟
= −⎢
+
⋅ ∇ r ρ ⎥.
⎜
⎟
∂t
⎢⎣ ∂θ
⎥⎦
⎝ sin θ ⎠
• Conservation of matter:
∫ ρ (t , r,θ )drdθ = 1,
G
• Incorporating time and/or
spatial derivatives in the
equation for u yields a coupled
system of PDEs for ρ and u.
∀t.
• Energy functional:
Vc (t ) =
[ (
• However, the underlying space
is 3-dimensional (for planar
formations).
)
]
1
~
~
~
~
~
~
−
ln
1
+
cos(
θ
−
θ
)
+
h
(|
r
−
r
|)
ρ
(
t
,
r
,
θ
)
ρ
(
t
,
r
,
θ
)
d
r
d
θ
d
r
d
θ
.
∫
∫
G
G
2
Boston University--Harvard University--University of Illinois--University of Maryland
References
1. E.W. Justh and P.S. Krishnaprasad, “A simple control law for UAV
formation flying,” Institute for Systems Research Technical Report TR 200238, 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, pp. 36093614, 2003.
3. E.W. Justh and P.S. Krishnaprasad, “Equilibria and steering laws for planar
formations,” Systems and Control Letters, Vol. 52, pp. 25-38, 2004.
4. E.W. Justh and P.S. Krishnaprasad, “Formation control in three
dimensions,” preprint, 2004.
5. F. Zhang, E.W. Justh, and P.S. Krishnaprasad, “Boundary following using
gyroscopic control,” Submitted to IEEE Conf. Decision and Control, 2004.
See also http://www.isr.umd.edu/~justh
Boston University--Harvard University--University of Illinois--University of Maryland