NONLINEAR TRACKING OF NATURAL MECHANICAL SYSTEMS FOR HWIL SIMULATION Except where reference is made to the work of others, the work described in this thesis is my own or was done in collaboration with my advisory committee. This thesis does not include proprietary or classified information. _______________________________ Justin N. Martin Certificate of Approval: _________________________ _________________________ John E. Cochran, Jr. Andrew J. Sinclair, Chair Professor Assistant Professor Aerospace Engineering Aerospace Engineering _________________________ _________________________ David A. Cicci Joe F. Pittman Professor Interim Dean Aerospace Engineering Graduate School NONLINEAR TRACKING OF NATURAL MECHANICAL SYSTEMS FOR HWIL SIMULATION Justin N. Martin A Thesis Submitted to the Graduate Faculty of Auburn University in Partial Fulfillment of the Requirements for the Degree of Master of Science Auburn, Alabama August 4, 2007 iii NONLINEAR TRACKING OF NATURAL MECHANICAL SYSTEMS FOR HWIL SIMULATION Justin N. Martin Permission is granted to Auburn University to make copies of this thesis at its discretion, upon request of individuals or institutions and at their expense. The author reserves all publication rights. ______________________________ Signature of Author ______________________________ Date of Graduation iv THESIS ABSTRACT NONLINEAR TRACKING OF NATURAL MECHANICAL SYSTEMS FOR HWIL SIMULATION Justin N. Martin Master of Science, August 4, 2007 (B.S. Aero. Eng., West Virginia University, 2003) (B.S., Mech. Eng., West Virginia University, 2003) 123 Typed Pages Directed by Andrew Sinclair Auburn University has entered into collaboration with the US Department of Defense for academic study and development of hardware-in-the-loop simulation laboratory. One aspect of this collaboration has been research into new concepts for the control of flight motion tables, a critical component in HWIL simulations. Commonly used Proportional-Integral-Derivative (PID) controllers can suffer limitations in applications with nonlinear and multi-input/multi-output systems. To overcome these limitations, a nonlinear dynamic-inversion controller was developed. Applying Lagrange?s equations to determine equations of motion, a Lyapunov function was used to develop a globally asymptotically stable controller. v After comparing PID and dynamic-inversion controllers through multiple commanded motions and adjustments to gain, the dynamic-inversion was more stable and produces less error. Both controllers are capable of performing real-time applications. vi ACKNOWLEDGMENTS The author would like to thank Scottie Mobley of the Aviation and Missile Research Development and Engineering Center and Ryan Brindley and Jeffrey Gareri of Simulation Technologies, Inc. for their continued assistance with HWIL systems. vii Style manual or journal used: Modern Language Association Style Manual Computer software used: Microsoft Office Word 2003 Microsoft Office Excel 2003 Matlab 7.3.0 (R2006b) UGS Solid Edge V19 viii TABLE OF CONTENTS LIST OF TABLES.............................................................................................................. x LIST OF FIGURES ........................................................................................................... xi I. INTRODUCTION..........................................................................................................1 II. FLIGHT MOTION TABLES........................................................................................4 III. PID CONTROLLERS .................................................................................................7 Proportional Term....................................................................................................9 Integral Term .........................................................................................................10 Derivative Term.....................................................................................................11 IV. DYNAMIC-INVERSION CONTROLLERS............................................................12 V. MODEL AND CONTROLLER DEVELOPMENT...................................................15 Development of Equations of Motion....................................................................15 PID Program Structure...........................................................................................24 Derivation of Dynamic-inversion Controller.........................................................35 Dynamic-inversion Program Structure ..................................................................38 VI. MODEL SIMULATIONS AND ANALYSIS...........................................................49 General Experimental Set-up.................................................................................49 Commanded Variables and Descriptions of Functions..........................................49 Gain Selections ......................................................................................................52 Model Simulations.................................................................................................53 ix VII. COMPARISON OF PID AND DYNAMIC-INVERSION CONTROLLERS........69 Comparison of Controls for Each Input.................................................................69 Error Analysis for each Input.................................................................................77 Runtime Analysis for each Input ...........................................................................87 VIII. CONCLUSIONS.....................................................................................................92 BIBLIOGRAPHY..............................................................................................................94 APPENDICES ...................................................................................................................96 Appendix A: Complete Derivation of Equations of Motion.................................97 Appendix B: Complete Derivation of Dynamic-inversion Controller................103 Appendix C: PID and Dynamic-inversion Controller Program..........................105 x LIST OF TABLES Table 1. Table 2. Table 3. Table 4. Table 5. Performance Specifications .........................................................................6 Trends due to Change in Gains....................................................................8 Moments of Inertia for 3-Gimbaled System ..............................................23 Total Error Comparison of PID and DI controllers ...................................85 Analysis of Runtimes.................................................................................89 xi LIST OF FIGURES Figure 1a. Figure 1b Figure 2a,b Figure 3 Figure 4 Figure 5 Figure 6a,b Figure 7a,b Figure 8a,b Figure 9 Figure 10 Figure 11 Figure 12a Figure 12b Figure 12c Figure 12d Figure 13a Figure 13b Figure 13c Layout of HWIL Laboratory ....................................................................2 ECSEL Laboratory in Point Mugu, CA ...................................................2 Three Axis Flight Motion Table ...............................................................6 Block Diagram of a PID Program.............................................................9 Block Diagram of a Dynamic-inversion Program ..................................13 Inertial CS aligned wit Component #1 CS..............................................16 Component #1 CS aligned with the Inertial CS......................................17 Component #2 CS aligned with the Component #1 CS..........................17 Component #3 CS aligned with the Component #2 CS..........................18 Gimbaled System set to Zero Deflections ..............................................19 PID Program Structure............................................................................25 Dynamic-inversion Program Structure ...................................................39 PID Model for Constant Control and KP = 200, KD = 50 .......................55 DI Model for Constant Control and KP = 200, KD = 50..........................56 PID Model for Constant Control and KP = 400, KD = 100 .....................57 DI Model for Constant Control and KP = 400, KD = 100........................58 PID Model for Step Control and KP = 200, KD = 50 ..............................60 DI Model for Step Control and KP = 200, KD = 50.................................61 PID Model for Step Control and KP = 400, KD = 100 ............................62 xii Figure 13d Figure 14a Figure 14b Figure 14c Figure 14d Figure 15a Figure 15b Figure 16a Figure 16b Figure 17a Figure 17b Figure 18a Figure 18b Figure 18c Figure 19a Figure 19b Figure 19c Figure 20a Figure 20b Figure 21a Figure 21b DI Model for Step Control and KP = 400, KD = 100...............................63 PID Model for Sinusoidal Control and KP = 200, KD = 50.....................65 DI Model for Sinusoidal Control and KP = 200, KD = 50.......................66 PID Model for Sinusoidal Control and KP = 400, KD = 100...................67 DI Model for Sinusoidal Control and KP = 400, KD = 100.....................68 Control for Constant Command and KP = 200, KD = 50.........................71 Control for Constant Command and KP = 400, KD = 100.......................72 Control for Step Command and KP = 200, KD = 50................................73 Control for Step Command and KP = 400, KD = 100..............................74 Control for Sinusoidal Command and KP = 200, KD = 50......................75 Control for Sinusoidal Command and KP = 400, KD = 100....................76 Error for Constant Command and KP = 200, KD = 50 ............................78 Error for Step Command and KP = 200, KD = 50 ...................................79 Error for Sinusoidal Command and KP = 200, KD = 50..........................80 Error for Constant Command and KP = 400, KD = 100 ..........................82 Error for Step Command and KP = 400, KD = 100 .................................83 Error for Sinusoidal Command and KP = 400, KD = 100........................84 Error Comparison in PID and DI with KP = 200, KD = 50 .....................86 Error Comparison in PID and DI with KP = 400, KD = 100 ...................87 Runtime Comparison in PID and DI with KP = 200, KD = 50 ................90 Runtime Comparison in PID and DI with KP = 400, KD = 100 ..............90 1 I. INTRODUCTION Prior to Hardware-in-the-Loop (HWIL) simulations, analyses of missile seeker head performance were conducted by live-fire tested at firing ranges. Missiles, with the seeker heads already installed, were sold in batches. To demonstrate acceptable performance, a certain percentage from each batch was field tested. Not only was this risky with the entire sale depending on a small, random potion of the batch, but it was also expensive with guaranteed losses of the tested missiles. HWIL systems simplify this process of testing seeker heads. Rather than needing an entire range to fire a missile, HWIL systems use a flight motion table and scene generation to simulate the flight of a seeker head on a missile. The seeker head is placed in the gimbaled flight motion table while the simulated target is located at a stationary point in front of the flight table. Using the scene generator and synthetic lines of sight, the seeker simulates the tracking of a target. Figure 1a (reproduced from Reference [High Performance]) and Figure 1b (reproduced from Reference [Hardware]) demonstrate the basic layout of a HWIL laboratory. Figure 1a displays the components and connections for an HWIL simulation, while Figure 1b is 2 a photograph of the Electronic Combat Simulation and Evaluation Laboratory (ECSEL) located in Point Mugu, CA. Figure 1a: Layout of HWIL Laboratory Figure 1b: ECSEL Laboratory in Point Mugu, CA 3 In the summer of 2006, Auburn University, in collaboration with Simulation Technologies, Inc. and the US Department of Defense, began a program to receive, install and test a flight table for a HWIL system. One of the goals of Auburn University is to study the design and implementation of controllers to guide the gimbaled system. The following paper presents the design and testing of a nonlinear dynamic-inversion controller with comparison to a Proportional-Integral-Derivative (PID) controller. 4 II. FLIGHT MOTION TABLES Flight motion tables consist of multiple gimbaled joints whose motion is generated by hydraulic or electric actuators. The goal of using a flight table in a HWIL simulation is to reproduce some aspect of the rotational motion of the flight hardware. They are designed to accurately and precisely direct the motion of a missile seeker head towards a simulated target in order to replicate an engagement of that target in an actual flight. However, these tables are not limited in use to missile simulations. Three main functions this table can serve are: 1) simulating missile seeker head motions for HWIL systems 2) development and testing of guidance and navigational apparatuses 3) testing the stability and motion of satellite systems (Carter 425) The development of more highly maneuverable missiles, tables and seeker heads adjust to meet the demands of HWIL simulations. This has led to tables capable of higher angular accelerations and angular velocities for dictating faster responding (Carter 426). The actuators being incorporated into these flight tables are required to generate enough torque to accelerate 50-100 lb. gimbals at rates of 50,000 deg/s2. These requirements can dictate choices in the materials used to reduce vibration and deformation and also the types of actuators used to generate the needed torques (Carter 427). 5 Unfortunately, no matter how well the flight table and gimbaled arrangements are constructed, there will still remain some space between all bearings and connections. Seeing as how the control in each mode creates an oscillatory damping function until the error is nearly eliminated, the control will constantly be demanding reversal of torques. This reversing may lead to problems with pieces impacting each other, vibrations, and noise (Collins 579). Granted these are not short term catastrophic problems, but it may lead to wear and tear on the flight table. Along with the physical design of gimbaled joints and actuators, it is up to the engineer to determine a controller that will allow motion tracking with as little error as possible. Errors in the table motion refer to orientation errors: when the system's gimbaled angles do not exactly match the commanded values. Major manufacturers of flight motion tables for HWIL laboratories include Acutronic USA, Inc. and Ideal Aerosmith. They produce tables that can be used in a pitch-yaw-roll simulation which greatly reduces the cost of developing seeker heads. The basic model of the table studied in this work is shown in Figures 2a and 2b below and is similar to the Carco Series S-450R-3 Simulator produced by Acutronic (Three). Some representative specifications of the flight table are shown in Table 1. 6 Figures 2a-b: Three Axis Flight Motion Table (a) (b) Table 1: Performance Specifications Component and Axis Rotation Performance Spec. Type of Motion Component #1: Pitch Component #2: Yaw Component #3: Roll Angular Freedom - +/- 50 deg +/- 50 deg +/- 50 deg Positioning Accuracy - 0.002 deg 0.002 deg 0.002 deg Continuous +/- 200 deg/sec +/- 200 deg/sec +/- 1800 deg/sec Rate Range Non- Continuous +/- 200 deg/sec +/- 200 deg/sec +/- 200 deg/sec Continuous 20,000 deg/sec2 20,000 deg/sec2 18,000 deg/sec2 Acceleration, w/ load Non- Continuous 20,000 deg/sec2 20,000 deg/sec2 20,000 deg/sec2 7 III. PID CONTROLLERS In nearly all mechanically operated systems in the industrial world, controllers are used. These controller algorithms need to be designed in order meet performance requirements while also maintaining a reasonable amount of simplicity (Gutirrez). Proportional- integral-derivative (PID) controllers are widely used to satisfy these conditions. PID or some forms of the algorithm are currently being used in approximately 95% of control loops found in modern industries (Astrom 216). The versatility of the PID-control approach is one reason PID controllers are so prevalent in modern industries. The three terms of a PID controller (proportional, integral and derivative) all serve a specific purpose in the control algorithm. As shown in the paragraphs to come, each part determines how the system will behave. A quick overview will show that the proportional term allows the controller motion to converge to the desired response but does not eliminate the steady state error. The integral term will eliminate the steady error but can degrade the transient response. The derivative term will increase the stability of the system (PID-Tutorial). Figure 3 below is a block diagram demonstrating the basic outline of a PID algorithm. The controller signal, u, for a single-input, single-output (SISO) is: 8 tKtKK DIP ddd eeeu ++= ? (1) where KP, KI and KD are the corresponding gains for each term and e is the system error (the difference between the input and the output) at each time step (PID-Tutorial). Table 2 (reproduced from Reference [PID-Tutorial]) shows the effects of each of the three terms on a closed loop system. Each cell in the table represents a general system where there is a small increase or decrease in the gains KP, KI or KD. Table 2: Sensitivity due to Changes in Gains CL RESPONSE RISE TIME OVERSHOOT SETTLING TIME S-S ERROR KP Decrease Increase Small Change Decrease KI Decrease Increase Increase Eliminate KD Small Change Decrease Decrease Small Change 9 Figure 3: Block Diagram of a PID Program Integral Proportional Derivative Plant Input Output + - Error + ++ Proportional Term As mentioned before, the purpose of the proportional term is to force the system to respond in the direction of the input. The response is based purely on the error in the system. If the system is far from convergence, the error is large. Because of the direct relationship between the proportional control and the error, if the error is large then the control due to the proportional term is also large and vice versa. The convergence rate of the system can be greatly affected by the magnitude of KP. By increasing the value of the proportional gain, the rise time (time it takes to approach the commanded value) decreased. However, one detail to note is the overshoot (the amount the gimbaled motion exceeds the commanded motion). Although it will take the system 10 longer to reach the ideal state, the overshoot is smaller, which can help lead to faster convergence. The proportional term allows for a brisk adjustment in the controlled variable. However, it does not provide ?zero offset? even though it significantly reduces the error in the system. The term?s primary purpose is to quicken the response (Marlin 270). Integral Term The primary purpose of the integral term is to eliminate or reduce the steady state error, the error between the input and output of the system as the time approaches infinity. As shown in Equation 1, the integral term is the area under the curve in an error vs. time graph. An increase in the transient error is one flaw to the integral term. With an increase in the integral gain, KI, the rise time decreases. However, increases in overshoot and settling time (time taken for the system to converge to the commanded value) will occur. The integral term can help achieve zero offset in the system response to a step input; unfortunately, it may sometimes cause instability due to its poor dynamic performance (Marlin 271). 11 Derivative Term The derivative term is the final piece in the PID controller for creating a stable system. The derivative term is proportional to the time rate of change in the error. The derivative term requires ?lead?, that is, information about future values of the error, allowing the controller to react faster to any changes (Gutirrez). This prediction allows the controller to converge quickly, while increasing stability without the transient error. By increasing the derivative gain, KD, the damping in the system can be increased. Greater damping will result in a more rigid system that is slower in convergence. A negative aspect of this term is that it may require numeric differentiation that can amplify high frequency noise in the system (Marlin 274). 12 IV. DYNAMIC-INVERSION CONTROLLERS As discussed in the previous section, PID controllers are not only simple in structure, but solve a wide range of control cases with suitable results. In order to achieve good performance with the PID, the tuning of the parameters and the employment of a functional such as anti-windup and derivative filtering are vital (Visioli). There are also a few limitations to a PID controller. With a PID, it can be difficult to prove stability for nonlinear systems (such as a multi-gimbaled system on a flight table). Also, the implementation of a PID is less clear for multi-input, multi-output (MIMO) systems. An alternative control structure that may overcome these limitations is dynamic-inversion control. According to Looye and Joos, ?dynamic-inversion is a straight forward methodology for designing multi-variable control laws for nonlinear systems (1).? Dynamic-inversion methods are commonly used in aerospace applications. One such example is the development of a controller to operate in nonlinear flight schemes such as post-stall applications (Looye 1). In terms of a flight table, a dynamic-inversion controller is motivated by the multiple gimbals whose motions affect neighboring gimbals (this is shown explicitly in the derivation of the equations of motion found in Appendix A). 13 The defining trait of a dynamic-inversion controller is the use of a dynamic model (i.e. equations of motion) to compute the inputs necessary to generate the desired output. Hence, the name refers to the inversion of the dynamic model from the form typically used in solving for system motion. It is noteworthy that a model of the system is built into the controller, which is not the case in PID control. A dynamic-inversion controller consists of both feedback and feed-forward sections, as shown in Figure 4. An inner-loop, structured as a closed loop, applies an inverse in dynamics in order to negate the nonlinearities in the system (Plett 360). This closed-loop system is simplified into a set of integrators to be used in the feed-forward section (Looye 1). The feedback section is the outer portion in this arrangement. The feedback loop contains a standard linear controller, such as a PID controller, in order to minimize ?mismatches? and disturbances in the model created by the nonlinearities (Plett 360). Figure 4: Block Diagram of a Dynamic-inversion Program Desired Trajectory Inverse Model PD Controller Plant + Torque Output ++ - 14 One key to the dynamic-inversion controller is the dynamic model located in the nonlinear feedback block. The dynamic model is a model of the input-output relationship for the system to be controlled. Plett suggests allowing the dynamic reference model is a delayed version of the actual model of the system (364). This would allow the controller to adjust a priori to a delayed inverse of the system dynamics (Plett 360). There are some problems to consider when introducing a dynamic-inversion controller into a system. Looye and Joos state that dynamic-inversion tends to lead to poor robustness (1). Because the system model is built into the controllers, it is sensitive to errors in this model. The authors believe the uncertainties can be counteracted by attempting to increase the robustness of the system within the linear loop (Looye 1). Other problems created by the dynamic-inversion include the requirement that an inverse exists (non-singular) and the system typically needs a priori information that may need to be more precise than that available. Dynamic-inversion techniques, such as adaptive inverse control, can be applied to the arrangement (Plett 360). 15 V. MODEL AND CONTROLLER DEVELOPMENT Development of Equations of Motion To develop controller designs through numerical simulation, a model of the flight-table motion is required. A specific model is developed in this section in the form of equations of motion for a flight motion table. Key terms in these equations of motion include the mass matrix and the Coriolis vector. The equations of motion for the flight table derived here are based on several assumptions: (1) All rotations of coordinate axes are about a single, inertial point. (2) Piece #1 and #3 are balanced and symmetric, therefore, the moments of inertia are centered about each component?s symmetrical center of geometry. (3) Each component of the system analyzed is a rigid, non-flexible body. (4) Friction and other applied forces in joints are negligible. Describing the system kinematics begins with establishing an inertial coordinate system (CS) ( )ZYX ,, and a separate, body-fixed coordinate system for each individual 16 component studied ( )3,2,1for ,,, =izyx iii . All inertial coordinates originate about the point of all rotations. The coordinate systems are shown in Figures 5 through 8. The figures of all the components and their set up were designed in Solid Edge. The pieces are also to scale with the flight table currently located at Auburn University. The coordinate systems in Figures 6 through 8 are all body-fixed; therefore they are attached to and rotate with the specific component. Figure 5: Inertial CS aligned with Component #1 CS with the +Y-axis into the surface X, x1 Z, z1 17 Figures 6a and 6b: Component #1 CS aligned with the Inertial CS A positive rotation occurs in the +y1 direction Figures 7a and 7b: Component #2 CS aligned with the Component #1 CS A positive rotation occurs in the +z2 direction Y, y1 Z, z1 Z, z1 X, x1 z1 z2 y1, y2 y1, y2 x1 x2 18 Figures 8a and 8b: Component #3 CS aligned with the Component #2 CS A positive rotation occurs in the +x3 direction The entire apparatus with all angles in the { } 3,2,1,, zyx axes set to zero radians is shown in Figure 9. Note that in the actual experiment, this may not be the starting position of the components. This is only the reference position used in the equations of motion. z2, z3 z2, z3 y2 y3 x2 x3 19 Figure 9: Gimbaled System set to Zero Deflections To specify the orientations of the four coordinate systems, the rotation matrices were developed. A rotation matrix, in the case of a 3-dimensional system, is a 3x3 matrix that transforms the unit vector of one coordinate system into a corresponding vector in another coordinate system. In this case, the rotations are about one of the three axes. Equation 2a demonstrates the transformation from the coordinate system of component Y X Z 20 #1 to the inertial coordinate system. Note that the rotation occurs about the J (also 1?j ) axis, which is why a ?1? is the multiplying factor in the J row. ?? ?? ? ?? ?? ? ? ? ? ? ? ? ? ? ? ? ? = ?? ?? ? ?? ?? ? 1 1 1 11 11 ? ? ? cos0sin 010 sin0cos k j i K J I ?? ?? (2a) Equations 2b and 2c are the transformation matrices for converting the coordinate system of component #2 to component #1 and the coordinate system of component #3 to component #2, respectively. In the #2 to #1 transformation, the rotation occurs about the 1 ?k (also 2 ?k ) axis. The rotation in the #3 to #2 transformation is about the 2?i (also 3?i ) axis. ?? ?? ? ?? ?? ? ? ? ? ? ? ? ? ? ? ? ? = ?? ?? ? ?? ?? ? 2 2 2 22 22 1 1 1 ? ? ? 100 0cossin 0sincos ? ? ? k j i k j i ?? ?? (2b) ?? ?? ? ?? ?? ? ? ? ? ? ? ? ? ? ? ? ?= ?? ?? ? ?? ?? ? 3 3 3 33 33 2 2 2 ? ? ? cossin0 sincos0 001 ? ? ? k j i k j i ?? ?? (2c) Angular velocities of the coordinate systems can be obtained by inspection. A listing of the transformed angular velocities can be found in Appendix A. 21 The next step in deriving the equations of motion is to determine the energy in the rotational system. In his treatise the M?canique analytique, Lagrange demonstrated laws of virtual work, which could be applied to the mechanics of both solids and fluids (Joseph). Rather than follow the work of D?Alembert and Euler by tracking the complete motion of a particle, he showed that ?if we determine its configuration by a sufficient number of variables whose number is the same as that of the degrees of freedom possessed by the system, then the kinetic and potential energies of the system can be expressed in terms of those variables, and the differential equations of motion thence deduced by simple differentiation (Joseph).? The form of this equation is: fqqq =??+????? ? ? ??? ? ? ? VTT t &d d (3) where [ ]T321 ???=q and [ ]T321 ??? &&&& =q are the generalized coordinates and generalized velocities, T is the kinetic energy of the system defined as the addition of the rotational kinetic energy of each component, V is the total potential energy, and f is the generalized external forces on the system. The energies are shown in Equation 4a-b as: 22 ( )( )2cos1cossin ??? ?+= LamgV where ( ) 1 1 2 2 1 4 cos12 #2component in offset CG of distance #2component of mass ?pi? ?pi? ? ?= ?= ?= = = La L m (4a) ? = = 3 12 1 i i T iT ?I? i (4b) For component #2, estimations were made for the distance of the center of gravity offset. In Equation 4b, I indicates the inertia of each body relative to the fixed center of rotation for the system. By substituting Equation 4 into Equation 3, one can obtain equations of motion that allow for the development of the mass matrix and the nonlinear Coriolis vector terms. The complete organization of these matrices and vectors can be found in Appendix A. A unique, assumed characteristic of the flight motion table system considered here is that all parts of the system rotate about a single point. Also, it should be noted that component #3 is merely a hollowed out cylinder that can be rotated about its longitudinal axis. Therefore, it can be assumed that I3y and I3z are equivalent. (The actual code used in numerical simulation is written for the possible case of zy II 33 ? for future experiments). Equations 5 and 6 are the resultant mass matrix and nonlinear Coriolis 23 vector for the assumption of zy II 33 = . The moments of inertia and mass elements were calculated based on the geometry of the modeled part. The pieces were modeled as prisms, assumed to be solid casts. Table 3 is a representation of the moments of inertia for each component. Table 3: Moments of Inertia for 3-Gimbaled System Component and Axis Rotation Body Axis of Rotation Component #1: Pitch (234.9 kg) Component #2: Yaw (202.2 kg) Component #3: Roll (152.8 kg) x 57.6 kg-m2 11.9 kg-m2 2.8 kg-m2 y 20.4 kg-m2 10.7 kg-m2 6.8 kg-m2 z 37.8 kg-m2 9.9 kg-m2 6.8 kg-m2 The resulting equations of motion have the form .fhqM =+&& The terms qM && and h have the following form when zy II 33 = . ( ) ( ) ( ) ?? ?? ? ?? ?? ? ? ? ? ? ? ? ? ? ? ? + ++++ = 3 2 1 323 32 233 2 322 2 321 0sin 00 sin0cossin ? ? ? ? ??? && && && && xx yz xyyxxy II II IIIIII qqM , (5) 24 ( ) ( ){ } ( ){ } ? ? ? ? ?? ? ? ? ? ? ? ? ?? ? ? ? ? ?++?+?? ? ?++?+? = 2321 2 3323322121 1 3323322122 cos sincos sin2cos , ??? ?????? ?????? x xyxyx xyxyx I VIIIII VIIIII && &&& &&& &qqh (6) The generalized forces of the system are the three control torques exerted by the hydraulic/electric actuators of the flight table. ?? ??? ?? ??? ? ? ? = 3 2 1 f (7) In operation, the torques are the control variables used to force the system motion to track the commanded motion. PID Program Structure The block diagram shown in Figure 10 demonstrates the structure of the Proportional- Integral-Derivative program. The following is an explanation of each block in the figure. The explanation includes the parameters involved, the objective of the structure and the output from the block. 25 Figure 10: PID Program Structure 4. Update Time Step PID 5. Call RK44 Function (deriv_con) 6. Calculate Angle, Angular Rate Derivs and Integrals 7. Update (9x1) State Vector 8. Call Com. Angles and Rates Func. (commanded_values) 9. Choose Commanded Angle Types 10. Call Control Function (control_develop) 11. Determine Errors and Control 1. Initialize Variables See Block Diagram on Dynamic Inversion Controllers 2. Choose Controller 3. Create State Matrix Is Runtime Complete? Stop DI x, f k (1-4) i, t(i+1) Commands x f Yes No 26 1. Initialize Variables Besides variables based on common knowledge, such as 281.9 smg = , the section takes into account the initialization of the angles of each component on the fight table and the angular velocity of each. The physical geometry of each rotating piece, such as moments of inertia, distance of offset from the rotation point (used for potential energy) and the mass of the offset components are defined. The three basic variables involved with time are defined. The total time is the duration the simulation is allowed to run, the time step for each integration step, and the number of iterations allowed before a control adjustment are defined. For instance, if the system runs for 10 seconds, and each time step is 0.01 seconds, 1000 steps are analyzed. If a control is adjusted every 2 time steps, the system is running at 500 Hz. Finally, the initialization contains as experimental variables the gains for the proportional, integral and derivative terms. 2. Choose Desired Controller This part of the program allows the user to select which type of controller they would like to examine; a PID or dynamic-inversion controller. Once 27 the user selects a controller, all the characteristics of that controller are applied for the remainder of the program. The controller not chosen is negated for the remainder of the runtime. In this chapter, the PID controller was selected. 3. Create State Vector The state vector groups together all the components of the simulation that will be integrated across time. In the case of the PID controller, the state vector is a (9 x 1) vector consisting of angular positions for the 3 rotating components, the three angular rates associated with those components and the three errors associated with the integral gain in the controller itself. Grouping all nine components together assures that each cell is updated simultaneously with the other eight cells. 4. Update the System Time The arrangement in question is a discrete time system. Therefore, the values at a specific time step are determined and then evaluated at a small forward step in the system time, .t? When the words ?updating the system time? are used, the program is not automatically changing all the times 28 associated with calculated variables to some new time, ,tt ?+ but it?s storing the old and new variables which can then be used for comparison and error calculations. 5. Call Proportional Integral Derivative RK-44 Function (derive_con) The PID program was written to take small time steps, integrate them across those time steps and update the state vector consisting of angles, angular rates, and integral error, all using a fourth-order Runge-Kutta integration method. The RK-44 uses the beginning, middle, and end point of each time step in order to calculate an average time rate of change of each parameter over that discrete time. The following equations represent the RK-44 for a second-order system: ( ) ( )( )[ ] ( ) ( ) ( )( )[ ]tt tt tt tt ft ft ft ft f ukqqxk ukqqxk ukqqxk uqqxk qqq ,, ,21, ,21, ,, , 34 23 12 1 +?= ?? ? ?? ? ? ? ?? ? ? +?= ?? ? ?? ? ? ? ?? ? ? +?= ?= = & & & & &&& The k-terms sent back into the ?deriv_con? function, which represents the RK-44, are (9 x 1) vectors as well. Each cell in the vector corresponds to 29 its respective angle, angular rate or integral error which will be used to update the state vector, as shown in section 7. 6. Calculate Angle, Angular Rate Derivatives, and Error Integral The function, ?deriv_con?, serves as the integration section of the RK-44. With the inputs of each portion of the time step, as shown in part 5, the function calculates the k and z (integral) terms. The k terms are also referred to as the ?& and ?&& terms. The equations of motion (derived in the upcoming sections and Appendix A): ( ) ( ) fqqhqqM =+ &&& , (8) Provide the second time derivatives of the Euler angles. ( ) ( )( )qqhfqMqq &&&& ,dd 1 ?== ?t (9) Although the mass matrix, M, is diagonally dominant and invertible, the ?\? function was used in Matlab. This applies a Gaussian Elimination method for finding a matrix equation of the form bAx = , where A is (nxn) and x and b are (nx1). The integral for each portion of the time step is also 30 calculated by taking difference between the commanded angles and the actual angles. The returned vector is a (9 x 1) vector in the form: ( ) ( ) ( )[ ]Txxx 31313141 zqqk &&&&=? Note that all the components are the derivatives, or slope, of its integrated part. This vector is returned to the main program in order to update the state vector. 7. Update State Vector Once all the derivatives of the angles, angular rates and integral errors are sent back to the main program, the last piece of the 4th Order Runge-Kutta is applied. This part of the RK-44 updates the angles, angular rates and error integral across the discrete time step using the equation: ( )( )43211 261 kkkkxx ++++=+ ii (10) The updated state matrix will be used for calculating the errors present and the required controls for the next iteration of the main program. 31 8. Call the Commanded Angles and Rates Function (commanded_values) This section of the program loop is one of the key factors for designing the experiment. The commanded angles, angular rates and angular accelerations are very similar to the simulated path of a target that the seeker is trying to follow. This portion of the program relays the position, speed, and the rate of change in speed the gimbals should be achieving. These commanded angles and rates are calculated in the ?commanded_values? function. The variables sent to this function are the time step iteration number and the updated time of the system. 9. Determine Control Angle and Rate Types The simulated motion can take many shapes and forms. The three types of simulations that are going to be examined are the constant direction and angular velocity, a constant position and angular velocity followed up with an impulsive change in position at a given time step, and a constantly changing (in this case sinusoidal) position and velocity. The constant position and angular velocity scenario takes a random gimbaled placement and tracks the motion of the positions of the interceptor and target as they maneuver to another position over the course of the simulation. The position and angular velocity with an impulsive change tracks the position 32 as it settles, instantaneously changes to another commanded position, and tracks the gimbals? motion for the duration of the simulation time. The sinusoidal position and velocity applies a commanded sine form of position, velocity and acceleration that the gimbals must track. The main program will receive a (9 x 1) vector, represented as: [ ]Tcomcomcomcommand qqqy &&&= The three scenarios are further described and demonstrated in the Commanded Variables and Description of Functions section. 10. Call the Control Function (control_develop) Now stored in the program is the updated system time, a corresponding state vector and a commanded position and rate vector. However, as mentioned before, the values of these two vectors may not agree, creating some error in the results. To minimize this error, a control is applied to the necessary gimbals where it is required. This control is determined in the function ?control_develop?. The required inputs for this function are both the state vector and commanded angles and rates vector. The 33 function will calculate the control required for the next iteration of the main program. 11. Determine the Three PID Errors and Controls As discussed in previous chapters, there are three types of errors to analyze in a PID controller system; the proportional, integral and derivative. Having a commanded angle and an angular rate relayed to the function, the position and speed each component should calculated. Also, after completion of the RK-44 and an update of the variables, an actual set of angles and angular rates are determined. Unless everything in the system was 100% perfect, there is bound to be some error in the nonlinear arrangement. Therefore, it is necessary to find the errors for each section of the PID controllers. The error used in the proportional controller is simply the difference between the commanded and actual position of each component. The integral controller error is represented by the ?z? term found from integrating the difference between the commanded and actual angle of each component from the beginning to the current time of the simulation. The error used in the derivative portion of the controller is difference between the commanded and actual angular velocities of each component. 34 As discussed before, the basic equation used in the majority of PID controllers is represented by Equation 1: tKtKK DIP ddd eeeu ++= ? Note that in the previous section, the three errors defined correspond, respectively, to the errors in the equation. The values of the gains have already been defined in the initialization of all parameters section. These gains can be adjusted in order permit the system to more precisely converge to the commanded angles and angular velocities. The value of the control, u, a (3 x 1) vector in this case, is then updated and applied back into Equation 9, until further updated. The control, u, is then relayed back to the main program. 12. Is Runtime Complete? No matter how long the system runs, there is always going to be some oscillation error due to fact that a small time segment is used, rather than a continuous signal. However, after a certain time, the error in the system is considered negligible. The runtime value, defined in the initialization section, is used by the programmer to ensure the program will terminate 35 after a specific simulation time. Once a discrete value of t? is added to the system time, the program checks to see if the termination time has been achieved. If so, the program terminates. If the time has not been reached, the program loops back to Section 2, and repeats all the steps leading up to Section 12. Derivation of Dynamic-inversion Controller When deriving the dynamic-inversion controller for a motion flight table, the problem is classified in the category of tracking controllers for nonlinear natural mechanical systems. Remember that a natural mechanical system is one where all the terms in the kinetic energy are quadratic in the generalized velocities, .q& Because of this property of natural mechanical systems, the kinetic energy of the system can be written as: ( ) ( ) jiijT qqMT &&&&& 2121, == qqMqqq (11) where T is the kinetic energy and M is the mass matrix. In order to determine the equations of motion, as required for the dynamics of the flight table, Lagrange?s equation 36 (Eq. 3) needs to be applied. This is done most conveniently in index notation. Expansion of Equation 8 leads to the equations of motion, represented as: k k ji k ij ij j ki iki fq Vqq q Mqq q MqM = ? ?+ ? ?? ? ?+ &&&&&& 2 1 (12a) or kkiki fhqM =+&& (12b) where k ji k ij j ki k q Vqq q M q Mh ? ?+ ??? ? ??? ? ? ?? ? ?= && 2 1 (13) In vector notation, Equation 12 can be represented as: ( ) ( ) fqqhqqM =+ &&& , (14) Based on this form of the equations of motion, a general form of a dynamic-inversion controller will be developed. The control law for the generalized forces, f, in design is desired to track the motion of the commanded inputs, dq and dq& . For this, a Lyapunov function will be selected. ( ) ( ) ( ) ( )dTddTdQ qqqqqqqq &&&& ??+??= 2121 (15) A check point is to be sure 0=Q , when the actual motion identical to the commanded motion. Otherwise, .0>Q Taking the derivative of Q, one gets: 37 ( ) ( )ddTdQ qqqqqq ?+??= &&&&&&& (16) Here, dq&& are the accelerations associated with the commanded motion. The condition of a global asymptotically stable system can be achieved by choosing: ( )ddd qqqqqq &&&&&& ??=?+? (17) The commanded motion can be substituted into the equations of motion to define df , the generalized forces necessary to produce this motion. ( ) ( )( )ddddd qqhfqMq &&& ,1 ?= ? (18) A globally asymptotically stable controller used to track a desired motion for the natural mechanical system may be found by solving Equation 14 for q&& and substituting along with Equation 18 back into Equation 17. ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( )[ ]dd dddd qqqqqM qqhqMqMqq,hfqMqMf -1d && && ?+?? ?+= ? ,1 (19) The performance of this nonlinear controller will be compared with a typical linear PID controller. A more detailed derivation can be found in Appendix B. From Equation 19, it can be seen that the feedback terms in this controller are a form of proportional- 38 derivative (PD) control. The PD controller can alleviate unmodeled dynamics and disturbances (Yan 199). Therefore, in comparison with the PID controller laid out in the previous section, the dynamic-inversion controller does not require error integral information. This is, however, a result of the choice of the Lyapunov function. The dynamic-inversion controller does require knowledge of commanded accelerations (in order to compute df ), which were not required by the PID controller. In implementation some means of computing dq&& is necessary. Dynamic-inversion Program Structure The block diagram shown in Figure 11 demonstrates the structure of the dynamic- inversion program. The following is an explanation of each block in the figure. The explanation includes the parameters involved, the objective of the structure and the output from the block. Many of these sections are very similar to those in the PID description, mainly because the dynamic-inversion controller is the only difference in the system. A positive attribute to this program is the flexibility of examining whichever controller the user prefers. Therefore, the majority of the steps and processes are mirrored for each controller. 39 Figure 11: Dynamic-inversion Program Structure 4. Update Time Step DI 5. Call RK44 Function (deriv_con) 6. Calculate Angle, Angular Rate Derivs and Integrals 7. Update (6x1) State Vector 8. Call Com. Angles and Rates Func. (commanded_values) 9. Choose Commanded Angle Types 10. Call Control Function (control_develop) 11. Determine Errors and Control 1. Initialize Variables See Block Diagram on Prop-Int-Deriv Controllers 2. Choose Controller 3. Create State Matrix Is Runtime Complete? Stop PID x, f k (1-4) i, t(i+1) Commands x f Yes No 12. Components of Actual and Reference Controls ComponentsAngleVelocity 40 1. Initialize Variables To compare the PID arrangement to that of the dynamic-inversion in a fair manner, the same initial variables must be used. Therefore, the moments of inertia, mass of each component, distance of component offset, initial positions and angular velocities, and time step variables (including the frequency of control adjustments) must remain equal to those in the PID experiment. The only difference lies in the gains used in the dynamic- inversion. Only proportional and derivative gains are used and their values will be different following proper tuning to compliment the arrangement. 2. Choose Desired Controller As mentioned before, the user has the ability to select which type of controller they chose to examine; a PID or dynamic-inversion controller. Once the user selects a controller, all the characteristics of that controller are applied for the remainder of the program. In this chapter, the dynamic-inversion controller was selected. Therefore, the PID controller characteristics and processes are negated for the remainder of the runtime. 41 3. Create the State Vector The state vector combines all the components that will be integrated across the simulation time. In the case of the dynamic-inversion controller, the state vector is a (6 x 1) vector consisting of angular positions for the three rotating components and the three angular rates associated with those components: [ ]Tqqx &= . Unlike the PID state vector, there is no integral error present due to the form of the controller. The error across this controller is not integrated; therefore, the gain is not necessary. This set-up assures that all six variables will be updated and applied simultaneously throughout the simulation. 4. Update the System Time The process of updating the system time by a discrete time step, ,t? serves the same purpose as the PID controller. This update is merely a process of storing and comparing the values of the state vector, command vector, and control vector for any given time step throughout the simulation. It allows the user to specify which value at a specific instant they would prefer to examine. 42 5. Call Dynamic-inversion RK-44 Function (deriv_con) Similar to the PID program, the dynamic-inversion is analyzed over constant time steps of ,t? therefore integration across those discrete times is necessary. Once again, a fourth-order Runge-Kutta numerical integration is going to be applied to the system. All equations are going to be the same, with the exception of the k terms. Before, in the PID program, each returned value of k contained three angular derivatives, three angular velocity derivatives, and three error integrations. The error integrations are not necessary; therefore, the k values are a (6 x 1) vector, rather than a (9 x 1). The called function, ?deriv_con?, is the same function used for PID controller, only modified to cater to both controllers. 6. Calculate Angle and Angular Rate Derivatives The function, ?deriv_con?, is the exact same function used for the PID controller, except in this situation, the calculation of the integral error over the specific time step is not applied. Both functions serve the purpose of determining the rate change in the angular velocity, as previously shown in Equation 9: 43 ( ) ( )( )qqhfqMqq &&&& ,dd 1 ?== ?t The resulting q& and q&& terms are returned to the main program as a (6 x 1) vector in the form of: ( ) ( )[ ]Txx 313141 qqk &&&=? This vector is returned to the main program in order to update the state vector by completing the RK-44 integration. 7. Update the State Vector Once all of the angular rates and angular accelerations are returned to the main program, the last function of the fourth-order Runge-Kutta is performed. This part of the RK-44 updates the angles and angular velocities across the discrete time step, ,t? using the same equation as that of the PID controller: ( )( )43211 261 kkkkxx ++++=+ ii 44 The updated state matrix will be used for calculating the errors in the commanded and actual angles and the required controls for the next iteration of the main program. 8. Call the Commanded Angles and Rates Function (commanded_values) The commanded angles, angular rates, and angular accelerations are the motion that the controller is trying to track. This portion of the program relays the position, the speed, and the rate of change in speed the gimbals should be achieving. For comparison, the commanded values calculated in this section will be the same as those used in the PID experiments. These commanded angles and rates are calculated in the ?commanded_values? function. The variables sent to this function are the time step iteration number and the updated time of the system. 9. Determine Control Angle and Rate Types Three basic types of simulations were chosen to study the performance of the dynamic-inversion controller: (1) constant direction and angular velocity, (2) constant position and angular velocity followed up with an impulsive change in position at a given time step, and (3) constantly 45 changing (in this case sinusoidal) position and velocity. The same commanded angles and angular velocities will be used in the PID program to ensure an accurate comparison in the results of both controllers. The returned variable is a (9 x 1) vector, represented as: [ ]Tcomcomcomcommand qqqy &&&= The three scenarios are further described and demonstrated in the Commanded Variables and Description of Functions section. 10. Call the Control Function (control_develop) The variables required to calculate the updated errors and control for the 1+it conditions are now saved into the state vector and command vector. Assuming the vectors do not agree, whether machine error or an error caused by a change in the commanded angles and velocity, the ?control_develop? function allows for a correction control to be produced. The required inputs for this function are both the state vector and commanded angles and rates vector. The function will calculate the control required for the next iteration of the main program. 46 11. Call Commanded and Actual Components Function (model_components) One of the unique characteristics of a dynamic-inversion controller is the comparison between the actual components of the equations of motion and some reference components based off commanded values in order to determine the control applied to the next time interval. The reference value components consist of the angles, angular rates, mass matrix, Coriolis terms and control. These terms are calculated in the function, ?model_components? and returned to ?control_develop? function for analysis. Once the terms are returned, they are applied in the control equation for this particular dynamic-inversion controller which will be derived in the following section: ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( )[ ]dd dddd qqqqqM qqhqMqMqq,hfqMqMf -1d && && ?+?? ?+= ? ,1 (19) The first part of the Equation [19] represents the feedback portion of the controller. The second condition of the equation is a nonlinear feedback term comparing the difference between the Coriolis terms of the reference and actual motions. The last term is a PD form of a controller that is a nonlinear feedback as well. 47 All the terms in Equation 19 are generated in ?model_components?, with the exception of the commanded and actual angles and angular rates. The inputs for this function are the actual and commanded angles and angular velocities. Once the new control is calculated, it is returned to the main program in order to be applied to the next iteration, starting at Section 4. 12. Determine Commanded and Actual Value Components The function, ?model_components?, for the reference portion uses Equation 5 to determine the mass matrix, Equation 6 to determine the Coriolis terms, and Equation 8 to determine the reference control. All these values are in correspondence with the simulated motion of the system. The actual value components consist of the angles, angular rates, mass matrix, and Coriolis terms. These values are determined in a similar fashion to the reference components, using the same order and equations, with the exception that the returned variables represent the actual characteristics of the system. The applied control is not calculated until the five characteristics of the motion at that particular time step are returned to the ?control_develop? function. 48 13. Is Runtime Complete? Just like in the PID controller, there is always going to be some sort of small error in the simulation, where after a certain time, the error in the system is negligible (if properly designed). Once a discrete value of t? is added to the system time, the program checks to see if the termination time has been achieved, ceasing to program if it has. If the time has not been reached, the program loops back to Section 2, and repeats all the steps leading up to Section 13, only to be tested again. 49 VI. MODEL SIMULATIONS AND ANALYSIS General Experimental Set-up The tri-gimbaled system has three basic parameters which need to be considered when designing an accurate and precise response to a given command. The factors which will be examined are the type of controller, the type of commanded angles, angular velocities and angular accelerations, and the gain levels used in the simulation. As discussed in previous chapters, the controllers analyzed are of type PID and dynamic-inversion, and the commanded positions and rates are constant, steps and sinusoidal. The experiments will also examine how the system changes with increases in both the proportional and derivative gains. Commanded Variables and Description of Functions The commanded variables consist of constant variables, alternating variables (a step function), and a constantly adjusting sinusoidal command. 50 Prior to every simulation, some factors of the gimbals are set. All three gimbals are initially set to +0.4 radians from their ?home? position. This offset will require an initial maneuver to be performed in order to approach the commanded position. The gimbals will all start from rest. The controller must now track one of the following commanded parameters: 1. Constant Commanded Angles and Rates With all three gimbaled positions set to +0.4, an alternate set of gimbaled positions is commanded to force the controller to adjust all three controls simultaneously. The values for these gimbals after the commanded controls begin are: [ ] [ ] [ ]Tcom T com T com 000 000 2.01.01.0 = = = ? ? ? && & 2. Constant Commanded Angles and Rates with a Step Function This commanded function is very similar to the constant commanded positioning function described in the previous section with one basic 51 difference. The commanded function travels along a constant angle for all components then impulsively maneuvers to a different angular position. For the experiments, the positions are: [ ][ ]T com T com t t 5.02.02.0:sec105 2.01.01.0:sec50 =?= =?= q q Because this is a discrete time system and the time step is small, the angular velocity and angular acceleration expressions remain the same. 3. Consistent Sinusoidal Commanded Path For this commanded motion, the angles of all three components are all going to be based off a sinusoidal wave with amplitude of 0.5 radians and frequency of pi21 Hz. Because these angular positions are based on time, the angular velocities and accelerations can be represented as the first and second derivative of the position vector. [ ] [ ] [ ]Tcomcom Tcom com T com tttt tttt ttt sin5.0sin5.0sin5.0dd cos5.0cos5.0cos5.0dd sin5.0sin5.0sin5.0 2 2 ???== == = qq qq q && & 52 This method of determining the rates is valid as long as the commanded position of each component is a function of time. Gain Selections It is typically assumed that the larger the gain in a system, the less robust (more sensitive to errors in the dynamic model) the system becomes. However, the experiments performed are not to find the optimal gains in the simulation; only to deduce the effect that a change in gains has on each controller. After numerous trials of various gains, two sets of gains were selected as appropriate for the experiments. The first set was KP = 200 and KD = 50. This set of gains allows the motion and response to be more flexible. The second set is a more aggressive set with the gains set to KP = 400 and KD = 100. For each experiment run with the PID controller, the KI gain was set equal to the KP gain. The dynamic-inversion controller does not utilize the integral gain. These two sets of gains were compared alongside the three commanded motion and the two different types of controllers, producing twelve sets of results. 53 Model Simulations Three commanded motion, two sets of controller gains, and two different controllers are to be examined. Adjusting every parameter to perform a single study in order to find out the characteristics and effects of that parameter leads to twelve different situations to consider. This section will break those twelve simulations into three groups of four tests. Those three groups will be run with the types of commanded motion. Within each group, the changes in both the gains and controller will be observed through the angular motions of the gimbals. An analysis of the applied controls, errors, and run times will be addressed in later sections. Input 1: Constant Commanded Angles and Rates The first simulation tested will compare the PID and dynamic-inversion controllers with a change in gains. These will be conducted with the commanded angles, angular velocities and angular accelerations held constant throughout a 10 second test period. Figures 12a and 12b represent a PID and dynamic-inversion controller under the influence of a proportional gain equal to 200 and a derivative gain equal to 50. After the initial jump from the +0.4 radian position, all components controlled through the PID show difficulties in convergence. The pitch component (piece 54 #1) shows the greatest error in convergence. However, the dynamic-inversion controller seems to converge in nearly every piece before one second has elapsed. Figures 12c and 12d are for the same situation, except the gains have increased significantly. Note that the PID system still has convergence problems, but seems more in control due to the gain increase. The dynamic-inversion does not seem to have difficulties with the larger gains. The convergence time in the dynamic- inversion controllers remained about the same. With the larger gains applied, the PID controller began to respond similarly to the dynamic-inversion controller. 55 Figure 12: a) PID Model for Constant Control and KP = 200, KD = 50 0 1 2 3 4 5 6 7 8 9 100 0.2 0.4 0.6 0.8 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #1: Constant Function Actual Reference 0 1 2 3 4 5 6 7 8 9 10-0.1 0 0.1 0.2 0.3 0.4 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #2: Constant Function Actual Reference 0 1 2 3 4 5 6 7 8 9 100.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #3: Constant Function Actual Reference 56 Figure 12: b) DI Model for Constant Control and KP = 200, KD = 50 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #1: Constant Function Actual Reference 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #2: Constant Function Actual Reference 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #3: Constant Function Actual Reference 57 Figure 12: c) PID Model for Constant Control and KP = 400, KD = 100 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #1: Constant Function Actual Reference 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #2: Constant Function Actual Reference 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #3: Constant Function Actual Reference 58 Figure 12: d) DI Model for Constant Control and KP = 400, KD = 100 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #1: Constant Function Actual Reference 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #2: Constant Function Actual Reference 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #3: Constant Function Actual Reference 59 Input 2: Constant Commanded Angles and Rates with a Step Function The procedure of testing for this mode is nearly identical to that of Input 1 with the exception that a step function was applied at the 5 second mark. However, from 0 to 5 seconds and 5 to 10 seconds, the commanded angles and rates remain constant. Figures 13a and 13b represent a PID and dynamic-inversion controller with a proportional gain equal to 200 and a derivative gain equal to 50. Figures 13c and 13d represent the same except that more aggressive KP and KD gains of 400 and 100 respectively were applied. Similar to the previous mode, the dynamic-inversion has quicker reaction and convergence times than the PID controller. In fact, the PID controller only seems to be efficient when the gains are high. However, even after the gains are increased, there is still a significant amount of overshoot in the PID. The dynamic-inversion controller contains little to no overshoot and dampens out quickly in both cases. 60 Figure 13: a) PID Model for Step Control and KP = 200, KD = 50 0 1 2 3 4 5 6 7 8 9 100 0.2 0.4 0.6 0.8 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #1: Step Function Actual Reference 0 1 2 3 4 5 6 7 8 9 10 0 0.2 0.4 0.6 0.8 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #2: Step Function Actual Reference 0 1 2 3 4 5 6 7 8 9 100.1 0.2 0.3 0.4 0.5 0.6 0.7 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #3: Step Function Actual Reference 61 Figure 13: b) DI Model for Step Control and KP = 200, KD = 50 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #1: Step Function Actual Reference 0 1 2 3 4 5 6 7 8 9 10-0.1 0 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #2: Step Function Actual Reference 0 1 2 3 4 5 6 7 8 9 100.1 0.2 0.3 0.4 0.5 0.6 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #3: Step Function Actual Reference 62 Figure 13: c) PID Model for KP = 400, KD = 100 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #1: Step Function Actual Reference 0 1 2 3 4 5 6 7 8 9 10-0.1 0 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #2: Step Function Actual Reference 0 1 2 3 4 5 6 7 8 9 100.1 0.2 0.3 0.4 0.5 0.6 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #3: Step Function Actual Reference 63 Figure 13: d) DI Model for KP = 400, KD = 100 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #1: Step Function Actual Reference 0 1 2 3 4 5 6 7 8 9 10-0.1 0 0.1 0.2 0.3 0.4 0.5 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #2: Step Function Actual Reference 0 1 2 3 4 5 6 7 8 9 100.1 0.2 0.3 0.4 0.5 0.6 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #3: Step Function Actual Reference 64 Input 3: Sinusoidal Commanded Motion The following mode is most similar to that of a guided missile simulation in an HWIL system. The relative motion between the seeker and target is more likely to be in the form of smooth curves getting sharper as the missile approaches the target rather than instantaneous steps in position. Therefore, the results from this mode are probably more relevant to flight motion table simulations. The motion is a simple sinusoidal wave function which oscillates for a total duration of 10 seconds. Figures 14a and 14b represent a PID and dynamic-inversion controller with a proportional gain equal to 200 and a derivative gain equal to 50. Figures 14c and 14d represent the same except that more aggressive KP and KD gains of 400 and 100 respectively were applied. In this mode, the dynamic-inversion has little to no overshoot in both cases while still managing to converge to the curve within first second of the simulation. Contrary to the dynamic-inversion results, the PID controller seemed to struggle in its convergence on the constantly changing curve, most likely due to the integral term. Figure 14a shows difficulty in the pitch and yaw, while Figure 14c shows difficulty converging at the peaks of the sine wave. 65 Figure 14: a) PID Model for Sinusoidal Command and KP = 200, KD = 50 0 1 2 3 4 5 6 7 8 9 10-1 -0.5 0 0.5 1 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #1: Sinusoidal Function Actual Reference 0 1 2 3 4 5 6 7 8 9 10-1 -0.5 0 0.5 1 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #2: Sinusoidal Function Actual Reference 0 1 2 3 4 5 6 7 8 9 10-1 -0.5 0 0.5 1 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #3: Sinusoidal Function Actual Reference 66 Figure 14: b) DI Model for Sinusoidal Command and KP = 200, KD = 50 0 1 2 3 4 5 6 7 8 9 10 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #1: Sinusoidal Function Actual Reference 0 1 2 3 4 5 6 7 8 9 10 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #2: Sinusoidal Function Actual Reference 0 1 2 3 4 5 6 7 8 9 10 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #3: Sinusoidal Function Actual Reference 67 Figure 14: c) PID Model for Sinusoidal Command and KP = 400, KD = 100 0 1 2 3 4 5 6 7 8 9 10 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #1: Sinusoidal Function Actual Reference 0 1 2 3 4 5 6 7 8 9 10 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #2: Sinusoidal Function Actual Reference 0 1 2 3 4 5 6 7 8 9 10 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #3: Sinusoidal Function Actual Reference 68 Figure 14: d) DI Model for Sinusoidal Command and KP = 400, KD = 100 0 1 2 3 4 5 6 7 8 9 10 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #1: Sinusoidal Function Actual Reference 0 1 2 3 4 5 6 7 8 9 10 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #2: Sinusoidal Function Actual Reference 0 1 2 3 4 5 6 7 8 9 10 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 Time (sec) Po sit ion A ng le (ra d) Tracking Motion of Component #3: Sinusoidal Function Actual Reference 69 VII. COMPARISON OF PID AND DYNAMIC-INVERSION CONTROLLERS In addition to studying the gimbaled-angle histories, other methods can be used to compare the performance of the PID and dynamic-inversion controllers. As shown in the previous chapter, the PID controller consistently showed more fluctuation between the actual and commanded motion. This section describes an investigation of the control torques and processor time required by both the PID and dynamic-inversion controllers. Comparison of Controls for each Input Two factors being considered while looking at the control for each of the twelve systems are the settling time (how long is the controller required to operate before convergence) and the maximum control required. These two factors can play a role not only in the system performance, but also the life span of the flight table. The controls for all three components, both controllers and all three commanded inputs are shown in Figures 15-17. A common trend is present in all results shown. Concerning the PID controller, the reaction is slower and the control is more oscillatory. However, the controls are very low. The maximum control applied to the system is around a 70 magnitude of 200 N-m. With the dynamic-inversion controller, the reaction is nearly instantaneous in all cases with little overshoot and a short damping time. However, the control approaches levels around a magnitude of 1200 N-m. 71 Figure 15: a) Control for Constant Command and KP = 200, KD = 50 0 1 2 3 4 5 6 7 8 9 10 -200 -150 -100 -50 0 50 Time (sec) Co ntr ol (N -m ) PID Control for Three Components: Constant Function Comp. #1 Comp. #2 Comp. #3 0 1 2 3 4 5 6 7 8 9 10-300 -250 -200 -150 -100 -50 0 50 Time (sec) Co ntr ol (N -m ) DI Control for Three Components: Constant Function Comp. #1 Comp. #2 Comp. #3 72 Figure 15: b) Control for Constant Function and KP = 400, KD = 100 0 1 2 3 4 5 6 7 8 9 10-200 -150 -100 -50 0 50 Time (sec) Co ntr ol (N -m ) PID Control for Three Components: Constant Function Comp. #1 Comp. #2 Comp. #3 0 1 2 3 4 5 6 7 8 9 10-300 -250 -200 -150 -100 -50 0 50 100 Time (sec) Co ntr ol (N -m ) DI Control for Three Components: Constant Function Comp. #1 Comp. #2 Comp. #3 73 Figure 16: a) Control for Step Command and KP = 200, KD = 50 0 1 2 3 4 5 6 7 8 9 10 -200 -150 -100 -50 0 50 Time (sec) Co ntr ol (N -m ) PID Control for Three Components: Step Function Comp. #1 Comp. #2 Comp. #3 0 1 2 3 4 5 6 7 8 9 10-300 -200 -100 0 100 200 300 400 500 Time (sec) Co ntr ol (N -m ) DI Control for Three Components: Step Function Comp. #1 Comp. #2 Comp. #3 74 Figure 16: b) Control for Step Command and KP = 400, KD = 100 0 1 2 3 4 5 6 7 8 9 10-200 -150 -100 -50 0 50 100 150 Time (sec) Co ntr ol (N -m ) PID Control for Three Components: Step Function Comp. #1 Comp. #2 Comp. #3 0 1 2 3 4 5 6 7 8 9 10 -200 0 200 400 600 800 1000 1200 Time (sec) Co ntr ol (N -m ) DI Control for Three Components: Step Function Comp. #1 Comp. #2 Comp. #3 75 Figure 17: a) Control for Sinusoidal Command and KP = 200, KD = 50 0 1 2 3 4 5 6 7 8 9 10-300 -250 -200 -150 -100 -50 0 50 100 150 Time (sec) Co ntr ol (N -m ) PID Control for Three Components: Sinusoidal Function Comp. #1 Comp. #2 Comp. #3 0 1 2 3 4 5 6 7 8 9 10-500 -400 -300 -200 -100 0 100 Time (sec) Co ntr ol (N -m ) DI Control for Three Components: Sinusoidal Function Comp. #1 Comp. #2 Comp. #3 76 Figure 17: b) Control for Sinusoidal Function at KP = 400, KD = 100 0 1 2 3 4 5 6 7 8 9 10-250 -200 -150 -100 -50 0 50 100 Time (sec) Co ntr ol (N -m ) PID Control for Three Components: Sinusoidal Function Comp. #1 Comp. #2 Comp. #3 0 1 2 3 4 5 6 7 8 9 10-500 -400 -300 -200 -100 0 100 Time (sec) Co ntr ol (N -m ) DI Control for Three Components: Sinusoidal Function Comp. #1 Comp. #2 Comp. #3 77 In comparing the controllers, the PID controller causes oscillatory reversing torques, while the dynamic-inversion controller requires greater peak torques. Depending on the application, it is not clear which is more desirable. However, other factors, such as the results of the error analysis and processing time also factor into the comparison decision. Error Analysis for Each Input Like in any system, one of the most effective ways to compare the performance of two controllers is to analyze the error produced between systems. Because there are three angles examined, all having some sort of error, the error analyzed in Figures 18a-c and 19a-c are the norm of each error vector, giving a scalar quantity to plot. The first set of errors examined compares the angle errors throughout the entire simulation. Figures 18a-c compare the errors throughout the simulation for the lower gain settings (KP = 200, KD = 50). These gains are easy to compare due to the lack of convergence of the PID function after the initial angle jump. In every case, there is an oscillatory motion associated with the PID controllers. Every time the system oscillates, more error is added into the system. In all cases, the dynamic-inversion controller converges to zero error and remains relatively constant. 78 Figure 18: a) Error for Constant Command and KP = 200, KD = 50 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 0.6 Angle Error for PID: Constant Function Time (sec) No rm of A ng le Er ror V ec tor (r ad ) 0 1 2 3 4 5 6 7 8 9 100 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 Angle Error for DI: Constant Function Time (sec) No rm of A ng le Er ror V ec tor (r ad ) 79 Figure 18: b) Error for Step Command and KP = 200, KD = 50 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 0.6 Angle Error for PID: Step Function Time (sec) No rm of A ng le Er ror V ec tor (r ad ) 0 1 2 3 4 5 6 7 8 9 100 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 Angle Error for DI: Step Function Time (sec) No rm of A ng le Er ror V ec tor (r ad ) 80 Figure 18: c) Error for Sinusoidal Command and KP = 200, KD = 50 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 Angle Error for PID: Sinusoidal Function Time (sec) No rm of A ng le Er ror V ec tor (r ad ) 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 Angle Error for DI: Sinusoidal Function Time (sec) No rm of A ng le Er ror V ec tor (r ad ) 81 After examining Figures 19a-c, one can compare the errors throughout the simulation for the higher gain settings (KP = 400, KD = 100). With the more aggressive gains used, the errors for both controllers converge with less overshoot, thereby reducing the error after the initial jump from the starting parameters to the commanded parameters. 82 Figure 19: a) Error for Constant Command and KP = 400, KD = 100 0 1 2 3 4 5 6 7 8 9 100 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 Angle Error for PID: Constant Function Time (sec) No rm of A ng le Er ror V ec tor (r ad ) 0 1 2 3 4 5 6 7 8 9 100 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 Angle Error for DI: Constant Function Time (sec) No rm of A ng le Er ror V ec tor (r ad ) 83 Figure 19: b) Error for Step Command and KP = 400, KD = 100 0 1 2 3 4 5 6 7 8 9 100 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 Angle Error for PID: Step Function Time (sec) No rm of A ng le Er ror V ec tor (r ad ) 0 1 2 3 4 5 6 7 8 9 100 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5 Angle Error for DI: Step Function Time (sec) No rm of A ng le Er ror V ec tor (r ad ) 84 Figure 19: c) Error for Sinusoidal Command and KP = 400, KD = 100 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 Angle Error for PID: Sinusoidal Function Time (sec) No rm of A ng le Er ror V ec tor (r ad ) 0 1 2 3 4 5 6 7 8 9 100 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 Angle Error for DI: Sinusoidal Function Time (sec) No rm of A ng le Er ror V ec tor (r ad ) 85 In this case, the more aggressive gain settings resulted in less overshoot. Therefore, the majority of angle error is dependent upon the area under the curve from the initial maneuver to the commanded values. Also, as mentioned in previous chapters, more aggressive gains tend to lessen the oscillation in the PID controller, while increasing the rise time in the dynamic-inversion controller. In certain situations, mostly for short periods of time, it may be difficult to determine which controller produces less error. Therefore, Table 4 takes into account all twelve experiments and sums up the total error history over the course of each simulation. As shown, in every case the amount of error in the dynamic-inversion system is significantly less than that in the PID controller. Table 4: Total Error Comparison of PID and DI controllers Commanded Input Type Gains Used Controller Type Constant Input Step (Impulse) Input Sinusoidal Input PID 0.11450 0.12880 0.30250 KP = 200 KD = 50 Dyn. Inv. 0.01170 0.02000 0.01740 PID 0.04790 0.06200 0.09890 KP = 400 KD = 100 Dyn. Inv. 0.01170 0.02000 0.01730 The difference in the error from Table 4 can further be demonstrated in Figures 20a-b. One concept to note is the difference in the PID and dynamic controller errors for the simulation. As discussed earlier, as the gains increase, the amount of discrepancies 86 between the error in the PID and dynamic-inversion controller is lessened. In Figure 20a, the total error in the PID is approximately 1600-1700% greater than that in the dynamic- inversion controller for sinusoidal motion. However, when the gains are increased, such as in Figure 20b, the differences between the total errors in the simulation decrease significantly. The PID controller error is approximately 400-500% greater than the dynamic-inversion for sinusoidal motion. There is an obvious gap change between the two controller gain adjustments. Figure 20: a) Error Comparison in PID and DI with KP = 200, KD = 50 0.00000 0.05000 0.10000 0.15000 0.20000 0.25000 0.30000 0.35000 Constant Input Step (Impulse) Input Sinusoidal Input To tal E rro r PID: Kp = 200, Kd = 50 DI: Kp = 200, Kd = 50 87 Figure 20: b) Error Comparison in PID and DI with KP = 400, KD = 100 0.00000 0.02000 0.04000 0.06000 0.08000 0.10000 0.12000 Constant Input Step (Impulse) Input Sinusoidal Input To tal E rro r PID: Kp = 400, Kd = 100 DI: Kp = 400, Kd = 100 Runtime Analysis for Each Input A third characteristic of each controller to contemplate when deciding which to use in a flight table is the required processor time. A dynamic-inversion controller could converge faster than PID controller, but if it is hugely more computationally expensive, then the PID would be the obvious choice, and vice versa. 88 In order to analyze the amount of time it takes to complete a simulation of each controller, a ?tic-toc? Matlab function was applied. The ?tic?, where Matlab starts recording the time, was placed right before the for-loop in the main program. The ?toc?, where Matlab stops timing the system, was placed directly after the for-loop was closed. Note that this sums the entire time required to simulate the system, not just the computational expense of evaluating the control. The other computations involved in each simulation should be similar, with the main difference being the controller evaluation. The placement ensured that the plotting of the variables is not taken into effect. Table 5 demonstrates the results of the controller runtimes, while Figures 21a-b graph the data for each set of gains. Note that in all cases the dynamic-inversion controller runs around 30% longer than the PID controller. It is the author?s belief that the structure of the dynamic-inversion controller program that causes the increase in runtime. When the PID controller calls the function ?control_develop?, the control is directly calculated through linear equations and returned to the main program for future processing. However, when the dynamic-inversion controller calls ?control_develop?, the function then must call the function ?model_components?, compute relatively large nonlinear system components (mass matrix, Coriolis terms, etc.) for both the commanded and actual system, return those values to ?control_develop? where another large equation is used to calculate the control 89 before it is sent back to the maim program for processing. These extra steps and large calculations attributed to the increase in runtime. Table 5: Analysis of Runtimes Commanded Input Type Constant Input Step (Impulse) Input Sinusoidal Input Gains Used Controller Type 10 sec 10 sec 10 sec PID 1.20552 1.20392 1.22571 Kp = 200 Kd = 50 Dyn. Inv. 1.57709 1.57208 1.56319 PID 1.22680 1.20592 1.20208 Kp = 400 Kd = 100 Dyn. Inv. 1.56431 1.60439 1.59235 90 Figure 21: a) Runtime Comparison for PID and DI with KP = 200, KD = 50 0.00000 0.20000 0.40000 0.60000 0.80000 1.00000 1.20000 1.40000 1.60000 1.80000 Constant Input Step (Impulse) Input Sinusoidal Input To tal R un tim e ( sec ) PID: Kp = 200, Kd = 50 DI: Kp = 200, Kd = 50 Figure 21: b) Runtime Comparison for PID and DI with KP = 400, KD = 100 0.00000 0.20000 0.40000 0.60000 0.80000 1.00000 1.20000 1.40000 1.60000 1.80000 Constant Input Step (Impulse) Input Sinusoidal Input To tal R un tim e ( sec ) PID: Kp = 400, Kd = 100 DI: Kp = 400, Kd = 100 91 On average, the time it takes to run the PID simulation is approximately 12% of real-time (it takes approximately 7.20 seconds to run a 60 second simulation). The time it takes the dynamic-inversion simulation to run is approximately 16% of real-time (approximately 9.60 seconds to run a 60 second simulation). HWIL systems work in real-time, as opposed to simulation time. For every minute long simulation, 2.40 seconds are lost due to the dynamic inversion program. This indicates that the dynamic-inversion controller could likely be implemented on a digital controller without great increase in cost. However, studies will have to be made to reduce the difference between the controller runtimes. 92 VIII. CONCLUSIONS Because of their ability to function well enough and its user-friendly simplicity, PID controllers are employed in the majority of industrial design. However, when the industrial environment does not require simplicity in a system, is there a nonlinear controller that can be rendered more effective. The dynamic-inversion controller developed in this thesis represents a controller capable of manipulating the movements of a three gimbaled, nonlinear system. Derived from Lagrange?s equations for kinetic and potential energies and Lyapunov global stability theory, the dynamic-inversion controller was tested. Results from these tests were compared with results obtained using a PID controller. The dynamic-inversion method consistently showed a higher quality of response for not only a wide range of gains, but also numerous types of commanded functions. The dynamic-inversion method showed less error through the simulation with faster convergence and larger damping in terms of the gimbaled motion and gain. The PID controller did execute faster than the dynamic-inversion, but the lag in the dynamic-inversion controller should not be enough to warrant a change in controllers. 93 Recommendations for future work would include applying and testing both PID and dynamic-inversion controllers to an actual flight table to analyze how the system results vary in the physical world. Also note that the experiments performed do not include a seeker head during the test. How would the extra mass and adjustment of the moments of inertial handle in a simulation and actual test site? One may also analyze the stresses present during a reverse torsion. The PID controller was consistently oscillating. For an active table, this would cause vibrations and noise. However, because the control in the dynamic-inversion was so large, would deceleration in the gimbals cause any unwarranted stress and strain on the system? Finally, the runtime of the dynamic- inversion was slightly higher than the run time of the PID controller. An examination on how that would affect performance may lead to a structure of the program that causes it to execute faster. 94 BIBLIOGRAPHY Astrom, Karl Johan. Control System Design: PID Control. 2002. Carter, J. and K. Willis. ?A History of Flight Motion Simulators used for Hardware-in- the-Loop Testing of Missiles.? In SPIE Conference on Technologies for Synthetic Environments: Hardware-in-the-Loop Testing III 1998, Orlando, FL. Collins, Jack A. Mechanical Design of Machine Elements and Machines. New York: John Wiley & Sons, Inc., 2003. Gutirrez, J. ?Proportional-integral-derivative Explained: Tuning PID Controls.? EDA Design Line 13 Apr. 2007. ?Hardware in the Loop Laboratories (HWIL).? EWSSA Integrated Products Team. 15 Sep. 2003. NavAir Weapons Division. 20 June 2007. ?High Performance Computing Workshop.? Developmental Test Command. 12 Aug. 2002. United States Army. 20 June 2007. ?Joseph Louis Lagrange (1736-1813).? Mathematicians of the Seventeenth and Eighteenth Centuries. Trinity College, Dublin. 22 May 2007 Looye, G. and H.-D. Joos. ?Design of Robust Dynamic-inversion Control Laws using Multi-Objective Optimization.? In AIAA Guidance, Navigation, and Control Conference and Exhibit 2001, Montreal CA. (AIAA-2001-4285). Marlin, Thomas E. Process Control: Designing Processes and Control Systems for Dynamic Performance. New York: McGraw-Hill, Inc., 1995. ?PID-Tutorial.? Control Tutorials for Matlab. 26 Aug. 1997. Carnegie Mellon and the University of Michigan. 15 May 2007. 95 Plett, G. ?Adaptive Inverse Control of Linear and Nonlinear Systems Using Dynamic Neutral Networks.? In IEEE Transactions on Neural Networks Vol. 14, No. 2 (Mar. 2003): 360-76. ?Three Axis Flight Motion Simulator Series S-450R-3.? Hardware-in-the-Loop Flight Motion Simulator. Acutronic. 30 May 2007. Visioli, A. and A. Piazzi. ?On the Use of Dynamic-inversion for the Improvement of PID Control.? In 16th IFAC World Congress on Automatic Control 2005, Prague CZ. Yan, L. and C. James Li. ?Robot Learning Control Based on Recurrent Neural Network Inverse Model.? In Journal of Robotic Systems Vol. 14, No. 3 (1997): 199-212. 96 APPENDICES 97 APPENDIX A: Complete Derivation of Equations of Motion Moments of inertia defined as: I Pc. # Axis therefore, I3z would be z-axis of pc. 3. ?? ?? ? ?? ?? ? ? ? ? ? ? ? ? ? ? ? ? = ?? ?? ? ?? ?? ? 1 1 1 11 11 ? ? ? cos0sin 010 sin0cos k j i K J I ?? ?? ?? ?? ? ?? ?? ? ? ? ? ? ? ? ? ? ? ? ? = ?? ?? ? ?? ?? ? 2 2 2 22 22 1 1 1 ? ? ? 100 0cossin 0sincos ? ? ? k j i k j i ?? ?? ?? ?? ? ?? ?? ? ? ? ? ? ? ? ? ? ? ? ?= ?? ?? ? ?? ?? ? 3 3 3 33 33 2 2 2 ? ? ? cossin0 sincos0 001 ? ? ? k j i k j i ?? ?? 98 Calculate Angular Velocities: ( ) ( ) ( ) ( ) ( ) ( ) 33213233232132133 33333323333213213 33222212213 3322113 222212212 22222212 22112 111 ?sincoscos?sincoscos?sin ??cos?sin?sin?coscos?sin ???cos?sin ??? ??cos?sin ??cos?sin ?? ? kji ikjkji ikji ikj kji kji kj j ?????????????? ??????????? ??????? ???? ?????? ????? ??? ?? &&&&&& &&&& &&&& &&& &&& && && & ?++++= +++?+= +++= ++= ++= ++= += = Determine Kinetic Energy: ? = = 3 12 1 h TT ???????? I [ ] [ ] { } ( ) ( ) ( ) }sincoscos sincoscossin{21 cossin21 cos sin 00 00 00 cossin21 2 1 0 0 00 00 00 0021 3 2 32132 3 2 323213 2 2133 2 2 222 22 122 22 12 2 21 21 2 2 2 221212 1 2 11 1 1 1 1 11 z yx zyx z y x y z y x I IIT IIIT I I I T IT I I I T ????? ???????? ????? ? ?? ?? ????? ? ?? && &&&& &&& & & & &&& & && ? ++++= ++= ?? ?? ? ?? ?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = = ?? ?? ? ?? ?? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = 99 Determine Potential Energy: ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? = ? ? ? ? ? ? ? ? ? ? ?= += ?= 1 1 1 2 2 2 1 1 1 ? ? ? cos0sin 010 sin0cos ? ? ? 2 1 4 2 1 4 2 k j i k j i ?? ?? ?pi? ?pi? ?pi? 100 ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) t t t y p ypt mghV LLh Lah LLh kiLjkiLr iLjiLr rrrrrr ah La LLLa hhh = ?? ? ?? ? ? ?+? ? ?? ? ? ? ??? ? ??? ? ? ? ?? ? ? ??= ?+= +?= ?+???= ????= ?=?+= = ?= ?+= += 211 2/1 1 2 2 11121124 222224 354435 2/1 2222 cos12cos214sin2cos12 cos1cossin coscoscos ? sin? cos?sin? sin? coscos ??sin?cos sin cos12 cos2 ??pi?pi?pi ??? ??? ?????? ?? ? ? ? --------------------------------------------------------------------------------- Lagrange?s Equations: fqqq =??+????? ? ? ??? ? ? ? VTT t &d d For 1? : 1 111d d ?= ? ?+ ? ?? ??? ? ??? ? ? ? ??? VTT t & ( ) ( ) ( ) ( ) ( ) ( ) ?? ? ?? ? ?? ? ?? ? ?+?? ? ?? ? ???? ? ?? ? ?? ???+ +++++= ? ?L??L???Lmg I???????I??????? I????I??I??I?t? // zy xyxy sincos1coscos122sinsincos122 ]}sincossincoscoscoscossincoscos sinsincossin[dd{ 2121 3323213233232321 3221322 2 122 2 1111 &&&& &&&&& 101 For 2? : 2 222d d ?= ? ?+ ? ?? ??? ? ??? ? ? ? ??? VTT t & ( ) ( ) ( ) ( ) ( ) { }?? ???????????????? ??????????? ????????????? cossin }sinsinsincoscoscossinsincoscos cossinsincossincos{ ]}cossincoscossinsincoscos[{ 2 332132132332132321 321213222 2 1222 2 1 33321323332321222 mgL II III IIIdtd zy xyx zyz +?++ ?++? ??+++=? &&&&&& &&&&& &&&&& For 3? : 3 333d d ?= ? ?+ ? ?? ??? ? ??? ? ? ? ??? VTT t & ( ) ( ) ( ) ( ) ( ) }coscossincoscos sinsincoscoscossincoscos sincossincoscos{]}sin[{ 332132132 3323213233232321 33213232132133 z zy yx I II IIdtd ???????? ?????????????? ??????????? &&& &&&&&& &&&&& ? ???+ ++??+=? Combine the q&& terms to find the mass matrix, M, the jiqq && terms and the potential terms to find the Coriolis vector, h, to give the equation form: ( ) ( ) fqqhqqM =+ &&& , 102 ( ) ( ) ( ) ( ) 0 sin sincoscos cossin sincoscossin 23 3213 3333212 333 33 2 33 2 222 33 2 33 2 22 2 322 2 111 332313 232212 131211 = = ?= = ++= +++++= ? ? ? ? ? ? ? ? ? ? = M IM IIM IM IIIM IIIIIIM MMM MMM MMM qM x zy x zyz zyyxxy ? ??? ?? ???? ( ) ( ) ( )( ) ( ) ( ) ( )( ) ( )( ) ( )( ) ( )( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )( )zyyzx yzzy zy yzzyx zyxyx yzzyx yz zyxyx yz IIIII IIIIh mgL II IIIII IIIIIh mgL IIIII II IIIII IIh h h h qqh 333 2 333 2 3221 3333 2 233332 22 13 2 333332 333 2 333 2 3231 33 2 33 2 32222 2 2 2 2/12/1 333 2 333 2 3232 33332 2 31 33 2 33 2 3222221 33332 2 21 3 2 1 sincoscos sincossincoscos cossin sincos2 cossincos sincossincos sincos1coscos122sinsincos122 sincoscos sincoscos2 sincossincos2 sincossin , ?+?+ +?+?= +? +?+?+ ???+??= ?+???? +?+?+ +? ++?+? +?= ? ? ? ? ? ? ? ? ? ? = ? ????? ??????? ?? ???? ????? ????? ??????? ????? ????? ?????? ???? && && && && & && && && & & 103 APPENDIX B: Complete Derivation of Dynamic-inversion Controller Kinetic Energy in Generalized Velocities ( ) ( ) jiijT qqMT &&&&& 2121, == qqMqqq Apply Lagrange?s Equations to Determine Mass Matrix and Coriolis Terms ( ) ( ) ( )qqqqq VTL ?= && ,, k kk fqLqLdtd =????? ? ? ??? ? ? ? & ( ) k k ji k ij iki fq Vqq q MqM dt d = ? ?+ ? ?? &&& 2 1 k k ji k ij ij j ki iki fq Vqq q Mqq q MqM = ? ?+ ? ?? ? ?+ &&&&&& 2 1 Equations of Motion in Index and Vector Notation kkiki fhqM =+&& where k ji k ij j ki k q Vqq q M q Mh ? ?+ ??? ? ??? ? ? ?? ? ?= && 2 1 ( ) ( ) fqqhqqM =+ &&& , Selected Lyapunov Function ( ) ( ) ( ) ( )dTddTdQ qqqqqqqq &&&& ??+??= 2121 104 Find Derivative to Determine if Global Asymptotically Stable ( )0?Q& ( ) ( ) ( ) ( )dTddTdQ qqqqqqqq &&&&&&&&& ??+??= ( ) ( )ddTdQ qqqqqq ?+??= &&&&&&& ( )ddd qqqqqq &&&&&& ??=?+? (Eq. B1) ( ) ( )( )ddddd qqhfqMq &&& ,1 ?= ? (Eq. B2) Substitute Desired Motion into the Equations of Motion ( ) ( ) dddddddd hqMqqhqqMf +=+= &&&&& , ( )dddd hfMq ?= ?1&& (Eq. B3) Substituting Eq. B2 and Eq B3 into Eq. B1 ( ) ( ) ( )ddddd qqqqhfMhfM && ??=?+??? ?? 11 ( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( )[ ]dddddd qqqqqMqqhqMqMqq,hfqMqMf -1d &&&& ?+???+= ? ,1 105 APPENDIX C: PID and Dynamic-inversion Controller Program global g d I1 I2 I3 m dt time_intervals ang_com angrate_com angaccel_com Kp Ki Kd ang_com_temp angrate_com_temp % Variables for the surrounding system g = 9.81; %m/sec^2 % Variables for the Flight Table Geometry d = 0.20; %m - distance of component offset from point of rotation I1x = 57.6257; %kg-m^2 I1y = 20.4346; %kg-m^2 I1z = 37.8028; %kg-m^2 I2x = 6.6928; %kg-m^2 I2y = 3.7280; %kg-m^2 I2z = 3.0442; %kg-m^2 I3x = 2.8279; %kg-m^2 I3y = 6.7884; %kg-m^2 I3z = 6.7884; %kg-m^2 I1 = [I1x 0 0;0 I1y 0;0 0 I1z]; I2 = [I2x 0 0;0 I2y 0;0 0 I2z]; I3 = [I3x 0 0;0 I3y 0;0 0 I3z]; m = 91.8523; %kg - mass of component 2 (yaw) f = [0;0;0]; %set the initial control = 0 for all components % Time Characteristics for Simulation i = 1; %Counter for the "for-loop" iter = 1; ti(i)=0.0; %sec - initial start time of the system tf = 5.0; %sec - final time of the system dt = 0.01; %sec - time steps for RK44 integration time_intervals = 1; %number of steps till control is adjusted tsteps=(tf-ti)/dt; %total number of iterations % Initial Position and Angular Rates of Gimbals (Commanded and Actual) ang_com_temp = [0.1;0.1;0.2]; angrate_com_temp = [0;0;0]; [commands] = commanded_values(i,0); ang_com = commands(1:3); angrate_com = commands(4:6); angaccel_com = commands(7:9); Ang_Com(:,i) = ang_com; AngRate_Com(:,i) = angrate_com; the1_0 = 0.4; %radians - Initial Position, Component 1 the2_0 = 0.4; %radians - Initial Position, Component 2 the3_0 = 0.4; %radians - Initial Position, Component 3 u_0 = angrate_com(1); %radians/sec - Initial Velocity, Component 1 v_0 = angrate_com(2); %radians/sec - Initial Velocity, Component 2 w_0 = angrate_com(3); %radians/sec - Initial Velocity, Component 3 the1(i) = the1_0; %Angle of Component 1, for graphing the2(i) = the2_0; %Angle of Component 2, for graphing the3(i) = the3_0; %Angle of Component 3, for graphing 106 u(i) = u_0; %Angular Velocity of Component 1, for graphing v(i) = v_0; %Angular Velocity of Component 2, for graphing w(i) = w_0; %Angular Velocity of Component 3, for graphing % PID Augmented Integration Term z = [0;0;0]; % Defining State Vector for Various Controllers controller = input('Enter "1" for PID, "2" for DI: '); if controller == 1 x = [the1(i);the2(i);the3(i);u(i);v(i);w(i);z]; elseif controller == 2 x = [the1(i);the2(i);the3(i);u(i);v(i);w(i)]; end; % Gains if controller == 1 Kp = 400; Ki = 400; Kd = 100; elseif controller == 2 Kp = 400; Ki = 0; Kd = 100; end; %Graphing Parameters time(i) = 0.0; ang_1(i) = x(1); ang_2(i) = x(2); ang_3(i) = x(3); control_1(i) = f(1); control_2(i) = f(2); control_3(i) = f(3); error_mag(i) = 0; error_norm_total = 0; % Integration Process for the RK44 for i = 1:tsteps ti(i+1) = ti(i) + dt; k1 = dt*deriv_con(controller,x,f); k2 = dt*deriv_con(controller,x+k1/2,f); k3 = dt*deriv_con(controller,x+k2/2,f); k4 = dt*deriv_con(controller,x+k3,f); x = x + (1/6)*(k1+2*(k2+k3)+k4); [commands] = commanded_values(i,ti(i+1)); ang_com = commands(1:3); angrate_com = commands(4:6); angaccel_com = commands(7:9); Ang_Com(:,i+1) = ang_com; AngRate_Com(:,i+1) = angrate_com; 107 [f,error_norm] = control_develop(controller,i,x); error_mag(i+1) = error_norm; error_norm_total = error_norm_total + error_norm; ang_1(i+1) = x(1); ang_2(i+1) = x(2); ang_3(i+1) = x(3); control_1(i+1) = f(1); control_2(i+1) = f(2); control_3(i+1) = f(3); time(i+1) = ti(i+1); end; error_norm_avg = error_norm_total/tsteps ************************************************************* function [k]=Controller_Main(controller,x,f) global g d I1 I2 I3 m dt ang_com angrate_com I1x = I1(1,1); I1y = I1(2,2); I1z = I1(3,3); I2x = I2(1,1); I2y = I2(2,2); I2z = I2(3,3); I3x = I3(1,1); I3y = I3(2,2); I3z = I3(3,3); the1 = x(1); the2 = x(2); the3 = x(3); t1d = x(4); t2d = x(5); t3d = x(6); M11 = I1y + (I2x+I3x)*(sin(the2))^2 + (I2y + I3y*(cos(the3))^2 + I3z*(sin(the3))^2)*(cos(the2))^2; M12 = (I3y-I3z)*cos(the2)*cos(the3)*sin(the3); M21 = M12; M13 = I3x*sin(the2); M22 = I3y*(sin(the3))^2 + I3z*(cos(the3))^2 + I2z; M23 = 0; M31 = M13; M32 = M23; M33 = I3x; M = [M11 M12 M13; M21 M22 M23; M31 M32 M33]; 108 h1 = (t2d^2)*sin(the2)*cos(the3)*sin(the3)*(I3z-I3y)... + t1d*t2d*2*cos(the2)*sin(the2)*(I2x-I2y+I3x -(I3y*(cos(the3))^2+I3z*(sin(the3))^2))... + t1d*t3d*(-2*(I3y-I3z)*cos(the3)*sin(the3)*(cos(the2))^2)... + t2d*t3d*cos(the2)*(I3x+I3y*((cos(the3))^2 -(sin(the3))^2)+I3z*((sin(the3))^2-(cos(the3))^2))... + m*g*(-(sqrt(2)/2)*d*(((1-cos((pi/2)-the1))^(-1/2))*sin((pi/2) -the1)*sin((pi/4)-0.5*the1)+sqrt(1-cos((pi/2)-the1))*cos((pi/4) -0.5*the1))+d*(1-cos(the2))*sin((pi/2)-the1)); h2 = (t1d^2)*cos(the2)*sin(the2)*(I2y-I2x -I3x+I3y*(cos(the3))^2+I3z*(sin(the3))^2)... + t1d*t3d*cos(the2)*(I3y*((cos(the3))^2 -(sin(the3))^2)+I3z*((sin(the3))^2-(cos(the3))^2)-I3x)... + t2d*t3d*2*(I3y-I3z)*cos(the3)*sin(the3)... + m*g*d*sin(the2)*cos((pi/2)-the1); h3 = (t1d^2)*(I3y-I3z)*(cos(the2))^2*cos(the3)*sin(the3)... + (t2d^2)*(I3z-I3y)*cos(the3)*sin(the3)... + t1d*t2d*cos(the2)*(I3x+I3y*((sin(the3))^2 -(cos(the3))^2)+I3z*((cos(the3))^2-(sin(the3))^2)); h = [h1;h2;h3]; ang_rate_dot = M\(f - h); if controller == 1 ang = [the1;the2;the3]; error = ang_com - ang; int_error = error; k = [t1d;t2d;t3d;ang_rate_dot;int_error]; elseif controller == 2 k = [t1d;t2d;t3d;ang_rate_dot]; end; ************************************************************* function [commands]=Controller_Main(i,t) global dt ang_com_temp angrate_com_temp % Commanded Angles and Rates for Sinusoidal Motion % ang_com = [0.5*sin(t);0.5*sin(t);0.5*sin(t)]; % angrate_com = [0.5*cos(t);0.5*cos(t);0.5*cos(t)]; % angaccel_com = [-0.5*sin(t);-0.5*sin(t);-0.5*sin(t)]; % Commanded Angles and Rates for Constant Motion ang_com = [0.1;0.1;0.2]; angrate_com = (ang_com - ang_com_temp)/dt; angaccel_com = (angrate_com - angrate_com_temp)/dt; ang_com_temp = ang_com; angrate_com_temp = angrate_com; % Step Function after a Given Time Period 109 % if i>=500 % ang_com = [0.2;0.2;0.5]; % end; commands = [ang_com;angrate_com;angaccel_com]; ************************************************************* function [ftemp,error_norm]=Controller_Main(controller,i,x) global ang_com angrate_com Kp Ki Kd ang = [x(1);x(2);x(3)]; vel = [x(4);x(5);x(6)]; angle_error = ang_com - ang; if i >= 0 error_norm = norm(angle_error); else error_norm = 0; end; if controller == 1 e = ang_com - ang; de = angrate_com - vel; z_vec = [x(7);x(8);x(9)]; ftemp = Kp*e + Ki*z_vec + Kd*de; elseif controller == 2 [qr,qdr,Mr,hr,fr,q,qd,M,h]=model_components(ang,vel); ftemp = M*(Mr\fr) + (h - M*(Mr\hr)) - M*(Kp*(q-qr)+Kd*(qd-qdr)); end; ************************************************************* function [qr,qdr,Mr,hr,fr,q,qd,M,h]=control_develop(the,vel) global g d I1 I2 I3 m dt ang_com angrate_com angaccel_com I1x = I1(1,1); I1y = I1(2,2); I1z = I1(3,3); I2x = I2(1,1); I2y = I2(2,2); I2z = I2(3,3); I3x = I3(1,1); I3y = I3(2,2); I3z = I3(3,3); 110 %%%%%%%%%%%%%%%%%%%%% Reference Values %%%%%%%%%%%%%%%%%%%%%%%%%%% % Positional Variables the1r = ang_com(1); the2r = ang_com(2); the3r = ang_com(3); qr = [the1r;the2r;the3r]; % Velocity Variables t1dr = angrate_com(1); t2dr = angrate_com(2); t3dr = angrate_com(3); qdr = [t1dr;t2dr;t3dr]; % Acceleration Variables t1ddr = angaccel_com(1); t2ddr = angaccel_com(2); t3ddr = angaccel_com(3); tddr = [t1ddr;t2ddr;t3ddr]; % Returned Variables M11 = I1y + (I2x+I3x)*(sin(the2r))^2 + (I2y + I3y*(cos(the3r))^2 + I3z*(sin(the3r))^2)*(cos(the2r))^2; M12 = (I3y-I3z)*cos(the2r)*cos(the3r)*sin(the3r); M21 = M12; M13 = I3x*sin(the2r); M22 = I3y*(sin(the3r))^2 + I3z*(cos(the3r))^2 + I2z; M23 = 0; M31 = M13; M32 = M23; M33 = I3x; Mr = [M11 M12 M13; M21 M22 M23; M31 M32 M33]; h1 = (t2dr^2)*sin(the2r)*cos(the3r)*sin(the3r)*(I3z-I3y)... + t1dr*t2dr*2*cos(the2r)*sin(the2r)*(I2x-I2y+I3x- (I3y*(cos(the3r))^2+I3z*(sin(the3r))^2))... + t1dr*t3dr*(-2*(I3y-I3z)*cos(the3r)*sin(the3r)*(cos(the2r))^2)... + t2dr*t3dr*cos(the2r)*(I3x+I3y*((cos(the3r))^2- (sin(the3r))^2)+I3z*((sin(the3r))^2-(cos(the3r))^2))... + m*g*(-(sqrt(2)/2)*d*(((1-cos((pi/2)-the1r))^(-1/2))*sin((pi/2) -the1r)*sin((pi/4)-0.5*the1r)+sqrt(1-cos((pi/2) -the1r))*cos((pi/4)-0.5*the1r))+d*(1-cos(the2r))*sin((pi/2) -the1r)); h2 = (t1dr^2)*cos(the2r)*sin(the2r)*(I2y-I2x -I3x+I3y*(cos(the3r))^2+I3z*(sin(the3r))^2)... + t1dr*t3dr*cos(the2r)*(I3y*((cos(the3r))^2 -(sin(the3r))^2)+I3z*((sin(the3r))^2-(cos(the3r))^2)-I3x)... + t2dr*t3dr*2*(I3y-I3z)*cos(the3r)*sin(the3r)... + m*g*d*sin(the2r)*cos((pi/2)-the1r); h3 = (t1dr^2)*(I3y-I3z)*(cos(the2r))^2*cos(the3r)*sin(the3r)... + (t2dr^2)*(I3z-I3y)*cos(the3r)*sin(the3r)... + t1dr*t2dr*cos(the2r)*(I3x+I3y*((sin(the3r))^2 111 -(cos(the3r))^2)+I3z*((cos(the3r))^2-(sin(the3r))^2)); hr = [h1;h2;h3]; fr = Mr*tddr + hr; %%%%%%%%%%%%%%%%%% Actual Values %%%%%%%%%%%%%%%%%%%% % Positional Variables the1 = the(1); the2 = the(2); the3 = the(3); q = [the1;the2;the3]; % Velocity Variables t1d = vel(1); t2d = vel(2); t3d = vel(3); qd = [t1d;t2d;t3d]; % Returned Variables M11 = I1y + (I2x+I3x)*(sin(the2))^2 + (I2y + I3y*(cos(the3))^2 + I3z*(sin(the3))^2)*(cos(the2))^2; M12 = (I3y-I3z)*cos(the2)*cos(the3)*sin(the3); M21 = M12; M13 = I3x*sin(the2); M22 = I3y*(sin(the3))^2 + I3z*(cos(the3))^2 + I2z; M23 = 0; M31 = M13; M32 = M23; M33 = I3x; M = [M11 M12 M13; M21 M22 M23; M31 M32 M33]; h1 = (t2d^2)*sin(the2)*cos(the3)*sin(the3)*(I3z-I3y)... + t1d*t2d*2*cos(the2)*sin(the2)*(I2x-I2y+I3x -(I3y*(cos(the3))^2+I3z*(sin(the3))^2))... + t1d*t3d*(-2*(I3y-I3z)*cos(the3)*sin(the3)*(cos(the2))^2)... + t2d*t3d*cos(the2)*(I3x+I3y*((cos(the3))^2 -(sin(the3))^2)+I3z*((sin(the3))^2-(cos(the3))^2))... + m*g*(-(sqrt(2)/2)*d*(((1-cos((pi/2)-the1))^(-1/2))*sin((pi/2) -the1)*sin((pi/4)-0.5*the1)+sqrt(1-cos((pi/2)-the1))*cos((pi/4) -0.5*the1))+d*(1-cos(the2))*sin((pi/2)-the1)); h2 = (t1d^2)*cos(the2)*sin(the2)*(I2y-I2x -I3x+I3y*(cos(the3))^2+I3z*(sin(the3))^2)... + t1d*t3d*cos(the2)*(I3y*((cos(the3))^2 -(sin(the3))^2)+I3z*((sin(the3))^2-(cos(the3))^2)-I3x)... + t2d*t3d*2*(I3y-I3z)*cos(the3)*sin(the3)... + m*g*d*sin(the2)*cos((pi/2)-the1); h3 = (t1d^2)*(I3y-I3z)*(cos(the2))^2*cos(the3)*sin(the3)... + (t2d^2)*(I3z-I3y)*cos(the3)*sin(the3)... + t1d*t2d*cos(the2)*(I3x+I3y*((sin(the3))^2 -(cos(the3))^2)+I3z*((cos(the3))^2-(sin(the3))^2)); h = [h1;h2;h3];