Active Disturbance Rejection and Energy Optimization for Autonomous Underwater Vehicles via Finite-Time Extended State Observer and Ocean Current-Adaptive Model Predictive Control

2.1 Mathematical 6-degree-of-freedom model of the Autonomous Underwater Vehicle under environmental disturbances

To design a spatial trajectory tracking and omnidirectional disturbance compensation controller, establishing a comprehensive mathematical model that describes the kinematics and dynamics of the AUV in a 6-degree-of-freedom (6-DOF) space is a prerequisite. The system utilizes two standard reference coordinate frames: the Earth-fixed frame {E} and the Body-fixed frame {B}, as illustrated in Figure 1.

Figure 1. Body-fixed and earth-fixed frame of Autonomous Underwater Vehicle (AUV)

The motion of the AUV in 3D space comprises six components: three translational motions (surge, sway, heave) and three rotational motions (roll, pitch, yaw). Let the position and Euler angle state vector in the $\{E\}$ frame be defined as $\eta=[x, y, z, \phi, \theta, \psi]^T$, and the linear and angular velocity vector in the $\{B\}$ frame as $v=[u, v, w, p, q, r]^T$. The kinematic equation describing the transformation relationship between the two coordinate frames is expressed as follows:

$\dot{\eta}=J(\eta) v$        (1)

where, $J(\eta) \in R^{6 \times 6}$ is the coordinate transformation matrix, composed of two 3 × 3 block matrices:

$J(\eta)=\left[\begin{array}{cc}J_1\left(\eta_2\right) & 0_{3 \times 3} \\ 0_{3 \times 3} & J_2\left(\eta_2\right)\end{array}\right]$        (2)

with $\eta_2=[\phi, \theta, \psi]^T$ and $J_1\left(\eta_2\right)$ is the directional cosine matrix used to transform the translational velocities, and $J_2\left(\eta_2\right)$ is the transformation matrix for angular velocities from the {B} frame to the {E} frame. Based on the Newton-Euler equations of motion for rigid bodies in fluids [15, 16], the general 6-DOF dynamic equation of the AUV under environmental disturbances is written in matrix form:

$M \dot{v}+C(v) v+D(v) v+g(\eta)=\tau+\tau_{\text {enn }}$         (3)

where, $M=M_{R B}+M_A \in R^{6 \times 6}$ is the inertia mass matrix; $C(v)=C_{R B}(v)+C_A(v) \in R^{6 \times 6}$ is the matrix of Coriolis and centripetal forces; $D(v)=D_l+D_{n l}(v) \in R^{6 \times 6}$. is the hydrodynamic damping matrix; $g(\eta) \in R^6$ is the vector of restoring forces and moments; $\tau=\left[\tau_u, \tau_v, \tau_w, \tau_p, \tau_q, \tau_r\right]^T \in R^6$ is the control force and moment vector generated by the actuators (propellers, horizontal hydroplanes, and vertical rudders); $\tau_{e n v} \in R^6$ is the external environmental disturbance vector.

When considering the effect of ocean currents with a spatial velocity of $V_c=\left[V_x, V_y, V_z, 0,0,0\right]^T$ in the $\{E\}$ frame, the relative velocity of the AUV in the $\{B\}$ frame is $v_r=v- J^{-1}(\eta) V_c$ [17]. The alteration of the hydrodynamic field caused by $v_r$ leads to variations in the C and D matrices, generating significant nonlinear deviations from the static nominal model. All these disturbances are lumped into the $\tau_{e n v}$ vector.

Let the extended state variables of the system be defined as $x_1=\eta \in R^6$ (position and attitude angles) and $x_2=\dot{\eta} \in R^6$. (velocity in the Earth-fixed frame). From Eq. (1), we obtain:

$\begin{aligned} \dot{x}_2=\dot{J}(\eta) J^{-1}(\eta) x_2 & -J(\eta) M^{-1}\left(C\left(J^{-1} x_2\right) J^{-1} x_2\right. \left.+D\left(J^{-1} x_2\right) J^{-1} x_2+g\left(x_1\right)\right) +J(\eta) M^{-1} \tau+J(\eta) M^{-1} \tau_{e n v}\end{aligned}$         (4)

To standardize this into the input equation form for the observer and controller, we define the system dynamic components in the $R^6$ space:

$\begin{aligned} f\left(x_1, x_2\right)=\dot{J}(\eta) & J^{-1}(\eta) x_2 -J(\eta) M^{-1}\left(C\left(J^{-1} x_2\right) J^{-1} x_2\right. \left.+D\left(J^{-1} x_2\right) J^{-1} x_2+g\left(x_1\right)\right)\end{aligned}$         (5)

$B\left(x_1\right)=J(\eta) M^{-1}$        (6)

$d(t)=B\left(x_1\right) \tau_{\text {env }}+\Delta f\left(x_1, x_2\right)$          (7)

where, $\Delta f\left(x_1, x_2\right)$ represents the deviation caused by parameter uncertainties in matrices $M, C, D$ and the restoring vector $g ; B\left(x_1\right)$ is the control input gain matrix; $d(t)$ is the lumped disturbance vector. Finally, the 6-DOF mathematical model of the AUV is compactly expressed in the standard state-space form [18, 19]:

$\left\{\begin{array}{l}\dot{x}_1=x_2 \\ \dot{x}_2=f\left(x_1, x_2\right)+B\left(x_1\right) u_c+d(t)\end{array}\right.$       (8)

This mathematical model provides a unified mathematical foundation for the FTESO framework to execute observation and disturbance compensation for each DOF independently and instantaneously.

To ensure a fully reproducible evaluation of the proposed FTESO-MPC framework, the numerical simulations are formulated based on the verified mathematical model of the REMUS 100 AUV [16], which is detailed in Table 1.

Table 1. Hydrodynamic and physical parameters of the REMUS 100 Autonomous Underwater Vehicle (AUV) [16]

2.2 Finite-Time Extended State Observer design

2.2.1 Finite-Time Extended State Observer structure

To actively counteract environmental disturbances, the underlying principle is to augment the system by treating the lumped disturbance $d(t)$ in Eq. (8) as an extended state $x_3= d(t)$, with its derivative denoted as $\dot{x}_3=h(t)$. Traditional linear observers often only achieve asymptotic convergence as time approaches infinity, resulting in significant phase lag when ocean currents fluctuate abruptly. To overcome this, this paper employs an FTESO framework utilizing non-smooth error feedback to achieve rapid convergence [20, 21]:

$\left\{\begin{array}{l}\dot{z}_1=z_2-\beta_1 \operatorname{sig}^{\alpha_1}\left(e_1\right) \\ \dot{z}_2=z_3+f\left(x_1, z_2\right)+B u_c-\beta_2 \operatorname{sig}^{\alpha_2}\left(e_1\right) \\ \dot{z}_3=-\beta_3 \operatorname{sig}^{\alpha_3}\left(e_1\right)\end{array}\right.$        (9)

where, $\left(z_1, z_2, z_3\right)$ are the respective estimated values of the states $\left(x_1, x_2\right)$ and the disturbance $x_3 \cdot e_1=z_1-x_1$ represents the measurable position observation error. The fractional power sign function is defined as $\operatorname{sig}^\alpha\left(e_1\right)=\left|e_1\right|^\alpha \operatorname{sgn}\left(e_1\right)$, which provides a large observation gain when the error approaches zero, thereby accelerating the convergence rate. The observer gains $\beta_1, \beta_2, \beta_3>0$ are selected such that the characteristic polynomial $s^3+\beta_1 s^2+\beta_2 s+\beta_3=0$ satisfies the Hurwitz stability criterion [22]. The exponents $\alpha_1, \alpha_2, \alpha_3 \in(0,1)$ are designed based on geometric homogeneity theory.

2.2.2 Stability analysis of the Finite-Time Extended State Observer

To prove the finite-time convergence of the FTESO, we define the estimation error vector as $e=\left[e_1, e_2, e_3\right]^T$ where $e_i=x_i-\hat{x}_i$. Based on Eqs. (8) and (9), the error dynamics are formulated as:

$\left\{\begin{array}{c}\dot{e}_1=e_2-\beta_1\left[e_1\right]^{\alpha_1} \\ \dot{e}_2=e_3-\beta_2[e]^{\alpha_2} \\ \dot{e}_3=h(t)-\beta_3\left[e_1\right]^{\alpha_3}\end{array}\right.$         (10)

where, $h(t)=\dot{d}(t)$ is the derivative of the lumped disturbance. Assumption 1: The disturbance rate is bounded by a known constant $L(|h(t)| \leq L)$, which is physically reasonable given the finite energy of ocean currents.

According to the geometric homogeneity framework [20], by selecting the gains $\beta_i$ such that $s^3+\beta_1 s^2+\beta_2 s+\beta_3$ is Hurwitz, and fractional powers as $\alpha_1=\alpha \in(1-\epsilon, 1), \alpha_2= 2 \alpha-1$, and $\alpha_3=3 \alpha-2$.

The nominal unperturbed system $(h(t)=0)$ is globally finite-time stable due to its negative homogeneity degree $\kappa= \alpha-1<0$.

For the perturbed system $(h(t) \neq 0)$, taking a homogeneous Lyapunov function $V(e)$ of degree $p$, its time derivative satisfies $\dot{V} \leq-c_1 V^{\frac{p+K}{p}}+c_2 L\|e\|^{p-1}$ for strictly positive constants $c_1, c_2$. This inequality guarantees that the estimation errors are robustly driven into a small neighborhood of the origin within a bounded finite time $T \leq \frac{p}{c_1|\kappa|} V(e(0))^{\frac{|\kappa|}{p}}$. Crucially, increasing the linear observer gains $\beta_i$ directly enlarges the parameter $c_1$, which proportionally reduces the upper bound $T$, thereby ensuring near-instantaneous disturbance estimation for the MPC framework.

2.3 Model Predictive Control design with an environment-aware cost function

Once the spatial lumped disturbance vector $\hat{d}(t) \in R^6$ is accurately isolated and estimated within a finite time by the FTESO, the subsequent step is to synthesize the 3D trajectory tracking control law. Distinct from conventional MPC frameworks that utilize fixed weighting matrices for AUV [6, 9, 21], this study develops an Environment-aware MPC mechanism. The core of this approach is a self-tuning cost function based on the observed disturbance, aiming to thoroughly resolve the conflict between tracking accuracy and actuator saturation risk when the AUV operates in complex ocean current regions.

To implement the MPC controller, the continuous 6-DOF state equation of the closed-loop system (incorporating the feed-forward compensation signal from the FTESO) must be transformed into the discrete-time domain. Utilizing the 4th-order Runge-Kutta (RK4) approximation method with a sampling period $T_s$, the nonlinear prediction model is represented in difference equation form:

$x(k+1)=F_d\left(x(k), u_c(k)\right)+B_d(x(k)) \hat{d}(k)$         (11)

where, $x(k)=\left[\eta^T(k), v^T(k)\right]^T \in R^{12}$ is the augmented state vector at step. $u_c(k)=\tau(k) \in R^6$ is the control force/moment vector. $F_d$ and $B_d$ are the discretized nonlinear functions corresponding to the nominal dynamics. $\hat{d}(k) \in R^6$ is the spatial environmental disturbance provided by the FTESO, which is assumed to be constant throughout a basic prediction period according to the Zero-Order Hold principle.

In the spatial trajectory tracking problem, if fixed penalty weights are employed, maintaining the translational velocity (surge/heave) when encountering high-intensity counter-currents will generate control signals that exceed the physical limits of the propellers. This not only violates the recursive feasibility of the optimizer but also causes severe energy waste [22-24]. To address this, this paper proposes a non-standard cost functional $J(k)$, where the weighting matrices are defined as analytical functions directly dependent on the disturbance vector $\hat{d}(k)$:

$\begin{gathered}J(k)=\sum_{i=1}^{N_p}\left(\left\|\eta(k+i \mid k)-\eta_{\text {ref }}\right\|_{Q_\eta}^2+\| v(k+i \mid k)\right. \\ \left.-v_{\text {ref }} \|_{Q_v(\hat{d})}^2\right)+\sum_{i=0}^{N_c-1}\left\|\Delta u_c(k+i \mid k)\right\|_{R(\hat{d})}^2\end{gathered}$         (12)

where, $N_p$ and $N_c$ are the prediction horizon and control horizon, respectively; $\eta_{\text {ref }}$ and $v_{\text {ref }}$ are the reference state vectors of the 3D trajectory; $Q_\eta \in R^{6 \times 6}$ is a positive semidefinite weighting matrix penalizing deviations in position and attitude angles. This component is kept fixed to ensure directional stability and that spatial deviations do not exceed safety limits; $Q_v(\hat{d}), R(\hat{d}) \in R^{6 \times 6}$ are adaptive weighting matrices that coordinate the balance between velocity error and control effort. The dynamic weight update mechanism for the main diagonal elements of $Q_v$ and $R$ is designed following an exponential function, ensuring continuous differentiability for the optimization problem. For the $j$-th degree of freedom $(j \in\{1 . .6\})$.

$Q_{v, j i}(\hat{d})=q_{\min , j}+\left(q_{0, j}-q_{\min , j}\right) \exp \left(-\lambda_j\left|\hat{d}_j\right|\right)$         (13)

$R_{j j}(\hat{d})=r_{0, j}+\rho_j\left(1-\exp \left(-\gamma_j\left|\hat{d}_j\right|\right)\right)$         (14)

When the AUV enters a strong disturbance zone, causing a sudden increase in $\left|\hat{d}_1\right|$, the velocity penalty weight $Q_{v, 11}$ will asymptotically decay to $q_{\text {min}, 1}$, while the energy penalty cost $R_{11}$ increases. This structural reconfiguration directs the optimizer to lower the priority of strictly tracking the nominal speed (deceleration). Conversely, for the roll and pitch rotational axes, by fine-tuning the sensitivity coefficients $\lambda_j$ and $\gamma_j$ to values near zero for these specific axes, the posture stability is robustly maintained, completely eliminating the risk of capsizing or loss of depth control under the impact of complex vortex ocean currents.

The parameters of the adaptive cost function, including the exponential decay coefficients $\left(\lambda_j, \gamma_j\right)$ and the sensitivity amplitude factor $\left(\rho_j\right)$, are determined heuristically based on the REMUS 100's actuator limits and extensive simulation trials. The decay coefficients $\lambda_j$ and $\gamma_j$ dictate the rate at which the adaptive weights transition when exposed to a specific disturbance intensity $\left|\hat{d}_j\right|$. A sensitivity analysis reveals a clear operational trade-off: setting larger values for $\lambda_j$ and $\gamma_j$ enables the AUV to rapidly deprioritize velocity tracking and severely penalize large rudder inputs during strong counter-currents. This maximizes energy conservation and prevents thruster saturation, albeit causing larger transient spatial deviations. Conversely, selecting excessively small decay coefficients forces the system to strictly maintain the reference path, which minimizes cross-track errors but exponentially drains battery power and risks directional instability. The final adaptive parameters listed in Table 2 represent an optimal empirical balance, ensuring both trajectory robustness and energy efficiency under severe hydrodynamic scenarios.

At each sampling period, the MPC solves a Nonlinear Programming problem to find the optimal control signal sequence $\Delta U_c^*=\left[\Delta u_c(k \mid k), \ldots, \Delta u_c\left(k+N_c-1 \mid k\right)\right]$, subject to the physical constraint inequalities of the actuators:

$u_{\min } \leq u_c(k+i-1 \mid k)+\Delta u_c(k+i \mid k) \leq u_{\max }$        (15)

$\Delta u_{\min } \leq \Delta u_c(k+i \mid k) \leq \Delta u_{\max }$        (16)

Due to the inherent nonlinear nature of the 6-DOF dynamics and the state-disturbance dependent cost function structure, the above optimization problem is solved using the Sequential Quadratic Programming (SQP) algorithm. By integrating the disturbance information $\hat{d}$ from the FTESO, the recursive feasibility of the SQP is significantly improved because the amplitude of the feed-forward compensation signal is actively regulated by the adaptive cost function, completely eliminating the risk of boundary violations for $u_{\max }$ and $u_{\min }$.

2.4 Energy consumption metric formulation

The energy consumption evaluated in this study is formulated as a combined index that comprehensively encompasses both the mechanical propulsion energy required to overcome hydrodynamic drag and the electrical effort consumed by the steering actuators.

The instantaneous propulsion power $P_{\text {prop}}(t)$ required by the main thruster is modeled based on the cubic physical relationship with the relative surge velocity:

$P_{\text {prop }}(t)=\frac{X_{u|u|}}{\eta_{\text {prop }}}\left|u(t)-u_c(t)\right|^3$         (17)

where, $X_{u|u|}$ is the surge quadratic drag coefficient, $\eta_{\text {prop}}$ is the overall propulsion efficiency, $u(t)$ is the actual surge velocity, and $u_c(t)$ is the longitudinal component of the ocean current.

The steering effort $E_{\text {steer}}$, which reflects the energy consumed by the servo motors to continuously actuate the control surfaces, is quantified by integrating the squared fin deflections:

$E_{\text {steer}}=\int_0^T\left(\delta_r^2(t)+\delta_s^2(t)\right) d t$        (18)

The total system energy $E_{\text {total}}$ over the mission duration T is defined as the combined sum of these two components:

$E_{\text {total}}=\int_0^T P_{\text {prop}}(t) d t+k_s \int_0^T\left(\delta_r^2(t)+\delta_s^2(t)\right) d t$        (19)

where, $k_s$ is a conversion weighting factor relating the mechanical steering deflection to equivalent energy. This combined index accurately reflects AUV propulsion energy because it simultaneously penalizes the power wasted by excessive rudder chattering and the exponential battery drain caused by maintaining high relative velocities against severe ocean currents.

2.5 Control structure

The overall control architecture is designed as a hierarchical closed-loop structure, illustrating the interaction between the FTESO and the MPC controller, as depicted in Figure 2. The measured position and attitude vector η_meas from the AUV's sensors is fed back to the FTESO to simultaneously estimate the spatial velocity vector $\hat{v}$ and the lumped disturbance vector $\hat{d} \in R^6$. The fused state information $\hat{x}=\left[\eta_{\text {meas}}^T, \hat{v}^T\right]^T$ is then provided to the MPC as the current state. The novelty of this structure lies in the routing flow of the signal $\hat{d}$. It is not only embedded into the internal prediction model for feed-forward disturbance compensation, but also directly fed into the cost function tuning module to update the weighting matrices $Q_v(\hat{d})$ and $R(\hat{d})$ online.

Figure 2. Overall block diagram of the proposed Finite-Time Extended State Observer and Model Predictive Control (FTESO-MPC)

Based on this continuously reconfigured optimization problem, the SQP solver computes the optimal control force and moment vector τ. Finally, the signal τ is applied to the vehicle's actuators and simultaneously fed back to the FTESO to accurately isolate the active control effort from environmental disturbances, thereby completely closing the operation cycle of the system.

This paper presents a novel FTESO-MPC framework for 6-DOF underactuated AUV. Based on the evaluated numerical simulations, the proposed method demonstrates a good trade-off between trajectory tracking accuracy and energy conservation under complex ocean currents. By providing finite-time disturbance estimation, the FTESO substantially reduces the phase lag commonly observed in traditional linear observers. Integrating these estimations into a dynamically adaptive cost function empowers the controller to proactively yield to severe head-on resistance, thereby preventing actuator saturation. Consequently, under the tested simulation scenarios, the proposed method significantly reduces trajectory tracking errors and conserves up to 41.67% of the total system energy compared to the standard MPC.

While these simulation results are promising, the current method requires further development before practical engineering deployment. The present framework assumes idealized conditions and must be expanded to handle real-world complexities. Therefore, clear next steps for future research include addressing sensor noise, parameter uncertainty, and complex actuator dynamics. Furthermore, achieving and validating the real-time implementation of this algorithm through Hardware-in-the-Loop testing remains a critical prerequisite prior to conducting full-scale sea trials.