Introduction to Motion Planning

1
Introduction to Motion Planning

• Phillip Coleman
• Algorithms Applications Group
• Parasol Lab, Dept. of Computer Science,
• Texas AM University

2
Outline

• Motion Planning Problem
• Sampling Approaches
• Alternative Approaches
• Differential Constraints
• Extensions and Variations

3
Motion Planning Problem
4
Motion Planning Problem
5
Configuration Space (C-Space)
C-Space

• robot maps to a point in higher
• dimensional space
• parameter for each degree of freedom
• (dof) of robot
• C-space set of all robot placements
• C-obstacle infeasible robot placements

3D C-space (x,y,z)
6D C-space (x,y,z,pitch,roll,yaw)
2n-D C-space (f1, y1, f2, y2, . . . , f n, y n)
6
The Complexity of Motion Planning
Most motion planning problems of interest are
PSPACE-hard

• The best deterministic algorithm known has
  running time that is exponential in the dimension
  of the robots C-space Canny 86
• C-space has high dimension - 6D for rigid body
  in 3-space
• simple obstacles have complex C-obstacles
  impractical to compute explicit
  representation of freespace for more than 4 or 5
  dof

7
Outline

• Motion Planning Problem
• Sampling Approaches
• Alternative Approaches
• Differential Constraints
• Extensions and Variations

8
Sampling Methods
Development of fast Collision Detection Sample
Points in C-space Form Roadmap Weaker form of
Completeness
PRMs Sample configurations and connect using a
simple local planner to build a roadmap.
start
goal
obstacles
9
Sampling Methods
Development of fast Collision Detection Sample
Points in C-space Form Roadmap Weaker form of
Completeness
Tree-based Explore the space from some start
configuration to reach goal configuration
start
goal
obstacles
10
Probabilistic Roadmap Methods (PRMs)
Roadmap represents connectivity of the
workspace Computed only once, multiple
queries Must maintain Accessibility Connectivit
y
11
Probabilistic Roadmap Methods (PRMs)
C-space
Roadmap Construction (Pre-processing)
C-obst
C-obst
C-obst
C-obst
C-obst
12
PRMs Pros Cons
13
Motion Planning
Given an environment (descriptions of moveable
object and obstacles), and start and goal
positions
Find a valid path (continuous sequence of
configurations) from start to goal (e.g., which
avoids collision with obstacles)
14
Basic RRT

• RRT_EXPAND(T, K, ?t)
• for k 1 to K do
• xrand random configuration
• xnear nearest neighbor in T to xrand
• xnew extend xnear toward xrand for step length
• if (edge can be created between xnew xnear)
• T.add_vertex(xnew)
• T.add_edge(xnear, xnew)

x init
15
Basic RRT

• RRT_EXPAND(T, K, ?t)
• for k 1 to K do
• xrand random configuration
• xnear nearest neighbor in T to xrand
• xnew extend xnear toward xrand for step length
• if (edge can be created between xnew xnear)
• T.add_vertex(xnew)
• T.add_edge(xnear, xnew)

x rand
x init
16
Basic RRT

• RRT_EXPAND(T, K, ?t)
• for k 1 to K do
• xrand random configuration
• xnear nearest neighbor in T to xrand
• xnew extend xnear toward xrand for step length
• if (edge can be created between xnew xnear)
• T.add_vertex(xnew)
• T.add_edge(xnear, xnew)

x rand
?
x new
x init
17
Basic RRT

• RRT_EXPAND(T, K, ?t)
• for k 1 to K do
• xrand random configuration
• xnear nearest neighbor in T to xrand
• xnew extend xnear toward xrand for step length
• if (edge can be created between xnew xnear)
• T.add_vertex(xnew)
• T.add_edge(xnear, xnew)

x init
18
Basic RRT
x rand

• RRT_EXPAND(T, K, ?t)
• for k 1 to K do
• xrand random configuration
• xnear nearest neighbor in T to xrand
• xnew extend xnear toward xrand for step length
• if (edge can be created between xnew xnear)
• T.add_vertex(xnew)
• T.add_edge(xnear, xnew)

x init
19
Basic RRT
x rand

• RRT_EXPAND(T, K, ?t)
• for k 1 to K do
• xrand random configuration
• xnear nearest neighbor in T to xrand
• xnew extend xnear toward xrand for step length
• if (edge can be created between xnew xnear)
• T.add_vertex(xnew)
• T.add_edge(xnear, xnew)

x near
x init
20
Basic RRT
x rand

• RRT_EXPAND(T, K, ?t)
• for k 1 to K do
• xrand random configuration
• xnear nearest neighbor in T to xrand
• xnew extend xnear toward xrand for step length
• if (edge can be created between xnew xnear)
• T.add_vertex(xnew)
• T.add_edge(xnear, xnew)

?
x new
x near
x init
21
Basic RRT

• RRT_EXPAND(T, K, ?t)
• for k 1 to K do
• xrand random configuration
• xnear nearest neighbor in T to xrand
• xnew extend xnear toward xrand for step length
• if (edge can be created between xnew xnear)
• T.add_vertex(xnew)
• T.add_edge(xnear, xnew)

22
Outline

• Motion Planning Problem
• Sampling Approaches
• Alternative Approaches
• Differential Constraints
• Extensions and Variations

23
Alternative Approaches
Non sampling approaches to the problem Restricted
to C-Space dimensionality R or R2 Polygonal
Obstacles Combinational Roadmaps
24
Maximum Clearance Roadmap
Stay away from obstacles C-Space R2 3 Different
Types of Curves Compute all curves for all
pairs Intersections become nodes O(n2)
Complexity
25
Vertical Cell Decomposition
Break the environment into Trapezoids or
Triangles Planning within polygon is simple Node
in each polygon and on every boundary Sweeping
Method Sweep along x-axis Create a vertical
boundary when vertices detected. O(n log n)
Complexity
26
Vertical Cell Decomposition

27
Shortest Path Roadmap
A roadmap containing the shortest paths around
obstacles Assumes movement in close proximity to
obstacles Formed by connecting the reflex
vertices of the Cobs Edge must be a bitangent
between vertices O(n2 m) complexity
28
Potential Field Approaches
Requires a Potential Function Rn -gt R Deals with
two forces Attraction (goals) Repulsion
(obstacles) Function forms a gradient that points
towards the goal Gradient Descent used to find
path Cons Able to get stuck in local minimum
29
Outline

• Motion Planning Problem
• Sampling Approaches
• Alternative Approaches
• Differential Constraints
• Extensions and Variations

30
Differential Constraints
Robot can deal with two types of
constraints Global Constraints (Obstacles,
Joint Limits) Local Constraints (Differential
Constraints) acceleration velocity turning
radius Functional Representation x f(q,u) Forms
an X-Space
31
Differential Constraints
X-Space x is in Xobs if q is in Cobs Region of
inevitable collision (momentum) Types of
Differential Constraint Planning Nonholonomic
planning Kinodynamic Planning Trajectory
Planning8
32
Discretization of Constraints
Restricted to X R or X R2 Discretize C, T, or
U (Space, Time, or Action) Discrete Time
Model Time is broken into steps Only a subset
of the Action Space U is considered Action u(t)
is constant during the time step
33
Discretization of Constraints
Reachability Tree From an initial state x apply
all discretized actions Trajectory determined by
integration of f(q,u) Dubins Car Incremental
Simulator (steps through potential actions)
34
Discretization of Constraints
Reachability Tree
35
Decoupled Approach
Split the planning into two steps 1st plan path
(simple motion planning problem) 2nd plan timing
function Search space spanned by (s, s) Path
parameter and first derivative
36
Kinodynamic Planning
Due to complexity many techniques are sampling
based Explore reachability tree Trees are dense
and hard to explore exhaustively Force it to
behave like a lattice Regular cell decomposition
over X Only one node allowed per cell Prunes
the tree
37
Kinodynamic Planning

38
Outline

• Motion Planning Problem
• Sampling Approaches
• Alternative Approaches
• Differential Constraints
• Extensions and Variations

39
Extensions and Variations
Closed Kinematic Chains Robot consists of loops
formed by links A robot holding an object with
two arms Loops are broken into
chains Constrained to configuration where loops
are maintained Deals with a subset of
C-Space PRMs and RRTs can be adapted.
40
Extensions and Variations
Manipulation Planning A robot must interact with
an obstacle Moving a box from one location to
another. Split into discrete and continuous
modes Move towards object Transport
object Switch is triggered by a defined set of
requirements.
41
Extensions and Variations
Time Varying Problem The obstacles or
environment changes over time Adds another
dimension to C-space Requires a directed
Roadmap All paths must move forward in time
42
Extensions and Variations
Multiple Robots Collisions with obstacles and
other robots C-space must encompass all degrees
of freedom C C1, C2, , Cn Centralized
Planning Optimize set of all paths Decentralize
d Planning Each robot plans path Other robots
considered obstacles
43
Conclusion
Motion Planning is a complex problem Numerous
Approaches Applications Many Challenges left to
solve
44
Conclusion
Questions?