Download
1 / 44
440 likes | 558 Views
Planning High-Quality Paths and Corridors. Ron Wein Jur P. van den Berg (U. Utrecht) Dan Halperin. Part I : The Visibility-Voronoi Complex and Its Applications. Motivation.
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) .
