An Experimental Study on Trajectory Tracking Control of Torpedo-like AUVs Using Coupled Error Dynamics

Abstract

In this paper, we propose a trajectory tracking controller with experimental verification for torpedo-like autonomous underwater vehicles (AUVs) with underactuation characteristics. The proposed controller overcomes the underactuation problem by designing the desired error dynamics in a coupled form using state variables in body-fixed and world coordinates. Unlike the back-stepping control requiring high-order derivatives of state variables, the proposed controller only requires the first derivatives of the states, which can alleviate noise magnification issues due to differentiation. We adopt time delay estimation to estimate the dynamics indirectly using control inputs and vehicle outputs, making the proposed controller relatively easy to apply without requiring the all of the vehicle dynamics. We also address some practical issues that commonly arise in experimental environments: handling measurement noises and actuation limits. To mitigate the effects of noise on the controller, a filtering technique using a moving window average is employed. Additionally, to account for the actuation limits, we design an anti-windup structure that takes into consideration the nonlinearity between the thrusting force and rotating speed of the thruster. We verify the tracking performance of the proposed controller through experimentation using an AUV. The experimental results show that the 3D motion control of the proposed controller exhibits an RMS error of 0.3216 m and demonstrate that the proposed controller achieves accurate tracking performance, making it suitable for survey missions that require tracking errors of less than one meter.

1. Introduction

Designing a trajectory tracking controller for torpedo-like autonomous underwater vehicles (AUVs) is challenging due to their underactuation characteristics. These vehicles have only three control inputs—surge force, pitch, and yaw moment—to control their 3D motion in space. The lack of control inputs leads to dissatisfaction of the matching condition, where certain uncertain terms in the state equation cannot be directly compensated for by the control inputs [1,2]. Additionally, the vehicles have nonlinear dynamics involving both rigid body dynamics and hydrodynamics [2]. In the development of AUVs and their control systems, it is crucial to verify their performance in experimental environments. During such verification, numerous issues can affect the system’s performance, including sensor measurement noise, modeling errors in system dynamics, disturbances, and imperfections in control systems, such as jitter in the sampling time. As a result, developing a trajectory tracking controller for AUVs is challenging in two aspects: the design of the control algorithm and the experimental verification process.

Regarding the controller design manner, there have been several research works to propose trajectory tracking control scheme for AUVs. To overcome the matching condition issues of the vehicles, several research works have proposed control schemes based on back-stepping control (BC). BC using the vehicle dynamic model was proposed in [3,4,5,6]. BC with time delay estimation (BCTDE), an indirect estimator of the vehicle dynamics, has also been studied [2,7,8]. BC provides an excellent and systematic method to handle matching condition issues. However, it requires high-order derivatives of the state variables, which may result in instability in experimental environments due to the magnification of noise effects. There have been several studies on controlling the vehicles using other schemes. Hierarchical design of the controllers has been researched to address the underactuation characteristics [9,10]. Sliding mode control has been applied to gain robustness against model errors and disturbances [11,12,13,14]. Adaptive schemes have been employed to resolve model uncertainty [15,16]. Neural networks have been used for robust path following [17,18].

Regarding the experimental verification manner, however, it is hard to find previous research works proposing trajectory tracking algorithms with experimental results. For example, the aforementioned previous research works primarily demonstrate control performance with simulation results and lack experimental verification. The reasons for the absence of experiments in previous works are not explicitly mentioned, but this could be attributed to the need for further investigation to address practical issues such as sensor noises or modeling errors in AUV dynamics, including disturbances. For example, the authors have attempted to verify the performance of the BCTDE [2,7,8] and found it difficult to determine stable gains for the BCTDE. Figure 1 illustrates the experimental results for depth control using the BCTDE, which indicate unstable responses. This instability arises because the BCTDE requires high-order differentiation of the state variables to handle unmatched dynamics and disturbances, thereby amplifying the effects of noise in the state measurements. As another reason for the lack of experimentation, the difficulty of obtaining a suitable experimental platform can be considered, as AUVs are costly plaforms. In constrast to the trajectory tracking problem, however, there have been several research works that proposed via point tracking control of underactuated AUVs with experimental verification [19,20,21]. These research works utilize traditional approaches employing PID-type controllers for the forward velocity, pitch angle (or depth), and heading angle, combined with a desired heading angle planner such as the line of sight (LOS) [22,23,24]. Designing via point tracking controllers is relatively straightforward since each controller focuses only on stabilizing states with dynamics matched to the control inputs. However, they mainly focus on waypoint tracking and are unable to handle time-varying trajectory tracking problems. There have also been research works suggesting trajectory tracking controllers for underwater vehicles with full degree-of-freedom (DOF) actuation [25,26,27,28,29]. In such cases, the vehicle can generate the control actuations for every controlled state, eliminating issues arising from a lack of satisfaction of the matching condition.

Figure 1. Experimental results for depth control using the BCTDE. The results indicate an almost unstable response due to the high–order differentiation of states required by the controller, which amplifies the noise effect in the state measurements.

In this paper, we propose a robust trajectory tracking controller for the 3D motion of underwater vehicles, along with experimental verification of its control performance. In terms of controller design, the proposed controller incorporates an appropriate design of the desired error dynamics and time delay estimation (TDE). To address the underactuation issues of the vehicle, the desired error dynamics is formulated in a coupled form between the state variables in body-fixed and world coordinates. By utilizing the TDE [2,30], the controller effectively compensates for the nonlinear dynamics and disturbances of the vehicle while maintaining a simple structure. The proposed controller only requires the state variables and their first derivatives, mitigating the issues of noise amplification due to differentiation. An initial version of the proposed controller was presented in [31], and in this paper, we extend the controller for motion control in a 3D space. In terms of experimental implementation, practical issues related to measurement noise and actuation limitations are addressed. A moving average filter is employed to mitigate the effects of sensor noise on control performance, and actuation limitations combined with nonlinear dynamics are also considered. We verify the tracking performance of the proposed controller through experiments conducted on an AUV.

2. Controller Design for the AUV

2.1. AUV Systems and Motion-Governing Equations

Figure 2 illustrates the AUV platform utilized in this research, which was developed by Hanwha Systems [32]. The AUV serves as a testbed for underwater docking tasks [33]. The linear velocities of the vehicle are measured using a Doppler velocity log (DVL), and the angular velocities are measured using an inertial measurement unit (IMU). The position of the vehicle in the world coordinate is estimated using an extended Kalman filter (EKF) that utilizes navigation sensor data from an IMU, a DVL, a depth sensor, a digital compass, and a global navigation satellite system (GNSS) [34,35,36]. The vehicle is equipped with a thruster for forward propulsion, as well as rudder fins and stern fins for lateral and vertical moments, respectively. The actuators are controlled by an ARM-based embedded system with a control sampling frequency of 10 Hz.

Figure 2. AUV used in the experiment. The vehicle was developed by Hanwha Systems [32]. The AUV serves as a testbed for underwater docking tasks [33].

To formulate the motion-governing equation, let us consider the control problem of the vehicle in a 3D space as shown in Figure 3. Assuming that the roll motion of the vehicle can be neglected, the governing equations for motion are given as follows [8,37]:

$$\begin{array}{cc}\hfill \dot{\mathit{\eta }}& =\mathbf{R}\mathbf{\nu }\hfill \\ \hfill \dot{\theta }& =q,\hfill \\ \hfill \dot{\psi }& =r/c\theta ,and,\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill m_{11}\dot{u}-m_{22}vr+m_{33}wq+f_u\left(u\right)u+{\tau }_{eu}& ={\tau }_u,\hfill \\ \hfill m_{22}\dot{v}+m_{11}ur+f_v\left(v\right)v+m_{22}{\tau }_{ev}& =0,\hfill \\ \hfill m_{33}\dot{w}-m_{11}uq+f_w\left(w\right)w-d_1+{\tau }_{ew}& =0,\hfill \\ \hfill m_{55}\dot{q}-(m_{33}-m_{11})uw+f_q\left(q\right)q+(d_2+{\tau }_{eq})& ={\tau }_q,\hfill \\ \hfill m_{66}\dot{r}-(m_{11}-m_{22})uv+f_r\left(r\right)r+{\tau }_{er}& ={\tau }_r,\hfill \end{array}$$

(2)

where $c•$ and $s•$ denote $cos(•)$ and $sin(•)$, respectively; $\mathit{\eta }={\left[x,y,z\right]}^T$; $\mathbf{\nu }={[u,v,w]}^T$; x, y, z, $\theta$, and $\psi$ are the positions and orientations of the vehicle in the world coordinate; u, v, w, q, and r are the translational velocities and angular velocities; ${\tau }_u$, ${\tau }_q$, and ${\tau }_r$ are the control inputs; ${\tau }_{eu}$, ${\tau }_{ev}$, ${\tau }_{ew}$, ${\tau }_{eq}$, ${\tau }_{eu}$, and ${\tau }_{er}$ are the bounded external disturbances, such as ocean currents and waves; $m_{ii}(i=1,2,3,5,6)$ represent the terms for the combined mass and intertia parameters; $d_1=(W-B)c\theta$; $d_2=(z_{g}W-z_{b}B)s\theta$; W is the gravity, B is the buoyancy of the vehicle; $f_k\left(k\right)(k=u,v,w,q,r)$ represents the hydrodynamic damping and friction terms; and $\mathbf{R}\equiv {\mathbf{R}}_z\left(\psi \right){\mathbf{R}}_y\left(\theta \right)$ is the rotation matrix of $\left\{B\right\}$ with respect to $\left\{W\right\}$. From Equation (2), the dynamics of the controllable states are rearranged as follows:

$$\begin{array}{c}\hfill {\overline{m}}_u\dot{u}+h_u={\tau }_u,\\ \hfill {\overline{m}}_q\dot{q}+h_q={\tau }_q,\\ \hfill {\overline{m}}_r\dot{r}+h_r={\tau }_r,\end{array}$$

(3)

where ${\overline{m}}_u$, ${\overline{m}}_q$, and ${\overline{m}}_r$ are the positive constants which represent the known ranges of inertia and $h_u$, $h_q$, and $h_r$ are nonlinear terms, defined as

$$\begin{array}{cc}\hfill h_u& \equiv (m_{11}-{\overline{m}}_u)\dot{u}-m_{22}vr+m_{33}wq+f_u\left(u\right)u+{\tau }_{eu},\hfill \\ \hfill h_q& \equiv (m_{55}-{\overline{m}}_q)\dot{q}-(m_{33}-m_{11})uw+f_q\left(q\right)q+(d_2+{\tau }_{eq}),\hfill \\ \hfill h_r& \equiv (m_{66}-{\overline{m}}_r)\dot{r}-(m_{11}-m_{22})uv+f_r\left(r\right)r+{\tau }_{er}.\hfill \end{array}$$

(4)

Figure 3. Definition of coordinates.

2.2. Desired Trajectory

Due to the underactuated nature, only three trajectory variables can be designed independently. Note that in Equation (2), there are only three independent control inputs to control five DOFs in the world coordinate. We can set the independent trajectory variables as x, y, and z in $\left\{W\right\}$. The trajectories can be represented by the following continuous time functions [8]:

$${\mathit{\eta }}_d={[x_d\left(t\right),y_d\left(t\right),z_d\left(t\right)]}^T.$$

(5)

Note that, when taking into account Equation (1), the angular and velocity trajectories according to Equation (5) satisfy the following relationship [8]:

$$\begin{array}{cc}\hfill {\theta }_d& =-si{n}^{-1}\left({\dot{z}}_d/\sqrt{{\dot{x}}_d^2+{\dot{y}}_d^2+{\dot{z}}_d^2}\right),\hfill \\ \hfill {\psi }_d& =atan2({\dot{y}}_d,{\dot{x}}_d),and,\hfill \\ \hfill u_d& =\sqrt{{\dot{x}}_d^2+{\dot{y}}_d^2+{\dot{z}}_d^2},\hfill \\ \hfill q_d& ={\dot{\theta }}_d,\hfill \\ \hfill r_d& ={\dot{\psi }}_{d}c{\theta }_d.\hfill \end{array}$$

(6)

The goal of this paper is to design a controller for an AUV system represented by Equations (1) and (3) to track the desired trajectories defined in Equations (5) and (6). The controller is specifically designed to reduce the tracking error along ${\mathit{\eta }}_d$ by using control inputs.

2.3. Dynamics of Tracking Error

In this subsection, we arrange the tracking error dynamics of the vehicle by describing them in the desired trajectory coordinate $\left\{D\right\}$. This approach helps to minimize changes in the relationship between variables in different coordinates [38,39]. When there is no tracking error, u affects ${}^{D}x$ for every attitude of the vehicle, and the transformation can be easily achieved as follows [40]:

$$\begin{array}{cc}\hfill & {}^D\mathit{\eta }={\mathbf{R}}_d^T\mathit{\eta }-{\mathbf{R}}_d^T{\mathit{\eta }}_d,\hfill \end{array}$$

(7)

where ${}^D\mathit{\eta }={\left[{}^{D}x,{}^{D}y,{}^{D}z\right]}^T$ denotes the translational position of the vehicle with respect to $\left\{D\right\}$ and ${\mathbf{R}}_d\equiv {\mathbf{R}}_z\left({\psi }_d\right){\mathbf{R}}_y\left({\theta }_d\right)$. Using Equation (7), the desired trajectories in Equation (5) can be transformed into those in $\left\{D\right\}$ as follows:

$${}^D{\mathit{\eta }}_d=\mathbf{0}.$$

(8)

Subtracting Equation (7) from Equaiton (8) yields the following relationship:

$$\begin{array}{c}\hfill {}^D{\mathit{\eta }}_e={\mathbf{R}}_d^T{\mathit{\eta }}_e,\end{array}$$

(9)

where ${•}_e\equiv {•}_d-•$. Note that from Equation (9), the tracking problem in $\left\{D\right\}$ is identical to that in $\left\{W\right\}$. Convergence of the tracking error in $\left\{D\right\}$ guarantees convergence in $\left\{W\right\}$. This is because ${\mathbf{R}}_d$ is a rotation matrix and cannot be singular. By taking the derivatives of Equation (9) with Equations (1) and (5) and introducing positive constants ${\overline{\alpha }}_u$, ${\overline{\alpha }}_{\psi }$, and ${\overline{\alpha }}_{\theta }$, the error dynamics of the state variable in $\left\{D\right\}$ can be rearranged as follows [8]:

$$\begin{array}{c}\hfill \left[\begin{array}{c}{}^D{\dot{x}}_e\\ {}^D{\dot{y}}_e\\ {}^D{\dot{z}}_e\end{array}\right]=\left[\begin{array}{c}{\overline{\alpha }}_u{u}_e\\ {\overline{\alpha }}_{\psi }{\psi }_e\\ -{\overline{\alpha }}_{\theta }{\theta }_e\end{array}\right]+\left[\begin{array}{c}{\lambda }_x\\ {\lambda }_y\\ {\lambda }_z\end{array}\right].\end{array}$$

(10)

Refer to [8] for a detailed derivation of Equation (10). The last term in the above equation represents the nonlinear terms defined as follows:

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\lambda }_x\\ {\lambda }_y\\ {\lambda }_z\end{array}\right]=-\left[\begin{array}{c}{\overline{\alpha }}_u{u}_e\\ {\overline{\alpha }}_{\psi }{\psi }_e\\ -{\overline{\alpha }}_{\theta }{\theta }_e\end{array}\right]+{\mathbf{\nu }}_d-{\mathbf{R}}_d^T\mathbf{R}\mathit{\nu }-{\mathit{\omega }}_d^{\times }{}^D{\mathit{\eta }}_e,\end{array}$$

(11)

where ${\mathit{\nu }}_d={\left[u_d,0,0\right]}^T$ and ${\mathit{\omega }}_d^{\times }$ is a skew-symmetric matrix of ${\mathit{\omega }}_d\equiv {[0,q_d,r_d]}^T$ [8]. Aside from that, from Equations (1) and (6), the attitude error is obtained as follows:

$$\begin{array}{cc}\hfill {\dot{\theta }}_e& =q_e,\hfill \\ \hfill {\dot{\psi }}_e& ={\overline{\alpha }}_r{r}_e+{\lambda }_{\psi },\hfill \end{array}$$

(12)

where ${\overline{\alpha }}_r$ is a positive constant and

$$\begin{array}{cc}\hfill {\lambda }_{\psi }& =-{\overline{\alpha }}_r{r}_e+r_d/c{\theta }_d-r/c\theta .\hfill \end{array}$$

(13)

As a result, the tracking error dynamics are expressed in Equations (10) and (12). The objective of this paper is to design a control input in Equation (3) to stabilize the error dynamics, particularly ${}^D{x}_e$, ${}^D{y}_e$, and ${}^D{z}_e$.

2.4. Controller Design Using the Coupled Error Dynamics and Time Delay Estimation

In this paper, our goal is to design the desired error dynamics for $u_e$, $r_e$, and $q_e$ that can asymptotically stabilize the tracking errors of ${}^D{x}_e$, ${}^D{y}_e$, and ${}^D{z}_e$, respectively. Note that from Equation (3), one can directly control the variables in $\left\{B\right\}$, namely u, q, and r, by designing appropriate control inputs ${\tau }_u$, ${\tau }_q$, and ${\tau }_r$, respectively. From Equations (10) and (12), u adjusts ${}^D{x}_e$, q affects ${\theta }_e$ and consequently ${}^D{z}_e$, and r determines ${\psi }_e$ and therefore ${}^D{y}_e$. Thus, coupled error dynamics between the variables in $\left\{B\right\}$ and $\left\{D\right\}$ can be designed to stabilize the variables in $\left\{D\right\}$. The desired error dynamics of $u_e$, $r_e$, and $q_e$ are designed as follows:

$$\begin{array}{cc}\hfill {\dot{u}}_e+K_u{u}_e+K_x{}^D{x}_e& =0,\hfill \\ \hfill {\dot{q}}_e+K_q{q}_e+K_{\theta }{\theta }_e-K_z{}^D{z}_e& =0,\hfill \\ \hfill {\dot{r}}_e+K_r{r}_e+K_{\psi }{\psi }_e+K_y{}^D{y}_e& =0,\hfill \end{array}$$

(14)

where $K_{•}>0$ represents the control gains.

In the manner of the computed torque control, the controller for Equation (3) can be designed as follows:

$$\begin{array}{cc}\hfill {\tau }_u=& {\widehat{h}}_u+{\overline{m}}_u{\mu }_u,\hfill \\ \hfill {\tau }_v=& {\widehat{h}}_q+{\overline{m}}_q{\mu }_q,\hfill \\ \hfill {\tau }_r=& {\widehat{h}}_r+{\overline{m}}_r{\mu }_r,\hfill \end{array}$$

(15)

where $\widehat{•}$ denotes the estimate of • and ${\mu }_u$, ${\mu }_q$, and ${\mu }_r$ are the command inputs to insert the desired dynamics for u, q, and r, respectively. When $\widehat{•}=•$, the controlled dynamics is obtained from Equations (3) and (15) as follows:

$$\begin{array}{c}\hfill {\mu }_u-\dot{u}=0,\\ \hfill {\mu }_q-\dot{q}=0,\\ \hfill {\mu }_r-\dot{r}=0.\end{array}$$

(16)

To induce the desired error dynamics in Equations (14)–(16), the command inputs are designed as follows:

$$\begin{array}{cc}\hfill {\mu }_u=& {\dot{u}}_d+K_u{u}_e+K_{xp}{}^D{x}_e,\hfill \\ \hfill {\mu }_q=& {\dot{q}}_d+K_q{q}_e-K_{\theta }{\theta }_e-K_{zp}{}^D{z}_e,\hfill \\ \hfill {\mu }_r=& {\dot{r}}_d+K_r{r}_e+K_{\psi }{\psi }_e+K_{yp}{}^D{y}_e.\hfill \end{array}$$

(17)

To implement the controller in Equation (15), it is necessary to obtain ${\widehat{h}}_u$, ${\widehat{h}}_q$, and ${\widehat{h}}_r$, the estimates of Equation (4). However, obtaining an exact dynamic model of Equation (4) is difficult and time-consuming. To address this, we employ the TDE [2,30,41,42] for robust and efficient estimation. The key idea behind TDE is that if the system dynamics are given as a continuous or piece-wise continuous function, then the variation in the dynamics during a very short time can be negligible. Thus, the value of the dynamics at the current time can be estimated by using the value of the dynamics at a short time before. Based on this idea, the system dynamics can be estimated indirectly by utilizing previous information on the system input and output. From Equation (3), the dynamics can be estimated as follows:

$$\begin{array}{cc}\hfill {\widehat{h}}_{u\left(t\right)}& =h_{u(t-L)}={\tau }_{u(t-L)}-{\overline{m}}_u{\dot{u}}_{(t-L)},\hfill \\ \hfill {\widehat{h}}_{q\left(t\right)}& =h_{q(t-L)}={\tau }_{q(t-L)}-{\overline{m}}_q{\dot{q}}_{(t-L)},\hfill \\ \hfill {\widehat{h}}_{r\left(t\right)}& =h_{r(t-L)}={\tau }_{r(t-L)}-{\overline{m}}_r{\dot{r}}_{(t-L)},\hfill \end{array}$$

(18)

where L denotes a short time delay which is commonly set as the sampling time of the control system. As a result, the final form of the proposed controller is Equation (15) with Equations (17) and (18).

It is noteworthy that the proposed controller in Equation (15) with Equations (17) and (18) only requires the first derivative of the state variables and the states themselves. The linear and angular velocities can be measured using IMU and DVL, while the vehicle’s position can be obtained through a navigation algorithm such as a Kalman filter, utilizing sensors such as IMU, DVL, the depth sensor, and the digital compass [34]. The advantage of not requiring high-order differentiation of the states is that it helps stabilize the controller in experimental environments. This is because differentiating the states amplifies the noise effect present in the state measurements. In comparison, the BCTDE [2,7,8] necessitates third-order differentiation of the states. Therefore, one can expect that the proposed controller is relatively easier to stabilize in experimental environments. In addition, note that the TDE method does not require the entire vehicle dynamics model. One can design the proposed controller by only selecting the inertial gains, such as ${\overline{m}}_u$, ${\overline{m}}_q$, ${\overline{m}}_r$, ${\overline{\alpha }}_u$, ${\overline{\alpha }}_{\psi }$, ${\overline{\alpha }}_{\theta }$, and ${\overline{\alpha }}_r$, as well as the feedback gains, such as $K_x$, $K_u$, $K_y$, $K_{\psi }$, $K_r$, $K_z$, $K_{\theta }$, and $K_q$.

2.5. Error Dynamics of the Proposed Controller

The TDE provides an efficient way to estimate the nonlinear dynamics of the vehicle, but it cannot estimate the dynamics variation exactly during a sampling time L. Thus, an estimation error of the dynamics remains. Taking into account the estimation error of the TDE, one can rearrange the error dynamics in Equaiton (14) by utilizing Equations (3) and (15) with Equations (17) and (18) as follows:

$$\begin{array}{cc}\hfill {\dot{u}}_e+K_u{u}_e+K_x{}^D{x}_e& ={ϵ}_u,\hfill \\ \hfill {\dot{q}}_e+K_q{q}_e+K_{\theta }{\theta }_e-K_z{}^D{z}_e& ={ϵ}_q,\hfill \\ \hfill {\dot{r}}_e+K_r{r}_e+K_{\psi }{\psi }_e+K_y{}^D{y}_e& ={ϵ}_r,\hfill \end{array}$$

(19)

where ${ϵ}_{•}$ denotes the TDE errors, which are defined as follows:

$$\begin{array}{cc}\hfill {ϵ}_u& \equiv {\overline{m}}_u^{-1}\left(h_{u\left(t\right)}-h_{u(t-L)}\right),\hfill \\ \hfill {ϵ}_q& \equiv {\overline{m}}_q^{-1}\left(h_{q\left(t\right)}-h_{q(t-L)}\right),\hfill \\ \hfill {ϵ}_r& \equiv {\overline{m}}_r^{-1}\left(h_{r\left(t\right)}-h_{r(t-L)}\right).\hfill \end{array}$$

(20)

From Equations (10), (12), and (19), the error dynamics of the controlled system for the forward, lateral, and vertical directions, respectively, are as follows:

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{}^D{\dot{x}}_e\\ {\dot{u}}_e\end{array}\right]& =\underset{{\mathbf{A}}_x}{\underbrace{\left[\begin{array}{cc}0& {\overline{\alpha }}_u\\ -K_x& -K_u\end{array}\right]}}\left[\begin{array}{c}{}^D{x}_e\\ u_e\end{array}\right]+\underset{{\mathbf{B}}_x}{\underbrace{\left[\begin{array}{c}{\lambda }_x\\ {ϵ}_u\end{array}\right]}},\hfill \end{array}$$

(21a)

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{}^D{\dot{y}}_e\\ {\dot{\psi }}_e\\ {\dot{r}}_e\end{array}\right]& =\underset{{\mathbf{A}}_y}{\underbrace{\left[\begin{array}{ccc}0& {\overline{\alpha }}_{\psi }& 0\\ 0& 0& {\overline{\alpha }}_r\\ -K_y& -K_{\psi }& -K_r\end{array}\right]}}\left[\begin{array}{c}{}^D{y}_e\\ {\psi }_e\\ r_e\end{array}\right]+\underset{{\mathbf{B}}_y}{\underbrace{\left[\begin{array}{c}{\lambda }_y\\ {\lambda }_{\psi }\\ {ϵ}_r\end{array}\right]}},\hfill \end{array}$$

(21b)

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{}^D{\dot{z}}_e\\ {\dot{\theta }}_e\\ {\dot{q}}_e\end{array}\right]& =\underset{{\mathbf{A}}_z}{\underbrace{\left[\begin{array}{ccc}0& -{\overline{\alpha }}_{\theta }& 0\\ 0& 0& 1\\ K_z& -K_{\theta }& -K_q\end{array}\right]}}\left[\begin{array}{c}{}^D{z}_e\\ {\theta }_e\\ q_e\end{array}\right]+\underset{{\mathbf{B}}_z}{\underbrace{\left[\begin{array}{c}{\lambda }_z\\ 0\\ {ϵ}_q\end{array}\right]}}.\hfill \end{array}$$

(21c)

By taking the Laplace transform of Equations (21a)–(21c), one can examine the stability of the error dynamics and the influence of the forcing functions ${\mathbf{B}}_x$, ${\mathbf{B}}_y$, and ${\mathbf{B}}_z$. The Laplace-transformed error dynamics can be obtained as follows:

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{}^D{x}_e\\ u_e\end{array}\right]& ={\left(s\mathbf{I}-{\mathbf{A}}_x\right)}^{-1}{\mathbf{B}}_x=\frac{1}{\left|s\mathbf{I}-{\mathbf{A}}_x\right|}\left[\begin{array}{cc}s+K_u& {\overline{\alpha }}_u\\ -K_x& s\end{array}\right]\left[\begin{array}{c}{\lambda }_x\\ {ϵ}_u\end{array}\right],\hfill \end{array}$$

(22a)

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{}^D{y}_e\\ {\psi }_e\\ r_e\end{array}\right]& =\frac{1}{\left|s\mathbf{I}-{\mathbf{A}}_y\right|}\left[\begin{array}{ccc}{s}^2+K_{r}s+{\overline{\alpha }}_r{K}_{\psi }& {\overline{\alpha }}_{\psi }s+{\overline{\alpha }}_{\psi }{K}_r& {\overline{\alpha }}_{\psi }{\overline{\alpha }}_r\\ -{\overline{\alpha }}_r{K}_y& s^2+K_{r}s& {\overline{\alpha }}_{r}s\\ K_{y}s& -K_{\psi }s-{\overline{\alpha }}_{\psi }{K}_y& s^2\end{array}\right]\left[\begin{array}{c}{\lambda }_y\\ {\lambda }_{\psi }\\ {ϵ}_r\end{array}\right],\hfill \end{array}$$

(22b)

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{}^D{z}_e\\ {\theta }_e\\ q_e\end{array}\right]& =\frac{1}{\left|s\mathbf{I}-{\mathbf{A}}_z\right|}\left[\begin{array}{cc}{s}^2+K_{q}s+K_{\theta }& -{\overline{\alpha }}_{\theta }\\ K_z& s\\ K_{z}s& s^2\end{array}\right]\left[\begin{array}{c}{\lambda }_z\\ {ϵ}_q\end{array}\right],\hfill \end{array}$$

(22c)

where s denotes the Laplace operator and

$$\begin{array}{cc}\hfill \left|s\mathbf{I}-{\mathbf{A}}_x\right|& =s^2+K_{u}s+{\overline{\alpha }}_u{K}_x,\hfill \end{array}$$

(23a)

$$\begin{array}{cc}\hfill \left|s\mathbf{I}-{\mathbf{A}}_y\right|& =s^3+K_r{s}^2+{\overline{\alpha }}_r{K}_{\psi }s+{\overline{\alpha }}_{\psi }{\overline{\alpha }}_r{K}_y,\hfill \end{array}$$

(23b)

$$\begin{array}{cc}\hfill \left|s\mathbf{I}-{\mathbf{A}}_z\right|& =s^3+K_q{s}^2+K_{\theta }s+{\overline{\alpha }}_{\theta }{K}_y.\hfill \end{array}$$

(23c)

From Equations (22a)–(22c), the influence of each term of the forcing functions ${\mathbf{B}}_x$, ${\mathbf{B}}_y$, and ${\mathbf{B}}_z$ can be estimated. The tracking errors in a steady state can be analyzed by using the final value theorem of the Laplace transform: ${lim}_{t\to \infty }x\left(t\right)={lim}_{s\to 0}sx\left(s\right)$. For example, from Equations (22a) and (23a), ${}^D{x}_e\left(s\right)/{\lambda }_x\left(s\right)=(s+K_u)/(s^2+K_{u}s+{\overline{\alpha }}_u{K}_x)$. Assume that ${\lambda }_x\left(s\right)$ is given as a step function: ${\lambda }_x\left(s\right)=l/s$ with a constant l. Then, ${}^D{x}_e{\left(t\right)|}_{t\to \infty }=K_{u}l/\left({\overline{\alpha }}_u{K}_x\right)$, and one can estimate that in the forward direction error, ${}^D{x}_e\left(t\right)$, there will be a steady state error dependent on the amount of disturbed dynamics l and the control gains ${\overline{\alpha }}_u$, $K_u$, and $K_x$. The controller cannot perfectly compensate for the influence of the forcing function, but it can attenuate the influence by selecting appropriate control gains.

The characteristic equations in Equations (23a)–(23c) are useful for selecting appropriate control gains. In order to ensure stable error dynamics, the characteristic equations must satisfy the Hurwitz condition, and the gains $K_{•}$ must be chosen accordingly. Additionally, the inertial gains ${\overline{m}}_u$, ${\overline{m}}_q$, ${\overline{m}}_r$, ${\overline{\alpha }}_u$, ${\overline{\alpha }}_{\psi }$, ${\overline{\alpha }}_{\theta }$, and ${\overline{\alpha }}_r$ can be obtained through tuning. Previous research works utilizing the TDE have suggested selecting inertial gains within a known range of the vehicle’s inertial terms. If the vehicle model is unknown, however, then the inertial gains can be obtained through tuning [2,30].

3. Practical Issues for the Experiments

When setting up the controller for the experiment, practical issues such as the noise effects of the measurements and actuator limitations have to be considered. These issues will be discussed in the following subsections.

3.1. Handling the Noise Effect in the TDE

The TDE provides an effective and efficient method for estimating the nonlinear dynamics of vehicles. However, the use of state derivatives in the TDE amplifies the noise effect in the measurement of the state variables. In the case of underwater vehicles, the vehicle’s position is usually estimated by Kalman filtering of the sensor data, such as acceleration from the IMU and velocity from the DVL, which have considerable measurement noise. The amplification of noise can undermine the stability conditions. One way to handle this is to use the low-pass filtering effects of the inertial gains of the TDE [30]. Decreasing the inertial gains shows a similar effect to low-pass filtering. However, we found experimentally that adjusting the inertial gains was not enough. Therefore, we devised a method to attenuate the noise effect of the TDE. The idea is quite simple: cut down the direct TDE value and supplement the remaining part with the averaged value of the TDE. Figure 4 shows the noise attenuation method in the TDE. Note that the use of an average filter in Figure 4 can attenuate the noise effect because the filter also averages the noise. However, the filter may slightly degrade the performance of the TDE because the filtered value of the TDE cannot estimate exactly any quick changes in vehicle dynamics. In the case of underwater vehicles, dynamic changes occur due to the vehicle dynamics and disturbance changes such as sea currents, which depend on the mission (or desired trajectory) and the environment. In the case of AUVs, dynamic changes may not be fast because they are commonly used for surveys of large areas, and disturbances such as sea currents change slowly according to tide variation.

Figure 4. Filtering the TDE to reduce the noise effect. The algorithm reduces the direct TDE value and supplements the remaining part with the averaged value of the TDE. This mitigates the noise present in the TDE thanks to the averaging effect.

3.2. Handling Nonlinearity and the Limits of the Actuators

When designing a controller, it is important to consider the actuator characteristics, such as nonlinearity and the actuation limits. For instance, the thruster of a vehicle exhibits nonlinear dynamics between the propulsion force and rotation speed of the thruster. Moreover, the thruster has limits on rotational velocity and acceleration because the thruster is a mechanical system. In this subsection, we address compensation methods for the actuator characteristics, focusing on the thrusters in particular and briefly touching on the rudder fins and stern fins. The thruster dynamics are as follows [23,43]:

$$\begin{array}{cc}\hfill {\tau }_u& =T_{\left|n\right|n}\left|n\right|n+T_{\left|n\right|u}\left|n\right|u,\hfill \end{array}$$

(24)

where n denotes the rotation velocity of the thruster and $T_{\left|n\right|n}$ and $T_{\left|n\right|u}$ represent the actuator coefficients corresponding to the rotational speed of the actuator and fluid speed around the actuator, respectively. By ignoring $T_{\left|n\right|u}\left|n\right|u$ and $T_{\left|n\right|n}$ from Equation (24), we adopted a simple thruster model, which is as follows:

$${\tau }_u^{\prime }=\left|n\right|n.$$

(25)

This is because the effect of $T_{\left|n\right|u}\left|n\right|u$, the term included in the RHS of Equation (24), can be compensated for by the feedback loop of the controller, and the scale coefficient $T_{\left|n\right|n}$ in Equation (24) can be adjusted by tuning the controller gain. Note that when substituting Equation (24) into the first equation of Equation (3), $T_{\left|n\right|u}\left|n\right|u$ can be treated as part of the nonlinear term $h_u$ (i.e., $h_u^{\prime }=h_u-T_{\left|n\right|u}\left|n\right|u$). The coefficient $T_{\left|n\right|n}$ simply scales the value of ${\overline{m}}_u$ (i.e., ${\overline{m}}_u^{\prime }={\overline{m}}_u/T_{\left|n\right|n}$).

Regarding the actuation limit of the thrusting force, we considered the limits on the rotational velocity and acceleration of the thrusting propeller as follows:

$$\underline{n}=\left\{\begin{array}{cc}n,\hfill & when\left|n\right|<n_{max},\left|\dot{n}\right|<{\dot{n}}_{max},n\ge 0,\hfill \\ f\left(n\right),\hfill & when any of above conditions is not satisfied,\hfill \end{array}\right.$$

(26)

where $\underline{n}$ represents n with the actuation limit and $f\left(n\right)$ is a limiting function that considers the limit of n and $\dot{n}$ as well as the sign of n. Note that in Equation (26), the sign condition $n\ge 0$ is included because only the forward thrusting force is available. In the case of the controllers using the TDE, handling the actuation limit to prevent the wind-up phenomenon is as straightforward as incorporating a limit block to ensure that the calculated control input for the TDE matches the actual value of the actuation applied to the vehicle [44]. Figure 5 shows the limiter block for handling the actuation limit of the thrusting force, while Figure 6 shows the structure of the limiter block, which is explained by Equation (26). In the case of the rudder fins and stern fins, one can handle the actuation limit in similar ways to the case of the thrusting force. The only differences are that (1) the actuation forces are linear with the fin motions [23] and (2) both positive and negative forces are available. By simply removing the condition $n\ge 0$ in Equation (26) and eliminating the blocks of sqrt() and square() in Figure 5 and Figure 6, the same limiting algorithm can be applied to the rudder fins and the stern fins.

Figure 5. TDE feedback block for $h_u$ of the controller. $Sgn(•)$ denotes a signum function, and $Sqrt(•)\equiv \sqrt{•}$. The ‘Limiter’ block is included to prevent the wind-up phenomenon due to the actuation limit [44]. The ‘$Sgn(•)Sqrt\left(\right|•\left|\right)$’ block is for compensating for the actuation dynamics in Equation (25).

Figure 6. Limiter block of the thrusting force ${\tau }_u$: The thruster has actuation limits on the rotation speed and acceleration. Therefore, before checking the limitation, the ‘$Sgn(•)Sqrt\left(\right|•\left|\right)$’ block is included to convert the control force ${\tau }_u$ into the rotation speed according to Equation (25).

4. Experimental Study

The tracking performance of the proposed controller was experimentally verified using the AUV platform depicted in Figure 2. The experiments were performed in the seawater at a port located in the South Sea of Korea. For the experiment, the control gains were set as follows. The inertial gains were set to ${\overline{m}}_u=3000$, ${\overline{m}}_q=0.7$, ${\overline{m}}_r=1.0$, ${\overline{\alpha }}_u=1.0$, ${\overline{\alpha }}_{\psi }=1.5$, ${\overline{\alpha }}_{\theta }=1.5$, and ${\overline{\alpha }}_r=1.5$ by tuning, and the feedback gains $K_x=1.0$, $K_u=2.0$, $K_y=0.216$, $K_{\psi }=1.08$, $K_r=1.8$, $K_z=0.125$, $K_{\theta }=0.75$, and $K_q=1.5$ were selected for the desired error dynamics having poles at $p_{dx}=-1.0$ (double poles), $p_{dy}=-0.6$ (triple poles), and $p_{dz}=-0.5$ (triple poles). In this case, the characteristic equations in Equations (23a)–(23c) had poles at $p_{cx}=-1.0$ (double poles), $p_{cy}=-0.502,-0.649\pm 0.740i$, and $p_{cz}=-0.897,-0.302\pm 0.344i$, which were placed in the LHP. Regarding the noise-handling algorithm in Figure 4, the window size for the average filter was set at $N=128$, and the $\beta$ values for ${\widehat{h}}_u$, ${\widehat{h}}_q$, and ${\widehat{h}}_r$ in Equation (18) were ${\beta }_u=0.7$, ${\beta }_q=0.5$, and ${\beta }_r=0.9$, respectively.

4.1. Experimental Verification of the Noise-Handling Issue

In this subsection, the noise handling method described in Section 3.1 is experimentally verified. The experiments were conducted on the surface of the sea, with actuations in the XY plane. The thrusters and rudder fins were activated, while the stern fins were deactivated. The tracking performances were compared between the case where the noise-handling algorithm was not applied and the case where the algorithm was applied. A trajectory involving linear motion was used, as shown in Figure 7. The experimental results are presented in Figure 7, Figure 8, Figure 9, Figure 10, Figure 11 and Figure 12. Figure 7, Figure 8 and Figure 9 show the responses for the case without the noise-handling algorithm, while Figure 10, Figure 11 and Figure 12 show the responses when the algorithm was applied. When comparing Figure 7 and Figure 10, it may be difficult to recognize a significant difference in performance between the two cases. Aside from the initial errors in Figure 10, the error bounds appear to be similar in both cases. However, when comparing Figure 9 and Figure 12, it is evident that the noise-handling algorithm was effective. In the case without the algorithm (Figure 9), the control input (rudder angle) switched frequently, resulting in erratic responses in r, $\psi$, and ${}^{D}y$. This behavior was due to the TDE in Equation (18), which included the first-order derivative of the velocity state and amplified the noise present in the state measurement. In contrast, when the noise-handling algorithm was applied (Figure 12), the control input (rudder angle) reacted smoothly to tracking errors, leading to smooth convergence in the velocity and position responses. From Figure 8 and Figure 11, it can be observed that the chatterings in the forward direction responses were alleviated when the noise-handling algorithm was adopted.

Figure 7. XY plot of the responses when noise-handling algorithm was not applied. The responses exhibited chattering behaviors due to the influence of measurement noise on the TDE.

Figure 8. Forward direction responses when noise-handling algorithm was not applied. The responses were stable; however, there was some minor chattering in the control input.

Figure 9. Lateral direction responses when noise-handling algorithm was not applied. The control input switched frequently, leading to chattering responses in r, $\psi$, and ${}^{D}y$.

Figure 10. XY plot of the responses when noise-handling algorithm in Figure 4 was applied. The responses smoothly converged to the desired trajectories even in the presence of an initial angular error.

Figure 11. Forward direction responses when noise-handling algorithm in Figure 4 was applied. The responses exhibited smoother convergence compared with the responses shown in Figure 8.

Figure 12. Lateral direction responses when noise-handling algorithm in Figure 4 was applied. The responses exhibited smoother convergence compared with the responses shown in Figure 9.

4.2. Experimental Verification of Tracking Performance in 3D Space Motion

The trajectory tracking performance of the proposed controller in 3D space motion was verified experimentally. As shown in Figure 13, the desired trajectory, drawing the shape of the number eight, was applied. The experimental results are presented in Figure 13, Figure 14, Figure 15, Figure 16 and Figure 17. Figure 13 and Figure 14 show the tracking responses in the world coordinate $\left\{W\right\}$, demonstrating that the controlled system had stable responses and followed the desired trajectory with bounded errors. The root mean square (RMS) errors in Table 1 indicate that the tracking performances were accurate enough to perform surveying missions requiring tracking errors of less than one meter. It can be observed from Table 1 that the RMS error in the vertical direction was slightly larger than those in the other directions due to buoyancy acting as a disturbance in the vertical direction. Figure 15, Figure 16 and Figure 17 show the responses and control inputs for each direction. Note that in Figure 17, the response of $\theta$ exhibited a large tracking error induced by the controller to reduce the tracking error of z. In Figure 15, it can be observed that the tracking error of ${}^{D}x$ bounced around at times t = 66 s, 87 s, and 141 s, which was caused by irregular DVL signals resulting from stiff changes in the seabed.

Figure 13. XYZ plot of the tracking responses.

Figure 14. The time responses of the positions in a Cartesian space.

Figure 15. The responses and actuation in the forward direction. ${}^{D}x$ converged to zero because, as explained in Equation (8), the desired trajectory expressed in the desired trajectory coordinate $\left\{D\right\}$ was zero. The responses of ${}^{D}x$ and u followed their desired trajectories smoothly, demonstrating the effectiveness of the noise filtering depicted in Figure 4. That aside, the tracking response of ${}^{D}x$ bounced around at times $t=66$ s, 87 s, and 141 s, which was caused by irregular DVL signals resulting from stiff changes in the seabed.

Figure 16. The responses and actuation in the lateral direction. The responses of ${}^{D}y$, $\psi$, and r followed their desired trajectories smoothly, demonstrating the effectiveness of the noise filtering depicted in Figure 4. The responses did not exhibit any significant chattering, in contrast to the responses in Figure 9, which represents the case without noise filtering.

Figure 17. The responses and actuation in the vertical direction. The responses of z, $\theta$, and q followed their desired trajectories smoothly. The response of z showed a steady state error because the buoyancy acted as disturbances in the vertical direction. The error of $\theta$ induced a reduction in the error of z, corresponding to the desired error dynamics in (14). The responses having non-zero errors were matched with the error dynamics analysis in Equations (22a)–(22c), which explains that the proposed controller may not converge to zero if there are non-zero disturbances on the vehicle dynamics.

Table 1. The root mean square (RMS) errors in $\left\{D\right\}$.

Direction	Forward	Lateral	Vertical	Total
RMS error (m)	0.0838	0.1595	0.2663	0.3216

5. Conclusions

In this paper, we proposed a trajectory tracking controller for AUVs in 3D space motion, along with experimental verification on the sea. The main concept of the proposed controller is to design the desired error dynamics that combine the state variables in the body-fixed coordinate and the world coordinate (i.e., the desired trajectory coordinate) to address the underactuated nature of the vehicle. The TDE, an indirect estimation method utilizing control inputs and vehicle outputs, was employed to estimate the nonlinear dynamics and disturbances of the vehicle. Consequently, the proposed controller is relatively easy to implement as it does not require the entire dynamic model of the vehicle. In terms of experimental implementation, the controller is relatively easy to stabilize since it only requires the first derivatives of the states and the states themselves, thereby potentially mitigating noise amplification arising from differentiation. Practical issues related to implementation in experimental environments were also addressed. A noise-filtering algorithm for the TDE was developed, and compensation methods for the mechanical limitations and nonlinear dynamics of the actuators were devised. The performance of the proposed controller was validated through experiments in seawater. The experimental results demonstrate the effectiveness of the noise-filtering algorithm in stabilizing the control performance. Through trajectory tracking control experiments in 3D space motion, it was verified that the proposed controller achieves accurate tracking performance, rendering it suitable for survey missions requiring precise tracking performance with errors of less than one meter.