Robot Lab Robot Path Planning

- William Regli Department of Computer Science (and

Departments of ECE and MEM) Drexel University

Introduction to Motion Planning

- Applications
- Overview of the Problem
- Basics Planning for Point Robot
- Visibility Graphs
- Roadmap
- Cell Decomposition
- Potential Field

Goals

- Compute motion strategies, e.g.,
- Geometric paths
- Time-parameterized trajectories
- Sequence of sensor-based motion commands
- Achieve high-level goals, e.g.,
- Go to the door and do not collide with obstacles
- Assemble/disassemble the engine
- Build a map of the hallway
- Find and track the target (an intruder, a missing

pet, etc.)

Fundamental Question

Are two given points connected by a path?

Basic Problem

- Problem statement
- Compute a collision-free path for a rigid or

articulated moving object among static obstacles. - Input
- Geometry of a moving object (a robot, a digital

actor, or a molecule) and obstacles - How does the robot move?
- Kinematics of the robot (degrees of freedom)
- Initial and goal robot configurations (positions

orientations) - Output
- Continuous sequence of collision-free robot

configurations connecting the initial and goal

configurations

Example Rigid Objects

Example Articulated Robot

Is it easy?

Hardness Results

- Several variants of the path planning problem

have been proven to be PSPACE-hard. - A complete algorithm may take exponential time.
- A complete algorithm finds a path if one exists

and reports no path exists otherwise. - Examples
- Planar linkages Hopcroft et al., 1984
- Multiple rectangles Hopcroft et al., 1984

Tool Configuration Space

- Difficulty
- Number of degrees of freedom (dimension of

configuration space) - Geometric complexity

Extensions of the Basic Problem

- More complex robots
- Multiple robots
- Movable objects
- Nonholonomic dynamic constraints
- Physical models and deformable objects
- Sensorless motions (exploiting task mechanics)
- Uncertainty in control

Extensions of the Basic Problem

- More complex environments
- Moving obstacles
- Uncertainty in sensing
- More complex objectives
- Optimal motion planning
- Integration of planning and control
- Assembly planning
- Sensing the environment
- Model building
- Target finding, tracking

Practical Algorithms

- A complete motion planner always returns a

solution when one exists and indicates that no

such solution exists otherwise. - Most motion planning problems are hard, meaning

that complete planners take exponential time in

the number of degrees of freedom, moving objects,

etc.

Practical Algorithms

- Theoretical algorithms strive for completeness

and low worst-case complexity - Difficult to implement
- Not robust
- Heuristic algorithms strive for efficiency in

commonly encountered situations. - No performance guarantee
- Practical algorithms with performance guarantees
- Weaker forms of completeness
- Simplifying assumptions on the space

exponential time algorithms that work in

practice

Problem Formulation for Point Robot

- Input
- Robot represented as a point in the plane
- Obstacles represented as polygons
- Initial and goal positions
- Output
- A collision-free path between the initial and

goal positions

Framework

Visibility Graph Method

- Observation If there is a collision-free path

between two points, then there is a polygonal

path that bends only at the obstacles vertices. - Why?
- Any collision-free path can be transformed into a

polygonal path that bends only at the obstacle

vertices. - A polygonal path is a piecewise linear curve.

Visibility Graph

- A visibility graphis a graph such that
- Nodes qinit, qgoal, or an obstacle vertex.
- Edges An edge exists between nodes u and v if

the line segment between u and v is an obstacle

edge or it does not intersect the obstacles.

A Simple Algorithm for Building Visibility Graphs

Computational Efficiency

- Simple algorithm O(n3) time
- More efficient algorithms
- Rotational sweep O(n2log n) time
- Optimal algorithm O(n2) time
- Output sensitive algorithms
- O(n2) space

Framework

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Breadth-First Search

Other Search Algorithms

- Depth-First Search
- Best-First Search, A

Framework

Summary

- Discretize the space by constructing visibility

graph - Search the visibility graph with breadth-first

search - Q How to perform the intersection test?

Summary

- Represent the connectivity of the configuration

space in the visibility graph - Running time O(n3)
- Compute the visibility graph
- Search the graph
- An optimal O(n2) time algorithm exists.
- Space O(n2)
- Can we do better?

Classic Path Planning Approaches

- Roadmap Represent the connectivity of the free

space by a network of 1-D curves - Cell decomposition Decompose the free space

into simple cells and represent the connectivity

of the free space by the adjacency graph of these

cells - Potential field Define a potential function

over the free space that has a global minimum at

the goal and follow the steepest descent of the

potential function

Classic Path Planning Approaches

- Roadmap Represent the connectivity of the free

space by a network of 1-D curves - Cell decomposition Decompose the free space

into simple cells and represent the connectivity

of the free space by the adjacency graph of these

cells - Potential field Define a potential function

over the free space that has a global minimum at

the goal and follow the steepest descent of the

potential function

Roadmap

- Visibility graph
- Shakey Project, SRI Nilsson, 1969
- Voronoi Diagram
- Introduced by computational geometry

researchers. Generate paths that maximizes

clearance. Applicable mostly to 2-D configuration

spaces.

Voronoi Diagram

- Space O(n)
- Run time O(n log n)

Other Roadmap Methods

- Silhouette
- First complete general method that applies to

spaces of any dimensions and is singly

exponential in the number of dimensions Canny

1987 - Probabilistic roadmaps

Classic Path Planning Approaches

- Roadmap Represent the connectivity of the free

space by a network of 1-D curves - Cell decomposition Decompose the free space

into simple cells and represent the connectivity

of the free space by the adjacency graph of these

cells - Potential field Define a potential function

over the free space that has a global minimum at

the goal and follow the steepest descent of the

potential function

Cell-decomposition Methods

- Exact cell decomposition
- The free space F is represented by a collection

of non-overlapping simple cells whose union is

exactly F - Examples of cells trapezoids, triangles

Trapezoidal Decomposition

Computational Efficiency

- Running time O(n log n) by planar sweep
- Space O(n)
- Mostly for 2-D configuration spaces

Adjacency Graph

- Nodes cells
- Edges There is an edge between every pair of

nodes whose corresponding cells are adjacent.

Summary

- Discretize the space by constructing an adjacency

graph of the cells - Search the adjacency graph

Cell-decomposition Methods

- Exact cell decomposition
- Approximate cell decomposition
- F is represented by a collection of

non-overlapping cells whose union is contained in

F. - Cells usually have simple, regular shapes, e.g.,

rectangles, squares. - Facilitate hierarchical space decomposition

Quadtree Decomposition

Octree Decomposition

Algorithm Outline

Classic Path Planning Approaches

- Roadmap Represent the connectivity of the free

space by a network of 1-D curves - Cell decomposition Decompose the free space

into simple cells and represent the connectivity

of the free space by the adjacency graph of these

cells - Potential field Define a potential function

over the free space that has a global minimum at

the goal and follow the steepest descent of the

potential function

Potential Fields

- Initially proposed for real-time collision

avoidance Khatib 1986. Hundreds of papers

published. - A potential field is a scalar function over the

free space. - To navigate, the robot applies a force

proportional to the negated gradient of the

potential field. - A navigation function is an ideal potential field

that - has global minimum at the goal
- has no local minima
- grows to infinity near obstacles
- is smooth

Attractive Repulsive Fields

How Does It Work?

Algorithm Outline

- Place a regular grid G over the configuration

space - Compute the potential field over G
- Search G using a best-first algorithm with

potential field as the heuristic function

Local Minima

- What can we do?
- Escape from local minima by taking random walks
- Build an ideal potential field navigation

function that does not have local minima

Question

- Can such an ideal potential field be constructed

efficiently in general?

Completeness

- A complete motion planner always returns a

solution when one exists and indicates that no

such solution exists otherwise. - Is the visibility graph algorithm complete? Yes.
- How about the exact cell decomposition algorithm

and the potential field algorithm?

Why Complete Motion Planning?

- Complete motion planning
- Always terminate
- Not efficient
- Not robust even for low DOF

- Probabilistic roadmap motion planning
- Efficient
- Work for complex problems with many DOF
- Difficult for narrow passages
- May not terminate when no path exists

Path Non-existence Problem

Obstacle

Obstacle

Main Challenge

- Exponential complexity nDOF
- Degree of freedom DOF
- Geometric complexity n
- More difficult than finding a path
- To check all possible paths

Obstacle

Approaches

- Exact Motion Planning
- Based on exact representation of free space
- Approximation Cell Decomposition (ACD)
- A Hybrid planner

Configuration Space 2D Translation

Workspace

Configuration Space

Goal

Free

Robot

y

x

Start

Configuration Space Computation

- Varadhan et al, ICRA 2006
- 2 Translation 1 Rotation
- 215 seconds

Obstacle

?

y

x

Robot

Exact Motion Planning

- Approaches
- Exact cell decomposition Schwartz et al. 83
- Roadmap Canny 88
- Criticality based method Latombe 99
- Voronoi Diagram
- Star-shaped roadmap Varadhan et al. 06
- Not practical
- Due to free space computation
- Limit for special and simple objects
- Ladders, sphere, convex shapes
- 3DOF

Approaches

- Exact Motion Planning
- Based on exact representation of free space
- Approximation Cell Decomposition (ACD)
- A Hybrid Planner Combing ACD and PRM

Approximation Cell Decomposition (ACD)

- Not compute the free space exactly at once
- But compute it incrementally
- Relatively easy to implement
- Lozano-Pérez 83
- Zhu et al. 91
- Latombe 91
- Zhang et al. 06

Approximation Cell Decomposition

Configuration Space

- Full cell
- Empty cell
- Mixed cell
- Mixed
- Uncertain

Connectivity Graph

Gf Free Connectivity Graph

Gf is a subgraph of G

Finding a Path by ACD

Initial

Goal

Finding a Path by ACD

- First Graph Cut Algorithm
- Guiding path in connectivity graph G
- Only subdivide along this path
- Update the graphs G and Gf

L Guiding Path

Described in Latombes book

First Graph Cut Algorithm

L

Only subdivide along L

Finding a Path by ACD

ACD for Path Non-existence

Initial

Goal

C-space

ACD for Path Non-existence

Connectivity Graph

ACD for Path Non-existence

Connectivity graph is not connected

No path!

Sufficient condition for deciding path

non-existence

Two-gear Example

Video

no path!

3.356s

Initial

Cells in C-obstacle

Roadmap in F

Goal

Cell Labeling

- Free Cell Query
- Whether a cell completely lies in free space?
- C-obstacle Cell Query
- Whether a cell completely lies in C-obstacle?

Free Cell Query A Collision Detection Problem

- Does the cell lie inside free space?

- Do robot and obstacle separate at all

configurations?

Robot

Obstacle

?

Configuration space

Workspace

Clearance

- Separation distance
- A well studied geometric problem
- Determine a volume in C-space which are

completely free

d

C-obstacle Query Another Collision Detection

Problem

- Does the cell lie inside C-obstacle?

- Do robot and obstacle intersect at all

configurations?

Robot

?

Obstacle

Configuration space

Workspace

Forbiddance

- Forbiddance dual to clearance
- Penetration Depth
- A geometric computation problem less investigated
- Zhang et al. ACM SPM 2006

PD

Limitation of ACD

- Combinatorial complexity of cell decomposition
- Limited for low DOF problem
- 3-DOF robots

Approaches

- Exact Motion Planning
- Based on exact representation of free space
- Approximation Cell Decomposition (ACD)
- A Hybrid Planner Combing ACD and PRM

Hybrid Planning

- Probabilistic roadmap motion planning
- Efficient
- Many DOFs
- Narrow passages
- Path non-existence

- Complete Motion Planning
- Complete
- Not efficient

Can we combine them together?

Hybrid Approach for Complete Motion Planning

- Use Probabilistic Roadmap (PRM)
- Capture the connectivity for mixed cells
- Avoid substantial subdivision
- Use Approximation Cell Decomposition (ACD)
- Completeness
- Improve the sampling on narrow passages

Connectivity Graph

Gf Free Connectivity Graph

G Connectivity Graph

Gf is a subgraph of G

Pseudo-free edges

Initial

Goal

Pseudo free edge for two adjacent cells

Pseudo-free Connectivity Graph Gsf

Gsf Gf Pseudo-edges

Initial

Goal

Algorithm

- Gf
- Gsf
- G

Results of Hybrid Planning

Results of Hybrid Planning

Results of Hybrid Planning

- 2.5 - 10 times speedup

3 DOF 3 DOF 4 DOF 4 DOF 4 DOF 4 DOF

timing cells timing cells timing cells

Hybrid 34s 50K 16s 48K 102s 164K

ACD 85s 168K ? ? ? ?

Speedup 2.5 3.3 10 ? 10 ?

Summary

- Difficult for Exact Motion Planning
- Due to the difficulty of free space configuration

computation - ACD is more practical
- Explore the free space incrementally
- Hybrid Planning
- Combine the completeness of ACD and efficiency of

PRM