“CostTrust: A Fast-Exploring, Iteratively Expanding Frontier-Based Kinodynamic Motion Planner”

Authors: Fetullah Atas, Grzegorz Cielniak and Lars Grimstad,
Affiliation: Norwegian University of Life Sciences and University of Lincoln
Reference: 2024, Vol 45, No 1, pp. 15-28.

Keywords: Motion Planning, Kinodynamic Planning, Sampling-Based Motion Planning

Abstract: Sampling-based motion planning has recently experienced considerable advancements, particularly in the domain of geometric motion planning for diverse robotic systems. Nonetheless, kinodynamic motion planning, which additionally considers a robot's kinematics and dynamics to generate a motion plan, remains an open challenge, necessitating further research. Kinodynamic planning, inherently more complex than geometric planning, mandates that the planner not only adheres to motion constraints but also account for system dynamics, including limitations in velocity and acceleration. Furthermore, kinodynamic planning often requires the navigation of extensive state and control spaces, rendering the process both computationally demanding and time-consuming. To effectively tackle kinodynamic motion planning, our proposed approach introduces a dynamic balance between exploration and exploitation, continuously adjusted throughout the execution. Our bi-directional and multi-threaded algorithm is specifically tailored to fulfill the efficiency requisites of kinodynamic motion planning. Our comprehensive benchmarks, conducted on an Ackermann-steered robot and a dynamic quadrotor, demonstrate that our method notably outperforms state-of-the-art baselines in terms of solution rate percentage and path cost. To facilitate accessibility and further research within the community, we have made the implementation of our method available. It is integrated with the Open Motion Planning Library (OMPL), a widely utilized resource in the field, enhancing our approach's practical applicability.

PDF PDF (4987 Kb)        DOI: 10.4173/mic.2024.1.2

[1] Gammell, J.D., Barfoot, T.D., and Srinivasa, S.S. (2020). Batch informed trees (bit*): Informed asymptotically optimal anytime search, The International Journal of Robotics Research. 39(5):543--567. https://doi.org/10.1177/0278364919890396, doi:10.1177/0278364919890396
[2] Hauser, K. and Zhou, Y. (2016). Asymptotically optimal planning by feasible kinodynamic planning in a state–cost space, IEEE Transactions on Robotics. 32(6):1431--1443. doi:10.1109/TRO.2016.2602363
[3] Hsu, D., Latombe, J.-C., and Motwani, R. (1997). Path planning in expansive configuration spaces, In Proceedings of International Conference on Robotics and Automation, volume3. pages 2719--2726 vol.3. doi:10.1109/ROBOT.1997.619371
[4] Karaman, S. and Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning, CoRR. abs/1105.1186. http://arxiv.org/abs/1105.1186.
[5] Karaman, S. and Frazzoli, E. (2013). Sampling-based optimal motion planning for non-holonomic dynamical systems, In 2013 IEEE International Conference on Robotics and Automation. pages 5041--5047. doi:10.1109/ICRA.2013.6631297
[6] Kavraki, L., Svestka, P., Latombe, J.-C., and Overmars, M. (1996). Probabilistic roadmaps for path planning in high-dimensional configuration spaces, IEEE Transactions on Robotics and Automation. 12(4):566--580. doi:10.1109/70.508439
[7] Kleinbort, M., Solovey, K., Bonalli, R., Bekris, K.E., and Halperin, D. (2019). RRT2, 0 for fast and optimal kinodynamic sampling-based motion planning. CoRR. abs/1909.05569. http://arxiv.org/abs/1909.05569.
[8] Kleinbort, M., Solovey, K., Littlefield, Z., Bekris, K., and Halperin, D. (2018). Probabilistic completeness of rrt for geometric and kinodynamic planning with forward propagation, IEEE Robotics and Automation Letters. PP:1--1. doi:10.1109/LRA.2018.2888947
[9] Lavalle, S. and Kuffner, J. (2000). Rapidly-exploring random trees: Progress and prospects, Algorithmic and computational robotics: New directions.
[10] LaValle, S.M. and James J.Kuffner, J. (2001). Randomized kinodynamic planning, The International Journal of Robotics Research. 20(5):378--400. https://doi.org/10.1177/02783640122067453, doi:10.1177/02783640122067453
[11] Li, Y., Littlefield, Z., and Bekris, K.E. (2014). Asymptotically optimal sampling-based kinodynamic planning, CoRR. abs/1407.2896. http://arxiv.org/abs/1407.2896.
[12] Littlefield, Z. and Bekris, K.E. (2018). Efficient and asymptotically optimal kinodynamic motion planning via dominance-informed regions, In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). pages 1--9. doi:10.1109/IROS.2018.8593672
[13] Luna, R., Şucan, I.A., Moll, M., and Kavraki, L.E. (2013). Anytime solution optimization for sampling-based motion planning, In 2013 IEEE International Conference on Robotics and Automation. pages 5068--5074. doi:10.1109/ICRA.2013.6631301
[14] Otte, M. and Correll, N. (2013). C-forest: Parallel shortest path planning with superlinear speedup, IEEE Transactions on Robotics. 29(3):798--806. doi:10.1109/TRO.2013.2240176
[15] Perez, A., Platt, R., Konidaris, G., Kaelbling, L., and Lozano-Perez, T. (2012). Lqr-rrt*: Optimal sampling-based motion planning with automatically derived extension heuristics, In 2012 IEEE International Conference on Robotics and Automation. pages 2537--2542. doi:10.1109/ICRA.2012.6225177
[16] Shome, R. and Kavraki, L.E. (2021). Asymptotically optimal kinodynamic planning using bundles of edges, In 2021 IEEE International Conference on Robotics and Automation (ICRA). pages 9988--9994. doi:10.1109/ICRA48506.2021.9560836
[17] Strub, M.P. and Gammell, J.D. (2020). Adaptively Informed Trees (AIT*): Fast asymptotically optimal path planning through adaptive heuristics, In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). pages 3191--3198, 2020. doi:10.1109/ICRA40945.2020.9197338
[18] Strub, M.P. and Gammell, J.D. (2020). Advanced bit* (abit*): Sampling-based planning with advanced graph-search techniques, 2020 IEEE International Conference on Robotics and Automation (ICRA), 2020. http://dx.doi.org/10.1109/ICRA40945.2020.9196580, doi:10.1109/icra40945.2020.9196580
[19] Sucan, I.A., Moll, M., and Kavraki, L.E. (2012). The open motion planning library, IEEE Robotics Automation Magazine. 19(4):72--82. doi:10.1109/MRA.2012.2205651
[20] Xie, C., vanden Berg, J., Patil, S., and Abbeel, P. (2015). Toward asymptotically optimal motion planning for kinodynamic systems using a two-point boundary value problem solver, In 2015 IEEE International Conference on Robotics and Automation (ICRA). pages 4187--4194. doi:10.1109/ICRA.2015.7139776

  title={{CostTrust: A Fast-Exploring, Iteratively Expanding Frontier-Based Kinodynamic Motion Planner}},
  author={Atas, Fetullah and Cielniak, Grzegorz and Grimstad, Lars},
  journal={Modeling, Identification and Control},
  publisher={Norwegian Society of Automatic Control}