8/14/2019 Lecture 9 Navigation Part1
1/38
Planning and NavigationWhere am I going? How do I get there?
?
"Position"Global Map
Perception Motion Control
Cognition
Real WorldEnvironment
Localization
PathEnvironment ModelLocal Map
8/14/2019 Lecture 9 Navigation Part1
2/38
Competencies for Navigation
Cognition / Reasoning :
is the ability to decide what actions are required to achieve a certain
goal in a given situation (belief state). decisions ranging from what path to take to whatinformation on the
environment to use.
Todays industrial robots can operate without any cognition
(reasoning) because their environment is static and very structured.
In mobile robotics, cognition and reasoning is primarily of geometric
nature, such as picking safe path or determining where to go next.
already been largely explored in literature for cases in which complete
information about the current situation and the environment exists (e.g.
sales man problem).
8/14/2019 Lecture 9 Navigation Part1
3/38
Competencies for Navigation
However, in mobile robotics the knowledge of about the environment
and situation is usually only partially known and is uncertain.
makes the task much more difficult
requires multiple tasks running in parallel, some forplanning (global),
some to guarantee survival of the robot.
Robot control can usually be decomposed in various behaviors orfunctions
e.g. wall following, localization, path generation or obstacle avoidance.
In this chapter we are concerned with path planning and navigation,except the low lever motion control and localization.
We can generally distinguish between (global) path planning and
(local) obstacle avoidance.
8/14/2019 Lecture 9 Navigation Part1
4/38
Global Path Planing
Assumption: there exists a good enough map of the environment for
navigation.
Topological or metric or a mixture between both.
First step:
Representation of the environment by a road-map (graph), cells or a
potential field. The resulting discrete locations or cells allow them to usestandard planning algorithms.
Examples:
Visibility Graph
Voronoi Diagram
Cell Decomposition -> Connectivity Graph
Potential Field
8/14/2019 Lecture 9 Navigation Part1
5/38
Path Planning: Configuration Space
State or configuration q can be described with kvalues qi
What is the configuration space of a mobile robot?
8/14/2019 Lecture 9 Navigation Part1
6/38
Path Planning Overview
1. Road Map, Graph construction
Identify a set of routes within the free
space
Where to put the nodes?
Topology-based:
at distinctive locations
Metric-based:
where features disappear or get visible
2. Cell decomposition
Discriminate between free and
occupied cells
Where to put the cell boundaries? Topology- and metric-based:
where features disappear or get visible
3. Potential Field
Imposing a mathematical function over
the space
8/14/2019 Lecture 9 Navigation Part1
7/38
Road-Map Path Planning: Visibility Graph
Join all pairs of vertices visible
to each other
Advantages Shortest path length
Simple to implement if
environment can berepresented by polygons
Disadvantages
Slow in densely populated
environments
Too close to obstacles
o Method: Grow obstacles to
avoid collisions
8/14/2019 Lecture 9 Navigation Part1
8/38
Road-Map Path Planning: Voronoi Diagram
Maximize the distance between
the robot and obstacles
Advantages:Easy executable: Maximize the
sensor readings, mitigate
encoder inaccuracy
Works also for map-building:
Move on the Voronoi edges
Disadvantages:
May fail in the case of limited
range localization sensors
8/14/2019 Lecture 9 Navigation Part1
9/38
Road-Map Path Planning: Cell Decomposition
Basic idea: to discriminate between free cells and occupied cells
Procedure:
Divide space into simple, connected regions calledcellsDetermine which open sells are adjacent and construct a connectivity
graph
Find cells in which the initial and goal configuration (state) lie andsearch for a path in the connectivity graph to join them.
From the sequence of cells found with an appropriate search algorithm,
compute a path within each cell.
o e.g. passing through the midpoints of cell boundaries or by sequence of
wall following movements.
8/14/2019 Lecture 9 Navigation Part1
10/38
Road-Map Path Planning: Exact Cell Decomposition
The boundary of cells
is based on geometric
criticality
The particularposition of the robot
within each cell of
free space does not
matter
Slow in dense and
complex environment
8/14/2019 Lecture 9 Navigation Part1
11/38
Road-Map Path Planning: Approximate Cell Decomposition
Narrow passageways mayNarrow passageways may
be lost due to the inexactbe lost due to the inexact
nature of the tessellationnature of the tessellation
8/14/2019 Lecture 9 Navigation Part1
12/38
Road-Map Path Planning: Adaptive Cell Decomposition
Fixed cell decomposition
The search is linear in the
number of cells only
Fundamental cost is memory
Adaptive cell decomposition
The free space is externally
bounded by a rectangle andinternally bounded by three
polygons.
The rectangle is recursively
decomposed into smallerrectangles
Adapt to the complexity of
environment
8/14/2019 Lecture 9 Navigation Part1
13/38
Path Planning Algorithms
For Path planning
A* for relational graphsWavefront for operating directly on regular grids
For Interleaving Path Planning and Execution
8/14/2019 Lecture 9 Navigation Part1
14/38
Motivation for A*
Single Source Shortest Path algorithms are exhaustive, visiting all
edges
Cant we throw away paths when we see that they arent going to thegoal, rather than follow all branches?
This means having a mechanism to prune branches as we go, rather
than after full exploration
Algorithms which prune earlier (but correctly) are preferred overalgorithms which do it later.
Issue -> the mechanism for pruning
8/14/2019 Lecture 9 Navigation Part1
15/38
A* Algorithm
Similar to breadth-first: at each point in the time the planner can onlysee its node and 1 set of nodes in front
The idea is to rate the choices, choose the best one first, throw awayany choices whenever you can
f*(n) is the goodness of the path from Start to n
g*(n) is the cost of going from the Start to node n
h*(n) is the cost of going from n to the Goal
H is for heuristic function because we must have a way of guessingthe cost of n to Goal since we cant see the path between n and the Goal
f*(n)=g*(n)+h*(n)
8/14/2019 Lecture 9 Navigation Part1
16/38
A* Heuristic Function
G*(n) is easy: just sum up the path costs to n
h*(n) is tricky
But path planning requires an a priori map
Metric path planning requires a METRIC a priori map
Therefore, know the distance between Initial and Goal nodes, just not
the optimal way to get there
h*(n)= distance between n and Goal
f*(n)=g*(n)+h*(n)
8/14/2019 Lecture 9 Navigation Part1
17/38
Example: A to E
But since youre starting at A and can only look 1 node ahead, this is
what you see:
AB
D
F E
1
1
1
1
1.4
1.4
AB
D
E
1
1.4
8/14/2019 Lecture 9 Navigation Part1
18/38
Two choices for n: B, D
Do both
f*(B)=1+2.24=3.24
f*(D)=1.4+1.4=2.8
Cant prune, so much keep going (recurse)
Pick the most plausible path first => A-D-?-E
AB
D
E
1
1.4
1.4
2.24
8/14/2019 Lecture 9 Navigation Part1
19/38
A-D-?-E
stand on D
Can see 2 new nodes: F, E
f*(F)=(1.4+1)+1=3.4f*(E)=(1.4+1.4)+0=2.8
Three paths A-B-?-E >= 3.24
A-D-E = 2.8
A-D-F-?-D >=3.4
A-D-E is the winner!
Dont have to look farther because expanded the shortest first, otherscouldnt possibly do better without having negative distances,
violations of laws of geometry
AB
D
E
1
1.4
1.4
F1
1
8/14/2019 Lecture 9 Navigation Part1
20/38
Class Exercise
Find the best path from A to E
D
F E
1
1
1
1
1.4
1.4
AB
C
Notes:Notes:A* is hard to use for path planning where there are factors otheA* is hard to use for path planning where there are factors other thanr than
distance to consider in generating the path, such as terrain condistance to consider in generating the path, such as terrain conditionsditions
8/14/2019 Lecture 9 Navigation Part1
21/38
Wavefront Planners
Well suited for grid map
Consider the Cspace to be a conductive material with heat radiating out
from the initial node to the goal node.Eventually the heat will spread and reach the goal if there is a way
Can combine terrain types into theCan combine terrain types into the
planning by assigning differentplanning by assigning different
levels of conductivity to the cellslevels of conductivity to the cells
8/14/2019 Lecture 9 Navigation Part1
22/38
Trulla Algorithm
obstacleobstacle
UndesirableUndesirable
terrainterrain
OpenOpen
areaarea
8/14/2019 Lecture 9 Navigation Part1
23/38
Interleaving Path Planning and Reactive Execution
Graph-based planners generate apath and sub-paths or sub-segments
What happens if blocked? What happens if avoid an obstacle andactually is now closer to the next sub-goal?
Causes Sub-goal obsession
When does the robot think it has reached (x,y)? What about encodererror?
Need a Termination condition, usually +/- width of robot
8/14/2019 Lecture 9 Navigation Part1
24/38
Trulla Example
8/14/2019 Lecture 9 Navigation Part1
25/38
Two Example Approaches
If compute all possible paths in advance, not a problem
D* ( by Tony Stentz)
Run A* over all possible pairs of nodes Solution to sub-goal obsession: continuously update the map and
dynamically repair the A* paths affected by the changes in the map ->
continuously replanning
Event-driven scheme to trigger replanning
Trulla (by Ken Hughes)
By-product of wave propagation style is path to everywhere
Using dot-product of the path vector and the actual vector as an
affordance to trigger replanning
8/14/2019 Lecture 9 Navigation Part1
26/38
Two Example Approaches (continued)
Both algorithms dont handle the situation where the real world is
actually friendlier, which means an obstacle thought to be there really
isnt.
The robot could achieve a significant savings in navigation by
opportunistically going through the gap
6.2.1
8/14/2019 Lecture 9 Navigation Part1
27/38
Potential Field Path Planning
Robot is treated as apoint under the
influence of an artificial potential field.
Generated robot movement is similar toa ball rolling down the hill
Goal generates attractive force
Obstacle are repulsive forces
6.2.1
8/14/2019 Lecture 9 Navigation Part1
28/38
Potential Field Path Planning: Potential Field Generation
Generation of potential field function U(q)
attracting (goal) and repulsing (obstacle) fields
summing up the fields
functions must be differentiable
Generate artificial force field F(q)
Set robot speed (vx, vy) proportional to the force F(q) generated by thefield
the force field drives the robot to the goal
if robot is assumed to be a point mass
===
y
Ux
U
qUqUqUqF repatt )()()()(
6.2.1
8/14/2019 Lecture 9 Navigation Part1
29/38
Potential Field Path Planning: Repulsing Potential Field
Should generate a barrier around all the obstacle
strong if close to the obstacle
not influence if fare from the obstacle
Field is positive or zero andtends to infinity as q gets closer to the object
8/14/2019 Lecture 9 Navigation Part1
30/38
Path Planning for Cye Mobile Robot (Batavia and Nourbakhsh)
Cye is a 2-wheeled differential drive robot
Primary mode of navigation is dead-reckoning
Challenges The robot must maintain adequate
distance from alls and other obstacles
There is uncertainty associated with the
creation of the map, and there can bedynamic obstacles
The robot should ideally use exploredareas, but should be willing to traverse
unexplored areas if doing so results in asignificant savings in path length
Path generation time must be short
The use of check points must be
incorporated into the path planner tominimize ded-reckoning error
8/14/2019 Lecture 9 Navigation Part1
31/38
Path Planning for Cye Mobile Robot (Batavia and Nourbakhsh)
Path planner
Grid based potential field
Assume that the robot is a holonomic platform
The field is constructed in two stages
o First stage: each cell contains a value denoting the traversability of
that point
distance to the nearest obstacle and terrain type
o Second stage: a potential field is created in which each world pointis the distance to the goal, nonlinearly weighted by the corresponding
value in the traversability grid
wavefront transformation
This field is used to create a path from start to goalwhich minimizes the path length plus the cost of
traversability
Check points to correct the localization errors
8/14/2019 Lecture 9 Navigation Part1
32/38
Path Planning for Cye Mobile Robot (Batavia and Nourbakhsh)
Avoiding local minima
Using unexplored areas to shorten path length
8/14/2019 Lecture 9 Navigation Part1
33/38
Exploration
How to cover an unknown area efficiently?
Random search
Avoid-past behavior combined with the random potential field
(visited cell as a repulsive field)
Move-to-unknown-area behavior (use the centroid of unknown area as agoal)
The above behavior-oriented approaches are simple and easy toimplement, however, they are inefficient when presented with two ormore unexplored areas
Can explore reactively (move to open area), but wed like to create
maps Two major methods
Frontier-based
Generalized Voronoi Graph (GVG)
8/14/2019 Lecture 9 Navigation Part1
34/38
Frontier Based Exploration
Robot senses environment
Borders of low certainty form
frontiers
Rate the frontiers
Centroid
Utility of exploring (big?Close?)
Move robot to the centroid and
repeat (continuously localize and map
as you go)
Open spaceOpen space
Occupied spaceOccupied space
FrontierFrontier
RobotRobot
currentcurrent
positionposition
8/14/2019 Lecture 9 Navigation Part1
35/38
GVG (Generalized Voronoi Graph)
Essentially, the robot tries to move ahead but stay in the middlEssentially, the robot tries to move ahead but stay in the middle or at ae or at a
tangent to surrounding objects. This path is a GVG edge.tangent to surrounding objects. This path is a GVG edge.
dead enddead end
If encounter branches inIf encounter branches inthe GVG edges, choosethe GVG edges, choose
one at random to followone at random to follow
Starting pointStarting point
8/14/2019 Lecture 9 Navigation Part1
36/38
Reaches deadend at 9, backtracks
8/14/2019 Lecture 9 Navigation Part1
37/38
Goes back and catches missing areas
8/14/2019 Lecture 9 Navigation Part1
38/38
Discussion of Exploration
Both methods work OK indoors, not so clear on utility outdoors
GVG
Susceptible to noise, hard to recover nodes
Frontier
Have to rate the frontiers so dont trash
Top Related