robot path planning

Robot Path Planning

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

Motion planning is the ability for an agent to compute its own motions in order to achieve

certain goals. All autonomous robots and digital actors should eventually have this ability

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

Compute a collision-free path for a rigid or articulated object among static obstacles

Inputs:•Geometry of moving object and obstacles•Kinematics of moving object (degrees of freedom)•Initial and goal configurations (placements)

Output:Continuous collision-free path connecting the initial and goal configurations

Types of Path Constraints

• Local constraints – Collision-free paths

• Differential constraints– A car cannot move sideways– Have bound curvature

• Global constraints– Shortest or optimal paths

Path Planning

Motion Planning

Non-Holonomic Constraints

v

θ

3 degrees of freedom, but…only 2 control parameters

Not every path in configuration space is valid...

On a Terrain

Planar motion, but 3-dimensional obstacles.

Multiple Robots

2RW

A

Robots translate and rotate

Collisions between robots.

)2,0[R

)2,0[R

)2,0[RC

2

2

2

'A

''A

Moving Obstacles

A

Add time dimension to C.

Only time-monotone paths are allowed

Movable Obstacles

A

Robot can change Cfree!

More

• Huge numbers of degrees of freedom.

• Group motions

• Deformable robots

• …

What is the number of DoF’s?

a polygon robot translating in the plane a polygon robot translating and rotating a spherical robot moving in space a spatial robot translating and rotating a snake robot in the plane with 3 links

Example: Rigid Objects

Piano Mover’s Problem

• 2D or 3D rigid models

• piano.mpg

• Bench6.avi

Example: Articulated Robot

Is it easy?

• Alpha.avi

• Alpha-another-path.avi

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]

Hardness The problem is hard when k is part of the

input [Reif 79], [Hopcroft et al. 84], … [Reif 79]: planning a free path for a robot

made of an arbitrary number of polyhedral bodies connected together at some joint vertices, among a finite set of polyhedral obstacles, between any two given configurations, is a PSPACE-hard problem

Translating rectangles, planar linkages

Complete solutions, I the Piano Movers series [Schwartz-Sharir 83],

cell decomposition: a doubly-exponential

solution, O(nd)3^k) expected time

assuming the robot complexity is constant,

n is the complexity of the obstacles and

d is the algebraic complexity of the problem

Complete solutions, II

roadmap [Canny 87]:

a singly exponential solution,

nk(log n)dO(k^2) expected time

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

Motion planning:the basic problem

Let B be a system (the robot) with k degrees of freedom moving in a known environment cluttered with obstacles. Given free start and goal placements for B decide whether there is a collision free motion for B from start to goal and if so plan such a motion.

Configuration spaceof a robot system with k degrees of freedom

the space of parametric representation of all possible robot configurations

C-obstacles: the expanded obstacles the robot -> a point k dimensional space point in configuration space: free,

forbidden, semi-free path -> curve

[Lozano-Peréz ’79]

Some Examples

Visibility Graphs

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

g

s

Issues With Visibility Graphs

Difficult to extend from point robots to rigid or articulated robots

A L-shaped robot

Framework

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

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

• SilhouetteFirst complete general method that applies to spaces of any dimensions and is singly exponential in the number of dimensions [Canny 1987]

• Probabilistic roadmaps

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

Point robot

www.seas.upenn.edu/~jwk/motionPlanning

Adjacency Graph

• Nodes: cells• Edges: There is an edge between every pair of

nodes whose corresponding cells are adjacent.

Trapezoidal Decomposition

Trapezoidal decomposition

c11

c1

c2

c4

c3

c6

c5c8

c7

c10

c9

c12

c13

c14

c15


Connectivity graph

c1 c10

c2

c3

c4 c5

c6

c7

c8

c9

c11

c12

c13

c14

c15

c11

c1

c2

c4

c3

c6

c5c8

c7

c10

c9

c12

c13

c14

c15


Computational Efficiency

• Running time O(n log n) by planar sweep

• Space O(n)

• Mostly for 2-D configuration spaces

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

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?

• Probabilistic roadmap motion planning– Efficient– Work for complex

problems with many DOF

– Difficult for narrow passages

– May not terminate when no path exists

• Complete motion planning– Always terminate

– Not efficient– Not robust even for

low DOF

Path Non-existence Problem

Obstacle

GoalInitial

Robot

Main Challenge

Obstacle

GoalInitial

Robot

• Exponential complexity: nDOF

– Degree of freedom: DOF– Geometric complexity: n

• More difficult than finding a path– To check all possible paths

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

xyRobot

Start

Goal

Free

Obstacle C-obstacle

ConfigurationSpace Computation

• [Varadhan et al, ICRA 2006]• 2 Translation + 1 Rotation• 215 seconds

Obstacle

Robot

x

y

Obstacles in C-spaceWorkspace Configuration Space

xyRobot

Start

Goal

Free

Obstacle C-obstacle

A 2D Translating Robot

Obstacles in C-space• A configuration q is collision-free, or free, if a

moving object placed at q does not intersect any obstacles in the workspace.

• The free space F is the set of free configurations.

• A configuration space obstacle (C-obstacle) is the set of configurations where the moving object collides with workspace obstacles.

Disc in 2-D Workspaceworkspace configuration

space

Polygonal Robot Translating in 2-D Workspace

workspace configuration space

Articulated Robot in 2-D Workspace

workspace configuration space

Fundamental Questionworkspace configuration space

Are two given points connected by a path?

Problem: Computing C-obstacles

• Input:– Polygonal moving object translating in 2-D

workspace – Polygonal obstacles

• Output: configuration space obstacles represented as polygons

Minkowski Sum

},|{ BbAabaBA

A B

Exercise

AB

A B = ?

Minkowski Sum

},|{ BbAabaBA

Minkowski Sum

Minkowski Sum

},|{ BbAabaBA

Minkowski Sum

• The Minkowski sum of two sets A and B, denoted by AB, is defined as A B = { a+b | a A, bB }

• Similarly, the Minkowski difference is defined as

A – B = { a–b | aA, bB }

p

q

Minkowski Sum of Non-convex Polyhedra

Configuration Space Obstacle• If P is an obstacle in the workspace and M is a

translating object. Then the C-space obstacle corresponding to P is P – M.

P -MObstacle

PRobot

M

C-obstacle

Classic result by Lozano-Perez and Wesley 1979

Minkowski Sum of Convex Polygons

• The Minkowski sum of two convex polygons A and B of m and n vertices respectively is a convex polygon A + B of m + n vertices.– The vertices of A + B are the “sums” of

vertices of A and B.

Complexity of Minkowksi Sum

• 2D convex polygons: O(n+m) • 2D non-convex polygons: O(n2m2)

– Decompose into convex polygons (e.g., triangles or trapezoids), compute the Minkowski sums, and take the union

• 3-D convex polyhedra: O(nm)• 3-D non-convex polyhedra: O(n3m3)

Complexity of Computing C-obstacles

• 3D rigid robots with both translational and rotational DOF– 6D C-space– Arrangement of non-linear surfaces– High combinatorial complexity

• Conclusion– Explicit computation of the boundary of C-obstacle is

difficult and impractical for robots more than 3 DOFs

C-spaceWorkspace Configuration Space

xyRobot

Initial

Goal

Free

Obstacle C-obstacle

A 2D Translating Robot

Computing C-Obstacles

• Difficult due to geometric and space complexity

• Practical solutions are only available for– Translating rigid robots: Minkowski sum– Robots with no more than 3 DOFs

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




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

full mixed

empty

Approximation Cell Decomposition

• Full cell

• Empty cell

• Mixed cell– Mixed– Uncertain

Configuration Space

Connectivity GraphGf : Free Connectivity Graph G: Connectivity Graph

Gf is a subgraph of G

Finding a Path by ACD

Goal

Initial Gf : Free Connectivity Graph

Finding a Path by ACDL: Guiding Path• First Graph Cut Algorithm

– Guiding path in connectivity graph G

– Only subdivide along this path

– Update the graphs G and Gf

Described in Latombe’s book

First Graph Cut Algorithm

Only subdivide along L

L

ACD for Path Non-existence

C-space

Goal

Initial

Connectivity Graph Guiding Path



Connectivity graph is not connected

No path!

Sufficient condition for deciding path non-existence

Two-gear Example

no path!

Cells in C-obstacle

Initial

Goal

Roadmap in F

Video 3.356s

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?

full mixed

empty

Free Cell Query A Collision Detection Problem

•Does the cell lie inside free space?

• Do robot and obstacle separate at all configurations?

Obstacle

WorkspaceConfiguration space

?

Robot

Clearance

• Separation distance– A well studied geometric problem

• Determine a volume in C-space which are completely free

d

C-obstacle QueryAnother Collision Detection Problem

•Does the cell lie inside C-obstacle?

• Do robot and obstacle intersect at all configurations?

Obstacle

WorkspaceConfiguration space

?Robot

‘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




• 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

Pseudo-free edges

Pseudo free edge for two adjacent cells

Goal

Initial

Pseudo-free Connectivity Graph:

Gsf

Goal

Initial

Gsf = Gf + Pseudo-edges

Algorithm

• Gf

• Gsf

• G

Results of Hybrid Planning

Results of Hybrid Planning• 2.5 - 10 times speedup

3 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

Outline

• Approximate cell decomposition

• Sampling-based motion planning

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

Octree decomposition

full mixed

empty

Approximate Cell Decomposition

• Full cell• Empty cell • Mixed cell

– Mixed– Uncertain

• Cell labelling algorithms– [Zhang et al 06]

Configuration Space


Goal

Initial


Goal

Initial Gf : Free Connectivity Graph

Finding a Path by ACDL: Guiding Path• First Graph Cut Algorithm

– Guiding path in connectivity graph G

– Only subdivide along this path

– Update the graphs G and Gf

First Graph Cut Algorithm

Only subdivide the cells along L

L : Guiding Path

new Gf

Finding a Path by ACDGf

• A channel

• Can be used for path smoothing.


C-space

Goal

Initial

Connectivity Graph Guiding Path



Connectivity graph is not connected

No path!

A sufficient condition for deciding path non-existence

• Live Demo– Gear-2DOF– Gear-3DOF

Five-gear Example

Video

Initial

Goal

roadmap in free space

Total timing 85s

# of total cells 168K

Two-gear Example

no path!

Cells in C-obstacle

Initial

Goal

Roadmap in F

Video 3.356s

Motion Planning Framework

• Continuous representation

• Discretization

• Graph search

Summary: Approximate Cell Decomposition

• Simple and easy to implement

• Efficient and practical for low DOF robots – Inefficient for 5 or more DOFs robot

• Resolution-complete– Find a path if there is one– Otherwise, report path non-existence– Up to some resolution of the cell

Outline

• Approximate cell decomposition

• Sampling-based motion planning

Motivation

• Geometric complexity• Space dimensionality

Probabilistic Roadmap (PRM)

free space

qqinitinit

qqgoalgoal

milestone

[Kavraki, Svetska, Latombe,Overmars, 95]

local path

Basic PRM AalgorithmInput: geometry of the moving object & obstaclesOutput: roadmap G = (V, E)

1: V and E .

2: repeat3: q a configuration sampled uniformly at random from C.4: if CLEAR(q)then5: 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)then8: Add an edge between q and q’ to E.

Two Geometric Primitives in C-space• CLEAR(q)

Is configuration q collision free or not?

• LINK(q, q’)

Is the straight-line path between q and q’ collision-free?

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

Two Tenets of PRM Planning Checking sampled configurations and

connections between samples for collision can be done efficiently. Hierarchical collision checking[Hierarchical collision checking methods were developed independently from PRM, roughly at the same time]

A relatively small number of milestones and local paths are sufficient to capture the connectivity of the free space. Exponential convergence in expansive

free space (probabilistic completeness)

Why does it work? Intuition• A small number of milestones almost

“cover” the entire free space.

Two Tenets of PRM Planning Checking sampled configurations and

connections between samples for collision can be done efficiently. Hierarchical collision checking[Hierarchical collision checking methods were developed independently from PRM, roughly at the same time]

A relatively small number of milestones and local paths are sufficient to capture the connectivity of the free space. Exponential convergence in expansive

free space (probabilistic completeness)

Narrow Passage Problem• Narrow passages are difficult to be

sampled due to their small volumes in C-space

Narrow passage

Alpha puzzle

qinitqgoal

2F

3F

Configuration Space

1F

Difficulty• Many small connected components

Strategies to Improve PRM

• Where to sample new milestones?– Sampling strategy

• Which milestones to connect?– Connection strategy

• Goal: – Minimize roadmap size to correctly answer

motion-planning queries

Sampling Strategies

small visibility sets

good visibility

poor visibility

Poor Visibility in Narrow Passages

• Non-uniform sampling strategies

But how to identify poor visibility regions?

• What is the source of information?– Robot and environment geometry

• How to exploit it?– Workspace-guided strategies– Dilation-based strategies– Filtering strategies– Adaptive strategies

Identify narrow passages in the workspace and map them into the configuration space

Workspace-guided strategies

F

O

Dilation-based strategies

• During roadmap construction, allow milestones with small penetration

• Dilate the free space– [Hsu et al. 98, Saha et al. 05, Cheng et al. 06, Zhang et al.

07]

A milestone with small penetration

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.

Weaker Completeness Complete planner Too slow Heuristic planner Too unreliable

Probabilistic completeness:If a solution path exists, then the probability that the planner will find one is a fast growing function that goes to 1 as the running time increases

Kinodynamic Planning

RRT for Kinodynamic Systems

• Rapidly-exploring Random Tree

• Randomly select a control input

More Examples

• Car pulling trailers (complicated kinematics -- no dynamics)

Summary: Sampling-based Motion Planning

+ Efficient in practice + Work for robots with many DOF (high-

dimensional configuration spaces)+ Has been applied for various motion planning

problems (non-holonomic, kinodynamic planning etc.)

- Narrow passages problems (one of the hot areas)

- May not terminate when no path exists

Summary

• Configuration space

• Visibility graph

• Approximate cell decomposition – Decompose the free space into simple cells and represent

the connectivity of the free space by the adjacency graph of these cells

• Sampling-based approach– High-dimensional Configuration Spaces– Capture the connectivity of the free space by sampling

References

• Books– J.C. Latombe. Robot Motion Planning, 1991.– S.M. LaValle, Planning Algorithms, 2006

• Free book: http://msl.cs.uiuc.edu/planning/

– H. Choset et al. Principles of Robot Motion: Theory, Algorithms, and Implementations, 2005

• Conferences– ICRA: IEEE International Conference on Robotics and

Automation– IROS: IEEE/RSJ International Conference on Intelligent RObots

and Systems– WAFR: Workshop on the Algorithmic Foundations of Robotics

robot path planning

Documents