Iterative Motion Planning and Safety Issue

  • Thierry FraichardEmail author
  • Thomas M. Howard


This chapter addresses safe mobile robot navigation in complex environments. The challenges in this class of navigation problems include nontrivial vehicle dynamics and terrain interaction, static and dynamic environments, and incomplete information.

This complexity prompted the design of hierarchical solutions featuring a multilevel strategy where strategic behaviors are planned at a global scale and tactical or safety decisions are made at a local scale. While the task of the high level is generally to compute the sequence of waypoints or waystates to reach the goal, the local planner computes the actual trajectory that will be executed by the system. Due to computational resource limitations, finite sensing horizon, and temporal constraints of mobile robots, the local trajectory is only partially computed to provide a motion that makes progress toward the goal state. This chapter focuses on safely and efficiently computing the local trajectory in the context of mobile robot navigation.

This chapter is divided into three sections: motion safety, iterative motion planning, and applications. Motion safety discusses the issues related to determining if a trajectory is safely traversable by a mobile robot. Iterative motion planning reviews developments in local motion planning search space design with a focus on potential field, sampling, and graph search techniques. The applications section surveys experiments and applications in autonomous mobile robot navigation in outdoor and urban environments.


Mobile Robot Motion Planning Motion Planner Rapidly Explore Random Tree Mobile Robot Navigation 
These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.



A portion of this research was carried out at the Jet Propulsion Laboratory, California Institute of Technology, under a contract with the National Aeronautics and Space Administration.


  1. Althoff D, Althoff M, Wollherr D, Buss M (2010) Probabilistic collision state checker for crowded environments. In: IEEE international conference on robotics and automation, Anchorage. doi:10.1109/ROBOT.2010.5509369Google Scholar
  2. Amar F, Bidaud P, Ouezdou F (1993) On modeling and motion planning of planetary vehicles. In: Proceedings of the 1993 IEEE/RSJ international conference on intelligent robots and systems, vol 2, Yokohama, pp 1381–1386Google Scholar
  3. Aubin JP (1991) Viability theory. Birkhuser, BostonzbMATHGoogle Scholar
  4. Bacha A, Bauman C, Faruque R, Fleming M, Terwelp C, Reinholtz C, Hong D, Wicks A, Alberi T, Anderson D, Cacciola S, Currier P, Dalton A, Farmer J, Hurdus J, Kimmel S, King P, Taylor A, van Covern D, Webster M (2008) Odin: team VictorTango’s entry in the DARPA urban challenge. J Field Robot 25(8). doi:10.1002/rob.20248Google Scholar
  5. Bautin A, Martinez-Gomez L, Fraichard T (2010) Inevitable collision states, a probabilistic perspective. In: IEEE international conference on robotics and automation, Anchorage. doi:10.1109/ROBOT.2010.5509233Google Scholar
  6. Bekris K, Kavraki L (2007) Greedy but safe replanning under kinodynamic constraints. In: IEEE international conference on robotics and automation, Rome. doi:10.1109/ROBOT.2007.363069Google Scholar
  7. Benenson R, Petti S, Fraichard T, Parent M (2006) Integrating perception and planning for autonomous navigation of urban vehicles. In: IEEE/RSJ international conference on intelligent robots and systems, Beijing. doi:10.1109/IROS.2006.281806Google Scholar
  8. Biesiadecki J, Maimone M (2006) The mars exploration rover surface mobility flight software: Driving ambition. In: Proceedings of the 2006 IEEE aerospace conference, Pasadena, 2006Google Scholar
  9. Borenstein J, Korem Y (1991) The vector field histogram - fast obstacle avoidance for mobile robots. IEEE Trans Robot Autom 7(3). doi:10.1109/70.88137Google Scholar
  10. Braid D, Broggie A, Schmiedel G (2006) The terramax autonomous vehicle. J Field Robot 23(9):693–708CrossRefGoogle Scholar
  11. Broadhurst A, Baker S, Kanade T (2005) Monte Carlo road safety reasoning. In: IEEE intelligent vehicles symposium, Las Vegas. doi:10.1109/IVS.2005.1505122Google Scholar
  12. Brockett R (1981) Control theory and singular Riemann Geometry. Springer, New YorkGoogle Scholar
  13. Broggi A, Bertozzi M, Fascioli A, Guarino Lo Bianco C, Piazzi A (1999) The ARGO autonomous vehicle’s vision and control systems. Int J Intell Contr Syst 3(4):409–441Google Scholar
  14. Broggi A, Medici P, Cardarelli E, Cerri P, Giacomazzo A, Finardi N (2010) Development of the control system for the vislab intercontinental autonomous challenge. In: IEEE international conference on intelligent transportation systems, Madeira. doi:10.1109/ITSC.2010.5625001Google Scholar
  15. Chan N, Zucker M, Kuffner J (2007) Towards safe motion planning for dynamic systems using regions of inevitable collision. In: Collision-free motion planning for dynamic systems workshop, RomeGoogle Scholar
  16. Cheng P, LaValle S (2001) Reducing metric sensitivity in randomized trajectory design. In: IEEE/RSJ international conference on intelligent robots and systems, Hawaii. doi:10.1109/IROS.2001.973334Google Scholar
  17. Coulter R (1992) Implementation of the pure pursuit path tracking algorithm. Technical report, Carnegie Mellon UniversityGoogle Scholar
  18. Daily M, Harris J, Keirsey D, Olin K, Payton D, Reiser K, Rosenblatt J, Tseng D, Wong V (1988) Autonomous cross-country navigation with the alv. In: Proceedings of the IEEE international conference on robotics and automation, Philadelphia, pp 718–726Google Scholar
  19. Delingette H, Gerbert M, Ikeuchi K (1991) Trajectory generation with curvature constraint based on energy minimization. In: Proceedings of the 1991 IEEE/RSJ International Conference on Intelligent Robots and Systems, Osaka, vol 1, pp 206–211Google Scholar
  20. Dubins L (1957) On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tagents. Am J Math 79:497–516MathSciNetzbMATHCrossRefGoogle Scholar
  21. Elnagar A, Gupta K (1998) Motion prediction of moving objects based on autoregressive model. IEEE Trans Syst Man Cybern A Syst Hum 28(6). doi:10.1109/3468.725351Google Scholar
  22. Erdmann M, Lozano-Perez T (1987) On multiple moving objects. Algorithmica 2:477–521MathSciNetzbMATHCrossRefGoogle Scholar
  23. Ferguson D, Stentz A (2006) Multi-resolution field d. In: Proceedings of the international conference on intelligent autonomous systems, Pittsburgh, pp 65–74Google Scholar
  24. Ferguson D, Darms M, Urmson C, Kolski S (2008a) Detection, prediction, and avoidance of dynamic obstacles in urban environments. In: IEEE intelligent vehicles symposium, Eindhoven. doi:10.1109/IVS.2008.4621214Google Scholar
  25. Ferguson D, Howard T, Likhachev M (2008b) Motion planning in urban environments: part i. In: Proceedings of the 2008 IEEE/RSJ international conference on intelligent robots and systems, Hoboken, 2008Google Scholar
  26. Fiorini P, Shiller Z (1998) Motion planning in dynamic environments using velocity obstacles. Int J Robot Res 17(7):760–772CrossRefGoogle Scholar
  27. Fletcher L, Teller S, Olson E, Moore D, Kuwata Y, How J, Leonard J, Miller I, Campbell M, Huttenlocher D, Nathan A, Kline FR (2008) The MIT – cornell collision and why it happened. Int J Field Robot 25(10):775–807CrossRefGoogle Scholar
  28. Fox D, Burgard W, Thrun S (1997) The dynamic window approach to collision avoidance. IEEE Robot Autom Mag 4(1):23–33CrossRefGoogle Scholar
  29. Fraichard T (1993) Dynamic trajectory planning with dynamic constraints: a ‘state-time space’ approach. In: Proceedings of the IEEE/RSJ international conference on intelligent robots and systems, Yokohama, doi:10.1109/IROS.1993.583794Google Scholar
  30. Fraichard T (2007) A short paper about motion safety. In: IEEE international conference on robotics and automation, Rome, doi:10.1109/ROBOT.2007.363138Google Scholar
  31. Fraichard T, Asama H (2004) Inevitable collision states. A step towards safer robots? Adv Robot 18(10):1001–1024CrossRefGoogle Scholar
  32. Frazzoli E, Feron E, Dahleh M (2002) Real-time motion planning for agile autonomous vehicle. AIAA J Guid Control Dyn 25(1):116–129CrossRefGoogle Scholar
  33. Haddad H, Khatib M, Lacroix S, Chatila R (1998) Reactive navigation in outdoor environments using potential fields. In: Proceedings of the 1998 IEEE conference on robotics and automation, Leuven, vol 2, pp 1232–1237Google Scholar
  34. Hart P, Nilsson N, Raphael B (1968) A formal basis for the heuristic determination of minimum-cost paths. IEEE Trans Syst Sci Cybern 4(2):100–107CrossRefGoogle Scholar
  35. Howard T (2009) Adaptive model-predictive motion planning for navigation in complex environments. PhD thesis, Carnegie Mellon UniversityGoogle Scholar
  36. Howard T, Kelly A (2007) Rough terrain trajectory generation for wheeled mobile robots. Int J Robot Res 26(2):141–166CrossRefGoogle Scholar
  37. Howard T, Green C, Kelly A, Ferguson D (2008) State space sampling of feasible motions for high-performance mobile robot navigation in complex environments. J Field Robot 25(6–7):325–345CrossRefGoogle Scholar
  38. Hsu D, Kindel R, Latombe JC, Rock S (2002) Randomized kinodynamic motion planning with moving obstacles. Int J Robot Res 21(3):233–255CrossRefGoogle Scholar
  39. Iagnemma K, Shimoda S, Shiller Z (2008) Near-optimal navigation of high speed mobile robots on uneven terrain. In: Proceedings of the 2008 international conference on robotics and automation, Pasadena, 2008Google Scholar
  40. Jackson J, Crouch P (1991) Curved path approaches and dynamic interpolation. In: IEEE aerospace and electronic systems magazine, GlendaleGoogle Scholar
  41. Jochem T, Pomerleau D, Kumar B, Armstrong J (1995) PANS: a portable navigation platform. In: IEEE intelligent vehicles symposium, Detroit, doi:10.1109/IVS.1995.528266Google Scholar
  42. Kalisiak M, van de Panne M (2007) Faster motion planning using learned local viability models. In: IEEE international conference on robotics and automation, Rome. doi:10.1109/ROBOT.2007.363873Google Scholar
  43. Kalmár-Nagy T, D’Andrea R, Ganguly P (2004) Near-optimal dynamic trajectory generation and control of a omni-directional vehicle. Robot Auton Syst 46:47–64CrossRefGoogle Scholar
  44. Kanayama Y, Hartman B (1989) Smooth local path planning for autonomous vehicles. In: Proceedings of the 1989 international conference on robotics and automation, Santa Barbara, vol 3, pp 1265–1270Google Scholar
  45. Kanayama Y, Miyake N (1985) Trajectory generation for mobile robots. In: Proceedings of the international symposium on robotics research, Gouvieux, pp 16–23Google Scholar
  46. Kelly A, Nagy B (2003) Reactive nonholonomic trajectory generation via parametric optimal control. Int J Robot Res 22(7):583–601CrossRefGoogle Scholar
  47. Kelly A, Stentz T (1998) Rough terrain autonomous mobility - part 2: an active vision and predictive control approach. Auton Robot 5:163–198CrossRefGoogle Scholar
  48. Kelly A, Stentz T, Amidi O, Bode M, Bradley D, Mandelbaum R, Pilarski T, Rander P, Thayer S, Vallidis N, Warner R (2006) Toward reliable off-road autonomous vehicles operating in challenging environments. Int J Robot Res 25(5):449–483CrossRefGoogle Scholar
  49. Khatib O (1986a) Real-time obstacle avoidance for manipulators and mobile robots. Int J Robot Res 5(1). doi:10.1177/027836498600500106Google Scholar
  50. Khatib O (1986b) Real-time obstacle avoidance for manipulators and mobile robots. Int J Robot Res 5(1):90–98MathSciNetCrossRefGoogle Scholar
  51. Knepper R, Srinivasa S, Mason M (2010) An equivalent relation for local path sets. In: Proceedings of the ninth international workshop on the algorithmic foundations of robotics, SingaporeGoogle Scholar
  52. Koenig S, Likhachev M (2002) D* lite. In: Proceedings of the AAAI conference on artificial intelligence, EdmontonGoogle Scholar
  53. Komoriya K, Tanie K (1989) Trajectory design and control of a wheel-type mobile robot using b-spline curve. In: Proceedings of the 1989 IEEE/RSJ international conference on intelligent robots and systems, Tsukuba, pp 398–405Google Scholar
  54. Kuwata Y, Karaman S, Teo J, Frazzoli E, How J, Fiore G (2009) Real-time motion planning with applications to autonomous urban driving. IEEE Trans Contr Syst Technol 17(5). doi:10.1109/TCST.2008.2012116Google Scholar
  55. Lavalle S (1998) Rapidly-exploring random trees: a new tool for path planning. Technical report, 98-11, Department of Computer Science, Iowa State UniversityGoogle Scholar
  56. Lavalle S (2006) Planning algorithms. Cambridge University Press. Accessed 26 Sep 2011
  57. LaValle S, Kuffner J (1999) Randomized kinodynamic planning. In: IEEE international conference on robotics and automation, Detroit, doi:10.1109/ROBOT.1999.770022Google Scholar
  58. Lozano-Perez T (1983) Spatial planning, a configuration space approach. IEEE Trans Compt 32(2):108–120MathSciNetzbMATHCrossRefGoogle Scholar
  59. Minguez J, Montano L (2004) Nearness diagram (ND) navigation: collision avoidance in troublesome scenarios. IEEE Trans Robot Autom 20(1):45–59CrossRefGoogle Scholar
  60. Mitchell I, Tomlin C (2003) Overapproximating reachable sets by hamilton-jacobi projections. J Sci Comput 19(1–3). doi:10.1023/A:1025364227563Google Scholar
  61. Montemerlo M, Becker J, Bhat S, Dahlkamp H, Dolgov D, Ettinger S, Haehnel D, Hilden T, Hoffmann G, Huhnke B, Johnston D, Klumpp S, Langer D, Levandowski A, Levinson J, Marcil J, Orenstein D, Paefgen J, Penny I, Petrovskaya A, Pflueger M, Stanek G, Stavens D, Vogt A, Thrun S (2008) Junior: the stanford entry in the urban challenge. J Field Robot 25(9). doi:10.1002/rob.20258Google Scholar
  62. Murray R, Sastry S (1993) Nonholonomic motion planning: steering using sinusoids. IEEE Trans Autom Contr 38:700–716MathSciNetzbMATHCrossRefGoogle Scholar
  63. Petti S, Fraichard T (2005) Safe motion planning in dynamic environments. In: Proceedings of the IEEE-RSJ international conference on intelligent robots and systems, EdmontonGoogle Scholar
  64. Pivtoraiko M, Knepper R, Kelly A (2009) Differentially constrained mobile robot motion planning in state lattices. J Field Robot 26(3):308–333CrossRefGoogle Scholar
  65. Prajna S, Jadbabaie A, Pappas G (2007) A framework for worst-case and stochastic safety verification using barrier certificates. IEEE Trans Autom Contr 52(8). doi:10.1109/TAC.2007.902736Google Scholar
  66. Reeds J, Shepp L (1990) Optimal paths for a car that goes both forwards and backwards. Pacific J Math 145(2):367–393MathSciNetGoogle Scholar
  67. Reichardt D, Shick J (1994) Collision avoidance in dynamic environments applied to autonomous vehicle guidance on the motorway. In: IEEE intelligent vehicles symposium, New York, doi:10.1109/IVS.1994.639475Google Scholar
  68. Reif J, Sharir M (1985) Motion planning in the presence of moving obstacles. In: IEEE symposium on the foundations of computer science, Cambridge, doi:10.1109/SFCS.1985.36Google Scholar
  69. Rogers-Marcovitz F, Kelly A (2010) On-line mobile robot model identification using integrated perturbative dynamics. In: Proceedings of the 2010 international symposium on experimental robotics, DelhiGoogle Scholar
  70. Rohrmuller F, Althoff M, Wollherr D, Buss M (2008) Probabilistic mapping of dynamic obstacles using markov chains for replanning in dynamic environments. In: IEEE/RSJ international conference on intelligent robots and systems, Nice. doi:10.1109/IROS.2008.4650952Google Scholar
  71. Schmidt C, Oechsle F, Branz W (2006) Research on trajectory planning in emergency situations with multiple objects. In: IEEE intelligent transportation systems conference, Toronto, doi:10.1109/ITSC.2006.1707153Google Scholar
  72. Seder M, Petrovic I (2007) Dynamic window based approach to mobile robot motion control in the presence of moving obstacles. In: IEEE international conference robotics and automation, RomaGoogle Scholar
  73. Shiller Z, Chen J (1990) Optimal motion planning of autonomous vehicles in three-dimensional terrains. In: Proceedings of the IEEE international conference on robotics and automation, Cincinnatti, pp 198–203Google Scholar
  74. Shin D, Singh S (1991) Path generation for robot vehicles using composite clothoid segments. Technical report, Carnegie Mellon UniversityGoogle Scholar
  75. Song D, Lee H, Yi J, Levandowski A (2007) Vision-based motion planning for an autonomous motorcycle on ill-structured roads. Auton Robot 23(3):197–212CrossRefGoogle Scholar
  76. Stentz A, Herbert M (1995) A complete navigation system for goal acquisition in unknown environments. Auton Robot 2(2):127–145CrossRefGoogle Scholar
  77. Stentz A, Kelly A, Herman H, Rander P, Amidi O, Mandelbaum R (2002) Integrated air/ground vehicle system for semi-autonomous off-road navigation. In: Proceedings of the AUVSI unmanned systems symposium, Orlando, 2002Google Scholar
  78. Thrun S (2010) What we’re driving at. The official Google blog. Accessed 26 Sep 2011
  79. Thrun S, Montemerlo M, Dahlkamp H, Stavens D, Aron A, Diebel J, Fong P, Gale J, Halpenny M, Hoffman G, Lau K, Oakley C, Palatucci M, Pratt V, Stang P, Strohband S, Dupont C, Jendrossek L, Koelen C, Markey C, Rummel C, van Niekerk J, Jensen E, Alessandrini P, Bradski G, Davies B, Ettinger S, Kaehler A, Naflan A, Mahoney P (2006) Stanley: the robot that won the darpa grand challenge. J Field Robot 23(8):661–692CrossRefGoogle Scholar
  80. Tilbury D, Laumond J, Murray R, Sastry S, Walsh G (1992) Steering car-like systems with trailers using sinusoids. In: Proceedings of the 1992 IEEE international conference on robotics and automation, NiceGoogle Scholar
  81. Ulmer B (1992) VITA-an autonomous road vehicle (ARV) for collision avoidance in traffic. In: IEEE intelligent vehicles symposium, New York, doi:10.1109/IVS.1992.252230Google Scholar
  82. Ulmer B (1994) VITA II-active collision avoidance in real traffic. In: IEEE intelligent vehicles symposium, New York, doi:10.1109/IVS.1994.639460Google Scholar
  83. Ulrich I, Borenstein J (2000) VFH: local obstacle avoidance with look-ahead verification. In: IEEE international conference on robotics and automation, Washington, DC, doi:10.1109/ROBOT.2000.846405Google Scholar
  84. Urmson C, Ragusa C, Ray D, Anahlt J, Bartz D, Galatali T, Gutierrez A, Johnston J, Harbaugh S, Kato H, Messner W, Miller N, Peterson K, Smith B, Snider J, Spiker S, Ziglar J, Whittaker W, Clark M, Koon P, Mosher A, Struble J (2006) A robust approach to high-speed navigation for unrehearsed desert terrain. J Field Robot 23(8):467–508zbMATHCrossRefGoogle Scholar
  85. van den Berg J, Overmars M (2008) Planning time-minimal safe paths amidst unpredictably moving obstacles. Int Journal of Robotics Research 27(11–12). doi:10.1177/0278364908097581Google Scholar
  86. Vasquez D (2007) Incremental learning for motion prediction of pedestrians and vehicles. PhD thesis, Inst. Nat. Polytechnique de Grenoble. Accessed 26 Sep 2011
  87. Vasquez D, Fraichard T, Laugier C (2009) Growing hidden Markov models: a tool for incremental learning and prediction of motion. Int J Robot Res 28(11–12):1486–1506CrossRefGoogle Scholar
  88. Vatcha R, Xiao L (2008) Perceived CT-space for motion planning in unknown and unpredictable environments. In: Workshop on algorithmic foundations of robotics, GuanajuatoGoogle Scholar
  89. Wettergreen D, Tompkins P, Urmson C, Wagner M, Whittaker W (2005) Sun-synchronous robotics exploration: technical description and field experimentation. Int J Robot Res 24(1):3–30CrossRefGoogle Scholar
  90. Xia T, Yang M, Yang R, Wang C (2010) Cyberc3: a prototype cybernetic transportation system for urban applications. IEEE Trans Intell Transp Syst 11(1). doi:10.1109/TITS.2009.2036151Google Scholar

Copyright information

© Springer-Verlag London Ltd. 2012

Authors and Affiliations

  1. 1.INRIA Grenoble - Rhône-AlpesCNRS-LIG and Grenoble UniversityGrenobleFrance
  2. 2.Jet Propulsion LaboratoryCalifornia Institute of TechnologyPasadenaUSA

Personalised recommendations