Fault-Tolerant Control of a Variable-Pitch Quadrotor under Actuator Loss of Effectiveness and Wind Perturbations

Abstract

The actuator fault-tolerant control problem for a variable-pitch quadrotor is addressed under uncertain conditions. Following a model-based approach, the plant nonlinear dynamics are faced with a disturbance observer-based control and a sequential quadratic programming control allocation, where only kinematic data of the onboard inertial measurement unit are required for the fault-tolerant control, i.e., it does not require the measurement of the motor speed nor the current drawn by the actuators. In the case of almost horizontal wind, a single observer handles both faults and the external disturbance. The estimation of the wind is fed forward by the controller, while the actuator fault estimation is exploited in the control allocation layer, which copes with the variable-pitch nonlinear dynamics, thrust saturation, and rate limits. Numerical simulations in the presence of measurement noise show the capability of the scheme to handle multiple actuator faults in a windy environment.

1. Introduction

Multirotor helicopters are a class of Unmanned Aerial Vehicle (UAV) that are employed in a wide range of applications, resulting in one of the most appealing classes of vehicles for both research and industry. The standard quadrotor represents the most common configuration, which is equipped with four fixed-pitch blades connected to independent motors. Variable-Pitch Quadrotors (VPQs) can instead vary both the rotation speed of each motor and the blade pitch of each propeller, theoretically doubling the degrees of freedom of each actuator. VPQs are also known as collective pitch quadcopters or heliquads.

This results in many advantages, from which it is possible to list higher thrust rate of change, reverse thrust, reverse flight capabilities, and scaling well with size [1,2,3]. Moreover, the popular ArduPilot control suite supports VPQs using the Heliquad configuration. Despite this, VPQs still represent a market niche, where few commercial devices (e.g., the Assault Reaper 500, the Stingray 500, and the WLtoys Assassin V383, for recreational use) are available. In most of the scientific literature, however, custom prototypes (e.g., [4,5,6]) are developed to test algorithms and control strategies. Fixed-pitch quadrotors and VPQs have similar dynamics, while relevant differences arise in the control effectiveness matrix. This allows for identical control laws for both configurations, but VPQs require a more complex control allocation algorithm. The additional degrees of freedom, provided by the pitch angles, can be exploited to satisfy additional constraints, such as minimizing energy consumption, handling the presence of faults and failures (as defined in [7]), etc.

Specifically, the control allocation algorithm, which manages the redundancy while respecting constraints, strictly depends on two key factors: the thrust model and the choice of the control inputs.

As regards the thrust model, several mathematical models have been proposed in the literature to catch the relation between motor speed, pitch angle, as well as propeller size and shape [3,5,6,8,9].

As for the choice of the control inputs, a VPQ instead has eight inputs available in general (i.e., four motor speeds and four pitch angles), a relevant input redundancy which is perfectly suited to provide actuator fault tolerance while satisfying actuation constraints. Two issues must be faced in this case. The increased number of actuators entails higher manufacturing costs and smaller payloads due to the extra weight. Moreover, the non-linearity of the control allocation problem makes it harder to solve online. In any case, the solution in which both the pitch angle and the motor speed can independently concur in providing the requested thrusts and torques is investigated in the literature, as carried out in [9,10] and, partially, in [6,11].

Alternatively, some simplifications have been proposed in the literature. The simpler solution is to assume constant motor speeds and use pitch angles as control inputs, as proposed in [1,3,8,12,13]; in this case, four inputs are available, so the allocation problem is very simple and there is no redundancy at all. Employing a single central motor to drive the four propellers makes a decrease in costs, weight, and inertia possible. However, assuming the rotational speed is constant is just a choice to simplify the control scheme, and employing a time-varying speed, even with a single central motor, is feasible (see, for example, [12,14]). Taking into account a variable centralized motor speed allows for some input redundancy (five inputs) and thus enables for fault-tolerant control, which is the focus of this paper.

Any fault that can affect a conventional quadrotor can affect VPQs as well, and they can involve any part of the rotor system (e.g., ESC, motor, propeller). The most common class of actuator fault is a Loss of Effectiveness (LOE), i.e., the produced thrust is lower than the expected one. In fact, LOE may be the consequence of many issues: battery voltage drop [15], increased drag due to the blade pitch [5], and of course any physical damage to the blades [16]. However, as for the specific case of fault diagnosis and tolerant control for VPQs, few works can be found in the literature. In [17], the presence of a pitch lock-in-place is considered for a VPQ, and both fault diagnosis and fault-tolerant control are developed. The authors of [14] focus on the effects of a lock-in-place in a centrally powered VPQ, but no fault diagnosis or fault-tolerant control are proposed. Finally, in [18], a fault detection and fault-tolerant control strategy to face both pitch lock-in-place and motor LOE is proposed.

A major issue in control of multirotors is the presence of wind. In the UAV literature, both active and passive fault-tolerant control schemes have been employed to cope with the presence of wind. In [19], the authors model the aerodynamic effects, define the upper bounds, and then deal with the wind by means of a robust sliding mode control; in [20], extended state observers are instead employed to mitigate the effects of wind. The presence of external disturbances is also detrimental to the fault diagnosis task, as the effects of both the disturbances and faults may trigger a fault detection algorithm. In this paper, we show that horizontal wind can be managed for both the purposes of fault diagnosis and disturbance rejection by means of a single observer that estimates the actuator faults and the effects of the wind.

The main contribution of this paper is proposing a novel, complete fault-tolerant control scheme for a VPQ under actuator faults and wind, which consists of three main tasks: fault and disturbance estimation, non-linear control with disturbance rejection, and fault-tolerant control allocation. The proposed control strategy fills a gap in the literature because, to the best of the authors’ knowledge, there are no fault-tolerant control schemes for VPQs that take into account common external disturbances such as the wind. With respect to the authors’ previous works [17,18], in this paper, we consider the effects of wind that introduce significant complexity in the first two tasks, i.e., fault estimation and control. As an additional contribution, we propose a generalized strategy that is well-suited for both centrally powered VPQs and VPQs with four independent motors. In fact, the fault and disturbance estimation strategy, as well as the non-linear control law, does not depend on the choice of the control inputs (i.e., centralized or independent propeller speed). On the other hand, the control allocation algorithm must exploit redundancy to achieve fault tolerance, so the choice of the control inputs is relevant; however, the proposed control allocation algorithm is designed such that it suffices to enable an equality constraint to deal with a centrally powered VPQ (instead of a conventional one with four separate motors).

The paper is structured as follows. In Section 2, the mathematical model of the vehicle is detailed, including the characterization of variable-pitch propellers and the model of actuator faults to be faced. Section 3 is devoted to the design of a Non-linear Disturbance Observer (NDO) to estimate faults and disturbances. In Section 4, a double-loop controller to track position and orientation, despite the presence of external disturbances, is introduced. In Section 5, a control allocation strategy that redistributes the control effort by taking into account the presence of actuator faults is detailed. Simulation results to show the effectiveness of the overall fault-tolerant control strategy are proposed in Section 6. Finally, conclusions and future works end the paper.

2. Mathematical Model

The overall kinematics and dynamics of a generic multirotor, thus including variable-pitch quadrotors, can be approximated as that of a unique rigid body with six degrees of freedom [21]. Let us consider an earth-fixed frame $R_E=(O_E,x_E,y_E,z_E)$, which is assumed to be inertial, and a body-fixed frame $R_B=(O_B,x_B,y_B,z_B)$, which is placed in the center of mass of the vehicle. For simplicity, we assume the center of mass coincides with the geometric center of the quadrotor, as depicted in Figure 1.

From now on, the standard basis vectors of ${\mathbb{R}}^3$ are denoted with $e_1,e_2,e_3$. Let us denote with $p_F=\mathrm{col}(x_F,y_F,z_F)$ the position of the center of mass with respect to $R_E$, and with $\omega =\mathrm{col}(p,q,r)$ the angular velocity of the vehicle. Neglecting the second-order effects (i.e., rotor dynamics, blade flapping, gyroscopic and inertial effects due to the rotors), the quadrotor kinematics and dynamics can be expressed as [21]

$$\begin{array}{cc}\hfill m{\ddot{p}}_F& =-k_t{\dot{p}}_F-mg{e}_3+R{F}_m^B+F_w^E\hfill \\ \hfill J\dot{\omega }& =-k_r\omega -\omega \times J\omega +{\tau }_m^B\hfill \\ \hfill \dot{\eta }& =T\left(\eta \right)\omega ,\hfill \end{array}$$

(1)

where m is the total system mass, $J=\mathrm{diag}(J_x,J_y,J_z)$ is the tensor of inertia along the $x_B$, $y_B$, and $z_B$ axes, $k_t$ and $k_r$ are the linear and angular friction coefficients, $\eta =\mathrm{col}(\phi ,\theta ,\psi )$ is the vector of the attitude angles (roll, pitch, and yaw, respectively) that characterize the rotation from $R_B$ to $R_E$.

$$R=\left[\begin{array}{ccc}{c}_{\psi }{c}_{\theta }& c_{\psi }{s}_{\phi }{s}_{\theta }-c_{\phi }{s}_{\psi }& s_{\phi }{s}_{\psi }+c_{\phi }{c}_{\psi }{s}_{\theta }\\ c_{\theta }{s}_{\psi }& c_{\phi }{c}_{\psi }+s_{\phi }{s}_{\psi }{s}_{\theta }& c_{\phi }{s}_{\psi }{s}_{\theta }-c_{\psi }{s}_{\phi }\\ -s_{\theta }& c_{\theta }{s}_{\phi }& c_{\phi }{c}_{\theta }\end{array}\right],$$

(2)

where $cos(·)=c_{(·)}$ and $sin(·)=s_{(·)}$ are introduced for the sake of brevity, $F_m^B$ and ${\tau }_m^B$ are the overall force and torque due to the motors (decomposed in $R_B$), $F_w^E$ is the force due to the wind (decomposed in $R_E$), and $T\left(\eta \right)$ is the kinematic coordinate transformation related to the adopted roll–pitch–yaw rotation, that is [22]:

$$T\left(\eta \right)=\left[\begin{array}{ccc}1& sin\left(\phi \right)tan\left(\theta \right)& cos\left(\phi \right)tan\left(\theta \right)\\ 0& cos\left(\phi \right)& -sin\left(\phi \right)\\ 0& \frac{sin\left(\phi \right)}{cos\left(\theta \right)}& \frac{cos\left(\phi \right)}{cos\left(\theta \right)}\end{array}\right].$$

(3)

Let us denote with $f_i\in \mathbb{R}$ the actual lift thrust produced by the i-th actuator. In VPQs, as in conventional quadrotors, all the actuators are oriented upwards and they produce collinear lift forces only, as depicted in Figure 2.

Denoting with $u_z$ the overall lift force, given by $u_z=f_1+f_2+f_3+f_4$, we have

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill F_m^B& =\mathrm{col}(0,0,u_z)\hfill \\ \hfill {\tau }_m^B& =\mathrm{col}({\tau }_p,{\tau }_q,{\tau }_r).\hfill \end{array}\end{array}$$

(4)

In other words, the overall lift thrust is along $z_B$ only, because none of the actuators can generate a horizontal force in $R_B$. Then, substituting (4) in the first row of (1), we obtain the following model:

$$\begin{array}{cc}\hfill m{\ddot{p}}_F& =-k_t{\dot{p}}_F-mg{e}_3+u_{z}R{e}_3+F_w^E\hfill \\ \hfill J\dot{\omega }& =-k_r\omega -\omega \times J\omega +{\tau }_m^B\hfill \\ \hfill \dot{\eta }& =T\left(\eta \right)\omega .\hfill \end{array}$$

(5)

The four-dimensional vector $\mathrm{col}(u_z,{\tau }_m^B)=\mathrm{col}(u_z,{\tau }_p,{\tau }_q,{\tau }_r)$ is the so called virtual input vector.

2.1. Input Mapping

Consider the i-th motor. We denote with $f_i\in \mathbb{R}$ the lift thrust generated by the propeller attached to the motor, with $d_i\in \mathbb{R}$ the generated drag, and with ${\mathsf{\Omega }}_i\in \mathbb{R}$ the angular rate of the propeller, taken to be positive when the rotation of the motor matches that of the propeller (clockwise or counter-clockwise). Let us consider the case of rectangular, untwisted blades (which are suitable for variable-pitch drones) with uniform inflow. Let us assume the blade pitch is limited in order to avoid propeller stalling, which is an undesired effect to be avoided in practice. Then, both the lift force and drag of each rotor can be expressed as a function of the rotational speed [3]:

$$\begin{array}{cc}\hfill f_i& =c_{L_i}{\mathsf{\Omega }}_i^2\hfill \end{array}$$

(6)

$$\begin{array}{cc}\hfill d_i& =c_{D_i}{\mathsf{\Omega }}_i^2,\hfill \end{array}$$

(7)

where $c_{L_i}$ and $c_{D_i}$ are the lift and drag coefficients that depend on the blade geometry. In the case of fixed-pitch propellers, $c_{L_i}$ and $c_{D_i}$ are constant. In a variable-pitch propeller, however, as the pitch angle ${\alpha }_i\in \mathbb{R}$ changes, the lift and drag coefficients change as well. Two main approaches to characterize the dependency of $c_{L_i}$ and $c_{D_i}$ on ${\alpha }_i$ can be found in [9], and they can be categorized into grey-box (e.g., [3]) and black-box (e.g., [5]) approaches. In this work, we follow the grey-box approach of [3], which makes use of the blade element theory and the momentum theory to obtain

$$\begin{array}{cc}\hfill c_{L_i}& =c_{T_i}\left({\alpha }_i\right)\rho \pi R_a^4\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill c_{D_i}& =c_{Q_i}\left({\alpha }_i\right)\rho \pi R_a^5,\hfill \end{array}$$

(9)

where $R_a$ is the propeller radius, $\rho$ is the air density, while $c_{T_i}(·)$ and $c_{Q_i}(·)$ are dimensionless lift and drag coefficients, which are functions of ${\alpha }_i$. Following [9,23], which assume steady-state linearized blade aerodynamics, we consider the implicit relations

$$\begin{array}{cc}\hfill {\alpha }_i& =k_1{c}_{T_i}+k_2\sqrt{|c_{T_i}|}\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill c_{Q_i}& =k_3\sqrt{|c_{T_i}{|}^3}+k_4,\hfill \end{array}$$

(11)

where $k_1=\frac{6}{\sigma C_{l_{{\alpha }_i}}}$, $k_2=\frac{3}{2\sqrt{2}}$, $k_3=\frac{1}{\sqrt{2}}$, and $k_4=\frac{1}{8}\sigma C_{d{0}_i}$. In detail, $\sigma =\frac{N_{b}c}{\pi R_a}$ is the blade solidity, $N_b=2$ is the number of blades, c is the chord length, $C_{l_{{\alpha }_i}}$ is the 2D lift–curve–slope of the airfoil section, comprising the rotor, and finally, $C_{d{0}_i}$ is the zero-lift drag coefficient (see [9] for further details). Without loss of generality, we consider a ’+’ quadrotor configuration (see Figure 1), so the overall input mapping is

$$\left[\begin{array}{c}{u}_z\\ {\tau }_p\\ {\tau }_q\\ {\tau }_r\end{array}\right]=\left[\begin{array}{c}{c}_{L_1}\left({\alpha }_1\right){\mathsf{\Omega }}_1^2+c_{L_2}\left({\alpha }_2\right){\mathsf{\Omega }}_2^2+c_{L_3}\left({\alpha }_3\right){\mathsf{\Omega }}_3^2+c_{L_4}\left({\alpha }_4\right){\mathsf{\Omega }}_4^2\\ -l{c}_{L_2}\left({\alpha }_2\right){\mathsf{\Omega }}_2^2+l{c}_{L_4}\left({\alpha }_4\right){\mathsf{\Omega }}_4^2\\ -l{c}_{L_1}\left({\alpha }_1\right){\mathsf{\Omega }}_1^2+l{c}_{L_3}\left({\alpha }_3\right){\mathsf{\Omega }}_3^2\\ -c_{D_1}\left({\alpha }_1\right){\mathsf{\Omega }}_1^2+c_{D_2}\left({\alpha }_2\right){\mathsf{\Omega }}_2^2-c_{D_3}\left({\alpha }_3\right){\mathsf{\Omega }}_3^2+c_{D_4}\left({\alpha }_4\right){\mathsf{\Omega }}_4^2\end{array}\right],$$

(12)

where l is the arm length. The mapping (12) is nonlinear in ${\alpha }_i$ and ${\mathsf{\Omega }}_i$, which are the actual control inputs for each motor i, for a total of eight inputs.

2.2. Explicit Lift and Drag Functions

Equations (10) and (11) are implicit functions of ${\alpha }_i$, so they are not practical for online computation. In the literature, such functions are solved iteratively by means of numerical approaches [9]. In this section, instead, we introduce two equivalent equations that determine the coefficients $c_{T_i}$ and $c_{Q_i}$ in terms of explicit functions of ${\alpha }_i$.

The lift function $c_{T_i}\left({\alpha }_i\right)$ in (10) is expected to be an odd function of ${\alpha }_i$ [9]. In fact, from (8), we obtain

$$c_{T_i}\left({\alpha }_i\right)=\frac{1}{k_1}{\alpha }_i+\frac{1}{2}\mathrm{sign}\left({\alpha }_i\right)\left(\frac{k_2^2}{k_1^2}-\frac{k_2}{k_1^2}\sqrt{k_2^2+4k_1\left|{\alpha }_i\right|}\right),$$

(13)

which is an explicit odd function of ${\alpha }_i$. The drag function $c_{Q_i}\left({\alpha }_i\right)$ in (11) is expected to be an even function of ${\alpha }_i$ [9]. Then, noting that $c_{T_i}\left(\right|{\alpha }_i\left|\right)=sign\left({\alpha }_i\right)c_T\left({\alpha }_i\right)$, (11) can be rewritten as

$$c_{Q_i}\left({\alpha }_i\right)=k_3\sqrt{sign\left({\alpha }_i\right)c_{T_i}{\left({\alpha }_i\right)}^3}+k_4,$$

(14)

which is an explicit even function of ${\alpha }_i$. We also calculate in advance the derivatives of (13) and (14) with respect to ${\alpha }_i$, as they are going to be employed in the remainder of the paper. We have

$$\begin{array}{cc}\hfill c_{T_i}^{\prime }\left({\alpha }_i\right)& =\frac{1}{k_1}-\frac{k_2}{k_1}\frac{1}{\sqrt{k_2^2+4k_1\left|{\alpha }_i\right|}}\hfill \\ \hfill c_{Q_i}^{\prime }\left({\alpha }_i\right)& =\frac{3}{2}{k}_3{c}_{T_i}^{\prime }\left({\alpha }_i\right)sign\left({\alpha }_i\right)\sqrt{sign\left({\alpha }_i\right)c_{T_i}\left({\alpha }_i\right)}\hfill \\ \hfill c_{L_i}^{\prime }\left({\alpha }_i\right)& =c_{T_i}^{\prime }\left({\alpha }_i\right)\rho \pi R_a^4\hfill \\ \hfill c_{D_i}^{\prime }\left({\alpha }_i\right)& =c_{Q_i}^{\prime }\left({\alpha }_i\right)\rho \pi R_a^5,\hfill \end{array}$$

(15)

where the second row is obtained with the use of

$$\frac{d{c}_{T_i}\left(\right|{\alpha }_i\left|\right)}{d{\alpha }_i}=sign\left({\alpha }_i\right)\frac{d{c}_{T_i}\left({\alpha }_i\right)}{d{\alpha }_i}.$$

(16)

2.3. Actuator Faults and Control Input Definition

A total of eight control inputs are considered, which are the pitch angles ${\alpha }_i$ and the squares of the desired rotor speeds, denoted as $u_i$, for $i=1,\dots ,4$. All the control inputs are collapsed in the vectors

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \alpha & =\mathrm{col}({\alpha }_1,{\alpha }_2,{\alpha }_3,{\alpha }_4),\hfill \\ \hfill u& =\mathrm{col}(u_1,u_2,u_3,u_4).\hfill \end{array}\end{array}$$

(17)

In the absence of faults, the relation ${\mathsf{\Omega }}_i^2=u_i$ holds. For each actuator i, a possible LOE is considered, so that the resulting angular speed may be lower than the desired one, i.e.,

$$\begin{array}{c}\hfill {\mathsf{\Omega }}_i^2=w_i{u}_i.\end{array}$$

(18)

Each $w_i$, for $i=1,\dots ,4$, is an unknown and time-varying quantity which models the fault magnitude on the i-th actuator. It is constrained in $w_i\in [0,1]$, where $w_i=1$ corresponds to the faultless scenario. For the sake of brevity, the fault magnitudes are collected in the vector

$$\begin{array}{c}\hfill w=\mathrm{col}(w_1,w_2,w_3,w_4).\end{array}$$

(19)

3. Wind and Fault Estimation

We assume the wind acts on the $x_E-y_E$ plane only, that is, $F_w^E=f_{wx}{e}_1+f_{wy}{e}_2$. In other words, we neglect the updraft, and we claim that the horizontal wind does not generate any significant torque due to the symmetry of the vehicle. Thus, six target independent variables need to be estimated; these are $f_{wx},f_{wy}$ and $w_1,w_2,w_3,w_4$, and they are summarized in the extended uncertainty vector $\overline{w}=\mathrm{col}(f_{wx},f_{wy},w_1,w_2,w_3,w_4)$. By handling the joint vector $\overline{w}$ as an external disturbance, a disturbance observer for the estimation $\widehat{\overline{w}}$ of $\overline{w}$ is then designed, which in our purpose can be defined as a dynamical system in the form

$$\begin{array}{cc}\hfill \dot{\widehat{s}}& =F(\widehat{s},\overline{x},\overline{u})\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill \widehat{\overline{w}}& =H(\widehat{s},\overline{x},\overline{u}),\hfill \end{array}$$

(21)

for which $∥\widehat{\overline{w}}-\overline{w}∥\to 0$ as $t\to \infty$, and where $\widehat{s}$ is the observer state, $\overline{x}=\mathrm{col}(p_F,{\dot{p}}_F,\omega ,\eta )$ and $\overline{u}=\mathrm{col}(u,\alpha )$ compactly refers to the quadrotor state and the control inputs, and where $F(·)$ and $H(·)$ are smooth function to design. Several disturbance observers have been proposed in the literature. Here, we follow the NDO proposed in [24], which can be summarized as follows.

• Define an auxiliary variable s which directly depends on the estimation target $\overline{w}$. Usually, the auxiliary variable $s=\overline{w}-\lambda (\overline{x},\overline{u})$ is chosen, for some smooth function $\lambda (·)$.
• Find the dynamics $\dot{s}$ of the auxiliary variable s. In this step, the disturbance observer design problem is turned into a state observer design problem.
• Based on the auxiliary dynamics $\dot{s}$, design a state observer for the estimation $\widehat{s}$ of s.
• Define $\widehat{\overline{w}}$ by using the inverse function of the chosen auxiliary variable definition.

For design purposes, the model is conveniently rewritten in Section 3.1. Then, the design of the NDO is proposed in Section 3.2.

Remark 1. 

The estimation of the actuator faults and the external force generated by the wind is obtained by the disturbance observer, where only kinematic data are required, i.e., without the need of measuring motor speeds, motor currents, etc.

Remark 2. 

The design of $\lambda (·)$ involves the solution of a set of partial differential equations.

Remark 3. 

If the dynamics of $\overline{w}$ is uncertain, the ultimate boundedness of $∥\widehat{\overline{w}}-\overline{w}∥$ is demanded instead of convergence.

3.1. Model Manipulation

Note that the input mapping (12) and the action of the fault (18) can be rewritten as

$$\left[\begin{array}{c}{u}_z\\ {\tau }_p\\ {\tau }_q\\ {\tau }_r\end{array}\right]=\left[\begin{array}{cccc}{c}_{L_1}\left({\alpha }_1\right)& c_{L_2}\left({\alpha }_2\right)& c_{L_3}\left({\alpha }_3\right)& c_{L_4}\left({\alpha }_4\right)\\ 0& -l{c}_{L_2}\left({\alpha }_2\right)& 0& l{c}_{L_4}\left({\alpha }_4\right)\\ -l{c}_{L_1}\left({\alpha }_1\right)& 0& l{c}_{L_3}\left({\alpha }_3\right)& 0\\ -c_{D_1}\left({\alpha }_1\right)& c_{D_2}\left({\alpha }_2\right)& -c_{D_3}\left({\alpha }_3\right)& c_{D_4}\left({\alpha }_4\right)\end{array}\right]\left[\begin{array}{c}{\mathsf{\Omega }}_1^2\\ {\mathsf{\Omega }}_2^2\\ {\mathsf{\Omega }}_3^2\\ {\mathsf{\Omega }}_4^2\end{array}\right]\triangleq B_c\left(\alpha \right)\left[\begin{array}{c}{\mathsf{\Omega }}_1^2\\ {\mathsf{\Omega }}_2^2\\ {\mathsf{\Omega }}_3^2\\ {\mathsf{\Omega }}_4^2\end{array}\right]$$

(22)

and

$$\left[\begin{array}{c}{\mathsf{\Omega }}_1^2\\ {\mathsf{\Omega }}_2^2\\ {\mathsf{\Omega }}_3^2\\ {\mathsf{\Omega }}_4^2\end{array}\right]=\left[\begin{array}{cccc}{w}_1& 0& 0& 0\\ 0& w_2& 0& 0\\ 0& 0& w_3& 0\\ 0& 0& 0& w_4\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\\ u_4\end{array}\right]=\mathrm{diag}\left(w\right)u=\mathrm{diag}\left(u\right)w,$$

(23)

resulting in the overall relation $\mathrm{col}(u_z,{\tau }_m^B)=B_c\left(\alpha \right)\mathrm{diag}\left(u\right)w$.

The quadrotor kinematics and dynamics in (5) can be equivalently expressed as

$$\frac{d}{dt}\left[\begin{array}{c}{p}_F\\ \eta \end{array}\right]=\left[\begin{array}{c}{\dot{p}}_F\\ T\left(\eta \right)\omega \end{array}\right]$$

(24)

$$\frac{d}{dt}\left[\begin{array}{c}{\dot{p}}_F\\ \omega \end{array}\right]=\left[\begin{array}{c}-\frac{k_t}{m}{\dot{p}}_F-g{e}_3\\ -k_r{J}^{-1}\omega -J^{-1}(\omega \times J\omega )\end{array}\right]+\frac{1}{m}\left[\begin{array}{cc}{e}_1& e_2\\ 0& 0\end{array}\right]\left[\begin{array}{c}{f}_{wx}\\ f_{wy}\end{array}\right]+\left[\begin{array}{cc}\frac{1}{m}R{e}_3& 0\\ 0& J^{-1}\end{array}\right]\left[\begin{array}{c}{u}_z\\ {\tau }_m^B\end{array}\right].$$

(25)

Then, defining $z_1=\mathrm{col}(p_F,\eta )$ and $z_2=\mathrm{col}({\dot{p}}_F,\omega )$, we can rewrite the overall quadrotor model in order to show that the non-linear model is affine with respect to the uncertainty $\overline{w}$. In fact, we have

$$\begin{array}{cc}\hfill {\dot{z}}_1& =h_1(z_1,z_2)\hfill \\ \hfill {\dot{z}}_2& =h_2\left(z_2\right)+P(z_1,u,\alpha )\overline{w},\hfill \end{array}$$

(26)

where

$$h_1(z_1,z_2)=\left[\begin{array}{c}{\dot{p}}_F\\ T\left(\eta \right)\omega \end{array}\right],h_2\left(z_2\right)=\left[\begin{array}{c}-\frac{k_t}{m}{\dot{p}}_F-g{e}_3\\ -k_r{J}^{-1}\omega -J^{-1}(\omega \times J\omega )\end{array}\right],$$

(27)

and $P(z_1,u,\alpha )=\left[\begin{array}{cc}{P}_1& P_2(z_1,u,\alpha )\end{array}\right]\in {\mathbb{R}}^{6\times 6}$, with

$$P_1=\frac{1}{m}\left[\begin{array}{cc}{e}_1& e_2\\ 0& 0\end{array}\right]\in {\mathbb{R}}^{6\times 2},P_2(z_1,u,\alpha )=\left[\begin{array}{cc}\frac{1}{m}R{e}_3& 0\\ 0& J^{-1}\end{array}\right]B_c\left(\alpha \right)\mathrm{diag}\left(u\right)\in {\mathbb{R}}^{6\times 4}.$$

(28)

Finally, P is almost everywhere nonsingular, since

$$\begin{array}{cc}\hfill det\left(P\right)& =\frac{1}{m^2}det\left(\left[\begin{array}{cc}\frac{1}{m}cos\left(\phi \right)cos\left(\theta \right)& 0\\ 0& J^{-1}\end{array}\right]B_c\left(\alpha \right)\mathrm{diag}\left(u\right)\right)\hfill \\ \hfill & =u_1{u}_2{u}_3{u}_4\frac{cos\left(\phi \right)cos\left(\theta \right)}{m^3{J}_x{J}_y{J}_z}det\left(B_c\left(\alpha \right)\right),\hfill \end{array}$$

(29)

where $det\left(B_c\left(\alpha \right)\right)\ne 0$ almost everywhere (assuming $l>0$). Noting that each $u_i$ is positive in real flight conditions, $cos\left(\phi \right)cos\left(\theta \right)$ is different from zero in non-aerobatic flight, and $det\left(B_c\left(\alpha \right)\right)\ne 0$, then the inverse of P is well-defined in practical flight conditions.

3.2. Disturbance Observer Design

The NDO is designed on model (26). Indeed, the NDO design in DOBC is well-known for control affine systems [25], so the idea is actually based on the affine uncertainty, which is $\overline{w}$ in our problem. Consider the auxiliary variable $s=\overline{w}-\lambda (z_1,z_2,u,\alpha )$, where $\lambda (·)$ is a function to design. For the sake of brevity, we denote $\lambda =\lambda (z_1,z_2,u,\alpha )$. By differentiation, we have

$$\begin{array}{cc}\hfill \dot{s}& =\dot{\overline{w}}-\frac{\partial \lambda }{\partial z_1}{h}_1(z_1,z_2)-\frac{\partial \lambda }{\partial z_2}\left(h_2\left(z_2\right)+P(z_1,u,\alpha )\overline{w}\right)-\frac{\partial \lambda }{\partial u}\dot{u}-\frac{\partial \lambda }{\partial u}\dot{\alpha }\hfill \\ \hfill & =\dot{\overline{w}}-\frac{\partial \lambda }{\partial z_1}{h}_1(z_1,z_2)-\frac{\partial \lambda }{\partial z_2}\left(h_2\left(z_2\right)+P(z_1,u,\alpha )(s+\lambda )\right)-\frac{\partial \lambda }{\partial u}\dot{u}-\frac{\partial \lambda }{\partial u}\dot{\alpha }\hfill \\ \hfill & =-\frac{\partial \lambda }{\partial z_2}P(z_1,u,\alpha )s+R_s+\dot{\overline{w}},\hfill \end{array}$$

(30)

where $R_s$ is a known term given by

$$R_s=-\frac{\partial \lambda }{\partial z_1}{h}_1(z_1,z_2)-\frac{\partial \lambda }{\partial z_2}\left(h_2\left(z_2\right)+P(z_1,u,\alpha )\lambda \right)-\frac{\partial \lambda }{\partial u}\dot{u}-\frac{\partial \lambda }{\partial u}\dot{\alpha }.$$

(31)

The disturbance observer is then designed as

$$\begin{array}{cc}\hfill \dot{\widehat{s}}& =-\frac{\partial \lambda }{\partial z_2}P(z_1,u,\alpha )\widehat{s}+R_s\hfill \end{array}$$

(32)

$$\begin{array}{cc}\hfill \widehat{\overline{w}}& =\widehat{s}+\lambda ,\hfill \end{array}$$

(33)

where $\widehat{s}$ and $\widehat{\overline{w}}$ are estimations for s and $\overline{w}$, respectively. The function $\lambda (·)$ is investigated through the estimation error dynamics. On this purpose, let us consider the error variables $\tilde{s}=s-\widehat{s}$ and $\tilde{\overline{w}}=\overline{w}-\tilde{\overline{w}}$. The error dynamics is

$$\begin{array}{cc}\hfill \dot{\tilde{s}}& =-\frac{\partial \lambda }{\partial z_2}P(z_1,u,\alpha )\widehat{s}+\dot{\overline{w}}\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill \tilde{\overline{w}}& =\tilde{s}.\hfill \end{array}$$

(35)

With the choice $\lambda (z_1,z_2,u,\alpha )=HP{(z_1,u,\alpha )}^{-1}{z}_2$, where $-H$ is Hurwitz, the overall estimation error boils down to

$$\dot{\tilde{\overline{w}}}=-H\tilde{\overline{w}}+\dot{\overline{w}},$$

(36)

which is an asymptotically stable linear time-invariant system, perturbed by the unknown input $\dot{\overline{w}}$.

4. Control Law Design

A classical inner/outer loop approach is employed to calculate the desired virtual inputs [26,27], where the small angle approximation is adopted ($\phi \approx 0$ and $\theta \approx 0$). Moreover, in order to avoid a conflict between the inner and the outer loop, a spectral separation is required; to achieve this, in practice, a higher rate is employed for the inner loop with respect to the outer loop [28].

4.1. Outer Loop Design

Given the time-varying position references $x_{F_r}$ and $y_{F_r}$ (for $x_F$ and $y_F$, respectively), the outer loop is designed to calculate the references ${\phi }_r$ and ${\theta }_r$ (for the angles $\phi$ and $\theta$, respectively) in order to track the position reference. Exploiting the small angle approximation [26,27], that is, $\phi \approx 0$ and $\theta \approx 0$, we have

$$\left[\begin{array}{c}{e}_1^T\\ e_2^T\end{array}\right]R{e}_3\approx \left[\begin{array}{cc}sin\left(\psi \right)& cos\left(\psi \right)\\ -cos\left(\psi \right)& sin\left(\psi \right)\end{array}\right]\left[\begin{array}{c}\phi \\ \theta \end{array}\right]\triangleq R_2\left(\psi \right)\left[\begin{array}{c}\phi \\ \theta \end{array}\right].$$

(37)

Substituting (37) in (5), it follows that

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{\ddot{x}}_F\\ {\ddot{y}}_F\end{array}\right]& =\left[\begin{array}{c}{e}_1^T\\ e_2^T\end{array}\right]{\ddot{p}}_F=-\frac{k_t}{m}\left[\begin{array}{c}{\dot{x}}_F\\ {\dot{y}}_F\end{array}\right]+\frac{u_z}{m}\left[\begin{array}{c}{e}_1^T\\ e_2^T\end{array}\right]R{e}_3+\frac{1}{m}\left[\begin{array}{c}{f}_{wx}\\ f_{wy}\end{array}\right]\hfill \\ \hfill & \approx -\frac{k_t}{m}\left[\begin{array}{c}{\dot{x}}_F\\ {\dot{y}}_F\end{array}\right]+\frac{u_z}{m}{R}_2\left(\psi \right)\left[\begin{array}{c}\phi \\ \theta \end{array}\right]+\frac{1}{m}\left[\begin{array}{c}{f}_{wx}\\ f_{wy}\end{array}\right].\hfill \end{array}$$

(38)

As the inner loop runs at a higher rate with respect to the outer loop, we can assume the inner loop is fast enough to track ${\phi }_r$ and ${\theta }_r$ [26]. Hence, by solving (38) for the variables $\phi$ and $\theta$, we obtain the ideal references ${\phi }_r$ and ${\theta }_r$ for $u_z\ne 0$, that is,

$$\left[\begin{array}{c}{\phi }_r\\ {\theta }_r\end{array}\right]=\frac{m}{u_z}{R}_2^T\left(\phi \right)\left(\frac{k_t}{m}\left[\begin{array}{c}{\dot{x}}_F\\ {\dot{y}}_F\end{array}\right]-\frac{1}{m}\left[\begin{array}{c}{\widehat{f}}_{wx}\\ {\widehat{f}}_{wy}\end{array}\right]+\left[\begin{array}{c}{v}_x\\ v_y\end{array}\right]\right),$$

(39)

where $v_x$ and $v_y$ are injection inputs, defined as

$$\begin{array}{cc}\hfill v_x& ={\ddot{x}}_{Fr}-{\alpha }_{x1}({\dot{x}}_F-{\dot{x}}_{Fr})-{\alpha }_{x0}(x_F-x_{Fr})\hfill \end{array}$$

(40)

$$\begin{array}{cc}\hfill v_y& ={\ddot{y}}_{Fr}-{\alpha }_{y1}({\dot{y}}_F-{\dot{y}}_{Fr})-{\alpha }_{y0}(y_F-y_{Fr}).\hfill \end{array}$$

(41)

The parameters ${\alpha }_{x1},{\alpha }_{x0},{\alpha }_{y1},{\alpha }_{y0}$ are chosen according to the desired pole placement problem. In fact, if the angles $\phi$ and $\theta$ follow references (39)–(41), we can show by substitution in (5) that the outer loop tracking error dynamics becomes:

$$\begin{array}{cc}\hfill {\ddot{e}}_x+{\alpha }_{x1}{\dot{e}}_x+{\alpha }_{x_0}{e}_x& =\frac{1}{m}{\tilde{f}}_{wx}\hfill \end{array}$$

(42)

$$\begin{array}{cc}\hfill {\ddot{e}}_y+{\alpha }_{y1}{\dot{e}}_x+{\alpha }_{y_0}{e}_y& =\frac{1}{m}{\tilde{f}}_{wy},\hfill \end{array}$$

(43)

where $e_x=x_F-x_{Fr}$, $e_y=y_F-y_{Fr}$, ${\tilde{f}}_{wx}=f_{wx}-{\widehat{f}}_{wx}$ and ${\tilde{f}}_{wy}=f_{wy}-{\widehat{f}}_{wy}$.

4.2. Inner Loop Design

Let $z_{Fr}$ and ${\psi }_r$ be the reference signals for $z_F$ and $\psi$ that we want to track, and let ${\phi }_r$ and ${\theta }_r$ be the reference signals for $\phi$ and $\theta$ (provided by the outer loop in (39)). From (5), it follows that

$$\begin{array}{cc}\hfill {\ddot{z}}_F& =-\frac{k_t}{m}{\dot{z}}_F-g+\frac{u_z}{m}cos\left(\phi \right)cos\left(\theta \right)\hfill \end{array}$$

(44)

$$\begin{array}{cc}\hfill \ddot{\eta }& =\dot{T}\left(\eta \right)\omega -k_{r}T\left(\eta \right)J^{-1}\omega -T\left(\eta \right)J^{-1}(\omega \times J\omega )+T\left(\eta \right)J^{-1}{\tau }_m^B,\hfill \end{array}$$

(45)

so we can set the reference values $u_{z,r}$ and ${\tau }_{m,r}^B$ (for $u_z$ and ${\tau }_m^B$, respectively) as

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill u_{z,r}& =\frac{m}{cos\left(\phi \right)cos\left(\theta \right)}\left(\frac{k_t}{m}{\dot{z}}_F+g+v_z\right)\hfill \\ \hfill {\tau }_{m,r}^B& =JT{\left(\eta \right)}^{-1}\left(-\dot{T}\left(\eta \right)\omega +k_{r}T\left(\eta \right)J^{-1}\omega +T\left(\eta \right)J^{-1}(\omega \times J\omega )+v_{\eta }\right).\hfill \end{array}\end{array}$$

(46)

The terms $v_z\in \mathbb{R}$ and $v_{\eta }\in {\mathbb{R}}^3$ are auxiliary inputs, and they are set as

$$\begin{array}{cc}\hfill v_z& ={\ddot{z}}_{Fr}-{\alpha }_{z1}({\dot{z}}_F-{\dot{z}}_{Fr})-{\alpha }_{z0}(z_F-z_{Fr})\hfill \end{array}$$

(47)

$$\begin{array}{cc}\hfill v_{\eta }& ={\ddot{\eta }}_r-{\alpha }_{\eta 1}(T\left(\eta \right)\omega -{\dot{\eta }}_r)-{\alpha }_{\eta 0}(\eta -{\eta }_r),\hfill \end{array}$$

(48)

where the scalars ${\alpha }_{z1},{\alpha }_{z0}$ and the matrices ${\alpha }_{\eta 1},{\alpha }_{\eta 0}$ are set according to the desired pole placement. If the variables $u_z$ and ${\tau }_m^B$ follow references (46)–(48), we can show by substitution in (5) that the inner loop tracking error dynamics becomes

$$\begin{array}{cc}\hfill {\ddot{e}}_z+{\alpha }_{z1}{\dot{e}}_z+{\alpha }_{z_0}{e}_z& =0\hfill \end{array}$$

(49)

$$\begin{array}{cc}\hfill {\ddot{e}}_{\eta }+{\alpha }_{\eta 1}{\dot{e}}_{\eta }+{\alpha }_{{\eta }_0}{e}_{\eta }& =0,\hfill \end{array}$$

(50)

where $e_z=z_F-z_{Fr}$ and $e_{\eta }=\eta -{\eta }_r$.

5. Control Allocation

In this section, we show how the virtual input (i.e., $u_z$ and ${\tau }_m^B$) can be distributed among the motors (i.e., u) and the pitch servos (i.e., $\alpha$). Let us consider a non-linear optimization problem in the optimization variables $\alpha$ and u:

$$\begin{array}{cc}\hfill \underset{\alpha ,u}{min}& J(\alpha ,u)\hfill \end{array}$$

(51)

$$\begin{array}{cc}\hfill s.t.& h(\alpha ,u)=0\hfill \end{array}$$

(52)

$$\begin{array}{cc}\hfill & g(\alpha ,u)\le 0,\hfill \end{array}$$

(53)

where $J(\alpha ,u)$ in (51) is a cost function to be determined, while (52) and (53) represent generic non-linear and nonconvex constraints. In the literature, the control allocation problem in multirotors is usually tackled with lightweight methods, such as pseudo-inverse and redistributed pseudo-inverse [29,30]. Such methods are motivated by the need for high rates, but they are not effective in the presence of constraints and rate limits. Moreover, they need a significant simplification of the problem formulation (i.e., linear or quadratic cost functions, linear equality constraints, potentially also removing inequality constraints). In the following, we set up the control allocation problem and we detail a method to solve it efficiently by means of a locally convex Quadratic Programming (QP) reformulation (as carried out, for example, in [31] for a marine vessel).

The method that is detailed in this section to solve the non-linear allocation problem (51)–(53) represents a simplified version of the generic Sequential Quadratic Programming (SQP) strategy (see, for example, [32]) to solve non-linear optimization problems. The main differences are

• SQP employs the Hessian of the Lagrangian in the cost function, while we employ the Hessian of the original cost function. In other words, we always set the Lagrange multipliers’ initial guess to zero for simplicity, so that the Hessian of the Lagrangian coincides with the Hessian of the original cost function.
• Once the search direction is found, a full step is performed in the proposed method, and no merit function is computed.
• The stopping criterion in the proposed method boils down to performing just a single iteration (i.e., a single QP is solved as in Real-Time Iteration [33], instead of a sequence of QP problems as done in SQP), and so no convergence criteria are needed.

These modifications introduce some loss of accuracy in the solution, but they reduce the number of evaluations, in order to perform the allocation faster in view of online implementation.

5.1. Equality Constraints

The equality constraints (52) originate from (22)–(23), namely

$$h(\alpha ,u)=B\left[\begin{array}{c}\mathrm{diag}\left(w\right)\mathrm{diag}\left({\overline{c}}_L\left(\alpha \right)\right)u\\ \mathrm{diag}\left(w\right)\mathrm{diag}\left({\overline{c}}_D\left(\alpha \right)\right)u\end{array}\right]-\left[\begin{array}{c}{u}_z\\ {\tau }_m^B\end{array}\right]$$

(54)

$$\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}=B\left[\begin{array}{c}\mathrm{diag}\left(w\right)\mathrm{diag}\left(u\right){\overline{c}}_L\left(\alpha \right)\\ \mathrm{diag}\left(w\right)\mathrm{diag}\left(u\right){\overline{c}}_D\left(\alpha \right)\end{array}\right]-\left[\begin{array}{c}{u}_z\\ {\tau }_m^B\end{array}\right]=0,$$

(55)

where

$$B=\left[\begin{array}{cccccccc}1& 1& 1& 1& 0& 0& 0& 0\\ 0& -l& 0& l& 0& 0& 0& 0\\ -l& 0& l& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& -1& 1& -1& 1\end{array}\right]$$

(56)

and

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\overline{c}}_L\left(\alpha \right)& =\mathrm{col}(c_L\left({\alpha }_1\right),c_L\left({\alpha }_2\right),c_L\left({\alpha }_3\right),c_L\left({\alpha }_4\right))\hfill \\ \hfill {\overline{c}}_D\left(\alpha \right)& =\mathrm{col}(c_D\left({\alpha }_1\right),c_D\left({\alpha }_2\right),c_D\left({\alpha }_3\right),c_D\left({\alpha }_4\right))\hfill \\ \hfill {\overline{c}}_L^{\prime }\left(\alpha \right)& =\mathrm{col}(c_L^{\prime }\left({\alpha }_1\right),c_L^{\prime }\left({\alpha }_2\right),c_L^{\prime }\left({\alpha }_3\right),c_L^{\prime }\left({\alpha }_4\right))\hfill \\ \hfill {\overline{c}}_D^{\prime }\left(\alpha \right)& =\mathrm{col}(c_D^{\prime }\left({\alpha }_1\right),c_D^{\prime }\left({\alpha }_2\right),c_D^{\prime }\left({\alpha }_3\right),c_D^{\prime }\left({\alpha }_4\right)).\hfill \end{array}\end{array}$$

(57)

Please note that $c_L\left({\alpha }_i\right)$ and $c_D\left({\alpha }_i\right)$ have been defined in (8), while $c_L^{\prime }\left({\alpha }_i\right)$ and $c_D^{\prime }\left({\alpha }_i\right)$ have been defined in (15). The time-varying numeric values for $u_z$, ${\tau }_m^B$, and w in (54) are given by (46) ($u_{z,r}$, ${\tau }_{m,r}^B$) and (33) ($\widehat{\overline{w}}$), respectively; in other words, the actual lift force and torques must satisfy those required by the inner loop, also taking into account the actual fault estimation.

Please note that the equality constraints are non-linear and nonconvex with respect to the optimization variables $\alpha$ and u. Non-linear optimization problems are hard to solve online due to computational limits, as well as because the existence of the solution is not guaranteed in general. To solve efficiently the problem online, the equality constraints (52) are linearized, as in SQP, so that a QP problem can be obtained. Using the first-order Taylor approximation around $({\alpha }_0,u_0)$, we have

$$h(\alpha ,u)\approx h({\alpha }_0,u_0)+\frac{\partial h({\alpha }_0,u_0)}{\partial \alpha }(\alpha -{\alpha }_0)+\frac{\partial h({\alpha }_0,u_0)}{\partial u}(u-u_0),$$

(58)

where ${\alpha }_0$ and $u_0$ represent the current value of $\alpha$ and u, respectively. Then, (54) can be approximated by

$$\underset{A_{eq}}{\underbrace{\left[\frac{\partial h({\alpha }_0,u_0)}{\partial \alpha }\frac{\partial h({\alpha }_0,u_0)}{\partial u}\right]}}\underset{x}{\underbrace{\left[\begin{array}{c}\mathsf{\Delta }\alpha \\ \mathsf{\Delta }u\end{array}\right]}}=\underset{b_{eq}}{\underbrace{-h({\alpha }_0,u_0)}},$$

(59)

where $\mathsf{\Delta }\alpha =\alpha -{\alpha }_0$, $\mathsf{\Delta }u=u-u_0$, and

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \frac{\partial h({\alpha }_0,u_0)}{\partial \alpha }& =B\left[\begin{array}{c}\mathrm{diag}\left(w\right)\mathrm{diag}\left(u_0\right)\mathrm{diag}\left(c_L^{\prime }\right)\\ \mathrm{diag}\left(w\right)\mathrm{diag}\left(u_0\right)\mathrm{diag}\left(c_D^{\prime }\right)\end{array}\right]\hfill \\ \hfill \frac{\partial h({\alpha }_0,u_0)}{\partial u}& =B\left[\begin{array}{c}\mathrm{diag}\left(w\right)\mathrm{diag}\left({\overline{c}}_L\left({\alpha }_0\right)\right)\\ \mathrm{diag}\left(w\right)\mathrm{diag}\left({\overline{c}}_D\left({\alpha }_0\right)\right)\end{array}\right]\hfill \\ \hfill h({\alpha }_0,u_0)& =B\left[\begin{array}{c}\mathrm{diag}\left(w\right)\mathrm{diag}\left({\overline{c}}_L\left({\alpha }_0\right)\right)u_0\\ \mathrm{diag}\left(w\right)\mathrm{diag}\left({\overline{c}}_D\left({\alpha }_0\right)\right)u_0\end{array}\right]-\left[\begin{array}{c}{u}_z\\ {\tau }_m^B\end{array}\right].\hfill \end{array}\end{array}$$

(60)

As w is not available, to build the control allocation problem online we replace it in (60) with its estimation contained in $\widehat{\overline{w}}$.

Remark 4. 

In the case of a centralized motor, it suffices to add the constraint $\mathsf{\Delta }{u}_1=\mathsf{\Delta }{u}_2=\mathsf{\Delta }{u}_3=\mathsf{\Delta }{u}_4$; provided that the same initial value is employed for u, then $u_1=u_2=u_3=u_4$ holds. In other words, (59) is replaced by

$$\underset{A_{eq}}{\underbrace{\left[\begin{array}{cc}{\displaystyle \frac{\partial h({\alpha }_0,u_0)}{\partial \alpha }}& {\displaystyle \frac{\partial h({\alpha }_0,u_0)}{\partial u}}\\ \begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}& \begin{array}{cccc}1& \mathrm{-1}& 0& 0\\ 0& 1& \mathrm{-1}& 0\\ 0& 0& 1& \mathrm{-1}\end{array}\end{array}\right]}}\underset{x}{\underbrace{\left[\begin{array}{c}\begin{array}{c}\mathsf{\Delta }\alpha \\ \mathsf{\Delta }u\end{array}\end{array}\right]}}=\underset{b_{eq}}{\underbrace{\left[\begin{array}{c}-h({\alpha }_0,u_0)\\ 0\\ 0\\ 0\end{array}\right]}}$$

(61)

5.2. Inequality Constraints

The inequality constraints of interest to be included in (53), in practice, are saturation constraints and rate limits, i.e.,

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \underline{\alpha }& \le \alpha \le \overline{\alpha }\hfill \\ \hfill \underline{u}& \le u\le \overline{u}\hfill \\ \hfill \underline{\mathsf{\Delta }\alpha }& \le \alpha -{\alpha }_0\le \stackrel{̲}{\mathsf{\Delta }\alpha }\hfill \\ \hfill \underline{\mathsf{\Delta }u}& \le u-u_0\le \stackrel{̲}{\mathsf{\Delta }u}.\hfill \end{array}\end{array}$$

(62)

Such inequality constraints are affine with respect to the optimization variables $\alpha$ and u, so they are tractable in online optimization.

Replacing (52) with its approximation (59) (or (61), for a single centralized motor) and expressing the constraints (62) in the variable $x=\mathrm{col}(\mathsf{\Delta }\alpha ,\mathsf{\Delta }u)$, we obtain an optimization problem in the form

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \underset{x}{min}& J\left(x\right)\hfill \\ \hfill s.t.& A_{eq}x=b_{eq}\hfill \\ \hfill & \underline{x}\le x\le \overline{x},\hfill \end{array}\end{array}$$

(63)

where

$$\begin{array}{c}\hfill \begin{array}{c}\hfill \underline{x}=\mathrm{col}(max(\underline{\alpha }-{\alpha }_0,\underline{\mathsf{\Delta }\alpha }),max(\underline{u}-u_0,\underline{\mathsf{\Delta }u}))\\ \hfill \overline{x}=\mathrm{col}(min(\overline{\alpha }-{\alpha }_0,\stackrel{̲}{\mathsf{\Delta }\alpha }),min(\overline{u}-u_0,\stackrel{̲}{\mathsf{\Delta }u}))\end{array}\end{array}$$

(64)

are defined to express the inequality constraints (62) in a more compact way. Please note that (63) is a QP problem by construction, provided that the cost function $J\left(x\right)$ is quadratic.

5.3. Cost Function

The main term to be considered in a cost function for UAVs, especially in the case of multirotors, is the energy consumption $E\left(x\right)$. A quadratic form for $E\left(x\right)$ is often assumed in the UAV literature [29,30]; such a choice is mainly motivated by simplicity, as it usually leads to a QP problem. Motivated by the need for autonomy in UAVs, more detailed representations, coming from physically accurate consumption models, have been proposed in the literature as well. The most employed models are second-order polynomials [9] and irrational functions [34], as well as more generic non-linear functions [6]. All of the previous choices fit well with the proposed framework, as it is always possible to locally approximate the cost function with a quadratic cost function, as done for example in [31]. For the purpose of this work, the steady-state thruster consumption from [34] is employed. Given a generic matrix $M=\left[m_{ij}\right]\in {\mathbb{R}}^{m\times n}$ and $k\in \mathbb{R}$, let us introduce, with a slight abuse of notation, the short ${\left|M\right|}^k=\left[\right|m_{ij}{|}^k]$ (i.e., the k-th power of the absolute values of the entries of M, in an element-wise fashion). Then, the steady-state thruster consumption for a quadrotor can be rewritten as

$$E\left(x\right)=\sum _{i=1}^4{q}_{u,i}{\left|u_i\right|}^{3/2}=|u^T{|}^{3/4}{Q}_u{\left|u\right|}^{3/4},$$

(65)

where ${\left|u\right|}^{3/4}$ and $|u^T{|}^{3/4}$ should be intended element-wise as already anticipated. Furthermore, $q_{u,i}$ is assumed to be constant for simplicity (see [34] for further details) and

$$Q_u=\mathrm{diag}(q_{u,1},\dots ,q_{u,4}),$$

(66)

with $q_{u,i}>0$ for $i=1,\dots ,4$. Hence, we can define the cost function

$$J\left(x\right)=E\left(x\right)+\mathsf{\Delta }{\alpha }^T{Q}_{\mathsf{\Delta }\alpha }\mathsf{\Delta }\alpha +\mathsf{\Delta }{u}^T{Q}_{\mathsf{\Delta }u}\mathsf{\Delta }u+{(\alpha -{\alpha }_p)}^T{Q}_{\alpha }(\alpha -{\alpha }_p),$$

(67)

where $Q_{\mathsf{\Delta }\alpha }$, $Q_{\mathsf{\Delta }u}$, $Q_{\alpha }$, $Q_u$ are symmetric positive definite matrices. The first term of (67), which takes into account energy consumption, is detailed in (65). The second and the third quadratic terms of (67) discourage variations in the control inputs to minimize actuator wear and tear. The last term of (67) defines preferred values ${\alpha }_p$ for the pitch angles $\alpha$. This term becomes necessary in order to compensate for the first term (i.e., the one that discourages energy consumption), because the actuator is more energy-efficient when the pitch angle is large in magnitude. Thus, in the absence of the fourth term, the pitch angle is often stuck to its saturation limit to minimize energy consumption. In such a case, the only way to increase the lift further is to increase the motor speed, as the pitch angle cannot be increased in magnitude, so the maneuverability of the vehicle worsens.

Considering the second-order Taylor approximation for $E\left(x\right)$, i.e.,

$$E\left(x\right)\approx |u_0^T{|}^{3/4}{Q}_u|u_0{|}^{3/4}+\frac{3}{2}{\left|u_0^T\right|}^{1/2}{Q}_u\mathsf{\Delta }u+\frac{3}{8}\mathsf{\Delta }{u}^T{Q}_u\mathrm{diag}\left(|u_0{|}^{-1/2}\right)\mathsf{\Delta }u,$$

(68)

and performing some manipulation, (67) becomes

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill J\left(x\right)=& \frac{1}{2}{x}^T\underset{H}{\underbrace{\left[\begin{array}{cc}2(Q_{\alpha }+Q_{\mathsf{\Delta }\alpha })& 0_{4\times 4}\\ 0_{4\times 4}& 2Q_{\mathsf{\Delta }u}+\frac{3}{4}{Q}_u\mathrm{diag}\left(|u_0{|}^{-1/2}\right)\end{array}\right]}}x\hfill \\ \hfill & +\underset{f^T}{\underbrace{\left[\begin{array}{cc}2{({\alpha }_0-{\alpha }_p)}^T{Q}_{\alpha }& \frac{3}{2}{\left(|u_0{|}^{1/2}\right)}^T{Q}_u\end{array}\right]}}x+c,\hfill \end{array}\end{array}$$

(69)

where c is a constant term (i.e., a term that does not depend on x), which is irrelevant for the solution of the optimization problem, so it can be neglected.

5.4. Scaling and Infeasibility

The problem formulation according to (59) (or (61), for a single centralized motor), (64), (69), that is,

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \underset{x}{min}& \frac{1}{2}{x}^{T}Hx+f^{T}x\hfill \\ \hfill s.t.& A_{eq}x=b_{eq}\hfill \\ \hfill & \underline{x}\le x\le \overline{x},\hfill \end{array}\end{array}$$

(70)

shows two drawbacks. First of all, the existence of a feasible solution is not guaranteed, so problem infeasibility must be dealt with. Moreover, the problem is ill-conditioned, because the magnitude of $\mathsf{\Delta }\alpha$ and $\mathsf{\Delta }u$ is very different; according to (17)–(18), $\alpha$ is typically in the range of decimals (in radians), while u is typically in the range of tens of thousands (in radians per second squared), and the same occurs for $\mathsf{\Delta }\alpha$ and $\mathsf{\Delta }u$.

To improve the problem conditioning, diagonal scaling is performed [32]. We define a new optimization variable

$$\begin{array}{c}\hfill z=P^{-1}x,\end{array}$$

(71)

where $P=\mathrm{diag}(max(|\underline{x}|,|\overline{x}\left|\right))$ is a diagonal matrix such that the components of z share the same magnitude, as they become constrained in $[-1,1]$ according to the rate limits in (62) (please note that the box constraint estimation $[-1,1]$ is conservative; tighter constraints may apply).

In order to deal with infeasible problems, we soften the equality constraint by adding a vector of unconstrained artificial variables $\zeta$ into the equality constraint, while minimizing the norm of the artificial variable. The overall problem (70), after the change of variables and the softening of equality constraints, becomes

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \underset{z,\zeta }{min}& \frac{1}{2}{z}^T{P}^{T}HPz+f^{T}Pz+{\zeta }^T{Q}_{\zeta }\zeta \hfill \\ \hfill s.t.& A_{eq}Pz+\zeta =b_{eq}\hfill \\ \hfill & P^{-1}\underline{x}\le z\le P^{-1}\overline{x},\hfill \end{array}\end{array}$$

(72)

which can be finally rewritten in the classical form that is usually required by QP solvers, i.e.,

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill y^{*}=\arg \underset{y}{min}& \frac{1}{2}{y}^T\check{H}y+{\check{f}}^{T}y\hfill \\ \hfill s.t.& {\check{A}}_{eq}y={\check{b}}_{eq}\hfill \\ \hfill & \check{A}y\le \check{b}\hfill \\ \hfill & \underline{y}\le y\le \overline{y},\hfill \end{array}\end{array}$$

(73)

where

$$\begin{array}{c}\hfill \begin{array}{ccc}\hfill \begin{array}{c}\hfill y=\left[\begin{array}{c}z\\ \zeta \end{array}\right]\end{array}& \check{H}=\left[\begin{array}{cc}{P}^{T}HP& 0_{8x4}\\ 0_{4x8}& Q_{\zeta }\end{array}\right]\hfill & {\check{f}}^T=\left[\begin{array}{cc}{f}^{T}P& 0_{1x4}\end{array}\right]\\ \hfill {\check{A}}_{eq}=\left[\begin{array}{cc}{A}_{eq}P& I_4\end{array}\right]& {\check{b}}_{eq}=b_{eq}\hfill & \underline{y}=\left[\begin{array}{c}{P}^{-1}\underline{x}\\ \underline{\zeta }\end{array}\right]& \overline{y}=\left[\begin{array}{c}{P}^{-1}\overline{x}\\ \overline{\zeta }\end{array}\right],\end{array}\end{array}$$

(74)

$\underline{\zeta }$ and $\overline{\zeta }$ are large enough to guarantee feasibility (e.g., $\overline{\zeta }=-\underline{\zeta }=M$, $M\to \infty$), while $\check{A}$ and $\check{b}$ are empty matrices, because no inequality constraints are left, with an exception made for the upper and lower bounds that are usually introduced separately.

Once the solution $y^{*}$ of (72)–(74) is calculated, the solution to the control allocation problem is

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill x^{*}& =\left[\begin{array}{c}\mathsf{\Delta }{\alpha }^{*}\\ \mathsf{\Delta }{u}^{*}\end{array}\right]=\left[\begin{array}{cc}P& 0_{8x4}\end{array}\right]y^{*}\hfill \\ \hfill {\alpha }^{*}& ={\alpha }_0+\mathsf{\Delta }{\alpha }^{*}\hfill \\ \hfill u^{*}& =u_0+\mathsf{\Delta }{u}^{*}.\hfill \end{array}\end{array}$$

(75)

Remark 5. 

The choice of a quadratic penalty for ζ is made for simplicity, as it introduces a small number of additional variables and it makes the cost function smooth and hence easier to solve online. This strategy is similar to the quadratic penalty method in [32] and, more precisely, to the weighted least squares in [35]. As a drawback, such a penalty term is not exact, i.e., the solution of the penalized problem is not equivalent to the solution of the original problem [32,36]. In particular, the higher the penalty through $Q_{\zeta }$, the more the solutions of the two problems coincide, but also the worse is the conditioning. Thus, $Q_{\zeta }$ is a trade-off between ideal solution quality and acceptable conditioning. We claim that this issue is not crucial, because several approximations are introduced to simplify the problem, and so the solution is not exact anyway. Alternatively, it is possible to introduce additional positively constrained slack variables and to employ exact penalty functions, such as ${\ell }_1$ and ${\ell }_{\infty }$ penalties, at the price of dealing with the nonsmoothness of the cost function (see [32] for further details).

6. Numerical Simulation

A numerical simulation is performed using MATLAB, where the inner loop and the outer loop run at 400 Hz (i.e., ArduPilot’s loop rate) and 10 Hz, respectively. A zero-order hold is performed between consecutive samples. The quadrotor parameters are set according to [18] and they are reported in

Table 1. Variable-pitch quadrotor parameters.

Parameter	Variable	Value	Unit
Total system mass	m	$1.37$	[kg]
Inertia about $x_B$, $y_B$	$J_x$,$J_y$	7.5 × 10${}^{-3}$	[kg· m${}^2$]
Inertia about $z_B$	$J_z$	1.3 × 10${}^{-2}$	[kg· m${}^2$]
Arm length	l	$0.3$	[m]
Lift curve slope	$c_{l_{\alpha }}$	$5.23$	$[-]$
Zero lift drag coefficient	$c_{d_0}$	$0.01$	$[-]$
Propeller radius	$R_a$	$0.18$	[m]
Propeller chord	c	$0.03$	[m]
Rotor solidity	$\sigma$	$0.106$	[-]
Gravitational acceleration	g	$9.81$	[m/s${}^2$]
Air density	$\rho$	$1.225$	[kg/m${}^3$]
Linear friction coefficient	$k_t$	0	[N· s/m]
Angular friction coefficient	$k_r$	0	[N · s · m]
Minimum pitch angle	${\alpha }_{min}$	$0.05$	[deg]
Maximum pitch angle	${\alpha }_{max}$	15	[deg]
Pitch angle rate limit	$\mathsf{\Delta }{\alpha }_{max}$	60	[deg/s]
Minimum desired square rotor speed	$u_{min}$	0	[rad${}^2$/s${}^2$]
Maximum desired square rotor speed	$u_{max}$	2 × 10${}^5$	[rad${}^2$/s${}^2$]
Motor square speed rate limit	$\mathsf{\Delta }{u}_{max}$	1.6 × 10${}^5$	[rad${}^2$/s${}^3$]

. Different from [18], the friction is neglected and the thrust-to-weight ratio is slightly increased (from $2.5$ to $3.131$) in order to face severe faults.

The control parameters were set empirically through a coarse grid search (as such, better performances could be achieved with a finer tuning). The closed-loop poles for the inner loop are placed in $-2$ and $-2.1$ for $z_F$, in $-10$ and $-11$ for $\phi$ and $\theta$, and finally in $-5$ and $-5.1$ for $\psi$. The outer loop poles are instead placed in $-1$ and $-2$ for both $x_F$ and $y_F$. A properly scaled white Gaussian sensor noise is simulated according to an MPU-9250 IMU TDK InvenSense [37], which is a common device in quadrotor flight controllers (i.e., in Pixhawk 4).

6.1. Independent Motors’ Speed

Let us first consider a VPQ equipped with four independent motors and four independent pitch servos. The simulation is performed for a total of 30 s under the external wind force reported in Figure 3, which also reports the correspondent estimations provided by the observer, where the gain matrix has been set as $H=5I$, with I being the identity matrix.

Due to the stochastic nature of the wind, achieving the boundedness of the estimation error replaces the ideal goal of a perfect convergence, which is here achieved in the case of bounded derivatives of the wind force. The estimation of the faults and the wind is practically started after a finite time from the beginning of the flight (i.e., after $1s$ of simulation in this example), in order to make the initial conditions of the observer practically irrelevant.

Faults components and their estimations are reported in Figure 4.

Two trapezoidal faults are injected. The first one affects motor 1 after 10 s (the LOE magnitude is $60\%$, i.e., $w_1=0.4$) and a second one affects motor 3 after 15 s (the LOE magnitude is $30\%$, i.e., $w_3=0.7$). In the range $t\in [0,1]$, each ${\widehat{w}}_i$ is set equal to 1, matching the reasonable assumption of no faults at the initial time (or, without loss of generality, we assume that we know the fault status at the beginning of the flight). Note that, since each estimation is provided independently from the NDO, the same results can be achieved in the case of simultaneous faults.

The tracking reference components and the related state variables are reported in Figure 5.

The references on $x_F$ and $y_F$ are sinusoidal signals, the reference on $z_F$ forces the quadrotor to face both ascending and descending profiles, while the yaw angle is kept at zero. The system is forced to change the velocity constantly in both magnitude and direction, with a linear speed magnitude up to 7 m/s. All the references are smoothly tracked with an acceptable error (the plotted variables are the actual state variables instead of the noisy ones), with the exception of the yaw component ($\psi$), which exhibits a temporary increasing error when the fault worsens.

The references ${\phi }_r$ and ${\theta }_r$ generated by the outer loop are reported in Figure 6.

Please note that, when the fault is rapidly worsening, the tracking capabilities in the inner loop are affected. The inner/outer loop approach, which employs a higher frequency of the inner loop and a zero-order hold, allows us to track the references, despite the presence of noise, faults, and disturbances.

The control inputs u and $\alpha$ are reported in Figure 7.

All the actuators are subject to the same saturation constraints. This allows us to plot the bounds with horizontal dashed lines in Figure 7 (upper bounds in this case). In particular, the saturation limits have been set as

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \underline{\alpha }& =\mathrm{col}({\alpha }_{min},{\alpha }_{min},{\alpha }_{min},{\alpha }_{min})\hfill \\ \hfill \overline{\alpha }& =\mathrm{col}({\alpha }_{max},{\alpha }_{max},{\alpha }_{max},{\alpha }_{max})\hfill \\ \hfill \underline{u}& =\mathrm{col}(u_{min},u_{min},u_{min},u_{min})\hfill \\ \hfill \overline{u}& =\mathrm{col}(u_{max},u_{max},u_{max},u_{max})\hfill \\ \hfill \stackrel{-}{\mathsf{\Delta }\alpha }& =-\underline{\mathsf{\Delta }\alpha }=\mathrm{col}(\mathsf{\Delta }{\alpha }_{max},\mathsf{\Delta }{\alpha }_{max},\mathsf{\Delta }{\alpha }_{max},\mathsf{\Delta }{\alpha }_{max})\hfill \\ \hfill \stackrel{-}{\mathsf{\Delta }u}& =-\underline{\mathsf{\Delta }u}=\mathrm{col}(\mathsf{\Delta }{u}_{max},\mathsf{\Delta }{u}_{max},\mathsf{\Delta }{u}_{max},\mathsf{\Delta }{u}_{max}),\hfill \end{array}\end{array}$$

(76)

where $u_{min}$, $u_{max}$, ${\alpha }_{min}$, and ${\alpha }_{max}$ are detailed in Table 1. Considering $\alpha =\overline{\alpha }$, $\overline{u}$ corresponds to a thrust-to-weight ratio equal to $3.131$.

At the beginning, when no fault is present, the opposite actuators (1 and 3, 2 and 4) show similar motor speeds and pitch angles, due to symmetry. As soon as the fault on motor 1 occurs, the opposite actuators 1 and 3 behave differently; in response to the severe fault acting on motor 1, the control allocation algorithm demands a high speed and a large pitch angle to compensate for the missing lift force.

6.2. Centralized Motor

In this section, we consider a VPQ equipped with a single centralized motor and four independent pitch servos. The simulation conditions and the wind components are the same as the previous simulation, including the same random seed. Figure 8 reports the wind effect and its estimations provided by the observer; no noticeable differences can be noted, as expected, because the fault estimation does not directly depend on the actual inputs.

The same consideration holds for the faults and their estimations (Figure 9).

The tracking reference components and the related state variables are reported in Figure 10 and Figure 11. The performances are similar; minor differences in the outer loop tracking capabilities can be detected.

Finally, the control inputs u and $\alpha$ are reported in Figure 12.

First of all, please note that the motor speeds coincide, as expected. Furthermore, in response to the fault acting on motor 1, the control allocation algorithm demands a high motor speed to compensate for the missing lift force. As there is only one centralized motor, the speed of every propeller must increase. Analogously, please note that, the larger the fault, the higher the average value of the pitch angle.

As the motor speed is higher on average in the case of the centralized motor (Figure 7 and Figure 12), the energy consumption is higher. This is unavoidable, because in the case of the centralized motor, the input redundancy is lower than in the case of independent motors. In

Table 2. Consumptions: independent motors’ speed vs. centralized motor.

Scenario	Consumption (${\left|\mathit{u}\right|}^{3/2}$)
Independent motors	$6.144\times {10}^9$
Centralized motor	$7.551\times {10}^9$

, we summarize the overall normalized (i.e., $Q_u=I$) consumption during the flight, as proposed in (65).

7. Conclusions

Unlike standard quadrotors that are equipped with four fixed-pitch blades and four independent motors, VPQs can vary both the rotation speed and the blade pitch of each propeller, thus increasing the degrees of freedom for control purposes. In addition to higher thrust rate of change, reverse thrust and reverse flight capabilities, and scaling well with size, VPQs enable for fault-tolerant control strategies thanks to their inherent input redundancy, whether they are equipped with four independent motors or a centralized one. We have shown that a single observer can estimate both actuator loss of effectiveness and the external disturbance given by horizontal wind. According to the disturbance observer-based control framework, the estimation of the wind is fed forward in the outer (position) control loop. The estimation of the actuator loss of effectiveness is instead employed in the control allocation algorithm. It manages each propeller’s pitch and motor speed to generate the force and the torques commanded by the inner (attitude and altitude) control loop. Simulation results show that the estimation of both the disturbance and the actuator loss of effectiveness is practically feasible in the presence of conventional measurement noise in commercial devices. We remark that the fault estimation is performed from kinematic data of the onboard inertial measurement unit only, without the need to measure the motor speed nor the current drawn by the actuators.

Comparing centralized and independent motors, the former is less efficient in terms of energy consumption, especially in the case of faults, because all the propellers must spin faster. Clearly, many other factors should be considered to evaluate the energy consumption properly, such as possible weight and cost reduction. In any case, both centralized and independent motors can tolerate actuator faults using the proposed scheme. The proposed control scheme could also deal with pitch lock-in-place, provided that reliable information on the stuck fault is available (e.g., measuring the current pitch angle or estimating it through a fault detection algorithm). Practically, it is sufficient to constrain the faulty pitch servo to the current value, and the control allocation algorithm can accommodate for the stuck fault using the remaining control inputs. However, in the case of a single centralized motor, the reduced amount of redundancy makes it less robust to lock-in-place faults and severe loss of effectiveness.