UFDC Home  Search all Groups  UF Institutional Repository  UF Institutional Repository  UF Theses & Dissertations  Vendor Digitized Files   Help 
Material Information
Subjects
Notes
Record Information

Full Text 
KINEMATIC AND DYNAMIC MODELING, ANALYSIS AND CONTROL OF ROBOTIC SYSTEMS VIA GENERALIZED COORDINATE TRANSFORMATION By ROBERT ARTHUR FREEMAN A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY UNIVERSITY OF FLORIDA 1985 Copyright 1985 by ROBERT ARTHUR FREEMAN TO MY PARENTS Thanks for everything! ACKNOWLEDGMENTS The author feels extremely fortunate and privileged to have had the opportunity to be involved with the Center for Intelligent Machines and Robotics (CIMAR) at the University of Florida, for the education gained from the faculty associated with CIMAR and for their friendship. It is for this opportunity to learn from (with) such a high quality group (both faculty and fellow students) of creative and enthusiastic individuals that the author is perhaps most endebted to his mentor and committee chairman, Dr. Del Tesar. The author is also greatly appreciative of the guidance and support of his chairman though out the author's educational experience. The author would particularly like to express his gratitude and appreciation to Dr. Joe Duffy for his instruction in the area of mechanism geometry and kinematics, Dr. Gary Matthew (and Keilah) for his patience and support in helping work out numerous technical details dealing with this and other works, Dr. Ralph Selfridge for introducing the author to the computer language APL which greatly facilitates the analyses in this work, Dr. Ed Kamen for the author's initiation into the field of modern control, and Professors Ken Hunt and Lou Torfason for their comments and interest concerning this work. The author would also like to acknowledge all his fellow Cimarians (or Cimarities) for their criticisms, support, encouragements, and, most of all, their friendship. Special iv thanks are extended to Harvey (and Leslie) Lipkin, Mark Thomas and Fahriborz Behi. Finally, the author is especially grateful to Becky Gee for her dedication and persistence in typing this manuscript. V TABLE OF CONTENTS Page ACKNOWLEDGMENTS . . . . . . . . . iv ABSTRACT . . . . . . . . . . . viii CHAPTERS I INTRODUCTION . . . . . . . . . 1 II DEVELOPMENT OF THE CONTROLLING EQUATIONS . . 12 Method of Kinematic Influence Coefficients . 13 Generalized Kinematics . . . . . 13 Kinematics of Serial Manipulators . . 23 System definition and notation . . 24 Firstorder kinematics . . . . 30 Secondorder kinematics . . . 34 Thirdorder kinematics . . . . 46 The Dynamic Model . . . . . . . 54 The Principle of Virtual Work . . . 55 The Generalized Principle of D'Alembert . 57 The Dynamic Equations . . . . . 62 Dynamics of Serial Manipulators . . . 64 Applied loads . . . . . . 65 Inertial effects . . . . . 65 The dynamic equations . . . . 77 The Linearization of the Dynamic Model . . 78 The Linearized Equations of Motion . . 79 Actuator Dynamics . . . . . . 87 State Space Representation . . . . 91 Note . . . . . . . . . . . 94 III TRANSFER OF GENERALIZED COORDINATES . . . 95 The Dynamic Equations . . . . . . 98 Kinematic Influence Coefficients . . 101 vi CHAPTERS III (Continued) Direct kinematic transfer . . . 101 Kinematic transfer of dependent parameters . . . . . . 104 Dynamic Model Parameters . . . . 106 The Linearized Equations . . . . . . 109 Kinematics . . . . . . . 109 State Space Equations . . . . . 111 IV APPLICATION OF GENERALIZED COORDINATE TRANSFORMATION TO DYNAMIC MODELING AND CONTROL A UNIFIED APPROACH . . . . . . . 115 Single Closedloop Mechanisms . . . . 116 Multiloop Parallel Mechanisms . . . . 127 Redundant Systems . . . . . . . 134 Overconstrained Systems . . . . . . 139 V CONCLUSIONS . . . . . . . . 145 APPENDICES A DEVELOPMENT AND DEFINITION OF GENERALIZED SCALAR (e) PRODUCT OPERATOR FUNDAMENTAL TO DYNAMIC MODELING AND TRANSFER OF COORDINATES . . . . . . . . . 150 B SECONDORDER TRANSFER FOR A SIMPLE MANIPULATOR . . . . . . . . 153 C SINGLELOOP PLANAR FIVEBAR ANALYSIS . . . 159 D SPECIAL CASE SINGLELOOP MECHANISMS . . . 165 E PARALLEL MECHANISM MODELING . . . . . 170 BIBLIOGRAPHY . . . . . . . . . . 176 BIOGRAPHICAL SKETCH . . . . . . . . 181 vii Abstract of Dissertation Presented to the Graduate School of the University of Florida in Partial Fulfillment of the Requirements for the Degree of Doctor of Philosophy KINEMATIC AND DYNAMIC MODELING, ANALYSIS AND CONTROL OF ROBOTIC SYSTEMS VIA GENERALIZED COORDINATE TRANSFORMATION By ROBERT ARTHUR FREEMAN August, 1985 Chairman: Delbert Tesar Major Department: Mechanical Engineering This work presents a unified approach to the dynamic modeling and analysis of the general case of rigidlink multidegree of freedom mechanical devices and includes the detailed treatment of the specific case of the serial manipulator. The approach is based on the transference of the system dependence from one set of generalized coordinates to another (e.g., from the relative joint angles to artesian referenced hand coordinates of the serial manipulator) and is shown to allow the analysis of any singleloop mechanism (e.g., the Bricard mechanism), multiloop parallelinput linkages, systems with a superabundance kinematically independent inputs (e.g., redundant manipulators), and systems containing a superabundance of kinematically dependent inputs (e.g., walking machines). The technique involves the initial modeling of the system (or its components) in terms of simple open kinematic chain relationships; then using the viii concept of loop closure and the kinematic constraints relating the possible sets of generalized coordinates the final desired system model is obtained. The derivation of the initial dynamic model is based almost entirely on the principle of virtual work and the generalized principle of d'Alembert and is treated in great detail (from firstorder properties through the linearized state space model, including actuator dynamics). The resultant model is expressed in terms of kinematic and dynamic influence coefficients and is particularly well suited for the transfer of generalized coordinates, especially the quadratic form of the nonlinear effective load terms. The validity of the results of the unified modeling approach is demonstrated by some simple yet sufficient examples. ix CHAPTER I INTRODUCTION one of the most pressing issues facing engineers today is the control of highly integrated mechanicalbased systems capable of addressing a wide variety of tasks, without physical alteration of the system itself. These integrated systems (commonly referred to as robotic devices) are, in essence, asked to manipulate an object (e.g., a tool attached to the endeffector) in the presence of varying process and environmental effects in a very precise manner. To respond to this desire for both diverse motion capability and precise maintenance (during operation) of the specified motion requires not simply a battery of sensors feeding back infinite information to a centralprocessingunit which magically assimilates and transforms these data into appropriate compensatory commands to the system actuators, but an acute awareness of the device itself. That is to say, a sufficient mathematical representation (model) of the object of control (the actuated mechanical linkage) must exist. The obtainment, and subsequent investigation of this model (which not only removes the magic from the control process, but also indicates what feedback information is necessary and sufficient for that process) is the primary focus of this work. While there is argument that one only 2 needs a qualitative model (e.g., knowledge that a particle's acceleration is linearly related to force) of the system for feedback compensation, there is no question that a quantitative model (e.g., a particle's acceleration is equal to the force applied to the particle divided by its mass) is essential for feedforward compensation. Now, while the author is not familiar with all the vagarities of adaptive control, it seems only reasonable that one would prefer to accomplish as much feedforward compensation as possible, and then use feedback techniques to obtain the (reduced) remaining required compensation. Admittedly, the quantification of the model (e.g., determination of the particle's mass) is considerably more difficult than its qualification, but this difficulty is not sufficient reason to dismiss the possibility (and potential) of combined feedforward, feedback control schemes for robotic devices. Keeping the goal of this combined control in mind, the kinematic and dynamic models developed in this work are derived in a qualitative manner while the final expressions require the quantification of basic system properties, such as link dimensions and mass parameters. The formalized modeling procedure presented establishes a bdse technology capable of addressing the full range of possible mechanisms, from a single generic approach. The procedure stems from the work of Tesar and his graduate students, most notably the general development of Benedict and Tesar (1971) and the rigidlink serial manipulator model 3 of Thomas (1981). The model is based on the use of kinematic influence coefficients. These coefficients describe the positiondependent reaction of such pertinent systems parameters as link centroids to the action of the independent generalized coordinates (system inputs). While the basic model is a rigidlink model, and robotic devices typically demonstrate some compliance (Sunada and Dubowsky, 1983), the model is presently being used as the basis for extension to the treatment of quasistatic deformations (Fresonke, 1985) and quasistatically induced vibrations (Behi, 1985), for optimized design procedures (Thomas, 1984 and 1985), and as an aid in the experimental identification of pertinent system parameters. The extension to nonrigidlink devices is not specifically addressed here, nor is the question of realtime dynamic compensation (see Wander, 1985). Again, the primary objective of this work is the development of a general, unified modeling procedure capable of addressing the full range of robotic devices, including redundant manipulators, cooperating robots, walking machines and multifingered endeffectors. For discussion and comparison of the various popular dynamic model formulations, the reader is referred to Tesar and Thomas (1979) and Thomas and Thomas (1982b), Silver (1982) and Lee (1982), among others. Before giving a more detailed overview of the contents of this work, it will prove beneficial to introduce the notational scheme developed herein. Referring (throughout) 4 to Table 11, the basic setul involves a block arrangement consisting of a central bloc' surrounded by both pre and post, superscripts and subscripts. The center block is reserved for a symbol repres( nting either a system parameter(s) (e.g., the set u)), a physical quantity (e.g., a vector of applied loads (T) or a mathematical operator or operation (e.g., partial differentiation). Next, the two superscript blocks are reserved for dependent system parameters (or their properties, such as velocity), with the postscript indicating which parameter(s) is involved and with the prescript giving a ditional information concerning the parameter(s). Finally, he two subscript blocks are used in exactly the same fasiion as the superscripts, but are reserved for independent system parameters (e.g., generalized coordinates (T) cr fixed linkage dimensions (ajk)). At this stage the reader is again referred to Table 1.1 to review the illustratie examples. While this notation is indeed redundant when dealing with a single (fixed) set of independent g neralized coordinates, it is far from redundant when deal ng with the analytic developments and application:. in Chapters III and IV where the system dependence is tra sferred from one possible generalized coordinate set t( another. Regardless, it is felt that even when dealing lith a fixed input set, that the separation of dependent and ndependent system parameters by the respective use of supers(ripts and subscripts serves as a valuable aid in the descri tion and analysis of the system 5 Table 11. Notation Dependent [Modifier] [Parameter(s)] Dependent FSymbol1 __________47 ~Operatorj_ ___Independent [Modifier] [Parameter(s)] Independent Examples 1. u = f(P u = (ul,u2,...,UP)T dependent [ul ] T = k~i'P2' ... M)T independent I( I uml 2. L() L ) [GUI = LG12] (Thomas amd Tesar, 1982b) where G (() See eqn~s. (25) and (26)] 3.3 42~ = 2 t] [Hu I = [ (hmsand Tesar, 3 W ( aoa ~ Q(P1982b) where H S ( ) [See eqn's. (21l)(227)] 5( )3(T 4. Mk[jGc]T[jGcj) = [P] [I I PTh*I PI* M where j E Mik[jGc ]T[iFc I (See eqn. (2157)] 6 under investigation. In addition to being descriptive in nature, this notational scheme underscores the fundamental theme of this work; that there are only two basic types of system parameters (dependent and independent), but that there is not a unique set of generalized coordinates from which to view the system. The notational scheme also has been developed with the assimilation of ten plus years of various kinematic and dynamic analyses presented by Tesar and his students into a single formalized unit. As a final note to readers familiar with the notation utilized by Thomas and Tesar (1982b), to obtain the parameter descriptions given in that work simply drop all subscripts (since only one set of generalized coordinates is addressed) and rotate the superscripts one block in the clockwise direction (e.g., the postsuperscript in this work becomes the subscript in Thomas and Tesar (1982b) as shown in examples 2 and 3 of Table 11). In keeping with the desire to present as generic a treatment as possible, Chapter II starts off with the development of the first, second and thirdorder kinematics of a general multidegree of freedom system. Here, the conceptual use of kinematic influence coefficients, along with the definition of the associated notation, is introduced. The concept of influence coefficients is fundamental to the modeling methodology employed throughout this work and the need to understand this basic idea cannot be overemphasized. Additionally, the 7 qualitative approach to deriving the highorder kinematics is stressed, as well as the utility and simplicity of the standard Jacobian format for arranging the results of vector differentiation. Again, the reader's complete familiarity with the method of derivation employed (and the notation involved in describing the results) in this section (II.A.1.) will greatly facilitate his understanding of the remainder of this work. Next, the kinematics of the general serial manipulator are addressed. First, the geometric parameters and the associated notation describing the serial manipulator are defined, then the first, second and thirdorder kinematics are addressed. The derivation presented here is based on the previously developed generalized kinematics, with the major emphasis here being on the determination of the kinematic influence coefficients for this specific device. The method of derivation and form of the results (simple vectors or vector cross products) are based directly on the work of Thomas (1981). Having completed the treatment of the system kinematics, attention is then turned to the development of the dynamic model. The dynamic model presented here is almost completely determined with the use of just two principles of mechanics: the principle of virtual work and the generalized principle of D'Alembert. In light of this, and in keeping with the generic nature of this work, the use of these two principles in the development of the dynamic equations for a general 8 device is presented first. Here the momentum form of Newton's second law is used. After establishing the general approach, the dynamics of the serial manipulator are addressed specifically. In this treatment (II.B.4.), the results for the manipulator are expressed in matrix form, this being consistent with the method of derivation as well as with the computer language (APL) that the author uses for simulation. In fact, the author's familiarity with the multidimensional array handling and operational capabilities of this language (APL) is most certainly a contributing factor in the chosen form of the resultant modeling expressions. Again, for the investigator familiar with the work of Thomas (1981) (aside from the notation and method of derivation) and the expression of the coefficients of the dynamic model in matrix (vector) form here, as opposed to the summation (scalar) form of Thomas (1981), is the only difference in the modeling results of the two works. Also, and of paramount importance to the transformation of generalized coordinates technique presented in Chapter III, a generalized vector scalar product operator (a) is introduced. Because of the significance of this operator to this entire work, it is addressed in detail in Appendix A. Next, for completeness, the linearization of the dynamic equations of the general serial manipulator is pursued. The derivation of these linearized equations is again attacked in a qualitative fashion (i.e., partials are taken with 9 respect to the generalized coordinate positions, velocities and accelerations). Unfortunately, the resultant expressions are extremely complicated and a nice compact form was not found. At any rate, having obtained expressions for the linearized equations, the drive train kinematics and actuator (dc motor) dynamics are incorporated into the model. The nominal voltages required to drive the system along some specified generalized coordinate trajectory are then determined and, finally, a velocityreferenced state space model of the linearized system is developed. While the resulting state space model can be used to address the possibility of applying modern linear control techniques to controller design for specific manipulator trajectories (Whitehead, 1984), this is not a goal of this work and is not pursued. This completes the material presented in Chapter II, covering the complete development of the kinematic and dynamic equations governing the motion of both a general mechanism and the serial manipulator in terms of a specific set of generalized coordinates. Having established the general procedure for obtaining the dynamic model directly in terms of a specific set of generalized coordinates, attention is now turned to the determination of the model with respect to any set of generalized coordinates. Chapter III specifically addresses this question, with the basic approach employing a transfer of system dependence from one set of generalized coordinates to another. The technique involves the determination of the 10 model (in the direct fashion of Chapter II) with respect to some initial set of generalized coordinates (selected for ease of modeling), then (using this initial model information along with the principle of virtual work and the system's kinematic constraints) the system model is effectively transferred to any arbitrary desired set of generalized coordinates. The treatment here is basic in nature and covers the three main elements comprising the system modeling of Chapter II: kinematics, dynamics and the linearized state space representation. The use of this concept is not without precedent as it has been employed in one form or another by numerous researchers (referenced in Chapter III), primarily with regards to system control. one of the selfimposed constraints on the model transformation presented here is that the resultant desired model maintain the same general form as the initial model (e.g., equations (27), (217) and (2198)). Also, as is shown in Chapter III, the generalized scalar product (Appendix A) is the key mathematical realization allowing the maintenance of the general form. Now, having established the basic procedures for obtaining an initial system model (Chapter II) and the subsequent determination of the system model referenced to any set of generalized coordinates (Chapter III), the full utility of the generalized coordinate transfer technique is addressed. Through the treatment of some fairly general applications, Chapter IV presents a unified 11 and straightforward approach to the kinematic and dynamic modeling of a wide variety of extremely complicated mechanical systems. This unified approach first entails the initial modeling of the system (or separable components of the system) in the relatively simple terms of an openloop kinematic chain. Then, having obtained this simple initial model, the system is constrained (or closed) by single or muitipie application of the transfer equations resulting in the desired dynamic model. This means that the most complicated kinematic device that one need model directly is the simplest possible, the open kinematic chain (or serial manipulator). Finally, for illustration of the full power and scope of this technique it is shown to be capable of modeling 1. Any singleloop mechanism (including such degenerate devices as the Bricard mechanism) with respect to any set of generalized coordinates. 2. Multiloop parallelinput mechanisms, such as the generalized Stewart platform. 3. Systems with a superabundance of kinematically independent inputs, such as the redundant manipulator. 4. Systems with a superabundance of kinematically dependent inputs; such as cooperating robots, walking machines and multifingered endeffectors. CHAPTER II DEVELOPMENT OF THE CONTROLLING EQUATIONS The investigation of the basic kinematic and dynamic nature of multidegree of freedom mechanisms and the subsequent determination of a set of differential equations useful in addressing the control of such mechanisms are presented in this chapter. The presentation is separated into three major divisions. The first section develops a generalized approach to the modeling and analysis of the system kinematics. The approach, which is based on the use of kinematic influence coefficients, is applicable to mechanisms consisting of rigid, or at least quasirigid, iinks. In the second section, the influence coefficient methodology is incorporated in the determination of the dynamic model, yielding the relationship between the system's load and motion states. Finally, the dynamic equations (developed in the second section) are linearized and a general state space representation is presented. This representation, which includes actuator dynamics, is useful when considering the control of the mechanism about some prespecified nominal trajectory. 12 13 Method of Kinematic Influence Coefficients This approach to dynamic modeling of rigidlink mechanisms is based on the separation of all kinematic (and dynamic) phenomena into a collection of purely position dependent functions (kinematic influence coefficients) operated on by independent functions of time (input time states). The concept of kinematic influence coefficients can be applied to all classes of mechanisms (e.g., parallelinput planar systems, serialinput spatial devices, etc.) and has been well established in the literature by the works of Tesar and his graduate students: Benedict and Tesar (1971), (1978a) and (1978b), Freeman and Tesar (1982) and (1984), Cox and Tesar (1981), Thomas (1981) and Sklar and Tesar (1984). Because of the fundamental nature of this concept and its almost limitless range of application, this section is presented in two parts. The first contains a detailed development, in general terms, of the basic methodology involved. The second addresses the specific case of the serial manipulator. Generalized Kinematics Consider a Pdimensional time varying vector U(t) = (ui(t), ...' uP(t))T (21) 14 representative of the motion parameters required to describe the kinematics of a given system where the superscripts indicate which parameter is involved (not that uP is u raised to the pth power). Further, consider that these parameters are functions of an Mdimensional set of generalized coordinates T(t) = ((Pl(t), P2(t),..., (M(t))T (22) This allows a parametric description of the system where uP = uP(_);p = 1,2,...,P (23) and (Pn = (Pn(t);n = 1,2,...,M (24) Adopting the standard convention for differentiation of a vector (e.g.,u(p)) by a vector (e.g., _(t)) one obtains the typical Jacobian form, where the nth column of the result is the partial derivative of u(cR) with respect to the nth component of ((t), for the first time derivative of u(t)) as Ti 9(2 3 (M (25) 3u_ ul ... 9uf N a iP 3 2 a (M (P2 Throughout this section an explicit shorthand notation is developed to serve as a descriptive aid in the expression of the system kinematics. The first order geometric 15 derivatives (i.e., kinematic influence coefficients) are denoted as follows: [y] = [Gu] = P x M matrix (P [__ = [G] [Gu u = P x i vector Pn p ;n n n (26) [auP] = [Gu E [Gu] = SP = 1 x M vector 5 Ipp; (P Q; 1xM vco [_uP] = [Gu] [GP] E gP = 1 x 1 Pn pP; n n n where the letter (g,G) is reserved for first order geometric derivatives, the superscript indicates the dependent parameter(s) involved (i.e., uP) and the subscript specifies which generalized coordinate(s) (i.e., (Pn) is involved. Also, the snape of the result is indicated by the dimensions of symbols used as the superscript and subscript(s). For example, the shape of qu is equal to the dimension of the subscript u (i.e., P) by the dimension of the subscript n (i.e., 1). Referring to equations (26), the first order kinematics of the system can be written in a variety of ways depending on the manner of expression one wishes to employ. The velocity (a) of the complete parameter set (u) can be expressed as S= [Gu] = (P x M)(M x 1) = P x 1 (27) or =Mg = gU'n = E(P x 1)(1 x i) = P x 1 vector n=l n (28) 16 And, the velocity (UP) of a particular dependent parameter (uP) is written as P = (1 x M)(M x 1) = 1 x 1 (29) or aP = M gP "n = (i x i)(1 x i) = 1 x 1 (210) n=i n Noting that firstorder influence coefficients (g,G) are functions of the generalized coordinates (T), one recognizes that the velocity vector (i) is a function both of the position (p) and velocity ( ) of the independent coordinates (pm; m = 1,2,...,M). This allows one to obtain the second time derivative (U) of the dependent parameters (u) from = + u = (211) The first partial derivative in equation (211) is = G = _U[U]) = [GU]_ = [GU]IMxM (212) 3 (b _P where the last term is the MxM identity matrix. This gives the partial acceleration of (u) due to the acceleration of the inputs (p) as = ] (213) Recalling the standard Jacobian form for derivatives of vectors, one has 17 D1l 8 2 3 M a= [Gu] = (aGU]) = 2 (214) DuP 3ap i M where M gP = uP = 8 ( E gP 5n), p = 1,2,...,P m 3 m nl n (215) m = 1,2,...,M Recalling the definition given in equation (26) for (gP), n and performing the differentiation, equation (215) becomes = M gP = n( 3 (8uP)]n m n=1 34m n M (216) = E hP $n n=1 mn = 1 x 1 where S (3uP) hP = 1 x 1 x 1 (217) aM 3 (n mn Alternately, equation (216) can be expressed in vector form as gp = &T(hP )T = 1 x 1 (218) m m where hP = (hP hP, ..., hP ) = 1 x 1 x M (219) m(P ml m2 mM 18 Here, the letter (h,H) is used to denote the secondorder geometric derivative, and the shape of the result is equal to the dimension of the superscript by the dimensions of the subscripts ordered from left to right. Using the vector form of equation (218), the pth row of equation (214) can be written as [] = [Gu = [GP] _P; P P; = T[(hP ) (hP ) ... (hP )Tj 10 2p "MP = T[HP ]T (220) = 1 xM where hP hP ... hP 11 12 lM [HP J [ (8uP)] = hP = 1 x Mx M S _21 (221) hP hP M1 MM From this, the partial acceleration of the pth dependent parameter (uP) due to the velocity of the inputs (0) is seen to be p. = T[HP ]T$ = i x 1 x 1 (222) (P (PO 19 Since the transpose of a scalar is equal to itself, equation (222) becomes p. = THP ] (223) Now, considering the complete parameter set (u), one has &T[H1 %TIIHl 1i "(P = _T[H 2] = (H u = P x (224) OT[(H ] were (Hu ] = P x M x M (225) and [Hu ] = (HP ] (226) QQP; (D(P Finally, from equations (211), (213) and (224), the acceleration (u) is found to be u = [Gu] + T[Hu (227) Observing that the acceleration (u) is a function of (2), (_6) and (Z6), the third order time derivative ('Y) can be determined from = B + + u (228) 20 Here =Glj = [G"jII'MxM (229) so (GuD = (230) The second term in equation (228) is found from (231) 3 4p. jpm where, from equation (222), = ~~ ( t (p~l (232) = ([HP T i + ( TfHP T = (jT[H] d + @jT[H$] T)~ since (CPTIHO(P);j = [.[Hj; jT ([Hj =T 1 x1 (233) The phrow of equation (231) can be written as dL= ( ((T r HP 1);j + (TrHP p p ;M ___ L (P(P~' p T) ;jjTCH11p(M M ( T[HP ]'1);M)(234) LP WT 21 The complete Jacobian of equation (231) is then S1 + 1 ] N[[H ] + H ]T] 2 2 = T[H2 J + [H2 ]T] (235) _T[[H ] + [H~ ]T] u uT ~T[[H ] + [H ]T] yielding = [[H U] + [Hu JT]g (236) where the transpose operation is performed plane by plane. The last partial in equation (228) can be separated into two parts, giving Ki.. Ki.. a 3 ( (237) where, recalling equations (213) through (220), G u .. = __[G_]_HiT (238) 2 = T[H ]T I T iT [H(PPJ 22 ~T[H ( ]HU = P x M and 3 .. T Hu TI (P r (Pp](29 F T H1 T i T H1( [ (P(P ) ( L M ] C ) e i 3 (P2 ( M *T 2 = H( [H (239) HP 1 T [HP T H qT H M Now 3T HP T( ( ( [Hp) D (Pi 3 el (240) ST[D ] g = 1 x 1 yielding 1 1 .T 1 _T[D1 T(D2eC]D ... jT[DMee iu 2 = T[D1e (241) p P T [ D 1ee] ( T [ DMee ] _TEDuJ = P x M The letter (d,D) is used to denote the thirdorder geometric derivative, and the shape of the quantity is equal to the dimension of the superscript by the dimensions of the subscripts, respectively. Further defining the third order kinematic influence coefficient (d,D) one has 23 3 u a = [D = P x M x M x M 3 3 33 p u p u = Up (D 1.. = [D ] = 1 x M x M x M (242) 2u u ( ) = (D J.. =[D(P] = P x 1 x M x M el 2 U p = (p (Du p ( (u)) = [D1(PPp;1;m;n; = dimn = 1 x 1 x 1 x 1 ai 3m a9n Now, substituting equations (238) and (241) into equation (237) gives the partial derivative of the dependent parameter acceleration (U) with respect to the generalized coordinate(s) position (p) as = ETCHU]T + T(Du(]I (243) Finally, from equations (228), (230), (236) and (243), the thirdorder time derivative (Y) of the dependent parameter set (u) is u u u u [] + iT[]+ [HT + ( HT+ _TD]) = ] + T(H + 2[H]T) + ( [DH]) (244) completing the general kinematic development. Kinematics of Serial Manipulators The analytical development presented here, altered notationally and extended to cover thirdorder kinematics, is based directly on the work of Thomas (1981) and is included 24 for completeness. The definition of the particular physical system that is being considered is addressed first. Then the notation defining the physical parameters is established. Next, the geometric relationships corresponding to equation (23) are developed. Finally, the kinematics of the serial manipulator are derived in terms of explicit equations defining the kinematic influence coefficients as opposed to the conceptual relationships of the preceding general development. System definition and notation The analytical development presented here, altered notationally and extended to cover thirdorder kinematics, is based directly on the work of Thomas (1981) and is included for completeness. The serial manipulator, in essence an open loop kinematic chain consisting of a series of rigid links joined by onedegree of freedom lowerpair connectors, is illustrated in Fig. 21. This treatment specifically addresses only revolute (R) and prismatic (P) pairs because most other typically encountered joints can be considered as combinations of the two (e.g., a cylindric joint is analyzed as an RP combination). Investigation of Fig. 2.1 shows that both types of joints, revolute and prismatic, have two independent parameters associated with them, (sjj, 8j) and (sj, E~j), respectively. The parameter sjj (or sji is the offset distance along the joint axis, sJ, between the two links connected by the joint connects. The parameter 8j (or 25 ejj) is the relative rotation about sJ between these links. Here the double subscript serves to indicate that the quantity is a fixed independent parameter, while the single subscript implies that the parameter is an independent variable. The two parameters ajk, the fixed distance between joints j and k as measured along their common perpendicular aJk, and k, the twist angle between the joint axes measured in a righthand sense about aJk, define link jk. The global reference is a fixed Cartesian system (X,Y,Z) with the Z axis directed along the first joint axis, sl, and the X axis located arbitrarily in the plane perpendicular to s1 (Note that the independent parameters associated with joint one are measured with respect to the X axis). Local (or bodyfixed) dextral Cartesian reference systems ((J)x, (J)y, (j)z), with (J)x along aJk and (J)z along sJ, are assigned for each link (jk). The vectors ajk and sJ are unit vectors expressed in terms of direction cosines and given by *aJk = Tj(j)ajk = (*xjk, *yjk, *zJk) (245) *sj = TjBJ )s3 = (*xj, *YJ, *ZJ) with (j)ajk = (1, 0, 0)T (246) ()sj = (0, 0, 1)T where the rotational transformation matrix Tj is Tj = *ajk sJxjajk *sj] (247) The preceding superscript, (J), is used to indicate that the vector is expressed in terms of the jth local reference, and the preceding superscript (*) denotes a vector given in 26 terms of its direction cosines. The direction cosine representation of the vectors aijk and si can be obtained, where all joints are assumed to be revolutes, from the initial direction cosines *s1 = (0, 0, 1)T (248) *al2 = (ce1, s81, 0)T and the recursive relations *sj = Tjl(jl)sJ = Tjl(0,Sal(jl)j, c (jl)jJ)T j = 2,3,...,M (249) *ajk = Tjl(jl)ajk = Tjl(c8j,ca(j)jsej,s(jl)jsj)T where c8 = cos8, sO = sin8, etc. If the joint is prismatic, simply replace 81 by 811 and s11 by sl here and in the subsequent general equations. Finally, the position vector, RJ, locating the origin of the jth reference is given by RJ = slls1 + (a(1)1a(11)1 + s ) 1=2 (250) Using equations (248), (249) and (250), the configuration of the serial manipulator is completely defined once all the zeroth order independent parameters are specified (e.g., sll. a(11)1, a(11)l, 81, etc.). The reverse position solution, that of solving for the variable input parameters given the desired hand location (e.g., R6, a67, s6), is (in general) considerably more difficult (Duffy, 1980) and is not addressed here. The notation involved in the kinematic 27 jk (j) a, X Sjk a j sj ]k sj, (J) \ \ (j)j 23 a Ri 22 a 12 s ss S ,Z Figure 21. Kinematic representation of the serial manipulator 28 Table 21. Manipulator Joint and Link Parameters SJ Vector for Joint Axes Between Links Sjj is Fixed Offset Value (Revolute) Ojj is Fixed Rotation Value (Prismatic) Dj Generic Input for Joint Axis sJ Sj Sliding Along SJ 8i Rotation About SJ a3k Fixed Link Dimensions Between Axes j and k ajk aik Common Perpendicular Along ajk ajk Twist Angle 29 Table 22. Coordinate Systems GLOBAL SYSTEM X Y Absolute Reference System (Z Along Axis Si) Z xi Y Direction Cosines for Joint S3 zJ *gjk *Yjk Direction Cosines for Link ajk *Zik LOCAL (j) SYSTEM (J)y Body Fixed System for Link jk (J)Z (J)X along ajk (J)z along Sjk 30 representation of the serial manipulator is given in Tables 2.1 and 2.2 for quick reference. Firstorder kinematics In the development of the generalized kinematics, the specific nature of the dependent motion parameters (up) and the independent input parameters (en) were not taken into account. In the case of the serial manipulator there is a fundamental difference in the nature of the two basic motion parameters considered. In this treatment the two parameters are the cartesianreferenced link orientations (which are not considered true coordinates) and the cartesianreferenced coordinates of a point (which are true coordinates). Also, the effect of an input on these parameters depends on whether the input is a revolute or prismatic joint. In light of these facts, the kinematics for rotational parameters amd those for translational parameters are treated separately, and the results for both revolute and prismatic inputs are given in each case. In dealing with the kinematics of rotational parameters, it is not convenient to start with an equation of the form of equation (21) (i.e., u = f(Qp)) since finite angles of rotation cannot be represented by vectors. Fortunately, however, it is possible to represent infinitesimal rotations by vectors (Meirovitch, 1970), and these infinitesimal rotations can most readily be interpreted as the angular velocity (w) of the body. So, instead of using a zerothorder holonomic constraint equation (e.g., u = f_) 31 to derive a link's firstorder rotational influence coefficients, a firstorder nonholonomic constraint equation expressing the link's angular velocity (wjk) is used. Referring to Fig. 21, the angular velocity addition theorem gives the angular velocity (wjk) of link jk as simply the sum of the relative angular velocities between preceding links in the chain J Wjk = in S n, n = 1,2,..,L (251) n~l where, 6nSn is the relative angular velocity between links (nl)n and n(n+l), and en is identically zero for a prismatic joint. Investigation of equation (251) shows that the angular velocity (wjk) can be separated into a function of position (i.e., sn = f() operated on by a function of time (i.e., 8 = 8(t)) or, in the form of equation (27), jk = f(Q)*B(t) = [G j ] ( 5 ( (252) Now, in order to obtain the 3xM configuration dependent matrix CG 1, one has that (also see equation (212)) (Pk 3(wjk) = _([G = [Gjk]_9(. ) (253) = [Gjk] In fact, for any position dependent vector (u = ( )) one has from calculus that 32 (d(u)) = 3 (3(u)de) n t D n p dt n (254) This turns out to be a very convenient relationship and is used to obtain the kinematic influence coefficients for translational parameters even though it is not necessary since the position vector u = P = (XP, yP, ZP)T exists and could be differentiated directly. Returning to the question at hand, from equations (251), (252) and (253), the rate of change of the angular orientation of link jk is jk = [GJk] (255) where _jk = (wjk) D E si) (256) { sn n < j; (Pn = 8n (revolute) o, otherwise Here, the angular velocities (e.g., wjk) are expressed in terms of cartesian reference frames and denoted, in a component sense, as wjk = (wjx, wjy, wjz)T (257) Consistent with this notation, the influence coefficients take the form 33 gjk = (gjX, gjy, gjz)T (258) n n n n where sn = (*Xn, *yn, *zn)T (259) The translational velocity (2) of a point (2) in link jk, and hence (from equation (254)) the translation influence coefficients, can be derived from P = R + TJ(J)P (260) where, (J)P is the location of point P in terms of the body fixed reference j, and multiplication of (J)P by the rotation matrix TJ transforms the coordinates from the jth reference to an intermediate system with the same origin but with axes parallel to the global reference (X, Y, Z). Substituting equation (250) for RJ and differentiating equation (260) gives the velocity of point P in link jk as S1S 1 2 (a(l1)1l(l1)1 + slls + 5Sl) (261) + d(TJ(J)P) dt Now, since a(l1)1 and s1 are unit vectors fixed in link (11)1 and (TJ(J)P) is a vector fixed in link jk, the time rate of change of these vectors can be expressed in terms of the vector cross product as a(11)1 =(11)1 x a(11)1 1 1 n sn x a(11)1 n=l (262) 1 =(11)1 x sl = n sn x sl  n=1 n 34 d(TJ(J)P = Jk x (TJ(J)P) =1 n) x (Tj(j)P) . .. ~~~n=1 ns (JJP dt Substitution of equations (262) into equation (261) and regrouping of terms leads to P = {nSn + nin x (a(ll)la(l1)1 + slls1) n=1 l=n+1 + TJ(J)P} (263) = n {S n + nn x (PRn)} n=1 where, referring to Fig. (21), 1(a()1(1)1 + 11) + TJ(J)P =(PR) l=n+l + _n (264) is a vector from the origin of the nth reference to the point P. Finally, recalling equation (254), the velocity of point P can be expressed in the form P . P= [G ]@ (265) where sn x(PRn) n~j; n=8n (revolute) P In = 9P =, nj; n=sn (prismatic) (266) a n>j Secondorder kinematics Now, relying heavily on the development presented for the second and thirdorder kinematics of a general motion 35 parameter, the higherorder kinematic influence coefficients, and hence the higherorder kinematics for the serial manipulator, can be obtained by directly operating on the firstorder coefficients given in equations (227) and (266). observing that both the rotational and translational influence coefficients are expressed in terms of position dependent vectors leads one to again use the relationship of equation (254) to obtain the (h,H) and (d,D) functions. Recalling the firstorder rotational coefficients and equations (254), one has s n ,nj ; Qn=En d(gjk) = (267) dt n o otherwise where n Z 1 i s) x sn (268) Performing the partial differentiation with respect to the mth generalized velocity yields, for the secondorder rotational coefficients, the expected nonsymmetric (i.e., hjk hjk) result inn nm sm x sn m n m nm n(269) o otherwise 36 Also, notice that the rotational (h,H) functions are zero if either input is prismatic. Referring to the general form of equation (227), the angular acceleration of link jk becomes ajk = [Gjk] + T[Hjk], (270) where ajk = (ajx, ajy, ajz)T (271) and (Hik] = 3 x M x M (272) with [Hjk] = hjk = (hix, hJY, hjz)T 273 QQ ;m;n mn mn mn mn The second order kinematics of a point P in link jk are perhaps most easily understood by identifying all possible input combinations and addressing each situation independently. Looking first at the case where the nth generalized coordinate is a prismatic joint (i.e., Qn = Sn), the time rate of change of the (g,G) function is 4P = n ,n~j; nS n n n=sn (274) Since sn can be viewed as a unit vector fixed in link (n1)n, one has that gP = w(n1)n x sn =(nil i i)x sn nj; n=sn n 1(275) Taking the partial derivative of this expression yields, for a prismatic joint n 37 sm x Sn m n (276) 2 m=sm ; (Pn=Sn Now, considering the case where the nth joint is a revolute (i.e., n = 8n), the time rate of change of the influence coefficient is gP = sn x (PRn) + sn x (pRn) (277) n (277) or, by differentiating equation (264) to obtain (PRn) and substituting the cross product form of equations (262) for the vector derivatives, 4P n=l )xsn n S= (( g s) x ) x (PRn) n 1=1 (278) snx( [( si) 11)+l + l=n+l in i) x (la1)a(11)1 + Silsl)] 1=n+1 1=1 + ( 8 isi) x (TJ(J)P)) i=l + sn x ( sl) l=n+l Investigating the case where the mth input is a prismatic joint (i.e., pm = sm), one sees immediately from the last term of the preceding equation that P P sn x sm n 8Sm o minsj (279) 38 The reader should note that this equation is consistent with equation (276) and shows that the result is nonzero only if the revolute precedes the prismatic joint in the serial P P chain. The symmetry (i.e., hnn = hnm) of the translational (h,H) functions should also be noted. Now, when the mth input is also a revolute (i.e., m = em) it is best to look at the case in two steps. First, when m 2 n, from the second term of equation (278) .P an = sn x (sm x [ (a(11)la(11)1  1=m+l (280) + sls1l) + Tj(j)P)) or, from equation (264) 3P n = sn x (sm x (PRm)) n If m < n, both of the first two terms in equation (278) are involved and, one finds that @gP n = (sm x sn) x (PRn) mm (282) + sn x (sm x ( (a11 la(11)l +slls + Tj(j)P)) 1=n+l ) or, again recalling equation (264) P n = (sm x sn) x (PRn) + sn x (sm x (PRn)) = sm x (sn x (PRn)) m5nj (283) Combining this result with that of equation (281) further illustrates the symmetry inherent in the translational (h,H) functions and yields 39 P P hmn = h = x (sJ x (PRJ)) i = min (m,n) = max (m,n) (284) Before proceeding with the kinematic development, it will prove beneficial to reinvestigate a representative sampling of the first and secondorder translational influence coefficients in a more geometric light. Recalling equations (28) and (266), the firstorder geometric influence coefficient for a point P in link jk with respect to the nth input is a vector ( ) which when multiplied by the nth generalized velocity ( n) yields the nth partial velocity (kn) of the vector (P) P = = i !P P = 0 ; n>j n n =0 n n (285) Let us first investigate the case where the nth input is a prismatic joint (i.e., n = Sn) and the desire is to find (san Here, the preceding subscript (s) is introduced to indicate that input n is prismatic. Referring to Fig. 22, one can readily see that the vector (P), and hence, its velocity (P), can be expressed in a variety of ways. In the manipulator of Fig. 22, where the second joint is prismatic (i.e., P2 = s2), it is advantageous to view the vector (P) as P = R2 + (PR2) (286) 40 Here the quantity (PR2) is considered a free vector and the pure translation of a free vector in no way alters the vector (i.e., (p:R2)=O). With this representation it is readily apparent that the action of the slider (s2) only affects the vector (R2) Also, by inspection, the rate of change of this vector (R2), and therefore, the rate of change of P due to the action of s2, is S= k2 L = 2 (287) since R2 = s11Sl + ai2a12 + s2s2 (288) From equation (277) one obtains the expected result (see equation (266)) P sin = sn n=2 (289) Now, consider the derivation of the (g,G) function for a revolute joint (e.g., Cn = 83)" In this situation, the coefficient (ei3) is most easily found by expressing the position vector P as P = R3 + (PR3) (290) In this case it is the vector R3 that is unaffected by the action of the revolute (83). Recalling the vector cross product representation of the velocity of a body fixed vector, one has the partial velocity of P due to the third input as 3 = (P:R3)3 = e3 s3 x (PR3) (291) since (PR3) can be viewed as a vector fixed in link 34. This again yields the expected result (see equation (266)) 41 jk (j) a, x pS __i, ) PR3 a3 eg Y PR2 73  / S F23 pa R R 3 12 a Yp S 9 R a/ s s ,Z Figure 22. Firstorder kinematic influence coefficients 42 P Ssn x (PRn) n=3 (292) E8gn = The difference between the translational firstorder coefficients for revolute and prismatic joints is perhaps more easily understood now, from equations (286) through (292), than from the more analytical development resulting in equation (266). That is, by expressing the vector (P) as P = Rn + (PRn) (293) it becomes obvious that, if the nth input is prismatic (i.e., 8n = Sn) the location of the origin of the local reference (Rn) fixed to link n(n+l) is translated and, if the nth input is a revolute (i.e., (n = 8n) the vector (PRn) from the local origin to the point of interest is rotated. To reinterpret the secondorder coefficients (i.e., hmn), take the case where the nth joint is a revolute. Here, referring to Fig. 23, one has P 8 n = sn x (P Rn) (294) First, if m S n, one can obtain (h Pn) immediately by noting that (8qn) as expressed in equation (294) represents a vector fixed in link n(n+l). From the firstorder results it follows that P salmn = o msn~j (295) and OOhmn = sm x (sn x (PRn)) m5n~j (296) 43 P pR33 S3 S1 SR3 S ,Z Figure 23. Secondorder kinematic influence coefficients when second joint is a revolute 44 since (9_q is affected in the same manner as (PRn) was previously. Now, if m > n, the first step is to reexpress equation (294) as (see Fig. 24) P = n x ((PRm) + (RmRn)] (297) Here, from past results, it can be seen that if the mth input is a prismatic joint only the vector (RmRn) is effected. On the other hand, if the mth input is a revolute the only affect is on the vector (PRm). This gives the secondorder results as P sohmn = sn x sm n and P eehimn = sn x (sm x (PRm)) n The remaining secondorder influence coefficients are left for the reader to reinterpret and compare with the more analytically derived equations (276), (279), (281) and (283). Now, with the secondorder influence coefficients fully established, the acceleration of point (P) can be obtained as Q=+~~] (2100) where = (RP, P, 2P)T (2101) and 45 P PR3 hp= slx (s3x (PR 3)) 8831 3 PR1 P PR2 R3R1 2 P 1 2 se21= xs X R 2R 1 S ,Z s 1 Figure 24. Secondorder kinematic influence coefficients when first joint is a revolute. 46 (H1 3 x M x M (2102) with IH PJ [H = I= (hmn hmn, hmnZ)T (PT m; OT n;m mn hmn mn (2103) Thirdorder kinematics The thirdorder kinematics require the development of the thirdorder influence coefficients (i.e., the (d,D) functions). Here, only a few representative cases will be derived. The rotational coefficients are obtained via the strict analytic approach, whereas the translational coefficients are addressed using the more illustrative graphic approach. The complete thirdorder coefficient results, along with those for the first and secondorder coefficients are presented in Tables 23, 24 and 25. Again, the rotational influence coefficients will be investigated first. Here, if any of the three inputs involved is a prismatic joint, then the thirdorder geometric derivative is identically zero. This result is due to the fact that the translation of a free vector does not alter the vector (as was also the case for the (h,H) function). Now, if all inputs considered are revolutes, one has xem snm~~ (2104) 0 ,n: m 47 and the concern is to determine the effect of the third revolute ((l = 81) on this secondorder property. There are two unique nonzero situations nere, l link (ml)m and the thirdorder geometric derivative is jk mi . edImn = M (( e1 s ) x (sm x sn)) 1i= (2105) = s x (sm x sn) i If m jk ni i) sn 888dlmn = sm x (( i ) x ) (2106) <01 i=l (2 106 = sm x (sl x sn) m One can now write the time rate of change of the angular acceleration of link jk as (see equation (244)) &jk = [GJk]" + 'T[[Hjk] + 2(HJk]T]$ Q0 (P(P P0 (2107) + ((T(Djk where all algebraic operations are as defined previously in the general kinematics section and jk [D ] = 3 x M x M x M (2108) with jk j dk jX j3Y jZ D(Q ];1;m;n = lmn = (dlmn,dlmn, dlmn)T (2109) 48 The thirdorder translational coefficients where one of the inputs is prismatic can be obtained in exactly the same manner as were the rotational coefficients for three revolute inputs (since the symmetry exhibited by the secondorder translational coefficients carries over to the higher order properties), where a ( a (P ) = a ( a (= . asn ae1 3m aE1 aem 9sn (2110) As with the translational (h,H) functions the only nonzero result occurs when the prismatic joint is the last joint considered and can be expressed as P P s nlm = 8slmn = ... = si x (sJ x sn) i=min (1,m) j=max (l,m) (2111) Considering the situation where all three inputs concerned are revolutes, and m:nsj, one has P h n= sm x (sn x (PRn)) (2112) Taking the more graphic approach (see Fig. 25), for 15m, P (mn) can be considered as a vector fixed in link m(m+l), yielding P 1 s sm sn P dlmn = s x (S x (Sn x (PRn))) 15msnsj (2113) For m 49 P PR sj 1 3 R j R33 Ps ,Z Figure 25. Thirdorder kinematic influence coefficients 13 31] 313 = sx (s3 (S (PR3))) Ri P R3 X s, Z Figure 25. Thirdorder kinematic influence coefficients 50 dmn = sm x (sl x (sn x (PRn))) m Finally, for m lmn = sm x (sn x (sl x (PR1))) m The remaining translational thirdorder geometric derivatives are left for the reader to derive and are given in Table 23. The time rate of change of the acceleration of a point P in link jk can now be expressed, from equation (244), as ... PPP . P [G] + 3T[H ]g + [2T[Dp ]@] (2116) where P [D ] = 3 x M x M x M (2117) with P P D J;l;m;n = dmn (2118) and the symmetry of the translational (h,H) functions has been observed (i.e., (H ]T = [H ]). This completes the treatment of the kinematics of serial manipulators. 51 Table 23. First Order Influence Coefficients for Serial Manipulators Joint Type Symbol at M at N Restrictions Value Rotational n sj Sn [GJ kin n>j 0 (P All n 0 Translational R nij Sn x (PRn) [JGP]n (P P nsj Sn R,P n>j 0 52 Table 2.4 Second order Influence Coefficients f or Serial Manipulators Joint Type Symbol At M At N Restrictions Value Rotational R R m [Hik lmn R R mvn or n>j 0 P P All m,n 0 R P All m,n 0 Translational R R m~n:5j SmxLSnx(PRn)] [)HP 1M~n R R n'~mj Snx[Smx(PRm)] P R n R P m'~n:j SmxSfl P R m~n~j 0 R P n~m:Sj 0 R,P R,P (m or n) < j 0 53 Table 25. Thirdorder Influence Coefficients for Serial Manipulators Joint Type Symbol 1 M n Restrictions value Rotational R R R 1 R R R m,lSn or ni 0 P all l,m,n 0 Translational [DP 1lm~ R R R 1l5m~n~j S lx(Smx[Snx(PRn)]) (P(M R R R m R R R m~n R R R n R R P l5m:5n~j SlX(SmXSn) R R P m~ln5j Smx(SlxSn) R R P m,1n or nj 0 R P p all other by symmetry 1 ,m, n~j P p all l,m,n 0 l,m,n>j 0 54 The Dynamic Model This section deals with the determination of the generalized input loads (forces and, or torques) required to cause the system in question to undergo some arbitrary desired motion. The derivation of the describing equations presented herein is based almost entirely on two fundamental principles of mechanics (the principle of virtual work and d'Alembert's principle). Lagrange's equation could have been employed, yielding the same results (typically in scalar form as in Thomas, 1981). However, it is felt that the approach taken here more directly stresses the geometric nature of the result. The principle of virtual work is used to obtain the generalized forces (or torques) necessary to counteract externally applied loads and put the system in a configuration (i.e., position) dependent static equilibrium. This principle has been employed by many researchers (e.g., Whitney, 1972, Paul, 1972 and Thomas, 1981) to deal with loads applied to the endeffector of a serial manipulator. It can also be used to handle loads generated by such system components as springs and dampers as demonstrated by Benedict and Tesar (1978b) and Freeman (1980). D'Alembert's principle is used in conjunction with the principle of virtual work to address the system's inertial dynamics. This approach to determining the generalized inertial loads is not unique to the author and has been ARM 55 observed by several investigators (e.g., Kane, 1961 and Silver, 1982). One form of the equations resulting from this approach has even been referred to recently (by Kane and Levinson, 1983) as Kane's dynamic equations. The concept (alluded to as early as 1923 by Wittenbauer), however, is a natural consequence of the two principles and is usually referred to as the generalized principle of d'Alembert (see Lanczos, 1962, Meirovitch, 1970 and Lee, 1982). Finally, the dynamic equations are expressed in terms of configuration (i.e., position) dependent coefficients operated on by the higherorder generalized input coordinate kinematics (i.e., velocity and acceleration). This result for the dynamic model is greatly facilitated by the use of kinematic influence coefficients and yields a highly geometric description of the system dynamics where the effect of the action of the generalized coordinates on each other is transparent. It should be noted that this transparency is of extreme importance (if not absolutely necessary) when dealing with the transfer of the dynamic model from one set of generalized coordinates to another. The Principle of Virtual Work This principle is the first variational principle in mechanics and aids in the transition from Newtonian (Vectorial) dynamics to Lagrangian (Analytical) dynamics. It is concerned with the static equilibrium of mechanical systems, and can be stated as: The work done by the applied 56 forces in infinitesimal reversible virtual displacements compatible with the system constraints is zero. To get a more analytical handle on this principle, consider a system containing N, Pdimensional, dependent motion parameters u = (iul iu2 iuP)T i =1,2,...,N acted on by N, Pdimensional, applied loads (2119) iTu = (iT1 iT2, ..., iTP)T ,i=1,2,...,N (2120) where the preceding superscript indicates which parameter is being considered, and the applied loads can be viewed as implicitly dependent on the generalized coordinates. Further, suppose that the system has M degreesoffreedom and, hence, its motion can be completely described in terms of M generalized inputs, q = (ql, q2..., qM)T (2121) and that these coordinates are acted on by M generalized forces jq = (T1, T2, ..., TM)T (2122) For the system to be in static equilibrium, the principle of virtual work states that the virtual work (8W) must be zero, or 6W = T q q + i1 u u (2123) i=1 (223 (Sq)T T + N (6iu)T iTu q i=1 = (5q)T Tq + N eTiuTiu =(q)T Tq + iE ((8q)T[iGu]T)iTu i=l q =0 57 since, for the virtual displacements (&iu) to be compatible with the system constraints (see equations (27) and (28)) 6iu = D J 6qn = [iGul& n=l Dqn q (2124) Now, because the virtual generalized displacements (6qn) are all independent, hence arbitrary, equation (2123) yields M independent equations for the required generalized loads as, in vector form N iGuTiTuTL il q q (2125) where TL is the effective load at the inputs due to all q applied loads. The Generalized Principle of D'Alembert The statement of this principle is as follows: the total virtual work performed by the applied and inertial forces through infinitesimal virtual displacements, compatible with the system constraints, is zero. To see how this principle can be used to obtain the generalized input loads (Tq) required to overcome the system's inertia and cause the desired motion, again consider the dependent parameters (iu) of equation (2119) where, now, they describe the kinematics of the system's mass parameters [iMu] = P x P i=l,2,...,N (2126) This allows the system's momentum (Lu) (both linear and angular) to be expressed by the N, Pdimensional, vectors (Lu) as 58 NN LU = E (iLu) E ([NiMUi( i=1 i=1 Further, assume that there exist N, Pdimensional, load vectors (iTu, i = 1,2,...,n) that if applied to the associated mass motion parameters (iu) would cause the desired system kinematics (i.e., the position, velocity and acceleration of the iu). The generalized principle can now be written as N N d_ _= Z (iTu iLu) iu = (iTu d ((iMu]iu))'iu = 0 i=l i=l t (2128) yielding, for the virtual work (6W) done by these hypothetical applied loads (iTu), the following relationship: N iu N 6W =E ITu 6iu = d([ MuJla) 61u (2129) This result, in itself, is not very useful since the virtual displacements (&iu, i = 1,2,...,n) are not independent. However, when one recognizes that the virtual work can also be expressed in terms of a set of M generalized forces (Tq) acting on a corresponding set of generalized coordinates (S), which are independent, as (where here the generalized forces are, in essence, replacing the hypothetical applied loads instead of opposing them, as in equation (2125) to obtain static equilibrium) N 8W = ~u iu = Tq 6q(210 i=l (2130) the following, very powerful, result occurs: 59 N Tq 8 d([iMu]i3) 6iu Tq 6 ~= 1~ .. N (2131) = N d([iMu1 ) [iGU]6i=l jt q or, following manipulations similar to equations (2123), the required generalized forces are N 1q = Ej [IGU]T { d([iMujiu)} = q dt (2132) Investigation of equation (2132) yields an interesting result. That is, the effect of the dynamics of each mass motion parameter (e.g., each link) on the generalized inputs can be considered independently (also see Silver, 1982, equation (25), where the dynamics of each link are expressed in terms of the NewtonEuler equations). In fact, the result of equation (2132) can be obtained from the derivation of the generalized input loads required to offset a set of applied loads (see equations (2123), (2124) and (2125)). To see this, simply replace the applied loads (iTu) in these equations with either the d'Alembert loads (&_([iMu]ia)) or the negative of the NewtonEuler dt equations. In this light, one could view the result of equation (2132) as being obtained from the virtual work of the d'Alembert loads. Now, recalling equation (27) for (iu), equation (2132) can be written as N Tq = E [iGu]T f d([iMu][iGu] )} i=1 q dt q (2133) 60 Recognizing that the momentum vectors are functions of the generalized coordinate positions and velocities (i.e., q and q), one has that d([iMu][iGuj4) = 3 ([iMu][iGu]4)4 + ([iMu][iGu])q d q q dt q (2134) The second of these terms follows immediately from equation (213) as 3 ([iMu]iGu]4)q = [iMu][iGujq (2135) q  q If the mass parameters ([iMu]) are configuration independent (i.e., [iMu] = f(q)) (which is not the case when dealing with the effects of inertia in spatial devices, but including the generalization gives no insight here and the question will be dealt with specifically in the case of the serial manipulator), then recalling the development of equations (214) through (224), the first term of equation (2134) becomes ([iMu][iGu]) = [iMu]qT(iHu ]q (2136) q q qqSubstituting the results of equations (2135) and (2136) into equation (2133) gives for the required input loads N Tq = E {Gu]T iMu]tiGu]} i1 q q (2137) N + N {[iGujT[iMul4T[iHu ]} i=l q qq Note that for constant mass ([iMu]), this equation can be immediately obtained by substituting equation (227) for (iu) into equation (2132). Now, equation (2137) can be written in the general form for the generalized inertia loads (TI ) as *q 61 TI = [I* ]q + Tfp* (2138) q qq L qqq where, the configuration dependent coefficient = N [I* Z [iGu]TiMu][iGu] = M x M qq q q (2139) is a positive definate matrix representing the effective inertia of the system as seen by the inputs. The system's kinetic energy is also expressable in terms of this coefficient (see Thomas, 1981) as KE = 1 iT[I* ]4 qq (2140) The configuration dependent coefficient * =N ((iuT[iMu]) LP* I E (([iGU] [iHu ]) = M x M x Mt qqq 1=1 q qq (2141) is the inertiapower modeling matrix dealing with the effects of the velocityrelated acceleration terms. Note that the subscripts on the model coefficients indicate, not only the dimension of the result, but also aid in investigating specific terms. For example, the nth generalized inertia load is TI = [I* ]j + 4T[p* ]q n nq nqg (2142) = rfj* I qj + q*T[P* qq n; qqq n;; where 62 nq ] N [iGu]T[iMu][iGuj =q n [q[ (2143) and [Pq J N {((iGU]T[iMu]) 2[1 nqq i=i n qq (2144) It should be noted that the form given in equation (2138) for the inertia related terms of the dynamic model is completely general and applies to all types of rigidlink mechanisms. The only differences lie in how the kinematic influence coefficients are obtained and, if the mass parameters ([iMul) are configuration dependent, the coefficient [P* ] contains an additional term (as will be nqq seen in the serial manipulator development). The Dynamic Equations Finally, recalling equations (2125) and (2138), the generalized input forces (or torques) required to generate the desired system trajectory under load are given by I L =q Tq Tq (2145) = [I* ]T + NT* ] [ _q +_q P Iq E[ iGU]T iTu qq qqq i=1 q This result shows the highly geometric nature of the dynamics of multibody systems, and is precisely the reason why this representation for the dynamic model is chosen over the NewtonEuler form. While this form may, or may not, immediately yield the most efficient scheme for realtime 63 computation (Hollerbach, 1980), the insight that can be obtained from this geometric view is considered essential for realtime control. What is meant by this is that, if one does not understand the essence (which is geometric) of how the effects of the system parameters (e.g., system mass, endeffector loads) on the inputs vary, then it is unlikely that one can intelligently address the question of any type of control, much less realtime control. Regardless, as will be seen later, when dealing with cooperating manipulators (including walking machines and multifingered hands) the geometric form of equation (2145) is extremely useful, if not altogether essential. This form (equation (2145)) is also convenient in addressing the question of dynamic simulation. Here, since the effective inertia matrix ([1* 1) is positive definite qq (i.e., its inverse always exists),one can determine the system's response by solving equation (2145) for the generalized accelerations as = Iq]q (~ qqq' N~ IuTiU (2146) While any of a number of numerical integration routines can be employed to solve this equation, multistep predictorcorrector methods (e.g., Adams) are suggested over singlestep methods (e.g., RungeKutta) due to their greater efficiency regarding computetime versus accuracy. This greater efficiency is desired because of the computational 64 complexity of the function evaluations required at each time step. It should be pointed out, however, that the effective inertia matrix is the only model coefficient necessary for the response prediction. The remaining terms are perhaps best obtained in terms of a recursive formulation due to the supposed greater computational efficiency afforded by that method (Walker and Orin, 1982). Also, the influence coefficient model formulation allows one to directly address the question of design (Thomas, 1981). Here, the model parameters can be used in conjunction with classical optimization techniques to develop actuator sizing (Thomas et al., 1984) and stiffness (Thomas et al., 1985) criteria, as well as motion and load capacity due to actuator limitations (Thomas and Tesar, 1982a). Dynamics of Serial Manipulators The development of the dynamic equations presented here wiil follow the structure of the previous parts of this section. First, the effect of applied loads on the manipulator's generalized inputs ((p) will be addressed using the principle of virtual work. Next, the systems inertial effects will be considered via the generalized principle of d'Alembert. Finally, the resulting dynamic equations are presented in the form of the generalized dynamic model of equation (2145). 65 Applied loads As with the kinematics, it is convenient to separate the rotational and translational properties when dealing with the dynamics of rigid bodies undergoing spatial motion. In this light, consider a set of M, 3component, force vectors (JfP, j = 1,2,...,M) and a set of M, 3component, moment vectors (mJk, j = 1,2,...,M) applied to their respective translational (J) and rotational (wjk) motion parameters. Now, immediately from the virtual work result of equation (2125), the effect of the applied loads on the generalized input coordinates ((p) is given by N TL = Y ((JGP]T JfP + [GJkjT mjkj ( j=l ( P (2147) where, the Jacobians are both 3xM matrices defined in equations (266) and (255). Inertial effects To address the effects of the system inertia one could, as previously implied, simply apply the principle of virtual work to the classical NewtonEuler equations of motion for a rigid body, yielding M TI= {[JGcjT gjk jac S =1 (2148) + [GJk]T([IiJk] aik + Wjk x [Iikjwjk)} Here, Rjk and [Ijk] are, respectively, the mass and global inertia matrix of link jk, with 66 3ac = [JGC]3 + CT(JHc ]c (2149) O P( being the acceleration of the center of mass of link jk (Jc), and W jk = [Gjk]p (2150) (P and cjk = [Gjk]jg + T[Hjk] (2151) are the absolute angular velocity and acceleration of link jk. Now, by substituting equations (2149), (2150) and (2151) into equation (2148) and algebraically manipulating the result one could obtain the desired general form of equation (2138). While this operation gives the desired result it is felt that a slightly more rigorous approach, starting with the system momentum, gives more insight into the geometric nature of the solution. The momentum of a rigid body (e.g., link jk) can be expressed in terms of two distinct vector quantities; these being the linear momentum (pjk) expressed in terms of the mass (Mjk) and the velocity (iVc) of the center of mass (Jc), and the angular momentum (Ljk) given in terms of the link's angular velocity (wjk) and the global inertia tensor ([IIjk]) about the centroid of the link. The effective generalized inertia loads resulting from the rate of change of the system's linear momentum can be obtained directly from the development of equations (2133) 67 through (2141). This is possible since the link's mass (Fjk) is constant with respect to the global reference (i.e., independent of the system's configuration) and can be expressed as [JMU] = jk(I]3x3 (2152) which allows one to write the link's linear momentum as pik = Rjk[jGC] (2153) The time rate of change is now jk = rjk([ jGcI + TJHc]  V (2154) and yields PT = M [jGCIT pjk Q j=1 0 (2155) as the total effective load due to changing linear momentum. Expressing equation (2155) in model form gives the load as TI = [PI* ] + T[Pp* (2156) ( (2156) with (PI* ] = M [Pyjk] =j (M[GCT[jGc]) = Mx M (P9 j j=Q (2157) and [p,] M M [P ] = [fPpjk ] = jk([jGc]T [JHc ]) = 3 x M x M VPQ j=l (P2 j=l 2 1 (2158) 68 where the Jacobians ([JGc]) have shape (3xM) and the secondorder coefficients ([jH ]) have shape (3xMxM). The development of the effective inertia loads (LT,,) caused by changes in the system's generalized angular momentum is more difficult. This is due to the fact that, while the local inertial properties ([(J)IIJk]) of link jk are constant, the inertia properties ([IIJk]) expressed in terms of the global reference are not. As previously mentioned, this configuration dependence, resulting from the rate of change of the local reference frame in which the link's constant local inertia matrix is expressed, introduces an additional term into the inertial power matrix L* ([Lp]) With this in mind, it is perhaps best to initially view the global angular momentum of link jk (LJk) in terms of the link's angular momentum as expressed in its own bodyfixed reference ((j)LJk). Recalling the use of the rotation matrix (TJ) illustrated by equations (247) and (258), one has Lk = TJ(j)LJk (2159) Now, the local angular momentum is (j)LJk = [(J)Iijkj(j)wjk (2160) where, noting that the rotation matrix is orthogonal (i.e., (TJ]T = (TJ]1), the link's locally expressed angular velocity is (j)jk = [TJjTwjk = [Ti]T[Gik] (2161) 69 Combining equations (2159), (2160) and (2161) gives the globally referenced angular momentum of link jk as Lik = [Ti][(J)IIk][Ti]T[Gjk]2 (2162) This is the desired form of the momentum since all configuration dependent terms are shown explicitly. Even so, notice that the global angular momentum of link jk (L3k) can be expressed in the same form as the local momentum (equation 2160) as Ljk = [ijk]wjk = [Iijk][Gjk]g (2163) (P where, from equation (2162) [IIjk] = [TJ][(J)IIk][Ti]T (2164) is the globally referenced inertia dyadic. As before, the momentum is seen to be a function of the generalized coordinate positions (p) and velocities (W). Therefore, the time rate of change of link jk's angular momentum can be obtained from jk Ljk jk .. Lk =DL ek + DL jk _a (2165) The second term in equation (2165) is immediately seen to be ak [IIjk](Gjkj (2166) =( 70 The position derivative is not as transparent and will be pursued in terms of the standard Jacobian formulation for vector differentials. Although there are three configuration dependent terms in the momentum expression, the differentiation will only be broken into two separate operations and addressed in the manner of Thomas (1981). This method gives, for the position derivative jk = ([ijJk]wjk) = _([IIjk][Gjk] ) (2167) = (T 3) [(j)IJk][Tj]T[Gjk] + (TJ]T[(j)j1 k]_ _([TJ]T[GJk] ) To obtain the first partial in equation (2167) it is beneficial to remember what the elements are that make up the columns of the rotation matrix: Tj= [*ajk *sj x*aJk *sJ (2168) Here, one sees that the vectors whose direction cosines make up the columns of (TJ) are free vectors fixed relative to (i.e., in) link jk. In this light, recalling the more geometric formulation of the kinematic influence coefficients, the differentiation yields the familiar vector cross product result 71 (T ) = gjk x TJ n=1,2,...,M (2169) On n Defining the remainder of the first term of equation (2167), for convenience, as b = [(j)jIjk][TJ]T[Gjkj T (2170) and observing the Jacobian convention, gives the nth column of the first partial in equation (2167) as T]b) = I([T ib) = gjk x [TJ]b ;n (n n (2171) where, the vector (b) is considered constant in this operation as indicated by the bar overstrike (b). Introducing a generalized cross product notation (x) one has [Gk]T x TJb S [gjk x TJb : .. gjk x TJb] 1 M (2172) where the first dimension (i.e., row) of the left argument operates across the first dimension of the right argument (i.e., across all rows). Now, recalling equations (2164) and (2170), equation (2172) gives, for the first partial in equation (2167), the relationship 3(TJ)[(j)IIJk][TJ]T[GJk] = (GJk]T x [IIk][GJk]i 3 Q (DQ (2173) Turning our attention to the second partial in equation (2167), the first step (Thomas, 1981) is to recognize that since the rotation matrix (TJ) is orthogonal, i.e., 72 TJ[TJ]T = [I]3x3 = constant, (2174) one has that 3(TJ[T]T[Gjk]) = ([Gk~) ( (P (2175) Using the chain rule on the lefthand side of equation (2175), and referring to equations (220) and (2173), yields [Gik]T x [Gjk]$ + TJ _([TJ]T(Gjk]$) = GT[Hjk]T ( P(P VPX (2176) Rearranging and premultiplying by [TJ]T gives _([Tj]TIGjk]g) = [TJT( T[Hik T GjkT x [Gjk]) ( P (P (P (2177) Direct investigation of the righthand side of equation (2177) shows that &T[HJk]T (GjkjT x [Gjk = T[Hik] (2178) Confirmation of the above result is left to the reader; however there is another way to show the validity of the final result which also points out an interesting result. Considering that the partial derivative of equation (2177) is to be postmultiplied by 4, the ultimate usage of the relationship given by equation (2178) can be justified by showing that ([GJk]T x [GJk) = (2179) ( (2179) 73 since T[Hjk]T = T[HJk (2180) (2 1 Recalling equations (255) and (2172), and manipulating the result, one has the following: ([Gjk]T x [Gjk]}() = ([Gik] x _jk) (P (P (P (2181) = [ajk x jk k x jk]1 = (ik xjk n=l n n = ijk x &jk = ([Gjk] ) x ([Gjkj$) (P (P =0 Therefore, while not rigorously verifying equation (2178), the preceding proves that _([TJjT[Gjk]_) = [Tj]T(AT(Hjk] ) z (2182) which is the term ultimately needed. Finally, by substituting equations (2166), (2173) and (2182) into equation (2165) and, recalling equation (2164), the time rate of change of the angular momentum of link jk can be expressed as Ljk = [(ijkj[Gjk] + [IiJk]2TlHjk]* 9 (2183) + ([Gik]T x fIIJk]l[Gjk])$ (PD 74 Note that, by combining the first two terms of equation (2183) and following the algebra of equation (2181), equation (2183) is an alternate form of Euler's equation of motion for a rigid body as expressed in the global reference N = L3k = [Iijk]0jk + ajk x (IIjk]wjk (2184) Now, referring to either equation (2132) or (2148), to obtain the generalized load required for this change in momentum (LTI) one premultiplies equation (2183) by the transpose of the link's first order rotational influence coefficient ([Gk jT), yielding LTI = M [G TLk = {[GJkT Jk][GJk E f(GjG]k]Tjjik[= k]( (D j=ll (2185) + [Gjk]T[Ijk] T[Hjk 5 + P W + [Gjk]T([Gjk]T x [Gjk])p)} (D (P (P To get this equation into the desired form of equation (2138) requires some manipulation of the velocity related terms. The first of these terms is readily accommodated by the generalized dot product (see Appendix A) giving [GJk]T(IIJk]T[Hjk]q = 2T([GjkjT[I!jk] [Hjkj)g (2186) The remaining term is a bit more obscure and is attacked with the aid of the following vector relationship: a b x c = c a x b (2187) . . .(2187) 75 Recalling LIk (jjjkj[Gjkj (2188) (P one has that (GjkjT([GjkjT x (Ijjkj[G"r]$) (P (P (P (Gjk]T([GjkjT x Ljk) (2189) (P (D [GjkjT( [gjk x Lik (P g3K x Lik] m jk qjk x Lik ... gjk gjk M x Lik ik gjk x Lik ... gjk gjk M M x Lik ;jjk ik x lik ... LJk ik ik x 2 m Lik _q k x aik Lik k x k 1 4 41, (Lik)T[[Gjk]T x CGjk]j (P (P where ([GjkjT x (1jk]] = gjk x [Gjk] = 3 x M n;; n (D so (2190) (Gjk]T((GJ'kjT x[jjjk] (P [Gjkj )& (P  (2191) T([Gj k]T[jjjk]((GjkjT x (Gjkj)) (D (P 76 Now, from equations (2185), (2186) and (2191), one has LmI [Li* TrLp* (2192) I (p + (p I W CPQ QQ(D where (Lj* M (Ljjk] ((Gjk]T(Ijjk][Gjk]) (DQ j=1 W j1 Q (P (2193) and M M LLp* E (Lpjk E f[GjkjT(Ijkj [Hik]) QW j=l WQ J=1 Q W ([Gjk]TIjjkj([GjkjT x (Gjkj))) (2194) Finally, combining the effects of both the linear and angular momentum gives TI = (1* 12 + T*r'nLP* 1$ (2195) Q W QQQ for the total generalized inertia load, where [I* [Pj* ] + [Lj* QQ Q(D QQ (2196) k[jGc]T[jGcj + (GjkiT[jjjk][Gjk,) j=1 (D Q Q Q J and [p* [Pp* [Lp* I QQQ QQQ (2197) M E (Rik([jGcjT [jHc 1) j=1 (D QQ ([GjkjT[Ijjkj [Hjkj) Q QQ (Gjk]Trjjjk]((G'kjT x (Gjk])), 77 The dynamic equations The dynamic equations for the general Mdegree of freedom serial manipulator can now be written in the desired general form of equation (2145), giving the required generalized input load as S[T (I E ([jGP]T jfP + [GJk]TmJk (P (2198) with the I* and P* matrices as defined in equations (2196) and (2197). Solving this equation for the generalized coordinate accelerations gives the simulation form of the dynamic equation = [I* I1{T [p* ] + TLJ (DCP (P((P Q (2199) used to predict the motion of the manipulator resulting from the load state on the system. This completes the development of the dynamic model for the general Mdegree of freedom serial manipulator. The two secondorder equations (2198) and (2199), or some other representation of these relationships, are the usual stopping points in the study of the motion of rigidlink mechanisms. While one can determine the input loads (T(P) (force or torque) required to cause a specified motion (p(t)/, or the response (((t)) of the system to the specified load state (T&), the question of control is not immediately addressable. The reason for this is that these equations assume the instantaneous availability of finite 78 torque. This availability is not generally the case since the actuators themselves are governed by their own dynamic equations. In an effort to overcome this deficiency the following section, cognizant of modern linear control theory, is included. The Linearization of the Dynamic Model Many researchers (e.g., Kahn and Roth, 1971, Dubowsky and DesForges, 1979, Horowitz and Tomizuka, 1980, Liegeois et al., 1980, Armada and No, 1981, LeBorgne et al., 1981, Golla et al., 1981, Freund, 1982, Guez, 1982, Vukobratovic and Stokic, 1982a, and Stoten, 1983) have investigated (and are continuing to investigate) various methods of linear and nonlinear, adaptive and nonadaptive, control. While it appears likely that nonlinear adaptive control schemes hold the greatest promise for the ultimate realtime control of highly nonlinear systems such as robotic manipulators, the development of such a scheme is not the purpose of this section. The purpose of this section is, instead, to use the well established technique of linearization about a nominal motion trajectory, to develop a generalized state space model. once this model is established, while again not the purpose here, one can apply the tremendous body of knowledge of modern control theory to address the feasibility of such control methods. Here, of primary concern would be the question of the time variance of the 79 coefficients in the state space model. In other words, for a given task (or trajectory), do the coefficients vary slowly enough to allow the system to be treated as time invariant with respect to controller design? While this type of formulation is not original (see Vukobratovic and Kircanski, 1982b, Lee and Lee, 1984, and Whitehead, 1984), it does not appear to have been fully investigated. Therefore, for possible future investigation (for specific shortterm solutions) and completeness, this section presents thedevelopment of the generalized controlling state space equations for the serial manipulator, including actuator dynamics. The Linearized Equations of Motion The first step in the development of the controlling state space equations is the linearization of the dynamic equations with respect to the prescribed state variables (ignoring, for the moment, the actuator dynamics). Here, adopting the standard velocityreferenced model, the deviations in the generalized coordinate positions (8o) and velocities (& ) make up the (2Mxl) state vector (x), or Q 6()=( actual p nominal CP actual CP nominal/ Now, noting that the generalized load (q:(,) is a function of the generalized positions ((p), velocities ()and accelerations ( 6), one has 80 = = + 6 + (2201) I(t)nom. (P(t)nom. (P(t)nom. as the general equation for the linearized dynamics (the question of independent external load disturbance is addressed later). The task at hand, then, is to derive expressions for the partial derivatives in equation (2201), to as great a degree as possible, in terms of the same general algebraic operations and parametric modeling coefficients employed in the dynamic equations themselves (to minimize the additional computational burden). Writing the generalized load in the form of equation (2198) gives /0 + iT(p* [jGu]T jTu (M (P j=1 (P (2202) where (JTU) is a general applied load vector associated with link jk consisting of both force and moment components (and is assumed to be constant in the following derivations). This form allows one to obtain the first two terms in equation (2201) directly from previous operations. Recognizing the similarity in the form of equations (227) and (2202) and, recalling the development of equations (228) through (236), one has that DT _CP (2203) and 81 d6 = ([P ] + LP ]T 4 (2204) Now, to address the third term of equation (2201), particularly in light of the previously described generalized vector dot (*) and cross (x) product operations, it is convenient to express the dynamics in terms of the NewtonEuler form of equations (2147), (2155) and (2185) as T =Z ([Gjk]T Lik + [jGc]T jk (jGuT jTu) (2205) Investigating the last term of equation (2205), where the preceding superscript (j) is dropped, one has the standard Jacobian result T 3TL TL i .1 1 9P1 3(2 YPM Gu T u TL TL (2206) [ T]I ) = (p = 2 9TL 3TL M M This gives the component in the mth row and nth column as 3TL uT gu u m = ((T ) (m)) = (Tu)T h D3n 8 n (2207) or, for the total MxM matrix result, reintroducing the superscript (j), one obtains the expression 82 JGu T i u ([ P] ) = (JTU)T [jHu T Bg 49 (2208) The contribution of the linear momentum can be determined, using the chain rule, from jGc T.jk 3Gc Tjk .k 3([ P1 ) = 2([ ) + [JGc]T (.3) (2209) The first term, where the change in momentum is considered constant (signified by the bar notation), follows directly from the previous derivation and yields 3Gc Tjk ([ G ] ) = (_jk)T [jHc (2210) with the symmetry in the translational (h,H) function observed. Recalling equation (2154), one has that 3. U )= jk 3(([JGc 1 + jT[H Hc ) 3 (2211) which, referring to equation (238) and (241), becomes jk +~~~ ~ ( jk ) = M]k(jTjHc + TDC (2212) Now, premultipling equation (2212) by the Jacobian transpose and substituting, with equation (2210), the result into equation (2209) gives ]Gc T jk2213 (Gm T )T ] (2213) 9(tP* QQj~ 83 + jkjG T jHC + Rjk[ jGCjT(T[HjD]) Application of the generalized inner product to the second term of this equation yields Mjk[jGc]T( TjHc 1) = T(jkjkGcjT [JHc ]) (P (P(P(P (P(P (2214) which, recalling equation (2158), gives the fortuitous result jk[J3Gc]TT(jHc T) = TPpjk (P W (PP (2215) Finally, substituting equation (2215) into (2213) one obtains the expression 3GC T.jk .k)T [jH1 3 ( P ] ) = (P ) Hc (2216) ST (Pjk] + gJk[ jGC T( T[ jDC ]8 Now, recalling the result of equation (2208) and the form of equation (2209), the contribution of a link's angular momentum to the last term in equation (2201) can be written immediately as ([GJk T jk = (JLik)T ([Hik]T + [Gjk]T a(i ) (2217) 84 To obtain the final partial derivative it is perhaps most convenient, especially considering previous derivations, to use Euler's form for the change in angular momentum (see equation(2184)), or =. j Jk]jk + [k x [Ijk]jk) (2218) Addressing the first term one has (see the development presented in equations (2167) through (2177)) ([IIjk]jk) [GJkjT x [Ijk]jaijk (2219) + [iiJk]( gjk [Gjk]T x a jk) Recalling equation (270) for link jk's angular acceleration and the form of equation (243) for the first geometric derivative of the acceleration allows one to rewrite equation (2219) as ([JIIjk]cjk)= ([Gjk T (jk(Gjk Tjk (2220) + [IIJk{(T[Hjk]T + T[Djk ](2220) [Gjk]T x ([Gjk] + T[Hjk)} WD Q (D(P Utilizing the result of equation (2178), equation (2220) can be reduced slightly to 3([iiJk]ajk) = ([Gjk]T x tjJk]((Gjk] + jT[Hjk] )) (2221) + [IIjk](T(Hjk] + T[Djk ]$ [Gjk]T x T[Hik]) VDP (P(P (pV 85 Now, returning to equation (2218), one has that _8(jk x [iIjk]jk) = __(mjk x LJk) (2222) = jk x Lik + wjk x Ljk Substituting the results of equations (220) and (2167), respectively, for the partial derivatives in equation (2222) give a(jk x Ljk) = (T(Hjk]T) x Lik 8 (2223) + wjk x ([Gjk]T x Lik + (jk IIk](T[Hjk])) Q P( Combining the results of equations (2221) and (2223) to obtain an expression for equation (2218) and substituting this expression into equation (2217) yields, in light of equation (2194) and the generalized vector dot (*) and cross (x) products, the 3xM matrix 3([GjkjTLjk) = (Ljk)T [Hjk]T (P TLpjk (2224) + [Gjk]T{([GjkIT x T( jJk] [HJk)) (D (P (D(P + (($T[Hjk]T x [IIjk]j[GJk]) + (([GJk] /T x ((jIIJk](T[HJk]T) + [GjkiT x ([IIjk][Gjk]))) (P (P 86 + [IIJk]((T[Djk ] [GJk]T x T[HJk]) (P(P Q OQ Finally, substituting the results of equations (2203), (2204), (2208), (2216) and (2224) into equation (2201) one has the general linearized dynamics given by (where all coefficients are evaluated at the nominal generalized coordinate values) ST = j 84 + T( + (2225) 0L L ((2225) = [I* ]B + [iT([p* ]+ [p* ]T)]6 + {(jTU)T [jHu ]T + (pjk)T [jHc ]T S(jkr)T. [Hik]T + [JGC]T g(pJk) QQD + [GJk]T (Ljk)}] 8( Notice that, if the load (JTu) is a known function of the generalized coordinates, the effect can be easily accounted for. For example, if the load is a function of velocity the second term in equation (2201) will become @T T [p* + p* ]T) M j u 8 l [([p ] + L ]) [JGU]T T!LT (2226) Also, if the load itself varies from its nominal value, which is likely, equation (2201) contains the additional term 87 M T M E =P E O(G]T &jTu j1 jT j=i Q~j (2227) Investigation of equation (2225) shows that no new computations are required for the first two coefficient matrices however; for the third (i.e., (3T~] there appears to be a considerable additional requirement. In view of this, especially with recognition of the considerable similarity in the basic nature of the various components making up this coefficient, a detailed investigation of this matrix to determine the minimal reduced equation is warranted. However, since the linearized model coefficients are based on the nominal trajectory and determined offline, this effort is left for future work. Actuator Dynamics In this section only one of many types (e.g., electromechanical, hydraulic, pneumatic, etc.) of actuators is investigated. For additional information on actuators and their dynamics the reader is referred to Borovac et al. (1983), Electrocraft (1978) and other sources. The specific type of actuator addressed here is the separately excited dc motor (Myklebust, 1982). In this device, the field current is held constant and speed control is accomplished by control of the armature terminal voltage. The controlling equations for this type of device are 88 Ldi + Ri + CEq = V (2228) dt TI + Bq CMi = Tq for a single actuator or, in matrix form, for all M actuators [Lq]i + [Rq]i + [CEq]k = Y (2229) [q] + [Bq]{ [CMq1] = q where i is the Mxl vector of armature currents (amp), v is the Mxl vector of terminal voltage (volt), [Rq] is the MxM diagonal matrix of armature resistances (Ohm), [Lq] is the MxM diagonal matrix of armature inductances (Henry), q is the Mxl vector of motor output shaft angles (rad), [Tq] is the constant MxM diagonal matrix of rotor inertias (in.lbfsec2), [Bq] is the MxM diagonal matrix of rotor damping (in.lbfsec), (CEq] is the MxM diagonal matrix of motor speed constants (voltsec), (CMq] is the MxM diagonal matrix of motor torque constants (in.lbf/amp) and Tq is the Mxl vector of load torques (in.lbf). Addressing the actuator dynamics one must relate the M output shaft parameters (q) to the M generalized joint coordinates ((p). Here, the following assumptions have been made: of constant transmission between the motor (g) and joint ((p) parameters (i.e., the Jacobian [GP] is constant), q zero compliance in the drive train, and zero backlash in the drive train. Recalling equations (27), (227) and (2130) allows one to write 89 = [Gm] q (2230) =[GO] [GP] = constant q and = [G(Pj TT (2231) q where the MxM Jacobian ([G(]) is dependent on the specific q drive trains employed. Here, the entry in row m, column n relates the motion of the mth joint parameter (om) to the nth motor shaft (qn). Typically, this matrix is diagonal since each joint is usually controlled by a single associated motor. In fact, for direct drive devices this Jacobian is simply the MxM identity matrix. At any rate, having obtained the Jacobian ((GQ]), and recognizing that it q is the joint motion (not the motor shaft) that one wishes to control, equation (2230) is used to find the desired relations = ](2232) q and q (2233) Now, substituting equations (2231), (2232) and (2233) into equation (2229) yields (Lq]i + [Rq]i + (CEq][G(]1 = V (2234) q 90 [Iq][G']1 + [Bq][G]_1 [CMqJi = [GP]TT q q q for the motor dynamics expressed in terms of the joint parameters. Finally, the actuator dynamics (equation (2232)) can be expressed as [Lq]i + [Rq]i + ICE(p](P = V_ (2235) and [I' ] I [BCJ + [CMW] = T (2236) where the second of equations (2234) was premultiplied by minus the inverse transpose of the Jacobian and, for example, where [I I]= [GQ]T[Yq][GIP]I P[q q (2237) One should note that, with the stated assumptions, all the coefficient matrices on the lefthand side of equations (2234) and (2236) are independent of the generalized coordinate positions (p) and, since the variation of the motor parameters is not addressed, are considered constant. Having obtained the desired expressions for the actuator dynamics, one can now determine the nominal input voltages (M) required to drive the system along the specified nominal trajectory (((t)). Recalling equation (2202) and solving equation (2236) for the current gives 1 = [CMI]I(T + [B ]p) (2238) where the effective rotor inertia ([I,,]) has been included in the effective inertia matrix ([I*]). Now, 91 differentiating equation (2238) with respect to time, and recalling equation (2225), yields i M ( + ( + [ BO (D I P (2239) Finally, assuming that the nominal joint kinematics are known up to the third order (iL.e., Q(t), 4(t), *iP(t) and t) are known), equations (2238) and (2239) can be substituted into equation (2235) to obtain the nominal voltage (V) as Ynominal = Y(1rii~oia (2240) As with equation (2225) for the linearized generalized load (6T~), equation (2240) could possible be reduced to a minimum generic form of the class of equation (2145) (TQ). Again, this question will be left open for now since the immediate usage of the linearized scheme only required offline computation of equation (2240). State Space Representation Having incorporated the manipulator dynamics into the describing equations for the actuator dynamics, and having determined the nominal input (voltage) requirements one can proceed to develop the state space model of the system. In the case of the dc motor driven manipulator, the state variables are chosen to be the deviations in the generalized coordinate positions (5(p) and velocities (6$) and the deviation in motor currents (6i). With this selection of the state variables the state space model is of the form 