RSD-TR-13-83 DYNAMIC SIMULATION OF A FLEXIBLE ROBOT ARM AND CONTROLLER Nabil G. C alhoub A. Ga ' OTsoy Department of Mechanical Engineering and Applied Mechanics September 1983 TtIE..'! % w~ B TY. Q w i-i XGAN v O- 4 i', Robot Systems Division COLLEGE OF ENGINEERING UNIVERSITY OF MICHIGAN ANN ARBOR, MICHIGAN 48109

tet )A u ( th1 LUA^UBtvk- V- t i k

TABLE OF CONTENTS ABSTRACT.................................................................................. 3 INTRODUCTION............................................................ 4 M ODELING.................................................................................. 5 CONTROL............................................................ 19 SIMULATION............................................................ 23 DISCUSSION............................................................ 29 SUM MARY................................................................................... 30 REFERENCES.............................................................................. 37 APPENDIX I - Static Equilibrium and Reaction Forces.................... 40 APPENDIX II - Equations of Motion............................................... 41 APPENDIX III - Equations of Rigid Body Motion Only....................... 48 APPENDIX IV- Derivation of the Controller Design........................ 54 APPENDIX V - Listing of the Computer Program............................ 59 FIGURES.................................................................................... 74 TABLES............................................................ 99

NOTATION The underbar beneath any variable ( ) indicates an n dimensional vector. The symbol d( ) in front of any variable denotes either virtual displacement or virtual force. A star superscript indicates a value related to the center of mass. Other values are related to the rotating coordinates (i, i, k). A dot or a double dot over any symbol, ( ) or ( ), indicates first or second derivative with respect to time. The symbol denotes the derivative with respect to time of the vector 6t components only (i.e derivatives with respect to time of the basis vectors i,j,k are excluded).

ABSTRACT High performance requirements in robotics have led to the consideration of structural flexibility in robot arms. This paper employs an assumed modes method to model both the rigid body and flexible motion of a spherical coordinate robot arm. This model is used to investigate relationships between the arm structural flexibility and a linear controller for the rigid body motion. This simple controller is used to simulate the controllers currently used in industrial robots. The simulation results illustrate the differences between leadscrew driven and unconstrainted axes of the robot; they indicate the tradeoff between speed and accuracy; and show potential instability mechanisms due to the interaction between the controller and the robot structural flexibility.

ROCHESTER INSTITUTE OF TECHNOLOGY, ONE LOMB UJ m z 0 I - E3BL BE 9 oC no- C I,w O0 BEI6 a mmLurL n JII 21U)I wIm a) - 53)C~l.> () 9 \ O EJS E L BE 0 m se31 2 0, ^00^-0 111 1^ rLn ^ mQ 3lm i v a 5: mmWW 32E z rn OmmmmC,~_U3,E Io - ' NOOSNH C:-LI 'iNa SiNinoa iw~nVcvI lL-lAlU '133I1s0 lS31 NOIinl OS3H oIH3inNtnHdl I IS

RSD-13-83 The first step in improving the performance of robots is to obtain a reasonably accurate dynamic model. Usual techniques solve for the gross body motion and the flexible motion separately. Since the elastic motion is small then its effect on the gross body motion is neglected. Therefore, only the effect of the rigid body motion on the flexible motion is considered. This is done by solving the gross body equations for inertial forces which in turn are introduced as excitation source to the elastic problem. Recent techniques use coupled reference position and elastic deformation models. Some researchers used finite element techniques to describe the elastic deformation [3], [4]. Others used the assumed modes method [5]. The latter approach is employed in this study. This approximate technique can be used to yield a set of equations which represent the combined rigid and flexible motion. The next step is to design a controller which insures the desired performance of the manipulator. The conventional linear control techniques have led to poor performance when robots are operated over a wide range of tasks. Therefore, a sophisticated controller design is needed to control the complex, nonlinear and nonstationary dynamic system. Adaptive controller techniques [6], [7] seem to be a promising solution to this problem. In this study, a simple integral plus state feedback controller is used to control the rigid body motion only. This simple, linear controller is used to simulate the controller used in present manipulators. The illustration of the inter-relationships between the robot structural flexibility and the controller design is discussed in this report. This should serve as a basis for the design of advanced controllers aimed at solving the arm flexibility problem. 2. MODELING The physical system is a spherical coordinate robot, which has two revolute and one prismatic joints. It consists of an arm connected to a rotating base. Dynamic Simulation 5

RSD-13 83 The arm is made of two beams such that the second beam can move axially into the first beam. The entire arm is free to rotate around the horizontal axis passing through the pivot point A and parallel to K. This is schematically illustrated in Figure 1. The robot arm is modeled by two Euler-Bernoulli beams of length L1 and L2 respectively as shown in Figure 2. A reference inertial frame (I, J, K) fixed at point 0, is chosen along with a non-inertial, body fixed, rotating reference frame ( j, j, k). The equations are derived in terms of the latter to keep the mass moment of inertia constant throughout the rigid body motion of the robot. The first beam can rotate around point 0 with angular velocity Wp around the vertical axis passing through J, and i around the horizontal axis along K. The second beam is fitted into the first one and has one additional degree of freedom r which allows it to move axially into the first beam. The load at the end-of arm is modeled with a concentrated mass at the end of the second beam. The stiffness of the beam is much higher in the i direction then in either j or k directions. Therefore, the longitudinal vibrations of the two beams are neglected and transverse vibrations only are considered in addition to the rigid body motion. Variations of the mass moment of inertia due to the flexible motion is assumed to be negligible. Expressions for the total kinetic energy and the total potential energy are derived. The generalized forces corresponding to the generalized coordinates are determined by the virtual work principle. These results are used in applying Lagrange's equations to obtain the dynamic equations of the system. Two separate cases are considered in this report to model the flexible motion in the dynamic equations: (i) Both beams of the robot arm are considered to be flexible Dynamic Simulation 6

RSD-13-83 (2) Only the potion of the second beam protruding from the first beam is considered to be flexible. For the first case, only the kinetic and potential energies are derived. The position vector of any point on the first beam is: RI = ri + Vj + Wk (1) where rl is time invariant, since flexible motion along i is neglected (i.e. no longitudinal vibrations). V and W represent the flexible motion of the beam in the j and k directions respectively. The rotation vector of the noninertial frame (j, j, k) is defined as follows: -n += + = sin + COSj + k (2) the time rate of change of R1 is given by: R= + n x R = (W(cos3 -iV)i + ( + 1 - W-,sin6)j (3) + (W + Vopsin5 - rcos)o)k The position vector of any point along the portion of the second beam located inside the first beam is R2 = x'i + Vj + Wk (4) where x' = Li + r - L2 + z and must satisfy the following condition, L1 - L2 + 7 <x' <L1 (4a) L1 is the length of the first beam, L2 is the length of the second beam, r is the length of the part of the second beam protruding from the first beam and x is the distance from the lower end of the second beam to the desired point within the range defined for x' (see figure 3). Note that ' = r, since longitudinal vibration along the second beam is neglected. This portion of the second beamr and the first beam are assumed to have the same mode shapes. Therefore, T arrd W defined in equation (4) are the same functions used for R1. The time r-., o" -har ge of Rf is: Dynamic Si: 1:-rtion 7

RSD-13-83 Rz = R + R xR = (r + WYcos - V)s + (V + l' - WYosin)j (5) + (W + V7sin6 - x'pcoso)k Similarly, the position vector of any point on the protruding part of the second beam is: R = y'i + vj + W'k (6) where y' L1 + y and y' = r = y. The time rate of change of R3 is 6Rs Bt ++ Q x R = (r + y W'coso - I)i (7) + (V' - W'sini + i ')j + ( W' + V'cPsink5 - y'ocos)A)k The assumed modes method [8] is used to obtain the expressions for the transverse vibrations V, W, V' and W'. They are written as a linear combination of admissible functions (i) which are functions of spatial coordinates, multiplied by time dependent generalized coordinates qi(t). V = iiq(t) (8) i=1 Treating the continuous beam as a two degree of freedom system, then n should be set to 2. The admissible functions are chosen to be the first two eigenfunctions of a clamped-free beam in order to satisfy the boundary conditions of the two beams. The eigen-functions are given in reference [9]. The final expression for V, W, V' and W' depend on the mode shape selected for each beam. These results are then used to develop the kinetic energy for each part of the robot-arm including the payload. The total kinetic energy will be T T + - f(Ri Ri)d m +.mp( R RP) (9) i=l i=1 7t r where m is the number of links in the robot arm, Ti is the kinetic energy of each link and Tp is the kinetic energy of the payload. The total potential energy of Dynamic Simulation 8

RSD-13-83 the robot arm consists of the strain energy of the two beams [10] plus the potential energy due to gravity. Vt= ml- L1- 2 L +m r - + p(Li + r) gsin3 (10) + MlVI Li + m'2VIL L2 r+ m 2F+I r + c "V,' 71 ^+^7+ V } ] COS^ ~2 2 L,- +2 2 =l1 '1 0 ^ 0 ) i4.fE( j ~ i=i o t 2o Where ~ is a dummy spatial coordinate used for integration. rm is the mass of the first beam, m'2 is the mass of the part of the second beam located inside the first beam, m "2 is the mass of the part of the second beam protruding from the first beam. Pi(t,t) is the tension force in the ith beam. The first term is the potential energy due to gravity. The second term is the potential energy associated with the flexible motion. The integral terms represent the strain energy of the two beams. Equations (9) and (10) are then substituted in Lagrange's equation [8] to write the unconstrained equations of motion: d Orl, Tt a+, l dtq a9qg + i=Q (11) where Qi is the generalized force associated with the generalized coordinate q (t). For the second case, a complete derivation of the equations of motion is presented. Note that this case can be obtained from the previous one by dropping out the flexibility terms associated with the first beam and the part of the second beam located inside the first beam. Since the latter undergo rigid body motion only, then the position vector of their mass center would be of Dynamic Simulation 9

RSD-13-83 great importance. For the first beam, we have R[ L1. R= i (12) The velocity term is 6R' L1 L1 - R 1 t + n x Rx - 2 T cos6k (13) 1 L 2(13) The position vector of the mass center of the part of the second beam located inside the first beam is R- =x'* (14) where x'* remains in the range defined in (4a). The time rate of change of R2 is given by & = 5gt+ n x Rb M +; me. - xcos~6k (15) ~_ 6I~ x ozosi Position and velocity terms for the part of the second beam protruding from the first beam are the same as their counterparts in the first case. They are defined in equations (6) and (7). Since the flexibility effect is restricted to a portion of the second beam only, then the prime sign (') on V and W is dropped in equations (6) and (7) for the rest of the report. Note that r cannot approach zero since it would violate one of the major assumptions set for the EulerBernouilli beam. That is r must always be much greater then the cross sectional dimensions. Following the same reasoning as before, the assumed modes method is implemented to obtain the expressions for V and W. Selecting the first two eigen-functions of a clamped-free beam [9] for the admissible functions, Vand W become: V = i2qli(t) = qll(t) + '2q12(t) i=: i2 (16) W = j q2j(t) = lq 21((t) + '2q22(t) j=l Dynamic Simulation 10

RSD-13-83 where i1 and $2 have the general forms Eiy iY [iY EiY (17).i = cosh. ~ - cos i - ai sinh ~ -- sin( ~-] (i7) The values of si and at for each mode are given in Table 1 [9]. Substitute (16) and (17) into (7) to get the complete form for ]. These results are then used to develop the kinetic energy for each link. Since the first beam undergoes a fixed point rotation, then its kinetic energy can be simply defined by: TI= 0 Ho (18) Where Ho is the angular moment of momentum around point 0. It has the following form: Ho = AQz + BQJ + CQzk (19) Assuming properties of a thin rod (i,e, dimensions of the cross section is < < then the length of the rod), gives A = 0 and B = C = Therefore, Ho becomes: m,L 1r m 1LL1 Ho = ~ cos oj + r1L (20) 3 3 Using (20) in (18), T, will be: T 1 m L 2 2 cos2 + i (21) 6 " The kinetic energy of the portion of the second beam inside the first beam has rotational and translational parts: T2= 2m '22 Rs + 2j H (22) where H* is the angular moment of momentum around the mass center located by R. It has the following form; H' = A1z + Ani + A3Qzk (23) Dynamic Simulation 1

RSD-13-83 where Ai's i=i,2,3 are the mass moment of inertia around the mass center. m'2(L2 - r)2 A1 = 0 and A2 A3 H= ' becomes: 12 H 2( -12 )2 b~cost + (24) 12 - 'J Using (24) into (22), T2 will be 72 = mf2{T + j|Li - + 2 — + 5L1 + - _ L]cos2i (25) T22- ('2) T L2 f L1 2+ (Lz -?2 o2cos2t+ + + ~O The kinetic energy of the part of the second beam protruding from the first beam is: f 2 3 = f(2 R)3)dm 2 {(r + Wcose - v)2 (26) + (V + y' - Wo sin)2 + (W + Vf sin -y'yocos5)2}d77"2 Position and velocity vectors of the payload have the same form as R3 and Rg but evaluated at a distance (L1 + r). The payload is considered to be a point mass, does not include rotation around its own mass center, then its kinetic energy will be: i=1 Dynaic Si= p(ulan 1 (7) = mpr+ Wcos~-i+ + V+ -L + r)-Wsin]2 + (W + Visini - (L + r)cosoc ), + The total kinetic energy is: 3 Tt = - T + Tp (28) Dynamic Simulation 12

RSD-13-83 The total potential energy consists of the strain energy of the two beams [10] and the potential energy associated with the rigid body motion. The strain energy terms include the effect of the bending moment and the axial force. The bending moment effect is represented as follows: r/( 2 rr4 2.2 JfEay2 2, ay2 while the axial force effect is: f T(,t) V(yt) 2dy + f T(yt)[ t) dy (30) where T(y,t) is the axial force (tension) in the second beam. It consists of two parts: 1. The axial force due to centripetal acceleration is [10]: r Ti(y,t) = f('2cos20 + i)pA2(L, + t)dc + ['2cos20 + i]mrp(Ll + r) v r2 L ty + y pAz2(2cos2,i + {) (Lr + - + ) 1- 2(L + r)[cos23 +] 2 + r) os Ljr + This term represents the effect of the rigid body motion on the flexible motion. 2. The axial force due to gravity effect: r T2(y,t) = Lp+ pA2 dAt g sint = Frnp + pA2(r - y)]gsini5 Combining the two components, we get Lly + y2 T(y,t) =pA2(2cos2 + ) (Llr + 2 - 2 (31) L mpr 2 Dynami SimulaLor + - + m( (L1 + r)[r 2cos2P +_ ] + [rrn + pA2(r - y)]g sini Dynamic Simulation 13

RSD-13-83 The axial force T(y,t) has a tendency to increase the stiffness of the second beam. The total potential energy will be: Vt = ml + m2t, + r- L2 + mp(LI + r) gsin6 (32) + M{2 +VI r ++ mp VILi+r }COS n + f EI2(y) V(y,t) dy + fEI(y) -w( dy + f 4T(y,t) ay dy + f2 T(yt) a W( ) where the first term represents the potential energy due to gravity. The second, third and fourth terms represent the potential energy associated with the flexible motion. The rest are strain energy terms associated with rigid body motion. Before we derive the virtual work of the system, we need to evaluate all internal and reaction forces. Both beams of the robot arm are connected to their driving motors by leadscrews. When all controlling forces and torques are not applied, the system will maintain its configuration due to the constraint forces exerted by the leadscrews which balance the weight of the system. Expressions for the constraint forces are obtained from the detailed analysis of the static equilibrium of the two beams. This analysis is included in APPENDIX I The virtual work principle is implemented to obtain an expression for the generalized forces. The virtual work of the first beam is (see Figure 4) 0 6 WB = (R'a + R'2jg). -mlg (sin6i + cos'j) R6R (33) -[(Fs + R1)i + R2j] 6R IL + (Tc, + T' - T,)k -X + T~c(sin'lS + cosi3j)'6X1 where R'l, R'2, F, R1, R?, T'1, and T1 are all defined in APPENDIX I. 6R1 is the virtual displacement of the displacement vector R1, T7c and T7w are the Dynamic Simulation 14

RSD-13-83 controller torque in 3 and cp directions respectively. 6X1 is the virtual rotation vector of the first beam. dX, = sin65~i + cosdioj + 6ik (34) after simplification we obtain, 6WB1 = (Tc, + azF) 6 + TcY (35) The virtual work of the second beam is, 6 WB2 = F 6R2 I (Ll+r-Lg) + (R1I + R2) R2 IL1 + Fsi 6R2 IL + TK'6X2 - m2g(sinti + cos6j) 6R2I L -m2"g(sindt + cosJj).6R3t r - npg(sini + + cos9pj) y6R(, +)) + fp(yt)V(y,t)dy (36) where Fc is the controller force, 6Rz and 6R3 are the virtual displacements of Rz and R3, 6X2 is the virtual rotation vector of the second beam. Note that in this case 6X, = 6X2. The last term in equation (36) represent the virtual work done by the transverse distributed force p (y,t). The latter arises from the tangential component of the acceleration vector. p(y t) = j[2pAa + pA.iocos6sin(L1, + 6 )d (37) = 2pA2*y + pA2z'cos6sinf(L,y + 2 ) Substituting these terms in equation (36) we get, 0.7035 2.0314 WB2 = jFc - m2g cosj7 qll1(t) + 1 q l2(t) lr + -rLFS + m2'"gsin-lo.668qli(t) + 1.43qi 2(t) + 2mpg sinm(q (t) - q 12t))] + {-0.6BBm"2g cost6 - 2mpg cos' + 1.14pAA'r2 Dynamic Simulation 15

RSD-13-83 + pA2 2cosa6sin3(O0.57r2L1 + 0.225r3)} q i(t) + {-1.43mr"2gcos5 + 2mpgcosi + O.182pA2/'*r2 + pA2~2cos6sin6(0.091r2LI - 0.02r)} q 12(t) The numerical values, that appear in these equations, are due to the integration of the product of spatial terms, cpi's and their derivatives, over the length, r, of the flexible part of the second beam. The integration is done numerically using Gaussian quadrature of high order (40 points) [11]. Therefore, the total virtual work will be: W Wt = 6wB, + 6 WB2 Fc -m2c ( 7035 (t) + 2.0314q 12)) + {Te }c] + {-0.68m 2"g cos13 - 2mp g c os + 1.14pA2*r2r (38) + pA2o2 cos-sin(0.57r2LI + 0.225r3)}qII (t) + {-1.43m2"g cos' 6 + 2pgcos3 + 0. 182pA25;r2 + pA24o2cossin(0.091r L1 - 0.02r)}q l 2(t) and the generalized forces are: Q, = F, -m "29 COS( 7035 q1(t) + q 2(t)) (38a) Q- = T - + mr "29sin5(0.68gq (t) + 1.43q12(t)) + 2Tpng sin,((q i(t) - q12(t)) Q# =Tc (3Bc) Dynamic Simulation 16

RSD-13-83 Qq1(t) = -0.68m"2gcosSi - 2npg cosi5 + 1.14pA2g'{,r2 (3Bd) + pA2zo2cosisin5(0.57r2L1 + O.225r3) Qq8 = - 1.43m"2gcost + 2mgCOS6 + 0.182pA24t'r2 (38e) + pA2g2cos6sini(O.09lrT2L - 0.02r3) Qq2, = Qqz = 0 (38f) The unconstrained equations of motion can now be derived by substituting the results obtained in equations 18-38 in Lagrange's equation which is defined earlier in equation 11. The resulting equations of motion are seven highly nonlinear, coupled, second order ordinary differential equations of motion. These equations can be greatly simplified once the role played by the leadscrews and the effect of the gross body motion on the flexible motion and vice versa are understood. The rigid body motion in r and 5 directions is completely controlled by the leadscrews. A self-locking condition is assumed [12], that is the leadscrew cannot rotate unless the control torque is applied. This means, that inertial forces (due to coriolis and centripetal acceleration), gravity and flexibility terms would not affect the rigid body motion in the r and 6 directions. Thus, rendering the behavior of the robot in these two directions to be similar to the simple system depicted in Figure 6. In other words, the leadscrews cancel the effect of flexible motion on r and 6 as well as the effect of inertial forces. Therefore, the constraint equations in the r direction could be stated as follows: 1. If the control force is not applied (i.e. Fc = 0), then r will no longer change and the constraint equation will be r = 0 (39) 2. If the control force is applied (i.e. Fc 0), then the leadscrew, which causes the translational motion of the second beam, exerts a dynamic reaction force to cancel the effect of both the rigid and flexible inertial terms. Thus, making the equation of motion for r independent of the other coordinates. It is simply Dynamic Simulation 17

RSD-13-83 written as: (m2 + 7p)r = F. (40) which is a linear, second order ordinary differential equation. Similarly, the constraint equations in the 3 direction could be written in the following way: 1. If the control torque is not applied, T,, = 0, then will no longer change. The constraint equation is: o=0 (41) 2. If the control torque is applied, TORO, then due to the leadscrew constraint which cancels the centrifugal, coriolis and flexible motion terms; the equation of motion for $ will only be dependent on r. rn lL rnM2L [L 2 + + m2L + r - + mp(L + r)2= TCd (42) 3 12 J Note that 1 depends on r in the inertia term only. The physical system does not impose any constraint on ~0. Therefore, it is expected that the elastic motion would affect the gross body motion in that direction. Moving to the second phase of the problem, which is the effect of the rigid body motion on the flexible motion, we note that the inertial forces generated from the gross body motion are serving as external driving forces into the elastic problem. This occurs twice in this study: 1. In the strain energy derivation, where inertial forces are introduced in equation (31) and are shown to increase the stiffness of the system. 2. In the virtual work principle where inertial forces are introduced in equation (37) and are shown to excite the motion in the qll(t) and q12(t) directions. The reader is referred to APPENDIX II for the listing of the constrained equations of motion. APPENDIX III shows that the constrained equations for the rigid body motion can be derived as a special case of the general equations of motion presented in APPENDIX II. This provides a Dynamic Simulation 18

RSD-13-83 partial confirmation of the validity of the modeling procedure. 3. CONTROL The dynamic model of the robot has a total of seven degrees of freedom, three for the rigid body motion ( r, $, Wo) and four for the flexible motion (qll(t), q12(t), q21(t), q22(t)). This results in a set of seven coupled, highly nonlinear, second order ordinary differential equations of motion. These equations reveal the nonlinearity and nonstationarity characteristics inherent in manipulators. The dependence of inertia characteristics of robots on their geometric configuration and the payload causes the system parameters to be time dependent. Therefore, in order for the robot to have good performance over a wide range of motions and payloads, a sophisticated controller design is needed. This design has to take into consideration the following: (1) The nonlinearity and the nonstationarity characteristics. (2) The flexible motion which is included in the equations of motion. Adaptive control method seems to be a promising technique to handle the first feature [3]. Since, its main task is to adjust the feedback gains of the manipulator so that its closed loop performance characteristics would closely match the desire ones. The second feature is very intricate. It involves the controllability and observability problems that arise due to the distributed nature of the beam. The protruding part of the second beam is assumed to have continuously distributed mass and elasticity. An infinite number of degrees of freedom are necessary to specify the position of every particle on the beam. Consequently, the beam possess an infinite number of elastic modes. Since, our interest is up to the second elastic mode only, then the sensors should be sensitive to the first two modes and insensitive to the remaining neglected ones. This is known as the observation spillover problem. Dynamic Simulation 19

RSD-13-63 A certain mode can be made unobservable to a sensing element if this latter is located at the node of this particular mode. Therefore, the locations of the sensing elements should be carefully chosen. Prior knowledge of the shape of the modes of the system will be of great help in the selection of the best locations for sensor elements to reduce the observation spillover [13] from the unwanted modes. However, this technique cannot be used to address the controllability problem since in our system, the forcing elements are restricted to the joints. Unfortunately, relocation of the sensing element can improve the observability problem but does not solve it completely. A search for special sensing systems which are able to filter out the unwanted signal or, in other words, could be insensitive for some modes showed that the comb filter is a good candidate for such tasks [13]. Should the flexible motion be included in the control action, then accurate measurements of the actual displacement and velocity of the flexible part are needed to be fed back into the controller. This requires an additional sensor for end-of-arm motion (i.e., accelerometer, strain gauge, etc.) since the flexible motion is not observable from the joint sensors alone. In the case where the complete state vector cannot be evaluated by measuring devices, then the state observer method [14] may be applied. Experimental work is needed to determine the shape of the first few modes. These experiments will be conducted on a stationary robot. The equipment to be used in the testing are a spectrum analyzer in conjunction with a HewlettPackard 85 micro-computer. A hammer, with an accelerometer attached to it, is used as the excitation source and the vibration is measured with an accelerometer mounted on the arm. Dynamic Simulation 20

RSD-13-83 In light of the problems that need to be tackled by the controller to maintain good performance of the manipulator over a wide range of motion and payload, a brief description of the robot controllers presently in use will be given. These are designed to control the rigid body motion only. In most controller designs, the nonlinear terms in the equations of motion have been neglected. This design practice is valid as long as the robot is restricted to slow motion. However, when manipulators are operated at high speed, the neglected nonlinear terms become significant and the omission of the flexible motion in the control action can lead to undesirable vibrations. For accurate performance of the robot, a great portion of the production time has to be wasted waiting for these vibrations to damp out. Therefore, in general, a linear control system is expected to perform poorly over a wide range of tasks. In an attempt to show the deficiency of the controller existing in current robots, a simple linear control system is designed to control the general motion described by the set of equations listed in APPENDIX II. To relax the controller requirements, the operating ranges for r, A5, and;C will be restricted to the vicinity of an equilibrium point. The payload is kept constant throughout this study. In an attempt to reduce link deformations [2], a common practice is to choose the ratio of the weight of the payload to that of the robot arm to be 10:1. Under these conditions, it would be possible to design a linear controller for this model. Since, the flexible motion is not intended to be included in the control action, then the linear controller should be designed based on the equations listed in APPENDIX III (i.e. equations of motion which describe the rigid body motion only). This linear controller would then be applied on the general equations of motion. The first step in building the controller is to write the rigid body equations of motion in terms of state variables which are defined as follows: Dynamic Simulation 21

RSD-13-83 yw = r Y3 = C Y5 = ( (43) Y2= Y4 T 6 = i The nonlinear state equations whose general form is y=f (u,ut) (44) are linearized around the equilibrium point ( 1, 0, 0). The resulting equation can be written as: =A+ Bu (45) where A6x6 is the plant matrix, U3x1 is the control vector. In general, equation (45) does not lead to the desired poles. Hence, a state feedback controller technique [15] is implemented to replace the system poles by the desired ones (see Figure 7). The latter are a compromise between the desired transient response and the physical limitations of the system. The detailed derivation of the state feedback gain matrix Ks is included in APPENDIX IV. Only its final form is presented in this section: (m + mp) W. 0 0 2(mz + mp)Wnr 0 0 Ks 0 ~a w 0 0 2sa WnW 0 (46) 0 0 a W 2 0 0 2ta Wn An integral action is added to the state feedback controller to eliminate the steady state error or any disturbances in the system. This is illustrated in Figure 8. The integral gains, Ki, are determined from the root locus diagrams for r, t and so respectively. The chosen values of the Ki's were found to lie in the vicinity of the breakaway point. When the rigid body controller is applied to the general equations of motion, listed in APPENDIX II, the following features are anticipated: (1) Since r and d equations of motion don't include any flexibility terms and are independent of the;o coordinate, then their motions are expected to closely follow the desired ones. Dynamic Simulation 22

RSD-13-83 (2) Equation of motion for o contains flexibility terms which act as a disturbance source to the rigid body motion in the o direction. However, the integral action in the rigid body controller will enable the system to overcome the disturbances and follow the desired response with zero steady state error. (3) The rigid body motion would excite the flexible motion. Instability might occur if at least one of the servo loop frequencies approaches the natural frequencies of the flexible part. 4. SIMULATION The simulation of the robot arm is very important tool in studying the system's behavior. Written in a form of a computer program, it gives the opportunity to conduct tests that were considered to be hazardous or physically unrealizable. These tests are of great importance especially when dealing with systems that are at their early stages of development. The mathematical description of the model along with the equations obtained from the controller lead to a set of complex, coupled, highly nonlinear equations. No closed form solutions exist for these equations. Instead, they are solved numerically on a digital computer. This set of equations involves both very rapidly and very slowly changing terms, all of a decaying nature. Systems possessing this feature are classified as stiff systems. In this work, Gear's method is implemented in the solution of these equations [i6]. It is a numerical method well suited for stiff systems. A FORTRAN subroutine package, based on a program written by C. W. Gear, is available through the Michigan Terminal System under the code N-AAS:NAL. It has the ability of solving the initial value problem for systems of ordinary differential equations. Such a system has to have the following form: Dynamic Simulation 23

RSD-13-83 =f(y,t) where y, y and f are vectors of length N > i. y is the first derivative of y with respect to time. t denotes the time variable. Hence, our system of equations has to be reduced to first order form. Thus, leading to seventeen first order ordinary differential equations (three equations are obtained from the controller) which form the bulk of the DIFFUN subroutine. The latter is called internally by the DGEAR subroutine. Flowcharts of the main computer program and the DIFFUN subroutine are included in this section. The reader is referred to APPENDIX V for the listing of the computer program. Dynamic Simulation 24

Numerical Integration RSD-1383 define the sgn function set common block and arrays dimension print heading for y(t) to y (6) and time ready(1) to y(17), time T counter I, number of equations N time increment H, error tolerance EPS, index Gear subroutine interr compute y() through - ly calls diffun subrouti c h^^^^I — ~- 4ru which in its turn calls y(17) by Gear method the sgn function 'I$_^^. 6 o~ 1 __ __test for the sing,.'.arit of the inertia matrix NO PRINT S PRINT TIME y-(1) through y(6) and store the values for y(1) to y(17) Dynamic Simulation 25

RSD-13-83 YES check if ~'~!^ ~index = O n intergration was successful increment the time PRINT ERROR RETURN II o YES I~<ETOUT < <E NO PRINT y (7) to y(17) and their headings PRINT no. of steps and the number of F & J evaluations DynamicSimulation 2 PLOT each variable y versus time, ISTOP ) Dynamic Simulation 26

RSD-13-83 SUBROUTINE DIFFUN read all variables needed in defining the equations of motion initialize the controller commands and set the desired position define the inertia matrix A define the B matrix - _ include all inertial terms define controller equations define the vector F controller torques and force defin the I |co gravity and inertial define the D vector - 1 forces effect computation of the right hand _ combines B, F, side of the equation, PALL and D matrices decompose the inertia matrix DLUD Dynamic Simulation 27

RSD-13-83 YES check the -<IV(M) = 0 + - singularity of the Xi ^^^^^I |inertia matrix NO solve y = A-' PALL define Y DOT IN is called define Y DOT IN __ sgn function TERMS OF PALL called __ __~_ _occasionally I~.ilreturn D[ nal,i Si nultion 28

RSD-13-83 5. DISCUSSION The computer program written to simulate the dynamic model of the robot arm is now used to investigate the inter-relationships between the robot structural flexibility and the controller design. These include the effect of constraints due to transmission mechanisms, resonance, forced excitation due to inertial forces, instability mechanisms and parametric excitation. The standard set of physical system parameters used in the computer program are: mass of the first beam (m l) = 0.465 Kg mass of the second beam (m,) = 0.9366 Kg mass of the payload (mp) = 0.07 Kg cross sectional area of the second beam (A2) = 0.000173 m2 length of the first beam (L1) = 0.361 m length of the second beam (Lz) = 2 m gravitational acceleration (g) = 9.81 rm/sec2 aluminum density (p) = 2707 Kg/rn m flexural rigidity (El) = 1121.9 Pa reference position for r is Im reference position for t3 is 0 rad reference position for.i is 0 rad desired reference position for r is 2m desired reference position for e is 0.5 rad desired reference position of W is 0.5 rad integral action gains are: forr: K{1 =8.8 for: Kz2 = 5.8 Dynamic Simulation 29

RSD-13-83 for o: K3= 44.2 The servo natural frequencies are for r: Wnr = 4 rad/sec for d': W. = 4 rad/sec for sp: Wn, = 8 rad/sec In an attempt to make the integration of the equations of motion less costly, the difference between the servo loop and the flexible motion frequencies is reduced. This is done by giving the second beam a length Lz = 2m which is six times greater then the actual length. This has resulted in a decrease in the stiffness of the second beam and helped to exaggerate its vibratory motion. Given these values, the equations of motion listed in appendices II and III are integrated separately. The results for the rigid boy motion only are shown in Figures 9-11 while the results obtained for the rigid and flexible motion are shown in Figures 12-18. Comparison of the results obtained from these two runs, show that there is no obvious difference between the results for r, ', o for the two cases. This can be explained as follows: (1) The plots for r and d6 are expected to be the same due to the leadscrew constraints which made them insensitive to gravity, inertial forces and flexible motion effects. (2) Since there is no constraint in the go direction, the plots in the two runs corresponding to the o motion are expected to be slightly different. This difference is mainly expected to be from the flexible motion, since in both cases, p is subjected to inertial forces and is not affected by gravity. Numerical values obtained from the computer runs show that the difference is roughly in the order of 10-6m. The small effect of the flexible motion on the rigid body motion explains the reason for getting similar - plots. Dynamic Simulation 30

RSD-13-83 Figure 15 represents the motion of the flexible coordinate q 7(t). Its transient response shows the whip effect that the rigid body motion has on the flexible motion. Note that in modeling the robot arm, the gravity effect on the flexible motion is included, and the general equations of motion represents a conservative system since no damping is considered in the derivation. Therefore, it is expected that the steady state response would be a sustained oscillation around a negative value. The latter represents the static deflection caused by gravity. The motion of q12(t) is illustrated in Figure 16. This plot has two additional features: (1) The magnitude of the vibratory motion is on the order of 10-3 while the one for qll(t) was on the order of 10-1. This is due to the larger amount of energy required to excite the higher modes. (2) After 4 seconds, the natural frequency of the second flexible mode is decreased due to the vanishing effect of inertial forces which tend to increase the stiffness of the flexible part. Figures 17 and 18, which illustrate the flexible motion of q21(t) and q22(t), represent a very important aspect of this particular robot design. Their transient responses show the whip effect, due to the rigid body motion, and then die out with time. These interesting results cctn De interpreted as follows: (1) Gravity doesn't affect the flexible motion in the q 2(t) and 2 t ) directions. Therefore, their motions are expected to either die out to zero with time or oscillate around zero (i.e. no static deflection). (2) Since there is no constraint on the rigid body motion in the o direction, then the flexible part would not be able to conserve the strain energy in the q2i(t) and q22(t) directions. The motion of the latter would act as a disturbance source on the rigid body motion in the p direction. These Dynamic Simulation 31

RSD-13-83 disturbances are compensated for in the rigid body controller. To better explain the second feature, a model, which simulates the rigid body motion in the o direction as well as the flexible motions of q2z(t) and q22(t) is shown in Figure 19. x (t) and x2(t) represent the rigid body motion and the flexible motion coordinates respectively. Note that only one flexible mode is considered in this model. F1(t) is the rigid body control force. F (t) is the flexible motion control force. However, in our system, the controller is designed for the rigid body motion only, then F2(t) will remain zero at all times. Only the resulting equations of motion are listed here. X1 + W21(1 - 2)= m 371,1 X2 + Wn2 (2 - l) =-, In an attempt to prove that the flexible motion cannot store the strain energy, an initial condition is given to the flexible motion coordinate, x2(t), while the rigid body coordinate, xl(t), is set to zero. The results of this simulation (see Figures 20-21) prove that the flexible motion, x2(t), could be driven to zero while exciting the rigid body motion, xl(t). The rigid body controller is then reactivated to bring x(t) to its initial position. Additional runs are made with modifications to the standard set of parameters to study the behavior of the system in the following areas: 1. Effect of gravity 2. Instability mechanisms 3. Speed - accuracy trade off. The first study is done by simply eliminating the effect of gravity. Set g = 0 in the base program. Since gravity affects the flexible motion in the ll(t) and qi1(t) directions only, then similar results are obtained to those as in the base run for the rigid body motion (i.e. plots for r, t and r) and the flexible motion in Dynamic Simulation 32

RSD-13-83 the q2(t) and q22(t) directions. The steady state response of the q i(t) and ql2(t) oscillate around zero instead of the static deformation position. This is illustrated in Figures 22-23. Instability can occur if one of the servo loop frequencies coincides with the flexible motion frequencies. There are two ways in which this can occur: (1) The servo loop frequency is increased to match the flexible motion frequency (i.e. a sharp increase in the speed of the rigid body motion). (2) The flexible motion frequency is decreased to approach the servo loop frequency (i.e. making the system more flexible). The first case should be ruled out since increasing the speed of the rigid body motion would drastically increase the effect of the inertial forces. The latter have a tendency to increase the stiffness of the system. Consequently, the flexible motion frequencies would become hard to predict. Hence, the second way is adopted in this study. In the base program, El is reduced to 17.04 Pa to make the system more flexible and g = 0 simply to reduce the cost of this run. The results are shown in Figures 24-28. The plots for r and 6 are the same as in the base run results (see Figures 12-13) since they are insensitive to either EI or g. Comparing Figures 24-28 with their counterparts in the base run (Figures 14, 17, 18, 22 and 23), we note the following: (i) Wo becomes more oscillatory (compare Figure 24 with Figure 14). Thus, for a "soft" system, the flexible motion has a very significant effect on the rigid body motion. (2) Figure 25 reflects the reduction in the flexible motion frequency and shows the increase in the magnitude of the oscillatory motion of q l(t). (3) Figure 26 shows the instability in the system. Dynamic Simulation 33

RSD-13-83 (4) The response of q21(t) and 722(t) are more oscillatory (compare Figures 27-28 with Figures 17-18). The speed accuracy trade off case is done by raising the servo loop natural frequencies for r, 7 and A, in the base run, from 4, 4 and 8 rad/sec to 20, 20 and 40 rad/sec. The integral gains have to be changed accordingly to KI{ = 1192, =K2 = 692 and Ki3 = 5537. Similarly, g has been set to zero in an attempt to reduce the cost of this run. Comparing the results obtained (see Figures 29-35) with those of the base run with g = 0 (see Figures 17,18,22, and 23), a general comment can be drawn. The overall oscillatory flexible motion has increased in magnitude. This increase is in the order of 10-2 to 10-' for 11l(t) and qg2(tj. Therefore, leading to the conclusion that high-speed operation would deteriorate the accuracy of the system. 6. SUMMARY The purpose of this study is to investigate the inter-relationships between the robot structural flexibility and the controller design. These relationships will be used as the basis for designing and evaluating controllers for the flexible as well as rigid body motion of the robot arm. A spherical coordinate laboratory robot is used as the focus for this study. The dynamic model for the robot includes the flexibility effects of the flexible link. The assumed modes method is used to approximate the dynamics of the infinite dimensional link. This approximation incorporates the first two flexible modes only. The latter is considered to adequately describe the flexible motion, since higher modes are unlikely to be excited. The generalized forces are obtained froni the virtual work principle, and Lagrange's equaciot is implemented co derive the equations of motion. An integral plus state feedback controller is used to control the rigid body motion only. This simole contrl-1!design is adopted to simulate the robot controllers currently in u e L'h-? Dynaimic Simulation 34

RSD-13-83 dynamic model and the controller design are used as the basis for the simulation studies. The latter demonstrates the potential mechanisms by which structural dynamics and controller design can interact. The effects of constraints, due to transmission mechanisms such as lead screws, have given the robot arm a cantilever beam-like behavior in the r and I directions. This leads to the conservation of the strain energy which is manifested by the sustained vibratory motion of qll(t) and q12(t). The absence of the constraint in the so direction caused the strain energy to dissipate since it acts as a disturbance on the rigid body motion. Consequently, the rigid body controller was able to damp out the vibration in the q21(t) and q22(t) directions. The possibility of resonance, parametric excitation, and forced excitation due to inertial forces are covered in this report. The simple controller design employed here, can be improved significantly by including the effect of the flexible motion in the control action. This can be done in three different ways: (1) Design a controller based on a detailed dynamic model of the manipulator which includes the effect of flexibility. This approach has many drawbacks. The dynamic model would be very complex even for low order beam models. Any discrepancy in the dynamic model or disturbances in the system would considerably affect the overall performance of the robot, since no actual measurements are being fed back to the controller. In other words, the robot would behave as an open loop system. (2) A full kinematic state measurement at the end point of the robot arm is fed back to the controller. The latter activates the actuators at the joints to correct for the malpositioning of the end effector. This approach compensates for the inaccuracy of the dynamic model as well as for any disturbances in the system. It does not require any additional actuators. Dynamic Simulation 35

RSD-13-83 The major drawback of this method is the difficulty of getting accurate measurements of the position and orientation of the end effector over a large operation volume. (3) Acknowledging the fact that large deflections in manipulators are due to bending, one approach calls for sensing and correcting the bending of each link separately by using a "straightness servo" [17]. This technique requires as many additional hydraulic actuators as links. Its main advantage is that each link is controlled by a simple independent control loop. This approach, in effect, requires both additional sensors and actuators to compensate for the flexible motion. Dynamic Simulation 36

RSD-13-83 7. REFERENCES [1] Asada, H., Kanade, T., Takeyama, I., "Control of a Direct Drive Arm," in Robotics Research and Advanced Applications, edited by Wayne J. Book, ASME Booklet, Nov. 1982, pp 63-72. [2] Thompson, B.S., Sung, C.K., "A Variational Formulation For The Dynamic Viscoelastic Finite Element Analysis Of Robotic Manipulators Constructed From Composite Materials," ASME Design and Production Engineering Conf. Dearborn, MI, Sept. 1983. F3] Sunada, W.H., Dubowsky, S., "On The Dynamic Analysis And Behavior Of Industrial Robotic Manipulators With Elastic Members," ASME Paper No. 82 -Det-45. [4] Shabana, A., Wehage, R.A., "Variable Degree Of Freedom Component Mode Analysis Of Inertia Variant Flexible Mechanical Systems," ASME Paper No. 82-Det-93. [5] Book, W.J., Maizza-Neto, O., Whitney, D.E., "Feedback Control Of Two Beams, Two Joint Systems With Distributed Flexibility," ASME Journal of Dynamic Systems, Measurements and Control, Vol. 97, No. 4, Dec. 1975, pp 424-431. [6] Dubowsky, S., Desforges, D.T., "The Application Of Model-Referenced Adaptive Control To Robotic Manipulators," ASME Journal of Dynamic Systems, Measurement and Control, Vol. 101, No. 3, Sept. 1979, pp 193-200. [7] Horowitz, R., Tomizuka, M., "An Adaptive Control Scheme For Mechanical Manipulators-Compensation Of Nonlinearity And Decoupling Control," ASME Paper No. 80-WA/DSC-6. [8] Meirovitch, Leonard, Elem.ents Of.ibration Analysis, McGraw Hill, New York, 1975. Dynamic Simulation 37

RSD-13-83 [9] Young, D., "Vibration Of Rectangular Plates By The Ritz Method," Journal of Applied Mechanics, Vol. 72, Dec. 1950, pp 448-453. [10] Meirovitch, L., Analytical Methods In Vibrations, McGraw Hill, New York, 1967, pp 440-445. [l1] Desai, C., Abel, J., Introduction To The Finite Element Method, A NVumerical Method For Engineering Analysis, Van Nostrand Reinhold, 1972. [12] Joseph E. Shigley, Mechanical Engineering Design, McGraw Hill, New York, 1977. [13] Balas, M., "Feedback Control Of Flexible Systems," IEEE Trans. on Automatic Control, Vol. AC-23, No. 4, Aug. 1978. [14] Landau, Adaptive Control Systems, Marcel Dekker, 1979. [15] D'Azzo, and Houpis, Linear Control System Analysis And Design, McGrawHill, New York, 981. [16] Gear, C.W., "Ordinary Differential Equation System Solver," Univ. of Michigan Computing Center, Dec. 1974. [17] Zalucky, A., Hardt, D.E., "Active Control Of Robot Structure Deflections," ASME Robotics Research and Advanced Applications, Nov. 1982, pp 83-100. [8] Takahashi, Y., Rabins, M., Auslander D., Control And Dynamic Systems, Addison-Wesley, Nov. 1972. Dynamic Simulation 38

RSD-13-83 ACKNOWLEDGEMENTS The authors are pleased to acknowledge the financial support of the Center for Robotics and Integrated Manufacturing at The University of Michigan, and the United States Air Force under AFOSR contract number F49620-82-C-0089. They are also grateful to NIary Ann Pruder for assistance with the preparation of this manuscript. Dynamic Simulation 39

RSD-13-83 8. APPENDIX I - Static Equilibrium and Reaction Forces Free body diagrams of the two beams are shown in Figures 4 and 5. The constraint torque exerted by the first leadscrew to prevent the rotation of the first beam around the pivot point 0 is denoted by T'1. The constraint forces and torque exerted by the second leadscrew on the second beam are R1, R2 and T1. F, is the friction force at the point of contact of the two beams. R'1 and R'2 are the reaction forces at the pivot point 0. The constraint variables and the reaction forces are computed from the conditions of static equilibrium. Only the results are listed in this appendix: R1 = (z2 + mp)g sinq - Fs R2 = (m2 + mp )gcos1 T1 =7rLp + m27 - 2 gcos3 - aFs R'1 =(mi + m2 + m ip)gsini R'2 = (mi + m2 + mp)gcos3 T' = m2[L + r + mp(LI + r) + m I, gcosl Dynamic Simulation 40

RSD-13-63 9. APPENDIX II - Equations of Motion Only, the resulting seven highly nonlinear, coupled, second order ordinary differential equations of motion are listed in this appendix. The equation of motion for r is, (m2 + mp)r =F for F, 0 r =0 for F,=0 The equation of motion for $ is, Mj L 12 mzL 22 Lz 1~i, TLd for 272 02 {( L - + mp I( - +r)2 + r2i + r 2-Jj Tc for TC 0 t=0 for Tc,=0 The equation of motion for o is, mL~ Vcos20 -So 2cos3sin) - pAg2r cos2l- 2 + 2pA incos(L2 -L + + c 2 2 + - pA 2 - + -sipn2cos - (L2r) 3- 2 -i 2in — rcos2(L2 - r)2 + _pcos23(L2 - r)3 L- + in ocosr (L2 - r)3 + pA2(r2cos2 + o- rcos - 2sicossin]j(qi + g2 ) + 2r2cos25(2gzii21 + 22qC2) + rrcost + r2cos$ -rrsin](0.78q2i + 0.43q22) + rrcos2(0.76q21 + 043qz2) - [rcos +-;rcosi - 'rsinj](glliq2i+ q12q22) - r2os( ii121 + 7q721 + 4 2722 +?222222 -[r+sini + rrrsino ( + rr*cos$](0.5qilq21 + 0.65q 1q22 - 0.65q12q21 + 0.5q 2q22) - rrsin[0.5(qiiq2i + 4 1121) + 0-65711q22 + 711q22) - 0.65(ql2q2l + ql2721) + 0.5(712q22 + -12722) Dynamic Simulation 41

RSD-13-83 - rsini6 + ricos3)[qg L1q2 + 12q22] - T sin'[ ll + q 121 + Q127q22 + q1222] - L-Lirsinr + trsind + ri0cos.](0.7q21q + 0.43q22) - PrLIsin5(0.7Bq21 + 0.43q22) - (r2sini5 + 2rrtsin6 +- r2cosr)[0.577 21 + 0.09q22] -r2 sin1(0.57q21 + 0.09q22) + [~rsin23 + orsin25 + 2-9rsind3cosO](qg 2 + 4q22)+ rsin2((2q2lq2l + 2q22q22) + [r2sin-z + rrsin5 + rrios3](0.5q21q11 + 0.65q2112 - 0.65q22q11 + 0.5q22q712) + rrsin.[0.5(q2lq7 + 217 11) + 0.65(q211q 2 + q2112) - 0.65(q22q11 + q22q,1) + 0.5(722712 + q22712)] + rsinm(gq2iq I + 721711 + q22712 + 722712) + [rsin3 + r3~os3](qg217l + 22q712)+ L9siini5(0.78rq2l + 0.43rq22 + 0.78rq21 + 0.43rq22) - Licos|0o. 78(r21l + rq2l) + 0.43(rq22 + r722)+ 0.78(rq21 + rq21) + 0.43(rq22 + TgZ2) - [rcos9 + rr os - rrsini] (0.35q21 - 0.2522)- rrcos5(0.35q21 - 0. 25qs22) - [2rrcos6 - r2isin ](0.57q21 + 0.09q22) - rcos 5(0.57q21 + 0.09q22) + (prsin2d + prsin2.5 + 2; sin7cos5)(q1i + q72)+ 2rsin2(qllqll + q12q12) -2L sincosr + cos - sn + rrcos - (*sin sinicos ](0.786ql + 0.43q 12) - 2-rTL sini'cos't(0.78q 11 + 0.43q12) - 2 [rpZsincos. t + 2rr csinncosi + -rI Tcosr -'Pr in ](0.57q l + 0.091q2) - 2r2sini5 cost6(0.57 11 + 0.09q12) + (L r + 2L rr + rr); cos2i 4+. 2r + L r2 + - (pcosi - 2-sinicos)}+ mp4[ cos2 - 2ocossin|ql -i+ 2 -2q 217 22) + 4ocos23(22q 212 + 2q22q22 - 2721722 - 2721722) + 2[rcos'i - r -sin](q2i 2- 22) + 2rcost(s i2 - 722) - 4[cos - -isin5]( 11q 2 - 7 11722 - 712q21 + q 127 22) - 4to0S3(11q21 + q71121 - 71122 - ql1122 -712721 - q71221 + 712722 + q712722) - 4 ios,(q 1121 - 711722 - 712 21 + 12722) - 4sin(q 11g721 711721 - 7 1122 - 711722 - 712721 - 712721 + 4 12722 + 7 12722) - 2, sini3(Li + r) + 4r sini5 + 4cos3(L i + r)](q21 -7 q22) - 2tin (L 4- + r)[ q2 - 7 22] + 4(Cosin2d + 2coi1in6cos7)[gq2l + g222 - 27217221 + a2sin2 G(qz212 + 42 7222 - 721722 - 721722) + 4'90os3q21711 - 721712 - 7227 11 + 722712) Dynamic Simulation 42

RSD-13-83 + 4sin l(q2g q1 + q2111- 2112- 12- q22 - q22q11 + q 22q 12 + 22q 12) -2[rcosi - (, + r)iinF](q q2) - 2(L + r)cosO(g9 - q22) + 4(Dosin2 2+ os sin7)( + q 2 - 24lq12)+ 86sin2(q lql + q - 12 - 1112) - 4[csinicos3(L1, + r)+ o;cos2(l1 + r) - sin2(3(L1 + r) +;rsindcos3](gli - q12) - 4osini6cos(t(L + r)(il - q 1) 4+ y(L1 + r)2cos 2 + 2(L, + r)+rcos2 - 2,iL4 + r)2cossintI = T Dynamic Simulation 43

RSD-13-83 The equation of motion for q 1(t) is, P2 (r + rr)(qll- 1.3q12) + rr(q1 - 1.3q12) + 2rq11 + 2rq11 + 1.56Li(ir + 1) + 1.4(ir2 + 2Thrr)- 2(prsinq21 + -3I- cos-q21 + crsin q2 1 + crsin6q2l) - 2irqgI + 1.56rr+ 2Sorcosgq21- 1.52r2q. - 1.062q12 -rr - 1.3rrq2 - 1 56r9L - 0.7r + 2.6eyrrsin gq22- 2sorsinq21 - 22rsin2q + 1.62,sin cos + 1562 1.14o2r 2sin6cosj} + -8( ll - q12) + 4 (L1 + r) + 4+ o - 2] - 8Bosingi(q21 - q2)- 8(q - q1 r) + 8 osi(q21 - q22) - 8(g21 - q22)osin 12 362 i7 i ( 11 - 6 1sin(i- 2)+ 4(sinicos6(Li + r)) = - 0.6Bm "g cos - 2mrpg cosi -.3 Elq(t) -pA( cos2 + i)(LIr + r ){4.65 q 7.38 2 + pA2z(2cos23 + i) [(3.06L1 + i.13r)gql - (6.96L1 + 3r)q12] '2COS2,6 + + IM 4,65 7.38 - [mp(L, + r)(po2cos2 + 4) + ( + pA2r)gsind][ 465 - 7- 12] T I + pA2g sinn[3.08q11 - 6.96q 12]+ 1.14r2pAgr + pA2o2cos6sind40.57r2Li + 0.225r3} Dynamic Simulation 44

RSD-13-83 The equation of motion for q 12(t) is, pA2 (r2 rr) + + rr(0.65q1i +0.5q12) + rrq2 + rq12+ 0.43L1(Qr + 4r) + 0.0 9(2'rr + \r2)- (r sin.iq 22 + 3Orcosiq 22 + orsin q22 + pr sinq 22)- irq 12 + 0.43rr 9 + ro~cosiq22 0.53r2q1 - 4.34r2q + 0.65rrq1 - 0.5rrq2 - 0.43r3L + 0.25rr5 - 1.3orrsin qgzl - orsinq 22- - r 2siniq 12 + 0.43r 2sinicos/Li + 0.09r 2osin cosj} + mp{4(12 - qll)- 2[(i + r) + r] - 4(ysin- + oposi)(q22 - 2zi) - 4osin5(q 22 - q21) - i(q -q 1 1) - 2rI + 4 039cos (q22 - q 21) - 4rsinV(q2 - 721)- 4o2sin2(q2 - q11) - 2 sinfcos((Li + r)} = - 1.43m"g cosi + 2mpg cos~- 48559Iq -pA2(o2cos2 + + ~)(LIr + -)[ - 7.38 + 324,12] + pA2(~o2cos26 + -)[ -(6.96L1 + 3r)q11 + (23.77L1 + 9.73r)ql2] - [ (Li + r)(~2cosZ O + - ) + (7mp + pA2r)gsin]- qll + q 12 { } + pAg sin.[ - 6.96q 1 + 23.77q12] + O.;82r pA2z + pA122cos$sinm[0. 091L ir2 - 0.02r3] Dynamic Simulation 45

RSD-13-63 The equation of motion for q2l(t) is, pA2 0.5(r22i+ rr + rr rr l) - 0.65(r q22 + rrq2 + rrq22) + rq2 + rq + rsinq + rcos + singq tl + cos + rsini + orsininq t - 0.78[r. oLjcost4 + roLcosi. - r L sini]- 0.57[2rr' c cosfi + r 2Ocosi - r2 popini] - o2rcos2iq2 l - 0. 78rr cos5 + r ocos6q l +;osin$(0.5rrq 11 - 0.65rrql2 + rqll) + 0.786r iLlsosin6 + 0.57r2osin'3 - rSasin2!q2l - 0.76r2q2l - 0.53r2q22 - 0.5rrql - 0.65rrq22 - osinr5(0.5rrql + 0.65rrql2) + 0.78rL~p~cosI + 0.35rr'ocosl}4 + m.p{4(q2l - q22) + 4(;osint6 + (*cosi5)[qji - q ]2 + 4-psin((ql - 12) -2[r cosoi + (L1 + r),ocos6 - (L1 + r)i9sin1]- 4c2Coso2(q2 22) - - 2rocosi + 4~2iosi~(qg - g12) + 4sosini3(qi - q12) + 2Zi3Li + r)posin, - 4cp2sin62(q2l - q22)} = -12 362 (-pA(cos2 + )(L1r + )[ 5 -.38 7t) - pA7*pCa + 7*)(L] r 3 EIq 21(t)O 21 2' T Q122] ra - ~ r r + pAz(2acos2, + i)[(3.08L1 + i.13r)q21 -(6.96L1 + 3r)q2] _[mp(LI + r)(~2cos 2 +.).+ (mp + pA2r)gsini3] { 4 2 -.3 22 + pA2gsin(3 0821 - 696q22) Dynam 2ic Simulation 46.96qa2 Dynamic Simulation 46

RSD-13-83 The equation of motion for q22(t) is, pA2{0.65(r q2i + rrq2, + rrq21) + 0.5(rrq22 + r2q2 rrq22) + rq22 + rq22 + or siniq 12 +;pfrcosigq12 + rT sin iq 12 + or sin6q 12 - 0.43L,(r cos O + rcos9 - r oini3) - 0.09(2rrcos5 + r2ocosz - r2.osini) -rT92Cos21q22 - 0.43rrscosi5 + r;Ilcosiql2 + 0.43riL iosini3 + 0.09r 2*sint - r o2sin2iq22 - 0.53r2q21 - 4.34r2q22 + 0.65rrq2 - 0.5rrq22 + Csinf(1.3rrg 11 + rq 12) + 0.43rLiocos3 - 0.25rrpcosj + mp 4( q22 - q2i) + 4(;csini5 + So*os )( qs -g q) + 4Spsin5(q2 - ql) + 2[rocos'i + (L1 + r)ocos3 - (L1 + r)poisin5] - 4O2cos2d(qz2-q21) + 2rocos + 4+ 4 os ) + sin (q lz - ql) - 2i9L1 + r)osin5 - 4-2sin2-(q22g - q2i)} = r { 7.38 32.42 485.519 Iq22(t) -pA2(cos2 + 4)(Lr + 73) - 7 + 3 42 r- 2 r 1 + pAz(p2cos%2 + &) [ - (6.96L1 + 3r)qZl + (23.77L1 + 9.73r)q22] -[m (Li + r)(o2cos2. + i)+ ( + pA)sn]{ 38 32.422 r r J + pA2g sini[-6.96q 2 + 23.77q22] Dynamic Simulation 47

RSD-13-83 T, = - Ho (3-8) where Q is the rotation vector of the (i, j, k) basis. H0 is the angular moment of momentum around point 0. It can be written in the following form: H = Ani + BQyj + CQzk (3-9) Assuming properties of a thin rod (dimensions of cross section is < < then the length of the rod ), gives A = 0 and B = C = Substituting the values of 3 A,B and C in (3-9) we get: mrtL2. lL2 Ho = cosOSpj + - k (3-10) 3 3 Then, the Kinetic energy of the first beam will be: T mL 6 L 4 2 cos2+ + (3-11) The Kinetic energy of the second beam has rotational and translational parts: Tz = mr2* 2 H (3-12) where H' is the angular moment of momentum around the mass center of the second beam, and has the following form: H' = A1QxZ + Az2yj + A3Qk (3-13) where Ai's i = 1,2,3 are the mass moment of inertia around the mass center rr,2Lz Al = O, A2 = A3 = Substituting (3-13) and (3-6) into (3-12) we obtain 12 V2-~i 2 M- 2L, - cos2 2\2r-2'] T2= 2 {r + L2 + -- CrTP= _ = r + (L1 + )i + (U1 + r)2-2 cos3 Dynamic Simulation 49 Dynam "ic Simulation 49

RSD-13-83 The total Kinetic energy is, Tt = T1 + T2 + Tp T,= + CL cos c5 + + + + + r os + r - 6 k 2{ 2 2 J 6 22 + 2 4 cos + f+ 2r 2 + (L1 + r)2 + (L1 + r)2 2cos2 (3-16) 24 2 POTENTIAL ENERGY The datum line is chosen to be aligned with the I unit vector. It is horizontal and passes through point 0. Then the potential energy is: YV n = m 2g sin,4 + Lrrgl + r - 2 sint + m pg(L1 + r)sin6 (3-17) V - 2sin% + rag L1 + r -- 2m which is the summation of the potential energy due to gravitational acceleration of the two beams and the payload. VIRTUAL WORK PRINCIPLE: The virtual work principle is implemented to obtain an expression for the generalized forces. The virtual work of the first beam is W3B = (R' 4 + R'21] ~ yf -mgi(sin5i + cosij) 6r; -Fs r, 1 -(R I t + RsJ) 6r IL1 + (T', + T,) K 6X1 (3-18) - Tik 6 X + Tc,(sint + cos2j) 6~ A where R'1, R'2, Fs, R1, R, T1 and T'1 are defined in APPENDIX I. After simplification we obtain 6 W1 = T - c6 + Tc,6 + aFS6 (3-19) where 6X1 is the virtual rotation vector of 0 6X1 = 6osin + 6ocos13 + cosj + fk The virtual work of the second beam is Ws32 = -m2g (sini4 + cosij) d 6r'- mpg (sin2i + cos3j) r2iL +r + (R1t j) + R2)3 622 FC Lz ++ FS i2 6r2 21 + Tl 6k X L -: r -_ _ which after simplification, gives the result, Dynamic Simulation 50

RSD-13-83 6 WB2 = F 6r - aFs 6 (3-20) The total virtual work is 6Wt = 5WB1 + 6WB2 = Tcj66 + Tc~,6P + FC6r (3-2 ) and the generalized forces are: QS = Tcd Q- = Tc, Qr = Fc (3-22) LAGRANGE'S PRINCIPLE: Now that we have obtained the expression for the total Kinetic energy and the generalized forces, we can apply Lagrange's equation to obtain the equations of motion. Lagrange's equation can be written as: d-t [-dqi _ ai a = Qi (3-23) dt [q 9 J q where T is the total kinetic energy, qi is the ith generalized coordinate and Qi is its corresponding generalized force. Making the necessary substitutions for each term in (3-23) we get the following: (i2 + -)r-m2L] +r-4 + mp - + r)}(o2cos2d + ) = F (3-24) The unconstrained equation of motion in the dt direction is: {3L~ + M2k1 + r -- + + rL + 3 2 12 m+ n2pL1- + r +m + (L r)+2r +,cos,6sin,1 11 + r 2fLL + r - j +m(L + r) = (3-25) The unconstrained equation of motion in the p direction is: 2 12 2 + Dya miL r - + mc2 4 Mnp(L1 + r)2}cos2a - 2tp*ossinJn Dynamic 3imulhation 51

RSD-13-63 + m 2Li + r - 2+ r7(Ll + r) 2rcos2- = T=o (3-26) Constraint Conditions The motion of the robot arm is caused by the rotation of the leadscrews which connect the rigid links to the driving motors. A self-locking condition is assumed [12], that is the leadscrews cannot rotate unless the control torques are applied. The constraint equations in their direction could be stated as follows: 1. If the control force is not present (i.e. Fc = O) then r will no longer change and the constraint equation will be r = 0 (3-27) 2. If the control force is applied (i.e. FC X 0), then the leadscrew, which causes the motion of the second beam, exerts a dynamic reaction force to cancel the inertial terms in the equation for r and the equation of motion in the r direction becomes: (m 2 + m)r = Fc (3-28) which is an ordinary, linear second order ordinary differential equation. Similarly, the constraint equations in the 5 direction could be written in the following way: 1. The control torque is not applied, T., = 0, then the constraint equation in the,3 direction will be; = 0 (3-29) 2. The control torque is applied, T, 0, then due to the leadscrew constraint, the equation of motion will be completely independent of the y coordinate. The system will behave as if it is constrained to move in a planar motion with the leadscrew constraint cancelling the centrifugal and Coriolis terms. This results in the following equation of motion, {?rLL m.L + + r - rLf + rn(L1 + )2} i T= (3-30) Dynamic Sirmulation 52

RSD-13-83 There is no constraint in the o direction. Equations of Motion The equations of motion are obtained by combining the constraint equations with equations (3-24) to (3-26). The resulting equations will be: 1. Equation of motion in the r direction: (m2 + m7p)r - 7T~LI + r - + mp7(L-+ r) cos2 sgn F + r l- sgn Fc ' 2 = - L + - + mp(LI + r) [2Cos23 + Jr sgn F (3-31a) The equation of motion in the 6i direction: { IL3 m L 2?L22 L ++ 2J 11 2 I2L2 LJ + m7(L1 + r)}22rb+ 2cos15sin-65 ) + M27 LI + r - + + (L1I + r)j2 sgn I T,.. \r + + \rp( Lz + L r + 3 2V = T+ cossin m + r- c + osin. + + r- #L + r. sr)2T,1 sg T + + r 1 + r 2( + The equation of motion in the ++ direction is the same as equation (3-26). The results obtained in (3-26) and (3-31) show that the rigid body case can be obtained from the general case by dropping out the flexibility terms. Dynamic Simulation 53

RSD-13-83 11. APPENDIX IV- Derivation of the Controller Design The state variables are defined as follows: y1 = r Y3 =;~ 7/ = Y 9 Y2 = Y4 = i Y6= =The rigid body equations of motion, written in terms of the state variables, lead to: Y = Y4 sgn ui 1 Y2 = YV sgn I L2 Y3 = Y6 U1 4 2 + mp (4-2) U2 Y5 = Y6 = 2Dy6y5cos(y2)sin(y2) - 24y6cos2(^2) M2 Ll + Y -2 y + ( +y) + u31(Dcos2(Y2))2~~ + +2mL1 y)2 where D 3 + L + + )2 3 12i2 The sgn function is defined as follows >O 0 sgn(z) =0 if x =0 -I x <0 The sgn function is used to express the constraint conditions imposed on r and d. That is, to insure the invariance in the position of r and 3 when the control variables ul and u2 are removed. Since the sgn function is not allowed to take on negative values in the equations of motion, then the absolute value of its argument is considered. u1. u2. and u3 are the controller torques and force. We need to linearize the state equations around the equilibrium point. The latter is determined by setting the above equations to zero. This leads to. Dynamic Simulation 54

RSD-13-83 0 0 0 0 0 0 0 0 0 1 0 O O 0 0 0 1 = -W, 0 0 -2nr 0 0 ( —) 0 -W 5 0 0 -24W,, 0 0 0 - W2, 0 0 -24 W, In order to evaluate Ks, B-1 has to be computed. The latter is a rectangular matrix and pseudo-inverse method is used to calculate B-1 [18], i.e., 0 0 0 (m2 + mp) 0 0 B-1 = (BTB)-1BT = 00 0 a 000 0 Oac. to O O a where BT is the transpose of B. Substituting A, Ad and B-1 in (4-8) we get (m2 + mp) W,2 0 0 2(m2 + mp)t Wnr O ( 0)0 0 0 aa 0 0W2, Ks= O a W 2, 0 0 24ac xW O ( 4-12) O Ot a LUWcW2, 0 0 2cxWn, The block diagram of the desired system is shown in Figure 7. An integral action is added to eliminate the steady state error or any disturbances in the system. This is illustrated in Figure 8. The integral block, K1, has the following form: 0 0 S K12 I K' 0 0 (4-13) S 0 0 K-3 where Kl's are determined from the root locus diagrams for r, ti, and o respectively. The open loop transfer functions are obtained separately for each degree of freedom. This is done, by first writing the equations for r Y1 0 I ' 12/11 0 ' =4. - 24Wr] -m g +; (14-14) Dyai SirnulationrnY4u1 57 mt2 + -p, Dynamic Simulation 57

RSD-13-83 the output equation is: Z1[1 0] 4 (4-15) The closed loop transfer function for the inner loop of r is 1 (m%2 + mw) (4-16) V(s ) s2 + 2a W,,s + W,2 The corresponding block diagrams are shown in Figures 36(a) and (b). The open loop transfer function is m_ 2 + rp (4-17) s (s2 + 2 Wnrs + W)2r Using the values in Table 2 for C and Wnr' we can draw the root locus diagram for r. Similar reasoning is followed for both 6 and op. The resulting transfer functions are shown in Table 3. The root locus diagrams are shown in Figures 37-39 The values of the Kj's are chosen by trail and error from the root locus. The controller is tuned according to the nonlinear equations of motion listed in APPENDIX III. The chosen values of the K{l's were from the neighborhood of the breakway point. Dynamic Simulation 58

C+***+*+** MAIN PROGRAM IDENTIFICATION *++++**' 467.000 C 474.000 C 475.000 C THIS PROGRAM IS WRITTEN TO SIMULATE lHE RIGID AND FLEXIBLE 475.100 C MOTION OF A ROBOT ARM. SEVEN DEGREES OF FREEDOM ARE USED IN 475.200 C -THIS STUDY. THE FIRST THREE DEGREES OF FREEDOM.DENO1ED Y( 1) 475.220 C TROUGH Y(3), ARE USED TO SIMULATE THE RIGID BODY MOTION, 475.300 C WHILE THE LAST FOUR DEGREES OF FREEDOMY(4) THROUGH Y(7), ARE 475.400 C USED TO SIMULATE THE FLEXIBLE MOTION WHICH IS TRUNCATED AFTER 475.420 C ITS SECOND MODE. THE SEVEN HIGHLY NONLINEAR SECOND ORDER 475.440 C ORDINARY DIFFERENTIAL EQUATIONS OF MOTION ALONG WITH THE STAIE 475.490 C FEEDBACK CONTROLLER EQUATIONS ARE TRANSFORMED TO SEVENTEEN FIRST 475.540 C ORDER ORDINARY DIFFERENTIAL EQUATIONS. 1HE LATTERS ARE THEN 475.590 C SOLVED SIMULTANEOUSLY USING GEAR'S METHOD. 475.640 C 475.700 C 475.800 C ++ ++++*+++,++*+***+****++*+*++***+++*+*++***+ 476.000 C 477.000 r~~~~C:~~~~~~~~~~~~~ 478.000 C '+ + + F + UNCTION IDENTIFICATION * 479.000 C 480.000 ~~~~~~~~~~~~~~C ~~~~~481.000 C THIS FUNCTION DEFINES THE SGN FUNCTION 482.000 C 483.000 C 484.000 d i +++ + + + *++**+*+* ***+ *+*+++*+**++ +**+++ + ~* + 485.000 ~ C 486.000 ~ ZI~~~~~~~~~~~C r ~~~487.000 k 0CO1 FUNCTION SGN(X) 488.000 & 0002 REAL*8 X,SGN 489.000 0003 SGN-O.ODO 490.000 0004 IF(X.NE..ODO) GO TO 100 491.000 W 0005 SGN=O.ODO 492.000 0006 OO O TO 101 493.000 0007 100 SGN-1.ODO 494.000 0008 101 RETURN 495.000 0009 END 496.000 OPiONHS IN EFFECT* ID.EBCDIC,SOURCE.NOLIST,NODECK,LOAD,NOMAP *OFrTIOtJS IN EFFECT' NAME = SGN. LINECNT = 57 *STATISTICS+ SOURCE STATEMENTS = 9,PROGRAM SIZE = 364 O +STATSlITICS NO DIAGNOSTICS GENERATED >i >e^

MIC:lGA(;^tl 11?PMlAl^l 1 SIEM I URIRANJ G(21.) MAIN 08-03 —83:22:59 PAGE PO() ^~~~~~c~~~~~~~~~~~~~~~ 497.000 c 498.000, 4 +, 4 ++ *4 +*+ 4 4 + + + 4 * + 4 * + + +, +* + * 4 499.00() ^~~~~~~~~~~~~~C ~~~500.000 C 501.000 ~C+4'**'*'~*^ MAIN PROGRAM BODY *~+" * + 502.000 c 503.000 C '504.000 0001 PEAL+8 TO,TOUFEPS,H.Y(17),S 505.000 00)7 REAL,4 Y1(220),Y2(220).V3(220).Y.1(220),Y5(220 2Y220),TI(220) 506.000 0003 REAL+4 Y7(220),Y8(220),Y9(220).Y10220).YIl(220).Y12(220) 507.000 0004 RFAL'4 Y 13(220).Y 4(220) 511.000 0005 COMMUN /GEAR9/ NSTEPNFE,NJE 512.000 o000 COMMON /SINGUL/ S 513.000 0007 WRITE(6.10) 517.000 0008 10 FORMAI(3X.'TOUT',8X, 'Y()',12X,'Y(2)',12X,'Y(3)',12X, 'Y(4)'.12X'Y 518.000 1 (1) '. 518.000 1 12X, 'Y(6)',8X) 519.000 0009 N=17 520.000 0010 1=0 521.000 0011 T O=0.ODO 522.000 0012 H=0.0000001D0 523.000 0013 Y(1)- 1.ODO 524.000 0014 Y (2)-0.ODO 525.000 0015 Y(3)=O.ODO 526.000 0016 Y(4)-O0.ODO 527.000 0017 Y(5) =0.ODO 528.000 0018 Y(6)=0.ODO 529.000 0019 Y(7)-0.ODO 530.000 O 0020 Y(8)-0.ODO 531.000 0021 Y(9)=0.ODO 532.000 0022 Y( 11)-.ODO 533.000 0023 Y(11)=0.ODO 534.000 0024 Y(12)=O.ODO 535.000 0025 Y(13)=0.ODO 536.000 0026 Y(14)=0.ODO 537.000 0027 Y(15)-0.ODO 537.100 0028 Y (16) =0.OD 537.200 0029 Y(17)-0.000 537.300 0030 TOUT-0. OOODO 538.000 0031 FPS-0.000100D 539.000 0032 MF- 22 540.000 0033 INE X = 541.000 non3I1 2,1 CALL DGEAR (N,TO.H, Y. OUT.EPS,MF.INDEX) 512.000 C 5,12.020 C 542.040 C tmE CONDITION FOR THE VARIABLE S I1c INCLUDED TO POINT 5,12.100 C OUT THE SINGULARITY OF THE INERTIA MAIRIX WH4FNEVER IT 512.200 C OCCURS AND THEN TO HALT THE EXECUIION OF TilE PROGRAM. 542.300 C 512.400 C. 542.500 0035 IF(S.EQ.O.ODO) GO TO 101 543.000 0036 GO TO 102 514.000 0037 101 WRITE(6,103) S 515.000 0038 103 FORMA ((1X, 'S=',F6.4) 516. 000

0039 GO 10 34 547.000 0040 102 WRITE(6.11) TOUT,Y(l),Y(2),Y(3),Y(4),Y(5),Y(G) 548.000 0041 11 FORMAI(1X,F7.4,2X,5(F14.8.2X),E1,.8,6X) 549.000 0042 I=1-1 550.000 0043 '( I)=Y(1) 551.000 0044 Y2( )=Y(2) 552.000 0045 Y3(1)=Y(3) 553.000 0046 Y(I )- (4) 554.000 0047 Y5( I )-Y (5) 555.000 0048 Y6(1)-Y(6) 556. 000 0049 Y7( )-Yv(7) 557.000 0050 v8(1)=Y(8) 558.000 0051 YI9( -Y (9) 559.000 0052 vIO(I)-Y(10) 560.000 0053 Y 11(I)- (11) 5 1.000 0054 Y12(I)=Y(12) 562.000 0055 Y13(I)-Y(13) 563.000 005G Y14(1)=Y(14) 564.000 0057 TI(I)=TOUT 565.000 0058 IF (INOEX.EQ.O) GO TO 13 566.000 0059 WRITE(6.12) INDEX 567.000 006 12 FORMAt(//,17X, 'ERROR RETURN WITH INDEX-'.17) 568.000 O0061 GO 10 34 569.000 0062 13 IOUT=TOUT0. 05DO 570.000 0063 IF (TOUT.LE.10.ODO) GO TO0 24 571.000 0064 N= I 571.200 0065 70 WRITE(6,71). 572.000 0066 71 FORMAT(3X,'TOUT',8X,'Y(')', 12X,'YfI)',12X.'Y(P)',12X,'Y(10)'.,1X.' 573.000 Y( I t)', 573.000 I 12X,'Y(12)',8X) 574.000 0067 DO 201 1=1,200 575.000 0068 WRITE(6,202) TI(I),Y7(I ),Y8(I).Y9( 1 ),YO( I),YI()Y12(I) 576.000 0069 202 FORMAT(1X,F7.4,2X,5(E14.8,2X), E14.8,6X) 577.000 0070 201 CONTINUE 578. 000 0071 WRIfE(6.203) 579.000 0072 203 rFORMAT(3X,'TOUT',8X'Y( 13)', 2X. ' 1 1 ( 14)') 580.000 0073 [D0 205 1-1,200 581.000 0074 WRITE(6,207) TI(1 ),Yi3(I), Y14(I) 582.000 0075 207 FORMAT(IX,F7.4,2X,2(E14.8,2X),2X ) 583.000 0076 205 CONrINUE 584.000 00771 1 WRIItF(6, 16) NSTEP.NFE.N.IE 585.000 0078 1 IORMAI(//, 'PROBLEM COMrLETFDI IN', 10.2X, 586.000 I 'STEPS',//,21X,I10,2X,'F EVALUATIONS',// 587.000 I 21X,110,2X, 'd EVALUATIONS'.//) 588.000 C 590.000 C 591.000 C4'-. ' ''- +* PLOTTER COMMANDS + 592.000 C 593.000 C 594.000 0079 CALL PSCALE(6.,l.,TIMIHN.11F,TI,IN, 1) 595.000 0080 CALL PSCALE(6.. 1..Y1MIN.YIF,YI,IN.1) 596.000 0081 CAIL PLIOFS(TIMIN,TIF.Y1MIN.Y1F,2..2.) 597.000 0082.CALL PLINE(TI,Yi,IN, 1,00 1) 598.000 0083 CALL PAXIS(2.,2.,'TIME',.-4,6.0.,lMIN, TIFr ) 599.000 008.1 CAl. PAXIS(2.,2.,'Y(I)'.4.6..90..YIMIN.Y1F.1. 600.000

MICHIGAN ITERMINAL SYSTEM FORTRAN G(21.8) MAIN 08-03-83 01:22:59 PAGE P0)3 0085 CALL PLTEND 601.000 0086 CALL PSCALr(6.,1.,TIMIN.TIrTIIN,1) 603.000 0087 CALL PSCALE(G.,l.,Y2MIN.Y2F.Y2.IN.i) 604.000 0088 CALL PLTOFS(TIMIN,TIF.Y2MIN,Y2F,2.,2.) 605.000 0089 CALL PLINE(T1,Y2,1N,1,0,0.1) 606.000 0090 CALL PAXIS(2..2.,'TIME',-4,6.,0..rIMIN,TIF,1.) 607.000 0091 CALL PAXIS(2.,2.,'Y(2)',.4,6.,90..Y2MIN.Y2r. ) 608.000 0092 CALL PLTEND 609.000 0093 CALL PSCALE(6.,1.,TIMIN.TIF, TI.IN1.) 611.000 0091 CALL PSCALE(6.,1..Y3MIN,.Y3r.Y3.1N.1) 612.000 0095 CALL PLTOFS(TIMIN,TIF,.Y3MIN,Y3.2..2.) 613.000 0096 CALL PLINF(TI,Y3,N,1,.0,0,1) 614.000 0097 CALL PAXIS(2.,2.,'TIME',-4.6.,O.,TIMINTIF,1I) 615.000 0098 CALL PAXIS(2.,2..'Y(3)',4.6.,90.,Y3MIN,Y3FI.) 616.000 0099 CALL PLTEND 617.000 0100 CALL PSCALE(6.1.,TIMIN.TIr.TI.INJ,1) 619.000 0101 CALL PSCALE(6.,1..Y4MIN.Y4F.Y4.IN,. ) 620.000 0102 CALL PLTOFS(TIMIN.TIF.,Y4MIN.Y4F.2.2.) 621.000 0103 CALL PLINE(TI,Y4,IN.1,0,0.1) 622.000 0104 CALL PAXIS(2.,2.. 'TIME',-4.6..0..TJMIN,.TIF,1.) 623.000 0105 CALL PAXIS(2.,2.,'Y(4)'.4,6.,90..Y4MINY4F. 1.) 624.000 0106 CALL PLTfND 625.000 0107 CA.L PSCALE(6.,1.,TIMIN,TIFTI,IN.I) 627.000 0108 CALL PSCALE(6.,1.,Y5MIN.Y5FY5,IN.1) 628.000 0109 CALL PLTOFS(TIMIN,TIF.Y5MIN.Y5F.2..2.) 629.000 0110 CALL PL1NE(TI.Y5.IN,1,0,0, 1) 630.000 0111 CALL PAXIS(2..2.,'TIME', -4,6.,0.,rt MIN.,TIF.1.) 631.000 0112 CALL PAXIS(2.,2.,'Y(5)'.4,6..90.,Y5MINY5F,1.) 632.000 0113 CALL PLTEND 633.000 0114 CALL PSCALE(6..1..TIMIN.TIr.TIIN,1) 635.000 0115 CALL PSCALE(6.,.1.Y6MIN,Y6F,Y6,IN.1) 636.000 0116 CALL PLTOFS(TIMIN,TIFrY6MIN,Y6F.,2.,2.) 637.000 0117 CALL PLINE(TI,Y6,IN,1,0,0, 1) 638.000 0118 CALL PAXIS(2.,2.,'TIME',-4,6..0..TIMIN.TIF.1.) 639.000 0119 CALL PAXIS(2.,2.,'Y(6)',4.6.,90..YGMIN.YGF.I1.) 640.000 0120 CALL PLTEND 641.000 0121 CALL PSCALE(6..1.TIMINTIF,TI.IN.1) 613.000 0122 CALL PSCALE(6..1.,Y7MIN,Y7F,Y7,I1N.1) 644.000 01213 CALL PLTOFS(TIMIN,.TIF.V7MIN.Y7r,2..2.) 615.000 0121 CAIL LNL(r1.Y7. IN.1.00. 1) G 646.000 012 CAlI PAXIS(2..2.. 'IIM. 4 r... O I IMIU.II. r ). 647.000 012G; CAIL l~AXJ'XI2..2.. 'YI)',4.G. 9(., YIMINYI 1.1. 64H.0O 0127 CALL PLTEND 649.000 0128 CALL PSCAIE(6.,1..TIMIN.TIF.TIIN.1) 651.000 0129 CALL PSCALE(6.,1.,Y8MIN,Y8F.Y8.IN.I1) 652.000 0130 CA.L PLTOFS(TIMIN.TIF.Y8MIN,Y8F.2..2.) 653.000 0131 CALL PLINE(TI,V8,.1N,1,.0,0.1) 651.000 0132 CALL PAXIS(2..2.,'TIME'.-4.6..0..rrnIIN.TIF.1.) 655.000 0133 CALL PAXIS(2..2.,'Y(8)',4.6..90.,Y8MINY8 FI.) 656.000 013'1 CALL PLTEND 657.000 0135 CALL PSCALE(6..1..TIMIN,.TIr, TI.IN.1) 659.000 0136 CALL PSCALE(G6.1..Y9MIN,Y9F.Y9,IN.I1) 660.000 0137 CALL rLTOrS(TIMIN.TIF.Y9MINY9F,2.,2.) 66, 1.000 0138 CALL PLINE(TI.Y9.IN,1.0.0.1) 662.000 0139 CALL PAXIS(2..2..'TIME'.-4.6.,0.,TIMIN.TIF.1t. 663.000

0O140 CALL PAXIS(2..2..'Y(9)',4,6..90.,Y9MIN,Y9F,1.) 64.000 0141 CALL PLTEND 665.000 01,2 CALL PSCALE(6.,1..TIMIN,TIF,TI.IN,1) 667000 01,:l CALL PSCALE(6.,1.,Y1OMIN.Y1OF,YIO,IN,1) 668.000 0144( CALL PLTOrS(TIMIN.TIF,1OMIN,Y0,2..2.) 669.000 01,1 CALL PLIME(TI,YIO,IN.1,0,0,1) 670.000 O146 CALL PAXIS(2.,2. 'TIME',-4.6..0.,IMIN,TIF,1.) 671.000 0147 ' ALL PAXIS(2..2.,'Y(10)',5,6.,90.,YOMIN.Y r, 1.) 672.000 0148 CALL PLTEND 673.000 014O CALL rSCALE(6.,. MN.TM TF,TI IN, 1 ) 675.000 0150 CALL PSCALE(6..1.,Y11MIN,Y1F,Y1. IN,N 1) 676.000 0151 CALL PLTOFS(TIMIN,11F,Y11MINY 1F.2.,2.) 677.000 0152 CALL PLINE(TI,Y11,IN1,0,0,0.) 678.000 0153 CALL PAXIS(2.,2.,'TIME',-4.6.,0..TIMIN,TIF,1.) 679.000 0154 CALL PAXIS(2.,2.,'Y(11)',5.6.,90.,Y11MIN,Y1IF,1.) 680.000 0155 CALL PLTEND 681.000 0156 CALL PSCALE(6.,1..TIMIN.TlF,TI.IN. ) 683.000 0157 CALL PSCALE(6.1..Y2MIN.Yl2FYt2,IN,1) 684.000 0158 CALL PLTOrS(TIMIN,TIF,Y12MIN.Yl2F.2..2.) 685.000 0159 CALL PLINE(TI,Y12,IN.1.0.0.1) 686.000 0160 CALL PAXIS(2.,2.,'TIME',-4,6.,0. MIN.TIMN.TF..) 687.000 0161 CALL PAXIS(2.2.,'Y(12)',5,6.,90.,Y12MIN.YI2F,1.) 688.000 0162 CALL PLTEND 689.000 0163 CALL PSCALE(6.,1..TIMIN,TIF,TII,I N,1 691.000 016;1 CALL PSCALE(6.1.,Y13MIN,YI3F,Yi3.IN,1) 692.000 0165 CALL PLTOFS(TIMIN,TIF,Y13MIN, Y13F, 2.. 2.) 693.000 0166 CALL PLINE(TI,Y13,1N,1,0,0,1) 694.000 0167 CALL. PAXIS(2.,2.,'TIME',-4.6.,0.TIMIN,TIF,f.) 695.000 0168 CALL PAXIS(2.,2.,'Y(13)',5,6.,90..Y13MIFI,Y13F,1. 1 696.000 0169 CALL PLTEND 697.000 C 0170 CALL PSCALE(6.,.,TIMIN,TIF,TI.IN,1) 699.000 0171 CALL PSCALE(6.,1.,Y14MIN.Y14F,Y14,IN,1) 700.000 0172 CALL PLTOFS(TIMIN,TIF,Y14MIN,YI4F.2..2.) 701.000 0173 CALL PLINE(TI,Y4,IN.1,.0.0,1) 702.000 0174 CALL PAXIS(2.,2.,'TIME', -4,6. 0.,TIMIN,TIF,1.) 703.000 0 175 CALL PAXIS(2.,2.,,'Y(14)'.5.6.,90.,Y 4MIN,Y14r,. ) 704.000 0176 CALL PLTEND 705.000 0177 CALL EXIT 706.000 0178 STOP 707.000 0179 FND 708.000 '(OPlI(!Nfl IN EF.rr:r.C IL,P.CI)IC,SOURCE,NOI IS1,NODECtK.LOAD.NOMAP +UorriPTJ- IN EFFFECI' NAME = MAIN, LNECNT = 57 'STA1STIC CSt SOURCE STATEMENTS = 179,PROGRAM SIZE = 19416 *SIAtIrSICS'" JO 1DIAGNOSTICS GENERATED

MICHIGAN 1FPMINAL SYSIEM FORTRAN G(21.8) MAIN 08-03-83 ()1:23:00 PAGE POOl lJh dh C 710.000 C 7 1 1. 000 C. +* ~ *+ + + + + t* + *** **,.+ * + +,+,* +*,+ | +, +.+.... t i 71 2.000 C 7 13.000 C 714.000 0001 SUBROUTINE DIFFUN (NTIME,Y.YDOT) 715.000 C 716.000 C 7 17.000 C' '* '4,,,+ PROGRAM IDENTIFICATION t t+J+4 * 718.000 C 719.000 C 720.000 C THIS SUBROUTINE DEFINES THE DIFFERENTIAL EQUATIONS OF MOTION 721.000 C 722.000 C 723.000 C+ * + ~ + +* * * ** *** * * * ** * * + + + + ++ + + 4 ++ + ++* *+ 724.000 C 725.000 c 726.000 C+++*++t++ VARIABLE IDENTIFICATION *. 44, 727.000 C 728.000 C 729.000 C MI-MASS OF THE FIRST BEAM 730.000 C M2=MASS OF THE SECOND BEAM 731.000 C MP-MASS OF THE PAYLOAD 732.000 C C1=TRANSLATIONAL CONTROLLER FORCE 733.000 C C2-ROTATIONAL CONTROLLER IORQUE AROUND K VECTOR 734.000 C C3=ROTATIONAL CONTROLLER TORQUE AROUND THE VERTICAL AXIS 735.000 C LI-LENGTH OF THE FIRST BEAM 736.000 C L2-LENGTH OF THE SECOND BEAM 737.000 C ZETA1=DAMPING RATIO IN THE r DIRECTION 738.000 C ZETA2=DAMPING RATIO IN THE TETA DIRECTION 739.000 C ZETA3=DAMPING RATIO IN THE FETA DIRECTION 740.000 C WNR=NATURAL FREQUENCY FOR r 741.000 C WNTETA=NATURAL FREQUENCY FOR TETA 742.000 C WNFETA=NATURAL FREQUENCY FOR FETA 74,13.000 C PRIREFERENCE INPUT IN THE r DIRECTION 744.000 C R2=REFERENCE INPUT IN THE TETA DIRECTION 745.000 C R3=REFERENCE INPUT IN THE FETA DIRECTION 7,16.000 C A2-CROSS SECTIONAL AREA OF THE SECOND BEAM 746.200 C ROW=DENSITY OF THE UNIFORM SECOND BEAM 7,16.400 C 747.000 C 7,18.000 C + + ++ + ++****+ +4++4++4+444* +, +++*++ 4 + 4 4+*+ +F++* 749 000 C 750.000 C 751.000 C*+ ++++** STORAGE BLOCK +*+++4 +4* 752.000 C 753.000 C 754.000 0002 REALB8 TIME,IK11,IK22,IK33,CENTAC.DIST,PALL(7) 755.000 0003 INTEGER IV(7),IJ,IN,IM.IY,IUH,JIJH,K 756.000 00041 REAL48 M1,M2,MP,L1,L2,RI,R2,R3.Y(N),YDOT(N),RC,DC 757.000 0005 REAL*8 ALPHA,BETA,CI,C2,C3,WNR,WNTETA.WNFETA.ZETAI,PBY(7) 758.000 0006 REAL+8 ZETA2,ZETA3,A2.ROW.A13.A14.AIS,ROW7,ROW8 759.000 0007 REAL+8 ROW1,ROW2,ROW3.ROW4,ROW5, ROW6 760.000 0008 REAL*8 ALI,AL2,ALIO,AL11,EI,ROWIO,ROW9 761.000

000q PEAI.*8 B(77)T7, 7 ), F(7),D(7),YVECT(7) 762.000 QOO1) RFAl. '8 Pi3B(7.7),PIrr(7).PTD(7),PTI3B(7).A(7,7).G 763.000 OO1 RPEAlIR PARIl.PART2.PAR3,.PAR14.PART5 7G4.000 0012 REAL'8 PARI6.PART7,PART8,PART9 766.000 0013 COMMON /SINGUL/ S 768.000 0014 M-7 769.000 0015 112n-0.93GGDO 770.000 0016 M 1=0.46500 771.000 0017 MP=0.0700 772.000 0018 A2=0.000173D00 773.000 0019 ROW-2707. ODO 774.000 0020 C 1=0.ODO 775.000 0021 C2=O.ODO 776.000 0022 C3=O.ODO 777.000 0023 R1=2.0DO 778.000 0024 R2=0.5D0 779.000 0025 R3=0.5D0 780.000 0026 L1=0.361D00 781.000 0027 12=2.0DO 782.000 0028 ZETA1=1.0D0 783.000 0029 ZETA2-=1.0DO 784.000 0030 ZETA3=1.ODO 785.000 0031 WNR=4.0D00 786.000 0032 WNTETA=4.0DO 787.000 0033 WNFETA=8.0DO 788.000 C 788.100 C. 788.200 C G IS SET TO ZERO TO SHOW.BY COMPARISON WITH THE 788.300 C GENERAL CASE, THE EFFECT OF GRAVITY ON THE FLEXI- 788.400 1 C BLE MOTION. 788.500 C 788.600 C 788.700 0034 G=O.ODO 789.000 0035 A13=0.7600 790.000 0036 A14=0.53D00 791.000 0037 A15=4.34D00 792.000 0038 AL1=0.6800 793.000 0039 AL2=1.43DO 794.000 0040 AL0O=12.362D00 795.000 0041 AL11=485.51900 796.000 0042 E l=1121.900 797.000 0043 IK11=8.8D00 797.100 0044 1K22=5.8D0 797.200 0(015 1K33-44.21)0 797.300 00,6 DO 222 1=1,7 797.400 0047 00 222 d=1,7 797.500 0048 T(I.J)=O.ODO 797.600 0019 222 CONTINUE 797.700 C 798.000 C 799.000 C 800.000 C 801.000 C 4** t+ + +++*,4**.*+* **++*+ **.*4+ 802.OO0 C 803.000 C 804.000

MIClIGANJ TEP.RMINAI SYSrEM tORTRAN G(21.8) DIFFUN 08-03-83 01:23:00 PAGE P003 C+**+*+* t+ SORT BLOCK **+ + * 805.000 C 806.000 C 807.000 C 808.000 0050 CENTAC —(Y( 10)+2)+((DCOS(Y(2)))' 2) (Y(9)*+2) 808.200 0051 DIST-(L1+Y(1))+((Y(1)*+2)/2.0DO) 808.400 0052 BC-LIvY(1)-(L2/2.0DO) 809.000 0053 ROW 1 -ROW+ 2 810.000 0054 ROW2=I. I1- (L2/2.000D)+(Y ( )/2.0DO) 811.000 0055 ROW3L2-Y ( ) 812.000 0056 ROW4=L1+Y(1) 813.000 0057 POW5-(LI+2)*Y(1)+L1(Y( 1)++2)+(Y(1)+*3)/3.0D0 814.000 0058 POW6-ROW/2.0 DO 815.000 0059 DC=(M1+L1++2)/3.0DO0-M2+BC+*2+(M2+L2**2)/12.0DOMP+ROW4+ 2 816.000 0060 ALPHA=(M1"L1**2)/3.0DO+(M2tL2**2)/12.0DO+M2 (L1+1.0DO- 817.000 1 (L2/2.0DO))*+2+MP*(L1+I.0DO)**2 818.000 0061 ROW7=MP+ROW4 819.100 0062 ROW8-((ROW1*Y(1))4MP)+G*DSIN(Y(2)) 819.200 0063 ROW9-ROW1iG*DSIN(Y(2)) 819.300 0064 ROW10=ROW1+(Y(10)*'2)+DSIN(Y(2))PDCOS(Y(2)) 819.400 0065 A(1,i)=M2*MP 820.000 0066 A(1,2)=0.0DO 821.000 0067 A(,3)=O.ODO 822.000 0068 A(1,4)=O.ODO 823.000 0069 A(,5)=0.OD0 824.000 0070 A(1,6)=O.ODO 825.000 0071 A(,7)=O.ODO 826.000 0072 A(2,1)=O.ODO 827.000 0073 A(2,2)=DC 828.000 0074 A(2,3)-0.ODO 829.000 0075 A(2.4)=O.ODO 830.000 CO 0076 A(2.5)=0.OD000 831.000 C 0077 A(2.6)=0.OD0 832.000 0078 A(2,7)=0.000 833.000 0079 A(3,1)=ROW1*(Y(1)+DCOS(Y(2))*(0.78D00Y(6)+ 834.000 1 0.43DO*Y(7))-1.3DOtY(1)+DSIN(Y(2))+(Y(4)'Y(7)-Y(5)+Y(6))- 835.000 1 LU*DCOS(Y(2))*(0.78DO*Y(6)+0.4300)Y(7))- 836.000 I Y(t1)-DCOS(Y(2))*(0.3500+Y(6)-0.2sDO+Y(7)))+MrP2.oDO* 837.000 1 DCOS(Y(2))'(Y(6)-Y(7)) 838.000 0080 A(3,2)=ROWI*(-Y( 1 )+DCS(Y(2))+(Y(4)+Y(6)+Y(5)tY(7)) 839.000 1 -L1"Y(1)'DSIN(Y(2))+(0.78DOY(6)0.43DO*Y(7))- 8,10.000 1 (Y( 1)+2)*)DSIN(Y(2))+(0.57D00+Y(G)40.0900Y(7)) )4-MPr 811.000 I (-4.0DO+DCOS(Y(2))+(Y(,4)+Y(G)-Y(.)+Y(7)-Y(5lY(6)~ 842.000 I Y(5)PY(7))-2.0DO*(L1+Y(1))+DSIN(Y(2))P(Y(6)-Y(7))) 843.000 0081 A(3,3)=((M1iL1**2)/3.0DO)*( (DCOS(Y(2) ) )**2)4ROWI+ROW3 844.000 I '(ROW2*+2)+((DCOS(Y(2)))*+2)4 845.000 I (ROWI*(ROW3+3)*((DCOS(Y(2)))**+2))/12.ODO-ROW+(Y(I) t 846.000 I ((DCOS(Y(2)))++2)+(Y(6)+*24Y(7)+'2)~Y(1)*((DSIN(Y(2)))++2) 817.000 1 4(Y(6)++2+Y(7)**2)+Y( 1)+((DSIN(Y(2)))**2)*(Y(4)*+2*Y(5)+42) 848.000 1 -2.0DO+L4DSIN(Y(2))*OCOS(Y(2))+Y( I 1 (0.78DOY(4)+0.43DO0 8-19.000 i ~(5))-2.O0O*DSIN(Y(2))'DCOS(Y(2))'(Y( 1)4'2)*0.5700+ 850.000 1 Y(4)+0.09DO*Y(5))+ROW5*((DCOS(Y(2) 1)+2))+MP+(4.0DO0 851.000 I ((DCOS(Y(2)))+*2)+(Y(6)4 2+Y(7) '^2-2.000*Y(G)*Y(7))+ 852.000 I 4.0DO*((DSIN(Y(2)))+*2)*(Y(6)*P24Y(7)*+2-2.ODO*Y(6)* 853.000 I Y(7)+Y(4)+*2+Y(5)**2-2.ODO*Y(4)'(5) )-4.ODOD)SIN(Y() )* 854.000

I CO0S(Y(2) ) ROW4~ ( 4) —Y(5) )4 (ROW' *2) ((DC)(Y(2) ) )' ) 855.000 00( 2 A (3.4 ) —PUW1*Y( 1 I'SIN(Y(2) )Y(G -MP 4.o0'lI)S N(Y(2) + 853G.000 I (Y(G)-Y(7)) 857.000 O(R' A(3.5)=-ROW1+Y( 1)*DSIN(YI2))+Y(7)-tr4'4.0r)ODSIN(Y(?))4 (Y(7) 858.000 i -Y(G)) 859.000 00841 A(3,6)-ROWI*(Y( 1 )*DSIJNY(2 ))Y(4)-I. 1DCOS(Y(2) )0.78DO* 860.000 i Y(1)-Y(1) +2+DCOS(Y(2))+0.57DO)*MP'(4.0ODO"DSIN(Y(2))+(Y(4) 861.000 I -Y(5))-2.0DO+ROW4*DCOS(Y(2))) 8G2.000 0085 A(3,7)=ROW1+(Y( 1)DSIN(Y(2))*Y(5)-Ll1 863.000 I DCOS(Y(2))"O.43DO+Y(1)-(Y(1)**2)PDCOS(Y(2)) 8G4.000 i +0.09DO)4MP+(4.0D0+DSIN(Y(2))+(Y(5)-Y(4))-2.000 865.000 1 4POW4'DCUS(Y (2))) 8GG6.000 0086 A(4, )-RPOW1*(Y(1)/2.0DO)+(Y(4)-I.3DO+Y(5)) 867.000 0087 A(4,2)=ROWG*(1.56DOLL1*Y(1)+1.14DO*(Y(i)*+2)) 868.000 I *MP'(2.00DOROW4) 869.000 ~0!8R A(4.3)=-ROWi+DSIN(Y(2))'Y(1)*-Y(6)-MP*4.0DO 870.000 i 'DSINJ(Y(2))+(Y(6)-Y(7)) 871.000 0089 A(J..4)=ROW1+Y(1)+4.0DO'MP 872.000 0090 A (4,5 ) = -4. ODO*MP 873.000 0091 A(4,6)=O.000 874.000 (0092 A 4.7 ) -0.000 875.000 0093 A(5, i)=ROW1IY(1)*(0.65DO+Y(4)4+0.500+Y(5)) 876.000 0094 A(5,2)=ROWl+(0.43DO+LI*Y( 1)+0.09DO+(Y( 1)t*2)) 877.000 i -MP+2.0DO*ROW4 878.000 0095 A(5.3)=-ROW1*DSIN(Y(2) )Y( I )*Y(7)-4.ODOMP* 879.000 I USIN(Y(2))+(Y(7)-Y(6)) 880.000 009 G A (5,4 ) — - 4. ODO MP 8 81. 000 0(97 A(5.5)=ROWI*Y( 1)-4.0DO'MP 882.000 0098 A (5,G6)-O.O000 883.000 0099 A (5.7)-0.000 884. 000 0100 A(6.1)=ROW+Y(i)*(0.5DOY(6)-0.65DO*Y(7)) 885.000 0101 A (6.2)=0O.000 88G. 000 0102 A(6,3)=ROW1*(Y( 1)+DSIN(Y(2))+Y(4)-0.7800D Y( 1 )" 887.000 I l)COS(Y(2))*L1-0.57DO*(Y( 1)++2)+DCOS(Y(2)))+MP+(4,.ODO- 888.000 I DSIN(Y(2))*(Y(4)-Y(5))-2.0DO ROW4+[)COS(Y(2))) 889.000 0103 A6.4) =0.D000 890.000 0101 A(6.5)=0.000 891.000 0105 A(6.6)=ROW1*Y(1)+4.0DO+MP 892.000 0106 A(6,7)=-MP*4.OD0 893.000 0107 A(7, )=ROWi*Y(1)*(0.65DO+Y(6)-O.5D00Y(7)) 894.000 0108 A(7,2 )=0.D000 895.000 0109 A(7,3)=R0W1+(Y(1)+DSIN(Y(2))*Y(5)-0.43D00L1+Y( 1) 896.000 1 DCOS(Y(2))-0.09DO+(Y(1)+*2)ODCUS(Y(2)))4MP+(4.ODO+DSIN(Y(2)) 897.000 I *(Y(5)-Y(4))+2.0DO+ROW44DCOS(Y(2))) 898.000 0110 A(7,4)=O.ODO 899.000 0111 A(7,5)=0.0D0 900.000 0112 A(7,6)=-4.0DO+MP 901.000 0113 A(7,7)=ROW14Y(1)44.0DO*MP 902.000 0114 ( 1,i)=0.000 910.000 0115 B( 1.2)=0.000 911.000 0116 B(.3)0.ODO 912.000 0117 B(,4)-O.OD0 913.000 0118 B( 1,5)=O.ODO 914.000 0119 B( t,6)-0.000 915.000 0120 B(1,7)-O.ODO 916.000

MIC IGAN' ITr MITNAI. SYSIFM )r ORIPAN G(21.8) t)lfrtJ rU 08 03-83 01:23:00 PAGE 1005 0121 F (2.1)-0. OD 917.000 0122 R(2,2)-0.01)0 918.000 0123 P,(2.3)-0.0)00 919.000 0124 B(2,4) —0.000 920.000 0125 B3(2,5)-0.ODO 921.000 0126 B(2,6)=0.ODO 922.000 0127 B(2,7)-0.000 923.000 0128 PART1=-RUW1iY(10) (ROW2+2 )+( (DCOS(Y(2)))'2) 924.000 I fROW1 ROW3+Y(10)*ROW2*( (DCOS(Y(2) ) )2)-( (RW1 925.000 1 (ROW3,+2))/4.0DO)Y( 10)+((DCOS(Y(2)))++2)RPOfW1 926.000 1 (Y( 10)((DCOS(Y(2)))*2)+(Y(6)*+2 927.000 1 4Y(7)+2)4(Y(8)+DCOS(Y(2))-Y(1)*Y(9)*DSIN(Y(2))) 928.000 1 '(0.78D0+Y(6)+0.43DO+Y(7)) +Y( 1DCOS(Y(2))+ 929.000 1 (0.78DO Y( 13)+0.43DO+Y( 14)I-Y(9)+DCOS(Y(2))+(Y(4)+Y(G)+ 930.000 1 Y(5)*Y(7))-DSIN(Y(2))+Y( 1)(0.50D'(Y(11)*Y(G)+ 931.000 I Y(4)*Y(13))+0.65DO+(Y( 1)*Y(7)4-Y(4, )Y(14)) 932.000 1 -0.6500*(Y( 12)*Y(6)Y(5)+Y(13))40.5DO(Y(12)4Y(7)+ 933.000 1 Y(5)Y( 14)))) 934.000 0129 PART2-ROW1(-DSIN(Y(2))P(Y(11)+*Y(6 Y(121*Y(7)) 935.000 1 -L1+Y(9)tDSIN(Y(2))+(0.78DO*Y(G6)0.4300+Y(7)) 936.000 1 -2.0DO+Y( 1)*( IN(Y9)DSIN(Y(2) (0.57DOY(6),0.09D*Y(7)) 937.000 1 'Y(10)*((DSIN(Y(2)))'t2)*(Y(6)++2-Y(7)+*2)+ 938.000 I (Y(8)+DSIN(Y(2))+Y( 1)Y(9)+DCOS(Y(2)))* 939.000 1 I 1.3DO+Y(6)*Y(5)-1.3DOY(7)+Y(4))) 940.000 0130 PART3-ROW 1(DSIN(Y(2) )Y( 1)* (0.5r)0(Y( 13) "Y( 1) + 941.000 1 Y(6)+Y( 1))+0.65DO+(Y( 13)*Y(5)fY(6)+Y(12))- 942.000 1 0.65D0*(Y(14)*Y(4)+Y(7)*Y(11))40.5DO4(Y(14)+Y(5) 943.000 1 +Y(7)+Y(12)))+DSIN(Y(2))+(Y(13)+Y(4)+ 944.000 Y( 14)+Y(5))+L1*Y(9)+DSIN(Y(2))+(0.78DO'Y(G)+ 945.000 1 0.43DO+Y(7))-L1*DCOS(Y(2))( 1.56DO+Y(13)+ 946.000 1 0.86DO+Y(14))) 947.000 0 0131 PART4-ROW1+(-(Y(8)*DCOS(Y(2))-Y( I)Y(9)+DSIN(Y(2)))+(0.35DO* 948.000 CD 1 Y(6)-0.25DOY(7))-DCOS(Y(2))+Y( 1)+(0.35D0* 949.000 1 Y(13)-0.25DO*Y(14))-2.0DO*Y(1)*DCOS(Y(2))* 950.000 I (0.57)0OY(13)+0.09DO*Y(14))+Y(10)t((DSIN(Y(2))) 951.000 1 t+2)+(Y(4)*+2+Y(5)*+2)-2.000D L1+Y(10)+DSIN(Y(2)) 952.000 1 +DCOS(Y(2))*(0.78DO+Y(4)+0.43DOY(5))-4.0DO+Y(1)+Y(10) 953.000 1 +DSIN(Y(2))*DCUS(Y(2))+(0.57D00Y(1)40.09DO+Y(5))) 954.000 0132 PARr5=ROW1*(Y(10)+((DCOS(Y(2)))++2)+((L1't2)+ 955.000 1 2.0DO*L1Y(1)+(Y(1)*+2)))+MP+(-4.0DOY(9)+DSIN(Y(2))+ 956.000 I (Y(6) —Y(7))-4.0DO*Y(tO)*DCOS(Y(2))*DSIN(Y(2)) 957.000 I (Y(4)-Y(5))+2.0DO0ROW4tY(10)+(IDCOS(Y(2)))** 2)) 958.000 0133 B(3,1)=PART1+PART2+PART3+PART4'PARr5 959.000 0134 PARr6 —0.6667DO+M1*(L1P*2)*Y(10)*DCOS(Y(2))* 960.000 1 DSIN(Y(2))-2.0DO+ROW1*ROW3+Y( 10)4(ROW2+*2)* 961.000 1 )SIN(Y(2))*DCOS(Y(2))-(ROW1*(ROW3+*3)+Y(O )DSIN(Y(2)) 962.000 1 'DCOS(Y(2)))/6.0DO+ROWI*(-2.0DO*Y( 10)Y( 1)4DCOS(Y(2))+ 963.00( r OSIN(Y(2))*(Y(6)**2+Y(7)**2)+Y(9)'Y( )+DSIN(Y(2)) 964.000 I (Y(4)*Y(6)+Y(5)*Y(7))-Y(1)+DCOS(Y(2))I(Y(11P)Y(6)+ 965.000 1 Y(4)+Y(13)+Y(12)*Y(7)+Y(5)*Y(14))-Y(1 +DCOS(Y(2) ) 966.000 1 (Y( 1)*Y(6)+Y( 2)*Y(7))) 967.000 0135 PART7=ROW1*(-L1*Y( 1)*Y(9)DCOS(Y(2) ) 968.000 I (0.7800+Y(6)+0.4300D Y(7))-L1*Y(I)*DSIN(Y(2))+ 969.000 I (0.78D0*Y(13)+0.43DOY(i4))-(Y(1)++2)}Y(9)*DCOS(Y(2))' 970.000 1 (0.5700*Y(6)+0.09DO*Y(7))-DSIN(Y(2))+(Y(1)+2)+ 971.000

I (0.57DoY( 13)+0.09DO0*Y 14))-2.0U0'OY( O)''V 1)' 972.000 1 rOSN(Y(2))*DCOS(Y(2)) (Y(6)*2Y(7 2) 913.000 1 4-Y( 1)DCOS(Y(2))+(Y(13)+Y(4)+Y(14)Y(5))) 974.000 0136 PARTS8-ROWi+(L1+DSIN(Y(2))(0.7e8D'Y( 1)+Y( 3) 975.000 1 0.43DO+Y(1)*Y(14)+[(Y( 1)++2)DSIt(Y(2))*(0.57DO+Y(13)9 976.000 i 0.09DO*Y( 14))+2.0DODSIN(Y(2)) DCOS(Y(2))+Y( 10)* 977.000 I Y( 1)*+Y(4)*2+Y(5)++2) 978.000 1 -2.0DO+L1"(Y( 10)*((DCOS(Y(2)))"' 2)4Y(1) 979.000 I -Y( 10)((DSIN(Y(2)))*2)+Y(1))+(0. 7 90.078 90 00 1 Y(4)+0.4300oY(5))-2..0DO+Y( 10)(Y( 1*2)* 981.000 i ( ((DCOS(Y(2)))**2)-((DSIN(Y(2)))++2))*(0.57DO+Y(4) 982.000 i +0.0900*Y(5))) 983.000 0137 PART9=ROW1*(-2.0DOY( 10)+DSIN((Y(2))DCOS(Y(2)) 984.000 1 *ROW5)+MP+(Y(10)*DCOS(Y(2))*DSIN(Y(2)) *8.ODO* 985.000 t (Y(4)++2+Y(5)+*2-2.0DO+Y(4)*Y(5 )-2.ODO+(ROW4+2)) 986.000 1 44.0DO*Y(9)+DSIN(V(2))t(Y(4)*Y(6)-Y(4)+Y(7)- 987.000 1 Y(5)+Y(6)4-Y(5)*Y(7))-4.0DO+DCUS(Y(2))*(2.0DO0Y( 1)+ 988.000 Y(6)+Y(4)+Y(13)-2.0DOY(t1)*Y(7)-Y(4)*Y(14) 989.000 I -2.0DO+Y(12)+Y(6)-Y(5)tY(13)+2.0DO+Y( 2)PY(7)+ 990.000 I Y(5)+Y(14))-2.0 0D Y(9)1ROW4+DCOS(Y(2))+(Y(6)-Y(7))+ 991.000 i 4.ODO+DCOS(Y(2))*(Y(3Y(13)Y(4)-Y( 13)Y(5)-Y(14)Y(4)+ 992.000 Y(14)+Y(5))-(Y(10)*((DCOS( Y(2)))+*2) ROW4-Y( 10) 993.000 i ((DSIN(Y(2)))**2)*ROW4)*(Y(4)-Y(5))+4.0DO) 994.000 0138 B(3,2)=PARTG+PART7+PART8sPART9 995.000 0139 B(3,3)=ROW1+(Y(1)*2.000+(Y(6)*Y(13)-Y(7)+Y(14))+ 996.000 1 Y(I)+((DSIN(Y(2)))**2)*2.0DO+(Y(4)Y( 11)4Y(5) 997.000 I Y(12))-2.0DO*Y(1)+DSIN(Y(2))+DCOS(Y(2))*(0.78DO* 998.000 1 L 1Y(11)+0.43DO+L1*Y(12)+0.57DO+Y( 1 )Y(1t)4-0.09DO 999.000 I Y(1)*Y(12)))4MP*(8.0DO+(Y(6)+Y(13)Y(7)+Y( 1)-Y(13) 1000.000 1,Y(7)-Y(6) Y(14))48.0DO'((DSIN(Y(2))) +2) (Yt4) Y( 1 I) 1001.000 1 Y(5)PY(12-V( )-Y(1)Y Y(4)*Y(12))-4.000D)SIN(Y(2))P 1002.000 C 1 DCOS(Y(f2))ROW4*(Y(11)-Y(12))) 1003.000 0140 (3,4)=0.ODO 1004.000 0111 8(3.5) 0.ODO 1005.000 0142 B(3,6)=0.OD0 1006.000 0143 (3,7) 0.ODO 1007.000 0144 B(4, 1)=ROW6 (Y(8)*(Y(4)-1.3D0*Y(5))-rY(9) Y( 1) 1008.000 1 3. 14DO-2.0DO*+Y SINY()DSN(Y(2))Y()-2.0DO ( 1009.000 1 rA13*Y(4)*A14*Y(5))+2.G6lO+Y( )+DIN(Y(2)+ 1010t.000 i Y(1)*Y(7))+MP+4.0DO+Y(9) 1011.000 0145 P,(4,2) ---PO(W1IY(9)+Y( ) +Y(4 -4.0O r, lF' Y(9) (Y (1) 1012.000 -Y(5)) 1013.000 o()1,1G rP,(4,3)-ROW6 (-2.00' Y( 10)* ([)Si( Y(2)) )' 2), (1) 1014.000 I Y(4)rY( 10)DfSIN(Y(2) )*DCOS(Y(2))'Y( 1 )'( t.50(; 0 1015.000 I L1+1.14DOY(1)))+MPt(-4.0DO*Y(10)*((DSINf(Y(2)))++2) 1016.000 1 i(Y(4)-Y(5))+2.0DO+Y(10)+DSIN(Y(2))'DCOS(Y(2)) 1017.000 1 'ROW4) 1018.000 0117 P,( 4.4)-ROW6*2.0DO *Y(8) 1019.000 0148 B(4.5) — ROW6*2.600+Y( I Y(8) 1020.000 0149 B(4,6)=-ROW6*Y( 10)4.0D+DSIN(Y(2))4Y( )-R 01)0 1021.000 I 'MP+Y(10)+DSIN(Y(2)) 1022.000 0150 P(4,7)=8.0DO+MP*Y( 10tDSIN(Y(2 ) 1023.000 0151 C(5. 1)=ROWI+(Y(8)(0.65UOOY(4)0.SDO*Y(5)) 1024.000 1 40.8GD00Y(9)+Y(1)+Y(10)*DSIN(Y(2))+Y(7)- 1025.000 I V'(8)+(A1i+Y(4)+A15"Y(5))-1.30DO+Y( 10) 1026.0(X0

MICtlIGAf, 1FRMINAL SYSIEM FORTRAN G(21.8) DIFFUN 08-03-83 01:23:00 PAGE P007 I DSI4(Y(2) )+Y( 1)Y(6))-MP*4.ODO+Y(9) 1027.000 0152 B(5,2)=ROW1*(-Y(9)*Y( )Y(5))-4.0[)4OMP 1028.000 1 "Y(9)'(Y(5)-Y(4)) 1029.000 0153 B(5.3)-ROW +(-Y( 1)Y(10)+((DSIN(Y(?))))+2) 1030.000 I 'Y(5IY(Y(10)DSIN(Y(2))'r)cOS(Y(2))"Y( 11 1031.000 1 (0.,143)0L l0.09DO+Y( 1 ) ) )+MP+(-,1.OD 1032.000 1 Y(10)"((DSIN(Y(2)))+*2)*(Y(5)-Y(' t 7.O!)O+ 1033.000 I Y( 10)DSIN(Y(2))*DCOS(Y(2))+ROW4) 1034.000 0154 B (5.4 )-ROW i+ 1. 3DO+Y( ) 'Y(8) 1035.000 0155 ( 5.5):ROW1*Y(8) 1036.000 0156 B(5.6)=MP'8.ODO+Y( 10)DSIN(Y(2)) 1037.000 0157 R(5,7)=-ROW1*2.0DOY( 10)+DSIN(Y(2))'Y( 1) 1038.000 I -8.ODO"MP'Y(10)*DSIN(Y(2)) 1039.000 015R B(6,1)-ROW1I(Y(8)+(O.5DO*Y(6)-0.;5O'Y(7)) 1040.000 I +Y(10)*DSIN(Y(2))+Y(4)*-Y(10)*DCOS(Y(2)) 1041.000 1 -(-1.57DOY(1))+Y(10)+DSIN(Y(2))+Y(1)+ 1042.000 1 (-1.3DO+Y(5))-Y(8)+(A13+Y(6)+A14"Y(7))) 1043.000 1 +MP+( -4.ODO*Y(10)*DCOS(Y(2))) 1044.000 0159 p(6,2)=ROW1+(2.0DO*Y(1)+Y(10),DCOS(Y(2) )Y(4)+ 10,15.000 1 1.56D04Y(1)+L1*DSIN(Y(2))*Y(10)+1.14DO"(Y(1)&+2) 1046.OOO 1 +Y(10i)DSIN(Y(2)))+MP+(8.ODOOY(10)'C.OS(Y(2)) 1047.000 1 +(Y(4)-Y(5))+ROW4*4.000-Y(lO)*DSIN(Y(2))) 10,18.000 O160 B(6,3)=-ROW1*Y( 1)Y(10)+Y(6)-4.0f)O0MP'Y(10) 1049.000 1 +(Y(6)-Y(7)) 1050.000 0161 G(6,4)-2.0DO+ROWU1*Y( 10)'DSIN(Y(2))*Y( 1)8.ODO 1051.000 I +MP+Y(10)*DSIN(Y(2)) 1052.000 0162 B(6.5) —8.ODO+MP+Y( 10)+DSIN(Y(2)) 1053.000 01G3 B(6,6)=ROWI*Y(8) 1054.000 0164 R(6,7)=-ROW1* 1.3DO*Y( 1 )Y(8) 1055.000 0165 f(7, 1)=ROW1*(Y(8)*(0.65DO+Y(6)+0.5DO*Y(7))+Y(10) 1056.000 o 1 +DSIN(Y(2))*Y(5)-0.86DOtY(1)*Y(10)*DCOS(Y(2)) 1057.000, I -Y(8)+(A14+Y(6)+A15Y(7))+1.3DO+Y( 10*DSIN(Y(2)) 1058.000 1 +Y(1)+Y(4)) 1059.000 1 4MP+4.ODO+Y(10)*DCOS(Y(2)) 1060.000 016O 6 3(7.2)=ROW1I(2.0DO*Y( 1)Y(10)+DCOS(Y(2))*Y(5)" 1061.000 0. 18DO*(Y(1)**2)*Y(10)+DSIN(Y(2))-O0.86DO+LP*Y(1) 1062.000 1 +Y( 1i0)DSIN(Y(2)))+MP*(8.0)00Y( 10)*fDCOS(Y(2))* 1063.000 I (Y(5) -Y(4))-4.0DO*ROW4dY(10)*1)SI M (2))) IOG61.000 0167 B(7,3) —POW1+Y( 1)+Y( 10)Y(7)-4.0I1O'MP*Y(10) 1065.000 1 (Y(7)-Y(6)) 1066.000 0168 D(7.4) —MPt8.0DO*Y(10)4DSIN(Y(2)) 1067.000 OIG6 t(7,5)=ROWI+2.0DO+Y( 10)'DSIN(Y(2 *Y( 1 )+8.0DO 1068.0C)O I 4MP'Y( 10)+DSIN(Y(2)) 1069.000 0170 B(7.6)=ROW1+I.3D0+Y( 1 )Y(8) 1070.000 0171 B(7.7)=ROWI*Y(8) 1071.000 0172 C1=-((M2+MP)+WNR**2+(Y(1)-R1)+2.O0DO7ETAI'WNR 1072.000 1 +(M24-MP)+Y(8))+IK11+Y(15) 1072.200 0173 C2=-( (WNTETA**2)*ALPHA+Y(2) 10173.000 I,2.0DO+ZETA2+WNTETA+AIlPHtA*Y(9))+IK22+Y(16) 1074.000 0174 C3=-((WNFETA**2)*ALPHMAY(3)+ 1075. 000 I 2.0tD0ZETA3+WNFETA*ALPHA*Y( 10))4IK33+Y(17) 1015.200 0175 F(i )=C1 1076.000 0176 F(2)-C2 1077.000 0177 F(3)-C3 1078.000 0178 F (4)-0.000 1079.000

0os. oUIMi ~011 - ~>,..........,r 5 0.0,10 08 0. 000 017 F FI)-0. O O0 1081.000 Oto0 I r(7)-0.00o 1082.000 0182 D)1)-0.001)0 1083.0(0 018n3 (2)-0.000 1084.000 0184 0(3)-0.000 1085.000 0180 D(4)-ROW1tY(1)*G OCOS(Y(2))4AL12.0DOMPG 1-086.000 'OCO5(Y(2))-(EI#Y(4)tAL10)/(Y( 1).3) 1087.000 -(( ROW i CENTAC DIST (4. G5DO^Y ('1)-7. 38DO*Y(5) ))/Y( 1 ))- 1087. 100 i ROW1$CENTACt((3.08DOoL1^1.13DOY(1))pY(4)(6.9GDOL1 1087.200 1 4-3.0P0n ( '18730 3 ~~.ono ^ vY(1))^Y(5))-((( ROW7 ^CENI ACPROW8) ( 4.oR 6 0Y ( )- 1087.300 i 7.38DO0Y ^)))/Y() )4ROW9(3..08D0Y0(4)-6.9c3DO'Y(5)) 1087.400 I 41.14DO(Y(1)*+2)*ROW41Y(8)*Y(9)+ROW10^(0.57pot 1087.500 1 (Y(I 1)2)L140.22500 (Y( 1)tt3)) 1087.600 0186 D(5) —ROW1iY(1)~G+DCOS(Y(2))*AL2+2.0DOMP 1088.000 1 4GDUCOS(Y(2))-(EI'Y(5)+AL1I)/(Y(1)P3) 1089.000 I -((ROWcPCFNrAC*DIST4(-7.38notY(4) 32.42flOtY(5)))/Y(1))f 1089.100 i POW1+CENTACt(-(6.96DO*L1+3.0DO+Y( ) )lY(4) - (23. 7700 L 1 1089.200 I '9.73DO*Y(1))tY(5))-(((ROW7+CENTAC-PO.W8).(-7.38DO+Y4 1089.300:32.42OvY(5)))/Y( )) ROW9( 4O -G.96GDOY(4y4)23.770*Y((5) 4 10089.400 1 0. 1821)0'Y(I1) *2) ROW1+Y( Y(9)-ROW10 C(O.091DO*L 1' 1089.500 1 (Y(1), *2)-0.02D0*(Y(1)*+3)) 1089.600 0137 D(6)= — (AL 1o E I *v(6))/(YY(1 ) + 3) - (ROW1ICENTACr.)I ST ' (4.65D0O Y(rn 1090.000 1 -7. 38DOY(7 ) ) )/Y( ) )+RCW t4CEIAC' A (3.0800 L1 1L. 10' ( 1 ) ) 1090. 10) I fY(6)-(6.96DO1+3.0Y(1))*Y(7))(((ROW'7CENTAC 1090.200 1 4ROW8)t(4.65DO0-Y(6)-7.38DO0 Y(7)n)/Y(i 1)..PRW9q1 1090.300 1 3.0800OY(G)-6.9GDO*Y(7)) 1090.400 0188 ^7) —(AL11*EI*Y(7))/(Y(1)3)-((WICNrAC^ST^-7.8L) 1091.000 1 Y(6)I32.42D0*Y(7)))/Y(1))ROW1CIJN1ACI(-(. 9600'1.1 1091.100 1 43.ODO+YV(1))Y(6)4(23.770DL11+9.73t)00Y(I,*Y(7)) 1091.200 1 (((RUW7'CENTAC+ROWO) ^(-7.38DOlY (6) 32.42UO"Y (7))) 1091. 300 1 /v(1U)ROW9*(-6.96DO*Y(6)+23.77DOY(7)) 1091.400 0189 YVECT(1)-Y(8) 1092.000 0190 YVEC1(2)=Y(9) 1093.000 0191 YVECT(3)=Y(10) 1094.000 0192 YVECT(4)=Y(11) 1095.000 0193 YVECT(5)=Y(12) 1096.000 0194 YVECT(6)-Y(13) 1097.000 0195 YVECT(7)=Y(14) 1098.000 0196 D00 60 IN-1,M 1098.050 0197 PBY(IN)=O.000 1098.100 0198 00 GO IM-1,M 1098.150 0199 PBY(IN)=PBY(IN)+(-B(JN.IM))'YVECT(IM) 1098.200 0200 G60 CONTINUE 1098.250 0201 DO 61 IY-1,M 1098.300 0202 PALL(IVY)=0.0D0 1098.350 0203 PALL(IY)=PBY(IY)+F(IY)+D(IY) 1098.400 020.1 61 CONTINUE 1098.450 0205 CALL DLUD(M,7,A,7.TIV) 1098.500 0206 IF(IV(M).EQ.O) GO TO 90 1098.550 0207 GO TO 91 1098.600 0208 90 S=0-.ODO 1098.650 0209 GO TO 14 1098.700 0210 91 =1.0DO 1098.750 0211 CALL DBS(M,7,T,IV,PALL) 1098.800

MICHIGAN IFRPMINAL SYSTEM FORTRAN G(21.8) DIFFUN 08-03-83 01:23:00 PAGE P009 0212 YDOT(1)=Y(8)*SGN(C1) 1143.000 0213 YDOI(2)-Y(9) SGN(C2) 1144.000 0211 YDOT(3)=Y(10) 1145.000 0215 vDOT(4)=Y(t1) 1146.000 0216 YDOT(5)=Y(12) 1147.000 0217 YDOT(6)=Y(13) 1148.000 0218 YDOT(7)=Y(14) 1149.000 0219 YDOT(8)=(PALL(1))+SGN(C1) 1149.800 0220 YOOT(9)=(PALL(2))*SGN(C2) 1150.600 0221 YDOT(10)=PALL(3) 1151.400 0222 YDOT(11)=PALL(4) 1152.200 0223 VDOT(12)-PALL(5) 1153.000 0224 YDOT(13)-PALL(6) 1153.800 0225 DOT( 14)-PALL(7) 1154.600 022G YDOT(15)=R1-Y(1) 1155.400 0227 YDOT(16)=R2-Y(2) 1156.200 0228 YDOT(17)-R3-Y(3) 1157.000 0229 11 RETURN 1159.000 0230 END 1160.000 +OPTIlOlOS IN EFFECI+ ID,EBCDIC,SOURCE,NOLIST,NODFCK.I.OAU,NOMAP +OIlIOrlS IN EFFEC1T NAME = DIFFUN, LINECNT = 57 *SIATISTICS+ SOURCE STATEMENTS = 230.PROGRAM SIZE = 27560 +STATISIICSS NO DIAGNOSTICS GENERATED.~ ~ ~ ~~~~~~~~~( U

rc~~~~~~~~~~~~~~~~~ ~~1161. J 000 I 1 1 3. n90 (C ''''' '''*'3.~'' '' ~~~ f''~' ''''' t s'' ~ 6, X o,, 1164.000 c 1165.0(00 c 1166.000 e'~~~~~~~~~~r! ~~~~~1167.000 0001 SUBROUTIfNE PIDERV (r.T,Y,FD,NO) 1 168.000 0002 RETURN 1169.000 000. FNFN 11 0.000 CO) /,

payload motor drive for second beam leadscrew second beamrr first beam leadscrew motor drive for the first beam cylindrical rotating base Figure 1. A schematic of the physical system 74

0I Figure 2. Illustration of the model 75

J Figure 3. Illustration of the parameter used in the dynamic model 76

,2R2 RpI IR 19 Tc R'2 Figure 4. Free body diagram of the first beam Fp Figure::. F,'- body diagra.'- ' the se' ---'., 77

y(t) F(t) Figure 6 Simple rigid body translational degree of freedom model 76

R I+1 Z Fgr.B d of the d c tr Is'.d o the s Figure 7. Block diagram of the state feedback controller applied to the linear system 79

state feedback controller r - - - _ + 1 8 ' fI I l I Rr Ki Figure 8. Block Diagram of the integral plus state feedback controller applied to the nonlinear system 80

ao z A m I TInE [sec] Figure 9. r displacement, obtained from the equations representing the rigid body motion only Figure 10. ' displacement obtained from the equations representing the rigid body of motion only

,f d T]ME [sec] Figure 11. v displacement, obtained from the equation describing rigid body motion only Tire [sec] Figure:2. r displacement, obtained from the base run 82

Ca I / L 2J1 4* I 6 LV R* ^~ TPIr [sec] Figure 13. e displacement, obtained from the base run 83

K 9~~^~~~^^ E~~~~~~~~sc J c, 1. s^e cS~^^h^ 5r~~~r ^1~~~~~~~~~~~~~~1 Fiue 5 ^~~~~~lt di <, fsecj~~~~~as r^ Q"^^

I S BJ lM 4D8 A M ~u u a ol^[sc] TDlE [sec] Figure 17. q2i(t) displacement, obtained from the base run 85 ^^ fI / 65

I ~lt)~ i xz(t) K i(t) h id Figure 19. Model of the rigid body motion with one fiexible iiLode 66

; I S~ TInE r [sec] Figure 20. Rigid body displacement, (t)t)

I3 9 TIE sec] Figure 22. q 1(t) displacement, obtained from the base run with g =0 3 T. TItE [sec] Figure23. q12(t) displacement, obtained from the base run with g =0,e. 88

Srj TIME [sec] Figure 24. rp displacement, obtained from base run X I d f 2.x 4Jw00 t oo s~ o u 121 TIHE [sec] Figure2q. SD displacement, obtained from base run with g =O and El = 17.04PG 5 TrIE [secl Figure 25. q 1(t) displacement, obtained from the base run with g =0 and El = 17.04Pa 89

ci I -I -a -C -C ~-C -~ — C ia TIME [sec] Figure 26. q 12(t) displacement, obtained from the base run with g = O and EI = 17.04Pa 1 q I US, 2......,. U. TtlE [sec] Figure 27. q21(t) displacement, obtained from the base run with g =0 and El = 17.04P, 90

x cm ". ' 2...,. Ul Su TIME [sec] Figure 28. q22(t) displacement, obtained from the base run with g =0 2.0 4.3 TV I 01r6. IO1 ^TIME [sec] Figure 29. r displacement, obtained from the base run with g =0 and high servo loop frequencies 91

ed ^l 2JI 4I 649 UI ~.I tZe A = InE [sec] Figure 30. d displacement, obtained from the base run with g =O and high servo loop frequencies S9 d TIME [sec] Figure 31. <o displacement, obtained from the base run with g =O and high servo loop frequencies 92

^ S i, I r~ [sec] uTA [sec] Figure 33. ql2(t) displacement, obtained from the base r un with g =O and high servo loop frequencies q lllt E ^1?I

9 TInE [sec] Figure 34. q21(t) displacement, obtained from the base run with g =0 and high servo loop frequencies. C 1 'C fin ',!, 4 TItlE [sec] Figure 35. q22(t) displacement, obtained from the base run with g =0 and high servo loop frequencies. 94

inner loop for r + e6 J + ~ ~ - R1I U1 YI ~l d L C_ I Figure 36(a). BLOCK DIAGRAM for r only G'l(s) Rel l I ~,,Kit Figure 36(b). Simplified form of the block diagram for r 95

ju / / /?y a, 10. i -2.67 l o o Figure 37. Root locus for r 96

*l iX a zo.o8 0/ = ^,-1.54 Mr - 91.33 a -4 -2.67 Figure 38. Root locus for 6 97

Jiw // /6 = 1 I.4 = 12.3 Figure39 Root locus for 98 ^\

RSD-13-83 First Mode Second Mode Si 1.8751041 4.640911 ai 0.7340955 1.01846644 TABLE 1 Values of ao and Ei for a Clamped Free Beam Settling Damping Number of Time Constant Natural Time ratio frequency W, (2% error) (rad / s) r 1.0 1.0 4 4 1.0 1.0 4 4 l ] 0.5 1.0 4 8 TABLE 2 Desired System Parameters. Dynamic Simulation 99

RSD-13-83 Closed-loop Transfer Function Open-loop Transfer Function for the inner loop for the inner loop r 1 K{i aG(s) =s (m22 + p ) sM + mp W n7 n7 s (s G (1 ) - ( ^ 2 w., C(s) = 2 + 2t Wns + Wn G 3( ) = s 25 + + Wn) s +^ s (s22 + W^ Wd - ^K 2 ) 2- (1) K3 G3(s)- Ga'3( ) a TABLE 3 The Required Transfer Functions for Drawing the Root Locus. Dynamic Simulation 100

UNIVERSITY OF MICHIGAN 3 9015 02651 8327