Trajectory Planning For Robot Manipulators

Trajectory Planning for Robot Manipulators
Claudio Melchiorri
Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)

Universita di Bologna
email: claudio.melchiorri@unibo.it
C. Melchiorri (DEIS) Trajectory Planning 1 / 61

Trajectory Planning Introduction
Trajectory planning
Kinematics: geometrical relationships in terms of position/velocity between the

joint- and work-space.
Dynamics: relationships between the torques applied to the joints and the
consequent movements of the links.
Control: computation of the control actions (joint torques) necessary to execute

a desired motion.
Trajectory planning: planning of the desired movements of the manipulator.
Usually, the user is requested to define some points and general features of the
trajectory (e.g. initial/final points, duration, maximum velocity, etc.), and the real
computation of the trajectory is demanded to the control system.

Trajectory planning
Trajectory planning: IMPORTANT aspect in robotics, VERY IMPORTANT for
the dimensioning, control, and use of electric motors in automatic machines (e.g.
packaging).
Original problem (in mechanics): substitution of mechanical cams with electric

cams.
Some suggested references:

C. Melchiorri, Traiettorie per azionamenti elettrici, Progetto Leonardo,
Esculapio Ed., Bologna, Feb. 2000;
G. Canini, C. Fantuzzi, Controllo del moto per macchine automatiche,
Pitagora Ed., Bo, 2003;
G. Legnani, M. Tiboni, R. Adamini, Meccanica degli azionamenti: Vol. 1 -
Azionamenti elettrici, Progetto Leonardo, Esculapio Ed., Bologna, Feb. 2002.
L. Biagiotti, C. Melchiorri, Trajectory Planning for Automatic Machines and
Robots, Springer, 2008.

Trajectory planning
Springer, 2008 Esculapio, 2000

Trajectory planning
The planning modalities for trajectories may be quite different:
point-to-point
with pre-defined path
Or:
in the joint space;
in the work space, either defining some points of interest (initial and final
points, via points) or the whole geometric path x = x(t).
For planning a desired trajectory, it is necessary to specify two aspects:

geometric path
motion law
with constraints on the continuity (smoothness) of the trajectory and on its
time-derivatives up to a given degree.

Geometric path and motion law

The geometric path can be defined in the work-space or in the joint-space.
Usually, it is expressed in a parametric form as
p = p(s) work-space
q = q() joint-space
The parameter s () is defined as a function of time, and in this manner the
motion law s = s(t) ( = (t)) is obtained.
time
t
0 T b (s = smax )
length
s
a (s = 0)
0 smax
px = px (s)
py = py (s) path
pz = pz (s)

Geometric path and motion law

Examples of geometric paths: (in the work space) linear, circular or parabolic
segments or, more in general, tracts of analytical functions.
In the joint space, geometric paths are obtained by assigning initial/final (and, in
case, also intermediate) values for the joint variables, along with the desired
motion law.
Concerning the motion law, it is necessary to specify continuos functions up to a

given order of derivations (often at least first and second order, i.e. velocity and
acceleration).
Usually, polynomial functions a of proper degree n are employed:
s(t) = a0 + a1 t + a2 t 2 + . . . + an t n
In this manner, a smooth interpolation of the points defining the geometric

path is achieved.

Trajectory planning
Input data to an algorithm for trajectory planning are:
data defining on the path (points),
geometrical constraints on the path (e.g. obstacles),
constraints on the mechanical dynamics
constraints due to the actuation system
Output data is:
the trajectory in the joint- or work-space, given as a sequence (in time) of the
acceleration, velocity and position values:
a(kT ), v (kT ), p(kT ) k = 0, . . . , N
being T a proper time interval defining the instants in which the trajectory is
computed (and converted in the joint space) and sent to each actuator.

Trajectory planning
Usually, the user has to specify only a minimum amount of information about the
trajectory, such as initial and final points, duration of the motion, maximum
velocity, and so on.
Work-space trajectories allow to consider directly possible constraints on the

path (obstacles, path geometry, . . . ) that are more difficult to take into
consideration in the joint space (because of the non linear kineamtics)
Joint space trajectories are computationally simpler and allow to consider
problems due to singular configurations, actuation redundancy,
velocity/acceleration constraints.

Trajectory Planning Joint-space trajectories
Joint-space trajectories
Trajectories are specified by defining some characteristic points:
directly assigned by some specifications
assigned by defining desired configurations x in the work-space, which are
then converted in the joint space using the inverse kinematic model.
The algorithm that computes a function q(t) interpolating the given points is
characterized by the following features:
trajectories must be computationally efficient
the position and velocity profiles (at least) must be continuos functions of
time
undesired effects (such as non regular curvatures) must be minimized or
completely avoided.
In the following discussion, a single joint is considered.

If more joints are present, a coordinated motion must be planned, e.g. considering
for each of them the same initial and final time instant, or evaluating the most
stressed joint (with the largest displacement) and then scaling suitably the motion
of the remaining ones.
Polynomial trajectories
In the most simple cases, (a segment of) a trajectory is specified by assigning
initial and final conditions on: time (duration), position, velocity, acceleration,
. . . . Then, the problem is to determine a function
q = q(t) or q = q(), = (t)
so that those conditions are satisfied.
This is a boundary condition problem, that can be easily solved by considering

polynomial functions such as:
q(t) = a0 + a1 t + a2 t 2 + . . . + an t n
The degree n (3, 5, ...) of the polynomial depends on the number of boundary
conditions that must be verified and on the desired smoothness of the trajectory.

Polynomial trajectories
In general, besides the initial and final values, other constraints could be specified
on the values of some time-derivatives (velocity, acceleration, jerk, . . . ) in generic
instants tj . In other terms, one could be interested in defining a polynomial
function q(t) whose k-th derivative has a specified value q k (tj ) at a given istant tj .
Mathematically, these conditions may be expressed as:

n!
k!ak + (k + 1)!ak+1 tj + . . . + an t nk = q k (tj )
(n k)! j
or, in matrix form:
Ma=b
where:
- M is a known (n + 1) (n + 1) matrix,
- b is the vector with the n + 1 constraints on the trajectory (known data),
- a = [a0 , a1 , . . . , an ]T contains the unknown parameters to be computed
a = M1 b
Third-order polynomial trajectories

Given an initial and a final instant ti , tf , a (segment of a) trajectory may be
specified by assigning initial and final conditions:
initial position and velocity qi , qi ;
final position and velocity qf , qf
There are four boundary conditions, and therefore a polynomial of degree 3 (at
least) must be considered
q(t) = a0 + a1 t + a2 t 2 + a3 t 3 (1)
where the four parameters a0 , a1 , a2 , a3 must be defined so that the boundary

conditions are satisfied.From the boundary conditions, it follows that
q(ti ) = a0 + a1 ti + a2 ti2 + a3 ti3 = qi

q(ti ) = a1 + 2a2 ti + 3a3 ti2 = qi
(2)
q(tf ) = a0 + a1 tf + a2 tf2 + a3 tf3 = qf
q(tf ) = a1 + 2a2 tf + 3a3 tf2 = qf


In order to solve these equations, let us assume for the moment that ti = 0.
Therefore:
a0 = qi (3)
a1 = qi (4)
3(qi qf ) (2qi + qf )tf
a2 = (5)
tf2
2(qi qf ) + (qi + qf )tf

a3 = (6)
tf3


Position, velocity and acceleration profiles obtained with a cubic polynomial and
boundary conditions: qi = 10o , qf = 30o , qi = qf = 0 o /s, ti = 0, tf = 1s:
Posizione (gradi) Velocita (gradi/s)
40 40
35
35
30
30
25
25
20
20 15
10
15
5
10
0
5
-5
0 -10
-0.2 0 0.2 0.4 0.6 0.8 1 1.2 -0.2 0 0.2 0.4 0.6 0.8 1 1.2
Accelerazione (gradi/s^2)
150
100
Obviously:
50
0
position cubic function
-50
velocity parabolic function
acceleration linear function
-100
-150
-0.2 0 0.2 0.4 0.6 0.8 1 1.2

Non null initial velocity: qi = 10o , qf = 30o , qi = 20 o /s, qf = 50 o /s, ti = 0, tf = 1s.
40 60
35
40
30
20
25
20 0
15
-20
10
-40
5
0 -60
-0.2 0 0.2 0.4 0.6 0.8 1 1.2 -0.2 0 0.2 0.4 0.6 0.8 1 1.2
400
300
200
100
-100
-200
-300
-400
-0.2 0 0.2 0.4 0.6 0.8 1 1.2


The results obtained with the polynomial (1) and the coefficients (3)-(6) can be
generalized to the case in which ti 6= 0. One obtains:
q(t) = a0 + a1 (t ti ) + a2 (t ti )2 + a3 (t ti )3 ti t tf
with coefficients
a0 = qi
a1 = qi
3(qi qf ) (2qi + qf )(tf ti )
a2 =
(tf ti )2
2(qi qf ) + (qi + qf )(tf ti )
a3 =
(tf ti )3
In this manner, it is very simple to plan a trajectory passing through a sequence of

intermediate points.


The trajectory is divided in n segments, each of them defined by:
initial and final point qk e qk+1
initial and final instant tk , tk+1 k = 0, . . . , n 1.
initial and final velocity qk , qk+1
The above relationships are then adopted for each of these segments.
9
0
0 1 2 3 4 5 6 7 8 9 10


Position, velocity and acceleration profiles with:
t0 = 0 t1 = 2 t2 = 4 t3 = 8 t4 = 10
q0 = 10o q1 = 20o q2 = 0o q3 = 30o q4 = 40o
q0 = 0o /s q1 = 10o /s q2 = 20o /s q3 = 3o /s q4 = 0o /s
50 20
15
40
10
30
5
20 0
5
10
10
0
15
10 20
2 0 2 4 6 8 10 12 2 0 2 4 6 8 10 12
40
30
20
10
10
20
30
40
2 0 2 4 6 8 10 12


Often, a trajectory is assigned by specifying a sequence of desired points
(via-points) without indication on the velocity in these points.
In these cases, the most suitable values for the velocities must be automatically
computed.
This assignment is quite simple with heuristic rules such as:
q1 = 0;

0 sign(vk ) 6= sign(vk+1 )
qk =
1
2 (vk + vk+1 ) sign(vk ) = sign(vk+1 )
qn = 0
being
qk qk1
vk =
tk tk1
the slope of the tract [tk1 tk ].


Automatic computation of the intermediate velocities (data as in the previous example)
t0 = 0 t1 = 2 t2 = 4 t3 = 8 t4 = 10
q0 = 10o q1 = 20o q2 = 0o q3 = 30o q4 = 40o
50 20
15
40
10
30
5
20 0
-5
10
-10
0
-15
-10 -20
-2 0 2 4 6 8 10 12 -2 0 2 4 6 8 10 12
30
20
10
-10
-20
-30
-2 0 2 4 6 8 10 12

Fifth-order polynomial trajectories

From the above examples, it may be noticed that both the position and velocity
profiles are continuous functions of time.
This is not true for the acceleration, that presents therefore discontinuities among
different segments. Moreover, it is not possible to specify for this signal suitable
initial/final values in each segment.
In many applications, these aspects do not constitute a problem, being the
trajectories smooth enough.
On the other hand, if it is requested to specify initial and final values for the
acceleration (e.g. for obtaining acceleration profiles), then (at least) fifth-order
polynomial functions should be considered
q(t) = a0 + a1 t + a2 t 2 + a3 t 3 + a4 t 4 + a5 t 5
with the six boundary conditions:
q(ti ) = qi q(tf ) = qf

In this case, (if T = tf ti ) the coefficients of the polynomial are
a0 = qi
a1 = qi
1
a2 = qi
2
1
a3 = [20(qf qi ) (8qf + 12qi )T (3qf qi )T 2 ]
2T 3
1
a4 = [30(qi qf ) + (14qf + 16qi )T + (3qf 2qi )T 2 ]
2T 4
1
a5 = [12(qf qi ) 6(qf + qi )T (qf qi )T 2 ]
2T 5
If a sequence of points is given, the same considerations made for third-order

polynomials trajectories can be made in computing the intermediate velocity
values.


Fifth-order trajectory with the boundary conditions:
qi = 10o , qf = 30o , qi = qf = 0 o /s, qi = qf = 0 o /s 2 , ti = 0s, tf = 1s.
Posizione (deg) Velocita (deg/s)
40 40
35
35
30
30
25
25
20
20 15
10
15
5
10
0
5
-5
0 -10
-0.2 0 0.2 0.4 0.6 0.8 1 1.2 -0.2 0 0.2 0.4 0.6 0.8 1 1.2
Accelerazione (deg/s/s
150
100
Obviously:
50
0
position 5-th order function
-50 velocity 4-th order function
acceleration 3-rd order function
-100
-150
-0.2 0 0.2 0.4 0.6 0.8 1 1.2


Comparison of fifth- and third-order trajectories with the boundary conditions:
qi = 10o , qf = 30o , qi = qf = 0 o /s, qi = qf = 0 o /s 2 , ti = 0s, tf = 1s.
40 40
35
35
30
30
25
25
20
20 15
10
15
5
10
0
5
5
0 10
0.2 0 0.2 0.4 0.6 0.8 1 1.2 0.2 0 0.2 0.4 0.6 0.8 1 1.2
2
Accelerazione (gradi/s )
150
100
50
blue fifth-order
0
green third-order
50
100
150
0.2 0 0.2 0.4 0.6 0.8 1 1.2


Position, velocity, acceleration profiles with automatic assignment of the intermediate
velocities and null accelerations. Posizione Velocita
50 15
10
40
5
30
20
10
10
0
15
10 20
2 0 2 4 6 8 10 12 2 0 2 4 6 8 10 12
Accelerazione
30
20
10
10
20
30
2 0 2 4 6 8 10 12
Note that the resulting motion has smoother profiles.

Trapezoidal trajectories
A different approach for planning a trajectory is to compute linear segments joined
with parabolic blends.
In the linear tract, the velocity is constant while, in the parabolic blends, it is a
linear function of time: trapezoidal profiles, typical of this type of trajectory, are
then obtained.
In trapezoidal trajectories, the duration is divided into three parts:

1 in the first part, a constant acceleration is applied, then the velocity is linear
and the position a parabolic function of time
2 in the second, the acceleration is null, the velocity is constant and the
position is linear in time
3 in the last part a (negative) acceleration is applied, then the velocity is a
negative ramp and the position a parabolic function.

Usually, the acceleration and the deceleration phases have the same duration
(ta = td ). Therefore, symmetric profiles, with respect to a central instant
(tf ti )/2, are obtained.
50
45
40
35
30
25
20
15
10
0 0.5 1 1.5 2 2.5 3

The trajectory is computed according to the following equations.
1) Acceleration phase, t [0 ta ].
The position, velocity and acceleration are described by
q(t) = a0 + a1 t + a2 t 2
q(t) = a1 + 2a2 t
q(t) = 2a2
The parameters are defined by constraints on the initial position qi and the
velocity qi , and on the desired constant velocity qv that must be obtained at
the end of the acceleration period. Assuming a null initial velocity and
considering ti = 0 one obtains
a0 = qi
a1 = 0
qv
a2 =
2ta
In this phase, the acceleration is constant and equal to qv /ta .
2) Constant velocity phase, t [ta tf ta ].
Position, velocity and acceleration are now defined as
q(t) = b0 + b1 t
q(t) = b1
q(t) = 0
where, because of continuity,

b1 = qv
Moreover, the following equation must hold
ta
q(ta ) = qi + qv = b0 + qv ta
2
and then
ta
b0 = qi qv
2

3) Deceleration phase, t [tf ta tf ].
The position, velocity and acceleration are given by
q(t) = c0 + c1 t + c2 t 2
q(t) = c1 + 2c2 t
q(t) = 2c2
The parameters are now defined with constrains on the final position qf and
velocity qf , and on the velocity qv at the beginning of the deceleration period.
If the final velocity is null, then:
qv tf2
c0 = qf
2 ta
tf
c1 = qv
ta
qv
c2 =
2ta

Summarizing, the trajectory is computed as
qv t 2

qi + 2t 0 t < ta

a
ta

q(t) = q i + q v (t 2 ) ta t < tf ta

2
q qv (tf t)

tf ta t tf

f t 2
a

Typical position, velocity and acceleration profiles of a trapezoidal trajectory.
Posizione (deg) Velocita (deg/s)
40 15
35
30 10
25
20 5
15
10 0
0 -5
-1 0 1 2 3 4 -1 0 1 2 3 4
Accelerazione (deg/s/s)
15
10
-5
-10
-15
-1 0 1 2 3 4

Some additional constraints must be specified in order to solve the previous
equations.
A typical constraint concerns the duration of the acceleration/deceleration periods
ta which, for symmetry, must satisfy the condition
ta tf /2
Moreover, the following condition must be verified (for the sake of simplicity,
consider ti = 0):
qm qa qa = q(ta )
qta = qm = (qi + qf )/2
tm ta
tm = tf /2

1 2
qa = qi + qta
2
from which
qta2 qtf ta + (qf qi ) = 0 (7)
Finally:
qf qi
qv =
tf ta
Any pair of values (q, ta ) verifying (7) can be considered.
Given the acceleration q (for example qmax ), then
q
tf q 2 tf2 4q(qf qi )
ta =
2 2q
from which we have also that the minimum value for the acceleration is
4|qf qi |
|q|
tf2
if the value |q| = 4|qftq

2
i|
is assigned, then ta = tf /2 and the constant velocity
f
tract does not exist.
If the value ta = tf /3 is specified, the following velocity and acceleration values
are obtained
3(qf qi ) 9(qf qi )
qv = q =
2tf 2tf2
Another way to compute this type of trajectory is to define a maximum value qa
for the desired acceleration and then compute the relative duration ta of the
acceleration and deceleration periods.
If the maximum values (qmax and qmax , known) for the acceleration and velocity
must be reached, it is possible to assign
q

ta = qmax acceleration time

max
qmax (T ta ) = qf qi = L displacement
2
Lqmax + qmax
T = time duration

qmax qmax
and then (tf = ti + T )

q + 1 q (t ti )2

ti t ti + ta
i 2 max

q(t) = qi + qmax ta (t ti t2a ) ti + ta < t tf ta (8)

qf 12 qmax (tf t ti )2

tf ta < t tf


In this case, the linear tract exists if and only if
2
qmax
L
qmax
Otherwise
r

ta = L acceleration time
qmax
T = 2ta total time duration

and (still tf = ti + T )
qi + 12 qmax (t ti )2

ti t ti + ta
q(t) = (9)
qf 21 qmax (tf t)2 tf ta < t tf


With this modality for computing the trajectory, the time duration of the motion
from qi to qf is not specified. In fact, the period T is computed on the basis of
the maximum acceleration and velocity values.
If more joints have to be co-ordinated with the same constraints on the maximum
acceleration and velocity, the joint with the largest displacement must be
individuated. For this joint, the maximum value qmax for the acceleration is
assigned and then the corresponding values ta and T are computed.
For the remaining joints, the acceleration and velocity values must be computed
on the basis of these values of ta and T , and on the basis of the given
displacement Li :
Li Li
qi = , qi =
ta (T ta ) T ta

Traiettorie di posizione giunto Traiettorie di velocita giunto
150 150
100
100
50
50
-50
-50
-100
-100
-150
-200 -150
0 0.5 1 0 0.5 1
Traiettorie di accelerazione giunto

1500
1000
500
-500
-1000
-1500
0 0.5 1

The trajectories in the workspace are:
Traiettoria cartesiana Traiettoria cartesiana x(t), y(t)
2 1.8
1.6
1.5
1.4
1.2
1
1
0.5 0.8
0.6
0
0.4
0.2
-0.5
0
-1 -0.2
-1 -0.5 0 0.5 1 1.5 2 0 0.5 1
Velocita cartesiane vx(t), vy(t)

0.02
0.015
0.01
0.005
-0.005
-0.01
-0.015
0 0.5 1
If a trajectory interpolating more consecutive points is computed with the above
technique, a motion with null velocities in the via-points is obtained.
Since this behavior may be undesirable, it is possible to anticipate the actuation of a
tract of the trajectory between points qk and qk+q before the motion from qk1 to qk is
terminated. This is possible by adding (starting at an instant tk ta ) the velocity and
acceleration contributions of the two segments [qk1 qk ] and [qk qk+1 ].
Obviously, another possibility is to compute the parameters of the functions defining the
trapezoidal trajectory in order to have desired boundary conditions (i.e. velocities) for
each segment.
Posizione (gradi) Velocita (gradi/s) Velocita (gradi/s)
70 30 30
20 20
60 10 10
0 0
50 10 10
20 20
40 30 30
0 5 10 0 5 10
Accelerazione (gradi/s^2) Accelerazione (gradi/s^2)

30
10 10
20 5 5
0 0
10 5 5
10 10
0 15 15
0 2 4 6 8 10 12 0 5 10 0 5 10
Spline
In general, the problem of defining a function interpolating a set of n points can
be solved with a polynomial function of degree n 1.
In planning a trajectory, this approach does not give good results since the
resulting motions in general present large oscillations.
Traiettoria polinomiale n1 (dash) e spline cubica
30
20 In general, given:
10
0
2 points = unique line
10
3 points = unique quadric
20 ...
30 n points = unique polynomial
with degree n 1
40
50
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
time [s]
Spline
The (unique) polynomial p(x) with degree n 1 interpolating n points (xi , yi ) can
be computed by the Lagrange expression:
(x x2 )(x x3 ) (x xn ) (x x1 )(x x3 ) (x xn )
p(x) = y1 + y2 + +
(x1 x2 )(x1 x3 ) (x1 xn ) (x2 x1 )(x2 x3 ) (x2 xn )
(x x1 )(x x2 ) (x xn1 )
+ + yn
(xn x1 )(xn x2 ) (xn xn1 )
Other (recursive) expressions have been defined, more efficient from a

computational point of view (Neville formulation).

Spline
Another (less efficient) approach for the computation of the coefficients of the
polynomial p(x) is based on the following procedure:
yi = p(xi ) = an1 xin1 + + a1 xi + a0 i = 1, . . . , n
x1n1 x1n2

y1 x1 1 an1
y2 x n1 x2n2 x2 1 an2
2
.. .. ..

y= = = Xa

. . .

yn1 x n1 n2
xn1 xn1 1 a1
n1
yn xnn1 xnn2 xn 1 a0
and then, by inverting matrix X, the parameters are obtained
a = X1 y
= Numerical problems in computing X1 for high values of n!!!

Spline
Given n points, in order to avoid the problem of high oscillations (and also of the
numerical precision):
NO: one polynomial of degree n 1

= YES: n 1 polynomials with lower degree p (p < n 1):
each polynomial interpolates a segment of the trajectory.
Usually, the degree p of the n 1 polynomials is chosen so that continuity of the

velocity and acceleration profile is achieved. In this case, the choice p = 3 is made
(cubic polynomials):
q(t) = a0 + a1 t + a2 t 2 + a3 t 3
There are 4 coefficients for each polynomial, and therefore it is necessary to

compute 4(n 1) coefficients.
Obviously, it is possible to choose higher values for p (e.g. p = 5, 7, . . .).

Spline
4(n 1) coefficients
On the other hand, there are:
- 2(n 1) conditions on the position (each cubic function interpolates the
initial/final points);
- n 2 conditions on the continuity of velocity in the intermediate points;
- n 2 conditions on the continuity of acceleration in the intermediate points.
Therefore, there are
4(n 1) 2(n 1) 2(n 2) = 2
degrees of freedom left, that can be used for example for imposing proper
conditions on the initial and final velocity.

Spline
The function obtained in this manner is a spline.
Among all the interpolating functions of n points with the same degree of
continuity of derivation, the spline has the smallest curvature.
q(t)
qn1
q2 qn2
q3
v1 qk+1
qk qn
... ...
vn
q1
t1 t2 t3 tk Tk tk+1 tn2 tn1 tn t

Spline
Mathematically, it is necessary to compute a function
q(t) = {qk (t), t [tk , tk+1 ], k = 1, . . . , n 1}
qk ( ) = ak0 + ak1 + ak2 2 + ak3 3 , [0, Tk ],

( = t tk , Tk = tk+1 tk )
with the conditions
qk (0) = qk , qk (Tk ) = qk+1 k = 1, . . . , n 1
qk (Tk ) = qk+1 (0) = vk k = 1, . . . , n 2
qk (Tk ) = qk+1 (0) k = 1, . . . , n 2

Spline - Computation
The parameters aki are computed according to the following algorithm.
Let assume that the velocities vk , k = 2, . . . , n 1 in the intermediate points are known.
In this case, we can impose for each cubic polynomial the four boundary conditions on
position and velocity:
qk (0) = ak0 = qk

qk (0) = ak1 = vk
q k (Tk ) = ak0 + ak1 + ak2 2 + ak3 3 = qk+1
ak1 + 2ak2 + 3ak3 2

qk (Tk ) = = vk+1
and then
ak0 = qk

=

ak1 vk
1 3(qk+1 qk ) 2v v

ak2 = k k+1
Tk Tk
1 2(qk qk+1 ) + v + v

ak3 =

k k+1
Tk2 Tk
... but the velocities vk are not known...

By using the conditions on continuity of the accelerations in the intermediate points, one
obtains
qk (Tk ) = 2ak2 + 6ak3 Tk = 2ak+1,2 = qk+1 (0) k = 1, . . . , n 2
form which, by substituting the expressions of ak2 , ak3 , ak+1,2 and multiplying by
(Tk Tk+1 )/2, one obtains
3 h i
Tk+1 vk + 2(Tk+1 + Tk )vk+1 + Tk vk+2 = Tk2 (qk+2 qk+1 ) + Tk+1
2
(qk+1 qk )
Tk Tk+1
These equations may be written in matrix form as

T2 2(T1 + T2 ) T1 v1 c1

0 T3 2(T2 + T3 ) T2 v c
2 2
. . .

. . = .

. . .

Tn2 2(Tn3 + Tn2 ) Tn3 0 vn1 cn1
Tn1 2(Tn2 + Tn1 ) Tn2 vn cn
where the ck are (known) constant terms depending on the intermediate positions and
the duration of each segments.

Since the velocities v1 and vn are known, the corresponding columns can be eliminated
form the left-hand side matrix, and then
2(T1 + T2 ) T1

T3 2(T2 + T3 ) T2 v2

.
.

. . =

.
.
Tn2 2(Tn3 + Tn2 ) Tn3 vn1
Tn1 2(Tn2 + Tn1 )
T12 (q3 q2 ) + T22 (q2 q1 ) T2 v1

h i
3
T1 T2

T22 (q4 q3 ) + T32 (q3 q2 )
h i
3

T2 T3

.
.
.

2 2
h i
3

Tn3 (qn1 qn2 ) + Tn2 (qn2 qn3 )

Tn3 Tn2

2 2
h i
3 Tn2 (qn qn1 ) + Tn1 (qn qn2 ) Tn2 vn
Tn2 Tn1
that is A(T) v = c(T, q, v1 , vn )
or Av = c

The matrix
P A is tridiagonal, and is always invertible if Tk > 0
(|akk | > j6=k |akj |).
Being A tridiagonal, its inverse is computed by efficient numerical algorithms

(based on the Gauss-Jordan method).
Once A1 is known, the velocities v2 , . . . , vn1 are computed as
v = A1 c
and the problem is solved.

Spline
The total duration of a spline is
n1
X
T = T k = tn t1
k=1
It is possible to define an optimality problem aiming at minimizing T . The values

of Tk must be computed so that T is minimized and the constraints on the
velocity and acceleration are satisfied.
Formally the problem is formulated as
Pn1
minTk T = k=1 Tk

|q(, Tk )| < vmax [0T ]
tale che

|q(, Tk )| < amax [0T ]
Non linear optimization problem with linear objective function, solvable with
classical techniques from the operational research field.
Spline - Example
A spline trough the points q1 = 0, q2 = 2, q3 = 12, q4 = 5 must be defined,
minimizing the total duration T and with the constraints: vmax = 3, amax = 2.
The non linear optimization problem
min T = T1 + T2 + T3
is defined, with the constraints reported in the following slide.
By solving this problem (e.g. with the Matlab Optimization Toolbox) the
following values are obtained:
T1 = 1.5549, T2 = 4.4451, T3 = 4.5826, T = 10.5826 sec

Spline - Esempio
Constraints on the optimization problem:

a11 vmax (velocita iniziale del I tratto vmax )

a21 vmax (velocita iniziale del II tratto vmax )
a31 vmax (velocita iniziale del III tratto vmax )

3a13 T12

a11 + 2a12 T1 + vmax (velocita finale del I tratto vmax )
3a23 T22

a21 + 2a22 T2 + vmax (velocita finale del II tratto vmax )

3a33 T32

a31 + 2a32 T3 + vmax (velocita finale del III tratto vmax )

2
a a
12 12

a11 + 2a12 + 3a13 vmax (velocita allinterno del I tratto vmax )

3a13 3a13
2
a a
a21 + 2a22 22 + 3a23 22 vmax (velocita allinterno del II tratto vmax )

3a23 3a23
2
a a

2a32 32 3a33 32

a31 + + vmax (velocita allinterno del III tratto vmax )
3a33 3a33

2a12 amax (accelerazione iniziale del I tratto amax )

2a22 amax (accelerazione iniziale del II tratto amax )
2a32 amax (accelerazione iniziale del III tratto amax )

2a12 + 6a13 T1 amax (accelerazione finale del I tratto amax )

2a22 + 6a23 T2 amax (accelerazione finale del II tratto amax )
2a32 + 6a33 T3 amax (accelerazione finale del III tratto amax )

Spline - Esempio
Position, velocity and acceleration profiles of the optimal trajectory.
POSIZIONE VELOCITA"
14 3
12
2
10
1
1
4
2
2
0 3
0 2 4 6 8 10 12 0 2 4 6 8 10 12
ACCELERAZIONE
2
1.5
0.5
0.5
1.5
2
0 2 4 6 8 10 12

Spline
The above procedure for computing the spline is adopted also for more motion
axes (joints). Notice that the matrix Aj (T) = A(T) is the same for all the joints
(depends only on the parameters Tk ), while the vector c(T, qj , vi 1 , vin ) depends
on the specific i-th joint.
10
2
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
10
2
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5

Spline
From the expressions of matrix A and the vector c (Av = c)
2(T1 + T2 ) T1

T3 2(T2 + T3 ) T2

A= ..
.

Tn2 2(Tn3 + Tn2 ) Tn3
Tn1 2(Tn2 + Tn1 )
3 T 2 (q q ) + T 2 (q q ) T v

T1 T2 1 3 2 2 2 1 2 1

3
T 2 (q q3 ) + T32 (q3 q2 )

T2 T3 2 4

c= ..

.
3
2
2

T
Tn3 Tn2 n3 n1(q q n2 ) + Tn2 (qn2 qn3 )

3
2
2

T (q qn1 ) + Tn1 (qn qn2 ) Tn2 vn
Tn2 Tn1 n2 n

Spline
If
the duration Tk of each interval is multiplied by a constant (linear scaling)
the initial and final velocities are null
one obtains that the new durationT , the velocities and accelerations of the new
trajectory are:
T = = T
1
vk = vk

1
ak = ak
2

Spline - Example
Comparison of a n 1 polynomial, a spline, and a composition of cubic polynomials.
11 points, vin = vfin = 0 m/s
Traiettoria polinomiale n1 (dash) e spline cubica Traiettoria cubica (dash) e spline cubica
30 20
20 18
10
16
0
14
10
12
20
10
30
8
40
50 6
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
time [s] time [s]

Spline - Example
Velocita per spline Velocita per cubica
100 100
50 50
0 0
50 50
100 100
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
Accelerazione per spline Accelerazione per cubica

1000 2000
500 1000
0 0
500 1000
1000 2000
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
time [s] time [s]

Trajectory Planning For Robot Manipulators

Uploaded by

Copyright:

Available Formats

Trajectory Planning For Robot Manipulators

Uploaded by

Document Information

Original Description:

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Trajectory Planning For Robot Manipulators

Uploaded by

Copyright:

Available Formats

Trajectory Planning for Robot Manipulators

Dipartimento di Elettronica, Informatica e Sistemistica (DEIS)

C. Melchiorri (DEIS) Trajectory Planning 1 / 61

Kinematics: geometrical relationships in terms of position/velocity between the

Control: computation of the control actions (joint torques) necessary to execute

Trajectory planning: planning of the desired movements of the manipulator.

C. Melchiorri (DEIS) Trajectory Planning 2 / 61

Original problem (in mechanics): substitution of mechanical cams with electric

Some suggested references:

C. Melchiorri (DEIS) Trajectory Planning 3 / 61

Springer, 2008 Esculapio, 2000

For planning a desired trajectory, it is necessary to specify two aspects:

C. Melchiorri (DEIS) Trajectory Planning 5 / 61

Geometric path and motion law

C. Melchiorri (DEIS) Trajectory Planning 6 / 61

Geometric path and motion law

Concerning the motion law, it is necessary to specify continuos functions up to a

Usually, polynomial functions a of proper degree n are employed:

In this manner, a smooth interpolation of the points defining the geometric

C. Melchiorri (DEIS) Trajectory Planning 7 / 61

a(kT ), v (kT ), p(kT ) k = 0, . . . , N

C. Melchiorri (DEIS) Trajectory Planning 8 / 61

Work-space trajectories allow to consider directly possible constraints on the

C. Melchiorri (DEIS) Trajectory Planning 9 / 61

In the following discussion, a single joint is considered.

q = q(t) or q = q(), = (t)

so that those conditions are satisfied.

This is a boundary condition problem, that can be easily solved by considering

C. Melchiorri (DEIS) Trajectory Planning 11 / 61

Mathematically, these conditions may be expressed as:

Third-order polynomial trajectories

where the four parameters a0 , a1 , a2 , a3 must be defined so that the boundary

q(ti ) = a0 + a1 ti + a2 ti2 + a3 ti3 = qi

C. Melchiorri (DEIS) Trajectory Planning 13 / 61

Third-order polynomial trajectories

2(qi qf ) + (qi + qf )tf

C. Melchiorri (DEIS) Trajectory Planning 14 / 61

Third-order polynomial trajectories

Third-order polynomial trajectories

C. Melchiorri (DEIS) Trajectory Planning 16 / 61

Third-order polynomial trajectories

In this manner, it is very simple to plan a trajectory passing through a sequence of

C. Melchiorri (DEIS) Trajectory Planning 17 / 61

Third-order polynomial trajectories

C. Melchiorri (DEIS) Trajectory Planning 18 / 61

Third-order polynomial trajectories

C. Melchiorri (DEIS) Trajectory Planning 19 / 61

Third-order polynomial trajectories

the slope of the tract [tk1 tk ].

C. Melchiorri (DEIS) Trajectory Planning 20 / 61

Third-order polynomial trajectories

C. Melchiorri (DEIS) Trajectory Planning 21 / 61

Fifth-order polynomial trajectories

Fifth-order polynomial trajectories

If a sequence of points is given, the same considerations made for third-order

C. Melchiorri (DEIS) Trajectory Planning 23 / 61

Fifth-order polynomial trajectories

C. Melchiorri (DEIS) Trajectory Planning 24 / 61