Defining new neighborhoods

For defining new neighborhoods, it is important to keep them simple because during execution, the neighborhoods that contain the state $x$ must be determined quickly. Suppose that all neighborhoods are open balls:

$$B(x,r) = \{x^\prime \in X \;\vert\; \rho(x,{ x}^\prime) < r\} ,$$ (8.52)

in which $\rho$ is the metric on ${\cal C}$. There are efficient algorithms for determining whether $x \in O$ for some $O \in {\cal O}$, assuming all of the neighborhoods are balls [700]. In practice, methods based on Kd-trees yield good performance [47,52] (recall Section 5.5.2). A new ball, $B(x,r)$, can be constructed in Step 3 for $x = {\alpha}(i)$, but what radius can be assigned? For a point robot that translates in ${\mathbb{R}}^2$ or ${\mathbb{R}}^3$, the Hausdorff distance $d$ between the robot and obstacles in ${\cal W}$ is precisely the distance to ${\cal C}_{obs}$ from ${\alpha }(i)$. This implies that we can set $r = d$, and $B(x,r)$ is guaranteed to be collision-free.

In a general configuration space, it is possible to find a value of $r$ such that $B(x,r) \subseteq {\cal C}_{free}$, but in general $r < d$. This issue arose in Section 5.3.4 for checking path segments. The transformations of Sections 3.2 and 3.3 become important in the determination of $r$. For illustrative purposes, suppose that ${\cal C}= {\mathbb{R}}^2 \times {\mathbb{S}}^1$, which corresponds to a rigid robot, ${\cal A}$, that can translate and rotate in ${\cal W}= {\mathbb{R}}^2$. Each point $a \in {\cal A}$ is transformed using (3.35). Now imagine starting with some configuration $q = (x,y,\theta)$ and perturbing each coordinate by some $\Delta x$, $\Delta y$, and $\Delta \theta$. What is the maximum distance that a point on ${\cal A}$ could travel? Translation affects all points on ${\cal A}$ the same way, but rotation affects points differently. Recall Figure 5.12 from Section 5.3.4. Let $a_r \in {\cal A}$ denote the point that is furthest from the origin $(0,0)$. Let $r$ denote the distance from $a_r$ to the origin. If the rotation is perturbed by some small amount, $\Delta \theta$, then the displacement of any $a \in {\cal A}$ is no more than $r \Delta \theta$. If all three configuration parameters are perturbed, then

$$(\Delta x)^2 + (\Delta y)^2 + (r \Delta \theta)^2 < d^2$$ (8.53)

is the constraint that must be satisfied to ensure that the resulting ball is contained in ${\cal C}_{free}$. This is actually the equation of a solid ellipsoid, which becomes a ball if $r = 1$. This can be made into a ball by reparameterizing $SE(2)$ so that $\Delta \theta$ has the same affect as $\Delta x$ and $\Delta y$. A transformation $h: \theta \mapsto r \theta$ maps $\theta$ into a new domain $Z = [0,2 \pi r)$. In this new space, the equation of the ball is

$$(\Delta x)^2 + (\Delta y)^2 + (\Delta z)^2 < d^2 ,$$ (8.54)

in which $\Delta z$ represents the change in $z \in Z$. The reparameterized version of (3.35) is

$$T = \begin{pmatrix}\cos(\theta/r) & -\sin(\theta/r) & x_t\\ \sin(\theta/r) & \cos(\theta/r) & y_t\\ 0 & 0 & 1 \\ \end{pmatrix} .$$ (8.55)

For a 3D rigid body, similar reparameterizations can be made to Euler angles or quaternions to generate six-dimensional balls. Extensions can be made to chains of bodies [983]. One of the main difficulties, however, is that the balls are not the largest possible. In higher dimensions the problem becomes worse because numerous balls are needed, and the radii constructed as described above tend to be much smaller than what is possible. The number of balls can be reduced by also allowing axis-aligned cylinders, but it still remains difficult to construct a cover over a large fraction of ${\cal C}_{free}$ in more than six dimensions.