G5B. Automatic Control of a Quadrotor in the Smart Building753869/FULLTEXT01.pdf · gyroscopes that...

G5B. AUTOMATIC CONTROL OF A QUADROTOR IN THE SMART BUILDING

Automatic Control of a Quadrotor in the SmartBuilding

Daniel Sjoholm and Martin Biel

Abstract—In this project, we investigate automatic control ofa quadrotor UAV and discuss its possible applications in theSmart Building. The purpose is to construct a control algorithmfor stable quadrotor flight and explore the use of UAV in asmart home. We derive the equations of motion for a quadrotor,construct a model and devise a control strategy to keep it stablein the air. The strategy will be to use cascaded controllersfor position and attitude. Simulations are performed and thecontrollers are then tested in practice. In the simulations, we uselead lag controllers as they provide easy loop shaping and goodperformance. In the practical implementation, we instead choseto implement PID controllers, due to their wide use in industry,easy implementation and ease of tuning. The end result is astable loop with acceptable performance in terms of positionalcontrol. A simple waypoint system is implemented as well, whichis key for trajectory planning and enables the vehicle to performvarious tasks.

I. INTRODUCTION

THE concept of a smart building has been graduallyreceiving more and more attention in recent time. The

smart building is all about autonomy and reacting on theworld around us, without human intervention. A possiblecomponent in the smart building is the quadrotor, a small 4-rotor Unmanned Aerial Vehicle (UAV). Quadrotors have todate been actively studied for various uses, including, but notlimited to: air transports [1], search and rescue [2] and securityrelated applications such as tracking [3].

In this project, we study control of quadrotors and theirpossible use in a smart building, as a part of attaining moreautonomous homes. The aim is to devise a control algorithmthat can keep a quadrotor stable in the air and follow simplepaths. We also wish to explore suitable applications in thesmart home. Similar projects have been done before, forexample [4] and [5].

Quadrotors are inherently unstable systems, and their dy-namics are too fast for a human to stabilize them manually.Computers are however quick enough to stabilize a quadrotor,but they require sensors in order to do it. As such, a look on therequired sensors and the mathematical model for stabilizing isrequired before we proceed to actually control a quadrotor.

Our work consists of a theoretical analysis followed by apractical implementation. First, we study the available sensorsrequired for quadrotor control in section II. Then, in sectionIII, we construct a mathematical model of the quadrotormovement and devise a control strategy for autonomous flightin section IV. After that, in section V, we simulate the controlloop to see if the strategy is viable. Finally, in section VI, weimplement the controllers together with a waypoint algorithm

in order to fly the quadrotor in the Smart Mobility Lab (SML)at KTH.

II. SENSORS

A couple of sensors are required in order to keep a quadrotorstable; these are at the very least: accelerometers and gyro-scopes. Readings from these sensors can be used to estimatethe current attitude. The gyroscope is required in order todetect the angular speed, while the accelerometer corrects thegyroscope for drifts in sensor output, as well as in stationaryprovide a direction for earth’s gravity [6].

In addition to the sensors required for stabilization, moresensors are of course of interest, such as for determiningposition via GPS or measuring real world attributes such astemperature and pressure.

A. Accelerometer

An accelerometer measures the acceleration in one or moreaxes. Single axis accelerometers have been the norm for quitesome while, but thanks to a decreased manufacturing cost andsmaller size than 3 individual single axis accelerometers byemploying MicroElectroMechanical Systems (MEMS) tech-nology, three axis accelerometers are quite common today [7].

An initial description of their mechanical design is simplyan object of mass located inside a frame, surrounded bysprings, shrunken down to micrometer size. The springs inturn separate two electrodes from each other. Acceleration willforce the mass against the springs, which causes the electrodesto move in relation to each other, creating a voltage difference.This voltage difference can be measured and correlated to thecurrent acceleration. However, when creating the accelerome-ter structure for measuring three axis at the same time in onepackage, the issue of sensing a change in one axis partially inanother, or so called crosstalk, occurs.

However, the measurements from a three-axis accelerometercan today be made very precise, with cross-talk lower than10%, and non-linearities at 3% or lower [8]. The quadrotoravailable in the SML uses a three-axis MEMS accelerometerfrom Analog Devices named ADX330, which has a cross-talk of 1% [9]. For situations where precision is not of utmostimportance one can thus use them without excessive additionalprocessing.

B. Gyroscope

A gyroscope measures the angular rate in one or moreaxes. There are several different types of gyroscopes, but the


type of gyroscope currently receiving the most attention isthe MEMS gyroscope. As with the MEMS accelerometers,the MEMS gyroscopes are cheaper than previous technologyand are seeing widespread acceptance [10]. Another reason fortheir success is the lack of rotating parts [11], which degradewith time and require replacing.

There are multiple ways to detect the angular rate, buta common way is via the Coriolis effect. The group ofgyroscopes that use the Coriolis effect are called Coriolisvibratory gyroscopes, to which MEMS gyroscopes belong.Inside such a gyroscope there is a mass suspended in astructure by flexible beams. This mass is then made to oscillateinside the structure with the help of electrodes pushing themass back and forth. If an external angular rate is present,the Coriolis force will now displace the mass as it oscillates,allowing for another set of electrodes to sense the change incapacitance. This change can then be correlated to the angularrate.

MEMS gyroscopes however have a quite prominent down-side compared to other larger gyroscopes, which is low ac-curacy. This is expressed in measurement bias and an errorthat builds up with time as the gyroscope is in use. It can becalibrated to some extent, but it will always be present. Themost cost efficient way of reducing the error is by filteringthe gyroscope signal in software. For this, Kalman filters incombination with auto-regressive models have been shown togive good results [12] [13].

C. Cameras

On board cameras can be used as a means of capturing animage or a video while in operation, but they can also be usedfor positioning assistance and target tracking. For imagingusage, one can either retain the footage on board the quadrotor,or send it via a suitable radio protocol to a base stationon ground. For positioning assistance and target tracking acommon method of data collection is by optical flow, for whichseveral different algorithms exist for extrapolating data froman image feed [14]. Also, there exists dedicated optical flowsensors with an additional layer of abstraction, that reducesthe calculations needed to be done by the control system [15].

Experiments have also been conducted on using optical flowas a possible replacement for GPS for motion estimation,which have shown that it could be a viable option as longas the image has enough details for optical flow [16]. Themeasurement bias and variance for optical flow measurementswill of course vary depending depending on the algorithmemployed, and one should choose an algorithm of suitableproperties. Optical flow is also a viable alternative to usingultrasonic sensors for distance detection to objects [17] and inturn avoidance.

III. MATHEMATICAL MODELING

Now, a mathematical model of the quadrotor is developed.The model will be a basis for how to use the measured valuesfrom the sensors in order to control the quadrotor. Similarmodels have been derived before, for example in [4].

A. Coordinate transformations

We introduce two coordinate systems: An earth-fixed framedenoted by the subscript E and a body-fixed frame alignedwith the quadrotor denoted by the subscript B.

Let φ, θ and ψ denote the Euler angles between the body-fixed frame and the inertial frame, which will describe theattitude of the quadrotor. The linear position is defined as(

x y z)E

[m]

in the earth-fixed frame. Further, the velocity(u v w

)B

[m/s]

and the angular rates of the quadrotor are defined accordinglyin the body-fixed frame(

p q r)B

[rad/s]

where p is the roll-rate, q is the pitch-rate and r is the yaw-rate.We require a way to transform between the two coordinatesystems. Coordinate transforms will be denoted as BRE , hereR is the transformation matrix from system E to B. Generalrotations of the body-fixed system with respect to the earthframe can be seen as rotations around each axis separately[18], as follows in matrix form

Rx =

1 0 00 cosφ sinφ0 − sinφ cosφ

(1)

Ry =

cos θ 0 − sin θ0 1 0

sin θ 0 cos θ

(2)

Rz =

cosψ sinψ 0− sinψ cosψ 0

0 0 1

(3)

If a yawing movement is performed around the z axis,followed by a pitching movement around the y axis and finallya roll around the x axis the complete transformation is acquiredthrough the direction cosine matrix

R = RxRyRz

which can be used to transform between vectors in theearth-fixed coordinate system and the body-fixed system. Inparticular, the velocity of the quadrotor is transformed byxy

z

E

= ERB

uvw

B

= R−1

uvw

B

= RT

uvw

B

It is important to note that transforming to the body-fixedframe from the inertial frame through subsequent rotationsrequires two intermediate frames. Therefore, the angular ratesof the quadrotor must each be transformed into the appropriateframe to acquire correct values of roll-,pitch- and yaw rate. Thetransformation matrix for the angular rates is given by [18],

S =

1 sinφ tan θ cosφ tan θ0 cosφ − sinφ0 sinφ sec θ cosφ sec θ

(4)


and hence φθψ

= S

pqr

B

B. Quadrotor forces and torques

The forces and torques acting on the quadrotor that aredeemed sufficiently large to have an effect, are all from the 4motors. See Fig. 1 for reference. The total thrust in the body-fixed z direction is the sum of the force generated from the 4individual motors,

T = T1 + T2 + T3 + T4 [N] (5)

The torques are M13 along the x-axis, and M24 along they-axis. These come in turn from the difference in generatedthrust from the motors numbered 1,3 and 2,4 respectively. Forinstance, if the length from the quadrotor origin to the motorcenter is l, then

M13 = l(T1 − T3) [Nm] (6)

M24 = l(T4 − T2) [Nm] (7)

The torque marked D comes from the air resistance as eachpropeller rotates through the x, y plane. The drag depends onthe rotational direction of the propellers, and propeller 1 and 3are defined to rotate counter-clockwise, and propellers 2 and4 rotate clockwise. The torque D can now be written as afunction of the individual thrusts generated by the motors:

D = d(T1 + T3 − T2 − T4) [Nm] (8)

where d is the drag coefficient, which corresponds to the dragor torque generated as a propeller moves through the air anddepends on the propeller geometry.

xB

yB

zBxE

yE

zE

T1

T2

T3

T4

M24

M13

D

Figure 1. Schematic over forces and torques acting on a quadrotor.

C. Dynamical equations

The governing equations for the quadrotor motion is derivedfrom first principles using Euler’s laws of motion. The first law(Newton’s law) is applied in the earth-fixed frame:

FE = mvE

Hence,

FE =

Fx

Fy

Fz

E

= m

xyz

E

(9)

The acting forces on the quadrotor are identified in theprevious section. Gravity is always acting along the z-axisof the earth-fixed frame:

WE =

00mg

E

(10)

The thrust T is directed along the negative z-axis of the body-fixed frame. The previously derived direction cosine matrix isused to acquire the thrust in the earth frame:

TE = ERB

00−T

E

=

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

− cosφ cos θ

E

T

(11)For simplicity, no other forces are assumed to act on the

quadrotor and hence

FE = WE + TE (12)

Combining (9),(10),(11) and (12) yields the following stateequations for the position of the quadrotor:

x = −(cosφ sin θ cosψ + sinφ sinψ)T

m(13)

y = −(cosφ sin θ sinψ − sinφ cosψ)T

m(14)

z = g − cosφ cos θT

m(15)

The second law in the body-fixed frame is now applied,which states that

MB = L = IωB = IωB + ωB × IωB

where the relative time derivative [19] is utilized. I is themoment of inertia of the quadrotor. Since the quadrotor issymmetric and its rotation axes coincides with the principalaxis it holds that [19]

I =

Ix 0 00 Iy 00 0 Iz

B

where Ix ≈ Iy due to symmetry. Hence,Mx

My

Mz

B

= I

pqr

B

+

pqr

B

× I

pqr

B

=

Ixp+ qr(Iz − Iy)Iy q + pr(Ix − Iz)

Iz r

B

(16)

The acting torques on the quadrotor are identified in theprevious section. The torques from the propellers discussedearlier are M13 along the y-axis M24 along the axis andfinally D along the z-axis. For simplicity, the aerodynamicand gyroscopic moments present on the quadrotor are ignoredsince they will have a relatively small effects at low speed


and horizontal attitude. Thus, (16) gives the following stateequations for the angular rates:

p =M24

Ix− qr Iz − Iy

Ix(17)

q =M13

Iy− pr Ix − Iz

Iy(18)

r =D

Iz(19)

which can be transformed into state equations for the Eulerangles using (4).

D. Linearization

The derived state equations describing the motion of thequadrotor are non linear. To design controllers using basictechniques we first linearize the model. It is obvious that thestable state of the system is when the Euler angles tend tozero and the quadrotor is in a hovering state with horizontalattitude, constant altitude and zero velocity. The stable pointof the system isxy

z

=

x0y0z0

φθψ

=

000

uvw

=

000

pqr

=

000

Using sinα ≈ α and cosα ≈ 1 for small angles α and lettingthe yaw angle ψ be zero, equations (13)-(15) become

x ≈ −θ Tm

y ≈ φ Tm

z ≈ g − T

m

Also,S → I

when the quadrotor tends to the stable point. Thus,φθψ

≈pqr

in a neighborhood around the stable point. Further, the crossterms tend to 0 fast near the stable point in equations (17),(18)and (19). Therefore, linearized state equations are as follows:

x = − Tmθ

y =T

mφ

z = g − T

m

φ = p = M24

θ = q = M13

ψ = r =D

Iz

E. Transfer functions

There are 4 different type of quadrotor movements thatcan be performed when the quadrotor is close to horizontalattitude. First, horizontal movement in the x-y plane. Secondly,vertical translation along the z-axis with constant attitude.Finally, yawing rotations around the z-axis with constantattitude. These movements can be described by the linearizedequations since horizontal attitude corresponds to the stablepoint. Transfer functions are derived for each of the move-ments.

1) Horizontal movement:

x = − Tmθ

At constant altitude, T is regarded as fixed, T ≈ mg. Thus,the transfer function for movement in the x direction is givenby

X(s) = − T

ms2Θ(s) = − g

s2Θ(s) (20)

at constant altitude, with the longitudinal position X asoutput signal and the pitch angle Θ as input signal.

The same arguments can be adapted for movement in the ydirection due to symmetry.

y =T

mφ

and the transfer function is hence given by

Y (s) =g

s2Φ(s) (21)

at constant altitude, with the lateral position Y as output signaland the roll angle Φ as input signal.

2) Vertical movement:

z = g − T

m

Since T ≈ mg around constant altitude U = g − Tm is

introduced as a control signal. U = 0 corresponds to constantaltitude and deviations from this point causes vertical move-ment. Thus, the transfer function for the vertical movement isgiven by

Z(s) =1

s2U(s) (22)

with the vertical position Z as output signal and U = g − Tm

(implicitly the thrust T ) as input signal.3) Yawing movement:

ψ = r

r =D

Iz

The transfer function for yawing movements is given by

Ψ(s) =1

Izs2D(s) (23)

with the yaw angle Ψ as output signal and the torque alongthe body z axis D as input signal (The yaw rate r is anintermediate signal).


MotorDynamics

QuadrotorDynamics

PositionControl

Positionreference

AttitudeControl

Thrust

Torques

Position

Attitude

Attitudereference

VoltageSignal

Figure 2. Outline of control system used for automatic control of quadrotormovement.

IV. CONTROL

We now design controllers for the quadrotor movementusing the linearized model. The control system will consist oftwo sets of cascaded controllers: one for position control andone for attitude control. The first set will include controllersfor each of the inertial coordinates x,y and z. Likewise, theattitude control consists of controllers for the Euler anglesφ, θ and ψ. The basic outline of the control system can beseen in Fig. 2. We will only perform a detailed design ofthe position controllers and leave a brief explanation of howattitude control can be performed.

A. Position control

Assuming a certain attitude can be attained using internalcontrollers on the quadrotor the roll and pitch angles can beregarded as input signals for movement in the horizontal plane.For the thrust input signal, mg is always used as a base valueto stay at constant altitude and then deviate from the baseto change height. Before designing controllers we decide onsome performance criteria and introduce constraints for thecontrolled variables. Since the quadrotor should closely followspecified paths the overshoot of step responses in horizontalmovement should be low, ideally below 10%. Secondly, thecomputed control signal should be bounded by a safe angle,at most 45◦ from horizontal attitude. Otherwise the attitudecontrollers can be asked to reach an angle large enough todiverge from stable flight. Finally, the vertical controller shouldideally settle the quadrotor on a new height quicker than5 seconds since deviations from the base value affects thehorizontal controllers. If we assume that there are no obstaclesin the vertical direction some overshoot can be accepted aslong as the quadrotor settles on a new height in a short timespan. Another idea would be to use a less aggressive verticalcontroller that yields a long rise and settling time and lowovershoot to influence the horizontal movement as little aspossible. Depending on the application of the vehicle thisapproach may however result in displacement that is too slow.

1) Horizontal control: The transfer functions from inputangles to horizontal position are given in (20) and (21). Thesystem contains two integrations and is unstable. A lead-lagcontroller is introduced to control the horizontal movement.The controller is designed using a toolbox in Matlab wherethe controller is tuned until it yields satisfactory performance

0 1 2 3 4 5 6 7 8 9 100

0.2

0.4

0.6

0.8

1

1.2

1.4

Time (seconds)

Am

plitu

de

Figure 3. Step response in the y-direction for the horizontal controller.

in relation to the design specifications. The designed controlleris given by

F (s) = 0.37(s+ 0.04)(s+ 6)

(s+ 11.98)(s+ 2.13)(24)

The closed loop transfer function from input angle to horizon-tal position is given by

Gc(s) =F (s)G(s)

1 + F (s)G(s)

Due to symmetry, the controller is only verified in the y-direction and thus

G(s) =g

s2

The step response is shown in Fig. 3. The overshoot is ≈ 5%,the rise time ≈ 1.8s and the settling time ≈ 23.5s. Theovershoot is low at the cost of a long settling time. This issuitable for path tracking applications as long as the UAVdoes not need to reach its goal in a short time. Therefore,the designed controller is deemed sufficient. As specified, theinput angle must be ensured to be bounded. The control signalcan be seen in Fig. 4. The angle required to produce thepositional output is bounded by ≈ 22◦ which is consideredokay.

2) Vertical control: The transfer function from input devi-ation from base thrust to vertical position is given in (22). Italso consists of two integrations and a lead-lag is designed inthe same manner. The controller is given by

F (s) = 10(s+ 0.44)(s+ 1.705)

(s+ 9.93)(s+ 1)(25)

The step response is computed and shown in Fig. 5. Therequired thrust input is shown in Fig. 6.

The overshoot is ≈ 29%, the rise time ≈ 0.85s and thesettling time ≈ 4.6s. Looking at the input signal one candeduce that during the first milliseconds the required thrustis ≈ 2mg which implies that the horizontal transfer functions(20) and (21) change by a factor 2. As predicted, the horizontalmovement will be affected during altitude changes; However,the rise and settling time are quite low, as specified, and theeffect should subside fast enough for the quadrotor to keep toits path. The price for this is a rather large overshoot.


0 1 2 3 4 5 6 7 8 9 10−0.05

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

Time (seconds)

Am

plitu

de

Figure 4. Control signal after a step reference for the horizontal controller.

0 1 2 3 4 5 6 7 80

0.2

0.4

0.6

0.8

1

1.2

1.4

Time (seconds)

Am

plitu

de

Figure 5. Step response in the z-direction for the vertical controller.

0 1 2 3 4 5 6 7 8−2

0

2

4

6

8

10

Time (seconds)

Am

plitu

de

Figure 6. Control signal after a step reference for the vertical controller.

B. Attitude control

To produce functional internal controllers for regulatingthe attitude of the quadrotor, one needs knowledge of themotor dynamics, and these differ between different vehicles.Additionally, comprehension of flight dynamics and the aero-dynamics of the propellers is required. We do not delve into

Figure 7. Simulink model

these subject areas and instead describe the fundamentalsof attitude control. Using a gyroscope and an accelerometerone can acquire state estimations of the current attitude andfeed it into a control loop. The current angular rate is alsoavailable through the gyroscope. The control structure couldthen for example consist of cascaded PID controllers or anLQ-regulator. The controllers calculate the necessary speedsof each propeller to attain a certain reference attitude and thensend appropriate voltage signals to the motors. A more detailedanalysis of quadrotor attitude control is carried out in [20].

Finally, control of the yaw angle needs to be addressed.If the body fixed system is not aligned with the earth fixedsystem then the positional controllers will not request correctpitch and roll angles from the attitude controllers. The problemcan be solved by keeping the yaw angle set to zero using acontroller designed with regard to eq. (23). Alternatively, thecoordinates can be transformed through axis projection in theyaw angle which ensures that the correct attitude angles arerequested regardless of the yaw angle.

V. SIMULATION

A simulation is now constructed using the non-linear model,which includes equations (13) - (15) and (17) - (19), inSimulink Matlab. In the simulation, the cascading controlstrategy discussed earlier is employed which make use of thedesigned controllers from the linearized model. The model canbe seen in Fig. 7 and the code details is shown in AppendixA.

The values for moment of inertia are taken from [21], wheremeasurements have been done on the quadrotor in the SmartMobility Lab, and are given by

Ix = Iy = 0.0015 [m2kg]

Iz = 0.0035 [m2kg]

The mass of the quadrotor in the simulation is 1.1 kg, which isthe same as the one in the lab. The attitude control is simulatedusing cascaded PID controllers that were tuned using anoptimization tool in Matlab, and the ”Attitude control” blockthus resemble functional controllers to which reference anglesare sent. The simulation does not include motor dynamicsand the attitude controllers calculate force and torque directlywith no intermediate motor considerations. The output of thesimulations are plots of the positional variables that can beanalyzed and also an animation that visualizes the quadrotormovement. Plots of step responses in all 3 directions, both


0 5 10 15 20 25 300

0.2

0.4

0.6

0.8

1

1.2

1.4

Time (seconds)

Am

plitu

de

Horizontal step with simultaneous vertical step

Horizontal step only

Figure 8. Horizontal step responses in the non linear simulation. One stepresponse (solid line) occurs at the same time as a vertical step is applied.

0 5 10 15 20 25 300

0.2

0.4

0.6

0.8

1

1.2

1.4

Time (seconds)

Am

plit

ude

Figure 9. Vertical step response in the non linear simulation

movements in several directions simultaneously and not, canbe seen in Fig. 8 and Fig. 9.

The controllers performance in the non-linear model isreduced compared to the linear model, in terms of increasedovershoot and prolonged rise and settling time. The predictedcoupling between the variables is apparent as the overshootin horizontal movement is increased if a vertical step occursat the same time. However, the outputs are stable and thecontrol strategy seems viable, at least in an ideal situation.In a real implementation, there are a lot of uncertainties suchas: motor dynamics, wind disturbances and irregularities inthe vehicle. Hence, the designed controllers will probably notwork as well as in the ideal case and may not even produce astable control loop. The simulation is modular and can easilybe extended to model the aforementioned uncertainties andthe theoretically designed controllers could then be improvedto work better in a real situation. However, it is very hard totake all uncertainties into account, and the theoretical approachmay never result in optimal controllers. Instead, a way togo about is to implement the cascading control strategy, buttune the parameters manually. The tuning should be carriedout with regard to the theoretical observations; in particular,

the coupling between the signals. Implementing the controlstrategy should result in stable quadrotor movement and theparameters can then be fine-tuned to yield good performance.

VI. EXPERIMENTAL IMPLEMENTATION

A practical implementation of the quadrotor control loopis performed in the Smart Mobility Lab (SML) at KTH. Thequadrotors in the lab include functional commercial attitudecontrollers [4] that are observed to perform very well and donot require further tuning. As such, the control strategy canbe implemented by introducing positional controllers on topof the existing attitude controllers and tune them manually.

Communication with the quadrotor is done over a one-way IEEE 802.15.4 implementation called Tmote Sky, whichallows a 10 Hz update frequency, something which wasdetermined experimentally.

The flow of data in the lab can be seen in Fig. 10. Thepositional controller in LabVIEW retrieves data from themotion capture system and uses this together with the currentpositional setpoint to send a reference signal for throttle &attitude. The reference signal is sent through the Tmote linkto the quad, upon which the attitude controller reacts to thereference signal and aligns the quadrotor to it.

A. Sensors

The lab is equipped with a collection of motion capturecameras from Qualisys, enabling millimeter-precision mea-surements of certain infrared markers. These infrared markerswere attached to the quadrotor and defined as a group in thecamera system, allowing for measurements of 6 degrees offreedom of the quadrotor: x,y,z position, pitch, roll and yawangles. This system was used as the sensor feedback regardingposition and attitude of the quadrotor. The data acquisitionspeed from the Qualisys system is much greater than 10 Hzand as such the communication link to the quadrotor was thelimiting factor.

Sensor feedback and control loops for positional controlwas handled on a stationary computer on the ground, whichsent four input signals to the quadrotor: throttle, pitch, rolland yaw rate. No sensor data from the quadrotor was sentto the computer, all sensor input for the positional controlwas retrieved from the Qualisys system. Our original plansincluded mounting a camera or ultrasonic sensor on thequadrotor for doing optical flow and distance measurementsas a sort of sensor feedback. However, cameras were deemedinappropriate given the radio links available [22], and in gen-eral the prospect of mounting additional sensors was deemedout of the scope of this project. The quadrotor itself howeverdid of course use the measurements from the gyroscope andaccelerometer in order to keep itself stable at the referencesignal that was sent to it.

B. Ground station

The processing of data and communication with the quadro-tor was done in LabVIEW on the ground computer. Wereceived a base file that was already set up to retrieve data


from the Qualisys system, and send the reference signals to thequadrotor. However, no safety features apart from immediatelyturning of the motors were implemented. As such, most of thework put in to the LabVIEW program was to make sure thatno erroneous signals would be sent to the quadrotor and thatit could be landed safely no matter what. An overview of themain LabVIEW program and the subroutine for the controlimplementation can be seen in Appendix B.

PositionalControl

ComputerTMote

QuadrotorTMote

AttitudeControl

Quadrotor

Motioncapturesystem

Figure 10. Overview of data flow in the lab. Dashed lines represent wirelessconnections, dotted lines optical observations, solid lines represent normalconnections.

1) Safety features: At times, the motion capture system wasunable to detect enough markers for calculating the position ofthe quadrotor; when this happens, it returns data that withoutprocessing is treated as +∞ in LabVIEW, which if unhandledwill disturb the control loop. Our solution to this was to alwaysstore the last good output from the motion capture system, andin case the current output was problematic, use the last one. Amore sophisticated approach would be to model the quadrotorand it’s movements and estimate where it currently is at.However, the duration of signal loss is usually short enoughto not impact performance negatively when using previousmeasurements and the solution was deemed good enough.

For emergency stops, turning off the motors instantly isrequired; however, turning off the motors in mid air willleave the quadrotor as a brick, which even at an altitudeof just 1 meter is probable to prove fatal for at least apropeller and possibly one of its arms. In order to reducethis chance, measurements for what throttle is required toalmost lift the quadrotor were conducted. The throttle valuewas then, together with attitude signals corresponding to thequadrotor being level, prepared to be sent when a button forsafety descent is pressed. When these signals are sent, thequadrotor descends in a moderately quick pace and lands withthe correct side down, where a styrofoam base protects all parts

Figure 11. The quadrotor used for experimental implementation

from impact. See Fig. 11 for a picture of the quadrotor flownin the lab, including the protective styrofoam base.

This system was in use extensively, and since implementa-tion the quadrotor did not experience any crash at all, evenunder very aggressive control implementations that were notstable.

As a passive safety system, both input and output valuessent to the quadrotors were sanitized. No position or altitudereference could be sent to the controllers that exceeded certaindimensions selected as a flyable area inside the lab. For theoutput, the signals were restricted to be within a certaindistance from the setpoint corresponding to a throttle valuearound hovering and certain angles from level position. Thethrottle was limited to never exceed 70% of the maximumthrottle, as even with this limitation the quadrotor was fastenough and higher values would only demand quicker reac-tions on the safety controls. The angles selected were about±20◦ in the pitch & roll direction and the yaw-rate was limitedto ±20◦/s. Angle limitations were done in order to reduce thepossibility of the quadrotor quickly moving out of the safeflying zone.

Should the quadrotor despite the safety features ascend toan altitude above a certain height, the signals for a safe descentwould automatically send and refuse to unlock until a programrestart, in order to protect equipment mounted in the ceilingeven when the operator is unable to react quick enough. Nosuch feature was developed for horizontal movements, as itwas deemed to do more damage than good in case of a quicksideways movement where the best chance to avoid damagewould be to let the controller move the quadrotor back towardsthe center, something descending would interrupt.

2) Control: PID controllers are utilized since they are easyto implement in the lab and are suitable for manual tuning.For positional control, 3 PID controllers were used: one foraltitude, affecting the throttle signal sent to the quadrotor; twofor horizontal position, one for the x-direction and another fory. In theory the x and y controllers should be the same, as thequadrotor body is assumed to be symmetrical in the directionof it’s arms. However, this was found out not to be the case,as one direction required a more aggressive tuning than the


other in order to keep it positioned within a setpoint. Thehorizontal controllers output was then transformed accordingto the rotation matrix (3), in order to control the quadrotoreven if the yaw angle was different from the initial one.

For the yaw-angle a P controller was deemed sufficient, asthe angle was kept close enough to the requested setpoint withonly a proportional controller.

3) Path planning: A simple time-based waypoint systemwas implemented, allowing for waypoints to be defined be-forehand and then followed by the quadrotor. The waypointsystem simply takes a vector of x, y, z, yaw and the timeto wait once the position has been attained. The waypointsare stored as matrix and whenever the quadrotor has held itsposition within a certain threshold of the current waypoint, thenext one is retrieved and fed to the positional controllers.

C. Results

The final PID parameters can be seen in table I. Thesewere tuned in order for a balanced performance capable ofboth quick outdoor movements and relatively precise indoormaneuvers.

The quadrotor achieved good reference tracking in the ydirection, see Fig. 12. However, despite lengthy attempts wewere unable to get the other positional controller in the x-direction to behave nearly as good. See Fig. 13.

0 10 20 30 40 50 60 70 80−1000

−500

0

500

1000

Time [s]

Positio

n [m

m]

Figure 12. Quadrotor (blue) following Y reference (dashed red). Initialdisturbance during takeoff around 5 s.

0 10 20 30 40 50 60 70 80−1000

−500

0

500

1000

Time [s]

Positio

n [m

m]

Figure 13. Quadrotor (blue) following X reference (dashed red)

Reference tracking of the simple proportional controllerfor the yaw can be observed in Fig. 14. The deviation fromsetpoint were observed to be at most 10◦ when the referencesignal was stationary, a deviation which was accepted and assuch no integral or derivative part was added.

The altitude controller can be seen functioning in Fig. 15.The integral part of the controller is the most significantcontribution since it corrects for the rapidly decreasing batterylevel, which requires a higher throttle signal to be sent in orderto achieve the same thrust.

0 10 20 30 40 50 60 70−100

−50

0

50

100

Time [s]

Angle

[degre

es]

Figure 14. Quadrotor (blue) responding to a series of steps in yaw angle(dashed red)

0 10 20 30 40 50 60 70 800

500

1000

1500

Time [s]

Altitude [m

m]

Figure 15. Quadrotor (blue) answering to altitude step (dashed red)

The waypoint system was used for testing real-world stepcharacteristics and the ability to fly in various geometricshapes such as circles and squares, as well as altitude changesin combination with large yaw-changes. Reference tracking ofa circle split into 40 waypoints can be seen in Fig. 16. Theflight was performed at an altitude of 1.3m. The radial RMSerror for one revolution was 124 mm, which is declared asadequate for the quadrotor’s purposes.

The yaw-angle was mostly left fixed during flights, as eventhough we could control the quadrotor in different yaw-angles,the performance of the controllers was affected with a changein yaw angle. As such, we tuned the controllers for one angleand let the quad be oriented towards that angle throughout itsflights.

−1000 −500 0 500 1000

−1000

−500

0

500

1000

X−position [mm]

Y−

po

sitio

n [

mm

]

Figure 16. Quadrotor (blue) following a reference circle (dashed red)


Table IFINAL PID PARAMETERS FROM THE LAB FOR THE VARIOUS

CONTROLLERS

X Y Height Yaw

Kp 23 20 20 1

Ki 2 1.5 7 -

Kd 20 20 7 -

VII. DISCUSSION

As could be seen in the simulations as well as in theexperimental results, the control strategy seems viable forpractical use. The quadrotor could follow simple paths withrelatively small error.

The assumption that the positional variables are decoupledand independent of each other when the attitude is horizontalis not entirely correct. Positional changes in the x and ydirection simultaneously showed signs of coupling between thecontrollers as the quadrotor took longer to reach its setpoint.The coupling between x and y in the lab can be explainedto some extent by the quadrotor in the lab not being assymmetrical as expected.

Compensations were included to make the positional con-trollers independent of the yaw angle. However the differencein output from the motors during a change in yaw causesthe quadrotor to deviate from its setpoint, which reveals thatadapting the positional controllers to be yaw angle independentis not enough to keep the quadrotor at its setpoint, at least notin the current experimental set up.

In the mathematical model, the x and y movements areaffected by the thrust from the motors, which degrades con-troller performance in simulations. The same phenomenonwas experienced during the practical implementation, as thequadrotor deviated from its positional set point during takeoff. Also, trying to move in the x or y direction resulted ina non horizontal attitude, which decreased the force availablefor lifting the quad. The altitude controller would then increasethe throttle in order to keep the quad at the same height, whichgave the positional controllers a quicker response, but the riskof overshoot also increased.

From both the simulation and the implementation one canobserve the pay-off for different controllers. Using a conser-vative set of controllers the quadrotor can closely follow itsgiven path, but it may take a long time. An aggressive setof controllers can follow the trajectory much faster, but withincreased error. The performance aspect that is valued higherdiffers for different applications and one should always weighthe speed against the precision of the movement in relation tothe given task.

Out of the four resulting controllers in the lab, we werepleased with the performance of three of them; altitude, yand yaw all tracked references good. However, we were notable to pinpoint the reason for why the x-controller never gotclose to the performance of the, in theory identical, controllerin the y-direction. Some ideas for why this happened is un-symmetrical arms and possible motor disparities, but therecould be many more reasons. The signal to be sent for both

the pitch and roll angles to keep the quadrotor level wasalso not exactly at the values where they should have beenaccording to the specifications. Once this was discovered thecontrollers had a much easier time at keeping the quadrotorin the desired position, but even with larger offsets sent forthe angle corresponding to the x-axis, the x-controller neverperformed as good as the y-controller.

A better trajectory planning algorithm could be built uponthe waypoint system, as the output of the trajectory planningwill in all cases be a couple of waypoints that the quadrotorshould follow. A very simple trajectory planning algorithm thatdoes not implement object detection and possible collisionswould be to simply split a provided path into shorter sub pathsthat ensure that the system is kept stable with less aggressivechanges from the positional controllers.

Finally, one should point out that the flight time of thequadrotor used in the SML is around five to ten minutes, andits usefulness in the smart building seems limited. However,the set up is just a prototype, and with a higher budget onecan construct a vehicle with longer flight time. Also, theperformance attained in the experiment seems enough for itto perform simple tasks, even though a prototype was used. Amore polished product should therefore be viable for use inor around the smart building.

VIII. APPLICATIONS

Since quadrotors, especially of this size and up, are quitenoisy, their use for the smart building is mostly outdoorswhere they could fly at an altitude high enough to decreasethe disturbance felt by people on the ground. With camerasmounted to quadrotors, they could be used outside as mobilesecurity cameras, tracking a target and transmitting video to anoperator. For example, in the case of a break-in, the quadrotorcould follow the villain and report position and video feedto local authorities. A UAV could vastly outperform a fixedcamera network for tracking a moving target, as it will beable to keep the target in view at all times, which wouldbe very expensive to do using stationary cameras. However,monitoring has ethical implications in case of misuse, such assomeone spying on innocent civilians via the quadrotor.

A possible new use of quadrotors could be to transportmail between smart buildings in a block. The postal companydelivers all the mail to a specified location in the block, fromwhere quadrotors then deliver it to the individual buildingsand drops it off for the individual floors in each building. Theimpact from this would either be time saving for the mailman,or make his service obsolete in case quadrotors could deliverthe mail all the way from the sorting facility to the block. Apractical way of delivering the mail needs to be implementedfor apartment buildings. The post can for example be deliveredto the roof and then transported through tubes in the buildingto the individual apartments, or the quadrotor delivers the mailto the individual apartments by itself.

Another possible new implementation could be miniaturefertilizer or pesticide dispensers for tiny farmlands (say thesize of a roof), where a tractor or airplane is too large. Aquadrotor could in a couple of passes do what would otherwiserequire manual labor due to space constraints.


Smaller quadrotors could be of greater use indoors, forexample to provide camera vision in a room where camerasare not usually mounted. This could be of use to assist elderlyin an emergency for when medical personnel needs vision onsight but are unable to transport themselves there, either dueto lack of resources or time.

Smaller quadrotors could as well be mounted with sensorsto capture various information about its surroundings, such astemperature and humidity and then fly through the building ingeneral in order to measure the required quantities.

IX. CONCLUSION

In this project a mathematical model of a quadrotor wascreated. It was then used as a base for simulations thatcontinued on into an experimental implementation. The ex-perimental implementation produced a control system capableof holding the quadrotor in a desired position, as well asa waypoint system and necessary safety features. In brief,the cascading control strategy could be implemented as acommercial product that have various applications in the smartbuilding.

X. FUTURE WORK

It would be interesting to have used only on-board compo-nents for controlling the quadrotor, as no other sensors thanthose needed to keep the quadrotor stable were mounted andinstalling other sensors was deemed to be out of the scope ofthis project. On board sensors that keep track of the quadrotorsposition is essential if the purpose is to fly outdoors, as thecamera system would not be available.

Future work done on quadrotors in the SML that utilizesLabVIEW could benefit from using our framework built uparound security and ease of use, in order to shorten develop-ment time. A more in depth theoretical study could also becarried out. The model and simulation can be improved by forexample including more disturbances. Such work could benefitfrom using the constructed simulation since it can easily beextended.

ACKNOWLEDGMENT

We would like to thank Kaveh Paridari, Carlo Fischione,Matteo Vanin, Diogo Almeida and the rest of the guys in thelab for their assistance during our project duration.

REFERENCES

[1] “Amazon prime air,” http://www.amazon.com/b?node=8037720011, on-line; accessed 2014-03-03.

[2] H. Almurib, P. Nathan, and T. Kumar, “Control and path planningof quadrotor aerial vehicles for search and rescue,” in SICE AnnualConference (SICE), Proceedings of, Sept 2011, pp. 700–705.

[3] S. Kota and M. Jamshidi, “Ats: Aerial tracking and surveillance system,”IEEE International Conference on System of Systems Engineering,SoSE, 2008. [Online]. Available: http://dx.doi.org/10.1109/SYSOSE.2008.4724193

[4] M. Vanin, “Modeling, identification and navigation of autonomous airvehicles,” Master’s thesis, KTH, Automatic Control, 2013.

[5] T. Jiinec, “Stabilization and control of unmanned quadcopter,” Master’sthesis, LTU, Department of Computer Science, Electrical and SpaceEngineering, 2011.

[6] Y.-S. Suh, “Attitude estimation using low cost accelerometer and gyro-scope,” in Science and Technology. Proceedings KORUS. The 7th Korea-Russia International Symposium on, vol. 2, July 2003, pp. 423–427vol.2.

[7] M.-H. Tsai, Y.-C. Liu, C.-M. Sun, C. Wang, and W. Fang, “Acmos-mems accelerometer with tri-axis sensing electrodes arrays,”Procedia Engineering, vol. 5, no. 0, pp. 1083 – 1086, 2010, eurosensorXXIV Conference. [Online]. Available: http://www.sciencedirect.com/science/article/pii/S1877705810008453

[8] M.-H. Tsai, Y.-C. Liu, and W. Fang, “A three-axis cmos-mems ac-celerometer structure with vertically integrated fully differential sensingelectrodes,” Microelectromechanical Systems, Journal of, vol. 21, no. 6,pp. 1329–1337, Dec 2012.

[9] “Analog devices ADXL330 datasheet,” http://www.analog.com/static/imported-files/data sheets/ADXL330.pdf, online; accessed on 2014-04-13.

[10] M. Armenise, C. Ciminelli, F. Dell’Olio, and V. Passaro, Advances inGyroscope Technologies. Springer, 2010.

[11] C. Acar and A. Shkel, MEMS Vibratory Gyroscopes: Structural Ap-proaches to Improve Robustness. Springer, 2008.

[12] X. Yan, C. Wenjie, and P. Wenhui, “Research on random drift modelingand a kalman filter based on the differential signal of mems gyroscope,”in Control and Decision Conference (CCDC), 25th Chinese, May 2013,pp. 3233–3237.

[13] Z. Diao, H. Quan, L. Lan, and Y. Han, “Analysis and compensationof mems gyroscope drift,” in Sensing Technology (ICST), SeventhInternational Conference on, Dec 2013, pp. 592–596.

[14] Y. Fang and B. Dai, “An improved moving target detecting and trackingbased on optical flow technique and kalman filter,” in Computer ScienceEducation. ICCSE ’09. 4th International Conference on, July 2009, pp.1197–1202.

[15] H. Chao, Y. Gu, and M. Napolitano, “A survey of optical flow tech-niques for uav navigation applications,” in Unmanned Aircraft Systems(ICUAS), International Conference on, May 2013, pp. 710–716.

[16] H. Chao, Y. Gu, J. Gross, G. Guo, M. Fravolini, and M. Napolitano,“A comparative study of optical flow and traditional sensors in uavnavigation,” in American Control Conference (ACC), June 2013, pp.3858–3863.

[17] J. H. Choi, D. Lee, and H. Bang, “Tracking an unknown movingtarget from uav: Extracting and localizing an moving target with visionsensor based on optical flow,” in Automation, Robotics and Applications(ICARA), 5th International Conference on, Dec 2011, pp. 384–389.

[18] M. V. Cook, Flight dynamics principles: a linear systems approach toaircraft stability and control. Butterworth-Heinemann, 2012.

[19] N. Apazidis, Mekanik II: Partikelsystem, stel kropp och analytiskmekanik. Studentlitteratur, 2012.

[20] F. Hoffmann, N. Goddemeier, and T. Bertram, “Attitude estimation andcontrol of a quadrocopter,” in Intelligent Robots and Systems (IROS),IEEE/RSJ International Conference on, Oct 2010, pp. 1072–1077.

[21] J. Orihuela Swartling, “Circumnavigation with a group of quadrotorhelicopters,” Master’s thesis, KTH, Automatic Control, 2014.

[22] N. Lundell, “A vision based on surveillance system using a wirelesssensor network,” Master’s thesis, KTH, Automatic Control, 2010.


APPENDIX ASIMULINK MODEL


APPENDIX BLABVIEW CODE

G5B. Automatic Control of a Quadrotor in the Smart Building753869/FULLTEXT01.pdf · gyroscopes that...

Documents

Transcript of G5B. Automatic Control of a Quadrotor in the Smart Building753869/FULLTEXT01.pdf · gyroscopes that...