5.1.2 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 represented by unit complex numbers, recall that the C-space is the subset of ${\mathbb{R}}^2$ given by $\{(a,b)\in {\mathbb{R}}^2 \;\vert\; a^2+b^2 = 1\}$. Any $L_p$ metric from ${\mathbb{R}}^2$ may be applied. Using the Euclidean metric,

$$\rho(a_1,b_1,a_2,b_2) = \sqrt{(a_1-a_2)^2 + (b_1 - b_2)^2} ,$$ (5.6)

for any pair of points $(a_1,b_1)$ and $(a_2,b_2)$. $\blacksquare$

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 ${\mathbb{R}}^2$ that connects the two points. This distortion may be undesirable. An alternative metric is obtained by directly comparing angles, $\theta_1$ and $\theta_2$. However, in this case special care has to be given to the identification, because there are two ways to reach $\theta_2$ from $\theta_1$ by traveling along the circle. This causes a $\min$ to appear in the metric definition:

$$\rho(\theta_1,\theta_2) = \min \big\{ \vert\theta_1 - \theta_2\vert,2 \pi - \vert\theta_1-\theta_2\vert \big\} ,$$ (5.7)

for which $\theta_1,\theta_2 \in [0,2 \pi] {/\sim}$. This may alternatively be expressed using the complex number representation $a + bi$ as an angle between two vectors:

$$\rho(a_1,b_1,a_2,b_2) = \cos^{-1}(a_1a_2+b_1b_2) ,$$ (5.8)

for two points $(a_1,b_1)$ and $(a_2,b_2)$. $\blacksquare$

Example 5..3 (An $SE(2)$ Metric)   Again by using the subspace principle, a metric can easily be obtained for $SE(2)$. Using the complex number representation of $SO(2)$, each element of $SE(2)$ is a point $(x_t,y_t,a,b) \in {\mathbb{R}}^4$. The Euclidean metric, or any other $L_p$ metric on ${\mathbb{R}}^4$, can be immediately applied to obtain a metric. $\blacksquare$

Example 5..4 ($SO(3)$ Metrics Using Quaternions)   As usual, the situation becomes more complicated for $SO(3)$. The unit quaternions form a subset ${\mathbb{S}}^3$ of ${\mathbb{R}}^4$. Therefore, any $L_p$ metric may be used to define a metric on ${\mathbb{S}}^3$, but this will not be a metric for $SO(3)$ because antipodal points need to be identified. Let $h_1, h_2 \in {\mathbb{R}}^4$ represent two unit quaternions (which are being interpreted here as elements of ${\mathbb{R}}^4$ by ignoring the quaternion algebra). Taking the identifications into account, the metric is

$$\rho(h_1,h_2) = \min \big\{ \Vert h_1-h_2\Vert,\Vert h_1+h_2\Vert \big\} ,$$ (5.9)

in which the two arguments of the $\min$ correspond to the distances from $h_1$ to $h_2$ and $-h_2$, respectively. The $h_1+h_2$ appears because $h_2$ was negated to yield its antipodal point, $-h_2$.

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 ${\mathbb{S}}^3$, 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 ${\mathbb{S}}^3$. It is easier to visualize this by dropping a dimension. Imagine computing the distance between two points on ${\mathbb{S}}^2$. 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 ${\mathbb{S}}^2$ (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 $v_1$ and $v_2$ in ${\mathbb{R}}^n$, $v_1 \cdot v_2 = \cos\theta$, in which $\theta$ is the angle between the vectors. This angle is precisely what is needed to give the proper distance along ${\mathbb{S}}^3$. The resulting metric is a surprisingly simple extension of (5.8). The distance along ${\mathbb{S}}^3$ between two quaternions is

$$\rho_s(h_1,h_2) = \cos^{-1}(a_1 a_2+b_1 b_2+c_1 c_2+d_1 d_2),$$ (5.10)

in which each $h_i = (a_i,b_i,c_i,d_i)$. Taking identification into account yields the metric

$$\rho(h_1,h_2) = \min \big\{\rho_s(h_1,h_2),\rho_s(h_1,-h_2)\big\} .$$ (5.11)

$\blacksquare$

Example 5..5 (Another $SE(2)$ Metric)   For many C-spaces, the problem of relating 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 $c_1 = c_2 = 1$, the range for ${\mathbb{S}}^1$ is $[0,2 \pi)$ using radians but $[0,360)$ using degrees. If the same constant $c_2$ is used in either case, two very different metrics are obtained. The units applied to ${\mathbb{R}}^2$ and ${\mathbb{S}}^1$ are completely incompatible. $\blacksquare$

Example 5..6 (Robot Displacement Metric)   Sometimes this incompatibility problem can be fixed by considering the robot displacement. For any two configurations $q_1, q_2 \in {\cal C}$, a robot displacement metric can be defined as

$$\rho(q_1,q_2) = \max_{a \in {\cal A}} \big\{ \Vert a(q_1) - a(q_2) \Vert \big\},$$ (5.12)

in which $a(q_i)$ is the position of the point $a$ in the world when the robot ${\cal A}$ is at configuration $q_i$. Intuitively, the robot displacement metric yields the maximum amount in ${\cal W}$ that any part of the robot is displaced when moving from configuration $q_1$ to $q_2$. 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. $\blacksquare$

Example 5..7 ( ${\mathbb{T}}^n$ Metrics)   Next consider making a metric over a torus ${\mathbb{T}}^n$. The Cartesian product rules such as (5.4) and (5.5) can be extended over every copy of ${\mathbb{S}}^1$ (one for each parameter $\theta _i$). This leads to $n$ arbitrary coefficients $c_1$, $c_2$, $\ldots$, $c_n$. 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. $\blacksquare$

Example 5..8 ($SE(3)$ Metrics)   Metrics for $SE(3)$ can be formed by applying the Cartesian product rules to a metric for ${\mathbb{R}}^3$ 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. $\blacksquare$