Collision Detection

Once it has been decided where the samples will be placed, the next problem is to determine whether the configuration is in collision. Thus, collision detection is a critical component of sampling-based planning. Even though it is often treated as a black box, it is important to study its inner workings to understand the informa- tion it provides and its associated computational cost. In most motion planning applications, the majority of computation time is spent on collision checking.

A variety of collision detection algorithms exist, ranging from theoretical algo- rithms that have excellent computational complexity to heuristic, practical algo- rithms whose performance is tailored to a particular application. The techniques from Section 4.3 can be used to develop a collision detection algorithm by defining a logical predicate using the geometric model of obs. For the case of a 2D world with a convex robot and obstacle, this leads to an linear-time collision detection algorithm. In general, however, it can be determined whether a configuration is

C

in collision more efficiently by avoiding the full construction of Cobs.

• • 1. Basic Concepts

As in Section 3.1.1, collision detection may be viewed as a logical predicate. In the current setting it appears as φ : C → {true, false}, in which the domain is C instead of W. If q ∈ Cobs, then φ(q) = true; otherwise, φ(q) = false.

Distance between two sets For the Boolean-valued function φ, there is no information about how far the robot is from hitting the obstacles. Such informa- tion is very important in planning algorithms. A distance function provides this

information and is defined as d : [0, ), in which the real value in the range of f indicates the distance in the world, , between the closest pair of points over all pairs from (q) and . In general, for two closed, bounded subsets, E and F, of Rn, the distance is defined as

A O

W

C → ∞

ρ(E, F) = min J min JIe − f I＼＼, (5.28)

e∈E

f ∈F

in which is the Euclidean norm. Clearly, if E F = , then ρ(E, F) = 0. The methods described in this section may be used to either compute distance or only determine whether q obs. In the latter case, the computation is often much faster because less information is required.

∈ C

I · I ∩ / ∅

Two-phase collision detection Suppose that the robot is a collection of m attached links, 1, 2, . . ., m, and that has k connected components. For this complicated situation, collision detection can be viewed as a two-phase process.

A A A O

• • • 1. Broad Phase: In the broad phase, the task is to avoid performing expensive computations for bodies that are far away from each other. Simple bounding boxes can be placed around each of the bodies, and simple tests can be per- formed to avoid costly collision checking unless the boxes overlap. Hashing schemes can be employed in some cases to greatly reduce the number of pairs of boxes that have to be tested for overlap [703]. For a robot that consists of multiple bodies, the pairs of bodies that should be considered for collision must be specified in advance, as described in Section 4.3.1.
      2. Narrow Phase: In the narrow phase, individual pairs of bodies are each checked carefully for collision. Approaches to this phase are described in Sections 5.3.2 and 5.3.3.
    • Hierarchical Methods

In this section, suppose that two complicated, nonconvex bodies, E and F, are to be checked for collision. Each body could be part of either the robot or the

obstacle region. They are subsets of R2 or R3, defined using any kind of geometric primitives, such as triangles in R3. Hierarchical methods generally decompose each body into a tree. Each vertex in the tree represents a bounding region that

contains some subset of the body. The bounding region of the root vertex contains the whole body.

There are generally two opposing criteria that guide the selection of the type of bounding region:

• • • 1. The region should fit the intended body points as tightly as possible.
      2. The intersection test for two regions should be as efficient as possible.

(a) (b) (c) (d)

Figure 5.9: Four different kinds of bounding regions: (a) sphere, (b) axis-aligned bounding box (AABB), (c) oriented bounding box (OBB), and (d) convex hull. Each usually provides a tighter approximation than the previous one but is more expensive to test for overlapping pairs.

Figure 5.10: The large circle shows the bounding region for a vertex that covers an L-shaped body. After performing a split along the dashed line, two smaller circles are used to cover the two halves of the body. Each circle corresponds to a child vertex.

Several popular choices are shown in Figure 5.9 for an L-shaped body.

The tree is constructed for a body, E (or alternatively, F) recursively as fol- lows. For each vertex, consider the set X of all points in E that are contained in the bounding region. Two child vertices are constructed by defining two smaller bounding regions whose union covers X. The split is made so that the portion cov- ered by each child is of similar size. If the geometric model consists of primitives such as triangles, then a split could be made to separate the triangles into two sets of roughly the same number of triangles. A bounding region is then computed for each of the children. Figure 5.10 shows an example of a split for the case of an L-shaped body. Children are generated recursively by making splits until very simple sets are obtained. For example, in the case of triangles in space, a split is made unless the vertex represents a single triangle. In this case, it is easy to test for the intersection of two triangles.

Consider the problem of determining whether bodies E and F are in collision. Suppose that the trees Te and Tf have been constructed for E and F, respectively. If the bounding regions of the root vertices of Te and Tf do not intersect, then it is known that Te and Tf are not in collision without performing any additional computation. If the bounding regions do intersect, then the bounding regions of

the children of Te are compared to the bounding region of Tf . If either of these intersect, then the bounding region of Tf is replaced with the bounding regions of its children, and the process continues recursively. As long as the bounding regions overlap, lower levels of the trees are traversed, until eventually the leaves are reached. If triangle primitives are used for the geometric models, then at the leaves the algorithm tests the individual triangles for collision, instead of bounding regions. Note that as the trees are traversed, if a bounding region from the vertex v1 of Te does not intersect the bounding region from a vertex, v2, of Tf , then no children of v1 have to be compared to children of v1. Usually, this dramatically reduces the number of comparisons, relative in a naive approach that tests all pairs of triangles for intersection.

It is possible to extend the hierarchical collision detection scheme to also com- pute distance. The closest pair of points found so far serves as an upper bound that prunes aways some future pairs from consideration. If a pair of bounding regions has a distance greater than the smallest distance computed so far, then their children do not have to be considered [638]. In this case, an additional re- quirement usually must be imposed. Every bounding region must be a proper subset of its parent bounding region [807]. If distance information is not needed, then this requirement can be dropped.

• • 1. Incremental Methods

This section focuses on a particular approach called incremental distance com- putation, which assumes that between successive calls to the collision detection algorithm, the bodies move only a small amount. Under this assumption the algorithm achieves “almost constant time” performance for the case of convex polyhedral bodies [636, 702]. Nonconvex bodies can be decomposed into convex components.

These collision detection algorithms seem to offer wonderful performance, but this comes at a price. The models must be coherent, which means that all of the primitives must fit together nicely. For example, if a 2D model uses line segments, all of the line segments must fit together perfectly to form polygons. There can be no isolated segments or chains of segments. In three dimensions, polyhedral models are required to have all faces come together perfectly to form the boundaries of 3D shapes. The model cannot be an arbitrary collection of 3D triangles.

The method will be explained for the case of 2D convex polygons, which are interpreted as convex subsets of R2. Voronoi regions for a convex polygon will be defined in terms of features. The feature set is the set of all vertices and edges of a convex polygon. Thus, a polygon with n edges has 2n features. Any point outside of the polygon has a closest feature in terms of Euclidean distance. For a given

feature, F, the set of all points in R2 from which F is the closest feature is called the Voronoi region of F and is denoted Vor(F). Figure 5.11 shows all ten Voronoi regions for a pentagon. Each feature is considered as a point set in the discussion

below.

Figure 5.11: The Voronoi regions alternate between being edge-based and vertex- based. The Voronoi regions of vertices are labeled with a “V” and the Voronoi regions of edges are labeled with an “E.”

For any two convex polygons that do not intersect, the closest point is deter- mined by a pair of points, one on each polygon (the points are unique, except in the case of parallel edges). Consider the feature for each point in the closest pair. There are only three possible combinations:

• Vertex-Vertex Each point of the closest pair is a vertex of a polygon.

Edge-Vertex One point of the closest pair lies on an edge, and the other lies on a vertex.

•

Edge-Edge Each point of the closest pair lies on an edge. In this case, the edges must be parallel.

•

Let P1 and P2 be two convex polygons, and let F1 and F2 represent any feature pair, one from each polygon. Let (x1, y1) ∈ F1 and (x2, y2) ∈ F2 denote the closest

pair of points, among all pairs of points in F1 and F2, respectively. The following condition implies that the distance between (x1, y1) and (x2, y2) is the distance between P1 and P2:

(x1, y1) ∈ Vor(F2) and (x2, y2) ∈ Vor(F1). (5.29)

If (5.29) is satisfied for a given feature pair, then the distance between P1 and P2 equals the distance between F1 and F2. This implies that the distance between P1 and P2 can be determined in constant time. The assumption that P1 moves only a small amount relative to P2 is made to increase the likelihood that the closest feature pair remains the same. This is why the phrase “almost constant time” is used to describe the performance of the algorithm. Of course, it is possible that the closest feature pair will change. In this case, neighboring features are tested

using the condition above until the new closest pair of features is found. In this worst case, this search could be costly, but this violates the assumption that the bodies do not move far between successive collision detection calls.

The 2D ideas extend to 3D convex polyhedra [247, 636, 702]. The primary difference is that three kinds of features are considered: faces, edges, and vertices. The cases become more complicated, but the idea is the same. Once again, the condition regarding mutual Voronoi regions holds, and the resulting incremental collision detection algorithm has “almost constant time” performance.

• • 1. Checking a Path Segment

Collision detection algorithms determine whether a configuration lies in Cfree, but motion planning algorithms require that an entire path maps into free. The interface between the planner and collision detection usually involves validation of a path segment (i.e., a path, but often a short one). This cannot be checked point-by-point because it would require an uncountably infinite number of calls to the collision detection algorithm.

C

Suppose that a path, τ : [0, 1] → C, needs to be checked to determine whether

τ ([0, 1]) free. A common approach is to sample the interval [0, 1] and call the collision checker only on the samples. What resolution of sampling is required? How can one ever guarantee that the places where the path is not sampled are collision-free? There are both practical and theoretical answers to these questions.

⊂ C

In practice, a fixed ∆q > 0 is often chosen as the C-space step size. Points t1, t2 ∈

[0, 1] are then chosen close enough together to ensure that ρ(τ (t1), τ (t2)) ∆q, in which ρ is the metric on . The value of ∆q is often determined experimentally. If

C

≤

∆q is too small, then considerable time is wasted on collision checking. If ∆q is too large, then there is a chance that the robot could jump through a thin obstacle.

Setting ∆q empirically might not seem satisfying. Fortunately, there are sound algorithmic ways to verify that a path is collision-free. In some applications the methods are still not used because they are trickier to implement and they often yield worse performance. Therefore, both methods are presented here, and you can decide which is appropriate, depending on the context and your personal tastes.

Ensuring that τ ([0, 1]) free requires the use of both distance information and bounds on the distance that points on can travel in R. Such bounds can be obtained by using the robot displacement metric from Example 5.6. Before ex-

A

⊂ C

pressing the general case, first we will explain the concept in terms of a rigid robot that translates and rotates in W = R2. Let xt, yt ∈ R2 and θ ∈ [0, 2π]/ ∼. Suppose that a collision detection algorithm indicates that A(q) is at least d units away from collision with obstacles in W. This information can be used to determine

a region in free that contains q. Suppose that the next candidate configuration to be checked along τ is q′. If no point on travels more than distance d when moving from q to q′ along τ , then q′ and all configurations between q and q′ must be collision-free. This assumes that on the path from q to q′, every visited config-

A

C

uration must lie between qi and qi′

for the ith coordinate and any i from 1 to n.

Figure 5.12: The furthest point on A from the origin travels the fastest when A

is rotated. At most it can be displaced by 2πr, if xt and yt are fixed.

If the robot can instead take any path between q and q′, then no such guarantee can be made).

When A undergoes a translation, all points move the same distance. For rotation, however, the distance traveled depends on how far the point on A is

from the rotation center, (0, 0). Let ar = (xr, yr) denote the point on that

A

has the largest magnitude, r = /x2 + y2. Figure 5.12 shows an example. A

r

r

transformed point a ∈ A may be denoted by a(xt, yt, θ). The following bound is obtained for any a ∈ A, if the robot is rotated from orientation θ to θ′:

Ia(xt, yt, θ) − a(xt, yt, θ′)I ≤ Iar(xt, yt, θ) − ar(xt, yt, θ′)I < r|θ − θ′|, (5.30)

assuming that a path in is followed that interpolates between θ and θ′ (using the shortest path in S1 between θ and θ′). Thus, if (q) is at least d away from the obstacles, then the orientation may be changed without causing collision as long as r θ θ′ < d. Note that this is a loose upper bound because ar travels along a circular arc and can be displaced by no more than 2πr.

A

C

| − |

Similarly, xt and yt may individually vary up to d, yielding xt xt′ < d and yt yt′ < d. If all three parameters vary simultaneously, then a region in free can be defined as

| − | C

| − |

{(x′t, yt′ , θ′) ∈ C | |xt − x′t| + |yt − yt′ | + r|θ − θ′| < d}. (5.31)

Such bounds can generally be used to set a step size, ∆q, for collision checking that guarantees the intermediate points lie in free. The particular value used may vary depending on d and the direction8 of the path.

C

For the case of SO(3), once again the displacement of the point on that has the largest magnitude can be bounded. It is best in this case to express the

A

bounds in terms of quaternion differences, Ih−h′I. Euler angles may also be used

8To formally talk about directions, it would be better to define a differentiable structure on

C. This will be deferred to Section 8.3, where it seems unavoidable.

to obtain a straightforward generalization of (5.31) that has six terms, three for translation and three for rotation. For each of the three rotation parts, a point with the largest magnitude in the plane perpendicular to the rotation axis must be chosen.

If there are multiple links, it becomes much more complicated to determine the step size. Each point a i is transformed by some nonlinear function based on the kinematic expressions from Sections 3.3 and 3.4. Let a : denote this transformation. In some cases, it might be possible to derive a Lipschitz condition of the form

C → W

∈ A

Ia(q) − a(q′)I < cIq − q′I, (5.32)

in which c (0, ) is a fixed constant, a is any point on i, and the expression holds for any q, q′ . The goal is to make the Lipschitz constant, c, as small as possible; this enables larger variations in q.

∈ C

∈ ∞ A

A better method is to individually bound the link displacement with respect to each parameter,

a(q1, . . . , qi−1, qi, qi+1, . . . , qn) a(q1, . . . , qi−1, qi′ , qi+1, . . . , qn) < ci qi qi′ ,

I − I | − |

(5.33)

to obtain the Lipschitz constants c1, . . ., cn. The bound on robot displacement becomes

n

Ia(q) − a(q′)I < ci|qi − qi′ |. (5.34)

-

i=1

The benefit of using individual parameter bounds can be seen by considering a long

chain. Consider a 50-link chain of line segments in R2, and each link has length 10. The C-space is T50, which can be parameterized as [0, 2π]50/ ∼. Suppose that the chain is in a straight-line configuration (θi = 0 for all 1 ≤ i ≤ 50), which means that the last point is at (500, 0) ∈ W. Changes in θ1, the orientation of the first link, dramatically move A50. However, changes in θ50 move A50 a smaller amount.

Therefore, it is advantageous to pick a different ∆qi for each 1 ≤ i ≤ 50. In this

example, a smaller value should be used for ∆θ1 in comparison to ∆θ50.

Unfortunately, there are more complications. Suppose the 50-link chain is in a configuration that folds all of the links on top of each other (θi = π for each

1 ≤ i ≤ n). In this case, A50 does not move as fast when θ1 is perturbed,

in comparison to the straight-line configuration. A larger step size for θ1 could be used for this configuration, relative to other parts of . The implication is that, although Lipschitz constants can be made to hold over all of , it might be preferable to determine a better bound in a local region around q . A linear method could be obtained by analyzing the Jacobian of the transformations, such as (3.53) and (3.57).

∈ C

C

C

Another important concern when checking a path is the order in which the samples are checked. For simplicity, suppose that ∆q is constant and that the path is a constant-speed parameterization. Should the collision checker step along from 0 up to 1? Experimental evidence indicates that it is best to use a recursive binary strategy [379]. This makes no difference if the path is collision-free, but

it often saves time if the path is in collision. This is a kind of sampling problem over [0, 1], which is addressed nicely by the van der Corput sequence, ν. The last column in Figure 5.2 indicates precisely where to check along the path in each step. Initially, τ (1) is checked. Following this, points from the van der Corput sequence are checked in order: τ (0), τ (1/2), τ (1/4), τ (3/4), τ (1/8), . . .. The process terminates if a collision is found or when the dispersion falls below ∆q. If ∆q is not constant, then it is possible to skip over some points of ν in regions where the allowable variation in q is larger.