CIMAT/CINVESTAV 2011 Tutorial: Filtering and Planning in I-Spaces PART 1: INTRODUCTION

Transcription

1 CIMAT/CINVESTAV 2011 Tutorial: Filtering and Planning in I-Spaces PART 1: INTRODUCTION Steven M. LaValle June 7, 2011 CIMAT/CINVESTAV 2011 Tutorial 1 / 42

2 Acknowledgments Thanks to my hosts: Dr. Luz Abril Torres-Mendez, CINVESTAV, Saltillo Dr. Rafael Murrieta, CIMAT, Guanajuato Support provided by: DARPA SToMP (Sensor Topology and Minimalist Planning) ONR/MURI IRIS (Inference in Reduced Information Spaces) NSF Robotics University of Illinois, Computer Science Department CIMAT (Centro de Investigaciones en Matematicas) CIMAT/CINVESTAV 2011 Tutorial 2 / 42

3 Overview of Topics 1. Introduction 2. Physical Sensors 3. Virtual Sensors 4. Filtering 5. Planning 6. Possible Futures Follow along in the tutorial paper: lavalle/gto11/paper.pdf (to appear in Foundations and Trends in Robotics) /vsk Schedule and presentation slides: lavalle/gto11/ CIMAT/CINVESTAV 2011 Tutorial 3 / 42

4 Who Am I? I worked in planning for many years...since 1993 or so. CIMAT/CINVESTAV 2011 Tutorial 4 / 42

5 Who Am I? I worked in planning for many years...since 1993 or so. In 2005, I finished a planning book, which nearly killed me CIMAT/CINVESTAV 2011 Tutorial 4 / 42

6 Who Am I? I worked in planning for many years...since 1993 or so. In 2005, I finished a planning book, which nearly killed me I came to realize that sensing is often an afterthought in planning. Information seems to come for free as input. We plan in perfect C-spaces and state spaces with obstacles. CIMAT/CINVESTAV 2011 Tutorial 4 / 42

7 Free Book PART I Introductory Material Chapters 1-2 PART II Motion Planning (Planning in Continuous Spaces) Chapters 3-8 PART III Decision-Theoretic Planning (Planning Under Uncertainty) Chapters 9-12 PART IV Planning Under Differential Constraints Chapters Free download ( 1000 pages): Also published by Cambridge University Press, May CIMAT/CINVESTAV 2011 Tutorial 5 / 42

8 In the beginning (1960s)... Nilsson, Stanford, late 1960s: Shakey, A, visibility graphs, STRIPS What is planning? Automated sequential decision making with heavy emphasis on algorithms and computation. CIMAT/CINVESTAV 2011 Tutorial 6 / 42

9 In the beginning (1960s)... Clear challenges: sensing, mapping, planning,... CIMAT/CINVESTAV 2011 Tutorial 7 / 42

10 Algorithms Need Discretizations The world is more or less continuous. Computation is discrete. 1970s: Grids, logic-based planning 1980s: Combinatorial motion planning 1990s: Sampling-based motion planning Also: Planning problems are implicitly encoded. CIMAT/CINVESTAV 2011 Tutorial 8 / 42

11 The C-Space Obstacles Lozano-Perez, 1979 C obs O Reasoning about exact geometry C obs C obs C free q G C obs q I Motion planning progressed after identifying the right spaces. CIMAT/CINVESTAV 2011 Tutorial 9 / 42

12 Combinatorial Motion Planning O Dunlaing, Yap, 1982; Schwartz, Sharir, Exact, structure-preserving discretizations. Beautiful, complete algorithms. R CIMAT/CINVESTAV 2011 Tutorial 10 / 42

13 Combinatorial Motion Planning Canny s roadmap algorithm, 1987: x 3 x 2 x 1 Solves general motion planning problem. Complexity is close to optimal. Never implemented? CIMAT/CINVESTAV 2011 Tutorial 11 / 42

14 Combinatorial Motion Planning Don t be harsh on combinatorial planning methods. Reif, 1979; Hopcroft, Schwartz, Sharir, 1983: PSPACE-hardness Specific problems are easier: e 4 e 2 R 13 R 12 R 11 R 8 R 10 e 1 x 1 R 2 R R 9 3 e 3 R 4 R 1 R 6 R 7 R 5 x 2 CIMAT/CINVESTAV 2011 Tutorial 12 / 42

15 Sampling-Based Motion Planning Geometric Models Collision Detection Sampling Based Motion Planning Algorithm Discrete Searching C Space Sampling Collision detection algorithms enabled a new abstraction. Incremental sampling and searching. Resolution or probabilistic complete. The methods are practical and widely used. CIMAT/CINVESTAV 2011 Tutorial 13 / 42

16 Sampling-Based Motion Planning 1984 Donald grid search with heuristics based on C-constraints 1987 Faverjon, Tournassoud distance computation, hierarchical CAD models 1989 Paden, Mees, Fisher uses GTK algorithm, distance comp., 2 d tree 1989 Kondo grid search, lazy collision checking 1990 Lengyel, Reichert, Donald, Greenberg search bitmap of C-obstacles 1990 Barraquand, Latombe randomized potential field, implicit grid 1990 Glavina sample all of free space, connect with local planner 1992 Chen, Hwang multiresolution grid search 1992 Mazer, Talbi, Ahuatzin, Bessiere Ariadne s clew algorithm 1994 Kavraki, Svestka, Overmars, Latombe Probabilistic Roadmaps (PRMs); multiple query 1997 Hsu, Latombe, Motwani Expansive planner, single-query, tree search 1999 LaValle, Kuffner Rapidly-exploring Random Trees (RRTs) Collision detection: Gilbert, Johnson, Keerthi, 1988; Lin, Canny, 1991; Quinlan, 1994; Gottschalk, Lin and Manocha, 1996; Mirtich, 1997, etc. CIMAT/CINVESTAV 2011 Tutorial 14 / 42

17 Multiple Query: Sampling-Based Roadmaps (PRMs) C obs α(i) C obs Use sampling to build a roadmap Search roadmap for paths PRM-based methods: PRM (Kavraki, Latombe, Overmars, Svestka, 1994); Obstacle-Based PRM (Amato, Wu, 1996); Sensor-based PRM (Yu, Gupta, 1998); Gaussian PRM (Boor, Overmars, van der Stappen, 1999); Medial axis PRMs (Wilmarth, Amato, Stiller, 1999; Pisula, Hoff, Lin, Manocha, 2000; Kavraki, Guibas, 2000); Contact space PRM (Ji, Xiao, 2000); Closed-chain PRMs (LaValle, Yakey, Kavraki, 1999; Han, Amato 2000); Lazy PRM (Bohlin, Kavraki, 2000); PRM for changing environments (Leven, Hutchinson, 2000); Visibility PRM (Simeon, Laumond, Nissoux, 2000),... CIMAT/CINVESTAV 2011 Tutorial 15 / 42

18 Single Query: Search Tree Methods Donald, 1984 Grid search over 6D C-space Search guided by heuristics based directly on C-constraints Barraquand, Latombe, 1990 (randomized potential field) Implicit grid search guided by potential field and random walks Direct use of collision detector to validate motions Mazer, Talbi, Ahuatzin, Bessiere, 1992 (Ariadne s clew) Search trees based on self avoidance Node placement obtained by genetic algorithm Hsu, Latombe, Motwani, 1997 (expansive space planner) Also based on self avoidance Node placement biased toward low-density regions LaValle, Kuffner, 1999 (Rapidly-exploring Random Trees - RRTs) Search tree based on Voronoi bias Growth obtained by sampling and nearest-neighbor searching CIMAT/CINVESTAV 2011 Tutorial 16 / 42

19 Rapidly Exploring Random Trees (RRTs) q n α(i) q 0 q 0 Introduced by LaValle and Kuffner, Applied, adapted, and extended in many works: Frazzoli, Dahleh, Feron, 2000; Toussaint, Basar, Bullo, 2000; Vallejo, Jones, Amato, 2000; Strady, Laumond, 2000; Mayeux, Simeon, 2000; Karatas, Bullo, 2001; Li, Chang, 2001; Kuffner, Nishiwaki, Kagami, Inaba, Inoue, 2000, 2001; Williams, Kim, Hofbaur, How, Kennell, Loy, Ragno, Stedl, Walcott, 2001; Carpin, Pagello, 2002;... Also, applications to biology, computational geography, verification, virtual prototyping, architecture, solar sailing, computer graphics,... In IEEE ICRA 2011 Proceedings, RRT occurs 928 times. CIMAT/CINVESTAV 2011 Tutorial 17 / 42

20 The RRT Construction Algorithm BUILD RRT(q init ) 1 T.init(q init ); 2 for k = 1 to K do 3 q rand RANDOM CONFIG(); 4 EXTEND(T, q rand ); EXTEND(T, q rand ) ε q new q init q near q Metric on C: ρ : C C [0, ) Nearest neighbors: Yershova [Atramentov], LaValle, 2002; Arya, Mount, 1997 Incremental collision detection: Lin, Canny, 1991; Mirtich, 1997 CIMAT/CINVESTAV 2011 Tutorial 18 / 42

21 A Rapidly-Exploring Random Tree (RRT) CIMAT/CINVESTAV 2011 Tutorial 19 / 42

22 Voronoi-Biased Exploration CIMAT/CINVESTAV 2011 Tutorial 20 / 42

23 Voronoi Diagram in R 2 Think about: Where is the nearest Oxxo? CIMAT/CINVESTAV 2011 Tutorial 21 / 42

24 Voronoi Diagram in R 2 CIMAT/CINVESTAV 2011 Tutorial 22 / 42

25 Voronoi Diagram in R 2 CIMAT/CINVESTAV 2011 Tutorial 23 / 42

26 Refinement vs. Expansion Refinement Expansion Where will the random sample fall? CIMAT/CINVESTAV 2011 Tutorial 24 / 42

27 Adding Differential Constraints Smooth RRT Bidirectional Search CIMAT/CINVESTAV 2011 Tutorial 25 / 42

28 The Left-Forward-Only Car Left-Forward RRT Bidirectional Search CIMAT/CINVESTAV 2011 Tutorial 26 / 42

29 Wiper Motor Assembly Kineo CAM and LAAS/CNRS, Toulouse, France Integrated into Robcad (em-workplace) Add-ons for 3D Studio Max, Solidworks Direct users: Renault, Airbus, Ford, Optivus,... CIMAT/CINVESTAV 2011 Tutorial 27 / 42

30 Sealing Cracks at Volvo Cars Fraunhofer Chalmers Centre and Volvo Cars, Sweden CIMAT/CINVESTAV 2011 Tutorial 28 / 42

31 Virtual Humans Marcelo Kallman, UC Merced James Kuffner, CMU CIMAT/CINVESTAV 2011 Tutorial 29 / 42

32 Humanoid Robots Kagami and H7 Planning University of Tokyo and AIST CIMAT/CINVESTAV 2011 Tutorial 30 / 42

33 Molecules, Etc. From Nic Simeon, LAAS/CNRS CIMAT/CINVESTAV 2011 Tutorial 31 / 42

34 Can the Present Successes be Repeated? Need to broaden the focus of planning. Better unification of ideas. Need to address important concerns: feedback, differential constraints, sensing, uncertainty. Fundamental: Information comes from sensors, not the Turing tape. CIMAT/CINVESTAV 2011 Tutorial 32 / 42

35 Separate Histories, Common Goals Control theory: analytical, continuous, differential, feedback, optimality Motion planning: algorithmic, continuous, paths, feasibility AI planning: algorithms, discrete, logic, feasibility Control Theory Motion Planning AI Planning These days, people are interested in the same issues. Is it planning algorithms? Algorithmic control theory? Continuous Discrete Analytical Algorithmic Differential Optimality Feasibility Feedback Uncertainty CIMAT/CINVESTAV 2011 Tutorial 33 / 42

36 Three Main Places for Prospecting After painfully putting together a landscape of literature, several enormous holes were visible. The hardest chapters to write: Motion planning under differential constraints (Ch 14) Feedback motion planning (Ch 8) Information spaces and sensing (Ch 11,12) CIMAT/CINVESTAV 2011 Tutorial 34 / 42

37 Motion Planning with Differential Constraints Various complications arise... q X ric X ric q > 0 X obs q = 0 q q < 0 X ric X ric Steady left turn Hover Forward flight Steady right turn Motion primitive problem (Frazzoli, Egerstedt, Pappas, Murphey, Belta) CIMAT/CINVESTAV 2011 Tutorial 35 / 42

38 Feedback Motion Planning x G u T Compute a collision-free velocity field over the C-space. Generally better than tracking a path. CIMAT/CINVESTAV 2011 Tutorial 36 / 42

39 Flows x goal CIMAT/CINVESTAV 2011 Tutorial 37 / 42

40 I-Spaces: The Next Generation of C-Spaces When there are sensors, planning naturally lives in an information space. We need to develop: Formulations of sensor models, I-spaces Models of complexity, computation over I-spaces Sampling-based planning methods Combinatorial planning methods For C-spaces, the early steps were already done (Lagrangian mechanics). CIMAT/CINVESTAV 2011 Tutorial 38 / 42

41 Where Did Information Spaces Arise? Where have information spaces arisen? Early appearance of concept: H. Kuhn, 1953 Extensive form games Unknown state information regarding other players. Stochastic control theory Disturbances in prediction and measurements cause imperfect state information. Robotics/AI Uncertainty due to limited sensing. Alternative names: belief states, knowledge states, hyperstates CIMAT/CINVESTAV 2011 Tutorial 39 / 42

42 Classical vs. Sensor-Centric Computation State Machine Sensing Infinite Tape M Actuation E Classical Sensor-Centric Classical state: finite machine state, head position, and tape string Sensor-centric state: internal, computational state and external, physical state CIMAT/CINVESTAV 2011 Tutorial 40 / 42

43 Tutorial: Possible Surprises Depending on your background, there might be surprises in this tutorial: 1. Discrete vs. continuous: Not very important CIMAT/CINVESTAV 2011 Tutorial 41 / 42

44 Tutorial: Possible Surprises Depending on your background, there might be surprises in this tutorial: 1. Discrete vs. continuous: Not very important 2. Information spaces, not information theory CIMAT/CINVESTAV 2011 Tutorial 41 / 42

45 Tutorial: Possible Surprises Depending on your background, there might be surprises in this tutorial: 1. Discrete vs. continuous: Not very important 2. Information spaces, not information theory 3. Perfectly accurate and reliable sensors yield huge amounts of uncertainty CIMAT/CINVESTAV 2011 Tutorial 41 / 42

46 Some Coming Themes Start from the task and try to understand what information is actually required to be extracted from the physical world. We can design combinatorial filters that are structurally similar to Bayesian or Kalman filters, but dramatically simpler. There is no problem defining enormous physical state spaces, provided that we do not directly compute over them. However, state estimation or recovery of a particular state in a giant state space should be avoided if possible. Virtual sensor models provide a powerful intermediate abstraction that can be implemented by many alternative physical sensing systems. CIMAT/CINVESTAV 2011 Tutorial 42 / 42