A Novel Algorithm for Enhancing Terrain-Aided Navigation in Autonomous Underwater Vehicles

Abstract

The position error in an inertial navigation system (INS) for autonomous underwater vehicles (AUVs) increases over time. Terrain-aided navigation can assist in correcting these INS position errors. To enhance the matching accuracy under large initial position errors, an improved terrain matching algorithm comprising terrain contour matching (TERCOM), particle swarm optimization (PSO), and iterative closest contour point (ICCP), named TERCOM-PSO-ICCP, is proposed. Initially, an enhanced TERCOM with an increased rotation angle is utilized to minimize heading errors and reduce the initial position error. The similarity extremum approach evaluates the initial matching outcomes, leading to an enhanced accuracy in the initial results. Next, artificial bee colony (ABC)-optimized PSO is employed for secondary matching to further reduce the initial position error and narrow the matching area. Finally, the ICCP, using the Mahalanobis distance as the objective function, is applied for the third matching, leveraging the ICCP’s fine search capabilities. The effective combination of these three algorithms significantly improves the terrain-aided navigation matching effect. Two tests show that the improved TERCOM-PSO-ICCP effectively reduces the matching error and corrects the position of the INS.

1. Introduction

The demand for autonomous underwater vehicles (AUVs) in both military and civilian fields has been rising. Despite their utility, the inertial navigation system (INS) used in AUVs suffers from accumulating position errors over time, which compromises the accuracy of its navigation. Currently, the positioning technologies associated with aided inertial navigation encompass satellite navigation, underwater acoustic positioning and navigation, and geophysical navigation. Satellite navigation technology often struggles with signal transmission in aquatic environments, making it challenging to achieve accurate positioning in deep water. Underwater acoustic positioning and navigation technology relies on support from a mother ship, which restricts its operational range. In contrast, geophysical navigation technology utilizes data from gravity, magnetic fields, and topographical features for positioning. Among these, gravity and magnetic field data are more susceptible to external influences, whereas terrain information remains relatively stable and is less affected by external conditions [1].

Terrain-aided navigation (TAN) offers a solution with its high precision and autonomous correction capabilities for INS errors. The accuracy and feasibility of TAN are influenced by the sensor equipment used in AUVs. While high-precision and high-configuration AUV technology is well-established, there is limited research and application concerning low configurations of small- and medium-sized AUVs. Consequently, investigating TAN for low configurations of small- and medium-sized AUVs is of significant importance. TAN operates by employing matching algorithms to align the indicated trajectory from the INS with the most similar path in a digital map. The effectiveness of TAN largely depends on the specific terrain matching algorithm utilized, as different algorithms yield varying levels of accuracy and performance, making it a key part of the system [2].

Globally, researchers have extensively studied various terrain matching algorithms. Cheng et al. [3] introduced a two-stage joint algorithm to enhance the terrain contour matching (TERCOM) accuracy, though it suffers from slow processing speeds. Zhao et al. [4] and Yoo et al. [5] suggested that reducing the course angle error could improve TERCOM’s matching accuracy, but this also resulted in slow speeds. Zhao et al. [6] proposed to add adaptive transfer to the TERCOM matching process to improve its matching efficiency. Wang et al. [7] improved the anti-interference capability of the iterative closest contour point (ICCP) by using the Mahalanobis distance instead of the Euclidean distance. Ji et al. [8] applied an improved particle swarm optimization (PSO) to terrain matching, reducing the matching time and improving the results, though the accuracy still needed enhancement with large initial errors. Wang et al. [9] proposed the artificial bee colony (ABC)-optimized PSO (F-WAPSO) to address PSO’s tendency for local optimization, demonstrating its feasibility in terrain matching tests. Wang et al. [10] introduced optimization of the ICCP using a sliding window technique to enhance positioning precision. Xu et al. [11] and Zhang et al. [12] employed a two-stage coarse-to-fine strategy to enhance the ICCP’s accuracy. Wang et al. [13] combined the ICCP with an improved PSO to mitigate the ICCP’s sensitivity to initial errors. Yuan et al. [14] integrated the TERCOM and ICCP algorithms, leveraging their performance differences to address TERCOM’s heading issues, although both algorithms’ batch processing nature extended the matching time. Finally, Wang et al. [15] combined the improved TERCOM and PSO algorithms, initially increasing the rough matching accuracy by reducing TERCOM’s heading errors, followed by fine matching by using an enhanced PSO to improve the overall terrain matching performance. Zong et al. [16] proposed utilizing PSO to enhance the performance of back propagation (BP) neural networks for classifying matching regions, thereby improving the overall matching effectiveness. Khalilabadi et al. [17], Potokar et al. [18], and Xu et al. [19] each made advancements to the improved Kalman filter (parametric filter) and extended Kalman filter (EKF) to accurately determine navigation positions. However, the inherent characteristics of these filtering algorithms often lead to positioning divergence in complex nonlinear terrains. Jiang et al. [20] and Ferreira et al. [21] enhanced simultaneous localization and mapping (SLAM) technology to improve navigation and positioning accuracy and robustness; however, they observed increased instability when integrating information from multiple sensors.

TERCOM is a classic batch processing algorithm known for its ease of implementation, simple operation, and initial alignment. However, it falls short in real-time performance and is particularly sensitive to heading errors [22]. The ICCP, on the other hand, excels in fine search capabilities by finding the closest matching point through continuous rigid transformations involving both rotation and translation. While the ICCP achieves a high matching accuracy with small initial errors, it requires longer processing times and may result in mismatches if the initial position error exceeds a certain range [23]. PSO, a typical swarm intelligence optimization algorithm, stands out for its simplicity and fast search speed. In underwater terrain matching, PSO can rapidly identify the optimal matching sequence, but is prone to becoming trapped in local optima [24]. Given the distinct characteristics of each matching algorithm, no single algorithm can fully meet the needs of AUV navigation. Therefore, combining different algorithms can be more effective.

This paper introduces an improved TERCOM-PSO-ICCP algorithm that integrates the strengths of TERCOM, PSO, and the ICCP to enhance the accuracy of TAN under large initial position error with a low configuration of small- and medium-sized AUVs. The main contents are as follows:

(1)

The implementation process of the improved TERCOM-PSO-ICCP is proposed. Initially, an improved version of TERCOM is utilized for the initial matching. This enhanced TERCOM incorporates a rotation angle mechanism, addressing the course sensitivity issues related to traditional TERCOM and improving its initial matching accuracy.

(2)

The second matching algorithm employs PSO optimized by the ABC method. By integrating ABC with PSO, the global optimization capability of PSO is enhanced. This combination swiftly identifies the optimal matching sequence, further minimizing the initial position error. The final stage utilizes an improved version of the ICCP, leveraging its fine search capabilities. This improved ICCP replaces the Euclidean distance in fitness function with Mahalanobis distance, reducing the impact of noise.

(3)

A similarity extremum method is proposed to evaluate the results of the improved TERCOM. Meanwhile, the confidence ellipse is used as the search region to improve the optimality of the algorithm.

(4)

Finally, simulation experiments are conducted to validate the performance of the improved TERCOM-PSO-ICCP.

2. The Improved Design of the TERCOM-PSO-ICCP Algorithm

The design of TERCOM-PSO-ICCP leverages the unique strengths of the TERCOM, PSO, and ICCP algorithms. TERCOM is capable of conducting extensive searches, even with significant initial position errors, making it ideal for the initial capture and positioning phase of terrain matching. However, TERCOM’s sensitivity to heading errors limits its matching accuracy. PSO excels in global optimization, rapidly identifying the optimal positions in terrain matching, but it is prone to local optima and requires enhancement. The ICCP, through continuous rigid transformations, can locate the best matching position with a high accuracy when the initial error is small. After the two algorithms’ matching, the ICCP further enhances the matching accuracy [25]. The improved TERCOM-PSO-ICCP algorithm combines these algorithms to optimize its performance, as shown in Figure 1.

2.1. Improved TERCOM Algorithm

TERCOM [26] is centered on the location indicated by the INS, creating a grid within a specified search range. It then traverses each grid cell within this range to generate multiple sets of parallel sequences with the INS track. A correlation analysis is performed between the depth values at each point in these sequences and the measured depth values to find the optimal matching sequence, thereby correcting the INS position. Traditional TERCOM only accounts for translational changes and not rotational changes, making it sensitive to heading errors. To mitigate this, the improved TERCOM incorporates rotation angles. The approach to enhancing TERCOM with rotation angles involves the following steps:

First, based on the INS-indicated track, the center of gravity coordinates $(x_h,y_h)$ of the entire matching track are determined by Equations (1)–(4):

$$x_h={\displaystyle \frac{d_i}{D}}{\displaystyle \sum _{i=1}^N{x}_i,y_h=\frac{d_i}{D}{\displaystyle \sum _{i=1}^N{y}_i}}$$

(1)

$$d_i=H_A(i)-H_S(i)$$

(2)

$$D(H_A,H_S)={\displaystyle \sum _{i=1}^N\sqrt{{[H_A(i)-H_S(i)]}^{\mathrm{T}}{C}^{-1}[H_A(i)-H_S(i)]}}$$

(3)

$$C=\mathrm{cov}(H_A,H_S)=\mathrm{E}[(H_A(i)-{\overline{H}}_A)(H_S(i)-{\overline{H}}_S)]$$

(4)

where $\left(x_i,y_i\right)$ indicates the track coordinates for the INS and D is the distance between the measured elevation sequence and the INS-indicated sequence. The Mahalanobis distance is calculated, which can reduce the influence of noise; d_i represents the difference between the value of the corresponding point in the digital map and the actual measured depth value; N is the length of the matching sequence; C is the covariance matrix; $H_A\left(i\right)$ is the depth value of the i-th measured sampling point; $H_S\left(i\right)$ is the value of the i-th point in the digital map; and ${\overline{H}}_A$ and ${\overline{H}}_S$ are the average values.

Secondly, taking the center of gravity of the matching sequence as the origin, Equations (5) and (6) for adding rotation angles to the track provided by the INS are as follows:

$$[x_p,y_p]=f(\alpha )[x-x_h,y-y_h]$$

(5)

$$f(\alpha )=\left[\begin{array}{cc}\cos \alpha & -\sin \alpha \\ \sin \alpha & \cos \alpha \end{array}\right]$$

(6)

where $\left(x_p,y_p\right)$ is the track coordinate after increasing the rotation angle and $f\left(\alpha \right)$ is a steering matrix composed of $\alpha$.

Finally, $[x_p,y_p]$ is used as the initial matching sequence for TERCOM matching to realize matching positioning.

The process of incorporating a rotation angle into the TERCOM matching is as follows:

(1)

Determine the center of gravity coordinates (x_h, y_h) based on the track position indicated by the INS.

(2)

For each grid matching, initiate the rotation angle $\alpha$ from 0° and incrementally rotate the original INS track by steps of s = 0.2°. With each rotation, traverse every grid in the search area and perform TERCOM matching according to the rotated and translated track. Continue the rotation process until the rotation angle exceeds twice the INS angle error (k), where k is equal to 2 × INS angle error.

(3)

Use the mean square difference (MSD) as the criterion for TERCOM matching. The MSD formula is provided in Equation (7). The MSD algorithm reflects the degree of fitting between the matched trajectory and the true trajectory, and can measure the accuracy of the matched trajectory well [27]. Identify the minimum MSD value and the corresponding optimal coarse rotation angle $\alpha$_i.

$$MSD\left({\tau }_x,{\tau }_y\right)={\displaystyle \frac{1}{N}}{\displaystyle \sum _{i=1}^N{\left[H_A(i)-H_S(x+i{\tau }_x,y+i{\tau }_y)\right]}^2}$$

(7)

where (${\tau }_x,{\tau }_y$) is the constant distance component of the vehicle in the directions of the two axes of the digital map.

(4)

Based on the coarse rotation results, take ${\alpha }_i$ as the center and perform fine rotations with a step size of $s/10$ within the range of $\left[{\alpha }_i-2s,{\alpha }_i+2s\right]$. Repeat the TERCOM translation matching process (step 3). When the difference between two matching results meets the limit difference, stop the iteration. The precise rotation angle ${\alpha }_j$ and the matching sequence $P_j$ after these fine rotations are obtained.

The matching process of TERCOM for incorporating rotation angles is shown in Figure 2.

TERCOM with an added rotation angle can more accurately align with the real track. The transition from rough rotation to fine rotation effectively minimizes course deviation. This enhanced version of TERCOM is referred to as TERCOM-A.

2.2. Improved PSO Algorithm

PSO [28] is extensively utilized in TAN due to its simplicity, rapid solution speed, and strong global optimization capabilities. To improve the stability of PSO, PSO is combined with ABC, forming the F-WAPSO. The F-WAPSO enhances the learning factors of traditional PSO, as illustrated in Equations (8) and (9), thereby improving its global optimization ability. Since ABC is a directional random search, it is introduced in the later stages of the improved PSO iteration. This inclusion helps to enhance the diversity of the PSO population and helps it to jump out of local optima. The integration of two algorithms not only increases population diversity, but also facilitates the discovery of the optimal solution.

$$v_{ij}(\tau +1)=\omega v_{ij}(\tau )+c_1(\tau )r_1\left[gbes{t}_j(\tau )-x_{ij}(\tau )\right]+c_2{r}_2\left[pbes{t}_{ij}(\tau )-x_{ij}(\tau )\right]$$

(8)

$$x_{ij}(\tau +1)=x_{ij}(\tau )+v_{ij}(\tau +1)$$

(9)

where i represents the total number of particles (i = 1, 2, …, s); j represents the dimensions (j = 1, 2, …, s); c₁ and c₂ are acceleration factors; $r_{1j}\left(\tau \right),r_{2j}\left(\tau \right)\in \left(\mathrm{0,1}\right)$ are random numbers; $\omega$ is the inertial weight; $x_{ij}\left(\tau \right)$ is the position of particle i in the j-th dimension; $v_{ij}\left(\tau \right)$ is the velocity of particle i in the j-th dimension; ${pbest}_{ij}\left(\tau \right)$ is the individual optimal solution of particle i in the j-th dimension; and ${gbest}_{ij}\left(\tau \right)$ is the global optimal solution in the j-th dimension.

Where $c_1\left(\tau \right)=2\times \mathrm{t}\mathrm{h}\tau \times c_1=2\times {\displaystyle \frac{e^{\tau }-e^{-\tau }}{e^{\tau }+e^{-\tau }}}\times c_1$ and $c_1$ becomes a time-varying function. As the number of iterations increases, $c_1$ gradually becomes larger. The inertia weight is shown in Equation (10).

$$w(\tau )=w_{\min }+(w_{\max }-w_{\min })\times {\displaystyle \frac{{\tau }_{\max }-\tau }{{\tau }_{\max }}}$$

(10)

where ${\tau }_{max}$ is the maximum number of iterations and ${\omega }_{\min }$ and ${\omega }_{\max }$ are the minimum and maximum inertia factors, respectively.

F-WAPSO first uses the improved PSO to search and then introduces ABC in the later iterations. In this combined algorithm, pbest_ij is regarded as the employed forager in ABC, and onlooker bees search around it greedily. ABC significantly mitigates PSO’s tendency to settle into local optima in later iterations. Figure 3 shows the implementation process of F-WAPSO.

2.3. Improved ICCP Algorithm

The ICCP [10,29] continuously performs rigid transformations during the matching process to find the set of points on the contour that are closest to the positions indicated by the INS. This involves repeated rotations and translations to identify the best matching points, demonstrating a strong capability for fine searching. In Figure 4, A_i (i = 1, 2, …, N) represents the bathymetric measurement points along the actual track, where N is the length of the matching sequence. C_i (i = 1, 2, …, N) represents the isobath, p_i (i = 1, 2, …, N) indicates the INS-provided locations, and y_i (i = 1, 2, …, N) denotes the matched locations. The concept of the ICCP is that each A_i must lie on a specific depth line, and the corresponding p_i should be close to C_i. The optimal estimation point y_i is then determined according to the given criteria. The optimal matching sequence is formed by connecting these points. The Euclidean distance used in the ICCP is expressed in Equation (11).

$$D(Y,TP)={\displaystyle \sum _{\mathrm{i}=1}^N{‖y_i-\left(t+R{p}_i\right)‖}^2}$$

(11)

where R represents the rotation transformation and t represents the translation transformation.

To strengthen the noise resistance of the ICCP, the Mahalanobis distance, as shown in Equation (12), is proposed, reducing the impact of errors on the system. The Mahalanobis distance effectively accounts for variable correlation, making the algorithm more robust and practical [30].

$$\begin{array}{l}{M}_D(Y,TP)={\displaystyle \sum _{i=1}^N\left[{\left(y_i-T{p}_i\right)}^T{V}^{-1}\left(y_i-T{p}_i\right)\right]}\\ T{p}_i=R{p}_i+t\\ V_{ij}=\mathrm{cov}(y_i,p_j)=E[(y_i-\overline{y})(p_j-\overline{p})]\end{array}$$

(12)

where V_ij represents the covariance matrix of the (i,j)th term and $\overline{y}$ and $\overline{p}$ are the means of the sample. The location with the smallest M_D value is the optimal matching location.

3. Optimization of the Improved TERCOM-PSO-ICCP Algorithm

The improved TERCOM-PSO-ICCP relies on the principles of TERCOM-A, with subsequent results from F-WAPSO and the improved ICCP depending on the initial success of TERCOM-A. If TERCOM-A achieves accurate matching the first time, the final matching result will be significantly closer to the actual position. Therefore, it is crucial to evaluate the initial matching result of TERCOM-A. When the initial error is substantial, the INS-indicated track is far away from the true track. In such cases, the search area should be broadened during the initial matching to ensure that the true position falls within the search range. However, an excessively large search area can increase the matching time and negatively impact the matching accuracy. Thus, selecting an appropriate search scope is a critical factor in the initial matching process.

3.1. Improved TERCOM Algorithm Matching Evaluation

TERCOM-A uses the MSD criterion to perform a correlation analysis across the entire search region to identify the optimal matching solution. This means that each grid in the search area is assigned a corresponding MSD value, with the minimum MSD value indicating the final position. However, when the initial error is significant and the search area is broad, numerous topographic grid sequences may exhibit a high similarity to the measured topographic elevation sequences, thus reducing the algorithm’s reliability. To address this, the initial matching results of TERCOM-A can be evaluated using the similarity extremum method.

Within the digital reference map’s matching search grid M × N, the MSD value corresponding to each grid (x, y) is denoted as M (x, y). The similarity extremum method is shown in Equations (13) and (14).

$${\left.{\displaystyle \frac{\partial M}{\partial x}}\right|}_{\begin{array}{c}x=x_0\\ y=y_0\end{array}}=0;{\left.{\displaystyle \frac{\partial M}{\partial y}}\right|}_{\begin{array}{c}x=x_0\\ y=y_0\end{array}}=0$$

(13)

$${\left.{\displaystyle \frac{{\partial }^{2}M}{\partial x^2}}\right|}_{\begin{array}{c}x=x_0\\ y=y_0\end{array}}>0;{\left.{\displaystyle \frac{{\partial }^{2}M}{\partial y^2}}\right|}_{\begin{array}{c}x=x_0\\ y=y_0\end{array}}>0$$

(14)

where $x_0$ is the x-coordinate of the grid center corresponding to the MSD and $y_0$ is the y-coordinate of the grid center corresponding to the MSD.

The minimum value of M (x, y) can be determined using Equations (13) and (14). Equation (13) calculates the first partial derivatives of M (x, y) in the x and y directions. The grid where the first-order partial derivative equals zero represents an extremum for the MSD. Equations (13) and (14) are used for solving the partial derivatives of continuous two-dimensional surface functions. Since actual underwater digital maps are processed as grids, the MSD determination method involves discrete grid data. Therefore, the finite difference method is employed to deal with the discrete partial derivatives. These formulas can be broken down into forward and backward difference quotients, as shown in Equations (15) and (16), respectively.

$$\begin{array}{l}{\left.{\displaystyle \frac{\partial M(x,y)}{\partial x}}\right|}_{\begin{array}{c}x=x_0\\ y=y_0\end{array}}\approx {\displaystyle \frac{M(x+h,y)-M(x,y)}{h}}\\ {\left.{\displaystyle \frac{\partial M(x,y)}{\partial y}}\right|}_{\begin{array}{c}x=x_0\\ y=y_0\end{array}}\approx {\displaystyle \frac{M(x,y+h)-M(x,y)}{h}}\end{array}$$

(15)

$$\begin{array}{l}{\left.{\displaystyle \frac{\partial M(x,y)}{\partial x}}\right|}_{\begin{array}{c}x=x_0\\ y=y_0\end{array}}\approx {\displaystyle \frac{M(x,y)-M(x-h,y)}{h}}\\ {\left.{\displaystyle \frac{\partial M(x,y)}{\partial y}}\right|}_{\begin{array}{c}x=x_0\\ y=y_0\end{array}}\approx {\displaystyle \frac{M(x,y)-M(x,y-h)}{h}}\end{array}$$

(16)

where h represents the grid width. If all grid values in the search area are evaluated and Equation (16) results in a value less than zero while Equation (15) results in a value greater than zero, the grid is considered to have a minimum value at that point. The sequence of minimum values across the entire region is denoted as Mn(i). The TERCOM-A mismatch diagnosis principle can then be expressed using Equation (17).

$$\left\{\begin{array}{c}Num\left(Mn\left(i\right)\_msd\right)=1match\\ Num\left(Mn\left(i\right)\_msd\right)>1mismatching\end{array}\right.$$

(17)

where Num() represents the number of grids that satisfy the condition and $Mn\left(i\right)\_\mathrm{m}\mathrm{s}\mathrm{d}$ denotes the minimum MSD value.

When the improved TERCOM-PSO-ICCP is used for underwater terrain matching, the first matching result is considered to be accurate if there is only one extreme point within the threshold range of TERCOM-A in the matching area.

3.2. Improved Search Range for the TERCOM-PSO-ICCP Algorithm

TERCOM-A, serving as the initial matching step in the improved TERCOM-PSO-ICCP process, requires an expanded search range to ensure that the real track position falls within this area. However, an excessively large search area can negatively impact the matching speed. To balance this, a suitable search region can be determined using a confidence ellipse. This ellipse, which is based on a probability criterion, contains the true location of the AUV with a certain degree of confidence, thereby optimizing the search region selection.

Research on INSs assumes that position errors are typically modeled by a standard normal distribution. This includes the east ${\sigma }_x$ and north ${\sigma }_y$ standard deviations, with associated variances ${\sigma }_x^2$ and ${\sigma }_y^2$, and covariances ${\sigma }_{xy}$ and ${\sigma }_{yx}$. The confidence ellipse equation is provided in Equation (18) [9].

$$\left\{\begin{array}{c}a={\widehat{\sigma }}_0\sqrt{{\displaystyle \frac{1}{2}}({\sigma }_x^2+{\sigma }_y^2+\sqrt{{({\sigma }_x^2-{\sigma }_y^2)}^2+4{\sigma }_{xy}^2})}\\ b={\widehat{\sigma }}_0\sqrt{{\displaystyle \frac{1}{2}}({\sigma }_x^2+{\sigma }_y^2-\sqrt{{({\sigma }_x^2-{\sigma }_y^2)}^2+4{\sigma }_{xy}^2})}\\ \phi ={\displaystyle \frac{\pi }{2}}-{\displaystyle \frac{1}{2}}\arctan (2{\sigma }_{xy}/({\sigma }_x^2-{\sigma }_y^2))\end{array}\right.$$

(18)

where a and b denote the lengths of the major and minor axes of the ellipse and φ describes the angle between the ellipse’s major axis and the north direction. The expansion factor ${\widehat{\sigma }}_0$ scales the ellipse, centered at the current INS position. For ease of computation, the circumscribing rectangle of the error ellipse is used as the underwater terrain search area, as shown in Equation (19).

$$\left\{\begin{array}{c}{x}_m=2\sqrt{a^2{\sin }^2\phi +b^2{\cos }^2\phi }\\ y_m=2\sqrt{a^2{\cos }^2\phi +b^2{\sin }^2\phi }\end{array}\right.$$

(19)

where x_m and y_m are the length and width of the enclosing rectangle, respectively.

4. The Matching Process of the Improved TERCOM-PSO-ICCP Algorithm

Figure 5 shows the implementation process of the improved TERCOM-PSO-ICCP.

The details are as follows:

(1)

Initial matching with TERCOM-A: Center the search range of TERCOM-A on the location indicated by the INS, ensuring that it encompasses the smallest rectangular region of the confidence ellipse to include the real location. Determine the center of gravity coordinates based on the INS-indicated track.

(2)

Grid traversal and initial rotation: Traverse each grid in the search area, using the center of gravity coordinates as the origin. Perform a rough rotation of the track as indicated by the INS, applying TERCOM translation matching based on the MSD criterion to obtain the initial rotation angle ${\alpha }_i$.

(3)

Refined search and rotation: Expand and refine the search area based on the initial rotation angle. Use TERCOM with the MSD criterion to determine the precise rotation angle ${\alpha }_j$ and matching sequence $p_m$ when the difference of two adjacent matching results meets the limit difference (the limit difference is 50 m).

(4)

Initial matching diagnosis with TERCOM-A: Verify the matching sequence $p_m$. If it satisfies the conditions of Equations (15) and (16), and Equation (17) shows only one minimum value in the entire search region, the matching sequence is confirmed. If not, return to Step 2 for further matching.

(5)

Second Matching with F-WAPSO: Center the search range on the position indicated by the initial matching result $p_m$ sequence. The smallest rectangular region containing the confidence ellipse is used as the search area. Initialize the PSO and ABC parameters randomly, setting the initial particle positions within the search region.

(6)

PSO Search: The fitness value of each particle is calculated using MSD as the criterion.

(7)

Updating pbest_ij and gbest_j: Compare the current fitness value of each particle with their individual best values pbest_ij. If the current fitness value is lower than pbest_ij, update pbest_ij to the current value. If not, pbest_ij remains unchanged. Similarly, compare each particle’s individual best value pbest_ij with the global best value gbest_j. If pbest_ij is lower than gbest_j, update gbest_j to pbest_ij. Otherwise, gbest_j remains the same.

(8)

ABC search: ABC conducts a search around the pbest_ij values, with employed and onlooker bees performing a greedy search. If an onlooker bee finds a fitness value lower than pbest_ij, it replaces pbest_ij with this new value.

(9)

Update pbest_ij and gbest_j: MSD is also selected as the judgment criterion in the ABC search. The sequence corresponding to the minimum MSD value is identified as the optimal matching sequence.

(10)

Check termination condition: Determine if the maximum number of iterations has been reached.

(11)

Output or continue: If the termination condition is reached, output the matching sequence and its corresponding location. If not, return to Step 6 for further iterations.

(12)

Initialize ICCP matching: Use the matching sequence $P_n$ as the initial iterative value, defining the contour line position and the search range.

(13)

Align the initial sequence $P_n$: Match the sequence $P_n$ as the initial alignment set, finding the nearest point on the contour line for each initial position point.

(14)

Refine with rigid transformation: Continuously apply rigid transformations to find the matching sequence with the minimum Mahalanobis distance.

(15)

Check the final termination condition: Determine if the Mahalanobis distance objective function is sufficiently small or reaches the maximum number of iterations.

(16)

Output the final sequence $P_o$: If the termination condition is satisfied, output the optimal matching sequence. If not, return to Step 13 for further refinement.

5. Simulation and Analysis

The improved TERCOM-PSO-ICCP demonstrates a significant capability to reduce matching errors through theoretical analysis. To assess the feasibility and accuracy of this algorithm, simulation tests were conducted for evaluation. To enhance the realism and reliability of the experimental results, all seabed depth data utilized in the tests were derived from actual measurements.

5.1. Simulation Condition

The underwater terrain simulation test was conducted within the geographic coordinates from 122.0568° E to 122.1263° E longitude and from 35.5704° N to 37.6078° N latitude in Weihai. During the test, the gyroscopic drift of the INS was measured at 0.01° per hour, and the accelerometer exhibited a constant bias of 0.01 mg. The underwater vehicle sampled data at 1 s intervals over a 30 s period, traveling at a speed of 8 kn with a speed error margin of 0.1 m/s. The sequence length for data matching was configured to 14, and the heading error was maintained at 2° [13,31].

For the F-WAPSO algorithm, the parameters were configured as follows: both the number of particles and colonies were set to 100, and the algorithm was allowed to run for a maximum of 100 iterations. The parameter c₁ increased with the number of iterations within (0, 2), while c₂ was fixed at 1.35. The inertia weight ω ranged from 0.4 to 0.9, decreasing progressively as the number of iterations increased [9,32].

To verify the performance of the improved TERCOM-PSO-ICCP, two tests were conducted, comparing the results with those of F-WAPSO, TERCOM-A, ICCP-a, and PSO-ICCP. Both ICCP-a and PSO-ICCP involve coarse matching using an enhanced PSO, followed by fine matching with the ICCP. In the ICCP-a, the improved PSO uses ABC to enhance PSO’s optimization capabilities. Conversely, in PSO-ICCP, the improved PSO utilizes a sliding window and quadtree method to refine PSO’s initialization strategy and accelerate convergence. Tests 1 and 2 were performed in different areas and with varying initial errors. The terrain parameters are detailed in

Table 1. Terrain parameters of the test areas.

Terrain Area	Terrain Elevation Standard Deviation	Terrain Elevation Entropy
Area-A	1.41	0.87
Area-B	1.15	1.82

. In the improved TERCOM-PSO-ICCP, as well as in the ICCP-a and PSO-ICCP, PSO was executed 20 times to obtain an average experimental value. Meanwhile, F-WAPSO was also run 30 times for averaging purposes. Additionally, the ICCP within the improved TERCOM-PSO-ICCP, ICCP-a, and PSO-ICCP algorithms underwent 10 iterations.

5.2. Simulation Result

In Test 1, the carrier began at coordinates of 122.07° E and 37.575° N. It traveled on a course of 10° north by east at a constant speed. The initial errors in the INS were 0.22′, 0.42′, and 0.62′ to the east and 0.22′, 0.42′, and 0.62′ to the north, respectively. Figure 6, Figure 7 and Figure 8 illustrate the trajectories of the five matching algorithms.

As shown in Figure 6, Figure 7 and Figure 8, the stability of all five algorithms decreased as the initial error increased. When the initial INS error was (0.22′, 0.22′), all five algorithms demonstrated a strong matching performance. However, upon closer inspection, the improved TERCOM-PSO-ICCP was closest to the actual track and exhibited the highest stability. With an initial INS error of (0.42′, 0.42′), Figure 7a reveals that the F-WAPSO algorithm’s matching performance significantly lagged behind the other four algorithms, highlighting the instability of the combined ABC and PSO algorithms. In Figure 7b, the improved TERCOM-PSO-ICCP outperformed the others. When the initial INS error was (0.62′, 0.62′), Figure 8a shows that the matching track of TERCOM-A deviated the most from the true track, resulting in the largest matching error, indicating TERCOM’s low accuracy under large errors. Additionally, the PSO-ICCP exhibited mismatches with the real track, further demonstrating PSO’s instability as an initial matching algorithm. However, the ICCP-a performed better than PSO-ICCP, indicating a stronger initial matching effect. Figure 8b shows that the improved TERCOM-PSO-ICCP was closest to the true track. To further evaluate the performance of the five algorithms, the statistical results of the five algorithms are shown in

Table 2. Terrain matching results of Test 1.

Matching Algorithm	Initial Position Error	Matching Time (s)	Matching Error (m)	Error Variance	Error of Longitude (m)	Error of Latitude (m)
TERCOM- PSO-ICCP	(0.22′, 0.22′)	15.9	30.8	14.4	35.6	28.2
	(0.42′, 0.42′)	19.3	42.9	25.6	49.3	33.5
	(0.62′, 0.62′)	27.9	60.3	30.1	55.9	80.6
ICCP-a	(0.22′, 0.22′)	8.1	40.2	15.9	43.7	39.3
	(0.42′, 0.42′)	9.7	58.9	26.6	75.2	57.2
	(0.62′, 0.62′)	14.8	76.4	39.3	99.1	58.9
PSO-ICCP	(0.22′, 0.22′)	7.9	42.8	17.6	47.9	36.8
	(0.42′, 0.42′)	9.2	60.7	29.5	70.7	38.8
	(0.62′, 0.62′)	13.5	99.9	40.7	110.8	81.3
TERCOM-A	(0.22′, 0.22′)	5.3	50.7	14.8	57.3	45.1
	(0.42′, 0.42′)	8.1	75.9	23.9	80.7	63.2
	(0.62′, 0.62′)	13.6	129.9	41.9	200.4	100.3
F-WAPSO	(0.22′, 0.22′)	4.1	53.5	19.2	60.6	39.8
	(0.42′, 0.42′)	7.9	82.6	34.9	98.7	76
	(0.62′, 0.62′)	12	97.2	42.1	123.6	79.7

. The matching error is expressed as the mean of the root-mean-square error in the longitude and latitude directions for the entire orbital sequence.

Table 2 highlights that, across three different scenarios, the improved TERCOM-PSO-ICCP consistently exhibited the smallest matching error. When the initial INS error was (0.22′, 0.22′), the improved TERCOM-PSO-ICCP reduced the matching error by 23.4%, 28%, 39.3%, and 42.4% compared with ICCP-a, PSO-ICCP, TERCOM-A, and F-WAPSO, respectively. With an initial INS error of (0.42′, 0.42′), the matching error reduction for the improved TERCOM-PSO-ICCP was 27.2%, 29.3%, 43.5%, and 48.1% relative to the ICCP-a, PSO-ICCP, TERCOM-A, and F-WAPSO algorithms. For an initial INS error of (0.62′, 0.62′), the algorithm achieved reductions of 21.1%, 39.6%, 53.6%, and 38.2% compared to the same respective algorithms. These data indicate that TERCOM-A had the lowest matching accuracy. In contrast, the ICCP-a and PSO-ICCP demonstrated better performances, suggesting that dual-phase matching is more accurate than single-phase matching. The improved TERCOM-PSO-ICCP delivered the highest matching accuracy, indicating that triple-phase matching surpasses dual-phase matching in precision. However, it is important to note that, with an increase in matching phases, the matching time also increased. Despite this, the improved TERCOM-PSO-ICCP had the smallest matching error variance, underscoring its superior stability.

Test 2 involved the carrier starting from the coordinates of 122.086° E, 37.577° N. The vehicle maintained an initial course of 150° north by east, traveling at a constant speed. The initial errors of the INS were 0.22′, 0.42′, and 0.62′ in the east direction and 0.22′, 0.42′, and 0.62′ in the north direction. The trajectories obtained from the five different matching algorithms are illustrated in Figure 9, Figure 10 and Figure 11.

As illustrated in Figure 9, Figure 10 and Figure 11, the stability of the five algorithms diminished as the error increased. When the initial INS error was (0.22′, 0.22′), Figure 9 shows that the improved TERCOM-PSO-ICCP remained closest to the actual track, followed by the ICCP-a and F-WAPSO, with PSO-ICCP being the farthest from the true track. With an initial INS error of (0.42′, 0.42′), Figure 10a indicates that the tracks of the improved TERCOM-PSO-ICCP and ICCP-a algorithms were nearer to the real track, while the other three algorithms exhibited significant deviations, highlighting their instability. Figure 10b further confirms that the improved TERCOM-PSO-ICCP was closest to the real track, whereas the ICCP-a track showed slight misalignment. When the initial INS error increased to (0.62′, 0.62′), Figure 11 demonstrates that the improved TERCOM-PSO-ICCP continued to be the closest to the true track, followed by the ICCP-a, with the remaining three algorithms showing greater deviations. The matching results are detailed in

Table 3. Terrain matching results of Test 2.

Matching Algorithm	Initial Position Error	Matching Time (s)	Matching Error (m)	Error Variance	Error of Longitude (m)	Error of Latitude (m)
TERCOM- PSO-ICCP	(0.22′, 0.22′)	20.1	41.8	16.9	50.3	33.7
	(0.42′, 0.42′)	30.9	72.9	29.1	69.3	85.1
	(0.62′, 0.62′)	43.3	90.2	37.7	105.9	70.6
ICCP-a	(0.22′, 0.22′)	10.3	49.6	17.9	36.9	57.2
	(0.42′, 0.42′)	15.2	88.1	31.8	75.2	87.6
	(0.62′, 0.62′)	22.8	109.4	41.3	109.1	71.8
PSO-ICCP	(0.22′, 0.22′)	9.9	89.3	20.1	77.9	105.2
	(0.42′, 0.42′)	14.9	105.8	37.2	139.7	89.1
	(0.62′, 0.62′)	20.1	115.4	43	130.6	82.5
TERCOM-A	(0.22′, 0.22′)	5.5	66.8	17.7	77.3	49.6
	(0.42′, 0.42′)	14.3	100.9	29.4	110.9	83.2
	(0.62′, 0.62′)	19.9	131.4	39.1	170.5	80.3
F-WAPSO	(0.22′, 0.22′)	4.5	58.9	25.9	70.3	45.5
	(0.42′, 0.42′)	13.1	93.7	37	118.7	86
	(0.62′, 0.62′)	15.7	127.5	45	188.6	99.7

.

Table 3 reveals that, across three different areas, the improved TERCOM-PSO-ICCP consistently achieved the smallest matching error. When the initial INS error was (0.22′, 0.22′), the matching error of the improved TERCOM-PSO-ICCP was reduced by 15.7%, 53.2%, 37.4%, and 29% compared with the ICCP-a, PSO-ICCP, TERCOM-A, and F-WAPSO algorithms, respectively. With an initial INS error of (0.42′, 0.42′), the improved TERCOM-PSO-ICCP showed matching error reductions of 17.3%, 31.1%, 27.8%, and 22.2% compared to the ICCP-a, PSO-ICCP, TERCOM-A, and F-WAPSO algorithms, respectively. For an initial INS error of (0.62′, 0.62′), the matching error reductions were 17.6%, 21.8%, 31.4%, and 29.3% compared with the ICCP-a, PSO-ICCP, TERCOM-A, and F-WAPSO algorithms, respectively. The improved TERCOM-PSO-ICCP exhibited the highest matching accuracy, demonstrating that three-stage matching outperformed two-stage matching, though it required more time. Moreover, the improved TERCOM-PSO-ICCP demonstrated the lowest variance in matching errors, highlighting its superior stability throughout the matching process.

The results indicate that the matching performance in Test 2 was not as strong as in Test 1. When the initial INS error was small, all five algorithms in Test 1 showed a better matching performance and were closer to the true track. In contrast, in Test 2, the five algorithms exhibited noticeable stratification, which became more pronounced as the initial error increased. This was primarily because the region traversed in Track 2 had a small topographic elevation standard deviation, gentle topographic changes, and many similar topographic areas, making the algorithms more susceptible to regional topographic variations. Despite these challenges, it is noteworthy that, across both sets of tests and three different initial position errors, the improved TERCOM-PSO-ICCP consistently produced matching tracks closest to the true track. It achieved the highest matching accuracy and the lowest standard deviation of matching error. Even with large errors, the improved TERCOM-PSO-ICCP demonstrated a strong performance, maintaining its stability and accuracy.

To further assess the performance of the algorithm, a weighted scoring method was employed to analyze the data from Test 1. Generally, a shorter matching time and smaller matching errors indicate a more effective terrain matching outcome. Consequently, it is essential to assign respective weights to the matching time and matching errors of each algorithm, followed by calculating the total score for each algorithm to evaluate their performance. Given that the magnitude of the matching error directly influences the reliability and accuracy of the algorithm, a slightly longer matching time is deemed acceptable. Therefore, the weight assigned to matching error is set at 80%, while the weight for matching time is set at 20%.

To facilitate a more effective data analysis, the matching time and matching errors presented in Table 2 were normalized using a minimum–maximum approach. The normalization formulas are provided in Equations (20) and (21), scaling the data to a range of (0, 1) to better reflect the overall performance of each algorithm. The comprehensive score is represented in Equation (22), where a lower score signifies a superior algorithm performance.

$$X^{\prime }={\displaystyle \frac{{X-X}_{min}}{X_{max}-X_{min}}}$$

(20)

where X′ represents the standardized matching error, $X$ represents the matching error, $X_{min}$ represents the minimum matching error, and $X_{max}$ represents the maximum matching error.

$$T^{\prime }={\displaystyle \frac{{T-T}_{min}}{T_{max}-T_{min}}}$$

(21)

where T′ represents the standardized matching time, $T$ represents the matching time, $T_{min}$ represents the minimum matching time, and $T_{max}$ represents the maximum matching time.

Composite score = α × standardized matching time + (1 − α) × standardized matching error

(22)

where α represents the ratio.

When the initial position errors of the INS were set at (0.22′, 0.22′), (0.42′, 0.42′), and (0.62′, 0.62′), the comprehensive scores of the five algorithms are presented in

Table 4. Comprehensive scores of the five algorithms when the initial position error of the INS is (0.22′, 0.22′).

Matching Algorithm	Standardized Matching Time	Standardized Matching Error	Comprehensive Score
TERCOM-PSO-ICCP	1.000	0.000	0.2 × 1 + 0.8 × 0 = 0.200
ICCP-a	0.339	0.414	0.2 × 0.339 + 0.8 × 0.414 ≈ 0.399
PSO-ICCP	0.322	0.529	0.2 × 0.322 + 0.8 × 0.529 ≈ 0.488
TERCOM-A	0.102	0.877	0.2 × 0.102 + 0.8 × 0.877 ≈ 0.722
F-WAPSO	0.000	1.000	0.2 × 0 + 0.8 × 1 = 0.8

,

Table 5. Comprehensive scores of the five algorithms when the initial position error of the INS is (0.42′, 0.42′).

Matching Algorithm	Standardized Matching Time	Standardized Matching Error	Comprehensive Score
TERCOM-PSO-ICCP	1.000	0.000	0.2 × 1 + 0.8 × 0 = 0.200
ICCP-a	0.158	0.403	0.2 × 0.158 + 0.8 × 0.403 ≈ 0.354
PSO-ICCP	0.114	0.448	0.2 × 0.114 + 0.8 × 0.448 ≈ 0.3812
TERCOM-A	0.018	0.831	0.2 × 0.018 + 0.8 × 0.831 ≈ 0.6684
F-WAPSO	0.000	1.000	0.2 × 0 + 0.8 × 1 = 0.800

and

Table 6. Comprehensive scores of the five algorithms when the initial position error of the INS is (0.62′, 0.62′).

Matching Algorithm	Standardized Matching Time	Standardized Matching Error	Comprehensive Score
TERCOM-PSO-ICCP	1.000	0.000	0.2 × 1 + 0.8 × 0 = 0.200
ICCP-a	0.176	0.231	0.2 × 0.176 + 0.8 × 0.231 ≈ 0.220
PSO-ICCP	0.094	0.569	0.2 × 0.094 + 0.8 × 0.569 ≈ 0.474
TERCOM-A	0.100	1.000	0.2 × 0.1 + 0.8 × 1 ≈ 0.820
F-WAPSO	0.000	0.53	0.2 × 0 + 0.8 × 0.53 ≈ 0.424

. In these tables, the scores corresponding to the optimal matching algorithms are highlighted in bold.

From Table 4, Table 5 and Table 6, it is evident that the comprehensive score of the improved TERCOM-PSO-ICCP was the lowest across the three different initial position errors, indicating that its matching performance surpassed that of the other four algorithms.

To further assess the algorithm’s robustness against noise, we evaluated the matching performance of all five algorithms under varying noise conditions. Each algorithm was subjected to different levels of white noise, with the initial position error set at (0.22′, 0.22′), as in Test 1. The added white noise followed a normal distribution, specifically N (0, 1), N (0, 3), and N (0, 6) respectively. The matching results are presented in

Table 7. Matching errors under varying noises.

Matching Algorithm	Matching Error (m)
	N (0, 1)	N (0, 3)	N (0, 6)
TERCOM-PSO-ICCP	30.8	43.2	60.9
ICCP-a	40.2	59.1	78.5
PSO-ICCP	42.8	74.5	105.7
TERCOM-A	50.7	83.7	110.2
F-WAPSO	53.5	85.1	114.6

.

From Table 7, it can be observed that the matching errors of the five algorithms increased with the level of noise. However, the improved TERCOM-PSO-ICCP exhibited the slowest rate of change, demonstrating that increasing the Mahalanobis distance effectively mitigated the impact of noise on the ICCP. This algorithm achieved the highest matching accuracy under all three noise conditions.

In summary, the improved TERCOM-PSO-ICCP demonstrated a robust matching capability, maintaining a good performance even with significant initial position errors. The feasibility of enhancing the matching process of TERCOM-PSO-ICCP was validated through two tests. Initially, stable alignment was achieved using TERCOM-A, followed by employing a similar extremum detection technique to evaluate the alignment results, thereby improving the matching hit rate. Subsequently, based on the initial matching outcomes, F-WAPSO was utilized for rapid matching, which helped to reduce the initial position error. Finally, the improved ICCP, which incorporates the Mahalanobis distance in place of the Euclidean distance, was employed for fine-tuning under small errors, further enhancing the matching accuracy.

However, this three-step matching process resulted in an increased matching time alongside an improved accuracy. Analysis using the weighted scoring method indicated that the improved TERCOM-PSO-ICCP provided the best matching performance, particularly when the proportion of matching time was low. If a shorter matching duration is required in practical applications, it is advisable to select an appropriate algorithm based on specific circumstances. Importantly, the improved TERCOM-PSO-ICCP also exhibited the smallest matching error, making it the optimal choice if matching error is the sole consideration in real-world scenarios. Additionally, under varying noise conditions, the improved TERCOM-PSO-ICCP consistently achieved the highest matching accuracy. Therefore, the integration and enhancement of TERCOM, PSO, and the ICCP can significantly improve the matching accuracy in TAN.

6. Conclusions

To enhance the matching effect of underwater TAN, the improved TERCOM-PSO-ICCP algorithm is introduced. This algorithm enhances the TERCOM, PSO, and ICCP algorithms individually. The integration of these three algorithms maximizes their individual strengths and enhances the overall matching efficiency. Additionally, the optimality of the combined algorithm is bolstered through the evaluation of initial matching results and the optimization of the search area. The improved TERCOM-PSO-ICCP effectively corrects INS positions. Notably, even with a substantial initial position error of (0.62′, 0.62′), the matching error is reduced to 60.3 m, achieving reductions of 21.1%, 39.6%, 53.6%, and 38.2% compared with the ICCP-a, PSO-ICCP, TERCOM-A, and F-WAPSO algorithms, respectively. This significant reduction effectively minimizes the accumulated errors of the INS, thereby satisfying the high-precision positioning requirements for long-term AUV operations. However, the implementation of the three-step matching process results in a decrease in matching speed. Consequently, future research will focus on strategies to reduce the matching time while maintaining accuracy.