Finding Curvature-Constrained Paths that Avoid ... - CiteSeerX
Finding Curvature-Constrained Paths that Avoid Polygonal Obstacles [Extended Abstract] Jonathan Backer
David Kirkpatrick
Department of Computer Science University of British Columbia 201-2366 Main Mall, Vancouver BC, V6T 1Z4, Canada
Department of Computer Science University of British Columbia 201-2366 Main Mall, Vancouver BC, V6T 1Z4, Canada
[email protected]
[email protected]
ABSTRACT
point on the vehicle (say, the mid-point of its rear axle), we prohibit reversal, and we permit arbitrary changes in motion curvature as long as its magnitude remains bounded (in effect, the steering wheel has a restriction on how far, but not how fast, it can turn). Specifically, we insist that the path is continuous and that |Δφ/Δl| < r−1 between any two points on the path, where Δφ is the change in forward direction measured in radians, Δl is the distance travelled between the points, and r the radius of curvature. Intuitively, this restriction means that every point on the path can be locally sandwiched between tangent circles of radius r (see Figure 1(b)). By suitable scaling, we can assume that r = 1. So, without loss of generality, we restrict our attention to paths of unit-curvature.
We describe an algorithm to find a unit-curvature path between specified configurations in an arbitrary polygonal domain. Whenever such a path exists, the algorithm returns an explicit description of one such path in time that is polynomial in n (the number of features of the domain), m (the precision of the input) and k (the number of segments on the simplest obstacle-free Dubins path connecting the specified configurations). Our algorithm is based on a new normal form for unit-curvature paths and a dynamic path filtering argument that exploits a separation bound for distinct paths in this normal form. The best result known for the feasibility of bounded-curvature motion in the presence of arbitrary polygonal obstacles involves a reduction to the first-order theory of the reals. It just determines if a feasible path exists (it does not return a path) and requires exponential time and space.
Categories and Subject Descriptors F.2.2 [Nonnumerical Algorithms and Problems]: Geometrical problems and computations
r
General Terms Algorithms, Theory
(a) amid obstacles
Keywords
(b) sandwiched
Figure 1: A bounded-curvature path amid obstacles (left) and an illustration of a bounded-curvature path wedged between constant-radius circles (right).
bounded-curvature, motion planning, path feasibility
1. PROBLEM STATEMENT We assume that the free space is specified by an environment with a polygonal boundary. We refer to the vertices and edges that describe this boundary as environment features, and we denote by n the total number of environment features and m the maximum input bit precision specifying any feature (defined in Section 5.2). An (instantaneous) configuration of the vehicle is specified by its position and direction of motion. Given an initial and final configuration, we want to determine if there exists a unit-curvature path within the free space that connects these two configurations. Furthermore, if such a path exists, we want to provide an explicit description of one such path. The path existence question is decidable: Fortune and Wilfong present a decision procedure whose time and space
We are interested in planning the collision-free motion of vehicles with a restricted turning radius in the presence of arbitrary polygonal obstacles (see Figure 1(a)). As in the bulk of previous work, we consider paths traced by a fixed
Permission to make digital or hard copies of all or part of this work for personal or classroom use is granted without fee provided that copies are not made or distributed for profit or commercial advantage and that copies bear this notice and the full citation on the first page. To copy otherwise, to republish, to post on servers or to redistribute to lists, requires prior specific permission and/or a fee. SCG’07, June 6–8, 2007, Gyeongju, South Korea. Copyright 2007 ACM 978-1-59593-705-6/07/0006 ...$5.00.
66
O(1)
complexity is exponential (2(n+m) ) [7]. However, their reduction does not provide an explicit description of a path, when one exists. Indeed, it is not clear if this is even possible in exponential time, since we do not have an upper bound on the complexity of the simplest feasible path, as a function of n and m. Finding a minimum-length bounded-curvature path in the presence of arbitrary polygonal obstacles is NP-hard [10]. Although efficient approximation algorithms exist [8, 11], they are incomplete: They only guarantee to find an approximation of the shortest bounded-curvature path that satisfies a robustness criterion. Thus, they may terminate without producing any path, if all feasible paths are nonrobust. The existence of even an exponential-time complete approximation algorithm is uncertain for the same reason that we described above. In this paper, we describe an algorithm to find a unitcurvature path between specified configurations in an arbitrary polygonal environment. Whenever such a path exists, the algorithm returns an explicit description of one such path in time that is polynomial in n, m, and k (the number of segments on the simplest obstacle-free Dubins path connecting the specified configurations). The parameter k provides a natural notion of intrinsic complexity of the desired output. Our algorithm is based on a new normal form for unit-curvature paths and a dynamic path filtering technique that exploits a separation bound for distinct normal paths. We start with a brief overview of related bounded-curvature results. Then we describe a new normal form for unitcurvature paths that restricts our attention to paths with a combinatorial description (see Section 3). This reduces determining path existence to searching a finite (but potentially exponential) space. Section 4 describes our algorithm. It associates with every feature a set of configurations that serve as an adequate set of potential checkpoints in the construction of a path. The total size of the these configuration sets is a dominant factor in the complexity of our algorithm. Section 5 shows how we obtain a polynomial upper bound on the size of individual configuration sets. To do this, we exploit a dynamic redundancy-elimination (filtering) argument together with a bound on the minimum separation of complexity-bounded paths in the presence of precision-restricted obstacles. Finally, Section 6 describes some applications and potential extensions of our results.
that the structure of a shortest path (the sequence of the types of segments of which it is made) must be either CSC or CCC, where each segment may have zero length. Wang and Agarwal argue that amid polygonal obstacles, the shortest bounded-curvature path is a sequence of these Dubins paths between obstacle contacts [11]. Any such path is said to be in Dubins normal form, and has complexity equal to its number of C-segments.
Figure 2: Short (≤ π) unit-curvature paths are sandwiched between two unit circles tangent to the initial configuration (dotted) and two unit circles tangent to the final configuration (dashed). The shortest such path between those configurations follows the bounding circles.
Fortune and Wilfong [7] describe a more restrictive normal form in which every C-segment is supported by (touches, but does not intersect) an obstacle feature. Although unstated by Fortune and Wilfong, their normalization can ensure that every π length subarc of a C-segment is supported. We refer to this strengthened version as F-W normal. The search for F-W normal paths can be viewed as the identification of configurations at environment features that, together, form an adequate set of checkpoints for the realization of unit-curvature paths. Fortune and Wilfong show how these configuration sets, though uncountable in size, can be compactly expressed in the first-order theory of the reals. This leads to an decision procedure for feasibility that is exponential in time and space. In Section 3, we introduce a restriction of F-W normal form paths that forms the basis of our algorithm.
2.2
Shortest Path Approximation
The NP-hardness of finding shortest paths amid polygonal obstacles [10] motivates two efficient approximation algorithms [8, 11]. Both algorithms explicitly construct a weighted visibility graph by uniformly sampling potential obstacle contacts with a granularity proportional to the desired approximation ratio. The shortest path in the resulting visibility graph corresponds to a (1 + ) approximation to the shortest (/2)-robust path, where robustness is a measure of how much a bounded-curvature path can be perturbed at its obstacle contacts while retaining its structure. As we noted earlier, these approximation algorithms are incomplete; specifically, the only feasible path may be 0robust, so these approximation algorithms may never find a feasible path regardless of the input . We note that, the complexity of the paths found by these algorithms polynomial in the input, which is consistent with our use of intrinsic complexity in runtime analysis. In particular, the worst case number of C-segments on any returned path is at most three times the number of edges in the visibility graph because every edge is traversed at most once and each edge corresponds to a Dubins’ path with structure CSC or CCC. Our algorithm non-uniformly samples potential obstacle
2. BACKGROUND AND RELATED WORK 2.1 Feasibility Testing via Path Normalization The identification of a normal form for paths often lies at the heart of path planning. Even the goal of constructing shortest paths can be viewed as a useful restriction of the search space (in addition to being a well-motivated optimization criterion). In the absence of obstacles, Dubins shows how to shrink suboptimal paths thereby demonstrating that shortest bounded-curvature paths either require or forbid certain properties [6]. These arguments implicitly describe a normalization process that iteratively shrinks a path until it is shortest. Dubins first notices that shortest unit-curvature paths are made of arcs of unit radius circles (C-segments) and straight line segments (S-segments), because short subpaths have this property (see Figure 2). Then Dubins shows
67
contacts to construct a visibility graph while it is explored. This real time construction is history dependent, which allows it to account for global obstacle geometry. However, the approximation algorithms just independently consider small subsets of features. Another difference is that every path that we find is 0-robust at every obstacle contact, whereas the approximation algorithms may fail to find such paths.
consequence, our normal form paths are described by a finite sequence of discrete path constraints. This combinatorial description implies that the set of all normal paths between the same initial and final configurations is countable and systematically searchable. We begin with a Dubins normal path, which is a finite sequence of C-segments and S-segments (see Figure 4(a)). First, we apply the normalization procedure of Fortune and Wilfong, effectively pushing all C-segments into contact with the environment (see Figures 4(b) and 4(c)). As noted by Fortune and Wilfong, the resulting path is at most a factor of n, the number of environment features, larger than the original path. Next, we iteratively twist each C-segment (while maintaining its environment contact) into an extreme position (see Figure 4(d)). In its extreme position, each Csegment is either anchored (doubly constrained by the local environment) or chained to an anchored C-segment by a sequence of couplings (singly constrained transitions).
2.3 Path planning in Restricted Domains Agarwal et al. characterize shortest paths amid simply connected, convex obstacles with bounded-curvature boundaries (see Figure 3(a)). This domain restriction limits the placement of C-segments and admits an efficient, complete approximation algorithm [2]. Boissonnat and Lazard refine this into an exact algorithm by eliminating a degree of freedom in the C-segment placement in one difficult case [4]. As the obstacles are convex and their boundaries can be traced by a bounded-curvature path, no obstacle boundary on a shortest path is touched more than twice in each direction. Thus, the number of environment features bounds the complexity of the shortest path.
(a) moderate obstacles
(a) initial Dubins path
(b) pop out turns
(c) pull in turns
(d) twist turns
(b) corridor Figure 4: Different phases of path normalization.
Figure 3: Unit-curvature paths are found efficiently in these restricted domains. The obstacle-free space is white and includes the black boundary.
Every Dubins path can be described by the sequence of oriented unit circles underlying each C-segment (see Figure 5(a)) because there is a unique (possibly degenerate) Ssegment from one oriented circle to another (see Figure 5(b)). Forcing C-segments to touch the environment restricts their underlying circle centres to a 1-D manifold. However, a record of each underlying circle’s orientation and supporting environment feature incompletely specifies the path because there remains a degree of freedom at each environment contact, which allows a continuous range of paths to match one such description (see Figure 4(d)). To refine the specification, we twist each underlying circle and augment the description with how each circle was twisted. When we twist, we push an underlying circle as far as possible in a given direction while maintaining three invariants: we keep the path F-W normal, we do not add any new underlying circles, and we preserve the obstacle support and orientation of each underlying circle. Figure 6 illustrates the consequences of these invariants as we twist an underlying circle C in a counterclockwise manner about its environment contact c. In Figure 6(a), we pivot C counterclockwise at c until it kisses the previous underlying circle B. At this point, C is said to be coupled to B because if B remains stationary, we cannot twist C further. So in Figure 6(b), we continue by simultaneously twisting C and B, maintaining the coupling throughout. We do this until any further simultaneous twisting eliminates the environment support (at a) of the path segment on circle A. This causes a second form of coupling between A and B that is maintained by simultaneously twisting A, B, and C.
Agarwal et al. analyze the structure of shortest boundedcurvature paths inside convex polygons to limit the placement of C-segments and get an efficient exact algorithm [1]. In this domain, the complexity of the shortest path is O(1). Bereg and Kirkpatrick examine feasible, but not necessarily shortest, bounded-curvature paths in corridors (regions swept out by a disk as its center moves along a polygonal path — see Figure 3(b)) [3]. Their objective is to make explicit a minimum corridor width, τ ≈ 1.505, that guarantees unit-curvature traversability, and to describe an efficient greedy algorithm to construct an unit-curvature traversal in any corridor of width at least τ . The complexity of the paths found by their algorithm is bounded by the complexity of the corridor. Bereg and Kirkpatrick leave open the question of whether a polynomial time algorithm exists for determining whether a given corridor with width less than τ is traversable. As we argue in Section 6, our algorithm answers this in the affirmative.
3. A NEW NORMAL FORM A F-W normal path, like a plumber’s snake that has been threaded through a sequence of coplanar pipes, has every bend constrained by the environment, but retains a degree of freedom at some of its environment contacts that permits lateral twisting motions. Our new normal form removes this degree of freedom from all environment contacts. As a
68
c
c
C
C b
(a) circle representation
A
(b) unique transition
d A
E
a
Figure 5: We can represent F-W paths by their underlying circles (left) because there is one F-W transition from one oriented circle to another (right).
b
(a) C restrict A
e
(b) E restricts C
c
In Figure 6(c), the underlying circle A eventually touches another obstacle and cannot be twisted further. At this point we say that the turn on A is anchored. The result of twisting C to extremity is that it becomes chained to an anchored turn by a sequence of coupled transitions.
E
a
e
C
b
d
A
a
C
a
b
b A
c
B
B
A
(a) B restricts C
b B
e
(c) d anchors C c
Figure 7: As we counterclockwise ants prohibiting requiring every π come into play.
(b) A restricts B
a
E
a
C
d
twist the underlying circle A in a manner, the normalization invariadditional underlying circles and length subarc to touch an obstacle
C
chained to an anchored segment by a sequence of coupled turns. This holds in general (a full version of this paper will include comprehensive description of twisting, including the case where turns are supported by an environment edge rather than a vertex). If we interpret the initial and final configurations as anchors this iterative twisting process results in a B-K normal path in which (i) every turn (Csegment) is supported by some environment feature; (ii) every turn subsegment of length π is supported by some environment feature; and (iii) every turn is anchored or chained to an anchored turn by a sequence of coupled turns. The completeness of our normal form is expressed in the following:
c
A
(c) A becomes anchored Figure 6: As we twist the underlying circle C in a counterclockwise direction, underlying circles restrict each other because we keep the path F-W normal, which forces us to move the underlying circles simultaneously to make further progress. Figure 7 demonstrates two other kinds of coupling (and another notion of anchoring) that can arise in the course of twisting. In Figure 7(a), we twist A counterclockwise about a until the S-segment from A to C touches the environment at b. At this point, A is coupled to C through b because our normalization invariants prohibit bending around b. So to continue twisting A, we must use b as a fulcrum. Figure 7(b) shows our simultaneous twisting of A and C, where C is perturbed as little as possible. This stops once the unsupported turn on C following the contact at c has length π (and C is thereby coupled to E). To twist A further, we must simultaneously move C and E. Figure 7(c) shows the result of this motion, where C and E are twisted as little as possible. When the S-segment from C to E touches the environment at d we say that the turn on C is anchored, and A is chained to C by a sequence of coupled transitions. In both of the scenarios just described, twisting an underlying circle to extremity preserves path complexity and increases the number of turns that are either anchored or
Theorem 1. If there exists an obstacle-free Dubins path of complexity k connecting two configurations (specified by position and direction) in a polygonal environment of size n, then there exists a obstacle-free path in B-K normal form, of complexity at most nk, connecting these same configurations. The proof of this theorem builds on the corresponding theorem for F-W normal form and, like that result involves an exhaustive case analysis of scenarios (like those illustrated earlier) that arise in the process of normalization.
4.
ALGORITHM
In this section, we describe an algorithm that constructs an obstacle-avoiding Dubins path of complexity at most O(nk), joining two specified configurations, if such a path exists, in time that is polynomial in k, n, and m. By Theorem 1 it suffices to construct a path in B-K normal form.
69
(a) configuration representation
Linkage(A, A , ) 1 for all environment features x do for all i ≤ do find all configurations C at x that can be reached from anchor A by a sequence of i coupled turns 2 for all environment features x do for all i ≤ do find all configurations C at x that can be reached from anchor A by a sequence of i coupled turns 3 for all pairs of environment features x, x do for all i ≤ do determine if there exist configurations C and C at x and x respectively such that: (i) C (resp. C ) can be reached from anchor A (resp. A ) by a sequence of i (resp. − i) coupled turns, and (ii) the straight edge joining C and C avoids all obstacles.
(b) unique F-W transition
Figure 8: We can also represent paths by their sequence configurations at obstacle contacts (left) because only one F-W transition between configurations has C-segments where every π of subarc contacts an obstacle (right).
So far we have represented F-W paths by their sequence of underlying circles. An alternative representation is by sequence of configurations where each underlying circle touches an environment feature (see Figure 8(a)). Although this representation explicitly discards underlying circles, the underlying circles are easily recovered; out of the four possible transitions using underlying circles tangent to two configurations, only one transition satisfies the condition that every π length subarc touches an obstacle (see Figure 8(b)). Moreover, the associated complexities differ by at most a constant factor; that is, a path represented by underlying circles is represented by at most configurations; and a path represented by configurations is represented by at most 2 underlying circles. The representation by configurations simplifies our discussion of filtering in Section 5, so we adopt it and its associated notion of complexity now. First, we note that any path in B-K normal form (where the configurations between which we are finding a path are viewed as anchored turns) can be interpreted as the concatenation of one or more (internally) anchor-free subpaths joining anchored turns. Such subpaths consist of a sequence of coupled turns all chained to the initial anchor, followed by a sequence of coupled turns all chained to the final anchor. Accordingly, we refer to such a subpath joining anchor A to anchor A as an (A, A )-linkage. By our definition of anchors, there are O(n3 ) possible anchored turns in total. Thus, it suffices to determine, for every pair of anchors A and A , and for every < O(nk), if there exists a (A, A )linkage of complexity . Figure 9 outlines a way finding such (A, A )-linkages. Since checking whether a fixed S-segment avoids all obstacles can be done in time linear in the number of environment features, it suffices to provide an efficient implementation of step 1 (equivalently, step 2). The simplest approach is to exploit the recursive nature of coupled sequences; specifically, a configuration C at feature x can be reached from anchor A by a sequence of i coupled turns if and only if there exists a feature y and a configuration B at y such that (i) B can be reached from anchor A by a sequence of i − 1 coupled turns and (ii) the turn through configuration B is coupled to the turn through configuration C. Thus, we can implement step 1 by a sequence of i configuration-propagation phases, where in the i-th phase all configurations from the (i − 1)-th phase are propagated to configurations at features by a coupled turn. There are O(n) coupled turns from one configuration. So, the number of configurations that can be reached from a fixed anchor by a sequence of i coupled turns could grow
Figure 9: Algorithm outline that finds a (A, A )linkage of length at most . exponentially with i (see Figure 10). Hence, this explicit generation of configurations may not be realized in polynomial time. Since it suffices to discover only a single (A, A )-linkage of low complexity, we only need to propagate a subset of reachable configurations at each feature in each phase provided that subset is guaranteed (somehow) to capture at least one (A, A )-linkage of low complexity.
Figure 10: One configuration (left) leads to five distinct configurations at a corner after four propagations (right). If this were repeated for another four propagations amid similar obstacles, it would branch to twenty-five distinct states at a corner.
Let S be a set of configurations at feature x that can be reached from an anchor A by a sequence of i coupled turns. Then S ⊆ S is a representative subset if, whenever there exists a B-K normal path of some complexity from some configuration C ∈ S to some anchor, there exists a B-K normal path of complexity at most from some configuration C ∈ S to the same anchor. We will argue that given S, we can find a representative set of size at most s. By interleaving propagation phases with thinning phases, in which the configurations S at each feature are reduced to a representative set of size at most s, we prevent the exponential blow up of configurations. The resulting algorithm provides an adequate implementation of step 1 of the (A, A )-linkage procedure that runs in time polynomial in n, and s.
70
5. PATH REDUNDANCY Our goal in this section is to efficient construct polynomialsize representative sets. As before, let S be any set of configurations at environment feature x that can be reached from anchor A by a sequence of i coupled turns. We will derive (in time polynomially bounded in |S|, n, m, and i) a representative set for S whose size is polynomially bounded in n, m and i. When we propagate a configuration C ∈ S, we try to reach a feature y by a coupled turn of type α, for every y and α. Our construction depends on x, y, and α, so we find ⊆ S such that whenever there exists a B-K normal path Sy,α of complexity from C ∈ S to some anchor whose first turn is α-coupled to a turn through y, there exists a B-K normal path of complexity at most from some configuration C in to the same anchor. There are O(n) choices of y and Sy,α . O(n) choices of α, so it suffices to let S = ∪y,α Sy,α In Figures 11(a), 11(b), and 11(c), one configuration at x subsumes another configuration at x with respect to a configuration at y: Configuration C reaches D by two coupled turns, whereas configuration C reaches D by two uncoupled turns. Any configuration that can be reached by a B-K normal path of complexity from C immediately through D can be reached by a F-W normal form path of complexity from C .
C
C
Cl C x
y
Cr
(b) Cl and Cr cover C
Figure 12: We consider the entire 2-reachable range at y from a configuration C at x (left). If the 2reachable ranges of two configurations (Cl and Cr ) cover a third (C), the third is unnecessary (right).
(see Figure 13(a)). Then, by reversing the transitions from Cl and Cr to D, we see that the reflections of Cl and Cr in x must be in the range of D . We say that the reflections of the range of D define a bin of configurations at x. Let B be a bin. In the absence of obstacles, the ranges of the two most extreme configurations Cl , Cr˘ ∈ S ∩ B cover¯the range of every C ∈ S ∩ B. Let Δ = D1 , D2 , . . . , Dj be a set of configurations at y such that the union of their associated bins covers S. Then we can construct S by taking the two extreme configurations of S in each bin associated with Δ. Figure 13(b) illustrates the bins covering configurations at x pointing to the right; a similar construction covers configurations pointing to the left.
x
y
D C
y
(a) range at y
y
x
C
x
C
D
(a) corner to corner
(b) wall to corner
D2
D x
C
x
y
x
D
Cl
C
Cr
D1
y D3
y
D4
D5
D
(a) bin given by D
(c) different underlying circle at y
(b) bins covering x
Figure 13: In the absence of obstacles, the ranges of configurations in a bin include the reflection D of the configuration defining the bin D (left). To eliminate redundancy, we first cover the configurations at x with bins (right).
Figure 11: The coupled turns from C to D are subsumed by the uncoupled turns from C to D. uses the expressiveness of a sinOur construction of Sy,α gle configuration C in S. That is, we consider the entire range of configurations at y reached from C using turns at x and y (see Figure 12(a)). In the absence of obstacles, the transitions generating a range sweep a simply connected region bounded by coupled transitions. Under what circumstances can a configuration C be culled from S? Suppose that there are configurations Cl , Cr ∈ S such that their ranges at y cover the range of C at y (see Figure 12(b)). Then any B-K normal path from C immediately through y with complexity at most can be replaced by FW normal path from either Cl or Cr immediately through y with complexity at most . When no obstacles interfere, the condition that the ranges of Cl and Cr cover the range of C at y implies that the ranges of Cl and Cr overlap. Let D be a common configuration in the ranges of Cl and Cr and D be the reflection of D in y
So far, bins only eliminate redundancy in the absence of obstacles. To deal with the presence of obstacles, we use two important properties of α-couplings: they are monotonic and non-crossing (we handle exceptions in a full version of this paper, which causes us to find paths twice as complex as optimal). To guarantee that the subsumption within a bin is feasible, we split our bins so that α-couplings from configurations in the same bin have the same homotopy. The monotonicity and non-crossing properties of αcouplings imply that there are O(n) homotopy classes of α-couplings between x and y (see Figure 14(a)). Hence, if our bins are chosen to have minimal overlap, splitting of bins by homotopy creates at most a O(n) additional bins. Knowing that the two α-transitions from the extreme bin
71
a x that can reach y. In this subsection, we describe how to generate a bin cover from corner x to corner y when the distance between x and y is less than two; a similar approach holds for when the distance is greater than two. When generating the set Δ = {D1 , D2 , D3 , . . .} of covering configurations, we lower bound the size of the bin associated with each configuration Dj ∈ Δ and, therefore, overestimate the number of bins needed (see Figure 16(a)). We use this weaker bound because it relates directly to NewtonRaphson root finding and becomes tight as the distance from x to y goes to zero.
configurations Cl ,Cr are in the same homotopy class implies that the region between the α-couplings is obstacle free (see Figure 14(b)). Moreover, we can show that the subsumption of C by either Cl or Cr is in this obstacle-free region. Cl x
C Cr
y
x
(a) homotopy classes
y
(b) feasible tion
subsump-
Figure 14: Consider couplings where underlying circles are tangent. The range of such couplings from x to y is divided into three homotopy classes by obstacles (left). If Cl , C, Cr are in the same bin, they are in the same homotopy class and the subsumption is feasible (right).
Cj+1
Dj+1
Cj
Dj
x
y
(a) bin underestimate Figure 15 shows how to construct a representative set for wall features. The coupling involved occurs when the two underlying circles are tangent. Two bins cover the entire range of configurations at x that can reach y (see Figure 15(a); the other bin is symmetric). We break these bins by the homotopy classes of the particular constraint that we are considering (see Figure 15(b)). The properties of the coupling guarantee that the non-extreme configurations within each bin are redundant (see Figure 15(c)).
D
(a) relevant bin
x
x
y
y
L
H y
x T
d∗ cj+1 cj
dj+1 dj
(b) underestimate construction Figure 16: We use an underestimate of the bin size (top) because it has a simple geometric construction (bottom), which can be interpreted as a NewtonRaphson iteration.
(b) bin split by homotopy x
Figure 16(b) shows the geometric construction of the underestimate: Each point e is the center of the underlying circle tangent to a configuration E; each such center has a reflection about the line L of points equidistant to x and y; the line T is tangent to the unit circle H about y at dj ; and the point dj+1 is the projection of the intersection of T and L onto H. The mapping of dj to dj+1 is a single Newton-Raphson iteration converging to d∗ , which allows us to modify the textbook analysis of convergence. Let the error ξj be the vertical difference between dj and d∗ . Our analysis of ξj examines the Taylor expansion of the fixed-point function at d∗ . We bootstrap the iterative process by starting at an appropriate d0 , which is possible because the distance between corners is non-zero. Then we use ad hoc arguments to prove ξj = 2−Ω(j) (i.e. linear conΩ(j) vergence) with just O(1) iterations and ξi = 2−2 (i.e. quadratic convergence) with O(m) iterations. No finite number of bins covers the entire range of configurations at x. However, a separation bound (in the next section) between the fixed point and configurations resulting
y
(c) non-extreme are redundant Figure 15: A bin (black) defined by D covers a large range of contact at x (top left). This is divided into two by homotopy classes of a type of coupling (top right). Within the left bin non-extreme configurations are redundant (bottom). Since each bin contributes at most two configurations to , it suffices to argue that the number of bins needed to Sy,α cover S is polynomial in n, m, and i.
5.1 Fixed-point Cover Generation As illustrated by Figure 15, when either feature x or y is a wall, a small number of bins covers the configurations
72
from a sequence of i couplings from an anchor implies that the range of all configurations at x (and hence all of S) is covered with O (i + m) bins.
the notion of intrinsic complexity still holds. This begs the interesting open problem of finding such paths amid polygonal obstacles in time polynomial in the obstacle and intrinsic path complexity.
5.2 Separation Bound The constraints between coupled underlying circles can be expressed geometrically with ruler and compass constructions. These constructions can then be converted into expressions relating the co-ordinates of underlying circles. Given an arithmetic expression composed of +, −, ×, /, √ and integers, Burnikel et al. describe how to construct a quotient of irreducible polynomials with integer coefficients such that the result of the arithmetic expression corresponds to a root of the quotient [5]. The magnitude of a non-zero root is bounded above and below by the degrees and coefficients of both polynomials. Rather than compute the polynomials, they recursively compute the corresponding root bounds directly from the expression. Burnikel et al.’s technique applied to the ruler and compass constructions of couplings bounds the difference between underlying circle centers on B-K normal paths, with the weak assumption that real values are approximated as a quotient of integers, where each integer has absolute value ≤ 2m .
7.
REFERENCES
[1] Pankaj K. Agarwal, Therese Biedl, Sylvain Lazard, Steve Robbins, Subhash Suri, and Sue Whitesides. Curvature-constrained shortest paths in a convex polygon (extended abstract). In SCG ’98: Proceedings of the fourteenth annual symposium on Computational geometry, pages 392–401, New York, NY, USA, 1998. ACM Press. [2] Pankaj K. Agarwal, Prabhakar Raghavan, and Hisao Tamaki. Motion planning for a steering-constrained robot through moderate obstacles. In STOC ’95: Proceedings of the twenty-seventh annual ACM symposium on Theory of computing, pages 343–352, New York, NY, USA, 1995. ACM Press. [3] Sergey Bereg and David Kirkpatrick. Curvature-bounded traversals of narrow corridors. In SCG ’05: Proceedings of the twenty-first annual symposium on Computational geometry, pages 278–287, New York, NY, USA, 2005. ACM Press. [4] Jean-Daniel Boissonnat and Sylvain Lazard. A polynomial-time algorithm for computing a shortest path of bounded curvature amidst moderate obstacles (extended abstract). In SCG ’96: Proceedings of the twelfth annual symposium on Computational geometry, pages 242–251, New York, NY, USA, 1996. ACM Press. [5] Christoph Burnikel, Stefan Funke, Kurt Mehlhorn, Stefan Schirra, and Susanne Schmitt. A separation bound for real algebraic expressions. In Lecutre Notes in Computer Science: European Symposium on Algorithms 2001, volume 2161, pages 254–. Springer-Verlag, January 2001. [6] L. E. Dubins. On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. Amer. J. Math., 79:497–516, 1957. [7] Steven Fortune and Gordon Wilfong. Planning constrained motion. Ann. Math. Artificial Intelligence, 3(1):21–82, 1991. Algorithmic motion planning in robotics. [8] Paul Jacobs and John Canny. Planning smooth paths for mobile robots. In Proceedings of the 1989 IEEE International Conference on Robotics and Automation, pages 2–7, 1989. [9] J. A. Reeds and L. A. Shepp. Optimal paths for a car that goes both forwards and backwards. Pacific J. Math., 145(2):367–393, 1990. [10] John Reif and Hongyan Wang. The complexity of the two dimensional curvature-constrained shortest-path problem. In WAFR ’98: Proceedings of the third workshop on the algorithmic foundations of robotics on Robotics : the algorithmic perspective, pages 49–57, Natick, MA, USA, 1998. A. K. Peters, Ltd. [11] Hongyan Wang and Pankaj K. Agarwal. Approximation algorithms for curvature-constrained shortest paths. In SODA ’96: Proceedings of the seventh annual ACM-SIAM symposium on Discrete algorithms, pages 409–418, Philadelphia, PA, USA, 1996. Society for Industrial and Applied Mathematics.
Lemma 1. Let p and q be B-K normal paths of complexity at most k. Then, the centers of distinct underlying circles realized by p and q at any environment feature are at least k 2−O(c m) apart for some constant c, where m is a measure of the input precision.
6. DISCUSSION Earlier, we mentioned the open problem of Bereg and Kirkpatrick [3]: Is there a polynomial-time algorithm for determining whether a given corridor with width less than τ is traversable? With slight modifications, our algorithm answers this question in the affirmative. The algorithm is polynomial time because the intrinsic complexity of a path (i.e. number of underlying circles required) in such a corridor is bounded by the corridor complexity. Specifically, any F-W path touches each corner at most once and need not touch a wall more than twice in each direction. One discrepancy between corridors and polygonal obstacles is that the boundary of a corridor is made of arcs as well as straight line segments. If the corridor width is less than one, then the curvature of the curved walls is too small and any Dubins path cannot touch such a wall. Otherwise, the curvature of the curved walls are greater than one and any such wall can be followed once it is contacted. We believe that our ideas can be used to construct a complete approximation algorithm. The first step, normalizing a shortest path by twisting, is complete. To prevent substantial stretching of the path, we can augment the set of anchored circles with regular samples at each environment feature (i.e. sample like the current approximation algorithms). Unfortunately, there are more types of couplings to consider and path arcs longer than π may not have environment contacts. Both of these violate the assumptions of our current binning argument. An obvious relaxation of the motion planning problem is to allow the vehicle to reverse. Reeds and Shepp have analyzed the shortest such paths in the absence of obstacles [9]. These paths are composed of C-segments and S-segments, so
73
David Kirkpatrick
Department of Computer Science University of British Columbia 201-2366 Main Mall, Vancouver BC, V6T 1Z4, Canada
Department of Computer Science University of British Columbia 201-2366 Main Mall, Vancouver BC, V6T 1Z4, Canada
[email protected]
[email protected]
ABSTRACT
point on the vehicle (say, the mid-point of its rear axle), we prohibit reversal, and we permit arbitrary changes in motion curvature as long as its magnitude remains bounded (in effect, the steering wheel has a restriction on how far, but not how fast, it can turn). Specifically, we insist that the path is continuous and that |Δφ/Δl| < r−1 between any two points on the path, where Δφ is the change in forward direction measured in radians, Δl is the distance travelled between the points, and r the radius of curvature. Intuitively, this restriction means that every point on the path can be locally sandwiched between tangent circles of radius r (see Figure 1(b)). By suitable scaling, we can assume that r = 1. So, without loss of generality, we restrict our attention to paths of unit-curvature.
We describe an algorithm to find a unit-curvature path between specified configurations in an arbitrary polygonal domain. Whenever such a path exists, the algorithm returns an explicit description of one such path in time that is polynomial in n (the number of features of the domain), m (the precision of the input) and k (the number of segments on the simplest obstacle-free Dubins path connecting the specified configurations). Our algorithm is based on a new normal form for unit-curvature paths and a dynamic path filtering argument that exploits a separation bound for distinct paths in this normal form. The best result known for the feasibility of bounded-curvature motion in the presence of arbitrary polygonal obstacles involves a reduction to the first-order theory of the reals. It just determines if a feasible path exists (it does not return a path) and requires exponential time and space.
Categories and Subject Descriptors F.2.2 [Nonnumerical Algorithms and Problems]: Geometrical problems and computations
r
General Terms Algorithms, Theory
(a) amid obstacles
Keywords
(b) sandwiched
Figure 1: A bounded-curvature path amid obstacles (left) and an illustration of a bounded-curvature path wedged between constant-radius circles (right).
bounded-curvature, motion planning, path feasibility
1. PROBLEM STATEMENT We assume that the free space is specified by an environment with a polygonal boundary. We refer to the vertices and edges that describe this boundary as environment features, and we denote by n the total number of environment features and m the maximum input bit precision specifying any feature (defined in Section 5.2). An (instantaneous) configuration of the vehicle is specified by its position and direction of motion. Given an initial and final configuration, we want to determine if there exists a unit-curvature path within the free space that connects these two configurations. Furthermore, if such a path exists, we want to provide an explicit description of one such path. The path existence question is decidable: Fortune and Wilfong present a decision procedure whose time and space
We are interested in planning the collision-free motion of vehicles with a restricted turning radius in the presence of arbitrary polygonal obstacles (see Figure 1(a)). As in the bulk of previous work, we consider paths traced by a fixed
Permission to make digital or hard copies of all or part of this work for personal or classroom use is granted without fee provided that copies are not made or distributed for profit or commercial advantage and that copies bear this notice and the full citation on the first page. To copy otherwise, to republish, to post on servers or to redistribute to lists, requires prior specific permission and/or a fee. SCG’07, June 6–8, 2007, Gyeongju, South Korea. Copyright 2007 ACM 978-1-59593-705-6/07/0006 ...$5.00.
66
O(1)
complexity is exponential (2(n+m) ) [7]. However, their reduction does not provide an explicit description of a path, when one exists. Indeed, it is not clear if this is even possible in exponential time, since we do not have an upper bound on the complexity of the simplest feasible path, as a function of n and m. Finding a minimum-length bounded-curvature path in the presence of arbitrary polygonal obstacles is NP-hard [10]. Although efficient approximation algorithms exist [8, 11], they are incomplete: They only guarantee to find an approximation of the shortest bounded-curvature path that satisfies a robustness criterion. Thus, they may terminate without producing any path, if all feasible paths are nonrobust. The existence of even an exponential-time complete approximation algorithm is uncertain for the same reason that we described above. In this paper, we describe an algorithm to find a unitcurvature path between specified configurations in an arbitrary polygonal environment. Whenever such a path exists, the algorithm returns an explicit description of one such path in time that is polynomial in n, m, and k (the number of segments on the simplest obstacle-free Dubins path connecting the specified configurations). The parameter k provides a natural notion of intrinsic complexity of the desired output. Our algorithm is based on a new normal form for unit-curvature paths and a dynamic path filtering technique that exploits a separation bound for distinct normal paths. We start with a brief overview of related bounded-curvature results. Then we describe a new normal form for unitcurvature paths that restricts our attention to paths with a combinatorial description (see Section 3). This reduces determining path existence to searching a finite (but potentially exponential) space. Section 4 describes our algorithm. It associates with every feature a set of configurations that serve as an adequate set of potential checkpoints in the construction of a path. The total size of the these configuration sets is a dominant factor in the complexity of our algorithm. Section 5 shows how we obtain a polynomial upper bound on the size of individual configuration sets. To do this, we exploit a dynamic redundancy-elimination (filtering) argument together with a bound on the minimum separation of complexity-bounded paths in the presence of precision-restricted obstacles. Finally, Section 6 describes some applications and potential extensions of our results.
that the structure of a shortest path (the sequence of the types of segments of which it is made) must be either CSC or CCC, where each segment may have zero length. Wang and Agarwal argue that amid polygonal obstacles, the shortest bounded-curvature path is a sequence of these Dubins paths between obstacle contacts [11]. Any such path is said to be in Dubins normal form, and has complexity equal to its number of C-segments.
Figure 2: Short (≤ π) unit-curvature paths are sandwiched between two unit circles tangent to the initial configuration (dotted) and two unit circles tangent to the final configuration (dashed). The shortest such path between those configurations follows the bounding circles.
Fortune and Wilfong [7] describe a more restrictive normal form in which every C-segment is supported by (touches, but does not intersect) an obstacle feature. Although unstated by Fortune and Wilfong, their normalization can ensure that every π length subarc of a C-segment is supported. We refer to this strengthened version as F-W normal. The search for F-W normal paths can be viewed as the identification of configurations at environment features that, together, form an adequate set of checkpoints for the realization of unit-curvature paths. Fortune and Wilfong show how these configuration sets, though uncountable in size, can be compactly expressed in the first-order theory of the reals. This leads to an decision procedure for feasibility that is exponential in time and space. In Section 3, we introduce a restriction of F-W normal form paths that forms the basis of our algorithm.
2.2
Shortest Path Approximation
The NP-hardness of finding shortest paths amid polygonal obstacles [10] motivates two efficient approximation algorithms [8, 11]. Both algorithms explicitly construct a weighted visibility graph by uniformly sampling potential obstacle contacts with a granularity proportional to the desired approximation ratio. The shortest path in the resulting visibility graph corresponds to a (1 + ) approximation to the shortest (/2)-robust path, where robustness is a measure of how much a bounded-curvature path can be perturbed at its obstacle contacts while retaining its structure. As we noted earlier, these approximation algorithms are incomplete; specifically, the only feasible path may be 0robust, so these approximation algorithms may never find a feasible path regardless of the input . We note that, the complexity of the paths found by these algorithms polynomial in the input, which is consistent with our use of intrinsic complexity in runtime analysis. In particular, the worst case number of C-segments on any returned path is at most three times the number of edges in the visibility graph because every edge is traversed at most once and each edge corresponds to a Dubins’ path with structure CSC or CCC. Our algorithm non-uniformly samples potential obstacle
2. BACKGROUND AND RELATED WORK 2.1 Feasibility Testing via Path Normalization The identification of a normal form for paths often lies at the heart of path planning. Even the goal of constructing shortest paths can be viewed as a useful restriction of the search space (in addition to being a well-motivated optimization criterion). In the absence of obstacles, Dubins shows how to shrink suboptimal paths thereby demonstrating that shortest bounded-curvature paths either require or forbid certain properties [6]. These arguments implicitly describe a normalization process that iteratively shrinks a path until it is shortest. Dubins first notices that shortest unit-curvature paths are made of arcs of unit radius circles (C-segments) and straight line segments (S-segments), because short subpaths have this property (see Figure 2). Then Dubins shows
67
contacts to construct a visibility graph while it is explored. This real time construction is history dependent, which allows it to account for global obstacle geometry. However, the approximation algorithms just independently consider small subsets of features. Another difference is that every path that we find is 0-robust at every obstacle contact, whereas the approximation algorithms may fail to find such paths.
consequence, our normal form paths are described by a finite sequence of discrete path constraints. This combinatorial description implies that the set of all normal paths between the same initial and final configurations is countable and systematically searchable. We begin with a Dubins normal path, which is a finite sequence of C-segments and S-segments (see Figure 4(a)). First, we apply the normalization procedure of Fortune and Wilfong, effectively pushing all C-segments into contact with the environment (see Figures 4(b) and 4(c)). As noted by Fortune and Wilfong, the resulting path is at most a factor of n, the number of environment features, larger than the original path. Next, we iteratively twist each C-segment (while maintaining its environment contact) into an extreme position (see Figure 4(d)). In its extreme position, each Csegment is either anchored (doubly constrained by the local environment) or chained to an anchored C-segment by a sequence of couplings (singly constrained transitions).
2.3 Path planning in Restricted Domains Agarwal et al. characterize shortest paths amid simply connected, convex obstacles with bounded-curvature boundaries (see Figure 3(a)). This domain restriction limits the placement of C-segments and admits an efficient, complete approximation algorithm [2]. Boissonnat and Lazard refine this into an exact algorithm by eliminating a degree of freedom in the C-segment placement in one difficult case [4]. As the obstacles are convex and their boundaries can be traced by a bounded-curvature path, no obstacle boundary on a shortest path is touched more than twice in each direction. Thus, the number of environment features bounds the complexity of the shortest path.
(a) moderate obstacles
(a) initial Dubins path
(b) pop out turns
(c) pull in turns
(d) twist turns
(b) corridor Figure 4: Different phases of path normalization.
Figure 3: Unit-curvature paths are found efficiently in these restricted domains. The obstacle-free space is white and includes the black boundary.
Every Dubins path can be described by the sequence of oriented unit circles underlying each C-segment (see Figure 5(a)) because there is a unique (possibly degenerate) Ssegment from one oriented circle to another (see Figure 5(b)). Forcing C-segments to touch the environment restricts their underlying circle centres to a 1-D manifold. However, a record of each underlying circle’s orientation and supporting environment feature incompletely specifies the path because there remains a degree of freedom at each environment contact, which allows a continuous range of paths to match one such description (see Figure 4(d)). To refine the specification, we twist each underlying circle and augment the description with how each circle was twisted. When we twist, we push an underlying circle as far as possible in a given direction while maintaining three invariants: we keep the path F-W normal, we do not add any new underlying circles, and we preserve the obstacle support and orientation of each underlying circle. Figure 6 illustrates the consequences of these invariants as we twist an underlying circle C in a counterclockwise manner about its environment contact c. In Figure 6(a), we pivot C counterclockwise at c until it kisses the previous underlying circle B. At this point, C is said to be coupled to B because if B remains stationary, we cannot twist C further. So in Figure 6(b), we continue by simultaneously twisting C and B, maintaining the coupling throughout. We do this until any further simultaneous twisting eliminates the environment support (at a) of the path segment on circle A. This causes a second form of coupling between A and B that is maintained by simultaneously twisting A, B, and C.
Agarwal et al. analyze the structure of shortest boundedcurvature paths inside convex polygons to limit the placement of C-segments and get an efficient exact algorithm [1]. In this domain, the complexity of the shortest path is O(1). Bereg and Kirkpatrick examine feasible, but not necessarily shortest, bounded-curvature paths in corridors (regions swept out by a disk as its center moves along a polygonal path — see Figure 3(b)) [3]. Their objective is to make explicit a minimum corridor width, τ ≈ 1.505, that guarantees unit-curvature traversability, and to describe an efficient greedy algorithm to construct an unit-curvature traversal in any corridor of width at least τ . The complexity of the paths found by their algorithm is bounded by the complexity of the corridor. Bereg and Kirkpatrick leave open the question of whether a polynomial time algorithm exists for determining whether a given corridor with width less than τ is traversable. As we argue in Section 6, our algorithm answers this in the affirmative.
3. A NEW NORMAL FORM A F-W normal path, like a plumber’s snake that has been threaded through a sequence of coplanar pipes, has every bend constrained by the environment, but retains a degree of freedom at some of its environment contacts that permits lateral twisting motions. Our new normal form removes this degree of freedom from all environment contacts. As a
68
c
c
C
C b
(a) circle representation
A
(b) unique transition
d A
E
a
Figure 5: We can represent F-W paths by their underlying circles (left) because there is one F-W transition from one oriented circle to another (right).
b
(a) C restrict A
e
(b) E restricts C
c
In Figure 6(c), the underlying circle A eventually touches another obstacle and cannot be twisted further. At this point we say that the turn on A is anchored. The result of twisting C to extremity is that it becomes chained to an anchored turn by a sequence of coupled transitions.
E
a
e
C
b
d
A
a
C
a
b
b A
c
B
B
A
(a) B restricts C
b B
e
(c) d anchors C c
Figure 7: As we counterclockwise ants prohibiting requiring every π come into play.
(b) A restricts B
a
E
a
C
d
twist the underlying circle A in a manner, the normalization invariadditional underlying circles and length subarc to touch an obstacle
C
chained to an anchored segment by a sequence of coupled turns. This holds in general (a full version of this paper will include comprehensive description of twisting, including the case where turns are supported by an environment edge rather than a vertex). If we interpret the initial and final configurations as anchors this iterative twisting process results in a B-K normal path in which (i) every turn (Csegment) is supported by some environment feature; (ii) every turn subsegment of length π is supported by some environment feature; and (iii) every turn is anchored or chained to an anchored turn by a sequence of coupled turns. The completeness of our normal form is expressed in the following:
c
A
(c) A becomes anchored Figure 6: As we twist the underlying circle C in a counterclockwise direction, underlying circles restrict each other because we keep the path F-W normal, which forces us to move the underlying circles simultaneously to make further progress. Figure 7 demonstrates two other kinds of coupling (and another notion of anchoring) that can arise in the course of twisting. In Figure 7(a), we twist A counterclockwise about a until the S-segment from A to C touches the environment at b. At this point, A is coupled to C through b because our normalization invariants prohibit bending around b. So to continue twisting A, we must use b as a fulcrum. Figure 7(b) shows our simultaneous twisting of A and C, where C is perturbed as little as possible. This stops once the unsupported turn on C following the contact at c has length π (and C is thereby coupled to E). To twist A further, we must simultaneously move C and E. Figure 7(c) shows the result of this motion, where C and E are twisted as little as possible. When the S-segment from C to E touches the environment at d we say that the turn on C is anchored, and A is chained to C by a sequence of coupled transitions. In both of the scenarios just described, twisting an underlying circle to extremity preserves path complexity and increases the number of turns that are either anchored or
Theorem 1. If there exists an obstacle-free Dubins path of complexity k connecting two configurations (specified by position and direction) in a polygonal environment of size n, then there exists a obstacle-free path in B-K normal form, of complexity at most nk, connecting these same configurations. The proof of this theorem builds on the corresponding theorem for F-W normal form and, like that result involves an exhaustive case analysis of scenarios (like those illustrated earlier) that arise in the process of normalization.
4.
ALGORITHM
In this section, we describe an algorithm that constructs an obstacle-avoiding Dubins path of complexity at most O(nk), joining two specified configurations, if such a path exists, in time that is polynomial in k, n, and m. By Theorem 1 it suffices to construct a path in B-K normal form.
69
(a) configuration representation
Linkage(A, A , ) 1 for all environment features x do for all i ≤ do find all configurations C at x that can be reached from anchor A by a sequence of i coupled turns 2 for all environment features x do for all i ≤ do find all configurations C at x that can be reached from anchor A by a sequence of i coupled turns 3 for all pairs of environment features x, x do for all i ≤ do determine if there exist configurations C and C at x and x respectively such that: (i) C (resp. C ) can be reached from anchor A (resp. A ) by a sequence of i (resp. − i) coupled turns, and (ii) the straight edge joining C and C avoids all obstacles.
(b) unique F-W transition
Figure 8: We can also represent paths by their sequence configurations at obstacle contacts (left) because only one F-W transition between configurations has C-segments where every π of subarc contacts an obstacle (right).
So far we have represented F-W paths by their sequence of underlying circles. An alternative representation is by sequence of configurations where each underlying circle touches an environment feature (see Figure 8(a)). Although this representation explicitly discards underlying circles, the underlying circles are easily recovered; out of the four possible transitions using underlying circles tangent to two configurations, only one transition satisfies the condition that every π length subarc touches an obstacle (see Figure 8(b)). Moreover, the associated complexities differ by at most a constant factor; that is, a path represented by underlying circles is represented by at most configurations; and a path represented by configurations is represented by at most 2 underlying circles. The representation by configurations simplifies our discussion of filtering in Section 5, so we adopt it and its associated notion of complexity now. First, we note that any path in B-K normal form (where the configurations between which we are finding a path are viewed as anchored turns) can be interpreted as the concatenation of one or more (internally) anchor-free subpaths joining anchored turns. Such subpaths consist of a sequence of coupled turns all chained to the initial anchor, followed by a sequence of coupled turns all chained to the final anchor. Accordingly, we refer to such a subpath joining anchor A to anchor A as an (A, A )-linkage. By our definition of anchors, there are O(n3 ) possible anchored turns in total. Thus, it suffices to determine, for every pair of anchors A and A , and for every < O(nk), if there exists a (A, A )linkage of complexity . Figure 9 outlines a way finding such (A, A )-linkages. Since checking whether a fixed S-segment avoids all obstacles can be done in time linear in the number of environment features, it suffices to provide an efficient implementation of step 1 (equivalently, step 2). The simplest approach is to exploit the recursive nature of coupled sequences; specifically, a configuration C at feature x can be reached from anchor A by a sequence of i coupled turns if and only if there exists a feature y and a configuration B at y such that (i) B can be reached from anchor A by a sequence of i − 1 coupled turns and (ii) the turn through configuration B is coupled to the turn through configuration C. Thus, we can implement step 1 by a sequence of i configuration-propagation phases, where in the i-th phase all configurations from the (i − 1)-th phase are propagated to configurations at features by a coupled turn. There are O(n) coupled turns from one configuration. So, the number of configurations that can be reached from a fixed anchor by a sequence of i coupled turns could grow
Figure 9: Algorithm outline that finds a (A, A )linkage of length at most . exponentially with i (see Figure 10). Hence, this explicit generation of configurations may not be realized in polynomial time. Since it suffices to discover only a single (A, A )-linkage of low complexity, we only need to propagate a subset of reachable configurations at each feature in each phase provided that subset is guaranteed (somehow) to capture at least one (A, A )-linkage of low complexity.
Figure 10: One configuration (left) leads to five distinct configurations at a corner after four propagations (right). If this were repeated for another four propagations amid similar obstacles, it would branch to twenty-five distinct states at a corner.
Let S be a set of configurations at feature x that can be reached from an anchor A by a sequence of i coupled turns. Then S ⊆ S is a representative subset if, whenever there exists a B-K normal path of some complexity from some configuration C ∈ S to some anchor, there exists a B-K normal path of complexity at most from some configuration C ∈ S to the same anchor. We will argue that given S, we can find a representative set of size at most s. By interleaving propagation phases with thinning phases, in which the configurations S at each feature are reduced to a representative set of size at most s, we prevent the exponential blow up of configurations. The resulting algorithm provides an adequate implementation of step 1 of the (A, A )-linkage procedure that runs in time polynomial in n, and s.
70
5. PATH REDUNDANCY Our goal in this section is to efficient construct polynomialsize representative sets. As before, let S be any set of configurations at environment feature x that can be reached from anchor A by a sequence of i coupled turns. We will derive (in time polynomially bounded in |S|, n, m, and i) a representative set for S whose size is polynomially bounded in n, m and i. When we propagate a configuration C ∈ S, we try to reach a feature y by a coupled turn of type α, for every y and α. Our construction depends on x, y, and α, so we find ⊆ S such that whenever there exists a B-K normal path Sy,α of complexity from C ∈ S to some anchor whose first turn is α-coupled to a turn through y, there exists a B-K normal path of complexity at most from some configuration C in to the same anchor. There are O(n) choices of y and Sy,α . O(n) choices of α, so it suffices to let S = ∪y,α Sy,α In Figures 11(a), 11(b), and 11(c), one configuration at x subsumes another configuration at x with respect to a configuration at y: Configuration C reaches D by two coupled turns, whereas configuration C reaches D by two uncoupled turns. Any configuration that can be reached by a B-K normal path of complexity from C immediately through D can be reached by a F-W normal form path of complexity from C .
C
C
Cl C x
y
Cr
(b) Cl and Cr cover C
Figure 12: We consider the entire 2-reachable range at y from a configuration C at x (left). If the 2reachable ranges of two configurations (Cl and Cr ) cover a third (C), the third is unnecessary (right).
(see Figure 13(a)). Then, by reversing the transitions from Cl and Cr to D, we see that the reflections of Cl and Cr in x must be in the range of D . We say that the reflections of the range of D define a bin of configurations at x. Let B be a bin. In the absence of obstacles, the ranges of the two most extreme configurations Cl , Cr˘ ∈ S ∩ B cover¯the range of every C ∈ S ∩ B. Let Δ = D1 , D2 , . . . , Dj be a set of configurations at y such that the union of their associated bins covers S. Then we can construct S by taking the two extreme configurations of S in each bin associated with Δ. Figure 13(b) illustrates the bins covering configurations at x pointing to the right; a similar construction covers configurations pointing to the left.
x
y
D C
y
(a) range at y
y
x
C
x
C
D
(a) corner to corner
(b) wall to corner
D2
D x
C
x
y
x
D
Cl
C
Cr
D1
y D3
y
D4
D5
D
(a) bin given by D
(c) different underlying circle at y
(b) bins covering x
Figure 13: In the absence of obstacles, the ranges of configurations in a bin include the reflection D of the configuration defining the bin D (left). To eliminate redundancy, we first cover the configurations at x with bins (right).
Figure 11: The coupled turns from C to D are subsumed by the uncoupled turns from C to D. uses the expressiveness of a sinOur construction of Sy,α gle configuration C in S. That is, we consider the entire range of configurations at y reached from C using turns at x and y (see Figure 12(a)). In the absence of obstacles, the transitions generating a range sweep a simply connected region bounded by coupled transitions. Under what circumstances can a configuration C be culled from S? Suppose that there are configurations Cl , Cr ∈ S such that their ranges at y cover the range of C at y (see Figure 12(b)). Then any B-K normal path from C immediately through y with complexity at most can be replaced by FW normal path from either Cl or Cr immediately through y with complexity at most . When no obstacles interfere, the condition that the ranges of Cl and Cr cover the range of C at y implies that the ranges of Cl and Cr overlap. Let D be a common configuration in the ranges of Cl and Cr and D be the reflection of D in y
So far, bins only eliminate redundancy in the absence of obstacles. To deal with the presence of obstacles, we use two important properties of α-couplings: they are monotonic and non-crossing (we handle exceptions in a full version of this paper, which causes us to find paths twice as complex as optimal). To guarantee that the subsumption within a bin is feasible, we split our bins so that α-couplings from configurations in the same bin have the same homotopy. The monotonicity and non-crossing properties of αcouplings imply that there are O(n) homotopy classes of α-couplings between x and y (see Figure 14(a)). Hence, if our bins are chosen to have minimal overlap, splitting of bins by homotopy creates at most a O(n) additional bins. Knowing that the two α-transitions from the extreme bin
71
a x that can reach y. In this subsection, we describe how to generate a bin cover from corner x to corner y when the distance between x and y is less than two; a similar approach holds for when the distance is greater than two. When generating the set Δ = {D1 , D2 , D3 , . . .} of covering configurations, we lower bound the size of the bin associated with each configuration Dj ∈ Δ and, therefore, overestimate the number of bins needed (see Figure 16(a)). We use this weaker bound because it relates directly to NewtonRaphson root finding and becomes tight as the distance from x to y goes to zero.
configurations Cl ,Cr are in the same homotopy class implies that the region between the α-couplings is obstacle free (see Figure 14(b)). Moreover, we can show that the subsumption of C by either Cl or Cr is in this obstacle-free region. Cl x
C Cr
y
x
(a) homotopy classes
y
(b) feasible tion
subsump-
Figure 14: Consider couplings where underlying circles are tangent. The range of such couplings from x to y is divided into three homotopy classes by obstacles (left). If Cl , C, Cr are in the same bin, they are in the same homotopy class and the subsumption is feasible (right).
Cj+1
Dj+1
Cj
Dj
x
y
(a) bin underestimate Figure 15 shows how to construct a representative set for wall features. The coupling involved occurs when the two underlying circles are tangent. Two bins cover the entire range of configurations at x that can reach y (see Figure 15(a); the other bin is symmetric). We break these bins by the homotopy classes of the particular constraint that we are considering (see Figure 15(b)). The properties of the coupling guarantee that the non-extreme configurations within each bin are redundant (see Figure 15(c)).
D
(a) relevant bin
x
x
y
y
L
H y
x T
d∗ cj+1 cj
dj+1 dj
(b) underestimate construction Figure 16: We use an underestimate of the bin size (top) because it has a simple geometric construction (bottom), which can be interpreted as a NewtonRaphson iteration.
(b) bin split by homotopy x
Figure 16(b) shows the geometric construction of the underestimate: Each point e is the center of the underlying circle tangent to a configuration E; each such center has a reflection about the line L of points equidistant to x and y; the line T is tangent to the unit circle H about y at dj ; and the point dj+1 is the projection of the intersection of T and L onto H. The mapping of dj to dj+1 is a single Newton-Raphson iteration converging to d∗ , which allows us to modify the textbook analysis of convergence. Let the error ξj be the vertical difference between dj and d∗ . Our analysis of ξj examines the Taylor expansion of the fixed-point function at d∗ . We bootstrap the iterative process by starting at an appropriate d0 , which is possible because the distance between corners is non-zero. Then we use ad hoc arguments to prove ξj = 2−Ω(j) (i.e. linear conΩ(j) vergence) with just O(1) iterations and ξi = 2−2 (i.e. quadratic convergence) with O(m) iterations. No finite number of bins covers the entire range of configurations at x. However, a separation bound (in the next section) between the fixed point and configurations resulting
y
(c) non-extreme are redundant Figure 15: A bin (black) defined by D covers a large range of contact at x (top left). This is divided into two by homotopy classes of a type of coupling (top right). Within the left bin non-extreme configurations are redundant (bottom). Since each bin contributes at most two configurations to , it suffices to argue that the number of bins needed to Sy,α cover S is polynomial in n, m, and i.
5.1 Fixed-point Cover Generation As illustrated by Figure 15, when either feature x or y is a wall, a small number of bins covers the configurations
72
from a sequence of i couplings from an anchor implies that the range of all configurations at x (and hence all of S) is covered with O (i + m) bins.
the notion of intrinsic complexity still holds. This begs the interesting open problem of finding such paths amid polygonal obstacles in time polynomial in the obstacle and intrinsic path complexity.
5.2 Separation Bound The constraints between coupled underlying circles can be expressed geometrically with ruler and compass constructions. These constructions can then be converted into expressions relating the co-ordinates of underlying circles. Given an arithmetic expression composed of +, −, ×, /, √ and integers, Burnikel et al. describe how to construct a quotient of irreducible polynomials with integer coefficients such that the result of the arithmetic expression corresponds to a root of the quotient [5]. The magnitude of a non-zero root is bounded above and below by the degrees and coefficients of both polynomials. Rather than compute the polynomials, they recursively compute the corresponding root bounds directly from the expression. Burnikel et al.’s technique applied to the ruler and compass constructions of couplings bounds the difference between underlying circle centers on B-K normal paths, with the weak assumption that real values are approximated as a quotient of integers, where each integer has absolute value ≤ 2m .
7.
REFERENCES
[1] Pankaj K. Agarwal, Therese Biedl, Sylvain Lazard, Steve Robbins, Subhash Suri, and Sue Whitesides. Curvature-constrained shortest paths in a convex polygon (extended abstract). In SCG ’98: Proceedings of the fourteenth annual symposium on Computational geometry, pages 392–401, New York, NY, USA, 1998. ACM Press. [2] Pankaj K. Agarwal, Prabhakar Raghavan, and Hisao Tamaki. Motion planning for a steering-constrained robot through moderate obstacles. In STOC ’95: Proceedings of the twenty-seventh annual ACM symposium on Theory of computing, pages 343–352, New York, NY, USA, 1995. ACM Press. [3] Sergey Bereg and David Kirkpatrick. Curvature-bounded traversals of narrow corridors. In SCG ’05: Proceedings of the twenty-first annual symposium on Computational geometry, pages 278–287, New York, NY, USA, 2005. ACM Press. [4] Jean-Daniel Boissonnat and Sylvain Lazard. A polynomial-time algorithm for computing a shortest path of bounded curvature amidst moderate obstacles (extended abstract). In SCG ’96: Proceedings of the twelfth annual symposium on Computational geometry, pages 242–251, New York, NY, USA, 1996. ACM Press. [5] Christoph Burnikel, Stefan Funke, Kurt Mehlhorn, Stefan Schirra, and Susanne Schmitt. A separation bound for real algebraic expressions. In Lecutre Notes in Computer Science: European Symposium on Algorithms 2001, volume 2161, pages 254–. Springer-Verlag, January 2001. [6] L. E. Dubins. On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. Amer. J. Math., 79:497–516, 1957. [7] Steven Fortune and Gordon Wilfong. Planning constrained motion. Ann. Math. Artificial Intelligence, 3(1):21–82, 1991. Algorithmic motion planning in robotics. [8] Paul Jacobs and John Canny. Planning smooth paths for mobile robots. In Proceedings of the 1989 IEEE International Conference on Robotics and Automation, pages 2–7, 1989. [9] J. A. Reeds and L. A. Shepp. Optimal paths for a car that goes both forwards and backwards. Pacific J. Math., 145(2):367–393, 1990. [10] John Reif and Hongyan Wang. The complexity of the two dimensional curvature-constrained shortest-path problem. In WAFR ’98: Proceedings of the third workshop on the algorithmic foundations of robotics on Robotics : the algorithmic perspective, pages 49–57, Natick, MA, USA, 1998. A. K. Peters, Ltd. [11] Hongyan Wang and Pankaj K. Agarwal. Approximation algorithms for curvature-constrained shortest paths. In SODA ’96: Proceedings of the seventh annual ACM-SIAM symposium on Discrete algorithms, pages 409–418, Philadelphia, PA, USA, 1996. Society for Industrial and Applied Mathematics.
Lemma 1. Let p and q be B-K normal paths of complexity at most k. Then, the centers of distinct underlying circles realized by p and q at any environment feature are at least k 2−O(c m) apart for some constant c, where m is a measure of the input precision.
6. DISCUSSION Earlier, we mentioned the open problem of Bereg and Kirkpatrick [3]: Is there a polynomial-time algorithm for determining whether a given corridor with width less than τ is traversable? With slight modifications, our algorithm answers this question in the affirmative. The algorithm is polynomial time because the intrinsic complexity of a path (i.e. number of underlying circles required) in such a corridor is bounded by the corridor complexity. Specifically, any F-W path touches each corner at most once and need not touch a wall more than twice in each direction. One discrepancy between corridors and polygonal obstacles is that the boundary of a corridor is made of arcs as well as straight line segments. If the corridor width is less than one, then the curvature of the curved walls is too small and any Dubins path cannot touch such a wall. Otherwise, the curvature of the curved walls are greater than one and any such wall can be followed once it is contacted. We believe that our ideas can be used to construct a complete approximation algorithm. The first step, normalizing a shortest path by twisting, is complete. To prevent substantial stretching of the path, we can augment the set of anchored circles with regular samples at each environment feature (i.e. sample like the current approximation algorithms). Unfortunately, there are more types of couplings to consider and path arcs longer than π may not have environment contacts. Both of these violate the assumptions of our current binning argument. An obvious relaxation of the motion planning problem is to allow the vehicle to reverse. Reeds and Shepp have analyzed the shortest such paths in the absence of obstacles [9]. These paths are composed of C-segments and S-segments, so
73
