Improving Matching Accuracy of Underwater Gravity Matching Navigation Based on Iterative Optimal Annulus Point Method with a Novel Grid Topology

Abstract

In this study, we improve the matching accuracy of underwater gravity matching navigation. Firstly, the Iterative Optimal Annulus Point (IOAP) method with a novel grid topology is proposed for breaking through the inherent grid structure limit of the canonical gravity matching algorithm and enhancing its underwater gravity matching accuracy. The theory of IOAP is as follows: (1) small-annulus matching and positioning mechanism on the tracking starting point is developed by employing the starting point and drift error of the INS (Inertial Navigation System), the fixed rotation angle, etc. The optimal matching location of the starting point is obtained by matching and comparing the matched points in this small-annulus grid, which contributes to heightening the initial-position error insensitivity of the algorithms. (2) Variable-angle three-layer annulus matching and positioning mechanisms on the tracking ending point were constructed by using the optimal matching location of the starting point and combining the tracking direction-and-distance information of the INS and the cumulative drift error, etc. It is used to generate the annulus matching points with the ring-type grid topology. (3) The optimal matching position of the ending point in this annulus is obtained by iteratively calculating the evaluation index value of the matching points and following the evaluation index optimal rule. Secondly, we comprehensively consider the main performance evaluation indexes of the underwater gravity matching algorithms, such as the statistical indicators of the matching accuracy, the average matching time and the matching success rate, and take them as a basis of the pros and cons of the matching analysis. Furthermore, under conditions that include different scale searching regions or different reference-angle ring radii, the statistical results verify that the IOAP had a different matching ability and better robustness. Finally, several trajectories with the starting points from different areas and the ending points in different gravity ranges are tested and compared to carry out the numerical simulations. These results indicate that the IOAP has many advantages, such as a high matching accuracy and strong positioning applicability in different gravity regions. Compared with the TERCOM (terrain contour matching algorithm), its average matching accuracy was the highest, increased by 40.39%.

1. Introduction

The Inertial Navigation System [1] (INS) is the core navigation system for realizing the autonomous navigation of underwater vehicles. It has the characteristics of short-time high-precision positioning [2]. However, due to the inherent errors of inertial components (such as gyros and accelerometers) and multiple integration of positioning calculations, the INS error is accumulated and diverged over time, which makes it difficult to meet the long-time high-precision positioning target of the underwater vehicle [3]. Therefore, the INS system needs to be calibrated regularly to maintain its navigation accuracy [3]. Moreover, the gravity field information, as one of Earth’s natural geographic attributes, is not easily influenced by uncertain environments, such as climate and sea waves [4]. It usually shows a relative stability for a long time, so the gravity field information is suitable for aiding the INS navigation of underwater vehicles. At present, the gravity aided navigation system is an important technology of underwater INS navigation [5,6] and has become an international hot issue studied by scholars at home and abroad.

An important topic of the gravity-aided INS system is the matching algorithm. Currently, the canonical gravity matching algorithms [7] mainly include the Sandia inertial terrain-aided navigation algorithm (SITAN), the iterative nearest contour point algorithm (ICCP) and the terrain contour matching algorithm (TERCOM [8]). By contrast, the TERCOM has the advantages of simple calculation, insensitivity to the initial error, strong robustness and high positioning accuracy [9], which have widely concerned and been studied by scholars for improving its matching accuracy, efficiency and reliability. On the one hand, in terms of improving the matching accuracy of the TERCOM, Yan et al. [10] presented a new matching algorithm by integrating the TERCOM and ICCP, in which the TERCOM was used to get the initial position, and the ICCP was used for accurate positioning. Tong et al. [11] presented a combined matching algorithm based on local gravity map approximation, which employed the two-dimensional Gaussian basis function for approximating the local gravity reference map and the quasi-Newtonian BFGS nonlinear optimization method for resolving the correlation extremum matching model. In addition, they integrated rough matching based on the TERCOM and measured the data preprocessing based on the difference methods to improve the matching accuracy of the algorithm. Zhao et al. [12] combined the TERCOM with a particle filter and put forward a new terrain-aided navigation algorithm for enhancing the positioning accuracy of the BITAN II algorithm. Zhang et al. [13] came up with an online false matching judgement criterion based on the joint probability of multiple reference points in a correlation plane to settle the mismatching problem that the TERCOM is susceptible to elevation measurement error and terrain similarity. Zhang et al. [14] applied the simulated method of the TERCOM to analyze the influence of major factors on the matching accuracy, such as submarine speed, depth sounding accuracy, initial position deviation, underwater terrain features and digital map resolution. On the other hand, in terms of improving the matching efficiency and reliability of the TERCOM, Han et al. [15] constructed an improved TERCOM by combining the shortest path algorithm and a new correlation analysis algorithm. Soon afterwards, they proposed a new matching algorithm with the mismatching diagnosis method by integrating [9] the spatial sequence constraint and the decision criteria limitation for improving the reliability and matching accuracy of the TERCOM. Li et al. [6] proposed a new hierarchical neighborhood threshold searching method based on the rough–thin matching strategies to improve the matching efficiency of the point-by-point traversal search in the TERCOM. In the same year, they [16] put forward a new geodesic-based method by coupling the shortest arc principle of spherical geometry with the attitude control theory in space and maritime environments to reduce the size of the matching area and improve the matching efficiency of the algorithms. Zhang et al. [17] came up with an underwater terrain matching algorithm based on a line–surface combination to improve the robustness and positioning accuracy of algorithms, in which the TERCOM was selected as the line-matching algorithm, and geometrical similarity was used in the surface matching method.

To sum up, most scholars’ studies have mainly focused on improving the underwater vehicle navigation performance of the TERCOM, but there is little research work on changing the matching grid topology of the TERCOM. Furthermore, the canonical TERCOM does not depend on the starting point location of the underwater vehicle in its trajectory and only takes three times the INS error at the ending point location in the trajectory as the half-side length to span a square-shaped grid matching point matrix. Then, it iterates and searches these pre-matched points to determine the best-matching position of the underwater vehicle. However, this searching mechanism requires a large amount of computation costs and is easy to lead to a low matching efficiency of the algorithms [18]. In addition, the TERCOM also has other shortcomings, including the difficulty in effectively dealing with the observation noise and the process noise [12] and the sensibility of the angle error in the INS range [19]. Therefore, it is necessary to further discuss and research the design problem of grid topology different from that of the TERCOM.

Different from the previous studies, this paper was inspired by the matching grid topology of the TERCOM and the drift error characteristics of the INS, and our main research motivation is the reconstruction of matching grid topology for innovatively proposing the iterative optimal annulus point method (IOAP) with a novel grid topology that aims at improving the matching accuracy of gravity-aided navigation on underwater vehicles. The calculation principle of the proposed IOAP is as follows. The small-annulus matching location mechanism of the trajectory starting point with a ring-type topology was constructed to reduce the sensitivity of the IOAP to the initial location error, in which the INS starting point location is put as the center and the preset drift error and rotation angle are used to span the small-annulus matching grid points. Then, the optimal matching position of the starting point is obtained by following the minimization principle of the gravity absolute deviation value. According to this optimal matching position of the starting point, the central position of the large-annulus matching grid is obtained by recombining the sailing direction-and-distance information of the INS. Simultaneously, the INS cumulative drift error is based on obtaining the number of matched grid rings in the large annulus. Next, the referenced rotated angle of the middle-ring radius and the inner-double and outer-half-based angle principle are employed to establish the ring-shaped distributed topology on these matched grid points and further construct the variable-angle three-layer annulus Matching and Positioning mechanism on the tracking Ending Point (EPMP). In the EPMP, every grid point is used to extract the corresponding gravity sequence from the gravity reference map and then compared with the real measured gravity sequence to calculate the mean square difference (MSD) between them. Afterwards, these MSDs are compared to gain the optimal matched position of the tracking ending point by following the MSD minimization principle.

2. Construction of Iterative Optimal Annulus Point (IOAP) Method with a Novel Grid Topology

The gravity matching navigation algorithm is one of the core components of the underwater vehicle navigation system. Its performance directly determines the positioning effects of the underwater vehicle location calibration, i.e., the positioning accuracy and the matching reliability. Benefitting from the high-precision properties of INS short-time output data [2] and inspired by the matching grid topology of the TERCOM with the INS drift error, the iterative optimal annulus point method (IOAP) with a novel grid topology is proposed to realize the effective search for a high-probability high-precision ring area and improve the positioning accuracy of the TERCOM. Briefly speaking, the calculation principle of the IOAP mainly includes two core modules, namely the Matching Positioning strategy of the tracking Starting Point in small annulus (SPMP) and the Matching Positioning mechanism of the tracking Ending Point in three-layer annulus (EPMP). Their corresponding operations will be described in detail as follows.

2.1. Matching Positioning Strategy of the Tracking Starting Point in Small Annulus (SPMP)

Considering the relative sensitivity [20] of gravity matching algorithms such as the SITAN and ICCP to the initial error of the tracking starting point, the Matching Positioning strategy of the tracking Starting Point in small annulus (SPMP) was established to improve, to a certain extent, the insensitivity (robustness) of the IOAP to the gravity matching initial error. In brief, it is centered around the underwater vehicle position indicated by the INS and uses a certain drift error and rotated angle to span a matched grid area with small annulus topology (termed as small annulus) for covering the real position of the tracking starting point with a probability one. Then, the optimal principle of the gravity-matched indexes on these matched points is evaluated to get the optimal matched location of the tracking starting point.

For the convenience of the mathematical formulation and follow-up description of the algorithms, denote the sampling time margin of a certain trajectory of the underwater vehicle as $\mathsf{\Delta }t$ and the tracking output location sequence of the INS as $S_1,S_2,\cdots ,S_L$. Meanwhile, their positioning coordinates and corresponding gravity values in this sequence are marked as $\left\{\left(x_1,y_1\right),\left(x_2,y_2\right),\cdots ,\left(x_L,y_L\right)\right\}$ and $g_1{g}_2\cdots g_L$, where $L$ represents the sampling sequence length of the INS-indicated trajectory. Thus, the SPMP strategy takes the location coordinate $\left(x_1,y_1\right)$ of the INS starting point $S_1$ as the center of the small annulus and sets three times the INS drift error standard deviation ${\sigma }_0$ with one sampling time margin $\mathsf{\Delta }t$ as the maximum searching boundary radius of the small annulus. Furthermore, if the northeast-rotated angle ${\theta }_0\in \left(0,360\right]$ and the ring radius equally spaced coefficient $\lambda \in \left(0,1\right]$ are set, the total number of matched grid points in small annulus and their positioning coordinates can be calculated and obtained under the SPMP strategy. Figure 1 shows the matched grid points distribution in the small annulus based on SPMP at ${\theta }_0=45$ and $\lambda =1/3$.

According to Figure 1 and the above parameter settings, the calculation formulation of the coordinate position $\left(x_i^j,y_i^j\right)$ of the matched grid points in the small annulus of the SPMP can be expressed as follows:

$$\left\{\begin{array}{l}{x}_i^j=x_1+r_i\sin \left({\beta }_j\right)\\ y_i^j=y_1+r_i\cos \left({\beta }_j\right)\end{array}\right.$$

(1)

where $x_i^j$ and $y_i^j$ denote the horizontal and vertical coordinates of the $j$th grid point on the $i$th small ring, respectively. $r_i=3{\sigma }_0\lambda i$ is the radius of the $i$th ring in the small annulus, where $i=1,2,\cdots ,i_{\max }=⌊\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$\lambda $}\right.⌋$ and $⌊\cdot ⌋$ show the floor, ${\sigma }_0={\left.\raisebox{1ex}{$E\times L\times \mathsf{\Delta }t$}\!\left/ \!\raisebox{-1ex}{$3600$}\right.\right|}_{L=1}$ indicates the standard deviation of the INS drift error between two adjacent tracking sampling points (in this paper, the accelerometer is set as 0.01 °/ h, while the gyro is 10⁻³ m/s², so the standard deviation of the INS drift error $E$ is about 1.8 km/h). ${\beta }_j=j\cdot {\theta }_0$ represents the rotated angle of the $j$th grid point on each ring in the small annulus, where $j=1,2,\cdots ,j_{\max }=⌊\raisebox{1ex}{$360$}\!\left/ \!\raisebox{-1ex}{${\theta }_0$}\right.⌋$.

On the basis of the gravity reference map and its grid resolution $C$, the set of matched grid points are obtained for the tracking starting point real position, including the INS tracking starting point position $\left(x_1,y_1\right)$ and the SPMP-based grid points position $\left(x_i^j,y_i^j\right)$. Then, they are mapped into the gravity reference map point by point, and the gravity value at the nearest grid point from the gravity reference map is taken as the gravity value of the matched point. Therefore, its corresponding expression is as follows:

$$\left\{\begin{array}{l}{g}_0^0=\mathrm{mapt}\left(\left[\frac{x_1}{C}\right],\left[\frac{y_1}{C}\right]\right)\\ g_i^j=\mathrm{mapt}\left(\left[\frac{x_i^j}{C}\right],\left[\frac{y_i^j}{C}\right]\right)\end{array}\right.$$

(2)

where $\mathrm{mapt}\left(\cdot ,\cdot \right)$ shows the gravity value matrix at the grid point position of the gravity reference map, and $\left[\cdot \right]$ implies the rounding technique.

In order to determine the optimal matching position $\left(b{x}_1,b{y}_1\right)$ of the tracking starting point of the underwater vehicle, the minimizing principle of the gravity absolute deviation value is introduced, and it can be expressed as

$$\left\{\begin{array}{l}b{x}_1=\underset{x_i^j}{\arg \min }\left|g_i^j-g_1\right|\\ b{y}_1=\underset{y_i^j}{\arg \min }\left|g_i^j-g_1\right|\end{array}\right.$$

(3)

where $i=0,1,2,\cdots ,i_{\max }$ and $j=0,1,2,\cdots ,j_{\max }$. It should be noted that $x_0^0$ is $x_1$, while $y_0^0$ is $y_1$, i.e., the corresponding starting position on the INS.

Remarks: The multimodal phenomenon may occur when multiple grid points in some small scale are projected onto the gravity reference map. Thus, in this paper, it is the first grid point with the minimization of gravity absolute deviation values. In addition, the random selection method can be used as a substitute to get the optimal matched position of the tracking starting point.

The SPMP strategy can realize the effective matched positioning of the tracking starting point of the underwater vehicle with a probability and weaken the sensitivity of IOAP to the initial position error. At the same time, it can also provide the position information of the tracking starting point for further ascertaining the tracking ending point position guided by the INS sailing direction and distance.

2.2. Matching Positioning Mechanism of the Tracking Ending Point in Three-Layer Annulus (EPMP)

Considering that the INS tracking sequence contains good short-term and high-precision sailing direction and distance information, combining the optimal matched position $\left(b{x}_1,b{y}_1\right)$ of the tracking starting point by the SPMP, the central position $O$ of the matched domains on the tracking ending point can be further estimated and gotten. Then, the novel matched grid point topology with annulus distribution and its matched positioning mechanism, termed as the Matching Positioning mechanism of the tracking Ending Point in a three-layer annulus (EPMP), is constructed based on the statistical characteristics of the INS drift error. If the standard deviation $\sigma$ of the INS cumulative drift error is $E\times L\times \mathsf{\Delta }t/3600$ and the grid resolution of the gravity reference map and the radius of the middle ring are marked as $C$ and $R$, respectively, the EPMP can follow the relative relationship among the three constants to obtain the maximum number of rings and the rotated angle between the matched grids in its ring coverage area and further span a large annulus grid with a changed-angle three-layer topological structure (termed as a large annulus) for covering the real position of the tracking ending point with a probability. Then, the coordinate position of every grid point in the EPMP would be mapped into the gravity reference map to extract a gravity sequence of the matched navigational trajectory, which is further compared with the surveyed gravity sequence of underwater vehicles to gain a certain evaluation index (like MSD, i.e., Mean Square Difference) and determine the optimal matched position of the tracking ending point by following the optimal principle of this index (like MSD).

For generating the location coordinates of all matched grid points in the large annulus of the EPMP, it is necessary to get the navigation direction ${\alpha }_{INS}$ and distance ${\alpha }_{INS}$ of underwater vehicles based on the tracking starting point $S_1$ and ending point $S_L$ output by the INS system. Then, combining the starting point optimal matched position $\left(b{x}_1,b{y}_1\right)$ obtained by the SPMP, the central position $\left(x_O,y_O\right)$ of the large annulus can be derived and calculated as the following formulation:

$$\left\{\begin{array}{l}{x}_O=b{x}_1+d_{INS}\cos \left({\alpha }_{INS}\right)\\ y_O=b{y}_1+d_{INS}\sin \left({\alpha }_{INS}\right)\end{array}\right.$$

(4)

where $d_{INS}={‖S_L-S_1‖}_p^p=\sqrt[p]{{\left(y_L-y_1\right)}^p+{\left(x_L-x_1\right)}^p}$ is the p-norm distance metric between the starting and ending points in the INS trajectory (in this paper, $p$ is set to 2, i.e., the corresponding Euclidean distance). ${\alpha }_{INS}=\mathrm{actan}\left(\frac{y_L-y_1}{x_L-x_1}\right)$ shows the sailing radian angle of the positive direction indicated by the abscissa of the coordinate system.

The EPMP consists of several important core parameters, including the central position $\left(x_O,y_O\right)$ of the large annulus guided by the INS sailing direction and distance, the maximum search radius $R_{\max }$ of the large annulus set by three times the INS cumulative drift error standard deviation and the scale margin between rings adjusted by the grid resolution $C$ of the gravity reference map. Following them, the total number of rings $M$ in the large annulus of the EPMP is $⌈R_{\max }/C⌉$, where $⌈\cdot ⌉$ is the ceil. Moreover, if further given a rotated angle $\theta$ between two adjacent grid points on each ring, the matched grid point set based on the EPMP can be calculated and determined. However, the matched grid point generation method of the small annulus in the SPMP is not suitable for the large annulus point generation based on the EPMP, because the fixed $\theta$ can keep the same number of the matched grid points on each ring, which results in the appearance of the phenomenon of “inner denseness and outer sparseness” in grid points obtained by the EPMP; that is, the separation distance between two grid points on the inner ring is too small, while, on the outer ring, is too large. Thus, the changed-angle three-ring grid point topology of the EPMP is designed and introduced, in which the ratio of the grid resolution $C$ to the radius $R_{⌈M/2⌉}$ of the middle ring is calculated to determine the matched grid point-generated reference angle $\phi$, and then, the inner and outer rotated angles between adjacent matched points, following the principle of “inner double and outer half”, are set to $2\phi$ and $\phi /2$ to obtain the matched grid point set in the large annulus of the EPMP. Figure 2 shows the distribution diagram of the matched grid point set in the large annulus of the EPMP at $\phi =\pi /6$ and $M=9$.

Considering that the normal distribution under natural conditions has a better applicability of the vast majority of systematic errors and its three-sigma rule has a statistically higher probability of covering a character than 99%, three kinds of IOAPs with $1\sigma$-EPMP, $2\sigma$-EPMP and $3\sigma$-EPMP are constructed, termed as $1\sigma$-IOAP, $2\sigma$-IOAP and $3\sigma$-IOAP, respectively, whose total number of rings in their large annulus are denoted by $M_1=⌈\sigma /C⌉$, $M_2=2⌈\sigma /C⌉$ and $M_3=3⌈\sigma /C⌉$, respectively. In addition, since $M_i$ may not be a multiple of three under different $\sigma$ principles, and in order to further enhance the good coverage effect of the large annulus grid points by the EPMP on the real location of underwater vehicles, $M_i$ should be adjusted as the following formulation, which contributes to splitting the total rings of the large annulus by the EPMP into the inner, middle and outer three layers:

$$M_i=\left\{\begin{array}{l}i⌈\sigma /C⌉+2,if \mathrm{mod}\left(i⌈\sigma /C⌉,3\right) =1\\ i⌈\sigma /C⌉+1,if \mathrm{mod}\left(i⌈\sigma /C⌉,3\right) =2\\ i⌈\sigma /C⌉+3, \mathrm{o}.\mathrm{w}.\end{array}\right.$$

(5)

where $i=1,2,3$ indicates the adopted $\sigma$-level principle, and the corresponding radius of rings is $R_j=jC,j=1,2,\cdots ,M_i$.

In order to determine the matched location of the three-layer grid points with the “inner-double and outer-half”-rotated angle, the gird resolution $C$ is used as the arc length between two adjacent grid points of the central ring in the large annulus, and the calculation expression of the reference angle ${\phi }_i$ for different $\sigma$ principles is

$${\phi }_i=\frac{C}{R_i^{0.5}},i=1,2,3$$

(6)

where $R_i^{0.5}$ represents the corresponding radius of the central ring in the large annulus under the ith $\sigma$ principle, and its formula is $R_i^{0.5}=C⌈\frac{M_i}{2}⌉$. According to Figure 2 and the above parameter settings, following the principle of “inner double and outer half” of the referenced angle ${\phi }_i$ with different $\sigma$, the matched grid point position $\left(x_i^{j,k},y_i^{j,k}\right)$ on the inner-middle-outer ring layers in the large annulus of the EPMP can be calculated as

$$\begin{array}{l}\mathrm{Inner}-\mathrm{layer} \mathrm{ring} \mathrm{points}:\hspace{1em}\hspace{1em}\left\{\begin{array}{l}{x}_i^{j,k}=x_O+R_j\sin \left(k\cdot 2{\phi }_i\right), j=1,2,\cdots ,n_i\\ y_i^{j,k}=y_O+R_j\cos \left(k\cdot 2{\phi }_i\right), k=0,1,\cdots ,n_{in}\end{array}\right.\\ \mathrm{Middle}-\mathrm{layer} \mathrm{ring} \mathrm{points}:\hspace{1em}\hspace{1em}\left\{\begin{array}{l}{x}_i^{j,k}=x_O+R_j\sin \left(k\cdot {\phi }_i\right), j=n_i+1,\cdots ,2n_i\\ y_i^{j,k}=y_O+R_j\cos \left(k\cdot {\phi }_i\right), k=0,1,\cdots ,n_{mid}\end{array}\right.\\ \mathrm{Outer}-\mathrm{layer} \mathrm{ring} \mathrm{points}:\hspace{1em}\hspace{1em}\left\{\begin{array}{l}{x}_i^{j,k}=x_O+R_j\sin \left(k\cdot \frac{{\phi }_i}{2}\right), j=2n_i+1,\cdots ,M_i\\ y_i^{j,k}=y_O+R_j\cos \left(k\cdot \frac{{\phi }_i}{2}\right), k=0,1,\cdots ,n_{out}\end{array}\right.\end{array}$$

(7)

where $n_i=M_i/3$ is the corresponding number of rings in each ring layer under the $i\sigma$ principle. $n_{in}=⌈\pi /{\phi }_i⌉-1$, $n_{mid}=⌈2\pi /{\phi }_i⌉-1$ and $n_{out}=⌈4\pi /{\phi }_i⌉-1$ indicate the number of grid point-rotated angles corresponding to the inner layer, the middle layer and the outer layer in the large annulus under the EPMP, respectively.

Given the high accuracy of the gravity value at the grid resolution in the gravity reference map, the reckon gravity value by the interpolation method may not truly reflect the actual gravity value at the matched point position. Thus, the matching process is similar to that of the traditional TERCOM [8], which is performed to determine the best-matched location of the underwater vehicle end point based on the EPMP. Set the total number of matched grid points in the inner, middle and outer three ring layers of the EPMP under a certain specific $\sigma$ principle as $N$; the $k\in \left\{1,2,\cdots ,N\right\}$ matched point in the large annulus is firstly taken as the end point $P_L^k$ of the underwater vehicle tracking sequence (the corresponding position $\left(x_k,y_k\right)$ in the gravity reference map) and is then compared with the grid resolution $C$ and the grid $bas{e}_L$ on the gravity reference map nearest to point $P_L^k$, obtained according to the rounding principle. Secondly, the gravity value $g_L^{base}$ is taken as the approximation of the $P_L^k$ gravity value of the matched point. Furthermore, a gravity tracking sequence $P_1^k{P}_2^k\cdots P_L^k$ corresponds to $P_L^k$ and is extracted by using the approximate grid position $bas{e}_L$ of $P_L^k$ and the sailing speed and direction information of the underwater vehicles. Simultaneously, their nearest neighbor gravity sequence $g_1^{base}{g}_2^{base}\cdots g_L^{base}$ is also obtained on the gravity reference map and compared with the real measured gravity sequence $g_1{g}_2\cdots g_L$ of the underwater vehicle to calculate the matched performance evaluation index. Taking the mean square deviation (MSD) as an example, the matched evaluation index value between them is marked as $MS{D}_k$. Similarly, all matched points in the large annulus of the EPMP are calculated to obtain their evaluation index value, termed as $\left\{\left.MS{D}_k\right|k=1,2,\cdots ,N\right\}$. Thus, following the principle of minimum MSD, the best matched position $\left(b{x}_L,b{y}_L\right)$ of the tracking end point of the underwater vehicle can be obtained.

2.3. Implementation Process of Iterative Optimal Annulus Point Algorithm for New Grid Topology

Coupling the SPMP in Section 2.1 with the EPMP in Section 2.2, the IOAP is proposed to realize the effective matching location of the tracking end point of underwater vehicles and, then, to modify the system control parameters of the INS and assist in accomplishing the long-time and long-distance navigation target of the underwater vehicle. In detail, the implementation process of the IOAP is as follows.

Step1: Import the gravity reference map ($\mathrm{mapt}$) of the study area, set the tracking starting position, the sailing speed $v$ and direction ${\alpha }_{INS}$ and the number $L$ of sampling points and sampling time interval $\mathsf{\Delta }t$, so as to further calculate and obtain the real position sequence $P_1{P}_2\cdots P_3$ of the simulated track. Moreover, the sailing speed error, the direction error and INS error are set, and the INS position sequence $S_1,S_2,\cdots ,S_L$ of the simulated track and the gravity value sequence $g_1{g}_2\cdots g_L$ are calculated and obtained.

Step2: The rotated angle ${\theta }_0$ and the proportional coefficient $\lambda$ of the ring radius in the SPMP mechanism are set to generate the matching grid points at the tracking starting point. Taking three times the INS error ${\sigma }_0$ at the tracking starting position as the maximum ring radius of the small ring domain, the ring radius $r_i$ of each ring and the rotated angle ${\beta }_j$ between the adjacent grid points can be calculated and obtained. Then, the INS starting point $\left(x_1,y_1\right)$ is taken as the center of the small ring domain, and each matching grid position $\left(x_i^j,y_i^j\right)$ in the small ring domain can be determined by Equation (1).

Step3: For each matching grid point $\left(x_i^j,y_i^j\right)$ in the small ring domain of the SPMP, the gravity value at its nearest grid point is extracted from the gravity reference map by using Equation (2) and used as the matching gravity value of this point $\left(x_i^j,y_i^j\right)$. Then, according to Equation (3), the best-matching position $\left(b{x}_1,b{y}_1\right)$ of the tracking starting point is obtained by following the principle of minimizing the absolute value of gravity deviation.

Step4: The tracking direction ${\alpha }_{INS}$ and distance $d_{INS}$ are derived and calculated according to the position relationship between the INS starting point $S_1$ and ending point $S_L$. Combined with the optimal matching position $\left(b{x}_1,b{y}_1\right)$ of the starting point based on the SPMP, the center position $\left(x_O,y_O\right)$ of the large ring domain in the EPMP can be obtained by employing Equation (4).

Step5: Following the different $\sigma$ principles of the INS errors, the total number $M_i$ of the large ring domains is calculated and achieved with Equation (5), and grid resolution $C$ is taken as the span interval between two adjacent ring radii $R_j$. At the same time, $C$ is taken as the arc length between the two adjacent grid points on the inter-ring of the large ring domain, and the reference angle ${\phi }_i$ between grid points in the large ring domain can be obtained by utilizing Equation (6).

Step6: Following the ring radius $R_j$ and the principle of “ inner double and outer half “ on referenced angle ${\phi }_i$, each matching grid position $\left(x_i^{j,k},y_i^{j,k}\right)$ of the large ring domain in the EPMP can be obtained according to Equation (7).

Step7: Each matching grid point $\left(x_i^{j,k},y_i^{j,k}\right)$ in the large ring domain of the EPMP is taken as the tracking ending point $P_L^k$. The gravity value $g_L^{base}$ of the grid position $bas{e}_L$ is extracted from the gravity reference map by following the nearest neighbor principle and is the approximate replacement of the gravity value of the matching point $P_L^k$. Then, according to the real sailing speed and direction and sampling time interval $\mathsf{\Delta }t$ of the underwater vehicles, a tracking sequence $P_1^k{P}_2^k\cdots P_L^k$ of the matching point $P_L^k$ is extracted from the gravity map and its corresponding nearest neighbor gravity sequence $g_1^{base}{g}_2^{base}\cdots g_L^{base}$ is calculated, which results in computing the MSD value of this matching point $P_L^k$, denoted by $MS{D}_k$.

Step8: For MSDs of all the matching points in the large ring domain of the EPMP, the optimal matching position $\left(b{x}_L,b{y}_L\right)$ of the tracking ending point based on the IOAP can be obtained by following the minimization optimization principle.

3. Verification and Application of the Proposed IOAP Algorithm

To test and verify the effectiveness and superiority of the proposed IOAP algorithm in the application of underwater vehicle gravity navigation, three groups of testes are carried out. In detail, Test1 is executed to verify the matched performance difference of the IOAP with different $\sigma$ criteria. In Test2, the reference angle calculated by different ring radii is used to examine the different influences on the matched performances of the algorithms. Test3 is performed to demonstrate the good applicability of the proposed IOAP for underwater gravity-matched navigation under different tracking starting points.

The instance data are sourced from the gravity anomaly data on the San Diego website (http://topex.ucsd.edu/, 10 June 2020) of the University of California, which originally has a $1^{\prime }\times 1^{\prime }$ resolution. As shown in Figure 3a, the gravity anomaly data in the South China Sea were selected for this paper, in which the longitude and latitude range are 113° E–115° E and 10° N–12° N, respectively. The bilinear interpolation method [21] is employed to transform the gravity anomaly datum data into 100-m × 100-m grid resolution gravity data, as shown in Figure 3b. Statistically the maximum value of the gravity anomaly in this area is 130.57 mGal, while the minimum value is −33.53 mGal, and the average value is 15.43 mGal. All tests were run with Windows 10 (20H2 Version), Intel(R) Core(TM) i7-8565U CPU @ 1.80 GHz 1.99 GHz and Matlab2018a.

3.1. Verification on the Matched Performance Difference of IOAP with Different $\sigma$ Criteria

The gravity anomaly grid resolution in the simulated sample block is $100 \mathrm{m} 100\mathrm{m}$, and the simulation parameters are set as follows: the accelerometer 0.01 °/h with gyro 10⁻³ m/s², the sailing speed 10 m·s⁻¹, the sailing direction northeast 70°, the initial position error 0 m, the velocity error 0.04 m·s⁻¹ and the sailing direction error 0.05°. In addition, the real-time measured data of the gravimeter are the sum of the sampled value in the gravity anomaly reference map and random noise with a standard deviation of 1 mGal, while the number of sampling points and the sampling period are set as 110 and 20 s, respectively. In this paper, suppose that the matching positioning accuracy is defined as $l$/m; the effective matching means that the absolute value difference between the matching position and the real position is located in the closed interval $\left[0,l\right]$. Thus, if the number of the effective matching of algorithms is $n$ in the $N$ experimental tests, the matching success rate can be shown as $\xi =\frac{n}{l}\times 100\%$. Meanwhile, the average value (mean), standard deviation (std), worst value (max) of the matching positioning accuracy in $N$ times tests and the average matching time T (excluding the simulation environment configuration time) are calculated and recorded as the performance evaluation index of the gravity matching algorithm.

To test and analyze the application performance of IOAPs with different $\sigma$ criteria in underwater vehicle gravity matching navigation, 100 times the independent experiments are carried out on 1$\sigma$-IOAP, 2$\sigma$-IOAP and 3$\sigma$-IOAP, while the TERCOM is used as the comparison algorithm. Moreover, the gravity reference map grid coordinates (1400, 1500) are selected as the simulation starting point for underwater vehicle navigation. The statistical results of 100 tests are shown in

Table 1. Comparison results of the gravity matching algorithms with different $\sigma$ criteria.

Algorithm	Mean/M	Std/M	Max/M	T/S	$\mathit{\xi }\left(\mathit{l}=100\right)$	$\mathit{\xi }\left(\mathit{l}=100\sqrt{2}\right)$
TERCOM	64.67	45.41	415.70	2.28 × 10−2	82	99
1$\sigma$-IOAP	394.95	518.07	2642.07	3.90 × 10−3	48	52
2$\sigma$-IOAP	99.23	191.91	1009.29	1.21 × 10−2	88	92
3$\sigma$-IOAP	49.39	40.68	370.01	2.41 × 10−2	95	99

, and the compared illustration of the matching positioning accuracy is shown in Figure 4.

As shown in Table 1, the matching positioning performance of the IOAPs is different under the different $\sigma$, where the matching performance of 3$\sigma$-IOAP is the best and significantly better than that of the canonical TERCOM. In terms of the T index, the average running time of 1$\sigma$-IOAP is the smallest, but the matching effect is poor, which results in limiting its effective application in actual underwater vehicle navigation to a certain extent. By contrast, the average running time of 2$\sigma$-IOAP is about half of the TERCOM, its average matching accuracy is less than one grid resolution and the matching probability of 2$\sigma$-IOAP is also better than the TERCOM (88% > 82%) under the condition of positioning accuracy $l=100$. Consequently, these results show that 2$\sigma$-IOAP has a certain potential navigation application value for the dual target conditions of matching efficiency and accuracy. Furthermore, 3$\sigma$-IOAP has almost the same T-index value as the TERCOM, while its mean, std and max index values on the matching accuracy, as well as the matching success probability, are better than all indexes of the TERCOM and most of the indexes of the other compared algorithms. It was fully demonstrated that the proposed 3$\sigma$-IOAP has a better matching performance and good potential practical value in the gravity-aided navigation of underwater vehicles.

To further analyze the differences of the successful matching effects of the four algorithms under the constraint of different positioning accuracies $l$, the comparison results of the successful matching probabilities for 100 of the tests are shown in

Table 2. Comparison results of the successful matching probability $\xi$/% of four algorithms under different positioning accuracies.

Algorithm	$\mathit{l}\mathbf{⩽}20$	$\mathit{l}\mathbf{⩽}40$	$\mathit{l}\mathbf{⩽}60$	$\mathit{l}\mathbf{⩽}80$	$\mathit{l}\mathbf{⩽}100$	$\mathit{l}\mathbf{⩽}120$	$\mathit{l}\mathbf{⩽}100\sqrt{2}$
TERCOM	0	0	82	82	82	82	99
1$\sigma$-IOAP	9	24	41	45	48	50	52
2$\sigma$-IOAP	17	47	61	79	88	92	92
3$\sigma$-IOAP	10	51	75	90	95	98	99

, where the $l$ s are set as 20, 40, 60, 80, 100 and 200, respectively. In addition, the comparison histogram is shown in Figure 5.

From Table 2, under different positioning accuracy $l$ constraints, the successful matching probabilities $\xi$ of $\sigma$-IOAPs are significantly different. Meanwhile, the three $\sigma$-IOAP models have been successfully matched a certain number of times at $l\le 40$, but the TERCOM fails. In view of the successful matching probability $\xi$ of algorithms with different positioning accuracies $l$, the matching performance of 3$\sigma$-IOAP is the best, and 2$\sigma$-IOAP is the second-best and better than the traditional TERCOM, but 1$\sigma$-IOAP is weaker than the TERCOM when the positioning accuracy is larger. Figure 5 further intuitively shows the excellent successful matching performance of 3$\sigma$-IOAP. To sum up, the above analysis conclusions still effectively verify the excellent performance of the proposed 3$\sigma$-IOAP in underwater gravity matching navigation.

To further explore and analyze the reasons why 3$\sigma$-IOAP is superior to the TERCOM and other IOAPs on the matching efficiency and accuracy, the scatter comparison graph between the matching position of the TERCOM and the underwater vehicle actual position are plotted as shown in Figure 6 (where the INS positions are used as the origin of image coordinates to ensure that the results of 100 tests can be drawn in the same image).

According to Figure 6a, the actual positions of the TERCOM in 100 tests are almost all within the $3\sigma$ error grid of the INS positions (see the solid-line square outside the dotted-line circle in Figure 6a), and all these positions are also almost located inside the $3\sigma$ annulus boundary dashed line centered on the INS position (see the dashed-line inscribed circle of the solid-line square in Figure 6a). These results, to a certain extent, show that there are a certain number of small probability-matched points in the rectangular grid of the TERCOM (see the points between the solid-line square and the dashed-line circle in Figure 6b). However, the matched probability of these points outside of the $3\sigma$ annulus is small but obviously affects the matched efficiency of the TERCOM. Thus, these outer matched points of the $3\sigma$ annulus are deleted for the gravity matching algorithm, which has no significant effect on the matched efficiency of the algorithms but can effectively improve the matched accuracy, which also indirectly proves the feasibility and effectiveness of designing the grid-matched mechanism based on the annulus topology.

To further analyze the reason why the matched accuracy of 3$\sigma$-IOAP is better than the other $\sigma$-IOAPs, the matched effects of different $\sigma$-IOAPs are drawn in Figure 7.

From Figure 7, it can be seen that the coverage matched effects of IOAPs with different $\sigma$ annulus are different for the real positions of underwater vehicles. On the one hand, 3$\sigma$-IOAP realizes the high precision matching positioning of the underwater vehicle by using the grid point coverage of its large annulus. On the other hand, 2$\sigma$-IOAP and 1$\sigma$-IOAP only achieve a good match for the real position, but the optimal matched point for the real position outside the domains is often only scattered on the boundary ring, and it is difficult to find a better matched position, which dramatically affects the matching accuracy of the models. To a certain extent, it further explains the difference of the successful matched probability by employing different $\sigma$ IOAPs in Table 1 and Table 2 and the better matched accuracy of 3$\sigma$-IOAP. In summary, 3$\sigma$-IOAP has better and more important potential application value in the gravity matching navigation of underwater vehicles.

3.2. Verification of the Difference Influence on Matching Performance of IOAPs with Different Ring Radius Reference Angle

To further study the effect of the reference angle $\phi$ based on a different ring radius $R$ on the matching performance of 3$\sigma$-IOAP, the maximum ring radius $R^1=R_{M/3}$ of the inner ring layer, the middle ring radius $R^{1.5}=R_{⌈M/2⌉}$ of the “inner-middle” ring layer, the maximum ring radius $R^2=R_{2M/3}$ of the middle ring layer and the middle ring radius $R^{2.5}=R_{⌈5M/6⌉}$ of the “middle-outer” ring layer are applied to calculate the reference angle $\phi$ of 3$\sigma$-IOAP, respectively, and then, the corresponding changed-angle three-annulus layer matched grid point sets are generated, in which these models are termed as 1-IOAP, 1.5-IOAP (i.e., 3-IOAP in Section 3.1), 2-IOAP and 2.5-IOAP, respectively. In addition, from Equation (6), it should be remarked that the larger the ring radius $R$, the smaller the reference angle $\phi$ of 3$\sigma$-IOAP, and the more the total number $N$ of the generated matched grid points, so, in theory, the matched efficiency of 1-IOAP is the fastest, while the run time of 2.5-IOAP is the slowest.

The parameter settings, including the simulated sample block and the grid coordinates of the tracking starting point, are all the same as Section 3.1. The matching positioning statistical results of 100 independent tests are collected and recorded in

Table 3. Comparison of the matched performances of IOAPs with different ring radius reference angles.

Algorithm	Mean/M	Std/M	Max/M	T/S	$\mathit{\xi }\left(\mathit{l}=100\right)$	$\mathit{\xi }\left(\mathit{l}=100\sqrt{2}\right)$
TERCOM	64.67	45.41	415.70	2.28 × 10−2	82	99
1-IOAP	53.93	32.85	156.43	1.77 × 10−2	88	98
1.5-IOAP	49.39	40.68	370.01	2.41 × 10−2	95	99
2-IOAP	46.82	27.53	145.58	3.11 × 10−2	95	99
2.5-IOAP	45.74	24.52	156.94	4.07 × 10−2	97	99

and

Table 4. Comparison of the matching success probability of IOAPs with different location accuracies (%).

Algorithm	$\mathit{l}\mathbf{⩽}20$	$\mathit{l}\mathbf{⩽}40$	$\mathit{l}\mathbf{⩽}60$	$\mathit{l}\mathbf{⩽}80$	$\mathit{l}\mathbf{⩽}100$	$\mathit{l}\mathbf{⩽}120$	$\mathit{l}\mathbf{⩽}100\sqrt{2}$
TERCOM	0	0	82	82	82	82	99
1-IOAP	15	44	64	83	88	95	98
1.5-IOAP	10	51	75	90	95	98	99
2-IOAP	15	46	77	88	95	98	99
2.5-IOAP	14	44	76	92	97	98	99

respectively, where the matched accuracy illustration associated with Table 3 is shown in Figure 8, and the successful matched probability of IOAP in Table 4 is compared and shown in Figure 9.

From Table 3, the matched accuracy and the matched success rate of 3$\sigma$-IOAP are almost all improved with the increase of the reference angle ring radius $R$. The performance of 2.5-IOAP is the best, followed by 2-IOAP, and the matching effect of 1-IOAP is relatively poor but still better than the TERCTOM in the 5/6 index, which indicates that the reference angle based on a different ring radius will affect the matching performance of 3$\sigma$-IOAP. In view of the T index, the IOAP models with different ring radius reference angles show the phenomenon of “matching efficiency decrease” with the increase of $R$, and the result is consistent with the above theoretical analysis conclusion, which shows that the total number of matched grid points for the different ring radius reference angles are different, which further leads to differences of the matched efficiency. Therefore, in actual applications of underwater vehicle navigation, 3$\sigma$-IOAP for the specific matching target requirements can be properly selected by using the suitable ring radius $R$ in order to implement certain specific navigation matching tasks. In summary, the proposed 3$\sigma$-IOAP model in this paper has a higher robustness and better application value for underwater gravity matching navigation.

In a further comprehensive analysis of the results in Table 4 and Figure 9, the matched accuracy of 2.5-IOAP and its success probability under the different $l$ scales are almost better than the other IOAPs and TERCOM, but its matching T index value is the highest and almost twice as long as the matched time of the TERCOM. Thus, 2.5-IOAP may be the best choice for the gravity matching algorithm when the underwater navigation objects have no high requirement for the matching efficiency and pay more attention to the matching accuracy. When the bi-objects, including the matched efficiency and accuracy, are required, 1-IOAP is the best choice for the gravity matching algorithm. Therefore, these conclusions further proved that 3$\sigma$ IOAP has a better robustness for underwater gravity matching navigation. In addition, 1.5-IOAP (i.e., 3$\sigma$-IOAP in Section 3.1) shows a better tradeoff effect between the matching efficiency and the matching accuracy and has a considerable matched performance with the TERCOM on the matching efficiency, so the next section will use 1.5-IOAP to further test its good matching performance for different tracking starting points.

3.3. Verification of the Good Matching Performance of IOAP for Different Tracking Starting Points

To further verify the good matching applicability of 1.5-IOAP in the underwater vehicle matching navigation, according to our team’s partitioning results of the suitability area in this researching simulated block [22], three regional grid coordinates, such as A (1660,740), B (1550,740) and C (1400,350), are respectively taken as the tracking starting points of underwater vehicles in this section. The other model parameters are set the same as Section 3.1, and the comparative statistical results of 100 tests are collected in

Table 5. Comparison matching results of IOAPs for different tracking starting points.

Starting Point	Algorithm	MEAN/M	STD/M	MAX/M	T/S	$\mathit{\xi }\left(\mathit{l}\right)$
						40	60	80	100	120	$100\sqrt{2}$
A	TERCOM	78.16	41.03	269.87	2.07 × 10−2	0	62	62	62	62	99
	1.5IOAP	62.24	31.15	142.37	2.36 × 10−2	30	47	69	87	97	99
B	TERCOM	96.82	47.78	415.70	2.03 × 10−2	0	36	36	36	45	99
	1.5IOAP	57.71	34.97	149.82	2.35 × 10−2	33	58	73	84	96	99
C	TERCOM	83.48	55.58	534.45	2.04 × 10−2	0	66	66	66	66	99
	1.5IOAP	71.89	30.07	148.78	2.39 × 10−2	14	33	60	82	94	99

.

As shown in Table 5, under the premise of little differences in the matching efficiency (T index), 1.5-IOAP is significantly better than the traditional TERCOM in the average matching accuracy (mean), the matching accuracy standard deviation (std) and the worst matching accuracy (max). Compared with the TERCOM, the max index values of 1.5-IOAP in the three different starting point situations are improved by 47.24%, 63.96% and 72.16%, respectively, while the mean index values are relatively improved by 20.37%, 40.39% and 13.88%, which shows that the proposed IOAP in this paper has a higher matching accuracy and better synchronous optimizing capacity of multiple tests. On the other hand, for different $l$ scales of the successful match, the proposed model shows a relatively superior matching success rate than the TERCOM. Especially when the matching accuracy is less than 40 m, 1.5-IOAP still has a certain probability to successfully achieve the positioning match of underwater vehicles. Thus, these results effectively prove the outstanding matching applicability of the proposed IOAP model for the different domain starting points in the underwater gravity matching navigation.

For intuitively displaying the matching difference effects between 1.5-IOAP and TERCOM in three test positions, a position comparison diagram of the worst match in 100 tests was drawn, as shown in Figure 10.

As shown in Figure 10, 1.5-IOAP, compared with the TERCOM, has a higher matching positioning accuracy for underwater gravity matching navigation with different-domain tracking starting points. Furthermore, since the ending points of these three sailing trajectories are located in different gravity ranges, it is effectively verified that the proposed IOAP algorithm also has good matching adaptability for the different gravity domains, to a certain extent. To sum up, the effectiveness and reliability of the IOAP based on the new grid topology are full demonstrated to improve the gravity matching accuracy of underwater vehicles.

4. Conclusions

In this paper, an iterative optimal annulus point model with a novel grid topology was proposed to improve the matching accuracy of underwater gravity-aided navigation.

(1)

The construction of an iterative optimal annulus point model with a novel grid topology. On the basis of breaking out from the traditional square-shaped grid topology of the TERCOM, the annulus-shaped topology of the matching grid points was constructed. Then, the Matching Positioning strategy of the tracking Starting Point in small annulus (SPMP) and the Matching Positioning mechanism of the tracking Ending Point in three-layer annulus (EPMP) were proposed by employing the INS sailing direction and distance information. Furthermore, an iterative optimal annulus point model with a novel grid topology was constructed by coupling the SPMP and EPMP.

(2)

The optimization selection of the $\sigma$ criterion and the reference angular ring radius $R$ for the optimal matching navigation in the large annulus at the tracking ending point. In this paper, the matching evaluation indexes, such as the average matching accuracy, matching standard deviation, average matching time and matching success rate, were comprehensively compared. These evaluation indexes were taken as the selection basis of the model parameters, which resulted in verifying the different influences of the parameters on the matching performances of the proposed iterative optimal annulus point algorithm and realizing the selection of good parameters to improve the underwater gravity matching accuracy.

(3)

The improvement of the underwater matching navigation accuracy. For the matching performance testing in three regions, the results showed that the iterative optimal annulus point model with a novel grid topology, compared with the TECOM, had little difference of the average matching time. The worst matching accuracies of the proposed model were also improved by up to 47.24%, 63.96% and 72.16%. Simultaneously, the average matching accuracies of the iterative optimal annulus point model were increased by up to 20.37%, 40.39% and 13.88%, respectively. In summary, the iterative optimal annulus point model with a novel grid topology contributed to enhancing the matching accuracy of underwater vehicle gravity matching navigation.