CS 326 A: Motion Planning
http://robotics.stanford.edu/~latombe/cs326/2002
Configuration Space
Basic Path-Planning Methods
What is a Path?
Tool: Configuration Space
(C-Space C)
Configuration Space
qn
q=(q1,,qn)
q1
q2
q3
Definition
A robot configuration is a specification
of the positions of all robot points
relative to a fixed coordinate system
Usually a configuration is expressed as
a vector of position/orientation
parameters
Rigid Robot Example
workspace
robot
reference direction
reference point
3-parameter representation: q = (x,y,)
In a 3-D workspace q would be of the form
(x,y,z,)
Articulated Robot Example
q = (q1,q2,,q10)
q2
q1
Configuration Space of a Robot
Space of all its possible configurations
But the topology of this space is usually
not that of a Cartesian space
C = S1 x S1
Configuration Space of a Robot
Space of all its possible configurations
But the topology of this space is usually
not that of a Cartesian space
C = S1 x S1
Configuration Space of a Robot
Space of all its possible configurations
But the topology of this space is usually
not that of a Cartesian space
C = S1 x S1
Structure of Configuration Space
It is a manifold
For each point q, there is a 1-to-1 map
between a neighborhood of q and a
Cartesian space Rn, where n is the
dimension of C
This map is a local coordinate system
called a chart.
C can always be covered by a finite number
of charts. Such a set is called an atlas
Example
Case of a Planar Rigid Robot
workspace
robot
reference direction
reference point
3-parameter representation: q = (x,y,) with
[0,2). Two charts are needed
Other representation: q = (x,y,cos,sin)
c-space is a 3-D cylinder R2 x S1
embedded in a 4-D space
Rigid Robot in 3-D Workspace
q = (x,y,z,)
The c-space is a 6-D space (manifold) embedded
in a 12-D Cartesian space. It is denoted by R3xSO(3)
Other representation: q = (x,y,z,r11,r12,,r33) where
r11, r12, , r33 are the elements of rotation matrix
R:
r11 r12 r13
r21 r22 r23
r31 r32 r33
with:
ri12+ri22+ri32 = 1
ri1rj1 + ri2r2j + ri3rj3 = 0
det(R) = +1
Parameterization of SO(3)
Euler angles: (
z
1 2 3 4
y
y
y
Unit quaternion: x
(cos /2, n1 sin /2, n2 sin /2, n3 sin /2)
Metric in Configuration Space
A metric or distance function d in C is a map
d: (q1,q2) C2 d(q1,q2) > 0
such that:
d(q1,q2) = 0 if and only if q1 = q2
d(q1,q2) = d (q2,q1)
d(q1,q2) < d(q1,q3) + d(q3,q2)
Metric in Configuration Space
Example:
Robot A and point x of A
x(q): location of x in the workspace when A is
at configuration q
A distance d in C is defined by:
d(q,q) = maxxA ||x(q)-x(q)||
where ||a - b|| denotes the Euclidean distance
between points a and b in the workspace
Specific Examples in R2 x S1
q = (x,y,), q = (x,y,) with [0,2)
= min{|| , 2||}
d(q,q) = sqrt[(x-x)2 + (y-y)2 + 2]
d(q,q) = sqrt[(x-x)2 + (y-y)2 + ()2]
where is the maximal distance between the
reference point and a robot point
Notion of a Path
q0
q1
q2
qn
(s)
q4
q3
A path in C is a piece of continuous curve
connecting two configurations q and q:
: s [0,1] (s) C
s s d((s),(s)) 0
Other Possible Constraints on Path
q0
q1
q2
qn
(s)
q4
q3
Finite length, smoothness, curvature, etc
A trajectory is a path parameterized by time:
: t [0,T] (t) C
Obstacles in C-Space
A configuration q is collision-free, or free, if the
robot placed at q has null intersection with the
obstacles in the workspace
The free space F is the set of free
configurations
A C-obstacle is the set of configurations where
the robot collides with a given workspace
obstacle
A configuration is semi-free if the robot at this
configuration touches obstacles without overlap
Disc Robot in 2-D Workspace
Rigid Robot Translating in 2-D
CB = B A = {b-a | aA, bB}
b1-a1
b1
a1
Linear-Time Computation of
C-Obstacle in 2-D
(convex polygons)
Rigid Robot Translating and
Rotating in 2-D
C-Obstacle for Articulated Robot
Free and Semi-Free Paths
A free path lies entirely in the free
space F
A semi-free path lies entirely in the
semi-free space
Remark on Free-Space Topology
The robot and the obstacles are modeled as
closed subsets, meaning that they contain their
boundaries
One can show that the C-obstacles are closed
subsets of the configuration space C as well
Consequently, the free space F is an open subset
of C. Hence, each free configuration is the center
of a ball of non-zero radius entirely contained in F
The semi-free space is a closed subset of C. Its
boundary is a superset of the boundary of F
Notion of Homotopic Paths
Two paths with the same endpoints are
homotopic if one can be continuously deformed
into the other
R x S1 example:
q
q
1 and 2 are homotopic
1 and 3 are not homotopic
In this example, infinity of homotopy classes
Connectedness of C-Space
C is connected if every two configurations can be
connected by a path
C is simply-connected if any two paths
connecting the same endpoints are homotopic
Examples: R2 or R3
Otherwise C is multiply-connected
Examples: S1 and SO(3) are multiply- connected:
- In S1, infinity of homotopy classes
- In SO(3), only two homotopy classes
C-Obstacle for Articulated Robot
Classes of Homotopic Free Paths
Motion-Planning Framework
Continuous representation
(configuration space formulation)
Discretization
Graph searching
(blind, best-first, A*)
Path-Planning Approaches
1. Roadmap
Represent the connectivity of the free space
by a network of 1-D curves
2. Cell decomposition
Decompose the free space into simple cells
and represent the connectivity of the free
space by the adjacency graph of these cells
3. Potential field
Define a function over the free space that
has a global minimum at the goal configuration
and follow its steepest descent
Roadmap Methods
Visibility graph
Introduced in the
Shakey project at
SRI in the late 60s.
Can produce
shortest paths in 2D configuration
spaces
Roadmap Methods
Visibility graph
Voronoi diagram
Introduced by
Computational Geometry
researchers. Generate
paths that maximizes
clearance. Applicable
mostly to 2-D
configuration spaces
Roadmap Methods
Visibility graph
Voronoi diagram
Silhouette
First complete general method that applies to
spaces of any dimension and is singly exponential
in # of dimensions [Canny, 87]
Probabilistic roadmaps
Path-Planning Approaches
1. Roadmap
Represent the connectivity of the free space
by a network of 1-D curves
2. Cell decomposition
Decompose the free space into simple cells
and represent the connectivity of the free
space by the adjacency graph of these cells
3. Potential field
Define a function over the free space that
has a global minimum at the goal configuration
and follow its steepest descent
Cell-Decomposition Methods
Two families of methods:
Exact cell decomposition
The free space F is represented by a
collection of non-overlapping cells whose
union is exactly F
Examples: trapezoidal and cylindrical
decompositions
Trapezoidal decomposition
Cell-Decomposition Methods
Two families of methods:
Exact cell decomposition
Approximate cell decomposition
F is represented by a collection of nonoverlapping cells whose union is contained
in F
Examples: quadtree, octree, 2n-tree
Octree Decomposition
Path-Planning Approaches
1. Roadmap
Represent the connectivity of the free space
by a network of 1-D curves
2. Cell decomposition
Decompose the free space into simple cells
and represent the connectivity of the free
space by the adjacency graph of these cells
3. Potential field
Define a function over the free space that
has a global minimum at the goal configuration
and follow its steepest descent
Potential Field Methods
Approach initially proposed for real-time
collision avoidance [Khatib, 86]. Hundreds of
papers published on this topic.
Potential field: Scalar function over the free
space
Ideal field (navigation function): Smooth, global
minimum at the goal, no local minima, grows to
infinity near obstacles
Force applied to robot: Negated gradient of the
potential field. Always move along that force
Attractive/Repulsive Fields
Khatib, 1986
Path planning:
Goal
- Regular grid G is placed over C-space
a best-first algorithm
- G is
FGoal
searched
k p ( x xusing
)
Goal
with potential field as the heuristic function
Goal
FObstacle
1 1 1
2
if 0 ,
0 x
0
if 0
Robot
Robot
