Identification of Pitch Dynamics of An Autonomous Underwater Vehicle Using Sensor Fusion

This paper presents a method for identification of the hydrodynamic coefficients of the dive plane of an autonomous underwater vehicle. The proposed identification method uses the governing equations of motion to estimate the coefficients of the linear damping, added mass and inertia, cross flow drag and control. Parts of data required by the proposed identification method are not measured by the onboard instruments. Hence, an optimal fusion algorithm is devised which estimates the required data accurately with a high sampling rate. To excite the dive plane dynamics and obtain the required measurements, diving maneuvers should be performed. Hence, a reliable controller with satisfactory performance and stability is needed. A cascaded controller is designed based on the coefficients obtained using a semi-empirical method and its robustness to the uncertainties is verified by the µ-analysis method. The performance and accuracy of the identification and fusion algorithms are investigated through 6-DOF numerical simulations of a realistic autonomous underwater vehicle.


Introduction
Autonomous underwater vehicles (AUV) are employed worldwide for numerous missions. According to their missions, they appear in various shapes, sizes and configurations. Some examples of these missions are long range oceanographic surveys, procedures in autonomous docking (Rentschler et al., 2003) and search and rescue. For some applications such as search and rescue, axisymmetric and slender AUVs, which can achieve relatively high speed, are more favorable. The dynamics of slender AUVs can be divided into roll, pitch and yaw subsystems. The performances of the pitch and yaw controllers are critical for stable diving and steering maneuvers. However, a high performance stabilizing controller can be synthesized only with accurate knowledge of the values of the dynamic and hydrodynamic parameters. Therefore, several research studies have been directed towards finding the values of the hydrodynamic coefficients. One method to determine hydrodynamic coefficients of an AUV is to use a planar motion mechanism (PMM) and tow-tank experiments (Pereira and Duncan, 2000). Another approach is to perform a Computational Fluid Dynamics (CFD) analysis (Kim and Cho, 2011). Both PMM and CFD methods are time consuming and costly. Moreover, the changes in the hydrodynamic coefficients of the AUV due to the change of payload configuration are not considered in CFD and PMM methods. However, in the preliminary design stages of an AUV, CFD analysis can be considered as a useful tool (Phillips et al., 2010;Wang et al., 2017). Another approach is to estimate hydrodynamic coefficients by employing semi-empirical methods (de Barros et al., 2006;Lewis, 1988;Newman, 2018). These methods are based on aerodynamic and hydrodynamic theories such as slender body theory for bodies and lifting surface theories for fins. In early stages of the design, these methods are useful to obtain approximate values of the coefficients and evaluate maneuverability and controllability characteristics. There also exist a number of software packages that provide estimations of the hydrodynamic coefficients based on semi-empirical methods (Williams and Vukelich, 1979). These methods are faster and less costly compared with CFD and experimental methods but the results are rather inaccurate.
Another approach, which is more accurate, faster and easily repeatable for different cases, is identification. In this process, using on-board sensors, the response of the vehicle to various known inputs is observed and the unknown parameters are estimated (Ljung, 1998). Identification has been the subject of many marine researches. A lumped parameter model of an Underwater Unmanned Vehicle (UUV) was presented and identified by Caccia et al. (2000). The model considered the effects of propeller-hull and propeller-propeller interactions. The method of least squares (LS) was used for identification of unknown parameters. Bossley et al. (1999) developed an adaptive neuro-fuzzy identification algorithm. In a number of studies (Kim et al., 2002;Ernani et al., 2015), the dynamics of AUVs was identified using extended Kalman filter (EKF). Hong et al. (2013) proposed an online identification method to identify pitch and yaw dynamics of STARFISH AUV using recursive least squares method. Rentschler et al. (2003Rentschler et al. ( , 2006 investigated the offline identification of the pitch and yaw dynamics of ODYS-SEY III AUV. Tiano et al. (2007) identified yaw dynamics of the Hammerhead AUV using an observer Kalman filter. Petrich et al. (2007) identified the dynamics of the dive plane of Virginia Tech 475 AUV using LS. Yan et al. (2014) proposed a recursive subspace identification algorithm and simulated for an AUV. Xu et al. (2012) identified the dynamics of an AUV through Zigzag-like excitations. Allotta et al. (2018) identified the main hull hydrodynamic parameters for Typhoon AUV. Chou et al. (2018) applied the laser line scanning method for dynamic identification of an AUV. El-Fakdi et al. (2003) compared the results of two identification methods for URIS UUV. The first method aims to minimize the error of acceleration prediction while the second method aims to minimize the error of one step velocity prediction. The second method has been studied further by Ridao et al. (2004).
The objective of this paper is to develop an offline identification method for the hydrodynamic coefficients of the dive plane of an AUV. To this end, a nonlinear six degrees of freedom dynamic model is derived. The equations of motion in heave and pitch directions are used to develop the identification algorithm. The normal and axial components of acceleration and velocity and pitch rate are required to identify the parameters of the equations of motion in the dive plane. The acceleration components and pitch rate are measured by an Inertial Measurement Unit (IMU). The axial velocity can be measured using a pitot tube. But expensive measurement instruments are required to measure the normal velocity. Moreover, the sample rate of the pitot tube is usually much lower than that of the IMU. It also contains a relatively large measurement noise which introduces error in the identification. Hence, in this study, a fusion algorithm is proposed to combine the measurements of the IMU, pitot tube, and depth gauge; and obtain the normal and axial components of the velocity with a high sample rate.
For identification of the dive plane dynamics, it is essential to carry out diving maneuvers. For such maneuvers, a stabilizing depth controller is required in the first place. To this end, a cascade stabilizing depth controller is designed based on the semi-empirical hydrodynamic coefficients. The robustness of the proposed controller is verified through µanalysis method. By using the devised fusion and identification algorithms, the hydrodynamic coefficients are identified with a high accuracy through measurements acquired in stabilized diving maneuvers.
In addition to the developed fusion algorithm, the devised identification method is also novel. The previous identification methods for the dive plane of slender AUVs were mostly able to identify a combination of the hydrodynamic coefficients (Rentschler et al., 2003;Hong et al., 2013). In other words, the hydrodynamic coefficients were not determined individually. Moreover, some methods did not consider the added mass and inertia coefficients (Kim et al., 2002;Ernani et al., 2015). Furthermore, in many studies the dynamic model was linearized for identification purpose by assuming the pitch angle to be small (Rentschler et al., 2003(Rentschler et al., , 2006Hong et al., 2013). This assumption can be a source of inaccuracy in diving maneuvers with a large pitch angle. In contrast, the identification method developed in this study can estimate the hydrodynamic coefficients individually, including the added mass and inertia coefficients. Moreover, the assumption of small pitch angle is not made in the proposed identification method. Consequently, the AUV may undergo maneuvers with large pitch angles without any inaccuracy in the identification. This paper is made up of seven sections: Section 2 derives the equations of motion. The AUV that is studied in the paper is also introduced in this section; in Section 3 the synthesis and analysis of the depth controller explained; Section 4 presents the proposed identification method; in Section 5 the sensor fusion algorithm is developed; in Section 6 a six-degree-of-freedom simulation of AUV is performed and the performance of identification and fusion algorithms are validated; the last section concludes the paper.

Dynamic modelling
The fusion and identification algorithms are going to be developed for the AUV shown in Fig. 1. The values of the dynamic parameters are determined using CFD and Computer-Aided Design (CAD) methods. The mass of the AUV is 50 kg; the overall length is 1.94 m; its maximum diameter is 17.6 cm. The vehicle has four control fins of type NACA0009. The AUV displaces 0.0493 m 3 which means that it has neutral buoyancy in sea water with a density of 1015 kg/m 3 . The center of gravity (CG) of the vehicle is exactly 1 cm beneath the center of buoyancy.
The equations of motion of the AUV are coupled and nonlinear (Ernani et al., 2015). Six degrees of freedom (6-DOF) differential equations can be obtained using Newton and Euler equations (Fossen, 1994). The equations are derived employing two coordinate frames ( Fig. 1): the body frame whose origin coincides with the center of buoyancy, and the earth fixed frame (Hong et al., 2013).
It is generally intended to simplify the coupled dynamic equations to three decoupled subsystems: roll, pitch (diving) and yaw (steering) (Hong et al., 2013). In this section it is intended to develop comprehensive equations of motion of each subsystem. The notation of SNAME (1950) is used for hydrodynamic coefficients.
The six degrees of freedom differential equations can be stated as: where is the vertical distance between the CG and the origin of the body frame. , and are the moments of inertia about the body frame axes. The acceleration of the origin of the body frame in x, y and z-axes of the body frame which are respectively marked by , and are equal to Hence, Eq. (1) can also be stated as:

Roll dynamics K
In Eq. (3), which is the rolling moment about the origin is equal to δa where , which is the deflection of the aileron, is defined as (Fig. 2): K mot z CG z CG g y is the reaction torque caused by the rotation of propulsive motor. As stated before, is the distance between the center of gravity and the center of buoyancy. Generally, the AUVs are designed in a way that the CG is exactly under the center of buoyancy (Rentschler et al., 2003(Rentschler et al., , 2006Hong et al., 2013). In this way, the righting moment of buoyancy provides roll and pitch stability. Hence, should be positive. , which is the gravity component in y-axis of the body frame, is equal to g y = gcosθ sin φ.
(6) According to Eq. (3), the dynamic equation of roll motion can be stated as: (7) Since the linear cascade controller is going to be designed, the following assumptions are made to linearize the roll dynamic equation: 3) The roll moment produced by motor rotation is treated as disturbance and is not included in the linear equation. a y ≈ 0 4) AUV maneuvers in the dive plane and the only forces exerted to it are its weight, buoyancy and vertical hydrodynamic forces, hence . According to these assumptions, Eq. (7) is linearized as: where and . Hence, by assuming and applying a Laplace transform, the transfer function is governed as: 2.2 Yaw dynamics According to Eq. (1) and making some simplifying as- sumptions, the linear equations of motion in the steering plane can be achieved as (Hong et al., 2013): δ r where is the deflection of rudder, and it can be obtained as (Fig. 2): (12) Eqs. (10) and (11) can be represented in state space as: Hence by applying a Laplace transform, the transfer function of yaw dynamics can be governed as: 2.3 Pitch dynamics According to Eq. (1), and making some simplifying assumptions, the linear equations of motion in the diving plane can be achieved as (Hong et al., 2013): δ e where is the deflection of elevator, and is obtained as (Fig. 2): u 0 and is the steady-state speed of the AUV. Eqs. (21), (22) and (23) can be represented in state space as: With the Laplace transform, the transfer functions of diving plane can be governed as:

Cascade controller design and analysis
To estimate the dive plane hydrodynamic coefficients, diving maneuvers are required. The diving maneuvers can be carried out by a depth controller. Also, to eliminate coup-ling terms in the equations of motion, it is essential to keep the roll angle close to zero and the yaw angle constant. For each subsystem, a classic linear controller is designed and its robustness is verified by μ-analysis. For the sake of briefness, only the depth controller is discussed. The general configuration of the cascade controller which is going to be designed and analyzed is as depicted in Fig. 3. In this plant, K 1 is a PI controller. Its integrator removes steady state error. On the other hand, K 2 is a PD controller. Its differentiator decreases response overshoot and adds damping and stability. The transfer function H is the dynamics of actuators and modeled as a first-order lag: where is the elevator command. In this study, a lag of is assumed, then .

Zẇ Mq
These values are assumed to involve up to 20% uncertainty. Reviewing the hydrodynamic coefficients of a number of similar AUVs (Healey and Lienard, 1993;Rentschler et al., 2006) and to guarantee conservative, ranges for possible values of and are considered as: , z CG = 1 cm These parameters are obtained from CAD and can also be measured accurately in reality. Hence, the transfer function from elevator deflection to pitch angle with nominal values is obtained as: θ δ e = −5.595s − 11.41 s 3 + 5.15s 2 + 3.845s + 0.164
By using root locus approach (Ogata, 2002), controllers K 1 and K 2 are designed as: The depth step response of the closed-loop nominal system is illustrated in Fig. 4. The nominal stability and performance of the designed controller is satisfactory. But, since the estimated hydrodynamic coefficients obtained by semi-empirical methods are not accurate, poor performance or even instability may happen in the actual system. Hence it is essential to verify the robustness of the designed controller. There exist various methods for evaluation of the robust performance and stability (Skogestad and Postlethwaite, 2005). This study uses the μ-analysis method because this method avoids unnecessary conservativeness (Doyle et al., 1982). To this end, it is required to reconfigure the control system of Fig. 3 to the general control configuration. This procedure is rather complicated and lengthy and is not explained here due to the length restriction.

Identification method
Now, it is intended to identify the dive plane unknown parameters. From Eq. (1), the heave dynamic equation is given by The designed robust controller guarantees the stability of diving maneuvers which are essential to acquire the necessary data for identification. During diving maneuvers it can be stated that v ≈ 0, p ≈ 0 , r ≈ 0.
(41)   China Ocean Eng., 2019, Vol. 33, No. 5, P. 563-572 567 Therefore Eq. (40) is written as: Zq Z By neglecting the contribution of , the normal force , is equal to (43) Hence . Dividing both sides of Eq. (44) by yieldṡ and . For identification purpose, at each instance k, this equation can be written as: where Vector is the input vector and is the vector of the unknown parameters. Consequently, the identification error model is defined by: wẇ K where, is the measured value of the parameter that is governed according to Eq. (2) as: where is the specific force which can be measured directly using the accelerometer of the IMU (Titterton and Weston, 2004) and is the gravity component in z-axis of the body frame and is obtained by (49) Eq. (47) can be stated in the vector form as: here, N is the number of samples. Now, to find the optimal value of the vector of unknown parameters, let us consider an optimization problem which tries to minimize the identification error by considering the following cost function (51) With the method of LS estimation, the optimal estimation of the unknown parameters is given bŷ (52) Hence, the estimated parameters arê Since the vehicle mass, m, is assumed to be accurately known, can be calculated as: (55) Hence, the parameters of the heave motion are identified. Now it is intended to identify the parameters of the pitch motion. From Eq. (1) the pitch dynamic equation is As mentioned earlier, during diving maneuvers and . Hence

Mẇ M
Neglecting the contribution of may make the pitching moment equal to Hence from Eqs. (2), (57) and (58) it yields where and the acceleration component is equal to where is the specific force which can be measured directly using the accelerometer of the IMU and is the gravity component in the x direction of the body frame, given by Hence, Eq. (59) can be written as: where , , and . At each time instance k, Eq. (62) can be written as: where the vector is the input vector and is the vector of the unknown parameters. The value of can be obtained by taking derivative of the gyro output of pitch rate. However, it is not recommended to take derivative due to high noise content. In this study, to overcome this issue, the identification method is developed based on the minimization of the one-step prediction error (El-Fakdi et al., 2003;Ridao et al., 2004). The one-step prediction error model is defined as: q f k where, is the gyro measurement of pitch rate and is equal to (65) f k Runge-Kutta numerical integration method is used to compute at each step. Now, it can be stated that where 568 Seid Farhad Abtahi et al. China Ocean Eng., 2019, Vol. 33, No. 5, P. 563-572 where N is the number of sampling times. Consequently, the identification is performed by finding the vector that minimizes the following cost function By applying LS estimation method, the optimal estimation of the unknown parameters is achieved as: Hence the estimated parameters arê Again, since the vehicle mass, m, is accurately known, can be calculated as: and by using this value, the unknown parameters are calculated as:

Sensor fusion
According to the previous section, to apply the proposed identification method, the values of axial and vertical velocities, u and w, are needed in Eqs. (46), (48) and (63). The axial velocity can be measured directly using a Pitot tube, however, such measurement suffers from large noises and low sampling rates which affect the accuracy of the identification. On the other hand, measuring the vertical velocity requires expensive equipment and the sample rates of them are also low. Hence, in this section a fusion algorithm is developed that incorporates the outputs of pitot tube, IMU and depth gauge to acquire an accurate estimation of u and w with a high sample rate. In diving maneuvers, from the kinematics it can be stated that (Fossen, 1994) and from Eq.
where is the time interval between the IMU measurements which is smaller than that of the Pitot tube and depth gauge. Considering an initial condition , we can integrate Eqs. (72) and (73), and compare the integration results of u and z with the measurements of Pitot tube and depth gauge and evaluate the consistency. However, is unknown because w cannot be measured. On the other hand, u and z are measureable. However, due to measurement noises, and are considered to be unknown. Hence, the following optimization problem should be solved to fuse the sensors outputs [ u 0 w 0 z 0 ] Optimization problem: find a set of such that the integral of the signals for N measurement instances of the Pitot tube and depth gauge, minimizes the following cost function zũ σ z σ uẑ u where and are the measurements of depth and axial velocity by the depth gauge and Pitot tube, respectively, and are the standard deviations of measurements noises and and are the depth and axial velocity data calculated by integration, respectively. The optimization problem is solved using particle swarm optimization (PSO) (Das et al., 2008). Hence, after fusion, accurate data set of w and u are obtained with the same sample rate as IMU.
The schematic of the proposed method of this study is shown in Fig. 5.

Simulation and results
To analyze the performance of the proposed identification and fusion algorithms, a 6-DOF simulation is performed. The actual values of the hydrodynamic coefficients are computed through CFD. The proposed identification method uses the simulation outputs to estimate the coefficients of the dive plane.

Simulation conditions
The AUV is initially at a depth of 3 m, moving in a straight line at a constant speed of 6 m/s. Periodic depth maneuvers are commanded to excite the dive plane dynamics. As it is illustrated in Fig. 6, the designed robust controller is successful in carrying out the commanded periodic maneuvers despite the parametric uncertainties and disturbances. Axial and vertical velocities, pitch angle and its rate and elevator deflection are illustrated in Fig. 7.
From CFD, the true hydrodynamic coefficients of the dive plane used in simulation are given in Table 1. The center of gravity position is . The noises of the gyro and accelerometer measurements have standard deviations of and 0.5 mg, respectively. The IMU sample rate is 100 Hz. The standard deviation of measurement noises of the depth gauge and Pitot tube are , respectively; the sample rate of both is 10 Hz. These parameters are employed to per-form a 6-DOF simulation and simulate the outputs of sensors. Now, to evaluate the proposed method, the developed fusion algorithm is applied to the simulated outputs of IMU, depth gauge and Pitot tube. The resulted values of u and w can be compared with their true values to evaluate the accuracy of the fusion algorithm. Then, using the computed values of u and w, the proposed identification algorithm is used to identify the coefficients of the dive plane. By comparing the identified coefficient with their true values the identification method is also evaluated.

Results and analysis
To implement the fusion algorithm, PSO is employed to minimize the cost function in Eq. (78) throughout the devised algorithm. The values of the parameters of PSO are shown in Table 2. As it can be seen in Fig. 8 the PSO algorithm estimates the unknown parameters iteratively and achieves a precise solution. By using these values and employing the developed integration algorithm, accurate data sets of u, w and z are obtained with 100 Hz sample rate. The resluts are illustrtaed in Figs. 9, 10 and 11. It can be seen that these data sets are precise. With the estimated values of u and w, the developed identification method is impelented and the result is presnted in Table 3. These resluts prove the accuracy of the proposed idetification method.

Conclusion
In this study, we devised a fusion algorithm and an identification method to estimate the hydrodynamic coefficients of the dive plane of an autonomous underwater vehicle. The    fusion algorithm uses measurements of the IMU, depth gauge and Pitot tube to provide the identification algorithm with the required data. The PSO method was employed to solve the optimization problem of the fusion algorithm. Based on the estimations by the semi-empirical methods, a robust cascade controller was designed to perform diving maneuvers and excite the dynamics for identification purpose. To evaluate the performance of the proposed method, 6-DOF simulations were performed. The results confirm the accuracy of the proposed fusion and identification methods.  . 8. Estimation of the initial kinematic parameters.