Motion Planning & Robot Planning
Prof.: S. Shiry
Mohsen gandomkar
M.Sc Artificial Intelligence
Department of Computer Eng. and IT
Amirkabir Univ. of Technology
(Tehran Polytechnic)
What is Motion Planning?
• Motion planning is aimed at providing robots
with the capability of deciding automatically
which motions to execute in order to achieve
their tasks without colliding with other objects
in their work space
Basic Definition
• 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
The Configuration Space
Configuration Space of A is the space (C) of all
possible configurations of A.
C
Cfree
qgoal
Cobs
qstart
For a point robot moving in 2-D plane, C-space is R 2
The Configuration Space
C
y
Z
Cfree
qgoal
Cobs
qstart
x
For a point robot moving in 3-D, the C-space is R3
What is the difference between Euclidean space and C-space?
The Configuration Space
• How to create it
• First abstract the robot as a point object.
Then, enlarge the obstacles to account for
the robot’s footprint and degrees of
freedom
• In our example, the robot was circular, so
we simply enlarged our obstacles by the
robot’s radius (note the curved vertices)
Example of a World (and Robot)
Free Space
Obstacles
Robot
x,y
Configuration Space:
Accommodate Robot Size
Free Space
Obstacles
x,y
Robot
(treat as point object)
Motion Planning
• Basic problem: Collision-free path planning for
one rigid or articulated object (the “robot”) among
static obstacles.
• Inputs
• geometric descriptions of the obstacles and the robot
• kinematic and dynamic properties of the robot
• initial and goal positions (configurations) of the robot
• Output
• Continuous sequence of collision-free configurations
connecting the initial and goal configurations.
Algorithmic Approaches
• Complete Algorithms
• Probabilistic Algorithms
• Heuristic Algorithms
Complete Algorithms
• Guaranteed to find a free path between two give
configurations when exists and report failure
otherwise
• Deal with connectivity of free space by capturing it
on a graph.
• Cell Decomposition - partition of free space
• Roadmap Technique - network of curves
Probabilistic Algorithms
• Trade-off exactness against running time
• Don’t guarantee a solution but if exists very likely
to find it relatively quickly
• Example: Probabilistic Roadmap Algorithm
• Experimental results show that computation takes
less than a second
Heuristic Algorithms
• Many work well in practice but offer no
performance guarantee
• Deal with a grid on configuration space
• Example 1 : Potential Field
• Example 2 : Approximate Cell Decomposition
Previous Approaches
Visibility Graphs
Voronoi Diagrams
Exact Cell Decomposition
Approximate Cell Decomposition
Potential Fields
Probabilistic Roadmaps Method
Problems before PRMs
• Hard to plan for many dof robots
• Computation complexity for high-dimensional
configuration spaces would grow exponentially
• Potential fields run into local minima
• Complete, general purpose algorithms are at best
exponential and have not been implemented
Difficulty with classic approaches
• Running time increases exponentially with the
dimension of the configuration space.
• For a d-dimension grid with 10 grid points on
each dimension, how many grid cells are there?
10d
• Several variants of the path planning problem
have been proven to be PSPACE-hard.
Probabilistic Roadmap (PRM):
multiple queries
local path
free space
milestone
[Kavraki, Svetska, Latombe,Overmars, 96]
Probabilistic Roadmap (PRM):
single query
Multiple-Query PRM
Classic multiple-query PRM
• Probabilistic Roadmaps for Path Planning in HighDimensional Configuration Spaces, L. Kavraki et al.,
1996.
Assumptions
• Static obstacles
• Many queries to be processed in the same
environment
• Examples
• Navigation in static virtual environments
• Robot manipulator arm in a workcell
Enter PRMs
• PRMs use fast collision checking techniques
• PRMs avoid computing an explicit representation of the
configuration space
• Two Phases
• A Learning Phase
• A Query Phase
The Learning Phase
• Construct a probabilistic roadmap by generating
random free configurations of the robot and
connecting them using a simple, but very fast
motion planer, also know as a local planner
• Store as a graph whose nodes are the
configurations and whose edges are the paths
computed by the local planner
PRM - Learning Phase
Free space
C-obstacle
PRM - Learning Phase
Free space
C-obstacle
PRM - Learning Phase
Free space
C-obstacle
milestones
PRM - Learning Phase
Free space
C-obstacle
milestones
The Query Phase
• Find a path from the start and goal configurations
to two nodes of the roadmap
• Search the graph to find a sequence of edges
connecting those nodes in the roadmap
• Concatenating the successive segments gives a
feasible path for the robot
Two geometric primitives
in configuration space
• CLEAR(q)
Is configuration q
collision free or not?
• LINK(q, q’)
Is the straight-line path
between q and q’
collision-free?
Uniform sampling
Input: geometry of the moving object & obstacles
Output: roadmap G = (V, E)
1: V   and E  .
2: repeat
3:
q  a configuration sampled uniformly at random from C.
4:
if CLEAR(q)then
5:
Add q to V.
6:
Nq  a set of nodes in V that are close to q.
6:
for each q’ Nq, in order of increasing d(q,q’)
7:
if LINK(q’,q)then
8:
Add an edge between q and q’ to E.
Difficulty
• Many small connected components
Resampling (expansion)
• Failure rate
• Weight
• Resampling probability
no. failed LINK
r (q) 
no. LINK
r (q)
w(q) 
 p r ( p)
Pr (q )  w(q )
Resampling (expansion)
Query processing
• Connect qinit and qgoal to the roadmap
• Start at qinit and qgoal, perform a random walk, and
try to connect with one of the milestones nearby
• Try multiple times
Error
• If a path is returned, the answer is always correct.
• If no path is found, the answer may or may not be
correct. We hope it is correct with high probability.
Why does it work? Intuition
• A small number of milestones almost “cover” the
entire configuration space.
Smoothing the path
Smoothing the path
Single-Query PRM
Lazy PRM
• Path Planning Using Lazy PRM, R. Bohlin & L.
Kavraki, 2000.
Precomputation: roadmap
construction
• Nodes
• Randomly chosen configurations, which may or
may not be collision-free
• No call to CLEAR
• Edges
• an edge between two nodes if the
corresponding configurations are close
according to a suitable metric
• no call to LINK
Query processing: overview
1. Find a shortest path in the roadmap
2. Check whether the nodes and edges in the path
are collision.
3. If yes, then done. Otherwise, remove the nodes
or edges in violation. Go to (1).
• We either find a collision-free path, or exhaust all
paths in the roadmap and declare failure.
Query processing: details
• Find the shortest path in the roadmap
• A* algorithm
• Dijkstra’s algorithm
• Check whether nodes and edges are collisions free
• CLEAR(q)
• LINK(q0, q1)
Node enhancement
• Select nodes that close the boundary of F
Bug algorithms
Bug algorithms
• Assumptions:
• Point robot
• Contact sensor (Bug1,Bug2) or finite range sensor
(Tangent Bug)
• Bounded environment
• Robot position is perfectly known
• Robot can measure the distance between two
points
Bug algorithms
• Algorithm consists of two behaviors:
1. Motion to goal – move toward the goal
• Bug1: move along the line that connects an
“initial” point to the goal until you reach the goal
or an obstacle (hit point).
• Bug2: move along the line that connects the start
point to the goal until you reach the goal or an
obstacle (hit point).
Bug algorithms
2. Boundary following – obstacle handeling
• Bug1: circumnavigate the entire perimeter of the
obstacle, find the closest point to the goal on the
perimeter (leave point), move to that point .
• Bug2: circumnavigate the obstacle until you reach
a new point on the line connecting start and goal,
that is closer to the goal (leave point).
Bug1 - example
q2L
q1L
qstart
q1H
q2H
qgoal
Motion to goal
Boundary following
Shortest path to goal
Bug2 - example
q2L
qgoal
q2H
q1L
qstart
q1H
Motion to goal
Boundary following
Line connecting start and goal
head-to-head comparison
What are worlds in which Bug 2 does
better than Bug 1 (and vice versa) ?
Bug 2 beats Bug 1
Start
Bug 1 beats Bug 2
head-to-head comparison
What are worlds in which Bug 2 does
better than Bug 1 (and vice versa) ?
Bug 2 beats Bug 1
Bug 1 beats Bug 2
Start
“zipper world”
Problem
Bug 2 beats Bug 1
Bug 1 beats Bug 2
“zipper world”
Problem
Adjusted bug algorithm
Bug M1 • use Bug2 until the robot finds
itself on the S-line farther from
the goal than it started
• if it does, revert to to Bug1 for
that obstacle
Problem
Adjusted bug algorithm
Bug M1
• use Bug2 until the robot finds
itself on the S-line farther from
the goal than it started
• if it does, revert to to Bug1 for
that obstacle
Bug1 vs. Bug2
Bug1
Bug2

Exhaustive search

Optimal leave point
Performs better with
complex obstacles
Path length :




n = # of obstacles
Pi = perimeter of
obstacle i
n
LBug1  d (qstart , q goal )  1.5 pi
i 1

Opportunistic
(greedy) search

Performs better with
simple obstacles
Path length :


ni = # of times the
start-goal line
intersects obstacle i
n
LBug 2  d (qstart , q goal )  0.5 ni pi
i 1
Finite range sensor
• Intervals of continuity
Tangent Bug algorithm
• Improvement to the Bug2 algorithm
• Assumptions:
• All assumptions of Bug1/Bug2 except for contact
sensor.
• Finite range sensor with 360◦ infinite orientation
resolution.
Tangent Bug algorithm
• Like Bug1/Bug2, iterates between two behaviors:
• motion to goal – consists of two parts:
• Move in a straight line towards the goal until you sense an
obstacle directly between you and the goal
• Move toward an intermediate point* Oj according to some
heuristic distance** until you reach the goal or until you
reach a local minimum Mi in which case, switch to
boundary following
• * Oj‘s are end points of an interval of continuity
• ** For example d(x, Oj)+ d(Oj,goal)
Tangent Bug algorithm
Motion to goal
o1
o1
o2
t=1
t=2
t=3
t=4
o2
• Tangent Bug algorithm
• boundary following – define two distances:
• dfollowing – The shortest distance between the sensed
boundary and the goal
• dreach – The distance between the point on the boundary
that has a line of sight to the goal, and the goal
•
continue moving around the obstacle in the same
direction until dreach < dfollowing then switch to
motion to goal
Tangent Bug algorithm
Boundary following
Motion to goal
M
goal
Tangent Bug - example
qgoal
qstart
Motion to goal
Boundary following
Bug algorithms
•
•
•
•
•
Simple and intuitive
Straightforward to implement
Success guaranteed (when possible)
Assumes perfect positioning and sensing
Sensor based planning – has to be incremental and
reactive
Multi-Robot Planning
Multi-Robot Planning Examples
Multi-Robot Planning
• An initial and a goal configuration are given as
input for each robot
• Result is a coordinated path between the two
configurations
• A coordinated path is one that indicates the
configuration of every robot at each instant
• Collisions must be avoided between each pair of
robot and obstacles, and between each pair of
robots
Centralized Planning
• Paths for all robots are planned
simultaneously by searching the cspace of the multi-arm robot
• Collisions between robots are selfcollisions of the multi-arm robot
• For spot-welding example, 6
robots each with 6 dofs, so C will
have 36-D
Centralized Planning
• Advantages
• Complete – guaranteed to find a solution if one exists (if the
underlying planner is complete)
• Disadvantages
• Potentially expensive – typically requires searching highdimensional spaces
• Requires knowledge of goals and states of all robots
Decoupled Planning
• First Phase - a collision-free path ti is generated
for each robot considering only obstacles
(ignoring other robots) in its space
Decoupled Planning
• Second Phase (Velocity Tuning) – coordination of
the robots’ velocities along their pre-generated
paths to prevent collisions between robots. Two
coordination methods discussed
• Pairwise Coordination
• Global Coordination
• Each robot is restricted to motion in its pregenerated path although it may stop, retreat or
change velocity to allow coordination with other
robots
Decoupled Planning with Pairwise
Coordination
• The paths t1 and t2 of the first two robots are
coordinated in their 2-dimensional coordination
space
• Results in a collision-free coordinated patht1,2
Done by using planning a path
between (0,0) and (1,1)
Decoupled Planning with Pairwise
Coordination
• The process is repeated for paths t1,2 and t3
resulting in a coordinated path t1,2,3
• Eventually a collision-free coordinate path
t1,2,…,m is generated that defines a valid
coordination of all m robots
Decoupled Planning with Global
Coordination
• The paths of all m robots are coordinated in an mdimensional coordination space
• Results in a collision-free path t1,2,….m
Done by planning a path
from (0,0,0,…) to (1,1,1,…)
Decoupled Planning
• Advantages
• Less expensive than centralized planning because lower
dimensional spaces are searched
• Disadvantages
• Incomplete : Failures usually occur in the second phase as
it might not be possible to coordinate the paths generated in
the first phase without collision between robots
Decoupled Planning
Failure Example
• Initial and goal configurations
Decoupled Planning
Failure Example
• Likely path generation
in 1st phase
Decoupled Planning
Failure Example
• Path coordination fails in second phase
Implemented Planners
• C-SBL – Centralized Planning
• DG-SBL – Decoupled Planning with Global
Coordination
• DP-SBL – Decoupled Planning with Pairwise
Coordination
• Experiments conducted with groups of 2, 4 and 6
robots on 3 separate sets of initial/goal
configurations
PRM Path Planner:
Sampling Strategy
• SBL Planner
• Single-query
• Bi-directional
• Lazy collision-checking
Problem I – Initial and goal
configurations
Problem II – Initial and goal
configurations
Problem III – Initial and goal
configurations
Experimental Results
• T = average running time (seconds)
• DG-SBL and DP-SBL - 20 runs per experiment
• C-SBL – 100 runs per experiment
• F = number of failures
• Maximum of 50,000 milestones allowed per call
to SBL
Experimental Results
• Centralized planning had no failures
• At least one failure suffered in each experiment with
decoupled planning
• Failure rate increased as problems became more
complex
Experimental Results
• pairwise coordination more unreliable than global
coordination
• Failure always occurred in the 2nd stage during path
coordination, a result of wrong path choices made in
the 1st stage
Experimental Results
• Similar running times for both planners in most
experiments
• However, centralized planning required a lot more
time than decoupled planning in 3rd problem with
6 robots
Conclusions
• Reliability – Decoupled planning can be quite
unreliable particularly in tight robot coordination.
Centralized planning appears to have perfect
reliability.
• Planning Time – Using SBL, there is not a huge
difference between the two methods
Conclusions Contd.
• Results invalidate the assumptions that loss of
incompleteness with decoupled planning is fairly
insignificant and can be ignored in practice.
• SBL makes usage of centralized planning for
multi-robot systems practical.
• But centralized planning still requires knowledge
of all robot states, which may be impossible in
some settings.
Sokoban
• Objective of Robot:
To push boxes into their
storage locations without
getting himself or boxes
stuck.
• Rules: Cannot pull, can
push only one box at a
time
Sokoban
Sample Sokoban Game