Introduction to ROBOTICS
Robot Motion Planning
Dr. John (Jizhong) Xiao
Department of Electrical Engineering
City College of New York
jxiao@ccny.cuny.edu
City College of New York
1
What is Motion Planning?
• Determining where to go without hit obstacles
City College of New York
2
Topics
• Basics
– Configuration Space
– C-obstacles
• Motion Planning Methods
– Roadmap Approaches
• Visibility graphs
• Voronoi diagram
– Cell Decomposition
• Trapezoidal Decomposition
• Quadtree Decomposition
– Potential Fields
– Bug Algorithms
City College of New York
3
References
• G. Dudek, M. Jenkin, Computational Principles of Mobile Robots, MIT
Press, 2000 (Chapter 5)
• J.C. Latombe, Robot Motion Planning, Kluwer Academic Publishers, 1991.
• Additional references
– Path Planning with A* algorithm
• S. Kambhampati, L. Davis, “Multiresolution Path Planning for
Mobile Robots”, IEEE Journal of Robotrics and Automation,Vol.
RA-2, No.3, 1986, pp.135-145.
– Potential Field
• O. Khatib, “Real-Time Obstacle Avoidance for Manipulators and
Mobile Robots”, Int. Journal of Robotics Research, 5(1), pp.90-98,
1986.
• P. Khosla, R. Volpe, “Superquadratic Artificial Potentials for
Obstacle Avoidance and Approach” Proc. Of ICRA, 1988, pp.11781784.
• B. Krogh, “A Generalized Potential Field Approach to Obstacle
Avoidance Control” SME Paper MS84-484.
City College of New York
4
The World consists of...
• Obstacles
– Already occupied spaces of the world
– In other words, robots can’t go there
• Free Space
– Unoccupied space within the world
– Robots “might” be able to go here
– To determine where a robot can go, we need to
discuss what a Configuration Space is
City College of New York
5
Configuration Space
Notation:
A: single rigid object –(the robot)
W: Euclidean space where A moves; W  R 2
or
R3
B1,…Bm: fixed rigid obstacles distributed in W
• FW – world frame (fixed frame)
• FA – robot frame (moving frame
rigidly associated with the robot)
Configuration q of A is a specification
of the physical state (position and
orientation) of A w.r.t. a fixed
environmental frame FW.
Configuration Space is the space of all possible robot configurations.
City College of New York
6
Configuration Space
Configuration Space of A is the space (C )of all possible
configurations of A.
Point robot (free-flying, no constraints)
C
Cfree
qslug
Cobs
qrobot
For a point robot moving in 2-D plane, C-space is R 2
City College of New York
7
Configuration Space
C
y
Z
Cfree
qgoal
Cobs
qstart
x
For a point robot moving in 3-D, the C-space is R 3
What is the difference between Euclidean space and C-space?
City College of New York
8
Configuration Space
Y
A robot which can translate in the plane
C-space:
2-D (x, y)
X
Euclidean space:R 2
Y
A robot which can translate and rotate in the plane

C-space: 3-D (x, y, )
Y
X
City College of New York
x
9
Configuration Space
b
b
a
a
2R manipulator
Configuration space
City College of New York
10
topology
Configuration Space
360
qrobot
270
b
180
b
a
90
qslug
0
45
Two points in the robot’s workspace
a
90
135
180
Torus
(wraps horizontally and vertically)
City College of New York
11
Configuration Space
If the robot configuration is within the blue area, it will hit the obstacle
360
qrobot
270
b
180
b
a
90
qslug
0
An obstacle in the robot’s workspace
What is dimension of the C-space of puma
robot (6R)?
45
a
90
135
a “C-space” representation
180
Visualization of high dimension
C-space is difficult
City College of New York
12
Motion Planning Revisit
Find a collision free path from an initial
configuration to goal configuration
while taking into account the constrains
(geometric, physical, temporal)
C-space concept
provide a
generalized
framework to
study the motion
planning problem
A separate problem for each robot?
City College of New York
13
What if the robot is not a point?
The Pioneer-II robot should
probably not be modeled as a
point...
City College of New York
14
What if the robot is not a point?
Expand
obstacle(s)
Reduce
robot
not quite right ...
City College of New York
15
Obstacles Configuration Space
C-obstacle
Point robot
City College of New York
16
Free Space
From
Robot Motion Planning
J.C. Latombe
City College of New York
17
Minkowski Sums
This expansion of one planar shape by
another is called the Minkowski sum 
Rectangular robot which can translate only
R
PR
P
(Dilation operation)
P  R = { p + r | p  P and r  R }
Used in robotics to ensure that there are free paths available.
City College of New York
18
Additional Dimension
What would the C-obstacle be if the rectangular
robot (red) can translate and rotate in the plane.
(The blue rectangle is an obstacle.)
y
Rectangular robot which can translate and rotate
x
City College of New York
19
C-obstacle in 3-D
What would the C-obstacle be if the rectangular
robot (red) can translate and rotate in the plane.
(The blue rectangle is an obstacle.)
3-D
y
360º
180º
x
City College of New York
0º
this is twisted...
20
C-obstacle in 3-D
What would the configuration space of a 3DOF
rectangular robot (red) in this world look like?
(The obstacle is blue.)
180º
y
can we stay in 2d ?
3-D
x
City College of New York
0º
21
One slice
Taking one slice of the C-obstacle in which the
robot is rotated 45 degrees...
PR
R
y
45 degrees
P
x
City College of New York
How many slices does P R have?
22
2-D projection
y
x
why not keep it this simple?
City College of New York
23
Projection problems
qinit
qgoal
City College of New York
too conservative!
24
Topics
• Configuration Space
• Motion Planning Methods
–
–
–
–
Roadmap Approaches
Cell Decomposition
Potential Fields
Bug Algorithms
City College of New York
25
Motion Planning Methods
The motion planning problem consists of the following:
Input
• geometric descriptions of a robot
and its environment (obstacles)
• initial and goal configurations
qrobot
qgoal
Output
• a path from start to finish (or
the recognition that none exists)
Applications
Robot-assisted surgery
Automated assembly plans
Drug-docking and analysis
Moving pianos around...
City College of New York
What to
26do?
Motion Planning Methods
(1) Roadmap approaches
Goal reduce the N-dimensional
configuration space to a set of
one-D paths to search.
(2) Cell decomposition
Goal account for all of the free space
(3) Potential Fields
Goal Create local control strategies that
will be more flexible than those above
(4) Bug algorithms
Limited knowledge path planning
City College of New York
27
Roadmap: Visibility Graphs
Visibility graphs: In a polygonal (or polyhedral) configuration space,
construct all of the line segments that connect vertices to one another (and that do
not intersect the obstacles themselves).
Formed by connecting all “visible” vertices, the start point and the end
point, to each other.
For two points to be “visible” no obstacle can exist between them
Paths exist on the perimeter of obstacles
From Cfree, a graph is defined
Converts the problem into graph search.
Dijkstra’s algorithm O(N^2)
N = the number of vertices in C-space
City College of New York
28
The Visibility Graph in Action (Part 1)
• First, draw lines of sight from the start and goal to all
“visible” vertices and corners of the world.
goal
start
City College of New York
29
The Visibility Graph in Action (Part 2)
• Second, draw lines of sight from every vertex of every
obstacle like before. Remember lines along edges are also
lines of sight.
goal
start
City College of New York
30
The Visibility Graph in Action (Part 3)
• Second, draw lines of sight from every vertex of every
obstacle like before. Remember lines along edges are also
lines of sight.
goal
start
City College of New York
31
The Visibility Graph in Action (Part 4)
• Second, draw lines of sight from every vertex of every
obstacle like before. Remember lines along edges are also
lines of sight.
goal
start
City College of New York
32
The Visibility Graph (Done)
• Repeat until you’re done.
goal
start
Since the map was in C-space, each line potentially represents part of a path
from the start to the goal.
City College of New York
33
Visibility graph drawbacks
Visibility graphs do not preserve their
optimality in higher dimensions:
shortest path
shortest path within the visibility graph
In addition, the paths they find are “semi-free,” i.e. in contact with obstacles.
No clearance
City College of New York
34
Roadmap: Voronoi diagrams
“official” Voronoi diagram
(line segments make up the
Voronoi diagram isolates a
set of points)
Property: maximizing the
clearance between the points
and obstacles.
Generalized Voronoi Graph (GVG):
locus of points equidistant from the closest two
or more obstacle boundaries, including the
workspace boundary.
City College of New York
35
Roadmap: Voronoi diagrams
• GVG is formed
by paths
equidistant
from the two
closest objects
• maximizing the
clearance
between the
obstacles.
• This generates a very safe roadmap which avoids
obstacles as much as possible
City College of New York
36
Voronoi Diagram: Metrics
• Many ways to measure distance; two are:
– L1 metric
• (x,y) : |x| + |y| = const
– L2 metric
• (x,y) : x2 +y2 = const
City College of New York
37
Voronoi Diagram (L1)
Note the
lack of
curved
edges
City College of New York
38
Voronoi Diagram (L2)
Note the
curved
edges
City College of New York
39
Motion Planning Methods
Roadmap approaches
• Visibility Graph
• Voronoi Diagram
Cell decomposition
• Exact Cell Decomposition (Trapezoidal)
• Approximate Cell Decomposition (Quadtree)
Potential Fields
Hybrid local/global
City College of New York
40
Exact Cell Decomposition
Trapezoidal Decomposition:
Decomposition of the free space
into trapezoidal & triangular cells
Connectivity graph representing the
adjacency relation between the cells
(Sweepline algorithm)
City College of New York
41
Exact Cell Decomposition
Trapezoidal Decomposition:
Search the graph for a path
(sequence of consecutive cells)
City College of New York
42
Exact Cell Decomposition
Trapezoidal Decomposition:
Transform the sequence of cells into a
free path (e.g., connecting the midpoints of the intersection of two
consecutive cells)
City College of New York
43
Optimality
Trapezoidal Decomposition:
15 cells
9 cells
Trapezoidal decomposition is exact
and complete, but not optimal
Obtaining the minimum number of convex
cells is NP-complete.
there may be more details in the world than the task needs to worry about...
City College of New York
44
Approximate Cell Decomposition
Quadtree Decomposition:
Quadtree:
recursively subdivides each mixed
obstacle/free (sub)region into four quarters...
City College of New York
45
further decomposing...
Quadtree Decomposition:
Quadtree:
recursively subdivides each mixed
obstacle/free (sub)region into four quarters...
City College of New York
46
further decomposing...
Quadtree Decomposition:
• The rectangle cell is recursively
decomposed into smaller rectangles
• At a certain level of resolution, only the
cells whose interiors lie entirely in the free
space are used
• A search in this graph yields a collision
free path
Again, use a graph-search algorithm to
find a path from the start to goal
Quadtree
is this a complete path-planning algorithm?
i.e., does it find a path when one exists ?
City College of New York
47
Motion Planning Methods
Roadmap approaches
Cell decomposition
• Exact Cell Decomposition (Trapezoidal)
• Approximate Cell Decomposition (Quadtree)
Potential Fields
Hybrid local/global
City College of New York
48
Potential Field Method
Potential Field (Working Principle)
– The goal location generates an attractive potential – pulling the robot
towards the goal
– The obstacles generate a repulsive potential – pushing the robot far away
from the obstacles
– The negative gradient of the total potential is treated as an artificial
force applied to the robot
-- Let the sum of the forces control the robot
C-obstacles
City College of New York
49
Potential Field Method
• Compute an attractive force toward the goal
C-obstacles
Attractive potential
City College of New York
50
Potential Field Method
• Compute a repulsive force away from obstacles
Repulsive Potential
 Create a potential barrier around the C-obstacle
region that cannot be traversed by the robot’s
configuration
 It is usually desirable that the repulsive potential
does not affect the motion of the robot when it is
sufficiently far away from C-obstacles
City College of New York
51
Potential Field Method
• Compute a repulsive force away from obstacles
• Repulsive Potential
City College of New York
52
Potential Field Method
• Sum of Potential
Attractive potential
Repulsive potential
C-obstacle
Sum of potentials
City College of New York
53
Potential Field Method
• After get total potential, generate force field
(negative gradient)
• Let the sum of the forces control the robot
Negative
gradient
Equipotential contours
Total potential
To a large extent, this is computable from sensor readings
City College of New York
54
Potential Field Method
Pros:
• Spatial paths are not preplanned and can be generated in real time
• Planning and control are merged into one function
• Smooth paths are generated
• Planning can be coupled directly to a control algorithm
Cons:
• Trapped in local minima in the potential field
• Because of this limitation, commonly used for local path planning
• Use random walk, backtracking, etc to escape the local minima
random walks are not perfect...
City College of New York
55
Motion Planning Methods
Roadmap approaches
• Visibility Graph
• Voronoi Diagram
Cell decomposition
• Trapezoidal decomposition
Full-knowledge
motion planning
• Quadtree decomposition
Potential Fields
Bug algorithm
Limited-knowledge path planning
City College of New York
56
Bug Algorithms
Path planning with limited knowledge
- Insect-inspired “bug” algorithms
Goal
• known direction to goal
• only local sensing
(walls/obstacles encoders)
• “reasonable” world
1) finite obstacles in any finite range
2) a line will intersect an obstacle
finite times
Start
City College of New York
57
Beginner Strategy
Insect-inspired “bug” algorithms
Switching between two simple
behaviors:
1.
Moving directly towards the
goal
2.
Circumnavigating an obstacle
“Bug” algorithm
1) head toward goal
2) follow obstacles until you can
head toward the goal again
assume a
leftist robot
3) continue
City College of New York
58
Summary
Configuration Space
Motion Planning Methods
• Roadmap approaches
• Cell decomposition
• Potential Fields
• Bug Algorithms
City College of New York
59
Thank you!
Homework 8 is posted on the web
Next class: Mapping
Time: Nov. 25, Tue
City College of New York
60