A Robust Inner and Outer Loop Control Method for Trajectory Tracking of a Quadrotor.

In order to achieve the complicated trajectory tracking of quadrotor, a geometric inner and outer loop control scheme is presented. The outer loop generates the desired rotation matrix for the inner loop. To improve the response speed and robustness, a geometric SMC controller is designed for the inner loop. The outer loop is also designed via sliding mode control (SMC). By Lyapunov theory and cascade theory, the closed-loop system stability is guaranteed. Next, the tracking performance is validated by tracking three representative trajectories. Then, the robustness of the proposed control method is illustrated by trajectory tracking in presence of model uncertainty and disturbances. Subsequently, experiments are carried out to verify the method. In the experiment, ultra wideband (UWB) is used for indoor positioning. Extended Kalman Filter (EKF) is used for fusing inertial measurement unit (IMU) and UWB measurements. The experimental results show the feasibility of the designed controller in practice. The comparative experiments with PD and PD loop demonstrate the robustness of the proposed control method.

1. Introduction

A quadrotor is a kind of unmanned aerial vehicle which consists of four fixed-pitch propellers attached to motors mounted in a square crossing rack. Due to its special structure, a quadrotor is capable of vertical takeoff and landing, as well as hovering. High maneuverability and flexibility make it possible for special applications such as search missions, monitoring and anti-poaching missions [1,2].

In order to realize autonomous quadrotor flight, a variety of control methods have been developed to realize trajectory tracking. Bouabdallah modeled the quadrotor with Euler-lagrange formalism and Euler-Newton formalism [1]. Some linear techniques (PID, linear quadratic regulator (LQR)) and nonlinear techniques (backstepping, sliding mode control and fuzzy logic control) have been applied to the control of quadrotor on this basis [3,4]. Besides, He transformed the nonlinear model to a linear model and proposed an internal model control method to realize the trajectory tracking [5]. Nicol et al. proposed a neural network controller for robust trajectory tracking of a quadrotor [6]. González-Vázquez et al. introduced the theory of singularly perturbed systems to the design of trajectory tracking [7]. However, all these approaches are based on Euler angles. The attitude model is obtained on the assumption that roll and pitch are close to zero. Tracking complicated trajectories becomes a challenge for the quadrotor. Tayebi et al. proposed a quaternion-based feedback control strategy for attitude stabilization of a quadrotor as an improvement [8,9,10]. The quaternion is double-covering to the rotation matrix. In order to avoid the singularity and redundancy from Euler angles and quaternion, Lee et al. developed a geometric tracking controller on the special Euclidean group. Results showed the hybrid controller can implement complicated acrobatic maneuvers [11,12,13]. The geometric control method provides a unique perspective to solve the control problem and can avoid the singularity and complexity when using local coordinate [14]. The method is feasible for tracking complicated trajectory.

Xu et al. proposed a sliding mode control approach to stabilize the under-actuated subsystem (UAS) [15]. Xiong et al. separated the quadrotor system into UAS and fully actuated subsystem (FAS) and designed a control algorithm for FAS and UAS [16]. Meanwhile, most researches separated the system into two cascaded loops. The outer loop governed translational motion and the inner loop stabilized the attitude. The inner-outer loop structure has a more explicit meaning and is easier to design and to tune [17,18].

The modeling inaccuracy and external disturbances would give rise to another challenge for accurate tracking trajectory of the quadrotor [19]. SMC has been developed to compensate for parameter uncertainty and bounded disturbances in many applications [20]. However, the chattering effect will be introduced into a system while implementing the SMC. The simplest way to reduce chattering is to replace the sign function with continuous functions such as saturation and hyperbolic tangent function [21]. In addition, high order sliding mode (HOSM) techniques have been proposed to reduce chattering and improve the control performance. The HOSM techniques for a relative degree r system cannot reduce the chattering substantially [22]. Then continuous terminal sliding mode control is presented to use on a relative degree r system while the chattering is entirely eliminated [23,24]. Considering that chattering effects can be reduced or even eliminated, SMC outperforms traditional control methods in terms of convergence rate and robustness in the presence of model uncertainty and disturbances.

In our work, we utilize an inner-outer loop structure. The outer loop governs the translational motion and generated reference rotation matrix for the inner loop together with the yaw angle. For the inner loop, the SMC law based on rotation matrix is designed, which frees the quadrotor from the constraints of small-angle flight. The outer loop is designed through SMC. The usage of SMC speeds up the response speed and improves the robustness of the system. The effects of parameters on the system have been studied. The effectiveness of the proposed controller is verified by tracking three representative trajectories. Comparative simulations in two cases are conducted to validate the robustness of the proposed controller. The proposed controller is also validated experimentally.

The paper is organized as follows: the next section describes the dynamic model of the quadrotor with disturbances. In Section 3, we develop a position controller as well as an attitude controller to achieve trajectory tracking. In Section 4, simulations are conducted to verify the effectiveness of the designed controllers. In Section 5, an experiment is carried out to verify the proposed method. In Section 6, conclusions are presented.

2. Dynamic Model

A quadrotor can be regarded as a rigid body. Rigid motion consists of rotation motion and translation motion. Consider the world frame W, as well as the body frame illustrated in Figure 1, the posture of the quadrotor can be described by the relative orientation between the world frame and the body frame. The rotation matrix from the body frame to the world frame is utilized to show the posture of quadrotor. Meanwhile, position and velocity in the world frame is denotes by , , respectively. The translation and rotation of the quadrotor satisfy: where denotes the angular velocity of quadrotor in the body frame, represents the hat map from to its skew-symmetric matrix.

Figure 1

The structure of quadrotor and its coordinate system.

According to Newton-Euler formalism, the translational motion of the center of quadrotor in the world frame and the rotational motion of quadrotor in the body frame are as follows: where m is the mass of the quadrotor. is the moment of inertia. denotes net force vector in the world frame and denotes the moment vector in the body frame.

Based on the mechanical analysis, the quadrotor is subject to gravity and thrust. When the quadrotor is in actual flight, it is also influenced by drag. Due to the special structure of the quadrotor, the thrust is along axis of the body, the amplitude of thrust is denoted by T. Therefore, the net force acting on the quadrotor in world frame is given by: where , is the i-th thrust. represents the drag coefficient matrix. .

When ignoring body and propeller gyro effects, the moment vector M can be written as: where represents the i-th counter torque, represents the distance from the axis of rotation of motors to the center of quadrotor.

In order to express easily in the simulation, the intermediate variables are introduced to represent as:

3. Trajectory Tracking Control

From the analysis above, we can derive that the system is a highly-coupled, under-actuated and nonlinear system and can be divided into two subsystems: an attitude system and a position system. Meanwhile, we can note that the rotational motion is independent of the position, but the translational motion is dependent on the rotational motion. From Equation (3), the z axis of the body can be determined once the position of the quadrotor is known. Besides, provided that the yaw angle is given, the attitude of the body can be uniquely determined. In other words, the motion of the quadrotor can be obtained once the position and yaw angle are given. The overall control structure diagram is shown in Figure 2.

Figure 2

Control structure diagram.

3.1. Trajectory Tracking Control

First, we define and as the desired trajectory. Now we define the position error and velocity error as:

Next, let’s consider the switching function:

Then we design the control law: where and are positive diagonal matrices.

Differentiating Equation (7) and substituting Equations (3) and (6), then:

Equating Equation (8) with Equation (9), the desired thrust T can be derived as follows: where is inner product for all .

Meanwhile, the direction of thrust should be:

Provided that the desired position is given, the thrust is designed as Equations (10) and (11). Then the system stabilizes the zero equilibrium of position error exponentially after a finite time.

In order to verify the designed control law can guarantee the stability of the system, we select the Lyapunov function candidate , differentiating and substituting Equation (8):

can be proved while and are positive diagonal matrices. Therefore, the system is stable and the sliding surface converges to zero exponentially. The stability of outer loop is proved. ☐

Since , will exponentially converge to zero after a finite time. In other words, the actual position will converge to the desired position. Here the convergence rate is determined by . In order to avoid chattering, the sign function is replaced by a saturation function.

Suppose the yaw angle is given, and can be obtained when the inertial frame rotates to the body frame following the sequence of . Given the yaw angle , the rotation matrix expressed in the form of coordinate axis can be written as: where , the singularity where is parallel to may exist only when the pitch angle or roll angle reaches to 90°. The probability of such a situation is small. In our work, we suppose that we will not encounter the singularity.

3.2. Attitude Control

Attitude control is the core of the overall control system. It keeps the orientation of the quadrotor to the desired values precisely. We define the attitude error as follows:

The attitude error represents the relative rotation from the body frame to the reference body frame. Based on the attitude error, we further define angular velocity error as:

Differentiating Equations (14) and (15) and combining the dynamic equation of rotation equation of quadrotor, the dynamic equation of attitude error and angular velocity error can be obtained as:

The control objective to make rotation matrix converge to its desired value is equivalent to make converges to identity matrix . Before the design of control law, one lemma is introduced to describe rotation matrix [25].

According to the Euler’s Rotation Theorem, every rotation matrix is equivalent to a rotation about a fixed axis through an angle . Specifically, the rotation matrix can be represented as:

The associated inverse map of Equation (18) is the logarithmic map, which is expressed as:

When converges to the identity matrix, converges to zero. From (19), we can deduce is mainly determined by the skew-symmetric matrix .

According to the analysis above, we define vector form of attitude error as: where is positive constant.

Then, we define sliding mode surface based on and as follows:

Next, we design the control law rendering satisfies: where , , are positive diagonal matrixes.

Differentiating Equation (20) and the first time derivative of yields [11]:

Differentiating with respect to time and calling Equations (17) and (23):

Equating Equation (22) with Equation (24), the moment vector can be obtained as:

Similar to stability proof of position loop, the stability of system can be easily proved. By Equation (21), when , we have:

By Equations (23) and (26), we have the following equation:

Then will exponentially converge to zero. Since converge to zero, it follows that will converge to zero according to Equation (26).

The inner-outer loop structure makes the controllers easy to design and to tune. However, closed-loop system stability can’t be guaranteed due to coupling between the inner and outer loop. The closed-loop stability analysis of a system in cascade has been studied [26]. The global stability analysis theorems of a quadrotor based on the modified Rodrigues parameters (MRPs) error model have been proposed in [27]. Theorem 1 is adopted to demonstrate the global stability of proposed control scheme.

Consider the quadrotor error model in [27], if Assumption A1–A3 are satisfied, then the closed-loop system is globally stable.

A1. The control inputs and stabilize the outer loop exponentially.

A2. The control input stabilizes the inner loop exponentially.

A3. The control input satisfies the condition , where and are positive constants, is general error of the outer loop.

Combining the translational dynamics and Equations (16) and (17), the error model based on the rotation matrix of the quadrotor can be expressed as:

The MRPs is another attitude representation. The attitude error is the corresponding attitude matrix of error of MRPs. The error of MRPs converges to zero only when converges to the identity matrix. The dynamics characteristics of the outer and inner loop are basically the same. The coupling term between the inner and outer loop is , which is the same as that in [27]. Therefore, Theorem 1 can be used to prove the closed-loop stability of the proposed control scheme.

From the stability analysis of inner and outer loop, we know the proposed controllers can stabilize two loops exponentially. Based on Equation (10), the control input can be rewritten as:

Using the properties of vector norm, satisfies: where is a positive diagonal matrix from the above definition. Thus, can be expressed by where , and are positive numbers. Based on the definition of sign function, . It is obvious that is bounded with . The desired acceleration is bounded with . The actual velocity is also bounded, hence . Where , and are positive constants. We render satisfies:

Subsequently, we have:

Besides, the square of satisfies:

Therefore, we have the following equation:

The control input satisfies the assumption A3 from Equation (34). The designed controllers can stabilize the inner loop and outer loop exponentially after a finite time based on the above stability proof of the dual loop, respectively. Above all, the designed controllers satisfy all assumptions in a finite time in Theorem 1. Thus, the closed-loop stability of the system is proved.

4. Simulations

In this section, the simulations are performed on Matlab/Simulink in order to test the validity of the proposed controller. The quadrotor model parameters are listed in Table 1.

Table 1

Quadrotor model parameters.

Name	Variable	Value	Units
Mass	M	2.0	kg
Arm length	L	0.20	m
Inertia on x axis	I_xx	1.25	Ns^2/rad
Inertia on y axis	I_yy	1.25	Ns^2/rad
Inertia on z axis	I_zz	2.5	Ns^2/rad
Drag coefficients	K_1=K_2=K_3	0.012	Ns/m

4.1. Parameter Selection

Heuristic method is used to select control parameters of SMC. Considering similar control laws are adopted in the inner and outer loop, we take the inner loop as an example. The controller is required to track the following rotation matrix:

Firstly, the parameters are required to be positive to guarantee the stability of the system according to design guidelines. The effects of each parameter on the system are investigated with four sets of parameters presented in Table 2. The simulation results are shown in Figure 3. Figure 3a–c depict the vector form of attitude error and Figure 3d–f depict the control inputs.

Table 2

The control parameters of the inner loop.

	β_1	β_2	β_3	λ_1	λ_2	λ_3	γ_1	γ_2	γ_3
Case 1	10	10	10	1	1	1	0.1	0.1	0.1
Case 2	10	10	10	5	5	5	0.1	0.1	0.1
Case 3	10	10	10	1	1	1	1	1	1
Case 4	12	12	12	1	1	1	0.1	0.1	0.1

Figure 3

Tracking rotation matrix simulation (a) ; (b) ; (c) ; (d) The control input ; (e) The control input ; (f) The control input .

The quality of system is further evaluated by integral absolute error (IAE), integral squared error (ISE) and measured integral squared input (ISCI) performance index, respectively. The performance indices are summarized in Table 3.

Table 3

Comparison of performance of SMC with different sets of parameters.

	IAE			ISE			ISCI
	e_R_1	e_R_2	e_R_3	e_R_1	e_R_2	e_R_3	u_2	u_3	u_4
Case 1	0.9682	0.3469	1.0215	0.4407	0.0845	0.3788	0.3028	0.3809	38.9353
Case 2	0.6981	0.3435	0.6408	0.2748	0.1024	0.2152	0.0275	0.1890	38.6488
Case 3	0.7641	0.2911	0.7862	0.3699	0.0791	0.3035	0.2422	0.3760	38.8503
Case 4	1.2993	0.4847	1.3257	0.5999	0.1073	0.5238	0.5210	0.4912	39.3407

It is obvious that affects the convergence rate of the system from Equation (21). We can see from Case 1 and Case 4 that the transient response deteriorates and control inputs increase when increases. Comparing Case 1 and Case 2, the IAE, ISE and ISCI decreases when increases. is the gain of the sign function. The increase in makes the system performance gets better with less control inputs from Case 1 and Case 3. In conclusion, Case 2 is the most appropriate one in terms of control performance and control inputs.

4.2. Trajectory Tracking Simulation

4.2.1. Trajectory Tracking

In order to verify that the quadrotor can achieve trajectory tracking, three representative trajectories are simulated, respectively. The first reference trajectory is and the desired yaw angle is . The initial state of the quadrotor is set to be and . The simulation results of helix tracking are shown in Figure 4.

Figure 4

Tracking helical trajectory simulation (a) Position; (b) Error of position; (c) Error of angular velocity; (d)Vector form of attitude error; (e) The sliding variables in outer loop; (f) The sliding variables in inner loop; (g) The control variables; (h) The desired trajectory and the actual trajectory in the simulation.

From Figure 4a,b, we can observe that and converge to their desired values without oscillation in 4 s, converges to its desired value after 4 s later although there exists a small fluctuation. Figure 4c,d depict the errors of angular velocity and errors of vector form of attitude error . Figure 4c demonstrates the attitude controller can track the desired angular velocities. As shown in Figure 4d, converges to zero within about 4s, which means the rotation matrix reaches the desired rotation matrix. Figure 4e,f display the behaviors of the sliding variables in the outer loop and inner loop. Obviously, all the sliding variables converge to their sliding surfaces within 4 s. Figure 4g shows the control inputs, whereby, converges to its steady state after several seconds. , and converge to zero after an oscillation within 0.5 s. Figure 4h depicts the the 3-dimensional trajectory. The results demonstrate the quadrotor can realize trajectory tracking.

In order to indicate that the quadrotor can track a nontrival trajectory, an edge-on circle is chosen as another reference trajectory. Besides, the desired yaw angle is 0. The initial state of the quadrotor is set to be and . Figure 5 shows the simulation results.

Figure 5

Tracking circular trajectory simulation (a) Position; (b) Error of position; (c) Error of angular velocity; (d) Vector form attitude error; (e) The sliding variables in outer loop; (f) The sliding variables in inner loop; (g) The control variables; (h) The desired trajectory and the actual trajectory in the simulation.

We can see from Figure 5a,b that the position can converge to its desired values soon. Figure 5c,d are the error of angular velocity and vector form of attitude error . The great changes of angular velocity occur when the attitudes of quadrotor changes greatly. Figure 5e,f show the sliding surfaces of the outer loop and inner loop. The sliding variables of the position loop reach the sliding mode surfaces smoothly, whereas, the sliding variables of the attitude loop have a similar variation trend to that of the error of angular velocity. Figure 5g depicts the corresponding control inputs. We can observe that periodically changes as time changes, , and converge to zero soon. The great changes of moment vector occur only when the attitude changes greatly. Figure 5h depicts the desired trajectory and the actual trajectory. From Figure 5h, we can see the controller can track trajectories with large Euler angles.

A rectangle is a common trajectory and is difficult to track due to the abrupt direction changes around the four corners. The third reference trajectory is chosen as:

Besides, the desired yaw is . The initial state of the quadrotor is set to be and . Figure 6 depicts the simulation results.

Figure 6

Tracking rectangular trajectory simulation (a) Position; (b) Error of position; (c) Error of angular velocity; (d)Vector form of attitude error; (e) The sliding variables in outer loop; (f) The sliding variables in inner loop; (g) The control variables; (h) The desired trajectory and the actual trajectory in the simulation.

As shown in Figure 6, the proposed control scheme can track the desired trajectory accurately after a few seconds. We can observe the position converges to its desired values soon from Figure 6a. The tracking errors reach maximum around the corner and the errors are controlled within 5 cm from Figure 6b. Figure 6c,d depict the error of angular velocity and vector form of attitude error . As shown in Figure 6c, the errors of angular velocity monotonously reduce to zero except for jumps around the corners. The vector form of the attitude error shows similar characteristics to that of error of angular velocity. Figure 6e,f display the behaviors of the sliding variables of the outer loop and inner loop, respectively. All the sliding variables can converge to zero soon after transient transitions around the vertex of the rectangle. Figure 6g shows the control inputs. At the vertex of the rectangle, large control inputs in a short time are needed. After the vertex, converges to its steady state value, , and converge to zero. The actual trajectory can track the ideal trajectory well from Figure 6h. Above all, the tracking helix demonstrates that the proposed controller can realize basic trajectory tracking. Tracking a circle and a rectangle in three-dimensional space verifies that the proposed control scheme can relieve from small angle approximation and can realize complicated flights.

4.2.2. Trajectory Tracking in Presence of Model Uncertainty and Disturbances

In order to verify the proposed control scheme is robust to parameter uncertainty and immune to bounded disturbances, two corresponding simulation experiments have been carried out [28]. As a contrast, PD and PD control combination is simulated under the same conditions [11]. In the first simulation (Case 1), we induce and () to the quadrotor at time and , respectively. In the second simulation (Case 2), bounded disturbance is added to the inner loop at time and is added to the outer loop at (Case 2). The simulation results are given in Figure 7.

Figure 7

Comparisons under both controllers for trajectory tracking simulation in two cases (a) The trajectory in Case 1; (b) The trajectory in Case 2; (c) The control inputs in Case 1; (d) The control inputs in Case 2.

Figure 7a,c depict the trajectory and control inputs in Case 1. Both controllers have a good tracking performance, even in presence of model uncertainty from Figure 7a. As shown in Figure 7c, when additional mass is added to the system at time , under both controllers increases to the same value to compensate for the change of model. When the moment of inertia changes at time , and adjust to new values to cancel the effects of changes of moment of inertia. Figure 7b,d depict the trajectory and control inputs in Case 2. Figure 7b shows the tracking performance is not influenced by disturbances. The disturbances occurring at time and are both rejected by rapid adjustments of control inputs from Figure 7d. In addition, MAE, IAE and ISE are utilized to evaluate the performance of control method. Table 4 shows the quantitative values.

Table 4

Comparisons for two control schemes in two cases.

	Index	x			y			z
Cases		MAE	IAE	ISE	MAE	IAE	ISE	MAE	IAE	ISE
Case 1	PD + PD	1	1.2819	0.6831	0.5441	0.8947	0.2843	1.0052	1.1981	0.6336
	SMC + SMC	1	1.0367	0.6080	0.2608	0.3024	0.0535	1.0017	1.0085	0.5592
Case 2	PD + PD	1	1.6168	0.7109	0.5465	1.2783	0.3123	1.0052	1.4237	0.6357
	SMC + SMC	1	1.1433	0.6104	0.2607	0.4089	0.0541	1.0017	1.0469	0.5601

As shown in Table 4, MAE along the direction occurs in the initial state and MAE with the help of both controllers is the same in Case 1. MAE along the other two directions, IAE and ISE are also less than those of PD and PD combination. It can be found that the combination of SMC and SMC has less tracking errors. While adding external disturbances to the system in Case 2, the tracking results are similar to the results in Case 1. The MAE along the direction is the same and other performance indices are less than those of PD and PD combination. In sum, the proposed controller shows a better tracking performance in two cases, which verifies the robustness of the designed control scheme in presence of model uncertainty and disturbances.

5. Experimental Results

5.1. Experimental Setup

The validity of the proposed method is verified using the system in Figure 8. The IMU provides angular speed, acceleration and magnetic field information for the system to estimate the pose of the quadrotor. The specifications of sensors used are listed in Table 5. The UWB system is used to provide position with an accuracy within 10 cm, which is transmitted to flight control unit via a serial port. The flight control unit consists of two embedded processors. The high level STM32F427processor (STMicroelectronics, Geneva, Switzerland) receives commands from the ground station, runs the control procedure and gives the pwm signals to motors. The low level processor STM32F103 (STMicroelectronics, Geneva, Switzerland) gives the measurement information to the STM32F427. Besides, various information including attitude angles, position and sensor information is transmitted to base station to display by wireless data transmission module 433.

Figure 8

The experimental system.

Table 5

Specifications of the IMU.

Sensor	Gyroscope	Accelerometer	Magnetometer
Type	MPU6000	MPU6000	LSM303D
Full scale	−1000~1000 (°/s)	−8~+8 (g)	−8~+8 (gauss)
Sensitivity	≤0.030 (°/s/LSB)	0.2 (mg/LSB)	≤0.320 (mGauss/LSB)

The system is divided into two parts in Figure 9. A traditional GPS positioning device is replaced by a UWB to realize indoor positioning. Meanwhile, the position can be obtained using the acceleration and angular rate provided by an accelerometer and gyroscope through the strapdown inertial navigation algorithm. Nevertheless, IMU measurements have significant drift errors over a long time. Therefore, EKF is adopted to fuse both to get a more precise position. The rotation matrix is acquired by a complementary filter. The gyroscope is compensated by the accelerometer and magnetometer. The control system has been introduced in Section 2 and Section 3.

Figure 9

The flow diagram of the algorithm of the system.

The UWB system is produced by INF Company (Harbin, China). The placement of base stations is shown as Figure 10.

Figure 10

The placement of base stations and the tag.

The positions of base station () are fixed and the tag is attached to the quadrotor. The tag provides the distances from the tag to each base station. Based on spatial relationship, Equation (37) can be obtained as:

In order to obtain more accurate position, the measured distances are processed by Kalman filter to eliminate the measurement noises. The state equation and measurement equation are as follows: where is the distance vector between tag and each base station .The subscript denotes sampling time. is process noise. is the distance measured from UWB is measurement noise. In the experiment,, .

The position can be determined by any three equations of Equation (34). Generally, the base station is regarded as standby station. When a failure happens to one station, the station is used to calculate the position. After the Kalman filter, the position of tag can be determined as follows by solving Equation (39):

In practice, we have tested the measurement accuracy of UWB system when the tag is placed in . Figure 11 shows actual position measured by UWB system and the corresponding errors.

Figure 11

The measured position by UWB system and the corresponding errors (a) The measured position by UWB system; (b) The corresponding position errors.

We can observe that the measurement errors of UWB system are within 5 cm from Figure 11b. Generally, the inertial measurements contain a certain bias and white Gaussian noise . The measurement models of angular velocity and acceleration yield [29,30]: where and are the measurements of gyroscope and accelerometer, respectively. and are true angular velocity and acceleration in the world frame, respectively. is gyro noise with covariance and is accelerometer noise with covariance . In addition, the gyroscope bias and accelerometer bias are modeled as random walk as: where and are white noise. Whereby and .

According to Newton’s law and Euler formula, the discrete-time kinematics equations are illustrated as follows: where denotes and is the sampling period.

For the representation of rotation matrix we use quaternions, the quaternion representation of rotation matrix is given by:

Based on the differential equation of quaternion, the prediction of quaternion is performed as: where:

Because IMU measurements have significant drift errors over a long time, EKF is utilized to fuse measurements from IMU and measurements from UWB to get more precise position. The design of EKF refers to the design procedure in [31,32,33]. The state vector is chosen as . The noise vector is . Combining Equation (39), Equation (41) and the discrete form of Equation (38), the state equation can be written as:

The process noise covariance . The noise parameters are given by IMU manufacturer.

The measurement of UWB is defined as . Then the measurement equation can be expressed as: where is the measurement noise. The measurement transformation matrix is . The observation covariance matrix can be calculated by system observations. The update of state follows the standard EKF procedure shown in Algorithm 1.

The state of EKF updates only when the measurements from UWB system update. In practice, the inner loop run faster than the outer loop, thus the updating rate of attitude is supposed to be faster than that of the position. During the interval of state update, the attitude is obtained by the IMU and magnetometer [34]. The accelerometer and the magnetometer are utilized to calculate the error between reference direction and measured direction, which is used to compensate for the angular velocity by PI regulator. The calibrated angular velocity is further used to update quaternion according to Equation (41). The ultimate rotation matrix is obtained based on Equation (40).

5.2. Experimental Results

In order to verify the validity of the tracking performance of the proposed method, we design a helical trajectory as the given trajectory. In the experiment, the outer loop runs at 100 Hz (the updating rate of UWB), while the inner loop runs at 1 kHz. Figure 12 shows the position.

Figure 12

Actual position of quadrotor in the experiment.

For the comparison purposes, a PD controller is employed with the same task (Case 1). The PD control parameters are tuned by a great deal of adjusting parameters. Three sets of parameters are chosen to show the effect of parameters on system. The quality of system is evaluated by maximum overshoot (MO), integral absolute error (IAE), integral squared error (ISE). The performance indices are summarized in Table 6.

Table 6

Comparison of experiment results under three sets of PD parameters.

	Index	x			y			z
Parameters		MO	IAE	ISE	MO	IAE	ISE	MO	IAE	ISE
k_p	16	0.2551	1.5721	0.1943	0.2415	2.1305	0.4862	0.1766	1.9916	0.4019
k_d	5.6
k_p	16	0.1263	1.6070	0.2033	0.1576	2.0041	0.3983	0.1637	1.9363	0.3997
k_d	10
k_p	18	0.1502	1.5100	0.1819	0.1074	1.9805	0.3766	0.084	1.9285	0.3883
k_d	10

As shown in Table 6, IAE and ISE are small enough, which means the system has reached its good performance. Meanwhile, the third set of parameters has smaller MO, IAE and ISE compared with the other two sets of parameters. Thus, the third set of parameters is chosen as ultimate control parameters. Figure 13 depicts the position errors and histograms of corresponding errors.

Figure 13

Position errors and histograms of position errors in Case 1 (a) position error; (b) Percentage of position error; (c) position error; (d) Percentage of position error; (e) position error; (f) Percentage of position error.

As shown in Figure 13, the position error along is controlled within m, about eighty percent of the error is in the range of m to m. The error in direction is in m~ m and the majority of the error is within m. The error for direction is controlled within m, ninety percent of the error is within the range. In order to demonstrate the robustness of the proposed control method, the same experiment is carried out under disturbances (Case 2). Figure 14 shows the position errors and histograms of corresponding errors. Meanwhile, range, the percentage of error within 0.2 m and standard deviation are adopted as quantitative performance indices to describe the control performance [35,36]. The results in the two cases are listed in Table 7.

Figure 14

Position errors and histograms of position errors in Case 2 (a) position error; (b) Percentage of position error; (c) position error; (d) Percentage of position error; (e) position error; (f) Percentage of position error.

Table 7

Comparison of experimental results under two cases.

	Controller	SMC + SMC			PD + PD
Index		x	y	z	x	y	z
Case 1	Settling time [s]	3.39	3.42	4.49	5.29	5.30	6.44
	Range [m]	(−0.25,0.26)	(−0.41,0.31)	(−0.76,0.32)	(−0.31,0.32)	(−0.72,0.41)	(−0.78,0.42)
	The percentage of error within ± 0.2 m	98.3%	95.6%	92. 7%	94.3%	87.8 %	89.4 %
	Standard deviation [m]	0.0921	0.1006	0.1353	0.09724	0.1514	0.1396
Case 2	Settling time [s]	3.43	3.45	4.53	5.69	5.68	6.83
	Range [m]	(−0.31,0.35)	(−0.43,0.33)	(−0.77,0.34)	(−0.38,0.45)	(−0.73,0.46)	(−0.79,0.53)
	The percentage of error within ± 0.2 m	96.2%	93.8%	90.8%	88.0%	80.4%	78.2%
	Standard deviation [m]	0.0935	0.1018	0.1364	0.1283	0.1698	0.1738

Fast convergence rate is one main advantages of SMC in comparison to PD control. The settling time shows the system under SMC reaches stability faster. The error range indicates that the maximum deviations of SMC are smaller than those of PD. The percentages of error demonstrate that more than 90% of the errors are within 0.2 m under SMC. It also can be observed that only the percentage of error along is more than 90% under PD control. Meanwhile, the percentages of error within 0.1 m are obviously greater than those under PD control from Figure 13. The errors along and beyond 0.2 m with help of PD control reach 10%, while the errors with the help of SMC are much less than 10%. The standard deviations of the designed controller are much less than those of PD controller. In other words, the proposed control scheme has better stability. Above all, the proposed control method can achieve a better tracking performance.

When the system is disturbed, the performance indices of SMC show small changes while the performance indices under PD controller obviously increase. The percentages of error within 0.2 m with the help of SMC slightly decrease. However, the percentages of error within 0.2 m under PD controller decrease in large amplitude. Meanwhile, the standard deviations significantly increase while the standard deviations with the help of SMC change slightly. Hence, the proposed control method shows a better performance even in presence of disturbances.

6. Conclusions

In this paper, a large scale dynamical model of a quadrotor is built according to the Newton-Euler equation. An inner and outer loop control method is proposed for trajectory tracking. The outer loop that governs the translational motion is designed using SMC law. A SMC inner loop controller based on a rotation matrix is designed to control the rotational motion. The dynamical model of the quadrotor with the controllers is simulated in Matlab/Simulink. The tracking helix verifies the control method has a good tracking performance. Tracking edge-on circle and rectangle trajectories further demonstrates the quadrotor can track nontrivial trajectories and realize large angle flight. The robustness of the proposed method is verified by a tracking helix in the presence of model uncertainties and external disturbances. The simulation results show that the proposed control method has better robustness compared with PD and PD combination. Based on the simulations, we conduct an experiment to further verify the feasibility of the method. The experimental results show the position is well controlled within a reasonable range. Comparative experiments with PD and PD combination further verify the effectiveness of the proposed control method. In our work, the rotation motion is realized through adjusting the rotation matrix to the desired value. The angular velocity is no longer approximated by the first time derivative of attitude angles. In this case, the quadrotor is able to track nontrivial trajectories. Meanwhile, SMC is utilized to govern the inner and outer loop. The use of SMC speeds up the convergence rate and improves the robustness of the system. Future works will focus on autonomous navigation of the quadrotor.