Navigation Functions for Patterned Formations

1. Navigation Functions for Patterned Formations Daniel E. Koditschek Electrical & Systems Engineering Department School of Engineering and Applied Science, University of Pennsylvania www.swarms.org

2. Navigation Functions • Definition: NF(Q) •  : Q! [0,1] • -1[0] = destination • -1[1] = boundary • no other minima • (nondegenerate) [Kod & Rimon, AAM ’90] [Rimon & Kod, TAMS’91] Theorem: for every smooth compact oriented manifold with boundary there exists an NF at each point • Exploit Invariance under Diffeomorphism for “Simple” Topology Theorem: ifh:M¼Qis a diffeomorphism and2NF(M) then ±h2NF(Q) We can fix these ! • Original Limitations • Fully Actuated • Completely Sensed • Presumption of known topological model Perhaps realistic ? SWARMS

3. Visual “Bead Patterns” [ Cowan, et al., IEEE TRA’02] • 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)] • The Visible Set: SWARMS

4. Encoding Bead Patterns: NF(I) [ Kod, Robotica ‘94] • is convex • Moreover each of the q := M(M-1)/2 connected components of B := { b2RM | bibj8ij } is also convex • Proposition: …hints toward a “syntax” for NF? b1-axis d2 d1 b2-axis Lemma 3 SWARMS

5. Gradient Vector Field Pullback [ Cowan, et al., IEEE TRA’02] • The camera map is a diffeomorphism onto its image,c : V¼I • Hence, if 2NF(I) then ±c2NF(V) yields a visual servo • for fully actuated kinematic rigid bodies • Safe initial conditions: q02c-1(I) =: V ) • Assure safe, convergent results: q(t) 2V & q(t) !c-1(d) • for fully actuated dynamical rigid bodies • (q,v)2 TSE(2); q0 2c-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?  y x • Unicycle System • Heisenberg System (illustrative example) • Scalar Assembly Problem [ Kod, Robotica, 1994. 12(2):137-155] • Brockett’s [Springer-Verlag,’81] canonical example: • completely controllable • not smoothly stabilizable SWARMS

7. Toward a Unified NF “Servo” Theory  y x [Kod&Lopes, IROS04] • 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: • Negative Gradient Field: • Orthogonal Field: • Analysis (idealized case) • Center Manifold of f1 ,W c • Stable Manifold of f1 ,W s • Flow of f2 • destabilizes W c • stabilizes W s • Realistic case: automated “parallel parking” SWARMS [Bloch, Kod&Lopes, in progress]

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 Joint work: Buehler & Full Commercial Prototype (Boston Dynamics Inc ’03) [Saranli et al,Int. J. Rob. Res, 2001. 20(7): 616-631] Design Concept (Buehler ‘98) Refined Mechanism (McGill ’00) Initial Prototype (UM ’99) Bioinspiration (Full ‘98) Well-tuned Controls (UM ’02) SWARMS

10. Tracking Circular Bead Patterns Clock1 Clock2 Clock2 Clock1 Clock4 Clock3 Clock3 Clock4 Clock6 Clock5 Clock5 Clock6 … … Environment 1 Environment n [cf. Jadbabaie, et al. ] [Klavins & Kod (2002) Int. J. Rob. Res. 21(3):257-275] • Ease of Design: Alternating Tripod Clock Example 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. • Empirical Value: Contrast Coordinated vs. FF Control At present, operating point must be tuned for each new environment Successful Traversals at ~2 m/s SWARMS FF Failures Alternating with Coordinated Controller Successes: Extreme Brick Bed [Weingarten et al., RAM’04]

11. Emerging Limitations of NF Tracking • Trackers Arise from sections • Bundle p : NF(Rn - )!Rn (projection onto goal pattern) • Section s : 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 p : NF(D2 – {o1, o2, o3})! (D2)3 - projection onto the obstacles • Result: p : 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 : P(X) !X£X has a continuous section on each Ui • Working Conjecture: p : NF(X) !X(projection onto the goal point) admits a continuous section if and only if TC(X)=1 SWARMS