Metric Path Planning --Algorithms (Planners)
Graph Based Planners


Finding path between initial node and goal node can
be done using graph search algorithms
However, many graph search algorithms require
visiting each node in graph to determine shortest
path:



Therefore, interest is in “branch and bound” search


Computationally tractable for sparsely connected graph
Computationally expensive for highly connected graph
Prune off paths that aren’t optimal
Classical approach: A*

Frequently used for holonomic robots
Dijkstra’s Algorithm
Evaluation function:
• f(n) = g(n)
• f(n) measures how good
the move to node n is
• g(n) measures cost from
initial node to node n
“A” Search Algorithm

“A” search:



Starts at initial node and generates optimal path
incrementally
Each update selects the best one among all
possible nodes that could be added to the path
Evaluation function:





f(n) = g(n) + h(n)
f(n) measures how good the move to node n is
g(n) measures cost from initial node to node n
h(n) is the cheapest cost of getting from n to goal
Problem: assumes you know h(n) for all nodes
Example of A Search Algorithm
1
F
1
D
E goal
1
1
C
2.24
1
B
1
1
A initial
Example of A Search Algorithm
F
1
1
D
10
1
2
B
E goal
C
1
1
A initial
A* Search Algorithm

Basic idea:





Reduces number of nodes to be explored
No need to explore a path if it cannot be a good
path
Estimates h(n), even if no actual path available
Use this estimate to prune out paths that cannot
be good
Evaluation function: f*(n) = g*(n) + h*(n)



* means these are estimates
g*(n) = g(n) in path planning
How to estimate h(n)?
Estimating h(n)



Must ensure that h*(n) is never greater than
h(n), which means estimation h*(n) is never
greater than the real cost h(n)
Admissibility condition: must never
overestimate remaining cost to reach goal
Easy way to estimate:



Use Euclidian (straight line) distance
Straight line will always be the shortest path
Actual path may be longer, but admissibility
condition still holds
Example of A*
A* Method for Example

The process:




Start expanding node(s) from initial node
Compute f*(n) = g*(n) + h*(n) for all
children
Select the best (defined by f*(n))
unexpended node to expand next time
Process continues until the Goal node has
been selected for expansion
A* Example
F
1
1
E goal
1.4
D
1.4
1
B
1
A initial
h*(A) = 2
h*(B) = 2.24
h*(D) = 1.4
h*(F) = 1
h*(E) = 0
More Examples of A*
Pros and Cons of A* Search

Advantage:


Can be used with any Cspace
representation that can be transformed
into a graph
Limitation:


Hard to use for path planning when there
are factors to consider other than distance
E.g., rocky terrain, sand, etc.
Wavefront-Based Path Planners





Well-suited for grid representations
General idea: consider Cspace to be conductive
material with heat radiating out from initial node to
goal node
If there is a path, heat will eventually reach goal
node
Nice side effect: optimal path from all grid elements
to the goal can be computed
Result: map that looks like a potential field
Example of Wavefront Planning
start
goal
Example of Wavefront Planning
start
goal
Example of Wavefront Planning
start
goal
Example of Wavefront Planning
start
goal
Example of Wavefront Planning
start
goal
Example of Wavefront Planning
start
goal
Example of Wavefront Planning
start
goal
Example of Wavefront Planning
start
goal
Example of Wavefront Planning
start
goal
Example of Wavefront Planning
start
goal
Example of Wavefront Planning
start
goal
Example of Wavefront Planning
start
goal
Wavefront Planning Algorithm

Part I: propagate wave from goal to start






Part II: extract path using gradient descent




Start with binary grid; 0 = free space, 1 = occupied
Label goal cell with “2”
Label all 0-valued grid cells adjacent to the “2” cell with “3”
Label all 0-valued grid cells adjacent to the “3” cells with “4”
Continue until wave front reaches the start cell
Given label of start cell as “x”, find neighboring grid cell labeled
“x-1”; mark this cell as waypoint
Then, find neighboring grid cell labeled “x-2”; mark this cell as a
waypoint
Continue, until reach cell with value “2” (the goal)
Part III: smooth path



Iteratively eliminate waypoint i if path from waypoint i-1 to i+1
does not cross through obstacle
Repeat until no other waypoints can be eliminated
Return waypoints as path for robot to follow
Example: Use Wavefront Planner
Wavefront Propagation Can Handle
Different Terrains

Obstacle: zero conductivity

Open space: infinite conductivity


Undesirable terrains (e.g., rocky areas): low
conductivity, having effect of a high-cost
path
Also: To save processing time, can use dual
wavefront propagation

wave propagate from both start and goal
Example Using Dual Wavefront Propagation
Example Using Dual Wavefront Propagation
Example Using Dual Wavefront Propagation
Example Using Dual Wavefront Propagation
Example Using Dual Wavefront Propagation
Movie for Robot Using Wavefront
DILab@UTK
Path Planning and Reactive Execution


Most path planning algorithms: plan once,
then reactively execute style
Two potential problems with reactive
execution:

Subgoal obsession


Solution: place a tolerance of +/- the width of the robot
Lack of opportunistic replanning

Solution: D*, Wavefront planner
A* Replanner – Unknown Map



Optimal
Inefficient and
impractical in expansive
environments – the
goal is far away from
the start and little map
info exists (Stentz
1994)
How can we do better
in a partially known and
dynamic environment?
Extension: from A* to D*

D*: stands for “Dynamic A* search”




Dynamic: Arc cost can change during problem solving process,
replanning online
D* begins by searching backward from goal, it also maintains an
OPEN list
In D*, the estimated distance, h*, is based on traversability
Then, D* continues replanning, by updating map with newly
sensed information


When handling obstacle, only certain affected nodes are put back
to OPEN list for evaluation
Waves of RAISE and LOWER states touch only portion of search
space
Simulation of D* and D* Lite

Examples:


https://www.youtube.com/watch?v=e_7bSKXHvOI
https://www.youtube.com/watch?v=X5a149nSE9s
D* Applied to Mars Rover “Opportunity”
Disadvantages of Continuous Planning


Computationally expensive for a robot with
an embedded processor and memory
limitations
Highly dependent on sensing quality

Recompute course correction with false positive
Summary of Metric Path Planning

Converts world space to a configuration space

Use obstacle growing to enable representation of robot as a point



Cspace representations exploit interesting geometric properties of the
environment
Representations can be converted to graphs
A* works well with Voronoi diagrams, since they produce sparse
graphs

Wavefront planners work well with regular grids

Metric path planning tends to be computationally expensive

Limitation of popular path planners: assume holonomic robots