Single-query Path Planning Using Sample-efficient Probability Informed Trees

Abstract

In this work, we present a novel sampling-based path planning method, called SPRINT. The method finds solutions for high dimensional path planning problems quickly and robustly. Its efficiency comes from minimizing the number of collision check samples. This reduction in sampling relies on heuristics that predict the likelihood that samples will be useful in the search process. Specifically, heuristics (1) prioritize more promising search regions; (2) cull samples from local minima regions; and (3) steer the search away from previously observed collision states. Empirical evaluations show that our method finds shorter or comparable-length solution paths in significantly less time than commonly used methods. We demonstrate that these performance gains can be largely attributed to our approach to achieve sample efficiency.

I. Introduction

SOLVING high-dimensional path planning problems, such as for robot manipulator motion planning, remains a challenging and important problem. Many aspects of this problem are often addressed with sampling-based path planners, which sample a collision-check function in order to probe the configuration space and inform a search strategy. While, in theory, many sampling-based planners are guaranteed to eventually find a solution if one exists, in practice, even state-of-the-art approaches are unable to consistently solve challenging problems in reasonable amounts of time as they require many costly collision-check samples [1].

In this work, we present a novel sampling-based path planning method, called SPRINT (Sample-efficient PRobability INformed Trees) that finds solutions for high dimensional planning problems significantly faster than many state-of-the-art approaches. Our method uses heuristics designed to minimize the total number of collision-check samples required to find solutions by modeling how likely regions of the search space will yield useful samples. These heuristics, as part of newly proposed global and local tree searches, are used to (1) prioritize more promising search regions to foster a greedy first global search strategy, while still exploring broadly in the limit; (2) cull samples from local minima regions in the local search to avoid wasteful, unfruitful samples; and (3) steer the local search away from previously observed collision states toward search regions predicted to contain more useful free-space samples. Our heuristics can be implemented with efficient information storage and retrieval methods that further accelerate the search. Specifically, our method does not perform expensive nearest neighbor checking each time a node is added to the search graph structure.

We assessed the efficacy of our method by running a large testbed of simulation experiments and compared our method to numerous path planners on single-arm and bimanual robot manipulation problems (§VII). Our method found shorter or comparable-length solution paths in significantly less time than the alternatives. We demonstrate that these performance advantages were, in large part, due to an increase in sample efficiency from our heuristics. We conclude our work by discussing the implications of our results in robotics applications and beyond. We provide open-source code for an implementation of our method.¹

II. Related Works

In general, there are three main paradigms for solving path planning problems: sampling-based, local optimization, and search-based. Sampling-based planners, many of which are extensions of Rapidly-Exploring Random Tree (RRT) [2], Probabilistic Roadmap (PRM) [3], or Expansive Space Tree (EST) [4] methods, often use random configuration-space samples to bootstrap a broad search strategy. These algorithms often have guarantees, such as probabilistic completeness or asymptotic optimality [5], [6], though they often over-explore the space with many unnecessary samples in order to achieve such guarantees.

Our work shares particular similarities with informed-tree sampling-based approaches that iteratively build search strategies that trade-off between exploration and exploitation based on information gathered at run-time [7]–[11]. Our work draws on these approaches, but differs in two main ways: (1) these approaches generally use linear local search segments between samples, whereas our work proposes a new local search designed to maneuver through narrow passages and avoid over-sampling; and (2) the search strategies for these approaches are typically more focused on achieving asymptotic optimality, whereas the search strategies for our method are more singularly focused on reaching a first feasible solution as quickly as possible.

Local optimization-based planners, such as CHOMP [12] or STOMP [13], use non-linear optimization to iteratively transform a trajectory into a higher quality path. These methods are designed to be greedier than sampling-based methods, though they often have fewer guarantees, strongly depend on the quality of the initial condition, and commonly do not converge on a feasible solution due to local minima.

Search-based planners prioritize exploration based on a set heuristic, such as best-first search, depth-first search, breadth-first search, or the A* heuristic [14]. However, because these searches are often structured in a discrete, grid-like fashion, higher dimensional planning often scales intractably in terms of memory and run-time complexity.

Our method draws on all of the above paradigms, and attempts to unite many of the discussed themes. To illustrate, SPRINT also uses random samples to bootstrap its search and explore broadly, but local optimization and search-based concepts are used to optimize and prioritize certain parts of the search to increase the probability of sample usefulness. We compare our method to other common approaches in our evaluation (§VII) to assess whether this bridging of concepts is effective for path planning.

III. Preliminaries

A. Problem Statement

Consider χ as a d-dimensional configuration space with states within χ denoted as q. Suppose χ_obs is a subset of χ considered to be infeasible space (i.e., obstacles). Feasible space can then be defined as the subset of χ not in an obstacle region: χ_free ≡ χ \ χ_obs. A path in χ is a continuous function Γ[q_a,q_b] : [0, 1] → χ, where Γ[q_a,q_b](0) = q_a, Γ[q_a,q_b](1) = q_b, and all points along the path are in χ. The goal in path planning is to find a feasible, C⁰ continuous path Γ[q_init, q_goal] such that Γ[q_init, q_goal](0) is a given start state, q_init; Γ[q_init, q_goal](1) is a given goal state, q_goal; and Γ[q_init, q_goal](u) ∈ χ_free, ∀u ∈ [0, 1].

B. Graph-based Planning Structure

A common structure to solve the problem described above is a graph-based search [2]–[6], [10]. This framework organizes the search into a set of collision-free edges, E, between pairs of nodes in a set N to form a graph G = (N,E). The goal in these searches is to construct G such that a feasible path Γ[q_init, q_goal] is contained as some connected sequence of edge traversals between nodes within the graph. The feasibility of each edge is checked prior to its addition to the graph using a collision-check function. SPRINT interleaves global and local graph-based searches, overviewed in §IV-A.

C. Search-space Regions and Useful Samples

The overall goal of SPRINT is to minimize the number of collision-check samples by incorporating probability heuristics that model how likely regions of the search space will yield useful samples.

A search-space region is a segment of the configuration space χ loosely thought of as states in the vicinity of the line segment between two specified end-points. These regions are used to characterize and model where the planner has previously searched and where the planner could search in the future. Their loose definition is sufficient for these use cases. We denote a search-space region as R[q_a, q_b], where q_a and q_b serve as the end-point markers of the region.

Intuitively, a useful sample is a sample that plays an integral role in constructing a final solution path. More formally, we consider a sample to be δ–useful if it achieves two criteria: (1) it is in free-space, χ_free; and (2) it ultimately lies within a distance of δ of the final solution path, Γ[q_init, q_goal]. Note that this definition implies that it is unknowable if a sample is δ–useful until a solution path is found. However, our method uses heuristics to predict the probability that a particular search-space region will contain at least one δ–useful sample, which we will denote as Pr(U_δ(R[q_a, q_b])). The following sections will detail the structure and functionalities of these probability heuristics.

IV. Technical Overview

This section outlines the search structure and overall strategy of the SPRINT method. Pseudocode of the SPRINT planning sub-processes can be found in Alg. 1–7.

A. SPRINT Global planning Level

The global planning level in SPRINT (Alg. 1) uses a tree-graph as a search structure, T_g = (N_g, E_g), rooted at q_init. A set of sampled collision-free milestone points, M, serve as intermediary goals for the global search to reach en route to q_goal, reminiscent of FMT* [6]. On each global planning level loop, the planner selects a best search-space region for the next local search, $\mathrm{R}[{\mathbf{q}}_{\mathbf{n}}^{*},{\mathbf{q}}_{\mathbf{m}}^{*}]({\mathbf{q}}_{\mathrm{n}}^{*}\in {\mathrm{N}}_{\mathrm{g}},{\mathbf{q}}_{\mathrm{m}}^{*}\in \mathrm{M})$. Milestones that are reached via local searches to form global edges in E_g from already established nodes in N_g are added as global nodes in N_g. If the current set of milestone points does not foster a path to q_goal, more milestone points are added to M, and the search proceeds. This process iterates until q_goal is reached via T_g, provided a solution exists.

Algorithm 1:

sprint(q_init, q_goal)

Algorithm 2:

local_tree_search (${\mathbf{q}}_{\mathrm{n}}^{*},{\mathbf{q}}_{\mathrm{m}}^{*}$)

In general, the global planning level is intended to be a simple wrapper around the more sophisticated local search, outlined below. While the global planning level achieves probabilistic completeness in the limit (§VI), in practice, the default starting set of 50 milestone points is sufficient to quickly solve all problems that we have ever tried.

B. SPRINT Local Planning Level

The local planning level in SPRINT (Alg. 2) also uses a tree-graph as a search structure, T_ℓ = (N_ℓ, E_ℓ). The root of the local tree is the first boundary point of the search-space region selected by the global planning level, ${\mathbf{q}}_{\mathrm{n}}^{*}$, and the goal state that the local tree is trying to reach is the second boundary point of this region, ${\mathbf{q}}_{\mathrm{m}}^{*}$.

The local planning level in SPRINT is a greedy, depth-first-search-like algorithm that uses heuristics to intelligently select branching directions, backtrack to fruitful parts of the search tree, and stop as early as possible when a solution is unlikely to be found. The local search was designed to be particularly adept at steering around approximately convex-shaped obstacle sections and navigating through narrow passages (as seen in Figure 1a). While the local search has no guarantees and needs the global search to route around local minima regions, in practice, it is effective and solves many problems on its own, even in high dimensions.

Fig. 1.

(a) This 2D planning example shows the sample efficiency of the SPRINT planner, as many fewer collision-check samples were taken (left) compared to RRT-Connect (middle) and RRT (right). (b–c) This sample efficiency also makes path planning more efficient in high dimensions, such as for the illustrated robot manipulator motion planning problems.

The local planning level progresses in three steps: (1) Select a node from the tree to extend from, q_x ∈ N_ℓ; (2) Decide if q_x is worthwhile to extend (if not, return to step 1); and (3) If q_x is worthwhile to extend, calculate a candidate node to extend toward, ${\mathbf{q}}_{\mathrm{c}}^{*}$. After ${\mathbf{q}}_{\mathrm{c}}^{*}$ is calculated, it is checked for collision (Alg. 2, line 14). If ${\mathbf{q}}_{\mathrm{c}}^{*}$ is in free-space, it is added as a node to the tree, the algorithm assesses and stores information about how the search has improved or worsened given the addition of the new node, and it becomes the next q_x to extend (Alg. 2, lines 23–39). If ${\mathbf{q}}_{\mathrm{c}}^{*}$ is in collision, the collision-point is stored by the algorithm to help prevent the tree from colliding with the same region again and a next q_x is popped off of a stack of nodes to extend from, N_stack (Alg. 2, lines 15–21). These steps repeat until either the local goal is reached or N_stack becomes empty.

The heuristics used in the local search often rely on its ability to efficiently assess and characterize sub-trees within the tree. Therefore, the local search tree stores information allowing fast responses to queries such as “has the sub-tree rooted at node q recently progressed toward the goal ${\mathbf{q}}_{\mathrm{m}}^{*}$?”; or “what previously observed collision points are in close proximity to the sub-tree rooted at node q?”. To achieve these quick assessments, our method labels certain nodes in the local search tree as data checkpoints, then stores and accesses information at these checkpoints throughout the search. It would be highly inefficient for all nodes to be labeled as checkpoints. Thus, only the root node, ${\mathbf{q}}_{\mathrm{n}}^{*}$, and nodes that have more than one child node are labeled as checkpoints, which reduces the overall number of checkpoints and structurally places them as roots of salient sub-trees where distinct branching decisions were made.

When pertinent information is processed during the search, e.g., a node is added or a collision point is detected, this information is back-propagated through the tree and stored at all checkpoint nodes that reside on the path from the current extend node to the local tree root, ${\mathbf{q}}_{\mathrm{n}}^{*}$. This process can be seen in Alg. 2 lines 17 and 30–39 where information is updated in four hashmap data structures (H_exploit, H_explore, H_obs, H_num, explained more below) on checkpoint nodes $\mathbf{q}\in {\mathrm{N}}_{\text{checkpoints}}\cap \mathrm{\Gamma }[{\mathbf{q}}_{\mathrm{n}}^{*},{\mathbf{q}}_{\mathrm{x}}]$. This procedure ensures that checkpoints have access to information that has been processed “downstream” at newer, more distal parts of its sub-tree (illustrated in Figure 2).

Fig. 2.

(a) - (d) The detected collision states (red dots) get stored (dotted lines) in all checkpoint nodes (dots with purple outlines) along the path from the current extend node, q_x to the root, ${\mathbf{q}}_{\mathrm{n}}^{*}$. (e) - (f) Each added node passes its exploitation and exploration progress information back to its predecessor checkpoints until (g) the search decides that it is stuck in a local minimum trap based on lack of progress. (h) A new search-space region is selected by the global search to route around the local minimum region.

V. Probability Heuristics

In this work, our method uses three probability heuristics, one in the global planning level and two in the local planning level (highlighted in green in Alg. 1 and 2). Each heuristic tries to guide the search into search-space regions with a high probability of containing δ–useful samples. In this section, we describe each of these three heuristics.

A. Probability Heuristic 1

Probability Heuristic 1 is used by the global search to select which search-space region would be best to next attempt a local search. This assessment is based on two criteria: (1) A good search-space region should have an end-point ${\mathbf{q}}_{\mathrm{m}}^{*}\in \mathrm{M}$ that gets closer to q_goal. Intuitively, global edges that make progress toward the goal are more likely to have useful samples; and (2) A good search-space region should be far from any regions that already fostered local searches that did not reach their respective goals. The goal of this second criterion is to de-prioritize the search from repeating a local search in a region already estimated to be a local minimum trap (stored in the R_localmin set in Alg. 1).

Algorithm 3:

probability_heuristic_1 (N_g, M, R_localmin)

We model the two criteria outlined above as a mixture of two respective Gaussian functions:

$${\mathrm{Pr}}_1({\mathrm{U}}_{\delta }(\mathrm{R}[{\mathbf{q}}_{\mathrm{g}},{\mathbf{q}}_{\mathrm{m}}])|{\mathrm{T}}_{\mathrm{g}})={\varphi }^{-1}*[(w_1*g_1({\mathbf{q}}_{\mathrm{n}},{\mathbf{q}}_{\mathrm{m}},{\mathrm{T}}_{\mathrm{g}}))*(w_2*g_2({\mathbf{q}}_{\mathrm{n}},{\mathbf{q}}_{\mathrm{m}},{\mathrm{T}}_{\mathrm{g}}))]$$ (1)

These Gaussian functions can be seen in Alg. 3, lines 7 and 8–14. The ϕ scalar is a normalization constant, and the w scalars signify weights that adjust the peaks of their Gaussian terms. The default values for w₁ and w₂ were hand-tuned, though our experiments below show that performance is not overly sensitive to these parameters. The c scalar is the standard deviation over these Gaussian functions, which we set as the Euclidean distance from start to goal.

The g₁ function uses Euclidean distances to achieve the first criterion discussed above. Specifically, x₁, the input variable of the g₁ Gaussian function, is designed such that the closer a search-space region gets to q_goal compared to where it started, the higher the probability value will be from g₁. The g₂ function achieves the second criterion by projecting all candidate search-space regions onto rays cast from the endpoints of prior unfruitful search-space regions (stored in the set R_localmin) and deprioritizing candidate regions with endpoints close to any of these projections (Alg. 3, lines 9–14, seen illustrated in Figure 3).

Algorithm 4:

probability_heuristic_2(q_x, T_ℓ, Ω_ℓ)

Fig. 3.

Projection procedure of a search-space region R[q_n, q_m] onto a region already deemed to be a local minimum trap, R[q_a, q_b].

B. Probability Heuristic 2

Probability Heuristic 2 is used by the local search to decide whether a particular node in the local tree search, q_x, is worthwhile to extend. This decision is made by checking information stored at all checkpoints along the path from q_x back to ${\mathbf{q}}_{\mathrm{n}}^{*}$. At a high level, q_x is deemed worthwhile to extend if either (1) all of the sub-trees rooted at checkpoints along the path from q_x back to ${\mathbf{q}}_{\mathrm{n}}^{*}$ have shown recent progress in terms of getting closer to the goal, ${\mathbf{q}}_{\mathrm{m}}^{*}$. Throughout this work, we refer to this kind of progress as exploitation (stored and tracked in the H_exploit hashmap in Alg. 2); or (2) all of the sub-trees rooted at checkpoints along the path from q_x back to ${\mathbf{q}}_{\mathrm{n}}^{*}$ have shown recent progress in terms of getting further from the tree root, ${\mathbf{q}}_{\mathrm{n}}^{*}$. Throughout this work, we refer to this kind of progress as exploration (stored and tracked in the H_explore hashmap in Alg. 2). If the sub-trees that q_x is a part of show no sign of recent exploitation or exploration progress, the heuristic considers this region of the local tree trapped in a local minimum, and q_x is culled prior to extension.

We model Probability Heuristic 2 as the minimum of a set of probability functions:

$${\mathrm{Pr}}_2({\mathrm{U}}_{\delta }(\mathrm{R}[{\mathbf{q}}_{\mathrm{x}},{\mathbf{q}}_{\mathrm{m}}^{*}])|{\mathrm{T}}_{\ell })=\underset{\mathbf{q}\in \mathrm{B}}{\min }(g({\mathbf{q}}_{\mathrm{x}},{\mathbf{q},\mathrm{T}}_{\ell })),\mathrm{B}\equiv {\mathrm{N}}_{\text{checkpoints}}\cap \mathrm{\Gamma }[{\mathbf{q}}_{\mathrm{n}}^{*},{\mathbf{q}}_{\mathrm{x}}]$$ (2)

The probability functions are modeled as Gaussian functions, constructed in Alg. 4, lines 4 – 9. Here, x₁ and x₂ are the number of collision-check samples since a node has made exploitation and exploration progress, respectively. These values are stored and updated in the H_exploit and H_explore hashmaps in Alg. 2, lines 17 and 31–38. We model this sub-tree assessment process by taking the minimum of x₁ and x₂ in Alg. 4, line 7. Taking the minimum ensures that if either exploitation or exploration progress are occurring in the given sub-tree (i.e., either x₁ or x₂ are near 0), the sub-tree as a whole will still be considered promising. This value is inputted into the Gaussian function in line 9 to output the approximate probability value g ∈ [0, 1]. The standard-deviation c value is attenuated based on the number of nodes in a given sub-tree (stored and updated in the hashmap H_num in Alg. 2) such that new sub-trees with few nodes do not fail too abruptly even if they do not immediately show exploitation or exploration progress. In our prototype system, we use a probability cutoff of κ = 0.3, though our experiments below suggest that performance is not overly sensitive to this parameter selection.

Algorithm 5:

Common Functions

1	$\text{proj}\_\text{scalar}(\mathbf{q},\mathrm{R}[{\mathbf{q}}_{\mathrm{a}},{\mathbf{q}}_{\mathrm{b}}])=\frac{(\mathbf{q}-{\mathbf{q}}_{\mathrm{a}})\cdot ({\mathbf{q}}_{\mathrm{b}}-{\mathbf{q}}_{\mathrm{a}})}{({\mathbf{q}}_{\mathrm{b}}-{\mathbf{q}}_{\mathrm{a}})\cdot ({\mathbf{q}}_{\mathrm{b}}-{\mathbf{q}}_{\mathrm{a}})}$
2	proj(q, R[qa,qb]) = qa + proj_scalar(q, R[qa, qb]) * (qb − qa)
3	proj+(q, R[qa,qb]) = qa + max(proj_scalar(q, R[qa, qb]), 0) * (qb − qa)
4	$\text{hvs}(x)=\{\begin{array}{cc}0\hfill & \text{if}x\le 0\hfill \\ 1\hfill & \text{if}x>0\hfill \end{array}//\text{Heaviside step function}$

Algorithm 6:

probability_heuristic_3 (q_x, T_ℓ, Ω_ℓ)

C. Probability Heuristic 3

Probability Heuristic 3 is used by the local search to calculate which search-space region the local tree should extend into from an extend node q_x. This region is computed based on three criteria: (1) A good search-space region should follow a straight line path with respect to the predecessor edge of q_x. This criterion prevents the local search from making drastic turns and unnecessarily turning back on its own path; (2) A good search-space region should get closer to the local goal, ${\mathbf{q}}_{\mathrm{m}}^{*}$. Intuitively, making progress toward the goal is estimated to be useful; and (3) A good search-space region should move away from previously observed collision points nearby (stored in the H_obs hashmap in Alg. 2). A search-space region that is pointing in the direction of a previously observed collision point is also likely to collide, and thus, is less likely to lead to a useful sample.

Instead of explicitly modeling a Pr₃ model, we implicitly define the model through its gradient with respect to a search-space region end-point, ${\mathbf{q}}_{\mathrm{c}}:\frac{\partial {\mathrm{Pr}}_3({\mathrm{U}}_{\delta }(\mathrm{R}[{\mathbf{q}}_{\mathrm{x}},{\mathbf{q}}_{\mathrm{c}}]))}{\partial {\mathbf{q}}_{\mathrm{c}}}$. Then, given this gradient, we perform gradient ascent to steer local edges in a way that would approximately optimize a Pr₃. We model this gradient as proportional to the sum of three weighted sub-term gradients, each trying to achieve one of the three criteria from Probability Heuristic 3 above, respectively:

$$\frac{\partial {\mathrm{Pr}}_3({\mathrm{U}}_{\delta }(\mathrm{R}[{\mathbf{q}}_{\mathrm{x}},{\mathbf{q}}_{\mathrm{c}}]))|{\mathrm{T}}_{\ell })}{\partial {\mathbf{q}}_{\mathrm{c}}}\propto w_1\frac{\partial g_1}{\partial {\mathbf{q}}_{\mathrm{c}}}+w_2\frac{\partial g_2}{\partial {\mathbf{q}}_{\mathrm{c}}}+w_3\frac{\partial g_3}{\partial {\mathbf{q}}_{\mathrm{c}}}$$ (3)

The $\frac{\partial g_1}{\partial {\mathbf{q}}_{\mathrm{c}}}$ sub-term gradient pulls the search-space region in a straight line with respect to its predecessor edge. Here, q_p denotes the predecessor node of the extend node, q_x:

$$\frac{\partial {\mathrm{g}}_1}{\partial {\mathbf{q}}_{\mathrm{c}}}=\frac{{\mathbf{q}}_{\mathrm{x}}-{\mathbf{q}}_{\mathrm{p}}}{\Vert {\mathbf{q}}_{\mathrm{x}}-{\mathbf{q}}_{\mathrm{p}}\Vert }$$ (4)

The $\frac{\partial g_2}{\partial {\mathbf{q}}_{\mathrm{c}}}$ sub-term gradient pulls the search-space region toward the local goal, ${\mathbf{q}}_{\mathrm{m}}^{*}$:

$$\begin{gathered} \frac{\partial {\mathrm{g}}_2}{\partial {\mathbf{q}}_{\mathrm{c}}}={\psi }_2*\frac{{\mathbf{q}}_{\mathrm{m}}^{*}-{\mathbf{q}}_{\mathrm{c}}}{\Vert {\mathbf{q}}_{\mathrm{m}}^{*}-{\mathbf{q}}_{\mathrm{c}}\Vert } \\ {\psi }_2=\mathit{exp}(-{\Vert {\mathbf{q}}_{\mathrm{m}}^{*}-{\mathbf{q}}_{\mathrm{c}}\Vert }^2/4{\mathrm{\lambda }}^2)+1 \end{gathered}$$ (5)

Algorithm 7:

get_nearby_collision_points(q_x, T_ℓ, Ω_ℓ)

Here, ψ₂ increases the pull from $\frac{\partial g_2}{\partial {\mathbf{q}}_{\mathrm{c}}}$ as the local tree approaches its goal which helps avoid overshoot when doing gradient ascent. The λ scalar is the fixed length of all local search edges.

The $\frac{\partial g_3}{\partial {\mathbf{q}}_{\mathrm{c}}}$ sub-term gradient pushes the candidate edge away from a set of nearby, previously observed collision points q_obs ∈ N_obs (utility functions used in this sub-term gradient can be found in Alg. 5):

$$\begin{gathered} \frac{\partial {\mathrm{g}}_3}{\partial {\mathbf{q}}_{\mathrm{c}}}=\frac{1}{\left|{\mathrm{N}}_{\mathrm{obs}}\right|}\sum _{{\mathbf{q}}_{\mathrm{obs}}\in {\mathrm{N}}_{\mathrm{obs}}}{\psi }_{3,1}*{\psi }_{3,2}*\frac{\text{proj}({\mathbf{q}}_{\mathrm{obs}},\mathrm{R}[{\mathbf{q}}_{\mathrm{x}},{\mathbf{q}}_{\mathrm{c}}])-{\mathbf{q}}_{\mathrm{obs}}}{\Vert \text{proj}({\mathbf{q}}_{\mathrm{obs}},\mathrm{R}[{\mathbf{q}}_{\mathrm{x}},{\mathbf{q}}_{\mathrm{c}}])-{\mathbf{q}}_{\mathrm{obs}}\Vert } \\ {\psi }_{3,1}=\text{hvs}(\text{proj}\_\text{scalar}({\mathbf{q}}_{\mathrm{obs}},\mathrm{R}[{\mathbf{q}}_{\mathrm{x}},{\mathbf{q}}_{\mathrm{c}}])) \\ {\psi }_{3,2}=5*\mathit{exp}(-{\Vert \text{proj}({\mathbf{q}}_{\mathrm{obs}},\mathrm{R}[{\mathbf{q}}_{\mathrm{x}},{\mathbf{q}}_{\mathrm{c}}])-{\mathbf{q}}_{\mathrm{obs}}\Vert }^2/4{\mathrm{\lambda }}^2) \end{gathered}$$ (6)

The push from a particular collision point, q_obs, is given in the direction of q_obs towards its projection onto the candidate search-space region, R[q_x, q_c]. If this projection point lies “behind” q_x, i.e., proj_scalar(q_obs, R[q_x, q_c]) < 0, the collision point is considered already passed, and the ψ_3,1 term removes this collision point’s effect on the gradient. If q_obs lies “ahead” of q_x, ψ_3,2 attenuates the push strength away from q_obs such that a smaller distance between q_obs and its projection point corresponds to a stronger push, and vice versa. The scalar 5 coefficient on the ψ_3,2 term raises the peak of its corresponding Gaussian to further strengthen the push away from q_obs if it is close to its projection onto R[q_x, q_c]. The set of nearby collision states, N_obs, is efficiently accessed in constant time, i.e., without using a heavy data structure like a kd-tree, by using the checkpoint data back-propagation technique outlined in §IV-B (Alg. 7). Here, collision points are stored and accessed at checkpoint nodes in the H_obs hashmap throughout Alg. 2. Note that if |N_obs| ≡ 0, the effect of $\frac{\partial g_3}{\partial {\mathbf{q}}_{\mathrm{c}}}$ is cancelled by the Heaviside step coefficient in Alg. 6, line 6, and thus, $\frac{\partial g_3}{\partial {\mathbf{q}}_{\mathrm{c}}}$ does not need to be computed in this case.

The $\frac{\partial {\mathrm{Pr}}_3({\mathrm{U}}_{\delta }(\mathrm{R}[{\mathbf{q}}_{\mathrm{x}},{\mathbf{q}}_{\mathrm{c}}]))}{\partial {\mathbf{q}}_{\mathrm{c}}}$ gradient is used in gradient ascent iterations to steer the local tree, as seen in Alg. 6. Because this gradient ascent occurs at such a performance critical inner-loop process, we only do one or two iterations per local search loop in practice. By default, we use the point q_x + (q_x − q_p) as the initial condition for q_c in the gradient ascent. Also, if |N_obs| > 0, we add a small amount of noise to this initial condition. In our prototype system, this noise vector is drawn from a d-dimensional uniform distribution $\xi \sim U^{\mathrm{d}}(\frac{-\lambda }{100},\frac{\lambda }{100})$. Without this small random push, the local search would just stay on a single plane embedded in the configuration space. We also re-set the edge length to λ after each gradient ascent iteration (Alg. 6, line 7).

VI. Analysis

In this section, we show the probabilistic completeness property of SPRINT in ${ℝ}^{\mathrm{d}}$.

Sketch Proof:

Consider Γ[q_init, q_goal] as any feasible solution path found by a probabilistically complete planner parameterized by a set of k nodes, N ≡ {q₁, q₂, …, q_k}, connected by linear edges, E. For example, solution paths found by RRT or PRM would fit this definition. Now, suppose d-dimensional open balls of radii r₁ … r_k, are centered around nodes q₁ … q_k, which we will denote as ${\mathcal{B}}_{r_1}({\mathbf{q}}_1)\dots {\mathcal{B}}_{r_{\mathrm{k}}}({\mathbf{q}}_{\mathrm{k}})$. Each radius r_j will be selected such that all of ${\mathcal{B}}_{r_{\mathrm{j}}}({\mathbf{q}}_{\mathrm{j}})$ lies in free-space and all points in ${\mathcal{B}}_{r_{\mathrm{j}}}({\mathbf{q}}_{\mathrm{j}})$ can be connected by collision-free lines with all points in ${\mathcal{B}}_{r_{\mathrm{j}+1}}({\mathbf{q}}_{\mathrm{j}+1})\forall \mathrm{j}\in [1,2,\dots ,\mathrm{k}-1]$. Note that because Γ[q_init, q_goal] is feasible, a radius of r_j > 0 must exist for all ${\mathcal{B}}_{r_{\mathrm{j}}}({\mathbf{q}}_{\mathrm{j}})$ [15].

Observation 1: Because milestone points are uniformly sampled from χ_free and each radius r_j > 0, each ${\mathcal{B}}_{r_{\mathrm{j}}}({\mathbf{q}}_{\mathrm{j}})$ will become dense with milestones in the limit [15]. Observation 2: SPRINT will process a local search between all current global node and milestone point pairs each loop through Algorithm 1 lines 2–16. Observation 3: If a collision-free, straight-line path exists between a global node q_n and a milestone q_m, the local search in SPRINT resolves to a straight-line. From these observations, it can be seen that the following recursive sequence will always happen in SPRINT: a milestone point will eventually be sampled in ${\mathcal{B}}_{r_{\mathrm{j}}}({\mathbf{q}}_{\mathrm{j}})$, which will be reached by a straight line local search from a global node in ${\mathcal{B}}_{r_{\mathrm{j}-1}}({\mathbf{q}}_{\mathrm{j}-1})\forall \mathrm{j}\in [2,3,\dots ,\mathrm{k}]$. Note that q_init, i.e., q₁, and q_goal, i.e., q_k, start as a global node and milestone point, respectively, so these points bootstrap the recursive process and never have to be exactly sampled. Therefore, SPRINT must inherit the probabilistic completeness of any algorithm that would eventually find a feasible path Γ[q_init, q_goal] as a homotopically equivalent path will always eventually be found by SPRINT. □

VII. Evaluation

We have assessed the efficacy of our approach in three empirical experiments. Although we demonstrate that SPRINT is able to solve low-dimensional testbed problems used to validate planning algorithms, as illustrated in Figure 1, we focus our assessment on higher dimensional problems with robot manipulators. In this section, we overview our experiments and share results.

A. Implementation Details

our prototype SPRINT implementation is implemented in the Rust programming language. Robot manipulator self-collisions and environment collisions are detected using the ncollide Rust library. Configurations that exceed joint position limits are also considered in a collision state. All evaluations throughout this work were run on a Lenovo Legion laptop with an i7-9750H processor and 32GB RAM. All parameters used in SPRINT were held constant at their default values for all problems in our evaluation.

B. Evaluation Benchmark

We developed a set of seven benchmark tasks to compare our method against alternative path planners. The benchmark was designed to test the planners on a wide variety of tasks that range in dimensionality and topological structure. Our benchmark consists of two tasks on a Universal Robots UR5 (6-DOF), two tasks on a Rethink Robotics Sawyer (7-DOF), and three bimanual tasks on the DRC-Hubo+ (15-DOF). Two of these tasks can be seen illustrated in Figure 1(b–c). The UR5 tasks included (1) Single Box, where the robot moves over a large box; and (2) Table, where the robot moves its arm up and around a small table. The Sawyer tasks included (3) Vertical Bars where the robot maneuvers around six vertical bars; and (4) Narrow Passage Box where the robot maneuvers out of a cramped box through a narrow passage. Finally, the Hubo tasks included (5) Over Boxes where the robot moves its arm up and over a set of boxes; (6) Bookshelf where the robot moves an item from the top shelf to the bottom shelf; and (7) Arms around Table where the robot maneuvers both of its arms up and around a table to reach objects on top. Our benchmark consisted of 100 trials through each task. All tasks had a maximum evaluation time of one minute.

C. Experiment 1: Comparisons with Alternative Planners

In our first experiment, we compared performance on our benchmark tasks to nine commonly used path planners: RRT [16], RRT-Connect [17], RRT* [5], FMT* [6], KPIECE [18], BiEST [4], BiTRRT [19], CHOMP [12], and STOMP [13]. All methods were tested using their MoveIt! implementations with default planner options. Also, we implemented RRT, RRT-Connect, FMT*, and EST within our framework to ensure that the planning primitives in SPRINT, e.g., collision checking or graph operations, were not eliciting unintended performance gains. Our implementations used kd-trees for nearest neighbor checking. We used both our own implementations and MoveIt! in our evaluation and only report the better of the two results for fairness. Across all planners and tasks, the highly optimized implementations in MoveIt! were faster.

1). Results—Computation Time:

Figure 4a provides an overview of the average times needed to complete our benchmark tasks. SPRINT computed solution paths for the benchmark problems often one to three orders of magnitude faster than alternative approaches. While many of the planners did well on the somewhat simpler, lower dimensional UR5 problems, SPRINT still out-performed the other methods on these tasks.

Fig. 4.

Results from Experiments 1 and 2. Bar colors denote different tasks, with ordering from left to right being Single Box (UR5), Table (UR5), Vertical Bars (Sawyer), Narrow Passage Box (Sawyer), Over Boxes (Hubo+), Bookshelf (Hubo+), and Arms around Table (Hubo+). Error bars denote standard error. The values above the log10-scale bars in (a) denote the standard decimal values of the bars (in seconds). A circle with a line through it indicates that no solution was found on the given task in the allotted time.

2). Results—Path Length:

Figure 4b provides an overview of average path lengths per each planner and task. The paths computed by SPRINT are shorter or at most similar in length compared to other sampling-based planners. Thus, the performance advantages in computation time do not come at the expense of lower quality paths. While optimizing planners, such as RRT* and STOMP, did find higher quality paths on the UR5 Single Box task, these results came at a significantly higher computation cost. Further, these planners were not able to find any solutions in the allotted sixty seconds for any of the other benchmark tasks.

D. Experiment 2: Analysis of Performance Gains

In our second experiment, our goal was to investigate whether the performance gains exhibited by SPRINT in Experiment 1 could be attributed to enhanced sample efficiency. We assessed the ratio of points from the solution paths from Experiment 1 that were δ–Useful for each tested planner, which we defined in §III-C. We defined δ to be 2λ for this evaluation, where λ was the minimum edge length in the tree or graph structure. We only report this δ-Usefulness metric on the sampling-based planners as it is not applicable to the local-optimization-based planners (STOMP and CHOMP).

Figure 4c shows the δ–Useful metric results. We observe that SPRINT performs much better on this metric across all tasks, indicating that many more of its samples were useful. These results suggest that the performance gains from SPRINT were indeed due to our intended goal of enhancing sample-efficiency.

E. Experiment 3: Analysis of Heuristics and Parameters

In our final experiment, our goal was to observe how the performance of SPRINT changes if we systematically perturb its parameters or remove its probability heuristics. We assessed four conditions: (1) SPRINT-RP, which randomly offsets all parameters such that each varies by up to 25% above or below its original hand selected value. These random offsets were drawn from a uniform distribution; SPRINT ${\mathrm{Pr}}_1^{-}$, which replaced the Pr₁ heuristic with a random global edge selection; (2) SPRINT ${\mathrm{Pr}}_2^{-}$, which replaced the Pr₂ heuristic with a random, 50% chance of extending a node; and (4) SPRINT ${\mathrm{Pr}}_3^{-}$, which replaced the Pr₃ heuristic with a random extend direction process.

Figure 5 overviews the results from Experiment 3. First, we found that SPRINT-RP was comparable in terms of computation time and average path length, suggesting that SPRINT is not overly sensitive to parameter tuning. However, we see that performance significantly degrades when even one probability heuristic is removed, suggesting that all of our probability heuristics are integral and work in tandem in order to enhance sample-efficiency and, in turn, boost performance. Probability Heuristic 3 appears to be particularly important, as no solutions were found in the allotted sixty seconds without this heuristic model.

Fig. 5.

Results from Experiment 3. Bar colors denote different tasks, with ordering from left to right indicating Single Box (UR5), Table (UR5), Vertical Bars (Sawyer), Narrow Passage Box (Sawyer), Over Boxes (Hubo+), Bookshelf (Hubo+), and Arms around Table (Hubo+). The values above the log10-scale bars in (a) denote the standard decimal values of the bars (in seconds). A circle with a line indicates that no solution was found.

VIII. Discussion

In this work, we presented a path planning approach that is able to quickly and reliably solve high-dimensional path planning problems. Through the notion of enhanced sample usefulness afforded by a set of probability heuristics, we show that our approach achieves significant performance gains over standard approaches.

Limitations—

We note a number of limitations of our work that suggest future extensions. First, our probability heuristics were constructed by observation and intuition. While our three heuristics serve as proofs of concept for our overall premise, we believe that better models, either hand-engineered or data-driven, could improve results. Our approach also does not guarantee asymptotic optimality. Lastly, our work does not currently accommodate kinodynamic planning, though we believe that extensions to our work could explore kinodynamic steering functions compatible with Probability Heuristic 3.