Efficient collision checking in sampling-based motion planning via safety certificates

Abstract

Collision checking is considered to be the most expensive computational bottleneck in sampling-based motion planning algorithms. We introduce a simple procedure that theoretically eliminates this bottleneck and significantly reduces collision-checking time in practice in several test scenarios. Whenever a point is collision checked in the normal (expensive) way, we store a lower bound on that point’s distance to the nearest obstacle. The latter is called a “safety certificate” and defines a region of the search space that is guaranteed to be collision-free. New points may forgo collision checking whenever they are located within a safety certificate of an old point. Testing the latter condition is accomplished during the nearest-neighbor search that is already part of most sampling-based motion planning algorithms. As more and more points are sampled, safety certificates asymptotically cover the search space and the amortized complexity of (normal, expensive) collision checking becomes negligible with respect to the overall runtime of sampling-based motion planning algorithms. Indeed, the expected fraction of points requiring a normal collision check approaches zero, in the limit, as the total number of points approaches infinity. A number of extensions to the basic idea are presented. Experiments with a number of proof-of-concept implementations demonstrate that using safety certificates can improve the performance of sampling-based motion planning algorithms in practice.

Keywords

Sampling-based motion planning collision checking safety certificate robotics autonomous systems

1. Introduction

A motion planning problem is, roughly speaking, the search for a connected set of collision-free configurations that begin at a start region (or configuration) and end within a desired goal region. Such problems arise in a multitude of autonomous robotic applications including: aerospace, underwater exploration, manufacturing, and warehouse management. The future success or failure of automated personal transportation and mainstream consumer robotics will depend, in a large part, on the ability to solve motion planning problems more quickly.

Finding a safe motion plan fundamentally involves comparing robot state values with obstacle locations. Yet, in general, these two things are defined in two different spaces, i.e. the configuration space and the workspace, respectively. Different classes of motion planning algorithms address this discrepancy in different ways.

Exact motion planning algorithms assume that an exact obstacle representation can be constructed in the configuration space (see, for instance, those in Lavalle, 2006). While exact algorithms exist that are complete and optimal, the practical transformation of obstacles from the workspace into the configuration space is often intractable or even theoretically impossible.

Sampling-based motion planning algorithms are a more tractable alternative. These algorithms iteratively construct a graph of safe movement by randomly sampling the configuration space and then mapping points/trajectories from the configuration space into robotic positions in the workspace for collision checking versus obstacles. The resulting algorithms trade absolute completeness/optimality for probabilistic completeness and asymptotic optimality.

Sampling-based motion planning algorithms are the focus of the current paper. Their main components include:

a sampling scheme that provides a sequence of points from the configuration space;

collision-checking functions that determine if a configuration space point or trajectory is in collision with an obstacle, given a workspace obstacle set and a mapping function from configuration space to workspace;

a proximity search function that returns “neighbors” to a point in the configuration space, given a point set in the configuration space;

a local planning function that returns a configuration space trajectory between two given points.

Collision checking is widely considered to be the primary computational bottleneck of sampling-based motion planning (see, e.g., Lavalle, 2006). Although an individual collision check has runtime complexity that is upper-bounded by a constant,¹ the constant is often overwhelmingly large in practice. Our main contribution is to show that this does not need to be the case. We introduce a novel collision-checking technique that has negligible amortized complexity versus the nearest-neighbor searches that are already at the core of sampling-based motion planning algorithms. As a consequence, nearest-neighbor searches become the main practical determinant of runtime—as complexity analysis suggests they should (see, e.g., Karaman and Frazzoli, 2011).

Traditionally, sampling-based motion planning algorithms have considered collision checking as a “Boolean black box” subroutine that returns either “ $True$ ” or “ $False$ ” depending on whether a query point is in collision with obstacles or not, respectively. We place a stronger requirement on the collision-checking procedure and assume that it returns $d_{\min}$ which is a lower bound on the minimum distance to the obstacle set, see Figure 1. Although computing $d_{\min}$ may sometimes be harder, by up to a constant factor,² than computing the standard binary “ $True$ ”/“ $False$ ” check, the extra information provided by $d_{\min}$ allows us to eliminate a large fraction of subsequent collision checks.

Fig. 1.

(a) Collision-checked nodes $p$ store “safety certificates” (blue) defined by $d_{\min}$ (dashed black lines), a lower bound on the distance to the nearest obstacle (obstacles are red). (b) Future nodes, for example $q$ , within a certificate can forgo collision checking. (c) Pointers (solid red arrows) are maintained to certifying nodes. (d) A lower bound on the distance to the nearest obstacle can also be used.

Each collision-checked point $p$ remembers $d_{\min}$ , which defines a “safety certificate” near $p$ —i.e. a sub-set space containing $p$ that is known to be collision-free. New samples $q$ may forgo collision checking whenever they are located within a safety certificate, i.e. when $∥ p - q ∥ < d_{\min}$ . Certificates tend to cover more and more of the space as graph size increases, and thus the probability that a new sample will require a collision check decreases, see Figure 2. In Section 5 we show that the amortized computational complexity due to collision checking converges to zero, in the limit, as the number of samples approaches infinity (this trend can be observed in Figure 3).

Fig. 2.

Certificates (blue discs) asymptotically cover the space as graph size increases toward infinity. The ratio of collision checks versus graph size (nodes) approaches zero in the limit as graph size approaches infinity.

Fig. 3.

Illustration of the certificate covering for an implementation of RRT* for a planar point robot. As the number of samples increase (images from left to right) the likelihood of sampling from outside the certificates goes down.

It is important to note that $d_{\min}$ may be defined with respect to either the configuration space or the workspace; thus, it is possible to use certificates in either space, assuming $d_{\min}$ can be calculated. In practice, the convenience and utility of using certificates in one space or the other depends on the implementation details of a particular motion planning system, for example, the geometric constructs used to model the robot and the obstacles. We perform experiments using either type of certificate in Section 8. However, the implementations that we use are intended merely as proofs-of-concept that the general idea can be applied in a variety of ways and are not designed for use with any specific existing motion planning packages. We hope that our results will encourage the developers of future motion planning systems to consider certificate representations alongside other geometric and topological factors when choosing how to model robots and obstacles.

The rest of this paper is organized as follows. Background and related work are discussed in Section 2. Section 3 contains nomenclature and problem statements. The basic state-space certificate algorithm is presented in Section 4, and its analysis is presented in Section 5. In Section 6 we demonstrate how workspace certificates can be implemented in the case where robots and obstacles are modeled as sets of polytopes. In Section 7 we demonstrate how symmetries within the state-space of a multi-robot problem can be exploited to achieve performance gains. Experiments are presented in Section 8 and Section 9 contains a discussion of the results. Conclusions are presented in Section 10.

2. Background and related work

Most sampling-based motion planning algorithms can be classified along two different spectra: (1) single-query versus multi-query planning and (2) feasible-path versus shortest-path planning.

Single-query algorithms are used to find a single motion plan through a particular environment, while multi-query algorithms are used when many motion plans are expected to be found in the same environment. The Rapidly-exploring Random Tree (RRT) by LaValle and Kuffner (2001) and the Probabilistic RoadMap (PRM) by Kavraki et al. (1996) are popular single- and multi-query algorithms, respectively.

Feasible-path planning algorithms are designed to find a valid path between the start state and the goal set, while shortest-path planning algorithms attempt to find the shortest valid path with respect to a predefined metric over the search space. The aforementioned RRT and PRM are both feasible-planning algorithms, while PRM* and RRT* are shortest-path planning algorithms (Karaman and Frazzoli, 2011).

Many variations of these basic ideas exist, each designed to improve performance in certain cases or to provide additional performance guarantees. For example, Hsu et al. (1998) and Boor et al. (1999) biased sampling to occur in narrow passages and near obstacle boundaries, respectively, and Bohlin and Kavraki (2000) delayed collision checks until needed (LazyPRM). Karaman and Frazzoli (2011) provided asymptotic optimality guarantees on path quality with PRM* and RRT*. RRT^# by Arslan and Tsiotras (2013) maintains graph consistency (the idea that cost decreases should be propagated through the search-graph) to improve practical convergence toward the optimal solution. Otte and Frazzoli (2015) formally proved that maintaining graph consistency leads to faster convergence in theory (i.e. in addition to practice). They also showed that better performance can be achieved in the context of replanning by settling for $ε$ -consistency in RRT^X.

Much of the literature on collision checking focuses on static collision checking—determining the safety of a single point. More recent work has investigated the more difficult problem of continuous collision checking—determining the safety of a continuous trajectory between two points. In general, collision checking requires both interference testing and spatial indexing. An interference test checks if a configuration or set of configurations is in collision with a single obstacle. Spatial indexing is a strategy for creating a data structure which can reduce the number of obstacles that must be considered for interference testing. These components are often called broad-phase (navigating the index) and narrow-phase (interference testing) collision checking. A comprehensive survey appears in Jiménez et al. (2001).

Discrete interference testing between two convex objects is often implemented using the GJK algorithm by Gilbert et al. (1988). Efficient exact interference testing in three dimensions for a convex polytope undergoing smooth motion among convex polytope obstacles is given by Canny (1986). A derivative of this method which replaces actual motions between two configurations with an arbitrary surrogate that is sufficient for small time instances is given by Redon et al. (2000), where “small” is defined as being application dependent and valid in cases where the approximation $\tan (θ t / 2) / \tan (θ 2) \approx t$ , for time $t$ and rotation $θ$ , is valid. For the case that the motion can be approximated by a sequence of screw motions (infinitesimal translations/rotations), a method is given by Kim and Rossignac (2003). A combination of the two with the addition of conservative bounds given by interval arithmetic is given by Redon et al. (2002).

Alternative strategies are based on inexact continuous checking by discrete sampling, in many cases applying a static collision check at each of the sample points. A method of adaptive segment checking is presented by Schwarzer et al. (2004). Methods based on conservative certificates and interval arithmetic are described in Redon et al. (2004, 2005). An extension using Taylor models to approximate incremental motion is described by Zhang et al. (2007). A method based on refitting bounding spheres is given by Kavan and Žára (2005).

Data structures used as spatial indexes generally fall under the category of spatial decompositions and bounding volume hierarchies (Samet, 2006). As in the case of nearest-neighbor searching, the Voronoi diagram plays an important role in collision checking. In the case of two-dimensional systems and obstacles, in particular, the Voronoi diagram of convex sites yields an optimal data structure for set distances (McAllister et al., 1996). In the case where the system and obstacles are polygons the Voronoi diagram of line segments can be used (Held, 2001; Karavelas and Yvinec, 2003).

Voronoi diagrams provide an optimal spatial decomposition for set distance queries, but the complexity of constructing the diagram can prove to be unreasonable, especially in higher dimensions. One alternative based on non-optimal decomposition is a binary space partition (BSP) by Tokuta (1991). Bounding volume hierarchies (BVHs), especially those based on R-trees (Guttman, 1984) are extremely popular for collision checking. One of the most common is the Axis-Aligned Bounding Box tree (AABB-tree). A combined method based on the R-tree of the Voronoi diagram is given in Sharifzadeh and Shahabi (2010). Voronoi diagrams, BSPs and BVHs are shown to have $O (\log n_{obs})$ expected search time for a static collision query, where $n_{obs}$ is the size of the obstacle representation (usually number of vertices or faces of the obstacle mesh) (Samet, 2006). The latter is a consequence of requiring $O (\log n_{obs})$ expected interference tests per static collision query.

By its nature, collision checking is a computationally intensive process. Interference testing is $O (n_{f})$ in the number of features $n_{f}$ and spatial index searching is often $O (\log n_{o})$ in the number of obstacles $n_{o}$ . For many practical applications the values of $n_{f}$ and $n_{o}$ may be very large compared to other data structures in a planning algorithm. However, the low level mathematical operations required for interference testing and index searching tend to be elementary for hardware arithmetic units. Graphics co-processors (GPUs), designed with reduced capability but many more logical processors, are particularly well suited to solving the collision-checking problem.

Govindaraju et al. (2005) and Govindaraju et al. (2006) present bounding volume strategies designed for GPUs. Another BVH construction technique is presented by Lauterbach et al. (2009). Collision checking based on the latter appears in Lauterbach et al. (2010). A parallel method based on maintenance of the colliding-front of the bounding volume tree is presented by Tang et al. (2010). Hou et al. (2011) provide an alternative to breadth-first construction order for GPU BVHs. A modern general implementation for production use is described by Pan and Manocha (2011), Pan et al. ( 2012), and Pan and Manocha (2012).

Workspace certificates are closely related to collision-checking algorithms based on conservative advancement such as kinetic data structures (Basch et al., 1999; Kirkpatrick et al., 2000) and adaptive bisection (Redon et al., 2002; Schwarzer et al., 2004). The latter determines whether a path is collision-free by incrementally building a workspace certificate for some configuration in the path, removing the segment of the path which is certified collision-free, and then recursing on the remaining uncertified paths. These, in turn, are related to earlier methods that efficiently update pairwise collision-distance information as objects move versus time (Ponamgi et al., 1997; Mirtich, 1998).

Geometric coverings have also been used for the purposes of feasible control. In Yang and LaValle (2000) a ball-covering over the control space was used to construct a global control policy out of a set of local control policies (one per ball). The method was extended in Yang and LaValle (2002) and Yang and LaValle (2004) to achieve better speed and sampling performance. While Yang and LaValle (2000), Yang and LaValle (2002) and Yang and LaValle (2004) focused on achieving a valid control policy (see Yang and LaValle, 2004 for an insightful discussion on formulating robot kinematics for use with ball-decompositions), these methods are concerned with finding a feasible solution given that all new points sampled within a previously defined ball are rejected. In contrast, our method can be used with a wide variety of sampling-based algorithms, including those that solve the shortest path-planning problem. We also allow using non-spherical/cylindrical certificates.

Brock and Kavraki (2001) also used balls in the workspace to find a collision-free “tunnel” (sequence of collision-free balls) that was then mapped into the configuration space where artificial potential fields were used to determine the path traveled by the robot. During the tunnel creation phase new points were sampled on the surface of existing balls. Yang and Brock (2013) extended this idea by sampling on the medial between obstacles on the surface of balls. Deits and Tedrake (2014) used ellipsoids of free space to rapidly determining valid step locations in the context of legged robotics.

Portions of the current paper have previously appeared in a number of other places, albeit in preliminary forms. Bialkowski et al. (2013d) also used presented the first iteration of the basic state-space method and has significant overlap with Section 5 of the current paper. Section 7 and, to a lesser extent, Section 8 of the current paper borrow heavily from a technical report (Bialkowski et al., 2013a) that was written to accompany a workshop poster (Bialkowski et al., 2013b). Preliminary versions of Sections 2 and 6 and portions of Sections 5 and 8 have also appeared in the PhD dissertation of the first author (Bialkowski, 2013).

We have also presented an unrelated approach to collision-checking avoidance in which samples are drawn from a distribution that converges to uniform sampling over the free space (Bialkowski et al., 2013c). While both Bialkowski et al. (2013c) and the current paper attempt to minimize collision checking, they do so via different modalities. In particular, Bialkowski et al. (2013c) sought to avoid drawing samples from the obstacle space by updating the sampling distribution based on previous information, while the current paper leverages information from previous collision checks to determine when a new sample is collision-free. Combining the two ideas is an interesting direction for future work, but is beyond the scope of the current paper.

3. Nomenclature and problem statements

The configuration space, $X \subset ℝ^{D}$ , is the set of all configurations of the robotic system. The obstacle set, initial set, and goal set are denoted by $X_{obs}$ , $X_{start}$ , and $X_{goal}$ respectively. The free space is given by $X_{free} = cl (X \ X_{obs})$ , where $cl (\cdot)$ denotes set closure. We assume that $X$ is a metric space and that ∥·∥ is a metric over $X$ (e.g. Euclidean distance—see LaValle and Kuffner, 2001 for alternative choices).

$ω$ is an infinite sample sequence such that $x \in X$ for all $x \in ω$ . We shall use $S$ to denote a set of points, in general. $| \cdot | = card (\cdot)$ denotes the cardinality of “·” and $L (\cdot)$ denotes the Lebesgue measure of “·” in $ℝ D$ .

$X$ contains one dimension per each of the system’s degrees-of-freedom (e.g. position, rotation, joint-angles, etc.). It is often the case that the configuration space of the system has more dimensions then the physical (i.e. “real”) space in which it operates. In this case we refer to the latter as the workspace, denoted by $W \subset ℝ^{n}$ (typically $n = [2, 3]$ ), and define a mapping $w : X \to W$ from configurations to volumes of the workspace, i.e. given a configuration $x \in X$ , then $w (x) \subset W$ is the set of points in the workspace which are occupied by the system at configuration $x$ . If the obstacles are specified in terms of the workspace, $O_{i} \subset W$ , then free space is defined as $X_{free} = {x \in X | w (x) \cap X_{obs} = \emptyset}$ .

Given a function $σ : [0, 1] \to ℝ^{d}$ , total variation is defined as follows (Karaman and Frazzoli, 2011)

TV (σ) \overset{Δ}{=} sup_{n \in ℕ, 0 = t_{0} < t_{1} < \dots < t_{n} = 1} \sum_{i = 1}^{n} | σ (t_{i}) - σ (t_{i - 1}) |

We say a function $σ$ has bounded variation if $TV (σ) < \infty$ .

A path is defined to be a continuous function $σ : [0, 1] \to ℝ^{D}$ with bounded variation. A path is said to be collision-free if it does not intersect the obstacle set, i.e. $σ (τ) \in X_{free}$ , $\forall τ \in [0, 1]$ . A collision-free path is said to be feasible if it begins in the initial set and terminates in the goal set, i.e. $σ (0) \in X_{start}$ and $σ (1) \in X_{goal}$ .

For the sake of our discussion, we will assume that sampling-based algorithms return a sequence of graphs. Let $G_{n} (ω) = (V_{n} (ω), E_{n} (ω))$ denote the output of the algorithm, given the first $n$ elements of a sample sequence $ω$ , where $G_{n} (ω)$ is the search graph defined by a set of nodes $V_{n} (ω)$ and the set of edges between them $E_{n} (ω)$ .

$I_{path} (n)$ is an indicator random variable for the event that the $n$ th sample requires a “normal” collision check. $p_{cc} (n) = E [I (n)]$ is the related expectation.

The computational complexity of a sampling-based algorithm can be decomposed in terms of the complexity of its primitive operations. $c_{sample}$ , $c_{plan}$ , $c_{cc} (X_{obs})$ , and $c_{path} (X_{obs})$ are upper-bounds on the complexity of drawing a sample, calculating a trajectory, collision checking a point, and collision checking a trajectory, respectively. The former two are constants, while the latter two are functions of the obstacle representation.

3.1. Feasible motion planning

The feasible motion planning problem is to find a feasible path if one exists or to determine that one does not. Formally, given $X_{free}$ , $X_{start}$ , and $X_{goal}$ , find $σ : [0, 1] \to X_{free}$ subject to $σ \in C^{0}$ , $TV (σ) < \infty$ , $σ (0) \in X_{start}$ , and $σ (1) \in X_{goal}$ , where $σ = \emptyset$ if and only if all constraints are not met.

3.2. Optimal motion planning

Let us define $Σ$ as the set of all solutions to a feasible motion planning problem. A functional $J : Σ \to ℝ_{\geq 0}$ is said to be a cost functional if it assigns a non-negative value (cost) to all non-trivial paths, i.e. $J (σ) = 0$ if and only if $σ (τ) = σ (0)$ , $\forall τ \in (0, 1]$ (Karaman and Frazzoli, 2011).

The optimal motion planning problem is to find a feasible path with minimum cost provided one exists, or determine whether no feasible path exists. Formally, given $X_{free}$ , $X_{start}$ , $X_{goal}$ , and $J$ , find $\underset{σ}{argmin} (J (σ))$ subject to $σ : [0, 1] \to X_{free}$ , $σ \in C^{0}$ , $TV (σ) < \infty$ , $σ (0) \in X_{start}$ , and $σ (1) \in X_{goal}$ .

4. Certificate algorithm

In this section we present the most basic version of the certificate method: that in which certificates are defined as balls in the configuration space. This version has the advantage of being simple while also introducing all of the machinery required for in-depth analysis (presented in Section 5). Although this basic method can be applied directly to a handful of simple motion planning problems, its main purpose is to provide a strong foundation from which an entire family of more practical implementations can be generalized. In practice, we expect that each of the latter will have additional complications, for example, as a result of being tailor-made to solve a particular problem (e.g. those in Sections 6 and 7) or for the sake of compatibility with an existing motion planning code-base. In this section we ignore these complications and focus on the heart of the general idea itself.

Primitive subroutines and data structures are formalized in Section 4.1, while the resulting augmented collision-checking procedures are presented in Section 4.2. Versions of RRT and PRM* that use the augmented collision-checking procedures are presented in Section 4.3.

4.1. Data structures and primitive subroutines

4.1.1. Data structure

Any version of the certificate method works by storing additional data within the graph data structure that is already used by most sampling-based algorithms. Let $AG = (V, E, S_{free}, S_{obs}, Dist)$ represent the “augmented graph” $5$ -tuple that we use for the most basic version of the idea presented here. $V \subset X_{free}$ and $E \subset V \times V$ are the usual vertex and edge sets that are standard data fields in most sampling-based motion planning algorithms. We additionally include the sets $S_{free} \subset X_{free}$ and $S_{obs} \subset X_{obs}$ , which are sets of points for which an explicit collision check has been made. Finally, we include the map $Dist : S_{free} \cup S_{obs} \to ℝ_{\geq 0}$ , which stores a lower-bound on minimum distance to the obstacle set for points in $S_{free}$ (or to the free space for points in $S_{obs}$ ). In the simple case presented in this section, $Dist (x)$ for $x \in X$ defines the radius of the ball that is used for the certificate.

$V$ and $E$ are initialized according to the particular sampling-based algorithm in use (e.g. RRT, PRM*, etc). $S_{free}$ and $S_{obs}$ are initialized as empty sets. In practice, and assuming that a node $v$ is located at a point $x$ for $x \in (S_{free} \cup S_{obs})$ , it is convenient to store the results of $Dist (x)$ as a sub-field of the data structure used to define $v$ .

4.1.2. Sampling

$ω$ is an infinite sequence of samples from $X$ . The subroutine ${Sample}_{i} (ω)$ returns the $i$ th element of $ω$ . Formally, $: ω \to {{Sample}_{i} (ω)}_{i \in ℕ} \subset X$ . We assume that all samples $x \in ω$ are independent and identically distributed (i.i.d.). We also assume that samples are generated from a uniform distribution. However, our results naturally extend to any absolutely continuous distribution with density bounded away from zero on $X$ .

4.1.3. Nearest, k-nearest, and near neighbors

The subroutine $Nearest (S, x)$ returns the member of the point set $S$ that is closest to $x$ , i.e. $Nearest : (S, x) \to s \in S$ such that

Nearest (S, x) \overset{Δ}{=} \underset{s \in S}{argmin} ∥ x - s ∥

where $S \subset X$ by construction of the sampling-based motion planning algorithm and $card (S) < \infty$ .

$kNearest (S, x, k)$ returns the $k$ nearest neighbors of $x$ in $S$ with respect to ∥·∥. That is, $kNearest : (S, x, k) \to {s_{1}, s_{2}, \dots, s_{k}}$ . By convention, $kNearest (S, x, k)$ returns $S$ whenever $card (S) < k$ .

Similarly, $Near (S, x, r)$ returns the set $S_{near}$ containing all members of $S$ such that $s \in S | ∥ s - x ∥ \leq r$ , where $r \in ℝ_{> 0}$ is a positive real number. Formally, $Near : (S, x, r) \to S_{near} \subseteq S$ such that

Near (S, x, r) \overset{Δ}{=} {s \in S | ∥ s - x ∥ \leq r}

4.1.4. Set distance

The subroutine $SetDistance (S, x)$ returns a non-trivial lower bound on the minimum distance from a point $x$ to $S$ , i.e. for some $α \in (0, 1]$ . We assume that $S \subset X$ is a closed set and that the point $x \in X$ . Formally

α min_{s \in S} ∥ s - x ∥ \leq SetDistance (S, x) \leq min_{s \in S} ∥ s - x ∥

In particular, $SetDistance (S, x)$ is used to determine the distance to obstacles and thus the size of certificates. In practice, tighter bounds are desirable, assuming that they can be calculated efficiently. Additional discussion regarding $α$ can be found in Section 5.

4.1.5. Segment collision test

The subroutine $CFree$ $Path (x, y)$ returns $True$ if the path segment between points $x$ and $y$ lies in $X_{free}$ , otherwise it returns $False$ . Formally

CFreePath (x, y) \overset{Δ}{=} (\begin{matrix} True, & if [x, y] \cap X_{free} = [x, y] \\ False, & if [x, y] \cap (X \ X_{free}) \neq \emptyset \end{matrix}

assuming that $x, y \in X$ . In the most basic case of a single integrator holonomic system $[x, y]$ is a line segment, but in more complicated cases it may be some other continuous curve through $X$ .

4.1.6. Certifying node

It is advantageous for each node $x \in V$ to have access to the node that originally certified it as collision-free. In our pseudocode the subroutine $CertifierOf (x)$ returns the point $y$ such that $y$ certified $x$ when the node located at $x$ was added to $V$ . In practice, we expect that this will be implemented by simply storing a pointer to $y$ directly in the data structure of the node at $x$ .

4.1.7. Furthest safe point along a trajectory

The subroutine $FarSafe (x, y)$ returns the furthest point along the trajectory $[x, y]$ that is certified by the same certificate as $x$ (without leaving and then re-entering that certificate). Formally, let $σ_{traj} : [0, 1] \to [x, y]$ be the function that maps $[0, 1]$ to trajectory $[x, y]$ . Let $c$ be the point that certifies $x$ . Recall that $Dist (c)$ returns a bound on the distance between $c$ and the obstacle set (as defined in Section 4.1.1). A point $x'$ that is certified by $c$ has the property $∥ x' - c ∥ \leq Dist (c)$

\begin{matrix} FarSafe (x, y) \overset{Δ}{=} σ_{traj} (t^{*}) where \\ t^{*} = \underset{t \in [0, 1]}{argmax} \\ (t such that, for all t' \in [0, t], ∥ σ_{traj} (t') - c ∥ \leq Dist (c)) \end{matrix}

where $c = CertifierOf (x)$ . In practice, this can be relaxed to return any point $σ_{traj} (\tilde{t})$ , such that $0 \leq \tilde{t} \leq t^{*}$ , without invalidating the correctness of the algorithm (note that this relaxation has a similar effect to using a worse $α$ ). That said, using larger $\tilde{t} \leq t^{*}$ will yield better performance, assuming this can achieved in a similar computation time.

4.2. Modified collision-checking procedures

Our modified point and paths collision-checking procedures are shown in Algorithms 1 and 2, respectively. For convenience, we assume the augmented data structure $AG$ can be accessed directly and do not list it as an input.

Algorithm 1. $CFreePoint (x_{q})$
1 $x_{free} \leftarrow Nearest (S_{free}, x_{q})$
2 $x_{obs} \leftarrow Nearest (S_{obs}, x_{q})$
3 if $∥ x_{q} - x_{free} ∥ \leq Dist (x_{free})$ then
4 $CertifierOf (x_{q}) \leftarrow x_{free}$
5 return $True$
6 else if $∥ x_{q} - x_{obs} ∥ \leq Dist (x_{obs})$ then
7 return $False$
8 $d_{obs} \leftarrow SetDistance (X_{obs}, x_{q})$
9 if $d_{obs} > 0$ then
10 $S_{free} \leftarrow S_{free} \cup {x_{q}}$
11 $Dist (x_{q}) \leftarrow d_{obs}$ ;
12 $CertifierOf (x_{q}) \leftarrow x_{q}$
13 return $True$
14 else
15 $S_{obs} \leftarrow S_{obs} \cup {x_{q}}$
16 $Dist (x_{q}) \leftarrow SetDistance (X_{free}, x_{q})$
17 return $False$

Algorithm 2. $BatchCFreePath (S_{near}, x_{q})$
1 $H \leftarrow \emptyset$
2 $c_{q} \leftarrow CertifierOf (x_{q})$
3 foreach $x \in S_{near}$ do
4 if $∥ x - c_{q} ∥ \leq Dist (c_{q})$ then
5 $H \leftarrow H \cup {(x, x_{q})}$
6 else
7 $c \leftarrow CertifierOf (x)$
8 if $∥ FarSafe (x_{q}, x) - c ∥ \leq Dist (c)$ then
9 $H \leftarrow H \cup {(x, x_{q})}$
10 else if $CFreePath (x, x_{q})$ then
11 $H \leftarrow H \cup {(x, x_{q})}$
12 return $H$

4.2.1. Point collision test

The point collision-checking procedure $CFreePoint (x_{q})$ appears in Algorithm 1. Given a query point $x_{q} \in X$ , it returns $True$ if $x_{q} \in X_{free}$ and $False$ otherwise, i.e.

CFreePoint (x_{q}) \overset{Δ}{=} (\begin{matrix} True, & if x_{q} \in X_{free} \\ False, & if x_{q} \notin X_{free} \end{matrix}

If possible, we use previously computed certificate information to quickly determine if $x_{q}$ is guaranteed to be either collision-free or in-collision, lines 1.1–1.7. Let $x_{free} \in S_{free}$ be the configuration nearest to $x_{q}$ that owns a collision certificate (i.e. for which $Dist (\cdot)$ is defined). $x_{free}$ is found on line 1.1. If $x_{q}$ is closer to $x_{free}$ than $x_{free}$ is to an obstacle, then clearly $x_{q}$ is collision-free, lines 1.4–1.5. In this case $x_{free}$ “certifies” $x_{q}$ as being collision-free (or, alternatively, $x_{q}$ is within the certificate of $x_{free}$ ) and is recorded as doing so on line 1.4. Similarly, let $x_{obs} \in S_{obs}$ be the configuration nearest to $x_{q}$ that owns a collision certificate. $x_{obs}$ is found on line 1.2. If $x_{q}$ is closer to $x_{obs}$ than $x_{obs}$ is to the free space, then clearly $x_{q}$ is in-collision, lines 1.6–1.7. In this case $x_{free}$ certifies $x_{q}$ as being in-collision (i.e. $x_{q}$ is within the certificate of $x_{obs}$ ).

If the existing certificate information is insufficient to quickly determine the collision status of $x_{q}$ (i.e. when the two checks discussed above prove inconclusive), then an explicit check is performed using the underlying collision-checking data structure (line 1.8). Note that the latter is essentially a “standard” collision check, except that it returns $d_{obs}$ the approximate minimum distances from $x_{q}$ to the obstacle set. If $d_{obs} > 0$ , then $x_{q}$ is in $X_{free}$ and so it is added to $S_{free}$ (the set of vertices that have been explicitly determined to be collision-free), lines 1.9–1.10, and $Dist (x_{q})$ is set to $d_{obs}$ , line 1.11, and recorded as certifying itself, line 1.12. Otherwise, $x_{q}$ must be in-collision, i.e. $x_{q} \in X_{obs}$ . We add it to the set $S_{obs}$ (the set of vertices that have been explicitly determined to be in-collision) and record its distance to the free set in $Dist (x_{q})$ (lines 1.14–1.16).

4.2.2. Path set collision test

The procedure BatchC FreePath $(S_{near}, x_{q})$ appears in Algorithm 2 and returns the collision-free subset of all edges from $x_{q}$ to members of $S_{near}$ (as well as vice versa if the motion graph is directed), where $x_{q}$ is a query point and $S_{near}$ depends on the particular planning algorithm being used, as follows:

RRT: $S_{near} = Nearest (V, x_{q})$ ;

$k$ -PRM: $S_{near} = kNearest (V, x_{q}, k)$ ;

RRT* and PRM*: $S_{near}$ is either the expected $O (\log n)$ points within a ball of volume scaling as $\log (n) / n$ centered at $x_{q}$ , or the $(k \log n)$ -nearest neighbors to $x_{q}$ (depending on the particular implementation being used).

Formally, $BatchCFreePath (S_{near}, x_{q})$ returns the edge set $H \subseteq \cup {(x, x_{q})}$ , where $x \in S_{near}$ , such that $CollisionFreePath (x, x_{q})$ evaluates to $True$ . Note that the simple version presented here assumes an undirected graph structure within the search graph. While modifying the algorithm to work with directed search-graphs is straightforward, merely requiring that $(x, x_{q})$ and $(x_{q}, x)$ are handled individually, we ignore this complication in the interest of simplicity and brevity. In practice, $CFreePoint (x_{q})$ should be called prior to $BatchCFreePath (S, x_{q})$ in order to guarantee that $x_{q}$ is collision-free. We assume this is the case, and thus we are guaranteed that $∥ x_{q} - c_{q} ∥ \leq Dist (c_{q})$ , where $c_{q}$ is the point that certifies $x_{q}$ .

Let $x_{near} \in S_{free}$ be the vertex in $S_{free}$ that is nearest to $x_{q}$ , line 2.2 (i.e. $x_{near}$ is the particular point that certifies $x_{q}$ ). For each pair $(x, x_{q})$ such that $x \in S_{near}$ , the first step is to check whether the path segment $[x, x_{q}]$ can be declared collision-free using data from $AG$ (i.e. using certificates), lines 2.4–2.9. The details of how this procedure uses certificates to eliminate most of the (normal) collision checks depend greatly on practical details such as how certificates are defined and the shape of path-segment trajectories. In general, we seek to ensure that all points on $[x, x_{q}]$ are collision-free, which can be accomplished by showing that $[x, x_{q}]$ is covered by certificates.

In the easiest case, and the one assumed in this section, $[x, x_{q}]$ follows a geodesic of $X$ when moving between $x$ and $x_{q}$ and the distance metric is such that certificates are balls. The former is a common assumption in asymptotically optimal sampling-based motion planning, while the latter happens, for instance, in Euclidean spaces. (We note that the use of balls can immediately be relaxed to include any convex shape with positive finite measure without altering the discussion that follows.) Under these conditions $[x, x_{q}]$ is guaranteed to be collision-free whenever both $x$ and $x_{q}$ are closer to $x_{near}$ than $x_{near}$ is to the obstacle set (see Figure 4). This is checked on lines 2.4–2.5 using the assumption that $CFreePoint (x_{q})$ has previously been called by the motion planning algorithm, guaranteeing $∥ x_{q} - x_{near} ∥ \leq Dist (v_{near})$ .

Fig. 4.

Two certificates (blue and green) defined by points within distances (dotted lines) that are, respectively, less than or equal to $Dist (c_{1})$ from point $c_{1}$ and less than or equal to $Dist (c_{2})$ from point $c_{2}$ . Dashed arrows represent software pointers from each point to the point that certifies it. Solid black lines represent path segments. Path segment $[x_{1}, x_{2}]$ is guaranteed to be collision-free because it exists completely within a single certificate. Path segment $[x_{3}, x_{4}]$ is guaranteed to be collision-free because it exists within the union of both certificates. In practice the latter is calculated by determining that the furthest point along $[x_{3}, x_{4}]$ within the blue certificate, $y = FarSafe (x_{3}, x_{4})$ , is also within the green certificate.

Even if this first certificate-based test fails, it is still possible to guarantee $[x, x_{q}]$ is collision-free if another point $c$ certifies $x$ and every point along $[x, x_{q}]$ is either in the certificate of $x$ or the certificate of $x_{q}$ (also in Figure 4). This is checked on lines 2.7–2.9, by testing if the point returned by $FarSafe (x_{q}, x)$ , which is guaranteed to be in the certificate of $x_{q}$ , is also in the certificate of $x$ . While additional tests could be concatenated in a similar way to handle cases when $[x, x_{q}]$ is covered by no less than $i > 2$ certificates, such a strategy requires additional nearest-neighbor queries and has quickly diminishing returns. Therefore, we recommend against doing this.

In the event that certificate-based checks are inconclusive, then a full explicit collision check is performed using a “standard” path collision checker, lines 2.10–2.11.

4.3. Certificate examples with RRT and PRM*

We now show how RRT and PRM*, two popular sampling-based motion planning algorithms, can be modified to use our certificate method. Appropriately modified versions of RRT and PRM* appear in Algorithms 3 and 4, respectively. Note that the pseudo-code used here is based on that by Karaman and Frazzoli (2011). $AG$ is initialized on lines 3.1/4.1. Standard point collision checks are replaced with $CFreePoint$ (i.e. Algorithm 1), lines 3.4/4.4. (The quantity $γ_{PRM} (\log (| V |) | V |)^{1 / d}$ , appearing on line 4.7, is a parameter PRM* uses to determine the radius in which neighbor connections are considered, i.e. as a function of graph size). Neighbor connections are evaluated in a batch manner via a single call to $BatchCFreePath$ (i.e. Algorithm 2), lines 3.6/ 4.8.

Algorithm 3. Modified RRT
1 $V \leftarrow {x_{init}}$ ; $E \leftarrow \emptyset$ ; $S_{free} \leftarrow \emptyset$ ; $S_{obs} \leftarrow \emptyset$ ;
2 for $i = 1, \dots, n - 1$ do
3 $x_{rand} \leftarrow {Sample}_{i} (ω)$
4 if $CFreePoint (x_{rand})$ then
5 $x_{nearest} \leftarrow Nearest (V, x_{rand})$ ;
6 $H \leftarrow BatchCFreePath ({x_{Nearest}}, x_{rand})$ ;
7 if $H \neq \emptyset$ then
8 $V \leftarrow V \cup x_{rand}$ ;
9 $E \leftarrow E \cup H$ ;
10 return $G = (V, E)$ ;

Algorithm 4. Modified PRM*
1 $V \leftarrow {x_{init}}$ ; $E \leftarrow \emptyset$ ; $S_{free} \leftarrow \emptyset$ ; $S_{obs} \leftarrow \emptyset$ ;
2 for $i = 1, \dots, n - 1$ do
3 $x_{rand} \leftarrow {Sample}_{i} (ω)$
4 if $CFreePoint (x_{rand})$ then
5 $V \leftarrow V \cup {x_{rand}}$
6 foreach $x \in V$ do
7 $S \leftarrow Near (V, x, γ_{PRM} {(\log (\| V \|) \| V \|)}^{1 / d})$ ;
8 $H \leftarrow BatchCFreePath (S \ {x}, x)$ ;
9 $E \leftarrow E \cup H$ ;
10 return $G = (V, E)$ ;

5. Analysis

Our collision certificate method is designed to be used with many different sampling-based motion planning algorithms. In order to make our analysis as general as possible, we only assume that the particular sampling-based motion planing algorithm being used has the same general structure that is shared by RRT, RRT*, PRM, PRM*, etc. Let $SMP$ denote a particular sampling-based motion planning algorithm, for example, RRT or PRM*.

The results derived in this section hold if $SMP$ satisfies the first two following assumptions and obstacles satisfy the third:

(A1) SMP calls the $CFreePoint$ procedure $O (1)$ times per iteration;

(A2) at the $n$ th iteration, $SMP$ calls $BatchCFreePath$ procedure with $O (\log n)$ paths, in expectation, and such paths lie inside a ball of radius $o (| V |^{- 1 / (d + 1)})$ , almost surely, as $n \to \infty$ ;

(A3) obstacle boundaries are locally Lipschitz-continuous and have finite measure,

where the $n$ th sample is drawn during the $n$ th iteration of $SMP$ by convention. Assumption (A1) guarantees that a constant finite bound exists on the number of points added per iteration. Assumptions (A2) and (A3) are needed to ensure the asymptotic collision-checking behavior result of Section 5.1. We note that (A2) is met by many popular sampling-based motion planning algorithms, including those that employ a shrinking neighborhood ball to ensure asymptotic optimality (PRM*, RRG, RRT*, RRT^#, RRT^X), as well as those that iteratively connect a new point to a fixed number of nearest neighbors (RRT, $k$ -nearest PRM). Assumption (A2) does not hold for the $r$ -disc-graph-like version of PRM that used a fixed connection radius $r$ . The validity of Assumption (A3) technically depends on both the motion planning problem being solved and the particular way that obstacles are represented in memory. That said, the danger of violating Assumption (A3) accidentally while applying our method to practical robotics problems is remote. For example, obstacles would need to be endowed with fractal like surfaces and/or infinite surface area. Both Assumptions (A1) and (A2) are needed to ensure the per-sample runtime of the underlying sampling-based motion planning algorithms.

Let $I_{point} (n)$ denote the indicator random variable for the event that the $SetDistance$ procedure (see Algorithm 1) is called during the $n$ th iteration of $SMP$ . Similarly, let $I_{path} (n)$ be the indicator random variable for the event that the $CFreePath$ procedure in Algorithm 2 is called during the $n$ th iteration of $SMP$ . Define $I (n) \overset{Δ}{=} I_{point} (n) + I_{path} (n)$ , and let $p_{cc} (n) = E [I (n)]$ .

5.1. Asymptotic collision-checking behavior

Our main result is stated in Theorem 1.

Theorem 1 Assuming (A1), (A2), and (A3) hold and $SMP$ is implemented using the proposed collision-checking certificate algorithm, then $SMP$ has zero asymptotic expected incremental set-distance complexity, i.e.

\underset{n \to \infty}{lim sup} p_{cc} (n) = 0

The rest of this section is devoted to proving Theorem 1. The key idea is to use a convenient partitioning to divide the state space into several cells (see Figures 5 and 6). This allows us to compute a bound on the expected number of samples that require the set-distance procedure to be called in Algorithm 1. Finally, we show that this number grows more slowly than $n$ , which implies Theorem 1.

Fig. 5.

Fig. 6.

Two close-up views of the cell partitioning of $X$ used in Lemmas 1–4. For the purposes of clarity, only the boundaries of select cells are depicted. $l n$ is the side-length of a cell and $γ_{u} l n$ is the maximum distance between any two points in the same cell, where $γ_{u}$ depends on the distance norm being used ( $∥ \cdot ∥_{\infty}$ in this figure). $α$ is used to handle cases when the $SetDistance$ procedure returns $α$ -factor bound of the actual distance to the obstacle set boundary. Assuming that $x_{1}, x_{2} \in C_{n} (z)$ such that $C_{n} (z) \in Q_{n}$ , then $x_{nearest}$ the nearest neighbor of $x_{2}$ is either $x_{1}$ or some other point $x_{a} \in N_{n} (z)$ , and so $x_{Nearest}$ is guaranteed to be at least $l n (⌈ (1 / α) γ_{u} ⌉ + 1)$ from the obstacle boundary. See Lemmas 1–4 for more details.

Consider a partitioning of the configuration space $[0, 1]^{D}$ into openly disjoint hypercube cells with edge length $l n : = n^{- \frac{1}{D + 1}}$ . Note that the number of cells used in the partition increases versus the number of samples $n$ , while the size of a particular cell shrinks. Given $z \in Z^{D}$ , the cell with index $z$ is defined as the $L^{\infty}$ ball of radius $l n / 2$ centered at $l n z$ , i.e. $C_{n} (z) : = {x' \in X : ∥ x' - l n z ∥_{\infty} \leq l n / 2},$ where $l n z$ is the scalar–vector multiplication of $z$ by $l n$ , and $∥ \cdot ∥_{\infty}$ is the infinity norm. Let $Z_{n} \subset Z D$ be the smallest set for which $C_{n} : = {C_{n} (z) : z \in Z_{n}}$ covers the configuration space, i.e. $X \subseteq ⋃_{z \in Z_{n}} C_{n} (z)$ . Note that $Z_{n}$ is a finite set because $X$ is compact.

Let $γ_{u}$ denote the maximum distance between two points in the unit hypercube using the distance metric ∥·∥—recall that ∥·∥ is employed in the $SetDistance$ procedure. For example, $γ_{u} = \sqrt{D}$ if ∥·∥ is the $L_{2}$ norm and $γ_{u} = 1$ if ∥·∥ is the $L_{\infty}$ norm.

We group the cells in $C_{n}$ into two disjoint subsets: $B_{n}$ contains all boundary cells, while $Q_{n}$ contains all interior cells, see Figure 6. Let $B_{n}^{'}$ denote the set of all $C_{n} (z) \subset C_{n}$ that include a part of the obstacle set boundary, i.e. $C_{n} (z) \cap \partial X_{obs} \neq \emptyset$ . By construction $B_{n}$ contains exactly those cells that are within $2 (⌈ (1 / α) γ_{u} ⌉ + 1)$ cell distance to some cell in $B_{n}^{'}$ . Formally

\begin{matrix} B_{n} : = {C_{n} (z) : ∥ z - z' ∥_{\infty} \leq 2 (⌈ (1 / α) γ_{u} ⌉ + 1) \\ for some z' with C_{n} (z') \in B_{n}^{'}} \end{matrix}

where $α$ is a constant defined in the $SetDistance$ procedure (in particular, $α$ is a constant factor that defines the tightness of the obstacle distance lower bound). Finally, $Q_{n}$ is defined as the set of all cells that are not boundary cells, i.e. $Q_{n} : = C_{n} \ B_{n}$ . The choice of the number $2 (⌈ (1 / α) γ_{u} ⌉ + 1)$ will become clear shortly.

Recall that $L (\cdot)$ is used to denote the Lebesgue measure in $ℝ D$ . The total number of cells $K_{n}$ can be bounded for all $n$ as follows³

K_{n} \leq \frac{L (X)}{L (C_{n} (z))} = \frac{L (X)}{{(n^{- \frac{1}{D + 1}})}^{D}} = L (X) n^{\frac{D}{D + 1}}

(1)

Note that $K_{n}$ is an increasing and sub-linear function of $n$ .

Recall that $| B_{n} | = card (B_{n})$ denotes the number of boundary cells.

Lemma 1. There exists a constant $c_{1} > 0$ such that $| B_{n} | \leq c_{1} (K_{n})^{1 - 1 / D}$ for all $n \in ℕ$ .

Proof. This is true by construction and Assumption (A3). In particular, if $K_{n}$ is the number of equal-size cells that cover the configuration space, then the obstacle boundary is covered by $c_{1} {K_{n}}^{(D - 1) / D} = c_{1} {K_{n}}^{1 - 1 / D}$ cells.

The validity of Lemma 1 stems from the fact that, given Assumption (A3), obstacle boundaries are essentially $(D - 1)$ -dimensional structures embedded in a $D$ -dimensional state space. The following two paragraphs respectively describe two self-contained examples designed to highlight the geometric aspects of Lemma 1. These are depicted in Figures 7 and 8, respectively. These examples are included to provide intuition; they are not part of the proof of Lemma 1.

Fig. 7.

The $[0, 1]^{D}$ configuration space $X$ (dashed) is partitioned into openly disjoint hypercube cells with edge length $l n : = n^{- \frac{1}{D + 1}}$ , where $n$ is the number of points that have been inserted into the motion planning graph (not shown). Left to right depict the partitioning for increasing values of $n$ , respectively. A single non-convex polytopal obstacle is divided into three convex polytopal obstacles $O_{1}$ , $O_{2}$ , $O_{3}$ (dark black lines). Shaded cells contain part of an obstacle boundary (light gray, wavy, dark gray for $O_{1}$ , $O_{2}$ , $O_{3}$ , respectively). Axis numbers indicate cell indices.

Fig. 8.

The $[0, 1]^{D}$ configuration space $X$ (dashed) is partitioned into openly disjoint hypercube cells with edge length $l n : = n^{- \frac{1}{D + 1}}$ , where $n$ is the number of points that have been inserted into the motion planning graph (not shown). Left to right depict the partitioning for increasing values of $n$ , respectively. A single non-convex non-polytopal obstacle is shown. The set ${B'}_{n}$ of grids that contain part of the obstacle boundary are dark gray. The former are always inside $H_{1} \oplus B_{γ_{u} l n}$ (light gray, dotted boundary), which is the Minkowski sum of the obstacle boundary $H_{1}$ and $B_{γ_{u} l n}$ , a ball of radius $γ_{u} l n$ (striped). Axis numbers indicate cell indices.

Our first intuition-building example considers the case of polytopal obstacles. Given Assumption (A3), it is possible to decompose the obstacle space $X_{obs}$ into $c_{A} < \infty$ non-overlapping convex polytopal obstacles $O_{1}, \dots, O_{c_{A}}$ —at the cost of increasing the total number of boundary cells (i.e. new boundary cells appear wherever a concave obstacle has been split into two or more convex obstacles). $c_{A}$ can be interpreted as the minimum number of convex obstacles needed to cover the obstacle space (note that such a minimum decomposition is used here only as an analytical tool). Each of the resulting obstacles $O_{i}$ , being both convex and no larger than $X$ , requires a number of cells to cover its boundary that is no more than the number of cells required to cover the boundary of $X$ . In particular, each ( $D - 1$ )-dimensional face of (hypercube) $X$ requires $(K_{n})^{(D - 1) / D}$ cells to cover it, due to the fact that each one-dimensional edge of $X$ is $(K_{n})^{1 / D}$ cells long. There are $c_{B}$ faces of a $D$ -dimensional hypercube, so the total number of cells required to cover all obstacle boundaries is bounded by $| B_{n}^{'} | \leq c_{A} c_{B} (K_{n})^{(D - 1) / D}$ . Any cell in $B_{n}$ is within some constant cell distance $c_{C} < \infty$ of some cell in $B_{n}^{'}$ , where $c_{C}$ depends on $D$ , ∥·, and $α$ . Therefore, there exists some constant $c_{D} < \infty$ such that $| B_{n} | \leq c_{D} | B_{n}^{'} |$ , and so $| B_{n} | \leq c_{A} c_{B} c_{D} (K_{n})^{(D - 1) / D}$ .

Our second example follows. We consider a set ${O_{1}, \dots, O_{c_{A}}}$ of obstacles, where each obstacle $O_{i}$ is defined by a finite continuous surface $H_{i}$ . As in the previous example, $X$ is defined by a $D$ -dimensional hypercube (Figure 8 depicts such a case for $c_{A} = 1$ and $D = 2$ ). Let $B_{γ_{u} l n}$ be a $D$ -ball of radius $γ_{u} l n$ . By construction, the Minkowski sum $H_{i} \oplus B_{γ_{u} l n}$ will cover more volume than the set constructed from all grids containing a piece of $H_{i}$ (i.e. the subset of $B_{n}^{'}$ relevant to $O_{i}$ ). By extension, the sum of all such volumes over all $O_{i}$ is at least that contained in the union of all grids in $B_{n}^{'}$ ; formally, $L (B_{n}^{'}) \leq \sum_{i = 1}^{c_{A}} L (H_{i} \oplus B_{γ_{u} l n})$ , where we abuse our notation by letting $L (B_{n}^{'})$ denote the cumulative volume covered by all grids in $B_{n}^{'}$ . It follows from geometry and Assumption (A3) that $lim_{n \to \infty} L (H_{i} \oplus B_{γ_{u} l n}) = 2 γ_{u} l n L (H_{i})$ . In other words, as the thickness of $H_{i} \oplus B_{γ_{u} l n}$ decreases as a function of $n$ , the volume $L (H_{i} \oplus B_{γ_{u} l n})$ becomes increasingly well approximated by the surface area of $H_{i}$ multiplied by the diameter of $B_{γ_{u} l n}$ . This, along with the fact that $L (H_{i} \oplus B_{γ_{u} l n})$ is finite for all $n \in ℕ$ , guarantees the existence of a constant $c_{E}$ such that $\sum_{i = 1}^{c_{A}} L (H_{i} \oplus B_{γ_{u} l n}) \leq c_{E} \sum_{i = 1}^{c_{A}} 2 γ_{u} l n L (H_{i})$ for all $n \in ℕ$ . Let $O_{big}$ denote the obstacle with the largest surface area

O_{big} = \underset{O_{i} \in {O_{1}, \dots, O_{c_{A}}}}{argmax} L (H_{i})

Using substitution we get $L (B_{n}^{'}) \leq c_{A} c_{E} 2 γ_{u} l n L (H_{big})$ . Next, we observe that dividing $L (B_{n}^{'})$ by the volume of a grid gives, to within some constant factor, $| B_{n}^{'} |$ . Thus, we can choose a finite constant $c_{F}$ such that $| B_{n}^{'} | \leq c_{F} L (B_{n}^{'}) / l n D$ for all $n \in ℕ$ . Let $H_{X}$ denote the boundary of $X$ . We also observe that the surface area (i.e. $(D - 1)$ -dimensional hypervolume) of both $H_{X}$ and $H_{big}$ is constant versus $n$ . Thus, $L (H_{big}) = c_{G} L (H_{X})$ for some finite constant $c_{G}$ . Finally, we observe that because $X$ is a hypercube, it is possible to calculate $L (H_{X})$ by summing over the area contained in each of its $D$ -dimensional facets. This can be bounded in terms of $K_{n}$ , the total number of grids, as follows: $L (H_{X}) \leq c_{B} c_{H} (K_{n})^{(D - 1) / D} (l n)^{(D - 1)}$ , where $(K_{n})^{(D - 1) / D}$ is the number of grids per facet of $X$ and $(l n)^{(D - 1)}$ is the surface area of a single facet on a single grid, the (finite) constant $c_{B}$ is the number of facets in a $D$ -dimensional hypercube, and the finite constant $c_{H}$ is chosen to be large enough to account for the non-constant (yet deterministic) growth of $K_{n}$ versus $n$ . Combining these observations, along with the result (from the previous paragraph) that $| B_{n} | \leq c_{D} | B_{n}^{'} |$ for some constant $c_{D}$ , via substitution into $L (B_{n}^{'}) \leq c_{A} c_{E} L (H_{big}) 2 γ_{u} l n$ and then simplifying gives $| B_{n} | \leq c_{A} c_{B} c_{D} c_{E} c_{F} c_{G} c_{H} 2 γ_{u} (K_{n})^{(D - 1) / D}$ , where $γ_{u}$ is a finite constant given $D$ . This concludes our second self-contained example. We now resume building toward the proof of Theorem 1.

Lemma 1 implies that the fraction of all cells that are boundary cells is bounded by

\frac{c_{1} | B_{n} |}{K_{n}} \leq \frac{c_{1} {(K_{n})}^{1 - 1 / D}}{K_{n}} = c_{1} (K_{n})^{- 1 / D} \leq c_{1} (L (X))^{- 1 / D} n^{- \frac{1}{D + 1}} = c_{2} n^{- \frac{1}{D + 1}}

where $c_{2}$ is a constant.

We now bound the number of calls to the collision-distance procedure. This is accomplished by separately considering the mutually exclusive sets of samples that fall into interior cells and boundary cells, respectively. Recall that $S_{free}$ and $S_{obs}$ denote the sets of vertices that are explicitly checked and found to be in $X_{free}$ and $X_{obs}$ , respectively. Let $S_{checked} : = S_{free} \cup S_{obs}$ . The following lemma will later be used to derive a bound on the number of points in $S_{checked}$ that fall into interior cells.

Lemma 2. Assuming (A1) and (A2) hold, $SMP$ is implemented using the proposed collision-checking certificate algorithm and $C_{n} (z) \in Q_{n}$ is some interior cell. Then there exists at most one vertex from $S_{checked}$ in $C_{n} (z)$ , i.e.

| C_{n} (z) \cap S_{checked} | \leq 1 for all C_{n} (z) \in Q_{n}

Before proving Lemma 2, we must first introduce some additional notation and establish two intermediate results (Lemmas 3–4). Let $N_{n} (z) \subseteq C_{n}$ denote the set of all cells that have a cell distance of at most $⌈ γ_{u} ⌉ + 1$ to $C_{n} (z)$ ; formally, $N_{n} (z) : = {C_{n} (z') : ∥ z' - z ∥_{\infty} \leq ⌈ γ_{u} ⌉ + 1}$ . $N_{n} (z)$ includes the cell $C_{n} (z)$ as well as all cells that are within distance $⌈ γ_{u} ⌉ + 1$ of $C_{n} (z)$ . Note that $N_{n} (z)$ is defined such that the space occupied by its members excludes points sufficiently far from $C_{n} (z)$ , and thus $N_{n} (z)$ includes points sufficiently distant from the obstacle set boundary whenever $C_{n} (z) \in Q_{n}$ . These properties are formalized in the following two lemmas (see Figure 6 for an example illustrating the quantities used).

Lemma 3. Any point in a cell outside of $N_{n} (z)$ has a distance at least $γ_{u} l n$ to any point in $C_{n} (z)$ , i.e. $∥ x - x' ∥ \geq γ_{u} l n$ for all $x \in C_{n} (z)$ and all $x' \in C_{n} (z')$ with $C_{n} (z') \notin N_{n} (z)$ .

Proof. This is true by the construction of $N_{n} (z)$ .

Lemma 4. For all $z$ such that $C_{n} (z) \in Q_{n}$ , any point in a member of $N_{n} (z)$ is at least $(⌈ (1 / α) γ_{u} ⌉ + 1) \ln$ distant to any point on the obstacle set boundary, i.e. $∥ x - x' ∥ \geq (⌈ (1 / α) γ_{u} ⌉ + 1) \ln$ for all $x \in \partial X_{obs}$ and $x' \in C_{n} (z')$ such that $C_{n} (z') \in N_{n} (z)$ .

Proof. By construction, cell $C_{n} (z)$ is at least distance $2 (⌈ (1 / α) γ_{u} ⌉ + 1)$ from any cell that intersects the boundary of the obstacle set. Additionally, any cell in $N_{n} (z)$ is at most distance $⌈ γ_{u} ⌉ + 1 \leq ⌈ (1 / α) γ_{u} ⌉ + 1$ from $C_{n} (z)$ . Thus, by the triangle inequality, any cell in $N_{n} (z)$ is at least distance $⌈ (1 / α) γ_{u} ⌉ + 1$ away from any cell that intersects the obstacle boundary.

We can now prove Lemma 2.

Proof of Lemma 2. (Proof by contradiction). Suppose $x_{1}, x_{2} \in S_{checked}$ are two points that fall into $C_{n} (z)$ . Without loss of generality, suppose that $x_{1}$ is added to $S_{checked}$ before $x_{2}$ . Let $x_{nearest} \in S_{checked}$ denote the nearest point to $x_{2}$ that is in $S_{checked}$ (i.e. when Algorithm 1 is called with $x_{2}$ ).

First, we claim that $x_{nearest}$ must lie in some cell in $N_{n} (z)$ . Clearly, $x_{nearest}$ is either $x_{1}$ or it is some other point that is no farther from $x_{2}$ than $x_{1}$ . Note that the distance between $x_{1}$ and $x_{2}$ is at most $γ_{u} l n$ . By Lemma 3, any point that lies in a cell outside of $N_{n} (z)$ must be at least distance $γ_{u} l n$ from $x_{2}$ . Thus, $x_{nearest}$ must lie in some cell in $N_{n} (z)$ . However, Lemma 4 guarantees that $Dist (x_{nearest}) \geq (1 / α) ∥ x_{2} - x_{nearest} ∥$ , and thus $x_{2}$ cannot be in $S_{checked}$ , even when the $SetDistance$ procedure returns the $α$ -factor bound of the actual distance to the obstacle set boundary, i.e. $x_{2}$ must have been certified (either as collision-free or in-collision) by $x_{nearest}$ and therefore $x_{2}$ could not have been added to $S_{checked} = S_{free} \cup S_{obs}$ . This provides the necessary contradiction.

The following lemma (Lemma 5) provides an upper bound on the expected number of samples that fall into boundary cells. This result is then used in the subsequent lemma (Lemma 6) to calculate an upper bound on the expected number of points in $S_{checked}$ that fall into boundary cells. Let $X_{n}$ denote a set of $n$ samples drawn independently and uniformly from $X$ , and let $S_{n}$ denote the number of samples that fall into boundary cells, i.e.

S_{n} : = | {x \in X_{n} : x \in C_{n} (z) with C_{n} (z) \in B_{n}} |

Lemma 5. $E [S_{n}] \leq c_{3} n^{\frac{D}{D + 1}}$ for some constant $c_{3}$ that is independent of $n$ .

Proof. Let $E_{i}$ denote the event that the $i$ th sample falls into a boundary cell, and let $Y_{i}$ denote the indicator random variable corresponding to the event $E_{i}$ . Lemma 1 guarantees that $c_{2} n^{- \frac{1}{D + 1}}$ is the fraction of cells that are boundary cells. Thus, $E [Y_{i}] = ℙ (E_{i}) = c_{2} i^{- \frac{1}{D + 1}}$ and $E [S_{n}] = E [\sum_{i = 1}^{n} Y_{i}] = \sum_{i = 1} n E [Y_{i}] = \sum_{i = 1}^{n} c_{2} i^{- \frac{1}{D + 1}} \leq c_{2} \int_{1} n x^{- \frac{1}{D + 1}} dx$ , where

c_{2} \int_{1}^{n} x^{- \frac{1}{D + 1}} dx = \frac{c_{2} (D + 1)}{D} (n^{\frac{D}{D + 1}} - 1)

Thus, $E [S_{n}] \leq c_{3} n^{\frac{D}{D + 1}},$ where $c_{3}$ is a constant independent of $n$ .

The following provides a bound on the expected number of points in $S_{checked}$ .

Lemma 6 $E [card (S_{checked})] \leq c_{4} n^{\frac{D}{D + 1}}$ for some constant $c_{4}$ independent of $n$ .

Proof. By Lemma 2, the number of points in $S_{checked}$ that fall into an interior cell is at most the total number of interior cells. Thus the number of points in $S_{checked}$ that fall into an interior cell is less than the total number of cells $K_{n}$ , for $K_{n} \leq L (X) n^{\frac{D}{D + 1}}$ (see equation (1)). Clearly, the expected number of points in $S_{checked}$ that fall into a particular boundary cell is no more than the expected number of samples that fall into all boundary cells. By Lemma 5 the latter number is bounded by $c_{3} n^{\frac{D}{D + 1}}$ , where $c_{3}$ is a constant independent of $n$ . Thus, $E [card (S_{checked})] \leq c_{4} n^{\frac{D}{D + 1}}$ for some constant $c_{4}$ .

We are finally ready to prove Theorem 1.

Proof of Theorem 1. We begin by using a standard coupling argument to show that $p_{cc}$ is a non-increasing function of $n$ . Consider the run of the algorithm with $n + 1$ samples. The events ${I (n) = 1}$ and ${I (n + 1) = 1}$ are coupled as follows. Let $A_{n}$ and $A_{n + 1}$ denote the events that the $n$ th and the $(n + 1)$ th samples are explicitly checked, respectively. Clearly, $ℙ (A_{n + 1}) \leq ℙ (A_{n})$ in this coupled process, since the first $(n - 1)$ th samples are not affected by the act of drawing the $n$ th and $(n + 1)$ th samples. Furthermore, $ℙ (A_{n}) = ℙ ({I (n) = 1}) = p_{cc} (n)$ and $ℙ (A_{n + 1}) = ℙ ({I (n + 1) = 1}) = p_{cc} (n + 1)$ . Thus, $p_{cc} (n + 1) \leq p_{cc} (n)$ for all $n \in ℕ$ . This implies that $lim_{n \to \infty} p_{cc} (n)$ exists.

We now prove $lim_{n \to \infty} (1 / n) \sum_{k = 1}^{n} p_{cc} (k) = 0$ . Clearly, $\sum_{k = 1}^{n} I_{point} (k) = | S_{checked} |$ . Hence

\frac{\sum_{k = 1}^{n} E [I_{point} (k)]}{n} = \frac{E [\sum_{k = 1}^{n} I_{point} (k)]}{n} \leq \frac{c_{4} n^{\frac{D}{D + 1}}}{n} = c_{4} n^{- \frac{1}{D + 1}}

where the inequality follows from Lemma 6. Similarly

\underset{k \to \infty}{limsup} (E [I_{path} (k)] - E [I_{point} (k)]) = 0

since all paths fit into an Euclidean ball with radius $o (n^{- 1 / (d + 1)})$ , which is asymptotically smaller than the side-length of each cell $l_{n} = n^{- 1 / (d + 1)}$ . Hence

\begin{matrix} \underset{n \to \infty}{lim sup} \frac{\sum_{k = 1}^{n} p_{cc} (k)}{n} \\ = \underset{n \to \infty}{lim sup} (\frac{\sum_{k = 1}^{n} E [I_{point} (k)] + E [I_{path} (k)]}{n}) \\ = \underset{n \to \infty}{lim sup} \frac{E [\sum_{k = 1}^{n} I (k)]}{n} \leq \underset{n \to \infty}{limsup} \frac{c_{4} n^{\frac{D}{D + 1}}}{n} \\ = \underset{n \to \infty}{lim sup} c_{4} n^{- \frac{1}{D + 1}} = 0 \end{matrix}

Together, $p_{cc} (n + 1) \leq p_{cc} (n)$ for all $n \in ℕ$ and $lim_{n \to \infty} (1 / n) \sum_{k = 1}^{n} p_{cc} (k) = 0$ imply that $lim_{n \to \infty}$ $p_{cc} (n) = 0$ .

5.2. Effects on runtime

Recall that the complexities of drawing a sample and performing a collision check are bounded by $c_{sample}$ and $c_{cc} (X_{obs})$ , respectively, where the former is a constant and the latter is a function that depends on the obstacle description. The expected complexity of checking collisions with $n_{obs}$ convex obstacles is $O (\log (n_{obs}) D)$ (Samet, 2006). While a variety of methods achieve this bound⁴ (see, e.g., Samet, 2006), our discussion in this section does not assume any particular method is used; we assume only that whatever method is used runs in $O (\log (n_{obs}) D)$ .

The complexity of (approximate) proximity searches⁵ that return the $O (\log n)$ nearest elements to a query point from a set of cardinality $n$ is $O (\log n)$ (Samet, 2006). The latter applies to both $k$ -nearest-neighbor searches (both for a fixed $k$ and for $k$ scaling as $\log n$ ) and range searches among uniform random samples for those points within a ball of volume scaling as $\log (n) / n$ . Again, the discussion in this section assumes only that the method used runs in time $O (\log n)$ and does not assume a particular method is used.

The complexity of generating a trajectory $σ$ between two points (i.e. with a procedure often called the “local steering function”) is bounded by an environmentally independent constant $c_{plan}$ . For convenience we assume that $c_{plan}$ also includes the time required by all other bookkeeping operations that run in time $O (1)$ per sample point. Local trajectories (generated by the local steering function) must also be collision checked. The time required for a single trajectory collision check is bounded by a constant $c_{path} (X_{obs})$ that depends on the particular obstacle description that is used.

Using our certificate method causes standard collision checks to be replaced with approximate minimum-distance computations. In particular, we require a lower bound $\bar{d}$ on the minimum distance $d^{*}$ that satisfies $α d^{*} \leq \bar{d} \leq d^{*}$ , for some $α \in (0, 1]$ (i.e. a non-trivial lower bound on distance from a point in free space to an obstacle or the equivalent for a point in-collision to free space). The computational cost of such a procedure depends on the description of the environment and will be indicated with $c_{dist} (X_{obs})$ . In many cases $c_{dist} (X_{obs})$ is relatively small versus the time required for standard collision checking.

In general, the expected iterative complexity of sampling-based motion planning algorithms is a function of the size of the potential neighbor set of a candidate node, as well as which points and trajectories are collision checked. The following list outlines the major steps of four common motion planning algorithms.

RRT: Sample a point, find the nearest neighbor, saturate (generate a new point between the sample and its nearest neighbor), check the new point for collision, and connect the new point to the nearest neighbor (trajectory collision check).

$k$ -PRM: Sample a point, collision check the new point, find the new point’s $k$ -nearest neighbors, and connect to reachable subset of the $k$ -nearest neighbors ( $k$ trajectory collision checks).

RRT*, PRM*: Sample a point, saturate, collision check the sample, find the neighbors (all nodes within a ball of volume scaling as $\log (n) / n$ —or, alternatively, the $(k \log n)$ -nearest neighbors, for a fixed $k$ ), and connect to reachable subset of the aforementioned nodes (one trajectory collision check per member of the neighbor set).

Table 1 summarizes the resulting expected incremental complexities of these four algorithms—both with and without the modifications necessary to use collision certificates. Recall that local planning and trajectory collision checking occur only if a new sample is not in-collision. Note that $\underset{n \to \infty}{limsup} p_{cc} (n) = 0$ (see Theorem 1) as the number of samples increases. Thus the computational cost of collision detection is shifted more-and-more from standard collision checking to a nearest-neighbor search as $n$ increases. Indeed, this is a nearest-neighbor search that is already used for the graph-building aspects by most sampling-based motion planning algorithms.

Table 1.

Asymptotic bounds on the expected incremental complexity of standard sampling-based motion planning algorithms (with and without using certificates). Note that $\underset{n \to \infty}{limsup} p_{cc} (n) = 0$ (see Theorem 1). $c_{sample}$ , $c_{plan}$ , $c_{cc} (X_{obs})$ , and $c_{path} (X_{obs})$ are upper bounds on the complexity of drawing a sample, calculating a trajectory, collision checking a point, and collision checking a trajectory, respectively.

Algorithm	Proximity search	Point collision checking	Local planning	Path collision checking
RRT	$O (\log n)$	$c_{cc} (X_{obs})$	$c_{plan}$	$c_{path} (X_{obs})$
With certificate	$O (\log n)$	$O (\log n) + p_{cc} (n) c_{cc} (X_{obs})$	$c_{plan}$	$p_{cc} (n) c_{path} (X_{obs})$
$k$ -PRM	$O (\log n)$	$c_{cc} (X_{obs})$	$k c_{plan}$	$k c_{path} (X_{obs})$
With certificate	$O (\log n)$	$O (\log n) + p_{cc} (n) c_{cc} (X_{obs})$	$k c_{plan}$	$k p_{cc} (n) c_{path} (X_{obs})$
RRT, PRM	$O (\log n)$	$c_{cc} (X_{obs})$	$O (\log n) c_{plan}$	$O (\log n) c_{path} (X_{obs})$
With certificate	$O (\log n)$	$O (\log n) + p_{cc} (n) c_{cc} (X_{obs})$	$O (\log n) c_{plan}$	$O (\log n) p_{cc} (n) c_{path} (X_{obs})$

Our main result from Section 5.1 shows that certificates will approach a covering of the space $X$ as $n \to \infty$ , even if we can only calculate a non-trivial lower bound on point-obstacle distance, as parameterized by $α$ . Obviously, tighter lower bounds will cause faster convergence (versus $n$ ) toward the full covering due to the fact that each point will prevent future (normal) collision checks in a greater subset of $X$ . Assuming all other factors remain unchanged, using a tighter $α$ will decrease expected per-sample runtime. That said, the tightness of the bound is likely to be dependent on the obstacle representation used in practice and the motion planning problem being solved—and therefore fixed for a particular application.

If a system is designed such that it is possible to either set $α$ or tune $α$ “on-the-fly” (note that such an ability would only make sense if calculating a closer bound requires more work than calculating a loose bound), then choosing the correct “ $α$ ” must be done with some care. While in-depth discussion of this trade-off is beyond the scope of our current paper, we note that the bound should only be tightened as long as the marginal decrease in future collision-checking time (due to more nodes falling within an old certificate) is greater than the marginal increase in computation time (required for a single point-obstacle distance query).

6. Extension 1: workspace certificates

The basic collision certificate that has been presented so far has assumed that certificates are located in the configuration space. In this section we show one way that certificates can alternatively be used in the workspace to solve problems involving obstacles and robots that are modeled as sets of polytopes.

The use of certificates within a particular workspace is intimately related to both the robotic system itself and the obstacle representation that is used to model the environment. Any non-trivial workspace representation involves many practical details that we have been able to ignore until now. However, in this section we must ground our discussion in a particular obstacle representation and thus must flesh-out many of these details.

The main point of this section is to provide a proof-of-concept that certificates can be used directly in the workspace for a family of non-trivial motion planning problems. We believe that the particular representation we choose to study is widely applicable and the implementation details that we discuss are well motivated. However, we make no claim that these are the optimal representation or implementation for using certificates in the workspace, in general. Moreover, the idea may not even be immediately applicable to many popular “off-the-shelf” motion planning code packages.

6.1. Workspace representation and additional notation

We assume that the workspace is three-dimensional $W \subset ℝ^{3}$ , the robot and obstacles are each modeled as sets of polytopes in the workspace, and that each certificate is defined by the interior of a polytope in the workspace (see Figure 9). A polytope $P = (V, F)$ is a volumetric set of points defined by a set of vertices $V = {v_{j}}$ and a set of bounding planes $F = {f_{i}}$ , where $V$ and $F$ are mutually dependent. (Note that these vertices should not be confused with graph nodes in the configuration space). Each plane is defined by a normal vector $n$ and an offset distance $d$ . Due to convexity, for $P \subset ℝ^{n}$ , $n = 2, 3$ , we define the $i$ th face $f_{i} \subset P$ as the set of points $x$ satisfying $n_{i} \cdot x = d_{i}$ and $n_{j} \cdot x \leq d_{j}$ for $j \neq i$ . With some abuse of notation, we say face $f_{i} = (n_{i}, d_{i})$ . For our purposes, we assume $P \subset W$ . We will often refer to a polytope as a “volume,” for example, a polytope used as an obstacle is a “collision volume” and a polytope used for a certificate is a “certificate volume”.

Fig. 9.

Example of a workspace certificate for an L-shaped volume found from conservative dilation of the volume faces by the collision distance. The example volume is in green, the obstacles in red, and the certificate volumes in translucent blue.

Given a robot defined by a set of polytopes ${P_{0}, \dots, P_{m}}$ such that $P_{i} \subset W$ for $i \in$ ${0, \dots, m}$ , the configurations space is given by $X \subset ℝ^{D_{0}} \times SO (n_{0}) \times, \dots, ℝ^{D_{m}} \times SO (n_{m})$ , where $n_{i} \leq 3$ . Without loss of generality, we focus our discussion on a single polytope robot $P_{0} \subset X \subset ℝ_{D} \times SO (n)$ . In practice, robots containing multiple polytopes, including articulated arms, can be handled by considering each polytope independently according to the method presented in this section. “Robot” should be understood to mean “polytope” in what follows.

We assume that motion of the robot is constrained to translations and rotations. In general, workspace transformations between a robot’s local coordinate system and a global frame are often handled using an armature, i.e. a tree of reference frames each one referred to as a link of the armature. Each link is assigned a unique index $i$ , and we denote the reference frame of the $i$ th link as $F_{i}$ . Each link has a unique parent. We refer to the root of the armature as the single link whose parent is the global reference frame. Each reference frame in the armature is related to its parent by a rotation $R_{i}$ and a translation $T_{i}$ . Given a point $x$ represented in $F_{i}$ , the same point represented in its parent frame $F_{j}$ is given by

x_{j} = T_{i} + R_{i} x

We employ the Canny method (Canny, 1986) of interpolation to model motion between two points. The Canny method uses an interpolation that accounts for rotation based on a stereographic projection of quaternions; it becomes equivalent to linear interpolation as the magnitude of rotation goes to zero. Given two configurations $x_{a}, x_{b} \in X \subset ℝ_{D} \times SO (n)$ , the Canny method provides a trajectory $σ \subset X$ that is convenient for moving from $x_{a}$ to $x_{b}$ and both collision checking and using collision certificates in the workspace $W$ (as we will describe shortly). Formally, $σ : [0, 1] \to X \subset ℝ^{n} \times SO (n)$ , where $σ (0) = x_{a}$ , and $σ (1) = x_{b}$ . We use $s$ as the interpolation parameter along $σ$ , i.e. $s \in [0, 1]$ .

Let $F_{i, σ} (s)$ denote the transform from the local (workspace) coordinate system of link $i$ to that of its (workspace) parent as parameterized by $s \in [0, 1]$ . Thus, given the chain $J = {1, \dots, i}$ of links through the armature from the global coordinate frame $0$ to frame $i$ , it is possible to construct a transformation from link $i$ to the global coordinate frame as $Π_{j \in J} F_{j, σ} (s)$ . The chain $J$ commonly corresponds to the links in an articulated arm. However, the same formulation can be used to account for the motion of a vehicle that has undergone a sequence of motions, i.e. along the edges of motion planning graph. In the latter case $Π_{j \in J \ i} F_{j, σ} (s) = Π_{j \in J \ i} F_{j, σ} (1)$ is constant, which conveniently allows us to store the result of $Π_{j \in J \ i} F_{j, σ} (1)$ at each node in the motion planning graph in order to quickly compute

Π_{j \in J} F_{j, σ} (s) = (Π_{j \in J \ i} F_{j, σ} (1)) F_{i, σ} (s)

Appendix A contains derivations of $F_{j, σ} (s)$ Canny frame transforms that have been specifically tailored to the workspace certificate method presented in this section (additional details regarding articulated motion appear in Appendix B, and a thorough treatment also appears in Chapter 3 of Bialkowski, 2013).

In practice $F_{j, σ} (s)$ takes the form

F_{j, σ} (s) = [\begin{matrix} R_{j, σ} (s) & T_{j, σ} (s) \\ 0 & 1 \end{matrix}]

where $R_{j, σ} (s)$ and $T_{j, σ} (s)$ represent the rotational and translational components of movement along $σ$ as parameterized by $s$ . Given this matrix based machinery, it is notationally convenient to define

\underline{n} = [\begin{matrix} n \\ 1 \end{matrix}] a nd \underline{x} = [\begin{matrix} x \\ 1 \end{matrix}]

6.2. Querying workspace certificates

Let $P_{0}$ be a polytope robot undergoing a smooth motion through the workspace $W$ as described by the Canny interpolated path $σ \subset X$ . Let $P_{1}$ be a convex polytope collision certificate such that at $σ (0) \subset P_{1}$ and $P_{0} \subset P_{1}$ . We wish to determine if $P_{0}$ remains within its convex polytope certificate $P_{1}$ for $σ (s)$ for all values of the interpolation parameter $s \in [0, 1]$ . In general

\underline{n} \cdot (Π_{j \in J} F_{j, σ} (s)) \underline{x} - d \leq 0

We may restrict our focus to “Canny type A” collision predicates, where a vertex of the robot pierces a face of the certificate (see Figure 10(r)), i.e. because the robot is already inside the certificate, the certificate is a convex polytope, and faces include their vertices.

Fig. 10.

Canny type A contact occurs when a vertex of the moving polytope pierces a face of the stationary polygon (left and right). We are concerned with the case where a convex polytopal certificate is pierced by a smaller polytopal robot (right).

Theorem 2. If, at some $s \in [0, 1]$ , $σ (s)$ is such that $P_{0}$ leaves $P_{1}$ , $P_{0} \cup P_{1} \neq P_{1}$ , then there exists some vertex $v$ of $P_{0}$ and some face $f = (n, d)$ of $P_{1}$ such that

\underline{n} \cdot (Π_{j \in J} F_{j, σ} (s)) \underline{v} - d > 0

Proof. Assume that the converse is true, and there is no such vertex. Let $P_{0} \cup P_{1} \neq P_{1}$ at $s$ , and let $x$ be some non-vertex point $x \in P_{0}$ and outside of $P_{1}$ , $x \notin P_{1}$ . By definition of a polytope, $x$ may be represented by a convex combination of the vertices of $P_{0}$ . By assumption all vertices of $P_{0}$ , $v_{j} \in P_{1}$ , so by convexity of $P_{1}$ , $x$ must all be in $P_{1}$ , which is a contradiction.

Theorem 2 says that if the collision volume leaves the certificate volume the first point of contact will occur with a vertex of the collision volume moving from the interior half space of a face of the certificate volume to the exterior half space, i.e. type A predicates taking a zero value. In particular, a collision volume leaves a certificate volume at the minimum value of $s$ such that any vertex of the collision volume has a positive type A value with any face of the bounding volume.

To evaluate path containment within a certificate, we compute the Sturm polynomial (Hook and McAree, 1990) for the type A constraint associated with each vertex–face pair (vertex of the collision volume, face of the certificate volume). We then compute the Sturm value for all such constraint polynomials at the start ( $σ_{i} (0)$ ) and at the end ( $σ_{i} (1)$ ). If $σ_{i} (1) - σ_{i} (0) > 0$ for any vertex-face pair $i$ , then the collision volume exits the certificate volume at some point along that path. Otherwise the entire path lies within the certificate volume and is known to be collision free.

We note that evaluating certificate containment of a path in this way is significantly more complex than for configuration space certificates which require only computing a Euclidean distance. However, this computation requires only evaluating a set of polynomials at two values of their argument, whereas continuous collision checking requires solving a set of polynomials once for each obstacle identified in the broadphase algorithm. Furthermore, the test is applied only to a single certificate volume and not to a multitude of volumes as required in continuous collision checking.

6.3. Generating certificates from collision distances

Just as with configuration space certificates, workspace certificates can be built using the collision distance from the static collision checks. Exact distances may be computed with, for example, Lin–Canny (Lin, 1993) or extended GJK (Gilbert et al., 1988) in conjunction with a bounding volume hierarchy. Approximate collision distances similar to those used for the previous section may also be computed as a computational optimization using GJK.

Let $R_{i}$ and $d_{i}$ be the orientation and collision distance within the workspace of the $i$ th collision volume $C_{i}$ of the system that is located at a particular configuration $x_{i}$ . The Minkowski sum, $C_{i} \oplus B_{d_{i}}$ , of the $i$ th collision volume with a ball of radius $d_{i}$ yields a convex certificate whose interior is collision-free, but for which path containment is difficult to evaluate. We can select instead any polytope contained in $C_{i} \oplus B_{d_{i}}$ which also contains $C_{i}$ (see Figure 11). We may consider various certificate volume families for selecting such polytope certificates, trading off between complexity of the volume and spatial coverage, just as in how we select the collision volume itself. We now focus on using certificates which are dilated copies of the collision volume.

Fig. 11.

Example of two-dimensional certificates generated using dilated collision volume and collision distance (left) and successive pruning by supporting hyperplane of nearest obstacle (right).

A certificate volume can be generated quite easily by dilating each of the faces of the collision volume by a distance ${\tilde{d}}_{i} \leq d_{i}$ which is selected such that the surface of the certificate volume is at most $d_{i}$ from the surface of the collision volume. If two faces are adjacent with a very small angle between them (i.e. the dot product of the normals is near negative unity) then ${\tilde{d}}_{i}$ may be overly conservative, so it may be beneficial to augment the face set of the collision volume with a redundant face at the meeting edge with normal half way between the normals of the two faces (the average normal), as illustrated for a two-dimensional volume in Figure 12. Note that we may also simply assume such options where exploited in the modeling phase.

Fig. 12.

The addition of a redundant face allows for larger collision volumes.

6.4. Generating certificates by priority pruning

The method of computing certificates in Section 6.3 has the advantage of requiring only collision-distance output from the collision checker, the same as the basic method presented in Section 4. Certificates generated in this way may, however, be overly conservative in the case that a collision volume is very close to a single obstacle, but very far from any others. In this section we propose a certificate generation algorithm which is less likely to encounter this problem, but requires a deeper interaction with the standard collision-checking system. The algorithm is summarized in Algorithm 6.4.

Algorithm 5 builds a polyhedral certificate $P_{c}$ by first finding the nearest obstacle to the obstacle set. The algorithm returns the point $x \in X_{o}$ which is nearest to the collision volume (line 3). The supporting hyperplane at $x$ is a hyperplane coincident to $x$ and oriented so that the normal $n$ points from $x$ toward a nearest point in $P$ to $x$ (Gilbert et al., 1988) (line 6). This hyperplane is added as a face of $P_{c}$ , and the obstacle set is pruned to remove obstacle points on the outer half space of the supporting hyperplane (lines 7–8). The algorithm is then repeated on the remaining obstacle set until all obstacles lie outside of $P_{c}$ (line 9) or the point is found to be in collision (lines 4–5). Note that this method may actually return a polytope with some vertices and or faces unset (i.e. located at infinity); this is why use the term “polyhedral” to describe the certificate. However, it is important to note that the certificate is not concave.

Algorithm 5. $PruneCertificate (X_{o}, P)$ returns $incollision$ if $P \cap X_{o} \neq \emptyset$ . Otherwise returns a polyhedron $P_{c}$ which is collision-free and fully contains $P$ .
input: $X_{o}$ the workspace obstacle set
input: $P$ a workspace collision volume
1 initialize $P_{c} = ℝ^{n}$ ;
2 while $X_{o} \neq \emptyset$ do
3 $x \leftarrow Nearest (X_{o}, P)$ ;
4 if $x \in P$ then
5 return $incollision$ ;
6 $(n, d) \leftarrow SupportingHyperplane (P, x)$ ;
7 $P_{c} \leftarrow P_{c} \cap {x \| n \cdot x \leq d}$ ;
8 $X_{o} \leftarrow X_{o} \cap {x \| n \cdot x \leq d}$ ;
9 return $P_{c}$ ;

While this technique performs even more work than a collision-distance algorithm (which would terminate after finding the first nearest obstacle). The time complexity is now output sensitive in the number of faces of $P_{c}$ , which will depend on the distribution of obstacles around the collision volume. The trade-off being made, however, is that the collision certificate does not suffer from the same level of conservatism as the collision-distance method (see Figure 11).

7. Extension 2: exploiting symmetries

Some motion planning problems involve configuration spaces and/or workspaces with symmetries that can be exploited to make the certificate method more efficient. A canonical case, and the running example used in this section, is centralized multi-robot motion planning. We note that similar ideas can be used with the workspace collision certificate methods of Section 6.

Assuming a multi-robot team consists of $R$ members that share an environment,⁶ then the configuration space $X$ of the centralized problem is a Cartesian product of the space of each robot, $X = X_{1} \times \dots \times X_{R}$ . Points are defined $x = x_{1} \times \dots \times x_{R}$ , where $x \in X$ and $x_{i} \in X_{i}$ . Collision checking versus obstacles for any robot $i$ can be accomplished piece-wise in $X_{i}$ (robot versus robot collision checking can similarly be reduced to checking one robot versus another in a separate local coordinate system). We now explore two variations of the certificate method that are designed to exploit such symmetries.

7.1. Partial certificates

In the partial certificate variation of the certificate method each robot performs collision checking in its own projection of the full space, i.e. robot $i$ collision checks in $X_{i}$ (see Figure 13).

Fig. 13.

Partial certificate: if a point is not certified as safe with respect to a subspace projection, then only a partial collision check is required. $x = x_{1} \times x_{2} \times x_{3}$ and $y = y_{1} \times y_{2} \times y_{3}$ are two configurations in $X \subset ℝ^{3}$ . The projections $y_{1}$ and $y_{3}$ correspond to robots $1$ and $3$ and are within the certificates of $x_{1}$ and $x_{3}$ , respectively, but $y_{2}$ is not within the certificate of $x_{2}$ . Thus, only $1 / 3$ of a “full” team check is required for robot $2$ .

If $x \in X$ is not certified as either collision-free or in-collision with respect to all robots, then only a partial collision check is required versus the projection(s) that was not individually certified. For example, let two points in the team’s configuration space $X \subset ℝ^{3}$ be defined as $x = x_{1} \times x_{2} \times x_{3}$ and $y = y_{1} \times y_{2} \times y_{3}$ . If $y_{1}$ and $y_{3}$ are within the certificates of some points $x_{1}$ and $x_{2}$ , respectively, but $y_{2}$ is not within the certificate of $x_{2}$ , then only $1 / 3$ check is required to determine the safety of $y$ , i.e. because we only need to explicitly check $y_{2}$ instead of $y_{1}$ , $y_{2}$ , and $y_{3}$ .

In practice, the implementation of this method requires that each node in the motion-planning graph stores $R$ certificate pointers (i.e. instead of the single pointer required by the basic method). Storing one pointer per robot enables a new node to be certified by a combination of different old nodes and/or to calculate its own partial certificates as needed.

7.2. Shared projection

Assuming that robots are identical, then $X_{i} = X_{0}$ for all $i$ and so $X = (X_{0})^{R}$ . In this case it is possible for all robots to perform collision checking in the same copy of $X_{0}$ (see Figure 14). We call this idea Shared Projection.

Fig. 14.

Shared projection: all robots collision check in the same $D$ -dimensional projection (far right). Pointers from configuration space node projections to their collision-checking projection counterparts are depicted with blue/magenta/orange dotted lines, respectively. $x = x_{1} \times x_{2} \times x_{3}$ and $y = y_{1} \times y_{2} \times y_{3}$ are two configurations in $X$ . $x_{1}$ certifies $y_{3}$ and $x_{3}$ certifies $y_{2}$ .

This method causes $X_{0}$ to become populated with certificates $R$ times more quickly due to the fact that $R$ nodes are checked and possibly added during each iteration instead of $1$ . Thus, it requires even fewer (standard) collision checks than partial certificate. On the other hand, it requires an additional point-proximity data structure (e.g. a k-dimensional tree, otherwise known as a kd-tree) to store certificate points in $X_{0}$ .

In practice, the runtime of this variation can be improved by seeding collision certificate nearest-neighbor queries in $X_{0}$ based on the nearest neighbor as determined by normal (graph algorithm) nearest-neighbor queries in $X$ . For example, when searching a kd-tree in $X_{0}$ for the collision status of $y_{1}$ we begin the search at the location of $x_{1}$ instead of at the root of the tree, where $x$ is the point that has (already) been returned by the motion planning kd-tree search. Such a seeding is also likely to be possible with other tree-based point-proximity data structures.

8 Experiments

8.1. RRT and RRT* with certificates

In this section we perform experiments in a simulated environment to evaluate the performance of the basic certificate method. In particular, both the RRT and RRT* algorithms are used both with and without our collision certificate method. The simulated environment consists of a unit-square workspace with 150 randomly placed convex polygonal obstacles (see Figure 15(l)). A holonomic point robot starts at the bottom left corner and is tasked with reaching a goal region, a square with side-length 0.1 units, at the top right corner. In this simple case the workspace and configuration space are equivalent $X = W$ . Point-proximity (i.e. nearest-neighbor and $k$ -nearest-neighbor) queries are performed using a kd-tree (Bentley, 1975), while set distance queries are performed with a segment-Voronoi hierarchy (Aurenhammer, 1991).

Fig. 15.

(Left) The tree search tree and corresponding certificate set when $n = 2000$ for RRT*. Tree edges are black and certificates are purple. (Right) Runtimes for the four combinations of RRT and RRT* with and without the use of collision-checking certificates. Methods using certificates have “ball” appended in the legend. Each datapoint represents the mean value over 30 trials.

Thirty trials are performed per each combination of planning algorithm with and without our collision-checking modifications. All four algorithm combinations are run with the same initial random seed (i.e. so that all four use the sample sequence $ω$ ). Experiments are run on a 1.73 GHz Intel Core i7 computer with 4GB of RAM, running Linux. However, we note that the software implementation is single-threaded.

Figure 15(l) illustrates the set of collision-free balls and corresponding tree resulting after 2000 sample points have been sampled by RRT*. Note that the collision-free balls have filled a significant fraction of the free space, leaving only a small amount of area uncovered near the obstacle boundaries (white). This graphically illustrates the convergence result from Section 5, i.e collision checking becomes less likely as the number of samples increases.

Figure 15(r) depicts the wall-clock measured runtime versus tree size for each combination of RRT and RRT* with and without the use of collision-checking certificates. The proposed approach achieves greater time savings as the number of nodes in the graph increases. The RRT* time for $10^{4}$ and $10^{5}$ vertices is reduced by $40 %$ and $70 %$ , respectively, versus the baseline implementation of RRT*. Similarly, the RRT time for $10^{4}$ and $10^{5}$ vertices is reduced by $70 %$ and $90 %$ , respectively, versus the baseline implementation of RRT.

Using certificates increases the rate at which RRT* converges to the globally optimal solution due to the fact that performing less collision checking decreases average iteration time. Figure 16(l) shows the cost of the best path as a function of wall time. In general, the modified RRT* finds paths of lower cost significantly faster than the baseline RRT*. Figure 16(r) shows the mean number of explicit checks required for points which are not in collision versus different graph sizes. As the number of samples added to the database grows, the probability of performing an explicit check decreases. For example, only 1000 nodes out of 100,000 (i.e. $1 %$ ) require an explicit check, on average, when the certificate method is used.

Fig. 16.

(Left) RRT* best-path cost versus time, with and without the proposed method. Points represent the mean over 30 trials. Using certificates yields significantly faster convergence. (Right) The experimentally observed chance of performing an explicit collision query versus graph size. Note that only 1% of nodes require an explicit check when graph size is 100,000.

Figure 17 shows the single iteration runtime as a function of graph sizes (for all four algorithm combinations) versus two different obstacle configurations. In particular, environments with 500 and 1000 randomly generated obstacles. The 500 obstacle environment yields shorter iteration times, in general. However, when certificates are used, the difference between iteration times in either environment decreases. This is due to the fact that the certificate covering of the free space becomes complete, in the limit, and provides additional evidence of the utility of using certificates.

Fig. 17.

The computation time of a single iteration versus algorithm (plot style), mean over 30 runs, in a configuration with 500 and 1000 obstacles (left and right, respectively). Methods using certificates have “ball” appended in the legend.

If planning is performed in a low-dimensional space with polytope obstacles, then an exact collision-distance computation can be performed using a Voronoi diagram obstacle index (doing so is no more expensive than collision checking, in general). However, generating a Voronoi diagram in higher-dimensional geometric spaces is prohibitively complicated. We now evaluate the utility of using certificates in conjunction with a version of RRT that has been designed for these more difficult cases. In particular, a kd-tree is used for point-proximity searches and an axis-aligned bounding box tree is used to compute set distance queries and to perform collision checking (note that a lower bound on the actual collision distance is returned). Obstacles are randomly generated simplices.

Figure 18(l) illustrates the runtime as a function of graph size for the latter implementation of RRT, both with and without certificates. Figure 18(r) shows the observed frequency of performing an explicit collision check for a unit workspace in two to five dimensions when certificates are used.

Fig. 18.

Mean wall-clock runtime (left) and experimental $p_{explicit}$ (right), over 20 runs, for RRT in a unit workspace $X = [0, 1] D$ for several $D$ . Methods using certificates based on an approximate distance bound are labeled as “approx ball” in the legend, while those using no certificates are labeled as “noball”.

Using RRT with certificates (i.e. certificates defined by a lower bound on collision distance, as described above) works better than standard RRT in two and three dimensions, roughly the same in four dimensions, and performs worse in five dimensions. In even higher dimensions there is a significant degradation in performance. Performance issues in higher-dimensional configuration spaces are further discussed in Section 9.2.

8.2. Workspace certificates

In order to demonstrate the utility of workspace certificates and their ability to accelerate sampling-based planning algorithms we performed experiments on the piano mover problem in $W \subset ℝ^{3}$ and $X \subset ℝ_{D} \times SO (3)$ , as illustrated in Figure 9. The moving object is an L-shaped volume, decomposed into two convex collision volumes. The planning algorithm is PRM* (Karaman and Frazzoli, 2011), collision checking is done with FCL (Pan et al., 2012), and proximity queries are done with brute force on a GPU. The cost of a path segment between two configurations $(x_{a}, R_{a})$ and $(x_{b}, R_{b})$ is taken to be $∥ x_{b} - x_{a} ∥ + c_{w} \log (R_{a}^{T} R_{b})$ , where $c_{w}$ is a user defined parameter that affects the cost trade-off between translational and rotational movement (we use $c_{w} = 1$ ). This cost function was chosen because it works well in practice. That said, the choice of cost function used by PRM* does not affect the results presented in this sub-section, as the latter are only concerned with certificate coverage and graph size versus time. We use the simple certificate volumes which are dilated collision volumes by the collision distance found during rejection sampling. The certificate checking code computes the $n_{vertices} \times m_{faces}$ vertex-face constraint Sturm polynomials (i.e. where $n_{vertices}$ is the number of vertices on the polytopal robot and $m_{faces}$ is the number of faces on the polytopal certificate) and evaluates them all simultaneously on a GPU, leading to very low certificate checking overhead. The results presented in this section are averaged over 20 trials with different random seeds for the sampling process.

Figure 19 shows the wall-clock total normalized runtime (runtime divided by number of points) with and without the use of a workspace certificate cache. We see that the use of the certificate cache reduces the total runtime between 20% and 40% depending on the number of points. This performance increase comes from reducing the time to perform continuous collision queries for path segments. Building the cache, however, requires an increase in the cost of static collision checking each of the samples during the sampling phase. That is, by performing a collision-distance query instead of a collision-only query. This increase is from about 0.05 ms to about 0.25 ms (5×). However, because the static collision queries are such a small part of the overall runtime, this overhead is amortized by the acceleration of the continuous checking resulting in an overall performance boost for the planning algorithm.

Fig. 19.

Using workspace certificates accelerates continuous collision checking by up to 40%, resulting in a 40% total runtime reduction.

The benefits of the certificate cache exhibit a trend of increasing reward. As the number of samples increases, the inter-sample configuration distance is reduced, and so the likelihood that the path between two samples lies entirely within the certificate increases. Figure 20 shows the cache hit-rate for different point set sizes and demonstrates this increase for more samples.

Fig. 20.

The certificate success rate grows with the size of the point set, as the inter-configuration distance is shrinking.

Because of the anytime property of asymptotically optimal sampling-based algorithms, such as PRM*, we can see the end-to-end benefit of using the certificate cache. Depending on the planning time budget, we observe a 5 s to 10 s improvement in the time to find a solution of a given cost.

The symmetrically dilated collision certificate, while efficient to compute, is a rather conservative choice. When one face of the collision volume is close to an obstacle at a particular configuration, then all of the faces are dilated by that short distance. When the obstacles are rather tightly packed, this conservatism can greatly reduce the utility of the certificate cache. Figure 21 illustrates that the runtime of the cache-enabled implementation is only marginally improved over the baseline implementation when obstacles are tightly packed in this piano mover example and when using conservatively dilated certificates. We can see in Figure 22 that the conservatism leads to a very low cache hit-rate in this case.

Fig. 21.

When obstacles are close together ( $< 1 \times$ the length of the moving object) the benefit of the collision cache is greatly reduced.

Fig. 22.

The likelihood of the path to neighboring configurations lying within a sample’s certificate volume remains small for even relatively large point set sizes.

In this case, the higher coverage afforded by certificates generated from successive pruning (Algorithm 5) provides a more appropriate collision cache. Figure 23 shows the runtime of the cache accelerated algorithm for the closely packed obstacle case when certificates are generated with this method. We recover the runtime improvement of the widely spaced experiment. The trade-off between the two certificate generating methods is evident in Figure 24. The increased cost of generating the certificates is offset by the higher cache hit-rate, reaching 30% after 10,000 samples.

Fig. 23.

Caching of certificates generated by successive pruning yields significantly improved runtime when obstacles are close together with respect to the size of the collision volume.

Fig. 24.

Certificates generated by successive pruning provide a cache with a higher cache rate when obstacles are close together.

8.3. Spaces with symmetries

We now perform a number of experiments evaluating the effectiveness of using Partial Certificate and Shared Projection versus the basic method (which we will refer to as Basic Certificate) for centralized multi-robot planning.

Experiments are performed with both RRT and RRT* and with teams containing $R = 1$ to $5$ robots. The workspace $W \subset ℝ^{2}$ used for all experiments appears in Figure 25. Robots $1$ , $2$ , $3$ , $4$ , and $5$ start at the clock-wise positions of $9 : 00$ , $6 : 00$ , $3 : 00$ , $12 : 00$ , and $7 : 30$ , respectively, and each robot has the goal of reaching the opposite side of the workspace. Robots are discs with radius $0.5$ and holonomic, and therefore the configuration space is $X = Π_{i = 1}^{R} X_{i}$ . When an experiment is run with a team size $R < 5$ then robots $i > R$ are removed from the workspace.

Fig. 25.

The randomly generated workspace that is used for all centralized multi-robot team experiments. Robot starting locations and goals appear as circles and crosses, respectively. Note that a particular robot’s starting location and goal have the same color. Obstacles appear black and have been randomly generated.

Figures 26 and 27 show the average proportion of points that require a collision check (over 20 trials) for different team sizes (1 to 5 robots) versus iteration number ( $1$ to $10^{5}$ ). Note that fractional values are possible for Partial Certificate and Shared Projection when only some of the robots in a configuration require a check. Figure 26 shows results with RRT, while Figure 27 shows results with RRT*.

Fig. 26.

Proportion of nodes requiring a collision check versus team size (mean value over 20 trials), lower values are better. The RRT algorithm is used. It is important to note the difference versus the left-most sub-figure.

Fig. 27.

Proportion of nodes requiring a collision check versus team size (mean value over 20 trials), lower values are better. The RRT* algorithm is used. It is important to note the difference versus the left-most sub-figure.

Both Partial Certificate and Shared Projection require fewer collision checks at a particular iteration than the basic method.

9. Discussion

9.1 Removing the collision-checking bottleneck

Both analysis and experiments show that using collision certificates significantly reduces the proportion of time spent on collision checking. In particular, the expected time spent on collision checking per iteration approaches zero, in the limit, as the number of iterations approaches infinity (proven in Theorem 1 and observed experimentally). Given that collision checking is widely believed to be the main computational bottleneck in sampling-based motion planning, this result has the potential to improve the performance of nearly all sampling-based motion planning algorithms. In general, those satisfying Assumptions (A1) and (A2) in environments that satisfy Assumption (A3).

9.2. Limitations in higher-dimensional configuration spaces

In our experiments evaluating configuration-space-based certificates (Section 8.1) we observe decreasing performance versus configuration space dimensionality $D$ . We believe that decreasing performance in higher dimensions is a direct result of the curse of dimensionality. In particular, the fact that $lim_{D \to \infty} (L (B_{ℓ}) / L (X_{ℓ'})) = 0$ , where $L (B_{ℓ})$ is the hypervolume contained in a ball of radius $ℓ$ , and $L (X_{ℓ'})$ is the hypervolume contained in a state space with diameter $ℓ' > ℓ$ . Consequently, a certificate of a particular radius $ℓ$ will cover proportionally less of the state space as dimensionality $D$ increases, and thus have less chance of preventing future collision checks (although it is still expected to prevent an increasing number of collision checks versus iteration and runtime).

This illustrates one reason why workspace certificates (see Sections 6 and 8.2) are especially desirable for problems with high-dimensional configuration spaces. That is, regardless of the dimensionality of the configuration space, the workspace is limited to three dimensions for “real-world” robotics problems.

9.3. Certificates in configuration space versus workspace

Certificates can be used in either the configuration space or the workspace. The version that should be used in any particular case will likely be determined by the relative ease of calculating the certificate in either space, as well as practical concerns such as the obstacle representation that is chosen (or already implemented in a preexisting motion planning code-base). Configuration space certificates require that a bound on the minimum distance between a point and the obstacle set can be calculated inexpensively—an assumption that may not hold in all configuration spaces or in all practical implementations. Workspace certificates require many (constant times) more operations per certificate because the robot is represented as a volume instead of a point, but are more generally applicable in practice because obstacle volumes tend to be easily representable in the workspace and also have dimensionality fixed at three or less for real-world problems. We advise that the configuration space method be used when possible and the workspace method when practical (e.g. for problems with high-dimensional configuration spaces).

9.4. Considerations for configuration space certificates

The proposed approach is very general but there are some important implementation details to be aware of when using it. First, we require that points explicitly determined to be in-collision are kept in order to characterize the obstacle set (i.e. in addition to those that are collision-free). While the ratio of expected explicit collision checks versus the total number of samples goes to zero as the total number of samples approaches infinity, the number of points required to truly characterize the obstacle set can be quite large, especially in high dimensions. That said, our method is effective at marginalizing the extra collision checks in the asymptotically optimal variants of sampling-based motion planning algorithms. As an example, the expected runtime ratio between RRT* and RRT will be a constant which does not depend on the obstacle configuration, even if no in-collision samples are kept.

Calculating a sufficient approximation of the collision distance of a particular point and obstacle often does not require more computation than the worst case of performing a collision query. While this is true for a single obstacle, it is important to note that collision checking is often done using a spatial index and the choice of index may affect how the efficiency of a collision-distance query compares to a simple collision query.

9.5. Considerations for workspace certificate

Experimental results demonstrate significant runtime improvement using the workspace certificate cache. This is largely due to the fact that single certificate checking yields straightforward parallel implementations on modern hardware. Even if the success rate is zero, the additional cost of checking the initial certificate is marginal at worst. If the continuous collision-checking algorithm is in fact a variant of conservative advancement, the overhead is merged into the continuous collision-checking procedure.

There is significant room for improvement in parameterizing the collision volumes over the methods that we have presented here. Symmetrically dilated volumes have the advantage of requiring only one additional scalar of storage per configuration, but can be overly conservative, especially in dense obstacle environments. A particular area of future work that could greatly benefit applications of this algorithm would be designing efficient static collision-checking procedures which emit whole volumes rather than simply the collision distance. If such procedures can be designed in a way which does not incur too large an overhead, the effectiveness of the collision cache can be greatly improved (see Section 6).

9.6. Benefits versus different types of algorithms

Certificate methods may benefit multi-query algorithms (e.g. $k$ -PRM) and asymptotically optimal algorithms (e.g. PRM*, RRT*) more than single-query feasible planning algorithms (e.g. RRT). The latter return after finding a single path and thus experience less ball coverage of the free space and proportionately more explicit checks; in sparse environments, single query algorithms may find a solution before the end of the start-up phase (i.e. when long-term benefits of reduced collision checks have not yet offset the extra per-sample overhead of using certificates). Multi-query and optimal planners require more collision checks because they attempt many more nearest-neighbor connections per node and thus present more opportunities for savings. The start-up phase is less of a concern for multi-query algorithms due to the fact that their use assumes graph construction, and thus collision checking, happens off-line.

Sampling-based planning algorithms often consider multiple connections from a single source configuration. The same initial certificate may be used for all paths from the source configuration. Since the the source configuration is statically collision checked during the sampling phase, this initial certificate can be computed prior to any continuous collision checking. Increased sampling resolution decreases inter-configuration distances. When the configuration set is large enough, all candidates for connection will be sufficiently close to the source configuration that the entire path segment can be certified by a certificate.

One important negative theoretical result is that collision certificates will not necessarily be useful with versions of PRM that essentially create an $r$ -disc-graph due to the fact that, in such algorithms, a non-zero proportion of new trajectories will pass through more than two certificates—and thus require standard collision checks.

9.7. Certificates in spaces with symmetries

Certificates provide additional benefits in spaces with symmetries, for example, a centralized multi-robot team, where each robot operates within the same environment. Storing team certificates as the Cartesian product of individual robot certificates allows separate certification per each robot. Thus, new points may only require a fractional new certificate to certify the lower-dimensional projections (i.e. robots) of a point that do not fall into a certificate. When all robots are identical, all certification can be performed in a single lower-dimensional projection of the space (e.g. the certificate of one robot can be used for all robots). Experiments show that both of these ideas provide a significant reduction in the number of “real” collision checks that must be performed—even versus the basic certificate method.

9.8. Certificates versus different spaces

At any particular iteration, the expected benefits of using certificates are proportional to the relative amount of space that is covered by certificates, where the latter is a function of both obstacle clutter and space dimensionality. Thus, the benefits of our method tend to increase versus iteration number. The marginal benefit of adding a new certificate is proportional to how much additional space it certifies. The marginal benefits of using certificates tend to increase relatively quickly versus iteration number in spaces that are relatively free of obstacles (where certificates tend to be centered relatively far from obstacles and have large radii as a result) and in lower-dimension spaces (where certificates of a particular radius tend to cover more space). In high-dimensional spaces with many obstacles it may take many iterations before the marginal benefits become substantial. However, they will also likely provide a much greater reduction in computation time once they do (due to the fact that standard collision-checking algorithms also suffer from decreased performance in high-dimensional spaces, see Section 5.2 for more details).

Our best advice to practitioners is that certificate use is likely to have similar effects in environments with similar characteristics (clutter, narrow passage size, etc.). Therefore, one can test how effective the method in particular types of environments before deciding to use it (or not) in practice.

9.9. Possible extensions to general trajectories

Some of the subroutines in the current paper leverage the fact that, in many path planning problems, the feasible trajectory between two points is a straight line. In general, this is not the case for robotic systems subject to, for example, differential constraints. Extension to more general systems is relatively straightforward, assuming that it is possible (and tractable) to test if a trajectory is bounded by a certificate and/or where the trajectory intersects the certificate (or even a bound on the latter).

10. Conclusions

We have proposed a novel certificate-based approach to collision checking in sampling-based algorithms. In summary, whenever a point must be collision checked using standard methods, we save a certificate that defines a subset of space around that point that is also collision-free. New points that are sampled from within a certificate can skip “normal” collision checking because they are guaranteed to be collision-free. Similarly, the subset of any trajectory that passes through a certificate is also collision-free. In practice, checking the certification status of new points and trajectories can be combined with nearest-neighbor queries that are already used as subroutines in sampling-based motion planning algorithms.

The certificate method allows us to demonstrate, both theoretically and experimentally, that collision checking is not necessarily a computational bottleneck for sampling-based motion planning algorithms. Rather, the complexity is driven by nearest-neighbor searches within the graph constructed by the algorithms. Although complexity analysis has always suggested that this should be the case, the high complexity of collision checking has prevented it from being realized in practice in most previous work.

Footnotes

A. Canny interpolation

This section continues using the additional notation introduced in Section 6. For sampling-based motion planning, a requisite component is a “local planning method” or alternatively a “steering function” that, given two configurations $x_{a}, x_{b} \in X$ , generates a trajectory $σ$ between them. $σ : [0, 1] \to X$ , where $σ (0) = x_{a}$ and $σ (1) = x_{b}$ . We use $s$ as the interpolation parameter along $σ$ , i.e. $s \in [0, 1]$ .

Recall that $F_{i, σ} (s)$ is the transform from the local (workspace) coordinate system of link $i$ to that of its (workspace) parent as parameterized by $s \in [0, 1]$ . Thus, given the chain $J = {1, \dots, i}$ of links through the armature from the global coordinate frame $0$ to frame $i$ , it is possible to construct a transformation from link $i$ to the global coordinate frame as $Π_{j \in J} F_{j, σ} (s)$ . We now derive particular forms of $F_{i, σ} (s)$ that can be used to apply Canny’s interpolation method to workspace certificates used in conjunction with sampling-based motion planning.

For efficient sampling-based planning, the generated plan should be computed efficiently, admit efficient collision queries, and satisfy a notion of optimality: for a distance metric $d : X \times X \to ℝ$

lim_{d (x_{0}, x_{1}) \to 0} J (σ) J^{*} = 1

where $J^{*} = min_{σ \in Σ} J (σ)$ and $Σ$ denotes the set of all possible paths between configurations $x_{0}$ and $x_{1}$ . A common method of local planning is to use linear interpolation between configurations. For a particular configuration variable (say $θ_{i}$ for a one-degree-of-freedom joint) we choose $θ_{i} (s) = θ_{i}^{0} + s (θ_{i}^{1} - θ_{i}^{0})$ . This satisfies the requirements of being computed efficiently and satisfies a notion of optimality for many distance metrics, but is difficult to compute points of contact. The method of interpolating orientations developed by Canny (1986) using the stereographic projection of quaternions is asymptotically equivalent to linear interpolation as the magnitude of the angle goes to zero, but admits an efficient method of collision checking. We derive equations for representing Canny’s method of interpolation using rotation matrices and homogeneous transforms in order to derive algebraic conditions of collision for linked frames.

Consider a vector $x$ represented in the frame $F_{i}$ of link $i$ . At the initial configuration that reference frame is translated and rotated with respect to its parent by $T_{0}$ and $R_{0}$ , and at the target configuration that reference frame is translated and rotated with respect to its parent by $T_{1}$ and $R_{1}$ .

The path $σ$ between the two configurations is generated on a link by link basis as follows. The translation $T_{i} (s)$ is found by a linear interpolation

(2)

T (s) = T_{0} + s (T_{1} - T_{0}), 0 \leq s \leq 1

We generate intermediate rotations by linearly interpolating the tangent of the angle. We first find the direct relative rotation $Δ R_{0, 1}$ from $R_{0}$ to $R_{1}$

Δ R_{0, 1} = R_{1} R_{0}^{T}

Then we determine the axis-angle representation of that rotation by using the inverse (log) map from $SO (3)$ to $so (3)$

v = \frac{1}{Δ θ} ω

where $ω = \log (Δ R_{0, 1})$ and $Δ θ = ∥ ω ∥$ . Note that the magnitude of the direct rotation is at most $π$ . If the magnitude of the rotation is exactly $Δ θ = π$ , then the log map is undefined. There are two axis-angle rotations which satisfy Rodrigues’ formula (i.e. the reverse transform from $so (3)$ to $SO (3)$ ). For the purpose of local planning, we may simply consider both rotations.

We will generate a matrix polynomial in $s$ which yields a rotation about $v$ by an angle $θ (s)$ such that $\tan θ (s) = s \tan (Δ θ)$ . We do this by first defining an intermediate frame $F_{i}'$ in which the $x$ axis is coincident with $v$ . The translation for $F_{i'}$ is zero, and its rotation is given by

v' = \frac{e_{0} \times v}{∥ e_{0} \times v ∥}

where $θ' = \overset{- 1}{\sin} ∥ e_{0} \times v ∥$ , $R' = \exp (θ' v')$ , and $e_{0}$ is the unit vector along the $x$ axis. Within this intermediate frame, rotation about the vector $ω$ is expressed as a rotation about the $x$ axis. At an intermediate angle $θ$ such that $0 \leq θ \leq Δ θ$ , we may compute the orientation along minimum rotation path by the following equation

R (θ) = R_{x} (θ) R' R_{0}

where $R_{x}$ is the basic rotation matrix about the $x$ axis of magnitude $θ$

R_{x} (θ) = [\begin{matrix} 1 & 0 & 0 \\ 0 & \cos θ & - \sin θ \\ 0 & \sin θ & \cos θ \end{matrix}]

Because a rotation by $θ$ is equivalent to two rotations by $θ 2$ we have

\begin{matrix} (1 + \tan {(θ 2)}^{2}) R_{x} (θ) \\ = [\begin{matrix} 1 + \tan {(θ 2)}^{2} & 0 & 0 \\ 0 & - \tan {(θ 2)}^{2} & - 2 \tan (θ 2) \\ 0 & 2 \tan (θ 2) & - \tan {(θ 2)}^{2} \end{matrix}] \end{matrix}

We may ensure that any linear equation involving $R_{x} (s)$ is a polynomial in $s$ by picking $θ (s)$ such that $\tan θ 2$ is linear in $s$ . Thus, we define

(3)

γ = \tan Δ θ 2

(4)

θ (s) = 2 * \overset{- 1}{\tan} (s γ)

Then we have $\tan (θ (s) 2) = \tan (\overset{- 1}{\tan} (s γ)) = s γ .$ Note that this choice means that orientations are interpolated linearly along a central projection of the unit sphere in quaternion space (see Figure 28).

Because $R'$ and $R_{0}$ are defined by the pair of configurations in the interpolation and constant on $σ$ , we define $Δ R_{i, σ} = R' R_{0}$ , and we use $γ_{i, σ}$ to denote specifically the value of $γ$ for link $i$ on $σ$ so that $R_{x} (s)$ and $R (s)$ are

(5)

(1 + γ_{i, σ}^{2} s^{2}) R_{x} (θ) = [\begin{matrix} 1 + γ_{i, σ}^{2} s^{2} & 0 & 0 \\ 0 & - γ_{i, σ}^{2} s^{2} & - 2 γ_{i, σ} s \\ 0 & 2 γ_{i, σ} s & - γ_{i, σ}^{2} s^{2} \end{matrix}]

(6)

(1 + γ_{i, σ}^{2} s^{2}) R_{i, σ} (s) = (1 + γ_{i, σ}^{2} s^{2}) R_{x} (s) Δ R_{i, σ}

The homogeneous transformation for $F_{i, σ}$ along $σ$ is then given by

F_{i, σ} (s) = [\begin{matrix} R_{i, σ} (s) & T_{i, σ} (s) \\ 0 & 1 \end{matrix}]

\begin{matrix} (1 + γ_{i, σ}^{2} s^{2}) F_{i} (s) = [\begin{matrix} (1 + γ_{i, σ}^{2} s^{2}) R_{i} (s) & (1 + γ_{i, σ}^{2} s^{2}) T_{i} (s) \\ 0 & 1 \end{matrix}] \\ = [\begin{matrix} [\begin{matrix} 1 + γ_{i, σ}^{2} s^{2} & 0 & 0 \\ 0 & - γ_{i, σ}^{2} s^{2} & - 2 γ_{i, σ} s \\ 0 & 2 γ_{i, σ} s & - γ_{i, σ}^{2} s^{2} \end{matrix}] Δ R_{i, σ} & (1 + γ_{i, σ}^{2} s^{2}) (T_{i, 0} + s Δ T_{i, σ}) \\ 0 & (1 + γ_{i, σ}^{2} s^{2}) \end{matrix}] \end{matrix}

Note that each element of this matrix is a polynomial of degree at most three. Thus, after appropriate pre-multiplication, the matrix polynomial describing the homogeneous transformation from the global frame to $F_{i}$ is a polynomial of degree $3 k$ where $k$ is the number of links between the global frame and $F_{i}$ . This yields interference predicates which are univariate polynomials for the orientation of frame $F_{i}$ along the path $σ$ .

B. Additional derivations for constrained motions in articulated robot

In this section we present a few additional details related to the application of Canny’s method to articulated robots. Generally speaking, an articulated robot is composed of links which are joined in such a way as to constrain the relative motion of their reference frames. In particular, the joint between two links is most often actuated by either a linear or rotary actuator. A linear actuator allows only translational motion in a fixed direction, and a rotary actuator allows only rotation about a fixed axis. These types of constrained motions simplify the polynomial equation for $F_{i}$ . If $F_{i}$ is linked to $F_{j}$ by a linear actuator with direction $v$ in $F_{j}$ , then

(1 + γ_{i, σ}^{2} s^{2}) F_{i, σ} = [\begin{matrix} R & (1 + γ_{i, σ}^{2} s^{2}) (x_{0} + s Δ x) v \\ 0 & (1 + γ_{i, σ}^{2} s^{2}) \end{matrix}]

where $x_{0}$ is the translation at the initial configuration, and $x_{1} = x_{0} + Δ x$ is the translation at the final configuration.

Likewise if $F_{i}$ is linked to $F_{j}$ by a rotary actuator which rotates about $v$ in $F_{j}$ then

\begin{matrix} (1 + γ_{i, σ}^{2} s^{2}) F_{i, σ} \\ = [\begin{matrix} [\begin{matrix} 1 + γ_{i, σ}^{2} s^{2} & 0 & 0 \\ 0 & - γ_{i, σ}^{2} s^{2} & - 2 γ_{i, σ} s \\ 0 & 2 γ_{i, σ} s & - γ_{i, σ}^{2} s^{2} \end{matrix}] Δ R_{i} & 0 \\ 0 & (1 + γ_{i, σ}^{2} s^{2}) \end{matrix}] \end{matrix}

where $Δ R_{i}$ is fixed and does not change for different paths. We note that for rotary actuators with a range greater than $π$ , our choice of $Δ θ$ as the direct rotation with magnitude always $< π$ may be in a direction which is not feasible for the actuator. However for two configurations we may simply test whether or not is the case, and if $Δ θ$ as described above is in a direction which is infeasible for the actuator we simply choose $Δ θ' = Δ θ - 2 π$ , though we must then split the path into two intermediate configurations because $Δ θ'$ has magnitude greater than $π$ .

Funding

This work was partially supported by the Office of Naval Research (MURI grant number #N00014-09-1-1051), the Army Research Office (MURI grant number #W911NF-11-1-0046), and the Air Force Office of Scientific Research (grant number #FA-8650-07-2-3744) and the National Science Foundation (grant number IIP-1161029). The final version of this paper was completed while the second author was supported by the Control Science Center of Excellence at the Air Force Research Laboratory (AFRL) and “in residence” at AFRL.

Notes

References

Arslan

Tsiotras

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

Aurenhammer

(1991) Voronoi diagrams—a survey of a fundamental data structure. ACM Computing Surveys 23(3): 345–405.

Basch

Comba

Guibas

. (1999) Kinetic data structures: Animating proofs through time. In: Proceedings of the fifteenth annual symposium on computational geometry, Miami Beach, FL, 13–16 June 1999, pp.427–428. New York: ACM.

Bentley

(1975) Multidimensional binary search trees used for associative searching. Communications of the ACM 18: 509–517.

Bialkowski

(2013) Optimizations for sampling-based motion planning algorithms. PhD Thesis, Massachusetts Institute of Technology, USA.

Bialkowski

Otte

Frazzoli

(2013a) Fast collision checking: from single robots to multi-robot teams. (Technical Report). http://arxiv.org/abs/1305.2299.

Bialkowski

Otte

Frazzoli

(2013b) Fast collision checking: from single robots to multi-robot teams. In: IEEE international conference on robotics and automation: crossing the reality gap – from single to multi- to many robot systems, Karlsruhe, Germany, 10 May 2013.

Bialkowski

Otte

Frazzoli

(2013c) Free-configuration biased sampling for motion planning. In: International conference on intelligent robots and systems, Tokyo, Japan, 3–7 November 2013, pp.1272–1279.

Bialkowski

Otte

Karaman

. (2013d) Efficient collision checking in sampling-based motion planning. In: Algorithmic Foundations of Robotics X, Springer Tracts in Advanced Robotics, vol. 69. Berlin/Heidelberg: Springer, pp.365–380.

10.

Bohlin

Kavraki

(2000) Path planning using lazy PRM. In: Proceedings of the IEEE international conference on robotics and automation, vol. 1, San Francisco, CA, 24–28 April 2000, pp.521–528. IEEE.

11.

Boor

Overmars

van der Stappen

(1999) The gaussian sampling strategy for probabilistic roadmap planners. In: IEEE International Conference on Robotics and Automation, Detroit, MI, 10–15 May 1999, pp.1018–1023.

12.

Brock

Kavraki

(2001) Decomposition-based motion planning: a framework for real-time motion planning in high-dimensional configuration spaces. In: Proceedings of the IEEE international conference on robotics and automation, vol. 2, Seoul, South Korea, 21–26 May 2001, pp.1469–1474.

13.

Canny

(1986) Collision detection for moving polyhedra. IEEE Transactions on Pattern Analysis and Machine Intelligence 8(2): 200–209.

14.

Deits

Tedrake

(2015) Computing large convex regions of obstacle-free space through semidefinite programming. In: Akin

Amato

Isler

. (eds) Algorithmic Foundations of Robotics XI. Cham: Springer, pp.109–124.

15.

Gilbert

Johnson

Keerthi

(1988) A fast procedure for computing the distance between complex objects in three-dimensional space. IEEE Journal of Robotics and Automation 4(2): 193–203.

16.

Govindaraju

Lin

Manocha

(2005) Quick-cullide: fast inter- and intra-object collision culling using graphics hardware. In: Proceedings of the IEEE conference on virtual reality, Bonn, Germany, 12–16 March 2005, pp.59–66.

17.

Govindaraju

Lin

Manocha

(2006) Fast and reliable collision culling using graphics hardware. IEEE Transactions on Visualization and Computer Graphics 12(2): 143–154.

18.

Guttman

(1984) R-trees: a dynamic index structure for spatial searching. SIGMOD Record 14(2): 47–57.

19.

Held

(2001) Vroni: an engineering approach to the reliable and efficient computation of voronoi diagrams of points and line segments. Computational Geometry 18(2): 95–123.

20.

Hook

McAree

(1990) Using sturm sequences to bracket real roots of polynomial equations. In: Glassner

(ed) Graphics Gems. San Diego, CA: Academic Press Professional, Inc., pp.416–422.

21.

Hou

Sun

Zhou

. (2011) Memory-scalable gpu spatial hierarchy construction. IEEE Transactions on Visualization and Computer Graphics 17(4): 466–474.

22.

Hsu

Kavraki

Latombe

. (1998) On finding narrow passages with probabilistic roadmap planners. In: Agarwal

Kavraki

Mason

(eds) Robotics: The Algorithmic Perspective (WAFR ’98). Wellesley, MA: A.K. Peters/CRC Press, pp.141–154.

23.

Jiménez

Thomas

Torras

(2001) 3d collision detection: a survey. Computers & Graphics 25(2): 269–285.

24.

Karaman

Frazzoli

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

25.

Karavelas

Yvinec

(2003) The voronoi diagram of planar convex objects. In: Di Battista

Zwick

(eds) Algorithms - ESA 2003, Lecture Notes in Computer Science, volume 2832. Berlin/Heidelberg: Springer, pp.337–348.

26.

Kavan

Žára

(2005) Fast collision detection for skeletally deformable models. Computer Graphics Forum 24(3): 363–372.

27.

Kavraki

Švestka

Latombe

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

28.

Kim

Rossignac

(2003) Collision prediction for polyhedra under screw motions. In: Proceedings of the eighth ACM symposium on solid modeling and applications, Seattle, Washington, 16–20 June 2003. New York: ACM, pp.4–10.

29.

Kirkpatrick

Snoeyink

Speckmann

(2000) Kinetic collision detection for simple polygons. In: Proceedings of the sixteenth annual symposium on Computational geometry, Hong Kong, 12–14 June 2000. New York: ACM, pp.322–330.

30.

Lauterbach

Garland

Sengupta

. (2009) Fast BVH construction on GPUS. Computer Graphics Forum 28(2): 375–384.

31.

Lauterbach

Manocha

(2010) gProximity: hierarchical GPU-based operations for collision and distance queries. Computer Graphics Forum 29(2): 419–428.

32.

LaValle

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

33.

LaValle

Kuffner

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

34.

Lin

(1993) Efficient collision detection for animation and robotics. PhD Thesis, University of California at Berkeley, USA.

35.

McAllister

Kirkpatrick

Snoeyink

(1996) A compact piecewise-linear voronoi diagram for convex sites in the plane. Discrete & Computational Geometry 15: 73–105.

36.

Mirtich

(1998) V-clip: fast and robust polyhedral collision detection. ACM Transactions on Graphics 17(3): 177–208.

37.

Otte

Frazzoli

(2015) RRTX: asymptotically optimal single-query sampling-based motion planning with quick replanning. International Journal of Robotics Research. Epub ahead of print 20September. DOI: 10.1177/0278364915594679.

38.

Pan

Chitta

Manocha

(2012) FCL: a general purpose library for collision and proximity queries. In: IEEE international conference on robotics and automation, St. Paul, MN, 12–14 May 2012, pp.3859–3866.

39.

Pan

Manocha

(2011) Gpu-based parallel collision detection for real-time motion planning. In: Hsu

Isler

Latombe

. (eds) Algorithmic Foundations of Robotics IX, Springer Tracts in Advanced Robotics, volume 68. Berlin/Heidelberg:Springer, pp.211–228.

40.

Pan

Manocha

(2012) Gpu-based parallel collision detection for fast motion planning. The International Journal of Robotics Research 31(2): 187–200.

41.

Ponamgi

Manocha

Lin

(1997) Incremental algorithms for collision detection between polygonal models. IEEE Transactions on Visualization and Computer Graphics 3(1): 51–64.

42.

Redon

Kheddar

Coquillart

(2000) An algebraic solution to the problem of collision detection for rigid polyhedral objects. In: Proceedings of the IEEE international conference on robotics and automation, vol. 4, San Francisco, CA, 24–28April2000, pp.3733–3738.

43.

Redon

Kheddar

Coquillart

(2002) Fast continuous collision detection between rigid bodies. Computer Graphics Forum 21(3): 279–287.

44.

Redon

Kim

Lin

. (2004) Fast continuous collision detection for articulated models. In: Proceedings of the ninth ACM symposium on solid modeling and applications, Aire-la-Ville, Switzerland. Switzerland: Eurographics Association, pp.145–156.

45.

Redon

Lin

Manocha

. (2005) Fast continuous collision detection for articulated models. Journal of Computing and Information Science in Engineering 5(2): 126–137.

46.

Samet

(2006) Foundations of Multidimensional and Metric Data Structures. Burlington, MA: Morgan Kaufmann.

47.

Schwarzer

Saha

Latombe

(2004) Exact collision checking of robot paths. In: Boissonnat

Burdick

Goldberg

. (eds) Algorithmic Foundations of Robotics V, Springer Tracts in Advanced Robotics, volume 7. Berlin/Heidelberg:Springer, pp.25–42.

48.

Sharifzadeh

Shahabi

(2010) Vor-tree: R-trees with voronoi diagrams for efficient processing of spatial nearest neighbor queries. Proceedings of the VLDB Endowment 3(1-2): 1231–1242.

49.

Tang

Manocha

Tong

(2010) MCCD: multi-core collision detection between deformable models using front-based decomposition. Graphical Models 72(2): 7–23.

50.

Tokuta

(1991) Motion planning using binary space partitioning. In: Proceedings of the IEEE/RSJ international workshop on intelligent robots and systems, ‘intelligence for mechanical systems’, volume 1, International House Osaka, Osaka, Japan, 3–5 November 1991, pp.86–90. New York: Institute of Electrical and Electronics Engineers.

51.

Yang

LaValle

(2000) A framework for planning feedback motion strategies based on a random neighborhood graph. In: Proceedings of the international conference on robotics and automation, San Francisco, CA, 24–28 April 2000, pp.544–549.

52.

Yang

LaValle

(2002) An improved random neighborhood graph approach. In: Proceedings of the international conference on robotics and automation, Washington, DC, 11–15 May 2002, pp.254–259.

53.

Yang

LaValle

(2004) The sampling-based neighborhood graph: A framework for planning and executing feedback motion strategies. IEEE Transactions on Robotics and Automation 20(3): 419–432.

54.

Yang

Brock

(2013) Adapting the sampling distribution in PRM planners based on an approximated medial axis. In: Proceedings of the international conference on robotics and automation, Karlsruhe, Germany, 10 May 2013, pp.4405–4410.

55.

Zhang

Redon

Lee

. (2007) Continuous collision detection for articulated models using Taylor models and temporal culling. ACM Transactions on Graphics 26(3).