Path and trajectory planning

Lecture 4
Mikael Norrlöf

Up till now
Lecture 1
Rigid body motion
Representation of rotation
Homogenous transformation
Lecture 2
Kinematics
• Position
• Velocity via Jacobian
• DH parameterization
Lecture 3
Lagrange’s equation
(Newton Euler)
Parameter identification
• Experiment design
• Model structure

PhDCourse Robot modeling and control

Lecture 4

1
 Next lecture, @ABB

Feb 5
A bus (Wärnelius Buss) will leave from
Linköping University (taxi stand outside
Kårallen) at 07:15.
The lectures start at ABB Robotics
(ABB Robotics, Västerås, Finnslätten,
bnr 331) at 10.15. Acknowledge that you
are going to attend by
Lunch will be at Magneten ~ at 12 sending an email to
At 15 the bus will leave Västerås to [email protected]
come back to Linköping before Feb 2!!

The bus will arrive in Linköping approx

17.45

PhDCourse Robot modeling and control

Lecture 4

Outline

Path vs trajectory
Standard path planning techniques in industrial
robots
Trajectory generation – an introduction to the
problem
More general path planning algorithms
Potential field approach
An introduction to Probabilistic Road Maps (PRMs)
Rapid and robot programming (in Part II)

PhDCourse Robot modeling and control

Lecture 4

2
 Path and trajectory planning

What is the difference between path and trajectory?

Path: Only geometric Trajectory: Include time,

considerations. The way to i.e., consider the dynamics.
go from config a to b.

Compare: kinematics versus dynamics.

PhDCourse Robot modeling and control

Lecture 4

Path planning

In industrial robot applications two path planning

modes can be identified
Change configuration

Perform an operation

PhDCourse Robot modeling and control

Lecture 4

3
 Path planning

In industrial robot applications two path planning

modes can be identified
Change configuration
In RAPID (ABB’s programming language)
MoveJ p10, v1000, z10, tool;

Perform an operation
MoveL p20, v50, z1, tool;
MoveC p30, p40, v25, fine, tool;

PhDCourse Robot modeling and control

Lecture 4

Rapid, robot motion instructions

Linear in
joint space: MoveJ
Cartesian space: MoveL

Circle segment
MoveC

PhDCourse Robot modeling and control

Lecture 4

4
 Via points

Point-to-point (fine points in Rapid). Robot has to

stop.
p20
p40

p30
p10

Via points (zones in Rapid). Robot does not reach the

programmed position.

p20
p40

p30
p10

PhDCourse Robot modeling and control

Lecture 4

Geometric description
0.7
4 2 4

0.6

0.5

0.4

0.3
q(lc)

0.2

0.1

Inverse 0

-0.1

kinematics -0.2
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9
lc

One possible
1.45
parameterization is
cubic splines.
1.4

1.35

1.3
z

1.25

1.2 0 See MSc theses, “Path planning for

1.15
0.2
0.1
industrial robots” by Maria Nyström and
1 0.3 “Path generation in 6-DOF for industrial
robots” by Daniel Forssman.
0.9
0.4
0.8
0.7 0.5
y
Chap 5 in “Modeling and Control of Robot
x
Manipulators”, Sciavicco and Siciliano

PhDCourse Robot modeling and control

Lecture 4

5
 Spline

Cubic spline?

q(l) = a l3 + b l2 + c l + d, l ∈ [l1, l2[

Given q(l1), q(l2), q´(l1), q´(l2) the

parameters a, b, c and d can be derived
uniquely.
“Complete spline”

PhDCourse Robot modeling and control

Lecture 4

Orientation interpolation

The orientation along the path is interpolated to get a

smooth change of orientation.
Given initial orientation (as a quaternion), q0 and final
orientation q1 compute
q01 = q1q0-1
and interpolate the angle α from 0 to θ01 in
qip(α) = <cos(α/2), sin(α/2) s01>
where (θ01,s01) is the angle-axis representation of q01
The interpolation scheme is often referred to as slerp
interpolation

PhDCourse Robot modeling and control

Lecture 4

6
 Description of the trajectory generation problem

70

PhDCourse Robot modeling and control

Lecture 4

Description of the trajectory generation problem

70

PhDCourse Robot modeling and control

Lecture 4

7
 The trajectory generation problem

Optimal control!

qf

q0

PhDCourse Robot modeling and control

Lecture 4

The trajectory generation problem

Optimal control!

The path is parameterized in

some index l qr(l) which
qf
introduces additional
constraints.
q0

PhDCourse Robot modeling and control

Lecture 4

8
 Path parameterization

Path: P(l), q(l)

Dynamic properties are given by l(t)

PhDCourse Robot modeling and control

Lecture 4

The trajectory generation problem

Resulting optimization problem

Complexity:
Complexity:
•• Robots
Robots66
•• AArobot
robotcan
canhave
havemany
many
dynamic
dynamicconstraints
constraints

Total
Totalof
ofapprox
approx700
700
inequalities.
inequalities.

PhDCourse Robot modeling and control

Lecture 4

9
 Dynamic scaling of trajectories

The dynamic model can be rewritten in the form

D( q )q + C( q , q )q + g( q ) = τ
Γ( q )[ qq ]

Let r = ct (time scaling). The original speed/acc dep

torque is
τ s = D( q )q + Γ( q )[ qq ]
If time scaling is applied
τ s = r 2τ s + rD( q( r ))q′( r )
and with linear time scaling
τ s = c 2τ s

PhDCourse Robot modeling and control

Lecture 4

Trajectory generation problem

vd = 500 mm/s
1.15

1.1

vd = 1000 mm/s
1.05
z

xx
1
De
0.95 c . le
ng
th 0.4
0.9
1 0.3
0.8 0.2
0.6
0.4 0.1
0.2 0
y x

PhDCourse Robot modeling and control

Lecture 4

10
 Trajectory generation problem

vd = 500 mm/s
1.15

1.1

vd = 1000 mm/s
1.05
z

xx
1

0.95

0.9 0.4
1 0.3
0.8 0.2
0.6
0.4 0.1
0.2 0
y x

PhDCourse Robot modeling and control

Lecture 4

Dynamisk optimering

1.15

1.1

1.05
z

0.95 v d = 500 mm/s

0.9 0.4 1.15
1 0.3
0.8 0.2

OO
0.6
0.4 0.1 1.1
0.2 0
y x

PP
1.05
v d = 1000 mm/s
z

xx
1
TT
q(t)
0.95

II 0.9 0.4

M
1 0.3

M 0.8
0.6
0.2

EE 0.4 0.1
0.2 0
y x

RR 1.5

II 1

NN
GG 0.5
q(t)

-0.5

-1
0 0.5 1 1.5 2 2.5
Time [s ec]

PhDCourse Robot modeling and control

Lecture 4

11
 Application to a robot system
Use more than one robot to
increase the flexibility in an
application.

Here with application arc

weld.

PhDCourse Robot modeling and control

Lecture 4

A more general path planning scheme

PhDCourse Robot modeling and control

Lecture 4

12
 Configuration space

A complete specification of the location of every point

on the robot is a configuration (Q).

The set of all configurations is called the configuration

space.

q + kinematics give configuration.

PhDCourse Robot modeling and control

Lecture 4

Obstacles

The workspace of the robot is W. The subset of W

occupied by the robot is A(q).

The configuration space obstacle is defined as

QO = {q ∈ Q A( q ) ∩ O ≠ ∅}

with O = ∪Oi

The collision free configurations are

Qfree = Q \ QO

PhDCourse Robot modeling and control

Lecture 4

13
 Collision detection

A number of packages exists on the internet:

One example is from the group “team gamma” at Berkley
http://www.cs.unc.edu/~geom/collide/packages.shtml
Another at University of Oxford
http://web.comlab.ox.ac.uk/people/Stephen.Cameron/distances/
See also wikipedia
http://en.wikipedia.org/wiki/Collision_detection
An application where collision detection is used can
be found in MSc thesis
http://www.control.isy.liu.se/student/exjobb/xfiles/2050.pdf

PhDCourse Robot modeling and control

Lecture 4

General formulation of the path planning problem

Find a collision free path from an initial configuration qs

to a final configuration qf.

More formally γ : [ 0 ,1 ] → Qfree

with γ (0) = qs and γ (1) = qf.

Examples of methods:
Path planning using potential fields
Probabilistic road maps (PRMs)

PhDCourse Robot modeling and control

Lecture 4

14
Slide from: http://ai.stanford.edu/~latombe/

PhDCourse Robot modeling and control

Lecture 4

Artificial potential field

Idea: Treat the robot as a point particle in the

configuration space, influenced by an artificial
potential field. Construct the field U such that the
robot is attracted to the final configuration qf while
being repelled from the boundaries of QO.

In general it is difficult/impossible to construct a field

without having local minima.

PhDCourse Robot modeling and control

Lecture 4

15
 Construction of the field U

U can be constructed as an addition of an attractive

field and a second component that repels the robot
from the boundary of QO
U(q) = Uatt(q) + Urep(q)
Path planning can be treated as an optimization
problem, finding the global minimum of U(q). One
simple approach is to use a gradient descent
algorithm.
Let τ ( q ) = −∇U ( q ) = −∇U att ( q ) − ∇U rep ( q )

PhDCourse Robot modeling and control

Lecture 4

Construction of the field U

Comments
In general difficult to construct the potential field in
configuration space (the field is often based on the
norm of the min length to the obstacles)
Easier to define the field in the robot workspace

For an n-link manipulator a potential field is

constructed for each DH-frame
The link between workspace and configuration space
is the Jacobian

PhDCourse Robot modeling and control

Lecture 4

16
 The attractive field

Conic well potential (Uatt,i(q) = ||oi(q) - oi(qf)||)

Parabolic well potential (Uatt,i(q) = ½ ξi ||oi(q) - oi(qf)||2)
Parabolic well potential with upper bound
2
1
2
ζ i oi ( q ) − oi ( q f ) , oi ( q ) − oi ( q f ) ≤ d
U att ,i ( q ) = 2 1
dζ i oi ( q ) − oi ( q f ) − d 2ζ i , oi ( q ) − oi ( q f ) > d
2

and
(
− ζ i oi ( q ) − oi ( q f ) , ) oi ( q ) − oi ( q f ) ≤ d
Fatt ,i ( q ) = oi ( q ) − oi ( q f )
dζ i , oi ( q ) − oi ( q f ) > d
oi ( q ) − oi ( q f )

PhDCourse Robot modeling and control

Lecture 4

The repulsive field

Criteria for the repulsive field to satisfy,

Repel the robot from obstacles (never allow the robot
to collide)
Obstacles should not affect the robot when being far
away

PhDCourse Robot modeling and control

Lecture 4

17
 Path planning – obstacle free path

Notice:
Including only the origin of the DH-frames does not guarantee
collision free path. (Additional points can however be added.)

PhDCourse Robot modeling and control

Lecture 4

The repulsive field

One possible choice

2
1 1
U rep ,i ( q ) =
1
ηi − , ρ ( oi ( q )) ≤ ρ 0
2
ρ ( oi ( q )) ρ 0
0, ρ ( oi ( q )) > ρ 0
with
1 1 1
Frep ,i ( q ) = η i − ∇ρ ( oi ( q ))
ρ ( oi ( q )) ρ 0 ρ 2 ( oi ( q ))

If the obstacle region is convex and b is the closest point to oi

ρ ( oi ( q )) = oi ( q ) − b ,
oi ( q ) − b
∇ρ ( x ) x = o ( q ) =
i oi ( q ) − b

PhDCourse Robot modeling and control

Lecture 4

18
 Comment on the “convex assumption”

The force vector has a

discontinuity
Distance function not
differentiable everywhere

Can be avoided if repulsive

fields of distinct obstacles
do not overlap.

PhDCourse Robot modeling and control

Lecture 4

Mapping the workspace forces into joint torques

If a force is exerted at the end-effector

J vT F = τ

The Jacobian can be derived in all the points oi.

Notice that the full Jacobian can be used when mapping

forces and torques from workspace to torques in
configuration space.

PhDCourse Robot modeling and control

Lecture 4

19
 Path construction in configuration space

Build the path using the resulting configuration space torques

and an optimization algorithm

Gradient descent algorithm

Design parameters, αi, ζi, ηi, ρ0.

Typical problem: Can get stuck in local minima.

PhDCourse Robot modeling and control

Lecture 4

Escape local minima using randomization

When stuck in a local minimum execute a random walk

New problems:
Detect when a local minimum is reached
Define how the random walk should behave (how many steps,
define the random terms, variance, distribution, …)

PhDCourse Robot modeling and control

Lecture 4

20
 A more systematic way to build collision free paths

Slide from: http://ai.stanford.edu/~latombe/

The cost of computing an exact representation of the
configuration space of a multi-joint articulated object is often
prohibitive.
But very fast algorithms exist that can check if an articulated
object at a given configuration collides with obstacles.
Basic idea of Probabilistic Roadmaps (PRMs):
Compute a very simplified representation of the free
space by sampling configurations at random.

PhDCourse Robot modeling and control

Lecture 4

Probabilistic Roadmap (PRM)

forbidden space free space

Space ℜ
Slide from: http://ai.stanford.edu/~latombe/

PhDCourse Robot modeling and control

Lecture 4

21
 Probabilistic Roadmap (PRM)

Configurations are sampled by picking coordinates at random

Slide from: http://ai.stanford.edu/~latombe/

PhDCourse Robot modeling and control
Lecture 4

Probabilistic Roadmap (PRM)

Configurations are sampled by picking coordinates at random

Slide from: http://ai.stanford.edu/~latombe/

PhDCourse Robot modeling and control

Lecture 4

22
 Probabilistic Roadmap (PRM)

Sampled configurations are tested for collision (in workspace!)

Slide from: http://ai.stanford.edu/~latombe/

PhDCourse Robot modeling and control
Lecture 4

Probabilistic Roadmap (PRM)

The collision-free configurations are retained as “milestones”

Slide from: http://ai.stanford.edu/~latombe/

PhDCourse Robot modeling and control

Lecture 4

23
 Probabilistic Roadmap (PRM)

Each milestone is linked by straight paths to its k-nearest neighbors

Slide from: http://ai.stanford.edu/~latombe/

PhDCourse Robot modeling and control
Lecture 4

Probabilistic Roadmap (PRM)

Each milestone is linked by straight paths to its k-nearest neighbors

Slide from: http://ai.stanford.edu/~latombe/

PhDCourse Robot modeling and control

Lecture 4

24
 Probabilistic Roadmap (PRM)

The collision-free links are retained to form the PRM

Slide from: http://ai.stanford.edu/~latombe/

PhDCourse Robot modeling and control
Lecture 4

Probabilistic Roadmap (PRM)

The start and goal configurations are included as milestones

Slide from: http://ai.stanford.edu/~latombe/

PhDCourse Robot modeling and control

Lecture 4

25
 Probabilistic Roadmap (PRM)

The PRM is searched for a path from s to g

Slide from: http://ai.stanford.edu/~latombe/

PhDCourse Robot modeling and control
Lecture 4

Comments

In industrial robotic applications the path planning

problem is very much left to the user
New ideas from mobile robotics (potential field
algorithms, PRMs, etc. could be applied)
Automatic planning algorithms highly complex

The trajectory generation problem can be solved by

applying optimal control techniques
Conceptually easy to solve the offline problem
Difficult to implement online

PhDCourse Robot modeling and control

Lecture 4

26
 Comments

Other planning requirements in many applications

PhDCourse Robot modeling and control

Lecture 4

27