Outline. Robotics Planning. Example: Getting to DC from Atlanta. Introduction

Transcription

1 Outline Robotics Planning Henrik I Christensen Introduction Representations Planning Basics Search methods Evaluation (or the lack thereof) Summary Introduction Example: Getting to DC from Atlanta Generation of efficient representations for plan generation Efficient search strategies for high dimensional spaces Consideration of domain / application constraints These problems are not unique to robotics Abundance of progress in general CS, AI, Mathematics

2 Typical path planning example Typical path planning example Source: J-P. Lamond et al, LAAS Source: S. Lavall, UIUC Planning Deliberative Planning Two extremes Deliberative search/planning Purely reactive control Components to the puzzle A world representation A metric for optimization An algorithm for search

3 Planning Representations Grids Graphs Configuration Spaces Planning Grid based methods Graph search Typical planning representations Configuration space Visibility Graph Voronoi Representation Cell Decomposition Potential Fields Configuration spaces Configuration spaces Robots are embedded in a space (termed the physical environment) Robots have a finite spatial size (the robot geometry) A robot embedded can be changed In general a robot operates in an environment that we will term the workspace A state is considered free if the embedded of the robot in the workspace is collision free The subspace of collision free configurations in the state space denoted Sfree Sfree can be complex even for relatively simple systems The boundary of the workspace is a set of obstacles This complexity is the main challenge for motion planning systems The parameters for the robot defined a space termed the state space A point in this space is a state

4 Configuration spaces Configuration spaces 2-link planar arm among 5 obstacles 3-link planar arm Source: L. Kavraki, Rice Source: L. Kavraki, Rice Path Planning Query / problem definition A path is a continuous mapping in C :[0,L]! S free Where L is the length of the path The path is collision from if for all t A problem or query is Given two states q0 and qf Determine if there is a collision-free path between q0 and qf (t) 2 S free

5 Path Planning Roadmaps Given: World geometry Robot s geometry Start and goal configuration Compute: A collision-free, feasible path to the goal Obstacle Start : Robot Goal Roadmaps / Graphs How do we select the key nodes? Workspace Model Robot Model Motion Planner Collision-free Path Source: F. Moss, Rice Space decomposition Visibility Graph Partition Free-space Obstacles Generation of a tessellation Connect visible vertices in space Generate a search across the resulting graph

6 Voronoi Graph Cell decomposition Compute maximum distance to boundary points Search for shortest path along the graph Divide free space (Sfree) into single connected regions termed cells Generate connectivity graph Local Goal and Start cells Search the graph Generation a motion strategy Approximate cell decomposition Adaptive cell decomposition Easy to implement Use of standard methods for search such as wavefront & distance Widely used in simple environments Efficient representation of space Quad-trees are well-known from computational geometry Suited for sparse workspaces

7 Planning basics Bug-2 the obvious improvement The Bug family of algorithms [Lumelski] Simplest possible strategy Traverse obstacles Leave a point of minimum distance Inefficient but gets job done Do traversal but leave at point to goal point Efficient in open spaces Mazes a challenge Potential fields Potential fields Consider the robot a particle in a potential field Goal serves as an attractor Obstacles represents repellors When the potential field is differentiable the force is F (q) = ru(q) specifies locally the direction of motion Potential fiedls can be represented by harmonics Superposition principle specifies Goal dynamics can be represented by a potential field Each obstacle is a potential field The sum of the parts is still a potential field Source: IROS tutorial, 2011

8 Potential fields Wavefront propagation Potential fields are known to experience local minima Consider the world as a an homogenous grid Cells are free or occupied / or with walls Search from start to goal Neighbor metrics can be used to define behavior Wavefront propagation Wavefront propagation START GOAL START GOAL

9 Wavefront propagation Wavefront propagation START GOAL START GOAL Graph search using A * Basic tree-search A * is well known as a graph search heuristic based on estimated and actual cost c(~p) = cc(~s, ~p)+ where p is present position s is the start position g is the goal position cc is current cost gc is an estimate of the cost of achieving the goal position α, β represents weight factors gc(~p, ~g) function Tree-Search( problem, fringe) returns a solution, or failure fringe Insert(Make-Node(Initial-State[problem]), fringe) loop do if fringe is empty then return failure node Remove-Front(fringe) if Goal-Test[problem] applied to State(node) succeeds return node fringe InsertAll(Expand(node, problem), fringe) The challenge is the design of the expand function! The challenge is the design of the expand funtion Source: Russell & Norvig, Artificial Intelligence

10 Informed search strategies Example of navigation in maps The ideal - Best First Search Selection of an evaluation function f(n) Expand low-cost nodes before higher cost nodes Design a heuristic function h(n) Estimated cost of the cheapest path from n to goal Oradea 71 Neamt 87 Zerind Iasi Arad Sibiu 99 Fagaras Vaslui Timisoara Rimnicu Vilcea Lugoj 97 Pitesti Hirsova Mehadia Urziceni Bucharest 120 Dobreta 90 Craiova Eforie Giurgiu Straight line distance to Bucharest Arad 366 Bucharest 0 Craiova 160 Dobreta 242 Eforie 161 Fagaras 178 Giurgiu 77 Hirsova 151 Iasi 226 Lugoj 244 Mehadia 241 Neamt 234 Oradea 380 Pitesti 98 Rimnicu Vilcea 193 Sibiu 253 Timisoara 329 Urziceni 80 Vaslui 199 Zerind 374 Doing it greedily Properties of greedy search (a) The initial state Arad 366 (b) After expanding Arad Arad Sibiu Timisoara Zerind (c) After expanding Sibiu Arad Sibiu Timisoara Zerind Completeness: might get stuck in loops Repeated state check needed to break loops Time: O(b m ) - a good heuristic can improve performance Space O(b m ) - keep all nodes in memory Optimal? no greedy is not always optimal Arad Fagaras Oradea Rimnicu Vilcea (d) After expanding Fagaras Arad Sibiu Timisoara Zerind Arad Fagaras Oradea Rimnicu Vilcea Sibiu Bucharest 253 0

11 A* search A* optimality? Arad Increase nodes according to f value Gradually adds f contours to nodes (a la breadth first w. layers) O Sibiu Timisoara Zerind 447= = Arad Fagaras Oradea Rimnicu Vilcea 646= = Sibiu Bucharest Craiova Pitesti Sibiu 591= = = = Bucharest Craiova Rimnicu Vilcea 418= = = A T Z 380 D L M 400 S R C F P 420 G N B I U V H E A* properties What are our options? Complete? Yes Time: exponential in h - accuracy * h * (start) Space: all nodes in memory Optimal: Yes Variation of A* Iterative deepening A* (IDA) Recursive best first (RBDF) Memory bounded A* (MA) Simple MA (SMA) 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

12 Why randomized planners? Point robot in 2-D The structure of the C-space can be highly complex The space can be high dimensional 6+ a robot state Sample based tree planner Probabilistic Roadmaps (PRM) goal start

13 Probabilistic Roadmaps (PRM) Probabilistic Roadmaps (PRM) Probabilistic Roadmaps (PRM) Answering queries goal start

14 Operations of a PRM Operations of a PRM Operations of a PRM Operations of a PRM

15 Operations of a PRM Operations of a PRM Operations of a PRM Rapid Random Trees Could tree search be randomized to achieve some of the same functionality? There has been two recent approaches to randomized C-space search Probabilistic Roadmaps (PRM) Rapid Exploring Random Trees (RRT)

16 Operations of a tree based planner Sampling based tree planner goal goal start start Sampling based tree planner Sampling based tree planner goal goal start start

17 Sampling based tree planner Milk delivery goal start Randomly Exploring Random Trees (RRT) Variations of tree sampling based planners 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. 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 [Lavalle, Kuffer 1999, 2000]

18 Small Example Milk delivery Pickup milk Mobile Manipulator Drive to refrigerator Open Door Pickup milk Deliver milk Close door to refrigerator MIlk delivery Planning There are a rich variety of planning methods Consideration of the characteristics Complexity of the configuration space? Can domain constraints be imposed? Can we design deterministic search strategies? What are memory requirements? Do we need real-time response? Repositories for planner benchmarking are emerging Great literature Choset et al, Principles of Robot Motion, MIT Press Lavalle, Planning Algorithms, Cambridge University Press