Stable High-Speed Quadrotor Flight using Differential Trajectory

This paper addresses the trajectory tracking control issue of a quadrotor with environmental disturbances, which particularly focuses on attitude correction during ﬂight and response speed improvement. In this paper, the proposed control method uses the diﬀerential equation to transform the desired trajectory for improving the robustness of a quadrotor during high-speed ﬂight. Meanwhile, a nonlinear controller based on the rotation matrix is designed to avoid singularities of Euler angles. The controller can also stabilize the position error to zero without reducing the control thrust when the attitude error is large. Finally, we conduct simulations on a tethered quadrotor to achieve high-speed ﬂight in limited space. The simulation results show that the control method can stabilize a quadrotor at high-speed trajectory ﬂight and keep a quadrotor stable when it is disturbed by aerodynamic drag and mass change.


Introduction
Vertical-Take-Off and Landing Unmanned Aerial Vehicles (VTOL UAVs) have been widely used in scientific research and industry due to their simple mechanical structure. In recent years, the application of UAVs in inspection and transportation, which needs high robustness of the trajectory track control, has attracted a lot of research interests [1][2][3].
Existing studies are mainly about the robustness of UAVs during low-speed flight. However, transportation and inspection require high flexibility of UAVs to adapt to the complex environment [4,5]. Meanwhile, the flight velocity of UAVs must not be too low (higher than 8 m/s) and UAVs need to maintain stable flight with the disturbance of environmental factors such as aerodynamic drag and mass change [6]. us, the robustness of UAVs during high-speed flight plays a crucial role in practical applications.
is paper selects a quadrotor as the research object for the reason that the quadrotor UAVs have a simple structure, universal algorithm, and high mobility. e major environmental factors on the flight of UAVs are aerodynamic drag and mass change. ere are many studies on the trajectory tracking control of UAVs when facing instantaneous changes in environmental factors during transportation and inspection [7][8][9]. e difficulty of the high-speed stable trajectory control is the rapid correction of the attitude and the improvement of the response speed. Trajectory tracking controllers are often divided into attitude control and position control, which include linear controllers such as linear-quadratic regulator (LQR) [10,11] and nonlinear controllers such as sliding-mode technique [12], backstepping technique [13], and fuzzy-PID controller [14].
Most studies of attitude control are conducted using Euler angles to represent the attitude of a quadrotor. Reference [15] designs a nonlinear robust control strategy with Lyapunov stability proofs for attitude control of the UAV, which is based on the Euler angles representation. ough the Euler angles representation has the advantages of simplicity and intuition, it also has the drawbacks of singularities. What is more, [11] designs the LQR controller by linearizing the quadrotor model which is expressed through Euler angles, but the linearized model also has the disadvantage of not being accurate enough when the attitude movement is large. us, the rotation matrix and quaternion have been applied to the design of attitude control to solve the singularity problem of Euler angles. In [16], quaternion to control attitude is first implemented on the Draganflyer III quadrotor, which shows that the PD controller is globally asymptotically by the Lyapunov analysis. Besides, [17] designs and verifies various classical controllers using quaternion, such as PD controller, LQR, and backstepping technique, while [18] proposes a hierarchical control method combining the backstepping technique and H ∞ theory for a nonlinear model based on quaternion, which proves to be effective. Rotation matrix representation is used for attitude control of UAVs trajectory tracking in [19]. And [20] provides a novel rotation matrix based control method, which is shown to be exponentially stable by defining an error function for attitude and angular velocity on SE (3). As an extension of [20], a cascaded PID attitude controller for trajectory tracking control is designed by analysing the nonlinear model of quadrotor in [21]. Yu et al. [21] define the logarithmic operation to transform rotation matrix from SO (3) to so (3) through Li Group and Li Algebra theory, which can solve the problem that rotation matrix is with too many parameters. is control method can achieve a fast response of quadrotor attitude control. Although both rotation matrix and quaternion to attitude control can perform complex trajectory tracking tasks, the quaternion is overly complex to compute. To the authors' best knowledge, no research uses a rotation matrix based controller to deals with high-speed trajectory tracking issue.
Improving the response speed of the position control system is another significant issue for quadrotor high-speed trajectory tracking. Since a quadrotor is an underactuated system, there is no way for the quadrotor to completely track the desired trajectory (including position and attitude). Attitude control and position control affect each other. To attenuate this association, [22] designs an image-based visual servo controller for the pinpoint landing task with a small-scale attitude change. In [23], a passification-based adaptive controller is presented for stabilization in altitude. But simple flight strategies with small attitude movements also limit the application of quadrotors in complex environments. A control algorithm proposed in [20] reduces control thrust when attitude error becomes larger. is method can address the problem of application being limited, while quadrotors are also slower to recover from disturbances. So, it is not available for high-speed trajectory tracking control. Actually, [24] develops a trajectory tracking controller for thrust-vectoring UAVs, which can change the direction of control thrust to achieve partial decoupling of position and attitude. But the structure of thrust-vectoring UAVs is too complex to be widely applied.
In this paper, we propose a control method based on the differential trajectory to improve the response speed of quadrotor trajectory tracking control. Inspired by [25], we achieve the high-speed flight of a quadrotor in a limited space with the help of tether, but the dynamics model of [25] is too complex and the LQR controller of [25] requires a high-accuracy model for high-speed trajectory tracking. erefore, we perform a differential operation on the desired trajectory so that the desired velocity at each point in the inertial frame converges toward the desired trajectory. Our position controller uses the desired velocity of the differential trajectory as a feedforward term to design the position control algorithm, which can follow the desired trajectory without reducing control thrust when the attitude deviates from the desired attitude. To address the disadvantage of the rotation matrix with too many parameters, this paper presents the adaptive nonlinear attitude control law combining the rotation matrix and the Rodrigues' formula. is control method not only avoids the singularities of Euler angles but also has only three variables of attitude. e stability of this control system can be demonstrated by Lyapunov's second method of stability. Finally, the simulation results show that the control method has good robustness in high-speed flight and low-speed flight. e novelty of our work compared with existing research is as follows: (1) A control method for quadrotor based on the differential trajectory is proposed, which can guarantee the convergence of the position error without reducing the control thrust when the attitude greatly deviates from the desired attitude.
(2) e design of the controller takes into account the existence of environmental factors, demonstrating that the nonlinear control strategy combining differential trajectory and rotation matrix can improve the robustness of the quadrotor for high-speed flight.
is paper is structured as follows. e nonlinear model of the quadrotor is developed in Section 2. e detail about the proposed controller is described in Section 3. Finally, the simulation results are discussed in Section 4.

System Model
is section introduces the dynamic model of a quadrotor, the tether model, and the aerodynamic drag model. A quadrotor is a VTOL UAV driven by four motors and propellers. As shown in Figure 1, the structure of the quadrotor used in this paper is "+" shape. We assume that the center of mass and the origin of the body frame are at the same point. e directions of the inertial frame and the body frame are shown in Figure 1. e state of our quadrotor system can be represented by the position, velocity, attitude, and angular velocity. e control input for the system is the total thrust and the moment generated by propellers. In this paper, the rotation matrix is used to represent the attitude of the quadrotor. So we define: m ∈ R, the mass of quadrotor. J ∈ R 3×3 , the inertial matrix in the body frame. R ∈ R 3×3 , the rotation matrix from the body frame to the inertial frame. ω ∈ R 3 , the angular velocity in the body frame.
x ∈ R 3 , the position of the body frame in the inertial frame. v ∈ R 3 , the velocity of the body frame in the inertial frame. f ∈ R, the total thrust generated by four propellers. τ ∈ R 3 , the moment generated by propellers in the body frame. g ∈ R, the acceleration of gravity. ω i ∈ R, the rotational speed of the i -th motor. d ∈ R, the arm's length of the quadrotor. e quadrotor dynamic model is as where e 3 � 0 0 1 T is a unit vector and the definition of ∧ operation is as follows: Define ω � ω 1 ω 2 ω 3 T ; then, where the inverse of ∧ operation is ∨: Assuming the origin of the body frame and the origin of the inertial frame coincide, the coordinate of the point q in the body frame is q b and the coordinate of the point q in the inertial frame is q e ; the rotation matrix from the body frame to the inertial frame satisfies e rotation matrix R eb is a 3×3 orthogonal matrix, where x eb , y eb , and z eb , respectively, represent the x-axis, yaxis, and z-axis direction of the body frame in the inertial frame.
Generally, only the force generated by the propeller and gravity are taken into account when analysing the dynamic model of a quadrotor, but a quadrotor is also affected by aerodynamic drag during flight. erefore, this paper also considers aerodynamic drag to verify the robustness of the control method. e model of aerodynamic drag selected from [25] is as follows: where e v is the direction of the velocity in the inertial frame. We use a tether to conduct the performance evaluation of the controller in high-speed flight [25]. e tether provides the centripetal force for the quadrotor to follow a circle trajectory in a limited space. So, it is also necessary to consider the force generated by the tether. It is assumed that the suspension point of the tether at this quadrotor coincides with the center of mass and the tether is no-elastic, tensioned, and massless. From the law of centripetal force and Newton's second law, the dynamic model of tether force is in which f t is the magnitude of the tether force, p t is the direction of the tether force, and l t � |x| is the length of the tether: Finally, the thrust and moments model of our quadrotor is given. e input is the rotational speed of the four motors, and the output is the total thrust and the moment of the quadrotor: where c T and c M is constant.

Flight Trajectory Control Design
is section shows the design of the controller that allows a quadrotor to track the desired trajectory steadily with disturbance at high speed. e trajectory controller is divided into three parts including trajectory differentiation, attitude control, and position control in this paper. ere are the hypotheses for the controller that (a) the measurements of the position, velocity, attitude, and angular velocity can be obtained by the sensor at a sufficiently high frequency, and (b) the error of the measurements is small enough to be neglected. e structure diagram of the controller is shown in Figure 2. e input of the controller is the desired trajectory, including the desired position x d and the x-axis direction of the body frame b 1d . e output is the total thrust and the moment of this quadrotor. en, the thrust and the moments can be converted into the rotational speed of the four motors by the control allocation, which is the inverse operation of the thrust and moments model. e control allocation is as follows: 3.1. Trajectory Differentiation. Since our goal is to improve the robustness of a quadrotor, this section starts by introducing the design of the trajectory differentiation which guarantees the convergence of the desired velocity towards the desired trajectory. e input of the trajectory differentiation is the desired trajectory of the quadrotor and the output is the desired velocity in the inertial frame. For a certain point, the desired velocity of that point output by the trajectory differentiation converges to the desired trajectory so that the position controller only needs to get a quadrotor flown stably to the desired velocity. erefore, the design of the position controller is simplified and the performance of the position control is improved. e structure of the trajectory differential is shown in Figure 3. First, the desired position trajectory is given as e control period of the quadrotor is defined as dt. For a certain point x in the inertial frame, find the point x d (t 1 ) closest to this point x in the desired trajectory. us, the desired velocity at this point x is as follows: rough the trajectory differentiation, the desired velocity of every point in the inertial frame can be obtained, and the desired velocity converges toward the desired trajectory.
is can keep a quadrotor stable in the face of disturbance and accomplishes a quadrotor to fly stably at high speed. It also does not need to reduce control thrust to prevent the divergence of position error when the attitude error is large which means the response speed and the recovery speed after the disturbance will be faster.

Attitude Control Design.
Attitude control is a very important part of trajectory tracking control. Most attitude control methods are designed based on Euler angles because it is simple to express the attitude of a quadrotor with Euler angles. What is more, when the attitude change is small, the change rate of Euler angles is approximately equal to the angular velocity of a quadrotor, which simplifies the control model of a quadrotor [10][11][12][13]. But this Euler angles representation also exhibits singularities and is not intuitive. erefore, this paper represents the attitude of a quadrotor by a rotation matrix.
Since each column vector of the rotation matrix indicates the direction of the body frame xyz axis in the inertial frame, it is more intuitive to express the attitude compared to Euler angles and quaternion. And the attitude trajectory of the quadrotor is smooth. Although the rotation matrix has nine parameters, a rotation matrix is orthogonal with only three degrees of freedom. erefore, the rotation matrix can be represented with three variables through Rodrigues' formula to solve the problem of too many parameters.
Firstly, the desired attitude rotation matrix is selected as where b 3d represents the desired z-axis direction of the body frame output by the position controller and b 1d is the desired orientation of the quadrotor head: As the main goal of the attitude control is the convergence of the attitude error, we selected the error rotation matrix as where R c is the rotation matrix of the current quadrotor attitude. e error rotation matrix can be converted into a unit rotation axis ω ∈ R 3 and a rotation angle θ ∈ [0，2π) using the Rodrigues' formula. Because this transformation is a one-to-one correspondence, the singularities of Euler angles is avoided. e Rodrigues' formula is as follows: R � e ωθ � I + ωsinθ + ω 2 (1 − cosθ). (13) us, the error rotation matrix is converted into a vector e R ∈ R 3 using equation (12) and ∨ operation as en, the angular velocity error of the quadrotor is defined as e ω � ω d − ω. (15) erefore, the output of the attitude controller is selected as [21] τ � J _ ω d + ω × Jω, (16) in which _ ω d � k ω e ω , ω d � k R e R , and k ω , k R ∈ R are positive constant.
Finally, considering equations (14) and (15), the output of the quadrotor attitude controller is 4 Mathematical Problems in Engineering e stability of the attitude control method can be proved by Lyapunov's second method of stability. e Lyapunov function is chosen as It is shown in [21] that the attitude control system is stable.

Position Control Design.
e design of the position controller aims to guarantee the convergence of the position error. Since the quadrotor is an underactuated system, the position control is affected by the attitude control. When the attitude error is large, the position error is easy to diverge. erefore, [20] proposes the control strategy that reduces the control thrust of the quadrotor when there is a large attitude error. Although this can prevent the position error from diverging, the error convergence speed also decreases. However, the position error of our control system can be stably converged to zero without reducing control thrust even if the attitude deviates from the desired trajectory. at is because the desired velocity output by the trajectory differentiation converges toward the desired trajectory. e position error and the velocity error of the system are defined as where x is the current position of the quadrotor, v is the current velocity of the quadrotor, x d is the current desired position, and v d is the desired velocity of the current position. erefore, the quadrotor position controller outputs the desired total thrust f d and the desired z-axis direction of the body frame b 3d as where the desired acceleration of the quadrotor is a d � k v e v (k v ∈ R) and k v is constant. We can prove the stability of the position controller by selecting the Lyapunov function as en, the derivative of this function is where m_ e v is Assuming that the attitude error of the quadrotor is zero, we can get en, considering equations (22) and (24), is means that when ‖ e v ‖ ≥ ‖ (m _ v d − f a ) ‖ /mk v , then _ V 2 ≤ 0 and ‖ e v ‖ decreases. erefore, the position controller can control the velocity error within a finite range: Figure 3: Trajectory differentiation.

Position control
Attitude control

Mathematical Problems in Engineering
us, by choosing a larger gain k v , the position error can converge to zero. e control thrust and the desired z-axis direction of the body frame not only improve the robustness of a quadrotor when it is disturbed but also stabilize a quadrotor to accelerate to the desired velocity during highspeed flight.

Simulation Results and Analysis
We verify the robustness of the controller proposed in the previous section. e parameters of the quadrotor are m � 4 kg, en, the controller parameters are as follows: k ω � 4.8, e simulation is conducted in the following three cases. e first case is the low-speed flight in which we perform the performance evaluation of the trajectory tracking controller proposed in this paper, the PID controller, and the geometric tracking controller proposed in [20]. e desired trajectory and the initial conditions are as follows: e second case is the high-speed flight. We verify the robustness of our control method at high speed under three conditions: (a) disturbance-free condition, (b) aerodynamic drag disturbance, and (c) mass change condition. e desired trajectory and the initial conditions are as follows: e parameters of aerodynamic drag are selected as Using the method of [20], the simulation of the last case verifies the performance of our trajectory tracking controller and the geometric tracking controller proposed in [20] from being upside down initial attitude. e desired trajectory and the initial conditions are as follows: e simulation results of the first case are shown in Figures 4-6. In Figure 4, the attitude error of our trajectory controller and the geometry tracking controller rapidly reduces to (0,0.05) rad within 1 s and converge to 0 rad within 2 s. During convergence, the attitude error response curve of our controller is smoother with only a small oscillation compared to the geometry tracking controller. e attitude error response of the PID controller has an oscillation of decreasing amplitude in 0∼4 s and finally remains stable at (0.01,0.02) rad. In Figure 5, the z-axis position of the trajectory tracking controller can converge to 0 m within 2 s. It shows a faster response than the other two controllers. But the position response of our controller has a little delay relative to the desired trajectory, and this is because the desired velocity of the differential trajectory is determined by the current position of the quadrotor rather than the current time. Although this property of differential trajectory causes a small delay in the position response, it can also be prevented from the divergence of position error when the position error is large during the high-speed flight. And the position response latency of the trajectory tracking controller is less than 1 s, which is smaller than that of the PID controller. In Figure 6, the linear velocity of our controller can stabilize at the desired linear velocity. is indicates our trajectory tracking controller can converge to the desired trajectory within 3 s. Compared to our controller, the linear velocity of the PID controller has a slower response and larger steady-state error, and still it oscillates when stable, while geometric tracking control also has larger oscillation and longer adjustment time. us, our control algorithm performs much better than the other two controllers in both position and attitude control. e simulation results of the second case are illustrated in Figures 7-9. In Figure 7, the trajectory tracking controller can achieve that the quadrotor steadily accelerates to the desired velocity of 10.5 m/s. And the position error of the zaxis is stabilized at 0 m in 4 s. Moreover, the attitude error can also quickly reduce to (0,0.1) rad in 1 s and converge to 0 in 8 s. In Figure 8, the linear velocity is stabilized in the range of (7,8) m/s and aerodynamic drag causes periodic oscillations of attitude error whose amplitude is also kept within (0,0.1). Although the aerodynamic drag reduces the velocity of the quadrotor, the trajectory tracking controller still follows the desired path. In Figure 9, the z-axis position error increases when we increase the mass of the quadrotor to 6 kg at 5 s. But the position error is eventually stabilized at (0,0.2) m. e mass change has almost no effect on the attitude and velocity response. erefore, the controller proposed in this paper can effectively improve the robustness of the quadrotor in high-speed flight. e simulation results for the third case are presented in Figures 10 and 11. In Figure 10, both the attitude error and position error can converge rapidly to 0 in 2 s, and the maximum overshoot of the position error is below 0.08 m. In Figure 11, the attitude error of the geometric track controller can converge rapidly to 0 within 1 s, but the adjustment time of the position error is over 5 s. And the maximum overshoot of the position error is over 0.09 m. erefore, taking into account the dynamic performance under the presence of an external disturbance force, the trajectory controller proposed in this paper is also superior to the geometric controller proposed in [20].     Mathematical Problems in Engineering 9

Conclusions
In this paper, we proposed a trajectory tracking controller of VTOL UAVs to stabilize a quadrotor at a high-speed trajectory. First, the concept of the differential trajectory is exploited to compute the desired velocity which is toward the desired trajectory. e differential trajectory enhances the antidisturbance ability and the response speed of the position controller. en, the proportional-integral control method is used to guarantee that the attitude and position follow a high-speed trajectory with large attitude movements. Finally, the simulation is conducted by the tethered quadrotor to verify that the controller can improve the robustness of the quadrotor against aerodynamic drag and mass change in high-speed flight. Future work is to implement control algorithms on multirotor racing, improving the robustness and disturbance rejection.

Data Availability
e simulation flight data used to support the findings of this study are included within the article.

Conflicts of Interest
e authors declare that there are no conflicts of interest regarding the publication of this paper.