6.2.4 Shortest-Path Roadmaps

Instead of generating paths that maximize clearance, suppose that the goal is to find shortest paths. This leads to the shortest-path roadmap, which is also called the reduced visibility graph in [588]. The idea was first introduced in [742] and may perhaps be the first example of a motion planning algorithm. The shortest-path roadmap is in direct conflict with maximum clearance because shortest paths tend to graze the corners of ${\cal C}_{obs}$. In fact, the problem is ill posed because ${\cal C}_{free}$ is an open set. For any path $\tau: [0,1] \rightarrow {\cal C}_{free}$, it is always possible to find a shorter one. For this reason, we must consider the problem of determining shortest paths in $\operatorname{cl}({\cal C}_{free})$, the closure of ${\cal C}_{free}$. This means that the robot is allowed to ``touch'' or ``graze'' the obstacles, but it is not allowed to penetrate them. To actually use the computed paths as solutions to a motion planning problem, they need to be slightly adjusted so that they come very close to ${\cal C}_{obs}$ but do not make contact. This slightly increases the path length, but the additional cost can be made arbitrarily small as the path gets arbitrarily close to ${\cal C}_{obs}$.

Figure 6.10: A bitangent edge must touch two reflex vertices that are mutually visible from each other, and the line must extend outward past each of them without poking into ${\cal C}_{obs}$.

Figure 6.11: The shortest-path roadmap includes edges between consecutive reflex vertices on ${\cal C}_{obs}$ and also bitangent edges.

Figure 6.12: To solve a query, ${q_{I}}$ and ${q_{G}}$ are connected to all visible roadmap vertices, and graph search is performed.

Figure 6.13: The shortest path in the extended roadmap is the shortest path between ${q_{I}}$ and ${q_{G}}$.

The shortest-path roadmap, ${\cal G}$, is constructed as follows. Let a reflex vertex be a polygon vertex for which the interior angle (in ${\cal C}_{free}$) is greater than $\pi$. All vertices of a convex polygon (assuming that no three consecutive vertices are collinear) are reflex vertices. The vertices of ${\cal G}$ are the reflex vertices. Edges of ${\cal G}$ are formed from two different sources:

1. [] Consecutive reflex vertices: If two reflex vertices are the endpoints of an edge of ${\cal C}_{obs}$, then an edge between them is made in ${\cal G}$.
2. [] Bitangent edges: If a bitangent line can be drawn through a pair of reflex vertices, then a corresponding edge is made in ${\cal G}$. A bitangent line, depicted in Figure 6.10, is a line that is incident to two reflex vertices and does not poke into the interior of ${\cal C}_{obs}$ at any of these vertices. Furthermore, these vertices must be mutually visible from each other.

An example of the resulting roadmap is shown in Figure 6.11. Note that the roadmap may have isolated vertices, such as the one at the top of the figure. To solve a query, ${q_{I}}$ and ${q_{G}}$ are connected to all roadmap vertices that are visible; this is shown in Figure 6.12. This makes an extended roadmap that is searched for a solution. If Dijkstra's algorithm is used, and if each edge is given a cost that corresponds to its path length, then the resulting solution path is the shortest path between ${q_{I}}$ and ${q_{G}}$. The shortest path for the example in Figure 6.12 is shown in Figure 6.13.

Figure 6.14: Potential bitangents can be identified by checking for left turns, which avoids the use of trigonometric functions and their associated numerical problems.

If the bitangent tests are performed naively, then the resulting algorithm requires $O(n^3)$ time, in which $n$ is the number of vertices of ${\cal C}_{obs}$. There are $O(n^2)$ pairs of reflex vertices that need to be checked, and each check requires $O(n)$ time to make certain that no other edges prevent their mutual visibility. The plane-sweep principle from Section 6.2.2 can be adapted to obtain a better algorithm, which takes only $O(n^2 \lg n)$ time. The idea is to perform a radial sweep from each reflex vertex, $v$. A ray is started at $\theta = 0$, and events occur when the ray touches vertices. A set of bitangents through $v$ can be computed in this way in $O(n \lg n)$ time. Since there are $O(n)$ reflex vertices, the total running time is $O(n^2 \lg n)$. See Chapter 15 of [264] for more details. There exists an algorithm that can compute the shortest-path roadmap in time $O(n \lg n + m)$, in which $m$ is the total number of edges in the roadmap [384]. If the obstacle region is described by a simple polygon, the time complexity can be reduced to $O(n)$; see [709] for many shortest-path variations and references.

To improve numerical robustness, the shortest-path roadmap can be implemented without the use of trigonometric functions. For a sequence of three points, $p_1$, $p_2$, $p_3$, define the left-turn predicate, $f_l: {\mathbb{R}}^2 \times {\mathbb{R}}^2 \times {\mathbb{R}}^2 \rightarrow \{$TRUE$,\;$FALSE$\}$, as $f_l(p_1,p_2,p_3) =$   TRUE if and only if $p_3$ is to the left of the ray that starts at $p_1$ and pierces $p_2$. A point $p_2$ is a reflex vertex if and only if $f_l(p_1,p_2,p_3) =$   TRUE, in which $p_1$ and $p_3$ are the points before and after, respectively, along the boundary of ${\cal C}_{obs}$. The bitangent test can be performed by assigning points as shown in Figure 6.14. Assume that no three points are collinear and the segment that connects $p_2$ and $p_5$ is not in collision. The pair, $p_2$, $p_5$, of vertices should receive a bitangent edge if the following sentence is FALSE:

$$\big( f_l(p_1,p_2,p_5) \oplus f_l(p_3,p_2,p_5) \big) \vee \big( f_l(p_4,p_5,p_2) \oplus f_l(p_6,p_5,p_2) \big),$$ (6.1)

in which $\oplus$ denotes logical ``exclusive or.'' The $f_l$ predicate can be implemented without trigonometric functions by defining

$$M(p_1,p_2,p_3) = \begin{pmatrix}1 & x_1 & y_1 1 & x_2 & y_2 1 & x_3 & y_3 \end{pmatrix} ,$$ (6.2)

in which $p_i = (x_i,y_i)$. If $\det(M) > 0$, then $f_l(p_1,p_2,p_3) =$   TRUE; otherwise, $f_l(p_1,p_2,p_3) =$   FALSE.