CS 603 - Path Planning Rod Grupen 4/23/20 Robotics 1 4/23/20 - - PowerPoint PPT Presentation

1

4/23/20 Robotics 1

CS 603 - Path Planning

Rod Grupen

4/23/20 Robotics 2

Why Path Planning?

4/23/20 Robotics 3

Why Motion Planning?

Virtual Prototyping Character Animation Structural Molecular Biology Au Autonomous Control

GE

4/23/20 Robotics 4

Origins of Motion Planning

• 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

2

4/23/20 Robotics 5

Completeness of Planning Algorithms

a co complete planner finds a path if one exists resolution complete – complete to the model resolution probabilistically complete

Representation

…given a moving object, A, initially in an unoccupied region of freespace, s, a set of stationary objects, Bi , at known locations, and a goal position, g, …

4/23/20 Robotics 6

find a sequence of collision-free motions that take A from s to g

9

Mapping to Configuration Space - Translational Case (fixed orientation)

Robot Obstacle Reference Point C-Space Obstacle

changing q ?

4/23/20 Robotics 10

Obstacles in 3D (x,y,q)

Jean-Claude Latombe

3

4/23/20 Robotics 12

Exact Cell Decomposition

Jean-Claude Latombe

4/23/20 Robotics 13

Exact Cell Decomposition

Jean-Claude Latombe

4/23/20 Robotics 14

Exact Cell Decomposition

Jean-Claude Latombe

Representation – Simplicial Decomposition

4/23/20 Robotics 15

Schwartz and Sharir Lozano-Perez Canny

4

4/23/20 Robotics 16

Approximate Methods: 2n-Tree

4/23/20 Robotics 17

Approximate Cell Decomposition

Jean-Claude Latombe

again…build a graph and search it to find a path

Representation – Roadmaps

4/23/20 Robotics 18

Visibility diagrams: unsmooth sensitive to error

4/23/20 Robotics 19

Roadmap Representations

Jean-Claude Latombe

Voronoi diagrams a “retraction” …the continuous freespace is represented as a network of curves…

5

4/23/20 Robotics 29

Summary

• Exact Cell Decomposition
• Approximate Cell Decomposition

u graph search u next: potential field methods

• Roadmap Methods
• visibility graphs
• Voronoi diagrams
• next: probabilistic road maps (PRM)

state of the art techniques

30

Attractive Potential Fields

+

• Oliver Brock

31

Repulsive Potentials

+

• Oliver Brock

32

Electrostatic (or Gravitational) Field

depends on direction

6

33

Attractive Potential

F

att(q) = −∇φatt(q)

= −k (q − qref ) φatt(q) = 1 2 k (q − qref )T (q − qref )

34

A Repulsive Potential

Th e pic tur e ca n't be dis pla ye d.

F

rep(q) = 1

x

F

rep(q) = 1

x − 1 δ0

x

4/23/20 Robotics 35

Repulsive Potential

Frep(q) = −∇φ(q) = −k 1 (q− qobs) − 1 δ0 # $ % % & ' ( ( if ((q− qobs) <δ0

• therwise

) * + , +

• .

+ / +

φrep(q) = k 1 (q− qobst) − 1 δ0 ! " # # $ % & &

2

if ((q− qobs) <δ0 = 0

• therwise

36

Sum Attractive and Repulsive Fields

7

37

Artificial Potential Function

+ =

F

total(q) = −∇φtotal

φatt(q) + φrep(q) = φtotal(q)

4/23/20 Robotics 38

Potential Fields

• Goal: avoid local minima
• Problem: requires global information
• Solution: Navigation Function

Robot Obstacle Goal

Fatt Frep Fatt Frep

Navigation Functions

Analyticity – navigation functions are analytic because they are infinitely differentiable and their Taylor series converge to φ(q0) as q approaches q0 Polar – gradients (streamlines) of navigation functions terminate at a unique minima

4/23/20 Robotics 39

Navigation Functions

4/23/20 Robotics 40

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. Admissibility - practical potential fields must always generate bounded torques

8

The Hessian

multivariable control function, f(q0,q1,...,qn)

4/23/20 Robotics 41

if the Hessian is positive semi-definite over the domain Q, then the function f is convex over Q

Harmonic Functions

4/23/20 Robotics 42

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

Properties of Harmonic Functions

4/23/20 Robotics 43

Min-Max Property - ...in any compact neighborhood of freespace, the minimum and maximum of the function must occur

• n the boundary.

Properties of Harmonic Functions

4/23/20 Robotics 44

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.

¼ ¼ ¼ ¼

analog & numerical methods

9

Numerical Relaxation

4/23/20 Robotics 45

Jacobi iteration

4/23/20 Robotics 46

Harmonic Relaxation: Numerical Methods

Gauss-Seidel Successive Over Relaxation

Properties of Harmonic Functions

4/23/20 Robotics 47

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.

Minima in Harmonic Functions

4/23/20 Robotics 48

for some i, if ∂2φ/∂xi2 > 0 (concave upward), then there must exist another dimension, j, where ∂2φ/∂xj2 < 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...

10

Configuration Space

4/23/20 Robotics 49

Harmonic Functions for Path Planning

4/23/20 Robotics 50

Harmonic Functions for Path Planning

4/23/20 Robotics 51

Reactive Admittance Control

4/23/20 Robotics 52

11

4/23/20 Robotics 54

• k, back to graphical methods…

4/23/20 Robotics 55

Probabilistic Roadmaps (PRM)

• 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/23/20 Robotics 56

Probabilistic Roadmaps (PRM)

Oliver Brock

4/23/20 Robotics 58

Sampling Phase

• 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

12

4/23/20 Robotics 59

Path Extraction

• Connect start and goal configuration to roadmap

using local planner

• Perform graph search on roadmap
• Computational cost of searching negligible

compared to construction of roadmap

4/23/20 Robotics 61

Local Planner

q1 q2

tests up to a specified resolution d! d

4/23/20 Robotics 62

Another Local Planner

perform random walk of predetermined length; choose new direction randomly after hitting obstacle; attempt to connect to roadmap after random walk

4/23/20 Robotics 65

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

13

4/23/20 Robotics 68

Variations of the PRM

• Lazy PRMs
• Rapidly-exploring Random Trees

4/23/20 Robotics 69

Lazy PRM

• bservation: pre-computation of roadmap takes a

long time and does not respond well in dynamic environments

4/23/20 Robotics 70

Lazy PRM

Oliver Brock

4/23/20 Robotics 74

Rapidly-Exploring Random Trees (RRT)

Oliver Brock

14

4/23/20 Robotics 75

Rapidly-Exploring Random Trees (RRT)

Steven LaValle