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.
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 was detailed to be asymptotically optimal. That means will “almost surely” find the optimal path as the number of samples tends to infinity. applies the reconstruction of the searching tree, which always contains the optimal subtree at each iteration. Since then several 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 . 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 . If equals zero, it becomes essentially. In [26], 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.
2. Preliminaries and Problem Formation
2.1. Problem Formulation
Path planning problem can be generally viewed as a search problem in the C-space . For instance, the C-space of a planar two-wheel mobile robot can be defined as , that is, position , heading angle . In this way, any can describe the state of the mobile robot. Additionally, we define and as the obstacle C-space and free C-space separately. The path planning problems require a finite sequence of states in 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-space , obstacle space , initial state , goal region , denoted as tuple , return a path , such that , . 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 sequence that drives the AUV from to . Subsequently the path can also be viewed as a function of input sequence . Formally, we give the definition of trajectory planning problem for AUVs.
Problem 2 (trajectory planning problem for AUVs). Given and kinodynamic constraint of AUVs , return an available control sequence , such that the path , , .
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 Algorithm 1. 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].
| ||||||||||||||||||||
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 , 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 : that is, .
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 points and spacing on . Define the minimum distance from each grid point to the nearest node in the tree as . Thus, 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 , we define its coverage measure as in [34].where 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, 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 with given solution of the grids. Intuitively, we may try to optimize 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 as and then define the growth of the tree as [34] Because the differential form is intractably formed and computed, we utilize the difference form to indicate the growing speed of the tree. Obviously, is able to be used to test the nodes of tree appropriately. Let denote the threshold, if drops below with new node , it means that 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 Figure 2, it describes the scope of a parent node that could be extended to within time interval . Without loss of generality, we assume that an appropriate discrete control set is defined and the cardinality of is : that is, . Let denote the parent and let denote the successor of . The superscript means the control vector applied on , where the gray areas represent the obstacles. If locates in for all , 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, . Meantime, we denote the corresponding available control vector as . We denote the cardinality of as and define collision detective index as . Note that if , 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 aswhere 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. , defined in (3), may give a reasonable solution on the process of expansion of searching tree. Generally, we prefer the nodes that make 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 could be removed in case of unexpected failure. In this way, the set of state 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 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.
(a) Regression expansions
(b) Nonregressing expansions
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 aswhere the set 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 , where equals 1 if equation holds.
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 ofwhere is the number of total nodes at current iteration. Indeed, (5) reflects the expanding ability of a specific node. We translate in logs denoted as .Since log function has the same monotonicity, can describe the liveness of each node. Particularly, if is a dead point, that is, , we set .
Obviously, the liveness defined in (6) will be attached to each node once added to the random tree. The value of 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 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 initially (line (1)).
| ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
At each iteration, node 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 is generated. Meantime, is chosen from with probability (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 then is achieved based on forward processing drive (line (20)). CollisionFree is called to verify the validity of and (line (21)). Expanding process is implemented by adding and (line (22)).
Liveness of each node is updated by in Algorithm 3. From the definition of (see (5)), we only need to care about three nodes sets, that is, . contains only , since is constant and is determined based on the state and the environments. Here we define that “all parent nodes” of are the nodes set different from the parent node . includes exactly all parent nodes of . Because increases the successors of , we set . Specifically, we set the liveness of as . is the near set of : that is, . of will increase because of . Thus, we define .
| ||||||||||||||||||||||
4. Analysis of the Algorithms
To prove the following theorems, we remind the definition of attraction sequence [35]. Let be a finite sequence of sets as follows. (i) For each , there exists a set called the basin of , and and and , holds. (ii) , there exists an such that the sequence of action selected by LOCAL_PLANNER algorithm will bring the point into . (iii) and .
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 with length and letting and , we have the following lemma.
Lemma 3 (see [36]). Let be independent Poisson trials such that, for , , where . Then, for , , and ,
Lemma 4 (see [35]). If an attraction sequence of length exists, for a constant , the probability of basic RRT algorithm fails to find a trajectory after iterations are at most .
Let sequence be an ascending order that , where , and ; the following theorem can be achieved.
While is partitioned into connected regions, motion planning problem for multiple AUVs could be considered as an goals version of Problem 2. is an attraction sequence which connects and , where is the length of attraction sequence. Let , ; we have the following theorem.
Theorem 5. If attraction sequences of length exist, the probability of basic RRT algorithm fails to find trajectories after iterations are at most .
Theorem 6. If attraction sequences of length exist, the probability of liveliness-based RRT algorithm fails to find trajectories after iterations are at most
Proof. Let sequence be ascending orders satisfying , where , and . While using the strategy of eliminating the “useless” nodes, the random variable is viewed as Poisson trails with probability distribution . is viewed as Poisson trails with probability distribution ; then , and . According to Lemma 3, , where . In addition,This completes the proof.
Proposition 7. Given specific , , , and a set of goal regions , while solving Problem 2, the liveliness-based RRT algorithm always has smaller failing probability than the basic RRT algorithm.
Proof. Note that , according to Theorems 5 and 6,
In summary, we can see that Li-RRT enhances the expanding efficiency from Proposition 7.
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 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 , , . Meantime, we set m/s and rad/s.
(a) Hawaii DEM figure
(b) Path found by Li-RRT
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.
(a) RRT with 2075 iterations
(b) RRT with 3093 iterations
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.
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.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This work was supported by the National Natural Science Foundation of China (NSFC) under Grants 61472325 and 61273333.