Robot Lab: Robot Path Planning

1
Robot LabRobot Path Planning

• William RegliDepartment of Computer Science(and
  Departments of ECE and MEM)Drexel University

2
Introduction to Motion Planning

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

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

4
Fundamental Question
Are two given points connected by a path?
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

6
Example Rigid Objects
7
Example Articulated Robot
8
Is it easy?
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

10
Tool Configuration Space

• Difficulty
• Number of degrees of freedom (dimension of
  configuration space)
• Geometric complexity

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

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

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.

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

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

16
Framework
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.

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.

19
A Simple Algorithm for Building Visibility Graphs
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

21
Framework
22
Breadth-First Search
23
Breadth-First Search
24
Breadth-First Search
25
Breadth-First Search
26
Breadth-First Search
27
Breadth-First Search
28
Breadth-First Search
29
Breadth-First Search
30
Breadth-First Search
31
Breadth-First Search
32
Other Search Algorithms

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

33
Framework
34
Summary

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

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?

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

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

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.

39
Voronoi Diagram

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

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

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

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

43
Trapezoidal Decomposition
44
Computational Efficiency

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

45
Adjacency Graph

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

46
Summary

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

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

48
Quadtree Decomposition
49
Octree Decomposition
50
Algorithm Outline
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

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

53
Attractive Repulsive Fields
54
How Does It Work?
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

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

57
Question

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

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?

59
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

60
Path Non-existence Problem
Obstacle
Obstacle
61
Main Challenge

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

Obstacle
62
Approaches

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

63
Configuration Space 2D Translation
Workspace
Configuration Space
Goal
Free
Robot
y
x
Start
64
Configuration Space Computation

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

Obstacle
?
y
x
Robot
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

66
Approaches

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

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

68
Approximation Cell Decomposition
Configuration Space

• Full cell
• Empty cell
• Mixed cell
• Mixed
• Uncertain

69
Connectivity Graph
Gf Free Connectivity Graph
Gf is a subgraph of G
70
Finding a Path by ACD
Initial
Goal
71
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
72
First Graph Cut Algorithm
L
Only subdivide along L
73
Finding a Path by ACD
74
ACD for Path Non-existence
Initial
Goal
C-space
75
ACD for Path Non-existence
Connectivity Graph
76
ACD for Path Non-existence
Connectivity graph is not connected
No path!
Sufficient condition for deciding path
non-existence
77
Two-gear Example
Video
no path!
3.356s
Initial
Cells in C-obstacle
Roadmap in F
Goal
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?

79
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
80
Clearance

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

d
81
C-obstacle QueryAnother Collision Detection
Problem

• Does the cell lie inside C-obstacle?

• Do robot and obstacle intersect at all
  configurations?

Robot
?
Obstacle
Configuration space
Workspace
82
Forbiddance

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

PD
83
Limitation of ACD

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

84
Approaches

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

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

87
Connectivity Graph
Gf Free Connectivity Graph
G Connectivity Graph
Gf is a subgraph of G
88
Pseudo-free edges
Initial
Goal
Pseudo free edge for two adjacent cells
89
Pseudo-free Connectivity Graph Gsf
Gsf Gf Pseudo-edges
Initial
Goal
90
Algorithm

• Gf
• Gsf
• G

91
Results of Hybrid Planning
92
Results of Hybrid Planning
93
Results of Hybrid Planning

• 2.5 - 10 times speedup

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