Robot Motion Planning
Configuration Spaces
q=(q1,…,qn)
Configuration Space
q1
q2
q3
qn
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
reference point
Rigid Robot Example
3-parameter representation: q = (x,y,q)
In a 3-D workspace q would be of the form (x,y,z,a,b,g)
x
y
q
robot
reference direction
workspace
Articulated Robot Example
q = (q1,q2,…,q10)
q1
q2
Protein example
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
What is its Topology?
(S1)7xR3
q1
q2
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
reference point
Case of a Planar Rigid Robot
3-parameter representation: q = (x,y,q) with q Î [0,2p). Two charts are needed
Other representation: q = (x,y,cosq,sinq)
c-space is a 3-D cylinder R2 x S1
embedded in a 4-D space
x
y
q
robot
reference direction
workspace
Rigid Robot in 3-D Workspace
q = (x,y,z,a,b,g)
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
The c-space is a 6-D space (manifold) embedded
in a 12-D Cartesian space. It is denoted by R3xSO(3)
Parameterization of SO(3)
Euler angles: (f,q,y)
Unit quaternion:
(cos q/2, n1 sin q/2, n2 sin q/2, n3 sin q/2)
1 2 3 4
x
y
z
f
x
y
z
x
y
z
q
x
y
z
y
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’) = maxxÎA ||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), q’ = (x’,y’,q’) with q, q’ Î [0,2p)
a = min{|q-q’| , 2p-|q-q’|}
d(q,q’) = sqrt[(x-x’)2 + (y-y’)2 + a2]
d(q,q’) = sqrt[(x-x’)2 + (y-y’)2 + (ar)2]
where r is the maximal distance between the reference point and a robot point
q
q’
a
Notion of a Path
A path in C is a piece of continuous curve connecting two configurations q and q’:
t : s Î [0,1] t (s) Î C
s’ ® s Þ d(t(s),t(s’)) ® 0
q
1
q
3
q
0
q
n
q
4
q
2
t(s)
Other Possible Constraints on Path
Finite length, smoothness, curvature, etc…
A trajectory is a path parameterized by time:
t : t Î [0,T] t (t) Î C
q
1
q
3
q
0
q
n
q
4
q
2
t(s)
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 | aÎA, bÎB}
a1
b1
b1-a1
Rigid Robot Translating in 2-D
CB = B A = {b-a | aÎA, bÎB}
a1
b1
b1-a1
Linear-Time Computation of
C-Obstacle in 2-D
(convex polygons)
O(n+m)
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:
t1 and t2 are homotopic
t1 and t3 are not homotopic
In this example, infinity of homotopy classes
q
q’
t1
t2
t3
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