Corso di Laurea Magistrale in Aeronautical Engineering
Anti-windup design for position and attitude
control of quadrotor UAVs
Advisor: Prof. Marco LOVERA Co-Advisor: Dr. Davide INVERNIZZI
Thesis by:
Pietro GHIGNONI Matr. 898764
Arrivato a questo importante traguardo, non posso non ringraziare tutti i profes-sori che in questo percorso mi hanno insegnato ad apprendere, considerandomi come un vaso da innaffiare piuttosto che un secchio da riempire. Un ringrazia-mento doveroso va in particolare al prof. Marco Lovera e a Davide Invernizzi per avermi messo nella situazione di poter lavorare con passione, determinazione e serenit`a a questa tesi. Un ringraziamento va anche a Simone Panza per avermi aiutato durante tutta la fase di sperimentazione. Desidero ringraziare l’Opera Segapeli, il Lascito Cassarotti e la Fondazione Monti Torrioli per aver contribuito con il loro importantissimo gesto a rendere possibile questo percorso. Ringrazio la Maestra Giuliana per aver messo la parola inizio a questa esperienza, avendomi motivato pi`u volte a percorrere questa strada. Un grazie a tutti gli amici di una vita del campino, in particolare a Michele per avermi spesso sopportato in questi cinque anni. Un ringraziamento sentito va inoltre a tutti gli amici con i quali ho condiviso dei momenti belli al Politecnico o altrove, in particolare ad Alessan-dro e Nicol`o per avermi sempre dato il vostro supporto e preziosissimi consigli: credo che senza di voi questo lavoro non sarebbe mai iniziato. Ringrazio infine la mia famiglia: gli zii, le mie cugine, i miei nonni e soprattutto i miei genitori: nonostante le mie lunghe, interminabili assenze e i miei silenzi non smettete mai di farmi ricordare che voi ci siete e ci sarete, sempre.
Nowadays feedback control plays an important role in a great variety of appli-cations such as robotics, automotive, aeronautics, space, chemical and industrial processes, etc. Its main purpose is to ensure satisfactory performance levels in the whole set of operating conditions the system is supposed to work in. Unfortu-nately, when dealing with control input saturation, a worsening of the closed-loop properties is experienced. This thesis focuses on the development of appropri-ate control laws to reduce this saturation-induced malfunctioning for quadrotor unmanned aerial vehicles (UAVs) in presence of either saturating propellers or attitude constraints. Following the anti-windup compensation approach, in which the compensator acts on top of a baseline controller, a linear matrix inequality (LMI) based synthesis technique for discrete-time linear systems is developed to guarantee prescribed levels of stability and performance in presence of limited control authority. This synthesis method is then used to modify well consolidated quadrotor flight control laws, by tuning fixed-dynamics anti-windup compensators to improve either position or attitude tracking performance. The benefits of the augmentation are evaluated with flight test experiments made in the Flying Arena for Rotorcraft Technologies (FlyART) of Aerospace Systems and Control Labora-tory (ASCL) of Politecnico di Milano by comparing the performance of baseline and augmented controllers in real case scenarios.
Il controllo in retroazione assume un ruolo molto importante in svariate appli-cazioni di tipo aeronautico, automobilistico, spaziale, robotico, chimico, industri-ale, etc. Il suo principale scopo consiste nel garantire adeguate prestazioni in varie condizioni operative nelle quali il sistema in studio si trova ad operare. Purtroppo la presenza di saturazioni delle variabili di controllo comporta un peggioramento delle propriet`a del sistema in anello chiuso. Questa tesi ha lo scopo di sviluppare opportune leggi di compensazione al fine di ridurre il malfunzionamento indotto dalle saturazioni in aeromobili a pilotaggio remoto soggetti sia a limitazioni fisiche del sistema propulsivo e a vincoli di assetto. Seguendo l’approccio di compen-sazione anti-windup, secondo il quale si migliora la legge di controllo esistente tramite l’aggiunta di un compensatore, `e stato sviluppato un metodo di sintesi per sistemi lineari a tempo discreto basato su disuguaglianze lineari matriciali con lo scopo principale di garantire determinate prestazioni in presenza di limitazioni negli ingressi del sistema. Il metodo di sintesi `e stato poi utilizzato per modificare il controllore di un quadricottero; in particolare `e stato progettato un compen-satore a dinamica fissata per migliorare le risposte in anello chiuso d’assetto e posizione. Paragonando le prestazioni ottenute con legge di controllo originale e con compensatore, le prove di volo, svolte nella Flying Arena for Rotorcraft Technologies (FlyART) del Politecnico di Milano, hanno permesso di confermare l’efficacia delle modifiche.
Acknowledgments I
Abstract III
Sommario V
List of figures IX
List of tables XIII
Introduction 1
1 The anti-windup problem 5
1.1 The windup effect . . . 5
1.2 Modern approach to anti-windup compensation design . . . 8
1.2.1 Overview of modern anti-windup techniques: objectives and limitations . . . 8
1.2.2 Direct linear anti-windup . . . 11
1.2.3 Model recovery anti-windup . . . 15
1.2.4 Weston and Postlethwaite framework . . . 19
1.3 Comparative analysis of compensation strategies . . . 21
2 Quadrotor modelling and control 23 2.1 Quadrotor kinematics and dynamics . . . 23
2.1.1 Reference frames . . . 23
2.1.2 Attitude parametrisation . . . 25
2.1.3 Quadrotor dynamics . . . 28
2.2 Control system architecture . . . 32
2.3 Control allocation . . . 36
2.4 Motion decoupling . . . 37
3 Performance-oriented anti-windup compensation 41
3.1 Preliminaries of anti-windup augmentation . . . 41
3.2 A practical approach to performance-oriented anti-windup synthesis 45 3.3 Full MIMO and decentralised compensation . . . 54
4 Simulation and experimental results 59 4.1 Experimental equipment and setup . . . 59
4.2 CS1: Anti-windup compensator design for saturated attitude control 62 4.3 CS2: Anti-windup compensator design for pitch-limited position control . . . 69
4.4 CS3: Anti-windup compensator design for directionality effects . . 79
Conclusions 89 A Classic anti-windup methods 99 A.1 Reset anti-windup and conditional integration . . . 99
A.2 Conventional anti-windup . . . 100
A.3 Hanus conditioning technique . . . 101
A.4 Observer based anti-windup . . . 101
B Nonlinear systems 103 B.1 Stability sets . . . 103
B.2 Norms and gains . . . 104
B.3 Absolute stability . . . 105
C Optimization tools 109 C.1 Practical issues . . . 112
1.1 Scalar symmetric saturation function. . . 6
1.2 Example of integral windup. . . 7
1.3 Unconstrained closed-loop system. . . 8
1.4 Saturated closed-loop system. . . 9
1.5 Augmented closed-loop system. . . 9
1.6 DLAW compensation architectures. . . 13
1.7 The anti-windup closed-loop system (MRAW). . . 16
1.8 The MRAW separation principle. . . 18
1.9 Weston and Postlethwaite linear conditioning. . . 19
1.10 Weston and Postlethwaite linear conditioning: separation principle. 20 2.1 Inertial and body frames. . . 24
2.2 Body axes convention; bottom view. . . 24
2.3 Flight controller main modules. . . 32
2.4 Position controller architecture. . . 33
2.5 Linearised attitude control law. . . 35
2.6 Overall control architecture. . . 35
2.7 In-plane forces generation. . . 39
3.1 The unconstrained closed-loop interconnection. . . 42
3.2 The augmented closed-loop interconnection. . . 44
3.3 Linear fractional transformation of the interconnected systems. . . 45
3.4 Block diagram for performance-oriented synthesis. . . 47
3.5 Full MIMO compensation. . . 54
3.6 Decentralised compensation. . . 55
4.1 The FlyART indoor arena (source [1]). . . 60
4.2 The ANT-1 mini UAV (source [1]). . . 60
4.3 Pixfalcon FCU (source [2]). . . 61
4.4 NanoPi NEO Air companion (source [3]). . . 61
4.5 Vertical dynamics frequency response function: from ˆT to vd. . . . 62
4.6 Roll dynamics frequency response function: from ˆLp to p. . . 63
4.8 Yaw dynamics frequency response function: from ˆNp to r. . . 64
4.9 CS1; altitude mode augmented attitude controller (pitch channel). 67 4.10 CS1; simulated unconstrained pitch: excitation amplitude ±20deg. 67 4.11 CS1; simulated pitch with baseline controller: excitation amplitude ±20deg. . . 68
4.12 CS1: simulated pitch with AW augmentation: excitation amplitude ±20deg. . . 68
4.13 Measured pitch with baseline controller: excitation amplitude±20deg. 69 4.14 CS1; measured pitch with AW augmentation: excitation amplitude ±20deg. . . 70
4.15 CS1; measured pitch with baseline controller: excitation amplitude ±30deg. . . 70
4.16 CS1 measured pitch with AW augmentation: excitation amplitude ±30deg. . . 71
4.17 Geometric meaning of the planner. . . 71
4.18 CS2: plant and controller definitions. . . 73
4.19 CS2; augmented position controller. . . 73
4.20 CS2; measured position time history: baseline controller. . . 74
4.21 CS2; measured position time history: augmented controller. . . 74
4.22 CS2; measured pitch angle time history: baseline controller. . . 75
4.23 CS2; measured pitch angle time history: augmented controller. . . 75
4.24 CS2; simulated position time history: baseline controller. . . 76
4.25 CS2; simulated position time history: augmented controller. . . . 77
4.26 Simulated pitch angle time history: baseline controller. . . 77
4.27 CS2; simulated pitch angle time history: augmented controller. . . 78
4.28 CS2; simulated position time history: unconstrained system. . . . 78
4.29 CS2; mismatch estimates in off-design condition. . . 79
4.30 CS3; control system implementation. . . 81
4.31 CS3: implementation of the AW correction. . . 82
4.32 CS3: position controller augmentation. . . 82
4.33 CS3: attitude controller augmentation. . . 83
4.34 CS3; position time history: simulations. . . 84
4.35 Attitude time history: simulations. . . 85
4.36 CS3: position time history: experiments. . . 85
4.37 CS3: attitude time history: experiments. . . 86
4.38 CS3: motor throttles time history. . . 87
A.1 Reset based anti-windup. . . 99
A.2 Conventional anti-windup. . . 100
A.3 Hanus conditioning technique. . . 102
A.4 Observer based anti-windup. . . 102
4.1 ANT-1 main parameters (source [4]). . . 59 4.2 ANT-1 position controller gains. . . 64 4.3 ANT-1 attitude controller gains. . . 64
The presence of saturation in the control variables is one of the hardest challenges encountered in automatic control. Any physical system is subject to intrinsic lim-itations: for instance, the control surfaces of an aircraft cannot be deflected more than their mechanical limit or an engine cannot deliver more than a maximum amount of thrust. This ubiquitous and highly nonlinear phenomenon may ruin the performance of a closed-loop system from degraded transients to instability. Just to make few examples of catastrophic events, as reported in [5], actuator saturation has been involved in various aircraft crashes and the meltdown of the Chernobyl power station. In this setting, while the word windup is used to de-note this saturation induced-malfunctioning, the term anti-windup (AW) refers to any solution aiming at reducing as much as possible windup effects. Although most of the complications related to the limited control authority are generally underestimated, three possible remedies, if ever, can be considered:
• actuators oversizing. In this case the actuators of the control system are designed such that saturation bounds are never exceeded. It is worth noting that this practice could be particularly disadvantageous both in terms of cost and system efficiency. Most of the times this leads to the increment of actuator masses, making this solution unacceptable in many cases such as aerospace applications, for which the principle the lighter the better holds. • Saturation handling. In this approach the degradation of performance due
to saturation is faced by using appropriate control laws. In particular, this method adopts a single step approach to controller synthesis, in the sense that the controller is designed by taking into account both performance objectives and bounded input constraints and the same time. This usually leads to complex and highly nonlinear controllers. Despite of its appealing theoretical perspective, most of the times simpler solutions are preferred. • Saturation compensation. Even in this case, windup effects are counteracted
by relying on appropriate control laws. Differently from the saturation han-dling method, this strategy uses a two step approach to controller synthesis and it is referred as anti-windup compensation. The first step consists in designing a nominal control law as if no control bounds were present, while in the second step corrective actions for input limitations are introduced.
As a consequence of this separation, well consolidated control schemes can be used to ensure nominal performance, while addressing saturation-related issues separately and with minor changes to the nominal controller. Given its simple working principle, anti-windup compensation represents the most widespread solution to mitigate windup effects.
In the last two decades, anti-windup compensation has been studied intensely and many contributions have enriched the existing literature (see, e.g., [6, 7]), leading to the development of systematic synthesis tools to efficiently face the problem of input saturation while guaranteeing the desired levels of stability and performance. As a results, the term modern anti-windup augmentation has been introduced to denote this systematic methodology to face the issue of windup. De-spite the large amount of works present in the literature, saturation compensation has been rarely addressed in aerospace control applications. The main interest of this thesis is to make a contribution in this field by focusing on anti-windup compensation schemes for flight control, with particular attention to quadrotor UAV platforms. This class of vehicles is receiving great attention both in civil and military applications such as aerial surveillance and inspections, search and res-cue, precision agriculture, photography, entertainment, etc. Quadrotors are flying machines composed of four arms, each one carrying a propeller; unlike conven-tional aircraft configurations, their manoeuvrability is provided by the propulsive system, thanks to which attitude and position can be changed. Given the actual open-loop unstable nature of the system, feedback control is necessary to per-form any kind of mission. In this sense, modern autopilots feature different flight modes, making the job of the pilot easier; unfortunately, when operating in partic-ular conditions such as heavy payload transportation, manoeuvred flight or limited flight envelope, the presence of saturation in the control actions (e.g., propeller limits) degrades nominal closed-loop properties, by requiring proper anti-windup compensation. So far these issues have been addressed in very few contributions such as [8, 9, 10, 11, 12, 13], in which the problem of anti-windup compensation has been formulated only looking at attitude-altitude stabilisation. However, the control architecture of a quadrotor is composed of multiple cascaded loops and the aforementioned schemes give no guarantee of performance and stability when looking at high level objectives such as position control. In this sense, the con-tribution of this thesis is twofold. Firstly an intuitive and systematic synthesis algorithm for anti-windup design is derived along the lines of existing compen-sation methods; one of the advantages of this new tool of synthesis is to reduce the conservativity of the usual approaches such that improved closed-loop perfor-mance can be obtained. Secondly anti-windup compensation schemes are devel-oped to enhance quadrotors pose (position plus attitude) tracking performance in presence of saturation. In particular, after improving the attitude scheme of [12] in terms of performance guarantees, anti-windup compensation is extended to quadrotor position control. In the design phase, particular attention is given
to the structure of the compensator, by keeping the computational cost as small as possible without significant performance degradation. The effectiveness of the laws is demonstrated with dedicated flight experiments. These topics are reflected in the structure of the thesis, which is summarised in the following:
• in Chapter 1 the concept of actuator saturation is formalised from a math-ematical point of view and windup effects are explained with a simple aca-demic example. Moreover a definition of the anti-windup augmentation objectives is given and the fundamental limitations are recalled. In the final part of the chapter an overview of the state of the art is presented and pro et contra of the different augmentation schemes are discussed.
• In Chapter 2 the basic necessary notions for quadrotor modelling and control are presented. After the derivation of the attitude parametrisations used in flight dynamics, the complete equations of motion, the structure of the control system and saturation-induced effects are discussed.
• In Chapter 3 the main theoretical result of this thesis is illustrated. In particular this chapter contains the derivation of a synthesis procedure for the construction of an anti-windup compensator aiming at recovering the ideal (i.e., with no saturation) system behaviour. Moreover, a modification of this general result for quadrotor-like systems is presented.
• In Chapter 4 the compensation schemes are applied to a real quadrotor. Af-ter a description of the experimental system, three different case studies are investigated: augmentation for single-axis attitude control, augmentation for pitch-limited position control and augmentation for directionality effects in position control.
The anti-windup problem
The goal of this chapter is to provide a review of the state of the art concerning anti-windup. The topics covered herein are meant to provide just the basic ideas of anti-windup compensation; for a detailed analysis, the reader is referred to [6], from which the general concepts of this chapter are taken from. Before starting with the presentation of the most important paradigms of augmentation, a brief mathematical introduction to the windup effects is given in Section 1.1. Section 1.2 contains the statement of the AW problem and illustrates the main aspects of modern AW schemes, both in terms of limitations and strategies of compensation. Section 1.3 concludes the chapter with a comparative and critical analysis of the possible solutions to the windup issue.
1.1
The windup effect
As stated in the Introduction, one of the main causes of performance degradation of a closed-loop system is represented by the limited capabilities of an actuator. In general these limitations can be translated in magnitude bounds of the control variable and can be modelled by the saturation function (in Figure 1.1); consid-ering a signal u ∈ R and a scalar ¯u ∈ R>0, satu¯(?) : R → [−¯u, ¯u] is defined
as
satu¯(u) = sign(u) min (¯u,|u|) , (1.1)
where ¯u denotes the saturation level and sign(u) : R → {−1, 0, 1} is the sign function defined by the law
sign(u) := −1, if u < 0, 0, if u = 0, 1, if u > 0. (1.2)
In a multivariable setting (i.e., u ∈ Rm), the decentralised saturation function
u satu¯(u) ¯ u −¯u ¯ u −¯u
Figure 1.1: Scalar symmetric saturation function.
satu¯(u) is given by
satu¯(i)(u(i)) = sign(u(i)) min ¯u(i),
u(i)
, ∀i = 1, ..., m, (1.3) where for a generic vector a∈ Rp, a
(i), i = 1, . . . , p denotes its i-th component.
Remark 1. The definitions given in equations (1.1), (1.3) refer to symmetric bounds; the extension for the asymmetric case is straightforward. With abuse of notation, in the following the symbol satu¯(u) will be used to denote either
sym-metric or asymsym-metric saturation.
According to [14], windup generally arises because of the combined effect of input saturation and a control law with integral action; this issue is generally known in the literature as integrator windup. This is very common in widespread PID controllers, whose control action u∈ R is given by
u(t) = Kp yd(t)− y(t) + Ki Z t 0 yd(τ )− y(τ)dτ − Kd˙y(t), (1.4)
where yd ∈ R is the tracking signal, y ∈ R the measurement used for feedback
control and Kp, Ki, Kd ∈ R denote proportional, integral and derivative gain,
re-spectively. The windup phenomenon is qualitatively introduced by considering the following simple example: suppose to have the single-input single-output lin-ear time-invariant system ˙y = u regulated by a PI controller and operating in an equilibrium condition. Consider the situation in which the closed-loop is de-manded to track a very large step reference. In the initial part of the response (see Figure 1.2), given the large tracking error (i.e., yd−y in (1.4)), the demanded
term. In the rising time the error reduces, while its integral increases. Given the limited power in the actuation and the slow response, when the system has reached the new reference condition, the integral action is so large that it keeps the system locked in saturation, causing an undesired overshoot which degrades closed-loop performance. It is very important to notice that the windup effect
0 5 10 15 20 -2 0 2 4 0 1 2 3
Figure 1.2: Example of integral windup.
appears also in cases in which the controller features no integral term (see, e.g., [15]). As a consequence of that, the solutions to the problem which will discussed in the following are tailored to face the windup problem in general and they do not restrict their effectiveness to the particular case of integrator windup.
The first AW schemes appeared in the literature fall within the class of classic AW methods. An overview can be found in Appendix A (see also [13, 10]); as reported in [16], many of these compensation strategies can be considered as ”tricks” and ”fixes” proposed as quick solutions to the windup problem and many of them are mostly based on intuition with no (or little) theoretical foundation. For this reason they are also addressed as ad hoc methods. They clearly lack of [17]:
• quantitative exposition of the objectives of the augmentation; • guarantees of stability and performance;
• systematic methods for compensators synthesis and tuning guidelines. These weaknesses are overcome by modern anti-windup theory, whose fundamen-tals are briefly illustrated in the following section. The background knowledge necessary to face the study of saturated systems in this modern framework is summarized in Appendix B.
1.2
Modern approach to anti-windup
compen-sation design
1.2.1
Overview of modern anti-windup techniques:
objec-tives and limitations
For a rigorous problem definition, an illustration of the general setting and the characterisation of the main terms which will be used throughout this work is required. The starting point is represented by a feedback control system which is supposed to provide satisfactory behaviour in an unconstrained regime. The term unconstrained means that no bound is present in the magnitude of the control variable; most of the times, this ideal condition is also assumed during the phase of controller design. For this reason, the original controller is also addressed as unconstrained controller. Denoted with z ∈ Rnz a performance
index, w ∈ Rnw an exogenous signal (possibly containing reference, disturbance
and measurement noise), y ∈ Rny the measured output and u ∈ Rnu the plant
input, the unconstrained closed-loop is shown in Figure 1.3, in which the block P is used to denote the plant, while C is the controller. As it is clear from
P
C
w z y u wFigure 1.3: Unconstrained closed-loop system.
Section 1.1, the presence of saturation implies a degradation of performance of the real saturated system (in Figure 1.4) with respect to the unconstrained one. When the unconstrained controller of Figure 1.4 is augmented with the anti-windup compensator, the expression of augmented closed-loop system is used. Its corresponding general architecture is shown in Figure 1.5, where the block AW denotes the compensator and v is the compensation signal. The modern AW framework is supposed to operate in this setting. From a qualitative point of view, with such a compensation strategy, the following goals should be fulfilled (see, e.g., [6]):
P
C
sat¯u(?) w z y w uFigure 1.4: Saturated closed-loop system.
P
C
satu¯(?) AW w z y w u v −• small signal preservation: when the demanded control input is bounded within the limits of saturation, the response of the augmented system must match the unconstrained one;
• asymptotic stability: in absence of exogenous inputs, the operating point must be asymptotically stable with a basin of attraction containing all the states the system is supposed to move to during transients;
• input-output stability: the response of the augmented closed-loop must be bounded for initial conditions and exogenous signals expected during oper-ations;
• unconstrained response recovery: when it is possible, the augmented system response should asymptotically converge to the unconstrained one.
As it has been stated in [16], anti-windup synthesis can be formulated as a bounded stabilisation problem, with some constraints on small and medium signal be-haviour. As a consequence of that, before facing the AW problem per se, it is very important to recall some results regarding bounded stabilisation, such that all fundamental limitations are accounted for. Focusing this analysis to linear plants for simplicity, for which the notion of stability can be applied to the system, in [18] it has been proved that in presence of bounded inputs:
• global exponential stabilisation is only possible for exponentially stable open-loop plants;
• non-exponentially unstable plants (this includes also marginally stable/unstable plants) can be globally asymptotically stabilised;
• for exponentially unstable plants the null-controllability region (see Defini-tion B.1.2) is bounded and global results are not achievable.
From a practical point of view, the explanation of those limitations is the following: when the control input saturates, closed-loop properties are lost since the input to the plant assumes a constant value. In this condition, system dynamics is characterised by the open-loop dynamics of the plant, i.e., by the eigenvalues of its dynamics. In case of exponentially unstable modes along with control demands exceeding far enough actuation physical bounds, stability is lost; as a consequence of that, the worst case behaviour is determined by the plant and no compensation can improve performance in such cases. Equivalently in [16] the following intuition based mathematical formalisation has been proposed: the effect of the saturation can be thought as described by the following nonlinear law
satu¯(u) = gu¯(u)u, (1.5)
where gu¯(?) : R → (0, 1] is an equivalent gain, whose value is equal to 1 if the
As a matter of fact, when gu¯(u) → 0 better results than open-loop properties
cannot be achieved.
Because of these limitations, it is possible to distinguish between two different approaches to the problem:
• global: in this case results hold globally both in terms of stability and per-formance, no matter how large the external perturbations; in particular the outcomes are the following:
1. global asymptotic (or even exponential) stability is obtained;
2. an upper bound on the L2 gain (see Appendix B for its definition) is
ensured;
• regional: in this case, closed-loop properties hold only for a given subset of external perturbations and initial conditions; the focus of this analysis generally reflects into:
1. providing guarantees on the (nonlinear) L2 gain curve (see Appendix
B for its definition);
2. providing an estimate of the reachable set for bounded inputs; 3. providing an estimate of the domain of attraction.
The open-loop behaviour of the plant characterises the kind of achievable guaran-tees (local/global) and the kind of applicable analysis. In particular, by combining all the information gathered from the input bounded stabilisation framework, the following constraints have to be considered in the anti-windup problem:
• global exponential stability can be achieved only with exponentially stable plants;
• global asymptotic stability can be obtained only with non-exponentially un-stable plants; for marginally un-stable/unun-stable plants nonlinear compensation is required;
• regional asymptotic stability can be achieved with any kind of plant. In the following sections, three different approaches to modern anti-windup syn-thesis will be presented: Direct Linear Anti-Windup (DLAW), Model Recovery Anti-Windup (MRAW) and the Weston and Postlethwaite framework.
1.2.2
Direct linear anti-windup
Making reference to the general setting introduced in Section 1.2.1, direct lin-ear anti-windup methods can be applied only in presence of linlin-ear unconstrained
closed-loop systems; even if this condition poses a strong limitation for the appli-cability of the method, it is worth mentioning that AW compensation for nonlinear systems is generally addressed by using linearised models [11].
DLAW is based on the unified framework proposed in [17] and the first results have appeared in [19]. With this approach, either global or regional problems can be faced (see, e.g., [16, 5]). To state the problem from a quantitative point of view, consider an unconstrained linear time-invariant plant with state space form
˙xp = Apxp+ Bp,uup+ Bp,ww
y = Cp,yxp+ Dp,yuup+ Dp,yww
z = Cp,zxp + Dp,zuup+ Dp,zww,
(1.6)
where xp ∈ Rnp, up ∈ Rnu, w ∈ Rnw, y ∈ Rny, z ∈ Rnz are plant state, control
input, exogenous input, measurement output and performance output, respec-tively, and state space matrices Ap ∈ Rnp×np, Bp,u ∈ Rnp×nu, Bp,w ∈ Rnp×nw,
Cp,y ∈ Rny×np, Cp,z ∈ Rnz×np, Dp,yu ∈ Rny×nu, Dp,zu ∈ Rnz×nu, Dp,yw ∈ Rny×nw,
Dp,zw ∈ Rnz×nw. For the plant in (1.6), the unconstrained controller with state
space representation (
˙xc= Acxc+ Bc,yy + Bc,ww
u = Ccxc+ Dc,yy + Dc,ww
(1.7)
has been designed, where xc ∈ Rnc is the controller state and Ac ∈ Rnc×nc,
Bc,y ∈ Rnc×ny, Bc,w ∈ Rnc×nw, Cc ∈ Rnu×nc, Dc,y ∈ Rnu×ny, Dc,w ∈ Rnu×nw. If
up = u, then the interconnection of (1.6) and (1.7) reduces to the unconstrained
closed-loop of Figure 1.3, for which the controller (1.7) has been tuned according to existing synthesis techniques for linear systems (i.e., linear quadratic control, pole placement, loop shaping, structured H∞, etc.). However, when taking into account physical bounds, up = satu¯(u) and the interconnection reduces to the saturated
closed-loop of Figure 1.4. In the DLAW framework, the controller in (1.7) is augmented by injecting the outputs of an AW compensator, denoted hereafter as v1 ∈ Rnc and v2 ∈ Rnu in the controller dynamics and output, as follows:
(
˙xc= Acxc+ Bc,yy + Bc,ww + v1
u = Ccxc+ Dc,yy + Dc,ww + v2.
(1.8)
When the signal v1 directly affects the controller states as in (1.8), the
augmen-tation is denoted as full-authority; on the other hand, if the signal v1 is added to
the feedback measurement y, i.e. (
˙xc= Acxc+ Bc,y(y + v1) + Bc,ww
u = Ccxc+ Dc,y(y + v1) + Dc,ww + v2,
(1.9)
P
C
satu¯(?) AW w z y w u v1 − v2(a) Full authority.
P
C
satu¯(?) AW w z y v1 w u − v2 (b) External.Figure 1.6: DLAW compensation architectures.
of which kind of augmentation is used (full-authority or external), in the DLAW framework the compensator is characterised by a linear time-invariant state space description ˙xaw = Aawxaw+ Baw(u− satu¯(u))
v1 = Caw,1xaw+ Daw,1(u− satu¯(u))
v2 = Caw,2xaw+ Daw,2(u− satu¯(u)),
(1.10)
where xaw ∈ Rnaw is the anti-windup state, Aaw ∈ Rnaw×naw, Baw ∈ Rnaw×nu,
Caw,1 ∈ Rnc×naw, Caw,2 ∈ Rnu×naw, Daw,1 ∈ Rnc×nu, Daw,2 ∈ Rnu×nu are the
compensator matrices and naw ∈ N is the compensator order.
Remark 2. In full-authority augmentation, the combination of the expressions of v2 in (1.10) and u in (1.8) leads to the following implicit algebraic loop
u = Ccxc+ Dc,yy + Dc,ww + Caw,2xaw+ Daw,2(u− satu¯(u)). (1.11)
For the external counterpart, the arising implicit equation is
u = Ccxc+Dc,yy+Dc,ww+[Dc,yCaw,1+ Caw,2] xaw+[Dc,yDaw,1+ Daw,2] (u−satu¯(u)).
(1.12) An algebraic loop is well posed1 if it admits a unique solution. In general, all modern anti-windup schemes guarantee that algebraic loops originating from the controller compensator interconnection (i.e., equations (1.11), (1.12)) are well posed.
1In general, an interconnection of two or more dynamical systems is well posed if for any
All matrices in equation (1.10), along with the compensator order naw are free
parameters to be tuned according to a specific optimality criterion. Of course, to ensure small signals preservation matrix Aaw in equation (1.10) must be Hurwitz.
In the literature, there exist constructive synthesis procedures to provide either global or regional performance guarantees. Concerning the global problem, the formulation is the following (see, e.g., [20]).
Problem 1 (Global anti-windup problem). Given the order naw, find matrices of
the compensator (1.10), such that:
• the augmented closed-loop (1.6), (1.8), (1.10) is well posed;
• there exists a scalar ∈ R>0 and a Lyapunov function V (x) = xTP x, x :=
xT
p xTc xTaw
T
ensuring quadratic stability of the augmented closed-loop and L2 gain from w to z smaller than γ, i.e.
˙
V <−xTx− 1
γz
Tz + γwTw, ∀(x, w) 6= 0. (1.13)
In case of naw = 0 or naw ≡ np, the optimal solution to Problem 1 (i.e.,
the compensator matrices ensuring the satisfaction of (1.13)) can be found via semidefinite optimization (see, e.g., [19, 20]). As stated in Section 1.2.1, given the linearity of the compensator, global results can be achieved only for open-loop asymptotically stable plants [20, 16], making the global approach not suitable for many kinds of application. On the other hand, regional properties can be achieved for any kind of plants. Concerning the regional problem, three different objectives can be considered: disturbance rejection optimization, guaranteed reachable set for bounded disturbances or ensured basin of attraction (see, e.g., [22]).
Problem 2 (Disturbance rejection optimization). Given a bound s∈ R>0 on the
exogenous signal energy level, find matrices of the compensator (1.10) such that the augmented closed-loop is well posed and the following relationship is satisfied
kzkL2 ≤ γkwkL2, ∀w : kwkL2 < s, (1.14)
with the smallest γ ∈ R>0, where k ? kL2 denotes the L2 norm of the signal (see
Appendix B).
Problem 3 (Reachable set for bounded inputs). Given a bound s ∈ R>0 on the
exogenous signal energy level and a desired reachable set R ⊂ Rnp for the plant
states, find matrices of the compensator (1.10) such that the augmented closed-loop is well posed and with x(0) = 0, x := xT
p xTc xTaw
T
and kwkL2 < s the following conditions hold
xp(t)∈ R, ∀t > 0, (1.15)
kzkL2 ≤ γkwkL2, (1.16)
Problem 4 (Guaranteed region of attraction). Given a bound s ∈ R>0 on the
exogenous signal energy level and a desired region of attraction A ⊂ Rnp for the
plant states, find matrices of the compensator (1.10) such that with xp(0) ∈ A
there exists xc(0) ∈ Rnc and xaw(0) ∈ Rnaw satisfying
lim
t→∞x(t) = 0, (1.17)
kzkL2 ≤ γkwkL2, (1.18)
with the smallest γ ∈ R>0, where x :=
xT
p xTc xTaw
T
.
In analogy with the global approach these three problems can be recast as con-vex optimization only in the case of anti-windup compensators which are either static or dynamic, with the same order as the plant, for which constructive algo-rithms can be found in [23, 22]. Since regional analysis considers only a specific subset of the L2 class (i.e.,kwkL2 < s), in the considered region less conservative
results are obtained with respect to global methods. Although this brief presen-tation has focused on continuous-time systems, discrete-time formulations can be obtained as well (see, e.g., [24, 25, 26]).
Remark 3. Constructive procedures for the solution of problems 1, 2, 3, 4 are based on Lyapunov analysis (using quadratic Lyapunov functions) and on the con-cept of sector bounded nonlinearities (see Appendix B.3). These two assumptions are really useful since they allow obtaining optimal solutions using semidefinite optimization; on the other hand, they introduce conservativity in results. To im-prove the analysis, a more complex description of the saturation nonlinearity is necessary; as an example norm-bounded or polytopic differential inclusions (see, e.g., [27]) can be used instead of the well-known sector condition. Moreover, non-quadratic Lyapunov functions can be chosen in place of general non-quadratic candi-dates [28, 29]. Unfortunately, these methods cannot address synthesis problems efficiently2, making them applicable only for post-synthesis analysis.
1.2.3
Model recovery anti-windup
Differently from the DLAW approach, model recovery anti-windup3 can also be
applied in presence of nonlinear controllers and for special classes of nonlinear plants (see [31, 32]). This strategy of compensation, which takes its roots from [33], relies on a completely different approach with respect to the DLAW. For the sake of simplicity, to explain the basic ideas of this method, a linear time-invariant plant is considered and the architecture in Figure 1.7 is used. Consider
2These problems are formulated in terms of Bilinear Matrix Inequalities (BMIs), whose
so-lution requires the use of heuristic optimization algorithms (see, e.g., [29, 30]).
3In the literature this method is also referred as the
P satu¯(?) C AW w w uc yc v1 v2 u − yp
Figure 1.7: The anti-windup closed-loop system (MRAW).
the saturated plant described by the following linear time-invariant system
˙xp = Apxp + Bp,usatu¯(u) + Bp,ww
yp = Cp,yxp+ Dp,yusatu¯(u) + Dp,yww
zp = Cp,zxp+ Dp,zusatu¯(u) + Dp,zww,
(1.19)
where xp ∈ Rnp, u ∈ Rnu, w ∈ Rnw, yp ∈ Rny, z ∈ Rnz and Ap ∈ Rnp×np,
Bp,u ∈ Rnp×nu, Bp,w ∈ Rnp×nw, Cp,y ∈ Rny×np, Cp,z ∈ Rnz×np, Dp,yu ∈ Rny×nu,
Dp,zu ∈ Rnz×nu, Dp,yw ∈ Rny×nw, Dp,zw ∈ Rnz×nw. The unconstrained controller is
described by the following system: (
˙xc= f (xc, uc, w)
yc= g(xc, uc, w),
(1.20)
where xc ∈ Rnc is the state, yc∈ Rnu is the demanded control action, uc∈ Rny is
the controller input and the nonlinear functions f (?) : Rnc × Rny × Rnw → Rnc,
g(?) : Rnc× Rny× Rnw → Rnu, are locally Lipschitz. In MRAW the compensator
takes the same structure as the plant, namely
˙xaw = Apxaw+ Bp,u(satu¯(u)− yc)
v1 = h1(xaw, satu¯(u)− yc)
v2 = h2(xaw, satu¯(u)− yc),
(1.21)
with xaw ∈ Rnp and it is used to generate the two compensation signals v1 ∈ Rnu,
v2 ∈ Rny via the nonlinear laws h1(?) : Rnp×Rnu → Rnu, h2(?) : Rnp×Rnu → Rny,
respectively. In particular, according to the diagram in Figure 1.7, the following conditions hold
u = yc+ v1, (1.22)
Defined yaw ∈ Rny as
yaw := Cp,yxaw+ Dp,yu(sat¯u(u)− u) , (1.24)
if the signal v2 is chosen such that
v2 =−yaw, (1.25)
then the interconnection of (1.19), (1.20), (1.21), (1.22), (1.23), (1.25) leads to the following augmented closed-loop system:
˙xp = Apxp+ Bp,usat¯u(yc+ v1) + Bp,ww
yp = Cp,yxp+ Dp,yusatu¯(yc+ v1) + Dp,yww
zp = Cp,zxp+ Dp,zusat¯u(yc+ v1) + Dp,zww
˙xc= f (xc, yp− yaw, w)
yc= g(xc, yp− yaw, w)
˙xaw = Apxaw+ Bp,u(satu¯(yc+ v1)− yc)
yaw = Cp,yxaw+ Dp,yu(satu¯(yc+ v1)− yc) .
(1.26)
By applying the change of variable
xl := xp− xaw (1.27)
to equation (1.26) and defining
yl := yp− yaw, (1.28)
it is easy to get from (1.26) the system ˙xl= Apxl+ Bp,uyc+ Bp,ww yl= Cp,yxl+ Dp,yuyc ˙xc= f (xc, yl, w) yc= g(xc, yl, w), (1.29)
which represents the dynamics of the unconstrained system. As remarked in [6], this structure recovers the unconstrained plant model as seen from the viewpoint of the unconstrained controller; in other words, the input (and, thanks to the Lipschitz properties of functions f, g, the output) of the controller is the same as if saturation were not present. This stated, the unconstrained performance index zl ∈ Rnz is expressed by
zl= Cp,zxl+ Dp,zyc+ Dp,ww (1.30)
and, recalling the definition in equation (1.27), the quantity
P C AW satu¯(?) yc yl w w − v1 zp− zl
Figure 1.8: The MRAW separation principle.
denotes the deviation from the unconstrained performance. This idea of separation between unconstrained dynamics and its effective mismatch is shown in Figure 1.8. Equation (1.31) states that the larger the anti-windup state xaw, the larger the
effect of windup. Since all the compensator matrices in equation (1.21) are set, the only degree of freedom which remains is the selection of signal v1; according
to this separation principle, it is intuitive that the compensation action v1 must
be designed to make the expression in (1.31) as small as possible. The problem statement is given in the following [33].
Problem 5 (Nominal, globalL2 anti-windup problem). Find the signal v1 ∈ Rnu
such that:
• if xaw(0) = 0 and satu¯(yc) = yc ∀t, then z = zl ;
• given a set U := {u ∈ Rnu : sat
¯
u(u) = u}
dist(yc,U) ∈ Ln2u =⇒ (z − zl)∈ Ln2z, (1.32)
where the operator dist(yc,U) denotes the distance4 of the signal yc from the
set U, namely
dist(yc,U) := inf
w∈Ukyc− wk. (1.33)
The local version of Problem 5 consists in modifying equation (1.32) into kdist(yc,U)kL2 < s =⇒ k(z − zl)kL2 < γkdist(yc,U)kL2. (1.34)
Since the compensation signal v1 can be also selected as the output of a nonlinear
function of the AW states and input, it is possible to achieve global stability properties also in the case of marginally stable/unstable plants [35, 36, 37]. The majority of constructive procedures for the design of v1 are based on semidefinite
optimization and they address either global or local problems (see, e.g., [38, 35, 39, 16, 36, 37]). The corresponding discrete-time formulation can be found in [40].
4In the case of a decentralised saturation function,
1.2.4
Weston and Postlethwaite framework
This framework was introduced in [41] and it shares some analogies with the MRAW scheme. Making reference to the state space description of a stabilisable and detectable plant with the same form as in equation (1.19) and denoted with G(s) the transfer function of from the control input u to the output yp, namely
G(s)∼ Ap Bp,u Cp,y Dp,u , (1.35)
this linear conditioning scheme uses the knowledge of the plant to provide AW compensation as shown in Figure 1.9. The linear system N (s) ∈ RH∞5 must be
P satu¯(?) C N (s) G(s) N (s)− I w w yl yc − ud yd − Ψ yp
Figure 1.9: Weston and Postlethwaite linear conditioning.
optimized according to some criterion discussed later. Denoted with
Ψ = dzu¯(u) := u− sat¯u(u) (1.36)
the decentralised deadzone function, it is possible to prove by block manipulation that the schemes in Figures 1.9 and 1.10 are equivalent [44]. The structure shown in Figure 1.10 is appealing since, in analogy with the MRAW framework, it high-lights the separation between a linear and a nonlinear part. In particular, the stability of the augmented closed-loop in Figure 1.9 is strictly dependent on the stability of the nonlinear loop in Figure 1.10; on the other hand, its performance properties in presence of saturation are strongly influenced by the disturbance filter G(s)N (s). The usual selection of N (s) (see, e.g., [41]) makes reference to the coprime factors L(s), N (s) of the plant, namely
G(s) = L(s)N−1(s). (1.37)
5The subspace of
H∞ (see [42, Definition 3.28]) consisting of all proper and real transfer
P C dzu¯(?) N (s)− I G(s)N (s) Filter Nonlinear loop yc yl yp w w − ud Ψ yd Unconstrained system
Figure 1.10: Weston and Postlethwaite linear conditioning: separation principle.
These two factors can be related to the state space realisation of G(s) in (1.35) by the following relationship
N L ∼ Ap+ BF p,uF Bp,uHH Cp,y + Dp,yuF Dp,yuH
, (1.38) where matrices F ∈ Rnu×np, Hnu×nu are the free parameters to be optimized in
AW synthesis with the constraint that Ap+ Bp,uF is Hurwitz. The AW problem
formulated in the Weston and Postlethwaite framework is related to the definition of the nonlinear map Tp : yc → yd. In particular, the following formal statement
of the problem is given (see, e.g., [45]).
Problem 6. Find the coprime factorisation matrices H, F satisfying equation (1.38) such that the following conditions are satisfied:
• the nonlinear map Tp is well posed and internally stable;
• if dist(yc,U) = 0 ∀t, then yd= 0 ∀t;
• if dist(yc,U) ∈ Ln2u, then yd∈ Ln2y;
• ∃γ ∈ R>0 such that kTpkL2 < γ.
The optimal choice of matrices F and H in equation (1.38) can be made by solving optimization problems formulated using LMIs (see, e.g., [46, 45, 47]). Rigorously when applying this framework it is only possible to address problems involving asymptotically stable plants and global analysis. However, a suboptimal
extension to regional analysis is possible by using the so-called sector narrowing technique (see, e.g., [47, 10] for suboptimal regional applications). The formu-lation for discrete-time systems uses similar arguments and it can be found in [48].
1.3
Comparative analysis of compensation
strate-gies
The modern framework provides satisfactory arguments to overcome the three drawbacks of ad hoc techniques highlighted in Section 1.1. In particular, once the intrinsic limitations of the problem under study are known, its precise statement and solution are given in a systematic fashion. Within modern methods, three different schemes of compensation have been presented. In this thesis, only the DLAW approach is focused on. There are two main arguments which can be used to justify this choice. First of all, as stated in Section 1.2.4, the Weston and Postlethwaite framework approach only applies to asymptotically stable linear plants, which is not the case for quadrotor applications (and flight control appli-cations in general); the main reason for this is due to the fact that both position and attitude kinematics (see Chapter 2) introduce integral states in the model. The selection of the DLAW framework over the MRAW is due to the better trade off between complexity and efficiency. As said in Section 1.2.1, the use of nonlinear compensation allows one to obtain more advantages with respect to simple linear laws; as an example, marginally unstable plants are semiglobally asymptotically stabilisable with nonlinear compensation, while only regional stability properties can be guaranteed by using linear AW. Given its capability of designing nonlin-ear laws, the MRAW should seem to be the optimal approach to be used when dealing with complex dynamical systems such as multirotor UAVs. However, as cleared up by equation (1.21), the AW is plant order, meaning that the larger the number of the plant states, the more complex the compensator; in this sense, the AW could require much more computational power than the unconstrained controller. On the other side, the DLAW allows designing static compensators: this means that, compared with plant order filters, lower complexity can be in-troduced in existing flight controllers. The successful implementation of a static AW compensator in a general purpose UAV autopilot has been shown in [12], proving that satisfactory results can be achieved even when simple architectures are used. As a consequence of that, the DLAW seems to provide the right trade off between complexity and efficiency; therefore its main theoretical aspects will be investigated in detail in Chapter 3, while the MRAW based compensation will be referred to future works.
Quadrotor modelling and control
In this chapter the basic notions for the study of multirotor systems are illustrated; the organisation of the chapter is the following. In Section 2.1 all the elements for the description of quadrotor kinematics and dynamics are illustrated. In par-ticular the fundamental reference frames, attitude parametrisations, equations of motion and basics of aerodynamics and propulsion will be discussed in this order. Section 2.2 contains a detailed presentation of the control system architecture, while Section 2.3 describes the control allocation process. Section 2.4 illustrates the quadrotor linearised dynamics and the decentralised structure of the closed-loop system. Section 2.5 concludes the chapter by briefly presenting saturation effects for quadrotor platforms.
2.1
Quadrotor kinematics and dynamics
2.1.1
Reference frames
Quadrotor dynamics is studied by introducing two different reference frames: an inertial frame FI = {OI, ˆx, ˆy, ˆz} and a body frame FB = {OB, X, Y, Z} (Figure
2.1). The origin of the first reference OI is fixed and its position is arbitrary, while its axes ˆx, ˆy, ˆzare assumed to point in the north, east and down directions, respectively. The second frame origin OB is attached to the quadrotor and it is
placed in the vehicle centre of mass. In this work the following convention is used for body axes orientation X, Y, Z: the Z axis is orthogonal to the plane of propellers, while X, Y are oriented as in Figure 2.2. Given a vector v ∈ R3,
denoted with v(I) its components in frame F
I and with v(B) the components in
frame FB, the following relationship holds:
v(I)= Rv(B), (2.1) where R ∈ SO(3) := R ∈ R3×3 : RTR = I, det(R) = 1 denotes the direction
cosines matrix of triad X, Y, Z with respect to ˆx, ˆy, ˆz, i.e.
ˆ
y
ˆ
x
ˆ
z
O
IY
X
Z
O
BFigure 2.1: Inertial and body frames.
X
Y
Since R ∈ SO(3), the inverse mapping is given by
v(B)= RTv(I). (2.3) The time derivative of the direction cosines matrix can be related to the angular velocity vector ω ∈ R3 of the triad by mean of the Poisson formula
˙
R = (ω(I))×R, (2.4) where given vectors v, y ∈ R3the map (?)
×: R3 → so(3) :=
W ∈ R3×3 : W + WT = 0
is such that (v)×y = v × y. Equation (2.4) can be also rewritten by using the angular velocity expressed in body axes1 as
˙
R = R(ω(B))×. (2.5) Matrix R can be computed as function of appropriate rotation parameters as it will be shown in the following.
2.1.2
Attitude parametrisation
According to Euler theorem of rotations (see, e.g., [49, 50]), any three dimensional rotation can be fully defined by a unit vector (representing the axis of rotation) and an angle of rotation. As a consequence of that, only three independent parameters are necessary to describe the orientation of a triad. In the following part of the section, two of the most common parametrisations will be recalled.
Euler angles
Euler angles φ, θ, ψ ∈ R are a set of three parameters forming a minimal parametri-sation of the attitude of a rigid body, according to which the rotation matrix R is expressed as sequence of three consecutive planar rotations, each one parametrized by φ, θ, ψ. In particular, the following relationships hold for the roll-pitch-yaw sequence (see, e.g., [51])
Rφ= 10 cφ0 −sφ0 0 sφ cφ , (2.6) Rθ = cθ0 0 sθ1 0 −sθ 0 cθ , (2.7) Rψ = sψcψ −sψ 0cψ 0 0 0 1 , (2.8) R = RψRθRφ, (2.9)
where Rφ, Rθ, Rψ ∈ SO(3) and the abbreviations sin(?) = s(?), cos(?) = c(?)
have been used for simplicity. Introducing the expressions (2.6)-(2.8) in (2.9), the rotation matrix R assumes the form
R = sψcθ sψsθsφ + cψcφ sψsθcφcψcθ cψsθsφ− sψcφ cψsθcφ + sψsφ− cψsφ −sθ cθsφ cθcφ . (2.10) Using Euler angles, the angular velocity of the triad X, Y, Z with respect to ˆ
x, ˆy, ˆz can be derived by using Poisson formula (2.4) and the rotation matrix expression in equation (2.9): (ω(I))× = ˙RRT = ˙RψRTψ + RψR˙θRθRTψ + RψRθR˙φRTφRθTRTψ = (ωψ)×+ Rψ(ωθ)×RTψ + RψRθ(ωφ)×RTθRTψ = (ωψ)×+ (Rψωθ)×+ (RψRθωφ)×, (2.11) which implies ω(I)= ωψ + Rψωθ+ RψRθωφ, (2.12)
where ωφ, ωθ, ωψ ∈ R3 are the angular velocity vectors associated with planar
rotations in equations (2.6)-(2.8), i.e.
ωφ:= ˙ φ 0 0 , ωθ := 0˙θ 0 , ωψ := 00 ˙ ψ . (2.13) Applying the change of basis (2.3) to equation (2.12) and noting that Rφωφ =
RT
φωφ= ωφ, Rθωθ = RθTωθ = ωθ, Rψωψ = RTψωψ = ωψ, it yields
ω(B) = ωφ+ RTφωθ+ RTφRTθωψ, (2.14)
with equivalent compact form
ω(B) := pq r = 10 cφ0 cθsφ−sθ 0 −sφ cθcφ | {z } S(φ,θ,ψ) ˙ φ ˙θ ˙ ψ . (2.15)
If from one side Euler angles offer an intuitive description of the orientation, the main drawback of this parametrisation is that for θ = π
2 + kπ, k ∈ Z, matrix S
Euler Rodrigues parameters (quaternions)
To avoid singularities in the orientation description, Euler Rodrigues parameters e0 ∈ R, ˆe ∈ R3 can be used; they are defined as
ˆ e := sinϕ 2n, (2.16) e0 := cos ϕ 2, (2.17)
where n ∈ R3 is the unit vector denoting the axis of rotation and ϕ is the angle
of rotation. These four parameters form a unit quaternion e∈ S3, e :=e 0, ˆeT
T
, where Sn is the n-dimensional unit sphere, i.e. Sn := {e ∈ Rn+1 :kek = 1}.
De-noted with e1, e2, e3 the three scalar components of ˆe, namely ˆe = [e1, e2, e3]T, the
rotation matrix R can be written as a function of the quaternion e as (see, e.g., [52]) R(e) = I + 2e0(ˆe)×+ 2(ˆe)2× = 2e 2 0+ 2e21− 1 2e1e2− 2e0e3 2e1e3+ 2e0e2 2e1e2+ 2e0e3 2e20+ 2e22− 1 2e2e3− 2e0e1 2e1e3− 2e0e2 2e2e3+ 2e0e1 2e20+ 2e23− 1 , (2.18) where I ∈ R3×3 in the above expression denotes the identity matrix. The
in-verse rotation can be expressed by using the conjugate quaternion e∗ ∈ S3,
e∗ := e 0,−ˆeT
T
. Euler Rodrigues parameters can also represent composition of rotations. In particular, given eA, eB∈ S3, the rotation composition RC(eC) =
RB(eB)RA(eA) can be expressed by mean of equation (2.18) by using the
quater-nion eC ∈ S3 given by eC = eB⊗ eA:= eB0eA0 − ˆe T BeˆA eB0ˆeA+ eA0eˆB− ˆeB× ˆeA . (2.19)
As in the case of Euler angles parametrisation, quaternion time derivative can be related to the angular velocity by
0 ω(I) = 2 e0 ˆeT −ˆe e0I + (ˆe)× ˙e = 2 e0 e1 e2 e3 −e1 e0 −e3 e2 −e2 e3 e0 −e1 −e3 −e2 e1 e0 ˙e0 ˙e1 ˙e2 ˙e3 , (2.20)
with equivalent body axes description given by 0 p q r = e0 e1 e2 e3 −e1 e0 e3 −e2 −e2 −e3 e0 e1 −e3 e2 −e1 e0 | {z } ˜ S(e) ˙e0 ˙e1 ˙e2 ˙e3 . (2.21)
Since ˜S(e) is never singular, quaternion parametrisation does not suffer of the gimbal lock and it is addressed as global parametrisation; quaternions are com-monly used in the attitude estimation and flight control modules of small UAVs autopilot.
2.1.3
Quadrotor dynamics
The hypothesis of rigid body is used in the derivation of quadrotor equations of motion; even if real bodies are not rigid, this assumption provides a very good description of aircraft dynamics for flight control purposes. Accordingly, in an inertial reference frame momentum and angular momentum balance yield
( ˙
Q(I)= F(I)
˙Γ(I)= M(I), (2.22)
where Q∈ R3 is the linear momentum, Γ∈ R3 is the angular momentum, F ∈ R3
is the external force and M ∈ R3 is the external moment computed with respect
to the quadrotor centre of gravity. Using the change of basis in equation (2.1) and Poisson formula in (2.5), it is possible to obtain
(
R ˙Q(B)+ R(ω(B))×Q(B)= RF(B)
R ˙Γ(B)+ R(ω(B))×Γ(B)= RM(B), (2.23)
which pre-multiplied by RT yields
( ˙ Q(B)+ (ω(B))×Q(B)= F(B) ˙Γ(B)+ (ω(B)) ×Γ(B)= M(B). (2.24)
By linear momentum definition Q := mv, where m∈ R>0 is the UAV mass and
v ∈ R3 is the UAV centre of gravity speed; Γ(B) = Jω(B), where J ∈ R3×3 >0 is the
UAV inertia tensor with respect to the centre of mass
J := −JJxxxy −JJyyxy −J−Jxzyz −Jxz −Jyz Jzz , (2.25) where Jxx := Z m y2+ z2dm, Jyz := Z m yzdm, Jyy := Z m x2+ z2dm, Jxy := Z m xydm, Jzz := Z m x2+ y2dm, Jxz := Z m xzdm, (2.26)
and M(B):= [L M N ]T is the vector containing roll, pitch and yaw moment com-ponents. Assuming no mass variations during flight, the following set of equations is obtained (
m ˙v(B)+ ω(B)× mv(B) = F(B)
J ˙ω(B)+ ω(B)× Jω(B)= M(B). (2.27) Equation (2.27) clearly shows that angular motion is not affected by linear dy-namics if M(B) is assumed to be independent of the translational state. However
in most of the applications it is convenient to re-write the linear dynamics in the inertial frame, i.e. (
m ˙v(I) = F(I)
J ˙ω(B)+ ω(B)× Jω(B)= M(B). (2.28) Given the properties of symmetry of a quadrotor, it can be assumed with good approximation that body axes shown in Figure 2.2 are also principal axes; in this case, the inertia tensor J assumes the diagonal form J = diag (Jxx, Jyy, Jzz).
Assuming negligible aerodynamic drag, only two contributions are present in the external force F : gravity and propellers’ thrust. In particular, for a coplanar propeller configuration the following relationship holds
F(I) := FFne Fd = 00 mg + R 00 −T , (2.29) whereT ∈ [0, TM AX] is the total thrust generated by propellers and g = 9.81m/s2
is the gravity acceleration. The overall thrust is the sum of individual propeller contributions Ti ∈ [0, TM AX,i] , i = 1, . . . , 4, i.e.
T =
i=4
X
i=1
Ti. (2.30)
Blade element momentum theory is used to describe propeller characteristics and, assuming steady aerodynamics and negligible axial airspeed, Ti is only related to
the rotational speed of the propeller Ωi ∈ R>0 by the following static law (see,
e.g., [53])
Ti = KTΩ2i, (2.31)
where the term KT ∈ R>0 is identified from experimental data. A first order
dynamical model can be assumed for the actuators (see, e.g., [54]); the relationship between desired propeller forces2Td
i and actual propeller forcesTican be described
by the following law: ˙ Ti =− 1 τpTi + 1 τpT d i , i = 1, . . . , 4, (2.32)
2Desired propeller forces will be considered as the input to the plant. Practically speaking
the real input to the plant is the commanded rotational speed of the blades. However in virtue of equation (2.31), these two quantities can be considered equivalent.
where τp ∈ R>0 is the propeller time constant.
Assuming negligible gyroscopic effects due to blades rotation, two different contributions can be considered in the computation of external torquesL, M, N . The first one is due to propulsion: when the propeller rotates, air opposes to its motion by generating a torque on the UAV. Once again, blade element theory is used to obtain the value of these torques Qi ∈ R, i = 1, . . . , 4 by mean of the
static relationship
Qi = KQΩ2i, (2.33)
where the coefficient KQ ∈ R>0 is experimentally identified. According to the
axes convention in Figure 2.2, the propulsive yaw moment Np ∈ R is given by
Np = 4
X
i=1
(−1)i+1Qi. (2.34)
Since forces Ti in equation (2.31) originate at a distance b ∈ R>0 from the centre
of mass, an additional torque must be considered in L and M. For an X frame configuration as in Figure 2.2, the following equation is used to relate forces and torques to the rotational speed of the blades
T Lp Mp Np = KT (Ω21 + Ω22+ Ω23+ Ω24) KT√b2(Ω21 + Ω22− Ω23− Ω24) KT√b2(Ω21 − Ω22− Ω23+ Ω24) KQ(Ω21− Ω22+ Ω23− Ω24) , (2.35) which can be rearranged in matrix form as
T Lp Mp Np = KT KT KT KT KT√b2 KT√b2 −KT√b2 −KT√b2 KT√b2 −KT√b2 −KT√b2 KT√b2 KQ −KQ KQ −KQ Ω2 1 Ω2 2 Ω2 3 Ω2 4 . (2.36) Using R´enard formula in equation (2.31) the relationship
T Lp Mp Np = 1 1 1 1 b√1 2 b 1 √ 2 −b 1 √ 2 −b 1 √ 2 b√1 2 −b 1 √ 2 −b 1 √ 2 b 1 √ 2 β −β β −β | {z } χ T1 T2 T3 T4 (2.37)
can be obtained, where β := KQ
KT. Matrix χ∈ R
4×4 is called mixer map and it is
used to relate quadrotor physical inputs (single propeller forces) to overall thrust and body moments (also referred as virtual inputs).
The second contribution which is present in external moments is due to the effect of aerodynamic damping; assuming negligible the contribution of the UAV
frame, damping originates because of the propellers motion through the air and it can be modelled by the following linear relationship in the angular rate compo-nents MLdd Nd = L0/p M0/q 00 0 0 0 pq r , (2.38) where L/p,M/q ∈ R<0 are the stability derivatives which can be computed
ac-cording to the constitutive laws (see [55])
M/q =−4ρπd4Ω2H ∂KT ∂q b √ 2, (2.39) ∂KT ∂q = Clα 8 σp ΩHd b √ 2, (2.40)
where ρ ∈ R>0 is the air density, d ∈ R>0 is the propeller radius, ΩH ∈ R>0
is the propellers’ rotational speed at hovering, Clα ∈ R>0 is the lift curve slope
of the propeller aerofoil and σp ∈ R>0 is the propeller solidity. By symmetry
considerations it can be assumed L/p =M/q.
In conclusion, if attitude is parametrised with Euler angles, by considering equations (2.10), (2.15), (2.28), (2.29), (2.37) and (2.38), defined with r(I) :=
[rn re rd]T ∈ R3 the position vector (north, east and down components,
respec-tively), and with v(I) := [vn ve vd]T the speed, the following state space model
describes the complete rigid body dynamics for the UAV
m ˙vn=−(cos ψ sin θ cos φ + sin ψ sin φ)T
m ˙ve =−(sin ψ sin θ cos φ − cos ψ sin φ)T
m ˙vd= mg− (cos θ cos φ)T Jxx˙p = (Jyy− Jzz)qr +Lp+L/pp Jyy˙q = (Jzz − Jxx)rp +Mp+M/qq Jzz˙r = (Jxx− Jyy)pq +Np ˙rn= vn ˙re = ve ˙rd= vd ˙
φ = p + q tan θ sin φ + r tan θ cos φ ˙θ = p cos φ − r sin φ
˙
ψ = p sec θ sin φ + r sec θ cos φ,
(2.41)
with constitutive relationships for propulsive and aerodynamic terms given by equations (2.37), (2.39) and (2.40).
2.2
Control system architecture
In this section a cascade control architecture to control the motion of a quadrotor is described. Given position and orientation references to be tracked along with estimates of the quadrotor states, the flight control unit returns the desired forces and moments T , Lp, Mp, Np which the UAV must produce. Because of the
un-deractuated nature of the quadrotor (associated with the coplanar configuration of the propellers), it is not possible to obtain full pose tracking: intuitively, by expanding equation (2.29) FFne Fd =
−(cos ψ sin θ cos φ + sin ψ sin φ)T−(sin ψ sin θ cos φ − cos ψ sin φ)T mg− (cos θ cos φ)T
, (2.42)
it can be seen that to generate longitudinal and lateral forces Fn, Fe, the attitude
must be changed, meaning that it cannot assume arbitrary values. The formal proof is the following: since it can be shown that rn, re, rd, ψ are flat outputs for
(2.41) (see, e.g., [53, 56]), the states θ, φ can be written as functions of the flat outputs and their time derivatives.
Given these flatness properties, a hierarchical approach is used for quadrotor control. The block diagram of a general control module is shown in Figure 2.3: it is composed of three main elements whose tasks will be explained in the following. Given a desired trajectory rd : R
≥0 → R3, rd := rd n red rdd T to be tracked and Planner Position control law Attitude control law rd ψp Fd T d ed Ld p Md p Nd p e ω r v
Figure 2.3: Flight controller main modules.
feedbacks in terms of position and speed, the outer position controller generates the required force for position stabilisation Fd ∈ R3, Fd := Fd
n Fed Fdd
T
. For each axis the position controller has a decentralised P-PID cascaded structure
(shown in Figure 2.4), whose control law is given by vdn= Kp,n rnd− rn , (2.43) ved= Kp,e red− re , (2.44) vdd= Kp,d rdd− rd , (2.45) Fnd= Kp,vn+ Ki,vn 1 s (vnd− vn)− Kd,vn s 1 + sNv vn, (2.46) Fed= Kp,ve + Ki,ve 1 s (ved− ve)− Kd,ve s 1 + sNv ve, (2.47) Fdd= Kp,vd+ Ki,vd 1 s (vdd− vd), (2.48)
where s is the Laplace variable, Kp,?, Ki,?, Kd,? ∈ R are the proportional, integral
and derivative gains for the considered axis, Nv ∈ R>0is the filter time constant of
the derivative action. The knowledge of Fdalong with the desired yaw angle ψp ∈
Kp,n P Ivn(s) Dvn(s) Kp,e P Ive(s) Dve(s) Kp,d P Ivd(s) rd n rn − vn − vn vd n − Fd n rd e re − ve − ve vd e − Fd e rd d rd − vd − vd d Fdd
Figure 2.4: Position controller architecture.
R allows computing the desired attitude matrix Rd ∈ SO(3), Rd := [b1 b2 b3],
(or planner) as follows (see, e.g., [57]) Td=kFdk, (2.49) b3 =− Fd kFdk, (2.50) b2 = b3× bd1 kb3× bd1k , (2.51) b1 = b2 × b3, (2.52)
where the vector bd
1 ∈ R3, bd1 := [cos ψp sin ψp 0] T
is used to get the desired orientation of the yaw. Matrix Rdis obtained from (2.50)-(2.52) in a way that the
quadrotor body axis Z has the same direction as Fd, while the rotation about Z is
assigned by bd
1. The desired orientation is tracked by the inner attitude controller.
Defined with ed∈ S3the desired quaternion (associated with Rd) and with e
e ∈ S3,
ee := e∗⊗ ed the quaternion error, the attitude control law is expressed as follows
p d qd rd = 2 K0p,φ K0p,θ 00 0 0 Kp,ψ sign(ee0)ˆee, (2.53) Ldp = Kp,p+ Ki,p 1 s pd− p− Kd,p s 1 + sNω p, (2.54) Mdp = Kp,q+ Ki,q 1 s qd− q− Kd,q s 1 + sNω q, (2.55) Npd= Kp,r+ Ki,r 1 s rd− r− Kd,r s 1 + sNω r, (2.56)
where the gains have been denoted with similar notation to position controllers, ee0, ˆee are scalar and vectorial part of the quaternion error ee, respectively, ωd ∈
R3, ωd := pd qd rdT is the desired angular rate in body frame and Ldp, Mdp,
Nd
p ∈ R are the commanded roll, pitch and yaw moment, respectively. Assuming
small perturbations of the attitude with respect to the hovering condition and small attitude setpoints, equation (2.53) can be linearised and turns out to be equivalent to p d qd rd = K0p,φ K0p,θ 00 0 0 Kp,ψ φ d− φ θd− θ ψd− ψ , (2.57) where φd, θd, ψd ∈ R are the demanded Euler angles associated with
(2.50)-(2.52); as a consequence of that, the linearised attitude control law assumes the decentralised P-PID structure of Figure 2.5. Controller outputsTd, Ld
p,Mdp, Npd,
are actually just intermediate inputs for the quadrotor: the real inputs Td i , i =
1, . . . , 4 are obtained by using the inverse of the mixer map χ introduced in equa-tion (2.37), meaning that an allocaequa-tion step is required in the implementaequa-tion.
![Figure 4.2: The ANT-1 mini UAV (source [1]).](https://thumb-eu.123doks.com/thumbv2/123dokorg/7509558.105063/78.892.222.627.730.1034/figure-the-ant-mini-uav-source.webp)
