RSD-TR-17-84 SELECTION OF MINIMUM TIME GEOMETRIC PATHS FOR ROBOTIC MANIPULATORS1 Kang G. Shin Neil D. McKay Department of Electrical Engineering and Computer Science The University of Michigan Ann Arbor, Michigan 48109-1109 November 1984 CENTER FOR ROBOTICS AND INTEGRATED MANUFACTURING Robot Systems Division COLLEGE OF ENGINEERING THE UNIVERSITY OF MICHIGAN ANN ARBOR, MICHIGAN 48109-1109 1The work reported here is supported in part by the NSF Grant No. ECS-8409938 and the US AFOSR Contract No. F49620-82-C-0089. Any opinions, findings, and conclusions or recommendations in this paper are those of the authors and do not necessarily reflect the view of the funding agencies.

RSD-TR-17-84 TABLE OF CONTENTS 1. INTRODUCTION.................................................................................. 3 2. PROBLEM STATEMENT...................................................................... 7 3. SOME DYNAMIC PROPERTIES OF MANIPULATORS......................... 8 4. MINIMUM TIME GEOMETRIC PATH PLANNING................................ 14 4.1. A Special Case............................................................ 14 4.2. Approximate Minimum Time Paths................................................ 17 5. EX A M PLES........................................................................................... 25 6. CONCLUSIONS..................................................................................... 29 7. REFERENCES................................................................... 30 11

RSD-TR-17-84 ABSTRACT A number of trajectory or path planning algorithms exist for calculating the joint positions, velocities, and torques which will drive a robotic manipulator along a given geometric path in minimum time. However, the time depends upon the geometric path, so the traversal time of the path should be considered again for geometric planning. There are algorithms available for finding minimum distance paths, but even when obstacle avoidance is not an issue minimum (Cartesian) distance is not necessarily equivalent to minimum time. In this report, we have derived and shown geodesics as (i) an exact form of the minimum time geometric paths under certain restricted conditions, and (ii) an approximate minimum time geometric paths under more general conditions. As a numerical example, we have applied the path solution to the first three joints of the Bendix PACS arm, a cylindrical robot. This example has indeed demonstrated that geodesics require less time than Cartesian straight-line (i.e. minimum-distance) and joint interpolated paths. Minimum Time Geometric Paths 2

RSD-TR-17-84 1. INTRODUCTION Productivity increase is the goal of the utmost importance in contemporary automation with programmable robotic manipulators. Driving robotic manipulators as fast as possible, i.e. minimum time control of manipulators, is an important means of achieving this goal. Minimum time control of manipulators generates several interesting but difficult control and planning problems. This report is intended to treat one such problem, that is minimum time geometric path planning for manipulators. Loosely speaking, the problem of minimum time control (MTC) of a manipulator is concerned with the determination of control signals that will drive the manipulator from a given initial configuration to a given final configuration in as short a time as possible, given constraints on the magnitudes of the control signals and constraints on the intermediate configurations of the manipulator, i.e. given that the manipulator must not hit any obstacles. In general, it is extremely difficult, if not impossible, to obtain an exact closed form solution to the MTC problem due mainly to (i) the nonlinearity and coupling in the manipulator dynamics, and (ii) the complexity involved with collision avoidance. One way to sidestep the collision avoidance problem (ii) is to assume that the desired geometric or spatial path has been specified a priori. As to the difficulty (i), although there are a few suboptimal solutions derived using approximate manipulator dynamics [2, 3], the MTC problem is usually divided into two subproblems, i.e., trajectory (or path) planning and trajectory (or path) tracking, each of which is then solved separately. This division of the MTC problem can best be explained by Figure 1. From a task planner we obtain an ordered sequence of points in Cartesian space which represent a collision-free path if we connect them properly (e.g. by spline Minimum Time Geometric Paths 3

RSD-TR-17-84 functions or straight line segments). The geometric path generator (a) transforms these Cartesian points to the corresponding points in joint space,1 and (b) using the transformed points in joint space, generates geometric paths which are parameterized curves in joint space, and chooses the best one in the sense of minimizing the time. 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. With the division outlined above, we have formulated in [8, 9] the minimum time trajectory planning (MTTP) problem to determine controls which will drive a given manipulator along a specified curve in joint space in minimum time, given constraints on initial and final positions and velocities as well as on control signal magnitudes. Since a geometric path can be described as a parameterized curve, and the geometric path is assumed to be given, trajectory planning is relatively simple. By introducing a single parameter which describes the manipulator's position, the dimensionality of the problem has been reduced considerably. The current state (joint positions and velocities) of the manipulator can be described in terms of the parameter used to describe the geometric path and its time derivative. The MTTP problem is therefore essentially a two dimensional minimum time control problem with some state and input constraints. More formally, assume that the geometric path is given in the form of a parameterized curve, say q=f (X), O<\< max (1) where q' is the position of the i-th joint, the initial and final points on the trajectory 'This transformation requires the solution of the inverse kinematic problem, which is not, however, a subject of this report. 4 Minumum Time Geometric Paths

RSD-TR-17-84 correspond to the points X=O and X=Xmax' respectively, and the functions I' are continuous and piecewise differentiable. Also assume that the bounds on the actuator torques can be expressed in terms of the state of the system, i.e. the manipulator's speed and position, so that um"n(q,q) < ui < umnX(q,q) (2) where ui is an n-dimensional vector of actuator torques/forces; umax and u1min are ndimensional vectors that represent the maximum and minimum torque bounds, respectively; and n is the number of joints that the manipulator has. Note that (a) the torque bounds are usually functions of joint position and velocity, and (b) the vector inequality (2) denotes component-wise inequalities. Given the functions fi, the inequality (2), the desired initial and final positions and velocities, and the manipulator dynamic equations to be given in Eq. (4), the MTTP problem is to find q(X) and q(X), and hence the controls u (X) which minimize the time T given by T / ma= dt = r d 1 T - 1-'dt L m dk = L I dk (3) _dX where dt- is the "speed" of the manipulator. See [8, 10] for more detailed dt descriptions of our solution to the MTTP problem. Bobrow et. al obtained similar solutions independently of ours [1]. In terms of the trajectory planning problem, the geometric path planning problem is the problem of picking the parametric functions /i. In contrast to 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, Minimum Time Geometric Paths 5

RSD-TR-17-84 thereby leading to a more difficult problem to solve. Some of the earlier trajectory planning techniques restricted the form of the geometric paths to a set of corner points connected by straight lines [4. 7]. The trajectory planner then "rounds off" the corners. But straight lines are not simple motions to produce for most manipulators, and so are not necessarily the best paths to choose. It will be assumed in this report that a more flexible trajectory planning scheme like those in [1, 8] will be used. It should also be noted that the shortest path may not be the minimum time path. In particular, the shortest Cartesian path may have one or more sharp corners,2 and the manipulator would have to come to a complete stop at these points. This is certainly undesirable in view of minimum time control of manipulators. In this report, we will develop a method for determining the minimum time geometric path for our previous trajectory planners described above and in [8, 101. This is a significant departure from most of the conventional planning methods in which geometric path planning [6, 5] and trajectory planning have been performed separately and independently. Due to the intimate, synergistic relationship between the two, it is clear that the conventional methods will lead to inefficient solutions, e.g. not truly minimum time trajectory solutions or even infeasible solutions. Specifically, we intend in this report to remedy this inefficiency by combining both geometric path and trajectory plannings. This report is organized as follows. In Section 2, we state formally the minimum time geometric path planning (MTGPP) problem to be solved in conjunction with tra2Even if the Cartesian path does not have such sharp corner points, it does not usually yield a minimum time trajectory, e.g. consider a cylindrical manipulator along the shortest Cartesian path around its waist. 6 Minumum Time Geometric Paths

RSD-TR-17-84 jectory planning. Section 3 discusses some interesting dynamic properties of manipulators that are useful for deriving solutions to the MTGPP problem. In Section 4, we present and show geodesics as (i) an exact solution to the MTGPP problem under certain restricted conditions, and (ii) an approximate solution to the MTGPP problem under more general conditions. Section 5 provides a real feeling for our solutions by presenting some examples which are based on the first three joints of a cylindrical manipulator, called the PACS arm, manufactured by the Bendix Corp. The report concludes with Section 6. 2. PROBLEM STATEMENT The minimum time trajectory planning algorithms described in [8, 10] give the time history of manipulator's position, velocity, and joint torques required for the minimum time traversal of a given geometric path. However, these algorithms give no firm indication of how to pick a geometric path. The chosen geometric path ideally should be that which avoids all obstacles and can be traversed with the minimum time. In conjunction with trajectory planning, the minimum time geometric path planning (MTGPP) problem can be stated as follows. Problem MTGPP: Given the solution to the minimum time trajectory planning problem, choose the best geometric path or function f' in Eq. (1) so as to minimize the traversal time. We will take approaches to this problem which are totally different from the conventional control techniques such as use of the Pontryagin's maximum principle.3 First, the minimum time path will be determined for a restricted class of robotic manipulators 3Use of the maximum principle does not in any way lead to a workable solution. Minimum Time Geometric Paths 7

RSD-TR-17-84 using some geometric techniques. Second, a method will be proposed for generating approximate minimum time geometric paths for more general manipulators. Note that these solutions are derived for collision-free space motions. Extension of the solutions to the case of obstacle avoidance is also indicated in the Conclusion. We begin in the next section with a few dynamic properties of manipulators that are needed. 3. SOME DYNAMIC PROPERTIES OF MANIPULATORS In this section we will introduce some properties of manipulators which will prove to be useful later on. Most of these properties relate to the "inertia space" of the manipulator, i.e. that Riemannian space which has the manipulator's inertia matrix as its metric tensor. (See [11] for an explanation of Riemannian spaces and tensor notation.) Then the dynamic equations of a manipulator can be derived from Lagrange's equations, and take the form ui= J, v + [jk,i ]vvk + Rivi + g. (4) where ui is the generalized force/torque applied to the i-th joint, vi is the generalized velocity of the i-th joint, Jy is the inertia matrix, Ryi is the viscous friction matrix, and gi is the gravitational force on the i-th joint. The Einstein summation convention has been used here, so that every term with a repeated index is summed over that index, i.e. from 1 to n for an n-jointed manipulator. It should also be noted that Jiy, Rij, and g, may in general be functions of the generalized coordinates q'. The symbol [jk,i ] is called the Christoffel symbol of the first kind, and is defined by 8 Minumum Time Geometric Paths

RSD-TR-17-84 1 aIJy 9Jk OSyk \ ki ] 2 + -dJ - dJ (5a) 2 aqk 9q* 9q'c J The Christoffel symbol of the second kind is defined by {k} J'rn k,m I (5b) The inertia matrix Jim, written with superscripts, is just the matrix inverse of the inertia matrix Jij, i.e. J'1 Jj = 6k where 6k is the Kronecker delta or identity matrix. Eq. (4) can be written as u = J 6 + Ry v + g (6) St ii where the operator -- is the absolute derivative with respect to time. The absolute derivative of a contravariant vector (like the velocity v') with respect to the scalar X is defined as 6a' da' +i i dqk () 60 - dO jk dO The Riemannian approach to geometry is to specify a metric for a given space, where the metric is quadratic in the differentials of the coordinates of the space, and investigate the properties which the metric imposes upon the space. If, for example, s denotes arc length and the metric tensor of the space is Aij, then we may define the infinitesimal arc ds by the formula ds 2 = Aj dq' dq'. The metric tensor Aj may in general be a function of the coordinates q. If the metric tensor is just the identity matrix Si, then we have the usual sum-of-squares form associated with Euclidean Minimum Time Geometric Paths 9

RSD-TR-17-84 geometry. Other metric tensors give other geometries. For our purposes, we will define arc length s by the quadratic form ds 2 = Jij q dq' d. Since the kinetic energy of the manipulator is given by K=- 1 J dq' dq' (8) 2 dt dt it can be seen that the arc length ds in this space is related to the kinetic energy K of 2 the manipulator by the formula - d = 2K. { dt' The dynamic equations may now be expressed in terms of the arc length s and the time derivatives of 8. We have, since the absolute derivative obeys the chain rule, 6v' ds ui = J d + R v + g (9) dqd (9) Using the relationship v =, then d8 dt 6 [p,dds } d d s Ui = --- Ji p Ji "d dt + Rii pi' d' + gi (10) dfq' where pi -dq is the unit tangent to the manipulator's path. But ds ~ A dr ~ = P d8 + p i iA d8 b Adt 6 dt 6s8 dt (11) p ds pi d ds1 = - dp ds r d 68 dt ds dt j since the absolute derivative of a scalar is just the ordinary derivative. Plugging this into the dynamic Eq. (10), 10 Minumum Time Geometric Paths

RSD-TR-17-84 2 U Spj ds \ds d [ds ds i ii68 j dt J i dt d d tJ dt da [ds ] 2 Using the identity ds d [d d [ ds j d28 Udt d t Jt d — d J dt Uj = J PJ( + Ji d 2 + RPidt +J g +R+' (13) 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 mv term encountered in uniform circular motion. The second term, likewise, looks like the classical ma term one sees in onedimensional problems. 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 W= fuidqif = u ds uip' ds (14) Plugging in the expression for u, from Eq. (12), Minimum Time Geometric Paths 11

RSD-TR-17-84 W,[J pi }s dP t+ Jpi p d d (15) +Jp d8 dtd dts (15) +f'R,;p'pi d + fg,p'd + Rijppi ds d + g/pP ds Using the facts that the curvature vector ap is orthogonal to the unit tangent pi 6s and that pi is a unit vector, i.e. that Jij p' p' = 1, Eq. (15) transforms to w= t ds. d ds + d R ij pd P ds + gp' (16) 2 I dldt d.9l ~rfjdt d t+Js d (6 2 -1 di | + Rij d + fgp ds The power consumed by the manipulator at any given time is just dW ds d2 1 ds _- dt dt 2 + Rijp P - - + gpi P (17) -T dt dt2 t dt (17) In order to obtain minimum-time geometric paths, we need to know one more property of manipulator dynamics. This property is that of reversibility. To give an example, consider any frictionless manipulator. If the torques applied to the joints of this manipulator are given by u(t) over some time interval [0,T ], the initial and final positions are A and B, and the initial and final velocities are vA and vg, what can be determined about the torques required to move the manipulator from B back to A along the same curve? If the velocity starting at point B is -vB and the final velocity at point A is -vA, then the torques required to go from B to.4 will just be given by u(T-t) tE[0,T], i.e. the torques required to drive the manipulator from A to B only 12 Minumum Time Geometric Paths

RSD-TR-17-84 reversed in time. To prove this result, consider the dynamic equations of a frictionless manipulator, namely ui(t) = Jij(q(t)) d q(t) + Lk,i ](q(t)) dq(t) dqk(t) + gi(q(t)) (18) dt2 dt dt When t is replaced by T-r, differentiation with respect to t becomes d dr d d dt dt dr dr Eq. (18) then becomes d2q' (T-T) ui(T-r) = J (q(T-r)) d2q (19) + Ijk,i ](q(T-r) dq-(T-r) dq (T-r) + g,(q(T-r)) After cancelling the minus signs in the Coriolis terms, this is just the original dynamic equation with t replaced with r, so that q(T-r) is also a solution to the dynamic equations (18) if r is replaced by t. Since the velocities are negated with time reversed, the initial and final velocities will be negated, as previously stated. The usefulness of the reversibility property comes from the fact that it allows the construction of geometric paths starting from either the beginning or the end of the path. It implies that the results will be the same whether we start at the beginning and work forwards or start at the end and work backwards. It also allows paths to be constructed starting at both ends simultaneously, provided the paths meet at some point and have velocities which add to zero. Note, however, that the reversibility Minimum Time Geometric Paths 13

RSD-TR-17-84 property does not hold if Rij is not negligible. 4. MINIMUM TIME GEOMETRIC PATH PLANNING As was previously pointed out, use of the maximum principle for solving the MTGPP problem is practically impossible. Alternative approaches must be sought. First, using the dynamic properties discussed in Section 3 we derive and show that geodesics are an elegant, optimal path solution under some restricted conditions. We will then show that the same geodesics become an approximate solution to the MTGPP problem for the more general case. 4.1. A Special Case It will now be shown that if a manipulator has no friction terms and no gravitational terms and 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 1: If a manipulator is frictionless and has zero gravitational terms, i.e. Riy = 0 and g, = 0 in the dynamic equations (4), 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 14 Minumum Time Geometric Paths

RSD-TR-17-84 2 u- =J v + Iki v vk =- Jg dsdt + Jij p d2 (20) d8 -' d + dt2 The total power sunk or sourced by the manipulator is limited by symmetrical constant bounds, i.e. -Pmax < P < Pmax Then by Eq. (17), applying the constant maximum power gives ds d2s dy. P= Pmax - d A (21) dt dt2 dt where P =- -. (Note that the gravitational and friction terms have been dropped from Eq. (17)). Solving the differential Eq. (21) gives 3 2 -tPa or= 2 f t2 (22) 2 Mz,3 max since the manipulator starts at rest. Obviously, minimizing the traversal time for a given path requires that we maximize the "velocity" -. This in turn requires that the power P be maximized. dt Therefore, the maximum distance s which can be traveled in time t is given by Eq. (22). This leads to the idea of a reachable set, i.e. a set R(t) of points in the manipulator's joint space which are reachable in time t. In this case, the reachable sets are sets of points such that the distance from the points of the set to the initial position are less than or equal to the distance s given by Eq. (22). These reachable sets look more or less like spheres, though the underlying space is in general nonEuclidean. The important point is that the "radius" of the sphere increases monotonically with time, and is a function of time only. Minimum Time Geometric Paths 15

RSD-TR- 17-84 The criteria for reversibility clearly are met in the case at hand, so the reversibility property may be used freely. Now construct a geodesic from the initial point to the final point and consider two reachable sets, one centered on the initial point of the path and one on the final point. If the geodesic distance between the initial and final points of the path is L, then consider two reachable sets of radius L/2. These sets will meet at precisely one point, and that point will be on the geodesic between the initial and final points. Since the velocity depends only on the distance from the center of the reachable sets, the velocities for the two sets will be additive inverses of one another at the point of intersection of the reachable sets. But by the reversibility property we may reverse the velocity of the final point's reachable set and negate all the applied torques, and the manipulator will move from the surface of the second sphere to the desired final point. Therefore the geodesic between the two points is a realizable trajectory. If another trajectory is chosen, then it must, in the time required to reach the surface of the reachable set via the geodesic, either not reach the surface of the reachable set, or reach the surface at a point other than the midpoint of the constructed geodesic by traversing a different geodesic. In either case, it obviously will take longer to get to the desired final point with zero velocity than it took to travel along the geodesic. Therefore the geodesic is the optimal geometric path. Q.E.D. 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 gravitatioral terms because the presence of such terms makes the shapes of the reachable sets non-spherical. If, however, the rate of change of the potential energy during a traversal of the path is small compared to the power 16 Minumum Time Geometric Paths

RSD-TR-17-84 supplied to the manipulator, then the shapes of the reachable sets will not differ very much from spheres. Under these conditions it would be expected that geodesics would be good approximations to the true minimum time paths. 4.2. Approximate Minimum Time Paths Since the methods described above do not apply to all robotic manipulators, we consider in this section a method for generating geometric paths which are approximately minimum time. Since traversal time depends on the velocity and acceleration limits which the path imposes on the manipulator [8, 91, shortest-distance paths are not necessarily minimum time paths. Shortest-distance paths will often 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 necessary to strike a compromise between curves of shortest distance and curves of smallest curvature. 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. The most obvious measure of total curvature to use is the reciprocal of the maximum velocity. The length cannot be simple Cartesian distance, since Cartesian distance ignores changes in orientation. However, if the path is expressed in terms of an arbitrary parameter X, then the expression Xrra x o ax( ) (23) 10 ma(X) would appear to be a good choice, where p-X and ima(X) is the velocity limit at Minimum Time Geometric Paths 17

RSD-TR-17-84 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 (23), the value of the maximum velocity ima(X) is required. In [8, 9] we have derived this bound in terms of the manipulator's torque bounds and its dynamic equations. The set of admissible accelecations p is given by a set of inequalities of the form umn < Mi + Q, 2 + Ri + Si < u" (24) where M -J df' AM J M?. -R J dX d2fi dfi dfk Q = ': $ dX2 dX dX dfI S -g - ' dX Si = gi For a given position X and velocity p, these inequalities gives a range of accelerations O, and so may be thought of as assigning upper and lower acceleration bounds to each point (X,i) 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 18 Minumum Time Geometric Paths

RSD-TR-17-84 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 R, are zero. Also assume that at every point on the path the manipulator is capable of stopping and holding its position. Then we have Uin < S < Ua (25a) 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, Sp, then Qi is just the inertia matrix Ji multiplied by the curvature vector 6 v 8 If the path chosen is a geodesic, then the curvature vector is zero, and hence Qi = 0. Then the inequality (24) reduces to i'n < Mi + Si < u^n (25b) which is independent of the velocity /A, and by the strong manipulator assumption is satisfied identically for -=0. But if the bounds on p are independent of #, there can be no velocity limits; in other words, Pmax(X) = ~o, so that the integrand of (23) is zero. It may appear at first that the geodesic, since it maximizes velocity bounds, must be the true minimum time path. However, as shown in [8, 9] the manipulator must meet acceleration as well as velocity constraints, and it is not clear precisely what effects path changes have on the acceleration bounds. On the other hand, it should be noted that the velocity bounds are obtained from the acceleration bounds. The velocity limits occur because the acceleration bounds become very close. Since the velocity Minimum Time Geometric Paths 19

RSD-TR-17-84 bounds have been eliminated by choosing a geodesic as the path, the 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. The argument above applies if the friction terms R, are negligible;4 if this is not the case, then there may be finite velocity bounds, causing the integral (23) to become non-zero. In this case, a more complicated analysis is required as shown below. The admissible region of the phase plane is given by equations of the form [8, 9] Ai p2 + B1 p + Ci + Dij > 0 (26a) where i and j are joint numbers. The coefficients Ay, BiZ, Cii and Dij are functions of X. For constant torque bounds, these coefficients are given by the formulae Ci ~ +_ " Ii I Mi l Si -uJ _ Si -uiX Di J~ My Mi ai ra M max - min Umax +- mi where Ai and uM are given by A = -- and uiM= 2 2 respectively. Similar formulae hold if the torque bounds are linearly dependent upon 4As shall be seen later, the gravitational force does not affect the solution. 20 Minumum Time Geometric Paths

RSD-TR-17-84 velocity, as is the case for DC servos with constant voltage limits. Note that for every pair of joints i and j we have A, = -Ai,, B4y =-By, C-y = Ci;, and D,i = -Dii. Therefore we have two equations for each pair of joints, namely Eq. (26a) and -Ai 2 - B, + Cii - D > 0 (26b) Under the strong manipulator assumption, it can be shown that Cii + Di > 0 and Cii - DiJ > 0, so that the inequalities are always satisfied for p- = 0. Consider one joint pair i and j. We may without loss of generality assume that Aiy >0, since if Ay <0 we could simply interchange i and j. For notational convenience from here on the ij subscript pairs will be dropped. Then (26b) will have two real roots if A > 0 and one real root if A = 0. If A > 0, then (26b) will be satisfied for all values of # between the roots. On the other hand, (26a) may have complex roots, in which case it is always satisfied, or it may have two real roots, with the inequality satisfied for all values of p outside the interval between the roots. Explicitly calculating the roots, we have -B - /B2 _ 4AC+D) Pla -- 2A -B + B/2 - 4A(C+D) P2a 2A 2A -B + VB2 + 4A(C-D) #'2b -- 2A Minimum Time Geometric Paths 21

RSD-TR-17-84 If /la is real, then so is A2a, and we have plb < Ala < #2a < 12b and i must be in the interval [Plib,/1la] or the interval [/U2a,2b^ ]. Since the velocity p = 0 is admissible, one of these intervals must contain 0. Note that if B > 0, then /2a < 0 and P2b >0, so that the second interval must contain 0.5 If B < 0, then Plb < 0 and Illa > 0, so that the first interval contains 0. In any case, the maximum velocity is given by 12b; see Figure 2 for locations of feasible intervals within the phase plane in the three possible cases. Going back to the original problem, we have the following theorem which is useful in justifying geodesics as an approximate solution to the the MTGPP problem. Theorem 2: If B < 0 then lim P2b == o, and if B > 0 then M2b attains its maxA —0+ imum value when A = 0, i.e. geodesics are a pointwise optimal solution to the GTPP problem. Proof: If B < 0, then both -B and B2+4A(C-D) are greater than zero. Recalling that -B + VB2 + 4A(C-D) P2b 2A 2A and that C-D > 0 by the strong manipulator assumption, we see that the numerator in the expression for P2b is greater than or equal to 21B|, which is greater than zero. Therefore we have lim 1kb > lim 21BI - A-O+ 0 A-O+ 2A 5In this case, the first interval is in the region of jt<0. Hence, it is not in the feasible region (see [8, 9] for an informal proof). 22 Minumum Time Geometric Paths

RSD-TR-17-84 If B > 0, then the formula for #2b becomes indeterminate, i.e. both numerator and denominator become zero. Applying L'Hospital's rule, lia 12b lim -B + V/B2 + 4A(C-DD) - C-D A -.+ A -O+ 2A A-*O+ B2 + 4A(C-D) B In order to prove that P2b attains its maximum for A = 0 it is then sufficient to show a9 ub that -2b < 0 for all A > 0, i.e. that P2b increases as A decreases. Evaluating this dA partial derivative, 9P2b -B + B2 + 4A(C-D) 1 4(C-D) AA -2A2 2A 2 /B2 +4A(C-D) B- V'B2 4A(C-D) + (C-D) 2A2 A /B2 4A(C-D) Expressed as a single fraction this is a#2b BiB2 + 4A(C-D)- B2- 4A(C-D) + 2A(C-D) aA 2A2B2 + 4A( C-D) Since the denominator is real and greater than zero for all A > 0, we must only show that the numerator is less than zero. This is equivalent to showing that BB2 + 4A(C-D)- B2 - 2A(C-D) < 0 or BIB2 + 4A(C-D) < B2 + 2A(C-D) Since both sides of this inequality are positive, we may square both sides of the Minimum Time Geometric Paths 23

RSD-TR-17-84 inequality, giving B2(B2 + 4A(C-D)) < B4 + 4AB2(C-D) + 4A2(C-D)2 But this reduces to 4A 2(C-D)2 > 0, which is true. Therefore -2-b < 0. 9A It follows that the velocity limits are at their maximum, for any given position and direction in space, when Aij = 0. But if all the Aii are zero, then we must have Qi = 0 for all joints i. In other words, the curvature is zero, implying that geodesics are a pointwise optimal solution to the GTPP problem. Q. E. D. To see that geodesics are local optimum paths, consider two points which are close together, and draw the short geodesic between them. Now construct a second curve which has a small (constant) curvature. The tangents to these curves will be the same, on average, for each curve, differing only by small quantities. The change in position will be small also, so that the quantities B, C, and D can be regarded as constant. Then the length of the geodesic will be less than the length of the other curve, since geodesics are curves of shortest distance, and the maximum velocity along the geodesic will be greater than the maximum velocity for the non-geodesic. Therefore the objective function J --- da which for a short curve will just be d8, will maxM(8) Pmax(8) be smaller for the geodesic than for any other curve. This does not prove that geodesics are the true optimal paths, even in the sense of minimizing (23), since the maximum velocity depends upon direction and position as well as curvature; however, it does show that for a given position and direction, the locally optimum path has zero curvature. Thus we see that geodesics are always in some sense "good" paths, whether or not friction and/or gravitation are significant 24 Minumum Time Geometric Paths

RSD-TR-17-84 effects. 5. EXAMPLES To demonstrate the utility of the solutions described above, the traversal times for various geometric path have been calculated, using the method of [8, 9], for the Bendix PACS arm. This arm is cylindrical in configuration, and is driven by fixed-field D.C. motors. Only the dynamics of the first three joints are considered here (see Figure 3). Both construction of geodesics and trajectory planning require that the dynamic equations (4) of the robot be known. In particular, the inertia matrix and Coriolis coefficients are needed in order to construct geodesics. For the first three joints of the manipulator the inertia matrix takes the form Jt -Kr + Mr2 0 0 Jj. = 0 o Mt 0 (27) 0 0 MZ where q1 =, 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 values of Jt, K, Mt, and Mz are given in Table 1, along with friction coefficients and actuator characteristics. The Christoffel symbols of the first kind (Coriolis coefficients), found by differentiating Jij, are [11,1] = [13,1] = [31,1] = [22,1] = [23,1] = [32,1] = [33,1] = 0 (28a) [12,2] = [21,2] = [13,2] = [31,2] = [22,2] = [23,2] = [32,2] = [33,2] = 0 Minimum Time Geometric Paths 25

RSD-TR-17-84 [11,3] = [12,3] = [21,3] = [13,3] = [31,3] = [22,3] = [23,3] = [32,31 = [33,3] = 0 [12,1] = [21,1] = Mt r - K (28b) [11,2]= - Mtr (28c) d2q+ dqJ dqk The geodesics are solutions of the equations 0 = Ji dq + [jk,sl d d. Plug)ds*2 <"ds ging Eqs. (27) through (28c) into this equation gives the equations of the geodesics as K+M 2d 28 dr dO 0 = (Jt - Kr + M, r2) d2 + (2Mt r - K) (29a) 0 = M, d 2 (29c) In addition, we have the normality condition 2 2 2 (J- Kr+M r) + M dr+ M dz = 1 (30) It can be shown, after extensive calculations, that the differential equations (29a), (29b), and (29c) 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 gra-vitational forces on the r and 0 joints are zero, and the force on the z joint is AMf g. 26 Minumum Time Geometric Paths

RSD-TR-17-84 Trajectory planning also requires knowledge of the robot's actuator characteristics. To determine actuator characteristics, consider the circuit shown in Figure 4. It consists of a voltage source, a resistance, an inductance, 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 = km I, it can be shown from conservation of power that the voltage Vm across the ideal motor is just km w, where w is angular velocity. Since, V -VV - k W if the motor is not in saturation, r= k, I and I = _8- m = m where Rm Rm Vs is the source voltage, we can solve for torque in terms of voltage and angular velocity, giving kL 2 r= - VV -, (31) Rm Rm Assuming the power supply has constant voltage limits of Vrmin and Vmax, this gives torque limits of km mn (km )2 W km (km )2 n _ < r < V ( (32a) Rm Rm -- R( Rm In addition, at some point the iron in the motor srates, with the result that increasing the current through the motor has no effect on the torque. This yields two more Minimum Time Geometric Paths 27

RSD-TR-17-84 (constant) torque limits, so we also require that -at < r < rat (32b) Taking the gear ratio k g into account, this gives torque limits of ( Sat k~m u.i = max v,mi V - (k,- )2 2d 1i (33a) k -q R -m'k. RiRm(k)2 d X and I,gat< t;." (ki")2 dq ' ]2 u?!n-x - min,M V mI' - umax = mint-5, Vmax - R(k )2 i 1 (33b) k~" )RiAk ( Rim(k~)2 dX A trajectory planner for this robot was written in the C programming language and run under the UNIX6 operating system on a VAX-11/7807. 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, versus position X), plots of position X vs. time, and plots of torque and motor voltage vs. time are shown in Figures 5a through 7h. Figures 5a through 5h are for the straight line, Figures 6a through 6h are for the geodesic, and Figures 7a through 7h are for the joint interpolated curve. The traversal times for these paths are 1.782, 1.588, and 1.796 seconds respectively, showing that the geodesic does indeed have the shortest traversal time. 6UNIX is a trademark of Bell Laboratories. 7VAX is a trademark of Digital Equipment Corporation. 28 Minumum Time Geometric Paths

RSD-TR-17-84 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 0 joint is saturated. For the joint interpolated 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, and this apparently is the the reason that the geodesic can be traversed faster. 6. CONCLUSIONS Two methods 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. 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, but these corners could presumably be rounded off, as in [7]. On the other hand, Eq. (23) 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 Minimum Time Geometric Paths 29

RSD-TR-17-84 the best one chosen on the basis of formula (23). 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 available. 7. REFERENCES (1) 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). (2) 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) (3) B. K. Kim and K. G. Shin, "Suboptimal Control of Industrial Manipulators with a Weighted Minimum Time-Fuel Criterion," ASME Journal of Dynamic Systems, Measurement, and Control, pp. 164-172 (September 1971). (4) B. K. Kim and K. G. Shin, "An Efficient Minimum-Time Robot Path Planning under Realistic Constraints," IEEE Trans. on Automatic Control AC-30 (1) (January 1985). (5) T. Lozano-Perez, "Spatial Planning: A Configuration Space Approach," A. I. memo 605, MIT Artificial Intelligence Laboratory (December 1980). (6) T. Lozano-Perez, "Automatic Planning of Manipulator Transfer Movements," A. I. memo 606, MIT Artificial Intelligence Laboratory (December 1980). 30 Minumum Time Geometric Paths

RSD-TR-17-84 (7) J. Y. S. Luh and C. S. Lin, "Optimum Path Planning for Mechanical Manipulators," ASME Journal of Dynamic Systems, Measurement, and Control 102 pp. 142-151 (June 1981) (8) K. G. Shin and N. D. McKay, "Minimum-Time Control of a Robotic Manipulator with Geometric Path Constraints," Proceedings of the 22nd CDC, pp. 1449-1457 (Dec. 1983). (9) K. G. Shin and N. D. McKay, "Minimum-Time Control of a Robotic Manipulator with Geometric Path Constraints," IEEE Transactions on Automatic Control A C-30 (6) (June 1985). (10) K. G. Shin and N. D. McKay, "Open-Loop Minimum-Time Control of Mechanical Manipulators and Its Application," Proc. 1984 American Control Conference, pp. 1231-1236 (June 6-8, 1984). (11) J. L. Synge and A. Schild, Tensor Calculus, Dover Publications, New York (1978). Minimum Time Geometric Paths 31

RSD-TR-17-84 Parameter Description Value r7sat Saturation torque of 8 motor 2.0 Nt.-M. sat Saturation torque of r motor 0.05 Nt.-M. sat Saturation torque of z motor 2.0 Nt.-M. V-Lower voltage limit for 0 joint -40 v. v^Im Lower voltage limit for r joint -40 v. v, mm Lower voltage limit for r joint -40 v. Vm in -40 v. V x fUpper voltage limit for z joint 40 v. V,,m Upper voltage limit for r joint 40 v. Vm __ Upper voltage limit for z joint 40 v. k jrGear ratio for 0 drive 0.01176 k? Gear ratio for r drive 0.00318 Meters/radian k/v Gear ratio for z drive 0.00318 Meters/radian k m Motor constant for 0 joint 0.0397 Nt.-M./amp km Motor constant for r joint 0.79557 X 104 Nt.-M./amp k __Motor constant for z joint 0.0397 Nt.-M./amp R 7 Motor and power supply resistance, 0 joint 1 f Rm Motor and power supply resistance, r joint 1 f Rz Motor and power supply resistance, z joint. 1 1_ k Friction coefficient of 0 joint 8.0 Kg./sec. kr Friction coefficient of r joint 4.0 Kg./sec. kz 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. Jt Moment of inertia around 0 axis 12.3183 Kg.-M. 2 K Moment of inertia offset term 3.0 Kg.-M. Table 1. Dynamic coefficients and actuator characteristics for PACS arm 32 Minumum Time Geometric Paths

: Time history of 3 Specifications An ordered sequence of parameterized path position, velocity, H3 of Task and points in Cartesian space in joint space acceleration 3 Environment ~: Li =i (X) L qd:t) q.(1), qd(t) [ Geometric PathTActuatora LPathGene Path/Trajectory..l "I~~~~~~ PlaneTracker Ci or d,, igure 1.Fuctonllokigrm fanpuarui SensActuator Figure 1. Functional Block Diagram of Manipulator Position Control

RSD-TR-17-84 P f2iI T P2b I _t2 I -- >2b X lb 24 - lb (1) T tla (3) P116 (2) Figure 2. Admissible velocities for (1) /la and P2a complex (2) /Pl and 82a real and negative (3) /la and P2a real and positive. 34 Minumum Time Geometric Paths

iii ijF,c- znnI:, vl I I,.:: i,I ~;.:o,, i::,l t~ i; it]:hz~: *P?iF';x i: I, it::C i,r:i f~,fr:()-:~ 6: i: 14~,~~ I as: i. ~*f' %::9 ~ r~ ri5' h.L:~r zac~r*,~ ~ ~~::br ~ rs r r: ~ih iij.? r "h-:,r ~ v r.":d,,~ — t.ii ~,,~Pi( iiL;1,, i-i~Lr 1 Sici 1 i: ~s; " tt ~- z 114: -j " "' `Itl '"iI'" ~i-I. :i I, i ti,~ i '~~,Y;.. ~: 1 t *:i.. a ~-;;sti*`itttjar]L\SY'a~RB;i-: H~.. 75TdB *:~ *. u:~ -s J1~. ~ —~,,:a ~ *~r~*a **7du i:::,*.~,ra :::::?:r 'L:~ -Pt!~:rrk ~ if ~ ~-*re-* ii!i *I * L* 'ii tr Ilcp: t ~~ r:rejQ,~e:: t rcC.,i *,-t fc;* -I*~ ~+r T 3ri rmrc~ r3,1,9,l,a" g;c,;~;;cud(k4[it&5igIESD " r~ Sg , kt~ ~s:CC~i r*uu : '.? co- Itzt::sx .;,e d i4 r.::i ilrP-~: * ".t:,, 36 i S; ~-~ ~1':i~::::::r II ~;Tp " 1,,* r.~~ -wlt~l~-*' e3,e \1 tr. 'I" t-*E*mr*m.i'r~rrr*- I zr ia~ -~:::S: s* r. 5^9 19 fiia:"i;b~4Z"lls ii;9 a:?r 40: ~~.*: ~ I,, s Y 'it,7:: t IsUIE;~8Vl'rLm*r:CSXTiU*I~LlO;i:~: *~ J;, ti Y -i.llk.tr;IgQg?*e ~, p ~ts v.. I: x. ? i:: llb C cc ~~; =c ~-~~ ~ O I : 5;1:Y.: w~E F J. ~I ~ i: r r i,:~ "'i~ n ~:i: m ~ r;~ ib.s *1* I: r II,;f;e' i. r: r: i~ i iEsLi9 gr t= ~:, c,, riu~ ~i :tg::3 i \ ITTr i *~ri*~~r r r3~I-? ~3 r i~culi iPtT'I: t L, r ~fj i +: i~ ~, rS P \I i.i jC d r; 'Er: O r* AI;rtt *i:~;r7 rB fa * x o? *cr J1;1 F3; r; ~j*Iic 44)i i a 9" i jtb' -.z ~r i:: 7': **B Fitc r;t;S ~t 4 5::t' tjL'P: n W:.L _-xr*L L~~a,;t: lr~ t .x i: ~i~;~ i r ~::s w i t:3"5.:r:n~ sha ti* i`:~a;':" r~,, ~:~;c;( a:'ir. efl~t * :~ ~ r,,::~-rl ZFiS I:~t ~ *1. P: '' ik R 115. p ictt-:j:,t

RSD-TR-17-84 L R m Fj-wmr- -^HP - -1 vm V Vs I ideal motor Figure 4. Equivalent Circuit for a D.C. Motor Servo Drive 36 MMinumum Time Geometric Paths

RSD-TR-17-84 X i 7.000 -6.000 -5.000 -4.000 -3.000 2.000 1.000 2.000 4.000 6.000 0.500 1.000 1.500 Figure 5a. Phase plane plot for straight line Figure 5b. Plot of X vs. time for straight line 10.000 9~~D-1~~~~~~ \ ~5.000 / o0.500 1.000.500 - 1.000 1.500 D- -5.000 — -10.000 Figure.5c. Plot of 6 joint torque vs. time for straight line r joint force vs. time for straight line MinimumFigure 5d. Plot of r joint force vs. time for straight line Minimum Time Geometric Paths 37

RSD-TR-17-84 T/F V 40 400 \ 20. 30 200- 500 _1.000 1.500 -20 -100 I_____, _- _- -t -440 0.500 1.000 1.500 Figure 5e. Plot of z joint force vs. time for straight line Figure 5f. Plot of 6 motor voltage vs. time for stra VI V, 244 20 -0.500 1.000.500 -20- 10 -40- ____________-. 0.500 1.000 1.500 Figure 5g. Plot of r motor voltage vs. time for straight line Figure 5h. Plot of z motor voltage vs. time for strai 38 Minumum Time Geometric Paths

RSD-TR-17-84 7.000 -6.000 -5.000 -4.000 -3.000 2.000 -1.000 — 2.000 4.000 6.000 0.500 1.000 1.500 Figure 6a. Phase plane plot for geodesic Figure 6b. Plot of X vs. time for geodesic T/F 4.000 )- 1 \^ 2.000 -0.500 1.000 1.500 -2.000 -4.000 0.500 1.000 1.500 -6.000 -8.000 -10 Figure 6c. Plot of 0 joint torque vs. time for geodesic Figure 6d. Plot of r joint force vs. time for geodesic Minimum Time Geometric Paths 39

RSD-TR-17-84 V T/FI 40 -400 -20 300-* 0.500 1.000 1.500 -20 -100- ------------- -t -4 -0.500 1.000 1.500 Figure 6e. Plot of z joint force vs. time for geodesic Figure 6f. Plot of 0 motor voltage vs. time for ge( Vi V 10 0.500 1.000 11.500 - -10 -20 --20 — -30- \10 -40 0.500 1.000 1.500 Figure 6g. Plot of r motor voltage vs. time for geodesic motor voltage vs. time forge 40 Minumum Time Geometric Paths

RSD-TR-17-84 0 10- 8.000 6.000 4.000 2.000 -I I -—..X 0.500 1.000 1.500 2.000 4.000 6.000 8.000 10500 Figure 7a. Phase plane plot for joint interpolated curve Figure 7b. Plot of X vs. time for joint interpolated curve T/F 10.000 5.000-.,,.... a — t ': 0. 0Do --- o —D - S O 0.500 1.000 1.5( 0 -5.000 -10.000 - Figure 7c. Plot of 9 joint torque vs. time for joint Figure 7(1. Plot of r joint force vs. time for joint interpolated curve interpolated curve Minimum Time Geometric Paths 41

RSD-TR-17-84 T//F 40' --- ~300t *10 200 o I10 00- 10., 0.500 1.000 1.5 0 No___tI -20, 0.500 1.000 1.500 Figure 7f. Plot of 6 motor voltage vs. time for j Figure 7e. Plot of z joint force vs. time for joint interpolated curve interpolated curve V* V 40 — I 30a 20 0.500 1.000 1.5 0 ~~~~~~~~~-4 | |! I-~10 -20- - 0.500 1.000 1.500 Figure 7g. Plot of r motor voltage vs. time for joint motor voltge vs. time interpolated curve interpolated curve 42 Minumum Time Geometric Paths