Simulink-ROS software architecture to test the control system ...

Politecnico di Milano

SCHOOL OF INDUSTRIAL AND INFORMATION ENGINEERING

Department of Electronics, Information and Bioengineering

Laurea Degree Course in Automation and Control Engineering

Master Thesis

Simulink-ROS software architecture to test thecontrol system of an autonomous quadrotor

Advisor

Prof. Luca Bascetta

Co-advisor

Prof. Gian Paolo Incremona

Candidate

Lucca Corsi Silva

Thesis submitted in 2020

Abstract

This thesis describes the work done to develop a test platform for control

algorithms in quadcopters. The quadcopter chosen was the Parrot ArDrone 2.0

Elite, therefore, all the model parameters used are the ArDrone 2.0 ones. In order

to implement a low-level controller, the architecture available in the well-known

tum_simulator package, was reformulated in order to assign the motor velocities

directly, instead of the forces applied to the robot. Moreover, a controller was

implemented doing the time synchronization between Mathworks Simulink and ROS

Gazebo. The algorithm chosen was a Cascaded PID due to its simplicity compared

to other robust control algorithms, such as MPC or LQR. The controller, attached

to the new architecture, shows a very good result being able to follow a given spiral

trajectory.

i

Sommario

Questa tesi descrive il lavoro svolto per sviluppare una piattaforma di test

per algoritmi di controllo e pianificazione per quadricotteri. Il quadricottero scelto

come riferimento per lo sviluppo dell’architettura è stato il Parrot ArDrone 2.0

Elite, quindi tutti i parametri del modello utilizzati nella piattaforma sono quelli di

ArDrone 2.0. Per implementare un controllo di basso livello, l’architettura presente

nel noto pacchetto tum_simulator è stata riformulata in modo da poter assegnare

direttamente le velocità dei motori, invece delle forze applicate al quadricottero.

Inoltre, è stato implementato un controllo che esegue la sincronizzazione temporale

tra Simulink e ROS Gazebo. L’algoritmo di controllo scelto è un Cascaded PID per

la sua semplicità rispetto ad altri algoritmi di controllo robusto, come MPC o LQR.

Tale controllore, inserito nella nuova architettura, presenta degli ottimi risultati

permettendo al drone di seguire una traiettoria spiraliforme data.

iii

Contents

Abstract i

Sommario iii

1 Introduction 1

1.1 Unmanned Aerial Vehicles History . . . . . . . . . . . . . . . . . . . 1

1.2 State of the Art of UAV . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.2.1 Classifying according to Weight . . . . . . . . . . . . . . . . . 4

1.2.2 Classification according to the Wing Type . . . . . . . . . . . 4

1.2.3 Quantity of Rotors . . . . . . . . . . . . . . . . . . . . . . . . 5

1.2.4 Quadrotor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

1.3 Control Strategies for Quadrotors . . . . . . . . . . . . . . . . . . . . 6

1.3.1 Control System . . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.3.2 PID Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

1.4 Thesis Objectives and Motivation . . . . . . . . . . . . . . . . . . . . 8

1.5 Thesis Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

1.6 Thesis Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

v

vi CONTENTS

2 Robotics Operational Systems (ROS) and Setup 11

2.1 ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.1.1 Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

2.2 Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

2.2.1 Parrot ArDrone Elite 2.0 . . . . . . . . . . . . . . . . . . . . 13

2.2.2 Dell Inspirion 15 5000 . . . . . . . . . . . . . . . . . . . . . . 14

2.3 ArDrone Package . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

2.3.1 Issues with the Package . . . . . . . . . . . . . . . . . . . . . 15

2.3.2 Adaptation to ROS Kinetic . . . . . . . . . . . . . . . . . . . 16

3 Architecture of theArDrone Package 17

3.1 ROS Package Structure . . . . . . . . . . . . . . . . . . . . . . . . . 18

3.1.1 Code Structure . . . . . . . . . . . . . . . . . . . . . . . . . . 19

3.2 Architecture Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.2.1 rqt_graph . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

3.2.2 C++ code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

4 New Architecture 25

4.1 Quadcopter’s Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 25

4.2 Architecture Remodeling . . . . . . . . . . . . . . . . . . . . . . . . . 27

4.2.1 Package’s Plugins . . . . . . . . . . . . . . . . . . . . . . . . . 28

4.2.2 URDF files . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

CONTENTS vii

4.2.3 Code adaptation . . . . . . . . . . . . . . . . . . . . . . . . . 29

4.2.4 rqt_graph of the New Structure . . . . . . . . . . . . . . . . . 31

5 Control: ROS and Simulink 33

5.1 MathWorks Simulink Tool . . . . . . . . . . . . . . . . . . . . . . . . 34

5.2 Time Synchronization: ROS and Simulink . . . . . . . . . . . . . . . 34

5.2.1 Computational Power . . . . . . . . . . . . . . . . . . . . . . 36

5.2.2 ROS Time Model Stepping . . . . . . . . . . . . . . . . . . . 36

5.3 Control Strategy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

5.3.1 Control Scheme . . . . . . . . . . . . . . . . . . . . . . . . . . 38

5.3.2 Control Algorithm in Simulink . . . . . . . . . . . . . . . . . 38

5.3.3 Control and Model Parameters . . . . . . . . . . . . . . . . . 43

5.3.4 Project Repository . . . . . . . . . . . . . . . . . . . . . . . . 43

6 Simulation Results and Analysis 45

6.1 New Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45

6.2 Time Synchronization Evaluation . . . . . . . . . . . . . . . . . . . . 47

6.3 Control Evaluation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

7 Conclusions 53

Bibliography 55

List of Figures

1.1 Ryan 147, AQM-34 "Lightning Bug" and the family of Teledyne Ryan

Aeronautical UAVs (source: http://blog.udn.com/TUANPETERSON) 3

1.2 Classification regarding the mass [1] . . . . . . . . . . . . . . . . . . 4

1.3 Different types of MAVs, (a) fixed wing, (b) flapping wing, (c) fixed/flapping-

wing, (d) rotary wing, (e) VTOL, (f) ducted fan, (g) tilt-rotor, (h)

helicopter, (i) unconventional, (j) ornicopter [1]. . . . . . . . . . . . 5

1.4 Different types of rotary wing MAVs, (a) mono-copter, (b) twin-copter,

(c) tri-copter, (d) quad-copter, (e) penta-copter, (f) hexa-copter, (g)

octocopter, (h)deca-copter, (i) dodeca-copter [1]. . . . . . . . . . . . 6

1.5 Closed Loop System. Source: author . . . . . . . . . . . . . . . . . . 7

1.6 PID Controller Loop. Source: author . . . . . . . . . . . . . . . . . . 8

2.1 TUM simulator scheme. [2] . . . . . . . . . . . . . . . . . . . . . . . 15

2.2 ROS distributions overtime. [3] . . . . . . . . . . . . . . . . . . . . . 16

3.1 ROS package structure. Source: https://developpaper.com/study-

notes-of-autolabor-2-5-3-working-space-and-compiling-system-of-ros/ 19

3.2 C++ code structure scheme. Source: author . . . . . . . . . . . . . . 20

ix

x LIST OF FIGURES

3.3 rqt_graph generated from the ArDrone package without any modifi-

cations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

3.4 Screenshot of the experiment realized with the real-life drone. . . . . 23

3.5 Code’s lines where the force and torque are assigned instead of motor

velocities. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

4.1 URDF plugin file modification. . . . . . . . . . . . . . . . . . . . . . 29

4.2 Modification on the C++ code to subscribe to the new topic created. 29

4.3 Adaptation on the C++ code to execute the callback using the new

created topic. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

4.4 Callback function. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

4.5 Quadcopter’s dynamic equations. . . . . . . . . . . . . . . . . . . . 30

4.6 Motor commands being stored on ardrone/navdata topic. . . . . . . 31

4.7 Parameters from the URDF file being stored in a C++ variable. . . 31

4.8 rqt_graph of the new architecture. . . . . . . . . . . . . . . . . . . . 32

5.1 General system architecture scheme. (a): ROS provides the structure

of the nodes, sensor and plugins to be executed in the Gazebo Sim-

ulation. (b): Gazebo Simulation sensor’s data used in Simulink as

the controller input. (c): Output of both controllers published to the

ROS nodes (motor velocities and position references). . . . . . . . . . 34

5.2 Gazebo GUI during UAV simulation . . . . . . . . . . . . . . . . . . 35

5.3 Cascade PID scheme to roll and X references [4]. . . . . . . . . . . . 37

5.4 Cascade PID scheme to pitch and Y references [4]. . . . . . . . . . . 37

LIST OF FIGURES xi

5.5 Cascade PID scheme to Z reference [4]. . . . . . . . . . . . . . . . . . 38

5.6 Simulink: UAV control system. . . . . . . . . . . . . . . . . . . . . . 39

5.7 Trajectory Control subsystem . . . . . . . . . . . . . . . . . . . . . . 40

5.8 Control subsystem . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

5.9 U to Omega Square subsystem . . . . . . . . . . . . . . . . . . . . . 42

5.10 MATLAB conversion function code . . . . . . . . . . . . . . . . . . 42

5.11 UAV model parameters set in the Simulink controller . . . . . . . . . 42

5.12 Control parameters set in the Simulink controller . . . . . . . . . . . 43

6.1 Proportional altitude controller. . . . . . . . . . . . . . . . . . . . . . 46

6.2 MATLAB code to assign motor velocities . . . . . . . . . . . . . . . 46

6.3 Motor velocities being assigned . . . . . . . . . . . . . . . . . . . . . 46

6.4 Important nodes in time synchronization test. . . . . . . . . . . . . . 47

6.5 Altitude control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

6.6 Control position response . . . . . . . . . . . . . . . . . . . . . . . . 49

6.7 (X,Y,Z) response separately . . . . . . . . . . . . . . . . . . . . . . . 50

6.8 (X,Y,Z) response in space . . . . . . . . . . . . . . . . . . . . . . . . 51

List of Tables

2.1 Aerodynamic Parameters [5] . . . . . . . . . . . . . . . . . . . . . . . 14

2.2 Physical Parameters [5] . . . . . . . . . . . . . . . . . . . . . . . . . 14

4.1 Quadcopter dynamics equations notation . . . . . . . . . . . . . . . . 27

5.1 Gazebo parameters meaning . . . . . . . . . . . . . . . . . . . . . . . 36

6.1 Time synchronization evaluation . . . . . . . . . . . . . . . . . . . . 48

xiii

Chapter 1

Introduction

Currently, Unmanned Aerial Vehicles, also commonly denominated by its

acronym UAVs, are more used than ever in the history of our society. The importance

of this kind of vehicles increased over the time as its use became more common. A

great feature that UAVs brought us was the possibility to avoid putting human life

in high risk situations. Many of those activities, in the last decades of human history,

were handled by aerial systems that work autonomously [6].

Considering the growing importance of the UAVs in our life, this chapter

is dedicated to explain the history and the types of these vehicles in special the

quadrotor - the one used on this thesis work. Moreover, a section dedicated to control

strategies on UAVs will be also present. Finally, the objectives and motivations of

the thesis are reported.

1.1 Unmanned Aerial Vehicles History

Since its very beginning, UAV have tended to be driven by military applications.

As said before, the possibility to perform an attack with no risks to the soldiers of an

army grew a huge interest on these type of vehicles. Clearly it is undeniable that the

military area had a great impact on the technological field over human history[6].

1

2 CHAPTER 1. INTRODUCTION

One could say that the very first UAV was a stone thrown by a caveman in

man history early times, others, a Chinese rocket launched in the thirteenth century.

That is why it is very important to have a clear definition for these kind of vehicles.

If we define UAV as vehicles that generate aerodynamic lift including a medium

control of it, kites would perfectly fit in this definition [6].

In 1883, Douglas Archibald using an anemometer, to measure the wind velocity,

and a camera took hundreds of photographs during the Spanish-American war, which

may have been one of the first uses of UAVs in combat [6].

Not much time later, in the World War I, Charles Kettering developed a

biplane UAV used by the Armed Signal Corps. It could fly nearly 40 miles per hour

and carry 180 lbs of explosives. However, these vehicles did not have sophisticated

controls systems and crashes were very commons. The truly successful era for the

UAVs usage was not before the Vietnam-War [6].

With the start of the Cold War, UAVs began to be used as Intelligence,

Surveillance and Reconnaissance (ISR) systems, the Vietnam War was America’s

first conflict that counted on an extensive use of pilotless aircraft. There were more

than 3000 operational reconnaissance missions between 1964 and 1975 [6]. Due to the

extent of enemy anti-aircraft fire in Vietnam, UAVs were often used to take photos

from low and high altitude which could in turn be handled to strike plannings [6].

In Figure 1.1 we can observe that the vast range of models that were developed

and used in the military missions by the United States of America during this period.

Still, in the next decade after the war, the funding for the UAV programme was

scaled down, and the development of this kind of vehicles remained in the background

for a while.

However, some aircraft already built, to be more precise "the Pioneer", were

used during the Persian Gulf operations between 1990 and 1991. "The Pioneer"

flew 300+ combat reconnaissance missions and received extensive acclaim for its

performance [6].

1.2. STATE OF THE ART OF UAV 3

Figure 1.1: Ryan 147, AQM-34 "Lightning Bug" and the family of Teledyne Ryan Aeronau-tical UAVs (source: http://blog.udn.com/TUANPETERSON)

With the propulsion of this area by the military missions, the UAVs started

to be used also in domestic field, due to this a whole new discussion about safety

and privacy was raised. However, as showed before, pilotless aircraft brought to

the world a whole new range of possibilities. UAVs used for surveillance, disaster

exploration, such as fire, flood and earthquakes, monitoring leakage of hazardous

materials, search and rescue missions are some of the extent usage of these vehicles

nowadays [7]. This is one of the reasons why this topic brings a great interest to be

studied.

1.2 State of the Art of UAV

Considering that UAVs are relatively new in our world, many categories were

raised to classify them. In order to understand the type of the vehicle used in

our study, it is suggested a narrowing classification based on groups. Therefore, a

characteristic is placed under a classification parameter, then another aspect of the

drones that attend that first parameter is analyzed and so on. The idea here is

to establish a "funnel classification". The first group of classification is related to

the weight, then on the wing type and finally, we classify them by the quantity of


Figure 1.2: Classification regarding the mass [1]

rotors.[1].

1.2.1 Classifying according to Weight

It is important to highlight here the classification is restricted to MAVs (Micro

or Miniature Air Vehicles), where the weight is limited by the range from 50 g to 2

kg and its length is smaller than 100 cm [1]. Other types of the so called air drones,

such as UAV, microUAV, NAV, PAV and SD are not taking into account. In order to

simplify the terminology, the acronym UAV will be used in further chapters instead

of MAV [1].

1.2.2 Classification according to the Wing Type

Miniature Air Vehicles can be considered as Fixed Wing, Flapping Wing,

Fixed/flapping wing, Rotatory Wing, VTOL (vertical take off landing), hybrid model

(such as tilt-rotor and ducted fan), helicopter, unconventional and the so called

ornicopter as showed in Figure 1.3. In spite of these several types of taking off way,

the focus will be on the rotatory wing vehicles.

Rotatory Wing MAVs

This is one of the most famous types of MAVs. Rotatory wing are the well-

know drones. These kind of vehicles can hover and have a high maneuverability,

having rotatory blades or propeller-based systems they can fly in all directions

(horizontally and vertically) and hover in a fixed position[6].

1.2. STATE OF THE ART OF UAV 5

Figure 1.3: Different types of MAVs, (a) fixed wing, (b) flapping wing, (c) fixed/flapping-wing,(d) rotary wing, (e) VTOL, (f) ducted fan, (g) tilt-rotor, (h) helicopter, (i) unconventional,(j) ornicopter [1].

1.2.3 Quantity of Rotors

Into this category of MAVs (rotatory wing), there is a sub-classification based

on the quantity of rotors present on the vehicle. As showed in Figure 1.4, the number

of motors and blades is from one up to twelve.

1.2.4 Quadrotor

Finally, after several classifications we get to the Unmanned Aerial Vehicle

used in this thesis, the quad-copter, also called quadrotor. This kind of vehicles are

used on missions that require hovering flight. Rotatory wings with four rotors have

some unique abilities such as high maneuverability, small size and easy control.[8]

Their most significant applications outside the military field are search-and-rescue

and emergency response.


Figure 1.4: Different types of rotary wing MAVs, (a) mono-copter, (b) twin-copter, (c)tri-copter, (d) quad-copter, (e) penta-copter, (f) hexa-copter, (g) octocopter, (h)deca-copter,(i) dodeca-copter [1].

1.3 Control Strategies for Quadrotors

As imagined, each type of UAV has your particular control technique. De-

pending on the takeoff way, number of rotors, weight and so on, several different

approaches specially selected to contemplate these specifications were developed.

However, as the focus of this thesis study is quadcopters, the next sections will fall

over in a control strategy designed specially to this kind of UAV.

1.3.1 Control System

Firstly, it is important to give a background on this field of study, defining

what is a control system and what a control system exactly does. First thing first, a

control system is a an arrangement or a combination of several different components

that work together to arrange or regulate a system to achieve a desired outcome.[9]

Knowing this basic concept, in the Figure 1.5 is show the basic structure of control

systems. The input signal r(t) is also called as reference signal, which is basically the

set-point that you want to achieve in your system. In a control system of this type

(closed-loop system), the output signal c(t) is used in order to compare the actual

response of the loop to the desired set-point[9]. Therefore, we have the concept of

1.3. CONTROL STRATEGIES FOR QUADROTORS 7

Figure 1.5: Closed Loop System. Source: author

the error signal. The error e(t) is given by the following equation:

e(t) = r(t)− c(t) (1.1)

To make a long story short, the main objective of a control system is to get

the lowest possible error signal, by matching the output signal c(t) with the reference

signal r(t). However, to do that there are several different approaches and techniques,

one of them is the well-known PID Control systems.

1.3.2 PID Control

The Proportional-Integral-Derivative (PID) control offers the simplest and

yet most efficient solution to many real-world control problems [10]. The main idea

of this controller is to use not just the error as a input into the system, but also its

derivative and integrative response.

Being the most used control strategy nowadays, this controller provides us

a simple implementation and a good response [11]. The basic structure of the PID

control is given by the following equation:

G(s) = Kp +Ki1

s+Kds (1.2)

Where Kp is the proportional gain, Kd is the derivative gain and finally Ki


Figure 1.6: PID Controller Loop. Source: author

is the integrative gain.

During the evolution of this thesis study, an analog approach to the PID

controller will be used and implemented on the UAV. This approach is called Cascade

PID strategy, a controller commonly used to stabilize quadrotors [11].

1.4 Thesis Objectives and Motivation

Unmanned Aerial Vehicles have been the center of several studies all over

the world, and probably will still be for many other years. This area has a great

potential power to improve people’s life, using technology to make distances and

travel time shorter. Though, using this kind of vehicles for both load transportation

and rescue missions, represents a risk. Because it is an aerial vehicle, it can cause

great disasters. It is harder to control and at the same time the impact of a failure

is much more dangerous than a ground vehicle failure impact.

Therefore, the goal of this work is to develop a safety environment that enables

the evaluation of control algorithms using simulation tools.

This Master thesis comprehends a complete engineering project from the

selection of models to adaptation of algorithms and evaluation of results. The main

objectives are:

1.5. THESIS PROBLEMS 9

• to build a platform to test control algorithms in quadcopters;

• to design an architecture able to assign directly motor velocities (low level

control);

• to make possible to define the trajectory to be followed by the quadcopter in

space.

1.5 Thesis Problems

Unmanned aerial vehicles are a trending topic after the release of the so-

called commercial drones due to its fast way of solving problems [7]. Monitoring

deforestation areas, fighting fire in hard-to-reach locations or mapping regions has

never been easier. This technology is a game-changer in the world that we live.

During the progress of this project, some problems were arisen. It was

necessary to use a previous stable model as a base of study, since the development of

a whole architecture of a model simulator is a very complex task to be solved in a

Master’s Thesis. Many issues related to the version of the Operating System and the

packages were faced. Moreover, other two problems were detected: the package used

did not have the structure desired to implement the low-level control, as it was not

possible to assign motor’s velocities directly through the original files, and the time

synchronization between the Gazebo simulator and the controller was very difficult

to implement.

The first problem was solved doing a downgrade of the Linux Ubuntu to

the 16.04 LTS version. The assignment of motor velocities was fixed adapting the

original code, restructuring its architecture. Finally, the time synchronization issue

was solved lowering the computational effort made by the laptop used. This is just

an overview of every problem and solution faced during the development of this

Master’s Thesis project, a deeper explanation of each issue and its solution will be

presented in details in future chapters.


1.6 Thesis Results

After the development of the thesis, I was able to restructure the architecture

of the simulator to fulfilled our need for the project goals. With the modification

of the code, it is now possible to assign the motors velocities directly. Moreover, a

cascade PID controller was successfully implemented enabling the stabilization of

the UAV in air and also the tracking of a predetermined trajectory.

A future expansion of this project could be easily done, as the ROS package

used in the thesis was originally developed for real-life applications with the Parrot

ArDrone 2.0. Therefore, all the controllers designed and tested on the simulator

could be also used on the real drone.

Chapter 2

Robotics Operational Systems

(ROS) and Setup

To fully understand the use of Robot Operating System (ROS) in this thesis,

here we will define what an Operating System is. An Operating System (OS) is a

collection of softwares that controls the raw computing power of the hardware of the

computer. It decides how to allocate the computer resources, such as, CPU, main

memory, input-output devices, communication and data. Also, it provides a user-

friendly interface for the execution of application programs. An OS is the manager of

the machine and defines how the user sees and interacts with it [12]. In spite of the

name, ROS is not exactly an OS, it doesn’t go so deep on the computer processes

and memory allocation part, but concentrates more in the drivers compatibility and

state-of-the-art algorithms previously developed. In reality, ROS is a set of software

libraries and tools that help us build robot applications.

2.1 ROS

ROS is a Linux-based framework for modular use in robotic applications.

It was originally designed by Willow Garage and currently maintained by Open

11

12 CHAPTER 2. ROBOTICS OPERATIONAL SYSTEMS (ROS) AND SETUP

Source Robotics Foundation. This is a powerful tool because uses Object-Oriented

Programming (OOP), which is a method of programming that organizes software

design around data or objects, rather than functions and logic [13].

As said above, it works exactly as a regular Operating System present on

a personal computer. The difference is: instead of applications ROS programs

allow a user to control the mobile operations of a robot. ROS simplifies the task of

creating complex and robust robot behavior due to its collection of tools, libraries

and conventions [13].

2.1.1 Definitions

Software in ROS is distributed in packages and stacks of packages. A package

is a directory with a specific structure, which can contain libraries, executables,

configuration files and so on. To better understand this structure, main concepts

should be presented, such as nodes, messages, topics and bags [3].

Nodes

A node is basically a program (process) being executed. As defined by ROS

wiki, "node is a process that performs computation". It is an executable program

running inside the application. Nodes are combined into a graph and communicate

with each other using ROS messages through topics and services [3].

Messages

ROS uses a simplified message description language for describing the data

values published by a ROS node. Messages are exactly those data values. Messages

descriptions are stored in files located on a subdirectory of a ROS package [3].

2.2. SETUP 13

Topics

The concept of topic is very simple: topics are named buses over which nodes

exchange messages. In other words, topics in ROS enable the data to be exchanged

between nodes. A topic can have publishers and/or subscribers, where the first send

messages trough the topic and the last one receives those messages [3].

Bags

Last but not least, a bag is a file format in ROS for storing ROS message

data. They offer a convenient way to store all kinds of data during an execution.

The main analysis are made using ROS bags stored data [3].

2.2 Setup

The UAV simulated for this thesis was a Parrot ArDrone Elite 2.0. The

architecture design and all tests performed were made on a Dell Inspirion 15 5000

personal computer.

2.2.1 Parrot ArDrone Elite 2.0

In this section, the focus of the study is the availability of information to make

possible the control evaluation. Therefore, the most important aspects of the study

is the quantity of sensors, the aerodynamics parameters and the physical parameters

to be possible to create a reliable model. The ArDrone 2.0 includes an Inertial

Navigation System (INS) which contains a 3 axis accelerometer, 3 axis gyroscope and

3 axis magnetometer. To determinate the height during fly, the UAV uses a pressure

sensor and a ultrasound for ground altitude measurement [14]. Thus, relying on

these resources provided by sensors we can determine the velocity and acceleration

on the 3 axis (X,Y,Z) of the aircraft and the height in which the drone is currently


flying. In the other hand, the aerodynamic and physical parameters are defined by

Table 2.1 and Table 2.2.

Table 2.1: Aerodynamic Parameters [5]

Parameter Value Information

Jxx 0.0086 [kgm2] CalculatedJyy 0.0086 [kgm2] CalculatedJzz 0.0172 [kgm2] Calculatedb 3.13e-5 Assumedd 7.5e-7 AssumedJr 6e-5 [kgm2] Assumed

Table 2.2: Physical Parameters [5]

Parameter Value Information

m 0.38 [kg] Massl 0.17m [kgm2] Distance between rotor and center of massΩ 28500 [rpm] Max angular speed of the rotor -inrunner -14.5Vη 1/8.75 Ratio of angular speed propeller to rotorβ 8x3.8 Diameter x stroke of propeller

Considering all this information, it is possible to create a model.

2.2.2 Dell Inspirion 15 5000

All the simulations were performed using a Dell Inspirion 15 5000. The laptop

has a great computational power containing an Intel I7-8550 1.8 GHz quad-core

processor and 16 GB of RAM. Also, the computer have a dedicated graphical card, an

NVIDIA GeForce MX150 with 4GB DDR5. To be able to perform all the simulations

with ROS, a Linux Ubunutu 16.04 was installed alongside with Windows 10 (dual-

boot). To execute all the evaluations and analysis of the results MATLAB/Simulink

was used, and all the simulations were done in Gazebo software.

2.3. ARDRONE PACKAGE 15

Figure 2.1: TUM simulator scheme. [2]

2.3 ArDrone Package

During the development of this thesis, several ROS packages were tested until

we found the one that fulfills our interests. After some tests we were between two

possibilities: the Hector Qaudrotor[15] and the one made by the Technical University

of Munich (TUM) [2], the most famous ROS packages designed for quadrotors.

The package selected is the TUM Simulator, due to its adaptability and overall

because this package is already made using the Parrot ArDrone 2.0,the evaluated

controls algorithms could be easily applied into the real drone, as showed in Figure

2.1. This package has been written by Hongrong Huang and Juergen Sturm of the

Computer Vision Group at the Technical University of Munich and it is based on

another ROS package also developed in Germany called tu-darmstadt-ros-pkg [16].

2.3.1 Issues with the Package

Besides the ease to use the TUM Simulator in the project, this package showed

some issues to fully fill our requirements. The first big problem was related to the

version. The last update by the original developers was made in 2014 [2], therefore

the package was not plug-and-play with the currently Operating Systems and ROS

versions. The second big problem was related to its architecture, which will be

presented in deep details in the following chapter.


Figure 2.2: ROS distributions overtime. [3]

2.3.2 Adaptation to ROS Kinetic

In spite of the version problem, it was possible to find an adaptation of the

TUM Simulator package to ROS Kinetic Kame in GitHub[17]. To give a background,

all the ROS distributions are illustrated in Figure 2.2.

Chapter 3

Architecture of the

ArDrone Package

After the selection of the ROS Package, a deep analysis in the code architecture

has been made in order to verify if the need of the project is fulfilled. It is important

to highlight here that to fulfill our needs in this project the architecture should have

some features, as:

1. be possible to assign motor velocities directly;

2. be plug-and-play to the Parrot ArDrone 2.0 model;

3. be physically reliable.

The first feature is important to be able to set a low level control. This means

that our controller should have access to the root level of the drone, being possible

to modify parameters that affects directly the behaviour of the UAV. To clarify this

abstract concept, an example is provided: while a low-level control sets directly each

motor’s velocity, a high-level control would set the acceleration/velocity of the whole

UAV in X,Y and Z coordinates. The main difference here is the level of autonomy

that the control has in the system. Setting up each motor speed represents a much

higher autonomy than setting up the whole UAV velocity/acceleration, because in

17

18 CHAPTER 3. ARCHITECTURE OF THE ARDRONE PACKAGE

this last hypothesis we have a software underneath the control system that defines

the behaviour that the UAV should have to reach the desired velocities/accelerations.

The second feature is related to the evolution of this thesis work. If the

architecture defined by the ROS package is exactly the same one that is used on

the Parrot ArDrone 2.0, we can easily perform real-life tests previously evaluated by

the simulation. Last but not least, the third feature is related to the quality of the

simulator. It must have a strong physical basis to be reliable to real-world situations.

3.1 ROS Package Structure

In order to understand better this chapter, it is important to state what a

ROS Package is exactly. A ROS package is a compiled of code files that works

together following a determined structure. In this case, the code files are in the C++

programming language. Those files are linked in an specific order defined by the

CMake files - as described by [18]: "CMake is an extensible, open-source system that

manages the build process in an operating system and in a compiler-independent

manner".

Therefore, underneath the ROS structure itself (the nodes, topics and mes-

sages) we have to keep in mind also that the code files have their own architecture.

To illustrate this relationship between .cpp files - those written in C++ language,

and the ROS structure itself - Figure 3.1 is provided. Although, before to get deeper

in the C++ code structure, it is important to present the ROS package under study

general structure. The tum_simulator package presents four folders which each of

they play a fundamental role in the execution of the simulation [17]:

• cvg_sim_gazebo : contains object models, sensor models, quadrocopter models,

flying environment information and individual launch files for each objects and

pure environment without any other objects.

• cvg_sim_gazebo_plugins : contains gazebo plugins for the quadrocopter model.

quadrotor_simple_controller is used to control the robot motion and qaudro-

3.1. ROS PACKAGE STRUCTURE 19

Figure 3.1: ROS package structure. Source: https://developpaper.com/study-notes-of-autolabor-2-5-3-working-space-and-compiling-system-of-ros/

tor_state_controller delivers navigation information, such as: /ardrone/nav-

data. Others are plugins for sensors in the quadrocopter, such as barometer

sensor.

• message_to_tf : it is a package used to create a ros node, which transfers the

ROS topic /ground_truth/state to a /tf topic.

• cvg_sim_msgs : contains message forms for the simulator.

Here it is possible to observe that the part of the package that controls the UAV

is the folder cvg_sim_gazebo_plugins, as presented on its description. Therefore, if

we are interested on how the quadrotor behaves, we have to do an analysis on this

folder code structure.

3.1.1 Code Structure

After state the structure of the ROS package, we can go deeper into the code

architecture. In other words, how does the .cpp files work together in the simulation

to control the behaviour of the UAV.

A scheme is provided in Figure 3.2 to illustrate the architecture of the code on

the package. There are two entities that have a great impact on the behaviour of the

drone. First, the so-called Simple Controller, performs all the calculations to stabilize

the UAV on air. This entity is responsible to provide the control of the quadcopter


Figure 3.2: C++ code structure scheme. Source: author

defining how the drone should behave in order to hover or execute commands passed

by the joystick. All the actions are passed to the simulation and then the second

entity,the so-called State Controller, reads the information from the sensors and

generates all the information provided on a topic called /ardrone/navdata, presentend

on Figure 3.3.

This is the basic structure that works behind the tum_simulator ROS package.

In further sections will be presented a deeper analysis both in the package itself and

in the code structure.

3.2 Architecture Analysis

3.2.1 rqt_graph

To begin the analysis of the ArDrone ROS package, all the files were down-

loaded and executed in their original format. The first step was to generate the

rqt_graph - a ROS tool that shows all the interactions between the nodes and

messages - to check how the package was structured. It is important to highlight here

the taxonomy used by rqt_graph tool: the arrows represent interactions (messages

sent or received), the ellipses represent the nodes and the boxes represent the topics.

The name of the topic or nodes are written inside of their respective structure adopted

by the tool.

In Figure 3.3 it is possible to draw the first conclusions. The ArDrone UAV

is represented by the big square on the middle, which contains several topics. The

3.2. ARCHITECTURE ANALYSIS 21

Figure 3.3: rqt_graph generated from the ArDrone package without any modifications


entity ardrone has all the important information separated in those topics, in other

words, all the data generated by the sensors and the commands available (land,

take off, and reset) are separated in topics. An interesting feature that we can

see by the rqt_graph is the availability of the cameras images, represented by the

/ardrone/front/camera_info and /ardrone/bottom/camera_info topics. Also, the

ultrasound sensor to estimate the altitude of the UAV is represented outside of the

ardrone entity, because the design considers this sensor as an external source of

information, that is, as the sensor is placed outside of the drone body, on the diagram

it is also placed outside of the ardrone square.

Other arrangement that it is possible to observe is the joystick node. Repre-

sented by the three blocks (/joy_node, /joy and /ardrone_joystick) it is interesting

to see that the package has a joystick implemented to control the drone by commands.

A critical information can be extracted from this structure: the joystick’s commands

acts on the topic /cmd_vel and not on the ardrone itself. Therefore, this represents

an evidence that probably the velocities of each motor cannot be assigned directly to

the UAV. Although it is not a solid proof of that, a deeper analysis in the C++ code

is needed to state this.

All the actions taken by the drone are passed to Gazebo (/gazebo) that

simulates the behaviour of the UAV. Here a parenthesis is made to the ground truth

topic: it validates the data information acquired by the use of sensors. It is an

evidence of a good physical base for the UAV model under consideration. Therefore,

beforehand, we have to keep in mind that maybe this ROS package does not attend

all the goals desired to the project. As the joystick controller acts directly on the

topic /cmd_vel instead of an entity of the /ardrone, the architecture seems to not

matches the first goal: assign motor velocities directly. Moreover, about the second

feature nothing can be concluded for now. A test to check this capabilities is still

needed.

To check the second feature needed to the project an experiment was realized,

using the real-life Parrot ArDrone 2.0 and this package, it could be proved that

the architecture matches perfectly the second requirement: be plug-and-play to the

3.2. ARCHITECTURE ANALYSIS 23

Figure 3.4: Screenshot of the experiment realized with the real-life drone.

drone. As showed in Figure 3.4, it is possible to observe the ROS package retrieving

the camera image and the sensors data. Moreover, a rqt_graph is provided and the

structure it is exactly the same of the one showed in Figure 3.3. Thus, the second

requirement is fulfilled.

3.2.2 C++ code

In a deeper analysis of the architecture of the ArDrone ROS Package, the

question "Is it possible to assign motor velocities directly?" was answered. The short

answer is: no, however an adaptation of this structure could be performed.

First things first, analyzing the C++ code present on the Simple Controller

entity it is possible to note that there is no motor velocity assignment in any line of


Figure 3.5: Code’s lines where the force and torque are assigned instead of motor velocities.

the code. The controller consists of setting the forces and torques to the whole robot,

instead of motor’s speed, as showed in Figure 3.5. All the torques (X, Y and Z) are

set up considering the response of the controller and just the force in the Z-axis is

established. This happens because to control the UAV hovering movement, that is

its stabilization in the air, only the torque is needed. The force in the Z-axis just

determines the drone’s altitude.

However, and adaptation to this structure could be placed in a way that the

assignment of the motor speed would be possible. This modification will be explained

in details in the next chapter.

Chapter 4

New Architecture

With the previous explanations about the architecture of the ROS package,

the current situation is: the package does not have access to the motor velocities,

therefore a low-level control would be not possible because the plugin that controls

the UAV behaviour sets force and torque of it instead of motor speed, what is not

reliable in real-world. Thus, the solution to this issue was to modify the plugin’s

code in order to accept motor velocities and then convert it into torque and force of

the robot, as it is done in real UAVs.

4.1 Quadcopter’s Dynamics

In order to make possible the conversion from motor velocities to torque and

force, the model dynamics needs to be introduced. In a quadcopter, as it have four

motors, all the dynamic equations are dictated by these four components. As said

before, the only component of the force that is set is on the Z-axis to determine the

altitude of the drone. Therefore, we will have four input equations: X torque, Y

torque, Z torque and Z force, as described bellow.

U1 =

4∑i=1

bΩ2i

25

26 CHAPTER 4. NEW ARCHITECTURE

U2 = bl(Ω24 − Ω2

2)

U3 = bl(Ω23 − Ω2

1)

U4 = d(−Ω21 + Ω2

2 − Ω23 + Ω2

4),

where, U1 represents a force in the motor’s axis direction, U2 the roll torque,

U3 pitch torque, U4 yaw torque and Ωi the ith-motor velocity. The parameters b, l

and d were previously explained in Table 2.2.

These last four equations will be the ones used in the code adaptation, however,

for the sake of completeness the whole model equations and hypothesis assumed will

be reported bellow.

x =

4∑i=1

bΩ2i

cosψcosφsinθ + sinφsinψ

m

y =4∑i=1

bΩ2i

cosψcosφsinθ − sinφsinψm

z =4∑i=1

bΩ2i

cosθcosφ

m− g

p =Iy − IzIx

qr +bl

Ix(Ω2

4 − Ω22)−

JmIxqΩR

q =Iz − IxIx

pr +bl

Iy(Ω2

3 − Ω21) +

JmIypΩR

r =Ix − IyIz

pq +d

Iz(−Ω2

1 + Ω22 − Ω2

3 + Ω24)

The notation is explained in Table 4.1. The hypothesis assumed were:

• The effects of aerodynamic drag were considered

• It is not assumed Ix = Iy, which means a perfectly symmetric structure

• The inputs of the equations of motion are represented by combinations squares

of the velocities of the four blades. It can be identified four main motions that

4.2. ARCHITECTURE REMODELING 27

the quadcopter can perform:

1. shift in the direction of the rotor rotation axis;

2. roll with respect to the body axes;

3. pitch with respect to the body axes;

4. yaw with respect to the body axes.

Table 4.1: Quadcopter dynamics equations notation

Notation Meaning

(x, y, z) center of mass position relative to the earth axis reference(φ, θ, ψ) vector of the Euler angles that identify the orientation of the body

frame with respect to the fixed frameI diag(Ix, Iy, Iz) inertia tensor referred to the body axesb, d Aerodynamic coefficientsl distance of the center of the ith rotor with respect to the center of

the quadcopterm quadcopter’s massΩi speed of the ith rotorJm inertia of the rotor with respect to its axis of rotation(p, q, r) components of the angular velocity vector −→ω with respect to the

body axesΩR obtained as ΩR = −Ω1 + Ω2 − Ω3 + Ω4

4.2 Architecture Remodeling

After having defined how the motor velocities are going to be transformed in

torque and force applied to the UAV, it is important to understand how these velocities

will be assigned. Firstly, a new topic (ardrone/motors_cmd) in the ardrone entity

was created. This topic receives the command to each motor defining their velocities.

Afterwards, the plugin Simple Controller was reduced to UAV’s dynamics instead of

its dynamics and control. Finally, the plugin State Controller was subscribed to the

ardrone/motors_cmd topic to store the motors velocities into ardrone/navdata topic.


4.2.1 Package’s Plugins

Each plugin has a basic structure composed by three fundamental blocks:

Load, Update and Callback functions.

In the Simple Controller plugin, the load function is used to set up all the topics

in the simulation. It subscribes variables to the correct topics getting information

from the URDF files. It is executed a single time during the whole simulation. This

function also sets up the call of the others two functions to be executed every Gazebo

loop. Furthermore, the callback function updates every entity of the drone simulation,

such as IMU data and robot’s state. Finally, the update function calculates the

desired force and torque to compensate possible disturbances.

In the State Controller plugin, the load and callback functions have exactly

the same objective, while the difference is on the update function. This last function

writes information into the ardrone/navdata topic, being possible to retrieve all the

data provided by the UAV’s sensors and commands.

4.2.2 URDF files

An important part of the structure are the URDF files. As defined by ROS

wiki [19], URDF stands for Unified Robot Description Format which is an XML

extension file containing all the parameters of a robot model, sensors scenes and so

on. Therefore, in order to set the ArDrone parameters such as, its mass, the distance

between rotor and center of mass, max angular speed of the rotor and others, we

should modify these files. It is important to highlight here that each plugin has its

own URDF file. Moreover, by the URDF it is possible to set the topics presents in

the simulation.

The restructuring of the architecture is composed by two parts: create a new

topic and reduce the controller plugin. The first one can be performed changing the

URDF files present in the simulation. In order to do that, we have added the topic

motors_cmd inside of the ardrone entity as shown in Figure 4.1. Also, the parame-


Figure 4.1: URDF plugin file modification.

Figure 4.2: Modification on the C++ code to subscribe to the new topic created.

ters b, d and l are passed to this file as thrustCoef, dragCoef and distanceRCOM,

respectively.

4.2.3 Code adaptation

Although, modifying only the URDF file of the plugin is not sufficient. To

subscribe the plugins to the new created topic it is necessary to change the C++

code as well as shown in Figure 4.2. In this way, the plugin will look for a parameter

called motorTopic in the URDF file; if there is any it will subscribe to this topic,

otherwise it will assign the variable called motor_topic_ as motors_cmd.

Then, it was necessary to execute the callback of this topic in every loop of

the simulation in order to update and use the parameters written on it. Another

adaptation on the code was needed as shown in Figure 4.3. Afterwards, the callback

function itself should be created. The idea here is basically read the information

from the subscribed motor topic and store it in a variable to be used later. In Figure

4.4 it is possible to observe a factor n presented in Table 2.2, which is the ratio of

the angular speed propeller to the rotor


Figure 4.3: Adaptation on the C++ code to execute the callback using the new createdtopic.

Figure 4.4: Callback function.

.

Finally, using the dynamic equations described previously in this chapter, we

could modify the force and torque generated on the robot using the motors velocity

as shown in Figure 4.5. Also, the motor command was passed to the ardrone/navdata

topic, as described in Figure 4.6.

To complete the code restructuring part, the parameters present in the

URDF file were also passed to C++ code and stored in a variable to be used

in the force/torque equations. The logic is analog to the one used to subscribe the

motorTopic. The code search for a determined parameter on the URDF file and store

its value in a variable, as described in Figure 4.7.

Figure 4.5: Quadcopter’s dynamic equations.


Figure 4.6: Motor commands being stored on ardrone/navdata topic.

Figure 4.7: Parameters from the URDF file being stored in a C++ variable.

4.2.4 rqt_graph of the New Structure

After all these modifications, we have generated the rqt_graph of the new

architecture in order to evaluate it, as shown in Figure 4.8. Now, the old Simple

Controller plugin works just setting up the dynamic equations to the robot instead

of doing an effective control. It is possible to observe the ardrone/motors_cmd topic

sending information to the Gazebo simulation, represented by the arrow between

these two topics. All the other components were preserved, besides the joystick for

practical reasons. This feature can be easily implemented in the code in a future

evolution of this project.


Figure 4.8: rqt_graph of the new architecture.

Chapter 5

Control: ROS and Simulink

With the redesigned architecture, it is now possible to develop both low and

high-level controllers for the quadcopter. In order to have a deeper approach on

the control strategies used, it is important to differentiate a low and a high level

controller, but first we have to keep in mind this Master Thesis objectives introduced

in chapter 1. The main three objectives are: to build a platform to test control

algorithms; to design an architecture able to assign directly the motor velocities; to

make possible to define a trajectory to be followed by the UAV. So far, the second

objective was already achieved in chapter 4. The relationship between the first two

objectives and the last one is exactly the relation between a low-level and a high-level

controller. While the first focuses on performing all the calculations to stabilize

the UAV on air, the second one focuses in giving inputs to the system that will be

used by the low-level controller, in order to achieve its objective to follow a given

trajectory. In other words, the high-level controller will provide references that would

be used by the low-level controller, in order to achieve the desired position in space.

An overview of the general system architecture is provided in Figure 5.1. It is

important to highlight that both controllers, low and high level, were made in the

Simulink. The low-level control is contained in the subsystem Control System, while

the high-level one is inside the subsystem Trajectory Generator. Both subsystems

are presented in further sections of this chapter.

33

34 CHAPTER 5. CONTROL: ROS AND SIMULINK

Figure 5.1: General system architecture scheme.(a): ROS provides the structure of the nodes, sensor and plugins to be executed in theGazebo Simulation.(b): Gazebo Simulation sensor’s data used in Simulink as the controller input.(c): Output of both controllers published to the ROS nodes (motor velocities and positionreferences).

5.1 MathWorks Simulink Tool

The first task was to define which approach the controller would have. Pre-

viously in the original architecture all control tasks were taken care by the plugins.

However, with the new architecture the plugins were reduced to just define the

dynamics of the UAV. Therefore, we are more free to use another approach than a

controller written in C++ code.

The approach chosen was to develop a controller using the well-know Simulink

software, as it is commonly used in the Automation and Control Engineering areas,

we decided to choose this tool due to its features designed to solve control problems.

Moreover, Simulink has a package exclusively designed to deal with ROS Simulations,

what could saves us a considerable amount of time trying to integrate both systems.

5.2 Time Synchronization: ROS and Simulink

After defining the control approach, it is necessary to check if this strategy is

feasible. A huge problem that could rise is related to time synchronization. Since we

have different software dealing with the same simulation, we have to be sure that the

acquired data from Gazebo and the control response given by Simulink are contained

inside the time-step interval defined and, above all, using the same clock as reference.

5.2. TIME SYNCHRONIZATION: ROS AND SIMULINK 35

Figure 5.2: Gazebo GUI during UAV simulation

Therefore, in order to be possible to use Simulink as the controller it was required to

make sure that both Gazebo and Simulink were synchronized in time.

Firstly, it is important to give a background about the Gazebo Graphical User

Interface (GUI) features. As shown in Figure 5.2, we can observe that Gazebo has

two fundamental parts: the first one is the left side panel and the second one is the

bottom part indicators. The left panel is related to the proprieties of the simulation,

such as, the physical engine running, the real time update rate, the max step size and

so on. In other hand, we have the parameter indicator in the window bottom part.

This panel is a reference to monitor how the simulation is performing. There are

parameters such as: real time factor, simulation time, real time, number of iterations

and frame per second. The principal parameters and definitions are explained in

Table 5.1.


Table 5.1: Gazebo parameters meaning

Parameter Meaning

real time update rate Rate to update the physics enginemax step size The maximum time step size that can be taken by a

variable time-step solver during simulationsimulation time Time past in the simulation worldreal time Time past in the real worldreal time factor max step size x real time update rate sets an upper bound

of real time factor. If real time factor < 1 the simulationis slower than real time.

5.2.1 Computational Power

Many times, the synchronization issue is related to the computational power

demanded by both Gazebo simulation and Simulink. If we have a big demand,

the computer would not be able to execute both in the same loop, resulting in a

divergence of times. Therefore, it is important to guarantee a feasible computational

power demand.

The real time update rate controls the update rate of the physics engine. The

product between the real time update rate and the max step size determines the

target real time factor, though for computationally demanding simulations, the target

real time update rate may not be achieved due to the computational power demand.

For example, if we set the real time update rate to 10000.0, with a max step size of

0.001, then the simulation will run in fast motion, at about 1000% real time factor

(10.0). In other words, ten seconds in the simulation time are equivalent to one

second in real-life time. Therefore, decreasing the value of the parameter real time

update rate we could give more time to our simulation to correctly write the data

and do not overwrite past values, demanding a smaller computational effort.

5.2.2 ROS Time Model Stepping

Another great feature that Simulink provides, when dealing with ROS sim-

ulations, is the possibility to work with built-in libraries and functionalities. One

5.3. CONTROL STRATEGY 37

Figure 5.3: Cascade PID scheme to roll and X references [4].

Figure 5.4: Cascade PID scheme to pitch and Y references [4].

of these great functionalities is the ROS Time Model Stepping [20], which enables

the Simulink code to work using the ROS model time instead of the computer

clock. Therefore, the time past in Simulink is exactly the same past in Gazebo. In

this way, having libraries and specially this functionality, we can say that the time

synchronization is not a problem anymore. Although, some tests to verify it are

needed and provided in the next chapter.

5.3 Control Strategy

The control strategy used, as said in Chapter 1, was the Cascade PID to

regulate the roll and pitch movement, and a normal PID controller to regulate the

altitude and the yaw angle separately. The Cascade PID control strategy to regulate

pitch and roll is the most commonly used for quadcopters [21], usually it is an inner

control loop responsible for angular velocities and an outer control loop, responsible

for the angles. Although, in this project, the inner loop controls the angles and the

outer loop controls the position in the (X,Y,Z) reference system, since we want to

implement a trajectory control as well.


Figure 5.5: Cascade PID scheme to Z reference [4].

5.3.1 Control Scheme

To give an overview of the control system adopted, Figure 5.3, Figure 5.4

and Figure 5.5 are provided, representing the scheme used to control roll and X,

pitch and Y, and Z coordinates respectively. The schemes are analog to each other,

where Rx, Ry, and Rz are the positioning regulators, and Rθ and Rφ are the angle

regulators. The positioning errors are represented by ex, ey, and ez, while the angle

errors by eθ and eφ. The great functionality that the Cascade PID control has is to

use the error as the reference of the inner loop. Therefore, we have two PID loops

working together. The position references xd, yd, and zd are given by the trajectory

path forcing the UAV to follow the desired route.

5.3.2 Control Algorithm in Simulink

With the whole control system architecture scheme presented in the previous

section, we can move on to the code elaboration and description. A great feature that

Simulink has it is to be a visual programming software instead of a text programming

editor. Therefore, all the control blocks presented could be easily passed to Simulink.

Since we want to design a system that concatenates those three schemes, we have

decided to group the systems in subsystems as shown in Figure 5.6.

The control system is composed by seven fundamental blocks:

• Model Data Acquisition : this block has the function to read the data in-


Figure 5.6: Simulink: UAV control system.

formation from the Gazebo simulation, such as angles and velocities of the

model;

• Quaternion To Euler : this block converts the angles from quaternion to Euler

reference system;

• Trajectory Generator : to give the references to be used in the control block,

following a given trajectory;

• Control System: to determine the responses that the drone should have in

order to be stable on air and follow the given trajectory;

• U to omegaSquare: conversion from the four input equations shown in Chapter

4 to velocities to be assigned to each motor;

• Assign Array : creates the input structure to be assigned to the motors;

• Publish motor commands: assigns the velocities desired to each motor.

The overview of the principal blocks has been given, but it is important to

have a deeper look at the Trajectory Planning, Control System and U to omegaSquare

subsystems.


Figure 5.7: Trajectory Control subsystem

Trajectory Control Subsystem

To begin with this deeper analysis, the Figure 5.7 is provided. In this figure

it is possible to observe what is inside of the Trajectory Control subsystem. To

generate the desired path, the system uses a time-based position convention. In other

words, it assigns a position based on the time of the simulation, therefore, generating

a trajectory. That is why we have the first block acquiring the /clock node data,

followed by a conversion of this information to the desired unit: seconds. The time

is the input of the Trajectory Generator block that reads the time, and based on it,

give a desired position to (X,Y,Z) and the desired yaw angle.

Control Subsystem

The most important subsystem is provided in Figure 5.8. This system is

composed by four control schemes: Altitude control (Z); Pitch and Y control in

cascade; Roll and X control in cascade; and Yaw angle control, respectively. A

saturation block was added to the Cascade PID control to avoid the drone to flip as

it tries to reach a very far position when setting a intense response. In other words, if

the position reference in (X,Y) is too far from the actual position, the control would

set a huge angle which would make the drone flip. Besides, the control scheme is

exactly the one shown in Figures 5.3 and Figure 5.4.


Figure 5.8: Control subsystem


Figure 5.9: U to Omega Square subsystem

Figure 5.10: MATLAB conversion function code

U to Omega Square Subsystem

Finally, the U to Omega Square subsystem is provided in Figure 5.9. This

subsystem has the task of convert the four input equations into velocities to be

assigned to each motor of the UAV. This conversion is made through MATLAB code

described in Figure 5.10.

Figure 5.11: UAV model parameters set in the Simulink controller


Figure 5.12: Control parameters set in the Simulink controller

5.3.3 Control and Model Parameters

The parameters used in the Simulink code were passed by a MATLAB script

as shown in Figure 5.11 and Figure 5.12. It is important to highlight that, as the

UAV is not fully symmetric in X-Y axes, we have used the inertia moments of each

axis as a gain to compensate this symmetricality issue.

5.3.4 Project Repository

All the files used in this thesis can be found in the project repository on this

BitBucket url: https://bitbucket.org/bascetta/airdrone_control/src/master/ .

Chapter 6

Simulation Results and Analysis

In this chapter, final simulation results, achieved by applying a simple con-

troller, are illustrated.

6.1 New Architecture

Regarding the new architecture structure, we have stipulated a simple control

algorithm (proportional control), as shown in Figure 6.1, to verify if it is possible to

assign the motor velocities accordingly to an given input. The controller receives as

input the information produced by the sonar sensor and compares it to the reference.

The difference, also called error, is multiplied by a gain and applied to the motors

according to the code provided in Figure 6.2.

The test performed well, as we were expecting. We have set a subscriber to

the motors_cmd node to check the data passed to it. In Figure 6.3 it is possible to

observe the velocities being correctly applied to each motor in the Linux terminal.

45

46 CHAPTER 6. SIMULATION RESULTS AND ANALYSIS

Figure 6.1: Proportional altitude controller.

Figure 6.2: MATLAB code to assign motor velocities

Figure 6.3: Motor velocities being assigned

6.2. TIME SYNCHRONIZATION EVALUATION 47

6.2 Time Synchronization Evaluation

The next step in the test schedule was to check if the time synchronization

is being correctly set. In order to execute this test the simple controller presented

in the previous section was used. To give a clearer view of this step, the important

nodes used are provided in Figure 6.4. The Simple_controller node, as the own

name says, corresponds to the proportional controller for altitude. Moreover, a new

node (clock_acquisition) was created in order to measure the exact time when the

controller sends a command to the UAV. In this way, we could measure the frequency

that this controller uses. The sonar_height and /clock nodes provides us the altitude

of the UAV and the simulation time respectively. Finally, the command is sent to

the /cmd_vel node.

Figure 6.4: Important nodes in time synchronization test.

All the information passed to the clock_acquisition node was saved in ROS

bags in order to make possible to evaluate the result. Moreover, a MATLAB script

was written to perform all the calculations with the saved data. The result of this

test was really satisfactory presenting a average of exactly 1kHz as defined in the

Simulink controller fixed step size. In Table 6.1 it is possible to observe how the

frequency variance and the real time factor decreases as we decrease the maximum

step size. It is important to keep in mind that as the real time factor decreases, more

time is needed to perform the simulation.


Table 6.1: Time synchronization evaluation

Maximum Step Size Frequency Variance Real Time Factor

0.005s 1000.00 Hz 4.9971e-31 0.0049940.002s 1000.00 Hz 2.1089e-31 0.0019990.001s 1000.00 Hz 1.92e-31 9.1650e-4

6.3 Control Evaluation

We now discuss the tuning of the use controller. In PID controllers, many

methods were already elaborated in order to define a law to set the gains [22], such

as Ziegler-Nichols methods. However, the tuning was not an easy task and the most

efficient method used in this project was a trial and error procedure. Beforehand,

we can note that, as the quadcopter is approximately symmetric, the gains for Roll

and X controller are very similar those for Pitch and Y controller. Moreover, since

a Cascade PID control scheme is used the tuning has been performed in two steps

starting from the inner loop. Finally, all the regulators were set as shown in following

Laplace equations:

Rx = 600 · Ixg· s+ 1

0.1s+ 1(6.1)

Ry = −600 · Iyg· s+ 1

0.1s+ 1(6.2)

Rz = 150 · 80 · 0.25s+ 1

0.01s+ 1(6.3)

Rφ = 150 · 160 · Iy ·0.25s+ 1

0.0025s+ 1(6.4)

Rθ = 150 · 160 · Ix ·0.25s+ 1

0.0025s+ 1(6.5)

Rψ = 150 · 160 · Iz ·0.25s+ 1

0.0025s+ 1, (6.6)

as previously described in the MATLAB code in Figure 5.12.

The first test was related to the altitude controller, as it is simpler controller

than the others. We have set a reference height of 1 meter and the UAV presented a

6.3. CONTROL EVALUATION 49

Figure 6.5: Altitude control

very fast response without overshoot, as shown in Figure 6.5. However, the controller

presents a stationary error due its order [23], but this was already expected and its

response was considered acceptable being less than 5% of the reference.

Figure 6.6: Control position response

As the hovering control was being perfectly done during the previous test,

the next step was to evaluate the positioning control in (X,Y) references. On this


test, we have set up references to (X,Y,Z) as (5,0,1) meters, and the response was

one more time very satisfactory as shown in Figure 6.6. It is interesting to observe

the correction made by the Y controller, that slightly goes to a negative position

but quickly returns to the desired reference. One theory that could explain why this

correction happens is the fact that the UAV is trying to compensate the huge error

(difference between actual position and the reference position) related to the X-axis.

Thus, this small correction takes place in the Y-axis, while the drone is executing a

rough maneuver towards the reference in the X-axis.

Figure 6.7: (X,Y,Z) response separately

Finally, considering the previous results, the trajectory test was executed.

The considered reference is shown in Figure 6.8. The UAV had a very good response

being able to follow perfectly the path desired as shown in the figures 6.7 and 6.8.

6.3. CONTROL EVALUATION 51

Figure 6.8: (X,Y,Z) response in space

Chapter 7

Conclusions

The main goal of this thesis work was to develop a test platform for control

algorithms, more specifically for quadcopters. Underneath this main objective, two

requirements were needed: an architecture that allows to implement a low-level control

system and a controller capable to follow a given trajectory. Both requirements were

achieved modifying the original files from the TUM ardrone package and developing

a control through Mathworks Simulink software.

The architecture redesign was the most time-demanding, between these two

tasks, because programming technical knowledge was needed. However, the result

obtained was satisfactory and the motor velocity now can be directly assigned by

the ROS node created.

In other hand, the controller developed in Simulink had a great result as well,

and using a Cascaded PID controller we could stabilize the UAV in the air. Moreover,

the trajectory control was also possible and achieved good result. Hence, the time

synchronization between Simulink and Gazebo was verified.

As future works, I would suggest to verify all the results obtained with the

simulator in the real Parrot ArDrone 2.0 in order to assure the reliability of the

system. In fact, this was also a starting objective to the project, although, due to

the situation lived in 2020 with the COVID-19 all the laboratory tests were canceled

53

54 CHAPTER 7. CONCLUSIONS

and all the objectives of this project were restrained to simulation.

Bibliography

[1] M. Hassanalian and A. Abdelkefi, “Classifications, applications, and design

challenges of drones: A review,” Progress in Aerospace Sciences, 05 2017.

[2] G. Bertoli, “Wiki,” 2018. [Online]. Available: http://wiki.ros.org/tum_simulator

[3] “Ros wiki - documentation,” accessed: 2020-10-02. [Online]. Available:

http://wiki.ros.org/Documentation

[4] G. P. Incremona, “Modello e Controllo di un Quadricottero,” pp. 1–42, 2018.

[5] A. Koszewnik, “The parrot UAV controlled by PID controllers,” Acta Mechanica

et Automatica, vol. 8, no. 2, pp. 65–69, 2014.

[6] J. F. Keane and S. S. Carr, “A brief history of early unmanned aircraft,” pp.

558–571, 2013.

[7] F. S. Barbosa, “4DOF Quadcopter: Development, Modeling and Control,” p. 86,

2017.

[8] S. Norouzi Ghazbi, Y. Aghli, M. Alimohammadi, and A. A. Akbari, “Quadrotors

unmanned aerial vehicles: A review,” International Journal on Smart Sensing

and Intelligent Systems, vol. 9, pp. 309–333, 03 2016.

[9] S. N. Sy Salim and M. Zainon, Control Systems Engineering. Penerbit Universiti

Teknikal Malaysia Melaka, 2010, 01 2010.

[10] K. Ang, G. Chong, and Y. Li, “Pid control system analysis, design, and tech-

nology,” Control Systems Technology, IEEE Transactions on, vol. 13, pp. 559 –

576, 08 2005.

55

http://wiki.ros.org/tum_simulator

http://wiki.ros.org/Documentation

56 BIBLIOGRAPHY

[11] ——, “Pid control system analysis, design, and technology,” Control Systems

Technology, IEEE Transactions on, vol. 13, pp. 559 – 576, 08 2005.

[12] J. Kerr and K. Nickels, “Robot operating systems: Bridging the gap between

human and robot,” 44th IEEE Southeastern Symposium on System Theory

(SSST), pp. 99–104, 03 2012.

[13] O. A. Martinez and M. Cardona, “State of the Art and Future Trends on

Unmanned Aerial Vehicle,” Proceedings of the 2018 3rd IEEE International

Conference on Research in Intelligent and Computing in Engineering, RICE

2018, vol. 1, 2018.

[14] J.-S. Pleban, R. Band, and R. Creutzburg, “Hacking and securing the AR.Drone

2.0 quadcopter: Investigations for improving the security of a toy,” Mobile

Devices and Multimedia: Enabling Technologies, Algorithms, and Applications

2014, vol. 9030, no. January, p. 90300L, 2014.

[15] J. Meyer, “Wiki,” 2014. [Online]. Available: http://wiki.ros.org/hector_

quadrotor

[16] T. U. Darmstadt, “tu-darmstadt-ros-pkg/hector_quadrotor,” 2018. [Online].

Available: https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor

[17] A. Santa Maria, “angelsantamaria/tum_simulator,” 2018. [Online]. Available:

https://github.com/angelsantamaria/tum_simulator

[18] W. Hoffman, “Overview,” 2013. [Online]. Available: https://cmake.org/overview/

[19] “URDF - ROS wiki,” 2018. [Online]. Available: http://wiki.ros.org/urdf

[20] I. The MathWorks, “Enable ros time model stepping for deployed ros

nodes,” 2020. [Online]. Available: https://it.mathworks.com/help/ros/ug/

enable-ros-time-model-stepping-for-deployed-ros-nodes.html

[21] D. Tesch, D. Eckhard, and W. Guarienti, “Pitch and roll control of a quadcopter

using cascade iterative feedback tuning,” IFAC-PapersOnLine, vol. 49, 12 2016.

http://wiki.ros.org/hector_quadrotor

http://wiki.ros.org/hector_quadrotor

https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor

https://github.com/angelsantamaria/tum_simulator

https://cmake.org/overview/

http://wiki.ros.org/urdf

https://it.mathworks.com/help/ros/ug/enable-ros-time-model-stepping-for-deployed-ros-nodes.html

https://it.mathworks.com/help/ros/ug/enable-ros-time-model-stepping-for-deployed-ros-nodes.html

BIBLIOGRAPHY 57

[22] P. Chang and J. H. Jung, “A systematic method for gain selection of robust pid

control for nonlinear plants of second-order controller canonical form,” Control

Systems Technology, IEEE Transactions on, vol. 17, pp. 473 – 483, 04 2009.

[23] J. Canete, C. Galindo, and I. Moral, Introduction to Control Systems, 01 2011,

pp. 137–165.

Date post:	21-Apr-2023
Category:	Documents
Upload:	khangminh22
View:	0 times
Download:	0 times

Simulink-ROS software architecture to test the control system ...

Documents