[Paper Review] Open-Source, Cost-Aware Kinematically Feasible Planning for Mobile and Surface Robotics

Steve Macenski, Matthew Booker+2 more|arXiv (Cornell University)|Jan 23, 2024

Robotic Path Planning Algorithms4 citations

TL;DR

This paper introduces Smac Planner, an open-source, C++-based framework for kinematically feasible path planning in mobile and surface robotics, implementing Cost-Aware variants of 2D-A*, Hybrid-A*, and State Lattice planners. It achieves real-time performance (39–42ms) in complex environments, outperforming SBPL and NavFn by orders of magnitude, and is now the standard planning system in ROS 2's Nav2 for industrial and research robots.

ABSTRACT

We present Smac Planner, an openly available, search-based planning framework that addresses the critical need for kinematically feasible path planning across diverse robot platforms. Smac Planner provides high-performance implementations of Cost-Aware A*, Hybrid-A*, and State Lattice planners that can be deployed for Ackermann, legged, and other large non-circular robots. Our framework introduces novel "Cost-Aware" variations that significantly improve performance in complex environments common to mobile robotics while maintaining kinematic feasibility constraints. Integrated as the standard planning system within the popular ROS 2 Navigation stack, Nav2, Smac Planner now powers thousands of robots worldwide across academic research, commercial applications, and field deployments.

Motivation & Objective

• Address the lack of performant, accessible, and kinematically feasible planners for modern mobile and surface robots.
• Develop a minimal, extensible C++ framework to simplify integration of search-based planners.
• Introduce Cost-Aware variations of Hybrid-A* and State Lattice planners tailored to mobile robotics conventions and performance needs.
• Benchmark the new planners against established frameworks (SBPL, NavFn, OMPL) in simulated and real-world environments.
• Enable deployment of Ackermann, non-circular, and legged robots in ROS 2 Nav2 through standardized, high-performance planning.

Proposed method

• Design a templated C++ framework allowing new search-based planners to be implemented in ~200 lines of code.
• Implement three core planners: 2D-A*, Hybrid-A*, and State Lattice, with kinematic feasibility enforced via motion primitives and curvature constraints.
• Introduce Cost-Aware heuristics and traversal functions that prioritize smooth, drivable paths while minimizing path length and computational cost.
• Use pre-generated control sets for State Lattice planners and Dubins/Reeds-Shepp models for Hybrid-A* to ensure kinematic feasibility.
• Apply consistent heuristic and traversal functions across planners to enable fair performance comparison.
• Integrate the framework into ROS 2’s Nav2, enabling real-world deployment across diverse robot platforms.

Figure 1: Large, non-circular industrial robot using Nav2 uniquely enabled by this open-source Framework.

Experimental results

Research questions

• RQ1 How can a minimal, open-source framework be designed to enable rapid implementation of performant search-based planners for mobile robotics?
• RQ2 What modifications to Hybrid-A* and State Lattice are necessary to ensure kinematic feasibility while optimizing for real-time performance in complex environments?
• RQ3 How do Cost-Aware variants of 2D-A*, Hybrid-A*, and State Lattice planners compare in path quality, computation time, and robustness against established frameworks like SBPL and NavFn?
• RQ4 To what extent can kinematically feasible planners be standardized and integrated into mainstream robotics frameworks like ROS 2 Nav2?
• RQ5 Can the proposed planners enable reliable navigation for non-holonomic, non-circular, and Ackermann-style robots in real-world industrial settings?

Key findings

• Cost-Aware Hybrid-A* and State Lattice planners achieved average run-times of 39–42ms in a 20% occupied map, outperforming Cost-Aware 2D-A* by ~50% and NavFn by ~38%.
• The path lengths of Cost-Aware Hybrid-A* and State Lattice were within 0.2% of each other and within 2.5% of the shorter paths generated by Cost-Aware 2D-A*.
• SBPL’s State Lattice ARA* planner was 150x slower than the Smac Planner’s Cost-Aware State Lattice, with significantly longer and less smooth paths.
• In a real-world 363,000 ft² warehouse, Smac’s Hybrid-A* achieved 290ms average planning time, State Lattice 473ms, and 2D-A* 1358ms, demonstrating superior scalability in dense environments.
• The Smac Planner enabled ROS 2 Nav2 to support Ackermann, large non-circular, and legged robots for the first time, with deployment in over a dozen organizations worldwide.
• The framework’s performance and integration have made it the standard open-source planning system in ROS 2 Nav2, accelerating research and deployment across robotics domains.

Figure 2: Outline of the A* and key node template methods.