A Hierarchical Trajectory Planning Framework for Autonomous Underwater Vehicles via Spatial–Temporal Alternating Optimization

Abstract

Autonomous underwater vehicle (AUV) motion planning in complex three-dimensional ocean environments remains challenging due to the simultaneous requirements of obstacle avoidance, dynamic feasibility, and energy efficiency. Current approaches often decouple these factors or exhibit high computational overhead, limiting applicability in real-time or large-scale missions. This work proposes a hierarchical trajectory planning framework designed to address these coupled constraints in an integrated manner. The framework consists of two stages: (i) a current-biased sampling-based planner (CB-RRT*) is introduced to incorporate ocean current information into the path generation process. By leveraging flow field distributions, the planner improves path geometric continuity and reduces steering variations compared with benchmark algorithms; (ii) spatial–temporal alternating optimization is performed within underwater safe corridors, where Bézier curve parameterization is utilized to jointly optimize spatial shapes and temporal profiles, producing dynamically feasible and energy-efficient trajectories. Simulation results in dense obstacle fields, heterogeneous flow environments, and large-scale maps demonstrate that the proposed method reduces the maximum steering angle by up to 63% in downstream scenarios, achieving a mean maximum turning angle of 0.06 rad after optimization. The framework consistently attains the lowest energy consumption across all tests while maintaining an average computation time of 0.68 s in typical environments. These results confirm the framework’s suitability for practical AUV applications, providing a computationally efficient solution for generating safe, kinematically feasible, and energy-efficient trajectories in real-world ocean settings.

1. Introduction

The ocean contains vast reserves of natural resources, yet its highly unstructured and dynamic underwater environment imposes substantial challenges on direct human intervention. Benefiting from their operational autonomy and versatility, autonomous underwater vehicles (AUVs) have emerged as essential platforms for a broad spectrum of subsea tasks. Beyond conventional planar surveys, modern AUV deployments increasingly demand complex spatial maneuvering. Representative mission scenarios include submarine pipeline and cable inspection over steep and undulating terrain [1], hydrothermal vent sampling within obstacle-dense geological formations [2], and tracking of dynamic ocean plumes in regions dominated by strong current disturbances [3]. These tasks require the vehicle to navigate cluttered three-dimensional (3D) spaces while actively counteracting nonuniform hydrodynamic forces. As a result, it becomes necessary to develop trajectory planning methods that concurrently accommodate terrain complexity and flow field dynamics. In practice, an effective planner should ensure collision-free motion through rugged seabed features and mitigate energy consumption in adverse currents, thereby serving as a foundational capability for enhancing AUV autonomy in real-world operations.

Conventional path planning methods can be broadly categorized into search-based, sampling-based, and optimization-based approaches. Sampling and search-based planners such as the Rapidly-exploring Random Tree (RRT) [4] and its variants efficiently explore high-dimensional configuration spaces but often yield discontinuous or suboptimal paths [5]. To address this limitation, the B-spline curvature-constrained RRT* (BSRRT*) algorithm [6] was proposed to generate smoother trajectories and enhance computational efficiency. Similarly, a kinematically constrained motion planning method integrating grid-based maps, motion-driven search, and B-spline optimization effectively handles dynamic environments with nonzero initial velocities [7]. In contrast, optimization-based approaches leverage metaheuristic or swarm intelligence techniques—such as Genetic Algorithms (GA), Ant Colony Optimization (ACO), and Simulated Annealing (SA)—to achieve global convergence and adaptivity [8]. Nevertheless, they typically suffer from high computational cost and sensitivity to initialization.

A key limitation of many existing planning frameworks is their restriction to two-dimensional environments, which inadequately captures realistic three-dimensional ocean scenarios. Extending path planning from 2D to 3D not only increases the search dimensionality but also introduces nonlinear coupling among hydrodynamic forces, bathymetry, and energy consumption, making traditional planar methods insufficient for realistic underwater operations. To address 3D challenges, an Artificial Potential Field Grid (APFG)-based method [9] was proposed, introducing virtual target points and optimized repulsive forces to mitigate oscillations and local minima. Moreover, an Improved Compression Factor Particle Swarm Optimization (ICFPSO) algorithm incorporating seabed terrain and Lamb vortex models was developed to construct a 3D optimization framework that considers travel distance, bathymetry, and current constraints [10]. However, these approaches remain fundamentally confined to spatial path planning, producing geometry-only solutions without temporal parameterization. In the absence of a time law, neither the velocity profile nor the kinematic bounds (e.g., speed, acceleration, or maneuvering limits) is explicitly enforced. As a result, dynamic feasibility cannot be guaranteed, and key performance metrics—particularly those intertwined with temporal evolution, such as energy consumption under current disturbances—cannot be rigorously evaluated. This gap limits their applicability in field scenarios where both spatial safety and motion feasibility are simultaneously required.

To address temporal feasibility, a cooperative trajectory planning method for multiple bionic underwater vehicles (BUVs) was developed using four-dimensional spatio-temporal Bézier curves combined with the Particle Swarm Optimization (PSO) algorithm [11]. Although effective, this approach incurs high computational complexity, limiting its real-time applicability.

Generating smooth and dynamically feasible trajectories for AUVs in complex ocean environments thus remains an open challenge. Recently, safe-corridor-based methods [12], originally developed for aerial robotics, have been successfully extended to ground robots [13] and unmanned surface vehicles (USVs) [14]. The safe-corridor concept is particularly attractive for underwater environments where obstacles, currents, and terrain boundaries form highly nonconvex constraint spaces. By restricting the feasible search space and transforming nonconvex optimization problems into a series of convex subproblems, safe-corridor-based methods improve computational efficiency while guaranteeing safety. Inspired by this concept, this paper presents a current-aware hierarchical trajectory planning method for AUVs operating in complex 3D ocean environments. The proposed framework consists of three main stages: (i) a Current-Biased RRT* (CB-RRT*) algorithm that integrates ocean current information to generate an initial reference path; (ii) the construction of Underwater Safe Corridors (USCs) representing collision-free regions that dynamically adapt to local hydrodynamic disturbances; and (iii) a spatio-temporal alternating optimization scheme based on Bézier curve parameterization to refine the path into a smooth, dynamically feasible, and energy-efficient trajectory.

The main contributions of this paper are:

• A current-aware hierarchical trajectory planning framework is proposed that decouples path generation from trajectory optimization, enhancing scalability and computational efficiency in complex 3D marine environments.
• An internal constraint formulation for USCs is developed, decoupling collision-avoidance constraints from the number and geometry of obstacles, thereby ensuring safety while improving optimization efficiency.
• A spatio-temporal joint trajectory optimization method for AUVs that explicitly accounts for ocean-current effects. The objective is a weighted sum of path length, smoothness, energy consumption, and travel time. Using Bézier parameterization to construct the spatial path and a spatio-temporal alternating scheme to jointly optimize control points and segment durations, the method yields a smooth trajectory that satisfies kinematic constraints and provides time-varying position, velocity, and acceleration profiles.

The remainder of this paper is organized as follows: Section 2 models the marine environment and AUV kinematics. Section 3 presents the proposed hierarchical trajectory planning framework. Section 4 provides simulation results and comparative analyses. Section 5 concludes the paper and discusses potential future research directions.

2. Ocean Environment and AUV Kinematic Model

This section establishes the physical models of the ocean environment and the AUV kinematics, which collectively form the foundation for constructing a realistic and dynamically feasible trajectory optimization framework.

2.1. Modeling of Underwater Obstacles and Ocean Currents

The underwater environment contains static and semi-static structures, including the seabed and suspended obstacles. The seabed topography is represented as a superposition of Gaussian-shaped submarine peaks [15]. The height distribution $h(x,y)$ is defined as Equation (1).

$$h(x,y)=\sum _{i=1}^N{\alpha }_{i}exp\left[-\left({\displaystyle \frac{{(x-x_i)}^2}{{\sigma }_{x,i}^2}}+{\displaystyle \frac{{(y-y_i)}^2}{{\sigma }_{y,i}^2}}\right)\right]$$

(1)

where $(x_i,y_i)$ is the center of the i-th peak, ${\alpha }_i$ controls its height, and ${\sigma }_{x,i}$, ${\sigma }_{y,i}$ determine its spatial spread.

Remark 1.

The Gaussian superposition model (Equation (1)) is adopted as a mathematically controllable surrogate for seabed topography, enabling the generation of continuous, non-convex, and highly undulating terrain with adjustable spatial complexity. This facilitates rigorous stress-testing of the planner’s obstacle avoidance capability across diverse and unstructured environments. In practical deployments, this analytical terrain representation is replaced by bathymetric maps derived from Digital Elevation Models (DEM) obtained using multibeam echo sounders. Since the proposed framework operates on generic spatial representations (e.g., point clouds or occupancy grids), it is agnostic to the data source, and therefore directly compatible with real-world seabed measurements.

Suspended obstacles are represented as ellipsoids defined by Equation (2).

$${\displaystyle \frac{{(x-x_c)}^2}{a^2}}+{\displaystyle \frac{{(y-y_c)}^2}{b^2}}+{\displaystyle \frac{{(z-z_c)}^2}{c^2}}=1$$

(2)

where $(x_c,y_c,z_c)$ is the center coordinate of the sphere, representing its suspended position. Parameters a, b, and c are scaling factors in each direction, determining the stretching degree along the x, y, and z axes, respectively.

These factors enable the sphere to take the shape of an ellipsoid, allowing for a more accurate description of the geometry of underwater suspended objects. The final constructed seabed topography is shown in Figure 1.

The dynamic ocean current velocity field, ${\overrightarrow{V}}_{flow}\left(\overrightarrow{r}\right)={[V_x,V_y,V_z]}^{\top }$, plays a crucial role in determining the energy efficiency of AUV navigation. It is modeled using the Lamb-Oseen vortex formulation [16], which captures the essential rotational characteristics of turbulent ocean flows while maintaining analytical simplicity. The velocity components of a single vortex located at ${\overrightarrow{r}}_0$ are modeled by Equation (3).

$$\left\{\begin{array}{c}\begin{array}{cc}\hfill V_x\left(\overrightarrow{r}\right)& =-\Gamma {\displaystyle \frac{y-y_0}{2\pi |\overrightarrow{r}-{\overrightarrow{r}}_0{|}^2}}\left[1-e^{-{\displaystyle \frac{|\overrightarrow{r}-{\overrightarrow{r}}_0{|}^2}{{\delta }^2}}}\right]\hfill \\ \hfill V_y\left(\overrightarrow{r}\right)& =\Gamma {\displaystyle \frac{x-x_0}{2\pi |\overrightarrow{r}-{\overrightarrow{r}}_0{|}^2}}\left[1-e^{-{\displaystyle \frac{|\overrightarrow{r}-{\overrightarrow{r}}_0{|}^2}{{\delta }^2}}}\right]\hfill \\ \hfill V_z\left(\overrightarrow{r}\right)& ={\displaystyle \frac{\Gamma }{\pi {\delta }^2}}{e}^{-{\displaystyle \frac{|\overrightarrow{r}-{\overrightarrow{r}}_0{|}^2}{{\delta }^2}}}\hfill \end{array}\hfill \end{array}\right.$$

(3)

where $\Gamma$ denotes the circulation strength, with positive values ($\Gamma >0$) indicating counterclockwise rotation and negative values ($\Gamma <0$) representing clockwise rotation; $\delta$ represents the vortex radius.

Remark 2.

The Lamb-Oseen vortex model is employed as a controllable surrogate for shear flows and localized turbulence, capturing rotational behavior and velocity gradients that are representative of eddies and mesoscale vortices. These disturbance patterns impose stronger coupling between ocean dynamics and vehicle motion, thereby providing a rigorous benchmark for assessing the robustness of the proposed planner under disturbance-sensitive conditions. In practical deployments, the static current field ${\overrightarrow{V}}_{\mathrm{flow}}\left(\overrightarrow{r}\right)$ can be directly substituted with field measurements from Acoustic Doppler Current Profilers (ADCP) or numerical ocean forecasts (e.g., ROMS/FVCOM). Since the energy-aware cost function only relies on the ability to query local current vectors, the formulation in Equation (3) remains valid without modification, ensuring compatibility with real-world ocean current data.

By superimposing multiple such vortices, a three-dimensional flow field with complex structures can be synthesized. An example of a multi-vortex flow field is shown in Figure 2. In the simulations, the circulation strength $\Gamma$ is set within the range of ±1.5–3 m²/s, and the vortex radius $\delta$ is adjusted between 5 and 10 meters to emulate moderate-intensity oceanic turbulence conditions.

2.2. AUV Kinematic Model

The kinematic model defines the relationship between the AUV’s body-fixed velocities and its motion in the Earth-fixed frame. Following standard notation [17], two coordinate frames are used (Figure 3): the Earth-fixed frame ($O_e;X_e,Y_e,Z_e$) and the body-fixed frame ($O_b;X_b,Y_b,Z_b$).

Assuming negligible roll motion $(\varphi \approx 0,p\approx 0)$, which is standard for torpedo-shaped AUVs, the vehicle state in the Earth-fixed frame is defined by its position $\mathbf{p}={[x,y,z]}^{\mathrm{T}}$ and attitude $\mathit{\eta }={[\theta ,\psi ]}^{\mathrm{T}}$. The corresponding body-fixed velocity states consist of the linear velocities $\mathbf{v}={[u,v,w]}^{\mathrm{T}}$ (surge, sway, heave) and the angular rates ${[q,{\omega }_z]}^{\mathrm{T}}$ (pitch rate and yaw-axis angular rate). The transformation from the body-fixed velocities to the Earth-fixed kinematic rates is given by Equation (4).

$$\left\{\begin{array}{c}\dot{x}=(ucos\theta +wsin\theta )cos\psi -vsin\psi \hfill \\ \dot{y}=(ucos\theta +wsin\theta )sin\psi +vcos\psi \hfill \\ \dot{z}=-usin\theta +wcos\theta \hfill \\ \dot{\theta }=q\hfill \\ \dot{\psi }={\omega }_z/cos\theta \hfill \end{array}\right.$$

(4)

To account for the dominant environmental disturbance, the model is augmented with the ocean current velocity ${\overrightarrow{V}}_{flow}\left(\overrightarrow{r}\right)$ from Equation (3), yielding the complete kinematic model is Equation (5), which serves as a robust foundation for AUV trajectory planning and simulation under the influence of ocean currents.

$$\left\{\begin{array}{c}\dot{x}=(ucos\theta +wsin\theta )cos\psi -vsin\psi +V_x\hfill \\ \dot{y}=(ucos\theta +wsin\theta )sin\psi +vcos\psi +V_y\hfill \\ \dot{z}=-usin\theta +wcos\theta +V_z\hfill \\ \dot{\theta }=q\hfill \\ \dot{\psi }={\omega }_z/cos\theta \hfill \end{array}\right.$$

(5)

3. Hierarchical Trajectory Planning Framework

To achieve efficient trajectory planning for AUVs in environments with ocean currents, a hierarchical trajectory planning framework is proposed (Figure 4). The planning task is decomposed into three sequential yet interconnected modules: generation of reference path based on current-biased RRT* (CB-RRT*), construction of underwater safety corridors (USCs), and trajectory optimization based on spatio-temporal. This decomposition transforms a tightly coupled optimization problem into manageable sub-tasks, thereby improving both computational efficiency and solution quality.

3.1. Generation of Reference Path Based on CB-RRT*

The first module is designed to construct a geometrically feasible, current-aware reference path. To this end, the conventional RRT* is upgraded to a Current-Biased RRT* (CB-RRT*) formulation. The essential enhancement lies in a newly defined path cost function that explicitly embeds ocean current information, thereby biasing the tree expansion toward flow-favorable regions and reducing potential energy expenditure. Specifically, for each tree extension, the incremental cost of connecting a parent node ${\mathit{n}}_1$ to a newly sampled node ${\mathit{n}}_2$ is evaluated under the proposed current-weighted metric. The segment direction vector is $\mathit{d}={\mathit{n}}_2-{\mathit{n}}_1$. The ocean current velocity at ${\mathit{n}}_1$ is denoted as ${\mathit{V}}_{\mathrm{flow}}=[V_x\left({\mathit{n}}_1\right),V_y\left({\mathit{n}}_1\right),V_z\left({\mathit{n}}_1\right)]$. To quantify the influence of the current, the cosine similarity between the path direction and the current velocity is computed as Equation (6).

$$C={\displaystyle \frac{\mathit{d}·{\mathit{V}}_{\mathrm{flow}}}{\parallel \mathit{d}\parallel \parallel {\mathit{V}}_{\mathrm{flow}}\parallel }}$$

(6)

A value of C close to 1 indicates current assistance, reducing energy consumption, while C near $-1$ signifies opposition, increasing propulsion demand. A value around 0 implies strong lateral flow, which complicates path tracking. For a more detailed analysis, the current vector ${\mathit{V}}_{\mathrm{flow}}$ is decomposed into components parallel and perpendicular to the path segment (Equation (7)):

$${\mathit{V}}_{\Vert }=\left({\mathit{V}}_{\mathrm{flow}}·{\displaystyle \frac{\mathit{d}}{\parallel \mathit{d}\parallel }}\right){\displaystyle \frac{\mathit{d}}{\parallel \mathit{d}\parallel }},{\mathit{V}}_{\perp }={\mathit{V}}_{\mathrm{flow}}-{\mathit{V}}_{\Vert }$$

(7)

The proposed path cost function J is then defined as Equation (8).

$$J=\left\{\begin{array}{cc}\parallel \mathit{d}\parallel ,\hfill & \mathrm{if}\parallel {\mathit{V}}_{\mathrm{flow}}\parallel \le \epsilon \mathrm{or}\parallel \mathit{d}\parallel \le \epsilon \hfill \\ \parallel \mathit{d}\parallel \left(1-\alpha C+\beta {\displaystyle \frac{\parallel {\mathbf{V}}_{\perp }\parallel }{V_{max}}}\right),\hfill & \mathrm{otherwise}\hfill \end{array}\right.$$

(8)

where $\epsilon$ is a small positive constant preventing numerical instability, $V_{max}$ is the maximum current velocity, and $\alpha$ and $\beta$ are weighting coefficients balancing the influence of the parallel and lateral current components, set to $0.8$ and $0.2$ respectively in this study. This formulation encourages the selection of path segments that are not only short but also aligned with favorable currents while avoiding strong cross-flows.

The CB-RRT* algorithm iteratively expands the tree using this cost function for node selection and connection, with the specific procedure outlined in Algorithm 1. The final output is a reference path sequence ${\mathcal{P}}_{\mathrm{CB}-\mathrm{RRT}*}=\{{\mathit{n}}_1,{\mathit{n}}_2,\dots ,{\mathit{n}}_K\}$, where each node ${\mathit{n}}_k=(x_k,y_k,z_k)$ represents a 3D waypoint for subsequent stages.

3.2. Construction of USCs

The reference path from CB-RRT* is typically coarse and may graze obstacles. To define a safe operational volume for trajectory optimization, an USC is constructed. The USC comprises a series of connected convex polyhedra that encapsulate the reference path, providing explicit spatial safety constraints.

The construction begins by interpolating the reference path ${\mathcal{P}}_{\mathrm{CB}-\mathrm{RRT}*}$ with a fixed step size $\Delta d$ to obtain a dense waypoint sequence $\mathcal{P}=\{{\mathit{p}}_1,{\mathit{p}}_2,\dots ,{\mathit{p}}_n\}$. For each segment between consecutive waypoints, an initial axis-aligned bounding box (AABB) is created. A key innovation lies in dynamically adjusting the safety boundary based on local ocean currents. The safety radius r for corridor inflation is modulated as Equation (9).

$$r=r_{0}max\left\{{\epsilon }_1+{\displaystyle \frac{\parallel {\mathit{V}}_{\mathrm{flow}}\parallel }{V_{max}}}cos{\theta }_j,1\right\}$$

(9)

where $r_0$ is the baseline radius, ${\epsilon }_1$ is a minimum radius threshold, $\parallel {\mathit{V}}_{\mathrm{flow}}\parallel$ is the local current speed, and ${\theta }_j$ is the angle between the path segment and the current direction. This mechanism expands the corridor in assisting currents for greater optimization flexibility and contracts it in opposing currents for tighter control and safety.

Algorithm 1 CB-RRT* Algorithm

Require:  Start node ${\mathit{n}}_{\mathrm{start}}$, goal node ${\mathit{n}}_{\mathrm{goal}}$, obstacle set $\mathcal{O}$, flow field ${\mathit{V}}_{\mathrm{flow}}(·)$, step size $h_{\mathrm{step}}$, maximum iterations $N_{max}$, parameters $\alpha ,\beta ,\epsilon ,V_{max}$

Ensure: Reference path ${\mathcal{P}}_{\mathrm{CB}-\mathrm{RRT}*}=\{{\mathit{n}}_1,\dots ,{\mathit{n}}_K\}$

1: Initialize tree $\mathcal{T}\leftarrow \left\{{\mathit{n}}_{\mathrm{start}}\right\}$, set cost $J\left({\mathit{n}}_{\mathrm{start}}\right)\leftarrow 0$   2: Set parent pointer of ${\mathit{n}}_{\mathrm{start}}$ to NULL   3: for $k=1$ to $N_{max}$ do   4:     Sample random state ${\mathit{n}}_{\mathrm{rand}}$ in the free space with goal bias   5:     Find nearest node ${\mathit{n}}_{\mathrm{near}}$ to ${\mathit{n}}_{\mathrm{rand}}$ in $\mathcal{T}$   6:     Steer from ${\mathit{n}}_{\mathrm{near}}$ towards ${\mathit{n}}_{\mathrm{rand}}$ with step size $h_{\mathrm{step}}$ to obtain candidate node ${\mathit{n}}_{\mathrm{new}}$   7:     if segment $\overline{{\mathit{n}}_{\mathrm{near}}{\mathit{n}}_{\mathrm{new}}}$ is collision-free w.r.t. $\mathcal{O}$ then   8:         Compute direction vector $\mathit{d}={\mathit{n}}_{\mathrm{new}}-{\mathit{n}}_{\mathrm{near}}$   9:         Evaluate ocean current ${\mathit{V}}_{\mathrm{flow}}={\mathit{V}}_{\mathrm{flow}}\left({\mathit{n}}_{\mathrm{near}}\right)$ 10:         if $\parallel {\mathit{V}}_{\mathrm{flow}}\parallel \le \epsilon \mathrm{or}$ $\parallel \mathit{d}\parallel \le \epsilon$ then 11:             $j_{\mathrm{seg}}\leftarrow \parallel \mathit{d}\parallel$ 12:         else 13:             $C\leftarrow {\displaystyle \frac{\mathit{d}·{\mathit{V}}_{\mathrm{flow}}}{\parallel \mathit{d}\parallel \parallel {\mathit{V}}_{\mathrm{flow}}\parallel }}$ 14:             ${\mathit{V}}_{\Vert }\leftarrow \left({\mathit{V}}_{\mathrm{flow}}·{\displaystyle \frac{\mathit{d}}{\parallel \mathit{d}\parallel }}\right){\displaystyle \frac{\mathit{d}}{\parallel \mathit{d}\parallel }}$ 15:             ${\mathit{V}}_{\perp }\leftarrow {\mathit{V}}_{\mathrm{flow}}-{\mathit{V}}_{\Vert }$ 16:             $j_{\mathrm{seg}}\leftarrow \parallel \mathit{d}\parallel \left(1-\alpha C+\beta {\displaystyle \frac{\parallel {\mathit{V}}_{\perp }\parallel }{V_{max}}}\right)$ 17:         end if 18:         Set $J\left({\mathit{n}}_{\mathrm{new}}\right)\leftarrow J\left({\mathit{n}}_{\mathrm{near}}\right)+j_{\mathrm{seg}}$ 19:         Set parent of ${\mathit{n}}_{\mathrm{new}}$ as ${\mathit{n}}_{\mathrm{near}}$ 20:         Add ${\mathit{n}}_{\mathrm{new}}$ and edge $({\mathit{n}}_{\mathrm{near}},{\mathit{n}}_{\mathrm{new}})$ to tree $\mathcal{T}$ 21:         if $\parallel {\mathit{n}}_{\mathrm{new}}-{\mathit{n}}_{\mathrm{goal}}\parallel <d_{\mathrm{goal}}$ then 22:             Add ${\mathit{n}}_{\mathrm{goal}}$ to $\mathcal{T}$, set parent of ${\mathit{n}}_{\mathrm{goal}}$ as ${\mathit{n}}_{\mathrm{new}}$ 23:             break 24:         end if 25:     end if 26: end for 27: Backtrack from ${\mathit{n}}_{\mathrm{goal}}$ to ${\mathit{n}}_{\mathrm{start}}$ along parent pointers to obtain ${\mathcal{P}}_{\mathrm{CB}-\mathrm{RRT}*}$ 28: return${\mathcal{P}}_{\mathrm{CB}-\mathrm{RRT}*}$

Subsequently, the initial AABB is refined using the Fast Iterative Region Inflation (FIRI) algorithm [18], which iteratively pushes the facets of the polyhedron outward until they closely conform to nearby obstacles. The resulting convex polyhedron for the i-th segment is defined as Equation (10).

$${\mathcal{H}}_i=\{\mathit{x}\in {\mathbb{R}}^3\mid A_i\mathit{x}+{\mathit{b}}_i\le \mathbf{0}\}$$

(10)

where $A_i$ is the matrix of facet normals and ${\mathit{b}}_i$ is the offset vector.

Connectivity between adjacent polyhedra ${\mathcal{H}}_i$ and ${\mathcal{H}}_{i+1}$ is ensured by enforcing a non-empty intersection (Equation (11)).

$${\mathcal{H}}_i\cap {\mathcal{H}}_{i+1}\ne \varnothing ,\forall i\in \{1,2,\dots ,n-1\}$$

(11)

If this condition is violated, an intermediate transition polyhedron is inserted. The complete USC is the union of all these polyhedra, ${\mathcal{H}}_{\mathrm{USC}}={\bigcup }_{i=1}^n{\mathcal{H}}_i$. A schematic of the USC construction is depicted in Figure 5, illustrating how it provides a safe, connected volume for trajectory optimization.

3.3. Trajectory Optimization Based on Spatio-Temporal

Once the safe corridor is constructed, the final module concentrates on synthesizing a dynamically admissible trajectory that remains strictly within ${\mathcal{H}}_{\mathrm{USC}}$. This is formulated as a spatio-temporal optimization over piecewise Bézier curves. The smooth parametric representation and convex-hull property enable corridor adherence to be enforced through linear constraints on control points, which is essential for maintaining real-time tractability in operational environments. Simultaneous refinement of the geometric path and traversal time yields trajectories that comply with kinematic limits and are directly compatible with onboard control allocation and energy management modules in practical AUV deployments.

3.3.1. Spatial Optimization

The spatial path is represented by N segments of n-th order Bézier curves. The k-th segment is defined as Equation (12).

$${\mathbf{r}}_k\left(t\right)=\sum _{i=0}^n{\mathbf{p}}_{k,i}{B}_i^n\left(t\right),t\in [0,1]$$

(12)

where ${\mathbf{p}}_{k,i}$ are the control points and $B_i^n\left(t\right)=\left({\displaystyle \genfrac{}{}{0pt}{}{n}{i}}\right)t^i{(1-t)}^{n-i}$ is the Bernstein basis polynomial.

The spatial optimization aims to find the control point set $\mathbf{P}={\bigcup }_{k=0}^N\{{\mathbf{p}}_{k,0},{\mathbf{p}}_{k,1},\dots ,{\mathbf{p}}_{k,n}\}$ that minimizes a composite objective function (Equation (13)).

$$J_{\mathrm{total}}={\lambda }_1{J}_{\mathrm{length}}+{\lambda }_2{J}_{\mathrm{smooth}}+{\lambda }_3{J}_{\mathrm{current}}$$

(13)

which aggregates the following cost terms.

The trajectory length cost minimizes the total travel distance (Equation (14)).

$$J_{\mathrm{length}}=\sum _{k=1}^N\sum _{i=1}^n\parallel {\mathbf{p}}_{k,i}-{\mathbf{p}}_{k,i-1}\parallel$$

(14)

The trajectory smoothness cost reduces abrupt maneuvers by penalizing the cumulative variation of turning angles (Equation (15)).

$$J_{\mathrm{smooth}}=\sum _{k=1}^N\sum _{i=1}^{n-1}arccos\left({\displaystyle \frac{({\mathbf{p}}_{k,i}-{\mathbf{p}}_{k,i-1})·({\mathbf{p}}_{k,i+1}-{\mathbf{p}}_{k,i})}{\parallel {\mathbf{p}}_{k,i}-{\mathbf{p}}_{k,i-1}\parallel \parallel {\mathbf{p}}_{k,i+1}-{\mathbf{p}}_{k,i}\parallel }}\right)$$

(15)

The ocean current energy consumption cost encourages the trajectory to leverage favorable currents while avoiding adverse ones, thereby reducing propulsion energy requirements. Following the model in [19], it is defined as Equation (16).

$$J_{\mathrm{current}}=-\sum _{k=1}^N\sum _{i=1}^n{({\mathbf{p}}_{k,i}-{\mathbf{p}}_{k,i-1})}^{\mathrm{T}}{\mathit{V}}_{\mathrm{flow}}\left({\mathbf{p}}_{k,i}\right)\parallel {\mathbf{p}}_{k,i}-{\mathbf{p}}_{k,i-1}\parallel (cos{\theta }_{k,i}-sin{\theta }_{k,i})$$

(16)

where ${\mathit{V}}_{\mathrm{flow}}\left({\mathbf{p}}_{k,i}\right)$ denotes the ocean current velocity vector at the i-th control point of the k-th segment, and ${\theta }_{k,i}$ represents the angle between the local trajectory direction and the ocean current direction.

The optimization is subject to three constraints.

Obstacle avoidance: enforced by requiring all control points to lie within the USC (Equation (17)).

$${\mathbf{p}}_{k,i}\in {\mathcal{H}}_{\mathrm{USC}},\forall k,i$$

(17)

Boundary conditions: the trajectory must match the initial and final states $({\mathit{p}}_{\mathrm{start}}{\mathit{v}}_{\mathrm{start}},$${\mathit{a}}_{\mathrm{start}})$ and $({\mathit{p}}_{\mathrm{end}},{\mathit{v}}_{\mathrm{end}}{\mathit{a}}_{\mathrm{end}})$ (Equation (18)).

$$\left\{\begin{array}{c}{\mathbf{r}}_1\left(0\right)={\mathit{p}}_{\mathrm{start}},{\mathbf{r}}_N\left(1\right)={\mathit{p}}_{\mathrm{end}},\hfill \\ {\displaystyle \frac{d{\mathbf{r}}_1}{dt}}\left(0\right)={\mathit{v}}_{\mathrm{start}},{\displaystyle \frac{d{\mathbf{r}}_N}{dt}}\left(1\right)={\mathit{v}}_{\mathrm{end}},\hfill \\ {\displaystyle \frac{d^2{\mathbf{r}}_1}{d{t}^2}}\left(0\right)={\mathit{a}}_{\mathrm{start}},{\displaystyle \frac{d^2{\mathbf{r}}_N}{d{t}^2}}\left(1\right)={\mathit{a}}_{\mathrm{end}}\hfill \end{array}\right.$$

(18)

Dynamic continuity: $C^2$ continuity (position, velocity, acceleration) must be preserved at the junctions between consecutive segments (Equation (19)).

$$\left\{\begin{array}{c}{\mathbf{r}}_k\left(1\right)={\mathbf{r}}_{k+1}\left(0\right)\hfill \\ {\displaystyle \frac{d{\mathbf{r}}_k}{dt}}\left(1\right)={\displaystyle \frac{d{\mathbf{r}}_{k+1}}{dt}}\left(0\right)\hfill \\ {\displaystyle \frac{d^2{\mathbf{r}}_k}{d{t}^2}}\left(1\right)={\displaystyle \frac{d^2{\mathbf{r}}_{k+1}}{d{t}^2}}\left(0\right)\hfill \end{array}\right.$$

(19)

Together, these terms define the following nonlinear optimization problem (NLP) (Equation (20)).

$$\begin{array}{cc}\hfill \underset{\mathbf{P}}{min}& J_{\mathrm{total}}\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \mathrm{Obstacle}\mathrm{constraint}\left(17\right),\hfill \\ \hfill & \mathrm{Boundary}\mathrm{constraints}\left(18\right),\hfill \\ \hfill & \mathrm{Dynamic}\mathrm{continuity}\mathrm{constraints}\left(19\right)\hfill \end{array}$$

(20)

3.3.2. Temporal Optimization

The spatial optimization parameterizes the trajectory by a normalized time $t\in [0,1]$. To obtain a time-optimal trajectory that respects the dynamic capabilities of an AUV, the actual duration $T_k$ for each segment must be optimized. The primary objective is to minimize the total travel time $J_{\mathrm{time}}$ (Equation (21)).

$$J_{\mathrm{time}}=\sum _{k=1}^N{T}_k$$

(21)

An initial estimate for each segment duration $T_k$ is obtained assuming traversal at the maximum velocity $v_{max}$ of AUV (Equation (22)).

$$T_k={\displaystyle \frac{\parallel {\mathbf{r}}_k\left(1\right)-{\mathbf{r}}_k\left(0\right)\parallel }{v_{max}}}$$

(22)

By applying the chain rule to map the normalized Bézier parameterization to physical time, two key dynamic constraints are derived. The velocity constraint ensures that the AUV does not exceed its maximum speed (Equation (23)).

$${\displaystyle \frac{1}{T_k}}∥{\displaystyle \frac{d{\mathbf{r}}_k\left(t\right)}{dt}}∥\le v_{max},t\in [0,1]$$

(23)

and the acceleration constraint guarantees that the AUV remains within its acceleration capabilities (Equation (24)).

$${\displaystyle \frac{1}{T_k^2}}∥{\displaystyle \frac{d^2{\mathbf{r}}_k\left(t\right)}{d{t}^2}}∥\le a_{max},t\in [0,1]$$

(24)

To efficiently handle these nonlinear constraints, the temporal optimization problem is reformulated as a Second-Order Cone Program (SOCP). This transformation involves sampling the Bézier curve at multiple points in the normalized time domain and recasting the velocity constraint as a second-order cone constraint, thereby convexifying the problem as Equation (25).

$$\begin{array}{cc}\hfill \underset{\left\{T_k\right\}}{min}& J_{\mathrm{time}}=\sum _{k=1}^N{T}_k\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \mathrm{Velocity}\mathrm{constraint}\left(23\right)\hfill \\ \hfill & \mathrm{Acceleration}\mathrm{constraint}\left(24\right)\hfill \end{array}$$

(25)

3.3.3. Spatio-Temporal Alternating Optimization

The spatial trajectory optimization and temporal parameter optimization are inherently coupled. The spatial path geometry determines the kinematic feasibility under given time allocations, while the segment durations directly influence the achievable spatial optimality through velocity and acceleration constraints. Specifically, longer time allocations relax dynamic constraints, potentially enabling more energy-efficient paths, whereas spatial curvature imposes lower bounds on traversal times.

To address this tight coupling, a spatio-temporal alternating optimization strategy is developed. The joint optimization problem is represented as Equation (26).

$$\begin{array}{cc}\hfill \underset{\mathbf{P},\left\{T_k\right\}}{min}& J=J_{\mathrm{total}}\left(\mathbf{P}\right)+\rho J_{\mathrm{time}}\left(\left\{T_k\right\}\right)\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathbf{p}}_{k,j}\in {\mathcal{H}}_{\mathrm{USC}},\forall k,j,\hfill \\ \hfill & \mathrm{Boundary}\mathrm{constraints}\left(18\right),\hfill \\ \hfill & \mathrm{Dynamic}\mathrm{continuity}\mathrm{constraints}\left(19\right),\hfill \\ \hfill & {\displaystyle \frac{1}{T_k}}∥{\displaystyle \frac{d{\mathbf{r}}_k\left(t\right)}{dt}}∥\le v_{max},t\in [0,1],\forall k,\hfill \\ \hfill & {\displaystyle \frac{1}{T_k^2}}∥{\displaystyle \frac{d^2{\mathbf{r}}_k\left(t\right)}{d{t}^2}}∥\le a_{max},t\in [0,1],\forall k\hfill \end{array}$$

(26)

where $\rho >0$ is a weighting coefficient that balances the trade-off between trajectory quality (encompassing length, smoothness, and energy consumption) and traversal time. Larger $\rho$ values prioritize time efficiency, whereas smaller values emphasize energy conservation and path smoothness.

The alternating optimization algorithm proceeds as outlined in Algorithm 2. In each iteration, the current temporal parameters $\left\{T_k\right\}$ fix the dynamic constraint boundaries for spatial optimization, converting the joint problem into a standard NLP over control points $\mathbf{P}$. Subsequently, with spatial parameters $\mathbf{P}$ fixed, the temporal optimization becomes a convex SOCP problem over segment durations $\left\{T_k\right\}$.

Algorithm 2 Spatio-temporal alternating optimization

Require: Initial path ${\mathcal{P}}_{\mathrm{CB}-\mathrm{RRT}*}$, USC ${\mathcal{H}}_{\mathrm{USC}}$, weights ${\lambda }_{1:3}$, $\rho$

Ensure: Optimized trajectory $\mathbf{r}\left(t\right)$ with temporal profile $\left\{T_k\right\}$

1: Initialize $\left\{T_k^{\left(0\right)}\right\}$ using Equation (22), set iteration counter $i\leftarrow 0$   2: repeat   3:     Spatial optimization:   4:        Solve ${\mathbf{P}}^{(i+1)}=arg{min}_{\mathbf{P}}{J}_{\mathrm{total}}\left(\mathbf{P}\right)$ s.t. constraints (17)–(19) with $\left\{T_k^{\left(i\right)}\right\}$ fixed   5:     Temporal optimization:   6:        Solve $\left\{T_k^{(i+1)}\right\}=arg{min}_{\left\{T_k\right\}}\rho {\sum }_{k=1}^N{T}_k$ s.t. constraints (23) and (24) with ${\mathbf{P}}^{(i+1)}$ fixed   7:     $i\leftarrow i+1$   8: until $|J^{\left(i\right)}-J^{(i-1)}|<{ϵ}_{\mathrm{tol}}$ or $i>i_{max}$   9: return $\mathbf{r}\left(t\right)$ from ${\mathbf{P}}^{\left(i\right)}$ and $\left\{T_k^{\left(i\right)}\right\}$

4. Simulations and Analysis

4.1. Simulation Setup

The simulation environment was configured as a three-dimensional grid map with a spatial resolution of $1\mathrm{m}\times 1\mathrm{m}\times 1\mathrm{m}$. The coordinate origin was set at $(0,0,0)$, representing the center of the map.

Table 1. Key parameters for AUV trajectory planning simulation.

Parameter	Description	Value
${\mathit{v}}_{\mathrm{start}},{\mathit{v}}_{\mathrm{end}}$	Initial and terminal velocity	$0\mathrm{m}/\mathrm{s}$
${\mathit{a}}_{\mathrm{start}},{\mathit{a}}_{\mathrm{end}}$	Initial and terminal acceleration	$0\mathrm{m}/{\mathrm{s}}^2$
$v_{max}$	Maximum velocity	$1.4\mathrm{m}/\mathrm{s}$
$a_{max}$	Maximum acceleration	$0.4\mathrm{m}/{\mathrm{s}}^2$
$r_0$	Baseline safety radius of USC	$4\mathrm{m}$
$\Delta d$	Interpolation step size	$6\mathrm{m}$
${\lambda }_1$	Weight for trajectory length cost	$0.4$
${\lambda }_2$	Weight for trajectory smoothness cost	$0.6$
${\lambda }_3$	Weight for current-induced energy cost	$1.0$
$\rho$	Weight for travel time cost	$0.1$

lists the key parameters used in the AUV trajectory planning simulations.

4.2. Path Generation Experiments

4.2.1. Experimental Setup and Evaluation Metrics

To validate the effectiveness of the proposed CB-RRT* in generating current-aware reference paths under complex three-dimensional flow conditions, two classic and widely adopted sampling-based planners, Informed RRT* [20] and LazyPRM* [21], are selected as baselines for comparison. All experiments are performed under a unified software and hardware configuration. For fairness, the maximum iteration number of each algorithm is capped at 10, allowing us to specifically assess rapid-search capability under constrained computational resources. Two representative scenarios are considered:

• Downstream Scenario: The goal is located downstream within the flow field, where the current predominantly assists vehicle motion.
• Upstream Scenario: The goal lies upstream, forcing the vehicle to overcome current resistance or exploit low-velocity corridors to reduce energy loss.

The dimensions of both environments are set to $50\mathrm{m}\times 50\mathrm{m}\times 25\mathrm{m}$. The start and goal configurations are fixed at $(-22,22,0)$ and $(22,-22,18)$, respectively. In the Downstream case, the vortex center of the Lamb-Oseen flow is placed at $(0,0,10)$, whereas in the Upstream case it is shifted to $(25,25,10)$ to intensify adverse current effects.

Five quantitative metrics are adopted for evaluation: (i) Path Length, the geometric distance of the resulting path; (ii) Maximum Steering Angle, defined as the largest angular deviation between adjacent path segments, directly reflecting geometric smoothness and suitability as an initialization for subsequent temporal optimization; (iii) Current Energy, representing the line integral of the local current vector field along the trajectory, where positive and negative values indicate energy loss and gain, respectively (Equation (16)); (iv) Computing Time, measured from planning invocation to the output of a feasible solution; and (v) Spatial Objective Cost, a composite metric that jointly accounts for geometric efficiency and current interaction (Equation (13)).

4.2.2. Visualization Analysis

In both downstream and upstream environments, the distribution of trajectories clearly reflects the behavioral differences among the three planners. As depicted in Figure 6 and Figure 7, the proposed CB-RRT* (black) consistently maintains smoother curvature and a more favorable alignment with local flow structures, whereas Informed-RRT* (red) and LazyPRM* (yellow) exhibit less structured exploration patterns. Two representative observations can be summarized as follows:

• Path Smoothness: Across both flow conditions, CB-RRT* produces paths with gradual transitions and minimal curvature discontinuities, particularly at turning points. In comparison, Informed-RRT* and LazyPRM* frequently generate zigzag-like trajectories containing multiple large-angle deviations. These geometric discontinuities, although collision-free, introduce substantial control and actuation demands when later converted into dynamically feasible trajectories.
• Flow-Field Adaptability: CB-RRT* demonstrates an explicit tendency to exploit favorable current regions, selecting segments where local flow vectors provide propulsion assistance or reduced resistance. The baseline algorithms, however, largely prioritize geometric distance and therefore ignore the hydrodynamic implications of the surrounding flow field.

These qualitative differences motivate the subsequent quantitative evaluation, where improvements in energy cost and steering angle metrics substantiate the advantages of embedding current-awareness directly into the path search process.

4.2.3. Quantitative Performance Analysis

The numerical results in both downstream and upstream scenarios are presented in

Table 2. Quantitative performance comparison of path planning algorithms in different flow scenarios.

Algorithm	Path	Max Steering	Current	Time	Spatial Objective
	Length (m)	Angle (rad)	Energy	(ms)	Cost
Downstream Scenario
CB-RRT*	70.65	0.45	−9.12	10.23	20.19
Informed-RRT*	66.79	0.93	1.58	6.31	29.90
LazyPRM*	72.95	1.22	5.88	129.63	39.51
Upstream Scenario
CB-RRT*	67.93	0.54	8.15	10.35	36.68
Informed-RRT*	67.38	0.85	8.81	8.23	37.14
LazyPRM*	68.70	0.74	9.35	134.28	37.27

. The comparison shows that the proposed CB-RRT* tends to generate paths with noticeably lower heading variations. In the downstream case, for example, the maximum steering angle reaches 0.45 rad, whereas the two comparison methods exceed 0.9 rad; a similar pattern persists in the upstream case. These differences imply that the geometry of the path produced by CB-RRT* is more naturally compatible with dynamic feasibility. In particular, the discrete points do not induce sudden heading transitions, and the subsequent trajectory optimization stage is less likely to encounter failures arising from excessive initial curvature.

Another result is related to the relationship between the planned path and the surrounding flow field. The current-energy metric in the downstream case reaches $-9.12$ for CB-RRT*, indicating that the algorithm consistently guides the vehicle through regions where the current assists motion rather than resisting it. In contrast, Informed-RRT* and LazyPRM* report positive values, suggesting that their paths inadvertently traverse segments that require overcoming the current. Even in the upstream condition, where energy expenditure cannot be avoided, CB-RRT* maintains the lowest value (8.15), implying a more balanced compromise between distance, heading feasibility, and local hydrodynamic characteristics.

Planning time also reflects this trend. The average computation time (about 10 ms) is slightly higher than that of Informed-RRT*, yet both remain within the same order of magnitude and meet real-time requirements. LazyPRM* is significantly slower, limiting its applicability in scenarios with tight time budgets. Although CB-RRT* does not always yield the absolute minimum geometric length (e.g., 70.65 m vs. 66.79 m downstream), the experiments suggest that, in flow-dominated environments, a short Euclidean path is not necessarily equivalent to a practically efficient one. A marginally longer route that reduces steering demand and leverages favorable flow conditions often results in a lower effective navigation burden.

In short, the numerical evaluation indicates that CB-RRT* favors paths that are smoother, more compatible with the surrounding flow, and computationally efficient enough for real-time hierarchical planning. The algorithm does not pursue geometric optimality in isolation; instead, it forms reference paths that are better aligned with the requirements of the downstream spatio-temporal optimization stage and the constraints of realistic underwater navigation.

4.3. Trajectory Optimization Experiments

To verify the effectiveness of the proposed hierarchical optimization framework, we conducted comparative experiments against two representative trajectory generation strategies. All methods utilize the same path points generated by CB-RRT* (Section 4.2) as the initial front-end input, ensuring a fair comparison and isolating the contribution of the backend optimization. The turning angle is evaluated according to Equation (15), and the sampling interval is uniformly set to $0.2\mathrm{m}$ across all experiments.

The baseline methods are defined as follows:

• Baseline 1 (Decoupled Geometric Smoothing + Constant Speed): The initial path is smoothed using Bezier curves, after which a simple time allocation strategy is applied, enforcing a constant cruising velocity of $0.7\mathrm{m}/\mathrm{s}$ for the AUV. This method aligns with common practices in existing studies (e.g., [22,23]), where the optimization is restricted to geometric refinement, with temporal dynamics and execution feasibility largely overlooked.
• Baseline 2 (Decoupled Optimization + TOPPRA): The geometric profile is first refined using B-Spline smoothing, and the temporal parameterization is subsequently obtained via the Time-Optimal Path Parameterization (TOPPRA) algorithm [24]. To ensure comparability with the proposed framework, the velocity and acceleration bounds imposed in TOPPRA are kept identical to those adopted in our method, thereby isolating the effect of the optimization paradigm itself.
• Proposed Method: The proposed framework performs coupled optimization of the spatial geometry and temporal variables through a spatio-temporal alternating strategy. By jointly refining both aspects in an iterative manner, the resulting trajectory improves smoothness, feasibility, and dynamic consistency, enabling more reliable execution in complex three-dimensional ocean current environments.

It is worth noting that, in contrast to the purely decoupled nature of both baselines, the proposed method explicitly accounts for the mutual influence between geometric and temporal variables, thereby avoiding sub-optimal solutions caused by independent optimization stages.

4.3.1. Experiments in Basic Scenarios

The experiments in this section were conducted in the downstream scenario described in Section 4.2 to examine kinematic behavior and smoothness under standard flow conditions. This environment provides a stable reference for observing how trajectory generation interacts with spatial constraints and current-induced motion.

In the visualized results of trajectory optimization (Figure 8), the underwater safety corridors (USCs) appear as semi-transparent green polyhedra constructed from the initial CB-RRT* path. These polyhedra function as the explicit spatial envelope within which backend optimization operates, and the behavior of different methods becomes distinguishable when viewed in this constrained geometric context.

A comparison within this corridor highlights both safety and smoothness differences. Trajectories based on Bezier curves remain fully contained within the USC boundaries, indicating that the convex-hull property enforces hard collision avoidance. In contrast, the B-spline variant used in Baseline 2 extends beyond corridor limits at several points, suggesting that smoothness alone does not guarantee feasibility in restricted spaces. Relative to the piecewise linear CB-RRT* initialization, both optimized Bezier-based trajectories reduce curvature concentration, but only the proposed method distributes curvature to exploit the interior of the corridor. The resulting path avoids rigid adherence to the centerline and does not exhibit the sharp boundary-adjacent turns that characterize Baseline 2. These observations imply that smoothness and spatial feasibility are maintained simultaneously rather than being traded against each other.

The kinematic profiles shown in Figure 9 further clarify these distinctions. The constant speed of $0.7\mathrm{m}/\mathrm{s}$ in Baseline 1 retains zero tangential acceleration, yet centripetal acceleration peaks reach approximately $0.18{\mathrm{m}/\mathrm{s}}^2$ due to centripetal effects in high-curvature segments ($a_n=v^2/r$). This behavior reveals a practical limitation of constant-speed strategies: without velocity adaptation, curvature translates directly into control effort, which risks violating actuator bounds near tight bends.

The minimum-time strategy in Baseline 2 generates aggressive acceleration fluctuations ($0.3$–$0.4{\mathrm{m}/\mathrm{s}}^2$), which correspond to discontinuities in curvature and the absence of higher-order continuity. This characteristic shortens traversal time but raises concerns regarding propulsion stability and tracking load. By contrast, the bell-shaped velocity profile produced by the proposed method adapts speed according to curvature and current direction: acceleration increases in straight downstream regions and drops in advance of upcoming sharp turns. This pattern maintains moderate acceleration throughout the operation and leaves a more trackable reference for low-level controllers.

Trajectory smoothness is quantified through the turning angle along arc length (Figure 10). Baseline 1 contains multiple angle spikes above $0.25\mathrm{rad}$, indicating that geometric Bezier smoothing alone does not align control points with dynamic constraints. Baseline 2 suppresses the most severe peaks but retains oscillations near $0.1\mathrm{rad}$. The proposed method maintains angles predominantly below $0.05\mathrm{rad}$, attributable to the joint adjustment of spatial control points and temporal knot vectors. The trajectory is effectively “straightened” in the spatiotemporal manifold, reducing the need for corrective maneuvers.

Quantitative metrics in

Table 3. Quantitative Comparison in Basic Scenario.

Method	Length	Max Turning	Total Turning	Current	Travel
	(m)	Angle (rad)	Angle (rad)	Energy	Time (s)
Ours	70.52	0.050	2.35	−9.49	64.25
Baseline 1	74.46	0.300	4.49	−9.26	106.37
Baseline 2	77.09	0.140	5.29	−8.60	55.98

summarize the comparisons of path length, turning metrics, current energy, and travel time reflect distinct operational priorities among the three methods. Baseline 1 ensures feasibility through conservative speed but results in the slowest progression. Baseline 2 minimizes time but incurs larger curvature and acceleration, providing less benefit from downstream flow. The proposed method yields the lowest maximum turning angle and the lowest current energy value, suggesting effective flow utilization and dynamic feasibility while retaining competitive travel time.

Overall, in the basic downstream scenario, the proposed method generates trajectories that remain feasible within the USC constraints, maintain low curvature, and leverage the current field for reduced energy expenditure. These characteristics arise from spatial–temporal optimization rather than isolated geometric adjustments, supporting subsequent control and deployment without requiring additional smoothing or speed regulation layers.

4.3.2. Robustness Testing in Complex Scenarios

This section evaluates the generalization ability and robustness of the proposed algorithm in three environments that are more challenging than the basic setup. The first environment contains a 50% increase in obstacle density, forming narrow passages that require precise spatial allocation. The second introduces a flow field with doubled vortex intensity and rapid directional variation, imposing stronger coupling between trajectory geometry and fluid disturbances. The third enlarges the map to $100\mathrm{m}\times 100\mathrm{m}\times 50\mathrm{m}$ with start and goal changed to $(-47,47,0)$ and $(47,-47,32)$, respectively, extending the mission to more than twice the baseline path length.

Trajectory visualizations for the three scenarios are provided in Figure 11, Figure 12 and Figure 13. Black curves correspond to the proposed method, yellow to Baseline 1, and purple to Baseline 2. These visualizations provide the geometric context for subsequent metric interpretation.

In the dense obstacle scenario, Baseline 2 reacts to clustered obstacles with frequent curvature spikes, producing jagged trajectories that oscillate between obstacle boundaries. Baseline 1 remains feasible but exhibits conservative detours that lengthen path distance. The proposed method preserves geometric continuity within narrow passages and avoids high-curvature corrections near obstacle edges.

In the complex flow scenario, Baseline 1 drifts with the current before recovering its intended direction, which can be traced to delayed compensation against strong cross-stream forces. Baseline 2 counteracts disturbances more aggressively but introduces curvature fluctuations around vortex boundaries. The proposed method adapts its velocity profile to the flow field and removes most of the secondary corrective maneuvers.

In the large-scale scenario, Baseline 1 shows accumulation of curvature error and increasing deviation from the USC center region. Baseline 2 maintains feasibility but introduces higher local curvature compared with shorter missions. The proposed method retains low-curvature structure without observable degradation, indicating that the spatial–temporal coupling does not amplify oscillations over extended distances.

Quantitative metrics for the three scenarios are listed in

Table 4. Performance comparison in complex scenarios.

Scenario	Method	Length (m)	Max Turning Angle (rad)	Total Turn (rad)	Energy	Time (s)	Cost
Dense Obstacles	Ours	75.21	0.078	2.74	−8.57	67.12	29.87
	Baseline 1	77.90	0.226	4.46	−7.14	111.28	37.82
	Baseline 2	80.64	0.126	6.00	−8.22	56.89	33.33
Complex Flow	Ours	70.94	0.071	2.88	−7.83	58.69	28.14
	Baseline 1	68.52	0.454	3.83	−7.10	97.88	32.39
	Baseline 2	72.92	0.171	4.79	−7.36	53.12	29.99
Large Scale	Ours	150.49	0.047	2.59	−15.20	132.59	59.81
	Baseline 1	154.39	0.224	3.33	−14.96	220.56	70.85
	Baseline 2	157.54	0.103	4.23	−14.21	107.60	62.10

. Total Turning Angle (TTA) reflects trajectory smoothness and steering effort, and is therefore used as a primary indicator for robustness. The cost J corresponds to the unified objective used in global optimization in Equation (26). In the dense obstacle scenario, the proposed method reduces TTA to $2.74\mathrm{rad}$, approximately half of Baseline 2. In the large-scale scenario, TTA remains at $2.59\mathrm{rad}$ even as path length exceeds $150\mathrm{m}$, whereas both baselines increase above $3.0\mathrm{rad}$.

Energy results support this trend. In all three scenarios, the proposed method achieves the lowest (most negative) energy measure, indicating effective exploitation of favorable flow direction rather than resisting it. Improvements are the most pronounced in the complex flow environment, where fluid disturbances modify the cost landscape. This behavior suggests that the hierarchical optimization structure does not rely on local corrections alone, but benefits from flow-aware allocation of spatial and temporal degrees of freedom.

Across the three experimental settings, the proposed method maintains spatial feasibility, low curvature, and favorable energy characteristics even as environmental complexity increases. These results indicate that robustness does not originate from conservative behavior but from coordinated spatial–temporal adaptation that prevents error accumulation.

4.3.3. Computational Time Analysis

The evaluation focuses on the computational load of the three sequential stages in the hierarchical framework: CB-RRT* path finding, USC construction, and spatiotemporal trajectory optimization. The average runtime in four representative scenarios is listed in

Table 5. Computational Time Breakdown of the Proposed Hierarchical Framework.

Scenario	CB-RRT* (ms)	USCs Const. (ms)	Trajectory Opt. (ms)	Total Time (ms)
Basic Scenario	10.25	4.76	648.19	663.20
Dense Obstacles	10.64	7.31	657.19	675.14
Complex Flow	10.41	5.37	706.24	722.02
Large Scale	12.34	12.55	1495.37	1520.26

, measured in milliseconds.

Across all scenarios, the front-end remains lightweight. CB-RRT* maintains a stable runtime of approximately 10–12 ms even when obstacle density or current complexity increases, and USC construction consistently falls in a 4–13 ms range. The bounded growth in runtime confirms that the topology-guided search prevents front-end computation from escalating with environmental clutter.

The back-end trajectory optimization dominates the total computation. In the basic, dense, and complex-flow settings, the runtime remains concentrated around 650–700 ms. The slight increase under greater environmental complexity suggests that solver convergence is not strongly affected by irregular obstacle geometry or current dynamics. The stability of the back end in these cases highlights that the Bézier-based formulation retains numerical tractability.

Only the large-scale case exhibits a marked rise in optimization time, reaching 1.5 s. This is attributed to the need for a higher-resolution representation of the trajectory; longer distances require more control points and thus a higher-dimensional optimization variable set. Even so, the resulting planning latency remains compatible with typical AUV operational envelopes. Given common cruising speeds of 0.5–2.0 m/s and the slower dynamic response of underwater platforms relative to aerial robots, a replanning rate on the order of 1–1.5 Hz is sufficient for online navigation.

Overall, the framework maintains a practical balance between solution quality and runtime. The execution characteristics indicate that deployment in real underwater missions is attainable without sacrificing optimality or robustness.

5. Conclusions and Future Work

This paper presents a hierarchical trajectory planning framework for AUVs operating in three-dimensional ocean environments influenced by spatially varying currents. The framework targets the coupled issues of obstacle avoidance, dynamic feasibility, and energy expenditure by decomposing the planning process into three coordinated stages. The first stage applies a current-biased RRT* strategy to generate reference paths that follow favorable flow structures while reducing exposure to adverse currents, providing a topology that preserves accessibility in cluttered regions. Underwater Safe Corridors are then constructed around the obtained paths to supply convex spatial bounds for collision avoidance. The final stage performs alternating spatial–temporal optimization, where Bézier control points and temporal knot vectors are updated in turn. This joint treatment of geometry and timing enforces kinematic limits directly in the optimization, and the resulting trajectories exhibit a balanced compromise between smoothness, energy usage, and traversal time without relying on post hoc retuning.

Simulation studies and comparisons with representative baseline methods indicate consistent improvements in several aspects. In downstream regions, energy consumption is reduced through passive assistance from the flow field rather than increases in actuation. Turning profiles remain within a narrow angular bound; the maximum turning angle is maintained near $0.05\mathrm{rad}$, which lowers control effort and reduces sensitivity to model mismatch in the subsequent tracking stage. Tests in dense environments and over extended map scales show that solutions remain feasible without frequent solver restarts or degradation into locally confined behaviors. Runtime measurements place the total computation within $0.6$–$1.5\mathrm{s}$, corresponding to a replanning rate on the order of 1–$1.5\mathrm{Hz}$, compatible with the response characteristics of typical AUV platforms.

Several directions emerge from the current limitations. Real deployments require integration with onboard perception to manage unmodeled moving obstacles such as biological targets or nearby vessels, and coupling the planner with detection and filtering modules will be necessary to preserve feasibility under partial observability. The assumption of a fully known flow field may be relaxed by augmenting the model with in situ measurements; data-driven updates through, for example, Gaussian-process regression or compact neural predictors could refine current estimates online without destabilizing the optimization. Extension to multi-vehicle scenarios also appears viable, particularly for applications that benefit from coordinated coverage or formation maintenance. In such settings, trajectory generation may be paired with distributed collision avoidance or shared corridor allocation to prevent mutual obstruction while leveraging the structure of the proposed framework.