Papers
Topics
Authors
Recent
Gemini 2.5 Flash
Gemini 2.5 Flash
134 tokens/sec
GPT-4o
9 tokens/sec
Gemini 2.5 Pro Pro
47 tokens/sec
o3 Pro
4 tokens/sec
GPT-4.1 Pro
38 tokens/sec
DeepSeek R1 via Azure Pro
28 tokens/sec
2000 character limit reached

Fast Path Planning Through Large Collections of Safe Boxes (2305.01072v2)

Published 1 May 2023 in cs.RO, cs.SY, and eess.SY

Abstract: We present a fast algorithm for the design of smooth paths (or trajectories) that are constrained to lie in a collection of axis-aligned boxes. We consider the case where the number of these safe boxes is large, and basic preprocessing of them (such as finding their intersections) can be done offline. At runtime we quickly generate a smooth path between given initial and terminal positions. Our algorithm designs trajectories that are guaranteed to be safe at all times, and detects infeasibility whenever such a trajectory does not exist. Our algorithm is based on two subproblems that we can solve very efficiently: finding a shortest path in a weighted graph, and solving (multiple) convex optimal-control problems. We demonstrate the proposed path planner on large-scale numerical examples, and we provide an efficient open-source software implementation, fastpathplanning.

Definition Search Book Streamline Icon: https://streamlinehq.com
References (64)
  1. S. LaValle and R. Sharma, “On motion planning in changing, partially predictable environments,” The International Journal of Robotics Research, vol. 16, no. 6, pp. 775–805, 1997.
  2. P. Fiorini and Z. Shiller, “Motion planning in dynamic environments using velocity obstacles,” The international journal of robotics research, vol. 17, no. 7, pp. 760–772, 1998.
  3. S. Petti and T. Fraichard, “Safe motion planning in dynamic environments,” in International Conference on Intelligent Robots and Systems.   IEEE/RJS, 2005, pp. 2210–2215.
  4. N. Du Toit and J. Burdick, “Robot motion planning in dynamic, uncertain environments,” IEEE Transactions on Robotics, vol. 28, no. 1, pp. 101–115, 2011.
  5. E. Frazzoli, M. Dahleh, and E. Feron, “Real-time motion planning for agile autonomous vehicles,” Journal of guidance, control, and dynamics, vol. 25, no. 1, pp. 116–129, 2002.
  6. Y. Kuwata, J. Teo, G. Fiore, S. Karaman, E. Frazzoli, and J. P. How, “Real-time motion planning with applications to autonomous urban driving,” IEEE Transactions on control systems technology, vol. 17, no. 5, pp. 1105–1118, 2009.
  7. C. Katrakazas, M. Quddus, W.-H. Chen, and L. Deka, “Real-time motion planning methods for autonomous on-road driving: State-of-the-art and future research directions,” Transportation Research Part C: Emerging Technologies, vol. 60, pp. 416–442, 2015.
  8. D. Sadigh, S. Sastry, S. A. Seshia, and A. D. Dragan, “Planning for autonomous cars that leverage effects on human actions,” in Robotics: Science and systems, vol. 2.   Ann Arbor, MI, USA, 2016, pp. 1–9.
  9. L. Wang, A. D. Ames, and M. Egerstedt, “Safety barrier certificates for collisions-free multirobot systems,” IEEE Transactions on Robotics, vol. 33, no. 3, pp. 661–674, 2017.
  10. A. Liniger and J. Lygeros, “A noncooperative game approach to autonomous racing,” IEEE Transactions on Control Systems Technology, vol. 28, no. 3, pp. 884–897, 2019.
  11. R. Spica, E. Cristofalo, Z. Wang, E. Montijano, and M. Schwager, “A real-time game theoretic planner for autonomous two-player drone racing,” IEEE Transactions on Robotics, vol. 36, no. 5, pp. 1389–1403, 2020.
  12. R. Deits and R. Tedrake, “Efficient mixed-integer planning for UAVs in cluttered environments,” in International Conference on Robotics and Automation.   IEEE, 2015, pp. 42–49.
  13. T. Marcucci, M. Petersen, D. von Wrangel, and R. Tedrake, “Motion planning around obstacles with convex optimization,” Science Robotics, vol. 8, no. 84, p. eadf7843, 2023.
  14. J.-M. Lien and N. Amato, “Approximate convex decomposition of polygons,” in Proceedings of the twentieth annual symposium on Computational geometry, 2004, pp. 17–26.
  15. N. Ayanian and V. Kumar, “Abstractions and controllers for groups of robots in environments with obstacles,” in International Conference on Robotics and Automation.   IEEE, 2010.
  16. M. Ghosh, N. M. Amato, Y. Lu, and J.-M. Lien, “Fast approximate convex decomposition using relative concavity,” Computer-Aided Design, vol. 45, no. 2, pp. 494–504, 2013.
  17. R. Deits and R. Tedrake, “Computing large convex regions of obstacle-free space through semidefinite programming,” in Algorithmic Foundations of Robotics XI.   Springer, 2015, pp. 109–124.
  18. P. Werner, A. Amice, T. Marcucci, D. Rus, and R. Tedrake, “Approximating robot configuration spaces with few convex sets using clique covers of visibility graphs,” arXiv preprint arXiv:2310.02875, 2023.
  19. A. Amice, H. Dai, P. Werner, A. Zhang, and R. Tedrake, “Finding and optimizing certified, collision-free regions in configuration space for robot manipulators,” in Algorithmic Foundations of Robotics XV.   Springer, 2022, pp. 328–348.
  20. M. Verghese, N. Das, Y. Zhi, and M. Yip, “Configuration space decomposition for scalable proxy collision checking in robot planning and control,” IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 3811–3818, 2022.
  21. H. Dai, A. Amice, P. Werner, A. Zhang, and R. Tedrake, “Certified polyhedral decompositions of collision-free configuration space,” arXiv preprint arXiv:2302.12219, 2023.
  22. M. Petersen and R. Tedrake, “Growing convex collision-free regions in configuration space using nonlinear programming,” arXiv preprint arXiv:2303.14737, 2023.
  23. Y. Wang and S. Boyd, “Fast model predictive control using online optimization,” IEEE Transactions on control systems technology, vol. 18, no. 2, pp. 267–278, 2009.
  24. T. Marcucci, J. Umenberger, P. Parrilo, and R. Tedrake, “Shortest paths in graphs of convex sets,” arXiv preprint arXiv:2101.11565v4, 2021.
  25. T. Schouwenaars, B. De Moor, E. Feron, and J. How, “Mixed integer programming for multi-vehicle path planning,” in European control conference.   IEEE, 2001, pp. 2603–2608.
  26. A. Richards, J. Bellingham, M. Tillerson, and J. How, “Coordination and control of multiple uavs,” in AIAA guidance, navigation, and control conference and exhibit, 2002, p. 4588.
  27. D. Mellinger, A. Kushleyev, and V. Kumar, “Mixed-integer quadratic program trajectory generation for heterogeneous quadrotor teams,” in International Conference on Robotics and Automation.   IEEE, 2012, pp. 477–483.
  28. F. Augugliaro, A. Schoellig, and R. D’Andrea, “Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programming approach,” in International Conference on Intelligent Robots and Systems.   IEEE/RJS, 2012, pp. 1917–1922.
  29. J. Schulman, Y. Duan, J. Ho, A. Lee, I. Awwal, H. Bradlow, J. Pan, S. Patil, K. Goldberg, and P. Abbeel, “Motion planning with sequential convex optimization and convex collision checking,” The International Journal of Robotics Research, vol. 33, no. 9, pp. 1251–1270, 2014.
  30. X. Liu and P. Lu, “Solving nonconvex optimal control problems by convex optimization,” Journal of Guidance, Control, and Dynamics, vol. 37, no. 3, pp. 750–765, 2014.
  31. A. Majumdar and R. Tedrake, “Funnel libraries for real-time robust feedback motion planning,” The International Journal of Robotics Research, vol. 36, no. 8, pp. 947–982, 2017.
  32. R. Bonalli, A. Cauligi, A. Bylard, and M. Pavone, “Gusto: Guaranteed sequential trajectory optimization via sequential convex programming,” in 2019 International conference on robotics and automation (ICRA).   IEEE, 2019, pp. 6741–6747.
  33. X. Zhang, A. Liniger, and F. Borrelli, “Optimization-based collision avoidance,” IEEE Transactions on Control Systems Technology, vol. 29, no. 3, pp. 972–983, 2020.
  34. L. Kavraki, P. Svestka, J.-C. Latombe, and M. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” IEEE Transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996.
  35. S. LaValle, “Rapidly-exploring random trees: A new tool for path planning,” TR 98-11, Computer Science Department, Iowa State University, 1998.
  36. S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” The international journal of robotics research, vol. 30, no. 7, pp. 846–894, 2011.
  37. N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp: Gradient optimization techniques for efficient motion planning,” in 2009 IEEE International Conference on Robotics and Automation.   IEEE, 2009, pp. 489–494.
  38. M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor, and S. Schaal, “STOMP: Stochastic trajectory optimization for motion planning,” in 2011 IEEE International Conference on Robotics and Automation.   IEEE, 2011, pp. 4569–4574.
  39. K. Hauser, “Learning the problem-optimum map: Analysis and application to global optimization in robotics,” IEEE Transactions on Robotics, vol. 33, no. 1, pp. 141–152, 2016.
  40. C. Belta, V. Isler, and G. J. Pappas, “Discrete abstractions for robot motion planning and control in polygonal environments,” IEEE Transactions on Robotics, vol. 21, no. 5, pp. 864–874, 2005.
  41. R. Alur, T. A. Henzinger, G. Lafferriere, and G. J. Pappas, “Discrete abstractions of hybrid systems,” Proceedings of the IEEE, vol. 88, no. 7, pp. 971–984, 2000.
  42. R. Tedrake, I. Manchester, M. Tobenkin, and J. Roberts, “Lqr-trees: Feedback motion planning via sums-of-squares verification,” The International Journal of Robotics Research, vol. 29, no. 8, pp. 1038–1052, 2010.
  43. S. Singh, A. Majumdar, J.-J. Slotine, and M. Pavone, “Robust online motion planning via contraction theory and convex optimization,” in 2017 IEEE International Conference on Robotics and Automation (ICRA).   IEEE, 2017, pp. 5883–5890.
  44. A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, “Control barrier function based quadratic programs for safety critical systems,” IEEE Transactions on Automatic Control, vol. 62, no. 8, pp. 3861–3876, 2016.
  45. A. Weiss, F. Leve, M. Baldwin, J. R. Forbes, and I. Kolmanovsky, “Spacecraft constrained attitude control using positively invariant constraint admissible sets on S⁢O⁢(3)×ℝ3𝑆𝑂3superscriptℝ3SO(3)\times\mathbb{R}^{3}italic_S italic_O ( 3 ) × blackboard_R start_POSTSUPERSCRIPT 3 end_POSTSUPERSCRIPT,” in 2014 American Control Conference.   IEEE, 2014, pp. 4955–4960.
  46. A. Weiss, C. Petersen, M. Baldwin, R. S. Erwin, and I. Kolmanovsky, “Safe positively invariant sets for spacecraft obstacle avoidance,” Journal of Guidance, Control, and Dynamics, vol. 38, no. 4, pp. 720–732, 2015.
  47. K. Berntorp, R. Bai, K. F. Erliksson, C. Danielson, A. Weiss, and S. Di Cairano, “Positive invariant sets for safe integrated vehicle motion planning and control,” IEEE Transactions on Intelligent Vehicles, vol. 5, no. 1, pp. 112–126, 2019.
  48. C. Danielson, K. Berntorp, A. Weiss, and S. Di Cairano, “Robust motion planning for uncertain systems with disturbances using the invariant-set motion planner,” IEEE Transactions on Automatic Control, vol. 65, no. 10, pp. 4456–4463, 2020.
  49. J.-w. Choi, R. Curry, and G. Elkaim, “Path planning based on bézier curve for autonomous ground vehicles,” in Advances in Electrical and Electronics Engineering-IAENG Special Edition of the World Congress on Engineering and Computer Science 2008.   IEEE, 2008, pp. 158–166.
  50. B. Lau, C. Sprunk, and W. Burgard, “Kinodynamic motion planning for mobile robots using splines,” in International Conference on Intelligent Robots and Systems.   IEEE/RJS, 2009, pp. 2427–2433.
  51. M. Elbanhawi, M. Simic, and R. Jazar, “Continuous path smoothing for car-like robots using b-spline curves,” Journal of Intelligent & Robotic Systems, vol. 80, pp. 23–56, 2015.
  52. J. Park and H. J. Kim, “Fast trajectory planning for multiple quadrotors using relative safe flight corridor,” in International Conference on Intelligent Robots and Systems.   IEEE/RSJ, 2019, pp. 596–603.
  53. F. Koolen, “Balance control and locomotion planning for humanoid robots using nonlinear centroidal models,” Ph.D. dissertation, Massachusetts Institute of Technology, 2020.
  54. J. Tordesillas, B. T. Lopez, M. Everett, and J. P. How, “Faster: Fast and safe trajectory planner for navigation in unknown environments,” IEEE Transactions on Robotics, vol. 38, no. 2, pp. 922–938, 2021.
  55. N. Csomay-Shanklin, A. Taylor, U. Rosolia, and A. Ames, “Multi-rate planning and control of uncertain nonlinear systems: Model predictive control and control Lyapunov functions,” arXiv preprint arXiv:2204.00152, 2022.
  56. Ö. Arslan and A. Tiemessen, “Adaptive bézier degree reduction and splitting for computationally efficient motion planning,” IEEE Transactions on Robotics, vol. 38, no. 6, pp. 3655–3674, 2022.
  57. A. Zomorodian and H. Edelsbrunner, “Fast software for box intersections,” in Proceedings of the sixteenth annual symposium on computational geometry, 2000, pp. 129–138.
  58. M. Lobo, L. Vandenberghe, S. Boyd, and H. Lebret, “Applications of second-order cone programming,” Linear algebra and its applications, vol. 284, no. 1-3, pp. 193–228, 1998.
  59. P. Virtanen et al., “SciPy 1.0: Fundamental Algorithms for Scientific Computing in Python,” Nature Methods, vol. 17, pp. 261–272, 2020.
  60. R. Farouki and V. Rajan, “Algorithms for polynomials in Bernstein form,” Computer Aided Geometric Design, vol. 5, no. 1, pp. 1–26, 1988.
  61. S. Diamond and S. Boyd, “CVXPY: A Python-embedded modeling language for convex optimization,” Journal of Machine Learning Research, vol. 17, no. 83, pp. 1–5, 2016.
  62. A. Hagberg, D. Schult, and P. Swart, “Exploring network structure, dynamics, and function using NetworkX,” in Proceedings of the 7th Python in Science Conference, G. Varoquaux, T. Vaught, and J. Millman, Eds., Pasadena, CA USA, 2008, pp. 11 – 15.
  63. P. Goulart and Y. Chen, “Clarabel solver documentation,” 2023. [Online]. Available: https://oxfordcontrol.github.io/ClarabelDocs/stable/
  64. D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in International Conference on Robotics and Automation.   IEEE, 2011, pp. 2520–2525.
Citations (14)
View on Semantic Scholar

Summary

We haven't generated a summary for this paper yet.

Ai Sparkles Streamline Icon: https://streamlinehq.com Summarize Now