Planning With Uncertainty for UAVs

1. Planning With Uncertainty for UAVs SameerAnsari Billy Gallagher Kyel Ok William Sica

2. Introduction • Autonomous navigation in forest • Applications • Search & rescue • Stranded hikers • Injured persons • Forest fires • Aircraft crashes • Mapping • Exploration • Forest fires X

3. Introduction • Autonomous navigation in forest • Applications • Search & rescue • Stranded hikers • Injured persons • Forest fires • Aircraft crashes • Mapping • Exploration • Forest fires • Dense concentration of obstacles • Easily solved if map known • Maps not specific enough to mark exact location of trees • Need method to navigate with no prior knowledge of area X

4. Introduction • SLAM • Find and localize obstacles • Well established • Significant existing work • No planning

5. Introduction • SLAM • Find and localize obstacles • Well established • Significant existing work • No planning

6. Introduction • SLAM • Find and localize obstacles • Well established • Significant existing work • No planning

7. Introduction • SLAM • Find and localize obstacles • Well established • Significant existing work • No planning

8. Introduction • SLAM • Find and localize obstacles • Well established • Significant existing work • No planning

9. Introduction • SLAM • Find and localize obstacles • Well established • Significant existing work • No planning • SPLAM • Map obstacles while moving towards goal • Doesn’t try to map whole space, only path to goal • Attempt to find best path to goal given uncertainty of obstacles X

10. Introduction • SLAM • Find and localize obstacles • Well established • Significant existing work • No planning • SPLAM • Map obstacles while moving towards goal • Doesn’t try to map whole space, only path to goal • Attempt to find best path to goal given uncertainty of obstacles X

11. Introduction • SLAM • Find and localize obstacles • Well established • Significant existing work • No planning • SPLAM • Map obstacles while moving towards goal • Doesn’t try to map whole space, only path to goal • Attempt to find best path to goal given uncertainty of obstacles X

12. Introduction • SLAM • Find and localize obstacles • Well established • Significant existing work • No planning • SPLAM • Map obstacles while moving towards goal • Doesn’t try to map whole space, only path to goal • Attempt to find best path to goal given uncertainty of obstacles X

13. Introduction • SLAM • Find and localize obstacles • Well established • Significant existing work • No planning • SPLAM • Map obstacles while moving towards goal • Doesn’t try to map whole space, only path to goal • Attempt to find best path to goal given uncertainty of obstacles X

14. Introduction • SLAM • Find and localize obstacles • Well established • Significant existing work • No planning • SPLAM • Map obstacles while moving towards goal • Doesn’t try to map whole space, only path to goal • Attempt to find best path to goal given uncertainty of obstacles X

15. Introduction • Handling uncertainty in planning • SLAM is uncertain • Accuracy of obstacle location dependent on number of readings • More views of obstacle, better location estimate • Need planner to find path to goal • Stay farther from uncertain obstacles • Can get closer to well established obstacles • Must adjust to changing obstacle position estimates and introduction of newly found obstacles • Flexible to account for changes in global topology as well as uncertainty in local environment

16. Introduction Global Planner • Concept • Two level system • Higher level handles plan to global goal • Ignores uncertainty • Concerned only overall environment • Returns best known path to global goal • Replans when global topology changes • Lower levels plan in local environment • Navigate around nearby uncertain obstacles • Sets local goal as short distance along global path • Returns safest path to local goal • Replans as obstacle uncertainty decreases • High level planner runs less often than low level planner Local Planner Robot Controller

17. System Overview • Robot Simulator • Runs robot controller • Calls planning algorithms • Makes choices on how to execute plan • Global Planner • Evaluates entire known environment • Creates Voronoi decomposition • Finds optimal path through Voronoi diagram using A* • Returns local goal along Voronoi diagram path • Local Planner • Evaluates local visible environment • Determines safest path to local goal • Uses potential fields to push robot away from obstacles

18. Local Planner • Generates potential field of local environment • Linear distance to goal • Drives robot towards local goal • Gaussian distribution around uncertain obstacles • Higher uncertainty  wider Gaussian  stronger push • Keeps robot farther from more uncertain obstacles • Linear distance from obstacles when close • Keeps robot from getting to close to any obstacle

19. Local Planner • Generates potential field of local environment • Linear distance to goal • Drives robot towards local goal • Gaussian distribution around uncertain obstacles • Higher uncertainty  wider Gaussian  stronger push • Keeps robot farther from more uncertain obstacles • Linear distance from obstacles when close • Keeps robot from getting to close to any obstacle Known Obstacle Uncertain Obstacle Known Obstacle Uncertain Obstacle Local Goal Local Goal

20. Local Planner • Obstacle Uncertainty • Estimated location • Random Sample from distribution • Normal distribution • Mean • Actual position • Variance • Inversely related to Distance & View count • k – Multiplicative Factor k•Variance Actual location Estimated location Probability k•Variance

21. Local Planner • Gradient descent used to find path • Follow down steepest gradient towards minimum • Stops when • At goal • Gradient converges to zero • Position converges • Finds path that stays farther from more uncertain obstacles • If obstacle near or at goal, will get close to goal while still avoiding obstacle

22. Local Planner • Gradient descent used to find path • Follow down steepest gradient towards minimum • Stops when • At goal • Gradient converges to zero • Position converges • Finds path that stays farther from more uncertain obstacles • If obstacle near or at goal, will get close to goal while still avoiding obstacle

23. Local Planner • Gradient descent used to find path • Follow down steepest gradient towards minimum • Stops when • At goal • Gradient converges to zero • Position converges • Finds path that stays farther from more uncertain obstacles • If obstacle near or at goal, will get close to goal while still avoiding obstacle

24. Local Planner • Gradient descent used to find path • Follow down steepest gradient towards minimum • Stops when • At goal • Gradient converges to zero • Position converges • Finds path that stays farther from more uncertain obstacles • If obstacle near or at goal, will get close to goal while still avoiding obstacle

25. Local Planner • Gradient descent used to find path • Follow down steepest gradient towards minimum • Stops when • At goal • Gradient converges to zero • Position converges • Finds path that stays farther from more uncertain obstacles • If obstacle near or at goal, will get close to goal while still avoiding obstacle

26. Local Planner • Incomplete due to local minima • Path will end at zero gradient • Will prevent reaching global minimum (in local environment) • Won’t reach local goal • Good because won’t force way to goal if can’t get there • Bad because can get stuck behind wall of obstacles • Need higher level planner to provide new local goal

27. Global Planner • Overcome Local Minima Problem • Voronoi Decomposition • Voronoi Vertices are local minima • Way point for local planner • A* search

28. Global Planner • Overcome Local Minima Problem • Voronoi Decomposition • Voronoi Vertices are local minima • Way point for local planner • A* search

29. Global Planner • Overcome Local Minima Problem • Voronoi Decomposition • Voronoi Vertices are local minima • Way point for local planner • A* search

30. Integrated System • Final System Global Goal R(x,y) O(x,y) SLAM Robot Position R(x,y) Obstacle Position O(x,y) Global Planner Finds Local Goal Local Goal R(x,y) O(x,y) Local Planner Finds Path Controller Updates Robot Position Path (x,y) Simulator

31. Integrated System • Final System Significant change in O (x,y) SLAM Robot Position R(x,y) Obstacle Position O(x,y) Global Planner Finds Local Goal Local Planner Finds Path Controller Updates Robot Position Path (x,y) Simulator Every iteration

32. Simulator ImplementationOverview Simulator – Global Planner Field • Programming Language(s) • MATLAB • C++ subroutines - A* search • World Model • 2D Field • Circular Obstacles (Tree Trunks) • Minimum Collision Distance • Global Goal Position (GPS) • Senses for UAV • 360° View • Maximum view distance • Uncertain Obstacle positions • Gaussian Noise • Planner Implementations • Global - Voronoi Planner • Local - Potential Field Planner • Trials and Tribulations • Advantages/Disadvantages Global Goal (x,y) Local Goal (x,y) UAV/Robot Local Planner Field Range of View Position (x,y) Origin (0,0)

33. Legend - What are we looking at? Let’s see it in action! Global Goal What UAV Sees Actual Obstacle Positions Local Goal UAV Trail UAV Range of View

34. What did we just see? What the UAV sees Global Voronoi Planner

35. What’s going on behind the scenes? Global Voronoi Planner Local Potential Field Planner

36. Global Planner - Voronoi Virtual Node A* Path Local Goal • Voronoi Decomposition • Based on what UAV sees • Updates on global step versus local • A Star search along nodes • UAV to Global Goal • Provides A* node as Local Goal • Given to Potential Field Planner • Virtual Nodes • 4 Virtual Nodes Added • Corners to a box • Diagonal connects Robot to Global Goal • Gives starting-ending choices

37. Local Potential Field (PF) Planner Video Local Planner works on smaller areas of the playing field Local Goal given by Global Planner

38. SpecialPlanner Updates Notice how Potential Field is better than Voronoi A*locally When No uncertainty – Voronoi plan does not need to update When No obstacles – No planners, head heuristically towards goal

39. Proof of Completeness • If there is a solution, the algorithm returns it in finite time. • If no solution, the program terminates.

40. Proof of Completeness • Potential Field • Forces • Uncertainty • Better locally than global Voronoi planner • Proximity • Local Goal • Gradient descent • Incomplete by itself • Can get stuck in local minima • Can fail to find paths

41. Proof of Completeness • Voronoi Diagram • The start and goal points are Voronoi Vertices • -putting virtual obstacles around these points • All Voronoi Vertices are connected • -property of Voronoi decomposition • Start and goal are connected by Voronoi Edges Ignoring robot dimensions, there always is a path from start to goal (1 satisfied)

42. Proof of Completeness Taking robot dimensions into account: Voronoi Edges too close to obstacles are removed Using BFS, if the start is no longer connected to the goal, return no solution (2 satisfied)

43. Completeness - No paths • Voronoi planner complete for what it sees • For all visible obstacles • If a path to the goal exists, find it • If no path exists, returns no path • Overall planner waits for obstacle uncertainties to converge, ie. ‘Reality’ • If there is no path in reality, overall planner returns no path You shall not pass! No Path – Boxed In

44. More Videos! Path-Finding with A* Avoid Local Minima

45. More Obstacles! 1000 Obstacles Edge Pruning

46. Stress test for fun and science 10,000 Obstacles Of course it went the wrong way first >:( Not real-time at all, ~1-2 hours to simulate

47. Future Extensions • Localization & mapping part of SPLAM • Real-time implementation • Reduce Voronoicomplexity for faster performance • MATLAB is very good, but not that good • Do expensive work with C++/Mexsubroutines • Enhancements to robot • Varying update frequency of each planner • Limited field of view for UAV • Dynamic motion • Velocity, momentum • PID control systems integration

48. Questions? For those who are interested: elucidation.github.com/UAV-Motion-Planner-Ensemble/ The momentous and historic code creation process is immortalized on GitHub. Source code is freely available for downloading/forking.