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 O F T H E REQUIREMENTS FOR T H E D E G R E E OF Masters of Applied Science in T H E FACULTY OF G R A D U A T E STUDIES (Department of Electrical and Computer Engineering) We accept this thesis as conforming to the required standard The University of British Columbia July 2003 © 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 Ill 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 implementation of formationflyingcontrol 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 D r . M i h a i Huzmezan, my supervisor at the University of B r i t i s h C o l u m b i a , for his willingness to guide and discuss my progress, outstanding supervision, comprehensive knowledge, encouragement and efforts of my career seeking. T h e opportunity of being D r . M i h a i Huzmezan's graduate student has already changed m y life to an aspiring future. M a n y thanks also go to mechanical technicians, Leiff Kjolby and Dave Fletcher i n Electrical and Computer Engineering Department, for their help of designing and building the flying m i 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 w i t h w h o m I shared m y pleasant moments during the study at U B C and life inside and outside Canada. Ultimately, I truly appreciate m y parents and my wife for their unwavering encouragement and unconditional support. iv Table of Contents Abstract ii Acknowledgements iv Table of Contents v 1 Introduction and Background 1.1 1.2 1.3 1.4 1.5 1 UAVs Review Sensors for Unmanned Air Vehicles 1.2.1 Accelerometers 1.2.2 Gyroscopes 1.2.3 GPS 1.2.4 Attitude Estimation for UAVs based on GPS/IMU High Fidelity Linear Parameter Varying (LPV) Modelling 1.3.1 Gain Scheduling and LPV Models 1.3.2 Quasi-LPV Models Jfoo loop shaping design for UAVs MPC control for UAVs . . 1 5 5 7 8 11 12 12 13 15 18 2 The Experimental Rig and The Nonlinear U A V Model 2.1 The Quad-rotor Helicopter Experimental Setup 2.1.1 The Flying Mill 2.1.2 Data Flow in the Experimental System 2.1.3 The Pulse Modulator 2.2 The Nonlinear Four-rotor Helicopter Model 2.2.1 Dynamics of Four-rotor Helicopter 2.2.2 The Quasi-LPV model of the Four-rotor Helicopter 2.2.3 The SIMULINK Model of the Four-rotor Helicopter 21 21 .21 23 24 31 32 35 36 3 2 D O F Hoo and Combined M P C / H o o Flight Controller Design 3.1 The Roll And Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 3.1.1 The Design Procedure 3.1.2 Simulation Results 3.2 Longitudinal and Lateral Speed, Throttle and Yaw Angle Outer Loop Design 38 39 39 43 46 CONTENTS 3.3 3.4 4 3.2.1 Design Procedure 3.2.2 Simulation Results The Trajectory Outer Loop Design 3.3.1 Design Procedure 3.3.2 Simulation Results A Combined MPC/tfoo UAV Flight Controller 3.4.1 Motivation 3.4.2 Design Procedure And Results Formation Control 4.1 The Formation Control Path Planner 4.1.1 The Dijkstra's Algorithm 4.1.2 The Path Planner Design 4.2 The Trajectory Generator 4.3 5 vi 46 50 52 52 56 . 60 60 62 67 69 69 73 75 ,. Formation Controller 80 Conclusion and Future Work A M-file of the Two D O F 86 Loop Shaping Flight Controller Design B M-file of the Combined M B P C / H infinity Controller Design C M-files for Formation Control C.l M-file of Dijkstra's algorithm for Path Planner C.2 M-file of Trajectory Generator C.3 M-file of Formation Controller Bibliography 89 94 • 96 96 97 99 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 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 Flight Controller vii 39 LIST O F 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 47 Flight Controller 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 54 Flight Controller 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 O F F I G U R E S 3.32 M P C In Longitudinal and Lateral Channel ix 63 3.33 Longitudinal and Lateral Channel Step Responses i n The Combined M P C A n d Hoo Controller 65 3.34 Sensitivity Function O f T h e Combined M P C / F o o Controller (The Unconstrained Case) 65 3.35 Complementary Sensitivity Of The. Combined MPC/i^oo Controller (The Unconstrained Case) 66 4.1 Formation Control Scheme For M u l t i p l e U A V s 68 4.2 Dijkstra's A l g o r i t h m Example 69 4.3 Voronoi Diagram for Threat Locations 73 4.4 Initial P a t h from T h e P a t h Planner 74 4.5 P a t h Formed by The Trajectory Generator 75 4.6 Trajectory Generator and Q u o d Rotor U A V Architecture 76 4.7 Simulation Results ff Trajectory Generator and Closed L o o p U A V 78 4.8 Trajectory Simulation Results 79 4.9 3 U A V Equilateral Triangle Formation i n Inertial Frame 80 4.10 T h e Overall Formation Control Architecture 82 4.11 A n i m a t i o n of Equilateral Triangle Formation F l y i n g 83 4.12 Formation F l y i n g Results of Follower B U A V 84 4.13 Formation F l y i n g Results of Follower C U A V 85 List o f Tables 2.1 The Nomenclature Used in Theoretical Formulation 32 3.1 64 Longitudinal and Lateral Channel Constraints 4.1 Table Solution For The Example 70 x Chapter 1 Introduction and Background 1.1 U A V s 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 "MQM36 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 timerprogrammer, 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 reconnaissance" 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 preprogrammed 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 control 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 U A V s 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 capable HALE UAV named the "Condor" that was a significant milestone in the development of endurance UAVs. The Condor featured lightweight composite and honeycomb structures, 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 U A V s 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: • 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. • 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 linearization, backstepping, if oo loop-shaping and Model Based Predictive Control (MBPC) - are currently investigated as flight controllers for highly reliable and autonomous UAVs. • 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, V , to the x acceleration, a can be written as: s where O is the offset voltage and S is the device sensitivity. The offset voltage and x x sensitivity parameters are provided by the manufacturers by testing and complicated procedures 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 accelerations 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 equation from the output voltages to the accelerations of the X and Y axes can be written as: SyyiVx ~~ O ) S y(Vy SxxSyy S ySyx x X Oy) _ S (Vy — Oy) — S (V —O) y — FI 5 c J *Jyy ^xy^yx a xx (1.2.2) X yx x x . . (i-.t.o) xx 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 suspension; 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, piezoresistive and capacitive devices. Piezoelectric devices can not measure constant acceleration 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 Gyroscopes 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 subjected 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 vibration 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 A i r Vehicles 9 an I D . 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 geostationary 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 integrated 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 microcontroller 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 H i g h Fidelity Linear Parameter V a r y i n g ( L P V ) M o d e l l i n g 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) Modelling 1.3.1 G a i n Scheduling and L P V Models In recent years (15) (25), Linear parameter-varying (LPV) systems have received a growing interest in the control community, especially for UAVs. The LPV system theory is motivated generally by the available gain scheduling methodology for control system design. 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 controller 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 switching 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. A(a, a) X z = Bi(a,a) J5 (a,o;)J 2 X Ci{a,a) Dii(a,d) Di (a,d)l | |u| C {a,a) D i(a,a) 2 V 2 2 D (a,a) 22 (1-3.1) u 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 1.3 High Fidelity Linear Parameter Varying ( L P V ) 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 solution 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, another 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 Q u a s i - L P V 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 QuasiLPV 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 QuasiLPV transformation, the model is written in a form ready for linearization. In Equation 1.3.1, a family of equilibrium states x (a), eq u (a), eq v (a) eg , parameterized by the scheduling variable vector a , is obtained by setting the state derivatives to zero: A(x (a),u q(a),v (a)) eg e a e S — 0, eg where S is the space within which the scheduling variables lie. Associated with the above equilibrium family is the error equilibrium family, Ci(x (a), eq u (a), eq v (a)) eq = z (a) eq = 0, a € S 14 1.3 H i g h Fidelity Linear Parameter V a r y i n g ( L P V ) M o d e l l i n g and the measured output equilibrium family, C (x (a),u {a),v (a)) 2 eq eq = y {a) = 0, eq a e S eq 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) D i(a, a) (1.3.2) D22 (a, a) 2 where q is vector of plant states not used for scheduling. Embedding the family of equilibrium states x (a), u (a), v (a) into the model above, eq eq eq 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) A (a) A i(a) A {a) 2 a 12 + B (a) (1.3.3) u B i{a)_ 2 22 A family of equilibrium states is obtained by setting the state derivatives to zero: a 0 = f(a) + A{a) + (1.3.4) B{a)6 (a) eq aeq(a) Therefore, combining Equation 1.3.3 with Equation 1.3.4, we can obtain the final model as: a q6- qeq(a) 0' = 0 |0 A (a) B (a) 12 A 22 - £[q (a)}A (a) eq 12 -£[q ( )]A (a) eq a 12 a n B 21 - £[q {a)]B {a) 11 q - q (a) -£[ (a)}B (a) 6 - 5 (a) eq qeq u eq eq This final Quasi-LPV form is an exact representation of the nonlinear model and is directly used for control law design. 15 1.4 HQO loop shaping design for U A V s 1.4 ifoo loop shaping design for U A V s The Hoc loop shaping design procedure combines classic loop shaping and notion of bandwidth 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 • The shaped model with the controller and the shaping functions W\ and W 2 can be easily tuned to a required system bandwidth and to account for performance limitations. • The generalized stability margin e, a multivariable extension of gain and phase margins can be used to ensure robustness (21). • In this framework, the controller gain scheduling and anti-windup can be used in a straight forward manner due to an observer type implementation. • 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 radio 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 ifoo loop shaping design for U A V s 16 model uncertainty. Thus the controller is more robust. In 1997 (30), a fixed-gain twodegree-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 parametervarying (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 control 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 designing 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 providing 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 Kl + I Figure 1.1: 2 DOF • K2 * - l 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 K is the 2 controller for measurement 1.4 HQQ loop shaping design for U A V s or feedback signals. 17 The shaped plant is G = W2GW1, with a normalized coprime s factorization G — M " ^ . In Figure 1.1 Wi gives exact model-matching at steady state. 1 s 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 design. Using frequency-dependent precompensator W\ and/or a postcompensator W , 2 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 G is defined as G = W GWi. s s 2 The postcompensator W contains low-pass and lead-lag filters for noise rejection 2 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 T f between the commands and controlled Te outputs is selected. 3. Robust Stabilization: The stability margin e : e - V 2 = \\(I- G K )~ G Ki 1 s 2 s - r r e / ||oo 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 # 0 0 controller would guarantee robustness based on theoretical (18) and practical experience (21). If the stability margin is not satisfied, an adjustment of W\ and W or T f is required and 2 hence a restart from 1 is required. Te 18 1.5 M P C control for UAVs 4. Implementation: The 2 DOF controller K\ and K are then constructed by splitting 2 the K„o = 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 applied 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 optimization 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 performance aerospace system (27) (13). Nonlinear Model Predictive control (NMPC) is an optimization-based method where a nonlinear process model is used to predict the effect of future input moves with operating constraints and other demanding performance requirements. It is apparent from (27) and (13) that this method provides good tracking 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 19 1.5 M P C control for U A V s robust to modelling errors and last but not least, is easy to tune. PAST FUTURE • Set p o i n t Predicted output JJ] y ( k ) = r ( k ) Manipulated input Constant input u(k+l) k-3 k - 1 ~i—i—r -i k+l k+Nu k+Nl C o n t r o l h o r i z o n - Nu r—*k+N2 Minimum o u t p u t h o r i z o n - N l Maximum o u 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 (N ), where Au(k + I) = u(k + I) - u(k + 1 — 1). For prediction, it u is also assumed that Au(k + I) = 0 for I > N . u 2. Obtain the optimization of the following cost function: N N 2 J(k) = £ Ni u \\y(k + 1)- r(k + l)\\ 2 Q{l) + £ 1=0 \\Au(k + l)\\ 2 m 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 = x Qx. T 3. Minimize the above optimization by solving a standard Quadratic Programming problem subject to constraints on: . • Input Levels: ui(l) < u(l) < u (l) , where k < I < k + N — 1 u u • Input Change Rates: Aui(l) < Au(l) < Au (l), where k < I < k + N — 1 u u • Output Levels: yi(l) < y(l) < y (l), where k < I + Ni < k + N u 2 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 problem 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 The Flying Mill 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 Figure 2.1: Photo of The Flying Mill With Dragonflyer III 22 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 D a t a 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 Operator Dragonflyerlll Personal computer ^==^ IMU and GPS module Transmitter Receiver T v J v 4 rotors 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 « • 4 1.1 1.1 0.46 3 2 • 0.4 4 • 4 1.1 0.4 > 11.19msec 1.1 0.4 0.4 17.65msec Figure 2.3: P W M 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 P W M 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 R P M 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 channel number m 8 b i t d i g i t a l I/O i n dSPACE DS1102 board Microprocessor* RD 3 f o r the PWM signal RA 0-5 for pulse width data and RB 0,1 f o r 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 P W M 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 P W M 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 Modulator for Draganflyer III Made by: Ming Chen Date: 01 September, 2002 Puis e1 Pulse Widthl Pulse2 Pulse Wldth2 Out Puis 63 DS1102OUT Pulse Wldth3 Pulse4 Pulse Wldth4 P u!ss Modulator Figure 2.5: The Top Level Simulink Model of The Pulse Modulator Pulse Widthl 1*1" vt -KZD Out1 Pulse Width3 rxj 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 written 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 (Start) i Define and i n i t i a l i z e PORTs —4 I n i t i a l i z e related f i l e registers . _ t C a l c u l a t e TIMER1_L, TIMER1_H to TIMER5_L, TIMER5_H from PULSEDATA1 to PULSEDATA4 . 1 Delay 0.46 msec u s i n g T i m e r l ± T r a n s f e r data i n TIMER1_L and TIMER1_H to TMR1 r e g i s t e r p a i r s and output the PWM s i g n a l of channel 1 u s i n g TIMER1 i Delay 0.4 msec u s i n g T i m e r l . 4 T r a n s f e r data i n TIMER2_L and TIMER2_H to TMR1 r e g i s t e r p a i r s and output the PWM s i g n a l of channel 2 u s i n g TIMER1 Delay 0.4 msec u s i n g T i m e r l . * T r a n s f e r data i n TIMER3_L and TIMER3_H to TMR1 r e g i s t e r p a i r s and output the PWM s i g n a l of channel 3 u s i n g TIMER1 4 Delay 0.4 msec u s i n g T i m e r l ± T r a n s f e r data i n TIMER4_L and TIMER4_H to TMR1 r e g i s t e r p a i r s and output the PWM s i g n a l of channel 4 u s i n g TIMER1 i Delay 0.4 msec u s i n g T i m e r l T r a n s f e r data i n TIMER5_L and TIMER5_H t o TMR1 r e g i s t e r p a i r s , output the PWM s i g n a l of channel 5 u s i n g TIMER1 and update PULSEDATA1 to PULSEDATA4 ( End) Figure 2.7: Flow Chart of Pulse Modulator i n MASM 29 2.1 The Quad-rotor Helicopter Experimental Setup Q Initialize Start) related file registers T r a n s f e r d a t a i n TIMER5_L a n d TIMER5_H t o TMR1 r e g i s t e r p a i r s Start TIMER1 R e a d p u l s e c h a n n e l number f r o m PORTB 0-1 a n d p u l s e w i d t h d a t a f r o m PORTA 0-5 D e l a y 0 .2 msec a n d r e a d d a t a f r o m PORTB 0 -1 a n d PORTA 0-5 a g a i n N — A r e two ^readings of pulse channel lumber i d e n t i c a l ^ - Transfer the pulse width data t o the respective r e g i s t e r f r o m 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 30 2.2 The Nonlinear Four-rotor Helicopter Model 31 RAO-5 for each pulse channel. The data i n P U L S E D A T A registers are converted to the data i n 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 Significant Bytes of the first to the fifth pulse w i d t h 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 w i d t h data respectively. 2.2 The Nonlinear Four-rotor Helicopter Model Figure 2.9: T h e Four-rotor Helicopter M o d e l T h e U A V used i n the project is a commercial quad-rotor helicopter, Dragonfiyer III, currently w i t h a 3 m i n 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 i n Figure 2.9. T h e vertical motions along z-axis i n the body-fixed frame can be obtained by changing the 32 2.2 T h e Nonlinear Four-rotor Helicopter M o d e l Symbol u(l) u(2) «(3) «(4) zB x z x u{2) = F -F «(3) = F -F u(4) = F -F + F -F 4 3 FB FyB F F Fy F I ly Iz P X Definition u(l) = F i + F2 + F3 + F4 1 <t> 1 3 Q r 2 2 Symbol 4 force in body-axis x direction force in body-axis y direction force in body-axis z direction force in earth-axis x direction force in earth-axis y direction force in earth-axis z direction moment of inertia in x direction moment of inertia in y direction moment of inertia in z. direction roll rate (P) e 1> UB VB • W B U V W X y z Definition pitch rate (Q) yaw rate (R) roll angle (PHI) pitch angle (THETA) yaw angle (PSI) body-axis x direction velocity body-axis y direction velocity body-axis z direction velocity x direction velocity y direction velocity z direction velocity x position of COG y position of COG 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 bodyfixed 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 of Four-rotor Helicopter 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: cos ip — sin ip 0 sin ip REB = Rip-Re-R<t> cos ip 0 0 0 1 cos 9 0 0 sin( 1 1 0 cos 4> — sin cj) (2.2.1) 0 0 0 — sin 9 0 cos ( 0 sin 4> cos <f> | cos ip cos 9 — sin ip cos (p + cos ip sin 9 sin (f> sin •)/> sin <p + cos V ' sin 9 cos cos ^ cos <j> + sin ?/) sin 0 sin <> / — cos ip sin ^ + sin V > sin 0 cos 01 sin •J/' cos 9 — sin 9 cos 9 sin 0 cos 0 cos 9 Note that .Res = R%E • The transformation of velocities between body-fixed and earth-fixed coordinates is: u U V v B (2.2.2) B W w B 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: 0 F xB F B = = FyB 0 FB 2~2i=l Fi Z In the earth-fixed frame, the forces can be described as: sin ip sin <f> + cos ip sin 9 cos (p — cos ip sin <f> + sin tp sin 9 cos (f. i=l (2.2.3) cos <j> COS 0 Therefore, the equations of motion x,y,z in the earth-fixed frame are represented as: F X m y z = x - K l - x F -K -y y 2 F - mg - K z 2 • 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: m y 2~2i=i Fi (sin tp sin <p + cos tp sin 9 cos (p) — K\ • x F -K -x X x = x = F -K -y y 2 F - mg - K • z z z YA=I Fi (s™ sin 0 cos 4> — cos tp sin <f>) — K -y 2 Ylt=i Fi cos 4> cos 6 — mg — K • z 3 3 (2.2.4) 4> = l(F - F i - K <P)/I 3 4 6 = l(F - F 4 2 - $ = (M -M 1 X K d)/I 5 y + M -M - 2 3 K iP)/I 4 6 z = (Fi - F 4- F 3 - F 4 - K' i;)/l' 2 6 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 I includes the moment of inertia of z axis and the force to moment scaling z factor. These moments can be measured or identified. For convenience and compatibility with the control panel of the Futaba radio transmitter used in conjunction with the Dragonflyer III, we define the inputs to be: u(l) = F + F + F + F l 2 3 4 u(2) = F - F 4 2 . u(3) = F - F i 3 u(4) = F -F l 2 + F -F 3 4 Therefore, the motion equations become: u(l)(sin?/> sin0 + cos ip sin 6 cos <fr) — K\ • x m y= •u(l)(sin ip sin 8 cos (p — cos tp sin (p) — K • y 2 m (2.2.5) 35 2.2 T h e Nonlinear Four-rotor Helicopter M o d e l z = u(l) cosc/>cos# — K3 • z m 9 = (u(2) - g K 6)l/I 5 y 4> = (u(3) - K 4>)l/I 4 $ = (u(4) 2.2.2 x ^)//; T h e Q u a s i - L P V model of the Four-rotor Helicopter The nonlinear motion equation of the Four-rotor Helicopter, Equation 2.2.5, can be rewritten in the quasi-LPV form as follows: 0 0 0 0 0 0 0 0 0 0 0 -1 0 0 0 0 0 0 0 0 1> 0 0 0 0 9. 0 0 0 0 6 1 0 0 0 0 1 0 0 0 0 1 0 sin ij> sin <fi+cos iji sin 6 cos0c, m + 0 0 0 0 0 cos <p cos m 0 0 0 0 l/Iy 0 0 u(l) 0 0 l/Iy 0 u(2) 0 0 0 /; «(3) 0 0 0 0 u(4) 0 0 0 0 0 0 0 0 0 0 0 0 sin i\> sin 8 cos <j>—cos ip sin e m 2.2 The Nonlinear Four-rotor Helicopter Model where x y T z the Euler angles 9 <p i> 9 36 are vectors of model states not used for scheduling and T (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 M o d e l 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 — 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 Nonlinear Four-rotor Helicoper Model Made by: Ming Chen Date: September, 9,2002 CL> u(1) u(1)=F1+F2+F3+F4 37 1/m tntegrdtoi IntegratoM KD u f(u) -KID V Integrator Integrators -KD w f(iO -•CD Produd2 H Integrator^ Integrators K D y -KD *0 Fcn2 C D u(2) rj(2)=F4-F2 Uly / Uly pitch rate^ Integrator Integrator? - K D Ulx Integrators Integrators yaw rate C D — • THETA pitch angle CDu(3)=F3-F1 -KD 1/lz_dot 11(4) 1/K IntegratorlO Integratorll u(4)=F1-F2+F3-F4 Figure 2.10: Sub Level SIMULINK Model Diagram of Dragonfiyer III PHI roll angle - K D PSI yaw angle 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 U A V model first inner loop Controller Inner Loop outer loop controller The First Outer Loop Fig. 3.11 Fig. 3.1 Fig. 2.10 Controlled by the second outer loop controller The Second Outer Loop Fig. 3.20 Figure 3.1: 3 loop controller architecture 38 3.1 The Roll A n d 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 Design Procedure The objective of the inner loop controller is to achieve a robust stability control of the quadrotor 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: W u(1) THETA PHI x" = Ax+Bu y » Cx+Du R u(2) X - Ax+Bu y= Cx*Du U V •CD uO) -KID yaw angle 5 u(4) •CD ^ yawi angle *—KT) y Nonlinear Dragan Flyerlll rf = Ax+Bu rf = Ax+Bu y «• Cx+Du y= Cx+Du 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 T h e R o l l A n d P i t c h Angles, Yaw R a t e and Vertical Speed Inner L o o p Design 40 • 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. • 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 equation 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 Integrator tl(3^F3-F1 CD H Integrator^ - L B Integrator -X2D R u(4)"F1-F2+F3-F4 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 A n d Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 41 Unshaped Plant Open Loop Singular Values -ISO - ; l 10"*' V ^ ' ' 10"' i - L l ^ l L . i io u i 10' , io ;: ,....L.,..X.^1 ^ 10 3 Frequency (rad/sec) Figure 3.4: Open Loop Singular Values of The Unshaped Plant Shaped Plant Open Loop Singular Values 10" S 10"' 10° to 1 io z io 3 Frequency (fad/sec) Figure 3.5: Open Loop Singular Values of The Shaped Plant 3.1 The Roll A n d Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 42 Using frequency-dependent precompensator W\ and a postcompensator W , the sin2 gular values of the shaped plant G , defined as G = W GW\ can be shaped to the s s 2 designed open-loop shape and have crossover frequencies at around 7 rad/s, see Figure 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 W contains first-order low-pass filters for noise rejection and 2 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 W are as 2 follows: W\ = diag 1.52g+0.965 W = diag 90 s+35 2 23 73(s+2.31) s+35 13 1.5(17.2s+9.7) 73(s+2.31) s+35 90 s+35 3. Robust Stabilization: To reach the objective rising time, the reference closed-loop transfer function T f re was chosen as T j = diag re The 1 s +2s+l 2 9 s +2*0.95*3s+3 2 2 9 s +2*0.95*3s+3 2 2 9 s +2*0.95*3s+3 2 2 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. 4. Implementation: The controller architecture is presented in Figure 3.1. The has 20 states, 8 inputs and 4 outputs and was split to K final x K 2 controller, K 0 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 A n d 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° 10 1 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 Hooflightcontroller 3.1 The Roll A n d Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 44 Singular Values m il)"'' lO' VJ 1 Id' 10* 10'" Frequency (rad/sec) Figure 3.7: Closed-loop Complementary Sensitivity Function of The Inner Loop Step Response From: W..r 0 1 Ffcun: THETA.r 2 3 C 1 1 2 From:PHI..r 3 0 1 From: R,.r 2 30 1 Time (sec) Figure 3.8: Step Responses With HQO Controller 2 3 3.1 The Roll A n d Pitch Angles, Yaw Rate and Vertical Speed Inner Loop Design 45 From W Output Step Distrubance Response From; THfiTA kwp From: FHI loop top From: R loop -0.' Figure 3.9: Output Step Disturbance Responses Pitch angle (THETA) The vertical velocity (W) 20 -0.05 10 -0.15 -0.2 10 20 30 40 50 10 20 30 40 Yaw rate (R) Roll angle (PHI) 20 r 1.5 1 10 0.5 'J 10 20 30 40 10 20 30 Figure 3.10: Simulation of The Nonlinear Model 40 3.2 Longitudinal and Lateral Speed, Throttle and Yaw A n g l e Outer L o o p Design 2.51 The vertical velocity (W) , , . 0 10 20 30 1 301 40 " 0 46 Pitch Angle (THETA) ; ! 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 3.2.1 Longitudinal and Lateral Speed, Throttle and Yaw A n gle Outer Loop Design Design Procedure 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 K | "u y = C>c«-D< Wi Ax+Bu y =Cx*Du yaw angle yaw angk_r * - AxfDu V Innerloop and Dragan Flyerlll yaw angle -KD y K2 Figure 3.12: The First Outer 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 shall be as high as practically possible, and the rising time in each loop should be around 4 seconds. • The stability margin e is in the interval [0.3, 0.4] allowing for 30-40 % uncertainty in coprime factor for robustness. • 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 Dragonflyer III model in Figure 3.13 was linearized around the equilibrium point u =• 0 0 0 ol • 3.2 Longitudinal and Lateral Speed, T h r o t t l e and Yaw A n g l e O u t e r L o o p Design 48 W CD- u(1) THETA | W_r PHI vi = Ax+Bu THETA_r • CD—* PHI_f y = Cx+Du Wi K1 uC2) + + w R *• =Ax+Bu y ° Cx+Du U u(3) W1 V C D — • 2 R r u(4) yaw angle Dragon Flyer III nonlinear model * = Ax+Bu y= Cx+Du >( = Ax+Bu y= Cx+Du K2 W2 -K2D -KD) 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 T j re with the following diagonal entries : 1.69 7^+2~6s+TI§ 1.69 s +2.6s+1.69 2 1.69 s +2.6s+1.69 2 4 s +4.2s+4 2 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 io" ! 10"' io 1 10''' 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 •101 10'* • •—r™~rrm 10"' 1 ! — r ™ i 1 K)° 1—r rrm ! 10 ! 1 —•rrTrr 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 K which each has 4 inputs and 4 outputs. The scaling Wi — 2 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° 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 disturbance 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 irom: 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 3.3.1 The Trajectory Outer Loop Design Design Procedure The trajectory loop, shown in Figure. 3.21, was designed for the x and y direction trajectory 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: 53 3.3 T h e Trajectory Outer L o o p Design Step Response from: Input Poini(l) from: input Points) 5 From: input PointfJ) 10 From: Input Pointf-t) 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 x = Ax+Bu y= Cx+Du 1 Wi yam angle_r yaw angle yaw angle_r Degree to Radian Trajectory Draganflyerlll, Innerloop and Outerloop Ax+Bu y = Cx+Du yawj_angle Degree 4 K2 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. Wr z_r CO—"' U r U p. > K"i?^>-f> x? = Ax+Bu y = C>c*-Dt Wi1 V_r THETA.r Ax+Bu y = Cx+Du CH)—^ PHI r y a w anflle R r yaw angle_r mner Loop and Dragan flyer III nonlinear model X = Ax+Bu y - Cx+Du K2 1 Terminator V Terminatorl -KJD 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. 55 3.3 T h e Trajectory Outer L o o p Design 3. Robust Stabilization: The reference closed-loop transfer function T f was chosen as re T ref - diag s +2*l*s+l 2 s +2*l*s+l 2 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 — diag0.735 0.735 contains the static steady gain for each loop. Shaped Plant Open Loop Singular Values Frequency (rad/sec) Figure 3.23: Open Loop Singular Values of The Trajectory Loop Design 56 3.3 T h e Trajectory Outer L o o p Design 3.3.2 Simulation Results 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, -60 L10"'' 1 11 1 ; • ' ! • ! ' • ! ! i i-.j..Li.i.ix... ; ; ..........Li...! 10'' 10 ' ' ' ' ; ; LJ-Liiiii io' ; i i i-i^iiii 10 i .-..i..i..;..u.;^ 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 simulation 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«(2) Figure 3.26: Step Responses With HQQ Controller of The Second Outer Loop 3.3 T h e Trajectory Outer L o o p 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 a 4~- N 2J,- 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 C o m b i n e d 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 3.4.1 A Combined MPC/fZoo U A V Flight Controller 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 C o m b i n e d 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: • An optimal ability to account for actuator constraints and ensure stability when the UAV operates close to them. • Opportunity to operate actuators at their limits while rejecting disturbances without destabilizing the closed loop. • Flight operation closer to flight envelope constraints is safely permitted. • Drawbacks associated with fixed gain controllers such as time invariance of prefilters and lack of trajectory prediction are addressed. • The UAV multivariable control problem is handled naturally. • 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^ 63 U A V Flight Controller 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 T = 0.5 second. The gain of state observer in Figure 3.31, L, was set s to "l ol for DMC observer scheme. The observer architecture is coded in the MPC L° algorithm 1 Ik w X W w y W + + lb, w MBPC^ M B P C Controller Trajectory 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^ Limits - 4 5 ^ .< 9 < 4 5 ^ -20^ < q< 20^ -20^ < r < 20^ -2 < U < 2 -2 < F < 2 -1 < A t / < 1 -1 < AV < 1 U A V Flight Controller Constrained Variable Name pitch angle saturation roll angle saturation pitch rate saturation roll rate saturation velocity limits of x direction velocity limits of y direction velocity increment limits of x direction velocity increment limits of y direction 64 Unit rad rad. rad/s rad/s m/s m/s m/s m/s Table 3.1: Longitudinal and Lateral Channel Constraints horizons were 1 to ensure a fast response. The output weight was Q — 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 controller 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. MPC/H^ 3.4 A C o m b i n e d U A V Flight Controller 65 x and y position with MBPC controller 1 >ositton y posit on )! 0 i i [ 5 10 15 1 ^ * * «— — i 20 25 Seconds ii iIi ,1 1 1 30 35 40 45 50 40 45 50 x direction velocity U and y direction velocity V I j u j f — A / I 0 5 10 15 20 25 Seconds 30 35 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: ! 10" 3 • 10~ 2 i , ' . •• i i 10"' 10° 10' Frequency (radians/time) Figure 3.34: Sensitivity Function Of The Combined M P C / # o o Controller (The Unconstrained Case) 3.4 A C o m b i n e d 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 Unconstrained Case) MPC/tfoo Controller (The 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 identical 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 Generator 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 dynamical 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 UAV Follower #1 Formation Controller #2 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 T h e Formation C o n t r o l P a t h Planner 4.1 69 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 T h e Dijkstra's Algorithm To find the optimal way to be generated by the Path Planner Level, the Dijkstra's Algorithm, 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 A C E D B 6* 6* 8 Unknown Unknown 8* 11 Unknown 70 11* 20 15* 0* 6* 8* 11* 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 T h e Formation C o n t r o l P a t h 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 predecessor 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 T h e Formation C o n t r o l P a t h Planner Vertex Predecessor A A C A D C B D 72 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 4.1.2 73 T h e 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 T h e Formation C o n t r o l P a t h Planner 74 Figure 4.4: Initial Path from The Path Planner J = kJ i + (1 - k)J t rt f>i 0<k<l The cost, Ji, consists of the threat cost J ,j regarding to the risk incurred and the r 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 Dijkstra'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 optimal path generated by this algorithm through the path searching of the Voronoi diagram in Figure 4.3. 4.2 T h e Trajectory Generator 4.2 75 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: X VA ZA yaw angle^ RA X VB ZB yaw angles RB xc yc ZC yaw anglec Rc XD VD ZD yaw angler. RD A B 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 yaw_angle_r trajectory generator 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 (x — x) + (y — y) + [z — z) + W(yaw-angle — yaw.angle) 2 r 2 r 2 T 2 r 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 T h e 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 Figure 4.7: Simulation Results ff Trajectory Generator and Closed Loop UAV 30 4.2 The Trajectory Generator 79 4.3 Formation 4.3 80 Controller 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 | cos tp — sin tp sin ip OA is as : I cos tp Thus the coordinations of two follower UAVs, frame can be derived as: (XBI,VBV) a n d (xci,Vci), i the inertial n 4.3 Formation XBI X = R4, VB1 X C 1 yci B A X + yBA - 2 A y X XCA + = R-4> ycA XBA where 81 Controller A . A VA i / and L yBA XCA ycA 2 2 L n L is the lateral length. 2 The z positions and yaw angles of the two followers are identical to the leader. Therefore the overall relationship, which the formation controller implements, is as follows: XBl = Rip VBl ZBl L 2 + x A (4.3.1) yA_ = ^(A) yaw-angleBi = yaw.angle A and XCl yci L ~ 2 + x A (4.3.2) })A_ zci = Z(A) yawjanglecx — yaw.angle A Figure 4.10 represents the overall formation control architecture. Also an animation block to accommodate this structure has been defined. The 'Quad-rotor UAV closedloop 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 closedloop dynamics2' blocks represent the two follower Quad-rotor with their corresponding 4.3 F o r m a t i o n Controller 82 START UP y-f PHI Double click to load initial parameters before each simulation THETA| yaw_angle yaw_angle_r yaw_angle_r yaw_angle waypoint generator Quad-rotot UAV closed-loop dynamics X x1_r x_r y yl-r y_r z1_r z_r z PHI THETA y_r yaw_angle1_r yaw_angle_r yaw_angle x2_r y2_r Quad-rotot UAV closed-loop dynamicsl X x_r y z2_r yaw_angle_r yaw_angle2_r y_r z_r formation controller z PHI THETA yaw_angle_r yaw_angle Quad-rotot UAV closed-loop dynamics2 Figure 4.10: The Overall Formation Control Architecture 4.3 F o r m a t i o n 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 controller' 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 parameters. 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 Figure 4.12: Formation Flying Results of Follower B UAV 84 4.3 Formation Controller Figure 4.13: Formation Flying Results of Follower C UAV 85 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 comprehensive 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 M P C / 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 suggestions 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 M B P C / ^ o o 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 - I n f i n i t y Analysis and Synthesis % Designed by Ming Chen % Date: A p r i l 2003 function uav clear % Section One: Calcuate the H-inf c o n t r o l l e r f o r the inner loop °/ 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 = t f ( s y s ) ; G = pck(A,B,C,D); sigma(A,B,C,D,{0.01,1000}) 0 title('Unshaped Plant Open Loop Singular Values') g r i d 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] , [ 1 0 ] ) ; 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 c o n t r o l l e r K and get gamma= 2.8312 K = hinf2dof(Gs, T r e f ) ; 1 Step 4: Seperate K to K l and K2 minfo(K) % system: 20 states 4 outputs Kl = s e l ( 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); 8 inputs ; 7o Section Two: Calcuate the H_inf c o n t r o l l e r f o r 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 = t f ( s y s o ) ; 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 c o n t r o l l e r Ko and get gamma=2.5964 Gs = Gso; Tref = Trefo; K = hinf2dof(Gs, T r e f ) ; 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 i s 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 f o r x y control % Step 1: Linearize the plant and analysis °/ 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); 0 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 c o n t r o l l e r Kxy and get gamma=3.502 Gs = Gsxy; K = hinf2dof(Gs, T r e f ) ; Kxy. = K; 7. Step 4: Reduce the Kxy and seperate t o 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 C o n t r o l l e r Design 2D0F % synthesizes the H_inf 2-DOF c o n t r o l l e r % Uses MATLAB mu toolbox % Usage: K=hinf2dof(Gs, T r e f ) ; 1 INPUTS: Shaped plant Gs and reference model Tref "/, OUTPUT: Two degrees-of-freedom c o n t r o l l e r K 7, Coprime f a c t o r i z a t i o n of Gs %============================================================ function K=hinf2dof(Gs.Tref) [As,Bs,Cs,Ds] =unpck(Gs); [Ar,Br,Cr,Dr] = unpck(Tref); [nr,nr] = s i z e ( A r ) ; [lr.mr] = size(Dr); [ns,ns] = size(As); 93 [ls.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 ) ; Rl = Cs'*inv(Rs)*Cs; QI = B s * i n v ( S s ) * B s ' ; [ Z l , Z2, f a i l , reig_min] = r i c _ s c h r ( [ A l ' - R l ; -QI - A l ] ) ; Zs = Z 2 / Z 1 ; rho = 1.0; A = daug(As,Ar); BI = [ z e r o s ( n s . m r ) ( ( 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 ) ] ; Cl = [zeros(ms,ns+nr);Cs z e r o s ( l s , n r ) ; r h o * C s -rho*rho*Cr]; C2 = [ z e r o s ( m r , n s + n r ) ; C s z e r o s ( l s . n r ) ] ; D l l = [zeros(ms,mr+ls);zeros(Is,mr) sqrt(Rs);-rho*rho*Dr rho*sqrt(Rs)]; D12 = [ e y e ( m s ) ; D s ; r h o * D s ] ; 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 = [ z e r o s ( m r , m s ) ; D s ] ; B = [BI B2] ; C = [C1;C2]; D = [ D l l D12;D21 D 2 2 ] ; P = pck(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.01; [K, G n c l p , gam] = h i n f s y n ( P , nmeas, n e o n , g m i n , gmax, g t o l ) ; [Ac,Bc,Cc,Dc] = 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 • % Designed by Ming Chen 7. Date: A p r i l 2003 function uav_mpbc clear % % % % 7o The model Gxy used as i n t e r n a l model i s the l i n e a r model of the Draganflyer I I I , the H-inf inner loop and the f i r s t H-inf outer loop. The constraints of the input: -2<u<2; - K d e l t a u<l; -2<v<2; - K d e l t a v<l the nonlinear model of the Graganflyer I I I , the H-inf inner loop and the f i r s t H-inf outer loop was used as process model 7o l i n e a r e d state space model [Axy,Bxy,Cxy,Dxy]=unpck(Gxy); 7» D i s c r e t i z e the l i n e a r model and save i n the mod 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 c o n t r o l l e r parameters P = 10; % P r e d i c t i o n horizon M = 1; 7o Control horizon ywt = [5,5]; % Equal weighting of y ( l ) and uwt = [1,1]; % Equal weighting of u ( l ) and ulim = [ [-2 -2] [2 2] [1 1] ] ; 94 form y(2) u(2) 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 p l o t Ks = smpccon(imod,ywt,uwt,M,P); [clmod.cmod] = smpccKpmod,imod,Ks,Kest); f r e q = [-2,1,100]; [frsp,eyefrsp] = mod2frsp(clmod,freq); p l o t f r s p ( e y e f r s p , [1], [1]) % s e n s i t i v i t y grid p l o t f r s p ( f r s p , [1], [1]) % complementary s e n s i t i v i t y grid Appendix C IVI-files for Formation Control C.l M-file of Dijkstra's algorithm for Path Planner % Ming Chen % Quod-rotor UAV % May 30,2003 % % % % % % 7o 7o project Solving the Shortest Path Problem using D i j k s t r a ' s Algorithm - This function takes i n a square n x n matrix A as input. The matrix represents the weights of edges i n 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 v e r t i c e number and the second column shows the shortest path from the source to t h i s node. The l a s t column of sp presents the number of nodes that are i n the shortest path. 7» DIJKSTRA f i n d the shortest paths from the s t a r t i n g node to each 7» other node i n the network. 7. INPUT: % A = input matrix 7. OUTPUT: '/, sp = shortest paths to each node from s t a r t i n g node function [sp] = d i j k s t r a ( A ) [m,n] = s i z e ( A ) ; resultl=zeros(n,n-l); result=zeros(l,n-l); y = [zeros(l,n-l); A(l,2:n); o n e s ( l , n - l ) ] ; % shortest path 96 C.2 M-file of Trajectory Generator p = find(y(l,1:n-l)==0); % p = vector of non-terminated nodes i t e r = 1; while "isempty(p) % i t e r a t e when p i s not empty x = find(y(2,p)>0); % x=vector of indices r e f e r r i n g to nonzero entries 7, Step 2 [a,K] = min(y(2,p(x))); 7o a = smallest nonzero entry 7. K = index of x r e f e r r i n g to a o = p(x); J = o(K) +1; 7o J = index of A r e f e r r i n g to a y ( l , J - l ) =1; '/, changes termination b i t to 1 p = p ( f i n d ( 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 v i a J tempi = y ( 2 , r - l ) ; temp2 = y ( 3 , r - l ) ; h e l p e r l = y ( 2 , r - l ) > w; °/„ i f w i s l e s s than previous 7»re s u i t ( i t e r , i t e r 1) = r ; 7o shortest path y ( 3 , r - l ) = y ( 3 , r - l ) + (J - y ( 3 , r - l ) ) . * h e l p e r l ; 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 ) ) . * h e l p e r 2 ; 7. update y y ( 3 , r - l ) = y ( 3 , r - l ) + (J - y ( 3 , r - l ) ) . * h e l p e r 2 ; 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 97 C . 2 M-file of Trajectory Generator function y = generator(u) format short e 7„ define global v a r i a b l e s 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 p o s i t i o n and the current p o s i t i o n % a l l these d i f f e r e n c e s are to be reduced to zero by the "waypoint % c o n t r o l l e r " at the expense of speed v a r i a t i o n s Dx = x_r - x; Dy = y_r - y; Dz = z_r - z; • Dyaw_angle = yaw_angle_r-yaw_angle; % vector modulus i s computed DS = sqrt(Dx"2+Dy"2+Dz"2+ (10*Dyaw_angle)~2); % once i n the desired radius around the setpoint the next waypoint °/o w i l l be used as o v e r a l l 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 p o s i t i o n time stamped i s given to the inner loop or 98 C.3 M-file of Formation Controller 99 % t r a j e c t o r y generator y = [x_r; y_r; z_r; yaw_angle_r]; C.3 M-file of Formation Controller % Quad Rotor UAV Formation C o n t r o l l e r 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); 7 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]; 0 % Follower 1 ' s coordination i n the Leader frame xyl_leader=[-1.732/2*distant; distant/2;0]; 7o Follower l ' s coordination i n the earth frame xyl = rotate * xyl_leader + [x;y;z]; xl = x y l ( l , l ) ; yl = xyl(2 l); zl = xyl(3,l); yaw_anglel = yaw_angle; ) 7o Follower 2 's coordination i n the Leader frame xy2_leader = [-1.732/2*distant; -distant/2;0]; % Follower 2 ' s coordination i n the earth frame xy2 = rotate * xy2_leader + [x;y;z]; x2 = x y 2 ( l , l ) ; y2 = xy2(2,l); C.3 M-file of Formation Controller z2 = xy2(3,l); yaw_angle2 = yaw_angle; % output i s y = [ x l ; y l ; yaw_anglel; x2; y2; yaw_angle2] y = [ x l ; y l ; z l ; yaw_anglel; x2; y2; z2; yaw_angle2]; 100 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 envelopeflightcontrol 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 andflighttesting 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 101 13 (1998), 7-10. BIBLIOGRAPHY 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, Robust 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, A K (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, International 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). BIBLIOGRAPHY 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). BIBLIOGRAPHY 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 andflighttesting of an Hoo controller for the NRC Bell 205 experimentalfly-by-wirehelicopter, 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 linearisation, 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 helicopterflightcontrol using two-degreeof-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 BIBLIOGRAPHY postion control at hover, Proceedings 105 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.
- Library Home /
- Search Collections /
- Open Collections /
- Browse Collections /
- UBC Theses and Dissertations /
- Formation and flight control of affordable quad-rotor...
Open Collections
UBC Theses and Dissertations
Featured Collection
UBC Theses and Dissertations
Formation and flight control of affordable quad-rotor unmanned air vehicles Chen, Ming 2003
pdf
Page Metadata
Item Metadata
Title | Formation and flight control of affordable quad-rotor unmanned air vehicles |
Creator |
Chen, Ming |
Date Issued | 2003 |
Description | 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 ∞] 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 ∞] loop shaping control law was designed for stabilization and speed loops. For the trajectory control, two controllers, an H[sub ∞] 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 ∞] 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 ∞] and combined MPC/H[sub ∞] 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. |
Extent | 5609554 bytes |
Genre |
Thesis/Dissertation |
Type |
Text |
FileFormat | application/pdf |
Language | eng |
Date Available | 2009-10-30 |
Provider | Vancouver : University of British Columbia Library |
Rights | 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. |
IsShownAt | 10.14288/1.0065374 |
URI | http://hdl.handle.net/2429/14459 |
Degree |
Master of Applied Science - MASc |
Program |
Electrical and Computer Engineering |
Affiliation |
Applied Science, Faculty of Electrical and Computer Engineering, Department of |
Degree Grantor | University of British Columbia |
GraduationDate | 2003-05 |
Campus |
UBCV |
Scholarly Level | Graduate |
AggregatedSourceRepository | DSpace |
Download
- Media
- 831-ubc_2003-0369.pdf [ 5.35MB ]
- Metadata
- JSON: 831-1.0065374.json
- JSON-LD: 831-1.0065374-ld.json
- RDF/XML (Pretty): 831-1.0065374-rdf.xml
- RDF/JSON: 831-1.0065374-rdf.json
- Turtle: 831-1.0065374-turtle.txt
- N-Triples: 831-1.0065374-rdf-ntriples.txt
- Original Record: 831-1.0065374-source.json
- Full Text
- 831-1.0065374-fulltext.txt
- Citation
- 831-1.0065374.ris
Full Text
Cite
Citation Scheme:
Usage Statistics
Share
Embed
Customize your widget with the following options, then copy and paste the code below into the HTML
of your page to embed this item in your website.
<div id="ubcOpenCollectionsWidgetDisplay">
<script id="ubcOpenCollectionsWidget"
src="{[{embed.src}]}"
data-item="{[{embed.item}]}"
data-collection="{[{embed.collection}]}"
data-metadata="{[{embed.showMetadata}]}"
data-width="{[{embed.width}]}"
async >
</script>
</div>
Our image viewer uses the IIIF 2.0 standard.
To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.831.1-0065374/manifest