Sampling-Focused Marching Tree: Optimal Planning Based on Minimized Topological Refinement and Homotopy-Heuristic Exploration

In this article, we propose sampling-focused marching tree (SMT) to guarantee optimal solutions in complex environments efficiently. By synergistically integrating heuristic path planning, homotopy space computation, and adaptive sampling exploration, SMT swiftly identifies homotopic solutions to the optimal solution and focuses sampling efforts in their vicinity, continually refining the solution to efficiently attain the global optimum. In the heuristic path planning phase, the generalized Voronoi graph feature nodes are extracted by a filter to facilitate subsequent computations. Next, the feature visibility graph is constructed based on the feature nodes to plan a heuristic path. In the homotopy space computation phase, feature cell decomposition is executed using the feature nodes as well to refine the obstacle-free space. Then, the homotopy space is computed by examining the topological connections between cells and the heuristic path to narrow down the sampling space. In the adaptive sampling exploration phase, the sampling factor is adjusted based on the area of the sampling space to enhance the quality of samples. After adaptive sampling based on the factor, the fast marching tree is leveraged to rapidly explore the samples and find the optimal solution. A thorough analysis of SMT is provided, including completeness, asymptotic optimality, and computational complexity. Comprehensive simulation comparisons with current-leading planning approaches in complex scenarios, along with a series of convincing real-world studies have been conducted to provide evidence for verifying optimality and high-efficiency computation of the proposed SMT.