Sameer Ansari Billy Gallagher Kyel Ok William Sica

 Autonomous navigation in forest ◦ Applications  Search & rescue  Stranded hikers  Injured persons  Forest fires  Aircraft crashes  Mapping  Exploration  Forest fires X

 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

 SLAM ◦ Find and localize obstacles ◦ Well established ◦ Significant existing work ◦ No planning

 SLAM ◦ Find and localize obstacles ◦ Well established ◦ Significant existing work ◦ No planning

 SLAM ◦ Find and localize obstacles ◦ Well established ◦ Significant existing work ◦ No planning

 SLAM ◦ Find and localize obstacles ◦ Well established ◦ Significant existing work ◦ No planning

 SLAM ◦ Find and localize obstacles ◦ Well established ◦ Significant existing work ◦ No planning

 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

 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

 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

 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

 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

 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

 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

 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 Robot Controller Local Planner Global Planner

 Motivated by work in SPLAM ◦ Uncertainties in local data that converge over time ◦ Incomplete information about the global map ◦ Ability to update information about the world through successive iterations  Previous works in planning have used Voronoi diagrams to escape local minima in potential fields [1] [1] “Robot Motion Planning: A Distributed Representation Approach” - Barraquand and Latombe

 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

 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

 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 Local Goal Known Obstacle Uncertain Obstacle

 Obstacle Uncertainty ◦ Estimated location  Random Sample from distribution ◦ Normal distribution  Mean  Actual position  Variance  Inversely related to Distance & View count  k – Multiplicative Factor kVariance Estimated location Probability Actual location kVariance

 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

 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

 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

 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

 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

 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

 Overcome Local Minima Problem  Voronoi Decomposition ◦ Voronoi Vertices are local minima ◦ Way point for local planner ◦ A* search

 Overcome Local Minima Problem  Voronoi Decomposition ◦ Voronoi Vertices are local minima ◦ Way point for local planner ◦ A* search

 Overcome Local Minima Problem  Voronoi Decomposition ◦ Voronoi Vertices are local minima ◦ Way point for local planner ◦ A* search

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

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

 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 Simulator – Global Planner Field Origin (0,0) UAV/Robot Range of View Position (x,y) Global Goal (x,y) Local Goal (x,y) Local Planner Field

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

What the UAV seesGlobal Voronoi Planner

Local Potential Field Planner

Virtual Nodes 4 Virtual Nodes Added Corners to a box Diagonal connects Robot to Global Goal Gives starting-ending choices Virtual Nodes 4 Virtual Nodes Added Corners to a box Diagonal connects Robot to Global Goal Gives starting-ending choices 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 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

Local Planner works on smaller areas of the playing field Local Goal given by Global Planner

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

1. If there is a solution, the algorithm returns it in finite time. 2. If no solution, the program terminates.

 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

 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)

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)

No Path – Boxed In  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!

Path-Finding with A*Avoid Local Minima

1000 Obstacles Edge Pruning

10,000 Obstacles Of course it went the wrong way first >:( Not real-time at all, ~1-2 hours to simulate

 Short term possibilities ◦ Code has been designed with extension in mind ◦ Varying time-step per planner ◦ Limited field of view ◦ Dynamic motion  Velocity, momentum  PID control systems integration, etc.  Motion planner ensembles ◦ Raises new possibilities & challenges  Tasty apart, Delicious together.  Defining completeness (Overall, per planner, per step…)  Real-time implementation ◦ Simplified models can run real-time for (N<500)  Voronoi planner is the time-sink  Solution: Reduce Voronoi complexity  Longer time-steps  Less points ◦ Matlab is good, very good, but not that good.  Do expensive work with C++ subroutines (Mex files)

 Localization & mapping part of SPLAM  Real-time implementation ◦ Reduce Voronoi complexity for faster performance ◦ MATLAB is very good, but not that good  Do expensive work with C++/Mex subroutines  Enhancements to robot ◦ Varying update frequency of each planner ◦ Limited field of view for UAV ◦ Dynamic motion  Velocity, momentum  PID control systems integration

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.