Probabilistic Motion Planning

1
Probabilistic Motion Planning

• Shai Hirsch
• May, 2002

2
Outline

• Introduction
• Probabilistic Scheme
• Probabilistic Roadmap Planner
• Analysis
• Improved Solutions

3
Outline

• Introduction
• Probabilistic Scheme
• Probabilistic Roadmap Planner
• Analysis
• Improved Solutions

4
Exact General-Purpose Path Planners
- Schwarz and Sharir, 1983 Exact cell
decomposition based on Collins technique
- Canny, 1987 Silhouette method
5
Complete Solutions

• Small dofs
• Tailored solutions for specific problems.
• Example
• Non convex polygon (k sides)
• Translating and rotating
• Planar polygonal environment (O(n)).
• Constructing F in T((kn)3 log kn)
• Single query in O((kn)2?), for any ? gt 0.

6
Complete Solutions

• Advantages
• Complete
• Specifically, handles narrow / tight passages.
• Bounded running time.
• Behavior well characterized.
• Insight

7
Complete Solutions

• Disadvantages
• High complexity
• Hard to understand
• Hard to implement
• Inflexible to changing scenarios
• (different geometrical settings)

8
Complete Solutions

• Existing complete algorithms become practically
  useless unless dofs is low (between 3 and 5).
• Complete solutions cannot be used for rigid
  objects translating and rotating is 3D.
• Nor for 6-dof manipulator arms.

9
Real Applications

• Many dofs
• Problems may be more complex

10
Extensions to the Basic Problem

• Nonholonomic constraints
• Dynamic constraints
• Optimal planning
• Uncertainty in control and sensing

• Moving obstacles
• Multiple robots
• Movable objects
• Deformable objects
• Goal is to gather data by sensing

11
Application Graphic Animation of Digital Actors
James Kuffners PhD
12
Application Design for Manufacturing
General Motors
General Electric
General Motors
13
Application Computer-Assisted Surgical Planning
Joel Browns PhD
Rhea Tombropouloss PhD
14
Application Prediction of Molecular Motions
Amit Singhs PhD
15
Planner Wish List

• Complete
• Handles narrow / tight passages.
• Bounded running time.
• Behavior well characterized.
• Low complexity
• Not too hard to understand
• Not too hard to implement
• Scalable

16
Outline

• Introduction
• Probabilistic Scheme
• Probabilistic Roadmap Planner
• Analysis
• Improved Solutions

17
Principle of Randomized Planning
(Probabilistic Roadmap)
free space
Kavraki, Svetska, Latombe,Overmars, 95
18
Probabilistic Scheme

• General Idea
• Trade a limited amount of
• completeness
• against a major gain in
• computing efficiency.

19
Probabilistic Scheme

• Probabilistic Completeness
• If a solution path exists,
• the planner will find one
• in bounded time,
• with high probability.
• (weaker completeness but still interesting.)

20
Probabilistic Scheme

• Probabilistic completeness become particularly
  interesting if
• ?
• the running time grows slowly with the
• inverse of the failure probability.

21
Probabilistic Scheme
n dofs
A holonomic robot
C configuration space
Cfree free space
qinit, qgoal query configurations
22
Probabilistic Scheme

• C is the cube 0, 1n.
• Each configuration is an n-tuple
• (q1, , qn).

23
Probabilistic Scheme

• Cfree is implicitly given by a
• function CLEARANCE C??
• Maps any configuration q to the distance of the
  robot placed according to q and
• the obstacles
• two bodies of the robots
• (whichever is smaller)

24
Probabilistic Scheme

• Cfree is implicitly given by a
• function CLEARANCE C??
• For complete planners, we computed an complete
  and explicit representation of Cfree.
• With prms, we only get one glimpse at a time of
  Cfree.

25
Probabilistic Scheme

• CLEARANCE(q) is the clearance of q.
• In case where the robot collides with an obstacle
  or with itself, CLEARANCE(q) returns 0 or a
  negative number.
• CLEARANCE(q) gt 0 implies q ? Cfree .
• (Collision Detection)

26
Probabilistic Scheme

• Maximal Curve Tracing Constant
• ? robot A
• ? positive constant ? s.t.,
• when A moves along a straight line in C between
  any two configurations q and q
• No point of A traces a curve segment longer than
  ??maxqi qi

27
Probabilistic Scheme
from (?1, ?2) to (?1, ?2)
28
Probabilistic Scheme

• Adjacency
• Configurations q, q with clearances ?, ?,
• resp., are adjacent if
• ??maxqi qi lt ½ max ?, ? .

29
Probabilistic Scheme
?
?2
?
?1
30
Probabilistic Scheme

• Clearly, if two configurations are adjacent, then
  the straight line joining them lies entirely in
  Cfree .
• Why the ½ coefficient in
• ??maxqi qi lt ½ max ?, ?
• ?

31
Probabilistic Scheme
d
q
q
? ? d, ? 1 maxqi qi lt d
32
Probabilistic Scheme

• Note that the map
• C?C ? max qi qi
• is in the L? distance in C.
• We could choose other distances in C to define
  adjacency.
• Of course, the constant ? depends on this choice.
• (Choice of Metric)

33
Probabilistic Scheme

• Pick configurations in C at random
• retain configurations in Cfree (along with
• q-init and q-goal) as nodes of graph G
• connect adjacent configurations by links
• of G.
• Return YES if q-init and q-goal belong to the
  same connect component of G.
• Return NO if no path has been found after having
  generated c configurations (c an input parameter).

34
Probabilistic Scheme

• Then answer YES is always correct
• whenever the planner returns YES, there actually
  exists a collision-free path connecting q-init to
  q-goal.

35
Probabilistic Scheme

• Indeed - after a finite amount of computation
  (defined by the parameter c) - the planner may
  still not have found a path between q-init and
  q-goal, even if one exists.

36
Probabilistic Scheme

• The key component
• of this planning scheme is
• the sampling strategy.
• (begin with uniform)

37
Probabilistic Scheme

• We are interested in the multi-query case so we
  will invest preprocessing time to construct a
  roadmap of configurations.

38
Toward an Analysis

• Efficiency may be evaluated through
  experimentation.
• Formal analysis is desirable
• to compare planners
• to stress their strengths and weaknesses.
• Ideally a planner's outcome should be YES with
  the highest probability whenever a free path
  exists.
• This outcome should be generated in minimal time.

39
Toward an Analysis

• Analysis
• relate probability of an incorrect answer to
  running time.
• Given a small positive number ?, bound planner
  running time by a function of ?.

40
Toward an Analysis

• What relation makes a planner good?
• ?
• If function grows slowly with 1/?, the planner is
  particularly interesting.
• We get arbitrarily close to full completeness at
  a reasonable cost.

41
Toward an Analysis

• Assume for simplicity that CLEARANCE takes
  constant time.
• Is that a realistic assumption?
• ?

42
Toward an Analysis

• Assume for simplicity that CLEARANCE takes
  constant time.
• Running time mainly depends on
• ?
• of sample confs.
• of pairs checked for adjacency

43
Toward an Analysis

• sample configurations ? c.
• Only a fraction of them, f, are in Cfree, hence
  in the constructed graph G.
• In practice, often f ?? c.
• The planner should strive to get the greatest
  ration f/c
• But, the distribution of the generated free
  configurations is also crucial to eventually find
  a path.

44
Toward an Analysis

• adjacency checks is at most proportional to f
  2.
• However, the planner may choose not to test all
  pairs of free configurations for adjacency.

45
Toward an Analysis

• It seems likely that no strong property can be
  proven for any given planner, if we do not make
  some assumption about Cfree.
• Moreover, no single planner is likely to be the
  most efficient for all possible problems.
• This suggests that planners should be analyzed
  under some well specified assumptions.

46
Outline

• Introduction
• Probabilistic Scheme
• Probabilistic Roadmap Planner
• Analysis
• Improved Solutions

47
Probabilistic Roadmap Planner

• Preprocessing
• Constructing a network R of configurations
• Query Processing
• Connecting qinit, qgoal to two milestones.
• Checking of these two milestones are in the same
  connected component of R.

48
Probabilistic Roadmap Planner

• Configurations in R are called milestones.
• The milestones are a subset of all sample
  configurations generated.
• (The roadmap is not exactly G from the scheme.)

49
Probabilistic Roadmap Planner

• Planner uses simple algorithm Connect to
  construct the links of R.
• Connector
• Given two milestones m1 and m2, is the
  straight-line segment connecting them in Cfree.
• Dichotomically breaking the segments into shorter
  sub-segments.
• Stops breaking when a pair of milestones are
  adjacent, or if one of them is not free.

50
Probabilistic Roadmap Planner

• For simplicity, assume that connect only tries
  straight paths.
• It could try other canonical paths as well.
• A configuration sees another configuration if the
  straight-line segment joining them lies entirely
  in Cfree.

51
Probabilistic Roadmap Planner

• We limit the number of generated milestones to c.
• We construct an initial roadmap containing r
  milestones.
• We then expand the initial roadmap into a final
  one containing s gt r milestones.
• s and r are input parameters.

52
Probabilistic Roadmap Planner

• The first r milestones are chosen uniformly over
  Cfree.
• The remaining s-r milestones are selected in
  small regions considered difficult parts of
  Cfree.

53
Probabilistic Roadmap Planner

• i ? 0
• While i lt r do
• (a) Pick a configuration q in C at
• random
• (b) If CLEARANCE(q) gt 0 then
• i. Store q as a milestone of the roadmap
• ii. i ? i 1
• For every pair of milestones m1 and m2, whose
  distance is less than d, do Connect(m1, m2).
• Invoke Resample to expand the roadmap by s-r
  milestones.

Input parameter
quadratic in r, usually, r ltlt sampled
54
Probabilistic Roadmap Planner

• Resample selects milestones with probability
  inversely proportional to their degrees in the
  roadmap.
• For each selected milestone, Resample
• picks some new milestones at random in a
  neighborhood of it,
• invokes Connect to try and link each of these new
  milestones to other milestones.
• Terminates total milestones is s.

55
Probabilistic Roadmap Planner

• Roadmaps generated using expansion are
  consistently better
• subsequent queries were processed more quickly
• less incorrect NO answers
• Uniformly sampling ? of the milestones and
  expanding with the remaining ? gave good results.

56
Probabilistic Roadmap Planner

• Query-Processing (q-init, q-goal)
• 1. For i init, goal do
• (a) If there exists a milestone m that sees q(i)
• then m(i) ? m
• (b) Else
• i. Repeat g times
• Pick a configuration q at random in the
• neighborhood of q(i) until q sees both q(i) and
  a
• milestone m.
• ii. If all g trails failed then return NO and
  halt,
• else m(i) ? m
• 2. If m-init and m-goal are in the same component
  of the
• roadmap then return YES else return NO.

57
Outline

• Introduction
• Probabilistic Scheme
• Probabilistic Roadmap Planner
• Analysis
• Improved Solutions

58
Analysis

• Q If roadmap is only a glimpse of Cfree, how do
  we explain experimental success?
• ?
• A There usually exist many collision-free paths
  joining two configurations (!!!)
• Q How realistic is this intuition?
• ?

59
Analysis

• Intuition
• There usually exist many collision-free paths
  joining two configurations
• Capturing assumption
• Cfree satisfies some geometric properties

60
Analysis

• Key number affecting running time -number of
  milestones in the constructed roadmap.
• Single generated milestone maybe several
  randomly picked configurations in C.
• We implicitly assume µ(Cfree ) / µ(C) is not too
  small.
• µ(X) volume of X
• Bounding number of milestones is not sufficient
  does not account for running time of Connect.

61
Visibility Volume Assumption

• For any configuration q in Cfree, let S(q)
  consist of all those configurations q in Cfree
  that q sees.
• Configuration q is ?-good if
• µ(S(q)) ??µ(Cfree)
• Cfree is ?-good if all its configurations are
  ?-good.
• We assume Cfree is ?-good

62
Visibility Volume Assumption

• Q What is the underlying intuition?
• ?
• A It is easy to pick a set of configuration
  that, collectively, can see most of Cfree .

63
Why Does it Work?
64
Visibility Volume Assumption

• Q What does it fail to prevent?

65
Narrow Passage Issue
66
Analysis

• Therefore, we will use a variant of the algorithm
  that embeds a complex planner.
• This complex planner is error-free (discovers a
  path between two confs. whenever one exists).
• Of necessity, complex planner is expensive to
  run.
• Use sparingly (and only to preprocessing).

67
Analysis

• i ? 0
• While i lt r do
• (a) Pick a configuration q in C at
• random
• (b) If CLEARANCE(q) gt 0 then
• i. Store q as a milestone of the roadmap
• ii. i ? i 1
• For every pair of milestones m1 and m2, whose
  distance is less than d, do Connect(m1, m2).

1. Pick a representative milestone from each
   component of the current roadmap. (V set of
   representatives). Invoke Permeation(V) to improve
   the connectivity.

• Invoke Resample to expand the roadmap by s-r
  milestones.

68
Analysis

• The results of preprocessing
• Two nodes are in the same component iff the
• corresponding milestones are in the same
• component of Cfree.
• This is theoretical and assumes Permeation is
  complete. In practice this is not the case.

69
Analysis

• Old-Query-Processing (q-init, q-goal)
• 1. For i init, goal do
• (a) If there exists a milestone m that sees q(i)
• then m(i) ? m
• (b) Else
• i. Repeat g times
• Pick a configuration q at random in the
• neighborhood of q(i) until q sees both q(i) and
  a
• milestone m.
• ii. If all g trails failed then return NO and
  halt,
• else m(i) ? m
• 2. If m-init and m-goal are in the same component
  of the
• roadmap then return YES else return NO.

70
Analysis

• New-Query-Processing (q-init, q-goal)
• 1. For i init, goal do
• (a) If there exists a milestone m that sees q(i)
• then m(i) ? m
• (b) Else
• i. Repeat g times
• Pick a configuration q at random in S(q(i))
• until q sees both q(i) and a milestone m.
• ii. If all g trails failed then return FAILURE
  and
• halt, else m(i) ? m
• 2. If m-init and m-goal are in the same component
  of the
• roadmap then return YES else return NO.

71
PrePro Performance Guarantee

• A set of milestones M is Adequate
• µ(Cfree - S(M)) ½???Cfree
• If each milestone were a source of light, we
  would like a fraction of at least 1-? of Cfree to
  be illuminated.

72
PrePro Performance Guarantee

• Let K be a positive constant large enough that
  for any x?(0, 1,
• (1-x)(K/x)(log1/x log2/?) x?/2
• THEOREM 1
• ??(0, 1 If s (K/?)(log1/? log 2/?), then
  preprocessing generates an adequate set of
  milestones with probability at least 1-?.

73
PrePro Performance Guarantee

• By Theorem 1 most of Cfree is likely to be
  visible from some milestones in the roadmap.
• Queries can be answered quickly.
• Adequacy is not sufficient to imply that the
  roadmap is a good representation of the
  connectivity of Cfree.
• The use of the complex planner is inevitable to
  ensure a good probability that the query
  processing outcomes YES or NO.

74
Query Performance Guarantee

• ? ?0,1 is the failure probability we are
  willing to tolerate during a query.
• g log (2/?)
• THEOREM 2
• If the set of milestones chosen during
  preprocessing is adequate, then the probability
  that the query-processing algorithm outputs
  FAILURE is at most ?.

75
Query Performance Guarantee

• Expected number of sample loop iterations in
  the query-processing algorithm is at most 2.

76
Query Performance Guarantee

• Theorems 1 and 2 fit the multi-query case.
• We blend them for a single query
• We bound the probability for a FAILURE by (1-?)
  ? ?

77
Analysis

• Permeation determines which milestones in V are
  reachable from each other.
• Partitions V into subsets
• All milestones in the same subset belongs to the
  same component of Cfree.
• No two milestones in different subsets are in the
  same component of Cfree.

78
Analysis

• If pV, there are O(p2) invocations of the
  complex planner.
• Far fewer may suffice!

79
Analysis

• Abstract version of Permeation
• Input is a graph N with p vertices
• K disjoint cliques
• determine the clique partition
• Adjacency matrix representation
• cost is measured by the number of examined
  entries
• Edge probe model (used in evasive graph
  properties).

80
Analysis

• NC(p, k) - the non-deterministic complexity of
  this problem.
• Only verify some partition is the right one.
• At least one probe on each of the p vertices.
• Verify each of the pairs (k above 2) of cliques
  is in fact disconnected
• THEOREM 3
• For 1kp, NC(p, k) T(p k2)

81
Analysis

• Deterministic-Permeation(V)
• 1. Mark all vertices of N as being LIVE
• 2. Initialize x ? 1.
• 3. While x p do
• (a) G(x)
• (b) For y x 1 to p do
• i. If vertex y is marked LIVE then probe the edge
  (x, y) in N.
• ii. if edge (x, y) is probed and found present
  the mark y as DEAD and
• add y to G(x).
• (c) Output x Union G(x) as being a clique
• (d) Mark x as being DEAD
• (e) Set x to the smallest numbered LIVE vertex,
  of p1 if there are no
• LIVE vertices left.

82
Analysis

• T(p, k) - worst-case deterministic complexity
• THEOREM 4
• Deterministic-Permeation correctly solves the
  permeation problem using O(pk) probes.
• Theorem 5
• For 1kp, T(p, k) O(pk).

83
Analysis

• Randomized-Permeation(V)
• 1. Permute the vertices randomly. Rename the
  nodes by 1, ..., n in the order of generated
  list.
• 2. Invoke Deterministic-
• Permeation(V).

84
Analysis

• Worst case when all cliques are about p/k
• Worst case is O(pk)
• Best case when there is one giant clique and all
  others are singletons.
• Best case is T(p k2)

85
Other Assumptions

• Path Clearance Assumption
• Expansiveness

86
Outline

• Introduction
• Probabilistic Scheme
• Probabilistic Roadmap Planner
• Analysis
• Improved Solutions

87
Penetration

• Allow some penetration distance of the robot into
  the obstacle (dilated Cfree)
• Push milestones and links into Cfree.
• Rationale
• Dilatation widens narrow passages
• Easier to capture free space connectivity

88
Gaussian

• Rationale
• Samples needed in difficult regions
• We want the probability to depend on the amount
  of forbidden configurations nearby.
• Define a Gaussian on the configuration space.

89
Gaussian

• Avoid computations in the configuration space
  (PRM paradigm).
• In practice
• Sample pairs of configuration such that the
  second is at distance d from the first.
• Discard pairs where both are free or both are
  forbidden.
• From remaining pairs, keep free configuration.

90
Gaussian
91
Gaussian
Uniform Sampling
92
Gaussian
Gaussian Sampling
93
OBPRM

• Obstacle Based Probabilistic Roadmap Method
• Heuristics for generating nodes on or near
  C-obstacles.

94
OBPRM

• Prototype Node-Generation
• 1. cin colliding robot cfg with
• C-obstacle X
• 2. D m random directions emanating out from
• cin
• 3. for each d in D
• cout free cfg in direction d (if exists)
• find contact cfg on (cin,cout) by binary
  search
• endfor

95

• EOF