Formations and Obstacle Avoidance in Mobile Robot Control

1. Formations and Obstacle Avoidance in Mobile Robot Control Petter Ögren Topics from a Doctoral Thesis, 2003

2. Outline • A brief introduction of all four papers • Overview of how they relate to each other • Details of Paper A • Details of Paper B

3. Obstacle Avoidance Paper A Paper B Paper D Formations Paper C All four Papers

4. Paper A: A Convergent Dynamic Window Approach to Obstacle Avoidance Problem formulation: Drive a robot from A to B through a partially unknown environment without collisions. B A

5. Paper A: A Convergent Dynamic Window Approach to Obstacle Avoidance Proposed solution:Merge state-of-the-art heuristics with a provable approach, (using a CLF/MPC framework). ) Optimize pointwise over stabilizing controls

6. Paper B: Obstacle Avoidance in Formation Problem: How do we move the leader to guide a leader-follower formation through obstacle terrain? Can we use singel vehicle Obstacle Avoidance?

7. Paper B: Obstacle Avoidance in Formation Proposed solution: The concept of Configuration Space Obstacles is extended through an Input to State Stability (ISS) argument. )A map of the leader positions that guarantee followers enough free space. The leader does single vehicle obstacle avoidance using this map.

8. Paper C: A Control Lyapunov Function Approach to Multi Agent Coordination Problem: How to make set-point controlled robots moving along trajectories in a formation ”wait” for eachother?

9. Paper C: A Control Lyapunov Function Approach to Multi Agent Coordination • Idea: Combine Control Lyapunov Functions (CLF) with the Egerstedt&Hu virtual vehicle approach. • Under assumptions this will result in: • Bounded formation error (waiting) • Approximation of given formation velocity (if no waiting is necessary). • Finite completion time (no 1-waiting).

10. Paper D: Cooperative Control of Mobile Sensor Networks: Adaptive Gradient Climbing in a Distributed Environment Problem D1: Given a local-spring-damper formation control. • How do we translate, rotate and expand the formation? Problem D2: Given a field, i.e. temperature or nutrition density in water. • How do we estimate the gradient from noisy distributed measurements? • What formation geometries give good estimates?

11. Paper D: Cooperative Control of Mobile Sensor Networks: Adaptive Gradient Climbing in a Distributed Environment Proposed solution D1: • Introduce virtual leaders in the formation and move these. • Let direction of motion be governed by the mission, e.g. gradient climbing. • Let the speed of the motion be influenced by error feedback (from paperC).

12. Noisy Paper D: Cooperative Control of Mobile Sensor Networks: Adaptive Gradient Climbing in a Distributed Environment Proposed solution D2: • Estimate the gradient:Use the least Squares estimate of (a,b) in an affine approximation aTz+b ¼ T(z). Apply Kalman filter over time. • Formationgeometries:Minimize error due to measurement noise and second order terms. In 1-dimension: True Estimate This is generalized to m vehicles in Rn .

13. All four Papers Paper A Paper B Details! Paper D Paper C

14. Differential drive robots can be feedback linearized to this. Paper A: A Convergent Dynamic Window Approach to Obstacle Avoidance Drive a robot from A to B through a partially unknown environment without collisions. B A

15. Background: The Dynamic Unicycle q

16. Desirable Properties in Obstacle Avoidance • No collisions • Convergence to goal position • Efficient, large inputs • ‘Real time’ • ‘Reactive’, (to changes in environment)

17. Background: Two main Obstacle Avoidance approaches • Deliberative/Sense-Plan-Act • Trajectory planning/tracking • Navigation function (Koditschek ’92). • Provable features. • Changes are a problem Reactive/Behavior Based • Biologically motivated • Fast, local rules. • ‘The world is the map’ • No proofs. • Changing environment not a problem Combine the two?

18. Background: The Navigation Function (NF) tool • One local/global min at goal. • Gradient gives direction to goal. • Solves ‘maze’ problems. Obstacles and NF level curves Goal

19. Exact Navigation, using Art. Pot. Fcn. Koditscheck ’92 DWA, Fox et. al. and Brock et al Model Predictive Control (MPC) Control Lyapunov Function (CLF) MPC/CLF Framework, Primbs ’99 Convergent DWA Basic Idea • ‘Real time’ • Efficient, large inputs • ‘Reactive’, to changes • Convergence proof. • No collisions

20. Background: Model Predictive Control (MPC) • Idea: Given a good model, we can simulate the result of different control choices (over time T) and apply the best. • Feedback: repeat simulation every t<T seconds. How is this connected to the Dynamic Window Approach?

21. Robot Vy Velocity Space Current Velocity Cirular arc pseudo-trajectories Dynamic Window Vx Control Options Obstacles Vmax Global Dynamic Window Approach (Brock and Khatib ‘99)

22. Global Dynamic Window Approach (continued) • Check arcs for collision free length. • Chose control by optimization of the heuristic utility function: • Speeds up to 1m/s indoors with XR 4000 robot (Good!). • No proofs. (Counter example!) • Idea: • See as Model Predictive Control (MPC) • Use navigation function as CLF

23. V x Background: Control Lyapunov Function (CLF) • Idea: If the energy of a system decreases all the time, it will eventually “stop”. • A CLF, V, is an “energy-like” function such that

24. Exact Robot Navigation using Artificial Potential Functions, (Rimon and Koditscheck ‘92) • C1 Navigation Function NF(p) constructed. • NF(p)=NFmax at obstacles of Sphere and Star worlds. • Control: • Features: • Lyapunov function: => No collisions. • Bounded Control. • Convergence Proof • Drawbacks • Hard to (re)calculate. • Inefficient • Idea: Use C0 Control Lyapunov Function.

25. Our Navigation Function (NF) • One local/global min at goal. • Calculate shortest path in discretization. • Make continuous surface by careful interpolation using triangles. • Provable properties. The discretization

26. MPC/CLF framework Primbs general form: Here we write:

27. The resulting scheme: Lyapunov Function and Control Lyapunov function candidate: gives the following set of controls, incl. Compare: Acceleration of down hill skier.

28. Safety and Discretization • The CLF gives stability, what about safety? • In MPC, consider controls stop without collision. • Plan to first accelerate: then brake: • Apply first part and replan. Compare: Being able to stop in visible part of road ) safety

29. Evaluated MPC Trajectories

30. Simulation Trajectory

31. Single Vehicle Conclusions Properties: • No collisions (stop safely option) • Convergence to goal position (CLF) • Efficient (MPC). • Reactive (MPC). • Real time (?), small discretized control set, formalizing earlier approach. Can this scheme be extended to the multi vehicle case?

32. All four Papers Paper A Paper B Details! Paper D Paper C

33. Applications: Search and Rescue missions, lawn moving etc. Carry large/awkward objects Adaptive sensing, e.g. surveillance or ocean sampling Satellite imaging in formation Motivations: Flexibility Robustness Performance Price Why Multi Agent Robotics?

34. Paper B: Obstacle Avoidance in Formation How do we use singel vehicle Obstacle Avoidance?

35. Desirable properties • No collisions • Convergence to goal position • Efficient, large inputs • ‘Real time’ • ‘Reactive’, to changes & • Distributed/Local information

36. A Leader-Follower Structure Two Cases: • No explicit information exchange ) leader acceleration, u1, is a disturbance • Feedforward of u1) time delays and calibration errors are disturbances Leader Information flow How big deviations will the disturbances cause?

37. Background:Input to State Stability (ISS) We will use the ISS to calculate ”Uncertainty Regions”

38. Uncertainty Region ISS ) Uncertainty Region

39. ”Free” leader pos. ”Occupied” leader pos. Formation Leader Obstacles, an extension of Configuration Space Obstacles Obstacle How do we calculate a map of ”free” leader positions?

40. Formation Leader Map Formation Obstacles Unc. Region and Obstacles • Computable by conv2 (matlab). • Leader does obstacle avoidance in new map. • Followers do formation keeping under disturbance.

41. Simulation Trajectories

42. Conclusions, paper B • Obstacle Avoidance extended to formations by assuming leader-follower structure and ISS. • Future directions • Rotations • Expansions • Breaking formation )¸3 dim NF

43. All four Papers Paper A Paper B Details! Paper D Paper C

44. End of Presentation.

45. Paper C: A Control Lyapunov Function Approach to Multi Agent CoordinationP. Ögren, M. Egerstedt* and X. HuRoyal Institute of Technology (KTH), Stockholm and Georgia Institute of Technology*IEEE Transactions on Robotics and Automation, Oct 2002

46. Problem and Proposed Solution • Problem: How to make set-point controlled robots moving along trajectories in a formation ”wait” for eachother? • Idea: Combine Control Lyapunov Functions (CLF) with the Egerstedt&Hu virtual vehicle approach. • Under assumptions this will result in: • Bounded formation error (waiting) • Approx. of given formation velocity (if no waiting is nessesary). • Finite completion time (no 1-waiting).

47. Quantifying Formation Keeping Definition: Formation Function Will add Lyapunov like assumption satisfied by individual set-point controllers. => Think of as parameterized Lyapunov function.

48. Examples of Formation Function • Simple linear example ! • A CLF for the combined higher dimensional system: • Note that a,b, are design parameters. • The approach applies to any parameterized formation scheme with lyapunov stability results.

49. Main Assumption • We can find a class K function s such that the given set-point controllers satisfy: • This can be done when -dV/dt is lpd, V is lpd and decrescent. It allows us to prove: • Bounded V (error): V(x,s) < VU • Bounded completion time. • Keeping formation velocity v0, if V ¿ VU.

50. Speed along trajectory: How Do We Update s? • Suggestion: s=v0 t • Problems: Bounded ctrl or local ass stability We want: • V to be small • Slowdown if V is large • Speed v0 if V is small Suggestion: • Let s evolve with feedback from V.