[Paper] Efficient Hierarchical Any-Angle Path Planning on Multi-Resolution 3D Grids

Source: arXiv - 2602.21174v1

Overview

The paper introduces a new any‑angle path‑planning algorithm that works on multi‑resolution 3D voxel grids. By marrying the optimality of classic any‑angle planners (e.g., Theta*) with a hierarchical map representation, the authors achieve fast, provably complete routes even in massive, high‑resolution environments—something that traditional A* or sampling‑based planners struggle with.

Key Contributions

• Hierarchical any‑angle planner that operates directly on multi‑resolution volumetric maps, preserving optimality and completeness.
• Resolution‑aware search: coarse‑level nodes guide the global route, while fine‑level refinements guarantee Euclidean‑shortest‑path quality.
• Open‑source implementation (C++/ROS) that integrates with popular mapping pipelines (OctoMap, Voxblox, etc.).
• Extensive benchmarking on both synthetic mazes and real‑world 3‑D scans, showing up to 5× speed‑up over state‑of‑the‑art sampling planners while maintaining sub‑centimeter path error.
• Demonstrated compatibility with downstream motion‑planning stacks (trajectory optimizers, MPC) without additional preprocessing.

Methodology

1. Multi‑Resolution Grid Construction

   • The environment is stored as a hierarchy of voxel layers (e.g., 0.05 m, 0.2 m, 1 m).
   • Each layer contains occupancy information and connectivity edges between free cells, so the planner can “see” corridors at the appropriate scale.

2. Coarse‑to‑Fine Search

   • Phase 1 – Global Sketch: A modified A* runs on the coarsest layer, treating each voxel as a node and using straight‑line “any‑angle” edges that skip over empty space. This yields a rough corridor.
   • Phase 2 – Refinement: The algorithm recursively descends into finer layers only along the corridor identified in the previous phase. At each level it performs a local any‑angle expansion (similar to Theta*), tightening the path while preserving the global topology.

3. Any‑Angle Edge Validation

   • Straight‑line segments are checked for collision using a hierarchical ray‑casting that first tests the coarse layer (fast rejection) and then the finer layer only if needed.

4. Optimality Guarantees

   • Because every refinement step respects the Euclidean distance metric and never discards a shorter feasible segment, the final path is optimal within the resolution of the finest layer, matching the guarantees of classic any‑angle planners.

5. Integration Hooks

   • The planner exposes a ROS service and a C++ API, allowing downstream planners to request a path and receive a sequence of 3‑D waypoints ready for smoothing or trajectory optimization.

Results & Findings

• Speed: The hierarchical approach reduces the search space dramatically; most of the expensive fine‑resolution checks are confined to a narrow corridor.
• Quality: Path lengths are within 1 % of the true Euclidean shortest path, outperforming sampling methods that often produce longer, jittery routes.
• Scalability: Planning time grows sub‑linearly with map size, unlike flat‑resolution A* whose runtime explodes on large maps.

Practical Implications

• Robotics & Drones: Fast, optimal 3‑D routes enable real‑time navigation for UAVs in cluttered warehouses or inspection robots in large industrial plants.
• Game & Simulation Engines: The same algorithm can replace costly nav‑mesh generation, delivering instant path updates when the environment changes (e.g., destructible terrain).
• Autonomous Vehicles: Hierarchical any‑angle planning can serve as a high‑level route generator that feeds smoother, kinodynamic planners, reducing overall latency in perception‑planning loops.
• Cloud‑Based Mapping Services: Because the planner works directly on voxel maps, developers can expose a “path‑as‑a‑service” endpoint that accepts a 3‑D occupancy grid and returns an optimal trajectory without pre‑computing nav‑meshes.

Limitations & Future Work

• Resolution Dependency: The optimality guarantee holds only up to the finest voxel size; extremely fine resolutions may still be prohibitive for memory‑constrained platforms.
• Dynamic Obstacles: The current implementation assumes a static occupancy grid; handling moving obstacles would require incremental updates to the hierarchical connectivity.
• Non‑Euclidean Costs: The planner is tuned for pure Euclidean distance; extending it to cost fields (e.g., risk, energy) is left for future research.
• Future Directions: The authors plan to integrate incremental map updates, explore adaptive resolution selection based on local complexity, and benchmark the method on heterogeneous hardware (GPU‑accelerated ray‑casting).

If you’re interested in trying the algorithm yourself, the authors have released a ROS package under the BSD license on GitHub, complete with tutorials for OctoMap and Voxblox integration.

Authors

• Victor Reijgwart
• Cesar Cadena
• Roland Siegwart
• Lionel Ott

Paper Information

• arXiv ID: 2602.21174v1
• Categories: cs.RO, cs.AI
• Published: February 24, 2026
• PDF: Download PDF