diplomadolgozat - wordpress.com · 2014. 3. 23. · universityofpannonia...

DIPLOMADOLGOZAT

Domján Balázs

2013

University of Pannonia

Department of Electrical Engineering and Information

Systems

MSc in Computer Science and Engineering

MSc DISSERTATION

Control of a quadrocopter using smartphone

Balázs Domján

Supervisor: Dr. Attila Magyar

2013

ezen oldal helyett jon a feladatkiiras!!!!

NyilatkozatAlulírott Domján Balázs diplomázó hallgató, kijelentem, hogy a szakdolgozatot

a Pannon Egyetem Villamosmérnöki és Információs Rendszerek tanszékén készítet-

tem Mérnök Informatikus MSc szak (MSc in Computer Science and Engineering)

megszerzése érdekében. Kijelentem, hogy a szakdolgozatban lévő érdemi rész saját

munkám eredménye, az érdemi részen kívül csak a hivatkozott forrásokat (szakiro-

dalom, eszközök, stb.) használtam fel.

Tudomásul veszem, hogy a szakdolgozatban foglalt eredményeket a Pannon Egyetem,

valamint a feladatot kiíró szervezeti egység saját céljaira szabadon felhasználhatja.

Veszprém, 2013. május 6.

Aláírás

Alulírott Dr. MagyarAttila témavezető kijelentem, hogy a szakdolgozatot Domján

Balázs a Pannon Egyetem Villamosmérnöki és Információs Rendszerek tanszékén

készítette Mérnök InformatikusMSc szak (MSc in Computer Science and Engineer-

ing) megszerzése érdekében. Kijelentem, hogy a szakdolgozat védésre bocsátását

engedélyezem.

Veszprém, 2013. május 6.

Aláírás

Köszönetnyilvánítás

Köszönettel tartozom témavezetőmnek, Dr. MagyarAttilának a szakmai irányításért

és, hogy bármikor fordulhattam hozzá segítségért.

Abstract

The main goal of the project is to investigate the real-time behaviour of the Android

platform while demostrate it with a live quadrocopter model. Thus it is neccessary

to control the attitude of a quadrocopter with an application which runs on an on-

board Android [1] based device. Another goal is to investigate multiple nonlinear

controller design methods.

As we will see, the system consists of several software components, each com-

ponent does its responsibilities. There are four modules: motor control, sensor data

preprocessor, attitude control and remote control. Sensor data preprocessor filters

and fuses together multiple sensors to get an accurate estimation, and gives this data

to the attitude control. Attitude control calculates the desired thrust based on the

position estimation and the desired position. Motor control, hosted on an Arduino

board, converts the desired trust to PWM (PulseWidthModulation) signals. Remote

control can modify the parameters of the attitude controller and the filter module in

real-time.

Result showed that LQ and QP controllers provided similar results, however

when we used the latter method, we realized that the problem may become harder if

we want to feedback more variables. Because of the number of the quasimonomi-

als, the matrices describing the problem becomes larger, thus the controller design

becomes more complex. The Android phone which we used for the development

behaved very well under a medium load and in a real-time application.

Keywords: quadrocopter, nonlinear model, sensor fusion, nonlinear control

Tartalmi összefoglaló

A dolgozat célja, hogy megvizsgálja az Android operációs rendszerrel rendelkező

okostelefonok valós idejű feltételeket megkívánó alkalmazásokban nyújtott teljesít-

ményét. Emellett a négyrotoros helikopter nemlineárismodelljére tervezett irányítási

célú LQ és kvázipolinom szabályzók tervezési kérdéseit, teljesítményeit veti össze.

A vizsgált tárgy jellege miatt a feladat megoldásához hozzátartozott az elkészült

szabályzók implementálása az adott platformon. A vizsgálat és a fejlesztés menete

megkívánta a különböző eszközök (pl. valós idejű analizáló alkalmazás) fejlesztését

valamint Android platform által kínált szenzorok adatainak megfelelő minőségű fel-

dolgozását. Az adott hardver és szoftverkomponensek fejlesztése során fény derült

a tervezés bonyolultságára továbbá a teljesítményt szimulált környezetben lehetett

összehasonlítani. Az operációs rendszer valós idejű viselkedésétmegelőzőmérések-

kel és az elkészült rendszer éles működés közbeni viselkedésével lehetett felmérni.

Az eredmények azt mutatják, hogy a vizsgált LQ és kvázipolinom szabályozók

hasonlóan jó teljesítményt nyújtanak, viszont a második esetben a tervezés során

a probléma bonyolultsága könnyen megnövekedhet, ami megnehezíti a megfelelő

stabilizáló visszacsatolás megtalálását. A fejlesztéshez használt Android alapú mo-

bitelefon megfelelő számítási kapacitással rendelkezik a jelfeldolgozási, irányítási

műveletek elvégzésére, az időzítések időpontjai determinisztikusnak tekinthetők és

a rendszeren átfutó üzenetek rövid válaszidővel rendelkeznek.

Kulcsszavak: kvadrokopter, szabályozó, nemlineáris modell, szenzor fúzió

Contents

List of Symbols 18

1 Introduction 19

1.1 Motivations and aim . . . . . . . . . . . . . . . . . . . . . . . . . 19

1.2 Quadrocopter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

1.3 Android . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

1.4 Android platform sensors [4] . . . . . . . . . . . . . . . . . . . . . 23

1.4.1 Accelerometer . . . . . . . . . . . . . . . . . . . . . . . . 23

1.4.2 Gyroscope . . . . . . . . . . . . . . . . . . . . . . . . . . 24

1.4.3 Magnetometer . . . . . . . . . . . . . . . . . . . . . . . . 24

1.4.4 Pressure sensor . . . . . . . . . . . . . . . . . . . . . . . . 25

1.4.5 Orientation sensor . . . . . . . . . . . . . . . . . . . . . . 25

1.4.6 Gravity sensor . . . . . . . . . . . . . . . . . . . . . . . . 25

1.5 Inertial measurement unit . . . . . . . . . . . . . . . . . . . . . . . 25

2 Basic notions 26

2.1 Kalman filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

2.2 Linear quadratic servo [15] . . . . . . . . . . . . . . . . . . . . . . 28

2.3 Quasi-polynomial and Lotka-Voltera systems [10] . . . . . . . . . . 28

2.4 Embedding process systems into QP form . . . . . . . . . . . . . . 29

3 System description 30

3.1 Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

3.2 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

4 Processing sensor data 33

4.1 Using Android smartphone as an IMU . . . . . . . . . . . . . . . . 34

4.1.1 Android real-time capability . . . . . . . . . . . . . . . . . 35

4.1.2 Accessing sensors . . . . . . . . . . . . . . . . . . . . . . 36

4.1.3 Sample collection . . . . . . . . . . . . . . . . . . . . . . . 37

4.2 Rotation in 3D . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

4.2.1 Tilt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

4.2.2 Sensor fusion . . . . . . . . . . . . . . . . . . . . . . . . . 39

4.2.3 Tuning filter parameters . . . . . . . . . . . . . . . . . . . 42

4.2.4 Real-time analysis tool . . . . . . . . . . . . . . . . . . . . 43

4.2.5 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

4.2.6 Heading . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

4.3 Position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

4.3.1 Altitude filtering . . . . . . . . . . . . . . . . . . . . . . . 47

4.3.2 Altitude results . . . . . . . . . . . . . . . . . . . . . . . . 48

4.4 Inner state . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

4.4.1 Rotation matrix representation . . . . . . . . . . . . . . . . 49

4.4.2 Quaternion representation . . . . . . . . . . . . . . . . . . 50

5 Control 51

5.1 Nonlinear state-space model . . . . . . . . . . . . . . . . . . . . . 51

5.2 Tilt stabilization . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

5.2.1 Linearization . . . . . . . . . . . . . . . . . . . . . . . . . 53

5.2.2 Continuous time LQ-servo design . . . . . . . . . . . . . . 54

5.2.3 Discrete LQ-servo . . . . . . . . . . . . . . . . . . . . . . 57

5.2.4 Implementation of the LQ-servo . . . . . . . . . . . . . . . 58

5.3 Altitude stabilization using non-linear feedback input-output lineariza-

tion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

5.4 Result of the controller . . . . . . . . . . . . . . . . . . . . . . . . 61

5.5 Altitude stabilization problem seen as quasi-polynomial controller

design problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

5.5.1 Embedding altitude equations into QP form . . . . . . . . . 62

5.5.2 Design the controller . . . . . . . . . . . . . . . . . . . . . 63

5.5.3 Converting and solve the problem . . . . . . . . . . . . . . 66

5.5.4 Altitude equations in QP form using Taylor polynomial . . . 68

5.6 Performance testing . . . . . . . . . . . . . . . . . . . . . . . . . . 70

6 Remote control 70

6.1 Server side . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

6.2 Client side . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71

7 Actuation 72

7.1 Communication through USB . . . . . . . . . . . . . . . . . . . . 72

7.2 PWM generation . . . . . . . . . . . . . . . . . . . . . . . . . . . 74

8 Conclusion 76

8.1 Nonlinear control of the quadrocopter . . . . . . . . . . . . . . . . 76

8.2 Future work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

A RPM counter Arduino code 83

B Contents of DVD attachment 85

17

List of Symbols

x, y, z Frame axes

α Tilt angle [rad]

ω Angular velocity [rad/s]

Φ Roll angle [rad]

Θ Pitch angle [rad]

Ψ Yaw angle [rad]

Ixx,yy,zz Inertia moments [kgm2]

Jr Rotor intertia [kgm2]

l Horizontal distance from vertical rotor axis to center of gravity [m]

b Thrust factor [Ns2]

d drag factor [Nms2]

m overall mass of the quadrocopter [kg]

18

1 Introduction

1 Introduction

In this thesis I would like to create a four rotor flying device which autonomously

hovers, that is what I mean when I say 'control' the quadorocopter. In normal case,

there is a flying helicopter-like device and there is a person, who cotrols its attitude,

tilt or position. He or she tells the flying model to move to an arbitrary position

or move for example its left side slightly up or down. Normally, we can think of

that, if the four propellers are rotating the same speed, it horizontally hovers in the

air and stays in one position. However if one of its propeller has a different speed

then it sidles somewhere else. It is hard to pick up the right propeller speeds if there

are some random disturbances up in the air, like the wind blows. Therefore to help

that person balancing the device I put a computer onto the plane which constantly

monitors its attitude and calculates the necessary speeds to change its tilt back to the

desired one.

1.1 Motivations and aim

The main goal of the project is to investigate the possibility that an Android [1]

based mobile device can control the attitude of a quadrocopter with an application

which runs on it using its own sensors. The application must provide an interface for

another application that can be used as a remote controller. The remote controller is

necessary for the parameter tuning process and the position control. The application

utilizes the capabilities of modern smartphones, collects and processes sensor data

and sends control signals to the motors. The project contains signal processing and

control theory application elements by its nature. The completed model can be used

in the future for educational purposes.

Firstly, it is necessary to study the field of quadrocopter modeling and choose a

dynamical model, which is needed to be implemented in Matlab/Simulink environ-

ment. Parallelly, a physical quadrocopter model should be built using prefabricated

model parts, like a frame, motors and propellers. This physical model us used for the

experiments where the real performance of the designed controller is investigated.

For the low level actuation a microcontroller platform should be chosen, which

19

1 Introduction

will be installed on the physical model of the quadrocopter. The microcontroller

will give commands directly to the motors.

The microcontroller is only for the low lever operations. The model should be

extended with a selected Android based smartphone which will be the 'computing'

unit. The sensors of this smartphone will be monitored and processed to perform

controlling tasks with them. Thus it is necessary to investigate the real-time capa-

bilities of the Android operating system.

A communication protocol between themicrocontroller and the smartphone should

be designed. The physical layer of the communication should be chosen. Wired and

wireless options should be considered.

An Android application should be developed which implements a Kalman filter

to estimate the actual state of the quadrocopter. The application will implement

model-based controller which will adjust its tilt and altitude. A quasi-polynomial

representation of the system will be investigated and its applicability for controlling

purposes.

According to the limited time, the designed controllers should be implemented

for the dynamical model of the system, and then it would be nice to implement and

test one of these controllers on the physical model.

1.2 Quadrocopter

A quadrocopter, also called a quadrotor is a rotor-craft that is lifted by four rotors.

First quadrotors developed by Etienne Oehmichen, George de Bothezat and Ivan

Jerome in the early 1920s [2], but these engines were not mass produced. It has

recently come "back" into the public consciousness.

The completed quadrocopter can be used for some real world applications. With

the device we can observe and object or a land from birds sight, it has some military

aspect application. More than that hobby modelers often choose this kind of aerial

vehicle because of its easy controlling properties.

Unlike helicopters, quadrotors have a symmetric design, so it is easier to main-

tain the stability of the aircrafts. It is also an advantage, that there is no need to tilt

the rotor to change the direction. One can control the attitude, thus the position, of a

20

1 Introduction

quadrocopter by adjusting the rotation rate of the rotors. All of the four rotors have

thrust and torque. In helicopters there is a tail rotor to counterweight the torque gen-

erated by the main rotor. However in quadcopters the solution is simple: the rotors

have pairwise opposite rotating direction, so they kill each others generated torques.

All things considered, it is relatively simple to build one.

The attitude of the quadcopter can be defined by three angles: Roll, Pitch and

Yaw. These angles determine the rotation around y, x and z axis respectively. Rota-

tion around x axis means that the quadrocopter moves forward or backward. Rota-

tion around y axis means it sidles left or right, and changing the yaw means it turns

around its vertical axis. Note that x and y axes are reversed according to popular

configurations thus quadrocopter's frame coordinate system matches to the phone

coordinate system (see Figure 3)...

y axis

.

x axis

.

z axis

..

front

.

back

.

right

.

left

.

smartphone

.

roll

.

yaw

.

pitch

Figure 1: Quadrocopter frame shows the basic concept and direction of rotation of

the propellers

There are two popular configurations of a quadrotor: "X" config and "+" config,

which is implemented in this thesis. The "X" configuration is rotated by 45 degrees

around the z axis compared to the "+" configuration.

If we assume the "+" configuration and that, the quadrocopter is perfectly bal-

anced, we can control the attitude as follows: To change the current roll angle we

need to change the difference between the angular velocities of the left motor and

the right motor, if we assume that the front motor is on the y axis. The change

of the angle will be positive when the velocity of the right rotor is decreased, and

21

1 Introduction

the velocity of the left rotor is increased (moving right). A pitch motion can be

achieved by changing the front rotor and back rotor velocities. The change is pos-

itive when the velocity of the front rotor is increased, and the velocity of the back

rotor is decreased (moving backward). The quadrotor does a yaw motion if two

opposite motor velocities increased and the other two motor speeds decreased.

1.3 Android

Android is a popular open source operating system for mobile platform developed

by Google. There are more than 600000 applications and games are available on

that platform. It is a Linux based OS which supports multiple CPU architectures,

like ARM, x86, MIPS. The current version is 4.2 which called "Jelly Bean" and

this is the version on which I will develop the quadrocopter software. Android is

Figure 2: The Android logo

written in Java and the Eclipse based Android software development kit provides all

the necessary tools to develop applications. Android high level API provides rich

library to build user interfaces and general purpose mobile applications. The SDK

is available for Windows, Mac OS X and Linux. Android Developer tools plugin

for Eclipse contains various tools to manage the software life cycle. It contains

graphical UI builder, debugging, profiling tools, a scriptable test environment and

it is easy (one click) to deploy applications to the phone. It is also possible with

the SDK to develop on virtual devices: there is an android emulator which provides

virtual sensors as well.

22

1 Introduction

1.4 Android platform sensors [4]

In a high-priced smartphone there are several sensors: three-axis accelerometer,

three-axis gyroscope, barometer, magnetometer (compass), camera and GPS. Even

in a low-priced smartphone we can found an accelerometer and a compass. An An-

droid phone which equipped with the necessary sensors is expensive yet, but with

time it is getting cheaper and more and more demand arises for any software which

uses these sensors. On Android based devices there are also derived sensors, which

preprocess and fuse multiple sensors to get for example a rotation matrix. Figure 3

shows the coordinate system of Android sensors.

Figure 3: Android coordinate system [4]

1.4.1 Accelerometer

Android provides the three-axis accelerometer data inm/s2 units. This sensor mea-

sures acceleration along x, y, and z axes including the gravity. It measures the

current gravity along its z axis when the phone lies on a table. Both noise and drift

(offset) error could be added to the sensor data. With the known of the current grav-

ity on all three axes it is easy to calculate the the yaw, pitch and roll angles of the

quadrocopter. Integrating it twice we can get the displacement for a given time. On

Figure 4 an application can be seen which measures the linear acceleration.

23

1 Introduction

Figure 4: Z - Device Test [17] application measures linear acceleration

1.4.2 Gyroscope

Three-axis gyroscope sensor measures the angular velocity around x, y and z axes.

Its measurement unit is rad/s. The sensor provides raw rotational data, and it also

contains noise and drift. The sensor coordinate system is the same that the ac-

celerometer uses, and the rotational velocity is positive, when the direction is counter

- clockwise. In this project, gyroscope is used for an additional pillar to enhance the

accuracy of the tilt and the heading.

1.4.3 Magnetometer

The sensor provides themagnetic field strength data inµT units. The Earthmagnetic

field changes can be monitored with this device. Instead of the magnetometer, it

is recommended to use the orientation sensor which is a derived sensor type and

fuses the magnetometer with the accelerometer. The quadrocopter heading can be

calculated if we know the magnetic field strength on all axes.

24

1 Introduction

1.4.4 Pressure sensor

Pressure sensor measures the ambient air pressure in hPa ormbar units. And from

the pressure sensor data one can calculate the current altitude above sea level. This

sensor will be used by the altitude controller.

1.4.5 Orientation sensor

It is a derived sensor which means that it uses raw sensors to calculate a more accu-

rate measurement for easier usage. This sensor measures the attitude of the device

relative to the Earth frame. It provides the rotation angle around x (pitch), y (roll)

and z (yaw) axes in degrees unit. Azimuth has a range from 0 to 360, pitch has

from -180 to 180 and roll has from -90 to 90.

1.4.6 Gravity sensor

The gravity sensor is also a derived type sensor. This sensor uses accelerometer

to calculate the gravitational forces along axes. From the gravitational forces we

can easily calculate the tilt around x and y axes. The gravity and orientation sensor

is used for compare the implemented Kalman filter performance with the built-in

filtering.

1.5 Inertial measurement unit

Most flight stabilization systems use an IMU (Inertial Measurement Unit) to control

or stabilize the attitude of the multi rotor aircraft. IMU provides velocity, orientation

and attitude information of the device usingmultiple sensors such as accelerometers,

gyroscopes and sometimes magnetometers. There are GPS devices equipped with

an IMU so when there is no GPS satellite in sight it can help to determine the current

position. With the known of the initial position the navigation system can calculate

the current position by receiving only the acceleration vectors and the rotational

attributes from the IMU. The measurement unit usually fuses multiple sensor data

to get a more accurate measurement. It is important to mention that an only IMU

25

2 Basic notions

based navigation system suffer from accumulated error and it is necessary to correct

the reference or initial position periodically.

The process is also known as dead reckoning [3] which means that the device

determines its current position by using a previous calculated position. An example

for this is the early marine navigation method when a fix position is known, and

from the recorded speed, means of heading and the elapsed time the current position

of the ship is calculated. These positions are calculated at predetermined intervals

between two fix position measurement.

kepet

2 Basic notions

In the last section the reader was introduced to the basic concepts: what we want

to do, what is a quadrocopter, how it can be controlled and some words mentioned

about the Android platform and its sensors. In this section more advanced mathe-

matical tools will be described. These tools are used to preprocess sensor data and

control the device's nonlinear dynamic model.

2.1 Kalman filter

Kalman filter [16] is an recursive algorithm, which provide optimal estimation of

the state of the system, based on noisy input measurements. The algorithm has two

phases, Predict and Update:

1. Predict step Based on the last state and the dynamic model of the system,

Kalman filter estimates the current state and calculates the uncertainties. Un-

certainties represented by covariance matrices.

2. Update step The current prediction will be adjusted on the basis of the ob-

servation. This update takes into account the uncertainties, calculated in the

previous step.

The filter assumes that the system is a linear dynamic system, and all error variables

have normal distribution.

26

2 Basic notions

The formal description of the filter consist of the steps presented below:

1. Predict the next state using the model F and the previous state

xk|k+1 = Fkxk−1|k−1 +Bk−1uk−1

2. Calculate P which represents the prediction error(estimate covariance). Q is

the model covariance matrix.

Pk|k+1 = FkPk−1|k−1FTk +Qk

3. Calculate the difference between the prediction xk|k+1 and the measurement

zk. H matrix maps the current state to the measurement

yk = zk −Hkxk|k+1

4. Calculate S which represents the measurement error(innovation covariance).

R is the sensor inaccurateness covariance matrix

Sk = HkPk|k−1HTk +Rk

5. Calculate Kalman gain which depends on the rate of the model and measure-

ment error

Kk = Pk|k−1HTk S

−1k

6. Combine the measurement and the model prediction

xk|k = xk|k−1 +Kkyk

7. Update the estimate covariance

Pk|k = (I −KkHk)Pk|k−1

There are several algorithms which can be used to fuse multiple sensor data, includ-

ing Kalman filter, Bayesian networks and Dempster–Shafer theory. Because of its

simple development and tuning in this thesis I will use the Kalman filter to merge

multiple sensor data. Different covariances will be assigned to different sensors and

the inner state of the filter contains an estimation of the sensor drift. Drift usually

occurs at accelerometer sensor during integration and accumulation of errors.

27

2 Basic notions

2.2 Linear quadratic servo [15]

The linear quadratic regulator (LQR) is a widely applied robust control technique.

LQRs aim is to keep output signals at prescribed values, as close as possible, while

there are some amount of disturbances in the system. After linearization, nonlinear

systems are often controlled by LQR. The problem statement is as follows:

Given a multiple input multiple output linear time invariant(LTI) system represen-

tation in state-space form:

x(t) = Ax(t) +Bu(t), x(0) = x0

y(t) = Cx(t),

and we want to find aK feedback matrix, that minimizes the J(x, u) functional with

a given positive semi-definite Q and a positive definite R weighting matrix:

J =∫∞0

xTQx+ uTRu dt.

This can be done by solving the Control Algebraic Ricatti Equation(CARE) for x:

ATx+ xA− xBR−1BTx+Q = 0.

After we got x from the equation, above, we can get theK matrix doing the follow-

ing calculation:

K = R−1BTx.

If x is a vector of the state variables, the controlling input u of the system will be

u = −Kx

2.3 Quasi-polynomial and Lotka-Voltera systems [10]

Quasi-polynomial (QP) systems becoming a major technique to model a nonlinear

dynamical system. There is a wide class of nonlinear systems which can be trans-

formed into QP form. The local and global stability analysis of QP systems have

been investigated recently. In thesis [10] with the numerical tools of QP systems,

stabilizing feedback control design and its numerical aspects are investigated. In

this thesis such a feedback control will be designed for the nonlinear model of the

28

2 Basic notions

quadrocopter. Quasi-polynomial models are systems of ordinary differential equa-

tions of the following form:

yi = yi

(Li +

m∑j=1

Aij

n∏k=1

yBjkk

), i = 1, ..., n,

where y ∈ int(Rn+), A ∈ Rn×m, B ∈ Rm×n, Li ∈ R, i = 1, ..., n. QP models

are splitted into classes of equivalence according to the products: M = B · A and

N = B ·L. Lotka-Voltera form is a representation of these equivalence classes and

written in form

zj = zj

(Nj +

m∑i=1

Mijzi

), j = 1, ...,m,

and zj represent the quasi-monomials:

zj =n∏

k=1

yBjkk , j = 1, ...,m.

2.4 Embedding process systems into QP form

A wide class of nonlinear systems can be embedded into QP-form, if the satisfy the

following requirements:

1. The system of nonlinear ODEs should be in the form:

xs =∑

is1,...,isn,js

ais1...isnjsxis11 ...xisn

n f(x)js , s = 1, ..., n,

where f(x) is a scalar valued function, which is not reducible to quasimono-

mial form containing terms is the form of∏n

k=1 xΓj,kk , j = 1, ...,m, with Γ

which is a real matrix.

2. We require that the partial derivatives of the model fulfills:

∂f

∂xs

=∑

es1,...,esn,es

bes1...esnesxes11 ...xesn

n f(x)es

We introduce new variables instead of the non quasi-monomial functions.

µ = f q

n∏s=1

xpss

29

3 System description

Then instead of the original equation we write into QP form:

xs =

(xs

∑is1,...,isn,js

(ais1...isnjsµ

js/q

n∏k=1

xisk−δsk−jsPk/qk

)), s = 1, ..., n

where δsk = 1 if s = k and 0 otherwise. In addition, a new quasi-polynomial

equation set appears for the new variable µ.

µ = µ

(n∑

s=1

(psx

−1s xs+

∑isα,js,eaα,es

aisα,js , besα,esqµ(es+js−1)/q×

n∏k=1

xisk+esk+(1−es−js)pk/qk

))

Parameters ps and q can be chosen arbitrary, so the embedding is not unique. The

simplest way to embed a system is to choose ps = 0, s = 1, ..n, q = 1.


After discussing the basic concepts, in this section the created hardware and struc-

ture of the software, written on Android, will be presented. The hardware itself is

the quadrocopter model, and the software intended to filter sensors and control the

device.

3.1 Hardware

The hardware consist of a prefabricated wooden frame and a Lithium Polymer(LiPo)

battery, an Android smartphone, an Arduino Mega ADK [14], 4 ESCs(Electronic

Speed Controllers) and 4 motors mounted on it.

Arduino is an open-source microcontroller platform which consist of a simple

circuit and an integrated development environment (IDE), used to write the software

running on themicrocontroller. Arduino has several input and output ports, and there

are many extension panels available with sensors and various interfaces and these

extension panels can be easily connected to the IO ports. The Arduino programming

language, based on Wiring, is similar to the C programming language. On Figure 5

there is Arduino Mega ADK connected to an Android phone via USB cable.

The concept is simple: smartphone(Samsung i9250) senses the current state

through its sensors and sends the control signals to the Arduino board. The two

30


Figure 5: Arduino and a smartphone connected with an USB cable

device connected with an USB cable, such a way that the Arduino behaves as USB

host, and the phone is the USB slave device. The Arduino board translates angular

speed commands to PWM signals and sends them to the 4 ESCs. ESCs then control

the 4 brushless motor's three phase voltage input. Figure 6 shows the quadrocopter

with an additional RPM counter installed on one of the propellers.

31


Figure 6: Prefabricated quadrocopter model with the Arduino board under it. The

LEGO application on the rear left rotor is an RPM counter unit.

Arduino board natively supports USB communication, but the smartphone could

communicate over Bluetooth, WiFi and NFC(near field communication). The easi-

est way to build the communication channel is to connect them together with an USB

cable. There is a bluetooth and WiFi extension available on Arduino platform but

the Bluetooth has big latency and the WiFi communication is harder to implement.

USB communication is basically a serial port communication.

3.2 Software

Main decision logic and signal processing runs on the smartphone, and the rotor

control functionality runs on the Arduino board. Since Android applications can be

developed using Java [18], and Java is an object oriented language, it is reasonable to

organize our software into classes. On Figure 7 a block diagram shows the connected

objects. First of all, SensorEventListener collects the necessary sensor events,

and pushes to FilterManager. FilterManager decides what to do with a sensor

data, does simple calculations and synchronizes them and sets the filters. Filters

32

4 Processing sensor data..

Sensor

.

Event

.

Listener

...

accelerometer

.

gyroscope

.

magnetometer

.

GPS

.

FilterManager

.

roll filter

.

pitch filter

.

position filter

.

State

.

Controller

.....

heading filter

..

Tilt compensated compass, rotate linear acceleration

Figure 7: Software components - block diagram of the sensor data processor

remove noise from signals, and update the inner state object. State object records

the current state, and any other object which needs the current state just requests

from it. For example: FilterManager class receives the current magnetometer

data, and asks the current tilt from the State to compensate the measurement.

Controllers use the current state to control the position and the attitude. There are

two cascaded controller: one is the higher level position controller, which "desired"

data comes from outside (from a remote controller). Its output is an input of another

controller which controls the tilt.

The second controller puts its output into the USB data stream, and Arduino

board executes the control signal, produces PWM signals for ESCs.

The remote controller is an external software module, which communicates the

controller module via network sockets. The remote controller can run both on stan-

dard desktop computer or on another smartphone.

4 Processing sensor data

After the structure of the software and the basic processes were discussed, the pro-

cessing of sensor signals will be discussed. Sensor processing includes signal fil-

tering, smoothing, noise reducing and fusing. Firstly, the real-time of the Android

platform will be investigated, and then the tilt and position filtering methods will be

discussed. In this section, two tools will be presented which provide helping hand

for debugging and filter parameter tuning.

33

4 Processing sensor data..

State

.

Position controller

.

Remote controller

.

Tilt controller

.

Position x, y, z

.

Tilt x, y, z

.

Desired position x, y, z

.

Desired tilt x, y, z

.

Desired rotor thrust

.

Rotor controller

.

PWM

..

ARDUINO

.

Andorid

...

Figure 8: Software components - block diagram of the controller module

4.1 Using Android smartphone as an IMU

We can say that IMU is accurate for the short term, GPS is accurate for the long term.

This statement is also true when we speak about different sensors. The tilt of an

object can be calculated from gravitational forces measured with an accelerometer.

Accelerometer is accurate in long term, but it has slow response and usually sensitive

for noise disturbances. On the other hand we can also calculate the tilt of the device

using gyroscope measurements, but it has an accumulated error, so it is inaccurate

in long term. Gyroscope measures angular velocities; by integrate it we can get

the current rotation angle. Fusing the two sensors we can measure the current tilt

precisely.

To control the quadrocopter attitude the controller logic will need an accurate tilt

measurement. Similarly, to control the position, an accurate position measurement

needed.

34


4.1.1 Android real-time capability

Despite the fact that Android OS is not a real-time OS we are planning to control

a flying vehicle with an application running on it. According to [6] some measure-

ments taken to check how well can Android OS meet the deadlines under various

circumstances, that is how reliable the OS when an application continuously needs

to act instantly. Since that publication, mobile hardwares became stronger, as well,

the Android OS kernel changed much. As the reader will see, the quadrocopter

controller Android application usually operates with 10ms length timers and event

update intervals so in the measurements this range will be tested.

The test application schedules a measurement 1000 times at 10ms intervals. A

measurement schedule another task once at 10ms interval which calculates the dif-

ference between the schedule time and the execution time. Subtracting the 10ms

from the actual differences one can get the slippages or errors. This number reflects

the real-time behavior of the OS.

.....

0

.

100

.

200

.

300

.

400

.

500

.

600

.

700

.

800

.

900

.

1,000

.

1,100

.0

.

2

.

4

.

6

.

Samples

.

Slippage

[ms]

Figure 9: Slippage times under no load

Test result can be seen on Figure 9. Result shows that errors are most of the time

less than or equal to 1ms. However there are a few occurrences, when this time is

significantly higher. These belated events will be dropped.

We can consider the system is under a medium load when we take sensor data

and do some dummy floating point calculations on them every 10ms like we will

do when we need to filter sensors and calculate output signals. If we perform the

measurements when the system is under medium load then we get the data series on

35


Figure 10.

.....

0

.

100

.

200

.

300

.

400

.

500

.

600

.

700

.

800

.

900

.

1,000

.0

.

2

.

4

.

6

.

Samples

.

Slippage

[ms]

Figure 10: Slippage times under medium load

The result shows that ± 1ms slippage times became mode frequent and there

are more of the larger delays. At Section 4.1.3 a method will be described, that

synchronizes data samples and events to each other and processes them at once.

4.1.2 Accessing sensors

In Android API, sensors are managed by SensorManager class. After instantiate

this class we must listen for sensor data changes. First we need to specify a sensor,

which we are interested in and than we specify the listener which will get the data.

mySensorManager = (SensorManager) getSystemService(Context.SENSOR_SERVICE);List<Sensor> mySensors = mySensorManager

.getSensorList(Sensor.TYPE_ACCELEROMETER);

if (mySensors.size() > 0) mySensorManager.registerListener(mySensorEventListener,

mySensors.get(0), SensorManager.SENSOR_DELAY_UI);

Code 1: Using SensorManager class

It is possible to set the desired delay of the sensor event update. The actual up-

date interval of samples can be higher or lower. Predefined constants help our work,

and Table 4.1.2 shows the constants and corresponding average sensor event update

delays. Delay times averaged using 100 measurements on a Samsung i9250 device.

36


Constant Average delay(ms) Min. delay(ms) Max. delay(ms)

SENSOR_DELAY_NORMAL 200 197 211

SENSOR_DELAY_UI 70 51 85

SENSOR_DELAY_GAME 20 6 53

SENSOR_DELAY_FASTEST 10 2 20

My adaptive Kalman filter implementation, described in the next section, uses an

N size window to calculate the signal variance thus it can adjust the Q matrix every

sensor update. A single cycle of the filter takes an average of 2.8ms to calculate the

next filtered value if N set to 200. If adaptive feature is not active it can be even

faster so we can use the fastest sensor update mode.

To access sensor data, the SensorEventListener interface must be imple-

mented to get the sensor events. You can see an example in the next section.

4.1.3 Sample collection

The class which implements the SensorEventListener interface gets the sensor

event but, as you can see, events coming from different sensors are not synchro-

nized. This is because sample delays may differ from the desired delay, and multiple

sensors emit events independently. My filter usually fuses two sensors simultane-

ously, thus to solve this problem, one of the signals must be extrapolated (or the

other interpolated). The SensorEventListener class just forward the event to the

FilterManager which waits for necessary signals, interpolates the slower one, and

sends the pair of events to the filter.

The sensorEventListener catches the sensor event and passes to the FilterManager:

private SensorEventListener sensorEventListener = new SensorEventListener() public void onSensorChanged(SensorEvent event)

filterManager.addSensorEvent(event.sensor.getType(), event.values);

Code 2: SensorEventListener implementation

The FilterManager class interpolates the signal which arrived later than the

other. After that, it sends the two measurements and the elapsed time from the last

cycle to the filter. On Figure 11 one can see, the measurements and corresponding

37


calculations. "X"s represent accelerometer and gyroscope measurements, "+"s rep-

resent the calculated data. X axis shows the time in ms and y axis shows the value.

For example a gyroscope reading received at 800ms and an accelerometer event re-

ceived at 803ms. The method interpolates the accelerometer data thus it will higher

than the current value, and the dt will be 4ms (back shifted to 800ms). Because of

the interpolation, if the two sensors are delayed, the actual processed data will be

delayed.

Figure 11: Measured delays: Measurements ("X") were not taken in the same time

Corresponding code which calculates the measurements and pass to the filter:

if(gyro.size() == accelerometer.size()) //new accelerometer measurement receivedPair<Long, Float> gyrom = gyro.getLast(), accm = accelerometer.getLast();if(gyrom.first != accm.first) //if the last measurement timestamps are not the same

long dt = gyrom.first − lastUpdate; //gyro was faster so shift back to that time//get last but one accelerometer elementPair<Long, Float> accm0 = accelerometer.get(accelerometer.size()−2);//interpolation a0y + (gx−a0x) * (ax−a0x)/(ay−a0y)float accValue = accm0.second+

((accm.second − accm0.second)/(float)(accm.first − accm0.first))*(float)(gyrom.first − accm0.first);

//send to the filterfilterMeasurements((double)dt/(double)1000, accValue, gyrom.second);lastUpdate = gyrom.first;

Code 3: FilterManager adjusts samples

38


I choose the interpolation method instead of extrapolation because extrapolation

generates more errors.

4.2 Rotation in 3D

In the previous sections the typical slippage numbers were measured and a method

described to compensate these latencies. It is showed that Android API provides a

convenient way to accessing sensor data. Now we can assume that sensors are ready

for processing. In the next few sections these sensors are filtered, and the noise will

be reduced. At the same time tilt and altitude will be calculated.

4.2.1 Tilt

There are two choices for an accurate tilt calculation: using Android derived gravity

sensor directly to calculate rotational angles or using a sensor fusion technique to

filter accelerometer and gyroscope sensors and calculate the current rotation. Built

in gravity sensor uses an adaptive filter which is not adapt as fast as my solution

which parameters were tuned by taking the actual noise into account. From the

gravitational forces along x, y and z axes, we can calculate the roll and pitch angles

using equations:

rollAngle = arctan(−Gx√G2

y +G2z

) (1)

pitchAngle = arctan(−Gy√G2

x +G2z

) (2)

4.2.2 Sensor fusion

There are several ways to fuse multiple sensor data, one of them is applying Kalman

filter. Inertial Navigation Systems usually use this technique to calculate an accu-

rate data. Maybeck [5] showed many different configurations for different fusion

problems.

Two Kalman filter created to fuse accelerometer and gyroscope data both on x,

y axes. The filter configuration described below. Figure 12 shows the roll angle

processing using Kalman filter.

39


...

Kalman

.

filter

.

Calculate roll(rad) :=

.

atan(Gx/sqrt(Gy2 +Gz2))

..

Raw accelerometer data(m/s2)

.

Raw gyroscope data (rad/s)

...

Convert to degrees:

.

(roll * 180)/pi

...

y-angle(rad)

.

Figure 12: Using Kalman filter to fuse accelerometer and gyroscope

1. The filter input vector consist of the angle (α) in rad and the angular velocity

(ω) in rad/s.

measurement =

αω

(3)

2. The state vector contains the rotation angle (α), the angular velocity (ω) and

the gyroscope drift (b).

state =

α

ω

b

(4)

3. The process model F is using the angular velocity, the previous state and the

elapsed time to predict the next state and its errors.

F =

1 ∆t −∆t

0 1 0

0 0 1

(5)

4. H matrix maps the current angle, and the angular velocity from the state to

the measurement

H =

1 0 0

0 1 0

(6)

5. Ourmodel based on gyroscopemeasurements, andwe trust it in the short term.

The values of Q and R are chosen to satisfy this condition

Q =

0.8 0 0

0 2 0

0 0 1

(7)

R =

200 0

0 1

(8)

40


Kalman filter implementation On Android, KalmanFilter class implements

the logic. Kalman filter class communicates with ProcessModel class which con-

tains process specific information like F , Q and R matrices. If it is necessary to

apply a new filter on different sensors, in the future, one must only implement the

ProcessModel interface with the sensor specific data.

The next code snippet shows that KalmanFilter request predictions, and nec-

essary data-structures from the ProcessModel. The filter method called if a new

measurement received.

public Matrix filter(Matrix measured) // estimate next state from modelMatrix estimatedState = model.predictState(state); // x// estimate quality of model predictionMatrix modelErrorCovariance = model.predictError(stateErrorCovariance); // P// calculate the measurement and estimated value difference.Matrix estimateDifference = measured.sub(model

.mapStateToMeasurementMatrix().multiply(estimatedState));// set difference to the model to keep a window of the covariencemodel.setNewDiffernce(estimateDifference);// calculate the covariance matrix of measurmentMatrix measurementCovariance = model

.measurementCovariance(modelErrorCovariance);// calculates Kalman gain: divides the model error cov. with theMatrix kalmanGain = model.kalmanGain(modelErrorCovariance,

measurementCovariance);// update statestate = estimatedState.add(kalmanGain.multiply(estimateDifference));// update state covariance matrixstateErrorCovariance = new Matrix(new double[][] ).eye(

kalmanGain.rowCount(), kalmanGain.rowCount()).sub(kalmanGain.multiply(model.mapStateToMeasurementMatrix()));

return state;

Code 4: Kalman filter

A model should implement the ProcessModel abstract class. The implementation

of this class describes the behavior of the model:

public abstract class ProcessModel

//Method for model initialization Kalman filter calls it firstlypublic abstract void init();

//Estimates and returns the next data.public abstract Matrix predictState(Matrix state, Matrix measured);

//Update covariance matricespublic abstract Matrix predictError(Matrix estimationErrorCovariance);

//Takes the difference of the measurement and the prediction.public abstract Matrix measurementAndModelDifference(Matrix measurement, Matrix model);

//Multiplies the state with the H matrix and returns it.public abstract Matrix mapStateToMeasurementMatrix();

//Update covariance matricespublic abstract Matrix measurementCovariance(Matrix modelErrorCovariance);

//Calculates the Kalman Gainpublic abstract Matrix kalmanGain(Matrix modelErrorCovariance, Matrix measurementErrorCovariance);

41


Code 5: Process model

4.2.3 Tuning filter parameters

GUI-rich client applications can be created with JavaFX [11], thus I can use directly

the implemented KalmanFilter class in a desktop environment for testing a filter

performance when its input parameters changes. A simple application, which uses

this class, had been created for Kalman filter parameter tuning. There are sliders for

each Q and R value, and if a slider value changed, a plot shows the result instantly.

Figure 13 shows a noisy accelerometer input (red), and the filtered output (blue).

The application reads previously recorded input signal from a .csv file, and filters

each sample.

Figure 13: Tuning Kalman filter parameters with the tool, written in JavaFX

42


4.2.4 Real-time analysis tool

A simple tool was written in Qt, which is used to receive and view the sensor mea-

surements and filtered data. The phone sent the data via UDP socket and the Qt

desktop application received the data packet and viewed it.

The java client sends the data in a string in form of "key:value" pairs(separated

from UI thread):

public void sendMessage(final String msg)

Thread thread = new Thread(new Runnable() public void run()

DatagramPacket p = new DatagramPacket(msg.getBytes(), msg.length(), local,server_port);

try

s.send(p);catch(Exception e)

e.printStackTrace();

);thread.start();

Code 6: UDP Sender

The Qt desktop application receives such a packet using QUdpSocket class:

Receiver::Receiver()

udpSocket = new QUdpSocket(this);udpSocket−>bind(45454, QUdpSocket::ShareAddress);

connect(udpSocket, SIGNAL(readyRead()),this, SLOT(processPendingDatagrams()));

void Receiver::processPendingDatagrams()

while (udpSocket−>hasPendingDatagrams()) QByteArray datagram;datagram.resize(udpSocket−>pendingDatagramSize());udpSocket−>readDatagram(datagram.data(), datagram.size());emit message(datagram.data());

Code 7: UDP Receiver

On Figure 14 the tool shows servo output signals.

4.2.5 Results

To test the performance of the filter, I installed the smartphone to the quadrocopter

frame. The first test orientated to see the noise generated by the four propellers

43


Figure 14: Real-time analyzer, written in Qt, draws a graph from sensor data

when the device is on the ground, but rotors have such angular velocities that the

quadrocopter nearly takes off. On Figure 15 we can see the roll (in degrees) calcu-

lated from the raw accelerometer data (blue), the gyroscope only (red) and the filter

output (green).

.....

0

.

50

.

100

.

150

.

200

.

250

.

300

.

350

.

400

.

450

.−20

.

0

.

20

.

40

.

Time [10ms]

.

Roll[degrees]

Figure 15: The raw and fused data showed, while the quadrocopter was in the ground

with rotation propellers

Notice the red line which is constantly moving (drifting) upwards, that is came

from the gyroscope accumulated error. The result shows that the noise is success-

fully eliminated, and gyroscope drift error compensated.

44


Figure 16 shows it closely. There are two lines on the graph: gyroscope only

(blue) and filter output (green). Filter output starting from 0 and converges close to

.....

0

.

50

.

100

.

150

.

200

.

250

.

300

.

350

.

400

.

450

.0 .

1

.

2

.

3

.

4

.

Time [10ms]

.

Roll[degrees]

Figure 16: Filtered data has 1 degrees of error, while propellers were at medium

speed

the mean of the raw data. Initially the state of the Kalman filter is a zero vector.

4.2.6 Heading

A hover motion can be achieved if the controller controls the current roll and the

pitch angle of the quadrocopter. To control its position, ie. navigating from the cur-

rent position to the destination, the controller uses the current heading, to calculate

the desired direction. Controller can calculate the current heading using the magne-

tometer sensor. Magnetometer measures Earth gravitational forces on X, Y and Z

axes. With the knowledge of the gravitational force on X axis (Mx) and on Y axis

(My) one can calculate the heading in radians using the following equation:

heading = atan2(Mx

My

) (9)

Note that X and Y axes are reversed on android platform. atan2 function produces

results in range of (−π, π]. If we add 2π to the negative values then the range will

be (0, 2π].

With this equation the heading can be easily calculated however when the sensor

tilted the result will be inaccurate. Wemust compensate themeasured forces with the

current tilt. This can be achieved bymultiplying the 3-axis measurement vector with

45


the inverse of the rotation matrix. By applying this step we can get the compensated

Mx and My values and then using Equation (9) we can get the heading. Figure

17 shows the current tilt (red) and the uncompensated (blue) and the compensated

(green) tilt.

.....

−100

.

0

.

100

.

200

.

300

.

400

.

500

.

600

.

700

.

800

.

900

.

1000

.

1100

.

1200

.0

.

100

.

200

.

Time [10ms]

.

YawandRoll[degrees]

Figure 17: Compass tilt compensation

External electromagnetic effects and metal objects are influencing the magni-

tude of the error and the distorsion of the sensor readings. To overcome this source

of inaccuracies a Kalman filter applied to fuse magnetometer heading with Z-axis

gyroscope measurements. On Figure 18 there is an already compensated heading

output (blue), and the fused data with color green. One can see that the filter suc-

cessfully eliminates even the popcorn like noises.

.....

0

.

100

.

200

.

300

.

400

.

500

.

600

.

700

.140

.

150

.

160

.

170

.

Time [10ms]

.

Yaw[degrees]

Figure 18: Magnetometer fusion with gyroscope

Using the two techniques together, mentioned above, a relatively accurate head-

ing information can be produced. Unfortunately, sometimes there are some unex-

46


pected disturbances in the magnetometer data. This effect can be eliminated by us-

ing another magnetometer in a parallel way, of ensure that there are no larger metal

objects near the device.

4.3 Position

Integrating the linear acceleration twice we can calculate the actual (x, y, z) position

of the device if we know the starting point. Of course, due to the double integral,

the smallest offset in accelerometer data can cause quadratic error in position. To

minimize this offset, there is a 5 seconds length calibration process, while the device

is at rest and the program calculates the current offset on each axis. Note that the

gravitational force is an offset on Z-axis. We can get the linear acceleration by

subtracting these offsets from all axes.

No matter how well we estimate the offset, there will always be a bias in the

position, calculated from accelerometer only. To get rid of the bias we can combine

the calculated position with other sensor data. Fortunately there is a pressure sensor

in the smartphone, so we can get a relatively accurate Z-axis position(the altitude).

The knowledge of the current tilt and altitude is enough to keep the quadrocopter

hover motion.

If we want to know the exact x, y position, we can combine the tilt compensated

linear acceleration data with GPS signal if the device is used outside. However if the

device is used indoors, other positioning techniques should considered, like RSSI

based positioning[7] or using infrared/ultrasound sensors.

4.3.1 Altitude filtering

The same Kalman-filter, as the reader has seen in Section 4.2.2, applied here but the

model and state matrices are slightly different.

1. The filter input vector(the measurement), consist of the altitude (sv) in me-

ters, calculated from the pressure data, and the vertical acceleration av in ms2,

47


calculated from the tilt compensated accelerometer data.

measurement =

svav

(10)

2. The state vector contains the altitude (sv), the vertical speed (vv), the acceler-

ation (av), and the accelerometer drift (d).

state =

sv

vv

av

d

(11)

3. The model of the filter predicts its next state and the accelerometer drift (d).

F =

1 ∆t 1

2∆t2 −∆t

0 1 ∆t 0

0 0 1 0

0 0 0 1

(12)

4. Weighting (ie. covariance matrices) of measurements are:

Q =

1 0 0 0

0 1 0 0

0 0 1 0

0 0 0 1

(13)

R =

200 0

0 1

(14)

4.3.2 Altitude results

Figure 19 shows the raw altitude(red), and the filtered data(green). Note that the

sampling rate of the accelerometer sensor is ten times that of the pressure sensor.

The original purpose of the pressure sensor is to give better accuracy for the

GPS localization. The result shows that, if the device not moving on axis z, then

the filtered data fluctuate ± 20 cm around the actual position. It follows that, at

48


.....

0

.

100

.

200

.

300

.

400

.

500

.

600

.

700

.

800

.325

.

326

.

327

.

Time [10ms]

.

Altitude

[meters]

Figure 19: Altitude filtering result shows that the accurateness is around 20 cm

this precision the device can be used better in larger spaces. One can increase the

accuracy of the altitude measurements using an external ultrasonic distance sensor,

attached to the bottom of the quadrotor.

4.4 Inner state

Inner state representation stores the filtered data, and calculates more information

from it. We must represent the current rotation in an effective way which causing

less calculation overhead in the future.

4.4.1 Rotation matrix representation

Auto pilot keeps the current tilt, roll and heading as an inner state. Any module can

get the rotation matrix by calling inner state's getXRotationMatrix where X can

be Roll Pitch and Yaw. Inner state constructs the matrix dynamically. The roll (Ry)

and the pitch (Rx) rotation matrix for the quadrocopter frame is:

Ry =

cos(θ) 0 −sin(θ)

0 1 0

sin(θ) 0 cos(θ)

(15)

Rx =

1 0 0

0 cos(ϕ) sin(ϕ)

0 −sin(ϕ) cos(ϕ)

(16)

49


Sometimes it is necessary to do calculation with the inverse of these matrices,

therefore there is a function which constructs directly the inverse of these matrices.

The function uses the fact that the inverse of a rotation matrix is its transpose.

The following example shows how to compensate the compass sensor input val-

ues against the tilt, using rotation matrices:

//getter for Roll rotation matrix inverseMatrix getInvertedRollMatrix()

return new Matrix(new double[][] new double[] Math.cos(_roll), 0, Math.sin(_roll) ,new double[] 0, 1, 0 ,new double[] −Math.sin(_roll), 0, Math.cos(_roll) );

//getter for Pitch rotation matrix inverseMatrix getInvertedPitchMatrix()

return new Matrix(new double[][] new double[] 1, 0, 0 ,new double[] 0, Math.cos(_pitch), −Math.sin(_pitch) ,new double[] 0, Math.sin(_pitch), Math.cos(_pitch) );

//Compensate compass sensor values against the current tilt:Matrix rollCompensated = state.getInvertedRollMatrix().multiply(compassVector);Matrix fullCompensated = state.getInvertedPitchMatrix().multiply(rollCompensated);

Code 8: Rotate back the measurement vector

4.4.2 Quaternion representation

There is a tilt compensation calculation every 10 milliseconds, therefore there are

around 200 matrix multiplications in every second. Quaternions are commonly used

to represent 3D rotations. In this section I will examine the effectiveness of the

quaternion which applied in this quadrocopter controller.

Quaternions can be tough as an extension of complex numbers and can be rep-

resented and stored in a four-dimensional vector. [8] Equations (17) and (18) show

the concept:

q = a+ bi+ cj + dk, (17)

q =[a b c d

], (18)

where a, b, c, d are real numbers. The scalar(real) part of the number is stored in the

vector at index 0, and the vector(imaginary) part stored at index 1, 2 and 3.

3D rotations can be represented by quaternions and the p = (0, p) point after the

50

5 Control

rotation can be expressed by the formula:

p′ = qpq−1, (19)

where q is a unit quaternion.

Concatenating (chaining) rotations with quaternions costs less calculations then

using rotation matrices, but calculating a single rotation requires more. Unfortu-

nately this application requires that the current rotation must be calculated every

time instant. It is preferable to use rotation matrices instead of quaternions in this

application.

5 Control

One of the aims of the thesis is to implement a controller which is able to keep the

quadrocopter up in the air in hover motion. The controller controls both the tilt and

the altitude. In this section separated tilt and altitude controller realizes the function.

5.1 Nonlinear state-space model

The nonlinear state space model of a quadrocopter is written in a form [9]:

x = f(x) + g(x)U, (20)

where the state variables are:

x = (x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12) = (Φ, Φ,Θ, Θ,Ψ, Ψ, z, z, x, x, y, y),

(21)

51

5 Control

the functions are:

f(x) =

x2

a1x4x6 + a2Ωrx4

x4

a3x2x6 + a4Ωrx2

x6

a5x4x2

x8

−g

x10

0

x12

0

g(x) =

0 0 0 0

0 b1 0 0

0 0 0 0

0 0 b2 0

0 0 0 0

0 0 0 b3

0 0 0 0

ut1m

0 0 0

0 0 0 0

ux1m

0 0 0

0 0 0 0

uy1m

0 0 0

, (22)

and the inputs are:

U = (U1U2U3U4). (23)

`The propeller speeds can be easily calculated from input variables:

Ω2

1

Ω22

Ω23

Ω24

=

0.25 0 0.5 0.25

0.25 −0.5 0 0.25

0.25 0 −0.5 −0.25

0.25 0.5 0 0.25

·

U1/b

U2/b

U3/b

U4/b

. (24)

The necessary quadrocopter-specific parameters for the model can be seen in the

table below. The same Ixx, Iyy, Izz and Jr are used as int the thesis [9]. b parameter

estimated using the physical model: only b is unknown when the quadrocopter is

starting to lift up in the air, so the calculation is easy. The others are measured values.

The reader can see the parameters descriptions and unit in the List of Symbols, at

page 18.

52

5 Control

Parameter Value

Ixx 5 · 10−3

Iyy 5 · 10−3

Izz 4 · 10−3

Jr 5 · 10−3

l 0.26

b 3.13 · 10−5

d 7.5 · 10−7

m 0.8

Other derived parameters can be calculated from these parameters:

a1 =Iyy−Izz

Ixxb1 =

lIxx

a2 =JrIxx

b2 =l

Iyy

a3 =Izz−Ixx

Iyyb3 =

lIzz

a4 =JrIyy

a5 =Ixx−Iyy

Izz

(25)

ut = cos(Φ)cos(Θ)

ux = cos(Φ)sin(Θ)cos(Ψ) + sin(Φ)sin(Ψ)

uy = cos(Φ)sin(Θ)cos(Ψ)− sin(Φ)cos(Ψ)

(26)

5.2 Tilt stabilization

An LQ-servo designed to keep the roll, pitch and yaw angles at reference values. To

design such a controller, we need to linearize the non-linear model around at least

one point.

5.2.1 Linearization

We linearize the system around Φ = 0,Θ = 0 angles. The resulting system will be

stable in the narrow range around zero radians.

The linear model of the quadrocopter is

A =∂f

∂x|x = x∗

B = g(x∗),(27)

53

5 Control

and the variables arex∗1 2 3 4 5 6 8 10 12 = 0

x∗7 9 11.

(28)

The resulting state-space matrices are:

A =

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

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

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

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

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

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

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

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

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

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

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

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

, B =

0 0 0 0

0 b1 0 0

0 0 0 1

0 0 b2 0

0 0 0 0

0 0 0 b3

0 0 0 0

1m

0 0 0

0 0 0 0

0 0 0 0

0 0 0 0

0 0 0 0

(29)

5.2.2 Continuous time LQ-servo design

The tilt stabilization only cares about the angle dynamics, so the system which con-

tains the angle related differential equation is:

A =

0 1 0 0 0 0

0 0 0 0 0 0

0 0 0 1 0 0

0 0 0 0 0 0

0 0 0 0 0 1

0 0 0 0 0 0

, B =

0 0 0

b1 0 0

0 0 0

0 b2 0

0 0 0

0 0 b3

, C =

1 0 0 0 0 0

0 0 1 0 0 0

0 0 0 0 1 0

(30)

To design a servo controller which adjusts to a time dependent reference value, we

need to extend the system in the following manner:

A =

A 0

−C 0

, B =

B0

. (31)

54

5 Control

A =

0 1 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 1 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 0 0

−1 0 0 0 0 0 0 0 0

0 0 −1 0 0 0 0 0 0

0 0 0 0 −1 0 0 0 0

, B =

0 0 0

b1 0 0

0 0 0

0 b2 0

0 0 0

0 0 b3

0 0 0

0 0 0

0 0 0

. (32)

The K feedback matrix can be calculated by solving the Control Algebraic Ricatti

equation (CARE):

ATx+ xA− xBR−1BTx+Q = 0 (33)

Choosing matrices Q and R we can adjust the performance of the controller. If Q

and R matrices are

Q = 0.001 ·

20 0 0 0 0 0 0 0 0

0 20 0 0 0 0 0 0 0

0 0 20 0 0 0 0 0 0

0 0 0 20 0 0 0 0 0

0 0 0 0 20 0 0 0 0

0 0 0 0 0 20 0 0 0

0 0 0 0 0 0 200 0 0

0 0 0 0 0 0 0 200 0

0 0 0 0 0 0 0 0 200

, R =

1 0 0

0 1 0

0 0 1

,

(34)

then the result of the CARE will be:

K =

0.47 0.22 0 0 0 0 −0.447 0 0

0 0 0.47 0.22 0 0 0 −0.447 0

0 0 0 0 0.45 0.209 0 0 −0.447

(35)

We can get this result in Matlab with the built-in CARE solver. The Simulink model

directly uses theKx andKz feedbacks to stabilize the model.

55

5 Control

X = care(A, B, Q, R);

K = R^−1 * B' * X;

% the input gain matrices of the Simulink modelKx = K(1:3, 1:6);Kz = K(1:3, 7:9);

Code 9: CARE solver

The non-linear model and its linear LQ regulator tested in Matlab/Simulink envi-

ronment and its result can be seen in Figure 20. The roll, pitch and yaw integrators

has initial values of Φ = 1rad, Θ = −1rad, Ψ = −0.5rad. As the reader can see

Figure 20: Angle controller result shows that angles were converged to zero from

an initial value

the controller successfully generated the necessary motor speed combinations, and

all three angle converged to zero (ie. the quadrocopter hovered). One should aim

for minimize the time while the system converge to equilibrium. In real environ-

ment the controller should be more responsive, this can be achieved by adjusting

the values in matrix Q.

56

5 Control

5.2.3 Discrete LQ-servo

First we need to discretize the extended continuous time linear model, described by

(32). The discrete time state space model of the system will be

x(k + 1) = Φx(k) + Γu(k)

y(k) = Cx(k) +Du(k),(36)

where

Φ = L−1(sI − A)−1t=T

Γ = A−1(Φ− I)B.(37)

Unfortunately, we can not use the second equation, because A matrix is singular, so

use the following trick [12] to calculate Φ and Γ matrices.

e

A B

0 0

=

M11 M12

0 I

Φ = M11

Γ = M12.

(38)

The problem can be easily solved by using Matlab built in functions. If later, some

parameter changes then we just write into the Matlab model and it calculates the

new discretized system and feedback matrices. In discrete case Q and R matrices

remains the same.

sample_time = 0.01AB_extended = [ A_ext, B_ext;

zeros(3, 12)];

% Discretize the model with sample_timesyms a s tA1 = (s * eye(12, 12) − AB_extended)^−1;A2 = ilaplace(A1, t);result = subs(A2, t, sample_time);

Phi = result(1:9, 1:9);Gamma = result(1:9, 10:12);

% Design the LQ−servo with Qd and Rd matricesXd = dare(Phi, Gamma, Qd, Rd);Fd = (Rd + Gamma' * Xd * Gamma)^−1 * Gamma' * Xd * Phi;Fdx = Fd(1:3, 1:6);Fdz = Fd(1:3, 7:9);

Code 10: Discretizing and DARE solver

57

5 Control

The resulting feedbackmatrix looks very similar to the continuous case LQ feedback

matrix:

Fd =

0.461 0.225 0 0 0 0 −0.432 0 0

0 0 0.461 0.225 0 0 0 −0.432 0

0 0 0 0 0.439 0.203 0 0 −0.429

.

5.2.4 Implementation of the LQ-servo

Fx is the feedback of the inner control loop which keeps the angles at zero, Fz is the

outer control loop feedback matrix which keeps the angles at reference values. The

inputs of the angle controller are the angles (x1, x3, x5) and the angular speeds (x2,

x4, x6). The outputs of the controller are the inputs of the system which express the

propeller speed differences (U2, U3, U4). If we only want to control the angles, we

must set U1 to a reference value manually, which express the overall speed of the

four propellers and thus we can calculate angular speeds.

Controller class notifies every timewhen the quadrocopter state class changes.

If it happens, then the Controller calculates the necessary propeller speeds and

sends the command to the actuator. Graphically the reader can see the calculation

steps on the Simulink model, on Figure 21. An additional saturation block added to

Figure 21: Matlab/Simulink angle controller implementation

the controller to avoid infinitely large responses.

Controller get the reference values from the ParameterControl class which

cooperating with the remote controller. So basically the user set the reference values

directly. The reader can read more on the remote controlling process in Section 6.

58

5 Control

The Java implementation of the controller uses the QuadrocopterState and the

ParameterControl class:

public class Controller QuadrocopterState state;StateChangeNotifier notifier = new StateChangeNotifier()

@Overridepublic void stateChanged()

rollRefDiffIntegral += (rollReference − roll) * sampleTime;pitchRefDiffIntegral += (pitchReference − pitch) * sampleTime;yawRefDiffIntegral += (yawReference − yaw) * sampleTime;

// Outer loop gains reverence differencesaMatrix refDiffs = new Matrix(new double[][]

new double[]rollRefDiffIntegral,new double[]pitchRefDiffIntegral,new double[]yawRefDiffIntegral

);Matrix outerLoop = Fz.multiply(refDiffs);

// Inner loop multiplies all the state variablesMatrix variables = new Matrix(new double[][]

new double[]roll, new double[]droll,new double[]pitch, new double[]dpitch,new double[]yaw, new double[]dyaw

);Matrix innerLoop = Fx.multiply(variables);Matrix U_234 = outerLoop.add(innerLoop);

// Convert U1, U2, U3, U4 to rad/s valuesMatrix output = new Matrix(new double[][]

new double[]U1Reference,new double[]U_234.getValue(0, 0),new double[]U_234.getValue(1, 0),new double[]U_234.getValue(2, 0)

);output = output.multiply(1.0/b);output = outputConversion.multiply(output);output = output.sqrt();

;

public Controller()

// Register a state change notifierstate = QuadrocopterState.getInstance();state.setStateChangeNotifier(notifier);

Code 11: Controller class

The controller responses tested with the real-time analysis tool before the actual

testing.

5.3 Altitude stabilization using non-linear feedback input-output

linearization

The controller built with non-linear input-output linearization is using a smaller,

separate system to control its input values. The controller block has two real inputs

and one output. The two input is U1 and ut, the final output is U1. On Figure 22 one

59

5 Control

can see that the controller integrates ¯x8 and ¯x7 and then it subtracts the reference

signal href from ¯x7.

Figure 22: Altitude controller implementation in Simulink environment

The system, which contains only the height related equations looks like as fol-

lows:x7 = x8

x8 = −g + ut1mU1

First we need to linearize the input: U1 = ut1mU1, then the system will be:

˙x7 = x8

˙x8 = −g + U1

The feedback matrix will be the same if I want to track the reference point or not,

and later I will compare the results with a non servo controller. Let ¯U1 = U1 − g, so

the resulting system will be:˙x7 = ¯x8

˙x8 =¯U1

Now, the system is linear, and we can write the state-space model:

A =

0 1

0 0

, B =

01

The system can be stabilized with aK feedback matrix, and

¯U1 = K ·

¯x7

¯x8

The U1 can be calculated with the formula:

U1 =g − ¯U1

u11m

60

5 Control

The LQR technique can be applied to the system shown above, and with

Q =

1 0

0 1

, R =[1],

matrices we can get the resulting feedback matrix:

K =[0.1 0.4583

]

5.4 Result of the controller

The initial conditions of the simulation was -1 meters on axis z and the model had

to converge to zero altitude. Figure 23 shows a little overshoot on the trajectory and

the altitude reached zero around 12 seconds.

Figure 23: LQ Altitude controller result: the system converges to the zero point

from an initial position

5.5 Altitude stabilization problem seen as quasi-polynomial con-

troller design problem

Non-linear system can be represented in quasi-polynomial(QP) form. Basically the

original z-axis-system, described above, is a nonlinear system, which can be rewrit-

61

5 Control

ten into QP form easily. Once it is in the right form, the system can be stabilized by

solving a bilinear matrix inequality (BMI).

5.5.1 Embedding altitude equations into QP form

Original z-axis related differential equations describes the current altitude with a

function of propeller angular speeds and the current tilt:

x7 = x8

x8 = −g + cos(Φ)cos(Θ) 1mU1

(39)

All elements of these equations are quasi-polynomials, except for the cos function.

We can introduce new variables for each cosine:

z1 = cos(Φ) = cos(x1),

z2 = cos(Θ) = cos(x3).

Applying the chain rule and using x1's and x3's differential equations from Equa-

tions (22), we can express the derivatives of z1 and z2:

z1 = −sin(x1) · x2,

z2 = −sin(x3) · x4.

We should introduce new variables for sin functions:

z3 = sin(x1),

z4 = sin(x3).

And its derivatives are(using x1 and z2):

z3 = z1 · x2,

z4 = z2 · x4.

62

5 Control

Then the input-affine QP form of the system will look like this:

x7 = x7(x8 · x−17 )

x8 = x8(−g · x−18 ) + x8(m

−1 · z1 · z2 · x−18 )U1

z1 = z1(−z3 · x2 · z−11 )

z2 = z2(−z4 · x4 · z−12 )

z3 = z3(z1 · x2 · z−13 )

z4 = z4(z2 · x4 · z−14 )

x2 = x2(a1x4x6x−12 + a2Ωrx4x

−12 )

x4 = x4(a3x2x6x−14 + a4Ωrx2x

−14 )

x6 = x6(a5x4x2x−16 )

We can assume that Ωr is always zero, so the quasi-monomials of the system

are: x8x−17 , x−1

8 , z1z2x−18 , z3x2z

−11 , z4x4z

−12 , z1x2z

−13 , z2x4z

−14 , x4x6x

−12 , x2x6x

−14 ,

x4x2x−16 .

5.5.2 Design the controller

The manipulable input is U1, and we would like to control the altitude x7. The

system is in equilibrium when x1, x3, x8 = 0, substituting them into (39), we can

calculate that, input U1 must equal to m · g = 7.848. Let me introduce a new input

variable, in place of U1. That is U = U1 −mg, and if U is zero, the quadrocopter

hovers in case of laboratory conditions. The feedback structure was chosen to be the

same as in Section 5.3, where we designed an LQ. Basically we made a feedback

using x7 and x8 state variables. I will do the same thing in here, so there will be only

linear term in

U = k1 · x7 + k2 · x8

The state variables can not have even exponent, because we lose information about

the vertical position according to 0 and we lose the information that the quadrocopter

is going up or down at the current time. The number of quasi-monomials increased to

11, and these are: x8x−17 , x−1

8 , z1z2x7x−18 , z1z2, z3x2z

−11 , z4x4z

−12 , z1x2z

−13 , z2x4z

−14 ,

x4x6x−12 , x2x6x

−14 , x4x2x

−16 .

63

5 Control

The closed loop system with the feedback structure is:

x7 = x7(x8x−17 )

x8 = x8(−gx−18 +m−1k1z1z2x7x

−18 +m−1k2z1z2)

z1 = z1(−z3x2z−11 )

z2 = z2(−z4x4z−12 )

z3 = z3(z1x2z−13 )

z4 = z4(z2x4z−14 )

x2 = x2(a1x4x6x−12 )

x4 = x4(a3x2x6x−14 )

x6 = x6(a5x4x2x−16 )

Matrices that describes the system can be seen below. The A 9 × 12 matrix

contains quasi-monomials constant terms, emerging in all the 9 equations.

A0 =

1 0 0 0 0 0 0 0 0 0 0

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

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

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

0 0 0 0 0 0 1 0 0 0 0

0 0 0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 0 a1 0 0

0 0 0 0 0 0 0 0 0 a3 0

0 0 0 0 0 0 0 0 0 0 a5

A1 =

0 0 0 0 0 0 0 0 0 0 0

0 0 k1m

0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

, A2 =

0 0 0 0 0 0 0 0 0 0 0

0 0 0 k2m

0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

64

5 Control

B matrix contains the individual terms in quasi-monomials and their exponents.

The variables in columns are: x7, x8, z1, z2, z3, z4, x2, x4, x6.

B =

−1 1 0 0 0 0 0 0 0

0 −1 0 0 0 0 0 0 0

1 −1 1 1 0 0 0 0 0

0 0 1 1 0 0 0 0 0

0 0 −1 0 1 0 1 0 0

0 0 0 −1 0 1 0 1 0

0 0 1 0 −1 0 1 0 0

0 0 0 1 0 −1 0 1 0

0 0 0 0 0 0 −1 1 1

0 0 0 0 0 0 1 −1 1

0 0 0 0 0 0 1 1 −1

M0 = B · A0 =

−1 −g 0 0 0 0 0 0 0 0 0

0 g 0 0 0 0 0 0 0 0 0

1 g 0 0 −1 −1 0 0 0 0 0

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

0 0 0 0 1 0 1 0 a1 0 0

0 0 0 0 0 1 0 1 0 a3 0

0 0 0 0 −1 0 −1 0 a1 0 0

0 0 0 0 0 −1 0 −1 0 a3 0

0 0 0 0 0 0 0 0 −a1 a3 a5

0 0 0 0 0 0 0 0 a1 −a3 a5

0 0 0 0 0 0 0 0 a1 a3 −a5

65

5 Control

M1 =

0 0 k1m

0 0 0 0 0 0 0 0

0 0 −k1m

0 0 0 0 0 0 0 0

0 0 −k1m

0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

,M2 =

0 0 0 k2m

0 0 0 0 0 0 0

0 0 0 −k2m

0 0 0 0 0 0 0

0 0 0 −k2m

0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0 0

Next step is to convert the problem into a form which can be solved with Tom-

Lab/PENBMI solver, see the next subsection. After this conversion the solver gave

the result.

5.5.3 Converting and solve the problem

To able to use TomLab/PENBMI solver the problem must be converted into the

form:

Gi0 +

p∑k=1

xkGik +

p∑k=1

p∑j=1

xkxjKikj ≤ 0, i = 1, ..., q

Using the expression, presented in Thesis [10], we can formulate the problem:

∑mj=1 cjM0,j +

∑mj=1

∑pl=1

∑ri=1 cjkilMil,j ≤ 0

−c1 < 0...

−cm < 0,

where cj variables are the parameters of the Ljapunov function, kil variables

are the feedback parameters. M matrices can be calculated by adding matrix X to

XT , where X matrix is an m ×m matrix, which contains only the j-th column of

correspondingM0 orM1 matrices.

66

5 Control

The following Matlab code builds up M0,j and Mil,j matrices fromM0 andM1

matrices:

% SDP initializationA = [];b_U = [];b_L = [];

c = zeros(1, m); % cost vectorSDP = [];

% Q matrices% j = 1 ... mfor j = 1:m,

M0j = [Z(:,1:(j−1)) M0(:, j:j) Z(:,j+1:m)];SDP(1).Qj = M0j + transpose(M0j);

end

SDP(1).Qidx = reshape(1:m, m, 1); % 10 height column vector

% K matrices% j = 1 ... mfor j = 1:m,

M1j = [Z(:,1:(j−1)) M1(:, j:j) Z(:,j+1:m)];SDP(1).Kj = M1j + transpose(M1j);

end

SDP(1).Kidx = [reshape(1:m, m, 1) ones(m, 1)];

Code 12: Matrix conversions

The result of the solver depends on the boundary conditions that we define for

cj variables.

x_L = zeros(1, m);x_U = [];x_0 = zeros(1, m);

f_Low = [];

Prob = bmiAssign([], c, SDP, A, b_L, b_U, x_L, x_U, x_0,Name, f_Low);

Result = tomRun('penbmi', Prob, 1);

Code 13: Boundary conditions and calculation

If the boundary conditions restrict cj variables above zero, and kil variables between

-100 and 0 then the solver gives the result for k1 and k2:

K =[−13.3034− 13.3034

]The Simulink model of the controller can be seen on Figure 24. In the model the

recalculatedmg term added to the controlling input. The resulted feedback leads to

the following altitude simulation. Figure 25 shows this result with color red. For

comparison purposes the result of the LQ controller indicated with color blue. The

test case model had initial altitude of -1 meters, and the controller had have the

model to converge to zero point.

67

5 Control

Figure 24: QP Simulink model

Figure 25: QP controller result shows better trajectory against LQ

While we observe the two trajectories, we can see that QP controller has better

performance with these initial conditions. QP curve has no overshoot while con-

verged under less time then the LQ controller. Thus there exists such a configuration

when a QP controller behaves better than the linearized LQ.

5.5.4 Altitude equations in QP form using Taylor polynomial

In this section I will investigate that if is it worth to express trigonometric functions

with Taylor polynomial instead of embedding them like I did in the previous sec-

tions. Trigonometric functions can be represented with Taylor series. The original

68

5 Control

equation pair contains cos function, which is equal to the series:

cos(x) =∞∑n=0

(−1)n

(2n)!x2n = 1− x2

2!+

x4

4!− ...

Equation (39) can be rewritten using the formula above to approximate sin function

with 4 terms:

x8 = −g +(1− 1

2x21 +

1

24x41 −

1

720x61)(1−

1

2x23 +

1

24x43 −

1

720x63

) 1

mU1 (40)

Thus the QP system is as follows, if I neglect the last ( 1720

x6∗) term of the Taylor

polynomial:

x7 = x7(x8x−17 ),

x8 = x8

(− gx−1

8

)+ x8

( 1

576mx41x

43x

−18 − 1

48mx41x

23x

−18 +

+ 124m

x41x

−18 − 1

48mx21x

43x

−18 + 1

4mx21x

23x

−18 − 1

2mx21x

−18 +

+ 124m

x43x

−18 − 1

2mx23x

−18 + 1

mx−18

)U1

x1 = x1(x2x−11 )

x2 = x2(a1x4x6x−12 )

x3 = x1(x4x−11 )

x4 = x4(a3x2x6x−14 )

x6 = x6(a5x4x2x−16 )

The system is in equilibrium when x1, x3, x8 = 0, substituting them into (40),

we can calculate that, input U1 must equal to m · g = 7.848. Let me introduce a

new input variable, in place of U1: U = U1−mg, and if U is zero, the quadrocopter

hovers in case of laboratory conditions. The feedback structure was chosen to be:

U = k · x28 (41)

69

6 Remote control

The closed loop system with this feedback structure is:

x7 = x7(x8x−17 ),

x8 = x8

(− gx−1

8

)+ x8

( 1

576mx41x

43x8k − 1

48mx41x

23x8k+

+ 124m

x41x8k − 1

48mx21x

43x8k + 1

4mx21x

23x8k − 1

2mx21x8k+

+ 124m

x43x8k − 1

2mx23x8k + 1

mx8k), x1 = x1(x2x

−11 )

x2 = x2(a1x4x6x−12 )

x3 = x1(x4x−11 )

x4 = x4(a3x2x6x−14 )

x6 = x6(a5x4x2x−16 )

At this point we should realize that the problem will contain more quasimonomials

than the previous problem, thusM∗ matrices will be bigger and BMIs became harder

to solve.

5.6 Performance testing

The designed LQ angle controller was successfully integrated into the Android ap-

plication. Live test have been conducted to see if the quadrocopter stays in hover

motion. A ball attached to the bottom of the quadrocopter ensured that the quadro-

copter could turn around its axes and at the same time propellers were not touching

anything nearby.

Results showed that the device is super-sensitive for the turbulences and distur-

bances. The designed controller made the device to converge to hover motion, but

it was too slow. By adjusting its initial weighting parameters it is necessary to de-

sign a controller which has has less response time to changes. It is known that if the

approach the desired angle in less time, then overshoots will be increased.

6 Remote control

Reference values of a controller must be set from outside the system boundaries,

called remote controller. On server side (quadrocopter) a ParameterControl class

takes care about the reference values.

70

6 Remote control

6.1 Server side

Initially a TCP server listens on a port and if a change commend received then it

changes the ParameterControl class parameters. The client ie. the remote con-

troller (it can be a PC or another Android phone) sends string commands to the

server. The client can get a list about the changeable variables and their minimum

and maximum values. The Controller class initializes the ParameterControl

class with its reference values:

public Controller()

// Register reference values and their min, max values in radianspc.registerParameter(new Parameter("roll", "double", 0.0, −5.0, 5.0));pc.registerParameter(new Parameter("pitch", "double", 0.0, −5.0, 5.0));pc.registerParameter(new Parameter("yaw", "double", 0.0, −5.0, 5.0));pc.registerParameter(new Parameter("U1", "double", 7.84, −50.0, 50.0));pc.setParameterChangedCallback(parameterListener);

// Private variables: reference values and the ParameterControlParameterControl pc = ParameterControl.getInstance();double rollReference = 0;double pitchReference = 0;double yawReference = 0;double U1Reference = 7.84;

// A simple listener which notified when an event happensprivate ParameterListener parameterListener = new ParameterListener()

@Overridepublic void parameterValueChanged(String name, Object value)

try

if(name.equals("roll"))

rollReference = Double.parseDouble(value.toString());..

catch(NumberFormatException e)

Log.w("MainActivity.ProcessModel.ParameterListener","Parameter for "+name+" cannot be parsed as double. Value = "+

value);

;

Code 14: Parameter control

ParameterListener implementation changes the controller class variables when

the parameterValueChanged function triggered by the TCP socket.

6.2 Client side

The remote controller itself is also an Android application. The two phone must be

on the same wireless LAN network and there is another option, named Wi-Fi Direct

71

7 Actuation

[13], which enables the two phone to be connected directly. The latter technology

can only be used with Android 4.0 or newer.

The user interface of the remote controller software consists of a virtual joystick

and slider. The movement of the joystick updates the reference values of the an-

gle controller, the slider changes the overall reference propeller speeds (it basically

controls the altitude).

The communication interface is a single TCP client, connected to the ParameterControl

class and communicates with a text based protocol. The protocol contains two com-

mand:

1. get: The Remote Controller can get a list of all variables and their minimum,

maximum and current values

2. set: It has 2 parameters: the parameter name and a value. The remote con-

troller sets a parameter current value.

Figure 26 shows the minimalistic GUI of the application.

7 Actuation

Android phone sends commands to the Arduino board. Arduino board translates

these propeller speed commands into PWM signals and emits on an output pin. An

USB cable used to connect the phone and the board. The board receives the power

supply from one of the ESCs BEC(battery eliminator circuit) output. The board

behaves as an USB host, so the phone receives some power from it.

7.1 Communication through USB

OnAndroid platform the following simplified three methods open the Arduino USB

communication and send a message to it. An UsbAccessory object represents an

external hardware component which connected to the phone USB port. If an acces-

sory can be opened, the OS returns a file descriptor which we can use to read or

write to it.

72

7 Actuation

Figure 26: Remote controller GUI

//Tries to open USB device if it senses oneprivate final BroadcastReceiver mUsbReceiver = new BroadcastReceiver()

@Overridepublic void onReceive(Context context, Intent intent)

String action = intent.getAction();if (ACTION_USB_PERMISSION.equals(action))

UsbAccessory accessory = (UsbAccessory)intent.getParcelableExtra(UsbManager.EXTRA_ACCESSORY);

openAccessory(accessory);

;

//Opens the device, create IO descriptorsprivate void openAccessory(UsbAccessory accessory)

mFileDescriptor = mUsbManager.openAccessory(accessory);if (mFileDescriptor != null)

FileDescriptor fd = mFileDescriptor.getFileDescriptor();mInputStream = new FileInputStream(fd);mOutputStream = new FileOutputStream(fd);Log.d(TAG, "USB accessory opened");

//Send a command to the Arduinopublic void sendStringUSB(String command)

if (mOutputStream != null) try

mOutputStream.write(command.getBytes()); catch (IOException e)

Log.e(TAG, "Write failed", e);

73

7 Actuation

Code 15: Send message to Arduino

The POC code shown below, runs onArduino board, receives themessage through

USB cable. Arduino SDK uses AndroidAccessory and the USB_Host_Shield li-

brary to simply connect to theAndroidUSB channel. After calling powerOnmethod,

the code checks if there is an accessory which is connected. If it is connected then,

we can read some data from it into a buffer.

#include <Max3421e.h>#include <Usb.h>#include <AndroidAccessory.h>

AndroidAccessory accessory("Balazs Domjan", "Quadrocopter","Quadrocopter actuator", "1.0", "URI", "Serial");

//Setup phase: power on Accessory modulevoid setup()

accessory.powerOn();

byte recvmsg[255];short commandReceived = 0;

//Try to read the message from the devicevoid receiveCommand()

if(accessory.isConnected())

int length = accessory.read(recvmsg, sizeof(recvmsg), 1);if(length > 0)

commandReceived = 1;

//Main loopvoid loop()

receiveCommand();

Code 16: Arduino receives a message

7.2 PWM generation

Arduino board must generate an 50Hz PWM signal with 1ms to 2ms duty cycle.

The inputs of the standard servo motors and the input of the ESC must receive this

kind of PWM signal. A servo PWM signal can be seen on Figure 27

The board has 11 outputs on which it can generate a PWM signal. The SDK uses

the Servo library which maps the 0-180 interval to the 1ms-2ms length duty cycle

PWM signals. The example below, shows the usage of this library and changes the

74

7 Actuation

Figure 27: Servo PWM signal

angular speed of the front motor to the idle speed(2150 RPM).

#include <Servo.h>

Servo servoFront;int number = 30;

void setup()

servoFront.attach(12);servoFront.write(number);delay(5000);servoFront.write(42);

Code 17: Arduino PWM generation

After power on, an ESC waits for an 5 seconds length constant duty-cycle PWM

signal, and that will be the zero reference value. After the initialization, as the duty

cycle changes, the propeller speed changes. In the code above, the reader saw that

the reference value is 30, that is an 1.16ms length duty cycle PWM.

The joint characteristics of the brushless motor and the ESC was measured by

a tachometer, built from a LED and a photodiode. Propeller angular speed mea-

surements(blue line), shown on Figure 28, are taken from 2150 to 8900 RPM. 6900

RPM is a sufficient speed to lift the quadrocopter. The source code of the Arduino

based tachometer including the servo test control can be found in Appendix A. The

program waits for commands on serial port and sends the RPM information to serial

port.

On this figure we can see, that the propellers start spinning, if the servo out-

put value is at least 42, and the relation between the output value and the angular

75

8 Conclusion

.....

40

.

45

.

50

.

55

.

60

.

65

.

70

.

75

.

80

.2000

.

4000

.

6000

.

8000

.

Servo output [0-180]

.

Propellerspeed

[RPM

]

Figure 28: Servo output vs. RPM

speed is not linear. Therefore to translate the angular speed commands, received

from the phone, to servo output values we use polynomial interpolation which use

4 specific points. These points are (42, 2450), (50, 4800), (70, 8000), (80, 8900)

and the resulting polynomial is:

1271

12768x3 − 11833

532x2 +

5567005

3192x− 745550

19

And that can be seen on the figure with a green line. Swapping the coordinates:

(x, y) → (y, x) and doing an interpolation on them, we can get a function of angular

speed, in which we can substitute RPM values and results servo output values. That

equation is:

9.38× 10−11x3 − 8.5× 10−7x2 + 0.005365x+ 33.46

The actual implementation decompose the multiplications into parts, thus the pro-

gram does not need to handle too large and small numbers.

8 Conclusion

8.1 Nonlinear control of the quadrocopter

Using four motors, four ESCs and a frame a quadrocopter model created which

requires PWM signals to control its propeller speeds. The model consists of pre-

fabricated parts which are ordered from several sources. Unfortunately for the first

76

8 Conclusion

time, the electronic speed controllers were undersized to the currents of the brushless

motors. This kind of motor can easily picks 40 Ampere at 11.7 volts.

Arduino microcontroller platform has been chosen to form a bridge between the

smartphone and the hardware. The microcontroller can successfully convert the

controller RPM outputs to PWM values. The communication process effectively

carried out with a text-based protocol over a wired USB layer.

While the real life model evolved, a non-linear dynamic quadrocopter model was

chosen and realized in Matlab/Simulink environment. LQ controller for the angle

and the altitude was designed for the model. Thus it was possible to draw some

conclusions before the implementation of the real-life model controller.

Kalman filter proved to be a good solution for filtering sensor data. The fil-

ter successfully eliminates the noise generated by the four propellers. The filtered

attitude and position information stored efficiently using rotation matrices. Two

desktop tools created to analyze and tune sensor performance.

The main conclusion about the control design is that the LQ and QP controllers

provide similar results, however if we use the latter method, then we realize that

the problem may become harder if we want to feedback more variables. Because of

the number of the quasimonomials, the matrices, describing the problem, becomes

larger, thus the controller design becomes more complex. The Android phone which

we used for the development behave good under a medium load and in a real-time

application. The CPU usage is around half of its capacity and the timing of the

sensor interrupts are considered deterministic. The events and messages running

through the system have short response time.

8.2 Future work

While the designed controllers work very well in simulated environment, there are

some complications in practice. The controller cannot handle larger unexpected dis-

turbances, and it works only a narrow range around zero radians. It can be corrected

by an accurate parameter estimation and by optimizing controller feedback charac-

teristics with other initial weighting matrices. It would be nice if the QP controller

could follow to a time dependent reference value. So a future task could be to im-

77

8 Conclusion

plement a quasi-polynomial servo. It is worth to examine other signal processing

techniques and filters, such as Krein space linear estimation.

There are many other challenges about the subject. If the tilt and altitude con-

troller works well, then the position controlling options should be examined, using

better external sensors. Indoor navigation could be an interesting subject: compar-

ing alternative location techniques, such as ultrasound, WiFi, RFID-based naviga-

tion. Indoor cruising implies intelligent obstacle avoidance problem. These obstacle

detection and algorithms often uses computer visionwith the visible electromagnetic

spectrum, laser or ultrasound.

For the matter, if we talking about computer vision it is obvious to take the ad-

vantage of the camera, built in the smartphone, and the Android port of the OpenCV

[19] real-time computer vision library. This solution would be an elegant way to take

a movie scene or to take a virtual flight above the city. The completed device and its

interfaces can serve as a learning, research and experimental material in the student

robotics laboratory.

78

References

References

[1] Android official site. downloaded: 2012. 12. 02. http://www.android.com/

about

[2] Edvin Teale, Planes that go straight up. Popular Science, Mar 1935. [cited at p.

116]

[3] Douglas T. Peck, The History of Early Dead Reckoning and Celestial Nav-

igation: Empirical Reality Versus Theory. downloaded from: http://www.

newworldexplorersinc.org/EarlyNavigation.pdf

[4] Android API guide to motion sensors. downloaded: 2012. 12. 02.

http://developer.android.com/guide/topics/sensors/sensors_

motion.html

[5] Peter S. Maybeck Stochastic Models, and Control Volume 1. MATHEMATICS

IN SCIENCE AND ENGINEERING. Academic Press, 1979.

[6] Bhupinder S. Mongia, Vijay K. Madisetti, Fellow, IEEE Reliable Real-Time

Applications on Android OS. 2010.

[7] Anindya S. Paul and Eric A. Wan WI-FI BASED INDOOR LOCALIZATION

AND TRACKING USING SIGMA-POINT KALMAN FILTERING METHODS

2008.

[8] Jack B. Kuipers QUATERNIONS AND ROTATION SEQUENCES Geometry,

Integrability and Quantization; September 1-10 Coral Press, Sofia 2000., pp

127-143

[9] Ilona Sonnevend Analysis and model based control of a quadrotor helicopter

2010.

[10] Dr. Magyar Attila State estimation and state feedback control in quasi-

polynomial and quantum mechanical systems 2007.

[11] JavaFX official site. downloaded: 2013. 04. 14. http://www.oracle.com/

us/technologies/java/fx/overview/index.html

79

References

[12] Raymond A. Decarlo Linear Systems: A State Variable ApproachWith Numer-

ical Implementation Prentice Hall, 1989

[13] Wi-Fi Direct. downloaded: 2013. 04. 16. http://developer.android.com/

guide/topics/connectivity/wifip2p.html

[14] Arduino ADK. downloaded: 2013. 04. 16. http://arduino.cc/en/Main/

ArduinoBoardADK

[15] K. M. Hangos, J. Bokor, G. Szederkenyi Analysis and Control of Nonlinear

Process Systems Springer-Verlag 2004.

[16] Kalman, Rudolph Emil A New Approach to Linear Filtering and Prediction

Problems Transactions of the ASME--Journal of Basic Engineering, 1960

[17] Z-Device Test application. downloaded: 2013. 04. 22. https://play.

google.com/store/apps/details?id=zausan.zdevicetest&hl=en

[18] Joshua Bloch Effective Java Addison-Wesley Professional, 2008.

[19] The OpenCV library for Android platform. downloaded: 2013. 04. 22. http:

//opencv.willowgarage.com/wiki/

80

List of Figures

List of Figures

1 Quadrocopter frame shows the basic concept and direction of rota-

tion of the propellers . . . . . . . . . . . . . . . . . . . . . . . . . 21

2 The Android logo . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

3 Android coordinate system [4] . . . . . . . . . . . . . . . . . . . . 23

4 Z - Device Test [17] application measures linear acceleration . . . . 24

5 Arduino and a smartphone connected with an USB cable . . . . . . 31

6 Prefabricated quadrocopter model with the Arduino board under it.

The LEGO application on the rear left rotor is an RPM counter unit. 32

7 Software components - block diagram of the sensor data processor . 33

8 Software components - block diagram of the controller module . . . 34

9 Slippage times under no load . . . . . . . . . . . . . . . . . . . . . 35

10 Slippage times under medium load . . . . . . . . . . . . . . . . . . 36

11 Measured delays: Measurements ("X") were not taken in the same

time . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

12 Using Kalman filter to fuse accelerometer and gyroscope . . . . . . 40

13 Tuning Kalman filter parameters with the tool, written in JavaFX . . 42

14 Real-time analyzer, written in Qt, draws a graph from sensor data . . 44

15 The raw and fused data showed, while the quadrocopter was in the

ground with rotation propellers . . . . . . . . . . . . . . . . . . . . 44

16 Filtered data has 1 degrees of error, while propellers were at medium

speed . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

17 Compass tilt compensation . . . . . . . . . . . . . . . . . . . . . . 46

18 Magnetometer fusion with gyroscope . . . . . . . . . . . . . . . . 46

19 Altitude filtering result shows that the accurateness is around 20 cm 49

20 Angle controller result shows that angles were converged to zero

from an initial value . . . . . . . . . . . . . . . . . . . . . . . . . . 56

21 Matlab/Simulink angle controller implementation . . . . . . . . . . 58

22 Altitude controller implementation in Simulink environment . . . . 60

81

List of Figures

23 LQAltitude controller result: the system converges to the zero point

from an initial position . . . . . . . . . . . . . . . . . . . . . . . . 61

24 QP Simulink model . . . . . . . . . . . . . . . . . . . . . . . . . . 68

25 QP controller result shows better trajectory against LQ . . . . . . . 68

26 Remote controller GUI . . . . . . . . . . . . . . . . . . . . . . . . 73

27 Servo PWM signal . . . . . . . . . . . . . . . . . . . . . . . . . . 75

28 Servo output vs. RPM . . . . . . . . . . . . . . . . . . . . . . . . 76

29 List of files and directories on DVD . . . . . . . . . . . . . . . . . 85

82

A RPM counter Arduino code


#include <Servo.h>

//servo variablesServo servoFront;Servo servoBack;Servo servoLeft;Servo servoRight;int number = 30;

// set pin numbers:const int ledPin = 42; // the number of the LED pin

// Variables will change:int ledState = LOW; // ledState used to set the LEDlong previousMillis = 0, prevMMilis = 0; // will store last time LED was updated

long measureInterval = 300;

void setup() // set the digital pin as output:Serial.begin(115200);digitalWrite(A0, HIGH);pinMode(ledPin, OUTPUT);digitalWrite(ledPin, HIGH);

//setup servos:servoFront.attach(11);servoFront.write(number);servoBack.attach(9);servoBack.write(number);servoLeft.attach(5);servoLeft.write(number);servoRight.attach(3);servoRight.write(number);

int val = 0;short oldstate, state = LOW;unsigned long microSeconds, lastMicroSeconds = 0;short propellerCount = 7;double currentRPM = 0;

unsigned long minute = 1000000 * 60;

void loop()

val=analogRead(0);if(val < 100)

state = HIGH;else

state = LOW;

if(oldstate == LOW && state == HIGH)

microSeconds = micros();currentRPM = (minute/(microSeconds − lastMicroSeconds)) / propellerCount;lastMicroSeconds = microSeconds;

oldstate = state;

unsigned long currentMillis = micros();

if(currentMillis − prevMMilis > 300000) prevMMilis = currentMillis;val=analogRead(0);Serial.println(currentRPM);

//servo controlif (Serial.available() > 0)

83


int inByte = Serial.read()−'5';if(inByte > 5)

Serial.println("Increment bigger than 5!");return;

Serial.print("Add to number: ");Serial.println(inByte);Serial.print("New number is: ");number = number + inByte;Serial.println(number);if(number > 20 && number <= 100)

servoFront.write(number);servoBack.write(number);servoLeft.write(number);servoRight.write(number);

else

Serial.println("Number is out of bounds");

Code 18: Arduino RPM counter with servo controller. Results are logged to theserial port.

84

B Contents of DVD attachment

B Contents of DVD attachment

Figure 29: List of files and directories on DVD

85

diplomadolgozat - wordpress.com · 2014. 3. 23. · universityofpannonia...

Documents