Probabilistic Motion Planning: Algorithms and Applications

Transcription

1 Probabilistic Motion Planning: Algorithms and Applications Jyh-Ming Lien Department of Computer Science George Mason University

2 Motion Planning in continuous spaces (Basic) Motion Planning (in a nutshell): Given a movable object, find a sequence of valid configurations that moves the object from the start to the goal. start The Alpha Puzzle Swapping Cubes Puzzle goal obstacles

3 Hard Motion Planning Problems Highly Articulated (Constrained) Systems Paper Folding Articulated robot Polyhedron: 25 dof Line: 30 dof

4 Hard Motion Planning Problems Highly Articulated (Constrained) Systems Digital Actors Closed Chain System Reaching and grasping

5 Hard Motion Planning Problems Flocking: Covering, Homing, Shepherding Motion for coordinated entities Control the motion of coordinated entities

6 Hard Motion Planning Problems Deformable Objects Find a path for a deformable object that can deform to avoid collision with obstacles move a mattress in a house, elastic or air-filled objects, metal sheets or long flexible tubes virtual surgery applications computer animation and games Issue: difficult to find natural deformation efficiently

7 Hard Motion Planning Problems Intelligent CAD Applications Using Motion Planning to Test Design Requirements: Accessibility for servicing/assembly tested on physical mock ups Digital testing saves time and money, is more accurate, enables more extensive testing, and is useful for training (VR or e-manuals) Maintainability Problems: Mechanical Designs from GE flange Airplane engine

8 Hard Motion Planning Problems computational biology & chemistry Motion of molecules help understand important interactions - protein structure/function prediction diseases such as Alzheimer s and Mad Cow are related to misfolded proteins prion protein normal - misfold

9 Outline The basic ideas of motion planning Configuration space (C-Space) C-Space obstacles (C-obstacle) Motion planning in C-Space Probabilistic Roadmap Methods (PRM) Tradition PRM and the Narrow passage problem Obstacle-based PRM Gaussian PRM Medial axis PRM Feature-based PRM

10 Workspace Pickup the package Robot Fixed Base Two Joints Fixed length links Package

11 Workspace

12 Workspace

13 Workspace

14 Workspace

15 Workspace

16 Workspace

17 Workspace Degree of freedom (DOF) α β

18 Configuration Space C-Space β= β α= α C-Space 180

19 C-Space β= β 10 0 α= α C-Space 180

20 C-Space β= β 80 α= α C-Space 180

21 C-Space β= β α= α C-Space 180

22 C-Space β= β α= α 110 C-Space 180

23 C-Space β= C-Space β α= α C-Space 180

24 C-Space set of all robot placements Robot maps to a point (in usually higher dimensional space) Parameter for each degree of freedom (dof) of robot Each point in C-Space corresponds the robot s position and orientation in workspace 180 C-Space β 0 α 180

25 Workspace Robot Moving in -Y Rotating

26 Workspace x Θ Moving in -Y Rotating y

27 Theta Workspace vs. C-Space Workspace C-Space

28 Theta Workspace vs. C-Space Workspace C-Space

29 Workspace Obstacle Workspace Workspace theta (x,y) obstacle Configuration (x,y,theta) (4,5,45)

30 Thet a C-Space Obstacle C-Obstacle

31 Theta Finding a Path Find a path in workspace for a robot Find a path in C-space for a point Goal Initial

32 Motion Planning in C-space Simple workspace obstacle transformed Into complicated C-obstacle!! Workspace C-obst C-space C-obst! obst obst obst obst robot Path is swept volume C-obst C-obst x robot Path is 1D curve y

33 Configuration Space (C-Space) C-Space C-obst C-obst C-obst C-obst C-obst Robot maps to a point in higher dimensional space Parameter for each degree of freedom (dof) of robot C-space = set of all robot placements C-obstacle = infeasible robot placements 3D C-space (x,y,z) 6D C-space (x,y,z,pitch,roll,yaw) β α γ 3D C-space (α,β,γ) 2n-D C-space (φ 1, ψ 1, φ 2, ψ 2,..., φ n, ψ n)

34 The Complexity of Motion Planning General motion planning problem is PSPACE-hard [Reif 79, Hopcroft et al. 84 & 86] PSPACE-complete [Canny 87] The best deterministic algorithm known has running time that is exponential in the dimension of the robot s C-space [Canny 86] C-space has high dimension - 6D for rigid body in 3-space simple obstacles have complex C-obstacles impractical to compute explicit representation of freespace for more than 4 or 5 dof PSPACE So attention has turned to randomized algorithms which trade full completeness of the planner for probabilistic completeness and a major gain in efficiency NP P

35 Theta Probabilistic Roadmap Method [Kavraki, Svestka, Latombe,Overmars 1996] unknown Roadmap Construction (Pre-processing)

36 Theta Probabilistic Roadmap Method Roadmap Construction (Pre-processing) 1. Randomly generate robot configurations (nodes) - discard nodes that are invalid

37 Probabilistic Roadmap Method Roadmap Construction (Pre-processing) Theta 1. Randomly generate robot configurations (nodes) - discard nodes that are invalid 2. Connect pairs of nodes to form roadmap - simple, deterministic local planner - e.g., straight-line - discard connections that are invalid

38 Probabilistic Roadmap Method Roadmap Construction (Pre-processing) Theta 1. Randomly generate robot configurations (nodes) - discard nodes that are invalid 2. Connect pairs of nodes to form roadmap - simple, deterministic local planner (e.g., straightline) - discard paths that are invalid Query processing 1. Connect start and goal to roadmap 2. Find path in roadmap between start and goal - regenerate plans for edges in roadmap

39 Probabilistic Roadmap Method C-space goal C-obst C-obst C-obst C-obst Roadmap Construction (Pre-processing) 1. Randomly generate robot configurations (nodes) - discard nodes that are invalid 2. Connect pairs of nodes to form roadmap - simple, deterministic local planner (e.g., straightline) - discard paths that are invalid start C-obst Query processing 1. Connect start and goal to roadmap 2. Find path in roadmap between start and goal - regenerate plans for edges in roadmap

40 PRMs: Pros & Cons goal C-obst C-obst C-obst C-obst PRMs: The Good News 1. PRMs are probabilistically complete 2. PRMs apply easily to high-dimensional C-space 3. PRMs support fast queries w/ enough preprocessing start C-obst Many success stories where PRMs solve previously unsolved problems goal PRMs: The Bad News C-obst C-obst C-obst C-obst 1. PRMs don t work as well for some problems: unlikely to sample nodes in narrow passages hard to sample/connect nodes on constraint surfaces start

41 Related Work (selected) Probabilistic Roadmap Methods Uniform Sampling (original) [Kavraki, Latombe, Overmars, Svestka, 92, 94, 96] Obstacle-based PRM (OBPRM) [Amato et al, 98] PRM Roadmaps in Dilated Free space [Hsu et al, 98] Gaussian Sampling PRMs [Boor/Overmars/van der Steppen 99] PRM for Closed Chain Systems [Lavalle/Yakey/Kavraki 99, Han/Amato 00] PRM for Flexible/Deformable Objects [Kavraki et al 98, Bayazit/Lien/Amato 01] Visibility Roadmaps [Laumond et al 99] Using Medial Axis [Kavraki et al 99, Lien/Thomas/Wilmarth/Amato/Stiller 99, 03, Lin et al 00] Generating Contact Configurations [iao et al 99] Single Shot [Vallejo/Remmler/Amato 01] Bio-Applications: Protein Folding [Song/Thomas/Amato 01,02,03, Apaydin et al 01,02] Lazy Evaluation Methods: [Nielsen/Kavraki 00 Bohlin/Kavraki 00, Song/Miller/Amato 01, 03] Related Methods Ariadnes Clew Algorithm [Ahuactzin et al, 92] RRT (Rapidly Exploring Random Trees) [Lavalle/Kuffner 99]

42 An Obstacle-Based PRM To Navigate Narrow Passages we must sample in them most PRM nodes are where planning is easy (not needed) PRM Roadmap OBPRM Roadmap goal goal C-obst C-obst C-obst C-obst C-obst C-obst C-obst C-obst start start Idea: Can we sample nodes near C-obstacle surfaces? we cannot explicitly construct the C-obstacles... we do have models of the (workspace) obstacles...

43 Finding Points on C-obstacles 2 1 C-obst Basic Idea (for workspace obstacle S) 1. Find a point in S s C-obstacle (robot placement colliding with S) 2. Select a random direction in C-space 3. Find a free point in that direction 4. Find boundary point between them using binary search (collision checks) Note: we can use more sophisticated heuristics to try to cover C-obstacle

44 Gaussian Sampling PRM 1. Find a point in S s C-obstacle (robot placement colliding with S) d 1 C-obst 2. Find another point that is within distance d to the first point, where d is a random variable in a Gaussian distribution Note 2 3. Keep the second point if it is collision free Two paradigms: (1) OBPRM: Fix the samples (2) Gaussian PRM: Filter the samples None of these methods can (be proved to) provide guarantee that the samples in the narrow passage will increase!

45 Medial Axis PRM (MAPRM) Intuitively, points on the medial axis are points that are farthest away from the boundary Line segments and parabolic curves 3D medial axis is hard to compute

46 Medial Axis PRM (MAPRM) It is easier to sample on the Medial axis Property: Points on the Medial axis has more than one closest point to the boundary 3 closest points 1 closest point

47 Medial Axis PRM (MAPRM) Given a point that is not on the Medial axis, we can always retract the point to the medial axis 1. Push the point away from the closest boundary point 2. until the point has more than one closest boundary points

48 Medial Axis PRM (MAPRM) Sample a Configuration, p p is collision-free p is in collision q = NearestContactCfg_Clearance(p) V = p - q q = NearestContactCfg_Penetration(p) V = q - p Retract p to the Medial Axis of the free C-space in direction V samples < N Connect sampled configurations

49 Medial Axis PRM (MAPRM) 1000 samples 1000 samples PRM MAPRM

50 Medial Axis PRM (MAPRM) Sampling is increased in the narrow passage In-collision configurations are retracted to free C- space The volume of the narrow passage is increased Pro( Sampling in S ) = Vol(S )+Vol(B ) Vol(C )

51 Machine Learning for Feature-Sensitive MP Randomized methods Many available Strengths and weaknesses in problems with different features open clutter Algorithm less more most Basic PRM Kavraki,Sveska,Latombe,Overmars 96 Fuzzy PRM Nielsen, Kavaki 00 Lazy PRM Bohlin, Kavraki 00 RNG Yang, LaValle 00 OBPRM Amato, Wu 96 GaussPRM Boor, Overmars, van der Stappen 99 Visibility Rdmp Laumond, Simeon 00 MAPRM Wilmarth, Amato, Stiller 99 Ariadne s Clew Bessiere, Ahuactzin, Talbi, Mazer 93 RRT LaValle, Kuffner 99 Dilated Spaces Hsu,Kavraki,Latombe,Motwani,Sorkin 98 ClosestVE (surfaces) Dale, 00 User Input Bayazit, Song, Amato 00 narrow passage cluttered free isolated blocked passage free

52 Machine Learning for Feature-Sensitive MP Overview of the algorithm Subdivide C-space into regions Classify from features into free, cluttered, or narrow passage In each subdivision create a partial roadmap Integrate solutions Decision Tree classes Free nodes: black Unfeasible nodes: yellow Successful connections: black Unsuccessful connections: yellow

53 Conclusion Motion planning has many applications Motion planning is difficult (intractable) Probabilistic Motion Planners PRM and the Narrow passage problem OBPRM Gaussian PRM MAPRM Feature-based PRM There are still many problems to be answered Differential constrains Uncertainty Deterministic vs. Probabilistic Dynamic environments