Click here to load reader

Probabilistic Motion Planning Shai Hirsch May, 2002

Embed Size (px)

DESCRIPTION

Outline Introduction Probabilistic Scheme Probabilistic Roadmap Planner Analysis Improved Solutions

Citation preview

Probabilistic Motion Planning Shai Hirsch May, 2002 Outline Introduction Probabilistic Scheme Probabilistic Roadmap Planner Analysis Improved Solutions Outline Introduction Probabilistic Scheme Probabilistic Roadmap Planner Analysis Improved Solutions Exact General-Purpose Path Planners - Schwarz and Sharir, 1983: Exact cell decomposition based on Collins technique - Canny, 1987: Silhouette method Complete Solutions Small #dofs Tailored solutions for specific problems. Example: Non convex polygon (k sides) Translating and rotating Planar polygonal environment (O(n)). Constructing F in ((kn) 3 log kn) Single query in O((kn) 2+ ), for any > 0. Complete Solutions Advantages: Complete Specifically, handles narrow / tight passages. Bounded running time. Behavior well characterized. Insight Complete Solutions Disadvantages High complexity Hard to understand Hard to implement Inflexible to changing scenarios (different geometrical settings) Complete Solutions Existing complete algorithms become practically useless unless #dofs is low (between 3 and 5). Complete solutions cannot be used for rigid objects translating and rotating is 3D. Nor for 6-dof manipulator arms. Real Applications Many dofs Problems may be more complex Extensions to the Basic Problem Moving obstacles Multiple robots Movable objects Deformable objects Goal is to gather data by sensing Nonholonomic constraints Dynamic constraints Optimal planning Uncertainty in control and sensing Application: Graphic Animation of Digital Actors James Kuffners PhD Application: Design for Manufacturing General Electric General Motors Application: Computer-Assisted Surgical Planning Rhea Tombropouloss PhD Joel Browns PhD Application: Prediction of Molecular Motions Amit Singhs PhD Planner Wish List Complete Handles narrow / tight passages. Bounded running time. Behavior well characterized. Low complexity Not too hard to understand Not too hard to implement Scalable Outline Introduction Probabilistic Scheme Probabilistic Roadmap Planner Analysis Improved Solutions Principle of Randomized Planning free space qbqbqbqb qgqgqgqg milestone [Kavraki, Svetska, Latombe,Overmars, 95] (Probabilistic Roadmap) Probabilistic Scheme General Idea Trade a limited amount of completeness against a major gain in computing efficiency. Probabilistic Scheme Probabilistic Completeness If a solution path exists, the planner will find one in bounded time, with high probability. (weaker completeness but still interesting.) Probabilistic Scheme Probabilistic completeness become particularly interesting if ? the running time grows slowly with the inverse of the failure probability. Probabilistic Scheme n#dofs A holonomic robot C configuration space C free free space q init, q goal query configurations Probabilistic Scheme C is the cube [0, 1] n. Each configuration is an n-tuple (q 1, , q n ). Probabilistic Scheme C free is implicitly given by a function CLEARANCE: C Maps any configuration q to the distance of the robot placed according to q and the obstacles two bodies of the robots (whichever is smaller) Probabilistic Scheme C free is implicitly given by a function CLEARANCE: C For complete planners, we computed an complete and explicit representation of C free. With prms, we only get one glimpse at a time of C free. Probabilistic Scheme CLEARANCE(q) is the clearance of q. In case where the robot collides with an obstacle or with itself, CLEARANCE(q) returns 0 or a negative number. CLEARANCE(q) > 0 implies q C free. (Collision Detection) Probabilistic Scheme Maximal Curve Tracing Constant robot A positive constant s.t., when A moves along a straight line in C between any two configurations q and q No point of A traces a curve segment longer than max|q i q i | Probabilistic Scheme from ( 1, 2 ) to ( 1, 2 ) Probabilistic Scheme Adjacency Configurations q, q with clearances , , resp., are adjacent if max|q i q i | < max { , }. Probabilistic Scheme 22 11 Clearly, if two configurations are adjacent, then the straight line joining them lies entirely in C free. Why the coefficient in max|q i q i | < max { , } ? Probabilistic Scheme = = d = 1 = = d, = 1 max|q i q i | < d q q d Probabilistic Scheme Note that the map C C max |q i q i | is in the L distance in C. We could choose other distances in C to define adjacency. Of course, the constant depends on this choice. (Choice of Metric) (Choice of Metric) Probabilistic Scheme 1. Pick configurations in C at random; retain configurations in C free ( along with q-init and q-goal ) as nodes of graph G ; connect adjacent configurations by links of G. 2. Return YES if q-init and q-goal belong to the same connect component of G. 3. Return NO if no path has been found after having generated c configurations ( c an input parameter). Probabilistic Scheme Then answer YES is always correct Then answer YES is always correct whenever the planner returns YES, there actually exists a collision-free path connecting q-init to q-goal. Probabilistic Scheme Indeed - after a finite amount of computation (defined by the parameter c) - the planner may still not have found a path between q-init and q-goal, even if one exists. Probabilistic Scheme The key component of this planning scheme is the sampling strategy. (begin with uniform) Probabilistic Scheme We are interested in the multi-query case so we will invest preprocessing time to construct a roadmap of configurations. Toward an Analysis Efficiency may be evaluated through experimentation. Formal analysis is desirable to compare planners to stress their strengths and weaknesses. Ideally a planner's outcome should be YES with the highest probability whenever a free path exists. This outcome should be generated in minimal time. Toward an Analysis Analysis: relate probability of an incorrect answer to running time. Given a small positive number , bound planner running time by a function of . Toward an Analysis What relation makes a planner good? ? If function grows slowly with 1/ , the planner is particularly interesting. We get arbitrarily close to full completeness at a reasonable cost. Toward an Analysis Assume for simplicity that CLEARANCE takes constant time. Is that a realistic assumption? ? Toward an Analysis Assume for simplicity that CLEARANCE takes constant time. Running time mainly depends on ? # of sample confs. # of pairs checked for adjacency Toward an Analysis 1. # sample configurations c. 2. Only a fraction of them, f, are in C free, hence in the constructed graph G. 3. In practice, often f c. The planner should strive to get the greatest ration f/c But, the distribution of the generated free configurations is also crucial to eventually find a path. Toward an Analysis # adjacency checks is at most proportional to f 2. However, the planner may choose not to test all pairs of free configurations for adjacency. Toward an Analysis It seems likely that no strong property can be proven for any given planner, if we do not make some assumption about C free. Moreover, no single planner is likely to be the most efficient for all possible problems. Moreover, no single planner is likely to be the most efficient for all possible problems. This suggests that planners should be analyzed under some well specified assumptions. Outline Introduction Probabilistic Scheme Probabilistic Roadmap Planner Analysis Improved Solutions Probabilistic Roadmap Planner Preprocessing Constructing a network R of configurations Query Processing Connecting q init, q goal to two milestones. Checking of these two milestones are in the same connected component of R. Probabilistic Roadmap Planner Configurations in R are called milestones. The milestones are a subset of all sample configurations generated. (The roadmap is not exactly G from the scheme.) Probabilistic Roadmap Planner Planner uses simple algorithm Connect to construct the links of R. Connector: Given two milestones m1 and m2, is the straight-line segment connecting them in C free. Dichotomically breaking the segments into shorter sub-segments. Stops breaking when a pair of milestones are adjacent, or if one of them is not free. Probabilistic Roadmap Planner For simplicity, assume that connect only tries straight paths. It could try other canonical paths as well. A configuration sees another configuration if the straight-line segment joining them lies entirely in C free. Probabilistic Roadmap Planner We limit the number of generated milestones to c. We construct an initial roadmap containing r milestones. We then expand the initial roadmap into a final one containing s > r milestones. s and r are input parameters. Probabilistic Roadmap Planner The first r milestones are chosen uniformly over C free. The remaining s-r milestones are selected in small regions considered difficult parts of C free. Probabilistic Roadmap Planner 1. i 0 2. While i < r do: (a) Pick a configuration q in C at (a) Pick a configuration q in C at random random (b) If CLEARANCE(q) > 0 then (b) If CLEARANCE(q) > 0 then i. Store q as a milestone of the roadmap i. Store q as a milestone of the roadmap ii. i i For every pair of milestones m1 and m2, whose distance is less than d, do: Connect(m1, m2). 4. Invoke Resample to expand the roadmap by s-r milestones. Input parameter quadratic in r, usually, r 0 then (b) If CLEARANCE(q) > 0 then i. Store q as a milestone of the roadmap i. Store q as a milestone of the roadmap ii. i i For every pair of milestones m1 and m2, whose distance is less than d, do: Connect(m1, m2). 4. Invoke Resample to expand the roadmap by s-r milestones. 4. Pick a representative milestone from each component of the current roadmap. (V := set of representatives). Invoke Permeation(V) to improve the connectivity. Analysis The results of preprocessing: Two nodes are in the same component iff the corresponding milestones are in the same component of Cfree. This is theoretical and assumes Permeation is complete. In practice this is not the case. Analysis Old-Query-Processing (q-init, q-goal) 1. For i = {init, goal} do: (a) If there exists a milestone m that sees q(i) (a) If there exists a milestone m that sees q(i) then m(i) m then m(i) m (b) Else (b) Else i. Repeat g times Pick a configuration q at random in the Pick a configuration q at random in the neighborhood of q(i) until q sees both q(i) and a neighborhood of q(i) until q sees both q(i) and a milestone m. milestone m. ii. If all g trails failed then return NO and halt, ii. If all g trails failed then return NO and halt, else m(i) m else m(i) m 2. If m-init and m-goal are in the same component of the roadmap then return YES; else return NO. roadmap then return YES; else return NO. Analysis New-Query-Processing (q-init, q-goal) 1. For i = {init, goal} do: (a) If there exists a milestone m that sees q(i) (a) If there exists a milestone m that sees q(i) then m(i) m then m(i) m (b) Else (b) Else i. Repeat g times Pick a configuration q at random in S(q(i)) Pick a configuration q at random in S(q(i)) until q sees both q(i) and a milestone m. until q sees both q(i) and a milestone m. ii. If all g trails failed then return FAILURE and ii. If all g trails failed then return FAILURE and halt, else m(i) m halt, else m(i) m 2. If m-init and m-goal are in the same component of the roadmap then return YES; else return NO. roadmap then return YES; else return NO. PrePro Performance Guarantee A set of milestones M is Adequate ( C free - S(M)) C free If each milestone were a source of light, we would like a fraction of at least 1- of C free to be illuminated. PrePro Performance Guarantee Let K be a positive constant large enough that for any x (0, 1], (1-x) (K/x)(log1/x + log2/ ) x /2 THEOREM 1 (0, 1]; If s (K/ )(log1/ + log 2/ ), then preprocessing generates an adequate set of milestones with probability at least 1- . (0, 1]; If s (K/ )(log1/ + log 2/ ), then preprocessing generates an adequate set of milestones with probability at least 1- . PrePro Performance Guarantee By Theorem 1: most of C free is likely to be visible from some milestones in the roadmap. Queries can be answered quickly. Adequacy is not sufficient to imply that the roadmap is a good representation of the connectivity of C free. The use of the complex planner is inevitable to ensure a good probability that the query processing outcomes YES or NO. Query Performance Guarantee [0,1] is the failure probability we are willing to tolerate during a query. g = log (2/ ) THEOREM 2 If the set of milestones chosen during preprocessing is adequate, then the probability that the query-processing algorithm outputs FAILURE is at most . Query Performance Guarantee Expected number of sample loop iterations in the query-processing algorithm is at most 2. Query Performance Guarantee Theorems 1 and 2 fit the multi-query case. We blend them for a single query; We bound the probability for a FAILURE by (1- ) + Analysis Permeation determines which milestones in V are reachable from each other. Partitions V into subsets All milestones in the same subset belongs to the same component of Cfree. No two milestones in different subsets are in the same component of Cfree. Analysis If p=|V|, there are O(p 2 ) invocations of the complex planner. Far fewer may suffice! Analysis Abstract version of Permeation Input is a graph N with p vertices K disjoint cliques determine the clique partition Adjacency matrix representation cost is measured by the number of examined entries Edge probe model (used in evasive graph properties). Analysis NC(p, k) - the non-deterministic complexity of this problem. Only verify some partition is the right one. At least one probe on each of the p vertices. Verify each of the pairs (k above 2) of cliques is in fact disconnected THEOREM 3: THEOREM 3: For 1kp, NC(p, k) = (p + k 2 ) Analysis Deterministic-Permeation(V): 1. Mark all vertices of N as being LIVE 2. Initialize x While x p do: (a) G(x) { } (a) G(x) { } (b) For y = x + 1 to p do: (b) For y = x + 1 to p do: i. If vertex y is marked LIVE then probe the edge (x, y) in N. ii. if edge (x, y) is probed and found present the mark y as DEAD and ii. if edge (x, y) is probed and found present the mark y as DEAD and add y to G(x). add y to G(x). (c) Output {x} Union G(x) as being a clique (c) Output {x} Union G(x) as being a clique (d) Mark x as being DEAD (d) Mark x as being DEAD (e) Set x to the smallest numbered LIVE vertex, of p+1 if there are no (e) Set x to the smallest numbered LIVE vertex, of p+1 if there are no LIVE vertices left. LIVE vertices left. Analysis T(p, k) - worst-case deterministic complexity THEOREM 4: Deterministic-Permeation correctly solves the permeation problem using O(pk) probes. Theorem 5: For 1kp, T(p, k) = (pk). Analysis Randomized-Permeation(V): 1. Permute the vertices randomly. Rename the nodes by 1,..., n in the order of generated list. 2. Invoke Deterministic- Permeation(V). Permeation(V). Analysis Worst case when all cliques are about p/k Worst case is O(pk) Best case when there is one giant clique and all others are singletons. Best case is (p + k 2 ) Other Assumptions Path Clearance Assumption Expansiveness Outline Introduction Probabilistic Scheme Probabilistic Roadmap Planner Analysis Improved Solutions Penetration Allow some penetration distance of the robot into the obstacle (dilated C free ) Push milestones and links into C free. Rationale: Dilatation widens narrow passages Easier to capture free space connectivity Gaussian Rationale: Samples needed in difficult regions We want the probability to depend on the amount of forbidden configurations nearby. Define a Gaussian on the configuration space. Gaussian Avoid computations in the configuration space (PRM paradigm). In practice Sample pairs of configuration such that the second is at distance d from the first. Discard pairs where both are free or both are forbidden. From remaining pairs, keep free configuration. Gaussian Gaussian Uniform Sampling Gaussian Gaussian Sampling OBPRM Obstacle Based Probabilistic Roadmap Method Heuristics for generating nodes on or near C-obstacles. OBPRM Prototype Node-Generation 1. c in := colliding robot cfg with C-obstacle X C-obstacle X 2. D := m random directions emanating out from c in c in 3. for each d in D c out := free cfg in direction d (if exists) c out := free cfg in direction d (if exists) find contact cfg on (c in,c out ) by binary search find contact cfg on (c in,c out ) by binary search endfor endfor EOF