Calhoun: The NPS Institutional Archive Theses and Dissertations Thesis Collection 1994-03 Design of a GPS aided guidance, navigation, and control system for trajectory control of an air vehicle Hallberg, Eric N. Monterey, California. Naval Postgraduate School http://hdl.handle.net/10945/26457 DUDLEY KNOX LIBRARY NAVAL POSTGRADUATE SCHOOL MONTEREY CA 93943-5101 Approved for public release; distribution is unlimited Design of a GPS Aided Guidance, Navigation, and Control System for Trajectory Control of an Air Vehicle by Eric N. Hallberg Lieutenant, United States Navy B.S. University of Pennsylvania, 1984 Submitted in partial fulfillment of the requirements for the degree of MASTER OF SCIENCE IN AERONAUTICAL ENGINEERING from the NAVAL POSTGRADUATE SCHOOL March Department , 1994 of Aeronautics u and Astronautics UNCLASSIFIED SU^ITV CDSSIFIOTIBH 6F THIs pagE REPORT DOCUMENTATION PAGE REPORT SECURITY CLASSIFICATION i. RESTRICTIVE MARKINGS lb. UNCLASSIFIED SECURITY CLASSIFICATION AUTHORITY i. 3. DISTRIBUTION/AVAILABILITY OF REPORT Approved DECLASSIFICATION/DOWNGRADING SCHEDULE >. for public release; distribution PERFORMING ORGANIZATION REPORT NUMBER(S) NAME OF PERFORMING ORGANIZATION i. 6b. 5. OFFICE (if CA NAME OF MONITORING ORGANIZATION 7b. Naval Postgraduate School ADDRESS (City, State, and ZIP Code) Monterey, 8b. OFFICE SYMBOL ORGANIZATION ADDRESS 7a. 93943 NAME OF FUNDING/SPONSORING . (if (City, State, unlimited. applicable) AA Naval Postgraduate School ADDRESS (City, State, and ZIP Code) Monterey, SYMBOL is MONITORING ORGANIZATION REPORT NUMBER(S) 9. CA 93943 PROCUREMENT INSTRUMENT IDENTIFICATION NUMBER applicable) and ZIP Code) SOURCE OF FUNDING NUMBERS TASK PROJECT ELEMENT NO. NO. NO. 10. WORK PROGRAM TITLE (Include Security . Design of a GPS UNI"! ACCESSION Classification) Aided Guidance, Navigation, and Control System for Trajectory Control of an Air Vehicle PERSONAL AUTHOR(S) . la. LT. Eric N. Hallberg 14. DATE OF REPORT (Year, Month, Day) 13b. TIME COVERED FROM 01/92 TO 03/94 March 1994 TYPE OF REPORT Master's Thesis SUPPLEMENTARY NOTATION . The views expressed Icial policy or position of the COSATI CODES GROUP FIELD PAGE COUNT 119 15. in this thesis are those of the author and do not reflect the Department of Defense or the United States Government. 18. SUBJECT TERMS (Continue on reverse if necessary and identify by block number) SUB-GROUP Trajectory Control, GNC, Linear Quadratic Regulator, LQR, Kalman Filt Nonlinear Simulation, ^-Implementation, Bluebird ABSTRACT ('Continue on reverse if necessary and identify by block number) The advent of GPS has afforded the aerospace controls engineer a powerful, new means of controlling air vehic tiis work explores a new method of designing and implementing controllers and guidance systems for autonomous con l. air vehicles utilizing terest when a GPS integrated guidance, navigation and control system. realizing controllers to track reference trajectories given in iplementation, and dynamic simulation of a inertial reference This is a subject of consider* inertial reference frame. an Unmanned commands and other measured outputs (such The des: Air Vehicle (Uj precise tracking trajectory controller for presented. This design provides a natural conversion of )m an an as GPS sign frame to a body-fixed reference frame. This achieves automatic recruiting of the actual ule preserving the properties of the original design (linearization principle). i. *] a. DISTRIBUTION/AVAILABILITY OF ABSTRACT UNCLASSIFIED/UNLIMITED DsAME AS RPT. NAME OF RESPONSIBLE INDIVIDUAL Isaac I. APR ABSTRACT SECURITY CLASSIFICATION UNCLASSIFIED 22b. K aminer 83 21. DTIC USERS TELEPHONE (Include Area Code) 408) 656-2972 edition may be used until exhaust ed All other editions are obsolete 22c. OFFICE SYMBOL AA/KA :l a sffflcTrlo'TOFTHTSF UNCLASSLFLED ABSTRACT The advent means of GPS has afforded the aerospace controls engineer a powerful, new of controlling air vehicles. This work explores a implementing controllers and guidance systems utilizing a GPS an of designing autonomous control when The design, implementation, of a precise tracking trajectory controller for an Unmanned a subject and dynamic simulation Air Vehicle mmands sented. This design provides a natural conversion of co GPS is realizing controllers to track reference trajectories given inertial reference frame. outputs (such as and of air vehicles integrated guidance, navigation and control system. This of considerable interest in for new method signals) from an inertia! reference (UAV) is pre- and other measured frame to a body-fixed refer- ence frame. This achieves automatic recruiting of the actuators while preserving the properties of the original design (linearization principle). ui 6,1 TABLE OF CONTENTS I. INTRODUCTION 1 II. DEVELOPMENT OF THE DYNAMIC MODEL 4 A. III. IV. REFERENCE FRAMES 4 1. Local Tangent Plane Reference Frame 4 2. Body-Fixed Reference Frame 5 3. Flight Path or Wind Reference Frame 6 B. COORDINATE TRANSFORMATIONS 7 C. NOTATION 9 D. RIGID BODY EQUATIONS OF MOTION 10 1. Linear Motion 10 2. Angular Rotation 11 3. External Forces and 4. The State Space Representation 14 5. Trim and Linearization 19 Moments 12 THE LINEAR QUADRATIC REGULATOR DESIGN 23 A. LQR OVERVIEW 23 B. DESIGN REQUIREMENTS 27 C. THE SYNTHESIS MODEL 28 D. THE DESIGN PROCESS 32 E. LQR CONTROLLER PERFORMANCE 34 CONTROLLER IMPLEMENTATION ON THE NONLINEAR PLANT A. ^-IMPLEMENTATION B. ^-IMPLEMENTATION OF THE CONTROLLER 41 41 iv IN SIMULINK 47 MONTEREY CA C. GENERATION OF THE TRAJECTORY COMMANDS 48 D. STATE FEEDBACK TO OUTPUT FEEDBACK 54 E. V. 93943-510! 1. Sensor Modeling 54 2. Kalman 58 Filtering INTEGRATION OF THE FULL NONLINEAR SIMULATION . . APPLICATION TO THE CONTROL OF BLUEBIRD A. 63 P-IMPLEMENTED CONTROLLER PERFORMANCE CHARACTERISTICS B. 63 AN AIRPORT DEPARTURE AND ARRIVAL FLIGHT SIMULATION VI. 60 72 CONCLUSIONS AND RECOMMENDATIONS 78 A. CONCLUSIONS 78 B. RECOMMENDATIONS 79 APPENDIX A MATLAB APPENDIX B SIMULINK FILES APPENDIX C D-IMPLEMENTATION PROOFS REFERENCED FILES 81 98 ... 100 REFERENCES 106 INITIAL DISTRIBUTION LIST 107 LIST OF TABLES 3.1 EIGENVALUES OF BLUEBIRD 29 3.2 EIGENVALUES OF BLUEBIRD WITH YAW DAMPER 30 3.3 TRANSMISSION ZEROS OF SYNTHESIS MODEL 34 3.4 EIGENVALUES OF THE FEEDBACK SYSTEM 35 3.5 COMMAND AND CONTROL BANDWIDTHS 38 4.1 ACCELEROMETER CHARACTERISTICS 55 4.2 ANGULAR RATE SENSOR CHARACTERISTICS 56 4.3 INCLINOMETER AND MAGNETOMETER CHARACTERISTICS 56 VI LIST OF FIGURES 2.1 Local Tangent Plane Coordinate System 5 2.2 Body-Fixed Coordinate System 6 2.3 Wind 2.4 SIMULINK Nonlinear Sixteen State Dynamic Model of an Air Vehicle 20 2.5 SIMULINK Nonlinear Nine State Dynamic Model of an Air Vehicle 21 3.1 Standard 3.2 Feedback Configuration 3.3 Synthesis and Analysis Model 31 3.4 Control-Loop Bandwidth: Elevator Channel 35 3.5 Control-Loop Bandwidth: Throttle Channel 36 3.6 Control-Loop Bandwidth: Aileron Channel 36 3.7 Command- Loop Bandwidth: X Position Channel 37 3.8 Command-Loop Bandwidth: Y Position Channel 37 3.9 Command-Loop Bandwidth: Z Position Channel 38 3.10 Bank Angle and Heading Response 3.11 Pitch or Flight Path Reference Frame 7 LQR feedback configuration for . 24 Root Locus Analysis 27 to Ramp in Y Command 39 Angle and Altitude Response to Ramp in Z Command 39 3.12 Trajectory Error Due to a Constant Wind Disturbance 4.1 Block diagram of the nonlinear controller 4.2 V- Implementation 4.3 Commanded 4.4 Example 4.5 Generation of Trajectory 4.6 Angular Rate Sensor of K (A) of controller on Bluebird Trajectory Logic Block Commanded 40 46 49 50 Trajectory Revision 52 Commands 53 55 vii SIMULINK 4.7 Sensor Models in 4.8 Frequency Response of Position Filter 59 4.9 Frequency Response of Euler Angle Filter 60 4.10 SIMULINK Diagram 62 5.1 Test Trajectory 5.2 Trajectory #1: Position Error and Euler Angles 66 5.3 Trajectory #1: Velocity Data 67 5.4 Trajectory #1: Control Activity 68 5.5 Test Trajectory 5.6 Trajectory #2: Position Error and Euler Angles 70 5.7 Trajectory #2: Velocity Data 71 5.8 Trajectory #2: Control Activity 72 5.9 Trajectory #2: Position Error for Varying Turn Rates 73 57 of the Full Nonlinear Simulation #1 65 #2 69 5.10 Trajectory #2: Position Error for Varying 5.11 Departure and Arrival at Wind Velocities an Airfield 5.12 Airfield Scenario: Position Error and Euler Angles 5.13 Airfield Scenario: Control Activity vui 74 75 76 77 ACKNO WLED GMENT There are a few special people would not have been completed. It I would like mention without whom this project was a true pleasure to have studied aeronautics under Professor Isaac Kaminer and Professor Rick Howard. Their advice and professional counsel were invaluable. whose vision and expertise are avionics lab dividuals where who this I am work was accomplished. best while understanding the least. you I Kaminer directly responsible for the creation of the superb sacrificed the most. patience; without especially grateful to Professor Isaac To Finally, a special thanks to five in- Nicole, Katie, Eric, And to Patricia, and Patty, you were the thank you would not have been able to complete IX for your exceptional this project. INTRODUCTION I. The advent means of GPS has afforded the aerospace controls engineer a powerful new Current guidance schemes rely in some part on of controlling air vehicles. ground based radars, navigational and control of the onboard GPS capabilities. integrated The beams, Guidance etc. have been developed seperately and then combined in air vehicle a somewhat adhoc process. aides, beacons, localizer Precise tracking of inertial fixed trajectories using an GNC suite affords a quantum leap in autonomous flight greatest impact of the proposed technology autonomous unmanned of trajectory tracking control for is expected in the area vehicles, and automatic approach and landing of manned vehicles dicussed next. Control of the commercial air more and more demanding to the as traffic throughout the country continues to become an increased number of vehicles vie for limited access major commercial aviation hubs. Sophisticated and expensive ground based radar control facilities employ a large number of personnel to individually instruct the pilots of these aircraft on the trajectories they are to aircraft trajectory such as required by an approach into an attention from a ground based air traffic controller. effectively control the aircraft equipment available and Airfields is is fly. Precise control of the airfield requires Furthermore, ATC constant ability to influenced by ground based radar coverage and navaid often negatively influenced by atmospheric conditions. with limited resources are often unable to take-off and land aircraft requiring instrument departures and arrivals. Flight patterns around major aviation hubs are, in general, inertia! based trajectories. That is, an aircraft is required to track a certain path over the ground while adhering to a certian altitude schedule, irrespective of air mass disturbances. In some such as on cases, approach to land or when the aerodrome final significant terrain or cultural is utilizing a GPS integrated among development, precise adherence to the desired trajectory In any case, crucial for flight safety. situated is GNC commanding an inertial trajectory directly system could prove to be more cost effective and accurate than current methods. Furthermore, such a guidance scheme could open up many more airports to significant commercial air traffic without requiring the capital investment and maintenance of ground based radar Unmanned tionally, in in the of some air vehicles can be a cost effective means of power projection. Addi- human physiological cases performance of an paramount importance limits The air vehicle. as facilities. may prove to be the limiting factor precision delivery of munitions becomes weapons and weapon delivery platforms continue to in- crease in cost, thus limiting their numbers. All of these concerns can be addressed by autonomous a air vehicles utilizing GPS aided guidance, navigation and control suite. All of these applications have a the particulars of the vehicles tonomous control of may their trajectory. common thread running through them. While vary considerably, the intent As a proof of concept, this is to acheive au- work presents a new design process for the synthesis of a guidance, navigation, and control system for a UAV named jectories. Bluebird. Bluebird is Postgraduate School. and air is a The function of the this UAV operated at It the GNC system Unmanned is to track inertial tra- Air Vehicle Lab at the Naval has a 12.5 foot wingspan and a 20 pound payload capability, currently being equipped with a full avionics suite, including IMU, GPS, and data sensors. The design process began with the development of a nonlinear Bluebird implemented in SIMULINK. A dynamic model of typical cruise flight condition was chosen as the point for linearization. After linearization of the nonlinear model, the work cen- tered on the design of a linear controller. approach was used since controller within the sign of the it provides an intuitive framework of LQR controller, inertial to means synthesis of synthesizing a multivariable real world design constraints. Following the de- the challenge of implementing the linear controller on the nonlinear plant was addressed. from LQR (Linear Quadratic Regulator) A novel method of converting commands body reference cooridinates was used [Ref. 10]. and outputs This method achieves automatic recruiting of the actuators, while preserving a certain linearization property. Next, the accuracy of the nonlinear simulation was enhanced with the addition of high fidelity models of sensors used onboard Bluebird. Additionally, Kalman filters were designed in order to provide optimal state estimates. Finally, the performance of the controller was evaluated in simulations with the full nonlinear model. DEVELOPMENT OF THE DYNAMIC MODEL II. The development of an integrated guidance, navigation, and control system required a high fidelity nonlinear model of the aircraft dynamics. The discussion begins with an explanation of nomenclature, abbreviations, and a definition of frames of reference. REFERENCE FRAMES A. Three different reference frames are used in this report. They are: • Local Tangent Plane or Inertial Reference • Body- Fixed Reference Frame • Wind 1. or Flight Path Reference Frame Frame Local Tangent Plane Reference Frame The position of the air vehicle must be maintained with respect to the tangent plane coordinate system. This coordinate system ray from the center of the earth to its surface. east, the positive up. This is it will formed by extending a A plane is attached tangent of intersection of the ray with the Earth's surface. our purposes here is While it is local somewhat to the point arbitrary, for be convenient to define the positive x direction as pointing y direction as pointing north, and the positive depicted in Figure z direction as pointing 2.1. For the purposes of this development, the rotation of the earth and its associated Coriolis' forces can be ignored and the local tangent plane reference frame prime meridian equator Figure 2.1: Local Tangent Plane Coordinate System can be considered to be an inertia! reference frame. In this work, {/} is used to represent the inertia! reference frame. 2. Body-Fixed Reference Frame The body-fixed reference frame is a right hand orthogonal system with the origin at the center of gravity of the air vehicle. The positive x direction points towards the nose. The positive y direction points out the right wing and the positive z direction points towards the bottom of the air vehicle. The velocity of the air vehicle with respect to the inertia! reference frame, resolved along the body-fixed reference frame, are termed u, v, and x, y, w, respectively. and z axis of the The angular rate of rotation of the air vehicle with respect to the inertia! reference frame, resolved in the body-fixed reference frame, are called forces, are moments, angular shown in Figure 2.2. reference frame. rates, The and p, q, and r, respectively. Positive values for the linear velocities in the body-fixed reference abrieviation, {#}, is frame used to represent the body-fixed Yb zb q r components Aerodynamic force components u V w X Y Z Aerodynamic moment components L M N Moment "» y •i 'yz >« >«r 2.2: of inertia Body-Fixed Coordinate System Flight Path or The wind Yaw Axis P about each axis Products of inertia 3. Pitch Axis Angular rates Velocity Figure Roll Axis Wind Reference Frame reference frame is also a right hand orthogonal system with origin at the center of gravity, e.g., of the air vehicle. velocity vector of the air vehicle. The for a and (3 The x orientation of the respect to the body-fixed reference frame The equations [Ref. 11] is axis is its aligned with the wind reference frame with defined in terms of the angles a and /?. are given below. a= (w/u) (2.1) sin- (v/V) (2.2) tan and /3 = l where the vectors Figure 2.3. The Figure B. u, v, w, and abrieviation, 2.3: Wind V are velocity {W}, is components of the air vehicle defined in used to represent the wind reference frame. or Flight Path Reference Frame [Ref. 11] COORDINATE TRANSFORMATIONS In order to use these three coordinate systems, one between them freely. The Euler angles, must be able $,0, and #, termed to transform pitch, roll, and yaw, are defined in order to express the orientation of the body-fixed reference frame with respect to the inertial reference frame. For the purposes of this development, a 3-2-1 Euler angle transformation will suffice as grees. The 3-2-1 transformation is its given without explanation but a good development of Euler angle transformations in general can be found in [Ref. the angular rotation is equal to 90 de- singularity occurs at more apparent when the transformation 6]. is The nature of expressed as the product of three rotation matricies. In the case of a 3-2-1 rotation sequence, the three matrices in Equation 2.3 correspond to rotations about the yaw, pitch, and of the air vehicle. Of course, the three matrices can be multiplied out for result contained in a single matrix , although the resulting matrix is roll axes an analytic somewhat busy to inspect. In any case, the transformation between a free vector resolved in the inertial reference is frame and the same vector resolved in the body-fixed reference frame given by: cos l V= \P - sin $ sin cos $ # The l V is inverse the inverse Not 1 $ — sin $ cos 1 cos sin 1 where — sin cos a free vector resolved in {/} and B V is is also denned. is the V $ cos $ sin same vector resolved Conveniently, since the transformation is in transformations, however, are orthonormal. the case of angular rotation rates. The body-fixed Of {B}. orthonormal, simply the transpose of the rotation matrices shown in Equation all, (2.3) 2.3. particular interest is reference frame's angular rate of rotation with respect to the inertia! reference frame can be related to the rate of change of the Euler angles by a transformation matrix. The development forward and q, is fully explained in [Ref. 11]. final sin $ tan cos cos $ tan — sin $ $ sin$ sec0 straight transformation matrix from r to the time rate of change of the Euler angles, $, 0, ^, 1 By The is cos$ sec0 is p, given by: P (2.4) q r integrating Equation 2.4, the time history of the Euler angles can be obtained. Aerodynamic forces and moments are often calculated using derivatives defined with respect to the define the orientation of the stability and control wind reference frame. The angles, a and /?, wind reference frame to the body-fixed reference frame. 8 Therefore, a transformation matrix can be obtained that relates a free vector, such as lift is or drag, resolved in expressed {W} to the v= {B}. The transformation a cos ft — cos a sin /? — sin a sin sin is in as: cos where B V same vector resolved cos f3 a cos (3 /? — sin a sin /? a free vector resolved in {£} and ^V cos is wV (2.5) a the same vector resolved in {W}. NOTATION C. Some standardized abbreviations will simplify the development of the nonlinear kinematic model of the air vehicle. This short-hand where multiple frames of reference are common • Peg represents the position vector is used in the field of robotics [Ref. 5]. from the origin of the local tangent plane to the center of gravity of the air vehicle. • Vcg and B a cg represent the velocity and acceleration, measured gravity of the air vehicle, with respect to {/}, resolved in {B}. of • I v cg Vcg are commonly termed u, v, and at the center of The components w. and I a cg represent the velocity and acceleration, measured at the center of gravity of the air vehicle, with respect to {/}, resolved in {/}. • b ub is the angular velocity of the resolved in {B}. The components {B} coordinate system with of b ub are commonly termed • i ub represents the angular velocity of the to {/}, resolved in {/}. respect to {/}, p, q, and {B} coordinate system with r. respect • gR {B}, in {/}. The inverse in • {W}, in B F and B {B}. The inverse • • BL I L is is represented by g^R. {B}. the total external inertial force and the inertial angular the inertial angular its is momentum of u, its derivative the fixed reference frame. 1. is denoted as is denoted as Jj(u) (v) BODY EQUATIONS OF MOTION A strapdown IMU, as among motion of the acting on the resolved in { /}. with respect to {B} In general, an avionics suite on a Therefore, moment the body resolved in {B}. momentum of the body derivative with respect to {/} RIGID free vector resolved in {/}. Given a vector and D. in *F and *N denote body resolved • represented by B R N denote the total external inertial force and moment acting on the body resolved • is yyR represents the transformation matrix used to express a in • represents the transformation matrix used to express a free vector resolved modern air vehicle utilizes name implies, maintains a constant The output other reasons, of the sensors it is on the a strapdown IMU. orientation in the body- IMU are resolved in {B}. most convenient to develop the equations of air vehicle in the body-fixed reference frame. Linear Motion An application of Newton's Law the total external force applied to a body inertial acceleration. is to linear motion of a body states that equal to the mass of the body times This could be written in the inertial reference frame 10 as: its where = 'a '*», (2.6) or in the body-fixed reference frame as follows: B rj? _ = __ m B„a = mfRti) eg m **<.,. = Coriolis' theorem can be used to (2.7) relate the inert ial and body accelerations of the air vehicle as follows: \ = -\ + where the difference in the derivatives is B Wfl x\ (2.8) S) explained in Chapter II, part C. Equation 2.8 can be substitued into Equation 2.7 in order to obtain the desired expression for the sum of the external inertia! forces resolved in the body-fixed reference frame. BF = m{- B v = m -Jl at 2. cg Bv c9 + B uB x B v cg ) + rn (B U)B x B v cg ). (2.9) Angular Rotation Euler's law for the conservation of angular gravity states that: 11 momentum at the center of = 'Leg where I L cg is the total is the angular moment momentum of the air vehicle with respect to {/} and N cg as: BL cg = fR'Nc,. (2.11) theorem can be used again to expand B L cg obtaining: BL eg = ^ BL cg + B uB x B L cg can be shown that the angular momentum, B L cg of the of an , inertia tensor, all denned as JB and , (2.12) . It ignored J applied to the air vehicle. Equation 2.10 can be written in the body-fixed reference frame Coriolis' (2.10) 'Nc,,, air vehicle is the product B the body's angular velocity, uB , where we spining elements. Substituting this definition of B L cg into Equation 2.12, results in: BL cg = B j (JB UB) + %J x Jb b ujb . (2.13) t Recall that B L^ = f&Ncg = B N^. Using this relationship, Equation 2.13 can be equivalently expressed as: B N cg = ^-(JB Bu B) + B uB x JB B u> B (2.14) at 3. External Forces and Moments Equation 2.9 and Equation 2.14 from the preceding sections can be compactly expressed as follows. 12 Bp m fBv + m cg t U UB + JBT dt t By B B x v cg ) B U) X U JB BB,U B B ( Bu bringing derivative terms in Equation 2.15 to the d_ dt hand side, we obtain, BF - b ujb X B v cg + -Jg B uB x Jb b ujb + JE^8 N v,eg 3 left (2.15) (2.16) 1 ub Next, the forces and moments in Equation 2.16 can be expanded as follows: Bp B B FgRAVITATIONAL + FpBOPULSIVE + FAERODYNAMIC b Npropulsive + Na 'AERODYNAMIC N , (2.17) where force due to gravity FgRAVITATIONAL b Ff PROPULSIVE BF force due to engine's thrust generated by aerodynamic surfaces force moment generated by engine's thrust moment generated by aerodynamic surfaces AERODYNAMIC ^PROPULSIVE B AERODYNAMIC J N The gravitational force expressed in {/} given by is Fgravity = mg where "g" is the gravitational constant. Then B Fgravity = jBnl R Fgravity- (2.18) The expansion of the propulsive forces and moments is ering the case of centerline thrust. In that case, no external i.e., Npropulsive = 0, and the propulsive forces can simplified moments be expressed by consid- are generated, as: T B Fprop = 13 (2.19) where T represents the thrust of the powerplant. Aerodynamic forces and moments are commonly calculated using nondi- mensional stability and control derivatives. These derivatives are obtained by approximating the aerodynamic forces and moments acting on the air vehicle using a Taylor series expansion about a given trim point. Typically, values for these derivatives are Sometimes, a few second available for the first order terms of the expansion only. order terms are available, such as the terms associated with a and /? [Ref. 7]. All other higher order terms are usually ignored in this approximation. In general, the aerodynamic forces are computed and moments acting on the The nondimensional as follows. stability air vehicle and control derivatives are dimensionalized by multiplication by the appropriate constants, such as wing span, dynamic pressure, chord, etc. The dimensional derivative is then multiplied by the perturbations of each aerodynamic variable or control deflection from trim point. The summation of the forces variables and control moments, results in the total 4. and moments due to all of the nominal aerodynamic deflections, in addition to the trim value of the forces aerodynamic force and moment acting on the and air vehicle. The State Space Representation In order to implement the dynamic model in a state-space form suitable for numerical simulation, the states of the model need to be chosen. This arbitrary its and many choices will work so long as consistency is is somewhat maintained in the approach. As was evident from the development of the rigid body equations the body-fixed reference frame define the states. The first is of motion, the most convenient coordinate system in which to three states are defined as the inertial velocity of the air vehicle resolved in the body-fixed reference frame. 14 These are abrieviated as u, v, and w more compactly or as Bv The cg . fourth through sixth states are defined as the angular velocity of the air vehicle with respect to {/} resolved in {B}. abrieviated as p, q, and more compactly r or These are b ub as Control inputs are represented by the vector A: A= where 6C 6r and 6a are the , 6r , 6a ] [6 e , elevator, rudder, , (2.20) and aileron inputs, respectively. Control inputs for the throttle are represented by 6tr t> Typically, the terms in the Taylor series expansion for the aerodynamic stability derivatives are partial derivatives with respect to where U is the magnitude of the air vehicle's velocity vector and angles defining the orientation of variables, p, u/U, a, qr, and r, {B} with respect to are states of the model. The {W} first /?, p, q, a and [Ref. 3]. The /3 and r, are the last three three can be represented as a combination of states in the model as follows. First note that, U and for small values of angles a and w no,,v _ — BWr>B "• cgi /?, a is (2.21) approximately equal to w/U and /? is approximately equal to v/U. It turns out, the stability derivatives can be placed in matrix form as follows: Cl v Cyu Cl p Cyp Cl Cya Cl p Cyp CL q Cy Co a Cd p Cd„ Cyt Cd f Ci« ^ip Cf« C'r ^m a ^m p ^m, ^m p C na Gn p C n , O nr dC Cdu dx' Ciu Cd p Ch ^mu ^rnp Cnu C np Similarily, for the control derivatives: 15 Ci Cl t \Cl„ Cl,t CL,a Cy,. Cvlr Cy,. Co,. CD Co ta dC dA~ . c«. Ci lr Ci Sa Cmg e Crns r Crn Sa cnge Cn tr Cn fa where x' forces and moments can be expressed B where is the vector composed of u/U, a, Faebo NAERO M\ derivatives q, = qS %R frR and 5 ir /?, . and p, q, r. Now the aerodynamic as follows: ^ + S M + l>i+ lfA '* are matrices used to dimensionalize the stability and convert the >' < 2 22 > - and control state vector, x, to x': = q I = u v w T p q r . ] dynamic pressure, 5 = diag{— 5, 5, —5, 56, Sc, 56}, x' M' = diag{l/FT , = M'x, 1/Vt, 1/Vt, b/2VT c/2VT b/2VT }, , , and x' M' = = M'x, diag{0, C /(2VT ),6/(2yT ), 0,0,0}. Equation 2.16 can now be further expanded using expressions of the forces and moments derived above. 16 B d_ dt Vcg 'eg B + - b Jb\ b »b* b Jb b ub) UB BF B Js B l (2.23) N where BF B BF GRAV N -{ &R ^R { qS { Notice that there is r + bf PROP CFO + %M'x + %M>x + Strt+ |f A (2.24) . } } } a state derivative term on the right hand side of Equa- tion 2.23 due to the second order terms in the Taylor series expansion of the aerody- namic forces and moments in Equation 2.24. By bringing it to the left hand side of Equation 2.24 and combining terms, we obtain: d_ Ucg dt *UB - b ubx 1 b -*Jb (b ubX b Jb ub) -'"{[I MJ^TqS^M' b B B x 'eg + M-{ b Fgrav + + U>B Fprop + ^TqS{CFo + Stn &A) (2.25) where: rp — w1 _ B ByR &R and Mi = m BJ B and X= /e-M,-Wgf AT. Equation 2.25 expresses the derivative of the in matrix form. It is first six solved in the user denned 17 (2.26) states of the nonlinear MATLAB Fen model block, state-deriv.m, which is Appendix A. The physical parameters available for inspection in to Bluebird are stored in a MATLAB within the function, state-deriv.m .m file, Bluebird-data. m, and are specific called from In order to change the vehicle being modeled, . one simply changes the constants in Bluebird-data.m . Next, the Euler angles were added as the additional three states of the model. The time history of the Euler angles in compact form is denned Equation 2.4 and written in as: A = S(A)%,, (2.27) where A= and sin 1 5(A) = $ tan cos $ sin$ sec0 The user defined MATLAB Fen $ tan — sin $ cos cos$ sec0 block, eu/.m, solves Equation 2.27, the details of which can be found in Appendix A. Finally, the inertia! position of Bluebird lb r cg The rotation matrix, J can be computed as follows. _/ — „v cg —-ipB„ B*l Veg- B R, is (2.28) implemented in a MATLAB posb2i.m, in order to convert the vector B vcg to the vector is I v cg . then integrated to obtain a time history of the position of the local tangent plane. To four actuators, levator, of 1/12. The increase fidelity of the simulation, a Kidder, resulting nonlinear ^aileron, ^throttle, is first The block, vector J Vcg air vehicle in the order model of the included with a time constant dynamic model now has sixteen 18 Fen states which are computed using Equations states =[u d_ dt w v p 2.25, 2.27,and 2.28; r = [X B - fl Ypos Zpos 6epo , 6Tpot 6apot o; sx + - b Jb\ b ub* b Jb b ub) Mj^TqS^M' 'eg ] B UB BF P ROP Strt Fgrav + Mr 1 + { + ^TqS{CFo + ff A) A = S(A) B uB }} (2.29) (2.30) , (2.31) eg- The SIMULINK diagram 5. 6 tpo , T P T Ad*. "l A poa »-{[[ U>B for clarity: il $ Q V Xpos q 'eg B summarized here of the nonlinear model is shown in Figure 2.4. Trim and Linearization For linear controller design, the nonlinear equations above must be trimmed and linearized available The for a typical cruise flight condition for Bluebird. A SIMULINK tool is which can be used to find equilibrium points of nonlinear dynamic models. user specifies which states and control inputs are to remain fixed along with their stationary values and the trim routine searches for values of the state vector for which the derivative of the state vector equals zero. known, another SIMULINK in order to find the rate of resulting linear model is tool perturbs the states and input With the trim condition around the specified trim point change of the states and control inputs (Jacobian). The returned in state space format. Since Equation 2.28 can only 19 w+ ^ w 1 Mux * u > s w Elevator V w Rudder w p Aileron q Throttle r phi Returns Derivative of [u v wpq - r] theta psi posx posy posz Returns Derivative of Euler Angles elevator rudder aileron throttle Returns Derivative of Position in Rotate Velocity t{1} in {B}to{l} First Order Actuators Figure 2.4: Vehicle SIMULINK Nonlinear Sixteen State Dynamic Model of an be trimmed for v^ and 2.27, = 0, not a typical flight condition, and then include Equation 2.28 We we will Air trim Equations 2.25 for linearization. are interested in trimming the model in velocity. Hopefully, the deriva- tive of the position states will never equal zero in flight. Also, in trim, the control inputs are equal to the actuator positions. Therefore, actuator states are also removed from the nonlinear model. The nine state nonlinear model of Bluebird used is shown in Figure 2.5. A typical cruise flight condition for Bluebird 20 is given by: for trim u V w p Mux q r phi theta psi d(Euler)/dt=S(Euler)*w Equation 1.25 Figure 2.5: Vehicle SIMULINK Nonlinear Nine State Dynamic Model of an Air • flight speed equal to 73 feet per second • flight path angle equal to zero • wings level attitude The trim The nonlinear model depicted in Figure 2.4 values of the nine states, v, control inputs, £ e , 6r , 6a , model cannot be trimmed The origin of {/} is u, w, p, q, r, was trimmed at $, 0, and #, and the four and 6t were returned. While the sixteen in position, this condition. state nonlinear can be linearized at an arbitrary position. it conveniently chosen. These values were then used to linearize the complete nonlinear model, including position and actuator states. 21 The resulting linear model of Bluebird and numerical values the Appendix B. 22 for the trim condition are included in III. THE LINEAR QUADRATIC REGULATOR DESIGN The previous chapter developed a model for a to derive a linear controller is cruise flight condition. In this chapter, a linear The dynamic developed to provide trajectory tracking for the linear model. methodology was selected to design the intuitive nonlinear model of Bluebird, which was used means following is of manipulating the controller. LQR gains is LQR Based on design requirements, an presented. See [Ref. a brief review of the properties of an LQR 9] for details. controller utilized in this design process. A. LQR OVERVIEW Consider the linear system x g =\ z y i where x € Suppose # n , u € BT C\D\ = = Ax + Bu = CiX + D lU = x (3.1) and z £ B?. and D\ column rank. Then, is full T z z = x T C?ClX + u T D[D x u. Define a cost, J, as follows: J= and let Q= CfCi, and Assume (C\,A) The standard LQR is J (z T z)dt = R = D\D X . J {x T C?ClX + u T DlD Note: Q observable and (A, problem is >0 and B) is R> u)dt = (3.2) 0. controllable. to find a controller, u 23 x Consider Figure 3.1. K(s)x, such that the feed back system in Figure 3.1 is internally stable such controller uses a constant gain, u and J = —Kx, K= is minimized. It turns out, one where -R- l B J P, (3.3) and P solves the Alegbraic Riccatti Equation: -i D r 1 P+Q= A 1 P + PA - PBR^B Figure 3.1 shows the feedback interconnection of the plant Here the inputs w (3.4) Q and the controller K. can include commands and disturbances. Q ci w t Q u z X K Figure 3.1: Standard It turns out that the controller, margins of no less LQR feedback configuration. K, has guaranteed simultaneous phase and gain than 60 degrees and 3dB, respectively [Ref. 12]. Furthermore, the controller has asymptotic properties which are exploited in the design process are discussed next. Define the Hamiltonian matrix, H , as: bt H = \a -br-AT x Q Let T be given by: 24 and / T= p I then: I r -i = where P -p I solves the Riccatti equation, Equation 3.4. Note, A-BR~ B T P l T -i HT _ = 1 l Therefore the eigenvalues of det{sI-H) -BET B T P -AT + PBR~ B T H are the roots of the following polynomials: det{sI-T- x HT) = det(sI-A-\-BR- 1 B T P)det((sI+A T -PBR- 1 B T ) Clearly, the eigenvalues of the Hamiltonian consist of the eigenvalues of R= unstable reflections about the imaginary axis. Let det(sl It -H) = can be shown that the det(sl det{sI-H) — H) si -A (l/p)R^ sl pR\, R\ 1 > 0, Q d and their then; BT +A can be equivalently expresses as [Ref. 9]: = -l n det{sI-A)det{-sI-A)det{I+{l/p)R; B T {-sI-A T )- C{C 1 1 1 1 (sI-A)- B) Let 0(a) = det(sl - A) and 0{s) = C (aI-A)x l B. Then det(sl -H) = -r<f>(s)<f>(-s)det(I 25 + l {l/p)R^ 6{-s)6{s))) (3.5) The SISO example as p is varied from demonstrate what occurs to the eigenvalues of Q d will best to oo. Let e(s) where tp(s) are *M/*w the zeros of 0(s). Then, det(sl It = -H) = -l»tf *)*(-«)(/ follows that the eigenvalues of + (l/pW*)1>(-')/<K*)<K-*))' H are the roots of, <f>(s)<f>(-s) + (l/pMsM-s). Consider the feedback system shown in Figure SISO root and locus analysis its same as Equation are the eigenvalues of as p goes to • Q c/, 3.6. standard configuration for is is: + (l/p)1>(s)1>(-3), Since we know that the stable eigenvalues of H standard root locus techniques show that [Ref. 9]: p eigenvalues of Q go to the stable zeros of C\{sl <j reflection of the unstable zeros of zeros of This characteristic equation <p(s)<f>(s) exactly the 3.2. (3.6) C x {sl - A)~ • n-p eigenvalues of l Qd C\{sl — A)~ X B, — A)~ X B where p is or the stable the number of B. go to -oo in Butterworth patterns. as p goes to oo • n eigenvalues of Q d go to the stable eigenvalues of of the unstable eigenvalues of A. 26 A and the stable reflections X V * 1/p > ^(-a)V'(s) Figure 3.2: Feedback Configuration for Root Locus Analysis. DESIGN REQUIREMENTS B. These properties of an to LQR controller were utilized in a design process in order meet the following design requirements. 1. Zero Steady State Error • Achieve zero steady state values commands in command for all error variables in response to position along the x, y, and z inertial axes. ramp Note that a ramp in postion corresponds to a constant heading, constant velocity trajectory. 2. Bandwidth Requirements • The input-output command response bandwidth (command-loop bandwidth) along any of the three command channels should be no greater than • The 1 radian per second and no less than 1/10 radian per second. control-loop bandwidth should not exceed 12 radians per second for the elevator and aileron actuators, and 5 radians per second for the throttle actuator. These numbers represent 80 27 % of the corresponding actuator bandwidths and shall ensure the actuators are not driven beyond their linear operating range. 3. Closed Loop Damping • The dominant closed loop eigenvalues should have a damping ratio of at least 0.7. C. THE SYNTHESIS MODEL The and the model synthesis LQR is the primary interface between the control design algorithm. At the heart of the synthesis model of Bluebird developed in Chapter is a linear model II. Bluebird has four control inputs, namely elevator, rudder, ailerons, and throttle. The elevator and throttle are natural choices z position in steady state. The remaining two to control the lateral variable (y position). means of generating is control inputs could be used Both rudder and aileron provide accelerations in the lateral plane. In fact, rudder effective at generating sideslip x and for controlling is than aileron. In the linear plant, lateral position the double integral of lateral acceleration. Subsequently, the resulting controller will attempt to use rudder to null out errors using ailerons and to use rudder for turn coordination. it is LQR in lateral position, to turn the plane. However, the desired controller response presence of wind, more is to bank i.e., to turn Furthermore, in the desired that Bluebird fly wings level, crabbed into the wind, rather than use a wing down, top rudder technique. For these reasons, the rudder was removed as a control input to the linear model. As can be seen from Table damped. This light damping 3.1, the dutch roll mode of Bluebird is lightly of 0.111 could pose a performance problem. Since 28 rudder is available and not used in the design of the trajectory controller, the nonlinear model of Bluebird was modified to include a dutch roll Yaw damping. rate yaw damper for improved was fed back to the rudder through a constant gain block with a value of 0.55. Additionally, the rudder was external input. Note that Bluebird is still fully controllable removed as an with the remaining three control inputs. TABLE 3.1: EIGENVALUES OF BLUEBIRD Mode Frequency Longitudinal rad/sec Damping Short Period 5.9 0.735 Phugoid 0.497 0.0344 Dutch Roll 2.4 0.111 Spiral 0.0384 -1 Roll Response -4.572 1 Lateral-Directional The nonlinear model of Bluebird with three inputs and was linearized, as per Chapter s II, integral yaw damper returning the linear model, = Ax + Bu = -{i (3.7) where x = uvwpqrQQV iT Xpos Zpos Ypos 6epot 6apot 6tpot and W = \T [ Sdtvaior This linear model was used in the ^aileron LQR ^throttle design. with a yaw damper are given in Table 3.2 where 29 The it eigenvalues of Bluebird can be seen that the dutch roll mode has been fifteen since TABLE 3.2: damped Note that the number of states decreases to out. the rudder actuator state was removed. EIGENVALUES OF BLUEBIRD WITH YAW DAMPER Mode Frequency Longitudinal rad/sec Short Period 5.9 0.735 Phugoid 0.497 0.0344 Dutch Roll 2.35 0.5 Spiral 0.1788 1 Roll Response -4.5686 1 Damping Lateral-Directional Consider Figure linear 3.3. Here K is the controller to be designed, model of Bluebird, and the block, 5, within the dotted line is G is the the synthesis model. The signal, iu, represents the w= The [ XpOScmd commanded trajectory inputs: ZpOS^d YpOScmd signal xi represents the linear Qcmd Q^nd and angular position <P cmd j states in the linear model. = Xi The tory. Xpos signal e represents the errors The signal z is Zpos # $ iT between the commanded and current trajec- comprised of the outputs of the matrices, Q, and R. Since zero steady state error position, Ypos is desired while tracking a ramp command two integrators were placed on each error 30 signal. in inertia! This also ensures * JL X Figure 3.3: Synthesis and Analysis Model perfect tracking of constant heading trajectories in the presence of a constant wind disturbance. Thus Q was chosen to be; ' 9i Q= t 93 . 1 a £31 £32 1 »2 * * values of CxX were chosen to place six transmission zeros from u to z at appropriate locations. will £12 £21 q2 . The £11. i ' move very near If they are well chosen, six poles in the closed loop plant to the placed transmisssion zeros. With that in mind, the transmission zeros were chosen as appropriate target locations for the poles added by the addition of the error The states. q xx weightings are used as a command bandwidth. mechanism for obtaining the desired Increasing the value of qxx increases the relative propor- tion of that error state in the regulated output vector z. gain increases the command bandwidth controlled state to its commanded value 31 The resultant in that channel in order to more quickly. LQR move the The matrix R the plant Q is a constant diagonal matrix required to be has three control inputs, R is full rank. Since of the form, / rn R=\ r 22 T33 \ The elements of R are used as a mechanism for selecting the control bandwidth. An increase in rxx increases the relative proportion of that regulated output z. The resultant LQR actuator energy in the gain decreases the control bandwidth of that control input. THE DESIGN PROCESS D. Design requirements given in the previous section are SISO in nature. They are expressed as bandwidth limitations of the individual actuators and rise time and damping characteristics along the command channels. Note, the rise time LQR is inversely proportional to the command bandwidth. The following design process provided a means of obtaining a multivariable solution to achieve SISO design specifications. With an appropriate model that incorporated linear representation of Bluebird and a synthesis well placed transmission zeros, the design "knobs" were adjusted in order to meet performance requirements. The design "knobs" are the elemental weightings, q xx and r xx , in the Q and R matrices. The design R to sat- process iterated through the following steps. 1. Initially let qxx equal isfy control 1. Iteratively determine weights for loop bandwidth requirements. Increasing r xx decreases the control bandwidth along that channel. 2. With R from step 1, iteratively 32 determine weights for qxx to satisfy command loop bandwidth requirements. Increasing qxx increases the bandwidth along that channel and decreases the 3. If it is command rise time. required to increase the damping of a lightly damped mode, use an eigenvector decomposition to determine the primary states affecting that mode. Include a weighting on the derivative of those Ensure that control-loop bandwidths are 4. values of qxx in step 2. It is Connect the formance in terms of LQR still satisfactory with the possible that all of the performance requirements are not acheivable within control 5. states in the output z. bandwidth limitations. controller to the linear plant command and evaluate the per- response and disturbance rejection. 6. Confirm satisfaction of other design requirements, including damping. 7. If any step is appropriate changes. deleted. Synthesis Bode unsatisfactory, go back to the synthesis may need Transmission zeros model and make to be added, moved, or model outputs may need to be reevaluated. plots were used to determine compliance with the requirements. After five iterations through the seven step process, the following values for and R Q matrices resulted in a controller design that met design requirements. 0.5 Q= i 02 0.01 i (M 0.0625 0*4 0.0625 a a2 1 1 1 1 5000 R= 1000 1000 The transmission zeros created in the sythesis ble 3.3. 33 model are shown in Ta- TABLE E. 3.3: TRANSMISSION ZEROS OF SYNTHESIS MODEL Channel Cxi Cx2 Cx3 Freqency (rad/sec) Damping Xpoa 1 0.2 0.01 0.1 1 Y 1 poa 1 0.4 0.0625 0.25 0.8 Zpoa 1 0.4 0.0625 0.25 0.8 LQR CONTROLLER PERFORMANCE The eigenvalues of the feedback interconnection of the plant and axe given in Table 3.4. It is controller apparent that the zeros created in the synthesis model were well placed and attracted the integrators created by the addition of the error states. There are two sets of lightly present a problem because their frequency is damped poles. These do not an order of magnitude greater than the frequency of the eigenvalues associated with the trajectory commands. Notice that the actuator poles did not change, indicating that the control band- widths were slower than the actuator bandwidths. Actuator models provide a simple means of determining if the control bandwidths exceeded the actuator bandwidths. Figures 3.4 through 3.6 depict the control-loop bandwidths for the elevator, aileron, and flight controls throttle. was so The cross coupling slight that it is between longitudinal and lateral not shown due to scale. Figures 3.7 through 3.9 depict the command-loop bandwidth for step com- mands in inertia! position. Notice that there mand and Z some coupling between X com- response; the rest are essentially uncoupled. A summary is is of the resulting presented in Table command and 3.5. 34 control bandwidths achieved TABLE EIGENVALUES OF THE FEEDBACK SYSTEM 3.4: Mode Frequency (rad/sec) Damping Axis Response 0.08 0.92 Axis Response 0.21 0.77 Z Axis Response 0.21 0.77 elevator 12.1 1.0 aileron 12.3 1.0 throttle 12.4 1.0 others 0.62 0.79 1.90 0.35 2.17 1.0 2.35 0.18 2.26 0.68 4.54 1.0 5.86 0.74 X Y Frequency (rad/sec) u : '': S>-180 : : ! i —i__^ TTT ...,;... 7 :->; T ' :•• \ ™ -360 L '. i. -540 . i ' ' 10 : i i i i j ; 1 : I I < i I 1 1 i 10 10 10 Frequency (rad/sec) 10 Figure 3.4: Control-Loop Bandwidth: Elevator Channel The response is the reponse to a in the to two types of trajectory ramp command in commands Y position. is of interest. The first This corresponds to a change heading of the commanded trajectory. The response in terms of angle of bank and heading activity is shown in Figure 3.10. 35 The controller achieves the — throttle Frequency (rad/sec) : ; : i : : i -.J.. : i o deg CO o 0> o r> J-i :• • : 1 • • ' i i- Phase i 10 10 Figure 3.5: i 10 Frequency (rad/sec) rm-i 10 . — —^_ : .. , 10 Control-Loop Bandwidth: Throttle Channel aileron co O -50 - 100- Frequency (rad/sec) &-180 -360 - 10' 1 10 10 Frequency (rad/sec) 10 10 Figure 3.6: Control-Loop Bandwidth: Aileron Channel desired result of turning Bluebird to the required heading. phase response of the heading state The response to a sponse to a change in is due to adverse yaw. ramp command flight The nonminimum in Z position corresponds to the re- path angle of the commanded trajectory. Figure 3.11 36 X channel 100 -200 10 Frequency (rad/sec) ~Tt-i~4—-^ -t^^JTTj^^^ 0g>-180 35 5 ^^^^^ -360 .:: £ -540 -720 i 10 i i i i I 10 Figure 3.7: i : 1 1 1 ; 1 1 I ; 1 1 1 1 1 10 Frequency (rad/sec) : : 1 L.LU.J.U1 !~ : f . 10 10 Command-Loop Bandwidth: Y Li|i[| X Position Channel channel m 100 to o -200- 10 Frequency (rad/sec) 10 10 10 Frequency (rad/sec) 10 10 1* -360 95 CO £ -720 -1080- 10 -2 10 Figure 3.8: Command-Loop Bandwidth: command shows the response to this Note that the positive z direction is in The wind orientation is Position Channel terms of pitch angle, 6, and altitude 2. down, hence the negative pitch angle. Finally, the response to a constant ure 3.12. Y such that 37 wind disturbance it is shown affects all three axes of in Fig- Bluebird ; Z channel .ii .1 ; ;Y mil. channel P—BEgr^^T^ 1 -:i j CD 100 - O X channel j -200 ~ f "i 10 "r .... * * l ' i'r-i"r* -2 ^ "!-; ; , T"ji r 1 . Til"! i i__ =-t i :::::! V T I _I_i 4^" 10 10 10 Frequency (rad/sec) 10 ! ., ".'_" ! !!!!!! | ! ' : 1 ~ CT> S -360 CO £ -720 f -1080 ' : TABLE 3.5: : 1 1 : 1 : 1 1 i 1 1 Command-Loop Bandwidth: Z i i 1 10 T~ : 1 1 10 Position Channel COMMAND AND CONTROL BANDWIDTHS Loop Control Loop Break Frequency Elevator 4.0 Aileron 5.0 Throttle 1.0 rad/sec Command Loop X Y equally. 1 10 Frequency (rad/sec) 10 Figure 3.9: '* ' in " 10 The wind Axis 0.75 Axis 1.0 Z Axis 1.0 disturbance begins at 38 t = 0. 0.05 0.04 1 i i - w 0.03- f\ = 0.02 I 0.01 j Heading k_ y a Bank Angle I /\ -0.01 I 1 10 (sees) Bank Angle and Heading Response Figure 3.10: — 25 30 25 20 15 Time Ramp to in Y Command ! 20 Altitude 15 10 - 5Pitch Ang le - degrees • 1 10 Time _ _ 1 Figure 3.11: Pitch Angle and Altitude Response to 39 20 15 (sees) Ramp in Z Command 40 Figure 3.12: Trajectory Error 60 Time (sees) Due to a Constant 40 100 Wind Disturbance CONTROLLER IMPLEMENTATION ON THE NONLINEAR PLANT IV. In Chapter III, of an air vehicle. a linear controller was designed to control the trajectory However, the nonlinear plant. It this controller would only be cannot be implemented, as effective for commanded is, on trajectories that represent relatively small perturbations from the specific trajectory for which it was designed. Intuitively, it depend on the heading angle gravity $ or 0. is ty. clear that the dynamics of the plant do not Furthermore, the orientation of the force of the only change in the dynamics of the air vehicle due to changes in It turns out that these issues were addressed in [Ref. methodology method is for implementing controllers on nonlinear plants involves differentiating some 10], is where a new proposed. The of the inputs to the controller, hence the term, P-implementation. This chapter begins with a general description of the structure of V- implementation. Furthermore, the specifics of linear model simulation is in SIMULINK are discussed. its implementation on the non- Next, the fidelity of the nonlinear improved by incorporating output feedback. This step involves inclusion of high fidelty sensor models and Kalman filters. Finally, all of the pieces of the complete nonlinear simulation are brought together in A. SIMULINK. D-IMPLEMENTATION Using the development in ...Chapter II, the vehicle pressed in state space form as follows: 41 dynamics can be ex- B j v cg = fv ( Bv »u>B ,u)+fR(A)g cg t B B j u>B = U{ v - P^ ='B R(A) B v B UB,u.) cg , t eg (4.1) where /„ and fw are continuously of control inputs. Xti . — To condense the where xi € 6 i? , 1 'eg := TK)9 UG7J6 denotes the vector define eg /.(•) := /.(•) /-(•) fl(.) L(.) := S(.) x 2 £ B? and L 6 as the vector of linear With we A 6*6 .ft r := track. notation, B /»(•) and differentiable . Furthermore, we define Pc Ac €#, (4.2) and angular position commands that Bluebird must this notation, the dynamics of the augmented plant can be written as follows: *• Xp Q :=< ei e2 j/i I where y\ V2 = h{x v ,u) + f2 (x p = Li\Xp)X v = [/ 0](y 2 - r) = [0 /](j/2 - r) = /i(x v ,u) = Xv and y 2 are the available measurements, errors separated into linear t\ ) and . ( l e 2 are the trajectory and angular components. Notice that L function of the orientation vector A = have not included any extra dynamics 42 [0 I]x p . ^ '*' is only a For simplicity of exposition, for the actuators or sensors. we The £ set Q of trajectories where the plant is expected to operate given is by vo ^VQ S {(x^Xp^ucro) := r 'PO /l(**0»^Po> u o) where T corresponds to the This Bluebird. + /2(*po) = set of prescribed linear a broad definition of trimmed is €r}, ,r : ° - and angular positions of Notice that flight. it does not preclude the presence of an inertia! acceleration due to centripetal force. As usual, we restrict the angular positions to (— 7r/2,7r/2) x [— 7r/2,x/2], as the inverse of L{.) Notice, from the definition of B x^ G £ and Notice that the set £ , Xpo , u , r ) € £, Xp,,, u , yi , follows that: by Xp„ = tq 6 T. Given M z n» uo) yio := ^2o := Spo 6u,6yi, xp turbations of x„, x^, , it ir/2. we obtain e 2o 6x p = not denned at x constant easily parameterized is of [— 7r/2,7r/2] constant. e l0 := [/ 0](y 2o Let 6x„, is equation (4.1) B wb £ £ —* wb = AG£— A= (im some subset , y 2o u, , yi, r , := 6y 2 [0 , y2, e lo , /](y2o - r ) := " r ) := 0.. £r, 6ei and 6e 2 correspond to small per- r, ci, and and earized models associated with the rigid 43 e 2o e2 about the nominal values respectively. body Q and the The family set £ is of lin- defined as Qi := {Gi (r r ), <E T}, where 6x v 6x p 6yi £/ (fo) := £y 2 { fei *e 2 k <*o = Ai^x,, + A 2 6x p + B^u = Z6x w + A4^x p = C\8x v + i^i^u = Sx p = [/ 0](<fy2 - <5r) = [0 TPife-Jr) = ^(xvojrcuo), (4.4) and g xf RiAotf-^Ao) A2 = -' R(Aq)Vq x S~ l {Ao) b A,= (4.5) Matrices A 2 and A4 were derived using the identity in Appendix C. The intent of this derivation is to isolate the plant that dynamics that are a function of Ao. Notice A2 represents the contribution of the force of gravity to the dynamics of Bluebird and A* represents the sensitivity of Bluebird's trajectory to changes in the air vehicle's spatial orientation. Let r*o € S be given. Define 6x v 6x p 6yi o)=< 6y 2 6e\ 6e 2 a b = = = = = = = A\Sx v 6x v + C\6x v + A 2 6x p + B6u A^Sxp + D\6u 6x p (4.6) [I 0](6y2 [0 I](Sy 2 -6r) -6r) u(a\*,r ,uo), where A2 = g)<ffl(A )" , A4 = -v x (4.7) Notice that Equation 4.6 decribes the linear model of Bluebird used for the design of the as the origin of LQR controller in the previous chapter, where ro was chosen {/} with Ao equal to zero. Note, at this condition, f R(0) Recall, the structure of the controller developed in 44 Chapter III is = given by: /. = Bc2 Sx c2 = [Se, 0] T = Cc i6x cl + Cc2 6x c2 + Dcl 6y! + [Da D c2 )\6e 6x c i fCi = \ . 6x c2 6u Based on K\ nonlinear plant Q , we propose x T . ] implement the following controller to for the : f x el ic2 fC (4.8) 6e 2 (A) := = Bc2 L" (A)[e 1 0] T = Ccix* + CaL-\A)[ei 1 +De2 L-\\)[0 i A u = = + D e iyi (4.9) [0/]y 2 + ^oaX-^A)^! Xea b Figure 4.1 shows the block diagram of gain scheduling variable. e2 ] T 0] T fC (A). 0] T . Notice that As a function of A, L~ l serves A serves as a to properly resolve the trajectory error at the input to the controller. Note, the controller forms the derivative of the measured outputs, y\. Recall, y\ is the measurement of the states x v and the dynamics of x v are essentially independent of the air vehicle's spatial orientation or position in {/}. An integrator at the output of the controller serves to recover the properties of the linear design. formed using the outputs y 2 and the commanded trajectory measurement of the in {/}, in It states x p L~ l is is the formed turns out that the implementation of Figure 4.1 has an important prop- we need to make the following assumptions: Dim(x c2 ) = dim(u) = dim(y 2 ). — Ad Bc2 — Cd CC2 si full Recall, y 2 serves to resolve this error, originally A2. The matrix has error {B}. erty discussed next. First Al. . r. The rank at s = 0. 45 yi 5 x c2^ n St7 — 1 tn \J 2/2 e K, s u i L' 1 V 1 D*L- 1 B,c2 K = Cd cl D c, <->c2 cl [Da Da] Figure 4.1: Block diagram of the nonlinear controller A3. The matrix pair (A c i,Cc i) Also denote by T(Qi results from connecting Similarly, T(Q be , its K, ) we let Ti(Q , Qi ) : to Ki JC )(ro) 6r , Then the • the feedback systems —* £y 2 the closed loop Unear system that and by T(Qi K\ )(s) its transfer matrix. denote the linearization of the closed loop system at the equilibrium point transfer matrix. (A) observable. is Ki K. %{Q , determined by and ro let Ti(Q , K )(ro)(s) following hold. K )(r ) and T(Qi , K,\ ) have the same closed loop eigenvalues; Ti(G ,fC )(r )(s) = L(A )T(g lo A = [0 , K { ){a)L-\ho), /jxpo Thus, the eigenvalues of the linearizations at each operating point axe preserved; furthermore, the input-output behavior of the linearized operators 46 is preserved in a well-defined sense. The reader complete discussion on approximations to differentiation. The proof of this result is this is 10] for a referred to [Ref. method that avoid using pure contained in Appendix C. P-EVIPLEMENTATION OF THE CONTROLLER IN SEVIULINK B. In general, three steps are required to implement the linear controller developed Chapter on the nonlinear plant. III form the derivative of x v as per Equation tor [ B Vcg, B UB]. The first not compute the derivative of B Vcg since b u>b is Recall, x v is three elements of the vector, B v cg processed acceleration outputs from the derivative of 4.9. First, the controller computed by the IMU. it will equal to the vec, are available as Therefore, the controller need have it available directly. tion. The is vector e\ The controller. Secondly, the controller needs to act on the vectors, ei and position error e\ needs to formed as the difference in commanded and e2. The linear current posi- B then multiplied by the transformation matrix R. This is effectively resolves the linear trajectory error in the body-fixed reference frame. Along angles. this position trajectory, there exists Recall that x v jectories. = constant is a corresponding trajectory of Euler one of the constraints on the This includes a broad range of flight conditions set of tra- such as steady turns, steady pull-ups, climbing or descending turns, or constant heading. is natural to define the linear trajectory as a sequence of positions, While it is it more convenient to define the derivatives of the Euler angles rather than the values of the angles directly. Consider, for example, that for in £, the components of A can be described by: 6 turn rate. many trajectories = 0; = 0; and of interest $= desired Furthermore, the relationship between the rates of change of the Euler angles and b ub is used as a means of resolving the angular position error 47 in {B}. Recall, b = S-1 (A)A. u>b (4.10) Therefore, the derivative of the Euler angle states Euler angle rate is removed to form e'2. is formed and the commanded This error is resolved in {B} Equation 4.10. The integrator at the end of the controller recovers the using effect of the initial differentiation. Thirdly, the required error states are formed by integrating the rotated Figure 4.1 indicates that integral action linear trajectory error vector t\. is accomplished at the output of the controller in order to recover the original properties of the linear design. This accounts for one of the integration steps on the error. Therefore, only one additional integrator double integration action on the trajectory error B for a complete description. The required to provide t\. Figure 4.2 shows the P-implemented controller in See Appendix is SIMULINK, file plantl.m. LQR gain has been parsed into several separate matrices for clarity of control action. C. GENERATION OF THE TRAJECTORY COMMANDS The commanded trajectory is specified with respect to the inertial refer- ence frame. At this point, it is performance capabilities known and is those capabilities provided there restrictions with respect to the assumed that a knowledge of the is that the specified trajectory no wind. The air air vehicle's is within air vehicle has certain airspeed mass that cannot be violated regardless of the desired trajectory to be tracked. These restrictions typically provide lower and upper bounds on the velocity of the air vehicle with respect to the airmass. 48 Yaw Damper Rudder Gain Proportional Accelerations Gain on Stales Angular Rates du/dt Actuators Positon Gain on Euler Angles Euler Angle Error du/dt Elevator Aileron 1st Integral Gain on Form Commanded Rotate Position Error Error {1} 2nd \m to {B} Trajectory ++ Throttle Integral Gain Proportional Gain on Postion Error Figure 4.2: D-Implementation of controller on Bluebird An example of a lower limit is the stall speed of the air vehicle. Such limits are usually based on fundamental physical limitations of the airframe and a "flyable" trajectory can become "unflyable" under a logic block positioned between the ensures that the certain conditions. commanded commanded trajectory can be Therefore, trajectory and the controller flown at current flight conditions within user defined indicated airspeed limits, shown in Figure 4.3. manded linear trajectory enters the block as a time inertial reference frame. the IMU and air mass Onboard velocities stamped position The comfix in the sensors provide both inertia! velocities from from the 49 pitot-static system. Furthermore, a dual vane device instrumented on Bluebird provides both a and {3 measure- ments. Note: a close approximation to these readings could be obtained from IMU measurements Portion as outlined in -J diVdt Chapter II. I Schedule Figure With 4.3: Commanded Trajectory Logic Block these measurements, the wind vector resolved in {/} *W = 7n RB Vca eg is calculated as: w v,cg -' B£,R B "W" v (4.11) where w Vcg = and V t is the indicated airspeed obtained from the pitot-static system. The commanded indicated airspeed of the air vehicle, Vt-cmd, 50 is calculated as: where / u cm(f is the numeric derivative of the commanded indicated airspeed is trajectory altered as follows. The is 9 commanded linear trajectory. If the not within specified limits, the and angles = tan-\v x lvx tp commanded are calculated as follows: (4.12) ) and iJ> = sin- where the components of I v an d are define the commanded Finally, the 1 I (vy /\ v cmd \) [v x ,v v ,v z ] T . (4.13) Note that the angles 6 and velocity vector's orientation in {/}. amount that Vt-cmd is outside of indicated airspeed limits subtracted from the magnitude of I v cm i, resulting in the magnitude of the commanded inertia! velocity, Vmod where I v mo d is = the termed V. file V is —cos(0)sin(xl>) atn(V') cos{\j)) sin(6)cos(ip) —sin(6)sin(xj>) new commanded that implements this logic is to: —sin(0) V (4.14) cos(0) inertia! velocity. commanded is new then resolved in {/} according cos(9)cos(tJ>) grated and sent to the controller as the .m ij> This command trajectory. The is inte- MATLAB windlogic.m and can be found in Appendix A. The net effect of the trajectory logic block up against one of its airspeed followed. A but change choice its is limits, the simple. When Bluebird runs commanded trajectory can no made to maintain the magnitude. Notice that is this direction of the longer be commanded method does not affect the velocity turn rate associated with the trajectory and subsequently, no processing of the angular 51 trajectory fly commands is required. In this way, Bluebird a trajectory that would force The performance is initially flying never commanded to to exceed the performance limits. of the trajectory logic block can be seen in Figure 4.4. The lower limit for Bluebird was bird it is arbitrarily choosen as 63 feet per second. Blue- due north at a ground speed of 73 into 20 feet per second of feet per second, crabbed wind from the west. The commanded trajectory turns 90 degrees to the east. Notice that the original trajectory would result in an indicated airspeed of 53 fps while the revised trajectory results in a commanded trajectory of 63 fps. 85 100 120 - 60 seconds 80 Time 100 120 - 60 seconds 80 Time 40 20 100 t 50- O-50 20 40 Figure 4.4: Example of If the commanded Commanded trajectory is Trajectory Revision generated using a velocity rather than position schedule, the differentiation block in Figure 4.4 can be removed. velocity schedule is specified in {/} as a sine frequency, and phase along the x and y axis. corresponds to the magnitude and the wave of appropriate magnitude, The commanded ground speed commanded turn rate corresponds to the frequency of the sine function. Constant heading flight 52 A is a subset of these trajectories where the frequency is zero. This commands makes determining turning method of generating trajectory As an example, con- rate (hand) simple. sider the case of generating the velocity schedule for a circular flight pattern at a ground speed of 100 fps and a turn rate of 0.1 radians per second. derivative of the r := < commanded by time trajectory, r, parameterized x axis velocity 100sin(0.1< y axis velocity z axis velocity 100sm(0.1< + + The is: pi/2) 0) desired climb or descent rate (4.15) 6 e 0.1 The SIMULINK block that generates the commanded trajectory is in Figure 4.5. Velocity Angular Schedule Linear Processed ' Trajectory Commands indices A, re peed Limit inertia. Velocity | Logic 1 I 1 I s 00 1 63 J lower limit 1 u S3 1 upper limit Figure 4.5: Generation of Trajectory 53 Commands shown . STATE FEEDBACK TO OUTPUT FEEDBACK D. Up until this point, the development of the tracking controller has dealt exclusively with full state feedback. This section will detail a the full state feedback in conjunction is method whereby replaced by high fidelity models of the onboard sensors with Kalman filters designed to provide optimal state estimates, using onboard sensor data. Sensor Modeling 1. This work builds upon sensor models developed in two prior theses. In [Ref. 1], a detailed model of the inertial measurement unit, oped. In [Ref. 2], a detailed model of the GPS unit is IMU, developed. is devel- The IMU is a compact, lightweight, low power unit which integrates nine sensor measure- ments in one box. These sensors are three axis accelerometers, three axis rate gyros, pitch and roll inclinometers, and a magnetometer. The accelerometers are instrument grade, signal conditioned, and temperature compenstated. Full scale output is +/ — 3 g's. The accelerometer's frequency response 100 Hz. However, the antialiasing inside the of all of the sensors to 20 Hz. to compenstate An IMU limits the effective internal initialization for accelerometer bias and rate gyros used by the bandwidth cross axis error. Table 4.1 IMU past program allows the unit specifications of the accelerometers incorporated into the sensor The is flat shows the model [Ref. 8] are solid state vibrating element angular rate sensors. This relatively new technology uses no moving parts. piezoelectric bender element angle. The element is mounted end fastened to the base is to end but rotated A at a 90 degree resonantly driven such that the sense element swings a reciprocating arc. Under zero angular rate conditions, the motion of the sense element due to the drive element does not produce any 54 TABLE ACCELEROMETER CHARACTERISTICS 4.1: Acceleration Range ±2g's Acceleration Bandwidth 20 Acceleration Scale Factor 0.2% of Full Scale 0.2% of Full Scale Acceleration Noise Floor 0.0005 g's Cross Axis Sensitivity 0.5% of Full Scale Acceleration Bias bending of the sense element. cause Hz momentum When a rate of rotation exists, Coriolis forces to be transfered into the plane perpendicular to the of the drive element, thus causing bending of the sense element. transducer picks up a signal from the sense element when it is A motion pressure bent that is proportional to the angular rate with a phase dependent on the direction of rotation. Figure 4.6 shows the configuration of two rate sensors mounted in a "tuning fork" configuration. Table 4.2 shows specifications of the rate sensors incorporated into the sensor model [Ref. Figure 4.6: 8]. Angular Rate Sensor 55 TABLE 4.2: ANGULAR RATE SENSOR CHARACTERISTICS Rotational Rate Range ±114.6deg / sec Rotational Rate Bandwidth 20 Rotational Rate Bias 2.0% of Full Scale Rotational Rate Scale Factor 0.5% of Full Scale Rotational Rate Noise Floor 0.05% of Full Scale 0.5% of Full Scale Cross Axis Sensitivity The inclinometers low bandwidth sensor to ( utilize a liquid crystal pendulous sensor. approximately 0.12 radians per second) that be integrated with the rate sensors angular position. The Hz The fluxgate for high is It is a meant bandwidth measurements of magnetometer provides heading measurements. specifications incorporated into the Euler angle sensor models are shown in Table 4.3 [Ref. 8]. TABLE 4.3: INCLINOMETER AND MAGNETOMETER CHARAC- TERISTICS Pitch and Roll Bandwidth ±50 deg 1/2 Hz Pitch and Roll Accuracy 0.2 deg Pitch and Roll Range GPS plane position. Heading Range ±180 deg Heading Accuracy 3.0 deg Heading Repeatability 0.5 deg Heading Linearity 0.5% provides data in a form that can be converted to local tangent A brief summary of the errors included in the follows. GPS Error Sources Selective Availability 56 GPS model - intentional degradation of pseudorange signal by Departmant of Defense • Clock Differences • drift and bias GPS in clock Ephemeris Error - error introduced in converting pseudoranges to inertia! position fix Each of these sensor components is simulated in block diagram form utilizing internal sources of error. models is The upper in Figure 4.7 M— Q level SIMULINK and contained in the diagram of these sensor SIMULINK urad BodyThnj« Aoo*W*tora dJCSS* J" ImiM Aooitirdlon* *l » In rn RateQyro* out* *m IflflHMMMSM -* It M«gno*«t«f > on l»artftn -» SIMULINK modeling principles based on manufacturer specifications and known shown in OaMM Aetwttom Figure 4.7: Sensor Models in 57 SIMULINK file plant l.m. 2. Kalman Filtering With the outputs of the modeled sensors available, Kalman filters were developed along the linear and angular position channels to provide optimal An estimates of the states for the controller. approach analogous to the LQR design was used. Consider the linear system described by: x . where v and = Ax + Bu + Gw = Cz + v V A gain matrix, L, was found such that the W. x x. The Kalman L= Y is Kalman filter given by: = Ax + Bu + L(z - Cx) produces an optimal estimate of where .m w are zero mean white noise with respective power spectral densities of and /. (4 - 16) positive semidefinite gain L (4.17) is calculated as follows: YCT V~\ and solves the algebraic Riccati equation: AY + YA T - YCT V- CY + GWGT = X A synthesis original plant. model was formed that included the dynamics of the The IMU used in Bluebird incorporates an initialization routine that removes steady state bias from the sensors. Therefore, extra dynamics were not required in the Kalman filter to compensate for bias. The design process was primarily driven by the bandwidth limitations of the inclinometers and The values of V and W were used as "knobs" 58 GPS. in the iterative design process. — GPS For the was sensor, a break frequency of 1/2 radian per second desired. For the Euler angle sensors, a break frequency of 1/10 radian per second was desired. It was wash out the accelerometers and rate sensor also desired to biases at low frequencies. The Kalman filter for the position estimate blends processed ac- GPS position fixes. Figure 4.8 shows the frequency celerometer outputs with the response along the x axis of the Kalman position filter. The other two axes have similar dynamics. i i i i gae^n i | '^tt4^2 i | l ' ' ' -T L " 1 J-J-LLiLL CO 8-2 y' ' c Acceferometer Sensor dashed Ime (0 * -40- 1 i •" ; 10- degrees - ;;;; io-' ; <3PS? $0n$or: solicf ; \ ; ; ;;;; • ;;;;;:;; ioFrequency - rad/sec g 8 g 4f«i !::;:! ; ; ; ; ; f"-i^L?f!?H i-ffH- o 'f? Phase g ill 10' 10" <— >• •j-j-j|j-i-: 10 io- Figure 4.8: :••••:•••:-• lf4^if[-----i--l-l-H- 10 Frequency 10" - 10' rad/sec Frequency Response of Position Filter The Kalman filter for the Euler angles blends the outputs lar rate sensors, of the angu- converted to Euler angle rates, with Euler angle measurements from the inclinometers and magnetometer. Figure 4.9 shows the frequency re- soonse of one channel of the Euler angle filter. The remaining two angle channels 59 are similar. CD -5 : g-10 " : - :" 'r 3 o>-15 . .y*. : f... .'. : : : \\a" : : : : : : !4lt ^vl :: : ::::::: i : i ^<v:y::-lnc1lh6riheierS^ .. '. .;. i .; , .:.L . Angular ^at^^ensorr^JaBhiBdirei-V^-j- ; ro u«>y -20 10 • C- 10" 10Frequency - rad/sec 10" 10' 10 10Frequency - rad/sec 10 10' 5° ! o S o "? o (0 (0 £-50 10 Figure 4.0: Frequency Response of Euler Angle Filter E. INTEGRATION OF THE FULL NONLINEAR SIMULATION The full nonlinear simulation can ter II, the nonlinear rigid Wind reference frame, therefore a simulation whose output in {/}. If the simulation is in a Chap- SIMULINK expanded to include the a moving airmass, the dynamics of Bluebird can be simulated at an arbitrary night condition. tial together. Recall in body dynamics were implemented block labeled Equations of Motion. effects of now be pieced is is SIMULINK a vector Next, the wind velocity velocity of Bluebird, Bv cg . usually referenced with respect to the iner- The is wv block, Wind, is included in the full comprised of the wind velocity resolved resolved in {J5} and added to the inertia! result 60 is the velocity of Bluebird with respect Bv to the airmass, resolved in {B}. This velocity, rather than computing the aerodynamic contribution to the used in Equation 2.25. in the .m A more detailed state-deriv.m file is description of SIMULINK the avionic sensor suite onboard Bluebird. output is SIMULINK how this come from a from the block to process the filter erates actuator commands is and moment accomplished block, Sensors, that models The output from block, Kalman trajectory. The is filtered The commands trajectory block which uses the commanded these sensors filters. directed to the V-Implemented Controller block. the controller manded total external force used when contained in Appendix A. All sixteen states are sent to a appropriately processed in a cg , is to measured outputs The controller gen- necessary to maintain the air vehicle on the com- trajectory, thus completing the loop. the SIMULINK nonlinear simulation the SIMULINK file is plantl.m. 61 shown The complete top in Figure 4.10 level view of and contained in QPS IMU Air Data Kalman Filters Sensors Trajectory Generation Figure 4.10: SIMULINK Diagram 62 of the Full Nonlinear Simulation V. APPLICATION TO THE CONTROL OF BLUEBIRD D-IMPLEMENTED CONTROLLER PERFORMANCE CHAR- A. ACTERISTICS The performance characteristics of the trajectory tracking controller de- signed in Chapters III and sensor and filter IV were evaluated using a two step process. First, the blocks were removed and the controller was connected directly to the nonlinear equations of motion block. Utilizing pure state feedback, Blue- bird was flown along two fundamentally different types of trajectories. These two trajectories served as general examples of the Next, the sensor and in Chapter IV, Equation A general deparure and arrival trajectory, which 4.4. set of all trajectories defined types of trajectories tested in the first step, is filter blocks were added. a combination of the two was commanded and flown with the controller utilizing output feedback. Data from this simulation were used as input to a virtual prototype simulation discussed later. The dynamic tion. At trimmed flight simulations were started using the same this initial condition, Bluebird for level flight at 73 fps. The is initial condi- aligned with the positive x-axis and positive x direction is considered to be heading north. The mechanics of the dynamic simulation use a right-hand thogonal coordinate system described in Chapter is pointing east and the positive z-axis is II. As such, the positive y-axis pointing down. venient from a computational standpoint since it or- This choice is con- coincides with the body-fixed reference frame of Bluebird at the initial condition specified above. Typically, 63 however, the positive z axis orthogonality west. is is considered to be pointing up. If the right-hand maintained, the positive y-axis would then point towards the For ease of visualization, this the cooridinate system used to display is the results of the simulations. The in simulations are intended to evaluate the capabilities of the controller terms of the nature of trajectories that can be followed in a stationary and in an air flight serve as mass moving at constant velocity. Two basic kinds of the bases for the test trajectories. Each test trajectory no- wind conditions, and then again with the wind added at the air mass trimmed is flown in some point during flight. The simplest form of trimmed flight is constant velocity, constant heading. This corresponds to a trajectory defined by a ramp tion. command in inertial posi- This was the basic trajectory that the controller was designed to track. Figure 5.1 shows a three dimensional plot of the first test In this trajectory. case the trajectory encompasses 30 seconds of flight heading north at 73 fps followed by a 90 degree turn to join a trajectory heading east at 73 fps while On climbing at 300 feet per minute. second from the north time = is added the second at the flight, time the turn is a wind of 20 feet per commanded (elapsed 30 seconds). Figure 5.2 contains the first four graphs of flight data. The first graph shows the time history of Bluebird's distance from the commanded trajectory. The next three graphs show the time history of the Euler angles. Consider the baseline flight (no- wind data). Bluebird begins the turn at an elapsed time of 30 seconds and exits the turn at an elapsed time of 45 seconds. Approximately 30 seconds later the trajectory error is 64 nearly zero. In the presence of 20 feet commanded position (10 second intervals) li'o' 500 N flight path: 400 N ©300 s ground tarck: "....., .;" 1200 (0 100, x range y range (ft) Figure 5.1: Test Trajectory (ft) #1 per second of wind from the north, the trajectory error also goes to zero in about the same period of time. The graphs of the Euler angles indicate that Bluebird is flying wings level, crabbed into the cross wind, which is the desired result. Figure 5.3 shows the groundspeed and indicated airspeed during the flights. Notice that in both cases, the second is commanded groundspeed of 73 feet per eventually maintained. Finally, Figure 5.4 shows the time history of the control activity during the Trimmed flight does flights. not necessarily have b u>b equal to zero. For instance, any steady turning manuever fits the definition of trimmed flight given in Chap65 Magnitude of Position Error 60 80 Time -seconds Bank Angle 50 /^V 50 20 40 60 80 100 120 140 100 120 140 Time -seconds Pitch Angle 80 60 Time -seconds Heading W 100 <D »_ 0) •100 20 40 60 80 Time -seconds Figure 5.2: Trajectory #1: Position Error and Euler Angles ter IV. Figure 5.5 shows the three dimensional plot of the second In this case the test trajectory rate is is test trajectory. a helix flown at 73 feet per second. one revolution per minute and the helix angle 66 is The turn 4 degrees which corre- — — 1 Groundspeed 78 ! j\ 76 no wind: J ^ wind: ': " " 1 "-.-.- '* \ 72- ^Y^^ V ' 70 ' 1 1\ / w 74- 1 ! ! 1 1 i 20 40 60 i 1 100 80 140 120 Time -seconds Indicated Airspeed 95 -1 / I — i \ : 1 1 — \: 90- j : : 1 1 85- r ; ":V a : " \ 80i r< i x » r v — -:-•'" 75- : "*^ i 70 " 40 20 Figure " 60 80 Time -seconds 5.3: i i 100 120 140 Trajectory #1: Velocity Data sponds to a climb rate for Bluebird of 300 feet per minute. For this test, no indicated airspeed limits were placed on Bluebird. Consider Figure 5.6 which shows the position error and Euler angle time history for the helix trajectory. Notice that with no ers Bluebird to join the The constant the second commanded controller manuev- trajectory with zero error in steady state. pitch and bank angles confirm the steady state performance. flight, On a wind of 20 feet per second from the east was added at the start of the helical trajectory (elapsed it is wind the clear that the time equal to 40 seconds). Intuitively, bank and pitch angle must vary as Bluebird traverses the 67 Elevator Activity .'"*! ' no \ . '-.-.-.-' wind: h. L. 0) <D V -2 20 40 60 80 100 120 140 Time -seconds Rudder Activity I 1 i i i i i i i i i i 40 60 80 0) 0) (1) "0 20 100 120 140 100 120 140 100 120 140 Time -seconds Aileron Activity W (1) 0) <D T5 20 40 60 80 Time -seconds Throttle Activity 2 c a. 20 40 60 80 Time -seconds Figure helix. 5.4: Trajectory #1: Control Activity To an observer on Bluebird, the wind, while constant in {/}, represents a sinusoidal disturbance. This explains the sinusoidal nature of the position error around the helix. Figure 5.7 shows the groundspeed and indicated airspeed 68 commanded flight path; position (10. second intervals): "d" "__£_"'' 1200 x range y range (ft) Figure 5.5: around the helix during the two groundspeed is oscillation experienced. is Test Trajectory flights. (ft) #2 For the no- wind maintained while for the flight in Finally, Figure 5.8 flight, the commanded wind, a one foot per second shows the time history of the control activity during the flights. Four additional flights were flown using the helix trajectory in an attempt to ascertain the sensitivity of the position error to changes in rate and wind velocity. commanded turn Figure 5.9 shows the position error around the helix for three different turn rates with a constant wind of 10 feet per second. The dashed line corresponds to a turn rate of 3 degrees per second or 2 minutes ' Magnitude of Position Error 10 f no \ : e 5 ..../', S.\. \"\ \ \ " v- \csA 50 V V" i 100 v V \ V.' i 150 200 250 200 250 Time -seconds Bank Angle 50 (0 o ^. TJ -50 50 100 150 Time -seconds Pitch Angle 20 i i 1 i i i i 50 100 150 200 i 10 .*'" CD r— ft-r . . . - n . •• . TJ -20 250 Time -seconds Heading 250 150 100 Time -seconds Figure 5.6: Trajectory #2: Position Error and Euler Angles per revolution; the solid line corresponds to a turn rate of 6 degrees per second or 1 minute per revolution; and the dash dot line corresponds to a turn rate of 9 degrees per second or 40 seconds per revolution. 70 The error increases with Groundspeed 76 75741 no wind: — '»:v".»' i*! y • 1 V ' \ t\ X X §73sJi^?C72- wind: ': 1- -> V/ l' -;r 71 70 50 100 250 200 150 Time -seconds Indicated Airspeed 100 7 * 90 \ /•"TV /; ' : i : 80 / v v \ / : Q. 70 : \ / i : :/ 60 50 100 50 250 200 150 Time -seconds Figure 5.7: increasing turn rate. The state angle of bank of command trajectories place an upper Trajectory #2: Velocity Data 9 degree per second turn rate corresponds to a steady thirty degrees, requiring bound on the no wind. more than a It may not be desireable to certain angle of bank and this may error. Figure 5.10 shows the position error around the helix for three different wind velocities at a constant turn rate of 6 degrees per second. in velocity from 10 to 25 feet per second. The local The wind maxima values of the position error are proportional to the magnitude of the disturbance. 71 varies Elevator Activity »5 " ..^•v ..r:"' "X***" \ '5 ! ! ho wind:" wind:"-.-.-.-" / \ 50 ! •."''" .""^s 100 ' : 150 "v .< \ / 200 250 200 250 200 250 Time -seconds Rudder Activity w 10 4r i_ 0) i o -10 100 50 150 Time -seconds Aileron Activity u l 4* w - \x , *m*t , m+m t V 10 50 100 150 Time -seconds Throttle Activity 50 100 150 200 Time -seconds Figure B. 5.8: Trajectory #2: Control Activity AN AIRPORT DEPARTURE AND ARRIVAL FLIGHT SIM- ULATION many cases, the good example of this In A trimmed flight condition of an air vehicle changes often. would be a standard instrument departure or 72 arrival 100 150 Time -seconds Figure 5.9: Trajectory #2: Position Error for Varying Turn Rates to an airfield. These trajectories are typically a combination of constant radius turns and wings level flight, while often climbing and descending. Trajectory position errors become critical when the air vehicle is on final approach with a constant heading, constant velocity trajectory. Consider Figure 5.11. If an airfield is imagined to be located at the origin, then this trajectory would be indicative of a typical departure followed by a typical arrival to that airfield. different radii is The scenario utilizes turning trajectories of three connected by straight line trajectories. The a constant 73 feet per second throughout. 73 Wind commanded velocity zero. Thirty is initially 100 150 Time -seconds Figure 5.10: Trajectory #2: Position Error for Varying seconds into the At 90 seconds east. flight, the wind is added into the flight the wind on final Finally, with Bluebird the wind is is at 10 feet per Wind Velocities second from the east. increased to 20 feet per second from the approach tracking a 4 degree glideslope, rapidly shifted 90 degrees to the north and decreased in magnitude to 5 feet per second. Figure 5.12 shows the time history of the position error, wind velocity, and Euler angles during the activity. as it is flight. Figure 5.13 shows the time history of the control Note, however, the relative difficulty of analyzing data of this nature somewhat difficult to visualize. Flight 74 simulation data was saved to a file flight path: ground '_ track: 600, commanded position (10 second '6' intervals): £>400. y range (ft) 2000 x range (ft) Figure 5.11: Departure and Arrival at an Airfield and processed for compatiblity as package, Designer's Workbench. Bluebird was developed in [Ref. and an input A 13]. file for a 3-D visualiztion software virtual prototype of Monterey Airport and The simulation was then run arrival to the virtual prototype airfield. In Designer's as a departure Workbench, the flight can be viewed from multiple perspectives and virtual prototypes of standard cockpit displays further enhance visualization. on video tape, [Ref. 14]. 75 One possible result is captured Magnitude of Position Error 100 250 150 Time -seconds Bank Angle 20 (I) <D <D i_ 0) <D "0 20 100 150 Time -seconds Pitch Angle 100 250 150 Time -seconds Heading 500 <D (D L. 0) (D "0 500 50 100 150 200 250 Time -seconds Figure 5.12: Airfield Scenario: Position Error and Euler Angles 76 Elevator Activity ^^V to '• • y*»« .*/-• ^-- <D T3 50 100 200 150 250 Time -seconds Rudder So "0 J Vrrr*wr V v~ — Activity A- r Ju v . 100 50 200 150 250 Time -seconds Aileron Activity .5 £o j hi i . . m » i y— . V—f ^""Tfr*^ Ju V 1 -5 50 100 150 200 250 200 250 Time -seconds Throttle Activity 1.5 c 2 1 0) a 0.5 50 100 150 Time -seconds Figure 5.13: Airfield Scenario: Control Activity 77 CONCLUSIONS AND VI. RECOMMENDATIONS CONCLUSIONS A. Based on the data presented in this thesis, the following conclusions are drawn. • SIMULINK provides an effective environment for the developement of nonlinear simulations for air vehicles. model of the plant at As a result of this an arbitrary trim condition development, a linear easily obtained for is design purposes. • LQR design techniques utilizing a synthesis model and weighting "knobs" provide a straight forward means of obtaining satisfactory controller gains for MIMO systems while meeting design requirements. • ^-Implementation of the linear trajectory tracking controller allows the controller to operate effectively conditions, trajectories defined For in steady state. wind disturbance by an arbitrary is by an arbitrary flight conditions [vq, cjo] flight are tracked perfectly with wind, rejection of a constant accomplished along the family of trajectories defined [t>o»<*>o disturbance in {/} on the nonlinear plant. In no- wind is = Oj. However, for turning flight, seen as a sinusoidal disturbance in soidal tracking error results. For errors are usually small. 78 a constant wind {B} and a moderate bank angles and turn sinu- rates, the • Preprocessing of the trajectory commands by an adaptive filter allows for steady state control of the air vehicle's velocity with respect to the air mass in the forward path, thus not affecting stability. With sufficient margins transient deviations in indicated airspeed, this would allieviate the concern of stalling the air vehicle when for major tracking an inertial trajectory in wind. • When and analyzing a nonlinear plant and controller, test simulations are vital in some cases the only means of performance evaluation. The three dimensional plots and time history graphs are fine for simple trajectories, but are difficult to analyze for more complex cases. virtual prototyping software package, like Designer's pressive. The enhanced situational awareness of watching the designed controller operate for a "pilot's perspective" The capabilities of a Workbench, are im- and visualization capabilities on a virtual prototype allow feedback not otherwise attainable on the desk top. RECOMMENDATIONS B. Based on the conclusions presented above and the experience of developing the simulation package presented in this thesis, the following recommendations are made. • W^hile the rigid body equations of motion are nonlinear with respect to the kinematics involved, they are completely linear with respect to the stability and control derivatives. The constant control derivatives could be replaced by functions is available. 79 coefficient stability when and further flight data • A similar design process used for the trajectory controller could be done using Tioo design methodology. • The trajectory preprocessor could be used to convert an inertial fixed tra- jectory into an air mass fixed trajectory. where the pared to • This might have applications air vehicle's inertial position is of its performance with respect to the Running simulations real secondary importance comair mass. time in Desiner's Workbench rather than using batch post processed data would be the next logical step. Further work might lead to virtual prototype visualizations based on real-time simulations or downlinks from actual air vehicles. 80 APPENDIX The SIMULINK models lowing MATLAB .m files A: MATLAB of Bluebird (plant Lm & FILES plant2.m) use the fol- as user defined functions. STATE.DERIV.M X X % Function to calculate derivative of u,v,w,p,q,r X % based on X % 1: kinematics X % 2: gravity X % 3: propulsion X % 4: aerodynamics X X X y, Variables brought from workspace: X X y, X x [contrl inputs, state variables (1 - 9), wind vel] */, X - (da,de,dr,dtrt,u,v,w,p q,r,phi,theta psi, wind xyz)X X x % Variables called from function "blue.data" X x X air density X rho % b = wing span X X 81 X c = wing cord '/• X s = X X Cfo X Cfu = Stability derivative for control inputs X X m = airplane mass X X lb X To = Thrust scale term % X Pe = Engine postion matrix X wing area Steady state force term inertia tensor matrix (body frame) */• X X X X % X X function accel = state_deriv(x) XXXXXX Function call to get the aircraft data [uO,wO,rho,Cfx,Cfo,Cfu,Cfxdot,s,b,c,m,Pe,To,Ib] = blue.data; XXXXXX seperate the combined vector into seperate elements u = [x(D; x(2); x(3)] dtrt » x(4); state = [x(5); x(6) ; x(7) ; x(8); x(9) lambda = [x(ll); x(12); x(13)] 82 ; x(10)] wind = Cx(14); x(15); x(16)] y.'/.'/.'/.X'/. calculate velocity wrt the airmass and form state vector '/.'/.y.'/.'/.y. that will be used to calculate the aerodynamic forces/moments ias = u + wind statel = [ias(l)-uO; ias(2); ias(3)-w0; x(8) y.y.y.y.y.'/. ; x(9) ; x(10)] calculate total velocity, vt vt - (ias(l)*2 + ias(2)*2 + ias(3)~2)~ .5; milt calculate qbar qbar = .5*rho*(vt"2) XXXm calculate Ml Ml = diag([l/vt, 1/vt, 1/vt, (.5*b)/vt, (.5*c)/vt, (.5*b)/vt]); lWm calculate M2 M2 = diag([0, 0, (.5*c)/(vt~2) m%X% , calculate Sprime 83 0, 0, 0]); Sprime = diag([-l, mm 1, -1, b, c, b]*s); calculate Mu Mu = inv([eye(3)*m,zeros(3) zeros (3) ,Ib]) ; Wan calculate Tw2b alpha = ias(3)/vt; beta = ias(2)/vt; Tl = [cos(alpha), 0, -sin(alpha); 0,1,0; sin(alpha), 0, cos(alpha)]; T2 - [cos (beta), -sin(beta) Tw2b , 0; sin(beta) , cos (beta), 0; 0,0,1]; [T1*T2, zeros (3); zeros (3), T1*T2] calculate chi y.y.y.y.y.y. Chi = eye (6) - Mu*Tv2b*qbar*Sprime*Cf xdot*M2 calculate Propulsion matrix '/.y.'/.'/X/. Prop = [ eye (3); 0,-Pe(3),Pe(2); Pe(3),0,-Pe(l); -Pe(2),Pe(l),0]; 84 y.y.'/.y.'/.'/t calculate gravity vector and rotation matrix {1} to {B} Rot = [1, 0, -sin(lambda(2)); 0, cos(lambda(l)) 0, -sin(lambda(l)), cos(lambda(2))*cos(lambda(l))] , cos(lambda(2))*sin(lambda(l)) Ru2b = [Rot zeros (3) ] ; g = [0; 0; 32.174]; FgU = m*g; '/.'/.'/. '/.'///. '/.y.'/.'/t'/t'/. put the components due to gravity; thrust; and control surface deflections together for their contribution to x-dot thrust = Prop*To*dtrt gravity = Ru2b*FgU; Ctrl qbar*(Tw2b*(Sprime*(Cfo + (Cfu*u)))); xdotu = (Mu*(ctrl+thrust+gravity)) '/.'/.'/.y.'/.y. calculate kinematic contribution omegax = [0,-state(6) ,state(5) ;state(6) ,0,-state(4) ;-state(5) ,state(4),0] ¥ilb = (-inv(Ib))*(omegax*Ib) 85 ; ; ; [-omegax, zeros (3); zeros (3), wxlb] Rot xdotrot = Rot*state; \IJ*hl% state vector feedback component xdot xdotcfx = qbar*(Mu*(Tw2b*(Sprime*(Cfx*(Ml*statel))))); XXXXXX add three components of xdot together and premult by inv(Chi) xdot* inv( Chi)* (xdot rot+xdotcfx+xdotu) XXXXXX return xdot accel«xdot EULER-B2I.M X Transformation [p q r] to X 86 lambda-dot function ldot = eul_b2u(x) , , /. /.'/.'/.7. separate the composite vector %%%%% and [phi theta psi] 'x' into [p q r] omega = [x(l); x(2); x(3)] phi=x(4); theta= x(5) psi=x(6) %%%%% calculate the rotation matrix {1} to {B} y.'/.'/.y.'/. based on euler angles Rb2u = [1, sin(phi)*tan(theta) 0, cos(phi), -sin(phi) 0, sin(phi)*sec(theta) VkkhU , , cos (phi) *tan (theta) ; cos (phi) *sec (theta)] calculate lamda-dot ldot = Rb2u*omega; EULERJ2B.M xmxxxmxxxxmxxmmmxmmmxxmmxmmmmmm 87 % y. Transformation lambda-dot X to [p X q r] mmmmmmmmmmmmmmmmmitxmmmm function omegadot = eul_i2b(x) y.'/.'/X/. seperate the composite vector %*/,%%% and [phi theta psi] 'x' into lambda-dot . ldot = [x(l); x(2); x(3)] phi=x(4); theta= x(5) psi»x(6); %%*/*%*/* calculate the rotation matrix {B} to {1} y.y.X'/. based on euler angles Rb2i = [1, sin(phi)*tan(theta) , cos (phi) *tan (theta) 0, cos(phi), -sin(phi) 0, sin(phi)*sec(theta), cos (phi) *sec (theta)] y.'/.y.'/. calculate lamda-dot omegadot = inv(Rb2i)*ldot; 88 ; P0SITI0N-B2I.M mmmmmmxmmxmxxmxmmxmmxmmmmxm x x % From the workspace: X X X X 1: free vector 'u* resolved in {B} e(l:3) X X 2: euler angle vector {phi,theta,psi} e(4:6) X X % % Returns X : X X X 1: free vector 'u resolved in {1} X X X function ans pos_b2i(e) XXXXX this will rotate the trajectory error through phi, theta, psi XXXXX from {b} to {i}. (3-2-1 transformation phi=e(4); theta=e(5) psi-e(6); ; ; ; ; m.psi = [cos(psi) ,sin(psi) ,0 -sin(psi) ,cos(psi) ,0 0,0,1]; m_theta = Ccos(theta) ,0,-sin(theta) 0,1,0 sin(theta) ,0, cos (that a)] m_phi [1,0,0 0, cos (phi) , sin (phi) 0,-sin(phi) ,cos(phi)] rotb2i = inv(m_phi*m_theta*m_psi) u = Ce(l); e(2); e(3)] ans = rotb2i*u; POSITIONJ2B.M xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx x x 90 X From the workspace: •/. X 1: free vector 'u' resolved in {1} e(l:3) X 2: euler angle vector {phi,theta,psi} e(4:6) 1: free vector 'u resolved in {B} */. '/. Returns X X X mmmmmmmmxmmmmmmmitmmmmiE function ans = pos_i2b(e) XXXXX this will rotate through phi, theta, psi XXXXX from {i} to {b}. (3-2-1 transformation) phi=e(4); theta=e(5) psi=e(6) m_psi = [cos(psi) ,sin(psi) ,0 -sin(psi) ,cos(psi) ,0 0,0,1]; m_theta = [cos (theta) ,0,-sin(theta) 0,1,0 91 sin(theta) ,0,cos(theta)] m_phi 0, [1,0,0 cos (phi) , sin (phi) 0,-sin(phi) ,cos(phi)3 roti2b (m_phi*m_theta*m_psi) u = Ce(l); e(2); e(3)] axis ; = roti2b*u; LIMITXOGIC.M xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx X X X funtion to limit trajectory commands, if required X X X X from workspace: X X 1: commanded inertial velocity X X 2: inertial wind X 92 X 3: lower IAS limit % 4: upper IAS limit revised commanded velocity returns: % x WW •/•/•/•/•/•/•/•/•/•/•/•/ •/•/»/•/•/ •/•/•/•/ WWWWWWWV'i function vcom=limit(u) seperate u vel.i = [u(l);u(2);u(3)]; =* [u(4),u(5),u(6)]; ll=u(7) ul=u(8); '/.'/•'/•'/•'/• calculate magnitud and direction of commanded velocity gs=sqrt (vel.i (1)~2 + vel_i(2)~2); ang=atan2(vel_i(2) ,vel_i(l)) 1*1*1*1*1*1* '/. y. •/•/•/•/•/•/•/•/•/•//•/•/•/•/•/•/•/•/ */*/*/*/*/ wind.i 1* % % 1X1*1,1 */. calculate commanded IAS (steady state) 93 vt= sqrt((vel_i(l)+vind_i(l))~2 +(vel_i(2)+wind_i(2))~2 + (vel_i(3)+wind_: y.'/.'/.'/.'/.y. Prepare return variable (may not be limited) vcom = vel_i; */.'/•'/•'/•%'/• Check limits and revise if outside if vt > ul; over = vt - ul; vcom(l) - (gs - over)*cos(ang) vcom(2) s (gs - over)*sin(ang) end; if vt < 11; under = 11 - vt; vcom(l) = (gs + under) *cos (ang) 94 vcom(2) = (gs + under) *s in ( ang) end; BLUEBIRD -DATA.M •/•/•/•/•/•/•/•/•/•/•/ WW •/•/•/ •/•/•/•/ WW*/ •/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/•/ •/•/•/ 35 Aircraft data for Bluebird X % mmmmmmmmmmummmmmmmxmxmi function [uO,wO,rho,Cfx,Cfo,Cfu,Cfxdot ,s,b,c,m,Pe,To,Ib] = blue_data /././. 7. trimmed flight speed and angle of attack /. uO = 73.3; wO =» 0; M.W.W. Density: Sea level- std day rho = .0023769; y.V/.V/. derivative matrix due to state variables 95 xxxxx rows: [CD CY CL CI Cm Cn] xxxxx col: [u v/U w/U p q r] Cf x = [0 .188 -.31 0; .0973; 3.94 0; 4.22 -.363 0.1; -.0597 -1.163 -11.77 -.0481 .0487 0; -.0452]; XXXXX derivative matrix due to control inputs XXXXX rows: [CD CY CL CI Cm Cn] XXXXX col: [elev rud ail] Cfu - [.065 0; .0697 0; .472 .0147 0; .0028 .265; -1.41 0; -.0329 -.0347]; XXXXX derivative matrix due to x-dot (alpha_dot k Beta.dot) 96 Cfxdot = [000000; 0; 1.32 0; 0; 0]; steady state force vector y.'/.'/.X'/. Cfo = [.03; 0; .385; 0; 0; 0] XXXXX physical dim. XXXXX WT =55 LBS. m 1. 7095; s = 22 .38; b s 12 .42; c ss 1. 802; engine data (4 HP motor) XXXXX Pe = [0; 0; 0] To = [15 ;0;0]; inertia tensor matrix y.y.'/.y.y. lb « [ 10 0; 16.12 0; 7.97]; 97 APPENDIX The nine file state nonlinear model E0M-9.m and was trimmed of bluebird contained in the is SIMULINK at a flight condition of • flight speed equal to 73 fps • flight path angle equal to zero using the SIMULINK FILES B: TRIM command. The resulting trim values for the state vector and input vector are: 73.3 -0.0023 and u = 0.2858 The model in LINMOD command was of Bluebird (contained in the Chapter in the III) MATLAB B synthesis SIMULINK file EOM-16.m and about this trim point. The resulting linear system file is described contained Linear 16. mat. The rudder was removed of the used to linearize the sixteen state nonlinear as a control input (remove the second column matrix) and the resulting linear model was used as a basis for the model contained model was used in the to determine the SIMULINK LQR 98 gain. file synthesis.m. The This synthesis synthesis model, Q and R weighting matrices, and resulting LQR gain is contained in the MATLAB file LQR-dat.mat. The full nonlinear simulation is contained in the SIMULINK file, The MATLAB ables. The file .m file simdata loads the workspace with the appropriate simdata trajectory schedule. plantl.m. calls the .m Any changes file to the trajectory. m commanded vari- in order to generate the trajectory or wind distur- bance schedule can be made in trajectory, m. A version of the nonlinear simulation that does not use the sensor blocks is contained in the SIMULINK file quicker than plantl.m. 99 plant2.m. It filter and runs considerably APPENDIX C: ©-IMPLEMENTATION PROOFS REFERENCED Identity. Lei x (z B? = const be given. Then d I ( B R(A)x) = - BI R(A)x x S-\A). (C.l) ti and = xxfR(A)S- -£ (fR(A)x) K Proof: To derive both equations we j ( = B uB B R(A)) t and the following will 1 (C.2) (\). need Poisson's Law: x B R(A), (C.3) identity: axb = -bxa for any vectors a and 6 of j ( (C.4) compatible dimensions. Now, consider B R(A)x) t = B wB = (j t x B R{A)x using equation (C.3), (C.4) and x Next, by the chain rule we = ( B R(A))x +{, = - B R(A)x R{A)j x t xB uB , (C.5) const. get i&RM*) = ^(^(a)x)|a = ^(^(A)x)S(A) %,. Equation (C.l) now follows by comparing equations (C.5) and (C.6). 100 (C.6) To obtain equation (C.2), consider j(iU(A)f R(\)) = j B R(\))fR(\) +'B (' R(\)j (fR(A)) = since B R(A)f R(A) = / VA. Now, using equations (C.3) and (C.7) d = -f R(A) B uB (fR(A)) dt A\. Suppose that assumptions A\ through Dim{x C 2) = dim(u) = get: (C.8) we obtain: = xxffl(A)5- 1 (A). ^(ffi(A)x) ..1 we x Finally, following the steps in the derivation of equation (C.l) Theorem (C.7) 0, t t A3 hold. <ftm(y 3 ). A2. The matrix si - Ad —Cc has full rank at s = Bc3 CC2 i 0. A3. The matrix pair (A ci, C \) is c observable. Then for each equilibrium point of Q in £ the following properties are observed: • the feedback systems %(Q , K )(r ) and T(Qi , K\ ) have the same closed loop eigenvalues; Ti(Q X )(r )(s) = L(A )T(Glo Ao 101 = [0 , /]x w /C, )(s)L-\A ), Proof: In the proof we D& D& set the controller matrices £) c i, to zero. This does not change the results but considerably simplifies the algebra. Further- more, we will drop explicit dependence of the controller parameters on a. Let (x vo ,x Po = Pov \ Ao ,u ,ro € S) be given. Consider the feedback interconnection J (Ao) and linear controller of the linear plant Qi AC/ . The F of this state matrix feedback system has the following form: A\ Ai Ai I F:= BjQcx BiCci Aci Bc3 (C.9) B \C\ C C2 Next we K . linearize the feedback interconnection of the plant However, in order to that, troller states first Q and the we must determine the x c i and x^ along the trajectory ro £ S. values of the con- From equation obtain: d —x clo = A d cl x cU + Br,cl —y u + B&Lr _ 1/k 1 (A , )e A ~n x ao = x c2o wo Notice, since along r CciaJcio + Cc2 L -1((Ao)e . : eo we = = 0, t/i = x C2 const, — get £ ~£ di ci — A. C \X C\ = Cc \x c 102 i . u controller = const (4.9) we = Now, using Assumption A3 we conclude that x c \ compute the In order to Q and K we must , first 0. linearization of the feedback interconnection of obtain the linearizations of equations (4.3) and (4.9) = about the operating points (x^^x^^uq) G £ and (x c i 0, cq = — y 2o the plant Q ro is = = «o,yi = determined by ro € S. The linearization of 0) respectively, The given by (4.4). 0,x c2o linearization of the controller K has the following form: U = A el U+Bj + Bc3 L- £c2 = 1 (0 2 1 Cclfcl + Cc2 L~ ($2 — -p) p) (CIO) It is easy to verify that the state matrix M:= Ax A2 L BaCiAi A| B el dA 2 M of %{Q , C )(r B + B^L- 1 A (C.ll) BrtP\B el Cc CcuLr \ Let 0" / L P:= 0/0 • 0/. .0 Obviously, "/ p-' )le M algebra it is 0' L- = 1 0/0 .000/ • easy to show that = P~ l MP r Ai A2 i A» BcxCxAy B B \C\A + Bc3 A 22 C C < 2 ci C c\ 103 ) is . BC \C\B Bc Ai A2 I Ai d \ / A\ A2 I Aa of the first M. The A cl B \C\B /] C ccl now part of the theorem an observation that the matrices and + £c3[0 cc2 o The proof B F follows from Assumption A2 and M are in the form of the matrices F and proof of the second part of the theorem consists of the following steps: 1. compute the transfer matrix of the linearization of the controller using equation (CIO) from controller inputs 6\, 2. 2 , K p to controller output apply a similarity transformation Pi / = to the linearization of the plant L- 1 Q (= Q\ (ro)) given by equation (4.4) derive the transfer matrix from the control inputs of the linear plant the outputs 6\ and 6 2 using this 3 (A) new 1 and 2 to get the A simple computation shows 01, #2, of the transfer matrices obtained in that the transfer matrix from the controller inputs is r/ given by: = Ccl (sl - Art)-\B#L4- to final result. p to the controller output f)(s) rj state-space realization; compute the feedback interconnection steps and B A{s) + Cc2 L- l c l -(b 2 (s) - p(s)) -(b 2 {s)-p(s)) 3 = Hs) A Us) L-ip(s) 104 (C.12) where Ki(s) is the transfer matrix for the controller K\ . Applying similarity transformation Pi to equation the transfer matrix from 77 to 0\ and " 3 Ai I L is A2 B • M JU (C.13) L . where the transfer matrix and computing results in: d few (4.4) given in packed matrix form. A simple observation shows that where G>&,(.s) is Now the transfer matrix for the plant Qi . routine algebra shows that the transfer matrix from p to 6 2 of the feedback interconnection of the transfer matrices in equations (C.12) and (C.13) is given by: Ti(Q X )(r )M = L(A )T(Glo 105 , fC, 1 )(3)X" (A ). REFERENCES 1. "A Non- Linear Simulation For An Autonomous Aerial Vehicle," Master's Thesis, Department Of Aeronautics, Naval Postgraduate School, Monterey, CA, September 1993. Kuechenmeister, David R., Unmanned " Integration of Differential GPS and Inertial Navigation using Complementary Kalman Filter, "Master's Thesis, Department Of Aeronautics, Naval Postgraduate School, Monterey, CA, September 1993. 2. Marquis, Carl, 3. Kaminer, I.I., Khargonekar, P.P., and Robel, G., "Design of Localizer Capture and Track Modes For a Lateral Autopilot Using #«, Synthesis," IEEE Control Systems Magazine, vol 10, pp. 13-21, 1990. 4. Roskam, Jan, and Lan, Edward, Airplane Aerodynamics And Performance, Roskam Aviation And Engineering Corp., 1980. Addison- 5. Craig, J.J., Introduction to Robotics Mechanics and Control, Wesley, New York, 1986. 6. Schmidt, L.V., Class Notes for AE3340, U.S. Naval Postgraduate School, Monterey, CA. 1992. 7. Roskam, J., Airplane Flight Dynamics and Automatic Flight Controls, Roskam Aviation and Engineering 8. Watson tries, 9. corp, Ottawa, Industries, Techical Specifications for KS, 1979. IMU-600D, Watson Indus- Eau-Claire, WI, 1993. Kaminer, I.I., Class Notes For AE4276, U.S. Naval Postgraduate School, Monterey, CA. 1993. 10. Kaminer, I.I., Pascoal, A.M., and Khargonekar, P.P., "A Velocity Algorithm For Implementation Of Non-Linear Gain- Scheduled Controllers, submitted for publication in Automatica. ' 11. Nelson, R,C, Flight Stability and Automatic Control, McGraw-Hill, Inc., 1989. Optimal Control Systems, John 12. Kwakernaak, H., and Sivan, Wiley and Sons, 1972. 13. Lagier, 14. Lagier, Mark Virtual Prototype Demonstration Tape, U.S. Naval Postgraduate School, Monterey, CA. March, 1994. R., Linear Mark, "An Application of Virtual Prototyping to the Flight Test and Evaluation of an Unmanned Air Vehicle, " Master's Thesis, Department Of Aeronautics, Naval Postgraduate School, Monterey, CA, March 1994. 106 INITIAL DISTRIBUTION LIST No. of Copies 1. Defense Technical Information Center Cameron Station VA Alexandria, 2. Library, 2 22304-6145 Code 52 2 Naval Postgraduate School Monterey, CA 93943-5002 3. Dr. Isaac Kaminer I. 5 Department of Aeronautics and Astronautics, Code AA/KA Naval Postgraduate School Monterey, CA 93943-5000 4. Dr. Richard M. Howard 1 Department of Aeronautics and Astronautics, Code AA/Ho Naval Postgraduate School Monterey, CA 93943-5000 5. Chairman Department 2 of Aeronautics and Astronautics Naval Postgraduate School Monterey, CA 93943-5000 6. Eric L. Hallberg 1 3328 Susquehanna Road Dresner, 7. PA 19025 Richard Kershaw 246 Glenbrook Lane Greenville, 1 SC 29615 107 No. of Copies 8. Eric N. Hallberg 372 F Bergin Drive Monterey, CA 93940 108 DUDLEY KNC" NAVAL POSTGk. MONTEREY CA 93*h4! GAYLORD S X>L

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

Download PDF

advertisement