87 Views

Download Presentation
## Planning High-Quality Paths and Corridors

- - - - - - - - - - - - - - - - - - - - - - - - - - - E N D - - - - - - - - - - - - - - - - - - - - - - - - - - -

**Planning High-Quality Paths and Corridors**Ron Wein Jur P. van den Berg (U. Utrecht) Dan Halperin**Motivation**Plan a “natural looking” collision-free motion path for a robot translating on the plane amidst a set P1, … , Pm of configuration-space obstacles, given start and goal configurations s and g. • By “natural looking” we mean the path should be: • Short – not containing unnecessary long detours. • Having some clearance – not getting to close to an obstacle. • Smooth – not containing sharp turns (C1-smooth).**Application**Kamphuis and Overmars (2004) Motion planning for coherent groups of entities (computing a backbone path).**Visibility Graphs**Lozano-Pérez (1979) Used to plan shortest paths. Constructed in O(n2 log n) time, where n is the total number of vertices. Output-sensitive O(n log n + S) algorithms also exist. Query time is O(n log n + S), using Dijkstra’s algorithm. Resulting paths have no clearance**The “Retraction” Method**Ó’Dúnglaing and Yap (1985), Rohnert (1991) Construct the Voronoi diagram of the obstacles in O(n log n). Query time is O(log n + k). Paths may be too long and may contain sharp turns**1. Dilate the obstacles (Minkowski sums).**2. Compute visibility edges between dilated vertices. 3. Add Voronoi chains between chain-points. 4. Compute visibility edges emanating from chain points. The VV(c)-Diagram**Properties of the VV(c)-Diagram**• Outputs shortest paths which keep the preferred amount of clearance from the obstacles, where possible. • Paths are smooth. • In case of narrow passages (between chain points), we get a path with the maximal possible clearance (locally).**The Evolution of the VV(c)-Diagram**c = 0, … , ½.**c = 0**c > 0 c = The VV-Complex The VV(c)-diagram interpolates between the visibility graph and the Voronoi diagram of the obstacles. • The VV-complex encodes all VV(c)-diagrams for all cvalues. • Easily queried for any given c, without the need to construct theVV(c)-diagram. • Constructed inO(n2 log n) time, handling (n2) events.**The VV-Complex (II)**Suppose the clearance value grows from 0 to . We can associate with each edge e we encounter a validity rangeR(e) = [cmin(e), cmax(e)] of c-values for which this edge is valid. The VV-complex of the scene of polygonal obstacles P1, … , Pmis therefore: • The Voronoi diagram V of the obstacles. • A set T of interval trees: • For each obstacle vertex u, T(u)stores the incident edgesto u indexed by their validity range • Similarly, there is also a tree T(p) for each chain point p.**Bitangents to Dilated Vertices**uvll v uvrl uvlr u uvrr For each obstacle vertex u we keep two circular lists Ll(u) and Lr(u) of “left” and “right” bitangents, sorted by their slopes.**Computing the VV-Complex (Initialization)**• Compute the visibility graph of the obstacles P1, … , Pm. • Examine each bitangent edge in the visibility graph: Assign 0 to be the minimum value of its validity range. • Initialize an empty event queue Q . • Construct Ll(u) and Lr(u) for each obstacle vertex u. • Compute the initial visibility events based on adjacencies in these lists. • Compute the Voronoi diagram of P1, … , Pm. • For each Voronoi arc a that contains the minimum clearance value cmin of its chain a, insert the chain eventcmin, a into Q .**v**uvll w uwlr u v uvrl w uwrl u Visibility Events • Occur when two edges uv and uw become equally sloped. • We assign a maximal value for the validity range of the blocked edges. • Some edges may become valid, and we assign a minimal value for their validity ranges. • There are (n2) visibility events, each takes O(log n) to handle.**a**a a p2(a) p1(a) Chain Events • Occur when a chain start appearing in the VV(c)-diagram.We create two chain points associated with this chain. • There are (n) chain events, each takes O(n log n) to handle. • The motion of the chain points along the chain causestangency events and endpoint events. • All these events are handled at O(n2 log n) time in total.**Querying the VV-Complex**Given a start configuration s, a goal configuration g and a preferred clearance value c’: • For each Voronoi chain compute the chain points (two at most) that correspond to the given c-value. • Perform radial sweep from s and from g and obtain their incident visibility edges. • Execute Dijkstra’s algorithm from s. The graph is implicitly maintained, as we obtain the incident edges of each vertex x we encounter from T(x). We do this until reaching g. The total query time is O(nlog n + k), where k is the number of edges encountered during the search.**Implementation and Experiments**We have implemented a CGAL-based application that can robustly construct the VV(c)-diagram for a given c-value. We employ: • CGAL’s segment Voronoi diagram package (Karavelas). • CGAL’s arrangement package with the conic-arc traits (W.). • The GMP and CORE number types. Diagram construction takes 3–60 seconds. Query time is 0.02—0.1 seconds (compared with 0.5–1 seconds that a smoothing phase would consume).**The Concept of 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 corridorC = (t), w(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 corridorC = (t), w(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.**Lemma: The backbone path of an optimal corridor connecting**any given start and goal positions s and g is smooth. p 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).**Refraction of an Optimal Path**• Snell’s Law: If the width function is not continuous, then the backbone path of the optimal corridor connecting p1 and p2 satisfies: p1 1 w1 w2 2 p2**Optimal Corridors amidst Point Obstacles**• We consider planar environments cluttered with point obstacles p1, …, 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|| 2wmax). • We first examine what happens when the two endpoints are both in the vicinity of a single obstacle p (inside B(p; wmax)).**1**The Law of Sines on pAB: Hence: 1 = 1. B Near a Single Point Obstacle Using Snell’s Law: p r2 1 A 2 r1**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):**Proposition: Given a single point obstacle located at the**origin, a start position and a goal position , wherers, rg wmax , the backbone of the optimal corridor connecting s and g is an arc of the logarithmic spiral , where: The Optimal Backbone Near a Single Point Obstacle If r0 = rs = rg then we have: a* = r0 , b* = 0 (a circle)**g***g’ s* p p s’ wmax wmax 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 intersectB(p; wmax), it is the optimal backbone. g • Otherwise, the optimal backbonecomprises the tangent ss*, the circulararc s*g* and the tangent g*g. s**The Visibility Graph**• Given a set O = {p1, …, pn} of point obstacles in the plane that are well-separated with respect to wmax , we use the visibility graph of the discs B(p1; 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 inO(E log n) time.**The Bounded Voronoi Diagram**• The bounded Voronoi diagramÛ (O) of a point set O w.r.t. wmax comprises 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.**In the bounded Voronoi cell of pi the optimal backbone path**between and is a spiral arc a whose weighted length is given by: Optimal Paths within Cells • In a cell whose clearance is above wmax , optimal backbone paths comprise line segments and circular arcs.The weighted length of each element equals its “normal” length. • Voronoi edges are also locally optimal.**q2**q’ q1 q2 q2 q1 q1 Moving amidst Dense Obstacles • We have to consider spiral arcs that shortcut sharp turns (Voronoi vertices and reflex vertices): pi pi pi v2 v1 v1 v2 pj v • In the worst case we have to consider shortcuts of whose complexity is (n), which take exponential time to compute.**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 I1 and I2 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(n2 / 2) time.**The Approximation Scheme (III)**• Given two endpoints, we can connect them to the graph D inO(n / ) time and use Dijkstra’s algorithm to obtain an approximation for the optimal backbone path * in O(2 / 2) time. • Lemma: If * comprises the path segments 1, …, k (where k = O(n)), then for each i there exists an edge e in Dsuch that: L*(e) L*(i) + 22 . • 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 obstaclesP = {P1, …, Pk} havingnvertices in total. • If the polygons are well-separated w.r.t. wmax , we can use the visibility graph of the dilated obstacles to answer corridor-planning queries.**The Bounded Voronoi Diagramfor 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.**3**2 1 Note that we may have a reflection point! Moving Near a Polygon Edge (I) Using Snell’s Law: 0 p0= (x0,0) x = 0**Let us now write:**Let x 0: Moving Near a Polygon Edge (II) We obtained an equation of a circle whose center lies on x = 0.**The weighted length of this arc is given by (**and w.r.t. the circle center): 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:**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(n2 / 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) .