Improving 3D Path Tracking of Unmanned Aerial Vehicles through Optimization of Compensated PD and PID Controllers

: The development of quadrotor unmanned aerial vehicles (QUAVs) is a growing ﬁeld due to their wide range of applications. QUAVs are complex nonlinear systems with a chaotic nature that require a controller with extended dynamics. PD and PID controllers can be successfully applied when the parameters are accurate. However, this parameterization process is complicated and time-consuming; most of the time, parameters are chosen by trial and error without guaranteeing good performance. The originality of this work is to present a novel nonlinear mathematical model with aerodynamic moments and forces in the Newton–Euler formulation, and identify metaheuristic algorithms applied to parameter optimization of compensated PD and PID controls for tracking the trajectories of a QUAV. Eight metaheuristic algorithms (PSO, GWO, HGS, LSHADE, LSPACMA, MPA, SMA and WOA) are reported, and RMSE is used to measure each dynamic performance of the simulations. For the PD control, the best performance is obtained with the HGS algorithm with an RMSE = 0.037247252379126. For the PID control, the best performance is obtained with the HGS algorithm with an RMSE = 0.032594309723623. Trajectory tracking was successful for the QUAV by minimizing the error between the desired and actual dynamics. and I.B.-V.; formal analysis, N.S.Z.-P., N.H.-R. and J.C.S.-T.-M.; investigation, N.S.Z.-P., N.H.-R. and J.C.S.-T.-M.; resources, J.M.-M. and J.C.S.-T.-M.; data curation, N.S.Z.-P.; writing—original draft preparation, N.S.Z.-P. and I.B.-V.; writing—review and editing, N.S.Z.-P. and I.B.-V.; visualization, N.S.Z.-P. and I.B.-V.; supervision, J.M.-M. and J.C.S.-T.-M.; project administration, J.M.-M. and J.C.S.-T.-M.; funding acquisition, J.M.-M. and J.C.S.-T.-M. All authors have read and published version of the manuscript.


Introduction
One of the most complex engineering problems is the limitation of algorithms to control unmanned aerial vehicles (UAVs). UAVs are usually referred to as drones, and are devices that can be autonomous or remotely piloted. They can also be multirotor, a manoeuvrability characteristic that enables vertical take-off and landing. Since 1970, study and development of QUAVs have been an active research topic for governments, industry, and scientists around the world [1].
Quadrotor UAV (QUAV) is the most simple UAV and contains four motors. QUAVs have a broad variety of applications, i.e., supervision and inspection, monitoring, imaging and mapping, search and rescue operations, and load carrying [2]. QUAVs complexity is mostly related to the fact that QUAVs are underactuated and nonlinear systems that respond in coupled dynamics and chaotic behaviors [3]. These features make them challenging to model and improve the design of controllers; for example, to guarantee QUAVs stability and robustness during flight.
QUAVs require high-level designing and implementation of efficient multidimensional control structures that improve their response. Controllers rely on parameters that need to be tuned in order to obtain an adequate performance. Parameterization is based on iterative experiments of different trial-and-error methods. This process is time-consuming, and it does not guarantee getting optimal parameters. The most popular controller types are proportional-derivative (PD) and proportionalintegral-derivative (PID) because of their simplicity of structure and implementation. Unfortunately, there are very few systematic methods to find optimal parameters [4][5][6]. Nowadays, PID controls add techniques such as neural networks and evolutionary algorithms, resulting in high accuracy and dynamic performance for different engineering control problems. Systems are becoming adaptive to external disturbances. For example, in [7], an adaptive PID tuning method is reported, which is pre-trained by a genetic algorithm to tune the weights of a neural network and auto-tunes online the proportional, integral, and derivative units of a PID.
The design and implementation of control laws in QUAVs consider multiple variants that are tuned for each control in a constrained-based optimization manner, similar to the path planning problem [8,9]. Those limitations motivate solutions of stochastic nature. There are different stochastic algorithms, the most popular are metaheuristics, both evolutive and population-based, with high effectiveness and easy implementation. Evolutionary metaheuristic algorithms are based on the laws of biological evolution, which do not share information with another generation. However, the best-qualified individuals composing the new group and better quality individuals will remain up to the end. Some of the most popular are genetic algorithm (GA) [10][11][12] and differential evolution (DE) [13] with many variants, refinements, and hybridization.
Population-based metaheuristics are motivated by collaboration capabilities, social interaction and sharing knowledge, present in many groups in nature. This type of metaheuristic is one of the most popular techniques to solve multiobjective optimization problems, thanks to their ability to compute multiple solutions in a single repetition [14].
The problem of tuning a PD or PID for a QUAV dynamic system results in a complex underactuated system, highly coupled in its variables, and many parameters to adjust, nonlinear, where it is not easy to apply conventional control techniques. Using these techniques, the dynamic model must meet convexity, differentiability, continuity, and linearity properties, leading to an excessive simplification of the model to obtain a global optimum, but failing to represent all the relevant features of the real system [15]. By using metaheuristic algorithms to solve this type of problem, it is impossible to show that the global optimum is reached; for this reason, a series of repetitions of the runs are carried out to verify the robustness and accuracy of the solutions obtained. Nevertheless, it generates satisfactory solutions in a reasonable time [16].
Previous studies have addressed the problem of tuning controllers applied to UAVs. In [17], a comparison between proportional (P), PD and PID controllers relating to time to tune and stability is done, applying a particle swarm optimization algorithm (PSO) for choosing the parameters to control attitude and altitude of QUAV (4 of 6 possible dynamics). Similarly, the authors in [18] have proposed the artificial bee colony (ABC) algorithm, PSO and GA to optimize PID parameters and add a comparison with Ziegler-Nichols method applied to QUAV attitude dynamics. In the same way, the optimization of a PID controller using GA, Crow Search Algorithm (CSA) and PSO only for orientation and altitude was performed in [19].
In this paper, we report the development of a dynamic model for a QUAV using the Newton-Euler formulation taking into account all the six dynamics for a QUAV. We compare eight optimization algorithms for PD and four for the PID controller. All algorithms were applied to the dynamic model. Initially, the parameters were obtained using different population-based metaheuristic algorithms, including PSO, Gray Wolf Optimization (GWO), Hunger Games Search (HGS), extended and improved SHADE (LSHADE, LSPACMA), Marine Predators Algorithm (MPA), Slime Mould Algorithm (SMA), and Whale Optimization Algorithm (WOA) [20][21][22][23][24][25][26][27][28].
Usually, a set of benchmark functions is used to demonstrate the performance of optimization algorithms by evaluating the accuracy, precision, and robustness. However, not all algorithms have the same performance capabilities for different engineering problems [29]. Thus, the main contribution of this work is to identify a set of metaheuristic algorithms that perform well in optimizing compensated PD and PID controls for trajectory tracking of a QUAV considering the mathematical model is nonlinear with aerodynamic moments and forces on the Newton-Euler formulation. Consequently, obtaining an optimized and compensated structure that overcomes the limitations of nonlinearity and instability of a full dynamics (a total of six motions and directions) of a quadcopter system with PD or PID controls.
The mathematical model derived to simulate flight dynamics of a quadcopter and detail control strategy is given in Section 2. Section 3 details the optimization of compensated PD and PID. The evaluation methods and experimental results are presented and discussed in Section 4. Section 5 presents the conclusions and further outlooks in this paper.

Quadrotor UAV: Modeling and Control
This section presents the dynamical model of QUAV using the Newton-Euler formulation and detail control strategy.

System Description and Modeling of a QUAV
As shown in Figure 1, to describe the motion of QUAV in 3D space, this requires the definition of two frames: the inertial World frame, W, and the Body frame, B. The first is a fixed coordinate system on Earth with x W -y W -z W and the second is fixed on the structure of the vehicle, denoted by x-y-z. The QUAV is a complex structure, and it involves different physical effects from aerodynamics and mechanic forces. The model developed assumes the following: • The structure of the system is rigid and symmetrical. • The gravity center and the B frame origin are coincident. • Propellers are rigid. • The attitude dynamics are limited to small angles and consequently slight variations of linear and angular velocities.
The absolute position of QUAV is defined as the coordinates in the B frame in regard to coordinates in the W frame, represented by position vector r = [x, y, z] T . The angular position, defined by the Euler angles (φ, θ, ψ), represent the orientation of the QUAV with respect to the W frame, these angles define the rotations of yaw, roll and pitch, they indicate how the B frame (which initially is aligned with the W frame) must be rotated to align with W frame again.
The rotation matrix from B to W, is given by W R B = c R B a R c W R a , where c R B , a R c , W R a , represents rotations about X, Y and Z axis, respectively, in that order, as shown in Figure 2.
Before presenting the movement equations, we establish the relation between velocities in W, (φ,θ,ψ), and velocities in B, (p, q, r), from transformation vectors: then, we can define the angular velocity in the body, ω B , as the relation by using a small angle approximation, we get  φ θψ Applying, Newton-Euler formulation, equations of motion are given by (1) and (2).

Strategy Control of Trajectory Tracking
In order for the QUAV to follow a predetermined path in three-dimensional space, the control of trajectory tracking is proposed as Figure 3, where we introduce the desired positions x d , y d , z d to the Translational and Altitude Controller, which computes the control signal u 1 and determines the desired angles φ d , θ d , which in turn are the references to the Attitude Controller. The Attitude Controller calculates the control signals u 2 , u 3 and u 4 for orientation dynamics. Due to the nonlinear nature of QUAV system, a compensated PID and PD controller for trajectory tracking is proposed [30].

PD Controller
The dynamics shown in (3) can be represented as a second-order system, in the form where f (χ), in this work, it is a nonlinear function due to the drag forces of the QUAV, χ = [χ,χ], g(χ) = 0 is a known function u, is the control signal. Control law is designed as where,χ d is the second derivative of the desired trajectory, (4), the closed-loop dynamics of the system arë e + k p e + k dė = 0,

• Translational and Altitude Control
The altitude dynamics shown in (3), can be represented as (4), Thus, if the control signal is designed as in (5) we get Equation (7) is constrained (−π/2 < φ < π/2( and (−π/2 < θ < π/2). For translation dynamic in the axis X, a virtual control signal is selected as µ x = cos φ sin θ cos ψ + sin φ sin ψ, then becomes as and the control signal is chosen as in (5) Similarly, for the dynamic on the Y axis, µ y = cos φ sin θ sin ψ − sin φ cos ψ, we get with the control signal as form as (5), therefore Equations (9) and (11) are true for u 1 > 0, which is physically correct because u 1 represents the lift force generated by the rotors of the vehicle.
Virtual control signals µ x y µ y allow to calculate desired angles for attitude controller roll, φ d y en pitch, θ d : • Attitude Controller For the orientation dynamics, they can be represented as (4) and the control signal can be designed as in (5).
For the movement of roll, we have the control signal u 2 as: For the movement of pitch, we have the control signal u 3 as: Finally, for the movement of yaw, we have the control signal u 4 as:

PID Controller
The PID controller is designed in the same way as the PD controller. The difference lies in the addition of a variable in the error E and in the gain vector K. They both have the same structure. We recall control law in (5) where,χ d is the second derivative of the desired trajectory. Now, E = [e eė] T , with e = χ d − χ and the closed-loop dynamics of the system arë e + k p e + k i e + k dė = 0. Figure 4, shows the implemented scheme for the controller parameterization, where each of the metaheuristic algorithms is evaluated. Initially, the population of the algorithm is generated and evaluated. The algorithm delivers a set of gains, which are applied to the designed controller, which in turn calculates the control signals for the QUAV. Then, the dynamic performance of each of the X, Y and Z axes is measured to calculate the error between the desired dynamics and the real one. This cycle is repeated until the number of iterations initially determined is reached or until the error or cost function reaches a set value.

Optimization of Compensated Controllers
To obtain the parameters of each of the six UAV dynamics, through the evolutionary algorithms, the population to optimize the PD controller is: and the population to optimize the PID controller is: with the search space for each individual S = [0, 10], chosen that way to restrict the values of the control signals due to the physical limitations of the actuators when performing platform experiments. The cost function calculated using the root mean square error: where N is the size of χ and G, represents each of the dynamics x, y, z, φ, θ, ψ of QUAV.

Results and Discussion
We validated a dynamic model through simulations that verified the behavior of coupled dynamics and no lineality. Calculations were performed in a Mac OS Big Sur (Intel core i9, RAM 24 GB, and 1 TB hard disk), using MATLAB 2015a (MathWorks) for each algorithm (PSO, GWO, HGS, LSHADE, SPACMA, MPA, SMA, WOA). The solution space for each parameter to optimize (total 12, Equation (16)) was constrained to [0,10]. We performed simulations for 20, 30, and 40 individuals using 20 iterations during 30 independent runs for PD controllers. We ran simulations with 80 and 100 individuals for PID controllers using 40 iterations and 30 independent runs. The evaluation performance of the algorithms is reported through statistics that considers four variables to check the dispersion or variability of the solutions, which are the best, the mean, the standard deviation and the worst. In the following tables, the best results are presented in bold, and the purpose is to quickly identify the best performing controller as well as its gains. Table 1 shows the RMSE statistics for the simulation with 20 individuals, 20 evolutions and 30 repetitions of the eight algorithms under test. The HGS algorithm is the one that obtains the best value RMSE = 0.037263535506379, the MPA algorithm is the one that obtains the best average, standard deviation and worst values. Figure 5a show the RMSE during the 20 iterations, the three best values are HGS, MPA and PSO. In the box plot, the lowest variability is obtained by GWO and MPA and the highest variability of the RMSE value is in PSO algorithm. The 12 parameters identified by each algorithm are shown in Table 2, where the HGS parameters are the best for obtaining a lower RMSE. Table 3 contains the RMSE statistics for the simulation with 30 individuals, 20 iterations and 30 repetitions. The algorithm that obtains the best performance is again the HGS with a RMSE = 0.03725093743996; for this case, the GWO algorithm gets the best average, standard deviation and worst values. Figure 5b show the RMSE during the 20 iterations; the three best values are HGS, PSO and MPA. In the box plot, the lowest variability is obtained by GWO, HGS, MPA y SMA and the highest variability of the RMSE value is obtained by PSO algorithm. The 12 parameters identified by each algorithm are shown in Table 4. The HGS parameters are the best for getting the lowest RMSE. Table 5 contains the RMSE statistics for the simulation of 40 individuals, 20 evolutions and 30 runs with the corresponding algorithms. The algorithm that obtains the best performance is the HGS with a RMSE = 0.037247252379126. The MSA algorithm gets the best average, standard deviation, and worst values for this case. The 12 parameters identified by each algorithm are shown in Table 6. In Figure 5c, the box plot shows the lowest variability is obtained by GWO, HGS, MPA and SMA. The highest variability of the RMSE value is obtained by PSO algorithm.

PD Controller
We evaluated the performance of optimized parameters obtained ( Table 2) for all six dynamics, (x, y, z, φ, θ, ψ) and algorithms (Figures 6-11) at 20 iterations with 20 individuals. For translational dynamics (x and y), the desired trajectory is trapezoidal and for altitude (z) and yaw (ψ) dynamics is a ramp. The sharp effect observed in the Z axis of Figure 8 is due to the compensation between the force of gravity and the control signal. Negative values are associated with the instant in which the value of gravity is more significant than the control signal. Please note that this effect is only visible in the simulation; in real life, this effect is observed as a time delay until the force of gravity is countered by the control signal. The best trajectory is determined by the minimal value of cost function achieved by HGS as shown in Table 1.
The highest cost function value will define an acceptable performance for all dynamics. Here, the algorithm LSPACMA reached the highest value (0.040248443990318). As we can see, HGS algorithms performed best with a minimum square error (RMSE), which is the lowest obtained for all algorithms. LSPACMA has the largest error in most of the dynamics performance. Figures 9 and 10 show the angles (φ and θ) of orientation of the QUAV. Dotted lines show the desired angles calculated as described in Section 2.2.1. Simulations show that φ angles are coupled with the axis Y and rotational movements of θ are coupled with axis X. When a change in direction is necessary, the controls are robust enough to generate changes in the orientation in response to the desired angles. Figure 12 shows the control signals calculated by (7), (13)- (15). It is noted that the controllers reach their steady states during the flight. Signals u 2 and u 3 respond to changes in direction on the desired path and u 1 is affected and responds to small variations in altitude. The dynamic of ψ converges in minimum time and consequently, the control signal u 4 is kept at a small value.
In Figure 13, we can see that the trajectory tracking in X-Y-Z space, evaluating the optimized parameters, is successfully developed. With the obtained parameters, we consider a different circuit. The desired trajectory is sinusoidal for translational dynamics (x and y), and altitude (z) is a step. We found that the QUAV can follow it, as shown in Figure 14.

PID Controller
From the previous section, the four algorithms with the best fitness during tuning of the PD controls were GWO, HGS, MPA and SMA; these are taken for the PID control. Eighteen parameters were identified (Equation (17)), so it was necessary to increase the number of individuals. Here, two simulations are reported, one with 80 and one with 100 individuals. The number of evolutions and repetitions is 40 and 30, respectively. Table 7 shows the RMSE statistics for the simulation with 80 individuals, 40 evolutions and 30 repetitions of the four algorithms under test. The HGS algorithm obtained the best value (RMSE = 0.032594329329481). We also show the best values for all algorithms for the mean, standard deviation, and worst. Figure 15a shows the RMSE during the 40 iterations. The three best values are HGS, MPA and GWO. In the box plot, the lowest variability is obtained by GWO, HGS, and SMA. The highest variability of the RMSE value is in MPA algorithm. The 18 parameters identified by each algorithm are shown in Table 8, where the HGS parameters are the best for obtaining a lower RMSE Table 9 contains the RMSE statistics for the simulation with 100 individuals, 40 iterations and 30 repetitions. The algorithm that obtains the best performance is the HGS with a RMSE = 0.032594309723623. The GWO algorithm obtains the best average and standard deviation for this case. The MPA obtains the worst values. Figure 15b show the RMSE during the 40 iterations. The three best values are HGS, GWO and MPA. GWO, HGS, and SMA obtain the lowest variability in the box plot. This graph shows a smaller dispersion of the optimization solutions compared to the simulation with 80 individuals. The 18 parameters identified by each algorithm are shown in Table 10. The HGS parameters are the best for obtaining the lowest RMSE. The 18 parameters identified by each algorithm are shown in Table 8, where the HGS parameters are the best for obtaining a lower RMSE.
We evaluated the performance of optimized parameters obtained (Table 10) for all six dynamics, (x, y, z, φ, θ, ψ) and algorithms (Figures 16-21) at 40 iterations with 100 individuals. The desired trajectories are the same as for the PD controller experiment. The best trajectory is determined by the minimal value of cost function achieved by HGS as shown in Table 9. The similarity of the performance of the six dynamics with the PD controller implementation can be noted, even when the number of parameters to optimize is larger and the number of individuals is bigger. Figure 22 shows that the signals calculated by the PID controllers stabilize the dynamics. The control signals are very similar to Figure 12 since the desired path is the same. In the same way, as in the PD control, signals u 2 and u 3 respond to changes in direction on the desired path and u 1 is affected and responds to small variations in altitude. The dynamic of ψ converges in minimum time and consequently, the control signal u 4 is kept at a small value.
Finally, Figure 23 shows the successful trajectory tracking in X-Y-Z space, evaluating the optimized parameters of PID controller using HGS algorithm with 100 individuals. With the same optimized parameters, we consider a different path. The desired trajectory is a sinewave for translational dynamics (x and y), and altitude (z) is a step. We found that the QUAV can perform it effectively, as shown in Figure 24.

Conclusions
In this paper, we have addressed a long-standing problem related to the tuning of compensated PD and PID controllers for path tracking of the QUAV vehicle modeled with six different dynamics. We compared eight metaheuristic algorithms ( PSO, GWO, HGS, LSHADE, LSPACMA, MPA, SMA and WOA), finding optimal parameters to select the most accurate algorithm. The HGS algorithm was the most suitable of the eight evaluated, as it obtained the lowest RMSE for the compensated PD and PID controls. The trajectory used was a square with an elevation change, with 40 individuals and 20 evolutions for the compensated PD control, RMSE = 0.03724725237912, and the number of optimized parameters is twelve. For the compensated PID control with 100 individuals and 40 evolutions, the RMSE = 0.032594309723623, the number of optimized parameters is eighteen. For the case of compensated PD control, the following best algorithms are GWO, MPA and SMA, with an RMSE close to HGS. This case corresponds to when 40 individuals are used, and for the case of compensated PID control, only the HGS, GWO, MPA and SMA were simulated. On the other hand, the algorithm LSPACMA was the most limited in optimization scope. We also found those optimized gains for the square path could take the QUAV to a different trajectory like the sinewave and step paths, resulting in the most suitable tuning for the compensated PD and PID controllers for QUAV trajectory tracking. This modeling approach is now ready and suited for implementation at an experimental level.  Data Availability Statement: Desired trajectories and dynamic performance data are publicly available at https://github.com/NadiaZunigaPena/Metaheuristics-enabled-path-tracking-Optimizationin-Unmanned-Aerial-Vehicles, accessed on 26 October 2021.

Conflicts of Interest:
The authors declare that they have no known competing financial interest or personal relationships that could have appeared to influence the work reported in this paper. Vector of forces due to gravity k 1,2,..., 6 Drag Coefficients