Bachelor Thesis: Planning and Control of a Multiple

Delft Center for Systems and Control

Planning and Control of a Multiple-QuadcopterSystem Cooperatively Carrying a Slung Payloadin Dynamical Environments

A Centralized Model Predictive Control Solution

Iñigo Moreno Caireta

Bach

elorT

hesis

Defense date: 26th June, 2018

Director: Javier Alonso-Mora Cognitive Robotics Department TU Delft

Tutor: Maria Teresa Abad Computer Science Department UPC

Degree: Informatics Engineering Computing

Sending University: Universitat Politècnica de Catalunya (UPC)

Sending Center: Facultat d’Informàtica de Barcelona (FIB)

Receiving University: Delft University of Technology (TU Delft)Receiving Center: Mechanical, Maritime and Materials Engineering (3mE)

Planning and Control of aMultiple-Quadcopter System Cooperatively

Carrying a Slung Payload in DynamicalEnvironments

A Centralized Model Predictive Control Solution

Bachelor Thesis

For the Bachelor’s degrees in Industrial Technology Engineering and inInformatics Engineering at Universitat Politècnica de Catalunya

Iñigo Moreno Caireta

19th June, 2018

Faculty of Mechanical, Maritime and Materials Engineering (3mE) · Delft University ofTechnology

Abstract

Unmanned Aerial Vehicles (UAVs) have been thoroughly used for transportation in unpre-dictable environments. However, one of the limits for these applications is usually the payloadmass, as it cannot be too big in relation to the drones’ mass. In this thesis, we propose the useof multiple drones for cooperative transportation through strings. In contrast with currentapproaches for the control of such systems, we propose the use of Nonlinear Model PredictiveControl (NMPC) to achieve more aggressive and agile maneuvers. The control algorithm’seffectiveness is demonstrated in simulated environments.

Bachelor Thesis Iñigo Moreno Caireta

ii

Iñigo Moreno Caireta Bachelor Thesis

Table of Contents

1 Introduction 11-1 Theme Relevance and Justification . . . . . . . . . . . . . . . . . . . . . . . . . 1

1-1-1 Multiple quadrotor system . . . . . . . . . . . . . . . . . . . . . . . . . 11-1-2 Model Predictive Control . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1-2 Distributed versus Centralized Control . . . . . . . . . . . . . . . . . . . . . . . 21-3 Research Objective . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21-4 Thesis Context . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31-5 Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2 Problem Formulation 52-1 Quadrotors-payload system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52-2 Dynamic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2-2-1 Equations of Motion (EOMs) . . . . . . . . . . . . . . . . . . . . . . . . 62-2-2 Quadrotor Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62-2-3 Aerodynamic Drag . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72-2-4 Environment Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . 72-2-5 Obstacle Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3 Model Predictive Control (MPC) Formulation 93-1 Formal Definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93-2 Election of State Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3-2-1 Reducing qi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103-2-2 Reducing wi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3-3 Slack Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123-4 Forces PRO formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 123-5 Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

3-5-1 State Variables Restrictions . . . . . . . . . . . . . . . . . . . . . . . . . 12


iv Table of Contents

3-5-2 Environment Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . 133-5-3 Wire Slackening Prevention . . . . . . . . . . . . . . . . . . . . . . . . . 13

3-5-4 Drone Speed Limit . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

3-5-5 Drone Distance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133-5-6 Collision Avoidance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

3-6 Objective function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

3-6-1 Destination Objective . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3-6-2 Final Speed Minimization . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3-6-3 Input Minimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3-6-4 Slack Minimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

4 System setup and control algorithm 17

4-1 Hardware and properties . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

4-2 Workspace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

4-3 Control algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

4-3-1 Simulation Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

4-3-2 Initial Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 204-3-3 Experimental setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

4-4 Data communication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 214-4-1 Rosbags . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

4-5 Visualizer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

5 Results 255-1 Simulation Scenario . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 255-2 Estimating time per iteration of the MPC solver . . . . . . . . . . . . . . . . . . 25

5-3 Step time outside the MPC solver . . . . . . . . . . . . . . . . . . . . . . . . . 26

5-4 Finding best parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

5-5 External planner testing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

5-6 Demonstration of Maneuvers . . . . . . . . . . . . . . . . . . . . . . . . . . . . 305-6-1 Moving obstacle avoidance . . . . . . . . . . . . . . . . . . . . . . . . . 30

5-6-2 High swing maneuvers . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

5-7 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

6 Conclusion 356-1 Future Studies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35


Table of Contents v

A Project Planning 37A-1 Planning and Scheduling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37A-2 Task Description . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

A-2-1 Paper reading . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37A-2-2 Installing and familiarizing myself with Robot Operating System (ROS) . 38A-2-3 Understanding Nikhil D. Potdar’s Code . . . . . . . . . . . . . . . . . . 38A-2-4 Finding an accurate dynamic model . . . . . . . . . . . . . . . . . . . . 38A-2-5 Transforming Nikhil D. Potdar’s Code for the multiple drone case . . . . 39A-2-6 Adding constraints and tweaking cost function . . . . . . . . . . . . . . . 39A-2-7 Optimizing the code for real-time performance . . . . . . . . . . . . . . . 39A-2-8 Writing the thesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39A-2-9 Testing the planner in different scenarios . . . . . . . . . . . . . . . . . . 39

A-3 Gantt chart . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

B Budget and Sustainability 41B-1 Sustainability auto-evaluation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41B-2 Sustainability and Social Commitment . . . . . . . . . . . . . . . . . . . . . . . 42

B-2-1 Environmental Dimension . . . . . . . . . . . . . . . . . . . . . . . . . . 42B-2-2 Social Dimension . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42B-2-3 Economical Dimension . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

B-3 Project Budget . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43B-3-1 Hardware Budget . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43B-3-2 Software Budget . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43B-3-3 Human Resources Budget . . . . . . . . . . . . . . . . . . . . . . . . . . 44B-3-4 Unexpected Costs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45B-3-5 Indirect Costs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45B-3-6 Total Budget . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46

Glossary 49List of Acronyms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49List of Symbols . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50


vi Table of Contents


List of Figures

2-1 Schematic of the Quadrotors-payload system . . . . . . . . . . . . . . . . . . . . 52-2 Environment Schematic . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82-3 Obstacle dimensions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3-1 Suspension angles schematic . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103-2 Possible values of qi with color hue mapped to the values of ϑi in (a) and ϕi in (b) 11

4-1 Drones workspace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184-2 Motion Capture System (MCS) camera . . . . . . . . . . . . . . . . . . . . . . 184-3 Bebop2 drone with markers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184-4 Hat with reflective markers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184-5 Gamepad by Logitech . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184-6 Simulation algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 204-7 External planner . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 204-8 Communications schematic . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 224-9 Screenshot of the visualizer Graphical User Interface (GUI) . . . . . . . . . . . . 23

5-1 Path of system with N = 20, ∆t = .05 . . . . . . . . . . . . . . . . . . . . . . . 265-2 Graph of tit/N . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 275-3 Scaling of tit/N when varying n . . . . . . . . . . . . . . . . . . . . . . . . . . 275-4 Scaling of tit/N when varying nobs . . . . . . . . . . . . . . . . . . . . . . . . . 275-5 Graph of tstep − tMP C . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 285-6 Graph of 1/tstep . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295-7 Moving obstacle avoidance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 315-8 High swing maneuver . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 325-9 Real distance between the payload and the quadrotors . . . . . . . . . . . . . . 33

A-1 Gantt chart of the project . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40


viii List of Figures


List of Tables

1-1 Thesis Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

5-1 Simulation Scenario Data . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

B-1 Sustainability matrix of the project . . . . . . . . . . . . . . . . . . . . . . . . . 42B-2 Hardware Budget . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43B-3 Software Budget . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43B-4 Task Time Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44B-5 Human Resources Budget . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44B-6 Unexpected Costs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45B-7 Indirect Costs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45B-8 Indirect Costs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46


x List of Tables


Chapter 1

Introduction

1-1 Theme Relevance and Justification

1-1-1 Multiple quadrotor system

Unmanned Aerial Vehicles (UAVs) have been researched and developed by the scientific,industrial and even military community since their first appearance. However, after the firstquadrotor was released commercially in 1999 [1] they started gaining more popularity, espe-cially due to their size, price, ability to fly in confined spaces and their fast maneuverability.These qualities make them the perfect candidate for mass-deployment in highly unpredictablereal-life scenarios.

Payload carrying has been the focus of development in the quadrotor research communityin the latest years, as it is clear that their versatility can be used to deliver packages invariable environments and even in emergency situations where land access is impossible, suchas in mountains or in the event of a flood.

There are two main payload-UAV solutions active today. Adding the payload mass directlyto the body of the UAV using grippers or adding it indirectly by attaching the payload to itusing strings. The former solution, however, poses a problem: the dynamic response of thesystem varies considerably [2], and, while Proportional-Integral-Derivative (PID) controllersmight be able to function with these unknowns, it is unsuitable for aggressive control andhigh dynamic responses.

The focus of this thesis is therefore focused on string-based systems. However, all thepayload-quadrotor solutions have a limit, the payload’s mass usually is small in relation tothe quadrotor’s. One could argue that if you need to carry a big payload, a bigger drone isneeded but this is not possible in real-life scenarios, as the end-user would need to have afleet of differently sized drones for each payload mass range.

This is the reason why a multiple quadrotor system is highly valuable. The end-user couldadapt the number of drones for each payload requirement while only needing a fleet of smallquadrotors, which would also be cheaper.


2 Introduction

1-1-2 Model Predictive Control

Traditionally, the control of multiple manipulator systems is solved by planning the wholesystem’s movement using formation control and then giving commands to each drone so thatthey follow the desired formation, such as in [3]. This assumes that the formation movementis slow enough so that the payload swing is negligible. This assumption limits the system’smovement and is not applicable for highly dynamical environments.

Other solutions include tracking of a predefined trajectory [4, 5]. This can achieve goodresults but requires the user to define said trajectory and in doing so, reduces the system’sautonomy.

The proposed solution uses Nonlinear Model Predictive Control (NMPC), a variant ofModel Predictive Control (MPC) to compute an optimal trajectory over a fixed time horizon.This is repeated over each time step. The controller uses a physics model to simulate thesystem over the time horizon and an objective function to go towards the goal. Additionalconstraints are added, such as wire slackening avoidance, workspace size restrictions andcollision prevention.

The use of MPC provides flexibility to the controller, with it, a new object can appearand the system will figure out an alternative path. Also, if the model isn’t exact it will keepre-planing using real-time data.

1-2 Distributed versus Centralized Control

In this problem, distributed control would mean to have a separate controller for each of thedrones while they share their positions. In such control the quadrotors would communicatebetween each other their position and their plan in order to eventually reach a consensusand move together. However, having separate controllers that need to reach a consensus istime-consuming and needing a consensus prevents the system from executing more aggressivemaneuvers. In this thesis, we will make use of a centralized control where only one agentmakes the decisions for all of the drones.

1-3 Research Objective

The objective of the research included in this thesis is to:

Design a control algorithm for systems with multiple UAVs carrying a slung pay-load in dynamic environments using NMPC and demonstrate it in simulated andreal scenarios.


1-4 Thesis Context 3

1-4 Thesis Context

This thesis is a result of the collaboration between Centre de Formació InterdisciplinàriaSuperior (CFIS) and Javier Alonso-Mora. Alonso-Mora proposed the theme to CFIS andCFIS suggested me, Iñigo Moreno to participate in it.

The research was performed in the lab at Cognitive Robotics Department (CoR), DelftUniversity of Technology (TU Delft) and was funded by CFIS, Erasmus+ and Agència deGestió d’Ajuts Universitaris i de Recerca (AGAUR).

This thesis is mostly a continuation of the research performed by Nikhil D. Potdar for hismaster thesis, in which he designed a control algorithm for a single UAV carrying a slungpayload [6].

1-5 Notation

Throughout this thesis the following notation will be used:

Symbol DefinitionScalars: x

Vectors: x

Elements of vector: x(i) i-th element of vector x

Position vectors: px

Coordinates of position vector: xx, yx, zx x,y,z coordinates of position px

Matrices: X

Sets: XTime derivatives: x ∂x

∂t

Desired values: x desired value of x

Predicted values: x predicted value of x

Table 1-1: Thesis Notation

All vectors are column vectors and all positions are expressed in the East-North-Up (ENU)inertial frame unless otherwise stated.


4 Introduction


Chapter 2

Problem Formulation

2-1 Quadrotors-payload system

The quadrotors-payload system can be represented as a set of n ∈ Z>0 quadrotors con-nected via cables to a mass as shown in Figure 2-1. The payload is simplified as a point mass.The cables are assumed to be massless links and to connect directly each quadrotor’s CenterOf Mass (COM) with the payload’s COM.

The payload’s location is defined as pl ∈ R3, and its mass is represented by ml ∈ R>0.The i-th link’s direction from the i-th quadrotor to the payload is denoted by qi ∈ S2 whereS2 ⊂ R3 is the set of unit vectors in R3 defined by S2 = {q ∈ R3 | ‖q‖ = 1}. The angularvelocity of this direction is defined as wi ∈ R3 and satisfies qi · wi = 0. The i-th link’s lengthis represented by li ∈ R>0. The i-th quadrotor’s mass is represented as mi ∈ R>0,and itsinput force is defined by Fui ∈ R3. The i-th quadrotor orientation is defined by its roll θi ∈ Rand pitch φi ∈ R. All quadrotors have zero yaw. The i-th quadrotor’s position is denoted bypi ∈ R3 and can easily be computed by equation 2-1.

pi = pl − qili 2-1

Figure 2-1: Schematic of the Quadrotors-payload system


6 Problem Formulation

2-2 Dynamic Model

2-2-1 Equations of Motion (EOMs)

The dynamic model we use for the system is based on the EOMs introduced in [4]:

qi = wi × qi ∀i ∈ {1 . . . n} 2-2

Mq(pl − ge3) =n∑

i=1(−mili‖wi‖2qi + F

‖ui) 2-3

wi = 1li

qi × (pl − ge3) − 1mili

(qi × F ⊥ui) ∀i ∈ {1 . . . n} 2-4

Where e3 = [0, 0, −1]T ∈ R3, Mq = ml ∗ I +∑n

i=1(miqiqTi ) ∈ S3

+ and the vectors F‖ui ∈ R3

and F ⊥ui ∈ R3 represent the orthogonal projections of Fui along qi and along the plane normal

to qi, respectively:

F‖ui = qiq

Ti Fui ∀i ∈ {1 . . . n} 2-5

F ⊥ui = (I − qiq

Ti )Fui ∀i ∈ {1 . . . n} 2-6

2-2-2 Quadrotor Dynamics

As in [6], to compute the input force ui of each quadrotor, we abstract the attitude andmotor control included in the drone we will be using, the Bebop2. As this drone’s inputs aredesired pitch, roll and vertical velocity we use these as inputs. We will use ut ∈ R3n to denoteall quadrotor inputs and ui ∈ R3 to denote the i-th drone’s inputs:

ui = [θi, φi, ¯zi] ∀i ∈ {1 . . . n} 2-7

As Parrot does not officially release the internal dynamics for its drones, we compute ablack-box model of these dynamics using experimental data. For the vertical velocity, weassume that the internal control algorithm compares the desired value with the estimatedvalue that comes from internal sensors. For the quadrotor orientation, the black box modeldirectly estimates both angles based on the inputs. Therefore, we use the following model:

[θi, φi, zi] = [hθ(θi), hφ(φi), hz(¯zi − zi)] 2-8

Where hθ,hφ and hz are identified using a continuous system state model of second order. Foreach quadrotor, the states introduced by hθi,hφi and hzi are denoted as ssθi ∈ R2, ssφi ∈ R2

and sszi ∈ R2 respectively. The combination of all three system states for each quadrotor isrepresented by ssi ∈ R6. From the quadrotor state we calculate the input force Fui using thefollowing equation:

Fui = mi

g tan(φi)

cos(θi)−g tan(θi)

zi + g

'↑

If anglesare small

mi

gφi

−gθi

zi + g

∀i ∈ {1 . . . n} 2-9


2-2 Dynamic Model 7

2-2-3 Aerodynamic Drag

We will use the same model for aerodynamic drag model as in [6]. The drag on thepayload, denoted as FDl ∈ R3, is modeled using a quadratic drag model, while the drag oneach quadrotor, denoted as FDi ∈ R3, is modeled using a linear drag model.

FDl = −kDl‖pl‖2 pl

‖pl‖= −kDl‖pl‖pl 2-10

FDi = −kDi‖pi‖pi

‖pi‖= −kDipi ∀i ∈ {1 . . . n} 2-11

Where kDl ∈ R and kDi ∈ R are the drag constants identified in [6].

Drag Inclusion in EOMs

To use include drag in our problem we need to include the drag force calculated in equations2-10 and 2-11 in the EOMs declared in equations 2-3 and 2-4.

The drag force on the drones FDi can be added to the input force Fui (we will denote thissum as FuDi). The drag force on the payload can be included by subtracting FDl

mlfrom all

appearances of pl in the EOMs, in the same way that ge3 is subtracted. The resulting EOMsfrom these changes are:

qi = wi × qi ∀i ∈ {1 . . . n} 2-12

Mq(pl − ge3 − FDl

ml) =

n∑i=1

(−mili‖wi‖2qi + F‖uDi) 2-13

wi = 1li

qi × (pl − ge3 − FDl

ml) − 1

mili(qi × F ⊥

uDi) ∀i ∈ {1 . . . n} 2-14

2-2-4 Environment Definition

The environment is defined as a 3 dimensional box with the origin in the floor’s center.We denote the box’s dimensions as dim ∈ R3

>0. A schematic of the environment can be seenin figure 2-2.


8 Problem Formulation

dim(1)

dim(3)

dim(2)x

z

y

Figure 2-2: Environment Schematic

dimobsi(3)

dimelli(3)

x

y

z

Figure 2-3: Obstacle dimensions

2-2-5 Obstacle Modeling

Obstacles are modeled as three dimensional boxes and their behavior is modeled as linearmovement using a Kalman Filter. This is required so that the planner takes into accountfuture movement of the obstacles. The number of obstacles is denoted by nobs ∈ Z≥0, theircenter position by pobsi

∈ R3 and their dimension by dimobsi∈ R3

>0.

Often we will use the smallest ellipsoid that contains the box. The equations to find theradial dimensions of the ellipsoid (denoted as dimelli ∈ R3) are:

dimelli =√

32 dimobsi

∀i ∈ {1 . . . nobs} 2-15

A schematic of an obstacle and its dimensions can be seen in figure 2-3. For furtherexplanation refer to his thesis[6].


Chapter 3

Model Predictive Control (MPC)Formulation

An MPC problem consists of a state vector which can be modified by a certain inputfollowing a set of inter-state equalities. The MPC repeats these equalities, generating a fixednumber (N ∈ Z+) of predicted stages. An objective function is defined which evaluates theperformance of these stages. The MPC solver finds, iteratively, the inputs that minimize thisobjective function. The search space can also be limited using constraints on the state vector.

In our case, the inter-state equalities are the discretization of the dynamics defined pre-viously with a fixed time-delta (denoted as ∆t ∈ R+). This discretization is done using the2nd order Runge-Kutta (RK2) method. Therefore, each stage represents a time in the futureand they are separated by ∆t.

The number of iterations that the solver will try to find the optimal solution depends onhow difficult the problem is in each stage and is highly variable. However, we can limit it bya maximum number of iterations (denoted as nmaxit ∈ Z+).

3-1 Formal Definition

The formal definition of the MPC problem is:

argminu1,u2...uN

∑Nk=1 C(xk, uk, sk) (Cost Function)

s.t. x1 = x(t) (Inital condition)xk+1 = f(xk, uk) (Discretised dynamics)I(xk, uk, sk) ≥ 0 (Constraints)

3-1


10 MPC Formulation

xy

z

qi

ϕi

ϑi

pi

Figure 3-1: Suspension angles schematic

3-2 Election of State Variables

To be able to use these dynamics in the MPC problem, we need to create a state vectorand write code to generate its derivative given the state and inputs.

It is important to choose the state vector correctly so that the number of variables isminimized and singularities are prevented.

Throughout this thesis, we will denote the number of variables as nvar ∈ Z>0 and the statevector as x ∈ Rnvar .

Considering the system definition introduced in section 2-1, the most straightforward ap-proach to choosing the state vector is the following:

x = [pl, q1 . . . qi . . . qn, pl, w1 . . . wi . . . wn, ss1 . . . ssi . . . ssn] 3-2

3-2-1 Reducing qi

However, as introduced in [6] it is best to use payload angles for the state vector, as theyreduce the number of variables. These angles are represented in figure 3-1 and are denotedby ϑi ∈ R and ϕi ∈ R. Using these angles we can define each of the qi vectors in the followingway:

qi =

cos(ϑi) sin(ϕi)

sin(ϑi)− cos(ϑi) cos(ϕi)

∀i ∈ {1 . . . n} 3-3

Deriving 3-3 we obtain:

qi =

− sin(ϑi) sin(ϕi)

cos(ϑi)sin(ϑi) cos(ϕi)

ϑi +

cos(ϑi) cos(ϕi)

0cos(ϑi) sin(ϕi)

ϕi ∀i ∈ {1 . . . n} 3-4

Using this particular representation we can reduce each qi to ϑi and ϕi while also prevent-ing singularities on the working space. In figure 3-2 we can see that the singularities are onthe qi values of [0, −1, 0]T and [0, 1, 0]T , which, due to the restrictions that will be imposedin section 3-5-1, are impossible to reach.


3-2 Election of State Variables 11

(a) ϑi mapping (b) ϕi mapping

Figure 3-2: Possible values of qi with color hue mapped to the values of ϑi in (a) and ϕi in (b)

3-2-2 Reducing wi

We can also reduce each wi to ϑi and ϕi.using the following equation which derives fromequation 2-2 and the fact that qi · wi = 0:

wi = qi × qi =

cos(ϕi)

0sin(ϕi)

ϑi + 12

sin(2 ϑi) sin(ϕi)− cos(2 ϑi) − 1

− sin(2 ϑi) cos(ϕi)

ϕi ∀i ∈ {1 . . . n} 3-5

These two reductions leave us with the following state vector:

x = [pl, ϑ1, ϕ1, ϑ2, ϕ2 . . . ϑn, ϕn, pl, ϑ1, ϕ1, ϑ2, ϕ2 . . . ϑn, ϕn, ss1, ss2 . . . ssn] 3-6

To use be able to generate the derivatives of the state vector (3-6) using the Equations ofMotion (EOMs) we need to compute qi and wi using equations 3-3 and 3-5 respectively. Wewill also need to go from wi to ϑ and ϕ.

In equation 3-3 we can see that ϑi is closely related to qi(2) and we can find that:

ϑi = sin(ϑi) ϑ2i + qi(2)

cos(ϑi)∀i ∈ {1 . . . n} 3-7

Where qi can be computed from wi using 2-2:

qi = wi × qi + wi × qi ∀i ∈ {1 . . . n} 3-8

Similarly, in equation 3-5 we can see that ϕi is closely related to wi(2) and we can find that:

ϕi = sin(2 ϑi) ϑi ϕi − wi(2)cos(ϑi)2 ∀i ∈ {1 . . . n} 3-9


12 MPC Formulation

3-3 Slack Variables

Slack variables are added to all inequalities in the MPC Problem. These variables have avery high cost added to them, this guarantees that they will always be zero if there are noproblems. The purpose of these variables is to prevent the problem from having no solutionand, therefore guaranteeing that the solver will always output something. For our problemwe use: s ∈ R≥0 and senv ∈ R. s can only be positive, while senv can be any value, and theyare used in different constraints (see section 3-5). The vector concatenating both variables isdenoted by s ∈ R2

3-4 Forces PRO formulation

Forces PRO uses the z vector to represent stages. This vector is the concatenation of theu, s and x vectors and has size 3n + 2 + nvar.

3-5 Constraints

Constraints are very useful, we can use them to prevent the predicted system from behavingin ways that would be impossible for the real system. They also have low effect on the totaltime of the solver as, while they increase the time of each iteration, they also reduce thesearch space, reducing the number of iterations needed to find the optimal solution.

3-5-1 State Variables Restrictions

Some variables in the z vector are restricted by inequalities, in equation 3-10 we can seeall of these inequalities.

−15◦ ≤ θi ≤ 15◦ ∀i ∈ {1 . . . n}−15◦ ≤ φi ≤ 15◦ ∀i ∈ {1 . . . n}

−2 m/s ≤ ¯zi ≤ 2 m/s ∀i ∈ {1 . . . n}0 ≤ s ≤ ∞

−∞ ≤ senv ≤ ∞−60◦ ≤ ϑi ≤ 60◦ ∀i ∈ {1 . . . n}−60◦ ≤ ϕi ≤ 60◦ ∀i ∈ {1 . . . n}

3-10


3-5 Constraints 13

3-5-2 Environment Constraints

The drones and the payload must stay inside the environment. As the inequalities neededfor this are bounded from both sides, we use senv, as it can also be negative.

−dim(1)

2−dim(2)

20

≤ pl + senv ≤

dim(1)

2dim(2)

2dim(3)

−dim(1)2

−dim(2)2

0

≤ pi + senv ≤

dim(1)

2dim(2)

2dim(3)

∀i ∈ {1 . . . n}

3-11

3-5-3 Wire Slackening Prevention

Our model does not take into account that the wire can be deformed or folded, instead,it is modeled as a solid beam. In early testing, it became apparent that the system wasoccasionally using the beams in order to push the payload, something impossible with wires.To prevent this from happening we need to add a constraint.

According to the literature[2], a wire goes from taunt to slack when the wire’s tensionbecomes negative. Therefore we must prevent the wire from having a negative tension. Toensure this we find the force exerted by the EOMs on each drone (denoted as Ti ∈ R3) andadd an extra inequality:

Ti = mipi − ge3 − FuDi ∀i ∈ {1 . . . n} 3-120 ≤ qT

i Ti + s ∀i ∈ {1 . . . n} 3-13

3-5-4 Drone Speed Limit

We need to limit the drone speed to keep the movement safe. To do that we limit thedrone speed:

‖pi‖ ≤ 2 m/s ∀i ∈ {1 . . . n} 3-14

3-5-5 Drone Distance

We need to keep a minimum distance between drones so that they don’t crash into eachother:

‖pi − pj‖ ≥ 0.4 m ∀i ∈ {1 . . . n}∀j ∈ {1 . . . n} 3-15


14 MPC Formulation

3-5-6 Collision Avoidance

As explained in section 2-2-5 (Obstacle Modeling), the obstacles are modeled as a box.However, for collision avoidance, we will use the smallest volume ellipsoid that contains it. Wedo this because it is important for the equations of the MPC to be as continuous as possibleso that the solver converges on a solution faster.

Calculating distances from a point to an ellipsoid can be difficult, as seen in [7]. However,we can use the approximation in equation 2-24 (given a point [x, y, z] and an ellipsoid withcenter [xc, yc, zc] and sizes a, b, c). This approximation is good if you only want to check itssign.

d =(

x − xc

a

)2+

(y − yc

b

)2+

(z − zc

c

)2− 1 3-16

To calculate this approximated distance for the whole system, we find the distance be-tween each of the wires and the expected position of the ellipsoid (assuming it follows linearmovement) and find the minimum value. We also need to add some extra space to the ellipseto prevent a collision when the drones do not follow exactly the model.

To do this we need a way to calculate the distance between an ellipsoid and a segment.We use the same equation described in the Appendix C of [6].

3-6 Objective function

The objective function consists of different objectives, each with their separate cost whichare all added together using different weights. We will denote costs and their respectiveweights as Cx and Wx. Each cost is calculated for all stages of the prediction and addedtogether, which is why a lot of the weights depend on the number of stages.

The objective function is really difficult to design, you can add a lot of different objectivesto it and make the controller more complex and more intelligent.

However, as you increase the complexity of the objective function is, the MPC solver hasmore trouble finding its optimal value. It is also recommendable to keep the function assmooth as possible, preferably with a continuous derivative, as the MPC solver relies on thederivatives to find the minimum.

This is why we will first introduce the most basic costs and more could be added if we hadextra computational power.


3-6 Objective function 15

3-6-1 Destination Objective

Cdest =n∑

i=0

(pi − pi)T

1 0 00 1 00 0 1.1

(pi − pi)

3-17

Wdest ={

5 if stage is final0 otherwise

3-18

Justification:

The whole objective of the system is to go from one point to another. This is accomplishedthanks to this objective.

Initially, we used the distance from the payload to the destination as an objective function,however, this poses a problem. The solver uses the inputs to minimize total cost, therefore itis best if the objective function is as closely related as possible to the inputs. This does nothappen if we use the payload position, as the input force is applied to the drones, and thenthey pull from the payload.

Therefore we ended up using the distance between the drones and the desired position ofthe drones, which is computed from the desired position of the payload and desired suspensionangles (using equations 2-1 and 3-3).

In order to prevent discouraging the system from performing complex movements, such asmoving away from the destination in order to dodge an obstacle, we will only optimize thisobjective for the final stage.

In order to get a simpler derivative of this cost term, we do not calculate the distance butthe squared distance.

We also weight the z coordinate of the distance slightly higher than the x and y coordinatesso that the system tries to stay in the same horizontal plane as the objective if possible.

3-6-2 Final Speed Minimization

Cspeed =n∑

i=0

(pT

i pi

)3-19

Wspeed ={

10−2 if stage is final0 otherwise

3-20

Justification:

When the system can already reach the objective within the time-horizon, it is best tomake sure that the final stage has also minimal velocity. This is not completely necessary, aswe only use the first stages for drone input, however it makes the plan be more realistic.


16 MPC Formulation

While using this cost across all stages would help stabilize the system, it is best not todo so as it is a really complex equation (we need to use equation 3-4 and the derivative ofequation 2-1).

The weight is set very low to prevent this cost from discouraging movement.

It was also found that adding this cost increased convergence when the system is near theobjective, as the last planner solution is more similar to the optimal plan.

3-6-3 Input Minimization

Cinput =n∑

i=0

(uT

i ui

)3-21

Winput = 10−2

N3-22

Justification:

The aim of this cost is to prevent the system from doing unnecessary movements, especiallywhen it has already reached the destination. This helps stabilization and is really simple tocalculate.

In order to prevent this cost from discouraging system movement, its weight is low com-pared to the other ones.

3-6-4 Slack Minimization

Cslack = sT s 3-23

Wslack = 105

N3-24

Justification:

As explained in section 3-3 slack variables should always be zero and this is accomplishedby this cost. The weight of this should be really high, as slack variables only have to be usedif it is impossible to find any other solution.


Chapter 4

System setup and control algorithm

As explained in the introduction, the control of the algorithm is centralized, therefore asingle agent controls the whole system. A Motion Capture System (MCS) is used to determinethe position and attitude of the drones.

4-1 Hardware and properties

This is the hardware used in the thesis:

• MCS by OptiTrack. The setup consists of 10 cameras filming at around 120 Hz.• 4 Quadcopters (Bebop2 by Parrot, Figure 4-3).• 1 Gamepad (Logitech Wireless Gamepad F710, Figure 4-5)• Laptop (HP Pavilion 15-au007ns)• Desktop computer (DELL OptiPlex 7050)

4-2 Workspace

The workspace of the drones, seen in figure 4-1, consists on a 6.0×3.0×2.6 m (L×W ×H)arena where they can move freely while being tracked by the MCS. The MCS setup consistsof 10 cameras such as the one seen in figure 4-2. The drones, payloads and obstacles areequipped with reflective markers to be tracked by the MCS. The markers can be seen infigures 4-3 and 4-4.


18 System setup and control algorithm

Figure 4-1: Drones workspaceFigure 4-2: MCS camera

Figure 4-3: Bebop2drone with markers

Figure 4-4: Hat withreflective markers

Figure 4-5: Gamepadby Logitech


4-3 Control algorithm 19

4-3 Control algorithm

The basics of the control algorithm will be explained for simulation and the necessaryextensions for the experimental setup will be explained in section 4-3-3 (Experimental setup).

4-3-1 Simulation Algorithm

We use the following control algorithm: The system starts in a predefined position. Then,using FORCES PRO, a Model Predictive Control (MPC) plan is generated such as the firststage equals the system state.

The algorithm assumes that the system follows this plan for one time-step (of ∆t) andcalculates the new state using the Equations of Motion (EOMs). The process is then repeatedusing the new state. A schematic of this solution can be seen in figure 4-6.

During this process, obstacles are also simulated according to predefined movements (thatthe controller is not aware of).

This data is sent through Robot Operating System (ROS) to a visualizer, which will befurther explained in section 4-5

External Planner

An additional simulation algorithm was tested and, although it did not prove useful, wewill also describe it.

The previous solution has a problem, if the MPC solver takes a long time calculating theplan(longer than 1/∆t s) the simulation will run slower than real time and in the experimentalsetup the quadrotors will keep repeating the first input for longer than the planner intended.

This can be solved if we separate the controller from the planner. The planner keeps re-planing using the controller’s data as fast as possible, while the controller runs the simulationusing inputs from the plan generated by the planner.

To determine the inputs for each quadrotor, the controller finds the plans stage where thedrones planned position is closest to the actual position. A schematic of this solution can beseen in figure 4-7.

When doing simulated experiments, it was found that cutting the maximum number ofiterations of the MPC solver in the initial algorithm was better than using an external planner.



Controller

Visualizer

Plan

Objective

MPC planner

EOMs

InputsPosition

Figure 4-6: Simulation algorithm

Controller

Visualizer

Closest stage

EOMs

Inputs

Position

Planner

ObjectivePlan

Plan

Figure 4-7: External planner

4-3-2 Initial Solution

When solving the MPC problem it is very important to give the solver a good initialsolution. To do this we record the previous solution and update it for the next stage.

If we are using the initial algorithm, the stage will be equal to the second stage in theprevious MPC solution. Therefore, we can shift the last solution by one stage and add a newstage at the end using the EOMs.

Initial Solution for the External Planner

The above solution is not suitable for the external planner algorithm as the system couldhave gone through many stages of the plan.

To do this we compute the path that the controller would follow using the last solutionusing the same functions that the controller uses to determine the inputs from the plan.

This initial solution is not good enough and is one of the main causes the external planneralgorithm performs worse than the original algorithm.

4-3-3 Experimental setup

As data from the drones tends to be inaccurate (it depends on GPS data and internalassumptions), we will use the MCS to determine the position and attitude of the drones.

A Kalman Filter (KF) is used to estimate the state of the system based on the EOMsand the data from the MCS. KF is also used to estimate the location and velocity of theobstacles, which are also tracked with the MCS.


4-4 Data communication 21

4-4 Data communication

All data goes through the ROS network, this means that we can separate different tasksthroughout several computers and increase performance. The tasks are:

• ROS core: One of the computers has to start the ROS core, this is a node that managesall communications through the ROS network.

• MCS data: Motive, a proprietary software by OptiTrack, computes the 3D locationof the objects based on their markers. This data is then translated by a ROS node andfed to the ROS network.

• Gamepad input: A gamepad is connected to one computer through Bluetooth. AROS node in the same computer feeds the controller inputs to the ROS network. Thegamepad lets the user control the drones manually before turning on MPC.

• Drone commands: Bebop2 drones create their own wireless network in order to com-municate with other hardware. For each drone, a ROS node receives commands fromthe ROS network and sends them through the drones wireless network.

• Controller: As explained in more detail in section 4-3 (Control algorithm), a controllerwritten in MATLAB is in charge of getting the data from the MCS and estimate the currentstate of the drones, payload and obstacles. Based on this information, it also generatesthe commands for the drone. If the experiment is running in simulation mode, this nodeis responsible for executing the simulation.

• Planner: As explained in more detail in section 4-3 (Control algorithm), a plannerwritten in MATLAB gets the position estimation from the controller and generates a planfor a fixed time horizon. Multiple planners can run at the same time.

• Visualizer: A visualizer, written in MATLAB, displays a Graphical User Interface (GUI)with the estimated position of the system and the obstacles, as well as the latest plan.From the GUI it is also possible to change the objective location which is shared throughROS.

In figure 4-8 a schematic for the whole control algorithm can be seen, including the commu-nications for the experimental setup.

4-4-1 Rosbags

One of the advantages of using ROS for data communication is that we can use rosbags.Rosbags let us record all data into a file and then replay it. Even more, we can parse thesefiles using MATLAB and analyze the data as we wish.



Controller

Closest stage

Estimator

Mode select

Control Inputs

Motive MCS node

Ethernet Broadcast

Visualizer Position

Planner

ObjectivePlan

Plan

Gamepad Gamepad node

BluetoothManual Inputs

MCS Cameras

Ethernet

Bebop node 2

Inputs

Bebop node 1

Bebop node n

Drone 1 Drone 2 Drone n

Wifi Wifi Wifi

Real Movement

MCS Data

Legend:

Hardware

Driver

Software

ROS Network

Figure 4-8: Communications schematic

4-5 Visualizer

The visualizer is a GUI created with MATLAB. A screenshot of the GUI can be seen infigure 4-9. Through this interface we can:

• See a representation of the system state and the obstacles in three dimensions.The drones are represented in black.The wires are seen as blue lines.The payload is drawn as a red dot.The obstacles are displayed as gray boxes.The ellipsoids around the obstacle are shown as a brown surface.The planned trajectories of the drones are displayed as pink lines.The planned trajectory of the payload is shown as a dashed pink line.The current objective is drawn as a pink dot.

• See the numeric values of the current system state and inputs.• Restart the simulated movement of the obstacles.• Change the objective.• Load and replay recorded data from a rosbag file.


4-5 Visualizer 23

Figure 4-9: Screenshot of the visualizer GUI




Chapter 5

Results

5-1 Simulation Scenario

To do tests we use a simple scenario. Three static obstacles represent a fence which thesystem has to overcome. We set the simulation so that the drone has to move back and forth,jumping the fence several times, this is accomplished by switching the start and end pointswhen the distance from the payload to the destination is less than .3 m. The simulation runsfor 20 seconds or until the solver cannot find a solution for 10 consecutive iterations. All thevariables of the scenario can be seen in table 5-1. An example of a path taken by the systemcan be seen in figure 5-1.

5-2 Estimating time per iteration of the Model Predictive Control(MPC) solver

In order to set the maximum amount of iterations of the MPC solver (nmaxit), we need toestimate the total time each iteration takes (denoted as tit). Even without access to the codefrom FORCES Pro, it is easy to guess that the time per iteration will depend linearly on thenumber of stages N but it will not depend on ∆t.

To check this assumption we run the experiments using the initial algorithm (no externalplanner) with ∆t ∈ {.1, .05} and N ∈ {10, 11, 14, 17, 20, 24, 29, 34, 41, 50} and on each step ofeach experiment we record the solve time and the number of iterations required by the solver.

Variable: n dim pl pl ϑ1 ϕ1 ϑ2 ϕ2 pobs1 dimobs1 pobs2 dimobs2 pobs3 dimobs3

Value: 2

63

2.6

−2.50.7

2.50.7

45◦ 0◦ -45◦ 0◦

0.01−1.20.6

0.20.21.2

0.011.20.6

0.20.21.2

0.01.010.5

12.41

Table 5-1: Simulation Scenario Data


26 Results

Figure 5-1: Path of system with N = 20, ∆t = .05

Then we plot the time per iteration divided by N using several box-plots. The results of thiscalculation can be seen in figure 5-2.

Using these results we can see that most iterations run under .1 ms per N, so we can setnmaxit using the following equation:

nmaxit =⌊ ∆t

10−5 ∗ N

⌋5-1

For a different number of drones or obstacles, it is clear that this constant will vary.However, we will not try to model this as the relation is non-linear. Ideally, it would bebetter to use a solver that can be limited by solve time instead of by the number of iterations.In figures 5-3 and 5-4, we can see how varying the number of drones or obstacles affects thesetimes.

5-3 Step time outside the MPC solver

When the ∆t is set to a low amount, the time of the functions other than the MPCsolver starts to be relevant. We can calculate this time subtracting the MPC solve time(tMP C ∈ R+) from the time of the control loop (tstep ∈ R+) We calculated these times usingthe same parameters as the ones used in section 5-2.

As it can be seen in figure 5-5, these results do not seem to follow a pattern that is easyto model, so we will not try to model this and will assume that they are zero, this howeverwill make the loop time slower than intended when setting ∆t to a small value.


5-3 Step time outside the MPC solver 27

Figure 5-2: Graph of tit/Nfor ∆t ∈ {.1, .05} and N ∈ {10, 11, 14, 17, 20, 24, 29, 34, 41, 50}

A horizontal line is drawn at .08 ms

Figure 5-3: Scaling of tit/N when varyingn with nobs = 3

Figure 5-4: Scaling of tit/N when varyingnobs with n = 2


28 Results

Figure 5-5: Graph of tstep − tMP C

for ∆t ∈ {.1, .05} and N ∈ {10, 11, 14, 17, 20, 24, 29, 34, 41, 50}

5-4 Finding best parameters

We then ran some more tests to find the best parameters. We tested the same problem as inthe previous sections with different ∆t and different time horizons (defined by thorizon = N∆t

). For each time horizon N is found using the formula N =⌈

thorizon∆t

⌉. The results of these

experiments are seen in figure 5-6.

From these results, and from further inspections on the data we concluded the following:

• If the time horizon is too low, the system will break constraints more often. This makessense as with a low time horizon the drone does not have time to react to the obstacles.As a good rule of thumb, the time horizon should be greater than the time needed forthe system to stop.

• If the time horizon is too high the N is high and following equation 5-1,nmaxit will besmaller. Therefore, the quality of the results decrease (this cannot be seen in figure 5-6but on inspection of the data obtained).

• When the ∆t is too high (1/∆t too low), the system tends to have more problems. Thisis mainly due to the errors that come from a rough discretization of the dynamics.

• When the ∆t is too low (1/∆t too high), as explained on section 5-3, the loop timeoutside the MPC solver will not be negligible and the loop will run slower than intended.

Taking into account all of this information, it was decided to have a time horizon of 1.2 sand a ∆t of 0.083 s (12 Hz). Therefore the N will have a value of 15.


5-4 Finding best parameters 29

Figure 5-6: Graph of 1/tstep for1/∆t ∈ {8.0, 9.08, 10.3, 11.7, 13.3, 15.1, 17.1, 19.4, 22.0, 25.0}Hz and

thorizon ∈ { 12 , 2

3 , 56 , 1, 7

6 , 43 , 3

2 , 53 , 11

6 , 2}sWhen the system broke any constraint for more than 1% of the time it is marked as "Broke"When the system did not manage to cross the fence more than 3 times it is marked as "Bad"

A line marks the tstep < ∆t maximum.


30 Results

5-5 External planner testing

When we are using an external planner, we can no longer use the assumption that thedrone has followed exactly the MPC plan. Therefore we need to find the initial solution forthe MPC solver using the algorithm described in section 4-3-2.

When testing the external planner algorithm we set the maximum number of iterations toa factor of the previous calculation. This factor, k ∈ R+, sets the maximum amount of stagesthe controller could perform without a new plan.

nmaxit =⌊

k∆t

10−5 ∗ N

⌋5-2

As it was explained before, this algorithm was not successful, as it performed worse thanthe original algorithm. This was true for several values of k tested. The possible causes are:

• The initial solution is poor, this means the solver takes more time to find the optimalsolution and therefore, the next plan will have an even worse initial solution.

• The algorithm to determine the best inputs depending on the location of the drone doesnot perform well when the drone is far away from the plan.

• Even when the drone is in the path of the plan, it can be in between two of the plannedstages, this causes the drone to follow inputs that were meant to be executed at aslightly different time.

5-6 Demonstration of Maneuvers

We will now demonstrate some maneuvers that our system is able to calculate. A videoof these maneuvers can be seen in https://youtu.be/lECcAQC_coE.

5-6-1 Moving obstacle avoidance

To demonstrate moving obstacle avoidance a scenario is designed where two drones movefrom side to side while some obstacles move perpendicularly through the optimal path.

In order to make it easier to visualize the movement from a top-down view, the obstaclesare set to have a very large height, much larger than the workspace dimensions.

In figure 5-7 we can see one of these maneuvers:

In (5-7a) we see how the system is dodging obstacle 2. However, it is not doing anythingto prevent a collision with obstacle 1, as it is not yet within the time horizon of the controller.

In (5-7b) we see how obstacle 1 is now on the system’s time horizon. Quadrotor 4 hasalready found a way to avoid the obstacle while quadrotor 3 is trapped, it cannot go forwardas it assumes that the obstacle will be there in the future and it cannot turn to the rightas it would crash against quadrotor 4, therefore the quadrotor 3 plans to stop as closely aspossible to its objective. Also, note how quadrotor 3 stops in a way that makes the payloadswing backward, avoiding a collision between the payload and the obstacle.


https://youtu.be/lECcAQC_coE

5-6 Demonstration of Maneuvers 31

(a) Starting position of maneuver (b) One drone stops

(c) A solution is found (d) The system avoids the obstacle

Figure 5-7: Moving obstacle avoidancePlotted with the same rules as the ones explained in section 4-5 (Visualizer)

Black lines have been added to represent the obstacles’ velocitiesFor each instance, the top view is shown

In (5-7c) quadrotor 4 has followed its plan and is moving forward. This has given quadrotor3 enough space to move behind quadrotor 4 and move towards its objective. Note how at theend of the planned path of quadrotor 3, it is not dodging the current position of the obstaclebut its expected position.

Finally, in (5-7d) we see how the system has successfully avoided the obstacle and is movingtowards the objective.

5-6-2 High swing maneuvers

In order to demonstrate maneuvers that utilize swing, we create a new scenario wheretwo drones have to overcome a horizontal barrier that is in the way. To prevent the dronesfrom simply going over it, we change the workspace limits to set a ceiling at a height of1 m. In order to exaggerate the movement, we increase the limit set to the swing angles inequation 3-10 from 60◦ to 85◦.

In figure 5-8 we can see one of these maneuvers:

In (5-8a) the drones have just encountered the obstacle within the system’s time horizonand have not yet found a solution to go over it, therefore the drones plan to get as close asthey can to the objective while staying behind the obstacle.


32 Results

(a) The system does not yet know how to pass the obstacle

(b) The system seems to have found a solution

(c) The system moves closer

(d) The system swings the payload over the obstacle

Figure 5-8: High swing maneuverPlotted with the same rules as the ones explained in section 4-5 (Visualizer)

For each instance, two side views are shown


5-7 Experimental Results 33

Figure 5-9: Real distance between the payload and the quadrotors

In (5-8b) the system seems to have found a solution to overcome the obstacle and thedrones are starting to execute it.

In (5-8c) the system has moved closer to the obstacle, and we can more clearly see whatthe plan was. Both drones have positioned themselves perpendicularly to the obstacle andare planning to swing the payload to the side while they go above the obstacle. This highlycomplex maneuver is only possible thanks to having a high time horizon and a good definitionof the cost function.

In (5-8d) we see how the payload is swinging above the obstacle and how each of the dronestarts planning a path to move to their respective objectives.

5-7 Experimental Results

Due to time limitations, very few experiments were performed with real drones and onnone of them we got good enough results.

Observing the logs of data obtained from these failures it was found that the systemwas generating correct MPC plans and the system seemed to follow them correctly, which ispromising. However, the length of the cables did not match the real distance between thepayload and the quadrotors and, as in our model the position of the drones is determined bythe position of the payload and the suspension angles (equation 2-1), this causes the positionof the drones to be different in the model than in reality and the system destabilizes.

A boxplot of the real distances can be seen in figure 5-9, we can see how these distancesvary considerably. This is because the cables are not attached to the center of mass of thedrones but to a different point.

With more time to experiment it could be possible to attach the drones differently orcontrol the payload using longer cables so that this difference is minimized. Another solutionwould be to set the length of the cables as a parameter of the MPC solver and set it to the realdistance of the drones. It would also be a possibility to change the dynamic model to accountfor the difference the swing point of the cables and the center of mass of the quadrotors. Ahigher control rate would also help stabilize the system.


34 Results


Chapter 6

Conclusion

In this thesis we introduced a new approach to the control of multiple Unmanned AerialVehicles (UAVs) carrying a slung payload through wires. This controller can be adapted to avariable number of quadrotors, obstacles and different environments. It is an agile controllerable to generate complex maneuvers on the go even with uncertainties. It is able to dynami-cally adapt to changing environments, such as unexpected movement of obstacles or changeof objective. The dynamic model is free of singularities, takes into account aerodynamicdrag. Constraints prevent the system from doing unsafe movements and avoid wire slacken-ing. The controller has been demonstrated to work in real-time simulated environments andthe experiments on real drones, although unsuccessful, look promising.

6-1 Future Studies

For future studies we recommend the following:

• Do more experimental studies of the controller.• Test the system with a wider variety of parameters, such as heavier payloads or longer

strings.• For control of systems with more than three drones, it is almost impossible to prevent

some of the wires from slackening, therefore a dynamic model that includes wire slack-ening would be preferred. Maybe a model where each of the wires can switch modessuch as in [8]

• A more realistic model of the strings, where their mass is not neglected and they actmore as springs rather than rigid cables would improve the accuracy of the dynamicequations.

• To improve realism and maybe fix the problems described in section 5-7 it would beinteresting to describe the string attachment points on the drone and on the payload.

• When the payload gets heavier, it would also be necessary to model it as a rigid bodyinstead of as a point mass. This is done in [10].


36 Conclusion

• The complex dynamic equations could be substituted by some artificial intelligencewhich mimics the dynamic movement of the system.

• Drones with more direct inputs could be used so that we would not need to generatemodels of the internal dynamics.

• It would be ideal to switch the Model Predictive Control (MPC) solver to one whichcan be limited by solve time instead of by the number of solver iterations.

• An on-board control could be tested to rid the controller from the need of a MotionCapture System (MCS).

• De-centralized control could be attempted for better scalability to an increase in thenumber of drones.


Appendix A

Project Planning

A-1 Planning and Scheduling

The thesis started on 1st February, 2018 and will end on 17th June, 2018, the week beforethe presentations start. This gives us an approximate project duration of 5 months.

The research is done in the laboratory at Cognitive Robotics Department (CoR) and anaverage of 40 hours a week are dedicated to the project.

Every two weeks, a meeting is scheduled with prof. Javier Alonso-Mora to demonstratethe work done and discuss the development of the project. The CoR Department also has amonthly meeting where the projects of all students in CoR are demonstrated and discussed.

This scheduling is not strictly enforced and will change depending on the availability ofboth the professor and me.

A-2 Task Description

A-2-1 Paper reading

Even before the project started, I began reading papers related to the thesis. Some wererecommended by prof. Javier Alonso-Mora, such as the Master Thesis my project was goingto be based on, by Nikhil D. Potdar [6]. I also read previous work by Alonso-Mora [3, 9] andsome related work such as Taeyoung Lee’s work in [4, 10]

This reading had a duration of about two weeks and helped me better understand thework ahead of me and familiarize myself with how the problem could be solved.


38 Project Planning

A-2-2 Installing and familiarizing myself with Robot Operating System (ROS)

The next step was to install ROS and understand how it worked. I chose to install ROSKinetic, as this was the version of ROS used in the laboratory. This meant I had to doa fresh installation of Ubuntu 16.04 as ROS Kinetic only supports Wily (Ubuntu 15.10),Xenial (Ubuntu 16.04) and Jessie (Debian 8). To install ROS I followed the instructions athttp://wiki.ros.org/kinetic/Installation/Ubuntu.

To better understand ROS I followed the official tutorials available at http://wiki.ros.org/ROS/Tutorials. I followed these tutorials until the intermediate level and tested myconnection through the laboratory’s Ethernet network.

I also Installed the ROS Drivers for the Drone which are available at http://bebop-autonomy.readthedocs.io/en/latest/# and the node for communication with the Motion CaptureSystem (MCS), which is available at http://wiki.ros.org/mocap_optitrack. I also testedcommunication with the drones and with the MCS.

Finally, I installed MATLAB (version R2017b) and the Robotics System Toolbox (version1.4) to communicate with MATLAB with the ROS Network. I also did some tests to familiarizemyself with the toolbox.

This process took about a week and a half, as I ran into problems with my Ubuntu instal-lation and had to repeat the whole process several times.

A-2-3 Understanding Nikhil D. Potdar’s Code

Nikhil D. Potdar’s Thesis was in control of a single drone carrying a payload, and therefore,his code could be very useful for my research. This is why I dedicated some time to readthrough his code and understand every decision made in the code by reading his thesis [6]in a lot of detail. To get used to the code and better understand it I implemented someadditional features on his code, such as inter-drone collision avoidance when multiple dronesare carrying payloads. This process took two weeks.

A-2-4 Finding an accurate dynamic model

The next step was to find an accurate dynamic model for the multiple drone system. Myfirst approach was to use something similar to what Nikhil D. Potdar used in his thesis,which consisted on defining the state using MATLAB’s symbolic toolbox and, calculating theLagrangian L of the system, and solving Lagrange’s equation:

d

dt

(∂L

∂q

)− ∂L

∂q= 0

However, as the multi-drone system was far more complex than the single-drone system,these equations were much more complex, this resulted in extremely long equations that werevery badly optimized. I then tried to use Wolfram Mathematica’s FullSimplify to simplifythe equations, but they were so big that not even Mathematica could simplify them.

Finally, I used a different approach, I used the dynamic model introduced by TaeyoungLee in [4] and later simplified it for better performance. This task took approximately threeweeks.


http://wiki.ros.org/kinetic/Installation/Ubuntu

http://wiki.ros.org/ROS/Tutorials

http://wiki.ros.org/ROS/Tutorials

http://bebop-autonomy.readthedocs.io/en/latest/#

http://bebop-autonomy.readthedocs.io/en/latest/#

http://wiki.ros.org/mocap_optitrack

A-2 Task Description 39

A-2-5 Transforming Nikhil D. Potdar’s Code for the multiple drone case

The next step was to transform Nikhil D. Potdar’s Code for the multiple drone case.The code had to be almost entirely rewritten, as a lot of parameters have to be added, thestate vector and therefore, most of the functions have to be adapted to work with a variableamount of drones. The process is also very time consuming as the code uses a Model PredictiveControl (MPC) problem solver called FORCES PRO. This solver has to be generated everytime a change is made in the MPC problem and, as the equations are much more complex,FORCES PRO takes a really long time to generate a new solver.

The total time invested in this task was about two weeks.

A-2-6 Adding constraints and tweaking cost function

The MPC problem has a lot of constraints and parameters that need to be tweaked andadjusted throughout the project. This task has been done in one week, but it continuedthroughout the project as new constraints came to my mind.

A-2-7 Optimizing the code for real-time performance

After transforming the code and running it in simulation, it became apparent that someoptimization was required. Several methods were applied, that will be explained further inthe final thesis. This process is still ongoing at the time this report was written and itsexpected duration is of three weeks.

A-2-8 Writing the thesis

The thesis was written in parallel to many of the other tasks described previously, andwill continue until the end of the project.

A-2-9 Testing the planner in different scenarios

When I am pleased with the results and speed of the planner, I will proceed to test it indifferent simulated scenarios. If I get a good enough planner speed in these scenarios, I willproceed to test it with real drones and document the results.

The expected duration of this process is of two weeks.


40 Project Planning

A-3 Gantt chart

TODAY

February March April May June

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22

Month:

Week:

Preamble

Paper reading

ROS installation

Understand code

Code development

Dynamic model

Transforming code

Adding constraints

Optimizing

Testing

Testing

Extract conclusions

Final Stage

Write GEP report

Write Thesis

End project

Presentation

Figure A-1: Gantt chart of the project


Appendix B

Budget and Sustainability

B-1 Sustainability auto-evaluation

When doing a project, there are several fields in which sustainability is necessary in orderto prevent doing harm. Social sustainability is needed to maintain the lifestyle and health ofour current society. Environmental sustainability helps keep our environment and, therefore,the planet in a good condition. Economic sustainability ensures the project will not be aneconomic failure.

In Information Technology projects, sustainability must also be considered, even if whatyou are creating is only software. Software can, for example, cause an increase in energyconsumption (like Bitcoin) or not be profitable enough. That is why in this project it is alsonecessary to evaluate its impact on the planet.


42 Budget and Sustainability

B-2 Sustainability and Social Commitment

PPP Useful life Risks

Environmental Design consumption Ecological footprint Environmental risks9/10 10/20 -5/-20

Economical Bill Viability plan Economical risks5/10 15/20 -3/-20

Social Personal impact Social impact Social risks7/10 19/20 -4/-20

Sustainability range 21/30 44/60 -12/-6053/90

Table B-1: Sustainability matrix of the project

B-2-1 Environmental Dimension

During the research of the project, the amount of energy required is relatively low. Wejust need one computer, we can calculate it using my laptop’s battery properties.

19.5 V · 3.33 A · 860 h = 55.84 kW h

We also need to charge the drones, which using the drone’s battery properties we get:

2700 mAh/charge · 11.1 V · 100 charges = 3 kW h

The rest of energy consumed, such as the Motion Capture System (MCS) and the labo-ratory lights are not included as they would be on even if the thesis would not have beendeveloped.

The total is 58.84 kW h which is extremely low compared to the amount consumed by theentirety humans in five months, which is 71.2 PW h.

B-2-2 Social Dimension

As stated in the Introduction (chapter 1), the results from this thesis have a very bigimpact on social life, as it pushes forward a technology that would be very useful in manyaspects of social life. The technology could be used to save lives in natural disasters and tomake life easier in general.

As for the possible ethical implications, the only negative one is that the use of drones canalso be used in military settings. However, this is something that is inevitable and researchersshould not stop doing research because it might benefit the military.


B-3 Project Budget 43

B-2-3 Economical Dimension

In section B-3 (Project Budget) a detailed description of all the costs involved to make theresearch happen. In the long term, the project should have a positive impact in the economy,as it would mean smaller drones are required to do tasks that until now are done by biggerdrones, reducing the cost of maintaining a fleet of drones.

B-3 Project Budget

B-3-1 Hardware Budget

To run the controller we need a laptop and the rest of the equipment is provided free ofcharge by the lab at Cognitive Robotics Department (CoR) and therefore does not affect thebudget of the project. Table B-2 shows an estimation of the hardware costs related to theproject.

Product Price Units Useful Life Amortization

HP Pavillion Laptop 15-au007ns 800 e 1 5 years 67 eLaboratory equipment 0 e 1 – –Total: 0 e 67 e

Table B-2: Hardware Budget

B-3-2 Software Budget

Table B-3 shows the software used to develop this thesis. It is important to note thatForces PRO was used through a 6-months research license and MATLAB used a student licensegranted by Facultat d’Informàtica de Barcelona (FIB).

Product Price Units Amortization

MATLAB and toolboxes 0 e 1 0 eForces PRO 0 e 1 0 eLATEX 0 e 1 0 eGitHub 0 e 1 0 eGitKraken 0 e 1 0 eTotal: 0 e 0 e

Table B-3: Software Budget



B-3-3 Human Resources Budget

In table B-4 we can find an estimation of the time required for each task. Using the totalsof this estimation, we compute the Human Resources Budget in table B-5.

Task Duration DedicationProject Manager Software Developer Tester

Paper reading 80 h 20 h 60 hROS installation 60 h 15 h 45 hUnderstand code 80 h 20 h 60 hDynamic model 120 h 30 h 60 h 30 hTransforming code 80 h 20 h 40 h 20 hAdding constraints 20 h 5 h 10 h 5 hOptimizing 60 h 15 h 30 h 15 hTesting 20 h 20 hExtract conclusions 20 h 5 h 15 hWrite GEP report 80 h 80 hWrite thesis 160 h 80 h 80 hEnd project 40 h 20 h 20 hFinal Presentation 40 h 20 h 20 h

Total: 860 h 330 h 440 h 90 h

Table B-4: Task Time Estimation

Role Hours e/hour Salary

Project Manager 330 35 11.550 eSoftware Developer 440 20 8.800 eTester 90 15 1.350 e

Total: 860 – 21.700 e

Table B-5: Human Resources Budget


B-3 Project Budget 45

B-3-4 Unexpected Costs

In table B-6 we add any unexpected costs that might come up.

Role Hours e/hour Salary

Project Manager 10 35 350 eSoftware Developer 20 20 400 eTester 10 15 150 e

Total: 860 – 900 e

Table B-6: Unexpected Costs

B-3-5 Indirect Costs

Other costs are included in table B-7 but these were all provided by Delft University ofTechnology (TU Delft) and therefore do not affect the project’s budget.

Products Price Units Cost

Electricity 0e/(kW h) 400 kW h 0 eInternet 0e/month 5 months 0 e

Total: – – 0 e

Table B-7: Indirect Costs



B-3-6 Total Budget

In table B-8 we add all the budgets mentioned previously and a 5% contingency for unex-pected expenses.

Concept Estimated Costs

Hardware 67 eSoftware 0 eHuman Resources 21.700 eUnexpected Costs 900 eIndirect Costs 0 e

Subtotal: 22.600 eContingency (5%) 1.130 e

Total: 23.730 e

Table B-8: Indirect Costs


Bibliography

[1] E. Darack, “A Brief History of Quadrotors,” Air&Space (Smithsonian), 2017.

[2] P. E. Pounds, D. R. Bersak, and A. M. Dollar, “Stability of small-scale UAV helicoptersand quadrotors with added payload mass under PID control,” Autonomous Robots,vol. 33, no. 1-2, pp. 129–142, 2012.

[3] J. Alonso-Mora, S. Baker, and D. Rus, “Multi-robot formation control and object trans-port in dynamic environments via constrained optimization,” International Journal ofRobotics Research, vol. 36, no. 9, pp. 1000–1021, 2017.

[4] T. Lee, K. Sreenath, and V. Kumar, “Geometric control of cooperating multiple quadro-tor uavs with a suspended payload,” Proceedings of the IEEE Conference on Decisionand Control, pp. 5510–5515, 2013.

[5] G. Wu and K. Sreenath, “Geometric control of multiple quadrotors transporting a rigid-body load,” Proceedings of the IEEE Conference on Decision and Control, vol. 2015-Febru, no. February, pp. 6141–6148, 2014.

[6] N. D. Potdar, Online Trajectory Planning and Control of a MAV Payload System inDynamic Environments. Master’s thesis, TU Delft, 2018.

[7] L. MAISONOBE, “Quick computation of the distance between a point and an ellipse,”no. September 2003, pp. 1–14, 2006.

[8] M. Bisgaard, J. D. Bendtsen, and A. L. Cour-Harbo, “Modeling of Generic Slung LoadSystem,” Journal of Guidance, Control, and Dynamics, vol. 32, no. 2, pp. 573–585, 2009.

[9] J. Alonso-mora, E. Montijano, T. Naegeli, O. Hilliges, and D. Rus, “Distributed Multi-robot Formation Control in Dynamic Environments,” Autonomous Robots, 2018.

[10] T. Lee, “Geometric Control of Quadrotor UAVs Transporting a Cable-Suspended RigidBody,” IEEE Transactions on Control Systems Technology, vol. 26, no. 1, pp. 255–264,2018.


48 Bibliography


Glossary

List of Acronyms

3mE Mechanical, Maritime and Materials Engineering

CoR Cognitive Robotics Department

TU Delft Delft University of Technology

UPC Universitat Politècnica de Catalunya

FIB Facultat d’Informàtica de Barcelona

CFIS Centre de Formació Interdisciplinària Superior

AGAUR Agència de Gestió d’Ajuts Universitaris i de Recerca

UAV Unmanned Aerial Vehicle

MPC Model Predictive Control

NMPC Nonlinear Model Predictive Control

RK2 2nd order Runge-Kutta

KF Kalman Filter

GUI Graphical User Interface

MCS Motion Capture System

ROS Robot Operating System

COM Center Of Mass

ENU East-North-Up

EOMs Equations of Motion

PID Proportional-Integral-Derivative


50 Glossary

List of Symbols

Greek Symbolsφi ∈ R Pitch of the i-th quadrotorθi ∈ R Roll of the i-th quadrotorϕi ∈ R Second payload angle of the i-th quadrotorϑi ∈ R First payload angle of the i-th quadrotor

Latin Symbols∆t ∈ R+ Time between stages in the Model Predictive Control (MPC) formulatione3 ∈ R3 Down vector, [0, 0, −1]T

FDi ∈ R3 Aerodynamic drag on the i-th droneFDl ∈ R3 Aerodynamic drag on the payloadFuDi Addition of Fui and FDi

Fui ∈ R3 Input force of the i-th quadrotorF

‖ui ∈ R3 Orthogonal projection of Fui along qi

F ⊥ui ∈ R3 Orthogonal projection of Fui to the plane normal to qi

Mq ∈ S3+ Mass Matrix

pi ∈ R3 Position of the i-th quadrotorpl ∈ R3 Payload’s locationqi ∈ S2 Direction of the i-th linkTi ∈ R3 Tension exerted on the i-th dronewi ∈ R3 Rotation of the i-th link’s directionz ∈ R3n+2+nvar Asddim ∈ R3

>0 Environment dimensionsdimelli ∈ R3 Ellipsoid dimensions of the i-th obstacledimobsi

∈ R3>0 Dimension of the i-th obstacle

pobsi∈ R3 Position of the i-th obstacle

ssi ∈ R6 System states for the i-th quadrotorsszi ∈ R2 System states of hz for the i-th quadrotorssφi ∈ R2 System states of hφ for the i-th quadrotorssθi ∈ R2 System states of hθ for the i-th quadrotorCx Cost of objective xhz Identified system for z

hφ Identified system for φ

hθ Identified system for θ

k ∈ R+ Maximum iteration factor for external plannerkDi ∈ R Drag constant of the i-th quadrotorkDl ∈ R Drag constant of the payloadli ∈ R>0 Length of the i-th link


51

mi ∈ R>0 Mass of the i-th quadrotorml ∈ R>0 Payload’s massN ∈ Z+ Number of stages hin the MPC formulationn ∈ Z>0 Number of quadrotorsnmaxit ∈ Z+ Maximum number of iterations of the MPC solvernobs ∈ Z≥0 Number of obstaclesnvar ∈ Z>0 Number of variables in the state vectorslack ∈ R≥0 First slack variableslackenv ∈ R Second slack variablethorizon ∈ R+ Planning time horizontit Time per iterationtMP C ∈ R+ MPC solve timetstep ∈ R+ Control loop timeWx Weight of objective x

OtherS2 ⊂ R3 Set of unit vectors in R3

slacks ∈ R2 Slack variablesui ∈ R3 Inputs of the i-th quadrotorut ∈ R3n Inputs of all quadrotorsx ∈ Rnvar State vector


Documents

Bachelor Thesis: Planning and Control of a Multiple