The quadrotor dynamic modeling and study of meta-heuristic algorithms performance on optimization of PID controller index to control angles and tracking the route

Received May 12, 2020 Revised Jun 7, 2020 Accepted Aug 4, 2020 In the last decade, because of the unique specification of vertical fliers, scientists and researchers had a special focus on them. The particular abilities of these fliers can be mentioned such as: high maneuver ability, low expenses, decrease in radar identifier and low threat for the human life. They also have no limitation in dimension. Moreover, because of some applications like photography, topography, news coverage, study of power lines and aerology analysis, they can be notable for using. These fliers also are significantly important because of monitoring in urban regions, agricultural harvest and spray poison, illegal imports, exports administration and fire distinction in order to control the fire. Besides, seek and rescue missing people and also natural disasters can be pre-determined which causes stimulus investigators to act and put different topics in front of them. One of these fields is using meta-heuristic algorithms with the capability of using in control systems. The PID controller as a classic model has some limitations, but by optimization of special index through meta-heuristic algorithms, it has shown acceptable results. In this study, first, the history of vertical fliers and quadrotor are investigated. Then, after a review of overused methods, the quadrotor control has been done. Afterward, the cinematic and dynamic of quadrotor is presented. Next by designing of PID controller, PID index optimization by nature inspired algorithm, particle swarm optimization (PSO), genetic algorithms (GA), and firefly algorithms (FA) have been studied. Dynamic system, controller and mentioned optimization methods of PID controller index have also been implemented in MATLAB software. Also, with due attention to the comparison criteria the PID-PSO controller has shown the best performance. Next, by applying challenging routes, the stability of controller in the simulation is evaluated. Then, making quadrotor is done in practice along with introducing the used implementation of PID-PSO controller results on the real robot, and its stability is evaluated practically.


INTRODUCTION
In recent years, quadrotor usage to do different tasks is raising. High maneuverability, low expenses, decrease the use of radar identifier, low threat for human life and no limitation on dimension, are the factors of leading to more attention [1]. The quadrotor is an unmanned flier with the frame shape of X and H. It is

SYSTEM MODELING
The system modeling included cinematic and dynamic quadrotor, driven by Newton-Euler equations. In order to study the quadrotor movement Its needed to have two coordinate systems [7]. One related to the ground coordinate device and another related to space which requires Euler's angles. In Euler's angles the angles are resulted from the rotation of the body coordinate system and relative to the earth coordinate device (XYZ) as follows. a. Euler angles For finding position of quadrotor in the space, the frame coordinate system should be placed toward the ground coordinate system. The position of quadrotor in the space is described by location vector, but to define its orientation in the space, the Euler angles are required [7]. These angles are the results of spinning around axis: Φ: roll angle (rotating around X-axis), θ: pitch angle (rotating around Y-axis), ψ: yaw angle (rotating around Z-axis). See Figure 1. As its name implies Earth frame considering ( Figure 1) as an inertial frame it uses the N-E-D notation where the axes point to the north, east and downwards respectively. Besides, the body frame is at the center of the quadrotor body, with its x-axis aiming towards propeller 1, y-axis pointing towards propeller 2 and the z-axis is indicating to the ground. The quadrotor orientation is described by using roll, pitch, and yaw angles (φ, θ and ψ) representing rotations about the x, y, and z-axes, respectively. Considering the order of As it shown in (1), c and s represent cos and sin as the derivation of the rotation. The rotation matrix R will be used in framing the dynamics model of the quadrotor, and its importance is due to the fact that some states are measured in the body frame (e.g. the thrust forces shaped by the propellers) although some others are measured in the inertial frame (e.g. the gravitational forces and the quadrotor's position). Therefore, to have a relation between both types of states, observation was done for a transformation from one frame. To obtain data about the angular velocity of the quadrotor, typically an on-board inertial measurement unit (IMU) is hired to have the velocity in the body coordinate frame. Then to relate the Euler rates, as in (2): that are measured in the inertial frame and angular body rates ω = [ p q r ] T , a transformation is needed as (3): where shown in (4): In (4), 'S' is the abbreviation of 'sin' and 'C' is the abbreviation of 'cos' and parameters ϕ, θ, ψ are roll, pitch, and yaw angles, respectively. Its noted that Around the hover position, small angle assumption is made where cos φ ≡ 1, Cos θ ≡ 1, and sin φ ≡ 0, Sin θ ≡ 0. Thus R r can be simplified to an identity matrix I [7]. To present of modeling, the system is required of torque calculation of quadrotor, swirl movement equations, the non-gravitational force of quadrotor and dynamic of rotors, which is describing in the following. b. The quadrotor torque Among applied force to turn, there is a force, called aerodynamics or lifting force and there is a produced torque, called aerodynamic torque which has shown in (5).
In (5) M B is the general equation of torque applied on a quadrotor, 1 is arm quadrotor, K f is an aerodynamic force and Ω 1 is the velocity of the first rotor, Ω 2 is the velocity of the third rotor, and Ω 4 is the velocity of the fourth rotor. c. Swirl movement equations Swirl movement equation of quadrotor is taken from Newton's second law and ground inertia frame which is shown in (6) [7].
As it has shown in (6), r = [x y z] T , which represent the distance of quadrotor to the inertial frame, m (quadrotor weight), g (quadrotor weight) and F B (non-gravitational force applied on quadrotor) and R is the connection of two frames coordinate system of quadrotor toward coordinate system [7]. d. Non-gravitational force applied on the quadrotor Whenever the quadrotor is in the horizontal orientations, the produced force by turning fans is the only non-gravitational force applied on it which is corresponding square angular velocity of fan (6). So, no non-gravitational force is effective on quadrotor. In this case F B is defined such as (7).
In (7), Ω 1 is the first Ω 2 is second, Ω 3 is the third rotor velocity, Ω 4 is the fourth rotor velocity, and K f is the aerodynamic force [7]. e. Rotors dynamics Usually brushless motor (BLDC) used in the quadrotor are DC motors or brushless, which have high torque and low friction. The dynamic of DC and brushless motor, in normal conditions, is like the regularly fixed engines, which has shown in (8).
In (8), K mot is motor torque constant, K M is aerodynamic moment constant, R mot is motor circuit resistance and J r is rotor's inertia [7]. f. The state space model Obtaining the state space model, helps to design of the controller and it is required X sate variable and their corresponding element. g. X state variable Considering the state vector of the quadrotor to be (9): X = [ 1 2 3 4 5 6 7 8 9 10 11 12 ] T (9) which is mapped to the degrees of freedom of the quadrotor in the following manner (10) [7].
In (10), x, y, and z are the positions; ϕ, θ, and ψ are the Euler angles; and their derivation which indicates the component velocity. The state space vector is notable because of the capability of showing position angle and the linear velocity of quadrotor [7]. h. Input variable of the state vector The parameter U as a control input vector containing of four inputs; U1 through U4 is defined as, shown in (11).
In the above relations, k f is aerodynamic forces, k m is fixed torque and  is the angular velocity resulted from rotors. Finally, the relation of (12)- (15) can be shown as the matrix form as shown in (16) [7]. [ According to relation (16), u 1 is the power of the four routers thar gives the quadrotor height from Z to Z. u 2 is the result of the difference in torque between engine and 4, which results in a quadrotor rotation around the roll angle from  to . u 3 is the difference of torque between the motors 7 and 3 that rotates the quadrant around the angle of the pitch from  to . u 4 results from the torque difference between the two pair of motors clock and clockwise and it rotates around the angle of your from  to . The speed of the rotors can be calculated by the control input. Therefore, the inverse relation between the control input and the rotor speed can be obtained as (17) [8]. [ In the above relations,  is the speed of constant torque, k m is aerodynamic force and K f is the angle of rotors. In the relation (17) the control inputs can be calculated by taking root from the rotor speed [9]. At the end, by taking the square root of that, the velocity of rotors can be calculated from the control inputs as shown in (18): Taking the square root of that, the rotors' velocities can be calculated from the control inputs as follows. In the above equation, K f is aerodynamic force, K M is constant torque, and Ω is the angular velocity of rotors. i. Rotational equation of motion It leads to dynamic equations of the system are described in (19) to (21 By select the control input vector U, it is clear that the rotational subsystem is fully actuated, it is only reliant on the rotational state variables x1 to x6 that correspond to φ, φ, θ, θ̇, ψ, ψ̇ respectively. j. Translational equation of motion: Considering (10) and (6) the total inserted torque on the UAV can be written as (22) [7]: which after replacing the parameter in (4), (23) to (25) can be substituted: ẍ= −U 1 m (sin ϕ sin ψ + cos ϕ cos ψ sin θ) ̈= −U 1 m (cos ϕ sin ψ sin θ − cos ψ sin ϕ) Based on mentioned equations, J r is rotors torque, I XX , I YY , I ZZ are the torque from inertia in the major axis of body frame ϕ̇ θ̇ ψ ̇a re velocity, ψ̈ ϕ̈ θ̈ are instant momentum of angular Euler [7]. Finally, according to the required parameters by physical simulation of quadrotor in CATIA software, some quadrotor parameters are computed and the rest of them are estimated by similar physics of robot's evaluation which have shown in Table 1. As it has presented in Table 1, I XX , I YY , I ZZ and I rotor are the moments of inertia around X-axis, Y-axis, Z-axis and motor axis respectively, m is the weight of motor, b is the propeller result force index, d is the propeller counterpoise index, g is the earth gravitational force.

PID CONTROLLER
The PID controller belongs to those group of controllers, which work based on the feedback, and more famous as a classical controlling and, it's design and implementation is very customary [8]. One of the great challenges of the flier robot pilots is obtaining suitable PID index according to the physical structure of quadrotor. Most of the quadrotor's pilots try the false and true method which leads to make so many problems and hardship to the robot. So, obtaining the PID indexes is significant [9]. To control of mentioned parameters, first, PID controller by meta-heuristic algorithms have been used which have been described in the following. Equation (26) is the general equation for PID controller which K p is proportional, gain, K d is derivative gain and K i is integrator gain [10].
According to (10), the control inputs, and PID controller of the quadrotor are defined such as (11) to (26) [11,12]. In the above equations, parameters K p , K dz , K iz respectively are controlled proportional, integrator and derivate gain, in height control. The indexes of K pφ , K dφ , K iφ are control gains to control 'roll'. K pθ , K dθ , K iθ are controlled gains to control 'pitch' and K pψ , K dψ , K iψ are control gains to control 'yaw' [13,14]. In order to find the PID parameter, Ziegler-Nichols rules are an innovative method of controlling parameters in obtaining values of integral derivative proportional controller coefficients. This method has been developed by Ziegler and Nichols. In this state, the system reaches the boundary of satiability and the period of k u fluctuation and its output are fined oscillating [15]. To use this method, the quadrotor linear dynamic should be modeled. Table 2 shows the Ziegler-Nichols rules for adjusting control gains [16].

PID optimization
In order to optimize the PID parameter, an appropriate cost function must be considered in order to appropriately quantify the control interest in the control algorithm. The cost function can be defined on the basis of desirable characteristics. For example, the amount of its excess is specified by M p , time to ascent specified by t r lasting response value is defined by e ss and settling time is defined by t s . The penalty function method is used to define the cost function for optimization algorithms. According to this method, the cost function is defined as (27): As mentioned in (27),  value can be changed. Considering the same condition of influencing the intended attributes on the cost function, this value is equal 0.69 [17][18][19][20]. Based on the cost function definition, the parameters of each response of optimization problems such as K pz , K dz , K iz are respectively the proportional-derivative and integration gains in control height. The K pφ , K dφ , K iφ are the control gain for roll control. K pθ , K dθ , K iθ are the control gain for pitch control and K pψ , K dψ , K iψ are the control gain for yaw control which is twelve parameters altogether. In other words, each response of problems are indicating twelve values for PID index and the duty of the defined algorithms is finding the best answer considering all introducing defined parameter's [15]. In PID indexes optimization, the nature-inspired algorithm has used [16] which are described shortly as following.

PSO algorithm
The PSO algorithm is taken from the aggregation of particles and it was inspired from bird's troop and fishes. These kinds of birds are putting in a searching space of pixel table in random and in every repetition, the closed neighbor of particle has selected and speed of the particle is replaced with the speed of its closest neighbor [17]. This action causes the fast movement of a group in the same direction with uncertain movement and converges without changing [18].

GA
Gene is the main saving unit in genetic. Inside cells, genes are connected to each other to produce chromosome. This new cell called 'diploid' and it caused through meiosis. In meiosis, each chromosome of the diploid cells makes a copy of themselves. Then, chromosomes group (new and main) are compounded to each other [21]. After it, chromosomes will separate one more time and 4 diploid cells are made. Mutation can take place in each stage. Every single change in chromosomes transfer to the children [22].

FA
FA is based on flickering which is discussed up to now. The base of flickering is just like a firefly's behavior. The flickering of these insects is based on a biological illumination process. So, determination of correct function of the signal producing system is yet to be discussed. The base of flickering is to create a connection and attract the opposite sex or bait to hunt. In general, flickering can be used as a protection alert mechanism [23]. According to the related light criteria, the light intensity I, in the distinct distance of a toward the source light generation, has the reverse ratio with the square value of this distance.
In the other words, after increasing distance from resource and light attraction by the air, it's intensity goes weak [24,25]. In order to compare optimization methods of parameters the same conditions between mentioned algorithms have been considered. Table 2 has shown the algorithm repetition and consideration.
As it is shown in Table 3, the components which can be defined in the same way, are including the population size in the most algorithm repetition. Some of parameters also are related to the internal setting of algorithms and they are not common between algorithms. These parameters have been determined and set by the help of values in the research articles or they set experimentally which have shown Table 4.
In Table 4, the PSO algorithm parameters, σ is the particle velocity, c 1 c 2 are the local impact indexes. With attention to the local particle and the best major particle, w is the added weight index in the relation of speed update. In the GA algorithms, p c is the possibility of crossing, p m is the possibility of changing in bit, M u is the mutation index. The parameters of FAs, I 0 is the light source intensity and γ is the light absorption index. β 0 is the equivalent of each firefly's attraction rate, α is the random parameters, and δ is the uniform distribution changing range.

SIMULATION
After the modeling, the Quad rotor the first step of this research is to test the model performance with three Meta-Heuristic Algorithms of PSO, GA, and FA to determine the PID controller parameters.

PSO algorithm
In order to determine PID controller index, optimized by PSO algorithm, to control pitch, roll, and yaw for step function the algorithm optimization process has shown in Figures 2(a)-(c). It is noted that with respect to the direct infernal relation of Euler angles the best cost is recognizable in each figure. As it is shown in Figure 2, the algorithm in the control of roll, pitch, and yaw angles has proper function and overshoot rate, steady state error, settling time, have shown low increasing time and the algorithm has reached to damping in 24 repetitions. With attention to Figure 2, the PID index of particles swarm optimization has shown in Table 5. With respected to the controller performance presented in Figure 2, the functional criteria obtained from the PSO algorithm have shown in Table 6.  In Table 6, the maximum value of overshoot criteria is related to the pitch parameter and the minimum value is related to the roll. In the steady state error criteria, almost all of them are zero. In the settling time, the maximum value is related to the pitch parameter and the minimum value is related to the roll and increasing time, the maximum value is related to the roll and the minimum value belongs to the pitch.

GA
The results of PID controller optimizing by GA in controlling pitch, roll, and yaw angles, to the step function, have shown in Figures 3(a)-(c) and the algorithm optimization process has shown in Figure 3(d).
As it has been shown in Figure 3, the algorithm has no good operation in to control of roll, pitch, and yaw angles. So, it could not track the function properly. The overshoot rate, steady state error and settling time, shows the very low increasing time and the algorithm in 25 repetitions has reached to damping, with attention to figures the PID indexes of GA and controller response a have shown in Tables 7 and 8. In Table 8, the maximum value of overshoot criteria is related to yaw parameter and the minimum value is related to the roll. In steady state error criteria almost all of the values are zero. In settling time, the maximum value belongs the yaw parameter and the minimum value belongs the pitch and in rise time, the maximum value connected to the pitch parameter and the minimum value connected to the roll.

FA
The results of PID controller, which optimized by FA in controlling yaw, roll and pitch angles to the step function input are presented in Figures 4(a) Table 9. As it is shown in Figure 4, the algorithms had a good performance to control of roll, pitch and yaw also overshoot rate, steady state error and settling time, shows the very low increasing time. Moreover, algorithm in 27 repetitions have reached to damping. The with attention to the controller function, in Figures 4(a)-(c) the operating parameters of the FA have shown in Tables 9 and 10.  In Table 10, the maximum value of overshoot criteria belongs to the yaw parameter and the minimum value related to the roll parameter. In steady state error, almost all values are zero. In settling time, the maximum value is connected the yaw parameter and the minimum value is connected the pitch. In the increasing time, the maximum value is related to the pitch.

Comparisons of PID controller optimization methods
As already stated, the five criteria of overshoot, steady state error, settling time, rising time are used to compare the coefficients of PID tuning via the mentioned algorithms. this comparison are summaries in Tables 11-13 for pitch, roll and yaw parameters. As it shown in Table 11, the controller performance in pitch parameter, the PID_FA, PID_PSO algorithms show better performance in overshoot than the two Ziegler-Nichols methods and PID_GA. The value of steady state error in all algorithms are very low and the settling time parameters, for the PID_PSO and PID_FA, perform better in compare to the two other algorithms. Table 12 shows the comparison of the controller's performance in the roll parameter. Which it shows the PID_FA and PID_PSO algorithms, there is a better performance in the overshoot and settling time criteria have better performance towards Ziegler-Nichols and PID_GA.  The steady state error in all algorithms is zero and Settling Time Criteria in PID_PSO and PID_GA is the least amount compare with other algorithms. As it decapitated in Table 13 from the performance of the controllers, the PID-PSO and PID_FA algorithm have the better performance in overshoot, towards Ziegler-Nichols and PID_GA methods. The steady state error is too less in all algorithm and for the settling time parameters, PID_PSO algorithm and Ziegler-Nichols have the better performance in comparison with PID_GA and PID_FA methods. Then the result shows the meta heuristic algorithm are act better than Ziegler-Nichols. then to have the to have a better study of algorithms, the other comparing criteria including three parameter of average computation time, success rate and standard deviation with the following description are considered: -The calculation time average: the time of calculating to turn algorithm once. -Success rate: The success rate of each algorithm is to achieve optimality parameters.
-The standard deviation of obtained responses: the standard deviation is a statistical index which is equal to the second root of variance. It shows the deflection of members in a complex from the average value. As much as the standard deviation is closer to zero, the data values dispersion is lower, and as much as this value is more, the data values dispersion is more the result of compared parameters are summarized in Table 14.
As it is shown in Table 14, PID-PSO has the least time, Success rate and the least standard deviation. So PID-PSO shows the better performance. As the last stage of simulation and to evaluate of PID-PSO controller, some routes with different challenges have applied to them and the function of them in the face of applied route is investigated. In the following, by using a circular, spiral, and 8-shaped and spline routes to the system (Table 15), the controller performance is evaluated ( Figure 5). The average of tracking error in examination routes has shown in Table 15. As it has been shown in Figure 5, the system function in tracking routes of resource, shows the proper performance. But in some routes, some errors have been seen. According to Table 15, the most error is related to 8-shaped ( Figure 5 (c)) and the least error is related to spiral-shaped.

STRUCTURE ASSEMBLY 5.1. Index practical implementation
The second stage in the designed plan is PID index implementation in practice and in the real size of quadrotor. As it is shown in Table 16 and Figure 6, gained values on the quadrotor have tested. The quadrotor consists of body, engine, controller system and energy supply. Designing a quadrotor often are related to its application. which all mentioned components have selected, for example, to carry the weight, the high-power consuming motor needing high power battery have been considered. For speed racing, the H shaped body with the low weight and small size have used. So, based on the application, the shape of designing is different. In this study, a sample of quadrotor consists of mentioned components in Table 16 have considered.
For the implementation of data, the software 'mission planner' has been used. Mission planner is an open source software, uses the windows operating system, installed on the ground station computer system. Mission planner setting steps: a. selecting kind of robot frame b. accelerometer calibration c. compass calibration d. board and power supply selection e. radio calibration f. reference route selection [26,27]   The start point position is the place which robot is ready to fly in there. It means, if RTL has been considered, the robot will return to the start point. -The second step, setting route point: To do this in the drop-down menu of the flight plan, the desired command in each row (point, height,) should be selected. After initial setting in mission planner and setting the PID indexes. the used operations and routes in simulation containing Figure 5 such as circle, spiral, 8 shaped and spline have tested in real which results of real tests can be seen in Figure 7 and Table 17.  (Figure 7(d)), show that the robot has a good performance in the tracking of path points and also corners. In some points, the path tracking was not done in a good way, because of some problems, such as GPS module error about 4-meter disconnection of the signal from telemetry module and system disturbance through the wind. In these tests, according to the weather station site, the wind speed was almost 7 km/h and the real robot moved up with a maximum height of 15 meters.
After implementation of control parameters on the real robot, the tracking path error in a practical test using mission planner software [27] has been shown in Table 17. According to the table, the maximum percentage of error is related to the 8-shaped path (Figure 7(c)) and the minimum error is related to the spline path (Figure 7(b)). Some of the result of the real experiment have been shown in YouTube link [28].

CONCLUSION
In this study, by using Newton-Euler method, the quadrotor dynamic has been studied and also the general equations of the system have been described. The control of quadrotor, a PID controller indexes, the nature-inspired optimization algorithm including PSO, GA, and FA has been introduced and implemented in the simulation. The simulation of the designed model has been done in MATLAB software and quadrotor 269 stability parameters have been investigated. The PSO algorithm, with attention to the comparing criteria such as overshoot rate, error, setting time, increasing time, calculating time, and standard deviation has operated as the best algorithm. In order to verify evaluation for the simulated system, four different routes with different challenges have been applied to the robot as different routes and the robot function has been studied in tracking the routes. Then, the simulated parameters of real quadrotor have been implemented and the robot functions in challenging situations with the real routes have been investigated. With attention to the test, in simulation mode, the most percentage of error is related to the 8-shaped path and the least error is related to the spiral path ( Figure 5). In the practical mode, the most percentage error is related to the 8-shaped and the least error is related to the spine ( Figure 7) and this difference can be in the result because of data sending error (telemetry) and GPS. With attention to the all direction, it can be said with considered indexes for the system, it has a good performance in stability and tracking the resource route and its applicable in various application in the robotic field [29][30][31]. The result shows the MATLAB capability and its navigation on the real platform.