A Low-Cost and High-Precision Underwater Integrated Navigation System

Abstract

The traditional underwater integrated navigation system is based on an optical fiber gyroscope and Doppler Velocity Log, which is high-precision but also expensive, heavy, bulky and difficult to adapt to the development requirements of AUV swarm, intelligence and miniaturization. This paper proposes a low-cost, light-weight, small-volume and low-computation underwater integrated navigation system based on MEMS IMU/DVL/USBL. First, according to the motion formula of AUV, a five-dimensional state equation of the system was established, whose dimension was far less than that of the traditional. Second, the main source of error was considered. As the velocity observation value of the system, the velocity measured by DVL eliminated the scale error and lever arm error. As the position observation value of the system, the position measured by USBL eliminated the lever arm error. Third, to solve the issue of inconsistent observation frequencies between DVL and USBL, a sequential filter was proposed to update the extended Kalman filter. Finally, through selecting the sensor equipment and conducting two lake experiments with total voyages of 5.02 km and 3.2 km, respectively, the correctness and practicality of the system were confirmed by the results. By comparing the output of the integrated navigation system and the data of RTK GPS, the average position error was 4.12 m, the maximum position error was 8.53 m, the average velocity error was 0.027 m/s and the average yaw error was 1.41°, whose precision is as high as that of an optical fiber gyroscope and Doppler Velocity Log integrated navigation system, but the price is less than half of that. The experimental results show that the proposed underwater integrated navigation system could realize the high-precision and long-term navigation of AUV in the designated area, which had great potential for both military and civilian applications.

1. Introduction

In the field of marine-related research, the cooperation between an unmanned surface vehicle (USV) and an autonomous underwater vehicle (AUV) is widely used in both civilian and military strategic fields [1,2,3,4]. For the search mission for underwater targets, the collaborative operation mode of USV range coarse scanning and AUV approach reconnaissance has the advantages of a high search efficiency, flexible deployment and real-time information transmission. The underwater environment has characteristics such as strong closure and complexity, so the underwater navigation system is the key to AUVs executing underwater navigation tasks [5,6,7]. The AUV navigation method can be subdivided into five main branches: dead reckoning, vision-aided navigation, sonar-aided navigation, map matching navigation and acoustic navigation.

1.1. Dead Reckoning

A typical dead reckoning system is an SINS/DVL integrated navigation system. When the AUV starts from a determined initial position, a Strap-down Inertial Navigation System (SINS) is used to measure the vehicle’s attitude, a Doppler Velocity Log (DVL) is used to measure the carrier’s velocity and then the current position at that time is calculated. Liu et al. [8] proposed a Doppler shift-aided coupling method of an SINS/DVL integrated navigation-based dual adaptive factor and used a chi-square detection-aided dual factors’ adaptive filter to suppress the outliers. Yuan et al. [9] proposed a new federated filtering algorithm based on the Sage–Husa adaptive Kalman filter to eliminate the influence of unknown or time-varying statistical characteristics. Shaukat et al. proposed a method for improving underwater navigation and vehicle positioning by using a multi-layer perceptron neural network trained using backpropagation. This method utilizes neural networks to enhance the noise processing ability of Kalman filtering for state estimation, which proposes a new improved underwater positioning algorithm. Guo et al. [10] proposed a square-root unscented information filter (SR-UIF) to solve the problem of low accuracy for the regular filter algorithm in SINS/DVL integrated navigation. For filtering algorithms, accuracy, operational speed, and adaptive adjustment ability are particularly important indicators. Jin et al. [11] proposed a novel data-driven approach enhancing a DVL/SINS integrated navigation system, by building a virtual beam predictor based on multi-output least-squares support vector regression (MLS-SVR), to improve the robustness and accuracy of UUV navigation with limited DVL beams. Zhang et al. [12] proposed a long short-term memory extended exponential weighted Kalman filter (LSTM-EEWKF) algorithm assisted by a long short-term memory (LSTM) neural network. Qin et al. [13] proposed a robust interactive multiple model (RIMM) algorithm to improve the performance of an INS/DVL integrated navigation system under a complex measurement environment. The existing filtering algorithms often have the problem of a high theoretical accuracy, but the computational complexity is large and difficult to apply in engineering practice.

1.2. Vision-Aided Navigation

Vision-aided navigation for AUV can generally be classified into two applications: visual SLAM and artificial mark recognition. Carrasco et al. [14] presents the application of an evolved stereo Graph-SLAM algorithm especially designed for underwater environments to improve the localization, navigation and control of the SPARUS II AUV. Stereo Graph-SLAM significantly improves positioning data due to additional pose constraints calculated from visual (stereo) loop closing. Drop et al. [15] proposed a navigation method for autonomous underwater robots based on visual odometry for small-scale motion. This method is based on the SLAM algorithm for stereo images and a loop detection algorithm. The operation of the position recognition algorithm is based on using a virtual network for coordinate binding, which is built during the vehicle movement. Xin et al. [16] proposed an end-to-end network for SLAM preprocessing in an underwater low-light environment to achieve low-light image enhancement. Xu et al. [17] integrated point features and object features to construct semantic landmarks. This proposed method can improve the performance of ORBSLAM2 in underwater scenarios. Most visual SLAM algorithms extract descriptors for matching after obtaining feature points of the video stream or use the sparse optical flow method for matching feature points before and after frames. After matching, the ICP and PnP methods are used to calculate the pose transformation matrix between frames, and some even use BA to optimize the pose transformation. Then, the pose changes are integrated, and position errors will gradually accumulate. Therefore, the above methods consider setting “landmarks” such as keyframes and eliminating accumulated errors through loop detection.

Jung et al. [18] designed artificial landmarks for camera positioning and proposed a new vision-based object detection technology, which is applied to the map-based Monte Carlo localization (MCL) algorithm. In the image processing step, a new correlation coefficient weighted sum, multi template-based target selection, and color-based image segmentation method were proposed to improve traditional methods. Duecker et al. [19] uses a camera to recognize the AprilTag marking system and obtain complete relative 3D pose information underwater. Yu et al. [20] proposed an ArUco code location determination method for AUV charging, based on deep learning and the law of refraction. Most of the above methods have been tested only in clean pools and simulated environments. While most water bodies have a high turbidity and short visual distance, it will result in a small viewing range for the camera.

1.3. Sonar-Aided Navigation

Sonar-aided navigation will not be affected by water turbidity. Leonard et al. [21] used the data obtained by a high-resolution array forward-looking sonar to extract feature points from the sonar image as features of the SLAM algorithm. They used the differential GPS in the experiment as a reference. The experimental results proved that the positioning results of the SLAM algorithm are better than those of INS/DVL. Palmer et al. [22] proposed an underwater SLAM algorithm based on a multi beam sonar. This algorithm combines the measurement of the seabed strip profile with the calculation of trajectory positioning results and implements the underwater SLAM algorithm based on point cloud registration considering uncertainty. The accuracy of the algorithm is verified based on the sea trial data of Girona 500. Daniel et al. [23] proposed an algorithm that uses acoustic shadow features to achieve side-scan sonar image matching. This algorithm achieves feature matching based on the geometric distribution characteristics of shadows in side-scan sonar images and their corresponding key position information. However, when the sonar detection direction is large, this matching method breaks down when the amplitude changes, causing the acoustic shadow to change significantly. Khater et al. [24] proposed a side-scan sonar image matching method based on SUSAN and Harris corner information. This method can achieve better matching results when the sonar image features are stable and uniform. However, usually, there are a large number of shadows and distortions on sonar images, and features are scarce and difficult to extract. Therefore, the generalization performance of this method is insufficient, and its application space in underwater scenes is limited. Zhou et al. [25] proposed using feature descriptors and style transfer methods based on deep learning to match side-scan sonar images. The experimental results show that even if the learned descriptors are trained based on optical image datasets, the matching effect is poor. It goes beyond traditional manual feature-based descriptors such as SIFT. When the style transfer method is introduced, the matching effect of side-scan sonar images is further improved. This research can provide a reference for underwater side-scan sonar image matching based on deep learning.

1.4. Matching Navigation

Matching navigation is a method that uses the inherent physical properties of the ocean such as gravity fields [26], magnetic fields [27], and seafloor terrain features [28] to perform positioning. The traditional matching algorithms are divided into two categories: single-point iteration represented by the Sandia Terrain-aided Navigation System (SITAN) and sequence matching represented by Terrain Contour Matching (TERCOM) and Iterative Closest Contour Point (ICCP). Zhao et al. [29] proposed a novel domain-center adaptive-transfer matching method to improve the matching efficiency and out-of-domain positioning reliability of underwater gravity matching navigation. Wang et al. [30] proposed a novel computationally efficient outlier-robust CKF-based matching algorithm to solve outlier interferences. Liu et al. [31] proposed a new gravity matching algorithm based on comprehensive features matching (CFM) to evaluate the similarity correlation between gravity measurement sequences and reference maps. Li et al. [32] proposed a new compensation method for an underwater magnetic vector measurement system that innovatively uses the geomagnetic total field as a benchmark. Liu et al. [33] proposed a fuzzy particle filter that dynamically estimates the variance of particle distribution under terrain gradients using fuzzy logic. Simulation experiments have shown that the algorithm still has good stability under various ocean current disturbances in the Arctic. However, this method must first conduct environmental mapping of each sea area, store it inside the AUV and compare it with the physical attributes collected by the sensor in real time during the navigation process.

1.5. Acoustic Navigaation

Acoustic navigation systems are generally categorized into three classes, i.e., Long Baseline (LBL) systems, Short Baseline (SBL) systems and Ultra Short Baseline (USBL) systems. LBL systems require the installation of acoustic beacons or acoustic signal responders with baseline distances ranging from hundreds of meters to several kilometers on the seabed, making deployment and retrieval difficult. SBL systems typically install three or more transducers on board the ship to form a transducer array, and the distance between the elements of the array needs to be strictly calibrated. USBL systems are installed in a transceiver to form an acoustic array. Typically, the distance between the acoustic array units is only a few centimeters, which is cheap and easy to install. In addition to the aforementioned systems, researchers have also proposed many different underwater acoustic positioning systems. Shi et al. [34] proposed the Acoustic Synthetic Baseline (ASBL) positioning technology, which can achieve target positioning through multi-frame acoustic distance estimates between the measured target and the underwater datum. He et al. [35] proposed an improved random drift particle swarm optimization (RDPSO)-aided approach for identifying the system-level installation parameters of SINS/USBL, leading to a good performance of the SINS/USBL integrated navigation system. Wang et al. [36] proposed an augmented underwater acoustic navigation with systematic error model based on a seafloor datum network to correct time delays and time-varying sound speed errors. Acoustic navigation systems are usually used as auxiliary methods for other navigation systems. The computational complexity and accuracy of the navigation system determine the efficiency and accuracy of underwater tasks. At the same time, AUVs are facing the requirements of swarm, intelligence, and miniaturization for military use, as well as the requirement of economy for civilian use. However, most underwater navigation systems of AUV use an optical fiber gyroscope (FOG) as the main equipment, DVL as the auxiliary equipment and a high-dimensional Kalman filter as the algorithm of the integrated navigation system [37]. It is obvious that mainstream underwater integrated navigation systems are expensive, bulky and heavy and cannot meet the growing requirements for military and civilian purposes.

The Micro-Electro-Mechanical Systems (MEMS) Inertial Measurement Unit (IMU) has the characteristics of a low price, small volume and light weight [38], while lightweight and inexpensive DVLs have also emerged in recent years. The integrated navigation system of MEMS IMU and DVL is also widely used, but due to the lack of auxiliary equipment to provide global positioning information, there is inevitably a problem of the navigation errors gradually accumulating. For the collaborative working mode of USV and AUV, a USBL known as underwater GPS is added, which can effectively eliminate accumulated errors in underwater positioning [39], useful in underwater navigation.

In this paper, we designed an integrated navigation system with a low cost, light weight, small volume and low computational complexity, which was mainly based on MEMS IMU, supplemented by DVL and USBL. Through correcting the sensor and fusing the data, a low-dimensional Extended Kalman filter was constructed to reasonably describe the system. According to the lake trial experiment, the performance of the system was the same as that of the expensive FOG/DVL integrated navigation system, whose average position error was 4.12 m, maximum position error was 8.53, average velocity error was 0.027 m/s and average yaw error was 1.41°. Its low price, light weight, small size, low computational complexity and high accuracy make it suitable for use without barriers in the vast majority of AUVs.

2. Sensor Devices of the Underwater Navigation System

IMU, DVL and USBL are the main equipment of the proposed underwater navigation system, and their data calculation and installation layout are key points of the system.

2.1. IMU

The underwater navigation system of an AUV usually uses a fiber optic gyroscope as the main navigation equipment. A fiber optic gyroscope utilizes the Sagnack effect to have a high accuracy, but its large volume and heavy weight make it difficult to apply to an AUV below 180 mm in diameter, and its expensive price also limits its use in underwater navigation. MEMS IMUs have significant advantages over a fiber optic gyroscope in terms of power consumption, weight and volume. Some MEMS IMUs carry magnetometers for auxiliary calibration, which have an accuracy level comparable to that of a fiber optic gyroscope. Some commercial MEMS IMUs can not only output the angular velocity and linear acceleration but also integrate magnetic information to output the directional angle, greatly improving its accuracy and convenience.

For some MEMS IMUs that cannot directly calculate the attitude angle, linear complementary filtering can be used for attitude calculation [40]. The stable attitude angle can be obtained by means of a three-axis accelerometer and three-axis magnetometer without an accumulated error. The calculation formula of attitude angles can be shown by:

$$\left\{\begin{array}{l}{\theta }_m=\mathit{arcsin}(\frac{-a_{x_{b}m}}{g})\\ {\varphi }_m=\mathit{arctan}(\frac{a_{y_{b}m}}{a_{z_{b}m}})\\ {\psi }_m=\mathit{arctan}(\frac{m_{y_{b}m}\mathit{cos}{\varphi }_m-m_{z_{b}m}\mathit{sin}{\varphi }_m}{m_{x_{b}m}\mathit{cos}{\theta }_m+m_{y_{b}m}\mathit{cos}{\varphi }_m\mathit{sin}{\theta }_m+m_{z_{b}m}\mathit{cos}{\varphi }_m\mathit{sin}{\theta }_m})\end{array}\right.$$

(1)

The attitude angle obtained from the above equation is the prior measurement angle. The classical complementary filtering formula of attitude angles can be shown by:

$$\left\{\begin{array}{c}\widehat{\varphi }\left(k\right)=\frac{\tau }{\tau +T_s}\left(\widehat{\varphi }\right(k-1)+T_s{\omega }_{x_{b}m}(k\left)\right)+\frac{T_s}{\tau +T_s}{\varphi }_m\left(k\right)\\ \widehat{\theta }\left(k\right)=\frac{\tau }{\tau +T_s}\left(\widehat{\theta }\right(k-1)+T_s{\omega }_{y_{b}m}(k\left)\right)+\frac{T_s}{\tau +T_s}{\theta }_m\left(k\right)\\ \widehat{\psi }\left(k\right)=\frac{\tau }{\tau +T_s}\left(\widehat{\psi }\right(k-1)+T_s{\omega }_{z_{b}m}(k\left)\right)+\frac{T_s}{\tau +T_s}{\psi }_m\left(k\right)\end{array}\right.$$

(2)

where $\tau /(\tau +T_s)$ and $T_s/(\tau +T_s)$ denote the weight coefficients, and ${\omega }_{x_{b}m}, {\omega }_{y_{b}m} \mathrm{a}\mathrm{n}\mathrm{d} {\omega }_{z_{b}m}$ denote the three-axis angular velocity. The weight coefficients are generally adjusted based on the accuracy performance of the gyroscope, accelerometers and magnetometers.

2.2. DVL

As Figure 1 illustrates, DVL usually adopts a four-beam Janus configuration for the velocity measurement, which involves configuring a pair of transducers in each of the four directions before and after the DVL, which can greatly eliminate the errors caused by the carrier’s front and rear shaking. According to the Doppler frequency shift principle, the DVL velocity measurement formula can be calculated by [41]:

$$\begin{gathered} v_x=\frac{c_0(f_{d1}-f_{d3})}{4f_0\mathit{cos}\alpha } \\ {v}_y=\frac{c_0(f_{d2}-f_{d4})}{4f_0\mathit{cos}\alpha } \\ {v}_z=-\frac{c0(f_{d1}+f_{d2}+f_{d3}+f_{d4})}{8f_0\mathit{sin}\alpha } \end{gathered}$$

(3)

where $c_0$ denotes the velocity of sound waves propagating in water, $f_{d1}, f_{d2}, f_{d3} \mathrm{a}\mathrm{n}\mathrm{d} f_{d4}$ denote the frequency shift of the four beams and $f_{d1}, f_{d2}, f_{d3} \mathrm{a}\mathrm{n}\mathrm{d} f_{d4}$ denote the velocity measured by DVL. The X and Y axes are shown in the figure, and the Z axis is perpendicular to the X and Y axes.

2.3. USBL

The USBL system achieves the positioning function by calculating the relative position between the carrier and the transponder. The positioning system for the USBL uses a transmitter to transmit a fixed frequency acoustic signal to a transponder at a known location on the seabed. The transponder receives the acoustic signal and then sends an acoustic signal to the hydrophone fixed on the AUV. The system determines the tilt distance based on the time difference between the transmitted signal and the received signal and determines the azimuth angle based on the phase difference between the received signal and different hydrophones to achieve the positioning information for AUVs [42].

As shown in Figure 2, the three hydrophones are located on two perpendicular baselines (x-axis and y-axis); Among them, the hydrophone at the origin serves as the reference unit, the distance between each primitive is $d$ and ${\theta }_{mx}$ and ${\theta }_{my}$ are the angles between the transmitted (received) sound wave and the X axis and Y axis, respectively. Assuming that the round-trip time of the sound wave is $T$, the slant distance $R$ can be calculated by:

$$R=0.5c_{0}T$$

(4)

The relationship between the phase difference $R$ of the acoustic signal received by two hydrophones and the incident angle of the acoustic wave $\varphi$ can be calculated by:

$$\varphi =\frac{2\pi d}{\lambda }\mathit{cos}{\theta }_m$$

(5)

Substituting (4) for (5), the position of the transponder can be represented as:

$$\begin{gathered} X=R\mathit{cos}{\theta }_{mx}=R\frac{\lambda {\varphi }_{12}}{2\pi d} \\ Y=R\mathit{cos}{\theta }_{my}=R\frac{\lambda {\varphi }_{13}}{2\pi d} \end{gathered}$$

(6)

where ${\varphi }_{ij}(i,j=\mathrm{1,2},3)$ denotes the phase difference between the received echoes of primitive $i$ and primitive $j$. According to the experiment, the USBL system has a higher positioning accuracy in the X and Y directions but a lower accuracy in the Z direction.

2.4. Equipment Layout and Frame Definition

The layout of relevant equipment and the definition of two reference frames are shown in Figure 3.

The navigation frame $\left\{O_N{X}_N{Y}_N{Z}_N\right\}$ has its origin on the surface and its axes pointing North, East and Down (NED reference frame); the body frame $\left\{O_b{X}_b{Y}_b{Z}_b\right\}$ is centered in the center of gravity of the AUV, with the x axis pointing in the direction of the forward motion of the vehicle, the z axis pointing down and the y axis completing a right-handed reference frame [43]. The main names and corresponding symbols in the body frame are shown in

Table 1. Main names and symbols in the body frame.

Motion		Xb	Yb	Zb
linear	drift	X	Y	Z
	velocity	u	v	w
rotation	angle	$\theta$	ϕ	ψ
	angular velocity	p	q	r

.

The IMU device uses the ENU reference frame, but the DVL device and the USBL device use the NED reference frame. For convenience, the integrated navigation system uses the NED coordinate system, and coordinate conversion is required for the data collected by the IMU. The data collected by the IMU, as described below, have undergone coordinate conversion. The lever arm between the IMU and DVL is measured as $l_{DVL}(0,0,-0.05)$. In order to reduce the impact of the AUV metal shell on the USBL underwater acoustic signal, the USBL is selected to be installed at the head of the AUV, while the lever arm between the IMU and USBL is measured as $l_{USBL}(0.5,0,0.05)$. Due to the large lever arms of the USBL and IMU, the lever arm error cannot be ignored.

3. Algorithm of the Underwater Navigation System

3.1. Flow Chart of the Algorithm

The core of AUV’s integrated navigation system is the filtering algorithm. In order to achieve high-precision navigation parameter estimation, the Kalman Filter (KF) has been widely valued due to its unique advantages. The KF method has the characteristics of being high-dimensional, non-stationary and time-varying, and it is a recursive algorithm that is very suitable for implementation on computers. Therefore, since its proposal, Kalman filters have received widespread attention in the field of engineering. However, KF is only suitable for linear system models. In engineering practice, integrated navigation systems always have certain nonlinear characteristics. If KF is used for filtering calculation, it will cause model approximation errors. Therefore, some improvement methods are constantly evolving. The application of the Extended Kalman Filter (EKF) is relatively widespread. The EKF algorithm performs first-order linearization truncation on the Taylor expansion of nonlinear state functions and measurement functions, transforming nonlinear filtering into a linear filtering problem. This algorithm is computationally simple and has a fast convergence speed.

Considering that there are many matrix multiplications in the EKF algorithm, and the time complexity of the matrix multiplication algorithm is $O(n^3$), this means that as the matrix dimension increases, the computation time will increase by a power of three. Therefore, choosing a low-dimensional EKF will greatly reduce the computational complexity.

The flow chart of the algorithm is shown in Figure 4. IMU can pre-integrate its own gyroscope, accelerometer and magnetometer to directly calculate the attitude angle and velocity in the body frame. DVL provides velocity observation values after correction, and USBL provides position observation values after correction. The navigation information is ultimately output through the Extended Kalman filter.

3.2. Model Design

The Euler angles yaw, pitch and roll can be represented by $\psi$, $\theta$ and $\varphi$, respectively. From the navigation frame to the body frame, attitude changes are defined in order of the roll angle around the x axis, the pitch angle around the y axis and the yaw angle around the z-axis.

The attitude transformation matrices corresponding to each rotation are:

$$C_1^n=\left[\begin{array}{l}c\psi -s\psi 0\\ s\psi c\psi 0\\ 0 0 1\end{array}\right], C_2^1=\left[\begin{array}{l} c\theta 0 s\theta \\ 0 1 0\\ -s\theta 0 c\theta \end{array}\right]{,C}_b^2=\left[\begin{array}{l}1 0 0\\ 0 c\varphi -s\varphi \\ 0 s\varphi c\varphi \end{array}\right]$$

(7)

The attitude rotation matrix from the navigation frame to the body frame is:

$$C_b^n=C_1^n{C}_2^1{C}_b^2=\left[\begin{array}{l}c\psi c\theta (c\psi s\theta s\varphi -s\psi c\varphi ) (c\psi s\theta c\varphi +s\psi s\varphi )\\ s\psi c\theta (s\psi s\theta s\varphi +c\psi c\varphi ) (s\psi s\theta c\varphi -c\psi s\varphi )\\ -s\varphi c\varphi s\theta c\varphi s\theta \end{array}\right]$$

(8)

The navigation frame is defined as ($\xi , \eta , \zeta$), and the body frame is defined as (x, y, z). They have the following transformation relationships:

$$\left(\begin{array}{c}\xi \\ \eta \\ \zeta \end{array}\right)=\left[\begin{array}{l}c\psi c\theta (c\psi s\theta s\varphi -s\psi c\varphi ) (c\psi s\theta c\varphi +s\psi s\varphi )\\ s\psi c\theta (s\psi s\theta s\varphi +c\psi c\varphi ) (s\psi s\theta c\varphi -c\psi s\varphi )\\ -s\varphi c\varphi s\theta c\varphi s\theta \end{array}\right]\left(\begin{array}{c}x\\ y\\ z\end{array}\right)=S\left(\begin{array}{c}x\\ y\\ z\end{array}\right)$$

(9)

Considering that the depth of an AUV can be accurately measured using a depth gauge, the integrated navigation system does not need to fuse the depth data. The state variables of the IMU system can be represented as follows:

$$X=[x y u v w]$$

(10)

where, $x,y$ is the position representation of the x axis and y axis in the navigation frame, and $u,v,w$ is the linear velocity in the body frame. According to the motion equations of AUV, the equation of the state of the system can be expressed as:

$$\dot{X}=FX+W$$

(11)

where $W$ denotes the process noise, and $F$ is defined as follows:

$$F=\left[\begin{array}{l}0 0 c\psi c\theta (c\psi s\theta s\varphi - s\psi c\varphi ) (c\psi s\theta c\varphi + s\psi s\varphi )\\ 0 0 s\psi c\theta (s\psi s\theta s\varphi + c\psi c\varphi ) (s\psi s\theta c\varphi - c\psi s\varphi )\\ 0 0\hspace{1em}0\hspace{1em}\hspace{1em}\hspace{1em}0\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} 0\\ 0 0\hspace{1em}0\hspace{1em}\hspace{1em}\hspace{1em}0\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} 0\\ 0 0\hspace{1em}0\hspace{1em}\hspace{1em}\hspace{1em}0\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} 0\end{array}\right]$$

(12)

According to reference [44], the velocity measurement error of the DVL mainly comes from the installation error angle and scale factor error. If the velocity measured centered on DVL is expressed as $V_{DVL}^{raw}=[u_{DVL}^{raw},v_{DVL}^{raw},w_{DVL}^{raw}]$, we can multiply it by the scale factor $\delta k$ to obtain the corrected DVL center measurement velocity as $\delta k{V}_{DVL}^{raw}$. The center of the IMU and the center of the DVL usually do not coincide. If the angular velocity output by the IMU is expressed as ω, then the lever arm error of the DVL can be expressed as $\omega \times l_{DVL}$. Eliminating the lever arm error from the velocity measured by the DLV, we can obtain the measured velocity centered on IMU, which can be calculated as:

$${\tilde{V}}_{DVL}=\delta k{V}_{DVL}^{raw}-\omega \times l_{DVL}$$

(13)

where ${\tilde{V}}_{DVL}$ denotes the measured velocity centered on IMU, which can correct the velocity of the system. The measurement equation can be expressed by:

$$\begin{gathered} \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{Z}_{IMU/DVL}=\left[\begin{array}{c}{\tilde{u}}_{DVL}\\ {\tilde{w}}_{DVL}\\ {\tilde{v}}_{DVL}\end{array}\right]=H_{IMU/DVL}X, \\ {H}_{IMU/DVL}=\left[\begin{array}{c}0 0 1 0 0\\ 0 0 0 1 0\\ 0 0 0 0 1\end{array}\right] \end{gathered}$$

(14)

For the measured value $P_{USBL}^{raw}=[x_{USBL}^{raw}, y_{USBL}^{raw}]$ of the USBL system, it is also necessary to eliminate the lever arm error. If the lever arm between IMU and USBL is expressed as $l_{USBL}$ and the rotation matrix of the IMU output is $C_b^n$, the measurement position centered on IMU can be obtained by eliminating the lever arm error, which can be calculated as:

$${\tilde{P}}_{USBL}=P_{USBL}^{raw}-C_b^n\times l_{USBL}$$

(15)

where ${\tilde{X}}_{USBL}=[{\tilde{x}}_{USBL},{\tilde{y}}_{USBL}]$ denotes the measured position centered on IMU, which can correct the position of the system, and the measurement equation can be expressed by:

$$\begin{gathered} \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{Z}_{IMU/USBL}=\left[\begin{array}{c}{\tilde{x}}_{USBL}\\ {\tilde{y}}_{USBL}\end{array}\right]=H_{IMU/USBL}X, \\ {H}_{IMU/USBL}=\left[\begin{array}{c}1 0 0 0 0\\ 0 1 0 0 0\end{array}\right] \end{gathered}$$

(16)

3.3. Extended Kalman Filter

We linearize the equation of the state of the system, which can be rewritten in the form of an extended Kalman filter:

$$\left\{\begin{array}{l}{X}_k={\Phi }_{k-1}{X}_{k-1}+W_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$$

(17)

with:

$${\Phi }_{k-1}=I+F_{k-1}\Delta t=\left[\begin{array}{l}1 0 c\psi c\theta \Delta t (c\psi s\theta s\varphi -s\psi c\varphi )\Delta t (c\psi s\theta c\varphi +s\psi s\varphi )\Delta t\\ 0 1 s\psi c\theta \Delta t (s\psi s\theta s\varphi +c\psi c\varphi )\Delta t (s\psi s\theta c\varphi -c\psi s\varphi )\Delta t\\ 0 0\hspace{1em} 1\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}0\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} \hspace{1em}\hspace{1em}0\\ 0 0\hspace{1em} 0\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}1\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} \hspace{1em}\hspace{1em}0\\ 0 0\hspace{1em} 0\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}0\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} \hspace{1em}\hspace{1em}1\end{array}\right]$$

(18)

where $W$ denotes the process noise, subject to Normal distribution $W\sim (0,Q)$, and $V$ denotes the measurement noise, subject to Normal distribution $V\sim (0,R)$. The filtering steps of the EKF algorithm are described as follows:

$$\left\{\begin{array}{l}{X}_{k\left|k-1\right.}={\Phi }_{k-1\left|k-1\right.}{X}_{k-1\left|k-1\right.}\\ P_{k\left|k-1\right.}={\Phi }_{k-1}{P}_{k-1\left|k-1\right.}{\Phi }_{k-1}^T+Q\\ K_k=P_{k\left|k-1\right.}{H}^T(H{P}_{k\left|k-1\right.}{H}^T+R{)}^{-1}\\ X_{k\left|k\right.}=X_{k\left|k-1\right.}+K_k(Z_k-H{X}_{k\left|k-1\right.})\\ P_{k\left|k\right.}=(I-K_{k}H)P_{k\left|k-1\right.}\end{array}\right.$$

(19)

Considering that the data acquisition frequency of the DVL usually is 5–7 Hz, and that of the USBL is usually 1–2 Hz, filtering is carried out according to the principle of sequential filtering to ensure real-time filtering and reduce the calculation amount of the measurement update of the EKF algorithm [45]. The measurement equation can be written as:

$$\left[\begin{array}{c}{Z}_k^{\left(1\right)}\\ Z_k^{\left(2\right)}\end{array}\right]=\left[\begin{array}{c}{H}_k^{\left(1\right)}\\ H_k^{\left(2\right)}\end{array}\right]X+\left[\begin{array}{c}{V}_k^{\left(1\right)}\\ V_k^{\left(2\right)}\end{array}\right]$$

(20)

where each parameter is defined as follows:

$$\begin{gathered} Z_k^{\left(1\right)}=\left[\begin{array}{c}{x}_k\\ y_k\end{array}\right],\hspace{1em}\hspace{1em}{Z}_k^{\left(2\right)}=\left[\begin{array}{c}{u}_k\\ v_k\\ w_k\end{array}\right], \\ \hspace{1em} H_k^{\left(1\right)}=\left[I_{2\times 2} 0_{2\times 3}\right] ,{ H}_k^{\left(2\right)}=\left[0_{3\times 2} I_{3\times 3}\right], \\ \hspace{1em}\hspace{1em} V_k^{\left(1\right)}\sim (0,R_k^{\left(1\right)}),\hspace{1em}\hspace{1em}{V}_k^{\left(2\right)}\sim (0,R_k^{\left(2\right)}), \end{gathered}$$

(21)

If the USBL position measurement value is obtained, the first sub measurement update will be performed as follows:

$$\left\{\begin{array}{l}{K}_k^{\left(1\right)}=P_k^{\left(0\right)}\left(H_k^{\left(1\right)}{)}^T\right(H_k^{\left(1\right)}{P}_k^{\left(0\right)}(H_k^{\left(1\right)}{)}^T+R_k^{\left(1\right)})\\ {\widehat{X}}_k^{\left(1\right)}=X_k^{\left(0\right)}+K_k^{\left(1\right)}(Z_k^{\left(1\right)}-H_k^{\left(1\right)}{X}_k^{\left(0\right)})\\ P_k^{\left(1\right)}=(I-K_k^{\left(1\right)}{H}_k^{\left(1\right)})P_k^{\left(0\right)}\end{array}\right.$$

(22)

If the DVL velocity measurement value is obtained, the second sub measurement update will be performed as follows:

$$\left\{\begin{array}{l}{K}_k^{\left(2\right)}=P_k^{\left(1\right)}\left(H_k^{\left(2\right)}{)}^T\right(H_k^{\left(2\right)}{P}_k^{\left(1\right)}(H_k^{\left(2\right)}{)}^T+R_k^{\left(2\right)})\\ {\widehat{X}}_k^{\left(2\right)}=X_k^{\left(1\right)}+K_k^{\left(2\right)}(Z_k^{\left(2\right)}-H_k^{\left(2\right)}{X}_k^{\left(1\right)})\\ P_k^{\left(2\right)}=(I-K_k^{\left(2\right)}{H}_k^{\left(2\right)})P_k^{\left(1\right)}\end{array}\right.$$

(23)

Finally, the optimal estimation of the state variable is obtained as $X_k={\widehat{X}}_k^{\left(2\right)},P_k={\widehat{P}}_k^{\left(2\right)}$.

3.4. Error Analysis

IMU, DVL and USBL, respectively, provide measurement values for angle, velocity and position, and error analysis is also conducted in terms of these three aspects. Although the EKF algorithm has been used for the optimal estimation of the motion state of AUVs, analyzing their motion equations can also elucidate the problem. The motion equation of AUV is as follows:

$$\left(\begin{array}{c}\xi \\ \eta \\ \zeta \end{array}\right)=\left[\begin{array}{l}c\psi c\theta (c\psi s\theta s\varphi -s\psi c\varphi ) (c\psi s\theta c\varphi +s\psi s\varphi )\\ s\psi c\theta (s\psi s\theta s\varphi +c\psi c\varphi ) (s\psi s\theta c\varphi -c\psi s\varphi )\\ -s\varphi \hspace{1em}\hspace{1em}\hspace{1em}c\varphi s\theta \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}c\varphi s\theta \end{array}\right]Vdt$$

(24)

Using the yaw angle $\psi$ as a typical example to analyze the impact of angles on the results, assuming there is a fixed deviation angle in the yaw angle: $\tilde{\psi }=\psi +∆\psi$, where $\tilde{\psi }$ denotes the measurement value, $\psi$ denotes the true value and $∆\psi$ denotes the fixed deviation. The equation of motion can be rewritten as:

$$\left(\begin{array}{l}\tilde{\xi }\\ \tilde{\eta }\\ \tilde{\zeta }\end{array}\right)=\left[\begin{array}{l}c(\psi +∆\psi )c\theta \left(c\right(\psi +∆\psi )s\theta s\varphi -s(\psi +∆\psi )c\varphi )\left(c\right(\psi +∆\psi )s\theta c\varphi +s(\psi +∆\psi )s\varphi )\\ s(\psi +∆\psi )c\theta \left(s\right(\psi +∆\psi )s\theta s\varphi +c(\psi +∆\psi )c\varphi )\left(s\right(\psi +∆\psi )s\theta c\varphi -c(\psi +∆\psi )s\varphi )\\ -s\varphi \hspace{1em}\hspace{1em}\hspace{1em}c\varphi s\theta \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}c\varphi s\theta \end{array}\right]Vdt$$

(25)

The above equation indicates that the added deviation will rotate the trajectory of the AUV with the deviation angle $∆\psi$. If the deviation angle $∆\psi$ is a small angle, the endpoint deviation (maximum error) $dL$ will be the deviation $∆\psi$ times the total length $L$:

$$dL=∆\psi L$$

(26)

As stated in Section 3.2, the velocity measurement error of the DVL mainly comes from the installation error angle and scale factor error. The installation angle of DVL, as discussed above, will also cause the AUV’s trajectory to rotate as a whole, so here we mainly discuss the scale factor error. Assuming there is a fixed scale factor error for velocity: $\tilde{V}=\left(1+∆k\right)V$, where $\tilde{V}$ denotes the measurement value, $V$ denotes the true value and $∆k$ denotes the fixed scale factor error. The equation of motion can be rewritten as:

$$\left(\begin{array}{l}\tilde{\xi }\\ \tilde{\eta }\\ \tilde{\zeta }\end{array}\right)=\left[\begin{array}{l}c\psi c\theta (c\psi s\theta s\varphi -s\psi c\varphi )(c\psi s\theta c\varphi +s\psi s\varphi )\\ s\psi c\theta (s\psi s\theta s\varphi +c\psi c\varphi )(s\psi s\theta c\varphi -c\psi s\varphi )\\ -s\varphi \hspace{1em}\hspace{1em} c\varphi s\theta \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} c\varphi s\theta \end{array}\right]\left(1+∆k\right)Vdt$$

(27)

The above equation reveals that the scale factor error will cause the overall scaling of the AUV’s trajectory. In special cases where the starting and ending points coincide, this error will not affect the position of the endpoint. In general, the maximum error will be calculated by the following equation:

$$dL=∆kL$$

(28)

The working principle of USBL ensures that the position error generated each time will not cause a long-term impact. The position error $∆P$ is also the global maximum error. The error analysis is shown in the

Table 2. Error Analysis.

Equipment	Measurement Value	Error Source	Maximum Error
IMU	angle	deviation angle $∆\psi$	$∆\psi L$
DVL	velocity	scale factor error $∆k$	$∆kL$
USBL	position	position error $∆P$	$∆P$

:

The scale factor error from DVL can be calculated by fitting coefficients of experimental data. The position error from USBL can be reduced by setting an appropriate covariance matrix to minimize its impact. However, the angle deviation of IMU is difficult to reduce in practice.

4. Experiments and Results

In order to verify the effectiveness of the proposed underwater navigation system, we conducted experiments on the lake, as shown in Figure 5. A test ship was installed with a USBL (SeaTrac X150, Cumbria, UK) base station directly below the Real Time Kinematic GPS (RTK GPS, Shanghai, China) receiver (Sinognss M900, Shanghai, China). Another experimental ship was equipped with a mobile station of USBL (SeaTrac X110, Cumbria, UK), IMU (Xsens MTi-G-700, Enshed, The Netherlands) and DVL (WaterLinked DVL A125, Trondheim, Norway) to form an underwater integrated navigation system. The installation relationship of each piece of equipment on the ship is shown in Figure 6. The position data of the RTK GPS installed above the IMU is considered as the true data of the system. By comparing the position data received by the RTK GPS and the results of the integrated navigation system, we can quantitatively measure the performance of the system. The main equipment parameters are shown in

Table 3. Equipment parameters.

Equipment	Parameters	Values
IMU	In-run bias stability	10 °/h
	In-run bias stability	15 μg
DVL	Long-term accuracy	1.01% velocity
USBL	Range Resolution	±0.1 m
	Angular Resolution	typ 2% of Acoustic Range ×(~±1°)
RTK GPS	Horizontal Accuracy	$\pm (8+1 \times {10}^{-6}$ × D) mm

, and it is obvious that the selected equipment was of a light weight and small volume, making it suitable for use on AUVs of any diameter. It should be emphasized that the horizontal accuracy of RTK GPS is ±(8 + 1 × 10⁻⁶ × D) mm. D means the distance between the base station and rover receiver. The accuracy of the RTK GPS is high enough to use its data as true values.

The test ship conducted five experiments and sailed multiple trajectories. Considering that AUVs usually conduct routine inspections using a rectangular trajectory and fine detection using a lawnmower trajectory, these two trajectories are used as display results.

Figure 7 shows the experiment of a rectangular trajectory. In the rectangular trajectory, the duration is 2848 s, the total length is 3.2 km, the terminal deviation is 5.60 m and the ratio of the deviation to the total range is 0.18%. Figure 8 shows the experiment of the lawnmower trajectory. In the lawnmower trajectory, the duration is 4983 s, the total length is 5.3 km, the terminal deviation is 5.63 m and the ratio of the deviation to the total range is 0.11%. The result is very close to that of the most FOG/DVL integrated navigation.

As mentioned earlier, the position output by RTK GPS was taken as the true value. Figure 9 and Figure 10 show the position error between the true position and the output of the navigation system. The average position error in the rectangular trajectory is 5.45 m, while the average position error in the lawnmower trajectory is 4.12 m. It can be seen that benefiting by the global positioning function of USBL, the position error of the integrated navigation system ultimately stabilizes within a range of approximately 6 m and does not diverge.

Considering that the position error output by RTK GPS was very small, the position difference divided by the time difference could be used as the true value of velocity. Figure 11 and Figure 12 show the velocity error between the true velocity and the output of the navigation system. The jumping data in the figure are obvious abnormal data. For the sake of rigor, it is still necessary to display it. The average velocity error of the rectangular trajectory is 0.027 m/s, and the average velocity error of the lawnmower trajectory is 0.038 m/s. In most cases, the estimated velocity is very close to the true value.

Among the three directional angles of roll, pitch and yaw, the heading angle was usually the focus. The angle between the front and rear positions of RTK GPS was used as the true value of yaw. Figure 13 and Figure 14 show yaw errors between the true yaw and the output of the navigation system, with an average heading error of 2° for rectangular trajectory tests and 1.41° for detection trajectory tests. Throughout the entire process, the filtered yaw was very close to the true value, without significant angle fluctuations.

The average results of multiple experiments are shown in the

Table 4. Comparison between the proposed system and a commercial system.

Parameters	Proposed System	Parameters	Commercial System
average position error	4.12 m	position error	0.3% of covered distance
maximum position error	8.53 m
average velocity error	0.027 m/s	long-term accuracy	0.1%/±0.001 m/s
average yaw error	1.41°	heading dynamic accuracy	0.09 °/h
dimensions	IMU: 57 mm × 41.90 mm × 23.60 mm DVL: ∅ 125 mm × 30 mm USBL: ∅ 59 mm × 139 mm	dimensions	SINS: 208 mm × 275 mm × 136 mm DVL ∅ 186 mm × 203 mm
total weight	1.495 kg	total weight	8 kg
total price	about USD 40,900	total price	more than USD 100,000

and compared with a commercial SINS/DVL system. The models of SINS and DVL are BlueNaute Platinum and Nortek DVL500, respectively. Although the evaluation criteria for the accuracy of the two are different, they can still indicate the advantages and disadvantages of the two.

It can be seen that thanks to USBL, the proposed system has an average position error of 4.12 m and a maximum position error of 8.53 m, which is smaller than the long-term error of SINS/DVL. The total weight of 1.495 kg is much smaller than the 8 kg of the commercial system, and the total price of USD 40,900 is also far smaller than the price of the commercial system. The dimensions also have advantages. Although there is a significant difference between the average velocity error of 0.024 m/s and the average heading angle error of 1.41° compared to those of the commercial system, their absolute values are still within the acceptable range.

It is obvious that although the proposed integrated navigation system has some disadvantages in terms of velocity error and yaw error, its size, weight and price make it achieve a more significant balance in terms of performance and cost. Although we have successfully validated the system in the same water area as other researchers, it is necessary to explore the impact of aquatic conditions such as the depth, salinity, temperature and pressure on the system. Considering the multiple influencing factors, using analysis methods such as analysis of variance will be one of the future works.

During the integration process of the entire system, there have been instances of DVL data anomalies, which have been effectively resolved through data validation and range limitations. The proposed system has significant advantages in terms of dimensions, weight and price compared to the traditional SINS systems. The most important error, position error, can remain non-divergent for a long time. Although the velocity and angle errors are not as good as those of traditional SINS systems, they are also within the allowable range. The system is very easy to integrate and deploy on different control boards, which makes it actually run on an AUV rather than simulations.

5. Conclusions

In this paper, a low-cost and high-precision underwater integrated navigation system was proposed to meet the requirements of AUV swarm, intelligence and miniaturization, which uses low-cost, lightweight and small-volume MEMS IMU, DVL and USBL. The integrated navigation algorithm fully considered the characteristics of hardware equipment and takes the position and velocity of the AUV as the state variables. Then, according to the motion equation of the vehicle, the state equation of the Kalman filter was constructed. Considering the source of the error, the integrated navigation algorithm eliminated the lever arm error from the raw positioning information to obtain the position measurement value and eliminated the scale error and lever arm error from the raw velocity measurement information to obtain the velocity measurement value. Then, a measurement equation of the Kalman filter was constructed. In order to reduce the computational complexity of the Kalman filter measurement update, and considering the low update frequency of the USBL, the sequential filter was used to update the measurement in order to obtain high real-time position and orientation information. Finally, a lake experiment was conducted, and the results confirmed that the proposed IMU/DVL/USBL integrated navigation system could precisely estimate the position, velocity and attitude of the vehicle. This proposed integrated navigation system has advantages such as a low cost, light weight, small volume, low computational cost and high precision, and it not only meets the requirements of collaborative operations between USV and AUV but also has high potential application value in military clusters and in the civilian detection of AUVs. Future work will focus on cluster navigation, on one hand, achieving better navigation accuracy by utilizing mutual positioning between AUVs. On the other hand, long-term reliability testing will be conducted to ensure productization.