Slide 1

Robot Lab:
Robot Path Planning
William Regli
Department of Computer Science
(and Departments of ECE and MEM)
Drexel University
Slide 2
Introduction to Motion Planning


Applications
Overview of the Problem
Basics Planning for Point Robot
Visibility Graphs
Roadmap
Cell Decomposition
Potential Field


Slide 3
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.)

Slide 4
Fundamental Question
Are two given points connected by a path?
Slide 5
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

Slide 6
Example: Rigid Objects
Slide 7
Example: Articulated Robot
Slide 8
Is it easy?
Slide 9

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]

Slide 10

Tool: Configuration Space






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

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

Slide 12

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
Slide 13

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.

Slide 14

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
Slide 15
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

Slide 16
Framework
Slide 17
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.

Slide 18
Visibility Graph






A visibility graphis a graph such that
Nodes: q
init
, q
goal
, 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.
Slide 19
A Simple Algorithm for Building
Visibility Graphs
Slide 20
Computational Efficiency
Simple algorithm O(n
3
) time
More efficient algorithms
Rotational sweep O(n
2
log n) time
Optimal algorithm O(n
2
) time
Output sensitive algorithms
O(n
2
) space

Slide 21
Framework
Slide 22
Breadth-First Search
Slide 23
Breadth-First Search
Slide 24
Breadth-First Search
Slide 25
Breadth-First Search
Slide 26
Breadth-First Search
Slide 27
Breadth-First Search
Slide 28
Breadth-First Search
Slide 29
Breadth-First Search
Slide 30
Breadth-First Search
Slide 31
Breadth-First Search
Slide 32
Other Search Algorithms
Depth-First Search
Best-First Search, A*
Slide 33
Framework
Slide 34
Summary
Discretize the space by constructing
visibility graph
Search the visibility graph with
breadth-first search

Q: How to perform the intersection test?


Slide 35
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?

Slide 36

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
Slide 37

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
Slide 38
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.

Slide 39
Voronoi Diagram

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

Slide 40
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

Slide 41

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
Slide 42

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

Slide 43

Trapezoidal Decomposition

Slide 44

Computational Efficiency

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

Slide 45
Adjacency Graph
Nodes: cells
Edges: There is an edge between every pair
of nodes whose corresponding cells are
adjacent.
Slide 46
Summary
Discretize the space by constructing
an adjacency graph of the cells
Search the adjacency graph

Slide 47

Cell-decomposition Methods

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

Slide 48

Quadtree Decomposition

Slide 49
Octree Decomposition
Slide 50
Algorithm Outline
Slide 51

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
Slide 52
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

Slide 53

Attractive & Repulsive Fields

Slide 54
How Does It Work?
Slide 55
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

Slide 56
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

Slide 57
Question

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

Slide 58
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?
Slide 59
Why Complete Motion Planning?
Probabilistic roadmap
motion planning
Efficient
Work for complex problems
with many DOF

Difficult for narrow
passages
May not terminate when no
path exists
Complete motion
planning
Always terminate



Not efficient
Not robust even for
low DOF
Slide 60
Path Non-existence Problem
Obstacle
Obstacle
Goal
Initial
Robot
Slide 61
Main Challenge
Obstacle
Goal
Initial
Robot
Exponential complexity: n
DOF
Degree of freedom: DOF
Geometric complexity: n
More difficult than finding a path
To check all possible paths

Slide 62
Approaches
Exact Motion Planning
Based on exact representation of free
space

Approximation Cell Decomposition
(ACD)

A Hybrid planner
Slide 63
Configuration Space: 2D Translation
Workspace
Configuration Space
x
y
Robot
Start
Goal
Free
Obstacle
C-obstacle
Slide 64
Configuration Space Computation
[Varadhan et al, ICRA 2006]
2 Translation + 1 Rotation
215 seconds
Obstacle
Robot
x

y
Slide 65
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
Slide 66
Approaches
Exact Motion Planning
Based on exact representation of free
space

Approximation Cell Decomposition
(ACD)

A Hybrid Planner Combing ACD and
PRM
Slide 67
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]
Slide 68
full

mixed

empty

Approximation Cell Decomposition
Full cell

Empty cell

Mixed cell
Mixed
Uncertain

Configuration Space
Slide 69
Connectivity Graph
G
f
: Free Connectivity Graph G: Connectivity Graph
G
f
is a subgraph of G
Slide 70
Finding a Path by ACD
Goal
Initial
G
f
: Free Connectivity Graph
Slide 71
Finding a Path by ACD
L: Guiding Path
First Graph Cut Algorithm
Guiding path in connectivity
graph G

Only subdivide along this
path

Update the graphs G and G
f

Described in Latombes book
Slide 72
First Graph Cut Algorithm
Only subdivide along L
L
Slide 73
Finding a Path by ACD
Slide 74
ACD for Path Non-existence
C-space
Goal
Initial
Slide 75
Connectivity Graph
Guiding Path
ACD for Path Non-existence
Slide 76
ACD for Path Non-existence
Connectivity graph
is not connected
No path!
Sufficient condition for deciding path non-existence
Slide 77
Two-gear Example
no path!

Cells
in C-obstacle
Initial
Goal
Roadmap
in F
Vide
o
3.356s
Slide 78
Cell Labeling
Free Cell Query
Whether a cell
completely lies in
free space?

C-obstacle Cell
Query
Whether a cell
completely lies in
C-obstacle?
full

mixed

empty

Slide 79
Free Cell Query
A Collision Detection Problem
Does the cell lie inside
free space?
Do robot and obstacle
separate at all configurations?

Obstacle
Workspace
Configuration space
?
Robot
Slide 80
Clearance
Separation distance
A well studied geometric problem





Determine a volume in C-space which
are completely free
d
Slide 81
C-obstacle Query
Another Collision Detection Problem
Does the cell lie inside
C-obstacle?
Do robot and obstacle
intersect at all configurations?

Obstacle
Workspace
Configuration space
?
Robot
Slide 82
Forbiddance


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





[Zhang et al. ACM SPM 2006]
PD

Slide 83
Limitation of ACD
Combinatorial complexity of cell
decomposition
Limited for low DOF problem
3-DOF robots
Slide 84
Approaches
Exact Motion Planning
Based on exact representation of free
space

Approximation Cell Decomposition
(ACD)

A Hybrid Planner Combing ACD and
PRM
Slide 85
Hybrid Planning
Probabilistic roadmap
motion planning
+ Efficient
+ Many DOFs

- Narrow passages
- Path non-existence
Complete Motion
Planning
+ Complete


- Not efficient
Can we combine them together?
Slide 86
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
Slide 87
Connectivity Graph
G
f
: Free Connectivity Graph G: Connectivity Graph
G
f
is a subgraph of G
Slide 88
Pseudo-free edges




Pseudo free edge for
two adjacent cells
Goal
Initial
Slide 89
Pseudo-free Connectivity Graph:
G
sf
Goal
Initial
Gsf = Gf + Pseudo-edges
Slide 90
Algorithm
G
f
G
sf
G



Slide 91
Results of Hybrid Planning
Slide 92
Results of Hybrid Planning
Slide 93
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 ?
Slide 94
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