Dynamic Mission Planning Framework for Collaborative Underwater Operations Using Behavior Trees

Abstract

This paper presents a behavior tree-based control architecture for end-to-end mission planning of an autonomous underwater vehicle (AUV) collaborating with a moving mothership in dynamic marine environments. The framework is organized into three phases—prepare and launch, execute the mission, and retrieval and docking—each encapsulated in an independent sub-tree to enable modular error handling and seamless phase transitions. The AUV and mothership operate entirely underwater, with real-time docking to a moving platform. An extended Kalman filter (EKF) fuses data from inertial, pressure, and acoustic sensors for accurate navigation and state estimation. At the same time, obstacle avoidance leverages forward-looking sonar (FLS)-based potential field methods to react to unpredictable underwater hazards. The system is implemented on the robot operating system (ROS) and validated in the Stonefish physics engine simulator. Simulation results demonstrate reliable mission execution, successful dynamic docking under communication delays and sensor noise, and robust retrieval from injected faults, confirming the validity and stability of the proposed architecture.

1. Introduction

In recent years, autonomous underwater vehicles (AUVs) have played an important role in marine robotics for various missions, including seafloor topographic surveys, resource exploration, and environmental monitoring [1,2,3]. However, AUVs have limited underwater communication bandwidth and range in real-world marine environments and are frequently exposed to environmental uncertainties [4] such as rapidly changing currents and unpredictable obstacles [5]. As a result, AUVs cannot rely solely on teleoperation and require a high degree of autonomy to resolve unexpected situations independently. System robustness and versatility are essential, especially for AUVs operating in remote waters. A focus has been operationalizing the pre-mission cycle, including AUV launch and retrieval, through collaboration with the mothership [6]. Launching an AUV from a mothership and recovering it after the mission offers significant benefits in terms of increased operational radius, remote support, and refueling [7]. However, docking an AUV to a moving mothership presents complex challenges [8], including difficulties in relative position estimation, collision risk due to simultaneous movement, communication delays, and sensor noise [9,10,11].

Behavior trees (BTs) are emerging as an effective control technique for these complex mission environments [12]. BTs have the advantage of being highly modular and scalable and can easily express complex decisions in a clear hierarchical structure [13]. In particular, fallback, sequence, and condition nodes can be utilized to handle exceptions on the fly and support flexible path re-planning in highly uncertain maritime environments [14]. However, existing studies mainly focus on static situations such as solo mission execution or collaboration with a stationary mothership, and there is a lack of research considering cooperative operation with a moving mothership. Therefore, this research proposes implementing a behavior tree-based architecture for cooperative missions between a moving mothership and an AUV, enabling consistent and integral responses to various exceptional situations.

In this paper, we leverage the advantages of modularity and scalability of behavioral trees to address a mothership–AUV collaboration scenario in a marine environment, proposing a modularized control structure that clearly distinguishes three phases: launch, mission, and retrieval, including situations where an AUV docks with a moving ship. Each phase comprises independent sub-trees, which allows us to design the error handling of sudden situations and the transition process between phases. Additionally, an extended Kalman filter (EKF) is employed for precise navigation and position estimation of the AUV. At the same time, a potential field technique based on forward-looking sonar (FLS) is employed for obstacle avoidance, enabling more efficient and safe path planning. The proposed architecture was experimentally validated through simulations using the robot operating system (ROS) and Stonefish, a physics engine-based simulator, to verify its validity and stability. The highlights of this paper are as follows.

• We propose a modular behavior tree-based framework that enables seamless end-to-end mission planning (launch, execution, retrieval, and dynamic docking) for cooperative operations between a moving mothership and an AUV in dynamic marine environments.
• Each mission phase is implemented as an independent sub-tree, allowing for robust error handling and smooth phase transitions.
• Standard localization and obstacle avoidance methods (EKF and FLS-PF) are integrated as modular components, ensuring future extensibility with minimal redesign.
• The proposed system is validated in realistic simulation scenarios (ROS, Stonefish), demonstrating reliable performance and resilience in the presence of communication delays, sensor noise, and injected faults.

2. Overview of Behavior Trees and Application

2.1. Concept of Behavior Trees

A behavior tree is a hierarchical decision-making framework widely used in robotics and autonomous systems. Structured as a directed acyclic graph, a BT decomposes complex logic into modular nodes, each returning a status of Success, Failure, or Running. Parent nodes aggregate their children’s results to determine the overall behavior, enabling flexible handling of exceptions and dynamic mission requirements thanks to the tree’s inherent modularity and extensibility.

2.2. Basic Node Types

The fundamental node types are sequence, fallback (selector), parallel, action, condition, and decorator. Action and condition nodes directly interact with the robot (issuing actuator commands or checking sensors). Sequence, fallback, parallel, and decorator nodes are parents that propagate a periodic tick from the top child downward, deciding their status by combining the children’s results according to the rules summarized in

Table 1. Behavior tree node characteristics.

Node	Symbol	Succeeds if	Fails if	Running if
Sequence		All children succeed	Any child fails	At least one child is Running
Fallback		One child succeeds	All children fail	At least one child is Running
Parallel		$\ge M$ children succeed	$>N-M$ children fail	Otherwise
Action		Action completes OK	Error encountered	Action still in progress
Condition		Predicate is true	Predicate is false	Never
Decorator		Custom rule	Custom rule	Custom rule

. Tick frequency can be tuned to meet responsiveness and computational constraints.

A BT tick starts at the root, flows downward through these node types, and returns upward once all necessary children have been evaluated. The following paragraphs and figures explain each node in turn.

Figure 1a shows a fallback node that ticks its children from left to right. It returns Success at the first successful child, Running if a child is still executing, and Failure only if every child fails. The corresponding pseudocode is given in Figure 1b.

The sequence node, shown in Figure 2a, also ticks children from left to right, but returns Failure or Running immediately when encountered. It succeeds only if every child succeeds, as illustrated in Figure 2b.

Figure 3a illustrates a parallel node that ticks all children each cycle, counting Successes and Failures. It returns Success when at least M children succeed, Failure when more than $N-M$ fail, and otherwise reports Running, as formalized in Figure 3b.

Figure 3a illustrates a parallel node that ticks all children each cycle, counting successes and failures. It succeeds when at least M children succeed, fails when more than $N-M$ fail, and otherwise reports Running, as formalized in Figure 3b.

Action and condition nodes directly interface with hardware or software components. As shown in Figure 4a, an action node keeps returning Running until its task completes, then outputs Success or Failure. A condition node (Figure 5a) instantly evaluates a predicate and outputs Success or Failure (never Running); their pseudocode counterparts are given in the panels of Figure 4b and Figure 5b.

Finally, the decorator node in Figure 6a wraps a single child and alters—or simply filters—its result according to a custom rule such as timeout, inversion, or repetition. The example pseudocode in Figure 6b enforces a time limit.

3. Methodology

3.1. Proposed Behavior Tree Structure

We proposed a systematic framework for the whole mission cycle of an AUV operating with a moving mothership in Figure 7. The mission is structured into three phases: prepare and launch, mission execution, and retrieval and docking. The necessary actions and conditions are organized for each phase using behavior tree sequence nodes, resulting in a robust and modular workflow.

This structure allows the AUV to safely initiate from the mothership, dynamically respond to various operational challenges, including obstacle avoidance, sensor faults, and communication interruptions, and ultimately return for retrieval. Sequence nodes ensure that all required sub-tasks within a phase are completed before the system advances to the next phase. If any task fails or remains incomplete, the system immediately triggers exception-handling procedures, guaranteeing that mission progress occurs only under safe and controlled conditions.

By organizing the entire operation in this way, the proposed approach provides a flexible and reliable solution for collaborative underwater missions involving multiple AUVs and complex scenarios. Figure 8 illustrates a simplified visual sequence of the AUV’s operation phases, including deployment, navigation, and retrieval. While it includes schematic 3D models, it serves as a conceptual illustration and may omit or simplify specific hardware details such as control surface configurations.

• Prepare and launch: After deployment is initiated, the AUV separates from the mothership while maintaining a designated safe distance to prevent collision. During this phase, the AUV descends to its target depth and conducts initial system checks, including sensor diagnostics and thruster functionality. The AUV continuously monitors the relative speed and position of the moving mothership, adjusting its maneuvers accordingly to ensure a stable and controlled departure.
• Execute mission: Upon successful launch and initialization, the AUV sequentially visits multiple mission waypoints, which are indicated as yellow markers in the scenario. At each waypoint, the AUV performs various tasks, including seabed mapping, acquiring photographic data, and avoiding obstacles. Throughout this phase, the system is designed to be robust against unexpected events. Suppose anomalies such as sensor faults or communication losses are detected. In that case, the behavior tree promptly activates a dedicated safety node, enabling the AUV to reroute, temporarily suspend its mission, or initiate retrieval actions to ensure operational safety.
• Retrieval and docking: Once all waypoint tasks are completed, the AUV navigates toward the moving mothership to initiate the retrieval sequence. The AUV aligns its trajectory and speed to match those of the mothership, approaches the designated retrieval zone, and carries out docking or ingress maneuvers. The process continues until the AUV is securely recovered within the mothership, marking the end of the mission cycle.

3.1.1. Launch Preparation

The primary objective of the launch phase is to ensure that the AUV safely undocks from the mothership and completes all essential pre-mission checks before commencing the primary mission. Figure 9 depicts the corresponding prepare-and-launch subtree, highlighting the sequence and fallback nodes that enforce these checks. The tree is organized around a top-level sequence node that triggers the critical tasks in order: issuing the launch command, verifying thruster and propeller operation, performing leak detection, checking depth limits, and confirming battery charge. Each test must return Success for the sequence to advance; if any anomaly (e.g., a faulty actuator, excessive depth, or low battery) is detected, execution is halted and routed to a diagnostic routine. Condition nodes also monitor whether the AUV has fully disengaged from the docking interface and whether sensor data streams are nominal. If undocking fails or a sensor fault is detected, a fallback branch issues a retry or, if necessary, aborts the launch. Only after all checks succeed and the AUV is clear of the mothership does control pass to the mission execution phase.

By combining sequence and fallback nodes in this manner, the behavior tree framework improves the safety and reliability of the launch process, providing prompt exception handling and ensuring the AUV proceeds only under verified operational conditions.

3.1.2. Mission Execution

At the start of the mission execution phase, the behavior tree evaluates a primary condition node that determines overall system safety [15]. The complete mission execution subtree, including this safety gate and its contingency branches, is shown in Figure 10. This check covers a range of factors, including sensor health, communication status, temperature limits, and other criteria summarized in

Table 2. Safety parameter thresholds used by the behavior tree condition nodes.

Safety Node	Parameter	Value/Threshold
Battery level	Min percentage	30%
Abort & surface	Min altitude	1.0 m
	Max depth	300.0 m
Navigator time	Sensor timeout	30 s
Internal temperature	Max temperature	Battery: 55 °C PC: 80 °C
Distance to mothership	Safe distance	1000 m

. If any abnormal condition is detected, the parent fallback node immediately triggers a safety action, such as evasive maneuvers or an emergency ascent, halting the mission to prevent further risk. This ensures that the AUV prioritizes rapid retrieval and system stabilization over continuing the mission in unsafe conditions. Next, a dedicated condition node verifies the stability of the navigation state by assessing the accuracy of position estimation, typically using data from the inertial navigation system (INS), global positioning system (GPS), or other localization sensors. If localization uncertainty is deemed too high, the mission is temporarily paused, allowing for recalibration or sensor checks before resuming operations. Mission synchronization is confirmed to ensure that all waypoints and operational scripts are correctly loaded and communicated. Should synchronization fail, an internal action node is automatically invoked to reload the data or receive updated mission parameters from remote sources.

Once all preparatory checks return success, the AUV proceeds to the core execute mission phase, performing navigation, exploration, and data acquisition tasks. Throughout the mission, the behavior tree continuously monitors for mission completion. In scenarios where obstacle detection is required, a high-priority fallback node checks for hazards using dedicated condition nodes. An avoidance action is immediately executed upon detecting an obstacle, recalculating a safe trajectory, or initiating evasive maneuvers as needed. This cycle of safety checks, navigation validation, mission synchronization, and dynamic obstacle handling repeats as the AUV sequentially visits each waypoint. The mission phase concludes when all tasks are completed, and the system transitions to the next stage. By strategically combining condition and fallback nodes, the behavior tree framework enables flexible and safe autonomous mission execution, ensuring the AUV can respond effectively to various operational contingencies.

3.1.3. Retrieval and Docking

The retrieval phase focuses on safely recovering the AUV to the mothership after the mission is completed, with an emphasis on accurate localization and robust communication. Because both the mothership and the AUV remain submerged during this process, satellite-based GPS is generally unavailable, and the system primarily relies on ultrashort baseline (USBL) acoustic positioning for navigation, which provides an accuracy of approximately 1–3 m under typical operating conditions. The detailed retrieval and docking subtree, including these checks and contingency branches, is illustrated in Figure 11. The condition node continuously monitors the reliability and accuracy of USBL-based localization. Suppose the position uncertainty exceeds acceptable thresholds or acoustic communication becomes unstable. In that case, the fallback node immediately invokes contingency actions such as system reset (prepare system) or emergency ascent (abort and surface), temporarily surfacing the AUV to reacquire GPS-based position data if necessary. Navigation uncertainty is further monitored in real-time, and should localization confidence degrade, the AUV ascends to the surface to obtain a GPS fix before returning to the retrieval zone. Once reliable position information is restored, the AUV attempts to reach the designated retrieval point by executing the go-to retrieval point action, which is repeated until the AUV is confirmed to be within the required distance and bearing from the moving mothership.

To achieve precise docking during the final approach, four LEDs are installed in a square configuration at the bow of the moving mothership, serving as visual markers for navigation. The AUV utilizes its onboard vision system to detect these LEDs. It aligns its position and heading with the geometric center of the LED pattern, thereby correcting any accumulated navigation error from prior underwater transit [16,17]. Once the AUV is centered, it performs a controlled maneuver by moving 2.5 m east and 2.5 m down in the NED (North-East-Down) frame relative to the LED center, bringing it to the entrance of the docking tube. At this stage, the AUV gradually reduces its forward velocity, allowing for a smooth and safe entry into the docking tube while continuously compensating for the dynamic motion of the mothership. The condition node ‘auv is retrieved?’ confirms that the AUV has been fully recovered inside the mothership, thus marking the end of the mission. By combining multiple conditions and fallback nodes, the behavior tree structure enables the AUV to robustly address complex issues such as navigation errors and communication loss, leveraging acoustic and visual cues to maximize the safety and reliability of the retrieval operation under dynamic, real-world conditions.

3.2. Pose Estimation with an EKF

State estimation is carried out with an EKF that alternates prediction and measurement-update phases after an initial mean ${\widehat{\mathbf{x}}}_{0|0}$ and covariance ${\mathbf{P}}_{0|0}$ have been assigned [18]. The filter maintains both the best-guess state and its uncertainty, enabling statistically consistent fusion of high-rate inertial measurement unit (IMU) data with lower-rate absolute sensors such as GPS, Doppler velocity log (DVL), depth, and USBL. Here, we let the state vector $\mathbf{x}$ contain the AUV’s position, velocity, and orientation, and all relevant variables are defined explicitly when introduced.

3.2.1. Prediction

The prediction step propagates state and covariance from $t-1$ to t using only IMU data. Let the state and IMU input at $t-1$ be

$${\mathbf{x}}_t={\left[\begin{array}{cccccccccc}x& y& z& v_x& v_y& v_z& q_x& q_y& q_z& q_w\end{array}\right]}_t^{\mathsf{T}},{\mathbf{u}}_{t-1}={\left[\begin{array}{cccccc}{a}_x& a_y& a_z& {\omega }_x& {\omega }_y& {\omega }_z\end{array}\right]}_{t-1}^{\mathsf{T}},$$

(1)

where $(x,y,z)$ are the position coordinates in the navigation frame, $(v_x,v_y,v_z)$ are velocities in the navigation frame, and $(q_x,q_y,q_z,q_w)$ are the quaternion elements representing orientation. The IMU input ${\mathbf{u}}_{t-1}$ includes body-frame linear accelerations $(a_x,a_y,a_z)$ and angular rates $({\omega }_x,{\omega }_y,{\omega }_z)$.

Navigation–frame acceleration is

$${\mathbf{a}}_{t-1}={\mathbf{C}}_{bn}\left({\mathbf{q}}_{t-1}\right)\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right],$$

where ${\mathbf{C}}_{bn}\left({\mathbf{q}}_{t-1}\right)$ is the rotation matrix converting vectors from body to navigation frame as defined by quaternion ${\mathbf{q}}_{t-1}$.

Over a step $\Delta t$, the state prediction is

$${\overline{\mathbf{x}}}_t=\mathbf{f}({\mathbf{x}}_{t-1},{\mathbf{u}}_{t-1}),$$

(2)

$$\mathbf{f}({\mathbf{x}}_{t-1},{\mathbf{u}}_{t-1})=\left[\begin{array}{c}{x}_{t-1}+v_{x,t-1}\Delta t+{\displaystyle \frac{1}{2}}{a}_{x,t-1}\Delta t^2\\ y_{t-1}+v_{y,t-1}\Delta t+{\displaystyle \frac{1}{2}}{a}_{y,t-1}\Delta t^2\\ z_{t-1}+v_{z,t-1}\Delta t+{\displaystyle \frac{1}{2}}{a}_{z,t-1}\Delta t^2\\ v_{x,t-1}+a_{x,t-1}\Delta t\\ v_{y,t-1}+a_{y,t-1}\Delta t\\ v_{z,t-1}+a_{z,t-1}\Delta t\\ {\mathbf{q}}_{t-1}\otimes exp\left({\displaystyle \frac{1}{2}}\mathbf{\Omega }\left({\mathsf{\omega }}_{t-1}\right)\Delta t\right)\end{array}\right],$$

(3)

where ⊗ denotes quaternion multiplication, $\exp (·)$ is the matrix exponential mapping angular rates to quaternion increment, and $\mathbf{\Omega }\left({\mathsf{\omega }}_{t-1}\right)$ constructs the quaternion kinematic matrix from the angular rate vector ${\mathsf{\omega }}_{t-1}$.

The covariance prediction is

$${\overline{\mathbf{P}}}_t={\mathbf{F}}_t{\mathbf{P}}_{t-1}{{\mathbf{F}}_t}^{\mathsf{T}}+{\mathbf{Q}}_{t-1},{\mathbf{F}}_t={\left.{\displaystyle \frac{\partial \mathbf{f}}{\partial \mathbf{x}}}\right|}_{{\mathbf{x}}_{t-1},{\mathbf{u}}_{t-1}},$$

(4)

where ${\mathrm{Q}}_{t-1}$ captures IMU noise, bias drift, and unmodelled dynamics, and ${\mathrm{F}}_t$ is the state transition Jacobian with respect to $\mathrm{x}$.

3.2.2. Measurement Update

When a measurement ${\mathbf{z}}_t$ arrives, the prior $({\overline{\mathbf{x}}}_t,{\overline{\mathbf{P}}}_t)$ is corrected as

$$\begin{array}{cc}\hfill {\mathbf{z}}_t& =\mathbf{h}\left({\overline{\mathbf{x}}}_t\right)+{\mathbf{v}}_t,{\mathbf{v}}_t\sim \mathcal{N}(\mathbf{0},{\mathbf{R}}_t),\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill {\mathbf{S}}_t& ={\mathbf{H}}_t{\overline{\mathbf{P}}}_t{{\mathbf{H}}_t}^{\mathsf{T}}+{\mathbf{R}}_t,\hfill \end{array}$$

(6)

$$\begin{array}{cc}\hfill {\mathbf{K}}_t& ={\overline{\mathbf{P}}}_t{{\mathbf{H}}_t}^{\mathsf{T}}{\mathbf{S}}_t^{-1},\hfill \end{array}$$

(7)

$$\begin{array}{cc}\hfill {\mathbf{x}}_t& ={\overline{\mathbf{x}}}_t+{\mathbf{K}}_t\left({\mathbf{z}}_t-\mathbf{h}\left({\overline{\mathbf{x}}}_t\right)\right),\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill {\mathbf{P}}_t& =(\mathbf{I}-{\mathbf{K}}_t{\mathbf{H}}_t){\overline{\mathbf{P}}}_t.\hfill \end{array}$$

(9)

where $\mathbf{h}\left({\overline{\mathbf{x}}}_t\right)$ denotes the measurement model mapping state to expected measurement; ${\mathbf{R}}_t$ is the measurement noise covariance; ${\mathbf{K}}_t$ is the Kalman gain; and ${\mathbf{S}}_t$ is the innovation covariance.

The Jacobian matrix ${\mathbf{H}}_t$ is defined as

$${\mathbf{H}}_t=\left[\begin{array}{c}{\mathbf{H}}_{\mathrm{GPS}}\\ {\mathbf{H}}_{\mathrm{DVL}}\\ {\mathbf{H}}_{\mathrm{Depth}}\\ {\mathbf{H}}_{\mathrm{USBL}}\end{array}\right],{\mathbf{H}}_{\mathrm{GPS}}=\left[{\mathbf{I}}_2{\mathbf{0}}_{2\times 8}\right],{\mathbf{H}}_{\mathrm{DVL}}=\left[{\mathbf{0}}_{3\times 3}{\mathbf{C}}_{bn}{\left(\mathbf{q}\right)}^{\mathsf{T}}{\mathbf{0}}_{3\times 4}\right],{\mathbf{H}}_{\mathrm{Depth}}=\left[001{\mathbf{0}}_{1\times 7}\right],{\mathbf{H}}_{\mathrm{USBL}}=\left[{\mathbf{I}}_3{\mathbf{0}}_{3\times 7}\right].$$

(10)

where ${\mathbf{I}}_n$ is the $n\times n$ identity matrix and ${\mathbf{0}}_{m\times n}$ is an $m\times n$ zero matrix. ${\mathbf{C}}_{bn}{\left(\mathbf{q}\right)}^{\mathsf{T}}$ rotates velocity from the navigation frame to the body frame.

To fuse DVL measurements, which are made in the body frame, with navigation-frame state, the rotation transformation ${\mathbf{C}}_{bn}\left(\mathbf{q}\right)$ and its transpose are used; this ensures alignment of the coordinate systems for proper sensor fusion. Note that heading errors in $\mathbf{q}$ can result in long-term drift in navigation performance.

The measurement vector ${\mathbf{z}}_t$ may contain GPS positions $(x,y)$ when surfaced, DVL velocity $(v_x,v_y,v_z)$, depth sensor pressure-derived z, and USBL-based $(x,y,z)$ acoustic fixes.

Combined through Equations (6)–(9), these heterogeneous measurements produce a statistically consistent pose estimate for higher-level mission logic.

3.3. Obstacle Avoidance Using FLS-Based Potential Fields

Obstacle information is obtained from the FLS, which returns, for each detected obstacle i, its polar coordinates $(r_i,{\theta }_i)$ in the AUV body frame (Figure 12) [19], where $r_i$ is the distance and ${\theta }_i$ is the bearing angle to obstacle i in the body-fixed frame.

$$U_{\mathrm{rep}}\left(r_i\right)=\left\{\begin{array}{cc}\frac{1}{2}\eta {\left({\displaystyle \frac{1}{r_i}}-{\displaystyle \frac{1}{r_0}}\right)}^2,\hfill & r_i\le r_0,\hfill \\ 0,\hfill & r_i>r_0,\hfill \end{array}\right.$$

(11)

where $\eta$ is the repulsive gain parameter, and $r_0$ is the influence threshold at which obstacles further than $r_0$ exert no repulsion.

$${\mathbf{F}}_{\mathrm{rep},i}=\eta \left({\displaystyle \frac{1}{r_i}}-{\displaystyle \frac{1}{r_0}}\right){\displaystyle \frac{1}{r_i^2}}\left[\begin{array}{c}cos{\theta }_i\\ sin{\theta }_i\end{array}\right],$$

(12)

where ${\mathbf{F}}_{\mathrm{rep},i}$ is the repulsive force from obstacle i, and the direction $\left[\begin{array}{c}cos{\theta }_i\\ sin{\theta }_i\end{array}\right]$ points away from obstacle i.

$${\mathbf{F}}_{\mathrm{rep}}=\sum _{i=1}^N{\mathbf{F}}_{\mathrm{rep},i},$$

(13)

where N is the total number of detected obstacles.

$${\mathbf{F}}_{\mathrm{att}}=K_{\mathrm{att}}\left[\begin{array}{c}cos{\theta }_g\\ sin{\theta }_g\end{array}\right],{\theta }_g=atan2(y_g-y,x_g-x),$$

(14)

where $K_{\mathrm{att}}$ is the attractive gain, $(x_g,y_g)$ is the target position, and $(x,y)$ is the current AUV position. ${\theta }_g$ is the angle to the goal from the current position.

$$\mathbf{F}={\mathbf{F}}_{\mathrm{att}}+{\mathbf{F}}_{\mathrm{rep}},$$

(15)

where $\mathbf{F}$ is the total force vector combining attraction and repulsion.

$$\Delta \psi =atan2\left(sin({\psi }_d-\psi ),cos({\psi }_d-\psi )\right),$$

(16)

where $\psi$ is the current heading of the AUV, ${\psi }_d=atan2(F_y,F_x)$ is the desired heading determined by the total force $\mathbf{F}$, and $\Delta \psi$ is the heading error to be controlled.

$$\dot{\psi }=K_{\psi }\Delta \psi ,$$

(17)

where $K_{\psi }$ is the yaw controller proportional gain and $\dot{\psi }$ is the commanded yaw rate.

$$v=v_{max}min\left(1,{\displaystyle \frac{r_{min}}{r_0}}\right),$$

(18)

where v is commanded surge speed, $v_{max}$ is the maximum surge speed, and $r_{min}$ denotes the distance to the nearest obstacle; as $r_{min}$ decreases, the AUV slows down for safety.

In the artificial potential field (APF) formulation [20], the mission goal is treated as an attractive source, whereas every obstacle contributes the repulsive potential of Equation (11). The negative gradient yields the single-obstacle force in Equation (12), and summing these forces gives the total repulsive term of Equation (13). Adding the attractive force of Equation (14) produces the net force in Equation (15), whose direction defines the desired yaw angle ${\psi }_d=atan2(F_y,F_x)$. The heading error $\Delta \psi$ in Equation (16) is cancelled by the proportional yaw-rate command of Equation (17), and the surge-speed law in Equation (18) scales with the nearest obstacle range $r_{min}$, slowing the AUV in confined spaces. This APF controller is computationally lightweight yet effectively merges goal seeking with real-time avoidance of FLS-detected hazards.

4. Simulation

4.1. Simulation Setup

This study implemented a docking scenario between an AUV and a moving mothership by integrating a physics-based simulator with ROS Noetic (Ubuntu 20.04). For the simulation environment, the Stonefish simulator (Version 1.5) [21], widely used for marine robotics research, was employed with the Bullet physics engine to model hydrodynamic drag, buoyancy, and collision effects. This setup enables the realistic reproduction of diverse and dynamic behaviors commonly encountered in marine environments. It enables the real-time integration of sensor and control nodes in ROS, creating an experimental platform that resembles actual field operations.

Both the AUV and mothership models were designed to be close to real-world scale using Autodesk Inventor for 3D modeling, then imported into the Stonefish simulator for visualization and dynamic simulation. The mothership model was configured with an overall length of approximately 89.4 m and a diameter of 9.6 m, featuring a horizontal launch tube near the bow to facilitate AUV ingress and egress. For simplicity, the propulsion system was implemented with a single thruster and rudder, allowing for straight-line motion and heading adjustments in the simulated environment. The AUV model includes both vertical and horizontal thrusters arranged in a vectored configuration, enabling full 6-DOF motion. These thrusters are symmetrically distributed along the hull and are not placed end-to-end, ensuring stable control authority in confined spaces.

The AUV is a compact platform measuring 2.5 m in length and approximately 0.2 m in diameter. It is equipped with a rear surge thruster for forward propulsion and pairs of horizontal and vertical thrusters, enabling maneuvering in both vertical and lateral directions. Detailed sensor and actuator specifications for both the AUV and mothership platforms used in the simulation are provided in Appendix A. To ensure physically plausible motion, the hydrodynamic effects acting on both the AUV and the mothership are approximated by the Bullet physics engine within the Stonefish simulator. Instead of explicitly defining hydrodynamic coefficients such as linear drag or added mass, the simulator derives forces from the geometry, volume, and inertial properties of each body. This enables realistic underwater dynamics based on high-level physical modeling, eliminating the need for manual tuning of specific coefficients.

Both models are subject to approximated hydrodynamic effects via the Bullet physics engine, with parameters such as mass, volume, and center of gravity set to closely match those of real marine AUVs, ensuring realistic dynamic behavior.

Within the simulation, the mothership and AUV interact physically in the Stonefish environment, with key variables such as AUV position, orientation, and thruster output monitored in real-time via ROS.

4.2. Results

In this section, we present simulation results demonstrating the effectiveness of the proposed behavior tree-based control architecture. The simulation environment is configured with a water depth of 50 m, and the AUV is tasked with traversing a mission route exceeding 3 km, during which it must avoid randomly positioned obstacles. The mothership advances along a prescribed path, continuously adjusting its heading throughout the scenario. To more accurately reflect underwater operating conditions, a constant ocean current was incorporated into the simulation environment. Specifically, the current was modeled with a velocity of 0.2 m/s in the positive x-direction and 0.01 m/s in the positive y-direction. These values were selected to simulate mild but non-negligible environmental disturbances affecting vehicle navigation.

Two main scenarios were investigated. The first scenario evaluates system performance when all behavior tree nodes function correctly, resulting in a nominal mission execution. The second scenario investigates how the system responds to a fault occurring in one of the behavior tree nodes, thereby validating the architecture’s fault tolerance and exception handling capabilities.

4.2.1. Scenario 1: Mission Success Under Nominal Conditions

In the nominal operation scenario result shown in Figure 13, all behavior tree nodes across the launch, mission execution, and retrieval phases function as intended, consistently returning success. The mothership continuously changes its heading while moving forward throughout the mission. The AUV initially navigates using USBL-based acoustic positioning and INS data to track its relative position and heading concerning the moving mothership. As the AUV enters the retrieval phase, it switches to a vision-based alignment mode that detects the LED markers mounted at the mouth of the retrieval tube. Figure 14 illustrates the visual alignment process. Figure 14a gives an overview of the retrieval scene, showing the AUV, the moving mothership, and the LED targets at the tube entrance. Figure 14b is the image captured by the AUV’s rear-facing camera, where the LED markers are detected. Figure 14c shows the binary segmentation mask derived from those detections, from which the LED centroids are calculated for fine positioning. Using these cues, the AUV adjusts its heading and lateral offset, aligns with the moving mothership, and glides into the retrieval tube to complete docking. The results confirm that the proposed behavior tree framework delivers robust and reliable mission execution, even during dynamic cooperative maneuvers.

4.2.2. Scenario 2: Fault Response and Exception Handling

In the second scenario, the result shown in Figure 15, mission termination is triggered by a low-battery condition rather than a hardware fault. Once the AUV is launched, a Parallel node supervises two concurrent tasks: (i) waypoint navigation and (ii) real-time battery monitoring via the BatterySufficient condition node. The critical threshold is fixed at 30 % (Table 2). When the state of charge drops below this value, BatterySufficient returns Success. Because the parallel node follows a success on one policy, this single success suffices to halt the mission branch and propagate success upward, prompting the higher-level tree to abort remaining waypoints and switch to the retrieval and docking subtree.

The resulting handover is immediate and orderly. The AUV ascends if necessary, navigates to the rendezvous point, and docks with the moving mothership. Figure 16 summarizes the full execution flow from nominal operation through mission completion, battery-triggered abort, and final retrieval, demonstrating how the parallel node architecture enables a real-time, battery-aware transition without hard-coded exception handlers.

4.3. Discussion

The simulation results show that the proposed behavior tree framework delivers robust and flexible mission planning for collaborative AUV–mothership operations. Its modular structure cleanly separates safety checks, navigation validation, and mission execution, so faults stay localized and the vehicle can resume its tasks with minimal disruption. Because mission logic is composed of reusable BT nodes, new behaviors can be added or existing ones modified without rewriting the entire control scheme, making the approach adaptable to diverse mission profiles and future platform upgrades.

5. Conclusions

This work proposed a behavior tree-based mission planning and control architecture to enable safe and flexible collaboration between AUVs and a moving mothership. By decomposing the mission into launch, mission execution, and retrieval phases, the framework supports efficient task management and rapid response to unexpected events. Simulation results confirmed the reliability and effectiveness of the approach, particularly in vision-based docking and dynamic obstacle avoidance scenarios. The modularity and scalability inherent in the behavior tree design further enhance the system’s adaptability to various operational contexts.

The proposed architecture was validated primarily through comprehensive simulation studies. As a next step, experimental verification with real, on-site AUV deployments is required further to assess the practical robustness and reliability of the system. While the simulations demonstrated the framework’s ability to manage multiple AUVs, further research is needed to verify scalability for larger fleets and more complex, heterogeneous mission profiles. Comparative analyses with other mission planning approaches, such as finite state machines and decision trees, are also planned to quantitatively evaluate the benefits and trade-offs of the behavior tree-based method. Additional future directions include expanding to more complex mission phase structures, adapting to heterogeneous vehicle teams, and further improving reproducibility and real-world deployment guidelines.