1

Navigation Functions for Patterned

Formations

Daniel E. KoditschekElectrical & Systems Engineering Department

School of Engineering and Applied Science, University of Pennsylvania

www.swarms.org

SWARMS

2

• Original Limitations• Fully Actuated• Completely Sensed• Presumption of known topological model

Navigation Functions

• Exploit Invariance under Diffeomorphism for “Simple” Topology

Theorem: for every smooth compact oriented manifold with boundary there exists an NF at each point

Theorem: if h: M ¼ Q is a diffeomorphism and 2 NF(M)

then ± h 2 NF(Q)

We can fix these !

Perhaps realistic ?

Definition: NF(Q) : Q ! [0,1] -1[0] = destination -1[1] = boundary no other minima (nondegenerate)

[Kod & Rimon, AAM ’90][Rimon & Kod, TAMS’91]

SWARMS

3

Visual “Bead Patterns”

The Visible Set:

Visual Landmarks: Standard Sensor Model pinhole camera: : A2 ! RP1 : (1, 2) 2 / 1 narrow field of view: (A2) µ [-E, E] µ R landmark: P = [ p1, p2, p3 ] 2 (A2)3

camera frame transformation: H(xc,yc,c) 2 SE(2) camera map: c : SE(2) ! [-E, E] 3 : H [(Hp1),(Hp2),(Hp3)]

[ Cowan, et al., IEEE TRA’02]

-2

-1

0

1

2

-2

-1

0

1

2

-2

-1

0

1

2

-2

-1

0

1

2

SWARMS

4

Encoding Bead Patterns: NF(I) is convex

[ Kod, Robotica ‘94]

Moreover each of the q := M(M-1)/2 connected components of B := { b 2 RM | bi bj 8 i j } is also convex

Proposition:

Lemma 3

b1-axis b2-axis d1

d2

…hints toward a “syntax” for NF?

SWARMS

5

Gradient Vector Field Pullback The camera map is a diffeomorphism onto its image, c : V ¼ I Hence, if 2 NF(I) then ± c 2 NF(V) yields a visual servo

for fully actuated kinematic rigid bodies Safe initial conditions: q0 2 c-1(I) =: V ) Assure safe, convergent results: q(t) 2 V & q(t) ! c-1(d)

[ Cowan, et al., IEEE TRA’02]

for fully actuated dynamical rigid bodies

(q,v) 2 TSE(2); q0 2 c-1(I) & v0TMv0 < 1 )

(q ,v) (t) 2 TV SE(2) & (q ,v) (t) ! c-1(d) £ {0}

[ Kod, JDynMechSys’91]

.. but what about underactuated rigid bodies?

and

SWARMS

6

Navigation for Nonholonomic Systems?

Heisenberg System (illustrative example)

Unicycle System

Scalar Assembly Problem

[ Kod, Robotica, 1994. 12(2):137-155]

x

y

Brockett’s [Springer-Verlag,’81] canonical example: • completely controllable• not smoothly stabilizable

-1 -0.5 0.5 1

0.2

0.4

0.6

0.8

1

SWARMS

7

Toward a Unified NF “Servo” Theory Ingredients

Underactuated System m = # actuators < dof = n nonholonomic constraints

Goal: appropriate sensor predicate Obstacle avoidance

to avoid physical obstacles to maintain gravitational balance to respect sensory limitations

Construction Projector onto column space:

Analysis (idealized case) C enter Manifold of f1 , W c

Stable Manifold of f1 , W s

Flow of f2

destabilizes W c

stabilizes W s

x

y

Orthogonal Field: Negative Gradient Field:

[Kod&Lopes, IROS04]

Realistic case: automated “parallel parking”

[Bloch, Kod&Lopes, in progress]

SWARMS

8

Encoding Disk Patterns: NF(R2 - )Recent sufficient conditions for non-colliding disks

[Karagoz, Bozma & Kod, UM Tech Report ’03]

SWARMS

9

RHex: a “Swarm” of Legs [Saranli et al, Int. J. Rob. Res, 2001. 20(7): 616-631]

Bioinspiration (Full ‘98)

Initial Prototype (UM ’99)

Refined Mechanism (McGill ’00)

Design Concept (Buehler ‘98)

Commercial Prototype (Boston Dynamics Inc ’03)Well-tuned Controls (UM ’02)

Joint work: Buehler & Full

SWARMS

10

Tracking Circular Bead Patterns

Terrain Without

Coord.

With Coord.

Linoleum 10/10 10/10

Bricks (easy) 28/30 15/17

Bricks (medium)

11/30 19/30

Bricks (hard) 6/30 16/30

Bricks (extreme)

1/10 4/10

Successful Traversals at ~2 m/s

[Weingarten et al., RAM’04]FF Failures Alternating with Coordinated Controller Successes: Extreme Brick Bed

• Empirical Value: Contrast Coordinated vs. FF Control

• Ease of Design: Alternating Tripod Clock Example[Klavins & Kod (2002) Int. J. Rob. Res. 21(3):257-275]

Clock1 Clock3 Clock5

Clock2 Clock4 Clock6

The system corresponding to this connection graph meets the specification: it has a single, global attracting behavior.

The same analysis on this system gives multiple stable orbits. The system does not perform the task specified.

Clock1 Clock3 Clock5

Clock2 Clock4 Clock6

Clock

Body

Motor Leg

Motor Leg

Motor Leg

Motor Leg

Motor Leg

Motor Leg

Clock

Feedback Feedforward

Decentralized

Decentralized

(c1, c2)

Clock

Body

Motor Leg

Motor Leg

Motor Leg

Motor Leg

Motor Leg

Motor Leg

Clock

Feedback Feedforward

Decentralized

Decentralized

(c1, c2)

At present, operating point must be tuned for each new environment

Environment 1 Environment n…

…

[cf. Jadbabaie, et al. ]

SWARMS

11

Emerging Limitations of NF Tracking Trackers Arise from sections

Bundle : NF(Rn - ) ! Rn (projection onto goal pattern) Section : Rn ! NF(Rn - ) such that ± = idRn

Controllers for tracking a moving pattern, r:R ! Rn - “Moving NF” (r,b) := ( ± r)(b) “Safe” Tracking Controller:

Topological Obstructions Hirsch & Hirsch [ Mich. Math. J. 1998 ]

Definitions: NF(D2 – {o1, o2, o3}) - the set of navigation functions on the three-point punctured 2-disk) The Bundle : NF(D2 – {o1, o2, o3}) ! (D2)3 - projection onto the obstacles

Result: : NF(D2 – {o1, o2, o3}) ! (D2)3 has no continuous section Farber

Definition [ Disc. Comp. Geom. 2003]: Topological Complexity, TC(X), of a topological space, X Definition: Pathspace, P(X), the set of continous paths between pairs of points in X The minimal cardinality, k, of an open cover {U1, …, Uk} of X £ X such that : P(X) ! X £ X has a continuous section on

each Ui Working Conjecture:

: NF(X) ! X (projection onto the goal point) admits a continuous section if and only if TC(X)=1