Quadrotor Trajectory-Tracking Control with Actuator Saturation

Abstract

As a very serious problem for control systems, actuator saturation often leads to an unstable system. The maneuvering flight of quadrotors may lead to actuator saturation, which commonly affects the flight quality, including trajectory-tracking accuracy. Quadrotors usually adopt double-closed-loop control, of which the outer loop is the position control used to calculate the desired attitude angle and the inner loop is the attitude control used to generate control actions for the vehicle. To address actuator saturation during the maneuvering flight of quadrotors, an attitude controller based on the conditioned super-twisting algorithm (CSTA) was designed. The sign functions in CSTA were replaced by a hyperbolic tangent function, which suppresses the chattering of CSTA. In order to improve the rapidity of the improved controller, the tanh-CSTA (TCSTA), a gain adjustment factor was utilized. The rapidity and smoothness of the controller can be satisfied by adjusting the gain factor. Additionally, a sliding surface was designed according to the characteristics of the quadrotor control structure, so that the TCSTA controller can be applied to the second-order system. The performances of the designed improved controller were simulated using MATLAB, and the results showed that the designed controller is more robust than the PID controller. Moreover, compared with the CSTA controller, the chattering of the designed controller was obviously suppressed without reducing the control accuracy.

1. Introduction

Quadrotor UAVs have been widely researched and applied because of their simple structure and vertical take-off and landing abilities [1,2,3]. The stability control of quadrotors includes hovering control and trajectory tracking control. Due to the underactuated, nonlinear, and strong coupling characteristics, the design of trajectory tracking control of quadrotors is difficult. Usually, the control system of the quadrotors can be divided into a fully driven inner loop attitude control and an under-driven outer loop position control [4,5].

Research on the trajectory tracking control of quadrotors has achieved rich results [6,7,8]. However, quadrotors are easily affected by external disturbances, which may lead to actuator saturation. The saturation affects the performances of the whole system, especially the trajectory-tracking performances of quadrotors. When the actuator is saturated, the error accumulation becomes increasingly, which causes windup problems. However, waiting until the accumulated error becomes too large, for the error to be reversed, and for the actuator to take correct actions takes too long, which may make the system unstable.

At present, some achievements have been made in the actuator saturation control of the quadrotor. Faessler et al. [9] divided the controllers into different priorities: the highest priority controllers were the roll and pitch controllers, and the controller with the second-highest priority was the thrust controller, while the controller with the lowest priority was the yaw controller. Combined with the designed iterative control allocation algorithm, the controller performances with actuator saturation were improved. Zaki et al. [10] obtained the inverse of the control allocation matrix with the quadratic optimal method, and the method was extended by Tariq et al. to handle actuator saturation [11]. In Chen et al. [12], an efficient thrust mixer was designed by reducing the priority of the yaw control, and the control allocation matrix was redesigned to deal with the problem of actuator saturation; the experimental results showed that the system performances were indeed improved. In [13,14,15], the trajectory tracking control of a quadrotor was realized by using fuzzy control.

Omid et al. [16] and Lin et al. [17] utilized the reinforcement learning method to design the controller. In order to ensure that the controller output did not exceed the limit, an output threshold was set during the learning process. Zhao et al. [18] utilized the backstepping method to design an adaptive fault-tolerant control, and an auxiliary system was employed to deal with actuator saturation. Li et al. [19] designed an adaptive controller commonly used in robot control for a quadrotor, and the auxiliary variable method was used to deal with the problem of input saturation. Wang et al. [20] designed a backstepping controller with auxiliary variables to ensure that the controller output was within the limits, thus avoiding actuator saturation. Sidi et al. [21] made the controller satisfy the input saturation limit by using second-order auxiliary variables, so that the sliding mode controller could achieve stable tracking of the desired trajectory. Liu et al. [22] dealt with the actuator saturation problem using a novel auxiliary variable.

Wang et al. [23] used the disturbance observer to estimate the external disturbance, and the output of the position controller was limited to ensure that the desired angles of the inner loop were within a certain range. Xu et al. [24] used the hyperbolic tangent function to approximate actuator saturation, and the controller outputs were adjusted by the hyperbolic tangent function and then applied to the actuator, so that the virtual control could be freely designed. Harshavarthini et al. [25] designed a non-fragile robust controller to solve the problem of actuator saturation with external disturbance. In Ji et al. [26] and Wang et al. [27], the saturation problem in the direction of the thrust was mainly considered. Shao et al. [28] designed adaptive sliding mode controllers to deal with actuator saturation.

There are other feasible ways to deal with the actuator-saturation problem, among which sliding mode control is widely researched [29,30]. Hu et al. [31] proposed terminal sliding mode control for the input-constrained problem, and a tanh-type slowly varying adaptive law was integrated into an auxiliary system to compensate for input saturation. Zhang et al. [32] proposed a novel three-dimensional adaptive sliding mode controller for missiles, and the tanh function was used to avoid chattering of the controller. The super-twisting algorithm (STA) is a typical second-order sliding mode controller. However, if actuator saturation exists in the system, the STA will be affected by the windup problem [33]. In order to solve the windup problem, Seeber et al. [34] designed a conditioned STA (CSTA) controller based on the feasible reference proposed by Hanus et al. [35]. In this paper, the CSTA was improved by replacing the sign function with the hyperbolic tangent function to avoid the chattering problem. The CSTA controller was designed for a first-order system, but the attitude subsystem in each direction is a second-order system. Therefore, an appropriate sliding surface was designed to apply the improved CSTA controller to the second-order system.

The rest of this paper is organized as follows. In Section 2, the mathematical model of the quadrotor with disturbance is established. In Section 3, the specific improvement method of the CSTA controller is presented, and the control system of the quadrotor is designed. In Section 4, the performances of the designed controller are verified. Conclusions are given in Section 5.

2. Modeling of Quadrotor

The dynamics model of the quadrotor was constructed using the Newton–Euler method. The model of the quadrotor is shown in Figure 1, the coordinate frames of the quadrotor consist of the world reference frame $\sum \mathit{E}$$\left(O_E{X}_E{Y}_E{Z}_E\right)$ and the body frame $\sum \mathit{B}$$\left(O_B{X}_B{Y}_B{Z}_B\right)$. The forces and torques generated by the four rotors can be combined into the torques and thrust of the quadrotor. The quadrotor can track the desired trajectory by adjusting the rotation speed of the four rotors.

2.1. Thrust and Torque

The motion of the quadrotor depends on the rotation of the four rotors. Thrust and torque are generated on the blade during high-speed rotation of the rotor. The thrust of the $i_{th}$ propeller is proportional to the square of the speed ${\omega }_i$:

$$T_i=C_t·{\omega }_i^2$$

(1)

where $C_t$ is the thrust coefficient and ${\omega }_i,(i\in 1,2,3,4)$ is the rotation speed of $i_{th}$ propeller. The total thrust $U_1$ is the summation of $T_i$:

$$\begin{array}{cc}\hfill U_1& =\sum _{i=1}^4{T}_i\hfill \\ \hfill & =C_t({\omega }_1^2+{\omega }_2^2+{\omega }_3^2+{\omega }_4^2)\hfill \end{array}$$

(2)

The thrust is always along the positive ${\mathit{Z}}_{\mathit{B}}$ axis, so the thrust presented in body frame $\sum \mathit{B}$ is as follows:

$${\mathit{F}}_{\mathit{b}}=U_1·{\mathit{e}}_{\mathbf{3}}=\left[\begin{array}{c}0\\ 0\\ U_1\end{array}\right]$$

(3)

where ${\mathit{e}}_{\mathbf{3}}={\left[\begin{array}{c}0,0,1\end{array}\right]}^T$ is the unit vector for the ${\mathit{Z}}_{\mathit{B}}$ axis.

The roll and pitch torques are generated by changing the rotation speed of the propellers. If the rotation speed of the second propeller decreases while the fourth propeller increases, the quadrotor will rotate along the ${\mathit{X}}_B$ axis. If the rotation speed of the third propeller decreases while the first propeller increases, the quadrotor will rotate along the ${\mathit{Y}}_B$. Therefore, the roll and pitch torques are computed as follows:

$$\begin{array}{cc}\hfill U_2& =l·C_t(-{\omega }_2^2+{\omega }_4^2)\hfill \\ \hfill U_3& =l·C_t({\omega }_1^2-{\omega }_3^2)\hfill \end{array}$$

(4)

where $U_2$ and $U_3$ are roll and pitch torques, respectively, and l is the length of the quadrotor’s arm.

When the speed of the rotors are high enough, the air resistance on the rotor produces torque. The torque of the ith propeller depends on the rotation speed and rotation direction. The value of torque can is proportional to the square of the speed ${\omega }_i$:

$$M_i=C_d·{\omega }_i^2.$$

(5)

where $C_d$ is the drag factor. These torques are what drive the yaw motion of the quadrotor, and the total torque is calculated as follows:

$$\begin{array}{cc}\hfill U_4& =\sum _{i=1}^4{(-1)}^i{M}_i\hfill \\ \hfill & =C_d·(-{\omega }_1^2+{\omega }_2^2-{\omega }_3^3+{\omega }_4^2)\hfill \end{array}$$

(6)

The torques presented in body frame are as follows:

$${\mathit{M}}_{\mathit{b}}=\left[\begin{array}{c}{U}_2\\ U_3\\ U_4\end{array}\right]$$

(7)

The control input of the quadrotor can be rewritten in matrix form:

$$\left[\begin{array}{c}{U}_1\\ U_2\\ U_3\\ U_4\end{array}\right]=\left[\begin{array}{cccc}{C}_t& C_t& C_t& C_t\\ 0& -l·C_t& 0& l·C_t\\ l·C_t& 0& -l·C_t& 0\\ -C_d& C_d& -C_d& C_d\end{array}\right]\left[\begin{array}{c}{\omega }_1^2\\ {\omega }_2^2\\ {\omega }_3^2\\ {\omega }_4^2\end{array}\right].$$

(8)

The gyroscopic torque is calculated as follows:

$$\begin{array}{c}\hfill {\mathit{\tau }}_{\mathit{g}}={\mathit{J}}_{\mathit{m}}({\mathit{\omega }}_{\mathit{b}}\times {\mathit{e}}_{\mathbf{3}})·(-{\omega }_1+{\omega }_2-{\omega }_3+{\omega }_4),\end{array}$$

(9)

where ${\mathit{J}}_{\mathit{m}}$ is the inertia moment of the motor and propeller, and ${\mathit{\omega }}_{\mathit{b}}$ is the angular velocity in the body coordinate frame. Just to make it easier to compute the matrix cross product, the vector can be written in skew symmetric matrix form. The angular velocity vector in the body fram is ${\mathit{\omega }}_{\mathit{b}}={[{\omega }_x,{\omega }_y,{\omega }_z]}^T$, and the skew symmetric matrix ${\mathit{\omega }}_{\mathit{b}}^{\times }$ can be defined as follows:

$${\mathit{\omega }}_{\mathit{b}}^{\times }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$

(10)

Therefore, the gyroscopic torque can be calculated as follows:

$$\begin{array}{cc}\hfill {\mathit{\tau }}_{\mathit{g}}& ={\mathit{J}}_{\mathit{m}}({\mathit{\omega }}_{\mathit{b}}\times {\mathit{e}}_{\mathbf{3}})·(-{\omega }_1+{\omega }_2-{\omega }_3+{\omega }_4)\hfill \\ \hfill & ={\mathit{J}}_{\mathit{m}}\left({{\mathit{\omega }}_{\mathit{b}}}^{\times }{\mathit{e}}_{\mathbf{3}}\right)·{\omega }_r\hfill \\ \hfill & ={\mathit{J}}_{\mathit{m}}\left[\begin{array}{c}{\omega }_y\\ -{\omega }_x\\ 0\end{array}\right]{\omega }_r.\hfill \end{array}$$

(11)

where ${\omega }_r=-{\omega }_1+{\omega }_2-{\omega }_3+{\omega }_4$ is the algebraic sum of four rotor speeds.

2.2. Dynamic Model of Quadrotor

The position of quadrotors in the world frame $\sum E$ is $\mathit{P}={[X,Y,Z]}^T$, and the attitude of the quadrotor represented by a Euler angle is $\mathsf{\Theta }={[\varphi ,\theta ,\psi ]}^T$, in which $\varphi$, $\theta$, and $\psi$ denote roll, pitch, and yaw angle, respectively. Let ${\mathit{R}}_{\mathit{b}}^{\mathit{e}}$ denote the rotation matrix, which can be denoted as follows:

$${\mathit{R}}_{\mathit{b}}^{\mathit{e}}=\left[\begin{array}{ccc}C\psi C\theta & S\psi C\varphi +C\psi S\theta S\varphi & S\psi S\theta +C\psi S\theta C\varphi \\ S\psi C\theta & C\psi C\varphi +S\psi S\theta S\varphi & C\psi S\theta +S\psi S\theta C\varphi \\ -S\theta & C\theta S\varphi & C\theta C\varphi \end{array}\right]$$

(12)

where $C_{\ast }$ and $S_{\ast }$ represent the $cos(\ast )$ and $sin(\ast )$, respectively. Therefore, the relationship between velocity ${[U,V,W]}^T$ in body frame and the time derivative of position ${[\dot{X},\dot{Y},\dot{Z}]}^T$ is as follows:

$$\left[\begin{array}{c}\dot{X}\\ \dot{Y}\\ \dot{Z}\end{array}\right]={\mathit{R}}_{\mathit{b}}^{\mathit{e}}\left[\begin{array}{c}U\\ V\\ W\end{array}\right]$$

(13)

The angular velocity of body frame ${\mathit{\omega }}_{\mathit{b}}$ can be converted to a time derivative of the Euler angle $\dot{\mathsf{\Theta }}={[\dot{\varphi },\dot{\theta },\dot{\psi }]}^T$:

$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]={\mathit{T}}_{\mathit{b}}^{\mathit{e}}\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]=\left[\begin{array}{ccc}1& sin\left(\varphi \right)tan\left(\theta \right)& cos\left(\varphi \right)tan\left(\theta \right)\\ 0& cos\left(\varphi \right)& -sin\left(\varphi \right)\\ 0& sin\left(\varphi \right)sec\left(\theta \right)& cos\left(\varphi \right)sec\left(\theta \right)\end{array}\right]\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]$$

(14)

The quadrotor can be regarded as a symmetrical rigid body. The origin of the body frame is located at the center of mass of the quadrotor. The dynamic model of the quadrotor can be obtained using the Newton–Euler equations as follows:

$$\begin{array}{cc}\hfill m\ddot{\mathit{P}}& =-mg{\mathit{e}}_{\mathbf{3}}+{\mathit{R}}_{\mathit{b}}^{\mathit{e}}({\mathit{F}}_{\mathit{b}}+{\mathit{F}}_{\mathit{d}})\hfill \\ \hfill {\mathit{J}}_{\mathit{b}}\dot{{\mathit{\omega }}_{\mathit{b}}}& =-{\mathit{\omega }}_{\mathit{b}}\times ({\mathit{J}}_{\mathit{b}}·{\mathit{\omega }}_{\mathit{b}})+{\mathit{\tau }}_{\mathit{g}}+{\mathit{M}}_{\mathit{b}}+{\mathit{M}}_{\mathit{d}}\hfill \end{array}$$

(15)

where m is the mass of the quadrotor; g is the gravitational acceleration; ${\mathit{R}}_{\mathit{b}}^{\mathit{e}}$, ${\mathit{F}}_{\mathit{b}}$, ${\mathit{\omega }}_{\mathit{b}}$, ${\mathit{\tau }}_{\mathit{g}}$, and ${\mathit{M}}_{\mathit{b}}$ are as described previously; ${\mathit{F}}_{\mathit{d}}={[f_{dx},f_{dy},f_{dz}]}^T$ are the external disturbance forces; ${\mathit{M}}_{\mathit{d}}={[{\tau }_{dx},{\tau }_{dy},{\tau }_{dz}]}^T$ is the external disturbance torques; ${\mathit{J}}_{\mathit{b}}$ is the inertia matrix of the quadrotor, considering the symmetry of the quadrotor; and the inertia torque has the following form:

$${\mathit{J}}_{\mathit{b}}=\left[\begin{array}{ccc}Jx& 0& 0\\ 0& J_y& 0\\ 0& 0& J_z\end{array}\right]$$

(16)

where $J_x$, $J_y$, and $J_z$ are the moments of inertia. In order to rewrite the dynamic model (15) as an equation of state, the state variable $\mathit{\chi }$ is defined as follows:

$$\begin{array}{c}\hfill \mathit{\chi }={[X,\dot{X},Y,\dot{Y},Z,\dot{Z},\varphi ,{\omega }_x,\theta ,{\omega }_y,\psi ,{\omega }_z]}^T.\end{array}$$

(17)

Thus, the dynamic model of quadrotor in state equation form is as follows:

$$\dot{\mathit{\chi }}=\mathit{f}(\mathit{\chi },\mathit{U})=\left[\begin{array}{c}\dot{X}\\ \frac{(sin\left(\theta \right)cos\left(\varphi \right)cos\left(\psi \right)+sin\left(\theta \right)sin\left(\psi \right))(U_1+F_{dx})}{m}\\ \dot{Y}\\ \frac{(sin\left(\theta \right)cos\left(\varphi \right)cos\left(\psi \right)-sin\left(\theta \right)sin\left(\psi \right))(U_1+F_{dy})}{m}\\ \dot{Z}\\ \frac{cos\left(\varphi \right)cos\left(\theta \right)(U_1+F_{dz})}{m}-g\\ {\omega }_{y}tan\left(\theta \right)sin\left(\varphi \right)+{\omega }_{z}tan\left(\theta \right)cos\left(\varphi \right)\\ \frac{J_y-J_z}{J_x}{\omega }_y{\omega }_z+\frac{J_m{\omega }_y{\omega }_r+U_2+{\tau }_{dx}}{J_x}\\ {\omega }_{y}cos\left(\varphi \right)-{\omega }_{z}sin\left(\varphi \right)\\ \frac{J_z-J_x}{J_y}{\omega }_x{\omega }_z+\frac{-J_m{\omega }_x{\omega }_r+U_3+{\tau }_{dy}}{J_y}\\ \frac{{\omega }_{y}sin\left(\varphi \right)}{cos\left(\theta \right)}+\frac{{\omega }_{z}cos\left(\varphi \right)}{cos\left(\theta \right)}\\ \frac{J_x-J_y}{J_z}{\omega }_x{\omega }_y+\frac{U_4+{\tau }_{dz}}{J_z}\end{array}\right]$$

(18)

3. Controller Design

The double-loop control structure is usually adopted by quadrotors, which is shown in Figure 2. The outer loop is position control, which obtains the desired attitude angle from positions X and Y. The inner loop is used to control the attitude and altitude of the quadrotor. The control action of the altitude and attitude affects the actuator directly. In order to meet the performance requirements of the system, an inner loop controller is designed to deal with actuator saturation.

3.1. Conditioned Super-Twisting Algorithm

The super-twisting algorithm (STA) is a typical second-order sliding mode control algorithm that can avoid the chattering phenomenon of a controller. The error of the system is denoted as $\sigma$, the control output is u, and the original STA designed in [36] is described as follows:

$$\begin{array}{cc}\hfill u& =k_1\sqrt{\left|\sigma \right|}sign\left(\sigma \right)+v,\hfill \\ \hfill \dot{v}& =k_{2}sign\left(\sigma \right).\hfill \end{array}$$

(19)

where $k_1$ and $k_2$ are positive parameters. The STA described in Equation (19) has a windup problem when the actuator is saturated. In order to deal with the windup problem caused by actuator saturation, Hanus et al. [35] proposed a realizable reference trajectory $r^{\ast }$, which can avoid the large output of controller. When the realizable reference $r^{\ast }$ is applied to the STA, the algorithm can be rewritten as follows:

$$\begin{array}{cc}\hfill u^{\ast }& =k_1\sqrt{|r^{\ast }-x|}sign(r^{\ast }-x)+v,\hfill \\ \hfill \dot{v}& =k2sign(r^{\ast }-x)\hfill \end{array}$$

(20)

where x is the system state. Seeber and Reichhartinger [34] found that $sign(r^{\ast }-x)=sign(u^{\ast }-v)$, so the conditioned super-twisting algorithm (CSTA) was proposed:

$$\begin{array}{cc}\hfill u& =k_1\sqrt{|r-x|}sign(r-x)+v,\hfill \\ \hfill \dot{v}& =k_{2}sign(u^{\ast }-v),\hfill \\ \hfill u^{\ast }& =sa{t}_U\left(u\right).\hfill \end{array}$$

(21)

where the $sa{t}_U\left(u\right)$ is the saturation function, which is defined as follows:

$$sa{t}_U\left(u\right)=\left\{\begin{array}{cc}u\hfill & \left|u\right|\le U_{max}\hfill \\ U_{max}sign\left(u\right)\hfill & \left|u\right|>U_{max}\hfill \end{array}\right.$$

(22)

3.2. Improved Conditioned Super-Twisting Algorithm

The CSTA proposed in [34] is aimed at the first-order system. The attitude of the quadrotor is a second-order nonlinear system, so the error e in the CSTA should be replaced by a high-order sliding surface. The general sliding surface is constructed as follows:

$$s={(\frac{d}{dt}+\lambda )}^{(n-1)}e$$

(23)

where $\raisebox{1ex}{$d$}\!\left/ \!\raisebox{-1ex}{$dt$}\right.$ is the differential operator, $\lambda$ is a positive constant parameter, and n is the order of the system. Sudden changes in the attitude cause a large angular velocity. If such a large angular velocity is directly introduced into the sliding surface, it will cause a step change in the sliding surface, which leads to dramatic change in the controller output. To deal with this problem, the following sliding surface is designed:

$$\begin{array}{cc}\hfill s_i& ={\lambda }_i{e}_i-{\dot{x}}_i,\hfill \\ \hfill e_i& \in \{{\varphi }_d-\varphi ,{\theta }_d-\theta ,{\psi }_d-\psi ,Z_d-Z\},\hfill \\ \hfill {\dot{x}}_i& \in \{{\omega }_x,{\omega }_y,{\omega }_z,\dot{Z}\}.\hfill \end{array}$$

(24)

where ${\varphi }_d$, ${\theta }_d$, ${\psi }_d$, and $Z_d$ are reference values.

The chattering of sliding mode control is mainly caused by discontinuity of the sign function $sign(·)$. The chattering can be avoided by using hyperbolic tangent function $tanh(·)$ instead of sign function $sign(·)$. However, the hyperbolic tangent function will make the response speed of the controller not fast enough. In order to improve the response speed of the improved controller, a gain factor is adopted by the hyperbolic tangent function. The specific form of the improved controller TCSTA (tanh-CSTA and TCSTA) is as follows:

$$\begin{array}{cc}\hfill u_i& =k_i^1\sqrt{|s_i|}tanh({\alpha }_i·s_i)+v_i,\hfill \\ \hfill {\dot{v}}_i& =k_i^{2}tanh\left({\beta }_i(u_i^{\ast }-v_i)\right),\hfill \\ \hfill u_i^{\ast }& =sa{t}_{U_i}\left(u_i\right).\hfill \end{array}$$

(25)

where $k_i^1$, $k_i^2$, ${\alpha }_i$, and ${\beta }_i$$(i\in \{\varphi ,\theta ,\psi ,Z\left\}\right)$ are positive gain coefficients for each controller.

The structure of designed sliding mode control TCSTA is shown in Figure 3. A large ${\alpha }_i$ makes the TCSTA controller have a fast response. However, if the value of ${\alpha }_i$ is too large, it will cause the controller chattering. A small ${\alpha }_i$ makes the TCSTA controller output smooth. Similarly, if the value of ${\alpha }_i$ is too small, the controller responds too slow. As shown in Figure 3, $v_i$ is used to compensate the error between the direct output of the controller and the output after amplitude limiting. If ${\beta }_i$ is too small, the error compensation will be too slow and the controller performances will be affected. At the same time, $v_i$ is used to make the controller smoother, so ${\beta }_i$ cannot be too large.

When a TCSTA controller is applied to the quadrotor, the specific control structure is shown in Figure 4, in which the roll $\varphi$ control is taken as an example.

3.3. Trajectory-Tracking Control

Outer loop control is used to calculate the desired attitude angle. The desired attitude ${\varphi }_d$ and ${\theta }_d$ can be calculated as in [37].

$$\begin{array}{cc}\hfill {\varphi }_d& =arcsin\left(\frac{u_{x}sin\left({\psi }_d\right)-u_{y}cos\left({\psi }_d\right)}{u}\right)\hfill \\ \hfill {\theta }_d& =arctan\left(\frac{u_{x}cos\left({\psi }_d\right)+u_{y}sin\left({\psi }_d\right)}{u_z}\right)\hfill \\ \hfill u& =\sqrt{u_x^2+u_y^2+u_z^2}\hfill \end{array}$$

(26)

where ${\psi }_d$ is the desired yaw angle and $u_x$, $u_y$, and $u_z$ are control actions for the X, Y, and Z axes, respectively.

$u_x$ and $u_y$ are used to calculate the desired ${\varphi }_d$ and ${\theta }_d$ and do not directly affect the actuator. Therefore, using the PID algorithm to calculate $u_x$ and $u_y$ can not only meet the requirements but also reduce computation cost.

$$\begin{array}{cc}\hfill u_x& =k_{px}{e}_x-k_{dx}U+k_{ix}\int e_{x}dt\hfill \\ \hfill u_y& =k_{py}{e}_y-k_{dy}V+k_{iy}\int e_{y}dt\hfill \end{array}$$

(27)

where $e_x=X-X_d$ and $e_y=Y-Y_d$ are horizontal position errors; U and V are horizontal velocities in the body frame; and $k_{px}$, $k_{py}$, $k_{dx}$, $k_{dy}$, $k_{ix}$, and $k_{iy}$ are controller parameters.

Because the Z axis control directly affects the actuator, TCSTA control is used on the Z axis. The specific form is as follows:

$$\begin{array}{cc}\hfill u_z& =k_z^1\sqrt{|s_z|}tanh\left({\alpha }_z{s}_z\right)+v_z\hfill \\ \hfill {\dot{v}}_z& =k_z^{2}tanh\left({\beta }_i(u_i^{\ast }-v_z)\right)\hfill \\ \hfill u_z^{\ast }& =sa{t}_{U_z}\left(u_z\right)\hfill \\ \hfill s_z& ={\lambda }_z{e}_z-\dot{Z}\hfill \end{array}$$

(28)

with the meaning of parameters described in Equations (24) and (25).

4. Simulations and Results

In order to verify the good performances of the TCSTA controller, the MATLAB/Simulink-based numerical simulations are carried out, and the CSTA and PID are used as comparison algorithms. In order to improve the performance of the PID controller, the PID controller adopts the following forms:

$$u=\left\{\begin{array}{cc}{k}_{pu}e+k_i\int edt-k_d\dot{x}\hfill & if e>e_t\hfill \\ k_{pl}e+k_i\int edt-k_d\dot{x}\hfill & if e\le e_t\hfill \end{array}\right.$$

(29)

where $e_t$ is the threshold value; $k_{pu}$, $k_{pl}$, $k_i$, and $k_d$ are controller parameters; and $\dot{x}$ is the derivative of the corresponding states. Specific parameters of the PID controller are shown in

Table A3. The parameters of the PID controller.

	${\mathit{e}}_{\mathit{t}}$	${\mathit{K}}_{\mathit{pu}}$	${\mathit{K}}_{\mathit{pl}}$	${\mathit{K}}_{\mathit{i}}$	${\mathit{K}}_{\mathit{d}}$
$\varphi$	0.1	4.0	1.65	0.50	0.199
$\theta$	0.1	4.0	1.65	0.50	0.199
$\psi$	0.785	10.0	9.0	0.55	39
Z	1	13.89	10.6	0.65	8.6

, and the parameters of the TCSTA and CSTA are shown in

Table A1. The parameters of the TCSTA controller.

	$\mathit{\lambda }$	${\mathit{K}}_1$	${\mathit{K}}_2$	$\mathit{\alpha }$	$\mathit{\beta }$
$\varphi$	3.75	2.52	6.9	0.25	3
$\theta$	3.75	2.52	6.9	0.25	3
$\psi$	3.50	3.20	6.5	0.35	2
Z	20.471	30.0	10.0	0.212	1

and

Table A2. The parameters of the CSTA controller.

	$\mathit{\lambda }$	${\mathit{K}}_1$	${\mathit{K}}_2$
$\varphi$	1.5	0.72	1.5
$\theta$	1.5	0.72	2
$\psi$	1.55	0.21	3.5
Z	2.1	11.0	14.0

, respectively.

To verify the robustness of the TCSTA controller, external disturbances are added to the quadrotor. The specific form of disturbance is as follows:

$$dis=a(c+r+bsin(\omega t\left)\right)$$

(30)

where c is a constant disturbance, r is a random number evenly distributed between $-1$ and 1, b is the amplitude of the sinusoidal disturbance, and $\omega$ is the angular frequency. The parameter values are different in different channels. Different cases of disturbance are regulated by a, which can be taken as 0, $0.5$, and $1.0$.

The performances of the three controllers on the attitude loop control are first compared. In different disturbance conditions, the setting time, steady-state accuracy, and controller output of the three controllers are compared. The magnitude–frequency characteristic is an important index to evaluate the performance of the controller. Then, different reference trajectories are designed to compare the tracking performances of the three controllers under the influence of disturbances.

The parameters of the quadrotor are shown in

Table 1. The parameters of the quadrotor.

Parameter	Values
Gravitational acceleration g	$9.81{\mathrm{m}/\mathrm{s}}^2$
Body mass m	$2\mathrm{Kg}$
Arm length l	$0.225\mathrm{m}$
Thrust coefficient $C_t$	$1.1830\times {10}^{-8}{\mathrm{N}/(\mathrm{r}/\min )}^2$
Torque coefficient $C_q$	$1.9250\times {10}^{-10}\mathrm{N}·{\mathrm{m}/(\mathrm{r}/\min )}^2$
Motor maximum speed n	$25,500\mathrm{r}/\min$
Moment of inertia around the $x_b$ axis $I_x$	$2.5557\times {10}^{-4}\mathrm{Kg}·{\mathrm{m}}^2$
Moment of inertia around the $y_b$ axis $I_y$	$2.5557\times {10}^{-4}\mathrm{Kg}·{\mathrm{m}}^2$
Moment of inertia around the $z_b$ axis $I_z$	$5.0238\times {10}^{-4}\mathrm{Kg}·{\mathrm{m}}^2$

. The quadrotor is configured in the “+” configuration. According to $M_{\varphi ,\theta }=l·C_t·n^2$, the maximum value of the roll and pitch control torque can be calculated as $M_{\varphi ,\theta }=1.73\mathrm{N}·\mathrm{m}$. According to $M_{\psi }=2l·C_q·n^2$, the maximum value of the yaw control torque can be calculated as $M_{\psi }=0.56\mathrm{N}·\mathrm{m}$. According to $T_z=4·C_t{n}^2$, the maximum value of thrust can be calculated as $T_z=30\mathrm{N}$. Considering the coupling between states, set $M_{\varphi ,\theta }=1.5\mathrm{N}·\mathrm{m}$, $M_{\psi }=0.5\mathrm{N}·\mathrm{m}$, and $T_z=30\mathrm{N}$.

4.1. Attitude-Control Simulation

Considering the symmetry of the quadrotor, only the roll $\varphi$ and yaw $\psi$ were simulated, and the performances of the three controllers were compared under different disturbances.

Simulation of Roll Control

In the case of no disturbance, the simulation results are shown in Figure 5. According to Figure 5a, three controllers can track the sine wave well in the undisturbed condition. As shown in Figure 5b, the tracking error is limited to $0.04$ rad, which is mainly caused by the phase error. The phase errors and peak-to-peak errors of the TCSTA, CSTA, and PID are shown in

Table 2. Comparison of rolling control under different disturbances.

	PID		CSTA		TCSTA
	Phase Error	P-P Error	Phase Error	P-P Error	Phase Error	P-P Error
Undisturbance	0.322	0.0065	0.107	0.0021	0.153	0.0062
0.5 disturbance	0.375	0.103	0.115	0.045	0.192	0.048
1.0 disturbance	-	-	0.139	0.053	0.213	0.056

. The controller outputs are shown in Figure 5c, and the CSTA controller output is greater than that of the TCSTA and PID controllers. The amplitude–frequency characteristics of the three controllers are shown in Figure 5d. The CSTA controller has a large amplitude at high frequency, while the TCSTA has a small amplitude, which indicates that the TCSTA controller can effectively avoid the controller-chattering problem.

When the amplitude of disturbance are $0.5$ times and $1.0$ times, the simulation results are shown in Figure 6 and Figure 7, respectively. It can be seen from Figure 6a,b and Figure 7a,b that both TCSTA and CSTA can suppress the disturbance effectively, while the tracking error of the PID controller increases with an increase in the disturbance. Compared with Figure 6c and Figure 7c, the outputs of the three controllers have the same trend. Moreover, chattering of the CSTA controller is the most serious, which can be verified by Figure 6d and Figure 7d.

In addition, due to the increase in disturbance, the outputs of the three controllers reach the threshold, which is shown in Figure 7c. In this case, the TCSTA and CSTA controllers can still track the reference value well, while the PID controller is unstable. Based on the above analysis, the TCSTA controller is as robust as the CSTA controller and can effectively avoid chattering.

To facilitate analysis, the main simulation results of the rolling control are listed in Table 2. From Table 2, the phase errors of the three controllers TCSTA, CSTA, and PID increase with the increase in disturbance, and the PID controller begins to become unstable when the disturbance increases to $1.0$ times. The phase error of the TCSTA is slightly greater than that of the CSTA. The error between the peak value of the response curve and that of the expected curve is considered, which is defined as a P-P error in Table 2. The P-P errors of the CSTA and TCSTA are always small, whereas the error of the PID controller increases with disturbance until the controller becomes unstable.

The simulation results of the tracking square signal are shown in Figure 8. It can be seen from Figure 8a,b that a large disturbance makes the tracking error increase. The TCSTA and CSTA can still ensure system stability, while the PID is no longer stable. It is mainly due to a sudden change in reference signal and large external disturbance that controller saturation occurs. From Figure 8d, the amplitude of the TCSTA at high frequency is almost zero, while the CSTA still has a large amplitude. The simulation results show that the TCSTA controller can effectively resist the external disturbance and suppress the chattering problem even with a large disturbance.

4.2. Trajectory-Tracking Control Simulation

In this section, the movement of the quadrotor along a single axis is simulated first. Then, an elliptic trajectory is designed to verify the performances of the controllers. Moreover, considering the influence of different disturbances, the controller performances are studied from the aspects of robustness to external disturbances, chattering suppression, tracking accuracy, and so on.

4.2.1. Simulation of X-Axis Control

The movement in the X direction can be used to evaluate the performances of the double closed-loop control structure of the system. The simulations of the tracking step and sinusoidal trajectory in the X direction are carried out to research the regulation performances and tracking performances of the system. The simulation results in the case of undisturbed and $1.0$ times disturbance are shown in Figure 9 and Figure 10. According to Figure 9a, three controllers can track the step signal almost without steady state error, and there is no significant difference in their setting time. According to Figure 9b, TCSTA and PID give large outputs during the transient process, and the controller output gradually converges to zero when the system reaches the desired position. The chattering of the CSTA is serious due to the discontinuity of the sign function, and the magnitude–frequency characteristics in Figure 9c also illustrate this conclusion.

According to Figure 10, the PID controller becomes unstable when the disturbance increases, while the TCSTA and CSTA can keep the quadrotor near the desired position. Although the output of the controller is saturated, the tracking error of TCSTA is not significantly greater than that of CSTA. Therefore, the TCSTA controller still delivers good performance in trajectory tracking control with controller saturation.

The simulation results of a quadrotor tracking sinusoidal trajectory in the X direction under the condition of $1.0$ times disturbance are shown in Figure 11. Since the PID controller is unstable, there are only TCSTA and CSTA response curves in Figure 11. TCSTA and CSTA controllers can track the sine wave well. The tracking errors in Figure 11b are mainly caused by the phase error. The phase errors of the TCSTA and CSTA are $0.2689$ rad and $0.2805$ rad, respectively. Figure 11c shows the outputs of the TCSTA and CSTA. There is no obvious difference between the two controllers, that is to say, the performances of the two controllers are equivalent in this environment configuration.

4.2.2. Simulation of Elliptical Tracking Control

The elliptical trajectory is a typical trajectory in which curvature changes with time. The elliptical trajectory is shown as follows:

$$\begin{array}{cc}\hfill X_d& =a+r·cos\left(\omega t\right)·u_1+r·sin\left(\omega t\right)·v_1,\hfill \\ \hfill Y_d& =b+r·cos\left(\omega t\right)·u_2+r·sin\left(\omega t\right)·v_2,\hfill \\ \hfill Z_d& =c+r·cos\left(\omega t\right)·u_3+r·sin\left(\omega t\right)·v_3.\hfill \end{array}$$

(31)

where $(a,b,c)$ is the center of the ellipse, $r=1$ is the radius, $\omega =0.2$ is the angular velocity, and $U={[u_1,u_2,u_3]}^T$ and $V={[v_1,v_2,v_3]}^T$ are two orthogonal vectors. If U and V have the same length, Equation (31) describes a circle in three dimensional Euclidean space; otherwise, Equation (31) describes an ellipse. In the simulation, U, V, and $(a,b,c)$ are set to $[0,-\sqrt{2},\sqrt{(}2){]}^T$, ${[1,0,0]}^T$, and $(0,0,5)$, respectively.

The simulation results of tracking the elliptical trajectory by the three controllers without disturbance are shown in Figure 12. As shown in Figure 12a, the performances of the three controllers in the undisturbed case are all satisfactory. The trajectory errors are obtained by subtracting the radius of quadrotor from the radius of the desired trajectory, and the results are shown in Figure 12b. From Figure 12b, it can be seen that the maximum tracking error could reach $0.15$ m. In order to analyze the cause of the error, the error is projected onto the $X-Y$ plane and the Z axis, respectively. Additionally, the results are shown in Figure 13. From Figure 13, there is almost no error in the $X-Y$ plane projection trajectory, while there are phase error $0.1682$ rad and amplitude error $0.04$ m on the Z axis. The phase error has been analyzed in Section 4.2.1, and it is difficult to directly observe the phase error of the closed curve in Figure 13a.

The simulation results of $0.5$ times and $1.0$ times disturbance are shown in Figure 14 and Figure 15, respectively. With the increase in disturbance, the error of the system under PID control increases gradually, until the system becomes unstable. The tracking errors of the TCSTA and CSTA also increase with the increase in disturbance. It can be seen from Figure 14a,b that due to the increase in disturbance, the TCSTA and CSTA can still control the quadrotor near the reference trajectory, while the trajectory of PID is far from the desired trajectory. When the disturbance further increases, the PID controller becomes unstable, which is shown in Figure 15a. The outputs of the three controllers are shown in Figure 14c. When the disturbance increases to $1.0$ times, the outputs of the three controllers reach saturation, which makes the PID controller unstable.

To compare the three controllers more intuitively, the tracking results of the three controllers are listed in

Table 3. Comparison of tracking control under different disturbances.

	PID		CSTA		TCSTA
	${\mathit{me}}_{\mathit{r}}$	${\mathit{ve}}_{\mathit{r}}$	${\mathit{me}}_{\mathit{r}}$	${\mathit{ve}}_{\mathit{r}}$	${\mathit{me}}_{\mathit{r}}$	${\mathit{ve}}_{\mathit{r}}$
Undisturbance	0.0927	0.0106	0.0758	0.0071	0.0799	0.0084
0.5 disturbance	0.1366	0.0487	0.0837	0.0098	0.0798	0.0089
1.0 disturbance	-	-	0.1183	0.0189	0.1255	0.0199

. Take the mean value of the absolute value of the tracking error $m{e}_r$ and the variance of the tracking error $v{e}_r$ as the evaluation index; then, the $m{e}_r$ and $v{e}_r$ are as follows:

$$\begin{array}{cc}\hfill e_r& =\sqrt{X_d^2+Y_d^2+Z_d^2}-\sqrt{X^2+Y^2+Z^2}\hfill \\ \hfill m{e}_r& =mean\left(\right|e_r\left|\right)\hfill \\ \hfill v{e}_r& =var\left(e_r\right)\hfill \end{array}$$

(32)

where, $X_d$, $Y_d$, and $Z_d$ are desired trajectories in (31) and X, Y, and Z are the positions of the quadrotor.

It can be seen from Table 3 that the errors of the CSTA and TCSTA are equivalent. When the disturbance increases to $0.5$ times, the absolute error mean value $m{e}_r$ and error variance $v{e}_r$ of the two controllers do not increase significantly. When the disturbance increases to $1.0$ times, $m{e}_r$ and $v{e}_r$ of the CSTA and TCSTA increases significantly. The $m{e}_r$ and $v{e}_r$ of PID is the largest, and the PID controller begins to become unstable when the disturbance increases to 1.0 times.

5. Conclusions

We improve the CSTA controller to suppress the controller chattering, and the designed TCSTA controller is applied to the inner loop of quadrotor control. Trajectory tracking performances of the system are validated by a simulation of attitude control and position control, and the performances of the TCSTA controller are evaluated by the tracking error, phase error, and amplitude–frequency characteristics of the controller output under different external disturbances. The simulation results show that the TCSTA controller can suppress controller chattering effectively and its robustness is equivalent to a CSTA controller.

Control allocation is an important factor affecting the performances of a quadrotor with actuator saturation. In the future, on the basis of the current TCSTA controller application, further improvements will be made in the trajectory tracking performances of a quadrotor with actuator saturation by improving the control allocation.