E190Q – Lecture 14

Autonomous Robot Navigation

Instructor: Chris Clark

Semester: Spring 2014

1 Figures courtesy of Probabilistic Robotics (Thrun et. Al.)

 Control Structures
Planning Based Control

Prior Knowledge Operator Commands

Localization Cognition

Perception Motion Control

2 !
 Introduction to Motion Planning

1.  MP Overview
2.  The Configuration Space
3.  General Approach to MP
4.  Metrics
5.  PRMs
6.  Single Query PRMs

3
 MP Overview

Assembly Planning, Latombe Cross-Firing of a Tumor, Latombe

4 Tomb Raider 3 (Eidos Interactive) Deformable Objects, Kavraki

 MP Overview

§  Goal of robot motion planning:

To construct a collision-free path from some initial

configuration to some goal configuration for a robot
within a workspace containing obstacles.

5
 MP Overview

§  Example:

Goal
Configuration

Trajectory

Start
Configuration

6
 MP Overview

§  Inputs
§  Geometry of robots and obstacles
§  Kinematics/Dynamics of robots
§  Start and Goal configurations

§  Outputs
§  Continuous sequence of configurations connecting
the start and goal configurations

7
 MP Overview

§  Extensions
§  Moving obstacles §  Uncertainty in model,
§  Multiple robots control and sensing
§  Movable objects §  Exploiting task mechanics
§  Assembly planning (under-actuated systems)
§  Goal is to acquire §  Physical models and
information by sensing deformable objects
§  Nonholonomic constraints §  Integration with higher-
level planning
§  Dynamic constraints
§  Stability constraints

8
 Introduction to Motion Planning

1.  MP Overview
2.  The Configuration Space
3.  General Approach to MP
4.  Metrics
5.  PRMs
6.  Single Query PRMs

9
 The Configuration Space

§  To facilitate motion planning, the

configuration space was defined as a tool
that can be used with planning algorithms.

(Latombe 1991)
10
 The Configuration Space

§  A configuration q will completely define the

state of a robot (e.g. mobile robot x, y, θ)
§  The configuration space C, is the space of all
possible configurations of the robot.
§  The free space F C, is the portion of the free
space which is collision-free.

11
 The Configuration Space

§  The goal of motion planning then, is to find a

path in F that connects the initial configuration
qstart to the goal configuration qgoal

12
 The Configuration Space

§  Example 1: 2DOF manipulator:

13
 The Configuration Space

§  Example 2: Mobile Robot

Obstacle
θ

¬F

X
Workspace Configuration Space
14
 The Configuration Space

§  Example 3: Mobile Robot with moving obstacle

Obstacle
θ

¬F
F

X
Workspace Configuration Space
15
 Introduction to Motion Planning

1.  MP Overview
2.  The Configuration Space
3.  General Approach to MP
4.  Metrics
5.  PRMs
6.  Single Query PRMs

16
 General Approach to MP

§  Motion planning is usually done with three

steps:

1.  Define C
2.  Discretize C
3.  Search C

17
 1. Define C

§  Each planning problem may have a different

definition of C.

§  Example 1: Include 3DOF for a mobile robot in

static environment - (x,y,θ).
§  Example 2: Include only 2DOF for a mobile robot
in static environment - (x,y).
§  Example 3: Include 5DOF for a mobile robot in
dynamic environment - (x,y,θ,v,t).
18
 1. Define C

§  Plan paths for a point robot

§  Instead of using a robot of fixed dimensions/size,
“grow” the obstacles to reflect how close the robot
can get.

19
 2. Discretize C

§  Typical Discretizations:

1.  Cell decomposition

2.  Roadmap
3.  Potential field

20
 2. Discretize C

§  Cell decomposition

§  Decompose the free space into simple cells and
represent the connectivity of the free space by the
adjacency graph of these cells

21
 2. Discretize C

§  Roadmap
§  Represent the connectivity of the free space by a
network of 1-D curves

22
 2. Discretize C

§  Potential field

§  Define a function over the free space that has a
global minimum at the goal configuration and
follow its steepest descent

23
 2. Search C

§  Given a discretization of C, a search can be

carried out using a Graph Search or gradient
descent, etc.
§  Example: Find a path from D to G

C D

A B
B E
D E G

F A C F G

24
 Introduction to Motion Planning

1.  MP Overview
2.  The Configuration Space
3.  General Approach to MP
4.  Metrics

25
 Metrics

§  Metrics for which to compare planning

algorithms:
1.  Speed or Complexity
2.  Completeness
3.  Optimality
4.  Feasibility of solutions

26
 Metrics

1. Speed or Complexity
§  Often, planners are compared based on the
running time of an algorithm.
§  Must specify the hardware when reporting, (e.g. processor
type, …)
§  Example:
§  Planner A outperformed Planner B in that it took half the
time to solve the same planning problem.

27
 Metrics

1. Speed or Complexity
§  Planners are also compared based on the
algorithm’s run time complexity
§  i.e. the number of steps or operations an algorithm must
take as a function of the size of the input.

28
 Metrics

1. Speed or Complexity
§  Example: For M particles and N sensors, calculate
the weights assuming expected measurements are
known

for (int i=0; i<M; i++) {

w(i) = 0.0001;
for (int j=0; j<N; j++){
w(i) *= gauss(z-z_exp(i,j));
}
}

29 §  In this example there are on the order of MxN operations, i.e O(MN)
 Metrics

2. Completeness
§  A complete algorithm is one that is guaranteed to
find a solution if one exists, or determine if no
solution exists.

§  Time Consuming!

§  An exhaustive search will search every possible path to
see if it is a feasible solution.
§  A complete planner usually requires exponential time in the
number of degrees of freedom, objects, etc.

30
 Metrics

2. Completeness
§  A resolution complete planner discretizes the
space and returns a path whenever one exists in the
discretized representation.

31 No Solution Solution!
 Metrics

2. Completeness
§  A probabilistically complete planner returns a
path with high probability if a path exists. It may not
terminate if no path exists.

§  E.g. P(failure) è 0 as time è ∞

§  Weaker form of completeness, but usually faster.

32
 Metrics

3. Optimality
§  Resolution of Discretization can lead to sub-optimal
solutions

33
 Metrics

3. Optimality
§  Some algorithms will only guarantee sub-optimal
solutions (e.g. Greedy Search).

34 Sub-Optimal R. Optimal
 Metrics

4. Feasibility of Solutions
§  Not all planners take into account the exact model
of the robot or environment.
§  E.g. Non-differential drive robot

35 Infeasible Feasible
 Metrics

§  We are left with…

§  Theoretical algorithms

§  Strive for completeness and minimal worst-case complexity
§  Difficult to implement

§  Heuristic algorithms

§  Strive for efficiency in common situations
§  Use simplifying assumptions
§  Weaker completeness
§  Exponential algorithms that work in practice

36
 Motion Planning:
Searching the Configuration Space

§  Example: Multi Robot MP

37
 Introduction to Motion Planning

1.  MP Overview
2.  The Configuration Space
3.  General Approach to MP
4.  Metrics
5.  PRMs
6.  Single Query PRMs

38
 Probabilistic Road Maps

§  Definition:
§  A probabilistic road map is a discrete
representation of a continuous configuration space
generated by randomly sampling the free
configurations of the C-space and connecting
those points into a graph.

39
 Probabilistic Road Maps

§  Goal of PRMs:

§  Quickly generate a small roadmap of the Free

Space F that has good coverage and
connectivity

40
 Probabilistic Road Maps

§  PRMS have proven to useful in mapping free

spaces that are difficult to model, or have
many degrees of freedom.
§  This can facilitate fast planning for these situations

§  Trade-off
§  PRMs often sacrifice completeness for speed

41
 Probabilistic Road Maps

42 Moving Objects, Kindel

 Probabilistic Road Maps

§  Two Main Strategies:

1.  Multi-Query:
§  Generate a single roadmap of F which can be used many
times.
2.  Single-Query:
§  Use a new roadmap to characterize the subspace of F
which is relevant to the search problem.

43
 Introduction to Motion Planning

1.  MP Overview
2.  The Configuration Space
3.  General Approach to MP
4.  Metrics
5.  PRMs
6.  Single Query PRMs

44
 Motion Planning:
Probabilistic Road Maps

§  Single-Query PRMs (a.k.a. Rapidly Exploring

Random Trees - RRTs)
§  Try to only sample a subspace of F that is relevant
to the problem.
§  Probabilistically complete assuming C is expansive
[Hsu et. al. 2000].
§  Very fast for many applications (allow for on-the-fly
planning).

45
 Motion Planning:
Probabilistic Road Maps

§  Two approaches:

1.  Single Directional:

§  Grow a milestone tree from start configuration until the
tree reaches the goal configuration

2.  Bi-Directional:
§  Grow two trees, one from the start configuration and one
from the goal configuration, until the two trees meet.
§  Can’t consider time in the configuration space
46
 Single Query PRMs: Outline

1.  Introduction
2.  Algorithm Overview
3.  Sampling strategies

47
 MP Overview

§  Example:

Goal
Configuration

Trajectory

Start
Configuration

48
 Motion Planning

§  Example: Iteration 1

49
 Motion Planning

§  Example: Iteration 2

50
 Motion Planning

§  Example: Iteration 3

51
 Motion Planning

§  Example: Iteration 11

52
 Motion Planning

§  Example: Construct Path

53
 Motion Planning

§  Example: Construct Path

54
 Probabilistic Road Maps:
Learning Phase

§  Nomenclature

R=( N, E ) RoadMap
N Set of Nodes
E Set of edges
c Configuration
e edge

55
 Motion Planning:
Probabilistic Road Maps

§  Algorithm
1.  Add start configuration cstart to R( N, E )
2.  Loop
3.  Randomly Select New Node c to expand
4.  Randomly Generate new Node c’ from c
5.  If edge e from c to c’ is collision-free
6.  Add (c’, e) to R
7.  If c’ belongs to endgame region, return path
8.  Return if stopping criteria is met
56
 Single Query PRMs: Outline

1.  Introduction
2.  Algorithm Overview
3.  Sampling strategies
§  Node Selection (step 3)
§  Node Generation (step 4)
§  Endgame Region (step 7)

57
 Motion Planning:
PRM Node Selection

§  One could pick the next node for expansion

by picking from all nodes in the roadmap with
equal probability.

§  This is easy to implement, but leads to poor

expansion Clustering

58
 Motion Planning:
PRM Node Selection

§  Cont’

§  Method is to weight the random selection of nodes

to expand, this can greatly affect the roadmap
coverage of the configuration space.
§  Want to pick nodes with probability proportional to
the inverse of node density.

59
 Motion Planning:
PRM Node Selection

§  Example:
§  Presented is a 2DOF configuration space where
the initial node in the roadmap is located in the
upper right corner.
§  After X iterations, the roadmap produced from an
unweighted expansion has limited coverage.

60 Unweighted Weighted
 Motion Planning:
PRM Node Selection Technique 1

§  The workspace was divided up into cells to

form a grid [Kindel 2000].
§  Algorithm:
1.  Randomly pick an occupied cell from the grid.
2.  Randomly pick a milestone in that cell.

61
 Motion Planning:
PRM Node Selection Technique 2

§  Commonly used in Rapidly exploring Random

Trees (RRTs) [Lavalle]
§  Algorithm:
1.  Randomly pick configuration crand from C.
2.  Find node c from R that is closest to node crand
3.  Expand from c in the direction of crand

62
 Single Query PRMs: Outline

1.  Introduction
2.  Algorithm Overview
3.  Sampling strategies
§  Node Selection (step 3)
§  Node Generation (step 4)
§  Endgame Region (step 7)

63
 Motion Planning:
PRM Milestone Generation

§  Use random control inputs to propagate robot

from previous node c to new configuration c’

§  Algorithm:
1.  Randomly select controls u and Δt
2.  Use known dynamics/kinematics equation f of robot to
generate new configuration
c’ = f (c, u, Δt)
3.  If path from c to c’ is collision-free, then add c’ to R

64
 Motion Planning:
PRM Milestone Generation

§  Example: Differential drive robot

1. 
. .
Randomly select controls φ , φ and Δt
left right
2.  Propagate:
1.  Get Δsleft and Δsright
2.  Calculate new state c’ with:

c’

3.  Use iterative search to check for collisions on path.

65
 Motion Planning:
PRM Milestone Generation

§  Example: Differential drive robot (cont’)

§  Iterative Collision checking is simple but not
always efficient:
§  Algorithm:
1.  Calculate distance d to nearest obstacle
2.  Propagate forward distance d along path from c to c’
3.  If d is too small, return collision
4.  If c reaches or surpasses c’, return collision-free
d d d
d
d d d c’
66 c d
 Single Query PRMs: Outline

1.  Introduction
2.  Algorithm Overview
3.  Sampling strategies
§  Node Selection (step 3)
§  Node Generation (step 4)
§  Endgame Region (step 7)

67
 Motion Planning:
PRM Endgame Region

§  We define the endgame region E, to be the set

of configurations that have a simple
connection to the goal configuration.
§  For each planning problem, we can define a unique
method of making simple connections.
§  This method will inherently define E.
goal
F configuration
C
E

68
 Motion Planning:
PRM Endgame Region

§  Given the complexity of most configuration

spaces, it is very difficult to model E.

§  In practice, we develop a simple admissibility test to

calculate if a configuration c’ belongs to the E
§  At every iteration of the algorithm, this test is used
to determine if newly generated configurations are
connected to the goal configuration.

69
 Motion Planning:
PRM Endgame Region

§  In defining E, we need two things for good

performance:

1.  The region E should be large: this increases the

chance that a newly generated milestone will
belong to E and provide us a solution.
2.  The admissibility test to be as fast as possible. This
test is conducted at every iteration of the algorithm
and will greatly affect the algorithm running time.

70
 Motion Planning:
PRM Endgame Region

§  Several endgame definitions exist:

1.  The set of all configurations within some radius r of
the goal configuration

71
 Motion Planning:
PRM Endgame Region

§  Several endgame definitions exist:

1.  The set of all configurations within some radius r of
the goal configuration
2.  The set of all configurations that have “simple”,
collision-free connection with the goal configuration.
§  Example: Use circular arc for differential drive robots.

cgoal
c’ C’’

72