Multi-Behavior Fusion Based Potential Field Method for Path Planning of Unmanned Surface Vessel

The problem of the unmanned surface vessel (USV) path planning in static and dynamic obstacle environments is addressed in this paper. Multi-behavior fusion based potential field method is proposed, which contains three behaviors: goal-seeking, boundary-memory following and dynamic-obstacle avoidance. Then, different activation conditions are designed to determine the current behavior. Meanwhile, information on the positions, velocities and the equation of motion for obstacles are detected and calculated by sensor data. Besides, memory information is introduced into the boundary following behavior to enhance cognition capability for the obstacles, and avoid local minima problem caused by the potential field method. Finally, the results of theoretical analysis and simulation show that the collision-free path can be generated for USV within different obstacle environments, and further validated the performance and effectiveness of the presented strategy.


Introduction
In the past decades, unmanned surface vessels (USVs) have been intensively researched due to the increasing demand from both military and civilian areas. USVs have more advantages over conventional manned-based vessels such that it can increase human safety and minimize human interference (Liu et al., 2016;Lazarowska, 2017). When being operated individually, USVs can be deployed in environmental and hydrographic surveys (Caccia et al., 2005) and pollutant tracking tasks (Xu et al., 2006). In addition, when collaborating with unmanned aerial vessels (UAVs), multiple USVs and UAVs can be deployed as a formation fleet to escort a mission (Liu and Bucknall, 2015;Fu et al., 2018). However, most of the USV used now are semiautonomous rather than fully autonomous USV. This is due to dangerous and complex marine environments and navigation errors. With the development of navigation technology (Liu et al., 2017;Ma et al., 2018;Zhang et al., 2018), we are constantly capable of improving the autonomy and reliability of the unmanned systems.
Path planning is an important part of the development of USV, which is to guide a vessel from an initial position to the destination while possessing accurate perception of the environment and effective collision avoidance capabilities (Zhuang et al., 2016). Path planning methods generally include two types: the pre-generative method (surrounding information and path generated prior are identified in advance) (Lin and Fu, 2017) and the reactive method (surrounding information is unidentified or partially unidentified) (Ye et al., 2006;Szyrowski et al., 2015) which is regarded as the 'dynamic path planning method'. Owing to uncertainties and complexities of actual marine environments, such as static obstacles (islands, buoys) and other dynamic obstacles (moving vessels and so on), it is changed in real-time, so it is impossible to obtain complete prior knowledge. In order to obtain the safe path, a series of computational approaches are studied, e.g genetic algorithms (GAs), graph search methods, artificial potential field (APF) etc.
GAs generates a group of probable paths which are evolved iteratively to pursue optimal results, using genetic operators (such as the mutation and crossover) (Cao, 2015). Nevertheless, shortcomings to GAs contain a lack of convergence and expensive computational costs, which mean the generated path may be a lack of consistency and limit real-time implementation, which makes it difficult for the vessel to track the planning path. Compared with GAs, graph search methods have better convergence and consistency due to using a discretized representation of the envir-onment. But, the vessels have the nonholonomic constraint, a further path smoothing procedure is required (Petres et al., 2007). And it may consume a high calculation time. Rapidly-exploring random tree (RRT) methods introduced by references (LaValle, 1998) do not require any resolution parameters to be explicitly set, so the RRT method can quickly and uniformly explore the environment space. However, the disadvantage of the RRT method is that it does not apply to dynamic path planning (Lolla et al., 2014).
Potential field algorithms through building an artificial potential field (APF) weigh obstacles and goal points (Khatib, 1986;Ge and Cui, 2002). The APF has become a key method due to effective collision avoidance capability and its easy implementation. It does not require processor consuming map calculations and estimation but directly gets input data from the range sensor. It is suitable for real-time control without global information, but for local minimum problems  (the vessel can be trapped in a U-shaped obstacle) it needs to be handled by global algorithms at a higher layer (Xue et al., 2017). Zhu et al. (2009) andWu et al. (2015) have improved APF approach to enhance planning performance. By combining a wall-following method, the local minima problem has been solved, which enables the vessel to leave the 'trapped' point following the edge of the obstacle. Besides, combinatorial strategy for APF has been developed by introducing the ant colony optimization (ACO) (Lazarowska, 2015). However, most potential field methods are used in static environments. When the vessel encounters moving obstacle, or experiences environmental changes, it will need to re-plan the path to avoid collisions, and it increases the computational burden.
Besides, behavior based approach has been developed in various work, and mainly be used for path planning of robots. Brooks (1986) firstly proposed a behavioral coordination technique for inclusive control structures, which were recognized as the basis of behavior based path planning. Further behavior-based path planning process is decomposed into a series of independent modules and sub-behaviors. On the one hand, Chen and He (2015) presented a behavior fusion path planning with fuzzy logic for indoor mobile robot during the autonomous navigation, in which positioning method of the robot uses Radio Frequency Identification technology (RFID). In Zhu et al. (2010), behaviorbased solution is proposed for mobile robots to solve the local minima issue by potential field method, and the path memory is incorporated in the improved wall-following to enhance the robot's cognitive ability to the environment. Thereby, this algorithm can weaken the blindness of the robot for decision making, which is of great use to select appropriate behaviors when facing different situations. On the other hand, Ma et al. (2014) proposed a multi-behavior fusion approach, which contains three basic behaviors that were designed to complete the path planning procedure.
Meanwhile, the angle-compensation method was applied to escape the local minimum. Kamil et al. (2017) developed a new navigation way to avoid unknown dynamic obstacles, based on future trajectory predictions and prior behavior of obstacles that consisted of various shapes. Nevertheless, previous work (Borenstein and Koren, 1989;Yun and Tan, 1997) is always limited to a simple environment due to the insufficient switching conditions. So, it is still an open question for path planning of USV within static and dynamic environment.
In view of the problem mentioned above, our work is devoted to developing a multi-behavior fusion based potential field method for USV with static and dynamic obstacles environment, which can not only avoid local minima caused by potential field but can also plan the safe path. Meanwhile, the detailed explanation of the different behavior and its activation conditions is presented in this paper. The block diagram of the structure can be seen in Fig. 1. Moreover, simulation results verify the effectiveness of the proposed approach, and show that it can be successfully used for marine environments (including general obstacles, trap obstacles, moving obstacles and hybrid obstacles). The main contributions of the paper can be summarized as follows.
(1) For path planning of USV in the presence of static and dynamic obstacles environment, a multi-behavior fusion based potential field method is proposed, including goal-seeking behavior, boundary-memory following behavior and dynamic-obstacle avoidance behavior. In order to ensure the switch between different behaviors, the activation conditions are designed.
(2) Memory information is introduced into the boundary following behavior, which can memorize and judge the gap and sub-goal point between obstacles. Thus, it can enhance cognition capability to the obstacles and avoid local minima.
The remaining structure of this paper is as follows. The problem of planning space representation is described in Section 2. In Section 3, the multi-behavior fusion based potential field method with its activation conditions design is proposed for path planning. Simulation and results analysis is carried out in Section 4. Eventually, conclusions and future endeavors are drawn in Section 5.

Planning space representation
In order to plan the safe path in a static and dynamic environment, it is necessary to represent the planning space of USV. In the marine environment, USV usually cannot obtain complete prior knowledge. Without losing its generality, we make the following assumptions.
Assumption 1. The position of both the USV and the goal is known, and the initial position and the goal position are not the same.
Assumption 2. The irregular obstacles are approximated and enclosed by a circular shape.
Assumption 3. The obstacle velocity is less than or equal to that of the USV, and the motion is a uniform linear motion.

Kinematic models of USV
To simplify the algorithm, USV is assumed to be a three-degree of freedom (3-DOF) including surge, sway and yaw (Liu et al., 2016). Thus, the kinematic model is formulated as: where, are the position and direction angle in the earth-fixed reference frame ({E}-frame), denote the linear and angular velocity in the body-fixed frame ({B}-frame), and is the transformation matrix. According to Fig. 2, Eq. (1) can be equivalently expressed as: denotes the resultant velocity for USV, and is the sideslip angle.

Obstacles
The obstacle velocity and direction are defined as , the first of which specifies the resultant velocity; and the other one denotes the obstacle direction measured by its angle relative to the positive x-axis (refer to Fig. 6). For the calculation of angle, the anticlockwise direction is used as the positive direction. Obstacles may be stationary or dynamic, and their position, velocity and orientation are random. Thus, the information of the obstacles is unidentified. Whereas, the position of the obstacle is well detected by sensors in USV, and the direction, radius and equation of motion for obstacle is calculated by sensor data (refer to Section 3.4.1).

Environmental representations
R About the marine environment, the USV should not collide with any obstacles, such as an island, shallows, buoys, lobster traps, fishing nets, submerged rocks, other moving ship, etc. Meanwhile, the USV is equipped with sensors (including radar, GPS and multi-beam forward looking sonars, etc.), which makes a visible scan and can sense obstacle information. In Fig. 2, is the maximum detection-making distance for a sensor. When the USV reaches a new position in the configuration space, it first detects and stores its distance to the neighboring obstacles.
The USV calculates the position, velocity and direction of the obstacle with two sequential repetitions (time intervals) and calculates the radius and motion equation of the obstacle. If the calculation reveals that the obstacle position does not change (refer to Section 3.4.2), then the obstacle is static; otherwise, it is dynamic. The information from the data sensor is used as data input, and the proposed algorithm will decide the next behavior depending on the activation conditions.

Strategy design
The flow chart for multi-behavior fusion based potential field method of the USV is shown in Fig. 3. In this section, we describe the concept and implementation process of the proposed strategy. Furthermore, the architecture can be adjusted and improved by increasing or decreasing the number of behaviors. The algorithm of multi-behavior fusion based potential field method will be described in detail as follows, such as functions, activation conditions, etc.

Potential field methods
The attractive potential function used for goal seeking is as (Ge and Cui, 2000): The repulsive function used for obstacle avoidance is as follows: In Eqs. (3) and (4), and represent the attractive and repulsive proportional gains of the functions.
is the distance between the USV and the goal. denotes the constant; means the number of obstacles, and varies from 1 to ; is the influence distance constant of the obstacle, and is the minimum distance from the USV and the obstacle. The distance is dependent on Euclidean distances and vector algebra.
The total forces are calculated by taking the derivative of the potential function based on Eqs. (3) and (4), respectively.
Besides, the USV resultant velocity and angular velocity are used to drive the USV from its actual configuration. The linear and angular control laws are formulated as follows.
where, and are the gain coefficients; and are the maximum values for resultant velocity and angular velocity, respectively; denotes the direction angle of the total force.

Goal-seeking behavior
The implementation of goal-seeking behavior is simple, and it firstly eliminates the deviation between the current and the goal direction, and then moves straight toward the goal (according to Assumption 1). From Fig. 2, the USV initially navigates toward the goal with the distance and angle by the direction angle equation and the Euclidean equation between the initial position and the goal position , respectively, as follows: Behavioral function: Performing a straight line motion along the goal direction.
a ∨ b ∨ c Activation conditions: Condition-(a) shows no obstacles in the decision-making distance. Condition-(b) is devised because the goal is located between the USV and the obstacles. The satisfaction of condition-(c) can withdraw the boundary following behavior (see Theorem 1). Then, clockwise or anticlockwise denotes the bypassing direction of USV; (see in Fig. 4) is the direction from USV to goal (if the direction is at the left side relating to the course of USV, its value denotes positive, otherwise negative); is the threshold angle that is used to protect the noise disturbance and theoretically can be zero. ∨ ∧ Additionally, ' ' means logical "or" and ' ' means logical "and" in this paper.
As the situation is shown in Fig. 4, without loss of generality, when USV is boundary following along the static obstacles in clockwise, . Theorem 1 (Zhu et al., 2010): After USV withdraws the   boundary following behavior, it cannot meet any point belonging to the obstacle in the goal direction. In other words, the condition-(c) shows that obstacles have been bypassed, and then it performs the next step.
As shown in Fig. 4, assume that Point on the left side of USV belongs to obstacle at this moment. When choosing any Points on the obstacle and connect , there are two cases as follows: CD AM F Case 1. Line and Line intersect at Point (behind the USV).
CD AM E Case 2. Line and Line intersect at Point (in the front of the USV).
The definition of convex polygon is first given as: any point on the connection-line of any two points in convex polygon is inside the convex polygon. , which is defined as Point . Similarly, Point must be inside the obstacle . Since USV always keeps a safe distance ( ) from the boundary, Point lies on the vertical line between the USV and the boundary, and their distance is smaller than the safe distance. Hence, Point must not belong to obstacle . With the same reason, does not belong to obstacles . So the assumption is also unreasonable.

O j
In view of the above analysis, there must not have been the points belonging to the obstacle in the front left of USV. Theorem 1 is proved.

Boundary-memory following behavior
Boundary following behavior can be utilized to avoid local minima, especially when encountering trap obstacles (such as long-shaped and U-shaped groove obstacles) during path planning using the potential field method. Moreover, memory information is introduced to enhance cognition capability to the obstacles environment and judge the gap between obstacles.
Behavioral function: This behavior allows the USV to judge the gap and sub-goal point between obstacles and moves along the obstacle boundary to bypass the obstacles. Meanwhile, it can avoid the local minima problem.
a ∨ b ∨ c Activation conditions: Condition-(a) indicates that the sum of the attractive force and the repulsive force is close to zero ( is a small positive number), which describes the case when the USV is at a local minimum. Condition-(b) indicates that the USV has little displacement during . , and is the sampling time.
is the displacement size of USV over a fixed time and is a threshold distance. In Condition-(c), is the displacement size of USV between Points and ; denotes percentage, and means the traveled distance of the USV in . It means that the movement for USV is of long distance but the displacement is very small. Sometimes, the USV is fallen into the local minimum. Owing to the inertial (Borenstein and Koren, 1989), it oscillates or moves in the closed loop and cannot be detected by Condition-(a). Therefore, Conditions-(b) and (c) design time and space constraints to detect this situation. In the trap situations, the USV easily falls into a local minimum as presented in Fig. 5, which often happens when using the potential field method. However, boundary following behavior incorporates into memory information and judges the gap and sub-goal point between obstacles, and then it moves along the obstacle boundary to bypass the obstacles and avoid the local minima. Moreover, memory information refers to the USV in need to memorize sub-goal points and judge obstacles gap, which can enhance cognition capability to the obstacles. As shown in Fig. 5, sub-goal points for obstacle detection in Position are and , respectively. Then, the next sub-goal points for detection in are and , respectively.
Therefore, and are the detection of the sub-goal points in . It can be seen that point is not within the range detected by , but it is historical detection information. Similarly, USV detects the obstacle sub-goals and at point . Among them, point denotes the start point. According to Theorem 1, point is the end-point of boundary following. In addition, when obstacles are too close, the USV needs to calculate and store the obstacle gap to determine whether it can be passed. Meanwhile, the USV can pass the gap required to meet the conditions (refer to Fig. 5). Where, is the width of the USV, , and is a safe dis- FU Ming-yu et al. China Ocean Eng., 2019, Vol. 33, No. 5, P. 583-592 587 tance to avoid USV too close to the obstacles and ensure the planned path safe.

Dynamic-obstacle avoidance behavior
After the dynamic obstacles enter the detection-making distance, the distance between the USV is determined, and the position information for the moving obstacle is detected. Then, the equation of motion and radius for obstacle is calculated by sensor data. Moreover, the USV takes the necessary obstacle avoidance strategy and departs from its original path if the moving obstacle and the USV have the danger of colliding, namely, satisfying the activation conditions.

Calculation of obstacle trajectories
First of all, the polar coordinate of point ( ) of obstacle is , which are the known sensor parameters, and , . The polar coordinate of the center point of the obstacle is , and indicates the position in the {E}-frame. An obstacle is assumed to enter the detection-making distance for USV position at , and the obstacle positions are and at the and time, respectively. Depending on the known conditions, simple mathematical analysis yields (refer to Fig. 6): Therefore, the following formula is obtained . The obstacle equation of motion is:

Analysis and strategy of dynamic obstacle avoidance
For every step of the proposed algorithm needs to judge whether the instantaneous positions of USV and moving obstacles have the risk of collision. There are two cases and strategies as follows.
Behavioral function: When USV has the risk of collision in the forward direction, this behavior makes the USV evade dynamic obstacles by bypassing and accelerating.
Case 1: The relative movement of USV and obstacle is on the same direction or opposite direction, as shown in Fig. 7.
Conditions-(a) and (b) indicate that dynamic obstacles were detected. In Conditions-(c) and (d), there is a risk of collision. Where, indicates the vertical distance between the trajectory for an obstacle and the USV, and is the parameter to be designed, and then the obstacle velocity is lower than the USV velocity. Condition-(e) represents the same direction (USV behind an obstacle) and opposite direction movement.
Strategy 1: When the activation condition of Case 1 is satisfied, the USV is bypassing the dynamic obstacle above or below it.
Case 2: The path for USV intersects the obstacle trajectory, which is shown in Fig. 8.   According to Fig. 6, represents the arrival time of the intersecting point for dynamic obstacle and USV, and indicates the safe time interval when USV encounters a dynamic obstacle. Such values can be viewed as indices to indicate the safety of the cross point. Condition-(c) shows that the obstacle or the USV first arrives at the intersection, respectively, and the time interval is less than . Hence, there is a danger of collision between USV and dynamic obstacles, and need to implement the following collision avoidance strategy.
Strategy 2: When the USV first arrives at the intersection, it accelerates through the intersection point. Otherwise, the USV is bypassing the rear of the obstacle.

Multi-behavior fusion based potential field method
As mentioned above, the potential field method combined with simple behavior switching strategy is shown in Fig. 3, which is with the advantages of concise structure and being easy to implement for its burdensome superiority, and avoid local minima. Based on Eq. (16), USV can decompose complex path planning process into a simple sequence of behaviors.
where, , and express the behavior of goalseeking, boundary-memory following and dynamic-obstacle avoidance behavior, respectively. , and represent switch parameters of three behaviors, i.e. activation conditions.
The proposed approach firstly applies potential field method, which motivates the USV to leave its original position to a destination position. Then, USV senses the surrounding area according to sensors to determine if there are obstacles within the detection-making distance. If the destination is reachable, the USV moves directly to the goal. Otherwise, USV encounters static or dynamic obstacles, and then it is determined different behaviors by the activation conditions. If the potential field method falls into local minima, the boundary-memory following behavior is adopted.
If it is dynamic obstacle, the USV will calculate the trajectory of the moving obstacle, and determine whether to start dynamic-obstacle avoidance behavior. Therefore, if the algorithm finds its next position and direction based on the proposed algorithm, it will replace the position of the USV until it reaches the goal point. Otherwise, the proposed algorithm fails.

Simulation
To validate the performance of the proposed algorithm, simulation studies are performed with different environment setups: static obstacles (general and trap situations) and hybrid obstacles environments (static and dynamic situations). Information about the positions, velocities and directions of static and dynamic obstacles is assumed to be incomplete prior knowledge. Additionally, different types of traps can be distinguished, such as long-shaped obstacles and U-shaped groove obstacles. Evaluations of the proposed algorithm performance and the planned path for USV are provided in Fig. 9−12. The black and blue objects are  the static and dynamic obstacles, and the initial point and goal point for USV are marked as pink and green as shown in the following simulation results. Besides, USV instantaneous positions with its planned path are drawn as red dots marker. Simulation parameters are defined as follows. Moreover, it should be noted that these parameters can be adapted to static and dynamic environments. , ,

Simulation in the static obstacles environments
Static obstacles are divided into two situations to verify 700 m × 700 m local minimum problem: general static obstacles and trap situations (including long-shaped obstacles and U-shaped groove obstacles). In the simulation environments, the dimension of the simulation area is . First of all, Fig. 9 illustrates that the USV was commanded to move from different start points ((−80, −80), (−80, 120), (−90, 300), (−90, 300)) to different goal points ((500, 500), (450, 350), (550, 300), (550, 300)) and then the USV moves towards goals and reaches there with the time steps of 58, 42, 58, and 44, respectively. Fig. 9a avoids 8 general static obstacles with different positions and sizes. When the USV encountered the local minima, as shown in parts (b−d), it could use boundary-memory following beha- Fig. 10. Behavior based on potential field method in Yun and Tan (1997).  vior and move along the boundary of the obstacle to bypass it, then successfully avoid local minima and reach the goal point. In addition, when obstacles are too close, the USV can calculate and memorize the obstacles gap to determine whether it can be passed (see in Fig. 9d). It can be noticed that, compared with part (c), part (d) has a shorter time by judging the obstacle gap and also avoid the local minima caused by the potential field. Yun and Tan (1997) designed a switch condition between APF and wall-following method to improve APF algorithm and escape local minima. However, in some trap environments, the path planning can still fail, for example, with U-shaped groove obstacles as shown in Fig. 10. In reference (Yun and Tan, 1997), we identified the following deficiencies: (a) Trap situations due to local minima (see Fig. 10a).
(b) Cannot judge and memorize obstacle gap, and path planning failed (see Fig. 10b).
By contrast, the method presented in this paper cannot only avoid the local minimum, but also plan path by judging the obstacles gap.

Simulation in moving and hybrid obstacles environments
In order to test the ability of the proposed algorithm to handle moving vessels and hybrid obstacles (the multi-static and dynamic obstacles), further simulation studies are conducted. For moving obstacles and hybrid obstacles situation, the dimension of simulation areas are 500 m×500 m, 850 m×850 m, respectively.
(−80, −80) First of all, USV moves from the start point to the goal point (300, 300) and encounters situation with 3 moving vessels. Then, the USV completes the dynamic obstacle avoidance and reaches the goal point with the time step of 62, as shown in Fig. 11a. Later, in Fig. 11b, distances between USV and moving vessels are recorded. It is worth noting that the closest distances for USV and vessels are 10.2 m, 1.7 m and 12.5 m, which indicates that USV can effectively avoid moving vessels and plan a safe path. Fig. 11c shows that the direction of USV changes frequently between the time step of 20 and 40 due to the implementation of dynamic-obstacle avoidance strategy. (−80, −80) Furthermore, hybrid obstacles with the start point and the goal point (650, 650) have 5 static obstacles and 3 dynamic obstacles, which complete the obstacle avoidance with the time step of 65. Fig. 12b shows the distances between dynamic obstacles and USV throughout the simulated time period. Smallest distance appears at the time step of 16 with the distances of 4.1 m between Dynamic obstacle 2 (DO2) and USV, which indicate that USV can efficiently plan collision-free paths. Fig. 12a shows the planed path for USV, and Fig. 12c denotes the changes of the direction for the USV. In all the cases, the USV maintains a safe distance from static and dynamic obstacles, which also prove the effectiveness of the proposed algorithm. Through designing activation conditions, the multi-behavior fusion based potential field method cannot only plan the path but also avoid local minima caused by potential field. The simulation results verify the effectiveness of the proposed method.

Conclusions and future endeavors
To aim at the path planning of USV in static and dynamic obstacles environment, multi-behavior fusion based potential field method has been proposed, which are developed with three behaviors, i.e. goal-seeking, boundarymemory following and dynamic-obstacle avoidance. As compared with previous researches, activation conditions are designed to ensure switching among different behaviors. Besides, we design three different simulation environments to show that the proposed algorithm can effectively memorize and judge the gap between obstacles, and avoid local minima problem caused by a potential field. Finally, the simulation results verify the effectiveness of the algorithm for USV path planning in static and dynamic obstacles environment.
For future work, the algorithms proposed will be improved so that the practicability of path planning can be further increased. Firstly, the proposal is to make use of the USV physical model and demonstrate its feasibility in a real marine environment. Secondly, the optimization method would be utilized to improve the performance of USV with consideration of energy consumption. Thirdly, the developed method can be further extended to handle more complex cases, such as dynamic-obstacle avoidance, i.e. the obstacle of any motion curve. These future efforts will benefit the development of autonomous USV, which is also the ultimate goal of our research.