An Improved A-Star Algorithm Considering Water Current, Traffic Separation and Berthing for Vessel Path Planning

Chenguang Liu 1 , Qingzhou Mao 2,* , Xiumin Chu 3 and Shuo Xie 3 1 State Key Laboratory of Information Engineering in Surveying, Mapping and Remote Sensing, Wuhan University, Wuhan 430079, China; cgliu@whu.edu.cn 2 School of Remote Sensing and Information Engineering, Wuhan University, Wuhan 430079, China 3 National Engineering Research Center for Water Transport Safety, Wuhan University of Technology, Wuhan 430063, China; chuxm@whut.edu.cn (X.C.); xieshuo@whut.edu.cn (S.X.) * Correspondence: qzhmao@whu.edu.cn


Introduction
The autonomous surface vessel (ASV) technologies have grown rapidly and obtained more attention than ever both in military and commercial applications [1]. Among them, path planning in complex navigation area is one of most difficult and challenging topics that need to be developed. The aim of path planning is to find the optimal navigation path from the start location to the destination location [2]. Generally, path planning can be divided to two classes, i.e., global path planning and local path planning (real-time) [3]. A global path planner usually calculates paths off-line in static environments with stationary obstacles. In contrast, a local path planner usually calculates paths on-line in dynamic environments with moving obstacles. For the global planning, path length, risk factor, dynamic constraint, etc. should be considered at the same time. Specifically, current effect and navigation rules should also be included during the path planning for a vessel. In this article, we try to propose a global path planning method to satisfy the requirements of safety and efficiency for autonomous surface vessel. For a vessel, especially an autonomous surface vessel, factors of the path length, the obstacle collision risk, the traffic separation rule and the manoeuvrability restriction should be all taken into account when a path is planned for navigation. To solve these problems, a lot berthing path planning and combined path planning, respectively. In the Section 5, the conclusions and future directions are presented.

Risk Modelling
During path planning, to find a shortest waterway is not the ultimate goal for a vessel because the safety of the vessel usually obtains more attention than the operation efficiency. However, existing path planning algorithms usually ignored the impact on the vessel safety when a vessel voyages nearby obstacles, such as bridge pier, shore, other vessels, port, etc., when the water current speed is large enough to be considered, when the traffic separation has been carried out, or when a vessel is for berthing. Hence, it is necessary to build relevant risk models to denote the impacts of these factors. In this section, we propose the four risk models designed for the obstacles, traffic separation, berthing and manoeuvrability, respectively.

Obstacle Modelling With Current
The traditional A-Star algorithm can generate an optimal path with keeping off the obstacle nodes, which shown in Figure 1. From Figure 1, it can be seen that the generated path could be very close to obstacles to guarantee the path shortest. However, if this path is adopted by a sailing vessel as the planning route, the vessel will have a high risk to collide the obstacles. Meanwhile, the current will also increase the collision risk if the current pushes the vessel to approach one of obstacles. Therefore, it is indispensable to consider both keeping an appropriate distance from obstacles and current influence during path planning. The obstacles for vessel can be divided into 5 types, namely, bridge pier, shore, vessel, port, and others. Different types of obstacles have different risks for vessel navigation. The risk degree r o [m, n] of navigable node N [m] nearby obstacle node O [n] is defined as follows: where d stands for the Euclidean distance from an navigable node N [m] to the obstacle node O [n]. a stands for the risk coefficient for different types of obstacles. In this article, a is assigned as 5, 5, 3, 4, 3 for bridge pier, shore, vessel, port, and others. v m stands for the current velocity. c m stands for the direction coefficient, which is as follows: where, φ c ∈ [0, 2π) is the current direction toward the true North; φ g ∈ [0, 2π) is the node direction relative to obstacle node O [n] toward the true North, i.e., the direction of vector A grid map considering the obstacle with current is represented in Figure 2. In Figure 2, the number in a navigable node denotes the risk degree, and the direction and length of an arrow denote the current direction and velocity, respectively. A navigable node N [m] could have k risk degree values if there exist k obstacle nodes nearby N [m] according to Equation (1). In this case, the maximum one will be selected as the final risk degree. Therefore, the final risk degree of node N [m], r o [m], is defined as follows: In Figure 2, the path is generated by the traditional A-Star algorithm, which chooses an optimal path node without considering the risk degree of the path node. N moored or anchored ship shore bridge pier port other obstacles start node destination node navigable node path generated by traditional A-Star current

Traffic Separation Modelling
Similar to the lane separation for vehicle driving on the road, a vessel also needs to obey the traffic separation rule in the waterway to decrease the risk of collision and increase the operation efficiency. Based on the traffic separation rule, a risk model of traffic separation for a navigable node N [m] is built as: where φ n ∈ [0, 2π) is the navigation direction by rule toward the true North. Assume that the path is defined as toward the true North. When the navigation direction is consistent with the path vector direction, i.e., 0 ≤ r s [m] < 1; otherwise, , 1 ≤ r s [m] ≤ 1 + α. The grid map considering traffic separation is shown in Figure 3. From Figure 3, the path generated by the traditional A-Star algorithm cannot guarantee the planning path to be consistent with the navigation direction by rule.

Berthing Modelling Considering Current
When a vessel needs to berth at a port or an anchorage in a waterway with a non-ignorable current, the vessel usually berths into the current, which means the vessel motion direction is opposite to the current direction. The reasons for it are summarized as: (1) it will give the vessel the advantage of relatively high speed through the water with a reduced speed over the ground; (2) it can be used to push the vessel alongside [10]. Figure 4 shows the berthing path is generated by the traditional A-Star algorithm. From Figure 4, the vessel motion direction planed by the traditional A-Star algorithm is the same as the current direction, which is not beneficial for the berthing. Hence, if the destination node is a berthing node, the berthing path should be considered.  The generated path with a berthing destination node consists of the normal path P n and the berthing path P b . The berth path P b should consider the current effect. In contrast to the normal A-Star algorithm, the berthing path has no specific destination node with the berth node as the start node. Therefore, how to generate a berth path without a destination node is a key point to deal with.
The berthing risk of a navigable node N [m], r be [m], is modelled as follows: where r be [m] ≥ 0, and c > 0 is a constant coefficient. s m = 0, 1, ..., N be is the sequence number of node N [m] from the berth node, in which N be is the node number of the berthing path. φ n and φ r are defined in Equation (4). The specific procedure to generate the berthing path will be described in Section 3.2.2.

Manoeuvrability Restriction Modelling
The minimum turning radius of a vessel represents the turning ability of the vessel when following a predefined path. Actually, this ability is restricted to the manoeuvrability of the vessel. If the restriction of turning radius is not set, the generated path could not be followed by the vessel and the risk of collision and stranding could even happen.
Assume that P i−1 , P i , P i+1 are the continuous nodes of a path, which is shown in Figure 5. θ ∈ [0, π) is the included angle between line P i−1 P i and line P i P i+1 . l is the shorter length between the length of P i−1 P i and the length of P i P i+1 . A circle is built through node P i−1 , which is tangent to line P i−1 P i and line P i P i+1 . Then the radius of the circle, R i , is obtained: If the minimum turning radius of the vessel R 0 is greater than R i , it means that the continuous nodes P i−1 , P i , P i+1 is not available for this vessel, and vice versa. Hence, a risk model for manoeuvrability restriction is defined as follows: where b > 0 is a constant coefficient.

Improved A-Star Algorithm
In consideration of the limitations of the traditional A-Star algorithm applying in the path planning of vessels, an improved A-Star algorithm is studied in this section based on the risk models built in Section 2. The traditional algorithm is introduced first, and then an A-Star algorithm considering risk modelling is presented.

Traditional A-Star Algorithm
A-Star algorithm was first proposed and described in detail in [15]. The A-Star algorithm aims to find a path from the start node (N [start]) to the destination node (N [dest]) with the smallest cost, e.g., least distance. The flowchart of the traditional A-Star algorithm is denoted in Algorithm 1. Mark N [i] as closelist. 8: if

Algorithm 1 Traditional
does not belong to closelist or obstacle node then 9: as closelist then 11:

12:
end if 13: end if 14: end if 15: end while 16: return "path is not found".  [9]. Please note that in contrast to the A-Star algorithm, the Dijkstra algorithm has no heuristic function that can accelerate the rate of finding an optimal path . The Euclidean distance is selected as h (N [i]) in this article.

A-Star Algorithm Considering Risk Models
If an A-Star algorithm is used to the path planning for vessels. Theoretically, the traditional A-Star algorithm can find the shortest path, which is not nevertheless the optimal path for vessels in some scenarios. In this section, we try to consider the path length, the path risk and navigation rules simultaneously during the path planning. According to the destination, a normal path generation and berthing path generation are introduced with a non-berthing destination and berthing destination.

Normal Path Generation of Vessels
To satisfy the requirements of considering risks for vessel path planning, model (8) needs to be modified. Here, g (N [i]) is redesigned as: where p ≥ 1 is the node amount from , in which N.parent represents the parent node of N. d (k) is the Euclidean distance between kth node to (k + 1)th node, where k stands for the node sequence from the start node. To be specific, assuming that the coordinates of kth node is (x k , y k ), then d (k) is as follows: r n (N [i]) is the risk function, which is as follows: where (3), (4) and (7), respectively. Please note that r s = 0 if traffic separation rule is not carried out in certain waterway.
A new evaluation function f n (N [i]) is defined as: where β n > 0 is a constant coefficient. In contrast with (8) The proposed A-Star algorithm of the normal path generation is presented as Algorithm 2.

Algorithm 2 Normal path generation with the improved A-Star algorithm (N [start] → N [dest])
1: Mark N [start] as openlist. 2: if the destination node is not the berthing one then 3: if traffic separation rule is not carried out then 4: r s = 0 at any time. 5: end if 6: while openlist = ∅ do 7: Select the open node N [i] whose value of evaluation function f n (N [i]) is smallest. 8: if return "path P n is found". Mark N [i] as closelist. 12: if does not belong to closelist or obstacle node then 13: Mark N i [j] as openlist.

Berthing Path Generation of Vessels
As previously mentioned, when the destination node is the berthing one, a specific path planning rule should be obeyed in order to guarantee the safety of vessels. The algorithm of generating berthing path P b described in Section 2.3 is presented in Algorithm 3, in which N [berth] is the starting node of P b .
The evaluation function for berthing path generation, f b (N [i]), is defined as: where = 0 at any time since the destination node is not known before the berthing path generation finishes. g b (N [i]) is as follows: where g (N [i]) = 0 at any time, τ b > 0 is a constant coefficient. r b (N [i]) is as follows: Please note that the traffic separation rule is not considered for the berthing path generation, i.e., r s = 0 at any time, because a vessel in the berthing process always finds the path that is against the current ignoring the traffic separation rule temporarily.
Combining (15), (16) and (17), f b (N [i]) can be denoted as: Please note that τ b = 1 and β b = 1 are both eliminated in (18)  if q b >= q 0 then 8: "path P b is found". 9: else 10: Mark N [i] as closelist. 11: if Successor N i [j] of node N [i] does not belong to closelist or obstacle node then 12: Mark N i [j] as openlist.   return "path P b is not found". 21: end if

Combined Path Generation of Vessels
In most of path planning scenarios, the normal path and the berthing path both need to be generated. Therefore, it should be considered that how to combine the normal path and the berthing path. The main problem is that the manoeuvrability risk r m [m] defined in (7) could be high, which will increase the risk of collision or stranding when the connection node of the normal path and the berthing path is inappropriate. For instance, in Figure 6, the sharp turn could not be tracked well by the vessel. The distance between two adjacent nodes, l n , is usually relevant to the minimum radius of the vessel, R 0 . In this article, we set l n = R 0 .
The combined path P c consists of the normal path P n and the berthing path P b , i.e., P c = P n + P b . To avoid a sharp turn at node N [berth], i.e., R 0 > R berth , where N [berth] is the first node of P b , a new node can be added between N [berth] and the second node of P b . As shown in Figure 7, a node is added to eliminate the sharp turn and make the generated path smooth enough be followed.
The algorithm of adding node or smoothing the combined path is denoted in Algorithm 4.

Case Study
To verify the effectiveness of the proposed improved A-Star path planning method, simulation experiments are conducted in this section. Firstly, a normal path is generated by the proposed A-Star algorithm in consideration of obstacle, traffic separation and manoeuvrability risk, and the performance of path planning is compared between the proposed A-Star method and traditional A-Star method. Then, the berthing path planning is realized by the proposed A-Star algorithm, and the berthing path is evaluated. The combination of the normal path and the berthing path is conducted by the proposed A-Star algorithm and the performance of the combined path is compared with the uncombined path. Finally, the proposed improved A-Star method is used to generate the path in a real scenario in Yangtze waterway.

Case 1: Normal Path Planning
During the normal path planning, the obstacle risk and the manoeuvrability restriction risk are always considered, and it depends on the traffic rule that whether the traffic separation risk is considered or not. In this case, all these types of risks are all taken into consideration.

Setup
To model the obstacle risk, related model parameters need to be determined. It is known that the current velocity near the shore is smaller than one far away from the shore. Hence, v m (m/s) at node N [m] is set as: where d s (m) is the vertical distance from node N [m] to the nearest shore node. Other parameters are shown in Table 1. The vessel starts from the start node N [start] (3,3) to the destination node (32, 21).

Results
The normal path generation performance with a traditional A-Star algorithm and an improved A-Star algorithm for vessels is shown in Figure 8. From Figure 8, it can be seen that the traditional A-Star algorithm generates the optimal path only by taking the shortest path length as the standard. On the contrary, the improved A-Star algorithm considers the path length, obstacle risk, the traffic separation rule and manoeuvrability restriction simultaneously. Specifically, the path length of the traditional A-Star algorithm and the improved A-Star algorithm is 34.0 and 38.1, respectively. Generally, the improved A-Star algorithm will generate a longer path than the traditional A-Star algorithm.
The sum of path node obstacle risk for the traditional A-Star algorithm and the improved A-Star algorithm is 150 and 22, respectively. The path generated by the traditional A-Star algorithm has much higher risk to collide with obstacles. For instance, the vessel is too close to the bridge piers and other vessels, which could increase the collision risks enormously in Figure 8.
The path generated by the improved A-Star algorithm has taken into account the traffic separation rule, which decreases the risk of collision with other navigating vessels. Moreover, the manoeuvrability risk is also considered in the improved A-Star algorithm to avoid the vessel failing to track the generated path.
In summary, the improved A-Star algorithm can improve the navigating safety with slightly increasing the path length.

Setup
As previously mentioned, a vessel should berth into the current when the vessel needs to anchor or berth at a port. In this case, the vessel starts from the node of N [start] (6,3) to the port whose node is N [dest] (32, 21). The node number of berthing path P b is set to 6. The other parameters are the same as Table 1. The performance of the berthing path planning is presented as Figure 9.

Results
In Figure 9, the path generated by the A-Star algorithm without considering the berthing path is hard to berth to the port because the vessel cannot be manipulated well if the current direction is consistent with the vessel sailing direction. When the berthing procedure is taken into account, the berthing path P b and the berthing node N [berth] are obtained firstly according to Algorithm 3, specifically, the coordinates of N [berth] are (37, 18). Then, the normal path P n can be generated with the destination node N [dest] = N [berth]. Finally, P n and P b are combined to become the complete path P c .

Setup
When combining normal path P n and berthing path P b , a sharp turn could encounter in the combination node, which will result in that the vessel cannot track the path because of the restriction of vessel's manoeuvrability. In this case, the vessel needs to berth in an anchorage from node (10.11) to node (17,13). The parameters are the same as Table 1 except the node range of x axis changes to 28. The comparison of path planning with considering smoothing the path or without considering smoothing the path is presented in Figure 10.

Case 4: Path Planning in Real Scenario
A real scenario, Wuhan waterway in Yangtze River, is selected to verify the effectiveness of the improved A-Star method in this case. In this scenario, a vessel is planned to navigate from one side of the river to the other side of the river in a bridge zone, which is shown in Figure 11. The navigable area for crossing bridge is indicated by the two beacon vessels, i.e., the middle area between the beacon vessel is navigable. To apply the proposed path planning method, the grid length, ln, is set as 50 m, and the minimum radius of vessel, R 0 , is set as 45 m. The latitude and longitude coordinates of the start point is (30 • 36 02.6 N, 114 • 19 23.4 E), and the latitude and longitude coordinates of the destination point is (30 • 37 35.9 N, 114 • 19 36.0 E). Please note that the traffic separation rule is not applied by the navigation rule in this waterway. Sailing vessels in this scenarios are not recognized as the obstacles because only moored or anchored vessels are considered as obstacles for the global path planning in this article. The other parameters are set the same as Case 1.
The paths planned with the proposed A-Star algorithm, which consists of the normal path and the berthing path, and the traditional A-Star algorithm are shown in Figure 11, respectively. From Figure 11, it can be seen that the traditional A-Star has high risk to collide with the bridge and cannot guarantee the safety of berthing. To verify that a heuristic algorithm, e.g., A-Star algorithm, has a higher efficiency than a non-heuristic algorithm. The computation time records for A-Star algorithm with the improved A-Star algorithm and the search algorithm without heuristic guide for this case are 935 ms and 1127 ms, respectively. It means that the algorithm without heuristic guide actually has a higher efficiency. The path smoothed with the uniform B-spline curve of order 3 for the improved A-Star algorithm is shown Figure 12. The path in Figure 12 can be set as a safety path that can be followed by a vessel.

Conclusions and Future Research
According to the defects of the traditional A-Star algorithm when it is used for vessel path planning, an improved A-Star algorithm has been proposed in this article. Firstly, the risk models of obstacles (bridge pier, moored or anchored ship, port, shore, etc.) with currents, traffic separation, berthing, manoeuvrability restriction have been built. Then, the procedure of the normal path generation and the berthing path generation with the proposed A-Star algorithm have been depicted, respectively. Moreover, the problem of combining the normal path and the berthing path has been also solved. Four cases have been studied to verify the effectiveness of the proposed A-Star algorithm. The simulation and real experiment results show that the proposed A-Star path planning methods can avoid the high risk of obstacle collision, obey the separation rule and meet the requirements of berthing for vessels. In a word, the proposed algorithms can realize the trade-off between the path length and the navigation safety.
In future work, wind and water depth can be also considered during vessel path following, because wind will affect the ship navigation safety and economical efficiency, and shallow water will increase the vessel stranding risk.