AUV Underwater Positioning Algorithm Based on Interactive Assistance of SINS and LBL

Abstract

This paper studies an underwater positioning algorithm based on the interactive assistance of a strapdown inertial navigation system (SINS) and LBL, and this algorithm mainly includes an optimal correlation algorithm with aided tracking of an SINS/Doppler velocity log (DVL)/magnetic compass pilot (MCP), a three-dimensional TDOA positioning algorithm of Taylor series expansion and a multi-sensor information fusion algorithm. The final simulation results show that compared to traditional underwater positioning algorithms, this scheme can not only directly correct accumulative errors caused by a dead reckoning algorithm, but also solves the problem of ambiguous correlation peaks caused by multipath transmission of underwater acoustic signals. The proposed method can calibrate the accumulative error of the AUV position more directly and effectively, which prolongs the underwater operating duration of the AUV.

1. Introduction

An AUV (autonomous underwater vehicle) is applied to execute all kinds of underwater tasks, including ocean exploration, underwater mine clearance and collecting bathymetry data of ocean and rivers [1,2,3,4]. In order to guarantee that underwater tasks will be completed smoothly and accurate underwater measurement data will be acquired, the AUV is required to have long-term autonomous high-precision positioning and navigation abilities and invisibility [5]. In an underwater environment, electromagnetic wave signals have the characteristic of serious attenuation. In deep sea or under an ice surface, adopting GPS and other radio positioning means cannot achieve ideal positioning effects. In order to meet the navigation requirements, DVL (Doppler velocity log) and SINS (strapdown inertial navigation system) are often used to integrate navigation [6], and the position will be estimated by dead reckoning. However, when this means is used for positioning, positioning errors will accumulate as time goes on [7]. When the AUV is performing tasks in shallow sea, it can adopt the navigation mode of “submerge, water surface calibration, submerge” to launch positioning and navigation; in other words, the AUV relies on SINS/DVL to launch positioning and navigation when navigating under water. After the AUV has been submerged under water for a certain time, in order to calibrate the accumulative errors, the AUV must emerge from the water, and the SINS/GPS integrated navigation system must be used to do the calibration [8]. Adopting this scheme can reach the goal of calibrating accumulative errors, but the AUV must be required to travel to and fro between the underwater operation position and the water surface, which will not only influence the working efficiency and increase the energy consumption, but also expose the position of the AUV. Especially when the AUV is operating in deep sea or under an ice surface, this scheme will be more impractical. Hence, it is very important to study a method in which reliable assistance positioning can be conducted for a long time underwater. This paper suggests an interactive assistance positioning method that integrates an LBL (long base line) underwater acoustic positioning system and an SINS/DVL/MCP (magnetic compass pilot) integrated navigation system, and this is a new idea for solving the above problems.

An LBL underwater acoustic positioning system, usually consisting of a seabed transponder matrix and an interrogation responder with a base length of hundreds to thousands of meters [9,10], adopts distance information between the underwater objective and the seabed matrix element to solve the target position. It can provide accurate positioning of an underwater vehicle within a local area without accumulative errors. Hence, an LBL underwater acoustic positioning system is very applicable to an underwater AUV to launch assistance positioning. Among some of the research of predecessors, Liu, Y. puts forward an underwater AUV positioning and navigation algorithm, which adopts an LBL underwater acoustic positioning system, an ADCP (acoustic Doppler current profiler) and depthometer-assisting INS [11]. Miller, P.A. et al. puts forward a tight integrated system based on LBL/DVL/INS [3]. Cheng,W.H. proposes a modification method, which is based on the periodically-measured actual navigation distance and is associated with the TOA positioning algorithm [12]. Jakuba, M.V. et al. report results for LBL acoustic navigation during autonomous under-ice surveys near the seafloor and adaptation of the LBL concept for several typical operational situations, including navigation in proximity to the ship during vehicle recoveries [13]. Eustice, R.M. et al. report recent experimental results in the development and deployment of a synchronous-clock acoustic navigation system suitable for the simultaneous navigation of multiple underwater vehicles [14]. Chen, Y.M. et al. propose a near-real-time approach to underwater inertial navigation with LBL, which uses a ping-response protocol, resulting in asynchronous measurements [15]. Although these systems have reached a certain positioning effect, there are some deficiencies. Firstly, these systems do not explain how to solve the positioning difficulty brought by the multi-path transmission of the underwater acoustic signal. In addition, acoustic velocity is distributed unevenly with the change of underwater depth, and sound ray transmission is curved, which will result in big positioning errors; additionally, the above systems have not proposed any solution.

When solving the target position, the LBL underwater acoustic positioning system can adopt the TOA (time of arrival) positioning algorithm and the TDOA (time difference of arrival) positioning algorithm. The equation set formulated by the TOA positioning algorithm can be directly transformed into a simple linear system of equations with a simple solution. However, strict time synchronization between the hydrophones and the sound source is required to measure a relatively accurate TOA value. It is very hard to do so in reality. The TDOA positioning algorithm acquires the TDOA value by conducting a generalized cross-correlation calculation of the signal received from one hydrophone and another, and then makes the positioning calculation. This method does not have to assure synchronization of the sound source and hydrophones, and the communication between them is quite simple. Hence, it is often adopted in wireless positioning. As acoustic signals will finally be in a coherence stack at one hydrophone through different paths, there will be multiple correlation peaks with approximate amplitudes in the generalized cross-correlation results, thus forming a phenomenon of ambiguous correlation peaks. Then, An, L. et al. came up with an ambiguity-solving algorithm based on underwater acoustic propagation characteristics [16]. This method, by studying the distribution rule of cross-correlation peaks forming the multipath transmission of underwater signal channels, tracks stable correlation peaks, which can effectively correct the miscalculation of the TDOA value caused by the ambiguity of the correlation peaks. However, under the condition that distances between fake peaks and the main peak do not differ that much, it is still very hard to accurately track, and the tracking error will be enlarged and finally diverge. In addition, underwater acoustic propagation channels will change as the underwater environment changes, so will the distribution rule of correlation peaks: if the former tracking strategy is still adopted, there will also be errors. Hence, the adaption of this tracking algorithm is not that strong.

In this paper, we propose an underwater positioning method based on the interactive assistance of LBL/SINS/DVL for an AUV. This positioning system consists of SINS, DVL and a sound source installed on the AUV and an LBL underwater matrix located at the seabed. The hydrophones of LBL receive signals from the sound source and conduct cross-correlation calculation and acquire the TDOA value. We adopt the hyperbolic model to solve the position of the sound source (namely, the position of the AUV), correct accumulative errors of SINS/DVL, use resolving results of SINS/DVL to assist in solving ambiguous correlation peaks when LBL is launching underwater acoustic positioning, estimate the TDOA value and improve the solution accuracy of LBL positioning. This scheme can not only directly correct accumulative errors caused by the dead reckoning algorithm, but also solves the problem of ambiguous correlation peaks caused by multipath transmission of underwater acoustic signals. Therefore, it is quite applicable to underwater positioning and navigation of an AUV.

The structure of this paper is as follows: Firstly, we introduce the principle and structure of the underwater assistance positioning system and then introduce key technologies of the system, such as underwater acoustic propagation channel modeling, the calculation of time delay differences, the Taylor series expansion algorithm, the TDOA position solution method and the interactive assistance algorithm. Finally, we verify the effectiveness of the algorithm through a simulation experiment.

2. Principle and Structure of the System

2.1. Placement and Positioning of the Hydrophone

The underwater LBL system needs to use a seabed hydrophone to confirm the position of the vehicle. The calculated position coordinates are the ones corresponding to the seabed hydrophone matrix. Hence, the hydrophone fixed on the seabed should be positioned firstly, and then, its absolute geographic position should be calculated.

As shown in Figure 1, we install the hydrophone reception matrix at the bottom of mother ship (at least three hydrophones, usually more than three), which will receive signals from the hydrophone (with the sound source) underwater and then calculate the three-dimensional position coordinates of each underwater hydrophone corresponding to the hydrophone matrix at the bottom of the mother ship according to short base line positioning principles. The GPS, IMU and compass are installed on the mother ship to provide the accurate geographic position (longitude, latitude and depth) of the mother ship, as well as the attitude angle. We combine factors, such as this information of the mother ship and the installation errors, and calculate the absolute geographic position of each hydrophone under geodetic coordinates. The underwater AUV can adopt these hydrophones (their accurate positions are already acquired) to launch the local area positioning of itself.

Figure 1. Positioning of the seabed hydrophone.

Figure 1. Positioning of the seabed hydrophone.

2.2. LBL Underwater Positioning Model Based on TDOA

TDOA positioning is a method that adopts delay inequality to perform the positioning. By measuring the time difference of a signal reaching different hydrophones, the distance difference between the signal source and different hydrophones can be acquired. As shown in Figure 2, suppose: hydrophones ${\mathrm{T}}_i(i=0,1,2)$ located at three different positions; a sound source on the AUV sends a signal, and the transmission time of the signal reaching three hydrophones is $t_i(i=0,1,2)$; the sound velocity is a steady-state value (suppose it is c); then:

$$R_2-R_0=c(t_2-t_0)=c\Delta t_{20}$$

(1)

$$R_1-R_0=c(t_1-t_0)=c\Delta t_{10}$$

(2)

$R_i(i=0,1,2)$ represents the distance between the sound source and the hydrophone ${\mathrm{T}}_i$. The above two equations respectively represent a hyperbolic curve n, which takes ${\mathrm{T}}_0$ and ${\mathrm{T}}_2$ as focal points, and hyperbolic curve m, which takes ${\mathrm{T}}_0$ and ${\mathrm{T}}_1$ as focal points; their point of intersection is the position of the sound source. As a certain error exists in the measured distance difference, there may be a condition with no solution. Hence, in view of this condition of multiple hydrophones placed on the seabed, redundant information is usually used to acquire the position closest to the actual position.

Figure 2. Schematic diagram of the TDOA positioning model.

Figure 2. Schematic diagram of the TDOA positioning model.

2.3. Working Process of the System

Figure 3 is a schematic diagram of AUV positioning based on LBL, with multiple fixed hydrophones on the seabed (there are four hydrophones in the diagram); a sound source fixed at the bottom of the AUV will send sound signals; first, accurate positioning of the hydrophones through sensors, like the GPS, IMU and compass, is performed, and the absolute geographic coordinates of each hydrophone are acquired, which is in preparation for solving the position of the sound source; then, a generalized correlation calculation of the sound source signals received by each hydrophone is done. As signals will be refracted and reflected during the transmission, multiple correlation peaks will be generated, resulting in the ambiguity of correlation peaks. Directed at this problem, this paper adopts SINS position assistance to estimate the time difference of sound source signals reaching each hydrophone, solves the distance difference according to the time difference and the equivalent transmission velocity of the signal and, finally, calculates the position of the sound source according to the hyperbolic positioning model. Hence, the interactive assistance positioning technology of SINS and LBL is an innovation point of this paper.

Figure 4 is the operating block diagram of the system. The positioning system mainly consists of LBL, an SINS/DVL/MCP integrated system and a data processing unit. The solution will be made according to the sequence number in the box; firstly, the hydrophones in the LBL system receive signals (Box 1) from the sound source on the AUV and conduct a generalized correlation calculation of received signal ($x_i(t)$,$x_j(t)$) of hydrophone i and hydrophone j, and the calculation result is a group of ambiguous correlation peaks (Box 2). Then, the current AUV position information $P_{SINS}$ and the absolute position of the hydrophones (Box 3) according to the SINS/DVL/MCP integrated system are acquired, and the delay inequality $t_{ij}^{\prime }$ (Box 4) of the sound source signal reaching hydrophone i and hydrophone j is calculated. We adopt the correlation peak screening module again to screen the former acquired ambiguous correlation peaks and acquire the actual delay inequality $t_{ij}$ (Box 5), then we acquire the distance difference by combining the sound velocity correction algorithm, formulate a positioning solution equation, take $P_{SINS}$ as the initial iterative position, adopt the Taylor series expansion algorithm to solve AUV position $P_{LBL}$ (Box 6) in LBL positioning and, finally, input the difference value of $P_{LBL}$ and $P_{SINS}$ as the external observation information into a Kalman filter; velocity information provided by the DVL and heading information provided by the MCP are also taken as an observed quantity. Filtering results will correct the errors of SINS, and navigation results, such as the accurate position $P_{AUV}$, velocity and attitude of the AUV, will finally be acquired.

Figure 3. Schematic diagram of AUV underwater LBL positioning.

Figure 3. Schematic diagram of AUV underwater LBL positioning.

Figure 4. Operation principle block diagram of the system. SINS, strapdown inertial navigation system; DVL, Doppler velocity log; MCP, magnetic compass pilot.

Figure 4. Operation principle block diagram of the system. SINS, strapdown inertial navigation system; DVL, Doppler velocity log; MCP, magnetic compass pilot.

3. Principle of the Interactive Assistance Positioning Algorithm of SINS/DVL/MCP/LBL

This section introduces the realization principles of the algorithms in Figure 4, including generalized cross-correlation calculation of hydrophone receiving signals; SINS assists in seeking the ideal time differences and AUV position calculation based on TDOA.

3.1. Generalized Cross-Correlation Calculation of Hydrophone Receiving Signals

x(t) represents the sound source signal; suppose that the signal received by No. i hydrophone is:

$$x_i(t)={\alpha }_{i}x(t-{\tau }_i)+n_i(t)$$

(3)

The signal received by No. j hydrophone is:

$$x_j(t)={\alpha }_{j}x(t-{\tau }_j)+n_j(t)$$

(4)

${\alpha }_i$ and ${\alpha }_j$ are attenuation coefficients of sound signals propagating underwater; $n_i(t)$ and $n_j(t)$ are non-correlative noise signals; ${\tau }_i$ and ${\tau }_j$ are propagation time.

The ross-correlation function of $x_i(t)$ and $x_j(t)$ is:

$$R_{x_i{x}_j}(\tau )=E[x_1(t)x_2^{\ast }(t-\tau )]=\frac{1}{T-\tau }{\displaystyle {\int }_{\tau }^T{x}_i(t)x_j(t-\tau )}dt$$

(5)

$\tau ={\tau }_j-{\tau }_i$ represents TDOA; $T$ is observation time. According to the characteristics of the correlation function, if the peak value of $R_{x_i{x}_j}(\tau )$ is found, then the corresponding $\tau$ is the right time difference.

3.2. Multi-Path Effect of Sound Signals Underwater

Figure 5 is a simplified multi-path underwater sound propagation model. Place a sound source and two hydrophones R1 and R2; simply consider nonstop path (Pid, i=1,2), sea surface reflection path (Pis, i=1,2) and seabed reflection path (Pib, i=1,2); set the sound source signal as x(t); then, the reception model of the hydrophone is as shown in Equation (6):

$$\{\begin{array}{l}{x}_1(t)={\alpha }_{1D}x(t-{\tau }_{1D})+{\alpha }_{1S}x(t-{\tau }_{1S})+{\alpha }_{1B}x(t-{\tau }_{1B})+n_1(t)\\ x_2(t)={\alpha }_{2D}x(t-{\tau }_{2D})+{\alpha }_{2S}x(t-{\tau }_{2S})+{\alpha }_{2B}x(t-{\tau }_{2B})+n_2(t)\end{array}$$

(6)

Figure 5. Simplified model of the underwater multi-path effect.

Figure 5. Simplified model of the underwater multi-path effect.

${\alpha }_{iD}$, ${\alpha }_{iS}$ and ${\alpha }_{iB}$ are respectively attenuation coefficients of Pid path, Pis path and Pib path (i = 1,2). ${\tau }_{iD}$, ${\tau }_{iS}$ and ${\tau }_{iB}$ are respectively the propagation time of Pid path, Pis path and Pib path. Suppose that sound source signal $x(t)$ is irrelevant to noise $n_1(t)$ and noise $n_2(t)$ and that $n_1(t)$ is irrelevant to noise $n_2(t)$, then the cross-correlation function of $x_1(t)$ and $x_2(t)$ is as shown in Equation (7). $R_{xx}(\tau )$ is a self-correlation function of $x(t)$. It can be seen from Equation (7) that: peak values of the cross-correlation functions of $x_1(t)$ and $x_2(t)$ occur respectively on nine time delay of arrival points, such as ${\tau }_{1D}-{\tau }_{2D}$, ${\tau }_{1D}-{\tau }_{2S}$ and ${\tau }_{1D}-{\tau }_{2B}$ (nine peak values will occur when the nine points are unequal; if an equality situation among nine points exists, then there will be an overlapping phenomenon, and the number of peak values will reduce); the peak value will be decided by the corresponding attenuation coefficient. The specific effect is as shown in Figure 6. Under a practical situation, we only need to calculate the time difference of arrival of a nonstop path (main peak), so other peak values will interfere with confirming the main peak, which makes it impossible to accurately estimate the TDOA of signals.

$$\begin{array}{ll}{R}_{x_1{x}_2}(\tau )& =E[x_1(t)x_2^{\ast }(t-\tau )]\\ & ={\alpha }_{1D}{\alpha }_{2D}{R}_{xx}({\tau }_{1D}-{\tau }_{2D}-\tau )\\ & +{\alpha }_{1D}{\alpha }_{2S}{R}_{xx}({\tau }_{1D}-{\tau }_{2S}-\tau )\\ & +{\alpha }_{1D}{\alpha }_{2B}{R}_{xx}({\tau }_{1D}-{\tau }_{2B}-\tau )\\ & +{\alpha }_{1S}{\alpha }_{2D}{R}_{xx}({\tau }_{1S}-{\tau }_{2D}-\tau )\\ & +{\alpha }_{1S}{\alpha }_{2S}{R}_{xx}({\tau }_{1S}-{\tau }_{2S}-\tau )\\ & +{\alpha }_{1S}{\alpha }_{2\text{B}}{R}_{xx}({\tau }_{1S}-{\tau }_{2B}-\tau )\\ & +{\alpha }_{1B}{\alpha }_{2D}{R}_{xx}({\tau }_{1B}-{\tau }_{2D}-\tau )\\ & +{\alpha }_{1B}{\alpha }_{2S}{R}_{xx}({\tau }_{1B}-{\tau }_{2S}-\tau )\\ & +{\alpha }_{1B}{\alpha }_{2\text{B}}{R}_{xx}({\tau }_{1B}-{\tau }_{2B}-\tau )\end{array}$$

(7)

Figure 6. Ambiguity phenomenon of correlation peaks.

Figure 6. Ambiguity phenomenon of correlation peaks.

3.3. SINS/DVL Assistance in Seeking the Ideal Delay Inequality

Because of the multi-path effect, multiple correlation peaks will appear in the cross-correlation function, and these peaks differ a little in value; it is very difficult to judge which one corresponds to the most ideal time difference. In multi-path time delay estimation, the methods that can be adopted are usually the self-adaptation method, the generalized cross-correlation method, the auto-correlation method, the cepstrum method, etc. Although the self-adaptation method is of high estimation accuracy and a strong resolution ratio, its search scope is quite broad, which makes it hard to guarantee convergence and results in a large calculated quantity and poor instantaneity; besides, it has a certain requirement for the signal-to-noise ratio. The generalized cross-correlation method is of simple calculation, but the main correlation peak is not obvious; and there are certain deficiencies in its performance. Resolution ratios of the auto-correlation method and the cepstrum method are used for calculating the time delay, as they need a relatively larger signal bandwidth [17,18,19,20]. This paper opens up a new path by putting forward a method that selects the main correlation peak based on SINS/DVL positioning assistance after combining the advantages and disadvantages of the generalized cross-correlation method. This algorithm is simple and easy to realize; besides, it can estimate the time differences of multiple paths. Even with a low signal-to-noise ratio, it can effectively do the estimation; in the meantime, it can solve the problem of selecting the main correlation peak with high accuracy when correlation peaks are ambiguous. Now, the principle of the algorithm will be introduced in detail:

In the LBL underwater acoustics positioning system, set the position of No. i hydrophone as $P_i(x_i,y_i,z_i)$ and the AUV position output by the SINS integrated system at time k as $P_{SINS}(x_{SINS}(k),y_{SINS}(k),z_{SINS}(k))$; adopt $P_{SINS}(x_{SINS}(k),y_{SINS}(k),z_{SINS}(k))$ to calculate the distance between the hydrophone and AUV at time k as:

$$D_i(k)=\sqrt{{(x_i-x_{SINS}(k))}^2+{(y_i-y_{SINS}(k))}^2+{(z_i-z_{SINS}(k))}^2}$$

(8)

The distance difference between two random hydrophones i and j and the AUV at time k is:

$$D_{ij}(k)=D_i(k)-D_j(k)(i\ne j)$$

(9)

Then, the calculation of the time difference of the two hydrophones receiving signals is:

$$\Delta t_{ij}^{\prime }(k)=\frac{D_{ij}(k)}{c_{ij}(k)}$$

(10)

$c_{ij}(k)$ is the equivalent sound velocity of signals corresponding to time difference $\Delta t_{ij}^{\prime }(k)$ at time k.

As at time k − 1, the surrounding environment of the AUV is not changed that much at time k, the change of the sound ray structure is little. Hence, the equivalent sound velocity of the last time cycle can be used as the current equivalent acoustic velocity; in other words, the velocity can be acquired by using the distance difference between hydrophones i and j and the sound source at time k − 1 to divide the time difference. The specific calculation is as follows:

At time k − 1, the corrected position of AUV by LBL is $P_{LBL/SINS}(x_{LBL/SINS}(k-1),y_{LBL/SINS}(k-1),z_{LBL/SINS}(k-1))$, and the distance of hydrophone i and the sound source is:

$$R_i(k-1)=\sqrt{{(x_i-x_{LBL/SINS}(k-1))}^2+{(y_i-y_{LBL/SINS}(k-1))}^2+{(z_i-z_{LBL/SINS}(k-1))}^2}$$

(11)

The distance difference between hydrophones i and j and the sound source is:

$$\Delta R_{ij}(k-1)=R_i(k-1)-R_j(k-1)$$

(12)

If the time difference of hydrophones i and j (which have been screened out) receiving the signal at time $t_{k-1}$ is $\Delta t_{ij(k-1)}$, then the current equivalent acoustic velocity is:

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

(13)

Substitute the computed results of Equations (9) and (13) into Equation (10), and time difference $\Delta t_{ij}^{\prime }(k)$ can be calculated. Seek the peak value that is the most proximate to $\Delta t_{ij}^{\prime }(k)$ among a group of ambiguous correlation peaks in Equation (7), and take the time difference corresponding to this peak value as the more accurate time difference $\Delta t_{ij}(k)$.

3.4. TDOA Three-Dimensional Positioning Algorithm Based on SINS/DVL Assistance

Three-Dimensional Positioning Algorithm Base on Taylor Series Expansion

For the moment, there are many algorithms that use TDOA measured to perform positioning, such as Chan’s algorithm, the Taylor algorithm, the Friedlander algorithm, etc. Chan’s algorithm has strict requirements for the measurement accuracy of the time difference and is more applicable to a line-of-sight transmission channel environment. Under a non-line-of-sight transmission condition, as for the measurement errors of the time difference of the signal arrival, the positioning errors of the algorithm will be large and will be easily influenced by the effects of reflection, scattering and refraction. What the Friedlander algorithm acquires is only the second-best solution. The Taylor algorithm has no special requirements for the statistic property of measuring errors or a priori information, and it can provide a higher positioning degree on a certain Gaussian noise level. However, this algorithm is an iterative one without a final expression solution, and algorithmic convergence needs to be guaranteed by an initial position that is not far from the actual position [21,22]. Hence, this paper suggests the TDOA positioning algorithm based on SINS/DVL assistance, and the algorithm takes the positioning results of SINS/DVL as the iterative initial value of the Taylor algorithm, which not only guarantees algorithmic convergence, but also reduces the iterations. It is very applicable to underwater positioning.

This algorithm will be introduced in detail as follows:

Suppose that there are n hydrophones in the matrix, then formulate (n – 1) equations according to the hyperbolic positioning model:

$$R_{i1}=R_i-R_1=c_{i1}\Delta t_{i1}(i=2,3,4,\dots ,n)$$

(14)

$R_{i1}$ is the function of $x,y,z,x_i,y_i,z_i$; $(x,y,z)$ represents the position of the AUV; $(x_i,y_i,z_i)$ represents the position of No. i hydrophone; then, it can be expressed as $f_i(x,y,z,x_i,y_i,z_i)=R_i-R_1$; set it as the objective function. Suppose that the measured value of objective function $f_i(*)$ (namely $f_i(x,y,z,x_i,y_i,z_i)$) is $m_i=c_{i1}\Delta t_{i1}$, and the actual value is $u_i$, $u_i=m_i-e_i$, $e_i$ is the measuring error. Adopt initial value $(\widehat{x},\widehat{y},\widehat{z})$, and they meet $x=\widehat{x}+\Delta x$, $y=\widehat{y}+\Delta y$, $z=\widehat{z}+\Delta z$; then, expand objective function $f_i(*)$ in $(\widehat{x},\widehat{y},\widehat{z})$ according to the Taylor series as the following Equation (15):

$$\begin{array}{l}{f}_i(x,y,z,x_i,y_i,z_i)=f_i(\widehat{x},\widehat{y},\widehat{z},x_i,y_i,z_i)+(\Delta x\frac{\partial }{\partial x}+\Delta y\frac{\partial }{\partial y}+\Delta z\frac{\partial }{\partial z})f_i(\widehat{x},\widehat{y},\widehat{z},x_i,y_i,z_i)\\ +\frac{1}{2!}{(\Delta x\frac{\partial }{\partial x}+\Delta y\frac{\partial }{\partial y}+\Delta z\frac{\partial }{\partial z})}^2{f}_i(\widehat{x},\widehat{y},\widehat{z},x_i,y_i,z_i)+\cdots \\ +\frac{1}{n!}{(\Delta x\frac{\partial }{\partial x}+\Delta y\frac{\partial }{\partial y}+\Delta z\frac{\partial }{\partial z})}^n{f}_i(\widehat{x},\widehat{y},\widehat{z},x_i,y_i,z_i)\\ +\frac{1}{(n+1)!}{(\Delta x\frac{\partial }{\partial x}+\Delta y\frac{\partial }{\partial y}+\Delta z\frac{\partial }{\partial z})}^{n+1}{f}_i(\widehat{x}+\xi \Delta x,\widehat{y}+\xi \Delta y,\widehat{z}+\xi \Delta z,x_i,y_i,z_i),(0<\xi <1)\end{array}$$

(15)

Ignore high-order terms above the quadratic term in the expanded section, then the above equation can be expressed as:

$$f_i(x,y,z,x_i,y_i,z_i)=u_i\approx f_i(\widehat{x},\widehat{y},\widehat{z},x_i,y_i,z_i)+(\Delta x\frac{\partial }{\partial x}+\Delta y\frac{\partial }{\partial y}+\Delta z\frac{\partial }{\partial z})f_i(\widehat{x},\widehat{y},\widehat{z},x_i,y_i,z_i)$$

(16)

It can be acquired according to the hyperbolic positioning model that:

$$f_i(x,y,z,x_i,y_i,z_i)=\sqrt{{(x-x_i)}^2+{(y-y_i)}^2+{(z-z_i)}^2}-\sqrt{{(x-x_1)}^2+{(y-y_1)}^2+{(z-z_1)}^2}$$

(17)

Expand Equation (17) according to Equation (16) and acquire:

$$f_i(x,y,z,x_i,y_i,z_i)\approx {\widehat{R}}_i-{\widehat{R}}_1+\Delta x(\frac{\widehat{x}-x_i}{{\widehat{R}}_i}-\frac{\widehat{x}-x_1}{{\widehat{R}}_1})+\Delta y(\frac{\widehat{y}-y_i}{{\widehat{R}}_i}-\frac{\widehat{y}-y_1}{{\widehat{R}}_1})+\Delta z(\frac{\widehat{z}-z_i}{{\widehat{R}}_i}-\frac{\widehat{z}-z_1}{{\widehat{R}}_1})$$

(18)

where:

$${\widehat{R}}_1=\sqrt{{(\widehat{x}-x_1)}^2+{(\widehat{y}-y_1)}^2+{(\widehat{z}-z_1)}^2}$$

(19)

$${\widehat{R}}_i=\sqrt{{(\widehat{x}-x_i)}^2+{(\widehat{y}-y_i)}^2+{(\widehat{z}-z_i)}^2}$$

(20)

Set ${\widehat{f}}_i=f_i(\widehat{x},\widehat{y},\widehat{z},x_i,y_i,z_i)$, then:

$$a_{i1}=\frac{\partial f_i(x,y,z,x_i,y_i,z_i)}{\partial x}{|}_{\begin{array}{l}x=\widehat{x}\\ y=\widehat{y}\\ z=\widehat{z}\end{array}}=\frac{\widehat{x}-x_i}{{\widehat{R}}_i}-\frac{\widehat{x}-x_1}{{\widehat{R}}_1}$$

(21)

$$a_{i2}=\frac{\partial f_i(x,y,z,x_i,y_i,z_i)}{\partial y}{|}_{\begin{array}{l}x=\widehat{x}\\ y=\widehat{y}\\ z=\widehat{z}\end{array}}=\frac{\widehat{y}-y_i}{{\widehat{R}}_i}-\frac{\widehat{y}-y_1}{{\widehat{R}}_1}$$

(22)

$$a_{i3}=\frac{\partial f_i(x,y,z,x_i,y_i,z_i)}{\partial z}{|}_{\begin{array}{l}x=\widehat{x}\\ y=\widehat{y}\\ z=\widehat{z}\end{array}}=\frac{\widehat{z}-z_i}{{\widehat{R}}_i}-\frac{\widehat{z}-z_1}{{\widehat{R}}_1}$$

(23)

and then:

$${\widehat{f}}_i+a_{i1}\Delta x+a_{i2}\Delta y+a_{i3}\Delta z\approx m_i-e_i$$

(24)

For the n matrix elements, there will be:

$$\epsilon \approx h-G\delta$$

(25)

where:

$$\epsilon =\left[\begin{array}{c}{e}_2\\ e_3\\ ⋮\\ e_n\end{array}\right]$$

(26)

$$h=\left[\begin{array}{c}{m}_2-{\widehat{f}}_2\\ m_3-{\widehat{f}}_3\\ ⋮\\ m_n-{\widehat{f}}_n\end{array}\right]=\left[\begin{array}{c}{c}_{21}\Delta t_{21}-({\widehat{R}}_2-{\widehat{R}}_1)\\ c_{31}\Delta t_{31}-({\widehat{R}}_3-{\widehat{R}}_1)\\ ⋮\\ c_{n1}\Delta t_{n1}-({\widehat{R}}_n-{\widehat{R}}_1)\end{array}\right]$$

(27)

$$G=\left[\begin{array}{ccc}{a}_{21}& a_{22}& a_{33}\\ a_{31}& a_{32}& a_{33}\\ ⋮& ⋮& ⋮\\ a_{n1}& a_{n2}& a_{n3}\end{array}\right]==\left[\begin{array}{ccc}\frac{\widehat{x}-x_2}{{\widehat{R}}_2}-\frac{\widehat{x}-x_1}{{\widehat{R}}_1}& \frac{\widehat{y}-y_2}{{\widehat{R}}_2}-\frac{\widehat{y}-y_1}{{\widehat{R}}_1}& \frac{\widehat{z}-z_2}{{\widehat{R}}_2}-\frac{\widehat{z}-z_1}{{\widehat{R}}_1}\\ \frac{\widehat{x}-x_3}{{\widehat{R}}_3}-\frac{\widehat{x}-x_1}{{\widehat{R}}_1}& \frac{\widehat{y}-y_3}{{\widehat{R}}_3}-\frac{\widehat{y}-y_1}{{\widehat{R}}_1}& \frac{\widehat{z}-z_3}{{\widehat{R}}_3}-\frac{\widehat{z}-z_1}{{\widehat{R}}_1}\\ ⋮& ⋮& ⋮\\ \frac{\widehat{x}-x_n}{{\widehat{R}}_n}-\frac{\widehat{x}-x_1}{{\widehat{R}}_1}& \frac{\widehat{y}-y_n}{{\widehat{R}}_n}-\frac{\widehat{y}-y_1}{{\widehat{R}}_1}& \frac{\widehat{z}-z_3}{{\widehat{R}}_n}-\frac{\widehat{z}-z_1}{{\widehat{R}}_1}\end{array}\right]$$

(28)

$$\delta =\left[\begin{array}{c}\Delta x\\ \Delta y\\ \Delta z\end{array}\right]$$

(29)

Take $\delta$ as an unknown variable; suppose that $Q$ is the covariance matrix of $\epsilon$; then adopt the method of weighing least squares estimation; it can be acquired that:

$$\left[\begin{array}{c}\Delta x\\ \Delta y\\ \Delta z\end{array}\right]=\delta ={[G^T{Q}^{-1}G]}^{-1}{G}^T{Q}^{-1}h$$

(30)

The calculation process of the Taylor algorithm can be concluded as follows:

(1)

Select an initial value $(\widehat{x},\widehat{y},\widehat{z})$;

(2)

Substitute $(\widehat{x},\widehat{y},\widehat{z})$ into Equations (20) and (28) to calculate G and ${\widehat{R}}_i$;

(3)

Substitute $c_{i1}$, $\Delta t_{21}$ and ${\widehat{R}}_i$ into Equation (27) to calculate $h$;

(4)

Substitute $G$, $h$ and $Q$ into Equation (30) and update $\delta$; if $\sqrt{{(\Delta x)}^2+{(\Delta y)}^2+{(\Delta z)}^2}<\eta$, $\eta$ is a very small threshold value, then the iteration ends; $(\widehat{x},\widehat{y},\widehat{z})$ is the final positioning result. Otherwise, update $(\widehat{x},\widehat{y},\widehat{z})$ according to Equation (31), and repeat $(\widehat{x},\widehat{y},\widehat{z})$ until the above conditions are met.

$$\{\begin{array}{c}\widehat{x}\leftarrow \widehat{x}+\Delta x\\ \widehat{y}\leftarrow \widehat{y}+\Delta y\\ \widehat{z}\leftarrow \widehat{z}+\Delta z\end{array}$$

(31)

The algorithm needs an estimated position value as an initial value to make iterations. The accuracy of the initial position value has great influence on the convergence of the algorithm. As shown in Figure 7, the convergence error threshold for the iterative algorithm is 10⁻⁷ m. When the initial position error is 80 m, the iterative result cannot converge. When the initial position error is 35 m, the iterative result converges, but the number of iterations is 468 steps. While the initial position error is set as 10 m, the number of iterations requires only 364 steps. Thus, the higher the accuracy of the initial position value, the faster the convergence rate. This paper selects the output position P_SINS of SINS/DVL as the initial position value of the iteration, which can not only satisfy the convergence of the algorithm, but also greatly reduce the iteration steps.

Figure 7. The iteration results with different initial position values.

Figure 7. The iteration results with different initial position values.

3.5. SINS/DVL/MCP/LBL Integrated System Modeling

When the AUV does not enter the effective signal scope of LBL, adopt the SINS/DVL/MCP integrated navigation system as shown in Figure 8 to perform the navigation and positioning. When the AUV enters the effective signal scope of LBL, adopt the SINS/DVL/MCP/LBL integrated navigation system as shown in Figure 9 to perform the navigation and positioning.

Figure 8. SINS/DVL/MCP integrated navigation system.

Figure 8. SINS/DVL/MCP integrated navigation system.

Figure 9. SINS/DVL/MCP/LBL integrated navigation system.

Figure 9. SINS/DVL/MCP/LBL integrated navigation system.

The state equation of the integrated system is:

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

(32)

where X is the state variable, F is the state-transition matrix, G is the transition matrix of the process noise and W is systematic noise.

Select the velocity error, attitude error, accelerometer zero offset and gyroscopic drift as state vector X:

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

(33)

$\delta V_E,\delta V_N,\delta V_U$ are respectively the velocity errors of the directions of east, north and the local vertical(up). ${\varphi }_E,{\varphi }_N,{\varphi }_U$ are respectively the misalignment angles of the directions of east, north and the local vertical(up). $\delta L,\delta \lambda ,\delta h$ are respectively the errors of latitude, longitude and altitude. ${\nabla }_{bx},{\nabla }_{by},{\nabla }_{bz}$ are respectively biased errors of the three axial directions of the accelerator. ${\epsilon }_{bx},{\epsilon }_{by},{\epsilon }_{bz}$ are respectively the drifts of the three axial directions of the gyroscopes. F can be confirmed by the SINS error equation.

The measuring equation of the integrated system is:

$$V_{SINS}=\left[\begin{array}{c}{V}_{SINSE}\\ V_{SINSN}\\ V_{SINSU}\end{array}\right]$$

(34)

Z is the observation vector; $P_{SINS}=\left[\begin{array}{c}{L}_{SINS}\\ {\lambda }_{SINS}\\ h_{SINS}\end{array}\right]$ is the position information output of the SINS system; $P_{LBL}=\left[\begin{array}{c}{L}_{LBL}\\ {\lambda }_{LBL}\\ h_{LBL}\end{array}\right]$ is the position information output by the LBL system; $V_{SINS}=\left[\begin{array}{c}{V}_{SINSE}\\ {\lambda }_{SINSN}\\ h_{SINSU}\end{array}\right]$ is the velocity information output by the SINS system; $V_{DVL}=\left[\begin{array}{c}{V}_{DVLE}\\ V_{DVLN}\\ V_{DVLU}\end{array}\right]$ is the velocity information output by the DVL system; ${\phi }_U$ is the heading information output by the SINS system; ${\phi }_{MCP}$ is the heading information output by the MCP system; V is the observation noise vector; and H is the measurement matrix, which satisfies:

$$H=\left[\begin{array}{ccccccccccccccc}1& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 1& 0& 0& 0& 1& 0& 0& 0& 0& 0\end{array}\right]$$

(35)

4. Simulation and Experiment

4.1. Underwater Sound Signal Propagation Channel Modeling

The underwater acoustic channel is a time varying and space varying random channel of high environmental noise, a narrow channel bandwidth, large transmission loss and a serious multi-path effect. Because of the slow underwater movement of the AUV, the acoustic channel can be seen as a slow time-varying system. The system can be approximated as a LTI ( linear time-invariant )system. Suppose that there are n paths for the acoustic signal to transmit from the sound source to the hydrophones, and then, the unit impulse response of the multi-path channel from the sound source to the hydrophones will be:

$$h(t)={\displaystyle \sum _{i=1}^N{a}_i\delta (t-{\tau }_i)}$$

(36)

$a_i$ is the attenuation coefficient of the No. i transmission path; ${\tau }_i$ is the relative time delay of the transmission along the No. i transmission path. The underwater acoustic signal transmission can be simplified as the model that is shown in Figure 10:

Figure 10. A simplified model of underwater sound transmission.

Figure 10. A simplified model of underwater sound transmission.

Then, the hydrophones receive signal y(t) as a convolution of sound source signal x(t) and the unit impulse response h(t), namely:

$$y(t)=x(t)*h(t)$$

(37)

Porter, M.B. et al. have developed BELLHOP software, which simulates the marine environment according to this model. This software can acquire amount N, the angle of incidence, the range and the time delay of intrinsic sound rays and provide the unit impulsive response of the system by inputting marine environment parameters [23]. This paper, according to the sound velocity distribution curve under a lake, which is as shown in Figure 11, adopts the BELLHOP software to establish an underwater sound signal transmission channel model, which will describe the sound field of the underwater environment and calculate the sound ray transmission path by setting the positions of the sound source and hydrophones, as shown in Figure 12; then, it will solve the unit impulse response function h(t) of the system and, finally, acquire the received signals of the hydrophones by the convolution operation and simulation.

Figure 11. Underwater sound velocity distribution curve of a lake.

Figure 11. Underwater sound velocity distribution curve of a lake.

Figure 12. Sound ray transmission path.

Figure 12. Sound ray transmission path.

The simulated sound source signal adopts the amplitude-modulated signal expressed in Equation (38). Its bandwidth is 50 kHz, and its center frequency is 25 kHz. Suppose that the noise model of the facility is a white noise model; then, the waveform of the sound source signal will be as shown in Figure 13.

$$sigIN=({\displaystyle \sum _i^{N}a\sin (2\pi i\omega t))\cos (2\pi {\omega }_{c}t)}$$

(38)

Calculate the unit impulse response as shown in Figure 14 according to the propagating sound rays, and the received signal can be calculated as shown in Figure 15 according to Equation (37). Adopt the sound field model established by BELLHOP and the underwater sound velocity distribution to simulate the situation of reflection and refraction, which may occur in the acoustic signal transmission process, as well as the signals received by the hydrophones, all of which will be used for the simulation and positioning calculation.

Figure 13. Oscillogram of the sound source signal.

Figure 13. Oscillogram of the sound source signal.

Figure 14. Unit impulse response h(t).

Figure 14. Unit impulse response h(t).

Figure 15. Oscillogram of received signals.

Figure 15. Oscillogram of received signals.

4.2. Simulation of SINS Assistance in the LBL Positioning Algorithm

As shown in Figure 16, set the positions of the hydrophones and the AUV; place five hydrophones underwater; their positions will be expressed with longitude and latitude, and they are respectively(118.01°, 32°), (118.01°, 32.01°), (118.01°, 32.02°), (118.02°, 32.01°), (118°, 32.01°); all of their depths are 30 m. Suppose that the current actual position of the AUV is (118°, 32°) with a depth of 10 m. The underwater acoustic velocity distribution data are acquired by an experiment on this lake. Simulate the acoustic signals received by the hydrophones according to the method introduced in Section 4.1, and calculate the time difference of different hydrophones receiving the signal.

Figure 16. Layout of the hydrophones and the AUV.

Figure 16. Layout of the hydrophones and the AUV.

4.2.1. Simulation of SINS Assisting LBL in Tracking the Optimal Time Difference

As shown in Figure 17, as the sound source signal will go through multiple paths, there will be also multiple correlation peaks in the generalized cross-correlation result. The correlation peaks whose amplitude is in the top twenty can be selected as alternative correlation peaks, while others are neglected due to excessive signal attenuation. The distance between the time difference with the highest amplitude (Point A in Figure 17) and the true time difference (the red circle in Figure 17) is relatively larger, so Point A cannot be selected as the true time difference. Using the proposed method, the time difference (symbol × in Figure 16), which is calculated through Equation (10) by the aiding of the SINS/DVL integrated navigation, better approaches the truth value. Therefore, the alternative correlation peak (Point B in Figure 17), which is closest to the symbol “×”, is selected as the ideal time difference, because it has the smallest distance to Point “A”. Traditional algorithms directly take the correlation peak that is corresponding to the maximum peak value as the main correlation peak to calculate the time difference; there will be larger errors under the multi-path effect. The improved algorithm adopted in this paper selects the main correlation peak through the positions obtained by the SINS/DVL integrated navigation system. A comparison of the calculation results and errors of the two algorithms is as shown in Table 1 and Table 2, which show that: the improved algorithm reduces the interference of multiple correlation peaks with time difference estimation under the multi-path effect and makes the calculation value accuracy of the final time difference superior to that of the traditional algorithm.

Figure 17. Screening out of the main correlation peak.

Figure 17. Screening out of the main correlation peak.

Table 1. Comparison of the calculation results of the time differences.

Time Difference of Two Hydrophones Receiving the Signal	Traditional Algorithm/s	Improved Algorithm/s	Truth Value/s
Ts1−Ts0	−0.3532	−0.3436	−0.3470
Ts2−Ts0	0.5080	0.5043	0.5044
Ts3−Ts0	0.6623	0.6499	0.6492
Ts4−Ts0	−0.2388	−0.2297	−0.2297

Note: The expression (Ts1−Ts0) represents the time difference from the sound source from the sound source propagating to Hydrophone 1 and Hydrophone 0, and so on.

Table 1. Comparison of the calculation results of the time differences.

Time Difference of Two Hydrophones Receiving the Signal	Traditional Algorithm/s	Improved Algorithm/s	Truth Value/s
Ts1−Ts0	−0.3532	−0.3436	−0.3470
Ts2−Ts0	0.5080	0.5043	0.5044
Ts3−Ts0	0.6623	0.6499	0.6492
Ts4−Ts0	−0.2388	−0.2297	−0.2297

Note: The expression (Ts1−Ts0) represents the time difference from the sound source from the sound source propagating to Hydrophone 1 and Hydrophone 0, and so on.

Table 2. Error comparison of the calculation values of the time difference.

Time Difference of Two Hydrophones Receiving the Signal	Error of the Traditional Algorithm/s	Error of the Improved Algorithm/s
Ts1−Ts0	0.0062	0.0034
Ts2−Ts0	0.0036	0.0001
Ts3−Ts0	0.0132	0.0007
Ts4−Ts0	0.0091	0.0001

Table 2. Error comparison of the calculation values of the time difference.

Time Difference of Two Hydrophones Receiving the Signal	Error of the Traditional Algorithm/s	Error of the Improved Algorithm/s
Ts1−Ts0	0.0062	0.0034
Ts2−Ts0	0.0036	0.0001
Ts3−Ts0	0.0132	0.0007
Ts4−Ts0	0.0091	0.0001

4.2.2. Simulation of the Acoustic Velocity Correction Algorithm

The traditional algorithm directly adopts acoustic velocity in a traditional sense to calculate the distance difference. The improved algorithm adopted in this paper calculates equivalent acoustic velocity, which will be used to calculate the distance difference. A comparison of the calculation results and errors of the two algorithms is as shown in Table 3 and Table 4, which show that: as the improved algorithm adopts the equivalent acoustic velocity to calculate the distance difference, it corrects the sound velocity, under situations of the multi-path effect and sound ray curve. It greatly enhances the accuracy of the calculation values of the distance difference. Therefore, adopting the time difference algorithm and distance difference algorithm in this paper can greatly reduce the errors of the time difference and distance difference, which play a significant role in improving the AUV underwater positioning accuracy.

Table 3. Comparison of the calculation results of the distance difference.

Distance Difference of Two Hydrophones Receiving the Signal	Traditional Algorithm/m	Improved Algorithm/m	Ideal Distance Difference/m
Ds1−Ds0	−523.8619	−509.5792	−510.0840
Ds2−Ds0	739.6344	734.1894	734.3538
Ds3−Ds0	973.0620	954.7788	954.1334
Ds4−Ds0	−360.3024	−346.6323	−346.1796

Note: The expression (Ds1−Ds0) represents the distance difference from the sound source propagating to Hydrophone 1 and Hydrophone 0, and so on.

Table 3. Comparison of the calculation results of the distance difference.

Distance Difference of Two Hydrophones Receiving the Signal	Traditional Algorithm/m	Improved Algorithm/m	Ideal Distance Difference/m
Ds1−Ds0	−523.8619	−509.5792	−510.0840
Ds2−Ds0	739.6344	734.1894	734.3538
Ds3−Ds0	973.0620	954.7788	954.1334
Ds4−Ds0	−360.3024	−346.6323	−346.1796

Note: The expression (Ds1−Ds0) represents the distance difference from the sound source propagating to Hydrophone 1 and Hydrophone 0, and so on.

Table 4. Error comparison of the calculation values of the distance difference.

Distance Difference of Two Hydrophones Receiving the Signal	Error of the Traditional Algorithm/m	Error of the Improved Algorithm/m
Ds1−Ds0	13.7779	0.5048
Ds2−Ds0	5.2806	0.1644
Ds3−Ds0	18.9286	0.6454
Ds4−Ds0	14.1228	0.4527

Table 4. Error comparison of the calculation values of the distance difference.

Distance Difference of Two Hydrophones Receiving the Signal	Error of the Traditional Algorithm/m	Error of the Improved Algorithm/m
Ds1−Ds0	13.7779	0.5048
Ds2−Ds0	5.2806	0.1644
Ds3−Ds0	18.9286	0.6454
Ds4−Ds0	14.1228	0.4527

4.2.3. Simulation of the TDOA Positioning Algorithm

In order to verify the positioning effect of the algorithm, we conduct a simulation under the MATLAB environment. Adopt the BELLHOP model to simulate the acoustic signal receiving from the hydrophones, and calculate the time difference and distance difference; finally, adopt the position obtained from SINS/DVL integrated navigation system as the iterative initial value of the Taylor series expansion method to solve the positioning results; then obtain the longitude, latitude and depth of AUV, and make a comparison with the positioning results of the traditional algorithm. The result is as shown in Figure 17.

It can be seen from Figure 18 that: the positioning result of the traditional algorithm has far deviated from the actual position, while that of the improved algorithm is very proximate to the actual value. This is because the error of the distance difference of the traditional algorithm is larger, which results in the non-convergence of the positioning result or big error; then, an acceptable positioning result cannot be acquired. However, the error of the distance difference of the improved algorithm is less than 1 m, and a more accurate positioning result can be acquired finally.

Figure 18. Comparison of the positioning results.

Figure 18. Comparison of the positioning results.

4.3. Dynamic Simulation of AUV Integrated Navigation System Based on SINS/LBL/DVL/MCP

In order to further verify the effectiveness of this algorithm when the AUV is in dynamic operation, a dynamic simulation has been performed on this algorithm. Place five hydrophones underwater; their positions are the same as the above simulation. Suppose that the AUV starts from (118°, 32°) and moves along north by east 45°, which is as shown in Figure 19. The random shift and constant value shift of the gyroscope is 50 μg; the constant value bias is 50 μg. The initial misalignment angles are respectively: pitching angle 1.5°, roll angle 1.5° and course angle 1.5°. The velocity error of DVL is 0.2 m/s. The heading error of MCP is 0.3°. The velocity of the AUV is 1 m/s. Adopt the algorithm in this paper to perform the positioning calculation, and integrate the positioning results with the SINS/DVL/MCP integrated system to calibrate accumulative errors; simulation time is three hours. In order to enhance the fault-tolerant ability of the system, if the positioning results are not converged, then the positioning results of LBL will not be used for the calibration. Figure 19 indicates that: when the AUV enters the scope of the hydrophones, the trajectory of the improved algorithm is fundamentally overlapping with the ideal trajectory, and that of the traditional algorithm deviates from the ideal trajectory. When the AUV leaves the scope of the hydrophones, the trajectory of both the traditional and improved algorithms deviates from the ideal trajectory. Figure 19 is a comparison graph of the positioning errors of the traditional and improved algorithms. The positioning errors will be expressed by the distance between the positioning results and the actual position. Figure 20 indicates that: within 0~1000 s, the positioning errors of both the traditional and improved algorithms will gradually enlarge, because within this period, the AUV is somewhat far away from the scope of the hydrophones; it takes a long time for the hydrophones to receive the sound source signal, and there will be great delay for the positioning results. Hence, LBL will not be used to calibrate within this period; instead, the SINS/DVL/MCP integrated navigation system will be used to perform the positioning. When the AUV approaches the scope of hydrophones, namely after 1000 s, we adopt LBL to perform the calibration. As the errors of adopting the traditional algorithm to calculate are larger, the LBL positioning results will not converge, which has nearly no contribution to calibrating for the SINS/DVL/MCP integrated navigation system. Within 1000 s to 3000 s, the positioning errors enlarge gradually, with the maximum reaching 30 m or so. However, the improved algorithm in this paper greatly reduces the errors of the calculation values of the distance difference. The LBL positioning results can calibrate the accumulative errors of the integrated navigation system, which will control the positioning errors within 2 m. When the AUV leaves the scope of the hydrophones, the LBL positioning system will lose its effect; however, in general, the precision is superior to the system that has not been calibrated by LBL. The above analysis shows that: under actual dynamic operation, when the AUV approaches the scope of the hydrophones, the improved algorithm can be used to perform the positioning calculation, and the calculation results can be used to calibrate the SINS/DVL/MCP integrated navigation system, which will greatly improve the positioning accuracy of the AUV when it navigates underwater.

Figure 19. Dynamic simulation of the AUV integrated navigation system.

Figure 19. Dynamic simulation of the AUV integrated navigation system.

Figure 20. Error comparison of the positioning results.

Figure 20. Error comparison of the positioning results.

5. Conclusions

This paper, directed at deficiencies in existing underwater positioning technology, proposes an underwater positioning system based on the mutual assistance of SINS/DVL/MCP and LBL. The whole system consists of an SINS/DVL/MCP integrated navigation system and an LBL underwater acoustic positioning system. The latter adopts the TDOA positioning algorithm based on Taylor series expansion, as well as the positioning results provided by the SINS/DVL/MCP integrated navigation system, which will assist in calculating the delay inequality and distance difference. In the meantime, the positioning results will also be used as the iterative initial value of the positioning algorithm. The final positioning results will be used to calibrate the accumulative errors of the position of the SINS/DVL/MCP integrated navigation system.

The final simulation results show that: compared to the traditional algorithm, this scheme can not only directly correct accumulative errors caused by the dead reckoning algorithm, but also solves the problem of ambiguous correlation peaks caused by the multipath transmission of underwater acoustic signals. This algorithm can greatly improve the AUV underwater positioning accuracy. Only when the AUV installed with the SINS/DVL integrated navigation system approaches the scope of the hydrophones can accumulative errors be effectively calibrated. Hence, it has strong practicability.