Drone flock: formation control of multi-drones

FACULDADE DE ENGENHARIA DA UNIVERSIDADE DO PORTO Drone Flock: formation control of multi-drones João Costa Master Degree in Electrical Engineering and Computer Science Supervisor: António Pedro Aguiar October 2019 Resumo Nas últimas décadas tem-se assistido a uma crescente utilização de drones para substituir veículos tripulados em aplicações de alto risco, alto custo ou restringidas a nível de espaço. As suas capacidades levaram ao crescimento constante do número de aplicações e os drones deixaram de ser usados apenas para fins militares. Novos algoritmos para controlo do movimento têm sido desenvolvidos e optimizados para tornar estes sistemas mais independentes e efectivos. No entanto, é importante salientar que este tipo de veículos sub-atuados têm uma dinâmica altamente não-linear e naturalmente instável, o que faz com que sejam problemas especialmente desafiantes do ponto de vista do controlo não-linear. Os avanços tecnológicos mais recentes conduziram a uma nova direção de investigação referente ao controlo de sistemas multi-agente a agirem cooperativamente. Existem inúmeras aplicações onde sistemas adaptáveis de pequenos veículos autónomos são a melhor solução. Nesta dissertação, é proposto um algoritmo de controlo baseado em funções de Lyapunov. O objetivo principal deste algoritmo denominado cooperative path following é capacitar os drones de realizarem a tarefa de seguir o trajeto desejado e em formação de modo a atingirem consenso entre as velocidades. Para isso, numa primeira fase começa-se por um controlador mais simples, a duas dimensões, de cooperative path following implementado num sistema de robôs tipo monociclo para introduzir as ferramentas a usar, alguns fundamentos teóricos e, principalmente, para mais facilmente abordar o problema do consenso. O controlador começa por lidar com o problema do seguimento de trajetória fazendo com que o drone siga um ponto virtual desejado nesse trajeto, parameterizado por uma variavel de controlo. Em seguida, a velocidade desejada para a variável de parametrização de cada drone é ajustada de modo a manter todo os drones em formação, o que acontece quando todas as variáveis de parametrização estão em consenso e evoluem à mesma velocidade. Para verificar as soluções obtidas, simulações são efetuadas numa ferramenta do Matlab, desenvolvida especificamente para a projecção de controladores e simulação de sistemas, chamada Virtual Arena. Simulações de robots e de drones em formação são feitas em duas configurações da rede de comunicações diferentes. Os resultados mostram que as leis de controlo não-linear baseadas em funções de Lyapunov funcionam devidamente tanto no seguimento de trajetória, como no consenso dos drones. i ii Abstract For decades, humans have been using drones to replace human-crewed vehicles in high risk, high cost, or space-constrained applications. Their capabilities lead to a constantly growing range of applications and drones are no longer military. To account with the increasing of applications and their complexity, motion control algorithms have been developed and optimized to make these systems as independent and effective as possible. This kind of under-actuated vehicle has highly non-linear and naturally unstable dynamics, which make their non-linear control especially challenging. The recent technological advances have incited developers towards cooperative motion control of multiple agent systems. There are several applications where adaptable flocks of small, autonomous vehicles seem to be the best solution. In this dissertation, a Lyapunov-based control algorithm is proposed and analized. The main goal of this algorithm is to implement a cooperative path following controller that makes the drones follow a desired path and the flock achieves consensus. To this end, we start first with a simpler, 2 dimentional, cooperative path following controller that is developed to a unicycle type robot flock to get introduced to the tools, the theoretical preliminaries, and, firstly, easily approach the consensus problem. This controller, firstly, deals with the path following problem by making drones follow a desired virtual point parametrized by a path parametric variable. Secondly, the desired parametric variable speed of each drone is adjusted in order to keep them in formation, which happens when these parametric variables are in consensus and evolving at the same desired speed. To verify the obtained solutions, simulations are done using an object-oriented Matlab toolkit for control design and system simulation, called Virtual Arena. Simulations for both unicycle and drone flocks are performed with several distinct communication network configurations. The results show that the nonlinear Lyapunov based control laws proposed work properly on both path following and consensus problems. iii iv Contents 1 Introduction 1 1.1 Context ....................................... 1 1.2 Motivation...................................... 2 1.3 Goals ........................................ 3 1.4 DocumentStructure................................. 3 2 State of the Art, Fundamentals and Tools 5 2.1 StateoftheArt ................................... 5 2.1.1 Drones ................................... 5 2.1.2 Flocking and Cooperative Path Following Algorithms . . . . . . . . . . . 8 2.2 TheoreticalPreliminaries.............................. 9 2.2.1 Graphs ................................... 9 2.2.2 Space State Model Representation . . . . . . . . . . . . . . . . . . . . . 11 2.2.3 Lyapunov’s Stability Theorem . . . . . . . . . . . . . . . . . . . . . . . 13 2.2.4 AgreementProtocol ............................ 14 2.3 VirtualArena .................................... 14 2.3.1 DroneFlockExample ........................... 15 3 System Model 19 3.1 CoordinationFrames ................................ 19 3.2 QuadcopterModel ................................. 20 4 Motion Control 23 4.1 PathFollowing ................................... 23 4.1.1 Unicycle .................................. 23 4.1.2 Quadcopter................................. 25 4.2 Cooperative Path Following . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 5 Simulation Results 31 5.1 UnicycleFlock ................................... 31 5.1.1 PathFollowing ............................... 32 5.1.2 Cooperative Path Following . . . . . . . . . . . . . . . . . . . . . . . . 34 5.2 DroneFlock..................................... 40 5.2.1 PathFollowing ............................... 40 5.2.2 Cooperative Path Following . . . . . . . . . . . . . . . . . . . . . . . . 43 6 Conclusions 49 v vi CONTENTS List of Figures 2.1 Multicopter configurations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 2.2 Direction of a quadcopter’s motor rotation . . . . . . . . . . . . . . . . . . . . . 7 2.3 Motors speed in each rotation move. . . . . . . . . . . . . . . . . . . . . . . . . 8 2.4 Direct vs. Undirected graphs . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 2.5 Three diferent graph configurations . . . . . . . . . . . . . . . . . . . . . . . . . 11 2.6 Inertial and body coordinate frames . . . . . . . . . . . . . . . . . . . . . . . . 12 2.7 Complete representation of the unicycle model . . . . . . . . . . . . . . . . . . 12 2.8 Fleet Model implementation in Virtual Arena . . . . . . . . . . . . . . . . . . . 15 2.9 Drone Flock controller call in Virtual Arena . . . . . . . . . . . . . . . . . . . . 16 2.10 Virtual Arena simulation setting and run . . . . . . . . . . . . . . . . . . . . . . 16 2.11 Cooperative Path Following for a Drone Flock Implementation in Virtual Arena . 17 2.12 Content of computeInput() function ........................ 18 3.1 Inertial and Body Coordinate Frames . . . . . . . . . . . . . . . . . . . . . . . . 19 3.2 Forces and inputs acting on our body . . . . . . . . . . . . . . . . . . . . . . . . 21 5.1 Actual (circles) and desired (line) positions of the unicycle (m) . . . . . . . . . . 32 5.2 Absolute position error of the unicycle (m) . . . . . . . . . . . . . . . . . . . . . 33 5.3 Simulation results for unicycle’s path following controller . . . . . . . . . . . . . 33 5.4 Diferent network configurations for the unicycle simulations . . . . . . . . . . . 35 5.5 Actual (circles) and desired (line) positions of the unicycle flock (m) - configuration1 ........................................ 36 5.6 Simulation results for unicycle’s cooperative path following controller - configuration1 ....................................... 37 5.7 Values of ˙ γand γof all agents of the network - configuration 1 . . . . . . . . . . 37 5.8 Actual (circles) and desired (line) positions of the unicycle flock (m) - configuration2 ........................................ 38 5.9 Simulation results for unicycle’s cooperative path following controller - configuration2 ....................................... 39 5.10 Values of ˙ γand γof all agents of the network - configuration 2 . . . . . . . . . . 39 5.11 Actual (circles) and desired (line) positions of the drone (m) . . . . . . . . . . . 41 5.12 Absolute position error of the drone (m) . . . . . . . . . . . . . . . . . . . . . . 42 5.13 Values of ˙ γ, roll and pitch of all agents of the network . . . . . . . . . . . . . . . 42 5.14 Diferent network configuration for the drones simulations . . . . . . . . . . . . . 44 5.15 Actual (circles) and desired (line) positions of the drone flock (m) - configuration 1 45 5.16 Absolute position errors (m) - configuration 1 . . . . . . . . . . . . . . . . . . . 46 5.17 Values of ˙ γand γof all agents of the network - configuration 1 . . . . . . . . . . 46 5.18 Actual (circles) and desired (line) positions of the drones (m) - configuration 2 . . 47 vii 6State of the Art, Fundamentals and Tools In fixed-wing aircrafts lift is generated by the wing, like commercial airplanes, e.g., and offer longer flight times than rotary wings aircraft. However, they need long distances to take off and land, and a minimum linear speed to keep themselves in the air. On the other hand, fixed-wing aircraft have the ability to take-off and land vertically, and, once they do not need minimum speeds to generate lift, they can hover in one place. Another factor that differentiates both categories is its maneuverability. Fixed-wing aircraft require, typically, a bigger turning radius when compared to rotary-wings. These can move freely in all axis from the point where they are. The market supply of both configurations, nowadays, differs significantly. There is a higher offer in platforms, hardware, and control software for multicopters than for fixed-wing aircraft. Together with its easy of use and smaller dimensions, those were decisive factors when choosing multicopters over fixed-wings as the platform to implement the path following controller designed in this dissertation. 2.1.1.2 Multicopter configurations and components (a) Quadcopter (b) Hexacopter (c) Octocopter (d) Coaxial Octocopter Figure 2.1: Multicopter configurations Multicopters can be classified by the number of rotors they have, normally one rotor in each arm. Quadcopters, hexacopters, and octocopters are the most common configurations, figures 2.1 a), b) and c), respectively. Depending on the application requirements, bigger multicopters or co-axial configurations can be built. For example, if we need enough thrust to lift a heavy load but have space constraints, a coaxial quadcopter as in figure 2.1 d), or a coaxial hexacopter, are one of the ways to go. This work will focus on quadcopters but, with minor adjustments, the controller may work for every multicopter configuration. The components presented next are specific for the model used in this dissertation, but most of them are common to all models. Quadcopters are composed of a carbon fiber frame where motors and its controllers are installed. Motors’ controllers are called ESC, from Electronic Speed Controller. The flight controller is responsible for the low-level control of our drone. It controls the motors’ speed, and, consequently, the drone’s stabilization, piloting and positioning based on GPS coordinates. 2.1 State of the Art 7 There is also a high-level control that can be done, or not, by the same flight controller, and its in charge of the algorithms of path following, coordination, and communication between the flock’s elements. To these boards are connected all the sensors, antennas and external communication devices, like GPS antennas, radio receivers, and communication modules, to communicate with a ground station or other drones. To distribute the energy from the battery to all components, a PDB (Power Distribution Board) is used. To better understand the role of each motor in drone’s movements, motors are enumerated as shown in the figure 2.2, with the x-axis pointing to the front of the drone, y-axis pointing to its right side and z pointing down. 2.1.1.3 Working principle In order to prevent the drone from rotating around itself when motors are spinning, opposite motors have opposite rotation directions, so that in this way the total momentum when all motors spin at the same speed is equal to zero. Otherwise, if the intent is to rotate the drone around z-axis (yaw), the speed of a group of opposite motors must be higher, or lower, than the other, as shown in figure 2.3 (c) and (f)! To hover, drones must keep all motors, approximately, at the same speed. Then, depending on what the desired move is, all four motors are adjusted accordingly. To move up or down, all motors must increase or decrease their speed. Figure 2.2: Direction of a quadcopter’s motor rotation Rotating right or left is achieved by having the speed of motors 1 and 4 higher than motors 2 and 3, and vice-versa, respectively, as shown in figure 2.3 (a) and (d). Rotating to the front or the back works the same way. Motors 1 and 2 must spin faster than 3 and 4, and vice versa (figure 2.3 (b) and (e)). 8State of the Art, Fundamentals and Tools (a) Roll to Right (b) Pitch Forward (c) Yaw to Right (d) Roll to Left (e) Pitch Backward (f) Yaw to Left Figure 2.3: Motors speed in each rotation move. 2.1.2 Flocking and Cooperative Path Following Algorithms A flock is a collective movement of individual interacting entities. This behavior is constantly observed in the nature, from small insect migrations to large mammals such as elephants, and obviously birds. This phenomenon can even be observed in microorganisms. The ability of these entities, normally of low intelligence, which communicate only with their nearest neighbors, to position themselves properly so that they move in a collective, collision free and ensuring the safety of the group, has awakened the interest of various researchers over the last decades. Several models of great relevance emerged from these investigations: •Reynolds’ Model (1987) [1] - which consists in three flocking rules: –Separation - attempt to avoid crowding and collision –Cohesion - attempt to stay close to its neighbors –Alignment - match velocity with nearby flock-mates This model led to the creation of the first computer animation of a flock. •Vicsek’s Model (1995) [2] - all agents move at the same speed with direction equal to the average of the directions in the previous time instant. •Veysel Gazi’s Attraction/Repulsion Model (2002) [3] - all agents are aware of the distance to there neighbors. These feel attraction at large distances and repulsion at short distances. 2.2 Theoretical Preliminaries 9 •Tanner’s Flocking Model (2003) [4] - these control laws are a combination of attraction/repulsion and alignment forces, ensuring that there are no collisions and the whole group moves together towards a common point. •Olfati-Saber’s Flocking Model (2006) [5] - which features three algorithms that improve the group’s collective movement: –Algorithm 1 is in charge of organizing the flock –Algorithm 2 adds to algorithm 1 terms that take into account the purpose of the group. –Algorithm 3 adds to previous algorithms parameters to avoid obstacles. Among others, some of them are minor adaptations of previously mentioned models, for example Moreau (2005), Ren and Beard (2005), Olfati-Saber (2007), Tanner et al. (2007), Gazi (2008), and Su et al. (2009a, 2009b). In the most recent bibliography of Jingyuan Zhan, Xiang Li and others, 2011 [19] and 2017 [20], predictive control models are introduced to control UAV networks. For several decades, from Woods (1959) to Montague et al. (1995), it has been experimentally proven that groups in nature have predictive intelligence, that is, each individual can predict their position and group’s position based on their and on the flock’s previous positions. Jingyuan Zhan, et al. (2008)|, shows that predictive mechanisms play an important role in the collective consensus of a flock. Therefore, the authors assume that predictive models also play an important role in multi-agent system’s control, and created a new algorithm based on predictive models, developing for that purpose a Model Predictive Controller (MPC) that improves the flock performance. Years later, in [20] a flocking algorithm based on MPC for decentralized multi-agent systems was developed - Decentralized Model Predictive Control Algorithm (DMPC Algorithm). After simulations and testing, the authors find that the proposed algorithm is applicable to multi-drone systems (with a passive leader) and has several advantages such as fast convergence, ease of tuning and low processing capacity. 2.2 Theoretical Preliminaries 2.2.1 Graphs 2.2.1.1 Concept A graph is composed of a set of nodes or vertices, V={v1,..,vn}, and a set of lines or edges, E={(v1v2),(v2v3),..,(vn−1vn)}, and is usually denoted by G(V,E), or simply G. 10 State of the Art, Fundamentals and Tools (a) Undirected graph (b) Digraph Figure 2.4: Direct vs. Undirected graphs Graphs are used in this work to model the communication between the vehicles in the fleet. When applicated to multi-agent systems, the nodes represent each agent of the system and the lines the existence of communication between them. If there is a pair of nodes with a edge joining them, they are called adjacent. Each pair is composed of a head and a tail, and information flows from the first node to the second one. If communication occurs in both directions, the graph is undirected. Otherwise, it is called a directed graph or digraph (see figure 2.4). 2.2.1.2 Algebraic Representation In order to represent graphs more understandable, a matrix will be used. In graph theory and computer science, this matrix is called Laplacian Matrix. It is expressed as following: L=D−A(2.1) where Dis the degree matrix, and Athe adjacency matrix. Let A(G)be the adjacency matrix with respect to G. Its elements indicate whether pairs of vertices are adjacent or not in the graph and are defined as ai j =1 if (vi,vj)∈E,ai j =0 otherwise. D(G)is the degree matrix of a graph G. The diagonal elements of this square matrix are the out-degrees of the node vii. That is, each diagonal element denotes the number of nodes that vii is connected to. In other words, how many edges of the graph start from this node. By definition, each row of the Laplacian Matrix Lsums up to zero. 2.2 Theoretical Preliminaries 11 (a) All nodes communicate with each other (b) Each node receives information from the previous one (c) Nodes 2 and 3 receive information from node 1 Figure 2.5: Three diferent graph configurations    2−1−1 −1 2 −1 −1−1 2      1−1 0 0 1 −1 −1 0 1      000 −110 −101   (2.2) In figure 2.5, we can see some directed graphs and, underneath it, the respective Laplacian matrices. 2.2.2 Space State Model Representation In control engineering, systems are frequently modeled by its state-space representation. This representation provides the plant dynamics as a set of state, x(t)=[x1(t),x2(t),...,xn(t)]T, input, u(t) = [u1(t),u2(t),...,ur(t)]T, and output variables, y(t) = [y1(t),y2(t),...,ym(t)]T, related by first-order differential equations. The state of a system refers to a set of variables, the state variables, that describe the system at a given instant in response to the values of input variables. For Non-Linear systems, the state equations may be written as ˙x=f(t,x,u). This way of describing a system expresses the time derivative of each state in terms of state variables and system inputs. The output of a system is defined to be any system variable of interest, and, for a majority of physical systems, it depends only on the state variables. The following equation shows us how the output equation of a system is normally written for this kind of systems, y=g(t,x,u). 2.2.2.1 Unicycle Example In this chapter, the model of a unicycle on a 2D plan will be designed using its space-state representation. Such a primary example is meant to clarify how space-state representation works, and the same study case will be later used to introduce the toolkit used during this dissertation. 12 State of the Art, Fundamentals and Tools Coordinate Frames This model has two reference frames: the inertial coordinate frame, I, and the body coordinate frame, B, attached to the unicycle’s body. Figure 2.6: Inertial and body coordinate frames The relation between both frames is given by the rotation matrix R, uses Euler angles, and for this 2D example takes the following form: R="cos(θ)−sin(θ) sin(θ)cos(θ)#(2.3) θis the orientation of the vehicle as represented in figure 2.6. Model Figure 2.7: Complete representation of the unicycle model 2.2 Theoretical Preliminaries 13 The variables of interest of this system are the position and orientation of the unicycle. So, our state vector will look like this: x(t) = hp(t)θ(t)iT∈R3(2.4) The inputs of the unicycle are the linear and angular velocities, vand ω, respectively. As our unicycle can only move forward, and backward, the velocity perpendicular to the body, vn(t), is equal to zero. Therefore,the linear vector velocity expressed in the body frame is given by v(t) = hvf0iT∈R2. The system’s control input vector may be written as follow u(t) = hvf(t)ω(t)iT∈R2(2.5) The kinematic model of the unicycle is described by ˙p(t) = R(t)v(t)(2.6) ˙ R(t) = R(t)sk(ω(t)) (2.7) where R is the rotation matrix and the matrix sk(ω(t)) is the skew-symmetric matrix associated with the angular velocity ω(t). The state-space model (2.6)-(2.7) can be further simplified to    ˙x(t) ˙y(t) ˙ θ(t)   =   cos(θ)0 sin(θ)0 0 1   "vf(t) ω(t)#(2.8) 2.2.3 Lyapunov’s Stability Theorem This method, called the direct method, uses a Lyapunov function V(x)to prove the stability of a system. Let ˙x=f(x)be a system with an equilibrium point at x=0 and D⊂Rnbe a domain containing x=0. Consider a function V(x):D→Rcontinuously diferentiable such that V(0) = 0, and V(x)>0,∀x∈D\{0}. Then, x=0 is stable if ˙ V(x)≤0,∀x∈D\{0}. x=0 is asymptotically stable if ˙ V(x)<0,∀x∈D\{0}, . If V(x):Rn→Ris a function continuously diferentiable such that V(0) = 0, V(x)>0,∀x∈D\{0}, and 14 State of the Art, Fundamentals and Tools kxk→ ∞⇒V(x)→∞,∀x\{0}. If ˙ V(x)<0,∀x\{0} x=0 is now globally asymptotically stable (GAS). This method will be used later to derive the control laws for our systems. Control Lyapunov functions will be proposed to find suitable control laws for the dynamic tracking error equation, as we will see later on this work. 2.2.4 Agreement Protocol Making a drone flock achieve consensus is the main goal of the controller designed in this dissertation. In this sub chapter, the agreement protocol used in that controller is introduced. That protocol is pretended to work on undirected and directed static networks. Consensus is achieved when the agents of a flock, swarm or another kind of network, join all in a state value, and it is one of the fundamental problems in multi-agent systems coordination. Static networks are those networks where the edges do not vary over time. Which is the case of this work, due to limitations of the toolkit used in the simulations. Let n be the number of dynamic agents that make up our network, and they can change information with other elements. The variation of each agent’s state results from the sum of its relative states with respect to its neighborhood (a subset of agents adjacent to it). Taking the graph presented in figure 2.5 a), an example of the agreement protocol will be presented next. Let each node’s state be represented as xi, being iits respective number. The dynamic of each node’s state is given by ˙x1= (x2−x1)+(x3−x1)(2.9) ˙x2= (x1−x2)+(x3−x2)(2.10) ˙x3= (x1−x3)+(x2−x3)(2.11) Or, in its generalized form, as follows ˙xi(t) = ∑ j∈Ni (xj(t)−xi(t)),i=1,...,n(2.12) 2.3 Virtual Arena VirtualArena is an open-source Object-Oriented Matlab Toolkit for control design and system simulation. The use of this toolkit reduces the time spent on the design and validation of a control architecture. 2.3 Virtual Arena 15 This toolkit provides a set of ready-to-use functions often used in control design that, along with its object-oriented architecture, increase the modularity, reliability and reusability of the controller’s components and reduce the development time. After defining our model’s system, its initial conditions, design a control input to drive it to the desired state, the architecture is validated via simulation. To make it more realistic, state and output disturbances can be added. All of these steps can be done with functions included in the toolkit. It makes the simulation phase as easy as specify the system, stopping criteria, discretization step and run it, for example. More information about how to use the toolkit can be found in [13] and [14]. In the next subsection the final implementation of this work in Virtual Arena will be presented for two reasons. First, to show a different, more complex, example of implementation with the toolkit, that anyone who’s trying to understand how it works or how to implement some specific component and don’t find documentation for it. Secondly, to show how the following work was implemented. For that, it’s advisable to come back after reading chapters 3 and 4. 2.3.1 Drone Flock Example As said in the beginning of this chapter, Virtual Arena keeps the design and simulation simple and organized in a few functions. 2.3.1.1 Fleet Model It starts with the Fleet Model definition. First specifying the type of system and some parameters as the state equations, lines 42 to 56, number of states and number of inputs, lines 57 and 58. Figure 2.8: Fleet Model implementation in Virtual Arena Lines 43 to 45 represent the first three states, ˙x(t). States 4, 5 and 6 are the speed evolution in each cycle, ˙v(t). As the inputs of the system (u(1) to u(5)) are the variation in each iteration and the dynamic kinematic equations need the absolute value of the inputs, we can take advantage of Virtual Arena’s integrated state integration, as well as its automatic rotation matrix update in each cycle. Therefore, 22 System Model FT=   0 0 −1   u1(3.6) Fg=RT   0 0 mg   (3.7) Some limitations must be taken into consideration to make these equations represent the model as accurately as possible. In practical applications, drones have limits, imposed by the user or its physical limitations, in thrust force’s intensity, on its angular velocities, and, depending on the flight mode, on the values that its angles can take. Therefore, when the controller is implemented on Virtual Arena, the following limitations are applied 0≤FT≤FTMax (3.8) Our model is from a quadcopter with a conventional propulsion system, it does not have 3D capabilities, as varying pitch helicopters, 3D freestyle airplanes or 3D drones, that can propel themselves downward. −RollMaxRate ≤ω1≤RollMaxRate (3.9) −PitchMaxRate ≤ω2≤PitchMaxRate (3.10) −YawMaxRate ≤ω3≤YawMaxRate (3.11) Chapter 4 Motion Control In this chapter, the system’s motion control is the case of study. Initially, control laws for path following are derived for a simple case of a unicycle and then for a quadcopter. Lastly, the control law that keeps the agents in formation is presented, which is the main goal of this dissertation. To easily understand the flocking behavior of a system and how the toolkit used in this dissertation works, a basic 2D example of a unicycle flock is first presented, and later by doing it for our, more complex, in 3D, quadcopter flock. 4.1 Path Following When we talk about motion control, trajectory tracking and path following are many times misunderstood. Path-following problems are concerned with the design of control laws that drive the vehicle to a geometric path parameterized in space, while simultaneously satisfying the dynamic specification of the parametric variable, unlike trajectory tracking. This variable is introduced in our system as a new control input, called γ, and can be seen as a virtual point traveling along the path. In the next two study cases, the problem is approached in two parts: the tracking error problem and the dynamic of the path parametric variable. 4.1.1 Unicycle 4.1.1.1 Path Following Problem Statement Consider our unicycle that is described by the kinematic model introduced in 2.2.2. Let pd(γ):R→R2, be our desired path. A controller must be designed such that the position of the unicycle converges to the desired one, that is, t→∞⇒p(γ)→pd(γ)(4.1) 23 24 Motion Control In other words, when t tends to infinite, the following error must tend to zero, as shown next: e=RT(p(γ)−pd(γ)) →0 (4.2) To make our vehicle follow the path with the desired dynamic, we provide the following requirement for the parameterization variable. Let vd∈Rbe our desired speed assignment along the path. A second controller’s goal is to make the drone’s parameterization speed meet vd, that is, t→∞⇒˙ γ→vd(4.3) It also means that the speed’s error must converge to zero over time. z=˙ γ−vd→0 (4.4) 4.1.1.2 Proposed Solution The path following tasks are formally introduced. Design a control law for uand ¨ γsuch that the two conditions mentioned above are satisfied. Consider the same kinematic model described in 2.2.2. The error dynamic is given by e=RT(p−pd(γ)) (4.5) ˙e=−sk(ω)e−RT˙ γ˙pd(γ)+∆u(4.6) With ∆and sk(ω)as follows ∆="1−ε2 0ε1#(4.7) sk(ω) = "0−ω ω0#(4.8) And the parametric variable error as exposed next z=˙ γ−vd(4.9) ˙z=¨ γ(4.10) It is now possible to define a Lyapunov Function composed by a Lyapunov function to the tracking error problem and another one to the path speed error: Vc=Ve+Vz(4.11) 4.1 Path Following 25 With the tracking error function: Ve=1 2eTe(4.12) ˙ Ve=eT˙e=eT(−sk(ω)e−RT˙ γ˙pd(γ)+∆u)(4.13) and Vz=1 2z2(4.14) ˙ Vz=z˙z=z¨ γ(4.15) Back to the Lyapunov composite function. Vc=1 2eTe+1 2z2(4.16) Then, its derivative with respect to time is ˙ Vc=˙ Ve+˙ Vz=eT(−sk(ω)e−RT˙ γ˙pd(γ)+u+z¨ γ(4.17) With ˙ γ=z+vd, ˙ Vc=eT(−sk(ω)e−RT(z+vd(γ)) ˙pd(γ)+u+z¨ γ ˙ Vc=eT(−sk(ω)e−RTvd(γ)˙pd(γ)+u+z(¨ γ+eRT˙pd(γ)) (4.18) To ensure stability, both terms of ˙ Vcmust be negative definite, what happens for the uand ¨ γ presented below. u=∆−1(RTvd(γ)˙pd(γ)−kee(γ)) (4.19) ¨ γ=eRT˙pd(γ)−kγz(4.20) with keand kγpositive values. Control laws to make our unicycle following a path were found. Simulation results can be found in chapter 5. 4.1.2 Quadcopter 4.1.2.1 Path Following Problem Statement Moving now to a 3 dimension path following example, both path tracking and path following objectives are the same as in the unicycle case. The position error, that has now three coordinates instead of only two, and the speed error must converge to zero over time. t→∞⇒p(γ)→pd(γ)(4.21) t→∞⇒˙ γ→vd(γ)(4.22) 26 Motion Control Error equations: e=RT(p−pd(γ)) (4.23) z=˙ γ−vd(γ)→0 (4.24) 4.1.2.2 Proposed Solution To solve this problem, a Lyapunov approach is used, as done in the unicycle example. Our position dynamic error is given by ˙e=−sk(ω)e+v−R0pd(γ)˙ γ(4.25) From the speed error equation, ˙ γ=z−vd(γ), by replacing ˙ γin the previous expression we get ˙e=−sk(ω)e+v−R0˙pd(γ)vd−R0˙pd(γ)z(4.26) A candidate Lyapunov function is now proposed. V1=1 2eTe(4.27) Its derivative is ˙ V1=eT˙e=eT(−sk(ω)e+v−R0˙pd(γ)vd−R0˙pd(γ)z) ˙ V1=eT(v−R0˙pd(γ)vd)−eTR0˙pd(γ)z (4.28) In equation (4.25), vis seen as a virtual input used to ensure that the derivative of the Lyapunov function is negative-definite, which it happens for v=RT˙pd(γ)vd−kee,ke>0,(4.29) Since vis a virtual input, we call vd2the desired control law for vdin equation (4.26) The goal is now to make vto converge to vd2. To this end, a new error variable, r1, is defined r1=v−vd2=v−R0˙pd(γ)vd+kee(4.30) ⇔v=r1+R0˙pd(γ)vd+kee(4.31) Replacing vin equation (4.28) ˙ V1=−keeTe+eTr1−eT(RT˙pd(γ))z(4.32) The first and last term of ˙v1are negative, but the second one is not. We must then make r1 converge to 0 and for that backstepping technics are used. 4.1 Path Following 27 Backstepping for r1: ˙r1=˙v−˙vd2 ˙r1=S(r1)ω+   0 0 −1/m   u1−RT¨pd(γ)+ke(v−RT˙pd(γ)vd2)−RT˙pd(γ)z (4.33) The new error variable r1can not always be driven to zero. To derive an expression for the virtual control inputs a matrix of values must be inverted, what happens only if it is made full rank by driving the error variable r1to a constant design vector δ. A new error variable Φis then defined and, after that, a new, composite, Lyapunov function created. Φ=r1−δ(4.34) V2=V1+1 2ΦTΦ =1 2eTe+1 2ΦTΦ (4.35) Taking the time derivative of V2, we get ˙ V2=˙ V1+ΦTΦ =−kceTe+eTδ+Φ(B(.)ζ−RT¨pd(γ)+ke(v−RT˙pd(γ)vd2)+e) −(ΦT+eT)(RT˙pd(γ))z (4.36) B(.)ζ=     0. . . 0 −δ3δ2 0. . .δ30−δ1 −1 m . . .−δ2δ10            u1 ω1 ω2 ω3       (4.37) More details on the backstepping derivation in equation (4.33) and on the derivation above in [12]. Note that ζis our control input vector used to enforce ˙ V2to be negative. To do that, a proper expression for ζis presented in equation (4.38). ζ=BT(BBT)−1(−e−RT¨pd(γ)+ ke(v−RT˙pd(γ)vd2)−kΦΦ),kφ>0 (4.38) To get the control law for the path parametric variable, the existing Lyapunov function is modified to equation (4.39). V3=V1+V2+1 2zTz(4.39) 28 Motion Control With its derivative ˙ V3=˙ V1+˙ V2+rγ(¨ γ−vd)(4.40) ˙ V3=−keeTe+eTδ+Φ(B(.)ζ−RT¨pd(γ)+ke(v−RT˙pd(γ)vd2)+e) −z(RT˙pd(γ)(ΦT+eT)−¨ γ)(4.41) Lastly, an expression for ¨ γis derived to make ˙ V3negative definite, as follows ¨ γ=RT˙pd(γ)(ΦT+eT)−kγz(4.42) where kγis a positive constant. The equations (4.38) and (4.42) are the control laws necessary to make our quadcopter follow the desired path. 4.2 Cooperative Path Following Let N={1,2,...,n}be the set that represents a fleet with n vehicles, and Ni={1,2,...,m}be the set of the ith vehicle’s neighbors, with m as the maximum number of elements of this set. Assume that each vehicle converges to its respective virtual target point, and that this virtual point travels along the desired path at the desired speed Vd. If all vehicles reach the virtual target point, which is, in another words, all γhave the same value, consensus is achieved and they are in formation. In the previous chapter, control laws for path following were derived. These laws make the vehicle follow a desired path parameterized by γ, and its value converge to Vdover time. In order to make each vehicle’s γhave the same value, we must adjust the value of Vdaccordingly. Assume that each vehicle i∈Nhas access to the variables γiand γj,j∈Ni. A control law for Vdmust be derived, such that, (γi−γj)→0 as t→∞. The desired speed for each vehicle of the fleet is then described by Vd_i=Vf ormation +Vcorrection (4.43) Vf ormation is the desired flock speed, and is defined when the controller is initialized. In subchapter 2.2.4, the agreement protocol was presented, as well as equation (2.12), that represents the dynamic of a node’s state. With γibeing the state we want to be controlled, Vcorrection is defined by the following equation. Vcorrection =1 m∑ j∈Ni (γj(t)−γi(t)) (4.44) where 1 mis a gain. In practice, it calculates the average weight that the correction speed must have in each of the vehicles. 4.2 Cooperative Path Following 29 Equations (4.43) and (4.44) are the control laws that solve our cooperative path following problem. 30 Motion Control Chapter 5 Simulation Results The objective of this chapter is to show the results of the simulations done in Matlab using a toolkit called Virtual Arena. The goal of the first simulation is to show us the proposed controller working on a 2D case scenario. In the second simulation, the path following and coordination capabilities of our drone flock are demonstrated in a 3D environment. For each simulation, the desired and actual position will be compared, the values of ˙ γ, the velocities and the absolute position error will be shown and analyzed. All simulations used a fixed sample time interval of 1ms. The desired speed was equal to 1 for the single-agent scenarios, and 0.75 for the flock scenarios. The desired trajectories will be described in each subchapter but are all based on circumferences parallel to the ground. 5.1 Unicycle Flock As it has been done throughout this document, the simulations’ phase starts with the simplest example: a unicycle flock. More concretely, by the simulation of the path following controller and lately the cooperative path following one. In chapter 3, it was told that our vehicles have limitations. In the unicycle case, the limitations are −2m/s≤v≤2m/s(5.1) −1.57rad/s≤ω≤1.57rad/s(5.2) For both cases, ε=h−0.5 0i. 31 38 Simulation Results required in equation (4.3). By looking at those pictures we can conclude that our system achieves the desired flock speed. 5.1.2.5 Simulation Results - Configuration 2 The second simulation for the unicycle flock scenario has some communication limitations as explained in subchapter 5.1.2.3. Each unicycle only knows the γvalue of its direct neighbors. Figure 5.8: Actual (circles) and desired (line) positions of the unicycle flock (m) - configuration 2 The results of the simulation with configuration 2 are quite similar to the one with the network in configuration 1. For the sake of avoiding repetitions, only the most notable differences are commented. As in the first scenario, all vehicles go to the desired trajectories over time. However, trajectories are not the same as before. 5.1 Unicycle Flock 39 (a) Absolute Position Errors (m) (b) Linear velocity (m/s) and angle (rad) of the unicycles Figure 5.9: Simulation results for unicycle’s cooperative path following controller - configuration 2 The difference in unicycle’s positioning error is easily understandable by looking at the absolute errors’ value, figure 5.9 a). Compared to the last scenario, the wider is the drone’s path, or in other words, the higher is its number, the biggest is the delay until it reaches zero error. It is easily noticeable in the green vehicle absolute error line where the time it takes to reach zero is more than one second that it was before. It happens because the dynamic of each agent is now depending on only one, or two, neighbors. It is expected that unicycles at the ends of the formation take more time until reach the formation desired speed. (a) Value of all ˙ γ(b) Value of all γ Figure 5.10: Values of ˙ γand γof all agents of the network - configuration 2 In figure 5.9 b), can be noticed that changes of speed happens later in this configuration than in the one simulated before. The same happens in the intensity of its changes that is smaller 40 Simulation Results than before. It happens because the smaller number of neighbors leads, in this case, to a smaller coordination error and so that minor adjustments than before. Each drone is correcting its point in the path accordingly to only its closest neighbor. This makes the flock take a bit longer to achieve coordination, as can be seen in figure 5.10. 5.2 Drone Flock Simulations of the 3D scenarios are done in this subchapter. Again, it starts with the simulation of the path following controller and then with the cooperative path following one. For both simulations, the drones’ mass is 650 grams and the gravity acceleration is equal to 9.81m/s. The following expressions describe the input limitations of the drone that were imposed. 0≤Thrust ≤20m/s(5.11) −1.57rad/s≤ω1≤1.57rad/s(5.12) −1.57rad/s≤ω2≤1.57rad/s(5.13) −1.57rad/s≤ω3≤1.57rad/s(5.14) For both cases, ε=h0 0 0.5i. 5.2.1 Path Following 5.2.1.1 Desired path The desired path for this example is a circumference with a 10m radius centered in origin, 5 meters high off the ground. xd=10sin(γ)(5.15) yd=10cos(γ)(5.16) zd=5 (5.17) During all flights, the controllers were instructed to keep the drones all pointing to the same starting direction, which means keep the yaw angle always at 0 rads. 5.2.1.2 Initial Conditions and Gains The vehicle’s starting position is the origin of the referential, and its orientation starts in 0 rads. The gains for the controller have the following values. ke=5 kφ=0.1 kψ=5 kγ=10 5.2 Drone Flock 41 5.2.1.3 Simulation Results (a) 3D View (b) Top View (c) Front View Figure 5.11: Actual (circles) and desired (line) positions of the drone (m) 42 Simulation Results As figure 5.11 shows, the controller presented to the 3D path following problem works as it should. The drone starts the simulation in the origin and drives itself to the desired path until its absolute position error gets almost zero, which can be verified in the next figure. Figure 5.12: Absolute position error of the drone (m) Regarding its speed behavior, we must look to the plots of ˙ γ. If this value leads to the desired speed, then our drone is following the speed assignment as desired. In this simulation, the desired speed value is constant and equal to 1m/s. After checking figure 5.13 a), we verify that ˙ γtends to the given desired speed and the path following task is correctly done by our controller. (a) Value of ˙ γ(b) Roll and pitch angles of the drone Figure 5.13: Values of ˙ γ, roll and pitch of all agents of the network In figure 5.13 b) are presented the roll and pitch angle values from a few seconds before the drone reaches the desired path. The desired yaw angle for all simulations is 0 rads, so, by looking at its roll and pitch angles we can find some characteristic points in its path, more precisely, when 5.2 Drone Flock 43 drones are in a point of the path where the tangent to it is parallel to the x-axis or perpendicular to the y-axis. When body’s x-axis is parallel to the inertial x-axis, it is only using pitch to move forward or backward, depending if its y coordinate is positive or negative. It happens for time around 44.5 seconds, where the roll angle is equal to 0. Accordingly to what was said above, this point must be a local maximum (or minimum) for the pitch angle value, which is verified. The opposite happens for t=60s, when the drone has its y-axis parallel to inertial’s y-axis. 5.2.2 Cooperative Path Following Now we get to the main goal of this dissertation. A cooperative path following controller to manage a flying drone flock. The flock used in this simulation has 6 drones (N = 6). As did in the unicycle example, first we tested the path following ability of our controller then we add the coordination controller to the equation. 5.2.2.1 Desired path For this simulation, a fleet of 6 drones was used. The desired trajectories are divided into two height levels, 5 and 10 meters, and in both levels, the trajectories are circumferences centered in origin and with 20, 25 and 30 meters radius. These trajectories are detailed next: xd(n) = ((20 +(5(n−1)))sin(γ),1≤n≤3 (20 +(5(n−4)))sin(γ),4≤n≤6(5.18) yd(n) = ((20 +(5(n−1)))cos(γ),1≤n≤3 (20 +(5(n−4)))cos(γ),4≤n≤6(5.19) zd(n) = (5,1≤n≤3 10,4≤n≤6(5.20) 5.2.2.2 Initial Conditions and Gains The drones started on the ground (z=0), three on them in front, aligned with the desired path, and the remaining three, 5 meters beyond them. More precisely, x0(n) = (0,1≤n≤3 5,4≤n≤6(5.21) y0(n) = (20 +5(n−1),1≤n≤3 20 +5(n−4),4≤n≤6(5.22) The gains have the following values. ke=5 kφ=0.1 44 Simulation Results kψ=5 kγ=10 5.2.2.3 Network Configurations For the unicycle flock simulation, two different network configurations were used. In the first configuration, all agents can change information inside the network. Then, for the second simulation, each vehicle can only communicate with its closest neighbor. (a) Configuration 1 (b) Configuration 2 Figure 5.14: Diferent network configuration for the drones simulations            011111 101111 110111 111011 111101 111110                       010000 101000 010001 000010 000101 001010            (5.23) Both configurations are represented in the graphs above. The respective adjacency matrices are presented right after that. 5.2.2.4 Simulation Results - Configuration 1 The simulation of a flock in the first configuration is here presented. Figure 5.15 illustrates the behaviour of the flock in this simulation. In figure 5.15 a), the drones reached the desired trajetory and are proper alligned as desired. Figures 5.15 b) and c) , are the top and front views, respetively, of their trajectories. 5.2 Drone Flock 45 (a) 3D view (b) Top view (c) Front view Figure 5.15: Actual (circles) and desired (line) positions of the drone flock (m) - configuration 1 To confirm the previous statement, the absolute position error is presented in figure 5.16. As 46 Simulation Results observed in the previous figures, The value of the absolute position error converges and remains closer to zero over time. Figure 5.16: Absolute position errors (m) - configuration 1 Looking to γand ˙ γvalues, we can see that consensus has been reached. All γconverged to the same value, as well its speed, that ended up around 0.75, as required. The drones from the top trajectories had their γvalue lower than the drones at the beggining, what is expected once they take more time to reach a farthest path. This extra effort to reach the formation desired speed is reflected in the ˙ γvalues that were changing more abruptly. (a) Values of ˙ γ(b) Values of γ Figure 5.17: Values of ˙ γand γof all agents of the network - configuration 1 5.2 Drone Flock 47 5.2.2.5 Simulation Results - Configuration 2 The second simulation had a few changes in the way the communication network was set up. As explained in subchapter 5.2.2.3, for this simulation drones can only change information with only those drones that are right next to them, in the same level. The only drones that can communicate between levels are the ones at the wider path, 3 and 6, accordingly to figure 5.14 b). (a) 3D view (b) Top view (c) front view Figure 5.18: Actual (circles) and desired (line) positions of the drones (m) - configuration 2