"Applied Science, Faculty of"@en . "Mechanical Engineering, Department of"@en . "DSpace"@en . "UBCV"@en . "Chu, Mark Sin Tian"@en . "2009-03-25T17:38:05Z"@en . "1997"@en . "Master of Applied Science - MASc"@en . "University of British Columbia"@en . "This thesis focuses on design, dynamical simulation, operation and performance\r\nevaluation of a variable geometry manipulator used in space as well as ground based\r\noperations. The system is composed of a mobile base supporting two modules connected\r\nin a chain topology. Each module consists of two links: one free to slew while the other\r\nis permitted to deploy. The governing equations of motion for the planar dynamics of the\r\nmanipulator system are obtained using the Lagrangian procedure. A Fortran computer\r\nprogram is written for the dynamical simulation of the system. The accuracy of the\r\nformulation and the validity of the computer simulation code are verified through energy\r\nconservation tests. An extensive parametric study follows which provides better\r\nappreciation of the complex interactions between the system variables, initial\r\ndisturbances, manipulator maneuvers, as well as the revolute joints and pay load\r\nflexibility. It also identifies critical combinations of system parameters and maneuvers\r\nwhich may lead to an unacceptable response. Results indicate that the coupling effects\r\ntogether with the flexibility at the revolute joints have significant impact on the\r\nmanipulator's performance. Obviously, this will affect the desired trajectory tracking by\r\nthe end-effector, suggesting a need for a suitable control algorithm. This is achieved\r\nthrough the classical Proportional-Integral-Derivative (PID) control strategy. The design,\r\nconstruction and integration of a prototype manipulator system, which progressed\r\nconcurrently with the numerical simulation phase are also described in detail. Finally, the\r\nnumerically predicted results are compared with the prototype performance. The\r\nremarkable agreement tends to substantiate integrity of the prototype design and effective\r\nimplementation of the controller. The study lays a sound foundation for further\r\nexploration of this class of novel manipulators."@en . "https://circle.library.ubc.ca/rest/handle/2429/6466?expand=metadata"@en . "10677392 bytes"@en . "application/pdf"@en . "DESIGN, CONSTRUCTION AND OPERATION OF A VARIABLE GEOMETRY MANIPULATOR MARK SIN TIAN CHU B.A.Sc. in Mechanical Engineering, The University of British Columbia, 1995 A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF THE REQUIREMENT FOR THE DEGREE OF MASTER OF APPLIED SCIENCE in THE FACULTY OF GRADUATE STUDIES DEPARTMENT OF MECHANICAL ENGINEERING We accept this thesis as conforming to the required standard THE UNIVERSITY OF BRITISH C O L U M B I A October 1997 \u00C2\u00A9 Mark Sin Tian Chu, 1997 In presenting this thesis in partial fulfillment of the requirements for an advanced degree at the University of British Columbia, I agree that the Library shall make it freely available for reference and study. I further agree the permission for extensive copying of this thesis for scholarly purposes may be granted by the head of my department or by his or her representatives. It is understood that copying or publication of this thesis for financial gain shall not be allowed without my written permission. The University of British Columbia Department of Mechanical Engineering 2324 Main Mall Vancouver, B.C., Canada V6T 1Z4 Date: ABSTRACT This thesis focuses on design, dynamical simulation, operation and performance evaluation of a variable geometry manipulator used in space as well as ground based operations. The system is composed of a mobile base supporting two modules connected in a chain topology. Each module consists of two links: one free to slew while the other is permitted to deploy. The governing equations of motion for the planar dynamics of the manipulator system are obtained using the Lagrangian procedure. A Fortran computer program is written for the dynamical simulation of the system. The accuracy of the formulation and the validity of the computer simulation code are verified through energy conservation tests. An extensive parametric study follows which provides better appreciation of the complex interactions between the system variables, initial disturbances, manipulator maneuvers, as well as the revolute joints and pay load flexibility. It also identifies critical combinations of system parameters and maneuvers which may lead to an unacceptable response. Results indicate that the coupling effects together with the flexibility at the revolute joints have significant impact on the manipulator's performance. Obviously, this will affect the desired trajectory tracking by the end-effector, suggesting a need for a suitable control algorithm. This is achieved through the classical Proportional-Integral-Derivative (PID) control strategy. The design, construction and integration of a prototype manipulator system, which progressed concurrently with the numerical simulation phase are also described in detail. Finally, the numerically predicted results are compared with the prototype performance. The remarkable agreement tends to substantiate integrity of the prototype design and effective implementation of the controller. The study lays a sound foundation for further exploration of this class of novel manipulators. ii TABLE OF CONTENTS A B S T R A C T i i T A B L E OF CONTENTS i i i LIST OF S Y M B O L S . . vi LIST OF FIGURES x LIST OF TABLES . xvii A K N O W L E D G E M E N T xviii 1. INTRODUCTION 1 1.1 Preliminary Remarks . . . . . . 1 1.2 A Brief Review of the Relevant Literature . . . 4 1.3 Scope of the Investigation . . . . . 11 2. THE PROTOTYPE ROBOTIC MANIPULATOR 15 2.1 General Description . . . . . . 15 2.2 Design Specifications. . . . . 15 2.3 Construction and Integration . . . . . 18 2.3.1 Manipulator base . . . . . 18 2.3.2 Manipulator module . . . . . 21 2.3.3 Elbow joint . . . . . . 21 2.4 Hardware and Software Interface . . . . 23 2.4.1 The computer system . . . . . 24 2.4.2 Motion controller board . . . 24 2.4.3 Power amplifier. . . . . . 25 2.4.4 DC servo motors . . . . . 25 2.4.5 Optical encoder . . . . . 25 2.4.6 The control program . . . . . 26 3. FORMULATION OF THE PROBLEM . 29 3.1 Preliminary Remarks . . . . . . 29 3.2 Kinematics of the System . . . . . 30 3.2.1 Reference frames . . . . . 30 3.2.2 Modelling of joint flexibility . . . . 30 iii 3.2.3 Modal function and boundary conditions for the flexible payload . . . . . 33 3.2.4 Position vectors . . . . . 34 3.2.5 Velocity vectors . . . . 36 3.2.6 Generalized coordinates . . . . 37 3.3 Kinetic Energy of the System. . . . . 38 3.4 Potential Energy of the System . . . . 38 3.4.1 Gravitational potential energy. . . . 38 3.4.2 Strain energy . . . . . . 40 3.5 Governing Equations of Motion . . . . 40 3.6 Specified Coordinates. . . . . . 41 4. COMPUTER SIMULATION 43 4.1 Preliminary Remarks . . . . . . 43 4.2 Structure of the Computer Code . . . . 43 4.3 Validation of the Formulation and Computer Code . . 46 5. DYNAMICAL RESPONSE TO INITIAL DISTURBANCES AND SPECIFIED MANEUVERS 56 5.1 Preliminary Remarks . . . . . 56 5.2 Response to Initial Disturbances . . . . 58 5.3 Manipulator with Macro and Micro Modules. . . 72 5.4 Specified Maneuvers . . . . . . 83 5.4.1 Slewing maneuvers . . . . . 86 5.4.2 Deployable maneuvers . . . . 91 5.4.3 Complex maneuver . . . . . 96 6. SIMULATION OF CONTROLLED BEHAVIOUR 102 6.1 Preliminary Remarks . . . . . . 102 6.2 Design of a Proportional-Integral-Derivative (PID) Controller 103 6.2.1 Structure of the control system . . . 103 6.2.2 Controller tuning . . . . . 104 6.3 System Response to Maneuvering Commands . . 109 iv 7. CONTROLLED BEHAVIOUR OF THE PROTOTYPE MANIPULATOR 120 7.1 Preliminary Remarks . . . . . . 120 7.2 Performance of the Prototype Manipulator with PID Control 120 8. CONCLUDING REMARKS 132 8.1 Contributions . . . . . . . 132 8.2 Conclusions . . . . . . . 132 8.3 Recommendations for Future Work . . . . 133 LIST OF REFERENCES 135 APPENDIX I: COMPONENTS AND CHARACTERISTICS OF THE PROTOTYPE MANIPULATOR 139 APPENDIX II: DESIGN AND ASSEMBLY DRAWINGS OF THE PROTOTYPE MANIPULATOR 145 APPENDIX III: KINETIC ENERGY 200 APPENDIX IV: POTENTIAL ENERGY 209 APPENDIX V: COMPUTER SOFTWARE SOURCE CODES 212 v LIST OF SYMBOLS A system matrix used for controller design, Eq. (6.1) A C acceleration limit parameter A; local transformation describing the position and rotation of the frame Fj with respect to the frame F M , Eq. (3.3) B input distribution matrix, Eq. (6.1) Bj j , h body, Fig. 3-4 C output distribution matrix, Eq. (6.1) Cj transformation matrix for the mass element drrij with respect to the inertial frame, Eq. (3.7) Cja position vector to the mass element dmj with respect to the frame F j 5 Eq. (3.7) Cjg inertial transformation matrix for the body frame Fj, Eq. (3.7) DC deceleration limit parameter Dj position vector of the frame F j 5 Fig. 3-4 dj linear displacement of module j with respect to the frame F j , j = 0,1,2; Fig. 3-1 drrii mass element located in the i t h body, Fig. 3-4 Dp position vector of the payload with respect to the inertial frame, Fig. 3-3 EI flexural rigidity of the payload ER position error limit parameter F 0 inertial frame, Fig. 3-1 Fj body attached frame at the link j g Earth's gravitational acceleration Hj matrix containing inertial rotation and position of the frame F j 5 Eq. (3.3) K D Derivative gain of the prototype manipulator control K d derivative gain matrix used for the controller design KI Integral gain of the prototype manipulator control vi Kj integral gain matrix used for the controller design Kj torsional spring constant of joint j , Fig. 3-2 K P proportional gain of the prototype manipulator control K p proportional gain matrix used for the controller design K u ultimate gain le length of the elbow joint Idj length of the deployable link of module j lp half-length of the payload lrj length of the rotor shaft at the slewing joint j lsj length of the slewing link of the module j M system mass matrix, Eq. (3.11) mi, mass of the mobile base me mass of the elbow joint Mm2/Mmi mass ratio of the micro and macro modules of the manipulator OE automatic error shut-off P u ultimate period Q vector of generalized forces associated with generalized coordinates, Eq. (3.14). q vector of generalized coordinates, Eq. (3.8) r position vector to the payload mass element with respect to the body frame of the payload, Fig. 3-3 R d m j position vector of the mass element drtij with respect to the inertial frame, Fig. 3-4 Rj rotation matrix of the body frame Fj with respect to the inertial frame, Eq. (3.2) Tj position vector of the mass element dmj with respect to the body frame Fj, Eq. (3.2) S distribution matrix that assigns constraint functions to the specified coordinates, Eq. (3.16) SP speed limit parameter vii T total kinetic energy, Eq. (3.10) t time TL torque limit parameter T q vector containing the partial derivatives of the kinetic energy with respect to the generalized coordinates, Eq. (3.15) U total potential energy u vector of the input position (rotor of the servo-motors), Eq. (6.1) ug strain energy, Eq. (3.12) u q vector containing the partial derivatives of the potential energy with respect to the generalized coordinates, Eq. (3.15) v(l,t) tip deflection of the payload, Fig. 3-3 v(x,t) deflection of the payload mass element along the Y 5-axis, &(x)5(t), Fig. 3-3 X vector of the degrees of freedom of the system, Eq. (6.1) X position of the mass element along the X axis, Fig. 3-3 y vector of the output position (manipulator joints), Eq. (6.1) Greek Symbols aj rigid component of the slewing angle for module j , Fig. 3-2 Pj flexibility contribution of joint j , Fig. 3-2 Yj orientation of the frame Fj with respect to the inertial frame F 0 , Eq. (3.2) S(t) generalized coordinate associated with the beam vibration, Fig. 3-3 Oj angular displacement of the module j with respect to the frame Fj, Fig. 3-2 A matrix of Lagrange multipliers, Eq. (3.15) Pj linear mass density of the body j 0(X) first mode of a cantilever beam, Fig. 3-3 j angle between the position vector R d m j and the local vertical Y 0 , Fig. 3-5 viii Abbreviations, Acronyms D.C. Direct Current ENC Optical Encoder i . e . Initial Conditions FLT Feedback Linearization Technique IRIS Institute of Robotics and Intelligent Systems L Q R Linear Quadratic Regulator L T R Loop Transfer Recovery L R M S Long Reach Manipulator System M D M Mobile Deployable Manipulator M F D M Mobile Flexible Deployable Manipulator MSS Mobile Servicing System N C D Nonlinear Control Design PD Proportional-Derivative PID Proportional-Integral-Derivative SPDM Special Purpose Dexterous Manipulator SSRMS Space Station Remote Manipulator System LIST OF FIGURES 1-1 An artist view of the proposed space station based Mobile Servicing System (MSS) 3 1-2 Dexterous manipulator performing obstacle (ring) avoidance maneuver. 5 1-3 Schematic diagrams of the Mobile Flexible Deployable Manipulator (MFDM) with modules of slewing and deployable links: (a) a single module system; (b) multimodule configuration that can change its shape in the snake-like fashion to accommodate environmental constraints in the form of obstacles. . . . 6 1-4 Schematic diagrams of space structure models: (a) Lips [8], rigid spacecraft with deployable beam-type members; (b) Ibrahim [9], rigid spacecraft with deployable beam- and plate-type members; (c) Shen [10], rigid spacecraft with slewing-deployable appendages; (d) Marom [11], flexible spacecraft with one slewing-deployable manipulator module and a rigid payload at the deployable end. . . . . . . . 8 1 -5 Flexible two-link manipulator. . . . . . 10 1 -6 Macro-Micro manipulator system. . . . . . 10 1-7 Configuration of the two-module mobile manipulator, with revolute (flexible) and prismatic joints, considered in the present study. . 12 1-8 Overview of the plan of study. . . . . . 13 x 2-1 A manipulator with two modules of slewing and deployable links undergoing dynamics and control tests. . . . . 16 2-2 Basic configuration of the two-module manipulator. . . 17 2-3 Schematic representation of the two-module manipulator system showing major components. . . . . . . 19 2-4 Schematic representation of the manipulator base assembly. . 20 2-5 Prismatic joint mechanism providing deployment and retrieval. . 22 2-6 Schematic representation of the elbow joint assembly. . 22 2-7 Servo-control system configuration. . . . . . 23 2- 8 Flow-chart of the prototype manipulator control program. . . 28 3- 1 Schematic representation of the manipulator model with its reference frames and generalized coordinates. . . . 31 3-2 Representation of a flexible revolute joint and relative slew angles. 32 3-3 Planar deformation of a flexible beam-type payload. . . 33 3-4 Position vector to an arbitrary mass element on the body j . . . 35 3- 5 Position of a mass element in the gravitational field. . . 39 4- 1 Modular architecture of the computer code. . . . . 44 4-2 Free oscillation of the rigid manipulator model with initial conditions as 9t = 80\u00C2\u00B0, 02 = O. . . . . . 47 xi 4-3 Time histories of the system parameters during the energy conservation test: Case 1; 9,(0) = 80\u00C2\u00B0, 62(0) = 0. . . 48 4-4 Time histories of the system parameters during the combined slewing disturbance of 0,(0) = 45\u00C2\u00B0, 02(O) = 45\u00C2\u00B0. . . . 49 4-5 Response of the system to an impulsive disturbance of 9X (0) = 1807s. Note, the error in the conservation of energy remains insignificant. . . . . . . . 51 4-6 Response of the system to severe impulsive disturbances: 6X (0) = 1807s; 62 (0) = -1807s. Conservation of energy attests to the validity of the formulation and the numerical integration code. . 52 4-7 Time histories of the system response accounting for flexibility at the revolute joints and the payload. . . . . . 54 4- 8 Response of the flexible components of the manipulator system subjected to an initial disturbance of Oi = 02 = 45\u00C2\u00B0: /?/ = vibration of flexible joint 1; fa = vibration of flexible joint 2; v = tip deflection of the payload. . . . . . . 55 5- 1 Response of the rigid manipulator system to an initial impulsive angular disturbance given to module 1 (Case A): (a) time histories of displacements and conservation of energy; . 60 (b) time histories of generalized velocities and accelerations. . 61 5-2 Response of the rigid manipulator system with an impulsive disturbance imparted to module 2 (Case B): (a) displacement time histories and conservation of energy; . 62 (b) velocity and acceleration time histories. . . . 63 xii 5-3 Response of the system during a combined slewing and deployment maneuver (Case C): (a) displacement time histories and energy conservation; . . 65 (b) velocity and acceleration profiles.. . . . . 66 5-4 Response of the rigid manipulator system to a demanding disturbance involving base motion and slewing velocities at the joints (Case D): (a) time histories of generalized coordinates and energy conservation; 67 (b) time histories of generalized velocities and accelerations. . 68 5-5 Response of the manipulator to rotational and translational disturbances in presence of a flexible payload (Case E): (a) displacement time histories; . . . . . 70 (b) time histories of generalized velocities and accelerations. . 71 5-6 A special case of the manipulator with macro- and micro-modules. 73 5-7 Configurational time history of the manipulator with a mass ratio Mm2/Mmi equal to: (a) 1; 74 (b) 0.5; 76 (c) 0.25; 77 (d) 0.1. 78 5-8 Configurational time history showing coupling effects when the initial impulsive disturbance is applied to module 2: (a) Mm2/Mml = \; 79 (b) Mm2/Mml = 0.5; 80 (c) Mm2/Mml = 0.25; 81 (d) Mm2/Mml = 0.\ 82 xiii 5-9 System response to an acceleration input profile (i.e. 92) given to the revolute joint at the elbow: (a) step function; . . . . . . . 84 (b) sinusoidal function. . . . . . . 85 5-10 (a) Specified slewing maneuver of 120\u00C2\u00B0 executed by module 2 with a sinusoidal acceleration profile; . . . . . 87 (b) Time histories of the response and generalized forces during the 120\u00C2\u00B0 slewing maneuver of module 2. . . . . . 88 5-11 (a) Specified slewing maneuver of 120\u00C2\u00B0 executed by module 1 with a sinusoidal acceleration profile; . . . . . 89 (b) Time histories of the response and generalized forces during the 120\u00C2\u00B0 slewing maneuver of module 1.. . . . . 90 5-12 (a) System configuration during the specified maneuver of 0.6 to 1.2 m executed by module 2 with a sinusoidal acceleration profile; 92 (b) Response of the generalized coordinates and forces during the deployment maneuver at module 2. . . . . . 93 5-13 (a) Deployment maneuver at module 1 from 0.6 m to 1.2 m in 8 s and associated geometry of the manipulator; . . . 94 (b) Time variation of generalized coordinates and forces during the deployment maneuver at module 1. . . . . . 95 5-14 Complex maneuver involving loading and unloading of a beam-type payload: (a) initial conditions and schematic diagrams showing the sequence of operations; . . . . . . . 98 (b) time histories of specified and generalized coordinates; . . 99 (c) variation of the generalized forces. . . . . 100 xiv 6-1 Structure of the control system. . . . . . 104 6-2 System response to a step-input with the ultimate control gain K u . . 105 6-3 System response to a step-input with the Ziegler-Nichols control gains. 107 6-4 System response to a step-input with the N C D toolbox control gains. 108 6-5 Comparison between desired and actual trajectories during a 120\u00C2\u00B0 slewing maneuver at module 1: (a) joint trajectories; . . . . . . . 110 (b) end-effector trajectories. . . . . . I l l 6-6 Comparative performance during a 120\u00C2\u00B0 slewing maneuver at module 2: (a) joint trajectories; . . . . . . . 112 (b) end-effector trajectories. . . . . . 113 6-7 Path followed by the end-effector during a 120\u00C2\u00B0 slewing maneuver of module 1 and module 2. . . . . . . 114 6-8 Schematic diagram showing a straight-line tracking with two revolute joints. . . . . . . . 115 6-9 Tracking of a straight-line using two revolute joints. Note, the module lengths are held fixed at 1 m. Two different scales along the X axis are used to emphasize the error. . . . . 116 6-10 Trajectory tracking of a horizontal straight-line using two prismatic joints: (a) schematic diagram showing the maneuver; (b) comparative study of prescribed and actual end-effector trajectories during tracking. . . . . . . . 118 X V 6- 11 Trajectory tracking using a module having two joints, one prismatic and the other revolute: (a) schematic diagram showing the maneuvers for a horizontal straight-line tracking; (b) discrepancy between the actual and prescribed paths. . . 119 7- 1 Flow charts for the control system: (a) system motion control elements; (b) controller functional elements. . . . 121 7-2 A comparative study of system responses to a 120\u00C2\u00B0 slew maneuver at module 1: (a) joint trajectory; . . . . . . . 122 (b) end-effector trajectory. . . . . 123 7-3 Comparison of experimental and simulated results for a 120\u00C2\u00B0 slew maneuver at module 2: (a) slew time histories; . . . . . . 125 (b) end-effector trajectories. . . . . . . 126 7-4 Path followed by the end-effector during a 120\u00C2\u00B0 slewing maneuver simultaneously carried out by modules 1 and 2. . . . 127 7-5 Accuracy of a horizontal straight-line tracking maneuver using the two revolute joints of the system. . . . . . 128 7-6 Straight-line tracking with prismatic joints. . . . . 130 7-7 Straight-line tracking using revolute and prismatic joints of a single module. . . . . . . . . 131 xvi LIST OF TABLES 5-1 System parameter and initial conditions used during uncontrolled dynamic simulations. . . . . . . 56 5- 2 Cases studied and the associated initial conditions to assess dynamical performance of the system. . . . . 58 6- 1 PID control gains obtained using the Ziegler-Nichols method. . 106 6-2 PID control gains obtained using the N C D toolbox. . . . 106 xvii ACKNOWLEDGEMENT I wish to thank Professor V.J . Modi for his teaching, guidance and friendship. His constant encouragement took me on an incredible journey of experience and learning throughout my graduate program. Thanks are also due to Professor C.W. de Silva for his support and advice towards the completion of my research project. Then, of course, there are my colleagues and friends: Dr. Satyabratha Pradhan, Dr. Sandeep Munshi, Dr. Mae Seto, Mr. Yuan Chen, Mr. Gary Lim, Mr. Mathieu Caron, Mr. Spiros Kalantzis, Mr. Gabriel Gilardi, Mr. Yang Cao, and Dr. Seiya Ueno. Thank you for allowing me to draw upon your expertise, cooperation and kindness. You have been members of a most outstanding team I have come across. This project was supported by the Natural Sciences and Engineering Research Council of Canada Grants No. 5-82181 and 5-82268. xviii To my parents, Kin Loy Chu and Siau Wan Sam, for being the source of my strength, happiness, and love. 1. INTRODUCTION 1.1 Preliminary Remarks In the 1960's, two superpowers competed for the prestige of being the first to place a man on the moon. Today, a similar race driven by technological advances is underway among the nations. The objective, however, is to gain the control of an international robotics market over the next few decades. In the last ten years, robotics systems have matured from laboratory demonstrations to become high speed and precision tools in almost all industrial fields. They have reached the point where commercially viable applications are not just futuristic concepts, but hard realities based on available capabilities [1]. A few key features that make robotics systems attractive to industrial environment are: decreased labor costs; increased precision and productivity; and production flexibility. The early robots were created using the combined capabilities of teleoperators and numerical controlled milling machines. With the rapid growth of the robotic industry, standards for design, control and planning were created to facilitate compatibility, information exchange and technology transfer. A widely used convention for kinematics study is the Denavit-Hartenberg notation [2,3] adopted by most academic as well as industrial groups. There is a vast number of standard robots used in the field today. Among them, one can name a few representatives [3] as: \u00E2\u0080\u00A2 The Unimate Robot, installed for the fist time in 1961, in a Trenton, New Jersey plant of General Motors; \u00E2\u0080\u00A2 The Stanford Arm, developed at Stanford University in 1971; \u00E2\u0080\u00A2 The Cincinnati Milacron T 3 with computer control, introduced in 1974; \u00E2\u0080\u00A2 The P U M A Robot, developed by Unimation in 1978; \u00E2\u0080\u00A2 The S C A R A Robot, designed in Japan in 1979. 1 In the future, besides being integral components of our industries, robotic systems will be expected to perform important tasks in space, in orbit around the Earth as well as in planetary exploration. Such applications introduce technical problems not encountered by conventional ground-based robots. To develop efficient and practical systems for meeting the requirements of future ground and space missions, substantial technical development is required in the areas of design, dynamical formulations, control, and computational methodologies. N A S A is currently conducting the Space Telerobotics Program to construct a set of integrated techniques aimed at the understanding of the fundamental mechanics of space robotics. The Long Reach Manipulators Systems (LRMS) are being considered as a promising solution for performing remotely operated servicing tasks in space. The proposed Special Purpose Dexterous Manipulator (SPDM), mounted on the Space Station Remote Manipulator System (SSRMS), is an example of a long reach robot under development. While having significant potential, the use of L R M S requires the solution of some fundamental control problems that can seriously limit their performance. External disturbances and the motion of the manipulator itself can excite vibration of the structure. Although control algorithms have been developed to solve these problems, they are yet to be tested on ground as well as in space. Canada is at the leading edge of this emerging robotics and space manipulator technology. Its leadership is evident with the development of the Canadarm used abode the Space Shuttle orbiter. The Mobile Servicing System (MSS) represents evolution of the Canadarm. It will be used for the construction, operation and maintenance of the proposed International Space Station Alpha. The MSS, presented as an artist view in Figure 1-1, can also act as platform for astronauts, handle cargo transfer, deploy and retrieve satellites, and assist the Space Shuttle during docking as well as separation maneuvers. 2 Figure 1-1 An artist view of the proposed space station based Mobile Servicing System (MSS). 3 Conventional manipulator designs, like the one shown in Figure 1-2, solely use revolute joints and fixed length links to emulate the movements of the human arm. The robot demonstrates capability to reach its target while avoiding an obstacle. A quick survey of the field of robotics suggests that dynamics and control of manipulators, with revolute joints, have been studied rather extensively, resulting in a vast body of literature. On the other hand, manipulators involving prismatic joints have received relatively less attention. This, indeed, is surprising as manipulators with a combination of revolute and prismatic joints present several attractive features. For example, a module of the two link manipulator with one prismatic and one revolute joint, as shown in Figure l-3(a), has: (i) unique positions for the links to reach a specified location in space, thus avoiding the need for a decision making algorithm; (ii) reduced inertia coupling leading to simpler equations of motion and inverse kinematic relations; (iii) better facility to overcome obstacles; (iv) no singular position. Integration of several modules, as indicated in Figure l-3(b), would result in a snakelike, reconfigurable manipulator. It retains advantages mentioned in (ii) and (iii), with a reduced number of singular orientations compared to a revolute joint based system having the same number of degrees of freedom. 1.2 A Brief Review of the Relevant Literature As can be expected, the literature in the general area of robotics is literally overwhelming and it is not intended here to review this aspect. The objective here is to briefly touch upon contributions directly relevant to the investigation undertaken in the thesis. Thus the focus is on research activities involving systems with deployable members, and the role of the present investigation in this exciting, evolving field. 4 Figure 1-2 Dexterous manipulator performing obstacle (ring) avoidance maneuver. 5 Figure 1-3 Schematic diagrams of the Mobile Flexible Deployable Manipulator (MFDM) with modules of slewing and deployable links: (a) a single module system; (b) multimodule configuration that can change its shape in the snake-like fashion to accommodate environmental constraints in the form of obstacles. 6 Dynamics of structures with flexible deployable members has received some attention in space technology, where weight is an important criterion in the design. Lightweight deployable structural appendages such as solar panels, radiators, antennas, booms, etc. are routinely used by spacecraft. Lips and Modi [4,5] have studied at length dynamics of spacecraft with rigid central body connected to deployable beam-type members. Modi and Ibrahim [6] presented a relatively general formulation for this class of problems involving a rigid body supporting deployable beam- and plate-type members. Subsequently Modi and Shen extended the study to account for deployment as well as slewing of the appendages [7]. Lips [8], Ibrahim [9] and Shen [10] have reviewed this aspect of the literature in some detail. In the above mentioned studies [4-10], although slewing and/or deployable members were involved, each appendage was directly connected to the central body, i.e. a manipulator-type chain geometry of the links (appendages) was not involved. Figure 1-4 shows schematically the different models described above. The new manipulator system schematically shown in Figures 1-3 (a) and l-4(d) was first proposed for space application by Marom and Modi [11]. Planar dynamics and control of the one unit mobile manipulator with flexible joints, located on an orbiting flexible platform, were investigated. Results showed significant coupling effects between the platform and the manipulator dynamics. Control of the system during tracking of a specified trajectory, using the computed torque technique, proved to be quite successful. Modi et al. [12] as well as Ffokamoto et al. [13-15] extended the study to the multimodule configuration, referred to as the Mobile Deployable Manipulator (MDM) system. The model with an arbitrary number of modules accounted for the joint as well as link flexibility. A relatively general formulation for three-dimensional dynamics of the system in orbit was the focus of the study by Modi et al., while Hokamoto et al. explored a free-flying configuration. More recently, Hokamoto et al. [16] studied control of a single unit system and demonstrated successful tracking of a straight-line trajectory at right angle to the initial orientation of the manipulator. 7 Figure 1-4 Schematic diagrams of space structure models: (a) Lips [8], rigid spacecraft with deployable beam-type members; (b) Ibrahim [9], rigid spacecraft with deployable beam- and plate-type members; (c) Shen [10], rigid spacecraft with slewing-deployable appendages; (d) Marom [11], flexible spacecraft with one slewing-deployable manipulator module and a rigid payload at the deployable end. 8 A comment concerning a rather comprehensive study by Caron [17] would be appropriate. He has presented an 0(N) formulation for studying planar dynamics and control of such formidable systems. The dynamical parametric study [18] clearly shows involved interactions between flexibility, librational dynamics, and manipulator maneuvers. Furthermore, Caron [17] successfully demonstrated control of a single module (i.e. two links) manipulator, free to traverse a flexible platform, using the Feedback Linearization Technique (FLT) in rigid degrees of freedom, and suppression of flexible members' response through the Linear Quadratic Regulator (LQR). Recently, Pradhan et al. [19] reported an elegant procedure to the Order N formulation for three-dimensional motion of a system, in tree topology, using the Lagrangian approach. A number of manipulator prototypes have been built to test on ground different control strategies which may eventually be implemented in space- and ground-based working environments. In 1989 Schmitz and Ramey [20] attempted vibrations control of a two-links flexible manipulator prototype using linear active modal controller (PD) and the discrete LQR/LTR procedure. In the early 1990's, Yoshikawa et al. [21] performed experimental work in vibration control and trajectory tracking utilizing a Macro-Micro manipulator system. Bridges et al. [22] also carried out numerical simulations and experiments on control of manipulators with flexible-joints and harmonic drive gearing. During this period, Lin and Patel [23] experimented with a redundant flexible-joint manipulator and introduced the hybrid Cartesian-joint control strategy, which combines link as well as joint tracking controllers. The prototypes studied by the above researchers use mainly revolute joints in their design, similar to the ones shown in Figures 1-5 and 1-6. As in the case of space application, manipulators tested during ground-based experiments did not use prismatic joints. Only recently, modelling, simulation and control of elastic robot arms with prismatic joints were studied by Miiller et al. [24,25] and Soffker et al. [26-29]. However, prototype manipulators utilizing prismatic as well as revolute joints are still rare. 9 10 1.3 Scope of the Investigation With the above as background, the present study focuses on dynamical performance of a variable geometry manipulator during ground-based operations. It represents a particular case of the orbiting system studied by Caron [17]. The system is composed of a mobile base, free to traverse in a straight-line, supporting two modules (i.e. four links) connected in a chain topology as shown in Figure 1-7. As indicated before, each module consists of two links: one free to slew while the other is permitted to deploy. The links are considered to be rigid while the two revolute joints are treated as flexible. This closely simulates a prototype constructed to assess dynamics of the manipulator as affected by the system parameters, to assist in its design, as well as effectiveness of various control strategies. The plan of study showing different stages of the investigation is presented in Figure 1-8. Chapter 2 discusses the development of the prototype manipulator, currently operational in the IRIS Spacecraft Control Laboratory. Special features of the design as well as the construction and integration processes are described at length. Formulation of the equations of motion is given in Chapter 3. The governing equations for the manipulator dynamics are derived following the Lagrangian approach [30]. The position of the mobile base, the angles of rotation of the revolute joints, the deployable lengths of the links controlled by the prismatic joints, as well as deflection of the flexible payload are taken as generalized coordinates. The trajectory of these coordinates can be specified by introducing constraint relations associated with the Lagrange multipliers. Chapter 4 focuses on the development of a FORTRAN computer code to integrate the equations of motions for numerical simulations. The formulation of the problem and the simulation routine were validated through conservation of energy tests. 11 Arbitrary Payload Figure 1-7 Configuration of the two-module mobile manipulator, with revolute (flexible) and prismatic joints, considered in the present study. DESIGN, CONSTRUCTION AND OPERATION OF A VARIABLE GEOMETRY MANIPULATOR Design, Construction and Integration of the Prototype Manipulator System Model and Equations of Motion Hardware and Software Interface; C++ Computer Program Performance Tests on the Integrated System and Individual Components Numerical Simulation; FORTRAN Integration Program Validation of the Equations of Motion; Verification of the Simulation Program PID Control and Gain Determination PID Controller Design Parametric Study; Dynamical Behaviour Trajectory Tracking Maneuvers Simulation of Controlled Trajectory Tracking Maneuvers Free Response to Initial Conditions Specified Maneuvers Correlation of Numerical Simulations with Experimental Data Summary of Conclusions; Recommendations for Future Work Figure 1-8 Overview of the plan of study. Dynamical response of the manipulator system to initial conditions or disturbances is explored in Chapter 5. It also studies an interesting case of interactions between macro- and micro-manipulator units. Results of the parametric study involving translational, slewing, deployment and combined complex maneuvers provide better appreciation of the coupling effects. With some comprehension of the system dynamics, the attention is turned to the design of a Proportional-Integral-Derivative (PID) controller for trajectory tracking (Chapter 6). The controlled response is studied to assess the effect of various system and controller parameters. Next, the ground-based, prototype manipulator is operated with the PID control. Maneuvers identical to those simulated in Chapter 6 are carried out by the prototype. The correlation of these experimental results and the numerical simulation data is presented in Chapter 7. Finally, Chapter 8 summarizes important results and conclusions based on them, lists main contributions, and provides recommendations for further studies. 14 2. THE PROTOTYPE ROBOTIC MANIPULATOR 2.1 General Description This chapter presents the design, construction and integration of the prototype robotic manipulator. The objective is to develop a working system for real-time tests and control implementation. Figure 2-1 shows the integrated and operational manipulator presently located in the IRIS Spacecraft Control Laboratory. The basic configuration of the manipulator is shown in Figure 2-2. It consists of a mobile base supporting two manipulator modules (i.e. four links) connected in series. Each module has two links: one is able to slew or rotate while the other is allowed to deploy and retract. The workspace of this manipulator system resembles contour of a human heart, extending 2.0 m from top to bottom and 2.5 m across. The rotational motions are achieved with revolute joints actuated by DC servo-motors. The deployment and retrieval of links are obtained with prismatic joints involving leadscrews and roller nuts assembly, which convert the rotational motion of the motors into linear displacements. The actuator motors and optical encoder sensors are connected to the SDK-1000 controller board which interfaces the robotic manipulator with a Pentium Pro 200 M H z PC. 2.2 Design Specifications The main design features established in the early stages of this project were: Slewing arm length 0.3 m; Maximum extension of the deployable arm 0.2 m; Slewing arm sweep range -135 to 135 deg; Maximum rotational speed 60 deg/s; Maximum deployament speed 0.04 m/s. Minimization of the manipulator weight was an important consideration during the design process. Any additional mass of the system would result in greater inertia 15 Figure 2-1 A manipulator with two modules of slewing and deployable links undergoing dynamics and control tests. 16 Figure 2-2 Basic configuration of the two-module manipulat* 17 thus adversely affecting its performance. In addition to the above criteria, the mechanical and electronic components were chosen based on their performance characteristics like speed, accuracy, reliability and robustness. Appendix I gives the details of the components and equipment utilized in the construction of the manipulator. Custom made parts like the base, links and joint supports were constructed out of Aluminum 6061 for its light weight (Density = 2710 kg/m3), strength (Yield Strength = 255 MPa) and highly machineable character [31]. A schematic representation of the overall assembly, displaying the main components of the system, is presented in Figure 2-3. 2.3 Construction and Integration The construction and integration of the prototype manipulator was successfully accomplished with the aid of highly qualified technicians from the machine shop and the electronic laboratory of the Department of Mechanical Engineering. Working in collaboration with technicians, a systematic construction procedure was established. The following subsections discuss in detail integration of the manipulator components. The final version of the design and assembly drawings are included in Appendix II. 2.3.1 Manipulator base The base primarily consists of a cubic structure which serves as support and slew platform of the manipulator system. Figure 2-4 shows the schematic of this setup. The manipulator module is attached to the pivot plate which is threaded to the pivot shaft. A 80 mm thrust bearing located between the pivot plate and the top plate of the base carries the load of the manipulator weight as well as provides the slewing freedom about the rotational axis. A second bearing is located under the top plate of the base. A lock nut under the second bearing is used to secure the thrust bearing, the pivot plate and the base together. A flexible coupling connects the pivot shaft to a gear head of 1:20 ratio which increase the torque delivered by the DC servo-motor. Slewing motion occurs at the 18 Slew Platform Slew Link Joint Optical Encoder Module 2 Base Sensory Feedback CONTROL UNIT (Computer) Interface Unit Figure 2-3 Schematic representation of the two-module manipulator system showing major components. Module 1 Slewing j Link 1 Flexible Coupling DC Servo-Motor Pivot Plate Deployable Link 1 Axial Bearing Flexible Coupling Lock Nut Pivot Shaft Gear Head (1:20) Thrust Bearing Top Plate of the Base Base Housing DC Servo-Motor Optical Encoder Figure 2-4 Schematic representation of the manipulator base assembly. 20 revolute joint as the rotation of the motor is transmitted through the gear box, the flexible coupling, the pivot shaft, and finally to the pivot plate holding the slew end of the manipulator. The entire base structure is mounted on a wheeled carriage capable of linear motion along a straight-line. 2.3.2 Manipulator module As mentioned before, the two modules composing the manipulator are identical. Each module consists of a slewing link and a deployable link as shown in Figure 2-2. The free end of the slewing link of module 1 sits on the pivot plate of the base, while the free end of the deployable link holds the pivot base of the elbow joint. In the case of module 2, the free end of its slewing link sits on the pivot plate of the elbow joint, while the free end of the deployable link serves as junction between the manipulator and its end-effector. Figure 2-5 shows a linear axial bearing (INA KNZ24PP) connecting the slewing and deployable links at a prismatic joint. Deployment occurs when a roller nut, fixed to the deployable link, is driven axially by the rotation of the lead screw [31]. The pitch of the lead screw is 2.5 mm, i.e., the deployable link moves 2.5 mm for each revolution. A flexible coupling connects the lead screw to the motor located on the pivot plate of the base. 2.3.3 Elbow joint The elbow joint is the junction that connects the deployable link of module 1 and the slewing link of module 2. It consists of two pivot base plates (top and bottom) bolted onto the deployable end of module 1. The base plates serve as housing for the slewing motor and the gear head as well as support platform for the pivot plates of module 2. The two pivot plates which hold the deployable motor and the slewing end of module 2 are attached to the base plates by two shafts. The shafts sandwiches the pivot plates, deep groove bearing and base plate together. As shown in Figure 2-1, the mechanism that 21 Flexible Coupling Slewing Link Deployable Link Lead Screw Linear Axial Bearing Roller Nut Figure 2-5 Prismatic joint mechanism providing deployment and retrieval. Slewing End of Module 2 Flexible Coupling Pivot Base Plate Deployable End of Module 1 Pivot Plate Pivot Base Deep Groove Bearing Flexible Coupling Motor Optical Encoder Figure 2-6 Schematic representation of the elbow joint assembly. 22 provides the slewing motion at the elbow joint is identical to the one located at the mobile base of the manipulator. Figure 2-6 presents the schematic of the elbow joint assembly. 2.4 Hardware and Software Interface The motion control components of the manipulator includes an I B M compatible host computer, Galil DMC-1040 controller board, power amplifier, and DC servo-motors with optical encoders. Figure 2-7 shows the setup of the servo-control system. The system components were identified, as well as servo-control configuration was determined and saved using the SDK1000 executable command provided by Galil Motion Control, Inc., California, U.S.A. Computer Controller Board 60 Pin Ribbon Cable Amplifier Signal Ground Power Supply Figure 2-7 Servo-control system configuration. 23 2.4.1 The computer system During the initial stage of the project, a 486-33 M H z PC was used to perform functional test of each component during integration. Later, at the final stage of development, when the instruction commands as well as the on-line real time calculations increased in complexity, the computing capability was also upgraded. The computer is required to choose control parameters and determine input commands within a time interval of 2.5 ms. This time interval is half of the data sampling rate from the encoders, as indicated by the Shannon's sampling theorem [32]. Currently, a Pentium Pro 200 M H z computer is used to control and monitor the manipulator system. The first attempt to access DMC-1040 was successfully performed using the T A L K 2 B U S executable command from the communication software. TALK2BUS runs the DMC-1040 interactive communication program. Strings of command words can be sent through this program to the controller to carry out simple maneuvering tests. The computer can also give commands by directly accessing the memory address of the R E A D and WRITE registers on the DMC-1040 controller card. This latter method is used by the control program to establish communication with the manipulator system. 2.4.2 Motion controller board A Galil DMC-1040 motion controller is used to simultaneously regulate the four DC servo-motors and identify their positions from optical encoders. With the data from the encoders and the capability of the built in digital filter, there is a closed loop servo-control of the motors. The controller accepts the command words with numerical parameters sent by the host computer in ASCII characters. The DMC-1040 has a number of pre-programmed control command for regulating the servo-motor motion as well as relaying position data back to the host computer. A complete set of instruction command is provided in the DMC-1040 Technical Reference Guide version 1.1 A [33]. 24 2.4.3 Power amplifier The power amplifier converts the pulse train i.e. the +/- 10 Volt signal from the controller into current to drive the motors. Each axis consists of the control lines of a servo-motor together with the feedback lines of the optical encoders. The amplifier gain is so set that a 10 Volt command generates the maximum current. For instance, i f the motor peak current is 10 A , the amplifier gain should be 1 A / V . 2.4.4 DC servo motors The slewing motors are Pittman 14202 (106 oz-in peak torque). These motors are connected to the revolute joints through a N E M A 23-20 reduction gear head of 1:20 ratio. The gear head reduced the rotation speed while increasing the output torque of the motor and the resolution of the encoder 20 times. The deployment motors are Pittman 9414 (24 oz-in peak torque). The servo-motors operate on DC current supplied by the motion controller through the power amplifier. 2.4.5 Optical encoders Each actuator motor has an optical encoder attached to its shaft. The encoder is of the offset track configuration. Each of the two identical tracks has 1000 windows. The tracks are offset (with respect to each other) by a quarter-pitch. A third track with a lone window generates a reference pulse for every revolution. The physical resolution of the encoder is given by A0 p = 36O\u00C2\u00B0/4N (2.1) where A0 p is the maximum resolution while N represents the number of windows per track [32]. According to Equation (2.1), the encoders used in the manipulator have a resolution of 0.09\u00C2\u00B0. The signal from the encoder, indicating the exact position of the motor, is monitored constantly by the controller. If a deviation from the target position is 25 registered, the controller will send a command, to generate the required current, to the motor to correct the error. 2.4.6 The control program The motion control program for the manipulator system was written in C language. The executable file for initial tests were created with a Turbo C compiler. When the new computer system was installed, the software package was also upgraded to a Windows base Visual C++ version 5. The main objective of this program is to provide an interface or to serve as an interpreter between the operator and the manipulator system. Its primary functions are listed here: \u00E2\u0080\u00A2 initialize the controller (handshake with input and output ports); \u00E2\u0080\u00A2 upload the control parameters (KP, KI, KD); \u00E2\u0080\u00A2 upload safety limits (SP, A C , DC, ER, TL, OE); \u00E2\u0080\u00A2 provide the operator with manual overwrite and direct command input option; \u00E2\u0080\u00A2 read the present position from encoder signals; \u00E2\u0080\u00A2 get target position from user; \u00E2\u0080\u00A2 check to ensure that target position is within the work space; \u00E2\u0080\u00A2 convert X , Y coordinates position into polar coordinates (0, r); \u00E2\u0080\u00A2 determine relative distance between the present position and the target position (A0, Ar); \u00E2\u0080\u00A2 convert relative distance into encoder counts; \u00E2\u0080\u00A2 convert numerical values into character strings and append the required command word; \u00E2\u0080\u00A2 write the command character string onto the output register address; \u00E2\u0080\u00A2 monitor the position feedback signals from the encoders while maneuver is carried out; \u00E2\u0080\u00A2 create output file and record the time history of the manipulator. 26 The algorithm shown in Figure 2-8 indicates the logic and interactions between the different subroutines within the control program. The main program is kept as simple as possible. Its only task is to coordinates the sequence in which the subprograms are called. The subroutines in the program are modular and their functions do not interfere with one another. The modular nature of the program empowers the programmer to edit a subroutine without affecting the other parts of the program. Also, new subroutines can be incorporated quite readily for diverse applications. The control capability of the program as well as the behavioural response of the prototype manipulator are discussed in Chapter 7. 27 Initialize Controller Upload control parameters and safety limits Get target position Manual overwrite and direct command input Read present position I If yes If no Check target position within workspace Convert Cartesian coordinates X , Y into polar coordinates 0, r T Determine relative distance between present and target positions AG, Ar Convert relative distance into encoder counts Convert numerical values into character strings and append the required command word Monitor the position feedback signals from the encoders while maneuver is carried out Write the command character string onto the output register address Create output file and record the time history of the manipulator If quit Exit If continue Figure 2-8 Flow-chart of the prototype manipulator control program. 28 3. FORMULATION OF THE PROBLEM 3.1 Preliminary Remarks The present chapter develops the governing equations of motion for a ground based slewing and deployable manipulator system. The schematic diagram of this model was presented earlier in Figure 1-7. It consists of a mobile base translating along a linear set of tracks, a two module manipulator attached to and supported by the base, and a flexible payload held by the end effector. Each of the two modules consists of two rigid links: one free to slew, while the other permitted to deploy (and retrieve). The translational motions involving deployment and retrieval are accomplished using prismatic joints, which are considered rigid. The rotational motions on the other hand, is achieved with flexible revolute joints. Hence the slewing motion of the links has rigid as well as flexible components. The deformation of the continuous flexible payload (beam) is represented by modal functions [34]. To summarize, the model involves a mobile manipulator with a rigid, moving base and slewing as well as deployable rigid links (four), having rigid prismatic joints but flexible revolute joints, supporting a flexible, Euler-Bernoulli beam-type payload. It must be emphasized that the manipulator and payload dynamics are constrained to a plane. The formulation starts with a mathematical model representing the prototype manipulator described in Chapter 2. Reference frames and generalized coordinates, which describe the state of the system, are identified, and the associated kinematic equations developed. Next, the kinetic and potential energy expressions for the system are derived. The equations of motion governing the system dynamics are now obtained using the classical Lagrangian approach [30,33]. As can be expected, they are highly nonlinear, nonautonomus, coupled, and extremely lengthy even in the matrix notation. Finally, Lagrange multiplier are introduced to account for constraint relations, as would be the case when the motion in some of the degrees of freedom is specified. 29 3.2 Kinematics of the System The manipulator's two modules are connected in series and supported by a mobile base. As shown in Figure 3-1, it is essentially a sequence of bodies arranged in an open chain topology. The two modules are identical. In space applications, with microgravity environment, one may employ several modules thus increasing the complexity as well as capability of the manipulator. 3.2.1 Reference frames F 0 represents the inertial reference frame for the system. F, is the body attached frame of the mobile base which move along the Y-axis of the inertial frame F 0 . This linear displacement of the base is represented by do. Thus the frame F,, attached to the base, remains parallel to the inertial reference. It serves as a reference for the slewing motion of module 1, through the body frame F 2 fixed to revolute joint 1. F 2 rotates about Z, with the angular displacement represented by 0j. The frame F 3 is attached to the deployable end of Module 1. F 3 translates along the Y 2 axis with the linear displacement denoted by dj giving the deployed length of Module 1. Similarly, F 4 and F 5 correspond to the body attached frames for Module 2, and are related to 02 and d2, respectively. Finally, S represents the generalized coordinate associated with the deflection of the flexible payload with respect to the frame F 5 . A l l the motions are confined to the plane X i 5 Y, (i = 0,1,2, ... ,5). 3.2.2 Modelling of joint flexibility As mentioned earlier, the revolute joints located at the mobile base and the elbow are considered flexible. The joint flexibility is modelled by a linear torsional spring which connects the rotor of the actuator motor to the slewing end of the module [35]. The stator of the motor is rigidly attached to the base platform or to the elbow platform. Figure 3-2 shows details of a flexible revolute joint. i \u00E2\u0080\u00A2 (3-5) Rearranging the components in the velocity vector, Eq. (3.5) can be rewritten as R d n ^ C j q , (3.6) where q is the vector of the generalized velocities and Cj is the transformation matrix for the mass element drttj with respect to the inertial frame, q - C f r i - j + Cja \u00E2\u0080\u00A2 (3.7) Here: Cjq = inertial transformation matrix of the body frame FJ; Cja = position vector to the mass element drrij with respect to Fj. 36 3.2.6 Generalized coordinates With the position, orientation, and velocity of the body attached frames with respect to the inertial reference determined, and the deformation of the elastic member (payload) known, the kinematic description of the system is complete. The set of generalized coordinates which describes the complete motion of the manipulator model is given as (Figures 3-1,3-2) d0 0, 02 d2 8 \u00C2\u00ABi a. (3.8) where: do = translation of the mobile base; 01 = total slewing angle of module 1; di = deployed length of module 1; 02 = total slewing angle of module 2; d2 = deployed length of module 2; 8 = generalized coordinate to represent deflection of the flexible payload; ai = rigid component of the slewing angle for module 1; a.2 = rigid component of the slewing angle for module 2. Note, the total slewing angle 0/ describing the rotation of j * revolute joint is composed of the rigid component a, and the flexibility contribution p) . Thus Pj = 0j-aj (3.9) 37 3.3 Kinetic Energy of the System With the expression for the velocity of a mass element determined, the total kinetic energy of the system can be written as where n is the number of bodies in the system. Rewriting the above equation in terms of the generalized coordinates r = i q r M q , (3.11) with M as the system mass matrix. A detailed description of the mass matrix M and its derivatives are given in Appendix III. 3.4 Potential Energy of the System Contribution to the potential energy (U) arises from two sources: gravitational potential energy (Ug); and the strain energy due to deformations of the elastic members (Us). The details of the potential energy terms and their derivatives are given in Appendix IV. 3.4.1 Gravitational potential energy Potential energy associated with a mass element was integrated over the entire system to get the total potential energy as ^ = - Z l . s G . \u00C2\u00AB R d J ^ > ( 3 - 1 2 ) 38 where: n = number of bodies in the system; g = Earth's gravitational acceleration; tyj = angle between the position vector Rdmj and the local vertical Yo. As shown in Figure 3-5, the gravitational potential energy for each member was calculated based on its position in relation to the reference Ug = 0. Figure 3-5 Position of a mass element in the gravitational field. 39 3.4.2 Strain energy The total elastic energy in the system can be written as s j H j 2H dx (3.13) where the first term represents the elastic energy contribution from the deformation of the torsional springs at the joints; while the second term arises from the flexible payload. As shown in Figure 3-2, Kj and fij are the torsional spring constant and the deformation angle at joint j , respectively. 3.5 Governing Equations of Motion The equations governing the system dynamics are obtained using the classical Lagrangian approach where t is the time; and Q, the vector of the generalized forces associated with the generalized coordinates q. Note that Q includes forces and torques which relate to translational and rotational motions, respectively. The final expression for the equations of motion in the matrix form can be written as d {dT\ dT dU = Q , (3.14) dt\di\) df\ dq (3.15) with dU 40 3.6 Specified Coordinates At times, the system may be required to perform specific tasks and may operate under given constraints. This would involve specifying certain generalized coordinates to undertake prescribed maneuvers. For instance, specified coordinates provide freedom to choose a trajectory path for the end-effector, velocity or acceleration profile for slewing and deployment maneuvers, etc. To that end, constraint relations are introduced in the equations of motion through the Lagrange multipliers as follows: where L = T - U is the Lagrangian, S is the distribution matrix that assigns constraint functions to the specified coordinates, and A is a matrix of Lagrange multipliers. S does not depend on time. Eq. (3.16) leads to (3.16) q - M - 1 /(q,q,r) + M- 1 SA , (3.17) where /(q,q,/) = T , - U , - M q Let G(/) = S r q ; then G(f) = S r q = S r M - ' /(q,q )^) + S r M - I S A . Solving for A and substituting into Eq. (3.17) gives (3.18) where: I identity matrix ; F r S^M-'S)\" 1 ; F(q.q.O \u00E2\u0080\u00A2 41 In the present study, step as well as sinusoidal acceleration profiles were adopted for prescribed maneuvers. The step input profile gives a constant finite acceleration during the first period of the maneuver. When the system reaches the desired velocity, the acceleration is set to zero. Finally, the system goes through a constant deceleration until it reaches zero velocity. The sinusoidal profile assures zero velocity and acceleration at the beginning and end of the maneuver. The time history considered here is as follows: At . f In t sin \u00E2\u0080\u0094 t 2n \At J > (3.19) In A q . \u00E2\u0080\u00A2 (2 q = (A . ) 2 sin \u00E2\u0080\u0094t \At ) where Aq is the maneuver range and At is the maneuvering time. 42 4. COMPUTER SIMULATION 4.1 Preliminary Remarks The focus here is on the development of a Fortran computer code to simulate dynamical behaviour of the manipulator. The governing equations of motion for generalized and specified coordinates are implemented through the program. The acceleration vector q(t) is integrated to obtain the time history of velocity q(^) and trajectory q(f) of the various degrees of freedom. Integration of highly nonlinear, nonautonomous and coupled set of equations presents a challenge which is further accentuated due to stiff character of the system, i.e. presence of widely differing time constants. For example, the slewing motion of the modules takes from 5 to 20 seconds to complete a revolution (i.e. slewing speed of 727s and 187s, respectively), while the vibrations induced at the flexible joints and payload have frequencies ranging from 1 to 100 Hz. The program uses the D-Gear integration routine provided in the ISML library which is well suited for stiff systems. It is so developed as to permit simulation of a wide range of cases. The group of subroutines composing the program are kept modular. This makes it possible to restructure specific routines, while maintaining the other parts of the program unaffected. 4.2 Structure of the Computer Code The constituents and organization of the simulation program are indicated in Figure 4-1. The Main Program is relatively simple; its purpose being to organize variables' transfer and coordinate calling of different functions. The Inputs block of the program allows the user to select specific system parameters for a particular simulation case such as: \u00E2\u0080\u00A2 size of the manipulator; \u00E2\u0080\u00A2 density and weight; 43 Inputs: Parameters; Properties; Dimensions; Simulation Time Inputs: Initial Conditions Qo\" Main Program <*>) Mass Matrix Energy: T and U Output File: Simulation Time t; Generalized and Specified Coordinates; V w System Energy: T U and Total. Matrix Inversion SLE Equations of Motion q = M- 1 [ ( l - rS r M- , )F + TG] Lagrange Multipliers Specified Coordinates Generalized Coordinates lt=j> x ( ' \u00C2\u00BB ) ISML Library ^ q DGEAR L X J - _q_ Figure 4-1 Modular architecture of the computer code. 44 \u00E2\u0080\u00A2 stiffness of the flexible revolute joints and the payload; \u00E2\u0080\u00A2 maneuver range; \u00E2\u0080\u00A2 geometry of the trajectory and the rate at which it is traversed. The initial conditions, simulation time period (to - (/), and the data recording time-step (At) are also chosen and entered through the Inputs block. At the end of a simulation time-step tj, the Main program calls the Output routine to record the status of all parameters at that instant of time. The state of the manipulator is communicated to the Mass matrix routine, which calculates the numerical value of the matrix, and transfers it to the Energy, Matrix inversion, and differentiation routines. The Energy routine calculates the kinetic, potential and total energy in the system for the current simulation time tj . Results from the energy calculations are then recorded by the Output routine. The Matrix inversion routine uses the SLE function from the ISML library to perform the matrix inversion task. The four Differential blocks are in charge of differentiating the mass matrix and the energy terms with respect to time, generalized coordinates and generalized velocities as the Lagrange procedure dictates. The Equations routine assembles the equations of motions according to Eq. (3.18) and cast them into the state space or first order form, (4.1) where x is integrated using Gear's method. Constraint relations are introduced using the Lagrange multipliers in a small block within the Equations routine. DGEAR routine takes the numerical value of x at time t = j and integrates it to give the value of x for the next time-step t - j+1. The value of x at t = j+1 is transferred back to the main program to be used as the initial condition for the calculation of x at t = j+1. The procedure is repeated until the simulation time t reaches the end of the simulation period tf. 45 The simulation program is developed using the Fortran Visual Workbench F32. The executable files of this program are created with the Fortran Power Station compiler. A copy of the source code is included in Appendix V . 4.3 Validation of the Formulation and Computer Code With the mathematical model established, the formulation of the governing equations of motion completed, and the simulation program developed, the next logical step would be to verify their accuracy. In a conservative system, i.e. for a system without damping and external, nonconservative, generalized forces, the total energy must remain constant at all time. Therefore, a convenient way to check the validity of the formulation and computer code is through the conservation of energy tests. To that end, a simple case of a double pendulum is considered. It may be pointed out that the following tests are conducted with the manipulator moving in a vertical plane where the only restoring force arises is from the gravitational pull. The first simulation test is performed with two modules of fixed length (1.38 m) connected by a revolute joint allowing relative rotational motion as shown in Figure 4-2 (Case 1). The revolute joints are considered to be rigid and the flexible payload is absent. Initially the modules are kept aligned with an angular displacement of 6i = 80\u00C2\u00B0 (\u00C2\u00AB 1.4 rad) with respect to the vertical. Note, 02 = 0. With zero initial velocity and acceleration, the system is released at time t = 0. A little later, after 0.5 second, module has moved to a new position (#/ \u00C2\u00AB 40\u00C2\u00B0 from the vertical), while module 2 is lagging behind due to a higher inertia about the system support. At t = 0.75 s, the manipulator reaches its lowest equilibrium position where both the modules are aligned with the vertical. In this equilibrium position, the manipulator is located closest to the ground and moving with the highest velocity, i.e. its kinetic energy reaches the peak while the potential energy is minimum. After crossing the equilibrium position, the system starts to decelerate as it 46 reaches higher heights. Now, the module 2 has higher momentum and hence it is able to overcome the retarding force better. Thus it leads module 1 at t = 1 s. The system goes through one-half cycle reaching the highest point on the opposite side at 1.5 s from where the reverse cycle begins. The corresponding variation in position, velocity and acceleration during the three-second-cycle are shown in Figure 4-3. It also shows variations of the kinetic, potential and total energies. The deviation in the total energy from its initial value was around 0.3x10\"4 J, i.e. approximately 10\"5 %. This negligible variation is attributed to the computational noise associated with the truncation error. Figure 4-2 Free oscillation of the rigid manipulator model with initial conditions as 0, = 80\u00C2\u00B0, 02 = 0. The second case considers application of an initial displacement to both the rotational joints as shown in Figure 4-4. The initial angle 0\ of module 1 with respect to the vertical line, and the angle 02 of module 2 with respect to module 1 are taken to be 45\u00C2\u00B0 (0.785 rad). As in the previous case, the initial velocity and acceleration are taken to be zero, and the manipulator's physical characteristics are kept the same. The simulation is run for 3 seconds, and the time histories for both the degrees of freedom are recorded. 47 Initial Conditions: Specified Coordinates Generalized Coordinates = 0; 0, =80\u00C2\u00B0, 0 2 = O; Linear mass density p, = 1 kg/m for all members. d, = d2 = 1.38 m, (i.e. lengths are held fixed). <9,=6>2=0; 0l = '02=O. Position, deg Velocity, deg/s 0.0 1.0 2.0 X 1 Q 3 Acceleration, deg/s2 3.0 0.0 1.0 2.0 3.0 Conservation of Energy A E -0.3 Time, s Figure 4-3 Time histories of the system parameters during the energy conservation test: Case 1; 0,(0) = 80\u00C2\u00B0, 02(O) = 0. 48 41 Linear mass density p, = 1 kg/m for all members. Specified Coordinates d0 = 0; dt = d1 = 1.38 m, (i.e. lengths are held fixed). Initial Conditions: Generalized Coordinates 0, =45\u00C2\u00B0, 6>2 = 45\u00C2\u00B0; 0 ,=0 2 =O; ^, =6>2 =0. Position, deg 9, Velocity, deg/s 0.0 1.0 2.0 X 1 Q 3 Acceleration, deg/s2 Energy, J 0.0 -0.3 x10-4 Conservation of Energy A E 0.0 1.0 2.0 Time, s 3.0 Figure 4-4 Time histories of the system parameters during the combined slewing disturbance of 0,(0) = 45\u00C2\u00B0, 92(0) = 45\u00C2\u00B0. 49 The position, velocity and acceleration plots show that the manipulator goes through an oscillation cycle similar to the one observed in Case 1. The results clearly show that the total system energy is conserved. In the above two cases, the system was subjected to an initial disturbance in terms of position. However, it would be beneficial to perform the energy conservation test with initial conditions involving parameters other than position alone. Case 3 considers an initial impulsive disturbance from the equilibrium point, as indicated in Figure 4-5. As in the previous cases, the lengths of module 1 and 2 are held fixed at 1.38 m. The two modules are initially aligned along the vertical, an equilibrium position. A n initial velocity input of = 1807s (rc rad/s) is given to the revolute joint of module 1, while keeping the second module undisturbed. At time t = 0, the kinetic energy is at its maximum value. In this position the manipulator is in its stable equilibrium orientation, where the potential energy is minimum. The system swings upward reaching an elevation where its total kinetic energy is converted into the potential energy; then, the reverse cycle begins. Note the conservation of energy even during such complex motion. The peak deviation is less than 1.6xl0'3 %. This gives considerable confidence in the formulation as well as the numerical integration code. Next, the severity of the disturbance was increased further. Case 4 studies the manipulator response with an initial velocity of 0X= 1807s and 62= -1807s, while keeping all other parameters unchanged. The manipulator is initially positioned along the vertical. The module 1 swings upward to the right while module 2 swings to the left because of the character of the disturbance. Figure 4-6 shows the time histories of the response. Note that even in this demanding situation (i.e. larger disturbances), the total energy is conserved during the entire 3 seconds of simulation, despite considerable transfer of energy between the degrees of freedom. The maximum variation in the total energy is in the order of 10\"4 %. 50 Linear mass density p | = 1 kg/m for all members. Specified Coordinates d0 = 0; dt= d2- 1.38 m, (i.e. lengths are held fixed). Initial Conditions: Generalized Coordinates 0,=O, 0 2 = O; 0,=18O%, <92=0; 0, = 0 2 =0. 0.0 1.0 2.0 3.0 Time, s Figure 4-5 Response of the system to an impulsive disturbance of ^ ( O ) ^ 1807s. Note, the error in the conservation of energy remains insignificant. 51 Initial Conditions: Specified Coordinates Generalized Coordinates \u00E2\u0080\u00A2 1 d0 = 0; 0, =0, 0 2 = O; Linear mass density p, = 1 kg/m for all members. d,= d2 = 1.38 m, (i.e. lengths are held fixed). \u00C2\u00A3 , = 1 8 0 % , 0 2 = - 1 8 O % ; \u00C2\u00A3 , = 6 > 2 = 0 . Position, deg -6. x 1 q3 Acceleration, deg/s 0, Velocity, deg/s 0.0 -0.3 x10\" Conservation of Energy A E 0.0 1.0 2.0 3.0 Time, s Figure 4-6 Response of the system to severe impulsive disturbances: <9,(0)= 1807s; #2(0) = -1807s. Conservation of energy attest to the validity of the formulation and the numerical integration code. 52 In the preceding investigations, the system was treated as rigid, i.e. flexibility of the revolute joints was ignored and beam-type payload was absent. The final energy test, Case 5, was carried out with all the flexible components (revolute joints 1 and 2, and flexible payload) added to the system. The initial conditions and system parameters given in this test are identical to those used in Case 2, where the initial position displacement of 45\u00C2\u00B0 was given to 0j and 02- The torsional spring constants Kj and K2 were taken as 1 Nm/rad and flexural rigidity EI of the payload was set at l x l 0 6 Nm 2 . The time histories of the generalized positions, velocities and accelerations, shown in Figure 4-7, are similar to those observed in Figure 4-4. Due to the flexible components, the response is modulated with high frequency vibrations. Note, however, that only the response at joint 1 is significant because of its large inertia coupling with the system, whereas the effect at joint 2 appears to be negligible. The trajectories of the flexible components are plotted separately in Figure 4-8. The vibrations of the flexible revolute joints in module 1 and 2 are represented by /?/ and respectively, while 8 denotes the magnitude of deflection at the tip of the payload. As expected, the plots show that energy is conserved through out the entire simulation period. The results of the above simulations give confidence in the accuracy and completeness of the formulation as well as the computer code. 53 Initial Conditions: k V Specified Coordinates Generalized Coordinates d\u00E2\u0080\u009E = 0; (9, =45\u00C2\u00B0, 0 2 =45\u00C2\u00B0; d, - d2 = 1.38 m, (i.e. lengths are held fixed); <5=0; ex = 'e2 = Q; Linear mass density p, = 1 kg/m 2lp = 2 m; \u00C2\u00A3 / = lxl06 Nm2. 6, = 612 = 0 ; K,=K2=\ Nm/rad. for all members. 0.0 -0.1 x10\"' -0.2 Conservation of Energy AE 0.0 1.0 Time, s 2.0 3.0 Figure 4-7 Time histories of the system response accounting for flexibility at the revolute joints and the payload. 54 Figure 4-8 Response of the flexible components of the manipulator system subjected to an initial disturbance of Oi = (5i - vibration of flexible joint 1; /?? = vibration of flexible joint 2; v = tip deflection of the payload. 92 = 45\u00C2\u00B0 5. DYNAMICAL RESPONSE TO INITIAL DISTURBANCES AND SPECIFIED MANEUVERS 5.1 Preliminary Remarks With the assurance that the formulation and the computer simulation code are valid, the next logical step was to explore the dynamical behaviour of the manipulator system. This chapter discusses results of a study aimed at uncontrolled system dynamics during a wide variation of parameters and initial conditions, as given in Table 5-1. This brings to light coupling effects, i.e. interaction dynamics, among the members of the robotic system. The intent is to establish behaviour characteristics of the time dependent variables subjected to prescribed maneuvers. For instance, data pertaining to the amplitude and frequency of vibrations at the joints and payload can assist in the design of the control algorithm and trajectory planning for the prototype manipulator. In addition, knowledge concerning the forces and torques experienced by the system can provide warning when the structural integrity is compromised. The first set of simulation results focus on the system response to initial conditions. Following, the effects of interactions between modules of different sizes is studied. Finally, dynamic simulations over a wide range of specified maneuvers are carried out to assess response characteristics of the system. Table 5.1 System parameter and initial conditions used during uncontrolled dynamic simulations. Parameters Test Conditions do Fixed or free at the origin position. 0, Range: 0 - 80\u00C2\u00B0 ; Speed: 0 - 30 7s dj Fixed at 1.38 m; Free: 0.6 -1.38 m; Speed: 0 - 0.15 m/s. Range: 0 - 80\u00C2\u00B0 ; Speed: 0 - 30 7s. d2 Fixed at 1.38 m; Free: 0.6 - 1.38 m; Speed: 0 - 0.15 m/s. S Payload length: 2 m, with various degree of flexural rigidity (ET). mb Weight: 5-10 kg. me Weight: 0 - 5 kg. Mm2/Mmi Dimension and mass ratio: 1; 0.5; 0.25; 0.1. 56 The amount of information generated through this parametric study is considerable. For conciseness, only a few representative cases, illustrating typical dynamical behaviour, are reported here. It may be pointed out that simulations presented in this chapter are carried out with the manipulator operating in the horizontal plane. Therefore, the parameter g representing the Earth's gravity force is set to zero. In a sense, this would approximate the microgravity environment in space. The prototype experimental system constructed for the comparative performance study (Chapter 2) also operates in the horizontal plane. Unless otherwise specified, the following numerical values are used during the simulation: \u00E2\u0080\u00A2 Mass of the mobile base, mt, = 5.0 kg; \u00E2\u0080\u00A2 Mass of the elbow joint, me = 0; \u00E2\u0080\u00A2 Size of the elbow joint, le = 0.1 m; \u00E2\u0080\u00A2 Slewing links; lsi, ls2 = 0.5 m; \u00E2\u0080\u00A2 Deployable links; lY 0 X 0 Rigid manipulator model. Initial Conditions: d0 = 0; d 0 = 0; 0 = 0; = 0; d, = 0.5 m; = 0.1m/ s; 0 = 90\u00C2\u00B0; 02 = 11.5\"/ 5 d2 - 0.5 m; d2 = 0. 0.0 d n , m -18.0 x10\" d 1 , m 135.0 d\u00E2\u0080\u009E, m 0.01 h Total J 0.00 Kinetic Potential 0.0 1.0 2.0 Time, s 3.0 Figure 5-3 Response of the system during a combined slewing and deployment maneuver (Case C): (a) displacement time histories and energy conservation. 65 0.00 d 0 , m/s -5.00 0.00 d 0 , m/s2 -0.06 0.0 1.0 2.0 3.0 2.50 r 24.00 6,, 7s 12.00 0.0 1.0 2.0 3.0 e \u00C2\u00B0/s2 0.00 d, , m/s -0.14 1.0 2.0 Time, s 3.0 -0.05 d 2 , m/s2 -0.10 0.0 1.0 2.0 Time, s 3.0 Figure 5-3 Response of the system during a combined slewing and deployment maneuver (Case C): (b) velocity and acceleration profiles. 66 0. 0, 1 Xo Rigid manipulator model. Initial Conditions: 0; d0 = 0.5m/s; 0,= 90\u00C2\u00B0; o\ = 6075; 4 = 1.0 m; dx = 0; 3 = 0; e\ = -60 7 s; 4 = 1.0 m; d2 = 0. d 0 , m 150.0 100.0 d\u00E2\u0080\u009E, m 0.15 J 0.00 T o t a l . , Kinetic Potential ^ 0.0 1.0 2.0 Time, s 3.0 Figure 5-4 Response of the rigid manipulator system to a demanding disturbance involving base motion and slewing velocities at the joints (Case D): (a) time histories of generalized coordinates and energy conservation. 67 0.50 d 0 , m/s O.Oi So 1.0 2.0 3.0 0.00 d\u00E2\u0080\u009E, m/s: 60.0 e , , 7s 0.00 e , , 7 s 2 -50.00 L 0.0 1.0 2.0 3.0 0.50 d,, m/s 0.00 1.00 d,, m/s2 0.00 0.0 3.0 0.00 e , , 7 s -50.00 50.00 e,,\u00C2\u00B0/s2 o.oo d 2 , m/s -0.20 0.0 1.0 2.0 Time, s 3.0 0.40 d,, m/s' 1.0 2.0 Time, s 3.0 Figure 5-4 Response of the rigid manipulator system to a demanding disturbance involving base motion and slewing velocities at the joints (Case D): (b) time histories of generalized velocities and accelerations. 68 Finally, Case E simulates an extreme operating condition, when all the four joints are subjected to initial disturbances in presence of a flexible beam-type payload. The initial angular velocity for both 0\ and <92 is set to 30 7s (n/6 rad/s), while the deployment velocities for dx and d2 are taken as 0.15 m/s. The mobile base is kept unrestrained as in the previous cases. The main objective is to assess the effect of payload flexibility on the response of the other degrees of freedom. Results in Figure 5-5(a) suggest modulations in the displacement time histories, due to the payload oscillations, are so small as to be imperceptible, unless magnified. However, the effect of the payload flexibility is quite apparent in the generalized velocities and accelerations, indicating the presence of small magnitude coupling forces. In summary, response results to initial disturbances suggests that the slewing motion of module 1 and the translational motion of the base have significant effects on the system response through coupling. On the other hand, a deployment maneuver has relatively small influence on the system dynamics. In general, the payload flexibility modulates the response, at the payload frequency, by a small amount. The results provided better appreciation of the complex interactions between system dynamics, inertia and Coriolis effects. Conservation of the total energy in all the cases provided further validation of the formulation as well as the numerical algorithm. 69 ex e2 f T 1 X 0 dx d2 V Rigid manipulator model. Initial Conditions: d0 = 0; 3 = 0; d, = 0.5 m; 3 = 0; 0.8 0.6 0.4 0.2 0 -0.2 0 0.5 - - t = 0.0s t = 0.5s - - t = 1.0s 1 = 1.5s t = 2.0s 1.5 X(m) Figure 5-8 Configurational time history showing coupling effects when the initial impulsive disturbance is applied to module 2: (b) Mm2/Mmi = 0.5. 80 Initial Conditions o n, a o o 9, d0 = 0; d0 = 0;\" 9i = 0; = 0 ; di = 0.6 m ; = 0 ; 02=0; 02 = 30 7s ; d2 = 0.6 m ; j 2 = 0.15 m/s 1.8 1.6 1.4 1.2 _ 1 \u00C2\u00A3 0.8 0.6 0.4 0.2 0 -0.2 0 0.5 - - t = 0.0s t = 0.5s - - t = 1.0s t= 1.5s t = 2.0s 1.5 X(m) Figure 5-8 Configurational time history showing coupling effects when the initial impulsive disturbance is applied to module 2: (c) Mm2/Mmi = 0.25. 81 Initial Conditions a 4> = 0 ; d0 = 0 ; 0 ; = 0 ; s Figure 5-10(b) Time histories of the response and generalized forces during the 120\u00C2\u00B0 slewing maneuver of module 2. 88 Figure 5-11(a) Specified slewing maneuver of 120\u00C2\u00B0 executed by module 1 with a sinusoidal acceleration profile. 89 Figure 5-11(b) Time histories of the response and generalized forces during the 120\u00C2\u00B0 slewing maneuver of module 1. 90 the mobile base is around 0.6 N (almost three times the previous case) causing translation of the manipulator by more than 40 cm. The peak vibration level at the tip of the payload remains essentially the same as before. 5.4.2 Deployment maneuver Maneuvers using the deployable joints are explored in this section. The dynamics of the manipulator due to a deployment maneuver of module 2 is studied first. The mobile base is initially located at the origin with module 1 aligned along the Y 0 axis of the workspace, while module 2 is at right angle to it parallel to the negative X 0 axis. The initial dimensions of the module 1 and 2 are 1.38 m and 0.6 m, respectively. From this initial position, module 2 is commanded to deploy from 0.6 to 1.2 m in 8 s with a sinusoidal acceleration profile. A l l the joints are left free to respond. Since the revolute joint 1 is not restrained, the reaction force generated by the deployment causes module 1 to rotate clockwise through ai \u00C2\u00AB 15\u00C2\u00B0 (Figure 5-12a). The vibratory response at the revolute joints is essentially negligible (Figure 5-12b). The magnitude of the payload deformation at the tip is also of no concern (\u00C2\u00AB 3x10\"2 mm), being an order of magnitude smaller than that encountered during slewing maneuvers. The torques and forces generated at the revolute and prismatic joints also remain rather small. Next, the response to the deployment motion at module 1 was explored. From the same initial position as before, module 1 is commanded to deploy from 0.6 to 1.2 m in 8 s following a sinusoidal acceleration profile. Figure 5-13(a) shows the sine-on-ramp trajectory of the deployment and captures the manipulator position at 1 second time interval during this simulation. In general, the system remains virtually unaffected by the maneuver except for the base motion of around 13 cm in the - Y 0 direction (Figure 5-13b). The elastic revolute joints at modules 1 and 2 show small amplitude oscillations of around 0.1\u00C2\u00B0, and the generalized forces continue to remain rather negligible. 91 d7, m 0.60 Figure 5-12(a) System configuration during the specified maneuver of 0.6 to 1.2 m executed by module 2 with a sinusoidal acceleration profile. 92 gure 5-12(b) Response of the generalized coordinates and forces during the deployment maneuver at module 2. 93 3 o o T=0 -X0, m 1 Module 2 0 Module 1 -1 Half-payload _ i L -1 0 1 Y0, m 1.20 dj, m 0.60 \" 0 2 4 6 8 10 Time, s Figure 5-13(a) Deployment maneuver at module 1 from 0.6 m to 1.2 m in 8 s and associated geometry of the manipulator. 94 Time, s Time, s Figure 5-13(b) Time variation of generalized coordinates and forces during the deployment maneuver at module 1. 95 The results suggest that compared to slewing maneuvers, the effect of deployment, either at module 1 or 2, on the system dynamics appears to be relatively less. 5.4.3 Complex maneuver With some appreciation as to the dynamical response of the manipulator to separate slewing and deployment maneuvers, the next logical step was to investigate response of the system to more complex maneuvers. As the variety of maneuvers is limitless, a typical case of the manipulator performing a material handling operation was considered. Figure 5-14(a) shows schematically the manipulator reaching out to pick up a payload from the right side. With the payload secured, the manipulator retracts to a small size allowing it to move around efficiently. In the retracted configuration, it translates approximately 4 m to the left, executes deployment maneuver and deposits the payload at the target position. The entire operation takes 20 s. Initial geometry of the manipulator as well as specified time histories of retraction, slew and deployment maneuvers are also indicated. As before, all maneuvers used the sinusoidal acceleration profile. Three operational phases can be identified as follows: Phase 1 - This involves retrieval and slewing maneuvers extending from t = 0 - 6 s, with the base locked in position. Phase 2 - This is dominated by translational maneuver from t = 6 - 13 s. A l l joints are locked. Phase 3 - This is characterized by deployment at the two modules as well as slewing maneuver of module 2, lasting from t = 13 - 20 s. Slewing joint 1 and base are locked. During phase 1, the two modules, with an initial length of 1.38 m each, retract to 0.6 m in 6 seconds. At the same time, module 1 slews from 30\u00C2\u00B0 to 135\u00C2\u00B0 while module 2 swings from -60\u00C2\u00B0 to -120\u00C2\u00B0, relative to module 1, in the anticlockwise sense. Small amplitude payload tip oscillations (< 2 mm) are apparent. Rotor oscillations due to flexible character of the revolute joints are also present. They are more pronounced at J, (\u00C2\u00B1 3\u00C2\u00B0) 96 compared to those at J 2 (\u00C2\u00B1 1\u00C2\u00B0). The associated time histories of generalized forces are presented in Figures 5-14(b) and 5-14(c), respectively. The translation phase 2 is relatively quiescent. The mobile base moves from 3 m to -1 m along the Y 0 axis. The modules remain in the retracted position (0.6 m) and the joints maintain their position except for small modulations caused by the joint vibrations. The payload tip displacement grows to \u00C2\u00B1 3.5 mm, however, it is still rather small. Finally, phase 3 involving the payload deposition follows. Primarily it involves deployment at module 1 and 2 from 0.6 to 0.96 m and slewing of module 2 from -120\u00C2\u00B0 to -30\u00C2\u00B0. As in the case of phase 1, the brake is applied to lock the base in position during this maneuver. The resulting generalized forces at the joints are rather small (forces < \u00C2\u00B1 7 N , torque < \u00C2\u00B1 1.5 Nm). This information is useful in assessing integrity of the prototype manipulator. The results suggest that the designed manipulator can withstand this loading quite readily with sufficient margin of safety. The maximum loading torque delivered by the actuator motor and gear head assemblies can get up to approximately 15 Nm. Therefore, even though the system is subjected to a demanding operating condition, the forces and torques observed are still below the design limit by a factor of 7. 97 Initial Conditions Phase do, m 0,, deg d], m 02, deg d2, m 1 3 30 1.38 -60 1.38 2 -1 135 0.6 -120 0.6 3 -1 135 0.6 -120 0.6 End condition -1 135 0.96 -30 0.96 Phase 3, Deployment and slewing Unloading Phase 2, Translation Phase 1, Retrieval and slewing / = 20s t= 13 s 1 t = 6s t = 0 Loading I Figure 5-14 Complex maneuver involving loading and unloading of a beam-type payload: (a) initial conditions and schematic diagrams showing the sequence of operations. gure 5-14 Complex maneuver involving loading and unloading of a beam-type payload: (b) time histories of specified and generalized coordinates. Force at the base N 0 0 5 10 15 20 N 0 Force at deployable joint 1 Nm \"0 5 10 15 20 -2 Torque at slewing joint 1 0 5 10 15 20 N 0 -2 Force at deployable joint 2 Nm 0 Torque at the slewing joint 2 0 5 10 15 20 Time, s \"0 5 10 15 20 Time, s Figure 5-14 Complex maneuver involving loading and unloading of a beam-type payload: (c) variation of the generalized forces. 100 Based on the results obtained in this chapter, following observations can be made: (i) Even during complex initial conditions, the system energy continues to be conserved attesting to the validity of the formulation and the numerical integration algorithm. (ii) The presence of a flexible payload has little influence on the system dynamics. (iii) The sinusoidal acceleration input profile is preferred against the step input for its continuity and smooth transition from one state to another. Its zero velocity and acceleration at the beginning and end of the maneuver reduces the residual vibrations of the flexible members. (iv) In general, the slewing maneuvers generate greater inertia coupling effects. This leads to larger response and generalized forces compared to those for the deployment maneuvers. (v) A maneuver carried out by module 1, which is closer to the base, imparts considerable dynamical disturbance to other components of the system, i.e. module 2 and the flexible payload. On the other hand, a maneuver carried out by module 2, generates only small coupling effects. (vi) As can be expected, the smaller the mass of module 2, the greater the disturbance it experiences. Conversely, its own maneuver has little effect on the rest of the system. (vii) For a given operation, it is recommended to use the coarse manipulation capability of module 1 first and then provide the necessary fine motions by module 2. This would reduce the demand on the control effort and improve operational accuracy. (viii) Generalized forces experienced by the manipulator system, even during demanding maneuvering operations, are well within the design tolerance. 101 6. SIMULATION OF CONTROLLED BEHAVIOUR 6.1 Preliminary Remarks The prescribed slewing, deployment and retrieval maneuvers presented in the previous chapter were achieved using constraint relations. The generalized coordinates are instructed to follow specified paths with the assumption that the actuator motors can deliver instantaneously forces and torques required to execute the maneuver. In other words, they are ideal systems whose actuators possess necessary power output. These simulations proved useful in understanding the dynamical behaviour of the manipulator systems under a wide range of operating conditions. In reality, however, the performance of any robotic system is governed by the capability of its actuators as well as the performance of the controller regulating its dynamics. A manipulator is required to perform a desired task with a specified level of accuracy. This requires operation of a system in presence of a controller. The present chapter deals with the selection of a control law that would allow the manipulator system to attain an acceptable performance. A wide variety of control strategies have been discussed in the literature and used in practice, each with certain advantages and limitations. The Proportional-Integral-Derivative (PID) is one such control scheme. PID controllers are used in virtually all types of systems because of their simplicity, robustness and ease of implementation. The aim of this part of the thesis is to design a PID controller for operating the prototype manipulator. The following section discusses the procedure of the PID controller design. Following, the system response to independent joint control using the PID scheme is examined, to assess the ability of the controller to follow a prescribed path. Finally, the path planning and trajectory tracking of the end-effector are simulated to assess the overall system performance. 102 6.2 Design of a Proportional-Integral-Derivative (PID) Controller The controller design is primarily based on the following criteria: \u00E2\u0080\u00A2 independent joint control, i.e. separate actuator to drive individual joint; \u00E2\u0080\u00A2 multiple-input, multiple-output system structure. 6.2.1 Structure of the control system The mapping of the control structure and location of the components in the block diagram are shown in Figure 6-1. The Input block contains the initial conditions as well as the desired trajectory data for each joint of the manipulator. The Controller block holds the ran control gain matrices K p , K i 5 and K d with n as the number of degrees of freedom to be controlled. The transfer functions for the manipulator are located in the Plant block, and they are arranged in the state-space form as (6.1) x = Ax + Bu , and y = Cx , where: x = vector of the degrees of freedom of the system; u = vector of the input position (rotor of the servo-motors); y = vector of the output position (manipulator joints); A = system matrix; B = input distribution matrix; C = output distribution matrix. The Feedback block carries the output signals back to the controller. Finally, the remaining component in the control system structure is the Output block which manages the collection and storage of the output data. It also provides connection to plotting units allowing on-line tracking of the manipulator response. 103 Input Controller Plant Output w w Feedback Figure 6-1 Structure of the control system. 6.2.2 Controller tuning The control gains were obtained using the Ziegler-Nichols method for tuning PID controllers [37]. This approach can be summarized as follows: \u00E2\u0080\u00A2 set the integral (Kj) and derivative (Kd) gains to zero and increase the proportional gain K p until the system becomes marginally stable; \u00E2\u0080\u00A2 define this K p value to be the ultimate gain K u and measure the oscillation period Pu; \u00E2\u0080\u00A2 set K p = 3Ku/5, K ; = 6KU/(5PU), and K d = 3KuPu/40. Figure 6-2 shows the response of the manipulator to a step input with the ultimate gain K u = 25 and 30 for module 1 and module 2, respectively. The values of P u are measured to be approximately 0.7 seconds for both the modules. With these two values, a set of PID control gains that would operate the manipulator within the stable region is found. Table 6-1 gives the values of the tunable parameters, Kp, Kj and K d obtained by following the Ziegler-Nichols procedure outlined above. 104 Figure 6-2 System response to a step-input with the ultimate control gain K u . 105 Table 6-1 PID control gains obtained using the Ziegler-Nichols method. K; K d Module 1 15 42.85 1.3 Module 2 18 51.43 1.58 Figure 6-3 shows the manipulator response to a step input with this set of PID control gains. A 12% overshoot is observed for module 1, while an overshoot of close to 25% is recorded for module 2. Note the residual vibrations at both the flexible joints. The low decay rate is attributed to the fact that the frequency of the vibration is far apart from the controller's operating range. In a real system this residual vibrations would be suppressed by the damping. In order to improve the response, the control gains given in Table 6-1 were used as starting values for further fine tuning using the Nonlinear Control Design (NCD) toolbox [38]. As shown in Figure 6-4, the overshoot and the residual vibration in the system are slightly reduced. The control gains obtained from this final tuning are used in all subsequent maneuvering operations. They are given in Table 6-2. Table 6-2 PID control gains obtained using the N C D toolbox. K< K d Module 1 10 38.85 1.1 Module 2 12 50.43 1.0 106 1.0 0.5 0.0 1.10 1.00 0.90 Module 1 0 Module 2 2 3 Time, s Figure 6-3 System response to a step-input with the Ziegler-Nichols control gains. 107 Module 2 2 3 Time, s Figure 6-4 System response to a step-input with the N C D toolbox control gains. 108 6.3 System Response to Maneuvering Commands With the controller and its control parameters established, the manipulator system is ready to perform maneuvering operations involving joint position tracking and end-effector path planing. The step input signal used in the previous section is now replaced by sinusoidal trajectory paths. The revolute joint of module 1 is tested first with a slewing command of 0 to 120\u00C2\u00B0 in 8 s following a sine-on-ramp position profile. The modules 1 and 2, each 1 m long, are alingned with the horizontal at t = 0. During the maneuver, module 2 is locked in position. Figure 6-5(a) shows the remarkable accuracy of the joint response with reference to the input signal. The lag at the beginning (t = 0) is observed to be only 0.01\u00C2\u00B0, the overshoot is reduced to 0.033%, and the residual vibrations are essentially suppressed. In contrast to the demanding step input signal, the sine curve ensures smooth transition to the desired orientation. Figure 6-5(b) compares the trajectory followed by the end-effector with the specified input signal. The difference between this two arcs is almost imperceptible. Next, module 2 was subjected to the same command. The corresponding joint and end-effector trajectories are shown in Figure 6-6. Here, the lag and the overshoot of the joint trajectory are slightly increased to 0.02\u00C2\u00B0 and 0.2%, respectively. However, this has little effect on the trajectory of the end-effector as shown in Figure 6-6(b). Figure 6-7 compares the trajectory followed by the end-effector with the prescribed path for a maneuver in which both modules slew from 0 to 120\u00C2\u00B0 in 8 seconds. A slight discrepancy between the two trajectories is evident only during the acceleration period of the maneuver when the response was observed to lag behind the input position by small amount. 109 I , I I I I I I I I I 1 , 1 I I I I f I ' i l l I 1 1 1 1 I 0 2 4 6 8 10 Time, s Figure 6-5 Comparison between desired and actual trajectories during a 120\u00C2\u00B0 slewing maneuver at module 1: (a) joint trajectories. Figure 6-5 Comparison between desired and actual trajectories during a 120\u00C2\u00B0 slewing maneuver at module 1 (b) end-effector trajectories. l I I I I I I I I I I I 1 I. I 'll I ' I 1 1 1 1 I 1 1 1 1 I 0 2 4 6 8 10 Time, s Figure 6-6 Comparative performance during a 120\u00C2\u00B0 slewing maneuver at module 2: (a) joint trajectories. Desired end-effector trajectory -X 0 , m Y 0 , m Figure 6-6 Comparative performance during a 120\u00C2\u00B0 slewing maneuver at module 2: (b) end-effector trajectories. Figure 6-7 Path followed by the end-effector during a 120\u00C2\u00B0 slewing maneuver of module 1 and module 2. To better appreciate the magnitude of deviation between the desired trajectory and the actual end-effector path, a straight line tracking operation is undertaken. The end-effector of the manipulator is commanded to follow a straight line from position (2,0) to position (1,0) in the workspace. The maneuver is to be completed in 8 s. This task is carried out with module 1 slewing from 0 to 60\u00C2\u00B0, while module 2 slews from 0 to -120\u00C2\u00B0 as shown Figure 6-8. Note, each module is 1 m long and slewing links are held fixed. Figure 6-9 displays the path followed by the end-effector compared to the desired trajectory. A magnification of scale along the X-axis shows that the peak deviation of the end-effector path from the desired trajectory is around 0.4 mm. Figure 6-8 Schematic diagram showing a straight-line tracking with two revolute joints. 115 0.01 r Desired end-effector trajectory (a) -X 0, m 0.00 -t = 8 s t = 0 End-effector path -0.01 0.0 0.5 1.0 1.5 Y 0 ,m 2.0 1.0 0.5 -X 0 , mm 0.0 -0.5 Desired end-effector trajectory t = 8 s End-effector path (b) t = 0 \u00E2\u0080\u00A21.0 0.0 0.5 1.0 Y 0 ,m 1.5 2.0 Figure 6-9 Tracking of a straight-line using two revolute joints. Note, the module lengths are held fixed at 1 m. Two different scales along the X axis are used to emphasize the error. 116 Next, the same task was accomplished using the retrieval capability of the two prismatic joints, i.e. the straight-line tracking performed by the two slewing joints is repeated with the prismatic joints as shown in Figure 6-10(a). The objective is achieved through retrieval of each link by 0.5 m. As the manipulator modules are aligned, the forces generated at the prismatic joints pass through the axes of rotation of the slewing joints. The absence of inertia coupling in this configuration allows the manipulator to perform the operation with virtually no disturbance. Note, in Figure 6-10(b), the desired trajectory and the actual path followed by the end-effector correlate quite well. The same straight-line tracking maneuver can be executed by a combination of slewing and deployment motions of the links in a single module as seen in Figure 6-11. The base is located at position (2,1) in the work-space. Here, the slewing joint rotates from -90\u00C2\u00B0 to -135\u00C2\u00B0, while the length of the module increases from 1 m to 1.41 m. The accuracy of the end-effector path, shown in Figure 6-11(b), is not as high as the one observed with the prismatic joints (Figure 6-10). However, the peak deviation is still only 2 mm. Figures 6-9, 6-10 and 6-11 showed the manipulator executing the same operation utilizing different combinations of its degrees of freedom. Thus redundancy of the prototype manipulator would allow it to complete the task even in the event of a joint or a module failure. To summarize, the simmulation results suggest that the manipulator is able to track a prescribed trajectory with an acceptable level of accuracy. 117 a Module 1 L Module 2 X X (a) Beginning End a XX Y n 1 1.0 0.5 -X 0 , mm 0.0 -0.5 -1.0 0.0 Desired end-effector trajectory t = 8 s End-effector path 0.5 1.0 Y n , m 1.5 (b) t = 0 2.0 Figure 6-10 Trajectory tracking of a horizontal straight-line using two prismatic joints: (a) schematic diagram showing the maneuver; (b) comparative study of prescribed and actual end-effector trajectories during tracking. 118 (a) X 0 t=Ss End Base (held fixed) Beginning 0.01 -X 0 , m 0.00 -0.01 (b) 0.0 End-effector path t = 8 s Desired end-effector trajectory 0.5 1.0 1.5 Y 0 ,m t = 0 2.0 Figure 6-11 Trajectory tracking using a module having two joints, one prismatic and the other revolute: (a) schematic diagram showing the maneuvers for a horizontal straight-line tracking; (b) discrepancy betweem the actual and prescribed paths. 119 7. CONTROLLED BEHAVIOUR OF THE PROTOTYPE MANIPULATOR 7.1 Preliminary Remarks The integrated prototype robotic manipulator described in Chapter 2 was now operated with the control scheme designed in the preceding chapter. The DMC-1000 controller card, used to interface the manipulator's sensors and actuators to the host computer, has a built-in PID control filter in its hardware. This feature allows input of the control parameters directly to a predefined memory address. The motion controller is a part of the complete servo-system as shown in Figure 7-1(a). Its basic functions are to structure the motion (motion profile generation), decode the quadrature signals of the encoders, close the feedback control loop, and perform PID system compensation as shown in Figure 7-1(b) [39]. In addition to these basic functions, the DMC-1000 performs high level routines such as processing of commands from the host computer, program sequencing , input/output processing, and error handling. These functions allow the motion controller to operate as a complete, stand-alone unit. The computer software, written in the C++ language, serves as manager for data exchange and coordinator of the above functions. A copy of the source code is included in Appendix V . 7.2 Performance of the Prototype Manipulator with PID Control The controller model and its numerical parameters were applied to the prototype manipulator for experimental dynamical observations. In order to assess its effectiveness when controlling the actual system, maneuvers similar to those presented in Chapter 6 were applied and response results correlated. A 120\u00C2\u00B0 slewing maneuver of module 1 and the corresponding trajectory of the end-effector are presented in Figure 7-2. A l l the remaining joints and the base are held fixed. As before, the maneuver is completed in 8 s. Each module is taken to be 60 cm 120 (a) Host Computer Controller Motion /^ ~>v ^ Compensation Program Filter Amplifier Motor Encoder Mechanical Coupling (b) Load Command Profile Generator Filter Amplifier Position Decoder Encoder Figure 7-1 Flow charts for the control system: (a) system motion control elements; (b) controller functional elements. 121 ' 1 I I I I I I I I I I I I I I I I I I I I I I 1 0 2 4 6 8 10 Time, s Figure 7-2 A comparative study of system responses to a 120\u00C2\u00B0 slew maneuver at module 1: (a) joint trajectory. Figure 7-2 A comparative study of system responses to a 120\u00C2\u00B0 slew maneuver at module 1: (b) end-effector trajectory. long, which corresponds to its maximum deployed length. The magnified inserts suggest that the prototype manipulator follows the specified path with higher accuracy than the predicted trajectory obtained through simulation. It is important to point out that, in the prototype system (manipulator), damping leading to energy dissipation are present at the joints. This is one of the reasons why the overshoot as well as the residual vibration, present in simulation, seem to be suppressed in the experimental results. The model used in simulation, as mentioned before, does not account for dissipation. The joint position during the experiment is obtained from the optical encoder mounted on the rotor shaft of the servo-motor. Although the connection between the rotor shaft and the link has high flexural rigidity, small deviation may persist. More accurate data, reflecting the actual position of the link, can be obtained from an encoder installed at the link. This improvement has been proposed and its implementation is planned in the near future. The 120\u00C2\u00B0 slewing maneuver was also performed by module 2 (with all the other joints and base held fixed). The initial configuration is the same as before. Again, as shown in Figure 7-3, results indicate that the system operates extremely well under the controller regulation. The trajectory path followed by the end-effector is also quite precise. Figure 7-4 compares the trajectories followed by the end-effector during the 120\u00C2\u00B0 slew maneuver carried out by both module 1 and module 2, simultaneously. As predicted by the simulation results, the actual end-effector path in the experimental test is not significantly affected despite the dynamic coupling between the two revolute joints. Next, a straight-line tracking using the two revolute joints was undertaken as shown in Figure 7-5. The mobile base as well as the prismatic joints were locked. The commanded time histories of the revolute joints at module 1 and 2 are also indicated. Note, the deviation of the end-effector position from the specified path is quite 124 I I I I ' l I I I I I I I I I I I I I I I I I 1 1 1 1 0 2 4 6 8 10 Time, s Figure 7-3 Comparison of experimental and simulated results for a 120\u00C2\u00B0 slew maneuver at module 2: (a) slew time histories. to <3\ 1.0 0.5 -X 0, m 0.0 -0.5 Specified path Base (fixed) Module 1 (fixed) _i i i i i i i i i_ -0.5 0.0 0.5 Simulation path Module 2 1.0 t = 0 1.5 Y 0 ,m Figure 7-3 Comparison of experimental and simulated results for a 120\u00C2\u00B0 slew maneuver at module 2: (b) end-effector trajectories. Figure 7-4 Path followed by the end-effector during a 120\u00C2\u00B0 slewing maneuver simultaneously carried out by modules 1 and 2. t= 8 s e 2 = -80\u00C2\u00B0 Module 1 Module 2 9, = 40 o t=8 - \u00E2\u0080\u00A2 Y n r = 0 e? Module 2 0.10 -Xn, m 0.00 -0.10 0.5 Specified path Simulation path \u00E2\u0080\u0094 \u00E2\u0080\u0094 . A ^ . -AT Experimental path 0.8 1.0 1.2 Y 0, m 1.5 Figure 7-5 Accuracy of a horizontal straight-line tracking maneuver using the two revolute joints of the system. 128 perceptible. Although the trajectory of the slewing joints are observed to be precise (less than 0.01\u00C2\u00B0 deviation), there is an error in the position of the end-effector that fluctuates between \u00C2\u00B1 2 mm. Figure 7-6 shows the straight-line tracking with the two prismatic joints, i.e. deployment of the links. Similar to the earlier simulation results (Figure 6-11), there is virtually no error in the end-effector path suggesting absence of the dynamic coupling, which is expected for this particular configuration. Finally the same tracking maneuver was executed by combining the capability of the slewing and deployable motions with a single module. From Figure 7-7, it is apparent that the actual path does not correlate precisely with the simulation prediction or the specified trajectory. However, magnitude of the maximum deviation still remains quite small (\u00C2\u00BB 2 mm). In summary, the close correlation of the experimental data with the simulation results indicates that, the behaviour of the prototype manipulator can be predicted quite precisely with the mathematical model developed in the thesis. In addition, the PID controller, designed using this mathematical model, shows excellent performance when implemented on the actual prototype system. 129 Module 1 Module 2 a X I Beginning End O a d\u00E2\u0080\u009E m 0.6 Module 1 h , m 0.5 Module 2 0 2 4 6 8 10 Time, s 0.10 -X0, m Specified path Simulation path 0.00 Experimental path -0.10 0.5 0.8 1.0 1.2 Y 0,m 1.5 Figure 7-6 Straight-line tracking with prismatic joints. 130 t=Ss End Base (held fixed) ^4r Beginning -120 -X0, m 0.00 h -0.01 0.5 Slewing link 0.66 d, m 0.63 0.60 Deployable link I ! I I I L _ 0 2 4 6 8 10 0 2 4 6 8 10 Time, s Time, s 0.01 h Simulation path Experimental path Specified path 0.8 1.0 1.2 1.5 Y0, m Figure 7-7 Straight-line tracking using revolute and prismatic joints of a sing module. 131 8. CONCLUDING REMARKS 8.1 Contributions The main contributions of this project can be summarized as follows: \u00E2\u0080\u00A2 A prototype variable geometry robotic manipulator has been designed, integrated and is operational. Construction and operation of such a manipulator has not been reported in open literature. \u00E2\u0080\u00A2 A mathematical model for predicting dynamics of a mobile manipulator with slewing, deployable links (two modules, four links) has been developed (accounting for revolute joint and payload flexibility), associated integration algorithm written, and their validity has been assessed. This has provided vital information about both uncontrolled dynamics as well as controlled behaviour of this novel manipulator system. \u00E2\u0080\u00A2 Application of the classical PID controller to the simulation model as well as the prototype represents a modest but innovative and fresh beginning in controlling this new class of manipulators. 8.2 Conclusions Based on the investigation, the following general conclusions can be drawn: \u00E2\u0080\u00A2 Significant coupling can exist among the base, joints and payload vibrations. In particular, operation of a revolute joint induces vibrations of the flexible members that have relatively significant amplitude. In contrast, deployment maneuvers, as anticipated, result in negligible disturbances. \u00E2\u0080\u00A2 The module located further away from the base (module 2) is more prone to coupling effects. On the other hand, motion of module 2 has very little effect on module 1 or 132 the base. Furthermore, the smaller the mass of module 2 (compared to module 1), the greater disturbance it experiences and smaller the effect it has on other members. \u00E2\u0080\u00A2 It is recommended to use the capability of module 1 first in approximately locating module 2 near the region of interest. Subsequently, fine manipulation of module 2 should be used to accomplish the task. This promises to reduce demand on the control effort and improve accuracy. Thus in situations where high level of accuracy is required, module 1 should act as the macro-unit responsible for the gross manipulation and module 2, of smaller size, should provide the fine manipulation for the final approach. \u00E2\u0080\u00A2 Controlled maneuvers show that prismatic motions (i.e. deployment and retrieval) can track a trajectory with an accuracy of an order of magnitude higher than that achieved by revolute joints. \u00E2\u0080\u00A2 The presence of a flexible beam-type payload has little effect on the manipulator dynamics. \u00E2\u0080\u00A2 The simulation results show that magnitude of forces and torques recorded during the most demanding maneuvers are well within the design limits. This assures that the prototype robotic unit should function satisfactorily. Actual tests with the prototype manipulator confirms this observation. 8.3 Recommendations for Future Work Based on the simulation results obtained and limited operation of the prototype manipulator, several avenues are suggested for attention in the future which are likely to be fruitful: \u00E2\u0080\u00A2 Develop an 0(N) formulation for three-dimensional dynamics of the system. This would assist in real-time implementation of various control strategies. \u00E2\u0080\u00A2 Simulate various linear, nonlinear, adaptive and knowledge based control strategies to assess their relative merit. Implement them on the prototype manipulator. 133 Develop smaller, lighter manipulator with larger number of modules to demonstrate versatile performance of the system in obstacle avoidance, trajectory tracking, capture dynamics, etc. in presence joint failure. Redundancy of the manipulator should also aid achieving optimal performance with respect to specified criteria (minimum time, minimum effort, suitable combination of the two, etc.). Study stability of the manipulator for different configurations. Closed-loop stability in presence of various structures of sensory feedback may be important. Simplify the control system by separating fine and gross manipulation tasks. Develop an intelligent and hierarchical, supervisory control system for the manipulator for executing complex tasks. Installation of position encoders at the slewing link should provide readings that would more accurately predict the actual position of the manipulator module. Animation of the simulation results would help in physical appreciation of the complex dynamical interactions involved. 134 LIST OF REFERENCES [1] Lavery, D., \"The Future of Telerobotics\", Robotic World, Summer 1996. [2] Denavit, J., and Hartenberg, R.S., \"A Kinematic Notation for Lower Pair Mechanism\", J. Applied Mechanics, Vol . 22, 1955, pp. 215-221. [3] Spong , M.W., and Vidyasagar, M . , Robot Dynamics and Control, John Wiley & Sons, Inc., U.S.A., 1989, pp. 3-4. [4] Lips, K.W., and Modi, V.J . , \"Flexible Appendages Deployment Dynamics as Applied to Satellite Attitude Motion\", Acta Astronautica, Vol . 5, 1978, pp. 787-815. [5] Lips, K.W., and Modi, V.J . , \"Three-Dimensional Response Characteristics for a Spacecraft with Deploying Flexible Appendages\", Journal of Spacecraft and Rockets, American Institute of Aeronautics and Astronautics, Vol . 4, 1981, pp. 650-656. [6] Modi, V.J . , and Ibrahim, A . M . , \"A General Formulation for Librational Dynamics of Spacecraft with Deploying Appendages\", Journal of Guidance, Control, and Dynamics, Vol . 7, 1984, pp. 563-569. [7] Modi, V.J . , and Shen, Q., \"On the Dynamics of Spacecraft with Flexible, Deployable and Slewing Appendages\", Advances in the Astronautical Sciences, Editors: A . K . Misra et al., Univelt Incorporated Publisher for the American Astronautical Society, San Diego, U.S.A., Vol . 85, Part I, pp. 877-896. [8] Lips, K.W., Dynamics of a Large Class of Satellites with Deploying Flexible Appendages, Ph.D. Thesis, University of British Columbia, Vancouver, B.C., Canada, 1980. [9] Ibrahim, A . M . , Mathematical Modelling of Flexible Multibody Dynamics with Application to Orbiting Systems, Ph.D. Thesis, University of British Columbia, Vancouver, B.C., Canada, 1988. [10] Shen, Q., On the Dynamics of Spacecraft with Flexible, Deployable and Slewing Appendages, Ph.D. Thesis, Simon Fraser University, Burnaby, B.C., Canada, 1993. 135 [11] Marom, I., and Modi, V.J . , \"On the Dynamics and Control of the Manipulator with a Deployable Arm\", AAS/AIAA Astrodynamics Specialist Conference, Victoria, B.C., Canada, August 1993, Paper No. AAS-93-670; also Advances in the Astronautical Sciences, Editors: A . K . Misra et al., Univelt Incorporated Publisher for the American Astronautical Society, San Diego, U.S.A., Vol . 85, Part III, pp. 2211-2229. [12] Modi, V.J . , Chen, Y . , Misra, A .K . , de Silva, C.W., and Marom, I., \"Nonlinear Dynamics and Control of a Class of Variable Geometry Mobile Manipulators\", Proceedings of the Fifth International Conference on Adaptive Structures, Sendai, Japan, December 1994, Editor: J. Tani et al., Technomic Publishing Co. Inc., Lancaster, PA, U.S.A., pp. 140-149. [13] Hokamoto, S., Modi., V.J . , and Misra, A . K . , \"Dynamics and Control of Mobile Flexible Manipulators with Slewing and Deployable Links\", A A S / A I A A Astrodynamics Specialist Conference, Halifax, Nova Scotia, Canada, August 1995, Paper No. AAS-95-322; also Advances in the Astronautical Sciences, A A S Publications office, San Diego, California, U.S.A., Editors: K. Terry Alfriend et al., Vol . 90, pp. 339-357. [14] Hokamoto, S., and Modi, V.J . , \"Nonlinear Dynamics and Control of a Flexible Space-Based Robot with Slewing-Deployable Links\", Proceedings of the International Symposium on Microsystems, Intelligent Materials and Robots, Sendai, Japan, September 1995, Editors: J. Tani and M . Esashi, pp. 536-539. [15] Hokamoto, S., and Modi, V.J . , \"Formulation and Dynamics of Flexible Space Robots with Deployable Links\", Transactions of the Japan Society for Mechanical Engineers, Vol . 62, No. 596, 1996, pp. 1495-1502. [16] Hokamoto, S., Kuwahara, M . , Modi, V.J . , and Misra, A . K . , \"Control of a Flexible Space-Based Robot with Slewing-Deployable Links\", Proceedings of the AIAA/AAS Astrodynamics Conference, San Diego, California, U.S.A., July 1996, A I A A Publisher, Paper No. AIAA-96-3626-CP, pp. 506-513. [17] Caron, M . , Planar Dynamics and Control of Space-Based Flexible Manipulators with Slewing Deployable Links, M.A.Sc. Thesis, University of British Columbia, Vancouver, Canada, 1996. [18] Caron, M . , Modi, V.J . , Pradhan, S., de Silva, C.W., and Misra, A . K . , \"Planar Dynamics of Flexible Manipulators with N Slewing Deployable Links\", Proceedings of the AIAA/AAS Astrodynamics Conference, San Diego, California, U.S.A., July 1996, A I A A Publisher, Paper No. AIAA-96-3625-CP, pp. 491-505. 136 [19] Pradhan, S., Modi, V.J . , and Misra, A . K . , \"Order N Formulation for Flexible Multibody Systems in Tree Topology - the Lagrangian Approach,\" Proceedings of the AIAA/AAS Astrodynamics Conference, San Diego, California, U.S.A., July 1996, A I A A Publisher, Paper No. AIAA-96-3624-CP, pp. 480-490; also Journal of Guidance, Control, and Dynamics, Vol 20, No. 4, July-August 1997, pp. 665-672. [20] Schmitz, E., and Ramey, M.F., \"Experiments in Active Control for Large Space Manipulators\", First Annual Report to AFOSR, Contract F49620-88-C-0037, Martin Marietta Space Systems Company, July 1989. [21] Yoshikawa, T., Hosoda, K. , Doi, T., and Murakami, FL, \"Dynamic Trajectory Tracking Control of Flexible Manipulator by Macro-Micro Manipulator System\", Proceedings of the IEEE International Conference on Robotics and Automation,^ Diego California, U.S.A., May 1994, Vol . 2, pp. 1804-1809. [22] Bridges, M . M . , Dawson, D .M. , and Martindale, S.C., \"An Experimental Study of Flexible Joint Robots with Harmonic Drive Gearing\", Proceedings of the Second IEEE Conference on Control Applications, Vancouver, B.C., Canada, September 1993, Vol . 2, pp. 499-504. [23] Lin, Z.C., and Patel, R.V., \"Cartesian Control of Redundant Flexible-Joint Manipulators\", Proceedings of the 32nd Conference on Decision and Control, San Antonio, Texas, U.S.A., December 1993, Vol . 1, pp. 605-610. [24] Muller, P.C., and Soffker, D., \"Modelling, Simulation and Control of Elastic Robot Arms with Prismatic Joints\". Euromech-Colloquim, Miinchen, September, 1990, pp. 268. [25] Muller, P.C., Giirgoze, M . , and Soffker, D., \"Modelling of Elastic Robot Arms with Revolute or Prismatic Joints for High Accurate Position Control\", Proceedings of the 8th Symposium on the Theory and Practice of Robots and Manipulators, Cracow, Poland, July, 1990. [26] Soffker, D., Giirgoze, M . , and Muller, P.C., \"Modelling, Simulation and Control of Elastic Robot Arms with Prismatic Joints\", Proceedings of the 8th IFToMM World Conference of the Theory of Machines and Mechanism, Prague, August, 1991, Vol . 4, pp. 1249-1252. [27] Soffker, D., \"Dynamics of Elastic Robot Arms with Varying Length - Systematic Modelling, Simulation, and Control\", Proceedings of the 10th Symposium on the Theory and Practice of Robots and Manipulators, Gdansk, Poland, September, 1994. 137 [28] Soffker, D., \"Elastic Robot Arms with Varying Length - Part I: A Systematic Nonlinear Modelling Approach\", Vibration of Nonlinear, Random, and Time-Varying Systems - Time Varying Systems and Structures, Editors: Sinha, S.C., Cusumano, J.P., and Pfeifer F., A S M E DE - Vol . 84-1, 1995, pp. 109-114. [29] Soffker, D., \"Elastic Robot Arms with Varying Length - Part II: Robust Control of Elastic Vibrations\", Vibration of Nonlinear, Random, and Time-Varying Systems -Time Varying Systems and Structures, Editors: Sinha, S.C., Cusumano, J.P., and Pfeifer F., A S M E DE - Vol . 84-1, 1995, pp. 115-119. [30] Greenwood, D.T., Principles of Dynamics, Prentice-Hall International, Inc., Englewood Cliffs, New Jersey, U.S.A., 1965, pp. 229-267 [31] Rivin, E.I., Mechanical Design of Robots, McGraw-Hill Book Company, New York, U.S.A., pp. 123, 205-269. [32] de Silva, C.W., Control Sensor and Actuators, Prentice-Hall, Inc., Englewood Cliffs, New Jersey, U.S.A., 1989, pp. 30, 225-235. [33] DMC-1000 Technical Reference Guide, Version 1.1 A, Galil Motion Control, Inc., Sunnyvale, California, U.S.A., 1993. [34] Hurty, W.C., and Rubinstein, M.F., Dynamics of Structures, Prentice Hall, Inc., Englewood Cliffs, New Jersey, U.S.A., 1964, pp. 28-35, 90-103. [35] Goodman, L.E. , and Warner, W.H., Dynamics, Wadsworth Publishing Company, Inc., Belmont, California, U.S.A., pp.113-120, 257-270, 581-583. [36] Rao, J.S., and Gupta, K. , Introductory Course on Theory and Practice of Mechanical Vibrations, Halsted Press Book, New York, U.S.A., 1984, pp.31-58, 240-241,345-357. [37] Franklin, G.F., Powell, J.D., and Emami-Naeini, A. , Feedback Control of Dynamics Systems, Addison-Wesley Publishing Company, Inc., Massachusetts, U.S.A., 1986, pp. 103-106. [38] Potvin, A.F. , Nonlinear Control Design Toolbox, The Math Works, Inc., Massachusetts, U.S.A., 1993. [39] Tal, J., Advanced Motion Applications, Galil Motion Control, Sunnyvale, California, U.S.A., January, 1991. 138 APPENDIX I: COMPONENTS AND CHARACTERISTICS OF THE PROTOTYPE MANIPULATOR DMC-1000 Motion Controller Manufacturer: Galil Motion Control, Inc. 575 Maude Court, Sunnyvale, C A 94086-2803, U.S.A. Supplier: Electromate Industrial Sales limited 4300 Steeles Avenue West, Unit #35, Woodbridge, Ontario, L4L 4C2, Canada. Important characteristics: \u00E2\u0080\u00A2 32-bit processing; \u00E2\u0080\u00A2 125 u.S per axis servo update rate; \u00E2\u0080\u00A2 build in PID filter with velocity and acceleration feedforward; \u00E2\u0080\u00A2 14-bit D A C ; \u00E2\u0080\u00A2 8,000,000 counts/s encoder feedback for high resolution; \u00E2\u0080\u00A2 500-line memory for custom application programs (2000 lines with - M X option). \u00E2\u0080\u00A2 multitasking features permits simultaneous execution of four independent applications programs; \u00E2\u0080\u00A2 programmable acceleration and deceleration with S-curve profiling to eliminate jerk; \u00E2\u0080\u00A2 relative and absolute positioning with more than \u00C2\u00B1 2,000,000,000 counts per move; \u00E2\u0080\u00A2 error handling including programmable software limits, automatic error shut-off, amplifier enable, user-defined error subroutines, and watchdog timer; 139 \u00E2\u0080\u00A2 programmable event triggers for positioning elapsed time, position, speed, and motion complete; \u00E2\u0080\u00A2 comprehensive status reporting for position, speed, torque, error, and inputs; \u00E2\u0080\u00A2 126 symbolic variables and 1600 element array space (expandable to 510 variables and 8000 elements with - M X option) for data storage. Note: The DMC-1000 Technical Reference Guide Version 1.1 A provides the following information: \u00E2\u0080\u00A2 description of the controller card; \u00E2\u0080\u00A2 specifications; \u00E2\u0080\u00A2 complete list of the controller features; \u00E2\u0080\u00A2 modes of motion; \u00E2\u0080\u00A2 command summary; \u00E2\u0080\u00A2 connection and interface procedure. AMP-1140 Mating Power Amplifier Features: \u00E2\u0080\u00A2 7 amps continuous, 10 amps peak; 20 to 80 V ; \u00E2\u0080\u00A2 connects directly to the DMC-1000 series controller via ribbon cables; \u00E2\u0080\u00A2 available with 1, 2, 3, or 4 amplifier(s); \u00E2\u0080\u00A2 screw-type terminals for easy connection to motors, encoders, and switches; \u00E2\u0080\u00A2 steel mounting plate with 0.25\" keyholes. Specifications: \u00E2\u0080\u00A2 minimum motor inductance = 1 mH; \u00E2\u0080\u00A2 P W M frequency = 30 kHz; \u00E2\u0080\u00A2 ambient operation temperature = 0 to 70 \u00C2\u00B0C; \u00E2\u0080\u00A2 dimensions = 5.7\" x 13.4\" x 2.5\"; \u00E2\u0080\u00A2 weight = 4 lbs; 140 \u00E2\u0080\u00A2 mounting keyholes = 0.25\" DIA; \u00E2\u0080\u00A2 gain = 1 amp/V. Module 1 Slewing Servo-Motor Manufacture: Pittman Harleysville, PA, 19438-0003, U.S.A. model: 14202D30G peak torque (stall): 109.34 oz-in no load speed: 4,250 rpm voltage: 30.3 V torque constant: 9.85 oz.in/A back EMF constant: 7.29 V/krpm terminal resistance: 2.74 ohm inductance: 4.05 mH no load current: 0.19 A peak current (stall): 11.1 A Module 1 Deployment Servo-Motor model: 9414K283 peak torque (stall): 24.4 oz-in no load speed: 3,050 rpm voltage: 24.0 V torque constant: 4.13 oz.in/A back EMF constant: 3.05 V/krpm terminal resistance: 4.06 ohm inductance: 3.25 mH 141 no load current: 0.14 A peak current (stall): 5.91 A Module 2 Slewing Servo-Motor model: 9413J763 peak torque (stall): 16.128 oz-in no load speed: 3,050 rpm voltage: 24.0 V torque constant: 5.60 oz.in/A back EMF constant: 4.14 V/krpm terminal resistance: 8.33 ohm inductance: 6.17 mH no load current: 0.10 A peak current (stall): 2.88 A Module 2 Deployment Servo-Motor model: 9414J764 peak torque (stall): 24.4 oz-in no load speed: 3,050 rpm voltage: 24.0 V torque constant: 4.13 oz.in/A back E M F constant: 3.05 V/krpm terminal resistance: 4.06 ohm inductance: 3.25 mH no load current: 0.14 A peak current (stall): 5.91 A Bayside Precision Gearhead with BCI Adaptor Kits 142 Manufacturer: Bayside Control Inc., 27 Seaview Boulevard, Port Washington, N Y 11050, U.S.A. model: 23-020 gearratio: 20:1 S/N: 16799 continuous torque: 50 in-lbs peak torque: 100 in-lbs backlash standard: 30 min backlash low: 15 min input speed: 4000 rpm minimum efficiency: 90 % maximum inertia 7.4 x 10\"5 oz-in-sec2 radial load: 20 lbs axial load: 40 lbs maximum weight: 1.2 lbs Star Precision Ball Screw Assemblies Manufacturer: Star Linear System 9432 Southern Pine Blvd., Charlotte, N C 28273, U.S.A. model: DIN 69051T.5 ball screw: 1531-2-3025; 280 mm length; 8 mm diameter; 2.5 mm lead, adjustable preload single nut: 1532-2-3004. 143 INA Linear Axial Bearing Manufacturer: INA Bearing Company Ltd. 5185 Tomken Road, Mississauga, ON, L4W 1P1, Canada. Supplier: BC Bearing Engineering Limited 3905 East 2nd Avenue, Burnaby, B.C., V5C 3W9, Canada. model: KNZ24PP; 1.5\" I.D.; 2.375\" O.D.; 3\" long. Aluminum Tubing Supplier: Wilkinson Steel & Metals-A Division of Premetalco Inc. 888 SE Marine Drive, Vancouver, B.C. Canada. type: Alcan 6061-T6 drawn seamless aluminum tubing, slewing link: 2-1/2 \" O.D.; 0.083\" thick, deployable link: 1-1/2 \" O.D.; 0.095 \" thick. 144 APPENDIX II: DESIGN AND ASSEMBLY DRAWINGS OF THE PROTOTYPE MANIPULATOR (II.A1) General Assembly of the base and module 1 of the prototype manipulator. 145 146 I I |fc*IC NOTES' I REMOVE ALL BURRS AND SHARP EDGES. 2. THE BEARING HOLES ON ITEM 9 1 6 , PIVOT BASE TOP 8. BOTTOM ARE TO BE MACHINED TO ASSURE TIC ALIGNMENT Of ITEM 10 8. 11. PIVOT SHAFT TOP I BOTTOM. 0 SIDE VIEW CHANG D tlW PIVCI PIAIC. M I C D * Pivot PLAIC I or MfVOI MOIOB llPPWI AMjU SLCU M r v SUPPORT \u00C2\u00AB - \u00C2\u00AB I M C v i n e \u00C2\u00BB i U M W t-x I M C una iscxti soaw t-\u00C2\u00BB IMC u\u00C2\u00ABLC I Q O C I i t x u l / * - U I M C K * . *4ll I M C M X . M U I otfv \u00C2\u00AB H M M ; ID, i t n.cw AIM NOH\u00C2\u00BB M P t O l X M l POICO HAftlHC UW1 RltC MAftUC M U N I UCV AM OTPOH \u00E2\u0080\u00A2 MPIIWC Know MMPUAlOl i\u00C2\u00AB2M wion ax tv t PIVOI \u00C2\u00BB w i . tax TO* PIVOT SMA/I. IOP PIVDI SUPPORT AMkt PIVOI SLPPOtl PI*lt PIVOI IAJC. totroH UAIL PlAIC rROMl VM.L PLAIC, ItAA \u00E2\u0080\u00A2ASI MOMIIMC Pi Alt U K I A t AI U N H I CiCClIP 11- L CX*C D H , Mb BAICOll |tcv. I octetslit* OO . 0 0 REAR VIEW SECTION *A-A* I. REMOVE ALL BURRS AND SHARP EDGES. a THE BEARING HOLES DN ITEM 9 1 6 . PIVOT BASE TOP I BOTTDM ARE TO BE MACHINED TO ASSURE THE ALIGNMENT OF ITEM 10 L II, PIVOT SHAFT TOP 8. BOTTOM. MAVN IV> V. OUNC U It* CtCCMft IT* U CKWlJ Mic. m a n I fivt to i *um (II.A2) Details of the manipulator base. 149 - 1 1 . 2 5 -3- !'! SIDE VIEW B 0 . 3 7 5 6 H O L E S 3- -()-3 . 2 5 =3-3 . 2 5 t?5-1 . 0 0 -(>--- 4 . 6 2 5 - - 4 . 6 2 5 -TDP VIEW N O T E S ' 1 R E M O V E A L L B U R R S A N O S H A R P C O R N E R S - 0 . 1 8 7 5 3 / 8 - 1 6 U N C 6 H O L E S END VIEW D*A\M If' V . CKAHG D I.UK \u00E2\u0080\u00A2 O H * \u00C2\u00ABt1 8 . 0 0 OCCEtP H- L C K K . MIC r t t \u00C2\u00BB M DVB \u00C2\u00BB 0 \u00E2\u0080\u00A2 * l f o i l I * tv . I KWtDi S I D E V I E W E N D V I E W NOTES. I. REHOVE ALL BURRS AND SHARP CORNERS I 2} DRILL TICSC 4 HOLES FDR ITEM 4. WALL PLATE. REAR. FOR MOUNTING ITCM7. PIVOT SUPPORT PLATE ON DRAWING BASC00I ITCH 5. VALL PLATE. FRONT. DO NOT HAVE THESE ruuR HOLES. U U l PlATC. Ifjul l|TLC>HM\u00C2\u00ABPU.ArCf) 1ASC. V A I L n \u00C2\u00BB i c r t w i . WJJ M A U N IT> V. CHANG ft IIM CWCKtfi II- L. Ct\u00E2\u0082\u00AC\u00C2\u00AB | l A i c . m a n mv. | K m n m II.B5 S I D E V I E W MD1CS> I. RCMJVC ALL BURRS AND SHARP CORNERS C2>tMIS HOLE HAS TO BE ALIGNED VITH 1HE HOLE IN DUG BASE008. PIVOT BASE. TOP TO ASSURC THE ALIGNHNET Dr THE TVO PIVOT SHAFT S. SEC DUG BASCOOL GENERAL ASSEMBLY c i c a r p II. L Q\u00E2\u0082\u00ACtc fc\u00C2\u00BBrt. ret \u00C2\u00AB n PHAUN \u00E2\u0080\u00A2 * V. CHWC ft It* DWC K> ftUCOM 6.5799 ~[ REV. | 1 APP. BY CIA 11 -2.00-FDP VIEW 1/4-20 U N C o n n i . c s -0.6875 3.3924 \u00E2\u0080\u0094 1.00 SIDE VIEW NOTES' 1. RCMOVE A L L BURRS AND SHARP CORNERS END VIEW - i ITEM SPEC. DESCRIPTION orr . COMPONENTS TITLE. MANIPULATOR BASE PIVOT SUPPORT PLATE DRAVN BY< V. CHAMO D. LUK TOLERANCES .XI * 01 .mix ton .MXKX IDOOl CHECKED BY' I CHENG SCALE' NTS DATE' TEB 9, 9b DVG NO BASEOOfc DIM. INCH PAGE' 1 OF 1 EVT| i APP. Br OAIC 1.00 J L 50 J .so \u00E2\u0080\u0094 4 -<) ()-1 \u00E2\u0080\u00A22.00-0.1075 J 1/1-20 UNC 4 HDL.ES -1.00 FRONT VIEW 2.00 -0.1875 SIDE VIEW .50 1 \ r 1 1 7 ,50-f ) 5 0 0 . 5 0 - ^ \u00E2\u0080\u0094 1 BOTTOM VIEW NQTES-1. REMOVE A L L BURRS AND SHARP CORNERS -2 -1 ITEM SPEC. DESCRIPTION OIY. COMPONENTS TITLE' MANIPULATOR BASE PIVOT SUPPORT ANGLE DRAVN BYi V , CHANG D. LUK Xtt *.OOI KMX 1.00OI ANGU.AA II/? CHECKED BY. L. CHENG DATE. EEB 8, 95 SCALE. NTS DVG f O BASE007 R E V . DESCRIPTION A P P . BY DATE \u00E2\u0080\u0094 00.945 i :i:r\u00E2\u0080\u0094-i-E5Ji: I I 0.1875-^ H h- \u00C2\u00AB\u00C2\u00BBl.0239 /-p-i O1.0234 v - e - J SIDE VIEW 8*0.1875 4 HOLES EQ. SP. ON 01.350 B.C. 1.000 \u00E2\u0080\u00A27.375-+ - O \u00E2\u0080\u0094 1.375 1.00-\u00C2\u00A3 .50 1.00 I \u00E2\u0080\u0094 .50 F R O N T E N D V I E W NOTES: 1. REMOVE A L L BURRS AND SHARP CORNERS BOTTOM VIEW R E A R E N D V I E W _2>THIS HOLE HAS TO BE ALIGNED WITH THE HOLE IN DWG BASE005, PIVOT BASE, BOTTOM TO ASSURE THE ALIGNMENT OF THE TWO P I V O T SHAFTS. SEE DWG BASE00I, GENERAL ASSEMBLY -1 ITEM S P E C . DESCRIPT ION QTY. COMPONENTS T I T L E ' MANIPULATOR B A S E P I V O T B A S E . T O P D R A V N BY' V . CHANG D. LUK T O L E R A N C E S C H E C K E D BY. L. C H E N G D A T E . F E B 8. 95 S C A L E . NTS D V G N O BASE 008 0 TDP V I E W NOTES' 1. R C M O V L A L L B U R R S A N D S H A R P C D G L S . IIILt.HA*flPUL*lt\u00C2\u00BB IAiC I O U P A N C U CtttKCD M< L CMMi It Al l - Ni l M I C - man tttAVN If. V . C1IAHG D I UK >IH INCH P A U > ) 1 \u00E2\u0080\u00A2 I V . D C S C \u00C2\u00AB U M D 4 II M i l Nortsi 1. RtMOVC ALL BURRS AND SHARP CURNCRS 12.00 -4--2.375-16.00 \u00E2\u0080\u00A24-\u00E2\u0080\u0094 4.625-3/8-16 UNC 10 HOLES ---6--2.375-4.625 \u00E2\u0080\u0094 wo 10.00 1.00-\u00E2\u0080\u0094 1.00 T D P V I E W E N D V I E W .375 T S I D E V I E W MAWH il' V. CHWC IX LUK [HtCKf D 11- U CHOC MHl' MX Ovt. t o I a I U U R C V . DESCRIPT ION A P P . B r U A l l 00.1875 4 HOLES 0 0.1875 4 HOLES EQ. SP. ON 01.531 B.C. 0.1875 TYP 6 - 3 2 UNC x 1/2 DEEP 7 HOLES 0.500 TYP NOTES' 1. REMDVE ALL BURRS AND SHARP CORNERS CD ASSEMBLY < 4 6 - 3 2 UNC x 1 /2 LG S O C K E T S C R E W 4 3 \u00E2\u0080\u00A2 0 . 3 7 3 x 1.75 L G A L . ROD 1 2 3 .B2S \u00C2\u00BB 2.50 x 0.125 A L . P L A T E 1 I G E N E R A L MOTOR SUPPORT A S S E M B L Y - 2 -1 ITEM S P E C . DESCRIPT ION OTY. COMPONENTS TITLE. MANIPULATOR BASE SLEW HOIOR SUPPORT TOLERANCES .KK 4.01 .XXX 1.001 XXXX 1.0001 ANGULAR 11/2 CHECKED BY. L. CHENG SCALC NTS DATE. TEB 8, 95 DWG NO BASE009 DRAWN BY. V . CHANG D. LUK DIM. INCH PAGE. 1 Of 1 Slewing link sub-assembly. 159 y// ///////n/,. O O O O O O O O O O O D c g o o o o o o o o o o SLEW ***Xi <>ecu\u00C2\u00AB\u00C2\u00A3 v\u00C2\u00ABttw s e r feces ui\u00C2\u00A3 R A t f W M / M A C H i M C T > \u00C2\u00ABU-TVS. \ SLEW A Q M IS JO\u00C2\u00BB^T TO T H E 6HD C A P By Ul.) A b A P T i y e \u00E2\u0080\u00A2 R O B O T I C M A N I P U L A T O R (II.C) Deployable link sub-assembly. 161 162 Dimensions of the slewing link. 163 (II.E) Dimensions of the deployable link. J 165 166 (II.F) Dimensions of the slewing link and motor support brackets. 167 (II.G) Components dimensions and details. 169 170 (II.H) Components dimensions and details (continued). 171 t ( M ) L \u00C2\u00A3 M > SCREW E \u00C2\u00BB D STOP (tans FIT W\"H HDfc'PT IVE ' R O B O T I C M&MipiJLATCXc, COMBOMEfJTS ( 5 V C\"0 S E T - '<7 T H I S H O L E HAS TO BE A L I G N E D W I T H THE H O L E IN DWG MANI003 , P 1 V D ! B A S E . BOTTDM TO A S S U R E THE ALIGNMENT* DE THE TWO P I V O T S H A F T S . S E E DWG MANI001, G E N E R A L A S S E M B L Y . 1 7 - 3 / 8 x 4 x 3 / 1 6 AL. PLAIE -1 QTY. ITEM SPEC. DESCRIPTION DASH ND COMPONENTS TITLE' MANIPULATOR ELBOW, PIVOT BASE, TOP TOLERANCES .xx *m .XXX 1.001 CHECKED BY' L. CHENG SCALE' NTS DATE' JAN 4. 95 DVG NO. HANI 002 iBO.1875 4 H O L E S EQ. SP . ON 01.350 B.C. 6 - 3 ? UNC 4 HEJLES 00 .3125 2 H O L E S 0.750 T Y P . \u00E2\u0080\u0094 0.750 -0 .0938 - - 7 . 3 7 5 -01 .0239 \"01.0234 \u00E2\u0080\u0094 1.500 \u00E2\u0080\u0094 N D T E S i 1. R E M O V E A L L B U R R S AND S H A R P C O R N E R S 1.5800 T > T H I S H O L E H A S TO BE A L I G N E D W I T H THE H O L E IN DWG MAN1002, P I V O I B A S E , TOP TO A S S U R E THE A L I G N M N E T I T THE TWO P I V O T S H A F T S . S E E DWG MAN1001, G E N E R A L A S S E M B L Y . 1 4x2x3/16 7 - 3 / 8 L G AL. C-CHANNEL -1 QTY. H E M SPEC. DESCRIPTION DASH NO COMPONENTS TITLE. MANIPULATOR E l B Q V , PIVOT BASE, BDTTDM TOLERANCES .XX I.DI .XXX l.OUl CHECKED BY. L. CHENG DATE. JAN 4, S5 SCALE. NTS DUG NO. HANIOO'J --l i l i 0.0625 - 1 J . J 0.1875 4.000 oo NOTES'-1. R E M O V E A L L B U R R S AND S H A R P C O R N E R S 1.4375 \u00E2\u0080\u0094 1 5x4x3/16 AL. PLATE - 2 -1 ITEM S P E C DESCRIPTION QTY. COMPONENTS TITLE' MANIPULATOR ELBOW, PIVOT PLATE, TOP TOLERANCES .XX 1.01 CHECKED BY. L. CHENG DATE. JAN 4, 95 SCALE' NTS DWG NO- MAN 100 4 a = J = r n r~r-0 . 0 6 2 5 - 0,1875 \u00C2\u00B1 J o 5.500 1.325 1.8125 T Y P N O T E S ; 1. R E M O V E A L L B U R R S AND S H A R P C O R N E R S \u00E2\u0080\u00A232 UNC 2 HGILES 01 ,875 6 - 3 2 UNC 4 H O L E S 00 .394 1.4375 \u00E2\u0080\u0094 1 5 -1 /2x5x3 /16 AL. PLATE - 2 -1 ITEM SPEC. DESCRIPTION QTY. COMPONENTS TITLE' MANIPULATOR ELBOW, PIVOT PLATE, BOTTOM TOLERANCES ,xx X.0I .XXX 1.001 CHECKED BY' L. CHENG DATE' JAN 4, 95 SCALE' NIS D U G MO. HANI 005 0 I U B 7 5 4 H f l L T S EQ. 6-32 UNC x 1/2 DEEP B HOLES 1.6875 1 6 2 5 0.500 TYP 0 A S S E M B L Y N O T E S . 1. R E M O V E A L L BURRS AND S H A R P C O R N E R S A A 6-32 UNC x 1/2 LG SUCKLI SCREW A 3 e0.375 x 1.625 LG AL. ROD 1 2 3.825xl.5x0.<25'. AL. PLATE 1 1 GENERAL MOTOR SUPPORT (ISSCMBLT -a -1 ITEM SPEC . DESCRIPTION OTY. COMPONENTS T ITLE'MANIPULATDR ELBDW, MOTOR SUPPORT TOLERANCES .xx a.oi .XXX 1.001 CHECKED Br> L. CHENG DATE, JAN A. 95 SCALE' NTS DWI1 NO. MANIOOa 00.500 0 0 . 1 8 7 5 . 4 H O L E S EQ. SP . ON 01.000 B.C. 0.375 - H TYP. 1 3 1 2 5 0 . 5 6 2 5 -1.125-\u00E2\u0080\u0094c 1 4 --00 .1875 2 H O L E S N O T E S ; 1. R E M O V E A L L B U R R S AND S H A R P C O R N E R S 1 3 -5 /16x1-1 /8 I /8FH 2 - 5 / B L G AL. - B -1 ITEM SPEC. DESCRIPTION QTY. COMPONENTS MANIPULATOR ELBOW, MOTOR SUPPORT ANGLE TOLERANCES CHECKED BYi L. CHENG DATE' JAN 8. 95 SCALE' NTS l lWG N i l ' HANI009 REV. DESCRIPTION APP. DY I'JA'I E 0 , 8 7 2 2 0 0 . 3 9 3 9 0 0 . 3 9 3 6 3 / 8 - 1 6 U N C 0 0 . 5 0 0 N O T E S : 1. R E M O V E A L L B U R R S A N D S H A R P E D G E S QTY. TITLE: MANIPULATOR ELBOW PIVOT SHAFT, I OP DRAWN BY: D. LUK V. CHANG ITEM SPEC. 01/2x1-1/8 AL.RDD DESCRIPTION COMPONENTS TOLERANCES .XX i.Ol .XXX \u00C2\u00B1.001 .XXXX \u00C2\u00B1.0001 ANGULAR \u00C2\u00B11/2 CHECKED BY: |_. CHENG DATE: JAN 1L 95 DIM: INCH SCALE: NTS DWG NO: MANI010 PAGE: 1 OE 1 3 / 8 - 1 6 U N C 0 0 , 3 9 3 9 0 0 , 3 9 3 6 0 , 3 6 9 7 REV. DESCRIPTION APP, BY HATE - 0 0 . 5 0 0 r - 0 . 3 0 0 j - 0 . 8 8 0 3 T 1 . 6 2 5 N O T E S : ' l . R E M O V E A L L B U R R ! A N D S H A R P E D G E S 0 0 , 3 7 5 1 -1 01/2x1-5/8 AL. ROD --2 -1 ITEM SPEC. DESCRIPTION QTY. COMPONENTS TITLE: MANIPULATOR ELBOW PIVOT SHAEL BOTTOM TOLERANCES .XX +.01 .XXX \u00C2\u00B1.001 .XXXX \u00C2\u00B1.0001 ANGULAR +1/2 CHECKED BY: |_. CHENG SCALE: NTS DATE: JAN 4, 95 DWG NO: MANI011 DRAWN BY: D. LUK V. CHANG DIM: INCH PAGE: 1 OF 1 00 .3125 3 H O L E S 0.125 0 .625 - H 1.375 \u00E2\u0080\u0094 0.125 0.4913 T T 1 1.375 L 00 .3125 2 H O L E S -N O T E S : 1. R E M O V E A L L B U R R S AND S H A R P C O R N E R S 1 1-3/8 EQ LEG 1/8TH 3 -1 /2 AL. - a -1 ITEM SPEC. DESCRIPTION QTY. COMPONENTS M I L E ' MANIPULATOR ELBOW, JOINT ANGLE LEG TDLERANCES .IX \u00C2\u00B1.oi .XXX I.OO) CHECKED BY' L. CHENG DATE' JAN II. 95 SCALE' NTS II Vf, Nl], MAN I (11 a ,1 0 .375 J 0.1875 J 0 .1875-T Y P . 0 . 5 6 2 5 -T Y P . 0 . 9 3 7 5 -T Y P . \u00E2\u0080\u00A22 .750 -- 2 . 3 7 5 \" X 7 Z . 1 1 \u00E2\u0080\u0094 6 - 3 2 U N C x 3 / 8 D E E P 2 P L A C E S 00 .1875 2 H O L E S 00 .1875 2 H O L E S N O T E S : 1. R E M O V E A L L B U R R S AND S H A R P C O R N E R S 6 - 3 2 UNC x 1 /2 D E E P 2 P L A C E S 0 .375 a 3 6 - 3 8 UNC 3 / 4 L G SDCKE1 SCREW 1 2 4 - 3 / 4 x 1 - 1 / 2 x 3 / 8 AL. PLATE 1 1 5 - 1 / 2 x 1 - 1 / 2 x 3 / 8 AL. P L A I E -1 ITEM SPEC. DESCRIPTION QTY. COMPONENTS TITLE- MANIPULATOR ELBOW, SLEW ARM SUPPORT TOLERANCES .XX X.OI .XXX tout CHECKED BY' L. CHENG SCALE. NTS DATE, JAN 31, 95 DWG Nil. MAN 101.4 REV. DESCRIPTION APP. BY DATE 4 - 4 0 U N C T H R U 1 0 0 . 3 7 5 0 , 5 0 0 NDTES: 1. REMEJVE ALL BURR: AND SHARP EDGES 1 03/8x1/2 LG. AL. REJD -1 QTY, ITEM SPEC. DESCRIPTION DASH NO COMPONENTS TITl_E: MANIPULATOR ELBOW, MOTOR SLEEVE TOLERANCES .XX \u00C2\u00B1.01 .XXX \u00C2\u00B1.001 .XXXX \u00C2\u00B1.0001 ANGULAR \u00C2\u00B11/2 CHECKED BY: |_. CHENG SCALE: NTS DATE: JAN 25,95 DWG NO: MANI015 DRAWN BY: D. LUK V, CHANG DIM: INCH PAGE: 1 DE 1 0 . 3 2 1 3 REV. DESCRIPTION APP. BY DAI 6 - 3 2 U N C 4 H G I L E S E Q . S P A C E O N 0 1 . 3 5 0 B . C . \u00E2\u0080\u0094 0 1 . H o i n i . l b : 1111 , i t-ii-0 1 . 0 2 3 9 0 1 , 0 2 3 4 j - 0 . 2 2 1 3 NOTES: 1. REMOVE ALL BURRS AND SHARP EDGES - 0 0 , 9 4 5 1 -1 01-3/4x5/16LG AL. ROD _ o [_ -1 ITEM SPEC. DESCRIPTION Q TY. COMPONENTS TITLE: MANIPULATOR ELBOW BEARING MOUN'I TOLERANCES .XX i.01 .XXX .1.001 .XXXX i.0001 ANGULAR \u00C2\u00B11/2 CHECKED BY: L. CHENG SCALE: NTS DATE: JAN 11, 95 DWG NO: MANI016 DRAWN BY: 1). LUK V. CHANG DIM: INCH PAGE: 1 OE 1 REV. DESCRIPTION APP. BY DATE 0 . 1 6 2 5 0 0 . 3 9 4 N O T E S : 1. R E M O V E A L L B U R R S A N D S H A R P E D G E S 0 0 , 5 0 0 1 01/2x3/16 LG. AL. RING \u00E2\u0080\u00A2 -1 QTY, ITEM SPEC. DESCRIPTION DASH NO COMPONENTS TITLE: MANIPULATOR ELBOW, BEARING SHAFT RING DRAWN BY: D, LUK V, CHANG TOLERANCES .XX 1.01 .XXX +.001 .XXXX +.0001 ANGULAR +1/2 CHECKED BY: L. CHENG DATE: JAN 25,95 DIM: INCH SCALE: NTS DWG NO: MANI017 PAGE: 1 OF 1 APPENDIX III: KINETIC ENERGY Transformation Matrices From the mobile base to the inertia frame, H, 1 0 0 0 0 1 0 d0 0 0 1 0 0 0 0 1 (III.l) From revolute joint 1 to inertia frame, H 2 = cos9x - sin 0X 0 0 sin 0X cos6x 0 d0 0 0 1 0 0 0 0 1 (III.2) From prismatic joint 1 to inertia frame, H 3 = cos 9, - sin 9, 0 sin 9, cos 9, 0 0 0 0 -dx sin 9X 0 dx cos9x + d0 1 0 0 1 (III.3) From revolute joint 2 to inertia frame, cos 9X cos 92 - cos 9X sin 02 - sin 0X sin 92 - sin 9X cos 92 \u00E2\u0080\u00A2 cos 9X sin 92 cos 9X cos 92 \u00E2\u0080\u00A2sin9x cos92 0 0 -sin9x sin92 0 0 -dx sin 9X 0 dx cos9x + d0 1 0 0 1 (III.4) 200 From prismatic joint 2 to inertia frame, cos9x cos 62 -sin9x sin62 -cos6x sinQ2 -sin6x cos62 -d2 cos6x sin02 -d2 sin6x cos62 -dx sin 0X H e = cos9x sin62 + sin9x cos92 cos 9X cos 92 - sin 9X sin 92 -d2 sin 9X sin 92 +d2 cos9x cos92 +dx cos 9X + dQ (III.5) 0 0 0 0 1 0 0 1 Mass Matrix 7=1 +^[c4[c\u00C2\u00BB}+IAc4lcA (HI.6) J C ; ( / j = inertial transformation matrix of the body frame F^ [ c ja j = position vector to the mass element dtrij with respect to F ;^ Point mass at the mobile base, M , = mh 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 (III.7) nib = 5 kg, mass of the base. 201 Slewing link of module 1, M 2 = pUs 3 ' ? , [ c i i q ] [C l s q] + -/?, [C l s q] [C l s a] + ^ n [ C 1 S a ] r [ c i s q ] + / n [C l s a ] 7 '[C l s a ] f ( / ^ - / A ) [ c l s J r [ c l s q ] + l ( / f 2 - / ^ ) [ c l s J 7 ' [ c l s a ] + \{ln - /n ) [C l s a r[C l s q ] + (lX2 - / n )[C l s a ] r [C l s a ] (III.8) [C*] = \"0 - cos 0X \"0 0\" 0 -sin6x i [Clsa] = 1 0 _0 0 0 0 /,, = 0.4 m, uniform section of slewing link 1; / 1 2 = 0.1 m, linear bearing section of the slewing link 1; pUs= 1 kg/m, linear mass density of the uniform section of slewing link 1; pX2s = 2 kg/m, linear mass density of the linear bearing section of slewing link 2. Deployable link of module 1 M 3 = / ? , 3 J 14rf - ^ ) [ c l d q f [ c 1 - q ] + | ( / , 2 4 - / , 2 3 ) [ c l d q ] 7 ' [ c l d a ] - ^ ) [ C l d a ] r [ C l d q ] + ( / I 4 -/ 1 3)[C l d a] 7'[C l d a] --p\c f [ c l - - / 2 f c f r c i ^ 14[ l d q j [ l 2 1 , Slewing link of module 2, ^2.[c21,nc2lq]+i/221[c2iqf[c21i] ^2>[c2sa]7[c2sq] + /21[c2saf[c2sa] \ (i* -1\, )[c2sq f [c2sq ] + \ (il - l\x )[c2sq f[c2sa ] +1 (i 222 - i2n)[c2sa]T[c2s lr2 = 0.1 m, length of the of the rotor at revolute joint 2; pr2 = linear mass density of the rotor at revolute joint 2. Total mass matrix, M = M, + M 2 + M 3 + M 4 + M 5 + M 6 + M 7 + M 8 + M 9 . (III. 16) d\M\ Time Derivative of the Mass Matrix, M = \u00E2\u0080\u0094 1\u00E2\u0080\u0094 1 . (III. 17) d t Partial Derivatives of the Mass Matrix with respect to the Generalized Coordinates, 4 M ] q l 3d (III. 19) (111.20) 207 M \u00E2\u0080\u009E = 4 ^ , (III.2,) M q J = 4 - J , (.11.22) M , 4 = 4 J . (I.I.23) M a 5 = \u00E2\u0080\u0094 L - 1 , (111.24) M q 6 = -Ljl , (111.25) M q 7 = \u00E2\u0080\u0094j1 , (IH.26) M q 8 = ^ J . (HI.27) 208 APPENDIX IV: POTENTIAL ENERGY Point mass at the mobile base, PEt = -mbgd{ o\u00C2\u00BB (IV.l) Slewing link of module 1, PEU = -g PnJn\ d0+^ucos9} (IV.2) Deployable link of module 1, Pl3d{lX4 -hi) d0 + 1 ^ 1 + ^ ( ' l 4 + / 1 3 ) ) C O s d \ + P\AdlU (IV.3) Point mass at the elbow joint, PEe = -gme(d0 + dx cos 0X) ; (IV.4) Slewing link of module 2, PE2s=~g Pl\s ^21 d0 + dxcos0x + \u00E2\u0080\u0094 l2x cos[ox +02) + Pi2S(I22 ~l2x) do+dxcos0x + | ( / 2 2 + l2l)cos(0x + 02) + P23s{l23 - 122 ) d0 +d}COS0x + Ul23 + / 2 2 )cOj ( t9 , + 02) (IV.5) 209 Deployable link of module 2 PlM (^25 \u00E2\u0080\u0094 ^24 ) + Pud hs PE2d=~g d0 + dx cos 0X \d2+\{h5+h^]cos[e,+e2) d0 + dxcos0x + [d2 + ^hsj cos(6x + 02^j (IV.6) Flexible beam, PE^-gp d0 + dx lfi cosQx + d2 lfi COS{G , + 0 2 ) 1 ,2 ( - ~ n + -llJbcos\0x+02--(IV.7) Strain energy of flexible payload in the first mode, PEfis^^EPjbW2S2 , where: W = 2.7244 ; (IV.8) (IV.9) Strain energy of flexible joints, PES = ^ ^Kx {0, - a , j + K2 [0 2 - a 2 j2 J ; (IV. 1 0) Rotor contribution to potential energy PErX =-gprilrX [d0 + | / r l cos a PEr2 =-gpr2l r2(d0+dxcos 0X + ~lr2 cos(0, + a 2) 1 ' ( IV . l l ) (IV. 12) Total Potential Energy, U = PEh+ PEXs + PExd+PEe + PE2s + PE2d + PEfi + PEfis + PE\u00C2\u00A3 + PErX + PEr2 ; (IV. 13) 210 Partial Derivatives of the Potential Energy with respect to the Generalized Coordinates dU (IV. 14) dU (IV. 15) u q3 dU (IV. 16) U 0 4 = dU MD(8,5) = MD(8,6) = MD(8,7) = MD(8,8) = MD(1,7) MD(2,7) MD(3,7) MD(4,7) MD(5,7) MD(6,7) O.DO O.DO MD(1,8) MD(2,8) MD(3,8) MD(4,8) MD(5,8) MD(6,8) MD(7,8) O.DO DERIVATIVE OF THE MASS MATRIX WITH RESPECT TO DO ALL TERMS IN THE 5X5 MATRIX HAVE ZERO (0) VALUES. MD0(I,J) = 0.0 DERIVATIVE OF THE MASS MATRIX WITH RESPECT TO T l MT1(1,1) = 0.D0 MT1(I,2) = (l.D0/2.D0)\u00C2\u00BB( C0S(T1)\u00C2\u00BB( -PI I S ' L l 1\"2.D0 +PI2S*(L11**2.D0-L12**2.D0) +P13D*(L13-L14)*( L13+L14 +2.D0\u00C2\u00BBD1) +P14D'L14'(2.D0\u00C2\u00BBD1+L14) -2D0*ME*D1 ) -P2IS*L21*( L21*CCSS+2.DO*COS(T1)*D1 ) -P22S*(L22-L21)\u00C2\u00BB( (L21+L22)*CCSS +2.DO*COS(T1)*D1 ) -P23S*(L23-L22)*( (L23+L22)*CCSS +2.DO*COS(T1)*D1 ) +P23D*(L24-L25)*((L25+L24+2.D0\u00C2\u00ABD2)*CCSS +2.DO\u00C2\u00ABCOS(T1)*D1 ) +P24D*L25*( (2.DO\u00C2\u00BBD2+L25)*CCSS +2.DO*COS(T1)*D1 )) -(l.D0/2.D0)*PFB*LB\u00C2\u00BB( CSSC'LB + 2.DO*COS(T1)*D1 + 2D0*CCSS'(D2+W*BT)) -(l.D0/2.D0)*PR2*LR2*(-LR2*SIN(AL2)\u00C2\u00BBSIN(Tl) +LR2*COS(AL2)*COS(T2) +2.D0\u00C2\u00BBDl*COS(Tl)) MT1(1,3) = -SIN(T1)\u00C2\u00BB(-P13D ,(L13-L14)-PI4D*LI4+ME+P21S*L21 +P22S*(L22-L21)+P23S\u00C2\u00AB(L23-L22) -P23D*(L24-L25) -P24D*L25 ) -PFB*LB*SIN(T1) -PR2*LR2*SIN(T1) MT1(1,4) = (1.D0/2.D0)*CCSS*(-P21S*L21*\u00C2\u00AB2.D0 -P22S*(L22**2.DO-L21\u00C2\u00BB*2.D0) -P23S*(L23**2.D0-L22**2.D0) +P23D\u00C2\u00BB(L24-L25)*(L24+L25 +2.D0*D2) +P24D\u00C2\u00ABL25*( 2D0*D2 +L25)) -(I.D0/2.D0)\u00C2\u00ABPFB*LB*(CSSC\u00C2\u00ABLB + 2.D0'CCSS*(D2+W*BT)) SCCS = SIN(T1)*C0S(T2)-C0S(T1)*SIN(T2) MT1(1,5) =\u00E2\u0080\u00A2 CSSC\u00C2\u00AB( P23D'(L24-L25) +P24D*L25 ) -PFB'LB'SCCS MT1(1,6) = -PFB*LB\u00C2\u00BBSCCS*W MT1(3,3; MT1(3,4; MT1(3,5; MTI(3,6; MT1(3,7; MT 1(3,8; MT1(4,1 MT 1(4,2; MT 1(4,3 MT 1(4,4; MT 1(4,5; MT 1(4,6; MT1(4,7; MT1(4,8; MT1(5,1 MT1(5,2; MT 1(5,3; MT 1(5,4; MT1(5,5; MT1(5,6; MT1(5,7; MT1(5,8; MT 1(6,1 MT1(6,2; MT1(6,3; MT1(6,4 MTI(6,5; MT1(6,6; MT1(6,7; MT1(6,8 MT 1(7,1 MT 1(7,2; MT1(7,3 MT 1(7,4; MT1(7,5 MT 1(7,6; MT1(7,7; MT1(7,8; MT 1(8,1 MT1(8,2; MT1(8,3 MT1(8,4; MT1(8,5; MT1(8,6; MT1(8,7; MT1(8,8; \u00E2\u0080\u00A20.D0 \u00E2\u0080\u00A2MT1(3,2) : O.DO 'O.DO \u00E2\u0080\u00A2O.DO \u00E2\u0080\u00A2O.DO \u00E2\u0080\u00A2MT1(1,4) \u00E2\u0080\u00A2MT 1(2,4) \u00E2\u0080\u00A2MT1(3,4) \u00E2\u0080\u00A2O.DO \u00E2\u0080\u00A2 O.DO \u00E2\u0080\u00A2O.DO \u00E2\u0080\u00A2O.DO \u00E2\u0080\u00A2O.DO \u00E2\u0080\u00A2MTI(1,5) \u00E2\u0080\u00A2MT1(2,5) \u00E2\u0080\u00A2MT1(3,5) \u00E2\u0080\u00A2 MT1(4,5) \u00E2\u0080\u00A2O.DO = O.DO \u00E2\u0080\u00A2O.DO \u00E2\u0080\u00A2O.DO \u00E2\u0080\u00A2 MT1(1,6) = MT 1(2,6) \u00E2\u0080\u00A2 MT1(3,6) \u00E2\u0080\u00A2MT 1(4,6) \u00E2\u0080\u00A2MT1(5,6) \u00E2\u0080\u00A2O.DO \u00E2\u0080\u00A2O.DO \u00E2\u0080\u00A2O.DO \u00E2\u0080\u00A2MT1(1,7) \u00E2\u0080\u00A2MT1(2,7) \u00E2\u0080\u00A2MT1(3,7) \u00E2\u0080\u00A2MT1(4,7) = MT1(5,7) \u00E2\u0080\u00A2MT 1(6,7) \u00E2\u0080\u00A2O.DO = O.DO = MT1(1,8) = MT1(2,8) \u00E2\u0080\u00A2MT1(3,8) = MT1(4,8) \u00E2\u0080\u00A2MT1(5,8) \u00E2\u0080\u00A2 MT1(6,8) = MT1(7,8) \u00E2\u0080\u00A2O.DO DERIVATIVE OF THE MASS MATRIX WITH RESPECT TO Dl MT1(1,7) = 0.D0 MT1(1,8) = 0.D0 MD1(1,1) = 0.D0 MD1(1,2) = SIN(T1)*( P13D*(L13-L14) -ME -P21S*L21 -P22S\u00C2\u00BB(L22-L21) -P23S*(L23-L22) +P23D*(L24-L25) +P24D*L25 ) -PFB*LB*SIN(T1) -PR2*LR2*SIN(T1) MD1(1,3) = 0.D0 MD1(1,4)\u00C2\u00AB0.D0 MD1(1,5) = 0.D0 MD1(1,6) = 0.D0 MD1(I,7) = 0.D0 MD1(1,8) = 0.D0 MD1(2,1) = MD1(1,2) MD1(2,2) = -P14D*L14*(2.D0'D1+L14) +2.D0*ME*D1 -P13D*(L13-L14)*(L13+L14+2.D0*D1) +P21S*L21\u00C2\u00BB( L21*COS(T2)+2.DO\u00C2\u00ABD1 ) +P22S\u00C2\u00AB(L22-L21)*( (L22+L21)\u00C2\u00ABCOS(T2) + 2.D0*D1 ) +P23S*(L23-L22)*( (L23+L22)*C0S(T2) + 2D0*D1 ) -P23D*(L24-L25)'( (L24+L25+2.D0'D2)'COS(T2) + 2D0\u00C2\u00BBD1 ) -P24D*L25*( (L25+2.D0*D2)\u00C2\u00BBCOS(T2) + 2D0*D1 ) +PFB*LB*( LB\u00C2\u00BBSIN(T2) +2.D0*D1 +2.D0*COS(T2)*D2 +2.D0\u00C2\u00BBW*COS(T2)*BT) +2.D0*PR2\u00C2\u00BBLR2*D1 MD1(2,3) = O.DO MT1(2,1) = MT1(1,2) MT1(2,2) - O.DO MT1(2,3)-0.D0 MT1(2,4) = 0.D0 MT1(2,5) = 0.D0 MT1(2,6) = 0.D0 MT1(2,7) = 0.D0 MT1(2,8) = 0.D0 MT1(3,1) = MT1(I,3) MT1(3,2)-MT1(2,3) MD1(2,4) = (1 D0/2.D0)*COS(T2)\u00C2\u00BB( P21 S*L21 \u00C2\u00BB*2.D0 +P22S*(L22*\u00C2\u00BB2.D0-L21**2.D0) +P23S*(L23'*2.D0-L22\"2.D0) -P23D*(L24-L25)*(L24+L25+2.D0\"D2) -P24D*L25\u00C2\u00BB(2.D0*D2+L25)) +(I.D0/2.D0)*PFB*LB\u00C2\u00BB(LB\u00C2\u00BBSIN(T2)+2.D0*COS(T2)*D2 +2.D0*W'COS(T2)*BT) MD 1(2,5) = -SIN(T2)*( P24D*L25 +P23D*(L24-L25) ) +PFB*LB*SfN(T2) 217 MD 1(2,6) = PFB*LB*SIN(T2)\u00C2\u00BBW MD1(2,7) -O.DO MD1(2,8) = (l,D0/2.D0)'PR2*LR2**2.D0*COS(AL2) MD1(3,1) = MD1(1,3) MD 1(3,2) = MD1(2,3) MD1(3,3) -O.DO C MD 1(3,4) = MD 1(3,2) MD1(3,5) -O.DO MD1(3,6; -O.DO MD1(3,7; -O.DO MD1(3,8) -O.DO MD1(4,1) = MD1(1,4) MD1(4,2) = MD 1(2,4) MD1(4,3; = MD1(3,4) C MD1(4,4) -O.DO MD1(4,5; = O.DO MD1(4,6; -O.DO C MD1(4,7; -O.DO MD1(4,8; -O.DO C MD1(5,1) = MDI(1,5) MD1(5,2) = MD1(2,5) MD1(5,3) = MD1(3,5) C MD1(5,4) = MD1(4,5) MD1(5,5) -O.DO MD1(S,6) -O.DO MD1(5,7) -O.DO MD1(5,8) = 0.D0 C MD1(6,1) = MD1(1,6) MD 1(6,2; = MD 1(2,6) MD1(6,3) = MD1(3,6) C MD 1(6,4) - M D 1(4,6) MD1(6,5; -MDI(5,6) C MD 1(6,6; -0.D0 MD 1(6,7) -O.DO MD 1(6,8) -O.DO C MD1(7,1) = MD1(1,7) MD1(7,2) = MD1(2,7) MD1(7,3) = MD1(3,7) MD 1(7,4; = MD1(4,7) MD 1(7,5; - MD1(5,7) MD1(7,6 = MD1(6,7) MD1(7,7) = O.DO MD1(7,8; -O.DO MD1(8,1 - MD1(1,8) MD 1(8,2) - M D 1(2,8) MD1(8,3 - M D 1(3,8) MD1(8,4) = MD1(4,8) MD1(8,5) = MD1(5,8) MD1(8,6) -MD1(6,8) MD1(8,7) = MD1(7,8) MD1(8,8) -O.DO DERIVATIVE OF THE MASS MATRIX WITH RESPECT TO T2 MT2(1,I) = 0.D0 MT2( 1,2) = (1 D0/2.D0)*CCSS\u00C2\u00AB(-P21 S*L21 \"2.D0 -P22S*(L22\u00C2\u00BB*2.D0-L21**2.D0) -P23S*(L23\u00C2\u00BB*2.D0-L22**2.D0) +P23D*(L24-L25)*(L25+L24+2.D0\u00C2\u00BBD2) +P24D*L25\u00C2\u00BB( 2D0\u00C2\u00BBD2 +L25)) -(1 D0/2.D0)*PFB*LB*( CSSC'LB +2.D0\u00C2\u00BBCCSS\u00C2\u00BB(D2 +W\u00C2\u00BBBT)) MT2(1,3) = 0.D0 MT2(1,4) = MT2(I,2) MT2(1,5) = CSSC*( P23D\u00C2\u00BB(L24-L25) +P24D\u00C2\u00ABL25 ) -PFB*LB*CSSC MT2(1,6) = -PFB*LB\u00C2\u00BBW*CSSC MT2(1,7) = 0.D0 MT2(1,8) = O.DO MT2(2,1) = MT2(1,2) MT2(2,2) = D1*SIN(T2)*( -P21S*L21*,2.D0 -P22S*(L22**2.D0-L21 \u00E2\u0080\u00A2*2.D0) -P23S*(L23**2.D0-L22**2.D0) +P23D*(L24-L25)*(L24+L25+2.D0*D2) +P24D*L25*(2.D0*D2+L25)) +PFB*LB'( LB*Dl\u00C2\u00BBCOS(T2) -2D0*DI\u00C2\u00BBD2*SIN(T2) -2.D0*W\u00C2\u00BBBT*D1*SIN(T2)) MT2(2,3) - (l.D0/2.D0)*COS(T2)\u00C2\u00BB( -P2IS*L21**2.D0 -P22S'(L22\u00C2\u00BB'2.D0-L21**2.D0) -P23S*(L23\"2.D0-L22* ,2.D0) +P23D*(L24-L25)*( L24 +L25 +2.D0*D2 ) +P24D*L25*(2.DO\u00C2\u00BBD2+L25)) -(l.D0/2.D0)*PFB*LB*( LB*SIN(T2) +2.D0*W*BT*COS(T2) +2.D0*D2'COS(T2)) MT2(2,4) = (1.D0/2.D0)*D1*SIN(T2)*(-P21S*L21**2.D0 -P22S'(L22*',2.D0 -L21 \"2.D0) -P23S\u00C2\u00BB(L23**2.D0 -L22**2.D0) +P23D*(L24-L25)*(L24+L25+2.D0*D2) +P24D*L25'(2.D0*D2 +L25)) +(l.D0/2.D0)*PFB*LB*(LB*Dl*COS(T2) -2.D0*D1*D2*SIN(T2) -2D0*W*BT*D1*SIN(T2)) MT2(2,5) = -Dl \"COS(T2)'( P23D\u00C2\u00BB(L24-L25) + P24D*L25 ) +PFB*LB'Dl'COS(T2) MT2(2,6) - PFB*LB*D1 *W*COS(T2) MT2(2,7) - O.DO MT2(2,8) = O.DO MT2(3,1)-MT2(1,3) MT2(3,2) = MT2(2,3) MT2(3,3) - O.DO MT2(3,4) = MT2(3,2) MT2(3,5) = SIN(T2)*( P24D*L25 +P23D'(L24-L25)) -PFB*LB*SIN(T2) MT2(3,6) =* -PFB*LB'W*SIN(T2) MT2(3,7) = MT2(3,8) = MT2(4,1)\u00C2\u00BB MT2(4,2) = MT2(4,3) = MT2(4,4) > MT2(4,5) = MT2(4,6) = MT2(4,7) = MT2(4,8) = MT2(5,1) = MT2(5,2) \u00E2\u0080\u00A2 MT2(5,3) = MT2(5,4) = MT2(5,5) = MT2(5,6) = MT2(5,7) = MT2(5,8) = MT2(6,1) = MT2(6,2) = MT2(6,3) = MT2(6,4) = MT2(6,5) = MT2(6,6) = MT2(6,7) = MT2(6,8) = MT2(7,1) = MT2(7,2) = MT2(7,3) = MT2(7,4) = MT2(7,5) = MT2(7,6) = MT2(7,7) = MT2(7,8) = MT2(8,1) = MT2(8,2) = MT2(8,3) = MT2(8,4) = MT2(8,5) = MT2(8,6) = MT2(8,7) = MT2(8,8) = O.DO O.DO MT2(1,2) MT2(2,4) MT2(3,4) O.DO O.DO O.DO O.DO O.DO MT2(1,5) MT2(2,5) MT2(3,5) MT2(4,5) O.DO O.DO O.DO O.DO MT2(1,6) MT2(2,6) MT2(3,6) MT2(4,6) MT2(5,6) O.DO O.DO O.DO MT2(1,7) MT2(2,7) MT2(3,7) MT2(4,7) MT2(5,7) MT2(6,7) O.DO O.DO MT2(1,8) MT2(2,8) MT2(3,8) MT2(4,8) MT2(5,8) MT2(6,8) MT2(7,8) O.DO DERIVATIVE OF THE MASS MATRIX WITH RESPECT TO D2 MD2(1,1) = 0.D0 MD2(1,2) = CSSC'( P23D*(L24-L25) +P24D*L25 ) -PFB*LB*CSSC MD2(1,3) = 0.D0 218 MBT(l,5) = O.DO MD2(1,4) = MD2(1,2) MBT(1,6) = 0.D0 \fBT(l,7) = 0.D0 MD2(l,5) = O.DO MBT(l,8) = O.DO MD2(1,6) = 0.D0 MBT(2,l)\u00C2\u00BBNfBT(l,2) MD2(1,7) = 0.D0 C MD2(1,8) = 0.D0 Q MBT(2.2) = 2DO*PFB*LB*( W'D2 +W*\u00C2\u00AB2 MD2(2,1) = MD2(1,2) MBT(2,3) = -PFB\u00C2\u00BBLB*W*SIN(T2) MD2(2,2) = -P23D*(L24-L25)'( L24+L25 +2.D0*(D2+D1 *C0S(T2)) ) MBT(2,4) = PFB*LB'( 2.D0*W*D2 +2.D0* -P24D*L25\"( L25 +2.D0*(D2+DI*COS(T2))) c +2.D0*PFB*LB*( D2 +W*BT +D1'C0S(T2) ) MBT(2,5) = O.DO +PR2\u00C2\u00BBLR2* *2.D0*COS(AL2) MBT(2,6) = O.DO MBT(2,7) = O.DO MD2(2,3) = SIN(T2)\u00C2\u00BB( P23D*(L24-L25) +P24D*L25 ) MBT(2,8) = O.DO -PFB\u00C2\u00BBLB\u00C2\u00BBSIN(T2) MBT(3,2) = MBT(1,3) KfBT(3,2) = MBT(2,3) MD2(2,4) = -P23D*(L24-L25)*( L25 +L24 +2.D0*D2 +D1 *COS(T2) ) MBT(3,3) - O.DO -P24D*L25*( L25 +2.D0\u00C2\u00ABD2 +D1*C0S(T2)) MBT(3,4) - -PFB*LB*W*SIN(T2) +PFB\u00C2\u00BBLB*( 2D0'D2 +2.D0*W*BT +D1*C0S(T2)) MBT(3,5) = O.DO MBT(3,6) = O.DO MD2(2,S) = O.DO MBT(3,7) = O.DO MD2(2,6) = O.DO MBT(3,8) = O.DO MD2(2,7) = O.DO NffiT(4,l) = NfBT(l,4) MD2(2,8) = O.DO MBT(4,2) = MBT(2,4) MD2(3,1) = MD2(1,3) MBT(4,3) = MBT(3,4) MD2(3,2) = MD2(2,3) MBT(4,4) = 2D0*PFB*LB*( W\"2.DO*BT MD2(3,3) = O.DO MBT(4,5) - O.DO MD2(3,4) = MD2(3,2) MBT(4,6) - O.DO MD2(3,5) = O.DO MBT(4,7) - O.DO MD2(3,6)-0.D0 NfBT(4,8) = O.DO MD2(3,7) = 0.D0 MBT(5,1) = MBT(1,5) MD2(3,8) = 0.D0 MBT(5,2) = MBT(2,S) MD2(4,1) = MD2(1,4) NfBT(5,3) = MBT(3,5) MD2(4,2) = MD2(2,4) MBT(5,4) = MBT(4,5) MD2(4,3) = MD2(3,4) MBT(S,5) = O.DO MBT(5,6)\u00C2\u00BB O.DO MD2(4,4) - -P23D*(L24-L25)'( L24 +L25 +2.D0*D2 ) MBT(5,7) = O.DO -P24D*L25'( L25 +2.D0*D2) MBT(5,8) - O.DO +2.D0*PFB\u00C2\u00BBLB*(D2 + W*BT) MBT(6,1) = MBT(1,6) MBT(6,2) = MBT(2,6) MD2(4,5) = O.DO NfBT(6,3) = MBT(3,6) MD2(4,6) = O.DO MBT(6,4) = MBT(4,6) MD2(4,7) = O.DO MBT(6,5) = MBT(5,6) MD2(4,8) = O.DO MBT(6,6) = O.DO MD2(5,1) = MD2(1,5) MBT(6,7) = O.DO MD2(5,2) = MD2(2,5) MBT(6,8) = O.DO MD2(5,3) = MD2(3,5) MBT(7,1) = MBT(1,7) MD2(5,4) - MD2(4,5) MBT(7,2) - MBT(2,7) MD2(5,5) = O.DO MBT(7,3) = NfBT(3,7) MD2(5,6) = O.DO \fBT(7,4) = MBT(4,7) MD2(5,7) = O.DO \fBT(7,5) = NfBT(5,7) MD2(5,8) = O.DO MBT(7,6) = MBT(6,7) MD2(6,1) = MD2(1,6) MBT(7,7) = O.DO MD2(6,2) = MD2(2,6) MBT(7,8) = O.DO MD2(6,3) = MD2(3,6) MBT(8,1) = MBT(1,8) MD2(6,4) - MD2(4,6) MBT(8,2) = MBT(2,8) MD2(6,5) = MD2(5,6) NfBT(8,3) \u00C2\u00AB\u00E2\u0080\u00A2 MBT(3,8) MD2(6,6) = O.DO MBT(8,4) = MBT(4,8) MD2(6,7) = 0.D0 MBT(8,5) = MBT(5,8) MD2(6,8) = O.DO MBT(8,6) = MBT(6,8) MD2(7,1) = MD2(1,7) MBT(8,7) = MBT(7,8) MD2(7,2) - MD2(2,7) MBT(8,8) = O.DO MD2(7,3) = MD2(3,7) c MD2(7,4) = MD2(4,7) c MD2(7,5) = MD2(5,7) c MD2(7,6) = MD2(6,7) c DERIVATIVE OF THE MASS MATRIX W MD2(7,7) = O.DO c JOINTS ANGLE MD2(7,8) = O.DO c MD2(8,1) = MD2(1,8) c MD2(8,2) = MD2(2,8) M A L l ( l , l ) = O.DO MD2(8,3) = MD2(3,8) MALI(l,2)-O.DO MD2(8,4) - MD2(4,8) MAL1(1,3) = 0.D0 MD2(8,5) = MD2(5,8) MAL1(1,4) = 0.D0 MD2(8,6) = MD2(6,8) MALl(l ,5) = O.DO MD2(8,7) = MD2(7,8) MAL1(1,6) = 0.D0 MD2(8,8) = O.DO c c c c C DERIVATIVE OF THE MASS MATRIX W.R.T. BT (FLEX BEAM) C c MBT( 1,1) \"O.DO C MBT( 1,2) = -PFB\u00C2\u00BBLB\u00C2\u00BBW*CSSC C MBT(l,3) = O.DO MBT(I,4) = MBT(1,2) MAL1 (1,7) = -(1 D0/2.D0)*LR1 **2.D0*COS(AL 1 )*PR 1 MAL1(1,8) = MAL1(2,1)' MAL1(2,2)-MAL1(2,3) = MAL 1(2,4) = MAL1(2,5) = MAL1(2,6) = MAL1(2,7) = MAL 1(2,8) = MAL 1(3,2) = O.DO MAL1(1,2) O.DO O.DO O.DO O.DO O.DO O.DO O.DO MAL1(1,3) 219 MAL1(3,2) = MAL1(3,3) = MAL 1(3,4) = MALI(3,5) = MAL 1(3,6) = MAL 1(3,7) = MAL I (3,8) = MAL1(4,1) = MAL 1(4,2) = MAL1(4,3) = MAL 1(4,4) = MAL1(4,5) = MAL1(4,6) = MAL 1(4,7) = MAL 1(4,8) = MAL1(5,1) = MAL1(5,2) = MAL1(5,3)> MAL1(5,4) = MAL1(5,5) = MAL1(5,6) = MAL1(5,7) = MAL1(5,8) = MAL1(6,1) = MAL 1(6,2) = MAL1(6,3) = M A L 1(6,4) = MAL1(6,5) = MAL 1(6,6) = M A L 1(6,7) = MAL1(6,8) = MAL1(7,1) = M A L 1(7,2) = MAL1(7,3) = M A L 1(7,4) = MAL1(7,5)\u00C2\u00BB MAL 1(7,6) \u00E2\u0080\u00A2 MAL 1(7,7)-MAL 1(7,8) = MAL1(8,1) = MAL 1(8,2) = MAL1(8,3) = MAL 1(8,4) = MAL 1(8,5) = MAL 1(8,6) = MAL1(8,7) = MAL1(8,8) = MAL1(2,3) O.DO O.DO O.DO O.DO O.DO O.DO MAL1(1,4) M A L 1(2,4) MAL1(3,4) O.DO O.DO O.DO O.DO O.DO MAL1(1,5) MAL 1(2,5) MAL 1(3,5) MAL1(4,5) O.DO O.DO O.DO O.DO MAL1(1,6) M A L 1(2,6) M A L 1(3,6) MAL 1(4,6) MAL1(5,6) O.DO O.DO O.DO MAL1(I,7) M A L 1(2,7) MAL1(3,7) M A L I (4,7) MAL1(5,7) MAL1(6,7) O.DO O.DO MAL1(1,8) MAL 1(2,8) MAL 1(3,8) MAL1(4,8) MAL 1(5,8) MAL 1(6,8) MAL 1(7,8) O.DO MAL2(1,I) = 0.D0 MAL2(1,2)' MAL2(1,3) = MAL2(1,4). MAL2(1,5) = MAL2(I,6) = MAL2(1,7) = -(l.D0/2.D0)*PR2*LR2\u00C2\u00BB*2.D0*(COS(Tl) ,COS(AL2) -SIN(T1)*SIN(AL2)) O.DO O.DO O.DO O.DO O.DO MAL2(l,8) = -(l.D0/2.D0)*PR2*LR2**2.D0*(COS(Tl)*COS(AL2) -SIN(T1)*SIN(AL2)) MAL2(2,1) = MAL2(1,2) MAL2(2,2) = -PR2*LR2\"2.D0*D2*SIN(AL2) MAL2(2,3) = -(l.D0/2.D0)*PR2*LR2**2.D0*COS(AL2) MAL2(2,4) = MAL2(2,5) = MAL2(2,6) = MAL2(2,7) = O.DO O.DO O.DO O.DO MAL2(2,8) = -(l.D0/2.D0)*PR2\u00C2\u00BBLR2\u00C2\u00BB*2.D0*Dl*SIN(AL2) MAL2(3,2) = MAL2(3,2) = MAL2(3,3) = MAL2(3,4) = MAL2(3,5) = MAL2(3,6) = MAL2(3,7) = MAL2(1,3) MAL2(2,3) O.DO O.DO O.DO O.DO O.DO MAL2(3,8) = -(l.D0/2.D0)*PR2*LR2**2.D0*COS(AL2) MAL2(4,1) = MAL2(4,2) = MAL2(4,3) = MAL2(4,4) = MAL2(1,4) MAL2(2,4) MAL2(3,4) O.DO MAL2(4,5; MAL2(4,6; MAL2(4,7; MAL2(4,8; MAL2(5,1 MAL2(5,2; MAL2(5,3; MAL2(5,4; MAL2(5,5; MAL2(5,6; MAL2(5,7; MAL2(5,8; MAL2(6,1 MAL2(6,2; MAL2(6,3; MAL2(6, 4; MAL2(6,5; MAL2(6,6; MAL2(6,7; MAL2(6,8; MAL2(7,1 MAL2(7,2; MAL2(7,3; MAL2(7,4; MAL2(7,5; MAL2(7,6] MAL2(7,7; MAL2(7,8; MAL2(8,1 MAL2(8,2; MAL2(8,3 MAL2(8,4; MAL2(8,5; MAL2(8,6; MAL2(8,7; MAL2(8,8 = 0.DO = O.DO = 0.D0 \"O.DO = MAL2(I,5) = MAL2(2,5) = MAL2(3,5) = MAL2(4,5) = O.DO = O.DO -O.DO -O.DO = MAL2(1,6) = MAL2(2,6) -MAL2(3,6) = MAL2(4,6) = MAL2(5,6) -O.DO -O.DO -O.DO -MAL2(1,7) = MAL2(2,7) -MAL2(3,7) = MAL2(4,7) = MAL2(5,7) = MAL2(6,7) -O.DO -O.DO = MAL2(1,8) = MAL2(2,8) \u00C2\u00BB MAL2(3,8) = MAL2(4,8) = MAL2(5,8) = MAL2(6,8) = MAL2(7,8) -O.DO VECTOR dT/dqi DERIVATIVE OF THE KINETIC ENERGY W.R.T. THE GENERALIZED COORDINATES TRANSPOSING THE QD VECTOR DO 50 H= 1,8 QDT(1,H) - QD(H) CONTINUE dT/dDO K.ED0 = O.DO dT/dTl CALL MATMUL(KET11,QDT,MT1,1,8,8) CALL MATMUL(KET12.KET 11 ,QD, 1,8,1) KET1 = (l.D0/2.D0)*KET12 dT/dDl CALL MATMUL(KED 1 l.QDT.MDl,1,8,8) CALL MATMUL(KED 12.KED 11 ,QD, 1,8,1) KED1 -(l.D0/2.D0)*KED12 dT/dT2 CALL MATMUL(KET21 ,QDT,MT2,1,8,8) CALL MATMUL(KET22,KET21 ,QD, 1,8,1) KET2 = (1 .D0/2.D0)*KET22 dT/dD2 CALL MATMUL(KED21,QDT,MD2,1,8,8) CALL MATMUL(KED22,KED2 l,QO, 1,8,1) KED2 = (l.D0/2.D0)*KED22 dT/dBT CALL MATMUL(KEBT1,QDT,MBT, 1,8,8) CALL MATMUL(KEBT2,KEBT1 ,QD, 1,8,1) KEBT = (1 .D0/2.D0)*KEBT2 220 c c C dT/dALl C CALL MATMUL(KEAL 11 ,QDT,MAL 1,1,8,8) CALL MATMUL(KEAL 12.KEAL 11 ,QD, 1,8,1) KEALI = (1.D0/2.D0)*KEAL12 C C C C dT/dAL2 C CALL MATMUL(KEAL21 ,QDT,MAL2,1,8,8) CALL MATMLJL(KEAL22,KEAL21,QD,1,8,1) KEAL2 = (l.D0/2.D0)*KEAL22 C C C c C dT/qi C KEQ(1) = KED0 KEQ(2) = KET1 KEQ(3) = KED1 KEQ(4) = KET2 KEQ(5) = KED2 KEQ(6) = KEBT KEQ(7) = KEALI KEQ(8) = KEAL2 C C C C dU/dqi C DERIVATIVE OF THE POTENTIAL ENERGY W.R.T. THE C GENERALIZED COORDINATES C c PEQ(l) = - G*( PI 1S*L11 + P12S\u00C2\u00BB(L12-L11) + P13D*(L14-L13) - P14D*L14 + ME + P21S*L21 + P22S*(L22-L21) + P23S*(L23-L22) + P23D\u00C2\u00BB(L25-L24) \u00E2\u0080\u00A2 P24D*L25 + MB ) - G ' P F B ' L B -G*(PRI*LR1+PR2*LR2) C PEQ(2) = G*( SIN(Tl) *( (l.D0/2.D0)*( PI 1S*L11**2.D0 +P12S*(L!2**2.D0-L1I\u00C2\u00AB2.D0)) + P13D*(L14 -L13)*( D1+ (l.D0/2.D0)*(L13 +L14)) -P14D*L14*(D1 +(1.D0/2.D0)*L14) + ME*D1 ) + P21S*L21*(D1\u00C2\u00BBSIN(T1)+(1.D0/2.D0)\u00C2\u00BBCSSC*L21 ) + P22S*(L22-L21)\"( D1*STN(T1) +(1.D0/2.D0)\u00C2\u00BBCSSC*(L22+L21)) + P23S*(L23-L22)*(DI*SIN(T1) +(1 ,D0/2.D0)*CSSC*(L23+L22)) + P23D\u00C2\u00BB(L25-L24)*( D1*SIN(T1) +CSSC*( D2 +(1 D0/2.D0)*(L24+L25) ) ) -P24D*L25\u00C2\u00BB(D1\u00C2\u00BBSIN(T1) +CSSC*(D2+(1.D0/2.D0)*L25))) +G*PFB\u00C2\u00BBLB\u00C2\u00AB(D1*SIN(TI) +D2*SIN(T1+T2) +(1.D0/2.D0)*LB*SIN(T1+T2-PI/2.D0)) +K1*(T1-AL1) +G*PR2*LR2*(D1'SIN(T1)+(LD0/2.D0)*SIN(TH-AL1)*LR2) C PEQ(3) = - G*COS(Tl)*( P13D*(L14-L13) - P14D*L14 + ME + P21S*L21+P22S*(L22-L21) + P23S*(L23-L22) + P23D*(L25-L24) - P24D\u00C2\u00BBL25) -G*PFB*LB*COS(Tl) -G ,PR2*LR2*C0S(T1) C PEQ(4) = G*CSSC*( (l.D0/2.D0)*( P21S*L21 \"2.D0 +P22S*(L22**2.D0-L21**2.D0) +P23S*(L23\"2.D0-L22**2.D0)) + P23D*(L25-L24)\"( D2 +(1 ,D0/2.D0)\u00C2\u00BB(L24+L25)) - P24D*L25*( D2 +(1.D0/2.D0)*L25 )) +G*PFB\u00C2\u00ABLB\u00C2\u00AB( D2*SIN(T1+T2) +(1.D0/2.D0)*LB'SIN(T1+T2-PI/2.D0)) +K2*(T2 - AL2) C PEQ(S) = - G\u00C2\u00BBC0S(T1+T2)*( P23D*(L25-L24) + P24D*L25 ) - G*PFB*LB\u00C2\u00ABCOS(Tl+T2) C PEQ(6)=(l.D0/4.D0)*EY*PFB*W\u00C2\u00BB\u00C2\u00BB2.D0*BT C PEQ(7) = KI*(AL1 - Tl) + (l.DO/2.D0)*G*PRl \u00E2\u0080\u00A2LR1**2.D0*SIN(AL1) C PEQ(8) = K2*(AL2-T2) +(1.D0/2.D0)*G*PR2*LR2*\u00C2\u00AB2.D0*SIN(T1+AL2) C C C C EQUATIONS OF MOTION WITH THE DEPLOYABLE JOINTS FIXED C (LAGRANGE MULTIPLIER) C c C CONSTRAINTS ASSIGNMENT MATRIX C DO 60 II- 1,8 DO 70 JJ= 1,5 C(I,J) = O.DO 70 CONTINUE 60 CONTINUE C C(l , l)= 1.D0 C(3,2)= I.DO C(5,3)= 1.D0 C(7,4) - I DO C(8,5) = 1.D0 C C CONSTRAINT FUNCTIONS MATRIX C IF (TIME LE. 6.D0) THEN GG(1) = 0.D0 GG(2) = (-1.44D0\u00C2\u00BBPI/36.D0)\u00C2\u00ABSTN(2.D0'PI\u00C2\u00BBTIME/6.D0) GG(3) = (-1.46D0*PI/36,D0)*SIN(2.D0*PI*TIME/6.D0) GG(4) = (7.D0\u00C2\u00BBPI\u00C2\u00AB*2.D0/216.D0)*STN(2.D0*PI*TIME/6.D0) GG(5) = (-PI**2.D0/54.D0)*SIN(2.D0*PI*TIME/6.D0) ELSE IF ((TIME .GT. 6.D0) .AND. (TIME .LE. 13.D0)) THEN GG(l)-(-12.D0*PI/49.D0)'SIN(2.D0*PI*TIME/7.D0) GG(2) - O.DO GG(3) \u00E2\u0080\u00A2= O.DO GG(4) = O.DO GG(5) = O.DO ELSE IF ((TIME .GT. 13.D0) AND. (TIME LE. 19.D0)) THEN GG(1) = 0.D0 GG(2) = (1.44D0\u00C2\u00BBPI/36.D0)*SIN(2,D0*PI*TIME/6.D0) GG(3) = (1.46D0,PI/36.D0)*SrN(2.D0*PI*TIME/6.D0) GG(4) = (PI\u00C2\u00BB*2.D0/216.D0)\u00C2\u00BBSIN(2,D0\u00C2\u00ABPI*TIME/6.D0) GG(5) = (PI**2.D0/18.D0)*SIN(2.D0\u00C2\u00BBPI*TIME/6.D0) ELSE GG(1) = 0.D0 GG(2) =\u00E2\u0080\u00A2 O.DO GG(3) = O.DO GG(4) - O.DO GG(5) = O.DO END IF C C IDENTITY MATRIX C DO 80 111= 1,5 DO 81 JJJ= 1,5 IX(1II,JJJ) = O.DO 81 CONTINUE IX(III,III)= 1.D0 80 CONTINUE C C CC BLOCK C D0 82RR= 1,5 DO 83 PP= 1,8 CT(RR,PP) = C(PP,RR) 83 CONTINUE 82 CONTINUE C CALL MATMUL(CM,CT,MLNV,5,8,8) CALL MATMUL(CMC,CM,C,5,8,5) C CALLSLE(5,5,CMC,5,5,IX,CMCI,IPC,5,TEMPC,DET,JEXP) C CALL MATMUL(CC,C,CMCI,8,5,5) C C FUNCTION F C CALL MATMUL(MDQD,MD,QD,8,8,1) C DO 84 PPP= 1,8 F(PPP) = KEQ(PPP) - PEQ(PPP) - MDQD(PPP) 84 CONTINUE C C LAGRANGE MULTIPLIER - LAMBDA (FORCE REQUIREMENT) C CALL MATMUL(CMF,CM,F,5,8,1) C DO 85 0 0 = 1,5 GCMF(OO) = GG(OO) - CMF(OO) 85 CONTINUE C CALL MATMUL(LAMB,CMCI,GCMF,5,5,1) C 221 c C EQUATIONS OF MOTION IN MATRIX NOTATION C CALL MATMUL(CCG,CC,GG,8,5,1) CALL MATMUL(CCC,CC,CT,8,5,8) CALL MATMUL(CCCMI,CCC,MINV,8,8,8) C DO 86 R R R - 1,8 DO 87KKK = 1,8 ICCMI(RRR,KKK) = UNIT(RRR,KKK) - CCCMI(RRR,KKK) 87 CONTINUE 86 CONTINUE C CALL MATMUL(ICCMIF,ICCMI,F,8,8,1) C DO 88 V - 1,8 EQ(V) = ICCMIF(V) + CCG(V) 88 CONTINUE C CALL MATMUL(QDDT,MINV,EQ,8,8,1) C DO 89 V V = 1,8 YPRIME(VV+8) = QDDT(VV) 89 CONTINUE C C C RETURN END C Q *\u00C2\u00BB*\u00E2\u0080\u00A2\u00C2\u00BB\u00E2\u0080\u00A2*\u00C2\u00BB**\u00E2\u0080\u00A2******\u00C2\u00BB*\u00C2\u00BB****************\u00C2\u00BB\u00C2\u00BB***\u00C2\u00BB******\u00E2\u0080\u00A2*\u00E2\u0080\u00A2\u00C2\u00AB* C SUBROUTINE FCNI(N,T,Y,PD) INTEGER N REAL*8 Y(N),PD(N,N),T RETURN END C Q ***************************************************** c c c c Q ***************************************************** C SUBROUTINE MATRIX MULTIPLICATION A = B * C C B(L,M) AND C(M,N) ARE INPUT MATRICES C A(L,N) IS OUTPUT MATRIX Q ***************************************************** c SUBROUTINE MATMUL (A,B,C,L,M,N) INTEGER L,M,N REAL*8 A(L,N),B(L,M),C(M,N) C DIMENSION A(L,N),B(L,M),C(M,N) C DO10I=l,L DO 10 J=1,N A(I,J)=0.D0 DO 10K=1,M 10 A(I,J) = A(1,J) + B(1,K)*C(K,J) C RETURN END C Q ***************************************************** C C C C Q ***************************************************** C SUBROUTINE ENERGY C INPUT THE GENERALIZED COORDINATES VECTOR C RETURN THE KINETIC AND POTENTIAL ENERGY AT AN C INSTANT OF TIME Q ***************************************************** C SUBROUTINE ENERGY(PE,KE,TE,Y) C C REAL*8 QD(8),QDT(1,8),M(8,8),PE,KE,TE,Y(16),KEE1(1,8),KEE2,CSSC, CCSS,D0,T1,D1,T2,D2,D0D,T1D,D1D,T2D,D2D,BT,BTD,PI, AL1,AL2,AL1D,AL2D INTEGER SS C REAL*8L11,L12,L13,L14,L21,L22,L23,L24,L25,G,MB,ME, P11S,P12S,P13D,P14D,P21S,P22S,P23S,P23D,P24D, LB,PFB,K1,K2,W,EY, LR1,LR2,PR1,PR2 P ARAMETER(L 11 =0.4D0,L 12=0.1 D0,L 13=-0.4D0,L 14\u00E2\u0080\u00940.1 DO, L21=0.1DO,LR1=0.1 D0,LR2=0.1D0, L22=0.3D0,L23=0.1D0,L24=-0.4D0,L25=-0.1D0,G=0.D0, W=2.72444I,LB=1.D0,K1=25.D0,K2=25.D0,EY=I000.D0, MB=5.0D0,ME=0.0D0) PARAMETER(P11S=1.0DO,P12S=1.0DO,P13D=1.0DO,P14D=1.0DO, P21 S=1.0D0,P22S=1 0D0.P23S-1 0D0,P23D=I ,0D0, P24D=1.0D0,PFB=0.10D0,PRI=1.0D0,PR2=1.0D0) C PI = 3.141592653589793D0 C C D0 = Y(l) T l = Y(2) Dl = Y(3) T2 = Y(4) D2 = Y(5) BT = Y(6) AL 1 = Y(7) AL2 - Y(8) DOD = Y(9) T1D = Y(10) D I D - Y ( l l ) T2D = Y(12) D2D = Y(13) BTD = Y(14) AL1D = Y(15) AL2D = Y(16) C C C C POTENTIAL ENERGY C C PE = -G *( PI 1S*L11*( DO +(l.D0/2.D0)*COS(TI)*Ll 1 ) + MB'DO + P12S*(L12-L11)\u00C2\u00BB(DO+(1.DO/2.DO)'COS(TI)\u00C2\u00BB(L12+L11)) + P13D*(L14-L13)*(DO +(D1 +(l.D0/2.D0)*(LI3+L14))*COS(Tl)) -P14D\u00C2\u00ABL14*(D0+(D1 +(l.D0/2.D0)*L14)*COS(Tl)) + ME*(DO + D1*COS(T1)) + P21S*L21*(D0+Dl*COS(TI)+(I.D0/2.D0)\u00C2\u00ABCOS(TI+T2)*L21) + P22S*(L22-L21)'( DO + Dl'COS(TI) + (l.D0/2.D0)*COS(Tl+T2)*(L22+L21)) + P23S*(L23-L22)*( DO + Dl'COS(TI) + (1 D0/2.D0)\u00C2\u00BBCOS(Tl+T2)*(L23+L22)) + P23D\u00C2\u00BB(L25-L24)*( DO + Dl'COS(TI) + D2*C0S(T1+T2) + (l.D0/2.D0)*COS(Tl+T2)*(L24+L25)) - P24D*L25*( DO + Dl\u00C2\u00ABCOS(Tl) + D2*C0S(T1+T2) + (l.D0/2.D0)\"COS(Tl+T2)*L25 )) -G*PFB*LB\u00C2\u00BB(DO + D1*COS(T1) + D2*COS(T1 + T2) + (l.D0/2.D0)*LB*COS( Tl + T2 - PI/2.D0 ) ) +(1.D0/8.D0)*EY*PFB*W*\u00C2\u00BB2.D0*BT**2.D0 +(I.D0/2.D0)*( K1'(T1-AL1)**2.D0 + K2*(T2-AL2)*\u00C2\u00BB2.D0 ) -G*PR1*LR1*(DO + (1.DO/2.DO)\u00C2\u00BBCOS(AL1)*LR1 ) -G*PR2*LR2*( DO + Dl 'COS(Tl) + (1DO/2.DO)\u00C2\u00BBCOS(T1+AL2)\u00C2\u00ABLR2 ) C C c C KINETIC ENERGY C C C VELOCITY VECTOR OF THE GENERALIZED COORDINATES C DO 10 SS = 1,8 QD(SS) = Y(SS+8) QDT(1,SS) = Y(SS+8) 10 CONTINUE C C C MASS MATRIX C M(l , l ) = MB + PI 1S\u00C2\u00BBL11 - P12s*(Ll 1-L12) - P13D*(L13-L14) -P14D*L14 + ME + P21S*L21 + P22S\u00C2\u00BB(L22-L2I) + P23S'(L23-L22) - P23D*(L24-L25) - P24D*L25 + PFB'LB + PR1*LR1+PR2*LR2 C CSSC = COS(Tl)'SIN(T2) + SIN(Tl)\u00C2\u00ABCOS(T2) CCSS = C0S(T1)*C0S(T2)-SIN(T1)\u00C2\u00ABSIN(T2) C M(l,2) = (l.D0/2.D0)*( SIN(TI)*( -PI 1S*L11 **2.D0 + P12S*( LI 1**2.DO - LI2**2.DO) + P13D*(L13-L14)*(L13 + LI4 + 2.D0*D1 ) + P14D*L14*( 2D0*DI + L14 ) -2.D0*ME*D1) -P21S*L21*(L21*CSSC + 2.D0*SIN(T1)*D1) -P22S*(L22-L21)*((L22+L22)*CSSC + 2D0*SIN(T1)*D1) -P23S*(L23-L22)*((L23+L22)*CSSC + 2.D0*SIN(T1)*D1) +P23D*(L24-L25)*((L24+L25+2.D0*D2)*CSSC + 2D0*SIN(T1)*D1) +P24D*L25*((2.D0*D2 + L25)*CSSC + 2D0*SIN(T1)*D1) ) -(l.D0/2.D0)*PFB*LB*(-CCSS*LB + 2D0*SrN(Tl)*Dl +2.D0*CSSC*(D2 + W*BT)) -(l.D0/2.D0)*PR2*LR2*( LR2*C0S(T1)*SIN(AL2) 222 +LR2*SIN(T1)\u00C2\u00BBCOS(AL2)+2.DO\u00C2\u00BBSIN(T1)*D1 ) C M(l,3) \u00E2\u0080\u00A2= C0S(T1)*( - P13D*(L13 -L14) - P14D*L14 + ME + P21S*L21 + P22S*(L22-L21) + P23S*(L23-L22) - P23D*(L24-L25) - P24D*L25 ) +PFB*LB*C0S(T1) +PR2\"LR2*C0S(T1) C M( 1,4) = (1 ,D0/2.D0)*CSSC*( - P21S*L2l**2.D0 - P22S*(L22**2.D0-L21*,2.D0) -P23S*(L23**2.D0-L22**2.D0) + P23D*(L24-L2S)*(L24 + L25 + 2D0*D2 ) + P24D*L25*( 2D0'D2 + L25 )) -(l.D0/2.D0)*PFB*LB*( -CCSS'LB +2.D0*CSSC*(D2 + W*BT)) C M(l,5) = -CCSS'( P24D\u00C2\u00BBL25 + P23D*(L24-L25)) +PFB*LB*CCSS C M(I,6) = PFB 'LB 'CCSS 'W C M(1,7) = -(1.D0/2.D0)*LR1**2.D0*SIN(AL1)\u00C2\u00BBPRI C M(I,8) = -(l.D0/2.D0)*PR2*LR2**2.D0\u00C2\u00AB( C0S(T1)\u00C2\u00BBSIN(AL2) +SrN(Tl) ,C0S(AL2)) C M(2,1)-M(l,2) C M(2,2) = (l.D0/3.D0)*( PI 1S\u00C2\u00ABL11**3.D0 -P12S'(L11**3.D0 -L12**3.D0) -P13D*(L13**3.DO-L14**3.DO +3.D0*D1*(L13 -L14)*(D1+L13+L14)) + 3.D0*ME*D1\u00C2\u00BB*2.D0 - P14D*L14*( L14**2.D0 + 3D0*D1*(D1+L14)) + P21S*L21*(L2I**2.DO + 3.DO*D1*(L21*COS(T2)+D1)) + P22S*(L22*'3.D0-L21**3.D0 +3.DO*D1*(L22-L21)*(D1+COS(T2)*(L21+L22))) + P23S*(L23**3.DO - L22*'3.D0 +3.DO*D1*(L23-L22)*(D1+COS(T2)*(L23+L22))) - P23D'( L24**3.D0 - L23**3.D0 +3.D0*(L24 -L25)*( ( C0S(T2)*D1 +D2 )*(L24+L25) + Dl \"2.D0 + D2**2.D0 + 2D0*D1 *D2*COS(T2))) - P24D*L25*( L25\"2.D0 + 3.D0*( L25*(C0S(T2)*D1+D2) + D1**2.D0 + D2**2.D0 + 2.DO*D1*D2*COS(T2)))) +(l.DO/3.DO)*PFB*LB\u00C2\u00AB(LB**2.DO + 3.DO*LB*SIN(T2)*Dl +3.DO*D1**2.DO+3.DO*D2**2.DO +6.D0*W*BT*D2 +3.D0*W*'2.D0*BT*'2.D0 +6.DO*COS(T2)*D1*D2+6.DO*COS(T2)*D1*W*BT) +(1.D0/3.D0)*PR2*LR2*( LR2\u00C2\u00AB*2.D0 +3.D0*LR2*D2*COS(AL2) +3.D0*D1\u00C2\u00BB*2.D0) C M(2,3) = (I.D0/2.D0)*SIN(T2) *( -P21 S*L21 **2,D0 - P22S*(L22\u00C2\u00BB*2.D0 -L21 **2.D0) - P23S*(L23**2.D0-L22**2.D0) + P23D*(L24-L25)\u00C2\u00BB(L24+L25 +2.D0*D2 ) + P24D*L25*(2.D0\u00C2\u00BBD2 +L25)) -(l.D0/2.D0)*PFB*LB\u00C2\u00BB( -LB*C0S(T2) +2.DO*SIN(T2)*W*BT +2.D0\u00C2\u00BBSIN(T2)*D2) -(l.D0/2.D0)*PR2*LR2**2*SrN(AL2) C M(2,4)\u00C2\u00BB(1.D0/6.D0)*(P21S*L21*\"2.D0'( 2D0'L21 +3.DO*D1*COS(T2)) +P22S*( 2.D0*(L22**3.D0 -L21**3.D0) +3 DO*D 1 *COS(T2)*(L22\u00C2\u00BB*2.D0 -L21 \u00E2\u0080\u00A2*2.D0)) +P23S*( 2DO\u00C2\u00BB(L23'\u00C2\u00AB3.D0 -L22**3.D0) +3.D0*DI\u00C2\u00BBCOS(T2)*(L23**2.D0 -L22\"2.D0)) -P23D\u00C2\u00BB( 2.D0*(L24*\u00C2\u00BB3.D0 -L25**3.D0) +3.D0\u00C2\u00BB(L24 -L2S)*( (L24+L25)*(2.D0*D2 +C0S(T2)*D1 ) +2.D0*( D2**2.D0 + Dl , D2 , COS(T2)))) -P24D*L25*( 2D0*L2S**2.D0 +3.DO*D1'COS(T2)*( L2S+2.D0*D2) +6.D0\u00C2\u00BBD2*( L25+D2))) +(I.D0/6.D0)\u00C2\u00BBPFB*LB*( 2D0*LB\u00C2\u00AB*2.D0+3.D0*LB*SrN(T2)*Dl +6.D0*D2\u00C2\u00BB*2.D0+12.D0'D2\"W\u00C2\u00BBBT +6.D0*W*'2.D0*BT*'2.D0 +6.DO\u00C2\u00ABCOS(T2)*D1*W\u00C2\u00BBBT +6.DO*COS(T2)*D1*D2) C M(2,5) = -D1*SIN(T2)*( P24D*L25 + P23D\u00C2\u00BB(L24-L25) ) +(1.D0/2.D0)\u00C2\u00BBPFB*LB,( LB + 2.D0*SIN(T2)*D1 ) C M(2,6) = (l.D0/2.D0)*PFB*LB\u00C2\u00BBW\u00C2\u00AB( LB + 2.D0*SIN(T2)\u00C2\u00BBD1 ) C M(2,7) = 0.D0 C M(2,8) = (l.D0/6.D0)*PR2*LR2**2.D0*( 2.D0*LR2 +3.DO*D1*COS(AL2)) C M(3,1) = M(1,3) M(3,2) = M(2,3) C M(3,3) = - P13D*(L13 -L14) - PI4D*L14 + ME + P21S*L21 + P22S*(L22 -L21) + P23S*(L23 -L22) - P23D*(L24 -L25) - P24D*L25 +PFB*LB +PR2*LR2 C M(3,4)\u00C2\u00BBM(3,2) C M(3,5) \u00C2\u00BB-C0S(T2)*( P24D'L25 + P23D*(L24-L2S) ) +PFB\u00C2\u00ABLB*C0S(T2) c M(3,6) = PFB*LB*C0S(T2)*W c M(3,7) = O.DO c M(3,8) = -(l.D0/2.D0)'PR2*LR2**2.D0*SIN(AL2) c M(4,l) = M(l,4) M(4,2) = M(2,4) M(4,3) = M(3,4) c M(4,4) = (l.D0/3.D0)*(P21S*L21\"3.D0 + P22S*(L22\"3.D0 -L21\"3.D0) + P23S*(L23**3.D0-L22\"3.D0) - P23D\u00C2\u00AB(L24**3.D0-L25**3.DO +3.D0,D2'(L24-L25)*(D2+L25+L24)) - P24D\u00C2\u00BBL25'(L25\"2.D0 +3.D0*D2*L2S +3.DO*D2**2.DO)) +(l.DO/3.D0)*PFB\u00C2\u00BBLB*(LB**2.DO+3.D0*D2**2.D0 . +3.D0*W**2.D0*BT**2.D0 +6.D0*W*BT*D2) c M(4,5) = (l.DO/2.D0),PFB\u00C2\u00ABLB**2.D0 c M(4,6) - (l.D0/2.D0)\u00C2\u00BBPFB\u00C2\u00BBW\u00C2\u00ABLB**2.D0 c M(4,7) = 0.D0 M(4,8) = 0.D0 c M(5,l) \u00C2\u00BBM(1,5) M(5,2) = M(2,5) M(5,3) - M(3,5) M(5.4) - M(4,5) c M(5,5) = -P23D*(L24 -L25) - P24D*L25 + PFB*LB c M(5,6) = PFB\u00C2\u00BBLB*W c M(5,7) = O.DO M(5,8) = O.DO c M(6,l) = M(1,6) M(6,2) = M(2,6) M(6,3; = M(3,6) M(6,4; = M(4,6) M(6,5) = M(5,6) c M(6,6) = PFB*LB*W**2.D0 c M(6,7) = 0.D0 M(6,8; = 0.D0 c M(7,l) \u00C2\u00BBM(1,7) M(7,2) = M(2,7) M(7,3) = M(3,7) M(7,4) = M(4,7) M(7,5) = M(5,7) M(7,6) = M(6,7) c M(7,7) = (1.D0/3.D0)*LR1*\u00C2\u00BB3*PR1 c M(7,8) = 0.D0 c M(8,l) = M(1,8) M(8,2) - M(2,8) M(8,3) = M(3,8) M(8,4) = M(4,8) M(8,5) - M(5,8) M(8,6) \u00C2\u00BB M(6,8) M(8,7) = M(7,8) c M(8,8) = (l.D0/3.D0)*PR2'LR2*\u00C2\u00BB3 c c c c CALL MATMUL(KEE1,QDT,M, 1,8,8) CALL MATMUL(KEE2,KEE1 ,QD, 1,8,1) C KE-(l.DO/2.DO)'KEE2 C C TOTAL ENERGY 223 c TE = PE + KE C RETURN END C C C C C C C SUBROUTINE TRAJECTORY C \u00E2\u0080\u0094 c SUBROUTINE TRAJECTORY(N,Y,BX,BY,JIX,J 1 Y,J2X,J2Y,BTX,BTY, BXS.B YS,J 1 XS,J 1 YS.J2XS, J2YS,BTXS,BTYS) C INTEGER N C REAL*8 Y(N),BX,BY,JIX,I1Y,J2X,J2Y,BTX,BTY,BXS,BYS,JIXS,J1YS, J2XS,J2YS,BTXS,BTYS,PI C PI = 3.I4I592653589793D0 C C C GENERALIZED POSITIONS C BX = Y(I) BY = O.DO C J1X = Y( 1) + Y(3)*COS(Y(2)) J1Y-Y(3)*SIN(Y(2)) J2X = J1X + Y(5)*COS(Y(2)+Y(4)) J2 Y = J1Y + Y(5)*SIN(Y(2)+Y(4)) BTX = J2X + 1 D0\u00C2\u00BBCOS( Y(2)+Y(4)-PI/2.D0 ) BTY = J2Y + 1 D0*SIN( Y(2)+Y(4)-PI/2.D0 ) C C SPECIFIED POSITIONS C BXS = Y(l) BYS = O.DO C JIXS = Y(l) + Y(3)*COS(Y(7)) ' J1YS = Y(3)*SIN(Y(7)) J2XS - J1X + Y(5)*COS(Y(7)+Y(8)) J2YS = JIY + Y(5)\u00C2\u00ABSIN(Y(7)+Y(8)) BTXS = J2X + 1 DO\u00C2\u00BBCOS( Y(7)+Y(8)-P1/2.D0 ) BTYS = J2Y + I.DO*SIN( Y(7)+Y(8)-PI/2.D0 ) C RETURN END C C C c C SUBROUTINE CAPTURE C c SUBROUTINE CAPTURE(BX,B Y,J 1 X,J 1 Y.J2X, J2Y,BTX,BTY,XAXIS, YAXIS) C REAL*8 BX,BY,J1X,J1 Y,I2X,J2Y,BTX,BTY,XAXIS(4),YAXIS(4) C X A X I S ( I ) - B X . XAXIS(2)-J1X XAXIS(3) = J2X XAXIS(4) = BTX C YAXIS(I) = BY YAXIS(2) = J1Y YAXIS(3) = J2Y YAXIS(4) = BTY C RETURN END C C 224 (V.2) C++ Operation Control Program for the Prototype Manipulator. 225 1***********************************************************************1 I* CONTROL PROGRAM FOR THE PROTOTYPE ROBOTIC */ /* MANIPULATOR */ /* */ /* MarkChu 1997 */ j* **********************************************************************f #include #include #include #include #include #define CMM_ADD 1000 /* READ AND WRITE ADDRESS OF THE CONTROLLER #define CONTROL 1001 /* STATUS CONTROL ADDRESS */ #define CHK.B1T5 32 /* BITMASK 100000 */ #define CHKBIT4 16 /* BITMASK 10000 */ #define PI 3.14159265359 void init(); void manual(); void automatic(); void question(); void tracking(); void polar(); /* subroutine functions for sending */ int send_eommand(); /* commands to the controller */ int pos_command(); int move(); long position(); int clear(); unsigned char *cmnd; /* global variables */ unsigned char command[20]; long dep_count,slew_count; int flag; double x_coord, x_pos, y_coord, y_pos; double x_current,y_current, theta_current,radius_current; const double count_rate=6.2; /* Pitch of the lead screw */ /* 6.2 mm/rev */ void main() { int sigue,manaut; flag=l; x_current=0; y_current=0; theta_current=0; radius_current=370; init(); /* Initialize all parameter and establish communication with the controller */ while(flag=l) { printf(\"Initialization complete! Flag is: %d\n\", flag); printf(\"\n\nTracking=0 , Manual=l , Automatic=2 , Exit=3\n\"); scanf(\"%d\", &manaut); if(manaut=0) tracking(); else if(manaut=l) manual(); else if(manaut==2) automatic(); else if(manaut==3) { flag=clear(); 226 printf(\"Instructions array cleared!\n\"); scanf(\"%d\", &sigue); printf(\"flag is: %d\n\", flag); exit(O); > flag=clear(); /* INITIALIZATION SUBROUTINE */ void init() { int sigue; outp(CONTROL, 0x05); outp(CONTROL, OxFO); printf(\"\n\n\"); pri ntf( \"****************** ******************************************* printf(\"* *\n\"); printf(\"* This program reads the absolute target position of *\n\"); printf(\"* the end effector of the Variable Geometry Manipulator and *\n\"); printf(\"* execute the positioning command using the last UNIT. *\n\"); printff* *\n\"); printf(\"* Mark Chu August 1996 *\n\"); printf(\"* *\n\"); p r; n(f^'**************************************************************\ n\u00C2\u00BB^ /* Initializing parameters: Position: absolute, format Velocity, Acceleration, Deceralation Torque and Error limits PID parameters */ flag=clear(); if(flag=l) { cmnd=\"DP 0,0,0,0;\"; flag=send_command(); cmnd=\"TL 3,3,3,3;\"; flag=send_command(); cmnd-'PF 7;\"; flag=send_command(); cmnd=\"SP 50000,5000,50000,5000;\"; flag=send_command(); cmnd=\"AC 50000,5000,50000,5000;\"; flag=send_command(); cmnd=\"DC 50000,5000,50000,5000;\"; flag=send_command0; cmnd=\"KP 9.36,14,9.36,11;\"; flag=send_command(); cmnd=\"KD 130,150,130,150;\"; flag=send_command(); cmnd=\"KI 2,2,2,2;\"; 227 flag=send_command(); cmnd=\"ER 1000,1000,1000,1000;\"; flag=send_command(); cmnd=\"OE 1,1,1,1;\"; flag=send_command(); > > /* SUBROUTINE CLEAR */ int clear() { unsigned char bit, error_string[100], error; char unused; int i; bit=0; i=0; flag=l; unused=0; bit=inp(CONTROL); while (((bit & CHKBIT5)=0) && (flag=l)) { unused=inp(CMM_ADD); if (unused=63) < cmnd-'TCl;\"; flag=send_commandO; bit=inp(CONTROL); while((bit & CHKBIT5)=0) { error=inp(CMM_ADD); if ((error != 58) && (error != 63)) { error_string[i]=error; i++; > bit=inp(CONTROL); > error_string[i]-\0'; printf(\"Error occurred ! Error Code = %s\n\n\", errorstring); printf(\"Program stopped due to the error !\n\"); flag=2; printf(\"unusederr= %c\n\",unused); } if (flag=2) break; bit=inp(CONTROL); } return (flag); > /* SUBROUTINE TO SEND COMMAND TO THE CONTROLLER PORT */ int send_command() { while(*cmnd) { outp(CMM_ADD, *cmnd); 228 cmnd++; > clear(); flag=l; return(flag); } /* Subroutine to request target position of the end effector */ void question() { if(flag=l) { fiag=clear(); do { printf(\"Enter the x,y coordinates of the target position (mm)\n\"); printf(\"Operation on the FIRST and SECOND quadrant ONLY\n\n\"); scanf(\"%lf%lf', &x_coord, &y_coord); printf(\"%lf %lf\n\", x_coord,y_coord); } while(y_coord < 0); } } /* Subroutine to determine the polar coordinates of the target position and determine the number of rotations for each axis */ void polar() { double gamma,radius,ratio,theta; double sleww.depll; printf(\"y_coord is: %lf\n\", y_coord); printf(\"x_coord is: %lf\n\", x_coord); ratio=x_coord/y coord; gamma=-atan(ratio); printf(\"ratio is: %lf\n\", ratio); printf(\"gamma is: %lf\n\", gamma); /* gamma=atan2(y_coord,x_coord); */ theta=gamma-theta_current; printf(\"theta_current is: %Iftn\", theta_current); printf(\"theta is: %lf\n\", theta); radius=sqrt( pow(x_coord,2) + pow(y_coord,2)); printf(\"radius is: %lf\n\", radius); dep_count=( ((radius-radius_current)/count_rate)* 10000); slew_count= theta*40000/PI; sleww=( ((radius-radius_current)/count_rate)* 10000); depll= theta*40000/PI; printf(\"sleww is: %lf\n\", sleww); printf(\"depll is: %lf\n\", depll); theta_current=gamma; x_current=x_coord; y_current=y coord; rad i us_current=rad i us; 229 printf(\"\ndep_count is: %ld\n\", dep_count); printf(\"slew_count is: %ld\n\", slew_count); printf(\"theta_current is: %lf\n\", theta_current); printf(\"x_current is: %lf\n\", x_current); printf(\"y_current is: %lf\n\", y_current); printf(\"radius_current is: %If\n\", radius_current); > /* Subroutine to move the end effector to the target position int move() { unsigned char *input_dep, *input_slew, *PR=\"PR \"; int sigue; ltoa(dep_count, inputdep, 10); ltoa(slew_count, input_slew, 10); strcpy(command, PR); strcat(command, input_dep); strcpy(input_dep, \"\"); strcat(command,\",\"); strcat(command, input_slew); strcpy(input_slew,\"\"); strcat(command,\";\"); printf(\"command line is: %s\n\", command); cmnd=command; printf(\"cmnd is: %s\n\", cmnd); flag=send_command(); scanf(\"%d\", &sigue); cmnd=\"SH;\"; flag=send_command(); cmnd=\"BG XY;\"; flag=send_command(); return (flag); > /* Subroutine for position tracking of the rotation axis */ void tracking() { unsigned char pos_string[50], position, bit; int i,sigue,again; again=l; while(again=l) { bit=0; i=0; flag=clear(); cmnd=\"TP;\"; flag=send_command(); printf(\"%c\", bit); bit=inp(CONTROL); printf(\"\Begining tracking... \n%c\n\", bit); /* printf(\"\nBegining tracking procedure\n\"); */ scanf(\"%d\", &sigue); while((bit & CHKBIT5)=0) { position=inp(CMM_ADD); if((position!=58)&&(position!=63)) { pos_string[i]=position; i++; } bit=inp(CONTROL); } pos_string[i]='\0'; printf(\"\nThe current position is: %s\n\", posstring); /* printf(\"%s\", pos_string); */ printf(\"\nExit tracking-0 , Tracking~l , Manual~2 , Exit program~9\n\"); scanf(\"%d\", &again); if(again==2) manual(); if(again==9) exit(O); } flag=clear(); } /* Subroutine for manual command input */ void manual() { char instruct[80], dummy; int again; again=l; while(again=l) { printf(\"Enter the instruction command line..An\"); scanf(\"%c\", &dummy); gets(instruct); printf(\"\nThe command line is: %s\n\", instruct); flag=clear(); cmnd=instruct; printf(\"cmnd = %s\n\", cmnd); flag=send_commandO; printf(\"Instruction command executed!\n\"); flag=clear(); printf(\"flag is: %d\n\", flag); printf(\"\nReturn--0 , Manual~l , Tracking~2\n\"); scanf(\"%d\", &again); if(again=2) tracking(); } > /* Subroutine Automatic */ void automaticfj { int again; again=0; while(again=0) { 231 question(); polar(); move(); printf(\"Next tareget=0 , Exit automatic=l\n\"); scanf(\"%d\", &again); 232 "@en . "Thesis/Dissertation"@en . "1997-11"@en . "10.14288/1.0080940"@en . "eng"@en . "Mechanical Engineering"@en . "Vancouver : University of British Columbia Library"@en . "University of British Columbia"@en . "For non-commercial purposes only, such as research, private study and education. Additional conditions apply, see Terms of Use https://open.library.ubc.ca/terms_of_use."@en . "Graduate"@en . "Design, construction and operation of a variable geometry manipulator"@en . "Text"@en . "http://hdl.handle.net/2429/6466"@en .