Optimal Motion Planning for Autonomous Robotic Systems: Foundations, Algorithms, and Challenges

Charles L. Clark; Biyun Xie

Abstract

Autonomous robotic systems are increasingly deployed across manufacturing, logistics, and healthcare due to their ability to efficiently complete difficult and dangerous tasks. Motion planning, which generates a safe and feasible motion from an initial state to a goal state, is fundamental to these systems. Many applications also require optimizing performance criteria such as time or energy while satisfying kinematic and dynamic constraints, motivating the study of optimal motion planning. This paper provides a comprehensive review of optimal motion planning algorithms for autonomous robotic systems. The fundamental concepts of optimal motion planning are first introduced. Planners are then categorized into three classes: graph-based, tree-based, and trajectory optimization-based methods, and state-of-the-art algorithms within each category are reviewed. Key properties of each class are compared, and guidance on selecting an appropriate planner for a given application is provided. Finally, challenges and future directions in this research area are discussed.

References

1.
Keshvarparast, A.; Battini, D.; Battaia, O.; et al. Collaborative Robots in Manufacturing and Assembly Systems: Literature Review and Future Research Agenda. J. Intell. Manuf. 2024, 35, 2065–2118.
2.
2. Silvera-Tawil, D. Robotics in Healthcare: A Survey. SN Comput. Sci. 2024, 5, 189.
3.
Gil, G.; Casagrande, D.E.; Cort´es, L.P.; et al. Why the Low Adoption of Robotics in the Farms? Challenges for the Establishment of Commercial Agricultural Robots. Smart Agric. Technol. 2023, 3, 100069.
4.
Le Mero, L.; Yi, D.; Dianati, M.; et al. A Survey on Imitation Learning Techniques for End-to-End Autonomous Vehicles. IEEE Trans. Intell. Transp. Syst. 2022, 23, 14128–14147.
5.
Szot, A.; Clegg, A.; Undersander, E.; et al. Habitat 2.0: Training Home Assistants to Rearrange Their Habitat. Adv. Neural Inf. Process. Syst. 2021, 34, 251–266.
6.
Rahman, Q.M.; Corke, P.; Dayoub, F. Run-Time Monitoring of Machine Learning for Robotic Perception: A Survey of Emerging Trends. IEEE Access 2021, 9, 20067–20075.
7.
Zhou, C.; Huang, B.; Fr¨anti, P. A Review of Motion Planning Algorithms for Intelligent Robots. J. Intell. Manuf. 2022, 33, 387–424.
8.
Abbas, A.K.; Al Mashhadany, Y.; Hameed, M.J.; et al. Review of Intelligent Control Systems with Robotics. Indones. J. Electr. Eng. Inform. 2022, 10, 734–753.
9.
Touzani, H.; S´eguy, N.; Hadj-Abdelkader, H.; et al. Efficient Industrial Solution for Robotic Task Sequencing Problem with Mutual Collision Avoidance & Cycle Time Optimization. IEEE Robot. Autom. Lett. 2022, 7, 2597–2604.
10.
Zhao, S.; Zhang, J.; He, X.; et al. A Harmonized Approach: Beyond-the-Limit Control for Autonomous Vehicles Balancing Performance and Safety in Unpredictable Environments. IEEE Trans. Intell. Transp. Syst. 2024, 25, 15827–15840.
11.
Ollero, A.; Tognon, M.; Suarez, A.; et al. Past, Present, and Future of Aerial Robotic Manipulators. IEEE Trans. Robot. 2021, 38, 626–645.
12.
Khatib, O.; Jorda, M.; Park, J.; et al. Constraint-Consistent Task-Oriented Whole-Body Robot Formulation: Task, Posture, Constraints, Multiple Contacts, and Balance. Int. J. Rob. Res. 2022, 41, 1079–1098.
13.
Dong, L.; He, Z.; Song, C.; et al. A Review of Mobile Robot Motion Planning Methods: From Classical Motion Planning Workflows to Reinforcement Learning-Based Architectures. J. Syst. Eng. Electron. 2023, 34, 439–459.
14.
de Lima, C.R.; Khan, S.G.; Tufail, M.; et al. Humanoid Robot Motion Planning Approaches: A Survey. J. Intell. Robot. Syst. 2024, 110, 86.
15.
Orthey, A.; Chamzas, C.; Kavraki, L.E. Sampling-Based Motion Planning: A Comparative Review. Annu. Rev. Control Robot. Auton. Syst. 2023, 7, 285–310.
16.
Wang, J.; Zhang, T.; Ma, N.; et al. A Survey of Learning-Based Robot Motion Planning. IET Cyber-Syst. Robot. 2021, 3, 302–314.
17.
Yang, Y.; Pan, J.; Wan, W. Survey of Optimal Motion Planning. IET Cyber-Syst. Robot. 2019, 1, 13–19.
18.
Gammell, J.D.; Strub, M.P. Asymptotically Optimal Sampling-Based Motion Planning Methods. Annu. Rev. Control Robot.
Auton. Syst. 2021, 4, 295–318.
19.
Ioan, D.; Prodan, I.; Olaru, S.; et al. Mixed-Integer Programming in Motion Planning. Annu. Rev. Control 2021, 51, 65–87.
20.
Penicka, R.; Scaramuzza, D. Minimum-Time Quadrotor Waypoint Flight in Cluttered Environments. IEEE Robot. Autom. Lett. 2022, 7, 5719–5726.
21.
Zhu, B.; Bedeer, E.; Nguyen, H.H.; et al. UAV Trajectory Planning in Wireless Sensor Networks for Energy Consumption Minimization by Deep Reinforcement Learning. IEEE Trans. Veh. Technol. 2021, 70, 9540–9554.
22.
Wu, G.; Zhang, S. Real-Time Jerk-Minimization Trajectory Planning of Robotic Arm Based on Polynomial Curve Optimization. Proc. Inst. Mech. Eng. Part C J. Mech. Eng. Sci. 2022, 236, 10852–10864.
23.
Carvalho, J.P.; Aguiar, A.P. Deep Reinforcement Learning for Zero-Shot Coverage Path Planning with Mobile Robots. IEEE/CAA J. Autom. Sin. 2025, 12, 1594–1609.
24.
Sakcak, B.; LaValle, S.M. Complete Path Planning That Simultaneously Optimizes Length and Clearance. In Proceedings of the 2021 IEEE International Conference on Robotics and Automation (ICRA); Xi’an, China, 30 May–5 June 2021;
pp. 10100–10106.
25.
Wasiela, S.; Cognetti, M.; Giordano, P.R.; et al. Robust Motion Planning with Accuracy Optimization Based on Learned Sensitivity Metrics. IEEE Robot. Autom. Lett. 2024, 9, 10113–10120.
26.
Koptev, M.; Figueroa, N.; Billard, A. Real-Time Self-Collision Avoidance in Joint Space for Humanoid Robots. IEEE Robot. Autom. Lett. 2021, 6, 1240–1247.
27.
Chen, B.; Zhang, H.; Zhang, F.; et al. CIMAP: A High-Performance Motion Planning Algorithm for Robotic Manipulators in Complex Environments Using Clearance Inference Network. IEEE Trans. Syst. Man Cybern. Syst. 2025, 55, 6383–6396.
28.
Wen, Y.; Pagilla, P. Path-Constrained and Collision-Free Optimal Trajectory Planning for Robot Manipulators. IEEE Trans. Autom. Sci. Eng. 2022, 20, 763–774.
29.
Jelavic, E.; Qu, K.; Farshidian, F.; et al. LSTP: Long Short-Term Motion Planning for Legged and Legged-Wheeled Systems. IEEE Trans. Robot. 2023, 39, 4190–4210.
30.
Pang, T.; Suh, H.T.; Yang, L.; et al. Global Planning for Contact-Rich Manipulation via Local Smoothing of Quasi-Dynamic Contact Models. IEEE Trans. Robot. 2023, 39, 4691–4711.
31.
Pivtoraiko, M.; Knepper, R.A.; Kelly, A. Differentially Constrained Mobile Robot Motion Planning in State Lattices. J. Field Robot. 2009, 26, 308–333.
32.
Dijkstra, E.W. A Note on Two Problems in Connexion with Graphs. In Edsger Wybe Dijkstra: His Life, Work, and Legacy; Association for Computing Machinery: New York, NY, USA, 2022; pp. 287–290.
33.
Hart, P.E.; Nilsson, N.J.; Raphael, B. A Formal Basis for the Heuristic Determination of Minimum Cost Paths. IEEE Trans. Syst. Sci. Cybern. 1968, 4, 100–107.
34.
Zhang, D.; Gai, Y.; Ju, R.; et al. A Cosine Similarity Based Multitarget Path Planning Algorithm for Cable-Driven Manipulators. IEEE/ASME Trans. Mechatron. 2024, 30, 2379–2388.
35.
Stentz, A. The D* Algorithm for Real-Time Planning of Optimal Traverses; Technical Report; The Robotics Institute, Carnegie Mellon University: Pittsburgh, PA, USA, 1994.
36.
Koenig, S.; Likhachev, M. D* Lite. In Eighteenth National Conference on Artificial Intelligence; American Association for Artificial Intelligence: Menlo Park, CA, USA, 2002; pp. 476–483.
37.
Likhachev, M.; Ferguson, D.I.; Gordon, G.J.; et al. Anytime Dynamic A*: An Anytime, Replanning Algorithm.In Proceedings of the Fifteenth International Conference on Automated Planning and Scheduling, Monterey, CA, USA, 5–10 June 2005; Volume 5, pp. 262–271.
38.
Latombe, J.-C. Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces. IEEE Trans. Robot. Autom. 1996, 12, 566–580.
39.
Karaman, S.; Frazzoli, E. Sampling-Based Algorithms for Optimal Motion Planning. Int. J. Rob. Res. 2011, 30, 846–894.
40.
Bohlin, R.; Kavraki, L.E. Path Planning Using Lazy PRM. In Proceedings of the 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065), San Francisco, CA, USA, 24–28 April 2000; Volume 1, pp. 521–528.
41.
Dobson, A.; Bekris, K.E. Sparse Roadmap Spanners for Asymptotically Near-Optimal Motion Planning. Int. J. Rob. Res. 2014, 33, 18–47.
42.
Marcucci, T.; Petersen, M.; von Wrangel, D.; et al. Motion Planning Around Obstacles with Convex Optimization. Sci. Robot. 2023, 8, eadf7843.
43.
Marcucci, T.; Umenberger, J.; Parrilo, P.; et al. Shortest Paths in Graphs of Convex Sets. SIAM J. Optim. 2024, 34, 507–532.
44.
Cohn, T.; Petersen, M.; Simchowitz, M.; et al. Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets. arXiv 2023, arXiv:2305.06341.
45.
von Wrangel, D.; Tedrake, R. Using Graphs of Convex Sets to Guide Nonconvex Trajectory Optimization. In Proceedings of the 2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Abu Dhabi, United Arab Emirates, 14–18 October 2024; p. 8.
46.
Clark, C.L.; Xie, B. Plan Optimal Collision-Free Trajectories with Nonconvex Cost Functions Using Graphs of Convex Sets. IEEE Trans. Robot. 2025, 41, 5604–5623.
47.
Morozov, S.; Marcucci, T.; Amice, A.; et al. Multi-Query Shortest-Path Problem in Graphs of Convex Sets. arXiv 2024, arXiv:2409.19543.
48.
Graesdal, B.P.; Chia, S.Y.C.; Marcucci, T.; et al. Towards Tight Convex Relaxations for Contact-Rich Manipulation. arXiv 2024, arXiv:2402.10312.
49.
Chia, S.Y.C.; Jiang, R.H.; Graesdal, B.P.; et al. GCS*: Forward Heuristic Search on Implicit Graphs of Convex Sets. arXiv 2024, arXiv:2407.08848.
50.
Natarajan, R.; Liu, C.; Choset, H.; et al. Implicit Graph Search for Planning on Graphs of Convex Sets. arXiv 2024,
arXiv:2410.08909.
51.
Werner, P.; Amice, A.; Marcucci, T.; et al. Approximating Robot Configuration Spaces with Few Convex Sets Using Clique Covers of Visibility Graphs. In Proceedings of the 2024 IEEE International Conference on Robotics and Automation (ICRA), Yokohama, Japan, 13–17 May 2024; pp. 10359–10365.
52.
LaValle, S. Rapidly-Exploring Random Trees: A New Tool for Path Planning; Research Report 9811; Iowa State University: Ames, IA, USA, 1998.
53.
Kuffner, J.J.; LaValle, S.M. RRT-Connect: An Efficient Approach to Single-Query Path Planning. In Proceedings of the 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065), San Francisco, CA, USA, 24–28 April 2000; Volume 2, pp. 995–1001.
54.
Hu, B.; Cao, Z.; Zhou, M. An Efficient RRT-Based Framework for Planning Short and Smooth Wheeled Robot Motion Under Kinodynamic Constraints. IEEE Trans. Ind. Electron. 2020, 68, 3292–3302.
55.
Chiang, H.-T.L.; Hsu, J.; Fiser, M.; et al. RL-RRT: Kinodynamic Motion Planning via Learning Reachability Estimators from RL Policies. IEEE Robot. Autom. Lett. 2019, 4, 4298–4305.
56.
Arslan, O.; Tsiotras, P. Use of Relaxation Methods in Sampling-Based Algorithms for Optimal Motion Planning. In Proceedings of the 2013 IEEE International Conference on Robotics and Automation (ICRA), Karlsruhe, Germany, 6–10 May 2013; pp. 2421–2428.
57.
Otte, M.; Frazzoli, E. RRTX: Real-Time Motion Planning/Replanning for Environments with Unpredictable Obstacles. In Algorithmic Foundations of Robotics XI: Selected Contributions of the Eleventh InternationalWorkshop on the Algorithmic Foundations of Robotics; Springer: Istanbul, Turkey, 2015; pp. 461–478.
58.
Janson, L.; Schmerling, E.; Clark, A.; et al. Fast Marching Tree: A Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions. Int. J. Rob. Res. 2015, 34, 883–921.
59.
Starek, J.; Schmerling, E.; Janson, L.; et al. Bidirectional Fast Marching Trees: An Optimal Sampling-Based Algorithm for Bidirectional Motion Planning; Workshop on Algorithmic Foundations of Robotics; Stanford University: Stanford, CA, USA, 2024.
60.
Gammell, J.D.; Srinivasa, S.S.; Barfoot, T.D. Informed RRT*: Optimal Sampling-Based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic. In Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Chicago, IL, USA, 14–18 September 2014; pp. 2997–3004.
61.
Gammell, J.D.; Srinivasa, S.S.; Barfoot, T.D. Batch Informed Trees (BIT*): Sampling-Based Optimal Planning via the Heuristically Guided Search of Implicit Random Geometric Graphs. In Proceedings of the 2015 IEEE International Conference on Robotics and Automation (ICRA), Seattle, WA, USA, 26–30 May 2015; pp. 3067–3074.
62.
Strub, M.P.; Gammell, J.D. Adaptively Informed Trees (AIT*) and Effort Informed Trees (EIT*): Asymmetric Bidirectional Sampling-Based Path Planning. Int. J. Rob. Res. 2022, 41, 390–417.
63.
Ratliff, N.; Zucker, M.; Bagnell, J.A.; et al. CHOMP: Gradient Optimization Techniques for Efficient Motion Planning. In Proceedings of the 2009 IEEE International Conference on Robotics and Automation (ICRA), Kobe, Japan, 12–17 May 2009; pp. 489–494.
64.
Kalakrishnan, M.; Chitta, S.; Theodorou, E.; et al. STOMP: Stochastic Trajectory Optimization for Motion Planning. In Proceedings of the 2011 IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China, 9–13 May 2011; pp. 4569–4574.
65.
Schulman, J.; Duan, Y.; Ho, J.; et al. Motion Planning with Sequential Convex Optimization and Convex Collision Checking. Int. J. Rob. Res. 2014, 33, 1251–1270.
66.
Sundaralingam, B.; Hari, S.K.S.; Fishman, A.; et al. CuRobo: Parallelized Collision-Free Robot Motion Generation. In Proceedings of the 2023 IEEE International Conference on Robotics and Automation (ICRA), London, UK, 29 May–2 June 2023; pp. 8112–8119.
67.
Chamzas, C.; Quintero-Pe˜na, C.; Kingston, Z.; et al. MotionBenchMaker: A Tool to Generate and Benchmark Motion Planning Datasets. IEEE Robot. Autom. Lett. 2022, 7, 882–889.

Scilight Press

Author Information

Abstract

Keywords

References

About Scilight

Journals

Publishing Policies

Contact Us