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 V′ (left) that has already been reached (solid arrow), we evaluate its effect on the cost to reach each vertex in V′ . 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), V′ 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.
Initialization
Path length μ±σ
None
10cm
Refinement
None
0.23% ± 0.43%
0.07% ± 0.13%
ϵ=10−2
0.16% ± 0.32%
0.04 % ± 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).
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.
Yiyuan Zou, Clark Borst
Control and Simulation, Faculty of Aerospace Engineering, Delft University of Technology, Delft, The Netherlands
Real-time volumetric mapping for a planning robot is dominated by the cost of integrating and storing information at the finest resolution, yet only a small fraction of any map ever lies on or near a path the planner actually considers. We exploit this with a tiered-resolution mapping mechanism that integrates LiDAR into a single map structure at two resolutions simultaneously: fine inside a corridor around the robot's current plan, and coarse everywhere else. As opposed to traditional navigation pipelines where path planning uses the mapping result and provides no feedback, we propose a feedback loop where planning also informs mapping to dynamically integrate information at different leaf resolutions. Across two real-world teleoperated datasets, replayed and scored offline against an independent full-resolution map, the tiered maps are 13-15x smaller and require 6-8x less integration CPU time than a uniform fine map, while producing paths at a similar cost per meter as the ones generated from fine resolution maps on both scenarios. A uniform coarse map reaches the same size savings but its path quality collapses (+210% cost per meter) on the harder scenario, because it over-discretizes terrain the robot must traverse. Tiering approximates the coarse map's cheapness without its failure mode.
Afonso E. Carvalho, David Portugal, Paulo Peixoto +1
Institute of Systems and Robotics, University of Coimbra, Coimbra, Portugal · Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USA
Mapping is essential in robotics and autonomous systems because it provides the spatial foundation for path planning. Efficient mapping enables planning algorithms to generate reliable paths while ensuring safety and adapting in real time to complex environments. Fixed-resolution mapping methods often produce overly conservative obstacle representations that lead to suboptimal paths or planning failures in cluttered scenes. To address this issue, we introduce Parallel OctoMapping (POMP), an efficient OctoMap-based mapping technique that maximizes available free space and supports multi-threaded computation. To the best of our knowledge, POMP is the first method that, at a fixed occupancy-grid resolution, refines the representation of free space while preserving map fidelity and compatibility with existing search-based planners. It can therefore be integrated into existing planning pipelines, yielding higher pathfinding success rates and shorter path lengths, especially in cluttered environments, while substantially improving computational efficiency.
Yihui Mao, Tian Tan, Xuehui Shen +2
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