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

AI summary

Overview

Research area: Robotics — search-based motion planning, specifically any-angle (Euclidean shortest path) planning on hierarchical 3D occupancy maps.

Technical level: Advanced. The paper assumes familiarity with A*, Theta*, octrees, cost-to-come fields, and concepts such as resolution completeness and asymptotic optimality.

Scope: The paper presents wavestar, a hierarchical any-angle planner that runs on multi-resolution 3D grids, matching Theta*'s path quality while being up to two orders of magnitude faster.

What This Paper Is About

Robots that must navigate large, cluttered 3D environments need to compute near-optimal collision-free paths quickly, but standard grid-based search such as A* scales linearly with explored volume and cubically with grid resolution, making it impractical at high resolution. Existing shortcuts — searching the centers of an octree's leaves, or using randomized sampling planners — are either noticeably suboptimal or offer no completeness or quality guarantees at all. The paper asks whether the accuracy of any-angle planning (straight-line paths that hug obstacle corners) can be combined with the efficiency of a multi-resolution octree representation, and answers that it can.

Key Contributions

1. A lossless multi-resolution cost field for any-angle planning. The authors observe that in Theta*, while the g-cost changes from cell to cell, large regions share the same predecessor. They exploit this by storing only predecessor(V) and g(predecessor(V)) per octree-leaf-sized region, and reconstructing the exact cost of any vertex inside it from straight-line distance. This compresses the search state without approximation error.

2. A coarse-to-fine hierarchical search with bounded error. The planner runs a modified A* over subvolumes (not vertices), expanding at the coarsest resolution possible and recursively subdividing only where a single predecessor cannot adequately serve all vertices in a region. A user-set threshold ε bounds the worst-case relative path-length suboptimality, and since the error is normalized by edge length, total accumulated error grows at most proportionally with path length.

3. Inflection-point initialization around obstacles. Because Euclidean shortest paths are taut — straight except where they wrap tightly around obstacles — the authors pre-seed the cost field with high-resolution traversable vertices adjacent to obstacles, ensuring no inflection point that Theta* would consider on the same grid is missed. This is done incrementally as the search expands, so its cost scales with explored volume rather than total map size.

4. An open-source framework plus a systematic evaluation. The full implementation is released at github.com/ethz-asl/wavestar, with ablations separating the effects of initialization and refinement, and comparisons against A*, Theta*, LazyTheta*, an octree-based LazyTheta* baseline, RRTConnect, and RRT* at three time budgets.

Main Findings

• Path quality effectively matches Theta.* With both initialization and refinement enabled, mean path length deviates from Theta* by only 0.04% ± 0.12%. With neither enabled it is 0.23% ± 0.43%. Occasional paths are marginally shorter than Theta*, because Theta* itself is not guaranteed optimal.

• Large speedups over the fixed-resolution baseline. The multi-resolution formulation runs up to two orders of magnitude faster than Theta* while retaining its accuracy, with the speedup independent of initialization resolution in obstacle-free regions.

• Randomized planners are far less reliable in constrained environments. On the Cloister sequence, RRT* at 0.1 s and 1 s succeeded on only 37% and 52% of queries respectively, versus 100% for all search-based planners including the proposed method. RRTConnect reached 97% there, but at a much higher path cost.

• Sampling-based paths are dramatically longer. On the Mine sequence, RRTConnect averaged 30.49 m versus 14.87 m for Theta*; on Park, 155.89 m versus 99.12 m. RRT* at a 10 s budget closed much of the gap but still trailed the search-based planners.

• The octree-leaf-center baseline confirms the paper's premise. OctreeLazyTheta* produced longer paths than even plain grid-based A* (16.34 m vs 15.96 m on Mine, 103.63 m vs 106.05 m on Park... i.e. it underperforms Theta* substantially), demonstrating that ignoring intra-leaf vertices causes real, non-recoverable optimality loss that post-processing cannot fix.

• Multi-resolution planners retain completeness empirically. Across all queries where any planner found a path, every search-based planner also found one, and no sampling-based planner solved a query the search-based planners could not.

• Both mechanisms contribute, and neither alone suffices. Initialization improves where inflection points are discovered; refinement improves where regions are served by multiple predecessors. Together they produce the lowest mean and lowest variance in path length relative to Theta*.

• Diminishing returns at coarse initialization. Initialization resolutions coarser than roughly 80 cm gave no measurable benefit, because the cost field's octree already conforms to obstacles, surrounding occupied cells with medium- and high-resolution subvolumes. The coarsest possible resolution is also capped at 6.4 m by the underlying octree structure, implicitly preventing very poor solutions.

• Lazy visibility checking transfers cleanly. The OursLazy and OursFast variants apply LazyTheta*'s deferred visibility evaluation, and OursFast further relaxes the initialization resolution to 40 cm, improving runtime with negligible path-quality cost.

Methodology in Plain English

The starting point is the observation that an occupancy map already contains a finite, structured description of free space — so instead of throwing that structure away by sampling random configurations, the authors search it directly, but at multiple resolutions simultaneously.

They build on Theta*, which differs from A* in one crucial way: when relaxing a neighbor, it checks whether that neighbor can be connected straight to the predecessor of the current node rather than to the current node itself. This removes intermediate waypoints, which is what produces those short, smooth, corner-hugging paths. The cost of implementing this is visibility checking, which is expensive in 3D.

The key insight for efficiency is that Theta*'s cost field is highly compressible. The g-cost varies continuously across the grid, but the predecessor is often identical across large swaths of space. So the authors partition the cost field into cubes matching an octree's leaves, storing one predecessor and one reference cost per cube, from which any contained vertex's cost is recovered by adding a straight-line distance.

On top of that representation they run a coarse-to-fine search. The open queue holds subvolumes rather than vertices, prioritized by the minimum f-score over their contents. When a newly discovered path reaches an already-reached subvolume, one of three things happens: the new path is better for every vertex inside (accept it), better for none (ignore it), or better for some and worse for others (subdivide the subvolume into its eight children and repeat recursively). This recursion stops when every child is unambiguously dominated by one predecessor, or when the remaining suboptimality falls below the threshold ε.

Two details make this practical rather than merely elegant. First, before searching, the planner seeds the cost field with candidate inflection points — high-resolution traversable vertices adjacent to obstacles — because those are the only places taut shortest paths can bend. Second, because all cost and heuristic terms are straight-line distances, the min/max over a subvolume's vertices can be evaluated by checking only a few extremal corners instead of every vertex.

The implementation is evaluated on five synthetic 100 m × 100 m × 100 m maps at 10 cm resolution with 0 to 4000 random obstacles, and on four real sequences from the Newer College Dataset mapped at 10 cm resolution with a 35 cm robot inflation radius, using 500 and 400 randomized collision-free start-goal pairs respectively.

Why This Matters

Impact on research. The paper demonstrates that multi-resolution search and any-angle optimality are not in tension — that the loss of path quality in prior octree-based planners (such as applying LazyTheta* to leaf centers) comes from a specific representational choice, not from hierarchy itself. It also challenges a common assumption that sampling-based planners are the only practical option for large 3D environments: on these benchmarks, deterministic search with multi-resolution compression was both more reliable and faster in cluttered scenes, and always produced shorter paths. The error-bounded refinement scheme offers a principled knob for trading quality against runtime, which is a useful pattern beyond this specific planner.

Real-world applications:

• Autonomous aerial and ground robots navigating between distant inspection points, storage locations, or work sites in large structures.
• Warehouse and logistics fleets where many start-goal queries must be answered quickly and consistently, and where a planner that reports infeasibility in finite time matters for fleet scheduling.
• Industrial and subterranean inspection in mines, tunnels, and cathedrals, where connectivity is limited, narrow passages are common, and simulation-based planners can stall indefinitely.
• Urban and off-road navigation, as demonstrated on the large Math and vegetated Park sequences, where the map spans hundreds of metres of mixed terrain.

Industry relevance. The framework is open-sourced and designed to drop into existing octree mapping stacks (it uses wavemap and interoperates with Octomap-based baselines). Deterministic, complete planning with explicit failure reporting is far easier to validate and certify than randomized planning with unbounded runtime, which matters for safety cases. The runtime reductions also translate directly into lower onboard compute requirements, and the ε parameter gives integrators explicit control over the accuracy-latency tradeoff for embedded systems.

Future Directions

• Motion constraints and kinodynamics. The planner explicitly ignores robot dynamics and treats the robot as a bounding sphere. Extending any-angle multi-resolution search to kinodynamic or state-lattice planning would broaden its applicability considerably.
• Robustness to imperfect maps. All evaluations use clean, static occupancy maps. How the approach behaves under map noise, partial observability, moving obstacles, and frequent replanning during execution remains unaddressed.
• Optimality relative to the true continuous optimum. The ε bound is relative to Theta* on the same grid, and Theta* itself is not guaranteed optimal. Quantifying and tightening the gap to the genuine Euclidean shortest path is an open problem.
• Cost-field reuse across queries and agents. Since the cost field stores predecessors rather than per-query path data, there may be substantial gains in multi-query or multi-robot settings from caching and incrementally updating it — a direction the paper does not explore.
• Adaptive threshold selection. ε and the initialization resolution are currently fixed by the user. Automatically tuning them based on environment structure or remaining time budget could make the anytime behavior more effective.

Target Audience

Robotics researchers and graduate students working on motion planning, navigation, and volumetric mapping; practitioners building autonomy stacks for drones, ground vehicles, or inspection robots who need fast, deterministic, near-optimal 3D path planning; and engineers interested in multi-resolution data structures, since the cost-field compression and recursive refinement scheme are transferable to other graph-search problems on hierarchical representations. Readers with a background in A* and octrees will get the most from the methods section; the ablation and comparison results are accessible to anyone evaluating planners for a practical system.

Authors’ abstract

Hierarchical, multi-resolution volumetric mapping approaches are widely used to represent large and complex environments as they can efficiently capture their occupancy and connectivity information. Yet widely used path planning methods such as sampling and trajectory optimization do not exploit this explicit connectivity information, and search-based methods such as A* suffer from scalability issues in large-scale high-resolution maps. In many applications, Euclidean shortest paths form the underpinning of the navigation system. For such applications, any-angle planning methods, which find optimal paths by connecting corners of obstacles with straight-line segments, provide a simple and efficient solution. In this paper, we present a method that has the optimality and completeness properties of any-angle planners while overcoming computational tractability issues common to search-based methods by exploiting multi-resolution representations. Extensive experiments on real and synthetic environments demonstrate the proposed approach's solution quality and speed, outperforming even sampling-based methods. The framework is open-sourced to allow the robotics and planning community to build on our research.