Robot Lab: Robot Path Planning

Slide 1

Robot Lab:Robot Path Planning

William RegliDepartment of Computer Science

(and Departments of ECE and MEM)Drexel University

Slide 2

Introduction to Motion Planning

• Applications• Overview of the Problem• Basics – Planning for Point Robot

– Visibility Graphs– Roadmap– Cell Decomposition– Potential Field

Slide 3

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

Slide 4

Fundamental Question

Are two given points connected by a path?

Slide 5

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

Slide 6

Example: Rigid Objects

Slide 7

Example: Articulated Robot

Slide 8

Is it easy?

Slide 9

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]

Slide 10

Tool: Configuration Space

Difficulty– Number of degrees of freedom (dimension of

configuration space)– Geometric complexity

Slide 11

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

Slide 12

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

Slide 13

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.

Slide 14

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

Slide 15

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

Slide 16

Framework

Slide 17

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.

Slide 18

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.

Slide 19

A Simple Algorithm for Building Visibility Graphs

Slide 20

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

Slide 21

Framework

Slide 22

Breadth-First Search

Slide 23

Breadth-First Search

Slide 24

Breadth-First Search

Slide 25

Breadth-First Search

Slide 26

Breadth-First Search

Slide 27

Breadth-First Search

Slide 28

Breadth-First Search

Slide 29

Breadth-First Search

Slide 30

Breadth-First Search

Slide 31

Breadth-First Search

Slide 32

Other Search Algorithms

• Depth-First Search

• Best-First Search, A*

Slide 33

Framework

Slide 34

Summary• Discretize the space by constructing

visibility graph

• Search the visibility graph with breadth-first search

Q: How to perform the intersection test?

Slide 35

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?

Slide 36

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

Slide 37

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

Slide 38

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.

Slide 39

Voronoi Diagram

• Space O(n)

• Run time O(n log n)

Slide 40

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

Slide 41

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

Slide 42

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

Slide 43

Trapezoidal Decomposition

Slide 44

Computational Efficiency

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

• Space O(n)

• Mostly for 2-D configuration spaces

Slide 45

Adjacency Graph

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

nodes whose corresponding cells are adjacent.

Slide 46

Summary

• Discretize the space by constructing an adjacency graph of the cells

• Search the adjacency graph

Slide 47

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

Slide 48

Quadtree Decomposition

Slide 49

Octree Decomposition

Slide 50

Algorithm Outline

Slide 51

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

Slide 52

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

Slide 53

Attractive & Repulsive Fields

Slide 54

How Does It Work?

Slide 55

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

Slide 56

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

Slide 57

Question

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

Slide 58

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?

Slide 59

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

Slide 60

Path Non-existence Problem

ObstacleObstacle

GoalInitial

Robot

Slide 61

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

Slide 62

Approaches• Exact Motion Planning

– Based on exact representation of free space

• Approximation Cell Decomposition (ACD)

• A Hybrid planner

Slide 63

Configuration Space: 2D Translation

Workspace Configuration Space

xyRobot

Start

Goal

Free

Obstacle C-obstacle

Slide 64

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

Obstacle

Robot

x

y

Slide 65

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

Slide 66

Approaches• Exact Motion Planning

– Based on exact representation of free space

• Approximation Cell Decomposition (ACD)

• A Hybrid Planner Combing ACD and PRM

Slide 67

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]

Slide 68

full mixed

empty

Approximation Cell Decomposition

• Full cell

• Empty cell

• Mixed cell– Mixed– Uncertain

Configuration Space

Slide 69

Connectivity GraphGf : Free Connectivity Graph G: Connectivity Graph

Gf is a subgraph of G

Slide 70

Finding a Path by ACD

Goal

Initial Gf : Free Connectivity Graph

Slide 71

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

Slide 72

First Graph Cut Algorithm

Only subdivide along L

L

Slide 73

Finding a Path by ACD

Slide 74

ACD for Path Non-existence

C-space

Goal

Initial

Slide 75

Connectivity Graph Guiding Path

ACD for Path Non-existence

Slide 76

ACD for Path Non-existence

Connectivity graph is not connected

No path!

Sufficient condition for deciding path non-existence

Slide 77

Two-gear Example

no path!

Cells in C-obstacle

Initial

Goal

Roadmap in F

Video 3.356s

Slide 78

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

Slide 79

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

Slide 80

Clearance

• Separation distance– A well studied geometric problem

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

d

Slide 81

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

Slide 82

‘Forbiddance’

• ‘Forbiddance’: dual to clearance• Penetration Depth

– A geometric computation problem less investigated

• [Zhang et al. ACM SPM 2006]

PD

Slide 83

Limitation of ACD

• Combinatorial complexity of cell decomposition

• Limited for low DOF problem– 3-DOF robots

Slide 84

Approaches• Exact Motion Planning

– Based on exact representation of free space

• Approximation Cell Decomposition (ACD)

• A Hybrid Planner Combing ACD and PRM

Slide 85

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?

Slide 86

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

Slide 87

Connectivity GraphGf : Free Connectivity Graph G: Connectivity Graph

Gf is a subgraph of G

Slide 88

Pseudo-free edges

Pseudo free edge for two adjacent cells

Goal

Initial

Slide 89

Pseudo-free Connectivity Graph:

Gsf

Goal

Initial

Gsf = Gf + Pseudo-edges

Slide 90

Algorithm

• Gf

• Gsf

• G

Slide 91

Results of Hybrid Planning

Slide 92

Results of Hybrid Planning

Slide 93

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 ?

Slide 94

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