Optimized PID Control of Quadrotor System Using Extremum Seeking Algorithm

This paper presents the development of an automatic tuning of conventional PI-D controllers for Quadrotor system. The most critical problem of tuning the parameters of the PID, is that they can drastically affect the performance of the system. Accordingly, Extremum seeking (ES) algorithm is utilized here to reduce a selected cost function that brings the required performance aspects. The results show that the ES-PID controller provides better stability and performance as compared with only PI-D controller. The parameters of ES-PID controller have been successfully tuned and modified to reach the desired destination with significantly less overshoot, oscillations, and faster convergence.


INTRODUCTION
A quadrotor or drone is a type of helicopter with four rotors. They are well suited to autonomously perform complicated civilian or military operations. Automated drones can complete a wide range of applications [1,2]. Automated drones can be used in oil & gas facilities for security [3], surveillance [4], emergency response [5] and infrastructure inspection [6]. Quadrotor is generally a small aircraft with multi-rotors, with nonlinear, under-actuated systems. When controlling the thrust, and consequently, the velocity of each one of the rotors, a motion in three paths (x, y, and z) and the rotation around three axes (Roll, Pitch, and Yaw) are generated. The quadrotors under-actuated property will cause a strong coupling between rotary and translational motions. As a result, it is a difficult task to control the trajectory tracking or the position of the quadrotor [7]. To overcome this issue, many control techniques have proven to provide positive performances, and to solve the problems of trajectory-tracking.
Recently, many works have been developed to model and control these platforms. An adaptive state feedback control method denoted as Classical Multiple Model Adaptive Control had been implemented by Raafat and Mahmoud (2018) [8], where each Kalman filter had been designed for a specific value of an equilibrium point with LQ-Servo controller. Suitable performance and tracking design have been achieved. Another study is done by Alawsi et al. (2020) [9], where a multimode control algorithm has been proposed as a solution for GPS signal denied through the autonomous flight, where it is based on GPS and accelerometer modes with Unscented Kalman Filter (UKF). The proposed algorithm can keep the quadrotor under control during GPS signal denial period and continue moving toward the desired path.
Kotarski1et al (2016) [10], designed a quadcopter control algorithm to improve the performance of standard PI-D controller. The implemented PI-D controller achieved an improvement in robustness, stability, and error boundedness. The utilization of PI-D Controller stops the control system from getting higher error signal (e) situation considering use of the derivative of the state rather than the error in its equation, the (D) derivative term works on the calculated plant output instead on the error signal. This PI-D controller avoids big control efforts throughout a step-change in the reference signal. Typically, tuning of PI-D parameters is commonly done manually. However, the effectiveness of the tuning is commonly based on the control engineer experiences. Also, in most practical scenarios, the plant model is undiscovered or just approximately known. Hence, a strategy for tuning the PID controller is very appealing in the closed loop setting [11]. Several approaches were developed to achieve this purpose. One of the most well-suited approach for achieving automated PID tuning, which is the Extremum Seeking Control (ESC) [12], where it utilizes a cost function that is based upon the difference between the real response and the preferred response. The output of the cost function will then be fed back to the extremum seeking (ES) loop to be able to tune the three parameters , and of the PID controller [13,14]. The ESC is a paradigm that aims to find an extremum value of an unknown nonlinear mapping. Therefore, it had been shown that it is useful to apply ESC to tune PID controller. Killingsworth and Krstic (2006) [15], presented an enhanced PID controller with discrete type of ES. Mainly, ES reduces a cost function that quantifies the accuracy of the PID controller. ES which is a non-model-based technique, iteratively modifies the cost function arguments (PID parameters) to ensure that the cost function output goes to a local minimum. ES technique has an advantage than standard PID model in some applications with actuator saturation. Moreover, the ES cost function may be specified to reflect the preferred performance attributes. Guay and Dochain (2014) [16], proposes an alternative design approach of ESC as a solution of real-time enhancement control issues. The approach considers a proportional integral method that avoids the requirement for a timescale separating in the formulation of the ESC. Raafat et al. (2015) [17], proposed a real-time optimized (PID) controller for a three Degree of Freedom (3DOF) helicopter. Similarly, Raafat and Hussein (2018) [18], developed an ES strategy for tuning PI controller to maximize the power capture in a variable speed variable pitch wind turbine (VSVPWT).
In this work, ESC has been developed for tuning PI-D control parameters. Six PID controllers has been used for (x, y, z, roll, pitch, and yaw) in the feedback and six PI controller in the feedforward at the reference signal. The ES algorithm is used in order updates the PI-D controller parameters to minimize the cost function which is computed from a step-response experiment. Then, we compared the results from our proposed model with traditional PID controller for three cases, displacement, when subject to uncertainty and the disturbance rejection performance.

MODELLING OF QUADROTOR:
Quadrotors are typically modeled and flown in two configurations: the (cross "x") and the (plus "+") configuration, as shown in Fig.1, where two motors rotate clockwise, and the two other motors rotate counterclockwise. This strategy helps the quadrotor to not rotate on its vertical axis as the rotational inertia is canceled out, discarding the requirement for a tail rotor that is used to balance the conventional quadrotor [7,19].
The quadrotor has 6 degrees of freedom, three rotational and three translational, as in Figure 1. The location on the x and y axes is named as such, although the position on the z-axis is described as altitude. Also, the orientation angles measured are referred to as Roll (φ), pitch (θ), and yaw (ψ), to be the rotation angles over the x, y, and z-axis, respectively. The ψ angle may also be referred to as heading [19].
The motors are numbered from 1 to 4, and each motor has its separate motor signal, denoted for each motor. When every motor rotates at the similar angular velocity, then the torques , and (these are generally the counter torques given to the aircraft because of the motor's rotation) will terminate each other out, and therefore the quadrotor is not going to spin about its -axis (ψ˙ = 0). The quadrotor is going to hover in case the angular velocities are in a way that the total thrust ( + + + ) made by the rotors is corresponding to the gravity force. To be able to identify the quadrotor movement and its attitude, a couple of reference frames are used, which is, the inertial and the body frame (see Fig. 1). The inertial frame is specified by the ground, with gravity pointing in the negative z-direction. The body frame is specified by the quadrotor orientation, with the rotor axes positioned in the positive direction and the arms positioned in the and directions. The quadrotor attitude is concluded by three angles, which are terms, roll-φ, pitch-θ, and yaw-ψ [14,20]. The equations of quadrotor linear motion are [21]: where , y, and z are the coordinates of the position of the quadrotor in the inertial frame, is the system mass, g is the acceleration related to gravity, is the drag force due to air friction, is the thrust vector in the body frame, and ( )is the overall rotation matrix. The drag due to air friction is modeled as a force relative to the linear velocity in every direction: where, is the coefficient of air friction. The thrust generated by the rotor (where n=4 rotor for a quadrotor) is given by the following expression: where is the friction constants; is the motor angular velocity.
The total thrust produced by the 4 rotors is determined by following equation: The overall rotation matrix ( ) is equal to ( ) ( ) ( ), then it is given by: where, =cos, =sin, At this point, it is useful to use Euler's equations designed for rigid body dynamics, that can be defined as follows: where"×" denotes cross product, = [ ] is the vector of external torques, is the inertia matrix, and [ ] is the angular velocity vector. The Body Frame Dynamics is: where, and are the moments of inertia of the quadrotor about the , and axes respectively. The roll moment and pitch moment torques can be derived from the essential mechanics as follows [22]: where is the distance between motor and the quadrotor centre (radius). The following equation gives the total torque about the -axis that is the yaw τψ torque: where, is the coefficient of propellers drag. The Roll, Pitch, and yaw rates are relating to the factors of the angular velocity vector utilizing the following expression: where, = tan.
The quadrotor motors generate Roll, Pitch, and yaw torques in the path corresponding to the main body frame [23]: where, is the distance from the quadrotor center to any of the propellers. To control the angles and throttle, the control signals , , , and are used [23].

4-Yaw Moment
Where is the distance from the centre of the quadrotor to any of the propellers; , and are input forces to the quadcopter generated by motors.
The overall rotation matrix R(Θ) is equal to ( ) ( ) ( ), then it is given by: By substance equations (4) and (15) in (1) we will get [ Then we get: The angular Euler-Lagrange equations are [] The matrix C (η, η˙) has the form, as shown in [23], By compensating the value of η and value of matrix C (η, η˙) in equation (26), and the following angular acceleration equations can be found as: Equations (20,21,22) and (27, 28 29) represent linear motions and angular motions of the quadrotor, respectively.

PI-D CONTROLLER DESIGN:
The main objective of deriving the quadrotor mathematical model is to help in develops of controllers for real quadrotors. The inputs of the proposed system involve the angular velocities of every rotor, as the voltages across the motors are all that can be controlled.
As specified in [23], PI-D control or three-term' control is based on computing the error between measured state and the desired set point along with the change rate of this error and the integration of this error. The controller output is a signal that is relative to the sum of all these errors, each one multiplied by a constant. To control the quadrotor using PI-D or PID -ES control, six single-input and single-output controllers are required to control ( , and ) (heading), and movement in the directions. The 1st step is to balance the ( , and ).
The mathematical form of PI-D is given as follows: The errors can be computed from the following equation: The required thrust force, , can be calculated by: where, , , and are altitude PI-D gains controller; is the earth's gravity and is the altitude error. To control attitude, three control signals are utilized to obtain the required quadrotor attitude. These signals are utilized to control the Roll ( ), Pitch ( ), and Yaw ( ) angles, which describe the warp angles of the quadrotor's body over the x, y and z axis, respectively. To calculate the controlling signals, a number of equations is used for each direction, which can be specified as in follows [24]: -The torque to rotate the quadrotor over the x-axis can be computed by: where: , and are roll angle PI-D gains controller; is roll angle error. - The torque to rotate the quadrotor over the y-axis can be computed by: where: , and are pitchangle PI-D gains controller; is pitch angle error. The torque to rotate the quadrotor over the z-axis can be computed by: where: , and are yaw angle PI-D gains controller; is pitch angle error.
The quadcopter has 6DOF and just Four actuators (motors) therefore it is impossible to control each one of these DOF straight. Thus, the control equations (21) to (24) are utilized to control Four DOF directly (z, ϕ, ϴ, ψ). The roll and pitch angles allow to move quadcopter in the direction of the desired x and y direction. Hence, the desired x and y can be utilized first to determine the desired roll and pitch angles to make it possible for controlling x and y positions indirectly. Thus, to calculate the desired roll and pitch angles, several equations have been used which are generally written in terms of required x and y accelerations. The equations that used to determine desired acceleration through desired x, y and z are [7]: The relations between the required linear accelerations with regards to the inertial frame, and the required related angles roll, Pitch and yaw for the body frame are described in following equations [7]:

EXTREMUM SEEKING (ES) TUNING OF PID CONTROL:
ES is an adaptable algorithm that iteratively improves the controller parameters using a cost function resulting from the output error signal of the closed-loop system. ES is a non-model-based approach that iteratively changes the input of a function in ways that the output of the function has come to the maximum or a local minimum [15]. ES makes this by perturbing the input variables of the system and then setting up an online estimation of the output gradient related to the inputs. This gradient data is then employed to adjust the input parameters in the following iteration toward the maximum and minimum of the output.
The cost function ( ⃗ ), which quantify the efficiency of a given PI-D controller, is evaluated during the application of ES-PI-D tuning experiment. In this work, we use the integrated square error (termed ISE) cost function [14]: where the error ( ⃗ ) ( ) ( ⃗ )is the main difference in between the reference signal and the output signal belonging to the closed−loop system and includes the PI-D parameters. The structure of PI-D controller is described by Eq. (19), where the meaning of , , and are given by ⃗ (42) Figure 3 illustrated the implemented closed-loop controlled system, where the controller is parameterized as: where is the reference signal, is the output signal and is the control signal. The y signal is regulated to the r signal by the 2DOF controller, and as shown in Figure 3. The implementation of time-domain of the discrete-time extreme seeking algorithm in Figure  2can be described as in follows: where: ( )It is a scalar, is the subscript signifies the entry of a vector. is the perturbation amplitude and , is the adaptation gain.
Convergence and stability are affected by the values of , and the cost function ( ⃗ )shape is nearthe minimizer. The modulation frequency is selected in a way that = , where a satisfies . In addition, the high-pass filter ( ) ( ) is designed with and a cut-off frequency which is under the modulation frequency .
From Figure 3, The ES algorithm updates the PI-D controller parameters θ(k) to minimize the cost function ( ⃗ ), which is computed from a step-response experiment carried out within the dashed box. However, the dashed box in figure 3 represents the step-response experiment, which is run iteratively. The cost ( ⃗ ( ))is calculated after the step-response experiment. The extreme seeking algorithm utilized the cost function value ( ⃗ ( ))in order calculated new controller parameters ⃗ ( ) An additional step function experiment will then be implemented with the new controller parameters, and then the process proceeds iteratively.
The proposed control structure consists of six PID controllers (for x, y, z, roll, pitch and yaw) in the feedback and six PI controller in the feedforward at the reference signal, so ⃗ contains 30 elements to be tuned. They include Θ1 for position parameters and Θ2 for acceleration parameters. The overall ES-PID tuning scheme is illustrated in Figure 3.

SIMULATION RESULTS:
The simulation and visualization of quadrotor have been designed by MATLAB software based on the system's model, as described in Section 2. For our test, we tested the proposed auto-tuning PI-D controller (ES-PID) controllers and compered them with PI-D of best gain parameters used for the same model. The Quadrotor parameters, PI-D and ES-PID control parameters used in our simulation are given in Table I, Table II and Table III.   The desired position in x, y, z coordinates of (10; 13; 6) have been used in all simulations. The previously described PI-D controller has been used to drive the quadrotor to the desired position, in which we compared both the controller in both driving error and response time. Also, we applied two turbulence factors that can be affected by the quadrotor, which are disturbance uncertainty.

I. Displacement Results
In this part, we simulate the quadrotor displacement and compare results for PI-D and ES-PID controller. The results are shown in Figures 4, 5 and 6.
From figure 4 (a), Figure 5 (a) and Figure 6 (a), both controllers start to guide quadrotor from zero position of x, y, z= [0, 0, 0]to the desired position of [10,13,6]. However, we can notice that ES-PID control is more accurate than PI-D control on both of stability and response time. By checked results, the ES-PID control is uniformly raised until reach to close from desired distance which is about 10.05m (i.e., error about 0.05m) and in about 9 sec while the results from PI-D control is non-uniform which is raised and down until reach stability at a distance near desired distance of about 10.08m (i.e., error about 0.08m) and in about 12 sec. From figure 5 (a), the ES-PID control is uniformly raised until reach to close from desired distance which is about 13.05m (i.e., error about 0.05m) and in about 9 sec while the results from PI-D control is non-uniform which is raised and down until reach stability at a distance near desired distance of about 13.1m (i.e., error about 0.1m) and in about 11 sec. From figure 6 (a), the ES-PID control is uniformly raised until reach to close from desired distance which is about 10.02m (i.e., error about 0.02m) and in about 10 sec while the results from PI-D control is nonuniform which is raised to 6.55m and down until reach stability at a distance near desired distance of about 10.02m (i.e., error about 0.02m) and in a relatively long time of about 16 sec. From figure 6 (c), we can notice that ES is tuning the parameters ( , , ) of the PID controllers and modifies them until reaching the desired displacement, and as compared with another algorithm [18], where PSO algorithm had been used, ES algorithm achieves better tracking performance.

II. Disturbance Rejection Performance of the Controllers:
In this part, we test the disturbance rejection performance of both controllers. In this test, we applied disturbance at 10 seconds. The results are shown in Figure 7.
Simulation results for disturbance effect on quadrotor displacement for From Figure7 (a), (b), and (c) as previously, both controllers start to guide quadrotor from zero position to the desired position. From the results, we can see the effect of disturbance started from the tenth second. Both controllers start to overcome disturbance and driven quadrotor to the desired value. However, ES-PID is starting to reduce disturbance faster than PI-D. From Figure 7 (a), the ES-PID starts to reduce the effect from about10.4 seconds reach the desired value in the fifteenth second, while PI-D is starting at 11second and reach the desired value in the fifteenth second. From figure7 (b), the ES-PID starts to reduce the effect from about10.4 sec and reach the desired value in the 17th second while PI-D is starting at 10.8 seconds and reach to the desired value in 17th second. From Figure 7 (c), the ES-PID is starting to reduce instantly from about10th sec and reach the desired value in the 17th second; however, the PI-D starting also reduce instantly from about10thsecond and reach to the desired value in 17th second. From these results, the ES-PID achieves better disturbance rejection performance than PI-D only.

III. Effect of the system Uncertainty
The nominal value of mass (m) in our application is 0.8kg, when we use a PI-D controller and ES-PID controller. In this part, we test the quadrotor controls for controlling the uncertain nonlinear to minimize the effect of the system uncertainties due to the uncertainty in the mass. For this purpose, we made a high variation of mass value in every second, these mass values are varied from (0.5 to 5) kg. The results are shown in Figure 8  For Figure 8, both controllers start to guide quadrotor from zero position to the desired position. From results, we can see the effect of uncertainty started from the beginning, and there are acute effects when there is a high variation. However, both controllers start to overcome uncertainty and driven quadrotor to the desired value. From results, PI-D is suffering from instability when compared with the ES-PID that shows uniform change and stability. From figure 8 (a), the ES-PID is start driven quadrotor to desired x position. It not affected by uncertainty. At the same time, there are acuate effects on PI-D from starting to 10th second and reach near desired value in the 11th second. From figure 8 (b), the ES-PID is start driven quadrotor to desired y position. It is also not affected by uncertainty, while there are acuate effects on PI-D from starting to 10th second. From figure8 (c), the uncertainty is more effected in z position than x, y position; however, ES-PID has successfully driven quadrotor to desired z position at 12th second smoothly, while there are acuate effects on PI-D from starting to 18th second.   In Table IV and table V we described the main defenses between the proposed model and traditional PI-D controller in term of overshooting, SteadyState error and setting time.

CONCLUSION
In this study, the advantage of using ES optimization algorithm to tune PI-D controller for quadrotors is investigated. Two controllers have been compared, which are: PI-D and ES-PID that are presented with the same disruptions and by using the exact derivative and quadrotor parameters. The test results show that the ES-PID controller is better than the PI-D controller at avoiding steadystate error. The results also show that the ES-PID controller tuning is successfully tuned gain parameters ( , , ) and modified them until they reach the desired displacement. The automatic selection of PID parameters is considerably better in overall. The ES tuned PID controller noticeably have less overshoot, a little swing in value, and faster converge, as we have seen in all results of uncertainty and disturbance the preference of the proposed method for all parameters of the model the obvious improvement. This is a result of the fact that the cost function of ES is emphasizes squared error, and therefore offers priority to reducing total error magnitude instead of long-term convergence.