Liveness-Based RRT Algorithm for Autonomous Underwater Vehicles Motion Planning

the


Introduction
In the last decade, motion planning for single robot or multiple robot system has gained a lot of research interest as one of the primary problems in robot field.In most application cases, it mainly involves the structured mobility to drive the robots to the final destination given any initial states [1].In general, the motion planning algorithms can be generally categorized into two types [2].The first ones return the positions along the path from start to the goal, which are called path planning algorithms.In addition, the second ones, that is, trajectory planning algorithms (also called motion planning algorithms), return a sequence set of input space that drives the robot towards the destination.
In recent years, various algorithms have been proposed for both path and trajectory planning problems.Artificial potential fields (APF) can be viewed as a powerful method for path planning, which was firstly proposed in [3].Although APF suffers from the presence of local minimums, it is adaptable in such cases with online intractable changes of environment.Meantime, various of methods are proposed to avoid sticking into local minimums, such as potential functions without local minimums [4] and random search methods [5][6][7].
Another remarkable probabilistic method was proposed as Probabilistic Roadmap (PRM) [8][9][10][11].The key idea of PRM is to generate a connective random graph that comprises valid nodes selected randomly in the free configuration space (C-space).Meanwhile, RRT was presented as a single query incremental algorithm [12].PRM and RRT are shown with more effectiveness, especially facing high-dimensional problems.Besides, they are insensitive to the complexity of the problems.RRT has provably probabilistic completeness, which means that the probability that RRT cannot find a valid path from the initial to the goal position converges to zero, as the number of samples tends to infinity.Also, the concept of probabilistic completeness is officially defined, which is a crucial property, although it is weaker than "completeness" in the field of sample based algorithms.Consequently, RRT has been applied in many applications ranging from industrial production to military battle systems.
Several RRT based algorithms are designed in order to deal with different constraints or to provide solutions more effectively.For example, in [13][14][15][16], additional dynamic differential constraints are considered when extending the random tree.A kind of new variation space, called trajectory parameter-space (TP-space), was presented in [17].
Particularly each edge is kinematically feasible once it is generated by two points in TP-space.In [18], the expanding process utilizes implicit flood-fill-like mechanism to avoid the local minimum.Thus, the expected time to find the solution is reduced.Because of the insensitivity to the dimension of problems, it is able to be used to search time invariant parameters [19].It is a fact that AUVs have been widely applied in the marine resources exploration and utilization [20][21][22][23].In order to map a scalar field, mutual information based multidimensional RRT algorithm was presented for multiple AUVs in [15].
However, RRT is proven to converge almost to a nonoptimal path [24], in which RRT * was detailed to be asymptotically optimal.That means RRT * will "almost surely" find the optimal path as the number of samples tends to infinity.RRT * applies the reconstruction of the searching tree, which always contains the optimal subtree at each iteration.Since then several RRT * based optimal planning algorithms were proposed [25][26][27][28] and applied in many fields [29,30].LBT-RRT was proposed in [25], which is a combination algorithm with RRT and RRT * .It defines a variable  to describe the "distance" away from the optimal path.In other words,  can be viewed as the comparison between RRT and RRT * .If  equals zero, it becomes RRT * essentially.In [26], RRT * is refined to be capable of replanning with dynamic environments.In [27], the limitation, that is, requirements for an asymptotical optimal steer function during the tree's expansion, is provably addressed based on sparse data structure.
From [31], we know that the sampling strategies have much effect on the efficiency of sampling-based algorithms.In early researches, typical choice is sampling uniformly in C-space [8,12,24].Every node in C-space is sampled with exactly the same probability, by which the prior information actually is not exploited.For instance, a new node intuitively prefers to be sampled away from the region where a large number of samples already exist.And it certainly reduces the probability that samples are generated in the case of narrow passages [31].For this reason, various heuristic methodologies are designed.Goal bias is generally utilized to enhance extending towards the goal due to its effectiveness.Medial axis is combined with path planning algorithms to search the C-space with much more efficiency [8].On the contrary, Gaussian sampling strategy prefers the region nearby obstacles [10,32].In this view, geometric shapes of obstacles are able to be quickly constructed.Some other hybrid sampling strategies are proposed to combine two or more methods.These methods uniformly consider the information of the environment to refine the sampling probability; however, the information of nodes is not totally utilized.Specifically, regions where a number of nodes are already sampled can be mostly considered with less worth.In other words, it is preferred to sample in "new" regions.
In this paper, we demonstrate the expanding mechanism of the random tree and make an estimation of the expanding ability of each node.We present several new indicators in the expanding procedure, which are utilized in our new algorithm.Also, we detail the indicators quantitative descriptions of the nodes.Meanwhile, we provide theoretical analysis and property of our algorithm with mathematical proof (i.e., probabilistic completeness and outperforming compared to RRT).The proof is based on a technical notation attraction sequence and probability theory.
The reminder of the paper is organized as follows.Section 2 introduces the definition and notations in this paper.The indicators which reflect the growing of the tree and expanding ability are analyzed in Section 3. Then our new algorithm, that is, Li-RRT, is described in detail.In Section 4, the analysis of Li-RRT is provided.Section 5 presents an application example for a planar mobile robot.In final, concluding remarks are drawn in Section 6.

Preliminaries and Problem Formation
2.1.Problem Formulation.Path planning problem can be generally viewed as a search problem in the C-space  ⊆ R  .For instance, the C-space of a planar two-wheel mobile robot can be defined as  ⊆ R 3 , that is, position (, ), heading angle .In this way, any  ∈  ⊆ R 3 can describe the state of the mobile robot.Additionally, we define  obs ⊂  and  free =  \  obs as the obstacle C-space and free Cspace separately.The path planning problems require a finite sequence of states in  free from an initial state to a given goal state.Here we present the basic path planning problem as defined in [24].
Facing path planning problems, each element of dimension is considered independently with no other constraints.However, it is unrealistic in most applications.Specifically, the kinodynamic of AUVs is restricted in case of uncontrollability.It means that there exists corresponding available input sequence () : [0, 1] →  that drives the AUV from  start to  goal .Subsequently the path can also be viewed as a function of input sequence ((⋅)) : (⋅) →  free .Formally, we give the definition of trajectory planning problem for AUVs.

Algorithms
sequentially towards the goal.We refer readers to more details about RRT in [12].
The main branches of RRT are firstly constructed as it rapidly reaches the far corners of the square.As samples are incrementally added in the searching tree, C-spaces are mostly covered by smaller branches gradually.In other words, given any interval  > 0, C-space will be filled with nodes as the number of samples tends to infinity.It means the probability that there exists at least one node of the random tree located in the goal set equals one, as the number of samples tends to infinity, if there exists one available path in

Coverage of a Graph.
A kind of coverage measure of the tree in C-space was proposed in [34].We uniformly overlay grids of   points and spacing  on .Define the minimum distance from each grid point  to the nearest node in the tree as   .Thus, min(  , ) can describe the radius of the largest ball centered at the grid point which contains no nodes of the tree and adjacent grid points as shown in Figure 1.Given a graph G, we define its coverage measure as in [34].
where 1/ removes the impact of different spacing distance.
The coverage of a tree can be viewed as the average of all nodes distances, normalized by the grid space .
The key idea of this measure is to describe the dispersion of all nodes, which is a conception proposed from the Monte Carlo method that indicates the unevenness of sample points.Primarily, the cover performance is significantly better with smaller coverage measure.In other words, expanding more widely and uniformly may lead to smaller coverage measure.In an extreme case, that is, (G) = 0 which is impracticable, if there exists at least one node of the tree in every grid point, the tree can be viewed well distributed in  free with given solution of the grids.Intuitively, we may try to optimize (G) with an acceptable computing complexity, which is addressed in Section 3.4.

Growth Speed of a
Tree.The growth of a tree is naturally defined as the derivative of the coverage measure.We denote the number of nodes in the tree as   and then define the growth of the tree as [34] Because the differential form (G)/  is intractably formed and computed, we utilize the difference form to indicate the growing speed of the tree.Obviously, (G) is able to be used to test the nodes of tree appropriately.Let  denote the threshold, if (G) drops below  with new node V  , it means that V  is almost worthless.

Liveliness of Nodes.
The factors which illustrate expanding ability of nodes in a tree can be roughly categorized into two parts: the external circumstance factors and the internal circumstance factors.

External Circumstance of Nodes.
External environment has significant influence on the expanding ability of each node.If some node locates in a blind alley, it has absolutely no contribution on searching.As shown in Figure 2, it describes the scope of a parent node that could be extended to within   We denote the cardinality of Ũ as   and define collision detective index as Se(  ) =   /.Note that if Se(  ) = 0, the point   is a dead point.If so,   is necessarily removed from the tree.Meanwhile, we could define the collision detective index of a tree as where   denotes the number of nodes in a tree, which is utilized to normalize the feature of the nodes.The collision detective index of a tree is the average value of all component nodes.It is a fact that the prior information about the environment is unavailable in many applications.In such cases, the performance of searching C-space is more important.Se(G), defined in (3), may give a reasonable solution on the process of expansion of searching tree.Generally, we prefer the nodes that make Se(G) as large as possible or larger than a threshold , because it means that the probability of reaching the target will stay high.
Unfortunately, it is commonly difficult to tell whether a node is stuck in a blind alley or just in a narrow passage.Note that it is still necessary to expand   if   is not equal to zero.In other words, only {  |   = 0} could be removed in case of unexpected failure.In this way, the set of state {  |   = 0} can be viewed as the inevitable obstacles.Unlike that, the dead nodes in this paper are naturally cheap to compute.

Internal Structure of Trees.
Internal structure of a tree also influences the expanding ability of nodes.Due to the stochastic sampling strategy, it may lead to crowd in some regions by random expanding.In Figure 3(a), the gray circles with dotted lines are considered as regression nodes, which are avoided by RRT-blossom method proposed in [18].Although it may be necessary to explore the regression states since the complex environment, the other unexplored regions have reasonable high priority.
In Figure 3(a), the hollow white dots are possible newly added nodes of an identical parent.The hollow white dots embraced in the ellipse together with a solid black dot are closer to the neighbor nodes than their parents.Figure 3(b) shows all expansions in the tree which avoid being closer to the existing structure.This strategy ensures that the tree will have a strong tendency to explore the whole C-space with inflated branches.
We define the degree of a node as the number of its successors, which is denoted by (  ).All offspring of a node is defined as where the set Us(  ) is the union of all offspring of the node   .It is obviously that a node with more successors and offspring has less expanding ability.Figure 4 illustrates a tree's distribution of degrees and offspring.The neighbors of a node are viewed as obstacles, which are shown in Figure 5.The neighbor nodes are viewed as circular or spherical obstacles with a fixed radius.Define the number of neighbor collisions as  nei  (  ) = ∑  =1 1{Collision Free(  , Considering all these factors, we design a hybrid indicator to illustrate the quantity of liveliness of each node.Synthesizing two external and internal factors, the liveliness index of each node in a tree is defined in the form of where   is the number of total nodes at current iteration.Indeed, (5) reflects the expanding ability of a specific node.We translate Li(  ) in logs denoted as LnLi. LnLi Since log function has the same monotonicity, LnLi(  ) can describe the liveness of each node.Particularly, if   is a dead point, that is, Se(  ) = 0, we set LnLi(  ) = −inf.
Obviously, the liveness LnLi defined in (6) will be attached to each node once added to the random tree.The value of LnLi is calculated and updated when the expanding process will be called.It can be viewed as a guidance that describes a better expanding direction for the searching tree.

Li-RRT.
In this section, we present the hybrid RRT algorithm, that is, Li-RRT, as shown in Algorithm 2. It utilizes the liveness of each node to guide the expanding process of the random searching tree.More efficient or useful nodes will be popped out to enhance the property of exploration.The main methodology of Li-RRT is based on RRT.Different from the typical RRT, we define a liveness set  = {LnLi(V) | V ∈ } initially (line (1)).
At each iteration, node max Li pops with max liveness from  in line (3).Then an input vector is randomly selected from function RandU (line (5)), according to which a random state in  free is generated.Meantime,  rand is chosen from  goal with probability W (line ( 13)).It is proposed as a goal bias sampling method, which is simple but effective.Similarly, the nearest node is calculated given the cost function (⋅).New node  new then is achieved based on forward processing drive (line ( 20)).CollisionFree is called to verify the validity of  new and ( nearest ,  new ) (line ( 21)).Expanding process is implemented by adding  new and ( nearest ,  new ) (line ( 22)).
Liveness of each node is updated by UpdateLi(V) in Algorithm 3. From the definition of LnLi(  ) (see ( 5)), we only need to care about three nodes sets, that is, { 1 ch ,  2 ch ,  3 ch }.  1  ch contains only  new , since Se(  ) is constant and is determined based on the state and the environments.Here we define that "all parent nodes" of   are the nodes set {V |   can be traced from V} different from the parent node  par (  ). 2  ch includes exactly all parent nodes of  new .Because  new increases the successors of V ∈  2 ch , we set LnLi(V ∈  2 ch ) = LnLi(V) + ((V)/  − ((V) + 1)/(  + 1)).Specifically, we set the liveness of  par ( new ) as LnLi( par ) = LnLi( par ) − 1.  3 ch is the near set of  new : that is, ch will increase because of  new .Thus, we define LnLi(V ∈  3 ch ) = LnLi(V) − 1.
An attractor region   like a funnel as a metaphor for motions converges into a small region in the space.As shown in Figure 6, a basin   can be viewed as a safety zone which ensures that a point of   will be selected by the NEAREST NEIGHBOR and potential well which attracts the point into   .Given an attraction sequence A with length  and letting   = (  )/( free ) and   = min   , we have the following lemma.Lemma 3 (see [36]).Let Lemma 4 (see [35]).
This completes the proof.
In summary, we can see that Li-RRT enhances the expanding efficiency from Proposition 7.

Applications of Li-RRT
In this section, we present a numerical simulation for planar AUV with data from NOAA.A region of Hawaii, that is, a rectangle from 157 ∘ 58  48  W, 21 ∘ 20  24  N to 157 ∘ 57  36  W, 21 ∘ 21  36  N, is utilized to testify the effectiveness of Li-RRT; see Figure 7(a).Without loss of generality, we assume that AUV only voyages at depth of 5 meters at least.And the dynamics of AUV is ẋ = V cos(), ẏ = V sin(), θ = .Meantime, we set 0 ≤ |V| ≤ 2 m/s and 0 ≤ || ≤ 0.15 rad/s.
We can see that a valid path is found by Li-RRT shown as in Figure 7(b).Although solutions returned by Li-RRT are mostly not optimal, it can supply candidate paths effectively in less planning time.A comparison between RRT and Li-RRT is described in Figure 8. Li-RRT obviously costs less searching iterations by utilizing existing information between current random tree and the environment.
Here we take multiple simulations by RRT with same conditions and present two better solutions.We can see that RRT needs more searching iterations than Li-RRT averagely.Sampling strategy in typical RRT utilizes only goal bias method which tries to make the random searching  tree towards the goal region.It certainly may generate useless sampling nodes in every searching step.For example, sampling that avoids an oversampled areas seems more efficient; however, typical RRT has no such ability.Thus, RRT needs more searching time or iterations to reach the goal.

Conclusions
In this paper, we have presented an analysis of rapidly exploring random trees and defined the several indicators of a tree, including the coverage and the growth speed describing the growing behavior of a tree.Considering the external and internal factors which influence the expanding ability, we also have defined indicators collision detective index, degree, offspring, and neighbor collisions.On this basis, we have proposed Li-RRT for AUVs motion planning.By using the tools attraction sequence, theoretical analysis has indicated that liveliness-based RRT enhances the expanding speed.Simulations are also provided to show the effectiveness of Li-RRT.It has been shown that Li-RRT performs well in finite planning time.Moreover, the growth direction of a tree also reflects some properties of a tree's expanding ability which could be utilized by RRT to conduct the expanding procedure.One of the possible research directions will focus on the planning without any prior information in the uncertain environment considering such indicators proposed in this work.

Figure 1 :
Figure1: Grid point distance from the nearest node in the tree. is an adjustable grid spacing, and the dashed arrow indicates the distance,   , from the grid points to their closet nodes in tree.

Figure 3 :Figure 4 :Figure 5 :
Figure 3: RRT-blossom regression.New point tends to stay away from the existing structure, like the hollow circles.

Figure 8 :
Figure 8: Planning with the same conditions by RRT.Start position locates at 157 ∘ 58  30  W, 21 ∘ 20  29  N; Goal locates at 157 ∘ 57  47  W, 21 ∘ 21  14  N, shown as a solid blue circle.Red line describes a valid path from start to goal.
time interval Δ.Without loss of generality, we assume that an appropriate discrete control set  is defined and the cardinality of  is : that is,  =  1 ,  2 , . ..,   .Let   denote the parent and let    +1 ,  = 1, . ..,  denote the successor of   .The superscript   means the control vector applied on   , where the gray areas represent the obstacles.If    +1 locates in  obs for all  = 1, 2, . .., , it certainly means that   is dead for the concurrent tree.Also, this expansion by   should be considered totally useless.By reducing useless nodes the searching process can be obtained more efficiently.Based on that, the available potential successor set of   is achieved by applying ; that is, X +1 = { 1 ,  2 , . . .,   be independent Poisson trials such that, for 1 ≤  ≤ , Pr[  = 1] =   , where 0 <   < 1.Then, for C = ∑  =1   ,  = E[C] = ∑  =1   , and 0 <  ≤ 1,

Proposition 7 .
Given specific ,  free ,  start , and a set of goal regions { 1goal ,  2goal , . . .,  goal }, while solving Problem 2, the liveliness-based RRT algorithm always has smaller failing probability than the basic RRT algorithm.