1. An Asymptotically-Optimal Sampling-Based Algorithm
for Bi-directional Motion Planning
Joseph A. Starek∗, Javier V. Gomez†, Edward Schmerling‡,
Lucas Janson§, Luis Moreno†, Marco Pavone∗
∗,‡,§Autonomous Systems Lab †Robotics Lab
∗Dept. of Aeronautics & Astronautics
‡Inst. for Computational & Math. Engin.
§Dept. of Statistics
Dept. of Systems Engineering
& Automation
Stanford University Carlos III University of Madrid
IROS 2015
September 30th, 2015
Supported by NASA STRO-ECF Grant #NNX12AQ43G

2. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Asymptotically-Optimal Sampling-Based
Planning
Asymptotic Optimality
Sample Count
Solution Cost
P lim sup
n→∞
J(σn) = J(σ∗
) = 1
=⇒ lim
n→∞
J(σn) = J(σ∗
)
1 / 17

3. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Asymptotically-Optimal Sampling-Based
Planning
FMT*
RRT*
RRT#
PRM*
Asymptotically-Optimal
BIT*
1
Gammell, Srinivasa, and Barfoot [2014]
2
Janson et al. [2015]
3
Karaman, Walter, et al. [2011]
4
Arslan and Tsiotras [2013]
5
Karaman and Frazzoli [2011] 1 / 17
1
2
3
4
5

4. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Well-Known Benefits of Bi-Directionality6
Bi-Directional Search
d µ(Xbidir-search)
µ(Xuni-search)
1 1
2 0.5
3 0.25
...
...
k 1
2
k−1
6
Pohl [1969], Luby and Ragde [1989], Hwang and Ahuja [1992]
2 / 17

5. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Well-Known Benefits of Bi-Directionality
SANDROS
RRT-Connect
Bi-dir. RRT*
SBL
BKPIECE
Bi-dir. RRT
Bi-Directional Search
7
Akgun and Stilman [2011], Jordan and Perez [2013] (unpublished)
8
Kuffner and LaValle [2000]
9
Chen and Hwang [1998]
10
S¸ucan and Kavraki [2010]
11
S´anchez and Latombe [2003] 2 / 17
7
8
9
10
11

6. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Bi-Directionality with Asymptotic Optimality
SANDROS
RRT-Connect
Bi-dir.#RRT*
SBL
BKPIECE
Bi-dir.#RRT
Bi-Directional#Search
FMT*
RRT*
RRTm
PRM*
Asymptotically-Optimal
BIT*
3 / 17

7. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Bi-Directionality with Asymptotic Optimality
? SANDROS
RRT-Connect
Bi-dir.MRRT*
SBL
BKPIECE
Bi-dir.MRRT
Bi-DirectionalMSearch
FMT*
RRT*
RRTy
PRM*
Asymptotically-Optimal
BIT*
3 / 17

8. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Bi-Directionality with Asymptotic Optimality
BFMT*
SANDROS
RRT-Connect
Bi-dir.#RRT*
SBL
BKPIECE
Bi-dir.#RRT
Bi-Directional#Search
FMT*
RRT*
RRTm
PRM*
Asymptotically-Optimal
BIT*
3 / 17

9. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Statement of Contributions
“An Asymptotically-Optimal Sampling-Based Algorithm for Bi-directional
Motion Planning”
• Introduces the Bi-Directional Fast Marching Tree
(BFMT∗) algorithm as the first AO tree-based,
bi-directional planner
4 / 17

10. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Statement of Contributions
“An Asymptotically-Optimal Sampling-Based Algorithm for Bi-directional
Motion Planning”
• Introduces the Bi-Directional Fast Marching Tree
(BFMT∗) algorithm as the first AO tree-based,
bi-directional planner
• Rigorously proves BFMT∗’s asymptotic optimality and
characterizes its convergence rate
4 / 17

11. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Statement of Contributions
“An Asymptotically-Optimal Sampling-Based Algorithm for Bi-directional
Motion Planning”
• Introduces the Bi-Directional Fast Marching Tree
(BFMT∗) algorithm as the first AO tree-based,
bi-directional planner
• Rigorously proves BFMT∗’s asymptotic optimality and
characterizes its convergence rate
• Demonstrates the numerical performance of BFMT∗ in
several path planning spaces
4 / 17

12. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
High-Level Description
The Fast Marching Tree Algorithm (FMT∗
)
Comparison/FMTstar˙Simulation.pdf
Comparison/FMTstar˙Simulation.jbig2
Comparison/FMTstar˙Simulation.jb2
5 / 17
FMT∗
(Continuous Planning)
⇐===⇒
Forward Dynamic
Programming
(Discrete Planning)
= Unexplored
= Frontier
= Interior

13. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
High-Level Description
The Fast Marching Tree Algorithm (FMT∗
)
Main Features:
• Expands a tree from xinit outward in cost-to-come space
= Unexplored
= Frontier
= Interior
6 / 17

14. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
High-Level Description
The Fast Marching Tree Algorithm (FMT∗
)
Main Features:
• Expands a tree from xinit outward in cost-to-come space
• Graph construction and search are conducted concurrently
= Unexplored
= Frontier
= Interior
6 / 17

15. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
High-Level Description
The Fast Marching Tree Algorithm (FMT∗
)
Main Features:
• Expands a tree from xinit outward in cost-to-come space
• Graph construction and search are conducted concurrently
• “Lazily” ignores the presence of obstacles
= Unexplored
= Frontier
= Interior
6 / 17

16. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
High-Level Description
The Bi-Directional Fast Marching Tree Algorithm (BFMT∗
)
Comparison/BFMTstar˙Simulation.pdf
Comparison/BFMTstar˙Simulation.jbig2
Comparison/BFMTstar˙Simulation.jb2
7 / 17
BFMT∗
(Continuous Planning)
⇐===⇒
Two-Source Dynamic
Programming
(Discrete Planning)
= Unexplored
= Frontier
= Interior

17. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
High-Level Description
The Bi-Directional Fast Marching Tree Algorithm (BFMT∗
)
Main Features:
• Generates a pair of search trees: one in cost-to-come
space from xinit and another in cost-to-go space from
xgoal
8 / 17

18. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
High-Level Description
The Bi-Directional Fast Marching Tree Algorithm (BFMT∗
)
Main Features:
• Generates a pair of search trees: one in cost-to-come
space from xinit and another in cost-to-go space from
xgoal
• Same graph construction, search, and laziness features as
FMT∗
8 / 17

19. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
High-Level Description
The Bi-Directional Fast Marching Tree Algorithm (BFMT∗
)
Main Features:
• Generates a pair of search trees: one in cost-to-come
space from xinit and another in cost-to-go space from
xgoal
• Same graph construction, search, and laziness features as
FMT∗
• Adopts performance properties of FMT∗, with the
potential exploration advantage of bi-directionality
8 / 17

20. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
BFMT∗
Properties
Bounded Convergence Rate
Let σ : [0, 1] → Xfree be a feasible path with strong
δ-clearance. For fixed > 0, an appropriate rn,12and a dense
sample set:
P[Jn > (1 + )J(σ)] = O n−η
d log− 1
d n
Asymptotic Optimality
Assume a δ-robustly feasible planning problem with optimal
path σ∗ and cost J∗. For fixed > 0:
lim
n→∞
P[Jn > (1 + )J∗
] = 0
12
sufficiently large for probabilistic exhaustivity, e.g., rn ∝ µ(Xfree)
ζd
log n
n
1
d
9 / 17

21. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Brief Discussion of BFMT∗
Variants
z := current node used for expansion
T , T := (current tree, companion tree)
xmeet := arg min
x
Cost(x, T ) + Cost(x, T )
Exploration
Alternating Swap trees (T , T ) on each iteration
Balanced
Maintain equal costs from the root within
each wavefront
Termination
First Path Stop once xmeet is defined
Best Path Stop once z is in companion tree interior
10 / 17

22. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Numerical Simulations
Open Motion Planning Library (OMPL) Test Problems
(a) SE(2) Bug Trap (b) SE(2) Maze
(c) SE(3) α-Puzzle (d) Hypercube (50% Clutter)
11 / 17

23. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Results
A Bug Trap in SE(2)-Space
BFMT
FMT
RRT
PRM
0 1 2 3 4
120
130
140
150
160
Execution Time s
SolutionCost
SE 2 Bug Trap OMPL.app
BFMT
FMT
RRT
PRM
0 1 2 3 4
0.0
0.2
0.4
0.6
0.8
1.0
Execution Time s
SuccessRate
SE 2 Bug Trap OMPL.app
12 / 17

24. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Results
A Maze in SE(2)-Space
BFMT
FMT
RRT
PRM
0 2 4 6 8
60
80
100
120
140
160
Execution Time s
SolutionCost
SE 2 Maze OMPL.app
BFMT
FMT
RRT
PRM
0 2 4 6 8
0.0
0.2
0.4
0.6
0.8
1.0
Execution Time s
SuccessRate
SE 2 Maze OMPL.app
12 / 17

25. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Results
The SE(3) ”Alpha” Puzzle
BFMT
FMT
RRT
0 20 40 60 80 100
200
300
400
500
600
700
Execution Time s
SolutionCost
SE 3 Alpha Puzzle OMPL.app
BFMT
FMT
RRT
0 20 40 60 80 100
0.0
0.2
0.4
0.6
0.8
1.0
Execution Time s
SuccessRate
SE 3 Alpha Puzzle OMPL.app
12 / 17

26. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Results
Cluttered Unit Hypercubes
BFMT*
FMT*
0.0 0.5 1.0 1.5 2.0
1.4
1.6
1.8
2.0
2.2
Execution Time *s)
SolutionCost
5D Point Robot, 50B Obstacle Coverage
(a) 5D, 50% coverage
BFMT*
FMT*
0 10 20 30 40
2.0
2.2
2.4
2.6
2.8
3.0
SolutionCost
10D Point Robot, 50g Obstacle Coverage
Execution Time Ms*
(b) 10D, 50% coverage
(all success rates were 100%)
13 / 17

27. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Conclusions
Synopsis
1. Introduced the BFMT∗ algorithm as a bi-directional
extension of the FMT∗ algorithm for fast sampling-based
path planning
2. Enabled bi-directionality without compromising FMT∗’s
asymptotic-optimality or convergence-rate guarantees
3. Demonstrated that BFMT∗ tends to an optimal solution
at least as fast as its state-of-the-art counterparts
14 / 17

28. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Concluding Remarks: Generalizations
Asymptotically-Optimal BFMT∗
Extensions
Non-Uniform Sampling:
• Ensure sampling density over the configuration space is
lower-bounded by a positive number
• Increase the connection radius rn by some constant factor
General Metrics/Line-Integral Costs:
• Use cost balls instead of Euclidean balls when making
connections
15 / 17

29. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Concluding Remarks: BFMT∗
in Practice
• Reachable volumes play a
significant role in FMT∗ and
BFMT∗ execution time
• Shows less improvement in
maze-like scenarios
• Cannot improve FMT∗’s ability
to escape bug traps
16 / 17

30. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Concluding Remarks: Availability
• FMT∗ is available within the Open Motion Planning
Library (OMPL):13
Open-Source Code for FMT∗
http://ompl.kavrakilab.org/planners.html
• A polished, open-source version of BFMT∗ is under
development
13
Ioan A. S¸ucan, Mark Moll, Lydia E. Kavraki, The Open Motion Planning
Library, IEEE Robotics & Automation Magazine, 19(4):72–82, December
2012. DOI: 10.1109/MRA.2012.2205651 [PDF] http://ompl.kavrakilab.org
17 / 17

31. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Introduction
Contributions
Algorithm
Description
Simulations
Conclusions
Thank you!
Joseph A. Starek, Javier V. Gomez, et. al.
Aeronautics & Astronautics,
ICME, Dept. of Statistics
Systems Engineering
& Automation
Stanford University Carlos III University of Madrid
jstarek@stanford.edu
17 / 17

32. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Problem Definition
path: σ : [0, 1] → X
n: Number of samples
σn: Solution returned by
planner
arc-length cost:
J(σn) =
N−1
i=1
||xi+1 − xi ||2
Optimal Path Planning
Given (Xfree, xinit, xgoal) and J : Σ → R+, find a feasible
path σ∗ ∈ Σ such that J(σ∗
) = min
σ
[J(σ) | σ is feasible]. If
none exists, report failure.
1 / 4

33. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
BFMT∗
Pseudocode
Expand Step
= Unexplored
= Frontier
= Interior
Constructing σ∗ from
Connection xmeet
= Unexplored
= Frontier
= Interior
2 / 4

34. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
BFMT∗
Pseudocode
Expand Step
= Unexplored
= Frontier
= Interior
Constructing σ∗ from
Connection xmeet
= Unexplored
= Frontier
= Interior
2 / 4

35. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Additional Simulations
Open Motion Planning Library (OMPL) Test Problems
(a) SE(2) Random Polygons (b) SE(2) Unique Maze
(c) SE(3) Easy (d) SE(3) Office Cubicles
3 / 4

36. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Additional Results
Random 2D Polygons in SE(2)-Space
BFMT
FMT
RRT
PRM
0 1 2 3 4 5 6
110
115
120
125
Execution Time s
SolutionCost
SE 2 Random Polygons OMPL.app
BFMT
FMT
RRT
PRM
0 1 2 3 4 5 6
0.0
0.2
0.4
0.6
0.8
1.0
Execution Time s
SuccessRate
SE 2 Random Polygons OMPL.app
4 / 4

37. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Additional Results
A Unique-Solution Maze in SE(2)-Space
BFMT
FMT
RRT
PRM
0 2 4 6 8
260
280
300
320
340
360
Execution Time s
SolutionCost
SE 2 Unique Solution Maze OMPL.app
BFMT
FMT
RRT
PRM
0 2 4 6 8
0.0
0.2
0.4
0.6
0.8
1.0
Execution Time s
SuccessRate
SE 2 Unique Solution Maze OMPL.app
4 / 4

38. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Additional Results
An Easy Problem in SE(3)-Space
BFMT
FMT
RRT
PRM
0 1 2 3 4 5 6 7
200
210
220
230
240
250
Execution Time s
SolutionCost
SE 3 Easy OMPL.app
BFMT
FMT
RRT
PRM
0 1 2 3 4 5 6 7
0.0
0.2
0.4
0.6
0.8
1.0
Execution Time s
SuccessRate
SE 3 Easy OMPL.app
4 / 4

39. An AO
Bi-directional
Planning
Algorithm
J. Starek, J.
Gomez, E.
Schmerling, L.
Janson, L.
Moreno, M.
Pavone
Additional Results
Navigating Office Cubicles in SE(3)-Space
BFMT
FMT
RRT
PRM
0 5 10 15
1600
1800
2000
2200
2400
2600
2800
Execution Time s
SolutionCost
SE 3 Cubicle Maze OMPL.app
BFMT
FMT
RRT
PRM
0 5 10 15
0.0
0.2
0.4
0.6
0.8
1.0
Execution Time s
SuccessRate
SE 3 Cubicle Maze OMPL.app
4 / 4