MINIMUM-COST CONTROL OF ROBOTIC MANIPULATORS WITH GEOMETRIC PATH CONSTRAINTS by Neil David McKay A dissertation submitted in partial fulfillment of the requirements for the degree of Doctor of Philosophy (Electrical Engineering) in The University of Michigan 1985 Doctoral Committee: Associate Professor Kang G. Shin, Chairman Professor N. Harris McClamroch Associate Professor Trevor N. Mudge Professor Richard A. Volz

ACKNOWLEDGEMENTS I would like to thank my advisor, Kang Shin, for his guidance and occasional prodding over the past five years. I would also like to thank my other committee members for their advice, my parents for their moral (and occasionally financial) support, and the other graduate students who live in the 1500 wing of East Engineering for all the good times they provided. The work described in this thesis was supported in part by NSF Grant No. ECS-8409938 and the US AFOSR Contract No. F49620-82-C-0089. Any opinions, findings, and conclusions or recommendations are those of the author and do not necessarily reflect the view of the funding agencies.

TABLE OF CONTENTS ACKNOWLEDGEMENTS.......................................................... ii LIST OF FIGURES........................................................................................ LIST OF TABLES.......................................................................................... ix CHAPTER 1. INTRODUCTION AND LITERATURE SURVEY............1...... 1 2. MATHEMATICAL PRELIMINARIES...............1........................ 12 2.1. Tensors and Tensor Notation........................................................ 12 2.2. Riemannian Spaces and Properties of Curves................................ 15 2.3. Robot Arm Dynamics....................................................... 19 2.4. Derivation of the Lagrangian Dynamic Equations......................... 20 3. PROBLEM STATEMENT....................................................... 27 4. TRAJECTORY PLANNING..................................................... 32 4.1. Parameterization of the Robot Dynamic Equations...................... 36 4.2. The Phase Plane Method....................................................... 38 4.2.1. Derivation of Pseudo-Acceleration Limits........................... 38 4.2.2. Formulation of the Optimal Control Problem.................... 41 4.2.3. Phase Plane Plots................................................................ 44 4.2.4. Determination of Optimal Trajectories.............................. 50 4.2.5. Numerical Examples....................................................... 57 4.3. The Dynamic Programming Method.............................................. 76 4.3.1. Problem Formulation....................................................... 77 4.3.2. Trajectory Planning Using Dynamic Programming............ 78 4.3.3. Case of Non-Interacting Torque Bounds............................. 79 4.3.4. Case of Interacting Torque Bounds..................................... 84 4.3.5. Accommodation of Jerk Constraints................................... 87 4.3.6. Algorithm Complexity....................................................... 89 1X1 111

4.3.7. Algorithm Speedup......................................................... 92 4.3.8. Convergence Properties....................................................... 94 4.3.9. Numerical Examples......................................................... 112 4.4. Trajectory Planning by Iterative Improvement.................. 154 4.4.1. The Perturbation Trajectory Improvement Algorithm......................................................................................................... 154 4.4.2. Computational Requirements.............................................. 159 4.4.3. Numerical Examples......................................................... 160 5. SELECTION OF GEOMETRIC PATHS.................................... 183 5.1. Near-Minimum Time Path Planning.............................................. 184 5.1.1. A Special Case......................................................... 185 5.1.2. Traversal Time Bounds....................................... 188 5.1.3. Approximate Minimum Time Paths.................................... 197 5.2. Numerical Examples......................................................... 201 8. COMPENSATION FOR DYNAMIC UNCERTAINTIES 215 6.1. Calculation of Dynamic Coefficient Errors.................................... 217 6.2. Calculation of Torque Error Bounds.............................................. 222 6.3. Numerical Examples....................................................................... 231 7. AUTOMATIC GENERATION OF TRAJECTORY PLANNERS........................................................................................ 257 7.1. Trajectory Planning System Structure.......................................... 258 7.2. Data Structures for Representing Geometric Paths....................... 259 7.3. Selection of a Trajectory Planning Method................................... 262 7.4. Generation of Dynamic Equations.................................................. 263 7.5. Generating the Constraint Function.............................................. 265 7.6. Ancillary Software......................................................... 266 7.7. Work Accomplished......................................................... 267 8. CONCLUSIONS......................................................... 274 APPENDIX......................................................... 278 iv

R EFER E NC E S............................................................................................... 298 V

LIST OF FIGURES Figure 4.2.1. Admissible regions of p determined by parabolic constraints...... 65 Figure 4.2.2. Intersection of admissible regions of p.......................................... 68 Figure 4.2.3. Acceleration and deceleration vectors at each phase point........... 67 Figure 4.2.4. Case when accelerating and decelerating curves intersect............ 68 Figure 4.2.5. Case when accelerating and decelerating curves do not intersect.............................................................................................................. 69 Figure 4.2.6. A complete optimal trajectory with three switching points......... 70 Figure 4.2.7. The two degree-of-freedom polar robot......................................... 71 Figure 4.2.8. Optimal trajectory, zero friction case........................................... 72 Figure 4.2.9. Admissible region, high-friction case........................................ 73 Figure 4.2.10. Optimal trajectory, high-friction case...................................... 74 Figure 4.2.11. Admissible region for cartesian manipulator............................... 75 Figure 4.3.1. Curves connecting adjacent DP grid points................................ 119 Figure 4.3.2. Curve connecting a grid point to an existing trajectory............... 120 Figure 4.3.3. Suboptimal trajectory with uncertainty swath............................. 121 Figure 4.3.4. Minimum time phase plane plot for polar manipulator, 10X10 grid.......................................................... 122 Figure 4.3.5. Minimum time phase plane plot for polar manipulator, 40X40 grid......................................................... 123 Figure 4.3.6a. Minimum time phase plot for PACS arm, 10X10 grid............... 124 Figure 4.3.6b. U joint torque vs. time, 10X10 grid............................................ 125 Figure 4.3.6c. r joint force vs. time, 10X10 grid.............................................. 126 Figure 4.3.6d. z joint force vs. time, 10X10 grid.............................................. 127 Figure 4.3.6e. Motor voltages vs. time, 10X10 grid........................................... 128 Figure 4.3.7a. Minimum time phase plot for PACS arm, 40X160 grid............. 129 Figure 4.3.7b. U joint torque vs. time, 40 X 160 grid........................................... 130 Figure 4.3.7c. r joint force vs. time, 40X160 grid............................................. 131 Figure 4.3.7d. z joint force vs. time, 40X 160 grid............................................ 132 Figure 4.3.7e. Motor voltages vs. time, 40X 160 grid......................................... 133 Figure 4.3.8a. Phase plot for PACS arm, minimum time-energy, 10X10 grid Figure 4.3.8b. O joint torque vs. time, 10X10 grid............................................ 135 Figure 4.3.8c. r joint force vs. time, 10X10 grid........................................ 136 Figure 4.3.8d. z joint force vs. time, 10X10 grid.............................................. 137 Figure 4.3.8e. Motor voltages vs. time, 10X10 grid........................................... 138 vi

Figure 4.3.9a. Phase plot for PACS arm, minimum time-energy, 20X40 grid Figure 4.3.0b. U joint torque vs. time, 20X40 grid................................1........ 140 Figure 4.3.9c. r joint force vs. time, 20X40 grid.......................1...................... 141 Figure 4.3.9d. z joint force vs. time, 20X40 grid.............................................. 142 Figure 4.3.9e. Motor voltages vs. time, 20X 40 grid........................................... 143 Figure 4.3.10a. Minimum time phase plot for PACS arm with power limit, 10 X 10 grid......................................................... 144 Figure 4.3.10b. I joint torque vs. time, ox 10 grid.................................. 145 Figure 4.3.10c. r joint force vs. time, OX10 gri d.......................................... 146 Figure 4.3.10d. z joint force vs. time, lOX 10 grid........................... 147 Figure 4.3.10e. Motor voltages vs. time, 10X10 grid.............................148..... 148 Figure 4.3.11a. Minimum time phase plot for PACS arm with power limit, 20 X 40 grid......................................................... 149 Figure 4.3.11b. i joint torque vs. time, 20X40 grid........................................... 150 Figure 4.3.11c. r joint force vs. time, 20X40 grid........................... 151 Figure 4.3.11d. z joint force vs. time, 20X40 grid................... 152 Figure 4.3.11e. Motor voltages vs. time, 20X40 grid......................................... 153 Figure 4.4.1. Results of Algorithm A......................................... 163 Figure 4.4.2. Results of Algorithm A'............... 164 Figure 4.4.3a. Phase plane plot for straight line................................................ 165 Figure 4.4.3b. Joint position vs. time for straight line....................................... 166 Figure 4.4.3c. Motor voltage vs. time for straight line....................................... 167 Figure 4.4.4a. Phase plane plot for geodesic............................................ 168 Figure 4.4.4b. Joint position vs. time for geodesic....................................... 169 Figure 4.4.4c. Motor voltage vs. time for geodesic........................................ 170 Figure 4.4.5a. Phase plane plot for joint-interpolated curve.............................. 171 Figure 4.4.5b. Joint position vs. time for joint-interpolated curve.................... 172 Figure 4.4.5c. Motor voltage vs. time for joint-interpolated curve.................... 173 Figure 4.4.6. Phase plane plot for straight line with jerk constraint, 25 points......................................................... 174 Figure 4.4.7. Phase plane plot for straight line with jerk constraint, 50 points........................................................................ 175 Figure 4.4.8. Illustration of deadlock caused by jerk constraints...................... 176 Figure 4.4.9a. Phase plane plot for straight line with jerk constraint, 50 points, velocity increment 0.1........................................ 177 Figure 4.4.9b. Joint position vs. time for straight line with jerk constraint, 50 points, velocity increment 0.1......................................................... 178 Figure 4.4.9c. Motor voltage vs. time for straight line with jerk constraint, 50 points, velocity increment 0.1......................................................... 179 Vri

Figure 4.4.10a. Phase plane plot for straight line with jerk constraint, 100 points, velocity increment 0.025......................................................... 180 Figure 4.4.10b. Joint position vs. time for straight line with jerk constraint, 100 points, velocity increment 0.025......................................................... 181 Figure 4.4.10c. Motor voltage vs. time for straight line with jerk constraint, 100 points, velocity increment 0.025.................................................. 182 Figure 5.2.1a. Phase plane plot for straight line................................................ 206 Figure 5.2.1b. Joint position vs. time for straight line..................................... 207 Figure 5.2.1c. Motor voltage vs. time for straight line....................................... 208 Figure 5.2.2a. Phase plane plot for joint-interpolated path............................... 209 Figure 5.2.2b. Joint position vs. time for joint-interpolated path..................... 210 Figure 5.2.2c. Motor voltage vs. time for joint-interpolated path..................... 211 Figure 5.2.3a. Phase plane plot for geodesic....................................................... 212 Figure 5.2.3b. Joint position vs. time for geodesic............................................. 213 Figure 5.2.3c. Motor voltage vs. time for geodesic............................................. 214 Figure 6.3.1a. Phase plane plot for zero error........................................ 239 Figure 6.3.1b. Voltage vs. time for zero error......................................... 240 Figure 6.3.2a. Phase plane plot for density 12 g./cc......................................... 241 Figure 6.3.2b. Joint position vs. time, 12 g./cc.................................................. 242 Figure 6.3.2c. Nominal and actual motor voltages, z joint, 12 g./cc............... 243 Figure 6.3.2d. Nominal and actual motor voltages, i joint, 12 g./cc............... 244 Figure 6.3.2e. Nominal and actual motor voltages, r joint, 12 g./cc............... 245 Figure 6.3.2f. Nominal and actual joint forces, z joint, 12 g./cc..................... 246 Figure 6.3.2g. Nominal and actual joint torques, a joint, 12 g./cc................ 247 Figure 6.3.2h. Nominal and actual joint forces, r joint, 12 g./cc.................. 248 Figure 6.3.3a. Phase plane plot for density 24 g./cc.......................................... 249 Figure 6.3.3b. Joint position vs. time, 24 g./cc..................................... 250 Figure 6.3.3c. Nominal and actual motor voltages, z joint, 24 g./cc............... 251 Figure 6.3.3d. Nominal and actual motor voltages, 0 joint, 24 g./cc............... 252 Figure 6.3.3e. Nominal and actual motor voltages, r joint, 24 g./cc............... 253 Figure 6.3.3f. Nominal and actual joint forces, z joint, 24 g./cc...................... 254 Figure 6.3.3g. Nominal and actual joint torques, 0 joint, 24 g./cc.................... 255 Figure 6.3.3h. Nominal and actual joint forces, r joint, 24 g./cc.................... 256 Figure 7.1.1. Overview of ATPG system......................................................... 269 Figure 7.2.1. Robot path data structures........................................ 270 Figure 7.3.1. Trajectory planner structure........................................................ 271 Figure 7.4.1. Schematic of Dynamic Equation Generator................................. 272 Figure 7.6.1. ATPG structure............................................................ 273 Figure A.1. Schematic of the first three links of the PACS robot..................... 293 Figure A.2. Link coordinate frames of the PACS robot.................................... 294 Figure A.3. PACS actuator circuit diagram....................................................... 295 V 111

LIST OF TABLES Table 4.3.1. Computation times for different grid sizes (seconds)..................... 118 Table 6.3.1. Traversal times for nominal and optimal trajectories.................... 236 Table 6.3.2. Minimum required voltages for nominal and actual payloads. 0000..... ~ ~~........ ~........ ~~.00~00~~~~..~......~.. ~~ e~~.....~ ~ 236 Table 6.3.3. Maximum required voltages for nominal and actual payloads. Table 6.3.4. Minimum required torques/forces for nominal and actual payloads.......................................................... 237 Table 8.3.5. Maximum required torques/forces for nominal and actual payloads......................................................... 238 Table A.1. Characteristics of the polar manipulator........................................ 291 Table A.2. Characteristics of the PACS arm........................... 292 Ix

CHAPTER 1 INTRODUCTION AND LITERATURE SURVEY During the past several years a great deal of attention has been focused on industrial automation techniques, especially the use of general-purpose robots. Those industries which in the past have constructed special-purpose devices for manufacturing their products are now looking at the possibility of using robots instead. Unlike special-purpose tools, robots' behavior can easily be modified, so that retooling is kept to a minimum. In some cases they can also make feasible the manufacture of a product in lots which would be too small to justify the creation of a special-purpose machine for their manufacture. Mechanical maintenance is also simpler, since there presumably would be only a few types of robots performing many different tasks. (It should be noted, though, that maintenance of specialpurpose machines is replaced with maintenance of special-purpose programs.) Since robots may be controlled in virtually any manner, one legitimately may ask how a robot should best be controlled. The obvious answer to this question is that the robot should produce as large a profit as possible per unit time. The usual assumption is that material costs and fixed costs dominate the cost per item produced, so that it is desirable to produce as many units as possible in a given time.

There are a variety of algorithms available for manipulator control. These algorithms usually assume that the control structure of the robot has been divided into two levels. The upper level is called path or trajectory planning, and the lower level is called path control or path tracking. Path control is the process of making the robots actual position and velocity match some desired values of position and velocity; the desired values are provided to the controller by the trajectory planner. The trajectory planner receives as input some sort of spatial path descriptor from which it calculates a time history of the desired positions and velocities. (The term "Path Planner" is often used in the literature; this is a misnomer, since it does not plan paths but rather supplies timing information to a pre-planned geometric curve. The term "trajectory planner" will therefore be used here.) The path tracker then compensates for any deviations of the actual position and velocity from the desired values. The reason for dividing the control scheme in this way is that the process of robot control, if considered in its entirety, is very complicated, since the dynamics of all but the simplest robots are highly nonlinear and coupled. Dividing the controller into the two parts makes the whole process simpler. The path tracker is frequently a linear controller (e.g. a PID controller). While the nonlinearities of manipulator dynamics frequently are not taken into account at this level, such trackers can generally keep the manipulator fairly close to the desired trajectory. More sophisticated methods can be used, though, such as resolved motion rate control [331, resolved acceleration control [24], and various adaptive techniques [4, 6, 7, 14, 16, 17]. Unfortunately, the simplicity obtained from the division into trajectory planning and path tracking often comes at the expense of efficiency. The source of the

inefficiency is the trajectory planner. In order to use the robot efficiently, the trajectory planner must be aware of the robot's dynamic properties, and the more accurate the dynamic model is, the better the robot's capabilities can be used. However, most of the trajectory planning algorithms presented to date assume very little about the robot's dynamics. The usual assumption is that there are constant or piecewise constant bounds on the robots velocity and acceleration [19, 22, 23]. In fact, these bounds vary with position, payload mass, and even with payload shape. Thus in order to make the constant-upper-bound scheme work, the upper bounds must be chosen to be global greatest lower bounds of the velocity and acceleration limits; in other words, the worst case limits have to be used. Since the moments of inertia seen at the joints of the robot, and hence the acceleration limits, may vary by an order of magnitude as the robot moves from one position to another, such bounds can result in considerable inefficiency or under-utilization of the robot. As previously mentioned, robot control is usually divided into trajectory planning and path tracking stages. At the lower (tracking) level, a great deal of work has been done, and a variety of methods have been used. One of the earliest tracking schemes was developed for a prosthetic arm by Whitney, and is called resolvedmotion rate control [33]. Resolved motion rate control (RMRC) makes use of the manipulator Jacobian to transform desired velocities in some coordinate system which is natural for the task at hand (e.g. Cartesians) into velocities in joint coordinate space. The velocities are then sent to the individual joint servos. The scheme has the obvious advantage that it allows the robot to be driven in world, rather than joint, coordinates. However, generating the joint velocities requires the inversion of the manipulator Jacobian, which may be time-consuming, and may, if the manipula

tor is at or near a degenerate configuration, be impossible or very inaccurate. Luh, Walker and Paul have extended RMRC, adding resolution of accelerations as well as velocities [24]. The result is resolved-acceleration control, or RAC. The idea here is to generate desired joint accelerations from accelerations in world coordinates, and to use the Newton-Euler dynamics formulation to generate joint torques in real time. The accelerations, however, are still assumed to be "generated in some reasonable way"[24]. In order to utilize the robot fully, the trajectory planner must be aware of the robot's structure and actuator limits, and must take these factors into account. This is very difficult to do in world coordinates. Another approach to path tracking is the use of adaptive controllers. Dubowsky and DesForges used the model-referenced adaptive control scheme (MRACS) to control a six degree-of-freedom manipulator at UCLA [7]. MRACS makes use of a reference model with the desired joint characteristics and a joint servo with adjustable feedback gains. The feedback gains are then adjusted on the fly so as to make the actual joint look like the reference model as much as possible. The results of MRACS appear to be quite good, but the system as presented in [7] is a simple position servo; this makes precise velocity control, as needed for minimum-time control schemes, difficult. It should be pointed out, though, that this is one of the few schemes where the effects of sampling on system stability and performance have been investigated [7], so that more is known about the theory of MRACS than about many other techniques. Koivo and Guo used an auto-regressive discrete-time model for the manipulator dynamics, and minimized a quadratic error measure while estimating the coefficients of the time series with a Kalman filter [16, 17]. Chung and Lee use a Kalman filter

in a different way; they assume that the desired trajectory is given, generate nominal joint torques using the Newton-Euler method, and use a linear model to generate corrections to the a priori computed torques. They then use a Kalman filter to estimate the parameters for the linear perturbation model [4]. All the work described above either calculates joint actuator inputs blindly, i.e., does not take actuator limits into account explicitly, or assumes that accelerations have been given and computes actuator torques from the accelerations. But in order to drive the robot in an efficient way, the dynamics and actuator characteristics of the robot need to be taken into account. This rules out the use of methods such as RMRC. The computed torque methods, combined with an appropriate trajectory planner, provide one means of accomplishing this task, i.e., picking accelerations in a reasonable way. One of the early minimum-time trajectory planning systems was developed by Luh and Walker [23]. It describes the desired manipulator path in terms of its initial and final points and a set of intermediate points. Each branch of the path has a maximum velocity assigned to it, and each intermediate point is assigned a maximum acceleration and a maximum position error. The time taken to go from the initial to the final point is, then, the sum of the times taken to traverse each branch of the path plus the sum of the times required to make the transitions from one branch to the next. The minimum possible sum of these times can then be found using linear programming. In this case, one must still choose appropriate maximum velocities and accelerations, and this cannot be done properly without either knowing the dynamic properties and actuator characteristics of the robot or having some experimental results which give maximum velocities and accelerations for given robot configura

tions. Also, since maximum accelerations and velocities are assumed to be constant over some interval, it is necessary to choose them to be lower bounds of the maximum values over the given interval, i.e., worst case bounds on acceleration and velocity. Since these bounds will in general depend on position and velocity, this could result in under-utilization of the robot's capabilities. Luh and Lin 122] present a modification of the scheme described above which uses nonlinear programming to generate the minimum-time trajectory. The major difference between the method of Luh and Lin and that of Luh and Walker is in a more careful treatment of the calculation of the times required for the transitions from one path segment to the next. Also, an efficient technique for solving the nonlinear programming problem is presented, along with a convergence proof. Lin, Chang and Luh [19] present a third variation on this trajectory planning method. Instead of using path segments which are straight lines in Cartesian space, they convert points in Cartesian space into the equivalent joint coordinates, and pass a cubic spline through these points. The maximum velocities and accelerations are, then, joint velocities and accelerations, which are easier to compute from the robot dynamics and actuator characteristics. There is, however, still some calculation (or measurement) required in order to determine these quantities. This work, incidentally, also allows for limits on the jerk (time-derivative of acceleration). Placing limits on jerk helps prevent mechanism wear. Kim and Shin [12, 13] have presented a method which is similar in some respects to the linear programming methods presented previously, but which uses the robot dynamic equations to obtain approximate acceleration bounds at each corner point. They also point out a set of conditions under which the linear programming

problem reduces to a set of local optimization problems, one for each corner point. This represents a change in computational complexity of from O (n 3) to O (n). Another type of trajectory planner has been developed by Bobrow, Dubowsky, and Gibson [3] and Shin and McKay [29, 30]. In these minimum-time trajectory planners, it is assumed that the desired path is given in a parameterized form. The parametric equations of the path can then be plugged into the manipulator dynamic equations, giving a set of second order differential equations in the (scalar) path parameter. Given these dynamic equations, bounds on individual joint torques can be converted into bounds on parametric accelerations (second time-derivatives of the path parameter); the allowable sets of second derivatives (one set per joint) are intersected, giving a single allowable set. Bounds on velocities (first derivatives of the path parameter) can also be found from these equations, since at some velocities there are no admissible accelerations. Then, using the fact that the minimum-time solution will be bang-bang in the acceleration, it is possible to construct phase plane plots which give the optimal trajectory in terms of the parameter and its derivatives. (The results found in [30] will be presented in Chapter 4.) The papers [3] and [30] differ primarily in two respects: first, in the method of Bobrow et. al. the required search for trajectories is carried out by actually constructing trajectories and seeing where they go; in Shin and McKay, the search has been reduced to the problem of finding a sign change in an easily computable function. Second, some complications may arise with respect to computation of the admissible velocities for a given manipulator position. It is possible that there may be several distinct allowable velocity ranges for a given position. In [3] no mention is made about this possibility, and their search technique may fail if there are distinct

regions. Shin and McKay present a second algorithm to take care of this possibility. While most people have taken the separate trajectory planning/trajectory tracking approach, several authors have made attempts at unified approaches to robot control. One early attempt was the near-minimum time control of Kahn and Roth [11], who linearized the dynamic equations, transformed the equations so as to eliminate coupling terms, and generated switching curves for this linear approximation. The result is a sub-optimal control which seems to give fairly good results if the initial and final states of the robot are fairly close. It does, however, have some problems with overshoot, as one would expect from such an approximation. Another controller using an approximate dynamic model to generate optimal controls is the near-minimum time-fuel method of Kim and Shin [14]. They use a model which is linear over one sample period, and use coefficients in their linear model which result from averaging the coefficients at the current point on the trajectory and at the final point. The controls which result from this approximation depend upon whether time or fuel is the predominant term in the objective function, and upon the sampling rate. The trajectories for minimum fuel were slower than those for minimum time but had less overshoot. Increasing sampling rate also had the effect of reducing overshoot; thus here two parameters could be varied so as to find a good compromise between manipulator speed and overshoot. One level up from trajectory planning is spatial planning, the process of finding collision-free paths for the manipulator to follow. Although the spatial planning problem is still largely unsolved, some work has been done, mostly with objects which are assumed to be spheres, cylinders, or convex polyhedra. Lozano-Perez [20] has reduced the spatial planning problem to two problems: find-space and find-path.

9 The find-space problem amounts to determining "safe" positions for the object being moved, i.e., positions where the object does not overlap any obstacles. Find-path is the process of finding a continuum of safe positions which takes the object from its initial configuration to a desired final configuration. In [20], Lozano-Perez presents algorithms for solving these problems in both two and three dimensions, though the three-dimensional algorithm does not in general give the "best" (minimum-distance) solution. In [21J, Lozano-Perez describes the manipulator spatial planning problem in terms of volumes swept out by the links of the manipulator, and introduces polyhedral approximations of these swept volumes in order to use the results which are available for polyhedra. Luh and Campbell [251 describe spatial planning in terms of obstacles and "pseudo-obstacles" which the manipulator must avoid. The pseudo-obstacles arise because all the links, and not just the payload, must avoid the real obstacles. Luh and Campbell determined the shapes of some of these pseudo-obstacles for the Stanford arm. In particular, they considered the problem of keeping the back end of the Stanford arm's single prismatic link from bumping into things, which has the effect of creating a pseudo-obstacle on the opposite side of the arm from the real obstacle which generates it. They also generated polyhedral approximations to these pseudoobstacles. More recently, Gilbert and Johnson [8] have developed a technique for determining optimal controls in the presence of obstacles. Their technique starts with some feasible path, given as a cubic spline. They then apply a gradient technique which both moves the points interpolated by the spline and changes the robot's velo city in such a way as to optimize some performance measure. Obstacle avoidance is

10 accomplished by including penalty functions in the performance index. Another aspect of the trajectory planning problem is the actual generation of path planners. For those methods which require the manipulator dynamic equations, this means that the equations must be derived. Since this process involves large amounts of symbolic calculation and is therefore highly susceptible to human error, several authors have attempted to mechanize the process of deriving the manipulator dynamic equations. In particular, Bejczy and Paul [1] and Luh and Lin [261 have done work in this area. Bejczy and Paul describe methods for determining when certain dynamic coefficients must be zero for geometric reasons or because of symmetry. Luh and Lin describe a method for manipulating the dynamic coefficients symbolically, and give criteria for eliminating insignificant terms in the dynamic equations. This thesis assumes that the geometric path planner has generated a parameterized curve in joint space, as described in [3] and [30]. The trajectory planning problem can then be reduced to a problem of small dimension by converting all the dynamic and actuator constraints to constraints on the single parameter which is used to describe the path, and the parameter's time derivatives. Within this framework, a variety of optimization techniques can be applied. The optimization methods described here apply to both minimum time problems and to more general minimum cost control problems. It is also possible to modify the constraints to take uncertain dynamics into account at the trajectory planning stage. The remainder of this thesis is structured as follows: Chapter 2 introduces the mathematical nLotation used throughout the following chapters, and gives a derivation of the dynamic model of a manipulator. Chapter 3 formally states the problems to be solved. Chapter 4 presents three related but distinct trajectory planning

algorithms, and gives results of these algorithms as applied to either a simple two degree-of-freedom arm or the first three joints of the Bendix PACS robot arm. Chapter 5 gives some results on selection of near-optimal (in the minimum-time sense) geometric paths. Chapter 6 presents a modification of one of the trajectory planners described in Chapter 4 which allows for uncertainties in the dynamic characteristics of the manipulator which is being controlled. Chapter 7 discusses how trajectory planners can be generated automatically. The thesis concludes with Chapter 8.

CHAPTER 2 MATHEMATICAL PRELIMINARIES Much of the discussion in subsequent chapters involves the dynamic equations of robot arms, and makes use of tensor analysis and Riemannian geometry. This brief introduction should help those who are familiar with the ideas of vector and matrix notation and elementary differential geometry but are unfamiliar with tensor analysis. 2.1. Tensors and Tensor Notation Tensor notation is much like vector notation except that the symbols used in tensor equations may have subscripts and/or superscripts. A vector is written as a symbol with one index, and a matrix will have two. It is possible in tensor notation to write arrays of three or more dimensions; a three-dimensional array simply has three indices. Tensor notation also provides some useful tools for dealing with curves in spaces of an arbitrary number of dimensions. Formally, a tensor is a quantity or set of quantities which obeys certain rules when transformed from one set of curvilinear coordinates to another. The transformation rules are of two types, as indicated by the position of the tensor's indices. Superscripts indicate that the index is contravariant and subscripts indicate that it is 12

13 covariant. A contravariant tensor of order one, or a contravariant vector, transforms from one set of curvilinear coordinates to another (in this case, from unprimed to primed coordinate systems) according to the law N Nq T~i = —- T (2.1.1) j-=1 qJ where N is the dimension of the space under consideration and the q' and q') are the coordinates in the unprimed and primed coordinate systems [32]. Likewise, a covariant vector transforms according to the rule [32] TW~ = --- ~0qJ Tj (2.1.2) Note that the differentials of the coordinates, d qi, transform according to the contravariant rule, though in general the coordinates themselves do not. Writing out the contravariant transformation rule with d q written for Ti and d qi written for T" and dividing by dt shows that velocities are contravariant vectors. A single quantity which retains its value when transformed from one coordinate system to another is called an invariant or a scalar. It is easily verified that the partial derivatives of an invariant with respect to the coordinates transform according to the covariant rule. The generalization to tensors of higher order is straightforward. Tensors of order two can have two subscripts, two superscripts, or one subscript and one superscript. These are called covariant, contravariant, and mixed tensors of order two, and they transform according to the laws

14 r~, = S S aqd T,, (2.1.3) N N aqr aq. Tic (2.1.4) T' =1q - = Dq r=1#=: Dq" aq' T', - - a.q - Tr (2.1.5) The generalization to tensors of order three and higher follows the obvious pattern. An important notational tool is the so-called summation convention. If an index appears twice in a product of two tensor expressions, then the expression is N summed from 1 to N over the repeated index. Thus as b' is shorthand for ai b'. *=1 Using this shorthand, the transformation rules for tensors of orders one and two are written as T 9q'Taq"j aq' (2.1.8) T'ii = 8q, 8q' T. ii @, qT" (2.1.7) aqi aq8# a qI aaq' T = aTr 8O q TT (2.1.8) It is important to note that a given index should not appear more than twice in any term of a tensor equation, and that repeated indices should appear once as a subscript and once as a superscript. If a repeated index appears, for example, twice as a subscript, then the resulting quantity will not in general be a tensor. If, on the other

15 hand, the index appears once as a subscript and once as a superscript, it is easily verified that the expression is a tensor whose character is indicated by the remaining (non-repeated) indices. For example, T,, xi is a covariant tensor of order one; the indices "cancel". 2.2. Riemannian Spaces and Properties of Curves Another advantage of tensor notation is that it is also the language of Riemannian geometry, so that the tools of this field of mathematics are available. As the reader may be aware, Riemannian geometry consists of the study of the metrical properties of spaces of an arbitrary number of dimensions. The space is described by its metric tensor, which gives the square of the differential line element as a quadratic form in the differentials of the coordinates, i.e., da2 = Jij dqidqj (2.2.1) where Jij is the metric tensor and the qi are the coordinates. Jij may without loss of generality be assumed to be symmetric, so that Jii = J ji. It also will be assumed throughout this work that the metric tensor is positive definite, i.e., that Jii x' x > 0 for all x P 0. Geometries in which this is not true can be developed, and indeed are used in general relativity theory, but for the cases dealt with here, Jij will always be positive definite. The introduction of the metric tensor allows distances and angles to be measured, and allows the computation of norms of vectors. Distances along curves are calculated by integrating the formula for the differential line element da. The norm of a contravariant vector is given by the formula

18 II x' II 2 = Ji, x' x (2.2.2) The angle between two vectors is given by the formula Jij Xi yi cosl =- LI x' yl (2.2.3) If Ji is positive definite, then it can be shown that the righthand side of equation (2.2.3) is always between +1 and -1, so that U is a real angle. Even if J is not positive definite, two vectors x and y are said to be orthogonal if and only if Ji, x' y' 0. Note that the expression Jii x' y behaves much as the scalar product behaves in Euclidean space, and indeed reduces to the scalar product in Euclidean space with Cartesian coordinates. The curves in a Riemannian space corresponding to straight lines are geodesics, or curves of minimum distance. The differential equation which describes these curves can be found from the form of the line element ds using variational techniques. Using such techniques, the differential equation obtained is Ji d2q + [jk,A dqJ dqk O (2.2.4) 2 da da The symbol [jk,i] is a Christoffel symbol of the first kind, and is defined by [jki] 2- k + d + (2.2.5) 2 GI~q el q (q It should be noted that these symbols are not tensors. Since the metric tensor Jx is positive definite, it is also invertible. The inverse of this matrix is denoted by Ji), and we have the relationship

17 ij p Ji.. fi=ifi -=k (2.2.8) jjSi __ =( Oifi'k Multiplying a contravariant tensor x' by Ji and summing over j gives a new covariant tensor xi Jij x'. This operation is called lowering a ouffiz. Likewise, a suffix can be raised by multiplying by J'3 and summing. Thus x - Ji" xi. If equation (2.2.4) is multiplied by J"' and summed over i we obtain the equation 2+ d q jk' dq = (2.2.7) dh2 ksJ do do The symbol (m{ m is the Christoffel mrnbol of the second kind, and is defined as (jk} J' [jk,i (2.2.8) As a special case, consider ordinary euclidean space with rectangular cartesian coordinates. Then the metric tensor is just the identity matrix (or Kronecker delta) bi6; and the Christoffel symbols are zero. Reassuringly, equations (2.2.4) and (2.2.7) then reduce to d2q - 0, the differential equations which describe straight lines. ds2 Equation (2.2.7) is often written 6 - d.q'.= 0 (2.2.9) where the operator ~ is called the absolute derivative with respect to s. The absolute derivative of a contravariant tensor of order one with respect to the scalar b is defined as

18 b~ _ dT' {i}Ti dqk 6t = dT+ (;ik }T' id (2.2.10) 6q do do Likewise, the absolute derivative of a covariant tensor is 6T, dT, A. d qk Each of these derivatives consists of the ordinary derivative followed by a term involving a Christoffel symbol. The absolute derivative of a tensor of order two has two terms involving Christoffel symbols, tensors of order three have three additional terms, and so forth. The absolute derivative of an invariant is just the invariant's ordinary derivative. It can be shown that the absolute derivative of a tensor is itself a tensor, unlike the ordinary derivative; this property makes the absolute derivative a useful quantity. In many ways the absolute derivative behaves as an ordinary derivative behaves; it obeys the same rules for derivatives of sums and products, and obeys something like the chain rule. In particular Ad ()A'+ )= -+ F (2.2.12) 6d b6A' 6 Bq + Id, (A'Bi ) = b^A~, Bi + Ai B' (2.2.13) 6, (A'Bi ) 6A BK + Ai B (2.2.14) A' _= A' d99 fiSI (2.2.15) 6q~ 6lD dg

19 A curve in a Riemannian space can be written parametrically as q' f' (X) (2.2.16) where X is some scalar parameter. The derivative of this position vector, d,is the tangent to the curve. In particular, if X is the line element a then it is the unit tangent to the curve. The absolute derivative of the unit tangent,.b ( d ), is the curvature vector. Note that for a geodesic the curvature vector is zero. As in ordinary differential geometry, the curvature vector is orthogonal to the unit tangent. To see this, consider the identity 1 — Ji, pi' p (2.2.17) where p' is the unit tangent dqi. Taking the absolute derivative of both sides of the equation, O' ii piPi' + Jii p' + J i bp (2.2.18) It can be shown that the absolute derivative of the metric tensor is identically zero, so A2 = ii i + Ji 6 pi = 2Ji6P p (2.2.19) Thus the unit tangent and the curvature vector are orthogonal. 2.3. Robot Arm Dynamics In order to control a robot properly, the robot's dynamics must be known. There are a number of ways of obtaining a robot's dynamic equations, the two most

20 commonly used methods being Lagrange's method [9, 28] and the Newton-Euler method [24]. The Newton-Euler formulation gives a set of recursive equations. These recursive equations require relatively few numerical calculations, but are not well suited to use in control problems. Lagrange's equations, on the other hand, express the joint torques/forces in terms of differential equations. Most commonly, these equations are written non-recursively, though recursive formulations do exist [31]. For the problems to be dealt with here, the (non-recursive) Lagrangian form is the = d (aL } -q (2.4.1) where ui is the its generalized force, qi is the i tH generalized coordinate, and the Lagrangian L = K - P = kinetic energy minus potential energy. For a robot arm, or any other system of mechanical linkages for that matter, the kinetic energy K has the form K = - Jjk (q)q' q (2.4.2) where Jj is a symmetric, positive-definite matrix describing the inertia of the robot and the inertial coupling between the robot's joints. (See [28] for a method of obtaining this matrix.) The Einstein summation convention has been used here, as described in Section 2.1. The intdex values range from to N, where N is the number of joints the robot has.

21 The potential energy P is a function of gravity and of the position of the robot arm, so that we can take P = P (q). The value of L is then 1 "'k L — 2Jk (q)q' - P(q) (2.4.3) Computing the partial derivative dL/ dq', o7L I $.. (q3qk 1I a = 12 Jk (q)~q + 2 Jii (q)ql (2.4.4) Making use of the symmetry of J and changing dummy indices, 9L 1 j +'i. (q)q aq' 2Jii (q)q 2Ji (q)q Ji (q)q (2.4.5) Differentiating this with respect to time gives d -L J=.i1i (q)q: + d (Lt j = J,(q)q' + (t qk q (2.4.6) Differentiating L with respect to qk, aL I oDJi,(q) - P (2.4.7) q' 2 q' qq q (2.4.7)' Plugging the results from (2.4.6) and (2.4.7) into (2.4.1) gives ui = Jii(q)q' + t aJi(q)_ 1 aJk (q) qJ + dP(q) (2.4.8) q- 2 Dq' J Dq+ Using the symmetry of Ji,, a simple manipulation of dummy indices shows that the term

22 [ acii (q} I aGik (q) k''' t qt - 2 9qi Iq q (2.4.9) qk 2 cq' q, qk may be written as ~2 _ _ +Qq DJ, (q) _ qJ ) J - - - [jk,i]q qk (2.4.10) The square bracket symbol is the Christoffel symbol of the first kind computed with respect to the metric J,. The dynamic equations of an N-degree-of-freedom robot arm thus take the general form q. = pi (2.4.11) ui Jij (q)vj +Rij vJ + Ciik (q)vj i vk + gi,(q) (2.4.12) where qi = itA generalized coordinate vi = itA generalized velocity ui = it generalized force Ji = the inertia matrix g - =aP gravitational force on the ith joint dqi Ciyk = [jk,i] = array of Coriolis and centrifugal coefficients and R;= - viscous friction matrix.

23 Note the correction for frictional effects, i.e., the introduction of the term involving Rii - As an alternative, the definition of the absolute derivative, equation (2.2.10), may be used to rewrite equation (2.4.12) as 6vj ui = Jij -s + Rjj vi + g (q) (2.4.13) This equation is a tensor equation. To see this, consider each term on the right side of (2.4.12) individually. Jij is known to be a covariant tensor of order two, since the (invariant) kinetic energy is given by Jii v' vI for any arbitrary velocity v'. Since J'i is symmetric, this proves that Jii has the tensor character indicated [32]. Since v' is a tensor, so is at, and hence so is Jii s. Likewise, the power consumed by frictional effects is an invariant, and is given by Rii vi vi. By an argument similar to that used to prove that Jii is a tensor, Rjj must also be a tensor, and hence also Rii vJ. The gravitational term g, is the partial derivative of the potential energy, which is of course invariant, so it is a covariant tensor of order one, as indicated by its single subscript. Since the right-hand side of equation (2.4.12) is a tensor, the left-hand side must also be a tensor. Hence the equation (2.4.12) is in tensor form. This implies that (2.4.12) will have the same form in any system of curvilinear coordinates, and all the quantities in the equation transform according to the tensor transformation laws given in the previous chapter. (Note, however, that these transformation laws break down if the determinant of the Jacobian matrix, 2, is zero.) dq}~

24 In Chapter 5 it will prove useful to express the dynamic equations (2.4.13) in terms of absolute derivatives with respect to arc length a. For our purposes, we will define arc length do by the quadratic form da2- Jij dqi d q. Since the kinetic energy of the manipulator is given by K = 1 J2 i dq' dq' (2.4.14) 2 dt dt (2.4.14) it can be seen that the infinitesimal arc da in this space is related to the kinetic energy of the manipulator by the formula dt = 2K. The dynamic equations may now be expressed in terms of the arc length a and the time derivatives of a. We have, since the absolute derivative obeys the chain rule, u Jj 6v dt + R + (2.4.15) Usingthe relationship v_ - d q ds then _p dt6 ui = Jij PJdt dr + R it d (2.4.16) ~s{ ds dtsdt where p dq is the unit tangent to the manipulator's path. But da 6 rda 6p' da _ da 6.s dtP 6 dt + p 61 d1.dt 6. dt ja (2.4.17) 6pJ d j d da l 6. dt da t dt)

25 since the absolute derivative of a scalar is just the ordinary derivative. Plugging this into the dynamic equations (2.4.16), ~u = Jii tda dt da P + R p t + g, (2.4.18) Using the identity d d d d _d [ d d2a dtd -i _dt J dt dt dt2' 6pi da 2 d2a' s d) + JiPi dt' + Ripi p + gi. (2.4.19) It is interesting to consider the form of the last equation. The left-hand side consists of externally applied forces. There are four terms on the right-hand side: a term proportional to the square of the velocity, a term proportional to the acceleration, a viscous friction term which is proportional to the velocity, and a gravitational term which is a function only of position. The first two terms are of particular interest. They are just the Coriolis and tangential acceleration terms respectively. The Coriolis term is just the (vector) curvature of the path multiplied by the square of the speed, and so has a form analogous to the familiar m! 2term encountered in uniform circular motion. The second term, likewise, looks like the classical ma term one sees in one-dimensional Newtonian mechanics. The most important fact to note is that it is clear from this form of the dynamic equations that the Coriolis terms result directly from the curvature of the path in the manipulator's inertia space. The work W done on the manipulator is

26 W = ui d= q Ui = dd d =fui p'i d. (2.4.20) Plugging in the expression for ui from Eq. (2.4.18), W= J[J,p )a d; + JijPpP dd t i dad + Rijp'pj dedt + gip'. (2.4.21) Using the facts that the curvature vector P — is orthogonal to the unit tangent p' 6s and that p' is a unit vector, i.e., that Jii pi pJ = 1, Eq. (2.4.21) transforms to da d d dd W- idt J d t d + Rip p'pi d + g, pi d )=- (-~J2f ~P' p'd i d| + fgp' d. (2.4.22) The power consumed by the manipulator at any given time is just P-dW d' d 2 + Ri. s + gi d' (2.4.23) dt dt dt 2 St (2.4.23dt'

CHAPTER 3 PROBLEM STATEMENT The goal of automation, as previously stated, 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 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. This important special case will be treated in some detail later. A loose statement of the minimum-cost path planning problem is as follows: What control signals will drive a given robot from a given initial configuration to a given final configuration with as small a cost as possible, given constraints on the magnitudes of the control signals and constraints on the intermediate configurations of the robot, i.e., given that the robot must not hit any obstacles? 27

While the problem of avoiding obstacles in the robot's workspace is not easily formulated as a control theory problem, the problem of moving a mechanical system with minimum cost is. One way to sidestep the collision avoidance problem, then, is to assume that the desired path has been specified a priori, for example as a parameterized curve in the robot's joint space. If this assumption is added, then one obtains a second, slightly different problem statement: 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? This form of the problem reduces the complexity of the control problem by introducing a single parameter 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. In this thesis, the minimum-cost control problem will be divided into two subproblems: 1. 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? 2. Given the solution to problem 1, what curve should be selected, i.e., what curve will give the best (minimum-cost) solution? This thesis will present several methods for solving the first problem, and some approximate methods for solving the second.

29 To state problem 1 more formally, assume that the geometric path is given in the form of a parameterized curve, say q'(X), O<X<X <x (3.1) where qi is the position of the ilk joint, and the initial and final points on the trajectory correspond to the points X=O and X=X-. Also assume that the bounds 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 uEE(q,q) (3.2) where u is a vector of actuator torques/forces, and E:R N XR N -R N is a set function. N is the number of joints the robot has. Given the functions f', the set function E, the desired initial and final velocities, and the manipulator dynamic equations (2.4.11) and (2.4.12), problem 1 is to find the controls u(X) which minimize the cost functional C given by C = f 0(u(X),q(X),q(X))dX (3.3) 0 Problem 2 may be stated in much the same way as problem 1, except that one set of constraints, the set of geometric path constraints, is replaced by a more general set of constraints. These more general constraints state that the robot must not pass through any configuration in which it hits obstacles in its workspace. There will in general be an infinite number of ways in which the robot can do this, even when many obstacles must be avoided in the workspace. In the past, paths have fre. quently been constructed simply by connecting a set of corner points with straight

30 lines and then allowing the path planner to "round off" the corners. But straight lines are not simple motions to produce for most robots, and so are probably not the best paths to choose. It should also be noted that the shortest path may not be the minimum-cost path. In particular, the shortest cartesian path may have one or more corners, and the robot would have to come to a complete stop at these points. If the objective is, for example, to minimize traversal time, then one would almost certainly not want a path with any corners in it. There are several other problems which must be surmounted in order to make a path planning scheme practical. In particular, in minimum-time path planning, one or more joints are driven at maximum torque. If there are any errors in the computed torques, such as those caused by modeling errors, then the path planner may ask the path tracker to use a higher torque than it is capable of generating. The robot will then stray from the desired path. It is therefore necessary to carry out path planning calculations with this in mind, so that any variation in the parameter measurements can be accommodated by the path tracker without saturating the actuators. In other words, if the robot's nominal dynamic equations are given by u= F1(qqq) (3.4) and its actual dynamic equations are given by ui = F2(qqq) (3.5) then it is desired that changes in the required torque due to the error F2 - F1 not result in a torque request which exceeds the capacity of the actuators. This leads to the problem

31 3. Given bounds on the modeling errors for the robot's dynamics, how can the trajectory planning scheme be altered so that the modelling errors do not make the actual required torques exceed the torques which the actuators can provide? Another 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 path 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. This gives rise to another problem, namely 4. Given a robot's dynamic equations, is it practical to generate a path planner for that robot automatically? In summary, the robot path planning problem can be divided into four parts: 1. Trajectory planning: Given a geometric path in joint space expressed as a parameterized curve, actuator torque limits expressed in terms of the robot's position and velocity, the dynamic equations of the robot, desired initial and final velocities, and (possibly) externally-imposed constraints on velocity and jerk (the time-derivative of acceleration), how can the joint torques which minimize a particular cost functional be generated? 2. Generation of geometric paths: How does one generate a geometric path which avoids collisions with obstacles but can also be traversed at low cost? 3. Handling uncertainties: How sensitive will the generated torques be to variations in the robot's dynamic parameters, and how can these variations be taken into account at the trajectory planning stage? 4. Automatic calculation: Can all calculations be handled mechanically, and if so, is there a systematic way of generating a path planner for a given robot? Solutions to these problems are the subjects of the next four chapters.

CHAPTER 4 TRAJECTORY PLANNING The trajectory planning problem is basically an optimal control problem. One possible approach to the solution of this problem is to apply one of the standard tools of optimal control theory, Pontryagin's minimum 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 minimum principle also sheds little or no light on the other auxiliary problems, such as sensitivity to parameter variations. Therefore, we will take a more intuitive but systematic approach. Three trajectory planners will be presented here. The first method will be 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. (Recall that the robot is to move along a geometric path in which the joint positions q' are given by a set of parametric functions, i.e., q' f i'(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 described here 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. 32

33 The phase plane method is in general applicable only to minimum time problems, but this is often the case in which we are most interested. 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 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 appears in the next section of this chapter, including a derivation of the velocity limits and an algorithm for generating the optimal trajectories. Other complications are also discussed. As an alternative, dynamic programming can be used to solve the trajectory planning problem. 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; thus only a 2-dimensional grid is required.

34 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 # value. One starts at the desired final state (the last column of the grid, with the row corresponding to the desired final # value) and assigns that state zero cost. All other states with position Xmax 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 limits on the actuator torques.) The incremental cost is easily computed for minimum time-energy problems, so a running sum can easily be kept for the total cost. Dynamic programming has the advantage that it is a well-established and wellunderstood 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

35 velocity bounds, but in general it would be necessary to either calculate velocity bounds in advance or create a new (larger) grid and 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 will be suggested 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, will be presented. 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, at least in the form in which it is presented here. 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.

38 4.1. Parameterization of the Robot Dynamic Equations Before delving headlong into the trajectory planning problem, the effects of restricting the manipulator's motion to a fixed path will be investigated. In what follows, the manipulator will be restricted to some geometric path q' f'(X), 0< (4.1.1) Plugging this into the dynamic equations (2.4.11) gives an expression for the velocity f =- dX d), dfV =q = - P (4.1.2) dX dt dX d (4.1.2) where pluX is the pcseudo-velocity of the manipulator. Equation (2.4.12), the equation for the joint torque/force vector, becomes s! i d~f j + r2 Jx)d f 2 u = Ji(X) dX P + Ji(X) d 2 (4.1.3) dX dXdX The equations of motion along the curve (i.e., the geometric path) then become X = p (4.1.4) u = J(X) df + J(X) 2 (4.1.5) dX2f +Cdk(X) df 2+ R df dX dX d

37 It is of course assumed that the coordinates q' vary continuously with X. It is also assumed that the derivatives df i and exist, and that the derivatives df dX d 2 dX are never all zero simultaneously. This ensures that the path never retraces itself as X goes from O to Xm. 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 spatial 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 of interpolation technique (e.g. cubic splines) to obtain a similar path in joint space (see [19] for an example). Introducing some shorthand notation, let Mi Ji df' + df i df' dfk dX2 " dX dX Si -gi. We then have us - Mi + Qip2 + Rip + Si (4.1.6) Note that the quantities listed above are functions of X. For the sake of brevity, the

38 functional dependence is not indicated in what follows. 4.2. The Phase Plane Method With the details of curve parameterization out of the way, the phase plane trajectory planning method may now be derived. (This derivation is substantially the same as that found in [30]. ) As was mentioned earlier in this chapter, the phase plane algorithm determines a series of alternately accelerating and decelerating phase trajectory segments. The acceleration and deceleration along these segments are the maximum allowable and minimum allowable values of the pseudo-acceleration p. These values will now be derived. 4.2.1. Derivation of Pseudo-Acceleration Limits Consider the constraints on the inputs, namely umn(q,q) < ui < u<mu(,q). The torque constraints along the parameterized curve can be found by inserting f'(X) for q' and d' p for q'. This gives constraints of the form dX u m1(X,p) < ui < U imX,p). The dynamic equations (4.1.6) can be viewed as having the form U- = 4,(X)p + n,(x, P) where cIt(X) = M5(X) and ni(X,p) = Q,(X)P2 + Rb(X)p + S (X). For a given state, i.e., given X and p, this is just a set of parametric equations for a line, where the parameter is p. The admissible controls, then, are those which are on this line in the input space and also are inside the rectangular prism formed by the input magnitude constraints. Thus the rectangular prism puts bounds on p. The reason for

39 converting from bounds on the input torques/forces to bounds on the pseudoacceleration p is that all the positions, velocities, and accelerations of the various joints are related to one another through the parameterization of the path. Given the current state (X,p), the quantity p, if known, determines the input torques/forces for all of the joints of the robot, so that manipulation of this one scalar quantity can replace the manipulation of n scalars (the input torques) and a set of constraints (the path parameterization equations). For evaluating the bounds on p explicitly, (4.1.6) can be plugged into the inequalities um_' <_ u mi u so that ui0'(x, ) < MAp + Qi p2 + Rip + Si < u P (X,p) (4.2.1.1) If Ml = 0, then these inequalities put no constraints on p. However, the inertia matrix Jij is positive definite, and by hypothesis the derivatives d are not all dXare not all zero simultaneously at any point on the curve. Therefore, df' df' -M.df > dX dX' dX > for all values of X. But then we must have at least one non-zero M;, so that there will always be aome constraint on the pseudo-acceleration. In those cases where Mi 3- 0, manipulation of inequalities (4.2.1.1) gives u i - Q. | - Ri p - Si u - Q, M - Ri p - Si IM,<I gn (Mi) <, (4.2.1.2)

LB; < UB, (4.2.1.3) where umin(M >0) + uMX(M, <0) - ( Q 2+Ri +S ) LBi. (4.2.1.4) and UB_ u(M >0) + u/m'n(M, <0)- (Q, p2+Ri p+Si ) UB, =...... (4.2.1.4) The expression (Mi >0) evaluates to one if Mi >0, zero otherwise. Since these constraints must hold for all n joints, p must satisfy max LB, < i < min UB, or GLB(X,p) < < LUB(, ) (4.2.1.) Note that it has not been assumed here that umin and umax are constants; they may indeed be arbitrary functions of X and p. Later these quantities will be assumed to have specific, relatively simple forms, but these forms should be adequate to describe most of the actuators used in practice. The difference between the trajectory planning algorithm to be presented and those which are conventionally used can be seen in terms of equation (4.2.1.6). Assume that the parameter X is arc length in Cartesian space. Then P is the speed and p the acceleration along the geometric path. Since most conventional trajectory planners put constant bounds on the acceleration over some set of position intervals, one would have GLB(X,r ) < Pm~i < i < Pmx < LUB(X,I,), where Pmi, and Pmax are constants. The conventional techniques, then, restrict the acceleration more than is really necessary. Likewise, constant bounds on the velocity will also be more

41 restrictive than necessary. 4.2.2. Formulation of the Optimal Control Problem With the manipulator dynamic equations and joint torque/force constraints in suitable form, we can address the actual control problem. Problems which require the minimization of cost functions subject to differential equation constraints can be expressed very naturally in the language of optimal control theory. The usual method of solving such a problem is to employ Pontryagin's maximum principle[6]. The maximum principle yields a two-point boundary value problem which is, except in some simple cases, impossible to solve in closed form, and usually is difficult to solve numerically as well. We will therefore not use the maximum principle, but will use some simpler reasoning, taking advantage of the specific form of the cost function and of the controlled system. In the case considered here, minimum cost is equated with minimum time, thus maximizing the operating speed of the robot. The cost function can then be expressed as T='o l dt where the final time t! is left free. It is assumed here that the desired geometric path of the manipulator has been pre-planned, and is provided to the minimum-time controller in parametric form, as described earlier. With this parameterization, there are two state variables, i.e., X and P, but (n +1) equations. One way to look at the system is to choose the equation X=p and one of the remaining equations as state equations, regarding the other equations as constraints on the inputs and on p. However, the problem has a more appealing symmetry if a single differential equation is obtained from the n equations (4.1.6) by multiplying the il equation by dAfx and sum over i, giving

42 df' dXI;~' c+&i p+d, (4.2.2.1) Ui -f mi. Mdf P + Qj df 2 + Ri df'i + S d' (4.2.2.1) X d.X dX dX dX or U = M(X)p + Q(X)p2 + R (X)p + S(X) (4.2.2.2) where, expanding the values of Mi, Qj, Rj, and Si, we have df - d d M(X) - Ji(x) dX d!X (4.2.2.3) R) Rj d dfx (4.2.2.5) S(X) Si() RdX (4.2.2.8) dX df' U(X)luai dX (4. 2. 7) This formulation has a distinct advantage. Note that the coefficient of p is quadratic in the vector of derivatives of the constraint functions. Since a smooth curve an always be parameterized in such a way that the first derivatives never all disappear simultaneously, and since the inertia matrix is positive definite, the whole equation can be divided by the non-zero, positive coefficient of p, providing a solution for p in terms of X and p. Now there are only two state equations, and the original n dynamic equations can be regarded as constraints on the inputs and on p. The M term in (4.2.2.2) is a quadratic form reminiscent of the expression for the manipulator's kinetic energy. In fact, if the parametric expressions for the qi are

plugged into the formula for kinetic energy, one obtains the expression K — 1 M2. The Q term represents the components of the Coriolis and centrifugal forces which act along the path plus the fictitious forces generated by the restriction that the robot stay on the parameterized path. The R term represents frictional components, and S gives the gravitational force along the path. U is the projection of the input torque vector onto the velocity vector. With this formulation, the state equations become X*-# (4.2.2.8) ==41 [U - Q R p- S] (4.2.2.9) The traversal time of the path, T, can be written in terms of X and p as t! XM X~l T - tf = fl.d = A f d d= x f ( d (4.2.2.10) Given these forms for the dynamic equations and the cost function, we have the Minimum Time Path Planning (MTPP) problem as follows. Problem MTPP: Find'(X) and u,'(X,p'(X)) by minimizing T subject to (4.2.2.8), (4.2.2.9), umin(X,p(X))<Ui'(X,'P(X)) _umax(X,p*(X)), 0 < X < Xmx, and the boundary conditions p'(O)=p0 and # (Xm,,)=-p.

44 4.2.3. Phase Plane Plots At this point, it is instructive to look at the system's behavior in the phase plane. The equations of the phase plane trajectories can be obtained by dividing equation (4.2.2.9) by (4.2.2.8). This gives do d' dt p _ i F - l dX= dXt =p = M[u - U -_ rUQ Rp-S] (4.2.3.1) dt Noting again that the total time T that it takes to go from initial to final states is X-C T = I 1 d X, it is easily seen that minimizing time requires that the pseudo0 t* velocity p be made as large as possible, a result which would be expected intuitively. The constraints on p have two effects. One effect is to place limits on the slope of the phase trajectory. The other is to place limits on the value of p. To obtain the limits on LX, one simply divides the limits on is by p, since To get the constraints on p, it is necessary to consider the bounds on p. If, for particular values of X and p, we have LUB(X,p)<GLB(X,p) then there are no permissible values of p. Therefore, for each value of X we can assign a set of values of ip as determined by the inequality LUB(X,p)- GLB(X,p)>O. This inequality holds if and only if UBj (X,p) - LBj (X,p)>0 for all i and j. The intersection of the regions determined by these inequalities produces a region of the phase plane outside of which the phase trajectory must not stray. This region will hereafter be referred to as the admiaaible region of the phase plane. Using the equations for the lower and upper bounds for all i and j,

uPm(Mi >0)+ uPi(M. <0)- (Qpu2 + Rip + S. I Mj~~~~ 8 ~~~~~(4.2.3.2) uM(Mj >O) + Ut?(Mj <O) _ (Qip2 + Rip + S, i >0 M, Rearranging this inequality, Qi Qi ~2 +.i Si A[ - -- ] + Mi Mpi -M M (4.2.3.3) mn..(Mi <o.- ~_(Mj> + [uimx(M <O) - um (M, >0) u "(Mo <O)- u,(M, >0) ]>0 IMA I IMI I It will prove convenient to "symmetrize" the input torque bounds in the discussion which follows. Each joint has a mean torque u and a maximum deviation A' M ax + Umax min given by uM Ai - 2-.. The inequality (4.2.3.3) can 2 2 then be rewritten as [Q Q M8 2 + QM R Mi ]R (4.2.3.4) ML A i, j1 rA,,MI A Mi lA I Mi. ~

At this point, a specific form for the torque bounds will be assumed. If the maximum and minimum torques for each joint are functions only of the states q' * -. and q' (i.e., the actuator torques are all independent of one another) and are at most quadratic in the velocities q', then this inequality yields a simple quadratic in p. This allows one to solve for the velocity bounds using the quadratic formula. A particularly simple and useful special case is that encountered when the actuator is a fixed-field D.C. motor with a bounded voltage input. In this case, the torque constraints take the form uPm =-V' + k',q' and ui V'-=V i, + k q' where V, and V, are proportional to the voltage limits and k,, is a constant which depends upon the motor winding resistance, voltage source resistance, and the back E.M.F. VI + VSd V - Vs generated by the motor. Let V,, = and' = m mi Then, 2 2 we get dX UM= Vgv: + kn q' -= V.e + k (4.2.3.5) From here on, the case outlined above will be used for the sake of simplicity. The only changes required for the more general case of quadratic velocity dependence of the torque bounds is a redefinition of the coefficients in some of the equations which follow. Introducing yet more shorthand notation, let QM (4.2.3.6)

47 ~~[ R R ] | V" df + k df dX c - i MA +..] (4.2.3.8) A~i I Mi I M;Noting that (at least in this case) A,,=-A,,, B,, -B,, C, (4.2.3.9) Noting that (at least in this case) A ii~-Aii, Bii -Bji, Cii Cii, and Dii — Dii, we have the inequalities Aji p2 + Bij p + Cij + Diij 0 (4.2.3.10) -A iijl2 - B,,i + Cij - Dij >O (4.2.3.11) The second inequality is obtained by interchanging i and j and using the symmetry or anti-symmetry of the coefficients. Only the cases where ij4i need be considered, so there are n(n-l) such pairs of equations, where n is the number of degrees of 2 freedom of the robot. If Aii' Bi =O, we have Cii - Dii > 0 and Cii + Dii > O, which are always true if the robot is "strong" enough so that it can stop and hold its position at all points on the desired path. If Aii = 0 and Bii 7 O, then we have a pair of linear inequalities which determine a closed interval for p. If A;i s 0, then, without loss of generality, we can assume that A,, >O. Then the left-hand side of the first of the inequalities (4.2.3.10) is a parabola which is concave upward, whereas for the

48 second it is concave downward. When the parabola is concave downward, then the inequality holds when p is between the two roots of the quadratic. If the parabola is concave upward, then the inequality holds outside of the region between the roots (Figure 4.2.1). Thus in one case p must lie within a closed interval and in the other it must lie outside an open interval, unless of course the open interval is of length zero. In that case, the inequality constraint is always satisfied and the roots of the quadratic will be complex. Since the admissible values of p are those which satisfy all of the inequalities, the admissible values must lie in the intersection of all the regions determined by the inequalities. There are n(n-1) inequalities which give closed intervals, so the inter2 section of these regions is also a closed interval. The other n (n -1) inequalities, when intersected with this closed interval, each may have the effect of "punching a hole" in the interval (Figure 4.2.2). It is thus possible to have, for any particular value of X, a set of admissible values for p which consists of as many as n(n -1)+2 2 distinct intervals. When the phase portrait of the optimal path is drawn, it may be necessary to have the optimal trajectory dodge the little "islands" which can occur in the admissible region of the phase plane. (Hereafter, these inadmissible regions will be referred to as islands of inadmissibility, or just islands.) It should be noted, though, that if there is no friction, then BijO =, which means that in the concave upward case the inequality is satisfied for all values of p. Thus in this case there will be no islands in the admissible region. In addition to the constraints on p described above, we must also have p > O. This can be shown as follows: if p < O, then the trajectory has passed below the line

= -= O. Below this line, the trajectories always move to the left, since p - dX < 0. Since the optimal trajectory must approach the desired final state through positive values of A, the trajectory would then have to pass through p = 0 again, and would pass from p < O to p > O at a point to the left of where it had passed from p > O to p < 0. Thus in order to get to the desired final state, the trajectory would have to cross itself, forming a loop. But, then, there is no sense in traversing the loop; it would take less time to just use the crossing point as a switching point. Thus we need consider only those points of the phase plane for which p > 0. Another way of thinking about the system phase portrait is to assign a pair of vectors to each point in the phase plane. One vector represents the 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. In particular, the angle between the two vectors varies with position. 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, and the condition p >0, determine the admissible region of the phase plane. This is illustrated in Figure 4.2.3. 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.

50 4.2.4. Determination of Optimal Trajectories For illustrative purposes, we first present an algorithm for finding the optimal trajectories for which there are no islands in the phase plane which need to be dodged. The only restrictions, then, will be that p must lie between a pair of values which are easily calculable, given X. The optimal trajectory can be constructed by the following steps called the Algorithm for Constructing Optimal Trajectories, No Ialands (A CO TNI). S1. Start at X=O, P-Po and construct a trajectory that has the maximum acceleration value. Continue this curve until it either leaves the admissible region of the phase plane or goes past =X a Note that "leaves the admissible region" implies that if part of the trajectory happens to coincide with a section of the admissible region's boundary, then the trajectory should be extended along the boundary. It is not sufficient in this case to continue the trajectory only until it touches the edge of the admissible region. S2. Construct a second trajectory that starts at X=Xmax, p=pp and proceeds backwards, so that it is a decelerating curve. This curve should be extended until it either leaves the admissible region or extends past X=O. S3. If the two trajectories intersect, then stop. The point at which the trajectories intersect is the (single) switching point, and the optimal trajectory consists of the first (accelerating) curve from X=O to the switching point, and the second (decelerating) curve from the switching point to X-=-Xm(Figure 4.2.4). S4. If the two curves under consideration do not intersect, then they must both leave the admissible region. Call the point where the accelerating curve leaves

51 the admissible region X1. This is a point on the boundary curve of the admissible region(Figure 4.2.5). If the boundary curve is given by p-=g (X), then search along the curve, starting at X1, until a point is found at which the quantity 0(X)-d dit dg changes sign. (Note that since g (X) determines the boundary of dX dX the admissible region, there is only one allowable value of d-. Also note that if ~~~~dg ~ 9(X) has a discontinuity, -d must be treated as +oo or -oo depending upon the direction of the jump.) This point is the next switching point. Call it Xd. S5. Construct a decelerating trajectory backwards from Xd until it intersects an accelerating trajectory. This gives another switching point (see point A in Figure 4.2.6). S6. Construct an accelerating trajectory starting from Xd. Continue the trajectory until it either intersects the final decelerating trajectory or it leaves the admissible region. If it intersects the decelerating trajectory, then the intersection gives another switching point (see point C in Figure 4.2.6), and the procedure terminates. If the trajectory leaves the admissible region, then go to S4. This algorithm yields a sequence of alternately accelerating and decelerating curves which give the optimal trajectory. Before discussing the optimality of the trajectory, one has to show that all steps of the ACOTNI are possible and that the ACOTNI will terminate. Addressing the first question, S1, S2, S3, S5, and S6 are clearly possible. S4 requires finding a sign change of the function +(X). Since +(X) must be greater than zero where the accelerating trajectory leaves the admissible region and less than zero where the decelerating trajectory leaves, there must be a sign change. Therefore all

steps are possible. In order to prove that ACOTNI terminates, it is must be shown that the search for switching points in step S4 will be performed a finite number of times. To prove this, it is sufficient to prove that the number of isolated zeros of (X), the number of intervals of positive extent over which +(X) is zero, and the number of intervals over which +(X) does not exist are all finite. To do this, some assumption about the form of the functions f'(X) must be made. It will be assumed here that the f' are real valued, piecewise analytic, and composed of a finite number of pieces. (In other words, the f' are analytic splines.) Under these assumptions, the following theorem proves the convergence of ACOTNI within a finite number of iterations. Theorem 4.2.1: If the functions f' are composed of a finite number of analytic, real-valued pieces, then the function q(X) has a finite number of intervals over which it is identically zero and a finite number of zeros outside those intervals. Proof: The inertia matrix, Coriolis array, and gravitational loading vector are all piecewise analytic in the q', and since the f'(X) are analytic in X, the inertia matrix, etc. when expressed as as functions of X (as in Eqs. (4.2.2.3) and (4.2.2.4)) are piecewise analytic and have a finite number of analytic pieces. The functions Mi, Qj, Ri, Si of Eq. (4.1.6) are, therefore, also piecewise analytic. Since a realvalued analytic function with no singularities in a finite interval must either have a finite number of zeros in that interval or be identically zero, the quantities Mi must either be identically zero in the interval considered or have a finite number of zeros. We cannot have all of the Mi zero, for if that were the case we would have AL df =Mdf i -- -O, which is not allowed by hypothesis. If only one of df dfdX' dX

53 the M; is non-zero, then there is no boundary curve to deal with, and so no zeros. With two or more not identically zero, there will be a boundary curve. The curve is given by one of the equations (4.2.3.10) (with ">" replaced by "=") for some pair of indices i and j. Since the coefficients A,B,C, and D in Eq. (4.2.3.10) are analytic except at the zeros of the MA, and because the Mi have a finite number of zeros, we can divide the interval under consideration further, using the zeros of the Mi as division points. Within each subinterval, then, only one of the equations (4.2.3.10) holds. Since Eq. (4.2.3.10) determines # as an analytic function of X within this interval, the bounding curve g (X) is piecewise analytic. The curve +(X), then, is also piecewise analytic and is either identically zero or has a finite number of zeros in each subinterval. Thus, since q(X) either is identically zero in each subinterval or has a finite number of zeros in the subinterval, the number of subintervals is finite, and the number of intervals is finite, the number of zeros and zero-intervals is finite. Finally, the following theorem proves the optimality of the solution generated by the ACOTNI. Theorem 4.2.2: Any trajectory generated by the ACOTNI is optimal in the sense of minimum time control. Proof: Proof of this theorem is straightforward. First, make three observations: (i) From the form (4.2.2.10) of the cost T, there must be some X0 such that the point (X0,,p') on the new trajectory is higher than the point (X0,p) on the ACOTNI trajectory, i.e., p'>p. Otherwise, we would not have a trajectory with a smaller travel time. (ii) The trajectory produced by ACOTNI consists of alternately accelerating and decelerating segments, and can therefore be divided into sections which consist of one accelerating and one decelerating segment. (iii) The admissible

64 portions of these sections which lie above the ACOTNI trajectory are bounded on the left and right by either the line X = 0, the line X = X, the boundary of the admissible region, or the ACOTNI trajectory itself. Now consider the point (X,#') and the trajectory on which it lies. This trajectory, if extended backward and forward from (X0,#t') must intersect a single section of the ACOTNI trajectory at two or more points, since otherwise it would either leave the admissible region or not meet the initial or final boundary conditions. One such point must occur for X < X0 and one must occur for X > X0. But since the accelerating segment of the trajectory precedes the decelerating segment, the new trajectory must either intersect the accelerating part of the ACOTNI trajectory twice, intersect the decelerating part twice, or first intersect the accelerating part then the decelerating part. But since the torques were chosen so as to minimize or maximize U in equation (4.2.3.1), any of these situations leads to a contradiction of a theorem on differential inequalities presented in [18], pp. 41-43. m The whole idea of the algorithm is to generate trajectories which come as close as possible to the edge of the admissible region without actually passing outside it. Thus the trajectories just barely touch the inadmissible region. In practice this would, of course, be highly dangerous, since minute errors in the control inputs or measured system parameters would very likely make the robot stray from the desired path. Theoretically, however, this trajectory is the minimum-time optimum. We are now in a position to consider the general case, i.e., the case in which friction, copper losses in the drive motor, etc., are sufficient to cause islands in the phase plane. In this case, the algorithm is most easily presented in a slightly different form. Since there may be several boundary curves instead of one, it is not possible to

55 search a single function for zeros, as was done in ACOTNI. Thus instead of looking for zeros as the algorithm progresses, we look for them all at once instead, and then construct the trajectories which "just miss" the boundaries, whether the boundaries be the edges of the admissible region or the edges of islands. The appropriate trajectories can then be found by searching the resulting directed graph, always taking the highest trajectory possible, and backtracking when necessary. More formally, the Algorithm for Construction of Optimal Trajectories (A COT) is: SI. Construct the initial accelerating trajectory. (Same as ACOTNI.) S2. Construct the final decelerating trajectory. (Same as ACOTNI.) S3. Calculate the function +(X) for the edge of the admissible region and for the edges of all the islands. At each of the sign changes of +(X), construct a trajectory for which the sign change is a switching point, as in ACOTNI S5 and S6. The switching direction (acceleration-to-deceleration or vice-versa) should be chosen so that the trajectory does not leave the admissible region. Extend each trajectory until it either leaves the admissible region, or goes past Xm,. S4. Find all intersections of the trajectories. These are potential switching points. S5. Starting at X=O, p=p,, traverse the grid formed by the various trajectories in such a way that the highest trajectory from the initial to the final points is followed. This is described below in the grid traversal algorithm ({GTA). Traversing the grid formed by the trajectories generated in S3 and S4 above is a search of a directed graph, where the goal to be searched for is the final decelerating trajectory. If one imagines searching the grid by walking along the trajectories, then one would try to keep making left turns, if possible. If a particular turn lead to

a dead end, then it would be necessary to backtrack, and take a right turn instead. The whole procedure can best be expressed recursively, in much the same manner as tree traversal procedures. The algorithm consists of two procedures, one which searches accelerating curves and one which searches decelerating curves. The algorithm is: AccSearch: On the current (accelerating) trajectory, find the last switching point. At this point, the current trajectory meets a decelerating curve. If that curve is the final decelerating trajectory, then the switching point under consideration is a switching point of the final optimal trajectory. Otherwise, call DecSearch, starting at the current switching point. If DecSearch is successful, then the current point is a switching point of the optimal trajectory. Otherwise, move back along the current accelerating curve to the previous switching point and repeat the process. DecSearch: On the current (decelerating) trajectory, find the first switching point. Apply AccSearch, starting on this point. If successful, then the current point is a switching point of the optimal trajectory. Otherwise, move forward to the next switching point and repeat the process. These two algorithms always look first for the curves with the highest velocity, since AccSearch always starts at the end of an accelerating curve and DecSearch always starts at the beginning of a decelerating curve. Therefore the algorithm finds (if possible) the trajectory with the highest velocity, and hence the smallest traversal

57 time. The proofs of optimality and convergence of this algorithm are virtually identical to those of ACOTNI, and will not be repeated here. Note that in the convergence proof for ACOTNI the fact that there is only a single boundary curve in the zerofriction case is never used; the same proof therefore applies in the high-friction case. 4.2.5. Numerical Examples To show how the minimum-time algorithm works, a numerical example follows. The robot used in the example is a simple two-degree-of-freedom robot with one revolute and one prismatic joint, i.e., a robot which moves in polar coordinates. Despite its simplicity, the example robot is sufficient to show most important aspects of the phase plane trajectory planning method. (More complicated examples are given later.) The path chosen is a straight line. Before applying the minimum-time algorithm, we must derive the dynamic equations for the robot. This requires calculation of the inertia matrix, so masses and moments of inertia of the robot must be given. A drawing of the hypothetical robot is shown in Figure 4.2.7. The robot consists of a rotating fixture with moment of inertia JO through which slides a uniformly dense rod of length Lr and mass Mr. The payload has mass Mp and moment of inertia J, and its center of mass is at the point (z,y) which is Lp units of length from the end of the sliding rod. (The full dynamic equations for this arm are derived in the Appendix.)

In the examples presented here, the robot will be moved from the point (1,1) to the point (1,-1). The equation of the curve can be expressed as r = sec U, where 0 ranges from +-4 to -. Introducing the parameter X, one possible parameterization 4 4 is - -— X r = sec(f -X) (4.2.5.1) Now introduce the shorthand expressions M- Mr + Mp, K M, (Lr + 2Lp ), L 2 and J1 a Je + Jp + Mr (Lp 2 + L, Lp + 3 ). Plugging these expressions and the expressions for the derivatives of r and 0 into the dynamic equations for the polar manipulator gives (see the Appendix for a detailed derivation) u, =-Mt sec(-4 -X) tan(-r -X) p - k, sec( -X) tan( 4-X) (4.2.5.2) 4 4 4 4 + [M sec( -X) sec( 4-X)+ tan2( 4 + M se( )2 + p2 12Mt sec( 4 -X) - K )sec( -X) tan( -X) Solving for p, we have

59 7rCcUr + k c(.-X) tan( -X) Mg sec( -X) tan(- -X) 1 s 4 4 (4.2.5.4) 4 4 _-2 lIMt sec(-) (se -) + sec( - ] 4 4 4 2 4 and u9 + kg ip - P_2 2M sec( -X) - K sec( 4 -X) tan( -X) ---— Ic=~~~~~...................(4.2.5.5) Jt - Ksec(! -X) + Mt sec2( -X) 4 4 The signs of the coefficients of u, and ue are -1 0<k<-! agn (ur) = and sgn (ue) =1 (4.2.5.6) +1 -~<X<The limits on p imposed by the O joint are the same over the whole interval. For simplicity, let u' = -iu for i - r, 0, then the limits are ue,+ {(2M, sec(i'-X) - K) sec( -X) tan( -X) p2 ke ___ 1..... (4.2.5.7) Jt - Ksec(! -X) + Mt sec2( -_X) 4 4 and

60 -u fax + [2Mt sec( -X) -K) sec(E -X) tan( 4 X)I2 ke + 1 4] J~~~~~~~~ ~ (4.2.5.8) Jt - K sec(-! X) + Mt sec2(_f X) 4 4 For the r joint, consider the case when X<-!. Then we also have 4 I ~ -- s r 7r uran (4.2.5.9) Mg sec(- +-X) tan(-! -X) 4 4 + 2M, sec(- -X) tan2( -X) + -! ] and 1C > 1 | (~U rmr (4.2.5.10) Mt sec(-._-X) tan(-X) (4.2.5-10) 4 4 + [2MA sec(f -X) tan2(- -X) + K 2 - k, p sec(t -X) tan( —) 4~-)tn 4R

51 For X> 4 the limits have the signs of ux reversed. Equating upper and lower limits on p gives the boundary of the admissible region. For X<-, Eqs. (4.2.5.8) and (4.2.5.9) give A 2 + Bp + C > 0 (4.2.5.11) where A = -KMf sec( -X) + 2M, sec3( -X) + 3 KM, sec2( -X) (4.2.5.12) 4 4 2 4 K2 7 KJt - (2Mt Jt + - )sec( -X) + 2 4 2 B = (Jt k, - Mg k#)sec(E -,)tan( 4 -X) - Kk, sec2(. 4-X)tan( 4-X) (4.2.5.13) 4 4 4 4 + Mt k, tan( -X,)sec3( -) 4 4 C = ua, (J, - Ksec(- -X) + M, sec2( -_ )) (4.2.5.14) 4 4 + u z, M, tan(-4 -) sec(. -X) 4 4 Likewise, Eqs. (4.2.5.7) and (4.2.5.10) give -A C2 - B + C > O (4.2.5.15) The same inequalities, with u negated, work when X >. -4

82 Finally, we need to determine the differential equations to be solved. These equations are 1. Jt - K sec(X)+ Mt sec4( [_X) (4.2.5.16) 4 4 - u, sec(E -X) tan( —-X) + 2 M sec4(.f -X) tan( + -X) 4 4 4 4 - (k tan2(f -X) sec2( f -X)+ k, j] _X =. (4.2.5.17) The numerical values of the various constants which describe the robot are given in the Appendix in Table A.1. Using these data, the differential equations were solved numerically using the fourth-order Runge-Kutta method, the program being written in C and run under the UNIX' operating system on a VAX-11/7802. The derivative of the boundary curve g(X) (needed to compute the function +(X)) was calculated numerically, and the sign changes of +(X) found by bisection. The graphs of the resulting trajectories and of the boundary of the admissible region are given in Figure 4.2.8 for the zero-friction case and in Figures 4.2.9 and 4.2.10 for the high-friction case. UNIX is a trademark of Bell Laboratories. 2VAX is a trademark of Digital Equipment Corporation.

63 Note in particular the shape of the admissible region boundary in Figure 4.2.9. For values of X less than about 0.42 there is not a single range of admissible velocities, but two ranges. Thus there is an "island" iu the phase plane, though the island is chopped off by the constraint that X be positive. While the existence of such islands may at first seem to defy intuition, the example shows that they do indeed exist. In this case, the island does not really come into play in the calculation of the optimal trajectory. Nevertheless, the example does demonstrate that there may be situations where the admissible region has a fairly complicated shape. Since most practical manipulators have more than two joints and have more complicated dynamic equations than those of the simple robot used here, it is conceivable that the admissible region of the phase plane for a practical robot arm could have quite a complicated shape. As a final example, to demonstrate clearly the existence of islands in the phase plane, we include a sketch of the admissible region of the phase plane for a twodimensional Cartesian robot moving along a circular path. In this case, the dynamic equations are a simple pair of uncoupled, linear differential equations with constant coefficients, i.e., us = m + ks z, uX = -my + k y where m = mass of z and y joints, k, a coefficient of friction of z joint, and k., coefficient of friction of y joint. Moving this manipulator in a unit circle, say in the first quadrant, requires that z = cosX, — = sinX, 0 < X < I (4.2.5.18) Plugging these expressions and their derivatives into the dynamic equations gives

64 u, = -mp sinX - m p2 cosX - ks, sinX (4.2.5.19) uy - m p cosX - m p2 sinX + ky p cosX (4.2.5.20) Now let the torque bounds be - T <u,u < + T. Then the bounds on p are -T - mp2 cosX\ - k, p sinX. +T - m2 cos - k, sinX 4.2.5.21) m sinX m sinX and -T + m p2sin - k pcos +T +mp2sinX-k, pcosX <p<< < CS (4.2.5.22) m cosX m cosX The admissible region consists of the region where the inequalities given above allow some value of the acceleration p, as previously described. Simplifying the resulting inequalities gives the admissible region as that area of the phase plane where m p2 + (ks - k, )p sin) cosX + T(sinX + cosX) > 0 (4.2.5.23) and -m p2 - (ks - ky )p sin) cosX + T(sinX + cosX) > 0 (4.2.5.24) Using the values m =2, ks =0 kV =10, and T c=V gives the region plotted in Figure 4.2.11 and clearly shows the island.

65 +4j L2 + Bij y+ Cij, Dij Inadmissible values of A t -A Lp B-i + C- Dj Admissible values of, Figure 4.2.1. Admissible regions of p determined by parabolic constraints

88 Constraint 1 l l ~ t' ~~~~~Constraint 2 I' I Constraint 3'~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~, I'' I I Constraint 4!~~ ~ ~~~~~~ I I I I~~~~~~~~~ I! l lI l ~ ~~~~~~~~~~ l intersectionof constraints Figure 4.2.2. Intersection of admissible regions of p

67 A = 0 I Amax Upper arrows give maximum accerleration Lower arrows give maximum deceleration Dashed curve is boundary of admissible region Figure 4.2.3. Acceleration and deceleration vectors at each phase point

68 p Boundary Accelerating' Decelerating Curve I Curve = 0 _ Switching Xmax Point Figure 4.2.4. Case when accelerating and decelerating curves intersect

69 Admissible Region Boundary I~~~~~~~~~~~~~ ~~~~~~~~~~I I~~~~~~~~~~~ ~~~~~~~~~~~I I~~~~~~~~~~~ I ~~~~~~~~~~~~I I I I A -- 0 x1 A2 Amax Figure 4.2.5. Case when accelerating and decelerating curves do not intersect

70 A X B X2C | max (a = xd) Accerlerate Decelerate Accelerate Decelerate A, B, and C are switching points B is a point of osculation between g(X) and the trajectory Figure 4.2.8. A complete optimal trajectory with three switching points

71 Figure 4.2.7. The two degree-of-freedom polar robot

72 0.8 0.6 0.4 A o0.6 1.2 C Boundary curve Optimal trajectory.. a Inadmissible region A,B,C Switching points Figure 4.2.8. Optimal trajectory, zero friction case

73 3.0 / 2.0 - 1.00.0 0 0.4 0.8 1.2 -....- Boundary curve g,7// Inadmissible region Figure 4.2.9. Admissible region, high-friction case

74 0.6 / i/ 0.4 / 0.2 0.0 X 0 0.6 A B 1.2 X Boundary curve Optimal trajectory Vz/zJ Inadmissible region A,B,C: Switching points Figure 4.2.10. Optimal trajectory, high-friction case

1. 25 I MSIL EIN| INADMISSIBLE REON IADMISSIBE REGION 2.0 1 5 INADMISSIBLE ISLAND 0].5. 0 I F f 0.2 0.4 0.6 0.8 1.o 1 4 1 6 Figure 4.2.11. Admissible region for cartesian manipulator

4.3. The Dynamic Programming Method The algorithms presented in the previous section are adequate for solving the minimum-time trajectory planning problem, provided that the actuator torque limits do not depend on the joint velocity in a complicated way and are independent of one another. However, in situations where driving the robot consumes large amounts of power, the assumption that minimum time is equivalent to minimum cost may not be valid; then the phase plane algorithm gives a solution to the wrong problem. Interdependence of actuator torque constraints is another very real possibility that the phase plane method cannot handle. This interdependence may occur, for example, when a robot uses a common power supply for the servo amplifiers for all joints, or when all joints of a hydraulic robot are driven from a common pump. Finally, it is assumed that the joint torques can be changed instantaneously. This is only approximately true, and indeed it is sometimes desirable to limit the derivatives of the joint torques (or, equivalently, the jerk, or derivative of the acceleration) to prevent excessive mechanism wear. One means of eliminating these limitations is to use a more general optimization technique. The method proposed here is to use dynamic programming [15] to find the optimal phase plane trajectory. The dynamic programming technique places few restrictions on the cost function that is to be minimized. Putting limits on jerk is also (theoretically) possible, and interdependence of torque bounds can be handled fairly painlessly, as will be seen later. One of the major drawbacks of dynamic programming, the "curse of dimensionality", is not an issue in the trajectory planning problems considered here, since the se of the parametric functions (4.1.1) reduces the dimension of the state space from 2n for an n -jointed manipulator to two.

77 4.3.1. Problem Formulation As before, the manipulator is assumed to move along a path given by (4.1.1), and the dynamics are written in the form of (4.1.8). Initially, it is also assumed that the set of realizable torques can be given in terms of the state of the system, i.e., in terms of the robot's position and velocity. Then we have u = (u,u2, ~ M.,u, )r E E(q,q) (4.3.1.1) where q is the first derivative of q with respect to time, and ui is the is actuator torque/force. E is a function from R" X R" to the space of sets in R ". In other words, given the position and velocity, E determines a set in the input space. The input torques u, are realizable for position q and velocity q if and only if the torque vector u is in the set E(q,q). Note that independence of the actuator torque limits is not assumed. If limits on the derivatives of the joint torque (or, equivalently, derivative of the acceleration, or the jerk) are also to be applied, then we also must satisfy the inequalities u, j < K,(Xl,p) (4.3.1.2) The cost C given by (3.3) may be transformed into C = L (X,p,u) d X (4.3.1.3) The minimum-cost trajectory planning problem then becomes that of finding p= ~'(X) which minimizes (4.3.1.3) subject to Eq. (4.1.8) and the inequalities (4.3.1.1) and (4.3.1.2).

78 4.3.2. Trajectory Planning Using Dynamic Programming To see how dynamic programming can be applied to this problem, first note that by using the parameterized path (4.1.1), the dimensionality of the problem has been reduced; there will be only two state variables X and p, rcgardlea. of how many joints the robot has. The "curse of dimensionality" has therefore been avoided. To apply dynamic programming, one first must divide the phase plane into a discrete grid. Then, the cost of going from one point on the grid to the next must be calculated. Note that since u8 will be determined as a function of X and A, Eq. (4.3.1.3) can be written strictly in terms of X and p; thus the cost computation can be done entirely in phase coordinates. Once costs have been computed, the usual dynamic programming algorithm can be applied, and positions, velocities and torques can be obtained from the resulting optimal trajectory and Eqs. (4.1.1) and (4.1.6). The informal description given above describes the general approach to the MCTP problem. In detail, there are some complications. Therefore, some simplifying but realistic assumptions will be made as we proceed. First, the grid's X-divisions are assumed to be small enough so that the functions Mi,, Qi, Ri, Si, and dA do not dX change significantly over a single X-interval. Then, the coefficients of Eq. (4.1.6) are effectively constant. So are the coefficients of (4.2.3.1), which we will use as our (single) dynamic equation. (Other assumptions, such as piecewise linearity, are possible. However, these assumptions complicate the analysis considerably.) Note that (4.2.3.1) does not explicitly depend on time. Therefore, for purposes of carrying out the dynamic programming algorithm, we may treat the quantities X and p as a stage variable and a single state variable rather than two state variables. Using (4.2.3.1) as our (single) dynamic equation, and noting that M, Q, R, and S are

79 approximately constant over one X-interval, we need to find a solution to (4.2.2.2) which meets the boundary conditions p(Xk )=o0, P(X\k+1)=P1 (4.3.2.1) in the interval [ Xk, Xj\+1 (see Figure 4.3.1). In order to do this, some form for the inputs u, needs to be chosen. It should be noted that as the DP grid becomes finer, the precise form of the curves joining the points of the grid matters less. As long as the curves are smooth and mrLc.4onic, the choice of curves makes a smaller and smaller difference as the grid shrinks. The implication of this is that we may choose virtually any curve that is convenient, and as long as the grid size is small, the results should be a good approximation to the optimal trajectory. We will use the form u, - Q, 2 + Rip + Vi (4.3.2.2) for the input, where the Vi are constants that may be chosen to make the solution meet the boundary conditions (4.3.2.1). Form (4.3.2.2) was chosen because it yields particularly simple solutions. In what follows, we first obtain a solution without torque bound interaction, and then extend the solution to accommodate torque constraints of a much mniore general type. 4.3.3. Case of Non-Interacting Torque Bounds When the joint torque bounds do not interact, the sets E in (4.3.1.1) are given by

80 E(q,q) = {(u,'',u.,) jutn(qq) < ui < u.(q,q)} (4.3.3.1) Taking the projection of the input torque vector, as given by (4.3.2.2), onto the velocity vector dfA gives U = Q p2 + R p+ V, where V= Vi d Plugging dX dX this into the differential Eq. (4.2.3.1) gives MdA + Qp+R + I(S Q2-Rp- V)=O (4.3.3.2) or dp _ =-1 (S - V) ~~~d ~ ~~~~~~~X # ~~~~M ~(4.3.3.3) dX a M Solving this equation, we have x = K - M) (4.3.3.4) 2(S-V) Evaluating the constant of integration K and the constant V so that (4.3.3.4) meets tthe boundary conditions (4.3.2.1), one obtains X ( 2_-p2) + Xt+,(p2_lo2) X 2 2 -k * (4.3.3.5) Solving for p in terms of X gives v(Xk -X_,)p2 + (X _ Xkt )P p —~ k {1 4.3.3.6)

81 Now that the path is known over one X-interval, we need to know the inputs u, and the components of the incremental cost. To evaluate the input torques, we may use Eq. (4.1.6) and the value of p. Notinc that'P dp and using Eq. (4.3.3.3), we obtain P dX p (V-S) = constant. (4.3.3.7) M The quantities A and S are given, and, using Eqs. (4.3.3.0) and (4.3.3.7), V can be calculated to be V=S+ M1 -Pi X (4.3.3.8) 2 Xk- +Xk which gives 2 2 2( +1 - d) (4.3.3.9) Therefore, the equations for ui become,2 2 ui = Q p2 + Ri p + Si + M 2(x ) (4.3.3.10) Assuming the joint torque limits are independent, determining whether joint i ever demands any unrealizable torques requires that we know the maximum and minimum values of u, over the interval [Xk,X\+11 (or equivalently over the interval [min(po,pL), max(pop1), since X is a monotonic function of, over the interval under consideration). The maxima/minima may occur at one of three p values, namely p0,

82 pl, and that value of p that maximizes or minimizes us over the unrestricted range of p. In the latter case, the value of p is Ri 1#= -— 2Q, (4.3.3.11) If the condition min(po,pl) < p. <_ max(po,pl) (4.3.3.12) holds, then the point p, needs to be tested. Otherwise the torques must be computed and checked only at the endpoints of the interval. (If Mi, Q,, R,, and Si were assumed to be piecewise linear in X, then the formula analogous to (4.3.3.10) would be a quartic rather than a quadratic, and in theory three "midpoints", found by solving a cubic, would have to be tested. But as a practical matter, testing the endpoints of the interval is probably adequate.) Given the formulae for the velocity and the joint torques, the incremental cost can be found using the formula Xk +x C- f L (X,p,uj)dX (4.3.3.13) where p and ui are given as functions of X by formulae (4.3.3.6) and (4.3.3.10), respectively. It may be possible to evaluate this integral directly; if not, then the integral may be approximated by any of the standard techniques. Section 4.3.8 shows that the DP algorithm converges when the integral is approximated using the Euler method. Using more sophisticated algorithms should give faster convergence than the Euler method.

With these formulae at hand, it is now possible to state the dynamic programming algorithm in detail. Initially, the algorithm will be stated for the case in which there are no limits on the time derivatives of the torques. These constraints will be considered later in Section 4.3.5. The algorithm, given the dynamic equations (4.1.6), the equations of the curve (4.1.1), the joint torque constraints (4.3.3.1), and the incremental cost (4.3.3.13), is: S1. Determine the derivatives ddf of the parametric functions f'(X), and from these quantities and the dynamic equations determine the coefficients of Eqs. (4.1.6). S2. Divide the (X,p) phase plane into a rectangular grid with Nx divisions on the X-axis and N, divisions on the p-axis. Associate with each point (X,,p, ) on the grid a cost C,,,,, and a "next row" pointer P,,,. Set all costs C,,,,, to infinity, except for the cost of the desired final state, which should be set to zero. Set all the pointers P,, to null, i.e., make them point nowhere. Set the column counter a to Nx. S3. If the column counter a is zero, then stop. S4. Otherwise, set the current-row counter X to 0. S5. If B = N,, go to S12. S6. Otherwise, set the next-row counter y to 0. S7. If - = N,, go to S1. S8. For rows, and %, generate the curve that connects the (a-l,B) entry to the (a,T) entry, as in Figure 4.3.2. For this curve, test, as described in the previous paragraphs, to see if the required joint torques are in the range given by

inequalities (4.3.3.1). If they are not, go to S10. S9. Compute the cost of the curve by adding the cost Ca. to the incremental cost of joining point (a-1,9) to point (a,-y). If this cost is less than the cost Ca-1,0, then set Ca,-,, to this cost, and set the pointer Pca1-, to point to that grid entry (a,y) that produced the minimum cost, i.e. set P, -l, to y. S10. Increment the next-row counter ^y and go to S7. S11. Increment the current-row counter 9 and go to S5. S12. Decrement the column counter a and go to S3. Finding the optimal trajectory from the grid is then a matter of tracing the pointers P.. from the initial to the final state. If the first pointer is null, then no solution exists; otherwise, the successive grid entries in the pointer chain give the optimal trajectory. Given the optimal trajectory, it is then possible to calculate joint positions, velocities, and torques. 4.3.4. Case of Interacting Torque Bounds It has been assumed in the preceding discussion that the joint torque limits do not interact, i.e., that increasing the torque on one joint does not decrease the available torque at another joint. This assumption manifests itself in the form of the torque constraint inequalities (4.3.3.1). This assumption is probably correct in many cases, but in others it certainly is not. Consider, for example, a robot that has a common power supply for the servo amplifiers for all joints. The power source will have some finite limit on the power it can supply, so that the sum of the power consumed by all the joints must be less than that limit. A similar situation arises when

85 a single pump drives several hydraulic servoes. The pump will have finite limits on both the pressure and the volume flow it can produce. In fact, it may be desirable to have torque limits interact, in the sense that using a single power source to drive the robot may cost less than using several independent power sources. In most cases multiple power sources would not be used at their maximum capacities simultaneously, so that there would be little to be gained by having independent power sources, while the cost of a single large power source would quite possibly be considerably less than that of several small ones. Because of the possibility of torque limit interaction, it will be assumed here that the inequalities (4.3.3.1) are replaced with the constraint (4.3.1.1), namely (u1,u2, **,u, )T E E(q,q). It is interesting to note that the limits described in the previous paragraph all produce set functions E in Eq. (4.3.1.1) which are convex. For example, if the sum of the power consumed (or produced) in all the joints is bounded, one obtains the bounds Pmi. u q' < Pm=. (4.3.4.1) For any given velocity, this is just the region between a pair of parallel hyperplanes in the joint space. Likewise, for independent torque bounds, the realizable torques are contained in a hyper-rectangular prism, another convex region. Since the intersection of any number of convex sets is a convex set, any combination of these constraints will also yield a convex constraint set. In this light, it is reasonable to make the assumption that the set E(q,q) is convex. This assumption is important in the analysis that follows.

80 To see how we may make use of this convexity condition, consider the test for realizability of torques used in the method presented thus far. This test made explicit use of the assumption that the torque bounds do not interact. In order to handle interacting torque bounds using an approach like that of Section 4.3.3, it must be possible to determine whether all torques are realizable over any given X-interval. If the torques have the form used in Eq. (4.3.2.2), then this is in general not possible with any finite number of tests; even in the two-dimensional case, the torques trace out conics in the input space, and there is no general way to determine whether a segment of a conic is entirely contained within a convex set. Though the question of whether a set of torques is realizable cannot in general be given a definite answer, the realizability question can be answered in some cases. To see how this can be done, consider again the tests for realizability previously described. The maximum and minimum torques for each joint are determined, and these torques are checked. While Eq. (4.3.2.2) describes a curve in the joint torque space, the individual torque limits describe a box-shaped volume. The curve describing.the joint torques will be entirely contained inside this box. Thus if every point in the box is admissible, then so is every point on the curve. This "reduces" the problem of determining whether every point of a one-dimensional set is realizable to the problem of determining whether every point of a higher-dimensional set is realizable. However, this higher-dimensional set has a special shape; it is a convex polyhedron, and will be contained in the (convex) set E if and only if all its vertices are in E. Thus by testing a finite number of points, the question of whether a particular set of torques is realizable may sometimes be given a definite "yes" answer.

87 If this test does not give a definite answer, then the set of inputs in question must be discarded, even though that set may in fact be realizable. However, as the grid size shrinks, the size of the bounding box for the torques also shrinks, so that in the limit the test becomes a test of a single point. Therefore as the grid shrinks, the percentage of valid torques thrown away approaches zero, and the optimal solution will be found. This method of handling interacting torque bounds requires only one change in the DP algorithm. Step S8, which checks to see if the torques are realizable, must be replaced with a step that generates all corners of the bounding box and tests these points for realizability. If any of the corners does not represent a realizable set of torques, then the test fails. Thus we have S81. For rows f and 7, generate the curve that connects the (a-1,B) entry to the (a,') entry, as in Figure 4.3.2. For this curve, generate the maximum and minimum torques at each joint. Check each torque n-tuple formed from the maximum and minimum joint torques. (These are the corners of the bounding box.) If any of these n-tuples are not contained in the set E, then go to S10. 4.3.5. Accommodation of Jerk Constraints The methods described thus far have ignored the jerk constraints (4.3.1.2) which limit the derivatives of the joint torques. Taking these limits into account effectively requires that a third state variable be added. That variable can be taken to be the pseudo-acceleration p, say Cp. Differentiating the equation for the torque, one obtains

88 u = Miv + Mv+ + 2+2Qi v + Rip + Ri v + Si. (4.3.5.1) Using the identity = d = this equation becomes dt dX dt d X dMi 2"'Q )vRi dQ5 dR, dS, UsAf-1/+v dX+ 2j I + i qv + -i |i + 7 + 7 (4.3.5.2) d X d X d'X d'X If there are no jerk constraints, then the parameter p in Eq. (4.1.6) can be manipulated as needed. When there are jerk constraints, we must instead manipulate v in Eq. (4.3.5.2). Eq. (4.3.5.2) and constraints (4.3.1.2) then give constraints on v, just as Eq. (4.1.6) and constraints (4.3.1.1) yield constraints on p. To solve the optimization problem with jerk constraints using dynamic programming, a three-dimensional grid is required, with one dimension for each of X, p, and v. Some form must be assumed for the "inputs" ui, as was done for ui when there were no jerk constraints, i.e., Eq. (4.3.2.2). Because the grid points that the DP algorithm must join form a pair of planes, rather than a pair of lines or columns, as in the two-dimensional case, the form of the input must contain two arbitrary constants instead of one. If only one parameter is used, then it will not be possible to connect arbitrarily chosen points in the DP grid. The problem is thus inherently more complicated than the two-dimensional case, at least in terms of the algebra required to produce a solution. The procedure is otherwise the same as that for the two-dimensional case. Because of the algebraic complexity and because of the computer time that would be consumed by a three-dimensional dynamic programming algorithm, no examples were worked for this case. However, section 4.4 presents a minimum time

89 algorithm which handles jerk constraints in a much more satisfactory manner. 4.3.6. Algorithm Complexity The usefulness of the dynamic programming technique depends on its being reasonably efficient in terms of use of computing resources, i.e., it must run reasonably fast and must not use too much memory. Since the trajectory planning is done offline, the algorithm's time requirements are not particularly critical; nevertheless, the time required must not be exorbitant if trajectory planning is to be worthwhile. Likewise, computer memory is relatively inexpensive, but nevertheless puts some limits on the accuracy with which the dynamic programming algorithm can be performed. In this section we present an approximate analysis of the time and memory requirements of the algorithm. Of course, precise numbers will depend rather heavily upon such variables as the computer on which the algorithm is to run, the language in which it is implemented, the compiler used, and the skill of the programmer who writes the code, so the expressions derived here contain a number of implementation-dependent constants. It is easy to compute the storage requirements for the algorithm. The memory allotted to the program itself is essentially fixed. The size of the grid used for the dynamic programming algorithm varies with the fineness of the grid and the amount of storage required per point on the grid. The grid has NO rows and Nx columns. Each entry must contain a cost C and a pointer P. The size of an entry will then be GS = Sc + S, (4.3.6.1)

90 where GS is the storage requirement for a single point of the grid and S, and S. are the amounts of storage required to record the cost and the pointer to the next row, respectively. In the implementation presented here, parameterized curves are represented as arrays of points. If one assumes that there is one point per X-division, then there is an additional Sd + NXS,, where Si is the storage required for one interpolation point on the curve and Sd is a certain fixed storage per curve. Multiplying GS by the number of grid entries and adding the amount of storage PS required for the program and the storage required for the curve gives total storage TS as TS = PS + NX N, (SC + Sp) + NS, + Sd. (4.3.6.2) For the numerical example presented in this paper, all arithmetic was done in double precision, and integers and pointers are four bytes long. Then for a six-jointed arm the storage required is, ignoring the program storage, TS = 12NxNp + 80 + 448Nx (4.3.6.3) For example, a 20X80 Arid requires 28,240 bytes. This can, of course, be reduced considerably by using single rather than double precision; however, even using double precision, the storage required is generally available on small microprocessors. Calculating the time required to perform the dynamic programming algorithm is somewhat more difficult. There will be Nx - 1 steps, where each step requires testing to see if each of the NV points in one column can be connected to each of the NV points in the next column. Each test must be done, but some of the tests are simpler than others. If the cost at the next grid point is infinite, then there is no point in

91 doing any further calculations. If on the other hand the cost is finite, then input torque bounds must be checked, and if the input torques are admissible, then costs must be calculated and compared. Though actual computation times will vary with the particular problem being solved, the way the time varies with grid size can be roughly determined. To get a bound on this time, assume that all the tests and computations must be performed. Then each step of the dynamic programming algorithm requires KN2 seconds, where K is a quantity which depends upon the computer being used and the number of joints the robot has. There are Nx- 1 such steps, so the time required is less than K(Nx - 1)N2. In other words, the execution time is roughly proportional to the cube of the grid density. In practice, the value of the constant K must be evaluated experimentally. This has been done for the numerical example in Section 4.3.9, which does indeed show a time dependence proportional to (Nx - 1)N2. The dependence of execution time on the number of joints n, i.e., the dependence of the constant K on n, is more difficult to assess. K in the equation above depends on both n and the representation used to describe the curve to be traversed. The functions Mo and R, depend on the matrices Ji3 and Rii respectively, and the Coriolis term Qj depends on the three-dimensional array Ct -h. In general, then, it might be expected that the evaluation of the function Qj might take time proportional to the cube of the number of joints. (See, for example, [15].) In any case, the time required for evaluation of the dynamic coefficients is heavily dependent upon the configuration of the robot. Fortnuately, in practical cases the number of joints would usually be no more than six, and almost certainly would be less than eight. Since these functions only need to be evaluated once per X-division of the DP

92 grid, their evaluation will probably be only a minor part of the total time consumed. This being the case, the dependence of execution time on n is not an important factor. (For the numerical example considered here, this is certainly true.) If the algorithm for handling interacting joints is used, then the dependence of the time on the number of joints increases exponentially with the number of joints, since there are 2' corners on the bounding box for the input. While this would seem to make the algorithm useless, it should be noted that the size of the bounding box decreases as the grid size shrinks. In practice it may be sufficient to test, for example, the endpoints of the current segment, rather than all 2" corners. 4.3.7. Algorithm Speedup Even though solution of the trajectory planning problem by dynamic programming requires only a two-dimensional grid, the algorithm uses large amounts of computer time when the grid gets fine enough to give accurate answers. Part of the reason for this is the exhaustive testing of paths in the grid; when connecting points in one. column to points in the next, all pairs of points are tested. Also, the dynamic programming algorithm generates the optimal trajectories from the all points in the grid to the desired goal state. If we can avoid generation of the unused trajectories, considerable speedup should result. The approach described here involves multiple iterations of the dynamic programming algorithm using a sparse, irregularly spaced grid. Suppose that an approximate solution to the trajectory planning problem is available, say as a result of using dynamic programming with a coarse grid. Then we may plot this approximation in the phase plane, and draw a "swath" around it, indicating the uncertainty of the

93 solution. Then, instead of superimposing a grid on the entire phase plane, we may superimpose a grid on the uncertainty swath. This grid may have a small, fixed number of i values for each X value, so that the cost of doing dynamic programming on this grid is relatively low. (Figure 4.3.3) If the original trajectory met all the constraints, and for each X value on the grid one of the grid points is on the original trajectory, then a solution will certainly be found when the dynamic programming algorithm is performed. Assuming that the grid includes the points on the upper and lower limits of the swath, the resulting trajectory then must either stay entirely within the swath, or touch the swath's edge. Now a new swath should be drawn, centered on the new optimal trajectory. If the new optimal trajectory touches the edge of the old swath, then the new swath should have the same grid density. If it doesn't touch the edge, then the size of the new swath should be decreased. This process may be repeated until the swath is narrow enough to guarantee that the solution is within desired accuracy limits. Roughly speaking, the algorithm finds an approximate solution in a reduced search area. If the solution touches the boundary of the search area, then the search area boundary is moved away from the solution. If the solution does not touch the edge of the search area, then a new smaller search area is tried, resulting in a more accurate approximation. By limiting the dynamic programming algorithm's attention to a small area of the phase plane, computation times are kept correspondingly small. This technique will not be used in any of the examples worked in this section, but a related technique will be described in the Section 4.4.

94 4.3.8. Convergence Properties The previous section describes the complexity of the DP algorithm. It is obvious from the discussion that the fineness of the DP grid will have a significant impact on the running time of the algorithm. It will also affect the accuracy of the results. This section describes the effect of the grid density on the accuracy of the DP solution in a quantitative manner. Bellman proved in [21 that discrete approximations to a continuous optimal control problem will converge (in a sense to be defined) as the step size of the DP 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 assumes that the dynamic equations of the system are not functions of the stage variable, which is the same as X in this paper. Here we prove a theorem which is an extension of that of Bellman in that it allows the dynamic equation and cost function to be (possibly discontinuous) functions of the stage variable. The proof presented here also corrects some minor errors in Bellman's proof. Like Bellman's proof in [2], we will prove that a sequence of discrete dynamic programming processes with decreasing step sizes will produce, under appropriate conditions, a convergent sequence of return functions. It should be noted that the optimal control policy may not converge even though the return functions do. But since the return function is of primary interest, not the details of the control policy, control policy convergence is not generally important. From the discussion thus far it is clear the the manipulator dynamics and required constraints take the form:

95 d -= G (X,p,v ) (4.3.8.1) where the control variable v (this is the same as p in the previous discussions) must meet some set of constraints: f, (X,p,v)< 0, q = 1,2,,M. (4.3.8.2) Also rewrite the objective function J = -C to be maximized as follows. J(V) = e(4(Xm)) + J F(X,P,v )dX (4.3.8.3) 0 subject to the initial condition P(O) = Po. Note that the boundary condition p(Xm.x) = pf can be enforced by taking O(p(Xm)) to be zero if p = p/ and -oo otherwise. The dynamic programming method approximates this continuous problem by discretizing the dynamic equation and objective function using the Euler method, giving: Pkt + - Pk + G (Xk,\ Pkv Y) (4.3.8.4) N-I J({vk }) = e(N) + E F(Xk P,k,vk )A (4.3.8.5) where A = Xmax/N, XA k A, P = -(X ), Vk = v(XA ), and the inputs vk are constrained by nfq(Xk,p,tvk)~<, O q =1,2, -.,C. (4.3.8.6) Now define f,,(c) for n =O,1,...,N by

98 N-i f,.(C ) = Su O((N) + E F(kP.k,Vpk) P)N| = C (4.3.8.7) (c)= Su~~,[e(PNt )+ lC F(X,,crr,~c)CI ]~ C1N-n G (4.3.8.7) kFk. k =N-" Then we have f 0(C ) = (c) (4.3.8.8) If.1( ) = Sup [F(XN-.-I.l,,V )+ f (C + (C + G(\N —1C, (4.3.8.9) Note that Sup has been used instead of max. This is done to allow the use of nonclosed constraint sets and discontinuous functions. It does not materially change the results of the dynamic programming process in that we may make the return function f, as close to the optimal value as we please. To see this, consider a single stage of an N-stage process. For each k and e>O, we may make fk to be within e/ 2k of its optimal value, thus making IN be within 2f of the optimum. Since E may be as small as we please, a control strategy can be constructed which will make the return function agree with the optimum value to within any desired tolerance. The proof of the main theorem requires the establishment of several lemmas. Lemma 4.3.1: If for the feasible input set D, r(c 1) = SuE [AC sV) + bi(C,V)] and H(c2) = SU [2(C2,)V + P2(C2,V)] then

97 r(c ) - H(C2) |< SugE Orc., -V 0C, | + SUD 11(Cl,v)- sC2,V) | Proof: For every e > O there exist v1 and V2, both elements of the set D, such that r(c1) < 1l(c 1,v1) + l1(c 1,v1) + f H(c2) < (C2,v2) + (0C2,V2y) + f We then have the inequalities k1(C 1,V2) + O1(c 1,iv2) ~ r(1) < () < (c,v) + bl(c,vl) + c C2(),V1) + I2(C2,V1) < H(C2) < 02(C2,V2) + i2(C2,V2) + f Since r(c 1) - f > Ol(c l,V2) +'1(c1,YV2)- and H(c 2) - E < 02(C 2,V2) +,&2(C2,V2) we have (r(c) -,)- (H(C2)- ) = r(c) - H(e2) > Ok(C1,,v2) - 2( 2,v2) + b1(C 1,2) - 24(C 2,V2) - Similarly, we have

98 r(c1) - H(C2) < #1(C1,v) - 04C2,V1) + O1(C1,V1) - b2(C2,v1) + f But these two conditions imply that Jr(c1)- H(c2) < Max [ jl(cl,v2) - 02(2,v2) + tPl(c1,V2) - 2(c2,v2) - -, kl(cl,vi) - 9k2(C2,v1) + P1A(C1,V1) - t2(Cc2,v1) + I < Max [ jI(CIl,v2) - 0(c2,v2) + jl(Ci,v2) - 3,c2,Yv2) + ef I,(CIVI) - 2(C2,V1) + IO(CVi) - I(4C2,V) I + f < SUp [ (,)- (C2,V) + kiP(C,V) - )- 2(C2,V ) + ] <SED,V)-2(C2,v ) + SU (CV C2V + f Since f may be made as small as we please, the desired result follows. U Lemma 4.3.2 shows that the return functions f,, satisfy a uniform Lipschitz condition which is independent of the step size A. Lemma 4.3.2: If for all cl, c2E[Pmi, pmPl and for all admissible X and v, the function F satisfies a Lipschitz condition IF(XC,,v ) - F(X,c,v) < K I c c2 -

99 for some a > 0, e satisfies e(c,) - e(c,2) < K I1 - and G satisfies IG(xCiV) - G(XC2CV) j < L I c - C2 I for some' > 1, then J, satisfies the uniform Lipschitz condition f. (c1)- - f(c2) |< I l- C2 I a where is independent of n, c,, c2, and A, provided that it can be guaranteed that Pmia < Pkt < Pma for all 0 < k < N. Proof: First we prove by induction that I.(C J )- f.(C2)1 < k, AI c - C2 I' For n =0O we have f o(c) - f o(c2) = I e(c1) - e(c2) I < K I C1 - C2Ia We may therefore take k0 K. Now assume that.fn(Cl)- f.(2) < kAIlcI- C21 Then we have

100 +l(C 1) - f x+l(c2) Sup [F(xN-n._,C1,V)A) + f/(c1 + G()N_ _~,CV)D)] -Sup [F(XN-...IC2,V)A + f,(c2 G(XN —IIC2V )A)]| Note that if c 1= Lk, then c1 + G(XN.l1,c l,v )A - Ik +1; this is where the admissibility of the states Pk for all k comes into play. Applying Lemma 4.3.1, we have f V +l(C ) - f +(C 2) < Sup IF(xN_,,-IC,V ) - F(XN-. C,V ) + Sup,, (c, + G(XN-, C l,v)A) - fn(C2 + G(XN_,c 1C2,v)A). Applying the Lipschitz conditions on F and f,,

101 I/n+.,i() - Inl(c2) J < KAl ci - C2 + k. A Sup Ic, + G(XN_.C_,cpv,)A -C2- G(XN-_ -I1,C2,V )A - K I c1 - C2 I + kA Sup J(cI - c2)+ (G(\XN —l,cl,v) - G(XN-.n-1C2,V)))A I < K A I c - C2 + k I C1 - C2 1 + Sup I G(XN_._-ICV,) - G(XN_-n-_l2,) I 2 a Applying the Lipschitz condition on G, ]f +1(C1) f, +1(c2) _< KA I C1 - C2 1 + k,,A I - C2 + L I C - C2 17 KA I cI - C2 I + k,,A|C1- C21(1 + LL I c,- c- C21'A)" < KA L c1 c2 I" + knA Ic-c2(1 +IL Pma, - i I'A) " + kA lci l- ~: | (1 + L m,, - Pmin I u-A)". Defining M - L Pm,,ax- groi 1-,

102 If+l(ci) - j+,(c) = KA 2c, - 2 a + kA I cl - C2 I(1 + MA)~. Using the binomial theorem to expand (1 + MA)', (1 + MA)1 = 1 + aMA + a(a-1) M 2A2+ a(a-lXa-2) M3A3 + 2! 3! =1 + aMA + A(A). Since the binomial series converges for I MA I < 1, the function vr(A) is bounded, and goes to zero as A goes to zero. Therefore, for A sufficiently small, we may take (1 + MA)~ < 1 +(aM + E)A =1+ PA where e is some small positive constant. This yields the inequality If +i(cI,) - f+1(c2) < KA l - C2 + k.A I CI - C2 l(1 + PA). We may then take k,+l to be defined by the recurrence relation'k+I -= K + k,(l + PA). The solution of this equation which satisfies the initial condition ko0 = K is k,, = K l+PA)"+-1 ]. Then at every stage n we have I.(~)-/f(c2) < kA I| - C2 = Jp [(1 + pA)"+- 1 C1- C2 1 — ~~~ ~1p) / r

103 Since k,, _ kN for n < N, we may use the condition f.(cI)-/ f.(c2) < kNe I l - C2 1 for all n < N. Now since A = Xx/N, we have If( cl)-f(c2) _-< (1 +PA) -1 IC -C21~. As A-+O, the quantity in brackets remains finite, and in fact approaches the limit C PX - 1. Since this quantity is also finite for all A > 0, we may construct the inequality f.(C,)- f-(C2) < IC1 - C2 1 where the constant 4 is given by K 111 aX 1,.,,,-X+ P o<< -1 + PA) P o< _< x, Since 4 is independent of n, c1, c2, and A, we have the uniform Lipschitz condition desired. N Lemma 4.3.3 establishes a connection between the dynamic programming process with step size A and that with step size 2A. Lemma 4.3.3: Let the process g9 with step size 2A satisfy the recurrence relations go(c ) -e(c)

104 gA+1(c) = Sup [F(XN2k 2,C,v)2A + g(c + G(\N-2k2,C,cv)2A)]. Define the auxiliary process h2k with step size 2A by the equations h(c ) = e(c ) h2k,+2(C) = Sup [F(x\N-2,-2C, )A F(N2k + FG(\N-2k-2,C,v )A,V )A + h2k(c + G(XN-2k-2,C,)A + G(XN-2k-1,c + G(XN_2, 2,C,V )A, ). This process can be thought of as a process with step size A in which the input policy is restricted so that the input for the N-2k-2 d interval is the same as for the N-2k -1' interval. Let F,G and e satisfy the Lipschitz conditions IF(X,,cl,v)- F(X2,c2,v) I < K I C1 - C2 I + B I XX1- X2 I F(X,c,v)- F(X,c2,v) I < K I c - C2 I e(c,)- e(c2) i < K I C- C 2 I " I G(X1,c1,v) - G (X2,C2,) I < L I Cl - c2 1 7 C I X, - X2 1 I G(X,clv) - G(X,C2,V) I < L I C, - C2 1 7 where K, B, L, and C are constants; a > 0, -7 > 1, i> 0, and 6 > 0, for all admissible X, c 1, c2, and v, and for all XI1, and X2 such that the interval [min(X1, X2), max(Xi,X2)I does not contain any of the Nd points of discontinuity

105 d 1, d', dN4,. Also let F and G satisfy I F (Xl,,c,,v) - F(X2C,2,v) I < K I cl - c2 I { + B and I G (X1,c1,v)- G(X2,c2,v) I< L C1 - C c217+ C for all admissible X1, X2, c 1, C2, and v. Then I h2k(C) - gk(C2) I < * c1 - 2 1 + EA where w = min{ 1, a, f, a(1 +y)-l, a(l+6)-1}, and 4 and E are constants. Proof: We first prove that there exist numbers pk such that I h 2k (C l) - 9I (C 2) < C l e - c 2 1 + l We proceed by induction. We have I ho(C) - 90(c2) I = I e(c1) - e(c2) I < K I C - C2 so the result holds for k - 0 with po = 0. Now assume that for some particular value of k. Applying Lemma 4.3.2 to the function g +1, we have 2k,+2(C ) - 9k t+1(C2) 1 = I h2 +2(cl) - k+,l( 1) + 9kl,+1(1) - k +(C 2) 1 < I h2t+2(Cl) - g+l(C1) I + I g,+l(C,) - g+,(C) I

108 < I c - c ~2 + I h2+2(C 1) - gk+,(c~) I = - Ic1 - c211 + Sup[F(*\N-2k-2,C,,V)A v + F(XN-2k -,cl+ G(XN-2k -2,C, )A,V )A + h 2k (C + G (XN-2k-2,C 1,V )A + G (XN-2k -1,C + G ( N-2k -2,C l, )A,v )A) -Sup [F (X\N_2_ -2C,2V )2A + gk(c, + G(XN-2k-2,ClV)2A)] IApplying Lemma 4.3.1, I2 +2((c 1) - 9k +1(c 1) < Sup IF(XN-2k-lC1 + G(XN-2k-2,C1,V)A,v) - F(YXN-2k2, Cl,v) + G (XN-2 k,c 1 + G (XN_2k _2,C 1,V )a,V )ya)

107 -g (c 1 + G (XN_2k,_,C 1,v )2A) < Sup K I G (XN_2_2,C l, )A I aA + BA1+# v + 4 Sup G (x\N-2kl,c + G (XN2_2,cl,v)A,v)A - G(\N-2kt2,ClV) V + Pkt if the interval [XN-2 -2, XN-2k ] does not contain any of the points dl,...,dN4. Applying the Lipschitz condition on G, we have I h2 +2(C 1) - g, +1(C 1) I < Sup K I G (XN-2A-2,C 1, )A I aA + B A1+ + 4 Sup L I G (\XN_-22,C l,)A I + C1 + + pk t l or I h2 +2(c 1) - k +l(c 1) I < Z A+l + BA'+B + A Aa(1+9) + Pk AI+ = (Z Aa-W + B A~$- + A Aa(l+)t1l-w + Pk )A+ where q = min(7,6), A -= - <Sup L G(XN2k 2,c l,v )I7A + + CA6-11 and Z = Sup K I G (XN-2k-2,Cv ) Now w is defined by w = min{l,a,,,ca(l+)1,ca(1+6)-1), so w is less than or equal to ca,,a, and ca(1+q)-1. Since we also have A < X,ma we have

108 I h+2(~l) - g9 +l(C) I < (D + Pk )a1' for the constant D - Z Aa-w + B ASw + A A(1+')-1. This gives I h2+2(C 1) - gt+(c2) < 4 I C1 - c2 1 + (D + Pt )Aw+ If, on the other hand, the interval [XN-2k -2, XN-2k I contains one of the di, then we have I h2t +2( 1) - g +1( 1) I < Sup K I G (XN_-2-2,c 1,v )/A I'A + B/A +SuP IL I G(XN_2i_2,c,,v)A|7+ C + + pkA or I h2k+2(C1) - gA+l(Cl) I < WA' + Pk A1 where -= min(1,1+a,a) - min(l,a), and W = Sup K G(XN-2.2, c,v) a1 +a-1 + B 1+ Sup L I G(XN-.2k-2,C1,v)A I | A We may therefore take (W+ Af-w1 if some di E[XN-2k -2,XN-2_ i Pk+1 = P + D otherwise But, then we have ph < PN = po + (N - Nd)D + Nd IWAS-' for all k <N. Noting that Xmax = 2N A,

109 I k2(C1)- g (C2)1 I C Icl- c2 I +((N - N,)D + Nd WAW' + po)AwYl =l, - C2 + wXm D X - NdD t+l + Nd WA < 4 j cI - c2 1 + EAW since, again, w < min(l,a) -= U Lemma 4.3.4 relates to convergence of infinite sequences. Lemma 4.3.4: If oo > W > u,,+l > u,s - a,, where a, > O and a,, < oo, then the sequence (u, } converges. Proof: Let zM and zp be two cluster points of the sequence (u, }. Let the sequences ( uM }) and {up) } converge to xM and Zp respectively, and let M1 < P1 < M2 < P2 < M3 < P3 <''' Then we have PI IUM -Up, >- E ak -Ck=M, and M, +, Up - uM,+. - 2 ak = -bi k =P, Since the series a,, converges, fj and 6, go to zero as i-*oo. But the left sides of these inequalities approach zM - Zp and zp - ZM respectively. Therefore, we have zM - zp > Oand zp - zM > O,or zM = Zp.

110 We are now in a position to state and prove the main theorem. It shows that the return functions for the dynamic programming problem converge, provided that the functions F and G satisfy some Lipschitz conditions everywhere except at a finite number of jump discontinuities, and provided that the variable p can be guaranteed to stay within appropriate bounds. Theorem 4.3.1: Let the input v satisfy O < v < 1, and let F and G satisfy the Lipschitz conditions I F(X,c 1,v) - F(X2,c2,v ) < K I C, - C2 1 + B I X1 - X2 I F(X,cl,v) - F(X,e 2,v) I < K I C1 - c2 {I I c(X,c1,v) - G(X2,c2,v )I < L I C1 - C2 1 7 + C I X1 - X2 16 G(X,cl,v) - G(X,c2,v) I < L I l - C21 1 where a > 0, - > 1, # > 0, and 6 > 0, for all admissible X and v, for all PmiU < cl,c2 _< ma,, and for all X1, and X2 such that the interval [min(X1,X2), max(X1,X2)] does not contain any of the Nd points of discontinuity d 1d, ~ ~2,...,dN. Also let F and G satisfy I F(Xl,cl,v ) - F(X2,c2,v ) < K I C1 - C2 l + B and I G(X,,c1,v) - G (X2,C2,V) < L I C1 - C2 1+ C for all admissible XA, X2, C1, C2, and v. Then the discrete dynamic programming pro

111 cess yields return functions which converge to a limit as the step size A goes to zero, provided that PIk can be guaranteed to stay in the interval [ pmin, Pml]Proof: Consideration of the intermediate process h2k described in Lemma 4.3.3 shows that f 2k (C ) h 2k (C ), so by Lemma 4.3.3 we may write f 2k (C) > hk2k(c) > gk(c)- EAW for some w > 0. If F is to satisfy the required Lipschitz conditinns, then F must be bounded, so that f remains finite. Therefore if we define Ur to be the return at stage 2' k with step size A/ 2r, then we have 00 > W > Ur+1 > Ur - E|, Therefore by Lemma 4.3.4 the sequence {u }) converges. Roughly speaking, this theorem shows that when the functions F and G are continuous of sufficiently high order in p and piecewise continuous in the stage variable X, then the discrete dynamic programming process converges. One important point is that in order to apply the theorem, it must be guaranteed that Pk stays within the range in which the Lipschitz conditions are valid. This can be accomplished in several ways. If, for example, G(X,pm,.x,v) < 0 and G(X.,pmi,v) > 0 for all X, then the optimal trajectory can never escape the interval [pmin, pmx]J. Another way to assure containment in this interval is to construct an objective function which guarantees that trajectories which stray outside the interval are heavily penalized and therefore never selected. This method works

112 for the examples in this paper; since the examples all penalize time, they all have terms which are inversely proportional to velocity, and so keep the velocity p greater than some e > 0. 4.3.9. Numerical Examples To demonstrate the use of the dynamic programming algorithm, we present several examples. These examples use the first three joints of a cylindrical electrically-driven manipulator, the Bendix PACS arm. The second and third joints of this robot are similar to the two degree-of-freedom robot used to demonstrate the phase plane method, so a direct comparison of the methods is possible in those cases in which the cost function is minimum time. In addition, several examples will treat cases to which the phase plane technique does not apply. Before presenting the example, an explicit form of the objective function must be chosen. The objective function used here has one component proportional to traversal time, T, and another component proportional to frictional and electrical energy losses. The servo drives of the arm are assumed to consist of a voltage source in series with a resistor and an ideal DC motor. This gives the form T T C = r T+ +,f Rii q dt + r I,2Ri' dt (4.3.9.1) =rglo 1 d+rcl 2R'j CJ0 * JX dX dX (4.3.9.2) + re If01,I2Rm' dX where rt and re are related to revenue generated per item and the energy costs of

113 the motion respectively, and Is and Ri' are the motor currents and resistances for joint *. Since the joint torques ui are related to the motor currents Is by the relationships kim Ui k Ii (4.3.9.3) where k," and k,' are the motor constant and the motor gearing respectively, the sum in Eq. (4.3.9.2) can be written as ( u.2. The torques ui are given in terms (k")2 of X, p, and # by Eq. (4.1.6), which is quadratic in p, so the integral in Eq. (4.3.9.2) can be expressed in terms of integrals of powers of p. Since the path is known over one X-interval, we can determine the components of the incremental cost. To do this, we need the integrals x 1+' dX, f, pd)X, fx' +p2dX, JX pdX. In general, we have;. k _ __ k _ __ [_ __+1 - Adk _ Xk+1 -k n (+2 a +2 (l+2)(? -0)(pi PO+2) Equations (4.3.9.2), (4.3.9.4), and (4.1.1) give the incremental cost as

114 C - 2rt4l + reRii dfX df i1 (4.3.9.5) + (k')2!? [Q2 3 + 2QR42 + ( + 2Q(S + M,))4) (ki) )2 + 2R (Si + M p)0 +(S, + M p)21] It should be noted that this cost function always has a 4-j term unless the time penalty is zero and the robot is not influenced by gravity. (In this case, the optimal solution is not to move at all!) Therefore the cost function will prevent the trajectory from going to zero velocity unless forced by boundary conditions. Also, since torques and motor voltages are bounded, the velocity # is bounded. Thus # in the DP algorithm stays within some interval Pmi, Pmjxl, as is required for convergence. Since the maximum and minimum values of the control variable p can be computed from Eq. (4.1.1), we may define a new control variable p by the relationship P = Pmin(XP) + (Pma(Xp)- Pmin(Xp))p (4.3.9.8) so that p ranges from zero to one. It is easily shown that the other conditions of the convergence theorem are met if the parameterized curve is suitably well behaved, so the DP algorithm will converge with this cost function. The dynamic equations and actuator characteristics for the PACS arm are given in the Appendix. The DP algorithm was implemented in the C programming language running under UNIX on a VAX1-780. The parameterized curves are represented as sequences of points. All computations are done for a path which is a

115 straight line from (0.7,0.7,0.1) to (0.4,-0.4,0.4), all (Cartesian) coordinates being given in meters. To verify the correctness of the dynamic programming algorithm, it was first applied to the simple two-degree-of-freedom robot which was used in section 4.2.5. The phase plane plots for minimum time, with a 10X10 and a 40 X 40 DP grid are plotted in Figures 4.3.4 and 4.3.5, along with the phase plane plots calculated by the phase plane method. Reassuringly, the trajectories calculated by the DP method seem to converge to the correct minimum-time phase plane plot as the grid gets finer. Figure 4.3.6a shows the phase plane plot for a OX 10 grid with a pure minimum time cost function. Figures 4.3.6b through 4.3.6d show joint torque or force versus time, and Figures 4.3.8e shows motor voltage vs. time. Figures 4.3.7 are the same, except that the grid is 40 columns by 160 rows. The calculated traversal times for 1OXIO, 20X40, and 40X180 grids are 2.000, 1.972, and 1.905 seconds respectively, compared to 1.782 seconds as calculated by the phase plane method. Figures 4.3.8 and 4.3.9 show phase plane, torque, and motor voltage plots for a time penalty of 1 unit per second and an energy penalty of ten units per joule. Note that the trajectory is lower than that which is obtained if only time is penalized. The grid sizes are 10X 10 and 20X40. Figures 4.3.10 and 4.3.11 show the results for a minimum time trajectory when, in addition to the torque and voltage constraints given in Table A.2, the total power sunk or sourced by the robot is limited to 2 kilowatts. The time consumed by the algorithm was measured for several different grid sizes, with, however, 400 interpolation points on the curve regardless of grid size.

Computation of the dynamic coefficients for 400 points usually took from 0.350 to 1.0 seconds of real time, so the computation time is probably about 0.35 seconds. The computation times for the dynamic coefficients vary as the cube of the number of joints. Thus for a robot with six degrees of freedom instead of three we would expect about eight times as much computation. Taking 100 times the 0.35 seconds, to be very conservative, gives 35 seconds for 400 points. But 400 interpolation points are hardly necessary for a grid with a value of 40 for Nx; 80 points would certainly be adequate, giving a computation time of about 7 seconds for the dynamic coefficients, not an unreasonable figure if the motion is to be repeated a large number of times. The times given in Table 4.3.1 are real times for the DP algorithm on a lightly loaded system (average of approximately 2 tasks running concurrently). The times must therefore be regarded as approximations to the actual computation times. Table 4.3.1 also lists the function 6XIO-4NxNN, which seems to give a good match to the actual running time, as predicted in Section 4.3.6. It should be noted that the program was run with all debugging features enabled, and that no serious attempt was made to optimize the source code. Indeed, there are redundant computations in several places which could be eliminated. The effect of grid size on the quality of the results of the dynamic programming algorithm is of practical importance. The grid must be fine enough to give good results but not so fine that the time required to perform the DP algorithm is excessive. Intuitively, varying the number of rows and number of columns in the grid will have different effects on the results. Varying the number of columns (the number of X-divisions) varies the accuracy of the dynamic model; using a smaller size yields a

117 more accurate approximation to the true dynamic model, since the "pieces" in the piecewise-constant dynamic model will be smaller. Varying the number of rows (the number of sp-divisions) varies the accuracy of the approximation to the true minimum-cost solution; a finer grid size will in general yield a better approximation. Another important factor is the ratio of the number of rows to the number of columns. If the number of columns is very large and the number of rows is very small, then the slope of the curves connecting one point in the DP grid to another must be either zero or very large. Since the torque bounds induce bounds on the slope of the curve, it is possible that the DP algorithm may not even be able to connect a point to its nearest diagonal neighbor. In this case, the algorithm will give no solution at all. Therefore when choosing grid size, one must (i) choose the X-divisions to be small enough to make the piecewie-constant dynamic model of the robot sufficiently accurate, (ii) choose the p-divisions to be small enough so that the resulting trajectory is a satisfactory approximation to the true minimum-cost trajectory, and (iii) make sure that the p-divisions are small enough so that the slope of a curve connecting two adjacent points in the grid is small.

118 N | N minimum average predicted 10 10 1.217 1.355 0.6 10 20 3.583 5.260 2.4 20 40 19.917 25.5 19.2 20 80 92.050 110.367 76.8 40 40 29.987 33.856 38.4 Table 4.3.1. Computation times for different grid sizes (seconds)

119 o X Xk+lI x Figure 4.3.1. Curves connecting adjacent DP grid points

120 PN,, -, -, a-n exn raecory Figure 4.3.2. Curve connecting a grid point to an existing trajectory

121 /\ I / \ /H \ Suboptimal trajectory..-.- -Swath boundary 0 Grid points Figure 4.3.3. Suboptimal trajectory with uncertainty swath

122 0.5 0.4 0.3 0.2 0.1 0.0 X X 0.0 0.2 0.4 0.0 0.8 1.0 1.2 1.4 1.6 Figure 4.3.4. Minimum time phase plane plot for polar manipulator, lOX10 grid

123 0.4 0.2 0.0 X 0.0 0.2 0.4 0.6 0.8 1.0 1.2 1.4 1.a Figure 4.3.5. Minimum time phase plane plot for polar manipulator, 40X40 grid

124 8.000 6.000 4.000 2.000 2.000 4.000 6.000 Figure 4.3.6a. Minimum time phase plot for PACS arm, 10X10 grid

125 T 100 0.5(0 1.000 1.' t -50 Figure 4.3.6b. U joint torque vs. time, 10oX 10 grid

126 F 10.000 5.000 0.500 1.000 1.50 -5oo000 -10.000 Figure 4.3.~c. r joint force vs. time, 10 X 10 grid

127 400 380 360 340 0.500 1.000 1.500 Figure 4.3.6d. z joint force vs. time, 10XlO10 grid

128 9 motor voltage -...*........ r motor voltage V z motor voltage 4 I...,,..... "'., - - QU~~-s ~t001.000 1.50 -2O -40 Figure 4.3.Oe. Motor voltages vs. time, 10X 10 grid

129 8.000 6.000 4.000 2.000 2.000 4.000 C.000 Figure 4.3.7a. Minimum time phase plot for PACS arm, 40 X 160 grid

130 T 100 ~~~~I~~ ~~~~~~~~~~~~ t.500.000 <.500 Figure 4.3.7b. 0 joint torque vs. time, 40X 160 grid

131 F 5.000 0.500 1.000 1.5 0 -5.000 - 10.000 Figure 4.3.7c. r joint force vs. time, 40X 160 grid

132 F 420 400- % 380 360340 0.500 1.000 1.500 Figure 4.3.7d. z joint force vs. time, 40 X 160 grid

133 -9 motor voltage................. r motor voltage z motor voltage - V t......... 0.500 1.000 l5 0 (0 1 ": -2o0,e ~ %: Figure 4.3.7e. Motor voltages vs. time, 40XI 10 grid

A.000 6.000 4.000 2.000 A 2.000 4.000 6.000 Figure 4.3.8a. Phase plot for PACS arm, minimum time-energy, 10X10 grid

135 T 50 Fgr 0.5t00oq v 1.01, O0' -50 Figure 4.3.8b. 0 joint torque vs. time, 10X IO10 grid

136 F 5.000 0.500 1.000 1.500 -5.000 -10.000 Figure 4.3.8c. r joint force vs. time, 10X 10 grid

137 F 400 360 t 00 1.00 1.5 Fiure 4.3.8d. z joint force vs. time, IOX 10 grid

138 -9 motor voltage................. r motor voltage z motor voltage -—.....V 20 I~~ t 0.500 ~~ ~5O1.000 1.00 -2o..4......0....0...0 *.; Figure 4.3.8e. Motor voltages vs. time, 10X 10 grid

139 8.000 6.000 4.000 2.ooo 2.000 4.000 6.000 Figure 4.3.9a. Phase plot for PACS arm, minimum time-energy, 20X40 grid

140 T 50 0.500 1.000 1. D0 2.000 -50 Figure 4.3.9b. d joint torque vs. time, 20X40 grid

141 F 5.000 0.500 1.000 1.500 2.000 -5.000 - 10.000 Figure 4.3.9c. r joint force vs. time, 20X40 grid

142 400 380 360 0.500 1.000 1.500 2.000 Figure 4.3.9d. z joint force vs. time, 20X 40 grid

143 90 motor voltage r motor voltage z motor voltage V Im.emle ~ mlmlole Im elm toot,~ ~~t -20 Figure 4..3.9e. Motor vot1.000ges. time, 20X40 grid

144 6.000 4.000 2.000 2.000 4.000 6.000 Figure 4.3.10a. Minimum time phase plot for PACS arm with power limit, 10X10 grid

145 T 5 25 Figre4. 2l~.000 toq3.000 -25 Figure 4.3.10b. 0 joint torque vs. time, lOX 10 grid

146 F 1.000 2.000 3.000 -5.000 Figure 4.3.10c. r joint force vs. time, 10X10 grid

147 F 400 390 380 370 1.000 2.000 3.000 Figure 4.3.10d. z joint force vs. time, 10X 10 grid

09 motor voltage.................... r motor voltage z motor voltage -— ~ —----- V ~,v —-- ~ 2.000! 000 I -.~._*_'*..e_:.. -20 Figure 4.3.10e. Motor voltages vs. time, lOX 10 grid

149 6.000 4.000 2.000 2.ooo 4.000 6.000 Figure 4.3.11a. Minimum time phase plot for PACS arm with power limit, 20 X 40 grid

150 T 50._. 0.500 1.000 1.500 2.000 -5Figure 43 joint torque. time, 20X40 gr Figure 4.3.11b. t joint torque vs. time, 20 X 40 grid

151 F 5.000 0.500 1.000 1.50000 15.000 -5.000 Figure 4.3.11c. r joint force vs. time, 20X40 grid

152 F 400 - 380 360i u..11.00 joint.500 2.000 Figure 4.3.11d. z joint force vs. time, 20X40 grid

153 90 motor voltage................... r motor voltage z motor voltage V 20............ t.me1.000 1.500.000 2oo ~ I Figure 4.3.11e. Motor voltages vs. time, 20X40 grid

154 4.4. Trajectory Planning by Iterative Improvement It was pointed out in the previous section that the dynamic programming algorithm, particularly when used with a very fine grid, wastes a great deal of computation time by testing admissibility criteria and evaluating cost functions for sections of trajectories which are not part of, and indeed are not even close to, the optimal trajectory. It was then suggested in Section 4.3.7 that reasonable computational accuracy could be obtained from the dynamic programming algorithm by performing the dynamic programming algorithm repeatedly using a grid with a small number of divisions of the state variable, while changing the grid spacing at each iteration. The trajectory planning method proposed here, the perturbation trajectory improvement algorithm, or PTIA, is a variation on this theme. In addition, the PTIA may be extended to include more general classes of torque constraints. The inclusion of jerk constraints in the algorithm will be demonstrated. At the start of PTIA, it is assumed that one phase trajectory 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; we wish to maximize p, so the phase trajectory should always be pushed upward. 4.4.1. The 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 points. As before, we choose to use the

155 latter representation, i.e., the curve (4.1.1) is represented as an ordered sequence of points (Xk(),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, i.e., ignore jerk constraints for the time being. Then all constraints can ultimately be given as X- and p-dependent constraints on p, or equivalently constraints on -A. In terms of the (X,p) plot, each point is assigned a dX set of allowable slopes. In the discrete approximation, this sets limits on the differences between the values of p at adjacent 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 constraint that the velocities at neighboring points not differ too much. 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. (This is just componentwise optimization, where the infinite-dimensional space of functions p(X) has been approximated by a finite-dimensional vector of real numbers.) If we call this Algorithm A, then we have Algorithm A: 1) Set all velocities to values which are realizable (usually all zeroes). 2) Push each intermediate point of the curve as high as possible consistent with

1568 the slope constraints. 3) If any of the velocities were changed in step 2, go back to step 2, otherwise exit. As a practical matter, the search required to find the highest possible velocity in step 2 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': 1) Set all velocities to values which are realizable (usually all zeroes). 2) Set the current increment to some large value. 3) Push each intermediate point of the curve up by an amount equal to the current increment, if this is consistent with the slope constraints. 4) If any of the velocities were changed in step 3, go back to step 3. 5) If the current increment is smaller than the desired tolerance, stop. Otherwise halve the increment and go to 3. 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. Algorithm A' is very simple, except possibly for the slope constraint check required in step 3. This requires a knowledge of the dynamics and actuator characteristics of the robot. However, this check is a simple "go/no go" check, and can be isolated as a single function call. (Hereafter this function will be

157 called the constraint function.) Hence the trajectory planner can be used with other robots by changing a single, though possibly complicated, function. An important characteristic of the constraint function is locality. In the case discussed above, the constraints are expressed in terms of X, p, and dX. We need two points to determine the slope d{, so the constraint depends only upon two points. Therefore when a point of the curve has its p value changed, it is constrained only by the two adLacent points; the rest of the curve has no influence. This allows much calculation to proceed in parallel. Step 3 of Algorithm A' can be divided into two steps, one which increments the odd numbered points and one which increments the even numbered ones. Since the even numbered points stay the same while the odd numbered ones are being incremented, and vice vtrsa, 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 acceleration) only require a more complicated constraint function. 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 3 would require three passes instead of two.

158 It also may make the algorithm diverge as the number of points on the curve increases, as will be seen later from the numerical examples. As a simple illustration of how the algorithm works, consider a simple one. dimensional problem. Suppose we wish to move an object of mass m from z =0-O to z =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-z. We then have d2z d2X d s d= dX d is dt2 dt2 dmt dX dt dX If we consider X-intervals of length 1, then the discrete approximation to the parameterized "curve" will have 5 points. The acceleration = tic can be approximated as dp ti+l_ - Pi i+l+Pi. i i+ - Pi. #$I - __ l2 d X Xi+, - Xi 2 Xi+,- Xi 2(X\+l- Xi) The torque constraints then become 2P0\+i - Xi' Fmax ~ IF I m l a = m 2(X,+1 -X ) ) If we use m = 1, Fma = 2, and Xi+1 - Xi == 1, this reduces to #sl2+1,-.2 < 4. 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, and I 22 - 02 1 = 4. Raising the middle point, point 2, we are constrained by the fact that p3 = O, which limits P2 to 2 also. Like

159 wise, 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 12 = 2V. On the next pass, no p 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 4.4.1 shows the discretized trajectory after passes zero, one, and two of Algorithm A.) Now look at what happens when we use Algorithm A'. Say we start with an increment of 2. Ther the result of tht first pass of Algorithm A' is the same as the result of the first pass of Algorithm A, namely Ill- = 12 = 1 = 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 4.4.2 shows the trajectory after passes zero, one, three, and four of Algorithm A'.) 4.4.2. Computational Requirements The PTIA, unlike dynamic programming, 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. In fact, the time increases as the square of the number of points. To see why this is so, consider what happens when the number of interpolation 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

160 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., the time dependence is of the form t = KN2, where t is the computation time, N is the number of interpolation points, and K is a constant. 4.4.3. Numerical Examples As an example, we again use the Bendix PACS arm. First we consider only constraints on joint torques/forces and motor voltages, without considering constraints on their derivatives. The perturbation trajectory planner was written in the C programming language, and run on a Vax-11/780 under the Unix operating systenm. The planner was tried with a straight-line path, a geodesic in "inertia space" (see the next chapter), and a joint interpolated path. The traversal times for these paths are 1.79 seconds, 1.59 seconds and 1.80 seconds respectively. Plots of p vs. X, joint positions vs. time, and motor voltage vs. time are shown in figures 4.4.3a through 4.4.5c. Comparing these results to those obtained in Chapter 5 using the phase plane method shows the traversal times and the various plots to be virtually identical. To demonstrate the application of the perturbation technique to problems in which there are constraints on the derivatives of the torques, we consider the same

161 problem with the additional constraint that the time derivatives of the joint torques and forces be less than 100. The time derivatives of the torques are computed using the identity du_ dui dX dui dt dX dt d X dui The derivative d was estimated by calculating the difference between the 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 4.4.8. For 50 points, the traversal time is 2.28 seconds; the phase plane plot is shown in figure 4.4.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 4.4.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 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 means of achieving this goal 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

162 appear. With this modification, a velocity increment of 0.1 at each pass gives the results plotted in figures 4.4.9a through 4.4.9c for a straight line with 50 points. 100 points and a velocity increment of 0.025 gives the results plotted in figures 4.4.10a through 4.4.10c. The calculated traversal times are 2.03 seconds in both cases. The numerical results obtained using the perturbation trajectory improvement algorithm agree with those obtained by the phase plane method in those cases in which both algorithms can be applied. While the iterative improvement algorithm is not as fast as the phase plane method and not as general as dynamic programming, it is extremely flexible and very easy to program.

183 * t 0 % 0 S 0 % 0 % 0 S 0 S 0 % 0 S 2.000 I I 0 % 0 % 0 % 0 ~ ~ % 0 % I~~~~~~~~ I~~~~~~~~ I~~~~~~~~~~ I~~~~~~~~~~~ 1 1.0000.4. Figure 4.4.1. Results of Algorithm A. I~~~~~~~~~~~ I ~~~~~~~~~~~~~ I~~~~~~~~~~~~ I~~~~~~~~~~~~~ 1.000 2.000 3.000 4.000 Figure 4.4.1. Results of Algorithm A.

184 I.''' le ~~ s I:,'SW. 2.000 t I % 1.000 2.000 3.000 4.000 Figure 4.4.2. Results of Algorithm A'.

165 10 8.000 6.000 4.000 2.000 2.000 4.000 6.000 Figure 4.4.3a. Phase plane plot for straight line

168'0 joint position -—...... q r joint position z joint position --------- 1.000 0.750. 0.500 "-. 0.500 1.000 1.500 -0.250 -0o00o -1.000 Figure 4.4.3b. Joint position vs. time for straight line

187 O9 motor voltage....... —r motor voltage z motor voltage... —------- V 40 i~rrr~'~~C'~~~r.rL_ ~ 0.500 1.000.500 -20_ _ 0. -40 Figure 4.4.3c. Motor voltage vs. time for straight line ~e~e~eme~ e 2 0 ~ ~ ~~( 0.500 1 000 1.500~~~~~~~~~~ eeeeeeeeeee~e -,0:~ -, --, - -- - -- Figure~~~~~~~~~ 4..c oo otg s.tm o tagtln

168 I 10 8.000 6.000 4.000 2.000 2.000 4.000 6.000 Figure 4.4.4a. Phase plane plot for geodesic

189 q a0 joint position................... r joint position z joint position 1.000 0.750 ee 0.500 0.250 - 9elt~~~ode ~ el 0.500 1.000' 1.500 -0.250 -0.500 1 -0.70 -1.000 Figure 4.4.4b. Joint position vs. time for geodesic

170 V9 motor voltage.............. r motor voltage z motor voltage ------------ 40 40f.... ------------- --- - r 7.. 20 0.500 1.000 1.500 -120 -40 M volag.v. fr g Figure 4.4.4c. Motor voltage vs. time for geodesic

171 8.000 6.000 4.000 2.000 2.000 4.000 6.000 8.000 10 Figure 4.4.5a. Phase plane plot for joint-interpolated curve

172 L9 joint position.......... —q r joint position z joint position...... 1.000 0.750....... 0.500 " -... 0.250 — *_,.t 0.500 1.0o 1.500,o. -02s0bo -0 250 -07'.. - 1.000 Figure 4.4.5b. Joint position vs. time for jointinterpolated curve

173 9 motor voltage........ r motor voltage V z motor voltage 40 20 t. l 0.500 1.000 1.50 -20-@ -40 Figure 4.4.5c. Motor voltage vs. time for joint-interpolated curve

Az 6.000 5.000 4.000 3.000 2.000 1'.000 - 2.000 4.000 6.000 Figure 4.4.6. Phase plane plot for straight line with jerk constraint, 25 points

175 6.000 5.000 4.000 3.000 2.000 1.000 2.000 4.000 6.000 Figure 4.4.7. Phase plane plot for straight line with jerk constraint, 50 points

176 % 0 1 2 3 Figure 4.4.8. Illustration of deadlock caused by jerk constraints

177 6.000 5.000 4.000 3.000 2.000 1.000 2.000 4.000 6.000 Figure 4.4.9a. Phase plane plot for straight line with jerk constraint, 50 points, velocity increment 0.1

178 q i3 joint position........... r joint position z joint position -------- 1.000 0.750.~ 0.500 %. 0.250'.. % t 0.500 1.000. 1.500 2.000 -0.250 - -0.300 -0.730 ~~ -1.000 Figure 4.4.9b. Joint position vs. time for straight line with jerk constraint, 50 points, velocity increment 0.1

179 09 motor voltage. —. r motor voltage z motor voltage 40 20 0.500 1.000 1; 5.00\ 2.0-o A.................._.,...._ I o\ / -20 -40 Figure 4.4.9c. Motor voltage vs. time for straight line with jerk constraint, 50 points, velocity increment 0.1

180 6.000 5.000 4.000 3.000 2.000 1.000 2.000 4.000 6.000 Figure 4.4.10a. Phase plane plot for straight line with jerk constraint, 100 points, velocity increment 0.025

q r joint position r joint position ~~1.0O~~~~ >z joint position 1.000 0. 50... t 0.500 1.000 a 1.500 2.000 -0.250 -0.500 -07....50. -1.000 Figure 4.4.10b. Joint position vs. time for straight line with jerk constraint, 100 points, velocity increment 0.025

V t0 motor voltage............ r motor voltage z motor voltage I dI;~~~~~~O ~~rrrrr,~~~~~~~~t.m......... 20 0.500.. -40 Figure 4.4.10c. Motor voltage vs. timee for straght line with jerk cntraint points, velocit. 0 incement 0.025

CHAPTER 5 SELECTION OF GEOMETRIC PATHS In general, it is extremely difficult to obtain an exact closed form solution, or even an accurate numerical solution, to the minimum time path planning problem, due to (i) the nonlinearity and coupling in the manipulator dynamics, and (ii) the complexity involved with collision avoidance. The collision avoidance problem (ii) has been sidestepped in the preceding chapters by assuming that the desired geometric or spatial path has been specified a priori. It has been assumed that a task planner provides a geometric path which is a parameterized curve in joint space. The trajectory planner receives these geometric paths as input and determines a time history of position, velocity, acceleration, and input torques which are then fed to the trajectory tracker. Even if collision avoidance is not a problem, the nonlinearity of the manipulator's dynamics usually makes the minimum-time path planning problem insoluble; it is only by reducing the dimensionality of the problem through the introduction of the parameterized path, i.e., the reduction of the full path planning problem to the trajectory planning problem, that solutions have been obtained in the previous chapter. In terms of the trajectory planning problem, the geometric path planning problem is the problem of picking the parametric functions f i in (4.1.1). In contrast to 183

the trajectory planning problem, in which the desired solutions can be expressed in terms of the position parameter X and its first and second time derivatives, the geometric path planning problem requires that a set of functions be chosen from an infinite dimensional space, thereby leading to a more difficult problem. In this chapter, we will develop a method for determining an approximate minimum time geometric path for the trajectory planners described in chapter 4. It is assumed that the manipulator has an unobstructed workspace, so that collision avoidance is not an issue. While this limits the applicability of the solutions obtained here, it does provide a feeling for what path characteristics determine traversal time, forming a possible base for further work. 5.1. Near-Minimum Time Path Planning As was previously pointed out, use of the maximum principle for solving the minimum-time geometric path planning (MTGPP) problem is practically impossible. Alternative approaches must be sought. In this section we will develop three methods for generating geometric paths. The first two use energy methods to derive a lower bound on path traversal times, and the third method uses the velocity limits derived in Chapter 4. The energy methods make use of the properties of the "inertia space" described in Chapter 2, especially the formulas (2.4.20) through (2.4.23) relating to power consumption. Using these relations, it will first be shown that geodesics in inertia space, i.e., solutions of the differential equations.s ds j = 0, are the optimal solutions to the MTGPP problem under some restricted conditions. Though the conditions

185 required in the special case are not met by realistic manipulators, the proof does provide a simple illustration of the method used here for obtaining lower bounds on traversal time; the curves which minimize the lower bound for the more general case can then be found using techniques similar to the one used in the special case, giving an absolute lower bound on the time required to move from one point to another. Then the derived traversal time bounds and the velocity limits derived in Chapter 4 are used to find approximations to minimum time paths. 5.1.1. A Special Case It will now be shown that if a manipulator's dynamic equations have no friction terms and no gravitational terms, and if the limitations on the joint torques consist only of limits on the total power supplied to (or taken from) the manipulator, then the minimum time geometric paths are geodesics in inertia space. Formally, we have the following theorem: Theorem 5.1: If a manipulator is frictionless and has zero gravitational terms, i.e., Rii O and gi = 0 in the dynamic equations (2.4.12), and the only restrictions on the torques applied to the manipulator arise from constant, symmetric limits on the total power supplied to (or taken from) the manipulator, then the minimum-time geometric path between any two configurations of the manipulator is a geodesic in inertia space provided that the initial and final velocities are zero. Proof: Under the stated conditions the dynamic equations for the manipulator become u, = Jjvi +[jik^i*vi = 6p' I 1d + JE d2* (5.1.1.1)

188 The total power sunk or sourced by the manipulator is limited by symmetrical constant bounds, i.e., -Pm,. < P < Pm. Then by Eq. (2.4.23), applying the constant maximum power gives dt dt2 dt where p - dt (Note that the gravitational and friction terms have been dropped from Eq. (2.4.23) for the special case.) Solving the differential Eq. (5.1.1.2) gives 3 - stPma or a 3=- /T t-2 (5.1.1.3) since the manipulator starts at rest. Obviously, minimizing the traversal time for a given path requires that we maximize the "velocity" -d This in turn requires that the power P be maximized. Therefore, the maximum distance s which can be traveled in time t is given by Eq. (5.1.1.3). Looking now at the end of the curve, we wish to have zero velocity at the end of the motion. Again, since we wish to minimize the traversal time, we want to stop as quickly as possible, which requires that we drain energy from the system as fast as possible. Applying the minimum power, dt P = (5.1.1.4) Solving this equation gives

$ 2 Pmx(T -t ), or -- S 2 -- (T - t)21.1.5) where S is the total "length" of the curve which is to be traversed and T is the (unknown) time when the destination point is reached. At some point in the middle of the curve there must be a switch from acceleration to deceleration. Let the time, distance, and velocity at this point be denoted by t,, e, and p,, respectively. Then Eqs. (5.1.1.3) and (5.1.1.5) must give identical results at the switching point, so we have P4 = 2 2Pt, 2P T - t, ) (5..1.6) and, =S- 2;P ( - t,)2 = -2 te (5.1.1.7) ~~3 3 If we eliminate t, from these equations, we can express T in terms of S. The resulting equation is 2 T -- 4P J (5.1.1.8) The total time T increases monotonically with S, so minimum distance in inertia space is, in this case, equivalent to minimum time. Therefore the geodesic, being the curve of shortest "distance" between any two points, is the optimal geometric path. U

188 The conditions under which this proof of optimality applies are not realistic, particularly the condition that gravitational terms be absent. The proof of optimality depends on the absence of gravitational terms because the presence of such terms makes the power supplied to the manipulator a function of position rather than a function only of the kinetic energy of the manipulator. The requirement that the joint torques/forces be only constrained by a total power limit for the entire manipulator is also unrealistic. However, it is possible in practice to obtain bounds on the total available power for the more general case; in the next subsection, such bounds will be used to find bounds on traversal times. 5.1.2. Traversal Time Bounds In this subsection, we show how energy methods similar to those used in the previous subsection may be used to obtain lower bounds on the time required to move from one point in the robot's workspace to another. We start with Eq. (2.4.23), the formula for the power supplied to the manipulator, namely do d2a Pt =-dt dt2 +R ipi +p dt (5.1.2.1) where Pt is the total power supplied to the robot. d8 d2s dp dR If we write Pk dt R dt2 P Pi and P g p d then we'dtd dt have Pt = P, - Pf - Pb. (5.1.2.2) We will find bounds on Pt by finding bounds on Pt, P/, and Pg.

189 Before computing these bounds, we need the following result: Lemma 5.1: The 2-norm of the vector p is bounded by: I(J) IIP I2< (J) = (5.1.2.3) ~TM-aXmJ) -- f (() VTaiT where f, (J) =- min mT Y fM(J) = max / (the quadratic form p#O pTp p3O p p Jij p' pI has been written in vector form as pTJp), Xmn(J) is the smallest eigenvalue of J for all positions q, and Xmx(J) is defined similarly. Proof: Since pi is a unit vector in inertia space, we have J _ p' pi pT jp = 1. Then we have ---- p p T p PTP min Pp ~>2 (j)II p 11 2 p p pO p p p Likewise, we have 1 = p TJ. Jp pT p p< pp Max T p < () p 2 Sinc- p i p p Since J is positive definite and symmetric, its eigenvectors span R". Expanding p in terms of the eigenvectors of J shows that f2 (J) - Xmin(J) and f2(J) = Xm(J) which, combined with the above inequalities, proves the lemma. - We now compute bounds on the total applied power Pt. It will be assumed here that bounds on Pt arise from constant bounds on the joint torques and from constant bounds on the total applied power. Then, we have

190 Lemma 5.2: If'do > 0, then max Pmin -' dt )- Pt < min P max d where PmmU and P=,, are the minimum and maximum powers that can be supplied to the robot, I U; 2 and II um l 2 is the maximum 2-norm of the torque xto the robot, min, i vector. Proof: By definition, we have Pit u p' do. The component of the torque in the direction of motion, ui p', can be bounded by ulsup I < I UNIX 12112P 11 2 < X (5.1.2.4) We therefore have P >- max {Pmns -1 u; 112 4d } maxf P in-., (5.1.2.5) and P <min(P11 UII,m 112 do a in(P Pt _<_ mindt dPmx, =MinPmi'at (5.1.2.6) o Xm.x(R) and d' ma(R) Xmin(J) Proof: By an argument similar to that used to derive bounds on II p 11 2, we have Xmin(R)'II P II 22 < pTRp Xmx(R) jI P / 22 so that, by Lemma 5.1,

191 Xm,n(R) TR < T5 X(R) Xma(J) — pRp_ X~min27(J) Multiplying by p2 gives the desired result. Lemma 5.4: The gravitational energy contribution PI is bounded by de -P dte = 1i 112 d( Proof: We have I gi P' I < 11 i 11 2 11 P' 11 2 < (5.1.2.8) by Lemma 5.1. Multiplying by ds proves the lemma. dt Using bounds derived in Lemmas 5.2 through 5.4, we are now in a position to obtain bounds on Pk Lemma 5.5: If we define p -- dt >0, max {mi - C) -'2p2 - + Pt < min (Pm= u P} - 2 + 4. Proving this Lemma is just a matter of plugging the bounds obtained in Lemmas 5.2 through 5.4 into Eq. (5.1.2.2). We can now determine maximum velocities, as was done in the previous subsection. We have the following theorem:

192 Theorem 5.2: If the initial and final velocities are zero, then -< minp m + S (1- -Ot, + (c(r-t) 1) } where T is the traversal time of the path and, = ax Proof: Consider two cases. In the first case, let Pk be limited by the joint torque bounds. Then we have -5p < Pi < Sp, so that 2 _ ( + +)p < S d < _-p2 + (V + )p. (5.1.2.9) But then, since we are considering positive values of p, - dt We must have zero velocity at the beginning and end of the path. If the (as yet unknown) traversal time is T, then # < ++S (1-ct } (5.1.2.11) and 1 < _I+ _ ((Tt) ) (5.1.2.12) In the second case, limits are imposed by the total power limits, i.e., P min < Pt < P m. Then we have n- j1p2_+ # d<pi <Pj - ~I2 + S. (5.1.2.13)

193 Again, we are only considering positive values of p; since in general Pmi, < 0, the lower bound in this inequality will always be less than zero. Therefore the roots of the lower bound occur for negative values of p, and we cannot place an upper bound on p. On the other hand, the upper bound has a positive root. Since we are starting dp at p - = 0, that value of p for which -d goes to zero cannot be exceeded, and we have CL < =m + + /+2 + P=ax (5.1.2.14) Since inequalities (5.1.2.11), (5.1.2.12), and (5.1.2.14) must all be met, the theorem follows. - To find lower bounds on traversal times, consider a manipulator, call it the eupcr-manipulator, for which the constraints on joint torques are such that Eqs. (5.1.2.11), (5.1.2.12) and (5.1.2.14) apply. Then the super-manipulator has limits only on the 2-norm of the tangential component of the torque vector and on the total kinetic energy. Since these constraints apply for the original manipulator, the old manipulator's realizable torques are a subset of the super-manipulator's, so that the super-manipulator can do anything that the original manipulator can do. Thus any path can be traversed by the super-manipulator at least as quickly as the old manipulator could traverse it. Finding the minimum traversal time for the supermanipulator therefore gives a lower bound on the traversal time for the original manipulator. Finding the lower bound on the traversal time T for the super-manipulator is simple. It is just a matter of finding a value of T such that the area under the

194 velocity vs. time curve is equal to the geodesic distance S between the initial and final points. Formally, we have Theorem 5.3: Let the times t1 and t2 be given by 1 r + 1 =5 - log I ca (5.1.2.15) and =T log + + 4+. ] (5.1.2.16) If t1 and t2 are both real and t1 < t2, then the minimum traversal time T for the super-manipulator can be found by solving the equation sa m P. x. r[ + + + S/, ] (5.1.2.17) -~ — r + r- Tlo If t is not rea or t > t then T can be found by oling the itne eq If t 1 is not real or t I> t2, then T can be found by solving the simultaneous equations ~i++~(l-e,) e)-= + (T —) 1) (5.1.2.18)

195 to 1 - ) (5.1.2.19) S +- 1) -(T - t,). rroof: Filnding the distance travelled, the area under the velocity vs. time curve, requires that we consider the two cases described above, which correspond to (i) the case in which the velocity limit p,3 is reached, and (ii) the case where it is not. Case (i) is relatively simple. First, we need to know the points where the curves described in (5.1.2.11) and (5.1.2.12) reach the limiting velocity P,,. These times may be obtained from (5.1.2.11) and (5.1.2.12) by setting p = P, and solving for t. These times are just t, for (5.1.2.11) and t2 for (5.1.2.12). Then the area under the velocity vs. time curve will be given by 0l t2 T S = | V + (1 - cdt +' dt + f +.+ (t(T-r) - 1) dt. +,+ oT l oi, 10go + S + +i 1 (5.1.2.20) - log Since S is linear in T, determining T is easy. In case (ii), we have a single switching time t,. We may match positions and velocities as was done in the special case in the previous subsection, giving the

198 equations - = /+S(1 + ~(e tl) ( - )- 5.1.2.21) (, T, = f S+ (1 - e- )d - - S f +_ ( r-'t)- 1) dt (5.1.2.22) 0 0 t, o = + t- _) (1 - S- S ((T These are just the equations given in the statement of the theorem. U Unfortunately, Eqs. (5.1.2.21) and (5.1.2.22) cannot be solved for t, in closed form. However, we can still use these equations to prove that T increases monotonically with S, and thus prove that the optimal path for the super-manipulator is a geodesic. This being known, the geodesic distance S between the initial and final points can be calculated, and Eq. (5.1.2.22) can be solved numerically. Theorem 5.4: The minimum traversal time T for the super-manipulator increases monotonically with the geodesic length S of the traversed path. Proof: To prove that T increases monotonically with S, we will show that dS d > 0. If case (i) of Theorem 5.3 holds, then the result is obvious. Case (ii) is slightly more complicated. First, we differentiate (5.1.2.21) and (5.1.2.22) with respect to the switching time t,, giving -' -t)dT - 1 (5.1.2.23)

197 __ +d +- (t dS+; dT ((T) (5.1.2.24) dS of - 1 I (5.1.2.24) Solving (5.1.2.23) for dT, lugging into (5.1.2.24), solving (51.224) for dS and Solving (5.1.2.23) for, plugging into (5.1.2.24), solving (5.1.2.24) for -,, and dS dT dividing -t by gives dS [1+0 (, - ( -{,) (5.1.2.25) + SAC' (1 -*'(2-")) ] which is greater than zero. If the actual lower bound is required, then Eq. (5.1.2.21) may be solved for T. To do this, we may make use of the equations for tl and t2. The maximum velocity p, can be varied in these equations until tl t= =,; then this value of t0 can be used in (5.1.2.23), which can be solved for T. 5.1.3. Approximate Minimum Time Paths In this subsection we consider two methods for generating geometric paths which are approximately minimum time. The first method uses the traversal time bounds derived in the previous section and the second method uses the velocity bounds derived in Chapter 4.

198 First, consider the lower bounds on traversal time. If these bounds are good estimates of the actual traversal times, then minimizing the lower bound should approximately minimize the traversal time. Since the lower bound increases monotonically with the geodesic length S of the traversed curve, geodesics (minimumlength curves) must minimize the lower bound. The "near optimal" paths may then be determined by solving the differential equations for a geodesic, namely 6 st d J = 0. This method of generating near-minimum time paths can be applied to most practical robots; however, it places no penalties on forces which are orthogonal to the traversed path, so that path curvature is not penalized. If any further constraints are applied which force the introduction of curvature terms, then ignoring the magnitude of the curvature terms could make the lower bound a poor estimate of the actual traversal time, causing a poor choice of path. The minimization of lower bounds leads to the selection of shortest-distance paths in inertia space, which could have corners at which the manipulator must come to a complete stop. Path segments of high curvature also slow the manipulator down. Thus it is necessarj to strike a compromise between curves of shortest distance and curves of smallest curvature. (This naturally leads to the second method of generating nearminimum time geometric paths.) In order to reach such a compromise, we choose as an objective function the product of the length of the curve and some measure of the total curvature. This, of course, requires some quantitative measure of both curvature and distance in an ndimensional space where n is the number of manipulator joints. One obvious measure of total curvature is the reciprocal of the maximum velocity, as computed in Chapter 4. If the path is expressed in terms of an arbitrary parameter X, then the

189 expression f0 dX ~ Pm\) (5.1.3.1) would appear to be a good choice, where p,-X and Pma(X) is the velocity limit at position X. This expression is independent of the parameterization chosen, and increases both as the length of the curve increases and as the curvature increases. In order to use (5.1.3.1), the value of the maximum velocity prc(X) needs to be computed. This is calculated from equation (4.2.1.1), namely umin < Mip + Qip2 + Rip + Si < ui. (5.1.3.2) For a given position X and velocity p, these inequalities gives a range of accelerations p, and so may be thought of as assigning upper and lower acceleration bounds to each point (X,p) in the phase plane. Since these inequalities must hold for all joints of the manipulator, the acceleration must fall between the greatest of the lower acceleration bounds and the least of the upper bounds. When one of the upper acceleration bounds is smaller than one of the lower acceleration bounds for some phase point (X,p), there are no accelerations which will keep the manipulator on the desired path. Thus the acceleration bounds generate restrictions on the velocities at the phase points which can be encountered during a traversal of the path. These relationships can be thought of as assigning velocity limits to a given position X. Now consider a frictionless manipulator, i.e., one for which the quantities Ri are zero. Also assume that at every point on the path the manipulator is capable of stopping and holding its position. Then we have

200 uIn < S S < u x (5.1.3.3) at all points on the path. (This will hereafter be referred to as the "strong manipulator assumption".) If the parameter X is defined to be the arc length s in inertia space, then Qi is just the inertia matrix Jij multiplied by the curvature vector 6p' 6s If the path chosen is a geodesic, then the curvature vector is zero, and hence Qi O0. Then the inequality (5.1.3.2) reduces to um', < M, p + Ss < Ul (5.1.3.4) which is independent of the velocity p, and by the strong manipulator assumption is satisfied identically for p=O. But if the bounds on p are independent of p, there can be no velocity limits; in other words, prnX() oo, so that the integrand of (5.1.3.1) is zero. Thus in this case the optimal solution coincides with that obtained from minimizing traversal time bounds. It may appear at first that the geodesic, since it maximizes velocity bounds, must be the true minimum time path. However, the manipulator must meet acceleration as well as velocity constraints. It would then be expected that along the optimal geometric path the maximum acceleration would be maximized during an accelerating portion of the path and the minimum acceleration would be minimized during a decelerating portion. This does not happen along a geodesic, but a similar phenomenon occurs: the acceleration bounds "spread out". To see this, note that velocity limits occur because the acceleration bounds become very close. Since the velocity bounds have been eliminated by choosing a geodesic as the path, the

201 acceleration bounds must never get close. Hence maximizing velocity bounds also gives a large range of accelerations to choose from. This would lead one to expect that geodesics are good, if not optimal, choices for geometric paths. In summary, we have two criteria for selecting near-minimum time geometric paths for a manipulator, one based on the minimization of a lower bound on the manipulator's traversal time and the other based on the minimization of the product of the path's length and its curvature. In either case, when the near-optimal path is determined, the path is found to be a geodesic in inertia space. This geodesic ca. be constructed by solving a set of differential equations and applying appropriate boundary conditions. One comment on calculating the geodesics is in order. Finding the geodesic which passes through two points is a two-point boundary value problem. Solving the problem using Pontryagin's maximum principle also gives rise to a two-point boundary value problem, so one may legitimately ask what the advantage of using geodesics is. The important difference is that the boundary conditions for finding the geodesic are easier to apply than are the boundary conditions which arise when using the maximum principle. The geodesics may be found using shooting methods, which involve guessing the initial direction of the geodesic, and refining the guess until the curve comes sufficiently close to the desired endpoint. 5.2. Numerical Examples To demonstrate the utility of the solutions described above, the traversal times for various geometric path have been calculated, using the phase plane method, for the first three joints of the Bendix PACS arm. We construct three paths: a straight

202 line, a geodesic, and a jointinterpolated curve. (The joint-interpolated curve has the form q' = q: + X(qj - q'), where 0 < X < 1 and q and q are the points at which the curve starts and finishes.) The construction of geodesics requires that the inertia matrix of the robot, Ji,, be known. For the first three joints of the manipulator the inertia matrix takes the form Jt -Kr + Mr2 0 0 Jiij0 - o M, 0 (5.2.1) O o M, where ql =_, q2 = r, and q3 = z. The constants Mt and Mz are the masses which the r and z axes must move. Jt is the moment of inertia around the 0 axis when r is zero. The K term is present because the center of mass of the structure for the r joint does not coincide with the 0 axis when r is zero. The form of the matrix is derived in the Appendix. The values of Jt, K, Mt, and M:, along with friction coefficients and actuator characteristics, may also be found there. The non-zero Christoffel symbols of the first kind (Coriolis coefficients), found by differentiating J3,, are [12,11= [21,11 = M, r - K (5.2.2) 2 [11,2]= - _ M,r. (5.2.3) 2 The geodesics are solutions of the equations 0 = j d2qi dqi dqk g2 +[ ikio d e Plugging Eqs. (5.2.1) through (5.2.3) into this equation gives the equations of the

geodesics as (d2# dr d ( o= (J - Kr + M, r )-2 + (2M, r - K) (5.2.4) o - M d2r + K - Mg d (5.2.5) ~o == M -.2 (5.2.6) In addition, we have the normality condition 2 2 2 z2 (J - Kr + Mt, + M= d - =. (5.2.7) The differential equations (5.2.4), (5.2.5), and (5.2.8) can be solved in terms of elliptic integrals. In practice, however, it is simpler to solve them numerically. The gravitational terms for this manipulator are particularly simple; the gravitational forces on the r and joints are zero, and the force on the z joint is M, g. A phase plane trajectory planner for the PACS robot was written in the C programming language and run under the UNIX3 operating system on a VAX-11/7804. The trajectory planner was used to generate trajectories for a straight line, a geodesic, and a joint interpolated curve, each of which extended from the Cartesian point (0.7,0.7,0.1) to (0.4,-0.4,0.4), all coordinates being measured in meters. Phase plane plots (plots of the speed p versus position X), plots of position q' vs. time, and plots of motor voltage vs. time are shown in Figures 5.2.la through 5.2.3c. Figures aUNIX is a trademark of Bell Laboratories.'VAX is a trademark of Digital Equipment Corporation.

204 5.2.1a through 5.2.1c are for the straight line, Figures 5.2.2a through 5.2.2c are for the joint interpolated curve and Figures 5.2.3a through 5.2.3c are for the geodesic. The traversal times for these paths are 1.782, 1.798, and 1.588 seconds respectively, showing that the geodesic does indeed have the shortest traversal time. When the robot is driven along a given path in minimum time, one or more of the actuators will be driven to its limit. For the straight line path, the r joint is driven at its maximum or minimum voltage except for two short intervals when the U joint is saturated. For the joint interploated curve, the r joint motor voltage is always the limiting factor. For the geodesic, the r joint is saturated most of the time, but both the 0 and z joints are driven to their limits at one time or another. The geodesic seems to distribute the workload more evenly among the joints than the other two curves do. Two methods (excluding the special case) have been proposed for finding geometric paths which allow a robotic manipulator to move from one point to another in minimum time or approximately minimum time; if obstacle avoidance is not' a consideration, both methods yield the same result. While these methods do not directly address the problem of obstacle avoidance, they do demonstrate that the problem of choosing minimum time paths is not simple, and in particular they show that minimum time is not in general equivalent to minimum Cartesian distance. Two approaches to the obstacle avoidance problem suggest themselves. If the geodesic which connects the desired initial and final positions of the manipulator happens to pass through an obstacle, then we may piece together geodesics to give a path which has shortest geodesic, rather than Cartesian, distance. This again has the disadvantage that the path will have corners at which the manipulator must stop,

205 but these corners could presumably be rounded off, as in [22]. On the other hand, Eq. (5.1.3.1) provides a means of evaluating the "goodness" of any given path without actually calculating the path's traversal time. If several paths can be found which avoid collisions with obstacles, then each one can be evaluated and the best one chosen on the basis of formula (5.1.3.1). This presumes that some method can be developed for generating collision-free paths quickly. It also presumes that at least some of the paths generated by the algorithm are reasonably close to the optimal path. But since minimization of the product of curvature and distance gives paths with short traversal times, some guidelines for generating paths are now available.

206 10 8.000 6.000 4.000 2.000 2.000 4.000 6.000 Figure 5.2.1a. Phase plane plot for straight line

207 i9 joint position r joint position q z joint position 1.000 0.750. - \ 0.500 0.250: 0.500 1.000 o 1.500 -0.250. -0.500 -0.750 -1000 Figure 5.2.lb. Joint position vs. time for straight line

208 0 motor voltage r motor voltage z motor voltage V 40,-% 20 \ *.. -40.... Figure 5.2.1c. Motor voltage vs. time for straight line

209 8.000 6.000 4.000 2.000 2.000 4.000 6.000 8.000 10 Figure 5.2.2a. Phase plane plot for joint-interpolated path

210'0 joint position........ r joint position q z joint position ------ -- 1.000 0.750 0.500 N. 0.250 -I jI~~~~t 0.500 1.o00 1.500',.... -0.750 0** -1.000 Figure 5.2.2b. Joint position vs. time for joint-interpolated path

211 O motor voltage -......... r motor voltage z motor voltage....... V 40 2O I ___I I t.1 0.500 1.000 1.50,-20 /0 -40 Figure 5.2.2c. Motor voltage vs. time for joint-interpolated path

212 10 8.000 6.000 4.000 2.000 2.000 4.000 6.000 Figure 5.2.3a. Phase plane plot for geodesic

213 ~~~q'1O~~~ joint Position r joint Position z joint Position. 1.000 0. 750 l..... 0.500 I -0.3500 -0.750 -1.000 -Fige 523b. Joint position rs. time for geodesic

214 0 motor voltage.. r motor voltage z motor voltage V 40. 20 I t 0.500 1.000 1.500 lli 20 9' -40 Figure 5.2.3c. Motor voltage vs. time for geodesic

CHAPTER 6 COMPENSATION FOR DYNAMIC UNCERTAINTIES In the previous chapters, it was assumed that the dynamics of the robot were known exactly. In practice, though, this will not be the case. Uncertainties in link inertias and payload characteristics will cause deviations from the nominal dynamic model. If the robot's actuators do not saturate and the dynamic errors are small, then the path tracker can compensate for these dynamic uncertainties. However, the minimum-time trajectory planners described in chapter 4 always generate nominal torques which are at the limits of the robot's capabilities for the given dynamics. Moving the robot along the desired path at speeds calculated for the nominal payload may therefore require torques which are beyond the robot's capabilities if the payload differs from the nominal one. If the robot's joints are controlled by independent servoes, as is usually the case, then attempting to make the robot move along the nominal trajectory will result in one or more joints "falling behind", so that the robot strays from the desired geometric path. In other words, the trajectory generated by the trajectory planner is realizable for the nominal dynamics, but not for the actual dynamics. Though link inertias vary somewhat from one robot to the next, a more important source of dynamic variations is uncertainty in payload mass, shape, and grip 215

216 position. The techniques described in this chapter are applicable to variations in link inertias as well as to variations in payload characteristics, but for the sake of simplicity, only compensation for payload characteristics will be described. There are a number of adaptive controllers which can compensate for the changes in load, provided that the plant (i.e., the robot joint drive) does not saturate [7, 17]. However if the plant saturates, as may happen if the actual and nominal payloads differ too much, then these controllers cannot possibly compensate for load changes. This chapter presents an analysis of the torque errors caused by payload changes, and shows how to incorporate the error information into the trajectory planning process so as to avoid saturation of the individual actuators. Changes in payload characteristics will be expressed as errors in the pscudoinertia of the payload; the pseudo-inertia is a matrix containing the mass and first and second moments of the payload. It will be shown that bounds on the joint torque errors can be calculated in terms of the norm of the error in the pseudoinertia of the payload, given the robot's kinematics. Either the dynamic programming trajectory planning algorithm or the perturbation trajectory planner can then be modified to handle uncertainties in the dynamics caused by the payload. If the actual and nominal payloads are described by the pseudo-inertias IA and IN respectively, then for a given positive real number E, the algorithm generates a trajectory which is realizable for all payloads IA for which IIIA - IN 11 < E. In order to avoid excessive torque requirements, we wish to compute a set of velocities and accelerations p and p such that if the nominal torques ui are given by U - M3l (IN)p + Q,(IN )2 + Rnp + S.(IN) (6.1)

217 then the actual torques uW' given by u', = M,(I, + AIN)I + Q,(IN + AIN)J, (6.2) + Rp + S(lIN + AIN) will be realizable, i.e., uI'"(x,ll) < ui < u m(~X,p). (6.3) Formally, the Robust Trajectory Planning (RTP) problem may be stated as follows: Given a geometric path described as a parameterized curve, the torque limits u"A and uma as functions of X and p, the dynamics of the robot when carrying the nominal payload IN, and a bound E on the norm of the difference between the pseudo-inertias of the actual and nominal payloads, determine the fastest trajectory (sequence of (X,p) pairs) such that the torques u'; given by (6.2) satisfy the constraints (6.3) for all points on the trajectory and for all payload errors AIN such that IIAIN II < E. We will solve this problem by calculating the worst-case torque error, as a function of X, p, and p, for a given payload error, and decreasing the torque limits by this amount when doing trajectory planning. 8.1. Calculation of Dynamic Coefficient Errors For a given path, we need to know the changes to the coefficients Ms, Qi, Ri, and Si in Eq. (6.1) which result from changes in the dynamics of the robot. In the sequel, changes in dynamics will be assumed to come from changes in payload characteristics. While changes in friction coefficients, and hence changes to Ri, also contribute to changes in required torques, such changes are independent of changes in payload characteristics, and for the sake of simplicity will not be dealt with here.

218 Determining the change to Mi, we have Mi (IN) = Jii(IN).. (6.1.1) d X and Mt (IN + A/N) = J,,(IN + AIN) d (..2) dX so that AM. (IN,AIN )- A (IN + AIN )-Mi(IN ) (N(6.1.3) Jij(IN + AIN) - J(IN) d. bJ.- (ININ) df X Differences between nominal and actual payload characteristics cause changes in the coefficients Ji,, Ck =[jk,i], and g, in equation (2.4.12). Here we determine the relationship between changes in these coefficients and changes in payload characteristics. Changes in payload characteristics will result in changes to the pseudo-inertia tensor of the last joint of the robot, i.e., the pseudo-inertia tensor will have the value IN + AIN instead of IN. In order to obtain AM;, we consider how this affects the inertia matrix. The coefficients in the inertia matrix are given in [28] as P( =m1(i, j) t aqJ aoqi where T is the 4X4 homogeneous transformation matrix which transforms vectors given in the coordinate system associated with the pte link of the robot to world or

219 base coordinates, and Ip is the pseudo-inertia of the p A link given in the p a link's coordinate frame. The pseudo-inertia is defined [28] as the matrix f 2dm fzy dm fzz dm fz dm f Zy dm fy2dm fyz dm fy dm I=P fzz dm fyz dm f2 dm fz dm (6.. f ddm fy dm fdm fdm The coordinates z, y, and z are expressed in the pth coordinate frame, and the integrals are all taken over the p tA link. Introducing an error AIN into the pseudo-inertia of the last joint gives Ji (IN + AIN) = - E'Tq TP I16) + Tr (IN + AIN). Subtracting (6.1.4) from (6.1.6) gives 6Ji,(IN,AIN) - =bJi(AIMN)= Tr ( ) AT N a. (6.1.7) Note that the error in the inertia matrix is linear in the pseudo-inertia error, and is independent of the nominal payload. To find 6Mi, simply plug (6.1.7) into (6.1.3), giving CM- = I J r aNa (6.1.8)

220 Computation of the errors 6Q, follows the same pattern as the computation of 6Mi. The errors in the Coriolis terms can be determined in much the same way as the errors in the inertia matrix. From [28] we have 0ijk =m (i T |I a i aq2T, dPT T|. (6.1.9) p =max (i,,k) 8qi q* Jq' The errors in the Coriolis terms due to errors in payload characteristics are therefore given by Cijk = Tr ai aqT AIN... (6.1.10) The definition of Qi gives aQ, = Tr (aTN aq d2f' (6.1.11) GI q' eql dX2 + 2TN aT dl/ df k,kaq'q aqi JdX dX Now we need to know the error in the gravitational terms. The gravitational forces g, are given by [28] gi -mGT (6.1.12) k=i raq' where

221 is the gravitational force vector, mk is the mass of the k tA link, g is the acceleration due to gravity, and is the center of mass of the k 1t link given in the coordinates of the k ^ frame. If we define wkE m rk, then we have N TaTk gi --- -GT kw. (.1W13) =.i aq But wk is just the last column of the pseudo-inertia matrix Ik, so that iN TOTk 0 $, -— G-'- Ik ~ (6.1.14) As before, introducing an error into the pseudo-inertia of the last link gives Si =6gi =-GT AIN[. (6.1.15) We may now calculate the error in ui by adding up the individual components, giving

222 bui =M AM + 6Qi + 6S. (6.1.16) 6.2. Calculation of Torque Error Bounds If the errors AIN were known exactly, then the torque errors could also be computed exactly. Of course, in practice AIN will not be known exactly. However, if bounds on the norm of AIN can be obtained, then we may find bounds on Au;. To obtain these bounds, note that AMi, 6Qi, and 6Si are all functions of the pseudo-inertia error AN; in fact, they are linear in AIN, so that Au, is also linear in AIN. If we write Au, = Z(AIN), (6.2.1) then we wish to maximize or minimize the linear function Z with respect to AIN, subject to IIAINII < E (6.2.2) where E is the bound on the pseudo-inertia error. At this point, some observations are in order. First, as we noted before, the errors in the 6us depend linearly upon the pseudo-inertia error AIN. Second, note also that 6u; depends only on the kinematics of the robot and on the desired velocity and acceleration, not on the nominal dynamics. These facts are consequences of the fact that both kinetic and potential energy are linear in mass. To see this, consider the form of the Lagrangian, namely L - K - P, where K and P are the kinetic and potential energies of the robot. Any difference between the actual and nominal

223 dynamics of the robot will cause a corresponding difference in the Lagrangian. If we think of the error in the pseudo-inertia as a piece of stray material stuck to the end effector, then both the kinetic and potential energies of this material, and therefore the Lagrangian, depend only upon the mass and shape of the stray material and upon the position and speed of the end effector. The Lagrangian is linear in the mass of the material; since Lagrange's differential equations are linear in L, the errors in the generalized forces can be separated from the forces resulting from the nominal mass of the manipulator. One consequence of this separability of forces is that the errors 6u, do not depend upon the nominal dynamics, as shown above. The implication of this is that much of the error analysis can proceed without regard to the nominal dynamics of the robot. The linearity of the 6u; in the pseudo-inertia has some other practical consequences as well. Consider the maximization which must be performed in order to evaluate bui. This maximization requires that the space of 4X4 symmetric matrices with norm less than E be searched, which in general is a rather formidable problem. However, by choosing a particular class of matrix norms, the problem can be made quite simple; in fact, it can be transformed into a linear programming problem. To see how this transformation can be performed, consider the problem of maximizing the function Z in equation (6.2.1), namely Problem A: maximize Z(M) = 2]ij Mij (6.2.3) subject to IIMII E and MMT. (24) (6.2.4)

224 Treatment of the minimization problem proceeds analogously to problem A. We will show that problem A transforms into a linear programming problem if the norm used to constrain the matrix M in (6.2.4) is chosen properly. This will be accomplished by eliminating some absolute values from the constraints. Z(M), the function to be maximized, is a linear function of M. It remains to be shown that the constraints can be made linear. Of course if the norm used in problem A is arbitrary, then in general the constraints will not be linear. However, there is a set of norms, all very easy to calculate, which will yield linear constraints. Consider the class of functions F: R4 4 —*R+ given by F (M) = max ai(M) where 4 4 ai(M)- SE Z aij IMj M kl j-1k =1 The matrix I-norm and oo-norm, max I Mij I and I Mij I are all functions of this.,j i,j form. We now show that, under suitable conditions, F is a norm. Lemma 8.1: If eaiir > 0 for all i, j and k, and if for every pair of indices (j,k) there is an i such that aiik,* O, then F(M) is a norm. Proof: In order to prove that F is a norm, we must show that 1) F(M) > O for all M, 2) F(M)-O iff M - 0, 3) F('yM) = I' j F(M) for all scalars y and all matrices M, and 4) F(X + Y) < F(X) + F(Y) for all matrices X and Y.

225 Obviously (1) is true, since F is the maximum of a set of non-negative quantities. F(O) = 0, proving the "if" part of (2). To prove the "only if" part, observe that if M is non-zero, then it has some non-zero element Mj k. For this particular jk pair, there is some i such that aijk is non-zero, so that ai > 0 for this i. Therefore F > 0. (3) is true, since 4 4 4 4,(lM) — SE Et I' Mp I - 11 E E ijk IMiP I = -Ilfi(M) j =Ik -1 j=lk=1 and hence F(yM) = max i ('yM) = max I 1 Io(M) - II max o,(M) = I I F(M). Finally, o(X + Y)= ctajk, IXi, + Yk I i k < Diu Ixk Ik + aiikIYijk I =o,(X) + oa(Y) so that F (X + Y) max (o(X + Y) < max (o (X) + o.(Y) < max o (X)} + max (o; (Y) -F (X) + F (Y) Problem A with this class of norms becomes

228 Problem B: maximize Z(M) = ijMii (6.2.5) 4 4 subject to max (k I Mj IJ < E and Mrk =Mk i j =lk-=1 This problem obviously is equivalent to Problem C: maximize Z(M) =,ISj M; (6.2.8) subject to 2 Caik IMik I < E V; and Mjk = Mk. Ik -— =I Problem C may be transformed into a standard linear programming problem by making the substitutions Mii - Pii - Nii and I Mij I Pii + Nii, where Pij and Ni% are non-negative real numbers. To prove that this substitution gives the correct result, first eliminate the symmetry constraint, giving 4 4 Problem C': maximize W(M) — E Bj Mik i ---- = I4 4 subject to E atj'ii I Mk I < E j =lk =j where j aik j k atiik — tjk + a ikj j k

227 and (l Bi — i Then we have the following theorem: Theorem 8.1: Let problem D be defined as: 4 4 maximize Z(P,N)= mE J ik (Pfi - Nk ) 4subject to ji =- -i Then the optimal W from problem C' is equal to the optimal Z from problem D. Proof: Let M~ be a solution of problem C', and let W' = W(M'). If we make the substitutions Mik Mik >0 Pik-O Mj < O and o M~ > 0 Nk - -M,; =Mk M, < 0 then we have M = Pi - Ni,, and I Mj I = Pi + Nii. Making these substitutions in problem C' gives

228 4 4 W' = W(M')= E /ik (Pji - Nik )= Z(PN) i=lk=j 4 4 Y E a'ik (Pik + Nk ) < E and P,>i 0, Nii > 0. i =1k =i The conditions for problem D are satisfied, so we must have W' < Z', where Z' is the optimal value of Z obtained from problem D. Likewise, let P' and N' be an optimal solution to problem D. Then for every pair of indices (j,k ) we have 9tk > 0,,p t = 0, or itk < 0. If,jt > 0, then we must have NAi = 0. Otherwise, we could substitute Pi + NAi for Pij and 0 for Nik; these new values still satisfy the required constraints, but increase the objective function, contradicting the fact that (P', N') is optimal. Similarly, if,jk < 0, then we must have Pit = 0. If,jk =- 0, then we may take Pik = NiA - 0, since this leaves the constraints satisfied and has no effect on the objective function. Therefore we always have either Pi = 0 or N t = 0. Taking Mii = P ji - Nij, it follows that ]Mi -I Pi} + Ni-. Making these substitutions in problem D, Z' = Z(P',N')-=- jkMjk < W' j =1k =j 4 4 T e rer k I MZ I < E i =1 =i Therefore W' < Z' < W', proving the theorem.

229 Now that torque error bounds can be obtained from pseudo-inertia errors, these results must be incorporated into the trajectory planning process. Direct use of the results derived above in the phase plane trajectory planner is not easy, since this trajectory planner requires that we solve (6.1) for # in terms of X, p, and ui. This solution is required because the trajectory planner must convert torque ranges into P ranges. When errors are introduced, we have u, ((X,) < M + Q + Ri + Si + mmin + 6Q +s + + + } m ( <M; +Ri+ Si + + + + max (.MP Qi+} + S + For given X and p, these inequalities determine a range of values of p. However, the worst-case values of EM,, 6Qi, and 6S, depend upon p, which in turn depends upon the values of 6Mi, 6Qi, and bSi, so finding the allowable range of values of p explicitly is difficult. Of course these equations can be solved numerically. For example, to find the maximum allowable value of p, the equation Mp I+ Q. p + R p + Si + max {6Mt + 6Q,2 + 6S, }= U (X,p) lAIN ll<E can be solved by bisection for p. A simpler solution to the problem is to do trajectory planning by either the dynamic programming method or the perturbation method. These trajectory

230 planners only need to have available a test function which determines whether or not a given (X,L,p) triple requires excessive torque; in effect, they automatically perform the numerical search for the allowable values of p. But such a function is easily constructed, since for a given (X,p,p) we can easily minimize or maximize 6u, in (6.1.16), and see if u, + 6u, f exceeds Umu(X,p() or us + umia falls below umin(X,p). In particular, the following algorithm checks to see if a particular (X,p,p) triple meets all the torque constraints: for each joint i do begin compute uN = M, (X) + Q (X)p2 + Ri(X)p + Si (X) compute 6u = max (6M(x\ AIN)i + 6Q (\,aIN)P2 + 6SX(\AIN)} AIIN (I<E compute 6uimi" = m in M ( ) + Q( N)2 + S(AIN)} if uN + 6u,!mn < u"mi"(X,p), then return REJECT if u.N + 6umax > u "x(X,1p), then return REJECT end return ACCEPT. It should be noted that this function is called for each (X,p) pair; it does not, for example, reject a (X,p,p) triple based on an error which is computed for all positions or all velocities. As a consequence, speed is sacrificed only when absolutely necessary to guarantee that the trajectory will be realizable for all payloads within the allowable range.

231 6.3. Numerical Examples As an example, we apply the methods of the previous section to the first three joints of the Bendix PACS robot arm. The trajectory planner uses the perturbation algorithm. The kinematics of the PACS arm are quite simple. If the coordinate frames of the base and hand are as shown in the Appendix in Figure A.2, then the coordinate transform T3 is cos 8 0 -sin 8 -r sin 0 sin 0 0 cos 0 r cos ( T3= 0 -1 0 z O 0 0 1 The calculation of the partial derivatives of T3 and the computation of the coefficients 6M,, 6Q,, and 6S, can be found in the Appendix. AWe will use the norm gHz == Fi a I H I where aii > 0. This makes the problem of finding the error bounds very simple. It is easily seen that if the functional to be maximized is Z - = F#qHii ii then the maximum over H for IiHiI < E occurs when all the H,, are zero except for those H,, for which | is a maximum; this number times E is also the maxOr,;

232 imum value of Z. If we use 1 i=i =1i -{ 1/ 2 i7 i then the resulting bounds on the I bus I are |d1X d x2 Pl ljP7l dz dZ d2 ebug < max d I dX d2 2'I' r-d: Is| + 2d- 2 + 2 + 2 d X #dX2 dX dX t2rTi + ix2 + 2 - A d +X2 dX dXi i6u I < max {dX + 2'2 d dr d2r2 d 02 dX + dX2 - dX 2 AINI The joint torques that can be applied to the PACS arm are limited by saturation of the drive motors, which gives a constant torque or force limit for each joint. In addition, there are limits on the voltages which can be applied to the motors, so we need to know how the errors in the joint torques translate into errors in the motor voltages. It will be assumed that the back-EMF constant, winding resistance,

233 and voltage source resistance are known exactly, though this is not necessary. Since for a given speed voltage is a linear function of torque, i.e., Vi = Ai us + B,, the change in voltage will be 6 V, = Ai Au;. These changes in voltage can then be added to the nominal voltage and tested against the motor voltage limits in much the same way that the torques are checked against the motor torque saturation limits. The perturbation to the nominal dynamics of the manipulator will be caused by placing a cube with edges of length L and uniform mass density p in the gripper of the robot, with its center of mass coincident with the origin of the end effector coordinate system. The pseudo-inertia of this cube is 1 pL 0 0 0 if2 0 1 pL 0 0 12 A13 - ~ 1'2 pL 12 0 o o O p" 0 0 0 pL3 The norm of this "error" is 11AI311 = pLS + - p). The maximum torque error for a given range of pseudo-inertia errors occurs when the error bound E is precisely equal to the norm of the actual pseudo-inertia error. Therefore the most stringent test of the results of the previous section is to use a tight error bound, i.e., E - l1A3ll.

234 This has been done for a cube with sides of 5 centimeters and densities of 0, 6, 12, 18, 24, and 30 grams/cc. The path traversed is a straight line from the (Cartesian) point (0.7,0.7,0.1) to (0.4,-0.4,0.4). For comparison, the true optimal solution has been calculated, using the actual dynamics (including the effects of the cube in the gripper). The results are summarized in Tables 6.3.1 through 6.3.5. Table 6.3.1 gives traversal times for the true optimal solution and for the case in which errors are included. The "percent difference" column gives the percentage by which the true optimal traversal time is exceeded. Tables 6.3.2 and 6.3.3 give minimum and maximum voltages, respectively. The actual and nominal values are both computed for the "nominal" trajectory, i.e., the trajectory which is calculated with errors included. The actual voltages are those required to move the robot with the cube in the gripper, while the nominal values are those which are required without the cube (i.e., with the nominal payload.) The minimum and maximum voltages available are -40 and 40 volts, and it is easily seen that these limits are not exceeded for any joint or for either payload. Tables 6.3.4 and 8.3.5 give the minimum and maximum torques or forces for each joint. The torque or force limits are given at the head of the column for the appropriate joint; again, the limits are not exceeded. The phase plane ( X vs. p ) plot and motor voltage vs. time plot for the zerodensity case are shown in Figures 6.3.1a and 8.3.1b. Since the error is zero in this case, the results are exact. For a density of 12 grams/cc., the optimal and nominal (i.e., with errors included) phase plane plots are shown in Figure 8.3.2a. Figure 6.3.2b gives joint positions vs. time; z and r are in meters, 0 in radians. Figures 6.3.2c through 8.3.2e give nominal and actual motor voltages required to drive the robot along the nominal trajectory for the z, U, and r joints respectively. Figures 6.3.2f

235 through 6.3.2h give the nominal and actual torques. (The nominal torques/voltages are those which would be required if the actual payload were identical to the nominal payload. The actual torques are the torques required to keep the robot with the perturbed payload on the nominal trajectory.) Figures 6.3.3a through 6.3.3h show the same plots for a density of 24 grams/cc. It was noted above that none of the joint torque or voltage constraints was violated. However, the minimum voltage for the r joint at one point meets the lower voltage limit. This indicates that the trajectory which is generated when payload errors are included is indeed the fastest possible trajectory for the given range of possible payloads; for this particular point, the worstcase payload happens to have the same characteristics as the actual payload. A larger payload would have resulted in violation of a voltage constraint. Another point to consider is the relationship between the nominal and optimal phase trajectories. It is expected that the nominal phase trajectory will be lower than the optimal trajectory; a nominal trajectory which was higher than the optimal one would lead to a contradiction of the optimality of the optimal trajectory. Also, the difference between the optimal and nominal trajectories increases as the payload error bound increases. This would be expected, since the nominal trajectory must accomodate all payloads within a given range; as the range of payloads increases, the worst-case errors also increase, resulting in more restrictive limits on the nominal torques, and hence slower trajectory traversal times.

238 Density Time (Seconds) percent difference Nominal Optimal 0 1.789 1.789 0 0 1.934 1.844 4.9% 12 2.076 1.898 9.4% 18 2.213 1.950 13.5% 24 2.340 2.002 18.9% 30 2.459 2.054 19.7% Table 6.3.1. Traversal times for nominal and optimal trajectories Minimum Voltages,'".... Density z joint jint r jint Nominal Actual Nominal Actual Nominal Actual 0 29.88 29.86 -39.93 -39.93 -40.00 -40.00 6 30.34 30.91 -38.28 -38.52 -37.94 -40.00 12 30.54 31.67 -33.05 -33.51 -36.12 -39.98 18 30.67 32.39 -24.92 -25.37 -34.47 -39.99 24 30.78 33.07 -19.94 -20.36 -33.01 -40.00 30 30.86 33.73 -18.80 -17.18 -31.70 -39.96 Table 6.3.2. Minimum required voltages for nominal and actual payloads.

237 Maximum Voltages Density z joint joint r joint Nominal Actual Nominal Actual Nominal Actual 0 37.64 37.64 39.88 39.86 40.00 40.00 6 37.40 38.03 37.14 37.73 32.62 32.60 12 36.79 38.04 17.64 18.25 27.01 27.48 18 35.78 37.61 10.61 11.21 23.79 24.94 24 35.12 37.54 7.30 7.89 21.39 23.41 30 34.69 37.69 5.39 5.98 19.51 22.39 Table 6.3.3. Maximum required voltages for nominal and actual payloads. Minimum Torques/forces Density z joint (Newtons) oint (Newton-Meters) r joint (Newtons) Limit =- -629 Nt. Limit = -170 Nt.-M. Limit - 15.7 Nt. Nominal Actual Nominal Actual Nominal Actual ~ 0 333.52 333.52 -112.29 -112.29 -9.99 -9.99 6 335.91 342.20 -104.78 -105.71 -9.47 -9.99 12 363.02 376.62 -87.70 -89.26 -9.02 -9.99 18 373.03 394.00 -62.91 -64.43 -8.61 -9.99 24 377.84 408.17 -48.15 -49.55 -8.24 -9.99 30 380.11 415.72 -39.14 -40.43 -7.92 -9.98 Table 6.3.4. Minimum required torques/forces for nominal and actual payloads.

238 Maximum Torques/forces Densit z joint (Newtons) U joint (Newton-Meters) r joint (Newtons) Limit = 629 Nt. Limit = 170 Nt.-M. Limit = 15.7 Nt. Nominal Actual Nominal Actual Nominal Actual 0 421.31 421.31 161.72 161.72 10.03 10.03 6 418.68 428.52 150.39 152.38 8.15 8.18 12 414.50 430.04 78.43 80.48 6.78 8.89 18 4068.868 429.55 51.39 53.41 5.98 8.25 24 402.17 432.31 38.18 40.19 5.36 5.87 30 400.42 437.96 30.20 32.20 4.89 5.81 Table 6.3.5. Maximum required torques/forces for nominal and actual payloads.

239 8.000 6.000 4.000 2.000 2.000 4.000 6.000 Figure 6.3.1a. Phase plane plot for zero error.

240 D motor voltage r motor voltage z motor voltage ------- V 55 1 I 0.500 1.000.500 * S -20 Ile Figure 6.3.lb. Voltage vs. time for zero error.

241 Optimal ---- With errors 4.000 2.000 2.000 4.000 6.000 Figure 6.3.2a. Phase plane plot for density 12 g./cc.

242 pjoint position. —.............. r joint position z joint position 0.500 " 0.500 1.000 *;50 2.000 -0.500 Figure 6.3.2b. Joint position vs. time, 12 g./cc.

243 Actual ------- Nominal V I. 34 /-4 5' 0 32 t;' 0.500 1.000 1.500 o2.000 Figure 6.3.2c. Nominal and actual motor voltages, z joint, 12 g./cc.

244 Actual -.-.-. -.-.. Nominal V 0.500 1000 100 2000 -20e. Figure 0.3.2d. Nominal and actual motor voltages, 0 joint, 12 g./cc.

245 Actual —.-.-.-. —.. Nominal V 20 0.O0 I.000 I 2. I Figure 6.3.2e. Nominal and actual motor voltages, r joint, 12 g./cc.

Actual Nominal F 420 400I 380 0.500 1.000 1.500 2.000 Figure 6.3.2f. Nominal and actual joint forces, z joint, 12 g./cc.

247 Actual Nominal T 0.500 1.000.00o 2. 0oo00 -50 Figure 6.3.2g. Nominal and actual joint torques, $ joint, 12 g./cc.

248 Actual Nominal F 5.000 0.500 1.000 1. 2. -5.000 F-,.i Figure 6.3.2h. Nominal and actual joint forces, r joint, 12 g./cc.

249 Optimal With errors /'4 s.ooo / /' 6.000ooo 4000 / 2.00O 2.000 4.000 6.000 Figure 6.3.3a. Phase plane plot for density 24 g./cc.

250 I joint position r joint position q z joint position ----.so00... 0.500 1.000 1.0 2e.000 -0.500 Figure 6.3.3b. Joint position vs. time, 24 g./cc.

251 Actual Nominal /i 36,, It,, i,, 0.500 1.000 1.500 2.000 Figure 8.3.3c. Nominal and actual motor voltages, z joint, 24 g./cc.

252 Actual -------—. Nominal V t 0.500 1.000 1.50 2.000 -10 Figure 6.3.3d. Nominal and actual motor voltages, 0 joint, 24 g./cc.

253 Actual ------ Nominal 0.500 1.000 1 2.000 -20 N Figure 6.3.3e. Nominal and actual motor voltages, r joint, 24 g./cc.

Actual F Nominal 420 i i i I 380 0.500 1.000 1.500 2.000 Figure 0.3.31f. Nominal and actual joint forces, z joint, 24 g./cc.

255 Actual -.-.-.-. — Nominal T Figure 8.3.3g. Nominal and actual joint torques, 0 joint, 24 g./cc.' w~~~~~~~~~~~~~~~~~~~~~1 Figure 6.3.3g. Nominal and actual joint torques, t~ joint, 24 g./cc.

258 Actual Nominal F.1 R Figure 6.3.3h. Nominal and actual joint forces, r joint, 24 g./cc. m~~r,,,,,,,,iB 3.000 f ~~~~~~~~~~~I f

CHAPTER 7 AUTOMATIC GENERATION OF TRAJECTORY PLANNERS As can be seen from the results presented in the previous chapters, 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 chapter is to describe the process of trajectory planner 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 machine 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 timeconsuming. 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 257

easy to evaluate, since a new trajectory planner can be generated quickly, and a new set of tests can be run. In the remainder of this chapter, 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. 7.1. 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 7.1.1. 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 next section.

259 7.2. 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 trajectory 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-to-end. 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

280 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 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 * (X-X\5) (Xs6-X)P s(X ), 72.1) where 0()4 if 0<0 (7.2.2) 8 0,- 1 if O>0 and p5(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

281 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 discretize the trajectory planning problem anyway, and the differential equations that need to be solved when using the phaseplane 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 the previous chapters. 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 seems to be a good choice of data structures. The data structure used in the examples in this thesis is shown in Figure 7.2.1a. 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 Mi, Qj, Ri, and Si, the parametric derivatives - and I, and the joint torques u; and motor voltages V;, are stored. A typical structure for these values (the D,, in Figure 7.2.la) is shown in Figure 7.2.lb.

262 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 Ad X and dx2, calculate the dynamic coefficients M,, Qi, dX dX2 Ri, and Si, and finally apply a trajectory planning algorithm which generates p, p, t, and u. 7.3. 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 this thesis, 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. The perturbation method isolates the torque constraints into a single constraint function, thereby isolating the torque constraint checks from the rest of the

263 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 7.3.1. 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, R,, and Si. 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. 7.4. 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, labor-intensive and errorprone part of the trajectory planner generation process, and so is the most important part to automate. Computer generation of dynamic equations may be timeconsuming, 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 systematicly.

284 However, systematic methods do exist; see [281. These systematic methods, combined with a system for doing symbolic algebra, such as MACSYMA [27] or REDUCE [101, 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 7.4.1. 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, and the various joint offsets and twists. The resulting transformation matrices can be computed as in [28]. 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 pseudoinertias, the inertia matrix, the Coriolis coefficient array and the gravitational forces can be calculated using the formulas in [28]. It is for these computations that a

265 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 twodimensional 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. 7.5. 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.

266 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 small, the resulting constraint function should produce a useful trajectory planner. 7.8. Ancillary Software As indicated in Figure 7.1.1, 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 can

267 not 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 [5J for an example.) AWhile this approach may not work for certain exotic robot designs, it probably will cover the vast majority of robots seen in practice. Another desirable feature of the driver routine is the generation of geodesics in inertia space. The traversal times for these curves are near-optimal, and so will give the robot designer an 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 7.6.1. 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. 7.7. Work Accomplished 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

268 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. Some of the rest of the code for the trajectory planner consists of fixed modules; these portions can be taken from the code written for the numerical examples in this thesis, and used either directly or with minor modifications. Though much work remains to be done, 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.

289 Curve endpoints Curve type kinematics link inertias actuator characteristics Driver Parameterized curve Automatic Trajectory Trajectory Planner Planner Generator Module Curve with timing information Stub Graphical output ) Program generation 1 Data Flow Figure 7.1.1. Overview of ATPG system

270 Number of Joints J Number of Points P Am42 ________ __ AO A1 A2 Ap| Po P1 12 PP-I Th sror1 D... PP-1I Fiur Di..lo | DlR Dphl, dat DsItrPIr a) Path data structure dq' ____ b) The structure Di,rn Figure 7.2.1. Robot path data structures

271 Parameterized curve DerivativeGenerator Curve with derivatives Dynamic kinematics Coeflicient Dynamic Equations and link inertias Planning Constraint Function Z -Actuator characteristics Calculator ithm Curve with timing information vesprio information and nominal torques Data Flow Figure 7.3.1. Trajectory planner structure Figure 7.3.1. Trajectory planner structure

272 Friction Coefficients Kinematic Data Link Pseudo-inertias Compute Tansforms IT1,...TN Compute Derivatives of Transforms OT..qJ Compute Compute Construct Viscous Inerti Gravitational Friction Matrix Matrix Terms Jij gi Compute Coriolis Array Rj J,j (jk, i] g, Figure 7.4.1. Schematic of Dynamic Equation Generator

273 link dimensions link inertias actuator data Kinematic Equation Generator Forward kinematics Inverse Dyamic Kinematics Dynamic Constraint Generator GEqnuation Function ~~~GnGeneratorra _Gene atorGenerator i dynamic inverse kinematics equations Constraint Function Geodesic Equation Planner Generator Generator Equations of.W, geodesics Driver Program Generator driver trajectory program planner Figure 7.6.1. ATPG structure

CHAPTER 8 CONCLUSIONS The aim of this thesis was to obtain methods for generating optimal point-topoint motion strategies for robot manipulators. Solving the optimal motion planning problem in its entirety will undoubtedly require much effort from many people, but the solutions of some significant subproblems have been presented here. Most of the thesis has been devoted to the trajectory planning problem, or the problem of assigning timing information to a predefined geometric path. Three methods have been presented for the solution of the trajectory planning problem: the phase-plane method, the perturbation algorithm, and the dynamic programming method. The phase plane method generates timing information which is optimal in the minimum-time sense. It may be applied to most robots which are driven by D.C. torque motors, but cannot be used if there are significant constraints on the derivatives of the joint torques or if the joint torque limits interact with one another. The perturbation method is also a minimum-time algorithm, though it could potentially be adapted to minimize other cost functions. The types of torque constraints that it can accommodate are much more-general than those that can be used by the phase plane method, and the perturbation algorithm is structured in such a way that the torque constraints and the robot dynamics can be isolated as separate 274

275 program sections or modules, making implementation for a wide variety of robots very easy. The dynamic programming method is very general; it can, in principle, optimize virtually any cost function under very general torque constraints. However, the problem must be discretized, and when the discretization is fine enough to give accurate results, the time required by the algorithm becomes prohibitive. On the other hand, a speed-up technique has been suggested which should considerably reduce the computation times. (Dynamic programming with this speed-up technique strongly resembles the perturbation algorithm.) While solving the trajectory planning algorithm for a robot with known dynamics is an important step in optimal motion planning, the dynamic equations of a robot are usually known only approximately. Since minimum-time trajectory planners generate joint torques which saturate the actuators, it is possible, if the nominal and actual dynamic models vary too much, that the torques required to keep the robot in the right place at the right time will exceed the torques computed using the nominal dynamic model. Therefore possible differences between the actual and nominal dynamic models must be taken into account by the trajectory planner. This thesis has presented a method for doing this. By using a parameterized dynamic model, torque constraints can be checked for all dynamic models with parameter values which fall into some range, rather than for a dynamic model corresponding to a single set of parameter values. This allows the computation of the fastest trajectory which guarantees that the required torques are realizable for all dynamic models in the desired range.

278 One practical aspect of the trajectory planning problem, the automatic generation of trajectory planners, has been discussed. Writing a trajectory planner is a laborious task, and should be automated; otherwise, the effort required to write the trajectory planner may outweigh the savings realized by using optimal trajectories. A framework for writing a trajectory planner generator has been presented here, including suggestions on the choice of data structures and trajectory planning algorithms. The task of writing a trajectory planner generator will certainly be a major effort, but it poses no theoretical problems. Finally, the problem of generating optimal geometric paths was investigated. The problem considered here was that of determining the geometric path which takes the robot from a given starting point to a given destination in the least time, given an unobstructed workspace. Though the problem was not solved completely, bounds on the traversal time were computed for a given geometric path, and these bounds could be minimized. The approximation obtained by minimizing these bounds seems to be fairly good, as shown by the simulation results. In addition, the sub-optimal paths produced by this method have an appealing intuitive interpretation: geometric paths with short traversal times are short in length and low in curvature. At the very least, this provides a good heuristic for finding low-cost geometric paths. The conclusions of most theses state that much work remains to be done, and this thesis will uphold that tradition. The trajectory planning problem has been explored fairly thoroughly in this thesis, and the paradigm used here for creating trajectory planners (parameterize the desired path to reduce the dimensionality of the problem, then apply your favorite optimization technique) has proven to be quite

277 successful. On the other hand, a number of control and planning issues remain unresolved. Some physical phenomena, such as Coulomb friction, are difficult to model, and how the introduction of such phenomena affect the results presented here is still unknown. At the spatial planning level, much work remains to be done also. Exact methods for computing globally optimal geometric paths in the presence of obstacles have yet to be found; the process of obtaining non-optimal collision-free paths is still an open research topic, and finding paths which are guaranteed to be optimal may prove to be prohibitively costly. If this is the case, then perhaps the results derived here will be of some use as heuristics for finding near-optimal paths in an efficient way.

APPENDIX 278

279 APPENDIX Kinematics and Dynamics of the PACS Robot The PACS (Programmable Assembly Cybernetics System) robot is a cylindrical robot arm, and was manufactured by the Bendix Corporation. (Figures A.1 and A.2 show the configuration of the first three joints of the PACS arm.) The dynamics of the first three joints of the PACS robot will be derived here, and the characteristics of the actuators for those joints will be given. The T3 matrix, which relates endeffector to world coordinates is obtained, and the partial derivatives end error coefficients required in Chapter 6 are calculated. All these derivations apply equally well to the 2-degree-of-freedom polar robot used in the numerical examples in Section 4.3; only numerical values need to be changed. The simplest way to obtain the dynamics for the PACS arm is to obtain an expression for the kinetic and potential energies of the arm by ad hoc methods. To find. the kinetic energy of the arm, first note that their is no coupling between the z joint and the r and 0 joints. We may therefore treat the z joint separately. The r and 0 joints are essentially the same as those of the two DOF polar robot. This robot consists of a rotating fixture with moment of inertia Jo through which slides a uniformly dense rod of length L, and mass Mr. On the end of the rod is a payload with mass Mp and moment of inertia J.. The payload is symmetric, and has length Lp. The moment of inertia of the sliding rod about the axis of rotation of the 0 joint

280 Jr = Mr [r2 (Lr + 2L)r + ( L + + + Adding up the kinetic energies of the various parts of the robot, KE1 [ J + J)22 + Mp r2 + r 202 The additional kinetic energy due to z axis motion is just 2 (M, + M, + M1 )Z2 If we define the constants J Jo+ JP, +M,(L +LLP +.3 } Mt — M + M, K = M,(L, + 2Lp) M, M, + M, + M/ then the inertia matrix is Jt- Kr + Mir2 0 0 Jii = 0 Mt 0 (A.1) 0 0 M, where ql _ 0, q2 - r, and qS = z. The Christoffel symbols of the first kind (Coriolis coefficients) are found by differentiating J.. The non-zero coefficients are [12,1] = [21,1] = At r -K (A.2)

281 [11,2] K - M, r. (A.3) 2 The gravitational forces g; are easily seen to be zero for the r and U joints, and AfM g for the z joint, where g is the acceleration due to gravity. The PACS actuators will be modelled as ideal motors. The circuit for each joint drive is as shown in Figure A.3. It consists of a voltage source, a resistance R', an inductance L, and an ideal motor, i.e., a device which generates a torque proportional to the current passing through it. The voltage source is the power supply, the resistance is the sum of the voltage source resistance and the motor winding resistance, and the inductance is the inductance of the motor windings. It will be assumed here that the inductance L can be neglected. This frequently is the case for D.C. motors, since the electrical time constant of such systems is generally much shorter than the mechanical time constant. Given that the torque r is proportional to the current, i.e., r = k I, it can be shown from conservation of power that the voltage V, across the ideal motor is just km w, where w is angular velocity. Since, if the motor is not in saturation, r k, I and v= vm v, - k.w I =-'- -=' where V' is the source voltage, we can solve for R R I torque in terms of voltage and angular velocity, giving r= =_ V, - - w. (A.4) R' Rm Assuming the power supply has constant voltage limits of Vmi and Vm, this gives torque limits of

282 V... w <<LVmu _ W( ). (A.5) R R iR - -- R" R In addition, at some point the iron in the motor saturates, with the result that increasing the current through the motor has no effect on the torque. This yields two more (constant) torque limits, so we also require that -r'd < r <'t. (A.6) Taking the gear ratio k into account, this gives torque limits of Utmi3 = max |-'* "' k Vi (k i")2 d q' (A7).usi* max -If —dX (A.7) and U == rimt k mq (A.8) I k Ri'3k' Rm-(kl)2dXp Table A.1 gives the dynamic and actuator characteristics for the polar manipulator, while Table A.2 gives the data for the for the PACS arm. The kinematics of the PACS arm, required in Chapter 8, are quite simple. If the coordinate frames of the base and hand are as shown in Figure A.2, then the coordinate transform T3 is easily shown to be cosU O0 -sin # -r sin l sin 00 cosU0 rcos (A.9) T3 0 -1 0 z 0 0 0 1 The partial derivatives of T3 are

283 0 0 0 0 -sin O 0 -cos -rrcos ] aT 0 Tos 0 -sin i -r sin A.10) GI O 0 atP -- 0 0 0 0 0000 0 0O O 0 0 -sin: aTr O0 0 cos a - =o 00 0 I (A.11) 000 0 There are two non-zero second partials; they are cos 0 sin S rsin ] a2T3 -sinO0 0 -cos 0 -rcosO A and O0 0 -os o a2T3 O O O -sin 0 1000 0 0o 000 0 We are now in a position to compute the 6Mj, as described in Chapter 8. If we let H = AIN, the pseudo-inertia error, and define mi, = Tr.H then we have 6Mi mi d

284 Computing the mi, 00 0 0 Taking the trace of this gives mz == H44. [0 0 0 0 -sin cos$ 0 0 aT3 aT3T 10 0 0 0 0 0 O Hz = H14 H24 Hs34 H44 -cosO -sine 0 0 O O O 0 I L-rcos -rsin# 0 0 The trace of this matrix is zero. lOT aT3Ts mr = = Tr (?z H d3r 0 0 0 o0 0 00 aT3 aT3T 0 0 0 0 0 0 0 0 - =H H14 H24 H34 H44 The trace of this matrix also is zero.. - TT (.. aT3 u H T3r j MOO T3

285 -sin -cos -rossl HI, H12 H13 H14 0T3 aTST [cos O -sinO -rsinj H12 H22 H23 H24 aH H a 0 0 0 0 HI3 H23 H3 Hal 0 0 0 H14 H24 H34 H44 The diagonal entries of this matrix are c1 == Hllsin28 + 2H13sinOcos0 + 2H14r sinlcosl + H33cos20 + 2Hs,4r cos2l + H44tr2cos2d C22 H11cos2D - 2H3ssinOcosO - 2H 14rsinlcosO + H33sin20 + 2H34r sin28 + H44r2sin2D C33 C44 0. Adding these up to get the trace, mna =e Tr a 2 J HT + H 33+ 2rH34 + r2H44 aT3 aT3T The diagonal entries of H are Mar MrO C, 9

11 -- H14sin20 + HS4sinlcosl + H44r sinOcosl e22 = H14cos20 - Hs4sinlcosl - H44r sinlcosf e33 = 44 = O. Adding these up gives mer = H14. mr., = Tr f H. 0 0 O -sin HIll H12 H13 H14 0 0 O o T3 aT3T O O O os H12 H2 H23 H24 0 0 00 dr ar O O1 o o1 H 13H23 H33 H o O O O 0 0 0 0 H1 H H H14 s2in cos 0 01 H44sin28 -H44sinOcosO 0 0 -H,,sincos$ H44cos2e 0 0 _ 0 0 00 The trace of this is m,, - H44. We can now find the bMj. We have dz H4" dA 6Ae = C(H11 + H33 + 2rH3 + r2H4) d + H dr 6M, H4 =H d- + H44 d

287 If we define c,,k by Cej = Tr a aNk H ag then we have d2f i df i df k We now compute the cjk Only six cases need to be considered, since all but two of the second partials of T3 are zero. c, = Tr a2 H 8T Z -cosO 0 sint r sin HI, H12 H13 H14 [ 0 0 0O 82T3 aT3 r-sin O0 -cos -r cosj H12 H22 H23 H24 10 0 0 0 as 820 0 0 0 Hz H3 H23 H3 H 04 0 0 10 0 0 0 H14 H24 H34 H44 0010 The trace of this matrix is zero. a(2T3 8T3 cee = Tr 2 H a2T3 aT3 The diagonal terms of -2 H are e 11, H lsinfcosf - H13sin2U - H14r sin20 + H13cos28 - H33sin0cosl - H34r sinlcosO + H14r cos20 - H34r sin8cosO - H44r2sinecosU c22 = -H11sinfcos9 - H13cos2G - H14r cos20 + H13sin28 + H33sinOcosO + H34r sinlcos9 + Hl4r sin2S + H34r sintcosS + H44r2sinkcosl

288 e33 = ~44 = 0 The trace is zero. c, ~=Tr 3H 1 82T3 aT3 The diagonal terms of -a3 H-. are 892 8r C. H14sin coss - H34sin2p - H44r sin2l C 22 -H 4sinfcosO - H34c0s20 - H44r c052l e 33 C44 = 0. The trace therefore is cr e -H34 - H44r. = a2T3 -T3 J ~,zO, =Tr aar 8z 0 0 0 -cosl 1H H12 H12 H3 H14 r d2T3 T 3 O O 0 -sinG H12 H22 H23 H24 0 O O 0 dOdr dz [0 O O H13 H23 H33 H3 O O O O O O O 0 H14 H24 H34 H44 0 1 0 The trace of this matrix is zero. C =, Tr a ar - 1T [ dO2T3 d T3 The diagonal terms of - H are dO dr 89

289 c 11 - H14sinlcosl + H34cos20 + H44r co0s20 e22 = -H14sinBcos0 + H34sin20 + H44r sin20 C33 = C =44 0. The trace is c"e, = H34 + H44r. 22T3 c T3| The diagonal terms of,2 T3H H r3 are O aar 7r ll, - H44sinOcosl C 22 -H44sinlcosO C 33 C 44 = 0. The trace is zero. Calculating the 6Qi, 6Q = H= e d2z -H44 d -2 2H d2o d2r + 2(H dr d t 6Q - = (H11 + H33 + 2rH34 + r2H + H + 2 + d d X d6Q H1 d2r b&, - Hilt d X2 + H. — 2- (H34 +,H44 d) d{X

290 The 6Si, the gravitational error coefficients, are easily seen to be 6S- = H44g and 6S -= bS, =r 0, where g is the acceleration due to gravity.

291 Constant Description Value Jo Moment of inertia of 0 joint 10o- Kg -M M, Mass of sliding rod 4.0 Kg. L, Length of rod 2.0 M. Jp Moment of inertia of payload 10 Kg. -M Mp Payload mass 1.0 Kg. LP Length of payload 0.1 rmax Maximum force on r joint 1.0 Kg. -M/eec2 umax Maximum torque on O joint 1.0 Kg. -M2/ee 2 k, Friction coefficient of r joint 0.0 (low friction) Friction coefficient of r joint 15.0 (high friction) kg Friction coefficient of U joint 0.0 Table A.1. Characteristics of the polar manipulator

Parameter Description Value rOf a Saturation torque of U motor 2.0 Nt.-M.'rsa Saturation torque of r motor 0.05 Nt.-M. rzef Saturation torque of z motor 2.0 Nt.-M. r~Vament~i" I Lower voltage limit for 0 joint -40 v. rVtri Lower voltage limit for r joint -40 v. Vni m Lower voltage limit for z joint -40 v. V# " Upper voltage limit for $ joint 40 v. ~VT Upper voltage limit for r joint 40 v. ~VImC Upper voltage limit for z joint 40 v. k Gear ratio for l drive 0.01178 k,9 Gear ratio for r drive 0.00318 Meters/radian k. Gear ratio for z drive 0.00318 Meters/radian k e Motor constant for U joint 0.0397 Nt.-M./amp kim Motor constant for r joint 0.79557 X 10-3 Nt.-M./amp | km Motor constant for z joint 0.0397 Nt.-M./amp; R "I Motor and power supply resistance, 0 joint 1 n Rrm Motor and power supply resistance, r joint 1 n R_ Motor and power supply resistance, z joint 1 n ke Friction coefficient of U joint 8.0 Kg./sec. k, Friction coefficient of r joint 4.0 Kg./sec. kr Friction coefficient of z joint 1.0 Kg./sec. Mt Mass of r joint 10.0 Kg. M: Mass of z joint 40.0 Kg. Jf Moment of inertia around 0 axis 12.3183 Kg.-M. 2 K Moment of inertia offset term 3.0 Kg.-M. Table A.2. Characteristics of the PACS arm

293 z ARM TORSO BASE el / Y Fgr/ A o t - l X Figure A.I. Schematic of the first three links of the PACS robot

294 t9~1 10 FgrA2Likco IntefasofthPASrb ~so T ~I Figure A.2. Link coordinate frames of the PACS robot

295 R, L In Vm Ideal Vt ~Motor I gur A3 PA S a t ao crc i d aga Figure A.3. PACS actuator circuit diagram

REFERENCES 296

297 REFERENCES [1] A. K. Bejczy and R. P. C. Paul, "Simplified Robot Arm Dynamics for Control," Proceedings of the 20th Conference on Decision and Control, pp. 261262, Dec. 1981. [2] R. Bellman, "Functional Equations in the Theory of Dynamic Programming - VI, A Direct Convergence Proof," Annals of MAathematicj, vol. 65, no. 2, pp. 215-223, March 1957. [3] 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, pp. 782-787, June 1983. [4] M. J. Chung and C. S. G. Lee, "An Adaptive Control Strategy for Computer-based Manipulators," report #RDS-TR-10-82, University of Michigan Center for Robotics and Integrated Manufacturing, Ann Arbor, MI, August 1982. [5] 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. [6] S. Dubowsky, "On the Adaptive Control of Robotic Manipulators: The Discrete-Time Case," Procecdings of the Joint Automatic Control Conference, June 1981, section TA-2B. [7] S. Dubowsky and D. T. DesForges, "The Application of Model-Referenced Adaptive Control to Robotic Manipulators," ASME Journal of Dynamic Systema, Measurement, and Control, vol. 101, pp. 193-200, September 1979. [8] E. G. Gilbert and D. W. Johnson, "Distance Functions and their Application to Robot Path Planning in the Presence of Obstacles," CRIM technical memo #RSD-TR-7-84, University of Michigan Center for Robotics and Integrated Manufacturing, Ann Arbor, MI., July 1984. [9] D. Ter Haar, Elements of Hamiltonian Mechanics, Second edition. Pergamon Press, 1971.

298 [10] A. C. Hearn, "REDUCE User's Manual," Report UCP-19, University of Utah, 1973. [11] 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. [12] B. K. Kim and K. G. Shin, "An Efficient Minimum-Time Robot Path Planning Under Realistic Constraints," Proc. 1984 American Control Conf., pp. 296303, June 1984. [13] 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. [14] B. K. Kim and K. G. Shin, "An adaptive model following control of industrial manipulators," IEEE Trans. Aerospace and Electronic Systems., vol. AES-19, no. 6, pp. 805-814, November 1983. [15] D. E. Kirk, Optimal control theory: an introduction. Englewood Cliffs, New Jersey: Prentice-Hall, 1971. [16] A. J. Koivo and T. H. Guo, "Control of a Robotic Manipulator with Adaptive Controller," Proceedings of the 0td IEEE Conference on Decision and Control, pp. 271-278, Dec. 1981. [17] A. J. Koivo and T. H. Guo, "Adaptive Linear Controller for Robotic Manipulators," IEEE Transactions on Automatic Control, vol. AC-28, no. 2, pp. 162-170, February 1983. [18] V. Lakshmikantham and S. Leela, Differential and Integral Inequalities. New York: Academic Press, 1969. [191 C.-S. Lin, P.-R. Chang, and J. Y. S. Luh, "Formulation and optimization of cubic polynomial joint trajectories for mechanical manipulators," Proc. 21 CDC, pp. 330-335, Dec. 1982. [20] T. Lozano-Perez, "Spatial Planning: A Configuration Space Approach," A. I. memo 605, MIT Artificial Intelligence Laboratory, December 1980. [21] T. Lozano-Perez, "Automatic Planning of Manipulator Transfer Movements," A. I. memo 608, MIT Artificial Intelligence Laboratory, December 1980.

299 [22] 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. [23] J. Y. S. Luh and M. W. Walker, "Minimum-Time Along The Path for a Mechanical Arm," Proceeding# of the IEEE Conference on Decision and Control, pp. 755-759, December 1977. [24] J. Y. S. Luh, M. W. Walker, and R. P. C. Paul, "Resolved-Acceleration Control of Mechanical Manipulators," IEEE Transactions on Automatic Control, vol. AC-25 No. 3, pp. 468-474, June 1980. [25] J. Y. S. Luh and C. E. Campbell, "Collision-Free Path Planning for Industrial Robots," Proceedings of the 21't CDC, pp. 84-88, Dec. 8-10, 1982. [26] J. Y. S. Luh and C. S. Lin, "Automatic Generation of Dynamic Equations for Mechanical Manipulators," Proceedings of the Joint Automatic Control Conference, June 1981, Section TA-2D. [27] J. Moses, "Algebraic Simplification: A Guide for the Perplexed," Proceedings of the 2" ACM Symposium on Symbolic and Algebraic Manipulation, pp. 282-304, 1971. [28] R. P. C. Paul, Robot manipulators: Mathematics, programming, and control. Cambridge, Mass.: MIT Press, 1981. [29] K. G. Shin and N. D. McKay, "Minimum-Time Control of a Robotic Manipulator with Geometric Path Constraints," Proceedings of the A22' CDC, pp. 1449-1457, Dec. 1983. [30] 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. [31] W. M. Silver, "On the Equivalence of Lagrangian and Newton-Euler Dynamics for Manipulators," Proceedings of the Joint Automatic Control Conference, June 1981, section TA-2A. [32] J. L. Synge and A. Schild, Tensor Calculus. New York: Dover Publications, 1978.

UNIVERSITY OF MICHIGAN ll 11i 1 l i 1ll il 11 1 iIIll ll iillI {fir 11111 il 3 9015 03095 0383 300 33 D.. Whitey, "Resolved Moton Rate Control of Maip and Human Prostheses, IEEE Tranaactio, n Man.Macie Sytem vol. MMad0, no. 2, pp. 47-53, June 196%neysej o.Mn9. yD