1. N.E. Leonard – U. Pisa – 18-20 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 naomi@princeton.edu, www.princeton.edu/~naomi www.princeton.edu/~naomi

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

3. N.E. Leonard – U. Pisa – 18-20 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.

4. N.E. Leonard – U. Pisa – 18-20 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

5. N.E. Leonard – U. Pisa – 18-20 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, …)

6. N.E. Leonard – U. Pisa – 18-20 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, 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.

7. N.E. Leonard – U. Pisa – 18-20 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, 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.

8. N.E. Leonard – U. Pisa – 18-20 April 2007 Slide 8 Coordinating Control with Interacting Potentials Leonard and Fiorelli, CDC 2001

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

10. N.E. Leonard – U. Pisa – 18-20 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

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

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

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

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

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

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

17. N.E. Leonard – U. Pisa – 18-20 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

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

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

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

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

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

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

24. N.E. Leonard – U. Pisa – 18-20 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.

25. N.E. Leonard – U. Pisa – 18-20 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

26. N.E. Leonard – U. Pisa – 18-20 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

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

28. N.E. Leonard – U. Pisa – 18-20 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.

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

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

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

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

33. N.E. Leonard – U. Pisa – 18-20 April 2007 Slide 33

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

35. N.E. Leonard – U. Pisa – 18-20 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 )

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

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

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

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

40. N.E. Leonard – U. Pisa – 18-20 April 2007 Slide 40

41. N.E. Leonard – U. Pisa – 18-20 April 2007 Slide 41

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

43. N.E. Leonard – U. Pisa – 18-20 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

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

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

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

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

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

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

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

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

52. N.E. Leonard – U. Pisa – 18-20 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.

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