Robot Motion Planning and (a little) Computational Geometry Topics: Transforms Topological Methods Configuration space Skeletonization Potential Functions Cell-decomposition Methods Non-holonomic Motion Collision Avoidance Additional reading: Latombe, Jean-Claude. Robot motion planning. Boston : Kluwer Academic Publishers, 1991. E. Rimon and D. E. Koditschek. Exact Robot Navigation Using Artificial Potential Functions. IEEE Transactions on Robotics and Automation, 8(5):501518, October 1992. S. Thrun. Learning metric-topological maps for indoor mobile robot navigation. Artificial Intelligence, 99(1):21-71, 1998. Images taken from slides by B. Bayazit, G. Dudek, J. C. Latombe and A. Moore Issues ● Statement: Compute a collision-free path for a rigid or articulated object (the robot) among static obstacles ● Inputs: – Geometry of robot and obstacles – Kinematics of robot (degrees of freedom) – Initial and goal robot configurations (placements) ● Output: – Continuous sequence of collision-free robot configurations connecting the initial and goal configurations – Number of degrees of freedom? – Holonomic vs. Non-holonomic? – Kinematic vs. Kino-dynamic? – Planning and control architecture – Topological vs. Metric? – Optimality? Convex Polygons How to reason about this polygon? Minimum Covering: 7 triangles Complexity? Convex Hull Complexity? Minimum Partitioning: 12 triangles Complexity? Type Holes Complexity Convex Polygons Y NP-Hard Convex Polygons N O(N 3 ) Rectangles Y O(N 3/2 logN) Rectangles N O(N) Triangulation Y O(NlogN) Configuration Space ● The set of legal configurations of the robot ● Size of C-Space depends on degrees of freedom of robot A and B are convex polygons. A is a free-flying object. C is represented by parameterizing each configuration q by (x,y, ) R 2 x [0,2 ]. A A B B y y y x x x The represented C-obstacle is a volume bounded by patches of ruled surfaces. 3-d C-Space = /2q q p =0q q p? Image adapted from J. C. Latombe Homogeneous Transforms ● Convenient representation of rigid body transformations ● 2-D motion is ● Rotations and translations are now multiplications only ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??? ??? = ? ? ? ? ? ? ? ? ? ? 1100 )cos()cos( )cos()cos( 1 ' ' y x y x y x θθ θθ 2-D Space: Visibility Graphs What happens in 3-D C-space? This figure shows the visibility graph G of a simple configuration space in which CB consists of three disconnected regions. The links of G also include the edges of CB. The bold line is the shortest path in cl(C free ) between q init and q goal . q init q goal Image adapted from J. C. Latombe Voronoi Diagrams Voronoi Graph Delaunay Triangulation Generalized Voronoi Diagram Skeletonization: Grass-fire Transform Evolve discrete points on the polygon curves “Shocks” occur where wavefronts collide “Critical points” occur at centres of maximal disks Good for motion planning? Not really Images adapted from G.Dudek Cell-decomposition Approximate Cell Decomposition Variable-resolution Approximate Cell Decomposition Exact Cell Decomposition Images courtesy of A. Moore Potential Functions Attractive Potential for goals Repulsive Potential for obstacles Combined Potential Field Move along force: F(x) = ?U att (x)-?U rep (x) Khatib 1986 Latombe 1991 Koditschek 1998 Images courtesy of B. Bayazit Numerical Navigation Functions 1) put a grid GC on C-space 2) label each grid point in GC with a 0 if it is in C free and 1 otherwise 3) use L1 distance (Manhattan distance) to x goal as navigation function U 4) compute L1 distance from x init to x goal by wavefront propagation a) set U(x goal ) = 0 b) set U(x’) = 1 for each 1-neighbor of the goal c) set U(x’) = 2 for each 1-neighbor of nodes labelled in (b) d) loop until labeled x init or no more nodes to label Images courtesy of B. Bayazit Non-holonomic Motion 1) Two-phase planning: Compute collision-free path ignoring nonholonomic constraints ? Transform this path into a nonholonomic one ? Efficient, but possible only if robot is “controllable” ? Plus need to have “good” set of maneuvers 2) Direct planning: Build a tree of milestones until one is close enough to the goal (deterministic or randomized) ? Robot need not be controllable ? Works in high-dimensional c-spaces θ φ y x l Ackerman steering example ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = ? ? ? ? ? ? ? ? ? ? ? ? = =? ++?= ?= 0 tan 1 sin cos , 1 0 0 0 0 ]0cos)cos()sin([ ]00cossin[ 1 2 1 φ θ θ φ φ φφθφθ θθ l g qw lw w 2 2 1 g fixed a for go would wedirection the us tell to g wantnow we ononly depends steering the means which us tells Intuition && ?2 constraints (front and rear wheels) ?2 inputs (steering and gas pedal) ?4 states ? ? ? ? ? ? ? ? ? ? ? ? = φ θ y x q Collision Avoidance ● Dynamic Window ● Fox et al., 1996 ● Vector-Field Histogram ● Borenstein et al., 1991, 1998 ● Lane-Curvature ● Simmons and Ko, 1998 ● Local Gradient Descent ● Konolige 2002 What you should know Practical above 2 or 3-d? Practical above 8-d? Fast to compute? Gives optimal? Spots impossibilities? Easy to implement? Useful for real robots? P ot e n t i a l / N a v - i g a t i o n F i e l d s V o r o n oi D i a g r a m s V i s i b i l i t y G r a ph s C e l l D e c om pos i t i on Example questions ● Here’s a motion-planning problem. What representation would you use and why? ● Here’s a motion-planning problem. What would you use to solve it and why? ● Can you use one motion-planning algorithm to improve the performance of a second algorithm?