Study on the Arctic Underwater Terrain-Aided Navigation Based on Fuzzy-Particle Filter

The ultra-low resolution underwater terrain maps of the Arctic region reduce the localization and navigation accuracy of the underwater vehicle relying on terrain-aided navigation. In this paper, we study the navigation ability of Autonomous Underwater Vehicles (AUVs) under the ultralow-resolution terrain map. Firstly, the low-resolution map is transformed into a continuous map by bilinear interpolation. Then, a Terrain-Aided Navigation (TAN) system based on the Particle Filter (PF) is constructed to estimate the state of AUV position by particles. Particles of a random distribution of fixed variance can effectively track targets. However, a fixed variance distribution is not well adapted to many different situations. To improve navigation accuracy and robustness, fuzzy logic is used to estimate the distribution variance of particles under the current terrain gradient dynamically. The simulation results show that our proposed Fuzzy-PF TAN system is robust under various current disturbance situations. The position error of our system is within a map resolution unit of 500 m.


Introduction
The Arctic region is rich in underwater minerals, diverse organisms, and shipping resources with great development potential. The collection and analysis of underwater information is conducive to the development of Arctic resources. AUV has been widely used in the detection and collection of underwater information. Localization and navigation ability is important to the working efficiency and recovery success rate of AUV. At present, the common localization and navigation methods of AUV are a combined GPS/INS localization and acoustic method [1]. However, the Arctic is covered with ice and snow all yeararound, which makes GPS unavailable. Using the inertial sensor alone will cause the error accumulation without boundary, and the magnetic field in the polar region changes strongly, which will affect the accuracy of the electronic compass and further increase the accumulated error [2].
In the environment with ice, using natural environment information on navigation will reduce the bad impact on polar harsh environment. There are some research results of the navigation of gravity [3], current [4], magnetic [5], and terrain navigation [6]. The gravity sensor normally has a large scale and is not suitable for underwater applications. The accuracy of current navigation and magnetic field navigation is low. Terrain-aided navigation only uses the range sensor to obtain the altitude data, which can achieve a better localization accuracy. It has been widely used in aircraft [7,8], space landers, and underwater vehicles [9] because it does not need the aid of external sensors. TAN is suitable for AUV to navigate underwater for a long time.
TAN uses collected terrain data to compare with the prior map and to determine the current position through & Yanji Liu liuyanji@shmtu.edu.cn calculation. In TAN, Kalman Filter (KF) [10], Point Mass Filter (PMF) [11], and PF [2] are widely studied. Kalman Filter is mainly used to deal with the linear system and the observed variance conforms to the Gaussian distribution. It has the advantage of fast computing. Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) algorithm uses the pseudo-linear mechanism to solve the nonlinear system, but it is still required that the observations conform to the Gaussian distribution. Point Mass Filter can solve the non-linear non-Gaussian problem by using the mesh to approximate the posterior non-Gaussian distribution. In [11,13], TAN is constructed based on PMF, and good results are obtained. However, the calculation of every divided cell in the posterior distribution will consume large energy, and there is no effective mechanism to inhibit divergence. Different from PMF, PF uses the particles of random distribution to approximate the posterior distribution. It does not need to mesh with the whole posterior region. PF has advantages in calculating energy consumption, and the classical PF has a resampling mechanism to suppress divergence. Recently, there are many successful research works of using PF-TAN system in underwater vehicles. Papers [2,14,15] report the practical application of PF-TAN, which shows that the PF can achieve high precision under the 10 m and 50 m resolution maps. By some methods, PF can achieve good robustness [16]. However, the newly published terrain map resolution in the Arctic region is only 500 m [17]. The low-resolution map will increase the uncertainty of observation. In particle filter, the search range of particles is determined by the process noise or variance, and the search range of particles affects the accuracy of navigation and positioning. If the scope of particle search is large, the accuracy of localization becomes poor. If the scope of particle search is small, it is easy to fall into local optimum under the condition of ultralow-resolution map, and losing the tracking ability. In practice, the seafloor terrain has both rough areas with rich terrain features and flat areas with weak features, and in the rough areas, a smaller search area should be set to achieve high-precision localization, while in the flat areas, a larger search area should be set to avoid falling into a local optimum position.
In addition, AUVs often have limited spatial movement direction and are usually under actuated, making the motion control and path tracking more difficult [18,19]. If the AUV falls into the wrong localization and cannot adjust and jump out in time, it will lead to a major failure [20]. Therefore, it is necessary to improve the localization and navigation accuracy and reduce the probability of AUV entering into the wrong localization area.
Fuzzy logic simulates human operation experience. It sets suitable parameters according to different situations to achieve better control results. Fuzzy logic has been widely used solve the parameter optimization problem of the particle filter algorithm. In [21], fuzzy logic is used to dynamically adjust the process noise and particle numbers, effectively filters out the disturbance in different environments, and improves the accuracy of target recognition. In [22], fuzzy logic is used to infer the posterior probability distribution function of Bayesian filtering, improving the sampling accuracy. In this study, the dynamic adjustment method of process noise of particle filter based on fuzzy logic is proposed.

Terrain-Aided Navigation
The essence of TAN is to fuse the dead reckon results and observation estimation. The motion model, observation, and map all affect the fusion results. In PF, process noise determines the range of random particle distribution and indirectly affects the convergence and accuracy of filter. Map accuracy and observation noise affect the distribution of particle weight.

Motion Model
This paper focuses on studying of localization ability of AUV in a horizontal plane, and a simplified 3-Dof motion model is constructed. Define ½v; w as the translational velocity and angular rate of AUV with respective to the body coordinate system, ½x; y; h as the pose of AUV in the global coordinate system, and h is heading angle. AUV motion model schematic is shown in Fig. 1. When there is an angular rate, the AUV rotates around the point O by a radius v=w j j, which is defined as a positive counterclockwise rotation. The motion model is shown in Eq. (1). The model is convenient for noise analysis.

Process Noise
The motion of AUV is mainly affected by two factors: one is the disturbance of its velocity and angular rate; the other is the disturbance of external environment such as sea current. Therefore, the process noise is divided into body noise and environmental disturbance noise, as shown in Eqs. (2and 3), where the superscript^represents the corresponding value with noise. X In Eq. (2), N represents the normal distribution, and x is the variance of x direction, and r 2 y is the variance of y direction.

Terrain Map
The area of the Arctic is mostly flat and the small part is steep. To verify the navigation ability under different types of terrain, the map with longitude range [-162.5°, 160.19°] and latitude range [75.27°, 76.3°] is selected as a study area. The map scale is [140, 60] km. The region contains both steep areas and flat areas, as shown in Fig. 2.
The resolution of the terrain map is 500 m, but the normal cruising speed of AUV is not more than 1 m/s. The direct application of a low-resolution map may lead to a large navigation error. Therefore, in this paper, bilinear interpolation is used to obtain the altitude information of the map cells. Equation (4) is the interpolation algorithm.
where f Q ij À Á ; i; j ¼ 1; 2 ð Þ is the altitude value of four points around the AUV, and x i ; y j À Á ; i; j ¼ 1; 2 ð Þis point coordination around the AUV. The relative position relationship of each point is shown in Fig. 3.  3 Fuzzy-Particle Filter of TAN TAN filtering system mainly includes a fuzzy logic module, particle filter module, and gradient analysis module.
According to the input altitude information, the particle filter matches with the seabed terrain map and get the position with the highest matching value. The fuzzy logic module calculates the reasonable process noise value according to the gradient and the gradient change rate information calculated by the gradient analysis module, and then transmits the process noise to the particle filter module.
The block diagram of TAN filtering system is shown in Fig. 4.

Particle Filter
The Sequential Importance Resampling (SIR) is used to fusion data. The SIR Particle Filter algorithm is shown in Fig. 5, in which P m is the process variance in two directions of x and y, usually it is a fixed value. In this study, P m will be dynamically adjusted by fuzzy logic.
In the Particle Filter, the systematic resampling technique is used in the resampling process because of the great performance. Gaussian likelihood estimation is used for calculating particles weight p z k x i k À Á .

Fuzzy Logic
The fuzzy logic is designed to infer the process noise value of the present state. Terrain gradient and gradient rate is set as the input, process noise as the output. According to the operation experience, the following basic logic exists.
IF the current terrain gradient is large, THEN set small process noise. IF the current terrain gradient is small, THEN set large process noise.
The altitude changes in real time. When the gradient value is large and gradient rate tends to slow down, the process noise should be increased in time; otherwise, it is easy to cause tracking loss. When the gradient is small and gradient rate tends to change greatly, the process noise should be reduced in time to increase the tracking accuracy.
According to the above basic logic, we use the input subset with a 7-degree division S in ¼ NB; NM; NS; ZO; f PS; PM; PBg, corresponding to the change from negative to positive of gradient and gradient rate, and use output subset with 7-degree division S out ¼ SS; S; SM; M; ML; L; LL f g , corresponding to the change from small to large of process noise. The triangle membership function is used for input and output, as shown in Fig. 6, and the rule table is shown in Table 1.

Parameter Setup
To analyze the influence of filter parameters on navigation performance, different process variances are selected. For process variance, only the variance caused by the external environment is changed, while the variance coefficient a 1 ; a 2 ; a 3 ; a 4 ð Þ remains unchanged. The values a 1 ; a 2 ; a 3 ; a 4 ð Þare 0:1; 0:1; 0:001; 0:1 ð Þ . The selection of parameters takes full account of the actual situation, that is, for the velocity term, the disturbance generated by the velocity component is large, while for the angular rate term, the disturbance generated by the speed and the angular rate is large, but because the value of the velocity is relatively large, the coefficient cannot be too large. According to experience, the process noise generated by the body should be fall within 50% of the true value.
For the observation noise, AUV is set to sail at a fixed depth, ignoring the noise difference caused by water depth difference, and a fixed standard deviation value of 5 m is used. The reason is that in the actual scene, the general altimeter can only detect the range of about 200 m.
For fuzzy logic, according to the gradient of arctic terrain, the domain of gradient is [-25,25], and the domain of gradient rate is [-25,25]. The output process noise range is [1,7]. See Table 2 for specific parameter settings. The direction of ocean current disturbance is in positive of X and Y axis.    Table 2, the highlight black parameter is the default simulation parameter.
We use a dual core 2.6 GHz CPU for the calculation. The average simulation setting time is 30000 s, and the filter operates once per second. Each calculation contains 1000 particle iterations. Under the MATLAB platform, the average running time is 23 s, that is the average running time of each step is 7.6 9 10 -4 s, the calculation time is very short, and it has a better real-time performance. Figure 7 shows the filter process noise versus the variance of the AUV localization in the absence of external current disturbance, when body noise is applied on AUV alone. The black line in the figure is the simulation result in the rough area and the red line is the simulation result in the flat area. It can be seen from the figure that as the process noise of the filter increases, there is an overall trend of increasing localization variance. One possible reason is that the process noise affects the search range of the particles, and a large particle distribution reduces the localization accuracy. It can also be seen from the figure that the overall variance in the flat area is greater than the variance in the uneven region because the measurements in the flat region are low differentiation to allow accurate regression of the filtering process. We can see that variance has a significant impact on localization accuracy. Figures 8, 9 show the navigation results under different process noise settings with the 0.1 m/s current disturbance. We can see that there is no linear relationship between the navigation accuracy and the standard deviation of process noise. When the standard deviation is 1, the particle search range is small. Thus, the correct area cannot be sampling  When the standard deviation of process noise is 2 or 3, the PF shows good tracking ability, as the noise matches the gradient of the map. When the standard deviation is as larger as 4, 5, or 6, the tracking error will be larger because the correct area cannot be searched intensively. Fuzzy-PF logic can generate reasonable process noise dynamically and show good tracking results. Figures 10, 11 show the navigation results under different process noise settings with the 0.15 m/s current disturbance. We can see that the process noise settings of 2 and 3 which show good tracking results under the 0.1 m/s current disturbance become worse under the current disturbance of 0.15 m/s. The maximum tracking error exceeds 1000 m and has exceeded 2 map units. When the process noise is 4, a better tracking process occurs. However, when reaching the top of the steep area, the top area is suddenly flat, and the gradient rate is small, as shown in Figs. 2b and 12, resulting in tracking failure. The path generated by Fuzzy-PF logic also shows good tracking results. We represent the characteristics of the terrain as the inner product of the gradient and gradient rate. The process noise is negatively related to the inner product of gradient and gradient rate, as shown in Fig. 13. When the inner product is large, the process noise is small, and the opposite is large according to the fuzzy logic. Figure 14 depicts the relationship between the localization variance under different filter noise as well as current interference. It can be seen from the figure that under the same filter noise condition, the effect of 0.1 m/s current interference on the localization variance is greater  Under the condition of 0.1 m/s current disturbance, when the noise is 2, the result is greater than that produced by fuzzy logic because the condition is already a more optimal result by noise 2. It can be seen that using fuzzy logic produces good results most of the time, but not all cases are better than the results of a fixed noise setting. Figure 15 shows the tracking results of Fuzzy-PF logic and common PF in flat terrain area. The simulation starts at [60,000,10,000] m position and moves along the x-axis under the current disturbance of 0.1 m/s. It can be seen that the tracking results of the two methods both are very poor, with the tracking error close to the error of dead reckoning estimation. Due to a lack of terrain features, the weight of each particle has no significant difference between each other. Consequently, the system lost the ability to follow the path.

Conclusion
In this paper, based on the classical SIR particle filter method, a TAN system is built, and the tracking performance of the particle filter under the low-resolution map is studied. The process noise is categorized into body noise and external environment noise. The external factors such as current disturbance are included in the external environment disturbance, which is convenient to study the influence of key disturbance factors such as current. For the map with a resolution of 500 m, the method of bilinear interpolation is used to obtain the middle altitude value.
The application of fuzzy logic to generate appropriate process noise parameters makes the search process of particle filter more reasonable, thus improving the navigation accuracy of TAN. The simulation results show that TAN based on Fuzzy-PF logic not only has a good tracking accuracy, but also has great robustness under different ocean current disturbance. However, in the flat area, a lack of gradient information of terrain results in bad localization that is close to the dead reckon navigation. The simulation results provide reference for the practical application of the Fuzzy-PF TAN system.