1. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 1 Mars Science Laboratory Rover Trajectory Planning Constrained Global Planning and Path Relaxation Mihail Pivtoraiko Robotics Institute Carnegie Mellon University

2. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 2 Autonomous Navigation The Challenge: Outdoor Autonomous Robots 2 NavLab, 1985 Boss, 2007 MER, 2004 Crusher, 2006 ALV, 1988 XUV, 1998 Stanford Cart, 1979

3. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 3 3 Introduction Ground-based rover operation –Time-consuming –Costly Rover autonomy –Risky –In development…

4. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 4 Unstructured Environments 4 Local Global ALV (Daily et al., 1988)

5. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 5 Unstructured and Unknown 5 Local Global ALV (Daily et al., 1988) ?

6. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 6 Unstructured and Unknown 6 Local Global ALV (Daily et al., 1988) ?

7. 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 replanning D* (Stentz ’94, others)

8. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 8 Outline Introduction Global planning –Representation limitations –Improvements –Results Local planning –Representation limitations –Improvements –Results Summary 8

9. 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 replanning D* (Stentz ’94, others)

10. 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 replanning D* (Stentz ’94, others) Local

11. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 11 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

12. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 12 Turn-in-place Difficulties 12 Extremal control –Energy expenditure: High

13. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 13 Turn-in-place Difficulties Extremal control –Energy expenditure: High Perception difficulties –Sudden view change –Loss of features (e.g. for VO) 13

14. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 14 Turn-in-place Difficulties Extremal control –Energy expenditure: High Perception difficulties –Sudden view change –Loss of features (e.g. for VO) 14

15. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 15 Turn-in-Place Difficulties Extremal control –Energy expenditure: High Perception difficulties –Sudden view change –Loss of features (e.g. for VO) Infeasibility 15

16. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 16 In the Field… 16 PerceptOR/UPI, 2005Rover Navigation, 2008

17. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 17 Heading-Aware Global Planner Problem: –Mal-informed global planner –Vehicle constraints ignored Heading Proposing: –Satisfying vehicle constraints –Heading-aware planning

18. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 18 Heading-Aware Global Planner

19. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 19 Heading-Aware Planning 19 Barraquand & Latombe, 1993 LaValle, 2006   Barraquand & Latombe: - 3 arcs (+ reverse) at  max - Discontinuous curvature - Cost = number of reversals - Dijkstra’s search

20. 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

21. 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

22. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 22 Robot-Fixed vs. World-Fixed 22 Barraquand & Latombe CONTROL STATECONTROL STATE State Lattice

23. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 23 Dynamic Re-planning 23 ? State Lattice –Regularity –Position invariance –Enables D*

24. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 24 Dynamic Re-planning 24 State Lattice –Regularity –Position invariance –Enables D*

25. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 25 Nonholonomic D* Expanded States Motion Plan Perception Horizon Graphics: Thomas Howard 25

26. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 26 Nonholonomic D* 26 Pivtoraiko & Kelly, 2007 Graphics: Thomas Howard

27. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 27 Dynamic Search Space 27

28. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 28 8-Connected Grid Alternative Space & time complexity –Linear with heading resolution 28 8-connected grid 8-connected state lattice x  y

29. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 29 8-Connected Grid Alternative Space & time complexity –Linear with heading resolution Optimality –W.r.t. path length –Nearly identity 29 8-connected grid 8-connected state lattice

30. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 30 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 30 8-connected grid 8-connected state lattice Obstacle Density, % 30 4050 Rel. Plan Failure, % 20 100 45

31. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 31 Outline Introduction Global planning –Representation limitations –Improvements –Results Local planning –Representation limitations –Improvements –Results Summary 31

32. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 32 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 32

33. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 33 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 33

34. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 34 Parameterized Representations Polynomial curvature functions –  (s) =  0 +  1 s +  2 s 2 + … +  n s n –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 +  1 s –De-coupled position and heading –Simplest such parameterization 34 Kelly & Nagy, 2002

35. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 35 Arcs vs. Clothoids 35

36. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 36 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 36

37. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 37 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 37

38. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 38 Fixed vs. Relaxed 38

39. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 39 Experimental Results 39 Rover Navigation Experiments Testing Local + Global 150 trials “Random Pose” Experiments Testing Local decoupled from Global 8784 trials

40. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 40 Outline Introduction Global planning –Representation limitations –Improvements –Results Local planning –Representation limitations –Improvements –Results Summary 40

41. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 41 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

42. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 42 State Lattices for Planning with Dynamics

43. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 43 Pivtoraiko, Howard, Nesnas & Kelly 43 Backup

44. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 44 State Lattice Benefits 44 State Lattice –Regularity –Position invariance Pivtoraiko & Kelly, 2005

45. 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

46. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 46 World Fixed State Lattice 46 HLUT Pivtoraiko & Kelly, 2005 Knepper & Kelly, 2006 State Lattice –Regularity –Position invariance Benefits –Pre-computing path swaths –Pre-computing heuristics

47. 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

48. 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 G0G0 G1G1 G3G3 G4G4 G5G5 Search graph  G 0  G 1  …  G n Pivtoraiko & Kelly, 2008

49. PRE-DECISIONAL DRAFT; For planning and discussion purposes only 49 Dynamic Search Space 49 Pivtoraiko & Kelly, 2008 Graphics: Thomas Howard