1. Introduction
Submarines are ships that can operate independently underwater. Since its appearance, it has been widely used in various military operations and has become an important part of modern naval operations.
With the continuous changes in the international situation, a series of studies on underwater navigation of submarines have received extensive attention. Different to surface ships, submarines face more unknowns and threats underwater, all of which pose challenges to the safe underwater navigation of submarines. In order to ensure the safety of navigation, submarines need to find the path that best meets the mission requirements among the many optional routes. Therefore, a suitable path planning algorithm is particularly important.
Due to the complexity and nonlinearity of the underwater 3D environment, the 2D land-based algorithms cannot be directly used to solve the underwater path planning problems.
In response to this issue, many scholars have focused on the development of underwater path planning algorithms. At present, they can be categorized into graph search-based approaches, such as Dijsktra algorithm [
1,
2,
3],
algorithm [
4,
5]; sample planning-based approaches, such as PRM algorithm [
6,
7],
algorithm [
8,
9]; artificial potential field (APF)-based approaches [
10,
11]; evolutionary algorithms (EAs)-based approaches, such as distribution estimation algorithm (EDA) [
12], particle swarm optimization (PSO) [
13,
14], genetic algorithm (GA) [
15,
16], differential evolution algorithm (DE) [
17]; heuristic algorithms (HAs)-based approaches, such as ant colony algorithm (ACO) [
18,
19], simulated annealing algorithm (SA) [
20,
21].
Based on these algorithms, a large number of scholars have developed path planning algorithms adapted to the three-dimensional environment or the marine environment.
Arinaga et al. applied Dijkstra algorithm to a global path search for AUVs in an underwater environment. The algorithm can avoid a set of obstacles and reach the end but does not consider the impact on the marine environment [
1]. Garau et al. implemented a path search with
algorithm and considered the influence of marine environmental factors [
22]. Carreras et al. employed the
algorithm to perform 2D AUV path planning, and the 3D results show that the adaptability of this method in the real complex environment is satisfactory [
8]. Jantapremjit et al. not only realize automatic obstacle avoidance by applying the APF algorithm but also introduced the state-dependent Riccati equation method to optimize the optimal high-order sliding mode control, which improved the robustness of the AUV motion [
23]. Liu et al. used the PSO algorithm for AUV path planning. Simulation experiments show that the algorithm is simply easy to implement, not sensitive to the population size, and has a faster convergence speed [
13]. Ma et al. introduced alarm pheromones in the ACO algorithm (AP-ACO) for path planning of underwater vehicles. The experimental results show that compared with the ordinary ACO algorithm, AP-ACO has a faster convergence speed and stability [
24]. Rafael et al. introduced a vortex field in the improved APF algorithm. This method can effectively reduce the collision risk between the UAV and obstacles and reduce the vibration of the path. In addition, the introduction of the artificial potential field algorithm effectively solves the local minimum and oscillation problems of the threshold [
25].
It is worth mentioning that, with the gradual complexity of application scenarios, the combination of multiple algorithms has become a hot research direction in the field of path planning.
Zhang et al. proposed a branch selection rapid exploration random tree (BS-RRT) algorithm to solve the global path planning problem in the narrow channel environment of UAVs. However, these two algorithms are aimed at aerial unmanned equipment, which is not the same as the background of this study [
26]. Yu et al. combined the improved Grey Wolf Optimization algorithm with the
Light algorithm and proposed a multi-target path planning algorithm for an unmanned cruise ship in an unknown obstacle environment [
27]. The algorithm effectively improves the path planning efficiency of unmanned ships and can achieve good results in complex simulation experimental environments. However, the algorithm is limited to two-dimensional path planning, and its adaptability in a three-dimensional environment has not been further studied. A summary of the algorithm is shown in
Table 1.
At present, due to the special mission background, there are few studies on the path planning of submarines. More researchers have turned to the path planning algorithms for underwater vehicles such as AUVs. Although the operating environment of AUVs is similar to that of submarines, there are significant differences between the two in terms of size, mission background, and dynamic characteristics. These differences are summarized in
Table 2. Therefore, simply using AUV’s path planning algorithm for submarine path planning is inappropriate and has hidden dangers. This paper aims to develop a path planning algorithm with significant advantages for the particularity of submarines.
Considering the applicability and stability of the algorithm, we based on the relatively mature ant colony algorithm, combined the artificial potential field algorithm, and proposed a new artificial potential field ant colony algorithm (APF-ACO). The algorithm is not a simple combination of several algorithms, but a special development for the characteristics of submarine underwater navigation. The algorithm can effectively deal with the underwater path planning problem of submarines. In this sense, the main contributions of this paper include:
In this paper, the APF-ACO algorithm is proposed to solve the underwater global path planning problem of submarines. The algorithm is able to converge rapidly in the underwater environment. Moreover, the path planning results obtained by this algorithm are more advantageous and more stable.
In order to further optimize the path planning results obtained by the APF-ACO algorithm and make it more in line with the navigation requirements of submarines, this paper develops an inflection point optimization algorithm and a path smoothing algorithm. Experimental results show that the algorithm significantly reduces the path length and the number of inflection points.
A dynamic obstacle avoidance algorithm based on the velocity obstacle method is proposed, which further improves the content of the submarine path planning algorithm. The experimental results show that the dynamic obstacle avoidance algorithm can accurately identify and avoid threatening dynamic obstacles.
Discuss the real performance of the algorithm in the submarine semi-physical simulation system. From the results, the real feasibility of the algorithm under the underwater navigation of the submarine is verified.
3. Path Optimization Algorithm
The results obtained by the path planning algorithm usually have shortcomings such as many inflection points and insufficient smoothness. To solve these problems, the path results need to be optimized. In this paper, the inflection points optimization algorithm and the path smoothing algorithm are used to obtain the path results that are more suitable for submarine navigation.
In addition to global static path planning, submarines also need to have dynamic obstacle avoidance capabilities to deal with sudden threats. This paper proposes a three-dimensional dynamic obstacle avoidance algorithm based on the velocity obstacle method to improve the survivability of submarines.
3.1. Path Inflection Point Optimization
First, find the inflection points in the path that the algorithm gets. Suppose there are
path points in the grid space along the path planning direction. The current path point is
, and
is the distance between point
and point
.
is the distance between point
and
.
is the distance between point
and point
. Then the inflection point can be judged according to the triangle rule:
All inflection points of the path can be obtained in this way. Add the start and endpoints, assuming there are
inflection points in total, and arrange all the inflection points in sequence:
. Connect with the second inflection point from the starting point. If the connected line does not pass-through obstacles, continue to connect with the third inflection point, and so on to connect with the
inflection points. If the starting point and the
point
pass through an obstacle when connecting, connect the starting point with the (
point as the first segment of the path after optimization. Then connect with the (
point as the starting point until the target point is reached. The flow chart of the inflection point optimization algorithm is shown in
Figure 5. The schematic diagram of the method is shown in
Figure 6.
3.2. Path Smoothness Optimization
Submarines should try to avoid large steering angles when sailing underwater. In order to meet the actual navigation requirements of submarines, this paper proposes a path smoothing algorithm adapted to the APF-ACO algorithm. The simple single-stage polynomial optimization cannot adapt to the complex underwater environment, and the complex high-order polynomial optimization is not efficient, so this paper chooses to use the Clothoid curve fitting algorithm to optimize the smoothness of the path. The Clothoid curve is based on the Fresnel integral, and the change in curvature of the curve is proportional to the arc length of the curve. The three-dimensional Clothoid curve equations are shown in (14) and (15). In this paper, two-dimensional Clothoid curve fitting is performed on
and
in turn, and then the fitting results are combined into three-dimensional fitting results.
where (
,
,
) is the starting point coordinate,
represents the tangent angle of the curve, and the expression is:
is the initial tangent angle,
is the initial curvature, and
is the arc length of the curve.
Suppose the path point obtained by the APF-ACO algorithm is
. Using the Clothoid curve to fit is to solve the Clothoid curve segment between the
path points under the condition of continuous curvature. Taking the first segment of the path as an example, the coordinates of the endpoints at both ends are
,
. According to (14), the two ends should meet the following conditions:
where
.
is the arc length of the
path. To ensure the continuity of curvature at the inflection point,
should satisfy:
This formula determines that the tangent angle and the curvature of the paths of the and sections at the inflection point are equal.
After initially completing the path smoothness optimization, it is also necessary to consider that the optimized path may collide with obstacles again. Therefore, the smooth path must be re-examined. As shown in
Figure 7, the initial smooth path is detected. If the path collides with an obstacle, the curvature
of the fitted path is reduced for re-planning until the smooth path does not collide with the obstacle.
The flowchart of the smoothing optimization algorithm is shown in
Figure 8.
3.3. Local Dynamic Obstacle Avoidance by Speed Obstacle Method
The velocity obstacle method is an algorithm that uses geometric constraints to express methods to avoid collisions with obstacles. Its schematic diagram is shown in
Figure 9. Where
is the carrier position,
is the obstacle position,
is the carrier velocity,
is the obstacle velocity,
is the relative velocity of
and
. The basic principle of the algorithm is to construct the velocity obstacle area by obtaining the position and velocity information of the carrier and the obstacle. Then, it is determined whether the carrier will collide with the obstacle by calculating whether the relative speed of the carrier and the obstacle is within the space of the obstacle area.
We inflate the submarine into a sphere of radius
and the obstacle into a sphere of radius
, then the radius of the obstacle sphere is
. It is defined that multiple rays drawn from the geometric center of the carrier are tangent to the expanded spherical obstacle area, and all the tangents form a triangular pyramid space. The
is the angle between
and the axis
and the
is 1/2 of the size of the cone apex angle.
and
are shown as (20) and (21).
where
is the distance between
and
.
When , there is a risk of collision, and collision avoidance measures should be taken; when , there is no risk of collision. Through this method, the dynamic obstacle avoidance problem of the carrier can be simplified to the static obstacle avoidance problem.
We assume that the velocity vector of the carrier and the obstacle do not change during the calculation process, then define a ray from the center of the carrier along the relative velocity direction:
where
represents time and
represents the ray composed of the current position of the carrier and the direction of the relative velocity vector. Then the collision conditions between the carrier and the obstacle are:
The relative velocities with collision risk are grouped together, which constitutes the “collision domain” of dynamic obstacle avoidance. The mathematical description of the “collision domain” is:
The velocity obstacle method uses a definite value when describing the dynamic obstacle, but there is a certain inevitable error between the sensor accuracy and the submarine’s own speed, so the submarine may accidentally collide with the obstacle. In order to solve this hidden danger, this paper decided to introduce the parameter of “safe distance”. Compared with the size of the submarine itself and the size of the obstacles, the marine navigation environment is very broad, so the parameter of “safety distance” has practical application value and feasibility. The “safety distance” radius is given by empirical values of sensor accuracy error and submarine speed error.
The corrected obstacle area radius is:
In addition, in order to enhance the adaptability of the algorithm, this paper supplements the dynamic obstacle avoidance algorithm. After dynamic obstacle avoidance, if the current node cannot safely reach the next node of the original path planning result, the current node is used as the starting point for re-path planning. Repeat this operation until the submarine reaches the end. The flow chart of the dynamic obstacle avoidance algorithm is shown in
Figure 10.
4. Test and Analysis
In this paper, a 10 × 10 × 10 (excluding boundary) three-dimensional space environment is constructed for simulation experiments, in which spheres represent obstacles. In order to verify the effectiveness of the APF-ACO algorithm proposed in this paper, relevant experiments are designed in this section.
4.1. Algorithm Performance Comparison Experiment
In order to verify the effectiveness of the APF-ACO algorithm, the experiments designed in this paper are compared with the other three algorithms in various obstacle environments. Among them, the Optimized ACO algorithm is an underwater ant colony optimization algorithm proposed by the reference [
24].
In this paper, experiments were designed in five obstacle environments, and each experiment was carried out independently 10 times, and statistical significance tests were performed to ensure the reliability of the results. The parameter settings of the algorithm are shown in
Table 3.
The experimental results are shown in
Figure 11 and
Table 4. It can be seen from the experimental results that APF-ACO and Optimized ACO have significant advantages compared with the original ACS and the original APF algorithm. This proves the superiority of these two algorithms. In addition, compared with the Optimized ACO algorithm, APF-ACO has advantages in the performance of the best results and the average results and is also better in the control of the worst results and standard deviations. In summary, the path planning results of the APF-ACO algorithm have better performance and are more stable.
In order to verify the effectiveness of the APF-ACO algorithm proposed in this paper, we further compare the computational time cost of four different algorithms under the same conditions, and the calculation results are shown in
Table 5. The experimental equipment parameters in this paper are Core i9 CPU and 16 G running memory.
It can be seen from the experimental results that the calculation time of The Original APF algorithm is the shortest, followed by the calculation time of The Original ACS. The computation time of the APF-ACO algorithm is almost the same as that of the Optimized ACO algorithm, which is about 10% longer than that of The Original ACS. The results show that the two optimization algorithms sacrifice about 10% of the computing time to obtain better path planning results, which have practical application value.
From the above experimental results, it can be seen that the APF-ACO algorithm proposed in this paper and the Optimized ACO algorithm have significant advantages. In order to further compare the performance of the two algorithms, this paper designs experiments to compare the number of path inflection points and the convergence speed of the operation. In five different experimental environments, the visual planning results of the two algorithms are shown in
Figure 12.
The establishment of five experimental environments mainly considers the influence of different obstacle distribution patterns on the calculation results of the path planning algorithm. The distribution of obstacles in test environments 1 and 2 is more discrete, the distribution of obstacles in test environment 3 is more concentrated, the distribution density of obstacles near the endpoint is increased in test environment 4, and the distribution density of obstacles near the start point is increased in test environment 5. It can be seen from the path planning results that in the above five test environments, both algorithms can obtain complete and feasible paths. However, it can also be clearly seen that the path obtained by the Optimized ACO algorithm is more tortuous when the obstacles are dense. In order to further compare the pros and cons of the two algorithms, we recorded the average number of inflection points of the planning results of the two algorithms in each experimental environment, and the results are shown in
Table 6.
It can be seen from the results that the path obtained by APF-ACO is straighter and the number of inflection points is reduced by about 18.8%. This is mainly because the APF-ACO algorithm introduces the potential field force parameter so that it can consider the distribution information of obstacles in the operation of each step. This allows the algorithm to avoid obstacles as early as possible. This feature of APF-ACO also fits the real needs of submarines sailing underwater.
Convergence speed is an important indicator to measure the performance of an algorithm. This paper next compares the convergence speed of APF-ACO and Optimized ACO. We conducted 10 independent experiments in experimental environment 3 and averaged the convergence rate results, and the obtained results are shown in
Figure 13.
As can be seen from the figure, the convergence speed of APF-ACO is about 47.7% faster than that of Optimized ACO. At the beginning of the iteration, APF-ACO can obtain better initial path results, mainly because the addition of the potential field force parameter can increase the cost difference between the path points to be selected, thereby greatly increasing the probability of selecting a better path. In the later stage of iteration, the convergence result of APF-ACO is significantly better than that of Optimized ACO, mainly because the introduction of the potential field force parameter can always guide the path to find the direction close to the endpoint, avoiding the convergence result falling into the local optimum.
In summary, it can be seen that the APF-ACO algorithm has certain advantages in terms of convergence results and convergence speed and can adapt to the complex underwater path planning application scenarios of submarines.
4.2. Path Smoothness Optimization
The simulation experiment of the path smoothing optimization algorithm is carried out in the experimental environment 5. The inflection points of the path are optimized before smooth optimization, and only the necessary inflection points of the path are retained. The final result is shown in
Figure 14:
The green dotted line in
Figure 14 is the inflection point optimization result. It can be seen intuitively that the inflection point optimization algorithm reduces the number of inflection points and shortens the path length on the premise of ensuring the safety of the path. In order to quantitatively analyze the effect of the inflection point optimization algorithm, we analyzed its performance under five different obstacle environments, and the results are shown in
Figure 15. The red implementation in
Figure 15 represents the number of inflection points of the original path, and the red dotted line represents the number of optimized inflection points. The inflection point optimization algorithm can effectively shorten the path length by about 21.7% and reduce the number of inflection points by about 53.5%. This verifies the effectiveness of the algorithm.
The purple solid line in
Figure 15 is the result of smooth optimization at the inflection point optimization path. It can be seen that the optimized path has no abrupt turning points, and the path is smoother. Not only that, but the introduction of the obstacle avoidance correction algorithm also ensures that the smoothed path will not collide with adjacent obstacles.
4.3. Local Dynamic Obstacle Avoidance Experiment
In order to verify the dynamic obstacle avoidance effect of the speed obstacle method, this paper constructs a dynamic obstacle environment for experiments. The experiment introduces dynamic obstacles on the basis of global path planning to verify the effectiveness of the dynamic obstacle avoidance algorithm and its compatibility with the APF-ACO algorithm. The visualization results are shown in
Figure 16.
We construct three dynamic obstacle environments to verify the reliability of the dynamic obstacle avoidance algorithm. We show two different perspectives of each path planning result. As shown in
Figure 16, the dynamic obstacle moves in the direction of the arrow. The red track indicates that the dynamic obstacle will collide with the original path, and the green track indicates that the dynamic obstacle will not collide with the original path track. It can be seen from the result in the figure that under the effect of the dynamic obstacle avoidance algorithm, the submarine accurately identified the threatening dynamic obstacles and re-planned them to avoid collisions. Dynamic obstacles without threats will not affect the original path trajectory. In experimental environment 2 and experimental environment 3, the re-planning node cannot safely reach the next node of the original planning result under the current step size requirement, so re-planning is carried out. It can be seen that the re-planned path results can overlap with the original path results as much as possible under the premise of successfully avoiding dynamic obstacles.
It can be seen from the experimental results that the dynamic obstacle avoidance algorithm proposed in this paper can achieve the expected effect and has a practical application value.
4.4. System Semi-Physical Experiment
In order to further verify the application of the algorithm proposed in this paper in the real environment, we applied APF-ACO to a semi-physical simulation software specially designed for the study of submarine underwater path planning for experiments. The system uses the 0.25° submarine topography data provided by GEBCO (General Bathymetric Chart of the Oceans) to generate virtual submarine topography to simulate the real underwater navigation environment of submarines. In addition, the system adopts the standard SUBOFF full-body submarine model and fully considers various maneuvering rules that submarines have underwater, such as maximum pitch angle, maximum diving depth, limit turning radius, etc.
The path planning range selected for this system test is
to
, and
to
. The starting point coordinates are
,
, and 100 m deep. The ending coordinates are
,
, and 100 m deep. The specific parameters of the APF-ACO algorithm used in this experiment are shown in
Table 7.
The specific experimental results are shown in
Figure 17.
It can be seen from the experimental results that the submarine can successfully avoid terrain obstacles and reach its destination. The result of the path is relatively straight as a whole, and it can actively maintain the navigation at the same depth, which is in line with the underwater maneuvering characteristics of submarines. The results verify the effectiveness of the APF-ACO algorithm as a submarine underwater path planning algorithm in the real marine environment.
5. Conclusions
In this paper, a path planning algorithm suitable for submarine underwater navigation is proposed. This algorithm is a composite method that includes global path generation and local path adjustment. In the global path planning, we presented an improved Artificial Potential Field Ant Colony Optimization (APF-ACO) algorithm to adapt to the underwater path planning needs of submarines. The experimental results showed that APF-ACO can stably obtain path planning results in a variety of experimental environments. Compared with the Optimized ACO, APF-ACO can obtain path results with shorter length, fewer inflection points, and better stability. Not only that, but APF-ACO also has a faster convergence speed, which meets the tactical needs of submarines.
We also propose an inflection point optimization algorithm and a smooth optimization algorithm to further improve the path obtained by APF-ACO, making it more in line with the actual underwater navigation of submarines. The experimental results show that the inflection point optimization algorithm can effectively reduce the unnecessary inflection points of the path and greatly reduce the length of the path. The smoothing optimization algorithm can increase the smoothness of the path without intersecting with obstacles.
In order to make the algorithm more complete, we have added a dynamic obstacle avoidance algorithm based on the motion obstacle method. The introduction of this algorithm can help submarines identify threatening moving obstacles and re-plan local paths. The experimental results show that the dynamic obstacle avoidance algorithm can successfully help submarines to identify and avoid dangerous dynamic obstacles.
We use a professional semi-physical simulation system for scene verification of the APF-ACO algorithm. In the system, we use real seabed topography data and real starting and ending position data to conduct experiments. The experimental results show that the APF-ACO algorithm can be effectively applied in the navigation tasks of submarines and has practical application value.
We insist on affirming the application value of path planning in submarine navigation in the new era. In future research, we will further optimize the submarine’s path planning algorithm. We consider introducing the influence of marine environmental elements on submarine navigation into the path planning algorithm to expand the scope of application of the algorithm. Next, we seek to apply the algorithm to real submarine navigation tasks and collect data in real experiments to further improve the algorithm.