Informed Sampling for Asymptotically Optimal Path Planning (Consolidated Version)

TL;DR

This paper introduces a direct informed sampling method for asymptotically optimal path planning that improves efficiency by focusing on promising regions, demonstrated with Informed RRT* on various problems.

Contribution

It formally defines informed sets for path planning, analyzes their effectiveness, and proposes a direct sampling technique that reduces the curse of dimensionality in RRT* extensions.

Findings

The method achieves faster convergence to optimal solutions.

It reduces the impact of high dimensionality on planning efficiency.

Experimental results show improved solution quality and speed.

Abstract

Anytime almost-surely asymptotically optimal planners, such as RRT*, incrementally find paths to every state in the search domain. This is inefficient once an initial solution is found as then only states that can provide a better solution need to be considered. Exact knowledge of these states requires solving the problem but can be approximated with heuristics. This paper formally defines these sets of states and demonstrates how they can be used to analyze arbitrary planning problems. It uses the well-known $L^2$ norm (i.e., Euclidean distance) to analyze minimum-path-length problems and shows that existing approaches decrease in effectiveness factorially (i.e., faster than exponentially) with state dimension. It presents a method to address this curse of dimensionality by directly sampling the prolate hyperspheroids (i.e., symmetric $n$-dimensional ellipses) that define the $L^2$ informed set. The importance of this direct informed sampling technique is demonstrated with Informed RRT*. This extension of RRT* has less theoretical dependence on state dimension and problem size than existing techniques and allows for linear convergence on some problems. It is shown experimentally to find better solutions faster than existing techniques on both abstract planning problems and HERB, a two-arm manipulation robot.

Equations

$${\sigma }^{\ast }=\underset{\sigma \in \Sigma }{\mathrm{argmin}}\left\{c\left(\sigma \right)|\sigma \left(0\right)={\mathbf{x}}_{\mathrm{start}},\sigma \left(1\right)\in X_{\mathrm{goal}}\right\},$$

$$P\left(\underset{q\to \infty }{\mathrm{limsup}}c\left({\sigma }_q\right)=c\left({\sigma }^{\ast }\right)\right)=1,$$

$$g\left(\mathbf{x}\right):=\underset{\sigma \in \Sigma }{\min }\left\{c\left(\sigma \right)|\sigma (0)={\mathbf{x}}_{\mathrm{start}},\sigma (1)=\mathbf{x}\right\},$$

$$h\left(\mathbf{x}\right):=\underset{\sigma \in \Sigma }{\min }\left\{c\left(\sigma \right)|\sigma (0)=\mathbf{x},\sigma (1)\in X_{\mathrm{goal}}\right\}.$$

$$X_f:=\left\{\mathbf{x}\in X_{\mathrm{free}}|f\left(\mathbf{x}\right)<c_i\right\}.$$

$$c_{i+1}<c_i⟹{\mathbf{x}}_{\mathrm{new}}\in X_f.$$

$${\mathbf{v}}_{\mathrm{nearest}}:=\underset{\mathbf{v}\in V}{\mathrm{argmin}}\left\{{\Vert {\mathbf{x}}_{\mathrm{rand}}-\mathbf{v}\Vert }_2\right\},$$

$${\mathbf{x}}_{\mathrm{new}}:=\underset{\mathbf{y}\in X}{\mathrm{argmin}}\left\{{\Vert {\mathbf{x}}_{\mathrm{rand}}-\mathbf{y}\Vert }_2|{\Vert \mathbf{y}-{\mathbf{v}}_{\mathrm{nearest}}\Vert }_2\le \eta \right\},$$

$$\forall i\ge \kappa ,c_{i+1}<c_i⟹{\mathbf{x}}_{\mathrm{rand}}\in X_f,$$

$$\forall i\ge \kappa ,P\left(c_{i+1}<c_i\right)\le P\left({\mathbf{x}}_{\mathrm{rand}}\in X_f\right),$$

$$X_{\hat{f}}:=\left\{\mathbf{x}\in X|\hat{f}\left(\mathbf{x}\right)<c_i\right\}.$$

$$\mathrm{Precision}\left(X_{\hat{f}}\right):=\frac{\lambda \left(X_{\hat{f}}\cap X_f\right)}{\lambda \left(X_{\hat{f}}\right)}.$$

$$\mathrm{Recall}\left(X_{\hat{f}}\right):=\frac{\lambda \left(X_{\hat{f}}\cap X_f\right)}{\lambda \left(X_f\right)}.$$

$$\forall \mathbf{x}\in X,\hat{f}\left(\mathbf{x}\right)\le f\left(\mathbf{x}\right).$$

$$c_{i+1}<c_i⟹{\mathbf{x}}_{\mathrm{new}}\in X_{\hat{f}}\supseteq X_f.$$

$$\forall i\ge \kappa ,c_{i+1}<c_i⟹{\mathbf{x}}_{\mathrm{rand}}\in X_{\hat{f}}\supseteq X_f,$$

$$\forall i\ge \kappa ,P\left(c_{i+1}<c_i\right)\le P\left({\mathbf{x}}_{\mathrm{rand}}\in X_f\right)\le P\left({\mathbf{x}}_{\mathrm{rand}}\in X_{\hat{f}}\right),$$

$$\hat{f}\left(\mathbf{x}\right)={\Vert \mathbf{x}-{\mathbf{x}}_{\mathrm{start}}\Vert }_2+{\Vert {\mathbf{x}}_{\mathrm{goal}}-\mathbf{x}\Vert }_2.$$

$$X_{\hat{f}}=\left\{\mathbf{x}\in X_{\mathrm{free}}|{\Vert \mathbf{x}-{\mathbf{x}}_{\mathrm{start}}\Vert }_2+{\Vert {\mathbf{x}}_{\mathrm{goal}}-\mathbf{x}\Vert }_2<c_i\right\}.$$

$$X_{\hat{f}}=X_{\mathrm{free}}\cap X_{\mathrm{PHS}},$$

$$X_{\mathrm{PHS}}:=\left\{\mathbf{x}\in {}^n|{\Vert \mathbf{x}-{\mathbf{x}}_{\mathrm{start}}\Vert }_2+{\Vert {\mathbf{x}}_{\mathrm{goal}}-\mathbf{x}\Vert }_2<c_i\right\}.$$

$$c_{\min }:={\Vert {\mathbf{x}}_{\mathrm{goal}}-{\mathbf{x}}_{\mathrm{start}}\Vert }_2,$$

$$\lambda \left(X_{\hat{f}}\right)\le \lambda \left(X_{\mathrm{PHS}}\right)=\frac{c_i{\left(c_i^2-c_{\min }^2\right)}^{\frac{n-1}{2}}{\zeta }_n}{2^n},$$

$${\zeta }_n:=\frac{{\pi }^{\frac{n}{2}}}{\Gamma \left(\frac{n}{2}+1\right)},$$

$$P\left({\mathbf{x}}_{\mathrm{rand}}\in X_{\hat{f}}|{\mathbf{x}}_{\mathrm{rand}}\sim \mathcal{U}\left(X_{\mathrm{samp}}\right)\right)\le \frac{\lambda \left(X_{\mathrm{PHS}}\right)}{\lambda \left(X_{\mathrm{samp}}\right)}$$

$$=\frac{{\pi }^{\frac{n}{2}}{c}_i{\left(c_i^2-c_{\min }^2\right)}^{\frac{n-1}{2}}}{2^n\Gamma \left(\frac{n}{2}+1\right)\lambda \left(X_{\mathrm{samp}}\right)},$$

$$\forall i\ge \kappa ,$$

$$\le \frac{{\pi }^{\frac{n}{2}}{c}_i{\left(c_i^2-c_{\min }^2\right)}^{\frac{n-1}{2}}}{2^n\Gamma \left(\frac{n}{2}+1\right)\lambda \left(X_{\mathrm{samp}}\right)}.$$

$$\forall i\ge \kappa ,P\left(c_{i+1}<c_i|{\mathbf{x}}_{\mathrm{rand}}\sim \mathcal{U}\left(X_{\mathrm{rect}}\right)\right)\le$$

$$\lambda \left(X_{\mathrm{rect}}\right)\ge c_i{\left(c_i^2-c_{\min }^2\right)}^{\frac{n-1}{2}}.$$

$$\forall i\ge \kappa ,P\left(c_{i+1}<c_i|{\mathbf{x}}_{\mathrm{rand}}\sim \mathcal{U}\left(X_{\mathrm{rect}}\right)\right)\le \frac{{\pi }^{\frac{n}{2}}}{2^n\Gamma \left(\frac{n}{2}+1\right)},$$

Full text

This paper formally defines these sets of states and demonstrates how they can be used to analyze arbitrary planning problems. It uses the well-known $L^{2}$ norm (i.e., Euclidean distance) to analyze minimum-path-length problems and shows that existing approaches decrease in effectiveness factorially (i.e., faster than exponentially) with state dimension. It presents a method to address this curse of dimensionality by directly sampling the prolate hyperspheroids (i.e., symmetric $n$-dimensional ellipses) that define the $L^{2}$ informed set.

The importance of this direct informed sampling technique is demonstrated with Informed RRT*. This extension of RRT* has less theoretical dependence on state dimension and problem size than existing techniques and allows for linear convergence on some problems. It is shown experimentally to find better solutions faster than existing techniques on both abstract planning problems and HERB, a two-arm manipulation robot.

I Introduction

There are many powerful path planning techniques in robotics. Popular approaches include graph-based searches, such as Dijkstra’s algorithm [1] and A* [2], and sampling-based methods, such as Probabilistic Roadmaps [3], Expansive Space Trees [4], and Rapidly-exploring Random Trees [5]. While sampling-based methods avoid the challenges of a priori discretizations, their stochastic nature limits their formal performance. They are said to be probabilistically complete if the probability of finding a solution, if one exists, approaches unity with an infinite number of samples. They are also said to be almost-surely asymptotically optimal if the probability of converging asymptotically to the optimum, if one exists, approaches unity with an infinite number of samples (e.g., asymptotically optimal (RRT*) [6]).

RRT searches a planning problem by incrementally building a tree through free space. RRT* extends this procedure to incrementally rewire the tree during its construction. This rewiring locally optimizes every vertex in the tree and allows the algorithm to almost-surely converge asymptotically to the optimal path to every state in the problem domain. This is an inefficient way to find the optimal solution to a single planning query.

The only states that need to be considered in single-query scenarios are those that can provide a better solution [7]. While exact knowledge of these states requires solving the planning problem, they can often be approximated with heuristics (Fig. 1). These heuristics have previously been used to focus almost-surely asymptotically optimal search [8, 9] but can also provide insight into the optimal planning problem.

This paper uses the set of states that can provide a better solution to analyze incremental almost-surely asymptotically optimal planning. It formally defines this shrinking set as the omniscient set and shows that sampling it is a necessary condition for RRT*-style planners to improve a solution. It defines estimates of this set as informed sets and provides metrics to quantify them in terms of their compactness (i.e., precision) and completeness (i.e., recall). It uses these results to bound the probability of improving a solution to a holonomic planning problem by the probability of sampling an informed set with $100\%$ recall.

The $L^{2}$ norm (i.e., Euclidean distance) is a well-known heuristic for problems seeking to minimize path length. It describes the omniscient set exactly in the absence of obstacles

and constraints (i.e., it is sharp111A bound or estimate is said to be sharp if it is exactly equal to the true value (i.e., has perfect precision and recall) in at least one case.) and always contains the omniscient set of a problem (i.e., it is universally admissible). This paper uses it to analyze the minimum-path-length problem and shows that existing focusing techniques (e.g., [8, 9]) are ineffective in high state dimensions. It is proven that these rejection-sampling approaches have a probability of improving a solution that goes to zero factorially (i.e., faster than exponentially) as state dimension increases.

This paper demonstrates how this minimum-path-length curse of dimensionality can be reduced by directly sampling the symmetric $n$-dimensional ellipse (i.e., prolate hyperspheroid), the $L^{2}$ informed set. The presented direct sampling approach always finds states that are believed to belong to a better solution regardless of the relative size of the $L^{2}$ informed set. It outperforms existing focusing techniques by orders of magnitude as state dimension increases.

The informed search approach is demonstrated with Informed RRT*. This extension of RRT* uses direct informed sampling and admissible graph pruning to focus the search for improvements. It is shown analytically to outperform existing techniques in terms of convergence rate, especially in high state dimensions, and to result in linear convergence on some problems. It is probabilistically complete and almost-surely asymptotically optimal. When the $L^{2}$ heuristic does not provide additional information (e.g., small planning problems and/or large informed sets) it is identical to RRT*. A version of Informed RRT* is publicly available in the Open Motion Planning Library (OMPL) [10].

Informed RRT* is evaluated experimentally on abstract problems and on the CMU Personal Robotic Lab’s Home Exploring Robot Butler (HERB) [11], a 14- degree-of-freedom (DOF) mobile manipulation platform. These experiments show that it outperforms existing focusing techniques as state dimension increases, especially in problems with large planning domains.

This paper is organized as follows. Section II defines omniscient and informed sets and their associated precision and recall in preparation for the literature review presented in Section III. Section IV presents a direct informed sampling technique for problems seeking to minimize path length which is demonstrated with Informed RRT* in Section V. Section VI analyzes the expected convergence rate of RRT* algorithms and Section VII demonstrates the practical advantages of this improvement on abstract and simulated problems. Section VIII finally presents a closing discussion and thoughts on future work.

I-A Statement of Contributions

This paper is a continuation of ideas first published in [12] and associated technical reports [13, 14] and makes the following specific contributions:

• •

Formally defines omniscient and informed sets (Definitions 3 and 7) and demonstrates how precision and recall can be used to quantify the performance of informed sampling (Definitions 8 and 9).

• •

Provides upper bounds on the probability that an incremental sampling-based planner improves a solution to a holonomic planning problem (Theorems 6 and 13).

• •

Bounds the expected next-iteration cost for RRT* algorithms on any minimum-path-length planning problem (Lemma 17) and shows that existing formulations of these algorithms for holonomic planning have a probability of improving a solution that decreases factorially with state dimension (Theorem 14).

• •

Develops a method to reduce this minimum-path-length curse of dimensionality by directly sampling the ellipsoidal $L^{2}$ informed set defined by a goal or countable set of goals and the current solution (Algs. 1–5).

• •

Proves that a planning algorithm using this approach, Informed RRT*, has better theoretical convergence (Theorems 18–20) and experimental performance than existing focused planning algorithms on holonomic problems.

II Omniscient and Informed Sets

A formal discussion of the optimal planning problem is presented in support of the literature review. It includes definitions of the states that can provide a better solution, the omniscient set (Definition 3), and estimates of this set, informed sets, quantified by precision and recall (Definitions 7–10). These sets provide theoretical upper bounds on the probability of improving a solution to a holonomic problem that are used throughout the remainder of the paper (Theorems 6 and 13).

Finding the optimal path from a start to a goal is formally defined as the optimal planning problem (Definition 1). The given definition is similar to [6].

Definition 1** (Optimal planning).**

Let $X\subseteq{}^{n}$ be the state space of the planning problem, $X_{\rm obs}\subset X$ be the states in collision with obstacles, and $X_{\rm free}=\mathrm{cl}\left(X\setminus X_{\rm obs}\right)$ be the resulting set of permissible states, where $\mathrm{cl}\left(\cdot\right)$ represents the closure of a set. Let $\mathbf{x}_{\rm start}\in X_{\rm free}$ be the initial state and $X_{\rm goal}\subset X_{\rm free}$ be the set of desired goal states. Let $\sigma:\;\left[0,1\right]\to X_{\rm free}$ be a sequence of states through collision-free space that can be executed by the robot (i.e., a collision-free feasible path) and $\Sigma$ be the set of all such nontrivial paths.

The optimal planning problem is then formally defined as the search for a path, $\sigma^{*}\in\Sigma$, that minimizes a given cost function, $c:\;\Sigma\to{}_{\geq 0}$, while connecting $\mathbf{x}_{\rm start}$ to $\mathbf{x}_{\rm goal}\in X_{\rm goal}$,

[TABLE]

where ≥0 is the set of non-negative real numbers.

Many sampling-based planners, such as RRT*, probabilistically converge towards the optimum of these problems. Such planners are described as probabilistically complete and almost-surely asymptotically optimal (Definition 2).

Definition 2** (Almost-sure asymptotic optimality).**

A planner is said to be almost-surely asymptotically optimal if, with an infinite number of samples, the probability of converging asymptotically to the optimum (Definition 1), if one exists, is one,

[TABLE]

where $q$ is the number of samples, $\sigma_{q}$ is the path found by the planner from those samples, $\sigma^{*}$ is the optimal solution to the planning problem, and $c\left(\cdot\right)$ is the cost of a path.

Once any solution is found, the set of states that can provide a better solution can be defined as the omniscient set (Definition 3).

Definition 3** (Omniscient set).**

Let $g\left(\mathbf{x}\right)$ be the cost of the optimal path from the start to a state, $\mathbf{x}\in X_{\rm free}$, the optimal cost-to-come,

[TABLE]

and $h\left(\mathbf{x}\right)$ be the cost of the optimal path from $\mathbf{x}$ to the goal region, the optimal cost-to-go,

[TABLE]

The cost of the optimal path from $\mathbf{x}_{\rm start}$ to $X_{\rm goal}$ constrained to pass through $\mathbf{x}$ is then given by $f\left(\mathbf{x}\right)\coloneqq g\left(\mathbf{x}\right)+h\left(\mathbf{x}\right)$. This defines the subset of states that can belong to a solution better than the current solution, $c_{i}$, as

[TABLE]

Exact knowledge of $X_{f}$ requires exact knowledge of the entire planning problem so we refer to it as the omniscient set.

RRT* builds a tree by incrementally adding states from the problem domain (Fig. 2). A necessary condition for it to improve a solution is that the newly added state belongs to the omniscient set (Lemma 4).

Lemma 4** (The necessity of adding states in the omniscient set).**

Adding a state from the omniscient set, $\mathbf{x}_{\rm new}\in X_{f}$, is a necessary condition for RRT* to improve the current solution, $c_{i}$,

[TABLE]

This condition is necessary but not sufficient to improve the solution as the ability of states in $X_{f}$ to provide better solutions at any iteration depends on the structure of the tree (i.e., its optimality).

Proof.

The proof of Lemma 4 from the supplementary online material appears in Appendix A-A. ∎

The state added by RRT* at each iteration, $\mathbf{x}_{\rm new}$, is generated from a randomly sampled state, $\mathbf{x}_{\rm rand}$, and the nearest vertex in the existing tree,

[TABLE]

through expansion and differential constraints (i.e., the $\mathtt{Steer}$ function). Absent any constraints (i.e., in holonomic planning) this takes the form

[TABLE]

where $\eta$ is a user-selected maximum edge length.

The number of tree vertices in the problem domain increases indefinitely with RRT* iterations. With an infinite number of iterations, eventually all reachable states will be no more than $\eta$ away from the nearest vertex in the tree. After these $\kappa$ iterations, sampling the omniscient set is a necessary condition to add a state from the omniscient set and improve the solution (Lemma 5).

Lemma 5** (The necessity of sampling states in the omniscient set in holonomic planning).**

Sampling the omniscient set, $\mathbf{x}_{\rm rand}\in X_{f}$, is a necessary condition for RRT* to improve the current solution to a holonomic problem, $c_{i}$, after an initial $\kappa$ iterations,

[TABLE]

for any sample distribution that maintains a nonzero probability over the entire omniscient set.

For simplicity, this statement is limited to holonomic planning but it can be extended to specific constraints with appropriate assumptions.

Proof.

The proof of Lemma 5 from the supplementary online material appears in Appendix A-B. ∎

This result provides an upper limit on the probability of RRT* improving a solution at any iteration (Theorem 6).

Theorem 6** (An upper bound on the probability of improving a solution to a holonomic planning problem given knowledge of the omniscient set).**

The probability that an iteration of RRT* improves the current solution to a holonomic problem, $c_{i}$, is bounded by the probability of sampling the omniscient set, $X_{f}$,

[TABLE]

for any iteration, $i$, after a sufficient vertex density is achieved in the initial $\kappa$ iterations.

For simplicity, this statement is limited to holonomic planning but it can be extended to specific constraints by expanding Lemma 5.

Proof.

Proof of Theorem 6 follows directly from Lemma 5. Sampling a state in $X_{f}$ is a necessary but not sufficient condition to improve the solution after $\kappa$ iterations; therefore, the probability of sampling such a state bounds the probability of improving the solution. ∎

Knowledge of an omniscient set requires solving the planning problem; however, these results can be extended to estimates of the omniscient set defined by solution cost heuristics (Definition 7).

Definition 7** (Informed set).**

Let $\widehat{f}\left(\mathbf{x}\right)$ represent a heuristic estimate of the solution cost constrained to go through a state, $\mathbf{x}\in X$. A heuristic estimate of the omniscient set can then be defined as

[TABLE]

Such a set will be referred to as an informed set.

There are an infinite number of potential informed sets for any planning problem and choosing the ‘best’ set requires methods to quantify their performance. In binary classification, estimates are evaluated in terms of their precision and recall (Fig. 3). Analogue terms can be defined in sampling-based planning to quantify the ability of informed sets to estimate the omniscient set (Definitions 8 and 9).

Definition 8** (Precision).**

The precision of an informed sampling technique is the probability that random samples drawn from the informed set could also be drawn from the omniscient set (e.g., the percentage of states drawn from the informed set, $X_{\widehat{f}}$, that belong to the omniscient set, $X_{f}$). For uniform sampling of an informed set, this is a ratio of measures,

[TABLE]

Any informed set with nonzero sampling probability that is a subset of the omniscient set will have $100\%$ precision.

Definition 9** (Recall).**

The recall of an informed sampling technique is the probability that random states drawn from the omniscient set could also be sampled from the informed set (e.g., the percentage of states that belong to the omniscient set, $X_{f}$, with a nonzero probability of being sampled from the informed set, $X_{\widehat{f}}$). For uniform sampling of an informed set, this is a ratio of measures,

[TABLE]

Any informed set with nonzero sampling probability that is a superset of the omniscient set will have $100\%$ recall.

Informed sets with $100\%$ recall (Definition 10) are important in almost-surely asymptotically optimal planning as less-than-perfect recall may exclude the optima to some problems.

Definition 10** (Admissible informed set).**

A heuristic is said to be admissible if it never overestimates the true value of the function,

[TABLE]

Any informed set defined by such an admissible heuristic will contain all possibly better solutions and have $100\%$ recall, i.e., $X_{\widehat{f}}\supseteq X_{f}.$

This set will be referred to as an admissible estimate of the omniscient set, or an admissible informed set. If the heuristic is an admissible estimate of the cost function for all possible problems then the set will be referred to as a universally admissible informed set.

These definitions allow the probability of improving a solution to a holonomic problem to be bounded by the probability of sampling any admissible informed set (Lemmas 11 and 12 and Theorem 13). The tightness of this bound will depend on the precision of the chosen estimate.

Lemma 11** (The necessity of adding states in an admissible informed set).**

Adding a state from an admissible informed set, $\mathbf{x}_{\rm new}\in X_{\widehat{f}}\supseteq X_{f}$, is a necessary condition for RRT* to improve the current solution, $c_{i}$,

[TABLE]

Proof.

Lemma 11 follows directly from Lemma 4 given that $X_{\widehat{f}}\supseteq X_{f}$. ∎

Lemma 12** (The necessity of sampling states in an admissible informed set in holonomic planning).**

Sampling an admissible informed set, $\mathbf{x}_{\rm rand}\in X_{\widehat{f}}\supseteq X_{f}$, is a necessary condition for RRT* to improve the current solution to a holonomic problem, $c_{i}$, after an initial $\kappa$ iterations,

[TABLE]

for any sample distribution that maintains a nonzero probability over the entire informed set.

For simplicity, this statement is limited to holonomic planning but it can be extended to specific constraints by expanding Lemma 5.

Proof.

Lemma 12 follows directly from Lemma 5 given that $X_{\widehat{f}}\supseteq X_{f}$. ∎

Theorem 13** (An upper bound on the probability of improving a solution to a holonomic planning problem given knowledge of an admissible informed set).**

The probability that an iteration of RRT* improves the current solution to a holonomic problem, $c_{i}$, is bounded by the probability of sampling an admissible informed set, $X_{\widehat{f}}\supseteq X_{f}$,

[TABLE]

for any iteration, $i$, after an initial $\kappa$ iterations.

For simplicity, this statement is limited to holonomic planning but it can be extended to specific constraints by expanding Lemma 5.

Proof.

Theorem 13 follows directly from Theorem 6 given that $X_{\widehat{f}}\supseteq X_{f}$. ∎

III Prior Work Accelerating RRT* Convergence

A review of previous work to improve the convergence rate of RRT* is presented using the results and terminology of Section II. All these techniques attempt to increase the real-time rate of searching the omniscient set by exploiting additional information. Most can be viewed as versions of sample biasing, sample rejection, and/or graph pruning (Sections III-A–III-D).

III-A Sample Biasing

Increasing the likelihood of sampling an informed set improves RRT* performance. This sample biasing creates a nonuniform sample distribution that will increase exploration of the informed set but invalidates the assumptions used to prove almost-sure asymptotic optimality. One method to maintain these formal performance guarantees is to calculate the random geometric graph (RGG) connection limit from a subset of samples that are uniformly distributed [16]. This maintains almost-sure asymptotic optimality but increases the required number of rewirings.

It is common to bias sampling around the current solution. This path biasing increases the likelihood of sampling a state that can improve the current solution but reduces the likelihood of finding solutions in other homotopy classes (i.e., it increases precision by decreasing recall; Fig 4a). The ratio of path biasing to global search is frequently a user-chosen parameter that must be tuned for each problem.

Akgun and Stilman [8] use path biasing in their dual-tree version of RRT*. Once an initial solution is found the algorithm spends a user-specified percentage of iterations refining the current solution. It does this by explicitly sampling near a randomly selected state on the current path. This increases the probability of improvement at the expense of decreasing the exploration of other homotopy classes. Their algorithm also employs sample rejection in exploring the state space (see Section III-B).

Nasir et al. [17] combine path biasing with smoothing in their RRT*-Smart algorithm. Solution paths are simplified and then used as biases for further sampling around the solution. Their path smoothing rapidly improves the current solution but the path biasing decreases the likelihood of finding a solution in a different homotopy class.

Kiesel et al. [18] use a two-stage sampling process in their f-biasing technique. Samples are generated by randomly selecting a region of the planning problem and then uniformly sampling it. The probability of selecting a region is calculated by solving a simple discretization of the planning problem with Dijkstra’s algorithm [1]. The regions along the discrete solution are given a higher selection probability but all regions maintain a nonzero probability to compensate for the incompleteness of the discretization. This technique provides a sampling bias for the entire RRT* search but once a solution is found it continues to sample states that cannot provide a better solution. It is stated that almost-sure asymptotic optimality is maintained but it is not discussed how to modify the rewiring neighbourhood to do so.

Kim et al. [19] also use a two-stage sampling process in their Cloud RRT* algorithm. They generate uniform samples from a series of collision-free, possibly overlapping, spheres defined by a generalized Voronoi graph [20]. New spheres are added on solution paths and the probability of selecting them is updated so that samples from the homotopy class of the solution are biased around the path while maintaining the probability of sampling other homotopy classes. Cloud RRT* successfully finds better solutions faster than other algorithms but continues to sample states that cannot improve the solution and its effect on almost-sure asymptotic optimality is not discussed.

Unlike sample biasing methods, the direct informed sampling used by Informed RRT* does not consider states that are known to be unable to improve a solution. It does result in a nonuniform sample distribution over the problem domain but it is still almost-surely asymptotically optimal as it has a uniform distribution in the informed set being searched.

III-B Sample Rejection

Ignoring samples outside an informed set improves RRT* performance. This sample rejection decreases the computational cost of states that cannot improve a solution but does not increase the probability of finding ones that can. If this probability is low (i.e., if the informed set is small relative to the sampling domain) then convergence will not be improved (Fig. 4b). It is shown that this probability decreases factorially with state dimension (i.e., faster than exponentially) in existing formulations of the holonomic minimum-path-length problem (Theorem 14).

Akgun and Stilman [8] use global rejection sampling in addition to sample biasing in their dual-tree algorithm. As samples are drawn from the entire problem domain, performance will decrease rapidly as the solution improves and/or in large or high-dimensional planning problems.

Otte and Correll [9] use adaptive rejection sampling in their parallelized Coupled Forest of Random Engrafting Search Trees (C-FOREST) algorithm. Samples are generated from a rectangular subset of the planning domain that bounds the ellipsoidal $L^{2}$ informed set and rejected using the $L^{2}$ heuristic. This increases sampling precision and improves performance in large planning problems but its effectiveness still decreases factorially with state dimension (Theorem 14).

Unlike sample rejection methods, the direct informed sampling used by Informed RRT* maintains high precision and $100\%$ recall regardless of the relative sizes of the informed set and problem domain. It focuses its search in response to solution improvements and does not decrease in effectiveness in large planning domains. It scales more effectively than existing approaches to high-dimensional planning problems

III-C Graph Pruning

Limiting the tree to an informed set improves RRT* performance. This graph pruning removes states that can no longer improve the existing solution and reduces the computational cost of basic operations (e.g., nearest neighbour searches). It can also be used reject potential new states given their connection and any constraints, e.g., (3). After a sufficient number of iterations, this incremental pruning is equivalent in holonomic planning to rejection sampling with the same heuristic (Lemma 12) but with the additional computational costs of expanding towards the sample.

Karaman et al. [21] use graph pruning to implement an online version of RRT* that improves solutions during path execution. They remove vertices whose current cost-to-come plus a heuristic estimate of cost-to-go is higher than the current solution. As current cost-to-come overestimates a vertex’s optimal cost-to-come (i.e., it is an inadmissible heuristic), this approach may erroneously remove vertices that could provide a better solution.

Arslan and Tsiotras [22, 23] combine incremental graph-pruning and incremental graph search techniques with Rapidly exploring Random Graphs [6] to reject samples in their RRT# algorithm. This incremental pruning focuses the search but its performance will also decrease rapidly as the solution improves or when used on large or high-dimensional planning problems. Some of the rejection criteria also use the current cost-to-come of vertices and may reject samples that could later improve the solution.

Unlike rejecting states with incremental graph pruning, the direct informed sampling used by Informed RRT* wastes no computational effort on states that are known to be unable to improve the solution. Its admissible graph pruning algorithm to remove unnecessary states also only removes vertices from the tree if doing so does not negatively affect the search.

III-D Other Techniques

Some techniques to improve RRT/RRT* performance do not fit neatly into the previous categories. Many of these methods could be further accelerated through direct informed sampling.

Urmson and Simmons [24] uses rejection sampling to create a “probabilistic implementation of heuristic search concepts” in their Heuristically Guided (hRRT). At each iteration, a uniformly distributed sample is probabilistically kept or rejected as a function of its heuristic value relative to the existing tree. This iteratively biases RRT expansion towards regions of the problem domain believed to contain high-quality solutions and often finds better solutions than RRT, especially on problems with continuous cost functions (e.g., path length [24]); however, it results in nonuniform sample distributions.

Ferguson and Stentz [7] recognize that an existing solution defines the set of states that could provide better solutions. Their Anytime RRTs algorithm attempts to incrementally find better solutions by searching a decreasing series of these ellipses. This shrinking search ignores some expensive solutions but does not guarantee better ones will be found.

Alterovitz et al. [25] add path refinement to RRT* in their Rapidly exploring Roadmap (RRM) algorithm. Once an initial solution is found, each iteration of RRM either samples a new state or selects an existing state from the current solution and refines it. Path refinement connects the selected state to its neighbours and results in a graph instead of a tree. The ratio of refinement to exploration is a user-tuned parameter.

Shan et al. [26] find an initial solution with RRT, simplify and rewire it using their RRT*_S algorithm, and then continue the search with RRT*. This can find better solutions faster than RRT* alone but the resulting search is not focused and continues to consider states that cannot provide better solutions.

Salzman and Halperin [27] relax performance to asymptotic near optimality in their Lower Bound Tree (LBT-RRT). Rewirings are only considered if they are required to maintain the desired tolerance to the optimum. This can reduce computational complexity but does not focus the search.

Devaurs et al. [28] use ideas from stochastic optimization to explore complex cost functions in their Transition-based (T-RRT*) and Anytime Transition-based (AT-RRT) algorithms. Transition tests accept or reject a potential new state depending on its cost relative to its parent. These tests help reduce the integral or mechanical work of the path in a cost space; however, for problems seeking to minimize path length are equivalent to graph pruning.

These algorithms, and those designed for more advanced purposes (e.g., RRTX [29]), can be improved with the direct informed sampling and admissible graph pruning techniques illustrated in Informed RRT*.

III-E Direct Informed Sampling for Path Length

This paper presents Informed RRT* as a demonstration of how direct sampling of $L^{2}$ informed sets increases the rate at which RRT* improves solutions for problems seeking to minimize path length. Unlike sample biasing, this approach considers all homotopy classes that could provide better solutions (i.e., $100\%$ recall) while maintaining uniform sample distribution over a subplanning problem. Unlike sample rejection or graph pruning, it is effective regardless of the relative size of the informed set or the state dimension (i.e., high precision). In situations where the heuristic does not provide substantial information (i.e., small planning problems and/or large informed sets), it performs identically to RRT*.

IV The $L^{2}$ Informed Set

A universally admissible heuristic is well defined for problems seeking to minimize path length in n and is commonly used in sampling-based planners (e.g., [7, 8, 9]). The cost of a solution constrained to pass through any state, $\mathbf{x}\in X$, is bounded from below by the $L^{2}$ norm (i.e., Euclidean distance) between it, the start, $\mathbf{x}_{\rm start}$, and the goal, $\mathbf{x}_{\rm goal}$,

[TABLE]

The set of states that could provide a better solution than the current solution cost, $c_{i}$, can then be referred to as the $L^{2}$ informed set,

[TABLE]

This informed set is a universally admissible estimate of the omniscient set and is exact in the absence of obstacles and constraints (i.e., it is sharp over all minimum-path-length problems). The size of this informed set will decrease as solutions improve.

The $L^{2}$ informed set is the intersection of the free space, $X_{\rm free}$, and a $n$-dimensional hyperellipsoid symmetric about its transverse axis (i.e., a prolate hyperspheroid),

[TABLE]

where

[TABLE]

The prolate hyperspheroid has focal points at $\mathbf{x}_{\rm start}$ and $\mathbf{x}_{\rm goal}$, a transverse diameter of $c_{i}$, and conjugate diameters of $\sqrt{c_{i}^{2}-c_{\rm min}^{2}}$, where

[TABLE]

is the theoretical minimum cost (Fig. 5). The Lebesgue measure of the informed set is

[TABLE]

where $\zeta_{n}$ is the Lebesgue measure of a $n$-dimensional unit ball,

[TABLE]

and $\Gamma\left(\cdot\right)$ is the gamma function, an extension of factorials to real numbers [30].

The probability of uniformly sampling this informed set by sampling any superset (e.g., a bounding box), $X_{\rm samp}\supseteq X_{\widehat{f}}$, can be written as a ratio of measures,

[TABLE]

which can be combined with Theorem 13 to bound the probability of improving a solution to a holonomic problem,

[TABLE]

This probability becomes arbitrarily small for

(i) costs, $c_{i}$, near the theoretical limit, $c_{\rm min}$,

(ii) large sampling domains, $\lambda\left(X_{\rm samp}\right)$, or

(iii) high state dimensions, $n$.

While the solution cost and sampling domain size may vary during the search of a problem, the state dimension is constant throughout. This motivates investigating the effect of state dimension on existing formulations of the holonomic minimum-path-length planning problem (Theorem 14).

Theorem 14** (The minimum-path-length curse of dimensionality).**

The probability that RRT* improves a solution to holonomic problems seeking to minimize path length decreases factorially (i.e., faster than exponentially) as state dimension increases,

[TABLE]

when uniformly sampling a (hyper)rectangle bounding the $L^{2}$ informed set, $X_{\rm rect}\supset X_{\rm PHS}\supseteq X_{\widehat{f}}\supseteq X_{f}$.

For simplicity, this statement is limited to holonomic planning but it can be extended to specific constraints by expanding Lemma 5.

Proof.

Theorem 14 is proven for RRT* but holds for any algorithm for which an equivalent to Theorem 13 exists.

The smallest possible $X_{\rm rect}$ that completely contains $X_{\rm PHS}$ is a (hyper)rectangle with widths corresponding to the diameters of the prolate hyperspheroid (Fig. 6a). The measure of any $X_{\rm rect}\supset X_{\rm PHS}$ is therefore bounded from below as

[TABLE]

When substituted into (IV) this gives

[TABLE]

proving Theorem 14 for all rectangular sets, $X_{\rm rect}$, such that $X_{\rm rect}\supset X_{\rm PHS}\supseteq X_{\widehat{f}}\supseteq X_{f}$. ∎

Theorem 14 is an upper bound on the utility of rectangular rejection sampling in holonomic planning and is illustrated by plotting (9) versus state dimension (Fig. 6b). The results show that while rectangular rejection sampling may be $79\%$ successful in 2, its success decreases factorially as state dimension increases and is only $2\%$ in 8 and $4\times 10^{-4}\%$ in 16. These numbers represent the best-case for rectangular rejection sampling and actual performance will depend on the size and orientation of the informed set relative to the sampling domain. This motivates a need for a direct method to sample the prolate hyperspheroid regardless of size, orientation, and state dimension.

IV-A Direct Sampling

A direct method to generate uniformly distributed samples in the $L^{2}$ informed set is adapted from techniques to sample hyperellipsoids [31].

Let $\mathbf{S}\in{}^{n\times n}$ be a symmetric, positive-definite matrix (the hyperellipsoid matrix) such that the interior of a hyperellipsoid, $X_{\rm ellipse}$, is defined as

[TABLE]

where $\mathbf{x}_{\rm centre}$ is the centre point of the hyperellipsoid. Uniformly distributed samples in the hyperellipsoid, $\mathbf{x}_{\rm ellipse}\sim\mathcal{U}\left(X_{\rm ellipse}\right)$, can be generated from uniformly distributed samples in the interior of a unit $n$-dimensional ball, $\mathbf{x}_{\rm ball}\sim\mathcal{U}\left(X_{\rm ball}\right)$, by

[TABLE]

where $\mathbf{L}\in{}^{n\times n}$ is the lower-triangular Cholesky decomposition of the hyperellipsoid matrix such that

[TABLE]

and

[TABLE]

For hyperellipsoids with orthogonal axes, there exists a coordinate frame in which the hyperellipsoid matrix is diagonal,

[TABLE]

where $r_{j}$ is the radius of $j$-th axis of the hyperellipsoid and $\operatorname*{diag}\left(\cdot\right)$ constructs a diagonal matrix. A rotation from this hyperellipsoid-aligned frame to the world frame, $\mathbf{C}_{\rm we}\in SO\left(n\right)$, can be used to write (11) in terms of $\mathbf{S}^{\prime}$ as

[TABLE]

given the orthogonality of rotation matrices, $\mathbf{C}_{\rm we}^{-1}\equiv\mathbf{C}_{\rm we}^{T}$, and that $\mathbf{L}^{\prime}\mathbf{L}^{\prime T}\equiv\mathbf{S}^{\prime}$.

The rotation between frames can be solved directly as a general Wahba problem [32] even when underspecified [33]. Generally, the rotation matrix from one set of axes, $\left\{\mathbf{a}_{j}\right\}$, to another set of axes, $\left\{\mathbf{b}_{j}\right\}$, is given by

[TABLE]

where $\boldsymbol{\Lambda}\in{}^{n\times n}$ is

[TABLE]

and $\det\left(\cdot\right)$ is the matrix determinant. The terms $\mathbf{U}\in{}^{n\times n}$ and $\mathbf{V}\in{}^{n\times n}$ are unitary matrices such that $\mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^{T}\equiv\mathbf{M}$ via singular value decomposition and $\mathbf{M}\in{}^{n\times n}$ is given by the outer product of the $j\leq n$ corresponding axes,

[TABLE]

In problems seeking to minimize path length, the hyperellipsoid is a prolate hyperspheroid described by

[TABLE]

Its local coordinate system is underspecified in the conjugate directions due to symmetry, making (15) just

[TABLE]

where $\mathbf{1}_{1}$ the first column of the identity matrix and the transverse axis in the world frame is

[TABLE]

Samples distributed uniformly in the $L^{2}$ informed set, $X_{\widehat{f}}=X_{\rm PHS}\cap X_{\rm free}$, can therefore be generated by using (IV-A) to transform samples drawn uniformly from a unit $n$-ball. These samples are mapped to the prolate hyperspheroid through scaling, (17), rotation, (14) and (18), and translation, (16).

Sun and Farooq [31] investigate various methods to generate samples in hyperellipsoids and provide the following lemma regarding the uniform sample density of this technique.

Lemma 15** (The uniform distribution of samples transformed into a hyperellipsoid from a unit $n$-ball. Originally Lemma 1 in [31]).**

If the random points distributed in a hyperellipsoid are generated from the random points uniformly distributed in a hypersphere through a linear invertible nonorthogonal transformation, then the random points distributed in the hyperellipsoid are also uniformly distributed.

Proof.

For brevity, [31] only presents anecdotal proofs of Lemma 15. The full proof from the supplementary online material appears in Appendix B. ∎

IV-A1 Algorithm

The $L^{2}$ informed set is an arbitrary intersection of the prolate hyperspheroid and the problem domain. It can be sampled efficiently by considering the relative measure of the two sets and sampling the smaller set until a sample belonging to both sets is found. These procedures are presented algorithmically in Algs. 1 and 2 and are publicly available in OMPL. Note that for most problems Alg. 2, Lines 2–2 are constant and only need to be calculated once.

The function $\mathtt{SVD}\left(\cdot\right)$ denotes the singular value decomposition of a matrix and $\mathtt{SampleUnitBall}\left(n\right)$ returns uniformly distributed samples from the interior of an $n$-dimensional unit ball. The measure of the prolate hyperspheroid, $\lambda\left(X_{\rm PHS}\right)$, is given by (5) and SampleProblem returns samples uniformly distributed over the entire planning domain. Implementations of SVD and SampleUnitBall can be found in common C++ libraries.

IV-A2 Practical Performance

Direct informed sampling (Alg. 1) is compared to the best-case performance of rectangular rejection sampling. The average computational time required to find a sample in the $L^{2}$ informed set is calculated by generating $10^{6}$ samples at each dimension (Fig. 6c). The results show that while rejection sampling may outperform direct informed sampling in low state dimensions (e.g., 2: $7.3\times 10^{-8}$ vs. $3.5\times 10^{-7}$ seconds), it becomes orders of magnitude slower as state dimension increases (e.g., 16: $4.0\times 10^{-2}$ vs. $7.2\times 10^{-7}$ seconds). These per-sample times are small but significant. Generating $10^{5}$ samples in 16 requires less than a second with direct informed sampling ($7.2\times 10^{-2}$ seconds) but over an hour with rectangular rejection sampling ($3953$ seconds).

This experiment represents optimistic results for both constant (e.g., the problem domain) and adaptive (e.g., [9]) rectangular rejection sampling. Constant sampling domains rarely provide tight bounds on the informed set and will generally have higher rejection rates than the experiment. Adaptive sampling domains may tightly bound the informed set but must account for its alignment relative to the state space. This requires either a larger rectangular sampling domain or a rotation between frames that increases the rejection rate or computational cost compared to the experiment, respectively.

IV-B Extension to Multiple Goals

Many planning problems seek the minimum-length path that connects a start to any state in a goal region, $X_{\rm goal}$. In these situations the omniscient set is all states that could provide a better solution to any goal. The multigoal $L^{2}$ informed set is

[TABLE]

For a countable goal region, $X_{\rm goal}\coloneqq\left\{\mathbf{x}_{{\rm goal},j}\right\}_{j=1}^{z}$, this set is the union of the individual informed sets of each goal,

[TABLE]

where $z$ is the number of goals and

[TABLE]

is the $L^{2}$ informed set of an individual $\left(\mathbf{x}_{\rm start},\mathbf{x}_{{\rm goal},j}\right)$ pair. If the individual informed sets do not intersect, then a uniform sample distribution can be generated by randomly selecting an individual subset, $j$, in proportion to its relative measure,

[TABLE]

and then generating a uniformly distributed sample inside the selected subset, $X_{\widehat{f},j}$.

If individual sets do intersect, then this approach will oversample states that belong to multiple sets (Fig. 7a). In these situations, uniform sample density can be maintained by probabilistically rejecting samples in proportion to their membership in individual sets. This creates a uniform sample distribution for multigoal $L^{2}$ informed sets defined by arbitrarily overlapping individual informed sets (Fig. 7b).

IV-B1 Algorithm

The algorithm is described in Algs. 3–5 as modifications to the sampling technique for a single-goal $L^{2}$ informed set, with changes highlighted in red (cf. Alg. 1). The measure of individual informed sets, $\lambda\left(X_{{\rm PHS},j}\right)$, is calculated from (5) using the appropriate goal, $\mathbf{x}_{{\rm goal},j}$. This same technique can also be applied to problems with a countable start region.

V Informed RRT*

Informed RRT* is an extension of RRT* that demonstrates how informed sets can be used to improve anytime almost-surely asymptotically optimal planning. It performs the same as RRT* until a solution is found after which the search is focused to the informed set through direct informed sampling and admissible graph pruning (Fig. 8). This increases the likelihood of sampling states that can improve the solution and increases the convergence rate towards the optimum regardless of the relative size of the informed set (e.g., near-minimum solutions or large problem domains) or the state dimension.

Informed RRT* uses direct informed sampling (Alg. 3), admissible graph pruning (Section V-B), and an updated calculation of the rewiring neighbourhood (Section V-C) to focus the search. The complete algorithm is presented in Algs. 6 and 7 as modifications to RRT*, with changes highlighted in red. It can also be integrated into other sampling-based planners, such as RRTX [29] and Batch Informed Trees (BIT*) [34, 35, 36].

At each iteration, Informed RRT* calculates the current best solution (Alg. 6, Line 6) from the vertices in the goal region (Alg. 6, Lines 6, 6–6). This defines a shrinking $L^{2}$ informed set that is used to both focus sampling (Alg. 6, Line 6; Alg. 3) and prune the graph (Alg. 6, Line 6; Alg. 7). This process continues for as long as time allows or until a suitable solution is found.

Informed RRT* retains the probabilistic completeness and almost-sure asymptotically optimality of RRT*. It is probabilistically complete since it does not modify the search for an initial solution. It is almost-surely asymptotically optimal as it maintains a uniform sample distribution over a subset of the planning problem in which it uses a local rewiring neighbourhood that satisfies the bounds presented in [6].

V-A Notation

The tree, $\mathcal{T}\coloneqq\left(V,E\right)$, is defined by a set of vertices, $V\subset X_{\rm free}$, and edges, $E=\left\{\left(\mathbf{v},\mathbf{w}\right)\right\}$, for some $\mathbf{v},\,\mathbf{w}\in V$. The function $g_{\mathcal{T}}\left(\mathbf{v}\right)$ represents the cost to reach a vertex, $\mathbf{v}\in V$, from the start given the current tree (the cost-to-come). The function $c\left(\mathbf{v},\mathbf{w}\right)$ represents the cost of a path connecting the states $\mathbf{v},\mathbf{w}\in X_{\rm free}$, and corresponds to the edge cost between those two states if they are connected as vertices in the tree. The notation $X\xleftarrow{\scriptscriptstyle+}\left\{\mathbf{x}\right\}$ and $X\xleftarrow{\scriptscriptstyle-}\left\{\mathbf{x}\right\}$ is used to compactly represent the compounding set operations $X\leftarrow X\cup\left\{\mathbf{x}\right\}$ and $X\leftarrow X\setminus\left\{\mathbf{x}\right\}$, respectively. As is customary, the minimum of an empty set is taken to be infinity and a prolate hyperspheroid defined by an infinite transverse diameter is taken to have infinite measure.

V-B Graph Pruning (Alg. 7)

Graph pruning simplifies a tree by removing unnecessary vertices. Vertices are often removed if their heuristic values are larger than the current solution (i.e., they do not belong to informed set). While this identifies vertices that cannot provide a better solution, it is not a sufficient condition to remove them without negatively affecting the search. Their descendants may still be capable of providing better solutions (i.e., they may belong to the informed set; Fig. 9) in which case their removal would negatively affect performance by decreasing vertex density in the search domain (i.e., the informed set; Fig. 10b).

An admissible pruning method that does not remove vertices from the informed set is presented in Alg. 7. It iteratively removes leaves of the tree that cannot provide a better solution until no such leaves exist. This only removes vertices if they and all their descendants cannot belong to a better solution (i.e., it only removes vertices from outside the informed set; Fig. 9). This retains all possibly beneficial vertices regardless of their current connections and does not alter the vertex distribution in areas being searched (Fig. 10c).

V-C The Rewiring Neighbourhood

RRT* almost-surely converges asymptotically to the optimum by incrementally rewiring the tree around new states. In the $r$-disc variant this is the set of states within a radius, $r_{\mathrm{rewire}}$, of the new state,

[TABLE]

where $\eta$ is the maximum allowable edge length of the tree and $r_{\mathrm{RRT}^{*}}$ is a function of the problem measure and the number of vertices in the tree [6]. Specifically,

[TABLE]

where $\lambda\left(\cdot\right)$ is the Lebesgue measure of a set (e.g., the volume), $\zeta_{n}$ is the Lebesgue measure of an $n$-dimensional unit ball, i.e., (6), and $\left|\cdot\right|$ is the cardinality of a set.

The rewiring neighbourhood in the $k$-nearest variant is the $k_{\mathrm{RRT}^{*}}$-closest states to the new state, where

[TABLE]

Informed RRT* searches a subset of the original planning problem. The rewiring requirements to maintain almost-sure asymptotic optimality in this shrinking domain will be a function of the number of vertices in the informed set, $\left|V\cap X_{\widehat{f}}\right|$, and its measure, $\lambda\left(X_{\widehat{f}}\right)$. The $L^{2}$ informed set is not known in closed form (it is an intersection of a prolate hyperspheroid and free space) but its measure can be bounded from above by the minimum measure of the prolate hyperspheroid and the problem domain,

[TABLE]

This updates (V-C) and (V-C) to

[TABLE]

and

[TABLE]

where $\lambda\left(X_{\rm PHS}\right)$ is a function of the current solution, i.e., (5).

These rewiring neighbourhoods will be smaller than (V-C) and (V-C) when they can contain fewer vertices (i.e., only those in the informed set) and/or a smaller problem measure (i.e., the measure of the informed set). Smaller rewiring neighbourhoods reduce the computational cost of rewiring at each iteration and improves the real-time performance of Informed RRT* while maintaining almost-sure asymptotic optimality.

VI Rates of Convergence

Almost-sure asymptotic optimality provides no insight into the rate at which solutions are improved. Previous work has found probabilistic rates for asymptotically optimal (PRM*) [37] and Fast Marching Tree (FMT*) [16] and estimated the expected length of RRT* solutions as a function of computational time [37].

Performance can be quantified analytically by evaluating the rate at which the sequence of solution costs converges to the optimum. This rate can be classified as sublinear, linear, or superlinear (Definition 16).

Definition 16** (Rate of convergence).**

A sequence of numbers, $\left(a_{i}\right)_{i=1}^{\infty}$, that monotonically and asymptotically approaches a limit, $a_{\infty}$, has a rate of convergence given by

[TABLE]

The sequence is said to converge linearly if the rate is in the range $0<\mu<1$, superlinearly (i.e., faster than linear) when $\mu=0$, and sublinearly (i.e., slower than linear) when $\mu=1$.

The expected convergence rate of an algorithm depends on its tuning and the planning problem. General rates can be calculated for holonomic minimum-path-length problems for RRT* with and without sample rejection and Informed RRT* (Theorems 18–20) by first calculating sharp bounds on the expected next-iteration cost (Lemma 17).

Lemma 17** (Expected next-iteration cost of minimum-path-length planning).**

The expected value of the next solution to a minimum-path-length problem, $E\left[c_{i+1}\right]$, is bounded by

[TABLE]

where $c_{i}$ is the current solution cost, $c_{\rm min}$ is the theoretical minimum solution cost, $n$ is the state dimension of the planning problem, and $p_{f}=P\left(\mathbf{x}_{\rm new}\in X_{f}\right)$ is the probability of adding a state that is a member of the omniscient set (i.e., that can belong to a better solution). While not explicitly shown, the subset, $X_{f}$, and the probability of improving the solution, $p_{f}$, are generally functions of the current solution cost.

This lower bound is sharp over the set of all possible minimum-path-length planning problems and algorithm configurations and is exact for versions of RRT* with an infinite rewiring radius (i.e., $\eta=\infty$, and $r_{\mathrm{RRT}^{*}}=\infty$) searching an obstacle-free environment without constraints.

Proof.

The proof of Lemma 17 from the supplementary online material appears in Appendix C. ∎

This result allows sharp bounds on the convergence rates of RRT* (with and without rejection sampling) and Informed RRT* to be calculated for any configuration or holonomic minimum-path-length planning problem. These bounds will be exact in problems without obstacles and constraints and with an infinite rewiring neighbourhood (i.e., $\eta=\infty$, and $r_{\mathrm{RRT}^{*}}=\infty$) and show that RRT* always has sublinear convergence to the optimum (Theorem 18).

Theorem 18** (Sublinear convergence of RRT* in holonomic minimum-path-length planning).**

RRT** converges sublinearly towards the optimum of holonomic minimum-path-length planning problems,*

[TABLE]

For simplicity, this statement is limited to holonomic planning but it can be extended to specific constraints by expanding Lemma 5.

Proof.

The proof of Theorem 18 follows directly from Lemma 17 when $p_{f}$ is given by (IV) and appears from the supplementary online material in Appendix D-A. ∎

Rectangular rejection sampling improves the convergence rate of RRT*. This improvement is maximized by sampling a rectangle that tightly bounds the informed set (Fig. 6a). The resulting adaptive rectangular rejection sampling (e.g., [9]) allows RRT* to converge linearly in the absence of obstacles and constraints and with an infinite rewiring neighbourhood (Theorem 19).

Theorem 19** (Linear convergence of RRT* with adaptive rectangular rejection sampling in holonomic minimum-path-length planning).**

RRT** with adaptive rectangular rejection sampling converges at best linearly towards the optimum of holonomic minimum-path-length planning problems but factorially approaches sublinear convergence with increasing state dimension,*

[TABLE]

For simplicity, this statement is limited to holonomic planning but it can be extended to specific constraints by expanding Lemma 5.

Proof.

The proof of Theorem 19 follows directly from Lemma 17 when $p_{f}$ is calculated by substituting (10) in (IV) and appears from the supplementary online material in Appendix D-B. ∎

This convergence rate diminishes factorially (i.e., quickly) as state dimension increases due to the minimum-path-length curse of dimensionality. Informed RRT* avoids this limitation with direct informed sampling. It also converges linearly in the absence of obstacles and constraints and with an infinite rewiring neighbourhood but has a weaker dependence on state dimension (Theorem 20).

Theorem 20** (Linear convergence of Informed RRT* in holonomic minimum-path-length planning).**

Informed RRT* converges at best linearly towards the optimum of holonomic minimum-path-length planning problems,

[TABLE]

where the lower-bound occurs exactly with an infinite rewiring neighbourhood in the absence of obstacles and constraints.

For simplicity, this statement is limited to holonomic planning but it can be extended to specific constraints by expanding Lemma 5.

Proof.

The proof of Theorem 20 follows directly from Lemma 17 when $p_{f}=1$ and appears from the supplementary online material in Appendix D-C. ∎

Theorems 18–20 result in the following corollary regarding the relative convergence rates of the algorithms.

Corollary 21** (The faster convergence of Informed RRT* in holonomic minimum-path-length planning).**

The best-case convergence rate of Informed RRT*, $\mu_{\mathrm{Inf}}^{*}$, is always better than that of RRT*, with or without rejection sampling in holonomic minimum-path-length planning,

[TABLE]

For simplicity, this statement is limited to holonomic planning but it can be extended to specific constraints by expanding Lemma 5.

Proof.

The proof follows immediately from the lower bounds in (25), (26), and (27). It is illustrated in Fig. 11. ∎

VI-A Experimental Validation and Extension

Convergence rates are investigated experimentally for infinite, constant finite, and decreasing finite rewiring radii. To isolate the effects of the rewiring parameters, Informed RRT* was run on obstacle- and constraint-free problems in 2, 4, and 8 for $10^{4}$ trials of each configuration. Each trial started from the same initial solution but used different pseudo-random seeds to search for improvements. The logarithmic error relative to the known optimum, $\log\left(c_{i}-c^{*}\right)$, and the resulting mean were calculated at each iteration of each trial and used to validate Theorem 20 and illustrate the effects of rewiring parameters on the convergence rate.

The experimental results for an infinite rewiring neighbourhood (i.e., $\eta=\infty$ and $r_{\mathrm{RRT}^{*}}=\infty$) show excellent agreement with the theoretical predictions in Theorem 20 (Figs. 12a–c). The mean solution cost converges linearly towards the optimum and closely matches the lower-bound predicted by Lemma 17.

The experimental results for a constant finite rewiring neighbourhood (i.e., $\eta=0.4$ and $r_{\mathrm{RRT}^{*}}=\infty$) show that the convergence rate is lower than predicted by Theorem 20 (Figs. 12d–f). The convergence rate appears to be initially nonlinear but then become linear. It is hypothesized that this is related to the density of samples relative to the maximum edge length as reflected by $\kappa$ in Theorem 6.

The experimental results for a decreasing finite rewiring neighbourhood (i.e., $\eta=\infty$ and $r_{\mathrm{RRT}^{*}}=1.1r_{\mathrm{RRT}^{*}}^{*}$) show that the convergence rate appears to be sublinear (Figs. 12g–i). It is hypothesized that this is a result of the rewiring neighbourhood shrinking ‘too’ fast relative to the sample density.

These experiments suggest that further research is necessary to study the tradeoff between per-iteration cost and the number of iterations needed to find a solution. While a shrinking rewiring neighbourhood limits the number of rewirings, the apparent resulting sublinear convergence would require significantly more iterations to find high-quality solutions. Alternatively, while linear convergence needs fewer iterations to find equivalent solutions, the required constant radius would allow the number of rewirings to increase indefinitely.

VII Experiments

Informed RRT* was evaluated on simulated problems in 2, 4, and 8 (Sections VII-A and VII-B) and for HERB (Section VII-C) using OMPL222The experiments were run on a laptop with $16$ GB of RAM and an Intel i7-4810MQ processor. The abstract experiments were run in Ubuntu 12.04 ($64$-bit) with Boost 1.58, while the HERB experiments were run in Ubuntu 14.04 ($64$-bit)..

It was compared to the original RRT* and versions that focus the search with graph pruning (e.g., Alg. 7), heuristic rejection on $\mathbf{x}_{\rm new}$, heuristic rejection on $\mathbf{x}_{\rm rand}$, and all three techniques combined.

All planners used the same tuning parameters and the ordered rewiring technique presented in [38]. Planners used a goal-sampling bias of $5\%$ and an RRT* radius of $r_{\mathrm{RRT}^{*}}=2r_{\mathrm{RRT}^{*}}^{*}$. The maximum edge length was selected experimentally to reduce the time required to find an initial solution on a training problem, with values of $\eta=0.3$, $0.5$, $0.9$, and $1.3$ used in 2, 4, 8, and on HERB (14), respectively. Available planning time was limited for each state dimension to $3$, $30$, $150$, and $600$ seconds, respectively. Planners with heuristics used the $L^{2}$ norm as estimates of cost-to-come and cost-to-go while those with graph pruning delayed its application until solution cost changed by more than $5\%$.

These experiments were designed to investigate admissible methods to focus search. More advanced extensions of RRT* were not considered as they commonly include some combination of the investigated techniques.

VII-A Toy Problems

Two separate experiments were run in 2, 4, and 8 on randomized variants of the toy problem depicted in Fig. 13a to investigate the effects of obstacles on convergence.

The problem consists of a (hyper)cube of width $l$ with a single start and goal located at $\left[-0.5,0,\ldots,0\right]^{T}$ and $\left[0.5,0,\ldots,0\right]^{T}$, respectively. A single (hyper)cube obstacle of width $w\sim\mathcal{U}\left[0.25,0.5\right]$ sits between the start and goal in the centre of the problem domain.

The first experiment investigates finding near-optimal solutions in the presence of obstacles. The time required for each planner to find a solution within various fractions of the known optimum, $c^{*}$, was recorded over $100$ trials with different pseudo-random seeds for maps of width $l=2$. The percentage of trials that found a solution within the target tolerance of the optimum and the median time necessary to do so are presented for each planner in Figs. 14a–c. Trials that did not find a suitable solution were treated as having infinite time for the purpose of calculating the median. The results show that Informed RRT* performs equivalently to rejection sampling algorithms in low state dimensions but outperforms all existing techniques in higher dimensions.

The second experiment investigates finding near-optimal solutions in large planning problems. The time required for each planner to find a near-optimal solution was recorded over $100$ trials with different pseudo-random seeds for maps of increasing width, $l$. Planners sought a solution better than $1.01c^{*}$, $1.05c^{*}$, and $1.15c^{*}$ in 2, 4, and 8, respectively. The percentage of trials that found a sufficiently near-optimal solution and the median time necessary to do so are presented for each planner in Figs. 14d–f. Trials that did not find a suitable solution were treated as having infinite time for the purpose of calculating the median. The results show that Informed RRT* outperforms all existing techniques in large-domain planning problems and that the difference increases in higher state dimensions.

These experiments show that increasing problem size and state dimension decreases the ability of nondirect sampling methods to find near-optimal solutions, as predicted by (IV). Informed RRT* limits these effects and outperforms existing techniques by efficiently focusing its search to the $L^{2}$ informed set using direct informed sampling.

VII-B Worlds with Many Homotopy Classes

The algorithms were tested on more complicated problems with many homotopy classes in 2, 4, and 8. The worlds consisted of a (hyper)cube of width $l=4$ with the start and goal located at $\left[-0.5,0,\ldots,0\right]^{T}$ and $\left[0.5,0,\ldots,0\right]^{T}$, respectively. The problem domain was filled with a regular pattern of axis-aligned (hyper)cube obstacles with a width such that the start and goal were $5$ ‘columns’ apart (Fig. 13b).

The planners were tested with $100$ different pseudo-random seeds on each world and state dimension. The solution cost of each planner was recorded every $1$ millisecond by a separate thread and the median was calculated from the $100$ trials by interpolating each trial at a period of $1$ millisecond. The absence of a solution was considered an infinite cost for the purpose of calculating the median.

The results are presented in Figs. 14g–i, where the percent of trials solved and the median solution cost are plotted versus run time. They demonstrate how Informed RRT* has better real-time convergence towards the optimum than existing techniques, especially in higher state dimensions.

VII-C Motion Planning for HERB

Informed RRT* was demonstrated on a high-dimensional problem using HERB, a 14-DOF mobile manipulation platform [11]. Poses were defined for the two arms to create a sequence of three planning problems (Fig. 15) inspired by [39]. The objective of these problems was to find the minimum path length through a $14$-dimensional search space with strict limits (each joint has no more than $\pi$–$2\pi$ radians of travel). While path length is not a common cost function for manipulation, these experiments illustrate that direct informed sampling is beneficial in high-dimensional problem domains even with strict search limits.

RRT*, RRT* with pruning and rejection, and Informed RRT* were each run for $50$ trials on each problem of the cycle. The resulting median path lengths are presented in Fig. 16. Trials that did not find a solution were considered to have infinite length for the purpose of calculating the median. This only occurred for the problem from (a) to (b), where the planners found a solution on $94\%$ of the trials.

RRT* with and without pruning and rejection sampling both fail to improve the initial solutions on all three planning problems but Informed RRT* is able to improve the path length by $3.9\%$, $7.9\%$, and $28.2\%$, respectively. The improvement for (a) to (b) is not statistically significant but (b) to (c) and (c) to (d) demonstrate the benefits of considering the relative sizes of the informed set and problem domain in high state dimensions.

VIII Discussion & Conclusion

RRT* almost-surely converges asymptotically to the optimum by asymptotically finding the optimal paths to every state in the problem domain. This is inefficient in single-query scenarios as, once a solution is found, searches only need to consider states that can belong to a better solution (i.e., the omniscient set; Definition 3, Lemma 4). Previous work has focused search to estimates of this set (i.e., informed sets; Definition 7) but has not used these estimates to analyze performance. This paper proves that for holonomic problems the probability of sampling an admissible informed set provides an upper bound on the probability of improving a solution (Theorem 13).

A popular admissible heuristic for problems seeking to minimize path length is the $L^{2}$ norm (i.e., Euclidean distance). This paper shows that existing techniques to exploit it are insufficient. The majority of approaches either reduce the ability to find solutions in other homotopy classes (i.e., reduce recall; Definition 9) or fail to account for the reduction of the $L^{2}$ informed set in response to solution improvement (i.e., have decreasing precision; Definition 8). Even existing adaptive techniques that address these problems (e.g., [9]) fail to account for its factorial decrease in measure with state dimension (i.e., the minimum-path-length curse of dimensionality; Theorem 14).

This paper presents a method to avoid these limitations through direct sampling of the $L^{2}$ informed set (Algs. 1–5; Section IV). This approach generates uniformly distributed samples in the informed set regardless of its size relative to the problem domain or the state dimension (i.e., it has $100\%$ recall and high precision). This paper presents Informed RRT* as a demonstration of how these techniques can be used in sampling-based planning (Algs. 6 and 7; Section V).

Informed RRT* considers all homotopy classes that could provide a better solution (i.e., $100\%$ recall), unlike sample biasing techniques. It is effective regardless of the relative size of the informed set or the state dimension, unlike sample rejection or graph pruning. When the heuristic does not provide any information (e.g., small planning problems and/or large informed sets) it is identical to RRT*.

This paper also uses the shape of the $L^{2}$ informed set to analyze the theoretical performance of RRT* on minimum-path-length problems (Section VI) by bounding the expected solution cost (Lemma 17) and convergence rates (Theorems 18–20). The bounds are sharp over the set of all (Lemma 17) or all holonomic (Theorems 18–20) minimum-path-length planning problems and algorithm configurations with the lower bounds exact for an infinite rewiring radius in the absence of obstacles and constraints. These results prove that RRT* converges sublinearly (i.e., slower than linear) for all configurations and holonomic minimum-path-length problems and that focused variants (e.g., Informed RRT*) can have linear convergence.

This analysis is extended experimentally to different configurations. The results confirm the theoretical findings and suggest that obstacle- and constraint-free convergence remains linear when the rewiring radius is constant but becomes sublinear when it decreases in the manner proposed by [6]. As previous analysis of this radius has focused on per-iteration complexity, we believe this result motivates future research into the trade off between per-iteration cost and convergence rate.

The practical advantages of Informed RRT* are shown on a variety of planning problems (Section VII). These experiments demonstrate how its theoretical convergence rate corresponds to better performance on real planning problems. The amount of improvement depends on how efficiently the $L^{2}$ informed set decreases the search domain and may be limited in small problem domains and/or long circuitous solutions (e.g., the small/low-dimensional problems in Section VII-A and the first problem of Section VII-C). The design of Alg. 3 assures that in these situations Informed RRT* performs no worse than other methods to exploit the $L^{2}$ heuristic (e.g., rejection sampling).

Designing these experiments highlighted the relationship between the maximum edge length, $\eta$, and algorithm performance. This user-selected value not only affected the time required to find an initial solution but, as a result of (19), also the quality of the solution found in finite time. Specifically, large values of $\eta$ appeared to decrease the difference between algorithms; however, also resulted in order of magnitude increases in the time required to find initial solutions. When coupled with the results of Section VII, this result should further motivate more research into the effects of the RRT* tuning parameters, $\eta$ and $r_{\mathrm{RRT}^{*}}$, on real-time performance. Given that anytime improvement of a solution is a major feature of RRT*, we tuned $\eta$ for these experiments to minimize the initial-solution time on a series of independent test problems.

We believe that defining precise and admissible informed sets is a fundamental challenge of using anytime almost-surely asymptotically optimal planners in real-world applications. The $L^{2}$ informed set is a sharp, uniformly admissible estimate of the omniscient set for problems seeking to minimize path length, even in the presence of constraints, and is exact in the absence of obstacles and constraints. This suggests that any informed set that is more precise must either

(i) exploit additional information about the problem domain (e.g., obstacles, constraints), and/or

(ii) be inadmissible for some minimum-path-length planning problems.

Finding ways to define new admissible heuristics from additional problem-specific information could potentially allow focused search algorithms to converge linearly in the presence of obstacles and/or constraints.

We ultimately believe that heuristics are a key component of successful planning algorithms. To this end, we are currently investigating methods to extend heuristics to entire sampling-based searches, similar to how A* [2] extends Dijkstra’s algorithm [1]. We accomplish this in BIT* [34, 35, 36] by extending the ideas presented in this paper to batches of randomly generated samples. These samples are limited to informed sets and searched in order of potential solution quality. Information on OMPL implementations of both Informed RRT* and BIT* are available at http://asrl.utias.utoronto.ca/code.

Appendix A Proofs of Lemmas 4 and 5

This section restates and proves Lemmas 4 and 5.

A-A Proof of Lemma 4

Lemma 4 (The necessity of adding states in the omniscient set).

Adding a state from the omniscient set, $\mathbf{x}_{\rm new}\in X_{f}$, is a necessary condition for RRT* to improve the current solution, $c_{i}$,

[TABLE]

This condition is necessary but not sufficient to improve the solution as the ability of states in $X_{f}$ to provide better solutions at any iteration depends on the structure of the tree (i.e., its optimality).

Proof.

At the end of iteration $i+1$, the cost of the best solution found by RRT* will be the minimum of the previous best solution, $c_{i}$, and the best cost of any new or newly improved solutions, $c_{\rm new}$,

[TABLE]

Each iteration of RRT* only adds connections to or from the newly added state, $\mathbf{x}_{\rm new}$, and therefore all new or modified paths pass through this new state. The cost of any of these new paths that extend to the goal region will be bounded from below by the cost of the optimal solution of a path through $\mathbf{x}_{\rm new}$,

[TABLE]

Lemma 4 is now proven by contradiction. Assume that RRT* has a solution with cost $c_{i}$ after iteration $i$ and that it is improved at iteration $i+1$ by adding a state not in the omniscient set, $c_{i+1}<c_{i},\,\mathbf{x}_{\rm new}\not\in X_{f}$. By (1), the costs of solutions through any $\mathbf{x}_{\rm new}\not\in X_{f}$ are bounded from below by the current solution,

[TABLE]

which by (29) is also a bound on the cost of any new or modified solutions,

[TABLE]

By (28), the cost of the best solution found by RRT* at the end of iteration $i+1$ must therefore be $c_{i}$. This contradicts the assumption that the solution was improved by a state not in the omniscient set and proves Lemma 4. ∎

A-B Proof of Lemma 5

Lemma 5 (The necessity of sampling states in the omniscient set in holonomic planning).

Sampling the omniscient set, $\mathbf{x}_{\rm rand}\in X_{f}$, is a necessary condition for RRT* to improve the current solution to a holonomic problem, $c_{i}$, after an initial $\kappa$ iterations,

[TABLE]

for any sample distribution that maintains a nonzero probability over the entire omniscient set.

For simplicity, this statement is limited to holonomic planning but it can be extended to specific constraints with appropriate assumptions.

Proof.

In RRT (and therefore RRT*), the distribution of vertices in the graph approaches the sample distribution as the number of iterations approach infinity [lavalle_tech98]. In the limit, all reachable regions of the problem domain with a nonzero sampling probability will therefore be sampled and the number of vertices in these regions will increase indefinitely with the number of iterations. This ever increasing number of vertices means that the worst-case distance between any state in a sampled subset and the nearest vertex in the graph will decrease indefinitely and monotonically.

Lemma 5 is now proven by contradiction. Assume that by iteration $\kappa$ there are a sufficient number and distribution of vertices in the tree such that all possible states in $X_{f}$ are no further than $\eta$ from a vertex,

[TABLE]

and that RRT* has a solution with cost $c_{i}$ after iteration $i\geq\kappa$. Now assume that RRT* improves the solution at iteration $i+1$ without sampling the omniscient set, $c_{i+1}<c_{i},\,\mathbf{x}_{\rm rand}\not\in X_{f}$.

As improving a solution requires adding a state from the omniscient set, $\mathbf{x}_{\rm new}\in X_{f}$, (Lemma 4) this implies that the state added to the graph is not the randomly sampled state, $\mathbf{x}_{\rm new}\not=\mathbf{x}_{\rm rand}$. These two states are related in holonomic planning by expansion constraints, (2) and (3), that find a new state as near as possible to $\mathbf{x}_{\rm rand}$ and no further than $\eta$ from the nearest vertex in the tree.

The triangle inequality implies that the nearest vertex to the sample, $\mathbf{v}_{\rm nearest}$, is also the nearest vertex to the proposed new state,

[TABLE]

which from (30) is bounded in its distance from $\mathbf{x}_{\rm new}$ by

[TABLE]

Due to (3), the relationship in (31) is only satisfied in holonomic planning when $\mathbf{x}_{\rm new}\equiv\mathbf{x}_{\rm rand}$. As by assumption the random sample is not a member of the omniscient set, $\mathbf{x}_{\rm rand}\not\in X_{f}$, then therefore neither is the newly added state, $\mathbf{x}_{\rm new}\not\in X_{f}$, and by Lemma 4 the solution is not improved, $c_{i+1}=c_{i}$. This contradicts the assumption that the solution was improved by sampling a state not in the omniscient set and proves Lemma 5. ∎

Appendix B Proof of Lemma 15

This section restates Lemma 15 as presented by [31] along with a full proof as presented in [14].

Lemma 15 (The uniform distribution of samples transformed into a hyperellipsoid from a unit $n$-ball. Originally Lemma 1 in [31]).

If the random points distributed in a hyperellipsoid are generated from the random points uniformly distributed in a hypersphere through a linear invertible nonorthogonal transformation, then the random points distributed in the hyperellipsoid are also uniformly distributed.

Proof.

Let the sets $X_{\rm ball}\subset{}^{n}$ and $X_{\rm ellipse}\subset{}^{n}$ be the unit $n$-dimensional ball and a $n$-dimensional hyperellipsoid with radii $\left\{r_{j}\right\}_{j=1}^{n}$, respectively, having measures of

[TABLE]

Let $p_{\rm ball}\left(\cdot\right)$ be the probability density function of samples drawn uniformly from the unit $n$-ball such that,

[TABLE]

Let $\tau\left(\cdot\right)$ be an invertible transformation from the unit $n$-ball to a hyperellipsoid such that,

[TABLE]

By definition, the probability density function in the hyperellipsoid, $p_{\rm ellipse}\left(\cdot\right)$, resulting from applying this transformation to samples distributed in the unit $n$-ball is then

[TABLE]

The proposed transformation in (12) has the inverse

[TABLE]

and the Jacobian

[TABLE]

Substituting (34) and (32) into (33) gives,

[TABLE]

using the fact that $\tau^{-1}\left(\mathbf{x}\right)\in X_{\rm ball}\iff\mathbf{x}\in X_{\rm ellipse}$. As $p_{\rm ellipse}\left(\cdot\right)$ is constant for all $\mathbf{x}_{\rm ellipse}\in X_{\rm ellipse}$, this proves that using (12) to transform uniformly distributed samples in the unit $n$-ball results in a uniform distribution over the hyperellipsoid and proves Lemma 15.

For hyperellipsoids whose axes are orthogonal (e.g., a prolate hyperspheroid), (35) can be expressed in a more familiar and intuitive form. Using (IV-A) for $\tau\left(\cdot\right)$ and the orthogonality of rotation matrices makes (35)

[TABLE]

where $\mathbf{L}^{\prime}=\operatorname*{diag}\left(r_{1},r_{2},\ldots,r_{n}\right)$ is a diagonal matrix which then simplifies (36) to

[TABLE]

since the determinant is a linear operator, all rotation matrices have a unity determinant, $\det\left(\mathbf{C}_{\rm we}\right)=1$, and the determinant of a diagonal matrix is the product of its diagonal entries. As expected, (37) is the inverse of the volume of an $n$-dimensional hyperellipsoid with radii $\left\{r_{j}\right\}_{j=1}^{n}$. ∎

Appendix C Proof of Lemma 17

This section restates and proves Lemma 17, which is used in support of Theorems 18–20. An earlier version of this proof appeared in [13].

Lemma 17 (Expected next-iteration cost of minimum-path-length planning).

The expected value of the next solution to a minimum-path-length problem, $E\left[c_{i+1}\right]$, is bounded by

[TABLE]

where $c_{i}$ is the current solution cost, $c_{\rm min}$ is the theoretical minimum solution cost, $n$ is the state dimension of the planning problem, and $p_{f}=P\left(\mathbf{x}_{\rm new}\in X_{f}\right)$ is the probability of adding a state that is a member of the omniscient set (i.e., that can belong to a better solution). While not explicitly shown, the subset, $X_{f}$, and the probability of improving the solution, $p_{f}$, are generally functions of the current solution cost.

This lower bound is sharp over the set of all possible minimum-path-length planning problems and algorithm configurations and is exact for versions of RRT* with an infinite rewiring radius (i.e., $\eta=\infty$, and $r_{\mathrm{RRT}^{*}}=\infty$) searching an obstacle-free environment without constraints.

Proof.

Proof of the upper bound is trivial. RRT* only accepts new solutions that improve its existing solution, assuring that the cost monotonically decreases,

[TABLE]

Proof of the lower bound comes from finding an exact expression for the expected value of the solution cost found in the absence of obstacles and constraints with an infinite rewiring neighbourhood.

The expected solution cost of RRT* depends on the probability of sampling the omniscient set,

[TABLE]

where $p_{f}=P\left(\mathbf{x}_{\rm new}\in X_{f}\right)$. Adding a state from the omniscient set, $X_{f}$, is a necessary condition to improve the solution (Lemma 4) and any other state will not change the solution cost, $E\left[c_{i+1}\,\middle|\,\mathbf{x}_{\rm new}\not\in X_{f}\right]=c_{i}$. This simplifies (C) to

[TABLE]

The costs of solutions found by adding states inside the omniscient are bounded from below by the optimal path through the newly added state,

[TABLE]

where $f\left(\mathbf{x}\right)$ is the cost of the optimal path from the start to the goal constrained to pass through a state, $\mathbf{x}$. With a uniform sample distribution over $X_{f}$ the right-hand side of (41) becomes

[TABLE]

When RRT* uses an infinite rewiring radius it attempts connections between every new state and the start and goal. In the absence of obstacles and constraints these paths will be feasible and represent the optimal solutions using the state. This makes the expected value of this best-case configuration of RRT* equivalent to the expected optimal solution cost in the absence of obstacles,

[TABLE]

The lower bound provided by (41) is therefore sharp over the set of all possible planning problems and algorithm configurations.

In this absence of obstacles and constraints, the optimal solution using any state is given by (4) and the omniscient set is the prolate hyperspheroid, $X_{f}\equiv X_{\widehat{f}}\equiv X_{\rm PHS}$. The measure of the omniscient set, $\lambda\left(X_{f}\right)=\lambda_{\rm PHS}$, is given by (5). This allows (42) to be written as

[TABLE]

The prolate hyperspheroidal coordinates, $\mu,\nu,\psi_{1},\ldots,\psi_{n-2}$,

[TABLE]

and the parameterization $a=0.5c_{\rm min}$, simplifies (4) to

[TABLE]

Substituting (44) and the prolate hyperspheroidal differential volume,

[TABLE]

into (C) results in

[TABLE]

where the integration limit for $\mu$ is derived from (44) as

[TABLE]

Integrating (C) requires applying a series of identities, first

[TABLE]

simplifies (C) to

[TABLE]

Next, the definite integral of the product of powers of $\sin$ and $\cos$,

[TABLE]

where $\mathrm{B}\left(\cdot,\cdot\right)$ is the beta function,

[TABLE]

is used to evaluate the integral over $\nu$ in (C), giving

[TABLE]

The identity,

[TABLE]

and the recursive nature of the $n$-dimensional unit ball,

[TABLE]

simplifies (C) to

[TABLE]

The indefinite integral,

[TABLE]

is then used to evaluate (C), giving

[TABLE]

Using (5) to expand the measure $\lambda_{\rm PHS}$ in (C) cancels the measure of the unit $n$-ball, giving

[TABLE]

Using the relationship

[TABLE]

some algebraic manipulation, and (46) finally simplifies (C) to

[TABLE]

an exact value for the best-case expected solution cost of RRT*. This result allows (40) to be written as the sharp bound,

[TABLE]

which when combined with (38) proves Lemma 17. ∎

Appendix D Proofs of Theorems 18–20

This section restates and proves Theorems 18–20.

D-A Proof of Theorem 18

Theorem 18 (Sublinear convergence of RRT* in holonomic minimum-path-length planning).

RRT** converges sublinearly towards the optimum of holonomic minimum-path-length planning problems,*

[TABLE]

For simplicity, this statement is limited to holonomic planning but it can be extended to specific constraints by expanding Lemma 5.

Proof.

The expected rate of convergence (Definition 16) of RRT* is

[TABLE]

since $\forall i,\,c_{i}\geq c^{*}$. As RRT* almost-surely converges asymptotically to the optimum, this sequence also almost-surely converges to a finite value, $0\leq\mu_{\mathrm{RRT}^{*}}\leq 1$,

[TABLE]

By Lebesgue’s dominated convergence theorem this allows the expectation operator to be brought inside the limit of (52), giving

[TABLE]

since $c_{i}$ is the only random variable at iteration $i$.

Lemma 17 provides sharp bounds for the expected solution cost at any iteration, $E\left[c_{i}\right]$, with the lower-bound corresponding to an infinite rewiring radius in the absence of obstacles and constraints. Substituting this lower bound and that $c^{*}=c_{\rm min}$ in the absence of obstacles into (53) and simplifying gives an expression for the expected best-case convergence rate,

[TABLE]

such that $E\left[\mu_{\mathrm{RRT}^{*}}^{*}\right]\leq E\left[\mu_{\mathrm{RRT}^{*}}\right]$ is a sharp bound over all possible planning problems and algorithm configurations. Applying l’Hôpital’s rule [hopitals_rule] with respect to $c_{i-1}$ gives

[TABLE]

As iterations go to infinity the probability of adding a sample in $X_{f}$ becomes the probability of sampling it (Lemma 5). The lower bound from Lemma 17 is for an obstacle- and constraint-free problem and therefore the informed set is the omniscient set, $X_{f}\equiv X_{\widehat{f}}$, and the probability of sampling it is given by (IV) with a partial derivative of

[TABLE]

Almost-sure convergence to $c_{\rm min}$ implies $\lim_{i\to\infty}c_{i-1}=c_{\rm min}$ and therefore $\lim_{i\to\infty}p_{f}=0$ and $\lim_{i\to\infty}\frac{\partial p_{f}}{\partial c_{i-1}}=0$, making (D-A),

[TABLE]

As by definition the expected rate of convergence of RRT* is bounded by,

[TABLE]

this result proves Theorem 18. ∎

D-B Proof of Theorem 19

Theorem 19 (Linear convergence of RRT* with adaptive rectangular rejection sampling in holonomic minimum-path-length planning).

RRT** with adaptive rectangular rejection sampling converges at best linearly towards the optimum of holonomic minimum-path-length planning problems but factorially approaches sublinear convergence with increasing state dimension,*

[TABLE]

For simplicity, this statement is limited to holonomic planning but it can be extended to specific constraints by expanding Lemma 5.

Proof.

Proof of Theorem 19 follows that of Theorem 18 but with the probability of adding a new state from $X_{f}$ instead calculated from (IV) using (10), as

[TABLE]

As $\frac{\partial p_{f}}{\partial c_{i-1}}=0$, (D-A) becomes

[TABLE]

Noting that almost-sure convergence to $c_{\rm min}$ implies $\lim_{i\to\infty}c_{i-1}=c_{\rm min}$ and substituting (55) into (56) results in

[TABLE]

As by definition the expected rate of convergence of RRT* with rectangular rejection sampling is bounded by,

[TABLE]

This result proves Theorem 19 with sharp bounds over all possible holonomic planning problems and algorithm configurations. ∎

D-C Proof of Theorem 20

Theorem 20 (Linear convergence of Informed RRT* in holonomic minimum-path-length planning).

Informed RRT* converges at best linearly towards the optimum of holonomic minimum-path-length planning problems,

[TABLE]

where the lower-bound occurs exactly with an infinite rewiring neighbourhood in the absence of obstacles and constraints.

For simplicity, this statement is limited to holonomic planning but it can be extended to specific constraints by expanding Lemma 5.

Proof.

Proof of Theorem 20 follows that of Theorem 18 but with a unity probability of adding a new state from $X_{f}$. From (D-A), the convergence rate of Informed RRT* is then,

[TABLE]

As almost-sure convergence to $c_{\rm min}$ implies $\lim_{i\to\infty}c_{i-1}=c_{\rm min}$, this gives,

[TABLE]

As by definition the expected rate of convergence of RRT* with rectangular rejection sampling is bounded by,

[TABLE]

This result proves Theorem 20 with sharp bounds over all possible holonomic planning problems and algorithm configurations. ∎

Acknowledgment

The authors would like to thank the editorial board and reviewers for their helpful comments. We would also like to thank Laszlo-Peter Berczi for discussions on precision and recall, Christopher Dellin and Michael Koval for discussions and insight on the sampling of arbitrarily overlapping shapes, Jennifer King and Clinton Liddick for help running the HERB experiments, and Paul Newman for providing the time to finish this manuscript at the University of Oxford.