Exploiting Euclidean Distance Field Properties for Fast and Safe 3D planning with a modified Lazy Theta*

TL;DR

This work introduces FS-Planner, a fast, safe 3D path planner built on a modified Lazy Theta* that leverages Euclidean Distance Fields to compute obstacle-proximity-aware costs. By defining an EDF-based edge cost $c(s_i,s_{i+1})$ that includes an EDF integral term and proving it satisfies the triangle inequality, the method enables efficient parent selection while maintaining safety. A gradient-based neighbour-selection strategy reduces expansions, and an analytic approximation of the EDF integral along segments accelerates cost evaluation. Extensive 3D and real-world experiments show significant reductions in computation time and node exploration, with improved safety and geometric smoothness, and the approach is applicable as a drop-in cost term for other planners. The results suggest EDF-informed planning can deliver real-time, robust performance in complex 3D environments, broadening the utility of EDFs in practical robotics planning.

Abstract

This paper presents the FS-Planner, a fast graph-search planner based on a modified Lazy Theta* algorithm that exploits the analytical properties of Euclidean Distance Fields (EDFs). We introduce a new cost function that integrates an EDF-based term proven to satisfy the triangle inequality, enabling efficient parent selection and reducing computation time while generating safe paths with smaller heading variations. We also derive an analytic approximation of the EDF integral along a segment and analyze the influence of the line-of-sight limit on the approximation error, motivating the use of a bounded visibility range. Furthermore, we propose a gradient-based neighbour-selection mechanism that decreases the number of explored nodes and improves computational performance without degrading safety or path quality. The FS-Planner produces safe paths with small heading changes without requiring the use of post-processing methods. Extensive experiments and comparisons in challenging 3D indoor simulation environments, complemented by tests in real-world outdoor environments, are used to evaluate and validate the FS-Planner. The results show consistent improvements in computation time, exploration efficiency, safety, and smoothness in a geometric sense compared with baseline heuristic planners, while maintaining sub-optimality within acceptable bounds. Finally, the proposed EDF-based cost formulation is orthogonal to the underlying search method and can be incorporated into other planning paradigms.

Figures (14)

• Figure 1: Paths computed by original A* algorithm in black, and computed by A*+M1 from iros2022_edf in purple considering distance to obstacles represented as EDF: 2D (left) and 3D (right). Both paths show how the distance to obstacles influences on the computed path by A*+M1.
• Figure 2: Computation of EDF integration for segments of length $L$ with a convex obstacle. The isocurves of the distance to obstacle are shown in blue.
• Figure 3: Computation of $g(s_n)$, $s_n$ is a neighbour node, through the parent node, $s_p$, and the current node, $s_c$, considering the distances between nodes and the EDF cost of each node. $L$ is the line of sight or distance between $s_p$ and $s_n$, $d$ is the distance between $s_c$ and $s_n$, and $a$ is the resolution of the grid or distance between $s_c$ and $s_n$. Note that distance can be $a$ or $\sqrt(2) \cdot a$
• Figure 4: Visibility-neighbour selection process. (a) The neighbour with the minimum value of $EDF'(s')$ is identified ($s_{edf}$) by the ComputeMinDerivEDF function, indicated by the two red arrows, together with the direction to the goal $s_goal$ (blue arrow). These two vectors are combined by the ComputeVector function to obtain the unit direction vector $\hat{u}$ (black arrow). (b) Once the best candidate neighbour (dark gray cell) is selected, the ChooseNeighbours function generates the reduced set of visibility neighbours; in this example, nine neighbours are selected (blue cells).
• Figure 5: Selection of neighbours in 2D. The current node, $S$, is in the coordinates' center, $O_1$ and $O_2$ are the obstacles, and $G$ the goal. The grid in the centre shows the eight neighbours of the current node. As an analogy with the Sun–Earth–Moon system, in each case $S$ can be viewed as the Sun, $O_1$ and $O_2$ as the Earth, and $G$ as the Moon. Thus, points $O_1$ and $O_2$ rotate around $S$ like the Earth around the Sun, while G rotate around O1 and O2 in each case, respectively, like the Moon around the Earth. First case (a): $G$ rotates with a small radius around $O_1$ (dotted line in red). Second case (b): $G$ rotates around $O_2$ (dotted line in blue) with a large radius. The difference between both setups is that in the second one the current node might fall between $G$ and $O_2$.