Robot Lab:

Robot Path Planning

William Regli
Department of Computer Science
(and Departments of ECE and MEM)
Drexel University

Introduction to Motion Planning

Applications
Overview of the Problem
Basics Planning for Point Robot

Visibility Graphs
Roadmap
Cell Decomposition
Potential Field

Goals
Compute motion strategies, e.g.,
Geometric paths
Time-parameterized trajectories
Sequence of sensor-based motion commands

Achieve high-level goals, e.g.,

Go to the door and do not collide with
obstacles
Assemble/disassemble the engine
Build a map of the hallway
Find and track the target (an intruder, a missing
pet, etc.)

Fundamental Question

Are two given points connected by a path?

Basic Problem
Problem statement:
Compute a collision-free path for a rigid or articulated
moving object among static obstacles.
Input
Geometry of a moving object (a robot, a digital actor, or a
molecule) and obstacles
How does the robot move?
Kinematics of the robot (degrees of freedom)
Initial and goal robot configurations (positions & orientations)

Output
Continuous sequence of collision-free robot
configurations connecting the initial and goal
configurations

Example: Rigid Objects

Example: Articulated Robot

Is it easy?

Hardness Results
Several variants of the path planning problem
have been proven to be PSPACE-hard.
A complete algorithm may take exponential
time.
A complete algorithm finds a path if one exists and
reports no path exists otherwise.

Examples
Planar linkages [Hopcroft et al., 1984]
Multiple rectangles [Hopcroft et al., 1984]

Tool: Configuration Space

Difficulty
Number of degrees of freedom (dimension of
configuration space)
Geometric complexity

Extensions of the Basic Problem

More complex robots

Multiple robots
Movable objects
Nonholonomic & dynamic constraints
Physical models and deformable objects
Sensorless motions (exploiting task
mechanics)
Uncertainty in control

Extensions of the Basic Problem

More complex environments
Moving obstacles
Uncertainty in sensing

More complex objectives

Optimal motion planning

Integration of planning and control
Assembly planning
Sensing the environment
Model building
Target finding, tracking

Practical Algorithms
A complete motion planner always returns a
solution when one exists and indicates
that no such solution exists otherwise.
Most motion planning problems are hard,
meaning that complete planners take
exponential time in the number of degrees
of freedom, moving objects, etc.

Practical Algorithms
Theoretical algorithms strive for completeness and
low worst-case complexity
Difficult to implement
Not robust

Heuristic algorithms strive for efficiency in

commonly encountered situations.
No performance guarantee

Practical algorithms with performance

guarantees
Weaker forms of completeness
Simplifying assumptions on the space: exponential
time algorithms that work in practice

Problem Formulation for

Point
Robot
Input
Robot represented
as a point in the
plane
Obstacles
represented as
polygons
Initial and goal
positions

Output
A collision-free path
between the initial
and goal positions

Framework

Visibility Graph Method

Observation: If there is a
collision-free path
between two points,
then there is a polygonal
path that bends only at
the obstacles vertices.
Why?
Any collision-free path
can be transformed into a
polygonal path that bends
only at the obstacle
vertices.

A polygonal path is a
piecewise linear curve.

Visibility Graph

A visibility graphis a graph such that

Nodes: qinit, qgoal, or an obstacle vertex.
Edges: An edge exists between nodes u and v if
the line segment between u and v is an obstacle
edge or it does not intersect the obstacles.

A Simple Algorithm for Building

Visibility Graphs

Computational Efficiency

Simple algorithm O(n3) time

More efficient algorithms
Rotational sweep O(n2log n) time
Optimal algorithm O(n2) time
Output sensitive algorithms

O(n2) space

Framework

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Other Search Algorithms

Depth-First Search
Best-First Search, A*

Framework

Summary
Discretize the space by constructing
visibility graph
Search the visibility graph with
breadth-first search
Q: How to perform the intersection test?

Summary
Represent the connectivity of the
configuration space in the visibility graph
Running time O(n3)
Compute the visibility graph
Search the graph
An optimal O(n2) time algorithm exists.

Space O(n2)
Can we do better?

Classic Path Planning

Approaches
Roadmap Represent the connectivity of
the free space by a network of 1-D curves

Cell decomposition Decompose the free

space into simple cells and represent the
connectivity of the free space by the
adjacency graph of these cells
Potential field Define a potential function
over the free space that has a global
minimum at the goal and follow the
steepest descent of the potential function

Classic Path Planning

Approaches

Roadmap Represent the connectivity of

the free space by a network of 1-D curves
Cell decomposition Decompose the free
space into simple cells and represent the
connectivity of the free space by the
adjacency graph of these cells
Potential field Define a potential function
over the free space that has a global
minimum at the goal and follow the
steepest descent of the potential function

Roadmap
Visibility graph
Shakey Project, SRI
[Nilsson, 1969]
Voronoi Diagram
Introduced by
computational geometry
researchers. Generate
paths that maximizes
clearance. Applicable
mostly to 2-D
configuration spaces.

Voronoi Diagram
Space O(n)
Run time O(n log n)

Other Roadmap Methods

Silhouette
First complete general method that
applies to spaces of any dimensions
and is singly exponential in the
number of dimensions [Canny 1987]
Probabilistic roadmaps

Classic Path Planning

Approaches
Roadmap Represent the connectivity of
the free space by a network of 1-D curves

Cell decomposition Decompose the free

space into simple cells and represent the
connectivity of the free space by the
adjacency graph of these cells
Potential field Define a potential function
over the free space that has a global
minimum at the goal and follow the
steepest descent of the potential function

Cell-decomposition Methods
Exact cell decomposition
The free space F is represented by a
collection of non-overlapping simple
cells whose union is exactly F
Examples of cells: trapezoids,
triangles

Trapezoidal Decomposition

Computational Efficiency
Running time O(n log n) by planar
sweep
Space O(n)
Mostly for 2-D configuration spaces

Adjacency Graph

Nodes: cells
Edges: There is an edge between every pair of
nodes whose corresponding cells are adjacent.

Summary
Discretize the space by constructing
an adjacency graph of the cells
Search the adjacency graph

Cell-decomposition Methods
Exact cell decomposition
Approximate cell decomposition
F is represented by a collection of nonoverlapping cells whose union is contained
in F.
Cells usually have simple, regular shapes,
e.g., rectangles, squares.
Facilitate hierarchical space decomposition

Quadtree Decomposition

Octree Decomposition

Algorithm Outline

Classic Path Planning

Approaches
Roadmap Represent the connectivity of
the free space by a network of 1-D curves

Cell decomposition Decompose the free

space into simple cells and represent the
connectivity of the free space by the
adjacency graph of these cells

Potential field Define a potential function

over the free space that has a global
minimum at the goal and follow the
steepest descent of the potential function

Potential Fields
Initially proposed for real-time collision avoidance
[Khatib 1986]. Hundreds of papers published.
A potential field is a scalar function over the free
space.
To navigate, the robot applies a force proportional
to the negated gradient of the potential field.
A navigation function is an ideal potential field that

has global minimum at the goal

has no local minima
grows to infinity near obstacles
is smooth

Attractive & Repulsive Fields

How Does It Work?

Algorithm Outline
Place a regular grid G over the
configuration space
Compute the potential field over G
Search G using a best-first algorithm
with potential field as the heuristic
function

Local Minima
What can we do?
Escape from local minima by taking
random walks
Build an ideal potential field
navigation function that does not have
local minima

Question
Can such an ideal potential field be
constructed efficiently in general?

Completeness
A complete motion planner always
returns a solution when one exists and
indicates that no such solution exists
otherwise.
Is the visibility graph algorithm complete?
Yes.
How about the exact cell decomposition
algorithm and the potential field
algorithm?

Why Complete Motion Planning?

Probabilistic roadmap
motion planning

Complete motion
planning

Efficient
Work for complex problems
with many DOF

Always terminate

Difficult for narrow

passages
May not terminate when no
path exists

Not efficient
Not robust even for
low DOF

Path Non-existence Problem

Robot

Initi
al

Obstacle
Obstacle

Goal

Main
Challenge
Exponential complexity: nDOF
Degree of freedom: DOF
Geometric complexity: n

More difficult than finding a path

To check all possible paths

Robot

Initia
l

Obstacle
Goal

Approaches

Exact Motion Planning

Based on exact representation of free space

Approximation Cell Decomposition (ACD)

A Hybrid planner

Configuration Space: 2D Translation

Workspace

Configuration Space

Goal
Obstacle
Robot
Start

C-obstacle
Free
y
x

Configuration Space Computation

[Varadhan et al, ICRA 2006]
2 Translation + 1 Rotation
215 seconds

Obstacle

y
x

Robot

Exact Motion Planning

Approaches

Exact cell decomposition [Schwartz et al. 83]

Roadmap [Canny 88]
Criticality based method [Latombe 99]
Voronoi Diagram
Star-shaped roadmap [Varadhan et al. 06]

Not practical
Due to free space computation
Limit for special and simple objects
Ladders, sphere, convex shapes
3DOF

Approaches

Exact Motion Planning

Based on exact representation of free space

Approximation Cell Decomposition (ACD)

A Hybrid Planner Combing ACD and PRM

Approximation Cell Decomposition (ACD)

Not compute the free space exactly at once
But compute it incrementally
Relatively easy to implement

[Lozano-Prez 83]
[Zhu et al. 91]
[Latombe 91]
[Zhang et al. 06]

Approximation Cell Decomposition

Full cell

Configuration Space
full
mixed

Empty cell
Mixed cell
Mixed
Uncertain

empty

Connectivity Graph

Gf : Free Connectivity Graph

G: Connectivity Graph

Gf is a subgraph of G

Finding a Path by ACD

Gf : Free Connectivity Graph

Initial

Goal

Finding a Path by ACD

First Graph Cut Algorithm

Guiding path in connectivity

graph G
Only subdivide along this
path
Update the graphs G and Gf

Described in Latombes book

L: Guiding Path

First Graph Cut Algorithm

L

Only subdivide along L

Finding a Path by ACD

ACD for Path Non-existence

Initial

Goal
C-space

ACD for Path Non-existence

Connectivity Graph

Guiding Path

ACD for Path Non-existence

Connectivity
graph is not
connected
No
path!

Sufficient condition for deciding path non-existence

Two-gear Example
Video

no path! 3.356s
Initi
al

Cells
in Cobstacle

Roadmap
in F

Goal

Cell Labeling
Free Cell Query
Whether a cell
completely lies in free
space?

C-obstacle Cell Query

Whether a cell
completely lies in Cobstacle?

empty

full

mixed

Free Cell Query

A Collision Detection Problem
Does the cell lie inside
free space?

Do

robot and obstacle

separate at all configurations?

Robot
Obstacle

?
Configuration space

Workspace

Clearance
Separation distance
A well studied geometric problem

d
Determine a volume in C-space which
are completely free

C-obstacle Query
Another Collision Detection Problem
Does the cell lie inside
C-obstacle?

Do

robot and obstacle

intersect at all configurations?
Robot

?
Obstacle

Configuration space

Workspace

Forbiddance
Forbiddance: dual to clearance
Penetration Depth
A geometric computation problem less investigated

PD
[Zhang et al. ACM SPM 2006]

Limitation of ACD
Combinatorial complexity of cell
decomposition
Limited for low DOF problem
3-DOF robots

Approaches

Exact Motion Planning

Based on exact representation of free space

Approximation Cell Decomposition (ACD)

A Hybrid Planner Combing ACD and PRM

Hybrid Planning
Complete Motion Planning

Probabilistic roadmap
motion planning
+ Efficient
+ Many DOFs

+ Complete

- Not efficient

- Narrow passages
- Path non-existence

Can we combine them together?

Hybrid Approach for Complete

Motion
Planning
Use Probabilistic
Roadmap
(PRM):
Capture the connectivity for
mixed cells
Avoid substantial subdivision

Use Approximation Cell

Decomposition (ACD)
Completeness
Improve the sampling on
narrow passages

Connectivity Graph

Gf : Free Connectivity Graph

G: Connectivity Graph

Gf is a subgraph of G

Pseudo-free
edges
Initial

Goal

Pseudo free edge for

two adjacent cells

Pseudo-free Connectivity Graph:

Gsf
Gsf = Gf + Pseudo-edges

Initial

Goal

Algorithm
Gf
Gsf
G

Results of Hybrid Planning

Results of Hybrid Planning

Results of Hybrid Planning

2.5 - 10 times speedup
3 DOF

4 DOF

4 DOF

timing

cells #

timing

cells #

timing

cells #

Hybrid

34s

50K

16s

48K

102s

164K

ACD

85s

168K

Speedup

2.5

3.3

10

10

Summary

Difficult for Exact Motion Planning

Due to the difficulty of free space configuration
computation

ACD is more practical

Explore the free space incrementally

Hybrid Planning
Combine the completeness of ACD and efficiency
of PRM