G R O B L A A UNI VERST I T EModelling and Control of AutonomousQuad-Rotor2ndSemester Projectof the Intelligent Autonomous Systems Master ProgrammeGroup 10833Faculty of Engineering, Science and MedicineUniversity of Aalborg, DenmarkJune 2010G R O B L A A UNI VERST I T EDepartment of Electronic SystemsFredrik Bajers Vej 7B9220 AalborgDenmarkhttp://es.aau.dkTitle: Modelling and Control of Autonomous Quad-RotorTheme: Modelling and ControlProject period: Spring 2010Group: 10gr833Group members:Claudia MaryLuminita Cristiana TotuSimon Konge KoldbkSupervisor:Flemming Schler Ph.d.Synopsis:The scope of this project is to design and implementastabilizingandtrackingsystemcontrol forasmallQuad-Rotor platform through a (roll, pitch, thrust, yaw)commandinterface, yinginhighprecisionmotioncapture room.Based on system identication, analysis of the physi-cal platform, and the fundamental laws of mechanics,anon-linearmodelofthesystemhasbeendesigned.The non-linear model is linearized at hovering operat-ing point. A state feedback Linear Quadratic controllerwith step reference tracking has been implemented inMATLAB Simulink.With the implemented control scheme, the Quad-Rotoris able to hover autonomously and perform small stepmovements in all directions. While the evaluated per-formance did not meet the set requirements, it did showpromising results as proof of concept. Several simpleimprovementscanbeadded, suchasintegral action,andareexpectedtosignicantlyimprovetheperfor-mance.Copies: 7Pages: 139Attachments: 1 CD attachedCompletion of project: 01.06.2010PrefaceThis report is presented by group 10GR833 from the Institute of Electronic Systems at AalborgUniversity. The report documents the 2ndsemester master project in relation to the study ofIntelligent Autonomous Systems.Thesemesterhasthepurposeofgivingthestudentsanintensiveanddeepunderstandingofmodelling and control in relation to Intelligent Autonomous Systems. The project concerns themanoeuvring of the X3D Quad-Rotor using Vicon motion tracking. The goal of the project is todesign and implement a control scheme that allows the platform to y autonomously in the AAUMotion Tracking laboratory.Thereportisdividedintosevenmainchapters: Introduction, SystemAnalysis, StrategyandRequirements, System Modelling, Controller design, Fault detection and Project evaluation. Alltest journals and general documentation related to the project, are located in the reports appen-dices. The relevant simulation models, components data sheets and MATLAB documents arelocated on the project CD. The project CD also contains a short video of the X3D Quad-Rotor.The authors wish to express special thanks to Anders Friis Srensen for his generous supportduring the project.V of 139Contents1 Introduction 11.1 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.2 Chapter Summary. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32 System Analysis 42.1 X3D Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42.2 X3D Remote Control and Manoeuvring . . . . . . . . . . . . . . . . . . . . . . 42.2.1 Reference frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52.2.2 X3D Control Inputs . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2.3 X3D Manoeuvring . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72.3 Motion Tracking System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.3.1 Motion Tracking Lab. . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.3.2 Vicon MX . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.3.3 Tracking precision . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112.3.4 Tracking errors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122.4 Interface Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152.4.1 Vicon Link . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152.4.2 Quad-rotor command interface . . . . . . . . . . . . . . . . . . . . . . . 152.5 Chapter Summary. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173 Strategy and Requirements 183.1 Modelling Strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183.2 Control Strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193.3 Requirements specication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 223.4 Chapter Summary. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23VII of 139CONTENTS4 Modelling 244.1 Model Structure. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 244.1.1 Internal controller & X3D aerodynamics . . . . . . . . . . . . . . . . . 254.1.2 X3D thrust generation . . . . . . . . . . . . . . . . . . . . . . . . . . . 254.2 Non-linear Simulation model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 284.2.1 Non-linear Simulation model Verication . . . . . . . . . . . . . . . . . 284.3 Model Linearisation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 304.3.1 Taylor series approximation . . . . . . . . . . . . . . . . . . . . . . . . 314.3.2 Rigid Body Dynamics & Kinematics . . . . . . . . . . . . . . . . . . . 314.3.3 Thrust generation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 324.4 State Space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 334.4.1 System States. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 344.4.2 System Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 344.4.3 Matrix Derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 344.4.4 Discrete State Space model . . . . . . . . . . . . . . . . . . . . . . . . . 354.5 Chapter Summary. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 375 Controller Design 385.1 Linear State Feedback . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 385.1.1 Methods for State Feedback . . . . . . . . . . . . . . . . . . . . . . . . 405.2 Linear Quadratic methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 405.2.1 Optimal Problem. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 405.2.2 Linear optimization with quadratic criterion. . . . . . . . . . . . . . . . 415.2.3 Innite Horizon optimization . . . . . . . . . . . . . . . . . . . . . . . . 435.3 LQ algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 445.3.1 Dynamic Programming . . . . . . . . . . . . . . . . . . . . . . . . . . . 445.3.2 Matrix Calculus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45Convergence over Innite Horizon. . . . . . . . . . . . . . . . . . . . . 48Algebraic Riccati Equation . . . . . . . . . . . . . . . . . . . . . . . . . 485.3.3 Pseudocode Algorithm. . . . . . . . . . . . . . . . . . . . . . . . . . . 495.3.4 Tracking reference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 495.3.5 Choosing the Weight matrices . . . . . . . . . . . . . . . . . . . . . . . 525.4 LQ implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 535.4.1 Linear model simulations . . . . . . . . . . . . . . . . . . . . . . . . . . 53VIII of 139 CONTENTSCONTENTS5.4.2 Non-Linear model simulations . . . . . . . . . . . . . . . . . . . . . . . 555.4.3 State estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 555.4.4 Online controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 575.5 Chapter Summary. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 586 Fault Detection 596.1 Component analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 606.2 Failure mode & Effects analysis . . . . . . . . . . . . . . . . . . . . . . . . . . 616.3 Fault Detection & Isolation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 636.3.1 Unknown Input Observer . . . . . . . . . . . . . . . . . . . . . . . . . . 64Implementation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65Verication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 686.4 Fault Accommodation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 716.5 Chapter Summary. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 717 Project evaluation 737.1 Results and Conclusion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 737.1.1 Position control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 737.1.2 Yaw control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 747.2 Future Work. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75Bibliography 78A Performance Tests 79B Performance Tests Results 82C First Principles model 89D Vicon Data Management 97E Non-linear Model Verication - Test ight 98F Vicon tracking precision 101G X3D components 114H Thrust Force Measurement 118CONTENTS IX of 139CONTENTSI Angular Velocity Measurements 125J Frame Transformation matrices 133K Software Notes 139X of 139 CONTENTSChapter 1IntroductionIn this chapter an introduction to the project is given in order to set the scene for the reader.The rst section is an overall analysis of possible application domains and project warrant. Thesecond section presents the project problem statement and specic goals.UAS (Uninhabited Aerial Systems) have received increasing attention in the last decade as areplacement for expensive human-piloted systems [9][7][5]. They have become a widely usedplatform for many applications for which human operation are considered unnecessary, repeti-tive, or too dangerous. UAS have an extremely varying application domains that include bothindoor and outdoor applications. Possible UAS applications are enumerated in the list below: Aerial Surveillance and Intelligence for Law Enforcement Hobbyist and Professional Aerial Photography and Video Property Assessment and Real Estate promotion Trafc monitoring and Trafc pattern Analysis Weed identication in Agriculture Educational learning PlatformsThe listed application domains require different levels of control and manoeuvrability. Someapplications may need high precision, while others may have a larger tolerance.An example of UAS is the RQ-11B Raven B that is used by several countries worldwide - includ-ing Denmark. The Raven illustrates the usefulness of small UAS for reconnaissance and surveil-lance as a xed wing platform for outdoor environments, and can cover a large area. Fixed wingplatforms are not intended to be own at low altitudes, indoors, or in an obstacle prone zones.For these types of applications, rotor based systems are preferred due to higher manoeuvrabilityand hovering capabilities.High performance combined with an increasing availability and reduced prices have led to agrowing interest in using rotor based platforms. One common task for quad-rotor autonomous1 of 1391.1 Problem StatementFigure 1.1: Picture of the platform used in this project - the X3D Quad-Rotoright is a conned environment in presence of multiple types of obstacles. Obstacles may bestationary like buildings or dynamic like other aerial crafts or human beings. In this project aQuad-Rotor platform, called X3D, is used as vehicle of designing a control system capable ofexecuting autonomous ight, hovering, and movement. The X3D Quad-Rotor is illustrated ingure 1.1.The application chosen as basis for this project is a ying museum custodian. The objectiveof the system is to provide a technological and entertaining alternative to human custodians. Thesystem is envisioned to guide guests around in a museum environment with multiple obstacles.The described application will be used as reference throughout the report and will be refereed toas the project application.Figure 1.2: Picture of a museum landscape with showcases [www.museodirect.com]1.1 Problem StatementIn this section the Project Problem Statement will be addressed. The Project Problem Statementis inferred from the desired functionality of the project application and the necessary controlproblems that must be addressed to achieve the desired functionality.2 of 139 1. Introduction1.2 Chapter SummaryThe goal of this project is to develop and implement a viable control system for the X3D Quad-Rotor. The control scheme must enable the X3D to perform stable position hovering and trajec-tory tracking within the limitations of the requirements specication described in more detail insection 3.3. The process of generating the trajectories for the X3D is not addressed in this projectas it is considered beyond the scope of the project.InordertoenabletheX3Dtotrackapredenedtrajectoryseveralcontrolproblemshavetobe addressed and solved. As the X3D system does not feature any means of performing stablehovering at a predened way-point1, a stable hovering controller must be designed and imple-mented. Subsequently, this controller has to have the ability to manoeuvre the X3D from oneway-point to another. If the results from these rst tests are satisfactory, the controllers abilityto track a continuous trajectory may be investigated.The collective goals and thus the course of action are described in the following problem state-ment.Is it possible to design and implement a system control that enables stabilization of the X3DQuad-Rotor position and attitude?The development of the collective system control can be divided into several sub-goals. Thecompletion of the collective sub-goals is necessary in order to develop a viable system controlthat will enable the X3D to track the requested way-points form the trajectory controller. Thecollective sub-goals of the project are listed below and serve as a course of action for the project. Design a viable state space model for the X3D Quad-Rotor Design and implement a stable hovering controller for the X3D Quad-Rotor Investigate the designed controllers ability to manoeuvre the X3D Quad-Rotor from one way-pointto another. Investigate the designed controllers ability to track a continuous trajectory.1.2 Chapter SummaryIn this chapter the main objectives of the project have been addressed. The project applicationhas been presented as a means of validating the development of system control for the X3DQuad-Rotor. Having set the scene for the reader in relation to the mission of the project and theencompassed elements, it is considered appropriate to address the nature of the project equip-ment and methods. The project equipment includes the AAU laboratory MTlab and the X3DQuad-Rotor. The process of describing these issues will be the main focus of the next chapter -Project Analysis 2 on the following page. Next, the project modelling and control strategy willbe discussed.1Reference point in physical space used for purposes of navigation.1. Introduction 3 of 139Chapter 2System AnalysisIn this chapter the project laboratory facilities and equipment are described. The rst part ofthe chapter deals with the physical design of the X3D and presents the X3D hardware whichas a collective make up the X3D Quad-Rotor. The second part of the chapter deals with theX3D manoeuvring possibilities, the control inputs and presents an introduction to the physicalforces that effect the X3D. The third part of the chapter deals with the software used in relation tocommunicating with the X3Dand the motion tracking laboratory - MTLab AAU. The MTLab AAUis also addressed in a dedicated section where the laboratory set-up is described as well as thetracking precision. The nal objective of this chapter is to enable the establishment of the usedmodelling and control strategy. It will also enable the formulation of the project RequirementSpecication which will severe as benchmarks throughout the project.2.1 X3D HardwareThis section presents the hardware components of the X3D. The X3D has been assembled, main-tained, and updated by multiple groups in the university laboratory. The platform is based on theX-3D-BL kit from Ascending Technologies [1] hobby line.In gure 2.1 the main hardware components of the X3D are illustrated. These are the X-CSMbody frame composed of 4 booms and a structural core, the X-Base main electronic board, X-BLbrushless motors and driver circuits, the radio controller receiver, and the Research Pilot sensorand control board. Based on the descriptions from the Ascending Technologies manual [14], adetailed presentation of the components has been compiled in appendix G on page 114.2.2 X3D Remote Control and ManoeuvringThe X3D is a highly manoeuvrable quad-rotor that enable the execution of precise and rapidmovementsin3Dspace. InthissectionameticulouspresentationoftheX3Dmanoeuvringpossibilities is presented. Moreover a series of reference frames used in the process of modellingthe X3D are introduced to better visualize the manoeuvres. This section also contains a smalldescription of the X3D remote control input.4 of 1392.2 X3D Remote Control and ManoeuvringIR-MarkerDC-MotorRotor CoreBoomSpectrumTransmitterluetooth Wireless LinkFigure 2.1: Illustration of the X3D Quad-Rotor hardware components2.2.1 Reference framesTo better understand the manoeuvring of the X3D a series of reference frame are introduced.In short the reference frames describe the coordinates system or environment in which the X3Dmanoeuvres. The coordinates system will be used as a reference in both this section as well as inthe modelling chapter 4 on page 24.The X3D modelling environment is illustrated in gure 2.2 where E is the earth frame and Bis the body frame. The earth frame E will be the reference frame throughout this chapter. Theframe is related to the MTlab in which the X3D is own and is orientated so the xy-plane is inthe laboratory oor and the z-axis pointing downwards. The body frame B is related to the X3Das shown on the gure 2.2.The vectors in the E frame will be referred as Ea and those in the Bframe as Ba. The X3D is able to rotate around all three body-axis. The sign of the rotations haveFigure 2.2: Illustration of the right-handed coordinate systems as dened in this projectbeen dened based on the signs related to the input commands from the wireless link. Positiveinputs result in positive right-hand rotations and visa versa. On the physical X3D the front ismarked with red tape to better identify the attitude of the X3D when own by a human operator.The x-body axis The Bx is always aligned with the front of the rotor. The orientation of the X3D2. System Analysis 5 of 1392.2 X3D Remote Control and ManoeuvringFigure 2.3: Illustration of the coordinate systems xy-body plane viewin the E frame can always be described by three successive right-hand rotations around the threeaxes. The rotations used in this project are a 3-2-1 sequence that is read as rotations aroundx,y then z axis also called roll, pitch and nally yaw. This sequence of rotation is called 3-2-1Euler angles and is addressed in detail in appendix J on page 133 along with the transformationmatrices used in the process of modelling the X3D.2.2.2 X3D Control InputsTheX3Disanaerialsystemthatusesfourpropellerstogeneratelift. TheX3Dfallsunderthe denition of a Quad-Rotor that can be classied as a type of helicopter. Most Quad-Rotorsuse xed-pitch blades as opposed to most commercial helicopters. The system is controlled byvarying the rotational velocity of each of the rotors and thereby changing the thrust and torqueproduced.Figure 2.4: Illustration X3D wireless remote control interfaceThe X3D is designed for manual control i.e. human control with visual feedback and it is thusnot possible to directly control the rotational velocity of the individual rotors. The X3D featuresseveraldifferentmodesofcontrolbutinthefollowingonlytheHeading-Holdmodewillbeaddressed. For a thorough description of the other modes see G on page 114. The X3D own inHeading-Hold mode utilises a on-board control circuit that manages the rotational velocity of thefour rotors and adjusts the velocity based on on-board sensory data. The human operator is onlyable to control the angular velocities of the X3D along the three body-axis and the generatedthrust. This allows a skilled operator to control the attitude and position of the platform thusenabling the operator to maintain the X3D in hover. In gure 2.4 the wireless remote controlinputs related to the change in platform attitude are illustrated.6 of 139 2. System Analysis2.2 X3D Remote Control and ManoeuvringParameter Min Max Nominal rangeRoll -2047 2047 [-1500 - 1500 ]Pitch -2047 2047 [-800 - 800 ]Yaw -2047 2047 [-800 - 800 ]Thrust 0 4096 [0-1800 ]Table 2.1: Wireless link output range [14]The control signals used by the wireless remote control interface are the same as the ones that areaccessible from the wireless link. The wireless link is the communication channel between theinterface PC and the X3D. The wireless link output range (references for the on-board controller)is illustrated in table 2.1. The nominal range is based on observation of the reference rangewhen the X3D is own in a aggressive behaviour by a human operator. The designed systemcontrol should be well beneath this range as the the operator focused on given the X3D the largestreferences possible without crashing. As the X3D is to be modelled it is considered appropriateto address the manoeuvring of the X3D.2.2.3 X3D ManoeuvringHaving presented the control Inputs of the X3D and the reference frame terminology the actualmanoeuvring possibilities of the X3D is presentedTheX3Dgeneratesupwardforceusingfourrotorswhichangularvelocitycanbecontrolledusing the wireless control interface 2.4. The force components generated by the four rotors areillustrated in gure 2.5. As the rotors rotate they are subject to drag caused by the air beingmoved. The drag results in a reactive moment on the rotors known as the induced moment. Theinduced moments 1 - 4 are illustrated on gure 2.5. The induced moment acts on the rotor in theFigure 2.5: Illustration of the angular velocity of the rotors while hoveringopposite direction of the rotors direction of rotation. On a classical helicopter the tail rotor coun-ters the induced moment caused by the main rotor but on a Quad-Rotor the effects of inducedmoment is cancelled out by rotating the motors in pairs in opposite directions as illustrated ingure 2.5. Rotor 1 and rotor 3 see gure 2.5 are rotating clockwise and the other two rotors, 2and 4, rotate counter clockwise.2. System Analysis 7 of 1392.2 X3D Remote Control and ManoeuvringGiven are Newtons second law of motion 2.1 and the rotational analogue to the law 2.2. F isthe total applied force to a rigid body, m is the mass of the body and a is the linear accelerationof the body. is the total torque exerted at the body and J is the body moment of inertia and isthe angular acceleration [6, page 117 ].F = m a (2.1) = J (2.2)When the X3D is in perfect hover it is in both perfect force and torque balance. As the rotors areof the same design the equal angular velocity will result in equal thrust and torque. Because ofthis the attitude of the X3D is preserved when equal lifting forces are created by each rotor. If therotors are rotating at the same angular velocity the total torque acting on the X3D is equal to zeroand the angular acceleration around either axis is exactly zero meaning that the X3D is hovering.This means that when the X3D is hovering the torque created by the rotors in the x-body axis iscancelled out by the torque created by the rotors in the y-body axis. The opposing moments putsimmense stress on both the booms and core of the X3D. The materials used for the booms andthe core are thus chosen to withstand harsh treatment. For a complete description of the X3Dhardware view section 2.1.RollRoll is when the X3D performs a rotation around the x-body axis. This is achieved by changingthe angular velocity of the rotors on the y-body axis that is rotor 2 and 4 while maintaining thesame angular velocity on rotor 1 and 3. If the desired rotation is positive the roll is performedby increasing the angular velocity of rotor 2 and decreasing the angular velocity of rotor 4. Thisresults in a positive roll or rotation around the x-body axis. The rotor velocities related to thismanoeuvre are illustrated in gure 2.6.Figure 2.6:Illustration of the angular velocities related to positive roll or rotation around thex-body axisPitchPitch is when the X3D performs a rotation around the y-body axis. This is achieved by changingthe angular velocity of the rotors on the x-body axis that is rotor 1 and 3 while maintaining the8 of 139 2. System Analysis2.2 X3D Remote Control and Manoeuvringsame angular velocity on rotor 2 and 4. If the desired rotation is positive the pitch is performedby increasing the angular velocity of rotor 1 and decreasing the angular velocity of rotor 3. Thisresults in a positive pitch or rotation around the y-body axis. The rotor velocities related to thismanoeuvre are illustrated in gure 2.7.Figure 2.7: Illustration of the angular velocities related to positive pitch or rotation around they-body axisYawYaw is when the X3D performs a rotation around the z-body axis. This is achieved mismatchingthe torque generated by the X3D rotors. During normal ight the net torque acting on the plat-form body is zero as the rotors along the x-body axis and y-body axis apply the same amountof torque to the platform. If the torques applied along the axis are mismatched the platform willperform a rotation around the z-body axis. If the desired rotation is positive the yaw is performedby increasing the angular velocity of rotor 2 and 4 while decreasing angular velocity of rotor 1and 3. This results in a positive yaw or rotation around the z-body axis. The rotor velocitiesrelated to this manoeuvre are illustrated in gure 2.8.Figure 2.8:Illustration of the angular velocities related to positive yaw or rotation around thez-body axis2. System Analysis 9 of 1392.3 Motion Tracking System2.3 Motion Tracking SystemIn relation to the project, it is necessary to continuously track the position and attitude of the X3DQuad-Rotor. This is achieved in the AAU MTLab, using the installed Vicon MX - a powerfuldigital optical system for motion capture. It it possible to track real-time, with high precision,moving objects within an indoor room.A specialized motion capture system is not a practical approach for a real deployment,but itis a very good setting for the initial design and tests of the model and control algorithm, since itprovides highly precise, low noise data for the position and orientation of the ying X3D. Thisis considered to be within the project objectives.2.3.1 Motion Tracking LabThe AAU MTLab (Motion Tracking Lab) consists of two rooms. The capture room is 5 x 6 x3 [m] obstacle free room with a multi-point video camera system. The adjacent control roomallows the operator to observe the capture room through a large plexiglas window. The controlroom also contains computers and additional hardware related to both autonomous and directcontrol of various UAS used at the AAU. Figure 2.9 shows pictures from MTLab as it appearedin March 2010.Figure 2.9: Pictures from MTLab AAU March 20102.3.2 Vicon MXThe Vicon MX system consists of special cameras, a hardware control module, and a host com-puter with software to analyze, relay, and present the data. The system can supply the positionand orientation of any tracked object. This information can also be accessed by any station onthe MTLab local network, by directly connecting to the host computer via TCP/IP. Moreover,Vicon Link is an easy way to use MATLAB Simulink block, build on top of the TCP/IP Viconprotocol, and is part of the MTLab available software. The Vicon link is the endpoint used in theproject to connect to the Vicon system. It is presented in the Control Software section.Though the group is not interfacing directly with the Vicon motion tracking system, it is consid-ered appropriate to give an overall description of the system structure, and highlight two aspects10 of 139 2. System Analysis2.3 Motion Tracking Systemthat need further consideration. In gure 2.10 the general structure of the MTLab is illustrated.The Vicon MX system works by identifying special markers (reective spheres). The markersXBeeRF moduleX-3D-BLQuad RotorVICOM MXBoxEthernetHUB100 MbEthernet100 MbEthernetGigabitEthernetUSBCableControl PCInterIace PCFigure 2.10: Illustration of the MTLab hardware structureare mounted on the object and tracked with high precision, at a data rate of 100Hz. The systemsupports both single and multi-body systems, where several markers are used to determine theposition of multiple parts of the body. The X3D quad-rotor is tted with a total of ve markersthat allow the determination of both the center mass position, and the attitude. In the captureroom, a conguration of 7 cameras is set in place. At any given time, a marker must be in the lineof sight of at least 4 cameras to be correctly monitored by the system. The tracking zone is specif-ically designed for aerial objects: the cameras are placed high in the room providing focus bothon the ground level and the on the y zone in the center of the room, where there are no obstacles.The Vicon cameras are connected to a controlling hardware module, the Vicon MX box. Thebox provides signal synchronization and is connected to the Interface Computer through a Gi-ganet Ethernet interface. Furthermore, the Interface Computer is connected by an Ethernet hubtoalocalareanetwork. ThesignalsfromtheViconMXboxaremanagedontheInterfaceComputer using the Vicon iQ and RTE (Real Time Engine). This software provides a GUI andtools to manage, set-up, record, and process a motion capture session in real-time. An impor-tant concept is the object model, dened by the user as a collection of markers with additionalproperties. Based on the model denition, the object is tracked with high accuracy, even in sit-uations with complex interactions between multiple objects and large number of markers. Theposition information of the markers coming from different cameras is combined and related withthe model denition, making it possible for the software to calculate the center mass position andthe orientation angles.2.3.3 Tracking precisionVicon precision in tracking a marker is said to be sub-millimeter on the MTLab website [8]. Inorder to get an estimate of the actual precision for the X3D quad-rotor dened as a Vicon iQobject model with 5 markers, a simple experiment was performed. The experiment measures the2. System Analysis 11 of 1392.3 Motion Tracking Systemnoise in the data when X3D is stationary, and is documented in the appendix F.It is concluded that the noise is very low, and more than adequate for the project. The recordedmaximumnoisetrackingthepositionofthestationaryquad-rotorwiththemotorsturnedon(platform is vibrating) is below 1.7 [mm] on each axis, and the maximum recorded noise in themeasured Euler angles is below 1.4 [].In the 3D space representation, the position measurements can be been plotted inside a cube withthe sides of 3.4 [mm].Figure 2.11: View of the position measurements taken with engines on, platform vibrating2.3.4 Tracking errorsWhile the Vicon system has a very good precision, it is limited to a relatively small area in thecenter of the room that is very well covered by all the cameras. Whenever the object is outsidethis area, some of the cameras lose sight of some or all of the markers, and tracking error occur.Some of the most important type of Vicon errors are discussed below. A precision recordingsession for a longer period of time shows level jumps in the signal values. These are caused by12 of 139 2. System Analysis2.3 Motion Tracking Systemone or more of the camera losing the object, or some of the reective markers, and the combinedtracking data from the rest of the cameras becomes offseted. In the Vicon case, the jumps in thesignal are very small. In GPS tracking systems, the same type of errors occur, at a larger scale.The following data has been recorded while the quad-rotor motors were off.Figure 2.12: Tracking jumps in the position signalsFigure 2.13: Tracking jumps in the orientation signals2. System Analysis 13 of 1392.3 Motion Tracking SystemAn in-ight recording shows other three common Vicon errors: signal-dead zones, spikes, andnoise.Figure 2.14: Errors in the position signalsFigure 2.15: Errors in the attitude signals14 of 139 2. System Analysis2.4 Interface Software2.4 Interface SoftwareMATLAB with Simulink is the predilect software environment choice for the project. The ma-jority of the tasks have been performed under the MATLAB environment: measurements, ofinecalculations, simulations, and the online implementation. The software from [8] that is used tointerface with the hardware is in the form of custom Simulink blocks compiled from C++ sourcecode with mex compiler. The operating system of choice for the project is Windows XP.2.4.1 Vicon LinkThe "Vicon Link" Simulink block is part of the MTLab software and was used without mod-ications. It implements a TCP/IP socket connection to the Vicon Interface Computer, wherethe Vicon RTE services on the default port 800, providing the tracking data on a selected objectmodels at a data rate of 100Hz.The Vicon block has 3 parameters: the IP address of the Interface Computer, the identier of thetracked object as dened in Vicon iQ (multiple objects are possible, however this usage is outsideof the of the project scope), and the sample time of 0.01 seconds, equivalent of 100Hz.Outputs of the Vicon block that are used in the project are Position and Euler signals. There are3 position signals (x, y, z) measured in meters and 3 Euler angles (roll, pitch, yaw) measured inradians.Figure 2.16: Vicon link block2.4.2 Quad-rotor command interfaceIt is possible to interface with the X3D quad-rotor through a serial interface. The Research Pi-lot sensor board has an Universal Asynchronous Receiver/Transmitter (UART) integrated circuitcontroller, the key component of the serial communications subsystem. The UART takes bytesof data and transmits the individual bits sequentially. At the destination (the PC), a second UARTreassembles the bits into complete bytes.The serial link between the quad-rotor and the control computer is set-up through a pair of small,wireless RF modules that use IEEE standard 802.15.4 standard to communicate. One of the mod-2. System Analysis 15 of 1392.4 Interface Softwareules is connected to the X3Ds UART connector, and the second is connected to the PC via anUSB port.The AscTec manual describes a simple serial protocol and the format of the packages that can beexchanged over the serial link to command the X3D quad-rotor and receive sensor informationfrom the on-board electronics. "X3D interface" is a Simulink block that implements the AscTecserial protocol. It is used in the project as the quad-rotor communication endpoint. The blockFigure 2.17: X3D Simulink blocktakes as inputs the 4 standard commands and 4 corresponding activation ags: pitch, roll, yaw asreferences for the angle velocities, and the reference for collective thrust. The command packetmust be send at a rate of minimum 20Hz and a maximum of 200Hz over the serial link.Theoutputoftheblockaretherawpitch, roll, andyawdatafromon-boardX3Dfromtheelectronic gyroscopes, the acceleration rates, and the readings from the 4 R/C channels. It is thuspossible to record the commands from an operator ying session. The output data packet ratecan be set from a minimum of 5Hz to a maximum of 300Hz.An important note is that throughout the project, the raw sensor data from the X3D on boardinstruments ( electronic gyroscopes, accelerometer, pressure sensor) is not used for control. In-stead, the more accurate Vicon measurements of position and orientation are used. The X3Dinterface is used only for command, and was also used during the system analysis and design torecord manual commands for the system identication part of the modelling.X3D interface is a new MTLab software package, initially available only for the Linux plat-form. It was required to port the software to Windows operating system. Two main tasks havebeen carried out: setting up the build procedure with small changes in the existing code for op-erating system compatibility, and implementing and integrating the serial interface component,which is platform specic. More information can be found in the Appendix K.16 of 139 2. System Analysis2.5 Chapter Summary2.5 Chapter SummaryIn this chapter the several main issues of the project has been addressed. The X3D has beendescribed in relation to physical characteristics, hardware components and maneuvering possi-bilities. The overall structure of the MTLab AAU has been described as well as the softwarerelated to communicating with both the MTLab AAU and the X3D. As an additional feature thetracking precision of the MTLab AAU has been investigated and found to be sufciently precisefor control.The following chapter will address the strategy related to modelling the X3D and the strategyfor designing the system control.2. System Analysis 17 of 139Chapter 3Strategy and RequirementsHaving described the physical features of the X3D and the AAU MTLab, it is considered appro-priate to introduce the modelling and control strategy. The modelling and control strategy willenable the group to identify the main aspects of project and provide a solid starting point for thefurther development of the project.3.1 Modelling StrategyIn order to design a valid control scheme for a system, a representative model is needed.An often used approach in relation to modelling physical systems is to divide them into subsys-tems and to dene the input and output relations of the subsystems using the laws of physics.The model should prevail from assumptions such as empirical modelling and tting of the modelparameters. This approach is often called the First Principles model as it is based exclusively onthe laws of physics.In relation to the X3D, it is possible to decompose the collective system into subsystems thusmaking the system more manageable. The X3D subsystems can be represented as illustrated ingure 3.1. Using the above decomposition and the established laws of physics, it is possible toInternal AttitudeControl DC -motor Dynamics Rotor -PlatfromAerodynamicsForces&TorquesRigid BodyDynamics & KinematicsFigure 3.1: Illustration of the X3D model sub-blocksderive the entire model of the X3D system. The opening steps are documented in appendix Con page 89. However, during this opening analysis of the X3D First Principles model, it becameclear that it was possible to model the Quad-Rotor using different techniques and that using aFirst Principles approach would add unnecessary work to the project process.18 of 1393.2 Control StrategyThe concept of controlling the X3D has been addressed by previous groups at the University.Friis, Nielsen, Andersen, Bnding, JochumsenandSrensenshowedthatlargepartsoftheX3D can be modelled using system identication [7]. [7] presented a valid control scheme andimplemented an autonomous hovering control for the X3D using a simplied model based onsystem identication. The model included an approximation for both the thrust generated by theX3D, and the dynamics related to the angular velocities along the three body-axes.Basedontheworkdoneby[7]thegroupintendstousetechniquesofSystemIdenticationinstead of a complete First Principles approach. It is assumed that this method will lead to a sim-ple and manageable model that will be representative for the response of the X3D system, andadequate for control. To this end, some attributes of the platform are approximated, and specicfeatures of the X3D may be omitted if their contribution to the nal model can be consideredinsignicant. Below is a list of the assumptions that have been made in relation to the modellingof the X3D. The X3D is modelled in normal operation that is in near hover ight. Booms and body are symmetrical along the xy-body axis. The X3D structure, booms, body, and rotors, are rigid. Any ground effects are assumed to be minute. The X3D is modelled in the Heading-Hold mode. View section G on page 117 for description ofHeading-Hold mode.The listed assumptions enable to disregard several complex phenomenons that effect the X3D,such as the ground effect. These phenomenons are less signicant when the X3D is in near hoveright at a medium height.3.2 Control StrategyThe objective of this project is to stabilize and control the X3D Quad-Rotor in ight. Based onthe application description, three main operational objectives have been identied:Autonomous hovering at a static way pointAutonomous manoeuvring from a static way point to anotherAutonomous trajectory tracking following a dynamic path referenceDifferent approaches can be used to achieve the performance required by the three modes of op-eration. There are inherent differences between static way points manoeuvring and following adynamic path reference, with the later being more complex than the former.Figure3.2illustratesacontrollerstructurecapableofmovingbetweenstaticreferencewaypoints. Inthiscongurationthereferencesconsistofstaticx, y, andzcoordinatesandyaw3. Strategy and Requirements 19 of 1393.2 Control Strategy0EuEoExEyEzExByBzBTxEreI yEreI zEreITo0uSSSSc0EuEoExEyEzETuEreIOperating point CompensationLinear ControllerQuad- RotorSystem SensoryManagementFigure 3.2: Static position reference controllerangle. To move from its initial position to the newly given way point, the linear controller gen-erates the pitch, roll, thrust, and yaw commands based on the given reference and the measuredstate of the system. In this control scheme there are no constraints on the yaw reference angle.Although not obvious, free yaw reference requires an operation-point compensation technique.The linear controller is based on a linearized system model, and will operate correctly only in theproximity of the chosen operating point. The operating point that xes several system states, suchas attitudes and translational velocities 4.27, and thus includes the yaw. To support the entire yawrange, the controller scheme must compensate for the rotation of the reference frames. Changesin yaw angle will result in the misalignment of the two system frames and special care must thusbe used as the controller commands are issued relative to the Quad-Rotors body frame, while themeasured attitude signals are in the Earth reference frame. Appendix J describes in more detailreference frame transformations. When following a trajectory path, the reference is dynamic andincludes multiple parameters in order to ensure a smooth and precise tracking. In gure 3.3 thestructure of the system control in relation to a possible implementation of a tracking controller isillustrated.0EuEoExEyEzExByBzBTxEreI yEreI zEreITo0uSSSSc0EuEoExEyEzEToeB0eBueBxEreI yEreI zEreI uEueTrajectoryControllerOperating Point CompensationLinear Controller Quad-RotorSystemSensory ManagementFigure 3.3: Dynamic position and velocity reference controller20 of 139 3. Strategy and Requirements3.2 Control StrategyA controller specically designed for trajectory tracking will most likely be able to achieve ac-ceptable performance in all three modes of operation, but the design of the trajectory trackingcontroller is more complex than that of the static position reference controller. The system con-tains several reference values and also requires that operating-point compensation be introducedfor more states.Having described the overall difference between the required modes of operation it is clear thatone of two approaches must be chosen. Designing and developing a trajectory tracking controllerwill provide the best performance in relation to the Project application. However, as this is thegroups rst time working with the Quad-Rotor some simplications in relation to the developedsystem control are considered acceptable. The chosen system control is thus designed with thestatic waypoint manoeuvring in mind. This decision calls for a series of requirements which willease the design of the system control. The requirements are listed below. The system control is designed as a Hovering controller The system control must feature some static waypoint manoeuvring capabilities The X3D yaw angle is maintained at zero meaning that the Earth and body frame are aligned (x-body axis and x-earth axis are kept parallel)In gure 3.4 the control scheme used in this project is illustrated. This controller is designed forhovering and will be tuned include to tracking of step references. It is assumed that an acceptableresponse will be achieved using the illustrated structure. The X3D system in this conguration0EuEoExEyEzExByBzBTo0uSSSSc0EuEoExEyEzETxEreIyEreIzEreIuEo0uFSSSzLinearControllerQuad-RotorSystemreIStaticReIerenceSensoryManagementNon-linearmapFigure 3.4: Final control scheme strategyhas a total number of six outputs.The position P and the attitude as Euler angles are all seenby the Vicon Tracking system that is in the Earth frame. The Sensor management block is used toestimate additional system variables that are not directly accessible. The structure of the Sensormanagement block has not been dened at this point. The reference to the system consists ofthe x, y and z position. The operating point for the yaw angle will be xed to zero. In this rstattempt at designing a system control for the X3D it is assumed acceptable not to include theuse of multiple operating points. The structure illustrated in gure 3.4 will be used as referencethroughout the rest of the report.3. Strategy and Requirements 21 of 1393.3 Requirements specication3.3 Requirements specicationIn this section the Project requirements specications are outlined. The Project requirementsspecications are based on the analysis of typical obstacles in an indoor environment dened bythe project application.Given the context described in the modelling and control strategy, the performance of the controlis to be evaluated by using two tests. The rst test investigates static hovering behavior and thesecond position tracking for steps in the x,y, and z axes.The objective of the X3D is to assume the role of a museum custodian, leading the guests aroundthe exhibition and supplying information about the exhibited objects. The X3D must navigatetrough the museum with great position precision. In this project, only static obstacles will beaddressed as the scope of the project does not include active obstacles avoidance. In most mu-seums, the dominating static obstacles are glass display cases or exhibition cases. These usuallyrectangular structures prevent the museums guests from touching the objects on display whilethey still enable the guests to fully view them.And as most display cases are made of glass, it ishighly desirable that the X3D does not come into contact with the fragile structure.In this context it is know that the X3D will not be able to perfectly maintain its position due todisturbances and hovering, different requirements are needed in relation to the precision to whichthe X3D manoeuvres. Based on the size of the platform and the project application and and workdone by [9] the following precision requirements have been dened. During autonomous hovering the Quad-Rotors z-axis position should be kept within 0.10 [m]. During autonomous hovering the Quad-Rotors xy-axis position should be kept within 0.05 [m]. During autonomous hovering the Quad-Rotors angle around the z-axis should be kept within 10.00 []. When changing the position 0.5 meter in either the x, y or z axis, the X3D should move to the newposition and remain in stable hover in the new position within 2 seconds.The angles around the xy-axis do not have any requirements since the quad rotor is hovering andadjusting pitch and roll to maintain its position. The stated requirements are not meant to be tech-nical requirements in relation to the designed control scheme but they are considered necessaryrequirements if the X3D is to manoeuvre in the project application.In order to validate the performance of the X3D a series of performance requirements tests havebeen devised. The objective of these tests is to validate that the designed system control schememeets the formulated requirements. The tests and a thorough description of their execution canbe found in appendix A on page 79. Notice that the tests are divided into two parts of varyinglevels of difculty. This division stems from the division used in the Problem Statement 1.1where a similar approach has been used in relation to the project sub-goals. It is however noticedthat all the performance requirements tests must be completed and passed in order for the projectto be deemed a complete success.22 of 139 3. Strategy and Requirements3.4 Chapter Summary3.4 Chapter SummaryIn this chapter related to modelling the X3D and the strategy for designing the system controlhave been presented. Based on the Project application and the X3D limitations addressed in thechapter a series of Requirement Specications and Requirement Specications Test have beenformulated. The following chapter will address the modelling of the X3D system and supply avalid non-linear system model that can be used in the process of designing and simulating thesystem control scheme.3. Strategy and Requirements 23 of 139Chapter 4ModellingIn this chapter, the non-linear dynamic system equations are derived. The used modelling ap-proach consists in writing mathematical equations that describe the movements and dynamics ofthe X3D as derived from the governing laws and principals of classical physics. The nal goal ofthis chapter is to produce a set of non-linear dynamic system equations that describe the physicalbehaviour of the X3D and implement them in MATLAB Simulink.4.1 Model StructureAsillustratedinappendixConpage89theprocessofmodellingthedynamicbehaviourofthe X3D using a rst principles is not an easy task. In this section the revised system modelstructure is addressed. The use of the internal controller removes several of the dynamic featuresrelatedtomodellingtheX3Dandenablestherevaluationofthesystemmodelstructure.There-evaluatedsystemmodelstructureisillustratedingure4.1AsitcanbeseenfromgureForce PolynomialRigid BodyDynamicsRigid BodyKinematicsInternal Attitude ControleScSTotalFaBePOFigure 4.1: Illustration of the approximated sub blocks4.1 the complexity of the system model has been reduced signicantly. The dynamics relatedto the angular movements of the X3D are managed by the internal controller. The translatorymovements are modelled using a combination of system identication and classical physics. Inthe following section the different sub-blocks illustrated in gure 4.1 are addressed in detail.24 of 1394.1 Model Structure4.1.1 Internal controller & X3D aerodynamicsIt has been chosen to use system identication for this part of the model. The method presentedby Friis, Nielsen, Andersen, Bnding, Jochumsen and Srensen entails the use of a high ordertransfer function to model the dynamics of the internal controller and X3D aerodynamics [7].This approach however renders the system equations more complex and thus increases the workrelated to generating the system state space model. Based on the work done by Anders Friss1aless comprehensive approach has been chosen. Instead of using as high order transfer functionto model the internal controller and X3D aerodynamics a simple scaling factor K will be used.This approach essentially means that the angular velocities requested from the wireless link areassumed to be executed in scaled value by the internal controller of the X3D. The assumptionassumes that the internal controller of the X3D is sufciently fast and precise. This claim issupported by the measurements illustrated in appendix I.A series of measurements related to the response of the X3D to various inputs has been per-formed. Based on the measurements illustrated in appendix I the scaling factor K illustratedin equation 4.1 has been found. The experiments and methods related to the derivation of thisscaling factor are described in appendix I.K =

KKK =

0.00115740.000926120.0016476 (4.1)Based on equation 4.1 the generated angular velocities relative to the reference input S form thewireless link can be calculated. The dynamic system equations assumes the structure illustratedin equation 4.3.B =S K(4.2)B = S 0.0011574 andB = S 0.00092612 andB = S 0.0016476 (4.3)Given are thus the dynamic system equations related to the angular velocity of the X3D.4.1.2 X3D thrust generationIn this section the thrust generated by the X3D is addressed. Prior to addressing the actual gener-ation of thrust, 4.4 is introduced whereECB is the direction cosine matrix.The equation relatesthe angular and translational velocity of the X3D in the body frame to angular and translationalvelocity in the earth frame.Ev = P = ECB()Bv andE == EHB()B (4.4)ThetotalforceactingontheX3Dcanbefoundasasummationofallexternalforces. Thesummation of the forces yields the equation illustrated in gure 4.2 whereBF1 - BF4 is the upwardforce generated by the X3D rotors respectively andBFg is the gravitational force. As it can be1Master Student 2010 AAU - Intelligent Autonomous Systems4. Modelling 25 of 1394.1 Model Structureseen from appendix C, the force generated by the X3D is dependent of the angular velocity ofthe rotors. The main objective of this project is to enable the X3D to y in hover mode. Whenthe X3D is is hover the angular velocity of the four rotors are ideally identical resulting in equalgenerated force. The force is always directed along the z-body axis and it is thus considered validto replace the four individual force contributions BF1 - BF4 with the summarized force BFrotor.BFtotal = BF1 + BF2 + BF3 + BF4 + BFg(4.5)= BFrotor + BFg(4.6)The magnitude of the gravitational force Fg in the body frame can be calculated using Newtons4123F1F2F3F4FgFTotalFigure 4.2: Illustration of the considered forces which affect the X3Dsecond law. The equation is illustrated in equation 4.7 where g is the gravitational accelerationwritten in vector form illustrated in equation 4.8 and m is the mass of the X3D.EFg = m g andBFg = BCE EFgBFg = BCE m g (4.7)g =

0 0 g

T(4.8)Using Newtons second law it is possible to derive an equation 4.9 for the translational accelera-tion E v as seen from the earth frame, [6, page 117 ].F = m a EFtotal = mE v EFtotalm= E v (4.9)The translational velocityEv of the X3D as seen from the earth frame can be written as is inequation 4.10 where Bv is the translational velocity of the X3D in the body frame and ECB() isthe transformation matrix from the body frame to the earth frame. The translational accelerationof the X3D E v as seen from the earth frame can be written as is in equation 4.11.Ev = ECB()Bv (4.10)E v = ECB()B v+E CB()Bv (4.11)26 of 139 4. Modelling4.1 Model StructureThe derivative of the transformation matrix ECB can be expressed as the cross product illustratedin equation 4.12, [3, page 24 theorem 4.4 ].E CB()Bv =BCE()(BBv) E v = ECB()B vBCE()(BBv) (4.12)Equation 4.9 and C.23 are combined yielding the equation illustrated in 4.13. The simpliedequation is illustrated in 4.17.EFtotalm= ECB()B vBCE()(BBv) (4.13)ECB()B v =EFtotalm+BCE()(BBv) (4.14)BCE()

ECB()B v

=BCE()

EFtotalm+BCE()(BBv)

(4.15)B v =EFtotalm+

BCE()BCE()

. .. .I

BBv

(4.16)B v =EFtotalm+BBv (4.17)BasedonthemeasurementsinappendixHithasbeenchosentomodelthegeneratedforceBFrotor as a function of the input Sc that is the scalar collective reference Sc from the controllerlink. The experiments related to the derivation of the linear function are described in appendixH. The resulting linear function related to the thrust generated by the X3D rotors is illustrated inequation 4.18.BFrotor(Sc) =

003.2174 1010 S3c1.9531 106 S2c +5.4632 104 Sc0.6698 (4.18)Based on equation 4.18 the force generated by the rotorBFrotorrelative to the reference inputfrom the wireless link can be calculated.However straight forward the use of equation 4.18 also implies that whenever a constant is givenas input toBFrotor(Sc) the X3D will experience a constant acceleration. This is not the case asthe platform during constant input will not accelerate indenitely but settle at a constant velocityafter initial acceleration. Based on the work of [7] it has been chosen to add an additional termto the model, see 4.19. This term emulates the effects of induced inow through the X3D rotorsduring translatory movement along the z-body axis.BFinf low = If Bvz(4.19)The induced inow term will prevent the X3D form accelerating indifferently and thus settlingat a velocity where theBFin f low is equalBFrotor+ BFg. Notice that Ifis a negative scalar. Theintroduction of equation 4.18 and the induced inow term 4.19 results in the dynamic system4. Modelling 27 of 1394.2 Non-linear Simulation modelequation illustrated in equation 4.21.BFtotal= BFrotor + BFg + BFinf low(4.20)= BFrotor(Sc) + BCE m g + If Bvz(4.21)Induced inow can be viewed as the total airow perpendicular to the rotors minus the airowgenerated by the rotation of the rotors. When the X3D is in hover mode the induced inow willbe zero. When the X3D moves along the z-body axis the term will grow relative to the translatoryvelocity. The value of the induced inow coefcient Ifcan be estimated by applying steps to theX3D while ying and sampling the response. This approach has not been used as the manoeuvresrequired to perform the experiment are quite demanding in relation to the skill of the operator.Instead, the induced inow coefcient Iffound by [7] is used. This approach is considered validas [7] used an identical X3D Quad-Rotor. While the parameters related to the internal controllermay have been changed the overall design of the X3D has not changed.4.2 Non-linear Simulation modelIn this section the non-linear system equations are implemented in Matlab Simulink in order tocreate the non-linear simulation model. The non-linear simulation model is used in the processof verifying the devised system model.In order to visualize the process the non-linear dynamicsystem equations are recapitulated in equation 4.23 to 4.24.EFtotal = BFrotor(Sc) +BCE m g+ If BvzandB v =BFtotalm+BBv (4.22)B = S Kand K =

KKK (4.23)Ev = ECB()Bv andE = EHB()B (4.24)Based on the non-linear dynamic system equations it is possible to derive a non-linear systemsimulation model. The process of designing the model is simply to implement equation 4.23to 4.24 in Matlab Simulink. The resulting non-linear system simulation model is illustrated ingure 4.3. The transformation matrices used in the model are addressed in detail in appendix Jon page 133. It is noticed that the simulation model operates with a initial state of zero on allstates. This means that when the simulation is initiated the simulated X3D is dropped in freespace at way point [0 0 0] with attitude [0 0 0] and with no angular or translatory velocity. If noinput is given to the model it will fall downwards due to the gravitational force.4.2.1 Non-linear Simulation model VericationIt is highly desirable to verify that the designed Non-linear Simulation model is representative forthe actual response of the X3D system. This will both improve the groups understanding of themodel as well as validate the use of the model in relation to designing the system control scheme.28 of 139 4. Modelling4.2 Non-linear Simulation modelInput ~> AnguIar veIocity Force generated by X3D RotorsGravitationaI force maped to body frameThrust VeIocity (Body)Induced InfIowAcceIeration (Body)AnguIar veIocity (Body)VeIocity (Erath)EuIer AngIes2Position (Erath)1MatrixMuItipIyMatrixMuItipIyMatrixMuItipIyUYUYRange LimitsProduct1s1s1sKphiKpsiKtheta[0 0 If]'1/massForce functionP(u)O(P) = 3EuIer rates TransformB ~> EEuIer H_B2ECosine matrixE ~> BEuIer C_E2BCosine matrixB ~> EEuIer C_B2E[0 0 mass*g]Input1RoI, Pitch, YawEuIer rates (Erath)Figure 4.3: Illustration of the Simulink implementation of the Non-linear modelThe approach of verication chosen in this project requires that a test ight of the X3D is per-formed. The objective of the test ight is to sample the response of the X3D when it is ownin normal operation mode, that is in near hovering mode. During the session related to the de-termination of the Thrust Force H a series of test ights were performed. The guidelines relatedto these test ights and the management of the sampled response are addressed in appendix Eon page 98. A total of ve test ight where performed and the measurement best t for use wasfound to be ight number three.The Non-linear Simulation models response to the input related to the test ight is illustratedin gure 4.4 and 4.5.Fromgure4.4itcanbeseenthattheresponseestimatedbythemodelissomewhatrepre-sentative for the measured response of the X3D system. It is noticed that the roll and pitch anglesare drifting initially being quite representative of the measured response of the X3D system butslowly becoming less and less correct. The estimated angles are drifting away form hoveringstate and the X3D model is thus directing the rotor thrust at an angle relative to straight down asin near hover. This means that there is less downwards force to counter the gravitational forceand that the X3D model is ying sideways instead of hovering. Notice that the estimated rolland pitch angles are close to 40 and 30 respectively meaning that the platform is signicantlytilted. Having these results in mind as well as the design of the model with no concept of ground,the estimated positions illustrated in gure 4.5 are less surprising.From gure 4.5 it can be seen that when the simulation is initiated, the X3D maintains the initialposition. At approximately 150 [ms] the X3D model begins to fall downwards. Initially this doesnot make any sense but looking at the estimated attitude it becomes obvious what is going on.4. Modelling 29 of 1394.3 Model Linearisation0 200 400 600 800 1000 1200 1400 1600 1800 2000~2002040Time [ms]AngIe [Degree]0 200 400 600 800 1000 1200 1400 1600 1800 2000~30~20~10010Time [ms]AngIe [Degree]0 200 400 600 800 1000 1200 1400 1600 1800 2000~30~20~10010Time [ms]AngIe [Degree]Measured RoIIEstimaed RoIIMeasured PitchEstimaed PitchMeasured YawEstimaed YawFigure 4.4: Illustration of the estimated attitude of the X3D0 50 100 150 200 250 300 350 400 450 500~50510Time [ms]Position [m]Estimaed xEstimaed yEstimaed zFigure 4.5: Illustration of the estimated position of the X3D PThe X3D model is signicantly tilted and most of the thrust is thus not directed downwards. In-stead the thrust propels the X3D model sideways in the earth frame while the elevation decreases.This manoeuvre is equivalent of heavily actuating the roll and pitch simultaneously.4.3 Model LinearisationIn this chapter the non-linear dynamic system equations derived in chapter 4 are linearised. Theequations are used in the process of designing the system state space model that is to be usedin the process of designing the system control. As the designed control is linear the non-lineardynamic system equations must be linearised around an specic operating point.30 of 139 4. Modelling4.3 Model Linearisation4.3.1 Taylor series approximationThe method used in the process of linearising the non-linear dynamic system equations is Taylorseries approximation. Taylor series approximation consist in representing a given function with aseries of terms calculated from the functions derivatives at a single point known as the operatingpoint2To linearise the system equations the rst order Taylor approximation is used. This meansthat each system variable is substituted with an steady-state value and a small signal gain [see13, page 60 ]. The Taylor approximation with one variable can be decried as in equation 4.25,where f(x) is the the function to be approximated at value x and x is the operating point value.The rst order Taylor approximation assumes the structure illustrated in equation 4.26. Noticethat the Lagrange notation is used for the derivative of the function f(x), the zeroth derivative off(x) is dened to be f(x) itself and (x x)0= 0! = 0.f (x) f ( x) +f

( x)1!(x x)1+f

( x)2!(x x)2+f

( x)3!(x x)3+. . . +fn( x)n!(x x)n(4.25)f (x) f ( x) + f

( x)(x x)

n=1(4.26)The linearisation of the non-linear dynamic system equations are performed around the systemnormal behaviour operating point. In the case of the X3D the normal behaviour is when thexy-body plane is parallel to the xy-earth plane equivalent to when the X3D is in perfect hoverand the yaw angle is zero. The system normal behaviour operating point can thus be specied bysetting the system parameters to their constant value associate with the specic operating point.The system operating point is when the platform is hovering that is when the attitude [ ]T[ 0 0 0 ]T. This means that the attitude is maintained close to or equal to [ 0 0 0 ]T. The linearisationis thus performed about the angles i = 0+i | i , , where i indicates a small angle deviation.The effect of specifying an operating point on the system parameters are illustrated in equation4.27.B 0 andBv 0 (4.27)The rst order Taylor series approximation in the operating point entails that the trigonometricequations can be approximated as illustrated in equation 4.28 to 4.29 in the evaluation point 0.sin(i) sin(0) +cos(0) i i (4.28)cos(i) cos(0) +(sin(0)) i 1tan(i) tan(0) +1(cos(0))2 i i (4.29)4.3.2 Rigid Body Dynamics & KinematicsIn this section the non linear dynamic system equations related to the rigid body dynamics andkinematics are linearised. The non linear dynamic system equations related to the translational2If the operating point is zero the Taylor series is called Maclaurin series.4. Modelling 31 of 1394.3 Model Linearisationand angular velocity of the X3D are illustrated in equation 4.30.Ev = ECB()Bv andE == EHB()B (4.30)The linearisation of the equations in 4.30 are performed using rst order Taylor series approx-imation on the direction cosine matrixECB() and the transformation matrixEHB() respec-tively. Using the approximations in equation 4.29 the direction cosine matrixECB() assumesthe structure illustrated in equation 4.32.ECB() =

cc ssscs csc+sscs sss+cc cssscs sc cc (4.31)

1 + +1 1

1 1 1 (4.32)If the linearised cosine matrix 4.32 is substituted into equation 4.30 the equation for the transla-tional velocity undertakes the structure illustrated in equation 4.35.Ev = ECB()Bv (4.33)

1 1 1Bv (4.34)=

BvxBvy +BvzBvx +BvyBvzBvx +Bvy +Bvx

BvxBvyBvz = Bv (4.35)The same approach in used in relation to the transformation matrix EHB().EHB() =

1 ts tc0 c s0 s/c c/c

1 0 1 0 1

1 0 0 1 0 1 (4.36)If the linearised transformation matrix 4.36 is substituted into equation 4.30 the equation for theangular velocity undertakes the structure illustrated in equation 4.38.E =EHB()B (4.37)

1 0 0 1 0 1B =

B +BBBB +B

BBB = B (4.38)4.3.3 Thrust generationIn this section the equation from section 4.1 related to the forces affecting X3D will be addressedand linearised. The equation is linearised using the same method as used in the previous section.32 of 139 4. Modelling4.4 State Space modelThe non-linear dynamic system equation related to the force is illustrated in equation 4.39.BFtotal=BFrotor(Sc). .. .Rotor force+BCE g m....Gravitational force+ If Bvz. .. .Induced Inow(4.39)Equation 4.39 consists of a summation of forces and it is thus possible to linearise the compo-nents individually. The gravitational forceBFg can be expressed as in equation 4.40. Using thetranspose of the linearised direct cosine matrix 4.32 equation for the gravitational force under-takes the structure illustrated in equation 4.40.BFg = BCE

00g m=

1 1 1

00g m=

1 g m (4.40)Equation 4.41 describes the translational acceleration of the X3D in the body frameB v. Usingthe equations 4.27 it is possible to rewrite the equation as in equation 4.42.B v =BFtotalm+BBv (4.41)BFtotalm=BFrotor +BFg +BFin f lowm(4.42)WhenB v is written in vector form, the componentsBFrotor andBFinf low are only present in thez-component of the vector. The collective linearised force equation in vector form undertakes thestructure illustrated in equation 4.43.B v

B vxB vyB vz=

g gFrotor(Sc)+mgm+ Ifm Bvz (4.43)The term Frotor(Sc) +m g is replaced with the collective force Fz, which describes the collectiveforce applied to the X3D along the z-body axis. The collective linearised system equations canthus be written ans in equation 4.44. x = Bvx y = Bvy z = BvzandB vx = gB vy = gB vz =Fzm + Ifm Bvzand = S K = S K = S K(4.44)Given are thus all the equations needed to derive the system state space model. The process ofthis will be the main focus of the following section.4.4 State Space modelIn this section the system equations from section 4.3 are converted into state space form. Thestate space model is required in order to utilise the desired LQ control strategy. After derivingthe continuous state space model from the linearised system equations the state space model isdiscretized using MATLAB.4. Modelling 33 of 1394.4 State Space model4.4.1 System StatesThe states and inputs illustrated in equation 4.45 and 4.46 respectively have been chosen as basisfor the state space model. The vectors in equations 4.45 and 4.46 will be referred to as the statevector xs and input vector us.xs = [ x y z BvxBvyBvz]T(4.45)us = [ SSFzS]T(4.46)Though most of the states have been addressed in previous chapters it is assumed appropriate torecapitulate their meaning. x, y and z are the position of the X3Ds center of mass Mc. , and are the 3-2-1 Euler angles.Bvx, Bvy and Bvz are the translational velocities of the X3D in the x,y and z direction in the body frame respectively. The vector in equation 4.45 will throughout thischapter be refereed to as the state vector.4.4.2 System EquationsThe linearised dynamic system equations on which the system state space model are designedare listed in equations 4.47. x = Bvx y = Bvy z = BvzandB vx = gB vy = gB vz =Fzm + Ifm Bvzand = S K = S K = S K(4.47)4.4.3 Matrix DerivationThe state space model describes the system using differential equations. It consists of two equa-tions, one related to the states of system and one related to the output parameters of the system.Notice that xsdoes not refer to the translatory velocity of the platform along the x-axis but avector containing the derivative of the states and that A, B, C and D are matrices.The continues state space model is typically written in the form illustrated in equation 4.48 and4.49. The model operates using a series of states contained in a state vector xs(t) that are relatedto the system input and output through the dynamic system equations.In order to abstract fromthe number of states, inputs and outputs, the system variables are expressed as vectors and theequations are expressed as matrices. In equation 4.48 the equation related to the system output isillustrated. The output equation contain the system matrices A and the input matrix B.State derivative....xs(t) = A....System matrixSystem states....xs(t) + B....Input matrixSystem input....u(t) (4.48)34 of 139 4. Modelling4.4 State Space modelInequation4.49theequationrelatedtothesysteminputisillustrated. Theoutputequationcontain the output matrices C and the direct transmission matrix D.y(t)....System output=Output matrix....C xs(t)....System states+Direct transmission term....D u(t)....System input(4.49)Using the linearised dynamic system equations it is possible to derive the continues state spacemodelfortheX3Dsystem. Byarrangingtheequationsthefollowing A, Band Cmatricesare given. Notice that the direct transmission matrix D is the zero matrix as there is no directtransmission from the system input to the system output.Axs(t) =

0 0 0 0 0 0 1 0 00 0 0 0 0 0 0 1 00 0 0 0 0 0 0 0 10 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 0 0 0 0 00 0 0 0 g 0 0 0 00 0 0 g 0 0 0 0 00 0 0 0 0 0 0 0Ifm

xyzBvxBvyBvz(4.50)Bu(t) =

0 0 0 0 0 K0 0 00 0 0 0 0 0 0 01m0 0 0 0 K0 0 0 00 0 0 K0 0 0 0 0T

SSFzS(4.51)Cxs(t)=

1 0 0 0 0 0 0 0 00 1 0 0 0 0 0 0 00 0 1 0 0 0 0 0 00 0 0 1 0 0 0 0 00 0 0 0 1 0 0 0 00 0 0 0 0 1 0 0 0

xyzBvxBvyBvz(4.52)Having derived the continues state space model using the linearised dynamic system equationsthe model is converted from continues time to discrete time. The process of this discretization isaddressed in section 4.4.44.4.4 Discrete State Space modelIn this section the continues state space model is discretized. In order for the system control tobe implementable it must be present in discrete time.4. Modelling 35 of 1394.4 State Space modelIn the previous section the continues system was described in the form illustrated in equation4.53 and 4.54. The equations are the input and output equations respectively. x= Axs(t) + Bu(t) (4.53)y= Cxs(t) +Dx(t) (4.54)A solution to this system can be described as illustrated in equation 4.55 as presented by [see 6,page 631 ]xs(t) = eA(tt0)xs(t0) +t

t0eA(t)Bu()d (4.55)Using equation 4.55 is it possible to obtain the discrete state space representation of the system.Changing the notation (t = kT + T and t0 = KT) it is possible to rewrite equation 4.55 into equation4.56.xs(kT +T) = eATxs(kT) +kT+T

kTeA(kT+T)Bu()d (4.56)Equation 4.56 is not dependent of the type of hold used as u is specied using its continues timehistory u(). The discrete model of a continues system can be found using Zero Order Hold(ZOH) where the input u() is constant throughout the sample interval.u() =u(kT), kT < kT +k (4.57)To ease the derivation the following notation is introduced. = kT +T (4.58)The use of enable equation 4.56 to be written i form illustrated in equation 4.59.xs(kT +T) = eATxs(kT) +

T

0eAd

Bu(kT) (4.59)Introducing the notation in equation 4.60 the discrete state space model can be extracted fromequation 4.59.= eATand =

T

0eAd

B and H=C (4.60)Thediscretestatespacemodelundertakesthestructureillustratedinequation4.61and4.62where the discrete version of the D matrix has been omitted.xs(k +1) =xs(k) +u(k) (4.61)y(k) = Hxs(k) (4.62)36 of 139 4. Modelling4.5 Chapter SummaryThe computation of , and Hare performed using the dedicated Matlab function c2d(sys,Ts,zoh).This function takes the continues system state space model (sys), the sampling time (Ts) and thechosen method of sampling (zoh). The found discrete system state space matrices are illustratedin equation 4.63 to 4.65.=

1.0000 0 0 0 0.0005 0 0.0100 0 00 1.0000 0 0.0005 0 0 0 0.0100 00 0 1.0000 0 0 0 0 0 0.01000 0 0 1.0000 0 0 0 0 00 0 0 0 1.0000 0 0 0 00 0 0 0 0 1.0000 0 0 00 0 0 0 0.0981 0 1.0000 0 00 0 0 0.0981 0 0 0 1.0000 00 0 0 0 0 0 0 0 0.9914(4.63) =

0 1.5136 1090 01.8917 1090 0 00 0 1.2255 10401.1574 1050 0 00 9.2612 1060 00 0 0 1.6476 1050 4.5410 1070 05.6751 1070 0 00 0 2.4476 1020(4.64)H=

1 0 0 0 0 0 0 0 00 1 0 0 0 0 0 0 00 0 1 0 0 0 0 0 00 0 0 1 0 0 0 0 00 0 0 0 1 0 0 0 00 0 0 0 0 1 0 0 0(4.65)4.5 Chapter SummaryIn this chapter the modelling of the X3D has been addressed. The system model has been formu-lated using both classical modelling techniques as well as system identication. based o the de-rived non-linear dynamic system equations a non-linear simulation model has been implementedin Simulink. The a non-linear simulation model has proven to be able to model the response ofthe X3D sufciently. The non-linear dynamic system equations have been linearised around thesystem normal operating point, that is close to hover mode. Based on the linearised dynamicsystem equations the system state space model has been derived.The system control is to be based on the system state space model. The process of designingthe system control is the main focus of the following chapter.4. Modelling 37 of 139Chapter 5Controller DesignAspresentedintheModellingchapter, thequad-rotorsystemisnon-linearandtime-variantin terms of battery power. However, for the project problem of stable hovering and way-pointtracking, the working model is LTI: linearized around the hovering point and considered time-invariant, as the ight time is limited making it possible to ignore the losses in battery power overtime. The main objectives of this chapter are to present design considerations and implementa-tion details of the linear optimal controller achieved by minimizing a quadratic performancefunction. Specic elements include tracking of step references, and state estimation using pro-cessing on the Vicon position and attitude data.All the methods and considerations are discussed for discrete-time.5.1 Linear State FeedbackThe main principle used to control the multiple input-output quad-rotor system is state feedback.Feedback is a powerful concept, as it can make a system resilient to both external disturbancesand variations in the behavior of the internal parts of the loop, and can create a linear input-outputbehavior from non-linear components.For dynamics described with state models,at each time step,the state vector contains all theinformation that is needed to compute the future behavior, allowing the controller to be memo-ryless, and in the general case a function of the state vector. In state feedback, it is assumed thatall states at each time k are measurable or available from an estimation.u(k) =F (x(k), k)1(5.1)38 of 1395.1 Linear State FeedbackA controller that is also linear has the form below, where at each time step K(k) is a matrix ofreal numbers, making the command a linear combination of the states.u(k) =K(k) x(k); K Rmxn(5.2)For time-invariant models, often the designed control is also time-invariant: K(k) = K = ct. Thecontroller is just a static matrix gain. This is the case of the Linear-Quadratic controller that isgoing to be designed in the next sections.For all practical purposes, the linear feedback also takes into account the reference, and thisaspect is further detailed in section 5.3.4.u(k +1) =K x(k) +Kr r(k); K Rmxn, Kr Rmxr(5.3)By creating the feedback loop, the dynamics of the system, governed by matrix A, change fromthe open loop (Ad) to (AdBd K). An adequate choice for the control matrix K can assign adesired behavior to the closed loop. Figure 5.1: OpenLoopOpen loop dynamics:x(k +1) = x(k) + u(k) (5.4)y(k) = Hx(k) (5.5)Closed loop dynamics:x(k +1) = ( K) x(k) + Kr r(k) (5.6)y(k) = Hx(k) (5.7)An important assumption is that all the states that dene the quad-rotor dynamics are availablefor online use in the state feedback controller calculations. This is not the case with real systems,not all of the states are measured directly with sensors. For the missing states, estimation is used:ltered measurements are combined with inferences based on the principle of mechanics, or a1In real discrete-time systems, there is often a delay of one or more time steps between time of the measured stateand the moment of command. This delay has not been taken into account.5. Controller Design 39 of 1395.2 Linear Quadratic methods u(k) x(k1) x(k) y(k) 1-r(k)Figure 5.2: State feedback closed loop with feedbackstate-space observer.5.1.1 Methods for State FeedbackThe problem of determining an appropriate control command is the same as determining the ma-trix K.For single input systems, it is possible to directly relate the matrix K to the position of the closedloop eigen values. The method is called pole placement. The main problem is the choice ofclosed loop poles locations. Criteria of stability, static and dynamic response requirements, suchas limit offset, rise time, overshoot must be met at a balance with the energy spent on the controlsignals.As discussed in the Optimal Control lecture notes [12], for multiple inputs systems it is not easyto relate the elements of the control matrix K to the positions of the closed loop poles. There aremore parameters than constraints, and an under-determined system of equations must be solved.As such, there is no unique solution K for a set of poles, and choosing the optimum K-values isnot trivial. For a large class of multiple input problems, Linear-Quadratic methods are a betterapproach.Another class of solutions that work well for MIMO systems with an LTI approximate model areH methods, or robust control. These methods are not discussed in the present project.5.2 Linear Quadratic methods5.2.1 Optimal ProblemOptimal control deals with the problem of nding the control law u such that a certain optimumcondition is met. The optimum criterion is dened as a cost function Hof states and control40 of 139 5. Controller Design5.2 Linear Quadratic methodsvariables, which must be balanced in a meaningful way to the real system. This formulation isrelevant for a large class of problems, where the goals of control are closely related to quantita-tive expressions on the systems inputs, outputs, and other intermediary measured or estimatedparameters of operation.The general, non-linear, discrete, optimal control problem starts from the system model:x(k +1) =G(x(k), u(k), k) (5.8)A performance function or index, I , is dened as the sum of the performance and cost criterionH(k) over a time horizon N, starting from t0 = 0. There are two types of problems:nite timehorizon N, and innite horizon N =.I=N1k=0H(x(k), u(k), k) +M(x(N)) (5.9)I=k=0H(x(k), u(k), k) (5.10)The purpose is to optimize the performance function by manipulating the command u. The opti-mized performance index will be dependent of the particular system dynamics G and the initialstate x0. In the case of the nite horizon problem, an additional terminal state cost Mis used inthe performance function, to put a specic weight on the state error at the nal time step.5.2.2 Linear optimization with quadratic criterionThe general system G is now assumed to be linear and time invariant.x(k +1) = x(k) + u(k); Rnxn, Rnxm(5.11)A particular case is when the performance criterion Htakes a quadratic form2. This is the classof Linear Quadratic methods.The performance index has the quadratic sum form below:I=N1k=0[x(k)T Q1 x(k) +u(k)T Q2 u(k)] +x(N)T QN x(N) (5.12)I=k=0x(k)T Q1 x(k) +u(k)T Q2 u(k)3(5.13)2A quadratic form is a homogeneous second order polynomial in n variables: pQ(x1, .., xn) = qi j xi xj. Thecoefcients of the polynomial can be arranged into a nxn symmetric matrix, and the same quadratic form can bewritten in the matrix form: pQ(x) = xT Q x. Real quadratic forms can always be brought to a diagonal form by alinear transformation (Jacobi theorem).5. Controller Design 41 of 1395.2 Linear Quadratic methodsSymmetric matrices Q1 and Q24are parameters of the control problem. They can be time-variant,but this case is beyond the scope of the project and Q1 and Q2 are constant in the following. Thenexttworestrictionsareusedinthelinearoptimizationproblem. ThestateweightmatricesQ1 and QN are positive-semidenite5, and the actuation weight matrix Q2 is strictly positive de-nite6. The diagonal state weighting matrix can contain factors 0, but the command scaling factorsare always positive.The advantage is that the resulting controller is a linear function of the states and as such, theLinear Quadratic optimization yields a linear state feedback solution.u(k) =L(k) x(k);7(5.14)Because of the quadratic form, the optimization is minimization. The solution of the problem,J= min(I (u)) is dependent of the state dynamics, as dened by constant real matrices (,),and the initial state x0. The important result in practical problems is the state-feedback controllerL(k) and the command sequence u(k), while the actual value of the minimum performance in-dex J (x0) is less interesting.For nite horizon problems, the state feedback gain matrix L(k) is variable with time. In manypractical situations, such as long or undetermined operation-time, a constant gain matrix is pre-ferred. Receding horizon or the innite horizon optimizations are two strategies that produce atime-invariant controller.The receding horizon strategy is to use a small to medium optimization horizon N, relative tosystem dynamics. Matrix weights Q1 and Q2 are chosen to balance the performance and cost ofthe dynamic response, while the terminal weight QNputs pressure on the steady state error. Ateach time step the issued command is u(k) = L(0) x(k), under the assumption that horizonfor control is pushed forward after each time sample. In practical situations, if the horizon timeN is too small, the weights are not well chosen, and the terminal cost is too high, the resultingoptimizing matrix gain can drive the closed loop unstable.3The performance function is a real quadratic sum form, in variables x and u4Often referred also as Q and R5The eigen values of the symmetric matrix Q, or the elements of the equivalent diagonal form, i >= 06The condition on matrix Q2 is met in the project implementation, but is not necessary for the general linear controlproblem solution. Q2 can be non-strictly positive denite, and in that case another relaxed condition must be met.7Notation u is used for the command sequence that optimizes the performance function.The LQ state feedbackgain is from now on noted as L.42 of 139 5. Controller Design5.2 Linear Quadratic methods5.2.3 Innite Horizon optimizationIn the project, the innite horizon controller solution has been used. Innite time horizon meansthat the performance function is an innite sum of positive terms, due to the quadratic denition.To minimize the sum, it must converge. The optimum controller sequence u(k) will drive theperformance and cost criterion to zero so as to converge the performance function. Since thematrices Q1 and Q2 are constant and positive denite, the states x(k) and commands u(k) mustalso tend to 0.J (x0) = minu(I (u)) = minuk=0H(k) = ct = (5.15)limk>H(k) =limk>(xT(k) Q1 x(k) +uT(k) Q2 u(k)) = 0, withQ1 = ct, Q2 = ct = (5.16)limk>x(k) = 0; limk>u(k) = 0 (5.17)The system model must be correctly specied to map to the problem as presented. The hoveringpoint must fulll the conditions 5.17. As it will be seen in section 5.3.4, where reference is in-troduced in the system, only 6 of the states will be weighted in the nal problem: position andattitude. For the position signals, the quantity being weighted in the performance criterion willbe the deviation from the reference: xrx, yry, z rz, and in this form will fulll the condition5.17. Roll, pitch, and yaw states are naturally 0 in the hovering point. The command must alsobe zero. The roll, pitch, and yaw commands - which represent angular velocities, are 0 or veryclose when hover has been reached. However,the thrust has a constant non-zero value. Theway this problem is approached is that in the linear model, thrust is not system input, but ratherFz, the resultant vertical force component. The vertical force is 0 in hover point. The controllercomputed based on the linear model will output the thrust command in the Fz form, and this ispassed through a non-linear transformation, identied in the thrust measurement modelling, tothe real command range, for the non-linear simulation and the online schema.Thegeneralinnitehorizonproblem, withafewadditionalconditions8, guarantiesauniquecommand sequence solution to the optimization. Additionally, the optimizing command u(k)is described by a constant state-feedback gain L(k) = L, and, can be proved, is stable in closedloop. As such, a single matrix can be computed ofine and used for an innite time horizon ofcontrol, to bring the system from an arbitrary initial state to the minimum cost state space pointof the linear system, 0, against point disturbances.The time-invariant feedback controller obtained as a result of an innite horizon optimizationproblem is the point of interest in the remainder of the chapter.8While not necessary, the following conditions are sufcient: (, ) controllable and Q2 > 0, and (, Q2) observ-able. More relaxed criteria exits.5. Controller Design 43 of 1395.3 LQ algorithm5.3 LQ algorithmThe approach used to nd the state feedback gain is a nite-time horizon algorithm that is basedon the optimality principle and backwards induction as described in Dynamic Programming sec-tion, 5.3.1. The iterations are run with a variable number of steps, until an approximate conver-gence of the solution is reached.This section explains the different steps used in the nal algorithm, and contains a pseudocodelisting that has been used in the project. The method is based on the course and notes of Opti-mal Control [12] from this semester, and the description of the Standard Regulator problem fordiscrete systems in [2]. Further, the system changes required to introduce the reference pointand guidelines for choosing the weight matrices Q1 and Q2, parameters of the optimization, areaddressed.5.3.1 Dynamic ProgrammingThe main idea of this section is the fact that when the controller is optimal in an interval [0;N],it will be optimal in an interval [k;N] with 0 k N. The performance function was introducedas:IN0=Nk=0H(x(k), u(k)) (5.18)But the performance only depends on the initial state vector and on the command signal so it canbe written that:IN0=IN0(x(0), u(0), u(1), ..., u(N)) (5.19)The other state vectors x(k) are determined by iteration thanks to x(k-1) and u(k-1). Anothernotation for the minimum of the performance function will also be introduced:IN0(x(0)) = minu(0),...u(N)IN0(x(0), u(0), u(1), ..., u(N)) (5.20)According to the dynamic programming strategy stated in the beginning of the section, the per-formance function in an interval [k,N] is stated:INk=Nl=kH(x(l), u(l)) (5.21)=INk(x(k), u(k), u(k +1), ..., u(N)) (5.22)44 of 139 5. Controller Design5.3 LQ algorithmThe next step is to nd the minimum of the pe