PLANNING OF MANIPULATOR TRAJECTORIES INTRODUCTION ‘With the discussion of kinematics and dynamics of a serial link manipulator in # previous chapters as background, we now turn to the problem of controlling | the folios anned of considerable interest to know whether there are any obstacles present in its path (obstacle constraint) and whether the manipulator hand must traverse a specified path (path constraint). These two constraints combined g) control modes, as tabulated i in Table 4.1. From this table, it is noted that the con- divided _into_two_coherent. subproblems—motion (or trajectory) planning and motion control. This chapter focuses attention on various trajectory planning schemes for obstacle-free motion. It also deals with the formalism of describing the desired manipulator motion as sequences of points in space (position and orientation of the manipulator) through which the manipulator must pass, as well as the space curve that it traverses. The space curve that the manipulator hand moves along from the initial location (posi- tion and orientetion) to the final location is called the path. We are interested in developing suitable formalisms for defining and describing the desired motions of the manipulator hand between the path endpoints.Table 4.1 Control modes of a manipulator constraint Obstacle constraint Yes Off-line collision-free. path planning plus ontine path tracking Positional controt plus on-line obstacle detection and avoidance No Off-line path planning plus online path tracking, Positional control‘Trajectory Pf). HG a Planes pam {pt B10). A. OO} |. ‘Maniputator's dynamics constraints Figure 4.1 Trajectory planner block diagram:GENERAL CONSIDERATIONS ON TRAJECTORY PLANNING. Trajectory planning can be conducted either in the joint-variable space or in tho cartesian space. For joint-variable space planning, the time history of all joint variables and their first two time derivatives are planned to describe the desired motion of the manipulator. For cartesian space planning, the time history of the manipulator hand’s position, velocity, and acceleration are planned, and the corresponding joint positions, velocities, and accelerations are derived from the hand information. Planning in the joint-variable space has three advantages: (1) the trajectory is planned directly in terms of the controlled variables during mation, (2) the trajectory planning can be done in near real time, and (3) the joint trajectories are easier to plan. The associated disadvantage is the difficulty in determining the locations of the various links and the hand during motion, a task that is usually required to guarantee obstacle avoidance along the trajectory.In general, the basic algorithm for generating joint trajectory set points is quite simple t= to; Joop: Wait for next control interval; feet An hh (2) = where the manipulator joint position should be at time 1; ft = ty, then exit; g0 to loop; where Af is the control sampling period for the manipulator, From the above algorithm, we see that the computation consists of a trajectory function (or trajectory planner) h{r) which must be updated in every control inter- val. Thus, four constraints are imposed on the planned trajectory. First, the tra- jectory set points must be readily calculable in a noniterative manner. Second, intermediate positions must be determined and specified deterministically. Third, the continuity of the joint position and its first two time derivatives must be guaranteed so that the planned joint trajectory is smooth. Finally, extraneous motions, such as “wandering,” must be minimized.The above four constraints on the planned trajectory will be satisfied if the ume histories of the joint variables can be specified by polynomial sequences. if the joint :rajectory for a given joint (say joint 1) uses p polynomials, then 3(p + 1) coefficients are required to specify initial and terminal conditions (joint position, velocity, and acceleration) and guarantee continuity of these variables at the polynomial boundaries. If an additional intermediate condition such as position is specificc, then an additional coefficient is required for cach intermediate condi- tion, In general, two intermediate positions may be specified: one near the initial ‘guarantee safe departure and approach directions, in addition to a better controlled (motion. Thus, one seventh-degree polynomial for cach joint variable connecting the initial and final positions would suffice, as would two quartic and one cubic (4-3-4) trajectory segments, two cubics and one quintic (3-5-3) trajectory segments, or five cubic (3-3-3-3-3) trajectory segments. This will be discussed further in the next section,For cartesian path control, the above algorithm can be madified to: cit AG ge to loop; Here, in addition to the computation of the manipulator hand trajectory function (2) at every coritrol interval, we need to convert the cartesian positions into their corresponding joint solutions, Q[H(t) 1.JOINT-INTERPOLATED TRAJECTORIES To servo a manipulator, it is required that its robot arm’s configuration at both the initial and final locations must be specified before the motion trajectory is planned. In planning a joint-interpotated motion trajectory for a robot arm, Paul [1972| shoyyod otlowis re - When picking up an object, the motion of the hand must be directed away from an object: otherwise the hand may crash into the supporting surface of the object. . If we specify a departure position (lift-off point) along the noriaal vector to the surface out from the initial position aud if we require the hand (Le., the origin of the hand evordinate frame} to pass through this position, we then have an admissible departure motion. If we further specify the time required to reach this position, we could then control the speed at which the abject is to be lifted. . The same set of liftoff requirements for the arm motion is also tue for the set-down point of the final position motion (Le., we must move to @ normal point out from the surface and then slow down to the final position) so that the correct approach direction can be obtained and controlled. . From the above, we have four positions for cach arm motion: initial, lif-off, set-down, and final (see Fig. 4.2). . Position constraints (q)itialpOSition: velocity and acceleration are given {normally zero). () Lae postin coninovs motion for intermediate points © same as lift-off position. (c)qRinaliposition: velocity and acceleration are given (normally zero).Figure 4.2 Position conditions for a joint trajectory.6. In addition to these constraints, the extrema of all the joint trajectories must be withia the physical and geomettic limits of each joint, 7. Time considerations {a) Initial and final trajectory segments: time is based on the rate of approach of the hand to and from the surface and is some fixed constant based on the characteristics of the joint motors. (®) Intermediate points or midtrajectory segment: time is based on maximum velocity and acceleration of the joints, and the maximum of these times is used (ie., the maximum time of the slowest joint is used for normaliza- tion).‘The constraints of a typical joint trajectory are tisted in Table 4.2, Based on these consiraints, we are concerned with selecting a class of polynomial functions of degree # or less such that the required joint position, velocity, and acceleration at these knot points (initial, lift-off, set-down, and final positions) are satisfied, and the joint position, velocity, and acceleration ate continuous on the entire. time inter- val {t), i]. One approach is to specify a seventh-degree polynomial for each joint i, A= at + all + as? + al + al + OP + mt + ay (43-1) where the unknown coefficients a, can be determined from the known positions and continuity conditions, However, the use of such a bigh-degree polynomial to interpolate the given knot points may not be satisfactory. It is difficult to find its extrema and it tends to have extrancous motion, AliiRIfShRGvS/EpproschliS/RISplie) the entire joint trajectory into several trajectory segments so that different interpo- lating polynomials of a lower degree can be used to interpolate in each trajectory€4.2 Constraints for planning joint-interpolated trajectory Initial position: 1. Position (given) 2. Velocity (given, normally zero) 3. Acceleration (given, normally zero) Intermediate positions: Lift-off position (given) Lift-o position (continuous with previous trajectory segment) Velocity (continuous with previous trajectory seyvent) Acceleration (continuous with previous trajectory segment) . Set-down position (given) jet-down position (continuous with next trajectory segment} Velocity (continuous with next rjectory segment) Acceleration (continuous with next trajectory segment) Final position: 12. Position (given) 13, Velocity (given, normally zero) Acceleration (given, normsify zero) segment. There are different ways a joint trajectory can be split, and each method possesses different properties. The most common methods are the following:3-4 Trajectory. Each joint has the following three trajectory segments: the first segment is a fourth-degree polynomial specifying the trajectory from the initial position to the liftoff position. The second trajectory segment (or midtrajec- tory segment) is a third-degree polynomial specifying the trajectory from the lift-off position to the set-down position. The last trajectory segment is a fourth-degrée polynomial specifying the trajectory from the set-down position to the final position. 38.3 Trajectory. Same as 4-3-4 trajectory, but uscs polynomials of different degrees for cach segment: a third-degree polynomial for the first segment, a fifth-degree polynomial for the second segment, and a third-degree polynomial for the last segment. 5-Cubie Trajectory. Cubic spline functions of third-degree polynomials for five trajectory segments are used. Note that the foregoing discussion is valid for each joint trajectory; that is, each joiat trajectory is split into cither a three-segment or a five-segment trajec~ tory. The number of polynomials for a 4-3-4 trajectory of an N-joint manipulator will have N jo:nt trajectories or N x 3 = 3N trajectory segments and 7N polyno- mial coefficients to evaluate plus the extrema of the 3N trajectory segments. We shall discuss the planning of a 4-3-4 joint trajectory and a S-cubic joint trajectory in the next section,43.1 Caleulation of a 43-4 Joint Trajectory, Since we are determining N joint trajectories in each trajectory segment, it is con- venient to introduce a normalized time variable, ¢ € (0, 1], which allows us to treat the equations of each trajectory segment for cach joint angle in the same way, with time varying from ¢ = 0 (initial time for all trajectory segments) to 2 = 1 (final time for all trajectory segments). Let us define the following variables: 1: normalized time variable, 1 € [0, 1} 1 in seconds real time at the end of the ith trajectory segment 4 = % — Tay: feal time required to travel through the ith segment to4 pe tS peta nh E01The trajectory consists of the polynomial sequences, h,(t), which together form the trajectory for joint j. The polynomial equatioas for euch joint variable in each tra~ jectory segment expressed in normalized time are: At) = ayt) + ap + ani + at + ayy (ist segment) (43-2) Ag(t) = a30 + ant + ant + ay (2nd segment) (43-3) and yt) = dal + ayat) + ayat? + dat + ayy Cast segment) — (4.3-4) The subseript of each polynomial equation indicates the segment number, and 1 indicates the last trajectory segment. The unknown coefficient aj indicates the ith coeflicient for the j trajectory segment of a joint trajectory. The boundary condi- tions that this set of jeint trajectory segment polynomials must satisfy are: 1, Initial position = 0 = 6(75) 2. Magnitude of initial velocity = vo (normally zero) @Magnitudeyofpnisialgzeeeleration ~ ay (normally zero) . Liftoff position = 8, = 8 (1) Continuity in position at f, [that ner) = 60F)] Continuity in WElociry at 1; [that is, v( wry tion at f, {that is, ar y= a(t )] = 9h) ty [that is, O(ts ) = OCs T at ty |that is, vty) = v(t) nici inetceeravion att [that is, a(t) = atty)] 65 = Ory) Cea AR13, Magnitude of final velocity ~ vy (normally zero) 14, Magnitude of final acceleration = a; (normally zero) ‘The boundary conditions for the 4-3-4 joint trajectory are shown in Fig, 4.3. ‘The first and second derivatives of these polynomial equations with respect to real time 7 can be written as: = A) AA de dhe) WO Ee ae = at 1 dace) , = =1, (43-5) 1 dr rm i 1,2, & )) and Pht) y(1) = a a(t) dn L =i) 1 = Le (43-6) i For ihe first trajectory segment, the governing polynomial equation is of the fourth degree: WO = au tan tap tant ae relO ip i437fay) = 8D) Bing) = Pir) = vy i) = I I! I 1 1 1 1 i ! 1 t ' ' t I 1 7 7 Bs “Ty ‘Real tne Figure 4.3 Boundary conditions for a 4-3-4 joint uajectory.From Eqs. (4.3-5) and (4.3-6), its first two time derivatives with respect to real time are Ay) danat) + Bay? + ant + an vO) = (4.3-8) 4 and RO) _ Pag? + bent + 2a. 439) a2) = H R 1. For ¢ = 0 (at the initial position of this trajectory segment), Satisfying the boundary conditions at this position leads to ayy = (0) = 0 (givea) (43-10)AO) f daygt? + Base + ant + ay; _ 4 L 4 hh which gives AF Yohy and ay = a = 2 (0) Lay? + bat + ay, _ 242 " moo which yields G34) (4.3-12)With these unknowns determined, Eq. (4.3-7) can be rewritten as: 3 aot) 2 in) = at tage + [SEL + Gnade + fm elt) 4343) 2, For # = 1 (at the final position of this trajectory segment). At this position, we relax the requirement that the interpolating polynomial must pass through the position exactly. We only require that the velocity and acceleration at this position haye to be continnous with the velocity and acceleration, respectively, at the begin- ning of the next trajectory segment, The velocity and acceleration at this position are: AG Aayg + 3a;3 + aot? + vor w(t) 2 y, = AED _ Maw + 3a tall vo yg aay th 4 ha 12 6 7 a(t) Aa, = AOU Bae + 6a + aot 3-45) e a For tite second trajectory segment, the governing polynomial equation is of the third degree: fn Qt) = ay? + ant + at tam te 0, I] (43-16)1. For t = 0 (at the lift-off position), Using qs. (4.3-5) and (4.36), the velocity and acceleration at this position are, respectively, fa) = ayy = 03(0) 43-17) Ay (0 Baz? + 2argt + y= BOL (=e | = 43.48 th % which gives a = uh and 6ay3t + 2a: a = = (Se (43-19) 7 which yieldsSince the velocity and acceleration at this position must be continuous with the velocity and acceleration at the end of the previous trajectory segment respectively, we have hg) AyD) (4.3-20) b qawhich, respectively, leads 0 _ f faut? + Bays + Qayt + an 7 +0 ( Bayt? + are + L fy 4 ist (43-21) or -4, Aa: 3a i os Ta Aaa Bis Molt oth g 43.22) 2 ts 7 t 1 and bast + 2a; Vay? + 6aygt + 2a fagt+ Qe | [ Pautl + San! + (4.3-23) G 10 a tetp+ = 0 (43-24) 2. For ¢ = 1 (at the set-down position), Again the velocity and acceleration at this position must be continuous with the velocity and acceleration at the beginning of the mext trajectory segment, ‘The velocity and acceleration at this position are obtained, respectively. as: fg (l) = a3 + ag, + ay + ap (4.3-25) fy (Ay ~ Bass? + ett + way = BO (=e 4326 7" " 3agy + 2ay + ayy ty and2 Sara + 2409 iy 3.99) 1 iQ) (Gast + 2am _ eQ) = = L 8 tal g For the lost trajectory segment, the governing polynomial equation is of the ee[O, 1) (4.3-28) fourth degree: Figt) = Gait! + agg + aygi® + ait + ay — 1 inte ¢ in the above equation, we have shifted the Jf we substitute F = in normalized time f from 7 € [0, 1] to 7@[-1, 0], Then Eq, (4.3-28) becomes fel-1,0] (4329 IAD = Ggat" + al + Gel? + ayt + ago Using Eqs. (4.3-5) and (4.3-6), its first and second derivatives with respect to real time are 40 = and (4.3-31) a, (2)1. For 1 = 0 (at the final position of this segment). Satisfying the boundary conditions at this final position of the trajectory, we have 40h ig which gives Bis = Vey and (4,3-32) (43-33) (4.3-34)which yiekis 2. For? = —1 (at the starting position of this trajectory segment). Satisfying the boundary conditions at this position, we have, at the set-down position, ate LF et, + Oy == Oo( 1) (43-35) Bgl 1) = Oy = ety + and if [ete te tn —4atqy + Begs — Gyhe + Vey nT (43-36) " ant? + baal + 2dr > ig 2 = a See + ayfz asap tyThe velocity and acceleration continuity conditions at this set-down point are Fo) Al we and {4,3-38) ey or (4.3-40)The difference of joint angles between successive trajectory segments can be found to be a By = By (LY = (0) = aye ays + SE + ag 4.341) 4) = hz (1) — fy (0) = aay + ay + ay (43-42) and 8: = hy(0) hel == Oey ha “LE tye BD All the unknown coeflicients of the trajectory polynomial equations can be determined by simultaneously solving Eqs. (4.3-41), (4.3-22), (4.3-24), (4.3-42),(43-39), (4.3-40}, and (4.3-43). Rewriting them in matrix vector notation, we have y = Cx (4.3-44) where + 744 ~ Yor ~ Gas b25 (43-45) at ' Api + pe dys By SE Ye 1 bt 9 0 9 0 0 Bt Ait mip 0 0 0 0 6} 12a? 0 -23 0 9 0 c=) 0 oO 1 1 1 9 0 (4.3-46) 0 0 Wy Wh hy = 3, AM, 0 0 @ 2? 6d Git? —1272 0 0 8 9 oO 1 =1 and X(T Mas Ange ay» 23 6 ys, Ga)” (4.3-47)‘Then the planning of the joiat trajectory (for each joint) reduces to solving the matrix vector equation in Eq. (4.3-44): 7 yy = Deysy (43-48) in or x= Cy (43-49) ‘The structure of matrix C makes it easy to compute the unknown coefficients and the inverse of C always exists if the time intervals f;, i = 1,2, 1 are positive values. Solving Eq. (4.349), we obtain all the coefficients for the polynomial equations for the joint trajectory segments for joint j, Since we made a change in normalized time to run from [0. 1] to [-1, O] for the last trajectory segment, after obtaining the coefficients a,, from the abovematrix equation, we need to seconvert the normalized sime back 10 [0, 1], This can be accomplished by substituting 1 = f+ 1 into f in Eq. (4.3-29). Thus we obtain ligt) = Gat? + (—4dye + Gg + (6a4 — Bag + 4,2)? + (4agg + Bayy ~ Qty + Oy t+ (ayy yx Hy tay + BO) re [0,1] (4,3-50) The resulting polynomial equations for the 4-3-4 trajectory, obtained by solving the above matrix equation, are listed in Table 4.3, Similarly, we can apply this tech- nique to compute a 3-5-3 joint tajectory. This is left as an exercise to the reader. The polynomial equations for a 3-5-3 joint trajectory are listed in Table 4.4.