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 AnimationStructural Molecular BiologyAutonomous 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 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 PointC-SpaceObstacle

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 freespaceis 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 ondirection

6

33

Attractive Potential

Fatt (q) = −∇φatt (q)= −k (q− qref )

φatt (q) =12k (q− qref )

T (q− qref )

34

A Repulsive Potential

The picture can't be displayed.

Frep(q) =1x

Frep(q) =1x−1δ0

x

4/23/20 Robotics 35

Repulsive Potential

Frep (q) = −∇φ(q)

=−k 1

(q− qobs )−1δ0

#

$%%

&

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

0 otherwise

)

*+

,+

-

.+

/+

φrep (q) = k1

(q− qobst )−1δ0

!

"##

$

%&&

2

if ((q− qobs ) < δ0

= 0 otherwise

36

Sum Attractive and Repulsive Fields

7

37

Artificial Potential Function

+ =

Ftotal (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 overthe 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 on the boundary.

Properties of Harmonic Functions

4/23/20 Robotics 44

Mean-Value - up to truncation error, the value of theharmonic 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 asthe 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

ok, 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

observation: 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