Dynamics Identification & Validation, and Position

Autonomous Systems Lab Prof. Roland Siegwart

Semester-Thesis

Dynamics Identification & Validation, and Position Control for a Quadrotor Spring Term 2010

Supervised by: H¨ urzeler Christoph Nikolic Janosch

Author: R¨ uesch Andreas

Abstract Autonomous mobile robots arouse a lot of interest in the past years, often with the goal to support human beings in numerous activities, likewise in the project AIRobots. Within this project, an airframe of a quadrotor was constructed and equipped with flight electronics. This thesis presents a complete system modeling and identification process of the dynamics of the quadrotor, motivated by the posterior derivation of a flight controller. For the sake of which flight data were gathered during manual controlled flights and the prediction error method was applied for the 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 allows the helicopter to hover smoothly. The control algorithm is based on an optimal control problem formulation, namely a linear quadratic regulator method, with integral extensions and anti-reset-windup add-ons, as well as a Kalman observer for the states estimation were applied.

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

i

ii

Acknowledgment This semester thesis was evolved at the Autonomous Systems Lab (ASL) of the Swiss Federal Institute of Technology (ETH) Zurich. The author is very grateful to his supervisors, Christoph H¨ urzeler and Janosch Nikolic for their great support and guidance during the whole process of this project, their assistance was essential for the accomplishment of this thesis. Expressing gratitude to Prof. Roland Y. Siegwart, the head of the Autonomous Systems Lab, for the opportunity to work at his laboratory under ideal conditions on a very exciting topic. Sincere thanks are given to the PhD students of the ASL team, who all the time 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 sustained support 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 1.1 Problem Formulation and Context . . . . . . . . . . . . . . . . . . . 1.2 Prior and Related Work . . . . . . . . . . . . . . . . . . . . . . . . .

1 1 2

2 Hardware and System Description 2.1 AIRobots Quadrotor . . . . . . . . 2.2 On-board Attitude Controller . . . 2.3 Development Environment . . . . . 2.4 Prior Project Status . . . . . . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

3 3 4 4 5

3 Definitions and Notation 3.1 Notation . . . . . . . . . . . . . 3.2 Coordinate Frames . . . . . . . 3.2.1 Inertial Frame . . . . . 3.2.2 Body Frame . . . . . . . 3.2.3 Transformation Matrices

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

7 7 7 7 8 8

4 Dynamic Model of the AIRobots Quadrotor 4.1 Point-Mass Model . . . . . . . . . . . . . . . 4.2 Black Box Model for the Attitude Controller 4.2.1 Decoupled Submodels . . . . . . . . . 4.2.2 Prediction Error Method . . . . . . . 4.3 Final System, Complete Dynamical Model . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

9 9 9 10 11 12

5 Flight Controller Design 5.1 Controller Design . . . . . . . . . . . . . . . . . . . . . . 5.1.1 LQR - Linear Quadratic Regulator . . . . . . . . 5.1.2 LQRI - Extension including Anti Reset Windup 5.1.3 Decoupled Position Control Approach . . . . . . 5.2 Observer Design . . . . . . . . . . . . . . . . . . . . . . 5.2.1 Kalman Filter . . . . . . . . . . . . . . . . . . . . 5.2.2 Implemented Observers . . . . . . . . . . . . . . 5.3 Final Closed-Loop Implementation . . . . . . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

13 13 14 14 16 18 18 18 19

. . . . .

v

. . . . .

6 Hardware Implementation and Testing 6.1 Plane Estimation . . . . . . . . . . . . . 6.2 AscTec Attitude Controller . . . . . . . 6.2.1 Coordinate System . . . . . . . . 6.2.2 Input Command Conversion . . . 6.2.3 Angle Offsets . . . . . . . . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

21 21 22 22 22 22

7 Results and Discussion 7.1 Dynamical Model Validation 7.2 Flight Control . . . . . . . . . 7.2.1 Hovering Flight . . . . 7.2.2 Step responses . . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

23 23 26 26 27

8 Conclusion and Outlook 8.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8.2 Outlook . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

29 29 29

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

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

. . . .

31 31 31 32 32

B Matlab/Simulink Documentation B.1 Source Folder Overview . . . . . . . B.2 Model Identification and Validation . B.2.1 Submodel Identification . . . B.2.2 Complete Model Validation . B.3 Position Control . . . . . . . . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

33 33 33 33 34 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 2.2 2.3

CAD model of the AIRobots Quadrotor . . . . . . . . . . . . . . . . Schematical illustration of the code generation . . . . . . . . . . . . Positioning of the exteroceptive sensors . . . . . . . . . . . . . . . .

4 5 5

3.1

Sketch of the Coordinate Frames . . . . . . . . . . . . . . . . . . . .

8

4.1 4.2 4.3

Point-Mass Model of the AIRobots Quadrotor . . . . . . . . . . . . . Principle of the Black Box Model . . . . . . . . . . . . . . . . . . . . Complete Dynamical Model of the AIRobots Quadrotor . . . . . . .

10 11 12

5.1 5.2 5.3 5.4 5.5 5.6

Main Control Principle . . . . . . . . . . . . . . . . . Block diagram of the closed-loop system with a LQR LQRI control system . . . . . . . . . . . . . . . . . . Principle of the Kalman Estimator . . . . . . . . . . LQRI combined with a Kalman estimator . . . . . . Final position controller . . . . . . . . . . . . . . . .

. . . . . . . . . state feedback. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

13 15 16 18 19 20

6.1

Attitude control-command conversion . . . . . . . . . . . . . . . . .

22

7.1 7.2 7.3 7.4 7.5 7.6 7.7 7.8 7.9 7.10 7.11 7.12

Results Results Results Results Results Results Results Results Results Results Results Results

23 24 24 25 25 26 27 27 28 28 28 28

from from from from from from from from from from from from

Model Validation: acceleration plot of flight01 . . . . . Model Validation: angles plot of flight01 . . . . . . . . Model Validation: acceleration plot of flight03 . . . . . Model Validation: angles plot of flight03 . . . . . . . . Model Validation: acceleration plot of flight04 . . . . . Model Validation: angles plot of flight04 . . . . . . . . Flight Control: hovering flight (front and side distance) Flight Control: hovering flight (altitude) . . . . . . . . Flight Control: hovering flight (heading angle) . . . . . Flight Control: step response (front and sideways) . . . Flight Control: step response (altitude) . . . . . . . . . Flight Control: 3D plot of a flown square . . . . . . . .

viii

List of Symbols Physical Variables ϕ θ ψ Ae x Aey Ae z RAB r OP F T dsharpL dsharpR dsharpS dr b l

Euler angle, roll [−π, π] Euler angle, pitch [−π/2, π/2] Euler angle, yaw [−π, π] unit vector in x direction, expressed in the coordinate frame A unit vector in y direction, expressed in the coordinate frame A unit vector in z direction, expressed in the coordinate frame A rotation matrix form coordinate system B to coordinate system A postion vector of the point P force vector [N ] total thrust moment [N ] measured distance of the left, front IR sensor measured distance of the right, front IR sensor measured distance of the side IR sensor absolute distance between the IR sensor and the C.G. absolute distance between the two front IR sensors gap between the front IR sensors and the C.G.

Controller Variables x(t) xk Ts A, B, C, D F, G, C, D K, KI Q, R, γ Kf P wd wn uk u ˆk yk

Continuous signal for the states Discrete signal for the states, with xk = x(k · Ts ) sampling time Continuous state space matrices Discrete state space matrices LQR gain respectively LQRI gain LQRI tuning parameters Kalman filter gain Transfer function of the plant process noise measurement noise saterurated input signal unsaterurated input signal controller output signal

ix

List of Acronyms and Abbreviations AscTec ASL CAD C.G. DOF ETH GPS IMU I/O IR KF LQR LQRI MAV NED PID RMS UAV US

Ascending Technologies GmbH Autonomous Systems Laboratory Computer aided design Center of gravity degrees of freedom Eidgen¨ ossische Technische Hochschule Global positoning system Inertial measurement Uuit In- and output Infra red Kalman Filter Linear quadratic regulator Linear quadratic regulator with integral extension Micro aerial vehicle North-East-Down system Proportional-integral-differential controller Root mean square Unmanned aerial vehicle Ultra sonic

x

Chapter 1

Introduction Autonomous flight control of unmanned aerial vehicles (UAVs) presents a challenging task. The high agility and autonomy of small-scaled UAVs lead to a huge area of applications. For example, industrial tasks like inspection, measurements or exploration as well as search and rescue tasks or even military applications are conceivable. The successful accomplishment of such applications requires that the flight controller is able to stabilize the motion of the UAV. Thus, the completion of maneuvers such as hovering, obstacle avoidance, autonomous navigation, docking and sensing, is essential. Besides a translational and rotational acceleration measurement, which is accomplished by the inertial measurement unit (IMU) and the gyroscopes, a position control algorithm of any autonomous mobile robot requires information about the position itself, to avoid any drifting errors or integration inaccuracies. The variety of such exteroceptive sensors is wide, e.g., infrared rangefinders, 3D laser rangers, ultrasonic or pressure sensors, global positioning system (GPS) sensors, cameras and many more. For indoor applications, the usage of GPS sensors is strongly limited, in contrast 3D laser rangefinders provide very precise distance measurements, but are usually expensive and due to their high power consumption and weight, the usage on micro aerial vehicles (MAV) is limited. Alternatively, computer vision, which utilizes simple 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 MAV is often a challenging task. This motivates us to develop a test platform, whose position is controlled by much simpler sensors, such as infrared rangefinders and ultrasonic sensors. The exteroceptive sensors controlled testing platform allows, in a further development phase, a step by step procedure, to replace single degrees of freedom with visual servoing algorithms.

1.1

Problem Formulation and Context

A flexible quadrotor platform based on the Asceding Technologies Hummingbird system [Asc] has been developed at ASL. This platform features a simple mechanical construction which easily allows exchanging and adding new components such as camera rigs and mounting racks for small range finders. It also features an electronic I/O system which allows interfacing the Hummingbird attitude controller and additional low-level sensors such as infrared or ultrasonic range finders. As this I/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 vehicle’s flight dynamics and implement a position controller based on infrared and sonar range sensors. The autopilot 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 and nonlinear system. The six degrees of freedom (DOF) of an unconstrained aerial vehicle present a challenging control task. A common approach is to use a cascaded control structure, where an inner loop stabilizes the rotational DOF of the UAV and an outer loop is used for the pose control. Within the project OS4, Bouabdallah et al. [BS07], [BNS04], [BS05] analyzed and applied several control techniques. For simulation purposes Bouabdallah et al. [BS07] evolved a complete physical model of the quadrotor’s dynamics. With an adoption of the integral back-stepping techniques a superior performance was achieved, for the attitude and pose control. As in the sFly project [BSWS] also in this thesis, the quadrotor was equipped with an on-board attitude controller provided by Ascending Technologies [Asc], which is basically a PD-controller. Bl¨osch, Scaramuzza, Weiss et al. proposed a black box modeling approach to model the system dynamics. Furthermore they applied a visual SLAM algorithm for the position estimation and a LQG/LTR based controller for stabilizing the vehicle at a desired set point. Shin et al. [SNFH05] proposed a model-based MIMO-control system for an unmanned helicopter, as well based on a cascaded control structure by applying Kalman 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 System Description The following chapter gives a short overview and description of the used hardware and 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 (Autonomous System Lab) [Roo09], furthermore it is equipped with the flight electronics of the AscTec Hummingbird quadrotor developed by Ascending Technologies GmbH [Asc]. The AIRobots Quadrotor is equipped with a quite simple and compact mechanical 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 additional sensors or cameras, and its relatively low-cost, the AIRobots Quadrotor qualifies as an ideal testing platform. Besides autonomous flight, also manually controlled flights are possible, which proves 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 Quadrotor is illustrated. Table 2.1 provides the most important facts about the AIRobots quadrotor and the AscTec Hummingbird’s flight electronics. nominal mass payload capacity total diameter rotor diameter flight time maximal speed micro processor

536 g 200 g 60 cm 20 cm continuous 50 km/h 1 x ARM7 1 x TMS C2000

piezo 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 Ascending Technologies [Asc], which stabilizes the three rotational degrees of freedom of the helicopter basically with PD controllers, see [BSWS]. Thus, no direct input signals to the rotors are needed, instead the desired tilt angles can directly be transmitted. 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 MATLAB/Simulink of MathWorks1 was used. For an automatic code generation from the Simulink models into the programming language c, the MathWorks products: Real-Time Workshop, Real-Time Workshop Embedded Coder, the Target Support Package 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 generation and 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 controller parameters or observer values, also MATLAB/Simulink was used. Using this setup, the control structure is quickly and easily changeable. The on-line recording of any data proved itself very valuable for the purpose of debugging.

1 MATLAB: 2 Code

version R2009b, August 12, 2009 for Unix and MS Windows systems 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 accomplished by the ASL team. The quadrotor had yet been equipped with three infrared rangefinders as well as a sonar ultra sonic sensor. These exteroceptive sensors are used to estimate the position of the helicopter in the room, namely its distance to the front and side wall, its heading angle relatively to the front wall and its altitude. The positioning of the sensors is illustrated in Figure 2.3. A position controller, based on four decoupled PID controllers for the three translational degrees of freedom plus for the helicopters heading angle relatively to the front wall, had already been implemented and tested. The PID-approach lead to average results, which encouraged to evolve and test an optimal control approach as well as derive a validated model of the helicopter. For the PID-parameters see appendix A.3. Sharp L Sharp R '$ '$ @ @s s @ @ &% &% @ @ @ @ '$@'$ sUS sensor @ s @s @ Sharp side &% &% @

Figure 2.3: Positioning of the exteroceptive sensors

3 Source:

MathWorks 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 notation used in the context of this thesis is depicted in the following, furthermore the used coordinate frames are defined. A complete list of all used symbols and abbreviations is provided at the beginning of this document.

3.1

Notation

Physical Variables Av

RAB ˙ , (·) ¨ (·)

Vector v expressed int the A coordinate system, please note vectors are always denoted in boldface. Transformation Matrix from coordinate system B to coordinate system A. Av = RAB ·B v First respectively second time derivation

Controller Variables x(t) Continuous signal for the states xk Discrete signal for the states, with xk = x(k · Ts ) where Ts denotes the sampling time A, B, C, D Continuous state space matrices F, G, C, D Discrete state space matrices K, KI LQR gain respectively LQRI gain Q, R, γ LQRI tuning parameters

3.2

Coordinate Frames

Within the scope of this project two coordinate frames were used, on the one hand the ground-fixed inertial frame and on the other hand the helicopter-fixed body system. 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) system. Since in this project an indoor MAV without a geographical environment was used, the “north” direction was redefined perpendicular to the front wall, pointing into the wall. The third axis was defined normal to the ground, pointing downwards. Due to the right-hand-system, the “east” direction points perpendicularly 7

Chapter 3. Definitions and Notation Be x    @ r r @   @ @h @ Be z  @ @ @r r   @ @ R Be y @

8

6 Iex ≡ N

h Iez ≡ D

Iey

≡E

-

Figure 3.1: Sketch of the Coordinate Frames

into the side wall. The origin of the inertial frame is laid in the corner, thus only flights 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 in the center of gravity (C.G.) of the helicopter. The quadrotor’s first, respectively second axis are defined to be coaxial with the quadrotor’s arms. The third axis is normal to the helicopters plane and points downwards. In Figure 3.1 the definition is illustrated.

3.2.3

Transformation Matrices

The transformation from the ground-fixed inertial frame into the body-frame, applied within the scope of this thesis, was defined according to the Tait-Bryan convention [Glo07]. Thus, for the rotation matrix RBI follows, RBI (ϕ, θ, ψ)

= =

=

Rϕ · Rθ · Rψ       1 0 0 cθ 0 sθ cψ sψ 0  −sψ cψ 0  ·  0 cϕ sϕ  ·  0 1 0  0 0 1 0 −sϕ cϕ −sθ 0 cθ   cθ cψ cθ sψ −sθ  sϕ sθ cψ − cϕ sψ sϕ sθ sψ + cϕ cψ sϕ cθ  , (3.1) cϕ sθ cψ + sϕ sψ cϕ sθ sψ − sϕ cψ cϕ cθ

where c(. . .) denotes the cos(. . .) function, and s(. . .) analogously the sin(. . .) function. The following relation −1 T RIB (ϕ, θ, ψ) = RBI = RBI ,

(3.2)

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

Chapter 4

Dynamic Model of the AIRobots Quadrotor In the following, the dynamical model derived for the AIRobots Quadrotor is presented. The basic idea is to combine a simple physical point-mass model, to approximate the kinematics of the micro aerial vehicle (MAV), with a black-box model, which represents the attitude controller’s dynamics as well as the rotor and blade dynamics.

4.1

Point-Mass Model

Due to the quadrotor’s small size and symmetry, it can be modeled as a pointmass. According to the principle of linear momentum (Newton’s 2nd Law) and the assumption that the mass of the quadrotor is constant, for the change of the momentum follows X m ·I r¨OP = (4.1) IF i, i

where r¨OP denotes the acceleration of the center of gravity (C.G.) and F i represents any outer force acting on the point-mass. Applied to the quadrotor (see Figure 4.1) for the equation of motion of the vehicle follows     0 0 1 ¨ = RIB (ϕ, θ, ψ) ·  0  +  0  , (4.2) Ir m −T g with T represents the total thrust moment, pointing in the opposing direction of the helicopter’s down axis, and the rotation matrix RIB (ϕ, θ, ψ) from the body frame into 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 (for further information see section 2.2) . Hence, the unknown parameters in the equation (4.2), explicitly the Euler angles and the thrust momentum, are output variables of the attitude controller. Since no exact description of the attitude controller exists and the control parameters are unknown, the dynamics of the inner loop are also identified in the system modeling task. For this purpose a black box modeling approach is used. 9

Chapter 4. Dynamic Model of the AIRobots Quadrotor

10

T      

Be z

 : Bex    Y QH H QHH  Q H Q H  s eH Q B y H  HH  HH  ? F G = mg

wall XXfront XX XX

H HH I r OP H HH H XXX e XXX HHH *I x  XXX H  HH XXX  XX H  XX  XXX  z X  Iey         ? Iez side wall

Figure 4.1: Point-Mass Model of the AIRobots Quadrotor

The main idea of black box modeling is to approximate the dynamics of the system with an educated guess. The system parameters are identified and validated with an adequate method using flight data sets. Furthermore, this modeling method avoids a sumptuous and expensive identification of the rotor dynamic, or an exact description of the quadrotor’s aerodynamics. Because of the quadrotor’s mechanical simplicity (i.a. non-existence of a swashplate, simple design) a black box approach is 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 the roll, pitch, yaw and thrust dynamics. Furthermore a brief overview of the used method 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 into four subsystems, which are identified separately. Roll Dynamics The roll dynamics are approximated with a 2nd order system and a delay element. Its input variable is the desired roll angle, and its output the current roll angle. Pitch Dynamics The pitch dynamics are described likewise with a 2nd order system and a delay element. The input respectively output variables are analogously the desired and the actual pitch angle. Yaw Dynamics For 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 input to this subsystem, respectively the current yaw rate its output variable.

11

4.2. Black Box Model for the Attitude Controller

black box model ϕ∗

θ∗

ω∗

T∗

k·ω02 s2 +2·δ ·ω0 ·s+ω02

· e−s·T

ϕ

k·ω02 s2 +2·δ ·ω0 ·s+ω02

· e−s·T

θ

k T ·s+1

· e−s·T

ω

k T ·s+1

· e−s·T

T

Figure 4.2: Principle of the Black Box Model

Thrust Dynamics The thrust dynamics are as well modeled with a 1st order system and a delay element, here too, the desired and current thrust momentum are the input respectively output values of the system. Please note that after the 1st order element and the delay element, the nominal thrust moment of m · g, required for hovering, is added.

4.2.2

Prediction Error Method

To estimate the model parameters the Prediction Error Method was used. This method uses an iterative nonlinear least-squares algorithm to minimize a cost function, 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) =

N X

e2 (t),

(4.3)

i=1

where e(t) denotes the error between the estimated and the measured values. For linear systems the error can be expressed as, e(t) = H −1 (q)[y(t) − G(q)u(t)].

(4.4)

As can be seen in the definition of the cost function, the estimation becomes the more accurate the more samples are taken. The measured data sets used in this project were gained during manual controlled flight maneuvers. (see appendix C) To apply this parameter identification method, the software package MATLAB was used. The MATLAB command pem computes the prediction error estimate of a general linear system, wherefore the delay elements were manually estimated and iteratively varied. To increase the accuracy of the estimation, the algorithm was applied several times, using the prior estimation as the new initial guess. For more detailed information about the prediction error method the interested reader is referred to [MAT10] and [Sha03], for more details about the application, see appendix B.2.

Chapter 4. Dynamic Model of the AIRobots Quadrotor

4.3

12

Final System, Complete Dynamical Model

Eventually, the final system of the complete model of the AIRobots quadrotor is illustrated in Figure 4.3. The model was used to build a simulator where for example control 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 model validation are provided in chapter 7. Helicopter Device point-mass model

black box model ϕ∗

θ∗

k·ω02 s2 +2·δ ·ω0 ·s+ω02

· e−s·T

k·ω02 s2 +2·δ ·ω0 ·s+ω02

· e−s·T

ϕ

θ I r¨

ω∗

T∗

k T ·s+1

· e−s·T

ω

k T ·s+1

· e−s·T

T

    0 0 = − m1 RIB(ϕ,θ ,ψ) ·  0  +  0  T g

I r¨

R

I r˙

R

Ir

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 complete flight control an outer loop to control the position is needed. The derivation of this position controller is presented in the following chapter. In Figure 5.1 the complete cascaded control principle is illustrated. Besides a brief introduction of the applied control principle, the state observer is provided, and eventually the final, complete control loop is presented. Helicopter Device Ir

ϕ ∗, θ ∗, ω ∗, T ∗

∗

Position Controller

AscTec attitude controller

Quadrotor plant

Ir

ϕ, θ , ω, T

Figure 5.1: Main Control Principle

5.1

Controller Design

Because of near hovering, the cross coupling effects between the roll, pitch, yaw and altitude dynamics are insignificant and thereby negligible, a decoupled control approach is conceivable. As mentioned in section 2.4 a former position controller, applying four decoupled PID controllers led to average results. Thus, for this project an optimal control approach was chosen. At the source of any optimal control approach the formulation of an optimization problem is to be found. According to Skogestad & Postlethwaite [SP96] and Guzzella [Guz09], two main groups of optimization problems are distinguished in modern control theory. The first method is the so-called H2 method, with the goal to minimize the mean value of the so-called “error” signal over all frequencies. On the other hand, a worst-case formulation where the cost function is defined through the worst possible value of the error signal over all frequencies is conceivable, it is called the H∞ method. In this project a linear quadratic regulator (LQR) is applied, which is a member of the H2 optimization group. It is described in more detail in the following. 13

Chapter 5. Flight Controller Design

5.1.1

14

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 system is 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 is defined 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 so that the following cost function is minimized Z ∞
x(t)T · Q · x(t) + u(t)T · R · u(t) · dt, (5.3) J(u) = 0

respectively in the discrete case J(u) =

∞ X

xTk · Q · xk + uTk · R · uk .

(5.4)

k=0

The corresponding weighting matrices Q and R, or in other words the design parameters, 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 + GT SG]−1 GT SF

(5.6)

and S is the unique positive semi-definite solution of the discrete-time algebraic Riccati equation (DARE) F T SG[R + GT SG]−1 GT SF + S − F T SF − Q = 0.

(5.7)

For more mathematical detail, interested readers are referred to the work of Skogestad & Postlethwaite [SP96], Guzzella [Guz09] and Lewis [LK92]. In the scope of this project the gain matrix K was determined by solving the DARE (5.7) with the solver provided by MATLAB. Figure 5.2 illustrates the LQR state 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 was extended with some integral action, because a standard LQR controller would not have a zero output for constant disturbances, e.g. through misalignment of the IMU 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, as outlined in Figure 5.3. For the extended linear system follows, x ˜k+1

=

y˜k

=

˜·u F˜ · x ˜k + G ˜k   γ·I C ·x ˜k

(5.8) (5.9)

15

5.1. Controller Design

Plant uk

xk+1

G

z−1

xk

C

yk

F

−K

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

 T with the new state vector x ˜ k = vk x k , an additional tuning parameter γ and the new system matrices are defined as " # " # I −C 0 ˜= F˜ = , G . (5.10) 0 F G ˜ for the linear quadratic regulator problem for the extended The solution K ˜ R} ˜ can be found analogously to the standard regulator problem ˜ C˜ T C, system {F˜ , G, applying the equations (5.6) and (5.7), see section 5.1.1. The new gain matrix has the following form   ˜ = K KI , K (5.11) the first element K is the solution of the standard LQR problem, the second element is 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 is larger than the maximal possible input, the system typically responds with a huge overshoot 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 are several 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, to avoid a large overshoot in the saturated case. For this purpose the plant input uk is limited by a saturation model of the actuator, i.e.    umax u ˆk uk =   umin

if

u ˆk > umax

if

u ˆk ∈ [umin , umax ]

if

u ˆk < umin

(5.12)

where u ˆk is the required input of the controller. Since a discrete LQRI controller is applied, the anti reset windup is quite simple. The difference between the required u ˆk and saturated uk input signal is negatively fed back to the integrator, thus in the following time step the integrator is reset. Figure 5.3 illustrates the integral extension combined with the anti reset windup.

Chapter 5. Flight Controller Design

16

− +

Plant rk

+

KI −

+

−

vk+1

z−1

vk +

+

Integral Extension

uˆk

uk

+

+

G

xk+1

z−1

xk

C

yk

+

anti reset windup F

−K

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 negligible near hovering flight, thus a decoupled control approach is conceivable. Below the four decoupled LQRI controllers are briefly described, please note that all of them correspond to the control schema derived above. Altitude Control The subsystem of the quadrotor’s altitude simply consists of the black box model of the thrust and basically a double integrator, see section 4.2.1. This leads to the following form for the transfer function of the linear altitude submodel Palt (s) =

s3

b0 , + a2 · s2

(5.13)

where b0 and a2 represent the constant gains, the numerical values can be found in the appendix A.2. The transfer function, (5.13) expressed in the canonical form, leads to the following state space representation   0 1 0 0 " # Aalt Balt  0 0 1 0   = (5.14)  0 0 −a2 1  . Calt Dalt b0 0 0 0 After the discretization, the discrete system matrices {Falt , Galt , Calt , Dalt } can be applied straightforward to the LQRI problem formulation respectively to the DARE equation (5.7). Front and Side Distance Control To control the distance to the wall in front and on the side of the quadrotor the forces in the corresponding directions can be derived from the principle of linear momentum (4.1). The principle of linear momentum can be rewritten and solved for the total force vector acting on the quadrotor, thus the following expression holds:       0 0 I Fx  I Fy  = RIB (ϕ, θ, ψ) ·  0  +  0  . (5.15) −T mg I Fz

17

5.1. Controller Design

According to the Tait-Bryan convention (see (3.1), (3.2)), for the forces in front and side direction follows I Fx

= −T · (cos ψ sin θ cos ϕ + sin ψ sin ϕ),

(5.16)

I Fy

= −T · (sin ψ sin θ cos ϕ − cos ψ sin ϕ),

(5.17)

I Fz

= −T · (cos θ cos ϕ) + mg.

(5.18)

Under the assumption, that cos θ and cos ϕ are different form zero, which always is the case around hovering flight, follows that I Fx

=

I Fy

=

I Fz − mg · (cos ψ sin θ cos ϕ + sin ψ sin ϕ), cos θ cos ϕ I Fz − mg · (sin ψ sin θ cos ϕ − cos ψ sin ϕ). cos θ cos ϕ

(5.19) (5.20)

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

=

−mg · tan θ,

(5.21)

I Fy

=

mg · tan ϕ.

(5.22)

are obtained. Due to the point mass model of the quadrotor (see equation (4.1)) the components of the force vector can be expressed as I Fi

= m ·I r¨i ,

(5.23)

thus for the subsystem of the helicopter’s plant in front direction with the input signal I Fx and output signal I x holds  " #  0 1 0 Adist Bdist =  0 0 1/m  , (5.24) Cdist Ddist 1 0 0 and for the subsystem in the side direction with the input signal I Fy and output signal I y analogously  " #  0 1 0 Aside Bside =  0 0 1/m  . (5.25) Cside Dside 1 0 0 After a discretization of the linear systems above, the LQRI control method is applied. Heading Control Last but not least the subsystem for the quadrotor’s heading angle has to be controlled. The black box model for the dynamics of the yaw rate is simply extended with a integrator to derive the yaw angle. Hence, the transfer function of the linearized subsystem is expressed as Pyaw (s) =

s2

b0 + a1 · s

(5.26)

where b0 and a1 represent the corresponding gains. The numerical values are as well to be found in the appendix A.2. Thus, the state space representation of the yaw subsystem expressed in the canonical form is given as  " #  0 1 0 Ayaw Byaw (5.27) =  0 −a1 1  . Cyaw Dyaw b0 0 0

Chapter 5. Flight Controller Design

18

Also for the heading control, after a discretization of the linear subsystem, it is applied 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 directly available. Hence, for an optimal control approach, as described above, a observer is required to estimate the unmeasurable states. For example, the distance of the quadrotor to the wall can be measured through a infrared sensor, its acceleration in the corresponding direction is provided by the IMU, but its velocity is not directly measured. The field of different observer methods is huge. A complete treatment is beyond the scope of this project. For the estimation of the states a standard Kalman filter approach was applied in the context of this project. This approach is briefly introduced 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 standard state observer, as illustrated in Figure 5.4. The difference between the measured system output and the estimated system output is scaled by the Kalman filter gain Kf and fed back to the observer. The Kalman filter gain Kf itself, is the solution of an optimization problem under the assumption that the process and measurement noise are uncorrelated white noise signals. I.e. the gain matrix, that minimizes the expectation of the estimation error E[(x − x ˆ)T (x − x ˆ)], is chosen. As for the linear quadratic regulator problem, the optimal solution for the Kalman optimization problem is found by solving a discrete algebraic Riccati equation. The solver provided by the software package MATLAB allows a straightforward implementation of a Kalman estimator. For further information see [DW01] or [SP96]. wd k uk

+

+

yk

Plant + +

wnk

+

Kf Gˆ

+ + xˆk+1

z−1

xˆk

−

Cˆ

yˆk

+

xˆk

Fˆ Kalman Estimator

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 through a misalignment between the IMU plane and the helicopter’s hovering plane. Therefore, to derive the observers for each LQRI controller, the state space representations of 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 no longer 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 to be uncorrelated zero-mean Gaussian signals. For the bias estimation in each subsystem an additional state is included, which is modeled as a so-called Random walk process, see [TR05]. Based on the assumption, that the biases of the acceleration measurements are constant over the time but priorly unknown, a Random walk model is conceivable. I.e. the additional state does not depend on any prior state, it depends only on the process noise, so that for the variation of the state the following expression holds x˙ bias = wd .

(5.30)

Due to the Random walk model, the Kalman filter is able to adjust the bias estimation slowly, using the knowledge gained through the measurments. For further information see [DW01] and [TR05]. The extended state space matrices are provided in Table 5.1. Before the corresponding 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 with the state observers, found in section 5.2. Thus, the structure of each decoupled controller corresponds to the block diagram shown in Figure 5.5. − +

wd k rk

+

KI −

+

−

vk+1

z−1

vk +

+

uˆk

uk

+

+

yk

Plant

+

Integral Extension

+

anti reset windup

+

wnk

+

Kf Gˆ

+ + xˆk+1

z−1

xˆk

−

Cˆ

+

Fˆ Kalman Estimator −K

xˆk

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

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

Chapter 5. Flight Controller Design

20

Altitude Observer A  0 0 0

 0 1 0

1 0 0

B

G

CT

D

  0 1 0

  0 1 1

  1 0 0

  0

CT

D

  1 0 0

  0

CT

D

  1 0 0

  0

CT

D

  1 0

  0

Front Distance Observer A  0 0 0

B  0 1 0

1 0 0

  0 1 0

G  0 1 0

 0 0 1

Side Distance Observer A  0 0 0

B  0 1 0

1 0 0

  0 1 0

G  0 1 0

 0 0 1

Heading Angle Observer A  0 0

B  1 0

  1 0

G  1 0

 0 1

Table 5.1: Extended state space matrices for the bias estimation

Helicopter Device

Position Controller Set Point

I

r∗

LQRI front

(ϕ ∗ , θ ∗ , ω ∗ , T ∗ )

LQRI side ˆ (x, ˆ y, ˆ zˆ, ψ)

LQRI heading

Attitude Controller

LQRI altitude

Observer

Kalman Estimator

Figure 5.6: Final position controller

IR/US sensors Hqaud IMU

Chapter 6

Hardware Implementation and Testing Some preliminary remarks to the implementation. The controllers were derived in MATLAB/Simulink, for the code generation the setup described in section 2.3 was used. The manual [Asc10] provides a lot of information about the Hummingbird flight electronics, which was mounted on the AIRobots quadrotor.

6.1

Plane Estimation

The quadrotor is equipped with three infrared sensors, for the positioning see Figure 2.3. With the two sensors pointing to the front wall, the helicopter’s heading angle as well as its distance to the wall were calculated by applying some simple geometric coherences. On the assumption that the quadrotor’s plane is not tilted, i.e. that it is parallel to the ground the heading angle ψ is calculated with   dsharpL − dsharpR (6.1) ψ = arctan b where dsharpL denotes the distance measured with the left infrared sensor, dsharpR denotes analogously the distance measured with the right infrared sensor and b denotes the absolute distance between the two sensors. For the x-component of the position vector I r of the quadrotor the following expression holds   dsharpL + dsharpR x=− + l · cos(ψ) (6.2) 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 to be taken into account. The absolute distance between the infrared sensor and the C.G. is denoted with dr . Remark: the sensor is assumed to be mounted exactly on the quadrotor arm, thus the angle between the sensor and the y-direction is equal to 45◦ . The side distance is expressed through ! √ √ 2 2 y = −dsharpS + · dr · cos(ψ) − · dr · sin(ψ), (6.3) 2 2 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 3rd axis 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 selects the 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 AscTec attitude controller does not satisfy the definition of the body frame as stated in section 3.2. In fact, it corresponds to the definition for the y-axis, but the x-axis is pointing towards the opposite direction, what is probably caused by a right-hand coordinate frame with a third axis pointing in direction of the thrust vector (not downwards). Within this project the body coordinate frame, as defined in section 3.2, was applied. To avoid any problems with the attitude controller, the sign of the 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 attitude controller are stored in an unsigned 8-bit integer value with the resolution of a tenth of 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, are stored in an unsigned 8-bit integer value as well. Its resolution is not explicitly indicated in the manual [Asc10]. Through experiments the resolution of a third of a degree was found, see Figure 6.1. Estimate Pitch command conversion

Estimate Roll command conversion

40

40 measurement fitted conversion

measurement fitted conversion 20 Measured roll angle [deg/10]

Measured pitch angle [deg/10]

20

0

−20

−40

−60

−80 110

0

−20

−40

−60

115

120 125 130 Input command [uint8]

135

140

−80 110

115

120 125 130 Input command [uint8]

135

140

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 the attitude controller. Hence biases arise between the quadrotor’s euler angles and the attitude controller’s euler. With different experiments the offsets were estimated and subtracted from the corresponding position controller outputs, the resulting offsets are denoted in appendix A.4. As a possible improvement, an estimation process, 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 appendix C) 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 the corresponding input data. The Figures (7.1 - 7.6) show the results of the system validation. Please note that, for a comparison between the simulated acceleration values and the acceleration values measured by the IMU a coordination transformation from the body frame into the inertial frame was required, the transformation matrix is provided in section 3.2.3. xacc I [m/s2]

accleration [m/s2]

1 measured simulated

0.5 0 −0.5 −1 2

4

6

8

10 time [sec]

12

14

16

18

yacc I [m/s2]

accleration [m/s2]

0.5 measured simulated 0

−0.5

−1 2

4

6

8

10 time [sec]

12

14

16

18

zacc I [m/s2]

accleration [m/s2]

1 measured simulated

0.5 0 −0.5 −1 2

4

6

8

10 time [sec]

12

14

16

18

20

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

23

Chapter 7. Results and Discussion

24

pitch angle 0.05

[rad]

measured simulated 0

−0.05 2

4

6

8

10

12

14

16

18

roll angle measured simulated

[rad]

0.05

0

−0.05

2

4

6

8

10

12

14

16

18

yaw rate 0.3

measured simulated

[rad/s]

0.2 0.1 0 −0.1 −0.2 2

4

6

8

10

12

14

16

18

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

xacc I [m/s2] accleration [m/s2]

1 measured simulated

0.5 0 −0.5 −1 0

5

10

15 time [sec]

20

25

30

yacc I [m/s2] accleration [m/s2]

1 measured simulated 0

−1

−2 0

5

10

15 time [sec]

20

25

30

zacc I [m/s2] accleration [m/s2]

4 measured simulated

2 0 −2 −4 0

5

10

15 time [sec]

20

25

30

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

25

7.1. Dynamical Model Validation

pitch angle 0.1 measured simulated

[rad]

0.05 0 −0.05 −0.1 0

5

10

15

20

25

30

roll angle 0.2 measured simulated

[rad]

0.1 0 −0.1 −0.2 0

5

10

15

20

25

30

yaw rate 1

[rad/s]

measured simulated 0.5

0

−0.5 0

5

10

15

20

25

30

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

xacc I [m/s2]

accleration [m/s2]

1

0

measured

−1

simulated −2 2

4

6

8

10 time [sec]

12

14

16

18

yacc I [m/s2]

accleration [m/s2]

1

0

−1

measured simulated

−2 2

4

6

8

10 time [sec]

12

14

16

18

zacc I [m/s2] 2 accleration [m/s2]

measured simulated

1 0 −1 −2 2

4

6

8

10 time [sec]

12

14

16

18

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

Chapter 7. Results and Discussion

26

pitch angle

[rad]

0.1

0 measured

−0.1

simulated 2

4

6

8

10

12

14

16

18

roll angle 0.1

[rad]

0 −0.1 measured −0.2

simulated 2

4

6

8

10

12

14

16

18

yaw rate

[rad/s]

0.2 0 measured

−0.2

simulated 2

4

6

8

10

12

14

16

18

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 in chapter 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 for the acceleration in all directions the obtained results are satisfying. For the rate of the yaw angle, the simulated result is a lot less noisy than the measured signal, but contains an offset.

7.2

Flight Control

In the second part of this chapter, the result of the autonomous controlled flights are 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
T ∗ −0.3m −0.5m −0.3m . The corresponding RMS values are provided Ir = in Table 7.1. As illustrated is the hovering of the quadrotor quite smooth, but still provides room for improvement.

front x side y altitude x heading ψ

desired value

RMS

−0.3m −0.5m −0.3m 0◦

0.0207m 0.0316m 0.0117m 0.1245◦

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

27

7.2. Flight Control

−0.24 x estimated ref value

−0.26

x [m]

−0.28

−0.3

−0.32

−0.34

−0.36 35

40

45

50

55 time [sec]

60

65

70

75

80

−0.4 y estimated ref value

y [m]

−0.45

−0.5

−0.55

−0.6 35

40

45

50

55 time [sec]

60

65

70

75

80

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

−0.26 z measured (US sensor) ref value

altitude [m]

−0.28 −0.3 −0.32 −0.34 −0.36 35

40

45

50

55 time [sec]

60

65

70

75

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 to a step-signal as its input. As can be seen, the controller reacts on a step of 30 centimeters in a horizontal direction approximately in 3 seconds, without a huge overshoot. 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 the simple trajectory. Please note, for more complex maneuvers and a higher performance an extension of the position controller would be required, which was beyond the scope of this thesis, see section 8.2.

Chapter 7. Results and Discussion

28

0.4 yaw measured ref value heading angle [deg]

0.2

0

−0.2

−0.4 35

40

45

50

55 time [sec]

60

65

70

75

80

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

0

−0.2

x estimated ref value

−0.3

−0.2

x [m]

x [m]

−0.4

−0.4

−0.5

−0.6

x estimated

−0.6

ref value

−0.8

−0.7 165

170

175

180 time [sec]

185

190

120

195

−0.2

125

130 time [sec]

135

140

−0.2 y estimated ref value

y estimated ref value

−0.4 y [m]

y [m]

−0.4

−0.6

−0.8

−0.6

−0.8

−1 165

170

175

180 time [sec]

185

190

−1

195

120

125

130 time [sec]

135

140

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

−0.2

−0.2

z measured (US sensor) ref value

−0.3 altitude [m]

−0.6

−0.8

−0.4 −0.5 −0.6 z measured (US sensor)

−0.7

ref value

−0.8 −1 110

115

120

125

130

135

140

145

170

175

180

185

time [sec]

190 195 time [sec]

200

205

210

215

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

Position 3D (inertial fram)

−0.1 −0.2

z direction (down) [m]

altitude [m]

−0.4

−0.3 −0.4 −0.5 −0.6 −0.7 −0.8 −0.2 −0.4

−0.25 −0.3 −0.6

−0.4

−0.35

−0.45 −0.5

−0.8 y direction (side) [m]

−0.55 −1

−0.6 −0.65

x direction (front) [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 the real quadrotor system quite well. Thus, in MATLAB/Simulink derived controllers can be applied to the real platform without retuning. The implementation (as described in chapter 6) of the position controller on the quadrotor was more complex than assumed, e.g. caused by the rough bias approximation of the attitude controller input signal or by the roughly estimated input conversion. Although the controller, based on the optimal control approach, allows the quadrotor to hover in a smoother way than the former PID approach, it still 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 crucial for a good flight control performance and offers room for improvement. Although, the AIRobots quadrotor equipped with the position controller, derived within this project, allows a step-by-step replacement of single degrees of freedom with vision-based control algorithms. An extension of the LQRI controller with a feed-forward addition, as proposed by Guzzella [Guz09], may improve the performance of tracking a desired reference signal, i.e. not only hovering on a given set point. Furthermore other control approaches, as H∞ or nonlinear control algorithms as back-stepping are conceivable to 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 −3023 s2 +61.36·s+941.4

Ppitch (s) =

· e−s·0.06

Proll (s) =

Transfer function of the yaw dynamics Pyaw (s) =

13.11 s+9.404

Transfer function of the roll dynamics −3096 s2 +61.98·s+950.9

· e−s·0.06

Transfer function of the thrust dynamics

· e−s·0.22

Pthrust (s) =

3.265 s+77.23

· e−s·0.02

Point-Mass Model mass m gravity constant g

A.2

0.536 kg 9.81 sm2

Controller Parameters

The controller and observer parameters are stored as LTI-systems in .mat-files in the folder src/Hquad_CTRL_OBSV_parameters, below the files of the final position controller version are provided. Controllers alt_ctrl_v10.mat yaw_ctrl_v10.mat dist_ctrl_v16.mat side_ctrl_v16.mat Observers alt_obsv_bias_v10.mat dist_obsv_v10.mat side_obsv_v10.mat yaw_obsv_v10.mat 31

Appendix A. Parameters

A.3

Controller Parameters - Prior PID Approach P

I

D

8

4

70

front

150

8

150

side

150

8

150

yaw

3

0.2

0

alt

A.4

Angle Offsets

command pitch

32

bias 1.996◦

roll

0.7047◦

yaw

1.667◦

Appendix B

Matlab/Simulink Documentation B.1

Source Folder Overview

All MATLAB source files can be found in the folder src, which is structured in the following way flightdata HquadModel IR_control_JN IR_control_lqr IR_control_Model_Identification

B.2

contains all gathered flight data contains the model parameters prior PID controller LQR position controller Model identification and validation

Model Identification and Validation

Pleas note that, all Simulink models are stored in the Simulink library files SimulinkModel_Hquad_submodels_LIB.mdl respectively SimulinkModel_Hquad_compmodel_LIB.mdl, thus the single blocks can easily be added to any Simulink model with “drag and drop”. Global changes can only be accomplished in those library files.

B.2.1

Submodel Identification

The identification of each subsystem is divided in three files main file runs the identification and validation for the specific submodel, e.g. for the pitch ID_submodel_pitch.m. estimation file applies the prediction error method to estimate the system’s dynamics, e.g. for the pitch ID_submodel_pitch_estimation.m. validation file compares the estimated system response with the measured data, e.g. for the pitch ID_submodel_pitch_validation.m. 33

Appendix B. Matlab/Simulink Documentation

B.2.2

34

Complete Model Validation

The validation of the complete system is divided in two files main file specifies the desired flight data set and runs the validation for the complete model, used file: ID_complete_model.m. validation file compares 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_parameters contains the derived parameters of the controllers and observers • plot_hquad_val contains plots of the autonomous flights • test_IMU contains the experiments of the input conversion 6.2.2 furthermore it contains some help functions and files which are not depict in the following. m-Files In the following, the most important files, for the derivation of the LQRI position controller, are briefly introduced • lqr_main.m main file for the position control • lqr_sim.m runs the simulator • lqr_alt_calc_ctrl.m to calculate the altitude controller • lqr_dist_calc_ctrl.m to calculate the front and side distance controllers • lqr_yaw_calc_ctrl.m to calculate the heading controller • lqr_obsv_calc.m to calculate the state observers • flight_results_analyze.m for the analyze and illustration of the flight data sets

35

B.3. Position Control

Simulink Library-Files • Hquad_model_LIB.mdl provides the models of the quadrotor’s dynamics, used for the simulator. • Hquad_control_LIB.mdl contains all decoubled controllers. Simulink Model-Files • host-models This files are used for on-line recording and logging during the flight with the quadrotor. The host-models run on a PC-station and are wireless (XBee) connected to the helicopter. • QuadIf-models The Simulink models for the position controller. The directly built and compiled 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-models To test the derived controllers without the real platform. Two simulators are provided, one for a simulation without an observer, one for tests with the states estimation.

Appendix B. Matlab/Simulink Documentation

36

Appendix C

Flight Results Overview and Description In the following list the most important flight data sets are povided. The plots in chapter 7 were drawn using the data flight_2010_03_19/flight1...4.mat for the model validation, respectively the data flight_2010_05_26/flight1.mat and flight_2010_05_21/flight15.mat for the controller results. flight_2010_03_19/flight1.mat ../flight2.mat ../flight3.mat ../flight4.mat

model model model model

validation identification validation validation

flight_2010_04_21/flight2.mat flight_2010_04_22/flight3.mat

altitude controller (LQRI) altitude controller step response

flight_2010_05_06/flight10.mat variance calculation (no movement) ../flight11.mat observer validation flight_2010_05_20/flight1.mat complete position ctrl (v1.0) ../flight10.mat estimation of the angle offsets flight_2010_05_21/flight15.mat (Quadif_v2_0_lqr, v1.6) step & square flight_2010_05_26/flight1.mat

(Quadif_v2_0_lqr, v1.6) hovering

37

Appendix C. Flight Results Overview and Description

38

Bibliography [Asc]

Ascending Technologies GmbH. AscTec Hummingbird Quadrotor Helicopter. http://www.asctec.de.

[Asc10]

AscTec. X-3D-BL Scientific User’s Manual. Ascending Technologies GmbH, http://www.asctec.de, 2010.

[BNS04]

S. Bouabdallah, A. Noth, and R. Siegwart. Pid vs lq control techniques applied to an indoor micro quadrotor. Proceedings of the IEEE International Conference on Intelligent Robots and Systems, 2004.

[BS05]

S. Bouabdallah and R. Siegwart. Backstepping and Sliding-mod Techniques Applied to an Indoor Micro Quadrotor. International Conference on Robotics and Automation, pages 2247–2252, April 2005.

[BS07]

S. Bouabdallah and R. Siegwart. Advances in Unmmande Aerial Vehicles, chapter Design and Control of a Miniature Quadrotor. Springer, 2007.

[BSWS]

M. Bl¨ osch, D. Scaramuzza, S. Weiss, and R. Siegwart. Vision Based MAV Navigation in Unknown and Unstructured Environments. not yet published.

[DW01]

H. Durrant-Whyte. Introduction to Estimation and the Kalman Filter. Australian Centre for Field Robotics, The University of Sydney, January 2001.

[Glo07]

Ch. Glockner. Mechanik III. ETH Zurich, 2007. Course Notes.

[GS03]

A. Glattfelder and W. Schaufelberger. Control Systems with Input and Output Constraints. Springer Verlag, Berlin, 2003.

[Guz09]

L. Guzzella. Discrete-Time Control Systems. ETH Zurich, 2009. Course Notes.

[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, December 2009. Semester Thesis.

[Sha03]

E. Shafai. Einf¨ uhrung in die Adaptive Regelung. IMRT Press, ETH Zurich, 2003.

[SNFH05] J. Shin, K. Nonami, D. Fujiwara, and K. Hazawa. Model-based optimal attitude and postioning control of small-scale unmanned helicopter. Robotica, 23:51–63, 2005. 39

Bibliography

40

[SP96]

S. Skogestad and I. Postlethwaite. Multivariable feedback control analysis and design. New York, 1996.

[TR05]

N. Trawny and S. Roumeliotis. Indirect Kalman Filter for 3D Attitude Estimation. Number 2005-002. Multiple Autonomous Roboitc Systems Laboratory, March 2005.