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

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

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

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)

Configuration Space Accommodate Robot Size

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? - Several variants of the path planning problem

have been proven to be PSPACE-hard.

10d

Probabilistic Roadmap (PRM) multiple queries

free space

Kavraki, Svetska, Latombe,Overmars, 96

Probabilistic Roadmap (PRM) single query

Multiple-Query PRM

Classic multiple-query PRM

- Probabilistic Roadmaps for Path Planning in

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

PRM - Learning Phase

PRM - Learning Phase

PRM - Learning Phase

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

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

- 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

Bug2 - example

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

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

zipper world

Start

Problem

Bug 2 beats Bug 1

Bug 1 beats Bug 2

zipper world

Problem

- 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

Bug M1

Adjusted bug algorithm

Problem

- 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

Bug M1

Adjusted bug algorithm

Bug1 vs. Bug2

- Bug1
- Exhaustive search
- Optimal leave point
- Performs better with complex obstacles
- Path length
- n of obstacles
- Pi perimeter of obstacle i

- Bug2
- Opportunistic (greedy) search
- Performs better with simple obstacles
- Path length
- ni of times the start-goal line intersects

obstacle i

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

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 lt dfollowing then switch

to motion to goal

Tangent Bug algorithm

Boundary following

Motion to goal

goal

M

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 c-space of the multi-arm robot - Collisions between robots are self-collisions 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 high-dimensional 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 pathbetween (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

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