Feedback Control of Quad-Rotors with a Matlab-Based Simulator

: Precise regulation of position and attitude (roll, pitch and yaw) is one of the most critical tasks to be considered in designing controllers for quadrotor types of Unmanned Aerial Vehicles (UAVs). The MATLAB-based simulator for quadrotors using PD-feedback control to achieve stabilization was developed. MATLAB/Simulink models are used to design and validate the control algorithms and the simulation results are presented. The quadrotor non-linear dynamics and kinematics were implemented using MATLAB S-function to generate a continuous state output. The simulation results are presented graphically and quantitatively. The developed MATLAB GUI interface is robust and interactive to implement the different control algorithms under various operating conditions.


Introduction
Mobile robot control has been the focus of active research in the past decades. During the last decade, with the advance in relevant technology, the demand of flying mobile robots or Unmanned Aerial Vehicles (UAVs) has rapidly increased. UAVs emergence also has to do with the simplicity of their construction and maintenance, their ability to hover and their Vertical Take-Off and Landing (VTOL) capability Armah (2015).
Rotorcraft UAVs have a variety of configurations that include conventional helicopter with a main and tail rotor, a coax with counter-rotating coaxial rotors and quad-rotors Corke (2011). Quad-rotor types of UAVs has four rotors that are controlled independently. The movement of the quadrotor results from changes in the speed of each rotor. The structure of quadrotor in this research paper is assumed to be rigid and symmetrical, the center of gravity and the body fixed frame origin are coincided, the propellers are rigid and the thrust and drag forces are proportional to the square of propeller's speed. It is also assumed that the earth is flat and non-rotating, which is a valid assumption for quad-rotors.
This research presents application of PD-feedback control to achieve stabilization for roll, pitch, yaw, altitude and position motions. MATLAB/Simulink models are used to design and validate the control algorithms and the simulation results are presented. The PD-gains in this research were obtained by tweaking the various values to obtain satisfactory responses. The quadrotor's non-linear dynamics and kinematics were implemented using MATLAB S-function to generate a continuous state output. Robust and interactive MATLAB GUI interface developed to implement the different control algorithms simulation models is also presented. Figure 1 shows the notation for the quadrotor model Hassan et al. (2013) and http://scholarsarchive.byu.edu/cgi/viewcontent.cgi?articl e=2324&context=facpub (2015). Different coordinate frames are used in identifying quadrotor's location and attitude and translation and rotation matrices can then be applied to transform one coordinate reference frame into another. Quadrotors has three main coordinate systems attached to it; the body-fixed frame, {B}, the vehicle frame, {V} and the global inertial frame, {I}. There are two other coordinate systems, not shown, that are of interest, the vehicle-1 frame, {V 1 } and vehicle-2 frame, {V 2 }. Frame {V 1 } is obtained by rotating frame {V} about the Z V -axis by a positive yaw angle, ϕ V , assuming no rolling or pitching, so that X V and Y V are aligned with X B and Y B respectively. Frame {V 2 } is also obtained by rotating frame {V 1 } in a right-handed rotation about the Y V 1 -axis by the pitch angle, θ V 1 , assuming no rolling, so that X V 1 and Y V 1 are aligned with X B and Y B respectively. , , , , , , , , , , ,
The quadrotor has four rotors, labeled 1 to 4, mounted at the end of each cross arm. The rotors are driven by electric motors powered by electronic speed controllers. The vehicle's total mass is m and d is distance from the motor to the center of mass.
The forces and the moments on the quadrotor is primarily due to gravity and the four propellers and since there are no aerodynamic lifting surfaces, it will be assume that the aerodynamic forces and moments are negligible. The total upward thrust, T, on the vehicle is given by: Where: ω i = The relationship between the rotor speed The upward thrust T i , is defined as Corke (2011): where, α>0 is the lift constant that depends on the air density, the cube of the blade radius, the number of blades and the chord length of the blade Corke (2011). The translational dynamics of the vehicle in the world coordinates is given by Newton's second law: Where: Where: g = The gravitational acceleration B V R = The rotation matrix from frame {V} to frame {B} given by Equation 19 Substituting Equation 5 into Equation 4 and rearranging, to obtain: where, J is the constant rotational inertia matrix of the vehicle given by Guenard et al. (2008): And if the airframe's mass distribution is assumed to be symmetrical with respect to frame {B}, Where: m c = Mass of the quadrotor's center (assuming a spherical dense center, with radius r) m e = The mass at the end of each cross arm, where the propellers are located The pair wise differences in rotor thrust cause the vehicle to rotate. The torque about frame {B} x-axis, the rolling (positive) torque, is given by: Substituting Equation 2 into Equation 11, to obtain: And similarly for the y-axis, the pitching (positive) torque is: Due to Newton's third law, the drag of the propellers produces a yawing torque on the body of the quadrotor. The aerodynamic torque is given by Corke (2011): where, b is the torque constant, which depends on the same factors as a. This torque exerts a reaction torque on the airframe which acts to rotate the airframe about the propeller shaft in the opposite direction to its rotation. Therefore the total yawing (positive) torque is given by Hassan et al. (2013): where, the different signs are due to the different rotation directions of the rotors, thus a yaw torque can be created simply by appropriate coordinated control of all four rotor speeds. The forces and torques on the quadrotor's airframe, obtained by combining Equation 1, 12, 13 and 15, can be written in matrix form as Corke (2011): Which gives the rotor speeds required to apply a specified thrust T and torque Γ B to the airframe, where: The matrix C is of full rank if a, b, d>0, thus making the vehicle controllable.

Control Algorithms
Control of the quadrotor input, ( ) about applying the appropriate thrust, T and/or torque, Γ B , to the airframe, which will be determined using the traditional PID-feedback controller, given by Katsuhiko (2002): Where: e = Defined for each task below, is the error between the desired value and the output value K p = The proportional gain K I = The integrator gain K D = The derivative gain and t is time In general the dynamics of rotational systems has a second order transfer function of the form Corke (2011): Where: B = The aerodynamic damping J = The rotational inertia Θ(s) = The output signal (e.g., pitch) τ(s) = The input signal (e.g., pitching torque) B is generally quite small, thus the integrator controller is not necessarily in the regulation of the quadrotor and therefore PD-controller is applied in this research.

Attitude Controllers
This section presents control algorithms that make quadrotor pitch, roll and yaw. The PD-controller is used to determine the required torques based on the error between desired angles ( ) and ϕ θ ∅ ɺ ɺ ɺ are small and can be ignored.

Controlling Pitch Motion
The pitch is controlled by increasing the speed, which in turn increase the thrust, of either rotor 1 or 3, whiles keeping the speed of rotor 2 or 4 the same or zero, as illustrated in Fig. 2. The required pitching torque on the airframe is given by:

Controlling Roll Motion
Similarly, the roll is controlled by increasing the speed, which in turn increase the thrust, of either rotor 2 or 4, whiles keeping the speed of rotor 1 or 3 the same or zero, as illustrated in Fig. 3. The required rolling torque on the airframe is given by:

Controlling Yaw Motion
The yaw is controlled by simultaneously applying the same speed, which in turn changes the thrusts, of rotors 2 and 4 whiles keeping speeds of rotors 1 and 3 the same or zero or vice versa, as illustrated in Fig. 4. The required yawing torque on the airframe is given by:

Position Controllers
This section presents control algorithms that make quadrotor undergoes translational motions in the x, y and z directions. Again PD-controller is used to determine the required control inputs based on the error between

Controlling Altitude Motion
The altitude is controlled by simultaneously applying the same speed, which in turn changes the thrusts, of all the four rotors, as illustrated in Fig. 5. For upward motion the total thrust, T, must be bigger than the weight, mg, of the quadrotor, taken into account the drag on the vehicle. The total thrust on the airframe is given by: where, the additive term is given as: ω o is the average rotor speed necessary to generate a thrust, T o , equal to the weight of the vehicle.
A feed-forward control approach-used to counter the effect of gravity, which otherwise is a constant disturbance to the altitude motion. The alternative approach would be to have very high gains for the PDcontroller. This second approach, not used in this research, often leads to actuator saturation and instability (Corke, 2011).

Controlling Motion in the x and y Directions
To move the vehicle in the x-direction (along X V its nose pitch down, which generates a force Corke (2011): which gives the force component that accelerates the vehicle in the X V -direction as: where, θ v is small. The velocity in this direction can be controlled using the P-controller (Corke, 2011): Combining Equation 29 and 30, the desired pitch angle required to achieve the desired forward velocity is obtained as: For a vehicle in vertical equilibrium the total thrust is equal to the weight of the airframe, thus m/T = m/T o in Equation 31 can be approximately substituted by g −1 .
Now, the desired velocity in frame {V} relative to frame {I} is then determined as (Corke, 2011): where, the desired velocity in frame {I} is given by the P-controller: Thus, to reach a desired x-position, the appropriate velocity is calculated and from that the appropriate pitch angle which will generate the force to move the vehicle is obtained. This indirection is consequence of the vehicle being under-actuated-the vehicle have just four rotor speeds to adjust but its configuration space is 6dimensional. Substituting Equation 33 into Equation 32 and then the result into Equation 31, a compact form control algorithm for computing the desired pitch angle can be obtained as: Similar analysis can be carried out to obtain the control algorithm to move the vehicle in the y-direction (along Y V ). To reach the desired y-position, the appropriate velocity, * V y v , is calculated and from that the appropriate roll angle, * V ∅ , which will generate the force to move the vehicle is obtained. The compact form control algorithm for computing the desired roll angle is also given as: To move the vehicle forward or sideway its airframe must first pitch down or roll down respectively so that the thrust vector has a horizontal component to accelerate it; the total thrust must be increased so that the vertical thrust component still balances gravity. As it approaches its goal the airframe must be rotated in the opposite direction, pitching up or rolling up, so that the backward component of the thrust decelerates the forward or the sideway motion. Finally, the airframe rotates to the horizontal with the thrust vector vertical. The cost of the under-actuation is once again maneuver. The pitch and the roll angles cannot be arbitrarily set, they are means to achieve the translation control. Equation 34 and 35 can be combined as: where, ( ) ( )

Simulation of the Control Algorithms
The simulations were carried out using MATLAB/Simulink models to design and validate the control algorithms. Simulink block, Quadrotor Dynamics, as shown in Fig. 6, is used to implement the dynamics and kinematics of the quadrotor discussed in Section II. The block employs a MATLAB S-function to generate a continuous state output, x.
S-functions (system-functions) provide a powerful mechanism for extending the capabilities of the Simulink environment. S-functions follow a general form and can accommodate continuous, discrete and hybrid systems. An algorithm in an S-function is implemented by following a set of simple rules and using an S-Function block to add it to a Simulink model. The block consists of a set of inputs, a set of states and a set of outputs, where the outputs are a function of the simulation time, the inputs and the states.
The Quadrotor Dynamic block is connected to a Control Mixer block, whose inputs are the three torques and the total thrust acting on the airframe. The Control Mixer block is used to compute the four rotor speeds, which serves as inputs to the Quadrotor Dynamics block. These blocks were developed by using Simulink models in Corke (2011).

Simulink Models for the Controllers
This section discusses various Simulink models developed for the different control algorithms discussed in section III.

Altitude Controller
The Simulink model shown in Fig. 7 is the altitude control algorithm. For a given desired height, z * , the altitude control loop, based on Equation 26, is used to generate the required thrust, T, whiles the torque, Γ B , to the airframe is set to zero.

Yaw Controller
The Simulink model shown in Fig. 8 is the yaw control algorithm. The vehicle must first moved to a desired height before yawing, hence the altitude controller is combined with the yaw controller, with an if-else control blocks to implement the conditional statement. Thus, for a desired yaw, yaw*, the altitude control loop drives the quadrotor to a desired height, z * and based on Equation 25, the yaw control loop is used to generate the required yaw torque, B ϕ τ , whiles keeping the other two torques on the airframe at zero.

Pitch Controller
The Simulink model shown in Fig. 9 is the pitch control algorithm. The vehicle must first moved to a desired height before pitching, hence the altitude controller is combined with the pitch controller, with an if-else control blocks to implement the conditional statement. Thus, for a desired pitch, pitch * , the altitude control loop drives the quadrotor to a desired height, z * and based on Equation 23, the pitch control loop is used to generate the required pitch torque, B θ τ , whiles keeping the other two torques on the airframe at zero.

Roll Controller
The Simulink model shown in Fig. 10 is the roll control algorithm. The vehicle must first moved to a desired height before rolling, hence the altitude controller is combined with the roll controller, with an ifelse control blocks to implement the conditional statement. Thus, for a desired roll, roll * , the altitude control loop drives the quadrotor to a desired height, z * and based on Equation 24, the roll control loop is used to generate the required roll torque, B τ ∅ , whiles keeping the other two torques on the airframe at zero.

Position Controller
The Simulink model shown in Fig. 11 is the x and y directions control algorithms. The vehicle must first moved to a desired height before moving to the desired position in the xy-plane and the yaw angle, ϕ, is also needed, hence this model combines the altitude controller, yaw controller and the xy-plane motion controller.

MATLAB GUI for the Simulations
A screenshot of a robust and interactive MATLAB GUI interface developed to implement the various control algorithms simulations discussed above is shown in Fig. 12.
The GUI interface has five sections: Initialization and information about the quadrotor settings, selecting the type of controller and its parameter settings, displaying the quadrotor current attitude and position values, displaying some performance index values of a run simulation and plotting the quadrotor's response and trajectory.
The GUI interface works by first initializing the quadrotor's parameters and then selecting the type of controller and its parameter settings required for the simulation. The selected controller is then simulated using its parameter settings selected. The quadrotor attitude and position real-time state values and responses can be displayed and the 3D trajectory of the quadrotor can also be visualized. An animated quadrotor is graphically designed, which receives the data from the simulation and execute the response in real time. After the simulation some performance index values are displayed.
The quadrotor real-time response and trajectory plots, shown in the GUI, were achieved by using Sfunction block in the Simulink models. The quadrotor real-time attitude and position state values, displayed in the GUI, were also achieved by using MATLAB Output block run-time object and an event-listener mechanism add_exec_event_listener command and then synchronizing the run-time object with the Simulink execution. The state output, x, is the input to the two blocks. Table 1 gives a summary of the basic physical parameters used for the quadrotor Becker et al. (2012).

Simulation of Individual Controllers Results
All the time domain Simulink simulations were carried out under 10 sec duration for each model (refer to the simulation setups in the Fig. 7-11). The responses, using the various control algorithms, are shown in the Fig. 12-16. The PD-gains in this research were obtained by tweaking the various values to obtain the satisfactory responses. The various PD-gains, some performance indicators.  Even though there were steady-state errors in the various values obtained, the results were encouraging. The errors in the steady state can be due to using PD-gains which were not determined from classical control design approach but by tweaking. Another possible cause of error in the steady state is in the modeling of the dynamics and the kinematics of the quadrotor such as unmodeled aerodynamic effects and uncertainties (e.g., wind disturbance and saturation). Furthermore, according to Brockett's conditions Brockett (1982), feedback control law, which is a continuously differentiable, time-invariant, cannot be used to obtain error-free stabilization.
Effect of the PD Gains on the Altitude Response

Conclusion
This study demonstrated design of feedback controllers for position and attitude regulation of quadrotor UAVs. Also, MATLAB/Simulink models are used to design and validate the control algorithms. The simulation results are presented graphically and quantitatively. A robust and interactive MATLAB GUI interface for implementation of the various control algorithms simulation models is developed. Even though the final steady-state values obtained from the experiments are affected by unexpected disturbances, noise and modelling errors, the simulation results were encouraging.
In future, the authors will implement these PDfeedback controllers developed on the Parrot AR. Drone. In fact, an attempt has been made at the implementation, but challenges and difficulty were encountered. The main difficulty was in using the S-function to implement the dynamics and kinematics of quadrotors. The problem is in C-code generation for real-time application.
In order to address this problem, linearized models are being developed to represent the dynamics of the quadrotor for the different controllers. Another approach being study is to use system identification to determine the models based on data collected from the Parrot AR.
Drone. These approaches will allow the use of a classical control design approach to determine robust control laws instead of the tweaking approach used in this research to determine the PD-gains.
Performance, in terms of efficiency and accuracy, will then be compared of controllers such as poleplacement, Linear Quadratic Regulator (LQR), Linear Quadratic Gaussian (LQG), Model Predictive Controller (MPC) and Model Reference Adaptive Control (MRAC). In addition, robust control algorithm considering time delays will be studied.