CHAPTER 5

Trajectory Planning
5.1 Introduction
In the previouschapters, we studied the
that, using the equations kinematics and
be if we have the joint of motion of the robot, we can dynamics of robots. This means
order to place the robotvariables or we can determine determine where the robot will
what the joint
and trajectory planning at a desired position and orientation with a variables must be in
in a controlled relates to the way a robot is desired velocity. Path
manner. In this chapter, we will moved from one location to another
must be made to create a study the sequence of
controlled movement movements
straight-line motions or sequential motions. between motion segments, whetherthatin
of both kinematics and
dynamics of robots. InPath and trajectory planning
So intensive that practice, precise requires the use
approximations are always necessary. nmotion requirements are
5.2 Path versus Trajectory
A path is defined as the
collection a sequence of configurations a robot
from one place to another withoutofregard makes to go
to the timing of these configurations.
Figure 5.1, if a robot goes from point So, n
(configuration)
sequence of the configurations betweenA and B and Ato point B to point C, he
Cconstitutes
related to the timing at which each part of the path must be a path. A trajectory 5
of when points B and Cin Figure 5.1 are attained. As a result, regardes
reached, the path is the same, wheres
depending on howfast each portion of the path is traversed, the trajectory
Therefore, the points at which the robot may be on apath and on may ditet.
time may be different, even if the robot traverses the sane points. a trajectory at a g
On a rayeo
depending on the velocities and accelerations, points BandCmay be reached at htteren
times, creating different trajectories. In this chapter, we are not only concerned about the
path a robot takes, but also its velocities and accelerations.

178
sJoint-
Spae
versus Cartesian-Space Descriptions 179

ure 5.1 Sequential robot movements in a path.

5.3)Joint-Space versus Cartesian-Space Descriptions

Cansider a 6-axis robot at a point A in space, which is directed to move to another
p IIsing point
the inverse kinematic equations of the robot, derived in Chapter 2, we may
alculate the total joint displacements the robot needs to make to get to the new location.
The ioint values thus calculated can be used by the controller to drive the robot joints to
their new values and. consequently, move the robot am to its new position. The
description of the motion to be made by the robot by its joint values is called joint-space
description. In this case,although the robotwill eventually reach the desired position, as
we will see later, the motion between the two points is unpredictable.
Now assume that a straight line isdrawn between points A and B, and it is desirable to
have the robot move from point A to point B in a straight line. To do this, it will be
necessary to divide the line into small portions, as shown in Figure 5.2, and to move the
robot through all intermediate points. To accomplish this task, at each intermediate
location, the robot's inverse kinematic equations are solved, a set of joint variables is
calculated, and the controller is directed to drive the robot to those values. When all
segments are completed, the robot will be at point B, as desired. However, in this case.
unlike thejoint-space case discussed above, the motion is known at all times. The sequence
ofmovements the robot makes is described in Cartesian-space and is converted to joint-space
at each segment. As is clear from this simple example, the Cartesian-space description is
much more computationally intensive than the joint-space description, but yields a
controlled and known path. Both joint-space and Cartesian-space descripions are very
Usetul and are used in industry. Howvever, each one has its own advantages and
disadvantages.

follow a suraight lne.

Figure 5.2 Sequential motions of a robot to
180 Chapterr 5. Trajectory
Planning

(a) (b)

Figure 5.3 Cartesian-space trajectory problems. In (a), the trajectory specified in Cartesin
coordinates may force the robot to run into itself; in (b), the trajectory may require a sudden
change in the joint angles.

Cartesian-space trajectories are very casy to visualize. Since the trajectories are in the
common Cartesian space in which we all operate, it is easy to visualize what the end
effector's trajectory must be. However, Cartesian-space trajectories are computationaly
expensive and require a faster processing time for similar resolution than joint-space
trajectories. Additionally, although it is easy to visualize the trajectory, it is difficult to
visually ensure that singularities will not occur. For example, consider the situation in
Figure 5.3(a). If not careful, we may specify a trajectory that requires the robot to run into
itself or to reach a point outside of the work envelope-which, of course, is impossible
and yields an unsatisfactory solution. This is true because it may be impossible to know
whether the robot can actually make a particular location and orientation before the
motion is made. Also, as shown in Figure 5.3(b), the motion between two points may
require an instantaneous change in the joint values (we discussed why this may happen in
Chapter 2), which is impossible to predict. Some of these problems may be solved by
specifying viapoints the robot must pass through in order to avoid obstácles and other
similar singularities.

5.4 Basics of Trajectory Planning

Tounderstand the basics of planning a trajectory in joint-space and Cartesian-space, let
considerasimple 2-DOF robot (mechanism). In this case, as shown in Figure 5.4,
desire to move the robot from point A to point B. The configuration ofthe robot at pot
Ais shown, with a = 20 and ß = 30°. Suppose it has been calculated that in order 0
the robot to be at point B, it must be at a = 40° and B= 80°. Also suppose that both
joints of the robot can nove at the maximum rate of 10 degrees/sec. One way to
the robot from point Ato Bis to run both joints at their maximum angular velocite,
This means that at the end of the second time interval, che lower link of the robot w
have finished its motion, while the upper link continues or another three seconds,
 Trajectory Planning
Y4 5 3 181
6
Time
B (sec)
20 30
1 30 40
A 2 40 50
40 60
4 40 70
40 80

5.4 Joint-space, non-normalized

Figure
movements of a 2-DOF robot.

shown in Figure
5.4. The trajectory of the end of
the path is
iregular, and the distances traveled the robot is also shown. As indicated,
by the robot's end are
Nowsuppose the motions of both joints of the robot not uniform.
motion are normalized such that
will move slower so that both joints will start the joint
smaller
mion simultaneously. In proportionally
with
this case, both joints move at and stop
inuously together. a changes 4 degrees/second different speeds, but move
The resulting trajectory will be different, as shown whileB changes 10 degrees/second.
ents of the movement are much more in Figure 5.5. Notice that the
ll irregular (and different trom the similar to each other than before, but the
path
previous case).
inint-space as we were only concerned with the values of Both of these cases were planned in
he end of the mechanism. The only the joints, not the location of
calculation needed was the joint values for the
destination and, in the second case, normalization of the
Now suppose we want the robot's hand to follow a joint velocities.
B. say, ina straight line. The simplest solution would be known path between points A and
and B, divide the line into, say, 5 segments, and solve for to draw a line between points A
point, as shown in Figure 5.6. This is called interpolation necessary angles a and Bat each
Notice that in this case, the path is a straight line, but the joint between points A and B.
angles are not uniformly
changing. Although the resulting motion is a straight (and consequently,
trajectory, it is necessary to solve for the joint values at each point. Obviously, known)
more points must be calculated for better accuracy; with so few segments the many
robot will
not exactly follow the lines at each segment. This trajectory is in
Cartesian-space since all

5 4 Time
6
(sec)
B 2 0 20 30
1 24 40
4 2 2850
3 32 60
4 36 70
5 40 80

a
Figure 5.5 Joint-space, normalized movements of arobot with 2DOF,
182 Chapter 5. Trajectory Plan ing
Time
6
5 4
(sec)
B 20 30
2 1 14 55
2 16 69
A 3 21 77
4 29 81
5 40 80

Figure 5.6 Cartesian-spaçe movements of a 2-DOF robot.

segments of the motion must be calculated based on the information expressed in .

Cartesian frame.
In this case, it is assumed that the robot's actuators are strong enough to provide the
large forces necessary to accelerate and decelerate the joints as needed. For example.
notice that we are assuming the arm will be instantaneously accelerated to have the
desired velocity right at the beginning of the motion in segment 1. If this is not true, the
robot will follow a trajectory different from our assumption; it will be slightly behind at
accelerates to the desired speed. Additionally, note how the difference between two
consecutive values is larger than the maximum specified joint velocity of 10 degrees
second (e.g., between times 0and1, the joint mustmove 25 degrees). Obviously, this is not
attainable. Also note how, in this case, joint 1moves downward first before moving up.
To improve the situation, we may actually divide the segments differently by starting
the arm with smaller segmentsand, as we speed up the arm, going at a constant cruising
rate and finally decelerating with smaller segments as we approach inverse point B. This s
still needto solve the kinemaic
schematically shown inFigure [Link], we
case. However, in
equations of the robot at each point, which is similar to the previous
we may divide
this case, instead of dividing the straight ineAB into equal segments,velocity of v = d.
based on x =at' until such time t when we attain the cruising
Similarly, the end portion of the mnotion can be divided based on
a decelerating regimen.
Another variation to this trajectory planning is to plan a path that
is not straight, bu
equation. To do ths,
one that follows some desired path, for example a quadratic

7 6

3 2 1

acceleration/deceleration regiment.
Figure 5.7 Trajectory planning with an
 Trajectory Planning 183

B B"

B'

(a)
(b)
Figure 5.8 Blending of different mnotion segments in a path.

coordinates of each segment are calculated based on the desired path and are used to
calculate joint variables at each segment; therefore, the trajectory of the robot can be
path.
planned for any desired
So far, we have only considered the movement of the robot between two points A and
B. However, in other cases,the robot may be going through many consecutive points,
including intermediate or via points. The next level of trajectory planning is between
multiple points and, eventually, for continuous movements.
Assume the robot 1s to go trom point Ato Band then to C, as shown in Figure 5.8.
One way to run the robot is to accelerate from point A toward point B, cruise,decelerate
and stop at point B, start again at point Band accelerate toward C, cruise, and decelerate
in order to stop at C. This stop-and-go motion will create jerky motions with
unnecessary stops. An alternative way is to blend the two portions of the motion at
point Bsuch that the robot will approach point B, decelerate if necessary, follow the
blended path, accelerate once again toward point C, and eventually stop at C. This
creates a much more graceful motion, reduces the stress on the robot, and requires less
energy. If the motion consists of more segmnents, all intermediate segments can be
blended together. Notice how, due to this blending of the segments, the robot will go
through adiferent point B (Figure 5.8(a) and not [Link] is necessary that the robot goes
through point B exactly, then a different point B' must be specified such that after
blending, the robot still goes through point B(Figure 5.8(b). Another alternative is to
pecity two via points C and D before and after point B, as shown in Figure 5.9 such that
POnt Btalls on the straight-line portion of the motion between the via points and thus
ensure that the robot goes through point B.
n the next sections, we will discuss different methods of trajectory planning in more
eGenerally, higher-order polynomials are used to match positions, velocities, and
accelerations at each point between two segments. When the path is planned, the
controller
the inverse uses the path information (coordinates) in calculating joint variables trom
the robot is kinematic equations and runsthe robot accordingly. Ultimately, if the path of
very complicated and involved, such that it cannot be easily expressed by an
equation, it may become necessary to physically move the robot by hand and record the
motions at
each joint. The recorded joint values can be used later to runthe robot. This is
commonly
welding done for teaching robots tasks such as spray pantng dutonobiles, seam
coomplicated shapes, and other similar tasks.
184 Chapter 5. Trajectory Planring
D
B

B'

Figure 5.9 An alternative scheme for ensuring that the robot will go through a
point during blending of motion segments. Two via points Cand D are picked such thspecifes
Bwill fall on the straight-line section of thesegment ensuring that the robot will pass
point B. through

5.5 Joint-Space TrajectoryPlanning

In this section, we will see how the motions of arobot can be planned in joint-space wirth
controlled characteristics. Anumber of different schemes such as polynomials of ditferent
orders and linear functions with parabolic blends can be used for this purpose. The
following is a discussion of some of these schemes used in joint-space trajectory planning
Notice that these schemes specify joint values, not Cartesian values. We will discus
Cartesian-space later.

5.5.1 Third-Order Polynomial Trajectory Planning

Inthis application, the initial location and orientation of the robot are known and, using
the inverse kinematic equations, the final joint angles for the desired position anu
orientation are found. However, the motions of each joint of the robot must be planne
individually. Therefore, consider one of the joints,which at the beginning of the mocen
segment at timet; is at ;, and which we want to move to a new value of 8, at time ; Oe
way to do this is to use a polynomial to plan the trajectory such that the initial and hna
boundary conditions match what we already know, namely that 0, and 8; are known uN
the velocities at the beginning and the end of the motion segment are zero (or oas
knownvalues). These four pieces of information allow us to solve for four unknowts (
a third-order polynomial) in the form of:
(1) = t citt or t a
where the initial and final conditions are:
O(,) = 0,
(5.2

e)= 0

Taking the first derivative of the polynomial of Equation (5.1), we get:

(5.3)
(1) = (+ 2r2t + 3cgt'
 185
Space
Trajectory Planning
Substituting the initial and final conditions into Equations (5.1) and (5.3) yelds:
s5Jnint--

(;) =() = 0,
(5.4)
O() = ( = 0
9(t) = ( + 22tr + 3(st, =0
which can also be written in matrix form as:

) 1 ()
C1
-
(5.5)
, C2
2t
3tf C3

these four
By solving This allows usequations simultaneously, we get the necessary values for the
constants. to calculate the joint position at any interval of time, which can
be used for
be used bythe controller to drive the joint to position. The same proces must
each joint individually, but they are all driven together from start to finish.
Obviously, if
cbe initialand final velocities are not zero, the given values can be used in these equations.
Therefore. applying this third-order polynomial to each joint motion creates a motion
profile that can be used to drive each joint.
If more than two points are specified, such that the robot will go through the points
succesively. the final velocities and positions at the conclusion of each segmnent can be
used as the initial values for the next segments. Similar third-order polynomials can be
used to plan each section. However, although positions and velocities are continuous,
accelerations are not, which may cause problems.
Example 5.1
It is desired to have the first joint of a 6-axis robot go from initial angle of 30° to a
final angle of 75° in 5 seconds. Using a third-order polynomial, calculate the joint
angle at 1, 2, 3, and 4 seconds.
Solution: Substituting the boundary conditions into Equation (5.4), we get:
(o(t;) = ) = 3) C) =30

(t;) =o + i(5) + o(5) + a(5)= 75

C =5.4
o(;) =cq = 0
((,) = + 22(5) + 3c:(5) = 0 C3 -0.72

This results in the following cubic polynomial equation for position as well as che
velocity and acceleration equations for joint 1:
b() = 30 + 5.4-0.72r
0() = 10.8r 2.16P
() = 10.8 4.321
Substituting the desired time intervals into che motion equaion will result in:
B(1) = 34.68°, (2) = 45.84°, (3) = 59.16°, (4) = 70.32°
186 Chapter 5. Trajectory
80

Positiop
Plannin,
60

40

2
Velocity

Acceleration
-20
0 1 2 3 4 6
Seconds

Figure 5.10 Joint positions, velocities, and accelerations for Example 5.1.

The joint angles,velocities, and accelerations are shown in Figure 5.10. Notice th
in this case, the acceleration needed at the beginning of the motion is 10.8°/sec
well as -10.8°/sec deceleration at the conclusion of the motion).

Example 5.2
Suppose the robot arm of Example 5.1 is to continue to the next point, where the
joint is to reach 105° in another 3 seconds. Draw the position, velocity, and
acceleration curves for the motion.
Solution: Atthe conclusion of the first segment, we know the position and velocity
of the joint. Using these values as initial conditions for the next segment, we get
(1) = o t cËtt gt t ct
(t) = + 2c2t + 3c3t
(1) = 2cg + 6c3t
where:
at t; = 0 ; = 75 9, = 0
at t, = 3 r= 105 ; =0

which will yield:

C = 0 C2 = 10 C3 = -2.222
Co =75
( ) =75 +102- 2,222
0() =201 6.6ó6
0() = 20 - 13.3321
velocities, and accelerations tor he
en
shows the positions,
Figure 5.11 However, no
you can see, the boundary conditions are as expected. curve

motion. As continuous, the slope of the velocity

that alhough the velocity curve Is an
i n s t a n t a n c o U S

positive at the intermediate point, creating: accelerationss or

changes from negative to robot is capable of ereating:
such
change in acceleration. Whether the
 -Space Trajectory Planning 187
120

00

Position

30

()
Velocity

-30 Acceleration
1
3 4 5 7
Seconds
iãure 5.1l Joint positions, velocities, and accelerations for
Example 5.2.
not is a question that must be answered depending on the robot's capabilities. To
ensure that the robot's accelerations will not exceed its capabilities,
acceleration
limits may be used to calculate the necessary time to reach the target. In that
À. = 0and r=0, the maximum acceleration will be:+
case, for

Imax
6(0, -0) 2
(ts- )
from which the ime-to-target can be calculated. You should also notice that the
velocity at the intermediate point does not have to be zero. In that case, the
concluding velocity at the intermediate point willbe the same as the initial velocity
of the next segment. These values must be used in calculating the coefficients of the
third-order polynomial.
5.5.2Fifth-Order Polynomial Trajectory Planning
Inthe previous section, we encountered accelerations that may be impossible to achieve
during motion trajectories; therefore, we may need to specify maximum accelerations as
well. Specifying the initial and ending positions, velocities, and accelerations ofa segment
yields six pieces of information, enabling us to use a fifth-order polynomial to plan a
trajectory, as follows:
(1) =o + ctt (2ft cot + rt+ cs (3.6)

(1) =cj + 22t +3c3r + 4c4r + 5cst (5.7)

=2r2 + 6c3t+ 12r4t +20cse

() (5.8)
polynonial with
ese equations allow us to calculate the coeticients of' a fitth-order
position, velocity. and acceleration boundary conditions.
Example 5.3 d inall deceleraion
Repeat 5.1, but assune the initual
acccleration will be
5/sec Example
 188 Chapter 5. Trajectory i Plan ing
80
Position
(60

40

20
Velocity

Acceleration
-20
2 3 4

Seconds

Figure 5.12 Joint positions, velocities, and accelerations for Example 5.3.

Solution: From Example 5.1 and the given accelerations, we have:

8, = 30° 9, = 0°/sec 0, = 5°/sec?
O, =75° 8, = 0°/sec , =-5°/sec?

Using Equations (5.6) through (5.8), with the given initial and final boundar
conditions, we get:
Co = 30 C =0 (2 =2.5

C3 = 1.6 c4 = -0.58 Cz = 0.0464

This results in the following motion equations:
O(t) = 30 + 2.5 + 1.6f 0.58 + 0.04645
(t) =5t +4.81 -2.32r + 0.232r4
(t) =5 +9.6t 6.96 + 0.928F
Figure 5.12shows the position, velocity, and acceleration graphs for the joint. The
maximum acceleration is 8.7°/sec

5.5.3 Linear Segments with Parabolic Blends

Another alternative for joint-space trajectory planning is to run the joints at constunt
speed between the initial and final locations, as we discuSsed in Section 5.4. This b
equivalent to afirst-order polynomial, where thevelocity is constant and acceleration b
zero. However, this also means that at the beginning and at che end of the noaeu
segment, accelerations mustbe infinite in order to create instantaneous velocities at
boundaries. To prevent this, the linear segnent can be blended with parabolhc secaos
the beginning and at the end of the motion segment, creating a continuous positon
velocity, as shown in Figure 5.13. Assuming the initial and tnal positions are , and or
imes t, =0and i, and the parabolic segments are symnerically blended wich che hnca
Joint-Space Trajectory Planning 189

Positon B

Time

Figure 5.13 Scheme for linear segments with parabolicblends.

section at blending times , and t-t, we can write:
1
O(t) = co t c1t
(5.9)
(1) = 1 t cot
(t) = (2
Obviously, in this scenario, acceleration is constant for the parabolic sections, yielding a
continuous velocity at the common points (called knot points) Aand B. Substituting the
boundary conditions into the parabolic equation segment yields:
O(t= 0) =0; = Co
(t= 0) =0=4 C1 =0
O() = (2
This will result in parabolic segments in the form:
1
(t) = 9;+ (5.10)

(i) =(2 (5.11)

Ö(t) = 2 (5.12)
chosen
Clearly, for the linear segment, the velocity will be constant and can be
zero initial velocity, a
based on the physical capabilities of the actuators. Substituting zero tinal velocity in
linear portion and
onstant known joint velocity w in the points 4, Band the tunal
Equation (5.11), we find the joint positions and velocities tor
point as follows:

04= (2t, = 0 (5.13)

0 = , +(04 -,)
0, = 0