Distance and Volume in C-Space

Virtually all sampling-based planning algorithms require a function that measures the distance between two points in . In most cases, this results in a metric space, which is introduced in Section 5.1.1. Useful examples for motion planning are given in Section 5.1.2. It will also be important in many of these algorithms to define the volume of a subset of . This requires a measure space, which is introduced in Section 5.1.3. Section 5.1.4 introduces invariant measures, which should be used whenever possible.

C

C

• • 1. Metric Spaces

It is straightforward to define Euclidean distance in Rn. To define a distance function over any , however, certain axioms will have to be satisfied so that it coincides with our expectations based on Euclidean distance.

C

The following definition and axioms are used to create a function that converts a topological space into a metric space.1 A metric space (X, ρ) is a topological

space X equipped with a function ρ : X × X → R such that for any a, b, c ∈ X:

• • • 1. Nonnegativity: ρ(a, b) ≥ 0.
      2. Reflexivity: ρ(a, b) = 0 if and only if a = b.
      3. Symmetry: ρ(a, b) = ρ(b, a).
      4. Triangle inequality: ρ(a, b) + ρ(b, c) ≥ ρ(a, c).

The function ρ defines distances between points in the metric space, and each of the four conditions on ρ agrees with our intuitions about distance. The final condition implies that ρ is optimal in the sense that the distance from a to c will always be less than or equal to the total distance obtained by traveling through an intermediate point b on the way from a to c.

Lp metrics The most important family of metrics over Rn is given for any p 1 as

≥

（ - ＼

n

ρ(x, x′) = |xi − x′i|p

i=1

1/p

. (5.1)

For each value of p, (5.1) is called an Lp metric (pronounced “el pee”). The three most common cases are

1. L2: The Euclidean metric, which is the familiar Euclidean distance in Rn.
2. L1: The Manhattan metric, which is often nicknamed this way because in R2 it corresponds to the length of a path that is obtained by moving along an axis-aligned grid. For example, the distance from (0, 0) to (2, 5) is 7 by

traveling “east two blocks” and then “north five blocks”.

1. L∞: The L∞ metric must actually be defined by taking the limit of (5.1) as

p tends to infinity. The result is

L∞(x, x′) = max {|xi − x′i|}, (5.2)

1≤i≤n

which seems correct because the larger the value of p, the more the largest term of the sum in (5.1) dominates.

1Some topological spaces are not metrizable, which means that no function exists that satisfies the axioms. Many metrization theorems give sufficient conditions for a topological space to be

metrizable [451], and virtually any space that has arisen in motion planning will be metrizable.

An Lp metric can be derived from a norm on a vector space. An Lp norm over Rn

is defined as

n

（ - ＼

IxIp = |xi|p

i=1

1/p

. (5.3)

The case of p = 2 is the familiar definition of the magnitude of a vector, which is called the Euclidean norm. For example, assume the vector space is Rn, and let

I · I be the standard Euclidean norm. The L2 metric is ρ(x, y) = Ix − yI. Any

Lp metric can be written in terms of a vector subtraction, which is notationally convenient.

Metric subspaces By verifying the axioms, it can be shown that any subspace Y X of a metric space (X, ρ) itself becomes a metric space by restricting the domain of ρ to Y Y . This conveniently provides metrics on any of the manifolds and varieties from Chapter 4 by simply using any Lp metric on Rm, the space in which the manifold or variety is embedded.

×

⊂

Cartesian products of metric spaces Metrics extend nicely across Cartesian products, which is very convenient because C-spaces are often constructed from Cartesian products, especially in the case of multiple bodies. Let (X, ρx) and (Y, ρy ) be two metric spaces. A metric space (Z, ρz ) can be constructed for the

Cartesian product Z = X × Y by defining the metric ρz as

ρz (z, z′) = ρz (x, y, x′, y′) = c1ρx(x, x′) + c2ρy (y, y′), (5.4) in which c1 > 0 and c2 > 0 are any positive real constants, and x, x′ X and

∈

y, y′ Y . Each z Z is represented as z = (x, y).

∈ ∈

Other combinations lead to a metric for Z; for example,

ρ (z, z′) = (c 「ρ (x, x′) p + c 「ρ (y, y′) p\1/p, (5.5)

z

1

x

2

y

is a metric for any positive integer p. Once again, two positive constants must be chosen. It is important to understand that many choices are possible, and there may not necessarily be a “correct” one.

• • 1. Important Metric Spaces for Motion Planning

This section presents some metric spaces that arise frequently in motion planning.

Example 5.1 (SO(2) Metric Using Complex Numbers) If SO(2) is repre- sented by unit complex numbers, recall that the C-space is the subset of R2 given by (a, b) R2 a2 + b2 = 1 . Any Lp metric from R2 may be applied. Using the Euclidean metric,

{ ∈ | }

ρ(a1, b1, a2, b2) = (a1 − a2)2 + (b1 − b2)2, (5.6)

/

for any pair of points (a1, b1) and (a2, b2). ．

Example 5.2 (SO(2) Metric by Comparing Angles) You might have noticed that the previous metric for SO(2) does not give the distance traveling along the circle. It instead takes a shortcut by computing the length of the line segment in R2 that connects the two points. This distortion may be undesirable. An alterna- tive metric is obtained by directly comparing angles, θ1 and θ2. However, in this case special care has to be given to the identification, because there are two ways to reach θ2 from θ1 by traveling along the circle. This causes a min to appear in the metric definition:

ρ(θ1, θ2) = min |θ1 − θ2|, 2π − |θ1 − θ2| , (5.7)

｛ 1

for which θ1, θ2 [0, 2π]/ . This may alternatively be expressed using the com- plex number representation a + bi as an angle between two vectors:

∈ ∼

ρ(a1, b1, a2, b2) = cos−1(a1a2 + b1b2), (5.8) for two points (a1, b1) and (a2, b2). ．

Example 5.3 (An SE(2) Metric) Again by using the subspace principle, a met- ric can easily be obtained for SE(2). Using the complex number representation of SO(2), each element of SE(2) is a point (xt, yt, a, b) R4. The Euclidean metric,

∈

or any other Lp metric on R4, can be immediately applied to obtain a metric. ．

Example 5.4 (SO(3) Metrics Using Quaternions) As usual, the situation be- comes more complicated for SO(3). The unit quaternions form a subset S3 of R4. Therefore, any Lp metric may be used to define a metric on S3, but this will not be a metric for SO(3) because antipodal points need to be identified. Let h1, h2 R4 represent two unit quaternions (which are being interpreted here as elements of R4 by ignoring the quaternion algebra). Taking the identifications into account, the metric is

∈

｛ 1

ρ(h1, h2) = min Ih1 − h2I, Ih1 + h2I , (5.9)

in which the two arguments of the min correspond to the distances from h1 to h2

and −h2, respectively. The h1 + h2 appears because h2 was negated to yield its

antipodal point, h2.

−

As in the case of SO(2), the metric in (5.9) may seem distorted because it measures the length of line segments that cut through the interior of S3, as opposed to traveling along the surface. This problem can be fixed to give a very natural

metric for SO(3), which is based on spherical linear interpolation. This takes the line segment that connects the points and pushes it outward onto S3. It is easier to visualize this by dropping a dimension. Imagine computing the distance between two points on S2. If these points lie on the equator, then spherical linear interpolation yields a distance proportional to that obtained by traveling along

the equator, as opposed to cutting through the interior of S2 (for points not on the equator, use the great circle through the points).

It turns out that this metric can easily be defined in terms of the inner product between the two quaternions. Recall that for unit vectors v1 and v2 in Rn, v1 v2 = cos θ, in which θ is the angle between the vectors. This angle is precisely what is needed to give the proper distance along S3. The resulting metric is a surprisingly simple extension of (5.8). The distance along S3 between two quaternions is

·

ρs(h1, h2) = cos−1(a1a2 + b1b2 + c1c2 + d1d2), (5.10)

in which each hi = (ai, bi, ci, di). Taking identification into account yields the metric

ρ(h1, h2) = min ｛ρs(h1, h2), ρs(h1, −h2)1. (5.11)

．

．

Example 5.5 (Another SE(2) Metric) For many C-spaces, the problem of re- lating different kinds of quantities arises. For example, any metric defined on SE(2) must compare both distance in the plane and an angular quantity. For example, even if c1 = c2 = 1, the range for S1 is [0, 2π) using radians but [0, 360) using degrees. If the same constant c2 is used in either case, two very different

metrics are obtained. The units applied to R2 and S1 are completely incompatible.

．

Example 5.6 (Robot Displacement Metric) Sometimes this incompatibility problem can be fixed by considering the robot displacement. For any two config-

urations q1, q2 ∈ C, a robot displacement metric can be defined as

ρ(q1, q2) = max ｛Ia(q1) − a(q2)I1, (5.12)

a∈A

in which a(qi) is the position of the point a in the world when the robot A is at configuration qi. Intuitively, the robot displacement metric yields the maximum

amount in W that any part of the robot is displaced when moving from configura-

tion q1 to q2. The difficulty and efficiency with which this metric can be computed depend strongly on the particular robot geometric model and kinematics. For a convex polyhedral robot that can translate and rotate, it is sufficient to check only vertices. The metric may appear to be ideal, but efficient algorithms are not

known for most situations. ．

Example 5.7 (Tn Metrics) Next consider making a metric over a torus Tn. The Cartesian product rules such as (5.4) and (5.5) can be extended over every copy of S1 (one for each parameter θi). This leads to n arbitrary coefficients c1, c2, . . ., cn.

Robot displacement could be used to determine the coefficients. For example, if the robot is a chain of links, it might make sense to weight changes in the first link more heavily because the entire chain moves in this case. When the last parameter is changed, only the last link moves; in this case, it might make sense to give it

less weight. ．

Example 5.8 (SE(3) Metrics) Metrics for SE(3) can be formed by applying the Cartesian product rules to a metric for R3 and a metric for SO(3), such as that given in (5.11). Again, this unfortunately leaves coefficients to be specified.

These issues will arise again in Section 5.3.4, where more details appear on robot displacement. ．

Pseudometrics Many planning algorithms use functions that behave somewhat like a distance function but may fail to satisfy all of the metric axioms. If such distance functions are used, they will be referred to as pseudometrics. One general principle that can be used to derive pseudometrics is to define the distance to be the optimal cost-to-go for some criterion (recall discrete cost-to-go functions from Section 2.3). This will become more important when differential constraints are considered in Chapter 14.

In the continuous setting, the cost could correspond to the distance traveled by a robot or even the amount of energy consumed. Sometimes, the resulting pseudometric is not symmetric. For example, it requires less energy for a car to travel downhill as opposed to uphill. Alternatively, suppose that a car is only capable of driving forward. It might travel a short distance to go forward from q1 to some q2, but it might have to travel a longer distance to reach q1 from q2 because it cannot drive in reverse. These issues arise for the Dubins car, which is covered in Sections 13.1.2 and 15.3.1.

An important example of a pseudometric from robotics is a potential function, which is an important part of the randomized potential field method, which is discussed in Section 5.4.3. The idea is to make a scalar function that estimates the distance to the goal; however, there may be additional terms that attempt to repel the robot away from obstacles. This generally causes local minima to appear in the distance function, which may cause potential functions to violate the triangle inequality.

• • 1. Basic Measure Theory Definitions

This section briefly indicates how to define volume in a metric space. This provides a basis for defining concepts such as integrals or probability densities. Measure theory is an advanced mathematical topic that is well beyond the scope of this

book; however, it is worthwhile to briefly introduce some of the basic definitions because they sometimes arise in sampling-based planning.

Measure can be considered as a function that produces real values for subsets of a metric space, (X, ρ). Ideally, we would like to produce a nonnegative value,

µ(A) [0, ], for any subset A X. Unfortunately, due to the Banach-Tarski paradox, if X = Rn, there are some subsets for which trying to assign volume leads to a contradiction. If X is finite, this cannot happen. Therefore, it is hard

∈ ∞ ⊆

to visualize the problem; see [836] for a construction of the bizarre nonmeasurable sets. Due to this problem, a workaround was developed by defining a collection of subsets that avoids the paradoxical sets. A collection of subsets of X is called a sigma algebra if the following axioms are satisfied:

B

• • • 1. The empty set is in B.
      2. If B ∈ B, then X \ B ∈ B.
      3. For any collection of a countable number of sets in B, their union must also be in B.

Note that the last two conditions together imply that the intersection of a count- able number of sets in is also in . The sets in are called the measurable sets.

B B B

A nice sigma algebra, called the Borel sets, can be formed from any metric space (X, ρ) as follows. Start with the set of all open balls in X. These are the sets of the form

B(x, r) = {x′ ∈ X | ρ(x, x′) < r} (5.13)

for any x X and any r (0, ). From the open balls, the Borel sets are the sets that can be constructed from these open balls by using the sigma algebra axioms. For example, an open square in R2 is in because it can be constructed as the union of a countable number of balls (infinitely many are needed because the curved balls must converge to covering the straight square edges). By using Borel sets, the nastiness of nonmeasurable sets is safely avoided.

B

∈ ∈ ∞ B

Example 5.9 (Borel Sets) A simple example of can be constructed for R. The open balls are just the set of all open intervals, (x1, x2) R, for any x1, x2 R

⊂ ∈

B

such that x1 < x2. ．

Using , a measure µ is now defined as a function µ : [0, ] such that the measure axioms are satisfied:

B B → ∞

1. For the empty set, µ(∅) = 0.
2. For any collection, E1, E2, E3, . . ., of a countable (possibly finite) number of pairwise disjoint, measurable sets, let E denote their union. The measure µ must satisfy

-

µ(E) = µ(Ei), (5.14)

i

in which i counts over the whole collection.

Example 5.10 (Lebesgue Measure) The most common and important mea- sure is the Lebesgue measure, which becomes the standard notions of length in R, area in R2, and volume in Rn for n 3. One important concept with Lebesgue measure is the existence of sets of measure zero. For any countable set A, the

≥

Lebesgue measure yields µ(A) = 0. For example, what is the total length of the point 1 R? The length of any single point must be zero. To satisfy the mea- sure axioms, sets such as 1, 3, 4, 5 must also have measure zero. Even infinite subsets such as Z and Q have measure zero in R. If the dimension of a set A Rn is m for some integer m < n, then µ(A) = 0, according to the Lebesgue measure on Rn. For example, the set S2 R3 has measure zero because the sphere has

⊆

⊂

{ }

{ } ⊂

no volume. However, if the measure space is restricted to S2 and then the surface area is defined, then nonzero measure is obtained. ．

Example 5.11 (The Counting Measure) If (X, ρ) is finite, then the counting measure can be defined. In this case, the measure can be defined over the entire

⊂ | |

power set of X. For any A X, the counting measure yields µ(A) = A , the number of elements in A. Verify that this satisfies the measure axioms. ．

Example 5.12 (Probability Measure) Measure theory even unifies discrete and continuous probability theory. The measure µ can be defined to yield probability mass. The probability axioms (see Section 9.1.2) are consistent with the measure axioms, which therefore yield a measure space. The integrals and sums needed to

define expectations of random variables for continuous and discrete cases, respec- tively, unify into a single measure-theoretic integral. ．

Measure theory can be used to define very general notions of integration that are much more powerful than the Riemann integral that is learned in classical calculus. One of the most important concepts is the Lebesgue integral. Instead of being limited to partitioning the domain of integration into intervals, virtually any partition into measurable sets can be used. Its definition requires the notion of a measurable function to ensure that the function domain is partitioned into measurable sets. For further study, see [346, 546, 836].

• • 1. Using the Correct Measure

Since many metrics and measures are possible, it may sometimes seem that there is no “correct” choice. This can be frustrating because the performance of sampling- based planning algorithms can depend strongly on these. Conveniently, there is a

natural measure, called the Haar measure, for some transformation groups, includ- ing SO(N). Good metrics also follow from the Haar measure, but unfortunately, there are still arbitrary alternatives.

The basic requirement is that the measure does not vary when the sets are transformed using the group elements. More formally, let G represent a matrix group with real-valued entries, and let µ denote a measure on G. If for any measurable subset A G, and any element g G, µ(A) = µ(gA) = µ(Ag), then

⊆ ∈

µ is called the Haar measure2 for G. The notation gA represents the set of all matrices obtained by the product ga, for any a A. Similarly, Ag represents all products of the form ag.

∈

Example 5.13 (Haar Measure for SO(2)) The Haar measure for SO(2) can be obtained by parameterizing the rotations as [0, 1]/ with 0 and 1 identified, and letting µ be the Lebesgue measure on the unit interval. To see the invariance property, consider the interval [1/4, 1/2], which produces a set A SO(2) of rotation matrices. This corresponds to the set of all rotations from θ = π/2 to θ = π. The measure yields µ(A) = 1/4. Now consider multiplying every matrix a A by a rotation matrix, g SO(2), to yield Ag. Suppose g is the rotation matrix for θ = π. The set Ag is the set of all rotation matrices from θ = 3π/2 up to θ = 2π = 0. The measure µ(Ag) = 1/4 remains unchanged. Invariance for gA may be checked similarly. The transformation g translates the intervals in [0, 1]/ . Since the measure is based on interval lengths, it is invariant with respect to translation. Note that µ can be multiplied by a fixed constant (such as 2π) without affecting the invariance property.

∼

⊂

∼

∈ ∈

An invariant metric can be defined from the Haar measure on SO(2). For any points x1, x2 ∈ [0, 1], let ρ = µ([x1, x2]), in which [x1, x2] is the shortest length

(smallest measure) interval that contains x1 and x2 as endpoints. This metric was already given in Example 5.2.

To obtain examples that are not the Haar measure, let µ represent probability mass over [0, 1] and define any nonuniform probability density function (the uni- form density yields the Haar measure). Any shifting of intervals will change the probability mass, resulting in a different measure.

Failing to use the Haar measure weights some parts of SO(2) more heavily than others. Sometimes imposing a bias may be desirable, but it is at least as important to know how to eliminate bias. These ideas may appear obvious, but in

the case of SO(3) and many other groups it is more challenging to eliminate this bias and obtain the Haar measure. ．

Example 5.14 (Haar Measure for SO(3)) For SO(3) it turns out once again that quaternions come to the rescue. If unit quaternions are used, recall that SO(3) becomes parameterized in terms of S3, but opposite points are identified.

2Such a measure is unique up to scale and exists for any locally compact topological group [346, 836].

It can be shown that the surface area on S3 is the Haar measure. (Since S3 is a 3D manifold, it may more appropriately be considered as a surface “volume.”) It will be seen in Section 5.2.2 that uniform random sampling over SO(3) must be done with a uniform probability density over S3. This corresponds exactly to the Haar

measure. If instead SO(3) is parameterized with Euler angles, the Haar measure

will not be obtained. An unintentional bias will be introduced; some rotations in

SO(3) will have more weight than others for no particularly good reason. ．