A Convergent Dynamic Window Approach to Obstacle Avoidance & Obstacle Avoidance in Formation

1. A Convergent Dynamic Window Approach to Obstacle Avoidance&Obstacle Avoidance in Formation P. Ögren (KTH) N. Leonard (Princeton University)

2. Differential drive robots can be feedback linearized to this. Problem Formulation Drive a robot from A to B through a partially unknown environment without collisions. B A

3. Background: The Dynamic Unicycle (or a Tank?) q

4. Desirable Properties • No collisions • Convergence to goal position • Efficient, large inputs • ‘Real time’ • ‘Reactive’, to changes

5. 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?

6. 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

7. 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

8. 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 used in the Dynamic Window Approach?

9. 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)

10. 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

11. 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

12. 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.

13. 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

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

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

16. 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

17. Evaluated MPC Trajectories

18. Simulation Trajectory

19. 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?

20. Applications: Search and Rescue missions Carry large/awkward objects Adaptive sensing Satellite imaging in formation Motivations: Flexibility Robustness Performance Price Why Multi Agent Robotics?

21. Obstacle Avoidance in Formation How do we use singel vehicle Obstacle Avoidance?

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

23. 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?

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

25. Uncertainty Region ISS ) Uncertainty Region

26. ”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?

27. 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.

28. Simulation Trajectories

29. Final Conclusions • Obstacle Avoidance extended to formations by assuming leader-follower structure and ISS. • Future directions • Rotations • Expansions • Braking formation )¸3 dim NF

30. Comparison