Lulea University of Technology Department of Computer Science, Electrical and Space Engineering. Master Thesis. by Alexander Lebedev

Transcription

1 Julius Maximilian University of Würzurg Faculty of Mathematics and Computer Science Aerospace Information Technology Chair of Computer Science VIII Prof. Dr. Sergio Montenegro Lulea University of Technology Department of Computer Science, Electrical and Space Engineering Space Technology Chair of Space Technology Division Dr. Victoria Baraash Master Thesis Design and Implementation of a 6DOF Control System for an Autonomous Quadrocopter y Alexander Leedev Examiner: Examiner: Supervisor: Prof. Dr. Sergio Montenegro Dr. Anita Enmark Dipl.-Ing. Nils Gageik Würzurg,

2 Declaration I, Alexander Leedev, herey declare that this thesis is my own work and that, to the est of my knowledge and elief, it contains no material previously pulished or written y another author nor material which to a sustantial extent has een accepted for the award of any other degree or diploma of a university or other institute of a higher education, except where due acknowledgment has een made in the text. Würzurg, Alexander Leedev 1

3 Astract This thesis is dedicated to design and implementation of a 6DOF control system for a quadrocopter. At the eginning of the work the quadrocopter was analyzed as a plant and physical effects with ehavior of continuous /discrete elements were descried. Based on the mathematical equations, continuous time invariant nonlinear mathematical model was designed. This mathematical model was linearized to create a 6DOF control system and validated thought experiments y test enches and a flying prototype of the quadrocopter. For the control system design a pole-placement approach was chosen and ased on the linear validated model, with taking into account requirements to a settling time, an overshoot and a steady-state error, the control system was designed. Its ehavior was checked in simulation and showed adequate results. Afterwards designed control system was implemented as a script and incorporated in a soft, developed inside Aerospace Information Technology Department, University of Würzurg. Then series of experiments y test enches and the flying prototype were fulfilled. Based on comparing experimental and theoretical results a conclusion was made. At the end of the work advantages and drawacks of the control system were discussed and suggestions for future work were declared.

4 Acknowledgment First, I would like to express my sincerely gratitude to Prof. Dr. Montenegro, Dipl. Ing. Nils Gageik and all other team memers for their help and support during this project. Also I would like to thank Dr. Anita Enmark for her patients and advice. My sincere thanks to Ms. Shahmary and Ms. Winneack for their kind help. Finally I would like to thank Dr. Victoria Baraash for her support and understanding. 3

5 Tale of Contents Declaration Astracts Acknowledgment Tale of Contents Areviations Page Introduction Motivation and tasks of the work 1.3 State of the Art 1.4 Chapters overview Mathematical Model of the Quadrocopter 10.1 Analysis of the quadrocopter 10. Mathematical description of the quadrocopter elements Free motion of the quadrocopter 14.. External forces 3..3 Quadrocopter s actuators 9..4 Discrete elements Mathematical model of the quadrocopter 30 3 Design of the Control System Pole-placement method: Ackermann approach Linear time-invariant mathematical model of the quadrocopter Desired poles for nd order system Attitude control Design of controllers Simulation results Altitude control Design of controllers Simulation results 5 4 Implementation of the Control System Transfer functions for pitch and roll orientation Elements of the system A linear model for test ench Coefficients calculation and verification Controller Design for pitch and roll orientation 63 4

6 4..1 Implementation of the regulator Implementation of the controller Controller for yaw orientation The altitude control system 74 5 Conclusion and Recommendations 5.1 Conclusion 5. Recommendations for a future work Biliography 80 Appendix A: Calculations 83 Appendix B: Scripts 85 5

7 Areviations CoG CoM CST DOF EMF MM MoI TF ToI UAV YPR center of gravity center of mass Control System Toolox degree of freedom electromotive force mathematical model moment of inertia transfer function tensor of inertia unmanned aerial vehicle yaw-pitch-roll 6

8 Chapter 1 Introduction 1.1 Motivation and tasks of this work A quadrocopter is a flying oject, which changes its altitude and attitude y four rotating lades. Quadrocopters are a variation of multicopters, which are rotorcrafts. During 0 th century there were several attempts to implement manned quadrocopters, earliest known cases are in 19 y Etienne Oemichen in France [5] and y George Bothezat in USA [6]. However, during the progress in rotorcrafts industry, the helicopters with different schemes of rotors adjusting were chosen. In last decades, ecause of great achievements in technologies such as electronics, microcontrollers, motors, sensors and software, an opportunity of uilding small unmanned aerial vehicles (UAVs) ecame wide world availale. This one leads to growing research and engineering interest to quadrocopters, which can e easily uilt. Nowadays quadrocopters are used mostly as toys, ojects for teaching purposes in universities and for panorama video recording, ut ones have good prospects in other areas. For expansion of application areas they should e more autonomous and intelligent. They are planned to e used in rescue operations [8], as a fire-fighter [7] or working as a group for fulfillment tasks with general purposes [9]. Quadrocopters have advantages such as a high maneuveraility, a relatively cheap price and a simple construction and have a great potential for using as rootic autonomous devices. However, there are several prolems that should e solved or improved for making ones closer to real applications. One of these prolems is a real time 6DOF control system that can control a position and an orientation of a quadrocopter, its linear and angular velocities. Such type of the control system is very important for fulfillment series of tasks, e.g. grasping other ojects, tracking other ojects or transmitting video information aout other ojects. Some good results of controlling a quadrocopter ehavior were otained and demonstrated y GRASP laoratory of Pennsylvania University [30] and inside project Flying Machine Area from Zürich University [31]. 7

9 Herey, the main motivation of this project is creating real time a 6DOF control system. This control system should control a position and an orientation of a quadrocopter. For creating such time of the system, several tasks should e solved. At the eginning a mathematical model of a quadrocopter should e created. Then, ased on this mathematical model, a 6DOF control system should e designed. At the end, designed control system should e implemented as a code in a microcontroller for a real quadrocopter. A quadrocopter that will e under consideration in this thesis is the one from AQopterI8 project, which is developed at Aerospace Information Technology Department, University of Würzurg. 1. State of the Art A mathematical model of a quadrocopter consists of descriing rigid ody dynamics, kinematics of fixed and ody reference frames and forces applied to the quadrocopter. There are several variants of the model. Firstly they vary in descriing of rigid ody dynamics; it can e done y Euler equations [5], Euler- Newton approach [0] or Lagrangian approach [1]. Secondly they vary in end representations of kinematics and direction of z axis of ody reference frame. Thirdly, they differ in how many forces and other effects are taken into account. The most complete model is represented y S. Bouadallah [1], the simplest variant y R. Beard [5] and the variant in the middle y T. Luukkonnen [0]. Also some researchers simplified a model of a motor, which rotates a lade, as proportional coefficients [5], and some of them as a 1 st order transfer function [1]. A model for this thesis is ased on models from two works [0], [5]. Control designs used in many works are ased on the mathematical model. Usually, original model is linearized to linear continuous time invariant model [0, 5] or to discrete one [4]. A controller for attitude control is usually PD [3] and there are several variants for altitude control. Hover control represented y N.Michael and others [3] was chosen for the quadrocopter control. There are several variants for the structure of control system for a whole plant. The variant from N.Michael and others [3] was chosen. 8

10 1.3 Chapters overview In chapter Mathematical model of the quadrocopter several issues are discussed. At the eginning an analysis of quadrocopter physical processes is done and collected as one process. Then deferential equations descried each process are represented. Based on these equations transfer functions were otained and implemented as a model in Matla/Simulink. Chapter 3 Design of the Control System dedicates to choosing structures of controllers and calculation their coefficients. It starts from short discussion aout pole-placement approach. A method for choosing poles ased on quality requirements is discussed. Then calculation feedack coefficients y Ackerman method is represented. At the end, implementation in Matla/Simulink is descried and results of simulation are shown. Chapter 4 Implementation of the Control System contains information aout experiments for a validation the mathematical model and a controllers adjusting. Firstly, the validation of the mathematical model for the pitch/roll, the calculation for controllers for pitch/roll and a comparison of modeling and experimental results were represented. Afterwards, the same information aout the yaw was descried. Then experiments with a flying prototype were shown and compared. Chapter 5 Conclusion contains discussion of the results and recommendation for a future work. 9

11 Chapter Mathematical model of the quadrocopter.1 Analysis of the quadrocopter The quadrocopter consists of four sticks, where each two are set symmetrically and perpendicularly to each other. On the end of each stick, symmetrically to geometrical center of the quadrocopter, actuators that provide flying are set. Each actuator consists of a motor and a lade, where the lade is fixed to the motor s shaft (fig..1). Rotation of these lades can lead to qudrocopter s motion. fig..1 Structure of the quadrocopter The quadrocopter has 6DOF that means it has linear and angular motions. This complex motion (called free motion) can e fully determined y two vectors: a position vector pst and an orientation vectorort. The position vector has current position of the quadrocopter in Earth reference frame and the orientation vector has current orientation angles of the quadrocopter comparing to Earth reference frame. For calculation current values of pst the following parameters should e known: a vector of linear velocity v, a vector of linear acceleration a. For calculation the 10

12 current values of ort the following parameters should e known: a vector of angular velocity, a vector of angular acceleration. Also initial conditions of all 6 vectors mentioned aove should e known. In addition, external forces and torques, which could e sustituted y net force netf and net torquenet, lead to changing in the linear and the angular acceleration of the quadrocopter and these changing influences on the position and the orientation. Herey, for creating the mathematical model (MM) the equations, for calculation vectors pst and ort ased on vectors mentioned aove, should e declared (fig..) fig.. Logical diagram for calculation the position and the orientation of the quadrocopter There are three sources of external forces such as gravitational field, air drag and rotations of the lades in the air. These sources create a gravitational force F mg, a drag force F drag and a thrust force T respectively. External torque H is created only y lades rotation (fig.3). 11

13 fig..3 Logical diagram for calculation net force and net torque In total there four lades and four BLDC motors. Assume that motors are numered from 1 to 4. Each lade is rotated y the corresponding motor with particular angular velocity l i, where index i indicates the numer of the motor. Angular velocity of the motor s shaft is regulated y a power ridge and each power ridge is regulated y a microcontroller (fig..4). fig..4 Logical diagram for calculation angular velocities of the lades Herey, the whole process of moving a quadrocopter in 6DOf can e descried as: a microcontroller sets signals (analogous or digital) that are transferred through power ridges and motors to angular velocities of the lades. A rotation of the lades creates forces and torques that together with gravitational and drag forces change the quadrocopter position and orientation (fig..5). 1

14 fig..5 Logical diagram for creation the MM of the quadrocopter The controller needs feedack information such as the position, orientation and appropriate parameters (e.g. linear angular velocity) that should e measured y sensors. Based on desired values of the position and the orientation and current feedack values measured y the sensors the designed controller should generate appropriate values for the power ridges (fig..6). fig..6 Logical diagram for the MM and the controller It can e concluded that the MM is consists of transfer functions that descrie or estimate elements and processes shown on fig..6. So the ehavior of the 13

15 quadrocopter in 6DOF should e descried y differential equations. Then relationships etween angular velocities of the lades and net force and net torque should e shown. External forces that cannot e controlled: gravitational and drag forces should e discussed. Elements that are needed for creating force and torque for moving: analogous elements (lades, motors, power ridges) and digital elements (sensors, a microcontroller) should e descried. Based on the MM the controller can e designed.. Mathematical description of the quadrocopter elements The MM should approximate the process shown on fig..5. A description of this process includes free motion of the quadrocopter, influence from applied forces, how lades rotations are produced and effects of digital elements (sensors, a microcontroller)...1 Free motion of the quadrocopter For descriing the quadrocopter s free motion (process is shown on fig..), the theory of free motion of a rigid ody is used. According to this one, free motion of the rigid ody is considered as a complex motion, which consists of two simple motions: translation motion of a point with mass equals to the mass of the ody (point mass), where the point is any point of the ody angular rotation of the ody around a fixed point, where the point mass chosen aove is considered as the fixed one. Translation motion of a point is descried as: netf dp, (.1) dt where netf is a net force of all external forces applied to the point mass, p is linear momentum of the point mass and d dt is differential operator. Angular rotation of a ody around the fixed point can e descried as: 14

16 dl net, (.) dt where net is a vector sum of all external torques and L is an angular momentum of the ody. Translation motion of a point mass For descriing translation motion of a pointed mass a fixed reference frame should e chosen. In some aritrary point of the space, noted as O, a fixed reference frame XYZ is created. Position of a point mass in frame XYZ can e descried y radius vector r (fig..7). fig..7 Position of the point mass in XYZ fixed reference frame To apply eq. (.1) to this point mass, a linear momentum of the point mass should e descried: p m* v, (.3) where m is a mass of the point mass and v is a linear velocity of the point mass. With taking into account eq. (.3), eq. (.1) can e rewritten as: dv d r net F m* m* dt dt, (.4). The changing in position of the point can e expressed from eq. (.4) as: 15

17 r netf, (.5). m Scalar form of eq. (.5) is: r r r X Y Z netf m netfy m netfz m X, (.6). Angular rotation around a fixed point As it was mentioned aove, for descriing free motion of a ody, an angular rotation around a fixed point should e considered. Assume a fixed reference frame xyz with origin in a fixed point of a rigid ody (fig..8). fig..8 Rotation of a rigid ody around a fixed point in xyz fixed reference frame Rotation of the rigid ody with random shape around the fixed point can e descried in fixed reference frame xyz. For using eq. (.) the angular momentum of the ody should e calculated. The ody is considered as a system of point masses, where mass of each point is dm and position of any of these points can e determined y vector from origin of xyz till the element dm. In this case the angular momentum is: 16

18 L vdm * dm, (.7), [9] where is an integral in the volume of the ody, v dm is a linear velocity of particular element. Eq. (1.7) in non-vector form is: L y * v z * v * dm x z y L z * v x * v * dm, (.8), [9] y x z L x * v y * v * dm z y x where x, y, z are coordinates of an element dm (or in other words coordinates of correspondent radius vector ), element. Linear velocity of each element v dm can e descried as: v x, v y, v z are projections of the velocity of this y* zz* y vel z* x x* z, (.9), [9] x* yy* x where is angular velocity of the ody and x, y, z are its projections in xyz. By rewriting eq. (.7) with taking into account (.9) the angular momentum is: * ( y z )* dm * ( x* y)* dm * ( z * x)* dm, L y *( * y * x) z *( * x * z) * dm *( y z ) * x* y * z * x * dm x x y z x x y z x y z * ( z x )* dm * z * y * dm * x* y * dm, L z *( * z * y) x*( * y * x) * dm *( z x ) * z * y * x* y * dm y y z x y y z x y z x * ( x y )* dm * x* z * dm * y * z * dm, L x*( * x * z) y *( * z * y) * dm *( x y ) * x* z * y * z * dm z z x y z z x y z x y or in vector form: 17

19 L ( y z )* dm ( x * y)* dm ( z * x)* dm x x y ( * )* ( )* * * * y * L z ( z * x)* dm z * y * dm ( x y )* dm z L L x y dm z x dm z y dm J, (.10), [9] where J is so-called tensor of inertia (ToI) and its components can e rewritten as follow: ( z * x)* dm z * y * dm ( x y )* dm J zx J zy J zz ( y z )* dm ( x * y)* dm ( z * x)* dm J xx J xy J xz J ( x * y)* dm ( z x )* dm z * y * dm J yx J yy J yz, (.11), [9] where 1st index of J corresponds to the index of L and second one to the index of and J J ; J J ; J J ;. xy yx xz zx yz zy Each component of inertia tensor is a moment of inertia (MoI) around particular axis. These components are constant, since origin of reference frame xyz is connected to the ody. Tensor of inertia (ToI) can e simplified in a case if axes of reference frame xyz are coincident with principal axes of the ody (axes of symmetry). To provide this case for rotating ody, the axes of the reference frame should e fixed with the ody. Assume new reference frame x yz, which axes are coincidence with principal axes of the ody and origin is in the fixed point of the ody. In reference frame x y z components J J J 0 [9] and eq. (.10) and eq. (.11) can e rewritten as: xy xz yz L x J 0 0 x x x L Ly 0 J 0 * * y y y J, (.1), [9] L z 0 0 J zz z To derive the angular momentum, an equation for the relative motion is used: r r r, (.13), [1] a where r a is an aritrary vector in inertial reference frame, r is the same vector in ody (non - inertial) reference frame, is an angular velocity of the ody 18

20 reference frame in the fixed reference frame. Derivation of angular momentum dl dt according to eq. (.13), leads to so-called Euler s equation []: d L d( J * ) d net J * J * J *, (.14), []. dt dt dt Eq. (.14) in more detail form is: net x J x x x x J x x x net net y 0 J 0 * 0 0 * y y y y J y y y net z 0 0 J 0 0 zz z z J zz z J * * * z * * z y y y z x x x x J xx x J xx J J x J y * * * * * y y y J y y y J y y y J xx J zz x z, J z * * * z z z J zz z J zz z Jy * y J x x x * y or in short form: net J * J J * * x x x x z z y y y z net J * J J * * y y y y x x z z x z net J * J J * * z z z z y y x x x y, (.16), []. Eq. (.16) should e rewritten in the form for finding changing in angular velocity as: 19

21 x y z net x J * * zz J y y y z J net y J * * x x J zz x z J xx yy net z J * * y y J xx x y J zz, (.17), []. As it was mentioned aove, description of rotation in ody reference frame x y z instead of fixed reference frame xyz simplifies calculation of angular momentum L to eq..1. On the other hand ecause of this simplification, another equation, that links angular velocity in xyz and orientation x yz relatively to xyz, is also needed. Orientation of the ody is defined y unique rotation around instantaneous axis of rotation. This rotation can e considered as sum of three simple rotations. Sequences of simple rotations are not unique [4]. Commonly used sequence in Aerospace applications for flying ojects is yawpitch-roll (YPR) rotation, where angles e.g. noted as,, correspondingly (fig..9). fig..9 Orientation of the ody reference frame x yz y yaw-pitch-roll angles in fixed reference frame xyz Changes in orientation are connected with y following equation: 0

22 1 sn * t c * t x 0 c sn * y, (.18). [5] sn c 0 z c c Herey, for a lock 6DOF (from fig..) three equations are needed: eq. (.6) descries dynamics of linear motion, eq. (.18) descries kinematics of angular motion and eq. (.17) descries dynamics of angular motion. Tensor of inertia and mass of the quadrocopter For calculation ToI, a real structure of the quadrocopter ( fig..1) is simplified to the structure, which consists of spherical dense center with mass M, radius R and several point masses of mass m located at distance l (fig..10).[5] M fig..10 Simplified structure of the quadrocopter ToI of the simplified structure ased on eq. (.11) and eq. (.1) can e descried as: ( y z )* dm 0 0 J 0 0 xx J 0 ( z x )* dm 0 0 J y 0 y 0 0 ( x )* 0 0 J zz y dm, (.19) 1

23 where * M * R J x * * x J y y l m M and 5 * M * R. 5 J z z 4* l * mm Eq. (.19) supplements eq. (.17). In case of chosen approximation the mass of quadrocopter is: m M 4* m M, (.0), where m M is total mass of the motor and the lade, M is mass of the spherical dense or in other words mass of the rest parts of the quadrocopter. Eq. (.14) supplements eq. (.6). Summary of equations for motion in 6DOF Herey, ased on written in section.1, free motion of the quadrocopter can e represented in reference frame XYZ (fig..11). fig..11 The quadrocopter s free motion Parameters and functions of this motion can e determined in several steps. Initial conditions are set accordingly to current experiment (e.g. equal to zero). Then mass and ToI are calculated ased on eq. (.19) and eq.(.0). Afterwards dynamics and kinematics of orientation are calculated ased on eq. (.18) and eq. (.17) and dynamics of linear motion is calculated ased on eq. (.6).

24 . External Forces A position and an orientation of the quadrocopter can e found y eq. (.6) and eq. (1.17) when the external net force netf and external net torquenet are known. As it was mentioned efore there are three sources of external forces: gravitational field, air drag and rotations of the lades in the air, which lead to a gravitational force F mg, a drag force F drag and a thrust force T respectively. Forces and torques from lades can e controlled. Gravitational and drag force cannot e controlled. Gravitational forces Forces of gravitational field applied to a ody can e represented as a net gravitational force applied to a center of gravity (CoG) of the ody. Direction of this force is constant and pointed to the center of the Earth. Relation etween this force and acceleration of an oject corresponds to Newton law and can e written as: F m* g, (.1) mg where m is mass of the quadrocopter and g is gravitational acceleration. For simplification assume that the CoG coincides with the CoM of the quadrocopter. Air drag force When an oject moves through the air, it overcomes air resistant. Air drag force can e descried as: F C S v drag d * * *, (.),[8] where C d is drag coefficient, is mass density of the air fluid, S reference area of the oject, v is the speed of the oject relative to the air fluid. Coefficient C d, which depends on the shape of the oject, should e measured in advance in a wind tunnel. Drag coefficients for several shapes are well known and availale in the form of the tales, e.g. for square shape C d equals to 0.64 [8]. Mass density of the air depends on the height aove the see level, e.g. for 1 3

25 meter aove sea level with temperature aout 15 degree, air density equals to 1.6 [8]. For movement in 3D eq. (.) can e rewritten as: Forces from lades F C * * S* v * v, (.3), drag d Interaction etween a rotating lade and air can e descried y vortex theory [7]. Assume that a lade is rotating with some angular velocity l in counterclockwise direction. This rotation leads to producing a numer of forces. To find net forces, the lade surface is theoretically divided y small elements and the force that applied to an element represented as sum of vertical force horizontal force Q el (fig..1). Sum of all vertical forces thrust force T and sum of all horizontal force Q el as hu torque H. T el and T el can e sustituted y fig..1 Net force and torque from interaction etween a lade and the air The thrust force T and hu torque H can e descried as: T *, (1.4), [1] T l H *, (1.5), [1] H l where p and are proportional coefficients, which depends on air density, angle of lade and area of lade. 4

26 The quadrocopter has four actuators; each of them consists of a lade, a motor and a power ridge. Notate linear movement of the quadrocopter as forward, ackward, left and right and numer actuators from 1 to 4 (fig..13). Blades and 4 rotate in counterclockwise direction with angular speed, 4 while lades 1 and 3 rotate in clockwise direction with angular speed 1, 3. fig Quadrocopter structure These rotations lead to four couples of thrust forces and hu torques (fig..14). fig..14 Thrust and hu forces from each lade These 4 forces can e replaced y net thrust force T and 4 hu torques y net hu torque H (fig..15). 5

27 fig..15 Thrust force and hu torque applied to the quadrocopter (an aritrary direction of H is chosen) Thrust force T can e represented as: Hu torque H can e represented as: T T1 T T 3 T 4, (.6), [1]. l * T T 4 * 3 1 H 1 H 3 H H 4 H x H H y l T T, (.7), [1] H z where sign minus corresponds to negative direction of roll, pitch and yaw angles. Equation (.7) supplements eq. (.17) for calculation orientation of the quadrocopter, so (.17) can e rewritten as: y z 4 l * T T J z * * z J y y y z x J 3 1 l * T T J x * * x J zz x z J xx yy H1 H3 H H 4 J J J zz * * y y x x x y, (.8). For finding translation motion thrust force T should e represented in xyz as: 6

28 x yz where R is a rotation matrix. xyz Rotation matrix of YPR is: * * x y z R R R R xyz x y z Tx x yz T Ty Rxyz * T T z 1 x y z, (.9), c 0 sn c sn 0 0 c sn * * sn c 0 0 sn c sn 0 c c *c c * sn s sn * sn *c c * sn sn * sn * sn c * c sn * c c * sn *c sn * sn c * sn * sn sn * c c * c c *c sn * sn * sn c * sn c * sn c * sn * sn c * sn sn * sn *c c * c sn * sn sn * c * c, (.30), [4] c sn sn c * c where c and sn are areviations for cosine and for sine respectively. Thrust force is always aligned with z axis, therefore, with taking into account eq. (.30), eq. (.9) can e rewritten as: Tx 0 x yz x yz T xyz Ty Rxyz * T Rxyz * 0 T z T c *c sn * sn * s c * sn c * sn c * sn * sn 0 c * sn sn * sn *c c * c s * sn sn * c * c * 0 c sn sn c * c T 1 1 7

29 c * sn c * sn * sn s * sn sn * c * c * T c* c, (.31). With taking into account eq. (.31), gravitational force eq. (.1) and drag force eq. (.3), eq. (.6) can e rewritten as: r r r X Y Z c * sn c * sn * sn * T Cd * * S * v m s * sn sn * c * c * T Cd * * S * vy m c * c * T m* g Cd * * S * vz m X, (.3). Therey, the free motion of the quadrocopter in 6DOF can e represented as linear motion its center of the mass (CoM) with mass m in XYZ and angular rotation of the quadrocopter around CoM. The rotation is descried y rotation of the quadrocopter in x yz and orientation of x yz relatively to xyz, where axes of xyz are parallel to relative axes of XYZ (fig..15). fig..15 Motion of the quadrocopter in 6DOF The influence from applied forces is descried y eq. (.3) (instead of (.6)) and the influence from applied torques is descried y eq. (.8) (instead of.17). 8

30 ..3 Quadrocopter s actuators The actuator consists of a lade, a motor and a power ridge. The ehavior of the lade is descried in previous section y eq. (.4), eq. (.5). Angular velocities of the lades depend on angular velocities of corresponding motors. For calculation a shaft velocity of a rushless DC motor (BLDC) a mathematical model of the motor should e used. This model depends on motors construction. In opposite to rushed DC motor, a BLDC motor needs a control system for rotation of its rotor. Sometimes MM of BLDC is needed for creating such type of the control system. However, for current case the MM is needed to estimate relationship etween the input and output. For this purpose MM of BLDC can e sustituted y MM of rushed DC [6]. The MM of rushed DC motor is ased on four equations: di U i * R L * em, (.33) dt e k *, (.34) m T k * m i, (.35) T J* d, (.36). dt Eq. (.33) descries the effect, when applied voltage leads to current in the armature with resistance R and to inductance L and to ack EMF e m. Eq. (.34) indicates that ack-emf e m proportional to angular velocity of the motor s shaft, where k is ack-emf constant. Eq. (.35) denotes that produced torque is proportional to the produced current, where km is the torque constant. Eq. (.36) descries transferring from the torque to angular acceleration of the plant, where J is sum of the moment of inertia of the plant and motor shaft. For current case, the plant is the lade, which has minimal MoI, so the plant MoI can e omitted. A power ridge can e represented as: u k * u, (.37) PWM max 9

31 where u is input voltage to the BLDC, umax is maximum input voltage of BLDC applied to the power ridge and k PWM is percent of pulse-width modulation (PWM). Time delay of the power ridge can e neglected since time for changing the electrical signals is much smaller comparing to the time delay in mechanical part of the system. The power ridge can e considered as continuous element...4 Discrete elements The system has several discrete elements such as: a microcontroller and sensors. These elements are discrete in time and level. They should e sustituted y quantizers with level of discretizations ldm and ld s correspond to their calculation precision and time discretizations td and td correspond to their delays in time. These parameters can e calculated as: M s ld M 1 1 ; ld NmofBt S, (.38) NmofBt where NmofBt is length of the microcontroller s register in ites. td s 1, (.39) f where f is frequency of the sensor in Hz. td N * t, (.40), M where N is numer of instructions in a code and t ins is time for fulfillment of one instruction. ins.3 Mathematical model of the quadrocopter The MM of the quadrocopter consists of transfer functions (TFs) of elements descried in previous sections. Logical diagram of this model with corresponding equations is shown on fig

32 fig..16 Logical diagram of MM of the quadrocopter in 6DOF This model has the following simplifications: the quadrocopter is a rigid ody center of mass (CoM) is in the geometrical center of the quadrocopter ToI of the quadrocopter is approximated as moment of inertia of several ojects CoG is coincided with CoM MoI of the lades is neglected Time delay of the power ridge is neglected For simulation purpose equations should e represented y transfer functions. TF will e represented in a form suitale for realization in Matla/Simulink. TF for eq. (.3) is: 31

33 1 c ( s)* sn ( s) c ( s)* sn ( s)* sn ( s) Cd * * S rx( s) * ( ) *( ( )) T s srx s s m m 1 s ( s)* sn ( s) sn ( s)* c ( s)* c ( s) Cd * * S ry( s) * T ( s) *( sr ( )) Y s, (.41) s m m 1 c ( s)* c ( s)* Cd * * S rz( s) * T ( s) g *( sr ( )) Z s s m m where s is Laplace operator. This TF is nonlinear since it has trigonometric functions and squaring of the inputs. Based on eq. (.41) it can e represented in Matla/Simulink (fig..17) fig..17 Representation of TF for translation motion in Matla/Simulink Block TransTtoXYZ (fig..17) has as inputs the thrust force Ts () and orientation angles,,. These inputs shoud e calculated for finding a quadrocopter s position. The TFs for orientation angles are calculated ased on eq. (.18) and eq. (.8) as: 3

34 ( s) 1 sn ( s)* t ( s) c ( s)* t ( s) x () s s ( s) 0 c ( s) sn ( s) * y ( s), (.4) ( s) sn ( s) c ( s) 0 z () s c( s) c( s) l J J s s T s T s s s zz y y x ( ) * ( ) 4( ) * ( )* ( ) y z Jx x J x x l J J s s T s T s s s x x zz y ( ) * 3( ) 1( ) * ( )* ( ) x z Jy y J y y 1 J J s ( s) * H ( s) H ( s) H ( s) H ( s) * ( s)* ( s) y y x x z x y Jz z J zz,(.43). These TFs are nonlinear since they have trigonometric functions and multiplying of the outputs. They can e represented in Matla/Simulink as shown on fig. (.18). fig..18 Representation of TF for angular rotation in Matla/Simulink The input of this lock is hu torque Hs. () The inputs Ts () and Hscan () e calculated ased on eq., (.4), (.5), (.6), (.7). The TFs are: T ( s) * ( s); T ( s) * ( s); T ( s) * ( s); T ( s) * ( s), (.44) 1 T l1 T l 3 T l3 4 T l 4 H ( s) * ( s) ; H ( s) * ( s) ; H ( s) * ( s) ; H ( s) * ( s), (.45) 1 H l1 H l 3 H l3 4 H l 4 33

35 T( s) * ( s) ( s) ( s) ( s), (.46) T l1 l l3 l 4 l * T ( s) T 4( s) 3 1 H 1( s) H 3( s) H ( s) H 4( s) Hx () s H y ( s) l * T ( s) T ( s), (.47). Hz () s These TFs are nonlinear since they have squaring outputs. They can e represented in Matla/Simulink as it shown on fig. (.19). fig..19 Representation of TFs in Matla/Simulink for creating Ts () and Hs () MM of BLDC is designed ased on eqs. (.33-36). Applying Laplace transformation to eq. (.33), (.34) leads to: U( s) k * ( s) R sl and to eq. (.35), (.36) leads to: Is (), (.48) k * ( ) * ( ) m I s J s s. (.49) 34

36 Transfer function can e otained y comining eq. (.48) and (.49) as: * U( s) k * ( s) * ( ) ( ) J k * m J s s U s R sl * s ( s ) k * ( s R sl k ) 1 U ( s)* ( s), (.50) J * L J * R s * s k k k m m m where Gs () 1 J * L J * R s * s k k k m m is nd order TF. More common form of eq. (.50), where parameters J, L and k m are sustituted, is represented in more appropriate form as: k k Gs () J L J R J R L J R s s k k k k R k k or in short way * * * * s * s k * * * 1 m * e * m * 1 s s m m m * m * 1 k Gs (), (.51) m * e * s m * s 1 * where J R m k * is mechanical constant and e m k Implementation of TF ased on eq. (.51) is shown on fig..0. L R is electrical constant. fig..0 Representation of BLDC TF in Simulink 35

37 Therey the TF functions of continuous elements of the quadrocopter can e represented as shown on fig..1. fig..1 Representation of TF of continuous part in Matla/Simulink 36

38 Chapter 3 Design of the Control System A goal of a controller design is to reach a new plant s ehavior, which corresponds to desired quality requirements. Any controller design procedure is ased on information how inputs and outputs of the plant are connected. In most cases relation etween inputs and outputs is descried y mathematical model of the plant. The MM can e created y three methods: mathematical description of physical processes, identification procedure and a sum of two previous methods. MMs otained ased on these procedures are called white ox, lack ox and grey ox respectively [1]. Most of design procedures are ased on the linear MM of the plant and most popular procedures are root-locus method, pole placement method and y Bode diagrams. As soon as the quadrocopter should have intelligent control system, the pole-placement method from mentioned one is chosen. 3.1 Pole-placement method: Ackermann approach Pole placement method is a method, where a designed controller should change poles of characteristic equation of the MM to the poles that give the system desired quality such as a settling time, an overshoot, a steady state error. However, it should e mentioned that this method has serious limitations such as: sensitiveness to how adequate the model is and not oservaility of particular connected to real physical parameters. Advantage of pole placement method is that the controller designed y this method can e easily expanded to an optimal or adaptive controller. One of the simplest and direct ways to design a system with chosen poles is using Ackerman equation. This equation transfers state space model in control canonical form and calculate new coefficients for feedacks [11]. In general the TF look like: G * s * s n1 n1 1 0 p s n n1 s an 1 * s a1 * s a0, (3.1). The control canonical form for a 3 rd order system is represented on fig

39 fig. 3.1 Structure diagram of control canonical form [10] The state equations for n-order system in control canonical form are: x( t) A* x( t) B * u( t), (3.) y( t) C * x( t) D * u( t) where A, a0 a1 a an 1 D B 0 1, C 0 1 n 1, The idea of Ackerman approach is to calculate new coefficients K that supplement existing coefficients to make roots of a closed-loop system equal to desired poles. Consider the case when the input of the system is zero, so called regulator control. In this case ut () consists only of feedack signals and can e descried as: u( t) Kx( t), (3.3). 38

40 So close-loop matrix (3.): Af can e written y sustituting result of eq. (3.3) to eq Af A BK * K1 K Kn 1 Kn a0 a1 a a n a0 K1 a1 K a K3 an 1 K Matrix K can e calculated as: n, (3.4) * *... n n * * * K B A B A B A B A, (3.5), where A is matrix polynomial formed with coefficients of the desired c characteristic equation e descried as: c s. The desired closed-loop characteristic equation can n n 1 s s s s s s s * * * 0, (3.6) c n n where 1 n are desired poles. Matrix polynomial A is descried as: c n n1 A A * A * A * I, (3.7), c n1 1 0 where I is identity matrix with dimension equals to dimension of A. Therey, pole placement procedure has three steps. First step is otaining the MM of the system, where the MM should e linear continuous time invariant model. Next step is calculation desired poles ased on requirements to quality of the c 39

41 system. Last step is calculation feedack coefficients that change poles of the MM to desired ones. 3. Linear time-invariant mathematical model of the quadrocopter The MM developed in chapter cannot e used for controller design y pole placement method and should e simplified. One of the approaches is known as small disturance theory [13]. Based on the assumption that motion of the flying oject consists of small deviations around a steady flight conditions, multiplication of angular velocity components can e omitted and eq. (.43) can e rewritten as: l s x ( s) * T( s) T4( s) J xx l s y ( s) * T3( s) T1( s) J yy 1 s z ( s) * H1( s) H3( s) H ( s) H 4( s) J zz, (3.8). Another simplification which leads from this fact is that changes in orientation angles equal to angular velocity and eq. (.4) can e rewritten as: () s x () s s ( s) y ( s), (3.9). () s z () s Comination of eq. (3.8) and eq. (3.9) can e shown as: l ( s) * T ( s) T ( s) sjxx sjyy sjzz 4 l ( s) * T ( s) T ( s) ( s) * H ( s) H ( s) H ( s) H ( s) 1 3 4, (3.10). Another assumption is that TFs etween PWM values and forces/torques can e sustituted y proportional coefficient or (if dynamics of the motors should e 40

42 taken into account) 1 st order TF. In the case of keeping dynamics into account, eq. (.44), (.45), (.51), (.37) can e rewritten as: 1 T * umax T () s k k kt * T * umax kpwm ( s) m * s 1 m * s 1 m * s 1, (3.11) 1 H * umax H () s k k kh * * u k ( s) * s 1 * s 1 * s 1 H max PWM m m m where k T * u * u. T max H max and kh k k In the case, if dynamics of the motors is small comparing to dynamics of whole system eq. (3.11) can e simplified to: T() s k () s PWM H() s k () s PWM k k T H, (3.1). Comination of eq. (3.10) and (3.1) gives: () s k k ( s) k ( s) s PWM 4PWM () s k k ( s) k ( s) s 3PWM 1PWM 1PWM 3PWM PWM 4PWM () s k k ( s) k ( s) k ( s) k ( s) s, (3.13) where l* kt k J, l* kt k xx J and kh k yy J. zz Eq. (.41) that descries quadrocopter s motion in XYZ can e simplified y neglected drag forces [5] and can e modified to: 41

43 1 c ( s)* sn ( s) c ( s)* sn ( s)* sn ( s) rx ( s) * ( ) T s s m 1 s ( s)* sn ( s) sn ( s)* c ( s)* c ( s) ry ( s) * T ( s), (3.14). s m 1 c( s)* c( s) rz ( s) * T ( s) g s m For design purpose this TF should e linearized to: 1 ( s)* c ( s)* sn rx ( s) * ( ) T s s m 1 ( s)* sn * c ry ( s) * T ( s), (3.15) s m 1 1 rz ( s) * T ( s) g s m where T( s) k * k ( s) k ( s) k ( s) k ( s). T PWM 1 PWM PWM 3 PWM 4 Therey, changes in the quadrocopter attitude and altitude can e descried y nd order functions from eq. (3.13) and eq. (3.15). So an approach for choosing desired poles for nd order system should e descried and controllers for nd order system y Ackerman equation should e designed. 3.3 Desired poles for nd order system Staility and quality of a system are determined y its characteristic equation. Choice of desired poles depends on required quality of the system. The simplest way of choosing poles for nd order system is to use standard approach descried e.g. in [11]. According to this approach a TF of nd order system should e represented as: Gs () s n * * n n, (3.16) where n is natural frequency and is damping ratio. 4

44 This TF is analyzed in time domain y step input. Based on the step response the quality of the system can e estimated. Typical step response with all specifications is shown on fig. 3.. fig. 3. Step response of a nd order system [10] The following quality parameters are shown: rise time, peak time, overshoot, settling time, steady state error. Usually for design purpose minimum settling time and overshooting should e specified. Percent of overshoot percovsh can e set y varying damping ratio (eq. 3.17) and settling time T s y varying natural frequency n (eq. 3.18). * / 1 percovsh e *100, (3.17) T s 4 *, (3.18). n For finding damping ratio and natural frequency n eq. (3.17) and (3.18) is rewritten as: 43

45 1 percovsh *ln *ln percovsh n, (3.0). * T s, (3.19) Based on and n the desired poles can e calculated as: s1, * * 1 n jn, (3.1). Therey, y choosing quality parameters: overshooting and settling timet s, the damping ratio and natural frequency n can e calculated (eq and 3.0). Based on the last ones the desired poles can e found y eq. (3.1). For control of the quadrocopter, as for most of other systems, the fastest response with minimal overshooting should e provided. The damping ratio for this ehavior is well known and equals to that corresponds to 4.3% of overshooting. Settling time in the model can e close to zero y choosing n close to infinity, ut in real system it is not possile. Based on current experimental result with the quadrocopter Ts should e no more than0.8s, so Ts 0.8s is chosen. Based on chosen quality parameters T 0.8s and percovsh 4.3% y using eq. (3.19, 3.0, 3.1) the desired poles are calculated as: s s1, j* (3.). The algorithm of calculation is realized as function Dpoles.m in Matla, where inputs are percovsh and settling time T s and outputs are desired poles (see App. B). 44

46 3.4 Attitude control Design of controllers Mathematical models from eq. (3.13) is nd order and in general can e represented as: G p * s 1 0 s s a1* s a0, (3.3). The state space equations in canonical form for a nd order system are: x( t) A* x( t) B * u( t), (3.4) y( t) C * x( t) 0 1 where A a0 a, 1 0 B 1, C, 0 1 that can e represented as shown on fig fig. 3.3 Canonical form of nd order system By comparing TF of quadrocopter (eq. 3.1) and nd order TF in general form (eq. 3.3) : 1* 0 s G p s G () s k, (3.5) s a s a s p 1* 0 where k in general represents k, k or k (calculation of k, k and k is in eq ). 45

47 It can e concluded that coefficients 0 equals to k and 1, a 0, a 1 equal to 0. So general state space form can e rewritten as: A , 0 B k, C 1 0, (3.6). The structure diagram is shown on fig. 3.4, where k is mentioned in eq fig. 3.4 Structure diagram of the pitch angle model For chosen system (eq. (3.5)), desired characteristic equation, from eq. (3.6), can e written as: c s s 1 * s 1 * 0, (3.7), and with taking into account that 1, j* (eq. (3.)), it can e stated that: c s s * s 5 0, (3.8). By comining eq. (3.6) with eq. (3.8) and eq. (3.7) it can e declared that: c A A A I * 5* * * * , (3.9). 46

48 Calculate the nd element from eq. (3.5). For current system it corresponds to the following equation: B A* B 1, (3.30) where k A* B * 0 0 k 0, so B A* B k 0 k k 0 k 0, (3.31). Thus, from eq. (3.9) and (3.31), eq. (3.5) can e rewritten as: 1 0 k K 0 1 * B A* B * c A 0 1 * * k * k 5* k *k 0 1 * 5* k * k K K1 K 5* k * k, (3.3)., or result in short form is: Therey, closed-loop system matrix A f (eq. (3.4)) can e represented as: A f a0 K1 a1 K 5* k *k, (3.33) The state-space equations for closed-loop system are: x( t) Af * x( t) B * u( t), (3.34) y( t) C * x( t) where A f 0 1 5* k *k, 0 B 1, C 1 0. Structure diagram that corresponds to closed-loop system with matrix on fig A f is shown 47

49 fig. 3.5 Feedack system with desired poles For implementation of this approach a function AckCont in Matla was written (see App. B). By regulators the system keeps its position around zero. However, the attitude controllers should follow desired values of pitch, roll and yaw. An output of a closed-loop TF corresponds to an input, if a gain of the closed-loop TF equals to one. So TF of the system from fig. 3.5 should e found. Represent the system as shown on fig fig. 3.6 Feedack system with desired poles: finding TF In this case inner TF The TF of the regulator G1 scan e descried as: 1 G s k k s k 1 * k s k * k s, (3.35). Gs with taking into account eq. (3.35) can e written as: 48

50 k 1 * s k * k s k, (3.36). k 1 1 * * k s k * k* s k * k1 1 s k * k s G s Based on eq. (3.36) the gain of Gs can e found as: G 0 1, (3.37). k Result of eq. (3.37) means that numerator ofg s should e multiplied y k 1. The final TF of the controller is: 1 G s 1 k* k s k * k* s k * k1, (3.38) Simulation results Control of orientation is implemented in Simulink ased on eq. (3.13) and eq. (.3) (fig. 3.7). Blocks roll_cont, pitch_cont and yaw_cont are represented control laws that are designed ased on eq. (3.3). With taking into account eq. (3.38) controllers for roll, pitch and yaw are created in Simulink for incorporation in whole system, shown on fig Example for roll control is shown on fig. (3.8). fig. 3.8 Implementation of attitude controllers as a lock for incorporation in whole model: Simulink 49

51 fig. 3.7 Orientation control scheme: Simulink The system was analyzed with different step signals. Theoutput for unit step signal is shown on fig. 3.9 for pitch angle and on fig for yaw angle (step response for roll angle is omitted, since it is identical to the results on fig. 3.9). fig. 3.9 Step response: pitch angle 50

52 fig Step response: yaw angle It has clearly seen that controllers provide quality requirements as T 0.8s and percovsh 4.3%. Additionally, it should e mentioned that steady state error for this model is zero, ecause TFs of the MM (eq. 3.13) has nd order integration As soon as designed controllers show required result, they should e implemented in a microcontroller and checked through experiments with real quadrocopter. s 3.5 Altitude control Design of controllers Position control is divided into two parts: height control along Z axis and D control in XY plane. Height control is ased on 3 rd equation from linearized model (eq. 3.15) and can kt e calculated ased on Ackerman approach (eq. 3.3), wherek. m For control law in XY plane the algorithm from [3] is chosen. Based on eq. (3.15) required acceleration can e calculated as: 51

53 d ( s) g * s rx( s)* sn s ry( s)* c, (3.39) s g s r s c s r s sn d ( ) * X( )* Y( )* d where g is gravitational acceleration, and d () s are desired values. In case if yaw angle equals to zero and constant during whole flight it can e even simplified to: 3.5. Simulation results d ( ) * Y ( )* s g s r s c, (3.40) s g s r s c d ( ) * X ( )* Control law ased on eq. (3.39) is realized in Matla/Simulink as shown on fig. (3.11). fig Altitude Control: Matla/Simulink Controllers X_cont, Y_cont, Z_cont have the same structures as attitude controls (fig. 3.8). Coefficients for Z_cont are calculated ased on Ackerman approach; coefficients for X_cont, Y_cont controllers are adjusted manually. 5

54 Herey, the structure of the whole system is implemented as shown on fig. (3.1). fig. 3.1 Altitude Control: Matla/Simulink d d d d Simulation results for X 1, Y 1, Z 1, are shown on fig. (3.13). 6 fig a X position 53

55 fig Y position fig c Z position There are no overshoots along X and Y axes and settling time aout 3s. The overshoot along Z axis is aout 50% and settling time aout s. These ones can e accepted for some application, ut in general it should e improved. For example if a quadrocopter should record visual information during its movement from one desired point to another one, the ehavior shown on fig is unacceptale and should e improved. On the other hand, if a quadrocopter should make several photos in a stale state, this ehavior is acceptale since the positioning y itself is precise enough. Additionally, a steady state error along Z axis is aout 0.14 m. It can e eliminated y using integral component in controller for Z axis. 54

56 Chapter 4 Implementation of the Control System The MM of the quadrocopter, attitude and position control systems were developed in previous chapters. To adapt the MM to the real quadrocopter and to check controllers, several experiments should e fulfilled. First of all the MM should e validated, afterwards control system should e implemented and tested. For the MM validation and adjusting attitude control two test enches are used. Test ench 1 has DOF and used for validating and adjusting pitch orientation. Test ench has 3DOF and used for validating and adjusting yaw orientation. Afterwards, position control system is checked in flying version. The control algorithms were implemented as a script in a frame of software created inside Aerospace Information Technology Department, Würzurg University. This software also was used though experiments for data sending and recording Transfer functions for pitch and roll orientation Transfer function for pitch and roll was otained in previous chapter. However, structure of test ench 1 is different from free motion of the oject; Because of this fact the TF for pitch and roll should e modified for test ench Elements of the system For adjusting pitch controller test ench 1 is used (fig. 4.1). This one consists of a cross-frame, four motors with propellers fixed on their shafts, four power ridges for motor control, a gyro sensor and a microcontroller. It has DOF: pitch (an axis of rotation shown as lack line) and yaw orientation. 55

57 fig. 4.1 Test ench 1: DOF The gyro sensor is set close to the center of the symmetry of the quadrocopter. The orientation of the x and y axes of gyro sensor are parallel to correspondent axes of xy (fig. 4.1). The pitch angle is limited y construction in the range of degree. Each actuator consists of power ridge BL-Ctrl1. [14], BLDC motor KA0- L [15], a lade EPP0845 [16]. The power ridge is controlled y the microcontroller AT3UC3A051-0ESAL fixed on evaluation oard EVK1100 [17]. Relation etween pitch angle and force and torque from a lade is shown on fig. 4.. An integer numer in a range 0 55 should e send y the microcontroller though IC to the power ridge. The power ridge produces control signals to rotate motor shaft with angular velocity which is proportional to integer value, where 0 corresponds to stop and 55 to rotation with maximal velocity. The lade which is fixed on the motor s shaft egins to rotate and produce thrust force and hu torque. 56

58 fig. 4. Structure diagram of actuator and feedack: test ench 1 The forces from propellers move quadrocopter around axes of rotation and change pitch angle. The angular velocity around pitch angle is measured y gyro IMU3000 [18] in deg/sec.integration of this value gives changing of pitch angle in deg. Error etween desired and current values of pitch angle should e changed according to the design law. This output is transferred through the power ridges, motors and lades to produce forces and torques. This motion corresponds to nd equation from eq. (3.13), ut since the axis of rotation is not in the center of symmetry of the quadrocopter additional force occurs. Therey, nd equation from eq. (3.13) should e modified for applying to test ench Linear model for pitch angle on test ench 1 Consider forces acting on the quadrocopter: thrust force from motor 1, thrust force from motor 3 and gravitational force (fig. 4.3). fig. 4.3 Forces applied to the quadrocopter: test ench 1 57

59 nd equation from eq. (3.10) can e rewritten as: y T 3 T 1 * r m* g * *sin, (4.1) J yy where T 3 and T 1 are projections of thrust forces T 3 and T 1 on perpendiculars to radius vector r ; r is a shortest distance etween a point, where a force T 3 or T 1 applied, and axis of rotation; *sin is the displacement vector for gravitational force. These projections can e calculated as: T T T 1 1 T 3 3 *cos, (4.) *cos where is a constant angle etween vector of force T 3/ T 1 and T 3/ T 1. This angle and radius vector r can e calculated as: arctan l r l, (4.3) where is a shortest distance from the center of the quadrocopter s symmetry to axis of rotation, l is a shortest distance from force T 3/ T 1 to the center of the quadrocopter s symmetry. Forces T 3 and T 1 can e calculated y 1 st equation from eq. (3.1) in rewritten form: T u * k 1 1 T u * k 3 3 T T, (4.4) where u 1 and u 3 are values in the range After comining eq. (4.1), (4.), (4.3), (4.4), the relation etween u 1, u 3 and y can e written as: 58

60 y u3 u1 * kt * l *cosarctan m* g * *sin l, (4.5) J yy or in a form of TF as: s s k u3 s u1 s k mg ( ) * ( ) ( ) *sin, (4.6) where k () s is output. kt * l *cosarctan l, J yy k mg m* g *, u3( s) u1( s) is input and J yy Herey, the TF of pitch angle for test ench 1 can e stated as: k 1 s * u3( s) u1( s) k *sin ( ) mg s, (4.7) s s This TF is nonlinear continuous one and for linearization, element sin should e sustituted y a linear element. With taking into account that fixed construction has pitch angle range approximately degree, this element can e sustituted y in radians [10]. So TF from eq. (3.13) can e rewritten as: () s G () s pitch u3( s) u1( s) s kmg k, (4.8). The TF ased on eq. (4.8) is created in Simulink and shown on fig fig. 4.4 Open-loop TF of pitch orientation: test ench 1 59

61 4.1.3 Coefficients calculation and verification Coefficients calculation To calculate coefficients kk, mg the length and weight of the quadrocopter should e measured. Initial data for calculation are: 0.11 m, l 0.05 m, m 0.49 kg, g 9.8 m. s Moment of inertia component theorem can e calculated as: J according to eq. (.19) and parallel axis yy * M * R J y * * * y l m M m, (4.9) 5 where R is chosen equal to l. Coefficients k mg and k ased on eq.(4.6) are: k kt * l *cosarctan l , (4.10) J yy k mg m* g * J yy where kt coefficient for the motor taken from here [19]. 1 1 So ased on theoretical model k mg andk s s Coefficients verification Feedack coefficients for designed controller fit the MM. The more precise the MM is, the more real ehavior of the quadrocopter with designed controller coefficients relates to simulation results. It has een done several simplifications during creation MM and verification coefficients k and kmg through experiment can improve MM. The verification procedure includes two types of experiments. 60

62 Verification of coefficient k mg First coefficient kmg was verified. This one corresponds to movements in gravitational field without influence from lade s forces. The quadrocopter fixed in test ench 1 is inclined four times to random angular position and released to fall till the limited angle. Changes in angular position were recorded and as graph represented on fig fig. 4.5 Changes in angular position during quadrocopter s free falling: test ench 1 Four initial angles are taken from these records and used as initial positions for 1 the MM from fig The same trajectories ased on MM with k mg s are shown on fig. 4.6 (colorful trajectories indicate experiment data and lack ones simulation). Comparing the sets of trajectories shows that current value of does not descrie the ehavior of the quadrocopter adequate. It can e seen that model is too fast, so it means that the calculated inertia momentum is less than real. 61

63 1 fig. 4.6 Experimental and theoretical trajectories: k mg s To make the model adequate was decreased until the longest trajectories from oth sets ecame as close as possile to each other s (fig. 4.7). 1 fig. 4.7 Experiment and theoretical trajectories: k mg.3006 s 6

64 1 The new k mg is renewed as k mg For making theoretical result close to s real one, the new value R 1.7* l is chosen. Verification of coefficient k The idea of verification is to find equilirium conditions etween torques from motor 1 and from gravitation force for various angles. Values of angular position and for motor control were recorded (see tale 4.1). Tale 4.1 angle, rad value Based on the eq. (4.7), with taking into account that angular acceleration s 0, the coefficient is calculated. It is value in the range The average value k is chosen. The script DOF_ver.m (see App. B) was written and used for processing and representation experiment data, for simulation TF from fig After analyses and validation the MM of pitch angle (test ench1) was specified. The mathematical model is represented in a form of linear continuous time invariant transfer function (eq. 4.8) and can e used for controllers design. 4. Control design for pitch and roll orientation By comparing TF of nd order function and TF of pitch orientation quadrocopter (eq. 4.8) coefficients for state space model are calculated as : 1* s 0 k Gs G () s s a s a s k pitch 1* 0 mg, (4.11). It can e concluded that 0 equals to k, a0 equals to, 1 and a1 equal to zero. kmg 63

65 Using the same logic as in section 3.4 and eq. ( ) feedack coefficients can e otained as (details see in App. A ): K K1 K kmg 5* k *k, (4.1). Therey, closed-loop system matrix A f (eq. (3.4)) can e represented as: Af a k 5* *k 0 K1 a1 K mg kmg k 5* k *k, (4.13) The state-space equations for closed-loop system are: x( t) Af * x( t) B * u( t), (4.14) y( t) C * x( t) where A f , 0 B 1, C 1 0. Structure diagram that corresponds to closed-loop system with matrix on fig A f is shown fig. 4.8 Feedack system with desired poles 64

66 Based on idea and equations mentioned aove the script in Control System Toolox (CST) was written (see App. B AckContSim.m ). The response of the closed-loop with desired poles and initial conditions 1 0 (means initial position is 1 radian and initial velocity is zero) is shown on fig It is clearly seen that for the model the overshooting is minimal (less than 4.3%), settling time is aout 0.8 seconds, steady-state error equals to zero. fig. 4.9 Response for initial conditions 4..1 Implementation of the regulator Control law corresponds to closed-loop TF (fig. 4.8) and, with taking into account that the gyroscope generates data in degrees, this law can e represented as: ( ) ( ) 1 * 1* * ( ) 1 d t t K DtR t *( K)* DtR * ( t) k k, (4.15) where d ( t) u3( t) u1( t) and DtR

67 In form applied to motor s values it can e rewritten as: 1 1 u1( t) ( t) *( K1)* DtR * ( t) *( K)* DtR * ( t) k k, (4.16). 1 1 u3( t) ( t) *( K1)* DtR * ( t) *( K)* DtR * ( t) k k Additionally, initial values of u () t and 3 u () 1 t should not e 0, ecause of two reasons. First reason is a simplification of the model for which this controller was designed. Time delays in changing velocities of quadrocopter s lades were neglected. The most important delay is during increasing the lade angular velocity from zero to some value. So the lades should always have some non-zero velocities. Moreover, there is some value of speed that provides hovering of the quadrocopter and all changes in control should e around this value. With taking into account these facts eq. (4.16) should e rewritten as: u1( t) umin uhov ( t), (4.17), u ( t) u u ( t) 3 min where () t is defined y eq. (4.16), u min is a minimal angular velocity of the lades, u hov is an angular velocity for hovering. hov The designed control law (eq. (4.17)) is incorporated in the software. Fulfilled experiments consist of two simple steps: change the quadrocopter pitch angle from 0 y some external force record system response Results are shown on fig. 4.10, the data are recorded with discretization of 100 Hz. 66

68 fig Responses of regulator on changing of initial conditions The system should return its pitch to zero. It is inclined four times and returns to initial zero. For settling time estimation an area from fig is zoomed (fig. 4.11). fig Response signal (zoomed part from fig. 4.10) It can e seen that the settling time is aout 0.6 second and a static error aout 0.3 degree. Settling time in experiment is close to settling time from model. There is no static error in the model, since the model does not take into account all facts, 67

69 e.g. some friction in test ench joint that can e the source of this error. This error can e compensate y an integral component. 4.. Implementation of the controller To provide control for general input, the gain of the TF should e set to one. TF of the system from fig. 4.8 is: and it s gain is: G k () s, (4.18), s k * s ( k k * k ) pitch _ f 1 mg G pitch _ f k (0) k k * k 1 mg, (4.19). So a coefficient, which is inversed to G _ (0), should e set etween the pitch reference signal that represents desired pitch angle and input of feedack TF. As soon as a desiredt angle is in degree and the model in radian, transfer coefficients: etween reference signal and input signal and 180 etween output signal and 180 results should e added (fig. 4.1). f fig. 4.1 Pitch angle: control system for non-zero input It means that for experiment the control law is: d K1 1 1 m( t) ( t)* DtR * DtR * *( K1)* ( t) *( K)* ( t) k k k, (4.0). 68

70 After corresponding changes in microcontroller one more experiment was fulfilled. Response function is shown on fig fig Pitch angle: control system for non-zero input Quality parameters are summarized in tale 4.. Tale 4. input signals, deg output signal, deg settling time, sec overshooting, deg Quality parameters are inside required range, so pitch and roll attitude control is designed. Results of experiments show that system works with required quality (overshooting and settling time) for oth cases: zero input and non-zero input. Static error for oth cases is inside 0.5 degree. With taking into account many approximations, this error is relatively small. To reduce steady state error integration should e used. 69

71 Roll controller y using the same logic can e stated as the following equations: u( t) umin uhov ( t), (4.18), u ( t) u u ( t) 4 min 1 1 where ( t) *( K1)* DtR * ( t) *( K)* DtR * ( t). k k Results of roll controller are identical to results of pitch controller. hov 4.3 Controller for yaw orientation Controller for yaw orientation is checked on test ench, which has 3DOF. Test ench has a structure close to test ench 1, ut fix point of test ench is close to CoM of the quadrocopter. The TF of yaw rotation ased on 3 rd line from eq. (3.13) can e stated as an equation: () s k u ( s) u ( s) u ( s) u ( s) s 1 3 4, (4.1) where k k J H. zz Component J 0.06 is calculated y eq. (.19). Coefficient k is measured zz y an experiment. Inputs u () s and 1 u () 3 s were set to value of hover and yaw rotation was recorded (fig. 4.14). In the range 10 1sec released and stop manually. Angular velocity is aout rad /sec. Coefficient k H was adjusted until angular rate of yaw in the model (file ) ecame rad /sec ; new 3 k H 0.5*10 and k. Controller for yaw orientation is calculated ased on eq. (3.3) where k Settling time is chosen equal to 3.5sec instead of 0.8sec.It is done to make feedack coefficients lower. k. 70

72 fig Yaw rotation open-loop: test ench Implementation of yaw controller can e descried as the following equations: u ( t) u u ( t) 1 min u( t) umin uhov ( t), (4.), u ( t) u u ( t) 3 min u ( t) u u ( t) 4 min 1 1 where ( t) *( K1)* DtR * ( t) *( K)* DtR * ( t). k k Experiments showed that when all three controllers for attitude control have high coefficients the system has very thin linear zone, which leads to ig oscillations. Results are shown on fig It can e seen that s settling time is aout 3.5sec and precision from 1 to degree. hov hov hov 71

73 fig Yaw rotation closed-loop: test ench Therey attitude control can e descried y following equations: u ( t) u u ( t) ( t) 1 min u( t) umin uhov ( t) ( t), (4.3). u ( t) u u ( t) ( t) 3 min u ( t) u u ( t) ( t) 4 min hov hov hov Results of attitude controller are received y test ench and shown on fig The quadrocopter was inclined several times and one returned to original positions. Moments of inclination are marked as: roll1, roll, pitch1, pitch, yaw1, yaw. 7

74 fig Attitude Controller: test ench Dynamics of the system corresponds to required quality. However original positions are changed during the time from 0 to 5 degree. This disadvantage can e eliminated y using integral component and y separating in time pitch-roll and yaw controllers. 4.4 The Altitude Control System The altitude control system consists of the controller for quadrocopter s hover and position controllers (see section 3.5.1). Implementation of the hover controller can e descried as the following equations: u ( t) u u ( t) 1 min u( t) umin uhov z ( t), (4.3), u ( t) u u ( t) 3 min u ( t) u u ( t) 4 min where 1 1 ( t ) *( K 1)* z ( t z ) *( K )* z ( t ) k k and k k T. m hov hov hov z z z 73

75 Result of experiment with a flying prototype for the hover control is shown on fig. 4.17, where a green line is desired height and a red line is a current height. fig The hover Controller with calculated coefficients: flying mode In the case when coefficients of the hover controller are calculated perfectly, the ehavior of current height (red line) should correspond to the modeling result (fig. 3.13c). However it does not since proportional coefficient ( K 1) is low and velocity/derivative coefficient ( K ) is extremely low. After slightly increasing of K 1 and ig increasing of K (aout ten times) the new experiment was conducted (fig. 4.18). The ehavior of the system is etter, ut still is not acceptale. For etter results the K was increased (in total aout 30 times comparing to original K ) and an integral component for elimination steady state error was added. Appropriate result is shown on fig

76 fig The hover Controller with K 10 : flying mode fig The hover Controller with K 35 : flying mode 75

77 Implementation of position controller corresponds to eq. (3.40). The ehavior of the quadrocopter in the flying mode with coefficients used in the simulation is shown on fig. 4.0, where green line corresponds to the desired height and other ones to changing along X and Y. fig. 4.0 Position Controller with calculated coefficients: flying mode It can e clearly seen that the system is unstale and simulation results (fig. 3.13a and fig. 3.13) do not match to experimental ones. New coefficients for position controllers were found though adjusting, results are shown on fig. 4.1, where green line is desired position along Y axis and red line is current position of the quadrocopter along Y axis. Also it should e mentioned that new K 1 is ten times less than K 1 from simulation and new K is thirty times more than original K. Position error of the system is inside 10 centimeters. Therey, it can e concluded that results of the simulation are different from results from real experiment. For the hover controller K 1 from the simulation is close to real one, ut K should e increased in thirty times. For the position control, oth coefficients otained from the simulation should e changed, K 1 should e decreased in ten times and K should e increased in thirty times. 76

78 fig. 4.1 Position Controller with adjusted coefficients: flying mode 77