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 naomi@princeton.edu, www.princeton.edu/~naomi Slide 1 Natural Groups Exhibit remarkable behaviors! Animals may aggregate for Predator evasion Foraging Mating Saving energy Photo by Norbert Wu Slide 2 1
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. Slide 3 Animal Group Models and Mechanics Lagrangian models for fish, birds, mammals: Locomotion forces (drag, constant speed) Aggregation forces (attraction/repulsion) Arrayal forces (velocity/orientation alignment) Deterministic environmental forces (gravity, fluid motions) Artificial potentials, Gyroscopic forces, Symmetry-breaking, Reduction, Energy functions, Stability Random forces (from behavior or the environment) Slide 4 2
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, ) Slide 5 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, 2002. 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 2968-2973, 2001. 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. Slide 6 3
D. Virtual Tensegrity Structures: Outline and Key References 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, 2006. 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, 2003. T.R. Smith, H. Hanssmann and N.E. Leonard. Orientation control of multiple underwater vehicles. In Proc. 40th IEEE CDC, pages 4598-4603, 2001. S. Nair and N.E. Leonard. Stabilization of a coordinated network of rotating rigid bodies. In Proc. 43rd IEEE CDC, pages 4690-4695, 2004. 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. Slide 7 Coordinating Control with Interacting Potentials Leonard and Fiorelli, CDC 2001 Slide 8 4
Stability of Formation Slide 9 A. Artificial Potential Plus Projected Gradient Feedback from artificial potentials and from measurements of environment: Bachmayer and Leonard, CDC, 2002 Vehicle group in descending Gaussian valley Slide 10 5
Gradient Descent: Single Vehicle with Local Gradient Information Slide 11 Gradient Descent: Multiple Vehicle with Local Gradient Information Slide 12 6
Gradient Descent: Multiple Vehicle with Local Gradient Information Slide 13 Gradient Descent: Multiple Vehicle with Local Gradient Information Slide 14 7
Gradient Descent Example: Two Vehicles, T = ½ x 2 y j r i x Slide 15 Gradient Descent Example: Three Vehicles, T = ½ x 2 y i j ρ A k x Slide 16 8
Gradient Descent Example: Three Vehicles, T = ½ x 2 k y j ρ B i x j y ρ A k i x Slide 17 Gradient Descent with Projected Gradient Information Slide 18 9
Gradient Descent with Projected Gradient Information: Single Vehicle Case Slide 19 Gradient Descent with Projected Gradient Information: Single Vehicle Case Slide 20 10
Gradient Descent with Projected Gradient Information: Single Vehicle Case Slide 21 Gradient Descent with Projected Gradient Information: Multiple Vehicle Case See also Moreau, Bachmayer and Leonard, 2002 Slide 22 11
Gradient Descent with Projected Gradient Information: Multiple Vehicle Case Slide 23 B. Virtual Bodies and Artificial Potentials for Cooperative Control Virtual beacons (virtual leaders) Manipulate group geometry: specialized group geometries, symmetry breaking. Slide 24 12
Artificial Potentials and Virtual Beacons N vehicles with fully actuated dynamics*: M virtual beacons are reference points on a virtual (rigid) body. describes the virtual body c.o.m. Assume all virtual beacons (i.e. virtual body) and reference frame move at constant velocity *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. Slide 25 i Control Law for Vehicle i f h f I k j V h V I h 0 h 1 h ik d 0 d 1 xij h 0 h 1 h ik d 0 d 1 x ij Slide 26 13
5 Body Simulation Slide 27 Schooling Case: N=2, M=1 h 0 d 0 d h 0 0 v 0 v 0 v 0 h 0 Stable (S 1 symmetry) Equilibrium is minimum of total potential. Unstable Slide 28 14
Symmetry Breaking Case: N=M=2 h 0 d 0 v 0 h 0 h 0 d 0 v 0 h 0 Slide 29 Schooling Case: N=3 d 0 h 0 v 0 d 0 d 0 d 0 d 0 v 0 Slide 30 15
Schooling: Hexagonal Lattice, N > 3 h 1 h 0 d 0 d 1 v 0 Slide 31 Schooling: Special geometries, N>3, M>1 v 0 d 0 d 1 Slide 32 16
Slide 33 Schooling Stability Slide 34 17
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 IEEE TAC, 2004) Slide 35 Stability of Formation Slide 36 18
Translation, Rotation, Expansion and Contraction Slide 37 Stability of x eq (s) at Any Fixed s Slide 38 19
Speed of Traversal and Formation Stabilization Slide 39 Slide 40 20
Slide 41 Simulation of Path in SO(2) Slide 42 21
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 Slide 43 Adaptive Gradient Climbing Slide 44 22
Least Squares Estimate of Gradient of Measured Scalar Field 0 Slide 45 Least Squares Estimate of Gradient of Measured Scalar Field Assumed measurement noise Slide 46 23
Least Squares Estimate of Gradient of Measured Scalar Field Slide 47 Optimal Formation Problem Slide 48 24
Optimal Formation Problem Slide 49 Optimal Formation Problem: Three-Vehicle Case Slide 50 25
Optimal Formation Problem: Three-Vehicle Case Slide 51 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. Slide 52 26
Rolling and Climbing Vehicle Group Slide 53 27