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

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][3][4], oil industry pipeline inspection and maintenance [5][6][7], search and rescue missions [8], and operations in polluted and shallow water [9,10]. PLOS  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][14][15][16][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][23][24][25][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 nonlinear 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 nonlinear systems. A comparison with the conventional PID is done to demonstrate the timedomain 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 nonlinear 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/ nframe) represented by [n, e, d], and finally the body-fixed reference frame (b-frame) repre- 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 −5 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. 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.
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).
Where P n is the AUV position vector in the NED frame, Θ is Euler angles vector, v b o is body-fixed linear velocity vector, o b nb is body-fixed angular velocity vector, f b o is body-fixed forces vector, m b o 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 b o to AUV inertial rates _ P n , the matrix in Eq (6) transforms from AUV angular velocities o b nb to Euler rates _ Y. 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. where; À sy cys0 cyc0

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. The aim of the oriented vertical and horizontal thrusters is to obtain a unified center of rotation for the AUV vessel as shown in The angles ξ h and ξ v is calculated using the equations: Inverse kinematics and self-tuned fuzzy PID for AUV control PLOS ONE | https://doi.org/10.1371/journal.pone.0179611 July 6, 2017 The inverse kinematic control model for the horizontal thrusters are represented by:  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: And for rotational motions are: Inverse kinematics and self-tuned fuzzy PID for AUV control Such that "m" is the mass of the vessel in kg and the r b 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: Where I xx , I yy , and I zz are the moments of inertia about x b , y b , and z b -axes: And I xy = I yx , I xz = I zx , and I zy = I yz are the products of inertia: 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 b g ¼ 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: According to [40] the rigid body dynamics can be expressed in vectorial form as: 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. ð22Þ C RB can be calculated from system inertia matrix. Such that: Where ν 1 = [u, v, w] and ν 2 = [p, q, r]. And expression S(.) denotes a skew-symmetric matrix or the cross operator such that:ã

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: ð26Þ Where; Such that ρ is the water medium density in kg/m 3 . C d is the unit-less drag coefficient which depends on Reynolds number. A d is the drag contact area in m 2 , 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: The non-linear drag moments due to rotational motions can be modeled as: The non-linear drag matrix is expressed as: The vectorial form of the dynamic model including damping forces will be: 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 n g will act through the center of gravity (CG) defined by r b g ¼ x g ; y g ; z g h i while the buoyancy force f n b will act through the center of buoyancy (CB) of the vessel defined by According to the SNAME (1950) notation [38], the submerged weight of the body and buoyancy force are defined as: Therefore; The weight and buoyancy force can be transformed to the body-fixed coordinate system by: Finally, the gravitational and buoyancy matrix can be expressed as: Which can be expanded to: gðZÞ ¼ ðW À BÞsinðyÞ À ðW À BÞcosðyÞsinð0Þ À ðW À BÞcosðyÞcosð0Þ À ðy g W À y b BÞcosðyÞcosð0Þ þ ðz g W À z b BÞcosðyÞsinð0Þ In this work it is assumed that both centers of gravity and buoyancy are coincided at origin O of the vessel r b g ¼ 0; 0; 0 ½ T and r b b ¼ 0; 0; 0 ½ T , and the vessel is neutrally buoyant W = B. Therefore; The vectorial form including the gravitation and buoyancy forces will be: 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: 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. Hence the vectorial form dynamic model including ocean current disturbances will be: For computer simulations, the ocean current speed and direction can be generated using first order Gauss-Markov processes: Where w i (i = 1, 2, 3) are zero-mean Gaussian white noise processes, and μ i ! 0 (i = 1, 2, 3) are constants. If μ 1 = μ 2 = μ 3 = 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: The expression in Eq (46) can be transformed from NED to body-axis frame of the vessel using Euler angle rotation matrix R n b ðYÞ shown in Eq (5) Thruster dynamics The thruster can be separated in to two parts which are the actuator and the propeller as shown in Fig 10. A model of brushed DC motor is used as an actuator and the dynamic equations are: The abbreviations of the physical parameters are defined in the Table 2.
The propeller torque and thrust equations as mentioned in [42] are formulated as: Inverse kinematics and self-tuned fuzzy PID for AUV control 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: 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.
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. Inverse kinematics and self-tuned fuzzy PID for AUV control

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: For a 2D circle shape trajectory the equation will be: 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: 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: 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. Inverse kinematics and self-tuned fuzzy PID for AUV control The representation in continuous-time domain can by expressed as: Regarding the discrete-time controller Forward Euler method is selected for the integration and derivation as they are shown in: The resultant discrete-time PID controller is represented by 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 . The new STFPID tuning parameters can be expressed by: 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. As a result, the discrete-time PID represented by Eq (69) can be reformulated such that To generate the dK p , dK i , and dK d a Fuzzy rules table has been proposed in Tables 3-5 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 Inverse kinematics and self-tuned fuzzy PID for AUV control 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. 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.

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.

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.
The parameters of both velocity and position controllers have been selected as demonstrated in Tables 7 and 8 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  Inverse kinematics and self-tuned fuzzy PID for AUV control 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 xyplane with injecting disturbances can be found in S1 File. The noise wave form is a sinusoidal wave with additive white Gaussian noise as shown in  Inverse kinematics and self-tuned fuzzy PID for AUV control 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.
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  Inverse kinematics and self-tuned fuzzy PID for AUV control 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 Inverse kinematics and self-tuned fuzzy PID for AUV control 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  Inverse kinematics and self-tuned fuzzy PID for AUV control 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.