Triangular Geometrized Sampling Heuristics for Fast Optimal Motion Planning

Abstract

Rapidly-exploring Random Tree (RRT)-based algorithms have become increasingly popular due to their lower computational complexity as compared with other path planning algorithms. The recently presented RRT^* motion planning algorithm improves upon the original RRT algorithm by providing optimal path solutions. While RRT determines an initial collision-free path fairly quickly, RRT^* guarantees almost certain convergence to an optimal, obstacle-free path from the start to the goal points for any given geometrical environment. However, the main limitations of RRT^* include its slow processing rate and high memory consumption, due to the large number of iterations required for calculating the optimal path. In order to overcome these limitations, we present another improvement, i.e, the Triangular Geometerized-RRT^* (TG-RRT^*) algorithm, which utilizes triangular geometrical methods to improve the performance of the RRT^* algorithm in terms of the processing time and a decreased number of iterations required for an optimal path solution. Simulations comparing the performance results of the improved TG-RRT^* with RRT^* are presented to demonstrate the overall improvement in performance and optimal path detection.

Keywords

Motion planning Sampling-based algorithm Optimal path planning Triangular geometry

1. Introduction

Motion planning algorithms provide robots with collision-free paths from a start point to a specified end point in any given configuration space. The motion planning problem has gained in importance over the past decade due to its widespread application in the field of robotics as well as its vital role in the fields of computer animation [1], manufacturing [2], computational biology [3], and many other aspects of daily life.

Various motion planning techniques have been proposed, such as neural networks [4], sampling-based algorithms [5] and potential field methods [6], etc. Some of these motion planners rely on exploration while others rely on the exploitation of configuration spaces for solving motion planning problems. Of these, incremental sampling-based algorithms, like the RRT [7, 8] and Probabilistic Road Maps (PRM) [5], rely upon the exploration of configuration spaces in order to improve the planner's understanding of a given space [13]. These planners have proven to be effective for a large class of problems in different domains. These are probabilistically complete algorithms, generating a path solution where one exists, irrespective of the obstacle geometry in the configuration space. For such motion planning algorithms, the probability of finding a collision-free trajectory from the start to end points in a test environment is said to be one, if the number of iterations performed are allowed to approach infinity. However, there also exist other classes of algorithms, such as complete and resolution complete classes of algorithms. Complete motion planning algorithms [9, 10] provide a solution, if one exists, in a finite time, but are often computationally inefficient [11] for common practical applications [12]. The other class, known as the resolution complete class of algorithms, also ensures certain convergence to a solution in a finite time, provided that the resolution parameters are tuned precisely. The algorithms providing resolution completeness include Artificial Potential Fields (APF) [6] and cell decomposition methods [14]. APF performs pure exploitation [13]. Exploitation assumes that the provided information is sufficient to admit a path solution; thus, exploitation makes the planner greedy and allows it to quickly compute the solution. However, this pure exploitation also causes APF to suffer from the local minima problem [15]. Moreover, on the other side, the cell decomposition method involves an extremely large number of cells, which makes it computationally inefficient. These limitations make these methods unsuited to the motion planning of robots placed in complex environments [12]. Algorithms ensuring probabilistic completeness are therefore the best choice for motion planning problems, since they do not require prior information of any obstacle geometry, which makes them computationally inexpensive. Arguably, the most well-known sampling-based algorithms are PRM [5] and RRT [7]. However, PRM and its variants are multiple-query methods. RRT is a single-query planning method and most motion planning problems can actually be resolved as single-query problems [12]. Moreover, PRM requires the preprocessing of road maps, which makes it unsuitable for many practical problems [12]. The RRT algorithm is therefore widely used due to its efficiency, and significant research in this field has resulted in a number of improvements [16, 17]. One of these was an extension of the RRT algorithm called the RRT^* algorithm. Unlike RRT, RRT^* provides asymptotic optimality, i.e., almost-certain convergence to an optimal solution without any significant computational overhead [12]. This is particularly attractive for applications running in real-time [18]. However, despite this optimality, some constraints still exist with RRT^*, inherited from the original RRT algorithm. These incremental sampling-based algorithms perform pure exploration, i.e, they do not assess whether a particular region is relevant to solving a problem. This pure exploration causes RRT^* to suffer from slow convergence rates in calculating an optimal path, which in turn causes it to utilize notably large amounts of memory. Moreover, it has also been proven that the most efficient motion planning algorithm would be one that performs both exploration and exploitation [13]. We therefore introduce the TG-RRT^* algorithm [19], another improved variant of the RRT^* algorithm, which performs both exploration and exploitation.

TG-RRT^* makes use of basic triangular geometrical techniques - such as triangular in-centre and centroid - to reduce the dispersion of random samples. While the RRT^* algorithm uniformly samples the configuration space, TG-RRT^* directs the random samples towards the centre of the triangle formed by the initial, goal and random sample locations. In this paper, we modify the TG-RRT^* algorithm to perform guided exploration. The proposed algorithm starts with exploitation by directing the random samples, using a triangular geometry, for a constant number of iterations. Once this limit is crossed, the algorithm switches from exploitation (directed sampling), to exploration (uniform sampling). Introducing this hybrid sampling heuristic (i.e., exploitation/exploration to TG-RRT^*) improves the robustness of the TG-RRT^* algorithm, making it a rapidly converging and memory-efficient algorithm for all types of environments. We further implement this modified version of TG-RRT^* in both 2D and 3D environments, providing detailed analysis of its probabilistic completeness, asymptotic optimality and computational complexity.

The rest of the paper is organized as follows. Section II describes some preliminaries, including the RRT^* algorithm and triangular geometric techniques, while Section III describes the TG-RRT^* path planning algorithm in detail. Section IV presents an analysis of the of the proposed algorithm in terms of probabilistic completeness, asymptotic optimality and computational complexity. Section V provides experimental evidence in support of the theoretical results presented in the previous section, whereas Section VI concludes the paper while suggesting some future areas of research in this particular domain.

2. Preliminaries

The motion planning problem is to find an optimal path solution connecting an initial state and the goal region as quickly as possible. This section describes the notations used throughout this paper and the formulation of the three major motion planning problems.

Let ℕ and ℝ denote the set of positive integers and real numbers, respectively. Given n-tuple U, a sequence on U defined as {u_i}_iεℕ is a mapping from ℕ to U, where u_i ε U and i ε ℕ. Moreover, each set U is equipped with delete and add procedures, such that U.add(u): = U ∪ {u} while U. delete(u): = U\{u}. Given that a, b ε ℝ, the open and closed interval is denoted as (a, b) and [a, b], respectively. Let G ⊂ ℝ^d represent the given state space, where d denotes the dimension of the state space, i.e., d ε ℕ: d ≥ 2. The obstacle-free and obstacle configuration space are defined as G_free ⊂ G and G_obs = G \ G_free, respectively. The initial state and the goal region are denoted as g_init ε G_free and G_goal ⊂ G_free, respectively. The Lebesgue measure of any set X ⊂ ℝ^d is denoted by μ(X). The Euclidean norm is denoted by ‖ · ‖. The spherical ball of radius r ε ℝ₊ centred at any state g ε G is defined as B_g,r: = {y ε G : ‖ y, g ‖ ≤ r}. Given two states g₁, g₂ ε G, a continuous path in G connecting g₁ and g₂ is represented as τ :[0,1, such that τ(0) = g₁ and τ(1)=g₂. This path τ is feasible if it lies completely in an obstacle-free state space G_free, and if it connects an initial state g_init and the goal region G_goal, i.e., τ(0) = g_init and τ(1) ε G_goal, such that τ:[0, 1] ↦ G_free. Problem 1 formalizes the feasibility problem of path planning.

Problem 1 (The Feasible Path Planning Problem) Given a bounded connected state space G and obstacle space G_obs, an initial state g_init and a goal region G_goal ⊂ G_free, the feasible path planning problem is to find a path τ: [0, 1] → G_free such that τ(0) = g_init and τ(1) ε G_goal.

The cost function c(·) calculates the path length in terms of the Euclidean norm. Let Σ_f denote the set of all collision-free paths of non-zero positive length between any two states in the closure obstacle-free space cl (G_free). The optimal path planning problem is to find a feasible path with minimum cost c^*, and can be formally described as follows.

Problem 2 (The Optimal Path Planning Problem) Assuming that the solution to a given path planning problem {G, G_obs, G_goal} exists, find a path τ^* = [0, 1] → G_free such that $c (τ^{*}) = {{min}_{τ \in c l (G_{f r e e})} c (τ)}$ .

Let t ε ℝ denote the time taken by the algorithm to determine the required path described in Problem 2. The rapid optimal path planning problem states that the algorithm must be able to compute the optimal path solution in the least possible time as stated in Problem 3.

Problem 3 (The Rapid Optimal Path Planning Problem) Given the path planning problem {G, G_obs, G_goal}, find an optimal solution, if one exists, in the least possible time t ε ℝ.

2.1 The RRT^* Algorithm

The RRT^* algorithm is a recently introduced variant of RRT algorithm which, unlike RRT, ensures asymptotic optimality, i.e., almost certain convergence to an optimal solution, without incurring any substantial computational overheads. This section describes a slightly modified implementation of the RRT^* algorithm [20]. This modification results in the improved computational efficiency of RRT^* by reducing the number of calls to the Collision-free procedure [20]. The following are some of the processes utilized by RRT^*:

Randomized Sampling: The RandomizedSample procedure returns an independent and identically distributed random sample from the collision-free space, i.e., g_rand ε G_free.

Collision Checking: The function Collision–free (τ) checks whether or not the given path τ:[0, 1] lies completely inside a collision-free space G_free. A true value is reported if τ → G_free, otherwise it reports failure.

Near Vertices: Given a tree T = (V, E) composed of a finite set of vertices V ⊂ G_free and a random sample g ε G, the NearSet (g, T) returns the finite set of vertices that are within the ball region $ℬ_{g, r}$ of radius r centred at g. More precisely, the set of near vertices is defined as:

NearSet (g, T) : = {v \in V : v \in ℬ_{g, r}} .

The radius r ε ℝ₊ depends upon the dimensions of the space, i.e., d ≥ 2, the number of vertices in the tree j ε ℝ_>0, and the independent constant γ, by the relationship r : = γ(log j/j)^1/d.

Nearest Vertex: Given the tree T = (V, E) with a finite set of vertices V ⊂ G_free, the procedure Nearest Vertex (T, g) returns the nearest vertex in the tree from any given configuration g ε G. More specifically, this function can be defined as:

NearestVertex (T, g) {:=argmin}_{v \in V} ‖ g - v ‖ .

Getting Sorted List: The procedure SortedList in Algorithm 2 constructs a list L with an ordered set of elements. Each element of this list is a triplet of the form (c_i, τ_i, g_i), where c_i ε R_≥0 is the cost, τ_i ε Σ_f is the trajectory, and g_i ε G. After populating the list L, the elements are sorted in ascending order of their cost value.

Steering: Given any two states g₁, g₂ ε G, the Steer(g₁, g₂) function returns the straight trajectory τ : [0, 1] connecting g₁ and g₂, i.e., τ(0) = g₁ and τ(1) = g₂.

Choosing the Best Parent: The BestParent(L) procedure iterates over the sorted list L and searches for the shortest collision-free path τ _i connecting the initial state g_init to the random sample g_rand through the configuration g_i. Since the list presented to this procedure is already sorted in ascending order of path costs, this saves computation time and allows the function to quickly return the configuration g_i connecting g_init to g_rand with a minimum-cost, collision-free trajectpry:

Figure 1.

Triangular Geometry Techniques

The RRT^* algorithm, presented in Algorithm 1, is asymptotically optimal. Once the preliminary initialization process takes place, the algorithm samples the entire given configuration space G (Line 3) by picking up a random sample g_rand iteratively. In each iteration, the set of near vertices G_near are determined. These are those vertices of the random tree lying within a ball region centred at g_rand. If, however, no such near vertices exist and the set G_near computed by the NearSet procedure contains no data, G_near is subsequently filled by the NearestVertex process (Line 4–6). As indicated by (Line 7), once this set has been filled, RRT^* sorts it in order to form a tuple, arranged in order of the increasing value of the cost function. The function BestParent in turn iterates over the list L (Line 8), using it to establish the most suitable parent vertex g_min ε G_near which can be used to construct a path in the obstacle-free region of the configuration space between the initial g_init and random sample g_rand points. Once such a state has been determined, the random tree is expanded by making g_rand a child of g_min and then rewiring the tree (Line 9–11). This rewiring process is illustrated in Algorithm 4. Each vertex g ε L lying within the sorted list L is examined. If the cost of the obstacle-free path connecting g_init and g through g_rand is found to be less than the existing cost of travelling to point g (Algorithm 4 Line 1–3), then the tree is rewired and g_rand is made to be the parent of g (Algorithm 4 Line 4–5). If, however, these conditions are not met, the tree remains unchanged and RRT^* moves on to examine the next vertex for the same conditions. This process continues for every vertex g in the sorted list L.

2.2 Triangular Geometrical Methods

Four common triangular centres exist, namely the incentre, the orthocentre, the circumcentre and the centroid. During the experimentation phase, all four of these centres were, one-by-one, appended to the TG-RRT^* algorithm and the behaviour of the resulting algorithm was carefully observed. Out of these aforementioned triangular centres, only the incentre and the centroid methods were successful in enhancing the performance parameters of the original RRT^* algorithm for almost all test environments. The other two methods, namely the circumcentre and the orthocentre, sometimes caused the RRT^* algorithm to take an extremely large amount of time to determine a conclusive, optimized path, thereby degrading the capabilities of RRT^* instead of enhancing them. This defection can be explained by the fact that out of the four, these two types of centres do not always lie within the triangle formed by the goal, root and random sample nodes. Therefore, instead of bringing the random sample and the goal closer, they may instead cause them to be pushed further away from each other, thereby increasing the time taken by the algorithm to reach an optimal solution. The four types of triangular centres are described in the following section:

2.2.1 Circumcentre

A circumcentre is the centre of a triangle's circumcircle, as shown in Fig. 1(a). It can be found as the intersection of the triangle's perpendicular bisectors.

2.2.2 Orthocentre

Shown in Fig. 1(b), the orthocentre is the point where the three “altitudes” of the triangle meet. An “altitude” is a line that passes through a vertex and is at right angles to the opposite side.

2.2.3 Incentre

The triangular incentre, as shown in Fig. 1(c), is the point of intersection of the three angle bisectors.

2.2.4 Centroid

The centroid is shown in Fig. 1(d) and is a point also known as the “centre of gravity”. The centroid is the point of intersection of the three medians. A median is a straight line that joins the midpoint of a side with the opposite triangular vertex.

3. Triangular Geometrized RRT^*

This section describes the proposed TG-RRT^* algorithm, outlined in Algorithm 5. Two triangular geometric techniques, the triangular incentre and the triangular centroid, guide the random samples and provide significant performance improvements in the motion planning algorithm. When a random sample g_rand is sampled from the configuration space using a uniform sampling heuristic (as dictated by RRT^*), our algorithm directs that sample by computing the geometrical centre of the starting state g_init, goal state g_goal ε G_goal and the random sample g_rand, and then treating this geometrical centre as the new random sample denoted by g_nrand. This step is described in Algorithm 5. The geometric centre is computed by either the incentre or the centroid method. If the TG-RRT^* algorithm utilizes the incentre method, it is called IC-RRT^*; if it instead uses the centroid method, then the resulting algorithm is referred to as C-RRT^*. Overall, the TG-RRT^* presents a hybrid sampling technique; it directs the random sample for a limited number of iterations κ ε ℝ₊ using either IC-RRT^* or C-RRT^*, after which it starts to perform uniform sampling (Lines 4–7). The value of κ is selected in order to maintain a balance between exploitation and exploration. In the current algorithm, the implementation the value of κ is assigned arbitrarily, so that it starts with exploitation by employing a greedy triangular geometric heuristic. After a selected number of iterations κ, it switches to the exploration of the configuration space. This allows the proposed planner to first exploit the configuration space by sampling the regions closer to the initial state g_init and goal region G_goal, and then begin exploring by sampling the remaining region. The advantage of this hybrid sampling will be illustrated in the experimental results. The following sections describe the implementation of the triangular geometric incentre and centroid techniques:

3.1 TGSample

This procedure directs the random sample to the geometric centre computed by either the incentre method or by the centroid method. The constant i ε z can take either zero or one as its value. If i = 0, then the incentre technique is used. If i = 1, the centroid method is used. The following subsections explain the implementation of the incentre and centroid methodologies.

3.1.1 Geometric Incentre

Algorithm 6 outlines the implementation of the incentre geometric method. Once this function is called, g_nrand contains the coordinates of the incentre of the starting, goal and random points. Moreover, the distance variable in our performance comparisons is computed using the Euclidean norm, since only the Euclidean space is considered. For other choices of distance function, see [21].

Algorithm 6: Incentre(g_rand).

1 d_rg ← ||g_init − g_goal||; 2 d_sg ← ||g_rand − g_goal||; 3 d_rs ← ||g_init − g_rand||; 4 d_total ← (d_rg + d_sg + d_rs); 5 g_nrand ← (d_rs × g_goal + d_rg × g_rand + d_sg × g_init)/d_total; 6 return g_nrand;

3.1.2 Geometric Centroid

Algorithm 7 describes the pseudo-code of the centroid function. Once this function is called, g_nrand contains the coordinates of the centroid of the starting, goal and random points.

Algorithm 7: Centroid(g_rand).

1 g_nrand ← (g_goal + g_rand + g_init)/3; 2 return g_nrand;

4. Analysis

4.1 Probabilistic Completeness

Most sampling-based algorithms ensure probabilistic completeness. Definition 1 formalizes the notion of probabilistic completeness, stating that if the solution to Problem 1 exists, then the probability of finding a feasible path approaches one as the number of iterations approaches infinity.

Definition 1 (Probabilistic Completeness) Given the path planning problem {G_free,g_init,G_goal}, an algorithm AL is probabilistically complete if $\lim_{i \to \infty} ℙ (V_{i}^{AL} \cap G_{goal} \neq ϕ) = 1$ , and the algorithmAL also connects g_init to g_goal ε G_goal.

RRT ensures probabilistic completeness, and its variant, i.e., RRT^* [12], also inherits this property from the original RRT as formulated in Theorem 1.

Theorem 1 (see [12]) Given the path planning problem {G_free, g_init, G_goal}, the probability of finding the solution to Problem 1, if one exists, approaches one as the number of iterations approaches infinity, i.e.,

\lim_{i \to \infty} ℙ ({V_{i}^{R R T^{*}} \cap G_{goal} \neq ϕ}) = 1

Similar to RRT^*, the proposed TG-RRT^* generates the random tree, which necessarily includes g_init as one of its states. Figures 2(a) to 2(c) show the exploration of an empty configuration space by IC-RRT^*, C-RRT^* and RRT^* respectively, while Figures 2(d) to 2(f) depict the Voronoi diagrams of the exploration of the configuration spaces by the corresponding algorithms due to their varying sampling heuristics. It can be seen in Figure 2, that TG-RRT^* has a Voronoi bias that first explores the space in close vicinity to the initial state and the goal region, only then exploring the rest of the configuration space by uniform sampling. Hence, TG-RRT^* performs random sampling exactly like the RRT^* algorithm, but goes on to direct those samples into the vicinity of the starting state and the goal region for a limited number of iterations. As such, TG-RRT^* explores the whole configuration space but in an intelligent way; therefore, the following theorem theorem formally states that TG-RRT^* also ensures probabilistic completeness.

Figure 2.

Voronoi biasing of IC-RRT^*, C-RRT^* and RRT^*

Theorem 2 Given a path planning problem {G_free, g_init, G_goal}, if a feasible path solution exists, then $\lim_{i \to \infty} ℙ ({V_{i}^{T G R R T^{*}} \cap G_{goal} \neq ϕ}) = 1$ .

4.2 Asymptotic Optimality

The proposed algorithm TG-RRT^* inherits the asymptotic optimality property from the original RRT^*. An algorithm is asymptotically optimal if it computes a minimum cost continuous path solution τ:[0, 1] such that τ(0) = g_init and τ(1) ε G_goal, all the while avoiding any collisions in the cluttered environment, i.e., τ ↦ G_free. This section analyses TG-RRT^* for its ability to solve Problem 2 based on the assumptions stated below.

Assumption 1 (Uniformity and Additivity of the Cost Procedure) For any set of paths in an obstacle-free space, i.e., τ₁,τ₂ ε Σ_f, the cost function c(·) must satisfy: c(τ₁)≤c(τ₁ | τ₂): c(τ₁ | τ₂) = c(τ₁) + c(τ₂).

Assumption 2 (Continuity of the Cost Procedure) The procedure (·) is a uniformly continuous function such that there exists a Lipschitz constant η for any two paths $τ_{1} : [0, l_{1}]$ and $τ_{2} : [0, l_{2}]$ , i.e., $| c (τ_{1}) - c (τ_{2}) | \leq η {sup}_{λ : [0,1]} ‖c (τ_{1} (λ l_{1})) - c (τ_{2} (λ l_{2}))‖$ .

Assumption 3 (δ -spacing of the Obstacle) For any state g ε G_free, there exists a ball region occurring in an obstacle-free space G_free (i.e., $ℬ_{g^{'}, δ} \subset G_{free}$ ) of radius δ ε ℝ_>0 centred around another point g′ ε G_free, such that $g \in ℬ_{g^{'}, δ}$ .

Assumption 1 simply states that the longer path has a higher cost than the shorter one. Assumption 2 ensures that two paths very close to one another and having approximately the same length also have a similar cost. Finally, Assumption 3 asserts that there exists some obstacle-free space around trajectories so that the algorithm can converge it to an optimal path solution. Based on the aforementioned assumptions, the RRT^* algorithm is an asymptotically optimal path planning algorithm provided that the dimension d ≥ 2 and the constant γ > γ^*: =2^d(1 +1/d) μ (G_free) [12]. The asymptotic optimality of RRT^* is formalized in the theorem stated below.

Theorem 3 (Asymptotic optimality of RRT^* [12]) Let Assumptions 1, 2 and 3 hold; then the RRT^* algorithm is asymptotic optimal whenever d ≥ 2 and γ > γ^* : = 2d(1+1/d) μ (G_free).

Figure 3.

(a) IC-RRT^*, (b) C-RRT^*, and (c) RRT^* performance in a 2D Cluttered Environment (A)

Figure 4.

(a) IC-RRT^*, (b) C-RRT^*, and (c) RRT^* performance in a 2D narrow passage environment

Recall that the TG-RRT^* algorithm introduces an intelligent hybrid sampling heuristic based upon a triangular geometry which only just assists in driving the random sample g_rand ε G_free into the vicinity of the starting state g_init and the goal region G_goal. After sample directionalization, the rest of the algorithm operates just like the original RRT^* algorithm (see Algorithm 5). The proposed heuristic is hybrid in the sense that it directs the random sample for a limited number of iterations, but then reverts to uniform sampling, which is the original sampling method of RRT^*. Since the rest of the algorithm's procedures, such as the NearSet and RewireVertices procedures, are the same for both TG-RRT^* and RRT^*, the following theorem presents itself.

Theorem 4 (Asymptotic optimality of TG-RRT^*) Let Assumptions 1, 2 and 3 hold; then the TG-RRT^* algorithm is asymptotic optimal whenever d≥2 and γ > γ^*: =2^d(1 + 1/d)μ(G_free).

This theorem relies on the fact that if γ > γ^*, the NearSet procedure would be able to compute a set of near vertices, which the RewireVertices procedure can then use to minimize distance variations (see Algorithm 4) in every successive iteration. The probability of converging to an optimal path solution therefore becomes one as a result of this minimization, dictating that the proposed TG-RRT^* algorithm will definitely provide a solution, if one exists, as the number of samples taken approaches a very large value.

4.3 Computational Complexity

This section compares the computational complexity of TG-RRT^* with the complexity of RRT^*. Let $ℳ_{j}^{TGRRT}$ and $ℳ_{j}^{RRT}$ denote the number of processes executed per iteration by TG-RRT^* and RRT^*, respectively. Theorem 5 proposes that the running time of all the processes executed per iteration by TG-RRT^* is a constant times higher than the running time of all the processes executed per iteration by RRT^*.

Theorem 5 The computational ratio of TG-RRT^* and RRT^* is such that there exists a constant φ, i.e., $\lim_{j \to \infty} � [\frac{ℳ_{j}^{TGRRT}}{ℳ_{j}^{RRT}}] \leq ϕ$ .

The above result is based on the fact that our proposed algorithm executes all procedures in exactly the same manner as RRT^*, except for its inclusion of the TGSample (g) procedure. The TGSample (g) procedure employs simple incentre and centroid geometric techniques for guiding the random sample, and is executed exactly once per iteration. It should also be noted that the TGSample (g) function can be executed in a constant number of steps and is independent of the number of vertices present in the tree.

Figure 5.

IC-RRT^* Performance in a 2D Cluttered Environment (B)

Figure 6.

C-RRT^* Performance in a 2D Cluttered Environment (B)

Figure 7.

RRT^* Performance in a 2D Cluttered Environment (B)

Figure 8.

IC-RRT^*,C-RRT^* and RRT^* performance in a 2D maze

5. Experimental Results

This section presents simulations performed on a 2.4 GHz Intel Core i5 processor with 4 GB RAM. The performance results of all the algorithms (i.e., IC-RRT^*, C-RRT^* and RRT^*) are compared with each other. For concrete comparison of the algorithms, experimental parameters including γ and the size of the configuration space G ε ℝ^d were kept the same for all the algorithms. Moreover, due to the large variations in results, shown by the sampling-based algorithms, the algorithms were run up to 100 times for each path planning problem. The maximum, minimum and average number of iterations j, as well as the time t (in seconds) utilized by each algorithm to reach an optimal path solution, is presented in Table 1. The maximum average limit for the tree nodes was kept at five million in order to restrain the computational time within reasonable limits. The column “failure” in the table denotes the number of runs for which the corresponding algorithm failed to find an optimal path solution within the node limits. Although the algorithms are able to determine a near-optimal path solution, this is still considered to be a failure, since the table provides a comparison for the determination of an optimal path solution only. Finally, c^* and c represent the cost of optimal and non-optimal path solutions, respectively, as returned by the algorithm in terms of the distance function. As explained earlier, the proposed TG-RRT^* uses the hybrid sampling heuristic, i.e, it initially performs exploitation i.e., directed sampling using triangular geometry for the limited number of iterations κ ε ℝ₊. Once the iteration limit κ ε ℝ₊ is achieved, it switches from exploitation to exploration through uniform sampling. In Table 1, if the value of K is equal to or greater than the number of iterations j used by an algorithm to achieve an optimal path solution, this indicates that the optimal path has been admitted - before switching to uniform sampling - by the corresponding algorithm. Moreover, it has been noticed through experimentation that an appropriate value of κ for IC-RRT^* is lower than that for C-RRT^* due to the fact that IC-RRT^* directs the random samples much closer to the starting and goal regions as compared to C-RRT^*. This can also be deduced from the experimental results provided in this section.

Table 1.

Experimental results for computing an optimal/near-optimal path

Environment	Algorithm	κ	j _min	j _max	j _avg	t _min ^(s)	t _max ^(s)	t _avg ^(s)	c ^*	Failure
^*2D-Cluttered (A) (figure 3)	IC-RRT^*	10000	5872	7496	7217	051	0.69	0.64	38.6	0
	C-RRT^*	30000	21783	25283	22754	0.84	1.02	0.94	38.4	0
	RRT^*	-	3765432	4859741	4682961	98.2	191.2	102.4	38.6	12
^*2D-Cluttered (B) (figures 5, 6, and 7)	IC-RRT^*	10000	179	423	281	0.02	0.04	0.03	47.2	0
	C-RRT^*	30000	18941	21981	20269	0.72	0.87	0.79	47.2	0
	RRT^*	-	989715	1018451	1009726	35.29	35.91	35.51	47.2	7
^*2D-Narrow Passage (figure 4)	IC-RRT^*	10000	3219	4726	4521	0.32	0.43	0.41	36.0	0
	C-RRT^*	30000	2183	2573	2318	0.09	0.12	0.09	36.2	0
	RRT^*	-	4827917	4842731	4837241	165.6	184.1	171.8	36.7	16
^*2D-Maze (figure 8)	IC-RRT^*	250000	678162	687891	680426	53.3	56.1	54.6	163.9	3
	C-RRT^*	250000	289280	310217	301574	10.2	12.1	10.6	163.4	2
	RRT^*	-	4789284	5360921	5091482	165.7	184.4	178.6	163.9	21
^*3D-Narrow Passages (figures 9, 10, and 11)	IC-RRT^*	100000	54562	70926	60277	4.31	6.08	5.41	69.1	0
	C-RRT^*	100000	94986	101072	98196	3.51	4.31	3.84	69.6	0
	RRT^*	-	2000101	2010218	2001619	65.2	78.1	71.5	69.8	7
^*3D-Multiple Barriers (figures 13)	IC-RRT^*	30000	24197	34186	30112	2.16	3.12	2.71	80.2	0
	C-RRT^*	90000	139421	159680	151087	4.82	6.31	6.22	80.4	3
	RRT^*	-	1828742	1941794	1901268	77.9	101.6	97.7	80.4	11
^*3D-Maze (figures 16)	IC-RRT^*	30000	45981	51092	50107	3.91	4.71	4.34	227	3
	C-RRT^*	100000	118512	141081	127012	4.43	6.07	5.21	227	0
	RRT^*	-	811027	928102	871486	34.9	52.6	46.2	227	6

Figure 9.

IC-RRT^* in a 3D workspace with a sequence of narrow passages

Figure 10.

C-RRT^* in a 3D workspace with a sequence of narrow passages

Starting with 2D environments, Figure 3 shows the performance results of all three algorithms in a very simple cluttered environment. In this environment, Figure 3(a) shows that IC-RRT^* was able to determine the optimal path with the least amount of iterations (j =7316) followed by C-RRT^* (j =23964). In the case of the RRT^* algorithm, the optimal path solution still could not be found after nearly five million iterations. Consequently, the time required by RRT^* is the most (t = 238.7s), and it is noticeably more than the time requirements for IC-RRT^* (t =0.66s) and C-RRT^*(t =0.96s). Figure 4 shows the working of the three algorithms in a 2D environment containing a narrow passage to reach the goal configuration. In this environment, both TG-RRT^* algorithms quickly converge to an optimal path as compared to RRT^*, which was able to admit the near-optimal path after executing a very large number of iterations (j =4,839,241), as shown in Figure 4 (c). Moreover, in Figure 4 (c), half of the region is fully covered by a pink-coloured coating due to the large number of edges resulting from the substantial number of samples generated after a large number of iterations performed by the RRT^* algorithm.

Figure 11.

RRT^* in a 3D workspace with a sequence of narrow passages

Figures 5 to 7 represent the convergence from an initial path to an optimal path by IC-RRT^*, C-RRT^* and RRT^*, respectively, in another 2D cluttered environment. As evident from Figure 5(c), IC-RRT^* successfully determines an optimal path in surprisingly few iterations (j =293), followed by C-RRT^* (j =20,171) and RRT^* (j =1,002,918). While in this case RRT^* was successful in determining the optimal solution, the time taken to do so was nearly a 1,000 times that of IC-RRT^*.

Figure 8 shows a particularly challenging maze-type configuration space. The environment has been set up in such a way that the starting and goal regions, while placed close together, are separated by the length of a winding maze. The environment requires a path solution that traverses the length of a maze and stretches away from the goal. Figures 8(a) and 8(b) illustrate the performance of KIRRT^*, while Figures 8(c) and 8(d) show the implementation of C-RRT^*. For the determination of the optimal path, the C-RRT^* algorithm takes the least number of average iterations (j =289,268) as compared to IC-RRT^*(j =681,315) and the extraordinarily large number of iterations taken by RRT^* (j =4,982,732), as shown in Figure 8(f). C-RRT^* therefore also takes much less time compared to the other algorithms. The large number of iterations consequently requires the RRT^* algorithm to take nearly 15 times that of C-RRT^* and three times that of IC-RRT^* to determine approximately the same optimal path.

Figures 9 to 11 present the 3D environment. In this setup, the openings between the walls separating the initial and goal regions are particularly narrow. IC-RRT^* determines an optimal path most quickly, taking only j =59,837 iterations. C-RRT^* takes relatively more iterations (j =98,746), while RRT^* once again utilizes an extremely large number of iterations (j = 2,001,120), nearly 33 times that of IC-RRT^*. As evident from the figures, the sampling performed by IC-RRT^* and C-RRT^* is clearly more focused in the region between the initial and goal points, while the RRT^* algorithm performs uniform sampling, leading to a significant utilization of time.

Figure 12.

Comparing the memory, iterations and time requirements for the three algorithms

Figure 13.

Performance of IC-RRT^*, C-RRT^* and RRT^* in a 3D environment with multiple barriers

Figure 14.

Running-time ratio

An even more complicated environment is set up in Figure 13, where parts a, b and c represent implementations of IC-RRT^*, C-RRT^* and RRT^*, respectively. As was the case with the previous 3D environment, in this case, IC-RRT^* is able to converge to an optimal solution much more quickly (j =29617) than either of the other two algorithms, with (j =149,979) for C-RRT^* and (j =1,891,871) for RRT^*. The time taken by IC-RRT^* (t = 2.42s) is almost one-third that of C-RRT^* and almost 37 times less than the required time for the RRT^* algorithm in determining the optimal path through the multiple barriers between the initial and goal regions.

Figure 15.

Convergence rates of IC-RRT^*, C-RRT^* and RRT^*

Figure 16.

Performance of IC-RRT^*, C-RRT^* and RRT^* in a 3D complex maze

Since triangular geometry (incentre and centroid) brings the random sample to within the close vicinity of the starting and goal regions, one might therefore suspect its robustness in a maze like environment, where the start and goal configuration are very close to each other but the path between them traverses a complicated maze which stretches far away from the goal. Therefore, to provide further evidence of the effectiveness of adding triangular geometry to the RRT^* algorithm, yet another extensive maze has been set up, as shown in Figure 16. The initial and goal points in this environment are placed at a significant distance from each other, with only a single access route between the two points. In this setup, while the optimal path determined by all three algorithms was identical, there were once again significant variations in the number of iterations required. IC-RRT^* was the fastest (j =49,813), followed by C-RRT^* (j =121,687) and RRT^* (j =846,452). In this case, the time requirements for IC-RRT^* and C-RRT^* are very similar, at t =4.12 and t =4.91, respectively, while the time required by RRT^* to reach the same optimal path conclusion is significantly higher (t =55.2).

Figure 12 shows comparisons between the three algorithms in terms of (a) memory consumed in bytes, (b) iterations consumed, and (c) time utilized by these three algorithms to achieve an optimal/near optimal path in 10 different - 2D as well as 3D - environments. In all the environments, RRT^* consumes the most memory as well as requiring the largest numbers of iterations and the most time, while IC-RRT^* consumes significantly less of all three.

Figure 14 shows the running time ratio of IC-RRT^* over RRT^* and C-RRT^* over RRT^* after each iteration is executed. It can be seen that as the number of iterations increases, the running time ratio reaches a constant value. This ensures that the proposed TG-RRT^* provides for the faster determination of an optimal path (a solution to Problem 3) without any significant computational overhead.

Another type of comparison using a boxplot is presented in Figure 15, where the convergence rate of IC-RRT^*, C-RRT^* and RRT^* are compared in 50 different environments, both 2D and 3D. Let an initial feasible path, denoted by τ_init, be computed in t_init time while an optimal path solution, denoted by τ^*, is computed in t_opt time. Then, the convergence rate is defined as $\frac{c (τ_{init}) - c (τ^{*})}{t_{opt} - t_{init}}$ . Since the process of convergence to an optimal path solution begins after finding an initial feasible path solution, the convergence rate is calculated after initial path computation. It is clear from the figure that, on average, the convergence rates of IC-RRT^* are highest, followed by C-RRT^* and RRT^*. There exists a sizeable difference between the convergence rates of IC-RRT^* and RRT^*.

6. Conclusions and future work

RRT^* ensures asymptotic optimality without incurring significant computational overheads. However, its slow convergence rate leads it to require large amounts of memory in admitting an optimal path solution. TG-RRT^* addresses this problem and provides a solution by incorporating a triangular geometrized sampling heuristic into RRT^*. TG-RRT^* utilizes a hybrid sampling heuristic; it initially samples the region close to the initial state and the starting region using triangular geometric methods. Later, it switches to the uniform sampling method. This hybrid sampling method allows TG-RRT^* to compute optimal path solutions very quickly as compared to RRT^*. It has been proven both experimentally and analytically that our proposed TG-RRT^* algorithm has the same asymptotic computational complexity as that of RRT^*, and it also inherits probabilistic completeness and asymptotic optimality properties from RRT^*.

In our future work, we hope to utilise TG-RRT^* for online motion planning, since the proposed algorithm allows for faster convergence and determines the optimal path solution quickly. It can therefore serve as an efficient solution to real-time path planning problems, such as those applied in international robotics competitions like RoboCup.

Footnotes

7. Acknowledgements

This research was supported by Basic Science Research Program through the National Research Foundation of Korea (NRF) grant funded by the Ministry of Science, ICT & Future Planning (MSIP) (2014023744).

References

Liu

Badfer

N. I.

(2003) “Real-time reach planning for animated characters using hardware acceleration”, IEEE International Conference on Computer Animation and Social Agents, pp. 86–93.

Nof

S. Y.

(1999) “Handbook of industrial robotics”, John Wiley and Sons, vol. 1.

Taylor

R. H.

Stoianovici

(2003) “Medical robotics in computer-integrated surgery”, IEEE Transactions on Robotics and Automation, pp. 765–781.

Glasius

Komoda

Gielen

S. C.

(1995) “Neural network dynamics for path planning and obstacle avoidance”, Journal of Neural Networks, pp. 125–133.

Kavraki

L. E.

Latombe

J. C.

(1998) “Probabilistic roadmaps for robot path planning”.

Khatib

(1986) “Real-time obstacle avoidance for manipulators and mobile robots”, The international journal of robotics research, vol. 5, no. 1, pp. 90–98.

LaValle

S. M.

(1998) “Rapidly-Exploring Random Trees: A new tool for path planning”.

LaValle

S. M.

Kuffner

J. J.

(2000) “Rapidly-exploring random trees: Progress and prospects”.

Schwartz

J. T.

Sharir

(1983) “On the piano movers problem. II. General techniques for computing topological properties of real algebraic manifolds”, Advances in applied Mathematics, vol. 4, no. 3, pp. 298–351.

10.

Lozano-Pérez

Wesley

M. A.

(1979) “An algorithm for planning collision-free paths among polyhedral obstacles”, Communications of the ACM, vol. 22, no. 10, pp. 560–570.

11.

Canny

(1988) “The complexity of robot motion planning”, MIT press.

12.

Karaman

Frazzoli

(2011) “Sampling-based algorithms for optimal motion planning”, The International Journal of Robotics Research, vol. 30, no. 7, pp. 846–894.

13.

Rickert

Brock

Knoll

(2008) “Balancing exploration and exploitation in motion planning” IEEE International Conference on Robotics and Automation (ICRA), pp. 2812–2817.

14.

Brooks

R. A.

Lozano-Perez

(1982) “A subdivision algorithm in configuration space for findpath with rotation”.

15.

Koren

Borenstein

(1991) “Potential field methods and their inherent limitations for mobile robot navigation” IEEE International Conference on Robotics and Automation (ICRA), pp. 1398–1404.

16.

Kuffner

J. J.

LaValle

S. M.

(2000) “RRT-connect: An efficient approach to single-query path planning”, IEEE International Conference on Robotics and Automation (ICRA), pp. 995–1001.

17.

Yershova

Jaillet

Siméon

LaValle

S. M.

(2005) “Dynamic-domain RRTs: Efficient exploration by controlling the sampling domain”, IEEE International Conference on Robotics and Automation (ICRA), pp. 3856–3861.

18.

Karaman

Walter

M. R.

Perez

Frazzoli

Teller

(2011) “Anytime motion planning using the RRT*“, IEEE International Conference on Robotics and Automation (ICRA), pp. 1478–1483.

19.

Qureshi

A. H.

Mumtaz

Iqbal

K. F.

Ayaz

Muhammad

M. S.

Hasan

Kim

W. Y.

(2014) “Triangular geometry based optimal motion planning using RRT*-motion planner”, 13th IEEE International Workshop on Advanced Motion Control, pp. 380–385.

20.

Perez

Karaman

Shkolnik

Frazzoli

Teller

Walter

M.R.

(2011) “Asymptotically-optimal path planning for manipulation using incremental sampling-based algorithms”, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4307–4313.

21.

LaValle

S. M.

Kuffner

J. J.

(2001) “Randomized kinodynamic planning”, The International Journal of Robotics Research, vol. 20, no. 5, pp. 378–400.

Triangular Geometrized Sampling Heuristics for Fast Optimal Motion Planning

Abstract

Keywords

1. Introduction

2. Preliminaries

2.1 The RRT* Algorithm

2.2 Triangular Geometrical Methods

2.2.1 Circumcentre

2.2.2 Orthocentre

2.2.3 Incentre

2.2.4 Centroid

3. Triangular Geometrized RRT*

3.1 TGSample

3.1.1 Geometric Incentre

3.1.2 Geometric Centroid

4. Analysis

4.1 Probabilistic Completeness

4.2 Asymptotic Optimality

4.3 Computational Complexity

5. Experimental Results

6. Conclusions and future work

Footnotes

7. Acknowledgements

References

2.1 The RRT^* Algorithm

3. Triangular Geometrized RRT^*