Planning Near-Optimal Corridors amidst Obstacles Ron Wein Jur

Planning Near-Optimal Corridors amidst Obstacles Ron Wein Jur P. van den Berg (U. Utrecht) Dan Halperin Athens May 2006

Path Planning Given a start configuration s and a goal configuration g, find a collision-free path connecting s and g that avoids a given set of obstacles O. This problem is often difficult, so most algorithms try to find a path, regardless of its quality. For many applications, it is necessary to have a high-quality path. The resulting path therefore has to undergo some postprocessing stages, such as pruning, smoothing, etc.

Using Corridors Instead of making the moving object(s) follow a 1 -dimensional motion path, we let them move in a corridor indicating the general direction for the motion. s g Inside the corridor, motion is determined by a local planner (e. g. using the potential field method).

Applications For many applications it is better to use a corridor, which allows more flexibility in comparison to having a 1 -dimensional path: coherent group motion, small moving obstacles (and large static ones), planning camera motions following a moving character, avoiding predictable motions (in games), allowing non-holonomic and kineodynamic planning.

Defining a Corridor A corridor C = (t), wmax in a d-dimensional workspace is the union of the balls: Where: : [0, L] d is the backbone path, w: [0, L] (0, wmax] defines the corridor width, wmax is the preferred width.

The Weighted-Length Measure Given a corridor C = (t), wmax , its weighted length, denoted L*(C), is given by: We can improve the quality of a corridor by either making it wider (up to the preferred width), or by making its backbone path shorter.

Properties of an Optimal Corridor Observation: The width function of an optimal corridor is given by: where c(p) is the clearance of the point p (its distance from the nearest obstacle). Lemma: The backbone path of an optimal corridor connecting any given start and goal positions s and g is smooth. p

$Refraction of an Optimal Path Snell’s Law: If the width function is not continuous,$

Refraction of an Optimal Path Snell’s Law: If the width function is not continuous, then the backbone path of the optimal corridor connecting p 1 and p 2 satisfies: p 1 w 2 1 2 p 2

Optimal Corridors amidst Point Obstacles We consider planar environments cluttered with point obstacles p 1, …, pn 2 and a preferred width wmax. Let us first assume that the points are well-separated with respect to wmax , that is the discs B(pi; wmax) are pairwise disjoint in their interiors (namely ||pi – pj|| 2 wmax). We first examine what happens when the two endpoints are both in the vicinity of a single obstacle p (inside B(p; wmax)).

Near a Single Point Obstacle Using Snell’s Law: The Law of Sines on p. AB: 1 p r 2 r 1 Hence: 1 = 1. B 1 A 2

The Logarithmic Spiral Assume w. l. o. g. that p is the origin. A curve that forms a constant angle with the radii is a logarithmic spiral, given by the polar equation (where b = cot-1 ):

The Optimal Backbone Near a Single Point Obstacle Proposition: Given a single point obstacle located at the origin, a start position and a goal position , where rs, rg wmax , the backbone of the optimal corridor connecting s and g is an arc of the logarithmic spiral , where:

A Single Point Obstacle: The General Case Proposition: We are given two points s and g lying outside the disc B(p; wmax), where p is a point obstacle: If the segment sg does not intersect B(p; wmax), it is the optimal backbone. Otherwise, the optimal backbone is comprised of the tangent ss*, the circular arc s*g* and the tangent g*g. g* g g’ s* p wmax s’ s

The Visibility Graph Given a set O = {p 1, …, pn} of point obstacles in the plane that are well-separated with respect to wmax , we use the visibility graph of the discs B(p 1; wmax), … , B(pn; wmax) to compute optimal corridors. This graph can be constructed in O(n log n + E) time, where E is the number of visibility edges. Proposition: It is possible to compute the optimal corridor connecting s and g amidst n well-separated point obstacles in O(E log n) time.

The Bounded Voronoi Diagram The bounded Voronoi diagram Û (O) of a point set O w. r. t. wmax is comprised of circular arcs (portions of the B(pi; wmax)’s) and line segments (portions of the Voronoi diagram). It subdivides the plane into: bounded Voronoi cells of obstacles, cells with clearance above wmax.

Optimal Paths within Cells In a cell whose clearance is above wmax , optimal backbone paths are comprised of line segments and circular arcs. The weighted length of each element equals its “normal” length. In the bounded Voronoi cell of pi the optimal backbone path between and is a spiral arc a whose weighted length is given by: Voronoi edges are also locally optimal.

Moving amidst Dense Obstacles We have to consider spiral arcs that shortcut sharp turns (Voronoi vertices and reflex vertices): pi q 1 pi pi q 2 v q 1 v 1 q 2 v 2 q 1 q’ pj In the worst case we have to consider shortcuts of whose complexity is (n), which take exponential time to compute. q 2 v 2

The Approximation Scheme (I) Given > 0, we subdivide the features of Û (O) into small intervals of length c(I) / wmax. (c(I) is the clearance of the interval I, which we assume to be constant as I is small). Let be the total weighted length of all the features of Û (O), then there are / intervals in total. We define a graph D over the set of / intervals, which forms the node set of the graph.

The Approximation Scheme (II) D contains an edge between I 1 and I 2 only if the intervals are incident to a common cell. Such an edge may be: a straight visibility segment, a circular arc, a spiral arc, a straight line segment on a Voronoi edge. An edge must not cross a feature of Û (O). There are O( 2 / 2) edges in the graph, which can be constructed in O(n 2 / 2) time.

The Approximation Scheme (III) Given two endpoints, we can connect them to the graph D in O(n / ) time and use Dijkstra’s algorithm to obtain an approximation for the optimal backbone path * in O( 2 / 2) time. Lemma: If * is comprised of the path segments 1, …, k (where k = O(n)), then for each i there exists an edge e in D such that: L*(e) L*( i) + 2 2 . Corollary: We can compute in polynomial time a backbone path whose weighted length satisfies: L*( ) L*( *) + O(n) .

Polygonal Obstacles Let us consider a scene cluttered with polygonal obstacles P = {P 1, …, Pk} having n vertices in total. If the polygons are well-separated w. r. t. wmax , we can use the visibility graph of the dilated obstacles to answer corridorplanning queries.

The Bounded Voronoi Diagram for Polygons The features of Û (P) also include: line segments on dilated obstacle boundaries, parabolic arcs of the Voronoi diagram, line segments that separate bounded Voronoi cells of a polygon vertex from the cell of an incident edge.

Moving Near a Polygon Edge (I) Using Snell’s Law: 3 0 Note that we may have a reflection point! x=0 1 2 p 0 = (x 0, 0)

Moving Near a Polygon Edge (II) Let us now write: Let x 0: We obtained an equation of a circle whose center lies on x = 0.

The Optimal Backbone Near a Polygon Edge Proposition: The optimal backbone path between s and g near a polygon edge supported by x = 0 is a circular arc a of radius r* centered at (0, y*), where: The weighted length of this arc is given by ( w. r. t. the circle center): and

Optimal Corridors amidst Polygons We can generalize our approximation scheme for the case of polygonal obstacles. For any > 0, we can construct a graph D based on the structure of Û (P) in O(n 2 / 2) time (w. r. t. a given wmax value). For any two given endpoints, we can query D in O( 2 / 2) time and obtain a path that approximates the best backbone path up to an error of O(n) .

!Thank you