Physical Consistent Path Planning for Unmanned Surface Vehicles under Complex Marine Environment

: The increasing demand for safe and efﬁcient maritime transportation has underscored the necessity of developing effective path-planning algorithms for Unmanned Surface Vehicles (USVs). However, the inherent complexities of the ocean environment and the non-holonomic properties of the physical system have posed signiﬁcant challenges to designing feasible paths for USVs. To address these issues, a novel path planning framework is elaborately designed, which consists of an optimization model, a meta-heuristic solver, and a Clothoid-based path connector. First, by encapsulating the intricate nature of the ocean environment and ship dynamics, a multi-objective path planning problem is designed, providing a comprehensive and in-depth portrayal of the underlying mechanism. By integrating the principles of the candidate set random testing initialization and adaptive probability set, an enhanced genetic algorithm is devised to fully exploit the underlying optimization problem in constrained space, contributing to the global searching ability. Accounting for the non-holonomic constraints, the fast-discrete Clothoid curve is capable of maintaining and improving the continuity of the path curve, thereby promoting strong coordination between the planning and control modules. A thorough series of simulations and comparisons conducted in diverse ocean scenarios has conclusively demonstrated the effectiveness and superiority of the proposed path planning framework.


Introduction
With artificial intelligence at the helm, the advent of unmanned surface vehicles (USVs) has garnered significant attention, fueled by their potential to revolutionize maritime operations by enhancing safety and efficiency [1][2][3][4][5][6]. However, the successful deployment of USVs depends on the development of autonomous technology, which refers to the ability of these vehicles to plan and execute their missions in complex environments without human intervention, thereby enabling safe and efficient navigation [7,8]. Generally speaking, the most common approaches that contribute to the autonomous level of USVs are perception, localization and mapping, path planning and decision-making, and control system design. Central to achieving autonomy in USVs is the challenge of path planning, which involves determining an optimal path for the vehicle to traverse in order to accomplish its mission objectives while adhering to a set of predetermined rules and regulations. Compared to other types of autonomous vehicles, such as unmanned ground vehicles (UGVs), USV path planning may incorporate specialized techniques to handle challenges, such as wave prediction models, collision avoidance strategies for vessels, or algorithms, that account for hydrodynamic effects on the vehicle's motion. This task is particularly challenging due to the dynamic nature of maritime environments, which are subject to 2 of 21 constantly changing weather conditions, currents, and other environmental factors that can impact navigation [9]. Achieving this goal requires the development of sophisticated path planning algorithms that enable these vehicles to navigate complex environments with minimal human intervention, paving the way for a future of safe and efficient maritime navigation [10].
The field of path planning for USVs has been an active area of research in recent years, with numerous studies investigating the development of effective planning strategies for USVs. In general, two primary categories of path planning algorithms have been proposed: global approaches and local approaches [7]. Global approaches involve the generation of a complete path for the USV based on prior knowledge of the environment, usually represented as a map. Such methods typically employ high-level planning techniques that treat the USV as a point object, neglecting its maneuverability and physical constraints. These methods are, therefore, more suitable for planning routes for long-distance voyages, where the emphasis is on efficient and safe navigation over extended periods. In contrast, local approaches generate a path by utilizing local information collected during the mission, enabling the USV to adapt to unexpected obstacles or changes in the environment. These methods fully consider the physical bounds of the USV's mechanical system, leading to more precise tracking performance for the low-level controller. Although the design of such methods is generally more complex, as it requires the integration of high-level planning and low-level control techniques to ensure effective operation, it is more applicable in practice.
Presently, there is a growing affinity for deterministic approaches in path planning, with various methods, such as A* and D* lite, basking in the limelight of scientific popularity. In particular, Yu and Wang [11] have put forward a hybrid algorithm that fuses artificial potential field (APF) and D* lite to navigate complex environments. This approach not only minimizes time cost but also enhances path safety through the APF. Nonetheless, it overlooks disturbances and energy consumption. Similarly, Yu et al. [12] have proposed an improved D* lite that reduces expanded nodes, validated via simulations. However, the simulation fails to consider ship dynamics, smoothness, and safety. Meanwhile, Song et al. [13] have utilized various smoothing techniques to mitigate the jagged effect in A*, which has been demonstrated to be effective through both experiments and simulations, making it a practical choice. Furthermore, Shah and Gupta [14] have presented a quadtree representation of the marine environment, which accelerates the A* algorithm without significantly sacrificing solution optimality, as shown in simulations. To facilitate path planning for working ships in offshore wind farms, Xie et al. [15] devised a multi-direction A* algorithm modified by an artificial potential field. Compared with the real-case trajectory, the minimum distance to the wind turbines has increased, and the path length outside the wind farm decreased dramatically. To solve the path planning problem under changing environments with multiple dynamic obstacles, Yao et al. [16] proposed an Improved D* lite algorithm, which has demonstrated its efficacy in real-time path planning through simulation results. Although deterministic approaches have emerged as popular and reliable methods for path planning, these methods can be computationally expensive, particularly when operating in large and complex environments. This can have a significant impact on their performance, making them less suitable for real-time applications.
As a result, the meta-heuristic algorithms, including ant colony optimization (ACO) [17], particle swarm optimization (PSO) [18], and genetic algorithms (GA) [19], have emerged as promising alternatives for path planning in marine robotics. These algorithms offer a set of high-level strategies to search for solutions, allowing them to optimize paths while considering multiple objectives with a comparatively low computational burden. Considering the effects of currents, Krell et al. [20] devised an improved PSO method implemented in visibility graphs. For the safe navigation of ships, a quasi-reflection-based PSO was proposed by Xue [21]. Incorporating the environmental loads, a hierarchical path planning framework based on GA is developed by Wang and Xu [10]. For rapid path generation, a leader-vertex ant colony optimization algorithm is proposed by Liang et al. [22], which ensures a leader of the ant colony and optimizes the route by vertex method. For both global and local path planning, a series of studies on artificial fish swarm algorithms have been conducted by Zhao et al. [7,8,23]. Under different current distribution, a comprehensive study has been conducted by Ma et al. [24] using multi-objective dynamic augmented PSO. Organically bridging the planning and tracking, Wang et al. [25] devised an elite-duplication GA (EGA) strategy to optimally generate sparse waypoints in a constrained space. However, meta-heuristic algorithms commonly encounter a significant hurdle with regard to their global searching capabilities, as they are prone to be trapped in local minima or suboptimum, thereby impeding the identification of the global optimum necessary for producing high-quality trajectories. Additionally, the computational efficiency of existing methods is not satisfactory enough to facilitate efficient path generation within high-dimensional configuration spaces [26]. Therefore, there is an imperative need for innovative techniques that can enhance the global searching capability and convergence rate of meta-heuristic algorithms for path planning.
In addition, a noteworthy limitation of most existing methods is the neglect of the non-holonomic constraints of the vehicle, which can lead to paths that are potentially infeasible. Specifically, the USV, being a non-holonomic robot, often functions as an underactuated system during its missions, resulting in limited maneuverability and motion flexibility [27,28]. Analogous to unmanned ground vehicles (UGVs), this restricts the USV's motion to forward velocity and manipulation of the heading angle to attain its desired position, thus precluding lateral movement. Consequently, it is of paramount importance to ensure smooth and continuous transitions of yaw and curvatures at turning points in order to devise an effective trajectory for a USV. For instance, sharp turns may be deemed unfeasible for a USV due to the significant sideslip that ensues, deviating from the planned path. Thus, the motion dynamics of the USV should be meticulously accounted for in the path planning process [10].
Inspired by the aforementioned literature review, this paper proposes a novel GAvariant meta-heuristic algorithm in combination with a fast-discrete Clothoid curve to optimize path generation. The main contributions of this paper are illustrated as follows: • By capturing the non-holonomic nature of USVs and the intricate ocean dynamics, a sophisticated optimization model is carefully devised for the path planning problem, whereby the effects of currents, increments of curvatures, and constraints of physical system are addressed jointly.

•
Introducing the random testing initialization algorithm and the adaptive design in the selection procedure, the proposed GA-variant facilitates strong global searching capabilities and a fast convergence rate, thereby contributing to the optimal generation of waypoint sequence. • Accommodating the non-holonomic constraints, the fast-discrete Clothoid curve is able to preserve and enhance the continuity of the path curve, resulting in robust coordination between the planning and control module.
This paper is organized in the following structures: Section 2 presents the detailed modeling of the environment, USV, and the optimization model. Section 3 introduces the methodology. Illustrative simulation results are shown in Section 4. The conclusion is drawn in Section 5.

Environment Model
In this research, we consider the marine surface area represented as M in the Euclidean space R 2 . M is divided into obstacle area M o and obstacle-free motion area M f , respectively. The relationship between these two grids is illustrated in Equation (1). The path P of the USV is defined as a sequence of connected elementary waypoints, denoted by p i (i = 1, 2, 3, . . . , m). By following the path P, the USV moves from the initial position p S (x S , y S ) to the final position p E (x E , y E ) while avoiding numerous obstacle areas Accordingly, to guarantee the safety, the generated path should be restricted to M f domain which is given as:

Currents Model
According to previous research [20], ocean currents have a significant impact on the energy consumption of USVs. Therefore, when carrying out activities, USVs tend to choose a path that allows them to take advantage of the currents. Two types of currents exist: fixed and time-varying. Fixed currents are common in inland water voyages while time-varying currents are present in large-scale ocean environments. Assuming that the velocity of the USV in the body frame is v = [u, v, r] T and the current velocity in the body frame is v c , the USV velocity, taking the effects of the currents into account, can be expressed as:

USV Model
Typically, the motion of the USV can be regarded as a rigid body motion on the horizontal plane, as shown in Figure 1a, with three degrees of freedom: surge, sway, and yaw. Consequently, the state-space model for the USV, accounting for the impact of the current, can be derived as follows: where the position and yaw angle in the earth-fixed inertial frame {n} are represented by the vector η = [x, y, ψ] T , while the relative velocities in the body-fixed frame {b} are denoted by v r = [u r , v r , r] T , and the control signals are represented by the vector τ = [τ u , 0, τ r ] T . This paper considers the underactuated configuration of the USV, which means that the surge force and yaw moment are the only control forces. With these assumptions, the rotation matrix R(ψ), mass matrix M, Coriolis matrix C, and damping matrix D can be expressed as: To represent the USV's orientation relative to the earth-fixed inertial frame, we use a rotation matrix R(ψ) that transforms the body-fixed frame. The mass matrix M = M T > 0 takes into account the USV's inertial properties and hydrodynamic added mass. The matrix D incorporates damping coefficients, while the Coriolis matrix C captures the Coriolis and centripetal effects and can be obtained from M.
Moreover, the turning angle at any point on the path must be restricted in the dynamic bounds, which gives:

Cruising Time
Since the path length and energy consumption objectives are interdependent, modifying the design variables affects both objectives equally. To jointly represent these objectives, we have adopted the cruising time t as the first objective. Let be the velocity of the USV at and be the current velocity. The resultant velocity is denoted by = + . The cruising time between and can be calculated as follows: Then, the total cruising time is calculated by: Due to its non-holonomic nature, a USV typically has limited maneuverability and motion flexibility during most operations [28]. Similar to UGVs, the non-holonomic constraint restricts its lateral motion, meaning that the USV can only use its forward velocity while adjusting the heading angle to reach a desired position.
In Figure 1b, T denote the position vector between two points, and p i and p i+1 represent the waypoints. The angles between d i and p i and p i+1 are defined as b i, i and b i, i+1 , respectively. In order to ensure a continuous path, the straight line and turning motions require that two consecutive positions p i and p i+1 lie on a common arc of constant curvature, which can be expressed as: Moreover, the turning angle at any point on the path must be restricted in the dynamic bounds, which gives:

Cruising Time
Since the path length and energy consumption objectives are interdependent, modifying the design variables affects both objectives equally. To jointly represent these objectives, we have adopted the cruising time t as the first objective. Let v i be the velocity of the USV at p i and v c be the current velocity. The resultant velocity is denoted by v r = v i + v c . The cruising time t i between p i and p i+1 can be calculated as follows: Then, the total cruising time is calculated by: The calculation of v c is performed using the current distribution function. In most cases, v i is considered to be a constant value in the same direction as p i . Therefore, t i represents the nominal cost of travel time and does not accurately reflect the actual travel time.

Smoothness and Continuity
The additional cost incurred due to yaw is closely linked to the motion control performance of the USV. In order to enhance the smoothness of the trajectory, an objective function is introduced. To achieve this, constraint (9) is added as a quadratic penalty term to the objective function. The turning angle ∆ψ i between waypoints p i and p i−1 in the path P is calculated as ). The smoothest path is achieved when the changes in ∆ψ i (i = 2, 3, . . . , m) and curvature |b i−1, i−1 − b i−1, i | are minimized. Hence, the objective function for achieving the smoothest path is defined as:

Path Safety
To ensure the safe movement of the USV, it is crucial to find a collision-free path that also maintains a safe distance from the obstacles. In addition to satisfying the conditions outlined in Equation (2), the minimum clearance from obstacles, denoted as d i , is used to determine the safety of the solution. Specifically, we define two circular areas with radii d min and d max around each path waypoint p i . The distance between each path waypoint p i and its closest obstacle The safety of each point along the path can then be evaluated by: Therefore, to ensure the safety of the path, the third objective is to minimize the minimum value of D i , see the following:

Problem Statement
The path planning model for the problem is formed as:

Adaptive-Elite Genetic Algorithm
The genetic algorithm (GA) was initially proposed by Professor J. Holland in 1973 as a meta-heuristic optimization method. By simulating the evolutionary process of an artificial population, the GA manipulates each individual in the population through genetic operations, such as selection, crossover, and mutation. The process generates a new population with the best-performing individuals from the previous generation as the parents. The population evolves through several generations, and the individuals with the best fitness values are selected as the optimal solutions.
The GA's strength lies in its ability to search a large solution space using stochastic searches and evolutionary operations, such as crossover and mutation, making it effective in handling non-linear and non-convex optimization problems. Moreover, the GA's population size enables it to mitigate the impact of hyperparameter selection by allowing the algorithm to sample from a diverse set of solutions. Given that the optimization problem presented in this paper is an NP-hard nonlinear problem, we choose the GA as the primary framework.

Chromosome Representation
In evolutionary algorithms, chromosomes can be represented in various ways, such as binary-coded, real-coded, and decimal-coded. In our paper, we utilized the real-coded chromosome to directly represent the USV's path. Specifically, we use a sequence of points that begins at an origin position and ends at a destination point. Each point, denoted as p i = (x i , y i ), is saved along with its x and y coordinates and a pointer to the next point in the path. Figure 2 illustrates this representation.

Adaptive-Elite Genetic Algorithm
The genetic algorithm (GA) was initially proposed by Professor J. Holland in 1973 as a meta-heuristic optimization method. By simulating the evolutionary process of an artificial population, the GA manipulates each individual in the population through genetic operations, such as selection, crossover, and mutation. The process generates a new population with the best-performing individuals from the previous generation as the parents. The population evolves through several generations, and the individuals with the best fitness values are selected as the optimal solutions.
The GA's strength lies in its ability to search a large solution space using stochastic searches and evolutionary operations, such as crossover and mutation, making it effective in handling non-linear and non-convex optimization problems. Moreover, the GA's population size enables it to mitigate the impact of hyperparameter selection by allowing the algorithm to sample from a diverse set of solutions. Given that the optimization problem presented in this paper is an NP-hard nonlinear problem, we choose the GA as the primary framework.

Chromosome Representation
In evolutionary algorithms, chromosomes can be represented in various ways, such as binary-coded, real-coded, and decimal-coded. In our paper, we utilized the real-coded chromosome to directly represent the USV's path. Specifically, we use a sequence of points that begins at an origin position and ends at a destination point. Each point, denoted as = ( , ), is saved along with its x and y coordinates and a pointer to the next point in the path. Error! Reference source not found. illustrates this representation.

Initialization
The original population for the algorithm is obtained through population initialization. In the case of the original GA, a certain number of solutions are randomly generated in the solution space without the use of a heuristic function. This can lead to a random and unfocused solving process, resulting in a high proportion of poor solutions and low-quality genes in the population. This, in turn, requires a long convergence time during subsequent evolution and makes the solving process prone to being trapped in a local optimum.
To address this issue, we have developed a modified initialization method for GA inspired by failure analysis techniques used in software systems. Specifically, we have incorporated a candidate set adaptive random testing (ART) approach to improve the diversity of the initial population. By enhancing the initial diversity, the ART-based initialization method allows the GA to explore a broader range of potential solutions. This exploration can improve the algorithm's ability to escape local optima and discover better solutions in the search space. Consequently, it enhances the chances of finding high-quality solutions and can potentially accelerate the convergence toward optimal or near-optimal solutions. In summary, compared to standard random initialization, the ART-based initialization method in the GA offers the advantage of generating an initial population that is more diverse and better distributed throughout the search space. This increased diversity can facilitate improved exploration of the solution space and potentially lead to better overall performance and convergence in the GA.
The main steps of the initialization process are illustrated as follows: Step 1: m candidate individuals C = c 1 , c 2 , . . . , c m are randomly generated.
Step 2: The objective distances between each candidate individual and the current individuals in the population set P = p 1 , p 2 , . . . , p n are calculated.
Step 3: The shortest distance between each candidate individual and the population set is identified.
Step 4: The candidate individual with the maximum distance value is selected and added to the population set P.

Selection Operator
In a genetic algorithm, the selection operator is responsible for choosing individuals from a population for the crossover operator. This selection process is carried out based on a predefined regulation called the Roulette Wheel Selection (RWS) method. To perform Roulette Wheel Selection (RWS) in a genetic algorithm (GA), first, we compute the fitness values for each individual in the population and normalize them to obtain probabilities. Then, calculate the cumulative probabilities by summing up the normalized fitness values. After that, generate a random number between 0 and 1, and select individuals whose cumulative probability exceeds this random number. Repeat the selection process as needed to obtain the desired number of parents. Use the selected parents for crossover or recombination to generate offspring for the next generation. This iterative process allows individuals with higher fitness to have a greater chance of being selected, promoting the propagation of favorable traits in the GA. The RWS method ensures that the fitter individuals have a higher chance of being selected for the crossover, thus improving the overall quality of the population in the subsequent generation. The selection probability of each individual can be expressed as follows: where x i denotes the individual and f (x i ) is the corresponding fitness value. As can be seen from Equation (15), better individuals have more chances to be selected by RWS, which leads to better solutions.

Hybrid Crossover
Crossover operators are utilized to combine two solutions and generate a new offspring with better performance in terms of a predefined objective. These operators can be applied to solutions with the same or different number of waypoints. The first crossover operator involves calculating the mean of the two parent solutions to produce a new offspring.
where two parents have gene coordinates denoted by x To enhance the variability of the population and explore the entire available space, the second crossover operator is utilized in which the two parents are randomly merged: where vector k consists of random numbers ranging from −1 to 1. When the number of genes differs between the parents, a similar approach to the first operator is used to combine genes with minimal distance. The primary aim of the first operator is to escape local optima, while the second one explores the environment randomly, preventing premature convergence.

Mutation Operator
The mutation operator in a Genetic Algorithm has a critical function in maintaining diversity within the population. The primary goal of the mutation operator is to randomly modify the value(s) of one or more genes within an individual's chromosome. By introducing such changes, the mutation operator assists in preventing the GA from becoming trapped in a local optimum, which would hinder the search for the global optimum. In the absence of the mutation operator, the GA may converge to a suboptimal solution that is in close proximity to the initial population. Hence, mutation serves as a crucial component of GA by promoting exploration of the search space and preventing premature convergence to suboptimal solutions. This paper introduces two mutation operators to facilitate the genetic process: The first operator is a random mutation that selects one position on chromosomes and changes the value in the free space as shown in the following figure: A different mutation operator is utilized to enhance the path's smoothness and length by adjusting the position of a gene. The operator integrates the present position (p i ) of a gene with the directions towards the genes located on either side, p i−1 and p i+1 , using the subsequent expressions: where m and n are random positive coefficients from 0 to 1. As illustrated in Figure 3b, this mutation operator leads to the creation of paths with shorter lengths and better smoothness. Combining both mutation operators result in a powerful tool that enhances both the searching and convergence capabilities of the algorithm.  In this paper, a multi-objective fitness function is devised. For this purpose, a weighted linear combination of the mentioned objectives is considered: The formula involves several parameters, including (cruising time), ϑ (smoothness objective), and (safety level), where w , w , and w represent weight values and their sum is equal to 1. To maintain consistency in the indicators' magnitudes, coefficients , , and have been set to 0.1, 1, and 100, respectively, as shown in the equation.

Fitness Design
In this paper, a multi-objective fitness function is devised. For this purpose, a weighted linear combination of the mentioned objectives is considered: The formula involves several parameters, including T (cruising time), ϑ (smoothness objective), and D (safety level), where w t , w θ , and w d represent weight values, and their sum is equal to 1. To maintain consistency in the indicators' magnitudes, coefficients c 1 , c 2 , and c 3 have been set to 0.1, 1, and 100, respectively, as shown in the equation.
Selecting appropriate weight values is a vital aspect of the algorithm's performance. However, relying solely on empirical methods can be subjective. In an effort to achieve more balanced results, the Delphi weighting method [19] was employed to determine the weight of each indicator. As a result, the weight coefficients for cruising time, smoothness, and safety are 0.395, 0.275, and 0.330, respectively.

Determination of p c and p m
The conventional Genetic Algorithm for USV path planning relies on two essential parameters, namely, the crossover rate and mutation rate, to regulate p c and p m of individuals in each iteration. However, using fixed values for these parameters may pose certain challenges. For instance, employing a large crossover and mutation probability can make it difficult to retain the best individuals, slow down population convergence, and consequently, delay the generation of the inspection path, thereby impacting operational efficiency. Conversely, a small p c and p m can negatively affect the searching process, leading to the local optimum. This, in turn, causes the USV to travel longer distances, reducing its efficiency.
To tackle the aforementioned challenges, a modified approach is suggested. This approach involves adjusting p c and p m during the algorithm execution. Specifically, in the early stages of the algorithm, p c and p m are increased to improve the global search ability, while in the final stage, the probabilities are decreased to facilitate good convergence. Adaptive probabilities allow the GA to dynamically adjust the rates of crossover and mutation based on the progress of the algorithm. Initially, higher probabilities promote exploration by encouraging diverse offspring. As the GA progresses, the probabilities can be reduced, shifting the focus towards exploitation of promising solutions. This balance between exploration and exploitation helps the GA efficiently search for optimal or nearoptimal solutions.
To achieve this, adaptive functions are formulated as follows: where p c0 represents the initial p c , a is the scaling coefficient, and F here is the average fitness of the population. Similarly, the mutation probability can be obtained with same structure as follows: where p m0 represents the initial p m and b is the scaling coefficient. These functions dynamically adjust the crossover probability based on the mean fitness degree of the population at each generation. As a result, the USV can achieve a balance between exploration and exploitation, leading to faster convergence and better results.

Fast-Discrete Clothoid Curve
To ensure real-time performance and accommodate the USV's kinematic constraints, we introduce a Fast-Discrete Clothoid Path (FDCP) to construct and connect the path. The FDCP employs a sequence of control points, referred to as waypoints, which are linked together using Clothoid segments. However, accurately generating Clothoids can be difficult due to their non-linear nature and multiple solutions. Thus, instead of directly computing the parameters of the Clothoid segments, our algorithm utilizes a variational approach that produces a polyline with linear discrete curvature, which approximates the Clothoid segment. This approach allows for efficient and precise path planning for the USV, while taking into account the vehicle's non-holonomic features.
To determine the position of intersection points, the following conditions must be met when inserting or updating point C between neighboring points B and D, as shown in Figure 4. To simplify the calculations, a normalized configuration is used where point B is located at (−1, 0) and point D at (1, 0). For each of the five control points denoted as P, its left and right neighbors are identified as P l and P r , respectively. The insertion point C must satisfy the following conditions to approximate the Clothoids accurately between these control points: • C must lie on the perpendicular bisector between B and D.

•
The curvature at each point should vary linearly.

| | + | |
As more and more points are inserted, the polyline gets refined and the angl tween segments approach . Therefore, with a large number of sample points, w approximate | | = | | = 1. Solving Equation (14), can be obtained by:

= (| | + 1) + (| | + 1) 2| || | + 3(| | + | |) + 4
Point C is now inserted on the perpendicular bisector between B and D in dis | | tan . By iteratively inserting the intersection points (such as point C), we can ap imate the Clothoid path with satisfactory computational performance. Clothoid curves provide a continuous change in curvature, resulting in a sm transition between straight segments and curved segments of a path. This helps r abrupt changes in the path and improves the vehicle's stability and comfort. More by gradually changing the curvature, Clothoid curves minimize lateral acceleration ing turns. This reduces the forces acting on the USV, enhancing safety and stability d maneuvering. Additionally, Clothoid curves enable more precise and controlled m vering. They allow for gradual changes in heading angle, facilitating smooth turns transitions between different paths or waypoints.
The flowchart of the methodology is illustrated by Error! Reference sourc found.. From the abovementioned conditions, we have the following equation: where ρ denote the curvature at each point, it can be approximated by: where φ P is the angle between |P l P| and |PP l |, and |P l P| is the scalar value of the length between P l and P. According to the geometric relations in Figure 4, we obtain: Substituting φ P in previous equation using Equation (16), we have: As more and more points are inserted, the polyline gets refined and the angles between segments approach π. Therefore, with a large number of sample points, we can approximate |BC| = |CD| = 1. Solving Equation (14), γ can be obtained by: Point C is now inserted on the perpendicular bisector between B and D in distance |CD| tan γ. By iteratively inserting the intersection points (such as point C), we can approximate the Clothoid path with satisfactory computational performance.
Clothoid curves provide a continuous change in curvature, resulting in a smooth transition between straight segments and curved segments of a path. This helps reduce abrupt changes in the path and improves the vehicle's stability and comfort. Moreover, by gradually changing the curvature, Clothoid curves minimize lateral acceleration during turns. This reduces the forces acting on the USV, enhancing safety and stability during maneuvering. Additionally, Clothoid curves enable more precise and controlled maneuvering. They allow for gradual changes in heading angle, facilitating smooth turns, and transitions between different paths or waypoints.
The flowchart of the methodology is illustrated by Figure 5.

Results and Discussion
In this section, illustrative simulations have been carried out to evaluate the performance of our proposed method progressively through convergence test and simulation under a time-varying environment. The simulations are conducted via MATLAB 2021a environment with a PC configured with Intel (R) Core (TM) i7-13400 CPU and 16-GB RAM.

Convergence and Quality Test
In this section, simulations have been carried out to analyze the convergence characteristic of our proposed method. We have selected some other state-of-the-art methods from existing reliable references for comparison, including conventional genetic algorithm, D* lite [16], Hybrid A* [29], and RRT* [30]. The selected environment maps are presented in Error! Reference source not found., the start and goal points are marked as blue and red dots, respectively. It is worth noting that since we only test the convergence behavior and solution quality, the effects of time-varying currents are not considered. To maintain the efficiency and without loss of solution quality, we set the maximum number of waypoints in each path is 20 according to [31]. Error! Reference source not found. shows the dimensions and coordinates of the given points.
The parameters are set as follows: • In this case, the ocean current is fixed with velocity of 0.3 m/s and direction of −70°.

Results and Discussion
In this section, illustrative simulations have been carried out to evaluate the performance of our proposed method progressively through convergence test and simulation under a time-varying environment. The simulations are conducted via MATLAB 2021a environment with a PC configured with Intel (R) Core (TM) i7-13400 CPU and 16-GB RAM.

Convergence and Quality Test
In this section, simulations have been carried out to analyze the convergence characteristic of our proposed method. We have selected some other state-of-the-art methods from existing reliable references for comparison, including conventional genetic algorithm, D* lite [16], Hybrid A* [29], and RRT* [30]. The selected environment maps are presented in Figure 6, the start and goal points are marked as blue and red dots, respectively. It is worth noting that since we only test the convergence behavior and solution quality, the effects of time-varying currents are not considered. To maintain the efficiency and without loss of solution quality, we set the maximum number of waypoints in each path is 20 according to [31]. Table 1 shows the dimensions and coordinates of the given points.  The tabulated data presented in Error! Reference source not found. provides quantitative results for the proposed algorithm. The results reveal that the algorithm exhibits a significant reduction in time cost with 0.36 s, 0.613 s, 0.484 s, and 0.391 s for the four scenarios, respectively. This represents a considerable improvement of over 60% when compared to the traditional GA. The algorithm's increased speed is primarily due to the new initialization process. Additionally, the algorithm's robustness is evaluated through the standard deviation (SD) of the time cost, which is 0.012 s, 0.022 s, 0.022 s, and 0.008 s for each case, respectively. Furthermore, the proposed algorithm is shown to provide a satisfactory minimum path length of 253.5 pixels, 352.0 pixels, 356.0 pixels, and 298.0 pixels for each scenario, respectively. Although other methods may produce slightly smaller values in some cases, the proposed algorithm provides more practical and reasonable solutions. It is important to note that the relatively low path lengths produced by D* lite and Hybrid A* are due to their reliance on optimal search based on A*, which aims for the shortest path. However, this approach often results in paths that are too close to obstacles.  The tabulated data presented in Table 2 provides quantitative results for the proposed algorithm. The results reveal that the algorithm exhibits a significant reduction in time cost with 0.36 s, 0.613 s, 0.484 s, and 0.391 s for the four scenarios, respectively. This represents a considerable improvement of over 60% when compared to the traditional GA. The algorithm's increased speed is primarily due to the new initialization process. Additionally, the algorithm's robustness is evaluated through the standard deviation (SD) of the time cost, which is 0.012 s, 0.022 s, 0.022 s, and 0.008 s for each case, respectively. Furthermore, the proposed algorithm is shown to provide a satisfactory minimum path length of 253.5 pixels, 352.0 pixels, 356.0 pixels, and 298.0 pixels for each scenario, respectively. Although other methods may produce slightly smaller values in some cases, the proposed algorithm provides more practical and reasonable solutions. It is important to note that the relatively low path lengths produced by D* lite and Hybrid A* are due to their reliance on optimal search based on A*, which aims for the shortest path. However, this approach often results in paths that are too close to obstacles. The visualized results of the simulations are presented in Figure 7. The red curve depicts the smoothed path generated by the proposed method. The results demonstrate that the curve is smooth without any abrupt turns. On the other hand, the results generated by GA, D* lite, Hybrid A*, and RRT* exhibit relatively large angle changes, particularly RRT*, which exhibits the poorest performance in terms of smoothness. Furthermore, the proposed method produces a safer path than the other methods. This is primarily due to the inclusion of a safety distance term in the cost function, which forces the path to remain at a safe distance from its nearest obstacle. In contrast, the results produced by other methods often remain too close to the obstacles in some sections of the path. Table 3 presents a comparative study of path quality, focusing on two key features: minimum clearance d from obstacles and path smoothness. The minimum clearance from obstacles measures the safety level of the results by calculating the distance between each path segment and its nearest obstacle. It is important to note that the safety distance utilized in the simulation is set at 5 m. Path smoothness measures the degree of smoothness of the path. The results presented in Table 3 reveal that the proposed method produces the safest path with the minimum distance from obstacles of 12.649 m, 11.663 m, 10.557 m, and 5.0 m for each scenario, respectively. In contrast, traditional GA and RRT* fail to satisfy the safety requirement in most cases. Moreover, the methods given by D* lite and Hybrid A* exhibit the worst performance in terms of safety, failing in all scenarios. Therefore, they are not suitable for real-world applications. In terms of path smoothness, the proposed method produces the smoothest paths (as seen in Figure 7) with values of 174.547, 149.454, 129.088, and 211.538 for each case, respectively, significantly outperforming the other methods. For instance, the proposed method's path smoothness value is 5-6 times smaller than that of Hybrid A* and D* lite.

Testing in USV Model
In this subsection, simulation studies and comprehensive comparisons are provided by conducting experiments on a prototype USV Otter (see Error! Reference source not found., www.maritimerobotics.com (accessed on 1 May 2023), Error! Reference source not found. shows the maneuvering derivatives). It is worth mentioning that the paths generated by D* lite and Hybrid A* are not suitable for real application because they would collide with the obstacles. Therefore, the paths given by GA and RRT* are selected for the simulation.

Testing in USV Model
In this subsection, simulation studies and comprehensive comparisons are provided by conducting experiments on a prototype USV Otter (see Figure 8, www.maritimerobotics. com (accessed on 1 May 2023), Table 4 shows the maneuvering derivatives). It is worth mentioning that the paths generated by D* lite and Hybrid A* are not suitable for real application because they would collide with the obstacles. Therefore, the paths given by GA and RRT* are selected for the simulation. In simulations, Error! Reference source not found. demonstrates the alteration course angle and speed for scenario 1 and scenario 2. The proposed method genera path with gentle and steady changes in course and velocities, as depicted in the fig The small deviation between the actual and desired signals suggests the feasibility o proposed method in conjunction with the USV control system. However, the path cre by GA exhibits sudden changes in both signals, resulting in a significant deviation a point of the sudden course alteration. On the other hand, RRT* generated the poores lution, with jagged changes in course angle and a substantial deviation between the erence and actual signals. Additionally, the simulation shows unstable velocity. Sim results are obtained by scenario 3 and scenario 4.
In Error! Reference source not found., the energy and time cost of the simula are displayed. The proposed method exhibits the lowest energy and time cost amon cases, as demonstrated in the table. This outcome is mainly attributed to the novel function incorporated into the proposed method, which directs the USV to move a the current direction. Conversely, the results produced by RRT* display the poorest formance with the highest energy consumption and computational time. This inferior formance is primarily due to the numerous abrupt changes in course angle and unn sary turns.   In simulations, Figure 9 demonstrates the alterations in course angle and speed for scenario 1 and scenario 2. The proposed method generates a path with gentle and steady changes in course and velocities, as depicted in the figures. The small deviation between the actual and desired signals suggests the feasibility of the proposed method in conjunction with the USV control system. However, the path created by GA exhibits sudden changes in both signals, resulting in a significant deviation at the point of the sudden course alteration. On the other hand, RRT* generated the poorest solution, with jagged changes in course angle and a substantial deviation between the reference and actual signals. Additionally, the simulation shows unstable velocity. Similar results are obtained by scenario 3 and scenario 4.
In Table 5, the energy and time cost of the simulations are displayed. The proposed method exhibits the lowest energy and time cost among all cases, as demonstrated in the table. This outcome is mainly attributed to the novel cost function incorporated into the proposed method, which directs the USV to move along the current direction. Conversely, the results produced by RRT* display the poorest performance with the highest energy consumption and computational time. This inferior performance is primarily due to the numerous abrupt changes in course angle and unnecessary turns.

Simulation in Time-Vary Ocean Environments
In this section, we will evaluate the method under time-varying ocean currents. We have selected some state-of-the-art methods from existing reliable references for comparison, including improved artificial fish swarm algorithm [8] and multi-objective enhanced GA (MOEGA) [32]. The ocean current model used in this paper is based on the numerical solution of water jet structure [33]: where ( ) and are the properly adimensionalized amplitude and wavenumber of the undulation in the stream function. The specific expression for ( ) is:

Simulation in Time-Vary Ocean Environments
In this section, we will evaluate the method under time-varying ocean currents. We have selected some state-of-the-art methods from existing reliable references for comparison, including improved artificial fish swarm algorithm [8] and multi-objective enhanced GA (MOEGA) [32]. The ocean current model used in this paper is based on the numerical solution of water jet structure [33]: where B(t) and k are the properly adimensionalized amplitude and wavenumber of the undulation in the stream function. The specific expression for B(t) is: with B o = 1.2, c = 0.12, k = 0.84, ω = 0.4, = 0.3 and θ = π/2. The velocity field is obtained by the following expression: where U(x, y, t) and V(x, y, t) are the x-and y-components of the velocity vector at time in the location (x, y). The parameters are set as follows: • Environment: map size: 500 * 500, Start = (80, 150), Goal = (480, 330), and the ocean current is set as Equation (27) for Case 1 while we multiply −1 on the y component for Case 2.  Table 6 presents the quantitative outcomes, including path distance, cruising time (T), smoothness (ϑ), and minimum distance to obstacles (d), for both Case 1 and Case 2. The visualized solutions for each case are also depicted in Figures 10 and 11. According to the results in Table 6, the proposed algorithm delivers solutions with higher quality paths (as highlighted in bold in the table), outperforming other methods in terms of cruising time, smoothness, and safety in most scenarios. However, it is noteworthy that the proposed algorithm results in the lowest safety value (8 m to the obstacle) due to the significant impact of energy consumption on optimization. It should be noted that an 8 m safety distance is acceptable in real application [34]. Moreover, the IAFSA method yields the worst outcomes, which may be attributed to its random behavior during the algorithm process, leading to abrupt points along the path. As demonstrated in Figures 10 and 11, our proposed model leverages the currents to reduce energy consumption by selecting intersection points that align with the current direction. Overall, the presented results indicate that the proposed method exhibits superior performance to the other two algorithms. IAFSA method yields the worst outcomes, which may be attributed to its random behavior during the algorithm process, leading to abrupt points along the path. As demonstrated in Error! Reference source not found. and Error! Reference source not found., our proposed model leverages the currents to reduce energy consumption by selecting intersection points that align with the current direction. Overall, the presented results indicate that the proposed method exhibits superior performance to the other two algorithms.

Conclusions
This paper presents a thorough investigation into the path planning problem for USVs. The proposed algorithm generates a path that is both optimally safe and quickly convergent, exhibiting strong adaptability to complex environments. In comparison to the existing literature, our method outperforms other algorithms across all problem variations. Additionally, the fast-discrete Clothoid curve is utilized to maintain path curve continuity and ensure reliable coordination between the planning and control modules, while also accommodating non-holonomic constraints. Simulation studies and comprehensive comparisons in various ocean scenarios have been conducted to illustrate the effectiveness and superiority of the proposed path planning framework.

Acknowledgments:
The authors thank the editor-in-chief, the associate editor, and the anonymous referees for their comments and suggestions.

Conflicts of Interest:
The authors declare no conflict of interest.

Conclusions
This paper presents a thorough investigation into the path planning problem for USVs. The proposed algorithm generates a path that is both optimally safe and quickly convergent, exhibiting strong adaptability to complex environments. In comparison to the existing literature, our method outperforms other algorithms across all problem variations. Additionally, the fast-discrete Clothoid curve is utilized to maintain path curve continuity and ensure reliable coordination between the planning and control modules, while also accommodating non-holonomic constraints. Simulation studies and comprehensive comparisons in various ocean scenarios have been conducted to illustrate the effectiveness and superiority of the proposed path planning framework.