Planning and Navigation

Where am I going? How do I get there?

Localization "Position" Cognition

Global Map

Environment Model Path

Local Map

Perception Real World Motion Control

Environment
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 what information on the
environment to use.
• Today’s 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).
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 for planning (global),
some to guarantee “survival of the robot”.
• Robot control can usually be decomposed in various behaviors or
functions
¾ 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.
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 use
standard planning algorithms.
• Examples:
¾ Visibility Graph
¾ Voronoi Diagram
¾ Cell Decomposition -> Connectivity Graph
¾ Potential Field
Path Planning: Configuration Space
• State or configuration q can be described with k values qi

• What is the configuration space of a mobile robot?

Path Planning Overview
1. Road Map, Graph construction 2. Cell decomposition
¾ Identify a set of routes within the free ¾ Discriminate between free and
space occupied cells

• Where to put the nodes? • Where to put the cell boundaries?

• Topology-based: • Topology- and metric-based:
¾ at distinctive locations ¾ where features disappear or get visible
• Metric-based: 3. Potential Field
¾ where features disappear or get visible ¾ Imposing a mathematical function over
the space
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 be
represented by polygons
• Disadvantages
¾ Slow in densely populated
environments
¾ Too close to obstacles
o Method: Grow obstacles to
avoid collisions
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
Road-Map Path Planning: Cell Decomposition
• Basic idea: to discriminate between free cells and occupied cells
• Procedure:
¾ Divide space into simple, connected regions called cells
¾ Determine which open sells are adjacent and construct a connectivity
graph
¾ Find cells in which the initial and goal configuration (state) lie and
search 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.
Road-Map Path Planning: Exact Cell Decomposition
• The boundary of cells
is based on geometric
criticality
• The particular
position of the robot
within each cell of
free space does not
matter
• Slow in dense and
complex environment
Road-Map Path Planning: Approximate Cell Decomposition
Narrow passageways may
be lost due to the inexact
nature of the tessellation
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 and
internally bounded by three
polygons.
¾ The rectangle is recursively
decomposed into smaller
rectangles
¾ Adapt to the complexity of
environment
Path Planning Algorithms

• For Path planning

¾ A* for relational graphs
¾ Wavefront for operating directly on regular grids

• For Interleaving Path Planning and Execution

Motivation for A*
• Single Source Shortest Path algorithms are exhaustive, visiting all
edges
¾ Can’t we throw away paths when we see that they aren’t going to the
goal, 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 over

algorithms which do it later.

• Issue -> the mechanism for pruning

A* Algorithm
• Similar to breadth-first: at each point in the time the planner can only
“see” it’s node and 1 set of nodes “in front”
• The idea is to rate the choices, choose the best one first, throw away
any choices whenever you can

f*(n)=g*(n)+h*(n)

• 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 guessing
the cost of n to Goal since we can’t see the path between n and the Goal
A* Heuristic Function

f*(n)=g*(n)+h*(n)

• 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
Example: A to E
1
F E
1
1.4
D 1.4
1

1
B A
• But since you’re starting at A and can only look 1 node ahead, this is
what you see: E

D 1.4

1
B A
 E
1.4

2.24
D
1.4

1
B A
• Two choices for n: B, D
• Do both
¾ f*(B)=1+2.24=3.24
¾ f*(D)=1.4+1.4=2.8
• Can’t prune, so much keep going (recurse)
¾ Pick the most plausible path first => A-D-?-E
 1
• A-D-?-E F E
¾ “stand on D” 1
1.4
¾ Can see 2 new nodes: F, E
D
¾ f*(F)=(1.4+1)+1=3.4 1.4
¾ f*(E)=(1.4+1.4)+0=2.8
1
• Three paths B A
¾ A-B-?-E >= 3.24
¾ A-D-E = 2.8
¾ A-D-F-?-D >=3.4

• A-D-E is the winner!

¾ Don’t have to look farther because expanded the shortest first, others
couldn’t possibly do better without having negative distances,
violations of laws of geometry…
Class Exercise
• Find the best path from A to E
1
F E
1
1.4
D C
1.4
1

1
B A

Notes:
A* is hard to use for path planning where there are factors other than
distance to consider in generating the path, such as terrain conditions
 Wavefront Planners

• Wellsuited 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 the

planning by assigning different
levels of conductivity to the cells
 Trulla Algorithm

Undesirable
terrain

Open
area

obstacle
Interleaving Path Planning and Reactive Execution
• Graph-based planners generate a path and sub-paths or sub-segments

• What happens if blocked? What happens if avoid an obstacle and

actually is now closer to the next sub-goal?
¾ Causes Sub-goal obsession

• When does the robot think it has reached (x,y)? What about encoder
error?
¾ Need a Termination condition, usually +/- width of robot
Trulla Example
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
Two Example Approaches (continued)
• Both algorithms don’t handle the situation where the real world is
actually friendlier, which means an obstacle thought to be there really
isn’t.
• The robot could achieve a significant savings in navigation by
opportunistically going through the gap
 6.2.1
Potential Field Path Planning

• Robot is treated as a point under the

influence of an artificial potential field.
¾ Generated robot movement is similar to
a ball rolling down the hill
¾ Goal generates attractive force
¾ Obstacle are repulsive forces
 6.2.1
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) ⎡ ∂U ⎤
⎢ ∂x ⎥
F (q ) = −∇U (q ) = −∇U att (q ) − ∇U rep (q ) = ⎢ ∂U ⎥
⎢ ⎥
⎢⎣ ∂y ⎥⎦
• Set robot speed (vx, vy) proportional to the force F(q) generated by the
field
¾ the force field drives the robot to the goal
¾ if robot is assumed to be a point mass
 6.2.1
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 and tends to infinity as q gets closer to the object
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 be
dynamic obstacles
¾ The robot should ideally use explored
areas, but should be willing to traverse
unexplored areas if doing so results in a
significant savings in path length
¾ Path generation time must be short
¾ The use of check points must be
incorporated into the path planner to
minimize ded-reckoning error
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 point
is 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 goal
which minimizes the path length plus the cost of
traversability
¾ Check points to correct the localization errors
Path Planning for Cye Mobile Robot (Batavia and Nourbakhsh)

Avoiding local minima

Using unexplored areas to shorten path length

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 a
goal)
¾ The above behavior-oriented approaches are simple and easy to
implement, however, they are inefficient when presented with two or
more unexplored areas
• Can explore reactively (move to open area), but we’d like to create
maps
• Two major methods
¾ Frontier-based
¾ Generalized Voronoi Graph (GVG)
Frontier Based Exploration Occupied space

• Robot senses environment

• Borders of low certainty form Open space

frontiers
Robot
• Rate the frontiers current
¾ Centroid position

¾ Utility of exploring (big?

Close?)
• Move robot to the centroid and Frontier

repeat
• (continuously localize and map
as you go)
 GVG (Generalized Voronoi Graph)
Essentially, the robot tries to move ahead but stay in the middle or at a
tangent to surrounding objects. This path is a GVG edge.

If encounter branches in
the GVG edges, choose
one at random to follow

dead end

Starting point
Reaches deadend at 9, backtracks
Goes back and catches missing areas
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 don’t trash