GH-FMT: Heuristic-Based Expansion and Sampling for Fast Path Planning in Unknown Environments

Mobile robots generally face formidable challenges in unknown dynamic environments, particularly in the aspects of rapidly searching feasible and high-quality solutions. In this article, Gaussian-heuristic fast marching tree (GH-FMT), including heuristic expansion and sampling, is proposed to provide high-quality solutions while simultaneously improving efficiency of planning and replanning in unknown dynamic environments. Specifically, an adaptive expansion region is defined using a boundary-based Gaussian distribution, minimizing potential redundant collision checking and supports effective and comprehensive exploration of the environments. A hybrid incremental search strategy is designed to reduce sample density and prioritized exploration of the search tree in promising directions using a heuristic single sampling method. Moreover, a hybrid optimization strategy is employed under time constraints to identify potential nodes, enabling efficient convergence toward high-quality solutions. Finally, through a series of challenging simulation scenarios and real-world experimental investigations, and by benchmarking against current-leading variants in the sampling-based planning class, the proposed GH-FMT demonstrated favorable advantages in flexibility, safety, and rapidity. It is shown that the search time is reduced on average by 15.8% and the path cost is reduced by on average 13.5%.