RSD-TR-13-85 MINIMUM-TIME TRAJECTORY PLANNING FOR INDUSTRIAL ROBOTS WITH GENERAL TORQUE CONSTRAINTS1 Kang G. Shin Neil D. McKay Department of Electrical Engineering and Computer Science The University of Michigan Ann Arbor, Michigan 48109 October 19865 CENTER FOR RESEARCH ON INTEGRATED MANUFACTURING Robot Systems Division COLLEGE OF ENGINEERING THE UNIVERSITY OF MICHIGAN ANN ARBOR, MICHIGAN 48109-1109 'This w ort wo sspported in part bynt No. the NUS AFOSR Contract No. F49620 82-G-0089. Any opinions, findings, and conclusions or recommendations expressed in this publication are those of the author and do not aecessarily reflect the views of the funding agencies.

RSD-TR-13-85 TABLE OF CONTENTS 1. INTRODUCTION............................................................................... 2 2. PROBLEM STATEMENT..................................................... 4 3. PERTURBATION TRAJECTORY IMPROVEMENT....................... 7 4. COMPUTATIONAL REQUIREMENTS AND CONVERGENCE oeec *- e............ ee e —............... e..e e e e........eeee e e e eee eeeee. e.e ee 13 5. NUMERICAL EXAMPLES.............................................. 15 6. CONCLUSION............................................ 17 U

RSD-TR-13-85 ABSTRACT There are a number of trajectory planning algorithms which generate the joint torques/forces required to drive a robot along a given geometric path in minimum or near-minimum time [2, 4, 6, 7, 8, 11]. These methods make fairly specific assumptions about the form of the joint torque/force constraints, thereby limiting their applicability. A method, called the perturbation trajectory improvement algorithm (PTIA), is developed here which can generate the joint positions, velocities, and torques required to move a robot along a specified geometric path in minimum time under very general torque constraints. The PTIA starts with a non-optimal trajectory which meets all the required torque constraints, and perturbs the trajectory in such a way as to always decrease the traversal time for the path. This perturbation process continues until the torque constraints prevent any further improvement in the traversal time. The torque constraints may be expressed in terms of quantities related to torque rather than torque itself; it is possible, for example, to limit velocity, acceleration, jerk, and motor voltage, either singly or in combination. The perturbation trajectory planner also is very simple to implement, and many of the calculations are independent of each other and can therefore be done in parallel. As a demonstrative example, the PTIA is applied to the first three joints of the Bendix PACS Arm. Minimum-Time Trajectory Planning 1

RSD-TR-1 S-85 1. INTRODUCTION An important goal of contemporary industrial automation with robots is to increase productivity. One way of accomplishing this goal is to move robots as fast as possible, i.e., minimum-time control of robots. However, due to the nonlinearity and joint couplings in the robot dynamics, it is possible to obtain only approximate solutions to the problem of direct control of robots in minimum time [3, 5]. To circumvent this difficulty, the control of robots is usually divided into two sequential problems: off-line trajectory planning followed by on-line trajectory tracking. We shall address in this paper the minimum-time trajectory planning problem. A number of techniques have been developed for planning minimum-time trajectories of industrial robots [2, 4, 6, 7, 8, 11]. One potential problem with these methods is the assumption that constraints on the torques/forces applied to the robot's joints have specific forms, thereby limiting their applicability. For example, the method described in [7] assumes that the path consists of a sequence of Cartesian straight line segments, and that constant limits on Cartesian velocity and acceleration are known a priori along each path segment. Note that it is almost impossible to select such limits without knowing the dynamic properties and the actuator characteristics of the robot. Moreover, since maximum accelerations and velocities are assumed to be constant over some interval, it is necessary to choose them to be the worst case bounds thereby resulting Minimum-Time Trajectory Planning 2

RSD-TR-13-85 in under-utilization of the robot's capabilities. To alleviate this problem, we have developed a method in [4] which uses the robot dynamics to obtain approximate velocity and acceleration bounds at each corner or knot point of the robot's path. More dramatic improvements can be found in [2, 11, 12]. The trajectory planner presented in [2] assumes that joint torque limits are given in terms of the joint's position and velocity, and that the joint torque limits are mutually independent. (Note that the joint actuator limits are dependent on each other when more than one joint actuator share a common power source, e.g., a power supply for DC servo motors and a pump for hydraulic actuators.) A similar trajectory planner described in [11] makes the additional assumption that the velocity dependence of the torque constraints is at most quadratic. Torque constraints which interact, as well as performance indices other than minimum time, can be handled by the trajectory planner described in [121, but the velocity must be discretized rather coarsely if the computations are to be performed in a reasonable amount of time. In practice, torque limits may be relatively complicated, and in fact there may be limits on the derivatives of the torques because of the presence of large motor inductances or because of the compressibility of the hydraulic fluid used in the robot's actuators. It is the goal of this paper to present a minimum-time trajectory planner which is simple and also allows such complicated torque constraints. This trajectory planner begins with an arbitrary trajectory meeting all the constraints and always alters the trajectory so as to reduce the traversal time 3 Minimum-Time Trajectory Planning

RSD-TR-13-85 without violating any constraints until a satisfactory solution is obtained. This planner is an iterative algorithm and is called the Perturbation Trajectory Improvement Algorithm (PTIA). The method is very easy to use and implement yet very powerful. Also, it is quite different from any known trajectory planners. The remainder of the paper is divided into four sections. We state the minimum-time trajectory planning problem in the next section. Section 3 describes the PTIA, and gives a simple example of its application. Section 4 analyzes the amount of computation required by the PTIA. In Section 5, the algorithm is applied to the first three joints of a real robot, and some complications regarding the application of jerk constraints are discussed. Section 6 concludes the paper. 2. PROBLEM STATEMENT The approach to trajectory planning described here assumes, as do the trajectory planners described in [2, 11, 12], that the geometric path to be followed by the robot is given as a parameterized curve in joint space, i.e., the joint positions q' are given by qi = f (X), 0 << X Xmax, I< < n (1) where X is a scalar parameter used to specify geometric paths and n the number of joints that the robot has. It should be noted that in practice the geometric paths are given in Cartesian space. While it is in general difficult to convert a Minimum-Time Trajectory Planning 4

RSD-TR-13-86 curve in Cartesian space to that in joint space, it is relatively easy to perform conversion for individual points. One can then pick a sufficiently large number of points on the Cartesian path, convert to points in joint space, and use some sort of interpolation technique (e.g. cubic splines or straight line segments) to obtain a similar path in joint space (see [6] for an example). The resulting joint path is assumed to have the form of Eq. (1). As in Eq. (1), the parameter X is sufficient to represent all joint positions, dX and hence referred to as a position variable. The speed p -d of the manipulator may be plotted versus the position X. The speeds and positions of the robot's joints can then be found from the values of X and p and the parametric functions f'. It is well-known that the dynamics of a robot take the general form ii = J;i + [jk,i]qiq + Ri,,q + g, (2) where u, is the i't generalized force, qi is the ith generalized coordinate, Jji is.1 J. 8 Jk a J the inertia matrix, the symbol [jk,iJ- — i 2 + -- + - is a Chrisi [ * l ii? a ' a~i ]l toffel symbol of the first kind and represents an array of Coriolis coefficients, Rii is the viscous friction matrix, and g, is the gravitational torque vector. By plugging the parametric functions given in Eq. (1) into these dynamic equations (2), joint torques/forces can be calculated in terms of X, A, f ' and their derivatives as follows (see [11] for a detailed derivation). 6 Minimum-Time Trajectory Planning

RSD-TR-13-85 X = i (3a) df'* 2 d~f ui =Jj (X) df i +J;j(X) Pd 2 udX dX2 +Ci (X),ddf/ dfk2 df + Ck (X) d 2 + Ri d- + g; (X). (3b) The controller inputs (e.g., voltages or currents) are related to the applied torques/forces, so that control input constraints can also be calculated in terms of these quantities, i.e., X, p, f ' and their derivatives. Also assume that joint torque constraints are expressed in terms of joint positions and velocities, u E E(q,q) = E(X,p) (4a) and the jerk constraints in terms of positions, velocities and accelerations, i ui I -- | F(q,q,q) | -- | F(X,#) < K; (4b) where E: R' XR' - RX is an admissible input function, F: R" XR" X R - R is a jerk function, and Ki a constant. It is further assumed that one phase trajectory (p vs. X plot) can be found which meets all constraints. (In practice, a trajectory with zero velocity usually suffices.) This trajectory can then be perturbed to find the one with the shortest traversal time. This perturbation process is made particularly simple by the fact that minimizing time is equivalent to maximizing velocity, i.e., Minimum-Time Trajectory Planning 6

RSD-TR-13-85 *l Xm dX T- 'dt = I d(5) subject to the torque constraints (3a), (3b), (4a) and (4b), where X(tf )=Xmax is the final position. Here t0 and tf denote the initial and final times, where tf is left free. The minimum-time trajectory planning problem is then to maximize p, so the phase trajectory should always be pushed upward without violating the torque constraints. 3. PERTURBATION TRAJECTORY IMPROVEMENT ALGORITHM In practice, a trajectory planner must deal with a variety of arbitrary parametric curves; two representations for curves which immediately suggest themselves are splines and simple sequences of (interpolation) points. We choose to use the latter representation, i.e., the curve (1) is represented as an ordered sequence of points (X(k),q(k)); this proves to be the most natural representation for the application of the PTIA. The trajectory planning process consists of assigning values of the "velocity" p and "acceleration" p at each point. For the sake of simplicity, consider only those constraints which can be expressed in terms of position- and velocity-dependent bounds on the torque (as in Eq. (4a)), i.e., ignore jerk constraints for the time being. Then all constraints can ultimately be given as Xand p-dependent constraints on ', or equivalently constraints on as shown in [11]. In terms of the (X,p#) plot, each point is assigned a set of allowable 7 Minimum-Time Trajectory Planning

RSD-TR-13-C5 slopes. In fact, having the torque constraint (4a) is equivalent to assigning a pair of vectors to each point in the X - p phase plane. One vector represents the maximum possible slope when the system is accelerating (i.e. p is maximized) and the other represents the slope for deceleration(i.e. p is minimized). This pair of vectors looks like a pair of scissors, and as the position in the phase plane changes, the angles of both the upper and lower jaws of the pair of scissors change. The phase trajectories must, at every point of the phase plane, point in a direction which lies between the jaws of the scissors. At particular points of the phase plane, though, the jaws of the scissors close completely, allowing only a single value for the slope. At other points the scissors may try to go past the closed position, allowing no trajectory at all. This phenomenon determines the admissible region of the phase plane. Note that the boundary of the admissible region passes through those points which have only a single vector associated with them, corresponding to those states where only a single acceleration value is permitted. In the discrete approximation, having the maximum and minimum slope sets limits on the differences between the values of p at adjacent interpolation points. The process of trajectory planning requires that the initial and final points of the curve have zero velocity (or some other fixed velocity) and that the velocities at all the intermediate points be as large as possible, consistent with the slope constraint that the velocities at neighboring points not differ too much. Minimum-Time Trajectory Planning 8

RSD-TR-13-85 One approach to the solution of this problem is to try to push the speed higher at each individual point. The value of p can be pushed higher at each point in succession until none of the velocities can be made any larger. If we call this Algorithm A, then we have Algorithm A: Al. Set all velocities to values which are realizable (usually all zeroes). A2. Push each intermediate point of the curve as high as possible consistent with the slope constraints. A3. If any of the velocities were changed in step A2, go back to step A2, otherwise exit. As a practical matter, the search required to find the highest possible velocity in step A2 of Algorithm A may be fairly expensive, especially since it may be repeated many times for a single point. A simpler approach is to just try adding a particular increment to each velocity, and then make the increment smaller on successive passes of the algorithm. This gives Algorithm A': Al'. Set all velocities to values which are realizable (usually all zeroes). A2'. Set the current increment to some large value. A3. Push each intermediate point of the curve up by an amount equal to the current increment, if this is consistent with the slope constraints. 9 Minimum-Time Trajectory Planning

RSD-TR-13-85 A4'. If any of the velocities were changed in step A3', go back to step A3'. A5'. If the current increment is smaller than the desired tolerance, stop. Otherwise halve the increment and go to A3'. Algorithm A' is really just a combination of gradient and binary search techniques. The direction in which the curve must move (i.e. the gradient direction) is known a priori, since increasing the velocity always decreases the traversal time, and the amount of the change is successively halved, as in a binary search, until some desired accuracy is achieved. Clearly, this algorithm will terminate in a finite number of steps. Algorithm A' is very simple, except possibly for the slope constraint check required in step A3'. This requires a knowledge of the dynamics and actuator characteristics of the robot. Note that as discussed earlier inadmissible regions are determined by the slope constraints. However, this check is a simple "go/no go" check, and can be isolated as a single function call. (Hereafter this function will be called the constraint function.) Hence the trajectory planner can be used with other robots by changing a single, though possibly complicated, function. Another important characteristic of the constraint function is locality. In the case discussed above, the constraints are expressed in terms of X, p, and.We need two points to determine the slope d, so the constraint depends dX d' only upon two points. Therefore when a point of the curve has its p value changed, it is constrained only by the two adjacent points (due to the slope constraints); the rest of the curve has no influence. This allows much calculation to Minimum-Time Trajectory Planning 10

RSD-TR-13-85 proceed in parallel. Step A3' of Algorithm A' can be divided into two sequential steps, one which increments the odd numbered points and one which increments the even numbered ones.l Since the even numbered points stay the same while the odd numbered ones are being incremented, and vice versa, the points either side of the incremented points remain stationary, so that the constraint checks are valid. (If all points were tested simultaneously, then it is possible, for example, to increment two adjacent points; since in each case the constraint check would be made on the assumption that the other point was remaining stationary, it is possible that the new configuration would not meet the required constraints.) It is easily seen that the process in Algorithm A' can be extended to more complicated constraints. For example, constraints on the jerk (the derivative of the torque or acceleration) only require a more complicated constraint function, i.e., both Eqs. (4a) and (4b). Of course in this case the constraint function needs three points to calculate second derivatives of the speed. Thus the constraints on a single point will be functions of two points either side of the point being checked, rather than one point. This affects the degree of parallelism which can be achieved; step A3' would require three passes instead of two. It also affects the convergence properties of the algorithm, as will be seen later in the examples of Section 5. As a simple illustration of how the algorithm works, consider a simple onedimensional problem. Suppose we wish to move an object of mass m from z =0 1Thus, step A3' requires two passes. 11 Minimum-Time Trajectory Planning

RSD-TR-13-85 to x =4. Further, suppose that there is no friction, and that there are constant bounds on the magnitude of the applied force. There will be only one parametric function f, which may be taken to be the identity function, so that X\=. We then have d2z d2X dp dp dX dp/ F =m =m =m- =m --- =mp-. (6) dt2 dt2 dt dX dt d X If we consider X-intervals of length 1, then the discrete approximation to the parameterized "curve" will have 5 points. The acceleration - = - d can be dX approximated as 2 2 dp i +1 -- i i+l+I*,i _i+I - Pi P+l - - -- " p ~- ~ ~^ ^ - =. - (7) dX\ Xi+ - Xi 2 Xi+I-X1 2(X,-+- (X The torque constraints then become 2 2 Fmax L F =m II =m 2(+1- X. (8) If we use m = 1, Fm = 2, and X;+i - Xi.= I for all i, this reduces to I,+ _ 2 < 4. (9) Now consider what happens if Algorithm A is applied. We may look at the intermediate points of the curve in sequence. First, point 1 can be raised by 2, since the adjacent points have p values of zero (i.e. po=sp2=O), and 1 22 - 02 1 = 4. Raising the middle point, point 2, we are constrained by the Minimum-Time Trajectory Planning 12

RSD-TR-13485 fact that p3 = 0, which limits p2 to 2 also. Likewise, we may change P3 to 2. This completes step 2 of Algorithm A. Since some of the p values changed, we try to increase them again. This time only point 2 can be raised, giving a value of P2 = 2v'2. On the next pass, no ps values change, so Algorithm A terminates. It is easily verified that the solution obtained from Algorithm A is indeed the optimal solution to the discretized problem. (Figure 1 shows the discretized trad jectory after passes zero, one, and two of Algorithm A.) Now look at what happens when we use Algorithm At. Say we start with an increment of 2. Then the result of the first pass of Algorithm A' is the same as the result of the first pass of Algorithm A, namely pi = |P2 = P3 = 2. If the increment is cut to 1, then there is no change. Cutting the increment to 1/ 2, we may raise the middle point to 2.5. Continuing in this fashion, the middle point gets closer and closer to 2/, the correct result. (Figure 2 shows the trajectory after passes zero, one, three, and four of Algorithm A'.) 4. COMPUTATIONAL REQUIREMENTS AND CONVERGENCE The PTIA, unlike dynamic programming [12], requires relatively little memory; it requires only one floating point number per interpolation point. However, computation of the CPU time requirements is interesting. Obviously, the computation time must increase at least linearly with the number of interpolation points on the curve, that is, the size of X intervals. In fact, the time increases as the square of the number of interpolation points. To see why this is so, consider what happens when the number of interpolation 13 Minimum-Time Trajectory Planning

RSD-TR-13-85 points is doubled. Since there are twice as many points to check on each pass of the algorithm, the computation time must increase by a factor of two. Recalling that the torque constraints translate into slope constraints, it is clear that the ratio of the amount by which a p-value may be raised to the distance between X-values will be approximately constant. Therefore halving the spacing of the interpolation points halves the size of the steps which can be taken in the p direction, thus doubling the number of steps. This factor of two times the factor of two which results directly from doubling the number of points gives a factor of four increase in computation time. If doubling the number of interpolation points quadruples the computation time, then the time dependence is quadratic in the number of points, i.e., 0 (N2) where Nx is the number of the X intervals. It is obvious from the discussion that the fineness of the X intervals will have a significant impact on the running time of the algorithm. It will also affect the accuracy of the results. Similarly to the convergence proof of the trajectory planning with dynamic programming in an extended version2 of [12] or in [9], we can treat the effect of the grid density on the accuracy of our solution in a quantitative manner. Bellman proved in [1] that discrete approximations to a continuous optimal control problem will converge as the step size of the dynamic programming stage variable decreases. However, the class of systems to which Bellman's proof applies does not cover those considered in this paper. In particular, Bellman 2This version is currently submitted elsewhere for publication. Minimum-Time Trajectory Planning 14

RSD-TR-13-85 assumes that the dynamic equations of the system are not functions of the stage variable, which is the same as X in this paper. We have proven a theorem in an extended version of [12] and [91 which is an extension of that of Bellman in that it allows the dynamic equation and performance index to be (possibly discontinuous) functions of the stage variable. A similar approach may be applied to the proof of the convergence of our trajectory planning method discussed in this paper. Thus, it is not repeated here. 5. NUMERICAL EXAMPLES For comparison purposes, we will use the same example robot considered in [12] and [10]. This robot is the Bendix PACS arm, which is cylindrical in configuration and is driven by fixed-field DC motors. (We consider only the first three joints.) The dynamics and actuator characteristics of this robot are given in [10]. First we consider only constraints on joint torques/forces and motor voltages, without considering constraints on their derivatives. The torque and voltage constraints are given in Table 1. The perturbation trajectory planner was written in the C programming language, and run on a VAX-11/780 under the Unix operating system. The planner was tried with a straight-line path, a geodesic in "inertia space" (see [10]), and a joint interpolated path. (The jointinterpolated path has the form qi = q, + p (ql - qi), where 0 < p < 1 and q: and q are the points at which the curve starts and finishes.) The traversal times for these paths are 1.79 seconds, 1.59 seconds and 1.80 seconds respec16 Minimum-Time Trajectory Planning

RSD-TR-13-85 tively. Plots of p vs. X, joint positions vs. time, and motor voltage vs. time are shown in Figures 3a through 5c. The traversal times and the various plots are virtually identical to the solutions obtained in [10]. To demonstrate the application of the perturbation technique to problems in which there are constraints on the derivatives of the torques, i.e., jerk constraints (4b), we consider the same problem with the additional constraint that the time derivatives of the joint torques and forces be less than 100, i.e., K;=100 for all 1<S<n. The time derivatives of the torques are computed using the identity du, dui dX dui Cc~-T~~~~~~~~ = -1 Z =(10) dt dX dt dX ' dei The derivative d was estimated by calculating the difference between the dXA applied torques on successive intervals and dividing by the average of the lengths of the intervals. For a straight-line path with 25 interpolation points, the traversal time is 2.04 seconds. The p vs. X plot is shown in Figure 6. For 50 points, the traversal time is 2.26 seconds; the phase plane plot is shown in Figure 7. Note that the trajectory has a "bump" in it; the process has not converged to the proper solution. To understand why this happens, consider the situation shown in Figure 8. The solid line shows the current trajectory, and the dashed lines show what happens when either of the two interior points is raised. In either case, a jerk limit is exceeded, even though the jerk constraint would very possibly be met if both points were raised simultaneously. Neither point can Minimum-Time Trajectory Planning 16

RSD-TR-13-85 move before the other does, resulting in a sort of "deadlock". Similar situations can occur with longer sequences of points. If jerk constraints are to be included, then obviously we must prevent this sort of situation from occurring. One fix which seems to work in practice is to perform the trajectory planning operation several times with some added constraints, relaxing the constraints each time the trajectory is "improved". The constraints used here were simple velocity limits. On each pass, the velocity limit is raised. If the velocity increment is small enough, the top of the phase trajectory remains flat, and the regions of high inflection which cause the anomalies in the phase trajectory never get a chance to appear. With this modification, a velocity increment of 0.1 at each pass gives the results plotted in Figures Oa through ec for a straight line with 50 points. 100 points and a velocity increment of 0.025 gives the results plotted in Figures 10a through lOc. The calculated traversal times are 2.03 seconds in both cases. 6. CONCLUSION A minimum-time trajectory planning scheme has been presented which (i) is extremely simple for actual implementation, and (ii) allows the use of very general types of torque constraints. This trajectory planner has been shown to give the same results as the trajectory planner described in [111 for the paths and torque constraints given in [10]. In addition, this scheme has been applied to a case where there are limits on jerk as well as torque. 17 Minimum-Time Trajectory Planning

RSD-TR-13-85 Since the PTIA is simple but general enough to accommodate various realistic constraints, it has high potential use for automatically generating trajectories of the growing number of industrial robots. REFERENCES [1] R. Bellman, "Functional Equations in the Theory of Dynamic Programming - VI, A direct Convergence Proof," Annals of Mathematics, vo. 65, no. 2, pp. 215-223, March 1957. [2] J. E. Bobrow, S. Dubowsky, and J. S. Gibson, "On the Optimal Control of Robotic Manipulators with Actuator Constraints," Proceedings of the 1983 Automatic Control Conference, [3] M. E. Kahn and B. Roth, "The Near-Minimum-Time Control of Open-Loop Articulated Kinematic Chains," ASME Journal of Dynamic Systems, Measurement, and Control, pp. 164-172, September 1971. [4] B. K. Kim and K. G. Shin, "Minimum-time path planning for robot arms and their dynamics," IEEE Trans. System, Man, and Cybernetics, vol. SMC-15, no. 2, pp. 213-223, March/April 1985. [5] B. K. Kim and K. G. Shin, "Suboptimal Control of Industrial Manipulators with a Weighted Minimum Time-Fuel Criterion," IEEE Trans. on Automatic Control, vol. AC-30, no. 1, pp. 1-10, January 1985. Minimum-Time Trajectory Planning 18

RSD-TR-13-85 [6] C.-S. Lin, P.-R. Chang, and J. Y. S. Luh, "Formulation and Optimization of Cubic Polynomial Joint Trajectories for Mechanical Manipulators," IEEE Trans. on Automatic Control, vo. AC-28, no. 12, pp. 1066-1074, December 1983. [7] J. Y. S. Luh and C. S. Lin, "Optimum Path Planning for Mechanical Manipulators," ASME Journal of Dynamic Systems, Measurement, and Control, vol. 102, pp. 142-151, June 1981. [8] J. Y. S. Luh and M. W. Walker, "Minimum-Time Along the Path for a Mechanical Arm," Proceedings of the IEEE Conference on Decision and Control, pp. 755-759, December 1977. [9] N. D. McKay, "Minimum-Cost Control of Robotic Manipulators with Geometric Path Constraints," Ph.D. thesis, The University of Michigan, September 1985. [10] K. G. Shin and N. D. McKay, "Selection of Near Minimum-Time Geometric Paths for Robotic Manipulators," Proceedings of the 1985 ACC, pp. 346-355, June 1985. (Also to appear in IEE Trans. on Automatic Control). [11] K. G. Shin and N. D. McKay, "Minimum-Time Control of a Robotic Manipulator with Geometric Path Constraints," IEEE Transactions on Automatic Control, vol. AC-30, no. 6, pp. 531-541, June 1985. 19 Minimum-Time Trajectory Planning

RSD-TR-13-85 [12] K. G. Shin and N. D. McKay, "Robot Path Planning Using Dynamic Programming," Proceedings of the 23rd CDC, pp. 1629-1635, Dec. 1984. Minimum-Time Trajectory Planning 20

RSD-TR-13-85 Parameter Descritio Value r/i"' Saturation torque of O motor 2.0 Nt.-M. r/'t Saturation torque of r motor 0.05 Nt.-M. r7/t Saturation torque of: motor 2.0 Nt.oM. VfAp Lower voltage limit for 0 joint 40 v. V," Lower voltage limit for r joint -40 v. V, __Lower voltage limit for z joint -40 v. Vr" Upper voltage limit for 0 joint 40 v. Vma Upper voltage limit for r joint 40 v. V, Upper voltage limit for z oint 40 v. kI Gear ratio for 0 drive 0.01176 k9 Gear ratio for r drive 0.00318 Meters/radian Pk/_ Gear ratio for z drive 0.00318 Meters/radian k" Motor constant for 0 joint 0.0397 Nt.-M./amp k*i 1Motor constant for r joint 0.79557 X 103 Nt-M./amp kz ___ Motor constant for z joint 0.0397 Nt.M./amp R o' Motor and power supply resistance, 0 joint 1 n RMt Motor and power supply resistance, r joint 1 2 R_ _ Motor and power supply resistance, z joint I f kl Friction coefficient of 0 joint 8.0 Kg./sec. k, Friction coefficient of r joint 4.0 Kg./sec. k, __ Friction coefficient of: joint 1.0 Kg./sec. AlM Mass of r joint 10.0 Kg. M, Mass of z joint 40.0 Kg. Jt Moment of inertia around 9 axis 12.3183 Kg.-Af. K' MMoment of inertia offset term I3.0 Kg.-M. Table 1. Dynamic coefficients and actuator characteristics for PACS arm. 21 Minimum-Time Trajectory Planning

RSD-TR-13-85 p 2.000 ~ --- —-e- ------,, %, %, % O S, % 1.000::, - - 1.000 2.000 3.000 4.000 Figure 1. Results of Algorithm A. Minimum-Time Trajectory Planning 22

RSD-TR-13-86 t % 0 % I- M 0 % 23 Mlnimum-Tlme Trajector7 Planning 1.000 m 23 MeTcP FIgre a eat ~AgrtlaP 23~~~~~~~~ Stin-~eTajcoyPsn

RSD-TR-13-85 A* 10 8.000 6.000 4.000 2.000 \ a. A 2.000 4.000 6.000 Figure 3a. Phase plane plot for straight line. Minimum-Time Trajectory Planning 24

RSD.TR.13s8 q 1.000 0750 25.e 0. 5 0M0 T". P ".% 0.250- 41 — -0 250 0 -0 500 j \ -070 I250 ee oe -1.0.500 -D550 3 2505 Te Trajectory Plannig

RSD-TR.13.85 V 40 I I f Figuer 3c Motor voltage ts. time ror traight line MininiumTime TraJector Planning ~~~~~~~~~~20 o~ 26 '""' ---- ] '000 <.500 Flo Ig ure 3 i -,:r.:~ -40~~~~~~~ De~ Figure~~ So oo otg s iefrsrih ie Minimum-Time~ TrjcoyPann

RSD-TR-13-85 A 10 8.000 6.000 4.000 2.000 I II.,.A 2.000 4.000 6.000 Figure 4a. Phase plane plot for geodesic. 27 Minimum-Time Trajectory Planning

RSD-TR-13-85 q 1.000 0.750 1... ^ 0.500 0.500 100.0 \ 1.500 -0.250 -0.500 ~~~~ I \-0.750 % -1.000 Figure 4b. Joint position vs. time for geodesic. Minimum-Time Trajectory Planning 28

RSD-TR-i3.86 40 029 29 c,~M UaXi-M-UT e TrpJec.l r Plan l,._ ll~e......... 1.000~~~~~ ' %e i~~~~ Minimu. Tm Taetoypl_.

RSD-TR- 3-85 8.000 6.000 4.000 2.000 2.000 4.000 6.000 8.000 10 Figure 5a. Phase plane plot for joint-interpolated curve. Minimum-Time Trajectory Planning 30

RSD-TR-13-85 q 1.000 0.750 ". 0.500 a0 -0.250 | -0.500 -0 50 ". -0 075;) 0 ""., ~ -1.000 Figure 5b. Joint position vs. time for joint-interpolated curve. 31 Minimum-Time Trajectory Planning

RSD-TR -13-85 V 40 20 ~~~~~~~~201 1. ~~~~~~~~~~ 0.500 1.000 1.50(,~,.Io -20 —' -40 -- --- Figure 5c. Motor voltage vs. time for joint-interpolated curve. Minimum-Time Trajectory Planning 32

RSD-TR.-13-85 6.000 5 000 4.000 3.000 2.000 1.000 2.000 4.000 6.000 Figure 6. Phase plane plot for straight line. 25 Interpolation points, with jerk constraints. 33 Minimum-Time Trajectory Planning

RSD-TR-13-85 6.000 5.003 4.003 3.003 2.000 2.000 4.000 6.000 Figure 7. Phase plane plot for straight line, 50 interpolation points, with Jerk constraints. Minimum-Time Trajectory Planning 34

RSD-TR-13-85 o 2 3 Figure 8. Illustration of "deadlock" which may occur when jerk constraints are applied. 35 Minimum-Time Trajectory Planning

RSD-TR-13-85 6.000 5.000 4.000 3.000 2.000 1.000 2.000 4.000 6.000 Figure Oa. Phase plane plot for straight line. 50 Interpolation points, velocity increment 0.1. Minimum-Time Trajectory Planning 38

RSD-TR-13-85 q 1.000 0.750... -... 0.500 '-....... ~, _ le kAt~~~~~~~~t 0.500 1.000 1500 2.000 9. C. 0.250 -0.500 0 -0.750 -1.000 Figure Ob. Position vs. time for straight line. 50 Interpolation points, velocity increment 0.1. 37 Minimum-Time Trajectory Planning

RSD-TR-13-85 V 40 20 0.500 1.000 1.500 2.0 o / \/ -40 Figure 9c. Motor voltage vs. time for straight line. 60 Interpolation points, velocity increment 0.1. Minimum-Time Trajectory Planning 38 Minimum-Time Trajectory Planning 38

RSD-TR-13-85 6.000 5.000 4.000 3.000 2.000 1.000 2.000 4.000 6.000 Figure 10a. Phase plane plot for straight line. 100 Interpolation points, velocity increment 0.025. 30 Minimum-Time Trajectory Planning

RSD-TR-13-085 q 1.000 0.750T........ 0.250.." I * t -0..503 -0.750 ''***-..... -1.000 Figure 10b. Position vs. time for straight line. 100 Interpolation points velocity increment 0.025. Minimum-Time Trajectory Planning 40

RSD-TR-13-s 5 IliIIj/ A V 40 I) Z *._ * e - _ s - o oqeo o~ t ~ eso_ * eOo-o- wO4* _4 o 20 -20 ico 2.0 0 '....e I nn \ Z.o ^^ PO ^ Y;^^^0^ 100 e/ latlon points, velocity Increment 0.025.p 41 Miniu.Time Trajectory Planning