Fisher-Information-Matrix-Based USBL Cooperative Location in USV–AUV Networks

Abstract

With the development of underwater technology and the increasing demand for ocean development, more and more intelligent equipment is being applied to underwater scientific missions. Specifically, autonomous underwater vehicle (AUV) clusters are being used for their flexibility and the advantages of carrying communication and detection units, often performing underwater tasks in formation. In order to locate AUVs with high precision, we introduce an unmanned surface vehicle (USV) with global positioning system (GPS) and propose a USV–AUV network. Furthermore, we propose an ultra-short baseline (USBL) acoustic cooperative location scheme with an orthogonal array, which is based on underwater communication with sonar. Based on the derivation of the Fisher information matrix formula under Cartesian parameters, we analyze the positioning accuracy of AUVs in different positions under the USBL positioning mode to derive the optimal array of the AUV formation. In addition, we propose a USV path planning scheme based on Dubins path planning functions to assist in locating the AUV formation. The simulation results verify that the proposed scheme can ensure the positioning accuracy of the AUV formation and help underwater research missions.

1. Introduction

With the rapid development of scientific research in the marine field, tools for exploring the ocean are emerging in an endless stream. Underwater robotic technology has attracted wide attention due to its potential applications in various fields such as environmental monitoring, seabed exploration and biological investigation [1,2]. The success of underwater robotic missions depends to a large extent on the accurate and reliable positioning of these robotic systems [3]. Underwater robots need to complete various tasks, and precise control of the robot’s working area can be achieved through positioning to improve the efficiency of the operation [4]. In an underwater environment, any problem encountered by the robot may lead to damage or loss of control. Through positioning, the position and state of the robot can be determined in real time, thereby increasing the safety of the operation and avoiding damage or accidents [5].

Currently, methods for positioning underwater robots can be mainly divided into the following categories: global positioning systems (GPSs), inertial navigation system positioning, and ultrasonic positioning. GPS determines the global position of the robot through its satellite positioning system, but underwater GPS signal transmission is susceptible to the influence of water refraction and scattering, transmission attenuation, delay and other problems, thus affecting the accuracy and efficiency of underwater positioning [6,7]. The inertial navigation positioning of an underwater vehicle uses devices such as accelerometers, gyroscopes and odometers to measure the robot’s motion and calculate its position, but long-term operation is prone to the accumulation of errors [8]. In addition, underwater robots also face some unique challenges in positioning [9], such as underwater signal attenuation, multi-path effect, and environmental noise [10,11]. Poor underwater illumination and visibility easily affect the accuracy and efficacy of robot sensors [12,13].

Fortunately, the emergence of co-localization provides a new way to solve this problem. Bahr et al. proposed a distributed algorithm that uses a set of autonomous underwater vehicles (AUVs) to dynamically calculate the locally optimal position of the beacon vehicle using information obtained from the broadcast communication of the survey vehicle in [14]. Tang et al. proposed a factor graph weighted particle-assisted distributed underwater node cooperative location algorithm (WP-DUCP) and applied it to underwater networks to improve the resolution of underwater positioning in [15]. Vasilijevi et al. established the internet of underwater things (IoUT) based on the unmanned surface vehicle (USV), so as to improve the efficiency of underwater positioning in [16]. Lin et al. proposed a hierarchical positioning framework based on a USBL positioning system to locate each AUV in the network in [17] and verified that the proposed scheme performed better than the existing distributed strategies.

However, the above collaborative positioning methods all have shortcomings in complex underwater environments: on the one hand, with an increase in the number of robots and sensors, the underwater acoustic channel becomes complicated, the computational complexity increases, the underwater node load is limited, and other problems arise [18,19]; on the other hand, the system requires a large amount of energy to achieve positioning and communication, so there are high requirements for the battery life and operating costs of the equipment.

Therefore, this study proposes a new surface–underwater cooperative positioning system based on an ultra-short baseline (USBL) acoustic positioning system, which is composed of an algorithm and a simulation to locate multiple underwater AUVs considering the Dubins path planning functions for a USV. The contributions of this paper are as follows:

(1)

We design a USV–AUV network structure for locating AUVs during underwater missions, where we model sonar-based underwater communication.

(2)

A collaborative positioning scheme based on USBL is proposed. The Fisher information matrix is used to analyze the positioning accuracy, and then the optimal array for the AUV formation is derived.

(3)

We design a USV path planning scheme for underwater positioning based on the Dubins paths.

The rest of this article is organized as follows: in Section 2, we introduce the system model; in Section 3, we propose a location method and introduce the Fisher information matrix to design a USV path planning scheme; in Section 4, we present the simulation experiments and a corresponding analysis of the designed schemes; finally, we give the conclusion in Section 5.

Definitions 1.

In this paper, the autonomous underwater vehicle is denoted as AUV, unmanned surface vehicle is denoted as USV, the internet of underwater things is denoted as IoUT, ultra-short baseline is denoted as USBL, Cramer–Rao inequality lower bound is denoted as CRLB, and global positioning system is denoted as GPS.

2. System Model

2.1. USV–AUV Networks

We consider the application scenario shown in Figure 1. Under a certain research water area, AUVs carry out scientific research tasks at a certain depth and form a formation to move uniformly to meet the task requirements. Each AUV can move or hover at varying depths. In order to accurately obtain the position of AUVs performing underwater tasks, the surface sailing USV and the AUV formation form USV–AUV networks based on underwater acoustic communication. Specifically, the USV is wirelessly equipped with GPS and communicates with the satellite to obtain its own position information. The USV follows the AUV formation and allows each AUV to communicate with it. Then, the position of each AUV can be obtained by means of the orthogonal array sonar installed on the surface ship and the depth information returned by the AUV. Due to the harsh underwater communication environment, the AUV formation should remain within the reliable communication range $r_c$ of the USV, which is denoted as $\mathrm{\Lambda }=\left\{1,\mathrm{\dots },k,\mathrm{\dots },m\right\}$. Each AUV transmits its own motion and attitude information to the USV to assist in path planning. At the same time, the AUV formation should adopt the optimal formation according to its position relative to the USV.

2.2. Underwater Acoustic Communication Model for USBL

The USV uses the orthogonal array USBL acoustic positioning system to measure the AUVs’ position information. To describe sonar-based communication between the USV and AUVs, we introduce the active sonar equation in an underwater environment [20]:

$$EM=SL+TS+DI-NL-DT-2TL,$$

(1)

where the units of the above parts are in dB. $EM$ is the echo margin received by the USV, $SL$ denotes the emission sound level of active sonars, $TS$ represents the return signal strength of the AUV’s transponder, and $DI=10\log (n_{tr})$ is the directivity index of the sonar, where $n_{tr}$ is the number of transducer units. In order to analyze the transmission loss $TL$ of acoustic signals in an underwater communication environment based on the transmission distance $l$ and center frequency $f$, we introduce underwater acoustics using Throp’s model [21]:

$$TL=\varpi (f)\frac{l}{{10}^3}+\lg (l^2),$$

(2)

$$\varpi (f)=3\times {10}^{-3}+\left(2.75\times {10}^{-4}\right)f^2+\frac{44f^2}{f^2+4100}+\frac{0.11f^2}{f^2+1},$$

(3)

where $\varpi (f)$ is the absorption coefficient for frequency. Moreover, the underwater noise model proposed by Stojanovic is used to analyze the characteristics of high noise in an underwater communication environment [22]:

$$N_{tu}={10}^{1.7-3\log f},$$

(4)

$$N_{sh}={10}^{2\varsigma +3+2.6\log (f)-6\log \left(f+0.03\right)},$$

(5)

$$N_{wa}={10}^{5+0.75\sqrt{w}+2\log (f)-4\log \left(f+0.4\right)},$$

(6)

$$N_{th}={10}^{2\log (f)-1.5},$$

(7)

where $N_{tu}$, $N_{sh}$, $N_{wa}$ and $N_{th}$ represent the ambient noise components from turbulence, ships, waves and thermal noise, respectively. Additionally, $w$ is the wind speed measured in m/s, and $\varsigma$ denotes shipping activity. Thus, the noise level can be derived as

$$NL=10\lg \left(N_{th}+N_{tu}+N_{wa}+N_{sh}\right).$$

(8)

Given the communication threshold $DT$ of the sonar, we obtain the effective detection range of USBL:

$$r_c=\underset{l}{\arg \max }\left\{EM(l,f)\ge 0\right\}.$$

(9)

3. Methods

3.1. Orthogonal Array USBL Cooperative Location

The USBL algorithm designed in this work adopts an orthogonal array method, as shown in Figure 2, where Xa, Xb, Ya, Yb and O are 5 transducer units with identical characteristics [23]. The arrays are arranged on the USV at equal spacing and the spacing meets $OXa=OXb=OYb=d/2$, where $d$ is the array spacing. A rectangular coordinate system is demonstrated in Figure 2, and P is the transponder fixed on the underwater target AUV. Determining the underwater sound speed $c$ and the signal frequency emitted by the transducer can be used to carry out the location process. By measuring the time $t$ from the transmitting signal by the transmitting transducer to the receiving signal by the receiving transducer, as well as the phase difference $\Delta {\phi }_x$ between receiving units a and b on X-axis and the phase difference $\Delta {\phi }_y$ between receiving units a and b on Y-axis, we can calculate the position parameters of each AUV:

$$x=\frac{c}{2\pi fd}\Delta {\phi }_{x}S,$$

(10)

$$y=\frac{c}{2\pi fd}\Delta {\phi }_{y}S,$$

(11)

$$S^2=\sqrt{x^2+y^2+z^2},$$

(12)

where $S$ represents the length of OP, and $\left(x,y,z\right)$ represents the coordinate of the transponder P of the target AUV.

3.2. Fisher Information Matrix Based on USBL Location Model

The Cramer–Rao inequality lower bound (CRLB) describes the lower bound of a covariance matrix for an unbiased estimate of an unknown quantity, the reciprocal of which is the Fisher information. The Fisher information matrix is often used to measure the amount of state-variable information contained in the quantity measurement of a system [24]. The greater the Fisher information, the more information contained in the system model, the lower the uncertainty of the system, and the higher the accuracy of the state quantity. Therefore, through the analysis of Fisher information of each AUV at different positions, the positioning accuracy of different positions under the orthogonal array USBL positioning mode can be evaluated.

According to the measurement information, we can deduce Fisher information matrix formula based on Cartesian parameters. The coordinates of the USV are denoted as $\left(x,y,0\right)$, the number of AUVs to be located within the communication range is $m$, and the coordinates of AUV $k$ are $\left(x_k,y_k,z_k\right)$. According to Equations (10)–(12), we can come to the following conclusion:

$$\Delta {\phi }_{x,k}=\frac{2\pi fd(x_k-x)}{c{S}_k},$$

(13)

$$\Delta {\phi }_{y,k}=\frac{2\pi fd(y_k-y)}{c{S}_k},$$

(14)

$$S_k=\sqrt{{\left(x_k-x\right)}^2+{\left(y_k-y\right)}^2+z_k^2}.$$

(15)

Since the depth of the AUV to be positioned underwater can be obtained by means of a depth sensor, the equation used to measure the positioning accuracy of the point position does not contain the depth parameter when the Fisher information matrix is established. Therefore, the measurement equation is

$$Z_k=h_k(X)+u_k,$$

(16)

where $X={\left[x,y\right]}^T$ is the target state vector, $h_k={\left[\Delta {\phi }_{x,k},\Delta {\phi }_{y,k}\right]}^T$, and $u_k$ is the zero mean Gaussian white noise. Let $R={\sigma }^{2}I$ be the measurement noise covariance matrix, and assume that the noise covariance matrix of different target AUVs is the same. Then, the probability density function of the measured data is:

$$p\left(Z,X\right)={\displaystyle \prod _{k=1}^m\frac{\exp \left\{-\frac{1}{2}{\left[Z_k-h_k\left(X\right)\right]}^T{R}^{-1}\left[Z_k-h_k\left(X\right)\right]\right\}}{\sqrt{2\pi \det (R)}}},$$

(17)

whose logarithmic form is

$$\begin{array}{ll}\ln \left(p\left(Z,X\right)\right)& =-\frac{m\left(\ln \left(2\pi \right)+\ln \left(\det (R)\right)\right)}{2}\\ & -\frac{{\displaystyle \sum _{k=1}^m{\left[Z_k-h_k\left(X\right)\right]}^T{R}^{-1}\left[Z_k-h_k\left(X\right)\right]}}{2},\end{array}$$

(18)

whose first partial derivative is

$$\begin{array}{cc}\frac{\partial \ln \left(p\left(Z,X\right)\right)}{\partial x}\hfill & =\frac{1}{2}{\displaystyle \sum _{k=1}^m\left\{{\left[\frac{\partial h_k\left(X\right)}{\partial x}\right]}^T{R}^{-1}\left[Z_k-h_k\left(X\right)\right]\right\}}\\ & +\frac{1}{2}{\displaystyle \sum _{k=1}^m\left\{{\left[Z_k-h_k\left(X\right)\right]}^T{R}^{-1}\frac{\partial h_k\left(X\right)}{\partial x}\right\}},\hfill \end{array}$$

(19)

based on which we can derive the second partial derivative as

$$\begin{array}{l}\frac{{\partial }^2\ln \left(p\left(Z,X\right)\right)}{\partial x^2}=\frac{1}{2}{\displaystyle \sum _{k=1}^m\left\{{\left[\frac{{\partial }^2{h}_k\left(X\right)}{\partial x^2}\right]}^T{R}^{-1}\left[Z_k-h_k\left(X\right)\right]\right\}}\\ +\frac{1}{2}{\displaystyle \sum _{k=1}^m\left\{{\left[Z_k-h_k\left(X\right)\right]}^T{R}^{-1}\frac{{\partial }^2{h}_k\left(X\right)}{\partial x^2}-2{\left[\frac{\partial h_k\left(X\right)}{\partial x}\right]}^T{R}^{-1}\left[\frac{\partial h_k\left(X\right)}{\partial x}\right]\right\}},\end{array}$$

(20)

whose expectation can be denoted as

$$\mathrm{{\rm E}}\left[\frac{{\partial }^2\ln \left(p\left(Z,X\right)\right)}{\partial x^2}\right]=-{\displaystyle \sum _{k=1}^m\left\{{\left[\frac{\partial h_k\left(X\right)}{\partial x}\right]}^T{R}^{-1}\frac{\partial h_k\left(X\right)}{\partial x}\right\}}.$$

(21)

Similar to the above derivation, we can further conclude that:

$$\mathrm{{\rm E}}\left[\frac{{\partial }^2\ln \left(p\left(Z,X\right)\right)}{\partial x\partial y}\right]=-{\displaystyle \sum _{k=1}^m\left\{{\left[\frac{\partial h_k\left(X\right)}{\partial x}\right]}^T{R}^{-1}\frac{\partial h_k\left(X\right)}{\partial y}\right\}},$$

(22)

$$\mathrm{{\rm E}}\left[\frac{{\partial }^2\ln \left(p\left(Z,X\right)\right)}{\partial y\partial x}\right]=-{\displaystyle \sum _{k=1}^m\left\{{\left[\frac{\partial h_k\left(X\right)}{\partial y}\right]}^T{R}^{-1}\frac{\partial h_k\left(X\right)}{\partial x}\right\}},$$

(23)

$$\mathrm{{\rm E}}\left[\frac{{\partial }^2\ln \left(p\left(Z,X\right)\right)}{\partial y^2}\right]=-{\displaystyle \sum _{k=1}^m\left\{{\left[\frac{\partial h_k\left(X\right)}{\partial y}\right]}^T{R}^{-1}\frac{\partial h_k\left(X\right)}{\partial y}\right\}},$$

(24)

$$\frac{\partial h_k\left(X\right)}{\partial x}=-{\left[\frac{2\pi fd\left({\left(y_k-y\right)}^2+z_k^2\right)}{c{S}_k^3},\frac{2\pi fd\left(x_k-x\right)\left(y_k-y\right)}{c{S}_k^3}\right]}^T,$$

(25)

$$\frac{\partial h_k\left(X\right)}{\partial y}=-{\left[\frac{2\pi fd\left(x_k-x\right)\left(y_k-y\right)}{c{S}_k^3},\frac{2\pi fd\left({\left(x_k-x\right)}^2+z_k^2\right)}{c{S}_k^3}\right]}^T,$$

(26)

based on which we can obtain the Fisher information matrix when the surface auxiliary ship is set as the origin of coordinates:

$$\begin{array}{l}{J}_m=\left[\begin{array}{c}\begin{array}{cc}-\mathrm{{\rm E}}\left[\frac{{\partial }^2\ln \left(p(Z,X)\right)}{\partial x^2}\right]& -\mathrm{{\rm E}}\left[\frac{{\partial }^2\ln \left(p(Z,X)\right)}{\partial x\partial y}\right]\end{array}\\ \begin{array}{cc}-\mathrm{{\rm E}}\left[\frac{{\partial }^2\ln \left(p(Z,X)\right)}{\partial y\partial x}\right]& -\mathrm{{\rm E}}\left[\frac{{\partial }^2\ln \left(p(Z,X)\right)}{\partial y^2}\right]\end{array}\end{array}\right]\\ =\left[\begin{array}{c}\begin{array}{cc}{\displaystyle \sum _{k=1}^m}\frac{4{\pi }^2{f}^2{d}^2\left({\left(S_k^2-x_k^2\right)}^2+x_k^2{y}_k^2\right)}{{\sigma }^2{c}^2{S}_k^6}& {\displaystyle \sum _{k=1}^m}\frac{4{\pi }^2{f}^2{d}^2\left({\left(S_k^2+z_k^2\right)}^2{x}_k{y}_k\right)}{{\sigma }^2{c}^2{S}_k^6}\end{array}\\ \begin{array}{cc}{\displaystyle \sum _{k=1}^m}\frac{4{\pi }^2{f}^2{d}^2\left({\left(S_k^2+z_k^2\right)}^2{x}_k{y}_k\right)}{{\sigma }^2{c}^2{S}_k^6}& {\displaystyle \sum _{k=1}^m}\frac{4{\pi }^2{f}^2{d}^2\left({\left(S_k^2-y_k^2\right)}^2+x_k^2{y}_k^2\right)}{{\sigma }^2{c}^2{S}_k^6}\end{array}\end{array}\right],\end{array}$$

(27)

which can be further expressed as follows when we have $\frac{y_k}{x_k}=\tan {\phi }_k$ and $p_k^2=x_k^2+y_k^2$:

$$\begin{array}{l}{J}_m=\frac{4{\pi }^2{f}^2{d}^2}{{\sigma }^2{c}^2}\left[\begin{array}{l}\begin{array}{cc}{\displaystyle \sum _{k=1}^m}\frac{\left(p_k^4-2S_k^2{p}_k^2\right)\cos 2{\phi }_k}{S_k^6}+\frac{1}{S_k^2}& {\displaystyle \sum _{k=1}^m}\frac{\left(2S_k^2{p}_k^2-p_k^4\right)\cos {\phi }_k\sin {\phi }_k}{S_k^6}\end{array}\\ \begin{array}{cc}{\displaystyle \sum _{k=1}^m}\frac{\left(2S_k^2{p}_k^2-p_k^4\right)\cos {\phi }_k\sin {\phi }_k}{S_k^6}& {\displaystyle \sum _{k=1}^m}\frac{\left(p_k^4-2S_k^2{p}_k^2\right)\sin 2{\phi }_k}{S_k^6}+\frac{1}{S_k^2}\end{array}\end{array}\right]\\ =\frac{4{\pi }^2{f}^2{d}^2}{{\sigma }^2{c}^2}\left[\begin{array}{l}\begin{array}{cc}{\displaystyle \sum _{k=1}^m\frac{\left(p_k^4-2S_k^2{p}_k^2\right)\left(1+\cos 2{\phi }_k\right)}{2S_k^6}+\frac{1}{S_k^2}}& {\displaystyle \sum _{k=1}^m\frac{\left(2S_k^2{p}_k^2-p_k^4\right)\sin 2{\phi }_k}{2S_k^6}}\end{array}\\ \begin{array}{cc}{\displaystyle \sum _{k=1}^m\frac{\left(2S_k^2{p}_k^2-p_k^4\right)\sin 2{\phi }_k}{2S_k^6}}& {\displaystyle \sum _{k=1}^m\frac{\left(p_k^4-2S_k^2{p}_k^2\right)\left(1-\cos 2{\phi }_k\right)}{2S_k^6}+\frac{1}{S_k^2}}\end{array}\end{array}\right],\end{array}$$

(28)

whose determinant is

$$\begin{array}{ll}\det \left(J_m\right)& ={\left(\frac{4{\pi }^2{f}^2{d}^2}{{\sigma }^2{c}^2}\right)}^2\left[{\left({\displaystyle \sum _{k=1}^m\frac{p_k^4-2S_k^2{p}_k^2+2S_k^4}{2S_k^6}}\right)}^2-{\left({\displaystyle \sum _{k=1}^m\frac{\left(2S_k^2{p}_k^2-p_k^4\right)\sin 2{\phi }_k}{2S_k^6}}\right)}^2\right]\\ & -{\left(\frac{4{\pi }^2{f}^2{d}^2}{{\sigma }^2{c}^2}\right)}^2\left[{\left({\displaystyle \sum _{k=1}^m\frac{\left(p_k^4-2S_k^2{p}_k^2\right)\cos 2{\phi }_k}{2S_k^6}}\right)}^2\right].\end{array}$$

(29)

Let $A_k=\frac{p_k^4-2S_k^2{p}_k^2}{2S_k^6}$; then, we have

$$\begin{array}{l}{\left({\displaystyle \sum _{k=1}^m\frac{\left(2S_k^2{p}_k^2-p_k^4\right)\sin 2{\phi }_k}{2S_k^6}}\right)}^2+{\left({\displaystyle \sum _{k=1}^m\frac{\left(p_k^4-2S_k^2{p}_k^2\right)\cos 2{\phi }_k}{2S_k^6}}\right)}^2\\ ={\left({\displaystyle \sum _{k=1}^m{A}_k\sin 2{\phi }_k}\right)}^2+{\left({\displaystyle \sum _{k=1}^m{A}_k\cos 2{\phi }_k}\right)}^2\\ ={\displaystyle \sum _{k=1}^m{A}_k^2}+{\displaystyle \sum _{1\le i<j\le m}^{m}2A_i{A}_j\cos 2{\phi }_i\cos 2{\phi }_j+{\displaystyle \sum _{1\le i<j\le m}^{m}2A_i{A}_j\sin 2{\phi }_i\sin 2{\phi }_j}}\\ ={\displaystyle \sum _{k=1}^m{A}_k^2}+{\displaystyle \sum _{1\le i<j\le m}^{m}2A_i{A}_j\cos 2\left({\phi }_i-{\phi }_j\right)}\\ ={\displaystyle \sum _{k=1}^m{A}_k^2}+{\displaystyle \sum _{1\le i<j\le m}^{m}2A_i{A}_j\left(1-2{\sin }^2\left({\phi }_i-{\phi }_j\right)\right)}\\ ={\displaystyle \sum _{k=1}^m{A}_k^2}+{\displaystyle \sum _{1\le i<j\le m}^{m}2A_i{A}_j}-{\displaystyle \sum _{1\le i<j\le m}^{m}4A_i{A}_j{\sin }^2\left({\phi }_i-{\phi }_j\right)}\\ ={\left({\displaystyle \sum _{k=1}^m{A}_k}\right)}^2-{\displaystyle \sum _{1\le i<j\le m}^{m}4A_i{A}_j{\sin }^2\left({\phi }_i-{\phi }_j\right)}.\end{array}$$

(30)

According to Equations (29) and (30), we have

$$\begin{array}{ll}\det \left(J_m\right)& ={\left(\frac{4{\pi }^2{f}^2{d}^2}{{\sigma }^2{c}^2}\right)}^2\left[{\left({\displaystyle \sum _{k=1}^m{A}_k+\frac{1}{S_k^2}}\right)}^2-{\left({\displaystyle \sum _{k=1}^m{A}_k}\right)}^2+4{\displaystyle \sum _{1\le i<j\le m}^m{A}_i{A}_j{\sin }^2\left({\phi }_i-{\phi }_j\right)}\right]\\ & ={\left(\frac{4{\pi }^2{f}^2{d}^2}{{\sigma }^2{c}^2}\right)}^2\left[\left({\displaystyle \sum _{k=1}^m\frac{1}{S_k^2}}\right)\left({\displaystyle \sum _{k=1}^{m}2A_k+\frac{1}{S_k^2}}\right)+4{\displaystyle \sum _{1\le i<j\le m}^m{A}_i{A}_j{\sin }^2\left({\alpha }_{ij}\right)}\right],\end{array}$$

(31)

where ${\alpha }_{ij}={\phi }_i-{\phi }_j$ represents the angle between the projection of the two AUVs and the surface auxiliary vessel on the horizontal plane. Let $\frac{z_k}{S_k}=\sin {\gamma }_k$. Then, Equation (31) can be simplified to

$$\begin{array}{ll}\det \left(J_m\right)& ={\left(\frac{4{\pi }^2{f}^2{d}^2}{{\sigma }^2{c}^2}\right)}^2\left[\left({\displaystyle \sum _{k=1}^m\frac{1}{S_k^2}}\right)\left({\displaystyle \sum _{k=1}^m\frac{{\sin }^4{\gamma }_k}{S_k^2}}\right)+4{\displaystyle \sum _{1\le i<j\le m}^m{A}_i{A}_j{\sin }^2\left({\alpha }_{ij}\right)}\right]\\ & ={\left(\frac{4{\pi }^2{f}^2{d}^2}{{\sigma }^2{c}^2}\right)}^2\left[\left({\displaystyle \sum _{k=1}^m\frac{{\sin }^4{\gamma }_k}{S_k^2}}\right)+{\displaystyle \sum _{1\le i<j\le m}^m\frac{\left({\sin }^4{\gamma }_i{\sin }^4{\gamma }_j+1\right){\sin }^2{\alpha }_{ij}}{S_i^2{S}_j^2}}\right]\\ & +{\left(\frac{4{\pi }^2{f}^2{d}^2}{{\sigma }^2{c}^2}\right)}^2\left[\frac{\left({\sin }^4{\gamma }_i+{\sin }^4{\gamma }_j\right){\cos }^2{\alpha }_{ij}}{S_i^2{S}_j^2}\right].\end{array}$$

(32)

3.3. Aided Location Analysis Based on Fisher Information Matrix

In this section, we discuss the optimal formation of different numbers of AUVs in the communication range. According to Equation (32), $\det (J_1)={\left(\frac{4{\pi }^2{f}^2{d}^2}{{\sigma }^2{c}^2}\right)}^2\frac{{\sin }^4{\gamma }_k}{S_1^4}$ when $m=1$. In order to make $\det (J_1)$ as large as possible, the best position for the AUV is directly below the USV. When $m=2,3,\mathrm{\dots }$, the AUV formation can be arranged in a variety of ways. This work only considers the influence of different angles between the AUVs of underwater targets on positioning accuracy. As $k\in 2,3,\mathrm{\dots },m$, $S_k=S_0$, and ${\gamma }_k={\gamma }_0$, Equation (32) can be simplified to:

$$\det \left(J_m\right)={\left(\frac{4{\pi }^2{f}^2{d}^2}{{\sigma }^2{c}^2}\right)}^2\left[3m\frac{{\sin }^4{\gamma }_0}{S_0^4}+\frac{{\left({\sin }^4{\gamma }_0+1\right)}^2}{S_0^4}{\displaystyle \sum _{1\le i<j\le m}^m{\sin }^2\left({\alpha }_{ij}\right)}\right].$$

(33)

When the distance between the AUVs and the USV is equal and the AUVs are at the same depth, only the angle of the AUVs is considered, and the influence of AUV formation on positioning accuracy is only related to ${\sin }^2{\alpha }_{ij}$. After calculation, the projections of the optimal array formations on the horizontal plane when $m=2,3,4$ are obtained, as shown in Figure 3. Specifically, $m=3$ has two optimal array formations.

3.4. USV Path Planning Based on Dubins Path

After the AUV transmits its motion and attitude information to the USV, the USV ensures the positioning accuracy of the AUV by planning the path based on the Fisher information matrix. In order to save the energy of the USV, the total path length of the USV should be reduced as much as possible in path planning. In terms of positioning accuracy, the horizontal distance $r_m=\arg \max \left\{\det \left(J_m\right)\right\}$ can be obtained according to Equation (32). In order to reduce noise interference, specific scientific missions require the USV and AUV $k$ to maintain a specific minimum horizontal spacing ${\widehat{r}}_k$. Based on the minimum turning radius $r_s$ (Dubins radius) of the USV, we can map the position of the AUV $k$ as a circle at the surface. The radius $r_k=\max \left\{{\widehat{r}}_k,r_m,r_s\right\}$ of the circle is the optimal horizontal distance between the USV and AUV $k$. For USV path planning with curvature constraints, the Dubins path should be used if the start and end locations are specified [25]. Dubins paths indicate that under the maximum curvature restriction, the shortest feasible path between two oriented points in the plane is the CLC path or the CCC path or a subset of them, where C stands for arc segment and L for line segment. This work considers CLC-type paths. The path planning algorithm is divided into two parts: path generation and path finding. The former is used to directly generate the Dubins path and check whether the radius requirements are met. If the radius requirements are not met, the intermediate path is generated according to the tangent line of the circle, and the intermediate path is checked until the radius limit is met.

For example, Figure 4 shows the path of the USV on the surface plane. $O$ is the current position of the USV, $T_k$ is the projection of the AUV $k$ on the surface, $T_d$ is the target position of the USV, and $\stackrel{⏜}{OA}-AB-\stackrel{⏜}{B{T}_d}$ is the path of the USV. The center angles of $\stackrel{⏜}{OA}$ and $\stackrel{⏜}{B{T}_d}$ are ${\phi }_1$ and ${\phi }_2$, respectively. If the heading angles of the USV and AUV are known to be $\delta$ and $\beta$, respectively, the USV velocity direction is taken as the positive direction of the Y-axis. The starting point $O$ is the origin, and the coordinate of $T_k$ is $\left(x_k,y_k\right)$. Then, the coordinates of each key point can be obtained:

$$P_A=[r_1-r_1\cos {\phi }_1,r_1\sin {\phi }_1],$$

(34)

$$P_{T_d}=[x_3-r_3\cos (\beta -\delta ),y_3-r_3\sin (\beta -\delta )],$$

(35)

$$\begin{array}{l}{x}_B=x_3-(r_3+r_2)\cos (\delta -\beta )+r_2\cos ({\phi }_2+\delta -\beta ),\\ y_B=y_3-(r_3+r_2)\sin (\delta -\beta )-r_2\sin ({\phi }_2+\delta -\beta ).\end{array}$$

(36)

If the $AB$ line is the common tangent of the two arcs, then the slope of $AB$ meets:

$$\frac{y_3-(r_3+r_2)\sin (\delta -\beta )-r_2\sin ({\phi }_2+\delta -\beta )-r_1\sin ({\phi }_1)}{x_3-(r_3+r_2)\cos (\delta -\beta )+r_2\cos ({\phi }_2+\delta -\beta )-r_1+r_1\cos ({\phi }_1)}=\frac{1}{\tan ({\phi }_1)}.$$

(37)

Figure 4. Schematic diagram of USV path planning based on Dubins paths.

Figure 4. Schematic diagram of USV path planning based on Dubins paths.

As we have ${\phi }_2-\left(\beta -\delta \right)={\phi }_1$, we can denote $C=y_3-(r_3+r_2)\sin \left(\beta -\delta \right)$, $D=x_3-(r_3+r_2)\cos \left(\beta -\delta \right)-r_1$, and $\tan \xi =D/C$. Thus, $\sin \left({\phi }_1-\xi \right)=\frac{r_1+r_2}{\sqrt{C^2+D^2}}$, and we can obtain ${\phi }_1$. Moreover, the entire path length of the moving USV is ${\phi }_1{r}_1+{\phi }_2{r}_2+\frac{C+\sin \left({\phi }_1\right)(r_1+r_2)}{\cos \left({\phi }_1\right)}$. Then, we can choose the appropriate path according to the length of the path as the location path for the USV.

4. Simulation Settings and Results

In this study, we considered the AUV formation for simulation verification in a $3000 \mathrm{m}\times 3000 \mathrm{m}\times 1000 \mathrm{m}$ underwater environment. The maximum number of AUVs in a formation is $4$. The underwater sound velocity is considered to be $1500 \mathrm{m}/\mathrm{s}$. The array space is $4 \mathrm{cm}$, and the Dubins radius of the USV is $3 \mathrm{m}$. The source level of the sonar is $TL=500 \mathrm{dB}$, the return signal strength is $TS=3 \mathrm{dB}$, the communication threshold is $DT=10 \mathrm{dB}$, the center frequency of the sonar is $f=10 \mathrm{kHz}$, and the number of transducer units is $n_{tr}=5$. As for the underwater environment, the wind speed is $w=0.2 \mathrm{m}/\mathrm{s}$ and the shipping activity is $\varsigma =0.5$. The above parameter settings are referenced and based on [20,21,22,26,27,28,29,30]. We then simulated and analyzed the proposed scheme based on the MATLAB platform.

4.1. Path Planning for the Moving USV

In the process of optimizing positioning accuracy with the Fisher information matrix, the USV needs to adjust its position according to the position of the AUVs. Let $p^2=x^2+y^2$, and we can analyze the magnitude of the determinant $\det (J_m)$ of the Fisher matrix in the vertical plane of the USV and AUVs with respect to horizontal distance and depth. In Figure 5, our simulation verifies the relationship between $\lg \left(\det (J_m)\right)$ and $z$, $p$ in different AUV formations. The USV corresponds to the origin in the upper left corner of the figure, and the positioning accuracy of the AUV at different positions in the figure is presented in the form of contour lines. With a given AUV depth, the relative position range of the USV and AUV can be obtained when the positioning accuracy is high. The range obtained can be used for subsequent Dubins path planning. In practice, each AUV needs to make a circular path in this area to perform certain scientific tasks. Then, the USV routing algorithm abstracts AUV $k$ into circular obstacles according to $r_k$, and it performs intermediate pose generation and path connection on the original Dubins path.

Specifically, the initial pose information $\left(x=20 \mathrm{m}, y=0 \mathrm{m}, \alpha =60°\right)$ and the final target pose information $\left(x=45 \mathrm{m}, y=20 \mathrm{m}, \alpha =60°\right)$ of the USV are given. Assuming that there are three unlocated AUVs, the corresponding coordinate information and limit distance are $\left(x_1=25 \mathrm{m}, y_1=10 \mathrm{m}, {\widehat{r}}_1=3 \mathrm{m}\right)$, $\left(x_2=25 \mathrm{m}, y_2=23 \mathrm{m}, {\widehat{r}}_2=4 \mathrm{m}\right)$, and $\left(x_3=36 \mathrm{m}, y_3=40 \mathrm{m}, {\widehat{r}}_3=3 \mathrm{m}\right)$. The USV path planning based on the proposed scheme can be seen in Figure 6. When there is more than one Dubins circle between the starting point and the destination point, there is more than one Dubins path. In this case, many different paths can be calculated, and when comparing the lengths of the all paths, the shorter path can be regarded as the better path, that is, to find the best path among all the combinations of paths that meet the conditions. As can be seen, we can make the USV approach the optimal distance of each AUV for the subsequent underwater positioning. By choosing the right path, we can reduce the total path length of the USV.

4.2. Analysis of Distance to Positioning Accuracy

The point error is introduced to analyze the positioning accuracy, that is, the sum of the variance of the X direction and the Y direction of the AUV’s positioning ${\epsilon }_p^2={\epsilon }_x^2+{\epsilon }_y^2$. In practice, the variance of sequence x and y coordinates is obtained via repeated independent sampling of the AUV’s positions under exactly the same conditions. Monte Carlo sampling is taken, where the number of repetitions is uniformly $500$. All position-related data are obtained using the USBL algorithm that simulates the USV and AUVs.

In Figure 7, we show the changes in point error and Fisher information with the horizontal distance between the AUV and USV, when the formations with different numbers of AUVs adopt the optimal array. It can be seen that with the increase in horizontal distance, the point error increases and the Fisher information decreases, indicating that under the optimal array, the closer the distance between the AUV and the USV, the higher the positioning accuracy.

4.3. Analysis of Formation to Positioning Accuracy

When the number of AUVs in the formation is $3$, ${\alpha }_{12}={\alpha }_{23}=\theta$ is used as the parameter to describe the array formation. When $m=3$, the positioning accuracy based on Fisher information varies with horizontal distance $p$ and the angle $\theta$, as shown in Figure 8. The experimental depth of AUVs was set at $800 \mathrm{m}$. It can be seen that the greater the ratio of horizontal distance to depth, the greater the impact of formation on positioning accuracy. When $m=3$, the best formation appears at $60°$ and $120°$, which is consistent with the results of theoretical derivation.

When $m=4$, set ${\alpha }_{12}={\alpha }_{34}=90°$ and use ${\alpha }_{13}=\theta$ to describe the parameters of array formation. When $m=4$, the positioning accuracy based on Fisher information changes with horizontal distance $p$ and angle $\theta$, as shown in Figure 9. The experimental depth was set at $800 \mathrm{m}$, and the conclusion was the same as $m=3$. The larger the ratio of horizontal distance to depth, the greater the influence of array formation on positioning accuracy. Since $\theta =0°$ does not meet the definition, the optimal formation occurs at $90°$, which is consistent with the theoretical derivation. In addition, the benefit of changing the formation of $m=4$ is smaller than that of $m=3$, which is speculated to be related to the increase in the total number of AUVs and the increase in the information available to the USV to help eliminate positioning errors.

5. Conclusions

When multiple AUVs perform similar underwater tasks, such as conducting scientific research in a certain area, the AUVs often need to form a formation to maintain a similar trajectory and move uniformly; they also need to confirm the position of each AUV in real time. At this time, if a USV equipped with GPS is introduced to follow the AUV formation, and each AUV can communicate with it, the high-precision positioning of each AUV can be realized. Based on the above background, we constructed a new surface–underwater cooperative positioning system based on underwater acoustic communication, which is called the USV–AUV network. Specifically, we proposed an orthogonal array USBL cooperative location scheme based on sonar carried by the USV. Using the derivation of the Fisher information matrix formula under Cartesian parameters, we analyzed the positioning accuracy of AUVs in different positions under the USBL positioning mode and gave the optimal array for the AUV formation. Additionally, we introduced Dubins path planning functions for the design of the USV path planning algorithm. Finally, a large number of simulation experiments verified the performance of the proposed scheme in AUV positioning, which can effectively help underwater research missions based on the AUV formation.