{"@context":{"@language":"en","Affiliation":"http:\/\/vivoweb.org\/ontology\/core#departmentOrSchool","AggregatedSourceRepository":"http:\/\/www.europeana.eu\/schemas\/edm\/dataProvider","Campus":"https:\/\/open.library.ubc.ca\/terms#degreeCampus","Creator":"http:\/\/purl.org\/dc\/terms\/creator","DateAvailable":"http:\/\/purl.org\/dc\/terms\/issued","DateIssued":"http:\/\/purl.org\/dc\/terms\/issued","Degree":"http:\/\/vivoweb.org\/ontology\/core#relatedDegree","DegreeGrantor":"https:\/\/open.library.ubc.ca\/terms#degreeGrantor","Description":"http:\/\/purl.org\/dc\/terms\/description","DigitalResourceOriginalRecord":"http:\/\/www.europeana.eu\/schemas\/edm\/aggregatedCHO","Extent":"http:\/\/purl.org\/dc\/terms\/extent","FileFormat":"http:\/\/purl.org\/dc\/elements\/1.1\/format","FullText":"http:\/\/www.w3.org\/2009\/08\/skos-reference\/skos.html#note","Genre":"http:\/\/www.europeana.eu\/schemas\/edm\/hasType","GraduationDate":"http:\/\/vivoweb.org\/ontology\/core#dateIssued","IsShownAt":"http:\/\/www.europeana.eu\/schemas\/edm\/isShownAt","Language":"http:\/\/purl.org\/dc\/terms\/language","Program":"https:\/\/open.library.ubc.ca\/terms#degreeDiscipline","Provider":"http:\/\/www.europeana.eu\/schemas\/edm\/provider","Publisher":"http:\/\/purl.org\/dc\/terms\/publisher","Rights":"http:\/\/purl.org\/dc\/terms\/rights","ScholarlyLevel":"https:\/\/open.library.ubc.ca\/terms#scholarLevel","Title":"http:\/\/purl.org\/dc\/terms\/title","Type":"http:\/\/purl.org\/dc\/terms\/type","URI":"https:\/\/open.library.ubc.ca\/terms#identifierURI","SortDate":"http:\/\/purl.org\/dc\/terms\/date"},"Affiliation":[{"@value":"Applied Science, Faculty of","@language":"en"},{"@value":"Electrical and Computer Engineering, Department of","@language":"en"}],"AggregatedSourceRepository":[{"@value":"DSpace","@language":"en"}],"Campus":[{"@value":"UBCV","@language":"en"}],"Creator":[{"@value":"Chen, Ming","@language":"en"}],"DateAvailable":[{"@value":"2009-10-30T23:15:31Z","@language":"en"}],"DateIssued":[{"@value":"2003","@language":"en"}],"Degree":[{"@value":"Master of Applied Science - MASc","@language":"en"}],"DegreeGrantor":[{"@value":"University of British Columbia","@language":"en"}],"Description":[{"@value":"For several decades, Unmanned Air Vehicles (UAVs) have generated considerable interest in the control and commercial community due to their advantages over manned systems. Cutting edge techniques in senors, communications and robust control can now make affordable commercial missions involving Unmanned Air Vehicles (UAVs) a reality. The aim of this work was to control an autonomous UAV using H[sub \u221e] loop shaping and MPC control laws and further demonstrate formation control with three such identical UAVs. The UAV used in our project is the four-rotor Dragonflyer III helicopter. To achieve the project objectives, a nonlinear model of the Dragonflyer III and further a Quasi-LPV model were developed. An H[sub \u221e] loop shaping control law was designed for stabilization and speed loops. For the trajectory control, two controllers, an H[sub \u221e] and an MPC, were designed and compared. This project then was extended to control 3 quad-rotor UAVs in equilateral triangle flying formation, objective for which, there layers, Path Planner, Trajectory Generator and Formation Controller, were introduced and implemented in Matlab and Simulink., ready for placing the hardware in the loop. The performance of the proposed methods was evaluated for several circumstances with satisfactory results. To validate the low level H[sub \u221e] control laws and demonstrate flying, an experimental system including a flying mill, a personal computer and DSP dSPACE board, a programmed microprocessor and a radio transmitter was built for testing. Comparing to previous UAV works, a quad-rotor helicopter is first chosen as the UAV dynamics in our project. The 2 DOF H[sub \u221e] and combined MPC\/H[sub \u221e] flight controller are firstly achieved and animated to control such high nonlinear system. A new implementation of formation flying control is proposed and simulated in MATLAB and SIMULINK. A self designed experimental setup is also built for identification and control testing purpose.","@language":"en"}],"DigitalResourceOriginalRecord":[{"@value":"https:\/\/circle.library.ubc.ca\/rest\/handle\/2429\/14459?expand=metadata","@language":"en"}],"Extent":[{"@value":"5609554 bytes","@language":"en"}],"FileFormat":[{"@value":"application\/pdf","@language":"en"}],"FullText":[{"@value":"Formation and Flight Control of Affordable Quad-rotor Unmanned Air Vehicles by Ming Chen B.A.Sc, Shanghai Jiaotong University, 1998 A THESIS SUBMITTED IN PARTIAL FULFILLMENT OF T H E REQUIREMENTS FOR T H E DEGREE OF Masters of Applied Science in T H E FACULTY OF GRADUATE STUDIES (Department of Electrical and Computer Engineering) We accept this thesis as conforming to the required standard The University of British Columbia July 2003 \u00a9 Ming Chen, 2003 In presenting this thesis in partial fulfilment of the requirements for an advanced degree at the University of British Columbia, I agree that the Library shall make it freely available for reference and study. I further agree that permission for extensive copying of this thesis for scholarly purposes may be granted by the head of my department or by his or her representatives. It is understood that copying or publication of this thesis for financial gain shall not be allowed without my written permission. Department of The University of British Columbia Vancouver, Canada DE-6 (2\/88) Abstract For several decades, Unmanned Air Vehicles (UAVs) have generated considerable interest in the control and commercial community due to their advantages over manned systems. Cutting edge techniques in senors, communications and robust control can now make affordable commercial missions involving Unmanned Air Vehicles (UAVs) a reality. The aim of this work was to control an autonomous UAV using HQQ loop shaping and MPC control laws and further demonstrate formation control with three such identical UAVs. The UAV used in our project is the four-rotor Dragonflyer III helicopter. To achieve the project objectives, a nonlinear model of the Dragonflyer III and further a Quasi-LPV model were developed. An loop shaping control law was designed for stabilization and speed loops. For the trajectory control, two controllers, an and an MPC, were designed and compared. This project then was extended to control 3 quad-rotor UAVs in equilateral triangle flying formation, objective for which, there layers, Path Planner, Trajectory Generator and Formation Controller, were introduced and implemented in Matlab and Simulink., ready for placing the hardware in the loop. The performance of the proposed methods was evaluated for several circumstances with satisfactory results. To validate the low level control laws and demonstrate flying, an experimental sys-tem including a flying mill, a personal computer and DSP dSPACE board, a programmed microprocessor and a radio transmitter was built for testing. ii I l l Comparing to previous UAV works, a quad-rotor helicopter is first chosen as the UAV dynamics in our project. The 2 DOF and combined MPC\/H^ flight controller are firstly achieved and animated to control such high nonlinear system. A new implementa-tion of formation flying control is proposed and simulated in MATLAB and SIMULINK. A self designed experimental setup is also built for identification and control testing purpose. Acknowledgements I am sincerely thankful to Dr . M i h a i Huzmezan, my supervisor at the University of Br i t i sh Columbia, for his willingness to guide and discuss my progress, outstanding supervision, comprehensive knowledge, encouragement and efforts of my career seeking. The opportu-nity of being D r . M i h a i Huzmezan's graduate student has already changed my life to an aspiring future. Many thanks also go to mechanical technicians, Leiff Kjolby and Dave Fletcher in Elec-trical and Computer Engineering Department, for their help of designing and building the flying mi l l for the Quad-rotor U A V s and their valuable suggestions for the U A V dynamics. I would also like to thank my good friends and colleagues wi th whom I shared my pleasant moments during the study at U B C and life inside and outside Canada. Ultimately, I truly appreciate my parents and my wife for their unwavering encourage-ment and unconditional support. iv Table of Contents Abstract ii Acknowledgements iv Table of Contents v 1 Introduction and Background 1 1.1 UAVs Review 1 1.2 Sensors for Unmanned Air Vehicles 5 1.2.1 Accelerometers 5 1.2.2 Gyroscopes 7 1.2.3 GPS 8 1.2.4 Attitude Estimation for UAVs based on GPS\/IMU 11 1.3 High Fidelity Linear Parameter Varying (LPV) Modelling 12 1.3.1 Gain Scheduling and LPV Models 12 1.3.2 Quasi-LPV Models 13 1.4 Jfoo loop shaping design for UAVs 15 1.5 MPC control for UAVs . . 18 2 The Experimental Rig and The Nonlinear U A V Model 21 2.1 The Quad-rotor Helicopter Experimental Setup 21 2.1.1 The Flying Mill . 2 1 2.1.2 Data Flow in the Experimental System 23 2.1.3 The Pulse Modulator 24 2.2 The Nonlinear Four-rotor Helicopter Model 31 2.2.1 Dynamics of Four-rotor Helicopter 32 2.2.2 The Quasi-LPV model of the Four-rotor Helicopter 35 2.2.3 The SIMULINK Model of the Four-rotor Helicopter 36 3 2 D O F Hoo and Combined M P C \/Hoo Flight Controller Design 38 3.1 The Roll And Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 39 3.1.1 The Design Procedure 39 3.1.2 Simulation Results 43 3.2 Longitudinal and Lateral Speed, Throttle and Yaw Angle Outer Loop Design 46 C O N T E N T S vi 3.2.1 Design Procedure 46 3.2.2 Simulation Results 50 3.3 The Trajectory Outer Loop Design 52 3.3.1 Design Procedure 52 3.3.2 Simulation Results 56 3.4 A Combined MPC\/tfoo UAV Flight Controller . 60 3.4.1 Motivation 60 3.4.2 Design Procedure And Results 62 4 Formation Control 67 4.1 The Formation Control Path Planner 69 4.1.1 The Dijkstra's Algorithm 69 4.1.2 The Path Planner Design 73 4.2 The Trajectory Generator ,. 75 4.3 Formation Controller 80 5 Conclusion and Future Work 86 A M-file of the Two D O F Loop Shaping Flight Controller Design 89 B M-file of the Combined M B P C \/ H infinity Controller Design 94 C M-files for Formation Control 96 C.l M-file of Dijkstra's algorithm for Path Planner 96 C.2 M-file of Trajectory Generator 97 C.3 M-file of Formation Controller \u2022 99 Bibliography 101 List of Figures 1.1 2 DOF Hoo Loop Shaping Controller 16 1.2 Prediction Strategy 19 2.1 Photo of The Flying Mill With Dragonfiyer III 22 2.2 The Data Flow In Experimental System 24 2.3 PWM Signal Definition 24 2.4 The Data Flow of The Pulse Modulator System 25 2.5 The Top Level Simulink Model of The Pulse Modulator 27 2.6 The Base Level of The Pulse Modulator Simulink Model 27 2.7 Flow Chart of Pulse Modulator in MASM 29 2.8 Flow Chart of Subprogram For Channel 5 in MASM 30 2.9 The Four-rotor Helicopter Model 31 2.10 Sub Level SIMULINK Model Diagram of Dragonfiyer III 37 3.1 3 loop controller architecture 38 3.2 Inner Loop of The Two DOF Flight Controller 39 3.3 Dragonfiyer III Reduced Model For Inner Loop Design 40 3.4 Open Loop Singular Values of The Unshaped Plant 41 3.5 Open Loop Singular Values of The Shaped Plant 41 3.6 Closed-loop Sensitivity Function of The Inner Loop 43 vii LIST OF F I G U R E S viii 3.7 Closed-loop Complementary Sensitivity Function of The Inner Loop . . . . 44 3.8 Step Responses With Controller 44 3.9 Output Step Disturbance Responses 45 3.10 Simulation of The Nonlinear Model 45 3.11 Simulation of The Nonlinear Model With Noise 46 3.12 The First Outer Loop of The Two DOF Flight Controller 47 3.13 Inner Loop And The Nonlinear Dragonfiyer III Model For The First Outer Loop Design 48 3.14 Open Loop Singular Values of The First Outer Loop Design 49 3.15 Shaped Open Loop Singular Values of The First Outer Loop Design . . . . 49 3.16 Closed-loop Sensitivity Function of The First Outer Loop 50 3.17 Closed-loop Complementary Sensitivity Function of The First Outer Loop . 51 3.18 Step Responses With Controller of The Velocity Outer Loop 52 3.19 Output Step Disturbance Responses of The Velocity Outer Loop 53 3.20 Simulation Of The Nonlinear Model of The Velocity Outer Loop 53 3.21 The Trajectory Loop of The Two DOF Flight Controller 54 3.22 Nonlinear Model For The Trajectory Loop Design 54 3.23 Open Loop Singular Values of The Trajectory Loop Design . . 55 3.24 Closed-loop Sensitivity Function of The Trajectory Loop 56 3.25 Closed-loop Complementary Sensitivity Function of The Trajectory Loop . 57 3.26 Step Responses With Controller of The Second Outer Loop 57 3.27 Output Step Disturbance Responses of The Second Outer Loop 58 3.28 Trajectory Simulation of The Nonlinear Model With 2 DOF Controller 58 3.29 Half Circle Scenario With 2 DOF Controller 59 3.30 Trajectory Scenario With 2 DOF Controller 59 3.31 A State Observer 62 LIST OF FIGURES ix 3.32 M P C In Longitudinal and Lateral Channel 63 3.33 Longitudinal and Lateral Channel Step Responses in The Combined M P C A n d Hoo Controller 65 3.34 Sensitivity Function Of The Combined MPC\/Foo Controller (The Uncon-strained Case) 65 3.35 Complementary Sensitivity Of The. Combined MPC\/i^oo Controller (The Unconstrained Case) 66 4.1 Formation Control Scheme For Mul t ip le U A V s 68 4.2 Dijkstra's Algor i thm Example 69 4.3 Voronoi Diagram for Threat Locations 73 4.4 Init ial Pa th from The Path Planner 74 4.5 Pa th Formed by The Trajectory Generator 75 4.6 Trajectory Generator and Quod Rotor U A V Architecture 76 4.7 Simulation Results ff Trajectory Generator and Closed Loop U A V 78 4.8 Trajectory Simulation Results 79 4.9 3 U A V Equilateral Triangle Formation in Inertial Frame 80 4.10 The Overall Formation Control Architecture 82 4.11 Animat ion of Equilateral Triangle Formation F ly ing 83 4.12 Formation F ly ing Results of Follower B U A V 84 4.13 Formation F ly ing Results of Follower C U A V 85 List o f Tables 2.1 The Nomenclature Used in Theoretical Formulation 32 3.1 Longitudinal and Lateral Channel Constraints 64 4.1 Table Solution For The Example 70 x Chapter 1 Introduction and Background 1.1 UAVs Review UAVs, or 'Unmanned Air Vehicles,' are defined as aircrafts without the onboard presence of pilots (12). UAVs have been already used to perform intelligence, surveillance, and reconnaissance missions. The technological promise of UAVs is to serve across a full range of missions. The first pilotless aircraft, intended for use as \"cruise missiles\", were first built during and shortly after World War I, and led to the development of radio-controlled pilotless target aircraft in Britain and the US in the 1930s. The first large-scale production UAV is the OQ-2A ordered by the US Army in 1941. The OQ-2A was a simple RC flying model aircraft, powered by a two-cylinder two-cycle engine, providing six horsepower and driving contra-rotating propellers. The ultimate result of this radio plane evolution, the \"MQM-36 Falconer\", was built for battlefield reconnaissance, with the first flight in 1955. The Falconer had an autopilot system with radio-control backup, and could carry cameras, as well as illumination flares for night reconnaissance. Although it only had an endurance of around a half hour, the Falconer went into service internationally with several different armies. A series of long-range reconnaissance UAVs derived from the Firebee, known generally 1 1.1 U A V s Review 2 as \"Lightning Bug\", were used by the US to spy on Vietnam, China, and North Korea in the 1960s and early 1970s. The Firebee was a small aircraft with a low radar \"cross section\", making it hard to detect. With lengthened wings, it was able to fly at high altitude, further increasing its elusiveness. The first modified Firebee for reconnaissance objective, \"Model 147A\", was equipped with a new guidance system, consisting of a timer-programmer, a gyrocompass and an altimeter. It could be programmed to fly in a specified direction at a definite altitude for a certain time, and then turn around and come back on the same track it went. Tests showed this UAV was almost invisible to ground radar, and interceptors that scrambled to find it ended up chasing each other. The Model 147B was designed for high-altitude reconnaissance, with its wingspan extended to 8.2 meters , raising the 147B's operational ceiling to 19 kilometers. In the early 1980s, Israel pioneered the operational use of battlefield UAVs and hence the international interest in battlefield UAVs picked up significantly. These battlefield UAVs fall into two broad categories designated as \"combat surveillance\" and \"tactical re-connaissance\" UAVs. The combat surveillance UAVs are to observe events on a battlefield in real time, orbiting over the battle area and relaying intelligence to a ground control station. The autopilot system directs the aircraft from sets of way-points that are pre-programmed before takeoff. Navigation is often verified by a GPS-INS navigation system. However, combat surveillance UAVs usually use the autopilot to get to the operating area and then are operated by radio control to find targets of opportunity. The UAV sensors are generally housed in a turret underneath the aircraft, and almost always contain day-night imagers and a laser designator to allow the UAV to mark targets for smart weapons. The tactical reconnaissance UAVs are usually larger, jet powered, with a longer range and a higher speed. Like the combat surveillance UAVs, they have an autopilot with radio con-trol backup, but they rely more on the autopilot than on the radio control. The tactical reconnaissance UAVs have less real-time intelligence, since they usually do not loiter over 1.1 UAVs Review 3 the battle area. The idea of designing a UAV that could remain in the air for a long time has been around for decades, but is only now starting to become a reality. Endurance UAVs for low-altitude and high-altitude operation, the latter sometimes referred to as \"high-altitude long-endurance (HALE)\" UAVs, are now being developed. HALE UAVs can be used as a cheaper alternative to satellites for atmospheric research, earth and weather observation, and particularly communications. In the mid-1980s, Boeing developed a large and capa-ble HALE UAV named the \"Condor\" that was a significant milestone in the development of endurance UAVs. The Condor featured lightweight composite and honeycomb struc-tures, autonomous controls, high altitude aerodynamics, and a fuel-economical propulsion system. It set an altitude record for piston-powered aircraft of 20,420 meters during its 141-hour flight test program, and stayed aloft for two and a half days during one of its test flights. The antennae and sensors are mounted in a boxy fuselage that was 1.32 meters high and 0.86 meters wide. The Condor was powered by two six-cylinder liquid-cooled Teledyne Continental piston engines, each providing 175 horsepower and used duplicate, redundant flight control computers which were capable of operating autonomously from takeoff to landing, using a flight control program. NASA's work on high-altitude UAVs was revived in the late 1980s, leading to the \"Small High-Altitude Science Aircraft (SHASA)\" program. The \"micro-aerial vehicle (MAV)\" or Micro UAV is another new trend in UAVs. DARPA conducted a series of \"paper studies\" and workshops on the concept of MAVs in 1995 and 1996, leading to early engineering studies by the Lincoln Laboratories at the Massachusetts Institute of Technology (MIT), and the Naval Research Laboratory (NRL) in Washington D.C, which demonstrated the concept being feasible. The objective of the DARPA project was a MAV that could fit into a sphere 15 centimeters in diameter, weigh no more than 140 grams, fly for up to 2 hours and have a range of 10 kilometers, an autonomous navigation capability, 1.1 UAVs Review 4 and carry a day-night camera relaying information back to the main station over a radio link. A number of different MAVs, such as the Lockheed Sanders \"MicroSTAR\", the AeroVironment \"Black Widow\", the Lutronix Corporation \"Kolibri\", the Micro Craft \"Small Lift Augmented Ducted Fan\" (SLADF) and the \"MicroBat\" ornithopter from the California Institute of Technology (CalTech), have been developed as part of the DARPA effort. UAVs provide now a great interest in research and practice for both military and civilian uses, after more than 70 years of limited application. This is due to the following fundamental breakthroughs that changed the poor stability and enhanced the limited utility of UAVs: \u2022 Advances in electronics technology made payloads with high levels of functionality, affordable cost and compatibility with UAV size, weight and power budgets possible. New technology of sensors such as gyroscopes, accelerometers, infrared (IR) imagers, micrometers, synthetic aperture radars (SAR) and GPS navigation system helped improve the stability and accuracy of UAVs. \u2022 Advances in control systems show promise in making UAVs more routine and robust than in the past. New modelling and simulation software can now simulate nonlinear UAV models within minor errors. New control algorithms such as feedback lineariza-tion, backstepping, if oo loop-shaping and Model Based Predictive Control (MBPC) - are currently investigated as flight controllers for highly reliable and autonomous UAVs. \u2022 In many important mission areas, UAVs give indications of delivering results at attractively lower economic and political costs than manned systems due to their increased maneuverability, reduced cost, reduced radar signatures, longer endurance, and almost zero risk to crews. Vertical take-off and landing (VTOL) type UAVs 1.2 Sensors for Unmanned Air Vehicles 5 exhibit even further advantages in maneuverability features. 1.2 Sensors for Unmanned Air Vehicles Essential attitude sensors, such as accelerometers and gyroscopes, and GPS receivers are commonly installed onboard of UAVs to record flight data in real time. In this section, background information on these sensors, in the above order, is offered. 1.2.1 Accelerometers Acceleration sensing devices, called accelerometers, use Newton's second law directly or indirectly to compute acceleration from force. The force is usually measured either through relative displacement sensing or stress'sensing of the components of the structure, which is a mass-damper-spring system consisting a proof mass suspended by a spring anchored on a fixed frame, and a damper for effects involving the dynamic movement of the mass. Normally, the proof mass is only responsive to the acceleration in one direction known as the sensitive axis of the accelerometer. When the conditional output signal of the accelerometer is an electrical voltage, the equation from the output voltage, Vx, to the acceleration, as can be written as: where Ox is the offset voltage and Sx is the device sensitivity. The offset voltage and sensitivity parameters are provided by the manufacturers by testing and complicated pro-cedures which involve a calibration with respect to different temperatures. However, for more accurate usage, regular calibration of these parameters before each experiment may be required due to aging, temperature variation and other environmental factors. Cross-axis sensitivity indicating how much an acceleration can be excited by accel-erations perpendicular to its sensitive axis is another important parameter especially for multiple axial accelerometers. Cross-sensitivity of a good accelerometer should not be 1.2 Sensors for Unmanned Air Vehicles 6 more than 3% of the main axis sensitivity. For a biaxial accelerometer, the revised equa-tion from the output voltages to the accelerations of the X and Y axes can be written as: SyyiVx ~~ Ox) SXy(Vy Oy) SxxSyy SXySyx (1.2.2) _ Sxx(Vy \u2014 Oy) \u2014 Syx(Vx \u2014 Ox) . . ay \u2014 FI 5 c (i-.t.o) Jxx*Jyy ^xy^yx where Sij is the cross-sensitivity along i axis for an acceleration along the j axis. The large size of an accelerometer due to a combination of mechanical and electrical components had limited its application for a long time. In the late 1980's, new knowledge and technology for fabricating electromechanical structures (micromachining) made the first commercial microaccelerometers available, see (1) (2). Microaccelerometers are now used in biomedical applications for activity monitoring; in the automotive industry to activate safety systems and to implement vehicle stability systems and electronic suspen-sion; in military applications for impact and void detection; in industrial vibration test equipments and also in our field of interest i.e. UAV application for inertial navigation and autopilot systems. Three types of the microaccelerometers are widely used: piezoelectric, piezoresis-tive and capacitive devices. Piezoelectric devices can not measure constant accelera-tion for more than a few seconds because of their leakage problem and thus they can not be used for low bandwidth applications. The first micromachined accelerometer was piezoresistive(24). The main advantage of piezoresistive accelerometers is the simplicity of their readout circuitry. However, they have larger temperature sensitivity, and smaller overall sensitivity compared to capacitive devices. The capacitive accelerometers which are the most commercially successful micromachined accelerometers, operate based on the variation of a capacitance. They have high sensitivity, good D.C. response and noise performance, low drift, low temperature sensitivity and low power dissipation(35). 1.2 Sensors for Unmanned Air Vehicles 7 The advantages of microaccelerometers are that they are less expensive and more durable than their larger counterparts. They are installed in small rugged or steel packages protecting them in harsh environments, hence they can be easily integrated with onboard signal processing circuits. The multiaxial accelerometer is much easier and more accurate to implement since alignment of the sense elements is accomplished at the fabrication level. However, the main disadvantage of microaccelerometers is that the silicon-based structure is more sensitive to temperature variation and has a higher nonlinearity error. Mechanical noise is also a problem due to their small proof mass, which is typically about 5 mg. 1.2.2 Gy roscopes There are mainly three types of gyroscopes encountered in practice: i) the spinning mass gyroscope, ii) the optical gyroscope and iii) the vibrating gyroscope. The spinning mass gyroscope, (ie. the classical gyroscope), has a mass spinning steadily with free movable axis which is also called gimbal. When the gyro is tilted, the gyroscopic effect causes motion orthogonal to the direction tilt sense on the rotating mass axis and hence the angle by which it moved is recorded. Because the mechanical constraints are causing numerous error factors, fixing the axis with springs where the spring tension is proportional to the precession speed is mandatory. By integrating the spring tension the angle can be measured. The dry tuned gyro, which means dynamically tuned gyro, is a type of spinning mass gyro, which has been designed to cause very small mechanical constraints once the spinning speed reaches a specific speed. The disadvantages of this device are its large size, heavy weight, low accuracy and insensitivity to small vibrations. The optical gyroscope lets laser rays reflect around many times within its enclosure. If the enclosure rotates, the duration between the moment of laser emittance to eventual reception will be different. In a RLG (Ring Laser Gyro), the laser go-around is achieved by mirrors inside the enclosure. In a FOG (Fiber Optic Gyro), the laser go-around is 1.2 Sensors for Unmanned A i r Vehicles 8 achieved by a coil of optical-fiber. Since the Laser emitter deteriorates with time and the fiber has finite life, optical gyroscopes are more fragile compared to other gyroscopes. The vibrating gyroscope contains a vibrating resonator which, when rotated, is sub-jected to Coriolis effects that cause secondary vibration orthogonal to the original vibrating, direction. By sensing the secondary vibration, the rate of turn can.be detected. For vibra-tion and detection, the piezo-electric effect is often used. Therefore, vibrating gyros are often called \"piezo\", \"ceramic\", or \"quartz\" gyros. The vibrating gyroscope has recently been the most widely used gyroscope because it features high resistance against external shock, a wide range of operating temperature from -40 to +80 deg C, low drift, high reliability and accuracy, low cost and light weight. The angular velocity sensor is actually, a rate-gyroscope. In fact, modern gyroscopes are usually rate-gyroscopes. This is the type of gyro used in the MicroStrain 3DM IMU in our application. 1.2.3 GPS The Global Positioning System (GPS) is a satellite-based navigation system made up of a network of 24 satellites placed into orbit by the U.S. Department of Defense. The 24 satellites powered by solar energy and backup batteries are orbiting the earth about 12,000 miles above us. They are constantly moving, making two complete orbits in less than 24 hours. The first GPS satellite was launched in 1978 and a full constellation of 24 satellites was achieved in 1994. GPS satellites transmit two low power radio signals, designated L l and L2. Civilian GPS uses the L l frequency of 1575.42 MHz in the UHF band. The signals pass through clouds, glass and plastic but do not go through most solid objects such as buildings and mountains. A GPS signal contains three different bits of information: i) a pseudorandom code; ii) an ephemeris data and iii) an almanac data. The pseudorandom code is simply 1.2 Sensors for Unmanned Ai r Vehicles 9 an ID. code that identifies which satellite is transmitting information. The ephemeris data, which contains important information about the status of the satellite (healthy or unhealthy), current date and time, is essential for determining a position. The almanac data tells the GPS receiver where each GPS satellite should be at any time throughout the day. GPS receivers take the signal information transmitted by the GPS satellites to the earth and use triangulation to calculate the user's exact location. Essentially, the GPS receiver compares the time a signal was transmitted by a satellite with the time it was received. The time difference tells the GPS receiver how far away the satellite is. A GPS receiver must be locked onto the signal of at least three satellites to calculate a 2D position (latitude and longitude) and track movement. With four or more satellites, the receiver can determine the user's 3D position (latitude, longitude and altitude). Once the user's position has been determined, the GPS unit can estimate other information, such as velocity, position, distance to destination and more. The GPS system works in any weather conditions, anywhere in the world, 24 hours a day. The accuracy of the GPS receivers can be improved by the their parallel multi-channel design skill. As a result 12 parallel channel receivers, widely used today, are quick to lock onto satellites when first turned on and maintain strong locks, even in dense foliage or urban settings with tall buildings. However, since certain atmospheric factors and other sources of error can affect the accuracy of GPS receivers, the 12 parallel channel GPS receivers are only accurate to within 10 meters on average. This is still not accurate enough for the UAV navigation objective. To achieve better accuracy, the Differential GPS (DGPS) is employed. This sensor corrects the GPS signals to an average of 3 meters. Differential GPS works well by placing a GPS receiver, called a reference station, at a known location. Knowing its exact location, the reference station can determine the errors in the satellite signals. It does this 1.2 Sensors for Unmanned Air Vehicles 10 by measuring the ranges to each satellite using the signals received and comparing these measured ranges to the actual ranges calculated from its known position. The difference between the measured and calculated range for each satellite is called the differential correction. The differential corrections for each tracked satellite are formatted into a correction message and transmitted to DGPS receivers. These differential corrections are then applied to the GPS receiver's calculations, removing many of the common errors and improving accuracy. The level of accuracy obtained is a function of the GPS receiver and the similarity of its environment to that of the reference station, especially its proximity to the station. The reference station receiver determines the error components and.provides corrections to the GPS receiver in real time. Corrections can be transmitted over F M radio frequencies, by satellite, or by beacon transmitters maintained by various government organizations. Another option to get an accuracy of better than three meters is to use Wide Area Augmentation System (WAAS). The WAAS program is developed by the Federal Aviation Administration (FAA) and the Department of Transportation for use in precision flight approaches. The WAAS consists of approximately 25 ground reference stations positioned across the United States to monitor GPS satellite data. These stations collect data from the reference stations and create a GPS correction message. This correction accounts for GPS satellite orbit and clock drift plus signal delays caused by the atmosphere and ionosphere. The corrected differential message is then broadcast through one of two geo-stationary satellites over the equator. The information is compatible with the basic GPS signal structure so that any WAAS-enabled GPS receiver can read the signal. The GPS module used in our project is GPS-PS2 GPS receiver board from the U-box company. The GPS-PS2 receiver is a fully contained and shielded GPS receiver board based on the SiRF star II technology. Operating at a nominal voltage of 5 Volts, it provides two serial ports which handle NMEA or SiRF data format or accept differential 1.2 Sensors for Unmanned A i r Vehicles 11 GPS data (RTCM). GPS-PS2's accuracy is around 1 meter with only 13 g weight. 1.2.4 Attitude Estimation for U A V s based on G P S \/ I M U Attitude sensors collect data for controllers to estimate the attitude and environment information of UAVs in real time. The conventional method for UAV attitude estimation is to use an Inertial Measuring Unit (IMU) which includes accelerometers, gyroscopes and the compass in conjunction with Kalman filters. The primary advantage of this method is that the acceleration, angular rotation and attitude data can be updated at high rates separately, allowing for a high reliability of the system. However, this approach requires very expensive IMU sensors. Also the errors caused by the bias in the sensor readings and the misalignment of the unit's axes with respect to the local navigation frame accumulate with time. There are methods to correct such discrepancies but they only add to the complexity of the system. The advanced approach used in this project is the use of low-cost and small size in-tegrated Global Positioning System\/IMU (GPS\/IMU). This relatively inexpensive sensor is used to estimate the attitude of the UAV (7). The GPS receiver is an external sensor that provides, as previously described, absolute geometrical information with a sampling time of one second. This sensor is combined with a silicon micro-machined low-cost, small size and highly reliable IMU, 3DM-G from MicroStrain Inc. 3DM-G with the weight of 40 g. This sensor consists of three angular rate gyros, three orthogonal DC accelerometers, three orthogonal magnetometers, muliplexer, 12 bit A \/D converter and embedded micro-controller to provide pitch, roll, yaw angle and rate, and accelerations and velocities along the three axes. The 3DM-G sensor provides orientation in matrix and quaternion formats which may be used directly or easily converted to Euler angles. The digital serial output can also be programmed to provide scaled sensor data from all nine sensors. Operating at a nominal voltage from 5.2 VDC to 12 VDC, 3DM-G has' 0.1 degree angle resolution 1.3 High Fidelity Linear Parameter Varying ( L P V ) Modell ing 12 and 0.0025 percent \/ degree C temperature drift. By combining the short-term stability of the up to date IMU, with long-term accuracy of DGPS the position accuracy can be in the range of 1 to 5 m. 1.3 High Fidelity Linear Parameter Varying (LPV) Mod-elling 1.3.1 G a i n Schedu l ing and L P V M o d e l s In recent years (15) (25), Linear parameter-varying (LPV) systems have received a grow-ing interest in the control community, especially for UAVs. The LPV system theory is motivated generally by the available gain scheduling methodology for control system de-sign. This type of modelling is required especially when the system's nonlinearities are significant. The classic gain-scheduling methodology involves the design of several LTI controllers for the parameterized family of linearized models of the system and further interpolating the controller gains. In practice, the nonlinear plant is linearized for con-troller design in a number of operating points. This design approach cannot include all parameter variations. Especially for high varying parameter systems, such an approach cannot guarantee stability, robustness and good performance (26). Moreover, the switch-ing surface between operating points must be stable which is sometimes difficult to achieve or analyze. The LPV model shown in Equation 1.3.1 is written as a linear state model where the coefficient matrices are functions of external scheduling variables a and a. Bi(a,a) J52(a,o;)J Dii(a,d) Di2(a,d)l | |u| (1-3.1) D2i(a,a) D22(a,a) In the above Equation 1.3.1, x is the state, u is the input, z is an error signal to be controlled, y is the measured output, v denotes the disturbance and a denotes the external X z = A(a, a) Ci{a,a) C2{a,a) X V u 1.3 High Fidelity Linear Parameter Varying (LPV) Modelling 13 scheduling variable vector. It should be noted here that the scheduling variable can be one or more states augmented in the vector a. The high fidelity LPV model can guarantee the closed loop stability and robustness, if external scheduling variables are carefully chosen and kept in some given ranges. The solu-tion for the LPV control synthesis problem can be achieved via a Linear Matrix Inequalities (LMIs) optimization problem. This synthesis problem is a convex problem for which fairly efficient optimization techniques are available (4). In conjunction with LPV models, an-other approach is to use a single or parameter-dependent Lyapunov function. This LPV structure is suitable whenever the value of the parameter is measurable in realtime. The resulting controller is time-varying and smoothly scheduled by the measurements of the scheduling parameters. 1.3.2 Quas i -LPV Models The LPV model described above uses an external scheduling variable G and is not directly applicable to the UAV control problem. An approach to this problem is the use of a Quasi-LPV model for which the scheduling variables are among the measured system states. The advantage of this method is that from a high fidelity nonlinear model by using the Quasi-LPV transformation, the model is written in a form ready for linearization. In Equation 1.3.1, a family of equilibrium states xeq(a), ueq(a), veg(a) , parameterized by the scheduling variable vector a , is obtained by setting the state derivatives to zero: A(xeg(a),ueq(a),veg(a)) \u2014 0, a e S where S is the space within which the scheduling variables lie. Associated with the above equilibrium family is the error equilibrium family, Ci(xeq(a), ueq(a), veq(a)) = zeq(a) = 0, a \u20ac S 1.3 High Fidelity Linear Parameter Varying ( L P V ) Modell ing 14 and the measured output equilibrium family, C2(xeq(a),ueq{a),veq(a)) = yeq{a) = 0, a e S As result, the Quasi-LPV model which is equivalent with the LPV model in Equation 1.3.1 when the scheduling variables are the measured states a can be written as: A(a,a) Bi(a,a) B2{a,a) Ci(a,a) Du(a,a) Du(a,a) C2 ( a , a) D2i(a, a) D22 (a, a) (1.3.2) where q is vector of plant states not used for scheduling. Embedding the family of equilibrium states xeq(a), ueq(a), veq(a) into the model above, we can obtain the Quasi-LPV model. The simple example below is introduced to explain in detail the procedure of converting a nonlinear plant model to a Quasi-LPV model. Assuming that the nonlinear model with scheduling variable a can be written as: A n (a) A12(a) A2i(a) A22{a) a Bu(a) + B2i{a)_ (1.3.3) A family of equilibrium states is obtained by setting the state derivatives to zero: 0 = f(a) + A{a) a aeq(a) + B{a)6eq(a) (1.3.4) Therefore, combining Equation 1.3.3 with Equation 1.3.4, we can obtain the final model as: a q- qeq(a) = 6-0' A12(a) Bn(a) 0 A22 - \u00a3[qeq(a)}A12(a) B21 - \u00a3[qeq{a)]B11{a) a q - qeq(a) 6 - 5eq(a) |0 -\u00a3[qeq(a)]A12(a) -\u00a3[qeq(a)}Bu(a) This final Quasi-LPV form is an exact representation of the nonlinear model and is directly used for control law design. 1.4 HQO loop shaping design for U A V s 15 1.4 i f o o loop shaping design for UAVs The Hoc loop shaping design procedure combines classic loop shaping and notion of band-width and performance limitation with model HQO robust stabilization. This method was first proposed by McFarlane and Glover (18). The particular advantages of Hoc design are \u2022 The shaped model with the controller and the shaping functions W\\ and W2 can be easily tuned to a required system bandwidth and to account for performance limitations. \u2022 The generalized stability margin e, a multivariable extension of gain and phase margins can be used to ensure robustness (21). \u2022 In this framework, the controller gain scheduling and anti-windup can be used in a straight forward manner due to an observer type implementation. \u2022 The system can guarantee closed-loop stability and a level of robust stability at all frequencies. Due to these features, the method has been widely used with significant attempts being made in the aerospace industry. In 1993 (11), a gain scheduled controller was imple-mented on a Generic VSTOL Aircraft Model (GVAM) developed by the Royal Aerospace Establishment (RAE). The GVAM is a highly realistic nonlinear model representative of the VSTOL aircraft and thus requires some form of gain scheduling with the operating point. In 1994 (34), H^, loop shaping controller was used for position control for a ra-dio controlled helicopter at hover. In 1996 (33), a fixed gain two-degree-of-freedom (2 DOF) Hcc loop shaping controller was designed for the Westland Lynx multirole combat helicopter. The two-degree-of-freedom architecture is more insensitive to disturbance and 1.4 i f o o loop shaping design for U A V s 16 model uncertainty. Thus the controller is more robust. In 1997 (30), a fixed-gain two-degree-of-freedom HQQ loop shaping controller was developed for the National Research Council of Canada's Bell 205 airbone simulator. In 1999 (21), a new linear parameter-varying (LPV) Hoc loop shaping design procedure was proposed for the design of a flight controller for the pitch dynamics of the VAAC Harrier. In this approach the nonlinear model of the VAAC Harrier is also approximated by a \"Quasi-LPV\" model. The con-trol law is then automatically scheduled over the flight envelope using this Quasi-LPV model. In 2001 (3), the two degree of freedom control architecture and recent advances in parameter-space control design techniques were combined to form a new approach for de-signing flight controllers for the high performance aircraft, F-16, throughout a large design envelope. This parameter-space technique extends traditional parameter-space techniques to enable the mapping of frequency response specifications into the parameter-space pro-viding a straightforward way of selecting the gains in a fixed control structure, to satisfy HQO robustness and performance specifications. In our work, we present three loop robust 2 DOF HQO flight controller scheme for a quad-rotor UAV. The three loop flight controller scheme can guarantee robustness stability and good decoupling of the nonlinearity of the model. The 2 DOF structure can provide good tracking and disturbance rejection ability. The quad-rotor UAV can achieve the higher maneuverability such as vertical take-off and landing and higher acceleration. Wi K l + I \u2022 K2 * - l Figure 1.1: 2 DOF Loop Shaping Controller The general 2 DOF Hoc controller scheme is shown in Figure 1.1. K\\ is the controller for commands and tracking, and K2 is the controller for measurement 1.4 HQQ loop shaping design for UAVs 17 or feedback signals. The shaped plant is Gs = W2GW1, with a normalized coprime factorization Gs \u2014 M \" 1 ^ . In Figure 1.1 Wi gives exact model-matching at steady state. The design procedure of 2 DOF loop shaping controller has the following steps: 1. Loop Shaping: Assume G denotes a linear time-invariant model of the plant for de-sign. Using frequency-dependent precompensator W\\ and\/or a postcompensator W2, the singular values of nominal plant G can be modified to a desired open-loop shape and also a specific loop bandwidth. The shaped plant Gs is defined as Gs = W2GWi. The postcompensator W2 contains low-pass and lead-lag filters for noise rejection and robustness augmentation. The precompensator W\\ contains proportional and integral (PI) filters to account for reference and disturbance characteristics. The proportional parts can reduce the phase lag around crossover and set the actuator range. The integral parts can improve the reference following and the disturbance rejection ability. 2. A desired closed-loop transfer function TTef between the commands and controlled outputs is selected. 3. Robust Stabilization: The stability margin e : e - V 2 = \\\\(I- GsK2)~1GsKi - r r e \/ | | o o is calculated, where p ensures more emphasis on performance or model matching than robust stability. If the stability margin is in the interval e > 0.3, the #00 controller would guarantee robustness based on theoretical (18) and practical experience (21). If the stability margin is not satisfied, an adjustment of W\\ and W2 or TTef is required and hence a restart from 1 is required. 1.5 M P C control for UAVs 18 4. Implementation: The 2 DOF controller K\\ and K2 are then constructed by splitting the K\u201eo = The resulting closed-loop system is simulated to verify the system stability and robustness. Iterations may be required at this level too . 1.5 M P C control for UAVs After being introduced in the late 1960's, Model Based Predictive Control (MPC) has received much interest and has been proven very successful in industrial applications (23). Due to the computational burden of the on-line optimization, MPC was traditionally ap-plied to low performance systems especially in the chemical industry (23). Further, in (8) (10) (9), the MPC scheme was applied to a linearized model of a high performance and high bandwidth aerospace process. In order to solve the nonlinear nonconvex optimiza-tion problem when the high performance nonlinear model is integrated into MPC, a new attractive linearisation technique, which is an input-output feedback linearisation (IOFL), is proposed in (31). IOFL is a strategy to find a static state feedback controller, such that the resulting closed loop system has a desired linear input-output behavior. Recent work has focused on extending MPC strategy to a nonlinear high perfor-mance aerospace system (27) (13). Nonlinear Model Predictive control (NMPC) is an optimization-based method where a nonlinear process model is used to predict the ef-fect of future input moves with operating constraints and other demanding performance requirements. It is apparent from (27) and (13) that this method provides good track-ing performance and outstanding robustness to parameter uncertainty as well as actuator saturation when nonlinearity and coupling dominate the UAV dynamics. The MPC technology can be applied to complex, multivariable, non-minimum-phase, open loop with a long time delay (14) unstable systems. The main advantages of MPC are that the constraints can be easily handled. Complex processes such as MIMO nonlinear process can be controlled without extremely special precautions. The method is quite 1.5 M P C control for UAVs 19 robust to modelling errors and last but not least, is easy to tune. PAST k-3 k-1 FUTURE \u2022 Set p o i n t P r e d i c t e d o u t p u t JJ] y ( k ) = r ( k ) M a n i p u l a t e d i n p u t C o n s t a n t i n p u t u(k+l) ~i\u2014 i\u2014r k+l k+Nu - i r\u2014*-k+Nl k+N2 C o n t r o l h o r i z o n - Nu Minimum o u t p u t h o r i z o n - N l Maximum ou t p u t h o r i z o n - N2 Figure 1.2: Prediction Strategy The basic strategy of MPC is the repeated optimization of a performance objective over a finite horizon. Figure 1.2 characterizes the way predictions are used within the MPC. It consists of the following three steps: 1. Given a reference trajectory r(k +1) based on up to date time k, we assume y(k +1) as the \/-step ahead predicted output over prediction horizon from Ni up to Ni. This predicted output will be a function of future control increments Au(k +I) over control horizon (Nu), where Au(k + I) = u(k + I) - u(k + 1 \u2014 1). For prediction, it is also assumed that Au(k + I) = 0 for I > Nu. 2. Obtain the optimization of the following cost function: N2 Nu J(k) = \u00a3 \\\\y(k + 1)- r(k + l)\\\\2Q{l) + \u00a3 \\\\Au(k + l)\\\\2m Ni 1=0 1.5 M P C control for UAVs 20 The weights Q(l) and R(l) are usually independent of k, but in special situations, they may need to vary with k. Note that'the norm ||.||Q is defined as ||.||Q = xTQx. 3. Minimize the above optimization by solving a standard Quadratic Programming problem subject to constraints on: . \u2022 Input Levels: ui(l) < u(l) < uu(l) , where k < I < k + Nu \u2014 1 \u2022 Input Change Rates: Aui(l) < Au(l) < Auu(l), where k < I < k + Nu \u2014 1 \u2022 Output Levels: yi(l) < y(l) < yu(l), where k < I + Ni < k + N2 4. Apply the first control signal of the sequence to the plant and displace the horizon one time-step towards the future and repeat the procedure. In order to obtain predictions of the process output, an internal model based on the current measurements is needed. By using a linear model, the resulting optimization prob-lem of the MPC will be a quadratic program. If necessary, the cost function above could involve adjustable weights to compute the present control move such that the predicted output follows the reference in the desired manner. Chapter 2 The Experimental Rig and The Nonlinear UAV Model 2.1 The Quad-rotor Helicopter Experimental Setup In order to carry out flight control experiments, a rig including a custom designed flying mill, a personal computer, a dSPACE DSP board, a microprocessor pulse modulator, a radio transmitter and the Dragonflyer III was built. 2.1.1 T h e F l y i n g M i l l A picture of the flying mill is shown in Figure 2.1. The steel base and carbon fiber boom limit the flight route of the UAV Dragonflyer III to a hemispherical shell of 1 meter radius. Two revolute joints at the base and near end of the boom provide two degrees of freedom required for flight testing and system identification. In order to mitigate friction in the joints, both the revolute joint at the base and the revolute joint at the near end of boom use two low-friction radial ball bearings. The bearings are mounted in-line at an appropriate distance apart, parallel to their respective rotational axis, to minimize the moment loads. The far end of the boom is connected to a custom designed spherical bearing which provides the tested UAV enough tilted degrees of freedom and also prevents its four propellers from touching the ground or the boom. Due to the above features of this rig, the modelling and identification performed for the UAV did not justify the modelling 21 2.1 The Quad-rotor Helicopter Experimental Setup 22 Figure 2.1: Photo of The Flying Mill With Dragonflyer III 2.1 The Quad-rotor Helicopter Experimental Setup 23 of the flying mill. This spherical bearing is made of aluminum to reduce the payload of the tested UAV. The additional payload, due to the weight of boom and the spherical bearing, is approximately 60 g. The flying mill is mounted on a solid board to prevent overturning during the experiment or a catastrophic failure. Another platform is built to support the UAV during take off and landing. This platform can be replaced by limiting the UAV elevation. Two optical encoders with resolution of 0.2 degree were used to sense the elevation and angles radial defining the UAV position. One optical encoder is fixed at the bottom of the steel base to record the rotational angle of the vertical shaft. Another optical encoder is mounted at the revolute joint at the near end of the boom to record the elevation of the tested UAV. The two optical encoders are connected to the D\/A dSPACE interface board, DS1102. During the experiment, the position of the UAV is provided by the data from the two optical encoders. This data is used for the calibration of the GPS device during system identification. The attitude data is updated by three gyroscopes on the UAV. The accelerations along three axes are given by the triaxial accelerometer also placed on the UAV. The encoders were used during the identification and validation of the UAV parameters. 2.1.2 Data Flow in the Experimental System Figure 2.2 shows the flight control structure and the data flow in the experimental setup. The attitude data and position data taken from IMU and GPS receiver module and the two encoders located on the flying mill, respectively, is transferred to the dSPACE D\/A interface board DS1102. Further the data is analyzed by a controller which is downloaded onto the dSPACE DS1102 processor. Control data can be generated either automatically, by the designed auto flight control laws, or manually, by the slider bars of the cockpit designed in the Control Desk of the dSPACE system. A microprocessor, PIC16F877, is 2.1 The Quad-rotor Helicopter Experimental Setup 24 D r a g o n f l y e r l l l Operator IMU and GPS module Transmitter Receiver 4 rotors T J v v Personal computer ^ ==^ Receiver Dspace D\/A DSP board(DS1102) Radio Transmitter! 4- Microprocessor Pulse Modulator Figure 2.2: The Data Flow In Experimental System programmed to transfer the control data to a pulse width modulated (PWM) signal in order to reduce significant DS1102 CPU load. This signal is further used to control the four rotors of the Dragonflyer III via a 4 channel Futaba radio transmitter working in training mode. The detailed implementation of the pulse modulator is disclosed in Section 2.1.3. Note the digital path between the controller implemented in dSPACE and the transmitter. Currently no wireless connection between the flight control stationary computer and the sensors is used. 2.1.3 The Pulse Modulator 1 W \u2022 1.1 2 \u00ab \u2022 1.1 3 4 \u2022 1.1 4 4 > 1.1 0.46 0.4 0.4 0.4 0.4 11.19msec 17.65msec Figure 2.3: PWM Signal Definition To transfer the control data from the dSPACE system to the Dragonflyer III actuators 2.1 The Quad-rotor Helicopter Experimental Setup 25 via the 4 channel Futaba radio transmitter, an input PWM signal to the trainer connector of the radio transmitter during one period shown in Figure 2.3 is implemented. The maximum pulse width for each channel is 1.52 msec while the minimum is 0.68 msec. This represents a range from 0 to maximum RPM from the UAVs DC motors. Therefore, 1.1 msec is the neutral point. Channel 1 is related to the roll angle of the UAV. Increasing the pulse width from the neutral point makes the UAV roll right left. Channel 2 controls the pitch angle of the UAV. Increasing the pulse width from the neutral point lets the UAV fly forward. Channel 3 defines the throttle. The UAV flies upward if the pulse width is increased. Channel 4 represents the yaw angle. The UAV yaws right side while the pulse width is increasing from the neutral point width. Channel 5 is varied to compensate for one constant period time. The first delay, 0.46 msec, and the other 4 delays, 0.4 msec, are constant. Cockpit i n Control Desk! Four pulse width data dSPACE target f i l e developed in SIMULINK 6 b i t pulse width data and 2 b i t pulse m channel number 8 b i t d i g i t a l I\/O i n dSPACE DS1102 board Microprocessor* RD 3 for the PWM signal RA 0-5 for pulse width data and RB 0,1 for pulse channel number Figure 2.4: The Data Flow of The Pulse Modulator System Figure 2.4 shows the implementation and data flow of the pulse modulator including the cockpit type interface designed in the Control Desk of dSPACE system, SIMULINK file, 8 bit digital I\/O in the dSPACE DS1102 board and the microprocessor, PIC16F877. During operation, the four pulse width data given either by the operator or by the flight control system in the cockpit is transferred to the dSPACE resident algorithm. This algorithm implemented in SIMULINK has been downloaded via the Matlab Real Time Workshop on to the DS1102 board. The implemented code converts the four pulse width 2.1 The Quad-rotor Helicopter Experimental Setup 26 data to 8 bit binary data where 6 bits represent the pulse width data and 2 bits are denning the pulse channel number. Further, using the 8 bit digital I\/O in dSPACE DS1102 board, the 8 bit binary data can be downloaded to the PIC16F877 microprocessor. The RA 0-5 pins of the microprocessor are the inputs of 6 bit pulse width data while RBO and 1 are the inputs of 2 bit pulse channel number. The RD3 is the output of the PWM signal. This is the signal transferred to the trainer connector of the radio transmitter. The cockpit designed using the layout feature of the Control Desk of the dSPACE system contains four separate slider bars related to the pulse width data of the four channel. The data of the slider bar from 0 to 60 maps the pulse width in Figure 2.4 from 0.68 to 1.52 msec. Thus incrementing the data by one unit reflects into one increment of the pulse width by 0.014 msec. The top level SIMULINK model of the pulse modulator is shown in Figure 2.5. The four constant values of the Pulse Widthl to the Pulse Width4 blocks can be manually controlled using the four slider bars designed as part of the cockpit face plate or automatically by the flight controller. The initial value is set to 30 which is mapped to the neutral point (1.1 msec) of the PWM signal in Figure 2.3. The output is an 8 bit binary data. This output is connected to the 8 bit digital I\/O ports via DS1102OUT block shown in the dSPACE RTI1102 Toolbox. The sampling time is 1 msec which nearly reaches the limit of the DS1102 DSP board. Figure 2.6 reveals the Pulse Modulator block at the top level SIMULINK model. This diagram generates a periodic signal with a period of 4 msec or 4 samples. In the first sampling of each period, the 8 bit binary data of the first pulse width is obtained. Second, third and fourth sample times of each period read the 8 bit binary data of the second, third and fourth pulse width data, respectively. Using this parallel processing, all the four pulse width data can be transferred to the microprocessor using only the 8 bit digital I\/O ports. The A \/ D l to A\/D4 subsystems in the diagram are converting the analog value of 2.1 The Quad-rotor Helicopter Experimental Setup 27 Pulse Widthl Pulse Wldth2 Pulse Wldth3 Pulse Wldth4 Puis e1 Pulse2 Out Puis 63 Pulse4 Pulse Modulator for Draganflyer III Made by: Ming Chen Date: 01 September, 2002 DS1102OUT P u!ss Modulator Figure 2.5: The Top Level Simulink Model of The Pulse Modulator 1*1\" vt Pulse W i d t h l Pulse Width3 r x j - K Z D Out1 Figure 2.6: The Base Level of The Pulse Modulator Simulink Model 2.1 The Quad-rotor Helicopter Experimental Setup 28 each pulse width data from 0 to 60 to 8 bit binary number whose 0 and 1 bit represent the pulse channel number. The bits 2 to 7 bit represent the pulse width data. Each A \/ D subsystem contains a Multiport Switch with 61 inputs which are connected to 61 different 1-D arrays with 8 array elements related to the 8 bit binary data of the pulse width data. The pulse width data is used as the control input of the Multiport Switch. The microprocessor used in the pulse modulator system is PIC16F877 from Microchip Inc.. This CMOSr FLASH-based 8-bit microprocessor with 40 pins features 256 bytes of EEPROM data memory, self programming, an In Circuit Debugger (ICD), 5 channels of 8-bit digital I\/O, 8 channels of 10-bit Analog-to-Digital (A\/D) converter, two 8-bit timers, one 16-bit timer, 2 capture\/compare\/PWM functions, the synchronous serial port configured as either 3-wire Serial Peripheral Interface (SPI) or the 2-wire Inter-Integrated Circuit (I2C) bus and a Universal Asynchronous Receiver Transmitter (USART). All of these features make it ideal for more advanced level A \/D applications in automotive, industrial, appliances and consumer applications. In this case it was very suitable for this UAV application which makes the subject of the project. The PORTA, PORTB and PORTD are bi-directional I\/O ports. RAO to RA5 of PORTA are defined as digital inputs of the 6-bit binary pulse width data from the SIMULINK model in Figure 2.6. RBO and RBI of the PORTB are set to the inputs of the 2 bit binary pulse channel number. RD3 of PORTD is the output of the PWM signal. Timer 1, a 16-bit timer, is used to record the specific delay and pulse width data. The external crystal oscillator of 20 Mhz is chosen to reach the single cycle instruction time Tcyde at l\/20Mhz * 4 = 0.0002msec. In order to implement the PWM signal in Figure 2.3, the MASM source code is writ-ten and programmed into the microprocessor. The flow chart of the MASM program and the subprogram of channel 5 are shown in Figure 2.7 and Figure 2.8 respectively. PULSEDATA1 TO PULSEDATA4 registers contain 6-bit binary pulse width data from 2.1 The Quad-rotor Helicopter Experimental Setup 29 (Start) i Define and i n i t i a l i z e PORTs \u20144 I n i t i a l i z e related f i l e registers . _ t Calculate TIMER1_L, TIMER1_H to TIMER5_L, TIMER5_H from PULSEDATA1 to PULSEDATA4 . 1 Delay 0.46 msec using Timerl \u00b1 Transfer data i n TIMER1_L and TIMER1_H to TMR1 re g i s t e r pairs and output the PWM signal of channel 1 using TIMER1 i Delay 0.4 msec using Timerl . 4 Transfer data in TIMER2_L and TIMER2_H to TMR1 register pairs and output the PWM signal of channel 2 using TIMER1 Delay 0.4 msec using Timerl . * Transfer data i n TIMER3_L and TIMER3_H to TMR1 register pairs and output the PWM signal of channel 3 using TIMER1 4 Delay 0.4 msec using Timerl \u00b1 Transfer data i n TIMER4_L and TIMER4_H to TMR1 register pairs and output the PWM signal of channel 4 using TIMER1 i Delay 0.4 msec using Timerl Transfer data i n TIMER5_L and TIMER5_H to TMR1 re g i s t e r pairs, output the PWM signal of channel 5 using TIMER1 and update PULSEDATA1 to PULSEDATA4 ( End) Figure 2.7: Flow Chart of Pulse Modulator i n MASM 2.1 The Quad-rotor Helicopter Experimental Setup 30 Q S t a r t ) I n i t i a l i z e r e l a t e d f i l e r e g i s t e r s T r a n s f e r d a t a i n TIMER5_L and TIMER5_H t o TMR1 r e g i s t e r p a i r s S t a r t TIMER1 Read p u l s e c h a n n e l number from PORTB 0-1 and p u l s e w i d t h d a t a from PORTA 0-5 D e l a y 0 .2 msec and r e a d d a t a from PORTB 0 -1 and PORTA 0-5 a g a i n N \u2014 A r e two ^ r e a d i n g s of p u l s e c h a n n e l lumber i d e n t i c a l ^ -T r a n s f e r the p u l s e w i d t h d a t a t o t h e r e s p e c t i v e r e g i s t e r from PULSEDATA1 t o PULSEDATA4 D e l a y 0.3 msec (_ End y* Figure 2.8: Flow Chart of Subprogram For Channel 5 in MASM 2.2 The Nonlinear Four-rotor Helicopter Model 31 RAO-5 for each pulse channel. The data in P U L S E D A T A registers are converted to the data in T I M E R 1 J L and T I M E R 1 . H registers which are available for T M R 1 register pairs of T I M E R 1 before T I M E R 1 starts. T I M E R 1 _ L 1 to T I M E R 1 . L 5 registers keep the Least Sig-nificant Bytes of the first to the fifth pulse width data while T I M E R I J H to T I M E R 1 _ H 5 registers hold the Most Significant Bytes of the first to the fifth pulse width data respec-tively. 2.2 The Nonlinear Four-rotor Helicopter Model The U A V used in the project is a commercial quad-rotor helicopter, Dragonfiyer III, currently wi th a 3 min flying time but extensible to 1 hour by adequate sizing of the power source and actuators. Quad-rotor helicopters using the variant rotor speeds to change the lift forces are dynamically unstable and therefore a control law is required to ensure their stability. Motions of the four-rotor helicopter are briefly described in Figure 2.9. The vertical motions along z-axis in the body-fixed frame can be obtained by changing the Figure 2.9: The Four-rotor Helicopter Mode l 2.2 The Nonlinear Four-rotor Helicopter Mode l 32 Symbol Definition Symbol Definition u(l) u(l) = F i + F2 + F3 + F4 Q pitch rate (Q) u(2) u{2) = F4-F2 r yaw rate (R) \u00ab(3) \u00ab(3) = F3-F1 <t> roll angle (PHI) \u00ab(4) u(4) = F1-F2 + F3-F4 e pitch angle (THETA) FXB force in body-axis x direction 1> yaw angle (PSI) FyB force in body-axis y direction UB body-axis x direction velocity FzB force in body-axis z direction VB \u2022 body-axis y direction velocity Fx force in earth-axis x direction WB body-axis z direction velocity Fy force in earth-axis y direction U x direction velocity Fz force in earth-axis z direction V y direction velocity Ix moment of inertia in x direction W z direction velocity ly moment of inertia in y direction X x position of COG Iz moment of inertia in z. direction y y position of COG P roll rate (P) z z position of COG Table 2.1: The Nomenclature Used in Theoretical Formulation speeds of all four rotors simultaneously. The forward motions along x-axis in the body-fixed frame can be achieved by changing the speeds of rotor 1 and 3 reversely and retaining the speeds of rotor 2 and 4. The lateral motions along y-axis in the body-fixed frame can be reached by changing the speeds of rotor 2 and 4 reversely and retaining the speeds of rotor 1 and 3. The yaw motions are related to the difference between the moments created by the rotors. To turn in a clock-wise direction, rotor 2 and 4 should increase speeds to overcome the speeds of rotor 1 and 3. The x,y axis definition becomes arbitrary since the structure presents an x,y symmetry. 2.2.1 D y n a m i c s o f F o u r - r o t o r H e l i c o p t e r Table 2.1 summarizes the nomenclature used in the theoretical formulation and further in the SIMULINK model of the four-rotor helicopter, Dragonflyer III. The rotational transformation matrix between the earth-fixed frame and the body-fixed 2.2 The Nonlinear Four-rotor Helicopter Model 33 frame can be obtained based on Euler angles: REB = Rip-Re-R<t> cos ip \u2014 sin ip 0 sin ip cos ip 0 0 0 1 cos 9 0 sin( 0 1 0 \u2014 sin 9 0 cos ( 1 0 0 0 cos 4> \u2014 sin cj) 0 sin 4> cos <f> (2.2.1) | cos ip cos 9 \u2014 sin ip cos (p + cos ip sin 9 sin (f> sin \u2022)\/> sin <p + cos V' sin 9 cos sin \u2022J\/' cos 9 cos ^  cos <j> + sin ?\/) sin 0 sin <\/> \u2014 cos ip sin ^  + sin V> sin 0 cos 01 \u2014 sin 9 cos 9 sin 0 cos 0 cos 9 Note that .Res = R%E \u2022 The transformation of velocities between body-fixed and earth-fixed coordinates is: u UB V v B w WB (2.2.2) Similarly, the accelerations, rotational velocities, positions, forces and moments can be transformed based on REB between the coordinate systems. In the body-fixed frame, the forces are defined as follows: FxB 0 FB = FyB = 0 FZB 2~2i=l Fi In the earth-fixed frame, the forces can be described as: i=l sin ip sin <f> + cos ip sin 9 cos (p \u2014 cos ip sin <f> + sin tp sin 9 cos (f. cos <j> COS 0 (2.2.3) Therefore, the equations of motion x,y,z in the earth-fixed frame are represented as: X F x - K l - x m y = Fy-K2-y z Fz - mg - K2 \u2022 z 2.2 The Nonlinear Four-rotor Helicopter Model 34 where R~i are the drag coefficients. Note that these coefficients are negligible at low speeds. As result the equations of motion can be defined below using force and moment balance: X Fx-Kx-x 2~2i=i Fi (sin tp sin <p + cos tp sin 9 cos (p) \u2014 K\\ \u2022 x m y = Fy-K2-y = YA=I Fi (s\u2122 sin 0 cos 4> \u2014 cos tp sin <f>) \u2014 K2 -y z Fz - mg - K3 \u2022 z Ylt=i Fi cos 4> cos 6 \u2014 mg \u2014 K3 \u2022 z (2.2.4) 4> = l(F3 - F i - K4<P)\/IX 6 = l(F4 - F 2 - K5d)\/Iy $ = (M1-M2 + M3-M4- K6iP)\/Iz = (Fi - F 2 4- F3 - F4 - K'6i;)\/l'z ' where Ms the length from the center of gravity of the helicopter to each rotor and Mi's are the moments of rotor i. The variable I represents the moment of inertia with respect to the axes and Iz includes the moment of inertia of z axis and the force to moment scaling factor. These moments can be measured or identified. For convenience and compatibility with the control panel of the Futaba radio trans-mitter used in conjunction with the Dragonflyer III, we define the inputs to be: u(l) = Fl + F2 + F3 + F4 u(2) = F4 - F2 . u(3) = F3 - F i u(4) = Fl-F2 + F3-F4 Therefore, the motion equations become: u(l)(sin?\/> sin0 + cos ip sin 6 cos <fr) \u2014 K\\ \u2022 x m (2.2.5) y = \u2022u(l)(sin ip sin 8 cos (p \u2014 cos tp sin (p) \u2014 K2 \u2022 y m 2.2 The Nonlinear Four-rotor Helicopter Mode l 35 u(l) cosc\/>cos# \u2014 K3 \u2022 z z = g m 9 = (u(2) - K56)l\/Iy 4> = (u(3) - K44>)l\/Ix $ = (u(4) - ^ ) \/ \/ ; 2.2.2 The Quas i -LPV model of the Four-rotor Helicopter The nonlinear motion equation of the Four-rotor Helicopter, Equation 2.2.5, can be rewrit-ten in the quasi-LPV form as follows: 1> 9. 6 sin ij> sin <fi+cos iji sin 6 cos c, m sin i\\> sin 8 cos <j>\u2014cos ip sin e m cos <p cos m 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 l\/Iy 0 0 0 l\/Iy 0 0 0 \/; 0 0 0 0 0 0 0 0 0 0 0 0 u(l) u(2) \u00ab(3) u(4) + 2.2 The Nonlinear Four-rotor Helicopter Model 36 where x y z 9 <p i> 9 T T the Euler angles are vectors of model states not used for scheduling and (p ip^ are the scheduling variables, which are the measured system states. This quasi-LPV form can be used directly for flight controller design. Freezing this model at various Euler angles can produce a suitable linear model. 2.2.3 The S I M U L I N K Model of the Four-rotor Helicopter The SIMULINK model of the four-rotor helicopter, Dragonflyer III, is detailed in this section. Figure 2.10 is based on the system Equation 2.2.5. The four inputs are u(l), u(2), u(3) and u(4) in body-fixed frame and the nine outputs are u, v, w, x, y, z, THETA, PHI and PSI. The definitions of input and output parameters can be found in Table 2.1. The model has 12 states. Saturation blocks are inserted before velocity and position in the z position of the earth-fixed frame to preserve the reality of the actuators. The expression sin ip sin <p> + cos ip sin 9 cos cp and sin ip sin 9 cos (p \u2014 cos ip sin <f> are implemented in Fen and Fcnl blocks shown in Figure 2.10. Fcn2 represents the Equation cos<\/>cos#. In Equation 2.2.5 the moments of inertia in all axes , the mass and the length from COG of the helicopter to each rotor have to be identified first so that this model reflects the dynamics of the Dragonflyer III. The Matlab M-file 'uavini.m' initializes the above parameters and hence requires running before the SIMULINK model is used. The values of the identification procedure are to be kept in this file. 2.2 The Nonlinear Four-rotor Helicopter Model 37 C L > u(1) u(1)=F1+F2+F3+F4 1\/m C D -u(2) rj(2)=F4-F2 C D -u(3)=F3-F1 C D \u2014 \u2022 11(4) u(4)=F1-F2+F3-F4 Uly \/ Uly Ulx 1\/lz_dot 1\/K f(u) f(iO Produd2 H *0 Fcn2 tntegrdtoi IntegratoM Integrator Integrators Integrator^ Integrators pitch rate^ Integrator Integrator? Integrators Integrators yaw rate IntegratorlO Integratorll Nonlinear Four-rotor Helicoper Model Made by: Ming Chen Date: September, 9,2002 K D u -KID V - K D w - \u2022 C D K D y - K D - K D THETA pitch angle - K D PHI roll angle - K D PSI yaw angle Figure 2.10: Sub Level SIMULINK Model Diagram of Dragonfiyer III Chapter 3 2 DOF HQO and Combined M P C \/ i f o o Flight Controller Design To achieve an autonomous UAV, the first step is the implementation of a 2 degree of freedom controller. The 2 DOF structure is applied for good reference tracking and disturbance rejection. This #oo controller architecture includes a number of loops. The inner loop, Figure 3.2, provides stability control at hover and the outer loop, Figure 3.12, provides velocity. A comparison with an MPC controller of the #oo controller is carried on for trajectory control purpose, Figure 3.32. Figure 3.1 illustrates the 3 loop controller architecture. Each loop is introduced as following. Controlled by the Controlled by the first outer loop inner loop Controller controller The First Outer Loop U A V model Inner Loop Fig. 2.10 Fig. 3.1 Fig. 3.11 The Second Outer Loop Fig. 3.20 Figure 3.1: 3 loop controller architecture Controlled by the second outer loop controller 38 3.1 The Roll And Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 39 3.1 The Roll And Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 3.1.1 T h e D e s i g n P r o c e d u r e The objective of the inner loop controller is to achieve a robust stability control of the quad-rotor helicopter, Dragonfiyer III, at hover and while flying in disturbed air. A secondary objective is to decouple the MIMO system. The decoupling can guarantee the use of diagonal controllers for flying a specified trajectory at higher levels. The two DOF #00 controller inner loop shown in Figure 3.2, is used to control the attitude and vertical velocity. This controller provides stabilization and decoupling as implemented in (5). The reference inputs are the vertical velocity (W_r), the pitch angle (THETA_r), the roll angle (PHI_r), the yaw rate (R_r). Its control inputs are the pedal, the longitude, the latitude and the yaw controls. This controller has been designed to fulfil the following requirements: x\" = Ax+Bu y \u00bb Cx+Du X - Ax+Bu y= Cx*Du u(1) W THETA PHI u(2) R U V uO) yaw angle u(4) y Nonlinear Dragan Flyerlll rf = Ax+Bu y \u00ab\u2022 Cx+Du rf = Ax+Bu y= Cx+Du \u2022CD \u2022CD -KID 5 ^ yawi angle *\u2014KT) Figure 3.2: Inner Loop of The Two DOF Flight Controller All the closed loop poles must lie in the left half of the s plane to ensure stability. The closed-loop bandwidth should be set so that the rising time in each loop is around 2 seconds. 3.1 The Rol l A n d Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 40 \u2022 The stability margin e is to be in the interval [0.3, 0.4] allowing for 30-40 % coprime factor uncertainty to ensure robust stability and performance. \u2022 Quick pulse and step disturbance rejection have to be achieved. The design procedure adopted for controller design can be described as follows: 1. Linearize the UAV Model: The nonlinear Dragonflyer III model in Figure 2.3 has to be reduced to the SIMULINK model in Figure 3.3 to eliminate redundant eigenvalues which make the Riccati equa-tion unsolvable. The linear model is obtained by linearizing the nonlinear model around the u = 4.8 0 0 0 equilibrium point. This is equivalent to freezing the scheduling states of the nonlinear controller around these control values. u(1>.F1+F2+F3+F4 tl(3^F3-F1 Integrator Integrator^ C D H u(4)\"F1-F2+F3-F4 - L B -Integrator - X 2 D R Figure 3.3: Dragonflyer III Reduced Model For Inner Loop Design 2. Loop Shaping: The singular values of the unshaped linear model G of the Dragonflyer III was shown Figure 3.4. Since the pitch angle and roll angle loops,are identical, the singular values of these two loops overlap each other. Therefore during controller tuning, the parameters of these two loops should be kept the same, otherwise u(2) and u(2>) would oscillate heavily around the equilibrium point. 3.1 The Roll And Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 41 Unshaped Plant Open Loop Singular Values -ISO l - ; V ^ ' i - L l ^ l L . i i , ,....L.,..X.^1 ^ 10\"*' ' 10\"' iou 10' io;: 103 Frequency (rad\/sec) Figure 3.4: Open Loop Singular Values of The Unshaped Plant Shaped Plant Open Loop Singular Values 10\"S 10\"' 10\u00b0 to1 ioz io3 Frequency (fad\/sec) Figure 3.5: Open Loop Singular Values of The Shaped Plant 3.1 The Roll And Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 42 Using frequency-dependent precompensator W\\ and a postcompensator W2, the sin-gular values of the shaped plant Gs, defined as Gs = W2GW\\ can be shaped to the designed open-loop shape and have crossover frequencies at around 7 rad\/s, see Fig-ure 3.5. The diagonal precompensator W\\ contains proportional and integral (PI) filters. The proportional parts can reduce the phase lag around crossover and set the actuator range. The integral parts can improve the disturbance rejection ability. The postcompensator W2 contains first-order low-pass filters for noise rejection and robustness augmentation and the LHP zeros in pitch angle and roll angle loops to guarantee the slope of -1 in crossover region. The expressions for W\\ and W2 are as follows: W\\ = diag W2 = diag 1.52g+0.965 23 13 1.5(17.2s+9.7) 90 73(s+2.31) 73(s+2.31) 90 s+35 s+35 s+35 s+35 3. Robust Stabilization: To reach the objective rising time, the reference closed-loop transfer function Tref was chosen as Trej = diag The synthesis gave e = 0.3532 which gives guarantees on the robustness and stability of the inner loop controller. Additional iterations were performed to get e = 0.3532. 1 9 9 9 s 2 + 2 s + l s 2 +2*0.95*3s+3 2 s 2 +2*0.95*3s+3 2 s 2 +2*0.95*3s+3 2 4. Implementation: The controller architecture is presented in Figure 3.1. The final controller, K0 has 20 states, 8 inputs and 4 outputs and was split to Kx K2 which each has 4 inputs and 4 outputs. The scaling Wj = diag 1.2863 10.132 10.145 1.2885 contains the inverse of the static steady gains for each loop. 3.1 The Roll And Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 43 3.1.2 Simulation Results The resulting closed-loop system, shown in Figure 3.2, which includes the H^o controller, weights and the plant model in Figure 2.3, was simulated to evaluate the robust stability and performance of the achieved controller. Singular Values 10\"' 10'' 10\u00b0 101 10* 10\"' Frequency (rad\/sec) Figure 3.6: Closed-loop Sensitivity Function of The Inner Loop Figure 3.6 shows the closed-loop sensitivity function S for the Hoo designed controller. The peak gain, which is 2.29 dB at 14.6 rad\/sec, allows for good disturbance rejection and reference tracking. The closed-loop complementary sensitivity function T is plotted in Figure 3.7. Its peak gain, 1.64 dB at 2.73 rad\/sec, guarantees good noise rejection. Figure 3.8 compares the nonlinear step responses of each loop separately with the Hoo controller in place. The rising time of each step response is around 2 seconds, satisfying the specification. Figure 3.9 shows the output step disturbance to each loop. The outputs return to zero after 2-3 seconds indicating very good step disturbance rejection provided by the controller. Nonlinear simulations were performed using the Hoo flight controller 3.1 The Roll And Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 44 Singular Values m il)\"'' lO' 1 VJ Id' 10* 10'\" Frequency (rad\/sec) Figure 3.7: Closed-loop Complementary Sensitivity Function of The Inner Loop Step Response From: W..r Ffcun: THETA.r From:PHI..r From: R,.r 0 1 2 3 C1 1 2 3 0 1 2 3 0 1 2 3 Time (sec) Figure 3.8: Step Responses With HQO Controller 3.1 The Roll And Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 45 Output Step Distrubance Response From W top From; THfiTA kwp From: FHI loop From: R loop -0.' Figure 3.9: Output Step Disturbance Responses The vertical velocity (W) -0.05 -0.15 -0.2 10 20 30 40 50 20 10 Pitch angle (THETA) 10 20 30 40 20 r 10 Roll angle (PHI) 10 20 30 40 1.5 0.5 Yaw rate (R) 1 'J 10 20 30 40 Figure 3.10: Simulation of The Nonlinear Model 3.2 Longitudinal and Lateral Speed, Throttle and Yaw Angle Outer Loop Design 46 The vertical velocity (W) Pitch Angle (THETA) 2.51 , , . 1 301 ; ! 0 10 20 30 40 \" 0 10 20 30 40 Figure 3.11: Simulation of The Nonlinear Model With Noise at hover and further during flight. Figure 3.10 shows the nonlinear simulation results of the four outputs when the reference values of the vertical velocity (W) was fixed to zero and the reference value of the pitch angle (9) and roll angle (4>) are changed from 0 to 15 degrees at 12 seconds. There is also a step from 0 to 1 rad\/s in the yaw rate at 30 seconds. Figure 3.11 illustrates the noise rejection ability of the controller. With white noise in the feedback of all channels, the flight controller can still guarantee the robustness and stability of the system. 3.2 Longitudinal and Lateral Speed, Throttle and Yaw An-gle Outer Loop Design 3.2.1 D e s i g n P r o c e d u r e The longitudinal and lateral speed, throttle and yaw angle outer loop, also called the velocity outer loop was designed based on 2 DOF Hoo controller and implemented as in 3.2 Longitudinal and Lateral Speed, Throttle and Yaw Angle Outer Loop Design 47 Figure 3.12. The reference inputs are the vertical position (Z_r), the x direction velocity in earth-fixed frame (U_r), the y direction velocity in earth-fixed frame (V_r), the yaw angle (PSI_r) and its control inputs are the vertical velocity (W_r), the pitch angle (THETA_r), the roll angle (PHI_r), the yaw rate (R_r), which are the reference inputs of the inner loop design. This controller should fulfil the following requirements: z_r yaw angk_r |K\"u Wi y = C>c\u00ab-D< Ax+Bu y = Cx*Du yaw angle * -AxfDu V K2 Innerloop and Dragan Flyerlll yaw angle - K D y Figure 3.12: The First Outer Loop of The Two DOF Flight Controller \u2022 All the closed-loop poles must lie in the left half of the s plane to ensure stability. \u2022 The closed-loop bandwidth shall be as high as practically possible, and the rising time in each loop should be around 4 seconds. \u2022 The stability margin e is in the interval [0.3, 0.4] allowing for 30-40 % uncertainty in coprime factor for robustness. \u2022 Quick pulse and step disturbance rejection. The design procedure can be presented as follows: 1. Obtain the Model: The nonlinear SIMULINK model diagram of the inner loop and the nonlinear Drag-onflyer III model in Figure 3.13 was linearized around the equilibrium point u =\u2022 0 0 0 ol \u2022 3.2 Longitudinal and Lateral Speed, Throttle and Yaw Angle Outer Loop Design 48 C D -W_r THETA_r \u2022 C D \u2014 * PHI_f C D \u2014 \u2022 R r Wi vi = Ax+Bu y = Cx+Du K1 + *\u2022 = Ax+Bu + w y \u00b0 Cx+Du W1 u(1) uC2) u(3) u(4) W THETA | PHI R U V 2 yaw angle Dragon Flyer III nonlinear model * = Ax+Bu >( = Ax+Bu y= Cx+Du y= Cx+Du K2 W2 -K2D - K D ) u -KD V - K D yaw angle Figure 3.13: Inner Loop And The Nonlinear Dragonfiyer III Model For The First Outer Loop Design 2. Loop Shaping: The singular values of the unshaped linear model of the inner loop and Dragonfiyer III are shown Figure 3.14. Since the Dragonfiyer III model has already been shaped in the inner loop design, only the diagonal precompensator W\\ with proportional gains is required to shape further the singular values. The singular value plot of the shaped linear model is shown in Figure 3.15. The crossover frequency is set to around 0.47 rad\/s. The expression for W\\ is W\\ = diag 0.5 0.05 0.05 0.5 3. Robust Stabilization: The reference closed-loop transfer function for the 2 DOF design was chosen as Trej with the following diagonal entries : 1.69 1.69 1.69 4 7^+2~6s+TI\u00a7 s 2 +2.6s+1.69 s 2+2.6s+1.69 s 2 +4.2s+4 The HQO optimization gave e = 0.3852, which guarantees the robustness and stability of the inner loop controller. 4. Implementation: 3.2 Longitudinal and Lateral Speed, Throttle and Yaw Angle Outer Loop Design 10\"' io\"! 10''' io1 10' Frequency (rad\/sec) Figure 3.14: Open Loop Singular Values of The First Outer Loop Design Shaped Plant Open Loop Singular Values lor the First Outer Loop \u2022101 \u2022 \u2022 \u2014 r \u2122 ~ r r m 1 ! \u2014 r \u2122 i 1 1\u2014r rrm ! 1 \u2014 \u2022 r r T r r 10'* 10\"' K)\u00b0 10! 10\" Frequency (rad\/sec) Figure 3.15: Shaped Open Loop Singular Values of The First Outer Loop Design 3.2 Longitudinal and Lateral Speed, Throttle and Yaw Angle Outer Loop Design 50 The Simulink Diagram in Figure 3.12 was implemented to check the performance of the achieved controller. The final Hoo controller, Koo, which has 64 states, 8 inputs and 4 outputs was first reduced to 20 states, 8 inputs and 4 outputs performing an optimal Hankel norm approximation of 20 order on the original model. The Vinnicombe's variation of the gap metric between the original model and the reduced model was close to zero to ensure the similarity. Then the reduced Hoo controller was split to Ki and K2 which each has 4 inputs and 4 outputs. The scaling Wi \u2014 diag 1.825 1.656 1.656 2.1116 contains the inverse of the static steady gains for each loop. 3.2.2 Simulation Results The achieved closed loop with the two degree of freedom Hoo controller for the velocity loop combined with the stabilization inner loop and the Dragonfiyer III plant, as in Figure 3.12, was simulated to check the closed loop system performance and robustness. Frequency (rad\/sec) Figure 3.16: Closed-loop Sensitivity Function of The First Outer Loop 3.2 Longitudinal and Lateral Speed, Throttle and Yaw Angle Outer Loop Design 11)'* to\"' 10\u00b0 io' V Frequency (rad\/sec) Figure 3.17: Closed-loop Complementary Sensitivity Function of The First Outer Loop The closed-loop sensitivity function presented in Figure 3.1G indicates the good distur-bance rejection provided by the HQO controller. The peak value is located at 2.31 dB and a frequency of 1.06 rad\/s. Figure 3.17 shows the closed-loop sensitivity function proving the good noise rejection of the Hoo controller. Figure 3.18 compares the step response to each loop with the Hoo controller in place. For the vertical position (z) loop, the rising time is about 5 seconds. For the x and y direction velocity loops ( U and V), the rising times are all around 4.5 seconds. The rising time of the yaw angle loop is around 3.5 seconds. These rising times met the specifications of the Dragonfiyer III. Figure 3.19 shows that the output step disturbance to each loop was rejected quickly by the Hoo controller. A nonlinear simulation of the first outer loop was implemented at the end to test the overall performance of the controller, see Figure 3.20. The reference vertical position (z) was fixed at 2. A step response from 0 to 3 occurred in both x direction velocity (U) and y direction velocity (V) loop at 10 seconds. At 20 seconds, the reference yaw angle was changed from 3.3 The Trajectory Outer Loop Design 52 0 to 40 degrees. The steps of U and V at 10 seconds also affected the vertical position (z). The controller in the z loop quickly rejected this coupling. Step Response i:rom: Inpul Point(l) f-rom: Input Points) f:.wi: Input Point(3j l:rom. Input Point^i 10 Cl 5 10 0 Time (sec) Figure 3.18: Step Responses With HQO Controller of The Velocity Outer Loop 3.3 The Trajectory Outer Loop Design 3.3.1 Des i gn P r o c edu r e The trajectory loop, shown in Figure. 3.21, was designed for the x and y direction tra-jectory control. For this 2 by 2 system, the reference inputs are the x direction position in earth-fixed frame (x_r) and the y direction position in earth-fixed frame (y_r) together with its control inputs are x direction velocity in earth-fixed frame (U_r) and y direction velocity in earth-fixed frame (V_r) which are the reference input No. 2 and 3 of the first outer loop design. The objective rising time of these two loops is around 5 seconds, which is close to the one of the velocity loop. The design procedure can be summarized as follows: 3.3 The Trajectory Outer Loop Design 53 Step Response from: Input Poini(l) from: input Points) From: input PointfJ) From: Input Pointf-t) 5 10 0 Time (sec) Figure 3.19: Output Step Disturbance Responses of The Velocity Outer Loop Figure 3.20: Simulation Of The Nonlinear Model of The Velocity Outer Loop 3.3 The Trajectory Outer Loop Design 54 yaw angle_r Wi x1 = Ax+Bu y= Cx+Du Trajectory Degree to Radian yam angle_r yaw angle Ax+Bu y = Cx+Du K2 Draganflyerlll, Innerloop and Outerloop 4 Degree yawj_angle Figure 3.21: The Trajectory Loop of The Two DOF Flight Controller 1. Obtain The Plant Model: The nonlinear SIMULINK model diagram of the first outer loop model in Figure iT 3.22 was linearized around u = 0 0 0 0. equilibrium point. Then, the loop Number 2 and 3 of this linear model were selected for the second outer loop design. z_r CO\u2014\"' p . U r > K\"i?^>-f> V_r Wi1 CH)\u2014^ yaw angle_r x? = Ax+Bu y = C>c*-Dt Ax+Bu y = Cx+Du W r THETA.r U V PHI r y a w a n f l l e R r mner Loop and Dragan flyer III nonlinear model X1 = Ax+Bu y- Cx+Du K2 Terminator Terminatorl - K J D yaw angle -+CO y Figure 3.22: Nonlinear Model For The Trajectory Loop Design 2. Loop Shaping: The singular values of the unshaped linear model, which are shown Figure 3.23, were nicely shaped due to the very good tuning of the inner loop and the velocity loops. The crossover frequency was set to around 0.43 rad\/s. Thus no weights were required for this loop. 3.3 The Trajectory Outer Loop Design 55 3. Robust Stabilization: The reference closed-loop transfer function Tref was chosen as Tref - diag s 2+2*l*s+l s 2+2*l*s+l The i\/oo optimization gave e = 0.2857 which guarantees the robustness and stability of the trajectory loop controller. 4. Implementation: The Simulink Diagram in Figure 3.21 was implemented to check the performance of the achieved controller. The final controller, K^, was first reduced to 20 states, 4 inputs and 2 outputs to accelerate the simulation. Then it was split to K\\ and K2, which each has 2 inputs and 2 outputs. Wi \u2014 diag static steady gain for each loop. 0.735 0.735 contains the Shaped Plant Open Loop Singular Values Frequency (rad\/sec) Figure 3.23: Open Loop Singular Values of The Trajectory Loop Design 3.3 The Trajectory Outer Loop Design 56 3.3.2 S imu l a t i on Resu l t s The closed-loop output sensitivity function and complementary sensitivity function T of the second outer loop are shown in Figure 3.24 and 3.25 respectively. The peak of the sensitivity function S is 3.67 dB at the frequency of 0.838 rad\/s while the peak of the complementary sensitivity function T is close to 0 dB. Such values imply good disturbance and noise rejection abilities of the second outer loop controller. Figure 3.26 checks the step responses of these two loops with all the controllers in place. The rising time is around 5 seconds with zero steady state error. The output step disturbance was also tested in Figure 3.27. The controlled outputs return to the set points within 6 seconds. Sinflufcr Values 10, 1 111 \u2022 ' ! \u2022 ! ' \u2022 ! ! ' ' ' -60 L- ; i i-.j..Li.i.ix... ; ; ..........Li...! ; ; LJ-Liiiii i i i-i^iiii i .-..i..i..;..u.;^ 10\"'' 10'' 10 ' io'; 10 10'' Frequency (rad\/sec) Figure 3.24: Closed-loop Sensitivity Function of The Trajectory Loop To check the performance of the 2 DOF i\/oo controller, a nonlinear trajectory simula-tion was implemented as in Figure 3.28. The reference values are the x direction position, y direction position, z direction position in earth-fixed frame and yaw angle. The initial position of Dragonfiyer III was at (0,0,0) in the earth-fixed frame with 0 degree yaw angle. 3.3 The Trajectory Outer Loop Design 57 Singular Values Frequency (rad\/sec) Figure 3.25: Closed-loop Complementary Sensitivity Function of The Trajectory Loop From: Input Point(1 J From: Input Pc!f\u00ab(2) Figure 3.26: Step Responses With HQQ Controller of The Second Outer Loop 3.3 The Trajectory Outer Loop Design 58 Figure 3.27: Output Step Disturbance Responses of The Second Outer Loop Figure 3.28: Trajectory Simulation of The Nonlinear Model With 2 DOF H^ Controller 3.3 The Trajectory Outer Loop Design 59 Half Circle Scenario With Combined MBPC\/H inf Controller 8 4~-a N 2 J , -Figure 3.29: Half Circle Scenario With 2 DOF Controller Trajectory Scenario With The Combined MBPC\/ H inf Controller Figure 3.30: Trajectory Scenario With 2 DOF Controller 3.4 A Combined MPC\/H^ U A V Flight Controller 60 First, the Dragonfiyer III was asked to fly to (8,6,3) meters with 0 degree yaw angle. At 15 seconds, the UAV flew to the next destination at (11,12,11) meters. At 30 seconds, the UAV changed its yaw angle to 30 degrees while keeping this hover position unchanged. Two trajectory scenarios were simulated in Figure 3.29 and Figure 3.30. Figure 3.29 shows the half circle trajectory. After vertically taking off from (0,0,0) to (0,0,10) meters , the Dragonfiyer III flew to (-20,0,10) meters through a half circle trajectory. Then, the UAV vertically landed. Figure 3.30 animates the other trajectory scenario. After vertically taking off from (0,0,0) to (0,0,10) meters, the Dragonfiyer III flew to (-9,0,10) meters. Then, the UAV flew to the next two destinations at (-9,-8,10) and (-4, -8,10) meters continuously. Finally, the Dragonfiyer III flew back to the original point at (0,0,0). 3 . 4 A Combined MPC\/fZoo U A V Flight Controller 3.4.1 Motivation A modern auto pilot for an UAV consists of a stability augmentation system (SAS) and a guidance system (GS). In the previous section #00 loop shaping flight controller design was employed as the SAS for short term control providing robust stabilization, disturbance rejection, noise rejection, system decoupling and setting the bandwidth of the closed loop including the inner loop and the velocity loop. The UAV fast dynamic system was perfectly controlled by the flight controller. For the UAV which makes the subject of our work, the GS was ensured by an outer loop, a trajectory outer loop, also designed based on a 2 DOF robust controller. For small manoeuvres not causing the saturation of the actuators, the HQO flight controller can guarantee good performance. However, if the manoeuvre is large enough to exceed the constraints of the actuators, the guidance system has the potential of losing control if no adequate anti-windup is employed. To ensure good performance of the GS for large manoeuvres, an MPC was implemented for the longitudinal and lateral outer loops, the trajectory loop, acting as a GS and 3.4 A Combined MFC\/H^ U A V Flight Controller 61 overall supervisor that accounts actuator and flight envelope constraints. This decision was motivated by the assessment provided in (27) of a similar controller structure which revealed significant benefits when the MPC is applied for higher supervisory level. In (17), comparisons of numerous advanced adaptive flight control techniques through a variety of multiple axis manoeuvres in normal operation, while the aircraft is subjected to failures or damage, placed also the MPC on a winning position. Beyond direct UAV control, much attention (32) was generated by the use of MPC to the trajectory generation of the follower spacecrafts as a function of the current position and attitude of the leader, all in the context of actuator constraints. The reasons in this project for introducing the MPC architecture for the trajectory loop are: \u2022 An optimal ability to account for actuator constraints and ensure stability when the UAV operates close to them. \u2022 Opportunity to operate actuators at their limits while rejecting disturbances without destabilizing the closed loop. \u2022 Flight operation closer to flight envelope constraints is safely permitted. \u2022 Drawbacks associated with fixed gain controllers such as time invariance of prefilters and lack of trajectory prediction are addressed. \u2022 The UAV multivariable control problem is handled naturally. \u2022 The combined MPC\/2 DOF architecture integrates perfectly the robustness of the Hoo controller for SAS and the constraint handling features of MPC at the GS level. The initial bottleneck of the MPC implementation was the computational load required to solve the optimization problem online especially when the sampling rate was fast and 3.4 A Combined M P C \/ 7 ^ U A V Flight Controller 62 the number of inputs and outputs increased. However, this problem has been removed with the advent of faster computers. 3.4.2 Design Procedure A n d Results While solving the optimization of the cost function, we can not measure the full state vector. Thus, a state observer, shown in Figure 3.31, was used to estimate the state vector. With the 'constant output disturbance' assumption, the DMC type observer, the default observer scheme of MPC, can be used. In our design and simulation, this scheme was selected due to the assumption of constant wind disturbance. Figure 3.31: A State Observer The overall architecture of the MPC\/2 DOF controller is shown in Figure 3.32. The internal model for the longitudinal and lateral speed channels for the MPC design was produced by firstly linearizing the nonlinear model that includes the Dragonflyer III, the inner and outer loops with the 2 DOF controllers, as presented in Figure 3.22. Due 3.4 A Combined MPC\/H^ U A V Flight Controller 63 to the large dimension of the internal model, model reduction was first applied to contain the computational burden via truncating irrelevant states. The originallinear model with 98 states was reduced to 10 states with nearly zero errors. The reduced internal model was sampled at Ts = 0.5 second. The gain of state observer in Figure 3.31, L, was set \" l ol to L\u00b0 1 algorithm for DMC observer scheme. The observer architecture is coded in the MPC Trajectory + l b , M B P C ^ + w MBPC Controller Ik X w W w y W Nonlinear Draganflyerlll, Innerloop and Outerloop Figure 3.32: MPC In Longitudinal and Lateral Channel For the MPC controller less robustness versus its inner and outer loop E^ counterparts is allowed since the uncertainty in the inner and outer loops is smaller than that of the open loop UAV model. This combined architecture operates as a multi-rate system with a high sampling rate, 100 Hz, for the E^ controllers and a 2 Hz sampling rate for the MPC controller. The low sampling rate for the MPC controller is also reducing the computational burden and provides an optimized reference to the inner closed loop without interfering with its stabilization and velocity loop achieved by the 2 DOF Eva controller. During the controller parameter tuning, an unconstraint MPC controller was first implemented to find out the right values of prediction horizon, control horizon, the output weights and input weights. Note that since the throttle range of the UAV is limited and normally does not cause saturation of the actuators, only the longitudinal and the lateral channels were considered. The prediction horizons were all set to 10 and the control 3.4 A Combined MPC\/H^ U A V Flight Controller 64 Limits Constrained Variable Name Unit - 4 5 ^ .< 9 < 4 5 ^ pitch angle saturation rad roll angle saturation rad. - 2 0 ^ < q < 2 0 ^ pitch rate saturation rad\/s - 2 0 ^ < r < 2 0 ^ roll rate saturation rad\/s -2 < U < 2 velocity limits of x direction m\/s -2 < F < 2 velocity limits of y direction m\/s -1 < At\/ < 1 velocity increment limits of x direction m\/s -1 < AV < 1 velocity increment limits of y direction m\/s Table 3.1: Longitudinal and Lateral Channel Constraints horizons were 1 to ensure a fast response. The output weight was Q \u2014 diag p 5J and the input weight was set to identity R = I. The MPC had associated the constraints shown in Table 3.1 for the longitudinal and lateral channels. Figure 3.33 shows the nonlinear simulation for large manoeuvres in the longitudinal and lateral channel using the combined M P C \/ F ^ controller architecture. The reference value in the longitudinal channel was 60 meters and the reference value in the lateral channel was -40 meters. The U and V saturated at the beginning and during the flight and returned to zero smoothly near the destination since the MPC controller optimized the input values taking account of all actuator constraints. Figure 3.34 shows the closed loop sensitivity function with the constrained MPC con-troller in place, in the unconstrained case. The peak of the sensitivity function is around 0.7 dB at the frequency of 0.754 rad\/s. This shows a good disturbance rejection ability and good frequency bandwidth of the combined scheme. Figure 3.35 gives the closed loop complementary sensitivity function with the MPC controller, in the unconstrained case, and is showing that this combined scheme guarantees good noise rejection. The Matlab code of the above implementation is listed in Appendix B. 3.4 A Combined MPC\/H^ U A V Flight Controller 65 x and y position with MBPC controller 1 >ositton 1 ^ * * \u2014 \u2014 y posit i i on i i i , )! i i [ \u00ab I 1 1 1 0 5 10 15 20 25 30 35 40 45 50 Seconds x direction velocity U and y direction velocity V I j j f \u2014 u A I \/ 0 5 10 15 20 25 30 35 40 45 50 Seconds Figure 3.33: Longitudinal and Lateral Channel Step Responses in The Combined MPC And #00 Controller Sensitivity Fuction of the Combined MBPC\/H inf controller 10' t: : : : : , : ! : : ; :5 : : ! : i : i: ! \u2022 i , ' . \u2022\u2022 i i 10\"3 10~2 10\"' 10\u00b0 10' Frequency (radians\/time) Figure 3.34: Sensitivity Function Of The Combined M P C \/ # o o Controller (The Uncon-strained Case) 3.4 A Combined M P C \/ J J Q Q U A V Flight Controller 66 Complementary Sensitivity of the Combined MBPC\/H inf controller Frequency (radiansAime) Frequency (radians\/time) Figure 3.35: Complementary Sensitivity Of The Combined MPC \/tfoo Controller (The Unconstrained Case) Chapter 4 Formation Control In recent years, there is a growing interest in employing formations of UAVs for various missions. Close formation flying of UAVs allows more aggressive maneuvers, a significant fuel and weight savings, lower costs, extra capacities and abilities over single UAV. It also initiates the potential for superior cooperations. This chapter presents the extension of the project to the formation control of 3 iden-tical quad rotor UAVs of the type presented in Chapter 2 and controlled in Chapter 3. To achieve formation flying with minimal human intervention, the UAVs require highly autonomous capacities at different levels such as: the Path Planner, the Trajectory Gen-erator and the Formation Controller. As a result a formation control structure with 3 UAVs, as in Figure 4.1 and implemented in Figure 4.10, is proposed. The first formation control layer, the Path Planner from Figure 4.1, will be introduced in Section 4.1. This planner constructs a path that represents a compromise between the cost or risk involved in bypassing obstacles and the fuel expense related to the path length. Other mission related issues can be also implemented at this level. The second layer explained in Section 4.2 is the Trajectory Generator. Due to dynam-ical constraints and physical limits on control actuators, not all trajectories connecting two waypoints derived from the Path Planner can be followed by the formation of UAVs. Therefore, the Trajectory Generator is used to generate a feasible trajectory for the UAVs 67 68 Path Planner Trajectory Generator UAV Leader Formation Controller #1 I Formation Controller #2 UAV Follower #1 UAV Follower #2 Figure 4.1: Formation Control Scheme For Multiple UAVs computing the corresponding control inputs for the formation leader while accounting for its dynamics. The third layer studied in Section 4.3 is the Formation Controller, multiplied in an identical structure for the individual UAVs. The purpose of this layer is to maintain the formation configuration during mission. The formation configuration is defined as certain desired coordinates (xf,yf) of the UAV followers in the leader's body frame. Thus the Formation Control Problem can be formulated as the design of a Formation Hold Autopilot (FHA) that generates the control inputs of each follower such that the relative coordinates of the leader with respect to the follower's body frame converge to the desired values asymptotically. This should happen even in the face of maneuvers by the lead vehicle, which generally are treated as disturbances. Some initial details of the formation control algorithm adopted in this work can be found in (16). For each follower, the problem of how to avoid collision during formation initialization was addressed in (28) (29) and also applied in the proposed design. 4.1 The Formation Control Path Planner 69 4.1 The Formation Control Path Planner The Path Planner is responsible for generating a path that avoids static obstacles and potential threats and reaches all targets in an optimal order. We adopt the approach proposed in (19). Under this method, an initial path from the source position of the UAV leader to the destination was generated based on the locations of static threats using a Voronoi diagram (6). This diagram has the property that for each site every point in the region around that site is closer to that site than to any of the other sites. The Dijkstra's Algorithm (6) providing a solution to this problem will be first introduced. 4.1.1 The Dijkstra's Algorithm To find the optimal way to be generated by the Path Planner Level, the Dijkstra's Algo-rithm, a systematic method of searching the shortest path from a source to a destination based'on the cost of each edge, is employed. In this subsection, the algorithm is introduced through an example and then its coding procedure is given. Figure 4.2: Dijkstra's Algorithm Example In Figure 4.2, the number on each edge represents the cost related to distance, time or risk. For easy understanding, we make a table as shown in Table 4.1 with a row for each node on the map. Each column represents an iteration of the algorithm and contains the distance to the source during that iteration. An asterisk (*) gives the distance when it is 4.1 The Formation Control Path Planner 70 A 6* 0* C 6* 6* E 8 8* 8* D Unknown 11 11* 11* B Unknown Unknown 20 15* 15* Table 4.1: Table Solution For The Example the shortest. The Dijkstra's algorithm can be explained using the Example in Figure 4.2 and the Solution in Table 4.1: 1. From the source, check the cost or distance of each edge to all adjacent nodes. Write these possible distances in the appropriate box in the table. If a node has not been reached, it is labelled as 'Unknown'. In the example, the source is A and adjacent nodes are C and E. 2. An asterisk * is placed on the lowest number in the column. A node with a star in its row is called 'settled' and the shortest distance from the source to the starred node is the starred value. The source itself is settled at a zero distance. In this Example, we settle C at a distance of 6 because any other route to C which goes through E is farther than the distance of 6. 3. The next column is filled out by finding all nodes adjacent to the settled nodes in the current column. In the Example, in the first column, A and C are settled and thus their adjacent nodes are D which is adjacent to C and E which is adjacent to A. The distance form these nodes to the source through the settled nodes to which they are adjacent is calculated and the values are recorded in the appropriate box in Table 4.1. In the Example, the distance from A to C is 6 and from C to D is 5, so the value 11 appears in the second column of Node D. If an unsettled node is adjacent to more than one settled node, the shortest distance is recorded in the 4.1 The Formation Control Path Planner 71 Table. 4. The Step 3 and 4 are repeated until all nodes have been settled. In the completed Table 4.1, node B has a value of 20 in the third column. However, since the value of 15 was found after the value of 20, it is changed to the shortest distance, 15, in the next box. From Table 4.1, the shortest distance from starting point A to destination B is 15 and the route is A-C-D-B. The Djikstra's algorithm, this algorithm can be considered as a spanning tree problem. The graph representing all the paths from one node to all the others must be a spanning tree which must include all nodes. There also should be no cycles since a cycle would define more than one path from the selected node to at least one other node. We define the graph, G, as G = (V, E) where V is a set of nodes and E is a set of edges. The Dijkstra's algorithm adopts two sets of nodes and two data arrays as follows: S the set of nodes whose shortest distances from the source have already been determined V-S the set of remaining nodes d an array of best estimates of shortest path to each node Pi an array of predecessors for each node In the array of predecessors, pi, each node's entry contains the index of its predeces-sor in a path through the graph. For example, in Figure 5.2, the predecessor list of the shortest path A-C-D-B is : 4.1 The Formation Control Path Planner 72 Vertex Predecessor A A C A D C B D Note that if a node, such as A, has no predecessor, it can point to itself or be set to -1. The main operation algorithm is: 1. Initialize d and pi. The initialization procedure proceeds as follows: This sets up the graph so that each node has no predecessor (pi[v] = nil) and the estimates of the cost or distance of each node from the source (d[v]) are infinite, except for the source node itself (d[s] = 0). Note that the edges in the predecessor sub-graph which is the list of predecessors of each node are (pi[v],v). 2. Set S to empty. 3. While there are still vertices in V-S. (a) Sort the vertices in V-S according to the current best estimate of their distance from the source. (b) Add u, the closest vertex in V-S, to S. (c) Relax all the vertices still in V-S connected to u. The relaxation procedure checks whether the current best estimate of the shortest distance to v (d[v]) can be improved by going through u (i.e. by making u the predecessor of v): The Matlab m-file of Dijkstra's algorithm is written and listed in Appendix C. l 4.1 The Formation Control Path Planner 73 4.1.2 The Path Planner Design In the approach (19) to generate an optimal path, a Voronoi diagram is firstly formed from the static threat locations. Figure 4.3 illustrates the Voronoi diagram for a set of threats represented by circles, and targets, given as triangles. The number of each triangle represents the sequence of the targets located by the UAVs and No. 1 is the initial UAV leader location. Note that the Voronoi diagram is created without considering the initial UAV location and all target locations. Figure 4.3: Voronoi Diagram for Threat Locations To generate the initial path, the UAV starting location is connected to the closest node of the Voronoi diagram. Then, the path can be formed by connecting the leader UAV to the targets sequentially through the edges of the Voronoi diagram. The problem comes to choose the optimal path from the numerous paths to the targets in the Voronoi diagram. Thus a cost for travelling along an edge, Ji, given below, is assigned to each edge i of the Voronoi diagram. 4.1 The Formation Control Path Planner 74 Figure 4.4: Initial Path from The Path Planner Jt = kJrti + (1 - k)Jf>i 0<k<l The cost, Ji, consists of the threat cost J r,j regarding to the risk incurred and the fuel cost Jf:i due to the fuel consumption. The threat cost depends on the proximity of the edge to the threats while the fuel cost is proportional to the length of the edge. The total cost, Ji, is a weighted sum of the threat cost and the fuel cost. The weight k can be adjusted from 0 to 1 to balance the two costs under the practical environment. After the cost of each edge of Voronoi diagram is calculated, we can employ the Di-jkstra's algorithm (6), explained detailedly in Section 4.1.1, to find the optimal path by searching the shortest path. Direction must be assigned to the nodes and targets based on the arrival sequence of the targets before the algorithm is used. Figure 4.4 shows an opti-mal path generated by this algorithm through the path searching of the Voronoi diagram in Figure 4.3. 4.2 The Trajectory Generator 75 4.2 The Trajectory Generator Due to the constraints and dynamics of the leader UAV, the initial optimal path found by the Path Planner cannot be directly sent as references to the leader. This path usually is delineated as x, y, z in the inertial frame and the yaw angle. A Trajectory Generator is therefore implemented to achieve a smooth trajectory by generating trackable reference inputs to the leader UAV. Generally, the trajectory generator problem can be regarded as the tracking of a set of reference step inputs of the leader UAV that must satisfy actuator constraints and generate a smooth path. In our controller design, the combined MPC \/#oo controller has already taken into account the actuator constraints and limits. Thus the objective of our trajectory generator for the leader UAV focuses on providing a smooth path only. source Figure 4.5: Path Formed by The Trajectory Generator Figure 4.5 illustrates the path generated by the Trajectory Generator. Except for the source and destination, each node has a designated spherical zone with a radius, R, and a center at each node. The generated trajectory has a smooth curve within this spherical zone since once in it the controller looks ahead to the next way point. This enables the UAV to avoid shape turns, generating high accelerations, and save fuel. After the initial optimal path has been generated, the 3D positions of all nodes from 4.2 The Trajectory Generator 76 the start to the end are delineated to a waypoint data matrix as follows: XA VA ZA yaw angle^ RA XB VB ZB yaw angles RB xc yc ZC yaw anglec Rc XD VD ZD yaw angler. RD where each row sequentially defines the 3D position of each node and expected yaw angle of the UAV during this trajectory section, followed by the adequate radius R around that node. yaw_angle yaw_angle_r trajectory generator yaw_angle_r yaw_angle leader quad-rotot UAV closed-loop dynamics Figure 4.6: Trajectory Generator and Quod Rotor UAV Architecture Figure 4.6 shows the trajectory generator and the closed loop leader UAV with the combined M P C \/ flight controller. The trajectory generator algorithm initially loads the initial values, which are the first row of the data matrix, and sets them as reference inputs for the closed loop UAV. Then at each sampling time, the vector modulus D defined as: D = \\J (xr \u2014 x)2 + (yr \u2014 y)2 + [zT \u2014 z)2 + W(yaw-angler \u2014 yaw.angle)2 is calculated. In the above calculation, W is the weight in the yaw angle channel. If the modulus D is smaller than R, the values of the next waypoint, which are stored in the next row, are loaded from the data matrix and set as the reference points for the UAV. 4.2 The Trajectory Generator 77 This procedure is iterated until the destination is reached. Figure 4.7 presents the simulation results of the Trajectory Generator implemented in Figure 4.6. The dashed line represents the reference values which are pre-defined in the data matrix and the solid line shows the exact trajectory of the UAV. The 'o's highlight all way points. The UAV first started at (0,0,0) and flew to (0,0,10). Arriving at 4.7 seconds, it flew to the next waypoint at (10,12,10). At around 10 seconds, it reached the waypoint and the Trajectory Generator allocated a new waypoint at (5,5,-6) to the UAV. Reaching the new waypoint with zero yaw angle at 14.5 seconds, the UAV changed it's yaw angle to 20 degree and back to 0 degree at 19.9 seconds and 25.4 seconds respectively. The UAV turned back to the starting point at 29.8 seconds. Figure 5.8 recorded the 3D trajectory of this simulation. The UAV turned smoothly to the next waypoint at waypoint A and B. Note that at waypoint C, the UAVs yaw angle was changed to 20 degree and back to 0 degree with fixed position. These simulation results show how well the Trajectory Generator satisfies its objectives of generating a smooth path automatically given predefined waypoints. The Trajectory Generator code written in Matlab is presented in Appendix C.2. 4.2 The Trajectory Generator 78 z (dashed), z (solid) and waypoint (o) 0 5 10 15 20 25 30 Figure 4.7: Simulation Results ff Trajectory Generator and Closed Loop UAV 4.2 The Trajectory Generator 79 4.3 Formation Controller 80 4 . 3 Formation Controller It is well known that UAVs flying in certain formation significantly reduce the fuel cost (20) (? ) (22) due to reduced drag as result of reduced vortex initiation. Civilian and military operations involving several UAVs also require specific formation of UAVs depending on the mission task. As an example, the purpose of this layer is to find formation controllers to maintain 3 UAVs in an equilateral triangle formation. X inertial axis Figure 4.9: 3 UAV Equilateral Triangle Formation in Inertial Frame Since the leader UAV follows the smooth path generated by the Trajectory Generator, two formation controllers are required so that the coordinations of two follower UAVs, in the leader UAVs frame, are fixed at two vertexes of the equilateral triangle as shown in Figure 4.9. In this figure, 'A' presents the leader UAV and 'B ' and ' C are two follower UAVs. The rotation matrix, R'^, of XA-VA plane around OA is as : | cos tp \u2014 sin tp I sin ip cos tp Thus the coordinations of two follower UAVs, (XBI,VBV) a n d (xci,Vci), i n the inertial frame can be derived as: 4.3 Formation Controller 81 XBI = R4, X B A + X A VB1 yBA - y A . X C 1 = R-4> XCA + X A yci ycA VA where XBA 2 i \/ and XCA 2 n yBA L 2 ycA L 2 L is the lateral length. The z positions and yaw angles of the two followers are identical to the leader. There-fore the overall relationship, which the formation controller implements, is as follows: XBl = Rip xA L + VBl 2 yA_ (4.3.1) ZBl = ^(A) yaw-angleBi = yaw.angle A and XCl xA L + yci ~ 2 })A_ (4.3.2) zci = Z(A) yawjanglecx \u2014 yaw.angleA Figure 4.10 represents the overall formation control architecture. Also an animation block to accommodate this structure has been defined. The 'Quad-rotor UAV closed-loop dynamics' block contains the leader Quad-rotor with the combined M P C \/ flight controller. The 'Quad-rotor UAV closed-loop dynamicsl' and 'Quad-rotor UAV closed-loop dynamics2' blocks represent the two follower Quad-rotor with their corresponding 4.3 Formation Controller 82 yaw_angle yaw_angle_r waypoint generator x1_r yl-r z1_r y_r yaw_angle1_r x2_r y2_r z2_r yaw_angle_r yaw_angle2_r formation controller y-f yaw_angle_r PHI THETA| yaw_angle Quad-rotot UAV closed-loop dynamics X x_r y y_r z z_r PHI THETA yaw_angle_r yaw_angle Quad-rotot UAV closed-loop dynamicsl X x_r y y_r z z_r PHI THETA yaw_angle_r yaw_angle START UP Double click to load initial parameters before each simulation Quad-rotot UAV closed-loop dynamics2 Figure 4.10: The Overall Formation Control Architecture 4.3 Formation Controller 83 combined M P C \/ #oo controllers. The 'waypoint generator' block runs the Trajectory Generator algorithm to generate a smooth path to the leader UAV. The 'formation con-troller' block using the Equation 4.3.1 and 4.3.2 calculates the reference values of two followers based on the reference values of leader UAV. The 'formation animation' block animates the equilateral triangle formation flying of 3 UAVs in real time. Note that before running the animation, the 'START UP' button has to be activated to load initial param-eters. The animation result of the equilateral triangle formation flying is shown in Figure 4.11, while the trajectory of the leader UAV is given in Figure 4.7 and Figure 4.8. The animation data of follower B and follower C of the equilateral triangle formation flying are shown in Figure 4,12 and Figure 4.13, respectively. The followers keep the formation during flying to any position and yaw angle changing of the leader UAV. The Formation Controller M-file is shown is Appendix C.3 Figure 4.11: Animation of Equilateral Triangle Formation Flying 4.3 Formation Controller 84 Figure 4.12: Formation Flying Results of Follower B UAV 4.3 Formation Controller 85 Figure 4.13: Formation Flying Results of Follower C UAV Chapter 5 Conclusion and Future Work In this thesis, we have presented a flight and formation control design for an affordable quad-rotor UAV! In Chapter 1, the introduction and background of UAVs was surveyed for a compre-hensive knowledge to readers. The onboard sensors, such as accelerometers, gyros, IMU, GPS and imaging sensors were introduced. Two advanced controller design method, 2 DOF loop shaping controller design and Model Based Predictive Controller design, were proposed. In Chapter 2, a high fidelity nonlinear UAV model with 12 states was obtained and represented in SIMULINK for design purpose. A quasi-LPV model was generated for robust control design. A testing flying mill was designed and built for the flight testing and parameter identification. A microprocessor, PIC16F877, was programmed in assembly language to transfer the control data from a personal computer to a pulse width modulated (PWM) signal to employ the Futaba radio transmitter to communicate with the UAV. In Chapter 3, a 2 DOF Hoo loop shaping controller was designed for robust stabilization and trajectory control based on the nonlinear model. In this control architecture, the inner loop provides the roll and pitch angles, the yaw rate and vertical speed stabilization control. The first outer loop ensures the longitudinal and lateral speed, throttle and yaw angle control and the last outer loop guarantees the longitudinal and lateral position 86 87 control. To allow constraint handling ability of the longitudinal and lateral channel in large manoeuvres, a constraint Model Predictive Control (MPC) was implemented in the second outer loop for trajectory control. Nonlinear simulations have shown this combined MPC\/ 2 DOF architecture yields satisfactory performance in the presence of disturbances and various input and \/or output constraints. Chapter 4 presented an extension of this project to the formation control of 3 identical Quad-rotor UAVs that have the previously designed flight controller in place. To achieve autonomous flying in an equilateral triangle formation, three layers, which are Path Plan-, ner, Trajectory Generator and Formation Controller, were designed and implemented in Matlab and Simulink. For comparison to previous UAV works, a quad-rotor helicopter was first chosen as the UAV dynamics for control. With the motion equations of this UAV derived and its SIMULINK model implemented, we are the first ones that use 2 DOF Hoo and combined MPC\/i \/oo flight controller to control such high nonlinear system. Then we proposed a new implementation of three layers of formation flight control. A self designed experimental setup was also achieved for identification and control testing purpose. This work is the starting point for a further larger scale UAV project. Several sugges-tions can be made to further improve in future work. 1. The Quasi-LPV model of the Quad-rotor UAV, derived in Section 2.1.2, can be used with the flight controller. This high fidelity Quasi-LPV model captures the nonlinearities of the UAV and would improve the performance of the controller. Quasi-LPV synthesis yields a gain-scheduled controller, which is very similar to the i\/oo controller or combined MBPC\/i^oo controller achieved in this thesis. 2. Fault Detection and Isolation (FDI) filters can be designed to detect actuator faults and sensor failure. Typically FDI filter design approach is elaborated for open loop 88 model and is applied in the closed loop to improve the robustness and reliability of the system when faults occur. 3. Completely implement the flight controller and combined MBPC \/^oo controller with Dragonflyer III. The Inertial Measurement Units (IMU) including 3-axle gyros, 3-axle accelerometers and the differential GPS installed on board should be interfaced with a personal, computer in a wireless manner. To achieve increased accuracy, Kalman adaptive filtering could be employed. 4. For the formation control, further research can be conducted on 3D or different flying formation control. More different flying tasks, such as formation initialization, formation contraction, formation expansion and coordination turn, can be simulated and animated based on the formation control methods employed in this thesis. Appendix A M-file of the Two DOF Loop Shaping Flight Controller Design '\/. Quad Rotor UAV 2-DOF H-Infinity Analysis and Synthesis % Designed by Ming Chen % Date: April 2003 function uav clear % Section One: Calcuate the H-inf controller for the inner loop \u00b0\/0 Step 1: Linearize the plant and analysis % the singular values of the unshaped model [x,u] = trim('uavmodel_reduct'); [A,B,C,D] = linmodC'uavmodel_reduct',x,u); sys = ss(A,B,C,D); sys_tf = tf(sys); G = pck(A,B,C,D); sigma(A,B,C,D,{0.01,1000}) title('Unshaped Plant Open Loop Singular Values') grid pause; 7, Step 2: Choose weights and Tref and analysis the singular % values of the shaped plant system matrix Gs wll = nd2sys([1.52 0.965], [1 0]); wl2 = nd2sys(13,l); wl3 = nd2sys(13,1); wl4 = nd2sys(l*[17.2 9.7] ,[10]); WI = daug(wll,wl2,wl3,wl4); w21 = nd2sys([90], [1 35]); 89 90 w22 = nd2sys(73*[l 2.31],[1 35]); w23 = nd2sys(73*[l 2.31],[1 35]); w24=nd2sys( [90],[1 35]); W2 = daug(w21,w22,w23,w24); [Awl,Bwl,Cwl,Dwl] =unpck(Wl); [Aw2,Bw2,Cw2,Dw2] = unpck(W2); Tref_model = nd2sys(2~2, [1 2*1*1 1\"2]); Tref_modell = nd2sys(3~2,[1 2*0.95*3 3~2]); Tref_model2 = nd2sys(6~2,[1 2*0.95*3 3\"2]); Tref = daug(Tref.model,Tref_modell,Tref_modell,Tref_model2); Gs = mmult(W2,G,Wl); [As,Bs,Cs,Ds] = unpck(Gs); sigma (As, Bs, Cs, Ds, -[0.01,1000}) title('Shaped Plant Open Loop Singular Values') grid pause; % Step 3: Synthesize 2-DOF controller K and get gamma= 2.8312 K = hinf2dof(Gs, Tref); 1 Step 4: Seperate K to Kl and K2 minfo(K) % system: 20 states 4 outputs 8; inputs Kl = sel(K,[l 2 3 4] , [1 2 3 4]) ; K2 = sel(K, [1 2 3 4], [5 6 7 8]); [Akl,Bkl,Ckl,Dkl] = unpck(Kl); [Ak2,Bk2,Ck2,Dk2] = unpck(K2); 7o Section Two: Calcuate the H_inf controller for the f i r s t outer loop 7o Step 1: Linearize the plant and analysis 7o the singular values of the unshaped model [x,u] = trim('uavmodel_innerloop'); [Aoi,Boi,Coi,Doi] = linmod('uavmodel_innerloop',x,u); syso = ss(Aoi,Boi,Coi,Doi); sys_tfo = tf(syso); Go = pck(Aoi,Boi,Coi,Doi); sigma(Aoi,Boi,Coi,Doi,{0.01,1000}) title('Unshaped Plant Open Loop Singular Values of the f i r s t outer loop') grid pause; 7o Step 2: Choose weight Wl(s) and Tref and analysis 7o the singular values of the shaped plant woll = nd2sys(0.5,1); wol2 = nd2sys(0.05,1); wol3 = nd2sys(0.05,1); wol4 = nd2sys(0.5,1); Wol = daug(woll,wol2,wol3,wol4); [Awol,Bwol,Cwol,Dwol] = unpck(Wol); Tref.modelol = nd2sys(2~2, [1 2*1*1.3 1.3~2]); Tref_modelo2 = nd2sys(3~2,[1 2*1*2.1 2~2]); Trefo = daug(Tref_modelol,Tref_modelol,Tref_modelol,Tref_modelo2); Gso = mmult(Go,Wol); [Aso,Bso,Cso,Dso] = unpck(Gso); sigma(Aso,Bso,Cso,Dso,{0.01,100}) title('Shaped Plant Open Loop Singular Values') grid pause; % Step 3: Synthesize 2-DOF controller Ko and get gamma=2.5964 Gs = Gso; Tref = Trefo; K = hinf2dof(Gs, Tref); Ko = K; % Step 4: Reduce Ko and seperate to Kol and Ko2 minfo(Ko); % system: 67 states 4 outputs 8 inputs % Reduce Ko to 20 states, 4 outputs and 8 inputs [Ko_cf,sigKo] = sncfbal(Ko); Ko_20 = cf2sys(hankmr(Ko_cf,sigKo,20,'d')); gapKo_20= nugap(Ko,Ko_20) % gap is closed.to zero. Kol = sel(Ko_20, [1 2 3 4], [1 2 3 4]); Ko2 = sel(Ko_20, [1 2 3 4], [5 6 7 8]); [Akol,Bkol,Ckol,Dkol] = unpck(Kol); [Ako2,Bko2,Cko2,Dko2] = unpck(Ko2); % Section Three: Outerloop for x y control % Step 1: Linearize the plant and analysis \u00b0\/0 the singular values of the unshaped model [x,u] = trim('uavmodel_outerloopno'); [A_4,B_4,C_4,D_4] = linmod('uavmodel_outerloopno',x,u); G_4 = pck(A_4,B_4,C_4,D_4); Gxy = sel(G_4,[2 3], [2 3]); [Axy,Bxy,Cxy,Dxy] = unpck(Gxy); 92 sigma(Axy,Bxy,Cxy,Dxy,{0.01,1000}) title('Unshaped Plant Open Loop Singular Values') grid pause; % Step 2: Choose Tref Tref.model = nd2sys(2~2,[1 2*1*1 1~2]); Tref = daug(Tref.model,Tref_model); Gsxy = Gxy; % Step 3: Synthesize 2-DOF controller Kxy and get gamma=3.502 Gs = Gsxy; K = hinf2dof(Gs, Tref); Kxy. = K; 7. Step 4: Reduce the Kxy and seperate to Kxyl and Kxy2 minfo(Kxy) % system: 102 states 2 outputs 4 inputs % Reduct Kxy model to 20-states, 2 outputs and 4 inputs [Kxy_cf,sigKxy] = sncfbal(Kxy); Kxy_20 = cf2sys(hankmr(Kxy_cf,sigKxy,20,'d')); gapKxy_20 = nugap(Kxy,Kxy_20) % gap i s closed to zero Kxyl = sel(Kxy_20, [1 2], [1 2]); Kxy2 = sel(Kxy_20, [1 2 ], [3 4]); [Akxyl,Bkxyl,Ckxyl,Dkxyl] = unpck(Kxyl); [Akxy2,Bkxy2,Ckxy2,Dkxy2] = unpck(Kxy2); %============================================================ '\/, Robust Controller Design 2D0F % synthesizes the H_inf 2-DOF controller % Uses MATLAB mu toolbox % Usage: K=hinf2dof(Gs, Tref); 1 INPUTS: Shaped plant Gs and reference model Tref \"\/, OUTPUT: Two degrees-of-freedom controller K 7, Coprime factorization of Gs %============================================================ function K=hinf2dof(Gs.Tref) [As,Bs,Cs,Ds] =unpck(Gs); [Ar,Br,Cr,Dr] = unpck(Tref); [nr,nr] = size(Ar); [lr.mr] = size(Dr); [ns,ns] = size(As); 93 [ l s .ms] = s i z e ( D s ) ; Rs = e y e ( l s ) + D s * D s . ' ; Ss = eye(ms)+Ds'*Ds; A l = (As - B s * i n v ( S s ) * D s ' * C s ) ; R l = C s ' * i n v ( R s ) * C s ; QI = B s * i n v ( S s ) * B s ' ; [ Z l , Z2 , f a i l , r e ig_min] = r i c _ s c h r ( [ A l ' - R l ; -QI - A l ] ) ; Zs = Z 2 \/ Z 1 ; rho = 1.0; A = d a u g ( A s , A r ) ; BI = [ze ros (ns .mr) ( ( B s * D s ' ) + ( Z s * C s ' ) ) * i n v ( s q r t ( R s ) ) ; B r z e r o s ( n r , I s ) ] ; B2 = [ B s ; z e r o s ( n r , m s ) ] ; C l = [zeros(ms,ns+nr) ;Cs z e r o s ( l s , n r ) ; r h o * C s - r h o * r h o * C r ] ; C2 = [ze ros (mr ,ns+nr ) ;Cs z e r o s ( l s . n r ) ] ; D l l = [ z e r o s ( m s , m r + l s ) ; z e r o s ( I s , m r ) s q r t ( R s ) ; - r h o * r h o * D r r h o * s q r t ( R s ) ] ; D12 = [eye(ms) ;Ds ; rho*Ds] ; D21 = [rho*eye(mr) z e r o s ( m r , l s ) ; z e r o s ( l s , m r ) s q r t ( R s ) ] ; D22 = [ ze ros (mr ,ms ) ;Ds ] ; B = [BI B2] ; C = [C1 ;C2] ; D = [ D l l D12;D21 D22]; P = p c k ( A , B , C , D ) ; [11,m2] = s i z e ( D 1 2 ) ; [12,ml] = s i z e ( D 2 1 ) ; nmeas = 12; neon = m2; gmin = 1 ; gmax = 10; g t o l = 0 . 0 1 ; [K, G n c l p , gam] = h i n f s y n ( P , nmeas, neon, gmin, gmax, g t o l ) ; [ A c , B c , C c , D c ] = unpck (K) ; % End Appendix B M-file of the Combined M B P C \/ H infinity Controller Design % Quad Rotor UAV Combined MBPC\/H-Infinity Analysis and Synthesis \u2022 % Designed by Ming Chen 7. Date: April 2003 function uav_mpbc clear % The model Gxy used as internal model is the linear model of the % Draganflyer III, the H-inf inner loop and the f i r s t H-inf outer % loop. The constraints of the input: -2<u<2; -Kdelta u<l; -2<v<2; % - K d e l t a v<l the nonlinear model of the Graganflyer III, the H-inf 7o inner loop and the f i r s t H-inf outer loop was used as process model 7o lineared state space model [Axy,Bxy,Cxy,Dxy]=unpck(Gxy); 7\u00bb Discretize the linear model and save in the mod form dt = 0.5; 7. Sampling time i s 0.5 second [PHI,GAM] = c2dmp(Axy,Bxy,dt); minfo = [dt,98,2,0,0,2,0]; imod = ss2mod(PHI,GAM,Cxy,Dxy,minfo); 7o Define controller parameters P = 10; % Prediction horizon M = 1; 7o Control horizon ywt = [5,5]; % Equal weighting of y(l) and y(2) uwt = [1,1]; % Equal weighting of u(l) and u(2) ulim = [ [-2 -2] [2 2] [1 1] ] ; 94 95 ylim = [ ]; % No constraints on y Kest = [ ]; % Use default estimator setpts = [60 -40]; % setpoint of x i s 60 m and setpoint of y i s -40 m tend = 60; % Simulation time i s 30 seconds [y,u,ym] = scmpcnK 'uavmodel_outerloopno_2', imod, ywt, uwt, M.P.tend,.. setpts,ulim,ylim,Kest, [1 1]); plotall(y,u,dt) grid % check the bode plot Ks = smpccon(imod,ywt,uwt,M,P); [clmod.cmod] = smpccKpmod,imod,Ks,Kest); freq = [-2,1,100]; [frsp,eyefrsp] = mod2frsp(clmod,freq); plotfrsp(eyefrsp, [1], [1]) % sensitivity grid plotfrsp(frsp, [1], [1]) % complementary sensitivity grid Appendix C IVI-files for Formation Control C . l M-file of Dijkstra's algorithm for Path Planner % Ming Chen % Quod-rotor UAV project % May 30,2003 % Solving the Shortest Path Problem using Dijkstra's Algorithm % - This function takes in a square n x n matrix A as input. The matrix % represents the weights of edges in a network of n nodes. A zero % indicates that nodes are not adjacent. The function returns % an (n-1) x 3 matrix, sp. The f i s t column of sp represents the vertice % number and the second column shows the shortest path from the source 7o to this node. The last column of sp presents the number of nodes that 7o are in the shortest path. 7\u00bb DIJKSTRA find the shortest paths from the starting node to each 7\u00bb other node in the network. 7. INPUT: % A = input matrix 7. OUTPUT: '\/, sp = shortest paths to each node from starting node function [sp] = dijkstra(A) [m,n] = size(A); resultl=zeros(n,n-l); result=zeros(l,n-l); y = [zeros(l,n-l); A(l,2:n); ones(l,n-l)]; % shortest path 96 C.2 M-file of Trajectory Generator 97 p = find(y(l,1:n-l)==0); % p = vector of non-terminated nodes it e r = 1; while \"isempty(p) % iterate when p i s not empty x = find(y(2,p)>0); % x=vector of indices referring to nonzero entries 7, Step 2 [a,K] = min(y(2,p(x))); 7o a = smallest nonzero entry 7. K = index of x referring to a o = p(x); J = o(K) +1; 7o J = index of A referring to a y ( l , J - l ) =1; '\/, changes termination bit to 1 p = p(find(p \"= J - l ) ) ; 7. update p % r e s u l t ( i t e r , i t e r l ) = J; i f ~isempty(p) z = f ind(A(J,p+l)>0) ; 7, z = vector of nodes adjacent to J r = p(z) + l7o r = indices (wrt A) of non-terminated adjacent nodes w = A(J,r) + y(2,J-l); % distance to a l l nodes via J tempi = y(2,r-l); temp2 = y(3,r-l); helperl = y(2,r-l) > w; \u00b0\/\u201e i f w i s less than previous 7\u00bbre suit (iter, i t e r 1) = r ; 7o shortest path y(3,r-l) = y(3,r-l) + (J - y(3,r-l)). *helperl; 7. update y y(2,r-l) = min(y(2,r-l), w); helper2 = tempi == 0; '\/, i f shortest path was zero . . . y(2,r-l) = y(2,r-l) + (w - y(2,r-l)).*helper2; 7. update y y(3,r-l) = y(3,r-l) + (J - y(3,r-l)).*helper2; 7o y stays unchanged i f w > y(2,r-l) end i t e r = i t e r + 1; end result=result1(1,:); sp = [(2:n)',y(2,:)',y(3,:)'] ; C.2 M-file of Trajectory Generator % Quad Rotor UAV Trajectory Generator 7o Designed by Ming Chen 7. Date: May 2003 C .2 M-file of Trajectory Generator 98 function y = generator(u) format short e 7\u201e define global variables global data index R uavworkspace time timer x_r y_r index =2; R = 0.5; % R i s the radius of sphere axound waypoint % inputs axe u = [x; y; z; yaw_angle], data, index x = u ( l ) ; y = u(2); z = u(3); yaw_angle = u(4); % output i s y = [x_r; y_r; z_r; yaw_angle_r] x_r = data(index,1); y_r = data(index,2); z_r = data(index,3); yaw_angle_r = data(index,4); % waypoint manager % the vector between the desired position and the current position % a l l these differences are to be reduced to zero by the \"waypoint % controller\" at the expense of speed variations Dx = x_r - x; Dy = y_r - y; \u2022 Dz = z_r - z; Dyaw_angle = yaw_angle_r-yaw_angle; % vector modulus is computed DS = sqrt(Dx\"2+Dy\"2+Dz\"2+ (10*Dyaw_angle)~2); % once in the desired radius around the setpoint the next waypoint \u00b0\/o w i l l be used as overall reference i f DS <= R index = index+1; x_r = data(index,1); y_r = data(index,2); z_r = data(index,3); yaw_angle_r = data(index,4); end % desired position time stamped is given to the inner loop or C .3 M-file of Formation Controller 99 % trajectory generator y = [x_r; y_r; z_r; yaw_angle_r]; C.3 M-file of Formation Controller % Quad Rotor UAV Formation Controller 7o Designed by Ming Chen 7. Date: May 2003 function y = formation(u) global data index R uavworkspace radius. distant = 8; 7o distant from leader to one follower d2r = 3.14\/180; 7o Degree to Radian conversion 7o inputs are u = [x; y; z; yaw_angle] x = u( l ) ; y = u(2); z = u(3); yaw_angle = u(4); 70 Rotation Matrix along z axis rotate=[cos(yaw_angle*d2r) -sin(yaw_angle*d2r) 0; sin(yaw_angle*d2r) cos(yaw_angle*d2r) 0;0 0 1]; % Follower 1 ' s coordination in the Leader frame xyl_leader=[-1.732\/2*distant; distant\/2;0]; 7o Follower l's coordination in the earth frame xyl = rotate * xyl_leader + [x;y;z]; xl = x y l ( l , l ) ; yl = x y l ( 2 ) l ) ; z l = xyl(3,l); yaw_anglel = yaw_angle; 7o Follower 2 's coordination in the Leader frame xy2_leader = [-1.732\/2*distant; -distant\/2;0]; % Follower 2 ' s coordination in the earth frame xy2 = rotate * xy2_leader + [x;y;z]; x2 = xy2(l,l); y2 = xy2(2,l); C .3 M-file of Formation Controller 100 z2 = xy2(3,l); yaw_angle2 = yaw_angle; % output i s y = [xl; y l ; yaw_anglel; x2; y2; yaw_angle2] y = [xl; y l ; z l ; yaw_anglel; x2; y2; z2; yaw_angle2]; Bibliography [1] H. Allen, S. Terry, and D. De Bruin, Accelerometer system with self-testable features, Sensors and Actuators 20 (1989), 153-161. [2] P. W. Barth, F. Pourahmadi, R. Mayer, J. Poydock, and K. Petersen, A monolithic silicon accelerometer with intergral air damping and overrange protection, IEEE Solid-State Sensors and Actuators Workshop (1988), 35-38. [3] Paul Blue, Levent Giivenc, and Dirk Odenthal, Large envelope flight control satisfying Hoo robustness and performance specifications, Proceedings of the American Control Conference, Arlington,VA (June 25-27,2001). [4] S. P. Boyd, L. El Ghaoui, E. Feron, and V. Balakrishnan, Linear matrix inequalities in systems control theory, SIAM, Philadelphia, PA, 1994. [5] M. L. Civita, G. Papageorgiou, W. C. Messner, and T. Kanade, Design and flight test-ing of a gain-scheduled Hoo loop shaping controller for a robotic helicopter, Submitted to the 2003 American Control Conference. [6] T. H. Cormen, C. E. Leiserson, and R. L. Rivest, Introduction to algorithms, New Jersey: Prentice Hall Inc., 1990. [7] Xiufeng He, Yongqi Chen, and Jianye Liu, Development of a low-cost integrated GPS\/IMU system, IEEE AES Systems Magazine 13 (1998), 7-10. 101 B I B L I O G R A P H Y 102 [8] S. A. Heise and J. M. Maciejowski, Model predictive control of a supermaneuverable vehicle, In Proceedings of the AIAA Guidance, Navigation and Control Conference, 1996. [9] M. Huzmezan and J. Maciejowski, Reconfiguration and scheduling in flight using quasi-lpv high-fidelity models and mbpc control, Proceedings of ACC (1998). [10] M. Huzmezan and J.M. Maciejowski, Flight management using predictive control, Ro-bust Flight Control, 1997 Springer Verlag, Lecture notes in Control and information Engineering series. [11] Richard A. Hyde and Keith Glover, The application of scheduled Hoo controllers to a VSTOL aircraft, IEEE Transactions on Automatic Control 38 (1993). [12] Christopher A. Jones, UAVs, New world vistas: Air and space for the 21st centry, Human systems and biotechnology systems 7.0 (1997), 17-18. [13] H. Jin Kim, David H. Shim, and Shankar Sastry, Nonlinear model predictive tracking control of rotorcraft-based unmanned aerial vechicles, Proceedings of the American Control Conference, Anchorage, AK (May 8-10,2002). [14] A. S. Krupadanam, A. M. Annaswamy, and R. S. Mangoubi, A multivariable adaptive controller for autonomous helicopters, Proceedings of the American Control Confer-ence, Anchorage.AK (May 8-10,2002). [15] D. J. Leith and W. E. Leithead, Survey of gain-scheduling analysis and design, Inter-national Journal of Control 73 (2000), 1001-1025. [16] S. M. Li , J. D. Boskovic, and R. K. Mehra, Globally stable automatic formation flight control in two dimensions, AIAA Guidance, Navigation, and Control Conference, Montreal, Canada (Aug. 2001). B I B L I O G R A P H Y 103 [17] Steinberg M., Comparison of intelligent, adaptive, and nonlinear flight control laws, Proceedings of Guidance, Control, and Dynamics 24 (2001), 693-699. [18] D. McFarlane and K. Glover, A loop shaping design procedure using synthesis, IEEE Trans, on Automatic Control 37 (June,1992). [19] T. W. McLain and R. W. Beard, Trajectory planning for coordinated rendezvous of unmmaned air vehicles, AIAA Guidance, Navigation, and Control Conference and Exhibit, Denver.CO, (Aug. 2000). [20] M. Pachter, J. J. D' Azzo, and J. L. Dargan, Automatic formation flight control, Journal of Guidance, Control, and Dynamics 17 (May, 1994), 1380-1383. [21] G. Papageorgiou and K. Glover, Taking robust LPV control into flight on the VACC Harrier, Proceedings of the 39th IEEE conference on Decision and Control (2000), 4558-4564. [22] A. W. Proud, M. Pachter, and J. J. D' Azzo, Close formation flight control, Proc. AIAA GNC, Porland, OR (Aug. 1999), 1231-1246. [23] J. Richalet, Industrial applications of model based predictive control, Automatica 29 (1993), 1251-1274. [24] L. M. Roylance and J. A. Angell, A batch-fabricated silicon accelerometer, IEEE Transaction on Electron Devices E D - 2 6 (1979), 1911-1917. [25] W. J. Rugh and J. S. Shamma, Research on gain scheduling analysis and design, Automatica 36 (2000), 1401-1425. [26] J. S. Shamma and M. Athans, Analysis of gain scheduled control for nonlinear plants, IEEE Trans. Automat. Contr. (1990). B I B L I O G R A P H Y 104 [27] C. M. Shearer and S. A. Heise, Contrained model predictive control of a nonlinear aerospace system, AIAA (1998), 4235. [28] S. N. Singh and M. Pachter, Adaptive feedback linearizing nonlinear close formation control of UAVs, Proceedings of the American Control Conference, Chicago, IL (June. 2000). [29] S. N. Singh, M. Pachter, P. Chandler, S. Banda, and S. Rasmussen, Input-output invertibility and sliding mode control for close formation flying, AIAA Guidance, Navigation, and Control Conference, Denver, CO (Aug. 2000). [30] A. Smerlas, I. Postlethwaite, D. Walker, M. Strange, J. Howitt, R. Horton, A. Gubbels, and S. Baillie, Design and flight testing of an Hoo controller for the NRC Bell 205 experimental fly-by-wire helicopter, Proceedings of the AIAA Guidance 98 (1998), 4000. [31] H. A. B. te Braake, M. Ayala Botto, H. J. L. Van Can, J. Sa da Costa, and H. B. Verbruggen, Linear predictive control based on approximate input-output feedback lin-earisation, IEEE Proc, Control Theory Appl. 146 (1999), 4. [32] Manikonda V., Arambel P., Gopinathan M., Mehra R., and Hadaegh F., A model predictive control-based approach for spacecraft formation keeping and attitude control, Proceedings of the American Control Conference (1999), 4258-4262. [33] D. Walker and I. Postlethwaite, Advanced helicopter flight control using two-degree-of-freedom Hoo optimization, Journal of Guidance, Control and Dyanmics 19 (1996), 461-468. [34] Martin F. Weilenmann, Urs Christen, and Hans P. Geering, Robust helicopter B I B L I O G R A P H Y 105 postion control at hover, Proceedings of the American Control Conference, Balti-more,Maryland (June 1994). [35] N. Yazdi, F. Ayazi, and K. Najafi, Micromachined inertial sensors, Proceedings of IEEE 86 (1998), 1640-1659. ","@language":"en"}],"Genre":[{"@value":"Thesis\/Dissertation","@language":"en"}],"GraduationDate":[{"@value":"2003-05","@language":"en"}],"IsShownAt":[{"@value":"10.14288\/1.0065374","@language":"en"}],"Language":[{"@value":"eng","@language":"en"}],"Program":[{"@value":"Electrical and Computer Engineering","@language":"en"}],"Provider":[{"@value":"Vancouver : University of British Columbia Library","@language":"en"}],"Publisher":[{"@value":"University of British Columbia","@language":"en"}],"Rights":[{"@value":"For non-commercial purposes only, such as research, private study and education. Additional conditions apply, see Terms of Use https:\/\/open.library.ubc.ca\/terms_of_use.","@language":"en"}],"ScholarlyLevel":[{"@value":"Graduate","@language":"en"}],"Title":[{"@value":"Formation and flight control of affordable quad-rotor unmanned air vehicles","@language":"en"}],"Type":[{"@value":"Text","@language":"en"}],"URI":[{"@value":"http:\/\/hdl.handle.net\/2429\/14459","@language":"en"}],"SortDate":[{"@value":"2003-12-31 AD","@language":"en"}],"@id":"doi:10.14288\/1.0065374"}