Robotics

Islam S. M. Khalil

German University in Cairo

Islam S. M. Khalil Trajectory Planning

Trajectory for Point to point Motion

Figure: Difference between joint and task spaces.

Islam S. M. Khalil Trajectory Planning

Trajectory for Point to point Motion

Figure: Difference between joint and task spaces.

Islam S. M. Khalil Trajectory Planning

Trajectory for Point to point Motion
The problem is to find a trajectory that connects the initial and
final configurations while satisfying other specified constraints at
the end points such as velocity and/or acceleration constraints.
Suppose that at time t0 the joint variables satisfies

q(t0 ) = q0 , q̇(t0 ) = v0

and we wish to attain the values

at tf

q(tf ) = qf , q̇(tf ) = vf

We may also wish to specify the

constrains on the initial and final
accelerations
Figure: A typical joint space
q̈(tf ) = α0 , q̈(tf ) = αf trajectory.

Islam S. M. Khalil Trajectory Planning

Cubic Polynomial Trajectories
Consider firs the case where we wish to generate a polynomial joint
trajectory between two configurations, and that we wish to specify
the start and end velocities for the trajectory. This gives 4
constraints the trajectory must satisfy. Therefore, we require a
polynomial with four independent coefficients that can be chosen
to satisfy the constraints.

Therefore, we consider a cubic

trajectory of the following form:

q(t) = a0 + a1 t + a2 t 2 + a3 t 3

Then the desired velocity is given

by

q̇(t) = a1 + 2a2 t + 3a3 t 2

Figure: A typical joint space
trajectory.
Islam S. M. Khalil Trajectory Planning
Cubic Polynomial Trajectories
The determinant of the
coefficient matrix is
Combining equations yields (tf − t0 )4 , and hence we
have a unique solution
q0 = a0 + a1 t0 + a2 t02 + a3 t03 provided a nonzero time
v0 = a1 + 2a2 t0 + 3a3 t02 interval is allowed for the
execution of the trajectory.
qf = a0 + a1 tf + a2 tf2 + a3 tf3
vf = a1 + 2a2 tf + 3a3 tf2

These equations can be combined into a

single matrix equation

1 t0 t02 t03
    
a0 q0
0 1 2t 2 3t 2  a1   v0 
0  
1 tf t 2 t 3  a2  = qf 
  
f f
0 1 2tf 3tf2 a3 vf
Figure: A typical joint space
Islam S. M. Khalil trajectory.
Trajectory Planning
Cubic Polynomial Trajectories
As an illustrative example, we mat consider the special case that
the initial and final velocities are zero. Suppose we take t0 = 0 and
tf = 1 s, with
v0 = 0 , vf = 0
Therefore, we need to move the initial position q0 to the final
position qf in 1 second, starting and ending with zero velocity.

    
1 0 0 0 a0 q0 q 0 = a0
0 1 0 0 a1   0 

1
  =   0 = a1
1 1 1 a2  qf 
0 1 2 3 a3 0 q f − q 0 = a2 + a3
0 = 2a2 + 3a3
The latter two equations can be solved and yield

a2 = 3(qf − q0 ) , a3 = −2(qf − q0 )

Islam S. M. Khalil Trajectory Planning

Cubic Polynomial Trajectories
The required cubic polynomial function is therefore
q(t) = q0 + 3(qf − q0 )t 2 − 2(qf − q0 )t 3
the corresponding velocity and acceleration curves are
q̇(t) = 6(qf − q0 )t − 6(qf − q0 )t 2
q̈(t) = 6(qf − q0 ) − 12(qf − q0 )t

Figure: Trajectories with q0 = 10 deg and qf = −20 deg.

Islam S. M. Khalil Trajectory Planning

Quintic Polynomial Trajectories
The cubic trajectory gives continuous positions and velocities at
the start and finish points times but discontinues in the
acceleration. The derivative of acceleration is called the jerk. A
discontinuity in acceleration leads to an impulsive jerk, which may
excite vibrational modes in the manipulator and reduce tracking
accuracy.

Figure: Trajectories with q0 = 10 deg and qf = −20 deg.

For this reason, one may wish to specify constraints on the

acceleration as well as on the position and velocity.
Islam S. M. Khalil Trajectory Planning
Quintic Polynomial Trajectories

In this case, we have six constraints. on each initial and final

configuration, initial and final velocity, and initial and final
acceleration. Therefore, we require a fifth order polynomial.

q(t) = a0 + a1 t + a2 t 2 + a3 t 3 + a4 t 4 + a5 t 5

we obtain

q0 = a0 + a1 t0 + a2 t02 + a3 t03 + a4 t04 + a5 t05

v0 = a1 + 2a2 t0 + 3a3 t02 + 4a4 t03 + 5a5 t04
α0 = 2a2 + 6a3 t0 + 12a4 t02 + 20a5 t05
qf = a0 + a1 tf + a2 tf2 + a3 tf3 + a4 tf4 + a5 tf5
vf = a1 + 2a2 tf + 3a3 tf2 + 4a4 tf3 + 5a5 tf4
αf = 2a2 + 6a3 tf + 12a4 tf2 + 20a5 tf5

Islam S. M. Khalil Trajectory Planning

Quintic Polynomial Trajectories
which can be written as
t0 t02 t03 t04 t05
    
1 a0 q0
0 2
1 2t0 3t0 4t0 3 4
5t0  a1   v0 
   
0 2 6t0 12t0 20t03 
2
 
0
 a2  = α0 
   

1
 tf tf2 tf3 tf4 tf5  a3   qf 
   
0 2
1 2tf 3tf 4tf 3 4
5tf a4   vf 

0 0 2 6tf 12tf 20tf3
2 a5 αf

Figure: Trajectories with q0 (0) = 10 deg and qf (2) = 40 deg.

Islam S. M. Khalil Trajectory Planning

Quintic Polynomial Trajectories
We have to observe that the this method does not provide
constant velocity profile along the path.

Figure: Velocity profile with q0 (0) = 10 deg and qf (2) = 40 deg.

Islam S. M. Khalil Trajectory Planning

Linear Segments with Parabolic Blends (LSPB)
Another way to generate suitable joint space trajectories is by
using so-called Linear Segments with Parabolic Blends
(LSPB). This type of trajectory has a Trapezoidal Velocity
Profile and is appropriate when a constant velocity is desired along
a portion of the path. It consists of 3 parts:

t0 → tb : Quadratic polynomial and

this results in a linear ramp
velocity.
At tb : At the blend time, switch
to linear function and this
corresponds to constant velocity.
tb → tf : The trajectory switch to
quadratic polynomial so that the
Figure: Blend times for LSPB
velocity is linear. trajectory.

Islam S. M. Khalil Trajectory Planning

Linear Segments with Parabolic Blends (LSPB)
we choose the blend time so that the positron curve is symmetric.
Suppose that t0 = 0 and q̇(tf ) = 0. Then between times 0 and tb
we have
q(t) = a0 + a1 t + a2 t 2
so that the velocity is

q̇(t) = a1 t + 2a2 t

The constraints q0 = 0 and q̇0 = 0 imply that

a0 = q0
a1 = 0

A time tb we want the velocity to equal a Figure: Blend times

given constant, say V . Thus, we have for LSPB trajectory.

q̇(tb ) = 2a2 tb = V → a2 = V /2tb

Islam S. M. Khalil Trajectory Planning
Linear Segments with Parabolic Blends (LSPB)

Therefore the required trajectory

between 0 and tb is give by
V 2 α
q(t) = q0 + t = q0 + t 2
2tb 2
V
q̇(t) = t = αt
tb
V
q̈(t) = =α
tb
where α denotes the acceleration. Figure: Blend times for LSPB
trajectory.

Islam S. M. Khalil Trajectory Planning

Linear Segments with Parabolic Blends (LSPB)
Now, between time tb and tf − tb , the trajectory is a linear
segment with velocity V
q(t) = q(tb ) + V (t − tb )
Since, by symmetry,
tf q0 + qf
q( )=
2 2
we have
q0 + qf tf
= q(tb ) + V ( − tb )
2 2
which implies that
q0 + qf tf
q(tb ) = − V ( − tb )
2 2
Since the two segments must blend at time tb we require
V q0 − qf + Vtf
q0 + tb = + Vtb
2 2
Islam S. M. Khalil Trajectory Planning
Linear Segments with Parabolic Blends (LSPB)

which, upon solving for the blend time gives

q0 − qf + Vtf
tb =
V
Note that we have the constraint 0 < tb < t2f . This leads to the
inequality
qf − q0 2(qf − q0 )
≤ tf ≤
V V
to put it another way we have the inequality

qf − q0 2(qf − q0 )
≤V ≤
tf tf
Thus, the specified velocity must be between these limits or the
motion is not possible.

Islam S. M. Khalil Trajectory Planning

Linear Segments with Parabolic Blends (LSPB)
The complete LSPN trajectory is given by

α 2
 q0 + 2 t ,
 0 ≤ t ≤ tb ;
q0 −qf +Vtf
q(t) = 2 + Vt, tb ≤ t ≤ tf − tb ;
 q − αtf2 − α t 2 , t − t < t ≤ t .

f 2 2 f b f

Figure: Blend times for LSPB trajectory for V =60 and tb =1/3.

Islam S. M. Khalil Trajectory Planning

Linear Segments with Parabolic Blends (LSPB)

Figure: Cubic, quintic, and LSPB trajectory planning.

Islam S. M. Khalil Trajectory Planning
Thanks

Questions please

Islam S. M. Khalil Trajectory Planning