Motion Planning: Probabilistic Roadmaps. Corso di Robotica Prof. Davide Brugali Università degli Studi di Bergamo

Motion Planning: Probabilistic Roadmaps Corso di Robotica Prof. Davide Brugali Università degli Studi di Bergamo

Tratto dalla lezione: Basic Motion Planning for a Point Robot CS 326A: Motion Planning ai.stanford.edu/~latombe/cs326/2007/index.htm Prof. J.C. Latombe Stanford University

Motion-Planning Framework Continuous representation Discretization Graph searching (blind, best-first, A*) 3

Configuration Space b a b a 2R manipulator Configuration space 4 topology

Configuration Space 360 b 270 180 q robot a 90 b Two points in the robot s workspace 0 q slug 45 a Torus 90 135 180 (wraps horizontally and vertically) 5

Configuration Space If the robot configuration is within the blue area, it will hit the obstacle 360 b 270 180 q robot An obstacle in the robot s workspace a 90 b 0 q slug 45 a 90 135 180 a C-space representation 6

Configuration Space: Tool to Map a Robot to a Point 7

Motion Planning Revisit Find a collision free path from an initial configuration to goal configuration while taking into account the constrains (geometric, physical, temporal) C-space concept provide a generalized framework to study the motion planning problem A separate problem for each robot? 8

Completeness of Planner A motion planner is complete if it finds a collision-free path whenever one exists and return failure otherwise. Exact cell decomposition, navigation function, Visibility graph, and Voronoi diagram provide complete planners Weaker notions of completeness, e.g.: - resolution completeness (Potential Fields with best-first search) - probabilistic completeness (Potential Fields with random walks) 9

Completeness of Planner A probabilistically complete planner returns a path with high probability if a path exists. It may not terminate if no path exists. A resolution complete planner discretizes the space and returns a path whenever one exists in this representation. 10

Weaker Completeness Complete path planning in high-dimensional C- spaces is too complex Boost performance by trading completeness for probabilistic completeness Probabilistic completeness: If there is a solution path, the probability that the planner will find is a (fast growing) function that goes to 1 as running time increases 11

Probabilistic Roadmap (PRM) A probabilistic road map is a discrete representation of a continuous configuration space generated by randomly sampling (milestones) the free configurations of the C-space and connecting those points into a graph. milestone free space mg mb 12 [Kavraki, Svetska, Latombe,Overmars, 95]

Two Tenets of PRM Planning Checking sampled configurations and connections between samples for collision can be done efficiently. Hierarchical collision checking A relatively small number of milestones and local paths are sufficient to capture the connectivity of the free space. Exponential convergence in expansive free space (probabilistic completeness) 13

PRM: Description Roadmap = undirect graph R = (N, E ) N : (nodes) set of selected configurations in Cfree E : (edges) collection of simple paths (the Local Paths). Local paths are computed by the fast but not powerful local planner Search R for a path 14

Learning Phase Three stages 1. Roadmap construction. Objectives: a) Obtain reasonable connected graph b) Be sure difficult regions contain a few nodes 2. Roadmap expansion. Objectives: Improve graph connectivity by selecting nodes of R which lie in (heuristic) difficult regions and adding nodes there 3. Roadmap Component reduction. Optional. Attempts to simplify the graph 15

Learning Phase:Construction Construction Step Start with empty Roadmap R=(N,E) Generate a random free configuration c and add to N Choose a subset Nc of candidate neighbors around c from N Try to connect c to each of selected nodes* in Nc in the order of increasing distance from c (with local planner) * Select only the nodes not graph-connected to c. Hence no cycles can be created and the resulting graph is a forest, i.e. a collection of trees. Add the edge found to E Repeat the above until satisfied 16

Learning Phase:Construction 1.Creation of random free configurations The nodes of R should constitute a rather uniform random sampling of C-free. Every such configuration is obtained by drawing each of its coordinates from the interval of values of the corresponding dof using the uniform probability distribution over its interval. The obtained configuration is checked for collision*. If it s collision-free, it is added to N; otherwise it is discarded. * Collision checking requires testing if any part of the robot intersects an obstacle and if two distinct bodies of the robot intersect each other. 17

Learning Phase:Construction 2.Local Path Planner If a powerful planner is used, it would often succeed in finding a path when one exists. Hence, relatively few nodes would be required to build a roadmap capturing the connectivity of the free C-space. Such a local planner would probably be rather slow, but this could be somewhat compensated by the small number of calls needed. If a very fast planner is used, it is likely to be less successful. It will require more configurations to be included in the roadmap; so it will be called more often, but each call will be cheaper. If the local planner is very fast, we can use the same planner to conect the start and goal configurations to the roadmap at query time. 18

Learning Phase:Construction 3.The node neighbors Choice of Nc (the set of candidate neighbors of c) is important. The local planner will be called to connect c with nodes in Nc and the cumulative cost of these invocations dominates learning time. We avoid calls of the local planner that are likely to return failure by submitting only pairs of configurations whose relative distance is smaller than some constant threshold maxdist. Nc={q N / MaxDist D(q,q )} 19

Learning Phase: Example Construction Step Efficiency-driven Robots with many dofs (high-dim C-spaces) Collision! Static environments 20

Learning Phase: Expansion Expansion Step N should be a fair enough covering of Cfree R often consists of a few large components and several small ones Expansion = add nodes to form a large component with as many nodes as possible Find the nodes in difficult regions using specific sampling strategies Efficiency-driven Robots with many dofs (high-dim C-spaces) Static environments 21

Query Phase:Procedure Given the start and goal config s and g, calculate feasible paths Ps and Pg to the nodes s and g on the roadmap (with Local Planner) Recalculate the path P from s to g using the roadmap Return the total path: Ps P Pg -1 22

Rapidly-growing Random Trees BUILD-RRT constructs a tree of sample points in C free, which is rooted at initial configuration. The procedure RandomConfig returns a random point from C free BUILD-RRT(q init ) T.init(q init ) for i=1 to K do q next =RandomConfig() EXTEND(T,q next ) return T. The procedure EXTEND spans the tree as much as possible to given direction. The procedure NEW-CONFIG is a local movement planning function. EXTEND(T,q) q near =NEAREST-NEIGHBOR(T,q) If NEW-CONFIG(q,q near,q new ) Then T.addVertex(q new ) T.addEdge(q near,q new ) If q new =q Then return Reached Else return Advanced 24

Rapidly-growing Random Trees e q near q q new q init Configurations in the tree Goal configuration An obstacle EXTEND advances to the distance at most e 25

Motion Planning : Sampling strategies Corso di Robotica Prof. Davide Brugali Università degli Studi di Bergamo Tratto dall articolo: The Gaussian Sampling Strategy For Probabilistic Roadmap Planners Valerie Boor, Mark H. Overmars and A. Frank van der Stappen ICRA 1999

Motivation 1. Connectivity checks between milestones are expensive. Provide coverage with fewest possible milestones. 2. Collision checks to create milestones are cheap. Take many samples, keep only the best. Main idea: Sample many configurations, but retain only a small subset of promising ones 27

Motivation Narrow passages are always close to the free space boundary Goal: Identify and retain configurations sampled near the free space boundary Uniform Random Sampler The Sampler we want 28

Gaussian Sampler c 1 c 2 29

Gaussian Sampler c 1 c 2 30

Gaussian Sampler c 1 c 2 31

Gaussian Sampler c 1 c 2 32

Effect of the Parameter If we choose a very small standard deviation Require a lot of samples to generate sufficient number of surviving samples If we choose a very large standard deviation Almost uniformly distributed A lot of surviving samples are redundant. Tune the parameter based on the setting of the specific problem. 33

Experimental Results A U-shaped robot has to twist to get through the narrow gap in the center. The random sampler (which required 10000 nodes) took about 13 times longer than the Gaussian sampler (which only required 750 nodes). 5000(intersecting) obstacles. Gaussian sampler needed 85 nodes to connect start and goal. 4 times as fast as the Random sampler, which required over 450 nodes. 34

The Bridge Test for Sampling Narrow Passages with Probabilistic Roadmap Planners David Hsu, Tingting Jiang, John Reif, and Zheng Sun Presented by Michael Graeb

Motivations Review: Provide coverage with fewest possible milestones. Take many samples, keep only the best. Gaussian Sampling: Best samples are near boundaries Bridge Test: Milestones in narrow passages important. Not all milestones near boundaries increase coverage. Uniform Sampler Gaussian Sampler Bridge Test 36

The Bridge Test: Create samples in narrow passages while( ) pick a point x from configuration space at random if ( CLEARANCE( x ) == false ) pick a point x in the neighborhood of x if ( CLEARANCE( x ) == false ) point m is midpoint of x and x if ( CLEARANCE( m ) == true ) add m as new milestone 37

Bridge Test: Examples 4 loops of the algorithm, producing only 1 milestone 41

Bridge Test: Distribution of Samples x: uniformly random distribution 42 keep if CLEARANCE(x) == false

Bridge Test: Distribution of Samples x : gaussian distribution in neighborhood of x 43 keep if CLEARANCE(x ) == false

Bridge Test: Distribution of Samples m: midpoint of x and associated x 44 keep if CLEARANCE(m) == true

Bridge Test: Complementary sampling Bridge Test reliably provides milestones in narrow passages Uniform sampling reliably provides milestones in open spaces. A union of results from each algorithm provides good coverage with minimal milestones. Issue: Number of milestones allotted to each algorithm. U = 45

Motion Planning: KinoDynamic constraints Corso di Robotica Prof. Davide Brugali Università degli Studi di Bergamo Tratto da: CS 326 A: Motion Planning http://robotics.stanford.edu/~latombe/cs326/2004

Underactuated Robots Fewer controls than dimensions in configuration space What is a degree of freedom: number of dimensions of C-space (global) or number of controls (local)? How can m controls generate span a C-space with n>m dimensions? By exploiting mechanics properties: - Rolling-with-no-sliding contact (friction), e.g.,: car, bicycle, roller skate - Conservation of angular momentum: satellite robot - Why is it useful? - Fewer actuators: less weight, Design simplicity 47

Example: Car-Like Robot y L q f f dx sinq dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/l) tan f f < F x Configuration space is 3-dimensional: q = (x, y, q) But control space is 2-dimensional: (v, f) with v = sqrt[(dx/dt) 2 +(dy/dt) 2 ] 48

Example: Car-Like Robot f dx sinq dy cosq = 0 y L q x f dx/dt = v cos q dy/dt = v sin q dq/dt = (v/l) tan f f < F q = (x,y,q) q = dq/dt = (dx/dt,dy/dt,dq/dt) dx sinq dy cosq = 0 is a particular form of f(q,q )=0 A robot is nonholonomic if its motion is constrained by a nonintegrable equation of the form f(q,q ) = 0 49

Nonholonomic Path Planning Approaches Two-phase planning: Compute collision-free path ignoring nonholonomic constraints Transform this path into a nonholonomic one Efficient, but possible only if robot is controllable Plus need to have good set of maneuvers Direct planning: Build a tree of milestones until one is close enough to the goal (deterministic or randomized) Robot need not be controllable Works in high-dimensional c-spaces 50

Path Transform Holonomic path 51 Nonholonomic path

Type 1 Maneuver CYL(x,y,dq,h) dq h h= 2r/cosdq d = 2r(1/cosdq - 1) dq r Allows sidewise motion 52

Type 2 Maneuver Allows pure rotation 53

Drawbacks of Two-phase planning Final path can be far from optimal Not applicable to robots that are not locally controllable (e.g., car that can only move forward) 54

Control Lie Algebra Lie Bracket [X,Y] = Basic maneuver based on 4 motions -X -Y Y X (dt) 55

Control Lie Algebra Lie Bracket [X,Y] = Basic maneuver based on 4 motions For example: dx sinq dy cosq = 0 X: Going straight X cosq, sin q,0 Y: Turning, angle f Y cosq, sin q, L tan f dx/dt = v cos q dy/dt = v sin q dq/dt = (v/l) tan f f < F 56

Control Lie Algebra Lie Bracket [X,Y] = Basic maneuver based on 4 motions For example: X: Going straight X cosq, sin q,0 -Y -X Y Y: Turning, angle f Y cosq, sin q, L tan f [X,Y] (dt 2 ) X (dt) 57

Control-Based Sampling Previous sampling technique: Pick each milestone in some region Control-based sampling: 1. Pick control vector (at random or not) 2. Integrate equation of motion over short duration (picked at random or not) 3. The endpoint is the new milestone Tree-structured roadmaps Need for endgame regions 58

Control-Based Sampling endgame region m g m b 59

Example dx sinq dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/l) tan f f < F 1. Select a milestone m 2. Pick v, f, and dt 3. Integrate motion from m new configuration 60

Nonholonomic Constraints Nonholonomic constraint: q = f(q,u) where u is the control input (function of time), with dim(u) < dim(q) dx sinq dy cosq = 0 dx/dt = v cos q dy/dt = v sin q dq/dt = (v/l) tan f u = (v, f) f < F 61

Nonholonomic Constraints Nonholonomic constraint: q = f(q,u) where u is the control input (function of time), with dim(u) < dim(q) Dynamic constraint: s = (q,q ), the state of the system s = f(s,u) where u is the control input 62