Dr. John (Jizhong) Xiao

1
Robot Motion Planning
Introduction to ROBOTICS

• Dr. John (Jizhong) Xiao
• Department of Electrical Engineering
• City College of New York
• jxiao_at_ccny.cuny.edu

2
What is Motion Planning?
without hit obstacles

• Determining where to go

3
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

4
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.1178- 1784.
• B. Krogh, A Generalized Potential Field
  Approach to Obstacle Avoidance Control SME
  Paper MS84-484.

5
The World consists of...

• Obstacles
• Already occupied spaces of the world
• In other words, robots cant 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

6
Configuration Space
Notation A single rigid object (the robot) W
Euclidean space where A moves 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.
7
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
8
Configuration Space
C
y
Cfree
qgoal
Z
Cobs
qstart
x
For a point robot moving in 3-D, the C-space is
What is the difference between Euclidean space
and C-space?
9
Configuration Space
Y
A robot which can translate in the plane
C-space
2-D (x, y)
X
Euclidean space
Y
A robot which can translate and rotate in the
plane
C-space
3-D (x, y, )
Y
X
x
10
Configuration Space
b
b
a
a
2R manipulator
Configuration space
topology
11
Configuration Space
360
qrobot
270
b
180
b
90
a
qslug
a
0
45
135
90
180
Two points in the robots workspace
Torus
(wraps horizontally and vertically)
12
Configuration Space
If the robot configuration is within the blue
area, it will hit the obstacle
360
qrobot
270
b
180
b
90
a
qslug
a
0
45
135
90
180
An obstacle in the robots workspace
a C-space representation
What is dimension of the C-space of puma robot
(6R)?
Visualization of high dimension C-space is
difficult
13
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?
14
What if the robot is not a point?
The Pioneer-II robot should probably not be
modeled as a point...
15
What if the robot is not a point?
Expand obstacle(s)
Reduce robot
not quite right ...
16
Obstacles Configuration Space
C-obstacle
Point robot
17
Free Space
From Robot Motion Planning J.C. Latombe
18
Minkowski Sums
This expansion of one planar shape by another is
called the Minkowski sum ?
Rectangular robot which can translate only
P ? R
R
P
P ? R p r p ? P and r ? R
(Dilation operation)
Used in robotics to ensure that there are free
paths available.
19
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
20
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º
0º
x
this is twisted...
21
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.)
3-D
180º
y
0º
x
can we stay in 2d ?
22
One slice
Taking one slice of the C-obstacle in which the
robot is rotated 45 degrees...
P ? R
R
y
45 degrees
P
How many slices does P ?R have?
x
23
2-D projection
y
x
why not keep it this simple?
24
Projection problems
qinit
qgoal
too conservative!
25
Topics

• Configuration Space
• Motion Planning Methods
• Roadmap Approaches
• Cell Decomposition
• Potential Fields
• Bug Algorithms

26
Motion Planning Methods
The motion planning problem consists of the
following
Input
Output

• geometric descriptions of a robot and its
  environment (obstacles)
• initial and goal configurations

• a path from start to finish (or the recognition
  that none exists)

qgoal
qrobot
Applications
Robot-assisted surgery Automated assembly
plans Drug-docking and analysis Moving pianos
around...
What to do?
27
Motion Planning Methods
(1) Roadmap approaches (2) Cell decomposition (3)
Potential Fields (4) Bug algorithms
Goal reduce the N-dimensional configuration space
to a set of one-D paths to search.
Goal account for all of the free space
Goal Create local control strategies that will be
more flexible than those above
Limited knowledge path planning
28
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.
29
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
30
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
31
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
32
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
33
The Visibility Graph (Done)

• Repeat until youre done.

Since the map was in C-space, each line
potentially represents part of a path from the
start to the goal.
34
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
35
Roadmap Voronoi diagrams
official Voronoi diagram
(line segments make up the Voronoi diagram
isolates a set of points)
Generalized Voronoi Graph (GVG) locus of points
equidistant from the closest two or more obstacle
boundaries, including the workspace boundary.
Property maximizing the clearance between the
points and obstacles.
36
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

37
Voronoi Diagram Metrics

• Many ways to measure distance two are
• L1 metric
• (x,y) x y const
• L2 metric
• (x,y) x2 y2 const

38
Voronoi Diagram (L1)
Note the lack of curved edges
39
Voronoi Diagram (L2)
Note the curved edges
40
Motion Planning Methods

• Roadmap approaches
• Visibility Graph
• Voronoi Diagram
• Cell decomposition
• Exact Cell Decomposition (Trapezoidal)
• Approximate Cell Decomposition (Quadtree)
• Potential Fields
• Hybrid local/global

41
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)
42
Exact Cell Decomposition
Trapezoidal Decomposition
Search the graph for a path (sequence of
consecutive cells)
43
Exact Cell Decomposition
Trapezoidal Decomposition
Transform the sequence of cells into a free path
(e.g., connecting the mid-points of the
intersection of two consecutive cells)
44
Optimality
Trapezoidal Decomposition
15 cells
9 cells
Obtaining the minimum number of convex cells is
NP-complete.
Trapezoidal decomposition is exact and complete,
but not optimal
there may be more details in the world than the
task needs to worry about...
45
Approximate Cell Decomposition
Quadtree Decomposition
recursively subdivides each mixed obstacle/free
(sub)region into four quarters...
Quadtree
46
further decomposing...
Quadtree Decomposition
recursively subdivides each mixed obstacle/free
(sub)region into four quarters...
Quadtree
47
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
is this a complete path-planning algorithm? i.e.,
does it find a path when one exists ?
Quadtree
48
Motion Planning Methods

• Roadmap approaches
• Cell decomposition
• Exact Cell Decomposition (Trapezoidal)
• Approximate Cell Decomposition (Quadtree)
• Potential Fields
• Hybrid local/global

49
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
50
Potential Field Method

• Compute an attractive force toward the goal

C-obstacles
Attractive potential
51
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 robots
  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

52
Potential Field Method

• Compute a repulsive force away from obstacles

• Repulsive Potential

53
Potential Field Method

• Sum of Potential

Attractive potential
Repulsive potential
C-obstacle
Sum of potentials
54
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
55
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...
56
Motion Planning Methods

• Roadmap approaches
• Visibility Graph
• Voronoi Diagram
• Cell decomposition
• Trapezoidal decomposition
• Quadtree decomposition
• Potential Fields
• Bug algorithm

Full-knowledge motion planning
Limited-knowledge path planning
57
Bug Algorithms
Path planning with limited knowledge
- Insect-inspired bug algorithms

• known direction to goal
• only local sensing (walls/obstacles
  encoders)

Goal

• reasonable world

1) finite obstacles in any finite range
2) a line will intersect an obstacle
finite times
Start
58
Beginner Strategy
Insect-inspired bug algorithms

• Switching between two simple behaviors
• Moving directly towards the goal
• Circumnavigating an obstacle

Bug algorithm
1) head toward goal 2) follow obstacles until you
can head toward the goal again 3) continue
assume a leftist robot
59
Summary

• Configuration Space
• Motion Planning Methods
• Roadmap approaches
• Cell decomposition
• Potential Fields
• Bug Algorithms

60
Thank you!
Homework 8 is posted on the web Next class
Mapping Time Nov. 25, Tue