Online planning low-cost paths for unmanned surface vehicles based on the artificial vector field and environmental heuristics

The study is concerned with the problem of online planning low-cost cooperative paths; those are energy-efficient, easy-to-execute, and low collision probability for unmanned surface vehicles (USVs) based on the artificial vector field and environmental heuristics. First, we propose an artificial vector field method by following the global optimally path and the current to maximize the known environmental information. Then, to improve the optimal rapidly exploring random tree (RRT*) based planner by the environment heuristics, a Gaussian sampling scheme is adopted to seek for the likely samples that locate near obstacles. Meanwhile, a multisampling strategy is proposed to choose low-cost path tree extensions locally. The vector field guidance, the Gaussian sampling scheme, and the multisampling strategy are used to improve the efficiency of RRT* to obtain a low-cost path for the virtual leader of USVs. To promote the accuracy of collision detection during the execution process of RRT*, an ellipse function-based bounding box for USVs is proposed with the consideration of the current. Finally, an information consensus scheme is employed to quickly calculate cooperative paths for a fleet of USVs guided by the virtual leader. Simulation results show that our online cooperative path planning method is performed well in the practical marine environment.


Introduction
The online path planning (OPP) module is essential for unmanned surface vehicles (USVs) when unreliable or delayed communication links between human and USVs exist. The OPP problem for USVs faces challenges, for example, limited computational ability of the shipborne computer, limited communication bandwidth and the strong external interferences, unstructured or changeable environmental information, and complicated motion constraints of USVs.
Besides, USV missions are typically performed under the strict energy constraint and the current interference. However, the changes in the velocity and direction of the current are generally small in a certain spatial-temporal range, and the characteristics can be used to improve the energy efficiency of USVs by executing downstream paths when USV moves at a relatively small speed compared to the speed of the current. Sometimes, USV enhances the motion stability by executing countercurrent paths. But, USV has a weak ability of the lateral current resistance. Therefore, one of the planning objectives is to generate paths containing many downstream sections and a small amount of countercurrent sections with few transverse parts. Besides, the path length, path smooth, and obstacleavoidance (OA) requirements are also important.
High efficiency is the first requirement of an OPP scheme to quickly response to the changes of USV states and environmental information. Since USVs are underactuated, and some control variables are complex. Meanwhile, the sophisticated motion model and parameters are rare. Thus, computing a desirable and feasible trajectory can be very difficult by directly adopting control-based methods.
Meanwhile, the cooperative path planning problem is more complicated than that of planning for a single path. If we plan paths for each USV separately, the computational complexity is huge, and even deadlock problem exists.
The motivation of the research is to online plan cooperative paths, which are energy-efficient, smooth, and low collision probability.
Up until now, a number of notable results on USV OPP problem have been reported in the literature. The forwardlooking path planning methods are applied more often than the behavior-based method, which is short-sighted.
The artificial potential field (APF) method computes a vector field by calculating an attractive force field from the goal and a repulsive force field from obstacles. Then, USVs can navigate using gradient descent to follow the potentials in real time. However, APF has difficulties when USV sails in narrow passages, and it does not consider the motion model.
The rapidly exploring random tree (RRT) algorithm accelerates the planning process by avoiding the explicit and accurate modeling of the environment. 1 Moreover, RRT works efficiently for systems with differential constraints and nonlinear dynamics, because RRT does not require exact connection between states. Particularly, RRT handles constraints piecewise and deals with motion errors or sudden situations according to real-time feedback by expanding the path tree incrementally. Since the path refinement is crucial, the optimal RRT (RRT*) is chosen as the basic path planning method. RRT* ensures that nodes are reached through a minimum cost path by "rewiring" the path tree.
The basic contradiction in path refinement is the one between the limited online time and path optimality. Because the probabilities of sampling spaces containing the optimal solution are not assessed, RRT* converges slowly to the optimal path, and the OPP module is performed in a low bandwidth along with the autonomy hierarchy. Thus, the efficiency of RRT* is required to be improved. The optimal result is hard to be achieved online. Thus, the heuristic schemes were extensively researched to promote the planning speed of the RRT method. 2 A Gaussian sampling method was applied to obtain more environmental information using less samples for exploration than random sampling scheme to improve the OA ability of RRT. 3 The environmental current field can be used as the heuristics for designing an energy-efficient path planning method. A numerical ocean model was constructed by Holland and McWilliams. 4 A method was introduced by constructing the model for the current with the B-spline surface. 5 The current disturbance was considered by Singh et al. 6 to plan energy-efficient paths. The current was considered during the path searching process to plan for paths with minimum energy expenditure. 7 An approach was proposed to guarantee the existence of a path in strong current fields by combining a cost function and optimization methods. 8 An energy-efficient path planning algorithm was proposed to address the challenges with the presence of the current. 9 A path planning method was proposed for USV under uncertain, inaccurate, and dynamic marine information. 10 An energy-efficient path planning algorithm was proposed to consider the current based on the A* method, and a realistic case of an autonomous underwater glider surveying the Western Mediterranean Sea was considered. 11 The problem of optimizing the energy cost was considered by Alvarez et al. 12 The USV speed was computed according to the input USV velocity and the current velocity by Garau et al. 13 The environment was modeled by the artificial vector field according to the known information by Lawrence et al. 14 and Chen et al. 15 A vector field was constructed by following a Dubins path based on the Lyapunov stability principle. 14 The path was defined as the intersection of two surface functions. 16 A continuous time-variant vector field was proposed based on the n-dimensional curve, which was defined by the intersection of n À 1 curved surfaces. 17 A Lyapunov vector field was produced to control robots to stably convergence to a cycle behavior. 18 The common USV formation control strategies include behavior-based approach, leader-follower approach, virtual structure approach, 19 and the methods based on consensus theory and graph theory. 20 The consensus theory was proposed to unify the leader-follower, behavior-based, and virtual structure approaches into the framework of consistency theory. 20 The cooperative formation control based on consistency algorithm can realize multichannel compound control to avoid obstacles.
The formation control problem for USVs was addressed by a distributed strategy based on virtual structure strategy. 21 A hierarchical control framework and relevant algorithms were proposed for autonomous navigation of USVs. 22 A three-layered architecture was devised for the real-time implementation of USV OA problem. 23 An improved APF method was responsible for avoiding obstacles smoothly. 24 An improved APF method was employed using ring-shaped repulsion for collision avoidance and OA. 25 The full-state regulation control problem for USVs under disturbances was solved by Wang et al. 26 Accurate trajectory tracking control problem of USVs disturbed by complex marine environments was addressed by Wang et al. 27,28 The novel contribution of the study is summarized below: (1) to effectively guide the USV path planning process, an artificial vector field is applied by following the reference path and the current. The vector field is used as the heuristics to plan for energy-efficient and low-cost paths. (2) The vector field following Gaussian sampling and multisampling schemes is used to improve the efficiency of RRT* to obtain a low-cost path for the virtual leader of USVs. The cooperative path planning strategy is investigated based on the distributed consensus. (3) A bounding box construction method is presented to improve the collision detection (CD) accuracy by taking the velocities of USVs and the current into account.

Online path planning framework
The framework of the cooperative paths planning system is built based on the virtual structure method and the information consensus scheme. We first plan paths for the virtual leader, then a follower computes its path according to the positions and velocities of the virtual leader and other followers based on the information consensus. Figure 1(a) shows the core hardware and software modules of the OPP system for the virtual leader. To maximize the given information, a global reference path is planned according to the known obstacle and current information. The central controlling module is mainly in charge of the global reference path planning, submodules scheduling, and the synchronization management. The numbers at the right bottom of modules show the sequential logic of the system. The scheduling sequence of the OPP module is the second. The OPP module plans low-cost paths locally using real-time USV states and environmental information.
To adapt the reference path, we model the environment by constructing a vector field that follows the global path and the current. To satisfy the feasibility of the path, we planned paths' online subjects to the USV motion model. To plan energy-efficient paths, the directions of USV motion and the current are considered.
The OPP method is proposed based on the model predictive control scheme. The most recent data are not necessarily the most suitable in a real-time system. 29 Thus, the OPP module sends a committed path to the central controlling module at specified update points, and the committed path is an unchanged one that is not influenced by the OPP process. The time difference between two adjacent update points (d e ) is the duration of the execution domain. The OPP module predicts a local path in the prediction time domain d c . The virtual leader is supposed to move along the committed path, and the adapting section subsequent to the next updated point is refined in d e time. If the reference path intersects with the perception boundary, then the intersection point closest to the goal is chosen as the subgoal of the local planning iteration, else, the nearest waypoint of the reference path is selected as the subgoal. Followers compute paths after cooperative information from the virtual leader and other followers are obtained. Figure 1(b) shows the cooperative paths computing framework for the followers. The experimental motion model is as follows  30 t u ¼ (F port þ F stdb ) denotes the surge force, t r ¼ (F port À F stdb )ÁB u /2 is the yaw moment, F port and F stdb are the thrusts from the propellers on the port side and starboard side, respectively, B u is distance between the propellers

Unmanned surface vehicle motion model and constraints
where the coefficients are derived by Mousazadeh and Kiapey. 30 The length L of the hull is 1 m and the width W is 0.25 m, the average depth of underwater penetration D is 0.3 m, and r is the water density. We set: The t wr (t) is the torque of current on hull.

Global path optimizing
To maximize the given environmental information, a global reference path is optimized using the A* method according to the known environmental information.  corresponds to a sequence of consecutive blank cells between the starting cell and the goal cell. The side lengths of the cells are defined as where v min is the minimum velocity of USV and Dt is the extension step duration of RRT*. The heuristic cost function for A* is defined as follows where g i denotes the center of a cell, dis(g i , goal) is the distance from g i to the goal, and angle(g i , c i ) is the angle between the directions of exploration and the current. The exploration direction is the direction of the segment from a waypoint to a g i . The A* method assumes that USV is omnidirectional without considering the motion characteristics of USV. Thus, the reference path is just used as a guidance of OPP process, and it should not be complicated and zigzagged. We try to prune redundant waypoints sequentially. The pruning start point is selected as the first waypoint and the end point is defined as the third point initially. If the line between the start and end points is collision-free, then the intermediate point is deleted and the end node moves to the next point. The iteration is performed until the line between the start and end points intersects with obstacles. Then, the next start point is chosen as the last end point.
Polygonal paths cannot represent the characteristics of USV motion. Thus, we smooth the reference path via Dubins curve, which is proven to be the shortest distance between vectors under the minimum turning radius constraint via using geometric analysis. We assume that the USV moves from a configuration to another one according to the Dubins maneuver. Dubins curve is composed of arcs with the minimum turning radii and their common tangents. The waypoints in the vector model are known as tangent points acquired by differential geometry.

Artificial vector field-based environment model
A vector field following the Dubins path and the current is constructed in the discrete environmental map to guide the OPP process. The magnitude of the vector at each position is related to the expected velocity of USV at the grid. The bigger the distance from USV to the reference path, the bigger the speed of USV. The expected magnitude of USV velocity is calculated as follows where d ur is the distance between USV and the nearest waypoint of the reference path, the weight l is set as 0.5, v max is the maximum speed of USV, and it is set to be10 kn (5 m/s). According to Lawrence et al., 14 we define a positive definite and continuous derivable potential energy function V F (r) that satisfies the following conditions: if USV is not on a path F, If the motion characteristics of USV obeys The vector field guarantees USV to converge to the path F, where G (r, t, l) is a n Â n semipositive definite symmetric matrix, and only if USV is on the path, G (r, t, l) 14 The conclusion can be proved as follows: the vector field is negative semidefinite. According to the Lyapunov theorem, USV can converge to F. In the vector field, À[(@V F (r)/@r) G (r, t, l)] T provides the force on the direction of negative gradient of vector field to help USV converge to the curve F. Y (r, t, l) provides the force perpendicular to the gradient direction of the vector field and its sign determines the direction of USV that moves along the curve F. g(r, t, l) is the compensation for the time-varying change of vector field or the compensation for the USV speed on the curve, and it deflects the vector field to the direction of the USV velocity.
It is difficult to define a vector field that can follow an arbitrary style of curve. Thus, we smooth the reference path via Dubins curve. Then, we design a vector field that follows the Dubins path. We define two kinds of vector fields that follow arcs and segments, respectively, then we integrate the two vector fields to follow an arbitrary Dubins path.
When the vector field follows arcs, the potential field function is defined as V F ¼ 0:5ðrðtÞ À RðtÞÞ 2 , where , and (x, y) is the USV position, (x 0 , y 0 ) is the center of the horizontal maneuver arc. @V F @r ¼ ðrðtÞ À RðtÞÞr r . We set G ðr; t; lÞ ¼ I 2Â2 ðr;t;lÞ , Y ðr; t; lÞ ¼ l 1r D ðr;t;lÞ , and gðr; t; lÞ ¼ l2rr ðr;t;lÞ , where ðr; t; lÞ ¼ ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ffi . Ther D guides USV to move on the direction of the tangent of the curve, whereasr r is on the gradient descent direction and it guides USV to move toward the curve, and where the function sgn(R -r(t)) means the sign of the item R -r(t). The parameter l 1 is the weight of the influence of the yaw motion on the velocity direction, and l 2 is the weight that influences the change of the vector field with time. Therefore, we can get f ðr; t; lÞ ¼ ÀðrðtÞÀRðtÞÀl 2 Þr r þl 1r D ðr;t;lÞ , where jf ðr; t; lÞj ¼ v E . The vector direction is as Figure 3(a) shows the vector field following an arc, and the parameters of l 1 and l 2 are values to make the vectors to return to the path and steer to the direction of the path. Figure 3(b) shows that the trend of vectors is more likely to follow the velocity of USV (steering to the direction of the path) on the path than to return to the path along with the increment of l 1 . The vectors close to the path are nearly parallel to the path without pointing at the path. That makes USV moves parallel to the path before it returns to the path. The velocity following the trend of vectors decreases after l 1 is reduced, as shown in Figure 3(c), the vectors are more intended to point at the path than following the velocity parallel to the tangent of the path. Figure 3(d) shows that after l 2 is increased, the vector field is more intended to return to the path than turning to the direction of the path. We set l 1 to be 15 and l 2 to be 0.3.
When the vector field follows a segment path, the potential energy function in defined as V F ¼ 0.5r(t) 2 , where r(t) is the distance between USV and the segment where (x 0 , y 0 ) is the projection of USV on the segment path. In the vector field function, we define G ðr; t; lÞ ¼ I 2Â2 ðr;t;lÞ , Y ðr; t; lÞ ¼ l 3r D ðr;t;lÞ , gðtÞ ¼ 0, thus, f ðr; t; lÞ ¼ ÀrðtÞr r þl 3r D ðr;t;lÞ , where ðr; t; lÞ ¼ ffiffiffiffiffiffiffiffiffiffiffiffi ffi The directions of vectors in the field can be calculated as The weight l 3 is set by the expected motion of USV following the reference path, and it is set to be 0.1. Figure 4 shows the vector fields with different parameters. Figure 4(a) shows that the vectors can point to the path while turning to the direction of the path. Figure 4(b) shows that the vector field has a bigger intention of returning to the path than steering to the direction of the path when l 4 is reduced, l 4 is set to be 2 in the study.
The vectors at the intersections of a segment and a circle on the Dubins path are the resultant vectors of the one following the segment and the one following the circle. The combined vectors exist within a scope around the intersection. Figure 5 shows the vector field following the  To consider the current in the definition of the vector field, the vector field is integrated with the current field. The vector field is proven to guide the USV to follow paths stably. However, the vector field does not consider the USV motion model and the OA problem. Thus, the vector field is used as the heuristics of OPP. In a USV system, the CD problem between formation members especially when changing formation is necessary to be solved. A formation potential field is designed based on the spring-damping model that consists of a "piston" and a "Hooke spring" with a nonzero static length. When the piston is pressed inward from the balance point, the spring will produce a repulsive force to prevent the piston from moving forward. When the piston moves forward to a certain distance, the spring resistance will prevent USV from moving forward. On the contrary, the spring will produce a pulling force to pull the piston back to the balance point. The principle is applied among the members to keep the activities of each USV within their own allowable range and ensure the formation of the team and the safety of the members. According to Hooke's law, the elastic potential field between USVs is as follows where U h ij is the elastic potential field between USV i and USV j , k h ij is the elastic potential field coefficient (it is set to be 25). r ij ¼ D ij -E ij , where D ij is the actual distance between USV i and USV j while E ij is the expected distance between USVs.
The elastic forces between members are computed as and if the distance between USVs is lower than a threshold (0.5), the force is set to be infinity. The related acceleration to the force is set as F h i =m, where m denotes the weight of USV. Meanwhile, the OA priorities of USVs are determined before the sail begins. The low-priority members give way to the high-priority USVs to avoid collisions.

Heuristic path tree extension scheme
Heuristic cost function. An RRT*-based OPP method is proposed to plan paths for the virtual leader. The vector field is used as heuristics in the sampling process of RRT*. A heuristic path tree extension scheme is presented to  improve the path quality and improve the path planning efficiency of RRT* in obstacle complex areas.
RRT* converges slowly to the optimal path. Thus, a heuristic RRT* method is proposed to efficiently search for and refine paths. The direction of path exploration is controlled and adjusted during the path exploration process of RRT*. To refine paths, a cost distance is defined by considering the energy expenditure, execution difficulty, and safety, as follows where a T is the angle between the extending direction (from q i to q j ) and the USV velocity vector, and a T is considered to guide the algorithm reducing the numbers and angles of turns on paths. The variable d obs is the minimum distance from the linear path between q i and q j to obstacles within a scope S centering at USV, and a i is the angle between extending direction and the direction from q i to the center of the nearest obstacle and v i is the resultant velocity of the velocity of USV on the path between q i and q j and the velocity of the current, and the item v i ÁDtÁcosa i is the estimated distance that USV moves toward an obstacle and maxf0, g o (d obs À v i ÁDtÁcosa)g is equal to 0 if the braking distance is not enough, e is a small number (set to be 0.01) that avoids the numerator of the second item being 0. Thus, the cost distance is rather big, if the linear path between two nodes gets very close to an obstacle. a y is the angle between the extending direction and the direction of vectors in the vector field. The dist(q i , q j ) is the Euclidean distance between q i and q j . The angles are limited as 0 a T p, 0 a i p, and 0 a y p. We set Thus, the cost distance definition considers the planning objective as follows: the first item considers the tortuosity of the path; the second item considers the collision probability of the path; the third item considers the energy efficiency of the path through the vector field that takes the current into account; and the formula also considers the path length by the variable dist(q i , q j ).
Heuristic sampling scheme. RRT* also has difficulty in searching for paths in obstacle-complex areas. Thus, a heuristic sampling scheme is proposed based on a Gaussian function 3 and the transition-based RRT method 2 to help RRT* generating reasonable samples to improve its path searching and refining ability.
The Gaussian sampling scheme is used to generate samples near the obstacle boundary to find a short OA path quickly according to the sampling of historical information. It spawns several samples and chooses the one that lies close to obstacles. The method may accelerate the OA path planning procedure because the samples near obstacles may help to find OA paths; the computational complexity of spawning an obstacle-free sample is much lower than that of the path extension process. The Gaussian function is defined as follows 'ðc; sÞ ¼ 1 where s is the standard deviation of the Gaussian distribution, it is determined by the smallest distance tolerance of USV toward obstacles, we set it like two times of the USV length.
We define a function for assessing the probability of a sample q s on the boundary of an obstacle as obsðyÞ Á 'ðq s À y; sÞdy, where obs(y) ¼ 1 if a configuration y locates at an obstacle-occupied area, and obs(y) ¼ 0 if y is in an obstacle-free area, and the integral scope is performed within an obstacle boundaryblurring domain D b . The radius of D b is set as three times of s. The function of f(q s , s) is used for blurring the obstacle boundaries as the application of Gaussian smoothing in the image processing area. The weights of the obs(y) are given by the Gaussian function '(q s À y, s). The closer a configuration y to the configuration q s , the higher the weight. The weights reflect the similarity of q s to the configuration y.
The Gaussian function-based fuzzy value of q s is defined as g(q s , s) ¼ maxf0, f(q s , s) À obs(q s )g, if q s is within the obstacle-occupied area, then g(q s , s) ¼ 0, else g(q s , s) ¼ f(q s , s). The function value of g(q s , s) is used as the fuzzy value of a sampled configuration q s to assess the probability of q s locating near the boundary of an obstacle in an obstacle-free area.
The path tree extension steps 1 and 2 are executed iteratively, as follows: 1) A sample q s is spawned randomly and the nearest path tree node q n is searched according to formula (6). 2) The nearest node q n is tried to extend according to the vector at q n when a randomly generated number rand 2 [0, 1] is lower than the threshold p (0.5). If the extension succeeds, then a new node is added to the path tree and the locally topologic path tree refinement (rewiring) is executed as the RRT* method.
If an extension collides with an obstacle, then the algorithm keeps the intersection point (C p ) of the extending section and the obstacle, and the OA path planning process is started as: several samples q s are respawned in the circular resampling domain centering at C p .
The collision-free samples are sorted by the cost distances from the nearest path tree node q n of a sample (q s ) to q s in the ascending order, and the Gaussian functionbased fuzzy values of samples are tested by the transition function that is defined as follows where K is a constant value (set to be 3) used to control the changing range of p(q s ). A random number R T 2 [0, 1] is chosen. If R T is lower than p(q s ), then the q s is kept and q n is tried to extend to q s , else go to the next sample. The iteration is executed until an extension succeeds. If all the samples are tried, then return to step 1. Else, a multisampling-based heuristic extension process is executed if rand exceeds p as follows.
Several samples q s are respawned in a resampling domain around q n . The collision-free samples are sorted by the cost distances from the nearest path tree node q n of a sample (q s ) to q s in the ascending order. Next, q n tries to extend to q s in order until an extension succeeds or all the extensions fail. Then, the algorithm returns to step 1.
The extension procedure of path tree has some randomness, which aims to search for high-quality paths by consideration of various candidate paths in a wider range. Figure 6 shows the illusion of the Gaussian sampling process, where q new is the newly generated path tree node and d is the radius of the resampling domain. The extension step length h ¼ v U Dt. The motion constraints are taken into account during the path extension procedure. Thus, the feasible extendable domain for a path tree node is fan shaped. We set the minimum turning radius as r min ¼ 4v/ o max , where v is linear velocity and o max is the maximum angular velocity of USV.
During the extension from q i to q j , the expected direction of USV velocity coincides with the extension direction. To consider the current, the USV velocity is redefined as the resultant velocity of the originally expected USV velocity and the current velocity. Then, q i is extended a step further on the direction of the resultant velocity.
The feasibility of the path is ensured since the path tree extends if the extension can pass the constraints checking. Once a local path is planned, the redundant waypoints are pruned from the path. The remained path is smoothed via Dubins curve.
Bounding box definition for collision avoidance. The accuracy of CD is important for the RRT-based methods, especially in the marine environment with the current. Thus, a bounding box is defined with the consideration of the USV velocity and the current velocity. The elliptic bounding box is used instead of a circular one to improve the OA efficiency of the planner because the USV body is asymmetrical. 31 Figure 7 shows the elliptic bounding box for USV CD, and the USV body-fixed frame fbg shows the USV motion frame. Figure 7(a) shows the traditionally symmetrical bounding box without considering the current. The direction of the Y-axis of the fbgframe coincides with the USV velocity direction. The long axis of the elliptic bow bounding box is defined as where v is the velocity of USV, L is the length of USV, Dt is the duration of a control step, and e U is a positive value and we set it to be e U ¼ 0.15 * v Dt, empirically. When USV sails, the probability of USV suddenly turning back is tiny. Thus, we set the short axis of the bow bounding box to be W b ¼ W þ e U , where W is the width of USV. The long axis of the stern bounding box is defined to be L s ¼ e U . Figure 7(b) shows the asymmetrical bounding box with the consideration of current, and the direction of the Y-axis of the USV body-fixed frame does not coincide with the direction of the resultant velocity of the USV velocity and the current velocity. The bounding boxes of bow and stern on the left and right sides of USV are calculated separately. The long axis of the elliptic bow bounding box is defined as L b ¼ L þ e U þ maxfv Dt þ v C * Dt sin q C , 0g, where v C is the velocity of the current, and q C is the angle between the positive X-axis and the current direction, Àp q C p. The short axis of the bow bounding box on the right side of USV is set to be W b ¼ W þ maxfe U þ v C * Dt cos q C , 0g and that on the left side of USV is set to be W b ¼ W þ maxfe U À v C *ÁDt cos q C , 0g. The long axis of the stern bounding box is defined to be L s ¼ maxfÀv C *ÁDt sin q C , 0g þ e U .
Unmanned surface vehicles cooperative path planning. Suppose that USVs communicate in low power with each other and a member-only makes one-direction communication with a couple of members. Meanwhile, the computational ability of the ship-borne computer is limited. Thus, a distributed consensus-based cooperative strategy is applied to compute paths for USVs efficiently. The proposed heuristically sampling-based RRT* (HSRRT*) method is performed on the computer of the leader to an online plan for each state of the virtual leader, denoted by x r ¼ (x c , y c , q c ). (x c , y c , q c ) denotes the location and the velocity direction of the virtual leader. x r is taken as the cooperation variable that is the minimum information that is communicated between USVs.
The communication between USVs can be modeled by a directed graph G ¼ (V, E, A), where V ¼ f1, 2, . . . , Ng denotes the set of nodes with every node representing a USV. E V Â V denotes the set of communication edges of the USV formation systems. A ¼ [a ij ] 2 R NÂN denotes a non-negative weighted adjacency matrix. Self-loops are excluded, that is, 1, 2, . . . , N) can receive information from USV j (j ¼ 1, 2, . . . , N).
The sketch map of the formation maintenance strategy is shown in Figure 8(a), where C 0 is the inertial frame, C Fi is the formation frame, and To calculate paths for USVs, each member should have a consensus of the cooperative variable, that is, x r ¼ (x c , y c , q c ). The cooperative variable calculator of each USV is as follows The virtual leader is regarded as the (n þ 1)'th member, G nþ1 c denotes the communication topological model of the n þ 1 members. A c nþ1 ¼ ½a c ij is the adjacency matrix of communication, g is a positive constant and it is set to be 0.3. h i ðtÞ ¼ P ½a c ij ðtÞ, where j ¼ 1 . . . n þ 1. Figure 8(b) shows the framework of cooperative paths planning framework, and x i denotes the cooperative variable calculated by USV j and J i (t) means the numbers of neighbors of USV i in terms of cooperative variable consensus, N i (t) means the numbers in terms of cooperative path planning. r d j is calculated after x j is obtained, and u i is the computed cooperative strategy.
Assume that the dynamic system of the USV member is single integral, and r i 0 ¼ u i ; i ¼ 1; 2; :::; n. We apply the following cooperative path calculation algorithm where a i is a positive constant and it is set as:  USVs can converge to the formation because G c n þ 1 and G v n have clusters of directed spanning trees. 20

Algorithm implementation
Algorithm 1 shows the implementation of the proposed HSRRT* method. Lines 5-16 are the OPP process. The path tree extends according to the vector field in the seventh line. Lines 10-12 show the path tree topological structure refining process by path tree rewiring, and the cost in the 11th line is the sum of Cdist values from the root of path tree to a tree node. The CD process in the 13th line is performed based on the bounding box of USV. Algorithms 2 and 3 are the Gaussian sampling-based and multisampling-based path tree extension schemes, respectively. The k r in Algorithm 2 and the n r in Algorithm 3 are equal to 9.

Algorithm analysis
The RRT* scheme can spend O(log n) time in finding the near neighbors and spend log n log d m time in path tree extension in the environment with m obstacles under the path tree construction phase. Thus, the time complexity of RRT* in the processing phase is O(n log n log d m) in terms of basic operations, for example, addition, multiplication, and comparison, and n is the number of extension times. 1 RRT* spent O(n) time in searching for a minimum-cost path during the path query phase. The computational complexity of spawning an obstacle-free sample is much lower than that of the path extension. Thus, it can possibly improve the efficiency of RRT* by spawning multiple samples and selecting one that may contribute to the pathfinding process by the extension of path tree near obstacles during the Gaussian sampling-based extension procedure. Meanwhile, the computational complexity of the multisampling-based path tree extension is similar to that of RRT*. Therefore, the path tree construction process of HSRRT* is possibly more efficient than that of RRT*.
During each path tree rewiring procedure, RRT* searches for the near neighbors (q near ) of a newly spawned sample q s on the path tree within the distance r(card(V)) ¼ minfg RRT * (log(card(V))/card(V)) 1/d , hg, where "card" denotes the number of path tree nodes and h is the step length of the path tree extension. If g RRT * ! 2(1 þ 1/d) 1/d (( free )/z d ) 1/d , then RRT* is proven to be asymptotically optimal. 1 is the volume of the obstacle-free space, and z d is the volume of the unit ball in the d-dimensional space. The radius of the resampling domain of HSRRT* is also set to be r(card(V)).
RRT* is probabilistically complete; thus, the decay rate of the failure probability is exponential under the assumption that the environment has good connectivity property, which can be guaranteed in most practical marine environments. Given that the heuristic sampling processes do not prevent RRT* from considering all potential low-cost paths as the planning time approaches infinity, 1,2 HSRRT* remains probabilistically complete and asymptotically optimal. The resulting paths of HSRRT* are feasible because the path tree expands on the basis of the motion model, and constraints are handled during the OPP process.
The global reference path is an optimal one under the known environmental information and the specific resolution. The OPP process of HSRRT* is guided by the reference path through the vector field within a probability threshold. The vector field, the cost distance function, and the heuristic sampling processes take the current into account. Meanwhile, the cost distance function considers the energy efficiency, path tortuosity, path collision probability, and path length. Thus, the path is possibly an energy-efficient, safe, and easy-to-execute one. As the biasing ratio through sampling reduction increases, the randomness in the exploration of the tree decreases because several samples are now used to refine the path in regions around tree nodes. A trade-off occurs between heuristic sampling rate and space exploration rate.
The bounding box definition considers the velocities of USV and the current, helping to improve the accuracy of the CD process compared to traditional bounding boxes.

Simulation results and analyses
Simulations are performed in a computer with Windows 10 64 bit OS, 16 G RAM, and Intel(R) Core(TM) i7-7820HQ CPU @ 2.90 GHz. The parameters of the USV dynamic   model are shown in Table 1 according to the reference 30.
Planning parameters are set in Table 2, where d c is the prediction time domain, d e is the control domain, PR is the radius of perception domain, Dt is the extending step duration, and g RRT * is used in a sample's near neighbor searching process. The maximum speed of USV is 10 kn (5 m/s). The absolute value of the maximum translational acceleration is 1.5 m/s 2 and the maximum lateral acceleration is 10 m/s 2 . Figure 9(a) and (b) shows the vector fields following linear and Dubins paths, respectively. Both fields take the current into account. The vector of a location in Figure 9(a) first finds the closest path segment, and then, the vector size and direction are computed. However, the linear path is not a practical one since it is not smooth with differential continuity and it does not consider the minimum turning radius of USV. Thus, the Dubins curve is used as the reference path, as shown in Figure 9(b). The vector calculation in Figure 9(b) is little complicated because arcs exist. Thus, the vector field is constructed by following the Dubins path. The current is taken into account in Figure 9(a) and (b). Figure 10 shows the OPP results. The green curves show the global reference path while the red curves are the resulting paths in Figure 10 Figure 10(a) shows the online generated path tree by HSRRT*, and the clustered lines consist of the path tree. In Figure 10(a), the red branches denote the vector following extensions and the blue branches denote the other heuristic extensions. Figure 10(b) shows an OPP result of the HSRRT* method. To certify the advantage of HSRRT*, results of other methods are shown. Figure 10(c) shows the planning result of the vector following multisampling RRT* (V-RRT*) method without the Gaussian sampling scheme. Figure 10(d) shows the result of the Gaussian function-based RRT* (Gaussian RRT*) method with vector field following mechanism but without multisampling mechanism. Figure 10(e) shows the planning result of the heuristic RRT* (HRRT*) method with Gaussian sampling and multisampling mechanism but without vector following mechanism. Figure 10(f) shows the result of the traditional RRT* method without heuristics.
The path tree in Figure 10(a) shows the path tree extension procedure of HSRRT*, and the vector following extensions guide the path returning to the reference path and handling the influence of the current. The randomness in path tree extensions is kept to find possible high quality paths. The randomness also prevents the planning from blocking by obstacles.
In Figure 10(b), the resulting path of HSRRT* deviates not far from the optimal reference path, demonstrating the low cost of the path, that is, mainly because of the vector field following scheme and the multisampling heuristic path tree extension. The path sections in the narrow passage on the right of figure have little turns, and the HSRRT* can find OA sections near obstacles, demonstrating the high collision-avoidance ability of HSRRT* probably because of the Gaussian sampling scheme. The path has many fieldfollowing sections, little counter-field sections and few transverse-field sections, demonstrating the effectiveness of the field following extensions. The path is possibly energy-efficient because of the consideration of the current in the vector field. Figure 10(c) shows the result of planning without the Gaussian function-based heuristics. The path is zigzagged and deviates a little far from the reference path. Thus, the path is possibly more costly than that in Figure 10(b). However, the path follows the vector field well. The planner is less intended to search paths near obstacles than those related to Figure 10(b) and (d), resulting in less smooth sections with bigger turns in passages. The observation probably certifies the applicability of the Gaussian function-based sampling scheme. Figure 10(d) shows the result of planning without the multisampling extension. The path is more zigzagged with more and bigger turns than those in Figure 10(b) and (c). The path has a bigger deviation from the reference path than that in Figure 10(b). The observation possibly testifies the effectiveness of the multisampling heuristics. Meanwhile, the path is smooth with less turns in the obstacledense passages on the right of the figure, comparing to that in Figure 10(c). That is possible because the planner can avoid obstacles by searching for paths near obstacle boundaries according to the Gaussian sampling-based extension scheme. Figure 10(e) shows the path planned without the vector field following mechanism. The path deviates further from the reference path and is more different from the reference path. The path has more transverse vector field sections especially on the top right of the path. Meanwhile, the path is more zigzagged (with more and bigger turns) than those in Figure 10(b) to (d), especially on the top left of the figure, where big obstacle-free areas exist. Thus, the path in Figure 10(e) is probably more costly. The observations probably testify the applicability of the vector fieldfollowing heuristics. Figure 10(f) shows the planning result of RRT* without heuristics. The path is zigzagged (especially in passages) and deviates far from the reference path. The observation possibly certifies the lower path refinement efficiency and OA ability of RRT*. The observation also possibly demonstrates that RRT* converges slowly to the optimal path and the online planning time is limited.
The statistically numerical simulation results are provided in Table 3 related to Figure 10 to compare the methods. The data were collected after 1000 simulations were performed for each method. The average path searching time is denoted by "time." The "time" is collected when the planner finds a path connecting the goal. The average path "cost" is computed by formula (6). The stabilities of algorithms are denoted by the standard deviations of time (time EVP) and cost (cost EVP).
The possible positions of USV at waypoint q i are assumed to obey the n-variates normal distribution, thus, the lower bound of the collision-free probability of a path (p) with n waypoints is computed subject to p c ðq i Þ and p c obeys the standard Gamma distribution, p c (q i ) % G (n/2, c q 2 /2), where c q is the standard deviation of the USV positions that USV can collisionfreely deviate from p and c q ¼ minfDist i g where Dist i is the distance between USV and an obstacle. FR denotes the planning failure rate of local path searching processes.
The statistical values are used to demonstrate the performance of the HSRRT* method by comparing HSRRT* with traditional schemes.
The time value of HSRRT* is similar to those of Gaussian-RRT* and V-RRT* and much lower than those of HRRT* and RRT*, demonstrating the high efficiency of the vector field following the procedure of HSRRT*. The cost and cost estimated deviation of paths (EVP) of HSRRT* are lower than other methods, demonstrating the path of HSRRT* is more energy efficient and easy-toexecute with lower collision risk. That can also testify the high efficiency of HSRRT* because the online planning time is limited, thus, if the asymptotically optimal RRT* based method has higher efficiency, it possibly generates lower cost result. The Cf(p) and FR values of HSRRT* are the best. The above observations testify the effectiveness of the vector field and the environmental heuristics embedded into HSRRT*.
The cost and cost EVP of HSRRT* are better than those of Gaussian RRT*, demonstrating the applicability of the multisampling process. The time, time EVP, Cf(p), and FR values of HSRRT* are lower than those of V-RRT*, certifying the effectiveness of the Gaussian sampling.
The time, time EVP, Cf(p), and FR values of HSRRT* and Gaussian-based RRT* are better than that of V-RRT*, certifying the effectiveness of the Gaussian sampling in improving the obstacle avoiding ability of RRT*. Meanwhile, the better cost of V-RRT* than that of Gaussian-RRT* certifying that the multisampling process can improve the path refinement ability of RRT*. The cost EVP of Gaussian-RRT* is better than that of V-RRT*, possibly because that low-cost paths have smooth sections in passages in the experimental environment and the Gaussian-RRT* has stronger OA ability comparing to V-RRT*.
The performance indexes of HSRRT*, Gaussian-RRT*, V-RRT*, and HRRT* are much better than those of RRT*, demonstrating the applicability of the vector field and the proposed heuristics.
The cost of HRRT* is similar to that of RRT*, probably demonstrating the shape and length of the paths of HRRT* and RRT* are similar to each other. Thus, the Cf(p) values of HRRT* and RRT* are comparable. The bounding boxes used by HRRT* consider the current while those used by RRT* do not consider the current. The Cf(p) value of HRRT* is much better than that of RRT*, certifying the applicability of the proposed bounding box for CD. Figure 11 shows the cooperative paths for USVs in a formation based on the information consistency and the virtual structure method, and the diamond formation helps to keep the communication links between USVs. In Figure 11(a), the green curve denotes the planned path for RRT*: rapidly exploring random tree; HRRT*: heuristic rapidly exploring random tree; HSRRT*: heuristically sampling-based rapidly exploring random tree; V-RRT*: vector rapidly exploring random tree; FR: failure rate.  the virtual leader, and then, the members compute their cooperative paths to maintain the formation. The simulation in Figure 11(a) does not consider the OA problem. Figure 11(b) shows that USVs change the formation when passing passages because USVs cannot pass through passages in the diamond formation. The simulations testify that the proposed OPP method and cooperation scheme are applicable. Figure 12 shows the planning result on a hardware-inthe-loop platform. Figure 12(a) shows the current in a harbor. The current model can be regarded as time invariant in a short time and a small range. It is simulated as follows and it is generally clockwise in the northern hemisphere c ðx; y; tÞ ¼ 1 À tan y À B c ðtÞ Á cosð0:84ð0:05x À 0:12tÞÞ ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi ffi 1 þ 0:7056B c 2 ðtÞ Á sin 2 ð0:84ðx À 0:12tÞÞ q where the parameters are set empirically, B c (t) ¼ 1.2 þ 0.3Ácos(0.4Át þp/2). The velocity field of the current is also a vector field. Figure 12(b) shows the OPP result, and the planning start point is shown by a red point. The green curve and the red curve are the reference path and the resulting path in Figure 12(b), respectively. The reference path is planned offline with the consideration of the current, thus, the reference path can generally follow the current. The current is also considered in the OPP process. The OPP result in Figure 12(b) has many downstream sections, thus, it is possibly energy efficient. That certifies the applicability and effectiveness of the proposed method with the consideration of the current. Figure 12(c) shows the threedimensional simulation of cooperative path planning for USVs, and the right figure shows the cooperative paths when USVs change formation. Figure 12

Conclusion
This article proposes a heuristically sampling-based random method to plan for energy-efficient, easy-to-execute, and low collision probability paths for USVs in the practical marine environment. The environment is modeled according to the vector field that follows the reference path and the current. Simulations certify that the vector field heuristics is applicable in improving the RRT*-based path planner. A Gaussian sampling-based path tree extension method is adopted, and it is proven to be effective in promoting the OA ability of the planner by extending to the samples near obstacles. A multisampling-based extension scheme is proposed and certified to be able to reduce the path cost. An elliptic bounding box is designed by considering the velocities of USV and the current, and results testify that the bounding box is applicable to decrease the collision probability of a path. Finally, an information consensus and virtual structure-based scheme is used to quickly calculate cooperative paths for USVs in a formation.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported in part by the National Natural Science

Supplemental material
Supplemental material for this article is available online.