Skip to main content
SpringerLink
Log in

Sampling-based optimal kinodynamic planning with motion primitives

  • Published:
Autonomous Robots Aims and scope Submit manuscript

Abstract

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.

This is a preview of subscription content, log in via an institution to check access.

Access this article

Log in via an institution

Price excludes VAT (USA)
Tax calculation will be finalised during checkout.

Instant access to the full article PDF.

Institutional subscriptions

Fig. 1
Fig. 2
Fig. 3
Fig. 4
Fig. 5
Fig. 6
Fig. 7
Fig. 8
Fig. 9
Fig. 10
Fig. 11
Fig. 12
Fig. 13
Fig. 14
Fig. 15
Fig. 16

Similar content being viewed by others

Notes

  1. This approach can be easily extended in case stronger invariance properties hold.

  2. In this section, a \({\varDelta }\) superscript is used to denote all variables that are associated with the grid state space, so as to distinguish them from their continuous state space counterpart.

  3. A system is STLA from a state \(\mathbf {q}\in Q\) if \(\forall T>0\) the reachable set of states from q in time \(0<t \le T\), \(\mathcal {R}(\mathbf {q}, \le T)\) contains a d-dimensional subset of \(\mathcal {N}\), where \(\mathcal {N}\) denotes the set of neighborhood states in terms of Euclidean distance, (Choset 2005).

  4. It is assumed that \(\mathcal {K}_f \ge 1\).

References

  • Bertsekas, D. P., & Tsitsiklis, J. N. (2002). Introduction to probability (Vol. 1). Belmont, MA: Athena Scientific Belmont.

    Google Scholar 

  • Boggs, P. T., & Tolle, J. W. (1995). Sequential quadratic programming. Acta Numerica, 4, 1–51.

    Article  MathSciNet  MATH  Google Scholar 

  • Choset, H. M. (2005). Principles of robot motion: Theory, algorithms, and implementation. Cambridge: MIT press.

    MATH  Google Scholar 

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

    Article  Google Scholar 

  • Donald, B., Xavier, P., Canny, J., & Reif, J. (1993). Kinodynamic motion planning. J ACM, 40(5), 1048–1066. https://doi.org/10.1145/174147.174150.

    Article  MathSciNet  MATH  Google Scholar 

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

  • 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).

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

    Article  MathSciNet  MATH  Google Scholar 

  • 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).

  • 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).

  • Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7), 846–894.

    Article  MATH  Google Scholar 

  • 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).

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

    Article  Google Scholar 

  • Khalil, H. K. (1996). Nonlinear systems (Vol. 2). New Jersey: Prentice-Hall.

    Google Scholar 

  • 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).

  • LaValle, S. M. (2006). Planning algorithms. Cambridge: Cambridge University Press.

    Book  MATH  Google Scholar 

  • LaValle, S. M., & Kuffner, J. J, Jr. (2001). Randomized kinodynamic planning. The International Journal of Robotics Research, 20(5), 378–400.

    Article  Google Scholar 

  • Li, Y., Littlefield, Z., & Bekris, K. E. (2016). Asymptotically optimal sampling-based kinodynamic planning. The International Journal of Robotics Research, 35(5), 528–564.

    Article  Google Scholar 

  • Likhachev, M., & Ferguson, D. (2009). Planning long dynamically feasible maneuvers for autonomous vehicles. The International Journal of Robotics Research, 28(8), 933–945.

    Article  Google Scholar 

  • Likhachev, M., Ferguson, D., Gordon, G., Stentz, A., & Thrun, S. (2008). Anytime search in dynamic graphs. Artificial Intelligence, 172(14), 1613–1643.

    Article  MathSciNet  MATH  Google Scholar 

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

    Article  Google Scholar 

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

    Article  MathSciNet  MATH  Google Scholar 

  • Pearl, J. (1984). Heuristics: intelligent search strategies for computer problem solving. Boston: Addison.

    Google Scholar 

  • 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).

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

    Article  Google Scholar 

  • Reif, J.H. (1979). Complexity of the mover’s problem and generalizations. In: Annual Symposium on Foundations of Computer Science, (pp. 421–427).

  • 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).

  • 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).

  • 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).

Download references

Acknowledgements

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

Author information

Authors and Affiliations

  1. Dipartimento di Elettronica, Informazione e Bioingegneria, Politecnico di Milano, Piazza L. Da Vinci 32, 20133, Milan, Italy

    Basak Sakcak, Luca Bascetta, Gianni Ferretti & Maria Prandini

Authors
  1. Basak Sakcak
    View author publications

    You can also search for this author in PubMed Google Scholar

  2. Luca Bascetta
    View author publications

    You can also search for this author in PubMed Google Scholar

  3. Gianni Ferretti
    View author publications

    You can also search for this author in PubMed Google Scholar

  4. Maria Prandini
    View author publications

    You can also search for this author in PubMed Google Scholar

Corresponding author

Correspondence to Basak Sakcak.

Additional information

Publisher's Note

Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.

Rights and permissions

Reprints and permissions

About this article

Check for updates. Verify currency and authenticity via CrossMark

Cite this article

Sakcak, B., Bascetta, L., Ferretti, G. et al. Sampling-based optimal kinodynamic planning with motion primitives. Auton Robot 43, 1715–1732 (2019). https://doi.org/10.1007/s10514-019-09830-x

Download citation

  • Received:

  • Accepted:

  • Published:

  • Issue Date:

  • DOI: https://doi.org/10.1007/s10514-019-09830-x

Keywords

Search

Navigation