A Novel INS and Doppler Sensors Calibration Method for Long Range Underwater Vehicle Navigation

Since the drifts of Inertial Navigation System (INS) solutions are inevitable and also grow over time, a Doppler Velocity Log (DVL) is used to aid the INS to restrain its error growth. Therefore, INS/DVL integration is a common approach for Autonomous Underwater Vehicle (AUV) navigation. The parameters including the scale factor of DVL and misalignments between INS and DVL are key factors which limit the accuracy of the INS/DVL integration. In this paper, a novel parameter calibration method is proposed. An iterative implementation of the method is designed to reduce the error caused by INS initial alignment. Furthermore, a simplified INS/DVL integration scheme is employed. The proposed method is evaluated with both river trial and sea trial data sets. Using 0.03°/h(1σ) ring laser gyroscopes, 5 × 10−5 g(1σ) quartz accelerometers and DVL with accuracy 0.5% V ± 0.5 cm/s, INS/DVL integrated navigation can reach an accuracy of about 1‰ of distance travelled (CEP) in a river trial and 2‰ of distance travelled (CEP) in a sea trial.


Introduction
Autonomous Underwater Vehicles (AUV) present a uniquely challenging navigational problem because they operate autonomously in a highly unstructured environment [1]. Autonomous operations in deep water or covert military operations require the AUV to handle submerged operation for long periods of time. Currently, few techniques exist for reliable navigation for long range AUVs. Ultra-short baseline (USBL) acoustic navigation systems are employed on industrial, military, and scientific underwater vehicles and are preferred for the task of docking a vehicle to a transponder-equipped docking station [2][3][4][5][6]. Terrain-or landmark-based navigation methods use real-time sensing and a terrain or landmark map (e.g., topographic, magnetic, gravitational, or other geodetic data) to determine the vehicle's position [2]. But an a priori map is seldom available in AUV terrain-or landmark-based navigation. The standard method for full ocean depth XYZ acoustic navigation is 12-kHz-long baseline (LBL) acoustic navigation [2,7], but the precision and update rate of LBL position fixes vary over several orders of magnitude depending on the acoustic frequency, range, and acoustic path geometry [2]. Global Navigation Satellite System (GNSS) provides superior three-dimensional navigation capability for both surface and air vehicles but its signal cannot be directly received by deeply submerged ocean vehicles. A strapdown inertial navigation system (INS) is a good choice for self-contained localization and navigation of AUVs, but its position error accumulates with time elapse due to the inherent bias errors of gyros and accelerometers. Hence a navigation system based on INS will have an unacceptable position error drift without sufficient aiding. INS/DVL integrated navigation system using the high accurate velocity offered by DVL to restrain the error accumulation of INS is a widely-used under-water integrated navigation technology [8][9][10][11][12][13][14][15][16][17][18]. Even when a DVL is included, the accuracy of INS/DVL integration will be reduced because of the scale factor error of DVL and the misalignments between INS and DVL.
Because the scale factor error of DVL and the misalignments between INS and DVL are the key factors which limit the accuracy of INS/DVL integration, calibration and compensation of these parameters must be done before a mission is conducted. This calibration is necessary to account for mechanical misalignments in the installations of the INS and DVL, as well as for potential errors in the velocity estimates of the units [7]. In practical engineering applications, the first adapted method is based on the assumptions: (a) both INS and DVL are mounted onto the same rigid structure throughout a mission; (b) the lever arms and misalignments between these devices remain constant and small. However, such assumptions are not realistic in the real world. The second adapted method is to treat the misalignments between INS and DVL as unknown and then GNSS is used to estimate misalignment parameters in three dimensions. However, only yaw misalignment parameter between INS and DVL was considered in some early work. For example, in [19], Joyce proposed a method to estimated yaw misalignment error by using least squares (LS) method. In [20,21], the heading accuracy was further considered as one of the key factors which limit the calibration accuracy. In [2,22,23], James and his colleagues improved the calibration method, with precise position of acoustic navigation sensors such as LBL, three dimensional misalignments between INS and DVL can then be estimated simultaneously. But this method is difficult to implement that it might cause some inconvenience for real applications. In [24], an online estimation method of DVL misalignment angle in SINS/DVL was presented. However, it requires the AUV to be operated with complex maneuvers to enhance observability of the unknown states. The paper proposes a novel alignment calibration method with external GNSS signals. However, there is no need to receive the GNSS signals continuously which make it suitable for AUV platforms. Furthermore, a recursive implementation which can eliminate the effects of the INS initial alignment is proposed. The accuracy of the calibration is further improved. This paper is organized as follows: Section 2 introduces the navigation equations, including INS/DVL system equations and observation equations. The parameter calibration method is proposed in Section 3, followed by an iterative implementation to reduce the effects of the INS initial alignment. After the scale factor of DVL and misalignments between INS and DVL are fixed, the simplified INS/DVL integrated navigation system is designed in Section 4. An experimental evaluation of the proposed navigation system is presented in Section 5, where in particular, the performance of the navigation system both in the river trial and the sea trial is discussed. Finally, conclusions are drawn in Section 6.

Navigation Equations Model
The Autonomous Underwater Vehicle discussed in the paper is equipped with a DVL and an INS consisting of three gyroscopes and three accelerometers. The INS calculates position, velocity and attitude using high frequency data from an Inertial Measurement Unit. Navigation propagation equations are introduced in this section. The estimated states include position, velocity, attitude, biases of the inertial sensors, and biases of DVL. The typical integrated navigation scheme for AUV is shown in Figure 1. INS/DVL integration is employed for autonomous navigation for most of the time in the missions. Once the GNSS signals are available, the current position can be reset.

INS/DVL System Equations
The local level frame North-Up-East (NUE) is chosen as the navigation frame n . b is the INS body frame; i is the Earth-centered inertial (ECI) orthogonal reference frame; e denotes the Earth-centered Earth-fixed (ECEF) orthogonal reference frame. The states of the system model include position n P , velocity n V , attitude parameters through the direction cosine matrix n b C , gyro bias g Δ , accelerometer bias a Δ and errors in DVL. The DVL measurement error is mainly caused by the scale factor error k and the misalignment error ε , both of which can be regarded as an constant during the mission. The system equations can then be presented as [25,26]: where b nb  is the angular rate of the navigation frame relative to the body frame; b ib ω is the angular rate of the inertial frame relative to the body frame; (2)

Observation Equations
The velocity measurements d V from DVL in the Doppler instrument frame d can be expressed as follows: where , , is the true value of the velocity of DVL, k is the scale factor error, d δV presents the Gaussian white noise.
Therefore, the observation equation can be expressed as: where b d C is the misalignment matrix between INS and DVL. It is the skew matrix of the misalignments ε .

Parameter Calibration Algorithm
The main advantage of the online calibration method proposed in [24] is that no external sensors are required. However, it requires the AUV to operate complex maneuvers. Generally, AUVs travel in a straight path at a constant velocity. Although the scale factor error and the misalignments can be chosen as the Kalman filter states for the INS/DVL integrated navigation system and hence estimated on line, it should be noted from the observability analysis that not all of the states are observable under that sailing condition [25][26][27][28][29]. Therefore, a novel parameter calibration method is proposed.

Formulas of the Proposed Method
It can be guaranteed that the misalignments ε are reduced to a small value during manufacture. Since the velocity of DVL in the lateral direction v d y and the up direction v d z are miniature, ignore the influence of the roll error ɛ x of the misalignments. Therefore, only the scale factor error k, the yaw misalignment error ɛ y , the pitch misalignment error ɛ z are considered.
Applying Equation (4), the DVL measurements can be expressed as: Rearranging gives: Ignoring the influence of the small products, so: Rearranging Equation (7) gives: During the process of calibration voyage, the AUV travels in a straight path at a constant velocity. Therefore, the roll γ and the pitch Ɵ remain small: cos sin 1 sin cos where  denotes the yaw angle.
Substituting Equation (9) into Equation (8) gives: where Rearranging gives: sin cos sin cos 00 cos sin cos sin The misalignments ε can be regarded as small values, so: From Equation (13), the following equation can be obtained: Both parts of the Equation (14) are integrated to yield: From Equation (13): Both parts are integrated to yield: Dot-multiplying both of its parts by Since: Substituting this into Equation (18) Dot-multiplying both parts of Equation (17) Since: Substituting this into Equation (21), the scale factor can be calculated as follows: Supposing the AUV travels in a straight path at a constant speed during   where L G (t 0 ) and L G (t 1 ) are the geographic latitude obtained by GNSS at t 0 and t 1 respectively; λ G (t 0 ) and λ G (t 1 ) are the geographic longitude obtained by GNSS at t 0 and t 1 respectively; L D (t 0 ) and L D (t 1 ) are the geographic latitude obtained by INS/DVL integrated navigation system at t 0 and t 1 respectively; λ D (t 0 ) and λ D (t 1 ) are the geographic longitude obtained by INS/DVL integrated navigation system at t 0 and t 1 respectively; R is the radius of the Earth. That is to say, the scale factor error k, misalignment yaw error ɛ y and pitch misalignment error ɛ z can be obtained only with the positions of INS/DVL integrated navigation system and GNSS at t 0 and t 1 in the case that the AUV travels in a near straight path at a constant speed.

An Iterative Implementation
The attitude error caused by INS initial alignment is a key factor which limits the accuracy of the calibration. In [30,31], the methods of INS initial alignment for AUV are presented. In order to reduce the effects of the INS initial alignment, an iterative implementation is proposed as follows (shown in  (1) and (2), the scale factor error k 0 , misalignment yaw error ɛ y0 and pitch misalignment error ɛ z0 can be obtained according to Equations (15), (20) and (23). (4) The estimated scale factor and misalignment parameters are used in the subsequent navigation.

Simplified INS/DVL Integrated Navigation System
Once the estimated parameters are fixed, state equations and observation equations of the INS/DVL integrated navigation system can be simplified.

Simplified INS/DVL Integrated Navigation System State Equations
After parameter calibration, the states of the Kalman filter can be reduced to: velocity error δV , attitude misalignments ψ , gyro bias g Δ , accelerometer bias a Δ . Thus, the INS partition of the state vector comprises the following 12 states: The system state equation in a matrix form is given by: where ( ) t W denotes the Gaussian white noise. The system matrix F(t), expressed in term of 3 × 3 submatrices corresponding to the components of the state vector in Equation (29), is: 11 12 3 3  14   21  22  23  3 3   3 3  3 3  3 3  3 3   3 3  3 3  3 3  3 where: where cos

Simplified INS/DVL System Observation Equations
The accuracy of the INS/DVL integrated navigation system is mainly caused by the inertial navigation attitude error, misalignments between INS and DVL and the velocity scale factor error of DVL, so: Differencing the velocity of INS and DVL gives: Once the scale factor and misalignments are determined by parameter calibration, the misalignment matrix b d C between INS and DVL can be fixed. Based on velocity measurements d V from DVL and the scale factor error, the true DVL velocity V d is: Combing Equations (39) and (40), the observation model is given by: where: and v(t) is the vector of the zero-mean Gaussian white noise.
Once the system state equations and observation equations are fixed, using the Kalman filter referred in [32,33], the states can be estimated.

Experimental Results and Discussions
Both river trials and sea trials were carried out to evaluate the performance of the proposed method. For this, an high performance INS Kit is designed. The INS Kit is a fully qualified inertial navigator that is based on three Ring Laser Gyros(RLG) produced by Huatian Photoelectron and INS Technology Co., Ltd., Changsha, China and three quartz accelerometers offered by Beijing StarNeto Technology Co., Ltd., Beijing, China. Inertial sensors specifications are shown Table 1. The INS Kit modular architecture allows for various on board aiding devices such as GNSS, Doppler Velocity Log and Depth Sensor. The primary navigation aiding sensors are shown in Table 2.  The bottom-locked Doppler sensor HEU DVL produced by Harbin Engineering University could provide three-axis transformation velocities. The INS Kit and DVL modular are shown in Figure 3.

The River Trial
In the river trial, the devices were fixed on a ship. DGPS positioning results were employed as the benchmark. Four suiets of INS were fixed on the deck of a ship. They were marked with N1, N2, N3 and N4 respectively. The DVL modules were put 1 m underwater.

River Trial Experimental Results
A near straight trajectory which is about 8 km was chosen for parameter calibration. The trajectory and forward velocity of the vessel in the river trial are shown in Figure 4.  Table 3. Table 3. Calibrated parameter estimates in the river trial. With calibrated parameter estimates from Table 3 and the positions of the forth dot (shown in Figure 4), the final position errors are calculated in Table 4.   Table 4, with calibrated parameter estimates, the final position errors of INS/DVL integrated navigation are within 5 m in the 7 km distance travelled. The accuracy of INS/DVL integrated navigation system is better than 1‰D (distance travelled). If the accuracy of INS/DVL integrated navigation system is bigger than 1.5‰D, with the positions of the third and the forth dot, the calibration parameters were updated with the iterative implementation.

Validation of the Calibrated Parameters Estimates in the River Trial
In order to evaluate the performance of the calibrated parameter estimates, another test was done. The calibrated parameter estimates were employed in the INS/DVL integrated navigation. During the experiment, the simplified INS/DVL integrated navigation scheme proposed in Section 4 is used. The trajectory of the vessel is shown in Figure 5. Comparing with the positions obtained from GNSS, the on-line experimental results are shown in Table 5.  Three experiments were carried out in the sea trial.

Experiment 1
In Experiment 1, after initial alignment, the AUV surfaced to receive DGPS measurements at the surface for a short period of time to calibrate the position error of INS/DVL integrated navigation system. Then the AUV submerged to travel a straight line approximately 17 Km in length, equivalent to about 2 h at nominal speed. The depth varies from 60 to 180 m. The trajectory obtained by INS/DVL integrated navigation system is shown in Figure 6. During the process of the experiment, the AUV surfaced twice to receive the GNSS signals. Comparing with the positions obtained from GNSS, the accuracy of the INS/DVL integrated navigation system is shown in Table 6. The accuracy of the INS/DVL integration is about 1.76‰D. In Experiment 2, the AUV travelled for about 120 km. The depth varies from 60 to 180 m. The trajectory obtained by INS/DVL integrated navigation system is shown in Figure 7. During the process of the experiment, the AUV surfaced five times to receive the GNSS signals.  Comparing the positions obtained from GNSS, the accuracy of the INS/DVL integrated navigation system is shown in Table 7. The accuracy of the INS/DVL integrated navigation system is within 3‰D.

Experiment 3
In Experiment 3, the AUV travelled for about 90 km. The depth varies from 60 to 180 m. The trajectory obtained by INS/DVL integrated navigation system is shown in Figure 8. During the process of the experiment, the AUV surfaced three times to receive the GNSS signals.  Comparing the position obtained from DGNSS, the accuracy of the INS/DVL integrated navigation system is shown in Table 8. The accuracy of the INS/DVL integrated navigation system is within 3‰D. According to the experimental results in Table 6-8, the INS/DVL integrated navigation system has reached the accuracy of about 2‰D (CEP) with the calibrated parameter estimates obtained by the proposed method.

Conclusions
In order to meet the requirements of the AUV, an INS/DVL integrated navigation method has been designed. As the scale factor of DVL and misalignments between INS and DVL are the key factors which limit the accuracy of the INS/DVL integrated navigation, a novel parameter calibration method has been proposed. With this method, it is needless to receive GNSS signals continuously, making this method suitable for AUV platforms. The proposed method has been evaluated with both river trial and sea trial data sets. With the calibrated parameter estimates, INS/DVL integrated navigation can reach the accuracy of about 1‰ of the distance travelled (CEP) in the river trial and 2‰ of the distance travelled (CEP) in the sea trials.