AUV Positioning Method Based on Tightly Coupled SINS/LBL for Underwater Acoustic Multipath Propagation

Abstract

This paper researches an AUV (Autonomous Underwater Vehicle) positioning method based on SINS (Strapdown Inertial Navigation System)/LBL (Long Base Line) tightly coupled algorithm. This algorithm mainly includes SINS-assisted searching method of optimum slant-range of underwater acoustic propagation multipath, SINS/LBL tightly coupled model and multi-sensor information fusion algorithm. Fuzzy correlation peak problem of underwater LBL acoustic propagation multipath could be solved based on SINS positional information, thus improving LBL positional accuracy. Moreover, introduction of SINS-centered LBL locating information could compensate accumulative AUV position error effectively and regularly. Compared to loosely coupled algorithm, this tightly coupled algorithm can still provide accurate location information when there are fewer than four available hydrophones (or within the signal receiving range). Therefore, effective positional calibration area of tightly coupled system based on LBL array is wider and has higher reliability and fault tolerance than loosely coupled. It is more applicable to AUV positioning based on SINS/LBL.

1. Introduction

Autonomous underwater vehicle (AUV) is a tool that is competent for various underwater tasks, such as detection, attack, convey, salvage, etc. It has become an important research direction of military marine technology at home and abroad because of its wide scope, small volume, lightweight and high hidden ability. High-accuracy underwater autonomous navigation, locating and tracking technology of AUV are precondition and key to accomplish underwater tasks [1].

Currently, SINS (Strapdown Inertial Navigation System)/DVL (Doppler Velocity Log) integrated navigation is the main underwater autonomous navigation technology used in AUV in both China and foreign countries. With calibrations based on global positioning system (GPS), acoustic positioning system and magnetic compass, it can improve navigation accuracy of an AUV system effectively [2,3,4]. Acoustic positioning system can be divided into long base line, short base line and ultra-short base line systems according to distance between elements. Among them, the long base line (LBL) acoustic positioning system determines AUV position according to distance between sound source on the carrier and seabed transponder array. It has been widely used in underwater vehicles for its wide reach and high positional accuracy [5,6].

SINS/LBL integrated navigation can be divided into loosely coupled and tightly coupled in view of involved physical parameters. In some previous studies, SINS/LBL loosely coupled navigation [7,8] inhibits divergence of position error by taking difference of location information between SINS and LBL as quantity measurement of Kalman filter. Although it could increase positional accuracy of AUV effectively, the LBL could not complete the positioning when there are fewer than four available hydrophones because of carrier movement or shoal of fish. Research on SINS/LBL tightly coupled navigation is represented by Lee et al. [9] from South Korean Vessel and Marine Engineering Institute. He proposed an AUV integrated navigation system based on inertial sensor and pseudo-LBL acoustic transponder. It added pseudo-LBL system based on the SINS/DVL system. The pseudo-LBL system is composed of sound source on AUV and two hydrophones at seabed. The distance between sound source and each hydrophone is called pseudo-range. The differences between SINS derived pseudo-ranges and LBL measured pseudo-ranges are used as observation variables of Kalman filter. This method improves effectiveness, robustness and navigation accuracy of SINS/DVL integrated navigation system. The observed information increased by Lee et al. is the distance, which is obtained by the sound velocity multiplying the time of arrival (TOA) between sound source and hydrophone. Acquisition of time requires strict synchronization on time for transponder and hydrophones, which is difficult in practice. Therefore, distance measurement error is inevitable. Method based on time difference of arrival (TDOA) determines position of carrier according to time differences for sound source signal arriving to different hydrophones, which lowers requirements on time synchronization and avoids error brought by time synchronization between sound source and hydrophones. Therefore, this paper applied TDOA-based LBL acoustic system to assist SINS positioning.

TDOA is often acquired through generalized cross-correlation operation to signals receiving by two hydrophones. Since sound signal sent from sound source will make a series of refraction and reflection during the propagation process, signals received by hydrophones are actually mutual interference superposition of multipath signals, thus resulting in multiple correlation peaks with similar amplitudes. This is known as fuzzy correlation peak problem. To solve this problem, An et al. [10] proposed a defuzzification algorithm based on correlation peaks identification and stable correlation peaks tracking according to stability and distribution law of correlation peaks generated by different paths. This algorithm could correct TDOA estimation error brought by multipath propagation. However, the distribution of the correlation peak varies with the change of the water environment, so the proposed tracking algorithm may still cause estimation error.

To solve these problems, this paper put forward an AUV positioning system based on SINS/LBL tightly coupled algorithm of underwater acoustic propagation multipath. This system is composed of SINS, DVL, MCP, sound source and LBL acoustic positioning array. Hydrophones in LBL array receive sound source signal from AUV. A group of fuzzy correlation peaks could be gained from correlation operation of the received signals. These correlation peaks are distinguished through location information of SINS and assistance of LBL, thus getting TDOA. Later, TDOA is changed into slant-range difference. Difference between this slant-range difference and the slant-range difference calculated from SINS position is taken as the observation variables of tightly coupled system, thus correcting accumulative position error of AUV. This system uses SINS assisting to solve TDOA calculation difficulty caused by multipath propagation of underwater sound signal. Furthermore, using slant-range difference as an observation variable for filtering correction of system error, the system expands navigational correction area of AUV and makes navigation track more flexible.

In this paper, Section 2 introduces principle and structure of the system. Then, calculation method of TDOA of underwater acoustic propagation multipath and SINS/LBL tightly coupled model based on slant-range difference are introduced. Finally, some simulation experiments are implemented to prove effectiveness of the proposed algorithm.

2. Principle and Structure of the System

2.1. Principle of TDOA-Based LBL Underwater Positioning

LBL system consists of sound source on AUV and hydrophones at seabed. Distance between hydrophones is generally 100~6000 m (Figure 1).

Common positioning algorithms in LBL acoustic positioning system include TOA positioning algorithm and TDOA positioning algorithm. TOA positioning algorithm locates AUV according to distances from sound source to hydrophones. This algorithm is easy to operate, but requires strict time synchronization between sound source and hydrophones. The TDOA positioning algorithm locates AUV according to time difference for sound signal to arrive to different hydrophones. It avoids error caused by poor time synchronization. As a result, TDOA-based LBL underwater positioning was used in this paper. Its basic principle is introduced in the following text.

Suppose AUV sends sound signal at time $t_0$ and hydrophone i receives this sound signal at time $t_i$. Then, propagation time from sound source to hydrophone is:

$$\Delta t_i=t_i-t_0,(i=0,1,2,3)$$

(1)

Distance between sound source and hydrophone i is:

$$R_i=c\cdot \Delta t_i$$

(2)

where c is sound velocity, which is assumed a constant to simplify the analysis.

Assumed positions of hydrophones 0, 1, 2 and 3 in Figure 1 are known, which are, respectively, expressed as $P_0(x_0,y_0,z_0)$, $P_1(x_1,y_1,z_1)$, $P_2(x_2,y_2,z_2)$ and $P_3(x_3,y_3,z_3)$. Position of AUV $P(x,y,z)$ is unknown that has to be calculated. Then, distance between sound source and hydrophone i could be expressed as:

$$\sqrt{{(x-x_i)}^2+{(y-y_i)}^2+{(z-z_i)}^2}=c\cdot \Delta t_i$$

(3)

where $c$ and $\Delta t_i$ are known. Therefore, $P(x,y,z)$ can be obtained by solving the following nonlinear equation:

$$\{\begin{array}{c}\sqrt{{\left(x-x_1\right)}^2+{\left(y-y_1\right)}^2+{\left(z-z_1\right)}^2}-\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2+{\left(z-z_0\right)}^2}=c(\Delta t_1-\Delta t_0)\\ \sqrt{{\left(x-x_2\right)}^2+{\left(y-y_2\right)}^2+{\left(z-z_2\right)}^2}-\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2+{\left(z-z_0\right)}^2}=c(\Delta t_2-\Delta t_0)\\ \sqrt{{\left(x-x_3\right)}^2+{\left(y-y_3\right)}^2+{\left(z-z_3\right)}^2}-\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2+{\left(z-z_0\right)}^2}=c(\Delta t_3-\Delta t_0)\end{array}$$

(4)

Equation (4) is a hyperboloid locating equation. It reflects that LBL 3D positioning requires at least four hydrophones.

2.2. Working Principle of the System

Working principle of the system is shown in Figure 2. Firstly, hydrophones receive signals transmitted from the sound source on AUV. Generalized cross-correlation operation to signals received by hydrophones i and j ($x_i(t)$ and $x_j(t)$) was implemented. Due to refractions and reflections of sound signals during underwater propagation, a group of fuzzy correlation peaks are gained from the generalized cross-correlation operation. Next, slant-range difference between hydrophones ($\Delta R_{SINS}$) is estimated according to current AUV position information ($P_{SINS}$) of SINS/DVL/MCP system, and time difference of sound source signal arrival to hydrophone i and j ($t_{ij}^{\prime }$) is calculated. Based on $t_{ij}^{\prime }$, fuzzy correlation peaks are screened by the correlation peak screening module, getting the ideal time difference of arrival ($t_{ij}$). Later, slant-range difference based on LBL ($\Delta R_{LBL}$) is calculated from sound velocity-assisted correcting algorithm. Finally, difference between $\Delta R_{SINS}$ and $\Delta R_{LBL}$ is input into Kalman filter as an external observation variable. There are another two observation variables: velocity offered by DVL and heading angle offered by MCP. Errors of SINS are corrected based on filtering results, thus getting the final accurate AUV position ($P_{AUV}$), velocity and attitude.

3. TDOA Calculation Method of Underwater Acoustic Propagation Multipath

3.1. Fuzzy Correlation Peaks in Multipath Propagation

Underwater sound signal will make a series of refractions and reflections during propagation and signal received by hydrophones is mutual interference superposition of multipath signals. Sound signal propagation channel can be modeled as a mutual interference multipath channel [11].

Figure 3 shows a simplified underwater sound multipath propagation model, which is composed of one sound source S and two hydrophones R1 and R2. Except for the straight propagation channels P_d1 and P_d2 from sound source to hydrophones, sound signal is reflected by sea surface (P_s1 and P_s2) and seabed (P_b1 and P_b2) once.

Cross-correlation function of signals $x_1(t)$ and $x_2(t)$ received by R1 and R2 is shown in Figure 4. Sampling period is 5 × 10⁻⁶ s. Except for peaks (main peaks) at TDOA of straight paths, there are other peaks (secondary peaks). The phenomenon that there are many peaks in cross-correlation function and it is impossible to estimate TDOA of signals is called fuzzy cross-correlation peak.

3.2. SINS-Assisted Method to Search the Optimum TDOA

According to above analysis, multipath effect of sound signal propagation brings several correlation peaks with similar amplitudes in generalized cross-correlation operation. Due to high propagation speed of sound signal in water, tiny time difference error will cause large slant-range difference error. Using the largest peak as the main correlation peak will surely cause large errors. Therefore, it is necessary to increase calculation accuracy of ideal time difference. Combining characteristics of underwater integrated navigation system, this paper uses SINS location information to assist LBL system to search the optimum time difference under multipath propagation conditions, thus increasing positioning accuracy of the system.

In the LBL system, set $(x_i,y_i,z_i)$ be position of the i-th hydrophone and $(x_S,y_S,z_S)$ be the AUV position (sound source is on AUV) output by SINS. Then, distance between AUV and hydrophones is:

$$R_{SINSi}=\sqrt{{(x_S-x_i)}^2+{(y_S-y_i)}^2+{(z_S-z_i)}^2}$$

(5)

The slant-range difference between the distances of any two hydrophones to the sound source is:

$$\Delta R_{SINSij}=R_{SINSi}-R_{SINSj}(i\ne j)$$

(6)

The calculated time difference of signal arrival between two hydrophones is:

$$\Delta t_{ij}^{\prime }=\frac{\Delta R_{SINSij}}{c_{ij}}$$

(7)

where $c_{ij(k)}$ is equivalent sound velocity of propagation path at the time k [12]. The influence factors of sound channel structure mainly include water temperature, salinity and depth. Comparing with the time k − 1, these factors are essentially unchanged, so the current equivalent sound velocity $c_{ij(k)}$ can be represented by the equivalent sound velocity $c_{ij(k-1)}$ of the last time cycle, which is:

$$c_{ij(k)}=c_{ij(k-1)}$$

(8)

The equivalent sound velocity is the ratio of the slant-range difference between AUV which position is output by the SINS/LBL tightly coupled system and hydrophones to the time difference. The calculation process is as follows.

Set the AUV position output by the SINS/LBL tightly coupled system at time $k-1$ as $P_{SINS/LBL(k-1)}(x_{S/L(k-1)},y_{S/L(k-1)},z_{S/L(k-1)})$, distance between AUV and hydrophones is:

$$D_{i(k-1)}=\sqrt{{(x_{S/L}-x_i)}^2+{(y_{S/L}-y_i)}^2+{(z_{S/L}-z_i)}^2}$$

(9)

The slant-range difference between AUV and any two hydrophones (i and j) is:

$$\Delta D_{ij(k-1)}=D_{i(k-1)}-D_{j(k-1)}$$

(10)

If the time difference between AUV and two hydrophones at time $k-1$ is $\Delta t_{ij(k-1)}$, then the equivalent sound velocity of propagation path is:

$$c_{ij(k-1)}=\frac{\Delta D_{ij(k-1)}}{\Delta t_{ij(k-1)}}$$

(11)

Substitute $c_{ij(k-1)}$ into Equation (7), and time difference $\Delta t_{ij}^{\prime }$ at time $k$ can be calculated. The peak closest to $\Delta t_{ij}^{\prime }$ is found out from the fuzzy correlation peaks in Figure 4. Time difference of this peak is viewed as the ideal time difference $\Delta t_{ij}$.

4. SINS/LBL Tightly Coupled Model

4.1. Establishing LBL-Based Slant-Range Difference Model

In TDOA method, signal arrival time difference between hydrophones (1, 2, 3) and hydrophone 0 could be gained through the method in Section 3.2, thus getting the corresponding slant-range difference [9,13,14] $\Delta R_i^{\prime }=R_i-R_0$ $(i=1,2,3)$:

$$\{\begin{array}{l}\Delta R_i^{\prime }=\Delta R_i+\delta R_i+{\upsilon }_{\Delta R_i}\\ \delta {\dot{R}}_i=-\frac{1}{{\tau }_{\delta R_i}}\delta R_i+{\upsilon }_{\delta R_i}\end{array}$$

(12)

$\Delta R_i$ is truth value of slant-range difference and $\delta R_i$ is error of slant-range difference, which can be expressed by the first-order Gauss-Markov process. ${\tau }_{\delta R_i}$ and ${\upsilon }_{\delta R_i}$ are correlation time and driven white noise of the first-order Gauss-Markov process. ${\upsilon }_{\Delta R_i}$ is Gaussian white noise.

4.2. State Equation and Measurement Equation of the SINS/LBL Tightly Coupled System

$$\left[\begin{array}{c}{\stackrel{•}{X}}_{SINS}\\ {\stackrel{•}{X}}_{LBL}\end{array}\right]=\left[\begin{array}{cc}{F}_{SINS}& 0\\ 0& F_{LBL}\end{array}\right]\left[\begin{array}{c}{X}_{SINS}\\ X_{LBL}\end{array}\right]+\left[\begin{array}{c}{W}_{SINS}\\ W_{LBL}\end{array}\right]$$

(13)

where $X_{SINS}$ and $X_{LBL}$ are state variables of SINS and LBL; $F_{SINS}$ and $F_{LBL}$ are SINS and LBL system matrixes; and $W_{SINS}$ and $W_{LBL}$ are system noises of SINS and LBL.

Considering the environment of AUV, velocity along the height direction and location information could not be neglected. State variable of SINS is chosen as:

$$\begin{array}{l}{X}_{SINS}=\\ {\left[\begin{array}{ccccccccccccccc}\delta V_E& \delta V_N& \delta V_U& {\phi }_E& {\phi }_N& {\phi }_U& \delta L& \delta L& \delta h& {\nabla }_{bx}& {\nabla }_{by}& {\nabla }_{bz}& {\epsilon }_{bx}& {\epsilon }_{by}& {\epsilon }_{bz}\end{array}\right]}^T\end{array}$$

(14)

where $\delta V_E,\delta V_N,\delta V_U$ are velocity errors of SINS toward east, north and up, respectively; ${\varphi }_E,{\varphi }_N,{\varphi }_U$ are misalignment angles of SINS toward east, north and up, respectively; $\delta L,\delta \lambda ,\delta h$ are SINS latitude, longitude and height errors, respectively; ${\nabla }_{bx},{\nabla }_{by},{\nabla }_{bz}$ are biased errors of accelerometers along three axial directions, respectively; and ${\epsilon }_{bx},{\epsilon }_{by},{\epsilon }_{bz}$ are three axial drifts of gyroscopes. $F_{SINS}$ can be determined by the SINS error equation.

$$X_{LBL}={\left[\begin{array}{ccc}\delta R_1& \delta R_2& \delta R_3\end{array}\right]}^T$$

(15)

where $\delta R_i$ $(i=1,2,3)$ is slant-range difference error.

$$F_{LBL}=diag(-\frac{1}{{\tau }_{\delta R_1}},-\frac{1}{{\tau }_{\delta R_2}},-\frac{1}{{\tau }_{\delta R_3}})$$

(16)

The measurement equation of tightly coupled system is:

$$Z_{SINS/LBL}=\Delta R_{SINS}-\Delta R_{LBL}=\left[\begin{array}{c}\Delta R_{SINS1}-\Delta R_{LBL1}\\ \Delta R_{SINS2}-\Delta R_{LBL2}\\ \Delta R_{SINS3}-\Delta R_{LBL3}\end{array}\right]=H_{SINS/LBL}X+{\mathsf{\eta }}_{SINS/LBL}$$

(17)

where $\Delta R_{SINSi}-\Delta R_{LBLi}$ is the slant-range difference error between SINS and LBL. ${\mathsf{\eta }}_{SINS/LBL}$ is measurement noise.

Let $\left(x_S,y_S,z_S\right)$ be the AUV position estimated by SINS. Then, slant-range difference is:

$$\Delta R_{SINSi}=\sqrt{{\left(x_S-x_i\right)}^2+{\left(y_S-y_i\right)}^2+{\left(z_S-z_i\right)}^2}-\sqrt{{\left(x_S-x_0\right)}^2+{\left(y_S-y_0\right)}^2+{\left(z_S-z_0\right)}^2}$$

(18)

In relative to the real AUV position $\left(x,y,z\right)$, it can get from Taylor linearization of Equation (18):

$$\Delta R_{SINSi}=R_i-R_0+e_{ix}\delta x+e_{iy}\delta y+e_{iz}\delta z$$

(19)

$$e_{ix}=\frac{\partial (\Delta R_{SINSi})}{\partial x}=\frac{x_S-x_i}{R_i}-\frac{x_S-x_0}{R_0}$$

(20)

$$e_{iy}=\frac{\partial (\Delta R_{SINSi})}{\partial y}=\frac{y_S-y_i}{R_i}-\frac{y_S-y_0}{R_0}$$

(21)

$$e_{iz}=\frac{\partial (\Delta R_{SINSi})}{\partial z}=\frac{z_S-z_i}{R_i}-\frac{z_S-z_0}{R_0}$$

(22)

where

$$R_i=\sqrt{{(x_S-x_i)}^2+{(y_S-y_i)}^2+{(z_S-z_i)}^2}$$

(23)

Slant-range difference of LBL is:

$$\Delta R_{LBLi}=R_i-R_0+\delta R_i+{\nu }_{\Delta R_i}$$

(24)

Therefore,

$$\Delta R_{SINSi}-\Delta R_{LBLi}=e_{ix}\delta x+e_{iy}\delta y+e_{iz}\delta z-\delta R_i-{\nu }_{\Delta R_i}$$

(25)

$\delta x,\delta y,\delta z$ in Equation (25) are expressed by $\delta \mathrm{L},\delta \lambda ,\delta h$:

$$\{\begin{array}{l}\delta x=\delta h\text{cos}L\text{cos}\lambda -(R_E+h)\sin L\text{cos}\lambda \delta L-(R_E+h)\text{cos}L\sin \lambda \delta \lambda \\ \delta y=\delta h\text{cos}L\sin \lambda -(R_E+h)\sin L\sin \lambda \delta L-(R_E+h)\text{cos}L\text{cos}\lambda \delta \lambda \\ \delta z=\delta h\sin L+\left[R_E(1-e^2)+h\right]\text{cos}L\delta L\end{array}$$

(26)

Substitute Equation (26) into Equation (25) and the measurement matrix in Equation (17) could be gained:

$$H_{SINS/LBL}=\left[\begin{array}{ccccc}{0}_{3\times 6}& H_{1(3\times 3)}& 0_{3\times 6}& -E_{3\times 3}& 0_{3\times 5}\end{array}\right]$$

(27)

where

$$H_1=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right]$$

(28)

$$E_{3\times 3}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$$

(29)

Elements in $H_1$ are:

$$\begin{array}{lll}{a}_{i1}& =& -\left(R_E+h\right)\sin L\text{cos}\lambda e_{ix}-\left(R_E+h\right)\sin L\sin \lambda e_{iy}\\ & & +[R_E(1-e^2)+h]\text{cos}L{e}_{iz}\end{array}$$

(30)

$$a_{i2}=-\left(R_E+h\right)\text{cos}L\sin \lambda e_{ix}-\left(R_E+h\right)\text{cos}L\text{cos}\lambda e_{iy}$$

(31)

$$a_{i3}=\text{cos}L\text{cos}\lambda e_{ix}+\text{cos}L\sin \lambda e_{iy}+\sin L{e}_{iz}$$

(32)

where $i(i=1,2,3)$, $R_E$ is radius of curvature in the normal plane perpendicular to meridian plane; and $e$ is ovality of the ellipsoid.

5. SINS/DVL/MCP/LBL Integration System

Figure 5 is federated Kalman filter structure of underwater integrated navigation system. It contains SINS/LBL tightly coupled sub-system, SINS/DVL sub-system and SINS/MCP sub-system.

5.1. SINS/LBL Tightly Coupled Sub-System

State equation and measurement equation of SINS/LBL tightly coupled sub-system are introduced in Section 4.2.

5.2. SINS/DVL Sub-System

(1) State equation of SINS/DVL sub-system is described as:

$$\left[\begin{array}{c}{\stackrel{•}{X}}_{SINS}\\ {\stackrel{•}{X}}_{DVL}\end{array}\right]=\left[\begin{array}{cc}{F}_{SINS}& 0\\ 0& F_{DVL}\end{array}\right]\left[\begin{array}{c}{X}_{SINS}\\ X_{DVL}\end{array}\right]+\left[\begin{array}{c}{W}_{SINS}\\ W_{DVL}\end{array}\right]$$

(33)

where the state vector of SINS ($X_{SINS}$) and system matrix $F_{SINS}$ are same as those in Section 4.2. $W_{SINS},W_{DVL}$ are system noises of SINS and DVL, respectively.

$$X_{DVL}=\left[\begin{array}{ccc}\delta V_{Dx}& \delta V_{Dy}& \delta V_{Dz}\end{array}\right]$$

(34)

where $\delta V_{Di}(i=x,y,z)$ is velocity measurement error caused by topographical changes, which could be described approximately by the first-order Gauss-Markov process:

$$F_{DVL}=diag(-\frac{1}{{\tau }_{\delta dx}},-\frac{1}{{\tau }_{\delta dy}},-\frac{1}{{\tau }_{\delta dz}})$$

(35)

where ${\tau }_{\delta di}(i=x,y,z)$ is correlation time of the first-order Gauss-Markov process.

(2) Measurement equation of SINS/DVL sub-system:

$$Z_{SINS/DVL}=H_{SINS/DVL}\left[\begin{array}{c}{X}_{SINS}\\ X_{DVL}\end{array}\right]+{\mathsf{\eta }}_{SINS/DVL}$$

(36)

where $H_{SINS/DVL}$ is measurement matrix of SINS/DVL sub-system and ${\mathsf{\eta }}_{SINS/DVL}$ is measurement noise.

Velocity difference between SINS and DVL is taken as the observation variable. Since DVL measures AUV speed in the carrier coordinate system directly, it shall transform output velocity of DVL into geographic coordinate system.

$$Z_{SINS/DVL}=(V_{SINS}^{\mathrm{n}}+\mathsf{\delta }{V}_{SINS}^n)-C_b^n(V_{DVL}^b+\mathsf{\delta }{V}_{DVL}^b)=\mathsf{\delta }{V}^n-V^n\times \mathsf{\phi }-C_b^n{V}_{DVL}^b$$

(37)

where $V_{SINS}^n$ and $\mathsf{\delta }{V}_{SINS}^n$ are truth value and error of SINS velocity in geographic coordinate system, respectively; $V_{DVL}^b$ and $\mathsf{\delta }{V}_{DVL}^{\mathrm{b}}$ are truth value and error of DVL velocity in carrier coordinate system, respectively; $V^n$ is AUV speed; $\mathsf{\phi }$ is misalignment angle; and $C_b^n$ is attitude transfer matrix from carrier coordinate to geographic coordinate.

$$Z_{SINS/DVL}=\left[\begin{array}{cccccc}1& 0& 0& 0& V_U& -V_N\\ 0& 1& 0& -V_U& 0& V_E\\ 0& 0& 1& V_N& -V_E& 0\end{array}{0}_{3\times 9}\begin{array}{ccc}-C_{11}& -C_{12}& -C_{13}\\ -C_{21}& -C_{22}& -C_{23}\\ -C_{31}& -C_{32}& -C_{33}\end{array}\right]\left[\begin{array}{c}{X}_{SINS}\\ X_{DVL}\end{array}\right]+{\mathsf{\eta }}_{SINS/DVL}$$

(38)

where $V_E,V_N,V_U$ are velocity of AUV toward east, north and up direction, respectively. $C_{ij}(i=1,2,3;j=1,2,3)$ are elements in the attitude transfer matrix $C_n^b$.

5.3. SINS/MCP Sub-System

State equation of SINS/MCP sub-system is described as:

$$\left[\begin{array}{c}{\stackrel{•}{X}}_{SINS}\\ {\stackrel{•}{X}}_{MCP}\end{array}\right]=\left[\begin{array}{cc}{F}_{SINS}& 0\\ 0& F_{MCP}\end{array}\right]\left[\begin{array}{c}{X}_{SINS}\\ X_{MCP}\end{array}\right]+\left[\begin{array}{c}{W}_{SINS}\\ W_{MCP}\end{array}\right]$$

(39)

State sector of SINS ($X_{SINS}$) and system matrix $F_{SINS}$ are same as those in Section 4.2.

Magnetic compass uses only one state variable—heading angle error:

$$X_{MCP}=\mathsf{\delta }{\mathsf{\psi }}_{MCP}$$

(40)

$\mathsf{\delta }{\mathsf{\psi }}_{MCP}$ can be set a random constant. Therefore,

$$F_{MCP}=O_{1\times 1}$$

(41)

Heading angle difference between SINS and MCP is used as the observation variable, and the measurement equation of SINS/MCP sub-system is:

$$Z_{SINS/MCP}={\widehat{\mathsf{\psi }}}_{SINS}-{\widehat{\mathsf{\psi }}}_{MCP}=H_{SINS/MCP}\left[\begin{array}{c}{X}_{SINS}\\ X_{MCP}\end{array}\right]+{\mathsf{\eta }}_{SINS/MCP}$$

(42)

where $H_{SINS/MCP}$ is measurement matrix of SINS/MCP sub-system, $H_{SINS/MCP}=\left[\begin{array}{cccc}{0}_{1\times 5}& 1& 0_{1\times 15}& -1\end{array}\right]$ and ${\mathsf{\eta }}_{SINS/MCP}$ is measurement noise.

6. Simulation and Experiment

6.1. Simulation of SINS-Assisted LBL Tracking Optimum Time Difference under Static Conditions

Hydrophones and sound source layouts are shown in Figure 6. Longitudes and latitudes of four hydrophones are (118.01°, 32°), (118.01°, 32.02°), (118.02°, 32.01°) and (118°, 32.01°), respectively. They are all places at a depth of 30 m. Position coordinates of sound source is (118.01°, 32.0925°) and its depth is 10 m.

The cross-correlation function curves of received signals by hydrophones T₁ and T₀ are show in Figure 7 Due to multiple reflections of sound signals underwater, many correlation peaks are gained from cross-correlation operation. Many correlation peaks have similar amplitude, which makes it difficult to determine which peak shows the correct time difference.

Since sound signal is reflected many times during underwater propagation, the generalized cross-correlation operation will bring many correlation peaks. In traditional algorithms, time difference corresponding to the correlation peak with the largest generalized cross-correlation function value (Point C in Figure 7) is used as TDOA of different hydrophones. It reveals from Figure 7 that TDOA with traditional method differs from truth value (Point A in Figure 7, red star) significantly, while TDOA with SINS aiding method (Point B in Figure 7, red diamond) is close to the true time difference. Thus, choosing the peak nearest to point B could get the minimum time difference error. The proposed algorithm selects the ideal time difference according to the position output by SINS/DVL/MCP system [12]. Comparisons between this improved algorithm and the traditional algorithm are shown in

Table 1. Comparison of TDOA (Time Difference of Arrival) calculation error.

TDOA of Two Hydrophones	Error of Traditional Algorithm/s	Error of SINS-Assisted Algorithm/s
$t_{T_1}-t_{T_0}$	0.0056	0.0010
$t_{T_2}-t_{T_0}$	0.0055	−0.0009
$t_{T_3}-t_{T_0}$	0.0034	0.0009

Note: $t_{T_i}-t_{T_0},(i=1,2,3)$ is TDOA between hydrophone T_i and hydrophone T₀.

and

Table 2. Comparison of slant-range difference calculation error.

Slant-Range Difference between Two Hydrophones and Sound Source	Error of Traditional Algorithm/m	Error of SINS-Assisted Algorithm/m
$R_{T_1}-R_{T_0}$	8.0626	1.4379
$R_{T_2}-R_{T_0}$	8.8442	−1.2836
$R_{T_3}-R_{T_0}$	5.0872	1.3466

Note: In Table 2, $R_{T_i}-R_{T_0},(i=1,2,3)$ is difference between sound source distance to the hydrophone T_i and sound source distance to the hydrophone T₀.

. The improved algorithm solves interference of multipath effect to the optimum TDOA estimation and has higher TDOA calculation accuracy than the traditional algorithm.

6.2. Simulation of SINS/LBL Tightly Coupled Algorithm

To study effectiveness of the proposed SINS/LBL tightly coupled algorithm and effect of the number of available hydrophones on positioning accuracy, simulation experiments of SINS combining with 5, 4 and 2 hydrophones are carried out. In Figure 8, five hydrophones are installed at T₀(118.005°, 32.005°), T₁(118.005°, 32°), T₂(118.01°, 32.005°), T₃(118°, 32.01°) and T₄(118.005°, 32.01°), at a depth of 30 m. AUV depth is 10 m. From the initial position (118.001°, 32.0085°), AUV travels at a constant speed (1 m/s) along 135° north by east. The random drift and constant drift of the gyroscope are both 0.04 °/h. Random drift and constant drift of the accelerometer are $50\mathsf{\mu }\mathsf{g}$ and $100\mathsf{\mu }\mathsf{g}$, respectively. The initial misalignment angles are: 0.01° pitching angle, 0.01° angle of roll and 0.1° heading angle. DVL velocity error is 0.1 m/s. Heading angle error of MCP is 0.3° and the simulation time is 1200 s. In the simulation, the number of hydrophones is set through software. When SINS combines with five hydrophones, it uses T₀, T₁, T₂, T₃ and T₄. When SINS combines with four hydrophones, it uses T₀, T₁, T₂ and T₃. When SINS combines with two hydrophones, it only uses T₂ and T₃. Simulation results are shown in Figure 9 and

Table 3. Position errors of SINS/LBL tightly coupled algorithm.

	No LBL Assistance	2 Hydrophones	4 Hydrophones	5 Hydrophones
Mean /m	4.8300	2.5760	1.5420	1.1900
Variance /m	2.1890	1.1200	0.7039	0.6336

.

It can be seen in Figure 9 and Table 3 that when four hydrophones are installed at seabed, LBL provides three slant-range differences to form a tightly coupled system with SINS. Position error of SINS/LBL tightly coupled system is small (<4 m) and convergent. At this moment, navigation performance is satisfying. When there are five hydrophones, position error is almost equal to that when there are four hydrophones, indicating that in this situation increase of hydrophones does not improve positioning accuracy significantly. When there are two hydrophones, LBL provides only one slant-range difference for SINS. Position error is large and fluctuates violently. However, it could still compensate for position error compared to the system without hydrophones. This technology could ensure that AUV can compensate for position error quickly when it just enters into the LBL array.

6.3. Simulation of AUV Dynamic Positioning Based on Federated Filter System

Simulation of AUV dynamic positioning was implemented in order to further verify effectiveness of SINS/LBL tightly coupled algorithm. Four hydrophones are installed at seabed. They are put at T₀(118°, 32.01°), T₁(118.01°, 32°), T₂(118.02°, 32.01°) and T₃(118.01°, 32.02°) at a depth of 30 m. Suppose AUV travels from the initial position (117.98°, 32.036°) at a constant speed (1 m/s) along 135° north by east. Rest simulation conditions are same as above and simulation time is set 3 h.

In the simulation, transmission range of sound source is 1.5 km. From 2000~3300 s, AUV approaches the hydrophone array, so T₂ and T₃ could receive sound signal. From 3300~4700 s, AUV enters into the hydrophone array and all four hydrophones could receive sound signal. From 4700~6000 s, AUV departs away from the hydrophone array gradually, when only T₀ and T₁ could receive sound signal. After 6000 s, AUV leaves. Simulation results are shown in Figure 10 and Figure 11.

Figure 10 is SINS/LBL dynamic simulation results. When AUV approaches the hydrophone array, movement track of SINS/LBL tightly coupled system deviates from the real track slightly, while movement track of SINS/LBL loosely coupled system deviates significantly. When AUV is at center of the hydrophone array, movement tracks of both SINS/LBL tightly and loosely coupled systems are similar with the real track. When AUV begins to depart away from the hydrophone array, movement track of SINS/LBL tightly coupled system keeps a small error with the real track for a while, while movement track of the SINS/LBL loosely coupled system deviates from the real track gradually.

Figure 11 shows position error comparison of SINS/LBL dynamics simulations. It reflects position errors of two algorithms during the whole process intuitively. From 0~2000 s, AUV is far away from the hydrophone array. During this period, it is impossible to use LBL to compensate position error and the system only involves SINS/DVL/MCP. As a result, position error accumulates gradually, reaching 8 m. From 2000~3300 s, AUV approaches the signal area of the hydrophone array gradually. T₂ and T₃ provide one slant-range difference for SINS, which corrects position error of AUV to a certain extent. Position error decreases to 5 m. Since SINS/LBL loosely coupled algorithm requires at least four hydrophones, it is invalid during this period and position error continues to accumulate, reaching 10 m. From 3300~4700 s, AUV enters into the hydrophone array completely and all four hydrophones could receive sound signals. LBL provides three slant-range differences, which reduces position error quickly. Meanwhile, loosely coupled algorithm is effective and inhibits divergence of position error. Therefore, both two algorithms achieve equivalent position error accuracy, less than 4 m. From 4700~6000 s, AUV departs away from the hydrophone array and only T₀ and T₁ could receive sound signals. Although LBL provides only one slant-range difference, the SINS/LBL integrated algorithm still could correct position error to a certain extent. Position error is controlled within 5 m. During this period, loosely coupled algorithm could not work normally and has the larger position error (7 m). After 6000 s, AUV leaves the hydrophone array completely and position information is provided by SINS/DVL/MCP. It reveals that there is still effective compensation of position error.

7. Conclusions

This paper puts forward a SINS/LBL tightly coupled navigation system to solve position error accumulation of AUV. It uses slant-range difference as an observation variable for filtering correction of system error. Moreover, to solve fuzzy correlation peaks caused by underwater propagation multipath of LBL sound signal, a SINS-assisted LBL method to search the optimal TDOA is proposed. This method is effective and easy to operate. Test results demonstrate that the SINS/LBL tightly coupled system is more reliable than SINS/LBL loosely coupled system and could still correct position error when there are fewer than four available hydrophones. It expands navigational area of AUV and makes navigation track more flexible.

All simulations in this paper assume that undersea hydrophones are laid in a diamond shape and the hydrophone line up is single, and the next step could be studying different ways hydrophones line up to verify the effectiveness of the SINS/LBL tightly coupled algorithm. Due to the limitations of experimental conditions, the algorithm has not yet been verified in actual conditions. If the conditions are met, we will do further work in the future.