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

TL;DR

This paper presents 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.

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 (9)

• Figure 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.
• Figure 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.
• Figure 3: Illustration of the cost-to-come ($g$ cost) and $\texttt{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 $\texttt{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.
• Figure 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.
• Figure 5: Illustration of our dynamic refinement procedure. When a new path (dashed arrow) is discovered to a subvolume $\mathcal{V}'$ (left) that has already been reached (solid arrow), we evaluate its effect on the cost to reach each vertex in $\mathcal{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), $\mathcal{V}'$ is recursively subdivided until each child subvolume is fully resolved under case i or ii. Subvolumes are colored by their $\texttt{predecessor}$.