MINISTÉRIO DA DEFESA EXÉRCITO BRASILEIRO …

MINISTÉRIO DA DEFESA

EXÉRCITO BRASILEIRO

DEPARTAMENTO DE CIÊNCIA E TECNOLOGIA

INSTITUTO MILITAR DE ENGENHARIA

CURSO DE MESTRADO EM ENGENHARIA ELÉTRICA

LUIS OTÁVIO GUEDES LOBO E SILVA

ATTITUDE CONTROL OF A QUADROTOR

Rio de Janeiro2016




Dissertação de Mestrado apresentada ao Curso de Mes-trado em Engenharia Elétrica do Instituto Militar de En-genharia, como requisito parcial para obtenção do título deMestre em Ciências em Engenharia Elétrica.

Orientador: TC Antonio Eduardo Carrilho da Cunha, Dr. Eng.

Rio de Janeiro

2016

c2016

INSTITUTO MILITAR DE ENGENHARIAPraça General Tibúrcio, 80 - Praia VermelhaRio de Janeiro - RJ CEP: 22290-270

Este exemplar é de propriedade do Instituto Militar de Engenharia, que poderá incluí-loem base de dados, armazenar em computador, microlmar ou adotar qualquer forma dearquivamento.

É permitida a menção, reprodução parcial ou integral e a transmissão entre bibliotecasdeste trabalho, sem modicação de seu texto, em qualquer meio que esteja ou venha aser xado, para pesquisa acadêmica, comentários e citações, desde que sem nalidadecomercial e que seja feita a referência bibliográca completa.

Os conceitos expressos neste trabalho são de responsabilidade do(s) autor(es) e do(s)orientador(es).

S586a Silva, Luis Otávio Guedes Lobo eAtitude control of a quadrotor / Luis Otávio Guedes Lobo e Silva; ori-

entado por Antonio Eduardo Carrilho da Cunha - Rio de Janeiro: InstitutoMilitar de Engenharia, 2016.

123p.: il.

Dissertação (Mestrado) - Instituto Militar de Engenharia, Rio de Janeiro,2016.

1. Curso de Engenharia Elétrica - teses e dissertações. 2. Quadricóptero.I. Cunha, Antonio Eduardo Carrilho da. II. Título. III. Instituto Militarde Engenharia.

CDD 621.3

2




Dissertação de Mestrado apresentada ao Curso de Mestrado em Engenharia Elétricado Instituto Militar de Engenharia, como requisito parcial para obtenção do título deMestre em Ciências em Engenharia Elétrica.

Orientador: TC Antonio Eduardo Carrilho da Cunha, Dr. Eng.

Aprovada em 02 de setembro de 2016 pela seguinte Banca Examinadora:

TC Antonio Eduardo Carrilho da Cunha, Dr. Eng. do IME - Presidente

Prof. Paulo César Pellanda, Dr. ENSAE do IME

Prof. Paulo Fernando Ferreira Rosa Dr. NIIDAI do IME

Prof. Mario Cesar Mello Massa de Campos, Dr. ECP do CENPES

Rio de Janeiro2016

3

This work is dedicated to my parents, Octávio RenatoDutra Lobo e Silva (in memorian) and Lucia de Fátima Guedes.

4

ACKNOWLEDGEMENT

Personally, it is an honour and happiness to have developed this work in the traditi-

onal Instituto Militar de Engenharia (IME). Historically, IME is a magnicent center

of engineering and has formed high capacity engineers who have been responsible

for many projects in my country, Brazil. So, I am very proud to belong on this

select group of professionals. However, I am not alone and many people were impor-

tant for the realization of that project. In this manner, I expose my acknowledgements to:

God, creator of the universe. Thanks for giving me a blessed life.

My parents Lúcia de Fátima Guedes and Octávio Renato Dutra Lobo e Silva(in memo-

rian) and my brothers Alzir Brilhante Neto, André Guedes Brilhante and Carlos Renato

Guedes Lobo e Silva. I am so grateful for all love I have received from my family since I

was a child.

Alexandre Marones Chaves dos Santos, Beniz Monteiro Fontes, Daniel Sousa Rocha,

Gabriel Crepaldi Antunes and Felipe Crepaldi Antunes for the long friendship since

childhood.

Antonio Manuel Calheiros da Graça Campinho, Cibele Ohana Lima de Sousa, Denis

Mota de Sousa, Eduardo Luiz Aranha Gomes, Pe. Gabriel de Moraes Coelho, João Paulo

Souza Rodrigues, Larissa Vitória Costa da Silva, Leeam de Jesus Maél, Leonardo Martins

dos Santos, Letícia Conceição de França, Luiz Felipe Tavares da Silva, Tamiris Pereira

Pussente, Samara Soares Silva, Suziane Aguiar Melo de Sousa and Yuri Gabriel Aliendre

Sousa da Silva, for friendship, emotional support and precious advices in hard moments.

The fellow students Eder Guimarães dos Santos, Giorgio de Moura Magalhães, Gustavo

Claudio Karl Couto, Luan Machado Borges, Patricia Martinez Kalil Núñez Teixeira and

Patricia Thompson Bandeira for cooperation in the moments of study and work.

The secretary Maria Lucia Ferreira Gomes for assisting me on paperwork.

5

The laboratory technician Victor Luiz Dias de Castro for support and maintenance of

Crazyies.

Coordenação de Aperfeiçoamento de Pessoal de Nível Superior (CAPES) for nancial

support of research.

The professors Geraldo Magela Pinheiro Gomes and Alberto Mota Simões for teaching me

important concepts of control systems engineering, and the professors Paulo Fernando

Ferreira Rosa and Mario Cesar Mello Massa de Campos for participation and for

providing valuable advices.

My advisor and friend, the professor Antonio Eduardo Carrilho da Cunha, for the aec-

tion, competence, patience, wise advices, academic teaching, constructive criticism and

for the opportunity to work in that wonderful dissertation topic that you have proposed.

Thanks for you trust in me. It is an honour.

6

SUMMARY

LIST OF FIGURES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

LIST OF TABLES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

LIST OF ABBREVIATIONS AND SIMBOLS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

1 INTRODUCTION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

1.1 The wide use of Unmanned Aerial Vehicles . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

1.2 Attitude Control of Quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

1.3 Work proposal and Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

1.4 Development . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

1.5 Dissertation Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

2 QUADROTOR DYNAMICAL MODELLING . . . . . . . . . . . . . . . . . . . 27

2.1 Mathematical Modelling of a Quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

2.2 Coordinate Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

2.3 State Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

2.4 Nonlinear Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

2.5 Output Equations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

2.6 Model Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

2.7 Chapter Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

3 ATTITUDE CONTROL DESIGN . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

3.1 Linearization and Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

3.2 Controller Synthesis Design for Inner Loop . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

3.3 Simulation with inner loop control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

3.4 Controller Synthesis Design for Outer Loop . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

3.5 Simulation with outer loop control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68

3.6 Decoupling Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74

3.7 Use of a Discrete Kalman Filter to Estimate Linear Speeds . . . . . . . . . . . . . . 80

3.7.1 The Discrete Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80

3.7.2 The lter to the horizontal speeds . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83


7

4 IMPLEMENTATION AND TESTS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88

4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88

4.2 Firmware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88

4.3 Firmware Changes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91

4.4 Isolated Tests . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94

4.4.1 Height Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97

4.4.2 Longitudinal and lateral speeds and Euler angles . . . . . . . . . . . . . . . . . . . . . . . 98

4.4.3 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102


5 CONCLUSION . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105

6 REFERENCES . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107

7 APPENDIXS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110

7.1 Stabilizer Routine Changes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110

8

LIST OF FIGURES

FIG.1.1 Crazyie. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

FIG.2.1 Crazyie modelling. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

FIG.2.2 Model for commands to rotors. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

FIG.2.3 Inertial Vehicle Frame and Vehicle Frame. . . . . . . . . . . . . . . . . . . . . . . . . . . 31

FIG.2.4 Vehicle-1 Frame. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

FIG.2.5 Vehicle-2 Frame. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

FIG.2.6 Body Frame. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

FIG.2.7 Sensor fusion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

FIG.2.8 Constant torque kt result from a rotor of the Crazyie (BORGES,

2015). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

FIG.2.9 Quadratic curve linearized in x = 0.5 and y = 1. . . . . . . . . . . . . . . . . . . . . 45

FIG.3.1 Blocks of linear model. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50

FIG.3.2 Decoupling mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

FIG.3.3 Linear model for θ mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

FIG.3.4 Block diagram for pitch control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

FIG.3.5 Block diagram for pitch control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

FIG.3.6 Linear analysis response for Pitch Mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

FIG.3.7 Linear analysis response for Roll Mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

FIG.3.8 Linear analysis response for Yaw Mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

FIG.3.9 Linear analysis response for Height Mode. . . . . . . . . . . . . . . . . . . . . . . . . . . 58

FIG.3.10 Simulink for inner loop. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

FIG.3.11 Pitch block. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

FIG.3.12 Positions (inner loop). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

FIG.3.13 Linear speeds (inner loop). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

FIG.3.14 Euler angles (inner loop). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

FIG.3.15 Angular speeds (inner loop). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

FIG.3.16 Thrust (inner loop). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

FIG.3.17 Torques (inner loop). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

FIG.3.18 Relation between force/torque × Duty Cycle (inner loop). . . . . . . . . . . . . 64

FIG.3.19 Linear Control for U mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

9

FIG.3.20 Block diagram for u control. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

FIG.3.21 Linear analysis response for U Mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

FIG.3.22 Linear analysis response for V Mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68

FIG.3.23 Simulink for outer loop. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

FIG.3.24 U controller block. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69

FIG.3.25 Positions (outer loop). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70

FIG.3.26 Linear speeds (outer loop). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

FIG.3.27 Euler angles (outer loop). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

FIG.3.28 Angular speeds (outer loop). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

FIG.3.29 Thrust (outer loop). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

FIG.3.30 Torques (outer loop). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73

FIG.3.31 Relation between force/torque × Duty Cycle (outer loop). . . . . . . . . . . . . 73

FIG.3.32 Simulation using a sine wave as reference on height mode. The top

view corresponds to the inner loop, and the bottom to the outer

loop. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76

FIG.3.33 Simulation using a sine wave as reference on pitch mode. . . . . . . . . . . . . . 77

FIG.3.34 Simulation using a sine wave as reference on roll mode. . . . . . . . . . . . . . . . 78

FIG.3.35 Simulation using a sine wave as reference on yaw mode. The top

view corresponds to the inner loop, and the bottom to the outer

loop. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79

FIG.3.36 Simulation using a sine wave as reference on U and V modes. The

top view corresponds to the U mode loop, and the bottom to the

V mode. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80

FIG.3.37 Block diagram: Discrete Kalman Filter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81

FIG.3.38 Steps of Discrete Kalman Filter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83

FIG.3.39 Simulink for outer loop with Kalman lter. . . . . . . . . . . . . . . . . . . . . . . . . . 85

FIG.3.40 Horizontal speeds estimation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86

FIG.3.41 Air drag coecients estimation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86

FIG.4.1 Hardware Diagram. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89

FIG.4.2 Block diagram of the communication between the PC client and

the rmware. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89

FIG.4.3 Crazyie rmware architecture (Retrieved from

https://wiki.bitcraze.io/projects:crazyie:rmware:arch) . . . . . . . . . . . . . 90

10

FIG.4.4 Software Crazyie PC client (ight control window) . . . . . . . . . . . . . . . . . . 90

FIG.4.5 Software Crazyie PC client (log toc window) . . . . . . . . . . . . . . . . . . . . . . . 91

FIG.4.6 Software Crazyie PC client (parameter window) . . . . . . . . . . . . . . . . . . . . 91

FIG.4.7 owchart of the modied stabilizer routine. . . . . . . . . . . . . . . . . . . . . . . . . . 93

FIG.4.8 Crazye tied between to columns for pitch control test. . . . . . . . . . . . . . . 94

FIG.4.9 Output curves of the pitch mode comparing the reference input,

the measured pitch angle and the simulated pitch angle. . . . . . . . . . . . . . 95

FIG.4.10 Crazye tied between to columns for roll control test. . . . . . . . . . . . . . . . . 95

FIG.4.11 Output curves of the roll mode comparing the reference input, the

measured roll angle and the simulated roll angle. . . . . . . . . . . . . . . . . . . . 96

FIG.4.12 Crazye tied between to columns for yaw control test. . . . . . . . . . . . . . . . 96

FIG.4.13 Output curves of the yaw mode comparing the reference input, the

measured yaw angle and the simulated yaw angle. . . . . . . . . . . . . . . . . . . 97

FIG.4.14 Height control on. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98

FIG.4.15 Longitudinal and lateral speeds in inner loop. . . . . . . . . . . . . . . . . . . . . . . . 99

FIG.4.16 Longitudinal and lateral speeds in outer loop. . . . . . . . . . . . . . . . . . . . . . . . 100

FIG.4.17 Euler angles in inner loop. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101

FIG.4.18 Euler angles in outer loop. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101

FIG.4.19 The estimates of ue and ve with the Crazyie standing over a sur-

face. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102

FIG.4.20 The estimates of ue and ve closing only the inner loop. . . . . . . . . . . . . . . . 103

FIG.4.21 Feedback with discrete Kalman lter with estimation of ue and ve. . . . . . 104

11

LIST OF TABLES

TAB.2.1 Rate gyro settings . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

TAB.2.2 Accelerometer settings . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

TAB.2.3 Magnetometer settings . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

TAB.2.4 Table of the model parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

TAB.2.5 Table of lumped parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

TAB.3.1 Table of weighting matrices for inner loop. . . . . . . . . . . . . . . . . . . . . . . . . . 59

TAB.3.2 Table of weighting matrices for outer loop . . . . . . . . . . . . . . . . . . . . . . . . . . 68

TAB.3.3 Decoupling analysis for inner loop. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74

TAB.3.4 Decoupling analysis for outer loop . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75

TAB.7.1 Table of variables and Log Blocks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115

TAB.7.2 Table of parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122

12

LIST OF ABBREVIATIONS AND SIMBOLS

ABBREVIATIONS

ANAC - National Agency of Civic Aviation

ESC - Electronic Speed Controller

FAB - Brazilian Air Force

LQR - Linear-Quadratic Regulator

MEMS - Micro-electro-mechanical-sensors

MIMO - Multiple Input and Multiple Output

PC - Personal Client

PWM - Pulse-width modulation

UAV - Unmanned Aerial Vehicles

SYMBOLSR1 - Front rotor

R2 - Right rotor

R3 - Back rotor

R4 - Left rotor

F - Thrust

F ′ - Normalized Thrust

τphi - Roll torque

τtheta - Pitch torque

τpsi - Yaw torque

τ ′phi - Normalized roll torque

τ ′theta - Normalized pitch torque

τ ′psi - Normalized yaw torque

l - Distance from the rotors to the center of mass of quadrotor

δ - Step send to the motors

kf - Rotor thurst constant

kt - Rotor torque constant

13

Fi - Inertial Frame

Fv - Vehicle Frame

F1 - Vehicle-1 Frame

F2 - Vehicle-2 Frame

Fb - Body Frame

Rv1v - Matrix of transformation from vehicle frame to vehicle-1 frame

Rv21 - Matrix of transformation from vehicle-1 frame to vehicle-2 frame

Rbv2 - Matrix of transformation from vehicle-2 frame to body frame

Rbv - Matrix of transformation from vehicle frame to body frame

Rvb - Transpose of Matrix of transformation from vehicle frame to body

frame

pn - North position

pe - East position

h - Height position

u - Longitudinal speed

v - Lateral speed

w - Attitude speed

φ - Roll angle

θ - Pitch angle

ψ - Yaw angle

p - Roll rate

q - Pitch Rate

r - Yaw Rate

m - Mass of quadrotor~f - Total force applied to the quadrotor

vb - Linear speed measured in the body coordinates frame

ωb - Angular speed measured in the body coordinates frame

g - Gravity acceleration

σw - Concentration of all parameters uncertainties of W mode

σφ - Concentration of all parameters uncertainties of φ mode

σθ - Concentration of all parameters uncertainties of θ mode

σψ - Concentration of all parameters uncertainties of ψ mode

Pb - Angular momentum

14

τ - Total torque

J - Constant of inertia~X - State vector~U - Input Vector

gx - Mensuration of angular rate p

gy - Mensuration of angular rate q

gz - Mensuration of angular rate r

ax - Mensuration of acceleration in axis x

ay - Mensuration of acceleration in axis y

az - Mensuration of acceleration in axis z

mN - Measured of magnetic eld in component North of the Earth

mE - Measured of magnetic eld in component East of the Earth

mD - Measured of magnetic eld in component Down of the Earth

P - Pressure

T - Temperature

b - Output of barometer

KIP - Integral gain of pitch mode

KPP - Proportional gain of pitch mode

KDP - Derivative gain of pitch mode

KIR - Integral gain of roll mode

KPR - Proportional gain of roll mode

KDR - Derivative gain of roll mode

KIY - Integral gain of yaw mode

KPY - Proportional gain of yaw mode

KDY - Derivative gain of yaw mode

KIW - Integral gain of W mode

KPW - Proportional gain of W mode

KIU - Integral gain of U mode

KPU - Proportional gain of U mode

KIV - Integral gain of V mode

KPV - Proportional gain of V mode

Qaug - Augmented matrix of weight matrix Q of LQR

Raug - Augmented matrix of weight matrix R of LQR

15

RESUMO

Esta dissertação de mestrado apresenta uma proposta para o controle de atitude deum quadricóptero usando uma arquitetura de controle do tipo cascata. O objetivo émanter o quadricóptero no ar em uma posição xa no espaço tridimensional, usando ossensores embarcados disponíveis. A estratégia de controle consiste em utilizar uma malhainterna para estabilizar a orientação e a altura do quadricóptero e uma malha externapara estabilizar a posição horizontal, utilizando as velocidades lineares como sinais dereferência.

O sistema foi modelado em função dos ângulos de Euler por serem intuitivos. O mo-delo foi linearizado e desacoplado em seis modos para facilitar o projeto do controle deatitude. A malha interna possui quatro modos, em que um modo corresponde ao movi-mento vertical e três modos correspondem aos movimentos de rotação, isto é, a rolagem,a arfagem e a guinada. A malha externa possui os dois modos restantes correspondentesas velocidades longitudinal e lateral. Cada modo foi analisado isoladamente.

Um controlador LQR com ação integral foi projetado para cada modo a m de oti-mizar a resposta dinâmica. Além disso, um observador de estados baseado em um ltrode Kalman discreto foi implementado para estimar as velocidades horizontais.

Os resultados apresentados nas simulações e nos experimentos provam que a adiçãoda malha externa melhora signicativamente a regulação da posição horizontal.

16

ABSTRACT

This Master's thesis presents a proposal for attitude control of a quadrotor using acascade control architecture. The control objective is to maintain the quadrotor at a xedpose using the available onboard sensors. The control strategy consists of using an innerloop to stabilize the orientation and the height of the quadrotor and an outer loop tostabilize the horizontal position using the linear speeds as references signals.

The system was modelled in function of Euler angles for being intuitive. The modelwas linearized and decoupled in six modes in order to facilitate the attitude control design.The inner loop has four modes, where one mode corresponds to the vertical movementand three modes correspond to the rotational movements, i.e, roll, pitch and yaw. Theouter loop consists of the remaining two modes corresponding to the longitudinal andlateral speeds. Each mode was analyzed in an isolated manner.

An LQR controller with integral action was designed for each mode to optimize thedynamics responses. Besides, a state observer based on a discrete Kalman lter wasimplemented to estimate the horizontal speeds.

The simulations and experiments results prove that the outer loop addition improvessignicantly the regulation of the horizontal position.

17

1 INTRODUCTION

The structure of this chapter is as follows. In section 1.1, we present an introduction

about the Unmanned Aerial Vehicles. In section 1.2, we discuss about the attitude control.

In section 1.3, we present the propose, motivations and contributions of this work. In

section 1.4, we present the steps of how this dissertation was developed. Finally, in

section 1.5 we present the dissertation outline.

1.1 THE WIDE USE OF UNMANNED AERIAL VEHICLES

In the last years, the use of Unmanned Aerial Vehicles (UAVs)1 in civil and military

elds has increased in many countries around the world. These vehicles are useful when

performing desired tasks in dangerous and/or inaccessible environments (TAYEBI, 2006).

The signicant advantages that drones2 have when compared to manned aerial vehicles

are such as:

More ability to realize maneuvers, once that physiologic human conditions are limi-

ted;

Perform ights in extremely hostile environment, without compromising the lives of

pilots;

Smaller and weightless aircrafts, as a consequence of the absence of pilots or a crew;

They can use rechargeable battery instead of gasoline or kerosene; and

Cheaper and optimized airframes.

Aerial robotics has developed intelligent UAVs with features of perceiving the envi-

ronment, tracking of moving objects, reactive control to avoid obstacles, coordination

and cooperation with other vehicles (VÁRQUEZ, 2012). Therefore, these aircrafts can be

useful in many operations, for example:

1The term Unmanned Aerial Vehicles refers to aircraft without onboard human pilots, controlled eitherautonomously or by remote control (AIR DRONE, 2016).

2Drone is a generic term for unmanned aerial vehicle (ANAC, 2015).

18

Urban surveillance to improve citizen security;

International border surveillance on land and sea, in order to combat drug dealing;

Transport of supplies and war material for the troops located in inaccessible location;

War simulation;

Detection, monitoring and control of re in forests, and also irregular deforestation;

Support on environment disasters, like overows and earthquakes;

Operations of search and safety;

Transport and delivery of packages;

Monitoring the ecosystems and meteorology;

Agricultural control;

Inspection of buildings and facilities;

Exploration of oil and gas pipelines; and

Video and picture recording.

However, we are concerned with the high number of accidents already reported with

both civilian and military drones. For instance, more than 400 USA military drones have

crashed in accidents around the world since 2001 (WHITLOCK, 2014). In 2012, an UAV

of Brazilian Air Force (FAB), the Hermes 450, was completely destroyed after taking o

because one of the motors stopped work (VALDUGA, 2012). The incident occurred in the

air base of Santa Maria (State of Rio Grande do Sul). In 2014, a drone nearly collided with

a passenger jet close to the Heathrow airport, in England (PIGOTT, 2014). Among the

causes, the premature deployment of insuciently tested devices and the suppression of

some security subsystems and sensors in order to get quickly a new product with less cost

are noteworthy in the engineer perspective (WHITLOCK, 2014). We consider therefore,

that the investigation of drone technology and control techniques to enhance the reliance

of such systems is a relevant task.

In this evident situation of high and increasing demand for drones, companies pressure

the authorities to implement commercial regulation. This is a worldwide situation, also

19

happening in Brazil. In this country, for operations that do not include the use of UAVs

for fun, recreation or hobby, an Authorization certicate for experimental ights must

be obtained by the National Civil Aviation Agency (in Portuguese, Agência Nacional de

Aviação Civil - ANAC). In addition, the operation must be performed with safety and

should not endanger people or property (in the air or on the oor), even unintentionally

(ANAC, 2015).

There are several types of UAVs with dierent ight dynamics, for instance, the xed-

wing (planes), the rotating propellers, as the helicopters, and other categories like balloons

or blimps (COSTA, 2012).

We have selected the quadrotors as a type of UAV to be studied in this work since

it has some interesting advantages over the other types of drone, such as, without being

exhaustive:

Quadrotors are vertical takeo and landing devices (VTOL) that makes them more

versatile to maneuvering when compared to xed-wing drones;

They do not need to get a minimal relative speed in relation to the air to keep ight;

They are cheaper, smaller and more durable than conventional helicopters;

There is a wide variety of quadrotors and aordable prices for sale;

They are suitable for indoors or outdoors environments;

They are easy to assemble and relatively simple to control; and

They can get pictures in lower attitude and close to an object in many angles.

Undoubtedly, these technical features render the quadrotor an ecient and an ex-

cellent choice for commercial and military applications. For military uses, quadrotors

are operational and tactical drones used for reconnaissance and support troops while the

xed-wings are strategical drones used for targeted killing. Quadrotors are ideal platforms

for autonomous ight in unknown and complex environments because they are conducive

to operation in conned spaces and avoiding obstacles (LEISHMAN, 2014).

The quadrotor is a multiple-input-multiple-output (MIMO) nonlinear system which

has a high coupling degree between the variables and many aerodynamical eects that

are dicult to measure or model precisely. It is a challenge to design a quadrotor control

system (BASRI, 2014).

20

From the perspective of control systems, quadrotors are relatively complex to be sta-

bilized due to noise in sensors, model uncertainty and others factors. There are several

problems that should be solved or improved (LEBEDEV, 2013). One of these problems

is a real time six degree of freedom control system that can control a position and an

orientation of a quadrotor, its linear and angular velocities (LEBEDEV, 2013). Besides,

these types of drones have a time-varying behavior due to battery discharge and they are

constantly aected by aerodynamic disturbances (RAFFO, 2009).

We can compare quadrotos with the inverted pendulum model (HEHN, 2011). It is a

classic problem in control theory and it is helpful in the study of the position control of

unstable systems (OGATA, 2003). Indeed, quadrotos have an unstable equilibrium point

like the inverted pendulum and many other features, according to what will be indicated

in this monograph.

On the other hand, the availability of low cost open architecture devices allows us to

deal with these problems working directly in the modelling of hardware components and

employing techniques that can be used in the physical device for more accurate models.

In this work, we use an experimental platform, the Crazyie 1.0, from the company

Bitcraze 3. It costs 143 dollars and the kit includes:

1 x Crazyie control board;

1 x Crazyradio;

1 x Duck antenna 2 dBi;

5 x Motor mounts;

5 x Coreless DC motors;

5 x CW propeller;

5 x CCW propeller;

1 x LiPo battery; and

1 x 2*5 pins 1.27mm pitch header.

3https://www.bitcraze.io/crazyie/

21

However, this product is discontinued and has been replaced with Crazyie 2.0. Figure

1.1 shows a Crazyie 1.0 and its antenna, which connects to a computer by a USB port

to exchange control and telemetry signals with the quadrotor.

FIG. 1.1: Crazyie.

The crazyie is a micro quadrotor with the following specications (BITCRAZE,

2015):

Small and lightweight, approximately 19 g and the distance from a motor to another

is 90 mm;

Flight time up to 7 minutes with standard battery of 170 mAh Li-Po;

Standard micro-USB connector for charging the battery (around 20 minutes of char-

ging);

32 bit micro-controller: STM32F103CB at 72 MHz;

Radio bootloader which enables wireless loading of rmware;

Up to 80m range depending on the environment;

3-axis-high-performance microelectromechanical systems gyro with 3-axis accelero-

meter;

Available footprints to manually solder magnetometer HMC5883L/HMC5983

or/and barometer MS5611 (mounted on the 10-DOF version); and

22

Open-source software development.

The purpose and motivation of this master dissertation is to design an attitude

control and implement it at the Crazyie 1.0 and keep it hovering. We seek to improve

the ight performance to pilot it in a safely and reliable way.

1.2 ATTITUDE CONTROL OF QUADROTOR

One of the most fundamental control task when considering a quadrotor system is the

attitude control. The attitude control is the fundamental cell of control of a quadrotor.

It is the lowest control level that provides stability to the pose and angular orientation of

quadrotor. The regulation of the pose without the attitude control is very hard for the

human pilot commanding the actuators in order to keep the quadrotor standing in the

air. So, the attitude control is essential for remote piloting device and autonomous ight.

For instance, by rst make the quadrotor being stable in a xed pose it is possible to,

using a higher level control loop, make it move from one point to the other or to follow

a specied trajectory. In addition, all the others forms of control, such as the execution

of a complex maneuver, are based in the attitude control. It is mainly based on onboard

sensors, like an inertial measurement unit (IMU) equipped with accelerometers, rate gyros

and magnetometers, or additional onboard sensors like barometers or sonars.

In short, the attitude is the orientation of an object in space. The movement of a

quadrotor has six degrees of freedom where three correspond to the translation movement

and three correspond to the rotation movement (SOUSA, 2011). More specically, the

attitude control directs the speed vector and the angular orientation to improve the sta-

bility and dynamic system (BLAKELOCK, 1991). Controlling vehicle attitude requires

sensors to measure vehicle orientation, actuators to apply the torques needed to reorient

the vehicle to a desired attitude, and algorithms to command the actuators (BISWAS,

2011).

However, it is important not to confuse the dierence between attitude control from

guidance and navigation. Guidance is the control of the inertial position of a device by

the center of the mass (FILHO, 1998) while navigation is the conduction of a vehicle

by observing signicant points in space that serve as reference (LIN, 1991), for instance,

23

celestial navigation.

The attitude control problem of a rigid body has been investigated by several rese-

archers and a wide class of controllers has been proposed (TAYEBI, 2006). So, we can

nd a lot of works that deal with attitude control of quadrotors such as (D'ANDREA,

2014; TAYEBI, 2006; RAFFO, 2009; BASRI, 2014; MARTIN, 2010; BOUABDALLAH,

2007; HANNA, 2014; COSTA, 2012; BORGES, 2015; LEBEDEV, 2013). For example,

D'ANDREA (2014) has developed a controllable attitude control of a quadrotor despite

the complete loss of one, two or three propellers. RASMUSSEN (2013) used the attitude

control principles in weed research. FRAUNDORFER (2012) described an autonomous

vision-based quadrotor for mapping and exploration.

Our model is based on the modelling presented in (BEARD, 2008; LEBEDEV, 2013).

However, we have obtained a relatively satisfactory control result by ignoring the air

resistance and the rotor drag proposed on (HANNA, 2014; LEISHMAN, 2014; MAR-

TIN, 2010), the uncertainties in (BASRI, 2014) and the quaternions in (TAYEBI, 2006;

BORGES, 2015).

Specically, the Crazyie is relatively a simple device to acquire and assemble. Besides,

there are many others low cost quadrotors with open source like Crazyie, for example,

Cheerson CX-204, Arducopter5, Cleanight6 and Aeroquad7. This variety allows the de-

velopment of a range of academical works that deal with attitude control in the system

control, mechatronics and computation applied in quadrotors, such as trajectory tracking

(CARELLI, 2013), backstepping and Lyapunov stabilization (COSTA, 2012), Intercon-

nection and Damping Assignment (ORTEGA, 2005), fuzzy logic (RAZA, 2010), and the

classical technique of PID control used by many researchers as (FRAUNDORFER, 2012),

(BOUABDALLAH, 2004) and (FINK, 2011).

In this work, we are interested in dealing with attitude control and develop a method

to obtain a basic module of this controller that can be used in others quadrotors and

eventually, in a immediate future, we will have an open-source product which will be

possible to use in others projects.

4http://www.rstquadcopter.com/downloads/cheerson-cx-20-rmware-driver/5http://www.arducopter.co.uk/6http://cleanight.com/7http://aeroquad.com/content.php

24

1.3 WORK PROPOSAL AND CONTRIBUTIONS

The control objective is to maintain the quadrotor at a xed pose using the available

onboard sensors. For instance, the Crazyie has an accelerometer, a rate gyro, a magne-

tometer and a barometer. Using the onboard sensor fusion routines (MARTIN, 2010), we

can obtain relatively accurate estimates of the angular orientation and the height of the

quadrotor. We propose a two layered control where we use additional information on the

linear speeds.

Our experiments show that the quadrotor cannot be regulated in a x pose without

the linear speeds feedback control. In this case, it is necessary to make some additional

actions by pilots or by a higher controller in the reference for the angular orientation and

then, control indirectly the linear speeds to regulate the position.

Many works have solved this problem using other external sensors, such as cameras

(BATTISTEL, 2012), image processing and photogrammetry (PRZYBILLA, 1979), which

are used to regulate the pose of the quadrotor. However, we propose an attitude con-

troller that tries to exploit the maximum possible from the quadrotor onboard sensors by

maintaining the quadrotor at a stable pose, without using external sensors.

The contribution of this work demonstrates that the linear speeds feedback control

improves the attitude control quality by regulating the position of the quadrotor. In

other words, it is possible to control the linear speeds and thus, regulate the pose of the

quadrotor.

The task of the controller is to get the ight stability at a xed point on space. For

that, an linear-quadratic regulator (LQR) is designed to operate the dynamic system of

the Crazyie that minimizes the quadratic cost function. Moreover, an integral action is

used to guarantee null steady state error on state feedback control. As it will be shown,

these control architecture will result in a control signal similar to a classic PID controller.

Currently, the estimates of the linear speeds are obtained by integrating of the acce-

lerometer variables and the state observer for linear time invariant systems are not used

due to non-observability with onboard sensors. Due to this fact, a discrete Kalman lter

is implemented on the Crazyie to improve the quality of the speeds data measured by

the sensors.

25

1.4 DEVELOPMENT

Basically, this dissertation was realized in three parts. The rst one consists of bibli-

ographic research about the main concepts and techniques involving the ight dynamics

of the quadrotors. The second part claries how the attitude control design was made via

software Matlab and Simulink. Finally, the last part expounds the experimental results

of attitude control implemented at the real quadrotor.

1.5 DISSERTATION OUTLINE

This dissertation is divided into ve chapters. In chapter 1, we present a brief introduc-

tion about the Unmanned Aerial Vehicles and its utility for modern human, the denition

of attitude control, as well as the development and work proposal of this dissertation. In

chapter 2, we provide a theoretical approach of the dynamical of modelling of a quadrotor,

that includes the coordinate frames, state variables, nonlinear equations, sensors and pa-

rameters. In chapter 3, we describe how to linearize the nonlinear equations and we show

the controller synthesis for inner and outer loops based on a cascade architecture, as well

as the simulation results by Simulink software. In chapter 4, we implement the attitude

control on the Crazyie and we report a tracking reference test. Then, we perform real

ights and we compare the results between the inner and outer loops. Also, we implement

a Discrete Kalman Filter to improve the linear speeds estimate. Finally, in chapter 5, we

bring up the conclusion and the nal considerations with the challenges that occurred

along the development of this work and gives some suggestions for future research.

26

2 QUADROTOR DYNAMICAL MODELLING

This chapter introduces the basic concepts of a quadrotor dynamical system that

provides the crucial information to develop a model to the Crazyie. The reader is

supposed to be familiar with some simple mechanical physical principles, like linear speed,

angular speed, forces, torques and moments of inertia.

The structure of this chapter is as follows. In section 2.1, we report the mathematical

modelling of a quadrotor. In section 2.2, we dene the coordinate frames and how the

Euler angles are generated. In section 2.3, we dene the state variables and, in section

2.4, we explain how to obtain the nonlinear equations.

2.1 MATHEMATICAL MODELLING OF A QUADROTOR

Figure 2.1 shows a schematic model of a quadrotor frame inspired in the Crazyie 1.0

(BITCRAZE, 2015). There are four motors Ri where i ∈ N\1 ≤ i ≤ 4 . Note that the

quadrotor is at the cross + conguration. So, the front of the quadrotor points towards

rotor R1, the back to rotor R3 and they spin counter-clockwise. Also, when looking from

above, the right rotor R2 and the left rotor R4 spin clockwise.

Many quadrotors use the X conguration nowadays, but we chose the cross congu-

ration because it is the default of Crazyie 1.0.

27

FIG. 2.1: Crazyie modelling.

The motors generate at the frame a force Fi and a torque τi according to the direction

of rotation. In the hovering position, the total force cancels the force of gravity while

the total torque is null because τ1 and τ3 cancel τ2 and τ4. For this reason, it is usual

dene the force and torques as control inputs. The total force and torque action on the

quadrotor are given by (BEARD, 2008):

F = F1 + F2 + F3 + F4

τφ = l(F4 − F2)

τθ = l(F1 − F3)

τψ = τ1 + τ3 − τ2 − τ4

(2.1)

where the thrust F is the total force from rotors; l is the distance from the rotors to the

center of mass; τφ is the roll torque; τθ is the pitch torque; and τψ is the yaw torque.

Figure 2.1 also shows the main coordinates frames used in this work, the body frame

(xb, yb, zb) attached to the body of the device and the inertial frame (xi, yi, zi) attached

to a xed ground reference.

The denitions of roll, pitch and yaw are as follows, indicated in gure 2.1. Roll is

28

the rotation movement of left and right wings around the xb axis. Pitch is the rotation

movement of front and back wings around the yb axis. And yaw is the rotation movement

around the zb axis when torque action is dierent to zero.

Besides, it is necessary to report the forces applied on the frame by the rotors to the

actual command sent by the controller board. The outputs of the controller are the PWM

duty cycles δi, i = 1...4, for the Electronic Speed Controller ESCi connected to rotors Ri,

as shown in gure 2.2. Notice that the PWM duty cycle is a real number between 0 and

1, δi ∈ [0,1]. The ESCi applies a voltage vi to the rotor Ri. δi is the duty cycle of the

PWM controllers implemented in the ESC. As a consequence, the propellers rotate in an

angular speed wi and an aerodynamic force Fi and torque τi are produced in the frame,

as shown in gure 2.2.

FIG. 2.2: Model for commands to rotors.

A signicant factor in modelling is to obtain the functions fi = f(δi) and τi = g(σi).

It is important to make the controller sends the right commands for the rotors and these

relations are often experimentally obtained. In (BEARD, 2008), a linear relation is pro-

posed.

Fi = kfδi

τi = ktδi(2.2)

where:

kf is a rotor thrust constant; and

kt is a rotor torque constant.

Alternative approaches can be found in (BOUABDALLAH, 2007), (HANNA, 2014)

and (MARTIN, 2010) which explicitly relate the angular speed ωi with Fi and τi using

29

aerodynamic modelling. Moreover, the dependence on the battery voltage can be taken

into account, as suggested by gure 2.2. For instance, this is the case of Crazyie 1.0 where

no voltage regulator is used between the battery and the ESC. We have not considered

this eect in this work. We have adopted the constants kf and kt experimentally obtained

in (BORGES, 2015).

Following a suggestion in (BORGES, 2015), we dened normalized forces and torques

as:

F ′ = F/kf

τ ′φ = τφ/lkf

τ ′θ = τθ/lkf

τ ′ψ = τψ/kt

(2.3)

Therefore, the relations between generalized forces/torques and the PWM duty cycle

are:

F ′ = δ1 + δ2 + δ3 + δ4

τ ′φ = δ4 − δ2τ ′θ = δ1 − δ3

τ ′ψ = δ1 + δ3 − δ2 − δ4

(2.4)

So, the real PWM commands sent to the motors are then:

δ1 = F ′/4 + τ ′θ/2 + τ ′ψ/4

δ2 = F ′/4− τ ′φ/2− τ ′ψ/4δ3 = F ′/4− τ ′θ/2 + τ ′ψ/4

δ4 = F ′/4 + τ ′φ/2− τ ′ψ/4

(2.5)

This approach has some advantages. First it will allow us to combine all the parameters

of the model into a single lumped parameters, as shown in section 2.4. Also, this will

be useful to concentrate uncertainties of model. Moreover, the quantities F ′, τ ′i will be

normalized to the order of the PWM duty cycles, assuming numerical values compatible

to the duty cycle δi. For instance, since δi ∈ [0,1], F ′ ∈ [0,4], τ ′φ and τ ′θ ∈ [−1,1] and

τ ′ψ ∈ [−2,2].

30

2.2 COORDINATE FRAMES

The reference frames and coordinate systems are used to describe the position and

the orientation of the quadrotor (BEARD, 2008). We assume that the Earth is at due

to the typical range of the missions. Figure 2.3 shows the Inertial frame, Fi, where theorigin is xed on a reference point on the ground(O); xi is aligned to the North direction;

yi is aligned to the East direction; and zi points to the center of the Earth. In addition,

gure 2.3 shows the Vehicle frame, Fv, which is parallel to the inertial frame Fi with the

origin in the center of mass of the quadrotor.

FIG. 2.3: Inertial Vehicle Frame and Vehicle Frame.

The Vehicle-1 Frame (gure 2.4), Fv1 has the same origin as the vehicle Frame Fv.However, Fv1 is obtained by a right-hand rotation about the axis zv and the Euler yaw

angle ψ is dened. Thereby, xv1 points out to the front of the quadrotor.

31

FIG. 2.4: Vehicle-1 Frame.

The matrix of transformation from Fv to Fv1 is:

Rv1v =

cosψ sinψ 0

− sinψ cosψ 0

0 0 1

(2.6)

Another right-handed rotation is done. Now, Fv1 rotates about the axis yv1 and the

Euler pitch angle θ is dened. In that case, the roll angle is null and xv2 points out the

front of the quadrotor, yv2 points along the left-right direction and zv2 points downwards

the frame, as shows gure 2.5. Note that the origin is still the center of the mass.

32

FIG. 2.5: Vehicle-2 Frame.

The matrix of transformation from Fv1 to Fv2 is:

Rv2v1 =

cos θ 0 − sin θ

0 1 0

sin θ 0 cos θ

(2.7)

Lastly, a third right-handed rotation is done about xv2 and we get the Body Frame,

Fb. Hence, the Euler roll angle φ is dened. Again, the origin is the center of the mass,

but the axis xb points out the front of the quadrotor, yb points out the right wing and zb

points out perpendicularly to the frame. Fb is shown in gure 2.6.

33

FIG. 2.6: Body Frame.

The matrix of transformation from Fv2 to Fb is:

Rbv2 =

1 0 0

0 cosφ sinφ

0 − sinφ cosφ

(2.8)

Recall gure 2.1 where we show schematically the inertial frame and the body frame.

All the quadrotor dynamical equations are based on rotations between the inertial frame

and the body frame.

The transformation from Fv to Fb, Rvb is just the matrix product of

Rbv2(φ)Rv2

v1(θ)Rv1v (ψ):

Rbv =

cos(θ) cos(ψ) cos(θ) sin(ψ) − sin(θ)

sin(φ) sin(θ) cos(ψ)− cos(φ) sin(ψ) sin(φ) sin(θ) sin(ψ) + cos(φ) cos(ψ) sin(φ) cos(θ)

cos(φ) sin(θ) cos(ψ) + sin(φ) sin(ψ) cos(φ) sin(θ) sin(ψ)− sin(φ) cos(ψ) cos(φ) cos(θ)

(2.9)

It can be shown that Rvb = (Rb

v)T where T means transpose (BEARD, 2008). Rv

b and

Rbv are the rotation matrices used in the quadrotor dynamics.

34

2.3 STATE VARIABLES

We dene twelve state variables to a quadrotor, grouped in four categories as follows.

The Inertial Positions ~P = [pn pe h]T are measured in meters with respect to the

inertial frame Fi:

pn : North position along xi;

pe : East position along yi; and

h : Height position along −zi (upward vertical).

The Linear speeds ~V = [u v w]T are measured in meters per second with respect to

the body frame Fb:

u : Longitudinal speed along xb-component;

v : Lateral speed along yb-component; and

w : Attitude speed along zb-component.

The angular orientations ~ω = [φ θ ψ]T are represented by the Euler angles, measured

in radians with respect to a sequence of rotations on Fv in order to obtain Fb:

φ : Roll angle obtained from the rotation of Fv2 to Fb;

θ : Pitch angle obtained from the rotation of Fv1 to Fv2; and

ψ : Yaw angle obtained from the rotation of Fv to Fv1.

The angular rates ~Ω = [p q r]T are measured in radians per second with respect to

the body frame Fb:

p : Roll rate along xb-component;

q : Pitch rate along yb-component; and

r : Yaw rate speed along zb-component.

In the next section we describe how to get the nonlinear equations in function of the

state variables.

35

2.4 NONLINEAR EQUATIONS

The following nonlinear equations are obtained by some of the fundamental principles

of physics using Newton-Euler Methods (BEARD, 2008).

Lagrange methods can also be applied as in (BOUABDALLAH, 2007). According

with the kinematic principles, speed is derived from position. However, the quadrotor

positions are in the inertial frame while the linear speeds are in body frame. For this

reason, it is necessary to transform equation Fb into Fv. The translational kinematics is:pn

pe

h

= Rvb

u

v

−w

(2.10)

The translational dynamics can be obtained by second Newton's law.

md~v

dti= ~f (2.11)

Where m is the mass of the quadrotor, ~f is the total force applied to the quadrotor

and ~v is the linear speed in inertial frame of center of mass of quadrotor. ddti

is the time

derivative in inertial frame. We can express d~vdti

in relation to the linear speed ~vb = [u v w]T

and the angular speed ~ωb = [p q r]T , both measured in the body coordinates using the

equation of Coriolis (BEARD, 2008).

md~v

dti= m(

d~vbdtb

+ ~ωb × ~vb) = m

u

v

w

+

0 −r q

r 0 −p−q p 0

u

v

w

= ~f (2.12)

where ddtb

is the time derivative in the body frame.

Consequently: u

v

w

=

rv − qwpw − ruqu− pv

+~f

m(2.13)

We consider thus that only the acceleration of gravity g and the thrust F act on the

body frame. Then:

36

~f

m= Rb

v

0

0

g

− 1

m

0

0

−F

(2.14)

Finally, using (2.14) in (2.13) and making F = kfF′ as in equation (2.3):

u

v

w

=

rv − qwpw − ruqu− pv

+Rbv

0

0

g

− σh

0

0

F ′

(2.15)

where

σh , kf/m (2.16)

F ′ is the generalized thrust force dened by equation (2.3). The equations of rotation

kinematics are not simple to obtain because the Euler angles φ, θ and ψ and the angular

rates p, q and r are in dierent coordinate frames. Thus, we need to transform the yaw

from Fv to Fb, pitch from Fv1 to Fb and roll from from Fv2 to Fb (BEARD, 2008):

p

q

r

= Rbv2(φ)

φ

0

0

+Rbv2(φ)Rv2

v1(θ)

0

θ

0

+Rbv2(φ)Rv2

v1(θ)Rv1v (ψ)

0

0

ψ

(2.17)

p

q

r

=

1 0 − sin(θ)

0 cos(φ) sin(φ) cos(θ)

0 − sin(φ) cos(φ) cos(θ)

φ

θ

ψ

(2.18)

Inverting, we get:φ

θ

ψ

=

1 sin(φ)tan(θ) cos(φ)tan(θ)

0 cos(φ) −sin(φ)

0 sin(φ)/cos(θ) cos(φ)/cos(θ)

p

q

r

(2.19)

Notice that equation (2.19) presents singularities for θ = ±π2. This problem is often

overcome by using quaternions (SARIYILDIZ, 2012), but it is out of the scope of this

work.

37

The rotation dynamics is obtained in the same way as the translational dynamics.

The second Newton's law for rotation is:

d~Pbdti

= ~τ , (2.20)

where ~Pb is the angular momentum and ~τ is the applied torque. Again, using the equation

of Coriolis (BEARD, 2008), we get:

d~P

dti=d~P

dtb+ ~wb × ~P = ~τ (2.21)

The equation (2.21) is resolved in body coordinate by:

~Pb = J ~w. (2.22)

where J is the constant inertia matrix given by (BEARD, 2008):

J ,

Jx −Jxy −Jxz−Jxy Jy −Jyz−Jxz −Jyz Jz

(2.23)

We assume symmetry about all three axis, Jxy = Jxz = Jyz = 0. Then J is given by:

J =

Jx 0 0

0 Jy 0

0 0 Jz

(2.24)

where Jx, Jy and Jz are the moment of inertia on axis xb, yb and zb respectively. Using

(2.22) and (2.21) in (2.20), we obtain:p

q

r

=

qr(Jy − Jz)/Jxpr(Jz − Jx)/Jypq(Jx − Jy)/Jz

+

τφ/Jx

τθ/Jy

τψ/Jz

(2.25)

To simplify the model, the moments of inertia Jx, Jy and Jz, the distance from the

rotors to the center of mass l, the rotor thrust constant kf and the rotor torque constant

kt are concentrated in a unique parameter:

38

σφ ,l·kfJx

σθ ,l·kfJy

σψ , ktJz

(2.26)

Thus: p

q

r

=

qr(Jy − Jz)/Jxpr(Jz − Jx)/Jypq(Jx − Jy)/Jz

+

σφτ

′φ

σθτ′θ

σψτ′ψ

(2.27)

where τ ′φ, τ′θ and τ

′ψ are the generalized torque dened in equation (2.3).

Finally, the nonlinear modelling can be expressed in terms of Euler angles by:

pn = u(cos(θ)cos(ψ)) + v(sin(φ)sin(θ)cos(ψ)− cos(φ)sin(ψ))

− w(sin(φ)sin(ψ) + cos(φ)cos(ψ)sin(θ))

pe = u(cos(θ)sin(ψ)) + v(sin(φ)sin(θ)sin(ψ) + cos(φ)cos(ψ))

− w(cos(φ)sin(ψ)sin(θ)− cos(ψ)sin(φ)

h = −usin(θ) + vcos(θ)sin(φ)− wcos(φ)cos(θ)

u = rv − qw − gsin(θ)

v = pw − ru+ gcos(θ)sin(φ)

w = qu− pv − σhF ′ + gcos(φ)cos(θ)

φ = p+ rcos(φ)tan(θ) + qsin(φ)tan(θ)

θ = qcos(φ)− rsin(φ)

ψ = rcos(φ)/cos(θ) + qsin(φ)/cos(θ)

p = σφτ′φ + qr(Jy − Jz)/Jx

q = σθτ′θ + pr(Jx − Jz)/Jy

r = σψτ′ψ + pq(Jx − Jy)/Jz

(2.28)

Note that the Euler angles are characterized by trigonometric functions with singula-

rities. An alternative approach using quaternios would lead to polynomial equations with

no singularities (MAGNUSSEN, 2013). We have decided not to use quaternions in this

work. One of the reason is that the quaternions do not lead to intuitive knowledge of the

angular orientation, that would bring diculty to analyze the experimental results.

For control purpose, we dene a state vector ~X = [pn pe h u v w φ θ ψ p q r]T and an

input vector ~U = [F ′ τ ′φ τ′θ τ′ψ]T . These vectors will be linearized in chapter 3.

39

2.5 OUTPUT EQUATIONS

The Crazyie has four micro-electro-mechanical (MEMS) sensors: a rate gyro, an

accelerometer, a magnetometer and a barometer.

The rate gyro measures the angular rate of the quadrotor body. The output equation

of the rate gyro is: gx

gy

gz

=

p

q

r

(2.29)

The readings of rate gyro are in degree/seconds. Table 2.1 presents possible settings

for full scale specications of the rate gyro where the default setting of the Crazyie

rmware is indicated by an asterisk (INVENSENSE, 2013).

TAB. 2.1: Rate gyro settings

Range (degrees per second) Degrees per second per LSB± 250 2 × 250/65536± 500 2 × 500/65536± 1000 2 × 1000/65536± 2000* 2 × 2000/65536

The accelerometer measures the dierence between the acceleration of the quadrotor

frame and the acceleration of the gravity. The output equation of the accelerometer is:ax

ay

az

= Rbv

0

0

g

−u

v

w

(2.30)

Applying equation (2.13):ax

ay

az

= − 1

m

0

0

F

−rv − qwpw − ruqu− pv

(2.31)

The readings of accelerometer are in G. Table 2.2 shows the technical specications of

the Crazyie onboard accelerometer where the default setting of the Crazyie rmware is

40

indicated by an asterisk (INVENSENSE, 2013).

TAB. 2.2: Accelerometer settings

Range (G) G per LSB± 2 2 × 2/65536± 4 2 × 4/65536± 8* 2 × 8/65536± 16 2 × 16/65536

The magnetometer measures the coordinate of the Earth magnetic eld with respect

to the body coordinate. Let mN , mE and mD be, respectively, the North, East and Down

components of the Earth magnetic eld in a specied region (INVENSENSE, 2013). The

output equation of the magnetometer is:mx

my

mz

= Rbv

mN

mE

mD

(2.32)

The readings of magnetometer are in Gauss. Table 2.3 presents the settings for the

onboard Crazyie magnetometer where the default setting of the Crazyie rmware is

indicated by an asterisk (INVENSENSE, 2013).

TAB. 2.3: Magnetometer settings

Range (Gauss) Gain (LSB / Gauss)± 0.88 2 × 1370± 1.3* 2 × 1090*± 1.9 2 × 820± 2.5 2 × 660± 4.0 2 × 440± 4.7 2 × 390± 5.6 2 × 330± 8.1 2 × 230

The Barometer measures pressure P in mbar and the temperature T in degree celsius.

The output equation of the barometer is given by Hypsometric equation:

41

h(P,T ) =((Po

P)

15.257 − 1)(T + 273.15)

0.0065(2.33)

where P0 is the reference pressure in sea level and P0 = 1013.5 mbar. Also, we can model

the processed output of the onboard barometer as (SPECIALTIES, 2012)

b = b(P,T ) = h (2.34)

In the current version of sensor fusion routines of the Crazyie, it is possible to obtain

the following variables: the height h from the barometer, the angular rates p, q and r

from the rate gyro; the angular orientations given by the Euler angles φ, θ and ψ either

from the accelerometer and the rate gyro applying a gradient descent algorithm from

(MADGWICK et al, 2011) or a lter from (MAHONY et al, 2008) using the accelerometer

and numerical integration. Also, we can obtain u, v and w in real time by integrating the

accelerometer variables ax, ay and az respectively.

In this work, we use the sensor fusion combining the sensory data of the accelerometer

with the gyroscope and the barometer, as shows gure 2.7. It improves the resulting

information by reducing the uncertainties and increasing accuracy.

FIG. 2.7: Sensor fusion.

Observe the sensor fusion of the Crazyie gives us the following sets of state variables.

42

p

q

r

,φ

θ

ψ

,u

v

w

and[h]

(2.35)

Some remarks:

The measurement of the height h is noisy and inaccurate because it depends on

current values of the pressure and the temperature.

The measurement of the yaw angle ψ is in relation to the angular orientation for that

the device is turned on. The reason is that we would obtain an absolute information

on ψ if the magnetometer data were in use.

There are not position sensors available for the pn and pe variables in the Crazyie

1.0. For obtaining them, we would need external sensors or a camera.

2.6 MODEL PARAMETERS

The table 2.4 exposes the model parameters for the Crazyie 1.0.

TAB. 2.4: Table of the model parameters

Parameter Symbol Value UnitMass m 1.875× 10−2 Kg

Moment of Inertia xb Jx 1.81× 10−5 Kg.m2

Moment of Inertia yb Jy 1.82× 10−5 Kg.m2

Moment of Inertia zb Jz 1.92× 10−5 Kg.m2

Acceleration of gravity g 9.81 m/s2

Thurst constant of rotor kf 8.62× 10−2 N/unitsTorque constant of rotor kt 4.2823× 10−4 N.m/units

Distance from the rotor to the center of mass l 4.25× 10−2 m

The mass m and the distance l were measured using a standard weighing machine

and a rule, respectively. On the other hand, the moments of inertia were obtained by

geometric relations in BORGES (2015).

The constant kf was experimentally obtained in BORGES (2015). Data indicate

parametric variation of kf depending on battery voltage that could be utilized in future

43

works. But we have considered out the scope of this work. Finally, the constant kt was

obtained by the following procedures:

a) A quadratic relation was obtained in BORGES (2015) by interpolation of data ingure 2.8.

FIG. 2.8: Constant torque kt result from a rotor of the Crazyie (BORGES, 2015).

τi = 4 · 10−5δ2i − 2 · 10−4δi (2.36)

where τi is the torque reaction in mN.m and δi is the duty cycle 0 < δi < 1.

b) An operation point δ0 is dened in such a way that the weight from quadrotor

equilibrates with the thrust:

δ0 =F ′04

=mg

4kf= 5.3346 · 10−1 (2.37)

c) The quadratic curve is linearized around the operation point δ0 = (0.5; 1), obtaining

kt = 4.2823 · 10−4, as shown in gure 2.9. This value is obtained by taking the

derivative of the green line.

44

FIG. 2.9: Quadratic curve linearized in x = 0.5 and y = 1.

d) Finally, the lumped parameters, as dened in equations (2.16) and (2.26), are pre-

sented in table 2.5.

TAB. 2.5: Table of lumped parameters

Parameter Symbol Value UnitRoll σφ 2.0240× 102 DimensionlessPitch σθ 2.0129× 102 DimensionlessYaw σψ 2.23036× 101 DimensionlessH σh 4.5973 Dimensionless

2.7 CHAPTER OVERVIEW

In chapter 2, we introduced the equations which describes the system dynamics of the

Crazyie that are decisive for designing a reliably attitude control. In the next chapter

we describe the controller synthesis.

45

3 ATTITUDE CONTROL DESIGN

In this chapter, we present the synthesis of an attitude controller for the Crazyie

quadrotor. We have conceived a controller with a cascade architecture with two control

loops. For the purpose of stabilizing the quadrotor on a hovering position, the inner loop

is responsible for adjusting the angular rates, angular orientations and height while the

outer loop is responsible for regulating the longitudinal and the lateral speeds.

According to (BOJORGE, 2013) in a cascade architecture, the outer loop regulates

the slowest dynamics and determines the set point for the inner loop. In turn, the inner

loop regulates the fastest dynamics and maintains the secondary variables in the desired

set point given by the outer loop controller.

We have tried to control the quadrotor only by using the angular orientations and

the angular rates, but the controller was unable to stabilize the quadrotor at a horizontal

position. Then, we chose the cascade architecture where the inner loop is responsible for

stabilization and the outer loop can be modied unaecting the inner loop. Therefore, it

will be possible to use dierent strategies control in the outer loop in further works.

The structure of this chapter is as follows. In section 3.1, we describe the linearization

of the system. In section 3.2, we explain the synthesis of the inner loop control and,

in section 3.3, we show the simulation results for inner loop control. In section 3.4, we

explain the synthesis of the outer control loop and, in section 3.5 we show the simulation

results for outer loop control. In section 3.6, we describe the decoupling analysis adding a

sine wave as reference for each mode in order to verify the inuence in others. In section

3.7, we implement a Discrete Kalman Filter in order to improve the estimation of the

horizontal speeds. Finally, section 3.8, brings concluding remarks for the chapter.

3.1 LINEARIZATION AND ANALYSIS

In accordance with the previous chapter, we have seen that the dynamical system of

the quadrotor is a set of nonlinear dierential equations (2.28). Also, the presented model

based on Euler angles has polynomial and trigonometric terms with singularities, that

makes the system complex.

Fortunately, the model can be linearized in order to perform a control design. The

46

linearization approximates a complex system to a linear one, allowing to use the linear

system techniques. But it is necessary to verify that the linear controller synthesis is valid

for the original nonlinear system.

In section 2.4, we expressed a set of nonlinear equations given by ~X = f(X,U) where~X = [pn pe h u v w φ θ ψ p q z r]T and ~U = [F ′ τ ′φ τ

′θ τ′ψ]T . Those equations can be

linearized around an equilibrium point ( ~X0, ~U0) for small signals, with the state ~x and the

input ~u for the system ~X = ~X0 + ~x and ~U = ~U0 + ~u, where ~x and ~u are the linearized

state and input, respectively. If the linear model is stable, there is a stable small region

in the nonlinear model around the equilibrium point (FRANKLIN and POWELL, 2013).

We have chosen the equilibrium point for the linearization to coincide with the hovering

situation in a three dimension position space given by (pn0, pe0, h0), in meters, and an

arbitrary orientation ψ0 = 0 rad.

Therefore, we have designed an attitude controller with the objective to maintain the

quadrotor at a xed pose with minimum variation of the horizontal position. Besides, we

do not use sensors from the outside world, using the maximum possible from the Crazyie

embedded sensor.

Let's start the denition of an equilibrium point corresponding to the quadrotor at a

static pose by arbitrating:u0

v0

w0

=

0

0

0

[m/s]

p0

q0

r0

=

0

0

0

[rad/s] (3.1)

By applying the above conditions to the nonlinear system equations ~X = f( ~X,~U), the

following values turn the remaining derivatives null:

F ′0 = mg/kf [N ]

τ ′φ0

τ ′θ0

τ ′ψ0

=

0

0

0

[N.m]

[φ0

θ0

]=

[0

0

][rad] (3.2)

Note that the derivatives are null independently from the value of pn, pe, h and ψ.

The equilibrium point is then:

47

~X0 = [pn0 pe0 h0 0 0 0 0 0 ψ0 0 0 0]T

~U0 = [mg/kf 0 0 0]T

(3.3)

where (pn0 , pe0 , h0) ∈ R3, in meters, and ψ0 ∈ R, in radians. In the following, we have

arbitrated ψ0 = 0 rad.

Applying a linearization method around the equilibrium point, as indicated in (FRAN-

KLIN, 2013), the linear state equations are:

~x = A~x+B~u (3.4)

where

A =

0 0 0 1 0 0 0 0 0 0 0 0

0 0 0 0 1 0 0 0 0 0 0 0

0 0 0 0 0 −1 0 0 0 0 0 0

0 0 0 0 0 0 0 −g 0 0 0 0

0 0 0 0 0 0 g 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 0 0 0 1

0 0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0

(3.5)

48

and

B =

0 0 0 0

0 0 0 0

0 0 0 0

0 0 0 0

0 0 0 0

−σh 0 0 0

0 0 0 0

0 0 0 0

0 0 0 0

0 σφ 0 0

0 0 σθ 0

0 0 0 σψ

(3.6)

It can be veried that the linear model has 12 eigenvalues at the origin and the pair

(A,B) are completely state controllable.

The linear model can be written as the following simple set of linear equations.

pn = u u = −gθ φ = p p = σφτ′φ

pe = v v = gφ θ = q q = σθτ′θ

h = −w w = −σhF ′ ψ = r r = σψτ′φ

(3.7)

Notice the simplicity of the equations of the linearized model. They are composed

of simple rst order dierential equations. Notice also the decoupling of the variables

in (3.7). This is an useful characteristic that will be exploited in the control design. In

gure 3.1, we try to illustrate the decoupling and the relation among the variables in the

linearized model.

49

FIG. 3.1: Blocks of linear model.

Observe there are four decoupling modes in the linearized model:

Vertical mode: connects the thrust F ′0 to the vertical speed w and the height h;

Longitudinal mode: connects the pitch torque τ ′θ to the pitch rate q, pitch angle θ

and longitudinal speed u;

Lateral mode: connects the roll torque τ ′φ to the roll rate p, roll angle φ and lateral

speed v; and

Yaw mode: connects the yaw torque τ ′ψ to the yaw rate r and yaw angle ψ.

We will exploit this decomposition in the controller design in section 3.2.

We work with a cascade control architecture. The inner loop control is responsible for

stabilization by regulating the Euler φ, θ, ψ angles and the height h. It can be shown

that this controller solely is not able to stabilize the quadrotor at a horizontal position.

Considering the possibility of estimating the linear speeds u and v from accelerometer

data, as indicated in (LEISHMAN, 2014), we propose that the outer loop control is

responsible for regulation of the horizontal speeds u and v. The outer loop uses u and v

as reference to determine the set point for the inner loop. Besides, it is possible to use

dierent control strategies for the outer loop unaecting the inner loop. The proposal

architecture is shown in gure 3.2.

50

FIG. 3.2: Decoupling mode.

Therefore, we consider the following decentralized controllers for the inner control

loop.

Pitch Controller: It makes the Euler angle θ tracks a reference θr.

Roll Controller: It makes the Euler angle φ tracks a reference φr.

Yaw Controller: It makes the Euler angle ψ tracks a reference ψr.

H Controller: It makes the height h tracks a reference hr

On the other hand, for the outer control loop we consider the following modes:

U Controller: It was inspired in the dependence of u and θ in the linearization. It

makes the longitudinal speed u tracks a reference ur.

V Controller: It was inspired in the dependence of v and φ in the linearization. It

makes the lateral speed v tracks a reference vr.

51

The decoupling is useful because we can design one controller separately for each

mode. Thereby, we can analyse the overall system response by verifying the stability,

the tracking and the controllability for each mode independently. In case of unwanted

response, we can rebuild a mode controller synthesis without aecting the others.

We have adopted a decentralized control architecture. We could instead use a centra-

lized control architecture not exploiting the decoupling of modes on the linearized model.

In fact we have started our design approach by designing a centralized controller for

the inner loop, using full state feedback.

But we realized that, as a consequence of the decoupling of the linearized model, the

controller gains were separated in blocks relating the variables of the decoupled modes.

Therefore, we decide to use a decentralized design and thus focus on each mode as an

isolated problem. One advantage is that, by using the decoupling, we could make isolated

experiments to analize the performance of each controller, as shown in section 4.4.

Nevertheless, we consider that the inuence of each decentralized controller in the

other can be considered as disturbances. Also, there is the coupling of the variables in the

nonlinear model. Moreover, we performed tests to estimate the coupling of the control

modes as shown in section 3.6.

3.2 CONTROLLER SYNTHESIS DESIGN FOR INNER LOOP

We consider the following controllers for the inner control loop: the Pitch control

regulates the pitch angle θ; the Roll control regulates the roll angle φ; The Yaw con-

trol regulates the yaw angle ψ. These three controllers use data from the sensor fusion

algorithm. Finally, Height control uses the vertical speed to stabilize the height.

Figure 3.3 shows the block diagram of the linear model used on pitch mode controller

synthesis of a quadrotor.

FIG. 3.3: Linear model for θ mode.

According with equation (3.7), θ = q and q = σθτ′θ. This corresponds to the following

state-space representation:

52

[θ

q

]=

[0 1

0 0

][θ

q

]+

[0

σθ

]τ ′θ (3.8)

We suppose that the Crazyie rmware and the sensor fusion routines provides us

accurate estimates of the pitch rate q and the pitch angle θ. We thus work with a

proposal of full state feedback with integral control as shown in gure 3.4.

The structure of the pitch controller is dened in gure 3.4 that corresponds to a

full state feedback with robust tracking by integral error (FRANKLIN and POWELL,

2013). This control scheme leads to zero steady state error for steps reference input. The

reference θr is given to the desired pitch angle θ.

FIG. 3.4: Block diagram for pitch control.

In the control scheme of gure 3.4, the integral state is dened by:

θI = θ − θr (3.9)

Then, the resulting control law is:

τ ′θ = −KIP θI −KPP θ −KDP q (3.10)

Observe that the control law consists implicitly of a proportional action KPP , an

integral action KIP and a derivative action KDP . For instance, the closed loop transfer

function of the system is:

G(s) =θ(s)

θr(s)=

KIPσθs3 +KDPσθs2 +KPPσθs+KIPσθ

(3.11)

An important restriction for the control is the excursion of the actuators. The nor-

malized torque τ ′θ is related with the commands to the rotors R1 and R3 responsible by

pitch movement, as shows gure 3.5.

53

FIG. 3.5: Block diagram for pitch control.

The pitch command is τ ′θ = δ1 − δ3, where δ1 and δ3 are the PWM duty cycles for

rotors R1 and R3, respectively, as we already have seen on equation (2.4). Also, δ1 and

δ3 are limited between 0 and 1, corresponding to the duty cycles ranging from 0 to 100%.

On the other hand, for the perspective of the pitch control, δ1 and δ3 can be related to τ ′θin the following way:

δ1 = F ′0 + τ ′θ

δ3 = F ′0 − τ ′θ(3.12)

where F ′0 is the normalized force that maintains the quadrotor hovering (F ′0 = m · g/kf ).From equation (2.4), F ′0 must be less than δ1+δ2+δ3+δ4 and supposing δ1 = δ2 = δ3 = δ4,

then δi ≥ F ′0/4, we obtain:

F ′04≤ δi ≤ 1 (3.13)

and

F ′04− 1 ≤ τ ′θ ≤ 1− F ′0

4(3.14)

So, the pitch controller must be designed to keep on the excursion of δ1 and δ3 between

0 and 1, considering that they also must include the necessary force to keep on the

quadrotor hovering, F ′0, and possible actions of others controllers such as: the roll, yaw and

54

height controllers. These others inuences on δ1 and δ3 can be considered as perturbations

on the control inputs. The same argument holds for τ ′θ.

The synthesis for the roll, yaw and height modes are analogues to the pitch mode.

The roll mode corresponds to the following state-space equation:

[φ

p

]=

[0 1

0 0

][φ

p

]+

[0

σφ

]τ ′φ (3.15)

As well as pitch, we suppose that the Crazyie rmware and sensor fusion routines

provides us accurate estimates of the roll rate p and the pitch angle φ. It is desirable to

make the roll angle φ follow a desirable reference φr. Thus, the integral state is dened

by:

φI = φ− φr (3.16)

and, the control law is:

τ ′φ = −KIRφI −KPRφ−KDRp (3.17)

The restriction for roll mode is analogue of pitch mode. So, τ ′φ is the same in (3.14).

The yaw mode correspond to the following state-space equation:

[ψ

r

]=

[0 1

0 0

][ψ

r

]+

[0

σψ

]τ ′ψ (3.18)

Again, we suppose that the Crazyie rmware and sensor fusion routines provides us

accurate estimates of the yaw rate r and the yaw angle ψ. It is desirable that the yaw

angle ψ follow a reference ψr. Thus, the integral state is dened by:

ψI = ψ − ψr (3.19)


τ ′ψ = −KIY ψI −KPY ψ −KDY r (3.20)

From equation (2.4), τ ′ψ = δ1 + δ3 − δ2 − δ4 and considering F ′04≤ δi ≤ 1 from

equation(3.13), the restriction for Yaw mode is:

55

−2 +F ′02≤ τ ′ψ ≤ 2− F ′0

2(3.21)

Finally, the height mode is given by:

[h

w

]=

[0 −1

0 0

][h

w

]+

[0

−σh

]F ′ (3.22)

The onboard barometer of the Crazyie measures the pressure P [mbar] and the tem-

perature T [degrees celsius]. By applying the Hypsometric equation, it is possible to

estimate the above sea level height and w by integration. However, this method is inaccu-

rate because h depends on pressure and temperature. An estimator can provides a better

estimation for h and w.

It is desirable to make the height h follow a reference hr. Thus, the integral state is

dened by:

hI = h− hr (3.23)


F ′0 = −KIHhI −KPHh−KDHw (3.24)

The restriction for height mode occurs due to the control action ∆F ′. According with

equation (2.4) and assuming ∆F ′ + F ′0 = δ1 + δ2 + δ3 + δ4, we obtain:

−F ′0 ≤ ∆F ′ ≤ 4− F ′0 (3.25)

The controllers are designed by LQR techniques. Furthermore, the matrices of state-

space representation in the pitch, roll, yaw and height mode are augmented by order 1

due to the additional of the integral states. Figure 3.6 shows the linear θ mode response

for the following inertial conditions: θi = 0 and θ = 1 [rad] and q = 1 [rad/s]. The linear

mode response and the inertial conditions for φ, ψ and h modes are analogues and their

results are shown in gures 3.7, 3.8 and 3.9.

56

FIG. 3.6: Linear analysis response for Pitch Mode.

FIG. 3.7: Linear analysis response for Roll Mode.

57

FIG. 3.8: Linear analysis response for Yaw Mode.

FIG. 3.9: Linear analysis response for Height Mode.

We analyze the linear θ mode response to select the weighting matrices Qaug and Raug

58

by trial and error.

Table 3.1 shows the selected weighting matrices Qaug and Raug and the values of poles

and gains for each mode of inner loop, where diag means diagonal matrix.

TAB. 3.1: Table of weighting matrices for inner loop.

Mode Qaug Raug poles KI KP KD

Roll diag(1,1,1) 100 −0.86638± 0.50061i 0.1 0.17801 0.10844−20.216

Pitch diag(1,1,1) 100 −0.86638± 0.50062i 0.1 0.17804 0.10848−20.0104

Yaw diag(1,1,1) 100 −0.88626± 0.56825i 0.1 0.20962 0.16970−2.0123

H diag(1,1) 100 −0.83460± 0.66885i −0.31623 −0.71026 −0.63952−1.2790

3.3 SIMULATION WITH INNER LOOP CONTROL

In order to analyze the pitch, roll, yaw and height controllers, we perform the simu-

lation of them in inner loop against the nonlinear model. Figure 3.10 shows the simulink

interface for the inner loop.

FIG. 3.10: Simulink for inner loop.

59

The pitch controller block is shown in gure 3.11. The roll, yaw and height controllers

blocks are analogues.

FIG. 3.11: Pitch block.

The sfunctionNonlinear block contains the nonlinear equations that have been descri-

bed by equation (2.28).

The system has been simulated for the following parameters:

Inertial conditions:

pn = pe = h = 0 [m]

u = v = w = 0 [m/s]

φ = θ = ψ = 1 [rad],

p = q = r = 0 [rad/s]

Reference inputs:

hr = 0 [m]

φr = θr = ψr = 0 [rad]

Simulation time: 15 [s].

The simulation response of the system are shown in gures 3.12, 3.13, 3.14, 3.15, 3.16,

3.17, 3.18.

60

FIG. 3.12: Positions (inner loop).

FIG. 3.13: Linear speeds (inner loop).

61

FIG. 3.14: Euler angles (inner loop).

FIG. 3.15: Angular speeds (inner loop).

62

FIG. 3.16: Thrust (inner loop).

FIG. 3.17: Torques (inner loop).

63

FIG. 3.18: Relation between force/torque × Duty Cycle (inner loop).

About the simulation of the system response using only the inner loop control, we can

conclude that:

The quadrotor is not hovering, but moving to north-west slowly. It is easier to see

in gures 3.12 where the position pn increases while the position pe decreases over

time. Also, gure 3.13 shows that the linear speeds u and v are nonzero.

Figures 3.12 and 3.13 show that the output tracking control on H mode is working

well. The states h and w remain in zero after 5 seconds because the reference input

hr = 0.

As well as the H mode, the outputs tracking controls on pitch, roll and yaw modes

are working well too, as can be seen in gures 3.14 and 3.15. Note that the settling

time is 5 seconds for φ, θ, ψ, p, q and r.

The value of thrust is less than 3 N (gure 3.16) and the values of δ1, δ2, δ3 and δ4

are between 0 and 1 (gure 3.18).

64

3.4 CONTROLLER SYNTHESIS DESIGN FOR OUTER LOOP

The inner loop controller solely is unable to stabilize the quadrotor at a horizontal

position. Considering the possibility of estimating the linear speeds u and v from acce-

lerometer data, as indicated in (LEISHMAN, 2014), we propose an external control loop

to regulate the horizontal speeds u and v. The outer loop uses u and v as references to

determine the set point for the inner loop. The main idea is to substitute the references

θr for ur and φr for vr. For that, it is necessary to design a controller synthesis for U

and V modes. Besides, it is possible to use dierent control strategies for the outer loop

unaecting the inner loop.

Figure 3.19 shows the block diagram of the linear model used on U mode controller

synthesis of a quadrotor.

FIG. 3.19: Linear Control for U mode.

According with equation (3.7), u = −gθ. It corresponds to the following state-space

representation:

[u]

=[

0] [

u]

+[−g

][θ] (3.26)

It is desirable to make the linear speed u follow a reference ur.

The structure of the U controller is dened in gure 3.20 that corresponds to a full

state feedback with robust tracking by integral error.

FIG. 3.20: Block diagram for u control.

65

The integral state is dened by:

uI = u− ur (3.27)


θr = −KIUuI −KPUu (3.28)

Observe that the control law treats implicity of a proportional action KPU and an

integral action KIU . For instance, the closed loop transfer function of the system is:

G(s) =θr(s)

ur(s)=

KIU

s(1 +KPU) +KIU

(3.29)

The U mode synthesis aims to control the north position by manipulating linear speed

u in axis x. The main idea is use ur = 0 to keep on the quadrotor hovering in a longitudinal

x position.

The u and v measurements are obtained by accelerometer integration from sensor

fusion. Also, u and v can be estimated by a kalman lter (LEISHMAN, 2014).

The V mode is analogue of U mode. It corresponds to the following state-space

representation:

[v]

=[

0] [

v]

+[g]

[φ] (3.30)

It is desirable to make the linear speed v follow a reference vr. Thus, the integral state

is dened by:

vI = v − vr (3.31)


φr = −KIV vI −KPV φ (3.32)

Observe that the control law treats implicity of a proportional action KPV and an

integral action KIV . For instance, the closed loop transfer function of the system is:

G(s) =φr(s)

vr(s)=

KIV

s(1 +KPV ) +KIV

(3.33)

66

The V mode synthesis aims to control the east position by linear speed v in axis y.

The main idea is use vr = 0 to keep on the quadrotor hovering in a lateral x position.

Again, the controllers are designed using LQR techniques. Furthermore, the matrices

of state-space representation in the U and V modes are augmented by order 1 due to the

additional of the integral states. Figure 3.21 shows the linear U mode response for the

following inertial conditions: ui = 0 [m/s] and u = 1 [m/s]. The linear mode response

and the inertial conditions for V mode are analogues and the results are shown in gure

3.22.

FIG. 3.21: Linear analysis response for U Mode.

67

FIG. 3.22: Linear analysis response for V Mode.

The linear analysis assists in the gain selection. Then, the table 3.2 shows the selected

weighting matrices Qaug and Raug and the values of poles and gains for each mode of outer

loop. Notice that the poles of U and V modes are closer to the origin than the poles of

roll and pitch mode. It means that the system response of U and V modes are slower

than the roll and pitch modes.

TAB. 3.2: Table of weighting matrices for outer loop

Mode Qaug Raug poles KI KP

U diag(1,1 · 103) 106 −0.10647 −3.1623 · 10−3 −4.0555 · 10−2

−0.29138V diag(1,1 · 103) 106 −0.10647 3.1623 · 10−3 4.0555 · 10−2

−0.29138

3.5 SIMULATION WITH OUTER LOOP CONTROL

In order to analyse the U and V controller, we perform the simulation of them in

closed loop.

Figure 3.23 shows the simulink interface for the outer loop.

68

FIG. 3.23: Simulink for outer loop.

The U controller block is shown in gure 3.24. The v controller block is analogue.

FIG. 3.24: U controller block.

The sfunctionNonlinear block contains the nonlinear equations that have been descri-

bed in equation (2.28).

The system has been simulated for the following parameters:

Inertial conditions:

pn = pe = h = 0 [m]

u = v = w = 0 [m/s]

φ = θ = ψ = 1 [rad],

p = q = r = 0 [rad/s]

69

Reference inputs:

hr = 0 [m]

ur = vr = 0 [m/s]

ψr = 0 [rad]

Simulation time: 60 [s].

The nonlinear simulation responses of the system are shown in gures 3.25, 3.26, 3.27,

3.28, 3.30, 3.29, 3.31.

FIG. 3.25: Positions (outer loop).

70

FIG. 3.26: Linear speeds (outer loop).

FIG. 3.27: Euler angles (outer loop).

71

FIG. 3.28: Angular speeds (outer loop).

FIG. 3.29: Thrust (outer loop).

72

FIG. 3.30: Torques (outer loop).

FIG. 3.31: Relation between force/torque × Duty Cycle (outer loop).

About the simulation of the system response for the outer closed loop control, we can

73

conclude that:

The quadrotor hovers in a xed position. It is easier to see in gures 3.25 where the

positions pn and pe remain in a x value after 15 seconds. Also, gure 3.26 shows

that the linear speeds u and v are zero in steady state regime.

Due to the decoupling, H and Yaw modes were not aected in the nonlinear model.

In contrast with the inner closed loop, gure 3.27 shows that all the Euler angles

have the same settling time in 15 seconds.

The reference inputs θr and φr in inner loop are replaced by ur and vr in outer loop,

respectively. However, the addition of the outer loop does not aect the angular

rates signicantly (gure 3.28).

The maximum value of thrust is 3 N (gure 3.29) and the values of δ1, δ2, δ3 and δ4

are between 0 and 1 (gure 3.31).

3.6 DECOUPLING ANALYSIS

We have simulated the inner and outer controllers applying a sine wave as reference

in the nonlinear model under the actuation of our proposed controller in each mode in

order to verify the decoupling of the modes.

For that, we have applied three sine waves with constant amplitude of 1 peak and

angular frequencies ω = 0.1, 1 and 10 rad/s in the references signals separately, observing

the inuence on the state variables. Table 3.3 summarizes the results for the inner loop

controller while table 3.4 is related to the outer loop controller. Both controllers were

connected to the nonlinear model in simulink as in gures 3.10 and 3.23.

TAB. 3.3: Decoupling analysis for inner loop.

h u v w φ θ ψ p q rhr X Xφr X X X X Xθr X X X X Xψr X X X X

74

TAB. 3.4: Decoupling analysis for outer loop

h u v w φ θ ψ p q rhr X Xur X * *vr X * *ψr X X

In the tables 3.3 and 3.4, the symbol X indicates a notorious change in the variables

in the table rst row by applying the sine wave in the corresponding reference in the table

rst column. On the other hand, ∗ indicates insignicant modications that have been

ignored and a blank space indicates no perceived inuence.

According with the tables 3.3 and 3.4, the height mode in both loops does not inuence

the other modes, as can be seen on gure 3.32 where we illustrate that there is no perceived

inuence of changes in the height mode on the other relevant modes.

75

FIG. 3.32: Simulation using a sine wave as reference on height mode. The top viewcorresponds to the inner loop, and the bottom to the outer loop.

The pitch mode inuences the height mode (h and w) because it aects the thrust

directly. Also, the longitudinal speed u and the roll rate q oscillate in consequence of the

Euler angle θ oscillation. The other modes are unaected, as we can see on the illustration

in gure 3.33. The same argument occurs to the roll mode, as illustrate in gure 3.34.

76

FIG. 3.33: Simulation using a sine wave as reference on pitch mode.

77

FIG. 3.34: Simulation using a sine wave as reference on roll mode.

In gure 3.35, a sinusoidal reference in the yaw mode for inner loop aects the longitu-

dinal and lateral speeds u and v respectively because that these variables are uncontrolled

by this controller. On the other hand, the outer loop acts on those variables and regulate

them. We have omitted the graphic for yaw rate r but it is analogous to roll and pitch

rates. The other modes were unaected.

78

FIG. 3.35: Simulation using a sine wave as reference on yaw mode. The top viewcorresponds to the inner loop, and the bottom to the outer loop.

At last, the gure 3.36 shows the results for U and V modes. Generally, the outer loop

does not aect the inner loop modes. Besides, note that the outer loop acts on regulating

the horizontal positions pn and pe. Consequently, u and v waveforms have a very low

amplitude.

79

FIG. 3.36: Simulation using a sine wave as reference on U and V modes. The top viewcorresponds to the U mode loop, and the bottom to the V mode.

3.7 USE OF A DISCRETE KALMAN FILTER TO ESTIMATE LINEAR SPEEDS

3.7.1 THE DISCRETE KALMAN FILTER

The Discrete Kalman Filter is a recursive algorithm that produces estimates of unk-

nown variables using a sequence of measures observed over time that contain inaccuracies

and noises. The plant is supposed to be a Linear Discrete-Time Variant System8.

8The informations of this section were retrieved from (ALAZARD, 2006), (BORGES, 2015) and(MATHWORKS, 2016)

80

FIG. 3.37: Block diagram: Discrete Kalman Filter.

The state equation is given by:

xk = Φkxk−1 + Γkuk−1 +Gwk−1 (3.34)

And the measurement equation is:

zk = Ckxk +Dkuk + vk (3.35)

The signals wk and vk represent the noises on the process and on the measures, res-

pectively. They are supposed to be gaussian white noises with null mean.

Process noise wk:

wk = N(0,Qk)

Qk = Ewk · wTk Ewk · wj = 0,∀j 6= k

Ewk = 0

(3.36)

Measurement noise vk:vk = N(0,Rk)

Rk = Evk · vTk Evk · vj = 0,∀j 6= k

Evk = 0

(3.37)

WhereQk and Rk are covariances matrices and E means expectation. The pair (Φk,Ck)

81

must be detectable (MATHWORKS, 2016). It means that, for a partially observable

system, if the unobservable modes are stable and the observable modes are unstable,

the system is said to be detectable. (OGATA, 2003). Besides, Rk must be invertible

(ALAZARD, 2006).

We dene x−k as the priori state estimate xk and xk the posteriori state estimate xk.

The priori and posteriori estimate errors are given by:

e−k = xk − x−kek = xk − xk

(3.38)

And the priori and posteriori covariance error matrices are:

P−k = E[e−k e−Tk ]

Pk = E[ekeTk ]

(3.39)

The estimate process occurs in two steps. The prediction step provides the priori

estimate xk and the priori covariance error P−k , along with their uncertainties. The update

step receives the measure vector zk and computes the Kalman gain Kk, the posteriori

estimative xk and the posteriori covariance error Pk.

Predict Equations:

Project the state ahead: x−k = Φkxk−1 + Γkuk−1

Project the error covariance ahead: P−k = ΦkPk−1ΦTk +Qk.

Update Equations:

Compute the Kalman Gain: Kk = P−k CTk (CkP

−k C

Tk +Rk)

−1

Update estimate with measurement zk: xk = x−k +Kk(zk − Ckx−k −Dkuk).

Update the error covariance: Pk = (I −KkCk)P−k

The initial conditions x0 and P0 must be provided for the rst interaction. Figure 3.38

shows the predict and current process and the respective equations.

82

FIG. 3.38: Steps of Discrete Kalman Filter.

The Extended Discrete Kalman Filter (EDKF) is the nonlinear version of the Discrete

Kalman Filter. The EDKF performs the linearization of the system for each iteration

around the posteriori estimate from the previous iteration (ALAZARD, 2006).

Consider the nonlinear system:

xk = fk(xk−1,uk−1)

zk = gk(xk)(3.40)

where the fk describes the transition between the consecutive states and gk describes

the measurements obtained by sensors. The Jacobian computation denes the following

matrices:

Φk =∂fk∂xk

(3.41)

Γk =∂fk∂Uk

(3.42)

Ck =∂gk∂xk

(3.43)

3.7.2 THE FILTER TO THE HORIZONTAL SPEEDS

To implement the Discrete Kalman Filter on Crazyie, we have worked with the

following model for the longitudinal speed u:

83

u = −g · θ − µ · uµ = δ

ax = µ · u

(3.44)

where µ is the air drag coecient, δ is a random variable with null mean and ax is

the measurement of the x-component of the accelerometer. This model for the air drag

coecients is reported in (LEISHMAN, 2014). The lateral model is analogous to the

longitudinal model. For this reason, we describe only the longitudinal model.

To apply the Kalman lter equations, we must write equations (3.44) in discrete time.

For that, consider x ≈ xk−xk−1

Twhere T is the sampling time. Applying on the longitudinal

model, we obtain:

uk−uk−1

T= −gθk−1 − µk−1uk−1 (3.45)

µk−µk−1

T= δk−1 (3.46)

That leads to:

uk = (1− Tµk−1)uk−1 − gTθk−1 (3.47)

µk = µk−1 + Tδk−1 (3.48)

For the longitudinal mode, we dene the state vector = [uk µk]T , the output ak and

the input θk.

F (Xk,Uk) =

[(1− Tµk)uk−1 − gTθk−1µk−1

µk−1

](3.49)

G(Xk,Uk) = µkuk (3.50)

Observe that we consider δk−1 = 0 in equation (3.48).

The Jacobian of the model for longitudinal speed is:

Φk =∂Fk∂Xk

=

[1− Tµk−1 −Tuk−1

0 1

](3.51)

84

Γk =∂Fk∂Uk

=

[−gT

0

](3.52)

Ck =∂Gk

∂Xk

=[µk−1 uk−1

](3.53)

We choose the following values: The sampling time T = 4 ms since the rmware

frequency is 250 Hz. The covariance matrices are arbitrated to be Rk = 5 and Qk =

diag(1,1), that is a identity matrix with order 2.

The Kalman gain equation calculates a matrix inversion for each iteration, in the

computation of the Kalman gain. This inversion is a problem when implemented on a

microcontroller. To solve that, we have developed the recursive equations analitically in

order to transform the matrix inversion in algebraic expressions using Matlab symbolic

tool box, and we have obtained a set of equations that were directly implemented in the

rmware code.

In order to analize the estimates for the horizontal speeds and air drag coecients, we

have simulated the outer loop control with a Kalman lter, as shows gure 3.39.

FIG. 3.39: Simulink for outer loop with Kalman lter.

In gure 3.40 we illustrate the longitudinal and lateral speeds estimated ue and ve

85

respectively, and in gure 3.41 we show the air drag coecients µue and µve .

FIG. 3.40: Horizontal speeds estimation.

FIG. 3.41: Air drag coecients estimation.

86

Notice that, in the nonlinear model we have made the drag coecients null. Comparing

the gure 3.40 with the gure 3.26, we perceive that the horizontal speeds estimated ue

and ve are more damped than u and v.


This chapter showed that the controller synthesis is done by linearization of the non-

linear equations presented in chapter 2 and by decoupling of the linear equations in six

modes to control each movement of the Quadrotor, separated in two layers. The next

chapter shows the controllers modes implemented on Crazyie and we perform reference

tests and experimental ights to verify if the outer loop improves the position stability.

87

4 IMPLEMENTATION AND TESTS

4.1 INTRODUCTION

In this chapter, we register the implementation of the developed attitude controller

in the Crazyie. For that, we have made changes in the original rmware and PC client.

Then, we have executed a series of experiments and analysed the results on Matlab.

First, we have done a tracking test for Pitch, Roll and Yaw modes. In order to verify

the purpose of this work, we have compared the performance of real ights between the

inner and outer loops. Also, we have performed ights using the height control for both

loops. Finally, we have implemented a discrete Kalman lter to improve the longitudinal

and lateral speeds estimation.

4.2 FIRMWARE

The Crazyie rmware architecture is based in the real time operation system FreeR-

TOS which runs dierent tasks such as radio communication, stabilization, power mana-

gement, etc. (BITCRAZE, 2015).

In gure 4.1, we illustrate the apparatus used to control the Crazyie 1.0. The user

sends the input commands for the computer using a joystick. The computer sends control

data for the Crazyie in real time and the communication between them is done by the

Crazyradio, a 2.4 GHz radio USB dongle. Also, the PC client receives data in real time

from the Crazyie. This is illustrated in gure 4.2 by block diagram.

88

FIG. 4.1: Hardware Diagram.

FIG. 4.2: Block diagram of the communication between the PC client and the rmware.

We illustrate the rmware architecture in gure 4.3. The MPU6050 is an integrated

6-axis MotionTracking device that combines a 3-axis gyroscope, 3-axis accelerometer and

a Digital Motion Processor. It accepts inputs from an external 3-axis compass to provide

a complete 9-axis MotionFusion output (INVENSENSE, 2013). The gyroscope sends data

in degree/second to the sensor fusion lter while the accelerometer sends data in Gal. An

inter-integrated Circuit (I2C) serial bus connects the MPU6050 with the sensor fusion

lter.

All the sensors are embedded and the setpoints can be obtained by remote control via

joystick or selecting them in the parameter window of the PC client.

89

FIG. 4.3: Crazyie rmware architecture (Retrieved fromhttps://wiki.bitcraze.io/projects:crazyie:rmware:arch)

.

It is possible to visualise the data by an interface programme, the Crazyie PC Client,

as shows gure 4.4.

FIG. 4.4: Software Crazyie PC client (ight control window)

Figure 4.4 shows the main window of the PC client, where we can monitor the piloting

of the Crazyie.

Figure 4.5 shows the Log TOC (table of contents), the variables that are send from

the rmware to the PC client, that is, the telemetry.

90

Figure 4.6 show the Parameters, were we can change parameters in the rmware.

FIG. 4.5: Software Crazyie PC client (log toc window)

FIG. 4.6: Software Crazyie PC client (parameter window)

4.3 FIRMWARE CHANGES

The aim of this subsection is to present the modied routine for the stabilizer.c library

of the Crazyie rmware. The stabilizer.c library contains the stabilizer routine respon-

91

sible for the control loop of the quadrotor. We present a pseudocode with the modied

routine and a table with the variables. We expect that the material presented in this

subsection serves as a guide for using the routine.

Initially, we show a simple owchart of the modied routine in gure 4.7. In this

owchart we seek to indicate the main performed operations and the possibles operation

modes. The controllers of the attitude and the height can be activated and deactivated

by ags from the rmware, as shown in gure 4.7. We have tried a way of performing a

simplied execution of the control loop in comparison to the original stabilizer routine.

In appendix 7.1 we explain the stabilizer routine changes.

A brief description of the owchart in gure 4.7 is presented below. After the initiali-

zation of the stabilizer routine, it begins a loop of 250 Hz for the attitude control. In this

loop the update and processing of the controller inputs are performed.

In particular, the reading of signals from sensors and the sensor fusion routine are

activated to obtain the Euler angles. Also, some variables are computed by integration

and the Kalman lter is updated (subsection 3.7.1. If commanded by PC client, some

variables are restarted and the variables for controller processing are conditioned.

The attitude controller is indicated by the ag activateAttitude. When it is activated,

the controller computations begin and the outputs are updated.

In the processing attitude controller, the controller references are updated from the

joystick commander or from the PC console. Then, if the height controller is activated,

indicated by the ag activateAttitude, it is made the correspondent computations (section

3.2). After that, the controllers outputs of the outer and inner loops are computed

successively, being the output of the outer loop directed to feedback some references

inputs of the inner loop (sections 3.2 and 3.4). Finally, the stabilizer output routine is

updated, that are the PWM duty cycles for the rotors based on the controller output.

92

FIG. 4.7: owchart of the modied stabilizer routine.

93

4.4 ISOLATED TESTS

We have implemented the roll, pitch and yaw modes controllers on the real quadrotor

and tested them isolately. First, we tied the quadrotor between two support columns in a

way to allow only one movement, as in gure 4.8, where we show the setup for the pitch

controller.

FIG. 4.8: Crazye tied between to columns for pitch control test.

The main idea is to check if the output θ tracks the reference input θr. For that, we

have started the test with θr = 0 rad and after 15 seconds, we changed θr to 5 rad, 0 rad

and -5 rad (within a period of 20 seconds). We have compared the measured output curves

with the expected outputs from the simulation of the linear model, under the action of

the respective mode controller, and subject to the same reference input. The results are

shown in gures 4.9.

94

FIG. 4.9: Output curves of the pitch mode comparing the reference input, the measuredpitch angle and the simulated pitch angle.

The reference tracking tests for the roll and yaw modes are analogues to the pitch

tracking. Figures 4.10, 4.11, 4.12 and 4.13 show how the Crazyie was tied for roll and

yaw movement and the results.

FIG. 4.10: Crazye tied between to columns for roll control test.

95

FIG. 4.11: Output curves of the roll mode comparing the reference input, the measuredroll angle and the simulated roll angle.

FIG. 4.12: Crazye tied between to columns for yaw control test.

96

FIG. 4.13: Output curves of the yaw mode comparing the reference input, the measuredyaw angle and the simulated yaw angle.

Despite of the wind force generated by the rotation of the propellers causes oscillations

at the quadrotor, observe that the recorded pitch, roll and yaw curves track the reference

signals with a behaviour very similar to the expected with the linear model. However,

the yaw tracking requires more time to be stabilized than roll and pitch tracking and it

is aected by elastic tension of rope.

4.4.1 HEIGHT CONTROL

To stabilize the Crazyie in an arbitraty height, the h state must tracks the reference

hr. Figure 4.14 shows a ying performance with the height control on.

97

FIG. 4.14: Height control on.

When we turn on the height controller, the h state tracks the reference hr in both

loop. Due to decoupling, the outer loop does not aect the height control of inner loop.

4.4.2 LONGITUDINAL AND LATERAL SPEEDS AND EULER ANGLES

To stabilize the Crazyie at an arbitrary xed 3D position, the linear speeds u and v

and the Euler angles φ, θ and ψ must be zero. Figure 4.15 shows that the speeds u and v

are hard to control for inner loop. Notice that |umax| = 2.0 [m/s] and |vmax| = 2.5 [m/s]

and those speeds diverge from zero because the inner loop lacks speed reference signals.

98

FIG. 4.15: Longitudinal and lateral speeds in inner loop.

For outer loop, we have now the speed reference signals as shown gure 4.16 and

notice that the speeds decrease signicantly to |umax| = 0.4 [m/s] and |vmax| = 0.7 [m/s].

Besides, u and v oscillate around the reference signal. Although the outer loop cannot

stabilize the Crazyie at a xed 3D position using only the linear speeds as reference

signals, the outer loop improves the regulation of position expressively.

99

FIG. 4.16: Longitudinal and lateral speeds in outer loop.

Figures 4.17 and 4.18 show the results for Euler angles. Remember that the inner loop

has the Euler angles φr, θr and ψr as reference signals and the outer loop replaces θr and

φr for ur and vr respectively.

For inner loop, the Euler angles states are stables and track their respective references.

Despite the replacement of the references signal, the results for outer loop do not show

signicant changes.

100

FIG. 4.17: Euler angles in inner loop.

FIG. 4.18: Euler angles in outer loop.

The longitudinal and lateral speeds were calculated by integrating the accelerometer

101

outputs for the inner and outer controllers. In the subsection 4.4.3, we show the hori-

zontal speeds provided by discrete Kalman lter and we compare the speeds obtained by

integration and estimation.

4.4.3 KALMAN FILTER

The linear speeds u and v were rst obtained by integration of ax and ay, respectively.

To improve the estimate of linear speeds, we have developed a discrete Kalman lter as

shown in section 3.7.1.

We have performed some experiments to Kalman lter in order to validate the esti-

mates. In gure 4.19, we have recorded the estimated data with the Crazyie standing

still over a plane surface.

FIG. 4.19: The estimates of ue and ve with the Crazyie standing over a surface.

Variables ue is the estimated longitudinal speed and ve is the estimated lateral speed.

The estimated speeds converge close to zero (the amplitude of ue and ve is less than 0.1

after 20 seconds). However, they do not reach zero because the rmware is estimating ue

and ve constantly.

In gure 4.20, we have recorded the estimated data from a real ight using only the

inner loop.

102

FIG. 4.20: The estimates of ue and ve closing only the inner loop.

Although the inner loop is unable to control the horizontal speeds appropriately, the

estimated speeds are closer to zero after 7 seconds. So, the discrete Kalman lter provides

a good perform for the horizontal speeds in the inner loop control even this controller does

not use those speeds as reference.

Figure 4.21 shows a ying performance using the discrete Kalman lter to estimate

the longitudinal and lateral speeds ue and ve, respectively.

103

FIG. 4.21: Feedback with discrete Kalman lter with estimation of ue and ve.

According with the gure 4.21, it is notorious that the estimated states ue and ve

perform a better reference tracking than the states u and v obtained by integration.

Comparing the feedback control for the outer loop using estimation (gure 4.21) and

integration (gure 4.16), observe that u and v oscillate around the reference in integration,

but do not reach the reference in estimation. However, the estimation uses the signals ue

and ve (not u and v) in feedback control that are closer to the reference zero. For this

reason, we conclude that the discrete Kalman lter provides better results in reference

tracking.


We have shown in this chapter that the outer loop improves the 3D position regulation.

We present, in the next chapter, the conclusion of this master dissertation and suggestions

for future works.

104

5 CONCLUSION

This work presents a mathematical modelling of a quadrotor using the Euler angles

representation to describe the orientation of the aircraft. An attitude control has been

implemented on the platform Crazyie, a micro quadrotor from the Bitcraze company.

The attitude control is composed by decoupling modes which perform each movement

of the quadrotor independently, using a cascade architecture with two loops.

The inner loop is responsible to stabilize the angular orientation and the outer loop

is responsible for regulating the horizontal position using the longitudinal and the lateral

speeds as reference signals. The contribution of this work demonstrates that the outer

loop improves the horizontal position regulation using only the sensors embedded on the

quadrotor.

An LQR controller with integral action was designed for each decoupling mode to

optimize the dynamics responses. Following a suggestion from (BORGES, 2015), an

observer based on Extended Kalman Filter have been implemented to improve the outer

loop performance and the results show a better quality of longitudinal and lateral speeds

estimation.

For future works, the author suggest:

To develop a synthesis for an attitude controller based on cascade architecture using

quaternions instead of Euler angles. The quaternions are a number system that

extends the complex number, being represented by polynomial equations instead of

trigonometric equations and they are singularity free. For this reason, quaternions

might improve the stability of angular orientation.

According with (RAFFO, 2009), a nonlinear H∞ control was developed to achieve

robustness in the presence of disturbances, parametric and structural uncertainties.

For this reason, a nonlinear H∞ control can be develop and implement on Crazyie.

Also, the reachability analysis techniques can be used in order to investigate and

improve the robustness of the controller.

To improve the Kalman lter to estimate the speeds u and v.

To use sensor fusion to improve the estimate of the height and vertical speed w.

105

The use of external sensors, like a camera or kinect, improve the positions regulation

and allows the Crazyie 1.0 performs autonomous ights.

The Crazyie 1.0 has a ight time up to 7 minutes and a maximum range of 80

meters. With a GPS module and a camera, it is possible to make a short mission

planning in a small area, for example, to capture images for mapping and photo-

grammetry.

To perform ight safety tests.

To test the Crazyie 1.0 robustness due to failures in one and two propellers. This

has been already done by (D'ANDREA, 2014) using a dierent quadrotor.

106

6 REFERENCES

ALAZARD, D. Introduction au ltre de Kalman. École Nationale Supérieure del'Aéronautique et de l'Espace, Octobre 2006.

ANAC. Força Aérea esclarece normas para voos de drones no Brasil, 2015. URLhttp://www.brasil.gov.br/defesa-e-seguranca/2015/03/forca-aerea-esclarece

-normas-para-voos-de-drones-no-brasil.

BASRI, M. A. M., HUSAIN, A. R. e DANAPALASINGAM, K. A. Intelligent AdaptiveBackstepping Control for MIMO Uncertain Nonlinear Quadrotor Helicopter System.Transactions of the Institute of Measurement and Control, October 2014.

BATTISTEL, A. e SILVA, A. L. D. Voo Autônomo e Estabilização de Câmera em umVeículo Aéreo Não-Tripulado Tipo Quadrirotor. Congresso Brasileiro de Automática,2012.

BEARD, R. W. Quadrotor Dynamics and Control. Brigham Young University, October2008.

BISWAS, R. e DIVYA, C. M. Control Momentum Gyro's Gimbal Control Using FPGA.International Journal for Research in Applied Science & Enginnering Technology, 2011.

BITCRAZE. Block diagram of stabilization system, 2015. URLhttps://wiki.bitcraze.io/projects:crazyflie:firmware:arch.

BLAKELOCK, J. H. Automatic Control of Aircraft and Missiles. Wiley-Interscience,1991.

BOJORGE, N. Malhas de Controle Avançado. Departamento de Engenharia Química ede Petróleo. Universidade Federal Fluminense, 2013.

BORGES, L. M., SANTOS, E. G. D. e MAGALHÃES, G. D. M. Controle de Atitude deum Quadricoptero. Instituto Militar de Engenharia, Outubro 2015.

BOUABDALLAH, S. Design and Control of Quadrotors with Application to AutonomousFlying. École Polytechnique Fédérale de Lausanne, February 2007.

BOUABDALLAH, S., NOTH, A. e SIEGWART, R. PID vs LQ Control Techniques Ap-plied to an Indoor Micro Quadrotor. IEEE Intelligent Robots and Systems, 2004.

CARELLI, R., AO, A. S. B., PIZETTA, I. H. e FILHO, M. S. Controle Líder Seguidor deuma Formação Vant VTVN. Simpósio Brasileiro de Automação Independente, 2013.

COSTA, E. B. Algoritmos de Controle Aplicados á Estabilização do Voo de um Quadrotor.Dissertação de Mestrado, Universidade Federal de Juiz de Fora, November 2012.

107

D'ANDREA, R. e MUELLER, M. W. Stability and control of a quadrocopter despitethe complete loss of one, two or three propellers. IEEE International Conference onRobotics and Automation, May 2014.

FILHO, W. D. C. L. Plataforma de Attitude Solidária. Segundo Simpósio Brasileiro deEngenharia Inercial, 1998.

FINK, J., MICHAEL, N., KIM, S. e KUMAR, V. Planning and control for cooperative ma-nipulation and transportation with aerial robots. The International Journal of RoboticsResearch, 2011.

FRANKLIN, G. F., POWELL, J. D. e EMAMI-NAENINI, A. Feedback Control of Dyna-mic Systems. Bookman, 6 edition, 2013.

FRAUNDORFER, F., HENG, L., HONEGGER, D., LEE, G. H., MEIER, L., TANS-KANEN, P. e POLLEFEYS, M. Vision-Based Autonomous Mapping and ExplorationUsing a Quadrotor MAV . IEEE/RSJ International Conference on Intelligent Roboticsand Systems, October 2012.

HANNA, W. Modelling and Control of an Unmanned Aerial Vehicle. Dissertação deMestrado, Charles Darwin University, May 2014.

HEHN, M. e D'ANDREAS, R. A Flying Inverted Pendulum. IEEE International Confe-rence on Robotics and Automation, 2011.

INVENSENSE. MPU-6000 and MPU-6050 Product Specication Revision 3.4, 2013. URLwww.invensense.com.

LEBEDEV, A. Design and Implementation of a 6DOF Control System for an AutonomousQuadrocopter. Dissertação de Mestrado, Universität Würzburg, September 2013.

LEISHMAN, R. C., JR., J. C. M., BEARD, R. W. e MCLAIN, T. W. Quadrotors andAccelerometers - State Estimation with an Improved Dynamic Model. IEEE ControlSystems Magazine, February 2014.

LIN, C.-F. Modern Navigation, Guidance, and Control Processing. Prentice Hall, 1991.

MADGWICK, S. O. H., HARRISON, A. J. L. e VAIDYANATHAN, R. Estimation ofIMU and MARG orientation using a gradient descent algorithm. IEEE InternationalConference on Rehabilitation Robotics, June 2011.

MAGNUSSEN, O., OTTESTAD, M. e HOVLAND, G. Experimental Validation of aQuaternion-based Attitude Estimation with Direct Input to a Quadrotor Control System.International Conference on Umanned Aircraft Systems, May 2013.

MAHONY, R., HAMEL, T. e PFLIMLIN, J.-M. Nonlinear Complementary Fiters on theSpecial Orthogonal Group. IEEE Transactions on Automatic Control, June 2008.

MARTIN, P. e SALAÜM, E. The True Role of Acceleration Feedback in Quadrotor Control.IEEE International Conference on Robotics and Automation, May 2010.

108

MATHWORKS. MATLAB DOCUMENTATION - Function for Kalman Filter design incontinuous and discrete time, 2016.

OGATA, K. Modern Control Engineering. Pearson, 4 edition, 2003.

ORTEGA, R., VAN DER SCHAFT, A., MASCHKE, B. e ESCOBAR, G. Interconnectionand damping assignment passivity-based control of port-controlled Hamiltonian systems.IEEE Transactions on Automatic Control, December 2005.

PIGOTT, R. Heathrow plane in near miss with drone, 2014. URLhttp://www.bbc.com/news/uk-30369701.

PRZYBILLA, H.-J. e WESTER-EBBINGHAUS, W. Bildug mit ferngelenkten kleinug-zeug Bildmessung und Luftbildwesen. Herbert Wichman Verlag, 1979.

RAFFO, G. V., ORTEGA, M. G. e RUBIO, F. R. An integral predictive/nonlinear H∞control structure for a quadrotor helicopter . www.elsevier.com/locate/automatica, No-vember 2009.

RASMUSSEN, J., NIELSEN, J., GARCIA-RUIZ, F., CHRISTENSEN, S. e STREIBIG,J. . C. Potential uses of small unmanned aircraft systems in weed research. An inter-national Journal of Weed Biology, Ecology and Vegetation Management, April 2013.

RAZA, S. A. e GUEAIEB, W. Intelligent Flight Control of an Autonomous Quadrotor.University of Ottawa, 2010.

SARIYILDIZ, E. e TEMELTAS, H. A new formulation method for solving kinematic pro-blems g multiarm robot system using quaternion algebra in the screw theory framework.May 2012.

SOUSA, P. N. D. Introdução á Tecnologia de Satélites - Subsistema de Controle de Atitude.Instituto Nacional de Pesquisas Espaciais, April 2011.

SPECIALTIES, M. MS5611-01BA03 Barometric Pressure Sensor, with stainless steel cap,2012. URL www.meas-spec.com.

TAYEBI, A. e MCGILVRAY, S. Attitude Stabilization of a VTOL Quadrotor Aircraft.IEEE Transactions on Control Systems Magazine, May 2006.

VALDUGA, F. Nota ocial da FAB sobre incidente com VANTHermes 450 da Base Aérea de Santa Maria, 2012. URLhttp://www.cavok.com.br/blog/nota-oficial-da-fab-sobre-incidente-com-vant

-hermes-450-da-base-aerea-de-santa-maria/.

VÁRQUEZ, R. M., ROMERO, M., PORTILLO, O., ÁVILA, J. C. e VILCHIS, A. H.Experimental Platform of a Physical Model for a Quadrotor Helicopter. IEEE NinthElectronics, Robotcs and Automotive Mechanics Conference, October 2012.

WHITLOCK, C. When Drones Fall from the Sky. Washington Post, 2014. URLhttp://www.washingtonpost.com/sf/investigative/2014/06/20/when-drones-

fall-from-the-sky/.

109

7 APPENDIXS

7.1 STABILIZER ROUTINE CHANGES

We present the pseudocode of the modied routine in details, explaining each com-

mand.

In Algortithm 1 and in the explanation of the pseudocode, we often use notations

such as controllerRoll,Pitch,Yaw in order to reference the three variables controllerRoll,

controllerPitch and controllerYaw.

Line 1: The main loop of the stabilizer routine is executed every 2 ms.

Line 2: The IMU data reading is performed by calling an external routine, imu9read().

It reads the data from the accelerometer, magnetometer and gyrometer.

Line 3: The reference commands are read from joystick (commander).

Line 4 to 51: The update routine loop of the attitude controller is executed every 4

ms, or 250 Hz. Originally, the stabilizer routine had this loop of 250 Hz for the angular

references and an inner loop of 100 Hz, or 10 ms, for the height controller. We have

veried by some tests that we could keep the whole attitude controller in 250 Hz. In this

way, we have reduced the stabilizer routine for only one loop.

Line 5: The quaternions are updated based on the data of the accelerometer and

gyrometer in the sensfusion6UpdateQ(), contained on the library sensfusion.c. Ob-

serve that the quaternions are not available for the stabilizer.c reading. In rou-

tine sensfusion6UpdateQ() it is implemented the sensor fusion methods established by

(MADGWICK, 2011) or (MAHONY, 2008). Actually, the method used on sensfu-

sion6UpdateQ()is the (MAHONY, 2008) algorithm. Also, the data of the magnetometer

are not used in the sensor fusion. Then, the reference for the yaw orientation is not

absolute, and corresponds to the initial orientation where the quadrotor is turned on.

Line 6: Compute the Euler angles in degrees from quaternions.

110

Algorithm 1 Algorithm of the Firmware Changes (Part 1)

1: while LOOP do // update @ 500Hz or 2ms

2: Read data from IMU acc.x,y,z gyro.x,y,z mag.x,y,z

3: Get data from commander commanderThrust,Roll,Pitch,Yaw

4: if Attitude controller then // update @ 250Hz or 4ms

5: Update quaternions

6: Update euler angles eulerRoll,Pitch,YawActual

7: Get vertical acceleration accWz

8: Compute vSpeed by anti-windup integration of accWZ

9: Get pressure, temperature and aslRaw from barometer

10: Compute asl from low pass lter of aslRaw

11: Compute controller variables: p,q,r,phi,theta,psi,ax,ay,az,h

12: end if

13: if synchroSignal↑ or activateController↑ or activateAltitude ↑ then14: Reset integrated variables: vSpeed, u,v,w,h_I,phi_I,theta_I psi_I, u_I, v_I, h0

15: Reset DKF variables

16: end if

17: Compute DKF variables u_e, v_e, mu_ue, mu_ve, Paij, Paij_v

18: Compute linear speeds u, v, w by anti-windup integration

19: if activateController then

20: if activateConsole then // update references from console

21: if consoleThrust > 0 then increment h_r

22: Get phi,theta,psi_r from consoleRoll,Pitch,Yaw // inner loop

23: Get u,v,psi_r from consoleRoll,Pitch,Yaw // outer loop

24: else // update references from commander

25: if commanderThrust > 0 then increment h_r

26: Get phi,theta,psi_r from commanderRoll,Pitch,Yaw // inner loop

27: Get u,v,psi_r from commanderRoll,Pitch,Yaw // outer loop

28: end if

29: if activateAltitude then

30: Compute h_I by anti windup integration of h-h_r

31: Compute Fl

32: else h_I, Fl < − 0

111

Algorithm 2 Algorithm of the Firmware Changes (Part 1)

33: end if

34: // Outer loop computations

35: Compute u_I by anti windup integration u_e - u_r

36: Compute theta_r

37: Compute v_I by anti windup integration v_e ? v_r

38: Compute phi_r

39: // Inner loop computations

40: Compute phi_I from anti-windup integration of phi - phi_r

41: Compute tauPhil

42: Compute theta_I from anti-windup integration of theta - theta_r

43: Compute tauThetal

44: Compute psi_I from anti-windup integration of psi - psi_r

45: Compute tauPsil

46: end if

47: else // Attitude controller is not activated

48: h,phi,theta,psi,u,v_r < − 0

49: h,phi,theta,psi,u,v_I < − 0

50: Fl, tauPhi,Theta,Psil < − 0

51: end if // activateController

52: end if // Loop de 250 Hz Compute controllerThrust,Roll,Pitch,Yaw

53: if activateConsole then

54: actuatorThrust < − consoleThrust

55: else actuatorThrust < − commanderThrust

56: end if

57: if activateController then

58: if activateAltitude then actuatorThrust < − controllerThrust

59: end if

60: ActuatorRoll,Pitch,Yaw < − controllerRoll,Pitch,Yaw

61: end if

62: if actuatorThrust then > 0

63: distributePower(actuatorThrust,Roll,Pitch,Yaw)

64: end if

65: end while

112

Line 7: Compute the vertical acceleration by accelerometer data analysis and gravity

acceleration compensation.

Line 8: Compute the vertical speed by simple integration from vertical acceleration.

It is used the anti-windup technique to saturate the integrated value in known limits,

avoiding that the integrated value increases indenitely.

Line 9: Pressure and temperature are read from the barometer chip. The raw value of

the above sea level(asl) height (aslRaw) is obtained by hypsometric equation (eq (2.33)).

Line 10: Obtainment of the ltrated value asl by low-pass lter.

Line 11: Compute the controller variables. Observe there is a discrepancy in the

variables references at the Crazyie hardware, with the conventional values in this work:

The angular rates in x and z are in opposite signs;

The yaw angle is inverted;

The acceleration on direction x is inverted;

We must change the signs of the x and z correspondents of the angular rates, the

yaw angle, and the z component of the accelerometer, since they are in opposite

direction than we have established in this work.

The zero reference of the height is dened by the current value of the asl at the

variable reset(line 12 to 15).

The acceleration data in G is converted to m/s2, the Euler angles and the angular

rates are converted from degrees to radians. Then, we have the following equations.

p = gyro.x ·π/180.

q = gyro.y ·π/180.

r = -gyro.z ·π/180.

φ = eulerRollActual ·π/180.

θ = eulerPitchActual ·π/180.

ψ = - eulerYawActual ·π/180.

ax = -g · accx.

113

ax = g · accy.

ax = g · accz.

Where g = 9.81m/s2 is the acceleration of gravity.

Line 12 to 15: The variables subject to numerical integration are reset in case of altera-

tion in controller mode or in recorded data. The alteration is signed with an up arrow(↑)near to the correspondent ag and indicates that the variable has changed the logic level

0 to 1. The ags alterations are viable for the tab "Parameter"at PC Client. The three

ags "if"on line 12 correspond to the activation of the movement and angles controllers

(activateController) and the height controller (activateHeight). The synchroSignal ag is

used for data synchronizing in dierent log blocks during the data recorded in PC Client.

Also, the height reference h0 is restarted based on the current value of the height asl.

Line 16: The estimated values of the longitudinal and lateral speeds are computed

using the Kalman lter implementation (subsection 3.7.2).

Line 17: The alternative computation of the linear speeds is done by anti-windup

integration from the accelerometer data.

Line 18 to 50: The computations of the attitude controller references proposed on

this work are done in these lines. If the activateController ag is false, the correspondent

variables are zeroed, lines 45 to 47.

Line 19 to 27: The reference variables for the controller are read. If the activateConsole

ag is true, the inputs references are obtained from the computer console. Otherwise,

the input references are obtained from the joystick, named commander in the original

rmware. Also, the obtained references depend on the controller loop that is activated.

The references phi_r, theta_r, psi_r are obtained for the inner loop and u_r, v_r,

psi_r for the outer loop. We have not created any ags to activate the controller loops

yet and this operation is done by commenting the correspondent line following by a

new compilation. The height reference is modied by an increment of its value when

console,controllerThrust is nonzero. There is still missing a decrement command for

the height.

The controller computations are performed as follows. Lines 28 to 33 refer to the

height controller computation (section 3.2). Lines 34 to 48 refer to the outer controller

loop (section 3.4). Lines 39 to 45 refer to the inner controller loop (section 3.2). Finally,

lines 47 to 49 reset the attitude controller variables if deactivated.

114

Line 52: The controller signals are computed for the Thrust, Roll, Pitch and Yaw

based on the control signals calculated on line 18 to 50. The Fl variable is a oat with

expected value between 0 and 4 (section 3.2) and it is transformed in the controllerThurst

variable, an unsigned integer of 16 bits, assuming values between 0 and 65535. The

variables tauPhi,Theta,Psi are oats with expected values between -1 and 1 (section

3.2) are transformed in the respective controller variables Roll,Pitch,Yaw, signed int of

16 bits, assuming values between ± 65535. Proportional linear transformations are also

inserted in the saturation limits in the nal variables.

Line 53 to 64: The stabilizer routine outputs are eectively computed. In the lines 53

to 57 we choose that the stabilizer routine nal output for the Thrust corresponds to the

console or commander, according with activateConsole ag. If activated, this command

goes to the controller output in line 59.

Line 59 to 61: The stabilizer routine outputs for the actuators commands correspond to

the controller outputs, according with the activateController ag. Otherwise, the output

comes from the consoler or from the commander according with the correspondent ag.

Line 62 to 64: The actuators commands variables motorPowerM1,M2,M3,M4 are

computed from the actuators values Thurst,Roll, Pitch, Yaw. In the distributePower

routine the duty cycles are computed for the actuators according with the quadrotor

conguration, X or +. In case of the Crazyie 1.0, the standart conguration is the

+.

Table 7.1 presents the description of the variables that are more relevant utilized on the

stabilizer routine. Beyond the name and the description of the variables, it is noteworthy

the type, the unit and the log block utilized for telemetry through the PC client. All the

logs are dened on the stabilizer.c library.

TAB. 7.1: Table of variables and Log Blocks

Variables Description Type Unit Log

acc.xAccelerometer signal in

x-directionoat G acc.x

acc.yAccelerometer signal in

y-directionoat G acc.y

Continued on next page

115

TAB.7.1 continued from previous page


acc.zAccelerometer signal in

z-directionoat G acc.z

gyro.xGyrometer signal in

x-directionoat degree/s gyro.x

gyro.yGyrometer signal in

y-directionoat degree/s gyro.y

gyro.zGyrometer signal in

z-directionoat degree/s gyro.z

mag.xMagnetometer signal in

x-directionoat Gauss mag.x

mag.yMagnetometer signal in

y-directionoat Gauss mag.y

mag.zMagnetometer signal in

z-directionoat Gauss mag.z

accWz Vertical acceleration oat G -

vSpeed Vertical speed oat G/saltHold.vSpeed

TemperatureTemperature measured

by barometeroat

Celsius

degreebaro.temp

PressurePressure measured by

barometeroat Pa baro.pressure

alsRaw

Above sea level height

calculated by hypsometric

equation

oat m baro.aslRaw

als

Above sea level height

calculated by hypsometric

equation (ltered value)

oat m baro.asl


116



eulerRollAc-

tual

Roll angle computed by

quaternion update from

the sensor fusion routine

oat degree stabilizer.roll

eulerPitchAc-

tual

Pitch angle computed by



oat degreestabili-

zer.pitch

eulerYawAc-

tual

Yaw angle computed by



oat degree stabilizer.yaw

Actua-

torThurst

Thrust command for the

actuators, nal output of

the stabilizer routine

unsigned

int 16-

actua-

tor.thrust

ActuatorRoll

Roll command for the



int 16 - actuator.roll

ActuatorPitch

Pitch command for the



int 16 -actua-

tor.pitch

ActuatorYaw

Yaw command for the



int 16 - actuator.yaw

motorPowerM1

PWM Duty cycle

command correspondent

to the rotor M1, range of

[0,1] mapped on [0,65536]

unsigned

int 32- motor.m1

motorPowerM2

PWM Duty cycle



[0,1] mapped on [0,65536]

unsigned

int 32- motor.m2


117



motorPowerM3

PWM Duty cycle



[0,1] mapped on [0,65536]

unsigned

int 32- motor.m3

motorPowerM4

PWM Duty cycle



[0,1] mapped on [0,65536]

unsigned

int 32- motor.m4

commanderTh-

rust

Reading from the joystick

commander thrust, range

of [0,65535]

unsigned

int 16- -

commander-

Roll


commander roll, range of

[-30,30] degrees

oat degree -

commander-

Pitch


commander pitch, range

of [-30,30] degrees

oat degree -

comman-

derYaw


commander yaw, range of

[-200,200] degrees

oat degree -

control-

lerThurst

Thrust command from

the controller for the

actuators

unsigned

int 16-

actua-

tor.thrust

controllerRoll

Roll command from the

controller for the

actuators

int 16 - actuator.roll

controllerPitch

Pitch command from the

controller for the

actuators

int 16 -actua-

tor.pitch


118



controllerYaw

Yaw command from the

controller for the

actuators

int 16 - actuator.yaw

p Roll rate oat rad/s states.p

q Pitch rate oat rad/s states.q

r Yaw rate oat rad/s states.r

phi Roll angle oat rad/s states.phi

theta Pitch angle oat rad/s states.theta

psi Yaw angle oat rad/s states.psi

u Longitudinal speed oat m/s states.u

v Lateral speed oat m/s states.v

w Vertical speed oat m/s states.w

h0

Reference height, copies

value of asl height when

reset from PC client

oat m -

hHeight with respect to

reference height h0oat m states.h

axAccelerometer signal x

componentoat m/s2 -

ayAccelerometer signal y

componentoat m/s2 -

azAccelerometer signal z

componentoat m/s2 -

u_eEstimated longitudinal

speedoat m/s DKF.u _e

v_e Estimated vertical speed oat m/s DKF.v_e

mu_eEstimated longitudinal

drag coecientoat ? DKF.mu _e


119



muv_eEstimated lateral drag

coecientoat ? DKF.muv_e

Paij

Covariance matrix

estimation error of

longitudinal speed and

drag coecient

oat[2][2] - -

Paij_v

Covariance matrix

estimation error of lateral

speed and drag coecient

oat[2][2] - -

h_r Reference for height oat m

u_rReference for longitudinal

speedoat m/s

v_rReference for lateral

speedoat m/s

phi_r Reference for roll angle oat rad

theta_r Reference for pitch angle oat rad

psi_r Reference for yaw angle oat rad

h_I Integral state for height oat m.s

u_IIntegral state for

longitudinal speedoat m

v_IIntegral state for lateral

speedoat m

phi_IIntegral state for roll

angleoat rad.s

theta_IIntegral state for pitch

angleoat rad.s

psi_IIntegral state for yaw

angleoat rad.s


120



FlGeneralized thrust

control inputoat -

tauPhilGeneralized roll torque

control inputoat -

tauThetalGeneralized pitch torque

control inputoat -

tauPsilGeneralized yaw torque

control inputoat -

In table 7.2 we present the description of the parameters table created. Parameters can be

modied on the rmware from PC Client. They are used to modify the attitude controller

operation modes and the references.

121

TAB. 7.2: Table of parameters


activateCon-sole

Flag to change input ofreferences from gamecommander or console

(keyboard)

boolean - bypass.cons

activateCon-troller

Flag to activate theattitude controller (bydefault except from the

height controller)

boolean - bypass.cont

activateAlti-tude

Flag to activate theheight controller

boolean - bypass.alt

synchroSignalFlag to synchronize thereadings from dierent

log blocksboolean - bypass.sync

consoleThrust

Thurst from PC consolereading, analogue of

commanderThurst, rangeof [0, 65535]

unsignedint 16

-con-

sole.consoleThurst

consoleRollRoll from PC consolereading, in radian

oat radcon-

sole.consoleRoll

consolePitchPitch from PC consolereading, in radian

oat radcon-

sole.consolePitch

consoleYawYaw from PC consolereading, in radian

oat radcon-

sole.consoleYaw

122

Documents

MINISTÉRIO DA DEFESA EXÉRCITO BRASILEIRO …