Multivariable Control of a Rolling Spider Drone

The research and application of Unmanned Aerial Vehicles (UAVs) has been a hot topic recently. A UAV is defined as an aircraft which is designed not to carry a human pilot or operated with remote electronic input by the flight controller. In this thesis, the design of a control system for a quadcopter named Rolling Spider Drone is conducted. The thesis work presents the design of two kinds of controllers that can control the drone to keep it balanced and track different kinds of input trajectories. The nonlinear mathematical model for the drone is derived by the Newton-Euler method. The rotational subsystem and translational subsystem are derived to describe the attitude and position motion of drone. Techniques from linear control theory are employed to linearize the highly coupled and nonlinear quadcopter plant around equilibrium points and apply the linear feedback controller to stabilize the system. The controller is a digital tracking system that deploys LQR for system stability design. Fixed gain and adaptive gain scheduled controllers are developed and compared with different LQR weights. Step references and reference trajectories involving significant variation for the yaw angle in the xy-plane and three-dimensional space are tracked in the simulation. The physical implementation and an output feedback controller are considered for future work.


Introduction
The Quadcopter is one of many types of unmanned aerial vehicles (UAVs).
Because of its reliability of maneuvering and the ability to be controlled and modeled, the research about quadcopter is becoming a hot topic in the area of robotics control.

Objectives and Motivation
This thesis work deals with the design of a control system for a specific quadcopter which name is called a Rolling Spider Drone [1]. It can take off and land vertically and hover and fly with high maneuverability. Also, one of the most important advantage of the Rolling Spider Drone is that the various control algorithms can be downloaded into Drone's controller via firmware to test the actual fly performance.
The challenge in control system design for a quadcopter is that the mathematical model of the system is highly coupled, nonlinear and multivariable [2]. The quadcopter dynamics contain 6 degrees of freedom, which are 3 rotational degree, (roll, pitch and yaw), and also translation along the x, y, z axes. Because the number of control inputs four motors are less than the system's degrees of freedom, it is an under-actuated system. The Rolling Spider Drone moves in a three dimensional plane. Optimal control theory and gain scheduling strategy will be employed in the design of controller.
Currently, the Control System Laboratory in ECBE Department at the University of Rhode Island does not have any hardware for flight control experiments.
The purpose of this thesis is to use the commercially available Rolling Spider Drone and Matlab/Simulink software to begin the development of a flight control testbed for multivariable control algorithms [3]. The specific goals are as follows: 1. Derive the theoretical state-space model for the drone.
2. Develop linear multivariable control algorithm based on linearized models using optimal control technique [4]. Evaluate the controller performance in simulation.
3. Extend the single controller to a set of controllers designed at a set of equilibrium points and evaluate a gain-scheduled control system for reference trajectories that require the use of multiple linear models.

Contribution of this Work
There are various control techniques that are used in order to provide stable position and orientation of quadcopter. Reference [5] presentes a PID tuned LQR controller with good robustness and easy implementation, however, only simulation provided, the actual applicability of the controllers is not proposed. Reference [6] presents a gain scheduling technique to design a controller for nonlinear system by linearizing the system at several equilibrium points. They use LQR controller to verify the controllability and observability. However, this paper did not provide closed-loop control for the trajectory tracking. The design of this system will be based on LQR controller design for linear models and use gain scheduling technique to deal with the nonlinear model of Drone. A digital tracking system [7] will be designed that would be suitable for implementation on the Rolling Spider drone.

Thesis Structure
This thesis is organized into five Chapters.

CHAPTER 2 System Modeling
In this chapter the flight dynamics model will be derived. The ordinary quad-copter has a symmetrical design. It consists of four rotors fixed at an equal distance from the body central as shown in Fig.1. Each of the rotors are deriven by a DC motor. Propellers 1 and 3 rotate in the same direction and propellers 2 and 4 rotate in the opposite direction to keep the system balance [1].

Kinematic Model
In order to set up the model of the quadcopter, the reference coordinate frames should be defined. The earth inertial references frame is with N, E, D axes and A, B, C body frame is with x, y, z axes. As shown in Fig.2, the inertial reference is fixed on a specific place and it uses the N-E-D notation which represents North, East and Downwards respectively. For the body frame, the origin located in the center of the quadcopter body with x-axis pointing towards propeller 1, y-axis pointing towards propeller 2 and the z-axis is pointing to the ground. It is known that the quadcopter is a 6 DOF object. The three Euler angles φ, θ, ψ represent the orientation of the quad-copter, where φ is roll angle about the x-axis, θ is pitch angle about the y-axis, ψ is yaw angle about z-axis. The Euler-angles are import for the definition of the transformation matrix from different reference frames.
The transformation matrix is formed by a sequence of three plane rotations, which are named Ryaw, Rpitch, Rroll. There are two intermediate coordinate systems being defined, the vehicle-1 frame and vehicle-2 frame; to describe the orientation of the quadcopter.
The inertial frame is rotated about its z-axis by the yaw angle ψ to get the The resulting frame is then rotated by the pitch angle θ around its y-axis to produce the vehicle-2 frame. This rotation matrix is The last rotation from vehicle-2 to the body frame is Finally, we get the rotation matrix from the NED world inertial frame to the body frame is Multiplying out the three individual rotation matrices yields where c and s denote cos and sin respectively. Also the rotation matrix that transforms from the body frame to the inertial world frame , R B2W is obtained by inverting R W 2B . The result can be shown to be These rotation matrices will be used to convert the states measured in one frame to another frame. In addition to transforming angular positions, it is also possible to transform angular velocities. There is an inverted Wronskian matrix W −1 that can transform the angular body rates ω = (p, q, r) T to the Euler rateṡ Θ = (φ,θ,ψ) T that are measured in the inertial frame. A transformation is described as follows,Θ where W −1 is given by

Dynamic Model
The motion of the quadcopter can be divided into two subsystems, translational motion (x,y positions and altitude), and rotational motion (roll, pitch and yaw) [2]. Based on the Newton-Euler formalism, the forces and moments acting on the quadcopter will be investigated.

Translational Equations of Motion
Assume that the structure of quadcopter is rigid and symmetrical about the center of gravity of the quadcopter, and the propellers are also rigid. Based on the Newton's second law, where F w = G − T . G is the gravity and T is the thrust generated from rotors.
Note that the T is described in the body frame, and must be transformed from body frame to inertial frame. Then equation 9 can be expressed aṡ Substituting R B2W into this equation gives an expression for the derivative of velocity rate in the inertial frame.
This vector equation can be written in component form as

Rotational Equations of Motion
In the inertial frame, it is known that the moment is defined as the time derivative of the angular momentum, using rigid body rotational law, we have where I is the body's inertia tensor Note that the off-diagonal terms are equal to zero due to the symmetric mass distribution of the quadcopter. In order to make the understanding and calculations more intuitive, it is better to set up the expression in the body frame using the Euler's equation, where the right-hand side is a vector of applied torques.
substituting this into equation 15 yields These equations can be solved for the derivatives of the angular rates to obtaiṅ p = (τ x + I yy qr − I zz qr)/I xẋ q = (τ y − I xx pr + I xx pr)/I yẏ r = (τ z + I xx pq − I yy pq)/I zz Using 7 and 8, the derivatives of the Euler angles are obtained by multiplying by W −1 to getφ

State Space Model 2.3.1 State Vector
The state vector of the quadcopter is defined to be, Note that the translational positions and velocities are defined in the inertial frame while the p q r are defined in the body frame.

Input Vector
The vector input U consisting of four inputs, is defined as, That is, the inputs to the quadcotper system are the squares of the motor speeds drving the four propellers.

Aerodynamic Moment and Force
As an effect of the propeller rotation, the lift and translational force called the aerodynamic force generated. The direction of the force is always aligned with the body frame Z-axis. Referring to the work of Pounds et al, the thrust force generated from rotors i = 1, 2, 3, 4 is proportinaol to the square of the motro speed The rotors also generate the aerodynamic moment. which is proportianl to the square of the rotor speed, For the low-altitude flight of quadcopters, air density can be considered as a constant number, and K a and K m in equation 22 and 23 are constants respectively.
Consider now the moments and thrust acting on the quadcopter.
For the moments about the body frame's y-axis,the thrust of rotor 1 generate a positive moment, while the thrust of rotor 3 generates a negative moment about the y-axis,the total moment can be expressed as, The moment about the body frame's z-axis is caused by the rotors' rotation from aerodynamic moment,while the thrust force has no effect on the moment for z-axis.
The thrust is given by Here in the body frames, we define the positive orientation is pointing downwards, so the thrust forces acting on the z axis produced by the rotation of four propellers is in the negative direction, while there is no forces acting on the x and y directions.
The non-linear state-space equationẊ = f (X, U ) can be written as,
[2] H. ElKholy, "Dynamic modeling and control of a quadrotor using linear and nonlinear approaches," Master's thesis, The Amerian University in Cairo, 2014.

Control System Theory
The goal of this chapter is to design a digital tracking system fro the quadcopter. Although this plant is a nonlinear system, we can still use resutsl from linear control thoery. As such, before we develop the various controllers, we first go over the basic knowledge of the linear control thoery.

Linear Control System
A linear time invariant (LT I) system may be represented by a state-space where Since the quadcopter is a multi-input multi-output(MIMO) system which has 12 state variables, 4 inputs and 4 outputs states want to track in the thesis, after linearization, the matrix A is 12 × 12, B is 12 × 4, C is 4 × 12 and D is 4 × 4.
Because we want to use digital control, we let the plant-input signals be generated through a D/A converter (zero-order hold) with sampling interval T seconds. Also, the plant outputs and state variables are sampled by A/D converters. The LT I continuous-time system in 29, when sampling-ed by D/A and A/D converters, is equivalent to the following discrete-time state-space model, where and Γ = e Aτ Bdτ . T is the sampling interval and k is the time value of the discrete-time sequences.

Controllability and Observability
Consider a continuous linear system that the feedforward matrix D = 0, so that the system can be written asẋ = Ax + Bu dynamics form with the output is the entire state that is C = I 12 , if it is possible to find an input u(t), t 0 ≤ t ≤ t 1 to drive the system from arbitrary initial state x(t 0 ) = x 0 to an arbitrary final state x(t 1 ) = x f with finite time, the system is said to be controllable. The controllability matrix W c can be defined as The pair (A, B) is controllable if and only if the rank of (A, B) is equal to n, the size of A matrix we defined in 29.
Consider a digital control system , if it is possible to find an input sequence u[k] that takes the system from an arbitrary initial state x[0] to any final state x[i] with finite step i, then the digital system is said to be controllable.
Here defines a controllability matrix W C for the digital system, According to [1], the controllability of the ZOH equivalent system (Φ, Γ) obtained from a continuous-time system (A, B) can be determined with two criteria.
First, the continuous-time system (A, B) must be controllable. Second, if the imaginary part of the poles with largest magnitude β max satisfies then (Φ, Γ) will be controllable.
The property of obsevability can be defined for a continuous-time LT I system, which is a simplified form of 29. This system is said to be observable, if the initial state x t0 can be calculated using the inputs and outputs where t 0 ≤ t ≤ t 1 , with

Linear Quadratic Regulator
The desired control system for the quadcopter is a digital tracking system in which reference trajectories are supplied for desired output variables. However, it will be seen that the design of a tracking system is accomplished by solving an associated regulator design problem. For ease of presentation, we first consider regulator design in continuous time.
The notion of linear quadratic regulator (LQR) comes from the M IM O control system design [2]. Since the feedback gain is not unique for multi-input systems, optimal control solutions provide the best possible feedback gains for M IM O system. Linear quadratic optimal control can be realized by specifying some kind of performance objective function to be optimized. Consider that we have the where the W c (A, B) is controllable. We would like to design a state feedback control u = −Kx such that the cost function, is minimized and drives state x to 0. Where Q ∈ n×n ,R ∈ m×m are positive definite symmetric matrices chosen by the designer.
For the infinite time LQR problems, assuming that (A, B) are constant matrix, 2 ) is detectable. In this case, the solution that minimizes cost function is the feedback control function with the feedback gain matrix The matrix P is the unique symmetric positive definite is a constant matrix that solves the Control Algebraic Ricatti Equation(CARE).
Then the optimal control input is where K is called Kalman gain matrix. The "optimal" state feedback regulator can be drawn as Fig.3.

Linearization near an equilibrium point
In order to use linear state feedback to regulate a nonlinear dynamic model, at an equilibrium point, we need to linearize the nonlinear system around this point. According the Lyapunov stability theory, we can guarantee the system be locally asymptotically stable around the equilibrium point and also we can find the Region of Attraction for the equilibrium point [3]. Suppose we have a nonlinear with an operating point (x eq , u eq ) and f (x, u) continuously differentiable in a domain containing the operating points such that f (x eq , u eq ) = 0. Expand the nonlinear function in a Taylor series about the operating point. Then, for x near x eq and u near u eq we define the actual plant state vector x as the constant equilibrium state vector x eq plus a vector of deviations w, Also for the actual plant input vector u as the constant equilibrium plant input vector u eq plus a vector of deviations v, Then the behavior of the nonlinear system in 43 can be approximated by a linear state-space model for the deviations as follows, where z is the deviation from the equilibrium output z = y − Cx eq . The nonlinear system looks like a linear system around the equilibrium point where Assume that the pair (A, B) is controllable,that means the closed loop system eigenvalues are distinct and not on the imaginary axis, the linearization gives a good approximation for the nonlinear system around the equilibrium point, we can develop a linear feedback control u = −Kx to regulate the closed loop nonlinear system. As Fig.3ẋ = f (x, −Kx).

Lyapunov Stability
In the sense of Lyapunov stability, an equilibrium point is to be asymptotically stable if it is both stable and convergent, that means for all ε there exist a δ > 0 For a particular physical nonlinear system we would deploy a scalar-valued positive definite function which is Lyapunov function to compute the stability.
Consider a n variables scalar-valued function V (x) is a positive definite function on some domain D which contains x eq with V (x eq ) = 0. IfV (x) is negative semidefinite in the domain D then x eq is stable. For the quadcopter system, we note that the origin x = 0 is an equilibrium point of the closed-loop system, since closed-loop linearized system A − BK is stable (Hurwitz), if follows that, for the nonlinear system x eq is asymptotically stable. A set M ∈ n (n dimensional state space) is said to be invariant sets x(0) ∈ M ⇒ x(t) ∈ M ∀t ≥ 0. In our control laws, the feedback matrix was computed by the linearized model of the quadcopter. The limitations for this method is that the feedback matrix is useful in a neighborhood of the equilibrium point. As for how large the valid neighborhood is, we will use Region of Attraction (RoA) to define the area. The definition of Region of Attraction can help us find the initial states for which x(t) → x eq . Given a linearized point, from Lyapunov stability theory, we may be able to obtain a conservative estimate for the RoA from the invariant sets.

Stability Margins For Linear Multivariable Control Systems
Consider the feedback interconnection of two stable systems as shown Fig.4.
According to the small-gain theorem, if on the small gain theorem, we can find a model from w to v with r set to zero. As the Fig. 6 shows, the system H(s) will be stable if, Then the robustness bound for this closed loop system can be defined by where H(s) is the system form w to v which can be calculated using the system infinity norm of (A, B, C, D). The value of the δ 1 implies that how large the system can bear for the perturbations.
Now, suppose that we have the perturbation model that is a diagonal matrix of unknown complex numbers, We know that this closed-loop system remains stable for all ∆(s) with ∆(s) ∞ < δ 1 . The maximum singular value of a diagonal matrix is simply the largest absolute value of the diagonal elements, So we have the conclusion that the control system will keep stable if max |c i | < δ 1 . This analysis allows us to make a connection between the stability robustness bound δ 1 and classical stability margins (gain and phase margins).
Suppose we set up a model for the plant uncertainty with a diagonal matrix of complex numbers which is the classical perturbation model as Fig.7 where Assuming δ 1 has been calculated for a given control system and ∆(s) is assumed to be a complex number c i , so the stability conditions on corresponding classical gain margins can be found by using Q = I + ∆ or q i = 1 + c i . If q i is a real number, the system stable condition is If q i is a complex number that is q = e −jφ = 1 + c i , where 1 is the center of the circle and c i is a disk with the radius δ 1 , we can find the classical phase margin If the system perturbation plant model with following structure Fig.8, the stability robustness bound can be defined as δ 2 . The system will be stable if 1 1+δ 1 < δ 2 . The connection between the classical perturbation model with this plant model is, Q = (I + ∆) −1 or q i = 1 (1+c i ) . Since c i is a real number then −δ 2 < c i < δ 2 , so the q i satisfy 1 1+δ 2 < q i < 1 1−δ 2 when q i is a real number. If q i is a complex number that is q = e −jφ = 1 + C i , where 1 is the center of the circle and c i is a disk with the radius δ 2 , we can find the classical phase margin (PM) is, Combining the results for δ 1 and δ 2 yields the following relationships between the classical stability margins and the robustness bounds δ 1 and δ 2 .

Gain-Scheduled Control
In the previous discussion, we implemented the linear controller around the equilibrium point x eq which is a single point. Although we can guarantee the local stability around this point, when a trajectory goes outside of the RoA, the designed controller would not be effective. This situation can be handled when there are a sequence of operating points. In this case we need to design a sequence of different linear controllers corresponding to the operating points in order to maintain the stability.
The result is called a Gain scheduled controller. The main idea of the gain scheduling is to linearize a range of operating points which contains the scheduled variables, and then compute the varying linearized dynamics and feedback matrices for the operating points. A series of linear controllers will be combined together to control the nonlinear model [4]. The control precision would be determined by the accuracy selection of scheduled variables ,it is usually affected by the computation speed and memory of the hardware for the quadcopter.
Consider the nonlinear systemẋ = f (x, u). Let the x eqi , u eqi , i = 0 be a set of equilibrium points and x eq0 = α φ . The value of the control inputs is u(α 0 ) where satisfies the equation, Based on the equation 3.6, we can linearize the nonlinear model about the (α 0 , u(α 0 )) then we have the state matrix and input matrix A(α 0 ) and B(α 0 ) where Assuming that the pair (A(α 0 ), B(α 0 )) is controllable, we can now continue to develop a feedback control such that A(α 0 ) − B(α 0 )K(α 0 ) is stable. Applying this to the linearized the state space equation results in the closed-loop system, We know that the Lyapunov stability can guarantee the closed-loop system is stable in the region of attraction of Rα(0) ∈ D. When we switch the equilibrium point to α 1 at time t 1 , where α 1 , x(t 1 ) ∈ Rα(1) we can linearize the system around (α 1 , u(α 1 )) to obtain a new controller,

CHAPTER 4 Tracking System Design and Simulation Analysis
As discussed, the technique of gain scheduling is well suited for linear parameter-varying systems where the varying parameter can be assigned to be the scheduling variable in the control law. This strategy can be applied to nonlinear systems when the linearized dynamics happen to depend on a parameter of interest to the control engineer. We will use the linearized dynamics of the quadcopter to develop a series of gain scheduled control laws that use the yaw angle as the scheduling variable. We will construct LQR control laws for use with both state and observer-based output feedback, where the output vector is chosen so that the linearized system is observable for all values of the scheduling parameter.Simulations will be conducted for each control law, tracking trajectories that exhibit the gain-scheduled nature of the controller.

Linearization and Controllability of Quadcopter Dynamics
We now are interested in linearizing the nonlinear dynamics of the quadcopter about its equilibrium points in order to develop our gain-scheduled controller. To do this, we must first find the equilibrium points of the quadcopter dynamics.
Linearizing the nonlinear dynamics about the steady state values results iṅ where the Jacobian matrices are given by The signals to be tracked are y = (x, y, z, ψ) T which can be expressed as We denote the controllability matrix of the linearized system as, it can be verified that ρ W c (A eq , B eq ) = 12, ∀ψ eq ∈ (77)

Obtaining a Tracking System by Regulating the Design Model
The goal of this chapter is to design a tracking system that cases the plant outputs to follow given reference trajectories. Chapter 3 show that a state-feedback gain matrix can be calculated that will regulate the plant (drive all state variables to zero). We now show how to extend the plant to a " design model " such that regulating the design model is equivalent to a tracking system for the plant.
The implement of control algorithm for the quadcopter is accomplished by digital tracking system. As Fig.9, the digital tracking system is comprised of the non-linear plant, a linear feedback matrix and additional dynamics.
then we haveż then 81 can be written asż =Ã z +B µ. A regulating gain matrixK can be computed for (Ã,B) using the LQR algorithm, choosing the loop with µ = −Kz.
This regulator drives The system will drive y to r and keep zero steady-state error despite of uncertain disturbance w.

Additional Dynamics Blocks
The additional dynamics block consists of m integrators in parallel whose outputs are multiplied by the gain matrix K 2 . As Fig.14 shows, the input to the additional dynamics are the differences between the measured outputs and the references inputs. We will supply reference input signals for the following four variables: x-position, y-position, z-position (height) and ψ (yaw angle). Thus the additional dynamics block will contain four integrators. Note that the state-space

LQR Design of a Linear Tracking System
Given a plant model (A, B, C) with n state variables and m outputs, the procedure developed in the previous subsections is used to calculate the gain matrices K 1 and K 2 that are needed to implement the tracking system shown in Fig.13.
The procedure may be summarized as follows: (1) For the matricesÃ andB for the design model shown in Fig.10.
(2) Choose LQR weighting matrices Q and R. This choice is discussed below.
(4) Partition theK matrix to obtain K 1 and K 2 . K 1 consists of the first n columns ofK and K 2 consists of the last r columns.
The Q and R matrices used in the LQR design are chosen using the following considerations. that minimizes the performance cost J are choosing using the following theory. The Q matrix is chosen to have the following form: where r × (n + r) matrix C d = the value of r 1 , r 2 , r 3 , r 4 will allocate more energy employed by the control inputs.
Varying α and ρ will affect the dynamics performance of the system and find a optimal u = −Kx where the feedback matrix K is calculated by the optimal Q and R to minimize the cost function J 38.

Design of a Fixed-Gain Tracking System
We will begin with developing the fixed-gain controller to track a step input reference. The nonlinear model of the quadcopter iṡ be the linearization about the equilibrium point x eq , u eq , where, x eq = (x, y, z, 0, 0, ψ, 0, 0, 0, 0, 0, 0) T (86) and The linear tracking sysem will be designed using x eq and u eq . In order to control the nonlinear drone, a linear feedback controller wiil be used, as shown in Fig.15. Note that the linearized dynamics do not depend on the spatial coordinates (x, y, z) of the steady state. As such, the gain matrix K only depends on the yaw angle configuration ψ of the quadcopter, so we can use this controller to track a step reference, We will now go through a series of choices for the Q and R weight matrices and evaluate the performance of the resulting tracking system. Firstly, we linearized the nonlinear dynamics at x eq = (2, 2, −2, 0, 0, pi/6, 0, 0, 0, 0, 0, 0) T with the balanced input u eq = (ω 2 1 , ω 2 2 , ω 2 3 , ω 2 4 ) T , where ω 1 = ω 2 = ω 3 = ω 4 = m g 4 ka , when the quadcopter stay in a constant altitude hover state. Then we got the   Step Reference tracking X and Y states energy for the controller to stabilize the plant. Fig.17 shows that the input angular velocity are equal in magnitude but have opposite direction for the opposite pair ω 1 , ω 3 and ω 2 , ω 4 when the quadcotper take off and hover at a stable state.
It takes more time for the height and yaw angle to reach a steady state.
So, we set the c 1 , c 2 , c 3 , c 4 to be (10, 10, 100, 100) giving more effort for z and ψ states control. We will choose some weights to penalize theẋ,ẏ andż and keep (u 1 , u 2 , u 3 , u 4 ), α and ρ as the first test. Fig.19 and 20 show that when effort applied to control state is increasing, it takes less time for the states reach a steady state. For x and y states, it needs about 30 seconds, while for z and ψ the time would be around 50 seconds. The overshoot for x, y, z, ψ is much better than previous values.
Although the overshoot has been improved and settling time is more quick, we want to seek a more better transient response with good combination of Q and The last test we set the c 1 , c 2 , c 3 , c 4 to be (100, 100, 1000, 1000) and keep α = 10, ρ = 0.001 (u 1 , u 2 , u 3 , u 4 ) as one and penalty forẋ,ẏ andż, thus Q = diag and R = diag. While for the yaw angle undergoes substantially variation, the fixed-gain controller will work not well. To correct this issue, a set of linear tracking gains can be calculated for a given set of equilibrium points, we will use the Q and R matrices shown in for each of theses designs. A gain-scheduled tracking system will be developed using the measured yaw angle as a scheduling variable.

Gain-Scheduled LQR Control
The fixed gain controller we designed have a good performance about step reference tracking. Now we switch to show how the feedback matrices K 1 and K 2 are obtained by linear interpolation for the inputs references with significant change for the yaw angle.

Figure -8 trajectory tracking
Tracking a figure eight curve in a 2 dimensional plane. During this test only the design values Q and R in the last group experiment for the step references tracking will be employed. The figure eight references inputs is generated in the commands block. The range of the desired x m and y m are defined as Where A x is the amplitude of x m , A y is the amplitude of y m and T is the Note that the tan −1 is calculated by the atan2 function to make it change smoothly in all quadrants. The main idea of gain scheduling controller is to design a family of linear controllers around operating points, or regions that should guarantee the robustness, and change according to the variation of scheduled variable ψ angle. The change of controllers usually be implemented with interpolation or by switching from different values of scheduled variable. [1] analyze that the instability created of switching among controllers which may come from the transition dynamics that the information is not covered by each switching linear controllers.
So, interpolation methods will be focused in the thesis. The values spacing of interpolation for the ψ angle will be determined firstly. Then, a sequence of linear feedback matrix could be calculated using equilibrium points with the changeable ψ angle. As Fig.28 shows, the matlab function inside the controller block defines tual feedback matrix is computed using the parameter α. In this way, the missing information between the transition dynamics could be detected mostly. Also, the modulo operation used to make sure the ψ angel value stay in the range (−π − π).
The control precision is usually determined by the spacing ∆ radians and limited by the physical hardware. We tried the following values for ∆ as π/3 , π/12 and π/30.
We can see that as the ∆ decreases from π/3 to π/12 and final to π/30, the tracking performance becomes much better. The actual tracking locus is very sloppy and fuzzy when ∆ = π/3. When the ∆ decreased to π/12, although with little distortion during the trajectory tracking,the overall performance is well. The improvement from last when ∆ = π/30 is more noticeable with precise tracking.

3D trajectory tracking
we will further conduct a simulation where the quadcopter is to track a rising sprial and lemniscate trajectory in 3-dimensional space to test the effectiveness of the gain-scheduled controller. The helix curves can be parameterized as Where A x is the amplitude of x m , A y is the amplitude of y m and T is the In the simulations, we will use A x = 15,A y = 15,A z = 0.5, t = 0.005, T = 10.
The quadcopter is manipulated to fly 3 rotations around z axis. The actual trajectory is slightly difference with the references around the start point due to the initial state setting for the quadcopter, this can also be seen in the xy-plane projection and y trajectory position response. At about 5 seconds, the measured trajectory begins to track the references well, although with little deformity. The Fig. 34 xy-plane plot gives a top vision of the behavior for the helix curve. Fig.33 shows that the two clockwise spinning and two counterclockwise spinning rotors have the same magnitude but opposite direction of the angular speed. This two pairs rotors make quadcopter rise quickly and balance drag-induced torque about z-axis. Fig.35 shows the roll angle variation is less than 0.8rad even for the big yaw Now we switch to another 3D trajectory, which is a lemniscate trajectory that was used in , which is defined as, where A x is the amplitude of x m , A y is the amplitude of y m and T is the period of the figure eight. The relation between the x m and y m with the ψ angle using the following equation, In the simulations, we will use A x = 2, A y = 2, A z = 0.5, t = 0.005, T = 10. The actual trajectory is slightly difference with the references around the start point due to the initial state setting for the quadcopter, this can also be seen in the xy-plane projection and y trajectory position response. To check the ability of the tracking system to "catch up " to the references, the initial state we set for the drone is different from the initial values of the references. Fig.40 shows that the two clockwise spinning and two counterclockwise spinning rotors have the same magnitude but opposite direction of the angular speed. This two pairs rotors make quadcopter rise quickly and balance drag-induced torque about z-axis. CHAPTER 5

Conclusions
The quadcopter is a MIMO nonlinear system with four inputs and twelve state variables. Linearization the nonlinear model using the Jacobian methods, we obtain the state space model around the desired equilibrium points . In the design of digital LQR controller, the altitude, x position, y position and yaw angle are tracked outputs and the twelve state variables are feed into the gain matrix. The weights choice for LQR regulator is tested using different pair of Q and R matrix.
The stability margin and settling time of the linearized system are guaranteed although with little overshoot. Techniques from linear control theory used to design the fixed gain digital tracking system. We extended the LQR regulator to a digital tracking system with the additional dynamics. When tracking references signals involving big variation in the yaw angle, we designed the gain-scheduled controller taking yaw angel as the scheduling variable, the adaptive gain matrices are calculated using the gain-scheduled algorithm. To demonstrate the effectiveness of the algorithm we developed, step references and trajectory tracking simulation we conducted. Also, we used the control laws tracking some trajectories in the three dimensional spaces such as helix and lemnsicate when there is no dramatic changes in the trajectory of yaw angle. The simulation result show that the controller we developed is able to successfully converge to the stable state and track the desired references.
This thesis provides a basis platform to design the trajectory tracking controllers for the Rolling Spider Drone. Although the control algorithm we developed is proved to be perfect, there are many parts of the design can be improved. The state-feedback regulator employed in the design is a full state feedback controller with all 12 state variables measured. In the practice, it is more necessary and smart to measure some of the variables with the sensors mounted on the Drone. Usually, the Inertial Measurement Unit (IMU) which is a 6 axis-accelerometer-gyroscope equipment and pressure sensor can obtain the acceleration and angular rates in the body-frame and heights in the inertial-frame. Then the measured inertial-frame velocity and x,y positions can be obtained by transformation matrix R B2W ??. The Euler angular rates is given by 7. The output feedback regulator can be designed by measuring the acceleration and angular rates in the body-frame and heights in the inertial-frame, an estimator can be used to obtain all the desired 12 state variables and feed them back to the controllers. Also, the physical experiment will need to performance with the real system Rolling Spider Drone to demonstrate the algorithms devised.