Access the full text.

Sign up today, get DeepDyve free for 14 days.

Electrical Engineering and Systems Science
, Volume 2020 (2002) – Feb 18, 2020

/lp/arxiv-cornell-university/incremental-nonlinear-fault-tolerant-control-of-a-quadrotor-with-3oHkuLCJYI

- ISSN
- 1552-3098
- eISSN
- ARCH-3348
- DOI
- 10.1109/TRO.2020.3010626
- Publisher site
- See Article on Publisher Site

IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 1 Incremental Nonlinear Fault-Tolerant Control of a Quadrotor with Complete Loss of Two Opposing Rotors Sihao Sun, Xuerui Wang, Qiping Chu, and Coen de Visser Abstract—In order to further expand the ﬂight envelope of quadrotors under actuator failures, we design a nonlinear sensor-based fault-tolerant controller to stabilize a quadrotor with failure of two opposing rotors in the high-speed ﬂight condition (> 8m/s). The incremental nonlinear dynamic inversion (INDI) approach which excels in handling model uncertainties is adopted to compensate for the signiﬁcant unknown aerodynamic eﬀects. The internal dynamics of such an underactuated system have been analyzed, and subsequently stabilized by re-deﬁning the control output. The proposed method can be generalized to control a quadrotor under single-rotor-failure and nominal Fig. 1: Snapshot of the tested Bebop2 subjected to failures of conditions. For validation, ﬂight tests have been carried out in rotor 1 & 3 (upper row), and subjected to failure of rotor 3 a large-scale open jet wind tunnel. The position of a damaged (bottom row). quadrotor can be controlled in the presence of signiﬁcant wind disturbances. A linear quadratic regulator (LQR) approach from the literature has been compared to demonstrate the advantages of the proposed nonlinear method in the windy and high-speed ﬂight condition. Index Terms—Air safety, Fault tolerant control, Nonlinear control systems, Unmanned aerial vehicles I. INTRODUCTION Multi-rotor drones have demonstrated their ability in a large variety of applications such as surveillance, delivery, and recreation. Due to the potential growth of the drone market in the coming decades, safety issues are of critical concern. Apart from sensor redundancies, and improving operational Fig. 2: Snapshot of the tested quadrotor in the wind tunnel, regulations, fault-tolerant control (FTC) is a key to improving with removal of rotor 1 and 3. The ﬂight video can be found safety in the face of unexpected structural and actuator failures. via https://youtu.be/-4rXX4D5HlA Among diﬀerent types of multi-rotor drones, quadrotors excel in their structural simplicity. However, they suﬀer more from actuator damages due to a lack of actuator redundancy. proposed by [14] indicates that the hovering ﬂight of a Partial damage on the rotors could result in the reduction quadrotor is possible with a loss of up to three rotors. With a of control eﬀectiveness, which has been extensively studied specially designed conﬁguration, a vehicle with only a single in the literature (e.g., [1], [2], [3], [4]). A more challenging rotor is tested using LQR with actuator saturations taken into problem is the complete loss of one or more rotors (Fig. 1). account [15] . Various control methodologies addressing this problem have The aforementioned literature assumes that the drone is been proposed and validated in simulations (e.g., [5], [6], [7], operated around the hovering condition and only limited aero- [8], [9], [10]). dynamic eﬀects are considered such as the rotational damp- In-ﬂight validations have been achieved by several pieces of ing [13], [14]. However, in out-door applications, signiﬁcant research where linear control methods were mostly adopted, aerodynamic forces/moments on the quadrotor are present due such as linear quadratic regulator (LQR) [11], proportional- to fast cruising speed and large wind disturbances [16], [17]. integral-derivative (PID) control [12] and linear parameter The system nonlinearity also becomes more signiﬁcant due varying (LPV) control [13]. The relaxed hovering solution to the complex variation of rotor aerodynamic characteristics The authors are with Control and Simulation Section, Faculty in high-speed conditions. Therefore, designing a high-speed of Aerospace Engineering, Delft University of Technology, 2629 HS capable robust nonlinear controller is essential for expanding Delft, The Netherlands (e-mail: s.sun-4@tudelft.nl; x.wang-6@tudelft.nl; q.p.chu@tudelft.nl; c.c.devisser@tudelft.nl) the ﬂight envelope of a quadrotor subjected to rotor failures, arXiv:2002.07837v2 [cs.RO] 26 Oct 2020 IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 2 and increasing its robustness against wind disturbances. II. Problem Formulation Incremental nonlinear dynamic inversion (INDI) is a sensor- A. Quadrotor Kinematic and Dynamic Model based nonlinear control approach that makes use of sen- There are two coordinate systems considered in this work. sor measurements to reduce its model dependency, thereby The inertial frame F =f𝑂 – 𝒙 – 𝒚 – 𝒛 g, is ﬁxed to the ground, I 𝐼 𝐼 𝐼 improving its robustness against model uncertainties. This with 𝒙 , 𝒚 and 𝒛 pointing to the north, east and aligning 𝐼 𝐼 approach has been adopted by the aviation industry in several with the local gravity. The body frame F =f𝑂 – 𝒙 – 𝒚 – 𝒛 g B 𝐵 𝐵 𝐵 applications, such as the control of ﬁxed-wing aircraft [18], is ﬁxed to the vehicle, with the origin located at the center spacecrafts [19], helicopters [20] and multi-rotor drones [21], of mass. As Fig. 1 shows, we assume the quadrotor has a [22], [23]. In [24], we made use of the INDI controller symmetric fuselage, which is a common conﬁguration for to control a quadrotor with a single rotor failure in the many commercially available quadrotors. As a convention, we wind tunnel. The control method has shown its advantage in deﬁne 𝒙 points forward, 𝒛 points downwards such that the 𝐵 𝐵 providing robustness to large aerodynamic disturbances while drone inertia is symmetric with respect to the 𝒙 𝒛 plane, 𝐵 𝐵 simplifying gain tuning, and eliminating the need to calculate and 𝒛 is parallel with the thrust direction. 𝒚 thus points an equilibrium for linear controller design. rightwards to render F a right-handed coordinate system. In 𝐼 𝐵 However, the INDI controller relies on a dynamic inversion the following context, the superscript »¼ and »¼ indicate step. This step requires the number of inputs to be no less the coordinate system in which a vector is expressed. than the number of outputs. For a quadrotor with only two The equations of motion of a quadrotor are formulated as opposing rotors remain, the attitude control problem becomes follows: under-actuated where the direct inversion is inapplicable. For 𝑷 = 𝑽 (1) this reason, we need to redesign the original control outputs of 𝐼 𝐵 𝑚 𝑽 = 𝑚 g ¸ 𝑹𝑭 (2) a quadrotor such as the thrust and attitudes. This subsequently 𝑣 𝑣 results in several internal dynamics of which the stability needs 𝑹 = 𝑹𝛀 (3) to be guaranteed. The selection of the outputs ensuring stable internal dynamics has been addressed on some under-actuated 𝐵 𝐵 𝐵 𝑰 𝛀 =𝛀 𝑰 𝛀 ¸ 𝑴 (4) 𝒗 𝒗 control problems, e.g., wheeled mobile robots [25], quadrotor 𝐼 𝐼 𝑇 𝑇 where 𝑷 = »𝑋– 𝑌– 𝑍¼ and 𝑽 = »𝑉 – 𝑉 – 𝑉 ¼ represent position control [26], and the attitude control of space aircraft 𝑥 𝑦 𝑧 the position and the velocity of the center of mass in F ; 𝑚 [27]. I 𝑣 is the vehicle gross mass and 𝑰 denotes the inertia matrix The main theoretical contributions of this research are of the vehicle including rotors. 𝒈 is the local gravity vector. twofold: (1) A detailed analysis of the internal dynamics of 𝑹2 SO¹3º indicates the transformation matrix from F to F . B I quadrotors with complete loss of two opposing rotors; (2) A 𝐵 𝑇 The angular velocity is expressed as 𝛀 = »𝑝 𝑞 𝑟¼ where 𝑝, subsequent novel robust fault-tolerant control method imple- 𝑞 and 𝑟 denote pitch rate, roll rate and yaw rate respectively. menting the INDI approach. The controlled quadrotor thereby 𝛀 is the skew symmetric matrix such that 𝛀 𝒂 = 𝛀 𝒂 for suﬀers less from model uncertainties caused by signiﬁcant any vector 𝒂 2 R . aerodynamic eﬀects during high-speed ﬂight. 𝐵 𝐵 The variables 𝑭 and 𝑴 denote the resultant force and To validate the proposed controller, ﬂight tests of a quadro- moment on the center of mass respectively, projected on F . tor with failure of two opposing rotors have been performed For a quadrotor with thrust parallel to the 𝒛 axis and rotor in an open jet wind tunnel (Fig. 2). With limited information directions shown in Fig. 3, we have on the model, the controller is able to stabilize the damaged quadrotor in wind of over 8 m/s, which is more than half of 2 3 6 7 its nominal maximum ﬂight speed. This could signiﬁcantly 𝐵 6 7 𝑭 = 0 ¸ 𝑭 (5) 6 7 increase the safety of quadrotors by expanding the ﬂight enve- 4 6 7 𝘅 ¯ 𝜔 4 𝑖=1 𝑖 5 lope under actuator failure conditions. With slight adaptation, 2 3 the same control scheme can be used on a quadrotor with a 2 3 6 7 𝑏 sin𝛽 𝑏 sin𝛽 𝑏 sin𝛽 𝑏 sin𝛽 6 7 6 7 single rotor failure for which the internal dynamics are proved 6 7 6 2 7 𝑏 cos𝛽 𝑏 cos𝛽 𝑏 cos𝛽 𝑏 cos𝛽 𝑴 = 𝘅 ¯ 6 7 6 7 to be inherently stable. A benchmark approach (LQR) has 6 7 6 3 7 𝜎 𝜎 𝜎 𝜎 been compared to demonstrate the advantage of the proposed 4 5 6 2 7 4 4 5 controller in the high-speed and windy ﬂight conditions. 2 3 2 3 𝐼 𝑞¹𝜔 𝜔 ¸𝜔 𝜔 º 0 𝑝 1 2 3 4 6 7 6 7 6 7 6 7 This paper is organized as follows. Section II provides ¸ 𝐼 𝑝¹𝜔 𝜔 ¸𝜔 𝜔 º ¸ 0 ¸ 𝑴 𝑝 1 2 3 4 6 7 6 7 6 7 6 7 information on the quadrotor model and the reduced attitude 𝐼 ¹𝜔¤ 𝜔¤ ¸𝜔¤ 𝜔¤ º 𝛾𝑟 𝑝 1 2 3 4 4 5 4 5 control. Section III introduces the INDI controller and Sec- (6) tion IV directly provides the detailed controller design for where 𝘅 ¯ is a thrust coeﬃcient valid in the hovering condition; a quadrotor with failure of two opposing rotors. Section V 𝜎 is a constant ratio between the thrust coeﬃcient and drag elaborates on the selection of control outputs and the stability coeﬃcient of the rotor; 𝑏 and 𝛽 are geometry parameters as of internal dynamics. Section VI generalizes the proposed Fig. 3 shows. Note that 𝛽 2 ¹0– 𝜋2º for a quadrotor. 𝝎 = method to the single-rotor-failure and the nominal conditions. »0– 0– 𝜔 ¼ is the angular speed of the 𝑖-th rotor with respect Finally, Sections VII and VIII demonstrate the ﬂight test results to the body. 𝐼 denotes the moment of inertia of each rotor in low-speed and high-speed ﬂight conditions respectively. about the rotational axis. Note that this model assumes that IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 3 The other selections of 𝒏 may be considered for the case with single rotor failure, which has been discussed in [11], [14]. The challenge of the problem is conducting relaxed attitude control of a quadrotor with only two opposing rotors remain in the presence of signiﬁcant model uncertainties 𝑭 and 𝑴 , 𝑎 𝑎 for instance, in high-speed ﬂight conditions where signiﬁcant aerodynamic eﬀects become apparent. To achieve this goal, we employ a sensor-based nonlinear control method to be described in Sec. III. The detailed implementation of this method will be provided in Sec. IV. Fig. 3: Deﬁnition of geometry parameters, rotor index and directions, and the body frame F . III. Methodology Incremental nonlinear dynamic inversion (INDI) is a sensor- jj𝛀jj ŸŸjj𝝎 jj, thus the magnitude of rotor angular speed with based nonlinear control approach. The approach stems from respect to the air is approximated by 𝜔 . Symbol jjjj is deﬁned the nonlinear dynamic inversion (NDI) control. INDI reduces as the 𝐿 norm of a vector. 𝛾 in (6) indicates the aerodynamic the model dependencies of NDI by replacing non-input related yaw damping coeﬃcient [11], [13]. model terms with direct sensor measurements, or sensor In the high-speed ﬂight condition, there are signiﬁcant measurement derived quantities, thereby greatly improving aerodynamic eﬀects such as thrust variation [28], rotor in- robustness against model uncertainties. plane force [29], rotor moment and airframe aerodynamic [30]. For aircraft systems, including nominal quadrotors, each These additional aerodynamic related forces and moments are sub-problem (e.g., the attitude and rate control loops) is fully then expressed as 𝑭 and 𝑴 in (5) and (6). They are regarded 𝑎 𝑎 actuated without internal dynamics to be analyzed [3], [18], as model uncertainties that need to be compensated for by the [21]. However, for a quadrotor with failure of two opposing robustness of the control method. rotors, the number of control inputs is less than the required output in a conventional cascaded control setup, yielding B. Reduced Attitude Control internal dynamics that have to be stabilized. In the following context, the generalized INDI control considering internal The concept of reduced attitude control [31] has been dynamics will be brieﬂy reviewed. Readers may refer to [32] adopted by [14] in the quadrotor fault tolerant control problem. and [33] for further details. We hereby brieﬂy introduce the concept. For a quadrotor with complete rotor failures, the full state Consider a nonlinear input-aﬃne system equilibrium becomes unattainable. This is due to the incapacity 𝒙¤ = 𝒇¹𝒙º¸ 𝑮¹𝒙º𝒖 of the remaining rotors to generate zero yaw moment while (9) 𝒚 = 𝒉¹𝒙º producing necessary thrust. As a consequence, the vehicle spins around the yaw axis. And the attitude control is reduced 𝑛 𝑛 𝑛 𝑙 where 𝒇 : R ! R and 𝒉 : R ! R are smooth vector ﬁelds. to a thrust vector pointing problem without considering the 𝑛 𝑛𝑚 𝑮 : R ! R is a function mapping with smooth vector yaw angle. ﬁelds as columns. The number of outputs is not larger than Deﬁne a unit vector 𝒏 ﬁxed to F where 𝒏 = the number of inputs (i.e., 𝑙 𝑚). There exists a nonlinear 𝐵 𝐵 𝐵 𝑇 »𝑛 – 𝑛 – 𝑛 ¼ . For a quadrotor with failure of two opposing 𝑥 𝑦 𝑧 𝑛 𝑛 transformation 𝑻 : R ! R such that the states 𝒙 can be 𝐵 𝑇 rotors, choosing 𝒏 = »0– 0– 1¼ is most energy-eﬃcient transformed to the normal form including internal states 𝜼 and [11] where 𝒏 aligns with the instantaneous thrust direction. external states 𝝃: Deﬁne another unit vector 𝒏 as the reference of 𝒏, which is calculated by the position controller or remotely provide by 𝜼 𝝓¹𝒙º = = 𝑻¹𝒙º (10) a pilot. Then aligning 𝒏 with 𝒏 (or vise versa) becomes the 𝝃 𝜽¹𝒙º primary task of the attitude controller. Therefore, we introduce where the following relaxed attitude kinematic equation: [24] 𝜽¹𝒙º = »𝜽 ¹𝒙º– 𝜽 ¹𝒙º–•••– 𝜽 ¹𝒙º¼ (11) 1 2 𝑙 𝐵 𝐵 𝑇 𝐼 ¤ ¤ 𝒏 =𝛀 𝒏 ¸ 𝑹 𝒏 (7) 𝑑 𝑑 𝑑 with 𝐵 𝑇 𝑇 𝐼 With the expressions 𝒏 = »ℎ – ℎ – ℎ ¼ and 𝑹 𝒏¤ = 1 2 3 𝑑 𝑑 𝑇 𝜌 1 »𝘆 – 𝘆 – 𝘆 ¼ , the expanded formula of (7) can be given as: 1 2 3 𝜽 ¹𝒙º = »ℎ ¹𝒙º– 𝐿 ℎ ¹𝒙º–•••– 𝐿 ℎ ¹𝒙º¼– 𝑖 = 1– 2–•••–𝑙 (12) 𝑖 𝑖 𝑓 𝑖 𝑖 2 ¤ 3 2 3 2 3 2 3 ℎ 0 𝑟 𝑞 ℎ 𝘆 1 1 1 6 7 6 7 6 7 6 7 where ℎ ¹𝒙º indicates the 𝑖th element in the vector ﬁeld 𝒉. 6 ¤ 7 6 7 6 7 6 7 ℎ = 𝑟 0 𝑝 ℎ ¸ 𝘆 (8) 𝜌 2 2 2 6 7 6 7 6 7 6 7 The notation 𝐿 ℎ ¹𝒙º indicates the 𝜌 th order Lie derivative 𝑖 𝑖 6 ¤ 7 6 7 6 7 6 7 ℎ 𝑞 𝑝 0 ℎ 𝘆 3 3 3 4 5 4 5 4 5 4 5 of the function ℎ with respect to the vector ﬁelds 𝒇¹𝒙º [33]. To align 𝒏 with 𝒏 , we can control ℎ and ℎ to track 𝜌 indicates the relative degree of the 𝑖th output 𝑦 . 𝑑 1 2 𝑖 𝑖 𝐵 𝐵 𝑛 and 𝑛 respectively. To be speciﬁc, with the selection of By deﬁning 𝜌 ¯ = Σ 𝜌 as the sum of relative degrees 𝑥 𝑦 𝑖 𝐵 𝑇 𝒏 = »0– 0– 1¼ , ℎ and ℎ need to be stabilized to zero. of each output, one can deﬁne the transformation 𝝓¹𝒙º = 1 2 IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 4 »𝜙 ¹𝒙º– 𝜙 ¹𝒙º– •••– 𝜙 ¹𝒙º¼. The selection of 𝝓¹𝒙º is not 1 2 𝑛𝜌 ¯ , , , , unique, but has to satisfy the following condition: , x,ref 𝜕𝜙 , ref ref y,ref 𝑮¹𝒙º = 0– 𝑖 = 1– 2–•••–𝑛 𝜌 ¯ (13) PID des || − || 𝜕𝒙 INDI Drone namely the ﬁrst-order derivative of 𝜼 as deﬁned per (10) does 2 z,ref inner-loop not include control input 𝒖. The nonlinear transformation 𝑻¹𝒙º ref is a diﬀeomorphism (i.e., smooth and invertible) in the domain outer-loop of interest. Fig. 4: The two-loops cascaded control scheme using PID as As a consequence, the problem is transformed to the normal outer-loop position control and INDI as inner-loop altitude / form attitude control. 𝜼¤ = 𝒇 ¹𝜼–𝝃º 𝝃 = 𝑨 𝝃¸ 𝑩 »𝜶¹𝒙º¸B¹𝒙º𝒖¼ (14) 𝒄 𝒄 error (𝒆 = 𝒚 𝒚 ) dynamics are 𝒆¤ = ¹𝑨 𝑩 𝑲º𝒆¸ 𝑩 𝜺 , 𝒚 = 𝑪 𝝃 𝑐 𝑐 𝑐 indi 𝒄 ref where 𝜺 is the residual error caused by model uncertainties, indi where the triplet ¹𝑨 – 𝑩 –𝑪 º is a canonical form representa- 𝒄 𝒄 𝒄 Δ𝒙-related term and 𝑹 in (21). The ultimate boundedness of 𝑛 𝑙 tion of 𝑙 chains of 𝜌 integrators (𝑖 = 1– 2–•••–𝑙); 𝜶 : R ! R 𝜺 and 𝒆 has been proved in [34]. indi 𝑛 𝑙𝑚 and B : R ! R are mappings determined by the system In INDI control, the model information of 𝜶¹𝒙º required (9). Subsequently, the output dynamics can be represented as for NDI controller, is not needed for implementation, which ¹𝝆º greatly reduces the eﬀort of modeling. The control eﬀective- 𝒚 = 𝜶¹𝒙º¸B¹𝒙º𝒖 (15) ness matrix B is relatively easier to be estimated oﬄine or ¹𝜌 º ¹𝜌 º ¹𝜌 º 1 2 𝑙 ¹𝝆º 𝑇 where 𝒚 = »𝑦 – 𝑦 –•••– 𝑦 ¼ . If matrix B has full 1 2 𝑙 identiﬁed online [21]. Apart form its reduced model depen- row rank, the NDI control law can be designed as dency, INDI control also has enhanced robustness as compared to its classical NDI counterpart [34]. 𝒖 =B¹𝒙º ¹𝝂 𝜶¹𝒙ºº (16) ndi Due to the measurement noise, the variables 𝒙 , 𝒚 can be where superscript »¼ indicates the Moore-Penrose inverse of low-pass ﬁltered in practice. To synchronize the time delay the matrix; 𝝂 2 R is called the pseudo-input. With a full caused by these ﬁlters, 𝒖 also need to be ﬁltered with the knowledge of 𝜶¹𝑥º and B¹𝒙º, control law (16) yields the same cut-oﬀ frequency [21]. We use subscript »¼ to denote closed loop dynamics the ﬁltered variables (e.g., 𝒙 ! 𝒙 , 𝒚 ! 𝒚 and 𝒖 ! 𝒖 ). 0 𝑓 0 𝑓 ¹𝝆º 0 𝑓 𝒚 = 𝝂 (17) Consequently, the INDI control law becomes For a command tracking problem with reference output 𝒚 2 ¹𝝆º ref ¸ 𝒖 =B¹𝒙 º ¹𝝂 𝒚 º¸ 𝒖 (22) 𝑙 indi 𝑓 𝑓 R that is 𝝆th order diﬀerentiable, selecting ¹𝝆º We will elaborate on applying the INDI control law (22) to 𝝂 =𝑲¹𝝃 𝝃 º¸ 𝒚 (18) ref ref the quadrotor control problem in the following sections. ensures that the reference output is being tracked asymptot- ically, where the gains 𝑲 is selected such that 𝑨 𝑩 𝑲 is 𝒄 𝒄 IV. Controller Design Hurwitz. The reference 𝝃 is denoted as ref The detailed design process of the controller for a quadrotor 𝝃 = »𝝍 – 𝝍 –•••–𝝍 ¼ – ref 1 2 𝑙 with complete failure of two opossing rotors is presented in (19) ¹1º ¹𝜌 1º this section. In general, we use a cascaded controller with two 𝝍 = »𝑦 – 𝑦 –•••–𝑦 ¼– 𝑖 = 1– 2–•••–𝑙 ref–𝑖 ref–𝑖 ref–𝑖 loops (Fig. 4), where INDI is applied in the inner-loop for In reality, the nonlinear model dependent terms 𝜶¹𝒙º and compensating model uncertainties (𝑴 – 𝑭 ). 𝑎 𝑎 B¹𝒙º are almost impossible to be obtained due to inevitable model uncertainties. In view of this, we take the ﬁrst-order A. Outer-loop Design Taylor series expansion of (15) around the condition at the last sensor sampling moment 𝑡Δ𝑡 (denoted by subscript »¼ ), The outer-loop contains a horizontal position controller then (15) becomes that computes the acceleration command from the reference ¹𝝆º horizontal position denoted by 𝑋 and 𝑌 . Due to the linear ref ref 𝒚 = 𝜶¹𝒙º¸B¹𝒙º𝒖 property of translational kinematics, a linear method such as 𝜕»𝜶¹𝒙º¸B¹𝒙º𝒖¼ ¹𝝆º = 𝒚 ¸B¹𝒙 ºΔ𝒖¸ Δ𝒙¸ 𝑹 (20) a PID controller can be employed. In addition, the reference 0 1 𝜕𝒙 altitude 𝑍 needs to be second-order diﬀerentiable. Therefore, ref where Δ𝒖 = 𝒖𝒖 – Δ𝒙 = 𝒙𝒙 ; 𝑹 is the ﬁrst-order Taylor ex- 0 0 1 we have pansion remainder. Design the incremental nonlinear dynamic 2 3 𝑘 𝑒 𝑘 𝑒¤ 𝑘 𝑒 𝑑𝑡 𝑝 𝑥 𝑑 𝑥 𝑖 𝑥 6 ∫ 7 inversion (INDI) control as 6 7 𝒂 = 𝑘 𝑒 𝑘 𝑒¤ 𝑘 𝑒 𝑑𝑡 (23) ref 𝑝 𝑦 𝑑 𝑦 𝑖 𝑦 6 7 ¹𝝆º ˆ 6 7 𝒖 ¯ =B¹𝒙 º ¹𝝂 𝒚 º¸ 𝒖 (21) indi 0 0 ref 0 4 5 where 𝝂 is selected as per (18), while B is the estimated con- where 𝑒 = 𝑋𝑋 , 𝑒 =𝑌𝑌 denote the horizontal position 𝑥 ref 𝑦 ref trol eﬀectiveness matrix. As a result, the closed-loop tracking errors in F ; control gains 𝑘 , 𝑘 and 𝑘 are positive. Then I 𝑝 𝑖 𝑑 IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 5 𝒏 for reduced attitude control (see Sec. II) can be calculated by 𝒂 𝒈 ref 𝒏 = (24) jj𝒂 𝒈 jj ref It is possible to replace (23) by more sophisticated position controllers to obtain 𝒂 , which will not be elaborated in this ref research. Fig. 5: Deﬁnition of F , 𝑦 , 𝘂 and 𝜒 when rotor 2 and rotor 2 1 B. Inner-loop Design 4 are removed. The altitude control is included in the inner-loop controller since the altitude reference 𝑍 is related to rotor thrust which ref 1) Control output deﬁnition: Since there are only two inputs contains model uncertainties 𝑭 . Consequently, the inner-loop remain, we can select a maximum of two variables as control is a combination of altitude and attitude control using the INDI outputs. To guarantee altitude tracking, we choose the ﬁrst approach. output as States for the inner-loop control are deﬁned as 𝒙 = in 𝑇 𝑦 = 𝑍 (31) »ℎ – ℎ – 𝑝– 𝑞– 𝑟– 𝑍– 𝑉 ¼ . There are two diﬀerent scenarios 1 2 𝑧 for a quadrotor with two opposite rotor failures. If only rotor The second output have to be associated with the reduced 1 and 3 remain functional, we deﬁne the control input as attitude control. Recall that in Sec. II-B, 𝒏 needs to align with 𝒏 by manipulating the vehicle attitude, and ℎ and 𝑇 2 2 𝑇 𝑑 1 𝒖 = »𝑢 – 𝑢 ¼ , »𝜔 – 𝜔 ¼ – 𝑠 = 1 (25) 1 2 𝑙 1 3 ℎ of (8) need to converge to zero. We hereby introduce a new coordinate system F =f𝑂 – 𝒙 – 𝒚 – 𝒛 g that is ﬁxed with S 𝑆 𝑆 𝑆 If only rotor 2 and 4 remain, then 𝑆 respect to the body frame. As Fig. 5 illustrates, F is generated 𝑇 2 2 𝑇 𝒖 = »𝑢 – 𝑢 ¼ , »𝜔 – 𝜔 ¼ – 𝑠 =1 (26) by rotating the body frame about 𝒛 . The rotation angle is 1 2 𝑙 𝐵 2 4 denoted as 𝜒. where 𝑠 2f1– 1g is a parameter indicating the type of failure. The second output 𝑦 is then deﬁned as the projection of Since the product of inertia is negligible compared with the 𝒏 on 𝒙 . In other words, 𝑦 becomes a linear combination 𝑑 𝑆 2 moment of inertia for a typical quadrotor, we can assume a of ℎ and ℎ scheduled by the angle 𝜒: 1 2 diagonal inertia matrix 𝑰 = diag¹𝐼 – 𝐼 – 𝐼 º. Thus the state 𝒗 𝑥 𝑦 𝑧 𝑦 = ℎ cos 𝜒¸ ℎ sin 𝜒 (32) 2 1 2 equations for the inner-loop states 𝒙 can be derived from (4) and (7), yielding Meanwhile, the projection of 𝒏 on 𝒚 that is perpendicular 2 ¤ 3 2 3 to the second output 𝑦 remains uncontrolled (see Fig. 5). We 𝑍 𝑉 6 7 6 7 6 ¤ 7 6 7 will elaborate in Sec. V that this variable, denoted by 𝘂 , is 𝑉 1 𝑔¸ 𝐹 𝑅 𝘅 ¯¹𝑢 ¸𝑢 º𝑚 𝑧 𝑎–𝑧 33 1 2 𝑣 6 7 6 7 6 ¤ 7 6 7 one of the internal states to be stabilized by properly selecting ℎ ℎ 𝑝 ℎ 𝑟¸𝘆 1 3 1 2 6 7 6 7 6 ¤ 7 6 7 the angle 𝜒. ℎ = ℎ 𝑞¸ ℎ 𝑟¸𝘆 (27) 2 3 2 1 6 7 6 7 6 7 6 7 Due to the symmetric property of a quadrotor, we can 𝐴 𝑟𝑞 2𝑎 𝑞𝜔 ¯𝑠 ¸ 𝑀 ¸𝑠 𝐺 ¹𝑢 𝑢 º 𝑝¤ 𝑥 𝑥 𝑛 𝑎–𝑥 𝑙 𝑝 1 2 6 7 6 7 6 7 6 7 determine 𝜒 by selecting its absolute value, using the following 𝐴 𝑟𝑝¸ 2𝑎 𝑝𝜔 ¯𝑠 ¸ 𝑀 ¸𝐺 ¹𝑢 𝑢 º 𝑞¤ 𝑦 𝑦 𝑛 𝑎–𝑦 𝑞 1 2 6 7 6 7 6 7 6 7 relationship: 𝐴 𝑝𝑞𝛾𝑟𝐼 ¸ 𝑀 𝑠 𝐺 ¹𝑢 ¸𝑢 º 𝑟¤ 𝑧 𝑧 𝑎–𝑧 𝑛 𝑟 1 2 4 5 4 5 𝜒 = 𝑠 j𝜒j (33) where It is noteworthy that j𝜒j is associated with the control perfor- 𝐴 = ¹𝐼 𝐼 º𝐼 – 𝐴 = ¹𝐼 𝐼 º𝐼 – 𝐴 = ¹𝐼 𝐼 º𝐼 (28) mance, which can be analogous to physically informed control 𝑥 𝑦 𝑧 𝑥 𝑦 𝑧 𝑥 𝑦 𝑧 𝑥 𝑦 𝑧 gains. The selection of j𝜒j will be further discussed in Sec. V. 𝑎 = 𝐼 𝐼 – 𝑎 = 𝐼 𝐼 – 𝑔 = jj𝒈 jj (29) 𝑥 𝑝 𝑥 𝑦 𝑝 𝑦 2) Control eﬀectiveness estimation: After deﬁning the out- puts we can take second order derivative of both 𝑦 and 𝑦 , 1 2 where 𝑅 in (27) represens the entry at the third row and yielding the third column of the matrix 𝑹; 𝜔 ¯ is the average angular 𝑦¥ = 𝑔¸ 𝐹 𝑅 𝘅 ¯¹𝑢 ¸𝑢 º𝑚 1 𝑎–𝑧 33 1 2 𝑣 rate of the remaining rotors; 𝑠 2 f1– 1g indicates the (34) handedness of the remaining rotors with 1 clockwise and 1 = 𝛼 ¸ 𝐵 ¹𝑢 ¸𝑢 º 1 1 1 2 counterclockwise. 𝐺 , 𝐺 and 𝐺 are control eﬀectiveness on 𝑝 𝑞 𝑟 ¥ ¥ 𝑦¥ = ℎ cos 𝜒¸ ℎ sin 𝜒 2 1 2 angular accelerations, where (35) = 𝛼 ¹𝒙– 𝜒º¸ 𝐵 ¹𝑢 𝑢 º 2 2 1 2 𝐺 = 𝘅 ¯ sin𝛽𝐼 – 𝐺 = 𝘅 ¯ cos𝛽𝐼 – 𝐺 = 𝜎𝘅 ¯𝐼 (30) 𝑝 𝑥 𝑞 𝑦 𝑟 𝑧 where 𝛼 can be calculated from (27) whereupon includes nonlinear terms and model uncertainties. 𝐵 and 𝐵 are control Normally, j𝐺 j ŸŸ minfj𝐺 j–j𝐺 jg. Note that state equations 1 2 𝑟 𝑝 𝑞 (27) are nonlinear and contain model uncertainties (𝐹 , 𝑀 , eﬀectiveness on 𝑦 and 𝑦 respectively: 1 2 𝑎–𝑧 𝑎–𝑥 𝑀 and 𝑀 ). The following content in this section designs 𝑎–𝑦 𝑎–𝑧 the INDI control law (22) for this speciﬁc problem. 𝐵 =𝘅 ¯𝑅 𝑚 (36) 1 33– 𝑓 𝑣 IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 6 𝐵 = 𝑠 ℎ ¹𝐺 sin 𝜒𝐺 cos 𝜒º 2 𝑙 3– 𝑓 𝑝 𝑞 For this problem, the output reference is deﬁned as ℎ 𝘅 ¯𝑏 sin𝛽 (37) 3– 𝑓 = sin¹𝘁j𝜒jº 𝑇 𝑇 𝒚 = »𝑍 – ℎ cos 𝜒¸ ℎ sin 𝜒¼ = »𝑍 – 0¼ (44) ref 1–ref 2–ref ref 𝐼 cos𝘁 ref where 𝘁 is a positive virtual angle deﬁned as Then, by substituting (43) and (44) into (18), we obtain the pseudo-input 𝘁 = tan cot 𝛽 (38) ¤ ¥ 𝐼 𝑘 ¹𝘉 𝑍 º 𝑘 ¹𝘉 𝑍 º¸ 𝑍 º 𝑦 𝑧–𝑝 1 ref 𝑧–𝑑 2 ref ref 𝝂 = (45) 𝑘 𝘉 𝑘 𝘉 𝑎–𝑝 3 𝑎–𝑑 4 Now, from (34) and (35), the estimated control eﬀectiveness matrix in (22) can be described as with positive gains 𝑘 – 𝑘 – 𝑘 – 𝑘 to be tuned. 𝑧–𝑝 𝑧–𝑑 𝑎–𝑝 𝑎–𝑑 Eventually, the control eﬀectiveness B, 𝒚¥ and 𝝂 is substi- 𝐵 𝐵 1 1 B¹𝒙 º = (39) 𝑓 tuted into (22) to obtain 𝒖 . The rotor speed command of indi 𝐵 𝐵 2 2 the remaining rotors can be subsequently calculated using (25) The estimation error of B mainly stems from the error of 𝑚 , or (26). 𝐼 , 𝐼 , and 𝘅 ¯. Note that the ﬁltered variables 𝑅 and ℎ 𝑥 𝑦 33– 𝑓 3– 𝑓 are used in (36) and (37) because B is a function of 𝒙 as 𝑓 V. Stability Analysis of Internal Dynamics per (22). For the attitude/altitude inner-loop, internal states that need 𝑅𝑒𝑚𝑎𝑟𝑘 1: As indicated by (35) and (37), the system has to be analyzed regarding their stability properties. As (32) the largest control eﬀectiveness on 𝑦 when j sin¹𝘁j𝜒jºj = 1. shows, the selection of 𝜒 is of great importance for inﬂuencing On the contrary, the control eﬀectiveness becomes zero when the internal dynamics, which will be elaborated in this section. sin¹𝘁j𝜒jº = 0. Small control eﬀectiveness leads to large con- trol input command and subsequently deteriorates the control A. Relaxed Trimming Equilibrium performance with the presence of actuator position and rate limit. Therefore, we enforce the eﬀectiveness on 𝑦 to be The relaxed trimming equilibrium is an extension of the greater than the minimum of 𝐺 and 𝐺 , which yields the relaxed hovering equilibrium [14] to the high-speed ﬂight 𝑝 𝑞 following constraint on j𝜒j: regime where the aerodynamic drag becomes apparent. Note that the term trimming indicates the condition at a constant j𝐵 ¹j𝜒jºj 𝑟 ¹j𝜒jº , 1 (40) 𝐵 forward ﬂight velocity. As Fig. 6a shows, the quadrotor spins minfj𝐺 j–j𝐺 jg 𝑝 𝑞 about the axis 𝒏 which represents the average thrust in a single In addition, the following constraints are made to prevent 𝐵 = revolution. In the relaxed trimming equilibrium, this averaged 𝐵 = 0, which is rather easy to fulﬁll: thrust is balanced with the average drag force (denoted by 𝑅 ≠ 0: the thrust direction does not remain in the 𝑭 ) and the gravity. If we assume the constancy of 𝑴 33– 𝑓 𝑎–𝑥𝑦 𝑎 horizontal plane of F . and 𝑭 in (27), we have I 𝑎–𝑧 ℎ ≠ 0: 𝒏 is not perpendicular to the current thrust 3– 𝑓 𝑑 ¯ ¯ ¯ ¯ 𝒙 = 𝒙 ¯ = »ℎ – ℎ – 𝑝 ¯– 𝑞 ¯– 𝑟 ¯– 𝑍– 𝑉 ¼ (46) in in 1 2 𝑧 direction (𝒛 ). 𝐵 𝑇 3) Second derivative of the output: 𝒚¥ = »𝑦¥ – 𝑦¥ ¼ Speciﬁcally, if 𝒏 = »0– 0– 1¼ , we have 𝑓 1– 𝑓 2– 𝑓 in (22) can be obtained by directly taking the second-order ¯ ¯ ℎ = ℎ = 𝑝 ¯ = 𝑞 ¯ = 0 (47) 1 2 derivative of ﬁltered outputs. This, however, is prone to be detrimentally aﬀected by measurement noise. Therefore, we In practice, 𝑴 and 𝑭 are non-stationary. Thus variables 𝑎 𝑎 can approximate 𝑦¥ by: 1– 𝑓 ℎ , ℎ , 𝑝, 𝑞 and 𝑟 normally oscillate about the equilibrium. 1 2 Nevertheless, as was analysed in [24], the average thrust ¥ ¤ 𝑦¥ = 𝑍 = 𝑉 ' 𝑎 𝑅 ¸𝑔 (41) 1– 𝑓 𝑓 𝑧– 𝑓 𝑧– 𝑓 33– 𝑓 direction remain unchanged as long as the reduced attitude where 𝑎 is the projection of the ﬁltered accelerometer ℎ and ℎ are bounded. The variation of yaw rate 𝑟 is also 𝑧– 𝑓 1 2 measurement on 𝒛 . relatively small compared to 𝑟 ¯. We therefore assume the 𝑦¥ can be obtained by numerically diﬀerentiating ﬁltered constancy of 𝑟 in the following analysis. 2– 𝑓 𝑦¤ . The latter can be derived from (8) and (32): Due to the spinning motion around the yaw axis during forward ﬂight, the local airspeed and angle of attack of each 𝑦¤ = cos 𝜒¹ℎ 𝑞¸ ℎ 𝑟¸𝘆 º¸ sin 𝜒¹ℎ 𝑝 ℎ 𝑟¸𝘆 º (42) 2 3 2 1 3 1 2 rotor can be diﬀerent (see Fig. 6b). The diﬀerence of local 4) Pseudo-input deﬁnition: The last step is to deﬁne the airspeed leads to the variation of thrust coeﬃcient 𝘅 of each pseudo-input 𝝂 as per (18). As presented in (34) and (35), the remaining rotor [30]. The rotor speeds, therefore, vary with the heading angle and the variation grows with the ﬂight speed. control input 𝒖 appears after taking the second derivative of both 𝑦 and 𝑦 . Thus the system relative degrees are 𝜌 = 𝜌 = 1 2 1 2 2. According to (11) and (12), there are four external states: B. Internal Dynamics 𝑇 𝑇 »𝘉 – 𝘉 – 𝘉 – 𝘉 ¼ = »𝑦 – 𝑦¤ – 𝑦 – 𝑦¤ ¼ = 1 2 3 4 1 1 2 2 The internal dynamics are analyzed around the relaxed- trimming equilibrium. The following assumptions are further 2 3 6 7 6 7 made to derive the internal states. (43) 6 7 6 7 𝐴𝑠𝑠𝑢𝑚𝑝𝑡𝑖𝑜𝑛 1: 𝑭 and 𝑴 are independent from the 𝑎 𝑎 ℎ cos 𝜒¸ ℎ sin 𝜒 1 2 6 7 6 7 control input 𝒖. ¹ℎ 𝑞¸ ℎ 𝑟¸𝘆 º cos 𝜒¸¹ℎ 𝑝 ℎ 𝑟¸𝘆 º sin 𝜒 3 2 1 3 1 2 4 5 IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 7 2 3 2 3 6 7 6 7 6 7 6 7 6 7 6 7 6 7 𝘉 cos 𝜒𝘂 sin 𝜒 3 1 6 7 6 7 6 7 6 7 𝘉 sin 𝜒¸𝘂 cos 𝜒 3 1 6 7 6 7 ℎ = ℎ 𝑠 𝑠 ¹𝘉 cos𝘁¸𝘂 cos𝜒º¸¹𝑚 𝜎𝘉 𝘂 ℎ º𝑠 𝘂 cos𝘁 6 7 6 7 3 𝑛 𝑙 4 2 𝑣 2 3 3 𝑙 1 6 7 6 2 7 ℎ 𝑠 sin¹j𝜒j𝘁º 6 7 6 3 7 Fig. 6: a.) Illustration of the force equilibrium at the relaxed ℎ 𝑠 ¹𝘉 sin𝘁¸𝘂 sinj𝜒jº¸¹𝑚 𝜎𝘉 𝘂 ℎ º𝘂 sin𝘁 6 7 6 3 𝑛 4 2 𝑣 2 3 3 1 7 6 7 6 2 7 trimming equilibrium. b.) The local velocities of the remaining ℎ 𝑠 sin¹j𝜒j𝘁º 6 7 6 7 4 5 6 7 rotors are diﬀerent due to the high angular and translational ¹𝘂ℎ 𝑚 𝜎𝘉 º¹ℎ 𝑠 º 3 𝑣 2 3 𝑛 4 5 (53) speed of the drone (rotor 2 and 4 remain). Then the dynamic equation of the internal states is derived as ˜ ˜ 𝜼¤ = 𝒇 ¹𝒙; 𝜒º = 𝒇 ¹𝑻 ¹𝜼–𝝃º; 𝜒º = 𝒇 ¹𝜼–𝝃 ; 𝜒º (54) 𝘂 𝘂 𝘂 𝐴𝑠𝑠𝑢𝑚𝑝𝑡𝑖𝑜𝑛 2 : The attitude reference 𝒏 is slowly time- 𝐼 𝑇 A suﬃcient condition of the local stability of internal dy- varying, thus 𝒏¤ = »𝘆 – 𝘆 – 𝘆 ¼ ' 0. 1 2 3 namics can be established via the notion of the zero dynamics 𝐴𝑠𝑠𝑢𝑚𝑝𝑡𝑖𝑜𝑛 3 : The average rotor speed 𝜔 ¯ is considered as [27]: a constant that is independent from the control input 𝒖. 𝜼¤ = 𝒇 ¹𝜼– 0; 𝜒º (55) Note that 𝐴𝑠𝑠𝑢𝑚𝑝𝑡𝑖𝑜𝑛 3 holds because of the near- constancy of the resultant thrust during the trimming condi- By substituting (53) into (55), we have: tion, which has been veriﬁed from the ﬂight data. But this 𝘂 𝑠 ℎ 𝘂 𝘂 cos¹j𝜒j 𝘁ºℎ 2 𝑛 1 3 3 assumption is invalid during aggressive thrust variation, such 𝘂¤ = (56) as vertical maneuvers. Nevertheless, the ﬂight experiments ℎ 𝑠 𝑠 sin¹j𝜒j 𝘁º 3 𝑛 𝑙 given in Sec. VII will demonstrate that the internal dynamics are still stable during vertical maneuvers. sin𝘁¹2𝑎 𝜔 ¯ ¸ 𝐴 𝘂 º¹𝘂 𝘂 sin𝘁𝘂 ℎ 𝑠 sinj𝜒jº 𝑥 𝑥 3 1 3 2 3 𝑛 Since 𝒙 has seven states in total and there are four external cos𝘁¹2𝑎 𝜔 ¯ ¸ 𝐴 𝘂 º¹𝘂 𝘂 cos𝘁𝘂 ℎ 𝑠 cos 𝜒º 𝑦 𝑦 3 1 3 2 3 𝑛 in 𝘂¤ = states as per (43), we need to determine three internal states. 𝑠 ℎ sin¹j𝜒j 𝘁º 𝑙 3 The selection of internal states is not unique as long as the (57) condition (13) is satisﬁed, namely the ﬁrst-order derivatives of 𝜼 do not include 𝒖. Based on the above assumptions, we 𝑔𝑚 𝜎ℎ 𝑠 𝘂 𝛾𝑠 𝑣 3 𝑛 3 𝑛 𝑠 𝐴 ¹𝘂 𝘂 cos𝘁𝘂 ℎ 𝑠 cos𝜒º¹𝘂 𝘂 sin𝘁𝘂 ℎ 𝑠 sinj𝜒jº 𝘂¤ = 𝑙 𝑧 1 3 2 3 𝑛 1 3 2 3 𝑛 (58) hereby make the following choices for the internal states: ℎ sin¹j𝜒j𝘁º 2 3 2 3 𝘂 ℎ sin 𝜒¸ ℎ cos 𝜒 1 1 2 According to the ﬁrst Lyapunov criterion, the equilibrium of 6 7 6 7 6 7 6 7 𝘂 = 𝑞 cos𝘁𝑠 𝑝 sin𝘁 (48) 2 𝑙 the nonlinear system is asymptotically stable if the linearized 6 7 6 7 6 7 6 7 𝘂 𝑟¸𝑠 𝘇𝑉 3 𝑛 𝑧 system is asymptotically stable [33]. At the relaxed trimming 4 5 4 5 equilibrium, the internal states are 𝜼 ¯ = »0– 0– 𝑟 ¯¸𝑠 𝘇𝑉 ¼ , and 𝑛 𝑧 where the local linearized system is derived from (56)-(58) as 𝘇 = 𝑚 𝜎ℎ (49) 𝑣 3 2 3 2 3 𝘂¤ 𝘂 1 1 6 7 6 7 𝑨 O 1 21 6 7 6 7 and 𝘁 is deﬁned as per (38). Note that the ﬁrst internal state 𝘂¤ 𝘂 = 𝜼 ¯ (59) 2 2 6 7 6 7 0 𝛾𝐼 6 7 6 7 𝘂 is the projection of 𝒏 on 𝒚 axis as illustrated in Fig. 5. 𝘂¤ 𝘂 1 𝑑 𝑆 3 3 4 5 4 5 Substituting (47) into (48) gives the internal state at relaxed where 𝑨 is expressed as per (50). Note that the yaw damping hovering equilibrium 𝜼 ¯ = 𝝓¹𝒙 ¯ º = »0– 0– 𝑟 ¯¸𝑠 𝘇𝑉 ¼ . in 𝑛 𝑧 𝛾 is positive deﬁnite and the system matrix of (59) is block 𝑃𝑟𝑜𝑝𝑜𝑠𝑖𝑡𝑖𝑜𝑛 1 : The internal dynamics of the inner-loop diagonal. Therefore, if 𝑨 is Hurwitz, namely every eigenvalue system are locally asymptotically stable at the relaxed trim- of 𝑨 has strictly negative real part, then the linear system (59) ming equilibrium 𝒙 ¯ if and only if 𝜒 = 𝑠 j𝜒j is selected such in 𝑙 is asymptotically stable. Subsequently the local asymptotic that every eigenvalue of the following 𝑨 matrix has strictly stability of the internal dynamics is satisﬁed. negative real part: One may approximate 𝑟 ¯ and 𝜔 ¯ from (27) with 𝑴 and 𝑭 𝑎 𝑎 neglected: 𝑟 ¯ cos¹j𝜒j 𝘁º 1 𝑙 √︂ 𝑨 = (50) 𝑚 𝑔 𝑟 ¯Λ Δ sin¹j𝜒j 𝘁º 𝑟 ¯ =𝑠 𝑚 𝑔𝜎𝛾– 𝜔 ¯ = (60) 𝑛 𝑣 2𝘅 ¯ where 𝑅𝑒𝑚𝑎𝑟𝑘 2: From (50) and (60) we have 2 2 Λ = ¹𝐴 𝑟 ¯ 2𝑎 𝜔 ¯𝑠 º sin 𝘁¸¹𝐴 𝑟 ¯¸ 2𝑎 𝜔 ¯𝑠 º cos 𝘁 (51) 𝑥 𝑥 𝑛 𝑦 𝑦 𝑛 Re 𝝀 ¹𝑠 𝑠 – j𝜒jº =Re 𝝀 ¹𝑠 𝑠 – j𝜒jº (61) 𝑨 𝑙 𝑛 𝑨 𝑙 𝑛 1 1 where 𝝀 denotes eigenvalues of 𝑨 . Note that for a speciﬁc 𝑨 1 Δ =¹𝐴 𝑟 ¯ 2𝑎 𝜔 ¯𝑠 º sin𝘁 sinj𝜒j¸¹𝐴 𝑟 ¯¸ 2𝑎 𝜔 ¯𝑠 º cos𝘁 cos 𝜒 1 𝑥 𝑥 𝑛 𝑦 𝑦 𝑛 quadrotor, the value of 𝑠 𝑠 is identical under both failure 𝑙 𝑛 (52) scenarios indicated by (25) and (26). Therefore, we can further Proof. The transformation »𝜼– 𝜻¼ = 𝑻¹𝒙º expressed as (43) conclude from (61) that 𝝀 , i.e. the stability property of and (48) is a diﬀeomorphism if ℎ sin¹𝘁j𝜒jº ≠ 0. The inverse internal dynamics, remain invariant despite the failure type transformation 𝒙 = 𝑻 ¹»𝝃– 𝜼¼º thus can be obtained as if j𝜒j is ﬁxed. IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 8 Fig. 7: Poles of matrix 𝑨 and 𝑟 varying with j𝜒j2¹𝘁– 𝘁¸𝜋º. Fig. 8: Time series of 𝑦 , 𝘂 , and 𝑢 𝑢 of three tests 1 𝐵 2 1 1 2 The unshadded area is the permitted region of j𝜒j. Tests in with diﬀerent j𝜒j in the simulation. A step-input of position the simulation are marked in the bottom plot with diﬀerent command is given at 𝑡 = 1 s. symbols indicating if the ﬂight succeeds. Unstable internal dynamic region obtained from real ﬂight data in is shown to zero. As j𝜒j = 70 deg, the violation of constraint 𝑟 1 in the top plot, which slightly moves rightward indicating a leads to a small control eﬀectiveness 𝐵 . As a result, 𝑢 𝑢 2 1 2 larger admissible region of j𝜒j. Though 𝑨 is stable in the area signiﬁcantly oscillates during the maneuver and the drone shaded red, crash still occurs because of 𝑟 Ÿ 1 that violates crashed due to limited actuator dynamics. On the other hand, the constraint (40). when j𝜒j = 140 deg, the internal dynamics are unstable and divergent oscillation of 𝘂 occurs that makes the drone crash. C. Case Study: Selection of j𝜒j VI. Generalization to Other Failure Conditions As previous analysis presents, parameter j𝜒j need to be In this section, the inner-loop control scheme introduced in selected such that : (1) The matrix 𝑨 in (50) is Hurwitz Sec. IV is generalized to a quadrotor with complete loss of a for stable internal dynamics. (2) Condition (40) is satisﬁed for single rotor, or without rotor failure (nominal condition). an acceptable control eﬀectiveness on 𝑦 . In this section, we conduct a case study on a speciﬁc type A. Single Rotor Failure of quadrotor, a modiﬁed Parrot Bebop2, in the simulation 1) Internal Dynamics Analysis: For a quadrotor with a to demonstrate the eﬀect of j𝜒j on the overall controller single rotor failure, there are three permitted inputs. Therefore, performance. The inertial and geometric property of this three outputs can be deﬁned. Similar to the condition with quadrotor is listed in Table III. Without loss of generality, we failure of two rotors, one output is deﬁned as the altitude: assume rotor 2 and 4 are removed (𝑠 =1, 𝑠 = 1, 𝜒 = j𝜒j). 𝑛 𝑙 Thus from (60), we have 𝑟 ¯ = 26•4 rads– 𝜔 ¯ = 1015 rads. 𝑦 = 𝑍 (62) The top plot in Fig. 7 shows the poles of 𝑨 versus j𝜒j 2 The other two outputs are related to the reduced attitude ¹𝘁– 𝘁¸𝜋º. Note that j𝜒j = 𝘁¸𝑘𝜋 (𝑘 2 Zº causes singularity as control: (50) shows. The shadded gray area represents positive real part 𝑦 = ℎ 𝑛 (63) 2 1 of poles that render unstable internal dynamics. The bottom plot in Fig. 7 presents 𝑟 ¹j𝜒jº as given in (40) with diﬀerent 𝑦 = ℎ 𝑛 (64) 3 2 j𝜒j. The shaded red represents the violation of the constraint 𝐵 𝐵 𝐵 𝐵 𝑇 To align the body ﬁxed unit vector 𝒏 = »𝑛 – 𝑛 – 𝑛 ¼ with 𝑥 𝑦 𝑧 𝑟 1. 𝐵 𝑇 the reference 𝒏 = »ℎ – ℎ – ℎ ¼ , the preceding 𝑦 and 𝑦 1 2 3 2 3 In the simulation, the quadrotor is commanded to transfer should be stabilized to zero. from 𝑋 = 0 to 𝑋 = 3 m at 𝑡 = 1 s. Various selections of j𝜒j The relative degrees of the inner-loop system are 𝜌 = 𝜌 = 1 2 are tested and given in the bottom plot of Fig. 7. The ﬂights 𝜌 = 2 from (27). Therefore, the external states are within the unshaded area succeed in conducting the transfer maneuver, whereas most of those in the shaded area failed. 2 3 2 3 2 3 𝘉 𝑦 𝑍 1 1 6 7 6 7 6 7 6 7 6 7 6 7 Three tests in the simulation with respective j𝜒j equal to 𝘉 𝑦¤ 𝑉 2 1 𝑧 6 7 6 7 6 7 6 7 6 7 6 7 70, 105, and 140 degrees are further demonstrated. Fig. 8 𝘉 𝑦 ℎ 𝑛 3 2 1 6 7 6 7 6 7 = = (65) shows the time series of output 𝑦 , internal state 𝘂 , and 6 7 6 7 6 7 2 1 ℎ 𝑞¸ ℎ 𝑟 𝘉 𝑦¤ 4 2 3 2 6 7 6 7 6 7 𝑢 𝑢 of these three ﬂights. When j𝜒j = 105 deg, the transition 6 7 6 7 6 7 1 2 ℎ 𝑛 𝘉 𝑦 5 3 2 6 7 6 7 6 7 is successful where both 𝑦 and internal states 𝘂 converge 6 7 6 7 6 7 2 1 𝘉 𝑦¤ ℎ 𝑝 ℎ 𝑟 6 3 3 1 4 5 4 5 4 5 IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 9 7 TABLE I: Parameters of the tested quadrotor. Note that 𝒙 2 R and there are 6 external states, the only in internal state can be selected as par. value unit par. value unit 𝘂 = 𝑟¸ 𝘇 𝑉 ¸ 𝘇 𝑝¸ 𝘇 𝑞 (66) 1 1 𝑧 2 3 3 2 𝐼 1.45𝑒 kg m 𝑚 0.410 kg 𝑥 𝑣 3 2 𝐼 1.26𝑒 kg m 𝑏 0.145 m where 𝘇 , 𝘇 , 𝘇 are calculated such that 𝘂¤ does not include 𝑦 1 2 3 1 3 2 𝐼 2.52𝑒 kg m 𝛽 52.6 deg the control input. These coeﬃcients are constant and related 𝑧 6 2 3 𝐼 8.00𝑒 kg m 𝛾 1.50𝑒 Nms to the handedness of the remaining rotors. 𝑝 6 2 𝘅 ¯ 1.90𝑒 kg m 𝜎 0.01 m After some tedious algebra, the zero dynamics can be calculated as 𝛾 Π TABLE II: Control gains. 𝘂¤ = 𝘂 ¸ 𝘂 ¸𝑔𝘇 (67) 1 1 1 ℎ Θ 2 3 1 outer 𝑘 »𝑠 ¼ 𝑘 »𝑠 ¼ 𝑘 »𝑠 ¼ 𝑝 𝑖 𝑑 where -loop 1.0 0.1 1.0 𝐵 𝐵 Θ = 𝑛 𝘇 ℎ ¸𝑛 𝘇 ℎ ¸ 1 (68) 2 3 3 3 𝑥 𝑦 2 1 2 1 inner 𝑘 »𝑠 ¼ 𝑘 »𝑠 ¼ 𝑘 »𝑠 ¼ 𝑘 »𝑠 ¼ 𝑎–𝑝 𝑎–𝑑 𝑧–𝑝 𝑧–𝑑 𝐵 𝐵 𝐵 𝐵 -loop 50 30 15 10 Π = 𝐴 𝑛 𝑛 ℎ ¸ 𝐴 𝑛 𝘇 ¸ 𝐴 𝑛 𝘇 (69) 𝑧 3 𝑥 2 𝑦 3 𝑥 𝑦 𝑦 𝑥 𝐵 𝐵 Particularly, when 𝑛 = 𝑛 = 0, namely the drone spins about 𝑥 𝑦 it thrust direction, we have where 𝑒 is the yaw angle tracking error; 𝑘 and 𝑘 are 𝜓 𝑝–𝜓 𝑑–𝜓 positive gains. 𝘂¤ =𝛾¹𝘂 𝘂 ¯ º (70) 1 1 1 Note that the rotor angular acceleration 𝜔¤ may deterio- Since the yaw damping 𝛾 ¡ 0, the internal state 𝘂 at the rate the yaw control performance while implementing this equilibrium 𝘂 ¯ is stable. 1 approach. Interested readers are referred to [21] to tackle this 2) Control Law: The above analysis demonstrates that the problem for a nominal quadrotor. stability of internal dynamics with the selection of outputs in (62)-(64). After selecting the control outputs, the same control VII. Experimental Validation scheme presented in Fig. 4 can be applied for the single rotor The proposed control method has been validated on a failure condition. Without loss of generality, we assume that modiﬁed Parrot Bebop2 drone with a lighter battery and rotor 4 is removed, then we have camera module removed. The geometric and moment of inertia 2 2 2 𝑇 𝒖 = »𝜔 – 𝜔 – 𝜔 ¼ (71) properties are given in Table III. During the ﬂight test, a 1 2 3 motion capturing system (OptiTrack) with 12 cameras pro- With the same procedure introduced in Sec. IV, the control vided the position measurements of the 4 markers attached to law for a quadrotor subjected to a single rotor failure can be the drone at 120 Hz. The inertial measurement unit (IMU) obtained using (22), where measured the angular rates (from gyroscope) and the speciﬁc force (from the accelerometer) at 512 Hz. A built-in brushless 2 ¤ ¥ 3 𝑘 ¹𝘉 𝑍 º 𝑘 ¹𝘉 𝑍 º¸ 𝑍 º 𝑧–𝑝 1 ref 𝑧–𝑑 2 ref ref 6 7 DC (BLDC) motor controller controled the rotor speeds of 6 7 𝑘 𝘉 𝑘 𝘉 𝝂 = (72) 𝑎–𝑝 3 𝑎–𝑑 4 each propeller, and also measured the rotor rotational rates 6 7 6 7 𝑘 𝘉 𝑘 𝘉 𝑎–𝑝 5 𝑎–𝑑 6 in RPM at 512 Hz. Subsequently, an Extended Kalman Filter 4 5 (EKF) was implemented to estimate the position, velocity of ¹𝝆º ¥ ¥ 𝒚 = 𝒚¥ = »𝑎¤ 𝑅 – ℎ – ℎ ¼ (73) 𝑧– 𝑓 33– 𝑓 1– 𝑓 2– 𝑓 the center of mass, and the attitude of the body frame. The ˆ proposed controller and the EKF were run onboard at 500 Hz The control eﬀectiveness matrix B can be estimated using (5) with the original processor Parrot P7 dual-core CPU Cortex and (6): 9. The control gains are given in Table II. 2 3 𝘅 ¯𝑅 𝑚 𝘅 ¯𝑅 𝑚 𝘅 ¯𝑅 𝑚 33– 𝑓 𝑣 33– 𝑓 𝑣 33– 𝑓 𝑣 6 7 ˆ 6 7 B¹𝒙 º = 𝘅 ¯𝑏 sin𝛽 𝘅 ¯𝑏 sin𝛽 𝘅 ¯𝑏 sin𝛽 6 7 A. Flight tests in windless conditions 6 7 𝘅 ¯𝑏 cos𝛽 𝘅 ¯𝑏 cos𝛽 𝘅 ¯𝑏 cos𝛽 4 5 (74) The ﬁrst set of ﬂights tested the 3-D trajectory tracking of the quadrotor without wind disturbance. Fig. 1 presents snapshots of the tested quadrotor with failure of one and B. Without Rotor Failure two rotors within 0.3 s; Fig. 9 shows the reference and the For a multi-rotor drone with more than three actuators, such measured positions. as a nominal quadrotor or a hexacopter, there are four or more For those with two rotor failures, we removed the rotor 1 permitted control inputs. We can then introduce the fourth and rotor 3, without loss of generality. Fig. 10a shows the output related to the yaw control: second output 𝑦 and the ﬁrst internal state 𝘂 . As is introduced 2 1 𝑦 = 𝑟 (75) 4 in Sec. IV, they represent the reduced attitude and need to converge to zero. Despite the misalignment during the take- An independent yaw controller can be appended to provide the oﬀ maneuver at the ﬁrst 3 seconds, a slight tracking error reference yaw rate 𝑟 , such as a PD controller: ref of 𝑦 is observed which is presumably due to the bias of 𝑟 =𝑘 𝑒 𝑘 𝑒¤ (76) the center of mass. In this ﬂight, j𝜒j = 90 deg was selected ref 𝑝–𝜓 𝜓 𝑑–𝜓 𝜓 IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 10 Fig. 11: Variables during the position tracking ﬂight test with failure of rotor 3. From top to bottom are: the output 𝑦 and 𝑦 ; the angular speed measurements of the rotor 1, 2, and 4; Fig. 9: Position tracking task under windless condition. Blue the angular rates measurements. solid lines indicate the 3-D position of the tested quadrotor with failure of two opposing rotors. Red dash lines indicate for stable internal dynamics. Consequently, the internal state those under single rotor failure conditions. The reference 𝘂 was conﬁned around zero. Fig. 10b shows the angular positions are presented as black dot dash lines. speeds of rotor 2 and 4 that remained almost constant at 𝜔 ¯ = 1000 rad/s during the horizontal maneuvers, which was in-line with the 𝐴𝑠𝑠𝑢𝑚𝑝𝑡𝑖𝑜𝑛 3. A yaw rate at about 25 rad/s shown in Fig. 10c indicates the fast spinning motion of the damaged quadrotor. With the same controller and the same set of gains, the condition with one rotor failure was also tested. The rotor 3 was removed in this test. As is shown in Fig. 11a, the internal state 𝘂 is replaced by the third output 𝑦 because of the 1 3 addition of one rotor compared to the condition with failure of two rotors. The reference 𝑦 = 𝑦 = 0 was employed 2–ref 3–ref in this ﬂight that required the rotor 1 (the one diagonal to the failed rotor) to generate no force. However, due to the lower saturation of rotor 1 presented in Fig. 11b, a constant tracking error of 𝑦 and 𝑦 are observed. In spite of these attitude 2 3 tracking errors, the drone under both failure cases were able to track the position commands. B. Eﬀect of 𝜒 in the Condition with Failure of Two Opposing Rotors The quadrotor in the condition with failure of two opposing rotors was tested in the hover region with diﬀerent sets of j𝜒j to experimentally demonstrate its eﬀect on the stability of the internal dynamics. Fig. 12 shows the internal state 𝘂 during a hovering ﬂight where the parameter j𝜒j was initialized at Fig. 10: Variables during the position tracking ﬂight test 90 deg which would lead to stable internal dynamics. At 𝑡 = 5 s, with failures of rotor 1 and rotor 3. From top to bottom j𝜒j was changed to 180 deg during the ﬂight and the internal are: the output 𝑦 and internal state 𝘂 ; the angular speed 2 1 state became unstable. This complies with the prediction from measurements of the rotor 2 and rotor 4; the angular rates 𝑃𝑟𝑜𝑝𝑜𝑠𝑖𝑡𝑖𝑜𝑛 1. measurements. It is noteworthy that the stable region boundary moves slightly to the right compared to the theoretical prediction, as is shown in Fig. 7. As a consequence, the admissible region IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 11 Fig. 12: Internal state 𝘂 in with diﬀerent selection of j𝜒j. Before 𝑡 = 5 s, j𝜒j = 90 deg is selected which leads to stable internal dynamics in both conditions. After 𝑡 = 5 s, j𝜒j is changed to 180 deg and the internal dynamics become unstable. TABLE III: Maximum ﬂight speed of the INDI and LQR controller with various control gains. 𝑄 for LQR indicates the cost on the reduced attitude. Fig. 13: 3D trajectories of the damaged quadrotor under 𝑉 = wind 5 m/s, where A to G represent the setpoints. INDI ﬁnished the INDI LQR trajectory tracking task while LQR failed during the transition 2 1 𝑘 »𝑠 ¼ 𝑘 »𝑠 ¼ 𝑉 [m/s] 𝑄 »¼ 𝑉 [m/s] 𝑎–𝑝 𝑎–𝑑 max max from setpoint D to E. 5 1 7.8 1 4.6 10 2 8.3 3 5.9 50 30 8•8 10 5.2 100 30 8.2 20 5.1 200 50 7.8 30 6.3 *Gains for the trajectory tracking task. of j𝜒j becomes larger. The diﬀerence might come from the omission of the aerodynamic damping on pitch and roll rate while conducting internal dynamic analysis. VIII. Validations in a Wind Tunnel To validate the robustness against unmodeled aerodynamic forces and moments in the high-speed ﬂight regime, ﬂight tests have been carried out in the Open Jet Facility (OJF), a large scale wind tunnel with an aperture of 2.85 meters (see Fig. 2). The parameter j𝜒j = 105 deg that lies roughly in the center of its admissible region is selected, as Fig. 7 illustrates. The linear quadratic regulator (LQR) is selected as the baseline for comparison in the wind tunnel. This method has been validated in practice in [14]. The same set of gains from this paper were implemented for comparison. Speciﬁcally, the costs on control inputs were set to one, with units N ; the cost on the reduced attitude was set to 20 and the cost on angular rates was set to zero. Since a diﬀerent drone was used, the Fig. 14: Reduced attitude, rotor speeds and lateral position time constant of the ﬁrst-order actuator model is set as 30 ms 𝑌 of the quadrotor during the transition from setpoint D that diﬀers from [14]. Both INDI and LQR used the same to E. The reduced attitude of the drone controlled by LQR outer-loop control gains as given in Table. II. became unstable, before the loss of OptiTrack measurement shaded in red, and eventually crashed. In comparison, the drone controlled by the INDI approach succeed to ﬁnish this A. Trajectory Tracking Task maneuver. A trajectory tracking task was performed under a wind ﬂow of 5 m/s. The wind ﬂow was along the negative direction IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 12 of the 𝒙 axis. Fig. 13 plots the trajectories using INDI and LQR respectively. The drone tracked setpoints A to G in sequence every 3 seconds. In addition, the step reference positions (𝑋 and 𝑌 ) were smoothed by a ﬁrst-order ﬁlter ref ref with a time constant 1 s. As Fig. 13 shows, both methods successfully tracked the setpoints before point D. However, the LQR approach failed during the transition from setpoint D to E, which was perpendicular to the wind ﬂow. Fig. 14 compares states between the two approaches in this period. From the plotted data, we can ﬁnd that the reduced attitudes of both methods were bounded before conducting the maneuver, despite oscillations of reduced attitude and rotor speeds caused by the wind ﬂow and yaw motion. During the maneuver from point D to E, the reduced attitude of the LQR approach diverged from zero (the linearization point). The instability of the inner-loop attitude control caused the failure of trajectory tracking and eventually lead to a crash. For LQR, the rotor speed commands (i.e. the control input) were naturally ampliﬁed (at around 𝑡 = 2 s in the mid-right plot of Fig. 14) to stabilize the reduced attitudes (ℎ and ℎ ) 1 2 as they were diverging from the equilibrium (top-right plot). Fig. 15: Variables during the wind-tunnel maximum ﬂight These increase of control input may destabilize the system in speed test of INDI. From top to bottom are: positions of the the presence of nonlinearities and model uncertainties caused drone; the internal state 𝘂 and the output 𝑦 ; the angular by the wind ﬂow. By contrast, the INDI approach used the 1 2 ¹𝝆º speed of the remaining rotor (rotor 2 & 4); the time series second-order derivative of the reduced attitude (𝒚 in (22)) of the wind speed which gradually increased until the loss-of- to compensate for the model uncertainties. The nonlinearities control happened. The red area represents loss of OptiTrack are also handled by the feedback linearization step of INDI. measurement. Thereby the reduced attitude can be stabilized without drasti- cally increasing the control input. B. Maximum Flight Speed Test To explore the maximum ﬂight speed of a quadrotor under the failure of two opposing rotors, maximum speed ﬂight tests were conducted using both INDI and LQR. During the entire ﬂight, the drone was hovering at a setpoint located in front of the wind tunnel (𝑷 = »0– 0– 1•5¼ ). The wind speed was ref gradually increased from 𝑉 = 0 to a 𝑉 until the drone wind max crashed. Table. III compares the maximum ﬂight speed achieved by the two approaches with diﬀerent set of gains. Similarly to the trajectory tracking task in the preceding section, INDI outperforms the LQR approach in terms of maximum ﬂight speed. Be that as it may, the drone controlled by LQR was still stabilized at relatively high-speeds thanks to the inherent stabilizing property of feedback control. Fig. 15 shows the time series of position, 𝘂 , 𝑦 , rotor speeds 1 2 and wind speed during a ﬂight controlled by INDI, where the drone crashed at 𝑉 = 8•8 m/s when 𝑡 = 107 s. The variations wind of 𝘂 , 𝑦 and rotor speeds signiﬁcantly increased with the 1 2 wind speed. Despite the gradual increase of the oscillation, Fig. 16: Close-up of the variables of Fig. 15 during last 7 the internal state 𝘂 was bounded near zero and subsequently seconds before the crash. The red area represents loss of ensured successful position tracking. OptiTrack measurement. To investigate the cause of loss-of-control of INDI, Fig. 16 shows the close-up of Fig. 15 during the last 7 seconds before the crash. In addition to the rotor speed measurements, the away from the setpoint along the wind ﬂow. Meanwhile, the rotor speed commands are also plotted. From the reduction of increase of 𝑍 indicates the continues reduction of the altitude 𝑋 in the top plot, we can ﬁnd that the quadrotor was blown in this process. These phenomena are believed caused by the IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 13 saturation of motors under wind resistance, which can be clearly seen in the bottom two plots of Fig. 16. In addition to the motor saturation, the control input lag due to motor dynamics can be observed, which may cause the gradual divergence of 𝘂 ad 𝑦 in the second plot of Fig. 16. We hence 1 2 infer that the motor capacity (bandwidth and power limit) is a major limiting factor of the maximum ﬂight speed. C. High-Speed Flight with Imperfect State Estimations Since the ultimate goal of this work is improving drone safety during the high-speed ﬂight in outdoor environments, preliminary validations of the proposed method using imper- fect state estimations have been conducted in the wind-tunnel. The sampling rate of the motion capturing system was reduced from 120 Hz to 10 Hz to simulate GPS-like update rates. Only Fig. 17: Comparison of pitch and roll estimations between the position measurements were transmitted to the onboard ﬂight complementary ﬁlter and the ground truth in diﬀerent ﬂight controller. A complementary ﬁlter [35] was implemented by speeds demonstrating degradation of the compensatory ﬁlter fusing the measurements from the IMU and the magnetometer, at these condition. to provide attitude estimates. In this setting, the INDI controlled drone achieved con- trolled ﬂight at 8.4 m/s inside the wind tunnel, indicating ro- bustness to signiﬁcant attitude estimation errors. These errors can be seen in Fig. 17, which compares the pitch and roll angles from the onboard complementary ﬁlter with the ground truth obtained with the OptiTrack system at three diﬀerent ﬂight speeds. As a consequence, the tracking errors of the re- duced attitude were greater using the onboard complementary Fig. 18: Boxplots comparing the tracking error of 𝑦 , denoted ﬁlter, especially at 0m/s and 5m/s as Fig. 18 shows. Note that by 𝑒 , between ﬂights with OptiTrack aided attitude estimation the increase of tracking error was less apparent at 8m/s where and those with onboard complementary ﬁlter, in diﬀerent ﬂight the drone was near the ﬂight envelope boundary, because the speeds. controller performance was not only degraded by imperfect state estimations, but also motor limitations. We hypothesize that the degradation of the complementary of signiﬁcant aerodynamic eﬀects, the control method is able ﬁlter is caused by the strong aerodynamic forces and cen- to stabilize the quadrotor at over 8.0 m/s. Compared with trifugal forces measured by the accelerometers. Improving the the linear quadratic regulator (LQR) approach, the proposed accuracy of the state estimator at high ﬂight speeds and high method was found to have higher robustness against model angular rate conditions is out of the scope of this paper, but uncertainties brought by the signiﬁcant aerodynamic eﬀects. it is a highly recommended future research. Flights with imperfect state estimations from onboard sen- sors have been conducted. Flight data in the high-speed regime IX. CONCLUSION with onboard sensors reveal the adverse eﬀects of aerody- namics on the state estimation. Future work is recommended In this research, an incremental nonlinear fault-tolerant to focus on improving the attitude estimation using onboard control method is developed for a quadrotor subjected to sensors by taking into account the eﬀect of aerodynamics and complete loss of two opposing rotors. The internal dynamics high angular rate motion on the state estimator. of this under-actuated control problem is analyzed. Thereby a criterion is given to select proper control outputs that guarantee ACKNOWLEDGMENT the stability of internal dynamics, which has been validated in both simulations and ﬂight tests. The control scheme can be The authors would like to appreciate the support from the generalized to a nominal quadrotor, or one with the loss of a MAVLab and the help of these individuals: Leon Sĳbers, Bram single rotor. Strack van Schĳndel, Bo Sun, Lan Yang and Prashant Solanki. The proposed method uses the incremental nonlinear dy- We also appreciate the Aerodynamics Group of TU Delft for namic inversion (INDI) approach to control the selected out- their permission to use the Open Jet Facility. puts. The INDI approach replaces non-input related model References terms with sensor measurements, which reduces the model de- pendencies and consequently increases the robustness against [1] T. Li, Y. Zhang, and B. W. Gordon, “Passive and active nonlinear fault-tolerant control of a quadrotor unmanned aerial vehicle based wind disturbances in the high-speed ﬂight regime. Flight tests on the sliding mode control technique,” Proceedings of the Institution of a quadrotor with complete loss of two opposing rotors of Mechanical Engineers, Part I: Journal of Systems and Control are conducted in an open jet wind tunnel. In the presence Engineering, vol. 227, pp. 12–23, jan 2013. IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 14 [2] L. Besnard, Y. B. Shtessel, and B. Landrum, “Quadrotor vehicle control [23] E. Tal and S. Karaman, “Accurate Tracking of Aggressive Quadrotor via sliding mode controller driven by sliding mode disturbance observer,” Trajectories Using Incremental Nonlinear Dynamic Inversion and Dif- Journal of the Franklin Institute, vol. 349, pp. 658–684, mar 2012. ferential Flatness,” pp. 4282–4288, dec 2018. [24] S. Sun, L. Sĳbers, X. Wang, and C. de Visser, “High-Speed Flight of [3] X. Wang, S. Sun, E.-J. van Kampen, and Q. Chu, “Quadrotor Fault Quadrotor Despite Loss of Single Rotor,” IEEE Robotics and Automation Tolerant Incremental Sliding Mode Control driven by Sliding Mode Letters, vol. 3, pp. 3201–3207, oct 2018. Disturbance Observers,” Aerospace Science and Technology, vol. 87, [25] X. Yun and Y. Yamamoto, “Internal dynamics of a wheeled mobile pp. 417–430, apr 2019. robot,” in Proceedings of 1993 IEEE/RSJ International Conference on [4] X. Wang, E.-J. van Kampen, and Q. Chu, “Quadrotor fault-tolerant Intelligent Robots and Systems (IROS ’93), vol. 2, pp. 1288–1294, IEEE, incremental nonsingular terminal sliding mode control,” Aerospace Science and Technology, vol. 95, p. 105514, dec 2019. [26] F. Lewis, A. Das, and K. Subbarao, “Dynamic inversion with zero- [5] A. Lanzon, A. Freddi, and S. Longhi, “Flight Control of a Quadrotor dynamics stabilisation for quadrotor control,” IET Control Theory & Vehicle Subsequent to a Rotor Failure,” Journal of Guidance, Control, Applications, vol. 3, pp. 303–314, mar 2009. and Dynamics, vol. 37, pp. 580–591, mar 2014. [27] E. M. Wallner and K. H. Well, “Attitude Control of a Reentry Vehicle [6] V. Lippiello, F. Ruggiero, and D. Serra, “Emergency landing for a with Internal Dynamics,” Journal of Guidance, Control, and Dynamics, quadrotor in case of a propeller failure: A backstepping approach,” vol. 26, pp. 846–854, nov 2003. in 2014 IEEE/RSJ International Conference on Intelligent Robots and [28] G. M. Hoﬀmann, H. Huang, S. L. Waslander, and C. J. Tomlin, Systems, pp. 4782–4788, IEEE, sep 2014. “Precision ﬂight control for a multi-vehicle quadrotor helicopter testbed,” [7] V. Lippiello, F. Ruggiero, and D. Serra, “Emergency landing for a Control Engineering Practice, vol. 19, pp. 1023–1036, sep 2011. quadrotor in case of a propeller failure: A PID based approach,” in 2014 [29] R. Mahony, V. Kumar, and P. Corke, “Multirotor aerial vehicles: IEEE International Symposium on Safety, Security, and Rescue Robotics Modeling, estimation, and control of quadrotor,” IEEE Robotics and (2014), pp. 1–7, IEEE, oct 2014. Automation Magazine, 2012. [8] P. Lu and E.-J. van Kampen, “Active fault-tolerant control for quadrotors [30] S. Sun and C. de Visser, “Aerodynamic Model Identiﬁcation of subjected to a complete rotor failure,” in 2015 IEEE/RSJ International a Quadrotor Subjected to Rotor Failures in the High-Speed Flight Conference on Intelligent Robots and Systems (IROS), pp. 4698–4703, Regime,” IEEE Robotics and Automation Letters, vol. 4, pp. 3868–3875, IEEE, sep 2015. oct 2019. [9] Y. V. Morozov, “Emergency Control of a Quadrocopter in Case of [31] P. W. Fortescue and G. G. Swinerd, “Rigid-Body Attitude Control,” Failure of Two Symmetric Propellers,” Automation and Remote Control, IEEE Control Systems, vol. 31, pp. 30–51, june 2011. vol. 79, pp. 463–478, mar 2018. [32] X. Wang, E.-J. van Kampen, Q. Chu, and P. Lu, “Stability Analysis [10] C. de Crousaz, F. Farshidian, M. Neunert, and J. Buchli, “Uniﬁed motion for Incremental Nonlinear Dynamic Inversion Control,” Journal of control for dynamic quadrotor maneuvers demonstrated on slung load Guidance, Control, and Dynamics, vol. 42, pp. 1116–1129, may 2019. and rotor failure tasks,” in 2015 IEEE International Conference on [33] H. K. Khalil, Nonlinear Systems. Prentice-Hall, 2002. Robotics and Automation (ICRA), pp. 2223–2229, IEEE, may 2015. [34] X. Wang, E.-J. van Kampen, Q. Chu, and P. Lu, “Incremental Sliding- [11] M. W. Mueller and R. D’Andrea, “Stability and control of a quadrocopter Mode Fault-Tolerant Flight Control,” Journal of Guidance, Control, and despite the complete loss of one, two, or three propellers,” in 2014 IEEE Dynamics, vol. 42, pp. 244–259, feb 2019. International Conference on Robotics and Automation (ICRA), pp. 45– [35] R. Mahony, T. Hamel, and J.-M. Pﬂimlin, “Nonlinear Complementary 52, IEEE, may 2014. Filters on the Special Orthogonal Group,” IEEE Transactions on Auto- [12] A.-R. Merheb, H. Noura, and F. Bateman, “Emergency Control of matic Control, vol. 53, pp. 1203–1218, jun 2008. AR Drone Quadrotor UAV Suﬀering a Total Loss of One Rotor,” IEEE/ASME Transactions on Mechatronics, vol. 22, pp. 961–971, apr [13] J. Stephan, L. Schmitt, and W. Fichter, “Linear Parameter-Varying Control for Quadrotors in Case of Complete Actuator Loss,” Journal of Guidance, Control, and Dynamics, vol. 41, pp. 2232–2246, oct 2018. [14] M. W. Mueller and R. D’Andrea, “Relaxed hover solutions for mul- ticopters: Application to algorithmic redundancy and novel vehicles,” The International Journal of Robotics Research, vol. 35, pp. 873–889, jul 2016. [15] W. Zhang, M. W. Mueller, and R. D’Andrea, “A controllable ﬂying vehi- cle with a single moving part,” in 2016 IEEE International Conference on Robotics and Automation (ICRA), vol. 2016-June, pp. 3275–3281, Sihao Sun received his B.Sc and M.Sc degree IEEE, may 2016. in Aerospace Engineering from Beihang University, [16] C. Russell, J. Jung, G. Willink, and B. Glasner, “Wind tunnel and hover China, in July 2014 and February 2017 respectively. performance test results for multicopter UAS vehicles,” Annual Forum Since September 2016, he has been working to- Proceedings - AHS International, 2016. wards the Ph.D. degree of Aerospace Engineering at [17] S. Sun, C. C. de Visser, and Q. Chu, “Quadrotor Gray-Box Model Section of Control & Simulation, Delft University Identiﬁcation from High-Speed Flight Data,” Journal of Aircraft, vol. 56, of Technology, the Netherlands. In 2020, he was pp. 645–661, mar 2019. a visiting scholar at the Robotics and Perception [18] S. Sieberling, Q. P. Chu, and J. A. Mulder, “Robust Flight Control Using Group, University of Zurich. His research interest Incremental Nonlinear Dynamic Inversion and Angular Acceleration includes system identiﬁcation, aerial robotics, and Prediction,” Journal of Guidance, Control, and Dynamics, vol. 33, nonlinear control. pp. 1732–1742, nov 2010. [19] P. Acquatella, W. Falkena, E.-J. van Kampen, and Q. P. Chu, “Robust Nonlinear Spacecraft Attitude Control using Incremental Nonlinear Dynamic Inversion.,” aug 2012. [20] P. Simplício, M. Pavel, E.-J. van Kampen, and Q. Chu, “An acceleration Xuerui Wang received her Ph.D. degree in measurements-based approach for helicopter nonlinear ﬂight control Aerospace Engineering from Delft University of using Incremental Nonlinear Dynamic Inversion,” Control Engineering Technology, the Netherlands, in July 2019. From Practice, vol. 21, pp. 1065–1077, aug 2013. May 2019 to May 2020, she was a Postdoctoral [21] E. J. Smeur, Q. Chu, and G. C. de Croon, “Adaptive incremental researcher with the Smart and Aeroelastic Struc- nonlinear dynamic inversion for attitude control of micro air vehicles,” tures Laboratory, Delft University of Technology, the Journal of Guidance, Control, and Dynamics, 2016. Netherlands. Since May 2020, she has been with the [22] E. J. Smeur, G. C. de Croon, and Q. Chu, “Gust disturbance alleviation faculty of Aerospace Engineering, Delft University with Incremental Nonlinear Dynamic Inversion,” in 2016 IEEE/RSJ of Technology, the Netherlands, as an Assistant International Conference on Intelligent Robots and Systems (IROS), Professor. Her research interests include nonlinear pp. 5626–5631, IEEE, oct 2016. control, fault-tolerant control and aeroservoelasticity. IEEE TRANSACTIONS ON ROBOTICS. PREPRINT VERSION. DOI: 10.1109/TRO.2020.3010626. © 2020 IEEE 15 Qiping Chu received his Ph.D degree from the Faculty of Aerospace Engineering, Delft University of Technology in 1987. He is presently an associate professor and the head of Aerospace Guidance, Nav- igation and Control Cluster at the Section of Control and Simulation, Faculty of Aerospace Engineering, TU Delft. The research interests of Dr. Chu are nonlinear control, adaptive control, system identiﬁ- cation and state estimation with applications to fault tolerant control, sensor integration, data analysis, and fault detection/diagnosis. Coen de Visser received the M.Sc. degree from the Delft University of Technology in 2007. In 2011 he received his Ph.D degree from the faculty of Aerospace Engineering at the Delft University of Technology in The Netherlands. Between 2011 and 2012 he was a Postdoctoral Fellow at the Delft Center for Systems and Control where he worked on the development of a very large scale distributed control system for the adaptive optics system of the European Extremely Large Telescope. In 2015 and 2016 he was a visiting scholar at the University of California at Santa Barbara, and NASA Ames research centre respectively. Currently, he is an assistant professor at the Faculty of Aerospace Engineering at Delft University of Technology. His research interests are multivariate spline theory, aircraft system identiﬁcation, ﬂight envelope prediction and protection, and fault tolerant control. He has served in the international program committee of the European Guidance, Navigation, and Control conference between 2013 and 2019. He is the co-chair of the Council of European Aerospace Societies (CEAS) Technical Committee on Guidance Navigation & Control, and serves in the American Institute of Aeronautics and Astronautics (AIAA) technical committee on Intelligent Systems. He is an Associate Editor for the International Journal of Micro Aerial Vehicles.

Electrical Engineering and Systems Science – arXiv (Cornell University)

**Published: ** Feb 18, 2020

Loading...

You can share this free article with as many people as you like with the url below! We hope you enjoy this feature!

Read and print from thousands of top scholarly journals.

System error. Please try again!

Already have an account? Log in

Bookmark this article. You can see your Bookmarks on your DeepDyve Library.

To save an article, **log in** first, or **sign up** for a DeepDyve account if you don’t already have one.

Copy and paste the desired citation format or use the link below to download a file formatted for EndNote

Access the full text.

Sign up today, get DeepDyve free for 14 days.

All DeepDyve websites use cookies to improve your online experience. They were placed on your computer when you launched this website. You can change your cookie settings through your browser.