Fast marching tree: A fast marching sampling-based method for optimal motion planning in many dimensions

Abstract

In this paper we present a novel probabilistic sampling-based motion planning algorithm called the Fast Marching Tree algorithm (FMT*). The algorithm is specifically aimed at solving complex motion planning problems in high-dimensional configuration spaces. This algorithm is proven to be asymptotically optimal and is shown to converge to an optimal solution faster than its state-of-the-art counterparts, chiefly PRM* and RRT*. The FMT* algorithm performs a ‘lazy’ dynamic programming recursion on a predetermined number of probabilistically drawn samples to grow a tree of paths, which moves steadily outward in cost-to-arrive space. As such, this algorithm combines features of both single-query algorithms (chiefly RRT) and multiple-query algorithms (chiefly PRM), and is reminiscent of the Fast Marching Method for the solution of Eikonal equations. As a departure from previous analysis approaches that are based on the notion of almost sure convergence, the FMT* algorithm is analyzed under the notion of convergence in probability: the extra mathematical flexibility of this approach allows for convergence rate bounds—the first in the field of optimal sampling-based motion planning. Specifically, for a certain selection of tuning parameters and configuration spaces, we obtain a convergence rate bound of order O(n ^−1/d+ρ), where n is the number of sampled points, d is the dimension of the configuration space, and ρ is an arbitrarily small constant. We go on to demonstrate asymptotic optimality for a number of variations on FMT*, namely when the configuration space is sampled non-uniformly, when the cost is not arc length, and when connections are made based on the number of nearest neighbors instead of a fixed connection radius. Numerical experiments over a range of dimensions and obstacle configurations confirm our theoretical and heuristic arguments by showing that FMT*, for a given execution time, returns substantially better solutions than either PRM* or RRT*, especially in high-dimensional configuration spaces and in scenarios where collision-checking is expensive.

Keywords

Robots motion planning path planning for manipulators manipulation manipulation planning motion control mechanics design and control

Get full access to this article

View all access options for this article.

References

Alterovitz

Patil

Derbakova

(2011) Rapidly-exploring roadmaps: Weighing exploration vs. refinement in optimal motion planning. In: IEEE conference on robotics and automation (ICRA 2011), Shanghai, China, 9–13 May 2011, pp. 3706–3712. Piscataway: IEEE Press.

Amato

Bayazit

Dale

Jones

Vallejo

(1998) Choosing good distance metrics and local planners for probabilistic roadmap methods. In: IEEE conference on robotics and automation (ICRA 1998), Volume 1, Leuven, Belgium, 16–21 May 1998, pp. 630–637. Piscataway: IEEE Press.

Arslan

Tsiotras

(2013) Use of relaxation methods in sampling-based algorithms for optimal motion planning. In: IEEE conference on robotics and automation (ICRA 2013), Karlsruhe, Germany, 6–10 May 2013, pp. 2421–2428. Piscataway: IEEE Press.

Arya

Mount

(1995) Approximate range searching. In: 11th annual symposium on computational geometry, Vancouver, Canada, 5–7 June 1995, pp. 172–181. New York: ACM Press.

Barraquand

Kavraki

Motwani

Latombe

J-C

T-Y

Raghavan

(1997) A random sampling scheme for path planning. International Journal of Robotics Research 16(6): 759–774.

Bertsekas

(2005) Dynamic Programming and Optimal Control. Belmont, MA: Athena Scientific.

Bezanson

Karpinski

Shah

Edelman

(2012) Julia: A fast dynamic language for technical computing. http://arxiv.org/abs/1209.5145.

Bohlin

Kavraki

(2000) Path planning using lazy PRM. In: IEEE conference on robotics and automation (ICRA 2000), San Francisco, USA, 24–28 April 2000, pp. 521–528. Piscataway: IEEE Press.

Cabello

Jejčič

(2014) Shortest paths in intersection graphs of unit disks. http://arxiv.org/abs/1402.4855.

10.

Chan

Efrat

(2001) Fly cheaply: On the minimum fuel consumption problem. Journal of Algorithms 41(2): 330–337.

11.

Cormen

Leiserson

Rivest

Stein

(2001) Introduction to Algorithms. Cambridge: MIT Press.

12.

Ding

König

(2011) Fast set intersection in memory. Proceedings of the VLDB Endowment 4(4): 255–266.

13.

Gammell

Srinivasa

Barfoot

(2014) BIT^*: Batch informed trees for optimal sampling-based planning via dynamic programming on implicit random geometric graphs. http://arxiv.org/abs/1405.5848v2.

14.

Hsu

(2000) Randomized single-query motion planning in expansive spaces. PhD Thesis, Stanford University, USA.

15.

Hsu

Latombe

J-C

Kurniawati

(2006) On the probabilistic foundations of probabilistic roadmap planning. International Journal of Robotics Research 25(7): 627–643.

16.

Hsu

Latombe

J-C

Motwani

(1999) Path planning in expansive configuration spaces. International Journal of Computational Geometry & Applications 9(4–5): 495–512.

17.

Jaillet

Siméon

(2004) A PRM-based motion planner for dynamically changing environments. In: IEEE conference on robotics and automation (ICRA 2004), New Orleans, USA, 26 April–1 May 2004, pp. 1606–1611. Piscataway: IEEE Press.

18.

Karaman

Frazzoli

(2011) Sampling-based algorithms for optimal motion planning. International Journal of Robotics Research 30(7): 846–894.

19.

Kavraki

Kolountzakis

Latombe

J-C

(1998) Analysis of probabilistic roadmaps for path planning. IEEE Transactions on Robotics and Automation 14(1): 166–171.

20.

Kavraki

Svestka

Latombe

J-C

Overmars

(1996) Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation 12(4): 566–580.

21.

Kobilarov

(2012) Cross-entropy motion planning. International Journal of Robotics Research 31(7): 855–871.

22.

Koenig

Likhachev

Furcy

(2004). Lifelong planning A. Artificial Intelligence 155(1): 93–146.

23.

Ladd

Kavraki

(2004) Measure theoretic analysis of probabilistic path planning. IEEE Transactions on Robotics and Automation 20(2): 229–242.

24.

LaValle

(2006) Planning Algorithms. Cambridge: Cambridge University Press.

25.

LaValle

Kuffner

(2001) Randomized kinodynamic planning. International Journal of Robotics Research 20(5): 378–400.

26.

Marble

Bekris

(2012) Towards small asymptotically near-optimal roadmaps. In: IEEE conference on robotics and automation (ICRA 2012), St Paul, USA, 14–18 May 2012, pp. 2557–2562. Piscataway: IEEE Press.

27.

Penrose

(2003) Random Geometric Graphs. Oxford: Oxford University Press.

28.

Phillips

Bedrossian

Kavraki

(2004) Guided expansive spaces trees: A search strategy for motion- and cost-constrained state spaces. In: IEEE conference on robotics and automation (ICRA 2004), New Orleans, USA, 26 April–1 May 2004, pp. 3968–3973. Piscataway: IEEE Press.

29.

Plaku

Bekris

Chen

Ladd

Kavraki

(2005) Sampling-based roadmap of trees for parallel motion planning. IEEE Transactions on Robotics 2(4): 597–608.

30.

Roditty

Segal

(2011) On bounded leg shortest paths problems. Algorithmica 59(4): 583–600.

31.

Salzman

Halperin

(2014) Asymptotically near-optimal motion planning using lower bounds on cost. http://arxiv.org/abs/1403.7714.

32.

Sánchez

Latombe

J-C

(2003) A single-query bi-directional probabilistic roadmap planner with lazy collision checking. In: Jarvis

Zelinsky

(eds) Robotics Research. Berlin: Springer, pp. 403–417.

33.

Schmerling

Janson

Pavone

(2014) Optimal sampling-based motion planning under differential constraints: the drift case with linear affine dynamics. http://arxiv.org/abs/1405.7421/.

34.

Schmerling

Janson

Pavone

(2015) Optimal sampling-based motion planning under differential constraints: the driftless case. In: IEEE conference on robotics and automation (ICRA 2015), Seattle, USA, 26–30 May 2015.

35.

Sethian

(1996) A fast marching level set method for monotonically advancing fronts. Proceedings of the National Academy of Sciences 93(4): 1591–1595.

36.

Sniedovich

(2006) Dijkstra’s algorithm revisited: the dynamic programming connexion. Control and Cybernetics 35(3): 599–620.

37.

Starek

Schmerling

Janson

Pavone

(2014) Bidirectional Fast Marching Trees: An optimal sampling-based algorithm for bidirectional motion planning. Technical report, Stanford University, USA.

38.

Şucan

Moll

Kavraki

(2012) The Open Motion Planning Library. IEEE Robotics & Automation Magazine 19(4): 72–82.

39.

Thrun

Burgard

Fox

(2005) Probabilistic Robotics. Cambridge: MIT Press.

40.

Valero-Gomez

Gomez

Garrido

Moreno

(2013) The path to efficiency: Fast marching method for safer, more efficient mobile robot trajectories. IEEE Robotics & Automation Magazine 20(4): 111–120.