Chapter 6
Trajectory Planning A trajectory is the path followed by the manipulator, plus the time profile 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 specific 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 difficult. 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 6.1.1
Joint Space Trajectories Cubic Trajectories
We begin with joint space planning using cubic polynomials . A cubic polynomial has 4 coefficients, and hence may be used to satisfy both position and velocity constraints at the initial and final positions. For joint variable qi , the constraints are:
qi (t0 ) = q0
qi (tf ) = q1
′
′
q˙i (t0 ) = q0
q˙i (tf ) = q1
where t0 is the starting time, the ending time, tqf is 0 and q0 are the specified initial position and velocity, and q1 and q1 are the specified final position and velocity. ′ ′
The cubic polynomial for joint position and its derivative for joint velocity are:
qi (t) = a0 + a1 t + a2t2 + a3 t3 q˙i (t) = a1 + 2a2 t + 3a3 t2
(6.1)
We have to relate the ai coefficients to the endpoint constraints. To begin, w.l.o.g. shift time such that t0 = 0. Then we can find a0 and a1 from the initial conditions:
qi (0) = a0 = q0 c John M. Hollerbach, October 2012
1
CHAPTER 6. TRAJECTORY PLANNING
2
′
q˙i (0) = a1 = q0 Next, we find a2 and a3 from the final conditions. Substituting for a0 and a1 in (6.1) evaluated at tf ,
qi (tf ) = q0 + q0 tf + a2 t2f + a3 t3f = q1
(6.2)
q˙i (tf ) = q0 + 2a2 tf + 3a3 t2f = q1
(6.3)
′
′
′
3
Eliminate a from (6.2) and (6.3):
3q1 − tf q1 = 3q0 + 3q0 tf + 3a2 t2f + 3a3 t3f ′
′
−
′
q0 tf
−
2a2 t2f
= 3q0 + 2q0 tf + a2 t2f ′
−
3a3 t3f (6.4)
Hence
a2 =
3(q1 − q0) − (2q0 + q1 )tf t2f
(6.5)
a3 =
2(q0 − q1) + ( q0 + q1 )tf t3f
(6.6)
′
′
Similarly, ′
′
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 figure.
10
5
0
−5
−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 controlle r. To move from an initial point q0 to a final point q1 , the joint is accelerated from zero to the maximum velocity V using a parabolic
6.1. JOINT SPACE TRAJECTORIES
3
q(t) q
1
.q(t)
(q + q )/2 1
0
q
V
0
tb
tf/2
tf−tb
tf
t
tb
(A)
tf/2
tf−tb
tf
t
(B)
Figure 6.2: Velocity (A) and position (B) profiles of LSPB trajectory. trajectory, then the joint cruises along at velocity V until it nears the endpoint, then decelerates to a stop with another quadrat ic polynomial (Figur e 6.2). The second quadrati c is symmetric to the first quadratic. Define the three polynomials as:
qi (t) = a0 + a1 t + a2 t2
0
≤
t
≤ tb
qi (t) = α0 + α1 (t − tb )
tb
≤
t
≤ tf − tb
− tb
≤
t
≤ tf
qi (t) = b0 + b1 (t − (tf
− tb )) +
b2 (t − (tf
− tb ))2
tf
(6.7)
where tb is the time of transition from the first quadratic to the linear segment, tf is the final 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 segmen ts is employed to simplify the development. There are 8 polynomial coefficients 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 constrai nts. It cannot also be specified 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.
CHAPTER 6. TRAJECTORY PLANNING
4 1. Ramping Up
There are three constraints on the first quadratic polynomial: position and velocity at the start, and the slew velocity at its end. The velocity equation is:
q˙i (t) = a1 + 2a2t
(6.8)
Substituting the 3 constraints into the position and velocity equations,
qi (0) = q0 = a0 q˙i (0) = 0 = a1 q˙i (tb ) = V = 2a2 tb Coefficients a0 and a1 are found immediately, and a2 can be expressed in terms of tb as a2 = V /(2tb ). Substituting, the first quadratic is now:
qi (t) = q0 +
V 2 t 2tb
0 ≤ t ≤ tb
(6.9)
2. Cruising Along There are two constraints acting on the linear segment: the symmetry constraint and the slew velocity.
qi (
tf tf q0 + q1 ) = = α0 + α1( 2 2 2
− tb )
q˙i (t) = V = α1 α1 is found immediately, and α0 can be expressed as: q0 + q1 2 α0
= α0 + V (
q0 + q1 = 2
tf 2
− tb )
tf −V(2
− tb )
Substituting, the linear segment is:
qi (t) =
q0 + q1 2
−
V(
tf 2
− tb ) +
V ( t − tb )
tb
≤
t ≤ tf
− tb
(6.10)
The positions of the first two polynomials at time tb must match. Equating (6.9) to (6.10) at t = tb ,
q0 +
V 2 t = 2tb b
q0 + q1 2
tb = tf +
−V(
q0 − q1 V
tf 2
− tb )
(6.11)
6.2. CARTESIAN TRAJECTORY PLANNING
5
3. Ramping Down The constraints on the second quadratic are that qi (tf ) = q1 , q˙i (tf ) = 0, and the matching constraint at t = tf − tb . The velocity of the second quadratic is:
q˙i (t) = b1 + 2b2 (t − (tf
− tb ))
(6.12)
Substituting the first two constraints into the position and velocity equations,
qi (tf ) = b0 + b1tb + b2t2b = q1 q˙i (tf ) = b1 + 2b2 tb = 0 It is easiest to apply the matching constraint in terms of velocity V , which immediately yields that b1 .
q˙i (tf
− tb )
= b1 = V
The other two coefficients can now be found:
b2 =
−
V 2tb
b0 = q1 −
V tb 2
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
5
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
6
. p1 p(t 1- τ)
p0
.
.
p(t 1 + τ)
.
.
p2
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 trajec tory that links two straight-line seg ments (Figure 6.4). We present a well-known solution due to Russ Taylor [2]. We will first consider how to plan the position trajectory p(t). p0 . Without any transition, the traj ectory would arrive at via point p1 in time t1 under constant velocity.
• The first straight-line trajectory segment starts at point
• 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.
τ before the scheduled arrival at p1 , we start the curved transition. Thus p(t1 − τ ) is the first transition point. Similarly, p(t1 + τ ) is the second transition point, from curved to the second straight-line trajectory.
• At a time interval
• 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 prespecified. We consider the constraints at the transition points. The position constraints for the constant-velocity straight line segments evaluated at the transition points are:
τ ∆p 1 t1 τ p(t1 + τ ) = p1 + ∆p2 t2 p(t1 − τ )
= p1 −
where
∆p1 = p1 − p0
where
∆p2 = p2 − p1
(6.13)
The velocity constraints for the constant-velocity straight-line segments at the transition points are:
˙ (t1 − τ ) p
=
∆p1 t1
(6.14)
˙ (t1 + τ ) p
=
∆p2 t2
(6.15)
Acceleration is constant during the curved transition path:
¨ ( t) = a p p
(6.16)
6.2. CARTESIAN TRAJECTORY PLANNING
7
where the acceleration ap is not know yet. The solution is found by integrating (6.16):
˙ (t1 − τ )(t − t1 + τ ) + p(t) − p(t1 − τ ) = p
ap
2
(t − t1 + τ )2
This is simply the elementary physics result for motion under constant acceleration ( Substituting from (6.13) and (6.14) and rearranging, p(t) = p1 +
∆p 1
t
ap
( t − t1 ) +
(6.17)
s = V0 t + at2 /2).
(t − t1 + τ )2
(6.18)
2
1
To solve for ap , evaluate (6.18) at the second transition point t = t1 + τ : ap =
1 2τ
∆p t2
2
−
∆p 1 t1
(6.19)
Substituting this ap into (6.18), p(t) = p1 −
∆p1 ∆p 2 (t − t1 − τ )2 + ( t − t1 + τ ) 2 4τ t1 4τ t2
(6.20)
Note that the trajectory does not actually pass through p1 . The three trajectory segments are summarized below.
t1 − t
p t ∆p ∆p p(t) = p 4τ t (t t p + t t ∆p t 1
−
0 ≤ t ≤ t1 − τ
1
1
1
1
−
−
1
−
1
−
τ )2 +
∆p 2 ( t − t1 + τ ) 2 4τ t2
1
1
2
t1 − τ
≤
t ≤ t1 + τ
t1 + τ
≤
t ≤ t 1 + t2
(6.21)
2
6.2.2
Rotational Transition
The rotational transition along the trajectory is formulated by finding 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 ) Rk1 (θ1 ) R1 Rk2 (θ2 ) Rk2 (θ2 )
= R1 = RT0 R1
(6.22)
= R2 = RT1 R2
(6.23)
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 first straight-line path is: R(t) = R1 Rk1 (−
t1 − t θ ), t1 1
0 ≤ t ≤ t1 − τ
(6.24)
CHAPTER 6. TRAJECTORY PLANNING
8 The rotational motion along the second straight-line path is: R(t) = R1 Rk2 (
t − t1 θ2 ), t2
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)
= R
1
Rk1 (−θ1
( t − t1 − τ ) 2 ( t − t1 + τ ) 2 2 k )R (θ ), 4τ t1 4τ t2 2
t1 − τ
≤
t ≤ t1 + τ
(6.26)
Taylor represents rotations as quaternions for efficiency. Another Cartesian trajectory planning scheme is due to Paul [1]. In the straight-line portion,
t (p1 − p0 ) T
p(t)
= p0 +
R(t)
= R0 Rk ( t/T)Rz (φt/T )
(6.27) (6.28)
where z is the approach axis of the tool frame, φ is the twist, and k is the cross product of the initial and final z axes. An advantage of Paul’s scheme is the planning of rotation in terms of tool axes. An advantage claimed of Taylor’s rotational scheme is uniformity of motion.
6.3. APPENDIX
6.3
Appendix
% cubic Cubic polynomial trajectory with specified end p & v, and tf. % % q = cubic(q0,q1,qdot0,qdot1,tf) generates a cubic polynomial % trajectory comprised of 100 equally-spaced points in time. % % 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 time. 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)) / tfˆ2; a(4) = (-2 * (q1-q0) + tf * (qdot0 + qdot1)) / tfˆ3; % 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 spac ing. plot(q); axis([0 100 -20 10]); % Save the plot for printing on a postscript printer. print -deps2 cubic.eps
9
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
