3D formations of unmanned aerial vehicles Zden ek Kasl · cable for a formation of autonomous...

Czech Technical University in Prague

Faculty of Electrical Engineering

Diploma thesis

3D formations of unmanned aerial vehicles

Zdenek Kasl

Supervisor: Dr. Martin Saska

May 2013

ii

iv

vi

viii

Acknowledgement

First of all, I would like to thank to the supervisor of this thesis, Dr. Martin Saska, who

dedicated a lot of time and patience to help me with this thesis. My thanks also belong

to my family for their support.

ix

x

Abstract

A 3D extension of the leader–follower approach of formation trajectory planning, appli-

cable for a formation of autonomous quad–rotor Unmanned Aerial Vehicles, is presented

in this thesis. The proposed trajectory planning approach is based on the Model Pre-

dictive Control (MPC) method. The implemented system is suitable for deployment of

groups of flying robots in environment with static as well as dynamic obstacles. Be-

side the obstacle avoidance functionality, a failure tolerance mechanism is included into

the formation coordination. Simulation results are provided to show the correctness of

the proposed methods.

Abstrakt

Tato prace se zabyva rozsırenım leader–follower systemu rızenı formace pozemnıch

robotu tak, aby byl pouzitelny pro rızenı formace bezpilotnıch kvadrokopter. Pro

planovanı trajektoriı jednotlivych clenu formace je pouzita metoda prediktivnıho rızenı

(MPC). Navrzeny system je pouzitelny pro rızenı formacı letajıcıch robotu v prostredı

se statickymi i dynamickymi prekazkami. Do systemu je implementovana metoda

pro zabranenı kolizı mezi cleny formace. Prezentovane vysledky numerickych simulacı

potvrzujı spravnost navrzenych metod.

xi

xii

Contents

List of figures xv

List of tables xvii

1 Introduction 1

2 State of the art 3

3 Preliminaries 4

3.1 Formation coordination . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3.2 Path planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3.2.1 Geometric methods . . . . . . . . . . . . . . . . . . . . . . . . . . 5

3.2.2 Rapidly exploring Random Trees . . . . . . . . . . . . . . . . . . 7

3.2.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.3 Model Predictive Control . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

3.4 Sequential quadratic programming . . . . . . . . . . . . . . . . . . . . . 8

4 Quadrotor modelling and control 10

4.1 Mathematical model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

4.2 Quadrotor control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

4.2.1 Acceleration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

4.2.2 Constant control input movement . . . . . . . . . . . . . . . . . . 12

4.3 Experimental verification . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

4.3.1 Quadrotor control in simulation . . . . . . . . . . . . . . . . . . . 13

4.3.2 Sampling time influence . . . . . . . . . . . . . . . . . . . . . . . 19

4.4 Trajectory parametrisation . . . . . . . . . . . . . . . . . . . . . . . . . . 21

5 Leader’s trajectory planning 22

5.1 Initialization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

5.1.1 Path planing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

5.1.2 Initial trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

5.2 Trajectory optimisation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

5.2.1 Objective function . . . . . . . . . . . . . . . . . . . . . . . . . . 24

5.2.2 Constraint function . . . . . . . . . . . . . . . . . . . . . . . . . . 26

6 Trajectory tracking for followers 28

6.1 Formation representation . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

6.2 Initialization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

xiii

6.3 Trajectory optimisation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

6.3.1 Objective function . . . . . . . . . . . . . . . . . . . . . . . . . . 30

6.3.2 Constraint function . . . . . . . . . . . . . . . . . . . . . . . . . . 32

7 Experimental verification 33

7.1 Formation keeping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

7.2 Obstacle avoidance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

7.3 Followers collision avoidance . . . . . . . . . . . . . . . . . . . . . . . . . 36

7.4 Real–world application simulation . . . . . . . . . . . . . . . . . . . . . . 36

7.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

8 Conclusion 43

9 Bibliography 44

A Content of enclosed CD I

xiv

List of Figures

1.1 Quad–rotor UAV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2 Model Predictive Control scheme . . . . . . . . . . . . . . . . . . . . . . 2

3.1 Trajectory representation . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

4.1 Accelerated motion analysis . . . . . . . . . . . . . . . . . . . . . . . . . 12

4.2 Circular motion analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

4.3 Simulation of accelerated motion . . . . . . . . . . . . . . . . . . . . . . 16

4.4 Simulation of circular motion . . . . . . . . . . . . . . . . . . . . . . . . 19

4.5 Comparison of analytical and simulated trajectory . . . . . . . . . . . . . 19

4.6 Dependence of time consumption on the ∆t . . . . . . . . . . . . . . . . 20

4.7 Vector representation of the trajectory . . . . . . . . . . . . . . . . . . . 21

5.1 Formation representation . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

5.2 Leader’s initialization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

6.1 Illustration of formation representation parameters p, q . . . . . . . . . . 28

6.2 Illustration of formation representation parameters q, r . . . . . . . . . . 29

7.1 Formation keeping verification . . . . . . . . . . . . . . . . . . . . . . . . 34

7.2 Formation shape . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

7.3 Control inputs x–component . . . . . . . . . . . . . . . . . . . . . . . . . 35

7.4 Control inputs y–component . . . . . . . . . . . . . . . . . . . . . . . . . 36

7.5 Control inputs z–component . . . . . . . . . . . . . . . . . . . . . . . . . 37

7.6 Obstacle avoidance 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38




7.10 Failure tolerance mechanism . . . . . . . . . . . . . . . . . . . . . . . . . 40

7.11 More complex environment simulation 1 . . . . . . . . . . . . . . . . . . 41

7.12 More complex environment simulation 2 . . . . . . . . . . . . . . . . . . 42

xv

xvi

List of Tables

4.1 Selected results of ∆t influence experiment. . . . . . . . . . . . . . . . . . 20

A.1 Content of the enclosed CD . . . . . . . . . . . . . . . . . . . . . . . . . I

xvii

xviii

CHAPTER 1. INTRODUCTION 1

1 Introduction

The group cooperation of autonomous robots is an intensively studied topic nowadays.

Due to the technical advance, there are various components, such as motors, control units,

batteries, etc., produced in large series and thus cheaply. Another important fact is that

all mentioned components are getting smaller, lighter and more powerful. Therefore, it

is possible to build affordable, small and light robots with sufficient computing power

and operational time.

This thesis is focused on the trajectory planning for formations of quad–rotor un-

manned aerial vehicles (UAVs), further referred to as quadrotors. The quadrotor is

a favourite type of UAV because of its properties [5]. It is capable of vertical take-off and

landing (VTOL). In comparison to a classic helicopter, the construction of a quadrotor

is simpler. There are four rotors with counter–rotating propellers situated on vertices of

an x–shaped frame.

Source: Wikimedia Commons

Figure 1.1: Quad–rotor UAV Parrot AR.Drone 2.0.

The goal of this thesis is to extend the 2D leader–follower formation driving method

presented in [15] to a three–dimensional space, applicable for driving formations of

quadrotor UAVs. For modelling of the quadrotor movement in numerical simulations,

the mathematical model of quadrotor’s dynamics is used [10, 12]. The main advantage of

the dynamic mathematical model over the kinematic–only mathematical model is higher

movement accuracy.

The trajectory planning is performed using the Model Predictive Control (MPC)

method. For purposes of the MPC planning, the trajectory is coded into a vector of

elements containing constant quadrotor control inputs. The illustration of function of

the method is shown in figure 1.2. Firstly, the initial trajectory has to be found. Then,

the control loop can be started. During each iteration, from robot’s current position

an optimal control problem is solved. Then, the first part of the resulting trajectory is

used as a control input, moving the robot to a new position. The loop continues until

2 CHAPTER 1. INTRODUCTION

the goal is reached.Sheet1

Page 1

Leader of the formation

Initialization

trajectory planning

MPC

SQP

Robot control

1 … N N+1 … N+M

Follower 1 Follower n

Initialization Initialization

trajectory planning trajectory planning

MPC MPC

SQP…

SQP

1 … N 1 … N

Robot control Robot control

Figure 1.2: Illustration of Model Predictive Control approach.

This thesis is organised as follows. In chapter 3, the method preliminaries is presented.

In chapter 4, the quadrotor’s motion model is presented. Also the proposed approach to

constant control input parametrisation of the quadrotor’s trajectory is presented there.

In the following chapter 5, the proposed approach to the leader’s trajectory planning is

presented. In chapter 6, the proposed approach to the follower’s trajectory planning is

presented. In the last chapter 7, the experimental verification of the proposed approaches

is presented.

CHAPTER 2. STATE OF THE ART 3

2 State of the art

The formation coordination of multiple flying UAVs offers a wide range of potential

applications. For example the approach presented in [3] is suited for patrolling and

search applications. Beside the military missions, there is a lot of civil applications of

the mentioned method. For example patrolling of a given area with suspected fire hazard,

search and rescue missions and others.

In the literature, there are numerous approaches dealing with coordination of multiple

robots in a formation. For example the behavioural techniques are based on an observed

behaviour of flocks or herds of animals [2]. Another approach uses virtual structures to

describe the formation shape [13]. The motion of the formation is planned as a motion

of the rigid body representing the formation. After the trajectory of the rigid body is

known, trajectory of each formation member can be determined. The robot’s trajectory

is defined as a set of points in the rigid body that represents the robot’s position. The last

approach is the already mentioned leader–follower method, where positions of followers

in a formation are defined relatively to the leader’s position [1, 14, 15, 16].

In [16], stability properties of a leader–follower formation driving approach are dis-

cussed. A leader–follower approach using MPC method for control of formation of heli-

copters is presented in [14]. The formation control for UAVs with communication delays

is presented in paper [1].

This thesis builds on methods presented in the PhD thesis [15], where a forma-

tion driving method a formation of Unmanned Ground Vehicles (UGVs) is presented.

The goal of this thesis is to extend this method for a formation of quad–rotor UAVs.

4 CHAPTER 3. PRELIMINARIES

3 Preliminaries

An introduction of methods used in this thesis is given in this chapter. In section 3.1,

a leader–follower formation keeping approach is introduced. In section 3.2, selected

methods of initial path planning are introduced. In section 3.3, the Model Predictive

Control method is described. In section 3.4, the Sequential Quadratic Programming

optimisation technique is described.

3.1 Formation coordination

As it was mentioned in the preceding chapter, that in literature, there are numerous

approaches dealing with the coordination of multiple robots in a formation. For example

behavioural techniques, based on behaviour of flocks or herds of animals [2], or virtual

structure approaches, where the formation is represented as a single rigid body [13]. This

thesis is built on the PhD thesis [15], where a leader–follower formation driving method

connected as a third driving approach is used. The goal of this thesis is to extend

the presented 2D approaches to a three–dimensional space, applicable for quadrotor

UAVs.

When using the leader–follower control method, one robot or several robots are con-

sidered as the leader. The position of followers in a formation is defined relatively

to the leader’s position. The main advantage of this approach is that the leader–to–

formation stability can be relatively simply solved [16]. This approach is suitable for

heterogeneous multi–robot systems, where the best equipped robot can be selected as

the leader. Followers can be less equipped and thus cheaper robots, because they are

only tracking the leader’s trajectory and avoiding collisions.

The implementation details of formation representation are provided in chapter 6.2.

3.2 Path planning

The path planning is used to obtain the initial seed for the MPC method, as it is shown

in figure 1.2. Implementation details of the initialization process are provided in chapter

5.1.

In literature, there exist numerous approaches of the path planning. Every approach

is convenient for another type of path planning problems. For example, there are numer-

ous geometric methods such as visibility graph or Voronoi diagrams [6]. Other approaches

use a grid space decomposition [6]. Then there are stochastic approaches, such as proba-

bilistic roadmaps [6, 8] or Rapidly exploring Random Trees (RRT) [7, 8]. Another widely

used approach is the potential field method [6].

Before the selected planning algorithms can be introduced, it is important to define

CHAPTER 3. PRELIMINARIES 5

the fundamental terms. Let us have a world W , which contains a set of all obstacles

O and a set of all quadrotor robots A. The configuration space C, often referred to as

a C–space, is a set of all possible robot configurations in the world W . C–space has two

subsets, the first subset is the obstacle configuration space Cobst ⊆ C. It is a set of all

states in which the robot is in a collision with any obstacle o ∈ O. The second subset is

the free configuration space Cfree = C \Cobst. It is a set of all non–collision configurations

of the robot. In this thesis, the configuration space C is defined as the special euclidean

group SE(3) = R3×SO(3). The SO(3) is the special orthogonal group defined by formula

SO(3) =R ∈ R3×3|RTR = E, det(R) = 1

, (3.1)

where E is the identity matrix.

The state q of a quadrotor robot is therefore represented by the three world coordi-

nates x = (x, y, z) and the Euler angles. In this thesis, the representation by the rotation

matrix R ∈ SO(3) is used rather than the Euler angles. It means that q = (x, R).

Now, it is possible to define a path P as a set of n consecutive states P = qi, i = 1 . . . n,

such that qi ∈ Cfree. Notice, that it is not defined how the quadrotor should move among

those states. The path is a sequence of states in Cfree that the quadrotor has to pass

through to get from the starting state qstart to the goal state qgoal. The path can also be

a continuous set of states, i.e. a curve.

Trajectory T is a path that the quadrotor robot follows through the configuration

space C as a function of time. It means that velocity and acceleration of the robot can

be obtained from the trajectory. Therefore it is possible to say that the trajectory also

defines how the quadrotor should move between the states. In the trajectory planning and

robot control, it is important to consider the feasibility of the trajectory. The feasible

trajectory satisfies limitations of robot’s movement abilities, i.e. the motion model.

The mathematical description of motion model of a quadrotor is introduced in chapter

4.1.

3.2.1 Geometric methods

In this section, the selected geometric methods suitable for leader’s initialization are

presented. Both of them use a graph representation of the configuration space. Therefore,

in order to find the shortest path, a graph search algorithm, such as the Dijkstra’s

algorithm, has to be used.

Voronoi diagram is a mathematical method used for space division into a given num-

ber of regions [6]. The space is divided by a discrete set of objects contained in the space.

The simplest case is division of a plain using a set of points. Let us have a space X ⊆ R2


and a discrete set of points K ⊆ R2. Now, it is possible to divide X by the set K, using

a distance function d. Let us define the euclidean distance as the distance function d.

The Voronoi region Ri is then defined as a set of points, that are closer to ki ∈ K, than

to any other kj ∈ K, i 6= j:

Ri = x ∈ X | d(x, ki) ≤ d(x, kj) for all i 6= j . (3.2)

In the path planning, the space X is the configuration space C and the discrete set of

objects K is the set of all obstacles O. The resulting graph used for the path planing is

constructed from edges dividing the Voronoi regions. Therefore the resulting path is in

the maximal possible distance from all obstacles.

This method is suitable for leader’s initialization, because the initial trajectory is

in maximal possible distance from obstacles. However, the definition suggests that this

method is easily implementable in the version with discrete point set K. The method

becomes complicated, when the set K is replaced by the set of generally defined obstacles

O and the plain is replaced by the three dimensional configuration space C.

Visibility graph is a C–space representation method, that is easily applicable when

obstacles are represented by polyhedrons [6]. The polyhedrons are enlarged by the size

of the robot, so the resulting path can be used with no other adjustments. The graph

is constructed by connecting each pair of mutually visible vertices of the polyhedrons by

an edge. When the graph is created, the start point and the goal point are connected to

all visible vertices in the existing graph.

The visibility graph path planning produces paths that are very close to obstacles. On

the other hand the resulting initial trajectory is usually shorter than the one produced

by the voronoi diagram.

Dijkstra’s algorithm is a graph search algorithm, originally presented in [4]. It can

be used for solving the shortest path problem in a graph with non–negative edge costs.

In case of this thesis, the graph is given by the Voronoi region method or by the visibility

graph method. The function of the Dijkstra’s algorithm is described by the following

steps:

1. Initialize the starting node cost to zero and all other node’s costs to infinity.

2. Select a node u with minimal cost in the unvisited list U . If the cost of u is infinity,

end the search. Otherwise, remove u from U and put it to the visited list W .

3. For all successors v of the node u, calculate the distance as d(v) = d(u)+c(u,v), where

d is the cost of the node and c is the cost of the edge connecting the two nodes. If


the calculated cost d(v) is smaller than the current cost of the node d(v′), the cost

is changed to d(v) and the parent of the node v is set to u.

4. If the set U is not empty, go to step two.

5. If the cost of specified goal point is not infinity, the path can be reconstructed from

the parental references.

3.2.2 Rapidly exploring Random Trees

The RRT is a method that can be used to find an initial trajectory considering the motion

model of the robot [7, 8]. Therefore, it is suitable for the leader’s initialization. The RRT

algorithm is based on randomized sampling of the free configuration space Cfree with

consideration of the robot’s motion model. Because of randomized approach of the C–

space exploration, the algorithm cannot guarantee quality of the resulting trajectory.

Usually, the resulting trajectories are very curvy and unnecessarily long. The function

of the RRT algorithm can be described as follows:

1. Add start state qstart to the graph G.

2. Generate random point qrand ∈ C.

3. Find nearest state qnear in the existing tree.

4. Determine new state qnew ∈ Cfree, that is in direction of qrand and add it to G.

The part of trajectory connecting states qnew and qnear has to satisfy the robot’s

motion model.

5. If a defined stopping criterion is not satisfied, go to step two.

6. Return G.

3.2.3 Summary

From approaches suitable for the leader’s initialization, mentioned in the beginning of this

section, three candidate methods were selected and described. The first selected method

is a geometric method using Voronoi diagrams C–space representation. It produces paths,

that are in the maximal possible distance from all obstacles. Therefore, it is convenient

for robust initialization, ensuring collision–free properties in environment with moving

obstacles.

The second selected method is also a geometric method, using visibility graph C–

space representation. Paths found using this method are close to obstacles. So the initial


path might be planned close to the optimal trajectory of the formation. Therefore, this

method is convenient to speed up the optimisation process.

The last selected method is the RRT algorithm. The main advantage of this method

is that it produces feasible trajectories. However, due to randomized approach of C–

space sampling, the algorithm cannot guarantee qualitative properties of the resulting

trajectory.

3.3 Model Predictive Control

Model predictive control method, also referred to as the Receding Horizon Control

(RHC), is a widely used control technique. Its original field of use is industry, where

it has been used since 1970s [11].

Originally, the control problem is coded into an optimisation vector with N control

elements. Where N denotes length of the horizon. Each element has constant duration

∆tN . More details of the MPC method and its historical development can be found in

[11]. For purposes of the trajectory planning of robots formations, an extended optimisa-

tion vector has been introduced in [15]. It is extended by additional M vector elements,

with variable duration ∆tM . The purpose of this extension is to code the whole trajec-

tory into the optimisation vector, not just a part of a control horizon given by N∆tN .

This approach allows the leader to react to immediate change of the local environment.

And due to M additional elements, it is as well capable of global trajectory optimisation.

An example of trajectory representation is shown in figure 3.1.

The loop of MPC can be described as follows: in each step, from robot’s current

position an optimal control problem is solved. Then the first part of found control inputs

from the result is applied, moving the robot to a new position. The loop continues until

the goal is reached.

3.4 Sequential quadratic programming

Sequential quadratic programming is a powerful method for nonlinear constrained opti-

misation. It is a generalisation of the Newton’s optimisation method. The disadvantage

of the SQP method lies in inability to overcome local extrema.

The SQP is suitable method for solving the MPC problem of trajectory planning for

nonholonomic robots. In trajectory planning, the effort is to reduce time consumption

as much as possible. The reason is a fast recalculation of the best trajectory, allowing

the robot to react to newly discovered obstacles. Therefore, an efficient implementation of

SQP method has to be used. In this thesis, the C code for Feasible Sequential Quadratic

Programming (CFSQP) library by Craig T. Lawrence, Jian L. Zhou, and Andre L. Tits


Figure 3.1: Trajectory representation.

[9] is used. More information about CFSQP and SQP can be found in the CFSQP

manual [9] and references therein respectively.

10 CHAPTER 4. QUADROTOR MODELLING AND CONTROL

4 Quadrotor modelling and control

4.1 Mathematical model

In chapter 3.2, it was mentioned that it is essential to have a mathematical model of

the vehicle’s movement in order to plan a feasible trajectory. The mathematical model

of a quad–rotor UAV dynamics is presented for example in [10] and in [12]. In this thesis,

the matematical model originally presented in [10] is used:

x = v (4.1)

mv = mge3 − fRe3 (4.2)

R = RΩ (4.3)

JΩ + Ω× JΩ = M (4.4)

In the equations:

m ∈ R is the total mass of the quadrotor,

g = 9.81 m·s−2 is the gravitational acceleration,

J ∈ R3×3 is the inertia matrix with respect to the body fixed frame,

R ∈ SO(3) is the rotation matrix from the body fixed frame to the inertial frame,

Ω ∈ R3 is the angular velocity with respect to the body fixed frame,

x ∈ R3 is the position of the center of the mass of the quadrotor in the inertial

frame,

v ∈ R3 is the velocity of the center of the mass of the quadrotor in the inertial

frame,

f ∈ R is the total thrust of the quadrotor’s propellers,

M ∈ R3 is the total moment in the body fixed frame.

Vectors e1, e2, e3 ∈ R3 are columns of the identity matrix E, i.e. e1 = [1 0 0]T , e2 =

[0 1 0]T and e3 = [0 0 1]T . Matrix Ω ∈ SO(3) is a skew–symmetric matrix such that

xy = x× y, x, y ∈ R3 [10, 12].

In the simulation framework the following constants are used: the weight of the quadro-

tor m = 4.34 kg and the inertia matrix

J =

0.0820 0 0

0 0.0845 0

0 0 0.1377

kg·m2. (4.5)

For more detailed information please see the original paper [10]. There, you can also find

the calculation of the individual forces fi, i = 1 . . . 4, for each of quadrotor’s propellers,

which are necessary to control the real quad–rotor UAV.

CHAPTER 4. QUADROTOR MODELLING AND CONTROL 11

4.2 Quadrotor control

The trajectory of the quadrotor is planned using a method based on model predictive

control. The MPC method is introduced in chapter 3.3, where it was discussed that

the trajectory has to be coded into a vector. The vector consists of N constant time

elements and M variable time elements. The control input remains constant over each

of those N +M elements.

It means that in order to plan the trajectory using such a method it is essential to be

able to control the quadrotor’s movement with constant control inputs. To achieve that,

it is possible to let the quadrotor move in circles. The final trajectory is then composed

of acceleration elements and the circular constant control input elements.

In the following text, the devised approach of quadrotor control with constant control

inputs is presented. Also, results of simulations designed to verify correctness of described

approaches are presented there.

4.2.1 Acceleration

For controlling a vehicle with constant velocity, it is important to be able to accelerate

it to the specified velocity. In this thesis, the movement of a quadrotor is described by

a velocity vν , normal to the trajectory, and a velocity vτ , tangential to the trajectory.

The normal velocity is equal to the z component of the three dimensional velocity vector

v taken with respect to the inertial frame, therefore

vν = vTe3. (4.6)

The tangential velocity vτ is given by a composition of the x and y components of

the velocity vector v

vτ = vTe1 cos (γ) + vTe2 sin (γ) , (4.7)

where γ is the yaw Euler angle of the quadrotor.

The quadrotor can be accelerated by rotating around the body fixed y–axis by angle

β. The total thrust force f is decomposed by the pitch angle β into two forces, fx = maτ

and fz = maz, as shown in figure 4.1. The normal acceleration is defined as aν = az − g,

where g is the gravitational acceleration. Therefore, the size of the pitch angle β and

magnitude of the total thrust f depend on magnitudes of both, normal and tangential

accelerations. After the required velocity is achieved, the quadrotor can be rotated back

around the body fixed y–axis by angle −β.


Figure 4.1: Decomposition of the total thrust f by quadrotor’s rotation around y–axisby angle β.

4.2.2 Constant control input movement

As it was said in the beginning of this section, after accelerating, the quadrotor should

move with constant control inputs given by the optimisation. It means that it continues

by the straight line motion or by the circular motion.

The circular motion can be achieved by rotating the quadrotor around the body fixed

x–axis by angle α. The following conditions have to be satisfied: 1) The centripetal force

fd, given by

fd =mv2τr, (4.8)

is equal to force fy generated by the decomposition of total thrust f by the roll angle α.

The decomposition is done into the transient coordinate system (red coloured in figure

4.2b) between the inertial coordinate system and the body fixed coordinate system.

2) The angular velocity ω around an axis o, which is parallel to the inertial system’s

z–axis (see figure 4.2a), satisfies the condition

ω =vτr, (4.9)

where r is radius of the followed circular trajectory. Thus ω is changing the yaw angle γ

(see figure 4.2b) in order to keep body fixed x–axis in tangential direction of the motion.

3) In the proposed method, the normal velocity does not need to be changed, while

the quadrotor is turning. So the third condition that has to be satisfied is fg = fz. If we

would like to change the normal velocity, normal acceleration aν has to be applied. It


is defined in the similar way to the approach presented at the end of chapter 4.2.1, i.e.

aν = az − g.

(a) Decomposition of the to-tal thrust f by quadrotor’srotation around x–axis byangle α.

(b) Comparison of the iner-tial coordinate system (green)and the quadrotor’s body fixed–frame (blue) orientation.

Figure 4.2: Circular motion analysis.

4.3 Experimental verification

The experimental verification of previously discussed approaches of quadrotor control

is presented in this section. The secondary purpose of this section is to further clarify

discussed methods and also to provide more details about their implementation.

For purposes of experimental verification, the mathematical model given by equations

(4.1) – (4.4) was implemented into an application. Results described further in this

section were obtained using this implementation. The same code is used for simulation

of the movement of quadrotors in the final application. Values of the control, such as

tangential velocity, normal velocity or angular velocity, are provided by the MPC method

in the final application.

4.3.1 Quadrotor control in simulation

It is not possible to control the Euler angles of the quadrotor directly. It is necessary to

control them by setting angular velocity ωr ∈ R around the appropriate axis or ∈ R3 for

the given amount of time.

The following simulation verifies devised approached, presented in chapters 4.2.1 and

4.2.2. It is divided into two parts. The quadrotor starts in position x = [0 0 0]T m


with initial rotation matrix of R = E. The initial angular velocity vector is Ω =

[0 0 0]T rad · s−1. And finally the initial velocity vector is v = [0 0 0]T m · s−1.

Part one – the acceleration verifies results presented in section 4.2.1. The goal is

to accelerate the quadrotor without changing its z–coordinate in the inertial frame, so it

is able to perform the turn in the next part of the simulation.

In the experiment, the quadrotor has to move with the constant velocity vτ =

0.6 m·s−1 after the first meter of its trajectory (sa = 1 m). Firstly, it is important

to calculate the needed acceleration. Time t can be expressed from equation

∆v = at (4.10)

and substituted into

s =

∫ t

0

(at+ v0) dt+ s0 =1

2at2 + v0t+ s0. (4.11)

Using s0 = 0 m, v0 = 0 m·s−1, previously edited equation (4.11) can be transformed into

a =v2

2s. (4.12)

Now, it is possible to calculate the pitch angle β and the total thrust f . Force fx from

figure 4.1 can be calculated as fx = maτ . The elevation of the quadrotor has to stay

constant, therefore aν = 0 m·s−2. The acceleration az = aν + g was defined in section

4.2.1 and thus fz = maz = mg. Finally, it is possible to calculate the pitch angle by

using the equation

β = arctan

(fxfz

)= arctan

(azg

). (4.13)

The total thrust can be calculated from the equation

f =fz

cos (β). (4.14)

The total thrust f remains constant during this part of the simulation.

Once the total thrust is known, the moment input needs to be calculated. The mo-

ment turns the quadrotor around body fixed y–axis by the previously calculated angular

velocity ωy. So it takes 0.1 s to rotate the quadrotor to the required pitch angle.

The moment vector M can be calculated from equation (4.4). Because the initial an-

gular velocity is zero (as we defined in the beginning of this section), it is possible to sub-

stitute Ω× JΩ = 0, which reduces the equation into M = JΩ. If Ω = [0 ωy 0]T rad·s−1

and matrix J (see (4.5)) are substituted, the required moment impulse M is obtained.


Opening tilt. According to section 4.2.1, in order to start the movement, the quadro-

tor has to be turned around the body fixed y–axis by angle β. And the total thrust force

f has to be set. The rotation is achieved by applying the calculated moment impulse M

which produces the angular velocity Ω. Constant angular velocity is kept for 0.1 s during

which the quadrotor rotates into the specified pitch angle β. After 0.1 s the opposite

moment impulse −M is applied. It stops the quadrotor’s rotation, leaving it rotated by

angle β around body fixed y–axis.

Closing tilt. After the quadrotor is accelerated to the specified velocity, it has to be

turned back so that the pitch angle is zero and the quadrotor is no longer accelerated. It

means that it is necessary to rotate the quadrotor around the body fixed y–axis by angle

β = −β. The opposite moment impulse −M is now used as the first input. It produces

angular velocity of Ω = [0 − ωy 0]T rad·s−1. After 0.1 s, i.e. when the quadrotor is

rotated back to zero pitch angle, the stopping moment impulse M is applied.

When the whole mentioned process is finished, the quadrotor should have tangential

velocity vτ = 0.6 m·s−1. In this case, the velocity vector should be v = [vτ 0 0]Tm·s−1.The quadrotor’s position vector should be x = [1 0 0]Tm and the angular velocity vector

Ω = [0 0 0]T rad·s−1.

Simulation results. The final results of simulation of the quadrotor acceleration

are following: velocity of the quadrotor is v = [0.5973 0 0.0002]Tm·s−1, its final posi-

tion is x = [1.0161 0 0.0003]Tm. The final angular velocity is Ω = [0 0 0]T rad·s−1 and

resulting pitch angle β =−8 ·10−18rad. The simulation results differ only slightly from

the analytically calculated values. The difference between the theoretical and the simu-

lation results is caused by omitting the transient during the rotation of the quadrotor.

The theoretical calculations presume immediate change of variables. Whereas in simu-

lations, and also in the real world, the change usually takes a finite time to apply.

In figure 4.3, there is shown the graphical representation of results obtained by

the simulation. The top graph shows moment around the body fixed y–axis. There

are four peaks, just as it was described earlier. Between those peaks the angular veloc-

ity around the body fixed y–axis (ωy) remains constant. Its graphical representation is

shown in the middle. On the bottom it is shown the pitch angle β. Notice, the change

between the two moment peaks. Then it stays constant as the quadrotor accelerates.

After the required speed is reached, the angle is returned back to zero.

Part two – the circular motion is continuation of the previous part. The tangential

velocity of the quadrotor is now vτ = 0.6 m·s−1 and the normal velocity is vν = 0.0 m·s−1.During the second part of the simulation, the heading of the quadrotor is turned to


0 0.5 1 1.5 2 2.5 3 3.5−0.02

0

0.02

t [s]

my [N

.m]

0 0.5 1 1.5 2 2.5 3 3.5−0.2

0

0.2

t [s]

ωy [ra

d.s

−1]

0 0.5 1 1.5 2 2.5 3 3.5−0.01

0

0.01

0.02

t [s]

β [ra

d]

Figure 4.3: Result of the simulation of the quadrotor acceleration. The top graph showsthe moment inputs controlling the quadrotor. The middle graph shows the angular veloc-ity of the quadrotor around the body fixed y–axis. The bottom graph shows the quadro-tor’s pitch angle β.

the right. It means that the yaw angle changes to γ = −π2

rad, as the quadorotor follows

a circular trajectory with radius r = 2 m. The movement is theoretically described in

section 4.2.2.

Firstly, it is important to calculate the roll angle α by using equation

α = arctan

(fyfz

)= arctan

(v2τrg

), (4.15)

where, fz = fg = mg and fy = fd. The force fd is defined by equation (4.8).

The example was intentionally set up, so the pitch angle from the previous part goes

along with the roll angle in this part. Thus the moment value rotating the quadrotor to

desired angle is the same as the one calculated in the previous part. The moment input

vector is M = [ωx 0 0]TN·m.

Once we have the roll angle, the total thrust force can be calculated using formula

f =fz

cos (α). (4.16)

Now, the initial turning moment input Minit and the keeping moment input Mkeep

need to be calculated. Those two moments cause that the quadrotor is turning around


an axis o. The axis o goes through the quadrotor’s center of mass and it is parallel with

the inertial frame z–axis. It can be considered that the axis o is the quadrotor’s body

fixed frame z–axis rotated back to the inertial system. Therefore, the rotation axis o can

be calculated as

o = R−1e3, (4.17)

where R is the quadrotor’s rotation matrix and vector e3 represents the z–axis of

the quadrotor’s body fixed frame. In this case R is a rotation matrix around x–axis

by angle α.

Now, it is possible to calculate the initial moments Minit. Analogically to the previous

part, the equation (4.4) is used. The quadrotor is moving on the straight line, therefore

the angular velocities around all three axis are zero. It means that Ω× JΩ = 0, which

reduces the equation into M = JΩ. The substituted Ω is obtained as Ω = ωo, where ω

is the angular velocity given by equation (4.9).

The purpose of the keeping moment Mkeep is to keep the quadrotor’s angular velocity

around axis o constant while the quadrotor is turning. Therefore Ω = 0 and equation

(4.4) is reduced into Ω× JΩ = M. The angular velocity that has to be kept is Ω = ωo,

i.e. the same angular velocity as in the previous step.

Opening tilt. As it is described in section 4.2.2, in order to start the circular move-

ment, the quadrotor has to be turned around body fixed x–axis by angle α. The newly

calculated total thrust force f has to be set. The rotation is then achieved by applying

the calculated moment impulse M, which produces the angular velocity Ω. Constant

angular velocity is kept for 0.1 s during which the quadrotor rotates into the given pitch

angle α. After 0.1 s the opposite moment impulse −M is applied. It stops the quadrotor’s

rotation, leaving it rotated by angle β around y body fixed axis.

In the case, that the quadrotor is rotated around the body fixed y–axis from the pre-

vious part, the closing tilt from the previous part can be applied along with opening tilt

from this part. It means that the quadrotor can be rotated to the zero pitch angle in

the same time it is rotating to the needed roll angle. This approach reduces the transient

time.

Turning. After the quadrotor is rotated by α around the body fixed x–axis, it starts

the circular movement. In order to follow the circular trajectory, the quadrotor has to

rotate with the angular velocity ω around the axis o. So the initial moment impulse

Minit has to be applied. In order to keep the angular velocity constant, the moment

input Mkeep has to be applied. The constant moment input Mkeep is kept for the whole

time of the quadrotor turning.


Closing tilt. Once the quadrotor’s yaw angle achieve the required value, the turn-

ing is stopped. It is done by applying opposite moment input impulse −Minit. After

doing so, the quadrotor’s angular velocity vector is equal to zero.

However, the quadrotor is still inclined by the roll angle α. It is necessary to maintain

the straight motion, so the quadrotor has to be rotated by the opposite angle −α around

the body fixed x–axis. It is achieved by applying the moment impulse −M. And after

the quadrotor rotates to zero roll angle, the stopping moment impulse M is applied.

The resulting yaw angle should be γ = −π2

rad and the final tangential velocity should

stay at vτ = 0.6 m·s−1. The quadrotor’s position vector should be x = [3 − 2 0]Tm and

the angular velocity vector Ω = [0 0 0]T rad·s−1.

Simulation results. The final results of the quadrotor circular movement simula-

tion are following: velocity of the quadrotor is v = [−0.0139 −0.6131 0.0002]Tm·s−1,its final position is x = [2.9971 −2.1222 0.0009]Tm. The final angular velocity is

Ω = [0 0 0]T rad·s−1 and the resulting yaw angle is γ = −1.5708 rad. Again, the sim-

ulation results differ only slightly from the analytically calculated values. As in the pre-

vious part, the difference between the theoretical and the simulation results is caused

by omitting the transient during the rotation of the quadrotor. While the theoretical

calculations presume immediate change of variables, in simulations, the change usually

takes a finite time to apply.

Figure 4.4 shows the graphical representation of results obtained in the simulation.

The first graph shows moment around body fixed x–axis, mx. One can see four mo-

ment impulses, which cause the rotation of the quadrotor around the body fixed x–axis.

The angular velocity ωx is shown in the second graph. The roll angle α is shown in

the third graph. Finally, the fourth graph shows the yaw angle γ, which represents

the heading of the quadorotor.

Simulation results conclusion. The figure 4.11 shows the comparison of the ana-

lytically calculated trajectory and the results of the presented simulation. As it was

mentioned before, the difference between the theoretical and the simulation results is

caused by omitting the transient during the rotation of the quadrotor. The theoretical

calculations presume immediate change of variables. Whereas in simulations, and also

in the real world, the change usually takes a finite time to apply. Based on presented

results, it is possible to say that the theoretically described approaches were successfully

confirmed.


0 1 2 3 4 5 6−0.02

0

0.02

t [s]

mx [n.m

]

0 1 2 3 4 5 6−0.2

0

0.2

t [s]

ωx [ra

d.s

−1]

0 1 2 3 4 5 6−0.02

0

0.02

t [s]

α [ra

d]

0 1 2 3 4 5 6−2

−1

0

t [s]

γ [ra

d]

Figure 4.4: Result of the simulation of the quadrotor’s circular motion. The first graphshows moment inputs controlling the quadrotor. The second graph shows the angu-lar velocity of the quadrotor around the body fixed x–axis. The third graph showsthe quadrotor’s roll angle α. The fourth graph shows the quadrotor’s yaw angle γ.

0 0.5 1 1.5 2 2.5 3

−2

−1.5

−1

−0.5

0

x [m]

y [m

]

Ideal trajectory

Simulation result

Figure 4.5: Comparison of the analytically calculated theoretical trajectory and the re-sulting trajectory obtained in simulation quadrotor’s.

4.3.2 Sampling time influence

As stated in [10], the set of nonlinear differential equations (4.1) – (4.4), that describes

the dynamics of the quadrotor, cannot be solved analytically. Therefore, this problem


∆t [s] 5 · 10−6 10−5 5 · 10−5 10−4 5 · 10−4 10−3 5 · 10−3

t [ms] 3150 1564 308 146 28 14 3∆l [m] 0.0890 0.0891 0.0898 0.0907 0.1002 0.1169 0.3113

Table 4.1: Selected results of ∆t influence experiment.

has to be solved numerically in the simulation. It means that each of the equations is

integrated and it is multiplied by a small time constant ∆t. Over the time interval ∆t,

the function is considered as linear. The time interval ∆t must be as small as possible,

so the inaccuracy caused by the numerical integration is minimised.

The numerical approach means that the calculation of a new state has to be done

every ∆t. Therefore, too small value significantly slows down the simulation, which is in

conflict with the simulation’s accuracy. It is necessary to choose such ∆t that provides

a good compromise between the accuracy and the time consumption.

The table 4.1 shows results of an experiment with different ∆t. It was designed to

illustrate the sampling time influence on the simulation. The simulation of quadrotor

dynamics is iteratively launched with changing ∆t. The simulation is run for ten times

for each ∆t and the average time t is stored. The position difference ∆l represents

the distance between the theoretical final position and the resulting final position of

the quadrotor in the simulation. The value of ∆l cannot be zero, because it is not

affected only by the sampling time interval.

The simulation from previous section was adjusted for purposes of this measurement,

so the quadrotor is accelerated and then it performs a turn to the right.

The dependence of time of simulation consumption on the time interval ∆t is shown

in figure 4.6. As it was expected, the time consumption significantly increases with

decreasing time interval ∆t.

0 1 2 3 4 5

x 10−3

0

500

1000

1500

2000

2500

3000

3500

∆ t [s]

t [m

s]

Figure 4.6: Dependence of the time consumption on the length of time interval ∆t.


As presented in table 4.1, the most suitable time of integration interval is ∆t =

5·10−4s. The time consumption is small in comparison to the most accurate measurement

and the accuracy ∆l differs only by 1.12 cm.

4.4 Trajectory parametrisation

In the beginning of this chapter, it was mentioned that the MPC method requires the tra-

jectory to be coded into a vector of constant control inputs. That is why the approach

presented in the preceding sections had to be devised. In this section, the proposed

vector representation of the trajectory is described.

The proposed approach is to represent the trajectory of a quadrotor by a vector of

constant control inputs, as shown in figure 4.7. In the vector:

vτ [m·s−1] is the tangential velocity, given by equation (4.7),

vν [m·s−1] is the normal velocity, given by equation (4.6),

ω [rad·s−1] is the angular velocity around an axis o, given by equation (4.9),

t [s] is the time interval.

vτ 1 vν 1 ω1 t1 ... vτ N vν N ωN tN ... vτ M vν M ωM tM

Figure 4.7: The quadrotor’s trajectory representation by a vector of consecutive constantcontrol input intervals. Each interval consists of the tangential velocity vτ i, normalvelocity vν i and angular velocity ωi, which are constant over given time ti.

The problem that arises with utilisation of this representation is the difference of

normal and tangential velocity in two consecutive time intervals. It can be solved by

adding an intermediate interval between each two consecutive constant control intervals.

During the intermediate interval, the quadrotor’s velocity is adjusted to match values

specified in the next control input.

22 CHAPTER 5. LEADER’S TRAJECTORY PLANNING

5 Leader’s trajectory planning

In this chapter, the proposed approach to the leader’s trajectory planning is presented. In

section 5.1, the planning of the initial trajectory is described. In section 5.2, the detailed

description of the trajectory optimisation is presented.

5.1 Initialization

The initialization method is important, because it designates global properties of the tra-

jectory. After initialization, the solver alters the trajectory only near the local extrema.

From the set of available path planning methods (presented in chapter 3.2), the visibil-

ity graph was implemented. The resulting initial path has to be converted to a trajectory.

Thus it becomes feasible and the functionality of proposed approaches can be verified.

The initialization is run only once, before the main loop is started. Therefore the initial

trajectory can be precomputed prior to the formation deployment.

5.1.1 Path planing

In the proposed system, obstacles are represented as cubes for purposes of the path

planning. Cubes are enlarged by the size of the formation rf . The radius rf is defined as

the maximal distance of the leader and followers, in the yz–plane of the leader’s body fixed

frame. Additionally, it has to be enlarged by size of the follower robot. The illustration

of the formation representation approach is shown in figure 5.1.

Figure 5.1: Representation of a formation by radius rf = rmax+ r. Red circle representsthe leader of the formation. Followers are represented by green circles.

The visibility connects mutually visible vertices of obstacles, creating a graph. Then,

the start and the goal points are connected to visible vertices in the graph, as it is

CHAPTER 5. LEADER’S TRAJECTORY PLANNING 23

described in chapter 3.2. When the graph is created, it is searched by the Dijkstra’s

algorithm in order to find a path.

5.1.2 Initial trajectory

After the path is obtained, it has to be converted into a trajectory. For this conversion

from path to trajectory, a method using the Thales theorem was devised. The method

is described in the following text and it is illustrated in figure 5.2.

Let us have a subset of the initial path that contains two consecutive points pn and

pn+1. It is also important to know the heading, i.e. the yaw euler angle, in the point pn

(in figure 5.2 represented by a red arrow). The trajectory parametrisation is divided into

two steps. The first step is to solve the trajectory in the xy–plane, then the z–component

is added.

Figure 5.2: Illustration of path conversion approach based on Thales theorem. The curvehighlighted by green color represents a feasible path between points pn and pn+1. The redarrow represents the heading, i.e. the Euler’s angle γ, in the point pn. Values indexedwith t describe the turning circle kt. Values indexed with T describe the Thales circlekT , which is used to determine tangent to the turning circle kt.

The xy–plane can be divided into two parts by a line going through pn collinear to

the heading of the leader. If the point pn+1 lies on that line, the leader can continue its

motion on a straight line. If the point pn+1 lies in one of the half–planes, then a turn

has to be applied. If z–coordinates of pn and pn+1 differ, the normal velocity has to be

added.


The center of the turn St can be denoted as a point in the appropriate half–plane.

The point St lies on a line that is perpendicular to the leader’s heading and that goes

through the point pn. The distance between the center St and the point pn is equal to

the turning radius rt. The turning radius is a user–provided constant. Now, a tangent

line to the circle kt (St, rt) is constructed, using the Thales theorem. Firstly, the center

of the Thales circle ST is denoted. It is the center of the segment |StPn+1|. The radius of

the Thales circle rT is equal to a half of the length of the segment |StPn+1|. After that, it

is possible to construct the Thales circle kT (ST , rT ). The intersections of the turn circle

kt and the Thales circle kT can be calculated analytically.

To determine the appropriate intersection point, the angle–comparison method is sug-

gested. Firstly, angles ψ1 and ψ2 are calculated according to figure 5.2. After the turn,

the leader should have the yaw Euler angle equal to either ψ or ψ′. Therefore the men-

tioned angles are compared to ]pnStT1 and ]pnStT2 respectively. The point whose angles

are matching is the required intersection point.

Finally, the z–component has to be added. The normal velocity is added as a z–

coordinate difference divided by time of the given interval. The time required for reaching

the speed in the next interval has to be taken into account. That is why the z–coordinate

is solved separately afterwards. This process can be run in a loop through all points in

the path converting it into a trajectory.

The resulting trajectory is divided to M elements with maximal duration tmax and

coded into a vector. The vector is shown in figure 4.7. In this form, it can be optimised

by the CFSQP solver.

5.2 Trajectory optimisation

Once the initialization process is complete, the vector containing the trajectory is passed

to the CFSQP solver. The optimisation is done with respect to user–defined objective

and constraint functions. The objective function indicates how good the new iterate

of the trajectory is. It is further described in section 5.2.1. As the name suggests,

the constraint function secures respecting the constraints. It is further described in

section 5.2.2.

During the implementation process, it was found out that the MPC method provides

good results when N = 0. It reduces the trajectory vector elements count to M . The time

consumption is therefore significantly reduced.

5.2.1 Objective function

As it was suggested, the purpose of this function is to provide evaluation of the trajec-

tory by user–defined parameters. The CFSQP solver can distinguish whether a result


of the trajectory optimisation step is better than the other using this function. There-

fore, it is important to choose relevant valuation components that indicate how ”good”

the given trajectory is. The component choice also affects stability and efficiency of

the optimisation process.

The proposed objective function F is denoted as a weighted sum of all components,

described in the following paragraphs. Weights provided by the user determine the im-

portance of a particular component.

Obstacle penalty is an evaluation component that prevents the leader from collisions

with obstacles. The obstacle penalty function should be zero, if the leader is in a safe

distance r from all obstacles. And it should rapidly increase with decreasing distance r.

The obstacle penalty function, presented in [15], fulfil these conditions:

ro =

[min

(0,r − rsr − ra

)]2. (5.1)

where, rs is the safety radius – the soft border. If r becomes smaller than rs, the value

ro starts increasing. Radius ra is the critical radius, the limit of formula (5.1) for r → ra

is infinity. The condition that has to be fulfilled is ra < rs.

In the simulation, the cube representation of obstacles, used for initialization, is

approximated by a spherical representation. The diameter of the sphere is defined as

euclidean distance of the center and a vertex of the cube. This approach allows an efficient

calculation of distance from obstacles. In any point p, the distance r can be determined

as r = |pco|− ro, where ro is the radius of the obstacle and |pco| is the euclidean distance

between the point p and the center of the obstacle co.

To evaluate the trajectory, it has to be sampled first. The result of sampling process

is a set of points that covers the trajectory. For each point, the previously described

calculation is done. The maximal value is taken as the obstacle objective function Fo

value. It means that Fo = max (Ro), where Ro is the set of results of the function (5.1)

for each point of the sampled trajectory.

Distance to goal is the second chosen component. It helps to speed up and stabilize

the CFSQP optimisation process. Its value Fg is denoted as the euclidean distance of

the last point of the sampled trajectory and the goal point.

The maximal allowed distance to the goal is ensured by the constraint function. But

the rejection of the trajectory slows down the optimisation. In some cases, the CFSQP

may fail to recover at all. That is why the distance to the goal–rating parameter is added.

During the optimisation process, it is forcing the trajectory towards the goal. Therefore,

it helps the solver to overcome the difference between the predicted and the detected

position of the quadrotor, which stabilizes the process.


Angular velocity denotes the curvature of the trajectory segment, see equation (4.9).

The straighter trajectories are preferred, when this evaluation component is added to

the objective function. The value of the angular velocity parameter Fa is denoted as

a sum of angular velocities of all trajectory elements, i.e. Fa =M−1∑i=0

ωi.

Trajectory length is a component that forces the solver to prefer shorter trajecto-

ries. The value of the parameter Fl is computed as a sum of lengths of all segments of

the trajectory.

Time of flight is the last used component. Its value is the sum of durations of all

segments of the trajectory, i.e. Ft =M−1∑i=0

ti. It helps to reduce the time of flight to

the goal. Also, it is added to stabilize and speed up the optimisation process.

5.2.2 Constraint function

The purpose of this function is to add motion constraints into the optimisation process.

The user–defined constants denote the number and type of the constraint functions, see

the CFSQP manual [9]. The constraint function is called with an index that denotes

the constraint to be tested. Unlike the objective function, the constraint function must

be called repeatedly with changing index if all constraints need to be tested.

For the usage of the CFSQP solver for the leader’s trajectory planning, the following

two inequality constraints are proposed. The inequality constraint is defined as the value

G of the constraint function that has to be less than zero. In the other case G ≥ 0,

the trajectory is considered unfeasible in the optimisation process.

Obstacle distance is the constraint that prevents solver from bringing the trajectory

too near to obstacles. Firstly, the minimal obstacle distance along the trajectory is

denoted. It can be done by sampling the trajectory and calculating the obstacle distance

r in each point p. The obstacle distance is defined in the same way as in the objective

function, i.e. r = |pco| − ro. Once the minimal obstacle distance rmin is known, it can

be compared with the constraint radius. It is convenient to choose the radius ra from

the objective function as the constraint radius. The reason is, that it is important to

keep the radius r in r > ra to keep formula (5.1) valid. The value of the constraint

function Go is defined as Go = ra − rmin.

Distance to goal constraint is designed to ensure that the leader reaches the target

zone. It works similarly as the previous constraint. Firstly, the final position is deter-

mined as the last point of the sampled trajectory. Then, the distance to goal rg can be


determined as the euclidean distance of the final position and the goal position. The final

value is determined as Gg = rg − rgmax, where rgmax is the maximal allowed distance

from the goal.

28 CHAPTER 6. TRAJECTORY TRACKING FOR FOLLOWERS

6 Trajectory tracking for followers

In this chapter, the proposed approach to trajectory planning for followers is presented.

In section 6.1, the method of formation representation is described. In section 6.2,

the processes of follower’s initialization and reinitialization is described. In section 6.3,

the trajectory optimisation method is presented.

6.1 Formation representation

The representation of the shape of ground robots formation is presented in the PhD

thesis [15]. The follower’s position in the formation is represented by a relative distance

from leader, defined in curvilinear coordinates. In this section, the proposed extension

of this formation representation for quadrotor UAVs is introduced.

The relative position of a follower in the formation is denoted by three parameters.

To the original pi, qi, a z–coordinate extension ri has to be added. The purpose of each

of these parameters is illustrated in figures 6.1 and 6.2.

(a) Straight motion, i.e. ωL =0 rad · s−1.

(b) Curved motion, i.e. ωL 6= 0 rad · s−1.

Figure 6.1: Illustration of formation representation parameters p, q. They are takenrelatively to the current position of the leader of the formation (red).

In the simulation, the time tpi is used to determine the position of a follower Fi. It

is the time, when the leader is in a state sL(tpi) ∈ SE(3), that is in travelled distance pi

backwards from its actual state sL(t) ∈ SE(3). The desired state of the follower Fi, with

formation parameters pi, qi, ri is sFi(t) ∈ SE(3). It can be obtained using the leader’s

state sL(tpi). The rotation matrix RFi(t) remains the same, i.e. RFi

(t) = RL(tpi).

The position vector xFi(t) can be denoted as

xFi(t) =

xFi(t)

yFi(t)

zFi(t)

=

xL(tpi)− qi sin(γ)

yL(tpi) + qi cos(γ)

zL(tpi) + ri

, (6.1)

CHAPTER 6. TRAJECTORY TRACKING FOR FOLLOWERS 29

Figure 6.2: Illustration of formation representation parameters q, r. They are takenrelatively to the current position of the leader of the formation (red).

where γ is the yaw Euler angle obtained from the matrix RL(tpi).

6.2 Initialization

In this section, the proposed initialization method is presented. Unlike the leader, in

case of followers, the initialization is a part of the control loop. The reason is that

the optimisation vector uses only the first N constant–duration elements, i.e. M = 0.

Therefore, only a small part of the trajectory is optimised. This approach is convenient,

because the follower has to track the leader’s trajectory and avoid obstacles. Thus it

does not need the global information about the environment.

The initialization is run before the loop is started. During the first initialization,

the follower obtains a trajectory vector containing N elements with fixed duration texecute.

The vector is a part of the leader’s trajectory, which is transformed with respect to

follower’s position in the formation given by pi, qi, ri. During each iteration, the first

element of the vector is used as a control input. This is the reason why, the initialization

is run again at the end of each iteration, adding the missing element.

The formation parameter pi is used to determine the time tpi . Then, the initial vector

starting in tpi can be created. From that time N elements are taken from the leader’s

trajectory vector, each with duration texecute. It may happen, that a follower requests

initial seed with negative tpi . The cause is that the follower starts in distance pi be-

hind the leader. In this case, the trajectory vector is replaced by a straight line, un-

til the leader’s start position is reached. From that moment, the time tpi is positive.

In case of reinitialization, the newly added interval’s starting time is determined as

tNstart = tpi + (N − 1) · texecute. The duration is constant, i.e. texecute.

The formation parameter qi affects the turning radius. For each interval that con-

tains non–zero angular velocity, the tangential velocity correction has to be performed.

The tangential velocity correction can be determined by substituting qi as a radius and

the angular velocity ωn into equation (4.9). The corrected tangential velocity is given as


vτ n = vτ L − qiωn, (6.2)

where vτ L is the original tangential velocity and n ∈ 〈1, N〉 denotes the index of the con-

trol element.

The formation parameter ri does not affect the control. It is because of the design of

the control parametrisation.

6.3 Trajectory optimisation

The proposed approach to the follower’s trajectory optimisation is presented in this

section. The optimisation process is similar to the leader’s one. There are used objective

and constraint functions to evaluate the trajectory too. However, the implementation is

different. The proposed objective function implementation is presented in section 6.3.1

and the constraint function implementation is further discussed in section 6.3.2.

6.3.1 Objective function

The purpose of this function is to provide an evaluation of the trajectory by user–defined

parameters. It is used by the CFSQP to distinguish whether one iteration of the trajec-

tory is better than the other.

The proposed objective function F is denoted as weighted sum of all components

described in the following paragraphs. User provided weights determine the importance

of a particular component.

Obstacle penalty is an evaluation component that prevents followers from collisions

with obstacles and with each other. The obstacle penalty function is defined in the same

way as defined in the leader’s implementation, see section 5.2.1. The only difference is

that in case of a follower, all other followers in the formation are registered as obstacles.

When a failure of any member of the formation occurs, all other followers are able to

avoid the collision.

For purposes of the follower collision avoidance, the time element has to be added

to the position vector. This approach allows to consider movement, when determining

possible collision. When a follower is asked for its future position, it can be easily

determined by a state prediction based on the control vector it holds. The mutual

distance is determined as euclidean distance between the followers reduced by sizes of

both quadrotors:

r = |xF1(tF1)xF2(tF1)| − rF1 − rF2 , (6.3)

CHAPTER 6. TRAJECTORY TRACKING FOR FOLLOWERS 31

where r is the obstacle distance, used in equation (5.1), rFiis the size of i–th quadrotor

and tF1 is the time when follower F1 is in position xF1 .

The trajectory has to be sampled first to be evaluated. The result of sampling process

is a set of points extended by the time component. For each point p, the distance

r is determined as r = |pco| − ro, where ro is the radius of the obstacle and |pco| is

the euclidean distance between the point p and the center of the obstacle co. Then, for

each point p, the follower collision distance is determined using equation (6.3). All values

are transformed using equation (5.1). Then the maximal value is taken as the obstacle

objective function Fo value. It means that Fo = max (Ro), where Ro is the set of results of

the function (5.1) for each point of the sampled trajectory, considering both environment

obstacle collision avoidance and follower collision avoidance.

Difference from the desired position in the formation is the crucial component

for tracking of the leader’s trajectory. The desired position is given by formation pa-

rameters pi, qi, ri and the actual leader’s position. It can be determined using equation

(6.1), as it is described in section 6.1. The difference is then defined as euclidean distance

of actual and the desired position.

This objective function slightly differs from others, because it consists of more values.

Firstly the difference is calculated in each point of sampled trajectory. The first two

components are the average difference and the maximal difference. Use of those two

components alone cause that followers start oscillating if a slight difference occurs. This

effect is caused by the trajectory parametrisation, which in fact divides the trajectory

into two separate parts: xy and z. For example, the oscillation happens if the solver

tries to minimize the z axis difference by a change of the angular velocity.

Therefore two other components had to be added to stabilize followers in the forma-

tion. The first additional component is an average difference in the xy plane. And finally

the last one is an average difference in the z axis direction.

The value of the component Ffd is defined as a weighted sum of all four components.

Angular velocity denotes the curvature of the trajectory segment. The straighter

trajectories are preferred, when this evaluation component is added to the objective

function. The value of the angular velocity parameter Fa is denoted as a sum of angular

velocities of all trajectory elements, i.e. Fa =M−1∑i=0

ωi.

Control difference helps to stabilize the leader’s trajectory tracking problem. The

value Fc is denoted as a sum of differences between the follower’s optimisation vector

elements and the leader’s control. Of course, the formation parameters pi, qi, ri have to

be taken into account. As it was mentioned in section 6.2, the parameter pi is used to


denote tpi . And the tangential velocity has to be corrected by qi as shown in equation

(6.2).

6.3.2 Constraint function

The purpose of this function is to add motion constraints into the optimisation process.

The function has the same structure as it has in the leader’s case. That means, there are

two constraints providing the value G of the constraint function. The value G has to fulfil

the condition G < 0, otherwise the trajectory is considered unfeasible in the optimisation

process.

Obstacle distance is the constraint that prevents solver from bringing the trajectory

too near to obstacles. Firstly, the minimal obstacle distance along the trajectory is

denoted. It can be done by sampling the trajectory and calculating the obstacle distance

r in each point p. The obstacle distance is defined in the same way as in the objective

function, i.e. r = |pco| − ro. Then, for each point p, the follower collision distance is

determined using equation (6.3). Once the minimal obstacle distance rmin is known, it

can be compared to the constraint radius. So the value of the constraint function Go is

defined as Go = ra − rmin.

Difference from the desired position in the formation constraint is designed to

limit the follower to a region around its desired position. The radius of the region has

to be sufficiently large. Thus allowing the follower to avoid collisions. The difference

is then defined as euclidean distance of the actual and the desired positions, as it is

described in the previous section. It is calculated in each point of the sampled trajectory.

And the distance from desired position in formation rfd is defined as the maximum

of all calculated differences. The value of the constraint function is defined as Gf =

rfd − rfdmax, where rfdmax is the maximal allowed distance from the desired position in

the formation.

CHAPTER 7. EXPERIMENTAL VERIFICATION 33

7 Experimental verification

In this chapter, the experimental verification of the proposed approaches is presented.

In section 7.1, the ability to keep the formation shape is verified. In section 7.2, the sys-

tem’s ability to avoid collision with obstacles is verified. In section 7.3, the verification

of the system’s ability to prevent collisions between followers is presented. Finally, in

section 7.4, the real–world application simulation result is presented. In section 7.5,

the verification results of simulations are discussed and summarised.

For better visualisation, figures showing trajectories are divided into three parts. In

the top–left corner, there is a 3D view. In the top–right corner, there is an xz–plane

view. And finally in the lower–left corner, there is an xy–plane view. In each of the plane

views of the figure, there are visualized only relevant obstacles. For example in figure

7.7, the top obstacle is relevant only in the xz–plane view, because in the xy–plane view

it would cover up details of the trajectories with no additional information value.

7.1 Formation keeping

The verification of the ability of the proposed system to keep formation is presented in this

section. It is a simple experiment performed in obstacle–free environment. The length

of the optimisation vector is N = 10, providing good formation stability properties.

Trajectories of the formation members are shown in figure 7.1. The shape of the formation

is shown in the figure 7.2. In figures 7.3, 7.4 and 7.5, there is provided overview of control

of the leader of the formation during the simulation.

In the figure 7.4, there is shown the phase of acceleration of the leader. The total

thrust force is set to required value and the leader is rotated to the required pitch angle

β. After the leader acceleration is complete it is rotated back and the total thrust force

is reduced.

In figures 7.3 and 7.5, there is an illustration of the turning phase. The leader is

rotated to required roll angle α (see figure 7.3) to perform the turn. Then the turning

process is started by a moment impulse around the axis o, as it is described in chapter 4.2.

It is illustrated in the bottom graph in the picture 7.5. In case of real quad–rotor UAV,

there would be only the first and the last impulses. The presence of multiple impulses is

caused by implementation of control function of the dynamics of the quadrotor. All of

the redundant impulses cancel each other out, without affecting the simulation.

7.2 Obstacle avoidance

In this section, the verification of the follower’s ability to adapt the formation shape

if it is enforced by obstacles is presented. Normally, all presented collision situations

34 CHAPTER 7. EXPERIMENTAL VERIFICATION

−10

12

3

−0.500.511.5−0.5

0

0.5

x [m]y [m]

z [

m]

−1 0 1 2 3−0.5

0

0.5

x [m]

z [

m]

−1 0 1 2 3−0.5

0

0.5

1

1.5

x [m]

y [

m]

Initial trajectory

Leader

Follower 1

Follower 2

Follower 3

Figure 7.1: The example of formation keeping behaviour.

Figure 7.2: The 3D model of the formation.

would be solved by the leader’s planning method, as it is demonstrated in figure 7.6. For

the purpose of simulation of unobserved obstacles, the leader’s formation representation

radius was significantly reduced. The reduction of the radius makes the leader to ignore

the obstacles in its planning loop, because they appear sufficiently distant. An obstacle is

discovered by a follower, in its short–term planning loop. In order to avoid the collision,

follower is enforced to divert from its ideal trajectory. To provide better maneuverability,

the length of the optimisation vector is set to N = 3.

In figure 7.7, the verification of the system’s ability to avoid the collision with pre-

viously unknown obstacles is presented. The second follower (green) performs sharp


0 1 2 3 4 5 6 7 8

42.6

42.8

t [s]

f [N

]

0 1 2 3 4 5 6 7 8−4

−2

0x 10

−3

t [s]

α [

rad

]

0 1 2 3 4 5 6 7 8−0.04

−0.02

0

t [s]

ωx [

rad

.s−

1]

0 1 2 3 4 5 6 7 8−5

0

5

t [s]

Mx [

N.m

]

Figure 7.3: The illustration of the x–components of the leader’s control inputs and statesduring the formation–keeping simulation. In the top graph, there is shown the totalthrust force. In the second graph, there is the roll Euler’s angle. In the third graph,there is shown the angular velocity around the body fixed x–axis. In the last graph,there is shown an x–component of the moment vector that controls the quadrotor.

left turn, after the obstacle is discovered. Thus making the first follower (red) to begin

avoiding the expected collision. The result is declination of the first follower’s trajectory,

noticeable around 2 m in the xz–view in figure 7.7. The third follower performs less

dramatic maneuver. In order to avoid the collision, it slightly declines its trajectory.

In figure 7.8, the second experiment is presented. Third obstacle was added to force

followers one and two to move against each other. It is obvious that the unknown

obstacles break the stability of the formation.

Both of previous examples are mainly focused on avoiding to cross the safety radius

at all costs. Therefore the stability of the formation is broken. In figure 7.9, results of

simulation with formation–keeping preferred are presented. It is achieved by reducing

the obstacle avoidance weight in the objective function used by the follower’s CFSQP.

As you can see, the formation is more stable. Followers are trying to either over or

underpass the obstacle. This approach increases the risk of collision with an obstacle.


0 1 2 3 4 5 6 7 8

42.6

42.8

t [s]

f [N

]

0 1 2 3 4 5 6 7 8−0.1

0

0.1

t [s]

β [

rad

]

0 1 2 3 4 5 6 7 8−1

0

1

t [s]

ωy [

rad

.s−

1]

0 1 2 3 4 5 6 7 8−200

0

200

t [s]

My [

N.m

]

Figure 7.4: The illustration of the y–components of the leader’s control inputs and statesduring the formation–keeping simulation. In the top graph, there is shown the totalthrust force. In the second graph, there is the pitch Euler’s angle. In the third graph,there is shown the angular velocity around the body fixed y–axis. In the last graph,there is shown an y–component of the moment vector that controls the quadrotor.

7.3 Followers collision avoidance

In this section, the proposed system’s ability to prevent collisions between formation

members is presented. Its functionality is already demonstrated in figure 7.7 in the pre-

vious section. More explicit demonstration is shown in figure 7.10. To simulate the failure

of a follower, a condition was added to the control loop. This condition makes the follower

to stop the control loop and stay at the same position until the end of the simulation.

The second follower (green), which is supposed to fly in front of the first follower (red),

stops its motion after a given time. As you can see in the figure 7.10, the follower number

one (red) reacts by a sharp decline of its trajectory, thus under–flying the malfunctioned

quadrotor.

7.4 Real–world application simulation

In this section, results of simulation of formation movement in a more complicated envi-

ronment are presented. In this simulation, several followers are deployed. The length of

the optimisation vector N = 5 is used. This setting was chosen compromising between


0 1 2 3 4 5 6 7 8

42.6

42.8

t [s]

f [N

]

0 1 2 3 4 5 6 7 80

0.5

1

t [s]

γ [

rad

]

0 1 2 3 4 5 6 7 8−0.2

0

0.2

t [s]

ωz [

rad

.s−

1]

0 1 2 3 4 5 6 7 8−50

0

50

t [s]

Mz [

N.m

]

Figure 7.5: The illustration of the z–components of the leader’s control inputs and statesduring the formation–keeping simulation. In the top graph, there is shown the totalthrust force. In the second graph, there is the yaw Euler’s angle. In the third graph,there is shown the angular velocity around the body fixed z–axis. In the last graph, thereis shown an z–component of the moment vector that controls the quadrotor.

maneuverability, formation stability and simulation time consumption. In comparison

to the previous simulations, the formation travels five times longer trajectory.

In figure 7.11, there are shown snapshots capturing the first quarter of the flight of

the formation. It is evident that during this period the formation begins to change its

shape. When the goal is reached, the formation has a different shape. The important

thing is that all followers reach the target zone, there are no lost robots.

In figure 7.12, there is shown the formation approaching to the target zone. The op-

posite direction of the follower robots is caused by the attempt to reach the desired

position in the formation.

7.5 Summary

The results presented in the first three sections verified the correct performance of the al-

gorithm. The last presented simulation, i.e. the real–world application simulation, shows

that the simulation framework is applicable only in a simple scenarios. It is mostly caused

by the simplified obstacle representation.


02

4

−202

0

1

2

x [m]y [m]

z [

m]

0 2 4

0

1

2

x [m]

z [

m]

0 2 4−2

−1

0

1

2

x [m]

y [

m]

Initial trajectory

Leader

Follower 1

Follower 2

Follower 3

Safety radius

Figure 7.6: The demonstration of the correct maneuver of the whole formation.

0

2

4

−2−1

0

0

1

2

x [m]y [m]

z [

m]

0 2 4

0

1

2

x [m]

z [

m]

0 2 4−2

−1

0

x [m]

y [

m]

Initial trajectory

Leader

Follower 1

Follower 2

Follower 3

Safety radius

Figure 7.7: The first collision avoidance demonstration with the reduced leader’s safetyradius settings is shown in this figure.

The implemented software has the potential to be applied on real quad–rotor UAVs

in the future. However, the obstacle representation has to be extended. Along with it,

more sophisticated obstacle distance calculation method has to be used. The leader’s


0 2 4−202

0

1

2

x [m]y [m]

z [

m]

0 2 4

0

1

2

x [m]

z [

m]

0 2 4−2

−1

0

1

2

x [m]

y [

m]

Initial trajectory

Leader

Follower 1

Follower 2

Follower 3

Safety radius

Figure 7.8: The second collision avoidance demonstration with the reduced leader’s safetyradius settings is shown in this figure.

planning method may be improved by dividing the trajectory into three parts instead

of two. The first, immediate control and the second long–term optimisation parts stay

as they are deployed in this thesis. The additional third component is intended to keep

the initial path to goal, thus only a part of the whole trajectory would be optimised, i.e.

coded in the preceding two parts. The promised contribution is a significant reduction

of the time needed for the optimisation.


024−2−1012

−0.5

0

0.5

1

1.5

2

x [m]y [m]

z [

m]

0 2 4

0

1

2

x [m]

z [

m]

0 2 4−2

−1

0

1

2

x [m]

y [

m]

Initial trajectory

Leader

Follower 1

Follower 2

Follower 3

Safety radius

Figure 7.9: The collision avoidance demonstration with formation–keeping preferred set-tings is shown in this figure.

−10

12

−10

1−0.5

0

0.5

x [m]y [m]

z [

m]

−1 0 1 2−0.5

0

0.5

x [m]

z [

m]

−1 0 1 2−1

−0.5

0

0.5

1

x [m]

y [

m]

Initial trajectory

Leader

Follower 1

Follower 2

Follower 3

Figure 7.10: Demonstration of the failure tolerance mechanism.


Figure 7.11: Snapshots capturing the formation flight around a big obstacle.


Figure 7.12: Snapshots capturing the formation approaching the target zone. The cam-era is situated behind the formation.

CHAPTER 8. CONCLUSION 43

8 Conclusion

The goal of this thesis was to extend the 2D leader–follower formation driving method

presented in the PhD thesis [15] to a three–dimensional space, applicable for driving

a formation of quadrotor UAVs. A Model Predictive Control method was used for

the formation control. In order to represent a trajectory of a quadrotor as a vector

of constant control inputs for the Model Predictive Control, an approach presented in

section 4.2 had to be designed. The proposed approach of the trajectory representation

by a vector of constant control inputs is more closely described in section 4.4.

The leader–follower method of the formation driving divides the problem of the for-

mation trajectory planning into two parts. The first part is the trajectory planning for

the leader of the formation. The leader’s trajectory planning is crucial because it includes

global properties of the formation motion. The proposed MPC approach of the leader’s

trajectory planning is described in chapter 5. The second part of the formation trajec-

tory planning problem is the trajectory tracking for followers. Each follower is supposed

to track the leader’s trajectory with the relative position to the leader, given by param-

eters pi, qi, ri. Thus the follower’s trajectory planning is performed only locally, with

respect to the leader’s trajectory. The proposed trajectory tracking method for followers

is presented in chapter 6.

The functionality of the proposed approaches was verified via numerical simulations.

Results of the experimental verification are presented in chapter 7.

There are numerous possible extensions of the simulation software. One of the sug-

gestions is to implement more initialization methods such as the Voronoi diagrams or

the RRT algorithm and compare their performance. Another possible extension is to im-

plement a more advanced method of representation of the environment. Together with

it, more sophisticated obstacle distance calculation method has to be used. The improve-

ment of the leader’s planning method, suggested in chapter 7.5, is to divide the trajectory

into three parts instead of two. The first, immediate control and the second long–term

optimisation parts stay as they are deployed in this thesis. The additional third compo-

nent is intended to keep the initial path to goal, thus only a part of the whole trajectory

would be optimised, i.e. coded in the preceding two parts. The promised contribution is

a significant reduction of the time needed for the optimisation.

44 CHAPTER 9. BIBLIOGRAPHY

9 Bibliography

[1] A. Abdessameud and A. Tayebi. Formation control of vtol unmanned aerial vehicles

with communication delays. Automatica, 47(11):2383–2394, 2011.

[2] T. Balch and R. C. Arkin. Behavior-based formation control for multirobot teams.

Robotics and Automation, IEEE Transactions on, 14(6):926–939, 1998.

[3] N. Basilico and S. Carpin. Online patrolling using hierarchical spatial representa-

tions. In Robotics and Automation (ICRA), 2012 IEEE International Conference

on, pages 2163–2169. IEEE, 2012.

[4] E. W. Dijkstra. A note on two problems in connexion with graphs. Numerische

mathematik, 1(1):269–271, 1959.

[5] V. Kumar and N. Michael. Opportunities and challenges with autonomous micro

aerial vehicles. The International Journal of Robotics Research, 31(11):1279–1291,

2012.

[6] J.-C. Latombe. ROBOT MOTION PLANNING.: Edition en anglais. Kluwer aca-

demic publishers, 1990.

[7] S. LaValle. Rapidly-exploring random trees: A new tool for path planning. Technical

report, TR 98-11, Computer Science Dept., Iowa State University, 1998.

[8] S. LaValle. Planning algorithms. Cambridge University Press, 2006.

[9] C. Lawrence, J. Zhou, and A. Tits. User’s guide for cfsqp version 2.5. University of

Maryland, 1997.

[10] T. Lee, M. Leoky, and N. McClamroch. Geometric tracking control of a quadrotor

uav on se (3). In Decision and Control (CDC), 2010 49th IEEE Conference on,

pages 5420–5425. IEEE, 2010.

[11] D. Q. Mayne, J. B. Rawlings, C. V. Rao, and P. O. Scokaert. Constrained model

predictive control: Stability and optimality. Automatica, 36(6):789–814, 2000.

[12] P. Pounds, R. Mahony, and P. Corke. Modelling and control of a quad–rotor robot. In

Proceedings Australasian Conference on Robotics and Automation 2006. Australian

Robotics and Automation Association Inc., 2006.

[13] W. Ren and R. W. Beard. Formation feedback control for multiple spacecraft via

virtual structures. In Control Theory and Applications, IEE Proceedings-, volume

151, pages 357–368. IET, 2004.

CHAPTER 9. BIBLIOGRAPHY 45

[14] M. Saffarian and F. Fahimi. Non-iterative nonlinear model predictive approach

applied to the control of helicopters’ group formation. Robotics and Autonomous

Systems, 57(6):749–757, 2009.

[15] M. Saska. Trajectory planning and optimal control for formations of autonomous

robots. PhD thesis, Schriftenreihe Wurzburger Forschungsberichte in Robotik und

Telematik, Band 3. Wurzburg: Universitat Wurzburg, 2010.

[16] H. G. Tanner, G. J. Pappas, and V. Kumar. Leader-to-formation stability. Robotics

and Automation, IEEE Transactions on, 20(3):443–455, 2004.

46 CHAPTER 9. BIBLIOGRAPHY

APPENDIX A. CONTENT OF ENCLOSED CD I

A Content of enclosed CD

The CD attached to the printed version of this thesis contains the source code of

the application, LATEXcode of this thesis, PDF version of this thesis and visualization of

the formation maneuvres.

Directory Contentthesis LATEXcode of the document.application Source code of the application.videos Visualisation of selected experiments.kaslzden DT.pdf Electronic version of this document.

Table A.1: Content of the enclosed CD

Date post:	22-Feb-2020
Category:	Documents
Upload:	others
View:	2 times
Download:	0 times

3D formations of unmanned aerial vehicles Zden ek Kasl · cable for a formation of autonomous...

Documents