Chapter 6

Trajectory Planning
A trajectory is the path followed by the manipulator, plus the time prole along the path. Trajectories can be planned either in joint space (directly specifying the time evolution of the joint angles) or in Cartesian space (specifying the position and orientation of the end frame). Issues in trajectory planning include attaining a specic target from an initial starting point, avoiding obstacles, and staying within manipulator capabilities. Planning in joint space is the simplest and fastest, because inverse kinematics are avoided. The drawback is that the end effector pose is not directly controlled, and hence collision avoidance is difcult. Planning in Cartesian space more directly allows the geometric constraints of the external world to be met, but then inverse kinematics must be solved.

6.1 Joint Space Trajectories

6.1.1 Cubic Trajectories
We begin with joint space planning using cubic polynomials. A cubic polynomial has 4 coefcients, and hence may be used to satisfy both position and velocity constraints at the initial and nal positions. For joint variable qi , the constraints are: qi (t0 ) = q0
qi (t0 ) = q0

qi (tf ) = q1
qi (tf ) = q1

where t0 is the starting time, tf is the ending time, q0 and q0 are the specied initial position and velocity, and q1 and q1 are the specied nal position and velocity. The cubic polynomial for joint position and its derivative for joint velocity are: qi (t) = a0 + a1 t + a2 t2 + a3 t3 qi (t) = a1 + 2a2 t + 3a3 t2 (6.1)

We have to relate the ai coefcients to the endpoint constraints. To begin, w.l.o.g. shift time such that t0 = 0. Then we can nd a0 and a1 from the initial conditions: qi (0) = a0 = q0
c John M. Hollerbach, October 2012

CHAPTER 6. TRAJECTORY PLANNING

qi (0) = a1 = q0

Next, we nd a2 and a3 from the nal conditions. Substituting for a0 and a1 in (6.1) evaluated at tf ,
qi (tf ) = q0 + q0 tf + a2 t2 + a3 t3 = q1 f f qi (tf ) = q0 + 2a2 tf + 3a3 t2 = q1 f

(6.2) (6.3)

Eliminate a3 from (6.2) and (6.3):

3q1 tf q1 = 3q0 + 3q0 tf + 3a2 t2 + 3a3 t3 q0 tf 2a2 t2 3a3 t3 f f f f = 3q0 + 2q0 tf + a2 t2 f

(6.4)

Hence a2 = Similarly, a3 =
2(q0 q1 ) + (q0 + q1 )tf 3 tf 3(q1 q0 ) (2q0 + q1 )tf t2 f

(6.5)

(6.6)

Example 6.1: Suppose q0 = 10 degrees, q1 = 20 degrees, and q0 = q1 = 0 degrees/second. Suppose tf = 1 second. Then a0 = 10, a1 = 0, a2 = 90, , a3 = 60

Figure 6.1 plots the resulting trajectory. The Appendix contains the Matlab program used to generate this gure.

10

10

15

20 0

10

20

30

40

50

60

70

80

90

100

Figure 6.1: Cubic spline.

6.1.2

Linear Segments with Parabolic Blends

A common trajectory for industrial robots is the linear segment with parabolic blend (LSPB). Joint velocities are often slew-rate limited: a maximum velocity V is set by the joint controller. To move from an initial point q0 to a nal point q1 , the joint is accelerated from zero to the maximum velocity V using a parabolic

6.1. JOINT SPACE TRAJECTORIES

q(t) q1 (q1+ q 0)/2 q0 tb

(A)

. q(t)
V

tf/2 tftb

tf

tb

tf/2
(B)

tftb

tf

Figure 6.2: Velocity (A) and position (B) proles of LSPB trajectory. trajectory, then the joint cruises along at velocity V until it nears the endpoint, then decelerates to a stop with another quadratic polynomial (Figure 6.2). The second quadratic is symmetric to the rst quadratic. Dene the three polynomials as: qi (t) = a0 + a1 t + a2 t2 qi (t) = 0 + 1 (t tb ) qi (t) = b0 + b1 (t (tf tb )) + b2 (t (tf tb ))2 0 t tb tb t tf tb tf tb t tf (6.7)

where tb is the time of transition from the rst quadratic to the linear segment, tf is the nal time, and tf tb is the time of transition from the linear segment to the second quadratic (due to symmetry). The time shift form of the second and third segments is employed to simplify the development. There are 8 polynomial coefcients to solve for, and consequently 8 constraints must be given. Endpoint positions q(0) = q0 and q(tf ) = q1 . Endpoint velocities q(0) = q(tf ) = 0. Slew velocity q(t) = V for tb t tf tb . Matching constraints in position at qi (tb ) and qi (tf tb ). Symmetry condition which can be expressed as the midpoint being the average of the endpoints: q(tf /2) = (q0 + q1 )/2. The transition time tb is a dependent variable, which will be solved for given these constraints. It cannot also be specied because there would be too many constraints, leading to an inconsistency and no solution. If one wishes to adopt tb as a constraint, then one of the other constraints would have to be dropped, such as the symmetry constraint.

4 1. Ramping Up

CHAPTER 6. TRAJECTORY PLANNING

There are three constraints on the rst quadratic polynomial: position and velocity at the start, and the slew velocity at its end. The velocity equation is: qi (t) = a1 + 2a2 t Substituting the 3 constraints into the position and velocity equations, (6.8)

qi (0) = q0 = a0 qi (0) = 0 = a1 qi (tb ) = V = 2a2 tb Coefcients a0 and a1 are found immediately, and a2 can be expressed in terms of tb as a2 = V /(2tb ). Substituting, the rst quadratic is now: qi (t) = q0 + 2. Cruising Along There are two constraints acting on the linear segment: the symmetry constraint and the slew velocity. tf ) = 2 tf q0 + q1 = 0 + 1 ( tb ) 2 2 V 2 t 2tb 0 t tb (6.9)

qi (

qi (t) = V = 1 1 is found immediately, and 0 can be expressed as: q0 + q1 2 tf tb ) 2

= 0 + V (

0 = Substituting, the linear segment is:

tf q0 + q1 V ( tb ) 2 2

tf q0 + q1 V ( tb ) + V (t tb ) tb t tf tb (6.10) 2 2 The positions of the rst two polynomials at time tb must match. Equating (6.9) to (6.10) at t = tb , qi (t) = V 2 t = 2tb b tf q0 + q1 V ( tb ) 2 2 q0 q1 V (6.11)

q0 +

tb = tf +

6.2. CARTESIAN TRAJECTORY PLANNING

3. Ramping Down

The constraints on the second quadratic are that qi (tf ) = q1 , qi (tf ) = 0, and the matching constraint at t = tf tb . The velocity of the second quadratic is: qi (t) = b1 + 2b2 (t (tf tb )) Substituting the rst two constraints into the position and velocity equations, qi (tf ) = b0 + b1 tb + b2 t2 = q1 b qi (tf ) = b1 + 2b2 tb = 0 It is easiest to apply the matching constraint in terms of velocity V , which immediately yields that b1 . qi (tf tb ) = b1 = V The other two coefcients can now be found: b2 = V 2tb V tb 2 (6.12)

b0 = q 1

Example 6.2: Suppose q0 = 0, q1 = 40, V = 60, and tf = 1. Then tb = 1/3. Figure 6.3 plots the resulting trajectory.
40

35

30

25

20

15

10

0 0

10

20

30

40

50

60

70

80

90

100

Figure 6.3: Linear segment with parabolic blends.

6.2 Cartesian Trajectory Planning

Cartesian trajectories can be fashioned just like joint trajectories. In the present section, we expand the types of trajectories to include via points. A via point is an intermediate point that the trajectory passes close to, but does not necessarily pass through. An analogy might be knot points in b-splines in computer graphics. We suppose that a Cartesian trajectory is composed of a series of straight-line, constant-velocity trajectories connected by via points. Both position and orientation of the end-link frame have to be interpolated from beginning to end of each trajectory segment.

CHAPTER 6. TRAJECTORY PLANNING

. p1
p(t 1- )

p(t 1 + )

p0

Figure 6.4: Cartesian straight-line segments and transition controlled by via point p1 .

6.2.1

Positional Transition

The key issue is the shape of the curved trajectory that links two straight-line segments (Figure 6.4). We present a well-known solution due to Russ Taylor [2]. We will rst consider how to plan the position trajectory p(t). The rst straight-line trajectory segment starts at point p0 . Without any transition, the trajectory would arrive at via point p1 in time t1 under constant velocity. The second straight-line trajectory ends at point p2 . Without any transition, the trajectory would begin at via point p1 and arrive at p2 in time t2 under constant velocity. At a time interval before the scheduled arrival at p1 , we start the curved transition. Thus p(t1 ) is the rst transition point. Similarly, p(t1 + ) is the second transition point, from curved to the second straight-line trajectory. The curved transition is a parabolic segment, i.e., the acceleration is constant. Transition times t1 and t2 , and points p0 , p1 , and p2 are prespecied. We consider the constraints at the transition points. The position constraints for the constant-velocity straight line segments evaluated at the transition points are: p1 t1 p(t1 + ) = p1 + p2 t2 p(t1 ) = p1 where p1 = p1 p0 (6.13) where p2 = p2 p1

The velocity constraints for the constant-velocity straight-line segments at the transition points are: p(t1 ) = p(t1 + ) = p1 t1 p2 t2 (6.14) (6.15)

Acceleration is constant during the curved transition path: p(t) = ap (6.16)

6.2. CARTESIAN TRAJECTORY PLANNING

where the acceleration ap is not know yet. The solution is found by integrating (6.16): p(t) p(t1 ) = p(t1 )(t t1 + ) + ap (t t1 + )2 2

(6.17)

This is simply the elementary physics result for motion under constant acceleration (s = V0 t + at2 /2). Substituting from (6.13) and (6.14) and rearranging, p(t) = p1 + p1 ap (t t1 ) + (t t1 + )2 t1 2 (6.18)

To solve for ap , evaluate (6.18) at the second transition point t = t1 + : ap = Substituting this ap into (6.18), p(t) = p1 p1 p2 (t t1 )2 + (t t1 + )2 4 t1 4 t2 (6.20) 1 2 p2 p1 t2 t1 (6.19)

Note that the trajectory does not actually pass through p1 . The three trajectory segments are summarized below.
p1 t1 t p1 t1 p 4 t1 t t1 p2 p1 +

0 t t1 p2 (t t1 + )2 4 t2 t1 t t1 + t1 + t t1 + t2 (6.21)

p(t) =

p1

(t t1 )2 +

t2

6.2.2

Rotational Transition

The rotational transition along the trajectory is formulated by nding the equivalent axis of rotation k. Let R0 be the orientation at the start, R1 the orientation at the via point, and R2 the orientation at the goal. Then R0 Rk1 (1 ) = R1 Rk1 (1 ) = RT R1 0 R1 Rk2 (2 ) = R2 Rk2 (2 ) = RT R2 1 (6.23) (6.22)

where the angle-axis representation Rk1 (1 ) transforms from R1 to R0 , and Rk2 (2 ) transforms from R2 to R1 . The amount of rotation about k1 and about k2 can then be made a linear function of time. The rotational motion along the rst straight-line path is: R(t) = R1 Rk1 ( t1 t 1 ), t1 0 t t1 (6.24)

8 The rotational motion along the second straight-line path is: R(t) = R1 Rk2 ( t t1 2 ), t2

CHAPTER 6. TRAJECTORY PLANNING

t1 + t t1 + t2

(6.25)

The transitions between path segments will now be reexamined to incorporate rotations. The results are stated, and may be derived analogously to the position transition equations (6.20): R(t) = R1 Rk1 (1 (t t1 + )2 (t t1 )2 )Rk2 (2 ), 4 t1 4 t2 t1 t t1 + (6.26)

Taylor represents rotations as quaternions for efciency. Another Cartesian trajectory planning scheme is due to Paul [1]. In the straight-line portion, p(t) = p0 + t (p1 p0 ) T (6.27) (6.28)

R(t) = R0 Rk (t/T )Rz (t/T )

where z is the approach axis of the tool frame, is the twist, and k is the cross product of the initial and nal z axes. An advantage of Pauls scheme is the planning of rotation in terms of tool axes. An advantage claimed of Taylors rotational scheme is uniformity of motion.

6.3. APPENDIX

6.3 Appendix
% cubic Cubic polynomial trajectory with specified end p % % q = cubic(q0,q1,qdot0,qdot1,tf) generates a cubic % trajectory comprised of 100 equally-spaced points % % q0, q1 = initial and final joint position. % qdot0, qdot1 = initial and final joint velocity. % tf = total movement time. % q = vector of joint positions, equally spaced in function q = cubic(q0,q1,qdot0,qdot1,tf) a = zeros(4,1); % Generate coefficients of cubic polynomial. a(1) = q0; a(2) = qdot0; a(3) = (3 * (q1-q0) - tf * (2 * qdot0 + qdot1)) / tf2; a(4) = (-2 * (q1-q0) + tf * (qdot0 + qdot1)) / tf3; % Divide time into 100 intervals. dt = tf/100; % This Matlab trick creates an array from 0 to tf with increment dt. t = 0:dt:tf; % Generate joint trajectory. q = a(1) + t .* (a(2) + t .* (a(3) + t * a(4))); % Plot the results. Control axis spacing. plot(q); axis([0 100 -20 10]); % Save the plot for printing on a postscript printer. print -deps2 cubic.eps & v, and tf. polynomial in time.

time.

10

CHAPTER 6. TRAJECTORY PLANNING

Bibliography
[1] Paul, R.P., Manipulator Cartesian path control, IEEE Trans. Systems, Man, Cybernetics, vol. SMC9, pp. 702-711, 1979. [2] Taylor, R.H., Planning and execution of straight-line manipulator trajectories, IBM Journal of Research and Development, vol. 23, pp. 424-436, 1979.

11