Ship Motion Planning for MASS Based on a Multi-Objective Optimization HA* Algorithm in Complex Navigation Conditions

Abstract

Ship motion planning constitutes the most critical part in the autonomous navigation systems of marine autonomous surface ships (MASS). Weather and ocean conditions can significantly affect their navigation, but there are relatively few studies on the influence of wind and current on motion planning. This study investigates the motion planning problem for USV, wherein the goal is to obtain an optimal path under the interference of the navigation environment (wind and current), and control the USV in order to avoid obstacles and arrive at its destination without collision. In this process, the influences of search efficiency, navigation safety and energy consumption on motion planning are taken into consideration. Firstly, the navigation environment is constructed by integrating information, including the electronic navigational chart, wind and current field. Based on the environmental interference factors, the three-degree-of-freedom kinematic model of USVs is created, and the multi-objective optimization and complex constraints are reasonably expressed to establish the corresponding optimization model. A multi-objective optimization algorithm based on HA* is proposed after considering the constraints of motion and dynamic and optimization objectives. Simulation verifies the effectiveness of the algorithm, where an efficient, safe and economical path is obtained and is more in line with the needs of practical application.

1. Introduction

As an unmanned intelligent marine carrier platform, the Unmanned Surface Vehicle (USV) is small in size, flexible to operate and of high security. It can be equipped with different sensors or weapon systems, as required, in order to perform various tasks in military and civilian fields [1,2]. Amid the continuous development of the global marine economy and the intensifying disputes over maritime rights and interests, USV is a technical driver which can not only promote the rapid and sound development of the marine economy, but can also boost the strength of marine equipment and safeguard maritime rights and interests [3,4]. In 2020, the European Maritime Safety Agency (EMSA) released an overview of 13,204 maritime casualties from 2014 to 2019 [5]. The data show that the main cause of the casualties was loss of control of the ships (31.4%), followed by ship collision/contact accidents (30.5%), as shown in

Table 1. Distribution of casualty events with a ship.

Types of Events		2014	2015	2016	2017	2018	2019	Percentage
Capsizing/listing		11	15	8	15	18	17	0.63%
Collision		332	293	317	292	279	256	13.40%
Contact		390	402	357	420	379	320	17.18%
Damage/loss of equipment		287	361	356	310	341	297	14.78%
Fire/explosion		160	173	131	133	133	124	6.47%
Flooding/foundering		60	56	44	62	35	46	2.29%
Grounding/stranding		325	329	290	292	301	228	13.36%
Hull failure		6	15	22	5	5	4	0.43%
Loss of control		589	572	680	751	759	796	31.40%

. About 52.3% of all the maritime accidents investigated were caused by personnel misconduct. The research on intelligent collision avoidance decision of USV can effectively reduce the influence of human factors and human errors on the navigation safety of ships [6], and gradually some routine or high-risk manual operations can be replaced by the USV. At present, the study of unmanned surface vehicles has become a significant issue in the field of international maritime affairs, attracting the attention of more and more shipping and shipbuilding countries in the world and emerging as a very important development direction in the future shipbuilding industry.

Considering that the unmanned surface vehicle often performs difficult tasks, it is vital to plan a feasible and optimal route. This field can be split into three stages: path planning, trajectory planning, and motion planning [7]. In the path planning stage, the research object is generally regarded as a particle without considering its own kinematics and dynamics constraints [8]. A large number of algorithms have been put forward in the research of path planning, some of which have achieved good results, but there is often a big difference between the planned path and the actual path, which makes the former difficult in terms of meeting the requirement of feasibility. Trajectory planning is an improvement of path planning. Kinematics parameters, such as speed, direction and rotation radius of the research object, are taken into account in the planned path [9,10]. Although the result of final planning is close to the real trajectory, the interaction between constraints is still ignored. In the stage of motion planning, the kinematic and dynamic constraints of these research objects are fully considered, and the concern is whether the planned path can be realized through its own control system [11]. Therefore, in this stage, the kinematics and dynamics models of the research object will be discussed further, and the path planning method will be improved based on the mathematical model to generate new nodes which meet the constraint conditions.

Motion planning is core to the USV achieving high autonomy in a highly dynamic and uncertain navigation environment [12,13], which represents the intelligence level of the unmanned surface vehicle to a certain extent and is also one of the bottleneck factors that restrict USVs in terms of achieving high autonomy at present [14]. Compared with the Unmanned Ground Vehicle (UGV) and the Unmanned Aerial Vehicle (UAV), when applied, the USV can be interfered with by wind, waves and currents. Complicated environmental disturbance has a great influence on its instantaneous speed and attitude angle during navigation, and as such it is easy to make the USV roll over due to the excessive leeway and drift angle or turning angle rate [15,16]. In addition, USVs use mostly underactuated systems, and their inertia and motion response time are also longer than those of UAVs and UGVs, which brings greater uncertainty to the control and motion planning of USVs in complex navigation environments. Therefore, the autonomy of the USV is essential. This depends on two complex and changeable environmental parameters: wind and current [17,18]. Ignoring the environmental impact in motion planning would not only lead to a great waste of energy when the USV navigates strong ocean currents, but would also increase the potential risk of hitting obstacles. However, most of the current research results are based on idealized assumptions that do not consider whether there is environmental interference or it is a steady environment, and the expression of constraints for USV motion planning is inaccurate and incomplete, which limits its application in different scenarios.

In the field of USV intelligent planning and control, studies focusing on single-objective motion planning, such as in terms of length, optimal sailing time, energy consumption, smoothness and safety, have gradually deepened knowledge [19,20]. Planning algorithms can generally be divided into graph search algorithms, random sampling algorithms, curve interpolation algorithms, machine learning and dynamic optimization methods [21]. In the practical application situation, the above methods are usually used in combination in order to complete the motion planning. Sang et al. [22] used the improved A* algorithm to keep a safe distance and avoid collisions by reducing search points near obstacles. At the same time, the turning cost is added to the heuristic function in order to reduce the turning points of the path, avoid frequent turns of USV and improve the smoothness of the path. Liu et al. [23] introduced a safety parameter into the FMS algorithm, which can adaptively adjust the influence of the obstacle size, so as to ensure obstacle avoidance in a constrained environment and improve navigation safety. Zuo et al. [24] proposed A*-LSPI hierarchical path planning method, by which the global path based on the A* algorithm was quickly found and the approximate optimal local planning strategy with LSPI was learned. Although the planning time was long, the generated path length was short. Han et al. [25] formulated the multi-criteria global shortest path planning problem with resource constraints as a single objective linear programming model, and provided a modified label-correcting algorithm to solve this problem within a rationally short time. However, the planning environment was too simplified, and the environmental impact was less considered. Subramani et al. [26] formulated a stochastic optimization method to compute energy-optimal paths from among time-optimal paths of autonomous vehicles navigating in a dynamic flow field. Xu et al. [27] generated the path for an automatic ferry on the basis of AIS historical data. The resulting path was safer and more economic, because the AIS data were recorded from the real-time trajectory of ships. Lei et al. [28] proposed a multi-direction A* algorithm to iteratively find an optimum neighbor node and APF in scalar mode, which can take into account both computational complexity and efficiency. Xu et al. [29,30] proposed the vector field guidance law for the path-following control problem of the underactuated surface ship, which considered straight-line and curved-path path following scenarios in the presence of ocean currents. R. Zaccone et al. [31] developed and proposed a ship voyage optimization method, aiming to find the voyage which would require minimum fuel consumption within safety and comfort constraints by using 3D Dynamic Programming optimization.

In conclusion, amid a complex navigation environment, motion planning considering multi-constraint control decision and multi-objective optimization has become a difficulty and represents a hotspot in USV motion planning research. Therefore, it is necessary to study the USV motion planning of multi-objective optimization with complex constraints in dynamic navigation environments, that is, to make reliable motion planning under the complex constraints of environments, kinematics, dynamics and optimization objects.

According to the planning process, motion planning involves three steps: the environment model, the motion mathematical model and the search algorithm. The structure of this paper is as follows: Firstly, an environmental model, including wind and current, is constructed in order to provide high quality navigation environmental information for USV navigation. Based on the MMG model, a motion mathematical model suitable for Dolphin-I USV is proposed. Combined with the above model, the cost function of multi-objective optimization is analyzed, and a motion planning algorithm MOHA* is proposed in order to solve the multi-objective problem in dynamic navigation environment. The algorithm is then used to simulate the motion planning of the USV. Finally, the reliability of the MOHA* algorithm is proved by analyzing the experimental results. The chapter structure is shown in Figure 1, and the full text structure is shown in Figure 2.

2. Materials and Methods

2.1. Construction of Navigation Environment Based on ENC

The establishment of the environment model includes the process of extracting and describing environmental characteristic information. The electronic navigational chart (ENC) classifies and stores relevant elements in the geometric form of points, lines, and planes in data files, which can display and select relevant marine environment information according to need. This has such advantages as a short storage time, fast display speed and high accuracy [32]. Reading the overall package information in the ENC, and further processing the data in the forms of point, line and plane required for conversion, are the basis of navigation environment modeling. Modeling methods commonly used for USV motion planning mainly include grids [33], topology graphs [34], Voronoi diagrams [35], visibility graphs [36], and others. The environment model in this paper is based on ENC. The number of environmental obstacles with a complicated structure is large. It can be too complicated to describe the obstacle nodes by using topology graphs, Voronoi diagrams and visibility graphs. The grid method has a simpler data structure than other environmental modeling methods, which can reduce the complexity and calculation of the boundary processing of complex-shaped obstacles [37,38]. In this paper, grids are used to divide the ENC information. The consistent expression of grid ENC is the basis for improving the efficiency of the path search algorithm. The size of grid granularity determines the advantages and disadvantages of modeling, to a certain extent. The grid size is set to be 25 m × 25 m, comprehensively taking the minimum turning radius of USV (9.6 m), navigating and positioning error (5 m), safety buffer distance (5 m) and electronic navigation chart error (5 m) into consideration, and making sure it can complete the steering operation in a grid size area.

In this paper, Zhoushan islands are selected as the research area, where the longitude range is E120°55′26″~E123°29′30″, the latitude range is N29°33′15″~N32°28′59″, and the proportional scale is 1:2,000,000. The S-57 ENC in this area was transformed by Mercator projection through ArcMap, and the global static obstacle information, including obstructions such as land, islands and the seabed, was extracted and further transformed into a grid map, as shown in Figure 3. The white grids represent navigable areas, and the black grids represent obstructed areas.

The second version of the NCEP Climate Forecast System (CFSv2) was made operational at National Centers for Environment Prediction (NCEP) in March 2011 [39]. This version has upgrades to nearly all aspects of the data assimilation and forecast model components of the system. This paper adopts the CFSv2 data set (http://cfs.ncep.noaa.gov, accessed on 10 January 2020) as the input wind field data, which contains 0.2°, 0.5°, 1.0° and 2.5° horizontal resolution data, and updates the data at hourly intervals.

The current data are obtained from the global Hybrid Coordinate Ocean Model (HYCOM) and Navy Coupled Ocean Data Assimilation (NCODA) 1/12° analysis, with a time update frequency of three hours. This system is configured for the global ocean with HYCOM2.2 as the dynamical model and NCODA for data assimilation [40]. Having gradually become a mainstream global ocean circulation model in recent years [41], HYCOM can select appropriate vertical coordinates according to different navigation environments and thus can better capture the various physical processes of oceans [42]. The NCODA system uses the model forecast as a first guess in a multivariate optimal interpolation scheme and assimilates available in-situ observations [43]. More details about the assimilation system can be found at http://hycom.org, accessed on 10 January 2020.

Due to the limitation of data resolution, the 0.2° × 0.2° wind field and the 1/12° × 1/12° current field which were adopted cannot cover all of the non-obstacle grids in the map. Therefore, it is necessary to perform interpolation based on known environment information (e.g., wind, current) to predict environmental information in non-numerical areas. In this paper, based on the grid map resolution of 25 m, the known data are calculated by bicubic interpolation, and the corresponding wind and flow field data are obtained.

The processed ENC information was fused with the wind field and current field information on 10 January 2020, and the results are shown in Figure 4 and Figure 5. Environmental information was added to each non-obstacle grid of the grid map. Besides terrain information, information about the speed and direction of the wind and current were also stored in the grid. The wind reanalysis products cover the time period of the global HYCOM and NCODA assimilation. Therefore, the two types of data can be updated synchronously at a time interval of three hours, and a dynamic environment map can be obtained.

2.2. Mathematical Model of USV under the Influence of Wind and Current

The mathematical model is the basis of USV motion simulation and control [44,45]. In actual navigation, the speed and direction were controlled by the longitudinal propulsion force generated by two propellers at the tail and the turning moment generated by the differential speed, with no lateral driving force.

Meanwhile, considering that the rolling direction of the hull itself is relatively stable, a three-degree-of-freedom plane motion model was established, including surge (longitudinal motion), sway (sideways motion), and yaw (rotation around the vertical axis). The navigation state is formulated by two coordinate systems: one is the $o_o{x}_o{y}_o$ inertial coordinate system demonstrating the absolute position/speed information of the unmanned surface vehicle, and the other is the $oxy$ attached coordinate system which studies change in the status of the unmanned surface vehicle, taking the first-order differential $\left(\dot{x},\dot{y},\dot{\theta }\right)$ as the amount $V={\left(u,v,r\right)}^T$ of status change of the unmanned surface vehicle, as shown in Figure 6. In this paper, Dolphin-I USV of Tianjin University is used as the experimental platform. It adopts a modular design which can install different modules according to the application scenarios. The detailed USV performance parameters are shown in

Table 2. USV performance parameter setting details.

Index	Parameters
Length (m)	3.2
Breadth (m)	2.2
Weight (kg)	120
Draft (m)	0.3–0.5
Velocity (m/s)	7.0
Advance (m)	16.5
Diameter Tactical (m)	24.5

.

The MMG model mainly works to decompose the hydrodynamic force and torque acting on the ship into the hydrodynamic force and torque acting on the bare hull, open-water propeller and open-water rudder according to the physical meaning, and the mutual interference fluid between them. Based on the hull performance (rudderless, double propellers) of Dolphin-I USV, the MMG separation modeling is adopted, and the origin of the appendage coordinate system is taken as the center of gravity of the unmanned surface vehicle, and the motion equation is:

$$\left\{\begin{array}{l}\left(m+m_x\right)\dot{u}-\left(m+m_y\right)vr=X_H+X_{2P}\hfill \\ \left(m+m_y\right)\dot{v}+\left(m+m_x\right)ur=Y_H+Y_{2P}\hfill \\ \left(I_{zz}+J_{zz}\right)\dot{r}=N_H+N_{2P}\hfill \end{array}\right.$$

(1)

where m is the mass of USV; m_x and m_y are additional inertial masses in x-axis direction and y-axis direction respectively; $I_{zz}$ and $J_{zz}$ are the rotational inertia torque and the additional inertia torque in the z-axis direction respectively; X, Y and N are external forces and torques, and subscripts H and 2P respectively represent bare hull and two propellers. The additional mass, inertia torque, additional inertia torque, forces and torques acting on hull and propellers can be calculated using the calculation methods described in the literature [46].

The Dolphin-I USV is differentially driven by two propellers, that is, it’s speed and direction are controlled by the speed difference between the double propellers. According to the thrust model of two brushless DC thrusters in USBV power system proposed by Jin et al. [47] and the USV dynamic model of rudderless dual thrusters proposed by Li et al. [48], the resultant thrust vector of differential drive USV is established as follows:

$$F_{thrust}=\left[\begin{array}{l}{X}_{2P}\\ Y_{2P}\\ N_{2P}\end{array}\right]=\left[\begin{array}{l}{F}_L+F_R\\ 0\\ d_{LR}\left(F_L-F_R\right)\end{array}\right]$$

(2)

in which $F_L$ and $F_R$ are the thrust produced by the left and right thrusters along the x-axis in the attached coordinate system, respectively, and $d_{LR}$ is the transverse distance from the centerline of the USV to the centerline of each thruster.

In this paper, the operational performance of the unmanned surface vehicle in the wind is studied, and the mathematical model of the USV under wind disturbance is established by using a wind tunnel test and approximate estimation. When the unmanned surface vehicle is sailing, the superstructure device is affected by the wind, leading to the deviation of course or operational difficulty. When the unmanned surface vehicle is sailing at low speed, it is impacted by the wind quite severely. The interference force of wind on the unmanned surface vehicle can be regarded as the superposition of the average wind pressure and the variable wind pressure. In this paper, only the average wind pressure $F_{wind}={\left[X_{wind},Y_{wind},N_{wind}\right]}^T$ is considered, and the calculation expression is:

$$\left\{\begin{array}{l}{X}_{wind }=0.5{\rho }_a{A}_f{U}_R^2{C}_{wx}\left({\alpha }_R\right)\\ Y_{wind}=0.5{\rho }_a{A}_s{U}_R^2{C}_{wy}\left({\alpha }_R\right)\\ N_{wind }=0.5{\rho }_a{A}_{s}L{U}_R^2{C}_{wn}\left({\alpha }_R\right)\end{array}\right.$$

(3)

where ${\rho }_a$ is the air density; $A_f$ and $A_s$ are the orthographic projection area and the side projection area above the waterline of the unmanned surface vehicle respectively; $U_R$ is the relative wind speed; $L$ is the total length of the USV; $C_{wy}\left({\alpha }_R\right)$, $C_{wy}\left({\alpha }_R\right)$ and $C_{wn}\left({\alpha }_R\right)$ are, respectively, the wind pressure torque coefficient in the direction of $x$-axis and $y$-axis, and the wind pressure coefficient around the $z$-axis, which are calculated according to Isherwood formula [49].

When the motion model of the unmanned surface vehicle was being established under the current interference force, considering the unevenness of the horizontal upstream, the velocity of the current field in Zhoushan maritime space is divided into fields according to the interval of 0.005 m/s, that is, the uneven current field is divided into uniform current fields in different areas for modeling. The impact of water at any position in the uniform flow is the same, which will cause the unmanned surface vehicle to drift and interfere with its original posture and motion state. In the inertial coordinate system, the relationship between absolute current velocity $V_c$, absolute current direction ${\psi }_c$, and the absolute velocity of the unmanned surface vehicle is established, and the expression is:

$$\left\{\begin{array}{l}u=u_r+V_c\cos \left({\psi }_c-\psi \right)\\ v=v_r+V_c\sin \left({\psi }_c-\psi \right)\end{array}\right.$$

(4)

where $u_r$ and $v_r$ are, respectively, the longitudinal and lateral velocity of the unmanned surface vehicle relative to the current; $V_c$ is the absolute current velocity; and ${\psi }_c$ is the absolute current direction.

The relative velocity method is used in the hydrodynamic calculation, and considering its additional force, the current disturbance creates the current force $F_{current}={\left[X_{current},Y_{current},N_{current}\right]}^T$ as given below:

$$\left\{\begin{array}{l}{X}_{current}=\left(m_x-m_y\right)V_c\sin \left({\psi }_c-\psi \right)\\ Y_{current}=\left(m_x-m_y\right)V_c\cos \left({\psi }_c-\psi \right)\\ N_{current}=0\end{array}\right.$$

(5)

Considering the environmental interference factors mentioned above, the overall stress analysis of the unmanned surface vehicle motion system is carried out, and a three-degree-of-freedom MMG kinematic model is established with the hull, two propellers and environmental interference force as a function:

$$\left\{\begin{array}{l}\left(m+m_x\right)\dot{u}-\left(m+m_y\right)vr=X_H+X_{2P}+X_{wind}+X_{current}\hfill \\ \left(m+m_y\right)\dot{v}+\left(m+m_x\right)ur=Y_H+Y_{2P}+Y_{wind}+Y_{current}\hfill \\ \left(I_{zz}+J_{zz}\right)\dot{r}=N_H+N_{2P}+N_{wind}+N_{current}\hfill \end{array}\right.$$

(6)

The Runge-Kutta method is used to solve differential equation (6) to obtain the actual speed $V_e={[u_e,v_e]}^T$ and corresponding heading angle of the USV under the influence of environmental factors. Under the interference of the wind and current, assuming that the initial surge of the USV is 5 m/s, the given wind speed is 6 m/s, the wind direction is 180°, the current velocity is 1 m/s, and the current direction is 0°, the gyrating motion experiment and direct speed stability experiment are carried out with MATLAB. The actual measurement of the gyrating motion of the Dolphin-I USV in the still water shows that the advance distance is 16.5 m, and the initial diameter of the gyration is 24.5 m, as shown by the yellow dotted lines in Figure 7a,c. Comparing the measured data with the simulation results, it is proved that the kinematic model can accurately describe the navigation motion of the unmanned surface vehicle. Figure 7a,c show that the influence of wind on the cycle trajectory is not significant, while the cycle trajectory under the influence of the ocean current shifts eastward with a larger amplitude. In Figure 7b,d show that the speed changes greatly within 50 s under the influence of environmental disturbance, and then tends to be stable over time.

2.3. Dynamic Model of USV under the Influence of Wind and Current

The dynamic model of the unmanned surface vehicle represents the change of its posture under the action of force and torque. In this paper, based on the rigid-body dynamics model proposed by Fossen [50], with the effect of environmental interference, a three-degree-of-freedom kinematic model of the Dolphin-I USV is established. The specific expression is:

$$\begin{gathered} F_{thrust}+F_{env}=M\dot{V}+C\left(V\right)V+D\left(V\right)V \\ M=\left[\begin{array}{lll}m+m_x& 0& 0\\ 0& m+m_y& 0\\ 0& 0& I_{zz}+J_{zz}\end{array}\right]=\left[\begin{array}{lll}{M}_{11}& 0& 0\\ 0& M_{22}& 0\\ 0& 0& M_{33}\end{array}\right] \\ C\left(V\right)=\left[\begin{array}{lll}0& 0& -M_{22}v\\ 0& 0& M_{11}u\\ M_{22}v& -M_{11}u& 0\end{array}\right] \\ D\left(V\right)=- \left[\begin{array}{lll}{X}_u& 0& 0\\ 0& Y_v& 0\\ 0& 0& N_r\end{array}\right] \end{gathered}$$

(7)

in which $M$ represents the inertia matrix, including the added mass parameters. $C$ is the Coriolis and centripetal matrix. $D$ is hydrodynamic drag matrix. $X_u$, $Y_v$, and $N_r$ are collectively referred to as the hydrodynamic derivative, and the specific values of the above three variables are calculated by using the formula of literature [17]. $F_{thrust}$,$F_{env}$ are the thruster and environment forces $\left(F_{wind}, F_{current}\right)$, respectively, applied on the USV.

3. Algorithm of Ship Motion Planning

Considering the spatial constraints and the constraints of planning behavior of the objects motion planning combines path planning with motion control [51]. Global motion planning refers to the calculation of the path from the departure point to the target point that meets certain performance requirements according to the established prior environmental map, such as shortest distance or the highest safety.

3.1. Traditional Hybrid A* Algorithm

Hybrid A* (HA*) is an algorithm for UGV kinematics and it was first proposed by Stanford Laboratory [52] in 2008. It can perform the heuristic search in a continuous coordinate system and guarantee that the generated trajectory meets the vehicle nonholonomic constraint. This algorithm is a variant of the A* algorithm, which adopts a four-dimensional search space and adds the orientation information of the mobile platform and the fourth dimension representing the forward and backward movement on the basis of the two-dimensional plane, considering the final directions of the starting point and the end point. The core of the algorithm is to design a cost function for each node to be searched to determine the accessing sequence of each node in the search:

$$f\left(x_n\right)=g\left(x_n\right)+h\left(x_n\right)$$

(8)

where $f\left(x_n\right)$ is the total cost estimate from the starting point $x_s$ through the current node $x_n$ to the goal node $x_g$; the actual cost $g\left(x_n\right)$ from the starting point $x_s$ to the current node $x_n$, and the heuristic cost estimate $h\left(x_n\right)$ from the current state $x_n$ to the goal point $x_g$.

$g\left(x_n\right)$ can be calculated by recursive formula:

$$g\left(x_n\right)=g\left(x_{n-1}\right)+\epsilon d\left(x_{n-1},x_n\right)\times DirectionCost+\sigma \left|K\right|$$

(9)

where $g\left(x_{n-1}\right)$ is the parent node of the current node, and $d\left(x_{n-1},x_n\right)$ is the Euclidean distance from the parent node to the current node. $DirectionCost$ indicates the change of motion direction where the forward driving value is 1, and the reverse driving value is −1, which is used to ensure the forward driving of USV as much as possible; $K$ is the curvature of the motion primitive; and $\epsilon$ and $\sigma$ are weights, which are used to unify the order of magnitude of each item.

$h\left(x_n\right)$ involves two heuristic functions: (1) The 2D heuristic function with holonomic constraints. When the search node is far away from the goal point, the USV always moves towards the goal point, and its nonholonomic characteristics can be ignored. Based on the obstacles shown in the environment map, the heuristic function with complete constraints is only used to consider the position information of USV (2) and the 3D heuristic function with nonholonomic constraints. The motion range was set as an accessible region and discretized into three-dimensional grids. Only considering the motion constraints of USV, the optimal path from the center point of each grid to the goal point is calculated by using the Reeds-Shepp curve. This heuristic function does not depend on the grid map information at runtime, and can perform off-line calculations in advance. The state of the current point is then matched after simple transformation and rotation, thus improving the calculation efficiency. The current node heuristic value is the larger 3D heuristic value and 2D heuristic value, in order to ensure the reliability of the search algorithm.

3.2. Multi-Objective Optimization Model of USV Motion Planning

In traditional motion planning, only a single optimization objective is usually considered, which means that it is difficult to generate a high-quality path that satisfies spatial constraints, time constraints and motion constraints. In practical navigation, the global planning of the USV can be regarded as a multi-constraint and multi-objective optimization problem. The shortest search time can ensure the ability of the USV to respond in time. The safest path is the premise that the USV can perform various tasks. The most energy-efficient route enables the USV to increase mileage. Therefore, under the condition that space constraints and USV motion constraints are met, a model is proposed to optimize the search efficiency, safety performance and energy consumption.

3.2.1. Graph Expansion/Search Model Based on Hybrid Motion Primitives

The traditional graph search algorithm uses 4-domain or 8-domain connection to expand nodes [53]. When the search map is expanded, the number of grids will increase rapidly, resulting in a sudden increase in the search time. Meanwhile, the obtained path has a large number of redundant nodes which do not meet nonholonomic constraints. Hence, firstly, the environment map is down-sampled, and the heuristic efficiency can be greatly improved by searching and calculating in the processed grid map. The heuristic function values of nodes in the original map are then restored by up-sampling.

When expanding nodes, continuous motion primitives—that is, trajectory segments that satisfy the motion constraints of USV—are used to ensure that the curvature of the path that is ultimately generated is less than the curvature corresponding to the minimum turning radius. In order to reduce the parameters needed by motion primitives and to reduce the computational complexity, circular arcs and straight lines are used to generate motion primitives. Accordingly, the short primitives are of better flexibility and stronger ability to bypass obstacles, but this will lead to an increase in the number of extended nodes. Long primitives can reach the vicinity of the final point faster, but they present a higher risk of collision with obstacles in complex conditions. Therefore, four kinds of motion primitives (forward and backward) are mixed in this paper. The long primitives and short primitives are divided with the grid size of 25 m as the base length, in which the short primitives are S1, S2, S1′, S2′, the long primitives are M1, M2, M1′, M2′, L1, L2, L1′, L2′, and the linear motion primitives are SL and SL′. The specific length and curvature are shown in

Table 3. Details of hybrid motion primitive parameters.

Motion Primitive	Length (m)	|K|
	25	0.04
	30	0.026
	38	0.013
	25/30/38	0

. This is achieved by taking full respective advantages of the above-mentioned motion primitives and setting different cost coefficients for them. The cost of long primitives is lower and the cost of short primitives is higher. This can make USV approach the goal point quickly in exposed waters, while ensuring that it passes through narrow obstacle areas. The expansion/search model based on hybrid motion primitives can effectively reduce the number of path nodes in planning, thus improving the computational efficiency and achieving drivability.

3.2.2. Risk Degree of Navigation Model Based on Ship Domain

Traditional collision detection takes the USV as a particle and expands the obstacle map. This method has high efficiency, but the expansion scale is difficult to choose and the accuracy cannot be guaranteed, resulting in a big difference between collision detection and real results. In actual navigation, it is necessary to maintain an exclusive domain around the USV which is defined as the navigation safety domain (NSD) [54,55] and is established in order to avoid encroachment by other ships or obstacles. NSD is usually oval-shaped, with its long axis being three to eight times the length of the ship. It is designed in order to delimit enough sea space for the USV to take actions to avoid collisions in advance. In this paper, combined with the quaternion ship domain (QSD) proposed by Wang et al. [56] and the basic navigation safety domain (BNSD) proposed by Zhou et al. [57], the ellipse model with four half axes in different directions is established with the USV as the origin coordinate, where the space domain can be divided into four sub-domains $\mathrm{\Omega }=\left\{{\mathrm{\Omega }}_1,{\mathrm{\Omega }}_2,{\mathrm{\Omega }}_3,{\mathrm{\Omega }}_4\right\}$. To calculate and simplify the model’s complexity, given the coefficients of overtaking encounters situations $s\left(i\right)=1$, $t\left(i\right)=0.2$, its mathematical expression is:

$$R=\left\{\begin{array}{l}{R}_{fore}=L+1.34\sqrt{A{D}^2+{\left(DT/2\right)}^2}\\ R_{aft}=L+0.67\sqrt{A{D}^2+{\left(DT/2\right)}^2}\\ R_{starb}=B+1.2DT\\ R_{port}=B+0.9DT\end{array}\right.$$

(10)

In the above equations, $R_{fore}$, $R_{aft}$, $R_{starb}$ and $R_{port}$ are the radii of the navigation safety domain. $L$ and $B$ represent the length and breadth of the USV. $AD$ is the advance distance, the longitudinal forward distance of the gravity center in the case of the USV turning 90° from the start of steering. $DT$ is the tactical diameter, the transverse distance of the gravity center in the case of the USV turning 180° from the start of steering.

Based on the above model, two safety domains $R_1=\left\{30.74,16.97,31.60,24.25\right\}$ and $R_2=\left\{46.74,32.97,42.60,35.25\right\}$ are constructed with one-time length and three-time length as input parameters, as shown in Figure 8.

According to the different heading angles, the reasonable radius range of the sub-safety zone from USV to static obstacles is obtained with different heading angles and the radiuses of navigation safety zones: when $0^{°}\le {\phi }_e<{90}^{°}$, $R\left(x_n\right)\in {\mathrm{\Omega }}_1$; ${90}^{°}\le {\phi }_e<{180}^{°}$, $R\left(x_n\right)\in {\mathrm{\Omega }}_2$; ${180}^{°}\le {\phi }_e<{270}^{°}$, $R\left(x_n\right)\in {\mathrm{\Omega }}_3$; ${270}^{°}\le {\phi }_e<{360}^{°}$, $R\left(x_n\right)\in {\mathrm{\Omega }}_4$. Collision risk index (CRI) is an evaluation parameter for the collision avoidance of ships, which is also a key concept in the field of collision avoidance research and operation. Based on the collision risk index (CRI) calculation method of distance to closest point of approach (DCPA), the shortest safe distance of the USV and the actual distance $d\left(x_n\right)$ between the USV and obstacles are used to reflect the navigation risk. The smaller the value is, the more suitable it is for navigation, while the larger the value is, the less suitable it is for navigation. Its mathematical expression is:

$$rd\left(x_n\right)=\left\{\begin{array}{ll}0& d\left(x_n\right)>R_2\left(x_n\right)\\ 0.5-0.5\pi \left[d\left(x_n\right)/R\left(x_n\right)-1.5\right]& R_1\left(x_n\right)<d\left(x_n\right)\le R_2\left(x_n\right)\\ 1& d\left(x_n\right)\le R_1\left(x_n\right)\end{array}\right.$$

(11)

3.2.3. Energy Consumption Model Based on Dynamic Analysis

Energy efficiency is an important characteristic of path planning algorithms for autonomous systems [58]. The consumed power of a USV is divided into two parts: the static power due to static consumption and the dynamic power involving the thrust power. The main USV power consumption is due to thrusters (80–90%), transferred to the mechanical power. To maximize the engine efficiency [15], only dynamic consumption is considered in this paper, and the mechanical power is modeled as a function of USV speed and environmental conditions. Compared with the approximate path cost of kinematics method, the dynamic analysis proposed by Fossen [50] can provide more accurate information on the energy consumption cost. According to the dynamics model of Dolphin-I USV created in Section 2.3, the scalar product of the thrust vector obtained by Formula (7) and velocity vector obtained by MMG formula can get the power dissipated by a given force. Assuming that the instantaneous acceleration during the navigation from the current node to the extended node is constant, the instantaneous power during the navigation is integrated. The heuristic function of the energy consumption cost from the current node to the extended node can be obtained as follows:

$$e\left(x_n\right)=\int F_{thrust}\left(x_n\right)·V_e\left(x_n\right)dt$$

(12)

3.3. Multi-Objective Optimization Algorithm for Ship Motion Planning Based on HA*

In the process of expanding nodes, the Hybrid A* algorithm takes a long time to make a search and requires lots of iterations and calculations. Moreover, this algorithm does not consider the energy consumption and multiple objectives optimization under the influence of navigation environment at the same time. In order to resolve the above-mentioned problem of USV motion planning in a steady navigation environment, a multi-objective Hybrid A* algorithm in a dynamic environment (MOHA*) is adopted to update the actual speed in the extended grid after being affected by the environment in real time, and to generate a multi-objective optimal path which is more in line with the actual application and meets the requirement to be the most efficient, safest and the most energy-saving. The complexity of motion planning mainly comes from the following two aspects: the influence of complex environment and the kinematics constraints of the USV. In order to ensure that the estimated cost of the optimal path is close to the actual optimal path cost, these two factors should be considered when designing heuristic functions. With the multi-objective optimization model of USV motion planning in Section 3.2, the framework of the MOHA* algorithm is presented, as shown in the Figure 9. The core idea of designing the optimal strategy is the definition of the cost function, which can be expressed as:

$$f\left(x_n\right)=\frac{g\left(x_n\right)-g_{\min }(x_n)}{g_{\max }(x_n)-g_{\min }(x_n)}+\frac{h\left(x_n\right)-h_{\min }(x_n)}{h_{\max }(x_n)-h_{\min }(x_n)}+\alpha sd(x_n)+\beta e(x_n)$$

(13)

where $g_{\max }(x_n)$ is the maximum of the actual cost, $g_{\min }(x_n)$ is the minimum of the actual cost, $h_{\max }(x_n)$ is the maximum of the heuristic cost, $h_{\min }(x_n)$ is the minimum of the heuristic cost, $sd\left(x_n\right)$ is safety cost heuristic value, $e\left(x_n\right)$ is energy cost heuristic value, $\alpha$ and $\beta$ are constants greater than 0, which are used to control the weight of safety cost and energy cost in the total cost, respectively, thus controlling their influence on the final path.

4. Simulation

In this paper, the MMG motion model and motion mathematical model of the USV are built in the environmental model by integrating ENC data and ocean reanalysis data, and expressing the complex constraints under ocean dynamic interference elements. Thus, the efficient solution and optimization of USV motion planning are realized. The proposed approach is simulated using MATLAB R2020b. All simulations are performed on a PC with Microsoft Windows 10 as OS with Intel i5 2.90 GHz quad core CPU and 8 GB RAM.

The grid map used in the planning is 5000 m × 5000 m in size and 25 m × 25 m in resolution. The approach takes $x_s$(1000, 6500, 0) as the starting point, and $x_{\mathrm{g}}$ (3500, 2500, −π/2) as the ending point. The initial surge speed is given as 5 m/s, and the number of extended motion primitives is six. The cost function values are calculated with five different optimization objectives, respectively, including only using original Hybrid A*, efficiency-optimized Hybrid A*, safety-optimized Hybrid A*, energy consumption optimized Hybrid A* and MOHA*. The paths that are obtained are shown in Figure 10a–f. Figure 10b represents a partial enlarged view of the traditional HA* algorithm path in Figure 10a. In the simulation result diagram, blue lines and blue dots represent the moving primitives and nodes in the expansion process, respectively, while gray lines and gray dots represent the moving primitives and nodes in the retreat, respectively, and their density represents the number of expanded nodes. The red curve is the final path obtained by planning. Figure 10c shows the efficiency-optimized Hybrid A* algorithm results. In Table 3, it can be seen that the number of extended nodes decreases and the search time is shortened overall. The comparison between Figure 10a,d shows that the curvature of some moving primitives of USV changes to a certain extent in the area close to obstacles with the safety-optimized HA* algorithm, which leads to a relative increase of expanded nodes. At the expense of a certain calculation time, the risk of generating paths is reduced by 41.3%. According to Figure 10a–f, MOHA* expands fewer nodes and has a larger distance from obstacles. The navigation state of USV is changed by the environmental interference, and the total energy consumption is reduced by 24.18%. The simulation results for the five different scenarios verify the effectiveness of the multi-objective motion planning model, and the number of extended nodes, risk degree, running time and energy consumption are compared. The results are shown in

Table 4. Comparison of Motion Planning Test.

Test	Number of Nodes	Risk Degree	Time (s)	Energy (KJ)
1	5450	46	14.56	4252
2	4164	46	11.16	4168
3	5477	27	14.73	4318
4	3742	43	9.78	3512
5	2987	22	7.57	3224

.

The MOHA* algorithm is used to carry out four simulation experiments on motion planning at different starting points. The results show that the extended search model, based on motion primitives and map downsampling, can effectively reduce the number of extended nodes. The specific combination of motion primitives are shown in

Table 5. Combination of motion primitives for Test 6-Test 9 motion planning paths.

Test	Start Position	Goal Position	Combination of Motion Primitives
6	(1000, 3000, π/4)	(4000, 5000, 0)	L1 + SL + SL + L1…M1 + M2 + L1 + SL
7	(2000, 4300, −π/2)	(3500, 2200, 0)	L2 + SL + M1 + S1…S1 + S2 + S1 + SL
8	(4000, 6500, 0)	(500, 4000, −π/4)	SL + L1 + L2 + L2…SL + M2 + S2 + S2
9	(3200, 6500, 0)	(2500, 3400, −π/2)	M1 + M2 + M1 + S1…M1 + M2 + S2 + SL

. The final paths of the four groups can avoid obstacles and always keep a safe distance from obstacles. Test 5 and Test 6 show that the USV can make good use of environmental interference and consume less energy when heading in the same direction as the environmental interference force. In the opposite navigation situation, the MOHA* algorithm can also reduce energy consumption while avoiding obstacles, and can select the optimal path, as shown in Figure 11a–d. In summary, the results show that, under different working conditions, using the MOHA* algorithm can result in the USV simultaneously satisfying multi-constraint and multi-objective optimization in a complex navigation environment, and find the optimal path.

5. Conclusions

Marine environment information is essential for optimal path planning. In order to quantitatively study the influence of wind and current on the navigation of USVs, this paper establishes a high-resolution marine environment model that is updated every three hours based on the reanalysis data of a numerical prediction model, which provides a high-quality marine environment field for USVs. On the basis of the MMG ship operation model, the kinematics and dynamics model of the Dolphin-I USV is established, the influence of environmental disturbance force on the motion of the USV is considered, and the simulation is carried out, which can quantitatively calculate the speed and direction of the USV. In this paper, a motion planning MOHA* algorithm is proposed in order to simultaneously optimize three objectives (efficient, safety and energy) in dynamic marine environments and satisfy multiple constraints. As the navigation environment information changes, the MOHA* algorithm can adjust the input parameters to complete the path update. The simulation experiments of single-objective optimization and multi-objective optimization show that the MOHA* algorithm can improve planning time, reduce navigation risk and decrease navigation energy consumption simultaneously, which can fully ensure the efficiency of the USV in performing tasks and improve their independent decision-making ability, thus supporting larger-scale coordinated motion planning and control research for USV clusters.