Robot Lab:Robot Path Planning: Robot Lab: Robot Path Planning William Regli Department of Computer Science (and Departments of ECE and MEM) Drexel University


Introduction to Motion Planning: Introduction to Motion Planning Applications Overview of the Problem Basics – Planning for Point Robot Visibility Graphs Roadmap Cell Decomposition Potential Field


Goals: 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.)


Fundamental Question: Fundamental Question Are two given points connected by a path?


Basic Problem: 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


Example: Rigid Objects: Example: Rigid Objects


Example: Articulated Robot: Example: Articulated Robot


Is it easy?: Is it easy?


Hardness Results: 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]


Tool: Configuration Space: Tool: Configuration Space Difficulty Number of degrees of freedom (dimension of configuration space) Geometric complexity


Extensions of the Basic Problem: 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


Extensions of the Basic Problem: 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


Practical Algorithms: 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.


Practical Algorithms: 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


Problem Formulation for Point Robot: 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


Framework: Framework


Visibility Graph Method: 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.


Visibility Graph: 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.


A Simple Algorithm for Building Visibility Graphs: A Simple Algorithm for Building Visibility Graphs


Computational Efficiency: Computational Efficiency Simple algorithm O(n3) time More efficient algorithms Rotational sweep O(n2log n) time Optimal algorithm O(n2) time Output sensitive algorithms O(n2) space


Framework: Framework


Breadth-First Search: Breadth-First Search


Breadth-First Search: Breadth-First Search


Breadth-First Search: Breadth-First Search


Breadth-First Search: Breadth-First Search


Breadth-First Search: Breadth-First Search


Breadth-First Search: Breadth-First Search


Breadth-First Search: Breadth-First Search


Breadth-First Search: Breadth-First Search


Breadth-First Search: Breadth-First Search


Breadth-First Search: Breadth-First Search


Other Search Algorithms: Other Search Algorithms Depth-First Search Best-First Search, A*


Framework: Framework


Summary: Summary Discretize the space by constructing visibility graph Search the visibility graph with breadth-first search Q: How to perform the intersection test?


Summary: Summary Represent the connectivity of the configuration space in the visibility graph Running time O(n3) Compute the visibility graph Search the graph An optimal O(n2) time algorithm exists. Space O(n2) Can we do better?


Classic Path Planning Approaches: 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


Classic Path Planning Approaches: 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


Roadmap: 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.


Voronoi Diagram: Voronoi Diagram Space O(n) Run time O(n log n)


Other Roadmap Methods: 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


Classic Path Planning Approaches: 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


Cell-decomposition Methods: 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


Trapezoidal Decomposition: Trapezoidal Decomposition


Computational Efficiency: Computational Efficiency Running time O(n log n) by planar sweep Space O(n) Mostly for 2-D configuration spaces


Adjacency Graph: Adjacency Graph Nodes: cells Edges: There is an edge between every pair of nodes whose corresponding cells are adjacent.


Summary: Summary Discretize the space by constructing an adjacency graph of the cells Search the adjacency graph


Cell-decomposition Methods: Cell-decomposition Methods Exact cell decomposition Approximate cell decomposition F is represented by a collection of non-overlapping cells whose union is contained in F. Cells usually have simple, regular shapes, e.g., rectangles, squares. Facilitate hierarchical space decomposition


Quadtree Decomposition: Quadtree Decomposition


Octree Decomposition: Octree Decomposition


Algorithm Outline: Algorithm Outline


Classic Path Planning Approaches: 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


Potential Fields: 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


Attractive & Repulsive Fields : Attractive & Repulsive Fields


How Does It Work?: How Does It Work?


Algorithm Outline: 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


Local Minima: 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


Question: Question Can such an ideal potential field be constructed efficiently in general?


Completeness: 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?


Why Complete Motion Planning?: Why Complete Motion Planning? Probabilistic roadmap motion planning Efficient Work for complex problems with many DOF Difficult for narrow passages May not terminate when no path exists Complete motion planning Always terminate Not efficient Not robust even for low DOF


Path Non-existence Problem: Path Non-existence Problem Obstacle Obstacle


Main Challenge: Main Challenge Obstacle Exponential complexity: nDOF Degree of freedom: DOF Geometric complexity: n More difficult than finding a path To check all possible paths


Approaches: Approaches Exact Motion Planning Based on exact representation of free space Approximation Cell Decomposition (ACD) A Hybrid planner


Configuration Space: 2D Translation: Configuration Space: 2D Translation Workspace Configuration Space x y Robot Start Goal Free


Configuration Space Computation: Configuration Space Computation [Varadhan et al, ICRA 2006] 2 Translation + 1 Rotation 215 seconds Obstacle Robot x  y


Exact Motion Planning: 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 3DOF


Approaches: Approaches Exact Motion Planning Based on exact representation of free space Approximation Cell Decomposition (ACD) A Hybrid Planner Combing ACD and PRM


Approximation Cell Decomposition (ACD): 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]


Approximation Cell Decomposition: Approximation Cell Decomposition Full cell Empty cell Mixed cell Mixed Uncertain Configuration Space


Connectivity Graph: Connectivity Graph Gf : Free Connectivity Graph Gf is a subgraph of G


Finding a Path by ACD: Finding a Path by ACD Goal Initial


Finding a Path by ACD: Finding a Path by ACD L: Guiding Path First Graph Cut Algorithm Guiding path in connectivity graph G Only subdivide along this path Update the graphs G and Gf Described in Latombe’s book


First Graph Cut Algorithm: First Graph Cut Algorithm Only subdivide along L L


Finding a Path by ACD: Finding a Path by ACD


ACD for Path Non-existence: ACD for Path Non-existence C-space Goal Initial


ACD for Path Non-existence: Connectivity Graph ACD for Path Non-existence


ACD for Path Non-existence: ACD for Path Non-existence Connectivity graph is not connected No path! Sufficient condition for deciding path non-existence


Two-gear Example: Two-gear Example no path! Cells in C-obstacle Initial Goal Roadmap in F Video 3.356s


Cell Labeling: Cell Labeling Free Cell Query Whether a cell completely lies in free space? C-obstacle Cell Query Whether a cell completely lies in C-obstacle?


Free Cell Query A Collision Detection Problem: Free Cell Query A Collision Detection Problem Does the cell lie inside free space? Do robot and obstacle separate at all configurations? Obstacle Workspace Configuration space ? Robot


Clearance : Clearance Separation distance A well studied geometric problem Determine a volume in C-space which are completely free d


C-obstacle QueryAnother Collision Detection Problem: C-obstacle Query Another Collision Detection Problem Does the cell lie inside C-obstacle? Do robot and obstacle intersect at all configurations? Obstacle Workspace Configuration space ? Robot


‘Forbiddance’: ‘Forbiddance’ ‘Forbiddance’: dual to clearance Penetration Depth A geometric computation problem less investigated [Zhang et al. ACM SPM 2006] PD


Limitation of ACD: Limitation of ACD Combinatorial complexity of cell decomposition Limited for low DOF problem 3-DOF robots


Approaches: Approaches Exact Motion Planning Based on exact representation of free space Approximation Cell Decomposition (ACD) A Hybrid Planner Combing ACD and PRM


Hybrid Planning: Hybrid Planning Probabilistic roadmap motion planning + Efficient + Many DOFs Narrow passages Path non-existence Complete Motion Planning + Complete Not efficient Can we combine them together?


Hybrid Approach for Complete Motion Planning : 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


Connectivity Graph: Connectivity Graph Gf : Free Connectivity Graph G: Connectivity Graph Gf is a subgraph of G


Pseudo-free edges: Pseudo-free edges Pseudo free edge for two adjacent cells Goal Initial


Pseudo-free Connectivity Graph: Gsf: Pseudo-free Connectivity Graph: Gsf Goal Initial Gsf = Gf + Pseudo-edges


Algorithm: Algorithm Gf Gsf G


Results of Hybrid Planning: Results of Hybrid Planning


Results of Hybrid Planning: Results of Hybrid Planning


Results of Hybrid Planning: Results of Hybrid Planning 2.5 - 10 times speedup


Summary : 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