Sampling-based optimal kinodynamic planning with motion primitives

  • Basak SakcakEmail author
  • Luca Bascetta
  • Gianni Ferretti
  • Maria Prandini


This paper proposes a novel sampling-based motion planner, which integrates in Rapidly exploring Random Tree star (\(\hbox {RRT}^{\star }\)) a database of pre-computed motion primitives to alleviate its computational load and allow for motion planning in a dynamic or partially known environment. The database is built by considering a set of initial and final state pairs in some grid space, and determining for each pair an optimal trajectory that is compatible with the system dynamics and constraints, while minimizing a cost. Nodes are progressively added to the tree of feasible trajectories in the \(\hbox {RRT}^{\star }\) algorithm by extracting at random a sample in the gridded state space and selecting the best obstacle-free motion primitive in the database that joins it to an existing node. The tree is rewired if some nodes can be reached from the new sampled state through an obstacle-free motion primitive with lower cost. The computationally more intensive part of motion planning is thus moved to the preliminary offline phase of the database construction at the price of some performance degradation due to gridding. Grid resolution can be tuned so as to compromise between (sub)optimality and size of the database. The planner is shown to be asymptotically optimal as the grid resolution goes to zero and the number of sampled states grows to infinity.


Optimal sampling-based planning Kinodynamic planning Motion primitives 



This work is supported by the European Commission under the project UnCoVerCPS with Grant Number 643921.


  1. Bertsekas, D. P., & Tsitsiklis, J. N. (2002). Introduction to probability (Vol. 1). Belmont, MA: Athena Scientific Belmont.Google Scholar
  2. Boggs, P. T., & Tolle, J. W. (1995). Sequential quadratic programming. Acta Numerica, 4, 1–51.MathSciNetCrossRefzbMATHGoogle Scholar
  3. Choset, H. M. (2005). Principles of robot motion: Theory, algorithms, and implementation. Cambridge: MIT press.zbMATHGoogle Scholar
  4. Dolgov, D., Thrun, S., Montemerlo, M., & Diebel, J. (2010). Path planning for autonomous vehicles in unknown semi-structured environments. The International Journal of Robotics Research, 29(5), 485–501.CrossRefGoogle Scholar
  5. Donald, B., Xavier, P., Canny, J., & Reif, J. (1993). Kinodynamic motion planning. J ACM, 40(5), 1048–1066. Scholar
  6. Gammell, J.D., Srinivasa, S.S., & Barfoot, T.D. (2014). BIT\(^{\star }\): Batch informed trees for optimal sampling-based planning via dynamic programming on implicit random geometric graphs. Tech. rep., Tech. Report TR-2014-JDG006, ASRL, University of Toronto.Google Scholar
  7. Goretkin, G., Perez, A., Platt, R., & Konidaris, G. (2013). Optimal sampling-based planning for linear-quadratic kinodynamic systems. In: IEEE International Conference on Robotics and Automation, IEEE, (pp. 2429–2436).Google Scholar
  8. Houska, B., Ferreau, H., & Diehl, M. (2011). ACADO Toolkit—An open source framework for automatic control and dynamic optimization. Optimal Control Applications and Methods, 32(3), 298–312.MathSciNetCrossRefzbMATHGoogle Scholar
  9. hwan Jeon, J., Karaman, S., & Frazzoli, E. (2011). Anytime computation of time-optimal off-road vehicle maneuvers using the RRT. In: IEEE Conference on Decision and Control and European Control Conference, (pp. 3276–3282).Google Scholar
  10. Karaman, S., & Frazzoli, E. (2010). Optimal kinodynamic motion planning using incremental sampling-based methods. In: IEEE Conference on Decision and Control, IEEE, (pp. 7681–7687).Google Scholar
  11. Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7), 846–894.CrossRefzbMATHGoogle Scholar
  12. Karaman, S., & Frazzoli, E. (2013). Sampling-based optimal motion planning for non-holonomic dynamical systems. In: IEEE International Conference on Robotics and Automation, (pp. 5041–5047).Google Scholar
  13. Kavraki, L. E., Svestka, P., Latombe, J. C., & Overmars, M. H. (1996). Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation, 12(4), 566–580.CrossRefGoogle Scholar
  14. Khalil, H. K. (1996). Nonlinear systems (Vol. 2). New Jersey: Prentice-Hall.Google Scholar
  15. Kuderer, M., Sprunk, C., Kretzschmar, H., & Burgard, W. (2014). Online generation of homotopically distinct navigation paths. In: IEEE International Conference on Robotics and Automation, (pp. 6462–6467).Google Scholar
  16. LaValle, S. M. (2006). Planning algorithms. Cambridge: Cambridge University Press.CrossRefzbMATHGoogle Scholar
  17. LaValle, S. M., & Kuffner, J. J, Jr. (2001). Randomized kinodynamic planning. The International Journal of Robotics Research, 20(5), 378–400.CrossRefGoogle Scholar
  18. Li, Y., Littlefield, Z., & Bekris, K. E. (2016). Asymptotically optimal sampling-based kinodynamic planning. The International Journal of Robotics Research, 35(5), 528–564.CrossRefGoogle Scholar
  19. Likhachev, M., & Ferguson, D. (2009). Planning long dynamically feasible maneuvers for autonomous vehicles. The International Journal of Robotics Research, 28(8), 933–945.CrossRefGoogle Scholar
  20. Likhachev, M., Ferguson, D., Gordon, G., Stentz, A., & Thrun, S. (2008). Anytime search in dynamic graphs. Artificial Intelligence, 172(14), 1613–1643.MathSciNetCrossRefzbMATHGoogle Scholar
  21. Park, J., Karumanchi, S., & Iagnemma, K. (2015). Homotopy-based divide-and-conquer strategy for optimal trajectory planning via mixed-integer programming. IEEE Transactions on Robotics, 31(5), 1101–1115.CrossRefGoogle Scholar
  22. Patterson, M. A., & Rao, A. V. (2014). GPOPS-II: A matlab software for solving multiple-phase optimal control problems using hp-adaptive Gaussian quadrature collocation methods and sparse nonlinear programming. ACM Transactions on Mathematical Software, 41(1), 1.MathSciNetCrossRefzbMATHGoogle Scholar
  23. Pearl, J. (1984). Heuristics: intelligent search strategies for computer problem solving. Boston: Addison.Google Scholar
  24. Perez, A., Platt, R., Konidaris, G., Kaelbling, L., & Lozano-Perez, T. (2012). LQR-RRT*: Optimal sampling-based motion planning with automatically derived extension heuristics. In: IEEE International Conference on Robotics and Automation, (pp. 2537–2542).Google Scholar
  25. Pivtoraiko, M., Knepper, R. A., & Kelly, A. (2009). Differentially constrained mobile robot motion planning in state lattices. Journal of Field Robotics, 26(3), 308–333.CrossRefGoogle Scholar
  26. Reif, J.H. (1979). Complexity of the mover’s problem and generalizations. In: Annual Symposium on Foundations of Computer Science, (pp. 421–427).Google Scholar
  27. Stentz, A. (1994). Optimal and efficient path planning for partially-known environments. In: 1994 IEEE International Conference on Robotics and Automation, 1994. Proceedings., IEEE, (pp 3310–3317).Google Scholar
  28. Stoneman, S., & Lampariello, R. (2014). Embedding nonlinear optimization in RRT* for optimal kinodynamic planning. In: 2014 IEEE 53rd Annual Conference on Decision and Control (CDC), IEEE, (pp 3737–3744).Google Scholar
  29. Webb, D.J., & van den Berg, J. (2013). Kinodynamic RRT\(^{{\star }}\): Asymptotically optimal motion planning for robots with linear dynamics. In: 2013 IEEE International Conference on Robotics and Automation (ICRA), IEEE, (pp. 5054–5061).Google Scholar

Copyright information

© Springer Science+Business Media, LLC, part of Springer Nature 2019

Authors and Affiliations

  1. 1.Dipartimento di Elettronica, Informazione e BioingegneriaPolitecnico di MilanoMilanItaly

Personalised recommendations