AUV Path Planning Algorithm for Terrain Aided Navigation

: Terrain aided navigation (TAN) technology can yield accurate navigation results in long-endurance underwater operation of autonomous underwater vehicles (AUVs). Most TAN research focus on the high-precision terrain matching algorithms. However, terrain matching algorithms estimate the position of the vehicle by observing a unique combination of features in the terrain, making it difﬁcult to locate a vehicle with ﬂat terrain. This paper presents a path planning method for AUV seabed TAN process to decrease positioning errors by avoiding ﬂat areas. In this paper, a TAN path planning algorithm based on sector search is proposed. Parameters of the sector search algorithm are determined through theoretical analysis, which means the generated path can stably provide accurate TAN location results. Meanwhile, a control coefﬁcient is proposed to maximize the terrain information contained in the sector searching area, bring out a high success rate of AUV positioning. The TAN performance of the paths generated using the proposed path planning method was veriﬁed in simulations, which show that the TAN can yield accurate positioning results stably on the path planned.


Introduction
Recently, autonomous underwater vehicles (AUVs) have become popular for mapping seabed terrain, surveying marine pastures, and inspecting underwater oil pipelines.These devices are essential for maintaining and monitoring our seas to protect our most precious natural resources.The navigational precision of an AUV directly affects its ability to complete a task; therefore, the navigational precision of AUVs is especially crucial for longdistance missions [1].The navigation error of AUV has a great influence on the completion of tasks listed below, especially in long-endurance tasks [1].Several other tasks such as GPS signals are attenuated drastically in water which makes it nearly impossible for AUVs to receive GPS signals while working underwater.However, long-baseline (LBL) and ultrashort-baseline (USBL) acoustic positioning systems require the assistance of preset seabed acoustic arrays or the mother ship, which greatly limits the operating range of AUVs [2].Inertial navigation system (INS) could provide precise location results with assistant of a doppler velocity log (DVL).However, INS is essentially a mathematical integration process, small velocity/acceleration measurement errors accumulate over time, which eventually leads to a huge navigation error after the INS operates for a period.This navigation error is continuously divergent over time, making it difficult to locate a vehicle accurately in a long-term and long-distance underwater operation [3].Terrain-aided navigation (TAN) systems can find areas with a similar terrain to the current location of AUV in large-scale seabed terrain map and yield an accurate location result for the vehicle with bounded errors.Thus, as a navigation system with a bounded navigation error, the TAN can solve continuous deviation of INS.
TAN holds high potential for AUV long-term navigation without supplements of other precision localization method.Tain et al. introduced The Fox's adaptive PF based on Kullback-Leibler distance (KLD) for TAN and improve the efficiency [4].Ling et al. presented a tightly-coupled navigation to successfully estimate the critical sensor errors by incorporating raw sensor data directly into an augmented navigation system [5].Georgios et al. proposed a low-complexity particle filter-based TAN algorithm for autosub long range, a long-endurance deep-rated AUV [6].Pengyun et al. proposed an underwater terrain positioning method (UTPM) using maximum a posteriori (MAP) estimation and a pulse coupled neural network (PCNN) model for highly accurate navigation by AUVs [7].Ye et al. presented an autonomous underwater vehicle optimal path planning method for seabed terrain matching navigation to avoid low precision areas [8].
Georgios et al. used TAN system to reduce the errors caused by INS [4].Brian et al. improved the jitter bootstrap algorithm and proposed a terrain aided navigation method for underwater gliders [9].The proposed method was tested in an actual sea trial experiment to demonstrate its positioning performance.Teng et al. optimized the particle initialization method and proposed an AUV localization and path planning algorithm, which reduced the localization error in terrain aided navigation [10].Camille et al. improved the weighted set Kalman filter (WEnKF), which ensured that the asymptotic variance of weights remained bounded and enhanced the robustness of terrain aided navigation [11].Khalilabadi M. R. et al. proposed a method to realize underwater vehicle navigation by measuring gravity and terrain, which improved positioning accuracy and realized effective navigation probability [12].
In TAN, location errors are determined by the topographic complexity of matching areas.TAN methods are likely to fail catastrophically in completely flat terrain, so a path planning algorithm is essential in TAN to avoid flat areas.Although some researchers focus on the robustness of TAN systems in flat areas, for example, Teixeira et al. and Meduna et al. designed a method to estimate the location of AUV in poorly informative terrain [13,14], TAN works well when it can find a unique area similar to the terrain currently observed by AUV.Thus, with a completely flat seabed (which means the observation terrain of AUV is flat), AUV can be at every position in this area, resulting in the failure of TAN.This is the purpose of our path planning, that is, to make the AUV avoid the flat area as much as possible [15].Therefore, choosing a path that can guarantee TAN accuracy is a necessary condition for a long voyage detection mission [16].Serin et al. designed a threedimensional path planning method based on the traveling salesman problem, and proved its practicability in real terrain and road network datasets [17].However, the real path is constantly changing.To solve the issue of real-time path shifts, Fairbairn et al. proposed a terrain representation method using terrain entropy and applied it to real terrain data, but the diversity of terrain features was ignored [18].
Compared to previously reported AUV systems, in this article, we propose a new path planning strategy for increasing accuracy of AUV for TAN.As shown in Figure 1, the method is proposed to find out the suitable area of TAN in the sector with the current position as the center of the circle and go to this area, and repeat until the target point appears in the current search sector of the AUV.At that time, the method will search the TAN suitable area in current sector as the target aided point, and the AUV will first arrive on this point for accurate positioning, and then go to the target.Based on this method, this paper analyzes, in detail, the half of a sector angle to be scanned by AUV, the selection of the limit line and the limit angle at the target point, as well as the selection of the suitable area, named as the target aided point, in front of the target point.The value range of sector half-open angle and limit angle is also calculated, which provides a theoretical basis for AUV path planning.In this paper, the actual seafloor terrain is used to verify the performance of the proposed path planning method.

Searching Strategy for TAN-Suitable Area
Starting from the start point, the vehicle needs to go through a sequence of TAN suitable areas to locate itself with a matching algorithm and reduce the INS accumulated error.
These TAN-suitable areas are selected according to the following rules: (1) These areas meet the need of the matching algorithm, terrain standard deviation (TSD) > 0.08702 [19].(2) The vehicle can reach the target area or point after passing these areas.
To ensure that the vehicle can reach the target point successfully, a new sector scan method is proposed to analyze the search of the TAN-suitable area.The new method can also ensure a TAN-suitable area exists near the line between the AUV position and the target area.The flow of this algorithm is shown in Figure 2. In the process from the star to the end of the mission, AUV first needs to select the sector searching area, including the calculation both terrain suitability for TAN (described in Section 2.1) and the sector angle Section 2.2), analysis of the limit line (Section 2.3) and the selection of the TAN-suitable area (the target aided point, a special TAN-suitable area, has been described in Section 2.4).When AUV reaches the TAN area, the TAN system is used to know whether i reaches the target point.If not, repeat this method until it reaches the target point.

Searching Strategy for TAN-Suitable Area
Starting from the start point, the vehicle needs to go through a sequence of TANsuitable areas to locate itself with a matching algorithm and reduce the INS accumulated error.
These TAN-suitable areas are selected according to the following rules: (1) These areas meet the need of the matching algorithm, terrain standard deviation (TSD) > 0.08702 [19].(2) The vehicle can reach the target area or point after passing these areas.
To ensure that the vehicle can reach the target point successfully, a new sector scan method is proposed to analyze the search of the TAN-suitable area.The new method can also ensure a TAN-suitable area exists near the line between the AUV position and the target area.The flow of this algorithm is shown in Figure 2. In the process from the start to the end of the mission, AUV first needs to select the sector searching area, including the calculation both terrain suitability for TAN (described in Section 2.1) and the sector angle Section 2.2), analysis of the limit line (Section 2.3) and the selection of the TAN-suitable area (the target aided point, a special TAN-suitable area, has been described in Section 2.4).When AUV reaches the TAN area, the TAN system is used to know whether it reaches the target point.If not, repeat this method until it reaches the target point.

Sector Searching Method for the TAN-Suitable Area
The first step of the proposed method is to calculate the TAN suitability for each block over the priori map.A TAN-suitable area is defined as a fixed size area with sufficient topographic features to enable the accurately positioning of TAN.The terrain elevation entropy is a commonly used index to evaluate the complexity of topographic relief, which means a TAN-suitable area should have a terrain elevation entropy, HM c , bigger than a given threshold.According to Ref. [6], the terrain elevation entropy, HM c , for an area with a size of M × N can be given by: where h(i, j) denotes the terrain depth at (i, j), and P(i, j) is a process value, make HM c unaffected by the mean value of terrain depth h(i, j).The threshold of HM c is the median value of terrain elevation entropy calculated in each block over a priori map.

Sector Searching Method for the TAN-Suitable Area
The first step of the proposed method is to calculate the TAN suitability block over the priori map.A TAN-suitable area is defined as a fixed size area w cient topographic features to enable the accurately positioning of TAN.The terra tion entropy is a commonly used index to evaluate the complexity of topograp which means a TAN-suitable area should have a terrain elevation entropy, HM than a given threshold.According to Ref. [6], the terrain elevation entropy, HM area with a size of M N × can be given by: , , , After determining the selection criterion of the TAN-suitable area, a sector searching method is proposed.As shown in Figure 3, the sector has a circle center of the current position of the vehicle (the starting point in Figure 3 at the initial time), and it is axisymmetric to the line between the current position of the AUV and the target point.
position of the vehicle (the starting point in Figure 3 at the initial time), and it is axisymmetric to the line between the current position of the AUV and the target point.The flow of the TAN path planning algorithm is as follows: The method starts running at the starting point and draws the first sector, to search a TAN-suitable area in this sector.The central point of the TAN-suitable area is taken as the next-goal of the vehicle.When the positioning result given by INS is close to the target point (the distance between the goal and the vehicle's position yield by INS is less than M/2), the vehicle will scan the terrain using a multi-beam echo sounder (MBES) and generate a gridded bathymetric map with a size of N N × .Then TAN system can yield an estimate of vehicle's position by fusing the INS data and the terrain matching results.Taking the current position of the AUV output from the tan system as the center of the circle, a new sector can be drawn.As shown in Figure 4, these processes continue until the vehicle reaches the final target area or target point.For convenience, the first TAN-suitable area arrived at by the vehicle is TAN-suitable area 1, the next TAN-suitable area arrived at is defined as the TAN-suitable area 2, until the end, as shown in Figure 4.In the proposed method, the central angle, the limit line at the target point and the selection of the target-aided point directly restricts the practicability of the search method.The flow of the TAN path planning algorithm is as follows: The method starts running at the starting point and draws the first sector, to search a TAN-suitable area in this sector.The central point of the TAN-suitable area is taken as the next-goal of the vehicle.When the positioning result given by INS is close to the target point (the distance between the goal and the vehicle's position yield by INS is less than M/2), the vehicle will scan the terrain using a multi-beam echo sounder (MBES) and generate a gridded bathymetric map with a size of N × N. Then TAN system can yield an estimate of vehicle's position by fusing the INS data and the terrain matching results.Taking the current position of the AUV output from the tan system as the center of the circle, a new sector can be drawn.As shown in Figure 4, these processes continue until the vehicle reaches the final target area or target point.For convenience, the first TAN-suitable area arrived at by the vehicle is TAN-suitable area 1, the next TAN-suitable area arrived at is defined as the TAN-suitable area 2, until the end, as shown in Figure 4.The flow of the TAN path planning algorithm is as follows: The method starts running at the starting point and draws the first sector, to search a TAN-suitable area in this sector.The central point of the TAN-suitable area is taken as the next-goal of the vehicle.When the positioning result given by INS is close to the target point (the distance between the goal and the vehicle's position yield by INS is less than M/2), the vehicle will scan the terrain using a multi-beam echo sounder (MBES) and generate a gridded bathymetric map with a size of N N × .Then TAN system can yield an estimate of vehicle's position by fusing the INS data and the terrain matching results.Taking the current position of the AUV output from the tan system as the center of the circle, a new sector can be drawn.As shown in Figure 4, these processes continue until the vehicle reaches the final target area or target point.For convenience, the first TAN-suitable area arrived at by the vehicle is TAN-suitable area 1, the next TAN-suitable area arrived at is defined as the TAN-suitable area 2, until the end, as shown in Figure 4.In the proposed method, the central angle, the limit line at the target point and the selection of the target-aided point directly restricts the practicability of the search method.In the proposed method, the central angle, the limit line at the target point and the selection of the target-aided point directly restricts the practicability of the search method.Therefore, these parameters will be discussed in Sections 2.2-2.4,respectively.

Central Angle of the Sector
With the given TAN suitability calculation equations, TAN suitable areas are chosen in a sector.When the vehicle searches for a TAN-suitable area through the sector searching method, the central angle of the sector has a direct impact on the size of the search area.and the unreasonable sector angle may make the vehicle fail to reach the target area due to a large amount of navigation errors accumulated in the long-distance navigation.
By analyzing the boundary condition of the sector, the calculation of the central angle in the sector searching method is discussed.The parameters within the sector are denoted in Figure 5. Therefore, these parameters will be discussed in Sections 2.2, Section 2.3, and Section 2.4, respectively.

Central Angle of the Sector
With the given TAN suitability calculation equations, TAN suitable areas are chosen in a sector.When the vehicle searches for a TAN-suitable area through the sector searching method, the central angle of the sector has a direct impact on the size of the search area.and the unreasonable sector angle may make the vehicle fail to reach the target area due to a large amount of navigation errors accumulated in the long-distance navigation.
By analyzing the boundary condition of the sector, the calculation of the central angle in the sector searching method is discussed.The parameters within the sector are denoted in Figure 5.In Figure 5, α is the 1/2 of the central angle, L R is the radius of the sector, and L r is the minimum distance required by the vehicle to meet the rotary motion, while the TAN-suitable area is demonstrated by a square area with a size of N × N. When only one N × N block can be included in the sector area, the minimum value α min of the half-sector angle can be obtained, as shown in Figure 6.In addition, calculation of α min is shown in Equations ( 2) and (3).Therefore, these parameters will be discussed in Sections 2.2, Section 2.3, and Section 2.4, respectively.

Central Angle of the Sector
With the given TAN suitability calculation equations, TAN suitable areas are chosen in a sector.When the vehicle searches for a TAN-suitable area through the sector searching method, the central angle of the sector has a direct impact on the size of the search area.and the unreasonable sector angle may make the vehicle fail to reach the target area due to a large amount of navigation errors accumulated in the long-distance navigation.
By analyzing the boundary condition of the sector, the calculation of the central angle in the sector searching method is discussed.The parameters within the sector are denoted in Figure 5.In Equation ( 3), x and y describe the vehicle's horizontal position in eastern and northern directions.It can be seen that the minimum central angle of the sector is determined by the size of the TAN-suitable area and the radius of the sector.However, when the central angle is set to the minimum value, only one N × N area can be in the searching area, making it difficult to find a N × N size TAN-suitable area in the sector.
Assume that the sector radius L R can be denoted as: combined with Equation (4), Equation (3) can be rewritten as: From Equation ( 5), α min decreases as the coefficient k increases.Therefore, selecting an appropriate coefficient k is helpful to expand the area of the sector.
Here, we denote the there is a minimum value α min and a maximum value α max at the 1/2 central angle of the sector.To enable the AUV to effectively move towards the target area, α max should be limited by a maximum value range.Otherwise, in the case of the TAN-suitable area is at edge, as shown in Figure 7a, the vehicle will have a large movement in the vertical direction of the line between the starting point and the target point, which will make the vehicle waste more energy and is not conducive to the large-scale transfer and movement of the vehicle.As shown in Figure 7b, the movement distance of the vehicle perpendicular to the line direction of the starting point and the target point is smaller than that of the line direction, which ensures that the vehicle can efficiently transfer and move to the target point.
In Equation ( 3), x and y describe the vehicle's horizontal position in eastern northern directions.It can be seen that the minimum central angle of the sector is d mined by the size of the TAN-suitable area and the radius of the sector.However, w the central angle is set to the minimum value, only one N N × area can be in the se ing area, making it difficult to find a N N × size TAN-suitable area in the sector.
Assume that the sector radius R L can be denoted as: combined with Equation (4), Equation (3) can be rewritten as: ( ) 5), min α decreases as the coefficient k increases.Therefore, sele an appropriate coefficient k is helpful to expand the area of the sector.
Here, we denote the there is a minimum value m in α and a maximum value α the 1/2 central angle of the sector.To enable the AUV to effectively move toward target area, max α should be limited by a maximum value range.Otherwise, in the ca the TAN-suitable area is at edge, as shown in Figure 7a, the vehicle will have a large m ment in the vertical direction of the line between the starting point and the target p which will make the vehicle waste more energy and is not conducive to the largetransfer and movement of the vehicle.As shown in Figure 7b, the movement distan the vehicle perpendicular to the line direction of the starting point and the target po smaller than that of the line direction, which ensures that the vehicle can efficiently t fer and move to the target point.
According to the law of cosines where the value of k needs to satisfy Then Equation ( 8) can be solved with  As shown in Figure 8, d x = d y and ∠ASC = 45 • , then According to the law of cosines if L R = k × N and α max = γ + 45 • , α max can be calculated as where the value of k needs to satisfy Then Equation ( 8) can be solved with 45 • < α max < 80.7 • .In addition, according to Equation ( 8), when k → +∞ , Equation (10) indicates that, when k → +∞ , the limit value of α max is 45 • , which is also the minimum value of α max .Therefore, in practical application, the 1/2 central angle α of the sector should not exceed 45 • .Combining with Equations ( 4) and ( 5), the range of the 1/2 central angle of the sector is calculated as follows: In addition, the value range of k calculated according to Equation (9) still cannot be applied in practice.Because only when k satisfies Equation ( 9), the corresponding value of α max exists.The value range of k is calculated considering that the position of the TANsuitable area should be at the edge of the sector scan area in the calculation process, and angle ∠BSC = 45 • is assumed to exist.Therefore, in practice, the value of k only needs to satisfy Equation ( 5), and the value of k can be obtained as follows: The Equation (12) has given the minimal central angle of the sector.For a sector which do not satisfy the Equation ( 12), the area of the sector is too small to contain even one N × N block, making it impossible to find a TAN-suitable area with a size of N × N in this sector.

Limit Line at the Target Point
The limit line is proposed to reduce the time taken for an AUV to reach the target point.When the sector searching method is used for path planning, there is another limiting case, that is, the TAN-suitable area obtained by search is always at the edge of the sector scan area and on the same side of the sector.In this limiting case, the vehicle starts from the starting point, and if the 1/2 central angle of the sector is a constant value α, the TAN-suitable area will cross the target area or target point after several selections of TAN-suitable areas.At this time, the TAN-suitable area obtained will appear behind the target area, as shown in Figure 9.
also the minimum value of max α .Therefore, in practical application, the 1/2 central angle α of the sector should not exceed 45°.Combining with Equations ( 4) and ( 5), the range of the 1/2 central angle of the sector is calculated as follows: In addition, the value range of k calculated according to Equation ( 9) still cannot be applied in practice.Because only when k satisfies Equation ( 9), the corresponding value of max α exists.The value range of k is calculated considering that the position of the TAN- suitable area should be at the edge of the sector scan area in the calculation process, and angle 45 B SC ∠ = °is assumed to exist.Therefore, in practice, the value of k only needs to satisfy Equation ( 5), and the value of k can be obtained as follows: The Equation (12) has given the minimal central angle of the sector.For a sector which do not satisfy the Equation ( 12), the area of the sector is too small to contain even one N N × block, making it impossible to find a TAN-suitable area with a size of N N × in this sector.

Limit Line at the Target Point
The limit line is proposed to reduce the time taken for an AUV to reach the target point.When the sector searching method is used for path planning, there is another limiting case, that is, the TAN-suitable area obtained by search is always at the edge of the sector scan area and on the same side of the sector.In this limiting case, the vehicle starts from the starting point, and if the 1/2 central angle of the sector is a constant value α , the TAN-suitable area will cross the target area or target point after several selections of TAN-suitable areas.At this time, the TAN-suitable area obtained will appear behind the target area, as shown in Figure 9.To avoid that, the path planning method will select to arrive on TAN-suitable areas which behind the target area, a method is proposed to restrict the sector searching area near the target.As shown in Figure 10, with the target point as the center point and the line between the starting point and the target point as the centerline, two limiting lines To avoid that, the path planning method will select to arrive on TAN-suitable areas which behind the target area, a method is proposed to restrict the sector searching area near the target.As shown in Figure 10, with the target point as the center point and the line between the starting point and the target point as the centerline, two limiting lines with an angle of β with the center line in the direction of the starting point is constructed.All the TAN-suitable areas are required to be within the angle between the two limit lines.The shaded areas A and B in the Figure 10 are the scan areas that have been deleted due to the appearance of the limit lines.
with an angle of β with the center line in the direction of the starting point is constructed.All the TAN-suitable areas are required to be within the angle between the two limit lines.The shaded areas A and B in the Figure 10 are the scan areas that have been deleted due to the appearance of the limit lines.At the target point, the existence of the limit lines can make the position of selected TAN-suitable area converge to the target point when the vehicle is close to the target point.At the same time, it can prevent the TAN-suitable area obtained by searching from crossing the target point and appearing behind the target point, while avoiding the vehicle from making redundant rotation movement simultaneously.
To prevent the TAN-suitable area from appearing behind the target point, the maximum angle β in Figure 10 between the limit line and the line of the starting point and the target point should not exceed 90°.When β =90°, the limit line is perpendicular to the line of the starting point and the target point.
At the same time, to make the range of the sector scan area are large enough and ensure the convergence of the searched TAN-suitable areas, the method similar to the value analysis of max α is adopted, which can be concluded that the angle β between the limit line and the line between the starting point and the target point should be no less than 45°.That is, the value range of β is:

Selection of Target-Aided Point
The final step of the proposed method is to find out a target aided point around the target.In some special mission such as seabed equipment installation, the vehicle should arrive in the target point extreme accurately.However, TAN cannot yield an accurate location for a vehicle when the terrain in the target area is flat.The vehicle should arrive on a TAN-suitable area around the target to locate itself firstly, and then the vehicle can reach the target position accurately.Target point may exist at the edge of the sector searching area of the vehicle, the long travel distance of the vehicle from current point to the target will result in a large cumulative error when the vehicle reaches the target point, as shown in Figure 11.At the target point, the existence of the limit lines can make the position of selected TAN-suitable area converge to the target point when the vehicle is close to the target point.At the same time, it can prevent the TAN-suitable area obtained by searching from crossing the target point and appearing behind the target point, while avoiding the vehicle from making redundant rotation movement simultaneously.
To prevent the TAN-suitable area from appearing behind the target point, the maximum angle β in Figure 10 between the limit line and the line of the starting point and the target point should not exceed 90 • .When β = 90 • , the limit line is perpendicular to the line of the starting point and the target point.
At the same time, to make the range of the sector scan area are large enough and ensure the convergence of the searched TAN-suitable areas, the method similar to the value analysis of α max is adopted, which can be concluded that the angle β between the limit line and the line between the starting point and the target point should be no less than 45 • .That is, the value range of β is:

Selection of Target-Aided Point
The final step of the proposed method is to find out a target aided point around the target.In some special mission such as seabed equipment installation, the vehicle should arrive in the target point extreme accurately.However, TAN cannot yield an accurate location for a vehicle when the terrain in the target area is flat.The vehicle should arrive on a TAN-suitable area around the target to locate itself firstly, and then the vehicle can reach the target position accurately.Target point may exist at the edge of the sector searching area of the vehicle, the long travel distance of the vehicle from current point to the target will result in a large cumulative error when the vehicle reaches the target point, as shown in Figure 11.
As can be seen from Figure 11a, a small INS accumulative error at the target point can meets the navigation accuracy requirements of the vehicle, and AUV can directly navigate to the target.However, in Figure 11b, the vehicle has to travel nearly the entire search radius to reach the target, and the large accumulative error will affect the accuracy of reaching the target point.Thus, an appropriate TAN-suitable area is selected for transition within an appropriate range before the vehicle reaches the target, and the center point of this area is called the target-aided point, as shown in Figure 12.As can be seen from Figure 11a, a small INS accumulative error at the target point can meets the navigation accuracy requirements of the vehicle, and AUV can directly navigate to the target.However, in Figure 11b, the vehicle has to travel nearly the entire search radius to reach the target, and the large accumulative error will affect the accuracy of reaching the target point.Thus, an appropriate TAN-suitable area is selected for transition within an appropriate range before the vehicle reaches the target, and the center point of this area is called the target-aided point, as shown in Figure 12.In Figure 12, the dotted line is the target exit limit line, and β denotes the limit   As can be seen from Figure 11a, a small INS accumulative error at the target point can meets the navigation accuracy requirements of the vehicle, and AUV can directly navigate to the target.However, in Figure 11b, the vehicle has to travel nearly the entire search radius to reach the target, and the large accumulative error will affect the accuracy of reaching the target point.Thus, an appropriate TAN-suitable area is selected for transition within an appropriate range before the vehicle reaches the target, and the center point of this area is called the target-aided point, as shown in Figure 12.In Figure 12, the dotted line is the target exit limit line, and β denotes the limit angle.|TA| is the minimum distance required by AUV from point A T to target T, which is jointly determined by the turning circle of AUV, the navigation error of the vehicle, and size of the matching scan area.|TB| is a predefined value that represents the maximum distance set for AUV from point A T to target T, ensuring that the vehicle can search for at least one target-aided point.The defined distance of |TA| and |TB| can be calculated as In Figure 12, the dotted line is the target exit limit line, and β denotes the limit angle.|TA| is the minimum distance required by AUV from point T A to target T, which is jointly determined by the turning circle of AUV, the navigation error of the vehicle, and size of the matching scan area.|TB| is a predefined value that represents the maximum distance set for AUV from point T A to target T, ensuring that the vehicle can search for at least one target-aided point.The defined distance of |TA| and |TB| can be calculated as where l is the turning circle of the vehicle, σ is the estimate on the maximum INS error of the vehicle after driving L R distance, and N has been described in Equation (2).Then, the value range of distance L between the target-aided point and the target point

Simulations
To test the performance of the TAN path planning method, a simulation is carried out.In this simulation, several sets of parameters are selected to test and compare the target-aided point and underwater path generation at different L max by the method in Section 2. The undersea terrain used in the simulation is collected from Dalian, China, using a ship-borne MBES.The main parameters in the simulation are shown in Table 1, and a priori map is shown in Figure 13.

Simulations
To test the performance of the TAN path planning method, a simulation is carried out.In this simulation, several sets of parameters are selected to test and compare the target-aided point and underwater path generation at different Lmax by the method in Section 2. The undersea terrain used in the simulation is collected from Dalian, China, using a ship-borne MBES.The main parameters in the simulation are shown in Table 1, and a priori map is shown in Figure 13.

Searching and Generation of the Target Aided Point
When the vehicle moves underwater, the starting point and the target point need to be defined.In other words, the position where the vehicle performs its task, the target point or area of its movement should be set.At the same time, a series of parameters need to be determined are shown in Table 2.

Searching and Generation of the Target Aided Point
When the vehicle moves underwater, the starting point and the target point need to be defined.In other words, the position where the vehicle performs its task, the target point or area of its movement should be set.At the same time, a series of parameters need to be determined are shown in Table 2.The value range of α and β are described in Equations ( 11) and ( 13), respectively.The distance L between the target-aided point and the target should satisfy Equation (16).L max and L min represent the maximum and minimum distances between two adjacent TAN-suitable areas.In addition, N is jointly determined by the INS error and the real-time scan area size of the vehicle.
According to the actual situation of a certain type of AUV, in this simulation, we assumed that l = 10 m and p = 0.05.A set of values of β and L max were given for comparison, listed in Table 3: In case study 1, the starting point was set as (60, 490), and the target point coordinate was set as (430, 10).The above parameters were used to conduct a simulation of target-aided point search, and the results are shown in Table 4: In case study 2, the starting point was set as (50, 50), and the target point coordinate was set as target (400, 460).Results are shown in Table 5: The TAN-suitable area with the aided point as its center point in Tables 4 and 5 is illustrated in Figures 14a and 14b, respectively.
In Figure 14a,b, when changing various parameters, the changes of the position of terrain-aided points are not significant.However, in Figure 14a, due to the expansion of the limit angle, the area with the minimum entropy value in the scan area appears at position No. 3, but the position of this point is basically near the maximum angle allowed within the limit angle.In addition, the distance between the target aided point and the target point is determined by the terrain characteristics of the terrain area near the target point, so the distance is a constant value after the determination of each parameter.Therefore, the larger the limit maximum single matching voyage L max is, the more significant the role of target aided point and target aided area is.The TAN-suitable area with the aided point as its center point in Table 4 and Table 5 is illustrated in Figure 14a 4; (b) The TAN-suitable area with the aided point as its center point in Table 5.
In Figure 14a,b, when changing various parameters, the changes of the position of terrain-aided points are not significant.However, in Figure 14a, due to the expansion of the limit angle, the area with the minimum entropy value in the scan area appears at position No. 3, but the position of this point is basically near the maximum angle allowed within the limit angle.In addition, the distance between the target aided point and the target point is determined by the terrain characteristics of the terrain area near the target  4; (b) The TAN-suitable area with the aided point as its center point in Table 5.

Generation of TAN Path
After the target aided points target and target aided areas are generated, the target aided areas in the direction of the vehicle are searched step by step.The selected parameters are shown in Table 6.At the same time, the maximum traveling distance L max is set as 100 m and 200 m, respectively, for comparison.The start and target point of the case study 3 were set as (60, 490) and (430, 10), respectively, and corresponding positions were (50, 50) and (400, 460) in case study 4, respectively.The above parameters were used to conduct a TAN-suitable area search simulation and generate the TAN path.The terrain matching algorithm based on a particle filter (PF) theory in Ref. [11] is applied to estimate the location of the vehicle, and the distance between the estimate and the true position of the vehicle in the horizontal plan is the TAN location error.
In case study 3 with L max = 100 m, the results are shown in Tables 7 and 8.
According to the points demonstrated in the Tables 7 and 8, the TAN-suitable area (includes a target-aided area) are shown in the contour map.Then the path can be generated by connecting the centers of the TAN-suitable areas, as shown in Figure 15a According to the points demonstrated in the Tables 7 and 8, the TAN-suitable area (includes a target-aided area) are shown in the contour map.Then the path can be generated by connecting the centers of the TAN-suitable areas, as shown in Figure 15a As can be seen from Tables 7 and 8, and Figure 15a,b, when max 100 L m = , there are many TAN-suitable areas obtained by searching, and they are basically distributed along the line between the starting point and the target aided point, so an appropriate path is formed.In both simulations, the TAN location error over the whole path is less than 5 m.
In case study 4 with max 200 L m = , the results are shown in Tables 9 and 10.As can be seen from Tables 7 and 8, and Figure 15a,b, when L max = 100 m, there are many TAN-suitable areas obtained by searching, and they are basically distributed along the line between the starting point and the target aided point, so an appropriate path is formed.In both simulations, the TAN location error over the whole path is less than 5 m.In case study 4 with L max = 200 m, the results are shown in Tables 9 and 10.   10.
As can be seen from Tables 9 and 10 , the searching area of the sector is relatively large, and more N N × blocks can be obtained within the area.According to the simulation results, with different target points, this method can well generate a TAN path required for the AUV.At the same time, as the distance between the TAN-suitable areas increases, the role of the target-aided area becomes more prominent.

Conclusions
This paper proposed a path planning algorithm using a sector searching method.The following conclusions can be drawn: (1) The value range of the half sector angle and limit angle are deduced, which provides a theoretical basis for TAN path planning.(2) The sector searching method can provide a path with terrain entropy > 7.2, which has As can be seen from Tables 9 and 10, and Figure 16a,b, when L max = 200 m, the TAN-suitable areas obtained by searching are less, and the distances between each two adjacent Tan-suitable areas are longer.This is because with L max = 200 m, the searching area of the sector is relatively large, and more N × N blocks can be obtained within the area.According to the simulation results, with different target points, this method can well generate a TAN path required for the AUV.At the same time, as the distance between the TAN-suitable areas increases, the role of the target-aided area becomes more prominent.

Conclusions
This paper proposed a path planning algorithm using a sector searching method.The following conclusions can be drawn: (1) The value range of the half sector angle and limit angle are deduced, which provides a theoretical basis for TAN path planning.(2) The sector searching method can provide a path with terrain entropy > 7.2, which has shown its ability to search areas with complex terrain.(3) According to the simulation results, the proposed path planning method can generate a path with TAN location error <5 m for the long-endurance AUV stably.
Under the influence of large-scale ocean currents, the movement ability of an AUV changes dramatically, which will also affect the selection of sector searching areas in the proposed path planning method.In future work, we will quantify the impact of ocean currents on the sector center angle calculation in our algorithm.

Figure 1 .
Figure 1.TAN-suitable area searching within a sector.

Figure 1 .
Figure 1.TAN-suitable area searching within a sector.

JFigure 2 .
Figure 2. Flowchart of the proposed path planning algorithm.

Figure 2 .
Figure 2. Flowchart of the proposed path planning algorithm.

Figure 3 .
Figure 3. Diagram of sector searching method at the starting point.

Figure 4 .
Figure 4. Sector searching at some time during the AUV mission.

Figure 3 .
Figure 3. Diagram of sector searching method at the starting point.

J
. Mar. Sci.Eng.2022, 10, x FOR PEER REVIEW 5 of 19position of the vehicle (the starting point in Figure3at the initial time), and it is axisymmetric to the line between the current position of the AUV and the target point.

Figure 3 .
Figure 3. Diagram of sector searching method at the starting point.

Figure 4 .
Figure 4. Sector searching at some time during the AUV mission.

Figure 4 .
Figure 4. Sector searching at some time during the AUV mission.

Figure 5 .
Figure 5. Parameters of sector searching area.In Figure 5, α is the 1/2 of the central angle, R L is the radius of the sector, and r L is the minimum distance required by the vehicle to meet the rotary motion, while the TAN-suitable area is demonstrated by a square area with a size of N N × .When only one N N × block can be included in the sector area, the minimum value min α of the half-sec- tor angle can be obtained, as shown in Figure 6.In addition, calculation of min α is shown

Figure 6 .
Figure 6.Sector searching area with a minimum central angle.

Figure 5 .
Figure 5. Parameters of sector searching area.

Figure 5 .
Figure 5. Parameters of sector searching area.In Figure 5, α is the 1/2 of the central angle, R L is the radius of the sector, and r L is the minimum distance required by the vehicle to meet the rotary motion, while the TAN-suitable area is demonstrated by a square area with a size of N N × .When only one N N × block can be included in the sector area, the minimum value min α of the half-sec- tor angle can be obtained, as shown in Figure 6.In addition, calculation of min α is shown

Figure 6 .
Figure 6.Sector searching area with a minimum central angle.

Figure 6 .
Figure 6.Sector searching area with a minimum central angle.

Figure 7 .
Figure 7. TAN-suitable area at edge using different central angles.(a) TAN-suitable area sear with a large central angle; (b) TAN-suitable area searching with a small central angle.Figure 7. TAN-suitable area at edge using different central angles.(a) TAN-suitable area searching with a large central angle; (b) TAN-suitable area searching with a small central angle.

Figure 7 .
Figure 7. TAN-suitable area at edge using different central angles.(a) TAN-suitable area sear with a large central angle; (b) TAN-suitable area searching with a small central angle.Figure 7. TAN-suitable area at edge using different central angles.(a) TAN-suitable area searching with a large central angle; (b) TAN-suitable area searching with a small central angle.To ensure that the movement distance of the vehicle along the line direction d x between the starting point and the target point is greater than that in the other direction d y (that is, d x ≥ d y ), it is necessary to calculate an appropriate value of α max , and the value of α max at condition d x = d y is shown in Figure8.

Figure 8 .
Figure 8.The method to calculate max α .

Figure 8 .
Figure 8.The method to calculate α max .

Figure 9 .
Figure 9. TAN-suitable area obtained will appear behind the target area.

Figure 9 .
Figure 9. TAN-suitable area obtained will appear behind the target area.

Figure 10 .
Figure 10.The generated path after adding limit line.

Figure 10 .
Figure 10.The generated path after adding limit line.

Figure 11 .
Figure 11.The target point located in the searching area.(a) The target point near the central of the searching area; (b) The target point at the edge of the searching area.

Figure 12 .
Figure 12.The setting of target aided point A T .
angle.|TA| is the minimum distance required by AUV from point A T to target T, which is jointly determined by the turning circle of AUV, the navigation error of the vehicle, and size of the matching scan area.|TB| is a predefined value that represents the maximum distance set for AUV from point A T to target T, ensuring that the vehicle can search for at least one target-aided point.The defined distance of |TA| and |TB| can be calculated as

Figure 11 .
Figure 11.The target point located in the searching area.(a) The target point near the central of the searching area; (b) The target point at the edge of the searching area.

Figure 11 .
Figure 11.The target point located in the searching area.(a) The target point near the central of the searching area; (b) The target point at the edge of the searching area.

Figure 12 .
Figure 12.The setting of target aided point A T .

Figure 12 .
Figure 12.The setting of target aided point T A .

Table 1 .
Parameters in the simulation.Parameters Value Size of a priori map 500 m × 500 m Size of simulated scanned terrain 20 m × 20 m Size of the TAN-suitable area 50 m × 50 m Turning circle of the vehicle 10 m Resolution of a priori map 1 m × 1 m Noise coefficient 0.3 m, 0.5 m Critical elevation entropy corresponding to noise coefficient 7.8055, 7.7246 INS error 5% Distance

Table 1 .Figure 13 .
Figure 13.A priori map in the simulation.

Figure 13 .
Figure 13.A priori map in the simulation.

Figure 14 .
Figure 14.The position of aided area in the contour map.(a) The TAN-suitable area with the aided point as its center point in Table4; (b) The TAN-suitable area with the aided point as its center point in Table5.

Figure 14 .
Figure 14.The position of aided area in the contour map.(a) The TAN-suitable area with the aided point as its center point in Table4; (b) The TAN-suitable area with the aided point as its center point in Table5.

Figure 15 .
Figure 15.The generated path in case study 3. (a) The path according to the points demonstrated in the Table 7; (b) The path according to the points demonstrated in the Table8.

Figure 15 .
Figure 15.The generated path in case study 3. (a) The path according to the points demonstrated in the Table 7; (b) The path according to the points demonstrated in the Table8.

Figure 16 .
Figure 16.The generated path in case study 4. (a) The path according to the points demonstrated in the Table 9; (b) The path according to the points demonstrated in the Table10.
, and Figure 16a,b, when max 200 L m = , the TAN-suitable areas obtained by searching are less, and the distances between each two adjacent Tan-suitable areas are longer.This is because with max 200 L m =

Figure 16 .
Figure 16.The generated path in case study 4. (a) The path according to the points demonstrated in the Table 9; (b) The path according to the points demonstrated in the Table10.

Table 3 .
Parameters for searching target-aided points.

Table 4 .
Simulation results of target aided points search in case study 1.

Table 5 .
Simulation results of target aided points search in case study 2.

Table 6 .
Parameter settings during path generation.

Table 7 .
Data of each point searched on the route of the first group of starting and ending points.

Table 8 .
Data of each point searched on the route of the second group of starting and ending points.

Table 9 .
Data of each point searched on the route of the first group of starting and ending points.

Table 10 .
Data of each point searched on the route of the second group of starting and ending points.
Similar to case study 3, the path generated is shown in Figure16a,b.