Robot Lab: Robot Path Planning William Regli Department of Computer Science (and Departments of ECE and MEM) Drexel University

Introduction to Motion Planning:

Introduction to Motion Planning
Applications
Overview of the Problem
Basics – Planning for Point Robot
Visibility Graphs
Roadmap
Cell Decomposition
Potential Field

Goals:

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:

Fundamental Question Are two given points connected by a path?

Basic Problem:

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: Rigid Objects

Example: Articulated Robot:

Example: Articulated Robot

Is it easy?:

Is it easy?

Hardness Results:

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:

Tool: Configuration Space
Difficulty
Number of degrees of freedom (dimension of configuration space)
Geometric complexity

Extensions of the Basic Problem:

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:

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:

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:

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:

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:

Framework

Visibility Graph Method:

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:

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:

A Simple Algorithm for Building Visibility Graphs

Computational Efficiency:

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:

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

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:

Other Search Algorithms Depth-First Search
Best-First Search, A*

Framework:

Framework

Summary:

Summary Discretize the space by constructing visibility graph
Search the visibility graph with breadth-first search
Q: How to perform the intersection test?

Summary:

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:

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:

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:

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:

Voronoi Diagram
Space O(n)
Run time O(n log n)

Other Roadmap Methods:

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:

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:

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:

Trapezoidal Decomposition

Computational Efficiency:

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

Adjacency Graph:

Adjacency Graph Nodes: cells
Edges: There is an edge between every pair of nodes whose corresponding cells are adjacent.

Summary:

Summary Discretize the space by constructing an adjacency graph of the cells
Search the adjacency graph

Cell-decomposition Methods:

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:

Quadtree Decomposition

Octree Decomposition:

Octree Decomposition

Algorithm Outline:

Algorithm Outline

Classic Path Planning Approaches:

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:

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 :

Attractive & Repulsive Fields

How Does It Work?:

How Does It Work?

Algorithm Outline:

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:

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:

Question
Can such an ideal potential field be constructed efficiently in general?

Completeness:

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

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:

Path Non-existence Problem Obstacle Obstacle

Main Challenge:

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

Approaches:

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

Configuration Space: 2D Translation:

Configuration Space: 2D Translation Workspace Configuration Space x y Robot Start Goal Free

Configuration Space Computation:

Configuration Space Computation [Varadhan et al, ICRA 2006]
2 Translation + 1 Rotation
215 seconds Obstacle Robot x y

Exact Motion Planning:

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:

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

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:

Approximation Cell Decomposition Full cell
Empty cell
Mixed cell
Mixed
Uncertain
Configuration Space

Connectivity Graph:

Connectivity Graph Gf : Free Connectivity Graph Gf is a subgraph of G

Finding a Path by ACD:

Finding a Path by ACD Goal Initial

Finding a Path by ACD:

Finding a Path by ACD L: 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:

First Graph Cut Algorithm Only subdivide along L L

Finding a Path by ACD:

Finding a Path by ACD

ACD for Path Non-existence:

ACD for Path Non-existence C-space Goal Initial

ACD for Path Non-existence:

Connectivity Graph ACD for Path Non-existence

ACD for Path Non-existence:

ACD for Path Non-existence Connectivity graph is not connected No path! Sufficient condition for deciding path non-existence

Two-gear Example:

Two-gear Example no path! Cells in C-obstacle Initial Goal Roadmap in F Video 3.356s

Cell Labeling:

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:

Free Cell Query A Collision Detection Problem Does the cell lie inside free space? Do robot and obstacle separate at all configurations?
Obstacle Workspace Configuration space ? Robot

Clearance :

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?
Obstacle Workspace Configuration space ? Robot

‘Forbiddance’:

‘Forbiddance’
‘Forbiddance’: dual to clearance
Penetration Depth
A geometric computation problem less investigated
[Zhang et al. ACM SPM 2006] PD

Limitation of ACD:

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

Approaches:

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

Hybrid Planning:

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 :

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:

Connectivity Graph Gf : Free Connectivity Graph G: Connectivity Graph Gf is a subgraph of G

Pseudo-free edges:

Pseudo-free edges
Pseudo free edge for two adjacent cells Goal Initial

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

You do not have the permission to view this presentation. In order to view it, please
contact the author of the presentation.