Anti-Disturbance Control for Quadrotor UAV Manipulator Attitude System Based on Fuzzy Adaptive Saturation Super-Twisting Sliding Mode Observer

: Aerial operation with unmanned aerial vehicle (UAV) manipulator is a promising ﬁeld for future applications. However, the quadrotor UAV manipulator usually suffers from several disturbances, such as external wind and model uncertainties, when conducting aerial tasks, which will seriously inﬂuence the stability of the whole system. In this paper, we address the problem of high-precision attitude control for quadrotor manipulator which is equipped with a 2-degree-of-freedom (DOF) robotic arm under disturbances. We propose a new sliding-mode extended state observer (SMESO) to estimate the lumped disturbance and build a backstepping attitude controller to attenuate its inﬂuence. First, we use the saturation function to replace discontinuous sign function of traditional SMESO to alleviate the estimation chattering problem. Second, by innovatively introducing super-twisting algorithm and fuzzy logic rules used for adaptively updating the observer switching gains, the fuzzy adaptive saturation super-twisting extended state observer (FASTESO) is constructed. Finally, in order to further reduce the impact of sensor noise, we invite a tracking differentiator (TD) incorporated into FASTESO. The proposed control approach is validated with effectiveness in several simulations and experiments in which we try to ﬂy UAV under varied external disturbances.


Introduction
Unmanned aerial vehicles (UAVs) have become a popular and active research topic among scholars worldwide [1,2]. It could work at locations where entry is difficult and humans cannot access [3]. Recently, they are not only used in traditional scenes, such as monitoring, aerial photography, surveying, and patrol, but are also employed in new application scenarios requiring physically interacting with external environment. Then, to conduct physical interaction, aerial robots are either equipped with a rigid tool [4] or an n-degree-of-freedom (DoF) robotic arm [5][6][7][8], which are called UAV manipulators. As for the UAV equipped with a robotic arm, several disturbances cannot be avoided, such as wind gust and model uncertainties, during task execution. Therefore, to meet the aerial operation task requirements with high reliability, disturbance rejection control problem should be investigated.
Many controllers have been proposed for disturbance rejection. The Robust Model Predictive Control (MPC) method is used for multirotor UAV to provide robust performance under an unknown but bounded disturbance [9]. Meanwhile, the adaptive control structure is built to obtain coefficients of system with uncertainty online. A controller based on Model Reference Adaptive Control is proposed in [10], which performs better on disturbance rejection compared with non-adaptive controllers.
However, as what we know, one of the biggest drawbacks of adaptive controllers is that they perform little robust under the bursting phenomenon.
Adopting the disturbance and uncertainty estimation and attenuation (DUEA) strategy, such as Disturbance observer (DOB) [11], unknown input observer (UIO) [12], and extended state observer (ESO) [13], would be a potential solution for disturbance rejection. Meanwhile, there are several types of DOBs applied to UAVs. Work [14] proposes an inner-loop control structure to recover the dynamics of a multirotor combined with additional objects similar to the bare multirotor using DOB-based method. A linear dual DOB is built in [15] to reject modeling error and external disturbance when designing the control system. The authors of [16] propose a DOB-based tracking flight control method for a quadrotor to reject disturbance, which is supposed to be composed of some harmonic elements, and applies it to flight control of the UAV Quanser Qball 2. A disturbance observer with finite time convergence (FTDO), which could conduct online estimation of the unknown uncertainties and disturbances, is incorporated into an hierarchical controller to solve the problem of path tracking of a small coaxial rotor-type UAV [17]. The authors of [18] introduce a robust DOB for an aircraft to compensate the uncertain rotational dynamics into a nominal plant and proposes a nonlinear feedback controller implemented for desired tracking performance.
As for ESO, it was first proposed by Han [19] and has been introduced in many control methods [20]. Traditionally, ESO methods mainly focus on coping with disturbances which slowly change [21]. Nevertheless, it is evident that the disturbance caused by the wind gust sometimes performs drastic change so that it can not be estimated by traditional ESO thoroughly. Thus, an enhanced ESO that could quickly estimate the disturbance is necessary in this field. A high order sliding mode observer is built to estimate unmodeled dynamics and external disturbances for an aerial vehicle to track the trajectory [22]. Moreover, a sliding mode observer is proposed for equivalent-input-disturbance approach to control the under-actuated subsystem of a quadrotor UAV [23]. However, chattering phenomenon is normal in traditional sliding mode observer (SMO) because of the discontinuous sign function. In order to reduce the chattering problem, the super-twisting algorithm is introduced in a ESO to mitigate estimation chattering [24], but there is still a little chattering. In order to solve it, the authors of [25] proposed a new SMO control strategy to reduce the estimated speed chattering of the motor, in which the switching function is replaced by a sigmoid function. The sigmoid function has also been used in SMO for UAV in [26]; however, it is conducted without experiment.
The main contributions of this study are given as follows.
(1) In order to alleviate the estimation chattering problem of traditional SMESO, a new observer named fuzzy adaptive saturation super-twisting extended state observer (FASTESO) is proposed, in which a saturation function is invited to replace the discontinued sign function, meanwhile a super-twisting algorithm is introduced to prevent excessively high observer gain and TD [19] and is incorporated for avoiding directly using acceleration information, which is full of noise.
(2) To stay robustness under disturbances with unknown bounds, fuzzy logic rules are introduced as an adaptive algorithm to adaptively adjust the observer switching gains. Furthermore, it also contributes to the chattering attenuation under high switching gain with low estimation value and observer performance improvement under low switching gain with high estimation value.
(3) The proposed method is verified with effectiveness on a quadrotor UAV manipulator prototype in several simulations and experiments.
The rest of this article is organized as follows. Section 2 introduces some preliminaries of this work. Section 3 describes the kinematic and dynamic models of the quadrotor UAV manipulator.
The construction of proposed FASTESO and attitude controller are given in Section 4. Additionally, several simulations are conducted in Section 5. Moreover, Section 6 shows the experiment. Finally, Section 7 presents the conclusion.

Preliminaries
In this section, some mathematical preliminaries are provided for understanding the whole paper more easily.

Notation
The 2-norm of a vector or a matrix is provided by · . λ max (Z) and λ min (Z) represent the maximal and minimum eigenvalue of the matrix Z, respectively. Moreover, the operator S(·) denotes a vector to a skew symmetric matrix as The sign function is given as

Quaternion Operations
The unit quaternion q = q 0 q v T ∈ R 4 , q = 1, is used to represent the rotation of the quadrotor in this paper. Several corresponding operations are given as follows.
The quaternion multiplication: The relationship between rotation matrix C B A and unit quaternion q is represented by: Take the time derivative of Equation (4), we could obtaiṅ The derivative of a quaternion and the quaternion error q e are provided, respectively, aṡ where q d represents the desired quaternion whose conjugate is q * d = q d0 −q dv T . Moreover, ω denotes the angular rate of the system.

Models of UAV
In this section, we present a mathematical description of the quadrotor UAV manipulator system model. The abstract graph is shown in Figure 1, in which the robotic arm is fixed at the geometric center of the UAV.   (10) where R t ∈ R 3×3 represents the transformation matrix for converting Euler angle ratesΨ into ω I . R b ∈ R 3×3 denotes the rotation matrix representing the orientation of O b relative to O I , and

Kinematic Model
c(·) and s(·) denote cos(·) and sin(·), respectively. Moreover, the position and angular rate of the frame O i , which is fixed to the robotic arm, with respect to O I are provided by , represent the position of O i and the angular rate of the i-th robotic arm frame with respect to O b , respectively. Additionally, more relationships are provided, where J pi ∈ R 2×2 and J ri ∈ R 2×2 are Jacobian matrices representing the translational and angular velocities of each robotic arm link to theη, respectively. According to Equations (12) and (13), we could get the translational and angular velocity of O i with respect to O I as where S(·) represents the skew-symmetric matrix.

Dynamic Model
In order to obtain the dynamic model of the quadrotor UAV manipulator system, the Euler-Lagrange equation is introduced.
where k = (1, ..., 8). L: the Lagrangian with kinetic energy K and potential energy U of the integrated system. u k : the generalized driving force. d k : external disturbance applied to the system. Additionally, K could be given in detail as where m b is the quadrotor base mass, m i is the i-th robotic arm mass (i = (1, 2)), I is the inertia matrix, and R b i the rotation matrix between the frame fixed to the center of mass of the i-th link and O b . Then, the total potential is provided as where e 3 denotes unit vector [0, 0, 1] T and g represents the gravity constant. By substituting Equations (16) and (17) into Equation (15), the dynamic model of the quadrotor UAV manipulator system could be obtained as Moreover, the total kinetic energy can be expressed: where Details of the inertia matrix M(H) and Coriolis matrix C(H,Ḣ) can be found in [27]. The G(H) could be obtained via partial derivative Equation (21): Express Equation (18) in detail as where u t , u r , u l represent the generalized control inputs corresponding to p I , Ψ, η. u l is not used, as we do not consider a dynamic robotic arm in this work. Vector d = [d t , d l , d r ] T are the lumped external disturbance. As for the quadrotor attitude loop subsystem, we would transform u r to the quadrotor which is a positive correlation vector with forces produced from the quadrotor propellers. Moreover, ω i represents rotor rate of quadrotor propeller (i = 1, 2, 3, 4). Then, convert the generalized control inputs to the propeller rate: where Λ T and Λ C are the thrust and drag coefficients, respectively. Additionally, l denotes the distance from each motor to the quadrotor CoG. According to Equation (22), we could obtain in detail the dynamics of the attitude loop subsystem which is the base of next section:

Method
Based on the UAV model, the proposed control strategy is described in detail in this section. As shown in Figure 2, quadrotor attitude control problem could be briefly divided into two components: FASTESO, used for estimating and compensating the disturbance, plays a role of the feedforward loop, and Backstepping controller, built for regulating the orientation to track the desired attitude timely, plays a role of the feedback loop. Meanwhile, TD, saturation function, and fuzzy logic methods are incorporated into the whole control method.

FASTESO
UAVs with robotic arms have more model uncertainty than traditional UAVs. Additionally, UAVs usually face some other disturbances such as external wind when flying outside. Moreover, the control performance of closed loop system will be largely determined by the observation performance. Therefore, in this part, in order to enhance the performance of feedback controller, a new SMO named FASTESO is built to estimate the lumped disturbance exerted on quadrotor UAV manipulator in finite time.
As for the traditional SMO, the sign function is generally adopted as the control function, and owing to switch time and space lag the SMO performs serious chattering problem [28,29]. In order to alleviate the chattering phenomenon, the sign function is replaced by a saturation function in this work. Moreover, the chattering phenomenon would also be invited by large switching gain because when the control signal crosses the sliding surface, larger switching gain would produce faster and bigger switching control parts and perform terrible chattering phenomenon. Additionally, the invited saturation function has a slower performance than the sign function on arithmetical speed, so there would be a time delay relatively. To further settle the problems of chattering and time delay mentioned before, fuzzy logic rules are employed in FASTESO to adjust the switching gains according to the states of the sliding surface.

Construction of Traditional Super-Twisting Extended State Observer (STESO)
In this part, according to our previous work [30], a traditional SMESO named STESO for quadrotor UAV manipulator is constructed. In this work, the situation, in which the robotic arm keeps constant while the UAV faces external disturbances during the flight, is mainly considered: (η =η = 0). That is a general scenario that the quadrotor UAV manipulator often encounters when conducting aerial tasks. Nevertheless, usually there is unwanted vibration of the equipped robotic arm caused by the high-speed motors and propellers. To consider all mentioned disturbances, the model (25) could be reconstructed as where ∆M 21 , ∆M 22 , ∆M 23 represent model uncertainties. ∆η and ∆η, considered as a portion of model uncertainty, denote the residuals from the temporary variation of the robotic arm parametersη,η. In order to build the observer, reconstruct the attitude dynamic Equation (26) as Introduce the lumped disturbance as Combining Equation (27) and Equation (28), we get where the variablep I is measured directly from the sensors or velocity differential is too noisy to use, so TD is introduced here to estimate the system acceleration for noise alleviation. Additionally,ṗ I anḋ Ψ measured from the sensors are also a little noisy, in that case, TD would be used to reduce noise, too. Moreover, the items M 21 , C 21 and G 2 could be obtained in advance according to the presented UAV model. As for dynamics model (29), considering the feedback linearization method, we could reformulate the control input as Combining Equation (29) and Equation (30), we get When building the STESO, it is supposed that every channel is independent from each other. Therefore, only one channel will be presented here and the other two are completely identical. As for model Equation (31), the one-dimensional model for STESO design is provided as Introduce a new extended state vector ζ = [ζ 1 ζ 2 ] T , where ζ 1 = J iΨi and ζ 2 = d ri * , (i = ϕ, θ, ψ). Reconstruct the model (32) as where χ denotes the derivative of d ri * , supposed by |χ| < f + . It means that the differentiation of lumped disturbance is bounded. Then, according to the work in [30], build STESO for the observable system (33) as where z 1 and z 2 are estimations of ζ 1 and ζ 2 , respectively. e 1 = ζ 1 −ζ 1 and e 2 = ζ 2 −ζ 2 denote estimation errors. We could obtain that the system estimation errors e 1 and e 2 would converge to zero within finite time under suitable observer gains α 1 , α 2 . Moreover, the details of the convergence analysis and parameter selection rules could be found in [30].

Saturation Function
To mitigate the chattering problem, we replace the sign function, usually expressing the discontinuous control in the observer, with a saturation function. The traditional STESO Equation (34) could be rewritten as where k f is the adaptive proportional factor of switching gain generated from fuzzy logic rules which will be introduced in next part. sat() represents the saturation function whose curve is shown in Figure 3, where particularly e is the difference between the real system and the estimated one. k e is the output of the saturation function. We could adjust the sliding mode effect by regulating values of e and k e . Obviously the chattering would be reduced via increasing e and decreasing k e . If e is big enough and k e is small enough, the high-frequency chattering could be avoided. However, meanwhile the robustness of the sliding mode method would also be reduced. Therefore the tuning of these parameters is a tradeoff between the chattering alleviation performance and the system robustness.
Therefore, e and k e should be considered overall according to real applications.

Adaptive Switching Gains with Fuzzy Logic Rules
The fuzzy logic controller was proposed many years ago [31]. In this part, the fuzzy rules are designed according to the fuzzy control theory for effectively optimizing the switching gain based on the states of the sliding-mode surface.
Normally, for purpose of enhancing the ability of observer on anti-disturbance and undertaking the generation of sliding mode, the sliding mode switching gain is often chosen to be too large, but this would further enhance the chattering noise in the disturbance estimation. Therefore, it is feasible to introduce the intelligent method to effectively estimate the switching gain according to the sliding mode arrival condition to alleviate the system chattering problem. For example, as the system state is far from the sliding surface, it means that the absolute value of difference |e| is relatively large, the proportional factor k f should be enlarged to drive the system state back. Similarly, as the system state is close to the sliding surface, proportional factor k f should be smaller.
The proposed fuzzy logic system in this work consists of one input variable and one output variable which would be quantized first. The input and output variables are divided into four fuzzy subsets.
Let |e| = ZR PS PM PB denote the input variable and k f = ZR PS PM PB represent the output variable, respectively. Each rank is depicted by a membership function of trigonometric function, as shown in Figures 4 and 5, where the fuzzy language is defined as ZR(zero), PS(positive small), PM(positive middle), and PB(positive big).  Then define the fuzzy rules as Rule 1: If |e| is PB, then k f is PB. Rule 2: If |e| is PM, then k f is PM. Rule 3: If |e| is PS, then k f is PS. Rule 4: If |e| is ZR, then k f is ZR. Finally, as for the fuzzy outputs, according to the work in [32], the center of gravity algorithm is adopted as the defuzzification method to convert the fuzzy subset duty cycle changes to real numbers, which is given as where µ out represents the resulting output membership function. It takes every rule into account performing the union of the resulting output membership function µ out,i of each Rule i (i = 1, ..., 4), which means the maximum operation between them.

Attitude Controller
In order to achieve high-precision attitude stabilization in the presence of external wind and model uncertainties, a backstepping controller is built here combined with FASTESO for UAV attitude system. The main objective of this part is to guarantee that the state of attitude q and angular rate w converge to the reference values q d and w d in real time. As this part is identical to our previous work [30] except the UAV model, which would not impact the controller design. Therefore we are not going to describe it in detail.
According to the authors of [30], the control signal vector u r is designed as whered r is the estimated result of disturbance from FASTESO. Additionally, like what we did at FASTESO, TD will also be adopted here to process several variables in Equation (37) to reduce noise. Moreover, K b1 , K b2 are controller gains to be designed, and ω e ,ω e , and q ev are defined in Equation (38), details of which can be found in [30].

Simulation
In this section, the performance of proposed FASTESO-based control method is validated on the PX4/Gazebo platform [33], which provides simulations of physics close to the real world. A manually designed scalable disturbance, which includes a sudden change in some special points, is defined in Equation (39) and adopted in the simulations. Moreover, its amplitude could be adjusted by a. The main nominal coefficients of the quadrotor base are generated by the online toolbox of Quan and Dai [34] given in Table 1. Table 1. Quadrotor base coefficients used in simulation.

Coefficients
Description Value

CASE 1 (Observers Performance Comparison)
In this section, several comparison simulations on observer performance of disturbance estimation are conducted.
As shown in Figure 6, both FASTESO and ASTESO have good estimation performance under scalar a = 10. The resultant observer gain of FASTESO is described in Figure 7. Nevertheless, the ASTESO starts to break down when the disturbance is larger, which could be found in Figure 8.
Under a = 20, the ASTESO fails to follow the larger disturbance, but the proposed FASTESO still performs well. This is because the fixed parameter of ASTESO is not big enough to estimate the disturbance with the amplitude in Figure 8. As shown in Figure 9, the adaptive gain of FASTESO also shows why it is effective under disturbances with different amplitudes. To summarize, this part shows that the proposed FASTESO could estimate a wider range of disturbance thanks to the fuzzy logic rules for adaptive observer gain.

Various Observers Comparison
To demonstrate the effectiveness of the introduction of the saturation function into SMO, a simulation similar with last part is conducted under FASTESO with an applied disturbance under a = 5 shown in Figure 10. Meanwhile the 2nd-order linear ESO (ESO2) and STESO are also invited for comparison. The parameters of the mentioned observers are listed. FASTESO:α 1 = diag(0.4,0.4,0.5); α 2 = diag(0.7, 0.7, 0.9); e = 0.0001; k e = 0.05. ESO2:k 1 = diag(4.5,4.5,6.5); k 2 = diag (22,22,28); k 3 = diag (5,5,8); STESO:α 1 = diag(0.2,0.2,0.3); α 2 = diag(0.3, 0.3, 0.4); BAC: K b1 = diag (5,5,8); (3,3,4). From the response results in Figure 10, we could find that FASTESO has a perfect performance on estimation even at the special point with a sudden change. Although the curve under ESO2 could usually follow the actual value, it performs an overestimation near the peak. As for STESO, it could follow the fast variation of disturbance; however, it has a terrible chattering problem which is also the main reason for the introduction of saturation function in this work. To summarize, the FASTESO performs well at all range of disturbance (a = 5,10,20). Meanwhile, it follows better than other traditional observers under varied disturbance.

CASE 2 (Composite Comparison)
To further illustrate the effectiveness of the whole proposed control strategy, we would show all-sided simulation results from Section 5.1.2. Moreover, a single backstepping controller without any observer is also conducted on quadrotor UAV manipulator system for comparison. Three-axis components of disturbance torque d r = d rϕ d rθ d rψ T are equal and shown in Figure 10, which are applied on the UAV simultaneously. The result curves of both attitude and angular rate are shown in Figures 11-16.      We could easily find that, without an observer, the single BAC has biggest attitude offset and angular rate fluctuation, which proves its bad robustness under disturbances. Specifically, the attitude curves even follows the applied torque disturbance well and angular rate has a dramatic change at special points. As for BAC+ESO2, although the estimation value follows the disturbance well to some extent, it has a performance of drastic change on both attitude and angular rate at peak point owing to the over estimation near the peak. As for BAC+STESO, in spite of the good performance of STESO on following the disturbance, it has a terrible chattering phenomenon and leads to a mess on UAV system. Compared to other situations, the UAV under BAC+FASTESO has the best performance on both attitude offset and angular rate fluctuation with varied torque disturbance.

Experiment
In this section, to verify the effectiveness of the whole proposed method, the quadrotor UAV manipulator is assigned to conduct a hovering task beside an electrical fan, which could generate several level gear wind as torque disturbance acting on the UAV system. BAC+FASTESO is adopted for UAV disturbance rejection test, meanwhile a single BAC without any observer is also conducted for comparison. The experimental scene is shown in Figure 17. The whole proposed control scheme is built in Pixhawk [35]. The parameters are chosen as follows. For BAC+FASTESO: (3,3,5); K b2 = diag(2,2,2.5). For BAC: K b1 = diag (6,6,9); K b2 = diag(3.5,3.5,5).
The fan is placed to mainly apply the external torque in roll channel. The results of UAV hovering under first and second gear wind are shown in Figures 18-23. We found that the attitude of hovering UAV changes following the variation of wind gear. Moreover, the attitude vibration offset is reduced by help of FASTESO compared to the situation with single BAC. Meanwhile, the angular rate fluctuation is also reduced. They intuitively verify the effectiveness of FASTESO. Additionally, Figures 22 and 23 show the control output generated only from BAC in the control situations single BAC and BAC+FASTESO, respectively. We could find that the control signal is around zero with FASTESO; however, the control signal is impacted so much owing to external disturbance in situation without any observer. Moreover, the controller gain in BAC+FASTESO could keep smaller to make the whole system stable thanks to the disturbance estimation and compensation from FASTESO, which also contributes to the better performance of BAC+FASTESO. Meanwhile, the estimation of the lumped disturbance including external wind and model uncertainties is shown in Figure 20. Although we do not know the ground truth of the torque disturbance generated from the fan and model uncertainties to verify if the estimation result in Figure 20 is right or not, according to Figures 20 and 22, we could see that their results are almost the same, which also demonstrates the effectiveness of FASTESO to an extent.

Conclusions
In this study, a new observer named FASTESO is proposed, which is incorporated with the saturation function, TD and fuzzy logic rules. Combined with backstepping controller, the whole control scheme for the quadrotor UAV manipulator system is finally built for rejection of lumped disturbance including external wind and model uncertainties. The effectiveness of the whole control structure is veritied in both several simulations and experiments.
In the future, we would pay more attention to the estimation separation of the model uncertainties from the external disturbance.