BIT*: Batch Informed Trees for Optimal Sampling-based Planning via

Dynamic Programming on Implicit Random Geometric Graphs

Jonathan D. Gammell1 , Siddhartha S. Srinivasa2 , and Timothy D. Barfoot1

Abstract— Path planning in continuous spaces has tradi- RRT* BIT*

tionally been divided between discrete and sampling-based
techniques. Discrete techniques use the principles of dynamic
programming to solve a discretized approximation of the
problem, while sampling-based techniques use random samples
arXiv:1405.5848v1 [cs.RO] 22 May 2014

to perform a stochastic search on the continuous state space.

In this paper, we use the fact that stochastic planners can
be viewed as a search of an implicit random geometric graph
(RGG) to propose a general class of planners named Bellman
random trees (BRTs) and derive an anytime optimal sampling-
based algorithm, batch informed trees (BIT*).
BIT* searches for a solution to the continuous planning
problem by efficiently building and searching a series of implicit
RGGs in a principled manner. In doing so, it strikes a balance
between the advantages of discrete methods and sampling-based
planners. By using the implicit RGG representation, defined as
set of random samples and successor function, BIT* is able to
scale more effectively to high dimensions than other optimal
sampling-based planners. By using heuristics and intelligently
reusing existing connections, like discrete lifelong planning
algorithms, BIT* is able to focus its search in a principled
and efficient manner.
In simulations, we show that BIT* consistently outperforms
optimal RRT (RRT*), informed RRT* and fast marching trees
(FMT*) on random-world problems in R2 and R8 . We also
present preliminary theoretical analysis demonstrating that
BIT* is probabilistically complete and asymptotically optimal
and conjecture that it may be optimally efficient under some
probabilistic conditions.
I. I NTRODUCTION
Path planning techniques for robotics are roughly divided
into two categories: graph-based searches that operate on a
discrete representation of the problem and stochastic incre-
mental searches that grow graphs by randomly sampling the
continuous state space.
Graph-based searches, such as Dijkstra’s algorithm [1] and
A* [2], use dynamic programming [3], [4] to solve a discrete
representation of the problem exactly. These algorithms are
not only resolution complete but also resolution optimal,
always finding the optimal solution to the discretization of
the planning problem, if one exists. A* accomplishes this
efficiently by using a heuristic, fb(·), to estimate the potential
cost of a solution constrained to pass through a state. The
result is an optimally efficient algorithm, meaning that any
other optimal algorithm with the same heuristic will expand
at least as many states as A* [2].
1 J. D. Gammell and T. D. Barfoot are with the Autonomous
Space Robotics Lab at the University of Toronto Institute for
Aerospace Studies, Toronto, Ontario, Canada. Email: {jon.gammell,
tim.barfoot}@utoronto.ca
2 S. S. Srinivasa is with The Robotics Institute, Carnegie Mellon
University, Pittsburgh, Pennsylvania, USA. Email: siddh@cs.cmu.edu
c = 1.54 c = 1.30
Fig. 1. An example of RRT* and BIT* run on a difficult random R2 world
for the same arbitrary computational time. BIT*’s use of heuristics allows
it to find a better solution (c = 1.30) faster than RRT* (c = 1.54) by
performing its search in a principled manner that initially prioritizes low-
cost solutions and then focuses on improving them. Cyan dots and blue
lines represent vertices and edges, respectively, in the resulting tree.
The original A* algorithm returns only the final optimal
solution to a given problem. Anytime algorithms based on A*
exist that initially provide suboptimal solutions quickly while
continuing to calculate the optimal solution in an efficient
manner [5]–[8]. The original A* algorithm is also inefficient
at handling changes to the graph; even small changes require
a complete recalculation of the solution. Replanning algo-
rithms based on A* are capable of incorporating changes in
ways that efficiently reuse the existing solutions, minimizing
the new calculations required [8]–[11].
The quality of the continuous-space solution resulting
from these graph-search techniques depends heavily on the
chosen discretization of the problem. Finer discretization
increases the quality of the solution [12], but also increases
the computational effort necessary to find it. This becomes
a significant problem when planning in high-dimensional
spaces, such as for manipulation planning, as the size of the
discrete state space grows exponentially with the number of
dimensions, a problem Richard Bellman himself referred to
as the curse of dimensionality [13].
Despite these limitations, dynamic programming has long
been applied to path-planning algorithms [14] including
for nonholonomoic robots [15], [16], kinodynamic plan-
ning [17], [18] and even manipulation planning [19]. Most
techniques used regularly-spaced discretizations, but existing
work also includes variable resolutions [20] or randomized
graphs [21].
Sampling-based planners, such as probabilistic roadmaps
(PRM) [23], rapidly-exploring random trees (RRT) [24], and
expansive-space trees (EST) [25], [26], avoid the problems

This is a preliminary draft of work in preparation for the

2015 IEEE International Conference on Robotics and Automation (ICRA)
 RRT* Informed RRT* FMT* BIT*

t = 11.8s t = 1.57s t = 1.72s t = 0.0485s

Fig. 2. An example of RRT*, informed RRT* [22], FMT* (n = 5000) and BIT* run on a random R2 world. Each algorithm was run until it found a
equivalent solution to FMT* (c = 1.34). BIT*’s use of heuristics allows it to find such a solution significantly faster (t = 0.0485s) than RRT* (t = 11.8s),
FMT* (t = 1.72s) and informed RRT* (t = 1.57s) by performing its search in a principled manner that initially prioritizes low-cost solutions and then
focuses on improving them. Cyan dots and blue lines represent vertices and edges, respectively, in the resulting tree.

of discretization by representing the planning domain as a set
of samples drawn randomly from a continuous state space.
This allows them to scale effectively to high-dimensional
configuration spaces. They are probabilistically complete,
having a probability of finding a solution, if one exists, that
goes to unity as the number of samples goes to infinity.
Optimal variants, such as RRT*, PRM* [27] and FMT* [28],
are also asymptotically optimal, having the probability that
the found solution is optimal, if one exists, also going to
unity as the number of samples goes to infinity. To date,
very little has been said about the efficiency with which
these algorithms find the optimal solution; however, informed
RRT* [22] showed that a heuristic can be used to improve
the rate at which a solution converges towards the optimum.
In this paper, we argue that optimal sampling-based plan-
ning algorithms that generate trees (e.g., RRT*, FMT*) can
be unified as a class of algorithms we refer to as Bellman
random trees (BRTs). This class of planners uses (exact or
approximate) dynamic programming to build an explicit tree
from the implicit random geometric graph (RGG) defined by
the random samples. We unify these observations with dy-
namic programming to develop an anytime optimal planning
algorithm, batch informed trees (BIT*) (Figs. 1, 2).
BIT* is a BRT algorithm that combines the anytime nature
of incremental BRTs (i.e., RRT*) and the efficiency of bulk
BRTs (i.e., FMT*) with a heuristic to focus the search. This
not only prioritizes low-cost solutions, but also minimizes
the size of the explicit tree constructed from the implicit
RGG. It does this by using dynamic programming to solve
increasingly dense RGGs that are constructed in a principled
manner, allowing for both quick initial solutions as well
as a focused search for improvements that reuses existing
information. Its performance depends on the properties of
dynamic programming and RGGs, making it simple to
analyze. It is shown that it is both probabilistically complete
and asymptotically optimal, and it is conjectured that under
some currently unknown probabilistic conditions, it may also
be optimally efficient.
The performance of BIT* is demonstrated relative to
existing optimal algorithms through a series of experiments
on random worlds in R2 (Figs. 4, 5) and R8 (Figs. 6, 7). BIT*
more reliably finds solutions and finds solutions of equivalent
cost faster than RRT*, informed RRT*, and FMT*.
The remainder of this paper is organized as follows.
Section II presents the general BRT class of planners while
Section III presents the specific BIT* algorithm. Section IV
presents initial analysis of the algorithm. Section V presents
the experimental results in more detail and Sections VI and
VII provide a discussion of the algorithm and ongoing work
and a conclusion.
II. B ELLMAN R ANDOM T REES (BRT S )
Karaman and Frazzoli [27] recognized that the random
samples from the state space in a planning algorithm can
be modelled as an RGG. RGGs are a probabilistic structure
with a number of important statistical properties depending
on their specific type. For planning, the type of RGG depends
on the connection scheme used by the planner, but common
types include r-disc graphs [29], [30], k-nearest neighbours
graphs [31], [32], and online graphs [33].
Karaman and Frazzoli [27] provide excellent definitions
of these graphs as they relate to planning. An r-disc RGG,
G near (n, r), is defined by a set of n randomly distributed
vertices and a radius of connection r, with edges existing
between vertices if and only if the distance between them
is less than r. A k-nearest neighbours graph, G near (n, k), is
similarly defined by a set of n random vertices but with edges
existing between vertices and their k-th nearest neighbours.
An online nearest-neighbour graph, G onn (n), is defined by
a sequence of n random vertices with edges existing only
between a vertex and the preceding vertex in the sequence
that minimizes the edge length. These different types of
RGGs will be necessary in the following sections to describe
how the different algorithms work.
Any set (or sequence) of samples from the state space,
V = {x ∈ Xfree }, can by viewed as an implicit RGG with
a specified successor function that uncovers a given vertex’s
connections, τ : v ∈ V 7→ {(v, w ∈ V )}. For example, in
the implicit r-disc graph this successor function would be a
nearest-neighbours search

This is a preliminary draft of work in preparation for the

2015 IEEE International Conference on Robotics and Automation (ICRA)
 By not explicitly storing the edge set, implicit graphs
are a compression that reduces the size of the graph while
increasing the computational effort necessary to search it.
Sampling-based planning algorithms that return a tree can
therefore be viewed as a procedure that searches an implicit
RGG for an explicit spanning tree, with efficient algorithms
minimizing computation. The successor function depends on
the specific planning algorithm and in some conditions, may
also uncover new samples (e.g., dynamic constraints, just-in-
time sampling, etc.). Given the common use of approximate
or exact dynamic programming to perform this search on
the RGG we propose naming this class of planners Bellman
random trees (BRTs).
RRT* is an example of an iterative, anytime BRT and
can be viewed as a random search through an RGG for a
approximate shortest-path spanning tree. Given a sequence
of random samples, {x1 , x2 , . . . , xn }, RRT* incrementally
does this by performing an approximate dynamic program-
ming step for the i-th vertex. Considering only the current
spanning tree, the current online nearest-neighbour RGG,
Gionn (ni ), and the r-disc RGG, Ginear (ni , ri ), RRT* finds
both the edge that minimizes the cost-to-come of the i-th
vertex in the tree and updates nearby vertices in the tree with
better edges from the r-disc graph. The algorithm is anytime
and can be run until a solution of suitable quality is found,
but the search is unfocused and it finds optimal solutions to
every state in the problem. As the order of vertices added
to the spanning tree is random, so is its construction and
the search for a solution to the optimal planning problem.
In finite time, the final solution will depend not only on
the random samples but also the order in which they are
processed.
FMT* is an example of a bulk BRT and can be viewed
as an approximation of Dijkstra’s algorithm on an implicit
RGG. It methodically builds an approximate shortest-path
spanning tree through an implicit r-disc RGG given by n
samples and a radius connection, r. It processes a queue of
open vertices from the tree, at each iteration expanding the
vertex in the queue with the cheapest cost-to-come. It applies
the successor function to this vertex to uncover vertices in
the implicit RGG not in the tree and adds them to the tree
in an approximately optimal manner (see Section 3.1 of
[28] for a discussion of a rare sub-optimal condition). The
ordered processing of the RGG provides a more efficient
conversion of the implicit RGG to an explicit spanning tree;
however it also finds optimal solutions to every state in the
problem. The algorithm is also not anytime, and if a solution
requires further improvement a completely new denser search
is required.
BIT* balances the advantages of bulk and iterative BRTs
to provide an anytime planning algorithm that is faster
than both RRT* and FMT*. It does this in a way that is
both principled and efficient by introducing a heuristic and
applying the principles of dynamic programming to search
the implicit RGG. Each batch is searched in an order that
prioritizes both growth towards the goal and the exploration
This is a preliminary draft of work in preparation for the
2015 IEEE International Conference on Robotics and Automation (ICRA)
of high-quality paths. Section III describes the algorithm in
detail.
III. BATCH I NFORMED T REES (BIT*)
The definition of the optimal planning problem is as in
d
[27]. Let X = [0, 1] be the configuration space of the
planning problem and Xobs ⊂ X be the states in collision
with obstacles. The permissible set of states is then Xfree =
X \ Xobs . Let xs ∈ Xfree be the initial state of the planning
problem and Xgoal ⊂ Xfree be the desired final state. Let
σ : [0, 1] 7→ X be a sequence of states (a path) and Σ be
the set of all nontrivial paths.
The formal definition of the optimal planning problem is
then the search for the path, σ ∗ , that minimizes a given cost
function, c : Σ 7→ R≥0 , and connects xs to xg ∈ Xgoal
through free space,
σ ∗ = arg min {c (σ) | σ(0) = xs , σ(1) = xg ,
σ∈Σ
∀s ∈ [0, 1] , σ (s) ∈ Xfree } ,
where R≥0 is the set of non-negative real numbers.
BIT* employes a batch approach to solving the optimal
planning problem that allows a balance between the anytime
nature of RRT* and the methodical nature of FMT*. It does
this by searching for the shortest path from start to goal
through a series of increasingly dense r-disc RGGs defined
by batches of random samples. By processing the vertices
in batches of size greater than one, BIT* is able to process
the samples in an ordered manner, like FMT*. Processing
multiple batches also provides a number of advantages over
a single large batch (FMT*) or a large number of individual
batches (RRT*). First, it is able to return solutions in an
anytime manner and run continuously towards the optimal
solution, like RRT*. Second, when solution improvements
are found, new batches of samples can be drawn from a
smaller subset of the problem, increasing sample density only
where it matters.
Informally, BIT* works as follows. An r-disc RGG,
Ginear (ni , ri ), is implicitly constructed from ni = n0 uni-
formly distributed random samples from Xfree and an ri
that satisfies the requirements for asymptotic optimality
(see [27]). A heuristically guided search then builds an
explicit shortest-path tree through the RGG until a path
between xs and xg is found. The length of this path then
defines a heuristically informed subset of the problem, Xfb,
that contains all possibly better solutions. A new implicit
near
RGG, Gi+1 (ni+1 , ri+1 ), is then generated from a batch
of ni+1 = ni + n0 random samples uniformally-distributed
on Xfb and used to update the existing tree. The radius of
connection, ri+1 is once again chosen such that it satisfies
the requirements for asymptotic optimality. Each batch of
samples not only increases the density of the RGG but is
also used to update the shortest-path tree through RRT*-style
local rewirings. This process is continued as time allows.
To perform the search in an efficient way, BIT* employs
a heuristic to guide the search through the RGGs. Not only
does this focus the search towards the goal and minimize
the number of vertices expanded, but it also prioritizes the
 Algorithm 1: BIT*(xs ∈ Xfree , xg ∈ Xfree ) Algorithm 3: UpdateFreeQueue(v ∈ V )
n o
1 V ← {xs } ; E ← ∅; T := (V, E); 1 fsample ← min fb(v) + 2r, fmax ;
2 Qfree ← {xg } ; Qedge ← ∅; Qrewire ← ∅;
3 fprev ← 0; fmax ← ∞; 2 if freq’d > fprev then
n  o
repeat λsample ← λ x ∈ X  fprev < fb(x) ≤ fsample ;

4 3
5 foreach v ∈ V do 4 ρ← λ n ;
6 UpdateEdgeQueue (v); sample
+

5 Qfree ← − Sample fprev , fsample , ρ ;
7 while Qedge 6= ∅ do n o 6 fprev ← fsample ;
8 (vm , xm ) ← arg min gT (v) + b
c (v, x) + b
h (x) ;
(v,x)∈Qedge
−
9 E ←− {(vm , xm )}; Algorithm 4: UpdateRewireQueue(v ∈ V )
10 if gT (vm ) + b c (vm , xm ) + b
h (xm ) < gT (xg ) then  
1 Vn ← w ∈ V  ||w − v||2 ≤ r ;
11 if g
b (vm ) + c (vm , xm ) + bh (xm ) < gT (xg ) then +
+ + 2 Qrewire ←− {(v, w ∈ Vn ) | gT (v) + b
c (v, w) < gT (w)};
12 V ←− {xm } ; E ← − {(vm , xm )};
−
13 Qfree ←− {xm }; i.e., fb(x) := gb (x) + bh (x).
14 UpdateEdgeQueue (xm );
15 UpdateRewireQueue (xm ); The function gT (x) represents the cost-to-come to a state
16 Rewire; x ∈ X from the start vertex given the current tree, T . We
17 else assume that a state not in the tree has a cost-to-come of
18 Qedge ← ∅; infinity, i.e., ∀x 6∈ V, gT (x) = ∞. Given the asymptotic
convergence of the RGG representation, these two functions
19 fmax ← gT (xg );
20 until STOP; will always bound the unknown true optimal cost to a state
21 return T ; from the start, g (·), i.e., ∀x ∈ X, gb (x) ≤ g (x) ≤ gT (x).
The functions b c (x, y) and c (x, y) represent an admissible
Algorithm 2: UpdateEdgeQueue(v ∈ V ) estimate of the cost of an edge and the true cost of an edge
1 UpdateFreeQueue (v); between states x, y ∈ X, respectively. We assume that edges
Qedge ←
−   that intersect the obstacle set have a cost of infinity and
− (u ∈ V,v)
2  (u, v) ∈ Qedge ;
otherwise ∀x, y ∈ X, b c (x, y) ≡ c (x, y). This is equivalent

3 Xn ← x ∈ n Qfree
 ||x − v||2 ≤ r ; o
+
4 Qedge ←− (v, x ∈ Xn )  g

b (v) + b
c (v, x) + b
h (x) < gT (xg ) ;
exploration of high-quality paths. In this way, we not only
reduce the computation spent searching the implicit RGG,
but also the size of the explicit shortest-path tree. This
improves performance compared to RRT* and FMT* by
elaborating only enough of the tree to find the best path
to the goal instead of to every point in the state space.
To further compress the implicit RGG, BIT* uses a just-in-
time successor function that generates explicit samples only
when necessary. Limiting the number of vertices explicit in
the RGG has the benefit of increasing the speed of related
subroutines (e.g., nearest-neighbours).
Using a heuristic to define a subplanning problem is
inspired by Anytime RRTs [34]. It focuses the remaining
search on only the subset of the problem where the optimal
solution can exist. For high-dimensional problems, direct
sampling of this subset is generally required for convergence
to the optimum as sampling a uniform distribution over any
larger volume has a probability of improving the current
solution that goes to zero as the dimension of the problem
increases [22].
A. Notation
Let T := (V, E) be an explicit tree graph with a set
of vertices, V ⊂ X, and edges, E = {(u, v)} for some
u, v ∈ V . The functions gb (x) and b h (x) represent admissi-
ble estimates of the cost-to-come to a state x ∈ X from the
start and the cost-to-go from a state to the goal, respectively.
The function fb(x) represents an admissible estimate of the
cost of a path from xs to xg constrained to pass through x,
This is a preliminary draft of work in preparation for the
2015 IEEE International Conference on Robotics and Automation (ICRA)
to delaying collision detection but otherwise knowing the
edge length and Section VII discusses the implications of
removing this assumption.
The function λ (·) represents the volume of a set (i.e., the
+
Lebesgue measure). Finally, we use the notation X ← − {x}
−
and X ← − {x} to compactly represent the compounding
operations X ← X ∪ {x} and X ← X \ {x}, respectively.
B. Algorithm
Alg. 1 presents the main algorithm, which can be run for
as long as desired and returns a tree, T := (V, E), such that
if a solution exists, xs and xg are connected through the tree
such and their path is minimized given the specific RGG. It
does this by using three queues: free states, Qfree , potential
new edges, Qedge , and potential replacement edges that may
improve the existing tree, Qrewire . Initially, the vertex set
consists of xs , and the free queue consists of xg (or samples
from Xgoal ) (Alg. 1 Lines 1–3).
For each batch, the queue of edges, Qedge , is populated
with all possible connections from the existing tree to states
in the free queue that could provide a better solution (Alg. 1
Lines 5–6). This queue is then iteratively processed (Alg. 1
Lines 7–19) to find the minimum path from xs to xg through
the current RGG that is defined by the samples in V ∪ Qfree ,
and the appropriate r given by [27].
At each iteration, we remove and process the potential
edge, (vm , xm ), from Qedge with the lowest heuristic esti-
mate of a solution for our current graph (Alg. 1 Lines 8–
9). This is the pair that minimizes the sum of the cost-to-
come through the tree, gT (vm ), the heuristic estimate of the
edge cost b c (vm , wm ) and the heuristic estimate of cost-to-
 Algorithm 5: Rewire(Qrewire ) Active vertex
1 while Qrewire 6= ∅ do n o Required nearby samples
2 (um , wm ) ← arg min gT (u) + b
c (u, w) + b
h (w) ; Volume of existing samples
(u,w)∈Qrewire
−
Shell of new samples
3 Qrewire ←− {(um , wm )};
4 if gT (um ) + b
c (um , wm ) + b
h (wm ) < gT (xg ) then
5 if gT (um ) + c (um , wm ) < gT (wm ) then
−
6 E ←− {(v ∈ V, wm ) | (v, wm ) ∈ E};
+ r
7 E← − {(um , wm )};
8 UpdateRewireQueue (wm ); v
xs xg
9 else
10 Qrewire ← ∅;

go, bh (wm ). If the minimum such edge could not improve

our current solution to the goal, gT (xg ), (Alg. 1 Line 10)
then by construction no other edge in Qedge can and we
fprev
clear the queue to generate a denser RGG (Alg. 1 Lines 17–
18). If the minimum edge could provide a better solution, freq’d
we evaluate the true cost of the edge (i.e., check the edge
for collisions, Alg. 1 Line 11) and if the resulting path can
still provide an improvement, we add it to the tree (Alg. 1
Line 12). We then remove the recently added state from Fig. 3. An illustration of just-in-time sampling.
the list of free states, Qfree , (Alg. 1 Line 13), update the Sample: Given two heuristic solution costs,
queues with potential edges originating from the newly added fprev , fsample ∈ R≥0 , and a sampling density, ρ ∈ R≥0 ,
state (Alg. 1 Lines 14–15) and process the rewirings (Alg. 1 the function Sample (fprev , fsample , ρ) returns independent
Line 16). and identically
n distributed
 (i.i.d.) samples from
o the shell
For problems seeking to minimize path length, the pro- Xshell := x ∈ X  fprev < fb(x) ≤ fsample such that

cessing order assures that rewirings will not be necessary
when processing the first batch of samples.
The subfunctions of the main algorithm are described
below:
UpdateEdgeQueue (Alg. 2): Given a vertex in the tree,
v ∈ V , the function UpdateEdgeQueue (v) updates the edge
queue, Qedge , with all possible connections from the vertex
to the nnearby free states
the number of samples, q, results in a sample density
ρ = q/λ (Xshell ). For planners seeking to minimize path
length, this is equivalent to drawing samples from the prolate
hyperspheroidal shell defined by transverse diameters fprev
and fsample and foci xs and xg [22], [35]. This can be
implemented by sampling the larger volume directly and
rejecting samples that are inside the smaller volume [22].
 that could improve the current solu- o Since in high-dimensions the majority of volume in a
tion, (v, x ∈ Xn )  gb (v) + b c (v, x) + b
h (x) < gT (xg ) ,

where Xn = {x ∈ Qfree | ||x − v||2 ≤ r}. It does this by
first calling UpdateFreeQueue (v) (Alg. 2 Line 1) to assure
that Qfree is appropriately populated. It then prunes the queue
of any edges to the given vertex (Alg. 2 Line 2) and adds
all potential edges from the given vertex that could be part
of a better solution (Alg. 2 Lines 3–4).
UpdateFreeQueue (Alg. 3): Given a vertex in the tree,
v ∈ V , the function UpdateFreeQueue (v) assures that
the volume sampled by Qfree includes the entire volume of
the near neighbourhood of v. It does this by incrementally
sampling the heuristically informed subsetn containingo the
neighbourhood of v, freq’d := maxx∈Xn fb(x) , where
Xn = {x ∈ X | ||x − v||2 ≤ r}. This is accomplished by
sampling prolate hyperspheroidal shells so that the sampling
density of the search subset remains uniform (Fig. 3). Each
time the function is called, the heuristic value to be sampled,
fsample , is checked against the previously sampled heuristic
value, fprev (Alg. 3 Lines 1–2). If it is less, we calculate the
sampling volume and density (Alg. 3 Lines 3–4) and sample
it (Alg. 3 Lines 5–6) Note, for problems seeking to minimize
path length freq’d ≤ fb(v) + 2r will always hold.
This is a preliminary draft of work in preparation for the
2015 IEEE International Conference on Robotics and Automation (ICRA)
unit-ball lies near the surface this is relatively efficient,
though exact methods are currently sought.
UpdateRewireQueue (Alg. 4): Given a vertex in the
tree, v ∈ V , the function UpdateRewireQueue (v) up-
dates the rewire queue, Qrewire , with all edges emanat-
ing from the given vertex that could improve the tree,
{(v, w ∈ Vn ) | gT (v) + b c (v, w) < gT (w)}, where Vn =
{w ∈ V | ||w − v||2 ≤ r}. It does this by adding all po-
tential edges from the given vertex to other vertices in the
tree that could lower the cost-to-come of the target vertex
(Alg. 4 Lines 1–2).
Rewire (Alg. 5): The function Rewire processes the
rewiring queue, Qrewire , to minimize the path length through
the tree from the start, xs , to every vertex in the tree, x ∈ V .
It does this by incrementally removing and processing the
potential edge, (vm , wm ), from Qrewire with the lowest
heuristic solution-cost in our current tree (Alg. 5 Lines 2–
3). This is the pair that minimizes the sum of the cost-
to-come through the tree, gT (vm ), the heuristic estimate
of the edge cost b c (vm , wm ) and the heuristic estimate of
cost-to-go, bh (wm ). If the minimum such edge could not
improve our current solution to the goal (Alg. 5 Line 4)

1.46
Solution cost
1.44
1.42
1.4
1.38
1.36
0 5 10 15
Run time [s]
RRT* Informed RRT* BIT* FMT*
Fig. 4. The median solution cost versus run time for 20 different exper-
iments on a typical random world in R2 for RRT*, informed RRT*, BIT*
with a batch size of 1000, and FMT* with n = 1000, 5000, & 10000.
The dashed line represents a median calculated from a number of solutions
between 10 and 20 and the solid line the median once all 20 runs had
found a solution with error bars denoting a non-parametric 95% confidence
interval. Note that BIT* outperforms all the other planners
then by construction no other edge in Qrewire can and we
clear the rewiring queue (Alg. 5 Lines 9–10). If it could, we
evaluate the actual cost of the edge, c (vm , wm ), (i.e., check
the edge for collisions, Alg. 5 Line 5), and if it improves the
path to wm , replace the existing incoming edge to wm with
(vm , wm ) (Alg. 5 Lines 6–7).
As discussed in Section VI, in practice we do not check
for resulting improvements in the graph, omitting Alg. 5
Line 8. This makes the rewiring algorithm at each batch an
approximation as in RRT*, but does not appear to affect the
final asymptotic optimality.
IV. A NALYSIS
Without prior knowledge of the planning problem to jus-
tify a nonuniform sampling distribution, the best solution we
can probabilistically expect from a sampling-based planner
is bounded by the properties of RGGs. Similarly, given a
specific G ∈ G near (n, r), dynamic programming provides
methods to efficiently search the graph for the optimal path
from xs to xg . It naturally follows that the analysis of
BIT* will depend on the probabilistic properties of the RGG
representation and the method used to search the graphs.
Sketches of these proofs are presented below, it is expected
that they will be expanded before final submission.
Theorem 1 (Probabilistic Completeness): The probability
that BIT* returns a solution to a given planning problem, if
a solution exists, goes to unity as the number of iterations,
i, goes to infinity.
Proof: (Sketch) For each batch, BIT* searches an r-disc
RGG that is equivalent to the graph defined by PRM* in [27]
for the same samples. Thus, the proof of its probabilistic
completeness follows from the probabilistic completeness of
PRM* (Thereom 22 in [27]) and the completeness of the
graph search performed on the RGG by BIT*.
Theorem 2 (Asymptotic Optimality): The probability that
the solution returned by BIT* is the optimal solution to a
given planning problem, if such a solution exists, goes to
unity as the total number of iterations, i, goes to infinity.
Proof: (Sketch) As in the proof of Thereom 1, we
recognize that the r-disc RGG searched by BIT* at any batch
is equivalent to the graph defined by PRM* for the same
samples. Thus, the proof of asymptotic convergence follows
This is a preliminary draft of work in preparation for the
2015 IEEE International Conference on Robotics and Automation (ICRA)
1.8
1.7
Solution cost
1.6
1.5
1.4
1.3
0 5 10 15 20 25 30 35 40 45
Run time [s]
RRT* Informed RRT* BIT* FMT*
Fig. 5. The median solution cost versus run time for 20 different
experiments on a difficult random world in R2 (Fig. 1) for RRT*, in-
formed RRT*, BIT* with a batch size of 1000, and FMT* with n =
1000, 5000, 10000, & 25000. The dashed line represents a median cal-
culated from a number of solutions between 10 and 20 and the solid line
the median once all 20 runs had found a solution with error bars denoting
a non-parametric 95% confidence interval. Note that BIT* outperforms all
the other planners
from the asymptotic optimality of PRM* (Theorem 34 in
[27]) and the ability of BIT* to find the optimal solution in
a given graph. The proof that the exact version of BIT* finds
the shortest-path tree in a graph follows from the theorems
of dynamic programming. The proof that the approximate
version (i.e., without cascaded rewiring) of BIT* also finds
the shortest-path tree should follow from proofs in [27] and
is currently being investigated.
Conjecture 1 (Probablisitc Optimal Efficiency): It is con-
jectured that for a given total number of batches, b, number
of samples per batch, n, a radius of connection in terms of the
total number of samples, r, and a heuristic, fb(·), the exact
version of BIT* is the optimally efficient method to solve
the planning problem under some probabilistic measure.
Proof: (Sketch) For the single batch case, this should
follow immediately from the proofs of A* and showing that
BIT* is equivalent to A* on an implicit RGG. It is believed
that it may be possible to make a more general statement
for the general planning problem under some appropriate
probabilistic limitations.
V. E XPERIMENTAL R ESULTS
BIT* was compared to RRT*, informed RRT* and FMT*
with various batch sizes on a series of random worlds in
R2 and R8 (Fig. 2). The worlds were randomly populated
with obstacles (axis-aligned rectangles and hyperrectangles,
respectively) such that at most a quarter of the environment
was obstructed (Fig. 1). In total, the planners were tested on
12 random worlds (6 in R2 and 6 in R8 ), with each world
being solved by the planners 20 different times with different
pseudo-random seeds1 .
In an attempt to minimize the effects of the RRT-steer
parameter on RRT* and informed RRT*, we set it equal to
the radius given in [27] for asymptotic optimality at each
iteration. This is the same calculation used by FMT* and
BIT* to define nearest neighbours. We evaluated various
samples sizes for FMT* and used batch sizes of n = 1000
in R2 and n = 5000 in R8 for BIT*.
1 The experiments were run on a MacBook Pro running Ubuntu 12.04
with a 4 GB of ram and a 2.67 GHz processor.
 3.6
Solution cost
3.4
3.2
3
2.8
0 10 20 30 40 50 60 70 80
Run time [s]
RRT* Informed RRT* BIT* FMT*
Fig. 6. The median solution cost versus run time for 20 different
experiments on a typical random world in R8 for RRT*, informed RRT*,
BIT* with a batch size of 5000, and FMT* with n = 1000, 2500, & 5000.
The dashed line represents a median calculated from a number of solutions
between 10 and 20 and the solid line the median once all 20 runs had
found a solution with error bars denoting a non-parametric 95% confidence
interval. Note that BIT* outperforms all the other planners.
These experiments demonstrated that in low dimensions
BIT* finds an initial solution in approximately the same
time as other planners, but finds solutions of equivalent
cost significantly faster (Fig. 4). This difference is even
more pronounced in worlds with narrow passages (Fig. 5).
In high dimensions, BIT* significantly outperformed the
existing algorithms in terms of time required to find an initial
solution, time required to find solutions of equivalent costs
(Fig. 6), and also in the probability of finding a solution by
a specific time (Fig. 7).
The algorithm implementations were unoptimized and
these results are preliminary; however, all algorithms share
the same primitive functions which should permit rough
relative comparisons. While optimizations will probably have
a greater effect on the slower algorithms, since BIT* ulti-
mately performs the fewest operations, the relative order of
performance should not change. It would be misleading to
present performance versus iteration as each algorithm does
a significantly different amount of work per iteration.
VI. D ISCUSSION
The BIT* algorithm can be considered to be a search
of a series of increasingly-dense r-disc RGGs. Thus, after
updating the existing tree with rewiring, we generally assume
that g (x) ≈ gT (x) holds for the entire processing of a single
Qedge (i.e., a batch). This is equivalent to solving the current
RGG while ignoring the future possibility of denser RGGs,
which helps limit the size of the queues. In practice, however,
we found two situations where the algorithm performed
better when we considered the possibility of future rewiring.
These are the addition of new edges to the edge queue
(Alg. 2, Line 4) and the addition of edges to the explicit tree
(Alg. 1, Line 11). At this time, it is not clear if the resulting
difference in performance is from a fundamental algorithmic
advantage or a limitation in our implementations.
In Section III-A we stated that we assumed that our
heuristic estimate of the edge-cost was always the true
edge cost, except for when the edge intersected obstacles,
c (·, ·) = c (·, ·) ∪ ∞. This is an acceptable assumption for
b initial solutions and rapidly converge towards the optimal so-
planning problems where the calculation of the edge cost
is not expensive. For problems where this is not true, the
This is a preliminary draft of work in preparation for the
2015 IEEE International Conference on Robotics and Automation (ICRA)
20
Number of solutions
15
10
5
0
90 0 10 20 30 40 50 60 70 80 90
Run time [s]
RRT* Informed RRT* BIT*
Fig. 7. The number of solutions from 20 different experiments on a typical
random world in R8 versus run time for RRT*, informed RRT* and BIT*
with a batch size of 5000. Note that BIT* reliably finds a solution quickly.
algorithm should be modified to consider whether edges in
Qedge could further improve a newly added vertex before
they are removed.
To exactly rewire the tree in Alg. 5, all descendents of
an updated vertex must also be evaluated for the potential
to further improve the tree as shown in Alg. 5, Line 8.
Cascading these rewirings can be expensive, and anecdo-
tal evidence suggests that the resulting computational cost
outweighs their benefit in iterative algorithms. There exists
precedence in both dynamic programming techniques [5] and
sampling-based planners [27] to limit this rewiring to only
the edges from the new vertex, and as a result we omit Line 8
from Alg. 5 in our implementation of the BIT* algorithm.
This makes the graph search performed during each batch
an approximate dynamic programming algorithm, though it
does not appear to affect the final asymptotic optimality.
In practice, there are a number of possible implementation
improvements. The heuristic can be used to remove both
samples from Qfree and vertices from V that cannot be part
of a better solution. In our experiments, we pruned Qfree but
not V . It may also be more efficient to check that the target
of a new edge (Alg. 1, Line 8) is not already in the tree than
to clear the queue of all such edges each time a new state is
added (Alg. 2, Line 2).
VII. C ONCLUSIONS & F UTURE W ORK
In this paper we define a general class of planners, Bell-
man random trees (BRTs), that generate an explicit shortest-
path tree from an implicit RGG. We use this classification
to motivate the development of an optimal anytime planning
algorithm, batch informed trees (BIT*). BIT* combines the
anytime advantages of RRT* with the ordered-processing
advantages of FMT*. It does this by iteratively processing
batches of samples, effectively searching a series of increas-
ingly dense RGGs embedded in the continuous configuration
space. The sizes of these batches can be viewed as a choice
between exploration (small batches) and exploitation (large
batches).
It uses a heuristic and dynamic programming to do this in
an efficient and principled manner that minimizes changes
to the shortest-path tree. This allows it to both quickly find
lution. By using a heuristic, it focuses its search towards the
goal and minimizes the size of the explicit tree constructed
from the implicit RGG, thus allowing it to scale more
effectively to higher dimensions than existing algorithms.
This focus also shrinks the search volume, accelerating the
increase in sample density near the optimal solution.
It is shown that the resulting algorithm is probabilistically
complete and asymptotically optimal. It is also conjectured
that under some unknown probabilistic measure it may be
the optimally efficient search for a given choice of batch
size, number of batches, connection radius, and heuristic.
BIT* performance was compared to RRT*, informed
RRT*, and various sample-densities of FMT* on a series
of random worlds in R2 and R8 . In both state spaces,
BIT* outperformed existing algorithms in terms of time
required to find solutions of equivalent quality, with the
differences becoming more pronounced in higher dimensions
and problems with narrow passages.
This manuscript is an early draft of work in preparation
for the 2015 IEEE International Conference on Robotics and
Automation (ICRA), and as a result there is a significant
amount of ongoing work not reflected in this document. This
includes experimental, analytical, and work on the document
itself. It is clear that the proofs as presented in Section IV
are insufficiently thorough, and it is is our current focus
to improve them as well as replace Conjecture 1 with a
proper theorem. It is also our intention to develop and release
BIT* in the open motion planning library (OMPL) [36] to
facilitate more accurate comparisons and to perform tests on
real planning problems (e.g., manipulation planning) as soon
as possible.
ACKNOWLEDGMENT
We would like to thank Christopher Dellin and Michael
Koval for their comments on early drafts of this work.
This research was funded by contributions from the Natu-
ral Sciences and Engineering Research Council of Canada
(NSERC) through the NSERC Canadian Field Robotics
Network (NCFRN) and the Ontario Ministry of Research
and Innovation’s Early Researcher Award Program.
R EFERENCES
[1] E. W. Dijkstra, “A note on two problems in connexion with graphs,”
Numerische Mathematik, vol. 1, no. 1, pp. 269–271, 1959.
[2] P. E. Hart, N. J. Nilsson, and B. Raphael, “A formal basis for the
heuristic determination of minimum cost paths,” IEEE Transactions
on Systems Science and Cybernetics, vol. 4, no. 2, pp. 100–107, July
1968.
[3] R. E. Bellman, “The theory of dynamic programming,” Bulletin of the
American Mathematical Society, vol. 60, no. 6, pp. 503–516, 1954.
[4] R. E. Larson, “A survey of dynamic programming computational
procedures,” IEEE Transactions on Automatic Control, vol. 12, no. 6,
pp. 767–774, December 1967.
[5] M. Likhachev, G. J. Gordon, and S. Thrun, “ARA*: Anytime A* with
provable bounds on sub-optimality,” in Advances in Neural Informa-
tion Processing Systems 16, S. Thrun, L. Saul, and B. Schölkopf, Eds.
MIT Press, 2004, pp. 767–774.
[6] M. Likhachev, D. Ferguson, G. Gordon, A. Stentz, and S. Thrun, “Any-
time dynamic A*: An anytime, replanning algorithm,” in Proceedings
of the Fifteenth International Conference on Automated Planning and
Scheduling (ICAPS), June 2005.
[7] D. Ferguson and A. Stentz, “The delayed D* algorithm for efficient
path replanning,” in Proceedings of the IEEE International Conference
on Robotics and Automation (ICRA), Apr. 2005, pp. 2045–2050.
This is a preliminary draft of work in preparation for the
2015 IEEE International Conference on Robotics and Automation (ICRA)
[8] M. Likhachev, D. Ferguson, G. Gordon, A. Stentz, and S. Thrun,
“Anytime search in dynamic graphs,” Artificial Intelligence, vol. 172,
no. 14, pp. 1613–1643, 2008.
[9] S. Koenig, M. Likhachev, and D. Furcy, “Lifelong planning A*,”
Artificial Intelligence, vol. 155, no. 1-2, pp. 93–146, 2004.
[10] A. Stentz, “The focussed D* algorithm for real-time replanning,”
in Proceedings of the International Joint Conference on Artificial
Intelligence, 1995, pp. 1652–1659.
[11] S. Koenig and M. Likhachev, “Fast replanning for navigation in
unknown terrain,” IEEE Transactions on Robotics (TRO), vol. 21,
no. 3, pp. 354–363, June 2005.
[12] D. P. Bertsekas, “Convergence of discretization procedures in dynamic
programming,” IEEE Transactions on Automatic Control, vol. 20,
no. 3, pp. 415–419, June 1975.
[13] R. E. Bellman, Dynamic Programming. Princeton University Press,
1957.
[14] T. Lozano-Pérez and M. A. Wesley, “An algorithm for planning
collision-free paths among polyhedral obstacles,” Communications of
the Association for Computing Machinery (CACM), vol. 22, no. 10,
pp. 560–570, Oct. 1979.
[15] J. Barraquand and J.-C. Latombe, “Nonholonomic multibody mo-
bile robots: controllability and motion planning in the presence of
obstacles,” in Proceedings of the IEEE International Conference on
Robotics and Automation (ICRA), vol. 3, Apr. 1991, pp. 2328–2335.
[16] K. M. Lynch and M. T. Mason, “Stable pushing: Mechanics, controlla-
bility, and planning,” The International Journal of Robotics Research
(IJRR), vol. 15, no. 6, pp. 533–556, 1996.
[17] M. Cherif, “Kinodynamic motion planning for all-terrain wheeled
vehicles,” in Proceedings of the IEEE International Conference on
Robotics and Automation (ICRA), vol. 1, 1999, pp. 317–322.
[18] B. Donald, P. Xavier, J. Canny, and J. Reif, “Kinodynamic mo-
tion planning,” Journal of the Association for Computing Machinery
(JACM), vol. 40, no. 5, pp. 1048–1066, Nov. 1993.
[19] K. Kondo, “Motion planning with six degrees of freedom by multi-
strategic bidirectional heuristic free-space enumeration,” IEEE Trans-
actions on Robotics and Automation, vol. 7, no. 3, pp. 267–277, June
1991.
[20] P. C. Chen and Y. K. Hwang, “SANDROS: a motion planner with
performance proportional to task difficulty,” in Proceedings of the
IEEE International Conference on Robotics and Automation (ICRA),
May 1992, pp. 2346–2353 vol.3.
[21] C. S. Sallaberger and G. M. D’Eleuterio, “Optimal robotic path
planning using dynamic programming and randomization,” Acta As-
tronautica, vol. 35, no. 2-3, pp. 143–156, 1995.
[22] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Informed RRT*:
Optimal incremental path planning focused through an admissible
ellipsoidal heuristic,” in Proceedings of the IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS), 2014, to appear.
arXiv:1404.2334 [cs.RO].
[23] L. E. Kavraki, P. Švestka, J.-C. Latombe, and M. H. Overmars,
“Probabilistic roadmaps for path planning in high-dimensional con-
figuration spaces,” IEEE Transactions on Robotics and Automation
(TRO), vol. 12, no. 4, pp. 566–580, 1996.
[24] S. M. LaValle and J. J. Kuffner Jr., “Randomized kinodynamic
planning,” The International Journal of Robotics Research (IJRR),
vol. 20, no. 5, pp. 378–400, 2001.
[25] D. Hsu, J.-C. Latombe, and R. Motwani, “Path planning in expansive
configuration spaces,” International Journal of Computational Geom-
etry & Applications, vol. 9, no. 4&5, pp. 495–512, 1999.
[26] D. Hsu, R. Kindel, J.-C. Latombe, and S. Rock, “Randomized kino-
dynamic motion planning with moving obstacles,” The International
Journal of Robotics Research (IJRR), vol. 21, no. 3, pp. 233–255,
2002.
[27] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal
motion planning,” The International Journal of Robotics Research
(IJRR), vol. 30, no. 7, pp. 846–894, 2011.
[28] L. Janson and M. Pavone, “Fast marching trees: a fast marching
sampling-based method for optimal motion planning in many dimen-
sions,” in Proceedings of the International Symposium on Robotics
Research (ISRR), Dec. 2013.
[29] E. N. Gilbert, “Random plane networks,” Journal of the Society for
Industrial and Applied Mathematics, vol. 9, no. 4, pp. 533–543, 1961.
[30] M. Penrose, Random Geometric Graphs, ser. Oxford Studies in
Probability, L. C. G. Rogers, Ed. Oxford University Press, 2003,
vol. 5.
[31] P. Balister, B. Bollobás, A. Sarkar, and M. Walters, “A critical constant
for the k nearest-neighbour model,” Advances in Applied Probability,
vol. 41, no. 1, pp. 1–12, 2009.
[32] F. Xue and P. R. Kumar, “The number of neighbors needed for
connectivity of wireless networks,” Wireless Networks, vol. 10, no. 2,
pp. 169–181, 2004.
[33] A. R. Wade, “Asymptotic theory for the multidimensional random on-
line nearest-neighbour graph,” Stochastic Processes and their Appli-
cations, vol. 119, no. 6, pp. 1889–1911, 2009.
[34] D. Ferguson and A. Stentz, “Anytime RRTs,” in Proceedings of the
IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS), 2006, pp. 5369–5375.
[35] M. W. Otte and N. Correll, “C-FOREST: Parallel shortest-path plan-
ning with super linear speedup,” IEEE Transactions on Robotics
(TRO), vol. 29, no. 3, pp. 798–806, June 2013.
[36] I. A. Şucan, M. Moll, and L. E. Kavraki, “The Open Motion Planning
Library,” IEEE Robotics & Automation Magazine, vol. 19, no. 4, pp.
72–82, Dec. 2012, http://ompl.kavrakilab.org.

This is a preliminary draft of work in preparation for the

2015 IEEE International Conference on Robotics and Automation (ICRA)