Autonomous Systems LabProf. Roland Siegwart

Semester-Thesis

Supervised by: Author:Hurzeler Christoph Ruesch AndreasNikolic Janosch

Dynamics Identification &Validation, and PositionControl for a Quadrotor

Spring Term 2010

Abstract

Autonomous mobile robots arouse a lot of interest in the past years, often withthe goal to support human beings in numerous activities, likewise in the projectAIRobots.

Within this project, an airframe of a quadrotor was constructed and equippedwith flight electronics. This thesis presents a complete system modeling and iden-tification process of the dynamics of the quadrotor, motivated by the posteriorderivation of a flight controller. For the sake of which flight data were gatheredduring manual controlled flights and the prediction error method was applied forthe parameter identification.

After the identification, a position controller was designed based on the model.The controller was successfully applied to the quadrotor without retuning and al-lows the helicopter to hover smoothly. The control algorithm is based on an optimalcontrol problem formulation, namely a linear quadratic regulator method, with in-tegral extensions and anti-reset-windup add-ons, as well as a Kalman observer forthe states estimation were applied.

Key words: UAV, Dynamical System Modeling and Identification, Position Con-trol, LQR(I), Anti Reset Windup, Kalman Observer.

i

ii

Acknowledgment

This semester thesis was evolved at the Autonomous Systems Lab (ASL) of theSwiss Federal Institute of Technology (ETH) Zurich. The author is very grateful tohis supervisors, Christoph Hurzeler and Janosch Nikolic for their great support andguidance during the whole process of this project, their assistance was essential forthe accomplishment of this thesis.

Expressing gratitude to Prof. Roland Y. Siegwart, the head of the AutonomousSystems Lab, for the opportunity to work at his laboratory under ideal conditionson a very exciting topic.

Sincere thanks are given to the PhD students of the ASL team, who all thetime had a sympathetic ear to any question about flying robots. Last but not least,many thanks to all Master students of the ASL flying room, for their sustainedsupport and our interesting discussions.

iii

iv

Contents

List of Tables vii

List of Figures viii

List of Symbols ix

List of Acronyms and Abbreviations x

1 Introduction 11.1 Problem Formulation and Context . . . . . . . . . . . . . . . . . . . 11.2 Prior and Related Work . . . . . . . . . . . . . . . . . . . . . . . . . 2

2 Hardware and System Description 32.1 AIRobots Quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.2 On-board Attitude Controller . . . . . . . . . . . . . . . . . . . . . . 42.3 Development Environment . . . . . . . . . . . . . . . . . . . . . . . . 42.4 Prior Project Status . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3 Definitions and Notation 73.1 Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.2 Coordinate Frames . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.2.1 Inertial Frame . . . . . . . . . . . . . . . . . . . . . . . . . . 73.2.2 Body Frame . . . . . . . . . . . . . . . . . . . . . . . . . . . . 83.2.3 Transformation Matrices . . . . . . . . . . . . . . . . . . . . . 8

4 Dynamic Model of the AIRobots Quadrotor 94.1 Point-Mass Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94.2 Black Box Model for the Attitude Controller . . . . . . . . . . . . . 9

4.2.1 Decoupled Submodels . . . . . . . . . . . . . . . . . . . . . . 104.2.2 Prediction Error Method . . . . . . . . . . . . . . . . . . . . 11

4.3 Final System, Complete Dynamical Model . . . . . . . . . . . . . . . 12

5 Flight Controller Design 135.1 Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

5.1.1 LQR - Linear Quadratic Regulator . . . . . . . . . . . . . . . 145.1.2 LQRI - Extension including Anti Reset Windup . . . . . . . 145.1.3 Decoupled Position Control Approach . . . . . . . . . . . . . 16

5.2 Observer Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 185.2.1 Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . 185.2.2 Implemented Observers . . . . . . . . . . . . . . . . . . . . . 18

5.3 Final Closed-Loop Implementation . . . . . . . . . . . . . . . . . . . 19

v

6 Hardware Implementation and Testing 216.1 Plane Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 216.2 AscTec Attitude Controller . . . . . . . . . . . . . . . . . . . . . . . 22

6.2.1 Coordinate System . . . . . . . . . . . . . . . . . . . . . . . . 226.2.2 Input Command Conversion . . . . . . . . . . . . . . . . . . . 226.2.3 Angle Offsets . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

7 Results and Discussion 237.1 Dynamical Model Validation . . . . . . . . . . . . . . . . . . . . . . 237.2 Flight Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

7.2.1 Hovering Flight . . . . . . . . . . . . . . . . . . . . . . . . . . 267.2.2 Step responses . . . . . . . . . . . . . . . . . . . . . . . . . . 27

8 Conclusion and Outlook 298.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 298.2 Outlook . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

A Parameters 31A.1 Model Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31A.2 Controller Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . 31A.3 Controller Parameters - Prior PID Approach . . . . . . . . . . . . . 32A.4 Angle Offsets . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

B Matlab/Simulink Documentation 33B.1 Source Folder Overview . . . . . . . . . . . . . . . . . . . . . . . . . 33B.2 Model Identification and Validation . . . . . . . . . . . . . . . . . . . 33

B.2.1 Submodel Identification . . . . . . . . . . . . . . . . . . . . . 33B.2.2 Complete Model Validation . . . . . . . . . . . . . . . . . . . 34

B.3 Position Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

C Flight Results Overview and Description 37

Bibliography 39

vi

List of Tables

2.1 Data Sheet of the AIRobots Quadrotor, according to [Asc] . . . . . . 3

5.1 Extended state space matrices for the bias estimation . . . . . . . . 20

7.1 Results from Flight Control: RMS values . . . . . . . . . . . . . . . 26

vii

List of Figures

2.1 CAD model of the AIRobots Quadrotor . . . . . . . . . . . . . . . . 42.2 Schematical illustration of the code generation . . . . . . . . . . . . 52.3 Positioning of the exteroceptive sensors . . . . . . . . . . . . . . . . 5

3.1 Sketch of the Coordinate Frames . . . . . . . . . . . . . . . . . . . . 8

4.1 Point-Mass Model of the AIRobots Quadrotor . . . . . . . . . . . . . 104.2 Principle of the Black Box Model . . . . . . . . . . . . . . . . . . . . 114.3 Complete Dynamical Model of the AIRobots Quadrotor . . . . . . . 12

5.1 Main Control Principle . . . . . . . . . . . . . . . . . . . . . . . . . . 135.2 Block diagram of the closed-loop system with a LQR state feedback. 155.3 LQRI control system . . . . . . . . . . . . . . . . . . . . . . . . . . . 165.4 Principle of the Kalman Estimator . . . . . . . . . . . . . . . . . . . 185.5 LQRI combined with a Kalman estimator . . . . . . . . . . . . . . . 195.6 Final position controller . . . . . . . . . . . . . . . . . . . . . . . . . 20

6.1 Attitude control-command conversion . . . . . . . . . . . . . . . . . 22

7.1 Results from Model Validation: acceleration plot of flight01 . . . . . 237.2 Results from Model Validation: angles plot of flight01 . . . . . . . . 247.3 Results from Model Validation: acceleration plot of flight03 . . . . . 247.4 Results from Model Validation: angles plot of flight03 . . . . . . . . 257.5 Results from Model Validation: acceleration plot of flight04 . . . . . 257.6 Results from Model Validation: angles plot of flight04 . . . . . . . . 267.7 Results from Flight Control: hovering flight (front and side distance) 277.8 Results from Flight Control: hovering flight (altitude) . . . . . . . . 277.9 Results from Flight Control: hovering flight (heading angle) . . . . . 287.10 Results from Flight Control: step response (front and sideways) . . . 287.11 Results from Flight Control: step response (altitude) . . . . . . . . . 287.12 Results from Flight Control: 3D plot of a flown square . . . . . . . . 28

viii

List of Symbols

Physical Variables

Euler angle, roll [, ] Euler angle, pitch [/2, /2] Euler angle, yaw [, ]Aeeex unit vector in x direction, expressed in the coordinate frame A

Aeeey unit vector in y direction, expressed in the coordinate frame A

Aeeez unit vector in z direction, expressed in the coordinate frame ARAB rotation matrix form coordinate system B to coordinate system ArrrOP postion vector of the point PFFF force vector [N ]T total thrust moment [N ]dsharpL measured distance of the left, front IR sensordsharpR measured distance of the right, front IR sensordsharpS measured distance of the side IR sensordr absolute distance between the IR sensor and the C.G.b absolute distance between the two front IR sensorsl gap between the front IR sensors and the C.G.

Controller Variables

x(t) Continuous signal for the statesxk Discrete signal for the states, with xk = x(k Ts)Ts sampling timeA,B,C,D Continuous state space matricesF,G,C,D Discrete state space matricesK,KI LQR gain respectively LQRI gainQ,R, LQRI tuning parametersKf Kalman filter gainP Transfer function of the plantwd process noisewn measurement noiseuk saterurated input signaluk unsaterurated input signalyk controller output signal

ix

List of Acronyms andAbbreviations

AscTec Ascending Technologies GmbHASL Autonomous Systems LaboratoryCAD Computer aided designC.G. Center of gravityDOF degrees of freedomETH Eidgenossische Technische HochschuleGPS Global positoning systemIMU Inertial measurement UuitI/O In- and outputIR Infra redKF Kalman FilterLQR Linear quadratic regulatorLQRI Linear quadratic regulator with integral extensionMAV Micro aerial vehicleNED North-East-Down systemPID Proportional-integral-differential controllerRMS Root mean squareUAV Unmanned aerial vehicleUS Ultra sonic

x

Chapter 1

Introduction

Autonomous flight control of unmanned aerial vehicles (UAVs) presents a chal-lenging task. The high agility and autonomy of small-scaled UAVs lead to a hugearea of applications. For example, industrial tasks like inspection, measurementsor exploration as well as search and rescue tasks or even military applications areconceivable. The successful accomplishment of such applications requires that theflight controller is able to stabilize the motion of the UAV. Thus, the completion ofmaneuvers such as hovering, obstacle avoidance, autonomous navigation, dockingand sensing, is essential.

Besides a translational and rotational acceleration measurement, which is ac-complished by the inertial measurement unit (IMU) and the gyroscopes, a positioncontrol algorithm of any autonomous mobile robot requires information about theposition itself, to avoid any drifting errors or integration inaccuracies. The varietyof such exteroceptive sensors is wide, e.g., infrared rangefinders, 3D laser rangers,ultrasonic or pressure sensors, global positioning system (GPS) sensors, camerasand many more.

For indoor applications, the usage of GPS sensors is strongly limited, in contrast3D laser rangefinders provide very precise distance measurements, but are usuallyexpensive and due to their high power consumption and weight, the usage on microaerial vehicles (MAV) is limited. Alternatively, computer vision, which utilizessimple low cost cameras, can provide a good description of the MAV surroundings.In practice though, the testing process of new visual servoing algorithms on a MAVis often a challenging task.

This motivates us to develop a test platform, whose position is controlled bymuch simpler sensors, such as infrared rangefinders and ultrasonic sensors. Theexteroceptive sensors controlled testing platform allows, in a further developmentphase, a step by step procedure, to replace single degrees of freedom with visualservoing algorithms.

1.1 Problem Formulation and Context

A flexible quadrotor platform based on the Asceding Technologies Hummingbirdsystem [Asc] has been developed at ASL. This platform features a simple mechan-ical construction which easily allows exchanging and adding new components suchas camera rigs and mounting racks for small range finders. It also features an elec-tronic I/O system which allows interfacing the Hummingbird attitude controllerand additional low-level sensors such as infrared or ultrasonic range finders. As thisI/O board can be programmed directly from MATLAB/Simulink models/software,fast-prototyping of control algorithms is possible. By means of this project, it is

1

Chapter 1. Introduction 2

our goal to derive a good and validated model of the vehicles flight dynamics andimplement a position controller based on infrared and sonar range sensors. Theautopilot is assumed to be able to hover smoothly in front of a corner.

1.2 Prior and Related Work

As commonly known, aerial vehicles are usually representing a highly unstable andnonlinear system. The six degrees of freedom (DOF) of an unconstrained aerialvehicle present a challenging control task. A common approach is to use a cascadedcontrol structure, where an inner loop stabilizes the rotational DOF of the UAVand an outer loop is used for the pose control.

Within the project OS4, Bouabdallah et al. [BS07], [BNS04], [BS05] analyzedand applied several control techniques. For simulation purposes Bouabdallah etal. [BS07] evolved a complete physical model of the quadrotors dynamics. Withan adoption of the integral back-stepping techniques a superior performance wasachieved, for the attitude and pose control.

As in the sFly project [BSWS] also in this thesis, the quadrotor was equippedwith an on-board attitude controller provided by Ascending Technologies [Asc],which is basically a PD-controller. Blosch, Scaramuzza, Weiss et al. proposeda black box modeling approach to model the system dynamics. Furthermore theyapplied a visual SLAM algorithm for the position estimation and a LQG/LTR basedcontroller for stabilizing the vehicle at a desired set point.

Shin et al. [SNFH05] proposed a model-based MIMO-control system for anunmanned helicopter, as well based on a cascaded control structure by applyingKalman filter-based LQI controllers. They have shown that a position controller,which considers the dynamics of the attitude closed-loop dynamics, is very useful.

The work of Skogestad & Postlethwaite [SP96], Lewis [LK92] and Guzzella[Guz09] proved to be strong references for background information in control theory.

Chapter 2

Hardware and SystemDescription

The following chapter gives a short overview and description of the used hardwareand system tools.

2.1 AIRobots Quadrotor

The quadrotor used within the scope of this project is called AIRobots Quadrotor.Its airframe was developed in a former student project at the ASL (AutonomousSystem Lab) [Roo09], furthermore it is equipped with the flight electronics of theAscTec Hummingbird quadrotor developed by Ascending Technologies GmbH [Asc].

The AIRobots Quadrotor is equipped with a quite simple and compact mechan-ical design, so that, in the worst case of a crash, it proves to be remarkably robust.Thanks to the ease of modifying the quadrotor, for example by mounting additionalsensors or cameras, and its relatively low-cost, the AIRobots Quadrotor qualifies asan ideal testing platform.

Besides autonomous flight, also manually controlled flights are possible, whichproves to be useful to gain flight data, for example to validate the system model.In Figure 2.1 the CAD model (computer-aided-design) of the AIRobots Quadrotoris illustrated. Table 2.1 provides the most important facts about the AIRobotsquadrotor and the AscTec Hummingbirds flight electronics.

nominal mass 536 gpayload capacity 200 gtotal diameter 60 cmrotor diameter 20 cmflight time continuousmaximal speed 50 km/hmicro processor 1 x ARM7

1 x TMS C2000piezo gyro and acceleration sensors

Table 2.1: Data Sheet of the AIRobots Quadrotor, according to [Asc]

3

Chapter 2. Hardware and System Description 4

Figure 2.1: CAD model of the AIRobots Quadrotor

2.2 On-board Attitude Controller

The AIRobots Quadrotor is equipped with an on-board attitude controller, by As-cending Technologies [Asc], which stabilizes the three rotational degrees of freedomof the helicopter basically with PD controllers, see [BSWS]. Thus, no direct in-put signals to the rotors are needed, instead the desired tilt angles can directly betransmitted. For more details about the flight control see chapter 5.

2.3 Development Environment

To derive the different controllers during this project the software package MAT-LAB/Simulink of MathWorks1 was used. For an automatic code generation fromthe Simulink models into the programming language c, the MathWorks products:Real-Time Workshop, Real-Time Workshop Embedded Coder, the Target SupportPackage as well as the software tool Code Composer Studio of Texas Instruments2

were used. This setup, as illustrated in Figure 2.2, allows a direct code generationand compilation, and furthermore the download of the software to the quadrotor.

For on-line recording and logging of IMU or sensor data, as well as controllerparameters or observer values, also MATLAB/Simulink was used. Using this setup,the control structure is quickly and easily changeable. The on-line recording of anydata proved itself very valuable for the purpose of debugging.

1MATLAB: version R2009b, August 12, 2009 for Unix and MS Windows systems2Code Composer Studio: version 3.3.83.19, exclusively on MS Windows systems

5 2.4. Prior Project Status

Figure 2.2: Schematical illustration of the code generation3

2.4 Prior Project Status

Before this semester project started some prior work had already been accomplishedby the ASL team.

The quadrotor had yet been equipped with three infrared rangefinders as wellas a sonar ultra sonic sensor. These exteroceptive sensors are used to estimate theposition of the helicopter in the room, namely its distance to the front and sidewall, its heading angle relatively to the front wall and its altitude. The positioningof the sensors is illustrated in Figure 2.3.

A position controller, based on four decoupled PID controllers for the threetranslational degrees of freedom plus for the helicopters heading angle relatively tothe front wall, had already been implemented and tested. The PID-approach leadto average results, which encouraged to evolve and test an optimal control approachas well as derive a validated model of the helicopter. For the PID-parameters seeappendix A.3.

s

s

s

s

&%'$&%'$

&%'$&%'$

@

@@@@@@@@@@@@

Sharp L Sharp R

Sharp side

sUS sensor

Figure 2.3: Positioning of the exteroceptive sensors

3Source: MathWorks http://www.mathworks.com/products/target-package/

http://www.mathworks.com/products/target-package/

Chapter 2. Hardware and System Description 6

Chapter 3

Definitions and Notation

Some definitions and notation are inevitable to avoid ambiguities. Hence, the no-tation used in the context of this thesis is depicted in the following, furthermorethe used coordinate frames are defined. A complete list of all used symbols andabbreviations is provided at the beginning of this document.

3.1 Notation

Physical Variables

Avvv Vector vvv expressed int the A coordinate system, please notevectors are always denoted in boldface.

RAB Transformation Matrix from coordinate system B to coor-dinate system A.Avvv = RAB B vvv

() , () First respectively second time derivation

Controller Variablesx(t) Continuous signal for the statesxk Discrete signal for the states, with xk = x(k Ts) where Ts

denotes the sampling timeA,B,C,D Continuous state space matricesF,G,C,D Discrete state space matricesK,KI LQR gain respectively LQRI gainQ,R, LQRI tuning parameters

3.2 Coordinate Frames

Within the scope of this project two coordinate frames were used, on the one handthe ground-fixed inertial frame and on the other hand the helicopter-fixed bodysystem. In Figure 3.1 both system are illustrated.

3.2.1 Inertial Frame

The inertial frame is defined according to a common North-East-Down (NED) sys-tem. Since in this project an indoor MAV without a geographical environment wasused, the north direction was redefined perpendicular to the front wall, pointinginto the wall. The third axis was defined normal to the ground, pointing down-wards. Due to the right-hand-system, the east direction points perpendicularly

7

Chapter 3. Definitions and Notation 8

r

r

r

r

@@@@@@@@

@@@R

h

Beeex

Beeey

Beeez

6

-h

Ieeex N

Ieeey E

Ieeez D

Figure 3.1: Sketch of the Coordinate Frames

into the side wall. The origin of the inertial frame is laid in the corner, thus onlyflights in the 8th octant are possible.

3.2.2 Body Frame

The body frame is defined to be fixed on the quadrotor. Its origin is located inthe center of gravity (C.G.) of the helicopter. The quadrotors first, respectivelysecond axis are defined to be coaxial with the quadrotors arms. The third axis isnormal to the helicopters plane and points downwards. In Figure 3.1 the definitionis illustrated.

3.2.3 Transformation Matrices

The transformation from the ground-fixed inertial frame into the body-frame, ap-plied within the scope of this thesis, was defined according to the Tait-Bryan con-vention [Glo07]. Thus, for the rotation matrix RBI follows,

RBI(, , ) = R R R

=

c s 0s c 00 0 1

1 0 00 c s

0 s c

c 0 s0 1 0s 0 c

=

c c c s ss s c c s s s s + c c s cc s c + s s c s s s c c c

, (3.1)where c(. . .) denotes the cos(. . .) function, and s(. . .) analogously the sin(. . .) func-tion. The following relation

RIB(, , ) = R1BI = RTBI , (3.2)

holds for the transformation from the body frame into the inertial frame, due tothe orthogonality of rotation matrices [Glo07].

Chapter 4

Dynamic Model of theAIRobots Quadrotor

In the following, the dynamical model derived for the AIRobots Quadrotor is pre-sented. The basic idea is to combine a simple physical point-mass model, to approx-imate the kinematics of the micro aerial vehicle (MAV), with a black-box model,which represents the attitude controllers dynamics as well as the rotor and bladedynamics.

4.1 Point-Mass Model

Due to the quadrotors small size and symmetry, it can be modeled as a point-mass. According to the principle of linear momentum (Newtons 2nd Law) andthe assumption that the mass of the quadrotor is constant, for the change of themomentum follows

m I rrrOP =i

IFFF i, (4.1)

where rrrOP denotes the acceleration of the center of gravity (C.G.) and FFF i representsany outer force acting on the point-mass.

Applied to the quadrotor (see Figure 4.1) for the equation of motion of thevehicle follows

Irrr =1mRIB(, , )

00T

+ 00

g

, (4.2)with T represents the total thrust moment, pointing in the opposing direction of thehelicopters down axis, and the rotation matrix RIB(, , ) from the body frameinto the inertial frame as defined in section (3.2.3).

4.2 Black Box Model for the Attitude Controller

An inner controller for the attitude is implemented on-board of the quadrotor (forfurther information see section 2.2) . Hence, the unknown parameters in the equa-tion (4.2), explicitly the Euler angles and the thrust momentum, are output vari-ables of the attitude controller. Since no exact description of the attitude controllerexists and the control parameters are unknown, the dynamics of the inner loop arealso identified in the system modeling task. For this purpose a black box modelingapproach is used.

9

Chapter 4. Dynamic Model of the AIRobots Quadrotor 10

*

XXXXXXz

?

Ieeex

Ieeey

Ieeez

XXXXX

XXXXXX

XXXXXX

XXX

front wall

side wall

:

QQQQQs

Beeex

Beeey

Beeez

HHH

HHH

HHH

HHH

HHH

HHH

HHH

HHH

HHY

IrrrOP

?

TTT

FFFG = mg

Figure 4.1: Point-Mass Model of the AIRobots Quadrotor

The main idea of black box modeling is to approximate the dynamics of thesystem with an educated guess. The system parameters are identified and validatedwith an adequate method using flight data sets. Furthermore, this modeling methodavoids a sumptuous and expensive identification of the rotor dynamic, or an exactdescription of the quadrotors aerodynamics. Because of the quadrotors mechanicalsimplicity (i.a. non-existence of a swashplate, simple design) a black box approachis adequate and leads to suitable results, as can be seen in section 7.1.

The following part describes the four decoupled subsystems, used to model theroll, pitch, yaw and thrust dynamics. Furthermore a brief overview of the usedmethod for the parameter identification is given.

4.2.1 Decoupled Submodels

As illustrated in Figure 4.2, the dynamics of the attitude controller are divided intofour subsystems, which are identified separately.

Roll DynamicsThe roll dynamics are approximated with a 2nd order system and a delayelement. Its input variable is the desired roll angle, and its output the currentroll angle.

Pitch DynamicsThe pitch dynamics are described likewise with a 2nd order system and a delayelement. The input respectively output variables are analogously the desiredand the actual pitch angle.

Yaw DynamicsFor the yaw dynamics a 1st order system and as well a delay element is used.In contrast to the roll and pitch dynamics, the desired angle rate is the inputto this subsystem, respectively the current yaw rate its output variable.

11 4.2. Black Box Model for the Attitude Controller

black box model

k20s2+2 0 s+20

esT

k20s2+2 0 s+20

esT

kT s+1 e

sT

kT s+1 e

sT

T

T

Figure 4.2: Principle of the Black Box Model

Thrust DynamicsThe thrust dynamics are as well modeled with a 1st order system and a delayelement, here too, the desired and current thrust momentum are the inputrespectively output values of the system. Please note that after the 1st orderelement and the delay element, the nominal thrust moment of m g, requiredfor hovering, is added.

4.2.2 Prediction Error Method

To estimate the model parameters the Prediction Error Method was used. Thismethod uses an iterative nonlinear least-squares algorithm to minimize a cost func-tion, which consists of a weighted sum of the squares of the errors. According to[MAT10] the cost function is defined as follows:

VN (G,H) =Ni=1

e2(t), (4.3)

where e(t) denotes the error between the estimated and the measured values. Forlinear systems the error can be expressed as,

e(t) = H1(q)[y(t)G(q)u(t)]. (4.4)

As can be seen in the definition of the cost function, the estimation becomes themore accurate the more samples are taken. The measured data sets used in thisproject were gained during manual controlled flight maneuvers. (see appendix C)

To apply this parameter identification method, the software package MATLABwas used. The MATLAB command pem computes the prediction error estimate ofa general linear system, wherefore the delay elements were manually estimated anditeratively varied. To increase the accuracy of the estimation, the algorithm wasapplied several times, using the prior estimation as the new initial guess.

For more detailed information about the prediction error method the interestedreader is referred to [MAT10] and [Sha03], for more details about the application,see appendix B.2.

Chapter 4. Dynamic Model of the AIRobots Quadrotor 12

4.3 Final System, Complete Dynamical Model

Eventually, the final system of the complete model of the AIRobots quadrotor isillustrated in Figure 4.3. The model was used to build a simulator where for examplecontrol algorithms can be tested before they are implemented on the real platform.The identified parameters can be found in appendix A.1, the results of the modelvalidation are provided in chapter 7.

Helicopter Device

black box model point-mass model

k20s2+2 0 s+20

esT

k20s2+2 0 s+20

esT

kT s+1 e

sT

kT s+1 e

sT

Irrr = 1m RIB(, ,)

00T

+ 00

g

T

T

Irrr Irrr Irrr

Figure 4.3: Complete Dynamical Model of the AIRobots Quadrotor

Chapter 5

Flight Controller Design

As mentioned above, on the AIRobots quadrotor an attitude controller from AscTec[Asc] was already implemented, building the inner control loop. For a completeflight control an outer loop to control the position is needed. The derivation of thisposition controller is presented in the following chapter. In Figure 5.1 the completecascaded control principle is illustrated.

Besides a brief introduction of the applied control principle, the state observeris provided, and eventually the final, complete control loop is presented.

Helicopter Device

PositionController

AscTecattitude

controller

Quadrotorplant

, ,,T

, ,,T Irrr Irrr

Figure 5.1: Main Control Principle

5.1 Controller Design

Because of near hovering, the cross coupling effects between the roll, pitch, yawand altitude dynamics are insignificant and thereby negligible, a decoupled controlapproach is conceivable. As mentioned in section 2.4 a former position controller,applying four decoupled PID controllers led to average results. Thus, for this projectan optimal control approach was chosen.

At the source of any optimal control approach the formulation of an optimiza-tion problem is to be found. According to Skogestad & Postlethwaite [SP96] andGuzzella [Guz09], two main groups of optimization problems are distinguished inmodern control theory. The first method is the so-called H2 method, with the goalto minimize the mean value of the so-called error signal over all frequencies. Onthe other hand, a worst-case formulation where the cost function is defined throughthe worst possible value of the error signal over all frequencies is conceivable, it iscalled the H method.

In this project a linear quadratic regulator (LQR) is applied, which is a memberof the H2 optimization group. It is described in more detail in the following.

13

Chapter 5. Flight Controller Design 14

5.1.1 LQR - Linear Quadratic Regulator

For any LQR control, it is assumed that the plant dynamics are linear and known,furthermore it is assumed that all states of the plant are available. Thus, the systemis given in the state space notation as

xk+1 = F xk +G uk (5.1)

yk = C xk +D uk (5.2)

As Skogestad & Postlethwaite [SP96] pointed out, the LQR control problem isdefined as a deterministic initial value problem, i.e. to find the control input u(t),which brings the linear system with the non-zero initial state x(0) to the origin sothat the following cost function is minimized

J(u) =

0

(x(t)T Q x(t) + u(t)T R u(t)

) dt, (5.3)

respectively in the discrete case

J(u) =k=0

xTk Q xk + uTk R uk. (5.4)

The corresponding weighting matrices Q and R, or in other words the design param-eters, have to be chosen suitably, such that Q = QT 0 respectively R = RT 0.The optimal solution to this regulator problem is the control signal

uk = K xk, (5.5)

where the gain matrix K is defined as

K = [R+GTSG]1GTSF (5.6)

and S is the unique positive semi-definite solution of the discrete-time algebraicRiccati equation (DARE)

FTSG[R+GTSG]1GTSF + S FTSF Q = 0. (5.7)

For more mathematical detail, interested readers are referred to the work of Sko-gestad & Postlethwaite [SP96], Guzzella [Guz09] and Lewis [LK92].

In the scope of this project the gain matrix K was determined by solving theDARE (5.7) with the solver provided by MATLAB. Figure 5.2 illustrates the LQRstate feedback controlled closed-loop system.

5.1.2 LQRI - Extension including Anti Reset Windup

Integral Action Add-on

To successfully complete the demanded flight task, the standard LQR problem wasextended with some integral action, because a standard LQR controller would nothave a zero output for constant disturbances, e.g. through misalignment of theIMU plane to the helicopter-hovering plane, or through varying mass of the vehicle.According to [Guz09], to each system output channel a (P)I element is added, asoutlined in Figure 5.3. For the extended linear system follows,

xk+1 = F xk + G uk (5.8)

yk =[ I C

] xk (5.9)

15 5.1. Controller Design

Plant

G

F

Cz1

K

uk xk+1 xk yk

Figure 5.2: Block diagram of the closed-loop system with a LQR state feedback.

with the new state vector xk =[vk xk

]T , an additional tuning parameter andthe new system matrices are defined as

F =

[I C0 F

], G =

[0

G

]. (5.10)

The solution K for the linear quadratic regulator problem for the extendedsystem {F , G, CT C, R} can be found analogously to the standard regulator problemapplying the equations (5.6) and (5.7), see section 5.1.1. The new gain matrix hasthe following form

K =[K KI

], (5.11)

the first element K is the solution of the standard LQR problem, the second elementis the gain matrix for the integral extension, see Figure 5.3.

Anti Reset Windup Add-on

The output range of any real actuator is limited through physical constraints. Thus,in the case of a limited plant input, i.e. the input required by the controller islarger than the maximal possible input, the system typically responds with a hugeovershoot and a very slow return to the reference value, e.g. pre-takeoff.

This phenomenon is well known from PI(D) control applications, and there areseveral possibilities of appropriate anti reset windup additions to PI controllers.The interested reader is referred to [LK92] and [GS03].

Since the LQR controller was augmented with a (P)I controller on each output(see above) also the LQRI controller required an anti reset windup addition, toavoid a large overshoot in the saturated case. For this purpose the plant input ukis limited by a saturation model of the actuator, i.e.

uk =

umax if uk > umaxuk if uk [umin, umax]umin if uk < umin

(5.12)

where uk is the required input of the controller. Since a discrete LQRI controller isapplied, the anti reset windup is quite simple. The difference between the requireduk and saturated uk input signal is negatively fed back to the integrator, thus inthe following time step the integrator is reset. Figure 5.3 illustrates the integralextension combined with the anti reset windup.

Chapter 5. Flight Controller Design 16

Plant

Integral Extension

anti resetwindup

G

F

Cz1

K

z1KIuk uk + xk+1 xk yk

++

+ vk+1 vk +rk +

+

+

Figure 5.3: LQR system with integral extension at the output and anti reset windup

5.1.3 Decoupled Position Control Approach

As mentioned in the beginning of this chapter, cross coupling effects are negligiblenear hovering flight, thus a decoupled control approach is conceivable. Below thefour decoupled LQRI controllers are briefly described, please note that all of themcorrespond to the control schema derived above.

Altitude Control

The subsystem of the quadrotors altitude simply consists of the black box modelof the thrust and basically a double integrator, see section 4.2.1. This leads to thefollowing form for the transfer function of the linear altitude submodel

Palt(s) =b0

s3 + a2 s2, (5.13)

where b0 and a2 represent the constant gains, the numerical values can be found inthe appendix A.2. The transfer function, (5.13) expressed in the canonical form,leads to the following state space representation

[Aalt Balt

Calt Dalt

]=

0 1 0 00 0 1 00 0 a2 1b0 0 0 0

. (5.14)After the discretization, the discrete system matrices {Falt, Galt, Calt, Dalt} can beapplied straightforward to the LQRI problem formulation respectively to the DAREequation (5.7).

Front and Side Distance Control

To control the distance to the wall in front and on the side of the quadrotor theforces in the corresponding directions can be derived from the principle of linearmomentum (4.1). The principle of linear momentum can be rewritten and solvedfor the total force vector acting on the quadrotor, thus the following expressionholds: IFx

IFyIFz

= RIB(, , ) 00T

+ 00

mg

. (5.15)

17 5.1. Controller Design

According to the Tait-Bryan convention (see (3.1), (3.2)), for the forces in frontand side direction follows

IFx = T (cos sin cos+ sin sin), (5.16)IFy = T (sin sin cos cos sin), (5.17)IFz = T (cos cos) +mg. (5.18)

Under the assumption, that cos and cos are different form zero, which always isthe case around hovering flight, follows that

IFx =IFz mgcos cos

(cos sin cos+ sin sin), (5.19)

IFy =IFz mgcos cos

(sin sin cos cos sin). (5.20)

As can be seen in the equations (5.19), (5.20) and outlined above, cross couplingeffects are insignificant around the hovering point, thus the following approximations

IFx = mg tan , (5.21)IFy = mg tan. (5.22)

are obtained.Due to the point mass model of the quadrotor (see equation (4.1)) the compo-

nents of the force vector can be expressed as

IFi = m I ri, (5.23)

thus for the subsystem of the helicopters plant in front direction with the inputsignal IFx and output signal Ix holds[

Adist Bdist

Cdist Ddist

]=

0 1 00 0 1/m1 0 0

, (5.24)and for the subsystem in the side direction with the input signal IFy and outputsignal Iy analogously [

Aside Bside

Cside Dside

]=

0 1 00 0 1/m1 0 0

. (5.25)After a discretization of the linear systems above, the LQRI control method isapplied.

Heading Control

Last but not least the subsystem for the quadrotors heading angle has to be con-trolled. The black box model for the dynamics of the yaw rate is simply extendedwith a integrator to derive the yaw angle. Hence, the transfer function of the lin-earized subsystem is expressed as

Pyaw(s) =b0

s2 + a1 s(5.26)

where b0 and a1 represent the corresponding gains. The numerical values are aswell to be found in the appendix A.2. Thus, the state space representation of theyaw subsystem expressed in the canonical form is given as[

Ayaw Byaw

Cyaw Dyaw

]=

0 1 00 a1 1b0 0 0

. (5.27)

Chapter 5. Flight Controller Design 18

Also for the heading control, after a discretization of the linear subsystem, it isapplied to the LQRI control method to derive a heading controller.

5.2 Observer Design

In distinction from a simulator, on a real platform are usually not all states directlyavailable. Hence, for an optimal control approach, as described above, a observeris required to estimate the unmeasurable states.

For example, the distance of the quadrotor to the wall can be measured througha infrared sensor, its acceleration in the corresponding direction is provided by theIMU, but its velocity is not directly measured.

The field of different observer methods is huge. A complete treatment is beyondthe scope of this project. For the estimation of the states a standard Kalmanfilter approach was applied in the context of this project. This approach is brieflyintroduced below, for further information the interested reader is referred to [DW01].

5.2.1 Kalman Filter

The Kalman filter (also called Kalman estimator) has the structure of a standardstate observer, as illustrated in Figure 5.4. The difference between the measuredsystem output and the estimated system output is scaled by the Kalman filter gainKf and fed back to the observer. The Kalman filter gain Kf itself, is the solution ofan optimization problem under the assumption that the process and measurementnoise are uncorrelated white noise signals. I.e. the gain matrix, that minimizes theexpectation of the estimation error E[(x x)T (x x)], is chosen. As for the lin-ear quadratic regulator problem, the optimal solution for the Kalman optimizationproblem is found by solving a discrete algebraic Riccati equation. The solver pro-vided by the software package MATLAB allows a straightforward implementationof a Kalman estimator. For further information see [DW01] or [SP96].

Plant

Kalman Estimator

G

F

Cz1

K f

+ xk+1 xk

+

+

+

yk

+

yk

xk

uk + +

wdk

+ wnk

Figure 5.4: Principle of the Kalman Estimator

5.2.2 Implemented Observers

The meauserment of the accelerations with the IMU contains a bias, caused througha misalignment between the IMU plane and the helicopters hovering plane. There-fore, to derive the observers for each LQRI controller, the state space representationsof the corresponding subsystems are augmented with an additional state to estimate

19 5.3. Final Closed-Loop Implementation

these biases. Furthermore the process noise wd and measurement noise wn are nolonger neglected, i.e. the submodels of the plant have the following continuous form

x = A x+B u+G wd (5.28)y = C x+D u+ wn (5.29)

where the process noise signal wd and measurement noise signal wn are assumed tobe uncorrelated zero-mean Gaussian signals.

For the bias estimation in each subsystem an additional state is included, whichis modeled as a so-called Random walk process, see [TR05]. Based on the assump-tion, that the biases of the acceleration measurements are constant over the timebut priorly unknown, a Random walk model is conceivable. I.e. the additional statedoes not depend on any prior state, it depends only on the process noise, so thatfor the variation of the state the following expression holds

xbias = wd. (5.30)

Due to the Random walk model, the Kalman filter is able to adjust the bias esti-mation slowly, using the knowledge gained through the measurments. For furtherinformation see [DW01] and [TR05].

The extended state space matrices are provided in Table 5.1. Before the corre-sponding optimization problem can be solved, the subsystems have to be discretized.

5.3 Final Closed-Loop Implementation

In the next step the LQRI controllers found, in section 5.1, are combined withthe state observers, found in section 5.2. Thus, the structure of each decoupledcontroller corresponds to the block diagram shown in Figure 5.5.

Integral Extension

anti resetwindup

Kalman Estimator

G

F

Cz1

K f

Plant

K

z1KIuk uk + yk

+

+ xk+1 xk

+

+

+

xk

+

+ vk+1 vk +rk +

+

+

+

wdk

+ wnk

Figure 5.5: Single LQRI controller combined with a Kalman state estimator

In Figure 5.6 the complete closed-loop control structure, which was implementedon the AIRobots quadrotor, is schematically illustrated.

Chapter 5. Flight Controller Design 20

Altitude Observer

A B G CT D0 1 00 0 10 0 0

010

011

100

[0]

Front Distance Observer

A B G CT D0 1 00 0 10 0 0

010

0 01 00 1

100

[0]

Side Distance Observer

A B G CT D0 1 00 0 10 0 0

010

0 01 00 1

100

[0]

Heading Angle Observer

A B G CT D[0 10 0

] [10

] [1 00 1

] [10

] [0]

Table 5.1: Extended state space matrices for the bias estimation

Position Controller Helicopter Device

LQRI front

LQRI side

LQRI heading

LQRI altitude

Set Point

Observer

KalmanEstimator

Attitude Controller Hqaud

IMU

IR/USsensors

Irrr

(x, y, z, )

(, ,,T )

Figure 5.6: Final position controller

Chapter 6

Hardware Implementationand Testing

Some preliminary remarks to the implementation. The controllers were derived inMATLAB/Simulink, for the code generation the setup described in section 2.3 wasused. The manual [Asc10] provides a lot of information about the Hummingbirdflight electronics, which was mounted on the AIRobots quadrotor.

6.1 Plane Estimation

The quadrotor is equipped with three infrared sensors, for the positioning see Figure2.3. With the two sensors pointing to the front wall, the helicopters heading angleas well as its distance to the wall were calculated by applying some simple geometriccoherences. On the assumption that the quadrotors plane is not tilted, i.e. that itis parallel to the ground the heading angle is calculated with

= arctan(dsharpL dsharpR

b

)(6.1)

where dsharpL denotes the distance measured with the left infrared sensor, dsharpRdenotes analogously the distance measured with the right infrared sensor and bdenotes the absolute distance between the two sensors. For the x-component of theposition vector Irrr of the quadrotor the following expression holds

x = (dsharpL + dsharpR

2+ l) cos() (6.2)

where l represents the gap between the C.G. and the sensors.The y-component is derived in a similar way, please note since only one sensor

is used for the measurement of the side distance, the sideways displacement has tobe taken into account. The absolute distance between the infrared sensor and theC.G. is denoted with dr. Remark: the sensor is assumed to be mounted exactly onthe quadrotor arm, thus the angle between the sensor and the y-direction is equalto 45. The side distance is expressed through

y =

(dsharpS +

2

2 dr

) cos()

2

2 dr sin(), (6.3)

where the sideways measured distance is represented by dsharpS.The z-component is simply the negative value of the measured value by the

ultra sonic sensors, since the 3rdaxis is pointing downwards. Please note that the

21

Chapter 6. Hardware Implementation and Testing 22

ultra sonic sensor is insensitive to little tilt angles, due to the fact that it selectsthe smallest distance of the measure conus for its altitude measurement.

6.2 AscTec Attitude Controller

6.2.1 Coordinate System

Experiments with the AIRobots quadrotor test platform have shown, that the As-cTec attitude controller does not satisfy the definition of the body frame as statedin section 3.2. In fact, it corresponds to the definition for the y-axis, but the x-axisis pointing towards the opposite direction, what is probably caused by a right-handcoordinate frame with a third axis pointing in direction of the thrust vector (notdownwards). Within this project the body coordinate frame, as defined in section3.2, was applied. To avoid any problems with the attitude controller, the sign ofthe input and output roll angle were changed.

6.2.2 Input Command Conversion

According to the manual of AscTec [Asc10], the estimated angles of the attitudecontroller are stored in an unsigned 8-bit integer value with the resolution of a tenthof a degree. An unsigned 8-bit integer data type provides a range of [0...255]where the neutral point is defined to be at 127.

The input command to the attitude controller, i.e. the desired euler angles, arestored in an unsigned 8-bit integer value as well. Its resolution is not explicitlyindicated in the manual [Asc10]. Through experiments the resolution of a third ofa degree was found, see Figure 6.1.

110 115 120 125 130 135 14080

60

40

20

0

20

40

Input command [uint8]

Mea

sure

d pi

tch

angl

e [d

eg/1

0]

Estimate Pitch command conversion

measurementfitted conversion

110 115 120 125 130 135 14080

60

40

20

0

20

40

Input command [uint8]

Mea

sure

d ro

ll an

gle

[deg

/10]

Estimate Roll command conversion

measurementfitted conversion

Figure 6.1: Attitude control-command conversion

6.2.3 Angle Offsets

The hovering plane of the quadrotor does not exactly correspond to the plane of theattitude controller. Hence biases arise between the quadrotors euler angles and theattitude controllers euler. With different experiments the offsets were estimatedand subtracted from the corresponding position controller outputs, the resultingoffsets are denoted in appendix A.4. As a possible improvement, an estimationprocess, which varies the input commands automatically, is conceivable.

Chapter 7

Results and Discussion

7.1 Dynamical Model Validation

A quarter of the flight data gained during manual controlled flight (see appendixC) was used for the parameter identification during the system modeling process,described in chapter 4. The other three quarters were used for the model validation,i.e. comparing the flight data with the response of the model stimulated with thecorresponding input data.

The Figures (7.1 - 7.6) show the results of the system validation. Please notethat, for a comparison between the simulated acceleration values and the acceler-ation values measured by the IMU a coordination transformation from the bodyframe into the inertial frame was required, the transformation matrix is providedin section 3.2.3.

2 4 6 8 10 12 14 16 181

0.5

0

0.5

1

time [sec]

accl

erat

ion

[m/s

2]

xacc I [m/s2]

measuredsimulated

2 4 6 8 10 12 14 16 181

0.5

0

0.5

time [sec]

accl

erat

ion

[m/s

2]

yacc I [m/s2]

measuredsimulated

2 4 6 8 10 12 14 16 18 201

0.5

0

0.5

1

time [sec]

accl

erat

ion

[m/s

2]

zacc I [m/s2]

measuredsimulated

Figure 7.1: Results from Model Validation: acceleration plot of flight01

23

Chapter 7. Results and Discussion 24

2 4 6 8 10 12 14 16 18

0.05

0

0.05

[rad

]

pitch angle

measuredsimulated

2 4 6 8 10 12 14 16 18

0.05

0

0.05

[rad

]

roll angle

measuredsimulated

2 4 6 8 10 12 14 16 18

0.2

0.1

0

0.1

0.2

0.3

[rad

/s]

yaw rate

measuredsimulated

Figure 7.2: Results from Model Validation: angles plot of flight01

0 5 10 15 20 25 301

0.5

0

0.5

1

time [sec]

accl

erat

ion

[m/s

2]

xacc I [m/s2]

measuredsimulated

0 5 10 15 20 25 302

1

0

1

time [sec]

accl

erat

ion

[m/s

2]

yacc I [m/s2]

measuredsimulated

0 5 10 15 20 25 304

2

0

2

4

time [sec]

accl

erat

ion

[m/s

2]

zacc I [m/s2]

measuredsimulated

Figure 7.3: Results from Model Validation: acceleration plot of flight03

25 7.1. Dynamical Model Validation

0 5 10 15 20 25 300.1

0.05

0

0.05

0.1

[rad

]

pitch angle

measuredsimulated

0 5 10 15 20 25 300.2

0.1

0

0.1

0.2

[rad

]

roll angle

measuredsimulated

0 5 10 15 20 25 300.5

0

0.5

1

[rad

/s]

yaw rate

measuredsimulated

Figure 7.4: Results from Model Validation: angles plot of flight03

2 4 6 8 10 12 14 16 182

1

0

1

time [sec]

accl

erat

ion

[m/s

2]

xacc I [m/s2]

measured

simulated

2 4 6 8 10 12 14 16 182

1

0

1

time [sec]

accl

erat

ion

[m/s

2]

yacc I [m/s2]

measured

simulated

2 4 6 8 10 12 14 16 182

1

0

1

2

time [sec]

accl

erat

ion

[m/s

2]

zacc I [m/s2]

measured

simulated

Figure 7.5: Results from Model Validation: acceleration plot of flight04

Chapter 7. Results and Discussion 26

2 4 6 8 10 12 14 16 18

0.1

0

0.1

[rad

]

pitch angle

measured

simulated

2 4 6 8 10 12 14 16 18

0.2

0.1

0

0.1

[rad

]

roll angle

measured

simulated

2 4 6 8 10 12 14 16 18

0.2

0

0.2

[rad

/s]

yaw rate

measured

simulated

Figure 7.6: Results from Model Validation: angles plot of flight04

As can be seen in the plots of the validation, the dynamical model evolved inchapter 4 approximates the behavior of the real plant of the helicopter quite well.Especially for the roll and pitch dynamics great results were achieved, but also forthe acceleration in all directions the obtained results are satisfying. For the rate ofthe yaw angle, the simulated result is a lot less noisy than the measured signal, butcontains an offset.

7.2 Flight Control

In the second part of this chapter, the result of the autonomous controlled flightsare presented.

7.2.1 Hovering Flight

The Figures (7.7 - 7.9) show the results of a hovering flight at a constant set-point at

Irrr =

(0.3m 0.5m 0.3m

)T . The corresponding RMS values are providedin Table 7.1. As illustrated is the hovering of the quadrotor quite smooth, but stillprovides room for improvement.

desired value RMS

front x 0.3m 0.0207mside y 0.5m 0.0316m

altitude x 0.3m 0.0117mheading 0 0.1245

Table 7.1: Results from Flight Control: RMS values of a hovering flight

27 7.2. Flight Control

35 40 45 50 55 60 65 70 75 800.36

0.34

0.32

0.3

0.28

0.26

0.24

time [sec]

x [m

]

x estimatedref value

35 40 45 50 55 60 65 70 75 800.6

0.55

0.5

0.45

0.4

time [sec]

y [m

]

y estimatedref value

Figure 7.7: Results from Flight Control: hovering flight (front and side distance)

35 40 45 50 55 60 65 70 750.36

0.34

0.32

0.3

0.28

0.26

time [sec]

altit

ude

[m]

z measured (US sensor)ref value

Figure 7.8: Results from Flight Control: hovering flight (altitude)

7.2.2 Step responses

The following plots (Figure 7.10 - 7.11) illustrate the response of the quadrotor toa step-signal as its input. As can be seen, the controller reacts on a step of 30centimeters in a horizontal direction approximately in 3 seconds, without a hugeovershoot. The performance of the altitude controller is even slightly better.

In Figure 7.12 the system response to a squared reference trajectory, is shown.As can be seen, the designed position controller allows the quadrotor to follow thesimple trajectory. Please note, for more complex maneuvers and a higher perfor-mance an extension of the position controller would be required, which was beyondthe scope of this thesis, see section 8.2.

Chapter 7. Results and Discussion 28

35 40 45 50 55 60 65 70 75 800.4

0.2

0

0.2

0.4

time [sec]

head

ing

angl

e [d

eg]

yaw measuredref value

Figure 7.9: Results from Flight Control: hovering flight (heading angle)

165 170 175 180 185 190 1950.7

0.6

0.5

0.4

0.3

0.2

time [sec]

x [m

]

x estimated

ref value

165 170 175 180 185 190 1951

0.8

0.6

0.4

0.2

time [sec]

y [m

]

y estimated

ref value

120 125 130 135 1400.8

0.6

0.4

0.2

0

time [sec]x

[m]

x estimatedref value

120 125 130 135 1401

0.8

0.6

0.4

0.2

time [sec]

y [m

]

y estimatedref value

Figure 7.10: Results from Flight Control: step response (front and sideways)

110 115 120 125 130 135 140 1451

0.8

0.6

0.4

0.2

time [sec]

altit

ude

[m]

z measured (US sensor)ref value

170 175 180 185 190 195 200 205 210 215

0.8

0.7

0.6

0.5

0.4

0.3

0.2

time [sec]

altit

ude

[m]

z measured (US sensor)

ref value

Figure 7.11: Results from Flight Control: step response (altitude)

0.650.6

0.550.5

0.450.4

0.350.3

0.25

1

0.8

0.6

0.4

0.20.8

0.7

0.6

0.5

0.4

0.3

0.2

0.1

x direction (front) [m]

Position 3D (inertial fram)

y direction (side) [m]

z di

rect

ion

(dow

n) [m

]

Figure 7.12: Results from Flight Control: 3D plot of a flown square

Chapter 8

Conclusion and Outlook

8.1 Conclusion

The simulator evolved during this project (chapter 4) represents the dynamics of thereal quadrotor system quite well. Thus, in MATLAB/Simulink derived controllerscan be applied to the real platform without retuning.

The implementation (as described in chapter 6) of the position controller onthe quadrotor was more complex than assumed, e.g. caused by the rough biasapproximation of the attitude controller input signal or by the roughly estimatedinput conversion. Although the controller, based on the optimal control approach,allows the quadrotor to hover in a smoother way than the former PID approach, itstill did not turn out as smooth as priorly expected.

8.2 Outlook

As outlined above, the interface between the inner and outer control loops is crucialfor a good flight control performance and offers room for improvement.

Although, the AIRobots quadrotor equipped with the position controller, de-rived within this project, allows a step-by-step replacement of single degrees offreedom with vision-based control algorithms.

An extension of the LQRI controller with a feed-forward addition, as proposedby Guzzella [Guz09], may improve the performance of tracking a desired referencesignal, i.e. not only hovering on a given set point. Furthermore other controlapproaches, as H or nonlinear control algorithms as back-stepping are conceivableto fly more complex maneuvers.

29

Chapter 8. Conclusion and Outlook 30

Appendix A

Parameters

A.1 Model Parameters

Black Box Models

Transfer function of the pitch dynamics Transfer function of the roll dynamics

Ppitch(s) = 3023s2+61.36s+941.4 es0.06 Proll(s) = 3096s2+61.98s+950.9 e

s0.06

Transfer function of the yaw dynamics Transfer function of the thrust dynamics

Pyaw(s) = 13.11s+9.404 es0.22 Pthrust(s) = 3.265s+77.23 e

s0.02

Point-Mass Model

mass m 0.536 kggravity constant g 9.81 ms2

A.2 Controller Parameters

The controller and observer parameters are stored as LTI-systems in .mat-files inthe folder src/Hquad_CTRL_OBSV_parameters, below the files of the final positioncontroller version are provided.

Controllers

alt_ctrl_v10.matyaw_ctrl_v10.matdist_ctrl_v16.matside_ctrl_v16.mat

Observers

alt_obsv_bias_v10.matdist_obsv_v10.matside_obsv_v10.matyaw_obsv_v10.mat

31

Appendix A. Parameters 32

A.3 Controller Parameters - Prior PID Approach

P I D

alt 8 4 70

front 150 8 150

side 150 8 150

yaw 3 0.2 0

A.4 Angle Offsets

command bias

pitch 1.996

roll 0.7047

yaw 1.667

Appendix B

Matlab/SimulinkDocumentation

B.1 Source Folder Overview

All MATLAB source files can be found in the folder src, which is structured in thefollowing way

flightdata contains all gathered flight dataHquadModel contains the model parametersIR_control_JN prior PID controllerIR_control_lqr LQR position controllerIR_control_Model_Identification Model identification and validation

B.2 Model Identification and Validation

Pleas note that, all Simulink models are stored in the Simulink library filesSimulinkModel_Hquad_submodels_LIB.mdl respectivelySimulinkModel_Hquad_compmodel_LIB.mdl, thus the single blocks can easily beadded to any Simulink model with drag and drop. Global changes can only beaccomplished in those library files.

B.2.1 Submodel Identification

The identification of each subsystem is divided in three files

main fileruns the identification and validation for the specific submodel, e.g. for thepitch ID_submodel_pitch.m.

estimation fileapplies the prediction error method to estimate the systems dynamics, e.g.for the pitch ID_submodel_pitch_estimation.m.

validation filecompares the estimated system response with the measured data, e.g. for thepitch ID_submodel_pitch_validation.m.

33

Appendix B. Matlab/Simulink Documentation 34

B.2.2 Complete Model Validation

The validation of the complete system is divided in two files

main filespecifies the desired flight data set and runs the validation for the completemodel, used file: ID_complete_model.m.

validation filecompares the estimated system response with the measured data, used file:ID_complete_model_validation.m.

B.3 Position Control

This section gives a short overview of the contains of the folder IR_control_lqr,which is used for the position control task. The folder has the following sub-folders

Hquad_CTRL_OBSV_parameterscontains the derived parameters of the controllers and observers

plot_hquad_valcontains plots of the autonomous flights

test_IMUcontains the experiments of the input conversion 6.2.2

furthermore it contains some help functions and files which are not depict in thefollowing.

m-Files

In the following, the most important files, for the derivation of the LQRI positioncontroller, are briefly introduced

lqr_main.mmain file for the position control

lqr_sim.mruns the simulator

lqr_alt_calc_ctrl.mto calculate the altitude controller

lqr_dist_calc_ctrl.mto calculate the front and side distance controllers

lqr_yaw_calc_ctrl.mto calculate the heading controller

lqr_obsv_calc.mto calculate the state observers

flight_results_analyze.mfor the analyze and illustration of the flight data sets

35 B.3. Position Control

Simulink Library-Files

Hquad_model_LIB.mdlprovides the models of the quadrotors dynamics, used for the simulator.

Hquad_control_LIB.mdlcontains all decoubled controllers.

Simulink Model-Files

host-modelsThis files are used for on-line recording and logging during the flight with thequadrotor. The host-models run on a PC-station and are wireless (XBee)connected to the helicopter.

QuadIf-modelsThe Simulink models for the position controller. The directly built and com-piled code from these models runs on the embedded system on the quadrotor;for more infromation about the code generation see section 2.3.

Simulator_hquad_lqr-modelsTo test the derived controllers without the real platform. Two simulators areprovided, one for a simulation without an observer, one for tests with thestates estimation.

Appendix B. Matlab/Simulink Documentation 36

Appendix C

Flight Results Overview andDescription

In the following list the most important flight data sets are povided. The plotsin chapter 7 were drawn using the data flight_2010_03_19/flight1...4.mat forthe model validation, respectively the data flight_2010_05_26/flight1.mat andflight_2010_05_21/flight15.mat for the controller results.

flight_2010_03_19/flight1.mat../flight2.mat../flight3.mat../flight4.mat

flight_2010_04_21/flight2.matflight_2010_04_22/flight3.mat

flight_2010_05_06/flight10.mat../flight11.mat

flight_2010_05_20/flight1.mat../flight10.mat

flight_2010_05_21/flight15.mat

flight_2010_05_26/flight1.mat

model validationmodel identificationmodel validationmodel validation

altitude controller (LQRI)altitude controller step response

variance calculation (no movement)observer validation

complete position ctrl (v1.0)estimation of the angle offsets

(Quadif_v2_0_lqr, v1.6) step & square

(Quadif_v2_0_lqr, v1.6) hovering

37

Appendix C. Flight Results Overview and Description 38

Bibliography

[Asc] Ascending Technologies GmbH. AscTec Hummingbird Quadrotor Heli-copter. http://www.asctec.de.

[Asc10] AscTec. X-3D-BL Scientific Users Manual. Ascending TechnologiesGmbH, http://www.asctec.de, 2010.

[BNS04] S. Bouabdallah, A. Noth, and R. Siegwart. Pid vs lq control techniquesapplied to an indoor micro quadrotor. Proceedings of the IEEE Interna-tional Conference on Intelligent Robots and Systems, 2004.

[BS05] S. Bouabdallah and R. Siegwart. Backstepping and Sliding-mod Tech-niques Applied to an Indoor Micro Quadrotor. International Conferenceon Robotics and Automation, pages 22472252, April 2005.

[BS07] S. Bouabdallah and R. Siegwart. Advances in Unmmande Aerial Vehi-cles, chapter Design and Control of a Miniature Quadrotor. Springer,2007.

[BSWS] M. Blosch, D. Scaramuzza, S. Weiss, and R. Siegwart. Vision BasedMAV Navigation in Unknown and Unstructured Environments. not yetpublished.

[DW01] H. Durrant-Whyte. Introduction to Estimation and the Kalman Filter.Australian Centre for Field Robotics, The University of Sydney, January2001.

[Glo07] Ch. Glockner. Mechanik III. ETH Zurich, 2007. Course Notes.

[GS03] A. Glattfelder and W. Schaufelberger. Control Systems with Input andOutput Constraints. Springer Verlag, Berlin, 2003.

[Guz09] L. Guzzella. Discrete-Time Control Systems. ETH Zurich, 2009. CourseNotes.

[LK92] F.L. Lewis and EW Kamen. Applied optimal control and estimation.Prentice Hall Englewood Cliffs, NJ, 1992.

[MAT10] MATLAB. System Identification ToolboxTM 7. MathWorks, 2010.

[Roo09] C. Roos. Design of a Collision-Tolerant Airframe for Quadrotors, De-cember 2009. Semester Thesis.

[Sha03] E. Shafai. Einfuhrung in die Adaptive Regelung. IMRT Press, ETHZurich, 2003.

[SNFH05] J. Shin, K. Nonami, D. Fujiwara, and K. Hazawa. Model-based opti-mal attitude and postioning control of small-scale unmanned helicopter.Robotica, 23:5163, 2005.

39

http://www.asctec.dehttp://www.asctec.de

Bibliography 40

[SP96] S. Skogestad and I. Postlethwaite. Multivariable feedback control anal-ysis and design. New York, 1996.

[TR05] N. Trawny and S. Roumeliotis. Indirect Kalman Filter for 3D AttitudeEstimation. Number 2005-002. Multiple Autonomous Roboitc SystemsLaboratory, March 2005.

List of TablesList of FiguresList of SymbolsList of Acronyms and AbbreviationsIntroductionProblem Formulation and ContextPrior and Related Work

Hardware and System DescriptionAIRobots QuadrotorOn-board Attitude ControllerDevelopment EnvironmentPrior Project Status

Definitions and NotationNotationCoordinate FramesInertial FrameBody FrameTransformation Matrices

Dynamic Model of the AIRobots QuadrotorPoint-Mass ModelBlack Box Model for the Attitude ControllerDecoupled SubmodelsPrediction Error Method

Final System, Complete Dynamical Model

Flight Controller DesignController DesignLQR - Linear Quadratic RegulatorLQRI - Extension including Anti Reset WindupDecoupled Position Control Approach

Observer DesignKalman FilterImplemented Observers

Final Closed-Loop Implementation

Hardware Implementation and TestingPlane EstimationAscTec Attitude ControllerCoordinate SystemInput Command ConversionAngle Offsets

Results and DiscussionDynamical Model ValidationFlight ControlHovering FlightStep responses

Conclusion and OutlookConclusionOutlook

ParametersModel ParametersController ParametersController Parameters - Prior PID ApproachAngle Offsets

Matlab/Simulink DocumentationSource Folder OverviewModel Identification and ValidationSubmodel IdentificationComplete Model Validation

Position Control

Flight Results Overview and DescriptionBibliography