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

TL;DR

The paper introduces FS-Planner, a fast 3D path planning algorithm that leverages Euclidean Distance Fields to improve safety, efficiency, and path smoothness in complex environments, validated through extensive experiments.

Contribution

It proposes a novel EDF-based cost function and gradient-based neighbor selection for efficient, safe, and smooth 3D path planning, applicable to various search algorithms.

Findings

Significant reduction in computation time and exploration effort.

Paths with smaller heading variations and improved safety.

Consistent performance improvements in both simulation and real-world environments.

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.