CSC-FMT*: Efficient 3D Path Planning Via Cylindrical Space-Cutting Fast Marching Tree
Maoyun Zhao, Depeng Liu, Peng Liu, Han Wang, Guofeng Shen
Abstract
Sampling-based path planning algorithms like the Fast Marching Tree (FMT<sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">*</sup>) often suffer from redundant exploration and inefficient sampling in complex 3D environments. To tackle these issues, this paper proposes the Cylindrical Space-Cutting Fast Marching Tree (CSC-FMT<sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">*</sup>) algorithm. The core of our approach is a dynamic cylindrical space-cutting mechanism. For each expanding node, it computes the central line towards the goal and leverages edge obstacle point clouds to determine the radius of the largest collision-free cylinder. This effectively prunes invalid search regions, reducing redundant sampling and costly collision checks. Coupled with a node deactivation strategy, this mechanism guides the tree expansion efficiently toward the goal. To enhance practicality for robotic execution, a post-processing pipeline incorporating B-spline smoothing and geometry-aware singularity optimization is applied, generating final trajectories that are both smooth and singularity-free. Extensive evaluations in complex 3D scenarios demonstrate that CSC-FMT<sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">*</sup> outperforms mainstream methods including PRM<sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">*</sup>, HBFMT<sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">*</sup>, and FMT<sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">*</sup> in terms of planning time, path length, and stability. The proposed algorithm also shows great potential for applications in complex medical robotic systems, such as Magnetic Resonance-guided High-Intensity Focused Ultrasound (MRI-HIFU), where planning efficiency and trajectory stability are paramount for safely navigating critical anatomical structures.
BibTeX
@inproceedings{ral2026_cscfmtefficient3,
title = {CSC-FMT*: Efficient 3D Path Planning Via Cylindrical Space-Cutting Fast Marching Tree},
author = {Maoyun Zhao and Depeng Liu and Peng Liu and Han Wang and Guofeng Shen},
booktitle = {RA-L 2026},
year = {2026}
}