a survey of differentially constrained planning
DESCRIPTION
A Survey of Differentially Constrained Planning. Mihail Pivtoraiko. Motion Planning. Stanford Cart, 1979. The Challenge: Reliable Autonomous Robots. ALV, 1988. NavLab , 1985. XUV, 1998. Crusher, 2006. Boss, 2007. MER, 2004. Agenda. Deterministic planners Path smoothing - PowerPoint PPT PresentationTRANSCRIPT
![Page 1: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/1.jpg)
1
A Survey ofDifferentially Constrained
PlanningMihail Pivtoraiko
![Page 2: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/2.jpg)
2
Motion PlanningThe Challenge:Reliable Autonomous Robots
NavLab, 1985
Boss, 2007MER, 2004Crusher, 2006
ALV, 1988
XUV, 1998
Stanford Cart, 1979
![Page 3: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/3.jpg)
Agenda
3
• Deterministic planners– Path smoothing– Control sampling– State sampling
• Randomized planners– Probabilistic roadmaps– Rapidly exploring Random
Trees
![Page 4: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/4.jpg)
Motivation
4
Local
Global
ALV (Daily et al., 1988)
![Page 5: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/5.jpg)
Unstructured Environments
5
Local
Global
• Structure imposed:– Regular, fine grid– Standard search (A*)
![Page 6: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/6.jpg)
6
Global
?
Unstructured and Uncertain• Uncertain terrain– Potentially changing
Local
![Page 7: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/7.jpg)
7
Global
?
Unstructured and Uncertain• Unseen obstacles– Detected up close– Invalidate the plan
Local
![Page 8: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/8.jpg)
8
Global
?
Unstructured and Uncertain
Efficient replanning
D*/Smarty (Stentz & Hebert, 1994)Ranger (Kelly, 1995)Morphin (Simmons et al., 1996)Gestalt (Maimone et al., 2002)
Local
![Page 9: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/9.jpg)
9
Mobility Constraints• 2D global planners lead to
nonconvergence in difficult environments
• Robot will fail to make the turn into the corridor
• Global planner must understand the need to swing wide
• Issues:– Passage missed, or– Point-turn is necessary…
Plan Step n
Plan Step n+1
Plan Step n+2
![Page 10: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/10.jpg)
10
In the Field…PerceptOR/UPI, 2005 Rover Navigation, 2008
![Page 11: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/11.jpg)
11
Mobility Constraints• Problem:
– Mal-informed global planner– Vehicle constraints ignored:
Heading
• Potential solutions:– Rapidly-Exploring Random Tree (RRT)
[LaValle & Kuffner, 2001]– PDST-EXPLORE
[Ladd & Kavraki, 2004]– Deterministic motion sampling
[Barraquand & Latombe, 1993]
LaValle & Kuffner, 2001
![Page 12: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/12.jpg)
12
Mobility Constraints
Bruntingthorpe Proving Grounds Leicestershire, UK
April 1999
![Page 13: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/13.jpg)
13
Extreme Maneuvering
Kolter et al., 2010
![Page 14: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/14.jpg)
14
Arbitrary…
![Page 15: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/15.jpg)
15
Dynamics Planning
![Page 16: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/16.jpg)
16
Definitions• State space
x , y, z, , , x , y, z, , ,
v
![Page 17: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/17.jpg)
17
Definitions• State space• Control space– Accelerator– Steering
x , y, z, , , x , y, z, , ,
v
![Page 18: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/18.jpg)
18
Definitions• State space• Control space• Feasibility– Satisfaction of
differential constraints– General formulation x = f(x, u, t)
![Page 19: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/19.jpg)
19
Definitions• State space• Control space• Feasibility• Partially-known
environment– Sampled perception map
![Page 20: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/20.jpg)
20
Definitions• State space• Control space• Feasibility• Partially-known
environment– Sampled perception map– Local, changing info
![Page 21: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/21.jpg)
21
Definitions• Motion Planning– Given two states, compute control sequence– Qualities• Feasibility• Optimality• Runtime• Completeness
![Page 22: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/22.jpg)
22
Definitions• Motion Planning• Dynamic Replanning– Capacity to “repair” the plan– Improves reaction time
![Page 23: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/23.jpg)
23
Definitions• Motion Planning• Dynamic Replanning• Search Space– Set of motion alternatives– Unstructured environments Sampling• State space• Control space
![Page 24: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/24.jpg)
24
Definitions• Motion Planning• Dynamic Replanning• Search Space• Deterministic sampling– Fixed pattern, predictable– “Curse of dimensionality”
![Page 25: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/25.jpg)
25
Definitions• Search space design– Input: robot properties– Output: state, control sampling– Maximize planner qualities (F, O, R, C)
• Design principle– Sampling rule
![Page 26: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/26.jpg)
26
Outline
• Introduction• Deterministic Planning
– Hierarchical– Path Smoothing– Control Sampling– State Sampling
• Randomized Planning– PRMs– RRTs
• Derandomized Planners• Some applications
![Page 27: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/27.jpg)
27
Local/Global
• Pros:– Local motion evaluation is fast – In sparse obstacles, works very well
Blocked motionsFree motions
Perception horizon
ALV (Daily et al., 1988)D*/Smarty (Stentz & Hebert, 1994)Ranger (Kelly, 1995)Morphin (Krotkov et al., 1996)Gestalt (Goldberg, Maimone & Matthies, 2002)
![Page 28: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/28.jpg)
28
Local/Global
• Pros:– Local motion evaluation is fast – In sparse obstacles, works very well
• Cons:– Search space differences
ALV (Daily et al., 1988)D*/Smarty (Stentz & Hebert, 1994)Ranger (Kelly, 1995)Morphin (Krotkov et al., 1996)Gestalt (Goldberg, Maimone & Matthies, 2002)
![Page 29: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/29.jpg)
29
Local/Global
• Pros:– Local motion evaluation is fast – In sparse obstacles, works very well
• Cons:– Search space differences
ALV (Daily et al., 1988)D*/Smarty (Stentz & Hebert, 1994)Ranger (Kelly, 1995)Morphin (Krotkov et al., 1996)Gestalt (Goldberg, Maimone & Matthies, 2002)
![Page 30: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/30.jpg)
30
Local/Global
• Pros:– Local motion evaluation is fast – In sparse obstacles, works very well
• Cons:– Search space differences
ALV (Daily et al., 1988)D*/Smarty (Stentz & Hebert, 1994)Ranger (Kelly, 1995)Morphin (Krotkov et al., 1996)Gestalt (Goldberg, Maimone & Matthies, 2002)
![Page 31: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/31.jpg)
31
Local/Global
• Pros:– Local motion evaluation is fast – In sparse obstacles, works very well
• Cons:– Search space differences
ALV (Daily et al., 1988)D*/Smarty (Stentz & Hebert, 1994)Ranger (Kelly, 1995)Morphin (Krotkov et al., 1996)Gestalt (Goldberg, Maimone & Matthies, 2002)
![Page 32: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/32.jpg)
32
Local/Global
• Pros:– Local motion evaluation is fast – In sparse obstacles, works very well
• Cons:– Search space differences
ALV (Daily et al., 1988)D*/Smarty (Stentz & Hebert, 1994)Ranger (Kelly, 1995)Morphin (Krotkov et al., 1996)Gestalt (Goldberg, Maimone & Matthies, 2002)
![Page 33: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/33.jpg)
33
Local/Global
• Pros:– Local motion evaluation is fast – In sparse obstacles, works very well
• Cons:– Search space differences
ALV (Daily et al., 1988)D*/Smarty (Stentz & Hebert, 1994)Ranger (Kelly, 1995)Morphin (Krotkov et al., 1996)Gestalt (Goldberg, Maimone & Matthies, 2002)
![Page 34: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/34.jpg)
34
Egograph
• Local/Global arrangement
• Imposing Discretization
• Pre-computed search space– Tree depth: 5– 17 state samples per level– 7 segments– 4 velocities– 19 curvatures
Lacaze et al., 1998
![Page 35: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/35.jpg)
35
Outline
• Introduction• Deterministic Planning
– Hierarchical– Path Smoothing– Control Sampling– State Sampling
• Randomized Planning– PRMs– RRTs
• Derandomized Planners• Some applications
![Page 36: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/36.jpg)
36
Path Post-Processing
Lamiraux et al., 2002
Laumond, Jacobs, Taix, Murray, 1994
Khatib, Jaouni, Chatila, Laumond, 1997
![Page 37: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/37.jpg)
37
Topological Property
![Page 38: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/38.jpg)
38
Outline
• Introduction• Deterministic Planning
– Hierarchical– Path Smoothing– Control Sampling– State Sampling
• Randomized Planning– PRMs– RRTs
• Derandomized Planners• Some applications
![Page 39: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/39.jpg)
39
Control Space Sampling
Barraquand & Latombe, 1993Lindemann & LaValle, 2006Kammel et al., 2008
Barraquand & Latombe:- 3 arcs (+ reverse) at max
- Discontinuous curvature- Cost = number of reversals- Dijkstra’s search
![Page 40: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/40.jpg)
40
Robot-Fixed Search Space• Moves with the robot
• Dense sampling– Position
• Symmetric sampling– Heading– Velocity– Steering angle– …
• Tree depth– 1: Local (arcs) + Global (D*) (Stentz & Hebert, 1994)– 5: Egograph (Lacaze et al., 1998)– ∞: Barraquand & Latombe (1993)
40
![Page 41: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/41.jpg)
41
Outline
• Introduction• Deterministic Planning
– Hierarchical– Path Smoothing– Control Sampling– State Sampling
• Randomized Planning– PRMs– RRTs
• Derandomized Planners• Some applications
![Page 42: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/42.jpg)
42
World-Fixed Search Space• Fixed to the world
• Dense sampling– (none)
• Symmetric sampling– Position– Heading– Velocity– Steering angle– …
• Dependency– Boundary value problem
Pivtoraiko & Kelly, 2005
Examples of BVP solvers:- Dubins, 1957- Reeds & Shepp, 1990- Lamiraux & Laumond, 2001- Kelly & Nagy, 2002- Pancanti et al., 2004- Kelly & Howard, 2005
![Page 43: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/43.jpg)
43
Robot-Fixed vs. World-Fixed
Barraquand & Latombe
CONTROL
STATE CONTROL
STATE
State Lattice
![Page 44: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/44.jpg)
44
State Lattice Benefits• State Lattice– Regularity in state sampling– Position invariance
Pivtoraiko & Kelly, 2005
![Page 45: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/45.jpg)
45
Path Swaths
Pivtoraiko & Kelly, 2007
• State Lattice– Regularity– Position invariance
• Benefits– Pre-computing path swaths
![Page 46: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/46.jpg)
46
World Fixed State Lattice
HLUT
Pivtoraiko & Kelly, 2005Knepper & Kelly, 2006
• State Lattice– Regularity– Position invariance
• Benefits– Pre-computing path swaths– Pre-computing heuristics
![Page 47: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/47.jpg)
World Fixed State Lattice
47
?
• State Lattice– Regularity– Position invariance
• Benefits– Pre-computing path swaths– Pre-computing heuristics– Dynamic replanning
![Page 48: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/48.jpg)
48
World Fixed State Lattice
• State Lattice– Regularity– Position invariance
• Benefits– Pre-computing path swaths– Pre-computing heuristics– Dynamic replanning
![Page 49: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/49.jpg)
49
Nonholonomic D*
Expanded States
Motion Plan
Perception Horizon
Graphics: Thomas Howard
![Page 50: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/50.jpg)
50
Nonholonomic D*
Pivtoraiko & Kelly, 2007Graphics: Thomas Howard
![Page 51: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/51.jpg)
51
Boss
• “Parking lot” planner
• Regular 4D state sampling
• Pre-computed search space– Depth: unlimited– Multi-resolution– 32 (16) headings– 2 velocities– No curvature
LIkhachev et al., 2008
![Page 52: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/52.jpg)
52
World Fixed State Lattice
• State Lattice– Regularity– Position invariance
• Benefits– Pre-computing path swaths– Pre-computing heuristics– Dynamic replanning– Dynamic search space G0
G1
G3
G4G5
Search graph G0 G1 … Gn
Pivtoraiko & Kelly, 2008
![Page 53: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/53.jpg)
53
Dynamic Search Space
Pivtoraiko & Kelly, 2008Graphics: Thomas Howard
![Page 54: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/54.jpg)
54
Dynamic Search Space
![Page 55: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/55.jpg)
55
World Fixed State Lattice
START
GOAL• State Lattice– Regularity– Position invariance
• Benefits– Pre-computing path swaths– Pre-computing heuristics– Dynamic replanning– Dynamic search space– Parallelized search
Pivtoraiko & Kelly, 2010
![Page 56: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/56.jpg)
56
World Fixed State Lattice
START
GOAL
Pivtoraiko & Kelly, 2010
![Page 57: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/57.jpg)
57
START
GOAL
World Fixed State Lattice
Pivtoraiko & Kelly, 2010
0 0.02 0.04 0.06 0.08 0.1 0.120
2
4
6
8
10
12
14
16
epsilon
tree
size
ratio
![Page 58: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/58.jpg)
58
Search Space Comparison
Robot-Fixed
Pros:- Any motion generation scheme
Cons:- NO Pre-computing path swaths- NO Pre-computing heuristics- NO Parallelized search- NO Dynamic replanning- NO Dynamic search space
World-Fixed
Pros:- Pre-computing path swaths- Pre-computing heuristics- Parallelized search- Dynamic replanning- Dynamic search space
Cons:- Boundary value problem
![Page 59: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/59.jpg)
59
Outline
• Introduction• Deterministic Planning
– Hierarchical– Path Smoothing– Control Sampling– State Sampling
• Randomized Planning– PRMs– RRTs
• Derandomized Planners• Some applications
![Page 60: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/60.jpg)
60
A Few Randomized Planners
• Probabilistic Roadmaps (PRM)– Kavraki, Svestka, Latombe & Overmars, 1996
• Expansive Space Tree (EST)– Hsu, Kindel, Latombe & Rock, 2001
• Rapidly-Exploring Random Tree (RRT)– LaValle & Kuffner, 2001
• R* Search– Likhachev & Stentz, 2008
LaValle & Kuffner, 2001
![Page 61: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/61.jpg)
61
Probabilistic Roadmap
• Static workspaces– E.g. industrial workcells
• Two phases:– Learning: construct the
roadmap– Query: actually plan
• Structure: undirected graph• Originally applied to holonomic robots
![Page 62: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/62.jpg)
62
Learning Phase
• Two steps:– Construction• Constructs edges and vertices to cover free C-space
uniformly– Expansion• Tries to detect “difficult” regions and samples them
more densely
![Page 63: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/63.jpg)
63
Construction Step
• Two sub-steps:– Sample a random configuration
and add to the graph– Select n neighbor vertices and
(try to) connect to the new vertex
• Components– Distance metric– Local planner• Connections between vertices
Kavraki, Svestka, Latombe & Overmars, 1996
![Page 64: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/64.jpg)
64
Query Phase
• Graph is constructed• Apply any shortest-path
graph search • Smoothing:– Find “shortcuts”– Local planner is reused
Kavraki, Svestka, Latombe & Overmars, 1996
![Page 65: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/65.jpg)
65
Outline
• Introduction• Deterministic Planning
– Hierarchical– Path Smoothing– Control Sampling– State Sampling
• Randomized Planning– PRMs– RRTs
• Derandomized Planners• Some applications
![Page 66: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/66.jpg)
Original RRT
• Rapidly-Exploring Random Tree• Proposed for kino-dynamic planning
– Holonomic randomized planners existed before– Proposed to meet the need for randomized planners under
differential constraints
• Today, a work-horse for randomized search– Probabilistically complete– No optimality guarantees– Avoids “curse of dimensionality”
![Page 67: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/67.jpg)
RRT in a Nutshell
• Given a tree (initially only x_{init})• Pick a sample, x, in state space, X
– Randomly– Sample uniform distribution over X
• Find nearest neighbor tree node, x_{near}, to x• Find a control that approaches x_{near}
– Unless you solve the BVP problem, won’t approach exactly
– Just do your best; you’ll arrive at x_{new} near to x
• Add x_{new} and the edge (x_{near}, x_{new}) to the tree
• Repeat
![Page 68: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/68.jpg)
RRT Origins• Khatib, IJRR 5(1), 1986
– Potential fields for obstacle avoidance, mobile robots and manipulators• Barraquand & Latombe, IJRR 10(6), 1993
– Builds and searches a graph connecting local minima of a potential field– Monte-Carlo technique to escape local minima via Brownian motions
• Kavraki, Svestka, Latombe & Overmars, Transactions 12(4), 1996– Probabilistic roadmaps– Randomly generate a graph in a configuration space– Multi-query method, best for fixed manipulators
• Hsu, Latombe & Motwani, Int. J. of Comput. Geometry & App., 1997– Expansive Space Tree (EST), single-query method– Choose tree node to extend via biased probability measure– Apply random control– Collision checking in state*time space
![Page 69: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/69.jpg)
Rapidly-Exploring
• Voronoi bias– Sampling uniform distribution over state space– Large empty regions have higher probability of being
sampled– Hence, tree “prefers” growing into empty regions
Naïve random tree RRT
![Page 70: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/70.jpg)
Voronoi Bias
![Page 71: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/71.jpg)
Analysis
• Convergence to solution– Probability of failure (to find solution)
decreases exponentially with the number of iterations
• Probabilistic completeness
![Page 72: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/72.jpg)
Examples
![Page 73: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/73.jpg)
Quick overview of CBiRRT
• Constrained Bi-directional RRT
• Start with “unconstrained” RRT• Assume some constraints, e.g.:
– End-effector pose– Torque
• For each sample, x_{rand}, in X– Get x_{new} by tweaking x_{rand} until
constraints satisfied– … via optimization (gradient descent)– In text, “project sample onto constraint
manifold”
![Page 74: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/74.jpg)
Growing the Tree
• Extending tree toward random sample– The sample is q_{target}– Nearest neighbor: q_{near}
• Step from q_{near} to q_{target}– In state space– Project each step onto constraint
manifold– Until you reach q_{target}’s
projection– Return, if can’t continue, e.g.
• Obstacles• Can’t project
![Page 75: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/75.jpg)
Results5kg
6kg
8kg
![Page 76: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/76.jpg)
76
Outline
• Introduction• Deterministic Planning
– Hierarchical– Path Smoothing– Control Sampling– State Sampling
• Randomized Planning– PRMs– RRTs
• Derandomized Planners• Some Applications
![Page 77: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/77.jpg)
Randomness for Planning
• Pros:– Allows rapid exploration of state space
(via uniform sampling)– Less susceptible to local minima
• Cons:– Inability to provide performance guarantees– Obscures other useful features of planners
• Moreover:– There are deterministic incremental sampling methods
• E.g., Halton points Van der Corput sequence (1935); generalized to multiple dimensions by Halton.
![Page 78: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/78.jpg)
Derandomized RRT
• Let’s implement Voronoi bias explicitly– Given tree– Compute Voronoi diagram wrt its nodes– Pick the sample to extend toward:
• Centroid of largest Voronoi region, or• Otherwise reduce size of largest empty ball
• Problem:– Voronoi diagram in arbitrary dimensions
– prohibitively expensive
![Page 79: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/79.jpg)
Semi-Deterministic RRT
• So let’s go for a middle ground• Instead of a single x_{rand}• Draw a set k samples• Multi-Sample RRT (MS-RRT)
– Sort tree nodes acc. to:• how many samples they’re nearest neighbor for
– Pick node that “collected” most neighbors– Grow tree towards average of the neighbor samples
• It’s an estimate of the Voronoi centroid• As k∞, we get exact Voronoi centroid
• “… I shall call him MS-RRTa!”
![Page 80: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/80.jpg)
Now, a Deterministic RRT
• … but with approximate Voronoi bias• Recall: picking k points– Instead of randomly,– Use k uniformly distributed, incremental deterministic
samples– E.g., Halton points
• The rest stays essentially the same
• Meet MS-RRTb!
![Page 81: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/81.jpg)
Results
• Local minima– Both MS-RRT are more greedily Voronoi-biased– Local minima issues – more pronounced than RRT– Paper’s workaround:
• Introduce obstacle nodes in tree• It’s the nodes that land in obstacles• A mechanism “to remember” not to grow the tree there any more
• Sensitivity to metrics– Increased for MS-RRTa,b, – Since Voronoi depends on metric
• Nearest-neighbor computation– More expensive than O(log n)– Increased demand for it in MS-RRTa,b
![Page 82: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/82.jpg)
Results
![Page 83: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/83.jpg)
83
Outline
• Introduction• Deterministic Planning
– Hierarchical– Path Smoothing– Control Sampling– State Sampling
• Randomized Planning– PRMs– RRTs
• Derandomized Planners• Some applications
![Page 84: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/84.jpg)
84
Mobile Manipulation• Arbitrary mobility constraints• Optimal solution– Up to representation
• Parallelized computation• Automatically designed
![Page 85: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/85.jpg)
85
Dynamics Planning
![Page 86: A Survey of Differentially Constrained Planning](https://reader036.vdocuments.us/reader036/viewer/2022070501/56816948550346895de0d89d/html5/thumbnails/86.jpg)
86
Summary• Deterministic planning– Hierarchical– Path Smoothing– Control Sampling– State Sampling
• Randomized planning• PRMs• RRTs