Abstract
The appeal for safe navigation of autonomous surface vessels (ASVs) has deemed the path planning problem as an attractive research interest. However, most of the previous works to solve the path planning problem focus on finding the shortest and collision-free path, but the solutions are scarcely satisfied by the safety requirements and constraints related to the USVs’ mechanical systems. To address this challenge, we present a novel path planning method based on a modified artificial fish swarm algorithm in combination with a path optimizer. The modifications are made from two perspectives: (1) Four customized operators and an adaptive factor are applied to improve the convergence performance of the algorithm. (2) A local path optimizer is proposed to enhance its feasibility of cooperating with the USV control system. Path safety, path smoothness, and nonholonomic constraints are considered. The path planning benchmark experiments have demonstrated its superior performance in terms of efficiency and path quality compared to other state-of-the-art algorithms. Moreover, the proposed method is also integrated into the USV’s control system in a practical environment with satisfactory feasibility. The simulation results provide strong evidence that the proposed method can be regarded as a practical approach for USV path planning problems.
1. Introduction
The benefits of deploying unmanned surface vehicles (USVs) in civil and military applications have raised a growing amount of interest worldwide [1, 2]. In general, USVs are unmanned ships that perform tasks in various complex ocean environments without or with limited human intervention. With high-level autonomy and coordinated cooperation features, they have been extensively applied in ocean research, ocean resources exploration, and maritime transport or rescue. The autonomy level of a USV ranges from manual operation to full autonomy, whereby the path planning techniques, connecting perceptional hardware, and control functionalities play substantial roles [3]. Central to path planning, the adaptability of the USVs’ mechanical system and avoidance of obstacles would dramatically determine the system’s performance [4].
Recently, there has been a growing appeal for path planning algorithms with the vigorous development of artificial intelligence [5, 6]. Developing path planning algorithms with high computing efficiency, robustness, and high-quality solutions is an attractive topic in the current studies [7–9]. Qin et al. [10] proposed a rapid USV path planning algorithm to decrease the computational complexity. Fan et al. [11] presented a second path planning method (SPP), which is an initial path optimization method based on the geometric relationship of the three-point path. The SPP is proposed to make the maximum curvature radius of the optimized path smaller than the rotation curvature radius of the USV. The work presented by Mahmoud Zadeh et al. [12] proposed an uninterrupted collision-free method that facilitates the operational performance of multiple USVs in an ocean sampling mission. This system was developed based on the integration of a novel B-spline data frame and a particle swarm optimization (PSO)-based solver engine. Krell et al. [13] introduced the concept of opportunistic reward-based planning and applied PSO to the path planning problem. The PSO is used to optimize routes that balance path efficiency and reward, and simulations have been carried out based on real ocean current data. The results have shown its superior performance compared to other references. The work of Wang et al. [14] integrated the ant colony optimization (ACO) algorithm with the immune algorithm (IA) to address its slow convergence, local optimum, and premature convergence in path planning. The simulation test proved the effectiveness and superiority of the designed global path planning algorithm in practical engineering. Han et al. [15] proposed a dynamically hybrid path planning scheme incorporating global guidance, real-time motion planning, and collision-free maneuvering for USVs. The results of comparative numerical analysis validate that the proposed algorithm works effectively. To improve the Gauss pseudospectral method (GPM) which cannot be applied to complex ocean scenarios, Ma et al. [16] modified and proposed a rapid-Gauss pseudospectral method (RGPM) to obtain a smooth time-optimal path for USVs. Lazarowska [17] utilized a discrete artificial potential field and a path optimization algorithm to calculate the path for USVs. The simulation tests with the use of real navigational data of a training ship Horyzont II.
However, many of the previous works to solve the path planning problem are to find a collision-free and the shortest path, but the solutions are not satisfied by the safety requirements [18, 19] and the nonholonomic constraints of USVs [20, 21]. Moreover, another issue seldom considered is that many algorithms that appear to be efficient theoretically are not applied and tested in the USV control system, which means they are not convincing in their practicability [22]. To address these challenges, we proposed a novel path planning method based on the modified artificial fish swarm algorithm (MAFSA) in combination with a local path optimizer. Customized operators and an adaptive factor are applied to MAFSA to enhance the convergence performance of the algorithm. Moreover, the path optimizer is able to optimize the path safety, path smoothness, and curvature continuity simultaneously. Illustrative simulation studies have been conducted and the new algorithm is tested in the USV control system model to verify its performance.
The remainder of this paper is organized as follows: Section 2 introduces the problem statement of this research. Section 3 presents the basic scheme of the modified artificial fish swarm algorithm and the local optimizer. Section 4 shows the simulation results in terms of convergence analysis, benchmark test, and application in the USV control model. The conclusion is presented in Section 5.
2. Problem Formulation
In this study, we focus on the path planning problem of USVs. First, we define the marine surface domain as in Euclidean space . Suppose the USV’s path consists of a sequence of linked elementary path segments . Following the path , the USV navigates from the initial position to the destination in the presence of numerous obstacles , is the number of obstacles. Therefore, the obstacle-free motion area of the USV is calculated as follows:
Accordingly, to guarantee the safety, the generated path should be restricted to , and the path must in the domain which is given as follows:
As can be seen from equations (1) and (2), the USV cannot collide with obstacles and should be bounded in the obstacle-free area . In addition, to enhance the safety level, the clearance from the obstacles should larger than the predefined safety distance . Suppose the distance between each path segment to its nearest obstacle is denoted as . Then the path safety requirement of each segment can be expressed as follows:
Moreover, as the properties of the path influence the motion of USVs and vice versa, it is essential to consider the constraints related to the USV’s mechanical system. Like UGVs, the nonholonomic feature prohibits its motion in the lateral direction, which means the USVs can only use the forward velocity while manipulating the heading angle to achieve a desirable position. This nonholonomic feature requires the path should be smooth and continuous at turning points to avoid large changes (sharp turnings) in curvature. The discontinuous turning points will cause instantaneous extra control signal which leads to the poor tracking performance. In Figure 1, suppose is the position vector between two consecutive poses and denote the path segments, then and define the angle between and path segment and , respectively. To achieve continuous path, the turning motion requires two consecutive positions and to be located on a common arc of constant curvature, which gives the following equation:
To sum up, the problem being addressed in this work can be stated as follows. Consider a marine environment , given a start and goal position of , plan a collision-free path denoting the set of positions from to subject to equations (2)–(4). In the end, report or if such a path has been found or not.
3. Methodology
3.1. Modified Artificial Fish Swarm Algorithm
3.1.1. Prey Behavior
The intention of searching for positions with higher food concentrations is a biological behavior among fish [22]. Generally, artificial fish (AF) utilize their vision to look for positions with a higher density of food, and then decide which direction to move. Accordingly, based on this characteristic, we simulated the AFs to find the global optimal solution to the problem.
The current state of an AF is denoted as , and is somewhere in the visual distance that the AF desires to move, which can be expressed as follows:
If the state at the visual position is better (aspect of food concentration) than the current state, it goes forward a step in this direction, as shown in the following equation:
If , then will be selected again. If the AF is not satisfied with the forward condition after time, the concerned AF performs random behavior to avoid the local optimum.
As can be seen from equation (6), before moving to the next position, the AF usually searches for the next state randomly in its visual range. This has raised some drawbacks to the algorithm. First, the time cost increases because the AF needs to try all the possible directions until the position is found. Second, the path generated by the prey behavior is not optimal due to the random process. To address these issues, this paper introduces a heuristic directional operator. During the prey behavior, the AF is inspired by the operator and consciously chooses the best position by itself, as shown in Figure 2(a). Suppose that represents the potential position number of AF number in the step range. represents the set of all the potential positions, it can be expressed as follows:
(a)
(b)
(c)
In prey behavior, the food concentration of all the potential positions in is calculated and then the best position is selected to be the next step, as shown in (8) and (9). The directional operator firstly guarantees the AF can find the best position with one cycle of calculation, which significantly reduces the computing time. Secondly, the directional operator replaces the random process, thereby the redundant turns and unnecessary nodes are reduced.
3.1.2. Follow Behavior
In the process of fish swarm moving when one or some of the fishes find food, the other neighbors and close fishes go after them and reach food quickly, as shown in Figure 2(b). Suppose the current position of AF is , and the position denotes the neighbor fish with better food concentration in its visual scope. The swarm center is defined as the arithmetic average of all AFs’ states, as shown in equation (10). is the number of AFs in the visual scope of the swarm center. denotes the parameter of the crowd factor. If and , which means the position has better food consistency and is not crowded. Then the AF moves a step in the direction of . The expression can be determined as follows:
If there are no neighbors to be found or the condition is not satisfied, the AF will perform prey behavior.
3.1.3. Swarm Behavior
The fish will assemble in groups naturally in the moving process, which is a kind of living habit to guarantee the existence of the colony and avoid dangers. In order to keep swarm generality, AFs attempt to move towards the central position in every time of iteration, the swarm behavior is illustrated by Figure 2(c). The central position is determined by the following equation:
Denote as the number of AF swarms in the visual range of . If and , it means the center position has better food consistency and the swarm is not crowded, then the AF can take a step in the direction of , which is done byotherwise, the AF executes prey behavior.
3.1.4. Random Behavior
To prevent local optimum, the AF would execute random behavior if the other behaviors failed to execute. Random behavior means the AF chooses an arbitrary state or position randomly in its visual field, and then it swims towards the selected state. It can be described as follows:
3.1.5. Adaptive Visual and Step
In standard AFSA, the visual and steps are fixed. At the beginning of the algorithm, large values of visual and step lead to better convergence speed. However, when AFs move close to the final position, the large values will cause problems such as local optimum or iterative jumps. But if the values are too small, the efficiency is decreased. Therefore, the algorithm has specific requirements on the size of the visual and steps in different stages. To balance the global search ability and convergence rate, an adaptive factor is introduced.
In this paper, the aggregation factor which is used to describe the degree of aggregation of all individuals at the optimal value is given as follows:where is the mean fitness value of all artificial fish, and denote the optimum value and minimum value of food concentration. Then the adaptive factor is given as follows:wherewhere , and are customized control parameters.
The expression of visual and step are determined as follows:
From (18) and (19), we can see the large visual and step value in the beginning stage ( and ) will lead AFs move towards positions quickly and make them gather. As the calculation continues, the visual and step are gradually getting smaller, which improves the efficiency and accuracy of searching.
The pseudocode of modified artificial fish swarm algorithm is presented in Algorithm 1.
|
3.2. Local Optimizer
USV is a nonholonomic and underactuated robotic system, which means it features low maneuverability and motion flexibility. The path generated by MAFSA contains numerous discrete waypoints and redundant changes in heading angle which may cause large control inputs, even for saturating actuators. A sufficiently smooth and continuous path is then critical. Moreover, since the Euclidean heuristic in MAFSA always selects the shortest path regardless of where the obstacles are located, the path would lie extremely close to the obstacles. Therefore, we introduce the local optimizer to address these issues. The objective function designed for the optimizer is illustrated as follows:where three terms of this cost function represent the path smoothness, path continuity, and path safety, respectively. The smoothing objective function optimizes the change of the heading angle, which is expressed as follows:where is the heading angle change of the USV. The second term denotes the path continuity. To avoid abrupt changes of the control inputs, the motion of the USVs requires two consecutive positions and to be located on a common arc of constant curvature. As we mentioned in Section 2, a common arc of constant curvature is obtained if and only if and are equal:
Therefore, should be minimized:
The third term ensures the path keeps a sufficient distance from the obstacles. Suppose the distance between each path segment to its nearest obstacle is denoted as . Then the path safety of each segment can be expressed as:
Then should be optimized as follows:
Figure 3 shows the scheme of the proposed method.
3.3. USV Model Design
To implement the proposed method for practical simulation, this section presents a USV control system model from a USV prototype given by [23]. The dynamics and kinematics model of a USV can be expressed as follows:where is the position vector of the USV in the inertial frame and denotes the yaw angle; is the linear velocity vector in the frame while is the yaw rate; , , and denote the inertial related parameters; and , , and represent the damping related parameters. and are the control input of surge and yaw. represents the saturation function of the control signals, and it is described as follows [24]:where denotes or , and are the maximum value and minimum value of , respectively. All the parameters related to the USV model are presented in Table 1.
Apart from the USV dynamics, three subsystems are also designed for implementation: navigation, guidance, and control (NGC) systems. The navigation system receives the state’s signal from the sensors and a customized extended Kalman filter (EKF). All the information obtained is processed and merged to determine the actual positions and velocities. The guidance subsystem includes the path planning module, which computes a collision-free route as the reference trajectory, and a line-of-sight (LOS)-based autopilot, which employs a closed-loop PID controller. The configuration of the USV model is presented in Figure 4.
4. Simulation and Discussion
In this section, computer-based simulations have been carried out to evaluate the performance of our proposed method. Notably, the simulations are conducted via the MATLAB 2021a environment on a PC configured with an Intel (R) Core (TM) i7-8700 CPU and 8 GB RAM. The specific configurations for the simulations are presented as follows:(i)Section 4.1 presents the efficiency benchmark test between the MAFSA and other existing references to verify the effectiveness of the algorithms.(ii)Section 4.3 shows the path planning benchmark study to evaluate the solution quality of MAFSA with a local optimizer.(iii)Section 4.3 shows the implementation of the proposed method into the USV control system to verify its performance in a practical situation.
4.1. Efficiency Benchmark Test
In this section, the path planning benchmark test is conducted between the proposed method and other existing references to evaluate its efficiency. As adjusting the parameters of the MAFSA is not the focus of this research, their values are determined by the general settings and previous works. For this work, the crowd factor is 0.618; the initial step length and visual scope are selected as 5 and 10 pixels; is 10; the population size is 50 for all the cases. The parameters of Hybrid and lite are set as the same in [24, 25]. To verify the effectiveness of the proposed methods, eight scenarios of binary maps including the complex grid maps and open water cases are considered as shown in Figure 5. Table 2 presents the environment information for each scenario. Moreover, to eliminate the randomness of the algorithms, it should be noted that 100 runs are conducted for each case. The robustness is evaluated by the standard deviation (SD) as shown in the following equation [26]:
(a)
(b)
Table 3 shows the quantitative results of the performance on each scenario. Figure 6 presents the boxplot of the time cost. From the corresponding simulation results, the following conclusions can be drawn:(1)It is observed from Table 3 that the proposed method yields the best performance regarding the time cost. The solutions of scenario 1–4 are obtained within less than 0.5 s and the time costs of scenario 5–8 are around 1 s, which is much efficient than the other algorithms.(2)As shown in Figure 6 and Table 3, MAFSA also presents the least standard deviation of time cost in all cases compared to the other methods. Such performance has demonstrated that the MAFSA possesses strong robustness.(3)Regarding the path quality, the MAFSA gives the optimal solutions in terms of path length in all cases. In particular, the path length is approximately 20% shorter compared to the conventional AFSA.
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
Figures 7 and 8 present the visualized results of the simulation. Table 5 shows the statistical measurements of the paths. From the simulation results, the findings are summarized as follows:(1)Inspection of Figures 7 and 8 have shown that the sharp turnings are successfully avoided by the proposed method, which gives a smoothed trajectory. Moreover, applying the local optimizer ensures the path is sufficiently far from the obstacles and does not cause a potential collision risk.(2)From the quantitative results presented in Table 5, the proposed method is the only one passes the safety requirement with the minimum distance from obstacles of 10.630 m and 10.440 m for each case. Furthermore, the smoothness values obtained by the proposed method are the least among them. These results indicate that the local optimizer is able to generate practical solutions for the path planning.
4.2. Solution Quality Benchmark Test
In this section, off-line path planning simulations are conducted to illustrate the path quality performance of the algorithm. Some other algorithms from existing reliable references are selected for the benchmark study, i.e., Hybrid [25], lite [24], and [27]. MAFSA is embedded with the local optimizer to solve the problem. The weight value of each objective is set as the same and the optimization problem is solved by the barrier method from the MATLAB optimization toolbox. The parameters of MAFSA and testing environment are set as the same as Section 4.1. The environment map is configured as shown in Table 4.
(a)
(b)
Visualized and quantitative results are shown in Figure 10 and Table 7. Figure 11 shows the path tracking results by the USV model. Figure 12 presents the course angle and speed of the USV model. The propeller command during the simulations is recorded in Figure 13. From the simulation results, the following findings can be summarized:(1)From Figures 10 and 11 and Table 7, it is shown in the results that the proposed method obtains the better solution quality in terms of safety and smoothness. The minimum clearance from the obstacle is 10.198 m and the smoothness value is 2.547 rad. This is directly in line with the findings from the previous section.(2)Comparing the results of the simulations, it can be observed that the red trajectory in Figure 7 is much smoother than the others, especially in the section denoted by the blue boundary. The tracking results presented by the other methods have shown winding trajectories with large deviations.(3)Inspection of the control signal in Figure 12 denotes that the deviation between the reference and control input of the proposed method is the smallest. This indicates that the solution fit the USV’s mechanical system well.(4)In Figure 13, the propeller command is more stable compared with others, and the abrupt changes in control inputs are avoided. This indicates that the proposed method is more feasible in cooperating with the USV control system in the practical environment.
(a)
(b)
(c)
(a)
(b)
(c)
(a)
(b)
(c)
4.3. Simulation in USV Model
To further evaluate the performance, the proposed method is implemented in the USV control system model for a real mission. The simulation site is selected at Qizhen Lake in the Zhejiang University as shown in Figure 9(a). The start and goal are marked as blue and red dots, respectively, in Figure 9(b). More detailed information is shown in in Table 6. The parameter settings are the same as in Section 4.1.
5. Conclusion
In this work, we present a modified artificial fish swarm algorithm in combination with a local optimizer to solve the path planning problem. The proposed algorithm is generally efficient and practical for the USV path planning problem. The efficiency and path quality tests have verified that it is better than several other algorithms from existing reliable references in terms of effectiveness and solution quality. Moreover, the simulations conducted in the USV model demonstrate that MAFSA can perfectly coordinate with the control system. Therefore, the simulation results have presented strong evidence that the proposed method is reliable for USV path planning.
Abbreviations
| ACO: | Ant colony optimization |
| AF: | Artificial fish |
| AFSA: | Artificial fish swarm algorithm |
| GPM: | Gauss pseudospectral method |
| IA: | Immune algorithm |
| MAFSA: | Modified artificial fish swarm algorithm |
| PSO: | Particle swarm optimization |
| RGPM: | Rapid-Gauss pseudospectral method |
| SD: | Standard deviation |
| SPP: | Second path planning |
| USV: | Unmanned surface vehicle. |
Data Availability
The data used to support the findings of this study are included within the article.
Conflicts of Interest
The authors declare that there are no conflicts of interest.
Acknowledgments
This work is mainly supported by the Stable Supporting Fund of Science and Technology on Underwater Vehicle Technology (Grant number: JCKYS2022SXJQR-01).