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 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

The Configuration Space

ConfigurationSpaceofAisthespace(C)ofall
possibleconfigurationsofA.
C

Cfree

qgoal
Cobs

qstart
Forapointrobotmovingin2Dplane,Cspaceis R 2

The Configuration Space

C

Cfree

qgoal
Cobs

qstart
x
Forapointrobotmovingin3D,theC-space is R 3
What is the difference between Euclidean space and Cspace?

The Configuration Space

How to create it
First abstract the robot as a point object.
Then, enlarge the obstacles to account for
the robots footprint and degrees of
freedom
In our example, the robot was circular, so
we simply enlarged our obstacles by the
robots 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

Dont 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:
8:

if LINK(q,q)then
Add an edge between q and q to E.

Difficulty

Many small connected components

Resampling (expansion)

Failure rate

no. failed LINK

r (q)
no. LINK

Weight

r (q)
w(q )
p r ( p)

Resampling probability

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.
2.
3.

Find a shortest path in the roadmap

Check whether the nodes and edges in the path
are collision.
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
Dijkstras 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
WhatareworldsinwhichBug2does
betterthanBug1(andviceversa)?
Bug2beatsBug1

Start

Bug1beatsBug2

head-to-head comparison
WhatareworldsinwhichBug2does
betterthanBug1(andviceversa)?
Bug2beatsBug1

Bug1beatsBug2

Start

zipper
world

Problem

Bug2beatsBug1

Bug1beatsBug2

zipper
world

Problem
Adjustedbugalgorithm

BugM1 useBug2untiltherobotfinds

itselfontheSlinefartherfrom
thegoalthanitstarted
ifitdoes,reverttotoBug1for
thatobstacle

Problem
Adjustedbugalgorithm

BugM1

useBug2untiltherobotfinds
itselfontheSlinefartherfrom
thegoalthanitstarted
ifitdoes,reverttotoBug1for
thatobstacle

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

* Ojs 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 pre-generated

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