Kinematic-Constraint-Frontloaded RRT with Layered Guidance for 3D Underwater Path Planning*

Traditional RRT* methods can produce geometrically reachable but kinematically difficult-to-track paths when vehicle maneuverability constraints are not enforced during tree construction. To address this limitation, a kinematic-constraint-frontloaded RRT* algorithm with layered height-error-driven guidance is proposed for 3-D underwater path planning. Turn-angle and pitch constraints are embedded into tree expansion, parent selection, and rewiring. A curvature bound derived from the vehicle’s maximum yaw rate at nominal cruising speed limits the associated directional-change rate and the induced normal acceleration, while the pitch bound limits the vertical component of the reference velocity under the assumed nominal planning speed within the feasible range. A gravity-gain-modulated vertical guidance compensates for the height-convergence loss inherent to pitch limitation. Constraint-preserving post-processing further improves path continuity without breaking feasibility. Ablation experiments across three obstacle configurations with 50 runs per method—comparing Informed RRT*, Fillet-RRT*, a kinematic Informed RRT* variant without guidance, and the full method—demonstrate that the proposed method achieves 100% geometric planning success and 100% constraint satisfaction. Six-degree-of-freedom tracking simulations using the MSS REMUS 100 AUV model with 3-D ALOS guidance confirm a 100% successful-tracking rate, with lateral and vertical cross-track errors of 0.75 m and 0.2 m RMS, respectively, compared with 0% for the two baselines that do not enforce the full three-dimensional maneuverability constraints. The combined planning-to-tracking pipeline validates that kinematic-level pre-optimization can substantially improve trackability without modifying the downstream controller.