Introduction to ROBOTICS Robot Motion Planning Dr John

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

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

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 3

References • G. Dudek, M. Jenkin, Computational Principles of Mobile Robots, MIT Press, 2000 (Chapter 5) • J. C. Latombe, Robot Motion Planning, Kluwer Academic Publishers, 1991. • 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. 135 -145. – Potential Field • O. Khatib, “Real-Time Obstacle Avoidance for Manipulators and Mobile Robots”, Int. Journal of Robotics Research, 5(1), pp. 90 -98, 1986. • P. Khosla, R. Volpe, “Superquadratic Artificial Potentials for Obstacle Avoidance and Approach” Proc. Of ICRA, 1988, pp. 11781784. • B. Krogh, “A Generalized Potential Field Approach to Obstacle Avoidance Control” SME Paper MS 84 -484. City College of New York 4

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 5

Configuration Space Notation: A: single rigid object –(the robot) W: Euclidean space where A moves; B 1, …Bm: fixed rigid obstacles distributed in W • FW – world frame (fixed frame) • FA – 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 FW. Configuration Space is the space of all possible robot configurations. City College of New York 6

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

Configuration Space C y Z Cfree qgoal Cobs qstart x For a point robot moving in 3 -D, the C-space is What is the difference between Euclidean space and C-space? City College of New York 8

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

Configuration Space b b a a 2 R manipulator Configuration space City College of New York 10 topology

Configuration Space 360 qrobot 270 b 180 b a 90 qslug 0 45 Two points in the robot’s workspace a 90 135 180 Torus (wraps horizontally and vertically) City College of New York 11

Configuration Space If the robot configuration is within the blue area, it will hit the obstacle 360 qrobot 270 b 180 b a 90 qslug 0 An obstacle in the robot’s workspace What is dimension of the C-space of puma robot (6 R)? 45 a 90 135 a “C-space” representation 180 Visualization of high dimension C-space is difficult City College of New York 12

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

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 14

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

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

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

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

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. ) y Rectangular robot which can translate and rotate x City College of New York 19

C-obstacle in 3 -D 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 y 360º 180º x City College of New York 0º this is twisted. . . 20

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

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

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

Projection problems qinit qgoal City College of New York too conservative! 24

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

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

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

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). 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 From Cfree, a graph is defined Converts the problem into graph search. Dijkstra’s algorithm O(N^2) N = the number of vertices in C-space City College of New York 28

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. goal start City College of New York 29

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. goal start City College of New York 30

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. goal start City College of New York 31

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. goal start City College of New York 32

The Visibility Graph (Done) • Repeat until you’re done. goal start 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 33

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

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

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 36

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

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

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

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 40

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

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

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

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

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

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

further decomposing. . . 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 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 ? City College of New York 47

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 48

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 49

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

Potential Field Method • Compute a repulsive force away from obstacles Repulsive Potential v Create a potential barrier around the C-obstacle region that cannot be traversed by the robot’s configuration v 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 City College of New York 51

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

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

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

Potential Field Method Pros: • 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 Cons: • 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 random walks are not perfect. . . City College of New York 55

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

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

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

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

Thank you! Homework 8 is posted on the web Next class: Mapping Time: Nov. 25, Tue City College of New York 60