A linear time algorithm for computing a convex path of ... - CiteSeerX

A linear time algorithm for computing a convex path of bounded curvature in a simple polygon Jean-Daniel Boissonnat, Subir Kumar Ghosh1 and Sylvain Lazard PRISME, INRIA, BP 93, 06902 Sophia Antipolis, France E-mail addresses: boissonn, sghosh, [email protected]

Abstract

In this paper, we consider the following open problem of non-holonomic robot motion planning. Given two points s and t inside an n-sided polygon P with or without holes, the problem is to compute a path of minimum length inside P from s to t consisting of straight-line segments and circular arcs such that (i) the radius of each circular arc is at least 1, (ii) each segment on the path is the tangent between the two consecutive circular arcs on the path, (iii) the given initial direction at s is tangent to the path at s and (iv) the given nal direction at t is tangent to the path at t. There is no known polynomial-time exact algorithm for this problem. Even if we remove the requirement for the path to be the shortest, there is no polynomial-time algorithm for this problem. Since a path of bounded curvature is the concatenation of convex sub-paths (i.e., sub-paths make only left turns or only right turns), an approach in solving this problem would be to design an algorithm for computing convex sub-paths and then glue them to form the path. However, no polynomial-time algorithm is known for any class of polygons with or without holes even if the entire path is convex. Here, we propose an O(n) time algorithm for computing a simple and convex path of bounded curvature (if it exists) in an arbitrary polygon without holes. Using the notion of complete visibility, the algorithm computes a special type of link path between s and t inside P and then constructs a convex and simple path of bounded curvature from the link path. Though the algorithm is for a non trivial instance of non-holonomic motion planning, the main idea of the algorithm, i.e., the relationship between link paths and the paths of bounded curvature, gives a new insight to the general problem. We also propose a necessary condition for the shortest path of bounded curvature, if it is convex and simple, in a polygon without holes. Based on the necessary condition, we show that the path computed by our algorithm can be transformed in O(n) time to another path whose length is at most twice the optimal.

1

On leave from the Tata Institute of Fundamental Research, Bombay 400005, India.

1

Robot

t

The diection of Path

BCP(s,t)

v

the steering

u ESP(s,t)

i u

i+1

w

s

Figure 2: BCP (s; t) intersects vui, uiui and ui w.

Figure 1: The car like robot is modeled as a rectangle.

+1

+1

1 Introduction One of the main problems in robotics, called motion planning, is to nd a collisionfree path amidst obstacles for a robot from its starting position to its destination. In motion planning, the set of all placements of a robot is called con guration space, and the set of all placements at which the robot does not intersect any obstacle is called free con guration space. For a free- ying robot, there exists a path from the robot's starting position to its destination if and only if these two positions lie inside the same connected component of the free con guration space. However, this may not be true if the robot has to obey non-holonomic constraints (for example, velocity/acceleration bounds, curvature bounds) imposed by its physical limitations. A robot is said to be non-holonomic if some kinematics constraints locally restricts the authorized directions for its velocity. A typical example of a non-holonomic robot is that of a car [11]: assuming no slipping of the wheels on the ground, the velocity of the midpoint between the two rear wheels of the car is always tangential to the path (Figure 1). In this paper, we consider the following open problem of non-holonomic robot motion planning. Given two points s and t inside an n-sided polygon P with or without holes, the problem is to compute a path of minimum length inside P from s to t consisting of straight-line segments and circular arcs such that (i) the radius of each circular arc is at least 1, (ii) each segment on the path is the tangent between the two consecutive circular arcs on the path, (iii) the given initial direction at s is tangent to the path at s and (iv) the given nal direction at t is tangent to the path at t. This problem of computing the shortest path of bounded curvature appears in many applications and goes back to Markov who studied the problem for joining pieces of railways. More recently, a great deal of attention has been paid to this problem in the context of non-holonomic robot motion planning [2, 3, 9, 10, 11, 12, 13, 14, 15, 17, 18]. Also see the CG impact task force report: Application challenges to computational geometry, 1996. In spite of several 2

results on non-holonomic robot motion planning, the problem is still open and it seems to be a dicult task to design a polynomial-time algorithm for this problem. Let us now review the results obtained so far on this problem. Jacobs and Canny [8] discretize the problem and calculate a path that approximates the shortest one in time 2 n L n L O(n ( " ) log n + "2 ), where " describes the closeness of the approximation and L is the total edge length of the obstacle boundaries. Wang and Agarwal [16] improved this 2 n result and proposed an algorithm whose time complexity is O( "2 log n), and thus does not depend on L. In another recent paper, Agarwal et al. [1] have considered a restricted class of obstacles called moderate obstacles. An obstacle is said to be moderate if it is convex and if its boundary is a dierentiable curve whose radius of curvature is at least 1 (i.e., no sharp corners). Under the assumption that all obstacles are disjoint and moderate, Agarwal et al. show that an approximate path can be computed in O(n log n + 1=") time. Recently, Boissonnat and Lazard [4] have proposed an O(n log n) time algorithm for moderate obstacles. This is the rst polynomial-time exact algorithm for a non trivial instance of the problem. Wilfong [17] studied this problem under the restriction that the robot must stay on one of m line segments (thought of as `lanes') and it can turn only between lanes. This algorithm pre-processes the scene in O(m (n + log m)) time so that any query can be answered in O(m ) time. Even we remove the requirement for the path to be the shortest, no polynomial-time algorithm is known for this problem. In [6] Fortune and Wilfong present an algorithm that can decide if a path of bounded curvature exists. However, their algorithm only decides the feasibility of a path, without necessarily nding one. This algorithm runs in O(2poly n;m ) time, where m is the number of bits of precision with which all points are speci ed. Since a path of bounded curvature is the concatenation of convex sub-paths (i.e., subpaths make only left turns or only right turns), an approach in solving this problem would be to design an algorithm for computing convex sub-paths and then glue them to form the path. However, no polynomial-time algorithm is known for any class of polygons with or without holes even if the entire path is convex. In Section 2, we propose an O(n) time algorithm for computing a simple and convex path of bounded curvature (if it exists) in an arbitrary polygon without holes. Using the notion of complete visibility, the algorithm computes a special type of link path between s and t inside P and then constructs a convex and simple path of bounded curvature from the link path. Though the algorithm is for a non trivial instance of non-holonomic motion planning, the main idea of the algorithm, i.e., the relationship between link paths and the paths of bounded curvature, gives a new insight to the general problem. In Section 3, we propose a necessary condition for the shortest path of bounded curvature, if it is convex and simple, in a polygon without holes. Based on the necessary condition, we show that the path computed by our algorithm can be transformed in O(n) time to another path whose length is at most twice the optimal. In Section 4, we conclude the paper with a few remarks. 2

( + )

+

2

4

2

2

(

)

3

2

2 An algorithm for computing a convex path In this section, we present a linear time algorithm for computing a simple and convex path of bounded curvature from s to t inside a simple polygon P , where the initial and nal directions of the path at s and t respectively are given. Let s0 be the closest point of s on the boundary of P (denoted by bd(P)) such that the given initial direction at s is from s to s0. Similarly, t0 be the closest point of t on bd(P ) such that the the given nal direction at t is from t0 to t. Let BCP (s; t) denote a path of bounded curvature from s to t inside P . BCP (s; t) is said to be convex if it makes only left turns or only right turns while traversing from s to t. Let ESP (s; t) denote the Euclidean shortest path from s to t inside P . In the following lemma, we show the relationship between BCP (s; t) and ESP (s; t) in P .

Lemma 2.1 If BCP (s; t) is simple and convex, then ESP (s; t) is also convex. Proof: Let ESP (s; t) = (u ; u ; :::; uk ) where s = u and t = uk . Assume on the contrary 1

2

1

that BCP (s; t) is simple and convex, but ESP (s; t) is not convex (Figure 2). So there exists an edge uiui such that ESP (s; t) makes a right turn at ui and a left turn at ui or vice versa. Extend uiui from ui (and ui ) to the boundary of P meeting it at v (respectively, w). Since P is a closed and bounded region, starting from s BCP (s; t) rst intersects the segments vui, then it intersects uiui and nally it intersects ui w. Since all three segments lie on the same segment vw, BCP (s; t) cannot intersect all three segments in that order if it is simple as well as convex, which is a contradiction. Q.E.D. Observe that if ESP (s; t) is convex, then BCP (s; t) may not be convex. If ESP (s; t) is just the line-segment st, then convex and simple BCP (s; t) can be found by drawing the appropriate tangents between the circles tangential to tt0 and ss0 and by testing the intersection of the path with bd(P ). If ss0 intersects tt0 , then convex and simple BCP (s; t) can be found by drawing an appropriate circle tangential to tt0 and ss0. Without loss of generality, we assume that ESP (s; t) is not the line-segment st and ss0 does not intersect tt0 . If ESP (s; t) makes a right turn (or a left turn) at each vertex on the path, all convex BCP (s; t) lie inside the region bounded by s0s, ESP (s; t), tt0 and counter clockwise (respectively, clockwise) boundary of P from t0 to s0. From now on we call this region as P. Before we show the relationship between a link path and BCP (s; t) in Lemmas 2.2 and 2.3, we need the following de nitions. A link path between two points x and y inside P is said to be convex if it makes only left turns or only right turns at every turning point of the path. A link path from x to y inside P is said to be feasible (denoted by FLP (x; y)) if the link path is convex and for every turning point z, there exists two links L and L in the link path such that (i) L is one of the links between x and z and L is one of the links between z and y and (ii) there exists a circular arc of radius at least 1 inside P which is tangential to both L and L and is facing z. +1

+1

+1

+1

+1

1

2

1

1

2

2

4

+1

v 1

w

w 2 FLP(s,t) z

u

z

BCP(s,t) 1

2

w 3

P’

ESP(s,t)

v 2 v

s’

w 1

s

t

w

t’

k

s’

Figure 3: FLP (s; t) is sw , w w , ..., wk t. 1

1

ESP(s,t)

s

t

t’

Figure 4: Pruning the polygon P to obtain P 0

2

Lemma 2.2 If BCP (s; t) is simple and convex, then there exists a feasible link path from s to t inside P (Figure 3).

Proof: Let w be any point on ss0 such that the tangent from w to BCP (s; t) lies inside 1

1

P . Let the tangent meet BCP (s; t) at z . Extend w z from z to bd(P) meeting it at v . Let w be any point on z v such that the tangent from w to BCP (s; t) lies inside P . Let the tangent meet BCP (s; t) at z . Extend w z from z to bd(P) meeting it at v . Repeat the construction till the link path intersects tt0 at some point wk . Since the link path sw , w w ,..., wk t turns in the same direction at every turning point wi, the link path is convex. In order to show that this is a feasible link path, we have to show that for any turning point wi there exists a circular arc of radius at least 1 facing wi such that the circular arc is tangential to both wi? wi and wiwi and is lying inside P . Let C 0 be the circle corresponding to the circular arc C of BCP (s; t) which is tangential to wi? wi. Translate C 0 along wi? wi towards wi till C 0 is tangential to wiwi . Observe that C 0 cannot intersect bd(P ) before touching wiwi and the radius of C 0 is at least 1 because C and C 0 have the same radius. Hence, (sw , w w ,..., wk t) is a feasible link path from s to t. Q.E.D. 1

2

1

1 1

2

2 2

1

1 1

2

2

2

1

1

2

1

1

+1

1

+1

+1

1

1

2

Lemma 2.3 If there is a feasible link path from s to t inside P , then there exists a convex

BCP (s; t). Proof: The proof follows from the de nition of a feasible link path. Q.E.D. It can be seen that there may be several feasible link paths from s to t. However, it is enough to compute any FLP (s; t) in order to compute a convex BCP (s; t). To compute a FLP (s; t), we construct a simple polygon P 0 from P such that all FLP (s; t) lie inside P 0. Without loss of generality, we assume from now on that ESP (s; t) makes a right turn at every vertex of the path (Figure 4). Compute the Euclidean shortest path tree in P rooted at s (denoted by SPT (s)). If any path in SPT (s) makes a left turn at any 5

vertex u, where the parent of u is a vertex v of ESP (s; t), then extend vu from u to bd(P ) meeting at a point w. Insert the segment uw in P and remove the region of P bounded by the segment uw and the clockwise bd(P ) from u to w. Analogously, compute the Euclidean shortest path tree in P rooted at t. If any path in SPT (t) makes a right turn at any vertex u, where the parent of u is a vertex v of ESP (t; s), then extend uv from u to bd(P ) meeting at a point w. Insert the segment uw in P and remove the region of P bounded by the segment uw and the counter clockwise bd(P ) from u to w. The remaining region of P is called P 0. P 0 is known as the Complete visibility polygon of P from ESP (s; t) and the following lemma holds for the region P 0. For details, see Ghosh [7] and Chandru et al. [5].

Lemma 2.4 All convex link paths from a point of ss0 to a point of tt0 lie inside P 0. Since all convex link paths lie inside P 0, all FLP (s; t) also lie inside P 0. Hence, we compute a FLP (s; t) inside P 0. Intuitively, P 0 consists of all those points of P whose both tangents to ESP (s; t) lie inside P as well as P 0. >From now on, the clockwise boundary of P 0 from s to t is referred as the upper chain. We also use the notation upper chain(x; y) to indicate the portion of the upper chain between two points x and y. Let Bs (or Bt) denote the circle of radius 1 such that ss0 (respectively, tt0 ) is tangent to Bs at s (respectively, t). Since there are two circles which are tangential at s (or t), the circle which lies to the left (respectively, right) of the ray drawn from s0 (respectively, t0 ) though s (respectively, t) is called Bs (respectively, Bt). Observe that the internal angle at s between ss0 and the edge of ESP (s; t) incident on s can be convex or re ex. If the angle is convex, then BCP (s; t) cannot pass through Bs. However, this is not true is the angle is re ex. Analogously, the internal angle at t between tt0 and the edge of ESP (s; t) incident on t can be convex or re ex. If the angle is convex, then BCP (s; t) cannot pass through Bt . However, this is not true is the angle is re ex. So, we have four situations depending upon whether or not the angles at s and t are convex. In the sequel, we consider only the situation where both angles are convex. Other situations can be handled keeping in mind that BCP (s; t) can pass through Bs or Bt . Let Cs denote the portion of the arc of Bs from s to the next intersection point in clockwise order between ESP (s; t) and Bs. The corresponding circular arc Ct at t is de ned analogously. We have the following lemma.

Lemma 2.5 If upper chain(s0 ; t0) intersects Cs or Ct , then there is no convex BCP (s; t). Proof: If upper chain(s0 ; t0 ) intersects Cs or Ct (Figure 5), then upper chain(s0 ; t0) also intersects any convex BCP (s; t) because Cs and Ct are nested between ESP (s; t) and any convex BCP (s; t). Hence, there is no convex BCP (s; t). Q.E.D. >From now on we assume that upper chain(s0 ; t0 ) does not intersect Cs or Ct . Let link chain(s; y) denote a simple path inside P 0 from s to some point y 2 upper chain such that for every point z 2 link chain(s; y), there exists a FLP (s; z) and no FLP (s; z) 6

u

u

w

i

i-1

v

upper chain

Cs s

Cs ESP(s,t)

Ct

s

C

t t

t

Figure 6: A portion of the upper chain is removed by adding the segment ui? w.

Figure 5: The upper chain can intersect Cs or Ct.

1

crosses link chain(s; y). By de nition, every convex BCP (s; t) lies inside the region bounded by ESP (s; t) and link chain(s; t). The algorithm traverses the upper chain from s to t and computes the link chain for every vertex of upper chain(s; t). Though the algorithm constructs link chains only for vertices, it ensures that the property of link chain is also preserved for the internal points of an edge. After computing link chain(s; t), the algorithm traverses link chain(s; t) from t to s and computes a FLP (s; t) such that all of its turning points lie on link chain(s; t). Before we state the procedure for computing link chain(s; t), we need a preprocessing step to ensure that the extension of any edge of the upper chain does not intersect Cs or Ct . Draw the tangent from Cs to ESP (s; t) and extend it to the upper chain meeting it at a point z (Figure 6). Let upper chain(s; z)=(u ; u ; :::; uk ), where u = s and uk = z. Assume that upper chain(u ; ui? ) has been considered and ui is the current vertex under consideration. Let vui? be the tangent from ui? to Cs meeting Cs at v. If ui lies to the right of the ray drawn from v through ui? , then move to the next vertex. Otherwise, extend vui? from ui? till it intersects upper chain(ui? ; z) at a point w. Since there is no FLP (s; u0) for all u0 2 upper chain(ui? ; w) (excluding the points ui? and w), this portion of the upper chain does not belong to the link chain. Hence, remove this portion from the upper chain by adding the segment ui? w. Analogously, draw the tangent from Ct to ESP (s; t) and extend it to the upper chain meeting it at a point z. Let upper chain(t; z)=(u ; u ; :::; uk ), where u = t and uk = z. Assume that upper chain(u ; ui? ) has been considered and ui is the current vertex under consideration. Let vui? be the tangent from ui? to Ct meeting Ct at v. If ui lies to the left of the ray drawn from v through ui? , then move to the next vertex. Otherwise, extend vui? from ui? till it intersects upper chain(ui? ; z) at a point w. Remove all 1

1

2

1

1

1

1

1

1

1

1

1

1

1

1

1

2

1

1

1

1

1

1

1

1

7

y

upper chain

x

y w

z

z1

C

z

z2

2

upper chain Bl

x

Bl w

1

C

s

wl s

l

Figure 8: The point z does not lie on the edge yx and the point z lies on an edge of the upper chain. 1

Figure 7: Both points z and z lie on edges of the upper chain. 1

2

2

points u0 2 upper chain(ui? ; w) from the upper chain by adding the segment ui? w. Henceforth, the remaining portion of the upper chain is referred to as the upper chain. We now state the procedure for computing link chain(s; t). Initially, link chain(s; s0 ) = ss0. Assume that link chain(s; y) has been computed and we wish to compute link chain(s; x), where x is the next clockwise vertex of y on the upper chain. Let link chain(s; y) = (sw , w w , ..., wl y). Let Ci denote the circular arc tangential to wiwi and wiwi? . Let Bi denote the circle corresponding to Ci (or the portion of circular arc by extending Ci up to ESP (s; t)). We call Ci and Bi as the circular arc and circle at wi respectively. Let ray(a; b) denote the directed segment from a point a to in nity through another point b. Let C (a; b) denote the circular arc of unit radius from a point a to another point b in clockwise direction. We have the following two cases. Case 1: The internal angle between wl y and yx is convex. Case 2: The internal angle between wl y and yx is re ex. Consider Case 1. It consists of three cases depending on the position of Bl with respect to the edge yx. Case 1.1: The edge yx does not intersect the circle Bl . Construct the circular arc C (z ; z ) such that it is tangential to ray(y; wl) and ray(y; x) at z and z respectively. Case 1.1.1: Both points z and z lie on the edges of the upper chain (Figure 7). If ESP (s; t) intersects C (z ; z ), then there is no convex BCP (s; t). Otherwise, link chain(s; x) = (sw ; :::; wl y; yx). 1

1

+1

1

1

2

1

2

2

1

1

1

2

2

1

1

8

y

y w1

z3 C

z4

x

wl

z1

C

upper chain z2

z1

upper chain

z2

x ESP(s,t)

Figure 9: The point z lie on the edge yx and the point z does not lie on an edge of the upper chain.

Figure 10: Both points z and z do not lie on the edges of the upper chain

1

1

2

2

Case 1.1.2: The point z1 does not lie on the edge yx and the point z2 lies on an edge of the upper chain. (Figure 8). Draw another circular arc C (w; x) such that it is tangential to wly at some point w and passes through x. If ESP (s; t) intersects C (w; x) , then there is no convex BCP (s; t). Otherwise, link chain(s; x) = (sw1; :::; wl y; yx). Case 1.1.3: The point z1 lie on the edge yx and the point z2 does not lie on an edge of the upper chain. (Figure 9). Observe that link chain(s; y) cannot intersect C (z2 ; z1) because it will imply that xy have intersected Bl . If ESP (s; t) does not intersect C (z2 ; z1), then link chain(s; x) = (sw1; :::; wly; yx). Otherwise, draw another circular arc C (z4; z3 ) such that it is tangential to some edge of link chain(s; wl ) at a point z4 and is also tangential to yx at a point z3 . If ESP (s; t) intersects C (z4; z3 ) , then there is no convex BCP (s; t). Otherwise, link chain(s; x) = (sw1; :::; wl y; yx). Case 1.1.4: Both points z1 and z2 do not lie on the edges of the upper chain (Figure 10). Observe that link chain(s; y) cannot intersect C (z2; z1 ) because it will imply that xy have intersected Bl . If ESP (s; t) does not intersect C (z2; z1 ), then link chain(s; x) = (sw1; :::; wly; yx). Otherwise, draw another circular arc C (z4 ; x) such that it passes through x and is tangential to some edge of link chain(s; y) at a point z4 . If ESP (s; t) intersects C (z4 ; x), then there is no BCP (s; t). Otherwise, link chain(s; x) = (sw1 ,..., wl y, yx). Case 1.2: The edge xy touches the circle Bl . If Bl is tangential to xy, then it analogous to either Case 1.1.1 or Case 1.1.3. If Bl passes through x, it is analogous to Case 1.1.2 by considering C (w; x) as the portion of circular arc of Bl lying between x and the tangent point w of Bl with wly.

9

y

wl Cl

w j+1 x

y B j+1

upper chain

ESP(s,t)

upper chain

s

wj

Figure 11: The circular arc Cl passes through a vertex of ESP (s; t). w

w z1

w y

x

w

y

C

x z

4

ESP(s,t)

ESP(s,t)

w

j

Figure 13: The circular arc C (z ; z ) is inside P 0. 2

l

z3

u w z

2

j+1

l

C z

s

Figure 12: The edge yx does not intersect Bj . w

j+1

w

x

Bj

j

Figure 14: The circular arc C (z ; z ) is not inside P 0.

1

2

1

Case 1.3: The edge yx intersects the circle Bl . If Cl passes through a vertex of ESP (s; t) (Figure 11), then there is no convex BCP (s; t). Otherwise, check the intersection between yx and Bl?1. If yx intersects the circle Bl?1, then check whether or not Cl?1 passes through a vertex of ESP (s; t). Repeat this process till Bj is found such that either yx does not intersect Bj (Figure 12) or xy touches Bj . If yx does not intersect Bj , then it is analogous to Case 1.1 by treating Bj as Bl . If x touches Bj , then it is analogous to Case 1.2 by treating Bj as Bl .

Consider Case 2. Extend xy from y till it intersects link chain(s; y) at a point w on the edge wj wj . Treating w as y and wx as an edge of the upper chain, check Case 1. If it reports that there exists a circular arc C (z ; z ) which is not intersected by ESP (s; t), where z and z are tangent points with an edge of link chain(s; w) and wx +1

2

2

1

10

1

respectively (Figure 13), then link chain(s; x) = (sw ; :::; wly; yx). Otherwise, traverse link chain(s; y) from w to y for locating the rst point u on some edge of link chain(s; y) such that there exists a circular arc C (z ; z ) which is not intersected by ESP (s; t), where z and z are tangent points with an edge of link chain(s; y) and uy respectively (Figure 14). Observe that C (z ; z ) passes through a vertex of ESP (s; t). Extend uy from y till it meets the upper chain at z. So there is no feasible link path to any point on the upper chain between y and z and the segment yz is considered as an edge of the upper chain for the subsequent computation. If yz intersects tt0 , then the intersection point (i.e. z itself) becomes t0 . If yz intersects Ct, instead of the upper chain, then there is no FLP (s; t). If the algorithm reaches t, then link chain(s; t) has been computed. In order to show that the algorithm correctly computes link chain(s; t), it is enough to show that a vertex x 2 upper chain(s; t) is in link chain(s; t) if and only if there is a convex and simple BCP (s; x) as shown in the following lemmas. 1

4

4

3

3

4

3

Lemma 2.6 A vertex x 2 upper chain(s; t) is in link chain(s; t) if there is a convex BCP (s; x).

Proof: Since x belongs to link chain(s; t), we show that there exists a convex BCP (s; x)

by constructing a convex BCP (s; x). Let B be the circle tangential to the edge yx at x, where y is the next counterclockwise vertex of x on link chain(s; t). Three cases can arise depending upon whether link chain(s; y) intersects B or link chain(s; y) touches B or link chain(s; y) does not intersect B . If link chain(s; y) intersects B , then construct another circle B 0 which passes though x and is tangential to some edge vv0 of link chain(s; y). Let C (z ; x) be the portion of circular arc of B such that it is tangential to vv0 at z . Cases 1.1.2 and 1.3 of the algorithm ensures that ESP (s; t) does not intersect C (z ; x). On the other hand, upper chain(x; t) cannot intersect C (z ; x) as x is completely visible from ESP (s; t). Therefore, C (z ; x) lies inside P 0. If link chain(s; y) touches B , then Case 1.2 ensures that ESP (s; t) does not intersect C (z ; x) where link chain(s; y) touches B at z . If link chain(s; t) does not intersect B , then translate B along xy till either it touches link chain(s; y) or B reaches y. If B touches link chain(s; y) at some point z and xy at some point z , then Cases 1.1.1 and 1.2 of the algorithm ensures that ESP (s; t) does not intersect C (z ; z ). If B reaches y, then y is a re ex vertex. Extend xy from y till it intersects link chain(s; y) at a point w. Let C (z ; z ) be the circular arc such that z and z are tangent points with an edge of link chain(s; w) and wx respectively. Observe that if ESP (s; t) intersects C (z ; z ), then x cannot belong to link chain(s; t) as it would have been removed by the algorithm in Case 2. Therefore, C (z ; z ) lies inside P 0. Above argument shows that there is a convex BCP (z ; x) inside P 0. Treating z as x, B can be translated along xy till another point z is found on link chain(s; z ) such that there is a convex BCP (z ; x). Repeating this process, a convex BCP (s; x) can be constructed inside P 0. Q.E.D. 2

2

2

2

2

2

2

2

1

2

1

2

2

2

2

1

1

1

1

2

3

3

11

2

2

zi z

ui

v"

i

link chain(s,t)

ui

v’

link chain(s,t) ESP(s,t)

ESP(s,t) v

t i

v

s

Figure 15: FLP (zi; z ) can be extended to vi.

s

t

i

Figure 16: FLP (zi; z ) can be extended to v0.

0

0

Lemma 2.7 A vertex x 2 upper chain(s; t) is not in link chain(s; t) if there is no

convex BCP (s; x). Proof: Since x is not in link chain(s; t), we show that there is no convex BCP (s; x). Since x is not in link chain(s; t), then x has been removed by the algorithm in Case 2. Let C 0 be the circular arc constructed by the algorithm while removing x. We know that C 0 passes through a vertex of ESP (s; t). Draw the tangent from x to ESP (s; t) and extend it to upper chain(s; x) meeting it at w. Let C (z2 ; z1) be the circular arc such that it is tangential to xw at z1 and is tangential to upper chain(s; w) at z2. Assume that ESP (s; t) does not intersect C (z2 ; z1). Since xw passes through a vertex of ESP (s; t) and C 0 also passes through a vertex of ESP (s; t), any tangent from C 0 does not reach any point of upper chain(x; t). Hence x cannot be removed by the algorithm if ESP (s; t) does not intersect C (z2; z1 ). So, we assume that ESP (s; t) intersects C (z2 ; z1 ). If ESP (s; t) touches C (z2 ; z1) (i.e., C (z2 ; z1) passes through a vertex of ESP (s; t)), then C (z2 ; z1) is same as C 0 . In that case, the algorithm cannot remove x because the tangent from x to C 0 lies inside P . So we assume that ESP (s; t) has properly intersected C (z2 ; z1). Translate the circle corresponding to C (z2; z1 ) along the upper chain in clockwise direction till a circular arc C (z4; z3 ) is found, where xz3 is the tangent to C (z4 ; z3) and C (z4; z3 ) is tangent to upper chain(s; x) at some point z4 , such that C (z4; z3 ) is inside P 0 (i.e., it passes through a vertex of ESP (s; t)) or some vertex y of upper chain(w; x) lies on xz3 . If C (z4 ; z3) passes through a vertex of ESP (s; t), then C (z4; z3 ) is same as C 0. In that case, the algorithm cannot remove x because the tangent from x to C 0 lies inside P . If some vertex y of upper chain(w; x) lies on xz3 , then there cannot be any convex BCP (s; x).

Q.E.D.

We now present the procedure for computing FLP (s; t) following the constructive proof of Lemma 2.6. Assume that (zi; :::; z ; z ) is a FLP (zi; z ), where t = z and for all 1

0

12

0

0

j , zj is a vertex or a point on link chain(s; t). Initially, FLP (t0; t)=tt0. Let ui be the next counterclockwise vertex of zi . If the internal angle at ui is convex, then FLP (ui; z ) = (ui; zi; :::; z ; z ). Otherwise, extend uizi from ui till it intersects link chain(s; t) at some point vi. If there exists a circular arc inside the region bounded by link chain(s; t) and ESP (s; t) such that it is tangential to zivi and also to some edge of (zi; :::; z ; z ) (Figure 15), then FLP (zi ; z ) = (zi ; zi; :::; z ; z ) where zi = vi. Otherwise, there exists another circular arc C (v0; v00) inside the region bounded by link chain(s; t) and ESP (s; t) such that it is tangential to some edge of link chain(s; ui) at a point v0 and is also tangential to some edge of (zi ; :::; z ; z ) at a point v00 (Figure 16). Construct a convex link path from v00 to v0 such that all links are tangential to C (v0; v00) and all turning points are on link chain(v0 ; v00). Concatenate this convex link path to FLP (v00; z ) to form FLP (v0; z ). Repeat this process till ss0 is reached. Once ss0 is reached, a FLP (s; t) has been computed. Since we know the tangent points of all circular arcs with the links of FLP (s; t), a convex BCP (s; t) can also be constructed by replacing the links of FLP (s; t) between the tangents points of each circular arc by the corresponding circular arc. Here we analyze the time complexity of the algorithm. Since the Euclidean shortest path tree can be constructed in O(n) time, the region P 0 can also be constructed in O(n) time [5, 7]. Then the algorithm constructs link chain(s; t). If the algorithm nds an intersection between the current edge xy and any previously constructed circular arc, then the algorithm does not consider this circular arc again. Therefore, the total number of intersections between edges and circular arcs can be at most O(n) because the algorithm constructs at most two circular arcs for each vertex. To check the intersection between ESP (s; t) and any circular arc, it is enough to the check the intersection between the circular arc and the edges of ESP (s; x) lying in the funnel formed by ESP (s; x) and ESP (s; y). Since these funnels are disjoint, total time required for checking is O(n), if each funnel is considered a constant number of times. Observe that while checking the intersect between ESP (s; t) and a circular arc tangential to the extension of a re ex edge, the edges of a funnel, which is already been considered once, will be considered again. It may be the case that extensions of several re ex edges may force the same funnel being considered again and again and it may so happen that the number of edges of ESP (s; t) in that funnel is large. In this special situation, the algorithm can even take O(n ) time. Suppose the points of the extension all the re ex edges on upper chain(s; t) are known before computing link chain(s; t). In that case, the algorithm can check the intersection of ESP (s; t) with the circular arc tangential to the extension of a re ex edge whenever it encounters such extension point on upper chain(s; t). Therefore, there will be no backtracking and the overall time complexity of the algorithm remains O(n). We show that it is enough to introduce some of the extension points of re ex edges on upper chain(s; t) before link chain(s; t) is computed and all these extension points can be computed in O(n) time by traversing the upper chain as follows. Let upper chain(s; t) = (u ; u ; :::; uk ) where s = u and t = uk . Traverse upper chain(s; t) from t to s and let ui be the re ex vertex encountered while traversing (Figure 17). Continue traversing 0

1

0

1

+1

0

+1

1

1

0

+1

0

0

0

2

1

2

1

13

0

u

m+1

ui

um

v

vi ui

u i+1

i

ESP(s,t) vm

t

Figure 18: A portion of the upper chain(vi ; ui) is removed if the extension of a re ex edge intersects C (z ; z ).

s

Figure 17: The lid um vm intersects the lid uivi .

4

3

till an edge is encountered such that ray(ui ; ui) has intersected the edge at a point vi. Introduce vi on upper chain(s; t). Observe that the segment uivi lies inside P 0 as ui and vi are completely visible from ESP (s; t). Any such segment uivi is called a lid. Traverse from ui again till a re ex vertex um 2 upper chain(ui; vi) is encountered such that ray(um ; um) intersects the lid uivi . Traverse upper chain(s; vi ) from vi till an edge is located which is intersected by ray(um ; um) at a point vm. Now umvm is the current lid and repeat the process by traversing from um. If the algorithm does not nd any appropriate re ex vertex whose ray intersects the current lid, it crosses the other endpoint of the lid and traversing till it nds another re ex vertex which gives a starting lid. By repeating this process, all extension points of those lids are introduced in upper chain(s; t) whose both endpoints are not contained between the endpoints of some other lid. As upper chain(s; t) is traversed at most twice, the time required for this computation is O(n). Let uivi be the lid of the re ex edge uiui , where ui is the next clockwise vertex of ui. When the algorithm encounters vi while computing link chain(s; t), it constructs the circular arc C (z ; z ), where C (z ; z ) is tangent to uivi at z and is tangent to link chain(s; vi ) at some point z . If ESP (s; t) does not intersect C (z ; z ), then the algorithm stores the information that there is a FLP (s; ui ) and it also stores the same information for all other re ex edges in upper chain(vi ; ui). Note that these re ex edges will not be considered again even if they lie in the upper chain between the endpoints of some other subsequent lid. If ESP (s; t) intersects C (z ; z ) (Case 2 of the algorithm), then it constructs another circular arc C (z ; z ) inside P 0 and removes the appropriate portion of upper chain(ui; t) starting from ui (Figure 18). The algorithm also removes the appropriate portion of upper chain(vi ; ui) starting from uj for all re ex edges uj uj 2 upper chain(vi ; ui) such that extension of uj uj from uj meets upper chain(vi ; ui) and +1

+1

+1

+1

2

1

2

+1

1

1

2

2

1

+1

2

4

1

3

+1

+1

14

also intersects C (z ; z ). Introducing the endpoints of lids on upper chain(s; t) before computing link chain(s; t) and modifying Case 2 of the algorithm for removing appropriate portion of upper chain(s; t) for all re ex edges nested inside any lid ensure that a convex and simple BCP (s; t) can be constructed correctly in O(n) time. We summarize the result in the following theorem. 4

3

Theorem 2.1 A convex and simple path of bounded curvature between two given points

inside a simple polygon with the given initial and nal directions of the path can be computed in linear time.

3 An algorithm for computing a sub-optimal convex path In this section, we present a linear time algorithm for computing a simple and convex path of bounded curvature from s to t inside a simple polygon P such that the length of the path is at most twice the optimal. Note that the shortest BCP (s; t) need not be convex even if there exists a convex and simple BCP (s; t). We start with the following observation.

Lemma 3.1 The total length of all circular arcs on any simple and convex BCP (s; t) inside P 0 is constant. Proof: For any two convex as well as simple paths between s and t in P 0, the total change in angle from the starting direction to the nal direction is the same. Hence, the total length of all circular arcs for any such path is constant. Q.E.D. Above lemma suggests that if the lengths of two paths are not same, then the sums of the length of line segments on two paths are dierent. We use this fact in proving the following necessary condition for the shortest path. Necessary Condition : Every circular arc of the shortest BCP (s; t), if it is simple as well as convex, passes through a vertex of ESP (s; t). Proof: Assume on the contrary that there exists a circular arc Ci on the shortest BCP (s; t) such that it does not pass through a vertex of ESP (s; t). Let Ci? and Ci be the previous and the next circular arc of Ci on the path respectively (Figure 19). Let ri? li be the tangent between Ci? and Ci. Analogously, let rili be the tangent between Ci and Ci . Let ui? , ui and ui be the center of the circles Bi? , Bi and Bi corresponding to Ci? , Ci and Ci respectively. Since Ci does not pass through any vertex of ESP (s; t) by assumption, Bi can be translated along uiui . Since the center c of the circle remains on uiui while translating, by triangle inequality, the sum of the length of ui? c and cui is less than that of the length of ui? ui and uiui . Observe that the length of ri? li is the same as that of ui? ui because ri? li, li ui, uiui? and ui? ri? 1

+1

1

1

+1

1

1

+1

+1

1

+1

+1

+1

+1

1

+1

1

1

1

15

1

+1

1

1

1

y u1

FLP(s,t) l i

C u

r i

i

v1 i

v2

c

Bi

z1 r i-1

r l i+1

C i-1 u

C

B i-1

i-1

B i+1

u

B1

1

i+1

ESP(s,t) i+1

x

Figure 19: The length of the path can be reduced by translation.

Figure 20: The construction of circular arc C . 1

form a parallelogram. Analogously, the length of rili is same as that of uiui because rili , li ui , ui ui and uiri form a parallelogram. Hence, by translating a new path can be constructed whose length is smaller than the length of the shortest BCP (s; t), a contradiction. Q.E.D. The above necessary condition suggests that starting from BCP (s; t) computed by the algorithm in Section 2, a shorter path can be constructed by translation as follows. Let R be the region of P 0 enclosed by ESP (s; t) and the FLP (s; t) computed by the algorithm in Section 2. The task is to compute a simple and convex BCP (s; t) inside R such that the path satis es the necessary condition. Draw the tangent r v between Cs and ESP (s; t) where r 2 Cs and v 2 ESP (s; t) and extend it to FLP (s; t) meeting it at u . Let y and x be the next counterclockwise and clockwise vertex of u respectively. Let B be the circle tangential to r u at v . Let C be the clockwise portion of B from v to the rst intersection point of B and ESP (v ; t). If FLP (y; t) does not intersect C , then draw the tangent between C and ESP (v ; t) and extend it to FLP (s; t) meeting it at u . If any edge of FLP (y; t) intersects C , replace C by another circular arc such that it passes through v and is tangential to some edge (say, xy itself) of FLP (y; t) at z (Figure 20). Observe that FLP (s; y) cannot intersect C because it will imply that FLP (s; t) is not a feasible link path. However, ESP (v ; t) can intersect C between v and z . A vertex v of ESP (v ; t) can be located such that there exists another circular arc C 0 which is tangential to xy, passes through v and is not intersected by FLP (y; t). Intuitively, C 0 can be viewed as a translation of the circular arc along FLP (y; t) which is tangential to some edge of FLP (s; y) and passes through a vertex of ESP (s; t). Replace C by C 0 . Draw the tangent between Cs and C . Draw the tangent between C and ESP (s; t) and extend it to FLP (s; t) meeting it at u . Construct C following the procedure for constructing C . By repeating this process, C , C , ... can be constructed till Ct is reached. Since each of Cs, C , C , ..,Ct passes through a vertex of ESP (s; t), the path satis es the necessary condition. It can be seen that the algorithm runs in O(n) +1

+1

+1

+1

+1

+1

1 1

1

1

1

1

1

1

1

1

1

1

1

1

1

1

1

1

2

1

1

1

1

1

1

1

2

1

1

1

2

1

1

1

2

1

2

1

1

2

16

2

time because the number of circles constructed by the algorithm is bounded by twice the number of vertices of ESP (s; t) and checking the intersection between the circular arcs and edges of FLP (s; t) is proportional to the number of edges in FLP (s; t) as FLP (s; t) is convex. We now show that the length of the path is at most twice the length of the shortest BCP (s; t) even if the shortest BCP (s; t) is not convex. Since every circular arc of the path passes through a vertex of ESP (s; t), the total length of the tangents between the circular arcs on the path is less than or equal to the length of ESP (s; t). Therefore, the length of the path is less than or equal to the length of ESP (s; t) + K , where K is the total length of circular arcs on the path which is constant as shown in Lemma 3.1. Since the length of ESP (s; t) is less than or equal to the shortest BCP (s; t), the length of the path is less than or equal to the length of the shortest BCP (s; t) + K . Dividing both sides by the length of the shortest BCP (s; t), we get a bound of 2 because the length of the shortest BCP (s; t) is at least K (even if the shortest BCP (s; t) is not convex). It can be seen that the upper bound is not tight as K is normally much less than the length of the shortest BCP (s; t). We summarize the result in the following theorem.

Theorem 3.1 If there exists a convex and simple BCP (s; t), then it can be transformed in linear time to another convex and simple BCP (s; t) such that every circular arc on the path passes through a vertex of ESP (s; t) and the length of the path is at most twice the optimal.

4 Concluding remarks We have shown that if the shortest BCP (s; t) is convex as well as simple, then every circular arc on the path passes through a vertex of ESP (s; t). In order to nd the shortest BCP (s; t), we have to nd a path such that the total length of the line segments on the path is minimum. Let s and t be the center of the circles corresponding to the circular arc Cs and Ct respectively. Let v , v ,..., vm be vertices of ESP (s; t) such that the circular arc Ci of the shortest BCP (s; t) passes through vi for all i. Let u , u ,.., um be the centers of the circles corresponding to circular arcs C , C ,.., Cm. It can be seen that the total length of the line segments on the shortest BCP (s; t) is minimum if and only if the sum of the lengths of s u , u u ,..., um? um and umt is minimum. Let B , B ,..., Bm be the circles of radius 1 where vi is the center of the circle Bi for all i. Observe that ui lies on the circumference of Bi. If we consider s as a source of light, B , B ,..., Bm as circular mirrors and t as an observer, then the path of the light from s to t after re ecting on mirrors B , B ,..., Bm should be the shortest path because light travels in the shortest path. Then the angle of incidence will be equal to the angle of re ection at each turning point on the path, where the normal is line drawn through the turning point and the center of the circle. However, it is not clear how to compute such a path. 0

0

1

2

1

1

0

1

1

1

2

1

2

2

0

2

0

2

0

0

1

1

0

2

17

s

s

t

Figure 21: The path makes both left as well a right turns.

t

Figure 22: The path is selfintersecting.

We have proposed an algorithm for computing a convex as well as simple BCP (s; t) in a simple polygon. It will be interesting to design an algorithm for computing a simple and convex BCP (s; t) (if it exists) in a polygon with holes. The main diculty in solving this problem is that the region of the polygon enclosed by ESP (s; t) and, a simple and convex BCP (s; t) may contain holes. Moreover, ESP (s; t) may not be convex even if there exists a simple and convex BCP (s; t). However, there exists an outward convex path EP , passing through vertices of the polygon from s to t inside the polygon, such that the region enclosed by EP and a convex as well as simple BCP (s; t) does not contain any hole. Once EP is located, using the algorithm in Section 2, a convex and simple BCP (s; t) can be computed. Since the number of outward convex paths between two points in a polygon with holes can be exponential, the naive algorithm by considering all outward convex paths between s and t runs in exponential time. It will be interesting to see if there is an approach which can avoid considering all possible outward convex paths between s and t so that the algorithm runs in polynomial time. As stated in the introduction, if there is no convex and simple BCP (s; t) inside a simple polygon, it can be that there exists a BCP (s; t) which makes both left as well as right turns even though ESP (s; t) is convex (Figure 21). Moreover, the path may be self-intersecting (Figure 22). Though any BCP (s; t) is a concatenation of convex subpaths, it seems to be a dicult task to locate a set of intermediate points or segments on the path such that two consecutive points or segments can be connected by a convex sub-path using the algorithm in Section 2.

References [1] P.K. Agarwal, P. Raghavan, and H. Tamaki, Motion Planning for a steeringconstrained robot through moderate obstacles, In Proceedings of the 27th Annual ACM Symposium on Theory of Computing, pages 343-352, 1995. 18

[2] J. Barraquand and J-C. Latombe, Nonholonomic mobile robots and optimal maneuvering, Revue d'Intelligence arti cielle, 3: 77-103, 1989. [3] J. Barraquand and J-C. Latombe, Nonholonomic multi-body mobile robots : controllability and motion planning in the presence of obstacles, Algorithmica, 10: 121-155, 1993. [4] J-D. Boissonnat and S. Lazard, A polynomial-time algorithm for computing a shortest path of bounded curvature amidst moderate obstacles, In Proceedings of the Annual ACM Symposium on Computational Geometry, 1996. [5] V. Chandru, S. K. Ghosh, A. Maheshwari, V. T. Rajan and S. Saluja, NC-Algorithms for minimum link path and related problems, Journal of Algorithms, 19: 173-203, 1995. [6] S. Fortune and G. Wilfong, Planning constrained motion, In Proceedings of the 20th Annual ACM Symposium on Theory of Computing, pages 445-459, 1988. [7] S.K. Ghosh, Computing the visibility polygon from a convex set and related problems, Journal of Algorithms, 12: 75-95, 1991. [8] P. Jacobs and J. Canny, Planning smooth paths for mobile robots, In Proceedings of the IEEE International Conference on Robotics and Automation, pages 2-7, 1989. [9] P. Jacobs and J-P. Laumond and M. Taix, Ecient motion planners for nonholonomic mobile robots, In Proceedings of the IEEE/RSJ International Workshop on Intelligent Robots and Systems, pages 1229-1235, 1991. [10] J-C. Latombe, A fast path-planner for a car-like indoor mobile robot, In Proceedings of the 9th National Conference on Arti cial Intelligence, pages 659-665, 1991. [11] J-C Latombe, Robot Motion Planning, Kluwer Academic Publishers, Boston, 1991. [12] J-P. Laumond, Finding collision-free smooth trajectories for a non-holonomic mobile robot, In Proceedings of the International Joint Conference on Arti cial Intelligence, pages 1120-1123, 1987. [13] J-P. Laumond and M. Taix and P. Jacobs and R.M. Murray, A motion planner for nonholonomic mobile robots, IEEE Trans. on Robotics and Automation, 1993. [14] Z. Li and J.F. Canny, Nonholonomic Motion Planning, Kluwer Academic Publishers, 1992. [15] Y. Nakamura and R. Mukherjee, Nonholonomic path planning and automation, In Proceedings of the IEEE International Conference on Robotics and Automation, pages 1050-1055, 1989. 19

[16] H. Wang and P.K. Agarwal, Approximation algorithms for curvature constrained shortest paths, In Proceedings of the 7th ACM-SIAM Symposium on Discrete Algorithms, pages 1-10, 1996. [17] G. Wilfong, Motion planning for an autonomous vehicles, In Proceedings of the IEEE International Conference on Robotics and Automation, pages 529-533, 1988. [18] G. Wilfong, Shortest paths for autonomous vehicles, In Proceedings of the IEEE International Conference on Robotics and Automation, pages 15-20, 1989.

20