Get 20M+ Full-Text Papers For Less Than $1.50/day. Start a 14-Day Trial for You or Your Team.

Learn More →

Incremental Nonlinear Fault-Tolerant Control of a Quadrotor with Complete Loss of Two Opposing Rotors

Incremental Nonlinear Fault-Tolerant Control of a Quadrotor with Complete Loss of Two Opposing... 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 flight 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 flight condition (> 8m/s). The incremental nonlinear dynamic inversion (INDI) approach which excels in handling model uncertainties is adopted to compensate for the significant unknown aerodynamic effects. The internal dynamics of such an underactuated system have been analyzed, and subsequently stabilized by re-defining 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, flight 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 significant 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 flight 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 flight video can be found safety in the face of unexpected structural and actuator failures. via https://youtu.be/-4rXX4D5HlA Among different types of multi-rotor drones, quadrotors excel in their structural simplicity. However, they suffer more from actuator damages due to a lack of actuator redundancy. proposed by [14] indicates that the hovering flight 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 effectiveness, which has been extensively studied specially designed configuration, 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 effects are considered such as the rotational damp- In-flight validations have been achieved by several pieces of ing [13], [14]. However, in out-door applications, significant 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 significant 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 flight 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 fixed 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 fixed-wing aircraft [18], is fixed 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 configuration 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 define 𝒙 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 suffers less from model uncertainties caused by significant any vector 𝒂 2 R . aerodynamic effects during high-speed flight. 𝐵 𝐵 The variables 𝑭 and 𝑴 denote the resultant force and To validate the proposed controller, flight 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 flight speed. This could significantly 𝐵 6 7 𝑭 = 0 ¸ 𝑭 (5) 6 7 increase the safety of quadrotors by expanding the flight 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 flight 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 coefficient valid in the hovering condition; a quadrotor with failure of two opposing rotors. Section V 𝜎 is a constant ratio between the thrust coefficient and drag elaborates on the selection of control outputs and the stability coefficient 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 flight test results to the body. 𝐼 denotes the moment of inertia of each rotor in low-speed and high-speed flight 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 significant model uncertainties 𝑭 and 𝑴 , 𝑎 𝑎 for instance, in high-speed flight conditions where significant aerodynamic effects 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: Definition 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 defined 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 coefficient [11], [13]. model terms with direct sensor measurements, or sensor In the high-speed flight condition, there are significant measurement derived quantities, thereby greatly improving aerodynamic effects 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 briefly reviewed. Readers may refer to [32] adopted by [14] in the quadrotor fault tolerant control problem. and [33] for further details. We hereby briefly introduce the concept. For a quadrotor with complete rotor failures, the full state Consider a nonlinear input-affine 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 fields. to a thrust vector pointing problem without considering the 𝑛 𝑛𝑚 𝑮 : R ! R is a function mapping with smooth vector yaw angle. fields as columns. The number of outputs is not larger than Define a unit vector 𝒏 fixed 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-efficient transformed to the normal form including internal states 𝜼 and [11] where 𝒏 aligns with the instantaneous thrust direction. external states 𝝃: Define 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 field 𝒉. 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 fields 𝒇¹𝒙º [33]. To align 𝒏 with 𝒏 , we can control ℎ and ℎ to track 𝜌 indicates the relative degree of the 𝑖th output 𝑦 . 𝑑 1 2 𝑖 𝑖 𝐵 𝐵 𝑛 and 𝑛 respectively. To be specific, with the selection of By defining 𝜌 ¯ = Σ 𝜌 as the sum of relative degrees 𝑥 𝑦 𝑖 𝐵 𝑇 𝒏 = »0– 0– 1¼ , ℎ and ℎ need to be stabilized to zero. of each output, one can define 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 first-order derivative of 𝜼 as defined per (10) does 2 z,ref inner-loop not include control input 𝒖. The nonlinear transformation 𝑻¹𝒙º ref is a diffeomorphism (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 effort of modeling. The control effective- 𝒚 = 𝜶¹𝒙º¸B¹𝒙º𝒖 (15) ness matrix B is relatively easier to be estimated offline or ¹𝜌 º ¹𝜌 º ¹𝜌 º 1 2 𝑙 ¹𝝆º 𝑇 where 𝒚 = »𝑦 – 𝑦 –•••– 𝑦 ¼ . If matrix B has full 1 2 𝑙 identified 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 filtered in practice. To synchronize the time delay the matrix; 𝝂 2 R is called the pseudo-input. With a full caused by these filters, 𝒖 also need to be filtered with the knowledge of 𝜶¹𝑥º and B¹𝒙º, control law (16) yields the same cut-off frequency [21]. We use subscript »¼ to denote closed loop dynamics the filtered 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 differentiable, 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 first-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 differentiable. Therefore, ref where Δ𝒖 = 𝒖𝒖 – Δ𝒙 = 𝒙𝒙 ; 𝑹 is the first-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 effectiveness 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: Definition 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 definition: 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 first approach. output as States for the inner-loop control are defined as 𝒙 = in 𝑇 𝑦 = 𝑍 (31) »ℎ – ℎ – 𝑝– 𝑞– 𝑟– 𝑍– 𝑉 ¼ . There are two different 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 define 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 fixed 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 defined 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 effectiveness estimation: After defining 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 effectiveness 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 (𝐹 , 𝑀 , effectiveness on 𝑦 and 𝑦 respectively: 1 2 𝑎–𝑧 𝑎–𝑥 𝑀 and 𝑀 ). The following content in this section designs 𝑎–𝑦 𝑎–𝑧 the INDI control law (22) for this specific 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 defined 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 defined 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 effectiveness matrix in (22) can be described as with positive gains 𝑘 – 𝑘 – 𝑘 – 𝑘 to be tuned. 𝑧–𝑝 𝑧–𝑑 𝑎–𝑝 𝑎–𝑑 Eventually, the control effectiveness 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 filtered 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 effectiveness on 𝑦 when j sin¹𝘁j𝜒jºj = 1. shows, the selection of 𝜒 is of great importance for influencing On the contrary, the control effectiveness becomes zero when the internal dynamics, which will be elaborated in this section. sin¹𝘁j𝜒jº = 0. Small control effectiveness 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 effectiveness 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 flight 𝑝 𝑞 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 flight 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 fulfill: 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: 𝒚¥ = »𝑦¥ – 𝑦¥ ¼ Specifically, 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 filtered outputs. This, however, is prone to be detrimentally affected 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 filtered 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 differentiating filtered 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 flight, the local airspeed and angle of attack of each 𝑦¤ = cos 𝜒¹ℎ 𝑞¸ ℎ 𝑟¸𝘆 º¸ sin 𝜒¹ℎ 𝑝 ℎ 𝑟¸𝘆 º (42) 2 3 2 1 3 1 2 rotor can be different (see Fig. 6b). The difference of local 4) Pseudo-input definition: The last step is to define the airspeed leads to the variation of thrust coefficient 𝘅 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 flight 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 different 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 sufficient 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 verified from the flight data. But this 𝘂 𝑠 ℎ 𝘂 𝘂 cos¹j𝜒j 𝘁ºℎ 2 𝑛 1 3 3 assumption is invalid during aggressive thrust variation, such 𝘂¤ = (56) as vertical maneuvers. Nevertheless, the flight 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 satisfied, namely the first-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 first 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 defined as per (38). Note that the first 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 definite 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 satisfied. 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 specific 𝑨 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 diffeomorphism 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 fixed. 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 different j𝜒j in the simulation. A step-input of position the simulation are marked in the bottom plot with different command is given at 𝑡 = 1 s. symbols indicating if the flight succeeds. Unstable internal dynamic region obtained from real flight 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 effectiveness 𝐵 . As a result, 𝑢 𝑢 2 1 2 larger admissible region of j𝜒j. Though 𝑨 is stable in the area significantly 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 satisfied for single rotor, or without rotor failure (nominal condition). an acceptable control effectiveness on 𝑦 . In this section, we conduct a case study on a specific type A. Single Rotor Failure of quadrotor, a modified Parrot Bebop2, in the simulation 1) Internal Dynamics Analysis: For a quadrotor with a to demonstrate the effect 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 defined. Similar to the condition with quadrotor is listed in Table III. Without loss of generality, we failure of two rotors, one output is defined as the altitude: assume rotor 2 and 4 are removed (𝑠 =1, 𝑠 = 1, 𝜒 = j𝜒j). 𝑛 𝑙 Thus from (60), we have 𝑟 ¯ = 26•4 rads– 𝜔 ¯ = 1015 rads. 𝑦 = 𝑍 (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 different 𝑦 = ℎ 𝑛 (64) 3 2 j𝜒j. The shaded red represents the violation of the constraint 𝐵 𝐵 𝐵 𝐵 𝑇 To align the body fixed 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 flights 𝜌 = 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 flights. 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 coefficients 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 modified 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 flight 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 specific 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 effectiveness 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 first set of flights 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 first 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 off maneuver at the first 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 flight, 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 flight 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 confined 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 flight 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. Effect 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 different sets of j𝜒j to experimentally demonstrate its effect on the stability of the internal dynamics. Fig. 12 shows the internal state 𝘂 during a hovering flight where the parameter j𝜒j was initialized at Fig. 10: Variables during the position tracking flight 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 flight 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 different 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 flight 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 finished 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 difference 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 flight regime, flight 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. Specifically, 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 different drone was used, the Fig. 14: Reduced attitude, rotor speeds and lateral position time constant of the first-order actuator model is set as 30 ms 𝑌 of the quadrotor during the transition from setpoint D that differs 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 finish this A. Trajectory Tracking Task maneuver. A trajectory tracking task was performed under a wind flow of 5 m/s. The wind flow 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 first-order filter 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 flow. Fig. 14 compares states between the two approaches in this period. From the plotted data, we can find 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 flow 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 amplified (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 flight 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 flow. 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 flight speed of a quadrotor under the failure of two opposing rotors, maximum speed flight tests were conducted using both INDI and LQR. During the entire flight, 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 flight speed achieved by the two approaches with different set of gains. Similarly to the trajectory tracking task in the preceding section, INDI outperforms the LQR approach in terms of maximum flight 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 flight controlled by INDI, where the drone crashed at 𝑉 = 8•8 m/s when 𝑡 = 107 s. The variations wind of 𝘂 , 𝑦 and rotor speeds significantly 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 flow. 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 find 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 flight speed. C. High-Speed Flight with Imperfect State Estimations Since the ultimate goal of this work is improving drone safety during the high-speed flight 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 flight complementary filter and the ground truth in different flight controller. A complementary filter [35] was implemented by speeds demonstrating degradation of the compensatory filter 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 flight at 8.4 m/s inside the wind tunnel, indicating ro- bustness to significant attitude estimation errors. These errors can be seen in Fig. 17, which compares the pitch and roll angles from the onboard complementary filter with the ground truth obtained with the OptiTrack system at three different flight 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 filter, especially at 0m/s and 5m/s as Fig. 18 shows. Note that by 𝑒 , between flights with OptiTrack aided attitude estimation the increase of tracking error was less apparent at 8m/s where and those with onboard complementary filter, in different flight the drone was near the flight 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 significant aerodynamic effects, the control method is able filter 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 flight 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 significant aerodynamic effects. 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 effects 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 effect 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 flight 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 Sijbers, Bram single rotor. Strack van Schijndel, 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 flight 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. Sijbers, 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. Hoffmann, H. Huang, S. L. Waslander, and C. J. Tomlin, Systems, pp. 4782–4788, IEEE, sep 2014. “Precision flight 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 Identification 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, “Unified 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. Pflimlin, “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 Suffering 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 flying 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 Identification 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 identification, 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 flight 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 identifi- 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 identification, flight 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. http://www.deepdyve.com/assets/images/DeepDyve-Logo-lg.png Electrical Engineering and Systems Science arXiv (Cornell University)

Incremental Nonlinear Fault-Tolerant Control of a Quadrotor with Complete Loss of Two Opposing Rotors

Download PDF
15 pages

Loading next page...
 
/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

Abstract

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 flight 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 flight condition (> 8m/s). The incremental nonlinear dynamic inversion (INDI) approach which excels in handling model uncertainties is adopted to compensate for the significant unknown aerodynamic effects. The internal dynamics of such an underactuated system have been analyzed, and subsequently stabilized by re-defining 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, flight 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 significant 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 flight 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 flight video can be found safety in the face of unexpected structural and actuator failures. via https://youtu.be/-4rXX4D5HlA Among different types of multi-rotor drones, quadrotors excel in their structural simplicity. However, they suffer more from actuator damages due to a lack of actuator redundancy. proposed by [14] indicates that the hovering flight 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 effectiveness, which has been extensively studied specially designed configuration, 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 effects are considered such as the rotational damp- In-flight validations have been achieved by several pieces of ing [13], [14]. However, in out-door applications, significant 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 significant 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 flight 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 fixed 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 fixed-wing aircraft [18], is fixed 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 configuration 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 define 𝒙 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 suffers less from model uncertainties caused by significant any vector 𝒂 2 R . aerodynamic effects during high-speed flight. 𝐵 𝐵 The variables 𝑭 and 𝑴 denote the resultant force and To validate the proposed controller, flight 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 flight speed. This could significantly 𝐵 6 7 𝑭 = 0 ¸ 𝑭 (5) 6 7 increase the safety of quadrotors by expanding the flight 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 flight 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 coefficient valid in the hovering condition; a quadrotor with failure of two opposing rotors. Section V 𝜎 is a constant ratio between the thrust coefficient and drag elaborates on the selection of control outputs and the stability coefficient 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 flight test results to the body. 𝐼 denotes the moment of inertia of each rotor in low-speed and high-speed flight 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 significant model uncertainties 𝑭 and 𝑴 , 𝑎 𝑎 for instance, in high-speed flight conditions where significant aerodynamic effects 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: Definition 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 defined 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 coefficient [11], [13]. model terms with direct sensor measurements, or sensor In the high-speed flight condition, there are significant measurement derived quantities, thereby greatly improving aerodynamic effects 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 briefly reviewed. Readers may refer to [32] adopted by [14] in the quadrotor fault tolerant control problem. and [33] for further details. We hereby briefly introduce the concept. For a quadrotor with complete rotor failures, the full state Consider a nonlinear input-affine 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 fields. to a thrust vector pointing problem without considering the 𝑛 𝑛𝑚 𝑮 : R ! R is a function mapping with smooth vector yaw angle. fields as columns. The number of outputs is not larger than Define a unit vector 𝒏 fixed 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-efficient transformed to the normal form including internal states 𝜼 and [11] where 𝒏 aligns with the instantaneous thrust direction. external states 𝝃: Define 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 field 𝒉. 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 fields 𝒇¹𝒙º [33]. To align 𝒏 with 𝒏 , we can control ℎ and ℎ to track 𝜌 indicates the relative degree of the 𝑖th output 𝑦 . 𝑑 1 2 𝑖 𝑖 𝐵 𝐵 𝑛 and 𝑛 respectively. To be specific, with the selection of By defining 𝜌 ¯ = Σ 𝜌 as the sum of relative degrees 𝑥 𝑦 𝑖 𝐵 𝑇 𝒏 = »0– 0– 1¼ , ℎ and ℎ need to be stabilized to zero. of each output, one can define 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 first-order derivative of 𝜼 as defined per (10) does 2 z,ref inner-loop not include control input 𝒖. The nonlinear transformation 𝑻¹𝒙º ref is a diffeomorphism (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 effort of modeling. The control effective- 𝒚 = 𝜶¹𝒙º¸B¹𝒙º𝒖 (15) ness matrix B is relatively easier to be estimated offline or ¹𝜌 º ¹𝜌 º ¹𝜌 º 1 2 𝑙 ¹𝝆º 𝑇 where 𝒚 = »𝑦 – 𝑦 –•••– 𝑦 ¼ . If matrix B has full 1 2 𝑙 identified 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 filtered in practice. To synchronize the time delay the matrix; 𝝂 2 R is called the pseudo-input. With a full caused by these filters, 𝒖 also need to be filtered with the knowledge of 𝜶¹𝑥º and B¹𝒙º, control law (16) yields the same cut-off frequency [21]. We use subscript »¼ to denote closed loop dynamics the filtered 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 differentiable, 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 first-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 differentiable. Therefore, ref where Δ𝒖 = 𝒖𝒖 – Δ𝒙 = 𝒙𝒙 ; 𝑹 is the first-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 effectiveness 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: Definition 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 definition: 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 first approach. output as States for the inner-loop control are defined as 𝒙 = in 𝑇 𝑦 = 𝑍 (31) »ℎ – ℎ – 𝑝– 𝑞– 𝑟– 𝑍– 𝑉 ¼ . There are two different 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 define 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 fixed 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 defined 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 effectiveness estimation: After defining 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 effectiveness 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 (𝐹 , 𝑀 , effectiveness on 𝑦 and 𝑦 respectively: 1 2 𝑎–𝑧 𝑎–𝑥 𝑀 and 𝑀 ). The following content in this section designs 𝑎–𝑦 𝑎–𝑧 the INDI control law (22) for this specific 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 defined 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 defined 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 effectiveness matrix in (22) can be described as with positive gains 𝑘 – 𝑘 – 𝑘 – 𝑘 to be tuned. 𝑧–𝑝 𝑧–𝑑 𝑎–𝑝 𝑎–𝑑 Eventually, the control effectiveness 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 filtered 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 effectiveness on 𝑦 when j sin¹𝘁j𝜒jºj = 1. shows, the selection of 𝜒 is of great importance for influencing On the contrary, the control effectiveness becomes zero when the internal dynamics, which will be elaborated in this section. sin¹𝘁j𝜒jº = 0. Small control effectiveness 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 effectiveness 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 flight 𝑝 𝑞 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 flight 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 fulfill: 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: 𝒚¥ = »𝑦¥ – 𝑦¥ ¼ Specifically, 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 filtered outputs. This, however, is prone to be detrimentally affected 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 filtered 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 differentiating filtered 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 flight, the local airspeed and angle of attack of each 𝑦¤ = cos 𝜒¹ℎ 𝑞¸ ℎ 𝑟¸𝘆 º¸ sin 𝜒¹ℎ 𝑝 ℎ 𝑟¸𝘆 º (42) 2 3 2 1 3 1 2 rotor can be different (see Fig. 6b). The difference of local 4) Pseudo-input definition: The last step is to define the airspeed leads to the variation of thrust coefficient 𝘅 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 flight 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 different 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 sufficient 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 verified from the flight data. But this 𝘂 𝑠 ℎ 𝘂 𝘂 cos¹j𝜒j 𝘁ºℎ 2 𝑛 1 3 3 assumption is invalid during aggressive thrust variation, such 𝘂¤ = (56) as vertical maneuvers. Nevertheless, the flight 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 satisfied, namely the first-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 first 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 defined as per (38). Note that the first 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 definite 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 satisfied. 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 specific 𝑨 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 diffeomorphism 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 fixed. 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 different j𝜒j in the simulation. A step-input of position the simulation are marked in the bottom plot with different command is given at 𝑡 = 1 s. symbols indicating if the flight succeeds. Unstable internal dynamic region obtained from real flight 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 effectiveness 𝐵 . As a result, 𝑢 𝑢 2 1 2 larger admissible region of j𝜒j. Though 𝑨 is stable in the area significantly 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 satisfied for single rotor, or without rotor failure (nominal condition). an acceptable control effectiveness on 𝑦 . In this section, we conduct a case study on a specific type A. Single Rotor Failure of quadrotor, a modified Parrot Bebop2, in the simulation 1) Internal Dynamics Analysis: For a quadrotor with a to demonstrate the effect 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 defined. Similar to the condition with quadrotor is listed in Table III. Without loss of generality, we failure of two rotors, one output is defined as the altitude: assume rotor 2 and 4 are removed (𝑠 =1, 𝑠 = 1, 𝜒 = j𝜒j). 𝑛 𝑙 Thus from (60), we have 𝑟 ¯ = 26•4 rads– 𝜔 ¯ = 1015 rads. 𝑦 = 𝑍 (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 different 𝑦 = ℎ 𝑛 (64) 3 2 j𝜒j. The shaded red represents the violation of the constraint 𝐵 𝐵 𝐵 𝐵 𝑇 To align the body fixed 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 flights 𝜌 = 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 flights. 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 coefficients 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 modified 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 flight 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 specific 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 effectiveness 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 first set of flights 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 first 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 off maneuver at the first 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 flight, 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 flight 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 confined 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 flight 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. Effect 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 different sets of j𝜒j to experimentally demonstrate its effect on the stability of the internal dynamics. Fig. 12 shows the internal state 𝘂 during a hovering flight where the parameter j𝜒j was initialized at Fig. 10: Variables during the position tracking flight 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 flight 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 different 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 flight 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 finished 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 difference 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 flight regime, flight 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. Specifically, 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 different drone was used, the Fig. 14: Reduced attitude, rotor speeds and lateral position time constant of the first-order actuator model is set as 30 ms 𝑌 of the quadrotor during the transition from setpoint D that differs 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 finish this A. Trajectory Tracking Task maneuver. A trajectory tracking task was performed under a wind flow of 5 m/s. The wind flow 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 first-order filter 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 flow. Fig. 14 compares states between the two approaches in this period. From the plotted data, we can find 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 flow 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 amplified (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 flight 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 flow. 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 flight speed of a quadrotor under the failure of two opposing rotors, maximum speed flight tests were conducted using both INDI and LQR. During the entire flight, 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 flight speed achieved by the two approaches with different set of gains. Similarly to the trajectory tracking task in the preceding section, INDI outperforms the LQR approach in terms of maximum flight 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 flight controlled by INDI, where the drone crashed at 𝑉 = 8•8 m/s when 𝑡 = 107 s. The variations wind of 𝘂 , 𝑦 and rotor speeds significantly 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 flow. 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 find 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 flight speed. C. High-Speed Flight with Imperfect State Estimations Since the ultimate goal of this work is improving drone safety during the high-speed flight 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 flight complementary filter and the ground truth in different flight controller. A complementary filter [35] was implemented by speeds demonstrating degradation of the compensatory filter 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 flight at 8.4 m/s inside the wind tunnel, indicating ro- bustness to significant attitude estimation errors. These errors can be seen in Fig. 17, which compares the pitch and roll angles from the onboard complementary filter with the ground truth obtained with the OptiTrack system at three different flight 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 filter, especially at 0m/s and 5m/s as Fig. 18 shows. Note that by 𝑒 , between flights with OptiTrack aided attitude estimation the increase of tracking error was less apparent at 8m/s where and those with onboard complementary filter, in different flight the drone was near the flight 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 significant aerodynamic effects, the control method is able filter 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 flight 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 significant aerodynamic effects. 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 effects 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 effect 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 flight 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 Sijbers, Bram single rotor. Strack van Schijndel, 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 flight 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. Sijbers, 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. Hoffmann, H. Huang, S. L. Waslander, and C. J. Tomlin, Systems, pp. 4782–4788, IEEE, sep 2014. “Precision flight 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 Identification 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, “Unified 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. Pflimlin, “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 Suffering 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 flying 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 Identification 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 identification, 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 flight 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 identifi- 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 identification, flight 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.

Journal

Electrical Engineering and Systems Science – arXiv (Cornell University)

Published: Feb 18, 2020

There are no references for this article.