RSD-TR-14-85 AUTOMATIC GENERATION OF TRAJECTORY PLANNERS FOR INDUSTRIAL ROBOTS1 Kang G. Shin Neil D. McKay Department of Electrical Engineering and Computer Science The University of Michigan Ann Arbor, Michigan 48109 October 1985 -CENTER FOR RESEARCH ON INTEGRATED MANUFACTURING Robot Systems Division COLLEGE OF ENGINEERING THE UNIVERSITY OF MICHIGAN ANN ARBOR, MICHIGAN 48109-1109 1This work was supported in part by the NSF Grant No. ECS-8409938 and the US Airforce Contract No. F49620 -82-C-0089. Any opinions, findings, and conclusions or recommendations expressed in this publication are those of the author and do not necessarily reflect the views of the funding agency.

TABLE OF CONTENTS 1. INTRODUCTION..................................................................... 2 2. TRAJECTORY PLANNING PROBLEM AND ITS SOLUTIONS..... 4 3. TRAJECTORY PLANNING SYSTEM STRUCTURE...................... 13 3.1. Data Structures for Representing Geometric Paths.............. 13 3.2. Selection of a Trajectory Planning Method................................ 17 3.3. Generation of Dynamic Equations.............................................. 18 3.4. Generating the Constraint Function......................................... 21 3.5. Ancillary Software...................................................................... 22 4. CONCLUDING REMARKS......................................... 23 ii

RSD-TR-14-85 ABSTRACT The control of industrial robots is usually divided into several sequential stages. Trajectory planning is an important off-line stage which is concerned with the generation of a time history of a robot's joint position, velocity, acceleration, and input torques. A number of trajectory planning methods have been developed [1], [4]-[6], [10], [11], [13], which usually entail complex computations and algebraic manipulations. Programming this sort of trajectory planners is very complex and errorprone, thereby limiting their applicability. To remove this limitation, we have begun the development of software for automating the trajectory planning, called the Automatic Trajectory Planner Generator (ATPG). This paper describes important components of the ATPG: three trajectory planners, data structures for describing geometric paths, generation of the robot's dynamic equations and constraint functions, and ancillary software. A large portion of the ATPG has been completed, and the remaining portion is currently under development. ATPG 1

RSD-TR-14-85 1. INTRODUCTION Due to the complexity and nonlinearity of the robot's dynamics, the robot control problem is usually divided into several subproblems, which are first solved individually and then combined. This division can best be explained by Figure 1. From a task planner we obtain an ordered sequence of points in Cartesian space which represent a collision-free path if we connect them properly (e.g., by spline functions or straight line segments). The geometric path generator (i) transforms this Cartesian points to the corresponding points in joint space, and (ii) using the transformed points in joint space, generates a geometric path which is a parameterized curve in joint space (to be discussed in Section 2). The trajectory planner receives these geometric paths as input and determines a time history of position, velocity, acceleration, and input torques which are fed to a trajectory tracker. The trajectory tracker drives the robot to follow the desired trajectory specified by the trajectory planner with feedback information on a subset of position, velocity, acceleration, and input torques. As can be seen from the results presented in our previous papers[4], [10], [11], [13] and others [1], [3], [6], writing trajectory planning programs can be a laborious task. Before actually writing the program, kinematic and dynamic equations must be derived, and actuator constraints must be described. In addition, other mundane details, such as what data structures to use, must be considered. The aim of this paper is to describe the process of trajectory planner ATPG 2

RSD-TR-14-85 generation in detail, and present sufficient guidelines so that the entire process can be automated. There are two major reasons for automating the trajectory planner generation process. First, generating a trajectory planner without computer assistance consumes much expensive human labor, even if the task is performed only occasionally. This is due in large part to the perils of hand calculation; deriving dynamic equations by hand is slow and error-prone. Second, writing a trajectory planner is time-consuming. To understand why this second factor is important, consider the use of trajectory planners as robot design aids. If trajectory planners can be generated quickly, then a hypothetical robot design can be tested easily and accurately; the robot can be pushed to its working limits, and the designer can then check to see, for example, if any joints are under-powered. It also makes the effects of design changes easy to evaluate, since a new trajectory planner can be generated quickly, and a new set of tests can be run. The paper is organized as follows. For completeness we begin in the next section with a brief description of the trajectory planning problem and its solutions. In Section 3, the major components of a trajectory planning system are described. This description includes both functional descriptions of the components and suggestions regarding implementation details such as the choice of data structures. The generation of these components can then be reduced to algorithmic form suitable for computer implementation. The paper concludes with Section 4, where the status of our automatic trajectory planner is given. 3 ATPG

RSD-TR-14-85 2. TRAJECTORY PLANNING PROBLEM AND ITS SOLUTIONS Before discussing the problem of generating an automatic trajectory planner, we will first describe briefly the trajectory planning problem and its solutions. (See [10], [11], [13] for detailed descriptions.) For the trajectory planning problem, we have to consider the effects of restricting the manipulator's motion to a fixed collision-free path, which is specified by the geometric path generator in Figure 1. In what follows, the manipulator will be restricted to some geometric path q - f (X), 0 < x < Xmax (1) where q' is the position of the i-th joint and X is a scalar parameter. Since the parameter X along with the functions f ' completely describes joint positions, it will be referred to as the "position" variable. The i-th joint velocity then becomes v _q' df' dX dfX ' -_ (2) dX dt dX d ( where IpaX is the pseudo-velocity of the manipulator. Plugging this into the usual dynamic equations ui = Jii(q)q/ + Cijt qq + Rij q + gi(q) (3) gives the following equations of motion along the the geometric path ATPG 4

RSD-TR-14-85 X = 1 (4a) us = Ji,(X) L + J.(X)d p2 dX dX2 + C, (X)-/ d 2 + d(X) (4b) where ui is the torque/force applied at the i-th joint, Jii the NXN inertia matrix, Cijk the Coriolis force array, Rii the matrix representing viscous friction, gi the gravitational force, and N the number of the robot's joints. The Einstein summation convention is used here,. and all indices run from 1 to N.. It is of course assumed that the coordinates q' vary continuously with X. It d f i dj 2 f i is also assumed that the derivatives dA and fexist, and that the derivadX dX2 tives dfX are never all zero simultaneously. This ensures that the path never retraces itself as X goes from 0 to X,.ax Such a retrace would force the parameter X to take a discontinuous jump in order for the point q' to move forward continuously. It should be noted that in practice the geometric paths are given in Cartesian coordinates. While it is in general difficult to convert a curve in Cartesian coordinates to that in joint coordinates, it is relatively easy to perform the conversion for individual points. One can then pick a sufficiently large number of points on the Cartesian path, convert to joint coordinates, and use some sort 5 ATPG

RSD-TR-14-85 of interpolation technique (e.g. cubic splines) to obtain a similar path in joint space (see [6] for an example). Introducing some shorthand notation, let Mi -Ji df i == d2f i di df Q Jij d f2 +C d X d ' R -R d j '- dX ' Si.=g. We can-then express Eq. (4b) by: u, = Mi A + Qi L2 + R s + S,. (5) Note that the quantities listed above are functions of X. For the sake of brevity, the functional dependence is not indicated in what follows. The goal of automation is to produce goods at as low a cost as possible. In practice, costs may be divided into two groups: fixed and variable. Variable costs depend upon details of the manufacturing process, and include, in the cases where robots are used, that part of the cost of driving a robot which varies with robot motion, and some maintenance costs. Fixed costs are those which remain constant on a per-unit-time basis. Fixed costs include taxes, heating costs, building maintenance, and, in the case of a robot, the portion of the electric power which the robot uses to run its computer controller and other ATPG 6

RSD-TR-14-85 peripheral devices. If one assumes that the fixed costs dominate, then cost per item produced will be proportional to the time taken to produce the item. In other words, minimum cost is equivalent to minimum production time. A loose statement of the minimum-cost trajectory planning problem is as follows: What controls will drive a given robot along a specified curve in joint space with minimum cost, given constraints on initial and final velocities and on control signal magnitudes? As was seen in Eqs. (1), (2), (4a) and (4b), this form of the problem reduces the complexity of the control problem by introducing a single parameter X which describes the robot's position. The time derivative of this parameter and the parameter itself completely describe the current state (joint positions and velocities) of the robot. The control problem then becomes essentially a two dimensional minimum-cost control problem with some state and input constraints. The minimum-cost control problem can be stated as follows. Given a curve in the robot's joint space (or some equivalent coordinate system), the robot's dynamic properties, and the robot's actuator characteristics, what set of signals to the actuators will drive the robot from its current state to a desired final state with minimum cost? To state the above problem more formally, assume that the geometric path is given in the form of a parameterized curve as in Eq. (1). Also assume that the constraints on the actuator torques can be expressed in terms of the state of the system, i.e., in terms of the robot's speed and position, so that u E E(q,q) = Ei(X,pl) (6) 7 ATPG

RSD-TR-14-85 where u=(U1,u2,..., N)T is a vector of actuator torques/forces, and E: RN XRN - RN and E: R2 - RN are set functions. N is the number of joints the robot has. Given the functions f ', the set functions E and E,, the desired initial and final velocities, and the manipulator dynamic equations (4a) and (4b), the trajectory planning problem is to find the controls u(X) which minimize the cost functional C given by C = Jf0~o(u(X),q(X),q(X))dX. (7) A practical aspect of the trajectory planning problem is that of the description of curves and the actual calculation of actuator torques. Some suitable method of representing curves is required, and all computations involving those curves should be done automatically. In particular, it should be possible, given a robot's dynamic equations, to generate a trajectory planner for that robot. This is especially desirable in view of the fact that the dynamic equations of all but the simplest robots are very complicated, and any manipulation of such equations will be prone to human error. One possible approach to the solution of the above trajectory planning problem is to apply one of the standard tools of optimal control theory, Pontryagin's maximum principle. However, this approach requires solving a two-point boundary value problem for a non-linear system of differential equations with non-linear constraints; clearly, this does not lead to a tractable solution. The maximum principle also sheds little or no light on the other auxiliary ATPG 8

RSD-TR-14-85 problems, such as sensitivity to parameter variations. Moreover, since the trajectory planning problem frequently requires state or mixed state-control constraints, the maximum principle is not usually applicable. Therefore, we have taken a more intuitive but systematic approach to develop the three trajectory planners in [10], [11], [13], whose brief descriptions are given below for completeness. (A similar approach was also proposed independently of ours [1].) The first method is referred to as the phase plane method. It is so called because it makes use of plots of the "pseudo-velocity" p = X vs. the position parameter X. Such a plot, in which a velocity is plotted as a function of position, is generally referred to as a "phase plane plot", hence the name. Actually all three trajectory planners make use of this idea in one way or another, and from here on, the term "trajectory" will be taken to mean "phase trajectory", or X-p plot. The phase plane method is in general applicable only to minimum time problems, i.eo, C = ^. (7') Since only minimum-time solutions are to be considered, it is useful to consider how this restriction on the objective function can be used. Obviously, minimizing traversal times is equivalent to maximizing traversal speed. Given this fact, it is easy to see that, at least in the simplest case, the minimum time solution consists of an accelerating and a decelerating part; the robot should accelerate 9 ATPG

RSD-TR-14-85 at its maximum rate, then "put on the brakes" at precisely that time which will bring it to a stop at the destination point. Of course, there will in general be some velocity limits as well as acceleration limits. The velocity limits are imposed by the interaction of velocity-dependent force terms in the dynamic equations and the actuator torque limits; the actuators must generate enough torque to overcome these forces and keep the manipulator on the desired path. If the robot is to avoid these velocity limits, then the trajectory must alternately accelerate and decelerate, and the switching points should be timed so that the trajectory just barely misses exceeding the velocity limits. A more precise description of this method can be found in [10], including a derivation of the velocity limits and an algorithm for generating the optimal trajectories. Other complications are also discussed there. As an alternative, we have used dynamic programming to solve the trajectory planning problem [11]. Dynamic programming is an impractical method for solving the general path planning problem for an arm with a large number of joints, since there are two state variables per joint, thus requiring a 2N dimensional grid. (This is a classic example of the "curse of dimensionality".) However, when the path is given, there are only two state variables (i.e., X and is); thus only a 2-dimensional grid is required. To use dynamic programming, the grid is set up so that the position parameter X is used as the stage variable. Thus a "column" of the grid corresponds to a fixed value of X, while a "row" corresponds to a fixed p value. One starts at the desired final state (the last column of the grid, with the row ATPG 10

RSD-TR-14-85 corresponding to the desired final pA value) and assigns that state zero cost. All other states with position Xmix are given a cost of infinity. Then the usual dynamic programming algorithm can be applied. The algorithm starts at the last column. For each point in the previous column one finds all the accessible points in the current column, determines the minimum cost to go from the previous to the current column, and increments costs accordingly. For each. of the previous grid points, the optimal choice of the next grid point is recorded. When the initial state is reached, the optimal trajectory is found by following the pointer chain which starts at the given initial state. In the case at hand, determining which points are accessible from one column to the next is simply a matter of checking to see if the slope of the curve connecting the two points gives a permissible value. (The slope limits can be found from the constraints on the actuator torques (6).) The incremental cost is computed for minimum cost problems, so a running sum can be kept for the total cost. Dynamic programming has the advantage that it is a well-established and well-understood optimization method. It also gives the control law for any point on the curve, and so makes provision for the robot to vary its speed if necessary. (This of course assumes that the robot stays on the desired path.) On the other hand, if it is implemented in the most obvious and straightforward manner, it requires a large array for computations, and if the array size is to be known in advance then an upper bound on the velocity is needed. In practice, there may be artificially imposed velocity bounds, but in general it would be necessary to either calculate velocity bounds in advance or create a new (larger) grid and 11 ATPG

RSD-TR-14-85 start all over if the trajectory left the grid. The computation times also increase rather quickly as the density of the grid, and hence the accuracy of the solution, increases. Some modifications to the algorithm are suggested in [7] which should considerably increase its speed. It should also be noted that dynamic programming may still be used even if the robot's actuator torque constraints are not independent of one another; this is not the case with the phase plane method. Making the phase plane algorithm work for non-independent actuators would require that the space of actuator torques be searched for an acceleration bound. Dynamic programming only requires that a function be available which returns a yes-or-no answer to the question "if this acceleration is desired, will the required torques be realizable?". The dynamic programming algorithm itself performs the search of the actuator torque space. A third algorithm, called the perturbation trajectory improvement algorithm (PTIA), is developed as a simple but powerful method [131. This algorithm is in some respects similar to the dynamic programming algorithm, though like the phase plane method it is only applicable to minimum time problems. This algorithm starts with an initial feasible trajectory, and perturbs the trajectory in such a way that the traversal time for the trajectory decreases, while the trajectory remains feasible. This method has most of the advantages of the dynamic programming method, and can be modified to generate minimum-time trajectories when there are limits on the jerk, or the derivative of the acceleration, as well as limits on joint torques. ATPG 12

RSD-TR-14-85 3. TRAJECTORY PLANNING SYSTEM STRUCTURE A trajectory planner is of no use in isolation; it must be part of a larger system. In a practical robot control system, the trajectory planner receives a path description from a geometric path planner, which generates the geometric path description from task descriptions. After the trajectory planner has assigned timing information to this path, it is passed on to a tracker which drives the robot in real time. However, if the trajectory planner is being used as a design aid, as suggested above, the geometric path planner, the path tracker, and the robot will not actually be present. In this case, a driver for generating geometric paths and a stub for analyzing the trajectory planner output are required. Such a system is shown schematically in Figure 2. The precise character of the trajectory planner, the driver routine, and the stub are determined largely by the choice of representations for the input and output data and by the particular type of trajectory planner chosen. These subjects are discussed in the following subsections. 3.1. Data Structures for Representing Geometric Paths Choosing the type of data structure used to represent geometric paths is probably the single most important decision to be made in designing a path planning system. It influences not just the design of the trajectory planner, but the design of the geometric path planner and the tracking system as well. This being the case, the geometric path representation must be considered very carefully. Several options are considered here, and their implications for the trajec13 ATPG

RSD-TR-14-85 tory planning system design are investigated. Three geometric path descriptions are discussed here: expression trees, splines, and arrays of points. An expression tree is a linked structure in which the internal nodes of the tree represent operators, the subtrees lying below a given node represent the operands for that node, and the leaves are irreducible expressions such as constants or variables. Such trees are equivalent to algebraic expressions. Splines are sequences of curves which are connected end-toend. These curves usually are chosen so that they have some particular number of continuous derivatives, even at the points where the curve segments meet. For example, if cubic polynomials are used to connect the intermediate points, then it is possible to have continuous first derivatives. The second derivatives exist, but in general are discontinuous. Finally, a curve can be represented as a sequence of points. The points must be chosen so that any reasonably smooth curve which connects all the points will not deviate too much from the actual desired path. Expression trees have the advantage that their manipulation as algebraic expressions is easy. Such manipulations mimic the processes carried out by humans, and the results are complete algebraic expressions. Such manipulations are required for deriving the robot's dynamic equations, so a formula manipulation system of some sort will be required anyway. However, expression trees have the disadvantage that introducing new types of curves for the robot to traverse may require that new types of operators be created. Also, the paths which robots are expected to traverse often are composed of connected pieces ATPG 14

RSD-TR-14-85 rather than single analytic curves. Some sort of conditional operator, such as a Heaviside step, is then required, and this introduces problems when the curve is differentiated; the resulting analytic expression may contain delta functions. Cubic (or other) splines can be thought of as a restricted case of an expression tree. The expression tree will consist of the sum of many conditional expressions, where each conditional expression consists of two step functions multiplied by a cubic polynomial. For instance, the segment of a spline which passes from point 5 to point 6 might be represented as a (X-XS)8 (\-X)P(X), (8) where t~ if 0<0 a()={ if 0>0 (9) and ps(X) is a cubic polynomial in X. The problems discussed above for expression trees also apply to cubic splines. A simple array of points does not lend itself to symbolic manipulation of any sort. However, most of the calculations which the trajectory planner must carry out can be done numerically, so this is not a big disadvantage. The major disadvantage of this method is that the path is not really completely specified; the motion between the interpolation points is undetermined. However, if the density of interpolation points is high enough, this will not matter in practice. Indeed, the perturbation and dynamic programming trajectory planners discre15 ATPG

RSD-TR-14-85 tize the trajectory planning problem anyway, and the differential equations that need to be solved when using the phase-plane method must in practice be solved using discrete, approximate methods. The representation is very simple, and for that reason was used in most of the examples presented in [10], [11], [13]. It is also a very easy structure to attach other information to; if the geometric path is represented as an array of points, and the points are represented as a structure or record, then time, torque, and velocity information can be attached to each point by simply adding new fields to the record. Attaching this information to a spline or an expression tree is much more difficult. For these reasons, an array of points appears to be a good choice of data structures. The data structure used in the examples in this paper is shown in Figure 3a. It is a record, and it contains information which applies to the path as a whole: the maximum value of the parameter X, the number of joints the robot has, and the number of interpolation points on the curve. In addition, this record contains pointers to several arrays, including an array of X-values for each point on the curve. Finally, it contains a pointer to an array of "joint paths", where each joint path is an array of joint data values, one array element per point. It is in these arrays that the information pertaining to the individual joints, such as the dynamic coefficients M,, Qi, Ri, and Si, in Eq. (5), the parametric derivatives -d- and -d2, and the joint torques ui and dX dX2 motor voltages V,, are stored. A typical structure for these values (the D,, in ATPG 16

RSD-TR-14-85 Figure 3a) is shown in Figure 3b. The trajectory planning process can be expressed entirely in terms of operations on this data structure. The input to the trajectory planner is a sequence of points, and the ultimate goal of the trajectory planner is to fill in the values of p, p, t, etc. at each point on the curve. The intermediate steps are to compute the values of the parametric derivatives d and d 2 i calcudX dX2 late the dynamic coefficients M,, Qi, Ri, and Si, and finally apply a trajectory planning algorithm which generates p, a, t, and u, for 1<i <N. 3.2. Selection of a Trajectory Planning Method The choice of geometric path representations influences the structure of the entire path planning system, from the geometric path planner down through the tracking system. The choice of trajectory planning techniques has more localized effects, but the effects on the implementation of the trajectory planner, which are of primary concern here, are major. Assuming that minimum-time trajectories are desired, the perturbation method should be chosen rather than dynamic programming, since it is considerably faster and can handle more general torque constraints. The phase plane method is the fastest of the three techniques described in Section 2, but cannot be generalized to handle all the kinds of torque constraints that the perturbation method can handle. The torque constraints also must be incorporated into the phase plane algorithm in several places, making changes to the torque limits more difficult in a phase-plane trajectory planner than in a perturbation trajectory planner. 17 ATPG

RSD-TR-14-85 The perturbation method isolates the torque constraints into a single constraint function, thereby isolating the torque constraint checks from the rest of the trajectory planner. Because of the ease of construction of perturbation trajectory planners, and because of the natural way that they break up into modules, they would appear to be good choices for a computer-generated trajectory planner. The major components of such a trajectory planner are shown in Figure 4. The components are a derivative generator, a dynamic coefficient generator, and the trajectory planning algorithm itself. Note that the constraint function "plugs into", or links to, the trajectory planning algorithm only, and that the trajectory planning algorithm itself is the same regardless of the torque constraints. The trajectory planning algorithm also is the same for any robot, regardless of its dynamics; everything that the trajectory planning algorithm needs to know about the robot's dynamics is distilled into the dynamic coefficients M., Qi, Ri, and S.. The derivative generator can be a simple numerical procedure and is independent of the robot characteristics also, so that only the constraint function and the dynamic coefficient generator need ever be changed. 3.3. Generation of Dynamic Equations So far, very little has been said about the actual generation of the dynamic equations for robots. This is the most time-consuming, laborintensive and error-prone part of the trajectory planner generation process, and ATPG 18

RSD-TR-14-85 so is the most important part to automate. Computer generation of dynamic equations may be time-consuming, but it is certain to be several orders of magnitude faster than hand calculation, and certainly will be more accurate. Though it sometimes is possible to derive dynamic equations of simple robots relatively quickly in ad hoc ways, such tricks are difficult to apply systematically. However, systematic methods do exist; see [9]. These systematic methods, combined with a system for doing symbolic algebra, such as MACSYMA [8] and REDUCE [3], and a symbolic differentiator make the generation of dynamic equations fairly straightforward. While the resulting equations may not be in simplest form, they will certainly be correct. It is important to note that having the dynamic equations in simplest form may not always be necessary. In particular, this is the case if the trajectory planning system is to be used primarily as a tool for robot design. In that situation, the trajectory planner will probably be run several times and then thrown away as design changes are made. It is much more important that the trajectory planner be generated quickly than that it run quickly. (Essentially the same reasoning is used to justify the existence of slow optimizing and fast non-optimizing compilers for conventional computer languages; there usually is no sense in spending ten minutes to optimize a small program that will be run only once or twice.) A schematic of the robot dynamics generation process is shown in Figure 5. The user first describes the robot's kinematics; this can be done by specifying the number of joints, whether the joint is revolute or prismatic, the 1Q ATPG

RSD-TR-14-85 various joint offsets and twists, and the link lengths. The resulting transformation matrices can be computed as in [9]. Once the forward kinematics of the robot are known, the user may describe the link inertias. A simple version of a dynamic equation generator may just prompt the user for the link pseudo-inertias; a more sophisticated version would compute the inertias from CAD models of the robot links. Given the kinematics and the pseudo-inertias, the inertia matrix, the Coriolis coefficient array and the gravitational forces can be calculated using the formulas in [9]. It is for these computations that a symbolic differentiator is needed, since the kinematic transform matrices must be differentiated with respect to the joint variables. The output of the dynamic equation generator will be arrays of expression trees representing the inertia matrix, the Coriolis coefficient array, and the gravitational force terms. These must be translated from arrays of expression trees to some form suitable for compilation by whatever high-level language compiler is chosen as an implementation language. If the target language has pointers to procedures, then an appropriate representation of, for example, the inertia matrix would be a two-dimensional array of pointers to procedures; each element of the array would be set to point to a function which would compute the appropriate inertia matrix coefficient, given the robot's current position. This requires only that the expression tree representations be translated into appropriate computer code, a very simple task. ATPG 20

RSD-TR-14-85 3.4. Generating the Constraint Function Generating the constraint function for a dynamic programming or perturbation trajectory planner poses some problems which are not amenable to general solution. Actuator torque constraints may in general be quite complicated; for example, in a cable-driven robot arm, moving one drive cable may move several joints, cables will stretch, and the cables must all be kept under positive tension. Generating constraint functions for completely arbitrary actuators is obviously very difficult, if not impossible. However, not every actuator which is theoretically possible, nor even every actuator which is practically realizable, need be considered. By restricting the trajectory planner generator to a few very common actuator types, most practical robot designs can be handled. One type of actuator which is very common is the D.C. torque motor. The torque bounds for such a device are determined by the saturation limits of the motor itself and by the properties of the amplifier which drives the motor. In addition, there are limits on the time derivative of the torque which are imposed by the motor inductance and the drive amplifier voltage limits, but these constraints can often be ignored in practice. Since only a few parameters are needed to describe the motor torque limits, generating constraint functions for this case is a fairly simple process. Other types of actuators, such as hydraulic servoes, may also be described in a simple manner provided that effects such as delays due to the finite speed of propagation of pressure waves are ignored. As long as these effects are 21 ATPG

RSD-TR-14-85 small, the resulting constraint function should produce a useful trajectory planner. 3.5. Ancillary Software As indicated in Figure 2, the trajectory planner is not the only component of a path planning system; if the trajectory planner is to be used as a design tool, a driver for generating test data and a stub for analyzing trajectory planner output are both required. The stub can be a very simple routine. Most of the time a graphical representation of joint speeds and forces suffices, and the generation of such output can be performed in a manner which does not depend on the kinematics or dynamics of the robot. The driver routine, on the other hand, may depend rather heavily upon the robot's kinematics. If the designer wants to evaluate the performance of the robot as it moves along a Cartesian straight line, then the inverse kinematics of the robot are required. This causes some problems, since the inverse kinematics of robot arms cannot in general be computed in closed form, and because the inverse kinematic solutions are not always unique. However, many robots fall into a relatively small set of kinematic configurations, so that the inverse kinematics can be computed for a generic member of each kinematic class. Then the designer need only plug the precise link dimensions into a standard formula. (See [21 for an example.) While this approach may not work for certain exotic robot designs, it probably will cover the vast majority of robots seen in practice. ATPG 22

RSD-TR-14-85 Another desirable feature of the driver routine is the generation of geodesics in inertia space. The traversal times for these curves are near-optimal 12], and so will give the robot designer and desi estimate of the robot's maximum capabilities. Since the differential equations of inertial geodesics are directly obtainable from the robot's dynamic equations, much of the work of writing a geodesic generator will have been done already. The equations need only be incorporated into an appropriate differential equation solver. The proposed trajectory planner generation system is shown schematically in Figure 6. The user provides kinematics, link inertias, and actuator characteristics, and the planner generator produces a trajectory planner, a stub, and a driver. The driver allows selection of straight line paths in joint space, Cartesian straight lines, and geodesics. 4. CONCLUDING REMARKS Most of the work performed to date on the automatic generation of trajectory planners consists of the construction of a system for manipulation of algebraic expressions and a symbolic differentiator. These routines consist of about 1400 lines of C, with an additional 1300 lines of code for testing the differentiator. Though these utilities only constitute a portion of the trajectory planner generating system, they are very important components. The largest single component of the trajectory planner generator will most likely be the dynamic equation generator, of which the algebraic manipulation system is an integral part. 23 ATPG

RSD-TR-14-85 Also completed is the stub for generating trajectory planner output, which consists of about 250 lines of code for the main routine and about 2700 lines for various graphical plots. Some of the rest of the code for the trajectory planner consists of fixed modules; these portions can be taken from the code already written for the numerical examples in [101, [11], [13], and used either directly or with minor modifications. The example for the phase plane plot method in [10] has been coded with about 350 lines of C, that for the dynamic programming method in [11] with about 400 lines, and that for the perturbed trajectory improvement algorithm in [13] with only 50 lines for the main routine and about 150 lines for generating the constraint function. Though much work remains to be done,1 it is clear from the discussion presented here that the major problems involved in producing an automatic trajectory planner generator involve writing some large but well-defined pieces of code; there are no major conceptual or theoretical problems to be solved. REFERENCES [1] J. E. Bobrow, S. Dubowsky, and J. S. Gibson, "On the Optimal Control of Robotic Manipulators with Actuator Constraints," Proc. of American Control Conference, pp. 782-787, June 1983. lsuch as completion of the three trajectory planners, inclusion of the other trajectory planners, CAD modeling of the robot links, etc. ATP.~G 24

RSD-TR-14-85 [2] S. J. Derby, "Kinematic Elasto-Dynamic Analysis and Computer Graphic Simulation of General Purpose Robot Manipulators," Ph.D. thesis, Rensselaer Polytechnic Institute, Troy, New York, August 1981. [3] A. C. Hearn, "REDUCE User's Manual," Report, UCP-19, University of Utah, 1973. [4] B. K. Kim and K. G. Shin, "Minimum-Time Path Planning for Robot Arms with Their Dynamics Included," IEEE Trans. on System, Man, and Cybernetics, vol. SMC-15, no. 2, pp. 213-223, March/April 1985. [5] J. Y. S. Luh and C. S. Lin, "Optimal Path Planning for Mechanical Manipulators," ASME Journal of Dynamic Systems, Measurements, and Control, vol. 102, pp. 142-151, June 1981. [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, vol. AC-28, no. 12, pp. 1066 -1074, December 1983. [7] N. D. McKay, "Minimum-Cost Control of Robotic Manipulators with Geometric Path Constraints," Ph.D. thesis, The University of Michigan, Ann Arbor, MI, September 1985. [8] J. Moses, "Algebraic Simplification: A Guide for the Perplexed," Proceedings of the second ACM Symposium on Symbolic and Algebraic Manipulation, New York, pp. 282-304, 1971. [9] R. P. C. Paul, Robot manipulators: Mathematics, programming, and control, MIT Press, Cambridge, Mass., 1981. [10] K. G. Shin and N. D. McKay, "Robot Path Planning Using Dynamic Programming," Proc. of 23rd CDC, December 1984. (Also to appear in IEEE Trans. on Automatic Control.) 11] K. G. Shin and N. D. McKay, "Minimum-Time Control of Robotic Mani pulators with Geometric Path Constraints," IEEE Trans. on Automatic Control, vol. AC-30, no. 6, pp. 531-541, June 1985. [12] K. G. Shin and N. D. McKay "Selection of Near-Minimum Time Geometric Paths for Robotic Manipulators," 25 ATPG

Time history of Specifications An ordered sequence of parameterized path position, velocity, of Task and points in Cartesian space in joint space acceleration Environment Task Planner _ _^Geometric Path/Trajectory Path Generator q.(X) Planner l) il qi. fl (X) q~/{f. qe(. qdO Of Xd, Pj~ Pj Actuator Signal Robot _- Path/Trajectory — Tracker I — I I~ ^ I I Sensors I Functiona Blockiagraofq[tP i(tti, Cqt(olt)C Figare 1. Functionsu Block DiAr'am of M nipnlator Porition Control.

RSD-TR-14-85 Curve endpoints Curve type kinematics link inertias Driver actuator characteristics Parameterized curve Automatic. Trajectory Trajectory _____ a Planner Planner Module Module Generator- Curve with timing information Stub Graphical output -— ani Program generation -- Date flow 27 Figure 2. Overview of ATPG system ATPG

Number of Joints J RSD-TR-14-S8 Number of Points P -— i --- "" ^* /o, O "- Yr | v _ - -L Mo1 I M Ix, | " *' | A2 _____ _iflIli i * *DO j D01 DO,2 a. J Doper i: a) Path data structure dq d q A<^. (X ) Q,(R.) R,(X. ) S,(X, ) 4 i V. b) The structure D,. ATPG Figure 3. Robot path data structures dq= d~~~~~q.2

Parameterized curve Derivative Generator Curve with derivatives ^Dynamic -__ Ikinematics Coefficient Dynamic Equations and link inertias Calculator Curve with derivatives and dynamic coefficients Trajectory Planning | Constraint Function * Actuator characteristics Algorithm Curve with timing information and nominal torques a priori information — Data flow 29 Figure 4 Trajectory nlanner structure ATPG

RSD-TR-14-85 Frrction Coefficients Kinematic Data Link Pseudo- nertlas Compute Transforms Tl,...,Tj Compute Derivatives of Transforms 8T, aCJ Compute Com Construct Viscous Ip; Construct Viscous nertia Gravitational Friction Matrix Terms Matrix erms Compute Coriolis Array R,] Jj Cyt g, ATPG 30 Figure 5. Schematic of Dynamic Equations Generator

link dimensions link inertias actuator data Kinematic Equation Generator Forward kinematics Inverse Dynamic Constraint Kinematics Equation Function Generator Generator kinematics _ Funct Geodesic Equation Planner Gene r Generator Equations. —. -- Of geodesics Driver Program Generator driver trajector program planner 31 ATPG Figure 6. ATPG structure