Exact Wavefront Propagation for Globally Optimal One-to-All Path Planning on 2D Cartesian Grids

TL;DR

This letter introduces an efficient $\mathcal {O}(n)$ compute and memory complexity algorithm for globally optimal path planning on 2D Cartesian grids that achieves exact wavefront propagation by pivoting the analytic distance function based on visibility.

Abstract

This paper introduces an efficient $\mathcal{O}(n)$ compute and memory complexity algorithm for globally optimal path planning on 2D Cartesian grids. Unlike existing marching methods that rely on approximate discretized solutions to the Eikonal equation, our approach achieves exact wavefront propagation by pivoting the analytic distance function based on visibility. The algorithm leverages a dynamic-programming subroutine to efficiently evaluate visibility queries. Through benchmarking against state-of-the-art any-angle path planners, we demonstrate that our method outperforms existing approaches in both speed and accuracy, particularly in cluttered environments. Notably, our method inherently provides globally optimal paths to all grid points, eliminating the need for additional gradient descent steps per path query. The same capability extends to multiple starting positions. We also provide a greedy version of our algorithm as well as open-source C++ implementation of our solver.

Figures (7)

• Figure 1: Euclidean distance function starting from the green point at the center. Contour lines are shown in black and the colormap ranges from cold to hot reflecting the increasing distance. Obstacles are shown in dark red. Computed using our proposed method. The colorbar is shown to the right with the distance unit being pixels. The environment is a $1000\times1000$ map. The visualization strategy between grid points is interpolation.
• Figure 2: Resulting $\mathcal{C}$ matrix from VBM when applied to the same environment as in Fig. \ref{['fig:contours_1']}. Original wave source is the green dot at the center, obstacles are shown in black, and the pivots are shown as white dots. Every unique color refers to grid cells that have the same parent node.
• Figure 3: Log-log plot of average compute time of the proposed algorithm with respect to the grid size $n_{x}\times n_{y}$. The data is fit with linear regression. The resulting equation $y = 0.298x$ is shown on the plot and has an $R^{2}$ value of $0.998$ which signifies a strong confidence in the algorithm's linear complexity.
• Figure 4: Comparison of marching results when utilizing different distance metrics. (a) $L_{1}$ distance, (b) $L_{3}$ distance, (c) $L_{\infty}$ distance, and (d) Quasi-Euclidean distance. Fig. \ref{['fig:contours_1']} shows the $L_{2}$ (Euclidean) distance.
• Figure 5: Multiple sources for marching to begin from marked as green dots. Same environment as in Fig. \ref{['fig:contours_1']}. No collisions or numerical issues arise.