5.3.4 Checking a Path Segment

Collision detection algorithms determine whether a configuration lies in ${\cal C}_{free}$, but motion planning algorithms require that an entire path maps into ${\cal C}_{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.

Suppose that a path, $\tau: [0,1] \rightarrow {\cal C}$, needs to be checked to determine whether $\tau([0,1]) \subset {\cal C}_{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. In practice, a fixed $\Delta q > 0$ is often chosen as the C-space step size. Points $t_1, t_2 \in [0,1]$ are then chosen close enough together to ensure that $\rho(\tau(t_1),\tau(t_2)) \leq \Delta q$, in which $\rho$ is the metric on ${\cal C}$. The value of $\Delta q$ is often determined experimentally. If $\Delta q$ is too small, then considerable time is wasted on collision checking. If $\Delta q$ is too large, then there is a chance that the robot could jump through a thin obstacle.

Setting $\Delta 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 $\tau([0,1]) \subset {\cal C}_{free}$ requires the use of both distance information and bounds on the distance that points on ${\cal A}$ can travel in ${\mathbb{R}}$. Such bounds can be obtained by using the robot displacement metric from Example 5.6. Before expressing the general case, first we will explain the concept in terms of a rigid robot that translates and rotates in ${\cal W}= {\mathbb{R}}^2$. Let $x_t,y_t\in {\mathbb{R}}^2$ and $\theta \in [0,2 \pi ]{/\sim }$. Suppose that a collision detection algorithm indicates that ${\cal A}(q)$ is at least $d$ units away from collision with obstacles in ${\cal W}$. This information can be used to determine a region in ${\cal C}_{free}$ that contains $q$. Suppose that the next candidate configuration to be checked along $\tau$ is $q^\prime$. If no point on ${\cal A}$ travels more than distance $d$ when moving from $q$ to $q^\prime$ along $\tau$, then $q^\prime$ and all configurations between $q$ and $q^\prime$ must be collision-free. This assumes that on the path from $q$ to $q^\prime$, every visited configuration must lie between $q_i$ and $q'_i$ for the $i$th coordinate and any $i$ from $1$ to $n$. If the robot can instead take any path between $q$ and $q^\prime$, then no such guarantee can be made).

Figure 5.12: The furthest point on ${\cal A}$ from the origin travels the fastest when ${\cal A}$ is rotated. At most it can be displaced by $2 \pi r$, if $x_t$ and $y_t$ are fixed.

When ${\cal A}$ undergoes a translation, all points move the same distance. For rotation, however, the distance traveled depends on how far the point on ${\cal A}$ is from the rotation center, $(0,0)$. Let $a_r = (x_r,y_r)$ denote the point on ${\cal A}$ that has the largest magnitude, $r = \sqrt{x_r^2+y_r^2}$. Figure 5.12 shows an example. A transformed point $a \in {\cal A}$ may be denoted by $a(x_t,y_t,\theta)$. The following bound is obtained for any $a \in {\cal A}$, if the robot is rotated from orientation $\theta$ to $\theta^\prime$:

$$\Vert a(x_t,y_t,\theta) - a(x_t,y_t,\theta^\prime)\Vert \leq \Ver... ...\theta) - a_r(x_t,y_t,\theta^\prime)\Vert < r \vert\theta - \theta^\prime\vert,$$ (5.30)

assuming that a path in ${\cal C}$ is followed that interpolates between $\theta$ and $\theta^\prime$ (using the shortest path in ${\mathbb{S}}^1$ between $\theta$ and $\theta^\prime$). Thus, if ${\cal A}(q)$ is at least $d$ away from the obstacles, then the orientation may be changed without causing collision as long as $r \vert\theta - \theta^\prime\vert < d$. Note that this is a loose upper bound because $a_r$ travels along a circular arc and can be displaced by no more than $2 \pi r$.

Similarly, $x_t$ and $y_t$ may individually vary up to $d$, yielding $\vert x_t- x_t^\prime\vert < d$ and $\vert y_t- y_t^\prime\vert < d$. If all three parameters vary simultaneously, then a region in ${\cal C}_{free}$ can be defined as

$$\{(x_t^\prime,y_t^\prime,\theta^\prime) \in {\cal C}\;\vert\; \ve... ...\vert + \vert y_t- y_t^\prime\vert + r \vert\theta - \theta^\prime\vert < d \}.$$ (5.31)

Such bounds can generally be used to set a step size, $\Delta q$, for collision checking that guarantees the intermediate points lie in ${\cal C}_{free}$. The particular value used may vary depending on $d$ and the direction^5.8 of the path.

For the case of $SO(3)$, once again the displacement of the point on ${\cal A}$ that has the largest magnitude can be bounded. It is best in this case to express the bounds in terms of quaternion differences, $\Vert h - h^\prime\Vert$. Euler angles may also be used 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 \in {\cal A}_i$ is transformed by some nonlinear function based on the kinematic expressions from Sections 3.3 and 3.4. Let $a : {\cal C} \rightarrow {\cal W}$ denote this transformation. In some cases, it might be possible to derive a Lipschitz condition of the form

$$\Vert a(q) - a(q^\prime) \Vert < c \Vert q - q^\prime\Vert,$$ (5.32)

in which $c \in (0,\infty)$ is a fixed constant, $a$ is any point on ${\cal A}_i$, and the expression holds for any $q, q^\prime \in {\cal C}$. The goal is to make the Lipschitz constant, $c$, as small as possible; this enables larger variations in $q$.

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

$$\Vert a(q_1,\ldots,q_{i-1},q_i,q_{i+1},\ldots,q_n) - a(q_1,\ldots,q_{i-1},q_i^\prime,q_{i+1},\ldots,q_n) \Vert < c_i \vert q_i - q_i^\prime\vert,$$ (5.33)

to obtain the Lipschitz constants $c_1$, $\ldots$, $c_n$. The bound on robot displacement becomes

$$\Vert a(q) - a(q^\prime) \Vert < \sum_{i=1}^n c_i \vert q_i - q_i^\prime\vert.$$ (5.34)

The benefit of using individual parameter bounds can be seen by considering a long chain. Consider a 50-link chain of line segments in ${\mathbb{R}}^2$, and each link has length $10$. The C-space is ${\mathbb{T}}^{50}$, which can be parameterized as $[0,2 \pi]^{50}{/\sim}$. Suppose that the chain is in a straight-line configuration ( $\theta_i = 0$ for all $1 \leq i \leq 50$), which means that the last point is at $(500,0) \in {\cal W}$. Changes in $\theta_1$, the orientation of the first link, dramatically move ${\cal A}_{50}$. However, changes in $\theta_{50}$ move ${\cal A}_{50}$ a smaller amount. Therefore, it is advantageous to pick a different $\Delta q_i$ for each $1 \leq i \leq 50$. In this example, a smaller value should be used for $\Delta \theta_1$ in comparison to $\Delta \theta_{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 ( $\theta_i = \pi$ for each $1 \leq i \leq n$). In this case, ${\cal A}_{50}$ does not move as fast when $\theta_1$ is perturbed, in comparison to the straight-line configuration. A larger step size for $\theta_1$ could be used for this configuration, relative to other parts of ${\cal C}$. The implication is that, although Lipschitz constants can be made to hold over all of ${\cal C}$, it might be preferable to determine a better bound in a local region around $q \in {\cal C}$. A linear method could be obtained by analyzing the Jacobian of the transformations, such as (3.53) and (3.57).

Another important concern when checking a path is the order in which the samples are checked. For simplicity, suppose that $\Delta 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, ${\nu}$. The last column in Figure 5.2 indicates precisely where to check along the path in each step. Initially, $\tau(1)$ is checked. Following this, points from the van der Corput sequence are checked in order: $\tau(0)$, $\tau(1/2)$, $\tau(1/4)$, $\tau(3/4)$, $\tau(1/8)$, $\ldots$. The process terminates if a collision is found or when the dispersion falls below $\Delta q$. If $\Delta q$ is not constant, then it is possible to skip over some points of ${\nu}$ in regions where the allowable variation in $q$ is larger.