CS Path Planning

Transcription

1 Why Path Planning? CS Path Planning Roderic A. Grupen 4/13/15 Robotics 1 4/13/15 Robotics 2 Why Motion Planning? Origins of Motion Planning Virtual Prototyping! Character Animation! Structural Molecular Biology! Autonomous Control! GE! T. Lozano-Pérez and M.A. Wesley: An Algorithm for Planning Collision-Free Paths Among Polyhedral Obstacles, 1979.! introduced the notion of configuration space (c-space) to robotics! many approaches have been devised since then in configuration space! 4/13/15 Robotics 3 4/13/15 Robotics 4 1

2 Representation given a moving object, A, initially in an unoccupied region of freespace, s, a set of stationary objects, B i, at known locations, and a goal position, g,!! Mapping to Configuration Space - Translational Case (fixed orientation) Obstacle! find a sequence of! collision-free motions! that take A from s to g! Robot! 4/13/15 Robotics 6 Reference Point! changing θ? C-Space! Obstacle! 9 Obstacles in 3D (x,y,q) Exact Cell Decomposition Jean-Claude Latombe! Jean-Claude Latombe! 4/13/15 Robotics 10 4/13/15 Robotics 12 2

3 Exact Cell Decomposition Exact Cell Decomposition Jean-Claude Latombe! 4/13/15 Robotics 13 Jean-Claude Latombe! 4/13/15 Robotics 14 Representation Simplicial Decomposition Approximate Methods: 2 n -Tree Schwartz and Sharir Lozano-Perez Canny 4/13/15 Robotics 15 4/13/15 Robotics 16 3

4 Approximate Cell Decomposition Representation Roadmaps Visibility diagrams: unsmooth sensitive to error again build a graph and search it to find a path! 4/13/15 Robotics 17 Jean-Claude Latombe! 4/13/15 Robotics 18 Roadmap Representations Summary Voronoi diagrams a retraction the continuous freespace is represented as a network of curves Exact Cell Decomposition! Approximate Cell Decomposition!! graph search!! next: potential field methods! state of the art techniques Roadmap Methods! visibility graphs! Voronoi diagrams! next: probabilistic road maps (PRM)! Jean-Claude Latombe! 4/13/15 Robotics 19 4/13/15 Robotics 20 4

5 Attractive Potential Fields Repulsive Potentials Oliver Brock + Oliver Brock Electrostatic (or Gravitational) Field Attractive Potential φ att (q) = 1 2 k (q q ref ) T (q q ref ) F att (q) = φ att (q) = k (q q ref ) depends on! direction!

6 A Repulsive Potential Repulsive Potential F rep (q) = 1 x! 1 φ rep (q) = k (q q obst ) 1 $ # " δ & 0 % 2 if ((q q obs ) < δ 0 = 0 otherwise x F rep (q) = 1 x 1 δ 0 25 F rep (q) = φ(q) ) = k # 1 (q q obs ) 1 & - + % $ δ ( if ((q q obs ) < δ 0 + * 0 '. + +, 0 otherwise / 4/13/15 Robotics 26 Sum Attractive and Repulsive Fields Artificial Potential Function φ att (q) + φ rep (q) = φ total (q) +! =! F total (q) = φ total

7 Potential Fields Goal: avoid local minima Problem: requires global information Solution: Navigation Function F rep! Robot! F att! F rep! Navigation Functions Analyticity navigation functions are analytic because they are infinitely differentiable and their Taylor series converge to φ(q 0 ) as q approaches q 0!! Polar gradients (streamlines) of navigation functions terminate at a unique minima! F att! Obstacle! Goal! 4/13/15 Robotics 29 4/13/15 Robotics 30 Navigation Functions The Hessian Morse - navigation functions have no degenerate critical points where the robot can get stuck short of attaining the goal. Critical points are places where the gradient of φ vanishes, i.e. minima, saddle points, or maxima and their images under φ are called critical values.! multivariable control function, f(q 0,q 1,...,q n )! Admissibility - practical potential fields must always generate bounded torques! if the Hessian is positive semi-definite over! the domain Q, then the function f is convex over Q! 4/13/15 Robotics 31 4/13/15 Robotics 32 7

8 Harmonic Functions Properties of Harmonic Functions if the trace of the Hessian (the Laplacian) is 0! then function f is a harmonic function! laminar fluid flow, steady state temperature distribution, electromagnetic fields, current flow in conductive media! 4/13/15 Robotics 33 Min-Max Property -!...in any compact neighborhood of freespace, the minimum and maximum of the function must occur on the boundary.!! 4/13/15 Robotics 34 Properties of Harmonic Functions Numerical Relaxation Mean-Value - up to truncation error, the value of the harmonic potential at a point in a lattice is the average of the values of its 2n Manhattan neighbors. Jacobi iteration! ¼ ¼ ¼ ¼ analog & numerical methods! 4/13/15 Robotics 35 4/13/15 Robotics 36 8

9 Harmonic Relaxation: Numerical Methods Properties of Harmonic Functions Gauss-Seidel Successive Over Relaxation Hitting Probabilities - if we denote p(x) at state x as! the probability that starting from x, a random walk process will reach an obstacle before it reaches a goal p(x) is known as the hitting probability!! greedy descent on the harmonic function minimizes the hitting probability.! 4/13/15 Robotics 37 4/13/15 Robotics 38 Minima in Harmonic Functions Configuration Space for some i, if 2 φ/ x i 2 > 0 (concave upward), then there must exist another dimension, j, where! 2 φ/ x j 2 < 0 to satisfy Laplace s constraint.! therefore, if you re not at a goal, there is always a way downhill......there are no local minima...! 4/13/15 Robotics 39 4/13/15 Robotics 40 9

10 Harmonic Functions for Path Planning Harmonic Functions for Path Planning 4/13/15 Robotics 41 4/13/15 Robotics 42 Harmonic Functions for Path Planning Reactive Admittance Control 4/13/15 Robotics 43 4/13/15 Robotics 44 10

11 Probabilistic Roadmaps (PRM) ok, back to graphical methods Construction! Generate random configurations! Eliminate if they are in collision! Use local planner to connect configurations! Expansion! Identify connected components! Resample gaps! Try to connect components! Query! Connect initial and final configuration to roadmap! Perform graph search! 4/13/15 Robotics 45 4/13/15 Robotics 46 Probabilistic Roadmaps (PRM) Sampling Phase Oliver Brock Construction! R = (V,E)! repeat n times:! generate random configuration! add to V if collision free! attempt to connect to neighbors using local planner, unless in same connected component of R! 4/13/15 Robotics 47 4/13/15 Robotics 49 11

12 Path Extraction Local Planner Connect start and goal configuration to roadmap using local planner! Perform graph search on roadmap! Computational cost of querying negligible compared to construction of roadmap! δ q 2 q 1 tests up to a specified resolution δ! 4/13/15 Robotics 50 4/13/15 Robotics 52 Another Local Planner Summary: PRM Algorithmically very simple! Surprisingly efficient even in high-dimensional C- spaces! Capable of addressing a wide variety of motion planning problems! One of the hottest areas of research! Allows probabilistic performance guarantees! perform random walk of predetermined length; choose new direction randomly after hitting obstacle; attempt to connect to roadmap after random walk 4/13/15 Robotics 53 4/13/15 Robotics 56 12

13 Variations of the PRM Lazy PRM Lazy PRMs! Rapidly-exploring Random Trees! observation: pre-computation of roadmap takes a long time and does not respond well in dynamic environments! 4/13/15 Robotics 59 4/13/15 Robotics 60 Lazy PRM Rapidly-Exploring Random Trees (RRT) Oliver Brock Oliver Brock 4/13/15 Robotics 61 4/13/15 Robotics 65 13

14 Rapidly-Exploring Random Trees (RRT) Steven LaValle! 4/13/15 Robotics 66 14