# Liveness-Based RRT Algorithm for Autonomous Underwater Vehicles Motion Planning

Authors: Yang Li; Fubin Zhang; Demin Xu; Jiguo Dai
Journal: Journal of Advanced Transportation
 (2017)

---

## Abstract

Motion planning is a crucial, basic issue in robotics, which aims at driving vehicles or robots towards to a given destination with various constraints, such as obstacles and limited resource. This paper presents a new version of rapidly exploring random trees (RRT), that is, liveness-based RRT (Li-RRT), to address autonomous underwater vehicles (AUVs) motion problem. Different from typical RRT, we define an index of each node in the random searching tree, called “liveness” in this paper, to describe the potential effectiveness during the expanding process. We show that Li-RRT is provably probabilistic completeness as original RRT. In addition, the expected time of returning a valid path with Li-RRT is obviously reduced. To verify the efficiency of our algorithm, numerical experiments are carried out in this paper.


---

## Body

## 1. 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–7].Another remarkable probabilistic method was proposed as Probabilistic Roadmap (PRM) [8–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–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–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–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. Section2 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.


## 2. Preliminaries and Problem Formation

### 2.1. Problem Formulation

Path planning problem can be generally viewed as a search problem in the C-spaceC⊆Rn. For instance, the C-space of a planar two-wheel mobile robot can be defined as C⊆R3, that is, position (x,y), heading angle θ. In this way, any x∈C⊆R3 can describe the state of the mobile robot. Additionally, we define Cobs⊂C and Cfree=C∖Cobs as the obstacle C-space and free C-space separately. The path planning problems require a finite sequence of states in Cfree from an initial state to a given goal state. Here we present the basic path planning problem as defined in [24].Problem 1 (path planning problem [24]).
The bounded C-spaceC, obstacle space Cobs, initial state xstart∈Cfree, goal region Xgoal∈Cfree, denoted as tuple {xstart,Cfree,Xgoal}, return a path ξ:[0,1]→Cfree, such that ξ(0)=xstart, ξ(1)∈Xgoal. If there exist no feasible paths, return failure.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 sequenceu(s):[0,1]→U that drives the AUV from xstart to Xgoal. Subsequently the path can also be viewed as a function of input sequence ξ(u(·)):u(·)→Cfree. Formally, we give the definition of trajectory planning problem for AUVs.Problem 2 (trajectory planning problem for AUVs).
Given{xstart,Cfree,Xgoal} and kinodynamic constraint of AUVs x˙=f(x,u), return an available control sequence u:[0,1]→U, such that the path ξ(u(·))={x∈Cfree∣ξ(u(0))=xinit, ξ(u(1))∈Cfree, ξ(u(s))=∫01[xinit+f(x,u)]ds}.In this paper, we focus on the trajectory planning problem for AUVs. Since the initial force has major impact on the motion of AUVs compared to ground vehicles, the kinodynamic constraints are more complex.


## 2.1. Problem Formulation

Path planning problem can be generally viewed as a search problem in the C-spaceC⊆Rn. For instance, the C-space of a planar two-wheel mobile robot can be defined as C⊆R3, that is, position (x,y), heading angle θ. In this way, any x∈C⊆R3 can describe the state of the mobile robot. Additionally, we define Cobs⊂C and Cfree=C∖Cobs as the obstacle C-space and free C-space separately. The path planning problems require a finite sequence of states in Cfree from an initial state to a given goal state. Here we present the basic path planning problem as defined in [24].Problem 1 (path planning problem [24]).
The bounded C-spaceC, obstacle space Cobs, initial state xstart∈Cfree, goal region Xgoal∈Cfree, denoted as tuple {xstart,Cfree,Xgoal}, return a path ξ:[0,1]→Cfree, such that ξ(0)=xstart, ξ(1)∈Xgoal. If there exist no feasible paths, return failure.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 sequenceu(s):[0,1]→U that drives the AUV from xstart to Xgoal. Subsequently the path can also be viewed as a function of input sequence ξ(u(·)):u(·)→Cfree. Formally, we give the definition of trajectory planning problem for AUVs.Problem 2 (trajectory planning problem for AUVs).
Given{xstart,Cfree,Xgoal} and kinodynamic constraint of AUVs x˙=f(x,u), return an available control sequence u:[0,1]→U, such that the path ξ(u(·))={x∈Cfree∣ξ(u(0))=xinit, ξ(u(1))∈Cfree, ξ(u(s))=∫01[xinit+f(x,u)]ds}.In this paper, we focus on the trajectory planning problem for AUVs. Since the initial force has major impact on the motion of AUVs compared to ground vehicles, the kinodynamic constraints are more complex.


## 3. Algorithms

RRT was firstly introduced by Lavalle as a sampling-based path planning algorithm, which is shown in Algorithm1. It has the ability of quickly searching or exploring the C-space by a random tree. At each iteration, the tree expands sequentially towards the goal. We refer readers to more details about RRT in [12].Algorithm 1:Body of basic RRT algorithm [33].
Require: C-spaceC, Free C-space Cfree, Initial state
xstart, Goal region Xgoal, max searching iteration K;
Ensure: Feasible pathτ
(1)V←(xstart); E←∅; G←(V,E); // Initialization
(2) fork=1,2,…,K do
(3)xrand←Sample(k); // Randomized sample in Cfree
(4)G(V,E)← Extend(G, xrand); // Extend random tree
(5) end for
(6)τ← Export(G); // Return the feasible path
(7) returnτ;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 Cfree: that is, limn→∞⁡P{∀x∈C,∃x′∈xRRT,|x-x′|<ϵ,ϵ>0}=1.


### 3.1. Coverage of a Graph

A kind of coverage measure of the tree in C-space was proposed in [34]. We uniformly overlay grids of ng points and spacing δ on C. Define the minimum distance from each grid point j to the nearest node in the tree as dj. Thus, min⁡(dj,δ) 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].(1)cG=1δ∑j=1ngmin⁡dj,δng,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 δ.Figure 1
Grid point distance from the nearest node in the tree.δ is an adjustable grid spacing, and the dashed arrow indicates the distance, dj, from the grid points to their closet nodes in tree.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,c(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 Cfree with given solution of the grids. Intuitively, we may try to optimize c(G) with an acceptable computing complexity, which is addressed in Section 3.4.


### 3.2. 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 asnt and then define the growth of the tree as [34](2)gG=-ΔcGΔnt. Because the differential form dc(G)/dnt is intractably formed and computed, we utilize the difference form to indicate the growing speed of the tree. Obviously, g(G) is able to be used to test the nodes of tree appropriately. Let g¯ denote the threshold, if g(G) drops below g¯ with new node vi, it means that vi is almost worthless.


### 3.3. 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.


#### 3.3.1. 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 Figure2, it describes the scope of a parent node that could be extended to within time interval Δt. Without loss of generality, we assume that an appropriate discrete control set U is defined and the cardinality of U is m: that is, U=u1,u2,…,um. Let xk denote the parent and let xk+1ui,i=1,…,m denote the successor of xk. The superscript ui means the control vector applied on xk, where the gray areas represent the obstacles. If xk+1ui locates in Cobs for all k=1,2,…,m, it certainly means that xk is dead for the concurrent tree. Also, this expansion by xk should be considered totally useless. By reducing useless nodes the searching process can be obtained more efficiently.Figure 2
Latent expansion with obstacles.Based on that, the available potential successor set ofxk is achieved by applying U; that is, X~k+1U={xk+1ui∣Collision_Free(xk,xk+1ui)=TRUE,i=1,2,…,m}. Meantime, we denote the corresponding available control vector as U~={ui∣Collision_Free(xk,xk+1ui)=TRUE,i=1,2,…,m}. We denote the cardinality of U~ as ma and define collision detective index as Se(xk)=ma/m. Note that if Se(xk)=0, the point xk is a dead point. If so, xk is necessarily removed from the tree. Meanwhile, we could define the collision detective index of a tree as(3)SeG=∑k=1nlSexknl,where nl 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 Se¯, 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 expandxk if ma is not equal to zero. In other words, only {xk∣ma=0} could be removed in case of unexpected failure. In this way, the set of state {xk∣ma=0} can be viewed as the inevitable obstacles. Unlike that, the dead nodes in this paper are naturally cheap to compute.


#### 3.3.2. 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 Figure3(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.Figure 3
RRT-blossom regression. New point tends to stay away from the existing structure, like the hollow circles.
(a)
Regression expansions
(b)
Nonregressing expansionsIn Figure3(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 byd(xk). All offspring of a node is defined as(4)dsxk=∑xi∈Usxkdxi,where the set Us(xk) is the union of all offspring of the node xk. 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.Figure 4
The degree of nodes in a tree.The neighbors of a node are viewed as obstacles, which are shown in Figure5. The neighbor nodes are viewed as circular or spherical obstacles with a fixed radius. Define the number of neighbor collisions as manei(xk)=∑i=1m1{Collision_Free(xk,xk+1ui=TRUE)}, where 1(·) equals 1 if equation (·) holds.Figure 5
Neighbor obstacles. The radius of neighbor obstacle is generally obtained from experience.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(5)Lixk=Sexkexp⁡-mhexp⁡-dxkexp⁡-dsxknt,where nt is the number of total nodes at current iteration. Indeed, (5) reflects the expanding ability of a specific node. We translate Li(xk) in logs denoted as LnLi.(6)LnLixk=ln⁡Lixk=ln⁡Sexk-mh+dxk+dsknt.Since log function has the same monotonicity, LnLi(xk) can describe the liveness of each node. Particularly, if xk is a dead point, that is, Se(xk)=0, we set LnLi(xk)=-inf.Obviously, the livenessLnLi 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.


### 3.4. Li-RRT

In this section, we present the hybrid RRT algorithm, that is, Li-RRT, as shown in Algorithm2. 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 L={LnLi(v)∣v∈V} initially (line (1)).Algorithm 2:Liveliness-based RRT algorithm.
Require: C-spaceC, Obstacle space Cobs, Initial point
xstart, Goal region Xgoal, Discrete input set U=[u1,u2,…,uf],
Maximum search stepsK;
Ensure: Feasible trajectoryξ
(1)V←xstart; E←∅; G←(V,E); Liveness set L={Li(xstart)}
// Initialization
(2) fork=1,…,K do
(3)max⁡Li←argmaxv∈V⁡LnLiv
(4)   ifRand()<B then
(5)urand←RandU(u1,…,uf)
(6)xrand←drive(max⁡Li,urand); xnearest←max⁡Li
(7)        for eachxnear∈Xnear do
(8)           ifc(xnear,xrand)<c(max⁡Li,xrand) then
(9)xnearest←xnear
(10)           end if
(11)    end for
(12)   else
(13)     ifRand()<W then
(14)xrand←xgoal∈Xgoal
(15)    else
(16)xrand←RandProc(Cfree)
(17)xnearest←Nearest(xrand)
(18)    end if
(19)   end if
(20)xnew←drive(xnearest,xrand)
(21)   ifCollisionFree(xnearest,xrand) then
(22)V←V∪xnew; E←E∪(xnearest,xnew)
(23)  end if
(24)  ifxnew∈Xgoal then
(25)objective=TRUE
(26) end if
(27)   for allv∈V do
(28)UpdateLi(v)
(29)   end for
(30) end for
(31) returnξ if objective=TRUE; else failureAt each iteration, nodemax⁡Li pops with max liveness from L in line (3). Then an input vector is randomly selected from function RandU (line (5)), according to which a random state in Cfree is generated. Meantime, xrand is chosen from Xgoal 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 c(·). New node xnew then is achieved based on forward processing drive (line (20)). CollisionFree is called to verify the validity of xnew and (xnearest,xnew) (line (21)). Expanding process is implemented by adding xnew and (xnearest,xnew) (line (22)).Liveness of each node is updated byUpdateLi(v) in Algorithm 3. From the definition of LnLi(xk) (see (5)), we only need to care about three nodes sets, that is, {Vch1,Vch2,Vch3}. Vch1 contains only xnew, since Se(xk) is constant and is determined based on the state and the environments. Here we define that “all parent nodes” of xk are the nodes set {v∣xkcanbetracedfromv} different from the parent node xpar(xk). Vch2 includes exactly all parent nodes of xnew. Because xnew increases the successors of v∈Vch2, we set LnLi(v∈Vch2)=LnLi(v)+(ds(v)/nt-(ds(v)+1)/(nt+1)). Specifically, we set the liveness of xparx(xnew) as LnLi(xparx)=LnLi(xparx)-1. Vch3 is the near set of xnew: that is, Vch3={v∣c(v,xnew)≤H}. mh of v∈Vch3 will increase because of xnew. Thus, we define LnLi(v∈Vch3)=LnLi(v)-1.Algorithm 3:Update liveness algorithm.
Require: Near set thresholdH; Three nodes set
{Vch1,Vch2,Vch3}
(1)LnLi(xnew)←ln⁡Se(xnew)+[-mh(xnew)]
(2) forv∈Vch2 do
(3)LnLi(v)←LnLi(v)+(ds(v)/nt-(ds(v)+1)/(nt+1))
(4) end for
(5)xpar←parent(xnew)
(6)LnLi(xpar)←LnLi(xpar)-1
(7) forv∈Vch3=Near(xnew) do
(8)LnLi(v)←LnLi(v)-1
(9) end for


## 3.1. Coverage of a Graph

A kind of coverage measure of the tree in C-space was proposed in [34]. We uniformly overlay grids of ng points and spacing δ on C. Define the minimum distance from each grid point j to the nearest node in the tree as dj. Thus, min⁡(dj,δ) 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].(1)cG=1δ∑j=1ngmin⁡dj,δng,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 δ.Figure 1
Grid point distance from the nearest node in the tree.δ is an adjustable grid spacing, and the dashed arrow indicates the distance, dj, from the grid points to their closet nodes in tree.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,c(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 Cfree with given solution of the grids. Intuitively, we may try to optimize c(G) with an acceptable computing complexity, which is addressed in Section 3.4.


## 3.2. 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 asnt and then define the growth of the tree as [34](2)gG=-ΔcGΔnt. Because the differential form dc(G)/dnt is intractably formed and computed, we utilize the difference form to indicate the growing speed of the tree. Obviously, g(G) is able to be used to test the nodes of tree appropriately. Let g¯ denote the threshold, if g(G) drops below g¯ with new node vi, it means that vi is almost worthless.


## 3.3. 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.


### 3.3.1. 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 Figure2, it describes the scope of a parent node that could be extended to within time interval Δt. Without loss of generality, we assume that an appropriate discrete control set U is defined and the cardinality of U is m: that is, U=u1,u2,…,um. Let xk denote the parent and let xk+1ui,i=1,…,m denote the successor of xk. The superscript ui means the control vector applied on xk, where the gray areas represent the obstacles. If xk+1ui locates in Cobs for all k=1,2,…,m, it certainly means that xk is dead for the concurrent tree. Also, this expansion by xk should be considered totally useless. By reducing useless nodes the searching process can be obtained more efficiently.Figure 2
Latent expansion with obstacles.Based on that, the available potential successor set ofxk is achieved by applying U; that is, X~k+1U={xk+1ui∣Collision_Free(xk,xk+1ui)=TRUE,i=1,2,…,m}. Meantime, we denote the corresponding available control vector as U~={ui∣Collision_Free(xk,xk+1ui)=TRUE,i=1,2,…,m}. We denote the cardinality of U~ as ma and define collision detective index as Se(xk)=ma/m. Note that if Se(xk)=0, the point xk is a dead point. If so, xk is necessarily removed from the tree. Meanwhile, we could define the collision detective index of a tree as(3)SeG=∑k=1nlSexknl,where nl 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 Se¯, 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 expandxk if ma is not equal to zero. In other words, only {xk∣ma=0} could be removed in case of unexpected failure. In this way, the set of state {xk∣ma=0} can be viewed as the inevitable obstacles. Unlike that, the dead nodes in this paper are naturally cheap to compute.


### 3.3.2. 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 Figure3(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.Figure 3
RRT-blossom regression. New point tends to stay away from the existing structure, like the hollow circles.
(a)
Regression expansions
(b)
Nonregressing expansionsIn Figure3(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 byd(xk). All offspring of a node is defined as(4)dsxk=∑xi∈Usxkdxi,where the set Us(xk) is the union of all offspring of the node xk. 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.Figure 4
The degree of nodes in a tree.The neighbors of a node are viewed as obstacles, which are shown in Figure5. The neighbor nodes are viewed as circular or spherical obstacles with a fixed radius. Define the number of neighbor collisions as manei(xk)=∑i=1m1{Collision_Free(xk,xk+1ui=TRUE)}, where 1(·) equals 1 if equation (·) holds.Figure 5
Neighbor obstacles. The radius of neighbor obstacle is generally obtained from experience.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(5)Lixk=Sexkexp⁡-mhexp⁡-dxkexp⁡-dsxknt,where nt is the number of total nodes at current iteration. Indeed, (5) reflects the expanding ability of a specific node. We translate Li(xk) in logs denoted as LnLi.(6)LnLixk=ln⁡Lixk=ln⁡Sexk-mh+dxk+dsknt.Since log function has the same monotonicity, LnLi(xk) can describe the liveness of each node. Particularly, if xk is a dead point, that is, Se(xk)=0, we set LnLi(xk)=-inf.Obviously, the livenessLnLi 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.


## 3.3.1. 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 Figure2, it describes the scope of a parent node that could be extended to within time interval Δt. Without loss of generality, we assume that an appropriate discrete control set U is defined and the cardinality of U is m: that is, U=u1,u2,…,um. Let xk denote the parent and let xk+1ui,i=1,…,m denote the successor of xk. The superscript ui means the control vector applied on xk, where the gray areas represent the obstacles. If xk+1ui locates in Cobs for all k=1,2,…,m, it certainly means that xk is dead for the concurrent tree. Also, this expansion by xk should be considered totally useless. By reducing useless nodes the searching process can be obtained more efficiently.Figure 2
Latent expansion with obstacles.Based on that, the available potential successor set ofxk is achieved by applying U; that is, X~k+1U={xk+1ui∣Collision_Free(xk,xk+1ui)=TRUE,i=1,2,…,m}. Meantime, we denote the corresponding available control vector as U~={ui∣Collision_Free(xk,xk+1ui)=TRUE,i=1,2,…,m}. We denote the cardinality of U~ as ma and define collision detective index as Se(xk)=ma/m. Note that if Se(xk)=0, the point xk is a dead point. If so, xk is necessarily removed from the tree. Meanwhile, we could define the collision detective index of a tree as(3)SeG=∑k=1nlSexknl,where nl 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 Se¯, 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 expandxk if ma is not equal to zero. In other words, only {xk∣ma=0} could be removed in case of unexpected failure. In this way, the set of state {xk∣ma=0} can be viewed as the inevitable obstacles. Unlike that, the dead nodes in this paper are naturally cheap to compute.


## 3.3.2. 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 Figure3(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.Figure 3
RRT-blossom regression. New point tends to stay away from the existing structure, like the hollow circles.
(a)
Regression expansions
(b)
Nonregressing expansionsIn Figure3(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 byd(xk). All offspring of a node is defined as(4)dsxk=∑xi∈Usxkdxi,where the set Us(xk) is the union of all offspring of the node xk. 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.Figure 4
The degree of nodes in a tree.The neighbors of a node are viewed as obstacles, which are shown in Figure5. The neighbor nodes are viewed as circular or spherical obstacles with a fixed radius. Define the number of neighbor collisions as manei(xk)=∑i=1m1{Collision_Free(xk,xk+1ui=TRUE)}, where 1(·) equals 1 if equation (·) holds.Figure 5
Neighbor obstacles. The radius of neighbor obstacle is generally obtained from experience.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(5)Lixk=Sexkexp⁡-mhexp⁡-dxkexp⁡-dsxknt,where nt is the number of total nodes at current iteration. Indeed, (5) reflects the expanding ability of a specific node. We translate Li(xk) in logs denoted as LnLi.(6)LnLixk=ln⁡Lixk=ln⁡Sexk-mh+dxk+dsknt.Since log function has the same monotonicity, LnLi(xk) can describe the liveness of each node. Particularly, if xk is a dead point, that is, Se(xk)=0, we set LnLi(xk)=-inf.Obviously, the livenessLnLi 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.


## 3.4. Li-RRT

In this section, we present the hybrid RRT algorithm, that is, Li-RRT, as shown in Algorithm2. 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 L={LnLi(v)∣v∈V} initially (line (1)).Algorithm 2:Liveliness-based RRT algorithm.
Require: C-spaceC, Obstacle space Cobs, Initial point
xstart, Goal region Xgoal, Discrete input set U=[u1,u2,…,uf],
Maximum search stepsK;
Ensure: Feasible trajectoryξ
(1)V←xstart; E←∅; G←(V,E); Liveness set L={Li(xstart)}
// Initialization
(2) fork=1,…,K do
(3)max⁡Li←argmaxv∈V⁡LnLiv
(4)   ifRand()<B then
(5)urand←RandU(u1,…,uf)
(6)xrand←drive(max⁡Li,urand); xnearest←max⁡Li
(7)        for eachxnear∈Xnear do
(8)           ifc(xnear,xrand)<c(max⁡Li,xrand) then
(9)xnearest←xnear
(10)           end if
(11)    end for
(12)   else
(13)     ifRand()<W then
(14)xrand←xgoal∈Xgoal
(15)    else
(16)xrand←RandProc(Cfree)
(17)xnearest←Nearest(xrand)
(18)    end if
(19)   end if
(20)xnew←drive(xnearest,xrand)
(21)   ifCollisionFree(xnearest,xrand) then
(22)V←V∪xnew; E←E∪(xnearest,xnew)
(23)  end if
(24)  ifxnew∈Xgoal then
(25)objective=TRUE
(26) end if
(27)   for allv∈V do
(28)UpdateLi(v)
(29)   end for
(30) end for
(31) returnξ if objective=TRUE; else failureAt each iteration, nodemax⁡Li pops with max liveness from L in line (3). Then an input vector is randomly selected from function RandU (line (5)), according to which a random state in Cfree is generated. Meantime, xrand is chosen from Xgoal 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 c(·). New node xnew then is achieved based on forward processing drive (line (20)). CollisionFree is called to verify the validity of xnew and (xnearest,xnew) (line (21)). Expanding process is implemented by adding xnew and (xnearest,xnew) (line (22)).Liveness of each node is updated byUpdateLi(v) in Algorithm 3. From the definition of LnLi(xk) (see (5)), we only need to care about three nodes sets, that is, {Vch1,Vch2,Vch3}. Vch1 contains only xnew, since Se(xk) is constant and is determined based on the state and the environments. Here we define that “all parent nodes” of xk are the nodes set {v∣xkcanbetracedfromv} different from the parent node xpar(xk). Vch2 includes exactly all parent nodes of xnew. Because xnew increases the successors of v∈Vch2, we set LnLi(v∈Vch2)=LnLi(v)+(ds(v)/nt-(ds(v)+1)/(nt+1)). Specifically, we set the liveness of xparx(xnew) as LnLi(xparx)=LnLi(xparx)-1. Vch3 is the near set of xnew: that is, Vch3={v∣c(v,xnew)≤H}. mh of v∈Vch3 will increase because of xnew. Thus, we define LnLi(v∈Vch3)=LnLi(v)-1.Algorithm 3:Update liveness algorithm.
Require: Near set thresholdH; Three nodes set
{Vch1,Vch2,Vch3}
(1)LnLi(xnew)←ln⁡Se(xnew)+[-mh(xnew)]
(2) forv∈Vch2 do
(3)LnLi(v)←LnLi(v)+(ds(v)/nt-(ds(v)+1)/(nt+1))
(4) end for
(5)xpar←parent(xnew)
(6)LnLi(xpar)←LnLi(xpar)-1
(7) forv∈Vch3=Near(xnew) do
(8)LnLi(v)←LnLi(v)-1
(9) end for


## 4. Analysis of the Algorithms

To prove the following theorems, we remind the definition of attraction sequence [35]. Let A={A0,A1,…,Ak} be a finite sequence of sets as follows. (i) For each Ai, there exists a set Ai⊆Bi⊆Cfree called the basin of Ai, and ∀x∈Ai-1 and y∈Ai and z∈Cfree∖Bi, x-y≤x-z holds. (ii) ∀x∈Bi, there exists an m such that the sequence of action {u1,u2,…,um} selected by LOCAL_PLANNER algorithm will bring the point into Ai⊆Bi. (iii) A0={xstart} and Ak={Xgoal}.An attractor regionAi like a funnel as a metaphor for motions converges into a small region in the space. As shown in Figure 6, a basin Bi can be viewed as a safety zone which ensures that a point of Bi will be selected by the NEAREST_NEIGHBOR and potential well which attracts the point into Ai. Given an attraction sequence A with length k and letting pi=μ(Ai)/μ(Cfree) and pm=min⁡pi, we have the following lemma.Figure 6
Attraction sequence.Lemma 3 (see [36]).
LetC1,C2,…,Cn be independent Poisson trials such that, for 1≤i≤n, Pr[Ci=1]=pi, where 0<pi<1. Then, for C=∑i=1nCi, μ=E[C]=∑i=1npi, and 0<δ≤1,(7)PrC<1-δμ<exp⁡-μδ22.Lemma 4 (see [35]).
If an attraction sequence of lengthk exists, for a constant δ, the probability of basic RRT algorithm fails to find a trajectory after n iterations are at most exp⁡[-(1/2)(npm-2k)].Let sequence{p1′,p2′,…,pn′} be an ascending order that p1′≤p2′≤⋯≤pn′, where pi′∈{p1,p2,…,pn}, p1′=mini⁡pi and pn′=maxi⁡pi; the following theorem can be achieved.WhileCfree is partitioned into m connected regions, motion planning problem for multiple AUVs could be considered as an m goals version of Problem 2. A(i)={A1(i),A2(i),…,Aki(i)} is an attraction sequence which connects xstart and Xigoal, where ki is the length of attraction sequence. Let pj(i)=μ(Aj(i))/μ(Cfree),i=1,2,…,m,j=1,2,…,ki, pm=minij⁡pj(i); we have the following theorem.Theorem 5.
Ifm attraction sequences of length ki,i=1,2,…,m exist, the probability of basic RRT algorithm fails to find m trajectories after n iterations are at most exp-(1/2)npm-2∑i=1mki.Theorem 6.
Ifm attraction sequences of length ki,i=1,2,…,m exist, the probability of liveliness-based RRT algorithm fails to find m trajectories after n iterations are at most exp⁡(1/n)∑i=1m∑j=1ni((pj′(i)/pm)-(1/2)npm+∑i=1mki).Proof.
Let sequence(p1′(i),p2′(i),…,pni′(i)) be m ascending orders satisfying (p1′i≤p2′i≤⋯≤pni′(i)), where p1′(i)=minj⁡pj′(i), and pni′(i)=maxj⁡pj′(i),j=1,2,…,ni,i=1,2,…,m. While using the strategy of eliminating the “useless” nodes, the random variable C is viewed as Poisson trails with probability distribution (pj′i),i=1,2,…,m;j=1,2,…,ni. C is viewed as Poisson trails with probability distribution pj′(i); then μ=E[C]=∑i=1m∑j=1nipj′(i), and 0<δ≤1. According to Lemma 3, Pr[C<(1-δ)μ]<exp⁡(-μδ2/2), where δ=1-∑i=1mki/(npm). In addition,(8)-μδ22=-12∑i=1m∑j=1nipj′ipm1-∑i=1mkinpm2=1n∑i=1m∑j=1nipj′ipm-12npm+∑i=1mki-∑i=1mki22npm<1n∑i=1m∑j=1nipj′ipm-12npm+∑i=1mki.This completes the proof.Proposition 7.
Given specificC, Cfree, xstart, and a set of goal regions {X1goal,X2goal,…,Xmgoal}, while solving Problem 2, the liveliness-based RRT algorithm always has smaller failing probability than the basic RRT algorithm.Proof.
Note that(1/n)∑i=1m∑j=1ni(pj′i/pm)>1, according to Theorems 5 and 6,(9)PrLiRRT=exp⁡1n∑i=1m∑j=1nipj′ipm-12npm+∑i=1mki<exp⁡-12npm-2∑i=1mki=PrbRRT.In summary, we can see that Li-RRT enhances the expanding efficiency from Proposition7.


## 5. 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 Figure7(a). Without loss of generality, we assume that AUV only voyages at depth of 5 meters at least. And the dynamics of AUV is x˙=vcos⁡(θ), y˙=vsin⁡(θ), θ˙=ω. Meantime, we set 0≤|v|≤2 m/s and 0≤|ω|≤0.15 rad/s.Figure 7
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.
(a)
Hawaii DEM figure
(b)
Path found by Li-RRTWe can see that a valid path is found by Li-RRT shown as in Figure7(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.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.
(a)
RRT with 2075 iterations
(b)
RRT with 3093 iterationsHere 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.


## 6. 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.



---

*Source: 7816263-2017-10-19.xml*