Uncertainty for UAVs Sameer Ansari Billy Gallagher Kyel Ok William Sica Autonomous navigation in forest Applications Search amp rescue Stranded hikers Injured persons Forest fires ID: 442227
Download Presentation The PPT/PDF document "Planning With" is the property of its rightful owner. Permission is granted to download and print the materials on this web site for personal, non-commercial use only, and to display it on your personal computer provided you do not modify the materials and that you retain all copyright notices contained in the materials. By downloading content from our website, you accept the terms of this agreement.
Slide1
Planning With Uncertainty for UAVs
Sameer
Ansari
Billy Gallagher
Kyel
Ok
William
SicaSlide2
Autonomous navigation in forest
Applications
Search & rescueStranded hikersInjured personsForest firesAircraft crashesMappingExplorationForest fires
Introduction
XSlide3
Autonomous navigation in forestApplications
Search & rescue
Stranded hikersInjured personsForest firesAircraft crashesMappingExplorationForest firesDense concentration of obstaclesEasily solved if map known
Maps not specific enough to mark exact location of treesNeed method to navigate with no prior knowledge of area
Introduction
XSlide4
SLAM
Find and localize obstacles
Well establishedSignificant existing workNo planningIntroductionSlide5
SLAM
Find and localize obstacles
Well establishedSignificant existing workNo planningIntroductionSlide6
SLAM
Find and localize obstacles
Well establishedSignificant existing workNo planningIntroductionSlide7
SLAM
Find and localize obstacles
Well establishedSignificant existing workNo planningIntroductionSlide8
SLAM
Find and localize obstacles
Well establishedSignificant existing workNo planningIntroductionSlide9
SLAM
Find and localize obstacles
Well establishedSignificant existing workNo planningSPLAMMap obstacles while moving towards goalDoesn’t try to map whole space, only path to goalAttempt to find best path to goal given uncertainty of obstacles
Introduction
XSlide10
SLAM
Find and localize obstacles
Well establishedSignificant existing workNo planningSPLAMMap obstacles while moving towards goalDoesn’t try to map whole space, only path to goalAttempt to find best path to goal given uncertainty of obstacles
Introduction
XSlide11
SLAM
Find and localize obstacles
Well establishedSignificant existing workNo planningSPLAMMap obstacles while moving towards goalDoesn’t try to map whole space, only path to goalAttempt to find best path to goal given uncertainty of obstacles
Introduction
XSlide12
SLAM
Find and localize obstacles
Well establishedSignificant existing workNo planningSPLAMMap obstacles while moving towards goalDoesn’t try to map whole space, only path to goalAttempt to find best path to goal given uncertainty of obstacles
Introduction
XSlide13
SLAM
Find and localize obstacles
Well establishedSignificant existing workNo planningSPLAMMap obstacles while moving towards goalDoesn’t try to map whole space, only path to goalAttempt to find best path to goal given uncertainty of obstacles
Introduction
XSlide14
SLAM
Find and localize obstacles
Well establishedSignificant existing workNo planningSPLAMMap obstacles while moving towards goalDoesn’t try to map whole space, only path to goalAttempt to find best path to goal given uncertainty of obstacles
Introduction
XSlide15
Handling uncertainty in planningSLAM is uncertain
Accuracy of obstacle location dependent on number of readings
More views of obstacle, better location estimateNeed planner to find path to goalStay farther from uncertain obstaclesCan get closer to well established obstaclesMust adjust to changing obstacle position estimates and introduction of newly found obstaclesFlexible to account for changes in global topology as well as uncertainty in local environment
IntroductionSlide16
ConceptTwo level system
Higher level handles plan to global goal
Ignores uncertaintyConcerned only overall environmentReturns best known path to global goalReplans when global topology changesLower levels plan in local environmentNavigate around nearby uncertain obstaclesSets local goal as short distance along global path
Returns safest path to local goalReplans as obstacle uncertainty decreases
High level planner runs less often than low level plannerIntroduction
Robot Controller
Local Planner
Global PlannerSlide17
Motivated by work in SPLAMUncertainties in local data that converge over time
Incomplete information about the global map
Ability to update information about the world through successive iterationsPrevious works in planning have used Voronoi diagrams to escape local minima in potential fields [1]Related Work
[1] “Robot Motion Planning: A Distributed Representation Approach” -
Barraquand and LatombeSlide18
Robot SimulatorRuns robot controller
Calls planning algorithms
Makes choices on how to execute planGlobal PlannerEvaluates entire known environmentCreates Voronoi decompositionFinds optimal path through Voronoi
diagram using A*Returns local goal along Voronoi diagram path
Local PlannerEvaluates local visible environmentDetermines safest path to local goalUses potential fields to push robot away from obstacles
System OverviewSlide19
Generates potential field of local environment
Linear distance to goal
Drives robot towards local goalGaussian distribution around uncertain obstaclesHigher uncertainty wider Gaussian stronger pushKeeps robot farther from more uncertain obstaclesLinear distance from obstacles when close
Keeps robot from getting to close to any obstacle
Local PlannerSlide20
Generates potential field of local environment
Linear distance to goal
Drives robot towards local goalGaussian distribution around uncertain obstaclesHigher uncertainty wider Gaussian stronger pushKeeps robot farther from more uncertain obstaclesLinear distance from obstacles when close
Keeps robot from getting to close to any obstacle
Local Planner
Known Obstacle
Local Goal
Local Goal
Known Obstacle
Uncertain Obstacle
Uncertain ObstacleSlide21
Obstacle Uncertainty
Estimated location
Random Sample from distributionNormal distributionMean Actual positionVarianceInversely related to Distance & View countk – Multiplicative Factor
Local Planner
k
•Variance
Estimated
location
Probability
Actual
location
k
•VarianceSlide22
Gradient descent used to find path
Follow down steepest gradient towards minimum
Stops whenAt goalGradient converges to zeroPosition convergesFinds path that stays farther from more uncertain obstaclesIf obstacle near or at goal, will get close to goal while still avoiding obstacle
Local PlannerSlide23
Local Planner
Gradient descent used to find path
Follow down steepest gradient towards minimum
Stops when
At goal
Gradient converges to zeroPosition convergesFinds path that stays farther from more uncertain obstacles
If obstacle near or at goal, will get close to goal while still avoiding obstacleSlide24
Local Planner
Gradient descent used to find path
Follow down steepest gradient towards minimum
Stops when
At goal
Gradient converges to zero
Position convergesFinds path that stays farther from more uncertain obstacles
If obstacle near or at goal, will get close to goal while still avoiding obstacleSlide25
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 obstacleSlide26
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 obstacleSlide27
Incomplete due to local minima
Path will end at zero gradient
Will prevent reaching global minimum (in local environment)Won’t reach local goalGood because won’t force way to goal if can’t get thereBad because can get stuck behind wall of obstaclesNeed higher level planner to provide new local goalLocal PlannerSlide28
Overcome Local Minima ProblemVoronoi Decomposition
Voronoi Vertices are local minima
Way point for local plannerA* searchGlobal PlannerSlide29
Overcome Local Minima Problem
Voronoi Decomposition
Voronoi Vertices are local minimaWay point for local plannerA* search
Global PlannerSlide30
Overcome Local Minima Problem
Voronoi Decomposition
Voronoi Vertices are local minimaWay point for local plannerA* search
Global PlannerSlide31
Final System
Integrated System
SLAM
Robot Position R(
x,y)Obstacle Position O(x,y)
Global PlannerFinds Local Goal
Global
GoalR(x,y
)O(x,y)
Local Planner
Finds Path
Local GoalR(x,y)O(x,y)
Path (
x,y)
ControllerUpdates Robot Position
SimulatorSlide32
Final System
Integrated System
SLAM
Robot Position R(
x,y)Obstacle Position O(x,y)
Global PlannerFinds Local Goal
Local Planner
Finds Path
Path (
x,y
)
ControllerUpdates Robot Position
Simulator
Significant change in O (
x,y
)
Every iterationSlide33
Simulator
Implementation
Overview
Programming Language(s)
MATLAB C++ subroutines - A* searchWorld Model2D FieldCircular Obstacles (Tree Trunks)Minimum Collision Distance
Global Goal Position (GPS)Senses for UAV360° ViewMaximum view distanceUncertain Obstacle positions Gaussian Noise
Planner ImplementationsGlobal - Voronoi PlannerLocal - Potential Field PlannerTrials and Tribulations
Advantages/DisadvantagesSimulator – Global Planner Field
Origin (0,0)
UAV/Robot
Range of View
Position (x,y)
Global Goal (x,y)
Local Goal (x,y)
Local Planner FieldSlide34
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!Slide35
What did we just see?
What the UAV sees
Global Voronoi PlannerSlide36
What’s going on behind the scenes?
Global Voronoi Planner
Local Potential Field PlannerSlide37
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 NodeSlide38
Local Potential Field (PF) Planner Video
Local Planner works on smaller areas of the playing field
Local Goal given by Global PlannerSlide39
Special
Planner Updates
Notice how Potential Field
is better than Voronoi A* locally
When No uncertainty – Voronoi plan does not need to updateWhen No obstacles – No planners, head heuristically towards goalSlide40
If there is a solution, the algorithm returns it in finite time.
If no solution, the program terminates.
Proof of CompletenessSlide41
Potential Field
Forces
UncertaintyBetter locally than global Voronoi plannerProximityLocal GoalGradient descentIncomplete by itselfCan get stuck in local minimaCan fail to find paths
Proof of CompletenessSlide42
Voronoi DiagramThe start and goal points are Voronoi Vertices
-putting virtual obstacles around these pointsAll Voronoi Vertices are connected-property of Voronoi decompositionStart and goal are connected by Voronoi Edges
Ignoring robot dimensions, there always is a path from start to goal (1 satisfied)
Proof of CompletenessSlide43
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 CompletenessSlide44
Completeness - No paths
No Path – Boxed In
Voronoi planner complete for what it sees
For all visible obstaclesIf a path to the goal exists, find it
If no path exists, returns no pathOverall 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!Slide45
More Videos!
Path-Finding with A*
Avoid Local MinimaSlide46
1000 Obstacles
Edge Pruning
More Obstacles!Slide47
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 simulateSlide48
Short term possibilities
Code has been designed with extension in mind
Varying time-step per plannerLimited field of viewDynamic motionVelocity, momentumPID control systems integration, etc.Motion planner ensembles Raises new possibilities & challengesTasty apart, Delicious together.
Defining completeness (Overall, per planner, per step…)Real-time implementationSimplified models can run real-time for (N<500)
Voronoi planner is the time-sink Solution: Reduce Voronoi complexityLonger time-stepsLess pointsMatlab is good, very good, but not
that good.Do expensive work with C++ subroutines (Mex files)
Future ExtensionsSlide49
Localization & mapping part of SPLAM
Real-time implementation
Reduce Voronoi complexity for faster performanceMATLAB is very good, but not that goodDo expensive work with C++/Mex subroutinesEnhancements to robot
Varying update frequency of each plannerLimited field of view for UAVDynamic motion
Velocity, momentumPID control systems integrationFuture ExtensionsSlide50
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.