Sampling-Based Robot Motion Planning. Lydia Kavraki Department of Computer Science Rice University Houston, TX USA

Transcription

1 Sampling-Based Robot Motion Planning Lydia Kavraki Department of Computer Science Rice University Houston, TX USA

2 Motion planning: classical setting Go from Start to Goal without collisions and while respecting all robot constraints.

3 Motion Planning Is done in a continuous world and with constrained motions. Needs to know robot and world geometry. Needs to know robot and world physics. Must be accurate and predictive to work in practice. Some notes: More powerful motion planning simplifies the task planner. More accurate motion planning simplifies motion execution. Motion planning is limited by model accuracy and complexity.

4 Motion planner is part of a replanning loop Bekris et al.

5 Motion planning is hard Problem Complexity Sofa Mover (3 DOF) O(n 2+ ε) not implemented Piano Mover (6 DOF) Polynomial no practical algorithm known n Disks in the Plane n Link Planar Chain Generalized Mover NP-hard PSPACE-Complete PSPACE-Complete Shortest Path for a Point in 3D NP-hard Curvature Constrained Point in 2D NP-hard Simplified Coulomb Friction Undecidable

6 Exact, approximate, and heuristic methods Method Advantage Disadvantage Exact theoretically insightful impractical Cell Decomposition easy does not scale Control-Based online, very robust requires good trajectory Potential Fields online, easy slow or fail Sampling-based fast and effective cannot recognize impossible query

7 Exact, approximate, and heuristic methods Method Advantage Disadvantage Exact theoretically insightful impractical Cell Decomposition easy does not scale Control-Based online, very robust requires good trajectory Potential Fields online, easy slow or fail Sampling-based fast and effective cannot recognize impossible query

8 Outline of this talk Basic concepts and definitions. Examples of sampling-based planners: Roadmap planner Tree-based planner Underlying key components. OMPL and future challenges in motion planning.

9 Outline of this talk Basic concepts and definitions. Examples of sampling-based planners: Roadmap planner Tree-based planner Underlying key components. OMPL and future challenges in motion planning.

10 Basic concepts and definitions Workspace Robot State space Path Planning Instance/Simple Setup Query/Problem

11 Workspace The workspace is the environment that the robot operates in. The boundary of the workspace determines the obstacles.

12 Robot The robot is defined by: Geometry Parameters or Degrees of Freedom (DOF) Different settings for the parameters embed the geometry in different ways into the workspace.

13 State space The parameter space for the robot is called the state space S. A point in this space is a state.

14 Free state space A state is free if the corresponding embedding of the robot s geometry lies in the workspace. The subspace of free configurations is free state space S free. S free can be very complex even for seemingly simple systems. This complexity is the main difficulty in motion planning.

15 Paths A path is continuous mapping in C :[0,L]! S free L is the length of the path. The path is collision-free if for all t (t) 2 S free

16 Planning instance/simple setup A planning instance consists of: Robot (S-space and embedding). Workspace. Constraints.

17 Query/Problem definition A problem or query is Given two states, q 0 and q f. PROBLEM: Determine if there is a collision-free path between q 0 and q f.

18 Outline of this talk Basic concepts and definitions. Examples of sampling-based planners: Roadmap planner Tree-based planner Underlying key components. OMPL and future challenges in motion planning.

19 Probabilistic Roadmap Planner (PRM) Kavraki, Svestka, Overmars and Latombe 96

20 PRM Uses random sampling. Uses simple local planner. Builds a roadmap of the state space.

21 PRM Illustrate with an easy planning instance/problem set up. Robot is a point in 2D. Robot moves freely. Simple example used for illustration only. Isolate primitive techniques. Generalize.

22 Point robot in 2-D a robot state

23 Operation of PRM : nodes, random states

24 Operation of PRM :edges, paths computed by local planner

25 Operation of PRM :edges, paths computed by local planner

26 Operation of PRM :edges, paths computed by local planner

27 Queries Given a roadmap G and query q 0, q f Connect q 0 and q f to G. Check to see if there is a path in G.

28 Answering Queries goal start plan a path: 1.connect start & goal to roadmap 2.perform graph search

29 Primitive Techniques Select Sample: (in the example) Uniform sampling to get milestones. Connect: (in the example) Local planner uses straight lines. Store in some data structure: (in the example) A graph. A roadmap is finite graph G=(V,E) S free V is a subset of. (s 1,s 2 ) in E implies that the local planner found a path.

30 Why use sampling? S free is impractical to represent explicitly. 360 β A B 270 q A 180 β α 90 0 q B 45 α Obstacle in workspace Obstacle in state space

31 Why use sampling? S free is impractical to represent explicitly. θ1 θ2 θ3 θ 3 θ 3 Obstacle θ 2 θ 1 TOP VIEW θ 1 θ 2

32 Why use sampling? S free is impractical to represent explicitly. Sampling can be very efficient. Resulting data structure can be very compact.

33 Connecting samples An example of a simple planner: - Computes the straight line path between q1, q2. - Checks to see if it is valid. - If so, returns SUCCESS and the path. - Otherwise, returns FAIL. May fail often

34 State validity checker For states Use e.g., collision checking, check any bounds For paths State validation along a path is done by recursive refinement. Bounds on clearance are combined with bounds on motion to cover the path with open balls or find a collision.

35 Operation of PRM : nodes, random states

36 Operation of PRM : states

37 Operation of PRM :edges, paths computed by local planner

38 Operation of PRM

39 Operation of PRM feasible path computed by local planner

40 Operation of PRM feasible path computed by local planner

41 Operation of PRM feasible path computed by local planner

42 Completeness of PRM If no path exists, then PRM cannot find the path. But if a path exists, it is possible PRM fails to find it. PRM is not complete but instead is probabilistically complete.

43 Theoretical Analysis of PRM (1/2) [Kavraki et al 96, 98, 00, 03, 07] ε-goodness property VS Tradeoff: planner may fail with probability Number of nodes/states: N 1 [log(1 )+log( 4 )] Important: Performance related to properties of the space

44 Theoretical Analysis of PRM (2/2) We sacrifice completeness for speed Probabilistic completeness Novel analysis and performance guarantees Pr(failure)=f(e cn ) How much can the assumptions be relaxed?

45 Primitive techniques

46 Primitives Select Sample: Uniform sampling is general but not the most efficient. Optimal selection remains elusive. Connect: Connect all to all is general but not efficient. Neighbors Notion of straight line or other local plan needs to be adapted. Store efficiently

47 Primitives Select Sample: Uniform sampling is general but not the most efficient. Optimal selection remains elusive. Connect: Connect all to all is general but not efficient. Neighbors Notion of straight line or other local plan needs to be adapted. Store efficiently

48 Several sampling strategies Gaussian sampling [Overmars et al]: VS Places samples close to objects. Distribution is Gaussian around the obstacle boundary. Medical-axis sampling [Amato et al]. Bridge Test sampling for narrow corridors [Hsu et al]. Quasi-Random sampling [LaValle et al]. Selective sampling [Kavraki el al]. Recent study confirmed it is one of the most critical parts of the planner [Hsu, Latombe 1998].

49 Primitives Select Sample: Uniform sampling is general but not the most efficient. Optimal selection remains elusive. Connect: Connect all to all is general but not efficient. Neighbors Notion of straight line or other local plan needs to be adapted. Store efficiently

50 Several connection strategies Nearness: Try to connect each configuration to a constant number of nearby configurations. nearest neighbors by kd-trees, k-nn, k-ann random neighbors may be helpful Component technique: Only test edges which reduce the number of connected components in the roadmap. Svestka, Overmars 96

51 Primitives Select Sample: Uniform sampling is general but not the most efficient. Optimal selection remains elusive. Connect: Connect all to all is general but not efficient. Neighbors Notion of straight line or other local plan needs to be adapted. Store efficiently

52 Outline of this talk Basic concepts and definitions. Examples of sampling-based planners: Roadmap planner Tree-based planner Underlying key components. OMPL and future challenges in motion planning.

53 A generic sampling-based tree planner

54 Sampling-based tree planner operation goal start

55 Sampling-based tree planner operation goal start grow random tree from start

56 Sampling-based tree planner operation goal start

57 Sampling-based tree planner operation goal start : occasionally attempt to connect tree to goal

58 Sampling-based tree planner operation goal start - Repeat until goal is connected to tree. - Bi-directional trees are possible when considering only geometric constraints.

59 Primitives Select Sample Expand from the sample Store efficiently

60 Rapidly Exploring Random Trees (RRT) Uses proximity query to guide construction (Voronoi Bias). Uses propagation instead of connection. Powerful heuristic for single-query planning. Bi-directional search can be implemented. [Lavalle, Kuffer 1999, 2000]

61 Expansive Trees (EST and SBL) EST: Uses density of nodes to guide expansion (density bias). [Hsu and Latombe, 1997, 1999] SBL: Uses some coverage estimates and density of nodes. [Sanchez and Latombe, 2001]

62 KPIECE Keeps tract of coverage by using discretization and by distinguishing the boundary from the covered space. Keeping of coverage can be done in a hierarchical fashion. Projections my be used. [Șucan, Kavraki 2008]

63 SyCLoP Using a discrete lead to help guide the expansion of the tree Plaku and Kavraki, 2008

64 Performance improvements for trees Bi-directional search. Lazy collision checking. Goal biasing. Accounting for constrained manifolds. Employing motion primitives. and many others.

65 Planning with Dynamics: Trees offer an advantage

66 Planning with dynamics Ladd et al. Bekris et al.

67 Planning with dynamics Ladd et al. Bekris et al.

68 Planning with dynamics Ladd et al. Bekris et al.

69 Planning with dynamics Ladd et al. Bekris et al.

70 Planning with dynamics Ladd et al. Bekris et al.

71 Physical Systems Planning or Equations Space of controls is defined

72 Physical system planning Given 1. an initial state q 0 Q 2. a goal set G Q The discrete physical systems planning problem is to compute a sequence u 0,, u N such that: F(q i,u i ) = q i+1 and q N+1 G is contained in the goal set.

73 Planning with dynamics Adding dynamics is essential to increase physical realism. Techniques from control theory can be used to create better paths or reduce differential equation integrations. Metrics tend to work poorly. Efficient planning for systems with dynamics is still fairly open: samplingbased tree planners offer an advantage.

74 Primitives Select Sample Expand from the sample Store efficiently These primitives are combined with various optimizations.

75 Variations of tree sampling-based planners EST [Hsu et al. 97, 00] RRT [Kuffner, LaValle 98] RRT-Connect [Kuffner, LaValle '00] SBL [Sanchez, Latombe 01] Guided EST [Phillips et al. 03] PDRRT [Ranganathan, Koenig '04] SRT [Plaku et al. '05] DDRRT [Yershova et al. 05] ADDRRT [Jaillet et al. 05] RRT-Blossom [Kalisiak, van Panne 06] PDST [Ladd, Kavraki 06] Utility RRT [Burns, Brock 07] GRIP [Bekris, Kavraki 07] Multiparticle RRT [Zucker et al. 07] TC-RRT [Stillman et al. '07] RRT-JT [Vande Wege et al '07] DSLX [Plaku, Kavraki, Vardi '08] KPIECE [Șucan, Kavraki '08] RPDST [Tsianos, Kavraki '08] BiSpace [Diankov et al. '08] GRRT [Chakravorty, Kumar '09] IKBiRRT [Berenson et al.'09] CBiRRT [Berenson et al.'09] J+RRT [Vahrenkamp '09] RRT* [Karaman et al, 10] and many others

76 Sampling-based planning (many possibilities) Core operations state sampling connection strategy... Common optimizations bi-directional goal-biasing...

77 Sampling-based planning (many possibilities) Core operations state sampling connection strategy... Common optimizations bi-directional goal-biasing...

78 Need for a systematized approach: OMPL

79 Benefits A repository of planners: choose the right planner and right parameters for that planner. Compare new planners to existing ones. Develop significantly more complex specialized planners. Enable challenging research. Support education of new scientists.

80 Challenges Uncertainty. Manipulation of rigid and flexible objects. Parallel Linkages. Dynamics. Hybrid planning. Real-time planning. and other.

81 THANK YOU Acknowledgements: Work at the Kavraki Lab on sampling-based planners has been supported by NSF