N.E. Leonard – U. Pisa – April 2007 Slide 1 Cooperative Control and Mobile Sensor Networks Cooperative Control, Part I, A-C Naomi Ehrich Leonard Mechanical and Aerospace Engineering Princeton University and Electrical Systems and Automation University of Pisa

N.E. Leonard – U. Pisa – April 2007 Slide 2 Natural Groups Photo by Norbert Wu Exhibit remarkable behaviors! Animals may aggregate for Predator evasion Foraging Mating Saving energy

N.E. Leonard – U. Pisa – April 2007 Slide 3 Animal Aggregations and Vehicle Groups Animal group behaviors emerge from individual-level behavior. * Simple control laws for individual vehicles yield versatile fleet. Coordinated behavior in natural groups is locally controlled: - Individuals respond to neighbors and local environment only. - Group leadership and global information not needed. * Minimal vehicle sensing and communication requirements. Robustness to changes in group membership. Herds, flocks, schools: sensor integration systems. * Adaptive, mobile sensor networks.

N.E. Leonard – U. Pisa – April 2007 Slide 4 Lagrangian models for fish, birds, mammals: Animal Group Models and Mechanics Locomotion forces (drag, constant speed) Aggregation forces (attraction/repulsion) Arrayal forces (velocity/orientation alignment) Deterministic environmental forces (gravity, fluid motions) Random forces (from behavior or the environment) Artificial potentials, Gyroscopic forces, Symmetry-breaking, Reduction, Energy functions, Stability

N.E. Leonard – U. Pisa – April 2007 Slide 5 Artificial Potentials for Cooperative Control Design potential functions with minimum at desired state. Control forces computed from gradient of potential. Potential provides Lyapunov function to prove stability. Distributed control. Neighborhood of each vehicle defined by sphere of radius d 1 (and h 1 ). Leaderless, no order of vehicles necessary. Provides robustness to failure. Vehicles are interchangeable. Concepts extend from particle models to rigid body body models. (Koditschek, McInnes, Krishnaprasad, …)

N.E. Leonard – U. Pisa – April 2007 Slide 6 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, 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 , 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 – April 2007 Slide 7 Outline and Key References D.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, 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, T.R. Smith, H. Hanssmann and N.E. Leonard. Orientation control of multiple underwater vehicles. In Proc. 40th IEEE CDC, pages , S. Nair and N.E. Leonard. Stabilization of a coordinated network of rotating rigid bodies. In Proc. 43rd IEEE CDC, pages , 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 – April 2007 Slide 8 Coordinating Control with Interacting Potentials Leonard and Fiorelli, CDC 2001

N.E. Leonard – U. Pisa – April 2007 Slide 9 Stability of Formation

N.E. Leonard – U. Pisa – April 2007 Slide 10 A. Artificial Potential Plus Projected Gradient Vehicle group in descending Gaussian valley Feedback from artificial potentials and from measurements of environment: Bachmayer and Leonard, CDC, 2002

N.E. Leonard – U. Pisa – April 2007 Slide 11 Gradient Descent: Single Vehicle with Local Gradient Information

N.E. Leonard – U. Pisa – April 2007 Slide 12 Gradient Descent: Multiple Vehicle with Local Gradient Information

N.E. Leonard – U. Pisa – April 2007 Slide 13 Gradient Descent: Multiple Vehicle with Local Gradient Information

N.E. Leonard – U. Pisa – April 2007 Slide 14 Gradient Descent: Multiple Vehicle with Local Gradient Information

N.E. Leonard – U. Pisa – April 2007 Slide 15 Gradient Descent Example: Two Vehicles, T = ½ ||x|| 2 x y i j r

N.E. Leonard – U. Pisa – April 2007 Slide 16 Gradient Descent Example: Three Vehicles, T = ½ ||x|| 2 i x y k j AA

N.E. Leonard – U. Pisa – April 2007 Slide 17 Gradient Descent Example: Three Vehicles, T = ½ ||x|| 2 i x y k j BB i x y k j AA

N.E. Leonard – U. Pisa – April 2007 Slide 18 Gradient Descent with Projected Gradient Information

N.E. Leonard – U. Pisa – April 2007 Slide 19 Gradient Descent with Projected Gradient Information: Single Vehicle Case

N.E. Leonard – U. Pisa – April 2007 Slide 20 Gradient Descent with Projected Gradient Information: Single Vehicle Case

N.E. Leonard – U. Pisa – April 2007 Slide 21 Gradient Descent with Projected Gradient Information: Single Vehicle Case

N.E. Leonard – U. Pisa – April 2007 Slide 22 Gradient Descent with Projected Gradient Information: Multiple Vehicle Case See also Moreau, Bachmayer and Leonard, 2002

N.E. Leonard – U. Pisa – April 2007 Slide 23 Gradient Descent with Projected Gradient Information: Multiple Vehicle Case

N.E. Leonard – U. Pisa – April 2007 Slide 24 B. Virtual Bodies and Artificial Potentials for Cooperative Control Virtual beacons (virtual leaders) Manipulate group geometry: specialized group geometries, symmetry breaking.

N.E. Leonard – U. Pisa – April 2007 Slide 25 *Extension to underactuated systems is possible. For example, Lawton, Young and Beard, 2002, consider dynamics of an off-axis point on a nonholonomic robot which by feedback linearization can be made to look like double-integrator dynamics. N vehicles with fully actuated dynamics*: M virtual beacons are reference points on a virtual (rigid) body. describes the virtual body c.o.m. Artificial Potentials and Virtual Beacons Assume all virtual beacons (i.e. virtual body) and reference frame move at constant velocity

N.E. Leonard – U. Pisa – April 2007 Slide 26 ik h h0h0 h1h1 VhVh h h0h0 h1h1 x ij d0d0 d1d1 VIVI ij x d0d0 d1d1 fhfh fIfI i j k Control Law for Vehicle i

N.E. Leonard – U. Pisa – April 2007 Slide 27 5 Body Simulation

N.E. Leonard – U. Pisa – April 2007 Slide 28 Schooling Case: N=2, M=1 h0h0 d0d0 h0h0 d0d0 Stable (S 1 symmetry) h0h0 Unstable v0v0 v0v0 v0v0 Equilibrium is minimum of total potential.

N.E. Leonard – U. Pisa – April 2007 Slide 29 Symmetry Breaking Case: N=M=2 h0h0 d0d0 h0h0 h0h0 d0d0 h0h0 v0v0 v0v0

N.E. Leonard – U. Pisa – April 2007 Slide 30 Schooling Case: N=3 h0h0 d0d0 v0v0 v0v0 d0d0 d0d0 d0d0 d0d0

N.E. Leonard – U. Pisa – April 2007 Slide 31 Schooling: Hexagonal Lattice, N > 3 v0v0 h1h1 h0h0 d0d0 d1d1

N.E. Leonard – U. Pisa – April 2007 Slide 32 Schooling: Special geometries, N>3, M>1 v0v0 d0d0 d1d1

N.E. Leonard – U. Pisa – April 2007 Slide 33

N.E. Leonard – U. Pisa – April 2007 Slide 34 Schooling Stability

N.E. Leonard – U. Pisa – April 2007 Slide 35 C. Artificial Potentials and Virtual Body with Feedback Dynamics Introduce feedback dynamics for virtual bodies to introduce mission: direct group motion, split/merge subgroups, avoid obstacles, climb gradients. Configuration space of virtual body is for orientation, position and dilation factor: Because of artificial potentials, vehicles in formation will translate, rotate, expand and contract with virtual body. To ensure stability and convergence, prescribe virtual body dynamics so that its speed is driven by a formation error. Define direction of virtual body dynamics to satisfy mission. Partial decoupling: Formation guaranteed independent of mission. Prove convergence of gradient climbing. (Ogren, Fiorelli, Leonard, MTNS 2002 and IEE E TAC, 2004 )

N.E. Leonard – U. Pisa – April 2007 Slide 36 Stability of Formation

N.E. Leonard – U. Pisa – April 2007 Slide 37 Translation, Rotation, Expansion and Contraction

N.E. Leonard – U. Pisa – April 2007 Slide 38 Stability of x eq (s) at Any Fixed s

N.E. Leonard – U. Pisa – April 2007 Slide 39 Speed of Traversal and Formation Stabilization

N.E. Leonard – U. Pisa – April 2007 Slide 40

N.E. Leonard – U. Pisa – April 2007 Slide 41

N.E. Leonard – U. Pisa – April 2007 Slide 42 Simulation of Path in SO(2)

N.E. Leonard – U. Pisa – April 2007 Slide 43 Mission Trajectories Let translation, rotation, expansion, and contraction evolve with feedback from sensors on vehicles to carry out mission such as gradient climbing. Augmented state space is (x,s,r,R,k). Express the vector fields for the virtual body motion as: To satisfy mission we choose rules for the directions

N.E. Leonard – U. Pisa – April 2007 Slide 44 Adaptive Gradient Climbing

N.E. Leonard – U. Pisa – April 2007 Slide 45 Least Squares Estimate of Gradient of Measured Scalar Field 0

N.E. Leonard – U. Pisa – April 2007 Slide 46 Least Squares Estimate of Gradient of Measured Scalar Field Assumed measurement noise

N.E. Leonard – U. Pisa – April 2007 Slide 47 Least Squares Estimate of Gradient of Measured Scalar Field

N.E. Leonard – U. Pisa – April 2007 Slide 48 Optimal Formation Problem

N.E. Leonard – U. Pisa – April 2007 Slide 49 Optimal Formation Problem

N.E. Leonard – U. Pisa – April 2007 Slide 50 Optimal Formation Problem: Three-Vehicle Case

N.E. Leonard – U. Pisa – April 2007 Slide 51 Optimal Formation Problem: Three-Vehicle Case

N.E. Leonard – U. Pisa – April 2007 Slide 52 Adaptive Gradient Climbing See Ogren, Fiorelli and Leonard (IEEE TAC, 2004) for - To improve the quality of the gradient estimate can use a Kalman filter and thus take into account the time history of measurements. - Results on convergence of formation to local minimum in measured field. To investigate how close the formation gets to true local minimum, investigate the size of the estimation error.

N.E. Leonard – U. Pisa – April 2007 Slide 53 Rolling and Climbing Vehicle Group