calibration of the Electronic Speed Controllers (ESCs), along with serial and I2C ..... available choices, the selected Microcontroller board is Arduino Due. ..... for example, can be used to linearize a non-linear system description locally or some.
Altitude Control of a Quadcopter
By
Aniqa Mukarram Usman Amin Fiaz Uzair Ijaz Khan
Department of Electrical Engineering Pakistan Institute of Engineering & Applied Sciences Nilore, Islamabad, Pakistan June 2015
Altitude Control of a Quadcopter
By
Aniqa Mukarram Usman Amin Fiaz Uzair Ijaz Khan
(Thesis submitted to the faculty of Engineering in partial fulfillment of requirements for the Degree of B.S. Electrical Engineering)
Department of Electrical Engineering Pakistan Institute of Engineering & Applied Sciences Nilore, Islamabad, Pakistan June 2015
i
Department of Electrical Engineering Pakistan Institute of Engineering and Applied Sciences (PIEAS) Nilore, Islamabad 45650, Pakistan
Declaration of Originality We hereby declare that the work contained in this thesis and the intellectual content of this thesis are the product of our own work. This thesis has not been previously published in any form nor does it contain any verbatim of the published resources which could be treated as infringement of the international and copyright law. We also declare that we do understand the terms ‘copyright’ and ‘plagiarism’, and that in case of any copyright violation or plagiarism found in this work, we will be held fully responsible of the consequences of any such violation.
Signature: _____________________ Name: ________________________ Date: _________________________
Signature: _____________________ Name: ________________________ Date: _________________________
Signature: _____________________ Name: ________________________ Date: _________________________
Place: ________________________
ii
Certificate of Approval This is to certify that the work contained in this thesis entitled “Altitude
Control of a Quadcopter” was carried out by Aniqa Mukarram Usman Amin Fiaz Uzair Ijaz Khan
under our supervision and that in our opinion, it is fully adequate, in scope and quality, for the degree of BS Electrical Engineering from Pakistan Institute of Engineering and Applied Sciences (PIEAS).
Approved By: Signature: _______________________________ Supervisor: Dr. Muhammad Aqil
Signature: _______________________________ Co-Supervisor: Dr. Muhammad Abid
Verified By: Signature: ________________________ Head, Department of Electrical Engineering Stamp:
iii
Dedicated to our beloved parents…
iv
Acknowledgements Gratitude and endless thanks to Almighty Allah, who bestowed mankind, the light of knowledge through laurels of perception, learning and reasoning, in the way of searching, inquiring and finding the ultimate truth. To whom we serve, and to whom we pray for help. We feel our privilege and honor to express our sincere gratitude to our supervisor Dr. Muhammad Aqil for all his kind help, guidance, suggestions and support through the development of this project. We would also like to express our endless thanks to cosupervisor Dr. Muhammad Abid for all his guidance and support in completion of the project. With due respect, we would also like to thank project panel and coordinator for useful discussions. We would like to thank Mr. Shahzad Nadeem as well for his kind support. Special mentions for Mr. Noman Masood as he was the supervisor of the earlier projects done in the same line of action. We would also like to thank Pakistan Institute of Engineering and Applied Sciences for providing very conducive educational environment. Finally, we wish to thank our families for all their support and encouragement during our studies.
v
Table of Contents Abstract ......................................................................................................................viii List of Figures .............................................................................................................. ix List of Tables .............................................................................................................. xi List of symbols ............................................................................................................ xii Chapter 1 : Introduction ............................................................................................. 1 1.1. Unmanned Aerial Vehicles (UAVs) .................................................................. 1 1.1.1.
Micro Aerial Vehicles (MAVs) ............................................................. 1
1.1.2.
Multi-rotor UAVs .................................................................................. 1
1.2. Literature Review............................................................................................... 2 1.3. Scope of the Thesis ............................................................................................ 4 1.4. Thesis Outline .................................................................................................... 4 Chapter 2 : Mechanical Design ................................................................................... 5 2.1. Basic Design Requirements ............................................................................... 5 2.2. Quadrotor Dynamics .......................................................................................... 5 2.3. Configuration ..................................................................................................... 6 2.4. Construction ....................................................................................................... 6 2.4.1.
Basic Requirements ............................................................................... 7
2.4.2.
Material Selection .................................................................................. 7
2.5. Rotor Blades / Propellers ................................................................................... 7 Chapter 3 : Electronics ................................................................................................ 8 3.1. Microcontroller Unit (MCU) ............................................................................. 8 3.2. Motors and Electronic Speed Controllers (ESCs) ........................................... 10 3.3. Inertial Measurement Unit (IMU) .................................................................... 10 3.3.1.
L3G4200D Three-Axis Gyroscope ...................................................... 11
3.3.2.
ADXL345 Three-Axis Accelerometer ................................................. 11
vi
3.4. User Interface ................................................................................................... 12 3.5. I2C Interface .................................................................................................... 12 Chapter 4 : Mathematical Modeling ........................................................................ 13 4.1. Introduction ...................................................................................................... 13 4.2. Equations of Motion ....................................................................................... 14 4.3. Effective Input Sources .................................................................................... 15 4.4. State Space Representation .............................................................................. 15 4.5. Verification of the Model................................................................................. 16 Chapter 5 : Controller Design .................................................................................. 18 5.1. PID Based Controller ....................................................................................... 18 5.1.1.
Reasons for Choosing PID ................................................................... 18
5.1.2.
Feedback Loop ..................................................................................... 18
5.1.3.
Tuning of PID Parameters ................................................................... 19
5.1.4.
5.1.3.1.
Simulation .................................................................... 19
5.1.3.2.
Results .......................................................................... 21
5.1.3.3.
Discussion .................................................................... 22
Optimization ........................................................................................ 22 5.1.4.1.
Altitude Control Optimization ..................................... 22
5.1.4.2.
PID with External Loops ............................................. 25
5.1.4.3.
Optimized Response with Disturbance ........................ 26
5.2. State Feedback Controller ................................................................................ 28 5.2.1.
Linearization of Quadcopter System ................................................... 28
5.2.2.
Pole Placement ..................................................................................... 31
5.2.3.
5.2.2.1.
Simulation Results ....................................................... 32
5.2.2.2.
Simulation Response with Disturbance ....................... 33
Linear Quadrature Regulator (LQR) .................................................... 34 5.2.3.1.
Simulation Results ....................................................... 35
vii
5.2.4.
5.2.3.2.
Simulation Response with Disturbance ....................... 36
5.2.3.3.
Effects of Variation in Q and R ................................... 38
Verification for a Non-Linear Model ................................................... 39
Chapter 6 : Hardware Implementation .................................................................. 41 6.1. Actuators .......................................................................................................... 41 6.2. Transceiver ....................................................................................................... 42 6.3. Orientation Data ............................................................................................... 42 6.3.1.
Data Acquisition from Accelerometer ................................................. 42
6.3.2.
Data Acquisition from Gyroscope ....................................................... 43
6.4. PID Tuning....................................................................................................... 43 6.5. Testing.............................................................................................................. 43 Chapter 7 : Summary ............................................................................................... 45 7.1. Targets Achieved ............................................................................................. 45 7.2. Conclusion ....................................................................................................... 45 Chapter 8 : Future Recommendations .................................................................... 46 Appendix ..................................................................................................................... 47 References ................................................................................................................... 50
viii
Abstract The objective of this project is to achieve the altitude control of a Quadcopter. This dissertation expounds on kinematics and dynamics of Quadcopter. It states modifications in the mechanical structure of an airframe and suggests a suitable mathematical model for the system. The electronics designed in this regard includes the Pulse Width Modulation (PWM) and power distribution boards, programming and calibration of the Electronic Speed Controllers (ESCs), along with serial and I2C interfaces for user operation and Inertial Measurement Unit respectively. A Proportional Integral Derivative (PID) based control system is designed to obtain the altitude control of the airframe. The validity of the proposed control system is carried out by simulation studies, which shows the satisfactory altitude control of the Quadcopter. For improved response, advanced controllers are designed and simulated via pole placement method and Linear Quadrature Regulator (LQR) methods. A comparison is made between the responses of both control strategies. The hardware involves the embed control system on the actual platform. Future recommendations may involve the design, simulation and implementation of further advanced controllers using Linear Quadrature Gaussian (LQG) and Sliding Mode Control which can be implemented directly to nonlinear model of the system. This can be followed by a comparison of performance via LQG and Sliding Mode Control techniques.
ix
List of Figures Figure 1-1: A Quadrotor UAV ....................................................................................... 2 Figure 2-1: Basic Quadrotor Dynamics ......................................................................... 5 Figure 2-2: Cross and Plus Quadcopter Configurations ................................................ 6 Figure 2-3: The Actual Platform with Motors Mounted on the Airframe ..................... 7 Figure 3-1: On-board Electronic Module Interconnects ................................................ 8 Figure 3-2: Arduino Due Board ..................................................................................... 9 Figure 3-3: Hobby Wing 40A ESC.............................................................................. 10 Figure 3-4: The IMU Module ...................................................................................... 11 Figure 3-5: FMS 6 Channel Transmitter/Receiver ...................................................... 12 Figure 3-6: Schematic of I2C Protocol ......................................................................... 12 Figure 4-1: Detailed Dynamics of a Quadrotor ........................................................... 13 Figure 4-2: Simulink Diagram for Open Loop Response ............................................ 16 Figure 4-3: Open Loop Response of the System ......................................................... 17 Figure 5-1: Simulation Diagram for the Closed Loop System .................................... 19 Figure 5-2: Closed Loop System Response ................................................................. 21 Figure 5-3: Optimized Response for Altitude .............................................................. 23 Figure 5-4: Optimized Response for Roll .................................................................... 24 Figure 5-5: Optimized Response for Pitch................................................................... 24 Figure 5-6: Optimized Response for Yaw .................................................................. 25 Figure 5-7: Controller Design with External loops...................................................... 25 Figure 5-8: Altitude Step Response for Optimized values ......................................... 26 Figure 5-9: Disturbance in all four controllers ........................................................... 27 Figure 5-10: Altitude Control with disturbance .......................................................... 27 Figure 5-11: DC Servo Design (Type I) ...................................................................... 32 Figure 5-12: State Feedback Controller Using Pole Placement Method .................... 33
x
Figure 5-13: DC Servo Design (type I) with disturbance ........................................... 33 Figure 5-14: State Feedback Controller Using Pole Placement with Disturbance ..... 34 Figure 5-15: DC Servo Design (type I) with LQR Gain ............................................. 35 Figure 5-16: Step Response of State Feedback Controller Using LQR Gain ............. 36 Figure 5-17: DC Servo Design (type I) with LQR gain and disturbance ................... 36 Figure 5-18: Response of State Feedback Controller Using LQR with Disturbance . 37 Figure 5-19: Effect of variation of Q and R ................................................................. 38 Figure 5-20: Implementation of State Feedback Controller on Non-Linear Model .... 39 Figure 5-21: Step Response for Non-Linear Model ................................................... 39 Figure 5-22: Response for Non-Linear System (LQR) with Q = 4*eye(12) ............... 40 Figure 5-23: Response for Non-Linear System (LQR) with Q = 4*eye(12),z = 0 ..... 40 Figure 6-1: Speed Calibration of motors and ESCs .................................................... 41 Figure 6-2: Threaded Quadcopter Flight .................................................................... 43 Figure 6-3: First Unthreaded Flight ............................................................................ 44 Figure 6-4: Flight Testing ........................................................................................... 44
xi
List of Tables Table 5-1: Parameters of PID ..................................................................................... 20 Table 5-2: Physical Constants ...................................................................................... 20 Table 5-3: Design Requirements ................................................................................. 23 Table 5-4: Optimized PID gains ................................................................................. 23
xii
List of Symbols x
forward position in body coordinate frame
y
lateral position in body coordinate frame
z
vertical position in body coordinate frame
X, Y, Z
position in world coordinate frame
u1
vertical thrust generated by the four rotors
u2
pitching moment
u3
yawing moment
u4
rolling moment
Ѳ
pitch angle
Ψ
yaw angle
Φ
roll angle
m
mass of the Quadcopter
Ki
drag coefficients for the system
fi
thrusts generated by four rotors
Ii
moments of inertia with respect to the axes
C
force to moment scaling factor
𝛚 = [𝛚𝒙 𝛚𝒚 𝛚𝒛 ]
𝑻
Euler angles
angular velocity vector of the Quadcopter resolved into body frame
KP
PID proportional gain
KI
PID integral gain
KD
PID differential gain
L
half the length of the Quadcopter
G
acceleration due to gravity
mp
linear momentum
1
Chapter 1 : Introduction 1.1. Unmanned Aerial Vehicles (UAVs) A UAV, an abbreviation of Unmanned Aerial Vehicle, sometimes also referred as Unpiloted Aerial Vehicle is a self-powered flying object with no human pilot. These vehicles find their immense applications in several fields, among them military and surveillance are of significant importance. These vehicles are generally classified into two main categories:
Remotely Piloted Aircrafts
Autonomous Aircrafts
In both categories, the common thing is that there is no human directly involved or present in the flight environment. In the 1st one, the UAV is controlled and guided through the way by humans through some sort of wireless communication. However in the 2nd category the vehicle is made intelligent enough to fly and perform the designated task by itself. Both the cases however highlight the extreme feasibility of UAVs to be used in applications where environment is not friendly to human life.
1.1.1.
Micro Aerial Vehicles (MAVs)
Among UAVs, there exists a popular class called the Micro Aerial Vehicles (MAVs). These are characterized by their small physical dimensions making them highly portable and convenient to use even in congested areas.
1.1.2.
Multi-rotor UAVs
One basic division in UAV Classification is on the basis of their landing and take-off abilities, i.e. ones with the Conventional Take-off and Landing (CTOL) capability and the others with the Vertical Take-off and Landing (VTOL) capability. Now a days, most of the commercially used UAVs are of the VTOL category. This capability provides the aircraft to land and take-off in places where conventional vehicles cannot. But CTOL category UAVs are popular for their speed and high load carrying capabilities.
2
However research and specific application demands make VTOL aircrafts the ultimate choice. Among this class of UAVs, Multi-rotor UAVs are the most popular ones because of their higher stability in flight and easy control. Quadrotor is also a type of Multi-rotor VTOL type UAV. It is a fixed pitch multi rotor air craft. The choice for fixed pitch rotors is explained by the need of tedious aerodynamic design and control strategy in case of UAVs such as Helicopters etc. that use variable pitch rotors. Figure 1-1 shows a basic Quadrotor UAV.
Figure 1-1: A Quadrotor UAV [15]
1.2. Literature Review The development of UAVs is a very vast and active research area in Control Theory. One reason is its importance in practical applications such as:
Remote sensing
Commercial aerial surveillance
Commercial and motion picture filmmaking and Sports
Oil, gas and mineral exploration and production
Search and rescue
Scientific research
Armed attacks, etc.
3
The main reason for the interest of people linked to control engineering, however, lies in its ability to act as a testing platform for various control techniques. So far, different control strategies such as, Robust Control, Adaptive Control, Model Predictive Control, and Feedback Linearization have been used to design its control system. The different papers published on the Quadcopter system vary in their design and implementation techniques like structure, pay load capacity, degree of autonomy, state estimation and control techniques. Most of them employ, Inertial Navigation System for the state estimation however Sensor Fusion can also be used for that, which has being exploited by the senior control students in recent years. The brief description of a few already employed popular control techniques is provided. Pennsylvania State university has done two projects on Quadrotor [10] [11]. Two control techniques are studied – one employing a series of mode-based, linearized feedback controllers and the other utilizing a back-stepping control law. The system developed in Middle East Technical University for attitude control of the Quadrotor is controlled with a Linear Quadratic Regulator (LQR) and Proportionate Differentiate (PD) controller [12]. Another Setup has been established in the Department of Electrical and Computer Engineering, University of British Columbia Vancouver, BC, Canada [13]. This project emphasized on the nonlinear modeling of a Quadrotor UAV. The setup consists of a system with a flying mill, a DSP system, a microprocessor to be programmed and a wireless transceiver that have been used to test the flight controller. Based on the nonlinear model, an H∞ loop shaping controller is implemented for flight stability, heading speed, and altitude control. The work done at Swiss Federal Institute of Technology (ETH) [4], focuses mainly on the mechanical design, modelling, sensors, and control of an indoor VTOL autonomous robot. The Autonomous Flying Vehicle (AFV) project at Cornell University [14] was developed to produce a reliable autonomous hovering UAV. Initially an Extended Kalman Filter was designed for state estimation but this filter proved to be too
4
complicated to implement, and thus a square root implementation of a Sigma Point Filter (SRSPF) was performed.
1.3. Scope of the Thesis The objective of the work in this thesis is to utilize the existing modeling and estimation techniques, to design and implement a control system for altitude control of a Quadcopter. At the initial stages of this work, the goal was set to design a PID controller for the system and design and develop the necessary electronics for its implementation. The targets achieved also include the design and review of advanced controllers based on the Pole Placement and the Linear Quadrature Regulator (LQR) techniques, and implementation of the PID controller on the actual system. This report provides an insight to the goals that have been achieved along with shedding some light on the future prospects of the project as well.
1.4. Thesis Outline The following chapters in the report are organized as follows. Chapter 2 presents the improvements and modifications made in the mechanical design of already available platform. Chapter 3 gives the description of the major electronics involved in the implementation of control system. Chapter 4 provides an overview of the dynamic mathematical modeling for a Quadrotor. In Chapter 5, the controller design is discussed with supporting evidence of simulations and results focusing particularly on PID based and state feedback controllers. Chapter 6 sheds some light on the hardware implementation of the designed controller. Chapter 7 concludes the report with a summary of accomplishments so far and Chapter 8 provides some future recommendations as well. In last, Appendix provides an introduction to Euler-Newton Formalism. This thesis is much influenced by the earlier work [2] [3], they had worked on the same systems’ mechanical design and state-estimation techniques.
5
Chapter 2 : Mechanical Design The mechanical design had been fabricated before. We inspected the design at hand, looking for two things particularly; first, the design should be aerodynamically efficient and second if the structure is taut enough to resist vibrations, as it may cause the aircraft to fall down when at altitude.
2.1. Basic Design Requirements The design of airframe entailed following requirements:
Aerodynamic Structure
Capability to lift the load of up to 1 kg
Provide sufficient space for mounting electronics
Sturdiness to withstand collision during flight
2.2. Quadrotor Dynamics Figure 2-1 shows the design that was given for this project. The design follows basic kinematics and utilizes four motors which can be used either in cross configuration or plus configuration.
Figure 2-1: Basic Quadrotor Dynamics
6
As shown in the figure, the two pairs of opposite rotors rotate in the opposite direction which not only balances the torque but also removes the need for another rotor for stabilization. The change in altitude is brought by simultaneously increasing or decreasing the speeds of all the motors. The yaw movement is produced by reducing the speed of one pair of propellers in the same direction while increasing the speed of the other pair of propellers; this allows an unbalanced torque to be produced on the Quadrotor while keeping the downward thrust constant.
2.3. Configuration On the basis of heading and flying configuration, Quadcopter platforms are classified as Cross (x) category and Plus (+) category structures. Figure 2-2 illustrates the difference among the two.
Figure 2-2: Cross and Plus Quadcopter Configurations
Because of easier control and more heading speed capabilities, the X configuration is generally preferred as was done in our case.
2.4. Construction The construction and material selection of the mechanical platform were derived earlier on the basis of requirements that are particular to a UAV for its desired operation.
7
2.4.1.
Basic Requirements
The basic requirements that were identified for construction of Quadrotor are:
Light weight
Strong and well balanced structure
Easy-to-construct and easy-to-repair structure
Provision of installing all the necessary accessories now and in future
2.4.2.
Material Selection
The materials available for fabrication were Carbon fiber, Glass Fiber and Aluminum. After taking into account the density, strength, cost and availability in the market, Aluminum was chosen. It is readily available and has a density of 2.70 g/cc, whereas its yield strength is 7-11 MPa.
2.5. Rotor Blades / Propellers Two types of rotor blades are used, rotating in clockwise and anti-clockwise directions, which are essential for balancing the counter torque imposed by the rotors on the Quadcopters. Choice of chosen blades depended upon the thrust they produced in addition to their availability. Figure 2-3 shows the actual airframe with the motors and blades mounted as well.
Figure 2-3: The Actual Platform with Motors Mounted on the Airframe
8
Chapter 3 : Electronics There are three main electronic units in our Quadcopter system. The Microcontroller Unit (MCU) for the implementation of controller, the Electronic Speed Controllers (ESCs) for the speed control of the brushless DC motors and finally the Inertial Measurement Unit (IMU) for providing the orientation data for state estimation. The selection and functionality of MCU and ESCs and their operation is described in this chapter, whereas the ongoing study on IMU is also discussed. Figure 3-1 depicts the basic inter-connects between different electronic hardware modules.
Figure 3-1: Block Diagram Showing On-board Electronic Module Interconnects
3.1. Microcontroller Unit (MCU) The microcontroller unit is like a brain of every digital system. It does all the calculations and all the important tasks. In our case, the speed control, the state estimation and the altitude control all is to be done by the MCU. Lots of choices for MCUs are available in the market depending upon the application. In our case, the selection is based on the ease of programming, interfacing and the fulfilment of the requirements of the task. After careful analysis, survey and comparison with the available choices, the selected Microcontroller board is Arduino Due.
9
Its main features are as follows.
32-bit ARM core AT91SAM3X8E-MCU
54 Digital I/O Pins (of which 12 provide PWM output)
CPU Clock Frequency up to 84MHz
12 Analog Input Pins
A DMA controller, dedicated to memory intensive tasks.
2x12 bit (DAC) Analog Outputs
512 KB Flash Memory
96 K Bytes of SRAM
The board is shown in figure 4-1 below.
Figure 3-2: Arduino Due Board
Arduino comes with a free programming environment. Code can be written and compiled with in the environment and using an on-board boot loader, the code can be easily burned into the flash memory.
10
3.2. Motors and Electronic Speed Controllers (ESCs) The Actuators used in the project are 3015-7T Out-runner Brushless DC Motors from Tower-Pro Corporation. The main features are as follows.
11000 rpm rated full load speed
400W, 10V, 40A rating
2 kg lift force
Hobby-wing 40A speed controllers (Figure 3-3) are used as speed controllers for the brushless motors. Having a current rating of 40A, these ESCs operate at the input PWM of 50Hz. The speed of the motors varies from zero to maximum for 5 - 10 % duty cycle of the input PWM.
Figure 3-3: Hobby wing 40A ESC
3.3. Inertial Measurement Unit (IMU) It is the most important part of a Quadcopter system, because it provides the orientation data for state estimation. The controller feedback also comes to the MCU from this unit. The inertial measurement unit used in our project is GY- 80 (Figure 3-4) embedded IMU module. It is a 10 DOF IMU with 3-axis Accelerometer, 3-axis Gyroscope, and 3-axis Digital Compass for orientation data.
11
Figure 3-4: The IMU Module
3.3.1.
L3G4200D Three-Axis Gyroscope
A gyroscope provides instantaneous value of angular velocity on a given axis when the sample is requested. The data is then calibrated for appropriate state estimation. The important features of L3G4200D are:
Three selectable full scales (250/500/2000 dps)
I2C/SPI digital output interface
16 bit-rate value data output
Embedded FIFO
3.3.2.
ADXL345 Three-Axis Accelerometer
A three axes accelerometer provides the instantaneous value of acceleration along the three axes of sensor module. The data is then calibrated for appropriate state estimation. The important features of ADXL345 are:
Selectable range from ±2g to ±16g
Free fall detection
SPI (3 and 4 wire) and I2C digital interfaces
12
3.4. User Interface FMS 2.4 GHz 6 Channel Transmitter/Receiver (Figure 3-5) is used for providing user interface with the control system. The altitude control is provided in a linear way with the throttle provided by the user. The other channels provide control for pitch, roll and safety turn on/off privileges to the user.
Figure 3-5: FMS 2.4 GHz - 6 Channel Transmitter/Receiver
3.5. I2C Interface 0I2C stands for Inter-Integrated Circuit. It is a
form of serial communication with one master and several slave devices. The protocol generally employs two lines; SDA-the data line, and SCL-the clock line. A typical I2C interface is shown in figure 3-6.
Figure 3-6: Schematic of I2C
In our project this I2C interface is used in the data acquiring process from IMU sensor module.
13
Chapter 4 : Mathematical Modeling The approach followed in this section is such that the Quadcopter body axes system is described initially which is then followed by a brief description of the forces acting on and affecting the flight of the vehicle. The section also includes the equations of motion of the Quadcopter.
4.1. Introduction The mathematical model of the Quadcopter describes its dynamics in a simplified manner. If we model all the effects on the behavior of Quadcopter during the flight, the model would be considerably more complicated resulting in more complex simulation. The mathematical model is taken from the already existing literature reference
[6]
.
Figure 4-1 shows the basic structure of Quadcopter along with the world coordinate system, the coordinate Quadcopter system, angular velocity directions of each rotor, and the torque and tension forces generated by rotors.
Figure 4-1: Detailed Dynamics of a Quadrotor
The aim here is to develop a model of the Quadcopter as realistically as possible. Basically, a Quadrotor has four rotors with fixed angles which represent four input forces, or more generally, the thrust generated by each of the four propellers as shown in Figure 4-1.
14
4.2. Equations of Motion The following equations of motions represent the Quadcopter system [2][3][6],
ẋ =
dx dt
ẏ =
dy dt
ż =
dz dt
ẍ = u1 (cosφ sinѲ cosψ + sinφ sinψ) – 𝐾1
ẋ m
ÿ = u1 (sinφ sinѲ cosψ + cosφ sinψ) – 𝐾2
ẏ m
z̈ = u1 (cosφ cosψ) − g − 𝐾3
ż m
(4-1)
Ѳ̇ = ωx ψ̇ = ωy φ͘ = ωz Ӫ = u2 − l𝐾4
Ѳ̇ I1
ψ̈ = 𝑢3 − l𝐾5
ψ̇ I2
φ̈ = 𝑢4 − 𝐾6
φ͘ I3
where x, y and z represent the translational motion of the Quadcopter in x, y and zplane respectively and φ, Ѳ, ψ represent the Euler angles for rotational motion. Here, variables with a single dot on top (ẋ, ẏ, ż) represent the velocity while those with a double dot (ẍ, ÿ, z̈) represent the acceleration in the respective directions.
15
4.3. Effective Input Sources Physical working of an aerial vehicle such as the Quadcopter involves controlling the speed of each of its four rotors independently in order to attain the desired altitude. Here in this section, we model these input speeds as a function of the thrusts of these motors. Following are the equations of four effective control inputs. These four equations represent the thrusts of the four motors in combinations. The first equation, u1, represents the total thrust. A step input to u1 would change the altitude of the Quadcopters. The second equation, u2, represents the rolling moment whereas the third equation for u3 represents the pitching moment input. The fourth equation, for u4, represents the yawing movement.
u1
=
(f1 + f2 + f3 + f4)
u2 =
𝑚 𝑙(−f1− f2 + f3 + f4) I1
(4-2)
u3 =
𝑙(−f1+ f2 + f3− f4)
u4 =
C(f1− f2 + f3− f4)
I2
I3
where fi are the thrusts generated by the respective rotors, Ii refers to the moment of inertia, m represents the mass of quadcopter, l the length of one side, and C is the force to moment scaling factor.
4.4. State Space Representation This section includes the state space representation of the Quadcopter system which allows us to readily use the proposed model to be utilized for controller design. If we write the state vector as: x = [x
y
and the input vector as:
z
ẋ
ẏ
ż
Ѳ
ψ
φ
ωx
ωy
ωz]T
(4-3)
16
u = [f1
f2
f3
f4]T,
(4-4)
we may write the dynamic equations in standard state space form as: 𝐱̇ = 𝐟(𝐱, 𝐮),
(4-5)
where x is a vector function of appropriate dimensions. The first six states in equation 4-3 represent the translational motion of the Quadcopter [2]. It is represented here in Cartesian coordinates and can be conveniently related to the output of the position sensors. The remaining six states represent the orientation of the Quadcopter which includes the Euler angles describing the rotation of the vehicle corresponding to the aeronautical convention; where ϴ is the pitch angle (rotation around y-axis), ψ is the yaw angle (rotation around z-axis), and φ is the roll angle (rotation around x-axis).
4.5. Verification of the Model A step input is applied to the four effective control inputs described in section 4.3.
Figure 4-2: Simulink Diagram for Open Loop Response
The open loop step response for translation in x, y and z direction is shown below.
17
1 0.8 0.6
Amplitude / Units
0.4 0.2 0 -0.2 -0.4 -0.6 -0.8 -1
0
1
2
3
4
5
6
7
8
9
10
Time / seconds
(a) 1 0.8 0.6
Amplitude / Units
0.4 0.2 0 -0.2 -0.4 -0.6 -0.8 -1
0
1
2
3
4
5
6
7
8
9
10
6
7
8
9
10
Time / seconds
(b)
0
-50
Amplitude / Units
-100
-150
-200
-250
-300
-350
-400
-450
(c)
0
1
2
3
4
5
Time / seconds
Figure 4-3: Open Loop Response of the System (a) Translation in ‘x’ Direction, (b) Translation in ‘y’ Direction, (c) Translation in ‘z’ Direction
This response verifies that our suggested model clearly depicts the characteristics of an actual Quadcopter and thus can be used as the basis of its controller design.
18
Chapter 5 : Controller Design 5.1. PID Based Controller Proportional Integral Derivative (PID) is one of the techniques that can be employed on the given hardware. The plant is controlled by PID based control system for now. The hardware at hand has the potential of employing more advanced control techniques such as Linear Quadrature Regulator (LQR) control or Sliding Mode Control.
5.1.1. Reasons for choosing PID PID controller was designed and implemented for the previously stated equations. The rationale behind the selection of a PID Controller is its simplicity and easy implementation. The PID gains can be designed upon the system parameters if they are estimated precisely. Moreover, the PID gains can be designed by treating the system as a ‘Black Box’ and by just tracking the error. However, one needs to balance all three parameters of PID and in an attempt to do so, one may compromise transient response of the system, such as settling time, overshoots etc.
5.1.2. Feedback Loop The Quadcopter plant represents an under-actuated system. The system has four virtual control inputs as defined earlier [5], which are u1, u2, u3 and u4. These four input states are used to monitor twelve states, described above in Equation 4-3. For the sake of simplicity, initially, four states were monitored using the four control inputs and it was assumed that controlling these four states will lead to control of the whole Quadcopter. PID was implemented on all four inputs with different gains. Height and the three angles (yaw, pitch and roll) were fed back and the errors served as input to the PID controllers as shown in the figure. All the states, needed to be controlled, were fed back the variable as inputs in their differential equations. For u1 (first step input), z (height) is fed back into it accordingly with the differential equations stated above. Similarly,
19
for u2, u3 and u4 the Euler angles, theta, psi and phi, are fed back into the feedback loop respectively.
5.1.3. Tuning of PID Parameters 5.1.3.1.
Simulation
The Simulink Diagram (Figure 5-1) shows how simulation was carried out and how the feedback loops were made.
Figure 5-1: Simulation Diagram of the Closed Loop System
20
Table 5-1: The tuned parameters of the proposed PID based controllers.
Controller
Gains of Controller
Number Controller 1
KP 10
KI 5
KD 1
Controller 2
1
0
0
Controller 3
10
10
0
Controller 4
1
0
0
Table 5-2: Values of the constants involved in the system.
Constants
Units
Value
I1
Ns2/rad
1.25
I2
Ns2/rad
1.25
I3
Ns2/rad
2.5
K1
Ns/m
0.010
K2
Ns/m
0.010
K3
Ns/m
0.010
K4
Ns/rad
0.012
K5
Ns/rad
0.012
K6
Ns/rad
0.012
M
Kg
2
L
M
0.2
G
ms-2
9.8
Here I1, I2, I3 are the moment of inertia in respective directions, x, y, and z. Similarly the Ks represent the drag coefficients in each direction. Here ‘m’ represents mass of the Quadcopter, ‘G’ represents the gravitational acceleration and ‘l’ represents half length of the Quadcopter.
21
5.1.3.2.
Results
A step input was applied to the four effective control inputs. The step response for translation in x, y and z direction along with the rotation in Yaw angle (theta) is plotted here. 1
1 Ideal Response Actual Response
0.6
0.6
0.4
0.4
0.2 0 -0.2
0.2 0 -0.2
-0.4
-0.4
-0.6
-0.6
-0.8
-0.8
-1
0
2
4
6
8
Ideal Response Actual Response
0.8
Amplitude / Units
Amplitude / Units
0.8
-1
10
0
2
4
6
8
10
Time / seconds
Time / seconds
(a)
(b)
1.4
1
Ideal Response Actual Response
1.2
Ideal Response Actual Response
0.8 0.6 0.4
Amplitude / Units
Amplitude / Units
1
0.8
0.6
0.2 0 -0.2 -0.4
0.4
-0.6
0.2 -0.8
0
0
2
4
6
8
10
-1
0
Time / seconds
(c)
2
4
6
8
10
Time / seconds
(d)
Figure 5-2: Closed Loop System Response (a) Translation in ‘x’ Direction, (b) Translation in ‘y’ Direction, (c) Translation in ‘z’ Direction, (d) Variation in Yaw Angle
22
The above results show that the height (variable ‘z’) reached the desired value of step input, which is 1. The states x, y and yaw angle showed no change and they remained constant at zero.
5.1.3.3.
Discussion
A step input to all four inputs (motors) means that there will be thrusts generated by all four motors, which should raise the height of the Quadcopter, provided it is able to overcome the inertia. In an uncontrolled response, the Quadcopter would not have achieved altitude without showing change in its three Euler angles (theta, psi and phi). If the Euler angles would have changed from zero, there would have been an increase in x and y variable, causing instability in the plant. The ideal response would have been that height variable (z) would reach its desired value, but with a better transient response. The ideal response should show little or no change in parameters other than height of the Quadcopter, when given a step input. The above response is the same as ideal response in case of x, y and yaw angle (theta). The response for height can be improved by further fine tuning of PID controller or by using an advanced control technique, like Sliding Mode control technique.
5.1.4. Optimization 5.1.4.1.
Altitude Control Optimization
PID controllers were further tuned for optimal response using the Simulink Optimization toolbox. The design requirements were given and subsequently iterations were done to ensure that the response comes inside the given bounds. Each controller was optimized independently at first, and their optimized values were plugged in afterwards for the best possible response. The tables 5-3 and 5-4 below list the design requirements and subsequently the optimized PID gains which appeared as a results of these requirements.
23
Table 5-3: Design Requirements
Initial value
0 seconds
Final value
0.9884 seconds
Step time
0 seconds
Response Time
12 seconds
Rise time
3.2312 seconds
% Rise
83.2973 %
Settling time
6.6705 seconds
% Settling
4.5446 %
% Overshoot
11.2954 %
% Undershoot
1.0118 %
Table 5-4: Optimized PID gains
P
I
D
Controller 1
9.7897
6.3017
0.9893
Controller 2
2.7588
-0.4779
3.7334
Controller 3
2.7588
-0.4779
3.7334
Controller 4
3.0341
-0.5870
3.7676
Figure 5-3: Optimized Response for Altitude
24
Figure 5-4: Optimized Response for Roll
Figure 5-5: Optimized Response for Pitch
25
Figure 5-6: Optimized Response for Yaw
5.1.4.2.
PID with External Loops
Figure 5-7: Controller Design with External loops
26
Two external loops were added to the already existing four internal loops, to control ‘x’ and ‘y’, as shown in the Figure 5-7. The idea was to control six variables with six controllers. The ‘x’ and ‘y’ variables depend on their angles psi and phi, hence the external loops were placed before the roll and pitch controllers, with the output variables ‘x’ and ‘y’ fed back to them. However, the results showed all parameters diverging and optimization could not be done for the values of PID gains in the controllers. This inability of PID controllers to provide enhanced control gives us the license to proceed to advance control techniques.
5.1.4.3.
Optimized Response with Disturbance
The Figure 5-8 shows the step response for altitude control of the non-linear system, when optimized values of PID controller were used. The minor ripples can be taken care of by using harder optimization bounds for the system.
Figure 5-8: Altitude Step Response for Optimized values
27
Figure 5-9: Disturbance in all four controllers
Figure 5-10: Altitude Control with disturbance
28
Disturbance was added to the system to model our non-linear system more accurately to the physical system we have at our disposal. The curve still follows the same trend as earlier. However, the graph is less smooth now and ripples have increased, which account for the presence of disturbance in the system. Such a verification of the optimized values of PID on the non-linear system with disturbance proves that the optimized values are good enough to be used in our real system, even though they would still require tuning. The response can still be improved by optimizing again for the gains of P, I and D but this time with harder bounds on the system.
5.2. State Feedback Controller 5.2.1. Linearization of Quadcopter System In the case of a non-linear control system like the Quadcopter system, it is found convenient to analyze its dynamics using the linearized mathematical model of the real system. For this purpose, the non-linear mathematical model is replaced with a linearized one. This linear approximation can be done in many ways. Differentiation, for example, can be used to linearize a non-linear system description locally or some kind of linear equivalent of a non-linear system can be derived for a particular input [16]. The text that follows uses the former approach to linearize the non-linear mathematical model of the Quadcopter already presented in this report. In order to obtain a linear mathematical model for a nonlinear system, we assume that the variables vary only slightly in their response from some operating condition. Consider a system whose input is x(t) and output is y(t). The relationship between y(t) and x(t) is given by: 𝑦 = 𝑓(𝑥)
(5-1)
If the normal operating condition corresponds to x̅, y̅, then Equation (5-1) may be expanded to a Taylor Series about this operating point: 𝑦 = 𝑓(𝑥̅ ) +
𝑑𝑓 𝑑𝑥
(𝑥 − 𝑥̅ ) +
1 𝑑2 𝑓 2! 𝑑𝑥 2
(𝑥 − 𝑥̅ )2 + ⋯
(5-2)
29
𝑑𝑓 𝑑2 𝑓
where the derivatives 𝑑𝑥 , 𝑑𝑥 2 , . . . are evaluated at 𝑥 = 𝑥̅ . The higher order terms may be neglected in cases where the difference between 𝑥 and 𝑥̅ is small. Equation (5-2) then becomes: 𝑦 = 𝑦̅ + 𝐾(𝑥 − 𝑥̅ )
(5-3)
𝑦̅ = 𝑓(𝑥̅ )
(5-4)
where
𝐾=
𝑑𝑓 | 𝑑𝑥 𝑥=𝑥̅
(5-5)
Equation (5-3) can be written as 𝑦 − 𝑦̅ = 𝐾(𝑥 − 𝑥̅ )
(5-6)
which tells that y − y̅is proportional to (x − x̅). Equation (5-6) gives a linear mathematical model for the non-linear system given by Equation (5-1) near the operating point (x − x̅), (y − y̅). A practical system like the Quadcopter system under consideration may have a large number of inputs. To tackle such a process, consider a non-linear system with two inputs x1 , x2 so the ouput y becomes: 𝑦 = 𝑓(𝑥1 , 𝑥2 )
(5-7)
Following the same procedure, equation (5-7) can be expanded into the Taylor series about the operating points x̅1 , x̅2 : 𝜕𝑓
𝑦 = 𝑓( 𝑥̅1 , 𝑥̅2 ) + [𝜕𝑥 (𝑥1 − 𝑥̅1 ) + 1
2
𝜕𝑓 𝜕𝑥2
(𝑥2 − 𝑥̅2 )] +
1 2!
𝜕2 𝑓 𝜕2 𝑓 (𝑥1 − 𝑥̅1 )(𝑥2 − 𝑥̅2 ) + 𝜕𝑥 2 (𝑥2 − 𝑥̅2 )2 ] 𝜕𝑥1 𝜕𝑥2 2
𝑑2 𝑓
[𝑑𝑥 2 (𝑥1 − 𝑥̅1 )2 + 1
+⋯
(5-8)
where the partial derivatives are evaluated at (x1 = x̅1 ), (x2 = ̅x2 ). The higher-order terms can be neglected near the normal operating point. So the linear mathematical model of this non-linear system in the vicinity of the normal operating condition is given by: 𝑦 − 𝑦̅ = 𝐾1 (𝑥1 − 𝑥̅1 ) + 𝐾2 (𝑥2 − 𝑥̅2 )
(5-9)
30
where 𝑦̅ = 𝑓(x̅1 , x̅2 ) 𝐾1 =
𝜕𝑓 | 𝜕𝑥1 (𝑥
𝐾2 =
𝜕𝑓 | 𝜕𝑥2 (𝑥
(5-10) (5-11)
𝑥2 ) 1 = 𝑥̅ 1 ),(𝑥2 = ̅
(5-12)
𝑥2 ) 1 = 𝑥̅ 1 ),(𝑥2 = ̅
The linearized system resulting from this technique is always a local estimate about the operating point. As the operating point varies, the linearized model also varies (for the same nonlinear system). That is why, if the operating conditions vary widely, such linear approximations are not valid and non-linear equations must be dealt with. It is important to be noted that a specific mathematical model utilized in analysis and design may precisely represent the dynamics of an actual system for some operating conditions, but may not be precise for other operating conditions. The usual objective of controller design using the linearized dynamics is “deviation minimization” (i.e. regulation). In the same way, in the case of Quadcopter, the linearization is accurate within a small region and most quadrotor controller research limits the flight envelop to stay within this trusted region [17]. For the Quadcopter system, the general form of the linearized system uses arbitrary values for all states. The state matrix A is filled with the partial derivatives with respect to each state while the B matrix constitutes of the partial derivatives with respect to each control input. ∆𝑋̇ = 𝐴 ∆𝑋 + 𝐵 ∆𝑈
(5-13)
∆𝑋 ≜ 𝑋 , ∆𝑈 ≜ 𝑈
(5-14)
𝑋̇ = 𝐴𝑋 + 𝐵𝑈
(5-15)
So,
𝜕𝑓
𝐴𝑛×𝑛 = [𝜕𝑥]
(𝑋0 ,𝑈0 )
=
𝜕𝑓1 𝜕𝑥1
⋮
𝜕𝑓𝑛 [𝜕𝑥1
⋯ ⋱ ⋯
𝜕𝑓1 𝜕𝑥𝑛
⋮
𝜕𝑓𝑛 𝜕𝑥𝑛 ](𝑋 ,𝑈 ) 0 0
(5-16)
31
𝜕𝑓
𝐵𝑛×𝑚 = [ ] 𝜕𝑈
(𝑋0 ,𝑈0 )
=
𝜕𝑓1 𝜕𝑢1
⋮
𝜕𝑓𝑛 [𝜕𝑢1
⋯ ⋱ ⋯
𝜕𝑓1 𝜕𝑢𝑚
⋮
𝜕𝑓𝑛 𝜕𝑢𝑚 ](𝑋 ,𝑈 ) 0 0
(5-17)
The table 5-2 shows the Quadcopter parameters whose values are required for the evaluation of matrices A and B.
5.2.2. Pole Placement In this method, closed loop poles of a plant are placed in pre-determined locations in the s-plane. Location of poles correspond directly to the Eigen values which in turn describe the behavior of the linear dynamical system. We have set the desired closed loop poles in P matrix. Based on those desired locations, the feedback gain K is calculated using the following MATLAB command. 𝐾 = 𝑝𝑙𝑎𝑐𝑒 (𝐴, 𝐵, 𝑃)
(5-18)
Through use of feedback, 𝑈 = −𝐾𝑥
(5-19)
one can attempt to change the behavior of the system in a way that it is more favorable. We can force the closed loop poles of the system to be at the desired locations (as stated in the P matrix), by constructing a feedback control system. We can tune this response by changing K, which we can do by changing the location of poles in P. If the closed loop transfer function of the system is represented by a state-space equation, 𝑥̇ = 𝐴𝑥 + 𝐵𝑢
(5-20)
𝑦 = 𝐶𝑥 + 𝐷𝑢
(5-21)
then the poles of the system are the roots of the characteristic equation given by 𝑑𝑒𝑡 |𝑠𝐼 − 𝐴| = 0
(5-22)
Substituting 𝑈 = −𝐾𝑥 in the above equation, we get 𝑥̇ = (𝐴 − 𝐵𝐾)𝑥
(5-23)
32
𝑦 = (𝐶 − 𝐷𝐾)𝑥
(5-24)
The roots of the characteristic equation of the pole placement based system are given by 𝑑𝑒𝑡 |𝑠𝐼 − (𝐴 − 𝐵𝐾)| = 0
(5-25)
However, the system needs to controllable in order to implement this method. A system is said to be controllable at time t0 if it is possible by means of an unconstrained control vector to transfer the system from an initial state x(t0) to any other state in a finite interval of time.[18] For a system to be controllable, the following matrix [𝐵 | 𝐴𝐵 | . . . 𝐴𝑛−1 𝐵]
(5-25)
which is called the controllability matrix needs to be of rank n or should have n linearly independent column vectors. For this purpose, the MATLAB command of controllability, ctrb, was used. 𝐶𝑜 = 𝑐𝑡𝑟𝑏 (𝐴, 𝐵)
(5-26)
𝑈𝑛𝐶𝑜 = 𝑙𝑒𝑛𝑔𝑡ℎ(𝐴) − 𝑟𝑎𝑛𝑘(𝐶𝑜)
(5-27)
The matrix UnCo came out to be zero in our case, meaning the system is controllable.
5.2.2.1.
Simulation Results
Gain3 Scope1 K*u Step
Uniform Random Number x' = Ax+Bu y = Cx+Du State-Space
K*u Gain1
Figure 5-11: DC Servo Design (Type I)
K*u Gain2
Scope
33
State Feedback Controller implemented using Pole Placement Method 7
6
Altitude (units)
5
4
3
2
1
0 0
1
2
3
4
5 Time (s)
6
7
8
9
10
Figure 5-12: State Feedback Controller Implemented Using Pole Placement Method
The graph shows a large overshoot with an acceptable settling time. The overshoot is large enough to cause problems when implementing on a physical system. The response can be improved by changing the ‘K’ for pole placement, by changing the location of poles.
5.2.2.2.
Simulation Response with Disturbance
Gain3 K*u
Scope1
Step
x' = Ax+Bu y = Cx+Du State-Space
Uniform Random Number
K*u Gain2
K*u Gain1
Figure 5-13: DC Servo Design (type I) with disturbance
Scope
34
State Feedback Controller implemented using Pole Placement Method with Disturbance 7
6
Altitude (units)
5
4
3
2
1
0 0
1
2
3
4
5 Time (s)
6
7
8
9
10
Figure 5-14: State Feedback Controller Implemented Using Pole Placement Method with Disturbance
The graph stays somewhat the same and shows no major differences because of disturbance. A possible explanation of this is that the small disturbance used has been suppressed by the dominating overshoot. The response, however, still is not acceptable for implementation on a physical system which gives us the license to shift to further advanced control techniques.
5.2.3. Linear Quadrature Regulator (LQR) In this method, the response is defined by ratio of Q & R. Based on the ratio, the feedback gain k is calculated. The states are fed back with this gain to achieve the desired closed loop response. We can tune this response by changing k, which we can do by changing the ratio of Q & R. It involves determination of an input signal which will take a linear system from the initial state 𝑥(𝑡0 ) to a final state 𝑥(𝑡𝑓 ), while minimizing a quadratic cost function. The cost function here is the time integral of a quadratic form in the state vector x and input vector u such as (xTQx + uTRu) where Q is non-negative definite matrix and R is a positive definite matrix.
35
There is no systematic method to choose Q and R. Hence identity matrix were chosen at first, the system was simulated for closed loop response and weights were adjusted. One has to know that greater is the weighting parameter, the smaller is the weighting signal. The main advantage of an LQR-based controller is that the optimal input signal u(t) is obtainable from full state feedback i.e. 𝑈 = −𝑘𝑥
(5-28)
for some k matrix. The feedback matrix k is obtained by solving the Riccati equation of the system. However, one of the disadvantages of the LQR-based controller is that obtaining an analytical solution to the Riccati equation can be difficult at times. In our case, a MATLAB command was used to compute the feedback matrix k. 𝑘 = 𝑙𝑞𝑟 (𝐴, 𝐵, 𝑄, 𝑅)
(5-29)
Another, more practical, disadvantage of the LQR-based controller is that the LQR design does not put any restrictions on the input signal u(t) amplitude. The optimizing input often turns out to have amplitudes which are impractical e.g. negative thrust in our case. Also, full state feedback is difficult to obtain, one is likely to have few output measurements only from which one has to infer the state information via the state observers, which can complicate the system.
5.2.3.1.
Simulation Results Gain3 K*u
Step
x' = Ax+Bu y = Cx+Du State-Space
K*u Gain2
K*u Gain1
Figure 5-15: DC Servo Design (type I) with LQR Gain
Scope
36
1.1
1
Altitude (units)
0.9
0.8
0.7
0.6
0.5
0.4
0
1
2
3
4
5 Time (s)
6
7
8
9
10
Figure 5-16: Step Response of State Feedback Controller Implemented Using LQR Gain
The step response plotted in Figure 5-16 shows the system settling at an altitude of z = 1 from an initial condition of z = 0.5 units. The settling time is under 6 seconds. Overshoots are not present, however a minor undershoot is present in the step response.
5.2.3.2.
Simulation Response with Disturbance Gain3 K*u Step
x' = Ax+Bu y = Cx+Du State-Space
K*u Gain2
Scope
K*u Uniform Random Number
Gain1
Figure 5-17: DC Servo Design (type I) with LQR gain and disturbance
37
1.2 1.1 1
Altitude (units)
0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2
0
1
2
3
4
5 Time (s)
6
7
8
9
10
Figure 5-18: Step Response of State Feedback Controller Implemented Using LQR Gain with Disturbance
Random disturbance was added in the system, as such disturbances exist during practical implementations. Hence, addition of such a disturbance models our system more accurately. The step response has somewhat deteriorated as shown in Figure 518, but however reaches the level z = 1, in a similar way as the previous graph. Presence of disturbance in the system accounts for slower convergence of the response and hence greater settling time.
38
5.2.3.3.
Effects of Variation in Q and R Effect of Changing Q and R
1
0.9
Altitude (units)
0.8
0.7
0.6
0.5
0.4
0
1
2
3
4
5 Time (s)
6
7
8
9
10
Figure 5-19: Effect of variation of Q and R
The LQR-based response rely on the ratio of Q and R. If both matrices are multiplied by the same constant, the end result plotted in Figure 5-19, will not be much different as the ratio of Q and R will stay the same. Hence, Q and R were taken as identity matrix and later multiplied with different constants to find the optimum Q and R matrices for the best possible response. From the results obtained, it is safe to conclude that there exists a trade-off between Q and R. Increasing R more than Q, helps with reducing the undershoots but increases the settling time. The graph above in figure [insert figure number] proves the same that for Q= 4 x eye (12) and R = 16 x eye (4), the undershoot decreases and settling time increases but the graph still converges to 1 as expected.
5.2.4. Verification for a Non-linear Model For the verification of our state feedback controller, the controller is applied on the nonlinear process of our plant given by the S-function.
39
Scope
Scope3
Scope4
Scope5
Gain3 K*u
QuadCoptor
K*u
Step1
Gain2
Scope1
Level-2 MATLAB S-Function1
K*u Gain1
Figure 5-20: Implementation of State Feedback Controller on Non-Linear Model
Non-Linear System Response 4
3
Altitude (units)
2
1
0
-1
-2
0
1
2
3
4
5 Time (s)
6
7
8
9
10
Figure 5-21: Step Response for Non-Linear Model
The plot shown in Figure 5-21, though depicts an acceptable settling time, but with massive undershoots which are undesirable, it needs amendment. A tuning is therefore proposed to improve the response.
40
Non-linear System (LQR) with Q = 4*eye(12) 3 2.5 2
Altitude (units)
1.5 1 0.5 0 -0.5 -1 -1.5
0
1
2
3
4
5 Time (s)
6
7
8
9
10
Figure 5-22: Step Response for Non-Linear System (LQR) with Q = 4*eye(12)
The gain ‘k’ used in LQR-based controller was changed by using Q = 4 x Identity matrix. This resulted in an improved settling time. However, large impractical undershoots remain in the system which are high undesirable. Non-Linear System (LQR) with Q = 4*eye(12) and z = 0 initially 3.5 3 2.5
Altitude (units)
2 1.5 1 0.5 0 -0.5 -1
0
1
2
3
4
5 Time (s)
6
7
8
9
10
Figure 5-23: Step Response for Non-Linear System (LQR) with Q = 4*eye(12), z = 0 IC
The initial conditions of the system were now varied. The graph now shows improvements in undershoots, as they have decreased significantly. Slight initial overshoots still remain in the system which still need to be catered by fine tuning. A minor problem in the above response is the presence of a steady state error. This can be catered by using an integrator before the Gain3. [18]
41
Chapter 6 : Hardware Implementation The implementation involved the combination of the designed controller with the electronics discussed earlier in Chapter 3. Starting from design and fabrication of PCBs for power distribution and signal communication, the PID based control system was implemented on the actual platform employing the commands from radio transceiver and feedback from the IMU.
6.1. Actuators The motors and ESCs needed to be calibrated for different input PWM pulse widths. Since the available ESCs did not come with a datasheet so, their manual testing and calibration was performed and minimum and maximum pulse widths were calculated. Further the operational frequency of the speed controllers was 50Hz while the default PWM frequency from Arduino is 490 Hz. This issue was tackled by reprogramming the Arduino timers on register level to obtain desired frequency.
Figure 6-1: Speed Calibration of motors and ESCs
42
6.2. Transceiver There are two basic needs for a distant user interface in case of a Quadcopter system. 1st and the most important is the safety of the operator during testing phase in particular and 2nd is the need for a user command to maintain a specific desired altitude. The FMS Radio Transceiver serves this purpose. The 6 channels have been employed to provide various input signals to the system for user operation. The principle is based on the fact that the MCU detects the pulse width of incoming signal from the receiver and calls an interrupt service routine (ISR) accordingly. Although the goal of the project was to achieve altitude control only but the privilege of other controls like that of roll and pitch have also been provided for manual user operation.
6.3. Orientation Data In particular, Arduino comes in very handy for the acquisition of orientation data from the Inertial Measurement Unit. Built in libraries are available that include the configuration files for GY-80 IMU; i.e. ADXL345 (3 axis Accelerometer) and L3G4200D (3 axis Gyroscope). In obtaining the information from both sensors, noise and other issues needed to be resolved to acquire correct orientation data. I2C communication is used for this since it is the simplest and most convenient way of doing that. Arduino dedicated Two Wire Lines SDA and SCL serve this purpose quite efficiently. The data obtained from both components is then employed to obtain the roll and pitch angles that are then used in the PID feedback loop for altitude control along with the average gyro data.
6.3.1.
Data Acquisition from Accelerometer
ADXL345 is being used at an update rate of 50Hz with sensitivity level of ±16g, since this higher range is suitable for fast speed tracking. A high pass filter is employed for reduction of noise caused by the physical disturbances. The offset values along the three axis are subtracted from the data for correct estimation of acceleration. From the instantaneous acceleration along x, y and z axis, the pitch and roll angles are calculated using math arc tangent function. Since, our main goal is to achieve altitude control, so there is no need for the yaw angle at the moment.
43
6.3.2.
Data Acquisition from Gyroscope
L3G4200D is being used at an update rate of 800 Hz for fast tracking of the platform orientation. The digital data from gyroscope is bit abrupt because of the inherent vibrations and speedy rotation of the four rotors. This issue is tackled by averaging out the data using a moving average filter along with a high pass filter afterwards for removal of mechanical noise. DC offset in also present in gyro data as was the case with accelerometer that needs to be subtracted from each data value.
6.4. PID Tuning The PID parameters are deduced from the simulations based on actual system specifications and physical dimensions. Five PID control loops are implemented as were proposed by the design i.e. Pitch (x), Roll (y), Yaw (z) and ‘x’ and ‘y’ angles. The corresponding gains that are KP, KI and KD were initially calculated using the optimization techniques discussed in Chapter 5. However, they needed to be tuned again for the actual system operation.
6.5. Testing The test flight was initially performed using threaded quadcopter. This was to prevent any damage in case of any loss of control.
Figure 6-2: Threaded Quadcopter Flight
44
The first testing revealed some problems were evident. PID was not working for missing accuracy in some gyroscope data. It was found that problem was because of the slow update rate i.e. 400 Hz which was shifted to 800 Hz later for correct results. The unthreaded flight afterwards first resulted in a Quadcopter inability to climb up straight. The observation was combined from both flights as the lack of proper take off.
Figure 6-3: First Unthreaded Flight
Later on with fine PID tuning and some modifications in mechanical structure, a steady and stable flight of the Quadcopter was achieved. Figure 6-4 shows some screen shots from the final stable flight.
Figure 6-4: Flight Testing
45
Chapter 7 : Summary 7.1. Targets Achieved
Understanding mathematical modeling of the Quadcopter system
Design of Proportional Integral Derivative (PID) based control system
Simulations for the designed PID controller
Design and fabrication of requisite electronics
Implementation of PID controller on the hardware
Design and Simulations for Advanced control technique, Linear Quadratic Regulator (LQR), based control technique
Comparison of proposed control techniques
A publication was also proposed on the work done in this project.
Project was also nominated for National ICT R&D Funding (Result not announced yet).
7.2. Conclusion Mechanical design of the Quadcopter was revisited and minor corrections were made. Batteries and motors were tested and made operational after generating PWM from the Microcontroller Unit. Moreover, the serial communication interface for user operation and I2C interface for the sensors was also achieved. Meanwhile, the Mathematical modeling for the system was understood. PID controller was designed and simulated for the system. The parameters of the controller were optimized to get the best possible system response. In a bid to improve the system response, advanced control techniques like Pole Placement technique and Linear Quadrature Regulator (LQR) were visited for a linearized model. The verification of the control law was done by using it on the nonlinear model later on. PID based control system was implemented on the physical system. The performance of the implemented controller was improved by manual tuning of the PID gains.
46
Chapter 8 : Future Recommendations Future recommendations involve implementation of state feedback controller on the physical system. Linear Quadrature Regulator (LQR) is one of the techniques that has shown promise in the simulations and hence seems fit to be implemented on the actual system. Linear Quadrature Gaussian (LQG), another control technique, is similar to LQR. It is rather an advancement of LQR in terms of efficacy. It can also be simulated and implemented on the physical system, as it is an advancement of LQR. Another possible avenue of exploration could be design, simulation and implementation of Sliding Mode Control. It is a non-linear control method that alters the system dynamics of a non-linear system by applying a discontinuous control signal. A comparison of the implemented control techniques would conclude the work, highlighting the pros and cons of each. Feasibility of each control strategy can be evaluated depending upon the application, considering the complexity they bring to the table. As far as the implementation is concerned, it is recommended to revisit the mechanical structure of the quadcopter. It will be easy to achieve flight control for a more rigid and light weight platform than was used till now.
47
Appendix Newton-Euler Formalism Newton-Euler equations give a global characterization of the dynamics of a rigid body which is subject to external forces and torques. The text that follows first reviews the standard derivation of the equations of rigid body motion and then examines its dynamics in terms of twists and wrenches. Consider a coordinate frame attached to the center of mass of a rigid body, relative to an inertial frame. Newton’s second law states that the rate of change of the linear momentum of a body is equal to the force acting on it which gives one of the translational equations of motion: 𝐝
𝐟 = 𝐝𝐭 (𝒎𝒑 )
(A1)
where f = force applied at the center of mass with the coordinates of f specified relative to the inertial frame, mp = linear momentum. As the mass of a rigid body is constant, the translational motion of its center of mass becomes: 𝐟 = 𝒎̇ 𝒑
These equations are independent of the angular motion of the rigid body because the center of mass of the body is used to represent its position. In the same way, the equations describing the body’s angular motion can be derived independent of the linear motion of the system. Consider the rotational motion of a rigid body about a point, subject to an externally applied torque τ. The change in angular
48
momentum is equated with the applied torque in order to derive the equations of angular motion: 𝝉 =
𝒅 𝒅𝒕
(𝑱 ′𝝎𝒔 ) =
⌈ ⌈⊔
(𝑹𝑱𝑹𝑻 𝝎𝒔 )
(A2)
where J ′ωs = angular momentum relative to an inertial frame, J ′ = instantaneous inertia tensor relative to the inertial frame, ωs = spatial angular velocity. Expansion and manipulation of terms in this equation leads us to the Euler’s equation: 𝑱′𝝎𝑺 + 𝝎𝑺 × 𝑱′𝝎𝑺 = 𝝉
(A3)
Equations (1) and (3) describe the dynamics of a rigid body in terms of force and torque applied at the center of mass of the object. However, the coordinates of the force and torque vectors here are written with respect to an inertial frame and not relative to a body-fixed frame attached at the center of mass. Similarly, the velocity pair (p,ωs) does not correspond to the spatial or body velocity, since p is not the correct expression for the linear velocity term in either body or spatial coordinates. So the Newton’s equation is rewritten using the body velocity and body force respectively as follows: 𝒗𝒃 = 𝑹𝑻 𝒑̇
(A4)
𝒇𝒃 = 𝑹𝑻 𝒇
(A5)
Using the expression for body force in equation (1) and then expanding the equation obtained gives the Newton’s law in body coordinates: 𝒎𝒗̇ 𝒃 + 𝝎𝒃 × 𝒎𝒗𝒃 = 𝒇𝒃
(A6)
In the same manner, Euler’s equation can be written in terms of the body angular velocity and the body torque respectively, as follows: 𝝎𝒃 = 𝑹𝑻 𝝎𝑺
(A7)
49
𝝉𝒃 = 𝑹𝑻 𝝉
(A8)
From here, a straightforward computation gives Euler’s equation in body coordinates: 𝑱 𝛚̇⌊ + 𝛚⌊ × 𝑱 𝛚⌊ = 𝛕⌊
(A9)
Note that in case of body coordinates, the inertia tensor is a constant and that is why J is used instead of J′. Combining equations (6) and (9) gives the equations of motion for a rigid body subject to an external wrench F applied at the center of mass and specified with respect to the body coordinate frame. This is called the Newton-Euler equation in body coordinates.
𝑚𝐼 0
0 𝒗̇ 𝒃 𝛚𝑏 + 𝒃 𝐽 𝛚̇⌊ 𝛚𝑏
𝑚𝑣 𝑏 = 𝐹𝑏 𝐽𝛚𝑏
(A10)
The Newton-Euler equation gives a global description of the equations of motion for a rigid body subject to an external wrench. Note that the linear and angular motions are coupled as the linear velocity in body coordinates depends on the current orientation.
50
References [1] A. S. Haider, “Design and Development of an Autopilot Based 3-DoF UAV Hover”, PIEAS, Islamabad, MS Thesis 2010. [2] M. Ahmed, “Design and Development of UAV Hover with Autopilot Based Control”, PIEAS, Islamabad, MS Thesis 2012. [3] D. Khatri, H. Faiz and M. Rizwan, “Design of Control System for Quadrotor UAV”, PIEAS, Islamabad, BS Thesis 2013. [4] S. Bouabdallah, “Design and Control of Quadrotors with Application to Autonomous flying”, EPFL, Lausanne, PhD Thesis 2007. [5] A. L. Salih, M. Moghavvemi, H. A. F. Mohamed and K. S. Gaeid, “Modelling and PID Controller Design for a Quadrotor Unmanned Air Vehicle”, Centre for Research in Applied Electronics (CRAE),University of Malaya. [6] R. Xu and U. Ozguner, “Sliding Mode Control of a Quadrotor Helicopter”, 45th IEEE Conference on Decision & Control, December 2006. [7] J. Li and Y. Li, “Dynamic Analysis and PID Control for a Quadrotor”, IEEE International Conference on Mechatronics and Automation, Beijing, China, August 7– 10, 2011. [8] C. Balas, “Modelling And Linear Control Of A Quadrotor”, Cranfield University [9] R. Murray, S.S. Sastri, Z.Li, “A Mathematical Introduction to Robotic Manipulation”, CRC, 1994. [10] S. D. Hanford, A Small Semi-Autonomous Rotary-Wing Unmanned Air Vehicle (UAV), Master thesis, December 2005. [11] E.Altuğ, Vision based control of unmanned aerial vehicles with applications to an autonomous four rotor helicopter, Quadrotor, PhD thesis, University of Pennsylvania, 2003.
51
[12] B.Çamlıca, Demonstration of a stabilized hovering platform for undergraduate laboratory, December 2004. [13] M. Chen, M. Huzmezan, A Combined MBPC/ 2 DOF H∞ Controller for a Quad Rotor UAV, Department of Electrical and Computer Engineering University of British Columbia Vancouver, BC, Canada, V6T 1Z4, 2003. [14] E. B. Nice, Design of a Four Rotor Hovering Vehicle, M.S. thesis, Cornell University, 2003. [15] J. Yeager, T. Areeckal, D. Smart, S. Leverance, T. Wilson and J. Limbacher “Quadrotor Research Platform”, Systems Engineering Research Laboratory, 2012. URL: http://systemsengineeringresearchlaboratory.org/projects/serl-quadrotor/ [16] Martin Enqvist, “Some Results on Linear Models of Nonlinear Systems”, Linköping
University,
SE–581
83
Linköping,
Sweden,
2003.
http://www.control.isy.liu.se/research/reports/LicentiateThesis/Lic1046.pdf [17] Justin M. Selfridge & Gang Tao, “A multivariable adaptive controller for a quadrotor with guaranteed matching conditions”, Systems Science & Control Engineering:
An
Open
Access
Journal,
Volume
2,
Issue
1,
2014.
http://www.tandfonline.com/doi/full/10.1080/21642583.2013.879050 [18] K. Ogata, Modern Control Engineering, 4th Edition, Pearson Education by
Prentice-Hall Inc. 2002.