TL;DR
This paper introduces Strongly-Connected Subgoal Graphs (SC-SGs), a novel approach to speed up path planning on state lattices. By sacrificing strict optimality for completeness and significantly reducing query times, SC-SGs achieve speedups of up to 200x over traditional A* search, finding paths that are only slightly longer than optimal.
What Problem Does This Paper Address?
Path planning is a fundamental problem in robotics and AI, aiming to find a sequence of actions to move an agent from a start state to a goal state. State lattices are a powerful representation for motion planning, extending grid-based representations to include orientation and other state variables, allowing for kinematically feasible motion primitives. However, searching these state lattices directly can be computationally expensive.
Subgoal Graphs (SGs) offer a solution by preprocessing the state lattice to create an overlay graph. This allows for much faster queries by searching a smaller graph and then refining the path. While SGs have shown great success on grid maps, their effectiveness on state lattices is limited, offering only modest speedups. This is partly because state lattices can be more complex and have more edges than grids, and the SG construction doesn't fully exploit the structure of state lattices. The authors aim to improve SG performance on state lattices, specifically targeting faster query times while maintaining completeness.
Methods
The paper proposes Strongly-Connected Subgoal Graphs (SC-SGs), a variant of Subgoal Graphs designed for state lattices. The core idea is to relax the optimality guarantee of traditional SGs to achieve significant speedups.
Key methodological contributions include:
- SC-SG Construction: This involves two main steps:
- Identifying Access Subgoals: A set of subgoals is identified such that every node in the state lattice can be "R-connected" (reach a subgoal) and "R-connected" (be reached from a subgoal) to at least one of these subgoals. This ensures completeness.
- Ensuring Strong Connectivity: Additional "connecting subgoals" are added to make the subgraph formed by the access subgoals and the edges between them strongly connected. This is crucial for guaranteeing that a path can always be found between any two subgoals.
- Canonical Freespace Reachability (CR): A new reachability relation is introduced. Unlike standard freespace reachability (FR), CR imposes a canonical ordering on freespace paths. This means that among all possible shortest freespace paths between two states, one is chosen as the "canonical" path based on a lexicographical ordering of motion primitives. This allows for more efficient "CR-connect" operations, which are used to link start/goal states to the SC-SG.
- Algorithm Modifications: The authors adapt standard SG algorithms (R-connect, R-refine) to work with SC-SGs and the new CR relation. They also explore pruning-based SG construction and graph spanners as alternative approaches, though these proved less effective for state lattices.
Datasets
The experiments were conducted on two main setups:
- Unicycle Setup: A smaller grid (209x281) with 16 discrete poses and 5 primitives per pose.
- Urban Setup: A larger grid (768x1024) with 32 discrete poses and 36 primitives per pose.
These setups are described as being representative of typical motion planning scenarios. The paper does not introduce new benchmark datasets but uses these configurations to evaluate their proposed methods against existing ones.
Key Insights and Ideas
- The Trade-off is Crucial: The paper strongly emphasizes that for state lattices, strict optimality is often too costly. Sacrificing optimality for completeness and significant speedups is a viable and effective strategy.
- Canonical Paths Unlock Efficiency: The introduction of Canonical Freespace Reachability (CR) is a key innovation. By defining a unique "canonical" freespace path, the authors enable much faster connection operations (CR-connect) compared to standard freespace reachability (FR), especially in complex environments with many primitives.
- SC-SGs are Significantly Smaller: The SC-SG construction results in drastically fewer subgoals and edges compared to traditional SGs, leading to faster search times. This is a direct consequence of focusing on connectivity and completeness rather than optimality.
- Connection Time Dominates: For SC-SGs, especially with more complex environments, the time to connect the start and goal nodes to the SC-SG (connection time) becomes the dominant factor in query time, highlighting the importance of efficient connection strategies like CR-connect.
Key Results
- Massive Speedups: SC-SGs achieve speedups of 1-2 orders of magnitude (up to 200x) compared to A* search on state lattices.
- Near-Optimal Paths: While not strictly optimal, the paths found by SC-SGs are "close to optimal," with average suboptimality typically in the range of 1-10%. For example, on the Urban setup with 15 million nodes, SC-SGs found paths that were on average 9.2% longer than A* paths.
- Reduced Size: SC-SGs have significantly fewer subgoals and edges. For instance, C-DR150 (an SC-SG using DR) had 97.87% fewer nodes and 98.90% fewer edges than G-DR150 (a growing SG).
- Efficient Construction: SC-SG construction has significantly less preprocessing time than traditional SGs.
- CR vs. FR: Canonical Freespace Reachability (CR) leads to faster connection times (CR-connect is 7.8-8.2x faster than FR-connect on the Urban setup) and faster preprocessing, though it can result in slightly more subgoals.
Limitations and Future Work
- Bounded Suboptimality: The paper acknowledges that SC-SGs do not guarantee bounded suboptimality. While paths are close to optimal in practice, there's no formal guarantee on how much longer they might be. This is identified as a key area for future work.
- Subgoal Placement: The authors suggest that further improvements could be made by exploring different orders for identifying access subgoals and by adjusting reachability bounds for different parts of the SC-SG construction.
- Smoother Paths: Future work could involve smoothing subgoal paths using CR-refine with higher reachability bounds or iteratively bypassing nodes on subgoal paths.
- Memory Trade-offs: While SC-SGs reduce query time and size, the memory requirements for storing lookup tables (especially for CR on complex state lattices) can still be substantial.
Summary updated