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

Victor Reijgwart; Cesar Cadena; Roland Siegwart; Lionel Ott

cs.ROFeb 24, 2026

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

Authors: Victor Reijgwart, Cesar Cadena, Roland Siegwart, Lionel Ott

Organizations: Autonomous Systems Lab, ETH Zürich, Switzerland

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.

Figures & tables

Fig. 1: Illustration of a solution generated by our proposed any-angle path planner, wavestar . The shortest path ( green ) consists of straight-line segments that efficiently traverse free space using a small number of waypoints which tightly fit the obstacles (voxels shaded by height). A 2D slice of the multi-resolution 3D cost field demonstrates how the hierarchical algorithm refines resolution only where necessary, ensuring both efficiency and accuracy.

Fig. 2: Comparison of A* on an octree’s leaves (left, blue ), A* on a fixed-resolution grid (right, blue ), and Theta* (right, green ). On the octree, A* produces highly suboptimal paths. While its search space includes a path (dashed blue ) on the correct side of the obstacle (striped box), this path is ignored due to the detour introduced by passing through the leaves’ centers. A* on the grid finds shorter, smoother paths, but still performs worse than Theta*. Our method matches Theta*’s path quality while operating on octrees.

Fig. 3: Illustration of the cost-to-come ( $g$ cost) and predecessor fields of Theta* in a 2D environment with a single obstacle (striped box). The $g$ cost field (left) changes from cell to cell, while the predecessor field (right) is largely constant. All cells to the left of the obstacle ( green ) are directly visible from the start vertex (black circle) and thus use it as their predecessor. Cells near the top right ( purple ) connect through the cell at the obstacle’s top-left corner, while the rest ( gold ) connect through the cell at its top-right corner.

Fig. 4: Illustration of the importance of initializing inflection points. Without initialization (left), the retrieved shortest path may take large detours around obstacles. Initializing the cost field at a higher resolution near obstacles (right) resolves this issue, resulting in shorter, smoother paths. As the number of added subvolumes ( blue ) is small, the performance overhead remains minimal.

Fig. 5: Illustration of our dynamic refinement procedure. When a new path (dashed arrow) is discovered to a subvolume

\mathcal{V}^{\prime}

(left) that has already been reached (solid arrow), we evaluate its effect on the cost to reach each vertex in

\mathcal{V}^{\prime}

. The algorithm handles three cases: i) the new path reduces the cost for all vertices, replacing the previous path; ii) the new path does not improve any costs and is ignored; iii) some costs improve while others worsen. In this last case (right),

\mathcal{V}^{\prime}

is recursively subdivided until each child subvolume is fully resolved under case i or ii. Subvolumes are colored by their predecessor .

Fig. 6: Initialization procedure’s impact on path length and runtime relative to Theta* ( blue line). Reading the plot from left to right, we see how increasing the inflection point initialization resolution brings our path lengths (top) closer to Theta* while remaining significantly faster (bottom).

Fig. 7: Ablation showing how the dynamic refinement strategy affects the path length and speedup (log scale) of our method relative to Theta* ( blue line). Reading the plot from left to right, we see that as the threshold is tightened, the path lengths decrease while runtime moderately increases.

Path length $\mu\pm\sigma$		None	$10\text{\,}\mathrm{cm}$
		Initialization
Refinement	None	0.23% $\pm$ 0.43%	0.07% $\pm$ 0.13%
Refinement	$\epsilon=10^{-2}$	0.16% $\pm$ 0.32%	0.04 % $\pm$ 0.12 %

TABLE I: Summary of ablations. Path lengths relative to Theta*.

Success rate (%)	Mine	Cloister	Math	Park
A*	88	100	98	99
Theta*	88	100	98	99
LazyTheta*	88	100	98	99
OctreeLazyTheta*	88	100	98	99
RRTConnect	88	97	98	97
RRT* 0.1s	80	37	96	56

TABLE II: Planning success rates per map for 100 randomly sampled queries each, including infeasible cases.

Fig. 8: Path lengths for selected search- and sampling-based planners and three variants of our multi-resolution planner. The upper plot shows absolute lengths, and the lower plot shows lengths relative to Theta* ( blue line). Outliers for sampling-based planners are partially omitted in the lower plot. In particular, only the bottom few quantiles of RRTConnect are visible.

Mean path length (m)	Mine	Cloister	Math	Park
A*	15.96	20.49	45.10	106.05
Theta*	14.87	19.06	42.20	99.12
LazyTheta*	14.88	19.06	42.22	99.14
OctreeLazyTheta*	16.34	20.42	44.67	103.63
RRTConnect	30.49	39.49	73.83	155.89
RRT* 0.1s	17.50	19.59	44.05	106.52

TABLE III: Average path lengths per map for queries where all planners succeeded.

Fig. 9: Execution times for selected search- and sampling-based planners and three variants of our multi-resolution planner. The upper plot shows absolute times (log scale), and the lower plot shows speedups relative to Theta* ( blue line).

Mean execution time (s)	Mine	Cloister	Math	Park
A*	0.20	1.15	1.89	8.19
Theta*	2.70	30.29	61.18	281.38
LazyTheta*	0.42	3.66	6.89	31.51
OctreeLazyTheta*	2.88	10.79	13.12	64.94
RRTConnect	1.24	1.63	0.21	0.39
Ours	0.51	2.69	3.86	20.22

TABLE IV: Average execution times per map for 100 randomly sampled queries (RRT* variants omitted due to fixed runtimes).

Motion Planning

Motion planning finds safe, feasible paths and trajectories for robots and vehicles amid geometric, dynamic, and task constraints.

See all 392 papers

Optimal any-angle path planning in static and dynamic environments
Jun 30, 2026Yiyuan Zou, Clark BorstGraph-Based Planning Motion Planning
Control and Simulation, Faculty of Aerospace Engineering, Delft University of Technology, Delft, The Netherlands
Tiered-Resolution Volumetric Mapping for Real-Time Simultaneous Planning and Mapping on Natural Outdoor Environments
Oct 14, 2026Afonso E. Carvalho, David Portugal, Paulo Peixoto +13D Mapping Robot Navigation
Institute of Systems and Robotics, University of Coimbra, Coimbra, Portugal · Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USA
Parallel OctoMapping: A Scalable Framework for Enhanced Path Planning in Autonomous Navigation
Mar 23, 2026Yihui Mao, Tian Tan, Xuehui Shen +2Occupancy Grid Mapping Robot Motion Planning
Department of Mechanical and Aerospace Engineering, University of Florida, Gainesville, FL 32611 USA · Department of Electrical and Systems Engineering, University of Pennsylvania, Philadelphia, PA 19104 USA

Jun 30, 2026J/K move · Enter open · S save

Optimal any-angle path planning in static and dynamic environments

Yiyuan Zou, Clark Borst

Control and Simulation, Faculty of Aerospace Engineering, Delft University of Technology, Delft, The Netherlands

Graph-Based Planning Motion Planning Dynamic Obstacle Avoidance Safe Interval Path Planning

Open paper AskPDF arXiv

Any-angle path planning extends traditional graph-based path planning by allowing movement between any pair of vertices, rather than being restricted by predefined edges. It can find straighter and shorter paths in continuous space with graphs, making it particularly suitable for navigation in open areas. Many any-angle path-planning algorithms have been proposed, but only a few can guarantee optimal solutions. To address this challenge, this article focuses on optimal any-angle path planning on grids and introduces two general techniques that accelerate computation while preserving optimality in both static and dynamic environments: 1) elliptical forward expansion, which leverages ellipse-based neighborhoods to restrict the search space, and 2) field of view, which replaces traditional line-of-sight methods to speed up visibility checks. To integrate these two techniques, inverted and forward scanning are introduced. Inverted scanning establishes visual connections from open nodes, whereas forward scanning initiates scans from closed nodes. Building on the proposed techniques, Zeta* and Zeta*-SIPP are developed for static and dynamic environments respectively. Zeta*, when combined with forward scanning, is similar to the state-of-the-art algorithm Anya and attains comparable performance. Unlike Anya, Zeta* can be readily extended to other settings. For example, Zeta*-SIPP, which extends Zeta* by integrating Safe Interval Path Planning (SIPP), can find time-optimal any-angle paths in dynamic environments. Benchmarking results show that Zeta*-SIPP achieves more than an order-of-magnitude speedup over the state-of-the-art optimal planner TO-AA-SIPP when using either scanning method. Overall, this research identifies the key requirements for achieving optimal any-angle path planning and introduces a unified approach suitable for different environments.

Abstract

Motion Planning

Explore similar work

Optimal any-angle path planning in static and dynamic environments

Tiered-Resolution Volumetric Mapping for Real-Time Simultaneous Planning and Mapping on Natural Outdoor Environments

Parallel OctoMapping: A Scalable Framework for Enhanced Path Planning in Autonomous Navigation