Calhoun: The NPS Institutional Archive Theses and Dissertations Thesis Collection 1992-09 Design and control of a space based two link manipulator with Lyapunov based control laws Sorensen, Dennis R. Monterey, California. Naval Postgraduate School http://hdl.handle.net/10945/23618 NAVAL POSTGRADUATE SCHOOL 3NIA 93943-5002 21 Unclassified Security Classification of this page REPORT DOCUMENTATION PAGE Report Security Classification la Unclassified 1 2a Security Classification Authority 3 2b Declassificatiun/Downgrading Schedule Performing Organization Report Number(s) 5 Name of Performing Organization Naval Postgraduate School 6c Address (city, state, Monterey, Name 8a Applicable) for public release; distribution is unlimited. Naval Postgraduate School 39 and ZIP code) 7 b Address CA 93943-5000 (city, state, Monterey, of Funding/Sponsoring Organization Markings Distribution Availability of Report Monitoring Organization Report Number(s) 7a Name of Monitoring Organization 6b Office Symbol (If Restrictive Approved 4 oa b 8b Office Symbol Applicable) and ZIP code) CA 93943-5000 Procurement Instrument Identification Number 9 (If 8c Address (city, state, and ZIP code) Source of Funding Numbers 1 Program Element Number | IV.jecl Nu DESIGN AND CONTROL OF A SPACE BASED Title (Include Security Classification) 1 [ Task Nu | Work l mi Accession : No TWO LINK MANIPULATOR WITH LYAPUNOV BASED CONTROL LAWS Personal Author(s) 1 Dennis R. Sorensen 13a Type of Report 13b Time Covered Engineer's Thesis From Supplementary Notation 16 Il4 Date of Report To | The views expressed (year, month,day) I September 1992 15 Page Count 1 in this thesis are those of the author and do not 28 reflect the official policy or position of the Department of Defense or the U.S. Government. Cosati Codes 18 Subject Terms (continue on reverse if necessary and identify by block number) Non-Linear Control System Design, Robotics, 1 Satellite Attitude Control if necessary and identify by block number Flexible Spacecraft Simulator (FSS) at the Naval Postgraduate School Abstract (continue on reverse 9 The flexible appendage with a two was modified by replacing the was designed to simulate motion main body, a two link manipulator, and link robotic manipulator. This experimental setup of a spacecraft about the pitch axis. The spacecraft consists of a momentum wheel with actuator to control the pitch attitude of the spacecraft. Position information from the main body and manipulators were obtained from Rotary Variable Displacement Transducers (RVDT's). The equations of motion were developed assuming that the main body and manipulator were rigid bodies. The resulting coupled, nonlinear, time variant equations of motion were used to analyze the dynamics and kinematics of the body, manipulator as well as the interaction between the two. Three different control strategies were developed using Lyapunov's Second or Direct Method. With the first controller, simple linear feedback of position and velocity information with constant gains was used to position the manipulator and stabilize the main body. A fifth order polynomial was used to generate a reference trajectory for the second controller. This trajectory was used in conjunction with a tracking Lyapunov controller to position and stabilize the system. In the third controller, a near-minimum-time technique was used to generate a reference trajectory. This reference trajectory was employed to design a tracking controller similar to that used in the polynomial reference controller. 20 Distribution/Availability of Abstract X| unclassified/unlimited same 2 DTIC as report I users 22a Name of Responsible Individual Brij N. Agrawal DD FORM 1473, 84 MAR 1 Abstract Security Classification Unclassified 22c Office Symbol 22b Telephone (Include Area code) (408) 646-3338 83 APR edition may be used until exhausted All other editions are obsolete | AA/Ag security classification of this page Unclassified TP^flRS^ Approved for public release; distribution is unlimited. DESIGN AND CONTROL OF A SPACE BASED TWO LINK MANIPULATOR WITH LYAPUNOV BASED CONTROL LAWS by Dennis R. Sorensen Lieutenant Commander, United States Navy B.S., Iowa State University, 1981 M.S. Astronautical Engineering, Naval Postgraduate School, 1991 Submitted in partial fulfillment of the requirements for the degree of ENGINEER OF AERONAUTICS AND ASTRONAUTICS from the NAVAL POSTGRADUATE SCHOOL September 1992 ABSTRACT The Flexible Spacecraft Simulator (FSS) was modified by replacing the flexible at the Naval Postgraduate School appendage with a two link robotic This experimental setup was designed to simulate motion of a manipulator. spacecraft about the pitch axis. link manipulator, and the spacecraft. The spacecraft momentum wheel consists of a main body, a two actuator to control the pitch attitude of Position information from the main body and manipulators was obtained from Rotary Variable Displacement Transducers (RVDT). The equations of motion were developed assuming that the main body and manipulator were rigid bodies. were used The resulting coupled, nonlinear, time invariant equations of to analyze the motion dynamics and kinematics of the main body and manipulator as well as the interaction between the main body and manipulator. Three different control strategies were developed using Lyapunov's Second or Direct Method. With the first controller, simple linear feedback of position and velocity information with constant gains and stabilize the main body. A fifth was used to position the manipulator order polynomial was used to generate a reference trajectory for the second controller. This trajectory was used in conjunction with a tracking controller to position and stabilize the system. In the third controller, a trajectory. near-minimum-time technique was used to generate a reference This reference trajectory was employed using a tracking controller similar to that used in the polynomial reference controller. 111 u* ACKNOWLEDGMENTS Many notable people have contributed among laboratory these people manager but the most to the success of this project was Mr. Rafford Bailey. Mr. Bailey was the for the Spacecraft Robotics Simulator and was primarily responsible for the design, manufacture, and assembly of the manipulator. Mr. Pat Hickey and Mr. John Moulton of the Department Of Aeronautics were instrumental in the manufacture of manipulator components in a timely and Air Force Captain Gary Yale and Dr. Hyochoong Bang expeditious manner. provided invaluable assistance with many of the theoretical and analytical aspects for the design and analysis of the control system. Junkins, the George visiting J. Eppright Chair Professor at Texas A& Professor John L. M University and Co-Chair of the Space Systems Academic Group at the Naval Postgraduate School presented a firm foundation for the non-linear control theory and Lyapunov stability theory that formed the theoretical basis for the control law design. Finally, to Professor Brij N. Agrawal, who provided the funding, spacecraft attitude, dynamics and control expertise, and was responsible for the overall coordination and success of IV this project. 93943. 5002 ; TABLE OF CONTENTS I. INTRODUCTION H. EXPERIMENTAL SETUP 4 A. SPACE ROBOTICS SIMULATOR DESCRIPTION 4 B. EXPERIMENT DESCRIPTION 4 C. HI. 1 Main Body 1. Spacecraft 2. Two 3. Granite Table 10 4. AC - 10 5. VAX 3100 Series Computer Link Manipulator 100 SYSTEM INTEGRATION 5 7 11 11 ANALYTICAL MODEL 14 A COORDINATE SYSTEMS 14 B. EQUATIONS OF MOTION 17 C. D. 1. Lagrange's Equations 2. Kinetic Energy 3. Equation 4. Applied Torques Of Motion Of Motion LYAPUNOV STABLE CONTROLLER DESIGN 17 17 18 19 20 1. Lyapunov Stable Controller With Linear Feedback 21 2. Lyapunov Stable Tracking Controller 24 REFERENCE TRAJECTORY GENERATION 25 1. Polynomial Reference Trajectory 25 2. Minimum-Time-Maneuver 29 3. E. IV. Near-Minimum-Time Rigid Body Maneuver 30 ANALYTICAL MODEL SUMMARY 34 RESULTS AND DISCUSSION 36 A. REFERENCE MANEUVER 36 B. LINEAR CONSTANT GAIN FEEDBACK CONTROLLER 37 C. POLYNOMIAL REFERENCE TRACKING CONTROLLER 41 D. NEAR-MINIMUM-TIME REFERENCE TRACKING CONTROLLER V. 45 CONCLUSIONS A. 52 SUBJECTS FOR FUTURE RESEARCH 54 APPENDIX A 56 APPENDIX B A. B. C. , SYSTEM KINETIC ENERGY 1. Body 2. Body 2 3. Body 4. Total Kinetic Energy 57 57 Kinetic Energy 57 Kinetic Energy 57 3 Kinetic Energy 57 1 57 EQUATIONS OF MOTION 1. Mass Matrix 2. Coriolis And 57 57 Centrifugal Acceleration Terms: POLYNOMIAL REFERENCE TRAJECTORY 58 59 1. Vector Polynomial Describing Tip Position: 59 2. Boundary Conditions And Polynomials 59 3. Vector Position 59 4. Vector Velocity 59 VI 5. Vector Acceleration 60 D. TWO LINK INVERSE KINEMATICS 60 E. NEAR MINIMUM TIME RIGID BODY MANEUVER 61 1. Near-Minimum-Time Maneuver 61 2. Torque Shaping Function 61 3. Boundary Conditions 62 4. Reference Angular Displacements 5. Near-Minimum-Time Relationships And Velocities APPENDIX C A. B. C. 1. Main Program For Linear Feedback Simulation 2. Linear Feedback Equation Of Motion Function POLYNOMIAL REFERENCE TRACKING CONTROLLER 1. Polynomial Reference Maneuver Tracker Main Program 2. Polynomial Reference Tracking Equations Of Motion 65 65 69 72 72 Function 75 Polynomial Reference Trajectory Function 77 NEAR-MINIMUM-TIME RERENCE TRACKING CONTROLLER D. 64 65 LINEAR FEEDBACK MATLAB SIMULATION PROGRAM 3. 62 80 1. Main Program 80 2. Near-Minimum-Time Equations Function 83 3. Near-Minimum-Time Reference Function 86 MISCELLANEOUS FUNCITONS 91 1. Equation Of Motion Coefficient Funciton 91 2. Inverse Kinematics Function 94 vu APPENDIX D 95 A. LINEAR CONSTANT GAIN FEEDBACK CONTROLLER 95 B. POLYNOMIAL REFERENCE TRACKING CONTROLLER 99 C. NEAR-MINIMUM-TIME REFERENCE TRACKING CONTROLLER 103 REFERENCES Ill INITIAL DISTRIBUTION LIST 113 Vlll LIST OF TABLES TABLE 2.1 Momentum Wheel TABLE 2.2 Link Actuator Characteristics TABLE 2.3 Kepco Power Supply TABLE 2.4 Mass And TABLE A.l Actuator Characteristics Characteristics Inertia Characteristics Servo Motor Characteristics IX 6 8 9 13 56 LIST OF FIGURES Figure 2.1 Spacecraft Robotics Simulator 5 Figure 2.2 Two 7 Figure 2.3 Control System Integration Figure 3.1 Inertial And Figure 3.2 Inertial Angles Figure 3.3 Polynomial Reference Maneuver 26 Figure 3.4 Bang-Bang Minimum Time References 29 Figure 3.5 Normalized Input Shaping Funciton With a= 0.1 31 Figure 3.6 Normalized Input Shaping Funciton With a= 0.25 31 Figure 4.1 Reference Maneuver Figure 4.2 Linear Constant Gain Feedback Joint Position Link Manipulator 12 Local Coordinate Systems And 15 Vectors 16 37 Time History WithGp=l AndGv=5 Figure 4.3 39 Linear Constant Gain Feedback Joint Velocity Time History WithGp=l AndGv=5 Figure 4.4 39 Linear Constant Gain Feedback Torque Time History With Gp=l AndGv=5 Figure 4.5 Linear Constant Gain Feedback History With Figure 4.6 40 Gp=l And Gv=5 40 Polynomial Reference Tracking Controller Joint Position Time History With Gp=l, Figure 4.7 Momentum Wheel Speed Time Gv=5 And Maneuver Time Of 2.5 Seconds....43 Polynomial Reference Tracking Controller Joint Velocity Time History With Gp=l, Gv=5 And Maneuver Time Of 2.5 Seconds.. ..43 Figure 4.8 Polynomial Reference Tracking Controller Torque Time History With Gp=l, Gv=5 And Maneuver Time Of 2.5 Seconds Figure 4.9 Polynomial Reference Tracking Controller 44 Momentum Wheel Speed Time History With Gp=l, Gv=5 And Maneuver Time Of 2.5 Figure 4. 1 44 Seconds Near-Minimum Time Tracking Figure 4.11 History With Figure 4. 1 2 47 Controller Joint Velocity Time Gp=l And Gv=5, Maneuver Time Of 2.5 Seconds Near-Minimum Time Tracking Controller Torque 2.5 Time History Seconds , And a = 0.25 48 Near-Minimum Time Tracking History With And a = Figure 4.14 , Wheel Speed Time Gv=5, Maneuver Time Of 2.5 Seconds , 48 Gp=l , Controller Joint Position Time Gv=5, Maneuver Time Of 2.5 Seconds , 49 0.1 Near-Minimum Time Tracking History With And a = Controller 0.25 History With Figure 4. 1 5 Gp=l Near-Minimum Time Tracking And a = , 47 0.25 With Gp=l And Gv=5, Maneuver Time Of Figure 4. 1 3 , 0.25 Near-Minimum Time Tracking And a = Time Gp=l And Gv=5, Maneuver Time Of 2.5 Seconds History With And a = Controller Joint Position Gp=l , Controller Joint Velocity Time Gv=5, Maneuver Time Of 2.5 Seconds , 49 0.1 XI Figure 4. 1 6 Near-Minimum Time Tracking With Gp=l , Controller Torque Time History Gv=5, Maneuver Time Of 2.5 Seconds And , a= 50 0.1 Figure 4.17 Near-Minimum Time Tracking History With And a = Figure D.l Gp=l , Controller Wheel Speed Time Gv=5, Maneuver Time Of 2.5 Seconds , 50 0.1 Linear Constant Gain Feedback Joint Position Time History With Gp=0.1 And Gv=0.2 Figure D.2 95 Linear Constant Gain Feedback Joint Velocity Time History With Gp=0.1 And Gv=0.2 Figure D.3 96 Linear Constant Gain Feedback Torque Time History With Gp=0.1 AndGv=0.2 Figure D.4 Linear Constant Gain Feedback History With Gp=0.1 Figure D.5 98 Gp=l , Gv=5 And Maneuver Time Of 5 Seconds 99 Gp=l , Gv=5 And Maneuver Time Of 5 Seconds... 100 Polynomial Reference Tracking Controller Torque Time History With Gp=l Figure D.8 And Gv=0.2 Polynomial Reference Tracking Controller Joint Velociy Time History With Figure D.7 Momentum Wheel Speed Time Polynomial Reference Tracking Controller Joint Position Time History With Figure D.6 97 , Gv=5 And Maneuver Time Of 5 Seconds Polynomial Reference Tracking Controller Speed Time History With Gp=l 5 Seconds , 101 Momentum Wheel Gv=5 And Maneuver Time Of 102 xu Near-Minimum Time Tracking Figure D.9 History With a= , 1 , Controller Joint Velocity Time Gv=5, Maneuver Time Of 5 Seconds , And 104 0.25 And a = , Controller Torque Time History Gv=5, Maneuver Time Of 5 Seconds , 0.25 1 Near-Minimum Time Tracking History With Figure D. 1 3 Gp=l Near-Minimum Time Tracking a= And 103 With Gp=l Figure D. 1 2 , 0.25 History With a= Time Gv=5, Maneuver Time Of 5 Seconds Near-Minimum Time Tracking Figure D. 1 Figure D. 1 Gp=l Controller Joint Position Gp=l , Controller Wheel Speed Time Gv=5, Maneuver Time Of 5 Seconds And , 106 0.25 Near-Minimum Time Tracking History With Gp=l , Controller Joint Position Time Gv=5, Maneuver Time Of 5 Seconds And , a = 0.1 Figure D. 14 107 Near-Minimum Time Tracking History With Gp=l , Controller Joint Velocity Time Gv=5, Maneuver Time Of 5 Seconds And , a = 0.1 Figure D. 15 Figure D. 1 6 108 Near-Minimum Time Tracking With Gp=l 05 , Time History Gv=5, Maneuver Time Of 5 Seconds And , Near-Minimum Time Tracking History With Controller Torque Gp=l , Controller a = 0. 1 . 1 09 1 10 Wheel Speed Time Gv=5, Maneuver Time Of 5 Seconds And , a = 0.1 xm INTRODUCTION I. During the past few years there has been a significant increase robotics. in the use of Applications range from performing routine tasks in manufacturing to The deep sea and interplanetary space exploration. extraterrestrial and environment has become the focus of research for future industrial development and scientific exploration. and cost of manned space upon robotics interplanetary With the hazards of flight, researchers will become for assembly, service, and repair of the exploration of space itself. Due this environment increasingly dependent equipment in space as well as to the requirements for terrestrial and space applications, there has been a significant increase in theoretical and experimental research in the areas of robotic dynamics and control. Space based robotic applications stabilize the manipulator. energy that is terrestrial applications in A space based manipulator, when forces on the spacecraft. In addition, there added to the system. To is momentum wheel is is one provided to repositioned, imparts no friction to dissipate counteract these forces and moments, an system normally consisting of thrusters attitude control in combination with a used to stabilize the spacecraft. Control system problems are exasperated as the mass of the load that relation to the from For the space based applications, no support important area. moments and differ mass of the spacecraft. is being positioned becomes larger in It is this interaction between spacecraft and the motion of a manipulator that warrants further research. The primary objective for any control system range of operating conditions while performance. It is still is to remain stable over a wide providing adequate levels of desired to meet this objective in the face of hardware characteristics, changing loads as well as unmodeled disturbances and system dynamics. These requirements and restrictions present the control system design engineer with significant challenges. Before the system can be analyzed, the equations of motions must be determined. The equations of motion for a robotic system can easily be developed through Lagrange's equations and are in the form of a set of second order differential equations. These equations are coupled and nonlinear with trigonometric and higher order terms. Attempts to simplify these equations result in equations of motion that are valid over a limited range of boundary conditions. The current motion or for specific trend in trajectory control requires highly nonlinear maneuvers that are valid over a wide range of applications and operating conditions. These requirements dictate that the full, nonlinear equations be used to describe the motion of the system. With the introduction of the nonlinear equations of motion, many traditional tools in control theory used to analyze linear, time-invariant systems are not available or are meaningless. Recently, research by Junkins [Ref. in using 1] and Bang [Ref. 2] has revived interest Lyapunov's second method for a flexible structure control system design. This technique is very attractive because it can be applied to nonlinear, time invariant, systems with guaranteed stability for a important feature of Lyapunov's second method Lyapunov function and wide range of conditions. is the freedom the corresponding feedback control law. An to select the The Lyapunov function can be selected based on physical insight and the control law can be selected to ensure that the system positive definite and is is stable. normally related mechanical natural systems. The to the The Lyapunov function must be system energy for a large class of control law can be selected such that the Lyapunov function or system energy will always decrease to zero or some equilibrium point. The purpose of Lyapunov this thesis is to apply a general methodology for finding stable control laws for stabilizing the spacecraft controlling a two main body while link manipulator attached to that spacecraft. description of the experimental setup is discussed in Chapter the physical characteristics of the manipulator, main body, simulation, and data collection equipment. In Chapter and the equations of motion are developed. III, II. A complete Topics include actuators, sensors, test, the coordinate systems Three different control strategies were developed using Lyapunov's Second or Direct Method. With the first controller, simple linear feedback of position and velocity information with constant gains was used to position the manipulator while stabilizing the main body. A fifth order polynomial was used to generate a reference trajectory for the second controller. This trajectory was used in conjunction with a tracking controller to position and stabilize the system. minimum-time technique was used In the third controller, a near- to generate a reference trajectory. This reference trajectory was employed with a tracking controller similar to that used in the polynomial reference controller. Chapter IV. Chapter for future research. V includes a Simulation results are presented in summary of the conclusions as well as topics II. A. EXPERIMENTAL SETUP SPACE ROBOTICS SIMULATOR DESCRIPTION The Spacecraft Robotics Simulator (SRS) is a modification to the Flexible Spacecraft Simulator (FSS) used for previous work by Hailey [Ref. Watkins [Ref. 4]. and replacing this central The FSS was modified by removing is and the flexible appendage appendage with a two link manipulator. The SRS consists of a main body with a two main body 3], link robotic manipulator. provided by a single momentum wheel Pitch axis control of the driven by an electric servo- motor. The central body was constrained to rotational motion only by an I-beam mounted over the over the granite table. supported by air bearings that float upon a thin cushion of air on an optical quality granite surface. A servo-motors. The main body and manipulator were Each of the two links were positioned via geared Rotary Variable Displacement Transducer (RVDT) was obtain position information at each joint for position feedback. designed to simulate a zero-gravity environment in EXPERIMENT DESCRIPTION The SRS is composed of the following major components: Spacecraft Two Main Body Link Robotic Manipulator RVDT Position Sensors Granite Table Electrical Power Supplies AC- 100 VAX 3100 Series Computer used to This setup was two dimensions. The SRS depicted in Figure 2.1. B. DC is Figure 2.1 Spacecraft Robotics Simulator 1. Spacecraft Main Body The spacecraft main body aluminum diameter, disk. consists of a rigid, 7/8 inch thick, 30 inch The main body is dimensional planar motion of a spacecraft about supported by three bearings is air designed to simulate the two its pitch axis. The main body bearings spaced at 120 degree intervals. capable of supporting a load of 60 pounds. A Each of is the fourth air bearing supported by an overhead I-beam constrains the spacecraft main body to rotational body on a motion only. The air bearings are designed to float the spacecraft main thin film of air supplied R30D, was connected by an external air source. to the rotor of the air bearing A RVDT, model by a bellow-type device. The RVDT was manufactured by Schaevitz Sensing Systems was used to measure angular displacements of the spacecraft main body. Attached to the spacecraft main body was a 10.7 kilogram momentum wheel and a torque to the momentum servo motor. The momentum wheel was designed to apply main body by increasing or decreasing the angular velocity of wheel. The motion of the spacecraft about controlled by the torque generated by this model JR16M4CH/F9T used PMI steel to drive the industries. Characteristics of this momentum its wheel. pitch axis the was The servo motor, momentum wheel was manufactured by motor are presented in Table 2.1. TABLE 2.1 Momentum Wheel Actuator Characteristics Units Characteristic PMI Motion Manufacturer Technologies JR16M4CH-1 Model Number Rated Speed rev per minute 3000 Rated Power horse power 1.4 Rated Torque inch-pound 31.85 Rated Current amps 7.79 Rated Voltage volts 168 Outside Diameter inches 7.4 Height inches 4.5 Weight pounds 17.5 An integral analog tachometer, Industries, Inc. was mounted on model ARS-C121-1A, manufactured by Watson the servo-motor to measure angular velocity. A more detailed description of the motor, body can be found 2. Two Link Attached the manipulator Department in [Ref. 3], and [Ref. wheel, and spacecraft main 4]. Manipulator to the main body was a two were designed and at the momentum built link manipulator. Components for by the Aeronautics and Astronautics Naval Postgraduate School. All components for the arm were manufactured from 7075 and 6061 series aluminum. All components were connected with SAE grade 8 medium carbon chrome alloy cap screws. of the manipulator can be found in Figure 2.2. Figure 2.2 Two Link Manipulator A picture The links of the manipulator manufactured by PMI. mounted on pointing A were positioned by two geared, servo-disk motors third motor, identical to the first the tip of link two for the purpose of some type of antenna. At each presented in Table 2.2. two actuators and was orienting a simulated tool or Characteristics for the link actuators are joint, a RVDT, model R30D, manufactured by Schaevitz was used to measure relative angular displacement. TABLE 2.2 Link Actuator Chars icteristics Units Characteristic PMI Motion Technologies Manufacturer 9FGHD Model Number Rated Speed rev per minute 22 Rated Torque inch-pound 80 Rated Current amps 5.6 Rated Voltage volts 12 Outside Diameter inches 4.75 Height inches 3 Weight pounds 3.2 The power supplies used to drive the actuators of the manipulators were manufactured by Kepco Inc. of Flushing, Bipolar Power Supplies were designed to be linear amplifiers. The BOP power supply New fast, is an York. The Kepco programmable, all series BOP fully dissipative, solid-state design, featuring integrated circuit operational amplifiers in the control circuit section and silicon power transistors mounted on special fan-cooled heat sinks in the 8 complementary power stage. Characteristics for the Kepco model BOP 20-10 power supply are presented in Table 2.3. TABLE 2.3 Kepco Power Supply Output Power watts 200 Max. Input Current amps 5.5 Max. Input Power watts 540 DC. Output Range volts ±20 amps ±10 volts/volt 2 amps/volt 1 Closed Loop Gain Bandwidth Rise BOP 18 (voltage kilohertz Time Recovery Step Load The Characteristics kilohertz 6 (current mode) microseconds 20 (voltage mode) microseconds 60 (current mode) microseconds 80 (voltage mode) microseconds 20 (current mode) can be operated in either the voltage or current bipolar control channels. These mode) modes mode through two are manually selectable through the front control panel or through remote signals. Each of the principal control channels is protected by bipolar limit circuits. All control and limit channels are connected to the output stage via an "Exclusive-Or" gate so that only one channel of the full BOP output at any one time. The output range by a ±10 BOP output can be volt signal applied to either is in control programmed over one of the inputs its to the voltage or current channel. by a to +10 The limit control channels can be remotely controlled volt signal applied to there respective inputs. Granite Table 3. The entire mechanical manipulator were supported by air assemblage, including the main body and bearings that float on a thin cushion of air on a granite table with dimensions of 8 feet by 6 feet by 10.5 inches thick. of the table was highly polished to optical quality grade valley). The smooth A The surface (0.001 inch peak to surface allows the air bearings to float freely over the surface of the granite table to minimize the effects of friction on the motion of the main body and manipulator. The gravity induced accelerations. granite table was carefully leveled The mass of the to eliminate table provided an extremely stable platform upon which to conduct the experiments. 4. AC - 100 The AC- 100 is a microprocessor based, programmable, real time control system manufactured by Integrated Systems International, California. The AC- 100 was designed to Inc. of San Jose, automate the development of real-time systems by combining graphical modeling tools with a real-time controller. In addition to modeling and controlling, the collection and storage. Intel The AC-100 AC- 100 was also capable of data consists of the following major components: 80386 Application Processor, Intel 80386 Multibus II Input / Output Processor, Intel 80386 Communication Processor, Intel 80387 Coprocessor, Weitek 3167 Coprocessor, Analog To Digital and Digital To Analog Digital Data Translation DT2402 Input/ Output Board, Two - INX-04 Encoder and Digital To Analog Servo Boards, Ethernet Interface Module, and Cabinet Enclosure and Power Supply 10 The software a AC- 100 tools used with the include a Design Package and Run-Time Package. The Design Package included Matrix x System Build, and , Auto Code. These tools are used for analysis, design, and code generation The Run-Time Software Package provides respectively. Interface (GUI) cross compiler, device the Graphical User drivers, data acquisition, interface required to run software code generated and Ethernet by Auto Code on the AC- 100. VAX 3100 Series Computer 5. The VAX 3100 Series Model 30 workstation was configured with 8 megabytes of main memory, a 19 inch (diagonal) color monitor, two 104 megabyte The Winchester hard disks and a mouse. VAX workstation is capable of 2.8 Million Instructions Per Second (MIPS). C. SYSTEM INTEGRATION The AC- 100 interface. is integrated with a In this system, the the control system. VAX VAX 3100 Series computer 3100 computer was used Auto Code was then used linked to the form of in turn C - AC- 100 were used is still the Ethernet interface. 2.3. two to The A system This code was then control system, in the conuol the servo-amplifiers which provided through the VAX Manual computer acting through block diagram of the integration of the control system link manipulator and spacecraft Mass and and design to drive the actuators of the manipulator in real time. control of the system for the via the Ethernet interface. code software can then be used to analyze to convert the final control design into fully compiled and linked "C" computer code. down via the Ethernet inertia characteristics are main body are presented summarized 11 in Table 2.4 in Figure Reaction Wheel VAX-3100 Matrix X System Build Autocode RVDT RVDT / Motor RVDT / Motor /'Motor Granite Table Figure 2.3 Control System Integration 12 TABLE 2.4 Mass And Body Arm Characteristic Units Value Mass kg 2.09 Inertia (1) kg-m^ 0.3102 Inertia (2) kg-m^ 0.032 cm 36.45 Mass kg 2.47 Inertia (1) kg-m^ 0.3542 Inertia (2) kg-m^ 0.054 Of Mass(3) cm 34.90 Mass kg 52.73 Inertia (1) kg^n^ 0.3542 Mass kg 10.67 Inertia (1) kg^n^ 0.0912 1 Center Arm2 Center Main Body Momentum Wheel Of Mass(3) 20 cm Moment Of Inertia About Arm Axis Of Rotation. Moment Of Inertia About Center Of Mass. Center Of Mass Location With Respect To Axis Of Rotation. Center Notes: (1) (2) (3) Inertia Characteristics Of Mass 13 ANALYTICAL MODEL III. The Spacecraft Robotics Simulator (SRS) which is consists of a central main body The motion of main body attached a two link robotic manipulator. move constrained to in a plane resulting in a the to is system with three degrees of freedom. This planar motion constraint greatly simplifies the problems associated with the derivation of the equation of motion and control system design yet retains the modeled most critical analytical as rigid structures. elements. still The manipulator and main body were Lagrange's equation was used to derive the equations of motion. This section will first describe how the equations of motion were derived and then how the control system was developed. A. COORDINATE SYSTEMS Before the spacecraft attitude and manipulator can be controlled the dynamics of the system must be carefully defined and understood. The in this process, is to determine the equations of motion for the system. In this model, the main body different coordinate systems this problem was fixed is constrained to rotational motion only. were used for in inertial this analysis. xi, yi, z\, is fixed in the xi axis pointing toward the attachment point for link is The Ni, N2, N3 Four axis for space coincident with the axis of rotation of the main body. The coordinate system, system first step 1. The main body with the xi, yi, z\ coordinate obtained by rotating about the N3 axis of the inertial coordinate, by an angle of 61 . In a similar axis of rotation of link 1 way, the coordinate systems, X2, yi, Z2, is fixed at the and points toward the attachment point for link 2. The x 2> Y2> Z2 coordinate system can be obtain by rotating about the N3 axis of the 14 . inertial coordinate system by an angle of 02- fixed in the 2. body of link The coordinate system, X3, y3, Z3, is 2 with the X3 axis pointing toward the end point of link This coordinate system is obtained by rotating by an angle, 3 about the N3 axis of the inertial system. Coordinate systems are presented in Figure 3.1 O Servo Motor Manipulator Arm Local Coordinate Axis Figure 3.1 Inertial And Local Coordinate Systems All angles, Q\, 62, and (%, as well as vectors r \ , r*2, and F3 are defined in terms of inertial coordinates where these quantities are defined in Figure 3.1 15 A Center O Of Mass Servo Motor fr* --" ^\ Manipulator ? /A Inertial Arm N2 |/\ e rl/ Momentum m -2^/Linkl yf/^\ lx Wheel /. y^\^2 As> o ./^ \ *^ \ \ ei Satellite r2, T Y* ^P cm. Main Body and r3 describe 832 = 021 =02-01 63 - 02 i Jr Figure 3.2 Inertial Angles , X S>Ni _^^^^^k. N3 >v vectors ri 3 rv, c- The #32 Axis / y' Link 2 l Cm And Vectors the location of the center of the manipulators in terms of inertial coordinates. mass of each of The angles 621, 631, and 632 represent relative displacements of the joints and can be derived from the inertial coordinates 81, 62, and 83 in such a @32 = 63 - way @2- 16 that 621 = 02 - 61, 631 = 63 - 9i, and B. EQUATIONS OF MOTION A momentum The motion of body. the wheel was used momentum wheel assumed It is 1. is main body. de-coupled from the motion of the main momentum wheel that the control the motion of the movement to apply a control torque to the is only an actuator that is used to main body and counteract torques generated by the of the manipulator. Lagrange's Equations Of Motion Lagrange's equation for n-dimensional dynamic systems are stated as 4j— —= - dtldqi/ Qj where i = , 1, 2, 3,..., n (3.1) dqi L = T-V V - Potential T - Kinetic Energy qi - i qj - 1 Generalized Coordinate th - i Qi For this particular to describe its velocities are 2. Energy Generalized Velocity Applied Non conservative Force problem, the system requires three degrees of freedom governing equations of motion. chosen to be in terms of T q = {ei,e 2 ,e 3 } q = {ei,e 2 ,e 3 ) T inertial The generalized coordinates and coordinates as presented below. = (qi,q2,q3)T = {qi,q2,q3) T (3-2) (3-3) Kinetic Energy The total kinetic first step is to determine the total kinetic energy of the system is given by 17 energy of the system. The T=XTi (3.4) i=l Ti = 2 ll ei i :th Ii - i i = i (3.5) .?i) i Mass 1 X f rir i= 3. lm (? Mass Moment Of Inertia About Center Of Mass :th mi - A more detailed + 1 = 1 (3.6) 1? l^Bi 0j (y 4 -yj) 1 algebraic procedure can be found in Appendix B. Equation Of Motion Lagrange's equations can be applied to equation (3.4) and the terms can be arranged in matrix form so that the M(e) e + G(e, e) = final form can be written as q (3.9). (3.9) where rriiimnmn M(6) = rri2i rti22 rri23 L11131 m32rri33 (3.10) and mn = II + L? [mi + m2 + m22 = Icm2 + L2 m2 1113] '^1 U + rri4 L4 (3.11) +m3 (3.12) I ™33 = Ic.m.3 mn = m2i = mi 3 =m 3 i + Lc.m.3 m (3.13) 3 Li L2 cos(G2i) ^ ksLi +m = Li Lcm3 cos(9 3 i) m3 18 3 (3.14) (3.15) m2 3 = m32 = L2 Lcm 3 m cos(6 32 ) (3.16) 3 G(l) G(9,9) = G(2) (3.17) LG(3) G(l)=[(m 2 L L c m )+(m 3 L 1 . . 2 1 L2)]sin(e2i)e2 + (3.18) LiL c m (m 3 . . )sin(e 3 i)e 3 3 G(2)=-[(m 2 L L c m )+(m 3 L L 2 )]sin(e 21 )ei 1 . . 1 2 (3.19) •2 (m 3 L 2 L c m )sin(e 31 )e 3 . G(3) = - (m 3 L i L cm . 3) . 3 sin (6 2 i) 6! - (3.20) (m 3 L 2 L cm )sin(e 32 )e 2 . 4. 3 Applied Torques D' Alembert's principle for virtual work expression was used to determine the expressions for Q for the equations of motion. As written, Q is a vector containing the torques applied by the actuators at each joint in terms of inertial coordinates. At this point local coordinates. The it will be beneficial to rewrite the virtual work applied to the Q system vector in terms of is given by the following equation. 5w = X,QiSe ^i;5w 1 i=l The elements of Q i=l are in terms of torques applied to inertial coordinates. each actuator applies a local torque, Q in (3.21) j it was beneficial to rearrange (3.21) and write terms of the local torques applied by the individual actuators. 19 Since The virtual work described in equation (3.21) can be described in terms of these local torques as follows. 5W =Ui6ei-u 2 56i 1 (3.22) 5W 2 = 112582-113862 (3.23) 5W 3 = u 3 59 2 (3.24) In matrix format Q can be written in terms of a control influence matrix B and the local torque vector, u as written in (3.25). Q = Bu=[Qi,Q 2 ,Q 3r Qi = 111 Q2 = u2 (3.25) -u 2 (3.26) u3 (3.27) - Q3 = U 3 (3.28) where 1-1 B= 1 Ul u = u2 -1 LO Lu 3 1. C LYAPUNOV STABLE CONTROLLER DESIGN This design for a stable non-linear controller Stability Theory. This theory method. This theory is is also known as covered in greater detail is based upon Lyapunov's Lyapunov's Second or Direct in [Refs. 1, 2, 14, and 15]. To review the Lyapunov Stability Theory, a system without any external forces or torques this is assumed. This system is assumed system, a positive definite function is 20 to have a single equilibrium assumed to be state. For an exact integral of the system under some idealization. This function is selected to satisfy the requirements that positive everywhere else. This function is zero it is may termed a Lyapunov function and at the desired equilibrium and be represented by the system total energy or the Hamiltonian of the system for most of the time invariant mechanical systems. If this system some increased to Lyapunov perturbed from its state, the energy state is Depending upon the nature of positive energy level. system dynamics dictate that the not increase with time, then the system • equilibrium the function, one of the following conclusions can be drawn. If the • is If the initial energy of the system does is stable. energy of the system monotonically decreases with time for conditions, and eventually goes to zero, then the system is all initial asymptotically stable. • If the energy increases for any • If the energy measure neither increases, nor decreases as a function of time initial condition, then the system is unstable. then no conclusion can be drawn. Despite the power of Lyapunov functions description of the found 1. in [Refs. 1 this theory, there is no unified process that globally satisfy stability requirements. Lyapunov and to find candidate A more complete stable controller design for flexible structures can be 2]. Lyapunov Stable Controller With Linear Feedback For this application, a simple non-linear controller with linear feedback of position and velocity information Lyapunov function is for this application u is the primary objective. of the form = E + f(5e,,5e2,5e 3 21 The candidate (3.29) ) where E is defined as the "work energy" of the Lyapunov function and positive function of that renders the 80 1, 802, and 803. Note that "f Lyapunov function = equilibrium point. In addition, 80j ' is "f" is a a pseudo potential energy positive definite with respect to the - 0j 0j f where , 0j, is new a constant that describe the final joint angle. (ei, 2 , 03, ei, 2, The Lyapunov function U = The "work energy e 3) f = (0if, 2 f, o, o, o) E+ V -^80! rate", (ui - 0.30) can be differentiated to obtain in (3.30), (3.31) E, of (3.31) can be directly obtained from (3.22) through (3.24) and the "pseudo energy rate" of the system E= e 3 f, u2 ) 80i + (u 2 "Pseudo Energy Rate" = defined in (3.33). + u 3 80 3 u 3 ) 80 2 - is (3.32) -^- S0j JT i=l ^ (3.33) l> Equations (3.32) and (3.33) can be substituted into (3.31) to form equation (3.34) which can be further simplified U = (ui - 112) to equation (3.35). + 80i (112 - 3 = 80i Ul - U2 + + 113 803 (3.34) . ?f £ U U3) 802 5(50.) -7 + 80 2 U 2 r " U3 + ^663 ^50! (3.35) + 80 3 |u 3 + 3(563), Based on which is (3.35), it is evident that a function can be selected such that the stability condition in the Lyapunov laws are chosen to satisfy the following relations 22 sense. U < 0, Therefore, the control " Ul U2 + = - g '- 8ei (336) = - g2 8G2 (337) 3(lj - U2 U3 + « - ^Jr*- 803 (338) where, gu > 0, g 2v > 0, g 3v > 0, g, > 0, g 2p > 0, and g 3p > for f>0 In addition, U By = 2 ( 2 2 + g 2v 60 2 + g 3v 56 3 gi ¥ 89! (3.40) ) selecting an appropriate function for "f" equation, the stabilizing control laws (ui, U2, U3) satisfying the U < requirement can be built. For this case, assume that "f" is defined as follows f=i(gipSe?+g2 P 8e 22 +g 3p 59?) where gi P > 0, g 2p > 0, g 3p > For a controller using pure linear feedback, (3.36-3.38) can be simplified as described below. U3=-g*89 3 -g3»893 (3.42) U2=u 3 -g*582-g2,8e 2 (3.43) ui=U2-gi 5e -g 69 (3.44) p 1 lv 1 23 5Gi = 6i - 9 if 50i = 6i - 6 , fori =1,2,3 (3.45) (3.46) if where 6^ 2. = fori = 1,2,3 Lyapunov Stable Tracking Controller In a similar way, the Lyapunov stable tracking controller can also be derived. Lyapunov [Ref. In this application, a function generator 1]. stability is not proven here but trajectory for the controller to follow. polynomial was used With this is discussed in was used detail in to generate a reference type of controller, a to generate a desired tip trajectory with a loop inverse kinematic solution in one case. more two fifth order link closed In another case a "near- minimum- time" torque shaping scheme was utilized to generate a reference trajectory. Both these trajectory generators will be discussed in The more detail in following sections. control torques required for this reference tracking controller are presented next. 5u 2 = 5u 3 - g 2p 5e 2 8ui = 8u 2 -gi p - g 2v 50 2 8ei-g l¥ 89i (3.48) (3.49) where 89i = 6 iref (3.50) 861 = 61-6^ (3.51) 8ui = Ui-u^ (3.52) 6i - 24 The control laws presented above can be rigorously proven to satisfy the Lyapunov Stability condition if 50j and 50j are very small. REFERENCE TRAJECTORY GENERATION D. For the tracking controller developed previously while discussing the Lyapunov stable tracking controller requires some reference Here trajectories. trajectory refers to a time history of position, velocity, and acceleration for each degree of freedom. The primary consideration for generating a trajectory must the fact that the trajectory calculated. In this thesis, two first of all be smooth and second it lies in must be easily different techniques will be used to generate the required trajectory information to stabilize and control the system. The first trajectory polynomial that will be discussed involves using a fifth order to describe the path of the tip of the link three of the manipulator. the second trajectory, a near-minimum-time In maneuver using input torque shaping will be used to generate the desired trajectory. 1. Polynomial Reference Trajectory For the tracking controller discussed difficult if not impossible to obtain the in the previous section, it is usually open-loop solutions for the theoretical reference system of differential equations. For practical considerations it is often advantageous to design the control system that will follow an easy to calculate path. For robotic applications, a polynomial of order 3 or higher is often used to specify the position of the end of the manipulator. For this application, imagine a two link manipulator attached to the spacecraft 3.3. In this case, the maneuver attempts main body to position link as depicted in Figure two from an of 20 degrees to a final position of 40 degrees and link three 40 to 60 degrees. For a two is initial angle maneuvered from link manipulator, there exists a closed loop solution 25 to the inverse kinematic closed loop solution is problem that can be quickly and easily computed. The described in more detail in appendix B. For this manipulator, given the beginning and final coordinates of the manipulator, the vector r of time. is used to specify the position of the end point of link two as a function This vector is presented in Figure 3.3. Reference Maneuver Polynomial •-.._ N? N, Figure 3.3 Polynomial Reference Maneuver 26 1 . ?(t)=?(to) + f (t)[F(tf)-7(to)] To simplify the calculations, given the start time x = to t (3.53) can be written as a function of normalized time, and finish time Note tf. that for to < t < tf, then <T< ^^ T, 1 (3.54) t f- A fifth order polynomial was used for this application. This allowed the user to specify the beginning and final velocities and accelerations as well as the beginning and final positions of the the tip. The polynomial for this controller was of form f(x) and was subject = Ci + C2 X + C3 X 2 to the following f(0) = 0,f(0) = + C4 X 3 + C 5 X4 + C 6 X5 (3.55) boundary conditions. 0,f(0) = f(l)=l,f(l) = 0,f(l) = The resulting expression for f(x) is obtained as f(x)=10x 3 -15x4 + 6x 5 (3.56) Equation (3.56) can be differentiated with respect to time. These expressions were utilized to calculate velocities and accelerations and are presented in equations (3.57) and (3.58). f(x) = 30x 2 -60x 3 + 30x4 f(x) = 60 x- 180 x2 + 120 x3 (3.57) (3.58) Equation (3.53) can also be differential with respect to time to give the velocity and acceleration of the tip of link three purely as a function of time. Vector Position: 27 1 F(T)=-?(to)+f(T)[F(tf)-7(t (3.59) )] Vector Velocity: r(T) = r(tf)-r(to)~ f(T) (3.60) tfto Vector Acceleration: ?(T) = (tf)-r(to) f(T) (3.61) 2 L With (tf - to) the position of the tip and 632 can be solved for directly known as a function of time, the angles 821 and 63 via the two link closed loop as can 62 solution of the inverse kinematics problem. assumed At this point 61, 61, The Jacobian can be used to be comparatively small. and 61 are all to define the relationship between the velocity and angular velocity as well as the tip acceleration and angular acceleration as described in the following equations. r where H is =Hje| (3.62) the Jacobian corresponding to the given configuration. H= - 1 2 - 13 sin (63) (3.63) 12 r sin (6 2 ), cos (8 2 ), I3 cos (e 3 ) =H(0) + H(e] - H= 2 cos (e 2 ), (3.64) - 13 cos (e 3 ) (3.65) - 1 2 sin (0 2 ), With the above equations, - 13 sin (83) the position (angular and cartesian), velocity (angular and cartesian), and acceleration (angular and cartesian), can determined as functions of time. With 28 this information, all be and the physical B ' characteristics of the system, the reference torques can also be determined using equation presented below. _1 = u re f [m (e) e + G(e,e)] 0.66) Minimum-Time-Maneuver 2. For an ideal case, of a single degree of freedom system, the minimum time achieved by applying the maximum available torque for one-half of the time required to complete the maneuver required to perform a particular maneuver maximum followed by the remaining half with the a controller where the torque rise to a torque is is always operated maximum at its value and gives shaping function, position, velocity, and accelerations profiles as presented in Figure 3.4. This type of controller bang negative torque. This results in is sometimes referred to as a bang- controller. 1.2 _,.• 0.8 | 0.4 ^ * y ^ •* * . .-''' "S. E ^- < ..._ 1 -0.4 - — ahape - - - : 1 unction __ - Referer ce Angle £ Referer ce Velocity -0.8 Rpfprpnrp Arrplpratinn -1.2 : 1 -0.2 0.2 0.4 0.6 0.8 Normalized Time Figure 3.4 Bang-Bang Minimum Time 29 References 1.2 For an application with a single degree of freedom, the relationship between the angular acceleration and the applied torque (3.67) with the applied torques given by equation is given by equation (3.68). I6 = u (3.67) < Umax, for < t £ 2 / U = \ -U max for , 0, for Bang-Bang control theory detail in [Refs. 3. 1 and ^<t< < tf for (3.68) tf t minimum time maneuvers discussed in more 2]. Near-Minimum-Time Rigid Body Maneuver A more general case of the bang-bang, controller. manipulator from the initial to final time controller controller does positions in the primary drawback of the bang-bang controller trajectory. minimum The bang-bang near-minimum-time which is is the maneuver the minimum amount is of time. The the rapid rise of the torque This results in a rapid acceleration followed by a rapid deceleration will require actuators with instantaneous switching capacity which is practical and a robust structural design for the manipulators themselves. not By introducing an input shape function, the instantaneous rise in the torque trajectory can be reduced, resulting in slightly smaller accelerations which will in turn require smaller actuators and reduced structural requirements. comaparison, the input shaping function for in Figures 3.5 The a= 0.1 and a= For 0.25 are presented and 3.6 respectively. differential equations above can be modified so I 6ref used to describe the minimum time maneuver that they are of the form. = Uref = U max f(At,t f ,t) 30 (3.70) — \ " i , 1.2 ---.. / 0.8 4 **. .-•*" | 1 0.4 o. E 1 # ***., »»* * -"" ,.-.*" # - _ . » \ < • - iN 5hape -0.4 — " -0.8 : I — "-lli.._ unction Referer ice Angle / Kctcrcr ice Velocity ' — ===== -1.2 1 1 0.2 0.1 0.4 0.3 0.6 0.5 0.8 0.7 0.9 Normalized Time Figure 3.5 Normalized Input Shaping Funciton With 1.2 a = 0.1 T ---... ._- 0.8 .'' N. ,'' 3 0.4 \ *° **. * -*" % .-"\ D. E ...'*'' < 1 -*SL*S. -0.4 o - Shape Function - Reference Angle y Z Reference Velocity -0.8 Referen LC /\CCCIC aiiun -1.2 1 0.1 1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 Normalized Time Figure 3.6 Normalized Input Shaping Funciton With 31 a= 0.25 Where the input shaping function is suggested in the form. & =1- 2 M < for t < At Ati = f(At,tf,t)= 2 " 3 1 (M for At < -i+M < ti 3-2(^1, \2At = t \ - 1 2At for t2 < t for < < t < t2 t3 - 2 3-21^ 3 At ti II hH for At t3 < t < tf (3.69) where tf = Maneuver Time ti = ts - At t2 =ts+At = t3 At tf - At = atf = Rise Time ts = P To determine maximum tf = Switching Time, where (3 0.5 a relationship between the angular acceleration and the available torque M(e)e + we G(e, e) begin with the non-linear equations of motion = Bu @.70) where M(6) - Mass Matrix G(0, 6) u = - - Coriolis Acceleration Local Torques 32 Terms Equation (3.70) order terms. is linearized by eliminating These linearization process all non-linear cosine, sine, and higher results in the following equation for an th 1 link. I 6ref = Uref I - = U max f(At,t f ,t) (3 71 ) Moment Of Inertia About Axis Of Rotation Equation (3.71) can be reorganized into the form of the following equation. Umax f(At 6ref = ' tf ' t} (3.72) In this equation, Umax is a design parameter that is determined by the characteristics of the actuator used to drive the manipulators. For this application, all the the actuators are the a variable. I, same geared motor. the linearized f(At,tf,t) mass moment of can be modified by varying inertia, is determined by the mass characteristics of the system and does not vary with time or changing geometry. Given a maximum torque Umax, and an input shaping function with suitable boundary conditions, the reference position and velocities can be determined by integrating the following equations piece wise of the time interval determined by the maximum torque. OreKO = 6 + ^ax f(At,t f ,T) dx (3.73) ./to OreKt) = 6 + Go (t-to) + Sm 33 f(At,t f ,x 2 ) dx 2 dX] (3.74) where t t are the = = 6(0) = Go e(t f) tf boundary conditions = 6f at the start setting t = tf 1- i- a+ 2 L4 G(tf) = The Appendix B. complete the maneuver can be obtained by to below and obtained from Appendix B. in the equation presented ef =smax = and completion of the maneuver. resulting reference equations are presented in The near-minimum-time 6(0) -J- a2 t 2 f (3.75) 10 I By doing and the this, maximum ^ = a relationship between the time to complete the torque available can be determined for an ,2/1 18 U this application, the l a+ a+ Xa 2 10 a76) {X,,) 2) ) time derived with the above equations represents the near-minimum-time required E. link. J Wtt-a maXl For i maneuver to perform the maneuver for only one joint. ANALYTICAL MODEL SUMMARY This research project analyzes how a two link manipulator can be controlled from a spacecraft main body while minimizing the upon the main body the links and diagrams. effect of manipulator In general one can calculate the interaction force main body by using the Newtonian approach with Based on this analysis, the more smoothly performed, the smaller the interactive force will be. were developed. In the first controller, linear 34 Two that the motion between free body maneuver is basic type of controllers feedback of position and velocity information from the joints was used to control the endpoint position. In the second type of controller, position and velocity information are used in conjunction with a reference trajectory to control endpoint position. For this type of controller, reference trajectory. In the two two first, different schemes were used to generate the a fifth order polynomial in conjunction with the link closed loop solution for the manipulator inverse kinematics and the Jacobian were used to generate the reference trajectory. minimum-time torque shaping technique was used trajectory. modeled to In the second, a near- determine the reference All components, including the manipulator and main body, are as rigid bodies. 35 RESULTS AND DISCUSSION IV. Analytical results from the simulation are presented in this section. different techniques manipulator. were used to control Three and stabilize the position of the Results from the linear feedback controller with constant gains, polynomial reference tracking controller and near-minimum-time reference tracking controllers are presented sequentially. A. REFERENCE MANEUVER The three most import criteria with respect to this research are the end position, joint torques, and the effect of manipulator main body. motion upon the main body on the spacecraft or the communications is critical effect controllers, a standard reference an the servo- effect of the manipulator to the excitation of flexible structures upon antenna pointing accuracy compare to maintain a the performance of the three different maneuver was developed. For desired rotation of the main body move from due The if for link. able to effectively inertial is critical Joint torques are important to verify actuators can perform the desired maneuvers. from an attitude of the Final position for the endpoint of the manipulator performing assembly tasks. To be motion on the tip final was zero. Link angle of 20 degrees to 40 degrees. initial inertial This reference maneuver is 1 this maneuver, the was programmed to move Link 2 was programmed to angle of 40 degrees to a final position of 60 degrees. depicted in Figure 4.1 36 Reference N2 Maneuver A Final Position k2 Polynomial w w Jm s-O S^Y S / Initial Position $ W$f Linkl )egfees e 3 = 40 Degrees Momentum Wheel 82 = 20 Degrees 8 1| ~ Degrees N, Main Body N- Figure 4.1 Reference Maneuver B. LINEAR CONSTANT GAIN FEEDBACK CONTROLLER The theory behind in more detail in Chapter velocity gains of 5 G v = 0.2 The ) the linear constant gain feedback, controller ( Gv = III and [Ref. 1,15]. 5 were used for these plots. ) plots are presented in Appendix linear feedback controller the easiest to implement. was D Position gains of 1 Small gain ( ( is described Gp = Gp = 1 0.1 )and and for comparison. the simplest controller conceptually and This controller provided stable control over a wide 37 range of gains and for a wide variety of maneuvers. mass or inertia characteristics was required and velocity gains, and beginning and No knowledge for this controller. final of the system Only the position arm positions were required to calculate the required control torques to perform the maneuver. The joint time history for this case velocity time history is is presented in Figure 4.2 and the joint Performance of presented in Figure 4.3. this control system was directly related to gain selection. The system was relatively sensitive to small the changes in the position and velocity gains. This resulted maneuver time and control system damping. The system was gains and maneuvers evaluated and it to optimize was no way manipulator and the stable for all Even with system stability not really to systematically select position and velocity gains maneuver time or torques The magnitude of changes to could be proven by Lyapunov stability theory that the system was globally stable. in question, there in to achieve desired performance measures. the gains directly impacted the performance of the stability of the main body. Larger gains generated larger control torques and reduced the time required to perform the maneuver. damping of this the system could also be varied with the gain selection. movement on the main body was small but nearly 2 degrees. Although this still cause degradation in is still The The effect of resulted in a displacement of apparently a small displacement, 2 degrees communication with the satellite due may to antenna pointing errors. Time history plots for torques and Figures 4.4 and 4.5 respectively. was characterized by momentum wheel The response of this speed are presented system in to the controller a very rapid increase in torque immediately after initiation of 38 ~ 70 60 1 M 50 40 j.-'"" - 30 20 — -— s Main Body Angular Displacement Joint 10 1 Angular Displacement Joint 2 Angular Displacement i -10 1 1 10 8 Time 14 12 16 18 20 (Sec) Figure 4.2 Linear Constant Gain Feedback Joint Position Time History With G p =l and G v =5 Ai\ i I o sv | •- Main Body An gular -- \ Velocity '\ , ~ Link l-\ - > - - * 1 Angular velocity Link 2 Angular vciutiiy ... < \ c ^'' \/ " ^**'V **ic_ — . 10 15 Time 20 25 30 (Sec) Figure 4.3 Linear Constant Gain Feedback Joint Velocity Time History With G p =l and G v =5 39 1 — 0.7 , i 0.6 —— 0.5 — - Main Body Torque — B 0.4 « 9 0.3 l^lll* 1 1 UlljUk. CT i 1 £ 1 0.2 1 - 0.1 - ",\r -0.1 i Time 30 25 20 15 10 (Sec) Figure 4.4 Linear Constant Gain Feedback Torque Time History With and G p =l G v =5 980 10 20 15 Time 25 30 35 40 (Sec) Figure 4.5 Linear Constant Gain Feedback Momentum Wheel Speed Time History With G p =l and G v =5 40 the was maneuver followed by a rapid decrease after a similar to an impulse input that commanded motion few seconds. This response became more pronounced with increased or with increased gains. Large gains are desirable to achieve acceptable levels of performance but must be balanced against actuator characteristics , magnitude of the maneuver, and commanded large and sudden increase in A structural considerations.. torque could saturate the actuator or excite flexible appendages attached to the spacecraft. The large initial torques, depicted in Figures 4.4 and 4.5, correspond to momentum rapid changes in the speed of the required during the initial portion of the wheel. maneuver Large speed changes are to generate large toques to counteract the forces generated by the manipulator. experience with the actuator and FSS and power supply, analysis of electrical it is Based on previous power requirements of the reasonable that the actuator will be able to compensate for the torques generated by the movement of the manipulator. C. POLYNOMIAL REFERENCE TRACKING CONTROLLER To rectify the problems encountered with the simple linear constant gain feedback controller a reference tracking controller was implemented. previous case, there was a large to correct. initial error for In the which the system was attempting In this controller, a smooth, fifth order polynomial was used to generate a reference trajectory so that the correction could be evenly partitioned over a specified maneuver time. With position, velocity, and acceleration As this information, reference torques, were obtained as a angular smooth function of time. a result the initial "jump" generated by the linear feedback controller eliminated and the manipulator was able to track a smooth path from 41 was initial to final conditions. A Lyapunov variation of the stable control law used in the previous controller was used with this reference trajectory controller. With this controller, the specified but was subject to the characteristics of the actuators and the task being Two maneuver performed. time required to perform the maneuver could be times were analyzed. perform the maneuver was fixed at 2.5 determine joint torques, velocities, and near the torque limit of the actuators. In the first case, the time to seconds. This time was selected to momentum wheel speeds while operating In the second case, a time of 5 seconds used to determine the effect of maneuver time on the actuators. These plots for the 5 second maneuver time are included in Appendix D to demonstrate the effect of maneuver time upon joint torques and velocities. Position and velocity time history of the manipulator for a maneuver time of 2.5 seconds are presented in Figure 4.6. and 4.7 respectively. momentum wheel Torque and speed plots for a maneuver time of 2.5 seconds are presented in Figures 4.8 and 4.9 respectively. Movement of the manipulator did not cause measurable disturbance to the main body. Manipulator torques were smoothly was able to counter these torques effectively The motion by the main body. motion is desirable since it applied, and the control system and in a timely manner, negating stability of the main body during manipulator will reduce pointing errors for unstablized imaging devices or high gain antenna communications systems. The polynomial reference controller demonstrated stable operation over a wide variety of operating conditions and feedback gains. The magnitude of the gains had little or no effect upon the maneuver time since specified in the reference maneuver. Commanded 42 this characteristic torques closely tracked the was 70 60 50 40 E —a. 30 5 - "" * J MainB od> ..--'"" .2 a - _„. Angular Displac ernent 20 ' Joint Angular Displacement 1 10 ~~ Joint 2 Anj ular Displacement -10 0.5 1.5 1 Time 2.5 (Sec) Figure 4.6 Polynomial Reference Tracking Controller Joint Position Time History With Gp=l, G v =5 And Maneuver Time Of 2.5 Seconds 18 —""*. „ * * "*-. 16 X >/ 14 * » \. 4 12 % \ 10 , / \ / 8 * \. ', 6 L. / 9 5 4 city c < i w 2 Link 2 Angular Velo city ,' ^r \ •''' -2 0.5 1.5 1 Time 2.5 (Sec) Figure 4.7 Polynomial Reference Tracking Controller Joint Velocity Time History With G p =l, G v =5 And Maneuver Time Of 2.5 Seconds 43 I 0.8 - Main B ody Torque 0.6 /•''' .- --.. ? 0.2 \ *** ' 0.4 !"*, X* - Link \ Link 2 Torque ""- v ^^** er © H rorque 1 ^ ^l % m .. -0.2 \x s>... -0.4 -0.6 / x • * ** *y / / x -0.8 -1 - C 1 0.5 1.5 1 Time 2 2. 5 (Sec) Figure 4.8 Polynomial Reference Tracking Controller Torque Time History With G p =l, G v =5 And Maneuver Time Of 2.5 Seconds 10R0 i icnn ^— . * 1 980- 1a. 930 1 C/3 J! Rsn i Rin 1 . i 7Rn . () 0.5 1.5 1 Time :I 2.5 (Sec) Figure 4.9 Polynomial Reference Tracking Controller Momentum Wheel Speed Time History With Gp=l, G v =5 And Maneuver Time Of 2.5 Seconds 44 reference torques with no discernible difference between the reference and commanded The polynomial reference tracking torques. smooth position control commands controller, there With the tracking for all maneuvers. were no discontinuities or sudden changes The performance was independent of controller provided the in torque commands. magnitude of the commanded maneuver Maneuver time could be with no effect of gain selection being noted. but was subject to actuator and structural characteristics. specified Feedback errors remained small throughout the maneuver as the controller smoothly tracked the reference. This resulted in smoothly changing torque commands with smooth and predictable motion of the manipulator. D. NEAR-MINIMUM-TIME REFERENCE TRACKING CONTROLLER The theory behind Chapter case, III a was the and [Ref. 1,2]. near-minimum-time tracking controller Two cases set equal to 0.25. for this controller were a the maneuver time. this reference are a approaches wave with a period of of 0.1 was used. zero, the input torque shape approached that of a square As This shape more closely approximated the bang-bang controller. A maneuver times of described in selected. In the first The shape function generated by sinusoidal in shape. In the second case, an is 2.5 seconds minimum was used so time that the polynomial reference and the near-minimum-time control system performance characteristics could be compared. Similar plots with a maneuver time of 5 seconds can be found in Appendix D. The general performance of controller provided some of previous control systems. the near-minimum-time reference tracking the advantages and disadvantages of the two Since this was a tracking controller and the feedback 45 errors were generally small, the commanded torques were generally smooth with only minor disturbances to the main body. Near minimum-time reference position and velocity time history of the manipulator for a maneuver time of 2.5 seconds with Figure 4.10. and 4.11 respectively. time of 2.5 seconds with respectively. a = Momentum wheel a = 0.25 are presented in speed plots for a maneuver 0.25 are presented in Figures 4.12 and 4.13 Near minimum-time reference position and velocity time history of the manipulator for a maneuver time of 2.5 seconds with Figure 4.14. and 4.15 respectively. time of 2.5 seconds with a = Momentum wheel 0.1 a= 0.1 are presented in speed plots for a maneuver are presented in Figures 4.16 and 4.17 respectively. With a = 0.25 the input torque shaping closely resembles that of the polynomial reference trajectory tracking controller. reference trajectory, the manipulator links final conditions. Note As with moved smoothly from the polynomial the initial to the that for this controller, there is only a very small angular displacement of the main body. Joint velocities for the manipulator links follow a ending with zero velocities. controller, there is links. With smooth path, beginning and this controller, note that unlike the previous no phase difference between the movement of the manipulator Both links move with the same velocity also displays a very small angular velocity. 46 at the same time. The main body 70 T , 60 ^ -a 4) 50 a 1 c E V u A ' 40 i 30 Q. Main Body Angular „ ••••"" .2 Q Displacement 20 "" 1 Angular Displacement Joint 2 Angular Displacement Joint 10 , i -10 0.5 2.5 1.5 Time (Sec) Figure 4.10 Near-Minimum Time Tracking Controller Joint Position Time History With Gp=l and G v =5, Maneuver Time Of 2.5 Seconds , And a = 0.25 18 ~i 16 14 1 s }» 6- io »>» |8 > ir Link I Velocity Angular ve locity -/ 5 c < Link 2 Angular ve Iocity — ^r^Z. 0.5 — ** 1.5 1 Time 2.5 (Sec) Figure 4.11 Near-Minimum Time Tracking Controller Joint Velocity Time History With G =l and G v =5, Maneuver Time Of 2.5 Seconds , And a = 0.25. p 47 1.5 _ r 1 Main Bo dy Torque / ^ - ,*• -... x i Link T arque 1 0.5 E Z >i^*' ^a — -'"" *^» Link 2 Torque **V ..^--' ~^r cr E H -0.5 >v *"».. ." ,. / -1 .,5c > 0.5 1.5 1 • Time 2 2.5 (Sec) Figure 4.12 Near-Minimum Time Tracking Controller Torque Time History With G p =l and G v =5, Maneuver Time Of 2.5 Seconds And a = 0.25 , 1100 1050 ~ 1000 E 950 22 9oo 850 800 750 0.5 1.5 1 Time 2.5 (Sec) Figure 4.13 Near-Minimum Time Tracking Controller Wheel Speed Time History With G =l and G v =5, Maneuver Time Of 2.5 Seconds , And a = 0.25 p 48 70 60 • I Q 50 a 40 „„.--*" .J - a Q Main B ody Angular ...—--*"*"" .2 Displac ement 20 Angular Displacement Ia io 4 Joint 2 Angular uispiacemem 10 0.5 2.5 1.5 1 Time (Sec) Figure 4.14 Near-Minimum Time Tracking Controller Joint Position Time History With G =l and G v =5, Maneuver Time Of 2.5 Seconds And a = 0.1 p 18 , - ! 16 • 14- §« B 1 "5 > io - — 8 Main Body Angular Velocity 6 J is IB < Link 1 Angular Velocity 4 Link 2 Angular Velocity 2 -2 - 0.5 () 1.5 1 Time 2 5 2 (Sec) Figure 4.15 Near-Minimum Time Tracking Controller Joint Velocity Time History With G =l and G v =5, Maneuver Time Of 2.5 Seconds And a = 0.1 p , 49 1 0.8 _ , . — _ / __ v 0.6 / Main Body Torque 0.4 ? 2 "v /'/ > t Link if 0.2 " **« *f * 1 Torqu e Link 2 Torqu e L_ V S 3 V \ 3-0.2 1 -0.4 s -0.6 1 \ -0.8 0.5 2.5 1.5 1 Time (Sec) Figure 4.16 Near-Minimum Time Tracking Controller Torque Time History With Gp=l and Gy=5, Maneuver Time Of 2.5 Seconds And a = 0.1 , 1 inn insn . L^»~ i —''"'^ (RPM) 1 1a Wheel 950 8 RSfl i 800 - () 0.5 1.5 Time :\ 2.5 (Sec) Figure 4.17 Near-Minimum Time Tracking Controller Wheel Speed Time History With G p =l and G v =5, Maneuver Time Of 2.5 Seconds , And a = 0.1 50 As more a square. to track goes toward zero, the shape of the input torque reference becomes The square corners of the torque input are difficult for the controller and required larger gains. This characteristic depicted plots with required. Even with a = 0.1. Note large gains, the that rapid to the optical As best noted in the previous changes main body was The jerky motion of the manipulator disturbed but detectable position changes. is the in the still disturbed by motions. main body and resulted in the previous controllers, main body can degrade communications imaging devices. 51 wheel speed are in small any disturbance links or cause pointing errors for V. CONCLUSIONS Three different control systems were simulated and analyzed. controllers were stable effective manner. out to be how and were able The most to position the significant difference effective the control system was All three manipulator in a timely and between the controllers turned stabilizing the main body while positioning the manipulator. The linear feedback controller the easiest to implement. was the simplest controller conceptually and This controller provided stable control over a wide range of gains and for a wide variety of maneuvers. Large gains were required to achieve acceptable levels of performance. Large gains corresponded torques and large displacements of the main body. had the to achieve all the control system was implemented, it was not possible to of these objectives for the given reference maneuver. The polynomial reference two Position and velocity gains be selected to balance control torques, system performance, and motion of main body. As classic to large trajectory for robotic applications represents a approach for generating a reference trajectory to position and control a link manipulator. This approach also offers the advantage that the reference trajectory can be pre-calculated off line prior to the maneuver. These pre- calculated values for the reference trajectory can be used in conjunction with the controller to minimize real time computational requirements. The polynomial reference tracking control commands for all maneuvers. controller provided With the tracking discontinuities or sudden changes in torque smooth position controller, there were no commands. The performance was independent of the magnitude of the commanded maneuver and only slightly 52 Maneuver time could be affected by gain selection. actuator and structural specified but was subject Feedback errors remained small characteristics. throughout the maneuver as the controller smoothly tracked the reference. resulted in smoothly changing torque to commands This with smooth and predictable motion of the manipulator. The polynomial reference tracking controller was the most effective controller implemented with respect and timely to accurate positioning of the manipulator, and stabilization of the main spacecraft body. The general performance of controller provided some of the near-minimum-time reference tracking the advantages and two previous control systems. some of the disadvantages of the Since this was a tracking controller and the feedback errors were generally small, the commanded torques were generally smooth with only minor disturbances The near-minimum-time maximum to the main body. for the reference maneuver was determined by torque capability of each actuator and the magnitude of the maneuver. The maximum of specified as the the three times calculated for each of the three joints maneuver time. For each maneuver, one joint the limiting case for the selected would operate toque capability in order to complete the maneuver at less at the than the same time Two of maximum as the other completed the maneuver. Large gains were required to force the controller to closely track the reference input as the input torque shape approached the time. was would then become maneuver based on these requirements. the three remaining actuators therefore links the Even with larger gains, the controller torques generated by the manipulator and were observed. 53 was not able some small to minimum maneuver completely negate the rotations of the main body Two different control laws linear feedback was utilized. were evaluated. In the first, constant gains with This controller proved to be the easiest to implement but did not effectively stabilize the main body during manipulator. The second successful controller way two different references to generate a reference trajectory. upon mass and controller represents a traditional and of positioning the manipulator. complex than the the control law entailed using a tracking controller with The reference tracking constant gains. movements by With the reference tracking were used. In the first a polynomial was used This technique proved to be only slightly more linear feedback controller with constant gains, inertia characteristics of the was dependent manipulator and was the most effective at positioning the manipulator while minimizing the motion of the main body. In the second, a near-minimum- time technique was employed to generate a reference trajectory. This reference was more complex, but provided the capability to position the manipulator in near-minimum-time. This technique provided greater flexibility in shaping the input torque reference but was not as successful as the polynomial reference tracking controller in stabilizing the main body. In the end, the polynomial reference tracking controller provided the best performance of the three controllers simulated. A. SUBJECTS FOR FUTURE RESEARCH Lyapunov theory represents only one methodology system design. Many for nonlinear control other methodologies including neural networks, adaptive controllers, sliding controllers, and robust controllers among others provide additional areas for research. With the addition of the vision server to the Spacecraft Robotics Simulation Lab, the capability to track a target as well as the endpoint of the manipulator will 54 become tasks. available. Once This provides the capabilty to perform end point tracking this is successful with a stationary target, the same can be repeated with a moving target. The design of the manipulators allows for the replacement of the rigid manipulator arm design with a flexible arm design. Control system design for accurately and quickly positioning the flexible manipulator provides another area for future research. The minimum time joint complete the maneuver was the longest of the three to maneuver times and was based on the maximum actuator and the magnitude of the maneuver. torque capabilities of the With the near-minimum-time reference tracking controller, the switching time for the maneuver was specified to be one half of the maneuver time. This methodology resulted the three actuators working minimum-time equations at maximum in another torque. way, it may By This may only one of parameterizing the near- be possible to optimize the solution in terms of a variable switching time and variable of the actuators. in maneuver time for each allow optimization of the torques or time required to perform the maneuver, maneuver time or torque requirements. 55 APPENDIX A TABLE Characteristic A.l Servo Motor Characteristics Units Manipulator Momentum Actuator Wheel Actuator 9FGHD Model Number Gear Reduction Ratio 147.51 : JR16M4CH-1 1 1:1.0 rev per minute 17 3000 Rated Torque inch-pound 80 31.85 Rated Current amps 5.6 7.79 Rated Voltage volts 12 168 Peak Torque inch-pound 119 341.43 Peak Current amps 62 79.3 Peak Voltage volts 13.2 7.67 Outside Diameter inches 4.75 7.4 Height inches 3.1 4.5 Weight pounds 3.2 17.5 oz-in/amp 3.23 69.01 volts/krpm 2.39 51 Terminal Resistance ohms 0.95 1.4 Average Friction Torque oz-in 2.5 11 Viscous Damping Constant oz-in/krpm 0.3 7.84 Moment Of Inertia oz-in/sec A 2 0.0052 0.084 Rated Speed Torque Constant Back EMF Constant 56 APPENDIX B A. SYSTEM KINETIC ENERGY 1. Body 1 Kinetic Energy ti =^ii 2. 2 e, (B.i) Body 2 Kinetic Energy t2 = £ 2 ic.m. 2 + e2 m 2 L! L m + a- m 2 L?. m + c m2 1- . , . 2 e! 2 l? e 2 9 2 cos(e 2 i) (B.2) 2 2 2 3. Body 3 Kinetic Energy t3 = 1 m + m + 4. 2 ic.m.3 3 3 03 + i m 3 [Lf 2 > er + . U ne 2 t 2 ~ 2 L 2 0i e2cos(02i)+m3L3Lc. m L 2 L c m 6 2 9 3 cos (e 32 Lj . .3 . + . 3 um - t 2 . 3 2l e 3 ~J ei e 3 cos(e 3 (B.3) i) ) Total Kinetic Energy 3 T=X (B.4) Ti i=l B. EQUATIONS OF MOTION 1. Mass Matrix The mass matrix is a function of the mass and system as well as the geometry of the system. inertia characteristics of the The mass matrix is defined in equation (B.5) where the elements of the mass matrix are described as follows in equations (B.6) through (B.I 1). 57 mnmi 2 mi3 m 22 m 23 m m 32 m 33 M(9) = m2i (B.5) 31 where mil = Hl22 = Ii + L? Icm.2 + m2 + rm] + [mi "Cjn.2 + L2 m2 L2 m33 = + Icm.3 mn =m2i = mn = m3 i Lcm.3 Coriolis And L4 (B.6) + m3 (B.7) > "^ Li L2 cos(0 2 i) (B.8) m2 Lcjn. 2 j j + m3 (B.9) = Li Lc m3 cos(0 3 i) m3 (B.10) m (B.ll) m23=m3 2= L2 Lcm.3 cos(6 32 2. 1114 ) 3 Centrifugal Acceleration Terms: G(l) G(9,9) = G(2) (B.12) G(3) G(l)=[(m 2 LiL c m )+(m 3 LiL 2 )]sin(G 2 i)e 2 + . . 2 (B.13) (m 3 LiL c m 3)sin(e 3 i)e 3 . . G(2)=-[(m 2 LiL c m 2)+(m 3 LiL 2 )]sin(e 2 i)ei . . + (B.14) (m 3 L 2 L c m )sin(e 3 i)e 3 . G(3) = - (m 3 Li Lc m . . 3) . 3 sin (0 2 i) 6? 58 - (m 3 L 2 L c m . . 3) sin (e 32 ) e2 (B.15) C. POLYNOMIAL REFERENCE TRAJECTORY 1. Vector Polynomial Describing Tip Position: F=F(to) + t (B.16) f(t)[F(tf)-?(to)] = Ll1o ,o<t<tf,0<i<l tf-to f(T) 2. = C] +C2T + C3T 2 + C4 T 3 + C5T 4 + C6T 5 Boundary Conditions And Polynomials f(0) = 0,ftO) = 0,f(0) = f(l)=l,f(l) = 0,f(l) = f(T)=10x 3 -15x 4 + 6T 5 3. f(T) = 30T 2 -60x 3 + 30T 4 f(x) = 60 x - 1 80 T 2 + 1 (B.18) (B.19) 20 T 3 (B.20) Vector Position r(T) 4. (B.17) = r(t ) + r f(T)[i (tf)-r(to)] (B.21) Vector Velocity F (t)= f( x) (lM^lM) I tf-to (B .22) I 59 1 Vector Acceleration 5. r(T) = f(T) (F(tf)-r(to)l (B.23) 2 (t f - to) D. TWO LINK INVERSE KINEMATICS Law Of Sines & Cosines: xl + yl=ii + 021 =82-81 i2 + 2i i2 1 cos(e 2 -ei) (B.24) (B.25) (B.26) sin (e 2 i ) = ± Vl-cos(8 2 i) 2 (B.27) 821=^2(^21)^ \C0S18 2 1 (B.28) ); Velocity Vector: r = [H] 9 - H= (B.29) 2 sin (62), - b sin (0 3 ) I3 cos (B.30) 12 cos (9 2 ), (63) Acceleration Vector: r=[H] e +[h] e (B.31) 60 H= E. - 12 cos - 1 2 sin (0 2 ), b - (62), cos (83) (B.32) sin (0 3 ) - 13 NEAR MINIMUM TIME RIGID BODY MANEUVER 1. Near-Minimum-Time Maneuver I 6ref = U re f = U max tf - 2. Maneuver Time t] = ts - At t2 = ts + At = tf - At At = a tf = = = tg (B.33) Moment Of Inertia About Axis Of Rotation I - t3 f(At,tf,t) (3 tf Rise Time = Switching Time, where p 0.5 Torque Shaping Function 3-2-1 / -te = f (At, t f,t) = 1-2 (LlL = W 2At l\ OreKO = Go + ^ I t < 3.2M1 t < At \ tj for t, < t < t2 l2At I -1+ i^a \ for At < 1 \2 = .= < for \At/J - 2 1 for t2 < t 3-2 t-t3 ) (B.34) < t3 for t3 < t < tf At At f(At,tf,T) dT 61 (B.35) GreKt) = 6 + Go ^ + (t-to) f(At,t f ,T 2 ) dx 2 dii (B.36) Boundary Conditions 3. to t = = =e e(o) 0; e tf ; (tf) = ef e(o) =o e(tf) =o Solution for the piece wise integration of equations (B.35) and (B.36) for the given time interval provides us with refeence angular displacements and angular velocity. Reference Angular Displacements 4. < t Velocities < At lllflillf 2 e ref= Umax At \AV L4 3 Umax t < t \ LVAti I At < And _ U (B.37) 10 VAt ^ 4 t (B.38) 2 \At ti 6 ref = Mmax J-t 2 e re f = 2 -ItAt + 2 ^-(At) v ; 5 (B.39) 20 ^(t-iAt (B.40) 62 1 tl < t < 1 t2 a+l 23<x2.3 L20 4 =^x e rPf 1 (2Atf t-ti .2 i f f 1 2At 1 . 2 / + t? 8 4 t-ti f 1 + 1 t-ti 2At ^ 5 + f | 5 / (ltf -|At)(t- (B.41) 2At I tl ) I e ref = ymax I t2 <t< L _lAt + 2At t-til 2Ati 2 I + (LLti 2 (t>iLf 2At 1 / (B.42) 2At \ t3 2 tf Qref = 20 - max 4 = %bsl -t 1 (tf 3At) - (t - 2) (B.43) i(t-^-At 'i ref + 8 + ti + -i- t2 - At (B.44) 2 t3 <t< tf 1 -J-a 2 U e ref = 2m& r 20 L 5fflas.(. i- a+ 2 t^lAtft-tsHAtf 4J (B.45) . e raf = - 1 2 t-t 3 f V t-t 3 f+1 ( At lAt + 4 ' ' At 4 _ j ' J_ t-t 3 ^ 5 ( 10 I At At (B.46) \ At / 63 \ At / 2 I 2At I o ; At time t = tf u max Qref = J- - L4 1a+ -i- 2 10 a2 t (B.47) . e ref = 5. 2 tf (B.48) Near-Minimum-Time Relationships tfi "iHe = fi -e J" Um -(4"2 a+ Umax; — (B.49) a2 io i,(e fl -eo,) ^4-2 a+ m ) (B.50) a: 64 APPENDIX C A. LINEAR FEEDBACK MATLAB SIMULATION PROGRAM 1. Main Program For Linear Feedback Simulation % % Constants Lenght of Manipulators L(l)= 15*2.54/100; L(2) = 17*2.54/100; L(3) = 17*2.54/100; L(4) = 20/100; % Distance From Axis Of Rotation To Center Of Mass Lcm(l) = 0/100; Lcm(2) = 36.45/100; Lcm(3) = 34.9/100; % Mass Of Major Components (kg) m(l) = 54.69; m(2) = 2.09364; m(3) = 2.466; m(4) = 10.667; % Inertias Of Major Components (kg-m A 2) 1(1) = 4.32132; 1(2) = 3.20338e-2; 1(3) = 5.38398e-2; 1(4) = 0.0912; 65 % % % t0 Integrate Equations Matlab Runge-Kutta 4 Routine Boundary Conditions = 0; tfinal xO = tol Of Motion Using Commercial = = 40; [0.0; 0.0; 0.0; 0*pi/180; 20*pi/180; 40*pi/180]; le-8; trace=l; Call "lfbrk" Function [t,x,uu] % And Integrate Equations Of Motion = rku2('lfbrk',t0,tfinal,x0,tol, trace); Program To Calculate Motor Torque, Current, and Voltage TF=1.41e-2; %N-m KT=2.28e-2; % N-m/amp % N-m/rad/sec % Volt/rad/sec % ohms KD=2.00573e-5; KE=2.28271e-2; RT=0.95; j=length(t) fori=l:j; amp22(i) = (uu(i,2)/148.51 + volt22(i) = KE * x(i,2) + RT amp33(i) = (uu(i,3)/148.51 + volt33(i) = KE * x(i,3) + RT TF + KD * x(i,2)* 148.51 ) / KT; ) / KT; * amp22(i); TF + KD * x(i,3)* 148.51 * amp33(i); end 66 % % Program To Calculate Motor Torque, Current, and Voltage For Momentum Wheel TF=0.0777; %N-m/amp KD=5.29131e-4; % N-m/rad/sec KE=0.487 11; % Volt/rad/sec RT=1.4; %ohms Iw=0.0912; % kg-mA 2 % rad/sec (= thd0=104.7; lOOOrpm) fori=l:j; tor(i) = uu(i,l); thddw(i) thdw(i) = tor(i)/Iw; = thddw(i) amp(i) = volt(i) = KE (tor(i) * t(i) + + TF + KD * thdw(i) thdO; * thdw(i) + RT * amp(i); xl(i)=L(l)*cos(x(i,4)); yl(i)=L(l)*sin(x(i,4)); x2(i)=xl(i)+L(2)*cos(x(i,5)); y2(i)=yl(i)+L(2)*sin(x(i,5)); x3(i)=x2(i)+L(3)*cos(x(i,6)); y3(i)=y2(i)+L(3)*sin(x(i,6)); end % Store Data For Plotting Later datal=[t,x*180/pi,uu]; , , , , data2=[t,xl ,yl ,x2 ,y2 ,x3 ) / , ,y3']; data3=[t,thdw'*30/pi,thddw'*30/pi]; 67 KT; , , data4=[t,amp ,amp22',amp33',volt ,volt22',volt33']; % Save Data In Text Format save lfb9.dat datal /ascii save lfbl0.dat data2 /ascii save lfbl 1 .dat data3 /ascii save lfbl2.dat data4 /ascii 68 2. Linear Feedback Equation % Function Containing Linear Feedback Equations function [xdot,Ul] % % Of Motion Function = Of Motion lfbrk(t,x) Constants Lenght of Manipulators L(l) = 15*2.54/100; L(2) = 17*2.54/100; L(3)= 17*2.54/100; L(4) = 20/100; % Distance From Axis Of Rotation To Center Of Mass Lcm(l) = 0/100; Lcm(2) = 36.45/100; Lcm(3) = 34.9/100; % Mass Of Major Components (kg) m(l) = 54.69; m(2) = 2.09364; m(3) = 2.466; m(4) = 10.667; % Inertias Of Major Components (kg-m A 2) 1(1) = 4.32132; 1(2) = 3.20338e-2; 1(3) = 5.38398e-2; 1(4) = 0.0912; 69 % Angles thd(l) = x(l); thd(2) = x(2); thd(3) = x(3); % Convert Input Variables Into Variable Used In This Function th(l) =x(4); th(2) =x(5); th(3) =x(6); % Final Conditions For Manipulator thfl=0; thf2=40*pi/180; thf3=60*pi/180; thdfl=0; thdf2=0; thdf3=0; % Linear Feedback Control Law gip=i; g2p=.i; g3p=.l; glv=.2; g2v=.2; g3v=.2; 70 And Main Body % Control Torques U(3)=-g3p*(th(3)-thf3)-g3v*(thd(3)-thdf3); U(2)=U(3)-g2p*(th(2)-thf2)-g2v*(thd(2)-thdf2); U(l)=U(2)-glp*(th(l)-thfl)-glv*(thd(l)-thdfl); U1=U'; % Calculate Coeffients For The Equation Of Motion And [MM,GM] = mgm(th,thd); B = [l,-l,0;0,l,-l;0,0,l]; thdd=inv(MM)*(B*Ul-GM*); xdot=[thdd;x(l);x(2);x(3)]; 71 Integrate B. POLYNOMIAL REFERENCE TRACKING CONTROLLER 1. Polynomial Reference Maneuver Tracker Main Program % EOM3.m Program % Constants % Lenght of Manipulators L(l)= 15*2.54/100; L(2) = 17*2.54/100; L(3) = 17*2.54/100; L(4) = 20/100; % Distance From Axis Of Rotation To Center Of Mass Lcm(l) = 0/100; Lcm(2) = 36.45/100; Lcm(3) = 34.9/100; % Mass Of Major Components (kg) m(l) = 54.69; m(2) = 2.09364; m(3) = 2.466; m(4) = 10.667; % Inertias Of Major Components (kg-m A 2) 1(1) = 4.32132; 1(2) = 3.20338e-2; 1(3) = 5.38398e-2; 1(4) = 0.0912; 72 ; % t0 Boundary Conditions And Integration Time = 0; tfinal xO = tol = = 10; [0.0; 0.0; 0.0; 0*pi/180; 20*pi/180; 40*pi/180]; le-8; trace=l % Integrate Equations [t,x,uu] Of Motion = rku2('eom3rk',tO,tfinal,xO,tol,trace); j=size(t); % Calculate Electrical Power Requirement For Manipulator Actuators % Motor Parameters TF=1.8e-2 %N-m KT=2.28e-2 % N-m/amp % N-m/rad/sec % Volt/rad/sec KD=2.00573e-5 KE=2.2827 le-2 % ohms RT=0.95 j=size(t) for i=l:j; amp22(i) = (uu(2,i)/148.51 + volt22(i) = KE * x(i,2) + RT amp33(i) = (uu(3,i)/148.51 + volt33(i) = KE * x(i,3) + RT TF + KD * x(i,2) ) / KT; ) / KT; * amp22(i); TF + KD * x(i,3) * amp33(i); end % % Program To Calculate Motor Torque, Current, and Voltage Momentum Wheel 73 % Motor Parameters TF=0.0777 KT=0.48732 KD=5.29131e-4 % N-m % N-m/amp % N-m/rad/sec % Volt/rad/sec % ohms KE=0.48711 RT=1.4 % kg-m A 2 % rad/sec (= lOOOrpm) Iw=0.0912 thd0=104.7 j=size(t) for i=l:j; tor(i) = uu(l,i); thddw(i) thdw(i) = tor(i)/Iw; = thddw(i) amp(i) = (tor(i) volt(i) = KE * t(i) + + TF + KD * thdw(i) thdO; * thdw(i) + RT ) / * amp(i); end 74 KT; 2. Polynomial Reference Tracking Equations Of Motion Function function [xdot,Ul] =eom3rk(t,x) % Constants L(l)= 15*2.54/100; L(2)= 17*2.54/100 L(3) = 17*2.54/100 L(4) = 20/100; Lcm(l) = 0/100; Lcm(2) = 36.45/100; Lcm(3) = 34.9/100; % Mass m(l) = 54.69; m(2) = 2.09364; m(3) = 2.466; m(4) = 10.667; % Inertia 1(1) = 4.32132; 1(2) = 3.20338e-2; 1(3) = 5.38398e-2; 1(4) = 0.0912; % Angles thd(l) = x(l); thd(2) = x(2); thd(3) = x(3); th(l) =x(4); 75 th(2) =x(5); th(3) =x(6); [MM,GM] = mgm(th.thd); % Gains glp=1.0; g2p=1.0; g3p=1.0; glv=5.0; g2v=5.0; g3v='5.0; [Uref,thref,thdref,thddref] = eom3ref(t,L,Lcm,m,I); dU(3)=-g3p*(th(3)-thref(3))-g3v*(thd(3)-thdref(3)); dU(2)=dU(3)-g2p*(th(2)-thref(2))-g2v*(thd(2)-thdref(2)); dU(l)=dU(2)-glp*(th(l)-thref(l))-glv*(thd(l)-thdref(l)); Ul=Uref+dU'; U=U1'; B = [1,-1,0;0,1,-1;0,0,1]; , thdd=inv(MM)*(B*U -GM'); xdot=[thdd;x(l);x(2);x(3)]; 76 3. Polynomial Reference Trajectory Function = eom3ref(t,L,Lcm,m,I); function [Uref,thref,thdref,thddref] thref(l)=0; % t0 tf Initial = = % And Final And Final Vector Positons Time For Maneuver 0; 10; Initial r3x0 = L(l)*cos(thref(l))+L(2)*cos(20*pi/180)+L(3)*cos(40*pi/180); r3y0 = L(l)*sin(thref(l))+L(2)*sin(20*pi/180)+L(3)*sin(40*pi/180); r3xf = L(l)*cos(thref(l))+L(2)*cos(40*pi/180)+L(3)*cos(60*pi/180); r3yf = L(l)*sin(thref(l))+L(2)*sin(40*pi/180)+L(3)*sin(60*pi/180); % Calculate Reference tau = (t-t0)/(tf-t0); Maneuver r3x = r3x0 + ( r3xf - r3x0 ) * ( 10 * tau A 3 - 15 * tau A4 + 6 * tau A 5 ); r3y = r3y0 + ( r3yf - r3y0 ) * ( 10 * tau A 3 - 15 * tau A4 + 6 * tau A 5 ); r3xd = ( r3xf - r3x0 ) / ( tf - r3yd = ( r3yf - r3y0 ) / ( tf - tO )* ( tO )*( 30 30 * tau A 2 * tau A 2 - - 60 60 * tau A 3 * tau A 3 + 30 + 30 * tau A 4); * tau A 4); r3xdd = (r3xf-r3x0)/((tf-t0) A 2)*(60*tau-180*tau A 2+120*tau A 3); r3ydd = (r3yf-r3y0)/((tf-t0) A 2)*(60*tau-180*tau A 2+120*tau A 3); 77 if(t>tf); r3x=0; r3y=0; r3xd=0; r3yd=0; r3xdd=0; r3ydd=0; end % Determine Inverse Kinematics th23 = kink(r3x-L(l)*cos(thref(l)),r3y-L(l)*sin(thref(l)),L(2),L(3)); thref(2) = th23(l); thref(3) = th23(2); % Calculate Joint Velocites Using Jacobean H= [-L(2)*sin(thref(2)),-L(3)*sin(thref(3));.... L(2)*cos(thref(2)),L(3)*cos(thref(3))]; thd23 = inv(H) * [ r3xd; r3yd thdref(2) = thdref(3) = thd23(2); % ]; thd23(l); Calculate Joint Acceleration Using Jacobean Hdot=[-L(2)*thdref(2)*cos(thref(2)),-L(3)*thdref(3)*cos(thref(3));.. -L(2)*thdref(2)*sin(thref(2)),-L(3)*thdref(3)*sin(thref(3))]; 78 thdd23=inv(H)*([r3xdd;r3ydd]-Hdot*[thdref(2);thdref(3)]); thddref(2) = thdd23(l); thddref(3) = thdd23(2); % Calculate Reference Control Torques [MM,GM] = mgm(thref,thdref); B = [l,-l,0;0,l,-l;0,0,l]; Uref = inv(B) * ( MM * thddref + GM' 79 ); C NEAR-MINIMUM-TIME RERENCE TRACKING CONTROLLER 1. Main Program % % nmtl program Constants L(l) = 15*2.54/100; L(2) = 17*2.54/100 L(3) = 17*2.54/100 L(4) = 20/100; Lcm(l) = 0/100; Lcm(2) = 36.45/100; Lcm(3) = 34.9/100; % Mass m(l) = 54.69; m(2) = 2.09364; m(3) = 2.466; m(4) = 10.667; % Inertia 1(1) = 4.32132; 1(2) = 3.20338e-2; 1(3) = 5.38398e-2; 1(4) = 0.0912; % Maximum Torque umax=0.300; % Shaping Function Coefficient 80 ; ; alfa=0.1; beta=0.5; tO=0; MM(1,1) = I(l)+L(l) A 2*(m(2)+m(3))+L(4) A 2*m(4); MM(2,2) = 1(2) + m(2) * Lcm(2) A 2 + m(3) MM(3,3) = 1(3) + m(3) * Lcm(3) A 2; % Initial And * L(2) A 2; Final Manipulator Position thO=[0*pi/l 80;20*pi/l 80;40*pi/l 80] thf=[0.01 *pi/l 80;40*pi/l 80;60*pi/l 80] % Determine Minimum Time To Perform Maneuver T(l)=sqrt(MM(l,l)*(thf(l)-thO(l))/... (umax(l)*(l/4-l/2*alfa+l/10*alfa A 2))); T(2)=sqrt(MM(2,2)*(thf(2)-thO(2))/... (umax(l)*(l/4-l/2*alfa+l/10*alfa A 2))); T(3)=sqrt(MM(3,3)*(thf(3)-thO(3))/... (umax(l)*(l/4-l/2*alfa+l/10*alfa A 2))) T_max=max(T) tf=T_max delt=alfa*tf tf=T_max dt=alfa*tf ts=beta*tf tl=ts-dt 81 ; t2=ts+dt t3=tf-dt echo off; t0=0; trace=l; tol=le-8; xO=[0;0;0;0*pi/l 80;20*pi/l 80;40*pi/l 80] % Integrate Equations [t,x,uu] = Of Motion rku2('nmtrk',tO,tf,xO,tol,trace); 82 2. Near-Minimum-Time Equations Function function [xdot,U] = nmtrk(t,x); umax(l)=0.300; % Constants L(l)= 15*2.54/100; L(2) = 17*2.54/100; L(3) = 17*2.54/100; L(4) = 20/100; Lcm(l) = 0/100; Lcm(2) = 36.45/100; Lcm(3) = 34.9/100; % Mass m(l) = 54.69; m(2) = 2.09364; m(3) = 2.466; m(4) = 10.667; % Inertia 83 1(1) = 4.32132; 1(2) = 3.20338e-2; 1(3) = 5.38398e-2; 1(4) = 0.0912; % Angles thd(l) = x(l); thd(2) = x(2); thd(3) = x(3); th(l) =x(4) th(2) = th(3) =x(6) x(5) [MM,GM] = mgm(th,thd); % Gains glp=.10; g2p=.10; g3p=.10; glv=.50; g2v=.50; g3v=.50; 84 [MM,GM] = mgm(th,thd); [Uref,thref,thdref,thddref] = nmt_ref(t); dU(3)=-g3p*(th(3)-thref(3))-g3v*(thd(3)-thdref(3)); dU(2)=dU(3)-g2p*(th(2)-thref(2))-g2v*(thd(2)-thdref(2)); dU(l)=dU(2)-glp*(th(l)-thref(l))-glv*(thd(l)-thdref(l)); Ul=Uref+dU*; u=ur, B = [l r l,0;0,l,-l;0,0,i]; xdot = [ inv(MM) * ( B * U' - GM' 85 ); x(l); x(2); x(3) ]; 3. Near-Minimum-Time Reference Function % Reference Generator Function function [Uref,thref,thdref,thddref] = nmt_ref(t); umax(l)=0.300; % Constants L(l)= 15*2.54/100; L(2) = 17*2.54/100 L(3) = 17*2.54/100 L(4) = 20/100; Lcm(l) = 0/100; Lcm(2) = 36.45/100; Lcm(3) = 34.9/100; % Mass m(l) = 54.69; m(2) = 2.09364; m(3) = 2.466; m(4) = 10.667; % Inertia 1(1) = 4.32132; 1(2) = 3.20338e-2; 1(3) = 5.38398e-2; 1(4) = 0.0912; MM(1,1) = I(l)+L(l) A 2*(m(2)+m(3))+L(4) A 2*m(4); 86 ; ; + m(2) * Lcm(2) A 2 + m(3) MM(3,3) = K3) + m(3) * Lcm(3) A 2; MM(2,2) = 1(2) * L(2) A 2; thO=[0*pi/l 80;20*pi/l 80;40*pi/l 80] thf=[0.01 *pi/l 80;40*pi/l 80;60*pi/l 80] alfa=0.1; T(l)=sqrt(MM(l,l)*(thf(l)-thO(l))/(umax(l)*... (l/4-l/2*alfa+l/10*alfa A 2))); T(2)=sqrt(MM(2,2)*(thf(2)-thO(2))/(umax(l)*... (l/4-l/2*alfa+l/10*alfa A 2))); T(3)=sqrt(MM(3,3)*(thf(3)-th0(3))/(umax(l)*... (l/4-l/2*alfa+l/10*alfa A 2))); T_max=max(T); tf=T_max; dt=alfa*T_max; tl=tf/2-dt; t2=tf/2+dt; t3=tf-dt; umax(3)=MM(3,3)*(thf(3)-thO(3))/... A 2*(l/4-l/2*alfa+l/10*alfa A (tf 2)); umax(2)=MM(2,2)*(thf(2)-thO(2))/... A 2*(l/4-l/2*alfa+l/10*alfa A (tf 2)); umax(l)=MM(l,l)*(thf(l)-thO(l))/(tfA 2*(l/4-l/2*alfa+l/10*alfa A 2)); % Near Minimum Time Reference Maneuver 87 if(t>=0&t<=dt); f=(t/dt) A 2*(3-2*(t/dt)); ff=dt*((t/dt) A 3-(l/2)*(t/dt)M); fff=(dtA 2)*((l/4)*(t/dt)M-(l/10)*(t/dt) A 5); elseif (t>dt & t<=tl ); f=i; ff=(t-(l/2)*dt); fff=((l/2)*t A 2-(l/2)*t*dt+(3/20)*dt A 2); elseif (t>tl f = ff 1 - = & t<=t2); 2*(((t-tl)/(2*dt)) A 2*(3-2*((t-tl)/(2*dt)))); -(l/2)*dt+tl+2*dt*(((t-tl)/(2*dt))-2*((t-tl)/(2*dt)) A 3+. ((t-tl)/(2*dt)) A 4); fff = ((23/20)*alfa A 2-(3/4)*alfa+(l/8))*tfA 2+(2*dt) A 2*... ((l/2)*((t-tl)/(2*dt)) A 2-(l/2)*((t-tl)/(2*dt)) A4... +(l/5)*((t-tl)/(2*dt)) A 5)+((l/2)*tf-(3/2)*dt)*(t-tl); elseif (t>t2 & t<=t3); f=-l; ff=-t+tl+t2-(l/2)*dt+2*dt*(((t2-tl)/(2*dt))-... 2*((t2-tl )/(2*dt)) A 3+((t2-tl )/(2*dt)) A 4); fff=(-(21/20)*alfaA 2+(l/4)*alfa+l/8)*tfA 2+... (l/2)*(tf-3*dt)*(t-t2)-(l/2)*(t-(l/2)*tf-dt) A 2; 88 ; elseif (t>t3 f & t<=tO; -l+(((t-t3)/(dt)) A 2*(3-2*((t-t3)/dt))); = ff=((l/2)*dt+dt*(-(t-t3)/dt+((t-t3)/dt) A 3-(l/2)*((t-t3)/dt) A4)); fff=(-(l/20)*alfa A 2-(l/2)*alfa+l/4)*tfA 2+(l/2)*dt*(t-t3)+... dt A 2*(-(l/2)*((t-t3)/dt) A 2+(l/4)*((t-t3)/dt) A 4-(l/10)*((t-t3)/dt) A 5); elseif (t>tf); f=0; ff=0; fff=0; end thref( 1 )=(umax( 1 )/MM( 1 , 1 )) *fff+thO( 1 ); thref(2)=(umax(2)/MM(2,2))*fff+thO(2); thref(3)=(umax(3)/MM(3,3))*fff+thO(3); thdref( 1 )=(umax( 1 )/MM( 1 , 1 ))*ff thdref(2)=(umax(2)/MM(2,2))*ff; thdref(3)=(umax(3)/MM(3,3))*ff; thddref(l)=umax(l)*f/MM(l,l); thddref(2)=umax(2)*f/MM(2,2); thddref(3)=umax(3)*f/MM(3,3); [MM,GM] = mgm(thdref,thref); 89 B = [1,-1,0;0,1,-1;0,0,1]; Uref = inv(B) * ( MM * thddref + GM' 90 ); D. MISCELLANEOUS FUNCITONS 1. Equation Of Motion Coefficient Funciton function % [MM.GM] = mgm(th,thd); Constants L(l)= 15*2.54/100 L(2) = 17*2.54/100 L(3) = 17*2.54/100 L(4) = 20/100; Lcm(l) = 0/100; Lcm(2) = 36.45/100; Lcm(3) = 34.9/100; % Mass m(l) = 54.69; m(2) = 2.09364; m(3) = 2.466; m(4) = 10.667; % Inertia 1(1) = 4.32132; 1(2) = 3.20338e-2; 1(3) = 5.38398e-2; 1(4) = 0.00912; 91 % Subroutine To Calculate Mass and "G" Matrix th21=th(2)-th(l); th31=th(3)-th(l); th32 = th(3) % - th(2); 'Mass' Matrix MM(1,1) = I(l)+L(l) A 2*(m(2)+m(3))+L(4) A 2*m(4); MM(1,2) = ((m(2)*L(l)*Lcm(2))+(m(3)*L(l)*L(2)))*cos(th21); MM(l,3) = m(3)*L(l)*Lcm(3)*cos(th31); MM(2,1) = MM(1,2); MM(2,2) = 1(2) + m(2) MM(2,3) = m(3) * * Lcm(2) A 2 + m(3) * L(2) A 2; L(2) * Lcm(3) * cos(th32); MM(3,1) = MM(1,3); MM(3,2) = MM(2,3); MM(3,3) = K3) + m(3) % 'G' * Lcm(3) A 2; Matrix cl22 = ((m(2)*L(l)*Lcm(2))+(m(3)*L(l)*L(2)))*sin(th21); cl33 = m(3)*L(l)*Lcm(3)*sin(th31); c211 =- ((m(2)*L(l)*Lcm(2))+(m(3)*L(l)*L(2)))*sin(th21); c233 = m(3)*L(2)*Lcm(3)*sin(th32); 92 c311 =-m(3)*L(l)*Lcm(3)*sin(th31); c322 = - m(3)*L(2)*Lcm(3)*sin(th32); cl=[0,0,0;... 0,cl22,0;... 0,0,cl33]; c2=[c21 1,0,0;... 0,0,0;... 0,0,c233]; c3=[c31 1,0,0;... 0,c322,0;... 0,0,0]; GM(l) = thd*cl *thd'; GM(2) = thd * c2 * thd*; GM(3) = thd * c3 * thd'; 93 2. Inverse Kinematics Function function theta=kink(x,y,Ll,L2) % Subroutine To Determine Inverse Kinematic Solution cl2=(x A 2+y A 2-Ll A 2-L2 A 2)/(2*Ll*L2); sl2=sqrt(l-cl2 A 2); thetal2=atan2(sl2,cl2); minv=inv([Ll+L2*cl2,-L2*sl2;L2*sl2,Ll+L2*cl2]); c=minv*[x;y]; theta(l )=atan2(c(2),c( 1 )); theta(2)=theta( 1 )+theta 12; 94 APPENDIX D LINEAR CONSTANT GAIN FEEDBACK CONTROLLER A. 70 "*--. --* 60 * 50 ,.-**' 40 Main Bodv Ancular 30 — Displacement 20 Joint 10 * < - - - 1 Joint 2 Angular Displacement Angular Displacement — .. -10 10 20 15 Time 25 30 35 40 (Sec) Figure D.l Linear Constant Gain Feedback Joint Position Time History With G =0.1 And G v =0.2 p 95 c i . / 1 / 1 Mail i Body Angular Velc city v *~ 1— * * 1 Link 1 Angular Velocity Link i. /Miguiai veiuuuy (Deg/Sec) \ • Velocity \ \ ! / -, „-" — Angular - i. t % ^ m m ** ** * y^T-^ * * " " ^•*-^">--^ * i V e=r ..''/ 1 1 c 1 5 10 20 15 Time 25 30 35 40 (Sec) Figure D.2 Linear Constant Gain Feedback Joint Velocity Time History With G p =0.1 And G v =0.2 96 — 0.07 i 0.06 V Main Body Torque 0.05 0.04 ? 0.03 ^3 0.02 f 0.01 z H i i L nk 1 — Torque _v ........ iL nk 2 Torque V \\ 1 Vi \ * \v . 0.00 \* -0.01 'T^ ^ \\\ *^j i? -------- rrrr^^ " mm \--. s / -0.02 -0.03 10 20 15 Time 25 30 35 40 (Sec) Figure D.3 Linear Constant Gain Feedback Torque Time History With G p=0.1 And G v =0.2 97 Figure D.4 Linear Constant Gain Feedback Momentum Wheel Speed Time History With G =0.1 And G v =0.2 p 98 B. POLYNOMIAL REFERENCE TRACKING CONTROLLER 70 I 60 i 50 40 * j 30 *» * ~ * Main Body Angular Displacement 20 ngular Displacement u c 10 < Join) 2 ^ ngular Displacement -10 1 1 0.5 1.5 2 2.5 Time 3.5 4.5 (Sec) Figure D.5 Polynomial Reference Tracking Controller Joint Position Time History With Gp=l and G v =5 And Maneuver Time Of 5 Seconds 99 1 .-'*' '""***., — .' / IV / Q 1 i •• — — / -/-— - \ > Main Body Angular Velocity ! /' — **-- / Link 1 \ Angular Velocity \ Link 1 Angular Velocity ,'' S ------- - 0.5 . l - 1.5 2.5 Time 3.5 4.5 (Sec) Time Figure D.6 Polynomial Reference Tracking Controller Joint Velociy Seconds Of Time 5 History With G p =l and G v =5 And Maneuver 100 0.25 1 0.2 Main Body Torque 0.15 1 1 0.1 y l S .» ..-• - "-. ' -- I E Z 0.05 £'• ^ " " *"* a- © 1 Torque Link 2 Torque "['"""-— v 3 L. — Link * *"*^. .-**"" -».. •'/ -0.05 // \%, -0.1 \* -0.15 i % V i - — - * ' -0.2 1 -0.25 1 0.5 1 1.5 2 2.5 Time 3 3.5 4.5 (Sec) Figure D.7 Polynomial Reference Tracking Controller Torque Time History With Gp=l and G v =5 And Maneuver Time Of 5 Seconds 101 1(W0 - 1020 - 1000 f 980 - \ z Z 960 _ cr j2 940 |\ 920 900 I i i RRO 0.5 1 1.5 2 2.5 Time 3 3.5 4 4.5 1 (Sec) Figure D.8 Polynomial Reference Tracking Controller Momentum Wheel Speed Time History With Gp=l and Gv=5 And Maneuver Time Of 5 Seconds 102 C. NEAR-MINIMUM-TIME REFERENCE TRACKING CONTROLLER 70 60 I 50 | 40 E u u « 30 D. n ? , — __" .--""" Main Body Angular ....—- Displacement 20 J2 I Joinl I 1 A ngular Displacement 10 5 Joint 2 Angular uisp lacement -10 0.5 2.5 1.5 Time 4.5 3.5 (Sec) Figure D.9 Near-Minimum Time Tracking Controller Joint Position Time History With G =l , G v =5, Maneuver Time Of 5 Seconds And a = 0.25 p , 103 9 8 |9 § Main Body Angular 1s Veloc — 0.5 ~y~ Link 1 Angular Velocity Link 2 Angular Velocity 2.5 1.5 Time V 3.5 4.5 (Sec) Figure D.10 Near-Minimum Time Tracking Controller Joint Velocity Time History With G p =l , G v =5, Maneuver Time Of 5 Seconds , And a = 0.25 104 0.4 0.3 Main B ody Torque 0.2 ' - - <* / e tV 01 2" o s _ Link f --- yf'\ 3 Link 1 Torque * *\ 1 ic >v e >""*».. H **"-—.. - _——- ]ljr . ^- * -0.1 ^. -0.2 "-._-»*" -0.3 0.5 1.5 2 Time Figure D.ll With 3 2.5 4.5 3.5 (Sec) Near-Minimum Time Tracking Controller Torque Time History G p =l G v =5, Maneuver Time Of 5 Seconds And a = 0.25 , , 105 1040 - 1090 - I 1000 w 1 .2 960 - s 940 Q9fl - ono 880 - 0.5 2.5 1.5 Time 4.5 3.5 (Sec) Figure D.12 Near-Minimum Time Tracking Controller Wheel Speed Time History With G =l , G v =5, Maneuver Time Of 5 Seconds And a = 0.25 p , 106 70 - 60 - | 50 | 40 3 « 30 a - „• Main Body Angular - -••'" _ _ „-.-- .83 ? 20- I 10 Displacement Joint < 1 Joint 2 -10 Angular Displacement A ngular Displacement - () 0.5 1 1.5 2 2.5 Time 3 3.5 4 4.5 5 (Sec) Figure D.13 Near-Minimum Time Tracking Controller Joint Position Time History With G p =l , G v =5, Maneuver Time Of 5 Seconds , And a = 0.1 107 9 8 Tt 7 "8 Main Body Angular Veloc ity Link c Link 1 Angular Velocity i. Angular Velocity ' -—^=— 0.5 2.5 1.5 Time 4.5 3.5 (Sec) Figure D.14 Near-Minimum Time Tracking Controller Joint Velocity Time History With G p =l , G v =5, Maneuver Time Of 5 Seconds And a = 0.1 , 108 0.25 - « - Main Body Torque 0.15 / / Link y - /' f Z 1 Torque / 0.05 Link 2 Torque A J;/ 3 4 \\ I" -0.05 H t ' -0.15 I V -0.25 0.5 1 1.5 2 2.5 Time 3 3.5 4 4.5 5 (Sec) Figure D.15 Near-Minimum Time Tracking Controller Torque Time History With G p =l , G v =5, Maneuver Time Of 5 Seconds , And a = 0.1 109 IVJJU j£ - IUUU 1a J __. 950 900 / - 2 3 Time (Sec) Figure D.16 Near-Minimum Time Tracking Controller Wheel Speed Time History With G p =l , G v =5, Maneuver Time Of 5 Seconds And a = 0.1 , 110 . REFERENCES 1. 2. Junkins, John L., Kim, Youdan, An Introduction To Dynamics And Control Of Flexible Structures, to appear, American Institute Of Aeronautics and Astronautics Education Series, pp. 73-97, 102-136, 1992. Bang, Hyochoong, Maneuver And Vibration Control Of Flexible Space Structures By Lyapunov Stability Theory, Phd Dissertation, Texas University, College Station, Texas, September 1992. A&M 3. Hailey, Jeffrey A., Experimental Verification of Attitude Control Techniques For Flexible Spacecraft Slew Maneuvers, Master's Thesis, Naval Postgraduate School, Monterey, California, March 1992. 4. Watkins Jr., R.J., The Attitude Control Of Flexible Spacecraft, Master's Thesis, Naval Postgraduate School, Monterey, California, June 1991. 5. Integrated Systems, Inc., 6. MATRIXX Core, 7 tn ed., January 1990. Kepco, Inc., Model BOP 20-10M Power Supply Instruction Manual, Hushing, New York, 1987. 7. Integrated Systems Inc., 8. Greenwood, Donald Prentice-Hall, AC-100 Users Guide, V2.4.05, February 1991 T., Principles New Jersey, Of Dynamics, 2nd ed., pp. 239-291, 1988. 9. Kalman, R.E., Bertram, J.E., "Control Systems Analysis And Design Via The Second Method Of Lyapunov, Part 1, Continuous Time Systems", Journal Of Basic Engineering, pp. 371-393, June 1960. 10. Craig, John, 11. Johnson, Eric R., Servomechanisms, pp. 209-230, Prentice-Hall, 1963. 12. Lauer, Henri, Lesnick, Robert U., and Matson, Leslie E., Servomechanism Fundamentals, 2nd ed., pp. 349-486, McGraw Hill, New York, 1960. 13. Klafter, Richard, D., Chmielewski, Thomas, A., and Negin, Michael, Robotic Engineering, An Integrated Approach, Prentice Hall, New Jersey, 1989. Introduction To Robotics Mechanics Addision-Wesley Publishing, New York, 1989. J., Ill And Control, 2 n " ed., New Jersey, 14. Slotine, Jean Jacques E., and Li, Weiping, Applied 88-97, 169-228, 266-271, 392-421, Prentice Hall, Nonlinear Control, pp. New Jersey, 1991. 15. Asada, H., and Slotine, J.J.E., Robot Analysis And Control, pp. 133-184, John Wiley and Sons, New York, 1986. 16. Nye, Theodore W., LeBlanc, David J., and Cipra, Raymond J., "Design And Modeling Of A Computer Controlled Planar Manipulator", The Kinematics Of Robot Manipulators, pp. 191-201, MIT Press, Cambridge, MA, 1987. 112 INITIAL DISTRIBUTION LIST No. Copies 1. Defense Technical Information Center 2 Cameron Station Alexandria, Virginia 22304-6145 2. Library, Code 52 2 Naval Postgraduate School Monterey, California 93943-5002 3. 4. 5. Chairman, Code AA Department Of Aeronautics and Astronautics Naval Postgraduate School Monterey, California 93943-5000 Chairman, Code SP Department Of Aeronautics and Astronautics Naval Postgraduate School Monterey, California 93943-5000 Professor Brij N. Agrawal, Code AA/Ag 1 1 2 Department Of Aeronautics and Astronautics Naval Postgraduate School Monterey, California 93943-5000 6. Hyochoong Bang, Code AA/Bn Department Of Aeronautics and Astronautics Dr. 1 Naval Postgraduate School Monterey, California 93943-5000 7. LCDR Dennis Sorensen Naval Air Warfare Center Point Mugu, CA 1 - Weapons Division 93042-5000 113 rxnuA Liunwn i 1 POSTGRADUATE SCH MONTEREY. CALIFORNIA 93943-5002 Thesis S66596 c.l Thesis S66596 c.l Sorensen of Design and control link two a space based manipulator with Lyapunov based control laws. Sorensen Design and control of a space based two link manipulator with Lyapunov basedcontrol laws. J5WC4>

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Download PDF

advertisement