Embed Size (px)
Citation preview
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 1
Mars Science Laboratory
Rover Trajectory PlanningConstrained Global Planning
and Path Relaxation
Mihail Pivtoraiko
Robotics InstituteCarnegie Mellon University
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 2
Autonomous Navigation
The Challenge:
Outdoor Autonomous Robots
2
NavLab, 1985
Boss, 2007MER, 2004Crusher, 2006
ALV, 1988
XUV, 1998
Stanford Cart, 1979
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 33
Introduction
• Ground-based rover operation– Time-consuming– Costly
• Rover autonomy– Risky– In development…
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 4
Unstructured Environments
4
Local
Global
ALV (Daily et al., 1988)
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 5
Unstructured and Unknown
5
Local
Global
ALV (Daily et al., 1988)
?
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 6
Unstructured and Unknown
6
Local
Global
ALV (Daily et al., 1988)
?
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 7
Unstructured and Unknown
7
Local
Global
D*/Smarty (Stentz & Hebert, 1994)Ranger (Kelly, 1995)Morphin (Simmons et al., 1996)Gestalt (Goldberg, Maimone & Matthies, 2002)
Dynamic replanningD* (Stentz ’94, others)
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 88
Outline
• Introduction
• Global planning– Representation limitations– Improvements– Results
• Local planning– Representation limitations– Improvements– Results
• Summary
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 9
Unstructured and Unknown
9
Local
Global
D*/Smarty (Stentz & Hebert, 1994)Ranger (Kelly, 1995)Morphin (Simmons et al., 1996)Gestalt (Goldberg, Maimone & Matthies, 2002)
Dynamic replanningD* (Stentz ’94, others)
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 10
Unstructured and Unknown
10
Global
D*/Smarty (Stentz & Hebert, 1994)Ranger (Kelly, 1995)Morphin (Simmons et al., 1996)Gestalt (Goldberg, Maimone & Matthies, 2002)
Dynamic replanningD* (Stentz ’94, others)
Local
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 1111
Challenges
• 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
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 1212
Turn-in-place Difficulties
• Extremal control– Energy expenditure: High
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 1313
Turn-in-place Difficulties
• Extremal control– Energy expenditure: High
• Perception difficulties– Sudden view change– Loss of features (e.g. for VO)
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 1414
Turn-in-place Difficulties
• Extremal control– Energy expenditure: High
• Perception difficulties– Sudden view change– Loss of features (e.g. for VO)
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 1515
Turn-in-Place Difficulties
• Extremal control– Energy expenditure: High
• Perception difficulties– Sudden view change– Loss of features (e.g. for VO)
• Infeasibility
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 1616
In the Field…
PerceptOR/UPI, 2005 Rover Navigation, 2008
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 1717
Heading-Aware Global Planner
• Problem:– Mal-informed global planner– Vehicle constraints ignored
• Heading
• Proposing:– Satisfying vehicle constraints– Heading-aware planning
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 1818
Heading-Aware Global Planner
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 19
Heading-Aware Planning
19
Barraquand & Latombe, 1993LaValle, 2006
Barraquand & Latombe:- 3 arcs (+ reverse) at max
- Discontinuous curvature- Cost = number of reversals- Dijkstra’s search
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 20
Robot-Fixed Search Space
• Moves with the robot
• Dense sampling– Position
• Symmetric sampling– Heading– Velocity– Steering angle– …
• Tree depth– 1: Local/Global– 5: Egograph (Lacaze et al, ‘98)– ∞: Barraquand & Latombe 20
20
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 21
World-Fixed Search Space
21
• 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
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 22
Robot-Fixed vs. World-Fixed
22
Barraquand & Latombe
CONTROL
STATE CONTROL
STATE
State Lattice
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 23
Dynamic Re-planning
23
?
• State Lattice– Regularity– Position invariance– Enables D*
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 24
Dynamic Re-planning
24
• State Lattice– Regularity– Position invariance– Enables D*
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 25
Nonholonomic D*
Expanded States
Motion Plan
Perception Horizon
Graphics: Thomas Howard25
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 26
Nonholonomic D*
26
Pivtoraiko & Kelly, 2007Graphics: Thomas Howard
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 27
Dynamic Search Space
27
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 2828
8-Connected Grid Alternative
• Space & time complexity– Linear with heading resolution
8-connectedgrid
8-connectedstate lattice
x
q
y
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 2929
8-Connected Grid Alternative
• Space & time complexity– Linear with heading resolution
• Optimality– W.r.t. path length– Nearly identity
8-connectedgrid
8-connectedstate lattice
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 3030
8-Connected Grid Alternative
• Space & time complexity– Linear with heading resolution
• Optimality– W.r.t. path length– Nearly identity
• Completeness– Random point-obstacles– Nearly identity
8-connectedgrid
8-connectedstate lattice
Obstacle Density, %30 40 50
Rel
. P
lan
Fai
lure
, %
20
100
45
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 3131
Outline
• Introduction
• Global planning– Representation limitations– Improvements– Results
• Local planning– Representation limitations– Improvements– Results
• Summary
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 3232
Following Global Guidance
• Local planning– Receding-Horizon MPC
• Common in rover navigation– Sampling discrete controls– Parameterized representations
• Natural environments– Pose challenges– “Beat” a set of discrete controls
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 3333
Following Global Guidance
• Local planning– Receding-Horizon MPC
• Common in rover navigation– Sampling discrete controls– Parameterized representations
• Natural environments– Pose challenges– “Beat” a set of discrete controls
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 3434
Parameterized Representations
• Polynomial curvature functions– (s) = 0 + 1s + 2s2 + … + nsn
– In the limit, represents any motion– By Taylor remainder theorem
• Constant-curvature arcs– (s) = 0
– Coupled position and heading– Limited expressiveness
• First-order clothoids– (s) = 0 + 1s– De-coupled position and heading– Simplest such parameterization
Kelly & Nagy, 2002
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 3535
Arcs vs. Clothoids
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 3737
Path Relaxation
• Natural optimization problem– Local nature of motion primitives– Parameterization– Available obstacle-aware objective functions
• Distance transform• Obstacle modeling
• Constrained, Regulated Gradient Descent– Modified gradient descent optimization– Completeness considerations
• Constraints imposed• Regulated step size
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 3838
Fixed vs. Relaxed
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 3939
Experimental Results
Rover Navigation ExperimentsTesting Local + Global150 trials
“Random Pose” ExperimentsTesting Local decoupled from Global
8784 trials
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 4040
Outline
• Introduction
• Global planning– Representation limitations– Improvements– Results
• Local planning– Representation limitations– Improvements– Results
• Summary
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 4141
Summary
• Global planning• Heading-aware planning
• Deliberative, intelligent decisions
• Local planning• Expressive clothoids
• Decoupling heading & position
• No cost!
• Relaxation• Continuum: arbitrary obstacles
• Acknowledgement– Jet Propulsion Lab MTP and SURP
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 42
State Lattices for Planning with Dynamics
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 4343Pivtoraiko, Howard, Nesnas & Kelly
Backup
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 44
State Lattice Benefits
44
• State Lattice– Regularity– Position invariance
Pivtoraiko & Kelly, 2005
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 45
Path Swaths
45
Pivtoraiko & Kelly, 2007
• State Lattice– Regularity– Position invariance
• Benefits– Pre-computing path
swaths
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 46
World Fixed State Lattice
46
HLUT
Pivtoraiko & Kelly, 2005Knepper & Kelly, 2006
• State Lattice– Regularity– Position invariance
• Benefits– Pre-computing path
swaths– Pre-computing heuristics
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 47
World Fixed State Lattice
47
START
GOAL• State Lattice
– Regularity– Position invariance
• Benefits– Pre-computing path
swaths– Pre-computing heuristics– Parallelized search
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 48
World Fixed State Lattice
• State Lattice– Regularity– Position invariance
• Benefits– Pre-computing path
swaths– Pre-computing heuristics– Parallelized search– Dynamic replanning– Dynamic search space
48
G0
G1
G3
G4G5
Search graph G0 G1 … Gn
Pivtoraiko & Kelly, 2008
PRE-DECISIONAL DRAFT; For planning and discussion purposes only 49
Dynamic Search Space
49
Pivtoraiko & Kelly, 2008Graphics: Thomas Howard
