planning with uncertainty for uavs
DESCRIPTION
Planning With Uncertainty for UAVs. Sameer Ansari Billy Gallagher Kyel Ok William Sica. Introduction. Autonomous navigation in forest Applications Search & rescue Stranded hikers Injured persons Forest fires Aircraft crashes Mapping Exploration Forest fires. X. Introduction. - PowerPoint PPT PresentationTRANSCRIPT
Planning With Uncertainty for UAVs
Sameer AnsariBilly Gallagher
Kyel OkWilliam Sica
Autonomous navigation in forest◦ Applications
Search & rescue Stranded hikers Injured persons Forest fires Aircraft crashes
Mapping Exploration Forest fires
IntroductionX
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
IntroductionX
SLAM◦ Find and localize obstacles◦ Well established◦ Significant existing work◦ No planning
Introduction
SLAM◦ Find and localize obstacles◦ Well established◦ Significant existing work◦ No planning
Introduction
SLAM◦ Find and localize obstacles◦ Well established◦ Significant existing work◦ No planning
Introduction
SLAM◦ Find and localize obstacles◦ Well established◦ Significant existing work◦ No planning
Introduction
SLAM◦ Find and localize obstacles◦ Well established◦ Significant existing work◦ No planning
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
IntroductionX
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
IntroductionX
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
IntroductionX
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
IntroductionX
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
IntroductionX
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
IntroductionX
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
Introduction
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
Introduction
Robot Controlle
r
Local Planner
Global Planner
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
System Overview
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
Local Planner
0 10 20 30 40 50 60 70 80 90 1000
10
20
30
40
50
60
70
80
90
100
0 10 20 30 40 50 60 70 80 90 100
0
10
20
30
40
50
60
70
80
90
100
0
1
2
3
4
5
6
7
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
Local Planner
0 10 20 30 40 50 60 70 80 90 1000
10
20
30
40
50
60
70
80
90
100
0 10 20 30 40 50 60 70 80 90 100
0
10
20
30
40
50
60
70
80
90
100
0
1
2
3
4
5
6
7
Known Obstacle
Local Goal
Local Goal
Known ObstacleUncertain 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
Local Planner
k•Variance
Estimatedlocation
Prob
abilit
y
Actuallocation
k•Variance
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
Local Planner
0 10 20 30 40 50 60 70 80 90 1000
10
20
30
40
50
60
70
80
90
100
0 10 20 30 40 50 60 70 80 90 100
0
10
20
30
40
50
60
70
80
90
100
0
1
2
3
4
5
6
7
Local Planner
0 10 20 30 40 50 60 70 80 90 1000
10
20
30
40
50
60
70
80
90
100
0 10 20 30 40 50 60 70 80 90 100
0
10
20
30
40
50
60
70
80
90
100
0
1
2
3
4
5
6
7
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
Local Planner
0 10 20 30 40 50 60 70 80 90 1000
10
20
30
40
50
60
70
80
90
100
0 10 20 30 40 50 60 70 80 90 100
0
10
20
30
40
50
60
70
80
90
100
0
1
2
3
4
5
6
7
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
Local Planner
0 10 20 30 40 50 60 70 80 90 1000
10
20
30
40
50
60
70
80
90
100
0 10 20 30 40 50 60 70 80 90 100
0
10
20
30
40
50
60
70
80
90
100
0
1
2
3
4
5
6
7
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
Local Planner
0 10 20 30 40 50 60 70 80 90 1000
10
20
30
40
50
60
70
80
90
100
0 10 20 30 40 50 60 70 80 90 100
0
10
20
30
40
50
60
70
80
90
100
0
1
2
3
4
5
6
7
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
Local Planner
-20 0 20 40 60 80 100 120 140 160
-40
-20
0
20
40
60
80
100
120
Overcome Local Minima Problem Voronoi Decomposition
◦ Voronoi Vertices are local minima◦ Way point for local planner◦ A* search
Global Planner
Overcome Local Minima Problem Voronoi Decomposition
◦ Voronoi Vertices are local minima◦ Way point for local planner◦ A* search
Global Planner
Overcome Local Minima Problem Voronoi Decomposition
◦ Voronoi Vertices are local minima◦ Way point for local planner◦ A* search
Global Planner
Final System
Integrated System
SLAMRobot Position R(x,y)
Obstacle Position O(x,y)Global Planner
Finds Local Goal
Global GoalR(x,y)O(x,y)
Local PlannerFinds Path
Local GoalR(x,y)O(x,y)
Path (x,y)ControllerUpdates Robot Position
Simulator
Final System
Integrated System
SLAMRobot Position R(x,y)
Obstacle Position O(x,y)Global Planner
Finds Local Goal
Local PlannerFinds Path
Path (x,y)ControllerUpdates Robot Position
Simulator
Significant change in O (x,y)
Every iteration
Simulator ImplementationOverview 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
ActualObstacle Positions
UAV Range of View
Trail UAV
Let’s see it in action!
What did we just see?
What the UAV sees Global Voronoi Planner
What’s going on behind the scenes?
Global Voronoi Planner Local Potential Field Planner
Global Planner - Voronoi
Virtual Nodes• 4 Virtual Nodes Added• Corners to a box• Diagonal connects Robot to Global Goal• Gives starting-ending
choices
A* Path
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 Goal
Virtual Node
Local Potential Field (PF) Planner VideoLocal Planner works on smaller areas
of the playing fieldLocal Goal given by Global Planner
SpecialPlanner Updates
Notice how Potential Field is better than Voronoi A* locallyWhen 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.
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
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)
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)
Proof of Completeness
Completeness - No paths
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!
More Videos!
Path-Finding with A* Avoid Local Minima
1000 ObstaclesEdge Pruning
More Obstacles!
Stress test for fun
and science10,000 Obstacles
Of course it went the wrong way first >:( Not real-time at all, ~1-2 hours to simulate
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
Future Extensions
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.