Landmark Guided Probabilistic Roadmap Queries
A landmark based heuristic is investigated for reducing query phase run-time of the probabilistic roadmap () motion planning method. The heuristic is generated by storing minimum spanning trees from a small number of vertices within the graph and using these trees to approximate the cost of a shortest path between any two vertices of the graph. The intermediate step of preprocessing the graph increases the time and memory requirements of the classical motion planning technique in exchange for speeding up individual queries making the method advantageous in multi-query applications. This paper investigates these trade-offs on graphs constructed in randomized environments as well as a practical manipulator simulation. We conclude that the method is preferable to Dijkstra’s algorithm or the algorithm with conventional heuristics in multi-query applications.
The probabilistic roadmap ()  is a cornerstone of robot motion planning. It is widely used in practice or as the foundation for more complex planning algorithms. The method is divided into two phases: the graph is first constructed followed by, potentially multiple, shortest path queries on this graph to solve motion planning problems. For a single motion planning query, a feasibility checking subroutine executed repeatedly during construction dominates run-time. However, once the is constructed it can be reused for multiple motion planning queries or modified slightly according to minor changes in the environment. Applicability to multi-query problems is one of the advantages of the over tree-based planners such as Rapidly exploring Random Trees ()  and Expansive Space Trees ()  which are tailored to single-query problems.
Recent efforts have focused on fine tuning various aspects of PRM-based motion planning for real-time applications. Highly parallelized feasibility checking using FPGAs was recently developed in  to alleviate this computational bottleneck during the construction phase. The sparse roadmap spanner was introduced in  to reduce memory required to store the and speed up the query phase by keeping only a sparse subgraph with near-optimality properties.
In this paper we examine the effectiveness of a landmark based admissible heuristic for reducing the running time of the query phase of the . The landmark heuristic was originally developed for vehicle routing problems in road networks  where many shortest path queries are solved on a single graph. In theory, any amount of time spent preprocessing the graph is negligible in comparison to the time spent solving shortest path queries if sufficiently many queries must be solved. This observation suggests solving the all-pairs shortest path problem in order to answer each routing problem in constant time with respect to graph size. However, the memory required to store a solution to the all-pairs shortest is prohibitive for large road networks. The landmark heuristic provides a trade-off between memory requirements and query times by solving a small number of single-source shortest path problems and using their solutions to construct an effective heuristic for a particular graph.
This investigation is inspired by the similarities between road networks and the ; multiple path queries are solved on both graphs and both graphs are, in practice, too large to store an all pairs shortest path solution in memory.
A useful feature of the landmark heuristic is that it can be used together with the sparse roadmap spanner and FPGA-based collision checking for a compounded speedup over a standard implementation. Based on the results presented in this paper, we conclude that the landmark heuristic is effective on graphs; solving shortest path queries as much as 20 times faster than Dijkstra’s algorithm and twice as fast as the Euclidean distance-based heuristic in cluttered environments. The downside to the approach is that constructing the heuristic requires preprocessing the graph which adds to the computation time required before the PRM can be used for motion planning queries.
An overview of the motion planning problem is presented in Section II, followed by a review of the build and query phases of the method. Section III introduces the landmark heuristic, discusses its admissibility and the complexity of its construction, and illustrates its utility with a simple shortest path problem. However, to better understand the effectiveness of the landmark heuristic in general, we construct randomized environments with a quantifiable degree of clutter and run numerous motion planning queries on these environments to obtain the average case performance. The environment construction and experimental results are presented in Section IV. In Section V we evaluate the landmark heuristic on a simulation of the Kinova Jaco robotic manipulator and find the landmark heuristic to be effective on realistic robot models. Lastly, we conclude with a discussion of our experimental observations in Section VI.
Ii Motion Planning Problem
The following optimal motion planning problem will be addressed: Let be an open, bounded subset of , and the set of continuous curves from to . Then let be the subset of whose image is contained in . The cost objective is a function that assigns a cost to each curve in . The cost function must be additive in the sense that if two curves satisfy then .
An individual motion planning query on consists of finding a curve from an initial state to a goal state . That is, and . The subset of curves in which satisfy these additional endpoint constraints are denoted . In addition to finding a curve in , we would like a curve which approximately minimizes the cost objective,
for a fixed . An approximate minimization is often used for two reasons: the first is that the problem may not admit a minimum, and second, without further assumptions on the cost objective and geometry of there are no practical techniques available for obtaining exact solutions when they exist.
Ii-a Probabilistic Roadmaps
The set has infinite dimension so the conventional approach to obtaining approximate solutions to motion planning problems is to construct a graph on whose vertices are points in . To avoid confusion with curves on , a path is a sequence of vertices in a graph such that is an edge in the graph. Curves in are approximated using paths in the graph by associating each edge of the graph with the line segment between the two vertices making up that edge. The method falls into this category of approximations to .
The method  is a popular variation of the because it generates a sparse graph with the following property: if and belong to a connected subset of , then for any fixed , the probability that the graph contains a curve satisfying
converges to as the number of vertices is increased.
Ii-B Graph Construction Phase
The construction phase of the method is summarized in Algorithm 1. The subroutine returns the points such that . The subroutine in Algorithm 1 returns a randomly sampled point from the uniform distribution supported on . The subroutine returns true if the line segment connecting to is an element of and false otherwise. In reference to line of Algorithm 1, is the Legesgue measure on , and is the ball of radius centered at .
Ii-C Motion Planning Query Phase
After construction, paths in the graph can be used to solve motion planning queries. One subtlety is that the probability of and being present in the graph is zero. There are a number of practical ways to resolve this issue, but to keep the exposition as concise as possible we will simply select the nearest vertex to and to as an approximation in light of (2).
Once initial and final states and are selected, the motion planning query reduces to a shortest path problem on the graph with edge weights determined by the cost of the line segments between vertices of the graph.
Algorithm 2 summarizes the algorithm for finding a shortest path in the graph from to . The function is used to keep track of the shortest path from to each vertex examined by the algorithm. Initially, parent maps all vertices of the graph to , but is redefined in each iteration of the algorithm as shorter paths from to vertices in the graph are found. The function maps each vertex to the cost of the shortest known path reaching that vertex from . The function initially maps all vertices to , but is updated at each iteration with the cost of newly discovered paths.
A set of vertices represents a priority queue. The distinguishing feature of the algorithm is the ordering of vertices in the priority queue according to the labeled cost of the vertex plus a heuristic estimate of the remaining cost to reach the goal . The subroutine returns a vertex such that
The heuristic is called admissible if it never overestimates the cost to reach the goal from a particular vertex. The algorithm is guaranteed to return the shortest path from if the heuristic in equation (3) is admissible.
The pathToRoot subroutine returns the sequence of vertices , terminating at , generated by the recursion
If pathToRoot is evaluated in Algorithm 2, then its output is a shortest path from to .
For graphs with nonnegative edge-weights the heuristic for all is clearly admissible. In this special case, the algorithm is equivalent to Dijkstra’s algorithm. However, the more closely underestimates the optimal cost from each vertex to the fewer iterations required by the algorithm to find the shortest path from to . Therefore, it is desirable to use a heuristic which estimates the optimal cost to reach the goal as closely as possible.
When the cost functional is simply the length of the path, as in equation (5), the canonical heuristic is the Euclidean distance between to which is the length of the optimal path in the absence of obstacles.
The Euclidean distance heuristic is specific to shortest path objectives, and may not be admissible for cost functionals other than (5).
Iii The Landmark Heuristic
The landmark heuristic is tailored to a particular graph and requires preprocessing the graph before it can be used in the algorithm. The resulting heuristic is admissible regardless of cost functional and environment making it a very general approach to obtaining an admissible heuristic.
The idea behind the landmark heuristic is as follows: Let be the function which returns the cost of a shortest path from one vertex of the graph to another; taking the value if no path exists. It follows from the definition that satisfies the triangle inequality. Consider a vertex that will represent a landmark. Rearranging the triangle inequality with and yields
Thus, the left hand side of (6) is a lower bound on cost of the shortest path to . While computing explicitly would require solving the all-pairs shortest path problem, only the solution to the single-source shortest path problem from is required to evaluate (6).
When the lower bound in (6) is evaluated at a vertex that lies on or near to the shortest path from to or vice-versa it provides a surprisingly close estimate of the minimum cost path from to . Figure 1 illustrates this lower bound. However, obtaining an effective heuristic for all origin-destination pairs requires having a collection of landmarks . The landmark heuristic then leverages (6) for each landmark:
To simplify the analysis presented in this paper, each landmark is an i.i.d. random variable selected from the uniform distribution on . However, other selection rules can be used to improve the heuristic.
Iii-a Complexity of the Landmark Heuristic
Generating the function for an individual landmark requires solving a single-source shortest path problem which can be accomplished with Dijkstra’s algorithm in time111This assumes the graph is constructed using Algorithm 1 which has edges . where denotes the cardinality of a set. Thus, the time complexity of constructing the heuristic is in . From this observation it is clear that this heuristic is only useful in instances where the number of motion planning queries that will be evaluated on the graph will be greater than since this many shortest path queries can be solved in the time required to construct the heuristic. Then evaluating the landmark heuristic (7) requires looking up the optimal cost to a landmark times so the complexity of (7) is linear in the number of landmarks.
Storing the cost of the shortest path to each vertex from a landmark for use in (7) requires memory per landmark for a total memory requirement in .
The next question is how many landmarks should be used? A natural choice is to select a fixed fraction of the vertices to be landmarks. That is, for some constant . This results in space required to store the heuristic’s lookup tables in memory. However, with just than 16 landmarks, the landmark heuristic has been observed to speed up routing queries by a factor of 9 to 16 on city to continent-scale road networks. On a with a shortest path objective, this observation can be made precise as stated in the next result.
If the number of landmarks relative to the number of vertices is given by for , then
The proof can be found in the appendix. With increasing graph size and an arbitrarily small fraction of vertices assigned to landmarks, the landmark heuristic will converge to the solution of the all-pairs shortest path problem.
Iii-B Demonstration of the Landmark Heuristic
To demonstrate the advantages of using the landmark heuristic, it was compared with Dijkstra’s algorithm and with the Euclidean distance heuristic in a bug-trap environment. A was constructed in the bug trap environment according to Algorithm 1 with a density of vertices per unit area for a total of vertices. The Landmark heuristic was then constructed with landmarks ( of vertices) obtained by randomly sampling from the vertices of the graph.
Figure 2 shows the environment and vertices expanded by the algorithm using Euclidean distance as a heuristic and the landmark heuristic. The algorithm with Euclidean distance heuristic required iterations and to find the shortest path; a marginal difference in performance in comparison to the iterations and required by Dijkstra’s algorithm. In contrast, the algorithm with the landmark heuristic required only iterations and to find the shortest path.
The results of this demo can be reproduced with the implementation of the landmark heuristic available in .
Iv Evaluation in Randomized Environments
Environments with randomly placed obstacles provides a simple and easily reproducible benchmark for motion planning algorithms [9, 10]. In this paper, the degree of clutter in these randomly generated environments is quantified as the probability of the line segment connecting two randomly sampled points being contained in .
Iv-a Environment Generation
A Poisson forest with intensity of circular obstacles with radius is used as a random environment This is simulated over a sample window by sampling the number of obstacles from the Poisson distribution
and then placing these obstacles randomly by sampling from uniform distribution on . The subset of occupied by the circular obstacles is denoted . Then we select . Embedding in simplifies subsequent calculations by eliminating boundary effects of the sample window.
Let and be independent random variables with the uniform distribution on , and let clear denote the event that the line segment connecting and remains in .
The next derivation relates the obstacle intensity to the marginal probability . Observe that a line segment intersects a circular obstacle of radius if and only if the circle of radius swept along this line segment contains the obstacle center. If the obstacle is placed by sampling from the uniform distribution on , the probability of collision is simply the ratio of the swept area of the circle along the line segment and the area of . Thus, conditioned on the number of obstacles and the points , the probability of clear is
In all of the numerical experiments of the next section random environments with obstacle radius were used.
Iv-B Numerical Experiments
Experiments were designed to evaluate how the effectiveness of the landmark heuristic varies with with the parameter and to validate Lemma 1. To facilitate obtaining the results in a reasonable time, experiments were run in parallel on the central high-performance cluster EULER (Erweiterbarer, Umweltfreundlicher, Leistungsfähiger ETH-Rechner) of ETH Zürich. Each compute node consists of two 12-Core Intel Xeon E5-2680 processors with clock rates varying between 2.5-3.5 GHz.
In the first set of trials a single random environment was sampled with . Three graphs were constructed on this environment with 40,000, 60,000 and 80,000 vertices. On each , 700 landmark heuristics were constructed, 100 each for landmark quantities . Then for each landmark heuristic, a random shortest path query is solved using with the landmark heuristic.
In the second set of trials, 20 logarithmically spaced values for the parameter from to were selected. For each of these values random environments were generated according to the construction outlined in Section IV-A. A with 100,000 vertices per unit area was constructed on each environment with vertices per unit area. Then for each , the landmark heuristics were constructed with landmark quantities . Finally, for each of the landmark heuristics, shortest path queries were evaluated on each using .
The first experiment, summarized in Figure 4, revealed how the effectiveness of the landmark heuristic varied with the fraction of vertices assigned to landmarks as well as with varying graph sizes. We observed a rapid reduction in iterations required to find a solution relative to Dijkstra’s algorithm with just of vertices assigned to landmarks. Secondly, the number of iterations required to find a solution with relative to that of Dijkstra’s algorithm decreased with increasing graph size. This validates Lemma 1 since the number of iterations required by decreases with an improving estimate of the optimal cost to reach the goal.
In the second experiment we observed that the effectiveness of the Euclidean distance heuristic rapidly diminishes with increasing clutter, while the the landmark heuristic was much less sensitive to . This is summarized in Figure 5 where the landmark heuristic reduced the number of iterations required to find a solution by a factor greater than 20 in highly cluttered environments whereas the Euclidean distance heuristic reduced the number of iterations by less than a factor of 3.
This experiment also showed the diminishing returns of increasing the number of landmarks in terms of iteration time. Recall that evaluating the landmark heuristic in (7) required checking the triangle equality for each landmark. In Figure 6 the average running time of the algorithm with the landmark heuristic reaches a minimum with with 50 landmarks.
V Robot Manipulator Example
To demonstrate suitability of the landmark-based heuristic for realistic manipulator models, we use a model of the six degree of freedom Jaco manipulator by Kinova Robotics. To simulate a complex planning task the arm must find a collision free motion through a window terminating with the end effector near the ground to simulate reaching for an object.
The landmark heuristic was implemented in the Open Motion Planning Library (OMPL)  and the problem was solved using the MoveIt  software tool. Two planning objectives were considered for this problem, a shortest path objective and a minimum mechanical work objective. Motivation for using the shortest path objective is that the Euclidean distance is available as an admissible heuristic. On the other hand, minimizing the mechanical work required to execute the motion is a more natural objective that is likely similar to the motion a human would use for the task. The drawback to the latter objective is that there is no obvious heuristic to inform the search. Since the landmark heuristic is admissible regardless of the objective it was applicable for this objective.
A vertex was constructed followed by the construction of a landmark heuristic with 50 landmarks. A minimum work path was computed in iterations and using the landmark heuristic while Dijkstra’s algorithm required iterations and . A shortest path was computed in iterations and using the landmark heuristic while using the Euclidean distance required iterations and . Figure 7 illustrates the minimum energy motion that was computed.
The landmark heuristic is well known in the vehicle routing literature where it has been shown to reduce shortest path query times by a factor of 9 to 16 on city to continent-scale road networks. Multi-query applications of the in robot motion planning have striking similarities with vehicle routing problems in road networks in that shortest path queries are evaluated repeatedly on a large graph. The goal of this investigation was to evaluate the effectiveness of the landmark heuristic in robotic motion planning applications. Since the heuristic is based on preprocessing the graph, our hypothesis was that its effectiveness would be independent of how densely cluttered the environment was—a useful feature for complex planning tasks.
To make this evaluation, we constructed a randomized environment parameterized by the probability that the line segment between two random points did not intersect obstacles. The average case relative performance of the landmark heuristic relative to the Euclidean distance heuristic was then measured through numerous randomized trials. Additionally, the performance of the landmark heuristic was evaluated on a manipulator arm model in a realistic planning scenario.
The landmark heuristic was empirically observed to be less sensitive to environment clutter than the Euclidean distance heuristic. For the range of parameters evaluated, the query times were reduced by a factor of 5 to 20 in comparison to Dijkstra’s algorithm. Secondly, a theoretical analysis showed that, with a fixed fraction of vertices assigned to be landmarks, the landmark heuristic converges to the optimal cost between any origin-destination pair with increasing graph size. This analysis was then validated in our experimental results.
The landmark heuristic is an effective heuristic for querying large graphs. In particular, it is more effective than the Euclidean distance heuristic in all but nearly obstacle free problem instances. However, the preprocessing time required to construct the heuristic makes it only suitable for multi-query applications where the heuristic will be used repeatedly on the same graph. A valuable direction for future investigation would be an efficient update to the heuristic when small changes are made to the as a result of changes in the workspace. The proof of Lemma 1 requires some additional notation. The symbol denotes the Lebesgue measure on so that the uniform probability measure of a measurable subset of is given by . Since each landmark is an i.i.d. random variable with the uniform distribution on , the set of landmarks can be viewed as a random variable on the product space, denoted . The probability of for subsets of is given by the product measure :
Next, an -net on is a subset of such that
Based on these two properties it is clear that the number of points making up an -net on is bounded by
Observe that not every is convex since it may intersect the boundary of . The the index set will identify open balls of the -net which have a convex intersection with . As the -net becomes finer, a greater fraction of points will lie on the interior of with a distance to the boundary greater than so
Proof (Lemma 1).
Consider the -net described above with , half the connection radius of the . Note that every vertex in is connected by a line segment for .
The probability that the landmarks for some can be written as
for constants that depend only on the dimension of . One can readily verify that this expression converges to zero as . Thus, the probability that there is at least one landmark in each for converges 1. It follows that every vertex has a landmark as a neighbor almost surely as . Therefore, for at least one landmark , the optimal cost from to satisfies . Thus,
Expanding with the triangle inequality between , , and yields
-  L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” IEEE transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996.
-  S. M. LaValle and J. J. Kuffner, “Randomized kinodynamic planning,” The International Journal of Robotics Research, vol. 20, no. 5, pp. 378–400, 2001.
-  D. Hsu, J.-C. Latombe, and R. Motwani, “Path planning in expansive configuration spaces,” in Robotics and Automation, 1997. Proceedings., 1997 IEEE International Conference on, vol. 3, pp. 2719–2726, IEEE, 1997.
-  S. Murray, W. Floyd-Jones, Y. Qi, D. Sorin, and G. Konidaris, “Robot motion planning on a chip,” in Robotics: Science and Systems, 2016.
-  J. D. Marble and K. E. Bekris, “Asymptotically near-optimal planning with probabilistic roadmap spanners,” IEEE Transactions on Robotics, vol. 29, no. 2, pp. 432–444, 2013.
-  A. V. Goldberg and C. Harrelson, “Computing the shortest path: A search meets graph theory,” in Proceedings of the sixteenth annual ACM-SIAM symposium on Discrete algorithms, pp. 156–165, Society for Industrial and Applied Mathematics, 2005.
-  S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” The International Journal of Robotics Research, vol. 30, no. 7, pp. 846–894, 2011.
-  B. Paden, Y. Nager, and E. Frazzoli, “Landmark guided probabilistic roadmap queries,” 2017. Available at: https://github.com/bapaden/Landmark_Guided_PRM/releases/tag/v0.
-  J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Batch informed trees (bit*): Sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs,” in 2015 IEEE International Conference on Robotics and Automation (ICRA), pp. 3067–3074, IEEE, 2015.
-  S. Karaman and E. Frazzoli, “High-speed flight in an ergodic forest,” in Robotics and Automation (ICRA), 2012 IEEE International Conference on, pp. 2899–2906, IEEE, 2012.
-  I. A. Şucan, M. Moll, and L. E. Kavraki, “The Open Motion Planning Library,” IEEE Robotics & Automation Magazine, vol. 19, pp. 72–82, December 2012. http://ompl.kavrakilab.org.
-  I. A. Sucan and S. Chitta, “Moveit!,” 2016.