*3.2. Path Planning Algorithms Considering Depth Hazard*

According to the results of the hydrodynamic analysis in Section 3.1, the minimum water depth required for safe navigation of unmanned surface vehicles is calculated by Equation (11):

$$S\_{\rm min} = z\_{\rm max} + 0.5L \tan \theta\_{\rm max} + T + e\_{\rm env} \tag{11}$$

where, *z*max is the largest downward settlement of the USV in several irregular waves at different speeds, *L* is the length of USV, θmax is the maximum trim angle, *T* is the average draft of the USV, and *eenc* is the depth error of ENC are considered. According to the minimum safe water depth for an unmanned surface vehicle, the restricted sea area of the unmanned surface vehicle is calculated, and the obstacle region is updated. The cost of water depth hazard in the obstacle region is set to 1.

Define the depth hazard of nodes *Ni* in feasible space as follows:

$$r(N\_i) = \frac{S\_{\min}}{D(N\_i)}, (0 < r(N\_i) < 1), \tag{12}$$

*D*(*Ni*) is the average water depth of each grid calculated by the interpolation algorithm in Section 2.2.

Total depth hazard of planned path is calculated by Formula (13):

$$\mathcal{R}\_{\mathbb{S}} = \sum\_{i=1}^{N} r(\mathcal{N}\_{i})\_{\prime} \tag{13}$$

where, *i* is the index of the grid through which the path passes, and *N* is the total number of all the grids through which the path passes. The depth hazard distribution map calculated by the combination of depth area and Formulas (11)–(13) is shown in Figure 9. The depth hazard map will be used as the input map of the path search algorithm.

**Figure 9.** The depth hazard distribution map.

The depth risk map and A\* algorithm are applied to plan the path. A\* algorithm was proposed originally by Hart Peter et al. [9]. The algorithm searches the minimum cost path from the starting node to the terminating node through the minimum cost function (15),

$$h(N\_i) = \sqrt{(\mathbf{x}\_{N\_i} - \mathbf{x}\_G)^2 + (y\_{N\_i} - y\_G)^2},\tag{14}$$

$$f(N\_i) = g(N\_i) + h(N\_i),\tag{15}$$

where *g*(*Ni*) is the actual distance cost from the ith node *Ni* to the starting node S that has been paid in the raster map namely the distance traveling from the start node S to the *i*th node *Ni*. *h*(*Ni*) is the heuristic distance from the start node to the goal node which has a great influence on the performance of A\* algorithm, in which (*xNi* , *yNi* ) is the coordinate the center of node *Ni* and (*xG*, *yG*) is the coordinate the center of goal node G. If it is always lower than (or equal to) real value, the shortest path can be found. The lower the number of extended nodes is, the slower the search speed. If it is equal to the actual value, the search only follows the best path and does not expand the redundant nodes, so the search speed is the fastest. If it is larger than the actual value, the shortest path cannot be guaranteed, but the search path is faster. In this paper, the Euclidean distance is chosen as a heuristic value when calculating the shortest distance path. Euclidean distance can ensure that the solution can be found if there is a shorter path. The shortest path planned by A\* algorithm combined with the grid method is presented in Figure 10b.

**Figure 10.** A\* path planning algorithm schematic diagram. (**a**) Schematic of adjacent node expansion; (**b**) The shortest path of A\* algorithm. The green star represents the starting point, and the yellow diamond represents the goal point. The shortest path is the black line between the starting point and the end point. The color grids are expanded nodes. The deeper the blue is, the closer the starting point is, and the deeper the red is, the closer the end point is. A set of white grids represents a feasible space, and black grids are obstacles.

The heuristic function of A\* algorithm considering depth hazard degree is defined as:

$$f(N\_i) = g(N\_i) + h(N\_i) + a \sum\_{i=1}^{N\_c} r(N\_i) \tag{16}$$

Not only the distance cost should be considered, but also the depth hazard, and the nodes with lower depth hazard degree should be selected to expand in neighbor nodes when expanding nodes. In Formula (14), *g*(*Ni*). The distance cost from the *Ni* node to the starting point S. *h*(*Ni*) is the heuristic distance cost from the *Ni* node to the goal node, *<sup>N</sup> c i*=1 *r*(*Ni*) is the water depth hazard cost sum from the starting node to the current node, and α is the constant greater than 0 used to control the weight of the risk cost in the total cost, thus controlling the impact of the water depth hazard on the final path. The pseudo code chart of the water depth risk level A\*(WDRLA\*) algorithm considering depth hazard is shown in Figure 11.

**Figure 11.** WDRLA\* algorithm pseudo code chart.

## **4. Discussions**

The spline function interpolation method with obstacles is performed to get the raster depth area with the water depth point selected as input point feature and the smoothing coefficient chosen as 0 and the grid size set as 25 m. The feasible space is updated according to the calculated minimum depth of safe navigation (1.29 m). Related parameters are shown in Table 6. The simulation is also based on the following assumptions: Assuming the unmanned vehicle as a mass point, unmanned vehicles can safely cross the junction vertex of the blocked grid and free gird between the obstacle area and the non-obstacle area, and unmanned vehicles can complete the turnaround operation in the grid. The proposed approach is simulated using Matlab R2018b. All simulations are performed on a PC with Microsoft Windows 10 as OS with Intel i5 2.712 GHz quad core CPU and 8 GB RAM.


**Table 6.** Table of Related parameters.

The positions of USV are represented by the grid index value of row and column where the point is located. S(500, 500) is chosen as the starting node and G(650, 650) as the goal node. Three kinds of cost function, including only distance, cost seeing Equation (15), only risk level cost seeing Equation (13), cost of combining seeing equation (16), were used to calculate the paths of shortest distance path (SDP), the shorter and safer path (SSP), the safest path (SFP) The results are shown in Figure 12. Compared with Figure 12a–c, it can be seen that the path planning algorithm considering water depth risk level will lead to more expanded nodes, so the search speed will be slower than A\* and the expanded nodes of WDRLA\* is less than those of SFP seeing Figure 12b,c. Figure 12d,e indicate that there is little difference between SSP and SFP, which means approximate depth risk level, but SDP is closer to the obstacle, so the risk level is much higher. Water depth risk level A\* algorithm (WDRLA\*) can reduce the depth risk and improve the safety of path, but sacrificing a certain calculation time.

**Figure 12.** Comparison of three kinds path. (**a**) Shortest distance path (SDP) that ignores the depth risk; (**b**) Shorter and safer path (SSP) that considers the depth risk level and distance. (**c**) Safest path (SFP) that considers only the depth risk. The colorful grids in figure (**a**–**c**) are extended nodes. The space of white grids represents the feasible space, and the black grids are obstacles. (**d**) Three paths, shown on the depth distribution map. (**e**) Three paths, shown on the depth risk level distribution map. (**f**) Three paths, shown on S57 ENC. The black line is the SDP, the blue one is SSP, and the purple one is SFP.

Take S(710, 902) as the starting node, and G(990, 1408) as the goal node, Results of three kinds of paths are shown in Figure 13. From Figure 13, it can be drawn that there are marked differences between SDP and SSP in shallow coastal waters. SDP passes through a narrow channel with shallow water depth which is more dangerous, while SSP and SFP avoid narrow channel by choosing deeper water area, where SSP reduces the risk of the path by 39.61% (see Table 7). At the same time, the number of extended nodes increases greatly, so WDLRA\* path search process is slower than A\*. SSP increases 1652m and 10.54% in length compared with SDP. The length of SSP and SFP is approximate, but SSP with less expanded nodes.

**Figure 13.** Comparison of three kinds path. (**a**) SDP that ignores the depth risk; (**b**) SSP that considers the depth risk level and distance. (**c**) SFP that considers only the depth risk. The colorful grids in figure (**a**–**c**) are extended nodes. The space of white grids represents the feasible space, and the black grids are obstacles. (**d**) Three paths, shown on the depth distribution map. (**e**) Three paths, shown on the depth risk level distribution map. (**f**) Three paths, shown on S57 ENC. The black line is the SDP, the blue one is SSP, and the purple one is SFP.


**Table 7.** Comparison of different starting and goal points.

In order to determine the reliability and consistency of the algorithm furtherly, disparate starting nodes and end nodes cases are simulated, and several parameters, such as the path length, the number of expanded nodes, and the risk level of the path, are compared simultaneously, in which the depth risk level is calculated by Equations (11)–(13). All of the results are shown in Table 7. From Table 7 combined with Figures 12–14, it can be seen that the depth risk level of the path can be reduced, and the path is relatively short, especially in the area with complicated underwater topography, such as Figure 13 with WDLRA\* algorithm, but in the cost of decreasing computational efficiency with redundant expanded nodes (see Figures 12c and 13c). This method can ensure the safety of small USV navigating in complicated sea areas.

**Figure 14.** The comparation of key parameters of three different sorts of paths. (**a**) The total distance of the path. The unit of distance is m. (**b**) Water depth risk level of the path. (**c**) The number of expanded nodes of three sorts of cost functions.
