Efficient Sampling-Based Bottleneck Pathfinding over Cost Maps
Kiril Solovey∗and Dan Halperin∗
November 12, 2018
Abstract
We introduce a simple yet effective sampling-based planner that is tailored for bottleneck
pathfinding: Given an implicitly-defined cost map M : Rd →R, which assigns to every point
in space a real value, we wish to find a path connecting two given points, which minimizes
the maximal value with respect to M. We demonstrate the capabilities of our algorithm,
which we call bottleneck tree (BTT), on several challenging instances of the problem involving
multiple agents, where it outperforms the state-of-the-art cost-map planning technique T-
RRT*. In addition to its efficiency, BTT requires the tuning of only a single parameter:
the number of samples. On the theoretical side, we study the asymptotic properties of our
method and consider the special setting where the computed trajectories must be monotone
in all coordinates. This constraint arises in cases where the problem involves the coordination
of multiple agents that are restricted to forward motions along predefined paths.
1
Introduction
Motion planning is a widely studied problem in robotics.
In its basic form it is concerned
with moving a robot between two given configurations while avoiding collisions with obstacles.
Typically, we are interested in paths of high quality, which lead to lower energy consumption,
shorter execution time, or safer execution for the robot and its surrounding. One of the most
studied quality measures is path length, which was first studied from the combinatorial and
geometric perspective [28] and has recently gained popularity with the introduction of PRM*,
RRT* [23] and subsequent work. Another popular measure is the clearance (see, e.g, [50]) of a
path: the clearance of a particular robot configuration is the distance to the closest forbidden
configuration. The goal here is to find a path that maximizes the minimum clearance of the
configurations along it.
The latter example is a special case of the bottleneck pathfinding problem: we are given an
implicitly-defined cost map M : C →R which assigns to every configuration c ∈C of the robot a
value; the goal is to find, given start and target configurations, a continuous path ν : [0, 1] →C,
which minimizes the expression maxτ∈[0,1]{M(ν(τ))}.
Bottleneck pathfinding can also arise in settings involving multiple robots. However, in most
cases multi-robot motion planning is computationally intractable (see, e.g., [20, 38]), even when
one is only concerned with finding a solution. Thus, decoupled planners (see, e.g., [48, 37, 4])
usually construct a set of d paths—one for each robot—and attempt to coordinate between the
robots along their predefined paths in order to avoid collisions. Now suppose that we also wish
∗Kiril Solovey and Dan Halperin are with the Blavatnik School of Computer Science, Tel Aviv University,
Israel. Email: {kirilsol,danha}@post.tau.ac.il. This work has been supported in part by the Israel Sci-
ence Foundation (grant no. 1102/11), by the Blavatnik Computer Science Research Fund, and by the Hermann
Minkowski–Minerva Center for Geometry at Tel Aviv University. Kiril Solovey is also supported by the Clore
Israel Foundation.
1
arXiv:1608.00261v2  [cs.RO]  27 Sep 2016
to find the safest coordination—the one which keeps the robots in a maximal distance apart.
This problem again, can be reformulated as a bottleneck-pathfinding problem, where d is the
dimension of the effective search space. In particular, denote by σi : [0, 1] →C the path assigned
for robot i, where 1 ≤i ≤d, and define for a given d-dimensional point x = (x1, . . . , xm) ∈[0, 1]d
the cost map M(x) := 1/ max1≤i<j≤c ∥σ(xi) −σ(xj)∥. In other cases, the robots are restricted
a priori to predefined paths, as in factory assembly lines [43], aviation routes [10] and traffic
intersections [14].
An interesting complication that typically arises from the examples of the previous para-
graph, is that each robot must move forward along its path and cannot backtrack. This restric-
tion induces paths that are monotone in each of the coordinates in the search space [0, 1]d. This
is reminiscent of the notion of Fr´echet matching, which can be viewed as another instance of
bottleneck pathfinding. Fr´echet matching is a popular similarity measure between curves which
has been extensively studied by the computational-geometry community (see, e.g., [3, 18]) and
recently was used in robotics [49, 32, 19]. This matching takes into consideration not only the
“shape” of curves but also the order in which the points are arranged along them.
Contribution. Rather than tackling each problem individually, we take a sampling-based ap-
proach and develop an effective planner that can cope with complex instances of the bottleneck-
pathfinding problem. We demonstrate the capabilities of our technique termed bottleneck tree
(BTT) on a number of challenging problems, where it outperforms the state-of-the-art tech-
nique T-RRT* by several orders of magnitude. In addition to its efficiency, BTT requires the
tuning of only a single parameter: the number of samples. On the theoretical side, we study
the asymptotic properties of our method and consider the special setting where the returned
paths must be monotone in all coordinates.
Notice that the monotonicity requirement makes the problem harder, particularly in the
sampling-based setting. Consider for example a PRM G that is constructed using a connection
radius rn and n samples. By removing all the non-monotone edges from G we obtain a much
sparser graph G′: informally, G′ may retain only a 1/2d fraction of the edges of G, where
d is the dimension of the problem. Fortunately, our analysis of BTT indicates that in order
to guarantee optimality in the monotone regime, the connection radius does not have to be
drastically increased over its value in the non-monotone case of say PRM.
We have already considered sampling-based bottleneck pathfinding in a previous work [39].
However, there we employed a PRM-based approach which is far inferior to BTT. Already
when working in a four-dimensional C-space, the PRM-base approach, which seeks to explore
the entire C-space, becomes prohibitively expensive. In contrast, BTT quickly produces low-
cost solutions even in a seven-dimensional space. We also provide here stronger theoretical
analysis of our new technique for the monotone case. These differences are discussed in more
detail in Sections 5 and 6.
In Section 2 we review related work. In Section 3 we provide a formal definition of bottleneck
pathfinding. In Section 4 we describe our BTT algorithm for the problem. In Section 5 we
provide an asymptotic analysis of the method in the monotone case. In Section 6 we report on
experimental results and conclude with a discussion and future work in Section 7.
2
Related work
Particular instances of the basic motion-planning problem involving a small number of degrees
of freedom can be solved efficiently in a complete manner [36]. Complete planners can even cope
with scenarios involving multiple robots, if some assumptions are made about the input (see,
e.g., [40, 1, 47]. However, in general motion planning is computationally intractable [20, 38, 44,
34]. Thus, most of the recent efforts in this area are aimed at the development of sampling-based
planners, which attempt to capture the connectivity of the free space using random sampling.
We continue our literature review with general-purpose sampling-based planners, with an
emphasis on planners that are applicable to settings involving cost maps. We then proceed to
specific examples of bottleneck pathfinding that were studied from the combinatorial-algorithmic
perspective.
2.1
Sampling-based motion planning
Early sampling-based planners such as PRM [25] and RRT [27] have focused on finding a
solution. Although some efforts were made to understand the quality of paths produced by
such planners [33, 30, 17] this issue remained elusive until recently. In their influential work
Karaman and Frazzoli [23] introduced the planners PRM* and RRT*, which were shown to
be asymptotically optimal, i.e., guaranteed to return a solution whose cost converges to the
optimum. Several asymptotically-optimal planners that focus on the path-length cost have later
emerged [5, 22, 35]. In our recent work [41] we develop a general framework for the analysis of
theoretical properties of a variety of sampling-based planners through a novel connection with
the theory of random geometric graphs.
We now proceed to discuss sampling-based planning in the presence of an underlying cost
map M, as in bottleneck pathfinding. RRT* theoretically guarantees to converge to the op-
timum even when the cost of the path is computed with respect to M. However, in practice
this convergence is very slow, since RRT* does not take into account the cost of M while ex-
ploring C, and puts too much effort in the traversal of regions of high cost. On the other hand,
T-RRT [21] equips RRT with a transition test, which biases the exploration towards low-cost
regions of M. However, T-RRT does not take path quality into consideration and cannot im-
prove the cost of a solution that was already obtained. Consequently, this planner provides no
optimality guarantees. The T-RRT* algorithm [16] combines RRT* with T-RRT, which yields
an asymptotically-optimal planner that also works well in practice. The authors of [16] test
T-RRT* for the case of integral cost, which sums the cost of the configurations along the path,
and mechanical cost, which sums the positive cost variations along the path. We mention that
T-RRT* is also applicable to the bottleneck cost, although this setting was not tested in that
work. Lastly, we note that we are not aware of other techniques that consider planning in the
presence of a cost map in the general setting of sampling-based planning.
2.2
Bottleneck pathfinding and other problems
Cost maps typically arise as implicit underlying structures in many problems involving motion
and path planning. de Berg and van Kreveld [15] consider the problem of path planning over a
known mountainous region. They describe data structures that can efficiently obtain, given two
query points (which are the start and goal of the desired path), paths with various properties,
such as those that strictly descend and paths that minimize the maximal height obtained, i.e.,
bottleneck paths.
We have already mentioned the problem of high-clearance paths. For the simple case of a disc
robot moving amid polygonal obstacles this problem can be solved efficiently by constructing a
Voronoi diagram over the sites that consist of the obstacles [31]. There is an obvious trade-off
between clearance and path length, a short paths tend to get very close to the obstacles. The
work by Wein et al. [50] describes a data structure that given a clearance threshold δ > 0
returns the shortest path with that clearance.
Agarwal et al. [2] consider a particular cost
function suggested by Wein et al. [51] that combines path length and clearance and develop an
efficient approximation algorithm for this case.
Perhaps the most popular example of bottleneck pathfinding from the recent years is Fr´echet
matching, which is concerned with quantifying the similarity of two given curves: the Fr´echet
distance between two given curves can be described as the shortest leash that allows a person
to walk her dog, where the former is restricted to move along the first curve and latter along
the other. It is generally assumed that this quantity tends to be more informative when only
forward motions along the curves are permitted, which transform into monotone paths in the
search space. Fr´echet matching has been extensively studied for the case of two curves: several
efficient techniques that solve the problem exist (see, e.g., [3, 8]). The problem can be extended
to multiple curves although only algorithms that are exponential in the number of curves are
known [18, 9].
Several papers consider extensions of the problem involving more complex
input objects [6, 7] or cases where additional constraints such as obstacles are imposed on the
“leash” [12, 11]. Finally, we mention our recent work [39] where we apply a sampling-based
PRM-like technique for solving several Fr´echet-type problems.
3
Preliminaries
We provide a formal definition of the bottleneck-pathfinding problem. For some fixed dimension
d ≥2, let M : [0, 1]d →R be a cost map that assigns to every point in [0, 1]d a value in R.
We will refer from now on to [0, 1]d as the parameter space in order to distinguish it from the
configuration space of the underlying problem.
Notice that the definition of M does not imply that we restrict our discussion to the setting
of a single robot whose configuration space is [0, 1]d. For instance, [0, 1]d can represent the
parameter space of d curves of the form σi : [0, 1] →Ci, where for every 1 ≤i ≤d, σi describes
that motion of robot i in the configuration space Ci. Typically in such settings involving multiple
robots having predefined paths the robots are only permitted to move forward along their paths
and never backtrack. This restriction induces monotone motions in [0, 1]d. Given two points
x = (x1, . . . , xd), y = (y1, . . . , yd) ∈Rd the notation x ⪯y implies that for every 1 ≤i ≤d it
hold that xi ≤yi. Additionally, we use the notation x ⪯δ y for δ > 0 to indicate that x ⪯y
and for every 1 ≤i ≤d it hold that yi −xi ≥δ.
For simplicity we will assume that the goal consists of planning paths between the points
0, 1 ∈[0, 1]d, where 0 = (0, . . . , 0), 1 = (1, . . . , 1).
Definition 1. A plan ν : [0, 1] →[0, 1]d is a continuous path in [0, 1]d that satisfies the following
two constraints: (i) ν(0) = 0, ν(1) = 1; (ii) it is monotone in each of the d coordinates, i.e.,
for every 0 ≤τ ≤τ ′ ≤1 and ν(τ) ⪯ν(τ ′).
Given a path (or a plan) ν, its bottleneck cost is defined to be M(ν) = maxτ∈[0,1] M(ν(τ)).
In Section 6 we will address specific examples of the following problem:
Definition 2. Given a cost map M : [0, 1]d →R the (monotone) bottleneck-pathfinding problem
consists of finding a plan ν which minimizes M(ν).
4
Bottleneck-tree planner
In Algorithm 1 we describe our sampling-based technique for bottleneck pathfinding, which
we call bottleneck tree (BTT). The algorithm can be viewed as a lazy version of PRM which
explores the underlying graph in a Dijkstra-like fashion, according to cost-to-come, with respect
to the bottleneck cost over M. Thus, it bears some resemblance to FMT* [22] since the two
implicitly explore an underlying PRM. However, FMT* behaves very differently from BTT since
it minimizes the path-length cost.
Algorithm 1 bottleneck-tree(n, rn)
1: V := {0, 1} ∪sample(n);
2: for all x ∈V do
3:
c(x) := ∞; p(x) := null
4: c(0) := M(0)
5: while ∃x ∈V such that c(x) < ∞do
6:
z := get min(V )
7:
if z == 1 then
8:
return path(T, 0, 1)
9:
V := V \ {z}
10:
Nz := near(z, V, rn)
11:
for all x ∈Nz do
12:
if z ⪯x and c(z) < c(x) then
13:
cnew := max{c(z), M(z, x)}
14:
if cnew < c(x) then
15:
c(x) := cnew; p(x) := z
16: return ∅
BTT accepts the number of samples n and the connection radius rn as parameters.
It
maintains a directed minimum spanning tree T of an underlying monotone PRM or a random
geometric graph (RGG) [41]1:
Definition 3. Given n ∈N+ denote by Xn = {X1, . . . , Xn}, n points chosen independently and
uniformly at random from [0, 1]d. For a connection radius rn > 0 denote by Gn = G({0, 1} ∪
Xn; rn) the monotone RGG defined over the vertex set {0, 1} ∪Xn. Gn has a directed edged
(x, y) for x, y ∈{0, 1} ∪Xn iffx ⪯y and ∥x −y∥≤rn, where ∥· ∥represents the standard
Euclidean distance.
The algorithm keeps track of the unvisited vertices of Gn using the set V . It maintains for
each vertex x the cost-to-come over the visited portion of Gn and its parent for this specific
path, are denoted by c(x) and p(x), respectively.
In line 1 we initialize V with 0, 1, and a set of n uniform samples in [0, 1]d, which are
generated using sample. In lines 2-3 the cost-to-come and the parent attributes are initialized.
In each iteration of the while loop, which begins in line 5, the algorithm extracts the vertex
z ∈V which minimizes c(z) , using get min (line 6). If z turns out to be 1 (line 7), then a path
from 0 to 1 is returned. This is achieved using path, which simply follows the ancestors of 1
stored in the p attribute until reaching 0. Otherwise, z is removed from V (line 9) and a subset
of its neighbors in Gn that are also members of V are obtained using near (line 10). For each
such neighbor x (line 11) it is checked whether z ⪯x and whether the cost-to-come of x can
potentially improve by arriving to it from z (line 12). An alternative cost-to-come for x which
is induced by a path that reaches from z is calculated (line 13). In particular, this cost is the
maximum between the cost-to-come of z and the cost of the edge2 from z to x with respect to
M. If the alternative option improves its cost then the parents and the cost of x are updated
accordingly (line 15).
In preparation for the following section, where we study the asymptotic behavior of BTT, we
mention here that given that the underlying graph Gn contains a path from 0 to 1, BTT finds
1PRMs and RGGs are equivalent in our context.
2The cost of an edge with respect to a given cost map can be approximated by dense sampling along the edge,
as is customary in motion planning.
the minimum bottleneck path over Gn, namely the path whose maximal M value is minimized.
We omit a formal proof of this fact here. It is rather straightforward and is very similar similar
to the completeness proof of the standard Dijkstra algorithm, but for the bottleneck cost; see,
e.g., [13, Theorem 25.10].
5
Asymptotic analysis
In the previous section we have argued that BTT returns a solution that minimizes the bot-
tleneck cost over a fixed graph G ∈Gn. A major question in the analysis of sampling-based
motion-planning algorithms is under what conditions does a discrete graph structure captures
the underlying continuous space.
For this purpose, we study the asymptotic properties of
Gn = G({0, 1} ∪Xn; rn) (Definition 3). To the best of our knowledge, existing proofs concern-
ing asymptotic optimality or completeness of sampling-based planners (see, e.g., [23, 22]) only
apply to the non-monotone (and standard) setting. Such results cannot be extended as-are to
the analysis of Gn. The remainder of this section is dedicated to strengthening our our analysis
concerning the special and harder case that imposes the monotonicity requirement.
Previously [39], we were able to give a convergence guarantee in the monotone case by
introducing a connection radius (rn) of the form f(n)

log n
n
1/d
, where f(n) is any function
that diminishes as n tends to ∞. Unfortunately, this statement does not indicate which specific
function should be used in practice, and an uncareful selection of f could lead to undesired
behavior of BTT. In particular, if f grows too slowly with n the graph becomes disconnected
and BTT may not be able to find a solution at all. On the other hand, f that grows too fast
with n could result in a needlessly dense graph which will take very long time to traverse. In
this paper, we replace this somewhat abstract definition with a connection radius of the form
γ

log n
n
1/d
, where γ is a constant depending only on d. This is a more typical structure of
bounds in the context of random geometric graphs and sampling-based planners. Furthermore,
we show in Section 6 that this formulation of rn leads to a quick convergence of BTT to the
optimum.
For the purpose of the analysis, we define a supergraph of Gn, denoted by Ln = L({0, 1} ∪
Xn; rn), which corresponds to the standard and non-monotone random geometric graph (see [41]).
In particular, it connects every pair of vertices x, y ∈{0, 1} ∪Xn with an edge if and only if
∥x −y∥≤rn, regardless of whether the monotonicity constraint is satisfied.
In order to guarantee asymptotic optimality, one typically has to make some assumptions
with respect to the solution one hopes to converge to. First, we introduce basic definitions.
Denote by Br(x) the d-dimensional Euclidean ball of radius r > 0 centered at x ∈Rd and Br(Γ) =
S
x∈Γ Br(x) for any Γ ⊆Rd. Given a curve ν : [0, 1] →Rd define Br(ν) = S
τ∈[0,1] Br(ν(τ)).
Additionally, denote the image of a curve ν by Im(ν) = S
τ∈[0,1]{ν(τ)}.
Definition 4. Given M : [0, 1]d →R, a plan ν := [0, 1] →[0, 1]d is called robust if for
every ε > 0 there exists δ > 0 such that for any plan ν′ for which Im(ν) ⊂Bδ(ν) it follows that
M(ν′) ≤(1 + ε)M(ν). A plan that attains the infimum cost, over all robust plans, is termed
robustly optimal and is denoted by ν∗.
The following theorem is the main theoretical contribution of this paper.
The constant
γ, which is defined below, was first obtained in [22], for a different problem, and involving
non-monotone connections.
Theorem 1. Suppose that Ln = L(Xn ∪{0, 1}; rn) with rn = γ

log n
n
1/d
, where γ = (1 +
η)2(dθd)−1/d with θd representing the Lebesgue measure of the unit Euclidean ball, and η > 0 is
any positive constant. Then Ln contains a path νn connecting 0 to 1 which has the following
properties a.s.: (i) M(ν) = (1 + o(1))M(ν∗); (ii) at most o(1) fraction of the edges along νn
are non-monotone.
This theorem implies that Ln contains a path ν that is almost entirely monotone and its
cost converges to the optimum. Notice that in theory this does not necessarily mean that BTT
is asymptotically optimal, as the formulation of the algorithm in Alg. 1 is concerned with paths
that are entirely monotone. However, our experiments (Section 6) suggest that this theoretical
gap can be bridged.
5.1
Proof of Theorem 1
This subsection is devoted to the proof of Theorem 1. It relies on some components that were
previously described in the works of Janson et al. [22], Karaman and Frazzoli [23], and our
work [39]. However, we note that several ideas employed here are brand new and so is the final
result.
Our proof follows the common “ball-covering” argument [23, 22, 24], which shows that there
exists a collection of samples from Xn, that are located in the vicinity of “landmark” points that
lie on some plan of interest. This guarantees that Ln contains a path that has a cost similar
to the robustly optimal plan ν∗. Then we use a refined “(α, β) ball-covering” [22] argument,
which shows that many of the samples lie very closely to the landmarks—so close that most of
them form monotone subchains. This induces the existence of paths that are almost-entirely
monotone and have desirable bottleneck cost.
We start by selecting a fixed ε > 0. As ν∗is robustly optimal, there exists δ > 0 such that
for every plan ν with Im(ν) ⊆Bδ(ν∗) it follows that M(ν) ≤(1 + ε)M(ν∗). Define ℓ= 2/δ.
Similarly to the proof of Theorem 6 in [39], there exists a sequence of ℓ−1 ≤k ≤ℓpoints
Q = {q1, . . . , qk} ⊂Im(ν∗) where q1 = 0, qk = 1, and qj ⪯δ/2 qj+1 for any 1 ≤j < k. Note that
ℓis finite and independent of n. This follows from the fact that 0 ⪯1 1 and every subpath of
ν∗must “make progress” in relation to at least one of the coordinates. In particular, for every
1 ≤j ≤d define Qj = {qj
1, . . . , qj
ℓ} ⊂Im(ν∗) to be a collection of ℓpoints along ν∗with the
following property: for every 1 ≤i′ ≤ℓthe jth coordinate of qj
i′ is equal to j · 2/δ. Observe
that for any 1 ≤i′ ≤ℓthere exists 1 ≤j ≤d and 1 ≤i′′ ≤ℓsuch that 0 ⪯(j−1)·2/δ qj
i′′ and
0 ̸⪯(j−1)·2/δ+∆qj
i′′ for any ∆> 0, i.e., one of the coordinates of qj
i′′ is equal to j · 2/δ (see
Figure 1). Thus, it follows that Q ⊂Sd
j=1 Qj.
As ν∗can be arbitrarily complex we define a simplified plan, which is located in the vicinity
of ν∗, and is consequently of low cost. Firstly, for every 1 ≤j < k denote by νj,j+1 the straight-
line path from qj to qj+1 (see Figure 1). Now, define ν1,k to be the concatenation of these k −1
subpaths. Observe that ν1,k is a plan.
Fix ψ ∈(0, 1) and define r′
n :=
rn
2(2+ψ). We generate a set of Mn “landmark” points P =
{p1, . . . , pMn }, which are placed along ν1,k, i.e., P ⊂Im(ν1,k), such that for every 1 ≤i ≤Mn−1
it holds that ∥pi −pi+1∥≤r′
n. Note that p1 = 0. For simplicity we also assume that pMn = 1.
Now define a set of balls centered at the landmarks. In particular, for every 1 ≤i ≤Mn
define the set Bn,i := Br′n(pi). Additionally, denote by An the event representing that for every
1 ≤i ≤Mn it holds that Xn ∩Bn,i ̸= ∅. We have the following lemma, which is proven in [22,
Lemma 4.4]:
Lemma 1. The event An holds a.s.
Corollary 1. The graph Ln contains a path νn connecting 0 to 1 such that M(νn) ≤(1 +
ε)M(ν∗) a.s..
δ/2
δ/2
q1 = q1
1 = q2
1
q2 = q1
2 = q2
3
q2
2
q1
3
ν2,3
ν∗
ν1,2
q3 = q1
4 = q2
4
Figure 1: Visualization of the proof of Theorem 1 for d = 2. The red curve represents ν∗, on
which lie the points Q1 ∪Q2 (represented by red bullets). Observe that Q ⊂Q1 ∪Q2. The
dashed blue straight lines represent ν1,2, ν2,3.
Proof. Suppose that An holds. Then for every 1 < i < Mn there exists xi ∈Xn such that
xi ∈Bn,i.
Observe that for any 1 ≤i < Mn it holds that ∥xi −xi+1∥≤rn and so Ln
contains an edge from xi to xi+1. Thus, we can define νn to be the path consisting of the points
0, x2, . . . , xMn−1, 1.
For any 1 < i < Mn denote by p′
i a point along ν∗which is the closest to pi. Observe that
∥pi −p′
i∥≤δ/2. This, combined with the fact that ∥xi −pi∥≤r′
n, implies that ∥xi −p′
i∥≤
δ/2 + r′
n ≤δ, which then guarantees that M(xi) ≤(1 + ε)M(ν∗), by definition of ν∗. Similar
arguments can be applied to the edges of νn. Thus, M(νn) ≤(1 + ε)M(ν∗).
In order to reason about the monotonicity of νn we make the following crucial observation:
Lemma 2. There exists a fixed constant β ∈(0, 1) such that for any 1 ≤i < Mn it holds that
pi ⪯βrn pi+1.
Proof. First, assume that there exists 1 ≤j < k for which pi, pi+1 ∈Im(νj,j+1), and note
that by definition the straight line from pi to pi+1 is a subsegment of νj,j+1. Now, recall that
qj ⪯δ/2 qj+1 and ∥pi −pi+1∥= r′
n. Additionally, denote by Lj,j+1 = ∥qj −qj+1∥and note
that this value is a constant independent of n. Finally, let βj be the maximal value for which
pi ⪯βjrn pi+1 and observe that Lj,j+1
r′n
=
δ/2
βjrn . Thus, βj =
ψδ
2(2+ψ).
Now consider the case where pi ∈Im(νj,j+1), pi+1 ∈Im(νj+1,j+2). Without loss of generality,
assume that ∥pi −qj+1∥≥∥pi+1 −qj+1∥. Thus, pi ⪯βjrn/2 qj+1, which implies that pi ⪯βjrn/2
pi+1. Finally, define β = min1≤j<k{βj/2}.
Corollary 2. Suppose that pi, pi+1 ∈P for some 1 ≤i ≤Mn.
Then for any two points
xi, xi+1 ∈Xn such that xi ∈Bβ′rn(pi), xi+1 ∈Bβ′rn(pi+1), where β′ ≤β/2, it holds that
xi ⪯xi+1.
This motivates the construction of another set of balls. For every 1 ≤i ≤Mn define the
set Bβ
n,i := Bβrn/2(pi). Additionally, define Kβ
n to be the number of Bβ
n,i balls which do not
contain samples from Xn. Formally, Kβ
n :=

n
Bβ
n,i ∩Xn = ∅
oMn
i=1
. We borrow the following
lemma from [22, Lemma 4.3].
Lemma 3. For any fixed α ∈(0, 1) it holds that Kβ
n < αMn a.s.
Corollary 3. For any fixed α ∈(0, 1) the following holds a.s.: Ln contains a path νn connecting
0 to 1 such that M(νn) ≤(1 + ε)M(ν∗), and with at most 2αMn non-monotone edges.
Proof. For every 1 ≤i ≤Mn define xi ∈Xn to be some point in Xn ∩Bβ
n,i if this set is non-
empty, and otherwise some point in Bn,i. By Corollary 1, the points x1, . . . , xMn induce a path
νn such that M(νm) ≤(1 + ε)M(ν∗), and such a path exists a.s. By Corollary 2, every two
consecutive points xi, xi+1 such that xi ∈Bβ
n,i, xi+1 ∈Bβ
n,i+1 contribute one monotone edge to
νn. Conversely, each point xi ̸∈Bβ
n,i contributes at most two non-monotone edges. By Lemma 3,
there are at most αMn points of the latter type a.s.
It only remains to replace the two constants ε, α with o(1) in order to obtain the exact
formulation of Theorem 1. Since we have made no assumptions concerning these two values,
they can be replaced with the sequences εi = 1/i, αi′ = 1/i′, where i, i′ ∈N. See more details
in [42, Theorem 6].
6
Experiments
In this section we report on experimental results of applying BTT to several challenging prob-
lems. We also compare its performance with T-RRT*, which is the state-of-the-art for planning
on cost maps. Since BTT is concerned with optimality, we compare it against T-RRT* rather
than with its non-optimal original version T-RRT. Our technique is faster than T-RRT* by sev-
eral orders of magnitude. We mention that in a previous work [39] we experimented on similar
problems but with a simpler PRM-flavor technique, which is significantly slower than BTT. Due
to this fact we do not compare against it. We also note that the we compute fully monotone
paths in all the experiments reported below, using the formulation described in Alg. 1.
We implemented BTT and T-RRT* in C++, and tested them on scenarios involving two-
dimensional objects. BTT has only two parameters, which are the number of samples n and
the connection radius rn. The latter was set to the value described in Theorem 1 with η = 1.
We note that even though this theorem only suggests that this value suffices for asymptotic
optimality in the monotone case, the experiments support this claim also after finite, relatively
short, running time. In particular, BTT obtains an initial solution fairly quickly and converges
to the optimum, in scenarios where the optimum is known.
Our implementation of T-RRT* is based on its ompl [45] implementation. T-RRT* uses
a connection radius, which we set according to [23]. T-RRT* also has the parameters T, Trate,
which are set according to the guidelines in [16]. The length of the extend routine, and the
rate of target bias, were set according to the ompl implementation of T-RRT*.
Nearest-neighbor search calls in T-RRT* were performed using flann [29]. As the set of
samples of BTT is generated in one batch, we use the efficient RTG data structure (see, e.g., [26])
for nearest-neighbor search. Geometric objects were represented with cgal [46]. Experiments
were conducted on a PC with Intel i7-2600 3.4GHz processor with 8GB of memory, running a
64-bit Windows 7 OS.
(a)
(b)
0.5s
1s
4s
12s
23s
(c)
Figure 2: (a) Scenario for (P1), for d = 2: the right (blue) curve consists of five circular loops
of radius 0.15, where the entrance and exit point to each circle is indicated by a bullet. The left
red curve is similarly defined, and the two curves are separated by a vertical distance of 0.04.
The optimal matching of cost 0.34 is obtained in the following manner: when a given circle of
the red curve is traversed, the position along the blue curve is fixed to the entrance point of
the circle directly to the right of the traversed circle, and vice versa. (b) Scenario for (P2):
two agents moving along the red and blue curves, respectively, must minimize the distance
to the leader on the horizontal purple curve while maintaining visibility with the leader. (c)
Scenario for (P3): finding the safest coordination between d = 7 moving agents whose routes
are depicted as red, blue, turquoise, orange, green, black, and purple curves. The red circles on
the right represent the amount of separation obtained by BTT for the specified running time:
given a plan returned by BTT, it induces a maximal radius of a disc, such that when d copies
of this disc are centered on the moving agents, they are guaranteed to remain collision-free.
6.1
Scenarios
We test BTT on several challenging problems, which are described below. We mention that the
underlying implementation of BTT and T-RRT* does not change between the different scenarios
and we only change the subroutine responsible for the computation of cost-map values.
(P1) Fr´echet matching: The goal is to find a traversal which minimizes the pairwise distance
between the traversal points along d ∈{2, 3, 4} curves. Recall that given d ≥2 curves σ1, . . . , σd :
[0, 1] →R2, the cost map M : [0, 1]d →R+ induced by the problem of Fr´echet matching [3] is
defined to be M(τ1, . . . , τd) = max1≤i<j≤d ∥σi(τi) −σj(τj)∥for τ1, . . . , τd ∈[0, 1]. The scenario
for d = 2 is depicted in Fig. 2a. For d = 3 we add an identical blue curve, and for d = 4 another
red curve. Note that the optimum is the same for all values of d here.
(P2) Leader following: The problem consists of three curves, where the purple one (Fig. 2b)
represents the motion of a “leader”, whereas the blue and the red curves represent the “follow-
ers”. The goal is to plan the motion of the three agents such that at least one of the followers
sees the (current position of the) leader at any given time, where the view of each agent can be
obstructed by the gray walls. In addition, we must minimize the distance between the leader to
its closest follower at a given time. The optimal solution is attained in the following manner:
while the leader passes between two vertically-opposite walls the blue follower keeps an eye on
it while the red follower goes around a similar set of walls. Then, when the leader becomes
visible to the red agent, the blue agent catches up with the other two agents in an analogous
fashion, and this process is repeated.
(P3) Safest coordination: This problem consists of finding the safest coordination among
d = 7 agents moving along predefined routes, which represents a complex traffic intersection.
The goal is to find the traversal which maximizes the pairwise distance between the d traversal
points. This can be viewed as “anti”-Fr´echet coordination. Each route is drawn in a different
color, where the direction is indicated by an arrow (see Fig. 2c).
6.2
Results
We report on the running times and the obtained cost values of BTT and T-RRT*, after
averaging over ten runs in each setting.
We start with (P1). For d ∈{2, 3}, BTT rapidly converges to the optimum (see Fig. 3).
For d = 4 the convergence is slower, although a value relatively close to the optimum is reached
(observe that the trivial solution in which the curves proceed simultaneously induces a cost of
0.64). Note that BTT’s performance for d = 3 is better than that of T-RRT* for d = 2. A
similar phenomenon occurs for BTT with d = 4 and T-RRT* for d = 3. Lastly, for d = 4
T-RRT* manages to find a solution that is only slightly better than the trivial one after 60s.
In (P2) BTT attains the (near-)optimal solution described earlier after 13s on average.
Here T-RRT* is unable to obtain any solution within 60s. In (P3), again, T-RRT* is unable to
obtain a solution within 60s. In contrast, BTT obtains a solution in less than a second. Observe
that the solution obtained by it for 22 seconds maintains a rather large safety distance between
the agents in such a complex setting (see caption of Fig. 2c). Both in (P2) and (P3) T-RRT*’s
transition test leads to an extremely slow exploration of the parameter space. We mention that
in these cases RRT* is able to find an initial solution quite rapidly, albeit one of poor quality.
Moreover, the convergence rate of RRT* is very slow and hardly manages to improve over the
solution obtained initially.
Now we analyze in greater detail the running time of BTT as a function of the number of
samples n for the specific scenario in (P3). The running time is dominated by three components:
(i) construction of the RTG NN-search data structure; (ii) NN-search calls; (iii) computation
0
10
20
30
40
50
60
0.4
0.5
0.6
0.34
0.64
time [sec]
cost
T-RRT*(4)
BTT (4)
T-RRT*(3)
BTT (3)
T-RRT*(2)
BTT (2)
OPT
Figure 3: Results of BTT and T-RRT* on (P1) for increasing number of curves 2 ≤d ≤4,
indicated in parenthesis. The black dashed line represents the optimal cost 0.34. In order to
display the quality of the solution of BTT as a function of the running time, rather than the
number of samples, we ran BTT on an increasing number of samples, with ten runs for each
number. For each n we plot the average cost and the average running time. We note that the
standard deviation of the running time is very low, and so the average accurately captures the
typical running time. For instance, for d = 3 the standard deviation ranges between 0.002 and
0.01.
of the cost map, which corresponds to collision detection in standard motion planning. BTT’s
proportion of running time spent between these components changes with n (see Fig. 4). In
particular, as n increases so does the proportion of NN calls. This trend can be observed in
other planners as well [26].
7
Discussion and future work
We have introduced BTT for sampling-based bottleneck pathfinding over cost maps. We showed
that it manages to cope with complex scenarios of the problem and outperforms T-RRT* in all
tests. In addition to its efficiency, BTT requires the tuning of only a single parameter: while
the number of samples n cannot be determined a priori, the connection radius rn specified in
Theorem 1 proves to be sufficient for all tested cases. We also provide theoretical justification
for this phenomenon in the same theorem. Note that this connection radius is widely used
in non-monotone settings of motion planning (see ompl [45]). Thus, we believe that rn can
be fixed to the aforementioned value. In contrast, T-RRT* requires the tuning of two more
parameters that are unique to that planner, as well as those that are inherited from RRT and
RRT*.
We do note that BTT might benefit from biasing some of its search towards the target as
is the case in RRT-type planners. We intend to pursue this direction in future work. We also
mention that since BTT is based on Dijkstra’s shortest-path algorithm, it may be applicable to
other cost functions such as integral cost and path length, by modifying the edge cost function
(lines 12,13) and the underlying discrete path planner (line 8) in Algorithm 1.
Currently the main obstacles to using BTT in more complex settings of bottleneck pathfind-
ing, e.g., higher dimensions, is the high memory consumption of the RTG data structure. In
particular, for n = 106 and d = 7 the program may exceed the 4gb limit provided by the OS.
Interestingly, in all the tested cases BTT examines only a small portion of the actual vertex set
8000
16000
32000
64000
128000
0.6
0.8
1
number of samples
time decomposition
cost map
NN query
NN preprocessing
Figure 4: Time percentage for each of the main components of BTT for (P3).
of Gn (at most 5%). Moreover, the examined vertices are usually grouped together in contigu-
ous regions of the parameter space. This calls for incremental versions of BTT and RTG which
generate samples and preprocess them in an online fashion.
On the theoretical side, we aim to refine the statement made in Theorem 1. Currently we
can only ensure that a solution that is “almost monotone” exists, but does a “fully-monotone”
solution over Gn exists as well? We conjecture that this statement is true.
References
[1] A. Adler, M. de Berg, D. Halperin, and K. Solovey. Efficient multi-robot motion planning
for unlabeled discs in simple polygons. T-ASE, 12(4):1309–1317, 2015.
[2] P. K. Agarwal, K. Fox, and O. Salzman. An efficient algorithm for computing high-quality
paths amid polygonal obstacles. In SODA, pages 1179–1192, 2016.
[3] H. Alt and M. Godau. Computing the Fr´echet distance between two polygonal curves. Int.
J. Comput. Geometry Appl., 5:75–91, 1995.
[4] F. Altch´e, X. Qian, and A. de La Fortelle. Time-optimal coordination of mobile robots
along specified paths. CoRR, abs/1603.04610, 2016.
[5] O. Arslan and P. Tsiotras. Use of relaxation methods in sampling-based algorithms for
optimal motion planning. In ICRA, pages 2421–2428, 2013.
[6] K. Buchin, M. Buchin, and C. Wenk. Computing the Fr´echet distance between simple
polygons. Comput. Geom., 41(1-2):2–20, 2008.
[7] K. Buchin, M. Buchin, and A. Schulz. Fr´echet distance of surfaces: Some simple hard
cases. In ESA, pages 63–74, 2010.
[8] K. Buchin, M. Buchin, W. Meulemans, and W. Mulzer. Four soviets walk the dog - with
an application to Alt’s conjecture. In SODA, pages 1399–1413, 2014.
[9] K. Buchin, M. Buchin, M. Konzack, W. Mulzer, and A. Schulz. Fine-grained analysis of
problems on curves. In EuroCG, 2016.
[10] S. Cafieri and N. Durand. Aircraft deconfliction with speed regulation: new models from
mixed-integer optimization. Journal of Global Optimization, 58(4):613–629, 2014.
[11] E. W. Chambers, ´E. C. de Verdi`ere, J. Erickson, S. Lazard, F. Lazarus, and S. Thite. Ho-
motopic Fr´echet distance between curves or, walking your dog in the woods in polynomial
time. Comput. Geom., 43(3):295–311, 2010.
[12] A. F. Cook and C. Wenk.
Geodesic Fr´echet distance inside a simple polygon.
ACM
Transactions on Algorithms, 7(1):9, 2010.
[13] T. H. Cormen, C. E. Leiserson, R. L. Rivest, and C. Stein. Introduction to Algorithms (3.
ed.). MIT Press, 2009.
[14] P. Dasler and D. M. Mount. On the complexity of an unregulated traffic crossing. In
WADS, pages 224–235, 2015.
[15] M. de Berg and M. J. van Kreveld. Trekking in the alps without freezing or getting tired.
Algorithmica, 18(3):306–323, 1997.
[16] D. Devaurs, T. Sim´eon, and J. Cort´es. Optimal path planning in complex cost spaces with
sampling-based algorithms. T-ASE, 13(2):415–424, 2016.
[17] R. Geraerts and M. Overmars. Creating high-quality paths for motion planning. I. J.
Robotics Res., 26(8):845–863, 2007.
[18] S. Har-Peled and B. Raichel. The Fr´echet distance revisited and extended. ACM Trans-
actions on Algorithms, 10(1):3, 2014.
[19] R. Holladay and S. Srinivasa. Distance metrics and algorithms for task space path opti-
mization. In IROS, 2016. to appear.
[20] J. E. Hopcroft, J. T. Schwartz, and M. Sharir. On the complexity of motion planning for
multiple independent objects; PSPACE-hardness of the “Warehouseman’s problem”. I. J.
Robotic Res., 3(4):76–88, 1984.
[21] L. Jaillet, J. Cort´es, and T. Sim´eon. Sampling-based path planning on configuration-space
costmaps. Trans. Robotics, 26(4):635–646, 2010.
[22] L. Janson, E. Schmerling, A. A. Clark, and M. Pavone. Fast marching tree: A fast marching
sampling-based method for optimal motion planning in many dimensions. I. J. Robotic
Res., 34(7):883–921, 2015.
[23] S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal motion planning. I.
J. Robotic Res., 30(7):846–894, 2011.
[24] L. E. Kavraki, M. N. Kolountzakis, and J. Latombe. Analysis of probabilistic roadmaps
for path planning. In ICRA, pages 3020–3025, 1996.
[25] L. E. Kavraki, P. ˇSvestka, J.-C. Latombe, and M. H. Overmars. Probabilistic roadmaps for
path planning in high dimensional configuration spaces. Trans. Robotics, 12(4):566–580,
1996.
[26] M. Kleinbort, O. Salzman, and D. Halperin. Efficient high-quality motion planning by fast
all-pairs r-nearest-neighbors. In ICRA, pages 2985–2990, 2015.
[27] J. J. Kuffner and S. M. LaValle. RRT-Connect: An efficient approach to single-query path
planning. In ICRA, pages 995–1001, 2000.
[28] J. S. B. Mitchell. Shortest paths and networks. In Handbook of Discrete and Computational
Geometry, Second Edition., pages 607–641. 2004.
[29] M. Muja and D. G. Lowe. Fast approximate nearest neighbors with automatic algorithm
configuration. In VISSAPP, pages 331–340. INSTICC Press, 2009.
[30] O. Nechushtan, B. Raveh, and D. Halperin.
Sampling-diagram automata: A tool for
analyzing path quality in tree planners. In WAFR, pages 285–301, 2010.
[31] C. ´O’D´unlaing and C. Yap. A ”retraction” method for planning the motion of a disc. J.
Algorithms, 6(1):104–111, 1985.
[32] F. Pokorny, K. Goldberg, and D. Kragic. Topological trajectory clustering with relative
persistent homology. In ICRA, pages 16–23, 2016.
[33] B. Raveh, A. Enosh, and D. Halperin. A little more, a lot better: Improving path quality
by a path-merging algorithm. Trans. Robotics, 27(2):365–371, 2011.
[34] J. H. Reif. Complexity of the movers problem and generalizations: Extended abstract. In
FOCS, pages 421–427, 1979.
[35] O. Salzman and D. Halperin.
Asymptotically near-optimal RRT for fast, high-quality
motion planning. Trans. Robotics, 32(3):473–483, 2016.
[36] M. Sharir. Algorithmic motion planning. In J. E. Goodman and J. O’Rourke, editors,
Handbook of Discrete and Computational Geometry, Second Edition., pages 1037–1064.
Chapman and Hall/CRC, 2004.
[37] T. Simeon, S. Leroy, and J. P. Laumond. Path coordination for multiple mobile robots: a
resolution-complete algorithm. Trans. Robotics, 18(1):42–49, 2002.
[38] K. Solovey and D. Halperin. On the hardness of unlabeled multi-robot motion planning.
In RSS, 2015.
[39] K. Solovey and D. Halperin. Sampling-based bottleneck pathfinding with applications to
Fr´echet matching. In ESA, pages 76:1–76:16, 2016.
[40] K. Solovey, J. Yu, O. Zamir, and D. Halperin. Motion planning for unlabeled discs with
optimality guarantees. In RSS, 2015.
[41] K. Solovey, O. Salzman, and D. Halperin.
New perspective on sampling-based motion
planning via random geometric graphs. In RSS, 2016.
[42] K. Solovey, O. Salzman, and D. Halperin. Finding a needle in an exponential haystack:
Discrete RRT for exploration of implicit roadmaps in multi-robot motion planning. I. J.
Robotic Res., 35(5):501–513, 2016.
[43] D. Spensieri, R. Bohlin, and J. S. Carlson. Coordination of robot paths for cycle time
minimization. In CASE, pages 522–527, 2013.
[44] P. G. Spirakis and C.-K. Yap. Strong NP-hardness of moving many discs. Information
Processing Letters, 19(1):55–59, 1984.
[45] I. A. S¸ucan, M. Moll, and L. E. Kavraki. The Open Motion Planning Library. IEEE
Robotics & Automation, 19(4):72–82, 2012. http://ompl.kavrakilab.org.
[46] The CGAL Project. CGAL user and reference manual. CGAL editorial board, 4.8 edition,
2016. URL http://www.cgal.org/.
[47] M. Turpin, N. Michael, and V. Kumar. Concurrent assignment and planning of trajectories
for large teams of interchangeable robots. In ICRA, pages 842–848, 2013.
[48] J. P. van den Berg and M. H. Overmars. Prioritized motion planning for multiple robots.
In IROS, pages 430–435, 2005.
[49] C. Voss, M. Moll, and L. E. Kavraki. A heuristic approach to finding diverse short paths.
In ICRA, pages 4173–4179, 2015.
[50] R. Wein, J. P. van den Berg, and D. Halperin. The visibility-voronoi complex and its
applications. Comput. Geom., 36(1):66–87, 2007.
[51] R. Wein, J. P. van den Berg, and D. Halperin. Planning high-quality paths and corridors
amidst obstacles. I. J. Robotic Res., 27(11-12):1213–1231, 2008.