# Path Planning For Unmanned Surface Vehicles Based On Modified Artificial Fish Swarm Algorithm With Local Optimizer

Authors: Fang Wang; Liang Zhao; Yong Bai
Journal: Mathematical Problems in Engineering
 (2022)

---

## 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.


---

## Body

## 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: Section2 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 R2. Suppose the USV’s path S consists of a sequence of linked elementary path segments sii=1,2,3,…,m. Following the path S, the USV navigates from the initial position xs,ys to the destination xd,yd in the presence of numerous obstacles OobOob=Oob1,Oob2,Oob3,Oob4,…,Oobk, k is the number of obstacles. Therefore, the obstacle-free motion area of the USV is calculated as follows:(1)Ωfree=Ω−Oob.Accordingly, to guarantee the safety, the generated path should be restricted toΩfree, and the path S must in the Ωfree domain which is given as follows:(2)S=∪i=1msi⊂Ωfree.As can be seen from equations (1) and (2), the USV cannot collide with obstacles and should be bounded in the obstacle-free area Ωfree. In addition, to enhance the safety level, the clearance from the obstacles should larger than the predefined safety distance Dsafe. Suppose the distance between each path segment si to its nearest obstacle OobsiOobsi⊂Oobs is denoted as di=si,Oobsi,i=1,2,3,…,m. Then the path safety requirement of each segment can be expressed as follows:(3)di≥Dsafe,i=1,2,3,…,m.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 Figure1, suppose di=xi+1−xi,yi+1−yi,0T is the position vector between two consecutive poses and sisi+1 denote the path segments, then κi,i and κi,i+1 define the angle between di and path segment si and si+1, respectively. To achieve continuous path, the turning motion requires two consecutive positions si and si+1 to be located on a common arc of constant curvature, which gives the following equation:(4)κi,i=κi,i+1.Figure 1
Definition of a path curve.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 S,G⊂Ω, plan a collision-free path S⊂Ω denoting the set of positions from S to G subject to equations (2)–(4). In the end, report P 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 asXit, and Xjt is somewhere in the visual distance that the AF desires to move, which can be expressed as follows:(5)Xjt=Xit+Visual×rand0,1.If the stateXjt 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:(6)Xit+1=Xit+Xjt−XitXjt−Xit×Step×rand0,1.IfYi>Yj, then Xjt will be selected again. If the AF is not satisfied with the forward condition after trynum 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 Xi,jp represents the potential position number j of AF number i in the step range. P represents the set of all the potential positions, it can be expressed as follows:(7)P=Xi,jpXi,jp−Xi≤Step,i=1,2,3,…,N,j=1,2,…,M.Figure 2
Illustration of the behaviors. (a) Prey behavior. (b) Swarm behavior. (c) Follow behavior.
(a)(b)(c)In prey behavior, the food concentration of all the potential positions inP is calculated and then the best position Xi,bestp 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.(8)Xi,bestp=minfXi,1p,fXi,2p,fXi,3p,…,fXi,Mp,(9)Xit+1=Xit+Xi,bestpt−XitXi,bestpt−Xit×Step×rand0,1.


#### 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 Figure2(b). Suppose the current position of AF is Xit, and the position Xjt denotes the neighbor fish with better food concentration in its visual scope. The swarm center Xct is defined as the arithmetic average of all AFs’ states, as shown in equation (10). nf is the number of AFs in the visual scope of the swarm center. δ denotes the parameter of the crowd factor. If Yj>Yi and nf/N<δ, which means the position Xjt has better food consistency and is not crowded. Then the AF moves a step in the direction of Xjt. The expression can be determined as follows:(10)Xit+1=Xit+Xjt−XitXjt−Xit×Step×rand0,1.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 Figure2(c). The central position is determined by the following equation:(11)Xct=1N∑i=1NXit.Denotenf as the number of AF swarms in the visual range of Xct. If nf/N<δ and Yc<Yi, 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 Xct, which is done by(12)Xit+1=Xit+Xct−XitXct−Xit×Step×rand0,1,otherwise, 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:(13)Xit+1=Xit+Step×rand0,1.


#### 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:(14)β=mfavgfmaxXit+1−mfminXitfmaxXit,(15)favg=1N∑i=1NfXit,where favg is the mean fitness value of all artificial fish, fmaxXit and fminXit denote the optimum value and minimum value of food concentration. Then the adaptive factor α is given as follows:(16)α=ke−βm+1−ke−i/Tmaxn,where(17)m=1.5−iTmax,n=1.5−β,where k∈0,1, m and n are customized control parameters.The expression of visual and step are determined as follows:(18)visual=visual×α+visualmin,(19)step=step×α+stepmin.From (18) and (19), we can see the large visual and step value in the beginning stage (m>1 and n>1) 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 Algorithm1.Algorithm 1: Modified artificial fish swarm algorithm.
(1)
Require: N: the population of the AFs(2)
Xi: the position state of AF i(3)
Visual: the maximum range in which AF can search(4)
Step: the maximum range AF can move(5)
Initialize the artificial fish population:N(6)
Initialize the state of each AF:Xi(7)
whilethe path is not found do(8)
for each AF do(9)
flag = stateEvaluation()(10)
caseflag = prey:(11)
Xit+1←Xit+steptowardsbestpotentialposition(12)
caseflag = follow:(13)
Xit+1←Xit+steptowardsneighborfishwithbetterposition(14)
caseflag = Swarm:(15)
Xit+1←Xit+steptowardscenterofthefishswarm(16)
caseflag = random:(17)
Xit+1←Xit+steptowardsrandomdirection(18)
end for(19)
Update the state of each AFXi(20)
Updatevisual and step(21)
end while(22)
route = selectBestRoute (Xi)(23)
Output: route


### 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:(20)J=Js+Jc+Jd,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:(21)Js=∑i=1nΔψi2,where Δψi 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 si and si+1 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 κi,i and κi,i+1 are equal:(22)κi,i−κi,i+1=0.Therefore,Jc should be minimized:(23)Jc=κi,i−κi,i+1.The third termJd ensures the path keeps a sufficient distance from the obstacles. Suppose the distance between each path segment si to its nearest obstacle OobsiOobsi⊂Oobs is denoted as di=si,Oobsi,i=1,2,3,…,m. Then the path safety of each segment can be expressed as:(24)Di=0,di≥dmax,di−dmindmax−dmin,dmin<di<dmax,1,di≤dmin.ThenJd should be optimized as follows:(25)Jd=∑i=1mDi.Figure3 shows the scheme of the proposed method.Figure 3
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:(26)ξ˙=ucosψ−vsinψ,η˙=usinψ−vcosψ,ψ˙=r,u˙=m22m11vr−d11m11u−1m11Satuτu,v˙=−m11m22ur−d22m22v,r˙=−m11−m22m22uv−d33m33r+1m33Satrτr,where ξ,ηT is the position vector of the USV in the inertial frame I and ψ denotes the yaw angle; u,vT is the linear velocity vector in the B frame while r is the yaw rate; m11, m22, and m33 denote the inertial related parameters; and d11, d22, and d33 represent the damping related parameters. τu and τr are the control input of surge and yaw. Sat∗ represents the saturation function of the control signals, and it is described as follows [24]:(27)Sat∗x=τ∗max,x>τ∗max,x,τ∗min≤x≤τ∗max,τ∗min,x<τ∗min,where ∗ denotes u or r, and τ∗maxτ∗min are the maximum value and minimum value of τ∗, respectively. All the parameters related to the USV model are presented in Table 1.Table 1
USV parameters.
NameValuem11 (kg)25.8m22 (kg)33.8m33 (kg∗m2)2.7L (length: m)1.5W (width: m)0.25d11 (kg/s)2d22 (kg/s)7d33 (kg∗m2/s)0.5τ∗max47.5τ∗min6.4Apart 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 Figure4.Figure 4
The configuration of the USV model.


## 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 asXit, and Xjt is somewhere in the visual distance that the AF desires to move, which can be expressed as follows:(5)Xjt=Xit+Visual×rand0,1.If the stateXjt 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:(6)Xit+1=Xit+Xjt−XitXjt−Xit×Step×rand0,1.IfYi>Yj, then Xjt will be selected again. If the AF is not satisfied with the forward condition after trynum 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 Xi,jp represents the potential position number j of AF number i in the step range. P represents the set of all the potential positions, it can be expressed as follows:(7)P=Xi,jpXi,jp−Xi≤Step,i=1,2,3,…,N,j=1,2,…,M.Figure 2
Illustration of the behaviors. (a) Prey behavior. (b) Swarm behavior. (c) Follow behavior.
(a)(b)(c)In prey behavior, the food concentration of all the potential positions inP is calculated and then the best position Xi,bestp 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.(8)Xi,bestp=minfXi,1p,fXi,2p,fXi,3p,…,fXi,Mp,(9)Xit+1=Xit+Xi,bestpt−XitXi,bestpt−Xit×Step×rand0,1.


### 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 Figure2(b). Suppose the current position of AF is Xit, and the position Xjt denotes the neighbor fish with better food concentration in its visual scope. The swarm center Xct is defined as the arithmetic average of all AFs’ states, as shown in equation (10). nf is the number of AFs in the visual scope of the swarm center. δ denotes the parameter of the crowd factor. If Yj>Yi and nf/N<δ, which means the position Xjt has better food consistency and is not crowded. Then the AF moves a step in the direction of Xjt. The expression can be determined as follows:(10)Xit+1=Xit+Xjt−XitXjt−Xit×Step×rand0,1.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 Figure2(c). The central position is determined by the following equation:(11)Xct=1N∑i=1NXit.Denotenf as the number of AF swarms in the visual range of Xct. If nf/N<δ and Yc<Yi, 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 Xct, which is done by(12)Xit+1=Xit+Xct−XitXct−Xit×Step×rand0,1,otherwise, 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:(13)Xit+1=Xit+Step×rand0,1.


### 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:(14)β=mfavgfmaxXit+1−mfminXitfmaxXit,(15)favg=1N∑i=1NfXit,where favg is the mean fitness value of all artificial fish, fmaxXit and fminXit denote the optimum value and minimum value of food concentration. Then the adaptive factor α is given as follows:(16)α=ke−βm+1−ke−i/Tmaxn,where(17)m=1.5−iTmax,n=1.5−β,where k∈0,1, m and n are customized control parameters.The expression of visual and step are determined as follows:(18)visual=visual×α+visualmin,(19)step=step×α+stepmin.From (18) and (19), we can see the large visual and step value in the beginning stage (m>1 and n>1) 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 Algorithm1.Algorithm 1: Modified artificial fish swarm algorithm.
(1)
Require: N: the population of the AFs(2)
Xi: the position state of AF i(3)
Visual: the maximum range in which AF can search(4)
Step: the maximum range AF can move(5)
Initialize the artificial fish population:N(6)
Initialize the state of each AF:Xi(7)
whilethe path is not found do(8)
for each AF do(9)
flag = stateEvaluation()(10)
caseflag = prey:(11)
Xit+1←Xit+steptowardsbestpotentialposition(12)
caseflag = follow:(13)
Xit+1←Xit+steptowardsneighborfishwithbetterposition(14)
caseflag = Swarm:(15)
Xit+1←Xit+steptowardscenterofthefishswarm(16)
caseflag = random:(17)
Xit+1←Xit+steptowardsrandomdirection(18)
end for(19)
Update the state of each AFXi(20)
Updatevisual and step(21)
end while(22)
route = selectBestRoute (Xi)(23)
Output: route


## 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 asXit, and Xjt is somewhere in the visual distance that the AF desires to move, which can be expressed as follows:(5)Xjt=Xit+Visual×rand0,1.If the stateXjt 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:(6)Xit+1=Xit+Xjt−XitXjt−Xit×Step×rand0,1.IfYi>Yj, then Xjt will be selected again. If the AF is not satisfied with the forward condition after trynum 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 Xi,jp represents the potential position number j of AF number i in the step range. P represents the set of all the potential positions, it can be expressed as follows:(7)P=Xi,jpXi,jp−Xi≤Step,i=1,2,3,…,N,j=1,2,…,M.Figure 2
Illustration of the behaviors. (a) Prey behavior. (b) Swarm behavior. (c) Follow behavior.
(a)(b)(c)In prey behavior, the food concentration of all the potential positions inP is calculated and then the best position Xi,bestp 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.(8)Xi,bestp=minfXi,1p,fXi,2p,fXi,3p,…,fXi,Mp,(9)Xit+1=Xit+Xi,bestpt−XitXi,bestpt−Xit×Step×rand0,1.


## 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 Figure2(b). Suppose the current position of AF is Xit, and the position Xjt denotes the neighbor fish with better food concentration in its visual scope. The swarm center Xct is defined as the arithmetic average of all AFs’ states, as shown in equation (10). nf is the number of AFs in the visual scope of the swarm center. δ denotes the parameter of the crowd factor. If Yj>Yi and nf/N<δ, which means the position Xjt has better food consistency and is not crowded. Then the AF moves a step in the direction of Xjt. The expression can be determined as follows:(10)Xit+1=Xit+Xjt−XitXjt−Xit×Step×rand0,1.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 Figure2(c). The central position is determined by the following equation:(11)Xct=1N∑i=1NXit.Denotenf as the number of AF swarms in the visual range of Xct. If nf/N<δ and Yc<Yi, 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 Xct, which is done by(12)Xit+1=Xit+Xct−XitXct−Xit×Step×rand0,1,otherwise, 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:(13)Xit+1=Xit+Step×rand0,1.


## 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:(14)β=mfavgfmaxXit+1−mfminXitfmaxXit,(15)favg=1N∑i=1NfXit,where favg is the mean fitness value of all artificial fish, fmaxXit and fminXit denote the optimum value and minimum value of food concentration. Then the adaptive factor α is given as follows:(16)α=ke−βm+1−ke−i/Tmaxn,where(17)m=1.5−iTmax,n=1.5−β,where k∈0,1, m and n are customized control parameters.The expression of visual and step are determined as follows:(18)visual=visual×α+visualmin,(19)step=step×α+stepmin.From (18) and (19), we can see the large visual and step value in the beginning stage (m>1 and n>1) 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 Algorithm1.Algorithm 1: Modified artificial fish swarm algorithm.
(1)
Require: N: the population of the AFs(2)
Xi: the position state of AF i(3)
Visual: the maximum range in which AF can search(4)
Step: the maximum range AF can move(5)
Initialize the artificial fish population:N(6)
Initialize the state of each AF:Xi(7)
whilethe path is not found do(8)
for each AF do(9)
flag = stateEvaluation()(10)
caseflag = prey:(11)
Xit+1←Xit+steptowardsbestpotentialposition(12)
caseflag = follow:(13)
Xit+1←Xit+steptowardsneighborfishwithbetterposition(14)
caseflag = Swarm:(15)
Xit+1←Xit+steptowardscenterofthefishswarm(16)
caseflag = random:(17)
Xit+1←Xit+steptowardsrandomdirection(18)
end for(19)
Update the state of each AFXi(20)
Updatevisual and step(21)
end while(22)
route = selectBestRoute (Xi)(23)
Output: route


## 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:(20)J=Js+Jc+Jd,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:(21)Js=∑i=1nΔψi2,where Δψi 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 si and si+1 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 κi,i and κi,i+1 are equal:(22)κi,i−κi,i+1=0.Therefore,Jc should be minimized:(23)Jc=κi,i−κi,i+1.The third termJd ensures the path keeps a sufficient distance from the obstacles. Suppose the distance between each path segment si to its nearest obstacle OobsiOobsi⊂Oobs is denoted as di=si,Oobsi,i=1,2,3,…,m. Then the path safety of each segment can be expressed as:(24)Di=0,di≥dmax,di−dmindmax−dmin,dmin<di<dmax,1,di≤dmin.ThenJd should be optimized as follows:(25)Jd=∑i=1mDi.Figure3 shows the scheme of the proposed method.Figure 3
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:(26)ξ˙=ucosψ−vsinψ,η˙=usinψ−vcosψ,ψ˙=r,u˙=m22m11vr−d11m11u−1m11Satuτu,v˙=−m11m22ur−d22m22v,r˙=−m11−m22m22uv−d33m33r+1m33Satrτr,where ξ,ηT is the position vector of the USV in the inertial frame I and ψ denotes the yaw angle; u,vT is the linear velocity vector in the B frame while r is the yaw rate; m11, m22, and m33 denote the inertial related parameters; and d11, d22, and d33 represent the damping related parameters. τu and τr are the control input of surge and yaw. Sat∗ represents the saturation function of the control signals, and it is described as follows [24]:(27)Sat∗x=τ∗max,x>τ∗max,x,τ∗min≤x≤τ∗max,τ∗min,x<τ∗min,where ∗ denotes u or r, and τ∗maxτ∗min are the maximum value and minimum value of τ∗, respectively. All the parameters related to the USV model are presented in Table 1.Table 1
USV parameters.
NameValuem11 (kg)25.8m22 (kg)33.8m33 (kg∗m2)2.7L (length: m)1.5W (width: m)0.25d11 (kg/s)2d22 (kg/s)7d33 (kg∗m2/s)0.5τ∗max47.5τ∗min6.4Apart 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 Figure4.Figure 4
The configuration of the USV model.


## 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)
Section4.1 presents the efficiency benchmark test between the MAFSA and other existing references to verify the effectiveness of the algorithms.(ii)
Section4.3 shows the path planning benchmark study to evaluate the solution quality of MAFSA with a local optimizer.(iii)
Section4.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; tryumber is 10; the population size is 50 for all the cases. The parameters of Hybrid A∗ and D∗ 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]:(28)SD=1100∑i=1100si−AVG2.Figure 5
(a) Complex scenario; (b) open water scenario.
(a)(b)Table 2
Environment settings of each scenario.
Map sizeStartGoalScenario 1 (complex map)10∗10(1, 1)(10, 10)Scenario 2 (complex map)10∗10(1, 1)(10, 10)Scenario 3 (complex map)20∗20(1, 1)(20, 20)Scenario 4 (complex map)20∗20(1, 1)(20, 20)Scenario 5 (open water)400∗400(175, 300)(150, 80)Scenario 6 (open water)400∗400(40, 340)(300, 240)Scenario 7 (open water)400∗400(285, 340)(390, 50)Scenario 8 (open water)400∗400(70, 350)(260, 250)Table3 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 Table3 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 Figure6 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.Table 3
Performance comparison between different methods for 8 scenarios.
Evaluation criteriaProposedStandard AFSAD∗ liteHybridA∗Scenario 1 (complex map)AVG travel distance (pixels)13.90016.24013.89015.070Time cost (s)0.0990.1370.1350.135Time SD (s)0.0120.0230.0280.026Scenario 2 (complex map)AVG travel distance (pixels)13.90016.24015.66013.890Time cost (s)0.1200.1460.1800.131Time SD (s)0.0140.0230.0240.026Scenario 3 (complex map)AVG travel distance (pixels)30.38035.07030.97031.800Time cost (s)0.3310.5360.5880.630Time SD (s)0.0210.430.0470.037Scenario 4 (complex map)AVG travel distance (pixels)27.79031.80029.21028.630Time cost (s)0.3060.5540.5760.623Time SD (s)0.0110.1150.0640.055Scenario 5 (open water)AVG travel distance (pixels)244.000258.800248.200249.300Time cost (s)1.2453.6098.1152.578Time SD (s)0.2100.7000.6400.560Scenario 6 (open water)AVG travel distance (pixels)395.600415.880408.960403.810Time cost (s)1.3524.5068.4702.610Time SD (s)0.3120.4410.6520.760Scenario 7 (open water)AVG travel distance (pixels)318.350344.180324.570326.330Time cost (s)1.1244.1248.7612.523Time SD (s)0.2120.4710.5720.778Scenario 8 (open water)AVG travel distance (pixels)359.000370.800360.010360.970Time cost (s)1.2355.2649.0211.812Time SD (s)0.3480.4730.6420.678Figure 6
Boxplot of the time consumption for each scenario. (a) Scenario 1. (b) Scenario 2. (c) Scenario 3. (d) Scenario 4. (e) Scenario 5. (f) Scenario 6. (g) Scenario 7. (h) Scenario 8.
(a)(b)(c)(d)(e)(f)(g)(h)Figures7 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 Figures7 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 Table5, 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.Table 4
Environment configuration.
ParameterValueScenario 1Map size (pixel)400∗300Start (pixel)(10, 260)Goal (pixel)(300, 90)Scenario 2Map size (pixel)400∗300Start (pixel)(100, 150)Goal (pixel)(260, 100)Figure 7
Path comparison of scenario 1.Figure 8
Path comparison of scenario 2.Table 5
Statistical measurements of the paths.
MethodsPerformanceScenario 1Scenario 2ProposedMinimum clearance (m)10.630 (О)10.440 (О)Smoothness (rad)2.2382.294D∗ liteMinimum clearance (m)0 (✕)1 (✕)Smoothness (rad)54.9785.816HybridA∗Minimum clearance (m)4.243 (✕)0 (✕)Smoothness (rad)10.1192.356RRT∗Minimum clearance (m)1.000 (✕)3.000 (✕)Smoothness (rad)5.7276.064Note. Safety distance is set as 10 m. О is shown if the safety requirement is satisfied.


### 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., HybridA∗ [25], D∗ lite [24], and RRT∗[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.Figure 9
Qizhen lake: N = 30.301956°, S = 30.299381°, W = 120.086141°, E = 120.088866°. (a) Satellite map; (b) concept map (the map size is precisely calculated by google map: height = 286 m, width = 261.9 m).
(a)(b)Table 6
Environment settings.
ValueMap size (pixels)261.9 (width)∗286 (height)Start (earth frame)(120.086671°E, 30.299859°N)Goal (earth frame)(120.088227°E, 30.301374°N)Start (local frame)(60 m, 60 m)Goal (local frame)(210 m, 220 m)Currents0.2 m/s, 20°Visualized and quantitative results are shown in Figure10 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 Figures10 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 Figure7 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 Figure12 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 Figure13, 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.Figure 10
Paths generated by the algorithms.Table 7
Quantitative results of simulation.
MethodsPerformanceMetricsProposedMinimum clearance (m)10.198 (О)Smoothness (rad)2.547HybridA∗Minimum clearance (m)2.236 (✕)Smoothness (rad)5.687D∗ liteMinimum clearance (m)0 (✕)Smoothness (rad)60.4757Note. Safety distance is set as 10 m. О is shown if the safety requirement is satisfied.Figure 11
Simulation results with USV model (a) tracking results of proposed method; (b) tracking results of standard AFSA; (c) tracking results of HybridA∗; and (d) tracking results of D∗ lite.
(a)(b)(c)Figure 12
Course angle and speed of the USV model, (a) tracking results of proposed method; (b) tracking results of HybridA∗; and (c) tracking results of D∗ lite.
(a)(b)(c)Figure 13
Propeller command of the USV model, (a) propeller command of proposed method; (b) propeller command of HybridA∗; and (c) propeller command of D∗ lite.
(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 Figure9(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.


## 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; tryumber is 10; the population size is 50 for all the cases. The parameters of Hybrid A∗ and D∗ 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]:(28)SD=1100∑i=1100si−AVG2.Figure 5
(a) Complex scenario; (b) open water scenario.
(a)(b)Table 2
Environment settings of each scenario.
Map sizeStartGoalScenario 1 (complex map)10∗10(1, 1)(10, 10)Scenario 2 (complex map)10∗10(1, 1)(10, 10)Scenario 3 (complex map)20∗20(1, 1)(20, 20)Scenario 4 (complex map)20∗20(1, 1)(20, 20)Scenario 5 (open water)400∗400(175, 300)(150, 80)Scenario 6 (open water)400∗400(40, 340)(300, 240)Scenario 7 (open water)400∗400(285, 340)(390, 50)Scenario 8 (open water)400∗400(70, 350)(260, 250)Table3 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 Table3 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 Figure6 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.Table 3
Performance comparison between different methods for 8 scenarios.
Evaluation criteriaProposedStandard AFSAD∗ liteHybridA∗Scenario 1 (complex map)AVG travel distance (pixels)13.90016.24013.89015.070Time cost (s)0.0990.1370.1350.135Time SD (s)0.0120.0230.0280.026Scenario 2 (complex map)AVG travel distance (pixels)13.90016.24015.66013.890Time cost (s)0.1200.1460.1800.131Time SD (s)0.0140.0230.0240.026Scenario 3 (complex map)AVG travel distance (pixels)30.38035.07030.97031.800Time cost (s)0.3310.5360.5880.630Time SD (s)0.0210.430.0470.037Scenario 4 (complex map)AVG travel distance (pixels)27.79031.80029.21028.630Time cost (s)0.3060.5540.5760.623Time SD (s)0.0110.1150.0640.055Scenario 5 (open water)AVG travel distance (pixels)244.000258.800248.200249.300Time cost (s)1.2453.6098.1152.578Time SD (s)0.2100.7000.6400.560Scenario 6 (open water)AVG travel distance (pixels)395.600415.880408.960403.810Time cost (s)1.3524.5068.4702.610Time SD (s)0.3120.4410.6520.760Scenario 7 (open water)AVG travel distance (pixels)318.350344.180324.570326.330Time cost (s)1.1244.1248.7612.523Time SD (s)0.2120.4710.5720.778Scenario 8 (open water)AVG travel distance (pixels)359.000370.800360.010360.970Time cost (s)1.2355.2649.0211.812Time SD (s)0.3480.4730.6420.678Figure 6
Boxplot of the time consumption for each scenario. (a) Scenario 1. (b) Scenario 2. (c) Scenario 3. (d) Scenario 4. (e) Scenario 5. (f) Scenario 6. (g) Scenario 7. (h) Scenario 8.
(a)(b)(c)(d)(e)(f)(g)(h)Figures7 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 Figures7 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 Table5, 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.Table 4
Environment configuration.
ParameterValueScenario 1Map size (pixel)400∗300Start (pixel)(10, 260)Goal (pixel)(300, 90)Scenario 2Map size (pixel)400∗300Start (pixel)(100, 150)Goal (pixel)(260, 100)Figure 7
Path comparison of scenario 1.Figure 8
Path comparison of scenario 2.Table 5
Statistical measurements of the paths.
MethodsPerformanceScenario 1Scenario 2ProposedMinimum clearance (m)10.630 (О)10.440 (О)Smoothness (rad)2.2382.294D∗ liteMinimum clearance (m)0 (✕)1 (✕)Smoothness (rad)54.9785.816HybridA∗Minimum clearance (m)4.243 (✕)0 (✕)Smoothness (rad)10.1192.356RRT∗Minimum clearance (m)1.000 (✕)3.000 (✕)Smoothness (rad)5.7276.064Note. Safety distance is set as 10 m. О is shown if the safety requirement is satisfied.


## 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., HybridA∗ [25], D∗ lite [24], and RRT∗[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.Figure 9
Qizhen lake: N = 30.301956°, S = 30.299381°, W = 120.086141°, E = 120.088866°. (a) Satellite map; (b) concept map (the map size is precisely calculated by google map: height = 286 m, width = 261.9 m).
(a)(b)Table 6
Environment settings.
ValueMap size (pixels)261.9 (width)∗286 (height)Start (earth frame)(120.086671°E, 30.299859°N)Goal (earth frame)(120.088227°E, 30.301374°N)Start (local frame)(60 m, 60 m)Goal (local frame)(210 m, 220 m)Currents0.2 m/s, 20°Visualized and quantitative results are shown in Figure10 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 Figures10 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 Figure7 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 Figure12 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 Figure13, 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.Figure 10
Paths generated by the algorithms.Table 7
Quantitative results of simulation.
MethodsPerformanceMetricsProposedMinimum clearance (m)10.198 (О)Smoothness (rad)2.547HybridA∗Minimum clearance (m)2.236 (✕)Smoothness (rad)5.687D∗ liteMinimum clearance (m)0 (✕)Smoothness (rad)60.4757Note. Safety distance is set as 10 m. О is shown if the safety requirement is satisfied.Figure 11
Simulation results with USV model (a) tracking results of proposed method; (b) tracking results of standard AFSA; (c) tracking results of HybridA∗; and (d) tracking results of D∗ lite.
(a)(b)(c)Figure 12
Course angle and speed of the USV model, (a) tracking results of proposed method; (b) tracking results of HybridA∗; and (c) tracking results of D∗ lite.
(a)(b)(c)Figure 13
Propeller command of the USV model, (a) propeller command of proposed method; (b) propeller command of HybridA∗; and (c) propeller command of D∗ lite.
(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 Figure9(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.



---

*Source: 1283374-2022-11-18.xml*