Chapter 5: Path Planning

Hadi Moradi

Motivation

• Need to choose a path for the end effector that avoids collisions and singularities

• Collisions are easy to define in the workspace, but need to be mapped into the configuration space for convenience

Workspace v. configuration space

• Workspace: volume swept out by the end effector (in inertial frame)

• Configuration: location of all points on a robotic manipulator

• Configuration space:

Obstacles

• Discrete obstacles are denoted Oi (in the workspace)

• Denote the robot as A(q) at configuration q• The configuration space obstacle, QO, is defined as:

• The free configuration space is the space of all collision-free configurations:

Motion Planning for a Point RobotMotion Planning for a Point Robot

free space

s

g

free path

ProblemProblem

semi-free path

Types of Path ConstraintsTypes of Path Constraints

Local constraints: lie in free space

Differential constraints: have bounded curvature

Global constraints: have minimal length

Motion-Planning FrameworkMotion-Planning Framework

Continuous representation

Discretization

Graph searching(blind, best-first, A*)

Example: Visibility Graph (A Roadmap Method)Example: Visibility Graph (A Roadmap Method)

Visibility graphIntroduced in the Shakey project at SRI in the late 60s. Can produce shortest paths in 2-D configuration spaces g

s

Example: Voronoi Diagram (A Roadmap Method)Example: Voronoi Diagram (A Roadmap Method)

Voronoi diagram Introduced by Computational Geometry researchers. Generate paths that maximizes clearance.

O(n log n) timeO(n) space

Cell-Decomposition MethodsCell-Decomposition Methods

Two classes of methods: Exact cell decomposition Approximate cell decomposition

F is represented by a collection of non-overlapping cells whose union is contained in FExamples: quadtree, octree, 2n-tree

Approximate Cell Decomposition: Quad Tree

Octree Decomposition (3D environment)Octree Decomposition (3D environment)

Potential Field MethodsPotential Field Methods

Goal

Robot

Goal

Robot

Approach initially proposed for real-time collision avoidance [Khatib, 86]. Hundreds of papers published on it.

Attractive and Repulsive fieldsAttractive and Repulsive fields

Potential Fields

Local-Minimum IssueLocal-Minimum Issue

Perform best-first search (possibility of combining with approximate cell decomposition) Alternate descents and random walks Use local-minimum-free potential (navigation function)

Ex: 2D Cartesian manipulator

• The configuration space is R2

• Consider only one object in the workspace– End effector and obstacle are convex polygons

• What is the configuration space obstacle?

Ex: 2D Cartesian manipulator

• The nice thing about this example is that the workspace and the configuration space are identical

Ex: planar two-link manipulator

• What is the configuration space obstacle for a two-link manipulator

MotivationMotivation

• Geometric complexity• Space dimensionality

Path planning overview

• Want to find a path from an initial position to a final position

Potential fields

• To develop the mapping, we incrementally explore Qfree• Consider the manipulator (statically) as a point in the

configuration space• The manipulator is subject to a potential field

– Attractive in the case of the goal configuration

– Repulsive in the case of an obstacle

qUqUqU repatt

Gradient descent

• In order to find minima of U, take the negative gradient:

qUqUqUq repatt

The attractive field

• We define a potential field that attracts each of the n DH coordinate frames from the initial position to the goal position

The attractive field

• Simple potential field, conic well potential

The attractive field

• Instead we use a continually differentiable function: parabolic well potential – Field grows quadratically with the distance from the goal

configuration

2

, 2

1fiiiiatt qoqoqU

Hybrid attractive field

• Combine the conic well potential and parabolic well potential fields– If the ith frame is close to the workspace goal, use the parabolic well

– If the ith frame is far from the workspace goal, use the conic well

• The distance d defines the distance from the goal that causes a transition from a conic to parabolic potential

• Since this is continuous everywhere, the workspace force is defined everywhere

dqoqodqoqod

dqoqoqoqoqU

fiiifiii

fiifiii

iatt

for2

1

for2

1

2

2

,

Hybrid attractive field

• Taking the gradient gives the workspace attractive force

dqoqoqoqo

qoqod

dqoqoqoqo

qUqF

fiifii

fiii

fiifiii

iattiatt

for

for

,,

Ex: planar two link manipulator

• For the 2-link arm shown below, assume that both links have length 1

1

1o ,

0

2 ,

1

0o ,

0

1

2/

2/

0

02211 fsfsfs qqoqqoqq

The repulsive field

• Prevent collisions by creating a repulsive force in the workspace– Again, create forces that act on the origins of the n DH coordinate

frames

• These forces should:– Repel the robot from obstacles

– Do nothing of the robot is far away from obstacles

The repulsive field

• Therefore, the workspace repulsive force is:

• To evaluate this, consider the distance function (oi(q)) as (x) where x is a three dimensional vector:

qUqF irepirep ,,

The repulsive field

• So we can write this force as:

0

020,

for0

for111

qo

qoqoqoqoqF

i

ii

iii

irep

Ex: planar two link manipulator

• Consider a convex obstacle close to o2

– Obstacle is outside the distance of influence for o1

– Again, the lengths are both 1

– Let b be the point on the obstacle closest to o2

• b = [2 0.5]T

• (o2(qs)) = 0.5

– Let 0 = 1 (no influence on o1)

– The initial repulsive force on o2 is:

Other considerations

1. what happens if either there are multiple objects, or an object is not convex?

Other considerations

2. what if the obstacle is closest to another part of a link (i.e. not the origin of the DH frame)?

The relation between workspace forces and joint torques

FJ Tv

Ex: two-link planar manipulator

• Consider the previous examples with an obstacle exerting a repulsive force on o2

• Find the attractive and repulsive forces on o1 and o2

Initial and goal configurations

Obstacle location

Ex: two-link planar manipulator

• To determine the joint torques, take the transpose of the Jacobians at the initial configuration

Composing workspace forces

• The total joint torques acting on a manipulator is the sum of the torques from all attractive and repulsive potentials:

i

irepT

oi

iattT

o qFqJqFqJqii ,,

Ex: two-link planar manipulator

• Consider again the two-link manipulator with a goal position and an obstacle near o2

• The total joint torque, due to these two potential fields is:

Initial and goal configurations Obstacle location

Gradient descent Path Planning Algorithm

1. First, determine your initial configuration

2. Second, given a desired point in the workspace, calculate the final configuration using the inverse kinematics– Use this to create an attractive potential field

3. Locate obstacles in the workspace– Create a repulsive potential field

4. Sum the joint torques in the configuration space

5. Use gradient descent to reach your target configuration

Local minima

• In the absence of obstacles, the gradient descent will always converge to the global minimum (qf)

• With obstacles, by proper choice of i, this will always converge to some minima

Local minima

• Instead we modify the gradient descent algorithm to add a random excitation in case we are stuck in a local minima

• We are stuck in a local minima if successive iterations result in minimal changes in the configuration

• If so, perform a random walk to get out

• The random walk is defined by adding a uniformly distributed variable to each joint parameter

2 goto .4

to walkrandom

if .3

,...,, return

else

if 2.

,0 1.

1

1

10

1

0

qq

q

qq

qqq

i

q

qqq

qq

qqi

i

mii

i

i

iiii

fi

s

Next class…

• Applications to numerically solving for the inverse kinematics• Probabilistic methods