Polar Cooperative Navigation Algorithm for Multi-Unmanned Underwater Vehicles Considering Communication Delays

Abstract

To solve the navigation accuracy problems of multi-Unmanned Underwater Vehicles (multi-UUVs) in the polar region, a polar cooperative navigation algorithm for multi-UUVs considering communication delays is proposed in this paper. UUVs are important pieces of equipment in ocean engineering for marine development. For UUVs to complete missions, precise navigation is necessary. It is difficult for UUVs to establish true headings because of the rapid convergence of Earth meridians and the severe polar environment. Based on the polar grid navigation algorithm, UUV navigation in the polar region can be accomplished with the Strapdown Inertial Navigation System (SINS) in the grid frame. To save costs, a leader-follower type of system is introduced in this paper. The leader UUV helps the follower UUVs to achieve high navigation accuracy. Follower UUVs correct their own states based on the information sent by the leader UUV and the relative position measured by ultra-short baseline (USBL) acoustic positioning. The underwater acoustic communication delay is quantized by the model. In this paper, considering underwater acoustic communication delay, the conventional adaptive Kalman filter (AKF) is modified to adapt to polar cooperative navigation. The results demonstrate that the polar cooperative navigation algorithm for multi-UUVs that considers communication delays can effectively navigate the sailing of multi-UUVs in the polar region.

1. Introduction

Unmanned Underwater Vehicles (UUV) are important pieces of equipment used in ocean engineering for marine development and have made great technological progress in recent years. UUVs can accomplish important missions, such as marine surveying, plane crash searches, etc. [1,2,3,4]. The future development of UUVs will follow two important trends. The first is that sailing trajectories will increase in length and depth. The second is that more intelligent equipment will be developed to achieve more complex missions. UUV navigation is essential because it is the foundation for completing the missions. This paper focuses on two challenges for UUV navigation. The first challenge involves solving issues that occur when UUVs are sailing in the polar region. The second is completing missions with a team of UUVs. The algorithm proposed in this paper can effectively solve the cooperative navigation problems of multi-UUVs in regard to communication delays in the polar region.

The severe natural environment and extreme navigation environment have caused many challenges to UUV missions in the polar region. Strapdown Inertial Navigation Systems (SINS) are the most widely used navigation device among the UUVs, since the SINS navigate UUVs autonomously. Multi-UUVs are faced with the same problems as single UUVs in the polar region, such as calculation overflow and error magnification, which cause the conventional north-oriented SINS to be invalid in the polar region [5]. The direction information and the orientation information cannot be distinguished, which causes the conventional wander-oriented SINS to be unavailable in the polar region [6]. The multipath effect in high latitude areas rapidly decreases the accuracy of the Global Positioning System (GPS), or even causes the failure of the GPS [7]. Therefore, it is necessary to propose a navigation algorithm that is available in the polar region and allows the cooperative sailing of multi-UUVs.

Teamwork is an essential part of work and school activities. It can improve work efficiency greatly, which is also true for UUVs. The intelligence development of a single UUV is limited, but it can be solved by multi-UUV cooperation. Multi-UUVs have a great advantage over a single UUV, especially in terms of time and efficiency. In general, there are two cooperative multi-UUV navigation types: the parallel type and the leader-follower type [8].

The cooperative navigation system, developed by the Virginia Tech research team, is a typical parallel UUV cooperative navigation system, in which the UUV sends information to its surrounding neighbors in the form of an acoustic broadcast [9]. The mobile baseline system, developed by the University of Porto [10], and the CADRE system (Cooperative Autonomy for Distributed Reconnaissance and Exploration System) developed by Massachusetts Institute of Technology (MIT) [11], are typical leader-follower UUV cooperative navigation systems. Using the leader-follower type, cooperative navigation can be achieved among multi-UUVs. There is no need for all the UUVs to be equipped with high-precision navigation devices. Therefore, the leader-follower UUV cooperative navigation system has a lower cost.

Many novel algorithms have been proposed to achieve the cooperative navigation of multi-robots in non-polar regions [12,13,14], but none of them are suitable for multi-UUVs in the polar region. Some researchers [15,16] proposed a leader-follower UUV cooperative navigation with acoustic communication time delay compensation as an approach. However, this approach is influenced by the rapid convergence of Earth meridians in the polar region, which can only be used in the non-polar region. Simultaneously, the filter is based on a standard Kalman filter (KF), which can only be used in dynamic linear system models. Compared to the cooperative navigation of multi-UUVs in non-polar regions, cooperative navigation of multi-UUVs in polar regions is still in the beginning stages, and few papers currently cover this aspect. The algorithm proposed in this paper aims to achieve cooperative navigation of multi-UUVs in the polar region, and a dynamic non-linear system model is considered. Based on the polar grid navigation algorithm and the leader-follower type, a polar cooperative navigation algorithm of multi-UUVs that considers communication delays is discussed in this paper. The leader UUV is equipped with high-precision navigation devices and the follower UUVs are equipped with low-precision navigation devices [17]. The leader UUV sends the information through underwater acoustic communication to the follower UUVs. In addition, the relative positions between the leader UUV and follower UUVs are determined by ultra-short baseline (USBL) acoustic positioning. Based on the information sent by the leader UUV and the relative position measured by the USBL, the follower UUVs can modify the navigation results from their own navigation devices through the filter method. Both the leader UUV and the follower UUVs can obtain accurate navigation results by using this polar cooperative navigation algorithm for multi-UUVs that considers communication delays.

To solve navigation problems of multi-UUVs in the polar region, a polar cooperative navigation algorithm for multi-UUVs that considers communication delays is proposed in this paper. The development of the polar cooperative navigation algorithm and modification of the adaptive Kalman filter (AKF) based on communication delays are the two main contributions of this paper. Based on this algorithm, cooperative navigation is achieved among the leader-follower UUVs team in the polar region. Although the follower UUVs are equipped with low-precision navigation devices, they can achieve high navigation accuracy with this algorithm. The following sections are arranged as follows. The formation configuration, acoustic communication delay and overall process of the polar cooperative navigation algorithm are discussed in Section 2. The error equations and polar cooperative navigation filter models are deduced in Section 3 and Section 4, respectively. The conventional AKF is modified in Section 5 with consideration to communication delays. The simulation and experiment results are presented in Section 6. Finally, Section 7 gives the conclusions.

2. Polar Cooperative Navigation System

Different formation configurations and acoustic communication modes will have different effects on the polar cooperative navigation system of multi-UUVs. Under the premise of ensuring navigation accuracy, it is our goal to reduce costs and improve efficiency as much as possible. This section will focus on the formation configuration, underwater acoustic communication and overall process of the polar cooperative navigation system.

2.1. Formation Configuration

Typical formation configurations of multi-UUVs mainly include the parallel type and the leader-follower type. In the parallel type, all the UUVs are required to have the same configuration. This means that all the UUVs should be equipped with high-precision navigation devices, and each UUV communicates with its neighbor UUVs. The number of multi-UUVs in a parallel, polar cooperative navigation system is limited by the underwater acoustic communication bandwidth. A parallel cooperative navigation system of multi-UUVs is shown in Figure 1, which shows five UUVs as an example.

Figure 1. Unmanned Underwater Vehicles (UUV) cooperative navigation system—parallel UUV cooperative navigation system.

Unlike the parallel type, the configuration of each UUV in the leader-follower type can be different. The leader UUV is equipped with high-precision navigation devices, and the follower UUVs are equipped with low-precision navigation devices, which can reduce costs. In addition, the underwater acoustic communication is only required between the leader UUV and follower UUVs. There is no need for follower UUVs to communicate with each other. The limitations caused by underwater acoustic communication bandwidth are greatly reduced. A typical leader-follower cooperative navigation system usually includes a single leader or double leaders. A leader-follower cooperative navigation system of multi-UUVs is shown in Figure 2, which shows one leader UUV and four follower UUVs as an example. In Figure 2, UUV_L is the leader UUV, and UUV_F1, UUV_F2, UUV_F3, UUV_F4 are the follower UUVs.

Figure 2. UUV cooperative navigation system—leader-follower UUV cooperative navigation system.

From the viewpoint of the underwater acoustic communication bandwidth and navigation equipment costs, the leader-follower type is superior to the parallel type. The leader UUV is equipped with high-precision navigation devices, including SINS, OCTANS (an all-in-one gyrocompass and motion sensor for diverse challenging applications, produced by iXblue), Doppler Velocity Log (DVL), depth meters and USBL. The follower UUVs are equipped with low-precision navigation devices only, including SINS, DVL, depth meters and USBL, to reduce costs.

2.2. Underwater Acoustic Communication Delay

Due to the extreme natural and navigation environment underwater in the polar region, communication becomes one of the difficulties in polar cooperative navigation of multi-UUVs. As the radio signal cannot transmit under deep water, underwater acoustic communication becomes the first choice for polar cooperative navigation systems. Underwater acoustic transducers are most widely used in underwater acoustic communication. Information is transmitted through sound waves rather than radio waves. Thus, underwater acoustic communication is less efficient than radio communication.

The working principles of underwater acoustic communication can be described as follows: First, information is converted into an electrical signal by an electrical transmitter. Second, after digital processing by an encoder, the transducer converts the electrical signal into an acoustic signal. Third, the acoustic signal propagates through the medium of water and propagates the information to the receiving transducer. In this case, the acoustic signal is converted into an electrical signal. Finally, after the digital signal is deciphered by the decoder, the information is converted to sound, text or picture by the electrical receiver.

According to the characters of underwater acoustic communication, there will be underwater acoustic delays in the propagation of underwater acoustics. These underwater acoustic delays usually consist of an underwater acoustic detection delay and an underwater communication delay. The underwater acoustic detection delay mainly depends on the detection equipment itself, and its value is relatively fixed and small. The underwater acoustic communication delay is caused by the high complexity of signal processing and the limitations from underwater bandwidth and low transmission rate. There will be a communication delay when the leader UUV sends its information to the follower UUVs in the polar cooperative navigation of multi-UUVs. As the efficiency of the underwater acoustic equipment varies with time and the distance between the leader UUV and the follower UUVs, the underwater acoustic communication delay is a random value [18]. According to the above analysis, the underwater acoustic delay can be expressed as follows:

$$N_i=\Delta t+d/C+\delta t,$$

(1)

where $\Delta t$ is the detection delay; $d$ is the distance between the leader UUV and the follower UUV; $C$ is the velocity of the underwater acoustic communication; and $\delta t$ is the random part of the underwater acoustic communication delay.

2.3. Overall Process of the Polar Cooperative Navigation Algorithm

Based on the above analysis, the leader-follower type is superior to the parallel type of navigation system. Like the formation configuration shown in Figure 2, the leader UUV is equipped with high-precision navigation devices. To simplify the analysis and combine the characteristics of this article, the leader UUV errors are ignored. As both the leader UUV and follower UUVs are equipped with depth meters, the depth information from each UUV can be obtained accurately. In addition, the communication mode from the leader UUV to each follower UUV is the same, so the navigation between one leader UUV and one follower UUV is considered in this paper. The situation of multiple follower UUVs can be deduced by analogy. The overall process of the polar cooperative navigation algorithm can be expressed as shown in Figure 3.

Figure 3. Overall process of the polar cooperative navigation algorithm.

Both the leader UUV and the follower UUVs are equipped with high-precision timing clocks. The clock synchronization is achieved before the multi-UUVs start their mission. The leader UUV sends information to the follower UUV through underwater acoustic communication, including the time, attitude, velocity, position of the leader UUV and the relative position between the leader UUV and follower UUV. The relative distance and orientation information between the leader UUV and follower UUV can be measured by the leader UUV through USBL. Through the combination of the information sent by the leader UUV, and information from its own navigation devices, the motion states of a follower UUV can be estimated accurately.

3. Error Equations of the Polar Cooperative Navigation Algorithm

In the leader-follower polar cooperative navigation system, the leader UUV is equipped with high-precision navigation devices, including a high-precision SINS, OCTANS, DVL, USBL and depth meter. Based on the polar grid navigation algorithm proposed in ref. [19], the leader UUV can accurately navigate in the polar region. Therefore, the attitude errors, velocity errors and position errors of the leader UUV are assumed to be zero. To save costs, the follower UUVs are equipped with low-precision navigation devices, including a low-precision SINS, DVL, USBL and depth meter. The navigation errors of the follower UUVs are bigger than those of the leader UUV, which cannot be ignored. The error equations of the follower UUV should be established based on the UUV characteristics and the environment of the polar region.

In the polar region, there will be calculation overflow and error magnification of the navigation in conventional north-oriented SINS. The direction information and the orientation information cannot be distinguished in conventional wander-oriented SINS. Therefore, conventional SINS is invalid in the polar region. The polar grid navigation algorithm proposed in [19] can solve these problems. Based on the polar grid navigation algorithm and the characteristics of a UUV, the attitude error equation, velocity error equation and position error equation of a follower UUV can be deduced as follows.

The relationships among the frames are important parts of the SINS. To simplify the description, the frames involved in this paper are briefly represented by the following symbols:

• Inertial frame—$i$ frame;
• Navigation frame—$n$ frame;
• Body frame of UUV—$b$ frame;
• Geographic frame—$g$ frame;
• Earth centered earth fixed frame—$e$ frame;
• Grid frame—$G$ frame;
• Body frame of DVL—$m$ frame;
• Acoustic array frame of USBL—$a$ frame.

3.1. Attitude Error Equation of a Follower UUV

Based on the polar grid navigation algorithm proposed in ref. [19] and in the $G$ frame, the attitude differential equation of the follower UUV in ideal conditions and in actual conditions can be described by Equations (2) and (3), respectively.

$${\dot{\mathit{C}}}_b^G={\mathit{C}}_b^G\left[{\mathit{\omega }}_{ib}^b\times \right]-\left[{\mathit{\omega }}_{iG}^G\times \right]{\mathit{C}}_b^G,$$

(2)

$${\dot{\widehat{\mathit{C}}}}_b^{G^{\prime }}={\widehat{\mathit{C}}}_b^G\left[{\widehat{\mathit{\omega }}}_{ib}^b\times \right]-\left[{\widehat{\mathit{\omega }}}_{iG}^G\times \right]{\widehat{\mathit{C}}}_b^G,$$

(3)

where ${\mathit{C}}_b^G$ and ${\widehat{\mathit{C}}}_b^G$ are the direction cosine matrices from the $b$ frame to the $G$ frame in ideal conditions and in actual conditions, respectively; ${\mathit{\omega }}_{ib}^b$ and ${\widehat{\mathit{\omega }}}_{ib}^b$ are the rotational angular velocities from the $i$ frame to the $b$ frame in the $i$ frame in ideal conditions and in actual conditions, respectively; and $\left[{\mathit{\omega }}_{ib}^b\times \right]$ is the anti-symmetric matrix of ${\mathit{\omega }}_{ib}^b$.

Subtracting Equation (2) from Equation (3), the attitude error equation of the follower UUV in the $G$ frame can be expressed as follows:

$${\dot{\mathit{\varphi }}}^G={\mathit{C}}_{\omega }{}^{-1}(-({\mathit{\omega }}_{iG}^G\times ){\mathit{\varphi }}^G+({\mathit{C}}_{\omega ieR}+{\mathit{C}}_{\omega eGR})\delta {\mathit{R}}^e+{\mathit{C}}_{\omega eGv}\delta {\mathit{V}}^G-{\mathit{C}}_b^{G^{\prime }}{\mathit{\epsilon }}^b),$$

(4)

where ${\mathit{\varphi }}^G={\left[{\varphi }_x^G {\varphi }_y^G {\varphi }_z^G\right]}^{\mathrm{T}}$ is the misalignment angle; $\delta {\mathit{V}}^G={\left[\delta V_x^G \delta V_y^G \delta V_z^G\right]}^{\mathrm{T}}$ is the velocity error; $\delta {\mathit{R}}^e={\left[\delta x \delta y \delta z\right]}^{\mathrm{T}}$ is the position error; and ${\mathit{\epsilon }}^b$ consists of gyro constant drifts, ${\mathit{\epsilon }}_c^b$, and gyro random drifts, ${\mathit{\epsilon }}_w^b$, that can be set as zero-mean Gaussian white noise.

$${\mathit{C}}_{\omega }=\left[\begin{array}{l}\cos {\varphi }_y^G 0 -\sin {\varphi }_y^G\cos {\varphi }_x^G\\ 0 1 \sin {\varphi }_x^G\\ \sin {\varphi }_y^G 0 \cos {\varphi }_y^G\cos {\varphi }_x^G\end{array}\right],$$

(5)

$${\mathit{C}}_{\omega ieR}=\frac{{\omega }_{ie}}{R_{eh}{\left(1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda \right)}^{\frac{3}{2}}}\cdot \left[\begin{array}{l}2{\mathrm{c}}^{2}L\mathrm{s}Ls\lambda \mathrm{c}\lambda -\mathrm{s}L\left[{\mathrm{c}}^2\lambda +{\mathrm{s}}^2\lambda \cdot \left({\mathrm{s}}^{2}L-{\mathrm{c}}^{2}L\right)\right] {\mathrm{s}}^{2}L\mathrm{c}L\mathrm{s}\lambda \\ {\mathrm{s}}^{2}L 0 -\mathrm{s}L\mathrm{c}L\mathrm{c}\lambda \\ -\mathrm{s}L\mathrm{c}L\mathrm{c}\lambda -\mathrm{s}L\mathrm{c}L\mathrm{s}\lambda {\mathrm{c}}^{2}L\end{array}\right],$$

(6)

$${\mathit{C}}_{\omega eGR}=\frac{1}{R_{eh}^2}\cdot \left[\begin{array}{l} v_{GN}\mathrm{c}L\mathrm{c}\lambda v_{GN}\mathrm{c}L\mathrm{s}\lambda v_{GN}\mathrm{s}L\\ -v_{GE}\mathrm{c}L\mathrm{c}\lambda -v_{GE}\mathrm{c}L\mathrm{s}\lambda -v_{GE}\mathrm{s}L\\ \frac{2 v_{GN} {\mathrm{c}}^{2}L\mathrm{s}\lambda \mathrm{c}\lambda }{{\left(1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda \right)}^{\frac{3}{2}}} - v_{GN}\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda } \frac{2 v_{GN} \mathrm{s}L\mathrm{c}L\mathrm{s}\lambda }{{\left(1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda \right)}^{\frac{3}{2}}}\end{array}\right],$$

(7)

where $s(\cdot )$ and $c(\cdot )$ represent $\sin (\cdot )$ and $\cos (\cdot )$, respectively. $L$ and $\lambda$ represent the latitude and longitude of the position, respectively.

3.2. Velocity Error Equation of a Follower UUV

Based on the $G$ frame, the velocity differential equations of the follower UUV in ideal conditions and in actual conditions can be described with Equations (8) and (9), respectively.

$${\dot{\mathit{V}}}^G={\mathit{C}}_b^G{\mathit{f}}^b-\left(2{\mathit{\omega }}_{ie}^G+{\mathit{\omega }}_{eG}^G\right)\times {\mathit{V}}^G+{\mathit{g}}^G,$$

(8)

$${\dot{\widehat{\mathit{V}}}}^G={\widehat{\mathit{C}}}_b^G{\widehat{\mathit{f}}}^b-\left(2{\widehat{\mathit{\omega }}}_{ie}^G+{\widehat{\mathit{\omega }}}_{eG}^G\right)\times {\widehat{\mathit{V}}}^G+{\widehat{\mathit{g}}}^G,$$

(9)

where ${\mathit{V}}^G$ and ${\widehat{\mathit{V}}}^G$ are the velocities of the follower UUV in ideal conditions and in actual conditions, respectively; ${\mathit{f}}^b$ and ${\widehat{\mathit{f}}}^b$ are the special forces measured by the SINS in ideal conditions and in actual conditions, respectively; ${\mathit{g}}^G$ and ${\widehat{\mathit{g}}}^G$ are the gravity accelerations in ideal conditions and in actual conditions, respectively.

Subtracting Equation (8) from Equation (9) and ignoring the second-order small terms, the velocity error equation of the follower UUV can be expressed as follows:

$$\delta {\dot{\mathit{V}}}^G={\mathit{f}}^G\times {\mathit{\varphi }}^G+\left[{\mathit{V}}^G\times {\mathit{C}}_{\omega eGV}-\left(2{\mathit{\omega }}_{ie}^G+{\mathit{\omega }}_{eG}^G\right)\times \right] \cdot \delta {\mathit{V}}^G +\left[{\mathit{V}}^G\times \left(2{\mathit{C}}_{\omega ieR}+{\mathit{C}}_{\omega eGR}\right)\right]\delta {\mathit{R}}^e+{\mathit{C}}_b^G{\mathbf{\nabla }}^b,$$

(10)

where ${\mathbf{\nabla }}^b$ is the accelerator bias, consisting of an accelerator constant bias, ${\mathbf{\nabla }}_c^b$, and an accelerator random bias, ${\mathbf{\nabla }}_w^b$, which can be set as zero-mean Gaussian white noise [20].

3.3. Position Error Equation of the Follower UUV

Due to the rapid convergence of the Earth’s meridians and the meridians converging in the poles, the conventional latitude and longitude representation is no longer applicable in the polar region. In the polar grid navigation algorithm, the coordinates in the $e$ frame, ${\mathit{R}}^e\left(x,y,z\right)$, are used to describe the position of the UUV.

Based on the $G$ frame, the position’s differential equation for the follower UUV in ideal conditions and in actual conditions can be described by Equations (11) and (12), respectively.

$${\dot{\mathit{R}}}^e={\mathit{C}}_G^e{\mathit{V}}^G,$$

(11)

$${\dot{\widehat{\mathit{R}}}}^e={\widehat{\mathit{C}}}_G^e{\widehat{\mathit{V}}}^G,$$

(12)

where ${\mathit{R}}^e$ and ${\widehat{\mathit{R}}}^e$ are the positions of the follower UUV in ideal conditions and in actual conditions, respectively; ${\mathit{C}}_G^e$ and ${\widehat{\mathit{C}}}_G^e$ are the direction cosine matrices from the $G$ frame to the $e$ frame in ideal conditions and in actual conditions, respectively.

By subtracting Equation (11) from Equation (12), the position error equation of the follower UUV can be described as follows:

$$\delta {\dot{\mathit{R}}}^e={\mathit{C}}_G^e\delta {\mathit{V}}^G-{\mathit{C}}_G^e\left({\mathit{V}}^G\times \right){\mathit{C}}_{\beta R}\delta {\mathit{R}}^e,$$

(13)

where

$${\mathit{C}}_{\beta R}=\frac{1}{R_{eh}}\left[\begin{array}{l} \frac{\mathrm{s}L}{\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }} 0 \frac{-\mathrm{c}\lambda \mathrm{c}L}{\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }}\\ \frac{\mathrm{s}\lambda \mathrm{c}\lambda {\mathrm{c}}^{2}L}{\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }} \sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda } \frac{-s\lambda \mathrm{s}L\mathrm{c}L}{\sqrt{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }}\\ \frac{\mathrm{c}L\mathrm{s}L\mathrm{s}\lambda }{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda } 0 - \frac{{\mathrm{c}}^{2}L\mathrm{s}\lambda \mathrm{c}\lambda }{1-{\mathrm{c}}^{2}L{\mathrm{s}}^2\lambda }\end{array}\right],$$

(14)

3.4. Error Model of DVL in the Follower UUV

Because of the accumulative errors in the SINS and the low-precision navigation devices, there will be cumulative errors in the follower UUV. According to the Doppler Effect, the DVL can measure the velocity of the UUV. With the help of the DVL, the follower UUV can correct its velocity to improve the navigation accuracy. However, there are installation errors, scale factor errors, and random measurement errors affecting the accuracy of the DVL, because of the characteristics of the DVL. The output of the DVL can be described in the $m$ frame and the $G$ frame as Equations (15) and (16):

$${\widehat{\mathit{V}}}_d^m=\left(1+\delta {\mathit{K}}_d\right){\mathit{V}}_d^m+\delta {\mathit{V}}_d^m+{\mathit{v}}_d^m,$$

(15)

$${\widehat{\mathit{V}}}_d^G={\mathit{V}}_d^G+{\mathit{v}}_d^G={\mathit{C}}_b^G{\mathit{C}}_m^b{\mathit{V}}_d^m={\mathit{C}}_b^G\left[\left({\widehat{\mathit{V}}}_d^m-\delta {\mathit{V}}_d^m\right)/\left(1+\delta {\mathit{K}}_d\right)\right],$$

(16)

where ${\widehat{\mathit{V}}}_d^m$ and ${\mathit{V}}_d^m$ are the actual velocity and ideal velocity, respectively; ${\mathit{C}}_m^b$ is the direction cosine matrix from the $m$ frame to the $b$ frame, which is approximately equal to ${\mathit{C}}_m^b=\mathit{I}$, since the installation error is small enough to be neglected; the scale factor and the random velocity error can be expressed by a random constant and the one-order Markov process as follows:

$$\{\begin{array}{l}\delta {\dot{\mathit{K}}}_d=0\\ \delta {\dot{\mathit{V}}}_d^m=-\delta {\mathit{V}}_d^m/{\tau }_v+{\mathit{w}}_v\end{array},$$

(17)

where ${\tau }_v$ is the correlation time and ${\mathit{w}}_v$ is the white noise.

3.5. Error Model of USBL

The USBL can obtain the relative position between the leader UUV and a follower UUV through the time that the acoustic signal arrives at the acoustic element. Since the size of the USBL acoustic array is particularly small, it is easy to choose a position on the carrier with low noise. Therefore, the application of the USBL is flexible and convenient.

The USBL is mainly composed of a transducer, an acoustic emission array, a transponder and an acoustic receiving array. The acoustic emission array and acoustic receiving array are fixed in the transducer to form the acoustic array. The mutual positions between the acoustic elements constituting the acoustic array have been precisely determined. The transponder is installed on the follower UUV. The phase comparison method is used in this system. The orientation of the transponder in the acoustic array frame is obtained by measuring the phase difference of the acoustic element. The relative distance between the leader UUV and the follower UUV is calculated by the propagation time of the acoustic signal in water. According to the above information, the relative position between the leader UUV and follower UUV can be determined.

$$R^2=X_a^2+Y_a^2+h^2,$$

(18)

$$X_a^2=R^2{\cos }^2{\theta }_{mx},$$

(19)

$$Y_a^2=R^2{\cos }^2{\theta }_{my},$$

(20)

where $R$ is the distance from the acoustic emission array to the acoustic receiving array; $X_a$ and $Y_a$ are the coordinates of the follower UUV in the acoustic array frame; ${\theta }_{mx}$ and ${\theta }_{my}$ can be measured by the USBL; ${\theta }_{mx}$ is the angle between the acoustic ray and the $X_a$ axis; ${\theta }_{my}$ is the angle between the acoustic ray and the $Y_a$ axis; $h$ is the relative depth difference between the leader UUV and the follower UUV. In addition, the depth information regarding the leader UUV and the follower UUV can be obtained from their own depth meters, respectively.

From Equations (18)–(20), $X_a$ and $Y_a$ can be obtained as follows:

$$X_a=\frac{h\cos {\theta }_{mx}}{\sqrt{1-{\cos }^2{\theta }_{mx}-{\cos }^2{\theta }_{my}}},$$

(21)

$$Y_a=\frac{h\cos {\theta }_{my}}{\sqrt{1-{\cos }^2{\theta }_{mx}-{\cos }^2{\theta }_{my}}},$$

(22)

The main factors that affect the accuracy of the USBL are the error of the system, a measurement error for the marine environmental parameters and an acoustic array installation error. To simplify the analysis and highlight the subject of this paper, these errors are regarded as zero-mean Gaussian white noise. The error equation of the USBL in the $e$ frame can be described as follows:

$$\delta {\dot{\mathit{R}}}_a^e={\mathit{C}}_a^e\delta {\dot{\mathit{R}}}^a={\mathit{C}}_a^e{\mathit{\beta }}^a,$$

(23)

where ${\mathit{\beta }}^a$ is zero-mean Gaussian white noise.

4. Filter Models of the Follower UUV in the Polar Cooperative Navigation Algorithm

Filter models of the follower UUV can be established based on the analysis in Section 3. The filter models consist of a dynamic model and an observation model. Details about the filter models are described as follows.

4.1. Dynamic Model

The states in the follower UUV are SINS states, DVL states and USBL states, which can be described as:

$$\mathit{X}={\left[{\left({\mathit{\varphi }}^G\right)}^{\mathrm{T}} {\left(\delta {\mathit{V}}^G\right)}^{\mathrm{T}} {\left(\delta {\mathit{R}}^e\right)}^{\mathrm{T}} {\left({\mathit{\epsilon }}_c^b\right)}^{\mathrm{T}} {\left({\mathbf{\nabla }}_c^b\right)}^{\mathrm{T}} {\left(\delta {\mathit{K}}_d\right)}^{\mathrm{T}} {\left(\delta {\mathit{V}}_d^m\right)}^{\mathrm{T}} {\left({\mathit{\beta }}^a\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}$$

Based on the attitude error equation, velocity error equation, and position error equation of the SINS (Equations (4), (10) and (13)) and the error models of the DVL and USBL (Equations (17) and (23)), the dynamic model of the follower UUV can be described as follows:

$$\{\begin{array}{l}{\dot{\mathit{\varphi }}}^G={\mathit{C}}_{\omega }{}^{-1}[-({\mathit{\omega }}_{iG}^G\times ){\mathit{\varphi }}^G+({\mathit{C}}_{\omega ieR}+{\mathit{C}}_{\omega eGR})\delta {\mathit{R}}^e+{\mathit{C}}_{\omega eGv}\delta {\mathit{V}}^G-{\mathit{C}}_b^{G^{\prime }}{\mathit{\epsilon }}^b]\hfill \\ \begin{array}{l}\delta {\dot{\mathit{V}}}^G={\mathit{f}}^G\times {\mathit{\varphi }}^G+[{\mathit{V}}^G\times {\mathit{C}}_{\omega eGv} -\left(2{\mathit{\omega }}_{ie}^G+{\mathit{\omega }}_{eG}^G\right)\times ]\cdot \delta {\mathit{V}}^G +\left[{\mathit{V}}^G\times \left(2{\mathit{C}}_{\omega ieR}+{\mathit{C}}_{\omega eGR}\right)\right]\delta {\mathit{R}}^e+{\mathit{C}}_b^G{\mathbf{\nabla }}^b\\ \delta {\dot{\mathit{R}}}^e={\mathit{C}}_G^e\delta {\mathit{V}}^G-{\mathit{C}}_G^e\left({\mathit{V}}^G\times \right){\mathit{C}}_{\phi R}\delta {\mathit{R}}^e\end{array}\hfill \\ {\dot{\mathit{\epsilon }}}^b = 0\hfill \\ \begin{array}{l}{\dot{\mathbf{\nabla }}}^b = 0\\ \delta {\dot{\mathit{K}}}_d=0\\ \delta {\dot{\mathit{V}}}_d^m=-\delta {\mathit{V}}_d^m/{\tau }_V+{\mathit{w}}_V\\ {\dot{\mathit{\beta }}}^a=0\end{array}\hfill \end{array},$$

(24)

This dynamic model can be expressed in the form of a vector as:

$$\dot{\mathit{X}}=\mathit{A}\mathit{X}+\mathit{B}\mathit{W},$$

(25)

where $\mathit{A}$ and $\mathit{B}$ are the system matrix and control matrix, respectively. $\mathit{W}$ is the system noise matrix. $\mathit{A}$, $\mathit{B}$ and $\mathit{W}$ can be expressed as follows:

$$\mathit{A}=\left[\begin{array}{l}{\mathit{A}}_{15\times 15}^{SINS} {\mathbf{0}}_{15\times 6} {\mathbf{0}}_{15\times 3}\\ {\mathbf{0}}_{6\times 15} {\mathit{A}}_{6\times 6}^{DVL} {\mathbf{0}}_{6\times 6} \\ {\mathbf{0}}_{3\times 15} {\mathbf{0}}_{3\times 6} {\mathit{A}}_{3\times 3}^{USBL}\end{array}\right],$$

$$\begin{gathered} \mathit{B}={\left[\begin{array}{l}{\mathit{B}}_1 {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3} {\mathit{B}}_2 {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{12\times 3} {\mathbf{0}}_{12\times 3} {\mathbf{0}}_{12\times 3}\\ {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3} {\mathit{I}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3}\end{array}\right]}^{\mathrm{T}}, \\ {\mathit{A}}_{15\times 15}^{SINS}=\left[\begin{array}{l}{\mathit{A}}_1 {\mathit{A}}_2 {\mathit{A}}_3 {\mathit{A}}_4 {\mathbf{0}}_{3\times 3}\\ {\mathit{A}}_5 {\mathit{A}}_6 {\mathit{A}}_7 {\mathbf{0}}_{3\times 3} {\mathit{A}}_8\\ {\mathbf{0}}_{3\times 3} {\mathit{A}}_9 {\mathit{A}}_{10} {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3}\end{array}\right], \\ {\mathit{A}}_{6\times 6}^{DVL}=\left[\begin{array}{l}{\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3} \\ {\mathbf{0}}_{3\times 3} {\mathit{A}}_{11}\end{array}\right], \\ {\mathit{A}}_{3\times 3}^{USBL}=\left[{\mathbf{0}}_{3\times 3}\right], \\ \mathit{W}={\left[{\left({\mathit{\epsilon }}_r^b\right)}^{\mathrm{T}} {\left({\mathbf{\nabla }}_r^G\right)}^{\mathrm{T}} {\left({\mathit{w}}_v\right)}^{\mathrm{T}}\right]}^{\mathrm{T}}. \end{gathered}$$

where ${\mathit{A}}_1={\mathit{C}}_{\omega }{}^{-1} (-({\mathit{\omega }}_{iG}^G\times ))$, ${\mathit{A}}_2={\mathit{C}}_{\omega }{}^{-1}{\mathit{C}}_{\omega eGv}$, ${\mathit{A}}_3={\mathit{C}}_{\omega }{}^{-1}({\mathit{C}}_{\omega ieR}+{\mathit{C}}_{\omega eGR})$, ${\mathit{A}}_4=-{\mathit{C}}_{\omega }{}^{-1}{\mathit{C}}_b^{G^{\prime }}$, ${\mathit{A}}_5=\left({\mathit{f}}^G\times \right)$, ${\mathit{A}}_6=\left[{\mathit{V}}^G\times {\mathit{C}}_{\omega eGv}-\left(2{\mathit{\omega }}_{ie}^G+{\mathit{\omega }}_{eG}^G\right)\times \right]$, ${\mathit{A}}_7= {\mathit{V}}^G\times \left(2{\mathit{C}}_{\omega ieR}+{\mathit{C}}_{\omega eGR}\right)$, ${\mathit{A}}_8={\mathit{C}}_b^G$, ${\mathit{A}}_9={\mathit{C}}_G^e$, ${\mathit{A}}_{10}=-{\mathit{C}}_G^e\left({\mathit{V}}^G\times \right){\mathit{C}}_{\beta R}$, ${\mathit{A}}_{11}=-1/{\tau }_v\cdot I_{3\times 3}$, ${\mathit{B}}_1={\mathit{C}}_{\omega }{}^{-1}{\mathit{C}}_b^{G^{\prime }}$, ${\mathit{B}}_2={\mathit{C}}_b^G$.

4.2. Observation Model

The SINS equipped in the follower UUV has low precision. There are accumulated errors in the follower UUV, which cannot be ignored. Because the leader UUV is equipped with high-precision navigation devices, the information from the leader UUV can be used to correct the errors of the follower UUV. Based on the information sent from the leader UUV and the measurement results from the USBL, accurate position information for the follower UUV can be obtained. Therefore, the position error is chosen as one of the observation states. In addition, the accurate velocity information regarding the follower UUV can be provided by the DVL. Therefore, the velocity of the follower UUV calculated from the SINS can be corrected by the DVL. The other observation state is the velocity error. Based on the analysis above, the observation states can be expressed as:

$$\mathit{Z}={\left[\begin{array}{cc}{\left(\delta {\mathit{V}}^G\right)}^{\mathrm{T}}& {\left(\delta {\mathit{R}}^e\right)}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$$

The observation model can be described as follows:

$$\dot{\mathit{Z}}=\mathit{H}\mathit{X}+\mathit{V},$$

(26)

where $\mathit{H}$ and $\mathit{V}$ are the observation matrix and measurement noise vector, respectively. $\mathit{V}$ is regarded as zero-mean Gaussian white noise and $\mathit{H}$ can be expressed as follows:

$$\mathit{H}={\left[\begin{array}{l}{\mathit{I}}_{3\times 3} {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 15}\\ {\mathbf{0}}_{3\times 3} {\mathbf{0}}_{3\times 3} {\mathit{I}}_{3\times 3} {\mathbf{0}}_{3\times 15}\end{array}\right]}^{\mathrm{T}}$$

5. Modified Adaptive Kalman Filter Considering Communication Delays

According to the characteristics of polar cooperative navigation, there is a communication delay in the process of multi-source information fusion. Based on the acoustic communication delay’s characteristics and the role of underwater acoustic communication in the whole system, a system model of underwater acoustic delay can be constructed. In conventional AKF, the estimation results of the states are corrected according to real-time measurement information. Thus, conventional AKF which does not consider communication delay is not suitable for polar cooperative navigation of multi-UUVs. The schematic diagram of polar cooperative navigation of multi-UUVs considering communication delays is expressed in Figure 4.

Figure 4. The schematic diagram of polar cooperative navigation of multi-UUVs considering communication delays.

The estimation processes of attitude, velocity and position in the follower UUV are described in Figure 4. These states are provided by the SINS in the follower UUV. Because of its low precision, there are errors in these states. The DVL, USBL and the information sent from the leader UUV are used to correct these states. Due to the underwater acoustic communication delay, the feedback position received by the follower UUV at time $k$ is not the position measured at time $k$. Conventional AKF corrects these states based on the real-time measured states. This method is not suitable for communication delay conditions. The conventional AKF is modified in this paper to consider underwater acoustic communication delays. The states of the follower UUV at time $k$ are corrected based on the states received by the follower UUV at time $k$. In addition, these states are not measured at time $k$. More accurate results can be obtained from the modified AKF.

The discrete expressions of the filter model and observation model can be expressed as follows:

$$\dot{\mathit{X}}={\mathbf{\Phi }}_{k,k-1}\mathit{X}+{\mathbf{\Gamma }}_{k,k-1}\mathit{W},$$

(27)

$$\dot{\mathit{Z}}={\mathit{H}}_k\mathit{X}+\mathit{V},$$

(28)

where the discrete expressions of $\mathit{A}$, $\mathit{B}$ and $\mathit{H}$ are ${\mathbf{\Phi }}_{k,k-1}$, ${\mathbf{\Gamma }}_{k,k-1}$ and ${\mathit{H}}_k$, respectively. Considering the characteristics of the application environment, the quantitative analysis is discussed in the following section. The conventional AKF is modified, as follows, to adapt for use in the polar cooperative navigation of multi-UUVs considering communication delays.

5.1. Conventional Adaptive Kalman Filter

Conventional AKF mainly consists of state updates (prediction) and measurement updates (correction) [21,22]. The states of this moment are estimated by the comprehensive states obtained at the last moment. Because of errors, these states need to be corrected based on the measurement results. Then the estimation result for the states can be obtained. The complete formula for the conventional AKF can be expressed by Equations (29)–(38).

$${\mathit{X}}_{k,k-1}={\mathbf{\Phi }}_{k,k-1}{\widehat{\mathit{X}}}_{k-1}+{\widehat{\mathit{q}}}_k,$$

(29)

$${\mathit{P}}_{k,k-1}={\mathbf{\Phi }}_{k,k-1}{\mathit{P}}_{k-1}{\mathbf{\Phi }}_{k,k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k,k-1}{\widehat{\mathit{Q}}}_{k-1}{\mathbf{\Gamma }}_{k,k-1}^{\mathrm{T}},$$

(30)

$${\mathit{v}}_k={\mathit{Z}}_k-{\mathit{H}}_k{\mathit{X}}_{k/k-1}-{\widehat{\mathit{r}}}_k,$$

(31)

$${\mathit{K}}_k={\mathit{P}}_{k,k-1}{\mathit{H}}_k^T{[{\mathit{H}}_k{\mathit{P}}_{k/k-1}{\mathit{H}}_k^T+{\widehat{\mathit{R}}}_k]}^{-1},$$

(32)

$${\widehat{\mathit{X}}}_k={\widehat{\mathit{X}}}_{k/k-1}+{\mathit{K}}_k{\mathit{v}}_k,$$

(33)

$${\mathit{P}}_k=(\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k){\mathit{P}}_{k/k-1},$$

(34)

$${\widehat{q}}_{k+1}=(1-d_k){\widehat{q}}_k+d_k({\mathit{X}}_{k+1}-{\mathit{X}}_{k+1,k}),$$

(35)

$${\widehat{\mathit{Q}}}_{k+1}=(1-d_k){\widehat{\mathit{Q}}}_k+d_k[{\mathit{K}}_{k+1}{V}_{k+1}{({\mathit{K}}_{k+1}{V}_{k+1})}^{\mathrm{T}}+{\mathit{P}}_{k+1}-{\mathbf{\Phi }}_{k+1,k}{\mathit{P}}_k{\widehat{\mathit{X}}}_k{\mathbf{\Phi }}_{k+1,k}^{\mathrm{T}}],$$

(36)

$${\widehat{r}}_{k+1}=(1-d_k){\widehat{r}}_k+d_k({\mathit{Z}}_{k+1}-{\mathit{H}}_{k+1,k}{\mathit{X}}_{k+1,k}),$$

(37)

$${\widehat{\mathit{R}}}_{k+1}=(1-d_k){\widehat{\mathit{R}}}_k+d_k[V_{k+1}{V}_{k+1}^T-{\mathit{H}}_{k+1}{\mathit{P}}_{k+1,k}{\mathit{H}}_{k+1}^T],$$

(38)

where $\mathit{P}$ is the covariance of the states; the mean and the covariance of the system noise matrix, $\mathit{W}$, are ${\widehat{q}}_k$ and ${\widehat{\mathit{Q}}}_k$, respectively; ${\widehat{r}}_k$ and ${\widehat{\mathit{R}}}_k$ are the mean and the covariance of the measurement noise matrix, $\mathit{V}$; $d_k=(1-b)/(1-b^k)$ and $0<b<1$ are the forgetting factors.

5.2. Underwater Acoustic Communication Delay in AKF

Due to the underwater acoustic communication delay, the states of the follower UUV cannot be corrected by the measurement results at the current time. To meet the needs of the multi-UUVs cooperative navigation algorithm considering communication delays for use in the polar region, the conventional AKF needs to be modified. The underwater acoustic communication delay is a major factor in improving the accuracy of the conventional AKF. The analysis and special representation of underwater acoustic communication delays are discussed in the following text.

According to the analysis in Section 2, the underwater acoustic communication delay is a random value. The acoustic communication delay can be easily understood by comparing the time axes of the leader UUV and the follower UUV. The time axes of the leader UUV and the follower UUV are shown as Figure 5.

Figure 5. Time axes of the leader UUV and follower UUV.

As shown in Figure 5, it is assumed that the leader UUV’s time and the follower UUV’s time are the same. The initial time of the leader UUV, t₀, is also the initial time of the follower UUV. The leader UUV sends a detection signal through USBL to the follower UUV at t₁. At the same time, information about the states of the leader UUV is self-recorded. Because of the underwater acoustic communication delay, this detection signal will be received by the follower UUV at t₂. After a short reaction time, the follower UUV sends a confirming signal to the leader UUV through USBL at t₃. This confirming signal is received by the leader UUV at t₄. The leader UUV calculates the relative positions of the leader UUV and the follower UUV. Then, the relative position and the information of the leader UUV at t₁ are sent to the follower UUV at t₅. After broadcasting through the water, this information is received by the follower UUV at t₆. The underwater acoustic delay can be described as follows [14,15].

$$\Delta t_1=t_2-t_1,$$

(39)

$$\Delta t_2=t_3-t_2,$$

(40)

$$\Delta t_3=t_4-t_3,$$

(41)

$$\Delta t_4=t_5-t_4,$$

(42)

$$\Delta t_5=t_6-t_5,$$

(43)

$$\Delta t_6=t_6-t_1,$$

(44)

where $\Delta t_1$ is the transmission time of the acoustic signal from the leader UUV to the follower UUV; $\Delta t_2$ is the reaction time for the follower UUV to respond to the leader UUV through the USBL; $\Delta t_3$ is the transmission time for the acoustic signal from the follower UUV to the leader UUV; $\Delta t_4$ is the calculation time for the leader UUV to obtain the relative position between the leader UUV and the follower UUV through USBL; $\Delta t_5$ is the transmission time for the acoustic signal from the leader UUV to the follower UUV; $\Delta t_6$ is the total underwater acoustic delay during a measurement cycle.

The underwater acoustic delay consists of the underwater acoustic detection delay and the underwater acoustic communication delay. During the measurement cycle, both $\Delta t_2$ and $\Delta t_4$ are underwater acoustic detection delays. $\Delta t_1$, $\Delta t_3$ and $\Delta t_5$ are the acoustic communication delays. Compared with the underwater acoustic communication delay, the value of the underwater acoustic detection delay is relatively fixed and small. These underwater acoustic detection delays are associated with the USBL’s performance, and they can be reduced by improving its performance. To simplify the analysis of this paper, the acoustic detection delays are ignored. Both the underwater acoustic detection delay and the underwater acoustic communication delay are regarded as the underwater acoustic communication delay.

To describe the underwater acoustic communication delay better, it can be quantified. Since time synchronization is achieved between the leader UUV and the follower UUV and the underwater acoustic communication delay is a limited value, the underwater acoustic communication delay can be expressed versus time. Thus, the acoustic communication delay can be quantized. After quantification, the underwater acoustic communication delay can be expressed as follows:

$$t=iT,$$

(45)

$$i\in \left\{1,2,3,4,\cdots ,D\right\},$$

(46)

where $t$ is the underwater acoustic communication delay; $T$ is the discretized unit time; $i$ is an integer that is associated with the acoustic communication delay and $D$ is an integer.

For example, $t=2T$ and $i=2$. This means that the states are obtained at time $k$, while the observations are measured at time $k-2$. Replaced by a general representation, the observation at time $k-i$ is used to correct the states at time $k$.

5.3. Modified Adaptive Kalman Filter Considering Communication Delays

Considering the existence of underwater acoustic communication delays, the information that the follower UUV receives from the leader UUV at time $k$ is not the observation at time $k$. It is measured at time $k-i$ [14,15]. Therefore, there will be errors when using conventional AKF. The states at time $k$ are corrected by observations measured at time $k$ in conventional AKF. However, in this paper, the states at time $k$ need to be corrected by observations measured at time $k-i$. According to the analysis above, the conventional AKF is modified to adapt to the polar cooperative navigation of multi-UUVs with consideration of communication delays. After an iterative calculation has been completed $i$ times, Equation (27) can be expressed as follows:

$${\mathit{X}}_k={\mathbf{\Phi }}_{k,k-i}{\mathit{X}}_{k-i}+{\mathbf{\Phi }}_{k,k-i+1}{\mathbf{\Gamma }}_{k,k-i+1}{\mathit{W}}_{k-i+1}+\cdots +{\mathbf{\Gamma }}_{k,k-1}{\mathit{W}}_{k-1},$$

(47)

$${\dot{\mathit{\varphi }}}^G={\mathit{C}}_{\omega }{}^{-1}{\mathit{\omega }}_{G{G}^{\prime }}^{G^{\prime }}={\mathit{C}}_{\omega }{}^{-1}\left(\left({\mathit{\varphi }}^G\times \right){\mathit{\omega }}_{iG}^G+\delta {\mathit{\omega }}_{iG}^G-{\mathit{C}}_b^{G^{\prime }}\delta {\mathit{\omega }}_{ib}^b\right),$$

(48)

Based on Equation (47), the states at time k − i can be described as follows:

$${\mathit{X}}_{k-i}={\mathbf{\Phi }}_{k,k-i}^{-1}{\mathit{X}}_k-{\mathbf{\Phi }}_{k,k-i}^{-1}{\mathbf{\Phi }}_{k,k-i+1}{\mathbf{\Gamma }}_{k,k-i+1}{\mathit{W}}_{k-i+1}-\cdots -{\mathbf{\Phi }}_{k,k-i}^{-1}{\mathbf{\Gamma }}_{k,k-1}{\mathit{W}}_{k-1},$$

(49)

By substituting Equation (49) into (28), the observation model can be modified as follows:

$${\mathit{Z}}_{k-i}={\mathit{H}}_{k-i}{\mathbf{\Phi }}_{k,k-i}^{-1}{\mathit{X}}_k+{\overline{\mathit{V}}}_{k-i},$$

(50)

$${\overline{\mathit{V}}}_{k-i}=-{\mathit{H}}_{k-i}{\mathbf{\Phi }}_{k,k-i}^{-1}{\mathbf{\Phi }}_{k,k-i+1}{\mathbf{\Gamma }}_{k,k-i+1}{\mathit{W}}_{k-i+1} -\cdots -{\mathit{H}}_{k-i}{\mathbf{\Phi }}_{k,k-i}^{-1}{\mathbf{\Gamma }}_{k,k-1}{\mathit{W}}_{k-1}+{\mathit{V}}_{k-i},$$

(51)

Therefore, the system model and observation model of the modified AKF adapted to polar cooperative navigation of multi-UUVs considering communication delays can be expressed as follows:

$${\mathit{X}}_k={\mathbf{\Phi }}_{k,k-1}{\mathit{X}}_{k-1}+{\mathbf{\Gamma }}_{k,k-1}{\mathit{W}}_{k-1},$$

(52)

$${\mathit{Z}}_{k-i}=\mathit{H}{\mathbf{\Phi }}_{k,k-i}^{-1}{\mathit{X}}_k+{\overline{\mathit{V}}}_{k-i},$$

(53)

To maintain the positive definiteness of the covariance of system noise matrix and measurement noise matrix as well as to simplify the system, Equations (36) and (38) in the conventional AKF are modified as follows:

$${\widehat{\mathit{Q}}}_{k+1}=(1-d_k){\widehat{\mathit{Q}}}_k+d_k[{\mathit{K}}_{k+1}{v}_{k+1}{({\mathit{K}}_{k+1}{v}_{k+1})}^{\mathrm{T}}+{\mathit{P}}_{k+1}],$$

(54)

$${\widehat{\mathit{R}}}_{k+1}=(1-d_k){\widehat{\mathit{R}}}_k+d_k[v_{k+1}{v}_{k+1}^{\mathrm{T}}],$$

(55)

The complete formula of the modified AKF considering communication delays and is adapted for the polar cooperative navigation of multi-UUVs can be expressed as follows:

$${\mathit{X}}_{k,k-1}={\mathbf{\Phi }}_{k,k-1}{\widehat{\mathit{X}}}_{k-1}+{\widehat{\mathit{q}}}_k,$$

(56)

$${\mathit{P}}_{k,k-1}={\mathbf{\Phi }}_{k,k-1}{\mathit{P}}_{k-1}{\mathbf{\Phi }}_{k,k-1}^{\mathrm{T}}+{\mathbf{\Gamma }}_{k,k-1}{\widehat{\mathit{Q}}}_{k-1}{\mathbf{\Gamma }}_{k,k-1}^{\mathrm{T}},$$

(57)

$$v_k={\mathit{Z}}_{k-i}-\mathit{H}{\mathbf{\Phi }}_{k,k-i}^{-1}{\mathit{X}}_{k/k-1}-{\widehat{\mathit{r}}}_k,$$

(58)

$${\mathit{K}}_k={\mathit{P}}_{k,k-1}{[\mathit{H}{\mathbf{\Phi }}_{k,k-i}^{-1}]}^{\mathrm{T}}\cdot {[\mathit{H}{\mathbf{\Phi }}_{k,k-i}^{-1}{\mathit{P}}_{k/k-1}{\left(\mathit{H}{\mathbf{\Phi }}_{k,k-i}^{-1}\right)}^{\mathrm{T}}+{\widehat{\mathit{R}}}_k]}^{-1},$$

(59)

$${\widehat{\mathit{X}}}_k={\widehat{\mathit{X}}}_{k/k-1}+{\mathit{K}}_k{\mathit{v}}_k,$$

(60)

$${\mathit{P}}_k=(\mathit{I}-{\mathit{K}}_k\mathit{H}{\mathbf{\Phi }}_{k,k-i}^{-1}){\mathit{P}}_{k/k-1},$$

(61)

$${\widehat{q}}_{k+1}=(1-d_k){\widehat{q}}_k+d_k({\mathit{X}}_{k+1}-{\mathit{X}}_{k+1,k}),$$

(62)

$${\widehat{\mathit{Q}}}_{k+1}=(1-d_k){\widehat{\mathit{Q}}}_k+d_k[{\mathit{K}}_{k+1}{v}_{k+1}{({\mathit{K}}_{k+1}{v}_{k+1})}^{\mathrm{T}}+{\mathit{P}}_{k+1}],$$

(63)

$${\widehat{r}}_{k+1}=(1-d_k){\widehat{r}}_k+d_k({\mathit{Z}}_{k+1}-{\mathit{H}}_{k+1,k}{\mathit{X}}_{k+1,k}),$$

(64)

$${\widehat{\mathit{R}}}_{k+1}=(1-d_k){\widehat{\mathit{R}}}_k+d_k[v_{k+1}{v}_{k+1}^{\mathrm{T}}],$$

(65)

To clearly express the modified AKF, the filter flow chart of the modified AKF used for the polar cooperative navigation of multi-UUVs considering communication delays is shown in Figure 6.

Figure 6. Flow chart of modified AKF considering acoustic communication delays.

6. Results and Discussion

To verify the advantages of the proposed algorithm, a simulation and experiment were performed using the polar cooperative navigation algorithm of multi-UUVs considering communication delays. By comparing the results in different situations, the advantages of the proposed algorithm can be obtained. To simplify the analysis, the simulations and experiments met the limitations proposed in Section 2. Therefore, the leader-follower formation structure was adopted in this paper. The leader UUV was equipped with high-precision navigation devices and the navigation error could be ignored. The depth of both the leader UUV and follower UUV could be obtained accurately. Thus, the two-dimensional situation after projection is analyzed in this paper. The case of a single leader UUV and a single follower UUV is discussed in this paper. In addition, the timing of the UUVs was synchronized. The UUVs were ordered to finish a straight-line mission that is shown as Figure 7. The initial position of the UUV was (80° N, 120° E).

Figure 7. The sketch of the planned mission.

6.1. Simulation Results and Analyses

According to the conditions proposed above and the UUV characteristics, a simulation of the polar cooperative navigation algorithm of multi-UUVs considering communication delays was designed. The initial conditions and limitations were as follows:

System simulation conditions: the simulation time was 12 h; the filtering period was 0.1 s. Initial conditions of UUV: the initial position of UUV consisted of latitude, $L$, and longitude, $\lambda$; $L$ was 80° and $\lambda$ was 120°. The attitudes of the UUV, including pitch angle, roll angle, and yaw angle, were described by sine functions and the amplitude of these angles were 4°, 5° and 3°, respectively. The periods of these angles were 3 s 5 s, and 7 s, respectively. The initial phases of these angles were 0°, 0° and 0°, respectively. Initial conditions of SINS: the gyro drifts consisted of gyro constant drifts and gyro random drifts that were $0.03°/\mathrm{h}$ and ${\left(0.001°/\mathrm{h}\right)}^2$, respectively. Similarly, the accelerometer bias consisted of the accelerometer constant bias and the accelerometer random bias, which were $1\times {10}^{-6}{g}_0$ and ${\left(1\times {10}^{-7}{g}_0\right)}^2$, respectively. Initial conditions of DVL: in the DVL, velocity random drifts were set as $\delta {\mathit{V}}_{dx}^m=\delta {\mathit{V}}_{dy}^m=\delta {\mathit{V}}_{dz}^m=0.005 \mathrm{m}/\mathrm{s}$, and the correlation time was set as ${\tau }_{\mathit{V}}=5 \min$, and the scale factor error was set as $\delta K_d{=10}^{-4}$.

According to the conditions set above, the simulation results of the follower UUV, including attitude errors, velocity errors and position errors, can be expressed as described in the following text. There were two comparison simulations. In the first comparison simulation, different random communication delay ranges were set in the conventional AKF. The impact of communication delay on the conventional AKF is clearly expressed as Figure 8 and Table 1.

Figure 8. Estimation errors of conventional adaptive Kalman filter (AKF) with different ranges of random communication delay: (a) attitude estimation errors; (b) velocity estimation errors; (c) position estimation errors.

Table 1. RMS errors of polar cooperative navigation of multi-UUVs based on conventional AKF.

Parameters	D1	D2	D3	D4
${\varphi }_x$/(′)	0.9893	0.3130	0.0474	0.0734
${\varphi }_y$/(′)	0.0646	0.1698	0.2496	0.1341
${\varphi }_z$/(′)	3.0777	1.5085	0.8161	0.8719
$v_x$/(m/s)	0.0045	0.0083	0.0150	0.0202
$v_y$/(m/s)	0.0520	0.0184	0.0401	0.0028
$r_x$/(m)	63.667	40.433	21.188	3.120
$r_y$/(m)	28.437	21.082	13.080	4.240

In this simulation, the simulation time was 1 h and $D1\ge D2\ge D3\ge D4$. To clearly describe the impacts of the different ranges on the navigation accuracy of the follower UUV, the RMS errors of the attitude estimation errors, velocity estimation errors and position estimation errors are shown in Table 1. $D1$, $D2$, $D3$ and $D4$ represent the range integers of the communication delay and $D1\ge D2\ge D3\ge D4$.

As shown in Figure 8 and Table 1, the communication delay had an impact on the navigation accuracy of the follower UUV. The errors were divergent with time flow, and the communication delay with a bigger range had a larger impact on the results. Therefore, the communication delay must be taken into consideration. The modified AKF described in this paper can effectively improve the navigation accuracy of the follower UUV.

In the second comparison simulation, Algorithm 1 was compared with Algorithm 2. Algorithm 1 represents the polar cooperative navigation algorithm of multi-UUVs considering communication delays that is proposed in this paper. Algorithm 2 represents the polar cooperative navigation algorithm of multi-UUVs based on conventional AKF that does not consider communication delays. The estimation errors of the follower UUV, including attitude errors, velocity errors and position errors, are shown as Figure 9.

Figure 9. Estimation errors based on Algorithm 1 and Algorithm 2 in the simulation: (a) attitude estimation errors; (b) velocity estimation errors; (c) position estimation errors.

For a clearer description of the results obtained from the simulation, the RMS errors from attitude errors, velocity errors and position errors are shown in Table 2.

Table 2. RMS errors of Algorithm 1 and Algorithm 2 in the simulation.

Parameters	Algorithm 1	Algorithm 2
${\varphi }_x$/(′)	0.0229	13.5681
${\varphi }_y$/(′)	0.0291	10.1113
${\varphi }_z$/(′)	1.2218	62.1246
$v_x$/(m/s)	0.0021	0.0550
$v_y$/(m/s)	0.0022	0.0355
$r_x$/(m)	37.3563	1252.2
$r_y$/(m)	9.5654	1244.7

As shown in Figure 9 and Table 2, the proposed polar cooperative navigation algorithm of multi-UUVs considering communication delays was superior to the polar cooperative navigation algorithm based on conventional AKF. In the polar cooperative navigation algorithm of multi-UUVs considering communication delays, the estimation errors of attitude, velocity and position in the follower UUV converged quickly, and they were stable near zero.

6.2. Experiment Results and Analyses

The polar cooperative navigation algorithm of multi-UUVs considering communication delays has been discussed in this paper. Due to geographical constraints, the current experiment could be conducted in the polar region. Therefore, a semi-physical simulation experiment was performed, which means that the experiment was conducted in a non-polar region and the data measured during the experiment is used for the semi-physical simulation. According to the UUV characteristics, gyro drifts and accelerometer bias are natural characteristics of UUVs. They do not change in different locations. Therefore, the gyro drifts and accelerometer bias can be obtained from an experiment in the non-polar region. Practically-measured data and the simulated data comprise the experimental data. The experimental data is composed of the angular velocity, ${\widehat{\mathit{\omega }}}_{ib}^b$, and special force, ${\widehat{\mathit{f}}}^b$. The true angular velocity, ${\mathit{\omega }}_{ib}^b$, and gyro drifts, $\delta {\mathit{\omega }}_{ib}^b$, comprise the angular velocity, ${\widehat{\mathit{\omega }}}_{ib}^b$. The true special force, ${\mathit{f}}^b$, and accelerometer bias, $\delta {\mathit{f}}^b$, comprise the special force, ${\widehat{\mathit{f}}}^b$.

$${\widehat{\mathit{\omega }}}_{ib}^b= {\mathit{\omega }}_{ib}^b+ \delta {\mathit{\omega }}_{ib}^b= {\mathit{\omega }}_{ib}^b+{\mathit{\epsilon }}^b,$$

(66)

$${\widehat{\mathit{f}}}^b={\mathit{f}}^b+ \delta {\mathit{f}}^b={\mathit{f}}^b+{\mathbf{\nabla }}^b,$$

(67)

No matter whether gained from simulation or experiment, the true values of Inertial Measurement Unit (IMU) ${\mathit{\omega }}_{ib}^b$ and ${\mathit{f}}^b$ are the same. The values of ${\mathit{\omega }}_{ib}^b$ and ${\mathit{f}}^b$ can be gained from a simulation once the attitude variation and maneuvers of the UUV have been confirmed. The practically-measured data consisted of gyro drifts and the accelerometer bias that was supplied by the IMU in the UUV.

The experiment was conducted in a rectangular pool at our institute in a non-polar region (N45°73′ E127°41′). The follower UUV accomplished a uniform liner motion. The gyro drifts and the accelerometer bias of the follower UUV were extracted from the following UUV that was produced by our laboratory. The follower UUV, called White Dolphin-100 UUV, is shown in Figure 10.

Figure 10. White Dolphin-100 Unmanned Underwater Vehicle: (a) White Dolphin-100 UUV ready to be launched into the water; (b) White Dolphin-100 UUV during the experiment.

The gyro drifts measured in the non-polar region included gyro constant drifts and gyro random drifts. The gyro constant drifts were $0.03°/\mathrm{h}$ and the gyro random drifts were ${\left(4.094\times {10}^{-6} \mathrm{rad}/\mathrm{s}\right)}^2$, ${\left(4.308\times {10}^{-6} \mathrm{rad}/\mathrm{s}\right)}^2$ and ${\left(2.386\times {10}^{-6} \mathrm{rad}/\mathrm{s}\right)}^2$, respectively. The accelerometer biases measured in the non-polar region included accelerometer constant biases and accelerometer random biases. The accelerometer constant biases were $1\times {10}^{-6}{g}_0$ and the accelerometer random biases were ${\left(0.00156 {\mathrm{m}/\mathrm{s}}^2\right)}^2$, ${\left(0.001747 {\mathrm{m}/\mathrm{s}}^2\right)}^2$ and ${\left(0.0004063 {\mathrm{m}/\mathrm{s}}^2\right)}^2$, respectively. Other relevant parameters in the experiment were the same as those in the simulation.

The experimental results of the follower UUV, including attitude errors, velocity errors and position errors, are expressed in Figure 10. In this semi-physical simulation, Algorithm 1 was compared with Algorithm 2. Algorithm 1 represents the polar cooperative navigation algorithm of multi-UUVs considering communication delays that has been proposed in this paper. Algorithm 2 is based on conventional AKF which does not consider communication delays.

To describe the experimental results clearly, the RMS errors of attitude errors, velocity errors and position errors are expressed in Table 3.

Table 3. RMS errors of Algorithm 1 and Algorithm 2 in the experiment.

Parameters	Algorithm 1	Algorithm 2
${\varphi }_x$/(′)	0.0242	13.5848
${\varphi }_y$/(′)	0.0266	10.1178
${\varphi }_z$/(′)	1.2140	62.1555
$v_x$/(m/s)	0.0021	0.0551
$v_y$/(m/s)	0.0022	0.0355
$r_x$/(m)	37.1366	1253.2
$r_y$/(m)	9.4960	1244.3

Comparing the experimental results and the RMS errors, it can be seen that the proposed polar cooperative navigation algorithm of multi-UUVs considering communication delays can be used to estimate states effectively. More accurate information regarding the states can be obtained from the proposed algorithm compared with the conventional algorithm.

6.3. Discussions

Conventional SINS is difficult to apply in the polar region. Therefore, the cooperative navigation algorithm based on conventional SINS is not suitable for multi-UUVs in the polar region. Using the grid frame and the UUV characteristics, error equations for the follower UUV that are suitable for the polar environment were established. Using the polar grid algorithm proposed in paper [19], the polar cooperative navigation algorithm was proposed in this paper to solve the navigation problems of multi-UUVs in the polar region.

According to the first comparison simulation (results in Figure 8), communication delay has an impact on results. Because of the communication delay, the estimation errors are divergent, and longer communication delays have larger impacts on the accuracy because the states at time $k$ are corrected by the measurement at time $k-i$. A bigger $D$ means that the time delay is larger. In addition, the measurement states at time $k-i$ with larger communication delays will have less of a relationship with the states at time $k$. Low-precision navigation devices have divergent errors and these errors cannot be corrected. Therefore, communication delays cannot be ignored in the polar cooperative navigation algorithm.

Both the second comparison simulation (results in Figure 9) and the comparison experiment (results in Figure 11) showed that Algorithm 1 is superior to Algorithm 2. Algorithm 1 and Algorithm 2 are suitable for the sailing of multi-UUVs in the polar region. Because of the underwater acoustic communication delay, the information that the follower UUV receives from the leader UUV is not real-time information, rather it is delayed information. In Algorithm 2, the delayed information is misused as real-time information to achieve data fusion. However, in Algorithm 1, the modified AKF considering underwater acoustic communication delay can achieve data fusion using the delayed information. Therefore, there is a bigger drift-away signal or oscillatory result in Algorithm 2 than in Algorithm 1. In Algorithm 1, the communication delay is taken into consideration and the conventional AKF is modified based on the communication delay. Therefore, the communication delay has little impact on the estimation of the errors. The attitude estimation errors, velocity estimation errors and position estimation errors of Algorithm 1 converge quickly and then are stable near zero. Therefore, the polar cooperative navigation algorithm of multi-UUVs considering communication delays is effective and accurate for the sailing of multi-UUVs in the polar region.

Figure 11. Estimation errors based on Algorithm 1 and Algorithm 2 in the experiment: (a) attitude estimation errors; (b) velocity estimation errors; (c) position estimation errors.

A polar cooperative navigation algorithm for multi-UUVs considering communication delays was proposed in this paper. We mainly focused on the construction of the polar cooperative navigation algorithm and only the communication delays were considered in this paper. To simplify the analysis and for the purposes of this paper, some hypothetical ideal situations were included in this paper. For example, the underwater acoustic communication was assumed to be well-performing and reliable. In addition, only communication delays were considered in this paper. There are substantially more complex situations in practical application, including communication interruption, communication lost-packets, communication drop-outs and so on. These situations will influence the accuracy of the cooperative navigation. We believe these problems will be solved with further improvement in underwater acoustic communication and navigation algorithms, which will be our future job.

7. Conclusions

A polar cooperative navigation algorithm for multi-UUVs considering communication delays was proposed in this paper. Using UUV characteristics and the environment of the polar region, a polar grid navigation algorithm for a leader UUV and follower UUV was established. The acoustic communication delay was analyzed, and a discretization model for the acoustic communication delay was established. Due to the existence of acoustic communication delays, there are large errors when conventional AKF is used in the polar cooperative navigation of multi-UUVs which consider communication delay. Therefore, the conventional AKF method was modified to consider underwater acoustic communication delays. The simulation and experimental results show that the proposed algorithm can be effectively used for cooperative navigation of multi-UUVs in the polar region.