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

Introduction

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

Introduction

X

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

Introduction

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

Introduction

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

Introduction

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

Introduction

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

Introduction

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

Introduction

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

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

Pro

babili

ty

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