City College of New York 1 Dr. John (Jizhong) Xiao Department of Electrical Engineering City College of New York Robot Motion Planning Introduction to ROBOTICS

City College of New York 2 What is Motion Planning? Determining where to go without hit obstacles

City College of New York 3 Topics Basics –Configuration Space –C-obstacles Motion Planning Methods –Roadmap Approaches Visibility graphs Voronoi diagram –Cell Decomposition Trapezoidal Decomposition Quadtree Decomposition –Potential Fields –Bug Algorithms

City College of New York 4 References G. Dudek, M. Jenkin, Computational Principles of Mobile Robots, MIT Press, 2000 (Chapter 5) J.C. Latombe, Robot Motion Planning, Kluwer Academic Publishers, Additional references – Path Planning with A* algorithm S. Kambhampati, L. Davis, “Multiresolution Path Planning for Mobile Robots”, IEEE Journal of Robotrics and Automation,Vol. RA-2, No.3, 1986, pp – Potential Field O. Khatib, “Real-Time Obstacle Avoidance for Manipulators and Mobile Robots”, Int. Journal of Robotics Research, 5(1), pp.90-98, P. Khosla, R. Volpe, “Superquadratic Artificial Potentials for Obstacle Avoidance and Approach” Proc. Of ICRA, 1988, pp B. Krogh, “A Generalized Potential Field Approach to Obstacle Avoidance Control” SME Paper MS

City College of New York 5 The World consists of... Obstacles –Already occupied spaces of the world –In other words, robots can’t go there Free Space –Unoccupied space within the world –Robots “might” be able to go here –To determine where a robot can go, we need to discuss what a Configuration Space is

City College of New York 6 Configuration Space Configuration Space is the space of all possible robot configurations. Notation: A: single rigid object –(the robot) W: Euclidean space where A moves; B1,…Bm: fixed rigid obstacles distributed in W F W – world frame (fixed frame) F A – robot frame (moving frame rigidly associated with the robot) Configuration q of A is a specification of the physical state (position and orientation) of A w.r.t. a fixed environmental frame F W.

City College of New York 7 Configuration Space For a point robot moving in 2-D plane, C -space is Configuration Space of A is the space ( C )of all possible configurations of A. q slug q robot C C free C obs Point robot (free-flying, no constraints)

City College of New York 8 Configuration Space For a point robot moving in 3-D, the C- space is  x y q start q goal C C free C obs What is the difference between Euclidean space and C- space?

City College of New York 9 Configuration Space X Y A robot which can translate in the plane X Y A robot which can translate and rotate in the plane x Y C- space: 2-D (x, y) 3-D (x, y, ) Euclidean space:

City College of New York 10 Configuration Space 2R manipulator topology Configuration space    

City College of New York 11 Configuration Space Two points in the robot’s workspace   Torus (wraps horizontally and vertically) q robot q slug  

City College of New York 12 Configuration Space An obstacle in the robot’s workspace   q slug q robot a “ C -space” representation   If the robot configuration is within the blue area, it will hit the obstacle What is dimension of the C -space of puma robot (6R)? Visualization of high dimension C -space is difficult

City College of New York 13 Motion Planning Revisit Find a collision free path from an initial configuration to goal configuration while taking into account the constrains (geometric, physical, temporal) A separate problem for each robot? C -space concept provide a generalized framework to study the motion planning problem

City College of New York 14 What if the robot is not a point? The Pioneer-II robot should probably not be modeled as a point...

City College of New York 15 What if the robot is not a point? Expand obstacle(s) Reduce robot not quite right...

City College of New York 16 Obstacles Configuration Space C -obstacle Point robot

City College of New York 17 Free Space From Robot Motion Planning J.C. Latombe

City College of New York 18 Minkowski Sums This expansion of one planar shape by another is called the Minkowski sum  P R P  R Used in robotics to ensure that there are free paths available. P  R = { p + r | p  P and r  R } Rectangular robot which can translate only (Dilation operation)

City College of New York 19 Additional Dimension What would the C -obstacle be if the rectangular robot (red) can translate and rotate in the plane. (The blue rectangle is an obstacle.) x y Rectangular robot which can translate and rotate

City College of New York 20 C -obstacle in 3-D x y 0º 180º 360º this is twisted... What would the C -obstacle be if the rectangular robot (red) can translate and rotate in the plane. (The blue rectangle is an obstacle.) 3-D

City College of New York 21 C -obstacle in 3-D What would the configuration space of a 3DOF rectangular robot (red) in this world look like? (The obstacle is blue.) x y 0º 180º can we stay in 2d ? 3-D

City College of New York 22 One slice Taking one slice of the C -obstacle in which the robot is rotated 45 degrees... x y 45 degrees How many slices does P  R have? P R P  R

City College of New York 23 2-D projection why not keep it this simple? x y

City College of New York 24 Projection problems q init q goal too conservative!

City College of New York 25 Topics Configuration Space Motion Planning Methods –Roadmap Approaches –Cell Decomposition –Potential Fields –Bug Algorithms

City College of New York 26 Motion Planning Methods The motion planning problem consists of the following: InputOutput geometric descriptions of a robot and its environment (obstacles) initial and goal configurations a path from start to finish (or the recognition that none exists) q goal q robot What to do? Applications Robot-assisted surgery Automated assembly plans Drug-docking and analysis Moving pianos around...

City College of New York 27 Motion Planning Methods (1) Roadmap approaches (2) Cell decomposition (3) Potential Fields (4) Bug algorithms Goal reduce the N-dimensional configuration space to a set of one-D paths to search. Goal account for all of the free space Goal Create local control strategies that will be more flexible than those above Limited knowledge path planning

City College of New York 28 Roadmap: Visibility Graphs Visibility graphs: In a polygonal (or polyhedral) configuration space, construct all of the line segments that connect vertices to one another (and that do not intersect the obstacles themselves). From C free, a graph is defined Converts the problem into graph search. Dijkstra’s algorithm O(N^2) N = the number of vertices in C -space Formed by connecting all “visible” vertices, the start point and the end point, to each other. For two points to be “visible” no obstacle can exist between them Paths exist on the perimeter of obstacles

City College of New York 29 The Visibility Graph in Action (Part 1) First, draw lines of sight from the start and goal to all “visible” vertices and corners of the world. start goal

City College of New York 30 The Visibility Graph in Action (Part 2) Second, draw lines of sight from every vertex of every obstacle like before. Remember lines along edges are also lines of sight. start goal

City College of New York 31 The Visibility Graph in Action (Part 3) Second, draw lines of sight from every vertex of every obstacle like before. Remember lines along edges are also lines of sight. start goal

City College of New York 32 The Visibility Graph in Action (Part 4) Second, draw lines of sight from every vertex of every obstacle like before. Remember lines along edges are also lines of sight. start goal

City College of New York 33 The Visibility Graph (Done) Repeat until you’re done. start goal Since the map was in C -space, each line potentially represents part of a path from the start to the goal.

City College of New York 34 Visibility graph drawbacks Visibility graphs do not preserve their optimality in higher dimensions: In addition, the paths they find are “semi-free,” i.e. in contact with obstacles. shortest path shortest path within the visibility graph No clearance

City College of New York 35 “official” Voronoi diagram ( line segments make up the Voronoi diagram isolates a set of points) Roadmap: Voronoi diagrams Generalized Voronoi Graph (GVG): locus of points equidistant from the closest two or more obstacle boundaries, including the workspace boundary. Property: maximizing the clearance between the points and obstacles.

City College of New York 36 Roadmap: Voronoi diagrams GVG is formed by paths equidistant from the two closest objects maximizing the clearance between the obstacles. This generates a very safe roadmap which avoids obstacles as much as possible

City College of New York 37 Voronoi Diagram: Metrics Many ways to measure distance; two are: –L1 metric (x,y) : |x| + |y| = const –L2 metric (x,y) : x 2 +y 2 = const

City College of New York 38 Voronoi Diagram (L1) Note the lack of curved edges

City College of New York 39 Voronoi Diagram (L2) Note the curved edges

City College of New York 40 Motion Planning Methods Roadmap approaches Visibility Graph Voronoi Diagram Cell decomposition Exact Cell Decomposition (Trapezoidal) Approximate Cell Decomposition (Quadtree) Potential Fields Hybrid local/global

City College of New York 41 Exact Cell Decomposition Decomposition of the free space into trapezoidal & triangular cells Connectivity graph representing the adjacency relation between the cells (Sweepline algorithm) Trapezoidal Decomposition:

City College of New York 42 Exact Cell Decomposition Search the graph for a path (sequence of consecutive cells) Trapezoidal Decomposition:

City College of New York 43 Exact Cell Decomposition Transform the sequence of cells into a free path (e.g., connecting the mid- points of the intersection of two consecutive cells) Trapezoidal Decomposition:

City College of New York 44 Obtaining the minimum number of convex cells is NP-complete. Optimality there may be more details in the world than the task needs to worry about cells9 cells Trapezoidal decomposition is exact and complete, but not optimal Trapezoidal Decomposition:

City College of New York 45 Quadtree Decomposition: Approximate Cell Decomposition recursively subdivides each mixed obstacle/free (sub)region into four quarters... Quadtree:

City College of New York 46 further decomposing... recursively subdivides each mixed obstacle/free (sub)region into four quarters... Quadtree: Quadtree Decomposition:

City College of New York 47 further decomposing... Again, use a graph-search algorithm to find a path from the start to goal Quadtree is this a complete path-planning algorithm? i.e., does it find a path when one exists ? Quadtree Decomposition: The rectangle cell is recursively decomposed into smaller rectangles At a certain level of resolution, only the cells whose interiors lie entirely in the free space are used A search in this graph yields a collision free path

City College of New York 48 Motion Planning Methods Roadmap approaches Cell decomposition Exact Cell Decomposition (Trapezoidal) Approximate Cell Decomposition (Quadtree) Potential Fields Hybrid local/global

City College of New York 49 Potential Field Method Potential Field (Working Principle) – The goal location generates an attractive potential – pulling the robot towards the goal – The obstacles generate a repulsive potential – pushing the robot far away from the obstacles – The negative gradient of the total potential is treated as an artificial force applied to the robot -- Let the sum of the forces control the robot C- obstacles

City College of New York 50 Compute an attractive force toward the goal Potential Field Method C- obstacles Attractive potential

City College of New York 51 Potential Field Method Repulsive Potential  Create a potential barrier around the C-obstacle region that cannot be traversed by the robot’s configuration  It is usually desirable that the repulsive potential does not affect the motion of the robot when it is sufficiently far away from C-obstacles Compute a repulsive force away from obstacles

City College of New York 52 Compute a repulsive force away from obstacles Potential Field Method Repulsive Potential

City College of New York 53 Sum of Potential Potential Field Method C- obstacle Attractive potentialRepulsive potential Sum of potentials

City College of New York 54 After get total potential, generate force field (negative gradient) Let the sum of the forces control the robot To a large extent, this is computable from sensor readings Equipotential contours Negative gradient Total potential Potential Field Method

City College of New York 55 random walks are not perfect... Potential Field Method Spatial paths are not preplanned and can be generated in real time Planning and control are merged into one function Smooth paths are generated Planning can be coupled directly to a control algorithm Pros: Trapped in local minima in the potential field Because of this limitation, commonly used for local path planning Use random walk, backtracking, etc to escape the local minima Cons:

City College of New York 56 Motion Planning Methods Roadmap approaches Visibility Graph Voronoi Diagram Cell decomposition Trapezoidal decomposition Quadtree decomposition Potential Fields Bug algorithm Full-knowledge motion planning Limited-knowledge path planning

City College of New York 57 Path planning with limited knowledge - Insect-inspired “bug” algorithms “reasonable” world 1) finite obstacles in any finite range 2) a line will intersect an obstacle finite times Bug Algorithms Goal Start known direction to goal only local sensing (walls/obstacles encoders)

City College of New York 58 Beginner Strategy “Bug” algorithm Switching between two simple behaviors: 1.Moving directly towards the goal 2.Circumnavigating an obstacle 1) head toward goal 2) follow obstacles until you can head toward the goal again 3) continue Insect-inspired “bug” algorithms assume a leftist robot

City College of New York 59 Summary Configuration Space Motion Planning Methods Roadmap approaches Cell decomposition Potential Fields Bug Algorithms

City College of New York 60 Thank you! Homework 8 is posted on the web Next class: Mapping