 # Robot Lab Robot Path Planning William Regli Department

• Slides: 94 Robot Lab: Robot Path Planning William Regli Department of Computer Science (and Departments of ECE and MEM) Drexel University Slide 1 Introduction to Motion Planning • Applications • Overview of the Problem • Basics – Planning for Point Robot – – Visibility Graphs Roadmap Cell Decomposition Potential Field Goals • Compute motion strategies, e. g. , – Geometric paths – Time-parameterized trajectories – Sequence of sensor-based motion commands • Achieve high-level goals, e. g. , – Go to the door and do not collide with obstacles – Assemble/disassemble the engine – Build a map of the hallway – Find and track the target (an intruder, a missing pet, etc. ) Slide 3 Fundamental Question Are two given points connected by a path? Slide 4 Basic Problem • Problem statement: Compute a collision-free path for a rigid or articulated moving object among static obstacles. • Input – Geometry of a moving object (a robot, a digital actor, or a molecule) and obstacles – How does the robot move? – Kinematics of the robot (degrees of freedom) – Initial and goal robot configurations (positions & orientations) • Output Continuous sequence of collision-free robot configurations connecting the initial and goal configurations Slide 5 Example: Rigid Objects Slide 6 Example: Articulated Robot Slide 7 Is it easy? Slide 8 Hardness Results • Several variants of the path planning problem have been proven to be PSPACE-hard. • A complete algorithm may take exponential time. – A complete algorithm finds a path if one exists and reports no path exists otherwise. • Examples – Planar linkages [Hopcroft et al. , 1984] – Multiple rectangles [Hopcroft et al. , 1984] Slide 9 Tool: Configuration Space Difficulty – Number of degrees of freedom (dimension of configuration space) – Geometric complexity Slide 10 Extensions of the Basic Problem • More complex robots – – – Multiple robots Movable objects Nonholonomic & dynamic constraints Physical models and deformable objects Sensorless motions (exploiting task mechanics) – Uncertainty in control Slide 11 Extensions of the Basic Problem • More complex environments – Moving obstacles – Uncertainty in sensing • More complex objectives – – Optimal motion planning Integration of planning and control Assembly planning Sensing the environment • Model building • Target finding, tracking Slide 12 Practical Algorithms • A complete motion planner always returns a solution when one exists and indicates that no such solution exists otherwise. • Most motion planning problems are hard, meaning that complete planners take exponential time in the number of degrees of freedom, moving objects, etc. Slide 13 Practical Algorithms • Theoretical algorithms strive for completeness and low worst-case complexity – Difficult to implement – Not robust • Heuristic algorithms strive for efficiency in commonly encountered situations. – No performance guarantee • Practical algorithms with performance guarantees – Weaker forms of completeness – Simplifying assumptions on the space: “exponential time” algorithms that work in practice Slide 14 • Problem Formulation for Point Robot Input – Robot represented as a point in the plane – Obstacles represented as polygons – Initial and goal positions • Output – A collision-free path between the initial and goal positions Slide 15 Framework Slide 16 Visibility Graph Method • Observation: If there is a collision-free path between two points, then there is a polygonal path that bends only at the obstacles vertices. • Why? – Any collision-free path can be transformed into a polygonal path that bends only at the obstacle vertices. • A polygonal path is a piecewise linear curve. Slide 17 Visibility Graph • A visibility graphis a graph such that – Nodes: qinit, qgoal, or an obstacle vertex. – Edges: An edge exists between nodes u and v if the line segment between u and v is an obstacle edge or it does not intersect the obstacles. Slide 18 A Simple Algorithm for Building Visibility Graphs Slide 19 Computational Efficiency • Simple algorithm O(n 3) time • More efficient algorithms – Rotational sweep O(n 2 log n) time – Optimal algorithm O(n 2) time – Output sensitive algorithms • O(n 2) space Slide 20 Framework Slide 21           Other Search Algorithms • Depth-First Search • Best-First Search, A* Slide 32 Framework Slide 33 Summary • Discretize the space by constructing visibility graph • Search the visibility graph with breadth-first search Q: How to perform the intersection test? Slide 34 Summary • Represent the connectivity of the configuration space in the visibility graph • Running time O(n 3) – Compute the visibility graph – Search the graph – An optimal O(n 2) time algorithm exists. • Space O(n 2) Can we do better? Slide 35 • Classic Path Planning Approaches Roadmap – Represent the connectivity of the free space by a network of 1 -D curves • Cell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells • Potential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function Slide 36 Classic Path Planning Approaches • Roadmap – Represent the connectivity of the free space by a network of 1 -D curves • Cell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells • Potential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function Slide 37 Roadmap • Visibility graph Shakey Project, SRI [Nilsson, 1969] • Voronoi Diagram Introduced by computational geometry researchers. Generate paths that maximizes clearance. Applicable mostly to 2 -D configuration spaces. Slide 38 Voronoi Diagram • Space O(n) • Run time O(n log n) Slide 39 Other Roadmap Methods • Silhouette First complete general method that applies to spaces of any dimensions and is singly exponential in the number of dimensions [Canny 1987] • Probabilistic roadmaps Slide 40 • Classic Path Planning Approaches Roadmap – Represent the connectivity of the free space by a network of 1 -D curves • Cell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells • Potential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function Slide 41 Cell-decomposition Methods • Exact cell decomposition The free space F is represented by a collection of non-overlapping simple cells whose union is exactly F • Examples of cells: trapezoids, triangles Slide 42 Trapezoidal Decomposition Slide 43 Computational Efficiency • Running time O(n log n) by planar sweep • Space O(n) • Mostly for 2 -D configuration spaces Slide 44 Adjacency Graph • Nodes: cells • Edges: There is an edge between every pair of nodes whose corresponding cells are adjacent. Slide 45 Summary • Discretize the space by constructing an adjacency graph of the cells • Search the adjacency graph Slide 46 Cell-decomposition Methods • Exact cell decomposition • Approximate cell decomposition – F is represented by a collection of nonoverlapping cells whose union is contained in F. – Cells usually have simple, regular shapes, e. g. , rectangles, squares. – Facilitate hierarchical space decomposition Slide 47  Octree Decomposition Slide 49 Algorithm Outline Slide 50 • Classic Path Planning Approaches Roadmap – Represent the connectivity of the free space by a network of 1 -D curves • Cell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells • Potential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function Slide 51 Potential Fields • Initially proposed for real-time collision avoidance [Khatib 1986]. Hundreds of papers published. • A potential field is a scalar function over the free space. • To navigate, the robot applies a force proportional to the negated gradient of the potential field. • A navigation function is an ideal potential field that – – has global minimum at the goal has no local minima grows to infinity near obstacles is smooth Slide 52 Attractive & Repulsive Fields Slide 53 How Does It Work? Slide 54 Algorithm Outline • Place a regular grid G over the configuration space • Compute the potential field over G • Search G using a best-first algorithm with potential field as the heuristic function Slide 55 Local Minima • What can we do? – Escape from local minima by taking random walks – Build an ideal potential field – navigation function – that does not have local minima Slide 56 Question • Can such an ideal potential field be constructed efficiently in general? Slide 57 Completeness • A complete motion planner always returns a solution when one exists and indicates that no such solution exists otherwise. – Is the visibility graph algorithm complete? Yes. – How about the exact cell decomposition algorithm and the potential field algorithm? Slide 58 Why Complete Motion Planning? • Probabilistic roadmap motion planning • Complete motion planning – Efficient – Work for complex problems with many DOF – Always terminate – Difficult for narrow passages – May not terminate when no path exists – Not efficient – Not robust even for low DOF Slide 59 Path Non-existence Problem Robot Initial Obstacle Goal Slide 60 • Main Challenge Exponential complexity: n. DOF – Degree of freedom: DOF – Geometric complexity: n • More difficult than finding a path – To check all possible paths Robot Initial Obstacle Goal Slide 61 Approaches • Exact Motion Planning – Based on exact representation of free space • Approximation Cell Decomposition (ACD) • A Hybrid planner Slide 62 Configuration Space: 2 D Translation Workspace Configuration Space Goal Obstacle Robot Start C-obstacle Free y x Slide 63 Configuration Space Computation • [Varadhan et al, ICRA 2006] • 2 Translation + 1 Rotation • 215 seconds Obstacle y x Robot Slide 64 Exact Motion Planning • Approaches – Exact cell decomposition [Schwartz et al. 83] – Roadmap [Canny 88] – Criticality based method [Latombe 99] – Voronoi Diagram – Star-shaped roadmap [Varadhan et al. 06] • Not practical – Due to free space computation – Limit for special and simple objects • Ladders, sphere, convex shapes Slide 65 Approaches • Exact Motion Planning – Based on exact representation of free space • Approximation Cell Decomposition (ACD) • A Hybrid Planner Combing ACD and PRM Slide 66 Approximation Cell Decomposition (ACD) • Not compute the free space exactly at once • But compute it incrementally • Relatively easy to implement – – [Lozano-Pérez 83] [Zhu et al. 91] [Latombe 91] [Zhang et al. 06] Slide 67 Approximation Cell Decomposition • Full cell Configuration Space full mixed • Empty cell • Mixed cell – Mixed – Uncertain empty Slide 68 Connectivity Graph Gf : Free Connectivity Graph G: Connectivity Graph Gf is a subgraph of G Slide 69 Finding a Path by ACD Gf : Free Connectivity Graph Initial Goal Slide 70 Finding a Path by ACD • First Graph Cut Algorithm – Guiding path in connectivity graph G L: Guiding Path – Only subdivide along this path – Update the graphs G and Gf Described in Latombe’s book Slide 71 First Graph Cut Algorithm L Only subdivide along L Slide 72 Finding a Path by ACD Slide 73 ACD for Path Non-existence Initial Goal C-space Slide 74 ACD for Path Non-existence Connectivity Graph Guiding Path Slide 75 ACD for Path Non-existence Connectivity graph is not connected No path! Sufficient condition for deciding path non-existence Slide 76 Two-gear Example Video no path! Initial 3. 356 s Cells in C-obstacle Roadmap in F Goal Slide 77 Cell Labeling full mixed • Free Cell Query – Whether a cell completely lies in free space? • C-obstacle Cell Query – Whether a cell completely lies in C-obstacle? empty Slide 78 Free Cell Query A Collision Detection Problem • Does the cell lie inside free space? • Do robot and obstacle separate at all configurations? Robot Obstacle ? Configuration space Workspace Slide 79 Clearance • Separation distance – A well studied geometric problem d • Determine a volume in C-space which are completely free Slide 80 C-obstacle Query Another Collision Detection Problem • Does the cell lie inside C-obstacle? • Do robot and obstacle intersect at all configurations? Robot ? Obstacle Configuration space Workspace Slide 81 ‘Forbiddance’ • ‘Forbiddance’: dual to clearance • Penetration Depth – A geometric computation problem less investigated PD • [Zhang et al. ACM SPM 2006] Slide 82 Limitation of ACD • Combinatorial complexity of cell decomposition • Limited for low DOF problem – 3 -DOF robots Slide 83 Approaches • Exact Motion Planning – Based on exact representation of free space • Approximation Cell Decomposition (ACD) • A Hybrid Planner Combing ACD and PRM Slide 84 • Hybrid Planning • Complete Motion Probabilistic roadmap motion planning Planning + Efficient + Many DOFs + Complete - Narrow passages - Path non-existence - Not efficient Can we combine them together? Slide 85 • Hybrid Approach for Complete Motion Planning Use Probabilistic Roadmap (PRM): – Capture the connectivity for mixed cells – Avoid substantial subdivision • Use Approximation Cell Decomposition (ACD) – Completeness – Improve the sampling on narrow passages Slide 86 Connectivity Graph Gf : Free Connectivity Graph G: Connectivity Graph Gf is a subgraph of G Slide 87 Pseudo-free edges Initial Goal Pseudo free edge for two adjacent cells Slide 88 Pseudo-free Connectivity Graph: Gsf = Gf + Pseudo-edges Initial Goal Slide 89 Algorithm • Gf • Gsf • G Slide 90 Results of Hybrid Planning Slide 91 Results of Hybrid Planning Slide 92 Results of Hybrid Planning • 2. 5 - 10 times speedup 3 DOF 4 DOF timing cells # Hybrid 34 s 50 K 16 s 48 K 102 s 164 K ACD 85 s 168 K ? ? Speedup 2. 5 3. 3 ≥ 10 ? Slide 93 Summary • Difficult for Exact Motion Planning – Due to the difficulty of free space configuration computation • ACD is more practical – Explore the free space incrementally • Hybrid Planning – Combine the completeness of ACD and efficiency of PRM Slide 94