Chapter 5:
Path Planning
Hadi Moradi
Motivation
•
•
Need to choose a path for the end effector that avoids collisions and
singularities
Collisions are easy to define in the workspace, but need to be mapped
into the configuration space for convenience
Workspace v. configuration space
• Workspace: volume swept out by the end effector (in inertial
frame)
• Configuration: location of all points on a robotic manipulator
• Configuration space:
Obstacles
• Discrete obstacles are denoted Oi (in the workspace)
• Denote the robot as A(q) at configuration q
• The configuration space obstacle, QO, is defined as:
• The free configuration space is the space of all collision-free
configurations:
Motion Planning for a Point Robot
free space
s
free path
g
Problem
semi-free path
Types of Path Constraints
 Local constraints:
lie in free space
 Differential constraints:
have bounded curvature
 Global constraints:
have minimal length
Motion-Planning Framework
Continuous representation
Discretization
Graph searching
(blind, best-first, A*)
Example: Visibility Graph (A Roadmap Method)
 Visibility graph
Introduced in the Shakey
project at SRI in the late 60s.
Can produce shortest paths in
2-D configuration spaces
g
s
Example: Voronoi Diagram (A Roadmap Method)
 Voronoi diagram
Introduced by Computational
Geometry researchers. Generate
paths that maximizes clearance.
O(n log n) time
O(n) space
Cell-Decomposition Methods
Two classes of methods:
 Exact cell decomposition
 Approximate cell decomposition
F is represented by a collection of
non-overlapping cells whose union is contained in F
Examples: quadtree, octree, 2n-tree
Approximate Cell Decomposition: Quad Tree
Octree Decomposition (3D environment)
Potential Field Methods
Approach initially proposed for real-time collision avoidance [Khatib, 86].
Hundreds of papers published on it.
Goal
Goal
Go
al F
orc
e
n
Mo tio
Robot
Robot
orc e
O bs tac le F

Attractive and Repulsive fields
Potential Fields
Local-Minimum Issue
 Perform best-first search (possibility of
combining with approximate cell decomposition)
 Alternate descents and random walks
 Use local-minimum-free potential (navigation function)
Ex: 2D Cartesian manipulator
• The configuration space is R2
• Consider only one object in the workspace
– End effector and obstacle are convex polygons
• What is the configuration space obstacle?
Ex: 2D Cartesian manipulator
• The nice thing about this example is that the workspace and the
configuration space are identical
Ex: planar two-link manipulator
• What is the configuration space obstacle for a two-link manipulator
Motivation
• Geometric complexity
• Space dimensionality
Path planning overview
• Want to find a path from an initial position to a final position
Potential fields
• To develop the mapping, we incrementally explore Qfree
• Consider the manipulator (statically) as a point in the
configuration space
• The manipulator is subject to a potential field
– Attractive in the case of the goal configuration
– Repulsive in the case of an obstacle
Uq   Uatt q   Urep q 
Gradient descent
• In order to find minima of U, take the negative gradient:
 q   Uq   Uatt q   Urep q 
The attractive field
• We define a potential field that attracts each of the n DH
coordinate frames from the initial position to the goal position
The attractive field
• Simple potential field, conic well potential
The attractive field
• Instead we use a continually differentiable function: parabolic
well potential
– Field grows quadratically with the distance from the goal
configuration
U att ,i q  
1
2
 i oi q   oi qf 
2
Hybrid attractive field
• Combine the conic well potential and parabolic well potential
fields
– If the ith frame is close to the workspace goal, use the parabolic well
– If the ith frame is far from the workspace goal, use the conic well
1
2






o
q

o
q
i
i
i
f

2
Uatt ,i q   
1
d i oi q   oi qf    i d 2
2

for oi q   oi qf   d
for oi q   oi qf   d
• The distance d defines the distance from the goal that causes a
transition from a conic to parabolic potential
• Since this is continuous everywhere, the workspace force is
defined everywhere
Hybrid attractive field
• Taking the gradient gives the workspace attractive force
Fatt ,i q   U att ,i q 
   i oi q   oi qf  for oi q   oi qf   d

o q   oi qf  for o q   o q   d

 d i i
i
i
f





o
q

o
q
i
i
f

Ex: planar two link manipulator
• For the 2-link arm shown below, assume that both links have
length 1
0
qs   
0
 / 2
1
0
2
 1








qf  

o
q

,
o
q

,
o
q

,
o
q

1 s

0 1 f
1 2 s 0 2 f
1
 / 2
 
 
 
 
The repulsive field
• Prevent collisions by creating a repulsive force in the workspace
– Again, create forces that act on the origins of the n DH coordinate
frames
• These forces should:
– Repel the robot from obstacles
– Do nothing of the robot is far away from obstacles
The repulsive field
• Therefore, the workspace repulsive force is:
Frep ,i q   Urep ,i q 
• To evaluate this, consider the distance function r(oi(q)) as r(x)
where x is a three dimensional vector:
The repulsive field
• So we can write this force as:
 
1
1
1



r oi q  for r oi q   r0

Frep ,i q    i  r oi q  r0  r o q 2
i

0
for r oi q   r0

Ex: planar two link manipulator
• Consider a convex obstacle close to o2
– Obstacle is outside the distance of influence for o1
– Again, the lengths are both 1
– Let b be the point on the obstacle closest to o2
• b = [2 0.5]T
• r(o2(qs)) = 0.5
– Let r0 = 1 (no influence on o1)
– The initial repulsive force on o2 is:
Other considerations
1. what happens if either there are multiple objects, or an object is
not convex?
Other considerations
2. what if the obstacle is closest to another part of a link (i.e. not the
origin of the DH frame)?
The relation between workspace forces and joint torques
  Jv F
T
Ex: two-link planar manipulator
• Consider the previous examples with an obstacle exerting a
repulsive force on o2
• Find the attractive and repulsive forces on o1 and o2
Initial and goal configurations
Obstacle location
Ex: two-link planar manipulator
• To determine the joint torques, take the transpose of the
Jacobians at the initial configuration
Composing workspace forces
• The total joint torques acting on a manipulator is the sum of the
torques from all attractive and repulsive potentials:
 q    Jo q Fatt ,i q    Jo q Frep ,i q 
T
T
i
i
i
i
Ex: two-link planar manipulator
• Consider again the two-link manipulator with a goal position and
an obstacle near o2
• The total joint torque, due to these two potential fields is:
Initial and goal configurations
Obstacle location
Gradient descent Path Planning Algorithm
1. First, determine your initial configuration
2. Second, given a desired point in the workspace, calculate the
final configuration using the inverse kinematics
– Use this to create an attractive potential field
3. Locate obstacles in the workspace
– Create a repulsive potential field
4. Sum the joint torques in the configuration space
5. Use gradient descent to reach your target configuration
Local minima
• In the absence of obstacles, the gradient descent will always
converge to the global minimum (qf)
• With obstacles, by proper choice of ai, this will always converge
to some minima
Local minima
•
•
•
•
Instead we modify the gradient
descent algorithm to add a
random excitation in case we
are stuck in a local minima
We are stuck in a local minima if
successive iterations result in
minimal changes in the
configuration
If so, perform a random walk to
get out
The random walk is defined by
adding a uniformly distributed
variable to each joint parameter
1. 0  i , qs  q 0
2. if q i  qf  
q
i 1
 q i 
 q a
 q i 
i
i
i 
else
return q 0 , q 1,..., q i
3. if q i  q i 1   m
random w alk to q 
q i 1  q 
4. goto 2
Next class…
• Applications to numerically solving for the inverse kinematics
• Probabilistic methods