Trajectory following and stabilization control of fully actuated AUV using inverse kinematics and self-tuning fuzzy PID

Abstract

In this work a design for self-tuning non-linear Fuzzy Proportional Integral Derivative (FPID) controller is presented to control position and speed of Multiple Input Multiple Output (MIMO) fully-actuated Autonomous Underwater Vehicles (AUV) to follow desired trajectories. Non-linearity that results from the hydrodynamics and the coupled AUV dynamics makes the design of a stable controller a very difficult task. In this study, the control scheme in a simulation environment is validated using dynamic and kinematic equations for the AUV model and hydrodynamic damping equations. An AUV configuration with eight thrusters and an inverse kinematic model from a previous work is utilized in the simulation. In the proposed controller, Mamdani fuzzy rules are used to tune the parameters of the PID. Nonlinear fuzzy Gaussian membership functions are selected to give better performance and response in the non-linear system. A control architecture with two feedback loops is designed such that the inner loop is for velocity control and outer loop is for position control. Several test scenarios are executed to validate the controller performance including different complex trajectories with and without injection of ocean current disturbances. A comparison between the proposed FPID controller and the conventional PID controller is studied and shows that the FPID controller has a faster response to the reference signal and more stable behavior in a disturbed non-linear environment.

Introduction

AUV is considered one of the most challenging and difficult fields of research. Contemporary markets are looking to improve this field of research because of its great commercial importance and need [1]. Recently, AUV usage expanded to include seabed mapping, oceanographic and underwater living species exploration and research [2–4], oil industry pipeline inspection and maintenance [5–7], search and rescue missions [8], and operations in polluted and shallow water [9, 10].

Controlling an AUV is challenging because of uncertainties in AUV parameters and coefficients, coupled AUV dynamics, and non-linearity of underwater environments due to ocean current disturbances, hydrodynamics drag forces, and uncertain coefficients [11, 12]. Because of this, the conventional linear controllers like PID will only give the desired behavior around certain inputs and disturbances that it is tuned for. That is why the utilization of artificial intelligence to do self-tuning of the PID parameters is required, and will allow the AUV to have a robust control system in the disturbed non-linear environment [13–17].

The desired controller should be robust and adapt to the changes in AUV and environment parameters. It should also be adaptive to the changes in the control performance because of the ocean current disturbance and the variation in AUV dynamics. Thus many control techniques have been proposed for the control of the AUV such as linear controllers [18, 19], Sliding-Mode Controllers (SMC) [20, 21], Fuzzy Logic Control (FLC) [22–26], adaptive Control [27, 28], and neural network-based control [29, 30].

The disadvantages of linear controllers like PID, Linear Quadratic Regulator (LQR) and Linear Quadratic Gaussian (LQG) are that they only have stable performance around specific operating points and are not stable for variations in the environment and AUV parameters. The SMC is considered as an efficient and robust control for high-order complex non-linear systems. The major advantage of sliding mode is its low sensitivity to plant parameter variations and disturbances that do not require exact modeling. However, the implementation of SMC may lead to an undesirable phenomenon of oscillations with finite frequency and amplitude called “chattering” that results in low control accuracy, high wearing of moving mechanical parts, and wasting energy in actuators. The main reason for the chattering is the ignoring of dynamics from actuators and sensors in the system modeling. Some researchers presented approaches for chattering mitigation and suppression [31]. The inverse neural network-based controller disadvantage is that it requires a long time for training, and it is very difficult to obtain a fit generic model as the model can be over-fitted.

Fuzzy logic is a way to make machines more intelligent by enabling them to reason in a fuzzy manner like humans. It was first proposed by Lotfy Zadeh in 1965 [32]. It can deal with uncertain and qualitative decision-making problems. Controllers that combine intelligent and conventional techniques are commonly used in the control of complex dynamic systems. In the design of the traditional controllers like the PID, the knowledge of the system’s realistic physical model is required but are mostly unavailable because of their complexity. Fuzzy controllers are rule-based controllers that benefit from the expertise of human knowledge. They use a reasoning rule base for estimating the required control signal regardless of the system’s physical model knowledge [22, 23]. The main disadvantage of the FLC is that it has a lot of parameters to tune like the ranges and shapes of the membership functions, as opposed to the PID that has only three parameters. Beside it requires much more computation time than the conventional PID because of the complex operations. The FLC doesn’t have much better characteristics in time domain than the PID but the main advantage is that it can work with non-linear systems [33].

In this study a combination of the FLC and PID is utilized to obtain an enhanced control response with the PID controller under the supervision of the FLC system. This method combines the simple mathematical equations and low computation time of the PID controller with the ability of the FLC to tune and adapt the PID parameters so that they may work with non-linear systems. A comparison with the conventional PID is done to demonstrate the time-domain performance and the function of the self-tuned Fuzzy PID controller (FPID). In the design of the fuzzy membership functions, a combination of trapezoidal, triangular inputs and Gaussian functions for outputs are utilized. In Khodayari’s 2015 study [34] a similar approach has been introduced, but the inputs have only triangular membership functions and the fuzzy tuning is used for non-fully actuated AUV. In this work the fuzzy rules differ from the study in [34] as they provide a better tuning behaviour for the fully actuated system in the disturbed non-linear environment. The aim of using Gaussian membership functions is to obtain a non-linear response because of the non-linearity of the AUV system dynamics and hydrodynamics [35]. The AUV configuration along with the inverse kinematics and control architecture used in this research is demonstrated and presented in our previous work [18]. The advantage of this configuration is that we are able to have a fully-actuated AUV in which all degrees of freedom are controllable. This is achieved by accurately controlling the angular speed of each thruster independently based on the reference trajectory signals. This feature makes the AUV capable of tasks that require precise stabilization to ocean current disturbances like underwater pipeline maintenance and path-following in very narrow spaces, such as in the exploration of drowned ships.

This paper is organized as follows. Section “Kinematics and Coordinate Systems” explains the coordinate systems and kinematic modeling. Section “Configuration and Inverse Kinematics control model” shows the AUV configuration, thruster model, and thrusters inverse kinematic equations. In Section “Dynamic Model”, the AUV and environment dynamic equations are discussed. In Section “Control Design”, the proposed control architecture and design is demonstrated. In Section “Results”, a comparative simulation’s results and analysis of the robustness of the controllers are presented.

Kinematics and coordinate systems

The frames of references and coordinate systems used in marine navigation systems are explained in Fossen’s 2002 study [36] and also shown in Fig 1, the Earth-Centered Inertial frame (ECI/ i-frame) represented by [x_i, y_i, z_i], the Earth-centered Earth-Fixed reference frame (ECEF/ e-frame) represented by [x_e, y_e, z_e], the North-East-Down coordinate system (NED/ n-frame) represented by [n, e, d], and finally the body-fixed reference frame (b-frame) represented by [x_b, y_b, z_b].

Fig 1. Coordinate systems.

The ECI frame is a non-accelerating frame in which its origin is at the center of the Earth. The ECEF frame also has its origin fixed at the center of the Earth, but its axes is rotating relative to the ECI frame, which is fixed in space. The ECEF frame rotates at angular rate ω_e = 7.2921.10⁻⁵ rad/sec [37]. The NED frame is defined as the tangent plane on the surface of the Earth. For this system, the x-axis points north, the y-axis points east, and the z-axis points to the center of the Earth. The location of the n-frame relative to the e-frame is determined by using angles l and μ to denote longitude and latitude, respectively. The body-fixed frame is a moving frame with the AUV and the origin “O” of the b-frame is usually coinciding with the center of gravity (CG). For marine vessels, the body axes [x_b, y_b, z_b] are chosen to coincide with the principal axes of inertia as shown in Fig 2.

Fig 2. Motion variables for a marine vessel (SNAME 1950).

The motion variables for a marine vessel underwater are represented using six parameters such that the first three parameters describe the translational motions in x, y, and z coordinates while the last three describe the rotations and orientations around the x, y, and z axes as shown in Fig 2 and SNAM’s 1952 study [38]. Table 1 defines each variable.

Table 1. The notation of SNAME (1950) for marine vessels.

DOF		Forces and Moments (b-frame)	Velocities and Angular Rates (b-frame)	Position and Euler Angles (n-frame)
1	motions in the x-direction (surge)	X	u	x
2	motions in the y-direction (sway)	Y	v	y
3	motions in the z-direction (heave)	Z	w	z
4	rotation about the x-axis (roll, heel)	K	p	ϕ
5	rotation about the y-axis (pitch, trim)	M	q	θ
6	rotation about the z-axis (yaw)	N	r	ψ

In this work, we assumed that the ECEF frame is fixed and the NED frame will be the inertial frame tangent to the surface of the Earth. This is because the angular speed of the Earth is very small, and the AUV application studied in this paper is low-speed and works within short distances. As a result, the Coriolis effect is negligible. The position and orientation of the AUV will be described in the NED inertial coordinates, while the linear velocities, angular velocities, forces, and moments will be described in the body-fixed frame coordinates as shown in Eqs (1), (2) and (3).

$$\begin{array}{c}\hfill P^n=\left[\begin{array}{c}n\\ e\\ d\end{array}\right]\in R^3,\Theta =\left[\begin{array}{c}\varphi \\ \theta \\ \psi \end{array}\right]\in S^3\end{array}$$ (1)

$$\begin{array}{c}\hfill v_o^b=\left[\begin{array}{c}u\\ v\\ w\end{array}\right]\in R^3,{\omega }_{nb}^b=\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\in R^3\end{array}$$ (2)

$$\begin{array}{c}\hfill f_o^b=\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]\in R^3,m_o^b=\left[\begin{array}{c}K\\ M\\ N\end{array}\right]\in R^3\end{array}$$ (3)

Where Pⁿ is the AUV position vector in the NED frame, Θ is Euler angles vector, $v_o^b$ is body-fixed linear velocity vector, ${\omega }_{nb}^b$ is body-fixed angular velocity vector, $f_o^b$ is body-fixed forces vector, $m_o^b$ is body-fixed moments vector. The generalized 6-DOF kinematic equation is as shown in Eq (4), that transforms velocities from body-fixed frame coordinates to NED inertial frame coordinates, such that the transformation matrix in Eq (5) transforms from AUV linear velocities $v_o^b$ to AUV inertial rates ${\dot{P}}^n$, the matrix in Eq (6) transforms from AUV angular velocities ${\omega }_{nb}^b$ to Euler rates $\dot{\Theta }$. The Euler rotation has a disadvantage such that a singularity might happen in the calculations which is called Gimbal Lock phenomenon. This singularity is later solved using quaternions in doing rotations instead of Euler.

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\dot{P}}^n\\ \dot{\Theta }\end{array}\right]=\left[\begin{array}{cc}{R}_b^n(\Theta )& 0_{3x3}\\ 0_{3x3}& T_{\Theta }(\Theta )\end{array}\right]\left[\begin{array}{c}{v}_o^b\\ {\omega }_{nb}^b\end{array}\right]\end{array}$$ (4)

where;

$$\begin{array}{c}\hfill R_b^n(\Theta )=\left[\begin{array}{ccc}c\psi c\theta & -s\psi c\varphi +c\psi s\theta & s\psi s\varphi +c\psi s\theta c\varphi \\ s\psi c\theta & c\psi c\varphi +s\psi s\theta s\varphi & -c\psi s\varphi +s\psi s\theta c\varphi \\ -s\theta & c\theta s\varphi & c\theta c\varphi \end{array}\right]\end{array}$$ (5)

$$\begin{array}{c}\hfill T_{\Theta }(\Theta )=\left[\begin{array}{ccc}1& s\varphi t\theta & c\varphi t\theta \\ 0& c\varphi & -s\varphi \\ 0& s\varphi /c\theta & c\varphi /c\theta \end{array}\right]\end{array}$$ (6)

Configuration and inverse kinematics control model

The AUV dimensions are 30cm, 30cm and 40cm for height, width, and length respectively. A total of eight thrusters are used. The thrusters have been mounted in a vectored orientation configuration, and they will be classified into two groups, vertical and horizontal thrusters. The proposed modular thrusters configuration allow the AUV to have a holonomic motion. Such modular configuration approaches have been studied in wheeled mobile robots [39] and has shown more controllability. The configuration is shown in Fig 3. The top and bottom floaters are adjusted such that the AUV is neutrally buoyant underwater.

Fig 3. AUV body configuration.

The aim of the oriented vertical and horizontal thrusters is to obtain a unified center of rotation for the AUV vessel as shown in Fig 4.

Fig 4. Vessel center point and thrusters allocation.

An inverse kinematic equations that relate resultant AUV vessel velocities in the body-axis frame to the required thruster angular velocities have been developed. With the aid of these equations it was possible to convert the AUV to be fully-actuated. The parameters used in the inverse kinematic model of the horizontal thrusters are demonstrated in Fig 5 where l_h is the distance between the center of the horizontal thruster and the center O of the AUV chassis. Figs 6 and 7 show the geometrical configurations of the vertical thrusters where l_v is the distance between the center of the vertical thruster and the center O of the AUV chassis. The thrusters are mounted such that the axle is normal to the diagonal where they are located.

Fig 5. Horizontal thrusters geometrical configuration.

Fig 6. Vertical thrusters geometrical configuration 1.

Fig 7. Vertical thrusters geometrical configuration 2.

The angles ξ_h and ξ_v is calculated using the equations:

$$\begin{array}{c}\hfill {\xi }_h=arctan(\frac{width}{length})\end{array}$$ (7)

$$\begin{array}{c}\hfill {\xi }_v=arctan(\frac{height}{\sqrt{widt{h}^2+lengt{h}^2}})\end{array}$$ (8)

The inverse kinematic control model for the horizontal thrusters are represented by:

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\omega }_{HFL}\\ {\omega }_{HFR}\\ {\omega }_{HRR}\\ {\omega }_{HRL}\end{array}\right]=\frac{\lambda }{P_{prop}}\left[\begin{array}{ccc}-sec\left({\xi }_h\right)& csc\left({\xi }_h\right)& l_h\\ -sec\left({\xi }_h\right)& -csc\left({\xi }_h\right)& -l_h\\ -sec\left({\xi }_h\right)& csc\left({\xi }_h\right)& -l_h\\ -sec\left({\xi }_h\right)& -csc\left({\xi }_h\right)& l_h\end{array}\right]\left[\begin{array}{c}u\\ v\\ r\end{array}\right]\end{array}$$ (9)

While for the vertical thrusters are represented by:

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\omega }_{VFL}\\ {\omega }_{VFR}\\ {\omega }_{VRR}\\ {\omega }_{VRL}\end{array}\right]=\frac{\lambda }{P_{prop}}\left[\begin{array}{ccc}sec\left({\xi }_v\right)& l_v& -l_v\\ sec\left({\xi }_v\right)& -l_v& -l_v\\ sec\left({\xi }_v\right)& -l_v& l_v\\ sec\left({\xi }_v\right)& l_v& l_v\end{array}\right]\left[\begin{array}{c}w\\ p\\ q\end{array}\right]\end{array}$$ (10)

Where propeller pitch P_prop is the axial distance covered in one revolution of the propeller (m/rev). λ > 0 is a unit-less control factor that is tuned to improve the controller response. The thruster angular velocity is ω_XYZ, such that “X” stands for Horizontal (H) or Vertical (V), “Y” stands for Front (F) or Rear (R), and “Z” stands for Left (L) or Right (R).

The rule of thumb to formulate the inverse kinematic control model is to define the role of each thruster in actuating a DOF such that, for example, in case of the Horizontal Front Right (HFR) thruster, The thruster axial motion may lead to surge and sway motions in the AUV besides yaw rotation. It does not contribute to the heave motion since the thruster is normal to the z-axis of the AUV body-frame and can not provide roll and pitch rotations. In the case of a vertical thruster for example the Vertical Rear Left (VRL) thruster, this thruster axial motion may lead to motions in surge, sway, and heave motions besides roll, pitch, and yaw rotations since it has polar and azimuthal angles in x-y-z planes. However, in generating the control signals, we don’t need to command vertical thrusters to do surge and sway motions and yaw rotation, because it will be accompanied with undesired rotations and hence unstable movements. Vertical thrusters are commanded only if a heave motion or roll and pitch rotations are required. The controller will correct the undesired surge and sway movements using equations for horizontal thrusters and the feedback loop.

Dynamic model

Rigid body dynamics

The rigid body dynamic equations used in this study are derived from and formulated by [36] using Newton-Euler method.

The final general equations for translational motions are:

$$\begin{array}{c}\hfill X=m[\dot{u}-vr+wq-x_g(q^2+r^2)+y_g(pq-\dot{r})+z_g(pr+\dot{q})]\end{array}$$ (11)

$$\begin{array}{c}\hfill Y=m[\dot{v}-wp+ur-y_g(r^2+p^2)+z_g(qr-\dot{p})+x_g(qp+\dot{r})]\end{array}$$ (12)

$$\begin{array}{c}\hfill Z=m[\dot{w}-uq+vp-z_g(p^2+q^2)+x_g(rp-\dot{q})+y_g(rq+\dot{p})]\end{array}$$ (13)

And for rotational motions are:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill K& \hfill =\hfill & I_{xx}\dot{p}+(I_{zz}-I_{yy})qr-(\dot{r}+pq)I_{xz}+(r^2-q^2)I_{yz}+(pr-\dot{q})I_{xy}\hfill \\ & \hfill +\hfill & m[y_g(\dot{w}-uq+vp)-z_g(\dot{v}-wp+ur)]\hfill \end{array}\end{array}$$ (14)

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill M& \hfill =\hfill & I_{yy}\dot{q}+(I_{xx}-I_{zz})rp-(\dot{p}+qr)I_{xy}+(p^2-r^2)I_{zx}+(qp-\dot{r})I_{yz}\hfill \\ & \hfill +\hfill & m[z_g(\dot{u}-vr+wq)-x_g(\dot{w}-uq+vp)]\hfill \end{array}\end{array}$$ (15)

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill N& \hfill =\hfill & I_{zz}\dot{r}+(I_{yy}-I_{xx})pq-(\dot{q}+rp)I_{yz}+(q^2-p^2)I_{xy}+(rq-\dot{p})I_{zx}\hfill \\ & \hfill +\hfill & m[x_g(\dot{v}-wp+ur)-y_g(\dot{u}-vr+wq)]\hfill \end{array}\end{array}$$ (16)

Such that “m” is the mass of the vessel in kg and the $r_g^b={[x_g,y_g,z_g]}^T$ is the vector from the vessel origin O to the center of gravity (CG) decomposed in b-frame and I_o is the inertia tensor of the vessel which is described by:

$$\begin{array}{c}\hfill I_o=\left[\begin{array}{ccc}{I}_{xx}& -I_{xy}& -I_{xz}\\ -I_{yx}& I_{yy}& -I_{yz}\\ -I_{zx}& -I_{zy}& I_{zz}\end{array}\right]\end{array}$$ (17)

Where I_xx, I_yy, and I_zz are the moments of inertia about x_b, y_b, and z_b-axes:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill I_{xx}& =\hfill & {\int }_V(y^2+z^2){\rho }_{m}dV\hfill \\ \hfill I_{yy}& =\hfill & {\int }_V(x^2+z^2){\rho }_{m}dV\hfill \\ \hfill I_{zz}& =\hfill & {\int }_V(x^2+y^2){\rho }_{m}dV\hfill \end{array}\end{array}$$ (18)

And I_xy = I_yx, I_xz = I_zx, and I_zy = I_yz are the products of inertia:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill I_{xy}& =\hfill & {\int }_{V}xy{\rho }_{m}dV\hfill \\ \hfill I_{yz}& =\hfill & {\int }_{V}yz{\rho }_{m}dV\hfill \\ \hfill I_{xz}& =\hfill & {\int }_{V}xz{\rho }_{m}dV\hfill \end{array}\end{array}$$ (19)

Further simplification for the 6-DOF rigid body equations of motion is done such that it is assumed that the vessel is already buoyant and the center of buoyancy and body-axis frame [O, x_b, y_b, z_b] coincides with the CG and principal axis of inertia, hence $r_g^b={[0,0,0]}^T$ and I_o = diag(I_xx, I_yy, I_zz). As a result, the simplified equations of motion is defined as:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill X& =\hfill & m[\dot{u}-vr+wq]\hfill \\ \hfill Y& =\hfill & m[\dot{v}-wp+ur]\hfill \\ \hfill Z& =\hfill & m[\dot{w}-uq+vp]\hfill \\ \hfill K& =\hfill & I_{xx}\dot{p}+(I_{zz}-I_{yy})qr\hfill \\ \hfill M& =\hfill & I_{yy}\dot{q}+(I_{xx}-I_{zz})rp\hfill \\ \hfill N& =\hfill & I_{zz}\dot{r}+(I_{yy}-I_{xx})pq\hfill \end{array}\end{array}$$ (20)

According to [40] the rigid body dynamics can be expressed in vectorial form as:

$$\begin{array}{c}\hfill M_{RB}\dot{\nu }+C_{RB}\left(\nu \right)\nu ={\tau }_{RB}\end{array}$$ (21)

Where ν = [u, v, w, p, q, r]^T is the body-fixed linear and angular velocity vector. τ_RB = [X, Y, Z, K, M, N]^T is a generalized vector of external forces and moments. M_RB and C_RB will be referred to as the rigid body inertia, and Coriolis and centrifugal matrices, respectively.

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill M_{RB}& =\hfill & \left[\begin{array}{cc}m{I}_{3\times 3}& -mS\left(r_g^b\right)\\ mS\left(r_g^b\right)& I_o\end{array}\right]\hfill \\ & =\hfill & \left[\begin{array}{cccccc}m& 0& 0& 0& m{z}_g& -m{y}_g\\ 0& m& 0& -m{z}_g& 0& m{x}_g\\ 0& 0& m& m{y}_g& -m{x}_g& 0\\ 0& -m{z}_g& m{y}_g& I_{xx}& -I_{xy}& -I_{xz}\\ m{z}_g& 0& -m{x}_g& -I_{yx}& I_{yy}& -I_{yz}\\ -m{y}_g& m{x}_g& 0& -I_{zx}& -I_{zy}& I_{zz}\end{array}\right]\hfill \end{array}\end{array}$$ (22)

$$\begin{array}{c}\hfill M_{RB}=M_{RB}^T=\left[\begin{array}{cc}{m}_{11}& m_{12}\\ m_{21}& m_{22}\end{array}\right]<0\end{array}$$ (23)

C_RB can be calculated from system inertia matrix. Such that:

$$\begin{array}{c}\hfill C_{RB}=\left[\begin{array}{cc}{0}_{3\times 3}& -S(m_{11}{\nu }_1+m_{12}{\nu }_2)\\ -S(m_{11}{\nu }_1+m_{12}{\nu }_2)& -S(m_{21}{\nu }_1+m_{22}{\nu }_2)\end{array}\right]\end{array}$$ (24)

Where ν₁ = [u, v, w] and ν₂ = [p, q, r]. And expression S(.) denotes a skew-symmetric matrix or the cross operator such that:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill \overrightarrow{a}\times & =\hfill & S\left(\overrightarrow{a}\right)\hfill \\ & =\hfill & \left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\hfill \end{array}\end{array}$$ (25)

Hydrodynamics

The hydrodynamic damping forces affecting underwater vehicles dynamics contain both drag and lift forces. However, the AUV works at low speeds so the lift force could be neglected because it has effect only at high speeds. As a result, only the drag forces will be considered. D’Alambert’s paradox states that that no hydrodynamic forces act on a solid moving completely submerged with constant velocity in a non-viscous fluid. But in a viscous fluid, frictional forces are present such that the system is not conservative with respect to energy. The drag forces can be separated into linear and non-linear terms, D(ν) = D_l + D_n(ν), where D_l is linear drag forces and D_n(ν) is non-linear drag forces. Since it is assumed that the vehicle body has symmetry about all planes, then D_l can be expressed as:

$$\begin{array}{c}\hfill D_l=\left[\begin{array}{cccccc}{X}_u& 0& 0& 0& 0& 0\\ 0& Y_v& 0& 0& 0& 0\\ 0& 0& Z_w& 0& 0& 0\\ 0& 0& 0& K_p& 0& 0\\ 0& 0& 0& 0& M_q& 0\\ 0& 0& 0& 0& 0& N_r\end{array}\right]\end{array}$$ (26)

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill X_l& =\hfill & X_{u}u\hfill \\ \hfill Y_l& =\hfill & Y_{v}v\hfill \\ \hfill Z_l& =\hfill & Z_{w}w\hfill \\ \hfill K_l& =\hfill & K_{p}p\hfill \\ \hfill M_l& =\hfill & M_{q}q\hfill \\ \hfill N_l& =\hfill & N_{r}r\hfill \end{array}\end{array}$$ (27)

Where;

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill X_u& =\hfill & -\frac{1}{2}\rho C_{du}{A}_{du}\hfill \\ \hfill Y_v& =\hfill & -\frac{1}{2}\rho C_{dv}{A}_{dv}\hfill \\ \hfill Z_w& =\hfill & -\frac{1}{2}\rho C_{dw}{A}_{dw}\hfill \\ \hfill K_p& =\hfill & -\frac{1}{16}\rho C_{dp}x{z}^4\hfill \\ \hfill M_q& =\hfill & -\frac{1}{16}\rho C_{dq}y{x}^4\hfill \\ \hfill N_r& =\hfill & -\frac{1}{16}\rho C_{dr}z{x}^4\hfill \end{array}\end{array}$$ (28)

Such that ρ is the water medium density in kg/m³. C_d is the unit-less drag coefficient which depends on Reynolds number. A_d is the drag contact area in m², and x, y, and z are the length, width, and height in meters of the AUV, respectively.

The non-linear drag forces due to vortex shedding in the translational motions can be modeled as shown by:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill X_{nl}& =\hfill & -\left(\frac{1}{2}\rho C_{du}{A}_{du}\right)u\left|u\right|\hfill \\ & =\hfill & X_{u\left|u\right|}u\left|u\right|\hfill \\ \hfill Y_{nl}& =\hfill & -\left(\frac{1}{2}\rho C_{dv}{A}_{dv}\right)v\left|v\right|\hfill \\ & =\hfill & Y_{v\left|v\right|}v\left|v\right|\hfill \\ \hfill Z_{nl}& =\hfill & -\left(\frac{1}{2}\rho C_{dw}{A}_{dw}\right)w\left|w\right|\hfill \\ & =\hfill & Z_{w\left|w\right|}w\left|w\right|\hfill \end{array}\end{array}$$ (29)

The non-linear drag moments due to rotational motions can be modeled as:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill K_{nl}& =\hfill & -\left(\frac{1}{16}\rho C_{dp}x{z}^4\right)p\left|p\right|\hfill \\ & =\hfill & K_{p\left|p\right|}p\left|p\right|\hfill \\ \hfill M_{nl}& =\hfill & -\left(\frac{1}{16}\rho C_{dq}y{x}^4\right)q\left|q\right|\hfill \\ & =\hfill & K_{q\left|q\right|}q\left|q\right|\hfill \\ \hfill N_{nl}& =\hfill & -\left(\frac{1}{16}\rho C_{dr}z{x}^4\right)r\left|r\right|\hfill \\ & =\hfill & K_{r\left|r\right|}r\left|r\right|\hfill \end{array}\end{array}$$ (30)

The non-linear drag matrix is expressed as:

$$\begin{array}{c}\hfill D_n\left(\nu \right)=\left[\begin{array}{cccccc}{X}_{u\left|u\right|}\left|u\right|& 0& 0& 0& 0& 0\\ 0& Y_{v\left|v\right|}\left|v\right|& 0& 0& 0& 0\\ 0& 0& Z_{w\left|w\right|}\left|w\right|& 0& 0& 0\\ 0& 0& 0& K_{p\left|p\right|}\left|p\right|& 0& 0\\ 0& 0& 0& 0& M_{q\left|q\right|}\left|q\right|& 0\\ 0& 0& 0& 0& 0& N_{r\left|r\right|}\left|r\right|\end{array}\right]\end{array}$$ (31)

The vectorial form of the dynamic model including damping forces will be:

$$\begin{array}{c}\hfill M_{RB}\dot{\nu }+C_{RB}\left(\nu \right)\nu +D\left(\nu \right)\nu ={\tau }_{RB}\end{array}$$ (32)

Such that D(ν) is the damping matrix.

Gravitational and buoyancy matrix

Besides mass and damping forces, the underwater vehicles will also be affected by gravity and buoyancy forces. In hydrodynamics terminology these are called restoring forces as shown in [36]. As shown in Fig 8 the gravitational force $f_g^n$ will act through the center of gravity (CG) defined by $r_g^b=[x_g,y_g,z_g]$ while the buoyancy force $f_b^n$ will act through the center of buoyancy (CB) of the vessel defined by $r_b^b=[x_b,y_b,z_b]$.

Fig 8. Gravitational and buoyancy forces acting on center of gravity and center of buoyancy of an underwater vessel.

According to the SNAME (1950) notation [38], the submerged weight of the body and buoyancy force are defined as:

$$\begin{array}{c}\hfill W=mg,B=\rho g\nabla \end{array}$$ (33)

Therefore;

$$\begin{array}{c}\hfill f_g^n=\left[\begin{array}{c}0\\ 0\\ W\end{array}\right],f_b^n=-\left[\begin{array}{c}0\\ 0\\ B\end{array}\right]\end{array}$$ (34)

The weight and buoyancy force can be transformed to the body-fixed coordinate system by:

$$\begin{array}{c}\hfill f_g^b=R_b^n{(\Theta )}^{-1}{f}_g^n,f_b^b=R_b^n{(\Theta )}^{-1}{f}_b^n\end{array}$$ (35)

Finally, the gravitational and buoyancy matrix can be expressed as:

$$\begin{array}{c}\hfill g\left(\eta \right)=\left[\begin{array}{c}{f}_g^b+f_b^b\\ r_g^b\times f_g^b+r_b^b\times f_b^b\end{array}\right]\end{array}$$ (36)

Which can be expanded to:

$$\begin{array}{c}\hfill g\left(\eta \right)=\left[\begin{array}{c}(W-B)sin\left(\theta \right)\\ -(W-B)cos\left(\theta \right)sin\left(\varphi \right)\\ -(W-B)cos\left(\theta \right)cos\left(\varphi \right)\\ -(y_{g}W-y_{b}B)cos\left(\theta \right)cos\left(\varphi \right)+(z_{g}W-z_{b}B)cos\left(\theta \right)sin\left(\varphi \right)\\ (z_{g}W-z_{b}B)sin\left(\theta \right)+(x_{g}W-x_{b}B)cos\left(\theta \right)cos\left(\varphi \right)\\ -(x_{g}W-x_{b}B)cos\left(\theta \right)sin\left(\varphi \right)-(y_{g}W-y_{b}B)sin\left(\theta \right)\end{array}\right]\end{array}$$ (37)

In this work it is assumed that both centers of gravity and buoyancy are coincided at origin O of the vessel $r_g^b={[0,0,0]}^T$ and $r_b^b={[0,0,0]}^T$, and the vessel is neutrally buoyant W = B. Therefore;

$$\begin{array}{c}\hfill g\left(\eta \right)=\left[\begin{array}{c}{0}_{6x1}\end{array}\right]\end{array}$$ (38)

The vectorial form including the gravitation and buoyancy forces will be:

$$\begin{array}{c}\hfill M_{RB}\dot{\nu }+C_{RB}\left(\nu \right)\nu +D\left(\nu \right)\nu +g\left(\eta \right)={\tau }_{RB}\end{array}$$ (39)

Such that g(η) is the vector of gravitational/ buoyancy forces and moments.

Ocean current and disturbances

As explained in [41], ocean currents are horizontal and vertical circulation systems of ocean waters produced by gravity, wind friction, and water density variation in different parts of the ocean. The oceans are conveniently divided into two water spheres, the cold and warm water spheres. Since the Earth is rotating, the Coriolis force will try to turn the major currents to the east in the northern hemisphere and west in the southern hemisphere. Finally, the major ocean circulations will also have a tidal component arising from planetary interactions and gravity. In coastal regions the tidal speeds can reach 2–3 m/s, which is considered a very high speed.

Ocean currents’ forces on marine crafts can be accounted for by replacing the generalized velocity vector in the hydrodynamic terms with relative velocities:

$$\begin{array}{c}\hfill {\nu }_r=\nu -{\nu }_c\end{array}$$ (40)

The ocean current speed is denoted by V_c while its direction relative to the moving vessel is expressed by angle of attack α_c and side-slip angle β_c as shown in Fig 9.

Fig 9. Angle of attack and side-slip angle for a marine craft.

Hence the vectorial form dynamic model including ocean current disturbances will be:

$$\begin{array}{c}\hfill M_{RB}{\dot{\nu }}_r+C_{RB}\left({\nu }_r\right){\nu }_r+D\left({\nu }_r\right){\nu }_r+g\left(\eta \right)={\tau }_{RB}\end{array}$$ (41)

For computer simulations, the ocean current speed and direction can be generated using first order Gauss-Markov processes:

$$\begin{array}{c}\hfill {\dot{V}}_c+{\mu }_1{V}_c=w_1\end{array}$$ (42)

$$\begin{array}{c}\hfill {\dot{\alpha }}_c+{\mu }_2{\alpha }_c=w_2\end{array}$$ (43)

$$\begin{array}{c}\hfill {\dot{\beta }}_c+{\mu }_3{\beta }_c=w_3\end{array}$$ (44)

Where w_i (i = 1, 2, 3) are zero-mean Gaussian white noise processes, and μ_i ≥ 0 (i = 1, 2, 3) are constants. If μ₁ = μ₂ = μ₃ = 0 then the models are reduced to a random walks corresponding to the time integration of the white noise. A limitation shall be applied to the integration process to limit the current speed:

$$\begin{array}{c}\hfill V_{min}\le V_c\left(t\right)\le V_{max}\end{array}$$ (45)

A 3-D irrotational ocean current model is obtained by transforming the ocean current speed V_c and directions (α_c; β_c) from current flow axes to NED velocities:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill v_c^n& =\hfill & R_{y,{\alpha }_c}^T{R}_{z,-{\beta }_c}^T\left[\begin{array}{c}{V}_c\\ 0\\ 0\end{array}\right]\hfill \\ & =\hfill & \left[\begin{array}{c}{V}_{c}cos\left({\alpha }_c\right)cos\left({\beta }_c\right)\\ V_{c}sin\left({\beta }_c\right)\\ V_{c}sin\left({\alpha }_c\right)cos\left({\beta }_c\right)\end{array}\right]\hfill \end{array}\end{array}$$ (46)

Where;

$$\begin{array}{c}\hfill R_{y,{\alpha }_c}=\left[\begin{array}{ccc}cos\left({\alpha }_c\right)& 0& sin\left({\alpha }_c\right)\\ 0& 1& 0\\ -sin\left({\alpha }_c\right)& 0& cos\left({\alpha }_c\right)\end{array}\right]\end{array}$$ (47)

$$\begin{array}{c}\hfill R_{y,-{\beta }_c}=\left[\begin{array}{ccc}cos\left({\beta }_c\right)& sin\left({\beta }_c\right)& 0\\ -sin\left({\beta }_c\right)& cos\left({\beta }_c\right)& 0\\ 0& 0& 1\end{array}\right]\end{array}$$ (48)

The expression in Eq (46) can be transformed from NED to body-axis frame of the vessel using Euler angle rotation matrix $R_b^n(\Theta )$ shown in Eq (5):

$$\begin{array}{c}\hfill \left[\begin{array}{c}{u}_c\\ v_c\\ w_c\end{array}\right]=R_b^n{(\Theta )}^T\left[\begin{array}{c}{V}_{c}cos\left({\alpha }_c\right)cos\left({\beta }_c\right)\\ V_{c}sin\left({\beta }_c\right)\\ V_{c}sin\left({\alpha }_c\right)cos\left({\beta }_c\right)\end{array}\right]\end{array}$$ (49)

Thruster dynamics

The thruster can be separated in to two parts which are the actuator and the propeller as shown in Fig 10.

Fig 10. Thruster parts.

A model of brushed DC motor is used as an actuator and the dynamic equations are:

$$\begin{array}{c}\hfill T=K_{t}i\end{array}$$ (50)

$$\begin{array}{c}\hfill e=K_e\dot{\theta }\end{array}$$ (51)

$$\begin{array}{c}\hfill J\ddot{\theta }+b\dot{\theta }=K_{t}i\end{array}$$ (52)

$$\begin{array}{c}\hfill L\frac{di}{dt}+Ri=V-K_e\dot{\theta }\end{array}$$ (53)

The abbreviations of the physical parameters are defined in the Table 2.

Table 2. DC motor model abbreviations.

Parameter	Abbreviation	Unit
J	moment of inertia of the rotor	kg.m2
b	motor viscous friction constant	N.m.s
Ke	electromotive force constant	V/rad/sec
Kt	motor torque constant	N.m/Amp
R	electric resistance	Ohm
L	electric inductance	H

The propeller torque and thrust equations as mentioned in [42] are formulated as:

$$\begin{array}{c}\hfill Q=K_Q\rho D^5\left|n\right|n\end{array}$$ (54)

$$\begin{array}{c}\hfill T=K_T\rho D^4\left|n\right|n\end{array}$$ (55)

Where T and Q are the thrust and torque produced by the propeller, K_T and K_Q are the thrust and torque coefficients, respectively. ρ is the water density, n is the propeller angular velocity in rev/s. The thrust T is the thrust produced in the thruster frame (t-frame).

The resultant forces acting on the AUV body-axis frame in are formulated using force analysis and based on the configuration demonstrated in Figs 5 and 6. They are expressed by:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill X& =\hfill & cos\left({\xi }_h\right)[-T_{HFL}-T_{HFR}-T_{HRR}-T_{HRL}]\hfill \\ & +\hfill & sin\left({\xi }_v\right)cos\left({\xi }_h\right)[-T_{VFL}-T_{VFR}+T_{VRR}+T_{VRL}]\hfill \end{array}\end{array}$$ (56)

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill Y& =\hfill & sin\left({\xi }_h\right)[T_{HFL}-T_{HFR}+T_{HRR}-T_{HRL}]\hfill \\ & +\hfill & sin\left({\xi }_v\right)sin\left({\xi }_h\right)[-T_{VFL}+T_{VFR}+T_{VRR}-T_{VRL}]\hfill \end{array}\end{array}$$ (57)

$$\begin{array}{c}\hfill Z=cos\left({\xi }_v\right)[T_{VFL}+T_{VFR}+T_{VRR}+T_{VRL}]\end{array}$$ (58)

$$\begin{array}{c}\hfill K=l_v\times [T_{VFL}-T_{VFR}-T_{VRR}+T_{VRL}]\end{array}$$ (59)

$$\begin{array}{c}\hfill M=l_v\times [-T_{VFL}-T_{VFR}+T_{VRR}+T_{VRL}]\end{array}$$ (60)

$$\begin{array}{c}\hfill N=l_h\times [T_{HFL}-T_{HFR}-T_{HRR}+T_{HRL}]\end{array}$$ (61)

Where the thrust force is T_XYZ, such that X stands for Horizontal (H) or Vertical (V), Y stands for Front (F) or Rear (R), and Z stands for Left (L) or Right (R). l_h is the distance between the center of the horizontal thruster and the center O of the AUV chassis. And l_v is the distance between the center of the vertical thruster and the center O of the AUV chassis, as shown in the above Figs 6 and 7.

Control design

The control system has been design with double control loops. The inner loop is for controlling the AUV b-frame velocity while the outer loop is for controlling the AUV n-frame global position and Euler orientations as shown in Fig 11. The usage of double control loops has two major advantages. First, it provides faster control actions and rapidly attenuating environment disturbances. Second, it gives the control architecture the flexibility to control position and velocity of the AUV independently based on the user need and the required task.

Fig 11. Control system block diagram.

The AUV system, ocean currents model and hydrodynamics model blocks are only used for computer simulation, while the rest of the control architecture should be coded and implemented in the electronic control unit (ECU) to control the real AUV system.

Trajectory generator

This block is responsible for generating the reference trajectory by which the control system will be tracked. The trajectory is generated by means of way-points at fixed time-steps. Since the underwater system is considered a slow system, a larger fixed time-step will be used such that the AUV has the enough time to reach the desired way-point. Different shapes for trajectories can be used, such as straight line, circle, infinite and Möbius shapes.

For a straight line trajectory in 3D the equation will be:

$$\begin{array}{c}\hfill {\overrightarrow{k}}_f={\overrightarrow{k}}_0+(t\times \overrightarrow{h})\end{array}$$ (62)

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill x& =\hfill & x_0+(t\times h_x)\hfill \\ \hfill y& =\hfill & y_0+(t\times h_y)\hfill \\ \hfill z& =\hfill & z_0+(t\times h_z)\hfill \end{array}\end{array}$$ (63)

Where $\overrightarrow{h}=[h_x,h_y,h_z]$ specifies the direction or the slope of the line, [x₀, y₀, z₀] specifies the initial point of the line, and [x, y, z] specifies the new point of the line.

For a 2D circle shape trajectory the equation will be:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill x& =\hfill & R\times cos\left(\delta \right)\hfill \\ \hfill y& =\hfill & R\times sin\left(\delta \right)\hfill \end{array}\end{array}$$ (64)

Where R is the radius and δ is the angle in radians. δ is incremented by a fixed value at each fixed time-step.

For a 3D Möbius shape trajectory the equation will be:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill x& =\hfill & (1+\frac{R}{2}cos(\frac{\delta }{2}))cos(\delta )\hfill \\ \hfill y& =\hfill & (1+\frac{R}{2}cos(\frac{\delta }{2}))sin(\delta )\hfill \\ \hfill z& =\hfill & \frac{R}{2}sin(\frac{\delta }{2})\hfill \end{array}\end{array}$$ (65)

Where R is the radius in x-y plane describing the width of the Mobius shape.

NED to body frame block

This block is responsible for transforming the position control signals in n-frame to b-frame. The equations are:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill {\upsilon }_o^b& =\hfill & R^{-1}\dot{P}\hfill \\ \hfill {\omega }_{nb}^b& =\hfill & T^{-1}\dot{\Theta }\hfill \end{array}\end{array}$$ (66)

Where R and T are shown in matrix notations Eqs (5) and (6).

Inverse kinematics control model

This block is responsible for generating the reference angular speed in rad/s for each thruster corresponding to the desired AUV motion based on Eqs (9) and (10).

Velocity and position controllers

The PID controller used in this research is of the parallel form as shown in Fig 12.

Fig 12. Continuous-time PID controller block diagram.

The representation in continuous-time domain can by expressed as:

$$\begin{array}{c}\hfill u\left(t\right)=K_{p}e\left(t\right)+K_i{\int }_0^{t}e\left(\tau \right)d\tau +K_d\frac{de\left(t\right)}{dt}\end{array}$$ (67)

Regarding the discrete-time controller Forward Euler method is selected for the integration and derivation as they are shown in:

$$\begin{array}{c}\hfill \begin{array}{c}\hfill Discrete-timeintegration\left(ForwardEuler\right)\Rightarrow \frac{Ts}{(z-1)}\\ \hfill Discrete-timederivation\left(ForwardEuler\right)\Rightarrow \frac{N}{1+\frac{N.Ts}{(z-1)}}\end{array}\end{array}$$ (68)

The resultant discrete-time PID controller is represented by

$$\begin{array}{c}\hfill u\left(z\right)=K_p+K_i\frac{Ts}{(z-1)}+K_d\frac{N}{1+\frac{NTs}{(z-1)}}\end{array}$$ (69)

Such that K_p, K_i, and K_d are the proportional, integral, and derivative gains, respectively. u(z) is the control output. Ts is the sampling time. N is a scaling factor.

A Self-Tuned Fuzzy PID (STFPID) is designed as shown in Fig 13 such that the fuzzy inference system tunes the PID parameters intelligently based on the fuzzy rules of the expertise. Where the inputs to the fuzzy inference system are the error e and change in error ce and the outputs are dK_p, dK_i, and dK_d.

Fig 13. Discrete-time STFPID controller block diagram.

The new STFPID tuning parameters can be expressed by:

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill K_p^{\prime }& =\hfill & K_p+d{K}_p\hfill \\ \hfill K_i^{\prime }& =\hfill & K_i+d{K}_i\hfill \\ \hfill K_d^{\prime }& =\hfill & K_d+d{K}_d\hfill \end{array}\end{array}$$ (70)

The purpose of fuzzy logic is to formalize and implement a human being’s method of reasoning. It can therefore be classified as a field of artificial intelligence. The fuzzy rule base tool is the most common tool that is used in control applications. It is made of rules based on the human expertise. A number of rules have been defined based on the experiments and expertise to tune the PID parameters based on the knowledge of error and change in the error. These inputs are fuzzified as a first step. Then a reasoning is performed based on the rules defined, and degree of activation is calculated for each rule that depends on the classes the fuzzified inputs belong to. After that implication is performed. Aggregation is done to compute the final fuzzified output from the outputs of each rule. Finally, the output is de-fuzzified to obtain a crisp output that can be used to tune the PID parameters as illustrated in Fig 14.

Fig 14. Fuzzy logic system.

As a result, the discrete-time PID represented by Eq (69) can be reformulated such that

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill u\left(z\right)& =\hfill & K_p^{\prime }+K_i^{\prime }\frac{Ts}{(z-1)}+K_d^{\prime }\frac{N}{1+\frac{NTs}{(z-1)}}\hfill \\ & =\hfill & [K_p+d{K}_p]+[K_i+d{K}_i]\frac{Ts}{(z-1)}+[K_d+d{K}_d]\frac{N}{1+\frac{NTs}{(z-1)}}\hfill \end{array}\end{array}$$ (71)

To generate the dK_p, dK_i, and dK_d a Fuzzy rules table has been proposed in Tables 3–5, respectively, based on the expertise. The fuzzy linguistic variables are Negative Big (NB), Negative Medium (NM), Negative Small (NS), Zero (ZO), Positive Small (PS), Positive Medium (PM) and Positive Big (PB).

Table 3. dK_p fuzzy rule table.

ce	NB	NM	NS	ZO	PS	PM	PB
e
NB	ZO	ZO	NS	NS	PS	ZO	ZO
NM	NS	NS	NM	NM	NS	ZO	NS
NS	PS	ZO	NS	PS	ZO	ZO	NS
ZO	PM	PM	PS	ZO	NS	NM	NM
PS	PS	PS	ZO	NS	NM	NM	NM
PM	PS	ZO	NS	NS	NM	NM	NB
PB	ZO	ZO	NM	NM	NB	NB	NB

Table 5. dK_d fuzzy rule table.

ce	NB	NM	NS	ZO	PS	PM	PB
e
NB	PB	PS	NB	NB	NB	NM	PB
NM	PB	NM	NB	NB	NM	NM	ZO
NS	PM	PB	NM	NB	NS	PS	PS
ZO	ZO	NS	NS	ZO	NS	NS	ZO
PS	ZO	ZO	ZO	ZO	ZO	ZO	ZO
PM	PS	NB	PB	PB	NM	NS	NB
PB	PB	NS	NB	NB	NB	NM	PB

Table 4. dK_i fuzzy rule table.

ce	NB	NM	NS	ZO	PS	PM	PB
e
NB	PB	PB	PB	PB	PB	PB	PB
NM	PB	PB	PB	PB	PB	PB	PB
NS	NB	NM	NM	PB	PB	PB	PS
ZO	NB	NM	NS	ZO	PS	PM	PB
PS	ZO	ZO	PS	PS	PM	PM	PB
PM	ZO	ZO	PS	PM	PM	PB	PB
PB	ZO	ZO	PS	PM	PM	PB	PB

When the deviation |e| is large, in order to have fast-tracking performance, k_p should be greater. Taking a smaller value of k_d prevents instantaneous value of |ec| to be too large, at the same time a larger system response in order to avoid the overshoot, the integral action should be limited, the k_i value should normally be very small.

When the deviation |e| is of medium size, in order to ensure fast system response and have small overshoot, k_p should be reduced. Larger k_d increases the impact of system response, k_i should be appropriate.

When |e| is small, k_p and k_i should be bigger to ensure that the system has the ideal static performance. To avoid the vicinity of a shock at the system settings, k_d shall be chosen by the change of |e|.

The membership functions for the inputs are trapmf and trimf, which represent trapezoidal and triangular membership functions, respectively, as shown in Fig 15. The output is gaussmf, which represents Gaussian membership function as shown in Fig 16.

Fig 15. Inputs membership functions.

Fig 16. Output membership functions.

The final schematic for the STFPID including the fuzzy inference system is as shown in Figs 17 and 18. The inputs and outputs of the fuzzy inference system are normalized by ranging factors that describe the ranges of inputs and outputs.

Fig 17. STFPID controller schematic.

Fig 18. PID.

AUV system

This block is used only for simulation and consists of the model of eight thrusters actuator as listed in Eqs (50)–(53), such that a closed loop control using conventional PID are implemented for each thruster as shown in Fig 19.

Fig 19. Thruster control loop.

The thruster model equations are that listed in Eqs (54) and (55). The block also consists of the thrusters-vessel forward kinematics listed in Eqs (56)–(61). These calculate the resultant forces and moments on the AUV resulting from each thruster’s propeller rotation. Finally, it also contains the dynamic model of the AUV formulated in Eq (20), but is reformulated as shown in Eq (72) such that the inputs are the forces acting on the AUV body frame and the outputs are the vessel velocity states, as shown in the hydrodynamic environment model shown in Eqs (27), (29) and (30).

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill \dot{u}& =\hfill & \frac{X}{m}+vr-wq\hfill \\ \hfill \dot{v}& =\hfill & \frac{Y}{m}+wp-ur\hfill \\ \hfill \dot{w}& =\hfill & \frac{Z}{m}+uq-vp\hfill \\ \hfill \dot{p}& =\hfill & \frac{1}{I_x}[K-(I_z-I_y)qr]\hfill \\ \hfill \dot{q}& =\hfill & \frac{1}{I_y}[M-(I_x-I_z)rp]\hfill \\ \hfill \dot{r}& =\hfill & \frac{1}{I_z}[N-(I_y-I_x)pq]\hfill \end{array}\end{array}$$ (72)

Results

The simulation of this research is implemented and validated using Mathworks Simulink. The system specifications and parameters that have been chosen are as demonstrated in the Table 6.

Table 6. System specifications and values used for the simulation.

Parameter	Value
Sampling Time (dt)	0.001 (s) or 1 (ms)
AUV length	0.4 (m)
AUV width	0.3 (m)
AUV height	0.3 (m)
Origin-horizontal thruster distance (lh)	0.17 (m)
Origin-vertical thruster distance (lv)	0.17 (m)
Propeller Pitch (Pprop)	0.1 (m/rev)
Control Factor (λ)	0.04 (unit-less)

The parameters of both velocity and position controllers have been selected as demonstrated in Tables 7 and 8, respectively.

Table 7. Velocity controller parameters.

Parameter	u Control	v Control	w Control	p Control	q Control	r Control
Kp	1	1.5	1	1	1	1
Ki	1	1.5	1	1	1	1
Kd	0.1	0	0	0	0	0
e range	2	2	2	2	2	2
ce range	500	500	500	200	200	200
dkp range	2	2	2	2	2	2
dki range	2	2	2	2	2	2
dkd range	0	0	0	0	0	0

Table 8. Position controller parameters.

Parameter	x Control	y Control	z Control	ϕ Control	θ Control	ψ Control
Kp	5	5	5	10	10	10
Ki	0	0	0	0	0	0
Kd	10	10	10	10	10	10
e range	4	1	1	0.7	0.7	0.7
ce range	400	400	400	0.05	0.05	0.05
dkp range	20	20	20	10	10	10
dki range	0	0	0	0	0	0
dkd range	0	0	0	0	0	0

Several test scenarios have been executed to validate the control model using different trajectories with and without ocean disturbances. The results shown in the figures below are for STFPID performance relative to conventional PID. In the first test scenario a circle trajectory in x-y has been generated with and without disturbances, the results are shown in Figs 20 and 21. Fig 22 shows the performance of following the reference of the yaw orientation. Figs 23 and 24 demonstrate the time response of the xy positions for the system without and with disturbances, respectively. From a time response perspective, Figs 20 and 23 show that STFPID response is faster to achieve the reference waypoints such that the rising times are T_s = 12.01s and T_s = 8.38s for PID and STFPID, respectively. Figs 21 and 24 show that the overshoots are ranges between 7.5% and 11.2% in the case of PID, which means that the STFPID has much better disturbance attenuation capability. The STFPID response time is better, at T_s = 8.383s as compared to T_s = 11.73s for PID. The data sets of the test scenario for circle trajectory in xy-plane with injecting disturbances can be found in S1 File.

Fig 20. x-y plan circle trajectory scenario (without disturbances).

The red trajectory is the output from the fuzzy PID controller, the blue trajectory is the output from the PID controller, and the green-dotted line is the reference trajectory.

Fig 21. x-y plan circle trajectory scenario (with disturbances).

The red line is the output from the fuzzy PID controller, the blue line is the output from the PID controller, and the green-dotted line is the reference.

Fig 22. Time response of yaw orientation of the x-y plan circle trajectory scenario (without disturbances).

The red line is the fuzzy PID yaw angle output, the blue line is the PID yaw angle output, and the green-dotted line is the reference yaw angle.

Fig 23. Time response of x-y plan circle trajectory scenario (without disturbances).

The red line is the output from the fuzzy PID controller, the blue line is the output from the PID controller, and the green-dotted line is the reference.

Fig 24. Time response of x position of the circle trajectory scenario (with disturbances).

The red line is the output from the fuzzy PID controller, the blue line is the output from the PID controller, and the green-dotted line is the reference.

The noise wave form is a sinusoidal wave with additive white Gaussian noise as shown in Fig 25 with a frequency of 0.1 Hz and a signal to noise ratio (SNR) 10 dB. The resultant ocean current in n-frame and b-frame are as shown in Figs 25 and 26, respectively.

Fig 25. Disturbance effect in n-frame coordinates.

The sinusoidal blue wave is the noise wave in the current disturbance reference frame. The red waves in the three below figures are the current disturbances in the inertial n-frame axes.

Fig 26. Disturbance effect in b-frame coordinates.

The disturbances in the body reference frame.

The second test scenario is an attitude control for yaw, pitch, and roll successively without injecting disturbances as demonstrated in Fig 27. For attitude and orientation control, the STFPID also proves a significant improvement in achieving the reference attitude with almost no oscillations and very small overshoots compared to conventional PID. It also shows stability over time, while in case of PID, the system starts to oscillate due to system non-linearity.

Fig 27. Attitude control (yaw-pitch-roll sequence).

The red line is the output from the fuzzy PID controller, the blue line is the output from the PID controller, and the green-dotted line is the reference.

The third test scenario is for validating separate control of speed and position at the same time. In this test, the AUV is controlled such that it follows a trajectory with a Mobius shape in the 3D plan, along with having a constant angular rate around the AUV z-axis [p angular velocity]. There were no disturbances in this scenario. The results for following the way-points in xyz plan and time-response of angular velocity are shown in Figs 28 and 29, respectively. Fig 30 shows the time response of the xyz positions where the STFPID reached the reference way-points at rising time equal to 12 s while the PID reached the reference at more than 20 s. The data sets can be found in S2 File.

Fig 28. Trajectory following of a Mobius shape trajectory in 3D environment.

The red line is the output from the fuzzy PID controller, the blue line is the output from the PID controller, and the green-dotted line is the reference.

Fig 29. Time response of the angular velocity about b-frame z-axis in following Mobius trajectory.

The red line is the output from the fuzzy PID controller, the blue line is the output from the PID controller, and the green-dotted line is the reference.

Fig 30. Time response of the xyz positions of Mobius trajectory.

The red line is the output from the fuzzy PID controller, the blue line is the output from the PID controller, and the green-dotted line is the reference.

The control architecture here proves that it is capable of isolating position and velocity control of any DOF. For example, one can control the position for x, y, and z DOFs, and at the same time control the angular velocity of the rotation about b-frame z-axis [z_b] as presented in Figs 28 and 29. As Fig 29 demonstrates, the oscillations frequency in the angular velocity is very high and that’s because the position controller for x, and y DOFs are controlling the horizontal thrusters, which affects the yaw rate rotation. At the same time, the angular velocity control of [r] changes the speeds of the horizontal thrusters as listed in Eq (9). So both controllers are pushing against each others, but the angular velocity is oscillating around the reference velocity and not deviating to instability. In case of using the STFPID the oscillations amplitude is higher than that of conventional PID but its response time is faster.

Conclusion

The designed STFPID controller coupled with the inverse kinematic control model studied in this research shows a significant improvement in the time-response performance in controlling a fully-actuated AUV with fast response and minimum error compared to conventional PID. STFPID also shows better performance, even when ocean current disturbances are injected to the AUV system with almost very small overshoots compared to conventional PID that had a very large overshoot and slow response time. Furthermore, the control architecture presented in this work shows that the double control loops make the system capable of controlling both velocity and position independently as desired by the user or the references.

Supporting information

S1 File. Data sets and script.

This file contains the data sets and script to visualize the scenario XY Circle with disturbances.

(RAR)

S2 File. Data sets and script.

This file contains the data sets and script to visualize the scenario XYZ Mobius trajectory.

(RAR)

Data Availability

All relevant data are within the paper and its Supporting Information files.

Funding Statement

The authors received no specific funding for this work. It is funded by ourselves.