Improved RRT algorithms to solve path planning of multi-glider in time-varying ocean currents

In this paper, through the application research of underwater gliders, it is found that ocean currents are the major influencing factor in the practical application of gliders. The objective of this study is to solve the path planning of glider formation in time-varying ocean currents. Using the existing glider model, energy consumption model and time-varying ocean current model are established based on the existing data, and a model close to the practical application of glider formation is established as well. The existing RRT algorithms are improved to be OCi-RRT (Ocean current improved RRT) algorithms based on environmental ocean currents. the algorithms are used to solve the path planning problems encountered in the practical application of gliders. Through simulations that are close to the restrictions of reality and the ideal communication state, it indicates that the improved RRT algorithms are suitable for path planning of glider formation in real ocean current environments. Then, a large number of simulation experiments are conducted, the results show that OCi-RRT* can reduce the number of cycles and path length by up to 14%, and the unit energy utilization can increase by up to 25% comparing with the RRT algorithm.


I. INTRODUCTION
U NDERWATER Gliders (UGs) have developed rapidly since its birth. Because of long-range endurance, significant autonomy and low manufacturing cost, UGs have proven very promising in ocean research, and gliders are meant to be one type of important observational tools [1]. Through autonomous control, UGs can propel or glide autonomously for a long time without external force, and can be recycled and reused [2]. At present, UGs have been widely used in many marine applications, including marine environmental monitoring, scientific investigation, survey, resource exploration, and missions in dangerous areas where humans are difficult to reach [3]. UGs are becoming more and more popular for their longer endurance and wider search area. However, with the complexity of underwater exploration tasks, a single Underwater Unmanned Vehicle (UUV) or glider can no longer cope with diverse mission requirements. Therefore, a class of underwater vehicles such as UGs should be used in clustered application.
Due to high efficiency and low cost, UGs or other different type gliders formation researches have attracted more attention, and have been widely used in marine observation [4], search and rescue and other fields [5]. UGs are significant components of ocean observation network applications [6] and integrated ocean observing systems [7]. And collaboration of UGs has great application prospects in large-scale ocean surveys [8].
Researches on formation of underwater unmanned focus on formation collision avoidance and obstacle avoidance [9].Gliders are usually deployed along ocean currents to increase their voyage range. However, the ocean environment of gliders operation is a dynamic system with high spatiotemporal variability [10]. Due to the influences of ocean environment changes on gliders, gliders may deviate from target area and increase a large number of overlapping routes, which will reduce the effectiveness of the target search.
In addition to objective factors such as environmental conditions, UGs lack power. Normally, the motion of gliders are highly dependent on buoyancy engines [11]. These UGs typically travel at relatively slow speeds, in many cases comparable or slow to the local ocean current speeds. In complex marine environments (e.g., coastal or deep-sea) the current is the main impact on glider movements. Hence, the effect of ocean currents to UGs motions can't be neglected and should be modeled.
It is difficult to do all the prescribed movements in currents, and there are varying degrees of yawing every few times or even every glide. Especially for long-range voyages, yaw is inevitable unless timely control. Many scholars had studied the path planning of underwater vehicles in currents. The planning problems have been considered time as an additional condition [12], the uncertainty of the water glider, and the variant of the A* algorithm [13]. There was already research in terms of path following and planning for multirobot systems in practical maritime environment with A* [14]. But the simulation environment is not particularly close to the actual ocean current, and the running time is relatively short.
Ocean models with minimum collision as the target are used to obtain ocean current prediction information [15]. UGs research is assisted by predictions from ocean models to overcome limitations by insufficient knowledge of ocean currents. This study also refers to the path planning of unmanned surface vehicles (USV) [16]. Ocean models can be largely categorized as either physics-based [17] or statistical [18], and improvements in successor studies [19].
Path planning is critical when the glider's maximum velocity is comparable to the ocean current [20]. The recent research of planning in ocean environment, generally available ocean current prediction sources provide a series of data at discrete time points [21]. But such dynamic prediction is difficult to be applied to motion planning. It is well known that problems involving time-varying costs are difficult to solve. The simplest general form is the time-dependent shortest path (TDSP) problem, which is known to be NP-hard [22]. If gliders are to perform dynamic path planning in ocean current, many variables will need to be defined [23]. Dynamic planning sometimes makes it better to defer action for a while (waiting in place or floating downstream). This is non-FIFO (first-in-first-out) attribute, and efficient algorithms for non-FIFO TDSPs have been gradually used in the past years [19].For example, the research of planning in time-dependent flow fields [24], due to the negative impact of dynamic current in the environment to navigation performance, the planning problem requires accurate or regular prediction of the ocean environment. However, some studies assume that ocean current of different depths have the same velocity [24], which obviously does not conform to the complex reality. Such dynamic programming is not very applicable in the case of limited prediction ability or in unfamiliar oceans.
Rapidly-exploring Random Tree (RRT) based on random sampling is another representative algorithm of Sampling Based Planning (SBP), and it is a single query planning based on probability completeness [25]. One-way random tree utilizes regression constraint function to generate new nodes [26]. The variable step size RRT is used to generate the initial path and can obtain the shortest current path during Unmanned Aerial Vehicle (UAV) cruise [27]. For multidirectional random tree expansion, OB-RRT establishes the root node through the narrow area, that the random tree can find the feasible path faster [28]. In addition to the optimization of RRT structure, topological neighborhood filtering is used to process the neighborhood node set in the spatial region to obtain the significantly reduced neighborhood node set [29].
The subsequent improved RRT* algorithm with asymptotic optimality can solve the defect that RRT can't converge to the optimal [30].The RRT*-Smart algorithm with intelligent sampling and path optimization functions can solve the slow convergence of RRT* [31]. The IRRT* constraint algorithm improves the overall convergence rate by sampling and extending nodes in a finite ellipse region [32]. HARRT* the algorithm uses human interference to plan a path from one topological space to another [33].
UGs need to operate in complex ocean environments. On account of the complex factor disturbance such as ocean current, the glider movement path will drift. Although the corresponding methods, strategies or algorithms are adopted to correct and improve, trajectory drift often occurs [34]. Due to this drift, the position points of UGs arriving at the target area are not fixed, not exactly identical with the planned path. In practical work, gliders often need in short time to find feasible path. Although improved algorithms or some complicated algorithms can get the optimal path in simulations, they are not completely applicable to the reality application of gliders. The accuracy, advantage and practicality of algorithms need to be balanced.
UGs path planning has some characteristics that low efficiency, long search time and insufficient information of ocean current environment. RRT algorithms can quickly find the feasible path compared with other simple algorithms, and have advantages such as short modeling time, strong search ability and easy to add non-holonomic constraints. Compared with RRT, the artificial potential field algorithm will fall into the local minimum problem [35], A* algorithm does not applicable to the continuous ocean current environment [13]. After comprehensive consideration, the improved RRT algorithms are selected in this work, which can be used to imitate the path characteristics of glider in the real environment. The RRT algorithms of UGs formation combined with ocean environments will also have disadvantages (e.g., poor path quality, overmuch corners, and randomness expansion of nodes), which are also problems to be considered in subsequent studies.
In reality, due to the uncertainty of ocean current disturbance and the randomness of floating motion, the path planning of UGs has problems and difficulties to a certain extent. In this study, the relevant time-varying ocean model and glider model are established, and the RRT algorithms are improved. The RRT and RRT* are improved to OCi-RRT (Ocean current improved RRT) and OCi-RRT* (Ocean current improved RRT*), that solve the specific problem of UGs formation planning in time-varying ocean. The algorithms can make the UGs move further with the same unit energy. This work combines mature algorithms with practical problems to solve the practical application of UGs formation deployment and tasks in a short time. This paper makes a contribution to solving the problem of UGs formation path planning without accurate sea current information.

A. PROBLEM STATEMENT
There are n gliders distributed in Euclidean space. The detection radius of each glider is h, with glider i as the center, the area within the working radius is the epsilon neighborhood of glider i, and other gliders j in the epsilon neighborhood (j=1, 2,...,n) is the adjacency set N i of i, expressed as For Vehicle Routing Problem (VRP), the problem of UGs can be defined by (X f ree , x init , X tgt ). X ∈ R N is bounded space in VRP, N is spatial dimension and N ≥ 2. Obstacle regions X obs ∈ X and obstacle free regions (free regions) X f ree ∈ X satisfy the condition of X f ree =X\X obs . X f ree contains collection of all state points for the workspace. For path, start point x init ∈ X f ree , target point X tgt ∈ X f ree , the continuity is represented by a function s : [0, 1] → R N satisfying its own constraints. For all τ ∈ [0, 1], S(τ ) ∈ X f ree that satisfies S(τ ) = x init and S(1) = X tgt , (X f ree , x init , X tgt ) can be defined. Represented as the solution of the problem, a continuous collision free path from start point x init to target point X tgt satisfying the constraint conditions of the glider itself is calculated through path planning algorithm in the X f ree .
The optimal path planning problem is shown in the following formula: is the set of all formable solutions of path s in environment X. The optimal path planning is usually defined as the problem of searching for the shortest path solution s * in . c : → R ≥ 0 is the cost function of connecting x init to X tgt through X f ree .

B. RRT ALGORITHM AND IMPROVEMENTS
The path planning of RRT algorithm starts from the root node and explores everywhere. When a node in the tree reaches the target point, it returns to the path from the child node to the root node. The initialization process of RRT includes the root node of the random tree, the node set V and the edge set E. The start point x init and target point X tgt are defined in the work environment. x init is a root node of random tree T . A node is randomly selected from the obstacle-free region X f ree as the expansion direction of the new node at the current moment to generate a new child node X new .
When any child node coincides with X tgt , the entire search is complete, returning the path line from that node to the root node. The expansion process of RRT is shown in Fig.1.

FIGURE 1. Schematic diagram of RRT algorithm expansion process
The initialization parameters and overall expansion strategy of RRT* algorithm is the same as RRT. Parent node recalculation and path rewiring are added in the process of single node expansion. The new optimization nodes are used to update the neighborhood node weights, prune the invalid edges in the random tree, complete the node expansion and optimization process, and improve the probability of RRT* asymptotic convergence. Without considering the time factor, the probability completeness of RRT* ensures that the algorithm converges to the global optimal solution.

III. MODELING AND EXPLANATION
Path planning is required to quickly find the optimal path connected by multiple line segments or path points in the planning space. Paths are commonly expressed in two ways. The one is based on dynamics, it is composed by speed and time series. The second way is based on geometry, namely the time series composed of spatial coordinates. Based on the general planning model, UGs formation path planning should further comprehensively consider formation environment constraints, self-constraints and formation constraints.
Aiming at the path planning of UGs formation system under time-varying ocean currents, the problem is decomposed into UGs system model, operating environment and regulatory strategy. UGs system can divide to single model and connection scheme between gliders. Operating environment is to simulate relatively real time-varying ocean. Environmental change is mainly time-varying, and it simplifies sensitive information such as seabed topography and depth. Single regulatory strategies include the diving, gliding, hovering, floating, obstacle avoidance and collision avoidance. Formation strategies include formation dispersing or converging, formation direction of travel, and system adjustment strategies, etc.

A. PECULIARITY OF SINGLE GLIDER
The UGs used in this paper have the characteristics of selfinspection, water surface communication and autonomous control of traditional underwater vehicles. At the same time, the UGs also have omnidirectional motion ability. Omnidirectional means that the UGs are not restricted by the VOLUME 4, 2016 motion direction. The gliders can adjust and control their own moving and turning in any direction, and can respond to change of direction in a short time. For these omnidirectional UGs, motion ability and control equation are shown in Fig.2 [36]. The UGs can motion in almost all direction, even spiral down in small range. The control equation is following.
For this equation For the glider, m is the mass. x G , y G , and z G are the barycentric coordinates. F x , F y , and F z are the resultant forces. M x , M y , and M z are the resultant moment of force. A 11 , A 22 , A 33 , A 44 , A 55 , and A 66 are added masses. I xx , I yy , and I zz are inertia moment around the axes.
In this study, the glider is in a cruising state during normal operation. In each glide cycle, the glider performs a fixeddepth motion, and has diving depth and speed limits. In still water, cruising state means that glider moves at the same horizontal speed and depth in each cycle. However, in practice, each step needs to be re-planned to eliminate errors and improve accuracy.

B. ENERGY CONSUMPTION OPTIMIZATIONE
One of the objectives in this paper is increasing energy efficiency per unit. The energy consumption of UGs is related to the model, ocean current velocity and direction, voyage distance, etc. The energy optimization of UGs is to find a path with the least energy consumption from many feasible paths.
After determining the shape and motion characteristics of UGs, the steady-state glide velocity is only related to net buoyancy and glide angle. During the steady-state motion, the net buoyancy and glide angle remain basically unchanged. In this research, the normal cruising state (standard cruising depth and constant depth movement) of glider is a constant value for speed in still water. This situation not only conforms to gliders' own motion characteristics, but also speeds up the calculation and simplifies the model. The motion of UGs is often simplified into sawtooth shape trajectory, and the whole voyage can be divided into several cycles. The schematic of a single cycle is indicating in Fig.3. Where θ is angle of pitch, α is the angle of attack, ξ is angle of glide, and ξ = θ − α. The glider transforms to different states A, B, and C by changing buoyancy and attitude.
Normally, energy consumption mainly comes from the control of UGs, namely force to move and change mass, and the load consumption (e.g., onboard processors and sensors) [37]. There are two categories of energy consumption. The first is regarded as energy consumption independent of time, including adjusting buoyancy consumption E f and adjusting attitude consumption E z .The second category is energy consumption related to the gliding time E t , which is mainly the sensors and sonars, etc., and the power of this part is constant.
where c is the number of glide cycles, [·] is round up to integer. In this work, E z include consumption of adjusting attitude control module and module standby. E t mainly include consumption of sensors standby and communication module.
After the sea trial of a single prototype glider, the average energy consumption of UGs is shown in Tab.1. The glider has a length of 1.5m, the limit depth of diving is 1000m. 21Wh. E f , E z , and E t can use the trial data in subsequent calculations.
In this study, under strong ocean current, the load consumption accounts for a small part of the total energy of the glider, and even can be relatively ignored to a certain extent. In this case, the consumption of glider is mainly due to change in position and regulating force.

C. OCEAN MODEL
UGs usually work in complex, dynamic ocean environments. And the velocity of UGs often between 0.2 m/s and 0.4 m/s. The influence of ocean current on UGs is very huge, especially in a large impact on movement parameters and trajectories. For gliders planning , currents will inevitably affect the gliders' trajectories at work [1]. Under deterministic conditions, path planning that ignores ocean currents often performs poorly in practical applications due to errors in navigation and operating modes [15]. In the real environments, ocean currents should not be ignored when time or energy consumption are used as criteria [38].
In the long-range voyage path planning of UGs, the effective utilization of ocean current is the key factor affecting the energy consumption. When the external force of ocean current is greater than glider's own power, the glider can't effectively countercurrent, bias or turn. It is necessary to investigate the motion and path planning in ocean current environments.
Existing works in underwater path planning have mostly studied two-dimensional (2D) or quasi-two-dimensional scenarios. However, ocean currents tend to be varied at different depths. In order to reach the destination within specified time, underwater vehicles need to utilize favorable currents and also need to avoid unfavorable currents by diving or rising to appropriate depth [39]. Therefore, underwater planning in the real three-dimensional (3D) ocean environment is of great significance. Oceans are complex dynamical systems with multiple time-scales from seconds to years, and lengthscales from meters to hundreds of kilometers [10]. Therefore, robust and accurate numerical schemes and data assimilation models are needed [40]. In order to plan feasible, safe, and optimal trajectories for UGs, it is necessary for path planning in real ocean.
Ocean current information normally can be obtained by means of satellite observation and high-frequency radar measurement. The ocean current data used in this paper come from HYCOM (Hybrid Coordinate Ocean Model) website. The data provided by HYCOM can be applied to ocean research [41]. The sensitive and confidential information are eliminated and become public and desensitization data. Ocean current estimates are typically provided in piecewiseconstant form, referencing to the time-varying current path planning study of USVs [42] , similar to upper atmosphere estimates [43].
The ocean current is expressed as follows: where V c is the velocity distribution of ocean currents, v is the current velocity. x, y and z are the position of ocean currents in 3D ocean space. t is time factor. i, j and k are the unit velocity perpendicular to the coordinate axis, respectively. Time-varying currents at the same point in ocean as follows: where F is strength of the current field. The data are desensitization, and ocean eliminate depth information characteristics. Examples of simple 3D timevarying currents in latitude and longitude coordinates at different time points are shown in Fig.4(a) and 4(b).
The 3D model is simplified to 2D model with multi-layer superposition. The current models at different depths adopt similar flow fields. Considering the characteristics of ocean currents, there may be some deviations and errors in the motion path of UGs, which should be carefully considered in research and calculation. Assumption 1. It is assumed that the actual ocean current is at least identical in characteristics to the above flow field.

IV. GLIDERS FORMATION DESIGN AND ALGORITHM IMPROVEMENT A. FORMATION SYSTEM DESIGN
The 3D problem can be reduced to 2D if the motion of vehicle is partially known, such as gliders performing deterministic oscillatory vertical motions [39]. The computational cost is reduced significantly. Due to ocean current disturbances, deviations from expected trajectories are prone to occur. When UGs diving, only through the load sensors to estimate the position, over time will produce errors. In this research, the omnidirectional UGs have lightweight and small size. This type glider is susceptible to environments. The research of movements in current space is effective to study gliders in real environment.
For this 3D Euclidean space where W ⊂ R 3 , consider a formation system of n UGs. Let where is the flow velocity vector at the current time point. Assumption 2. It is assumed that the position of the glider is the same as the real position during operation.
Generally, UGs are inevitably limited in communication, and some researchers have proposed even communication restrictions on larger formations have been studied [44]. This research mainly discusses the path planning of UGs formation, and makes some idealized assumptions in communication.
Assumption 3. Each UGs obtains its own velocity and location vector through on-board sensor systems and satellite or other terminal communications. All UGs communications with each other on surface of ocean by satellite or other terminals are considered ideal at all times with no loss of information.
To facilitate research, UGs are set to complete in a simplified 3D space. On this basis, UGs formation is formed. Formation can complete the advancement to the target area under time-varying current. Operation to avoid or reduce overlap with adjacent glider workspace, maintain a relatively stable formation. The formation system can move to the desired area, which requires the following conditions.
is the position between the ith and jth glider at time t comparing with the previous moment of time t-1. q ij (t) is the position between ith and jth glider at time t.

B. GLIDERS MOTION REGULATION
In this case, the motion of UGs is mainly determined by own pitch angle θ and heading angle ψ. It should be noted that the motion on the horizontal plane is decoupled from that on the vertical plane. The UGs dynamics can be modeled as a first-order system with motion pitch angle commands θ c and heading angle commands ψ c as follows.
where τ γ and τ ψ are time constants. Combined with Eq.(3) and the dynamic model, the glider is a model with flexible steering performance, which is not the particle model in many other planning researches. At the same time, because the control equation of glider combines the heading and pitch angle, it has the omnidirectional performance mentioned above the section. When the diving depth is fixed and the pitch angle is limited, it can also make free steering on the heading angle. The direction of planning is converted into the heading angle of UGs, and the control of the heading angle is realized by Eq.(3).

C. IMPROVED RRT CLASS ALGORITHM
Formation planning is to control a group of UGs to move along the desired path, while maintaining the desired formation, and adapt to environmental constraints, such as obstacles, limited space, ocean currents and communication constraints. Due to the limitation of the special underwater environment, the existing flight vehicle formation planning algorithms cannot be directly applied to UGs. Therefore, the formation technology of UGs is one of the focuses of group research.
In the face of various constraints of UGs formation (e.g., formation constraints and realistic constraints), the algorithms also need to be adjusted in the path planning process. Limited by equipment computing capacity, data transmission capacity or other hardware. In view of these characteristics, the planning algorithms need to have the characteristics of short modeling time, strong search ability and convenient ad-dition of other constraints. RRT class algorithms are suitable for the planning problems with the above constraints.
UGs path planning is under the realistic constraints of ocean current. Gliders generally adopt the way of along down the current, with the help of ocean to increase the voyage range. Therefore, algorithm and controller have to satisfy the conditions, that is, UGs can along the current direction to move.  Fig.5, glider i and j work in similar currents at close distance. d ij is distance of gliders. β is a limited angle of motion, and UGs' ability to move only allows gliders to adjust within angle β of the ocean current direction. RRT class algorithms require UGs to keep moving in direction along the ocean current as possible, and the randomly generated direction is within the angle β of current direction. One of objective is to reduce the number of maneuver cycles and improve the energy efficiency per unit.

Shown in
However, RRT algorithms have inherent defects. For example, when obstacles are numerous and disorderly, narrow channels have small area and low probability of being encountered, so it is difficult to find the path in narrow channels environment [45]. Different from narrow channel planning, restricted random direction RRT can be used for ocean current planning.
Algorithm optimization and UGs formation path planning have great significance for general and small UGs moving in the ocean currents. Improved RRT pseudocode indicates in Algorithm 1.
In Algorithm 1, through the UGs position P G , time t, and ocean current datasets V c , the ocean current velocity v c is obtained. In the formation, no UGs have reached the target area, algorithm entering the loop. 5-12 create new node, and Random(β) create node in X f ree within the course and heading angle limit β of currents. Nearest(X rand , T ) use Euclidian distance to select the node X near that is closest to X rand in the tree node. If there are obstacle in the line between X near and X rand , or either glider intrudes into the limits range of the other glider, terminate the current loop and start a new loop. NewState(X near , X rand ,x step ), new node X new is obtained by extending the variable step x step Algorithm 1 RRT pseudocode via ocean currents Input: Start point x init , target area center point X tgt , glider position P G , and start time t; Output: The random tree T of connect x init to X tgt viable path; 1: Loading data, x init , X tgt , v G , P G , K , t, and V c ; 8: if CollideObstacle or CollideOthers then X new ← NewState (X near , X rand , x step ) 13: if E T Minimum, dis (X new , X tgt ) < x step then 17: Return T

18:
end if 19: end for in the direction of the connection between X near and X rand , recording X near as the parent of X new . 13-15, the generated new node and new edge are added to the random tree, and new position information P G is generated for the UGs.
16-17, the termination condition of the algorithm is given. If the linear distance between the new node and the target area center dis(X new ,X tgt ) is less than the fixed step size or has reached the target area center, the random tree T is returned.
The algorithm directly reduces the sampling area of nodes by sampling subsets in the ocean current and the target direction, and improves the probability of nodes searching for the optimal path in the environment. Unit step length (i.e., unit moving distance) is roughly as follows: In Fig.6, the next UGs node sampling area can only be randomly selected within the limited β direction angle range.
In conventional problems, the random scalability of RRT makes the algorithm unstable and cannot converge to the global optimal path. So the RRT* method with progressive optimality is proposed [30]. The main difference between RRT* and the basic RRT algorithm is that the RRT* introduces a search for the neighboring nodes of the newly generated node, that is the pruning process. The purpose is to select a low-cost parent node. In addition, there are rewiring processes to further reduce the path cost. It is a method to solve the problem of high-dimensional optimal path planning. Under sufficient running time, the RRT* always converges to the optimal solution. Algorithm differences as the following pseudocode in Algorithm 2.
Algorithm 2 RRT* pseudocode in ocean currents via Algorithm 1 Input: Start point x init , target area center point X tgt , glider position P G , and start time t; Output: The random tree T of asymptotically optimal paths connect x init to X tgt viable path; 1: Same to 1-12 of Algorithm 1 2: X near ← Near (T, X new , r, β); 3: X min ← ChooseMin (X near , X new , X nearest ); 4: V ← T.addnode (X min , X new , V ); 5: T ← Rewire (T, X near , X min , X new ); 6: P G ← AddPosition (P G , x step ); 7: if E T Minimum then 8: Return T

9: end if
In Algorithm 2, compared with the optimized pruning of conventional RRT*, the pruning direction (UGs heading direction) of improved RRT* based on ocean current needs to be limited within angle β range. The depth and attitude of gliders determine their horizontal voyage range. Therefore, when the maximum diving depth is limited, the horizontal motion distance in a single cycle has a maximum value, that is, the step length of each step in the algorithm is the maximum limit. In ocean environment, RRT* need to be pruning with caution once the maximum step length of UGs at the position and time point is exceeded.
RRT* has asymptotic optimality, but due to the calculation amount and other problems, the calculation speed is too slow and the calculation data are too large. However, restrictions on the direction of ocean currents reduce these inherent defects. The applicability of the RRT under the external force of ocean currents have been improved. The improved RRT * also has its own characteristics in ocean current. When the current velocity is large, the heading angle is limited, so the path re-planning ability of RRT* is limited. As indicated in Fig.7.

FIGURE 7. Schematic diagram of RRT* restricted expansion process in ocean currents
In Fig.7. For the improved RRT* in ocean current, within the x near range of x new , there are the now available closest point x nearest and the closer point x min . In normal, the RRT* will be reconstructed, the connection between x nearest and x new will be cancelled, and the line between x new and x min will be reconstructed. But in ocean environment, when the ocean current speed is large, the heading angle ψ from x min to x new exceeds UGs control limits. Therefore, the improved RRT* for ocean current environment will not reconstruct the path from x min to x new and will make new choices when facing this situation. If there is no more optimized path within the range of x near , the path from x nearest to x new will be re-enabled.
RRT class algorithms can achieve the target, but they cannot guarantee that all the solutions are optimal, especially in the complex ocean environment. The algorithms will have great significance for kinds of miniaturization and poor power UGs which moving along with ocean currents.

V. PATH PLANNING SIMULATION AND ANALYSIS
The communication connection mode is full connection, but data interaction is only enabled when the depth of UGs is less than 2m, that is, data interaction is carried out on the water surface. The specifications of the computer used for simulation are Ryzen 2600X 3.6GHz processor, Nvidia 1660Ti GPU, and 16G memory. The algorithm is implemented in Python. In this study, there must be a consensus that in real ocean, the position of detectable and regular ocean currents usually has no natural large obstacles, such as large islands and reefs. Large and impenetrable obstacles often exist in areas where there is no current.
It is assumed that UGs can move to the target area at a limited maximum speed in a still water environment. The UGs periodic glide in the longitudinal section with the vertical depth maintained at 1km, the limit maximum horizontal range of the glider in one cycle is about 6.75km.
The improved algorithms are applied to the known 3D ocean current to verify the effectiveness of algorithms. The 3D ocean current models are simplified into several layered superposition 2D models, and similar ocean currents are used for different depth models. Fig.8 shows a path planning task with ocean currents and no obstacles environment.   Fig.8, in the current state, the formation uses the improved RRT to perform tasks. The formation consists of four gliders, from G1 to G4. The distance between gliders is 5km, which cannot be accurately displayed on a large scale, so they are represented by black squares. Fig.8(a) indicates the formation shape in different space positions at t=10, t=40 and FIGURE 9. Relative horizontal distance between gliders in the task formation t=75 periods. Fig.8(b) shows the path of formation as seen in a 2D plane view. Due to the distance limitation between gliders, even if the formation is executed strictly according to Eq.(8), there will be no collision in the formation. Fig.9 displays the inter distances of Fig.8. There are no internal collisions between gliders during the 80 operating cycles. Due to Eq.(8), gliders can also maintain a relatively close formation.

A. SIMULATION VIA IMPROVED RRT
Under the condition of still water, multiple obstacles are manually set, the improved RRT plans the UGs formation path, the minimum distance of 5km between gliders, as shown in Fig.10.
As shown in Fig.10. The X and Y axes are measured in kilometers. x ∈ (17.5, 22.5) and y ∈ (17.5, 22.5) are the start area of four gliders formation. The center point of the target area is (250,260), the green line is the exploration, the red line is the calculated path, and the gray area is the obstacle areas. Fig.10, the improved RRT adapts to the motion conditions of glider and can avoid obstacle in still water. It reflects the good maneuverability of UGs and formation.
Obstacles are selected circular and rectangular obstacles in 2D, can be understood as expanding the avoidance range of the practical obstacles, that increase the safety in the process of obstacle avoidance. Whether in 2D or 3D environments, the regular shapes can be integral to get irregular shape obstacles such as coastline and islands. It should be noted that in real environments, the water depth within 5 n miles of the coastline (the irregular obstacle area) is generally no more than 50m, which is less than the limit depth required by the operation of general gliders.
For gliders, the regular shape after expanding the obstacle avoidance range can be projected to represent 3D obstacles. Furthermore, due to restrictions on the motion ability and flexibility of gliders, it is not necessary for gliders to operate  strictly in accordance with the extended continental shelf of the coastline. Only needs to avoid the area with obstacles as the center to represent the obstacle avoidance ability of gliders in the 3D environment.
In the condition of ocean currents, no-obstacles, improved RRT path planning is displayed in Fig.11.
In Fig.11. For discrete current data, the improved RRT can make the formation reach the target area with the external force of the ocean. In Fig.11 and the following figures, in pictures with ocean currents, the arrow indicates the direction of currents and the length of the arrow line indicates the speed of currents. The different color areas in pictures also represent different speeds. In Fig.11(b), the speed direction refers to the arrow direction in Fig.11(a). Considering the influence of ocean current on formation, the improved RRT mainly uses high-speed currents to advance, and adjusts and guides under appropriate ocean current velocity. UGs use the ocean currents to follow the direction of the ocean, rather than confront the direction of currents.
In Fig.12, the formation performs planning with ocean currents and multi-obstacle environments. The improved RRT can make the formation reach the target area and can also perform obstacle avoidance operations. According to strategies, all gliders of the formation pass through or bypass the red high-speed area. And in the process of reaching the target area, the formation needs to avoid the artificial obstacle area along the way.
The improved RRT can find the target area to complete the task under three different environmental conditions (noobstacle still water, no-obstacle ocean current, and multiobstacle ocean current environment). For the UGs formation in Fig.10 to Fig.12, the results of multiple simulations are shown in Tab.2. In Tab.2, State 1 is an environment with still water and multi-obstacle (Fig.10), State 2 with ocean current and noobstacle (Fig.11), and State 3 with ocean current and multiobstacle (Fig.12). The cycle time is the average glide period of at least one glider in the formation reaching the target area and then stopping the algorithm. The horizontal distance is the average horizontal distance of all gliders when the formation reaches the target area. Energy consumption is the average energy consumption of the glider when the formation arrives at the target area. Computation time is the CPU calculation time. Fig.10 to Fig.12, and Tab.2, the formation can reach the target area under all three environmental conditions. In ocean current environment, State 2 compared to State 1, at the same start point and target point, the average horizontal distance is 8.73km longer, the energy consumption is 132.3Wh less, the average cycle times is 3 less, the computation time is 3.3s longer.
In State 3 and Fig.12, ocean currents help UGs formation reaching target area and avoiding obstacles. In the same start area and target area, State 3 formation relies on ocean currents to avoid obstacles, which does not consume more energy than State 2. In the case of almost the same horizontal moving distance, due to the joint action of obstacle avoidance motion, State 3 is even better than State 2 with ocean current and no-obstacle. In addition to the computation time, the other three indicators have certain advantages.
By following the ocean current, UGs in State 2 and State 3 can reach the target area in the same or even shorter time. At the same time, the formation sampling detection distance  is longer, the time is shorter, and the energy consumption is lower. Fig.13 display the path planning of formation under two different ocean current states (Current 2 and Current 3) which are different from the Current 1 in Fig.12. Fig.12 and Fig.13 show that the UGs formation can complete the scheduled task in different environments (e.g., Fig.12 with strong currents, weak currents in Fig.13(a)(b), and variable direction currents in Fig.13(c)(d)). When the high-speed ocean current (red area) is encountered, the lateral maneuver of the ocean current with huge energy consumption is not carried out, but is fine-tuned along the direction of ocean current with less energy consumption. When the ocean conditions permit, the heading angle increases and the formation moves towards the target area, eventually reaching the target. When the current has a turning trend, the formation can adjust its motion strategy in time, enter the low-speed ocean current area, and continue to move towards the target.
The improved RRT can be applicable to different ocean currents. In the application of UGs formation and the path planning of gliders, the ocean currents is not regarded as disturbance items, the task can also be completed. Ocean currents can help planning problems in some cases. Especially for this improved RRT, due to the environmental factors of ocean current planning problem, it can quickly find the path.

B. SIMULATION VIA IMPROVED RRT*
For the improved RRT* has the same capabilities as the improved RRT. The simulation environments (e.g., the obstacles and the start area and the target area) are the same. In the condition of ocean current, multi-obstacle, and 10000 steps of simulation. The improved RRT* is used to solve this planning problem. In order to achieve the optimal path approaching, enough calculation times are carried out. The exploration paths are dense and random enough, and all the exploration and selection paths conform to the motion law of UGs formation. Improved RRT* for the formation in different environments (i.e., Current 1, 2, and 3) can find the target area to complete the task. For the UGs formation planning in three different ocean environments, the simulation optimization using the improved RRT* is shown in Fig.14.
As shown in Fig.14, like the improved RRT, the formation can use ocean currents to maneuver, adjust and navigate. While reducing energy consumption, the ocean currents help the formation to reach the target area.
Multiple simulation results of the improved RRT* are compared with the results of improved RRT. The data are indicated in Tab.3.
The overall path of Fig.14 n the three current environments is smoother compared with Fig.12 and Fig.13. In order to obtain these smooth paths, the improved RRT* will also use more calculation resources to generate more test paths. Shown in Tab.3, more computation time is required.
It can be seen from Fig.12, Fig.14(a)(b), and Tab.3. When the ocean current velocity changes greatly, the formation does not maneuver across the current with large energy consumption, but fine-tunes along the current direction. Compared with improved RRT, the optimization degree of improved RRT* in horizontal distance and energy cost is close to 6%. Cycle times can also save nearly 3 cycles. The path is also smoother.
In Fig.13(a)(b), Fig.14(c)(d), and Tab.3. In addition to calculation time, the improved RRT* makes the three indicators of the formation better when the overall trend of the current environment is relatively mild and the velocity  Fig.13(c)(d), Fig.14(e)(f), and Tab.3. UGs formation sometimes needs to follow the direction of strong currents and fine-tune when self-regulation is needed to avoid and reduce confrontation with ocean currents. The improved RRT* can optimize the RRT limitedly when the ocean current is not strong, but the direction change is very large. In the case of complex ocean current direction changes, there is little difference between the two algorithms, and the improved RRT* has a slight advantage.
For UGs formation, the improved RRT* has an optimization effect on the planning in ocean current. The energy consumption is lower under the same conditions compared with the improved RRT. However, due to the characteristics of Fig.7, when calculating the problem, choosing the improved RRT* will generate more exploration paths and occupy more computing resources. Need to choose the algorithm according to the actual situation.
In order to verify the advantages and disadvantages of OCi-RRT and OCi-RRT*, under the same conditions, number of cycles, average path length and computation time of the two methods are compared with the traditional RRT comparison method. According to different ocean currents, 100 experiments were carried out. The experimental results are indicated in Fig.15 and Tab.4. It should be noted that once the path length and cycle number of OCi-RRT* are better than OCi-RRT, the energy evaluation is necessarily better, therefore, it is unnecessary to compare the energy consumption again.
In Fig.15 and Tab.4, compared with OCi-RRT, OCi-RRT* reduces the average path length and the number of cycles by 6.579% and 10.465%, respectively. It needs attention that due to the randomness of RRT, the average value of these two parameters cannot fully represent the operation effect of the two algorithms in the operating environment. This simply  means that OCi-RRT* is better in terms of trends in data and results.

C. COMPARED TO THE ORIGINAL ALGORITHM
For purpose of validate the advantages of OCi-RRT and OCi-RRT*, the two algorithms are compared with RRT and RRT* in terms of path cycle number, path length, unit energy path and computation time. Unit energy path refers to the distance the glider can travel in the horizontal direction under each unit energy. The unit energy path represents the energy utilization efficiency of glider, and the larger the value is, the better. The unit of this article is km/Wh. Because the original RRT and RRT* algorithms are not suitable for ocean environment, the experimental environment is set to still water with obstacles. Both algorithms have been redesigned to fit the UGs motion constraints. The experiment was conducted 100 times. The search step L step of RRT and RRT* algorithms is set as the limit horizontal distance of UGs motion 6.75 km. The results display in Fig.16 and Tab.5. In Tab.5, OCi and OCi* are abbreviations for OCi-RRT and OCi-RRT*, respectively. As shown in Fig.16 and Tab.5, OCi-RRT* performed well in the other three indicators except the computation time in the randomly selected incompletely identical experimental environment. The unit energy path index of RRT and RRT* is the same, which conforms with the characteristics of the gliders without optimization.
OCi-RRT* compared with RRT, RRT*, and OCi-RRT. Except that the computation time is not the least, the average path lengths of OCi-RRT* algorithm are reduced by 13.772%, 9.330%, and 3.441%.The average number of cycles decreased by 13.187%, 7.059%, and 3.659%, respectively. The unit energy path increased by 25%, 25%, and 7.143%, respectively. The effectiveness and superiority of OCi-RRT* are proved. By comparison, OCi-RRT also has advantages over the two original algorithms.

D. PRELIMINARY STUDY IN DYNAMIC ENVIRONMENT
The state-of-the-art path planning methods propose solutions for both static and dynamic environments. The improved algorithms of this article also need to face the planning problem in dynamic environment. The dynamic environment faced by gliders refers to the time-varying ocean, which is expressed by Eq.(5) and Eq. (6). For UGs, the power cannot guarantee that gliders are completely static on the water surface or suspended in the seawater. In this case, even if ocean currents are predicted, gliders are difficult to wait for the optimal currents for global planning as other dynamic planning [19]. And in practical applications, the unknown ocean environment is difficult to predict completely.
In the face of this situation, UGs need to have strategies (based on Algorithm 1 and 2) to cope with environmental changes, of which the most negative is drifting with the ocean current.
Dynamic environment is time-varying ocean current. The UGs formation uses OCi-RRT*, and the basic task of the formation is to reach the target area. By adjusting to more favorable currents, the formation can quickly reach the target area using currents. In order to compare in the time-varying ocean environment, the initial ocean currents of RRT* and OCi-RRT* are the same, and gliders can know the global current information at each moment. Gliders operate continuously from the beginning. It ensures that the two algorithms have the same ocean environment at the same time. The images shown in Fig.17, and the data displayed in Tab.6.  .17 and Tab.6. The blue path is the path generated by RRT* using ocean current at each moment as the static environment. Each step needs to recalculate the path to the target area, regardless of the global, is the shortest path from the current point to target. The blue dotted line is the path from the current location to target, and is used for comparison. The red path is the path generated by OCi-RRT*. Each step is additionally considered to improve unit energy path.
UGs cannot wait in place, and each decision needs to be made without violating motion restrictions and current direction restrictions, and better energy strategies should be considered. OCi-RRT* has the requirements of energy and steering angle. Adjusting to more favorable ocean current direction can make the formation cope with the dynamic current environment.

VI. CONCLUSION
In view of solving the path planning problem of gliders formation under large-scale ocean current in practical applications, RRT algorithms are selected in this study, and make the following improvements: Firstly, the RRT algorithms are improved according to the characteristics of gliders, so that the algorithms can optimize the path and reduce the number of path cycles while meeting the motion characteristics of gliders. Secondly, based on the energy consumption characteristics of individual glider, combined with the existing sea trial data, the formation planning decision conforming to the energy strategy optimization is obtained. A large number of simulation experiments show that the proposed algorithm has advantages over the traditional RRT in number of nodes, path length and energy utilization. OCi-RRT* algorithm has broad application prospects in the field of gliders formation and large-scale flow field planning.

A. MAJOR CONTRIBUTIONS
The contribution of this research is to explore the ability of UGs formation to perform path planning tasks in ocean currents. It provides support for the use and rapid deployment of UGs formation in practical applications. Low-speed gliders, taking UGs as an example, can be rapidly and effectively deployed and path planning in relatively unfamiliar ocean environments. Reduce too radical control strategies, such as forced crossing ocean currents, to expend the unit energy operating time. From another aspect, it also makes gliders have enough energy when facing the necessary maneuvers.
To some extent, this study proves that sampling algorithms such as RRT can also be applied to planning problems in large-scale flow field environment.

B. FUTURE WORK
In future research, in addition to improving the algorithm, the united planning of multiple UGs formations on a large-(a) Initial t=0 to t=25h, t=25 ocean graph (b) t=25h to t=50h, t=50 ocean graph (c) t=50h to t=75h, t=75 ocean graph, arrival target scale will be studied. For example, in the overall ocean current, the environmental information of the downstream (front) formation in currents can be used as the reference and prediction basis for the future environment of the upstream (rear) formation. While enhancing the accuracy of system environment prediction, the path planning of the upstream formation in the dynamic environment can be carried out. Make full use of environmental data to improve the accuracy and efficiency of planning. And artificial neural networks will also be considered to solve such problems, simplifying the input required for the entire problem and rapidly obtaining the optimal path. .

APPENDIX
Based on previous research [36] and sea trial of the prototype glider. In the forced surging test, the additional mass A 11 can be determined. Because of the glider omnidirectional athletic ability, the glider is designed that A 11 is equal to A 22 . A 33 can be determined in the heaving test. Furthermore, A 55 and A 66 can be calculated through the pitching test and the yawing test. And the value of A 44 is obtained due to the shape of the glider. The dimensionless hydrodynamic coefficient, additional mass and inertia moment of the glider are listed in Tab.7.