Quadcopter attitude control with vibration reduction by additive-state-decomposition dynamic inversion design with a notch filter

In practice, the unbalanced mass of the propeller is the leading cause of vibration in a quadcopter. Therefore, this paper proposes an additive-state-decomposition notch dynamic inversion controller to suppress the vibration noise. Firstly, the vibration mechanics model based on unbalanced mass is established and its characteristic frequency is analyzed. Then, the specific form of the notch filter is designed, and this characteristic frequency is taken as its internal parameter. Next, stability analysis shows that the proposed controller guarantees that all attitude signals are globally uniformly ultimately bounded. In particular, the notch filter can effectively reduce the vibration having a specific frequency. Finally, the proposed controller is performed on a real quadcopter to verify its vibration reduction performance.

geographic mapping and passenger carrying [1]. However, the multicopter will inevitably generate vibration in practice [2,3]. The airframe is an assembly formed by bolting various parts together. Due to bolt looseness, the stiffness of this airframe will become lower, which causes low-frequency vibration. Air disturbances, such as atmospheric turbulence, can generate nonuniform flow fields. The multicopter is affected by the disturbed forces and moments when the multicopter is involved in this nonuniform flow field. The unreasonable arrangement of sensors, flight control boards, GPS and other modules on the airframe will easily cause the center of gravity to offset. This offset will bring additional disturbance force, which will cause the airframe to vibrate. The output signals of these sensors in turn contain vibration noise [4]. These noise-containing signals are passed to the propulsion unit (composed of the propeller and motor) via designed control signals, which further amplify the amplitude of vibration. Moreover, due to manufacturing accuracy or installation errors, the mass center of the propeller may not be collinear with the shaft [5]. Therefore, the quadcopter produces strong disturbance torque due to the unbalanced propeller. Since the propeller always rotates in a circle around a fixed axis, the centrifugal force generated will also change periodically. Its period is proportional to that of the propeller. The disturbed torque by the centrifugal force will further affect the attitude dynamics and finally cause vibration [6]. Therefore, suppressing the disturbance torque caused by the unbalanced mass is one of main tasks in this paper.
Many research results have effectively solved various vibration reduction problems of multicopters during flight. Among them, input shaping, as a classic and widely used vibration reduction technique, has been applied to a variety of practical plants, such as rolling bearing systems [7], flexible spacecraft [8] and active suspension system [9]. Based on the input shaping technique, a differential flatness scheme is proposed to solve the vibration problem of microelectromechanical systems, and the related simulation results show that this scheme can save energy significantly [10]. The differential flatness scheme needs to constrain the reference trajectory and thus has a limited scope of applications. In addition, to reduce vibration noise, additional materials such as foam, gel pad and O-ring are often installed between the autopilot hardware and the frame [11]. These research results mainly take filtering, trajectory planning, isolation and other measures to suppress vibration noise. So far, there is less literature studying vibration suppression schemes by directly control the unbalanced propellers. In addition, the vibration problem can also be considered in the controller design process. For example, the Ref. [12] regards the vibration of sensors contained in flexible spacecrafts as total disturbance, and a active suspension controller was designed. Reference [13] proposed a controller to solve the vibration problem of a class of flexible manipulators. This paper considers reducing the quadcopter's vibration caused by the unbalanced mass of propellers, during the controller design. Our research achievements distinguish from the previous paper presented at CAC2013 [14]. In work presented in [14], an improved additive-state-decomposition-based (ASDB) dynamic inversion stabilizing controller was proposed. The concept of additive state decomposition is first proposed in the literature [15]. The additive state decomposition is a generalization of linear superposition principle. The additive state decomposition improves the control freedom degree through decomposition, and introduces the relevant control technology of linear systems to nonlinear systems, thus improving the robustness of the system. At present, the additive decomposition state framework is applied in many fields, such as iterative learning control [16], autonomous aerial refueling control [17] and vehicle attitude control [18]. Compared with [14], this paper first establishes an unbal-anced masses' mechanical model of the existing quadcopter model, which is allowed to study the vibration of the airframe more easily. Next, a stability proof of the improved ASDB stabilizing controller is given. The proof results show that all state variables of the improved ASDB stabilizing controller (proposed controller) are globally uniformly ultimately bounded. In particular, the notch filter can effectively reduce the vibration with a specific frequency. Finally, a softwarein-the-loop (SIL) simulation and a flight experiment show the effectiveness of the proposed controller. Compared with existing research results, the proposed control scheme mainly has the following contributions.
1. Establish a mechanical model of the unbalanced propeller and analyze its effect on attitude stability. 2. The structure of the proposed controller is simple.
It is mainly composed of a (proportional integral) PI term and a notch filter [19] term. The notch filter can suppress disturbances with a specific frequency. 3. The stability analysis is made. The improved ASDB stabilizing controller guarantees that all states are globally uniformly ultimately bounded for a nonlinear system. Furthermore, the proposed controller guarantees that all states converge to the origin asymptotically for a degraded linear system.
The remainder of this paper is organized as follows. The modeling and problem formulation are presented in Sect. 2. Section 3 describes how to design the proposed controller, and provides its stability and performance analysis. Then, SIL simulation and flight experiment results are presented to show the advantage of our proposed approach in Sect. 4. Finally, the conclusions of the full paper are drawn in Sect. 5.

Modeling and problem formulation
In this section, a quadcopter model, including an unbalanced mass model, is first established. Next, it is transformed into an abstract model for controller design.

Quadcopter model
The following dynamical model of a class of quadcopters, in the northeast-earth coordinate system [1, p. 104], is mainly considered: where p z , v z are the altitude and the velocity in the z-direction; g and m are the gravity acceleration and the total mass, respectively; Θ = φ θ ψ T ∈ 3 denotes the angular vector; φ, θ and ψ denote roll angle, pitch angle and yaw angle, respectively; ω = ω φ ω θ ω ψ T ∈ 3 denotes the angular velocity vec- , and u f are the true torque vector, and the true total thrust, respectively; W is defined as follows: (2) G a ∈ 3 denotes the gyroscopic torque, defined as follows: where e 3 = 0 0 1 T ; J R P denotes the total inertia moment of the motor rotor and the propeller. The dynamicsṗ z ,v z can be found in [20, p. 183], and the dynamicsΘ,ω can be found in [20, p. 179]; therefore, it will not be described in detail.

Modeling of unbalanced propeller
Due to manufacturing accuracy or installation errors, the mass center of the propeller may not be collinear with the shaft. Therefore, the quadcopter produces strong vibration due to the propeller's unbalanced mass. As shown in Fig. 1, the mass of the left propeller is greater than that of the right propeller. Then, there is an additional mass m u i on the left side of the propeller, and assume that its distance from the shaft is r u i . This unbalanced mass will generate centripetal force [5]. This centripetal force will produce a kind of highfrequency vibration which will further affect the measurement accuracy of the microelectromechanical system (MEMS) gyroscope.   Figure 2 shows the structure of an X-type quadcopter. The centripetal force caused by the ith unbalanced propeller is where i is the real angular velocity of the propeller i. Assume that the total centripetal force (vector sum of f c,1 , f c,2 , f c, 3 and f c,4 ) generates a vibration-related control efficiency matrix: ; ϕ i denotes the initial phase; d represents the airframe radius (see Fig. 2). After the above analysis, the real total thrust and torque are defined as follows: where H is the control effectiveness matrix defined as follows: where c T , c M are the thrust coefficient, and the torque coefficient, respectively. Figure 3 shows the structure of the closed-loop system. The true torque τ and thrust u f can generate the real angular velocity ( 1 , 2 , 3 , 4 ) after inverse control efficiency matrix. With the real angular velocity as the input, the gyroscope torque is obtained. Furthermore, with the help of the vibration-related control efficiency matrix (5), the disturbance torque due to vibration is obtained. Finally, the disturbance torque generated by the vibration will appear in the real control torque τ . Since the altitude channel uses the classic (proportional integral derivative) PID technology to design the control signal u f , its design process is omitted. Next, how to apply the proposed control algorithm to the attitude control of the real quadcopter is mainly described.

Problem formulation
The real quadcopter dynamics model (1) is not convenient for controller design, so it should be simplified necessarily. The roll channel is taken as an example (the modeling procedure for the pitch and the yaw channel are similar), and its linear dynamic model around a hover phase is studied: where σ φ (t, ω 0 ) denotes the disturbed torque caused by unbalanced propellers, and ω 0 represents the dominant frequency component of the disturbed torque. For clarity of description, let Then, the roll channel dynamic (8) is further rewritten aṡ Similarly, the pitch and yaw channel dynamics are described as follows: Because of uncertain weight of the battery and the consumption of oil, the true inertia moment matrix J has a large uncertainty. Therefore, assume that the nominal inertia moment matrix to be The virtual control signal u = u φ u θ u ψ T will be designed later. Based on (10) and (11), the dynamic model of the quadcopter is obtained as follows: where and the initial value Notice that quadcopters always work in hover conditions. Therefore, it is significant to consider the vibration of the quadrotor in the hover phase. In the hover mode, u f is approximately equal to the gravity of the quadcopter. In this case, ω 0 = mg/(4c T ) .
Let the gain matrix K be designed to make A to be a Hurwitz matrix with real negative eigenvalues. Then, there exists the following equality: where 0 < P, M ∈ 6×6 are symmetric positive definite matrices.
For (12), the objective of this paper mainly designs a virtual control signal u to make x converge to zero or a bounded ball around origin meanwhile attenuating the attitude vibration of the quadcopter.

Assumption verification
In order to facilitate the control design, the controlled plant is assumed to satisfy two assumptions.
Assumption 1 Assume that J 0 = J (I + Δ J ) with the uncertain matrix Δ J satisfying the following inequality:

Assumption 2
The unknown disturbance σ (t, ω 0 ) satisfies the following inequalities: where k σ =l J Δ K and l σ t = l J t K are positive constants. It is inferred from (13) that h (t, u) = J −1 J 0 u, and ∂h (t, u)/∂u = J −1 J 0 . Therefore, h (t, u) satisfies the following inequalities: For simplicity of description, h (t, u) is simplified as h; σ (t, ω 0 , x) is simplified as σ .

A transformed minimum-phase system
Since A is a Hurwitz matrix, Theorem 1 of Ref. [21] implies that there exists an output matrix C ∈ 6×3 as Let the output vector be defined as Based on (19), and (20), the first derivative of y along the solution to (12) is where Notice that the output y only contains part of elements (the output vector y has 3 elements) in all states (the state vector x has 6 elements x 1 , x 2 , . . . , x 6 ). Therefore, the remaining 3 state equations are expressed as [22, pp. 351] where η ∈ 3 is an internal state vector, A η ∈ 3×3 and B η ∈ 3×3 are constant matrices. According to (21) and (23), (12) is expressed in the form of a block matrix [21]: BecauseĀ is an upper triangular matrix, and the eigenvalues of A η are all from A. Therefore, A η is a Hurwitz matrix, namely, the internal dynamics (23) is stable. If y → 0, then η → 0 and then x → 0 according to (23). This implies that (24) with y as output is a minimum-phase system. Therefore, the next step is to design the control signal u making the output y gradually converge to zero. This is only a motivation to design. A formal proof will be give in Theorem 1.

Controller design
The dynamic equation (21) is viewed as the original system [15], and then the primary system is designed as where y p ∈ 3 denotes the state of the primary system. Based on the additive-state-decomposition framework [23], the secondary system is equal to the original system minus the primary system: The state of secondary system y s ∈ 3 is defined as: where y s (0) = C T x 0 . Then, the dynamic of secondary system (26) is further rewritten aṡ Let d l = y s . Then, the original system (21) is simplified as After the Laplace transform of (25), the transfer function between the primary system y p and the control input u is described as where s represents the Laplace variable. We have Then, the additive-state-decomposition dynamic inversion controller is generally designed as follows: where Q (s) is a filter specified later. Substituting (32) into (31) results in and It can be inferred that the filter Q (s) has an important influence on the convergence of output y (s), so it is necessary to study it carefully. Substituting (34) into (32) results in When there only exists a slow time-varying disturbance, the Ref. [21] chooses a low pass filter 1 εs+1 I 3 with a time constant ε; therefore, then an additivestate-decomposition dynamic inversion controller is designed as follows: However, there are not only low-frequency disturbances but also vibration components related to the angular velocity of propellers in the quadcopter. Therefore, Q (s) is designed as follows: where Q ω = (ω 0 /q)s s 2 +(ω 0 /q)s+ω 2 0 is a notch filter, and q represents the quality factor; ω 0 is the dominant frequency. Substituting the new filter (37) into (35) yields the proposed controller: where Whether to introduce the term u ω (s) is determined by the characteristics of external disturbance (vibration noise). If no particular frequency vibration is detected, set ω 0 = 0, that is, u ω does not work. Only when the sensor detects the presence of a specific frequency component ω 0 (such as vibration) in the closed-loop system. Then, u ω is incorporated into u, and the performance of suppressing disturbance is expected to be significantly enhanced.

Stability and performance analysis
Recalling (37), if s = 0, I 3 − Q (s) s=0 equal to s= jω 0 equal to Therefore, it is expected that the newly designed filter (37) can simultaneously suppress low-frequency disturbances and a frequency component around ω 0 according to (33). The bigger the value of q is, the more pronounced the filter effect has. Before proceeding further, an assumption on the lumped disturbance d l is imposed.

Assumption 3
There exists the Laplace transform of d l .
According to Assumption 3, the Laplace transform of the lumped disturbance d l is given as follows: Then, substituting (40) into (32) results in Since − C T B −1 C T x 0 is a constant value signal, its inverse Laplace transform is an impulse signal, and will vanish. Therefore, for simplicity, (41) is considered as The poles of Q ω is taken to study its frequency characteristics. Let the denominator of Q ω = 0 equal zero, that is to say, s 2 + (ω 0 /q) s + ω 2 0 = 0. Then, the following characteristic roots are obtained as In the real experiment, it is found that the average angular frequency of each propeller is about 99.06 Hz. If these design parameters q = 0.5, ω 0 = 100 Hz are chosen, then a higher decay rate is obtained (s 1 = s 2 = −100). Namely, the term v (s)−u (s) will rapidly decay to 0. For the sake of analysis, Assumption 4 is introduced.
Based on the Assumption 4, the inversion Laplace transform of (41) is rewritten as In the following, Theorem 1 and Corollary 1 are proposed to show that u PI +u ω not only can attenuate noise at a specific frequency but also guarantee the state y can converge to the neighborhood near the origin.
Proof See Appendix 1.

Controller discretization
The proposed controller (38) is further rewritten as where x (s) is viewed as the input signal; G B (s) denotes the transfer function from x (s) to u (s). In order to make the proposed controller realizable by a digital computer, the designed G B (s) should be discretized. The classical Tustin discretization method is used as where D z (z) denotes the digital transfer function; z denotes the discretization transform symbol; and T is the sampling period. However, the classical Tustin method is prone to frequency aliasing and cannot filter out signals of specified frequencies. To obtain a good vibration suppression effect, the proposed controller needs to accurately filter out the disturbance signal at a specified frequency. Therefore, the following Tustin transformation with the prewarping discretization method [24, p. 243] is selected: where ω 0 is the dominant frequency also the notch frequency. Fig. 4 Simulation procedure [25] 4

Experiment verification
In this section, a practical experiment is provided to illustrate that the proposed control algorithm has a stronger vibration suppression capability compared to the traditional additive-state-decomposition dynamic inversion controller. The detailed experimental process can be found in the video at http://rfly.buaa.edu.cn/ (Click the 'Videos Demonstration' button).

Introduction to experiment platform
This experiment adopts a professional experiment platform provided by [25]. As shown in Fig. 4, the whole experimental platform is divided into two parts: an SIL simulation and a flight experiment. In the controller design phase, the Simulink software is used to construct the proposed controller and the complete quadcopter model. Then, the SIL simulation block is performed such that the control parameters are preliminarily tuned. Next, the controller in Simulink is converted into C++ language. Finally, a flight experiment is performed to verify the vibration suppression capability of the proposed control algorithm. As shown in Fig. 5, this experiment takes an X-type quadcopter as the controlled plant, and its parameters are shown in Table 1.

Controller design
According to (38), the controller is designed as where C φ ∈ 2 , C θ ∈ 2 , C ψ ∈ 2 , ε φ , ε θ , ε ψ , Λ φ , Λ θ , Λ ψ are design parameters. The roll channel is taken as an example to introduce the design process of the proposed controller, while the design process of the pitch and yaw channels is the same.
Step 2: Choose eigenvalues of A T φ corresponding to its eigenvalue −1.
Step 3: The proposed controller is designed as (38).
For pitch and yaw channel, the proposed controller chooses the same control parameters. For a fair comparison, the PI controller with similar parameters is designed according to [21].
(2) Existing controller [21] (PI controller): The design steps for this existing controller are the same as for the proposed controller. Only the controller as described in (36) is used, so it has only one control parameter ε φ = 0.01.

SIL simulation
The sampling period of the SIL simulation is set to 0.004 s. Driven by the existing controller, the simu-   Figures 6 and 7 show attitude angle curves and attitude angle rate curves, respectively. Although attitude angle rate curves converge to zero, these curves are not very smooth. Because there exists high-frequency noise related to the angular frequency of the propeller. Therefore, it is necessary to introduce a notch filter in the existing PI controller to eliminate the highfrequency noise, which is one of the innovations of this paper. Figure 8 shows that the mean angular velocity of all propellers is approximately 621.8 rad/s. For the PI controller, the black dashed line in Fig. 9 shows the vibration spectrum ofω φ . It is observed that the peak of the vibration spectrum is concentrated around the frequency of 99.06 Hz (621.8 rad/s), and the amplitude reaches 17.89. Since the peak frequency is very close to the frequency propellers rotating, it implies that the vibration is mainly caused by the unbalanced mass of the propeller, which seriously affects the stability of the attitude.
In order to suppress the vibration signal of this specific frequency, additional control signal u ω (s) is then introduced, namely the proposed controller. The term u ω (s) has the following control parameters: ω 0 , q. The parameter ω 0 is mainly determined by the angular velocity corresponding to the peak frequency. Therefore, parameter ω 0 is selected as ω 0 = 99.06 Hz.

Analysis of key parameter selection
The influence of different q on the filter effect is studied, and the simulation results are shown in Fig. 10. It can be inferred from Fig. 10 that the smaller the value of the quality factor q is, the more obvious the suppression ability has, namely, the roll angle rate curve is smoother. However, Fig. 10 implies that the roll angle rate curve cannot converge to origin when q = 1.25. Therefore, too small parameter q will affect the stability margin of the closed-loop system. In order to balance the robustness and suppression ability of the closed-loop system, the quality factor q, in this paper, is chosen as 2.5.
Remark 1 It can be inferred from Fig. 11 that the farther the poles are from the origin, the faster the yaw angle converges. When poles (−6, −10) are selected, the yaw angle can converge rapidly at the initial stage, but it starts to move away from the origin at the steady stage, which indicates that the stability of the closedloop system has been seriously affected. Therefore, considering the response speed and system stability, these pole groups (−1.4, −5) are finally selected.

Discretization analysis
As shown by the solid red line in Fig. 9, the vibration spectrum forω φ is more uniformly distributed over the frequency range, driven by the proposed controller.
For the proposed controller with the Tustin discretization method, the peak appears at frequency 99. 16 Hz, which is attenuated by 1 − 4.774/17.89 = 73.31%. Notice that the designed controller for the quadcopter is discretized by prewarping Tustin transformation. This discretization method can accurately suppress the peak amplitude in a specific frequency range. The peak magnitude is proportional to the magnitude of the disturbance. For example, when 99.16Hz is used as the parameter of the prewarping Tustin transformation algorithm, the spectral peak of the roll channel is significantly reduced compared with the Tustin discretization method. The detailed simulation results are shown in Fig. 12, and the peak is attenuated by 1 − 2.118/4.774 = 55.63%. Therefore, the prewarping Tustin transformation will be used to discrete the

Flight experiment
The experimental environment is set as follows: Operating System (OS): Windows 10, 64-bit system; Processor: Intel(R) Core(TM) i7-9750H CPU @ 2.60 GHz; Memory: 16G; Software: MATLAB 2017b; Autopilot: Pixhawk 2; Remote controller: RadioLink AT9S; Sampling period: 0.004 s. A scissor is used to cut a tenth of the left propeller to get a unbalanced propeller. The whole experimental procedure is divided into the following steps: Step 1. Use the PI controller to control the attitude of the quadcopter and let it at a hovering phase in the air for tens of seconds, recording the average angular frequency of each propeller.
Step 2. Take the mean angular frequency of four propellers as the designed parameter ω 0 of the notch filter.
Step 3. Use the proposed controller with a notch filter to control the attitude of the quadcopter. Let the quadcopter take off in the self-stabilizing mode and record the angle acceleration information.
After Step 1, the mean angular frequency of the propeller was tested to be about 100 Hz. Therefore, the parameter ω 0 is designed as 100. Next, collect the data of the quadcopter in the hover mode and perform spectrum analysis to obtain the results shown in Fig. 13. As shown by the solid red line in Fig. 13b, the vibration spectrum forω φ is more uniformly distributed over the frequency range, driven by the proposed controller. For the PI controller [21], the peak is concentrated around the frequency of 99.4 Hz, and the peak amplitude reaches 8.598, shown in Fig. 13a. Compared with the PI controller, the peak of the proposed controller with a notch filter appears at the frequency 103.03 Hz, which is attenuated by 1 − 4.942/8.598 = 42.52%. This further shows the effectiveness of the proposed method.

Conclusions
The attitude stabilizing control for a class of quadcopters is considered in this paper. This research has following contributions. (1) The proposed controller structure is simple, mainly composed of a PI term and a notch filter term. The notch filter can suppress disturbances with a specific frequency. (2) The stability proof of the proposed controller is given. From the SIL simulation and the flight experiment, it is observed that the proposed controller has better vibration suppression performance. The SIL simulation shows that the spectrum peak is reduced by 88.16% compared with the PI controller. The flight experiment further shows the effectiveness of the proposed method over the existing PI controller. The limitations of the current control algorithm are mainly reflected in the following two aspects: (1) there is no theoretical analysis of how to ensure the stability of the controller when the poles are conjugate complex roots. For the sake of conservatism, negative real root is preferred as the pole. (2) If the characteristic frequency of vibration noise cannot be accurately identified, the proposed controller will be difficult to play a role in vibration suppression. How to solve the above two limitations will be the future research direction.

Proof of Corollary 1
Let us consider that the controlled system (12) is simplified into a class of linear system (47) with specific frequency disturbance σ (t, ω 0 ). Assume that the disturbance signal σ (t, ω 0 ) is a cosine signal vector with frequency ω 0 . Let σ (t, ω 0 ) = 1 3 cos (ω 0 t), where 1 3 ∈ 3 denotes a column vector whose elements are all ones, without loss of generality. According to the frequency retention characteristics of the linear system, its Laplace transform is expressed as σ (s, ω 0 ) = s s 2 + ω 2 0 1 3 .
Each steady-state variable in the dynamic equation (47) must contain a cosine signal with a frequency of ω 0 .