Robotic systems - Trajectory planning

Paolo Di Lillo

Università di Cassino e Lazio Meridionale

pa.dilillo@unicas.it
http://webuser.unicas.it/lai/robotica
academic year 2020/2021

Paolo Di Lillo Robotic systems - Trajectory planning 1 / 26

Trajectory planning

Trajectory planning
generation of the input to be used as references of the motion control
system

path and trajectory

operational space trajectories

Paolo Di Lillo Robotic systems - Trajectory planning 2 / 26

Limits of regulation control

A desired set-point is given to a nonlinear mechanical system (robot):

yd u
control robot
y

Control efforts mainly concentrated in the first instants

⇒ possible actuators saturation
Discontinuity in the control output
In industrial robotics, the controller generates a smooth trajectory
even when the user inputs a set-point

Paolo Di Lillo Robotic systems - Trajectory planning 3 / 26

Path and trajectory I
Actuator limits imply the generation of trajectories with a regular
curvature

Definitions:
path: locus of points in the operational space which the robot has
to follow in the execution of the assigned motion (geometric
description)
trajectory: path with an associated time law (e.g. velocity and/or
accelerations)

Paolo Di Lillo Robotic systems - Trajectory planning 4 / 26

Path and trajectory II
A trajectory planning algorithm is characterized generally by:
inputs:
path definition
path constraints
constraints imposed by the robot dynamics
output:
end-effector trajectory expressed as temporal sequence of the
position, velocity and acceleration

Paolo Di Lillo Robotic systems - Trajectory planning 5 / 26

Path and trajectory III

Useful to resort to a limited number of parameters

path
extreme points
eventually intermediate points
geometric primitives
motion law
total time
max velocity and/or max acceleration
velocity and/or acceleration in given points

Paolo Di Lillo Robotic systems - Trajectory planning 6 / 26

Joint space trajectories I
Generation of a function q(t) ∈ Rn which interpolates the assigned
values for the joint variables by satisfying the given constraints:
light computation load
joint pos., vel. and acc. continuous function of time
minimization of the undesired effects (e.g. regular curvature)
Point-to-point motion
extreme points and transition times
Motion through a sequence of points
extreme and intermediate points, transition times

Paolo Di Lillo Robotic systems - Trajectory planning 7 / 26

Joint space trajectories II
Point-to-point motion

generation of q(t) ∈ R which realizes the motion from qi to qf in the

time tf

Optimization problem on a simplified model minimizing the energy

Z tf
min τ 2 (t)dt
0

the solution is a quadratic polynomial in the velocity

Paolo Di Lillo Robotic systems - Trajectory planning 8 / 26

Joint space trajectories III
Let us choice the cubic polynomial

q(t) = a3 t3 + a2 t2 + a1 t + a0
q̇(t) = 3a3 t2 + 2a2 t + a1
q̈(t) = 6a3 t + 2a2

whose computation of the coefficients can be achieved by imposing null

initial and final velocities

a0 = qi
a1 = q̇i
a3 t3f + a2 t2f + a1 tf + a0 = qf
3a3 t2f + 2a2 tf + a1 = q̇f

Paolo Di Lillo Robotic systems - Trajectory planning 9 / 26

Joint space trajectories IV
Example
pos

[rad]
1

0 0.2 0.4 0.6 0.8 1

[s]
vel

cubic polynomial with assigned: 5

q(0) = 0, q(tf ) = π, 3

[rad/s]
2
q̇(0) = q̇(tf ) = 0, 1

tf = 1 s 0
0 0.2 0.4 0.6 0.8 1
[s]
acc

20

10
[rad/s^2]

−10

−20

0 0.2 0.4 0.6 0.8 1

[s]

Paolo Di Lillo Robotic systems - Trajectory planning 10 / 26

Joint space trajectories V
Trapezoidal velocity profile (practical), given tf it is defined as:

initial phase with constant acceleration

cruise phase with constant velocity
final with constant deceleration

Problem defined by assigning

q i , q f , tf

and
q̈c or q̇c

Paolo Di Lillo Robotic systems - Trajectory planning 11 / 26

Joint space trajectories VI

which provides as constraints:

qm − qc
q̈c tc =
tm − tc
1
qc = qi + q̈c t2c
2
q̈c t2c − q̈c tf tc + qf − qi = 0

Paolo Di Lillo Robotic systems - Trajectory planning 12 / 26

Joint space trajectories VII
If q̈c is assigned first check
4|qf − qi |
|q̈c | ≥ (with equal sign triangular vel. profile)
t2f

and
sign(q̈c ) = sign(qf − qi )
then we compute
s
tf 1 t2f q̈c − 4(qf − qi )
tc = −
2 2 q̈c
and thus a trajectory
1

2
qi + 2 q̈c t
 0 ≤ t ≤ tc
q(t) = qi + q̈c tc (t − tc /2) tc < t ≤ tf − tc
qf − 12 q̈c (tf − t)2

tf − tc < t ≤ tf


Paolo Di Lillo Robotic systems - Trajectory planning 13 / 26

Joint space trajectories VIII

Example
pos

[rad]
1

0 0.2 0.4 0.6 0.8 1

[s]
vel

q(0) = 0, q(tf ) = π, 5

q̇(0) = q̇(tf ) = 0, 3

[rad/s]
2
|q̈c | = 6π, 1

tf = 1 s 0
0 0.2 0.4 0.6 0.8 1
[s]
acc

20

10
[rad/s^2]

−10

−20

0 0.2 0.4 0.6 0.8 1

[s]

Paolo Di Lillo Robotic systems - Trajectory planning 14 / 26

Joint space trajectories IX
If q̇c is assigned, instead, first check
|qf − qi | 2|qf − qi |
< |q̇c | ≤
tf tf
then compute
qi − qf + q̇c tf
tc =
q̇c
q̇c2
q̈c =
qi − qf + q̇c tf
and thus a trajectory

1 2
qi + 2 q̈c t
 0 ≤ t ≤ tc
q(t) = qi + q̈c tc (t − tc /2) tc < t ≤ tf − tc
qf − 12 q̈c (tf − t)2

tf − tc < t ≤ tf


Paolo Di Lillo Robotic systems - Trajectory planning 15 / 26

Joint space trajectories X
The Matlab function is simply given by
function [q , dq , ddq ] = trapezoidal ( q_i , q_f , dq_c , tf , t )
%
% trapezoidal velocity profile at instant t
%
% [q , dq , ddq , err ] = trapezoidal ( q_i , q_f , dq_c , t_f , t )
%
% input :
% q_i dim nx1 initial configuration
% q_f dim nx1 final configuratio n
% dq_c dim nx1 cruise velocity
% tf dim 1 final time
% t dim 1 output time
% output :
% q dim nx1 position at t
% dq dim nx1 velocity at t
% ddq dim nx1 acceleration at t
...

Paolo Di Lillo Robotic systems - Trajectory planning 16 / 26

Joint space trajectories XI

with main commands for the i joint:

...
delta = q_f ( i ) - q_i ( i );
dq_c ( i ) = sign ( delta )* abs ( dq_c ( i ));
...
% constraint verification for joint i
dq_r = abs ( delta / tf );
err = ( abs ( dq_c ( i )) <= dq_r )|( abs ( dq_c ( i )) > 2* dq_r );
...
t_c = tf - delta / dq_c ( i );
ddq_c = dq_c ( i )/ t_c ;
% if on the time slots
if (t <= t_c )
q(i) = q_i ( i ) + 0.5* ddq_c * t ^2;
dq ( i ) = ddq_c * t ;
ddq ( i ) = ddq_c ;
elseif (t <=( tf - t_c ))
...

Paolo Di Lillo Robotic systems - Trajectory planning 17 / 26

Operational space trajectories I
Points of the path x(tk )
components xi (tk ) defined according to sequences of interpolating
polynomial
Path primitives
motion analytical expression

Paolo Di Lillo Robotic systems - Trajectory planning 18 / 26

Operational space trajectories II
Path primitives
Parametric representation of the a 3D curve

p = f (s) ∈ R3 , s∈R

dp
t =
ds
1 d2 p
n = 2 2
d p ds

ds2
b = t×n

Paolo Di Lillo Robotic systems - Trajectory planning 19 / 26

Operational space trajectories III
3D segment
s
p(s) = pi + (p − pi )
kpf − pi k f
where p(0) = pi e p(kpf − pi k) = pf
since
dp 1
= (p − pi )
ds kpf − pi k f
d2 p
= 0
ds2
the frame is not unequivocally defined

Paolo Di Lillo Robotic systems - Trajectory planning 20 / 26

Operational space trajectories IV
3D circumference
center in the origin and radius ρ:
 
ρ cos(s/ρ)
p′ (s) =  ρ sin(s/ρ) 
0

rototranslation and partial derivatives:

p(s) = c +Rp′ (s) 

− sin(s/ρ)
dp
= R  cos(s/ρ) 
ds
0
 
2 − cos(s/ρ)/ρ
d p
= R  − sin(s/ρ)/ρ 
ds2
0

Paolo Di Lillo Robotic systems - Trajectory planning 21 / 26

Operational space trajectories V

Position
p = f (s)
s(t) interpolating polynomial

dp
ṗ = ṡ = ṡt
ds

segment
s
p(s) = pi + (p − pi )
kpf − pi k f
ṡ
ṗ = (p − pi ) = ṡt
kpf − pi k f
s̈
p̈ = (p − pi ) = s̈t
kpf − pi k f

Paolo Di Lillo Robotic systems - Trajectory planning 22 / 26

Operational space trajectories VI
circumference
 
ρ cos(s/ρ)
p(s) = c + R  ρ sin(s/ρ) 
0
 
−ṡ sin(s/ρ)
ṗ = R  ṡ cos(s/ρ) 
0
 2 
−ṡ cos(s/ρ)/ρ − s̈ sin(s/ρ)
p̈ = R −ṡ2 sin(s/ρ)/ρ + s̈ cos(s/ρ)
0

Paolo Di Lillo Robotic systems - Trajectory planning 23 / 26

Operational space trajectories VII
Orientation
Not easy to interpolate the vectors n(t), s(t), a(t) due to the necessity
to impose the constraints
Possible interpolation of φ(t) (ex. Euler angles)
s
φ(s) = φi + (φ − φi )
kφf − φi k f
ṡ
φ̇ = (φ − φi )
kφf − φi k f
s̈
φ̈ = (φ − φi )
kφf − φi k f

Paolo Di Lillo Robotic systems - Trajectory planning 24 / 26

Operational space trajectories VIII

If a clear geometric interpretation is needed it is possible to use the

equivalent axis of rotation
Given Ri and Rf , knowing Rf = Ri Rif it holds:
 
r11 r12 r13
Rif = RT
i Rf = r21 r22 r23
 
r31 r32 r33

from which the axis and angle needed to implement the desired
rotation:
 
−1 r11 + r22 + r33 − 1
ϑf = cos
 2 
r32 − r23
1
r = r13 − r31 
2 sin ϑf
r21 − r12

Paolo Di Lillo Robotic systems - Trajectory planning 25 / 26

Operational space trajectories IX

The matrix Ri (t) is built: Ri (0) = I e Ri (tf ) = Rif

Notice: Ri (t) is a rotation around a constant axis r
The problem is now reduced to the assignment of a time law to ϑ(t)
such that ϑ(0) = 0 and ϑ(tf ) = ϑf
The instantaneous rotation is

R(t) = Ri Rir (ϑ(t))

and the angular velocity

wi = ϑ̇r

Paolo Di Lillo Robotic systems - Trajectory planning 26 / 26