# Modeling and Nonlinear Control of a Quadcopter for Stabilization and Trajectory Tracking

Authors: Ademola Abdulkareem; Victoria Oguntosin; Olawale M. Popoola; Ademola A. Idowu
Journal: Journal of Engineering
 (2022)

---

## Abstract

This paper presents an adequate mathematical representation of a quadcopter’s system dynamics and effective control techniques. A quadcopter is an unmanned aerial vehicle (UAV) that is able to do vertical take-off and landing. This study presents a nonlinear quadcopter system’s mathematical modeling and control for stabilization and trajectory tracking. The mathematical model of the system dynamics of the quadcopter is derived using Newton and Euler equations with proper references to the appropriate frame or coordinate system. A PD control algorithm is developed for the nonlinear system for stabilization. Another nonlinear control technique called full state feedback linearization (FBL) using nonlinear dynamic inversion (NDI) is developed and implemented on the quadcopter system. However, there is a problem with the normal approach of the complete derivation of the full state FBL system using NDI as gathered from the literature review. In such an approach, the PD controller that was used for attitude stabilization was able to stabilize the angles to zero states, but the position variables cannot be stabilized because the state variables are not observable. Thus, a new approach where the position variables are mapped to the angle variables which are controllable so as to drive all states to zero stability was proposed in this study. The aim of the study was achieved but the downside is that it takes a longer time to achieve this stability so it is not efficient and should only be considered when absolute zero stability is the aim without considering time efficiency. The study further investigates the problem of nonlinear quadcopter system’s mathematical modelling and control for stabilization and trajectory tracking using the feedback linearization (FBL) technique combined with the PD controller. The proposed control algorithms are implemented on the quadcopter model using MATLAB and analyzed in terms of system stabilization and trajectory tracking. The PD controller produces satisfactory results for system stabilization, but the FBL system combined with the PD controller performs better for trajectory tracking of the quadcopter system.


---

## Body

## 1. Introduction

A quadcopter, also known as a quadrotor, is a rotor-based aerial vehicle. They are multirotor aircrafts propelled by four rotors. To balance the torque, these rotors are built with two pairs of opposite rotors revolving clockwise and the other rotor pair moving anticlockwise. A quadcopter’s dynamics are highly nonlinear; it is an underactuated system with six degrees of freedom and four control inputs which are the rotor velocities. The quadcopter is controlled by adjusting the rotors’ angular speeds, which alters the quadcopter’s torque and thrust characteristics. The quadcopter is designed with four rotors in cross-configuration, as shown in Figure1. Two opposite rotors rotate in the same direction; the altitude and position of the quadcopter can be controlled by varying the rotor’s angular speed. The quadcopter can be kept in a balanced position without spinning if the generated torque of the motors T1, T2, T3, and T4 is the same.Figure 1
Quadcopter dynamics.Thrust controls the quadcopter’s altitude for ascending and descending and is achieved by increasing or reducing the rotational speed of motors 1, 2, 3, and 4, simultaneously. Roll is the movement of the quadcopter by tilting either left or right to allow side movements. Pitch is the movement of the quadcopter by tilting either front or back to allow forward or backward movement. Yaw is movement in a clockwise or anticlockwise manner while staying level to the ground to change the quadcopter’s direction. These flight motions can be achieved by controlling the rotational speeds of the four motors.Quadcopter designs have recently been popular in UAV research. To control and stabilize the aircraft, these UAVs use an electrical control system and sensors [1]. The small unmanned aerial vehicles (SUAV) have become a reality, thanks to recent microcomputer technology, sensor technology, control systems, and dynamics theory advancements. Due to their compact size, low cost, and agility, these systems are being used in various applications.Indoor quadcopters are usually relatively small in size and cannot utilize GPS for absolute positioning. They thrive in the absence of strong winds and relatively stable light conditions and their missions are usually shorter than those of an outdoor quadcopter [2, 3]. Outdoor quadcopters are relatively bigger in size than the indoor quadcopter and can utilize GPS for absolute positioning. They are more durable and can fly for longer periods of time than indoor quadcopters [2]. A DJI Phantom 4 is an example of an outdoor quadcopter [4].In the 1920s and 1930s, a few manned designs existed. These vehicles were among the first heavier-than-air vertical take-off and landing (VTOL) vehicles to be successfully tested [1]. However, early prototypes performed poorly due to weak stability augmentation and restricted control authority, while latter versions required too much pilot effort. Historically, basic linear control techniques were used to ensure easy computation and flight stability, but due to improved modeling techniques and faster computational capabilities, it is now possible to run comprehensive nonlinear techniques in real-time [5]. The major components of a quadcopter are frame, propeller, motor, speed controller, flight controller, battery, transmitter, and receiver [6, 7]. Linear control techniques can be implemented on the controller of a quadcopter by linearizing the system about an equilibrium point, but such approximation may not preserve the dynamics of the system at every point and are not usually very effective in practical scenarios.As the range and complexity of applications for quadcopters expand on a daily basis, the control techniques utilized on the system must also improve. This paper aims to develop a mathematical model of a quadcopter system and implement nonlinear control techniques on the derived model for stabilization and trajectory tracking of a quadcopter. The full state feedback linearization (FBL) system control based on dynamic inversion for a nonlinear system of a quadcopter is derived and computed in this paper without the use of small-angle approximation.


## 2. Literature Survey

The mathematical modelling and control of a quadcopter are presented in [8]. A matrix approach and the use of bond graphs to describe the dynamic model of a quadrotor are elaborated in [9]. A more detailed derivation of the system dynamics is given in [10]. The work in [8] shows the basics of quadcopter modelling and control to serve as a stepping stone for future research advancement. A detailed study of the mathematical model of the quadcopter dynamics is given, and the differential state equations of the system dynamics are derived from both the Newton–Euler and Euler–Lagrange equations. A simple mathematical model is presented in this paper, neglecting several aerodynamic effects and the modelling of the electric motor spinning the rotors. The developed model is simulated to analyze the behavior of the system, and control algorithms are implemented for stability and trajectory control of the system.The PD controller is used to achieve this, and a heuristic method is developed for trajectory control of the quadcopter flight. Integration of the PD controller and the heuristic method is used to minimize the disturbance on the quadcopter system caused by external forces. The PD control algorithm was integrated into the heuristic method because the heuristic method development did not account for unmodelled disturbances (wind). A decline in the performance of the PD controller was observed if the parameter values were extremely small or high. The necessity for an actual experimental prototype was identified, so those realistic and accurate findings could be acquired and comparison between the simulated results and real-life measurements would be possible.The PID controller may utilize the three controller terms of proportional, integral, and derivative gains to implement precise and optimum control output [11]. The controller attempts to minimize the error over time by adjusting the control variable u(t). The proportional gain produces an output that is proportional to the error value e(t). This output is obtained by multiplying the error value by a proportional constant. The integral gain considers the past error values and seeks to reduce the residual error by adding a control effect obtained from the integration of the cumulative previous errors. The derivative gain anticipates or predicts the future behavior of the error and exerts a control effect produced by the rate of change of the error with respect to time [12]. The linear quadratic regulator (LQR) is a popular linear control algorithm that provides optimally controlled feedback gains to enable stability and high-performance design of closed-loop systems. The LQR controller is implemented on a linearized system and makes use of a mathematical algorithm to minimize a cost function with when the weighting factors are provided. The cost function is often described as the sum of deviations of certain key measurements from their desired values [13].Several control algorithms have been researched for quadcopters for attitude stabilization and trajectory tracking. The objective is to implement a control algorithm that permits the quadcopter’s states to converge to an unpredictable set of time-varying reference states. It is possible to control the quadcopter by linearizing the system dynamics around a stable point and applying linear control techniques, as shown in [14, 15]. Nonlinear control algorithms such as backstepping, sliding mode [16, 17], and feedback linearization [16, 18] are effective for quadcopter control as better performances have been observed from their implementation. Numerous control algorithms for attitude control have been studied. These include the PID controllers [5, 8, 10], inverse dynamic control, sliding-mode controller [19], linear quadratic regulator [5], and feedback linearization control [18–20]. A hierarchical control approach is used in [10], where different levels (controlling rotor rotational speed, vehicle attitude, and trajectory position) form feedback loops.A review of control algorithms for autonomous quadrotors is presented in [20]. Several control algorithms are analyzed to propose hybrid systems that would combine the advantages of more than one control algorithm. The focus of this paper was to implement the optimal control algorithm of the quadrotor manned aerial vehicle for game counting in the protected game reserves in Africa. The control algorithms analyzed in this paper are PID, linear quadratic regulator (LQR), sliding-mode control, backstepping control, adaptive control, and artificial neural networks. From the review, no particular control algorithm gives the best performance in all required features in terms of fast response, disturbance rejection, stability, robustness, adaptability, optimality, and tracking ability. A hybrid control system could have the best combination of some of these features but does not guarantee overall optimum performance. Certain features are more important depending on the application of the quadrotors; hence, compromises would have to be made to implement control algorithms that excel in such areas of application. A quadrotor to be used for game counting is designed to prioritize specific characteristics such as high endurance, high agility, high cruising, low noise, and vertical take-off and landing ability. This paper concluded that the designed quadrotor would be suitable for assisting nature conservationists in game counting and obtaining accurate animal statistics. A PID controller with provision for angular acceleration feedback is utilized in [21] for attitude stabilization. This causes significant gain increase resulting in higher bandwidth.A comprehensive mathematical modelling of the kinematics and dynamics of a quadcopter system is described in [22]. The kinematic aspect of the modelling depicts the quadcopter motion without considering the forces acting on it, whereas the dynamics explain the forces causing the motion. This gives an overview of the response of quadcopters when under the influence of extraneous forces. Nonlinear state equations for the system were derived, and the manual method to obtain minimum steady-state error for PID tuning is presented. This research resulted in a quadcopter with a composite frame assembled and implemented in real-time for pitch, roll, and stabilization with PID control. The results yield adequate performance when the hardware configuration is at the hover point.A thesis on quadcopter flight mechanics model and control algorithms is shown in [2]. The focus of this thesis was to develop a nonlinear model of quadcopter flight mechanics with suitable control algorithms for stabilization and implement in MATLAB/Simulink. The quadcopter dynamics were derived from the equation of motion and forces, and a nonlinear model was obtained based on these dynamics. Aerodynamic effects such as nonzero free steam and blade-flapping are ignored in the derivation of the system equations, but other forces such as air friction and drag forces are considered. For analysis of the system, the model is linearized at a stable hover point, and both the linear and nonlinear models are analyzed. The gradient descent method is used for controller tuning to obtain optimum control parameters, and the system is simulated for a given period of time. A better performance was observed from the automatically tuned PID controller (using the gradient descent method) than the manually tuned PID controller.The design and control of quadrotors in relation to autonomous flying [1] are a thesis that entails the modelling, design, and control of small flying drones with a focus on vertical take-off and landing (VTOL) systems. In this thesis, an autonomous quadrotor called OS4 is modelled and simulated with various controllers. A more complex mathematical model is presented here as the system dynamics account for more realistic aerodynamic coefficients, sensor, and actuator models. The OS4 quadcopter model is simulated with five different control algorithms. The first one was based on Lyapunov theory and was implemented for attitude stabilization, but after simulation on OS4, it was observed that it was not stable enough to allow hover flight. The second control algorithm implemented was the PID controller, and it was suitable for attitude stabilization of the quadcopter system when flying near hover, but this was only possible in the absence of huge disturbances. The third control algorithm was an LQR controller that presented adequate stabilization, but this control algorithm was less adaptive than the PID. The fourth control algorithm was the backstepping controller, which achieved excellent stabilization and control in relatively large disturbances. The fifth control algorithm implemented was the sliding-mode technique, but this control algorithm did not give desirable results because the switching characteristics of the controller were not compatible with the dynamics of the quadcopter system. After the implementation and analysis of these five control algorithms, a suitable control algorithm was developed by integrating the PID and the backstepping algorithm called integral backstepping. This was used for altitude, attitude, position, and trajectory control, and the implementation of this control algorithm on the OS4 enabled the system to take-off, hover, land, and avoid collisions. The thesis noted that the OS4 was the first collision avoidance quadcopter system.A fundamental drawback in quadrotor control is related to their high energy and power consumption [23]. Therefore, a controller was proposed with a power reduction methodology and cuckoo search algorithm (CSA) for tuning the controller gains to control the quadrotor position by a PID controller while the orientation control is achieved through a model-based controller [24]. A control strategy composed of a sensor fault diagnosis unit and a disturbance observer-based sliding-mode controller for quadrotor helicopters to simultaneously accommodate sensor faults and external disturbances were developed and analyzed [25]. Another fault-tolerant control scheme [26] was proposed for attitude control using adaptive sliding-mode backstepping control to accommodate the actuator faults, despite actuator saturation limitation and disturbances.Classical controllers for UAVs and quadcopters involve the use of small-angle approximations [27]. For instance, a cascaded PID controller was designed for trajectory tracking but was made to hover at an altitude where the nonlinear model is linearized [28]. Sophisticated architectures such as genetic algorithm, neural networks, fuzzy logic [29], and adaptative control have to be integrated to classical controllers in order to achieve stability, robustness, optimal control, complex manoeuvres, and large angles. An example is a novel deep learning-based robust nonlinear controller that improves control performance of a quadrotor during landing by combining a nominal dynamics model with a deep neural network (DNN) that learns high-order interactions. A nonlinear feedback linearization controller was designed using the learned model and prove system stability with disturbance rejection [30]. A self-tuning PID controller based on fuzzy logic was designed to cater for payload weight variation. An adaptive robust control (ARC) was utilized to compensate for the parametric uncertainty for the UAVs altitude control [31]. The comparison between the conventional PID and self-tuning PID base on fuzzy logic shows the superiority of the self-tuning PID compared to the PID for variable payload weight [32]. The effectiveness of the PID controller was enhanced by the implementation of a genetic algorithm to autotune the controllers in order to adapt to changing conditions [33]. Five different control systems that include a traditional PID controller, two fuzzy-PD controllers, and two controllers autotuned using a GA were designed based on the developed dynamic model and their performance was compared. The fuzzy-PD controllers showed effectiveness with large angles and complex paths.In this section, previous works on modelling and control of a quadcopter were discussed and reviewed. A precise overview of those topics was analyzed and examined to provide a more rounded understanding of the quadcopter system components, modelling, and control. From the papers reviewed, the importance of implementing a good control for a quadcopter is emphasized and different linear and nonlinear control algorithms are studied. In this research, the FBL system control based on dynamic inversion for the nonlinear system will be derived and computed without the use of small-angle approximation in order to achieve greater accuracy in tracking of trajectory. The small-angle conditions of less thanπ/2 are realistic for many applications of a quadcopter but extremely small-angle approximations cannot achieve extreme manoeuvers [34]. This paper will focus on presenting an adequate mathematical representation of the system dynamics and effective control techniques for a quadcopter.


## 3. Methodology

This section describes the mathematical modelling, system stabilization, and trajectory tracking of a quadcopter. A state-space model of the quadcopter in terms of rotational and translational dynamics is first developed. For the quadcopter system stabilization, the control objective is to control the attitude variable and the linear motion of the quadcopter. Feedback linearization is used to linearize the nonlinear system dynamics so that linear control methods can be used. Trajectory tracking to take the quadcopter system from its current position to the desired position using the quadcopter’s rotor angular velocities is achieved using both PD control (using small-angle approximation) and FBL (without small-angle approximation) control.


### 3.1. Mathematical Modelling

A quadcopter is an aerial vehicle with four rotors that enable motion in different directions. Proper explanation of the system dynamics would require comprehension of the 6 degrees of freedom concept, which presents the position and orientation of the quadcopter in three dimensions (3-D).The 6 degrees of freedom (Figure2) describes the position of a body with six coordinates categorized in two reference frames. The first reference frame is the fixed coordinate system known as the inertial or Earth frame, which is depicted by the x, y, and z coordinates in the cardinal points of north, east, and down. The second reference frame is a mobile coordinate system known as the body frame, which is depicted by the ϕ, θ, and ψ angles with respect to the body center of gravity. The quadcopter is an underactuated nonlinear system because it has four inputs and six outputs. The system is complex and to control it, the quadcopter is modelled on the following assumptions [1]:(i)
The structure is rigid(ii)
The structure is axis symmetrical(iii)
The center of gravity and the body-fixed frame origin coincide(iv)
The propellers are rigid(v)
Thrust and drag are proportional to the square of the propeller’s speed.Figure 2
Six degrees of freedom.


#### 3.1.1. Euler Angles

The Euler angles are three angles introduced by Leonhard Euler to describe the orientation of a rigid body in a coordinate system. They are also used to describe the relationship between two reference frames and convert the coordinates of a point in one reference frame to coordinates of the same point in another reference frame. Euler angles are denoted asϕ, θ, and ψ for the roll, pitch, and yaw angles, respectively, and represent the rotations of a body about the axes of a coordinate system.Any orientation of a rigid body can be achieved by the combination of the three basic Euler angles. The rotation matrices are given by the following equation [5]:(1)Rxϕ=1000cϕ−sϕ0sϕcϕ,Ryϕ=cϕ0sϕ010−sϕ0cϕ,Rzϕ=cψ−sψ0sψcψ0001,where cϕ=cosϕ,sϕ=sinϕ,cθ=cosθ,sθ=sinθ,cψ=cosψ,sψ=sinψ. The rotation matrix depicting the relationship between the inertial frame and the body frame is given as follows:(2)R=Rzψ×Ryθ×Rxϕ,R=cθcψsϕsθcψ−cϕsψcϕsθcψ+sϕsψcθsψsϕsθsψ+cϕcψcϕsθsψ−sϕcψ−sθsϕcθcϕcθ.R is a rotational matrix and is orthogonal such thatR−1=RT.


#### 3.1.2. Reference Frame Transformation

We assumexyzϕθψT to be a vector of linear and angular positions in the inertial frame and let uvωpqrT be a vector of linear and angular velocities in the body frame.Typically, the derivative gain of angular positions should give angular velocities, but the angular positions and velocities above are in a different frame, so we require some transformation matrix to convert from one reference frame to another.Let(3)ξ=xyzT,η=ϕθψT.Let(4)vI=x˙y˙z˙T,ωI=ϕ˙θ˙ψ˙T.Let(5)vB=uvωT,ωB=pqrT.From this statement,vI≠vBandωI≠ωB, instead(6)vI=R⋅vB,ωI=ωη−1⋅ωB,ϕ˙θ˙ψ˙=1sϕtθcϕtθ0cϕ−sϕ0sϕcθcϕcθpqr.Conversely,(7)ωB=ωη⋅ωI,pqr=10−sθ0cϕsϕcθ0−sϕcϕcθϕ˙θ˙ψ˙.


#### 3.1.3. Rotational Motion

Assuming the quadcopter is a rigid body and using Euler’s equations for rigid bodies, the dynamics equation in the body frame is given as follows:(8)Iω˙B+ωB×IωB+Γ=τB.We also assume that the quadcopter has a symmetric structure with the four arms aligned with the bodyx- and y-axes. Therefore, the inertia matrix I is a diagonal matrix in which Ixx=Iyy.(9)I=Ixx000Iyy000Izz.The gyroscopic forcesΓ are caused by the combined rotation of the four rotors and the quadcopter body such that the equation is as follows:(10)Γ=∑i=14JrωB∧e^3−1i+1ωri.In matrix form,(11)Γ=JrωBωrwhereωr=−ω1+ω2−ω3+ω4.The external torque(12)τB=τϕτθτψT.The roll torque componentτϕ and the pitch torque component τθ are obtained from standard mechanics where i=1 and i=3 motors are arbitrarily chosen to be on the roll-axis while i=2 and i=4 are arbitrarily chosen to be on the pitch-axis.(13)τϕ=∑r×T=l−ktω22+ktω42=lkt−ω22+ω42,τθ=∑r×T=l−ktω12+ktω32=lkt−ω12+ω32.For the yaw-axis, the rotor axis is pointing in thez-direction in the body frame, and the torque created around the rotor axis is given as follows:(14)τψ=−1i+1kbωi2+Imωi,where −1i+1 is positive for the ith propeller if the propeller is spinning clockwise and negative if it is spinning counter-clockwise. The term Imω˙i can be ignored because it is in a steady-state, ω˙i≈0.Therefore, the total torque about thez-axis is given by the sum of all the torques from each propeller as follows:(15)τψ=kb−ω12+ω22−ω32+ω42.Therefore, the torque matrix can be written as follows:(16)τB=τϕτθτψ=lkt−ω22+ω42lkt−ω12+ω32kb−ω12+ω22−ω32+ω42.where kt is the thrust coefficient, kb is the drag coefficient, and l is the distance between the rotor and the center of mass of the quadcopter.


#### 3.1.4. Translational Motion

Using Newtonian equation to model the linear dynamics, the extraneous forces acting on the quadcopter are given as follows:(17)m˙v1=00−mg+RTB+FD.Drag forceFD is the force acting on the system as a result of fluid friction (air resistance). A simplified equation form is used where friction is modelled as being proportional to the linear velocity in all directions.(18)FD=−kdvI.The angular velocity of theith rotor creates a force Fi in the direction of the rotor axis (z-direction). The combined forces create thrust T in the direction of the body z-axis such that(19)T=∑i=14Fi=kt∑i=14ωi2.Since it acts in thez-axis,(20)TB=00T=kt00∑i=1ωi2.


#### 3.1.5. State-Space Model

From the rotational dynamics discussed, equation (8) can be rewritten as follows:(21)ω˙B=I−1−ωB×IωB−Γ+τB,ω˙B=I−1pqr×IxxpIyyqIzzr−Jrpqrωr+τB,p˙q˙r˙=Iyy−IzzqrIxxIzz−IxxprIyyIxx−IyypqIzz−JrqIxx−pIyy0ωr+τϕIxxτθIyyτψIzz.From the linear dynamics discussed, equation (16) can be written out as follows:(22)x¨y¨z¨=−g001+Tmcψsθcϕ+sψsϕsψsθcϕ+cψsϕcθcϕ,−1mkdx000kdy000kdzx˙y˙z˙.


### 3.2. Quadcopter System Stabilization

A quadcopter has only four control inputs and primarily six outputs of interestxyzϕθψ which makes it an underactuated system. This is resolved by separating it into two different control loops, with one working with attitude states and the other with position states. The quadcopter’s angular motion is independent of the linear components, but the linear motion is determined by the angle variables. As a result, the goal is to control the attitude variable, which is independent of the linear motion and then control the linear motion. It is possible to integrate the attitude control with the trajectory controller once it is designed and optimized. The control architecture to be implemented on the quadcopter system is shown in Figure 3.Figure 3
Control architecture.


#### 3.2.1. PID Control

The general form of a PID controller is given as follows:(23)et=rt−yt,ut=KPet+KI∫0teτdτ+KDddtet,where u(t) represents the control input, r(t) represents the desired state, and y(t) is the current or actual state. KP, KI, and KD are the gain parameters for the proportional, integral, and derivative gains of the PID controller.The proportional and derivative gains would be used for the quadcopter control. The torque generated is proportional to the angular velocities; therefore, we set the torques to be proportional to the controller output such that(24)τ=I×ut,τϕτθτψ=IxxkPϕϕdes−ϕ+kDϕϕ˙des−ϕ˙IyykPθθdes−θ+kDθθ˙des−θ˙IzzkPψψdes−ψ+kDψψ˙des−ψ˙.The input to the quadcopter system is the angular velocity of the rotors. Recall equation (15)relates the torque to the square of the angular velocity of the rotors; there are three equations, but four unknowns. The reason for this singularity is the use of Euler angles to model the quadcopter dynamics rather than quaternions [35]; physical singularities also occur due to the limitation of underactuation in the dynamics of a quadcopter.To allow simplification, the total thrustT which affects the acceleration in the z-direction, is set to be equal to mg. This constraint is enforced to keep the quadcopter flying. Converting this thrust equation to the appropriate reference frame and utilizing a PD to minimize the error in the z-axis,(25)T=g+kPzzdes−z+kDzz˙des−z˙mcϕcθ.Solving for the angular velocities of the rotorω12,ω22,ω32,ω42 by computing equation (23) and equating it to equation (15),(26)ω12ω22ω32ω42=ktktktkt0−lkt0lkt−lkt0lkt0−kbkb−kbkb−1Tτϕτθτψ.Simplifying equation (25) would give the equations as follows:(27)ω12=T4kt−τθ2lkt−τψ4kb,ω22=T4kt−τϕ2lkt+τψ4kb,ω32=T4kt+τθ2lkt−τψ4kb,ω42=T4kt+τϕ2lkt+τψ4kb.


#### 3.2.2. Feedback Linearization

Feedback linearization is a nonlinear control technique that has sparked a lot of interest recently. The main concept is to mathematically convert nonlinear system dynamics into (fully or partially) linear ones, allowing linear control methods to be used. The goal is to design a controller such that it exactly cancels out the system dynamics.From equation (21), we can replace x¨=kaxx˙−x˙d, x˙=kdxkxx−xd, where kax, kdx, and kx are gain values and replace y¨,y˙,z¨,andz˙ accordingly.(28)Tmcψsθcϕ+sψsϕsψsθcϕ+cψsϕcθcϕ=kaxx˙−x˙dkayy˙−y˙dkazz˙−z˙d+g001+1mkdxkxx−xdkdykyy−ydkdzkzz−zd.Expanding equation (26) would give the following equation:(29)kaxx˙−x˙d+kdxkxmx−xd=Tmcψsθcϕ,kayy˙−y˙d+kdykymy−yd=Tmsψsθcϕ+cψsϕ,kazz˙−z˙d+g+kdzkzmz−zd=Tmsψsθcϕ+cθcϕ.Taking the square of both sides and adding equations together, we get the following equation:(30)T2m2=kax2x˙−x˙d2+kdx2kx2mx−xd2+2kaxkdxkxmx−xdx˙−x˙d+kay2y˙−y˙d2+kdy2ky2my−yd2+2kaykdykymy−ydy˙−y˙d+kaz2z˙−z˙d2+kdz2kz2mz−zd+g2+2gkazz˙−z˙d+2gkdzkzmz−zd+2kazkdzkzmz−zdz˙−z˙d.To obtainϕd, from the x- and y-axis portion of equation (27), let(31)Fx=cψsθcϕ+sψsϕ,Fy=sψsθcϕ−cψsϕ.From equation (30),we get the following:(32)sθcϕ=Fy+cψsϕsψ.Substituting equation (31) into equation (30),we get the following:(33)Fx=cψsψFy+cψsϕ+sψsϕ,sϕ=Fx−Fy/tψcψ+sψtψ/tψ,sϕ=Fxtψ−Fycψ+sψtψ,ϕd=sin−1Fxtanψ−Fycosψ+sinψtanψ.To obtainθd, from the x- and z-axis portion of equation (27), let(34)Fz=cθcϕ,cϕ=Fzcθ.Substituting equations (32) and (33) into equation (30),(35)Fx=Fzcψsθcθ+Fxtψsψ−Fysψcψ+sψtψ,Fzcψtθ=Fx−Fxtψsψ−Fysψcψ+sψtψ,tθ=FxFzcψ−Fxtψsψ−FysψFzc2ψ+Fzs2ψ.We recall(36)cos2x+sin2x=1,tθ=FxFzcψ−Fxtψsψ−FysψFz,tθ=Fx−Fxs2ψ−FysψcψFzcψ,tθ=Fxc2ψ−FysψcψFzcψ,θd=tan−1Fxcos2ψ−FysinψcosψFzcosψ.For equations (32) and (34), Fx,Fy, and Fz are given as follows:(37)Fx=mkaxx˙−x˙d+kdxkxx−xdT,Fy=mkayy˙−y˙d+kdykyy−ydT,Fz=mkazz˙−z˙d+mg+kdzkzz−zdT.From equations (6) and (23) in terms of desired angles, we can replace ϕd=kϕϕ−ϕd, where kϕ is the gain values, and replace θd and ψd accordingly:(38)pdqdrd=1sϕdtθdcϕdtθd0cϕd−sϕd0sϕdcθdcϕdcθd−1kϕϕ−ϕdkθθ−θdkψψ−ψd˙,τϕτθτψ=IxxkPϕp−pdIyykPθq−qdIzzkPψr−rd−Iyy−Izzqdrd+JrqdωrIzz−Ixxpdrd+JrpdωrIxx−Iyypdqd.The angular velocitiesω1,ω2,ω3, and ω4 can be obtained using equation (25).


### 3.3. Trajectory Tracking

Trajectory control aims to take the quadcopter system from its current position to the desired position by regulating the quadcopter’s rotor angular velocities. Due to its complex dynamics, finding the best quadcopter trajectory is a huge challenge.


#### 3.3.1. Trajectory Tracking with PD Control

To relate the desired positionx and y, which are not controllable to the desired angles of ϕ and θ that are controllable, we make two assumptions:(i)
Small-angle approximation such thatsinx≈x and cosx≈1.(ii)
The desiredψ angle is zero.From the above assumptions, equation (21) can be simplified as follows.Equations (27) and (28) can be rewritten as follows:(40)θdes=1Tmx¨des+kdxx˙des+kPxyxdes−x,ϕdes=−1Tmy¨des+kdyy˙des+kPxyydes−y,Tdes=mz¨des+g+kdzz˙des.θdes, ϕdes, and Tdes represent the desired values of pitch angle, roll angle, and thrust.


#### 3.3.2. Trajectory Tracking with FBL Control

To relate the desired positionx and y, which are not controllable to the desired angles of ϕ and θ that are controllable using FBL control techniques, we make two assumptions:(i)
No small-angle approximation(ii)
he desiredψ angle is zero


## 3.1. Mathematical Modelling

A quadcopter is an aerial vehicle with four rotors that enable motion in different directions. Proper explanation of the system dynamics would require comprehension of the 6 degrees of freedom concept, which presents the position and orientation of the quadcopter in three dimensions (3-D).The 6 degrees of freedom (Figure2) describes the position of a body with six coordinates categorized in two reference frames. The first reference frame is the fixed coordinate system known as the inertial or Earth frame, which is depicted by the x, y, and z coordinates in the cardinal points of north, east, and down. The second reference frame is a mobile coordinate system known as the body frame, which is depicted by the ϕ, θ, and ψ angles with respect to the body center of gravity. The quadcopter is an underactuated nonlinear system because it has four inputs and six outputs. The system is complex and to control it, the quadcopter is modelled on the following assumptions [1]:(i)
The structure is rigid(ii)
The structure is axis symmetrical(iii)
The center of gravity and the body-fixed frame origin coincide(iv)
The propellers are rigid(v)
Thrust and drag are proportional to the square of the propeller’s speed.Figure 2
Six degrees of freedom.


### 3.1.1. Euler Angles

The Euler angles are three angles introduced by Leonhard Euler to describe the orientation of a rigid body in a coordinate system. They are also used to describe the relationship between two reference frames and convert the coordinates of a point in one reference frame to coordinates of the same point in another reference frame. Euler angles are denoted asϕ, θ, and ψ for the roll, pitch, and yaw angles, respectively, and represent the rotations of a body about the axes of a coordinate system.Any orientation of a rigid body can be achieved by the combination of the three basic Euler angles. The rotation matrices are given by the following equation [5]:(1)Rxϕ=1000cϕ−sϕ0sϕcϕ,Ryϕ=cϕ0sϕ010−sϕ0cϕ,Rzϕ=cψ−sψ0sψcψ0001,where cϕ=cosϕ,sϕ=sinϕ,cθ=cosθ,sθ=sinθ,cψ=cosψ,sψ=sinψ. The rotation matrix depicting the relationship between the inertial frame and the body frame is given as follows:(2)R=Rzψ×Ryθ×Rxϕ,R=cθcψsϕsθcψ−cϕsψcϕsθcψ+sϕsψcθsψsϕsθsψ+cϕcψcϕsθsψ−sϕcψ−sθsϕcθcϕcθ.R is a rotational matrix and is orthogonal such thatR−1=RT.


### 3.1.2. Reference Frame Transformation

We assumexyzϕθψT to be a vector of linear and angular positions in the inertial frame and let uvωpqrT be a vector of linear and angular velocities in the body frame.Typically, the derivative gain of angular positions should give angular velocities, but the angular positions and velocities above are in a different frame, so we require some transformation matrix to convert from one reference frame to another.Let(3)ξ=xyzT,η=ϕθψT.Let(4)vI=x˙y˙z˙T,ωI=ϕ˙θ˙ψ˙T.Let(5)vB=uvωT,ωB=pqrT.From this statement,vI≠vBandωI≠ωB, instead(6)vI=R⋅vB,ωI=ωη−1⋅ωB,ϕ˙θ˙ψ˙=1sϕtθcϕtθ0cϕ−sϕ0sϕcθcϕcθpqr.Conversely,(7)ωB=ωη⋅ωI,pqr=10−sθ0cϕsϕcθ0−sϕcϕcθϕ˙θ˙ψ˙.


### 3.1.3. Rotational Motion

Assuming the quadcopter is a rigid body and using Euler’s equations for rigid bodies, the dynamics equation in the body frame is given as follows:(8)Iω˙B+ωB×IωB+Γ=τB.We also assume that the quadcopter has a symmetric structure with the four arms aligned with the bodyx- and y-axes. Therefore, the inertia matrix I is a diagonal matrix in which Ixx=Iyy.(9)I=Ixx000Iyy000Izz.The gyroscopic forcesΓ are caused by the combined rotation of the four rotors and the quadcopter body such that the equation is as follows:(10)Γ=∑i=14JrωB∧e^3−1i+1ωri.In matrix form,(11)Γ=JrωBωrwhereωr=−ω1+ω2−ω3+ω4.The external torque(12)τB=τϕτθτψT.The roll torque componentτϕ and the pitch torque component τθ are obtained from standard mechanics where i=1 and i=3 motors are arbitrarily chosen to be on the roll-axis while i=2 and i=4 are arbitrarily chosen to be on the pitch-axis.(13)τϕ=∑r×T=l−ktω22+ktω42=lkt−ω22+ω42,τθ=∑r×T=l−ktω12+ktω32=lkt−ω12+ω32.For the yaw-axis, the rotor axis is pointing in thez-direction in the body frame, and the torque created around the rotor axis is given as follows:(14)τψ=−1i+1kbωi2+Imωi,where −1i+1 is positive for the ith propeller if the propeller is spinning clockwise and negative if it is spinning counter-clockwise. The term Imω˙i can be ignored because it is in a steady-state, ω˙i≈0.Therefore, the total torque about thez-axis is given by the sum of all the torques from each propeller as follows:(15)τψ=kb−ω12+ω22−ω32+ω42.Therefore, the torque matrix can be written as follows:(16)τB=τϕτθτψ=lkt−ω22+ω42lkt−ω12+ω32kb−ω12+ω22−ω32+ω42.where kt is the thrust coefficient, kb is the drag coefficient, and l is the distance between the rotor and the center of mass of the quadcopter.


### 3.1.4. Translational Motion

Using Newtonian equation to model the linear dynamics, the extraneous forces acting on the quadcopter are given as follows:(17)m˙v1=00−mg+RTB+FD.Drag forceFD is the force acting on the system as a result of fluid friction (air resistance). A simplified equation form is used where friction is modelled as being proportional to the linear velocity in all directions.(18)FD=−kdvI.The angular velocity of theith rotor creates a force Fi in the direction of the rotor axis (z-direction). The combined forces create thrust T in the direction of the body z-axis such that(19)T=∑i=14Fi=kt∑i=14ωi2.Since it acts in thez-axis,(20)TB=00T=kt00∑i=1ωi2.


### 3.1.5. State-Space Model

From the rotational dynamics discussed, equation (8) can be rewritten as follows:(21)ω˙B=I−1−ωB×IωB−Γ+τB,ω˙B=I−1pqr×IxxpIyyqIzzr−Jrpqrωr+τB,p˙q˙r˙=Iyy−IzzqrIxxIzz−IxxprIyyIxx−IyypqIzz−JrqIxx−pIyy0ωr+τϕIxxτθIyyτψIzz.From the linear dynamics discussed, equation (16) can be written out as follows:(22)x¨y¨z¨=−g001+Tmcψsθcϕ+sψsϕsψsθcϕ+cψsϕcθcϕ,−1mkdx000kdy000kdzx˙y˙z˙.


## 3.1.1. Euler Angles

The Euler angles are three angles introduced by Leonhard Euler to describe the orientation of a rigid body in a coordinate system. They are also used to describe the relationship between two reference frames and convert the coordinates of a point in one reference frame to coordinates of the same point in another reference frame. Euler angles are denoted asϕ, θ, and ψ for the roll, pitch, and yaw angles, respectively, and represent the rotations of a body about the axes of a coordinate system.Any orientation of a rigid body can be achieved by the combination of the three basic Euler angles. The rotation matrices are given by the following equation [5]:(1)Rxϕ=1000cϕ−sϕ0sϕcϕ,Ryϕ=cϕ0sϕ010−sϕ0cϕ,Rzϕ=cψ−sψ0sψcψ0001,where cϕ=cosϕ,sϕ=sinϕ,cθ=cosθ,sθ=sinθ,cψ=cosψ,sψ=sinψ. The rotation matrix depicting the relationship between the inertial frame and the body frame is given as follows:(2)R=Rzψ×Ryθ×Rxϕ,R=cθcψsϕsθcψ−cϕsψcϕsθcψ+sϕsψcθsψsϕsθsψ+cϕcψcϕsθsψ−sϕcψ−sθsϕcθcϕcθ.R is a rotational matrix and is orthogonal such thatR−1=RT.


## 3.1.2. Reference Frame Transformation

We assumexyzϕθψT to be a vector of linear and angular positions in the inertial frame and let uvωpqrT be a vector of linear and angular velocities in the body frame.Typically, the derivative gain of angular positions should give angular velocities, but the angular positions and velocities above are in a different frame, so we require some transformation matrix to convert from one reference frame to another.Let(3)ξ=xyzT,η=ϕθψT.Let(4)vI=x˙y˙z˙T,ωI=ϕ˙θ˙ψ˙T.Let(5)vB=uvωT,ωB=pqrT.From this statement,vI≠vBandωI≠ωB, instead(6)vI=R⋅vB,ωI=ωη−1⋅ωB,ϕ˙θ˙ψ˙=1sϕtθcϕtθ0cϕ−sϕ0sϕcθcϕcθpqr.Conversely,(7)ωB=ωη⋅ωI,pqr=10−sθ0cϕsϕcθ0−sϕcϕcθϕ˙θ˙ψ˙.


## 3.1.3. Rotational Motion

Assuming the quadcopter is a rigid body and using Euler’s equations for rigid bodies, the dynamics equation in the body frame is given as follows:(8)Iω˙B+ωB×IωB+Γ=τB.We also assume that the quadcopter has a symmetric structure with the four arms aligned with the bodyx- and y-axes. Therefore, the inertia matrix I is a diagonal matrix in which Ixx=Iyy.(9)I=Ixx000Iyy000Izz.The gyroscopic forcesΓ are caused by the combined rotation of the four rotors and the quadcopter body such that the equation is as follows:(10)Γ=∑i=14JrωB∧e^3−1i+1ωri.In matrix form,(11)Γ=JrωBωrwhereωr=−ω1+ω2−ω3+ω4.The external torque(12)τB=τϕτθτψT.The roll torque componentτϕ and the pitch torque component τθ are obtained from standard mechanics where i=1 and i=3 motors are arbitrarily chosen to be on the roll-axis while i=2 and i=4 are arbitrarily chosen to be on the pitch-axis.(13)τϕ=∑r×T=l−ktω22+ktω42=lkt−ω22+ω42,τθ=∑r×T=l−ktω12+ktω32=lkt−ω12+ω32.For the yaw-axis, the rotor axis is pointing in thez-direction in the body frame, and the torque created around the rotor axis is given as follows:(14)τψ=−1i+1kbωi2+Imωi,where −1i+1 is positive for the ith propeller if the propeller is spinning clockwise and negative if it is spinning counter-clockwise. The term Imω˙i can be ignored because it is in a steady-state, ω˙i≈0.Therefore, the total torque about thez-axis is given by the sum of all the torques from each propeller as follows:(15)τψ=kb−ω12+ω22−ω32+ω42.Therefore, the torque matrix can be written as follows:(16)τB=τϕτθτψ=lkt−ω22+ω42lkt−ω12+ω32kb−ω12+ω22−ω32+ω42.where kt is the thrust coefficient, kb is the drag coefficient, and l is the distance between the rotor and the center of mass of the quadcopter.


## 3.1.4. Translational Motion

Using Newtonian equation to model the linear dynamics, the extraneous forces acting on the quadcopter are given as follows:(17)m˙v1=00−mg+RTB+FD.Drag forceFD is the force acting on the system as a result of fluid friction (air resistance). A simplified equation form is used where friction is modelled as being proportional to the linear velocity in all directions.(18)FD=−kdvI.The angular velocity of theith rotor creates a force Fi in the direction of the rotor axis (z-direction). The combined forces create thrust T in the direction of the body z-axis such that(19)T=∑i=14Fi=kt∑i=14ωi2.Since it acts in thez-axis,(20)TB=00T=kt00∑i=1ωi2.


## 3.1.5. State-Space Model

From the rotational dynamics discussed, equation (8) can be rewritten as follows:(21)ω˙B=I−1−ωB×IωB−Γ+τB,ω˙B=I−1pqr×IxxpIyyqIzzr−Jrpqrωr+τB,p˙q˙r˙=Iyy−IzzqrIxxIzz−IxxprIyyIxx−IyypqIzz−JrqIxx−pIyy0ωr+τϕIxxτθIyyτψIzz.From the linear dynamics discussed, equation (16) can be written out as follows:(22)x¨y¨z¨=−g001+Tmcψsθcϕ+sψsϕsψsθcϕ+cψsϕcθcϕ,−1mkdx000kdy000kdzx˙y˙z˙.


## 3.2. Quadcopter System Stabilization

A quadcopter has only four control inputs and primarily six outputs of interestxyzϕθψ which makes it an underactuated system. This is resolved by separating it into two different control loops, with one working with attitude states and the other with position states. The quadcopter’s angular motion is independent of the linear components, but the linear motion is determined by the angle variables. As a result, the goal is to control the attitude variable, which is independent of the linear motion and then control the linear motion. It is possible to integrate the attitude control with the trajectory controller once it is designed and optimized. The control architecture to be implemented on the quadcopter system is shown in Figure 3.Figure 3
Control architecture.


### 3.2.1. PID Control

The general form of a PID controller is given as follows:(23)et=rt−yt,ut=KPet+KI∫0teτdτ+KDddtet,where u(t) represents the control input, r(t) represents the desired state, and y(t) is the current or actual state. KP, KI, and KD are the gain parameters for the proportional, integral, and derivative gains of the PID controller.The proportional and derivative gains would be used for the quadcopter control. The torque generated is proportional to the angular velocities; therefore, we set the torques to be proportional to the controller output such that(24)τ=I×ut,τϕτθτψ=IxxkPϕϕdes−ϕ+kDϕϕ˙des−ϕ˙IyykPθθdes−θ+kDθθ˙des−θ˙IzzkPψψdes−ψ+kDψψ˙des−ψ˙.The input to the quadcopter system is the angular velocity of the rotors. Recall equation (15)relates the torque to the square of the angular velocity of the rotors; there are three equations, but four unknowns. The reason for this singularity is the use of Euler angles to model the quadcopter dynamics rather than quaternions [35]; physical singularities also occur due to the limitation of underactuation in the dynamics of a quadcopter.To allow simplification, the total thrustT which affects the acceleration in the z-direction, is set to be equal to mg. This constraint is enforced to keep the quadcopter flying. Converting this thrust equation to the appropriate reference frame and utilizing a PD to minimize the error in the z-axis,(25)T=g+kPzzdes−z+kDzz˙des−z˙mcϕcθ.Solving for the angular velocities of the rotorω12,ω22,ω32,ω42 by computing equation (23) and equating it to equation (15),(26)ω12ω22ω32ω42=ktktktkt0−lkt0lkt−lkt0lkt0−kbkb−kbkb−1Tτϕτθτψ.Simplifying equation (25) would give the equations as follows:(27)ω12=T4kt−τθ2lkt−τψ4kb,ω22=T4kt−τϕ2lkt+τψ4kb,ω32=T4kt+τθ2lkt−τψ4kb,ω42=T4kt+τϕ2lkt+τψ4kb.


### 3.2.2. Feedback Linearization

Feedback linearization is a nonlinear control technique that has sparked a lot of interest recently. The main concept is to mathematically convert nonlinear system dynamics into (fully or partially) linear ones, allowing linear control methods to be used. The goal is to design a controller such that it exactly cancels out the system dynamics.From equation (21), we can replace x¨=kaxx˙−x˙d, x˙=kdxkxx−xd, where kax, kdx, and kx are gain values and replace y¨,y˙,z¨,andz˙ accordingly.(28)Tmcψsθcϕ+sψsϕsψsθcϕ+cψsϕcθcϕ=kaxx˙−x˙dkayy˙−y˙dkazz˙−z˙d+g001+1mkdxkxx−xdkdykyy−ydkdzkzz−zd.Expanding equation (26) would give the following equation:(29)kaxx˙−x˙d+kdxkxmx−xd=Tmcψsθcϕ,kayy˙−y˙d+kdykymy−yd=Tmsψsθcϕ+cψsϕ,kazz˙−z˙d+g+kdzkzmz−zd=Tmsψsθcϕ+cθcϕ.Taking the square of both sides and adding equations together, we get the following equation:(30)T2m2=kax2x˙−x˙d2+kdx2kx2mx−xd2+2kaxkdxkxmx−xdx˙−x˙d+kay2y˙−y˙d2+kdy2ky2my−yd2+2kaykdykymy−ydy˙−y˙d+kaz2z˙−z˙d2+kdz2kz2mz−zd+g2+2gkazz˙−z˙d+2gkdzkzmz−zd+2kazkdzkzmz−zdz˙−z˙d.To obtainϕd, from the x- and y-axis portion of equation (27), let(31)Fx=cψsθcϕ+sψsϕ,Fy=sψsθcϕ−cψsϕ.From equation (30),we get the following:(32)sθcϕ=Fy+cψsϕsψ.Substituting equation (31) into equation (30),we get the following:(33)Fx=cψsψFy+cψsϕ+sψsϕ,sϕ=Fx−Fy/tψcψ+sψtψ/tψ,sϕ=Fxtψ−Fycψ+sψtψ,ϕd=sin−1Fxtanψ−Fycosψ+sinψtanψ.To obtainθd, from the x- and z-axis portion of equation (27), let(34)Fz=cθcϕ,cϕ=Fzcθ.Substituting equations (32) and (33) into equation (30),(35)Fx=Fzcψsθcθ+Fxtψsψ−Fysψcψ+sψtψ,Fzcψtθ=Fx−Fxtψsψ−Fysψcψ+sψtψ,tθ=FxFzcψ−Fxtψsψ−FysψFzc2ψ+Fzs2ψ.We recall(36)cos2x+sin2x=1,tθ=FxFzcψ−Fxtψsψ−FysψFz,tθ=Fx−Fxs2ψ−FysψcψFzcψ,tθ=Fxc2ψ−FysψcψFzcψ,θd=tan−1Fxcos2ψ−FysinψcosψFzcosψ.For equations (32) and (34), Fx,Fy, and Fz are given as follows:(37)Fx=mkaxx˙−x˙d+kdxkxx−xdT,Fy=mkayy˙−y˙d+kdykyy−ydT,Fz=mkazz˙−z˙d+mg+kdzkzz−zdT.From equations (6) and (23) in terms of desired angles, we can replace ϕd=kϕϕ−ϕd, where kϕ is the gain values, and replace θd and ψd accordingly:(38)pdqdrd=1sϕdtθdcϕdtθd0cϕd−sϕd0sϕdcθdcϕdcθd−1kϕϕ−ϕdkθθ−θdkψψ−ψd˙,τϕτθτψ=IxxkPϕp−pdIyykPθq−qdIzzkPψr−rd−Iyy−Izzqdrd+JrqdωrIzz−Ixxpdrd+JrpdωrIxx−Iyypdqd.The angular velocitiesω1,ω2,ω3, and ω4 can be obtained using equation (25).


## 3.2.1. PID Control

The general form of a PID controller is given as follows:(23)et=rt−yt,ut=KPet+KI∫0teτdτ+KDddtet,where u(t) represents the control input, r(t) represents the desired state, and y(t) is the current or actual state. KP, KI, and KD are the gain parameters for the proportional, integral, and derivative gains of the PID controller.The proportional and derivative gains would be used for the quadcopter control. The torque generated is proportional to the angular velocities; therefore, we set the torques to be proportional to the controller output such that(24)τ=I×ut,τϕτθτψ=IxxkPϕϕdes−ϕ+kDϕϕ˙des−ϕ˙IyykPθθdes−θ+kDθθ˙des−θ˙IzzkPψψdes−ψ+kDψψ˙des−ψ˙.The input to the quadcopter system is the angular velocity of the rotors. Recall equation (15)relates the torque to the square of the angular velocity of the rotors; there are three equations, but four unknowns. The reason for this singularity is the use of Euler angles to model the quadcopter dynamics rather than quaternions [35]; physical singularities also occur due to the limitation of underactuation in the dynamics of a quadcopter.To allow simplification, the total thrustT which affects the acceleration in the z-direction, is set to be equal to mg. This constraint is enforced to keep the quadcopter flying. Converting this thrust equation to the appropriate reference frame and utilizing a PD to minimize the error in the z-axis,(25)T=g+kPzzdes−z+kDzz˙des−z˙mcϕcθ.Solving for the angular velocities of the rotorω12,ω22,ω32,ω42 by computing equation (23) and equating it to equation (15),(26)ω12ω22ω32ω42=ktktktkt0−lkt0lkt−lkt0lkt0−kbkb−kbkb−1Tτϕτθτψ.Simplifying equation (25) would give the equations as follows:(27)ω12=T4kt−τθ2lkt−τψ4kb,ω22=T4kt−τϕ2lkt+τψ4kb,ω32=T4kt+τθ2lkt−τψ4kb,ω42=T4kt+τϕ2lkt+τψ4kb.


## 3.2.2. Feedback Linearization

Feedback linearization is a nonlinear control technique that has sparked a lot of interest recently. The main concept is to mathematically convert nonlinear system dynamics into (fully or partially) linear ones, allowing linear control methods to be used. The goal is to design a controller such that it exactly cancels out the system dynamics.From equation (21), we can replace x¨=kaxx˙−x˙d, x˙=kdxkxx−xd, where kax, kdx, and kx are gain values and replace y¨,y˙,z¨,andz˙ accordingly.(28)Tmcψsθcϕ+sψsϕsψsθcϕ+cψsϕcθcϕ=kaxx˙−x˙dkayy˙−y˙dkazz˙−z˙d+g001+1mkdxkxx−xdkdykyy−ydkdzkzz−zd.Expanding equation (26) would give the following equation:(29)kaxx˙−x˙d+kdxkxmx−xd=Tmcψsθcϕ,kayy˙−y˙d+kdykymy−yd=Tmsψsθcϕ+cψsϕ,kazz˙−z˙d+g+kdzkzmz−zd=Tmsψsθcϕ+cθcϕ.Taking the square of both sides and adding equations together, we get the following equation:(30)T2m2=kax2x˙−x˙d2+kdx2kx2mx−xd2+2kaxkdxkxmx−xdx˙−x˙d+kay2y˙−y˙d2+kdy2ky2my−yd2+2kaykdykymy−ydy˙−y˙d+kaz2z˙−z˙d2+kdz2kz2mz−zd+g2+2gkazz˙−z˙d+2gkdzkzmz−zd+2kazkdzkzmz−zdz˙−z˙d.To obtainϕd, from the x- and y-axis portion of equation (27), let(31)Fx=cψsθcϕ+sψsϕ,Fy=sψsθcϕ−cψsϕ.From equation (30),we get the following:(32)sθcϕ=Fy+cψsϕsψ.Substituting equation (31) into equation (30),we get the following:(33)Fx=cψsψFy+cψsϕ+sψsϕ,sϕ=Fx−Fy/tψcψ+sψtψ/tψ,sϕ=Fxtψ−Fycψ+sψtψ,ϕd=sin−1Fxtanψ−Fycosψ+sinψtanψ.To obtainθd, from the x- and z-axis portion of equation (27), let(34)Fz=cθcϕ,cϕ=Fzcθ.Substituting equations (32) and (33) into equation (30),(35)Fx=Fzcψsθcθ+Fxtψsψ−Fysψcψ+sψtψ,Fzcψtθ=Fx−Fxtψsψ−Fysψcψ+sψtψ,tθ=FxFzcψ−Fxtψsψ−FysψFzc2ψ+Fzs2ψ.We recall(36)cos2x+sin2x=1,tθ=FxFzcψ−Fxtψsψ−FysψFz,tθ=Fx−Fxs2ψ−FysψcψFzcψ,tθ=Fxc2ψ−FysψcψFzcψ,θd=tan−1Fxcos2ψ−FysinψcosψFzcosψ.For equations (32) and (34), Fx,Fy, and Fz are given as follows:(37)Fx=mkaxx˙−x˙d+kdxkxx−xdT,Fy=mkayy˙−y˙d+kdykyy−ydT,Fz=mkazz˙−z˙d+mg+kdzkzz−zdT.From equations (6) and (23) in terms of desired angles, we can replace ϕd=kϕϕ−ϕd, where kϕ is the gain values, and replace θd and ψd accordingly:(38)pdqdrd=1sϕdtθdcϕdtθd0cϕd−sϕd0sϕdcθdcϕdcθd−1kϕϕ−ϕdkθθ−θdkψψ−ψd˙,τϕτθτψ=IxxkPϕp−pdIyykPθq−qdIzzkPψr−rd−Iyy−Izzqdrd+JrqdωrIzz−Ixxpdrd+JrpdωrIxx−Iyypdqd.The angular velocitiesω1,ω2,ω3, and ω4 can be obtained using equation (25).


## 3.3. Trajectory Tracking

Trajectory control aims to take the quadcopter system from its current position to the desired position by regulating the quadcopter’s rotor angular velocities. Due to its complex dynamics, finding the best quadcopter trajectory is a huge challenge.


### 3.3.1. Trajectory Tracking with PD Control

To relate the desired positionx and y, which are not controllable to the desired angles of ϕ and θ that are controllable, we make two assumptions:(i)
Small-angle approximation such thatsinx≈x and cosx≈1.(ii)
The desiredψ angle is zero.From the above assumptions, equation (21) can be simplified as follows.Equations (27) and (28) can be rewritten as follows:(40)θdes=1Tmx¨des+kdxx˙des+kPxyxdes−x,ϕdes=−1Tmy¨des+kdyy˙des+kPxyydes−y,Tdes=mz¨des+g+kdzz˙des.θdes, ϕdes, and Tdes represent the desired values of pitch angle, roll angle, and thrust.


### 3.3.2. Trajectory Tracking with FBL Control

To relate the desired positionx and y, which are not controllable to the desired angles of ϕ and θ that are controllable using FBL control techniques, we make two assumptions:(i)
No small-angle approximation(ii)
he desiredψ angle is zero


## 3.3.1. Trajectory Tracking with PD Control

To relate the desired positionx and y, which are not controllable to the desired angles of ϕ and θ that are controllable, we make two assumptions:(i)
Small-angle approximation such thatsinx≈x and cosx≈1.(ii)
The desiredψ angle is zero.From the above assumptions, equation (21) can be simplified as follows.Equations (27) and (28) can be rewritten as follows:(40)θdes=1Tmx¨des+kdxx˙des+kPxyxdes−x,ϕdes=−1Tmy¨des+kdyy˙des+kPxyydes−y,Tdes=mz¨des+g+kdzz˙des.θdes, ϕdes, and Tdes represent the desired values of pitch angle, roll angle, and thrust.


## 3.3.2. Trajectory Tracking with FBL Control

To relate the desired positionx and y, which are not controllable to the desired angles of ϕ and θ that are controllable using FBL control techniques, we make two assumptions:(i)
No small-angle approximation(ii)
he desiredψ angle is zero


## 4. Simulation and Results

The dynamic model and controllers are implemented in MATLAB 2021 for simulation with MATLAB programming language. For attitude stabilization, a PD controller is implemented on the nonlinear system to stabilize and drive the system states to zero. Simulation results for trajectory tracking using both PD and FBL control are presented.The quadcopter model is simulated with parameter values, as shown in Table1 [8]. The initial conditions assigned to the system for simulation are given in Table 2.Table 1
Quadcopter parameter values for simulation.
ParameterValueUnitG9.81m/s2M0.468kgL0.225mJr3.357×10−5kg m2kt2.980×10−6kb1.140×10−7Ixx4.856×10−3kg m2Iyy4.856×10−3kg m2Izz8.801×10−3kg m2kdx0.25kdy0.25kdz0.25(39)x¨des=Tmθdes−kdxmx˙des,y¨des=Tmϕdes−kdymy˙des,z¨des=−g+Tm−kdzmz˙des.Table 2
Initial conditions for simulation.
PositionValueStateValuex−1Φ10y2Θ−10z1Ψ5The simulation progresses at 0.001-second intervals to a total time of 15 seconds. The control inputs (angular velocities of the rotors) are shown in Figure4, and the positions x, y, and z and the angles ϕ, θ, and ψ are shown in Figure 5.Figure 4
Control inputs for the angular velocities of the rotors.Figure 5
Control inputs for the positionsx, y, and z and the angles ϕ, θ, and ψ.
(a)(b)The control input remains constant, and hence, the angles remain the same, but the positions become unstable.


### 4.1. Attitude Stabilization

A PD controller is implemented on the nonlinear system to stabilize and drive the system states to zero. The initial conditions assigned for the system remain the same, as shown in Table2. The control gain values for the PD controller are determined by manual tuning and are shown in Table 3.Table 3
Gain values for PD controller.
ParameterValueParameterValuekPϕ1.5kDϕ2.6kPθ1.5kDθ2.6kPψ1.5kDψ2.6kPz6kDz1.5The simulation progresses at 0.001-second intervals to a total time of 15 seconds. The anglesϕ, θ, and ψ are shown in Figure 6, the angular velocities of the rotors are shown in Figure 7, and the positions x, y, and z are shown in Figure 8. The angles are stabilized to zero after 10 seconds, and the position z is stabilized to zero after 5 seconds. The x and y positions do not stabilize to zero because the states are not observable.Figure 6
Result of anglesϕ,θ,andψ as a function of time for implemented PD controller to drive the system states to zero.Figure 7
The angular velocities of the rotors as a function of time for implemented PD controller for attitude stabilization.Figure 8
Positionsx, y, and z as a function of time for implemented PD controller for attitude stabilization.Using the concept of trajectory tracking, an attempt can be made to drive thex and y positions to zero. This can be done by mapping the zero desired states of x and y to the desired ϕ and θ states, which are controllable as shown in equations (28)–(30). An additional proportional controller gain kPxy, with a control gain of 0.04 is implemented to drive the system states to zero, and the simulation advances at 0.001-second intervals to a total time of 45 seconds. The angular velocities of the four rotors and the angles ϕ,θ,andψ are shown in Figure 9; the modified positions x,y, and z are shown in Figure 10.Figure 9
Control input and anglesϕ,θ,andψ with an additional proportional controller gain kPxy, with a control gain of 0.04, implemented to drive the system states to zero.
(a)(b)Figure 10
Positionsx, y, and z with an additional proportional controller gain kPxy, with a control gain of 0.04, implemented to drive the system states to zero.The anglesϕ, θ, and ψ are stabilized to zero after 35 seconds, and the positions x, y, and z are stabilized to zero after 45 seconds.


### 4.2. Trajectory Tracking

Two controls for trajectory tracking are considered and simulated for two different trajectories. The first control strategy is implemented with a PD controller, and the second strategy is a PD controller integrated with the linearized feedback system. The total time assigned for the simulation in both cases is 45 seconds.


#### 4.2.1. Trajectory Tracking with PD Controller

The comparison between the desired and actual trajectories for the first path is shown in Figure11. Figure 12 shows the graph of the position variables against time.Figure 11
Desired and actual trajectories with PD Controller.Figure 12
Coordinate-wise comparison between desired and actual trajectories of Figure11.
(a)(b)Another trajectory path is simulated, and the result is shown in Figure13. The graph of the position variables against time for this path is shown in Figure 14.Figure 13
Desired and actual trajectories with PD controller.Figure 14
Coordinate-wise comparison between desired and actual trajectories of Figure13.
(a)(b)A similar result as shown in Figure12 is also obtained; the quadcopter cannot follow the desired trajectory with precision.


#### 4.2.2. Trajectory Tracking with Feedback Linearization and PD Controller

The feedback linearization technique is implemented on the quadcopter model and simulated with a PD controller. The desired trajectory and the actual trajectory for the first path are shown in Figure15. Figure 16 shows the graph of the position variables against time.Figure 15
Desired and actual trajectories with feedback linearization and PD controller.Figure 16
Coordinate-wise comparison between desired and actual trajectories of Figure15.
(a)(b)Another trajectory path is simulated, and the result is shown in Figure17. A similar result as shown in Figure 15 is also obtained; the quadcopter can lock on to the desired trajectory with more accuracy with this control.Figure 17
Desired and actual trajectories with feedback linearization and PD controller.


## 4.1. Attitude Stabilization

A PD controller is implemented on the nonlinear system to stabilize and drive the system states to zero. The initial conditions assigned for the system remain the same, as shown in Table2. The control gain values for the PD controller are determined by manual tuning and are shown in Table 3.Table 3
Gain values for PD controller.
ParameterValueParameterValuekPϕ1.5kDϕ2.6kPθ1.5kDθ2.6kPψ1.5kDψ2.6kPz6kDz1.5The simulation progresses at 0.001-second intervals to a total time of 15 seconds. The anglesϕ, θ, and ψ are shown in Figure 6, the angular velocities of the rotors are shown in Figure 7, and the positions x, y, and z are shown in Figure 8. The angles are stabilized to zero after 10 seconds, and the position z is stabilized to zero after 5 seconds. The x and y positions do not stabilize to zero because the states are not observable.Figure 6
Result of anglesϕ,θ,andψ as a function of time for implemented PD controller to drive the system states to zero.Figure 7
The angular velocities of the rotors as a function of time for implemented PD controller for attitude stabilization.Figure 8
Positionsx, y, and z as a function of time for implemented PD controller for attitude stabilization.Using the concept of trajectory tracking, an attempt can be made to drive thex and y positions to zero. This can be done by mapping the zero desired states of x and y to the desired ϕ and θ states, which are controllable as shown in equations (28)–(30). An additional proportional controller gain kPxy, with a control gain of 0.04 is implemented to drive the system states to zero, and the simulation advances at 0.001-second intervals to a total time of 45 seconds. The angular velocities of the four rotors and the angles ϕ,θ,andψ are shown in Figure 9; the modified positions x,y, and z are shown in Figure 10.Figure 9
Control input and anglesϕ,θ,andψ with an additional proportional controller gain kPxy, with a control gain of 0.04, implemented to drive the system states to zero.
(a)(b)Figure 10
Positionsx, y, and z with an additional proportional controller gain kPxy, with a control gain of 0.04, implemented to drive the system states to zero.The anglesϕ, θ, and ψ are stabilized to zero after 35 seconds, and the positions x, y, and z are stabilized to zero after 45 seconds.


## 4.2. Trajectory Tracking

Two controls for trajectory tracking are considered and simulated for two different trajectories. The first control strategy is implemented with a PD controller, and the second strategy is a PD controller integrated with the linearized feedback system. The total time assigned for the simulation in both cases is 45 seconds.


### 4.2.1. Trajectory Tracking with PD Controller

The comparison between the desired and actual trajectories for the first path is shown in Figure11. Figure 12 shows the graph of the position variables against time.Figure 11
Desired and actual trajectories with PD Controller.Figure 12
Coordinate-wise comparison between desired and actual trajectories of Figure11.
(a)(b)Another trajectory path is simulated, and the result is shown in Figure13. The graph of the position variables against time for this path is shown in Figure 14.Figure 13
Desired and actual trajectories with PD controller.Figure 14
Coordinate-wise comparison between desired and actual trajectories of Figure13.
(a)(b)A similar result as shown in Figure12 is also obtained; the quadcopter cannot follow the desired trajectory with precision.


### 4.2.2. Trajectory Tracking with Feedback Linearization and PD Controller

The feedback linearization technique is implemented on the quadcopter model and simulated with a PD controller. The desired trajectory and the actual trajectory for the first path are shown in Figure15. Figure 16 shows the graph of the position variables against time.Figure 15
Desired and actual trajectories with feedback linearization and PD controller.Figure 16
Coordinate-wise comparison between desired and actual trajectories of Figure15.
(a)(b)Another trajectory path is simulated, and the result is shown in Figure17. A similar result as shown in Figure 15 is also obtained; the quadcopter can lock on to the desired trajectory with more accuracy with this control.Figure 17
Desired and actual trajectories with feedback linearization and PD controller.


## 4.2.1. Trajectory Tracking with PD Controller

The comparison between the desired and actual trajectories for the first path is shown in Figure11. Figure 12 shows the graph of the position variables against time.Figure 11
Desired and actual trajectories with PD Controller.Figure 12
Coordinate-wise comparison between desired and actual trajectories of Figure11.
(a)(b)Another trajectory path is simulated, and the result is shown in Figure13. The graph of the position variables against time for this path is shown in Figure 14.Figure 13
Desired and actual trajectories with PD controller.Figure 14
Coordinate-wise comparison between desired and actual trajectories of Figure13.
(a)(b)A similar result as shown in Figure12 is also obtained; the quadcopter cannot follow the desired trajectory with precision.


## 4.2.2. Trajectory Tracking with Feedback Linearization and PD Controller

The feedback linearization technique is implemented on the quadcopter model and simulated with a PD controller. The desired trajectory and the actual trajectory for the first path are shown in Figure15. Figure 16 shows the graph of the position variables against time.Figure 15
Desired and actual trajectories with feedback linearization and PD controller.Figure 16
Coordinate-wise comparison between desired and actual trajectories of Figure15.
(a)(b)Another trajectory path is simulated, and the result is shown in Figure17. A similar result as shown in Figure 15 is also obtained; the quadcopter can lock on to the desired trajectory with more accuracy with this control.Figure 17
Desired and actual trajectories with feedback linearization and PD controller.


## 5. Result Analysis

Detailed analysis on the performance of the PD controller for stabilization and its integration with the FBL system is required to determine the most suitable control strategy. The results show that the feedback linearization technique with the PD control is more effective in trajectory tracking than the regular PD control.


### 5.1. Altitude Stabilization

As discussed earlier, the PD controller with control gains shown in Table3 was able to stabilize the angles to the desired zero state after 10 seconds and drive the z position variable to zero in 5 seconds. The x and y positions are eventually stabilized, but they do not stabilize to the desired zero state. By mapping the desired states of x and y to the desired ϕ and θ states which are controllable, an additional proportional controller gain is used to drive all the system states to zero. However, this approach is inefficient because it takes too long for the angles and position to stabilize. The angles ϕ, θ, and ψ stabilize to zero after 35 seconds, and the positions x, y, and z stabilize to zero after 45 seconds. The ϕ, θ, x, and y states experience overshoots before stabilizing to zero. Side-by-side comparisons of the angle and position variables of the two approaches are shown in Figures 18 and 19.Figure 18
The anglesϕ,θ,andψ stabilize to zero after 35 s; the ϕ and θ states experience overshoots before stabilizing to zero.
(a)(b)Figure 19
The position variablesx, y, and z stabilize to zero after 45 s; the x and y states experience overshoots before stabilizing to zero.
(a)(b)


### 5.2. Trajectory Tracking

The results of the two control strategies implemented for trajectory tracking were previously discussed. Better performance is observed in the feedback linearized system with a PD control. The quadcopter is able to lock on to the desired trajectory and follow the path with accuracy in a lesser time period. Figures20–22 show the desired and actual trajectory plot after 15, 30, and 45 seconds, respectively, for both the PD control and the FBL with a PD control.Figure 20
The trajectory for PD (a) and FBL with PD control (b) after 15 seconds.
(a)(b)Figure 21
The trajectory for PD (a) and FBL with PD control (b) after 30 seconds.
(a)(b)Figure 22
The trajectory for PD (a) and FBL with PD control (b) after 45 seconds.
(a)(b)The quadcopter system simulated with the FBL and PD control is able to lock on to the desired trajectory after the first 15 seconds, while the system with only the PD control is unable to do that properly even after 30 seconds. This shows that the feedback linearization technique with the PD control is more effective in trajectory tracking than the regular PD control.


## 5.1. Altitude Stabilization

As discussed earlier, the PD controller with control gains shown in Table3 was able to stabilize the angles to the desired zero state after 10 seconds and drive the z position variable to zero in 5 seconds. The x and y positions are eventually stabilized, but they do not stabilize to the desired zero state. By mapping the desired states of x and y to the desired ϕ and θ states which are controllable, an additional proportional controller gain is used to drive all the system states to zero. However, this approach is inefficient because it takes too long for the angles and position to stabilize. The angles ϕ, θ, and ψ stabilize to zero after 35 seconds, and the positions x, y, and z stabilize to zero after 45 seconds. The ϕ, θ, x, and y states experience overshoots before stabilizing to zero. Side-by-side comparisons of the angle and position variables of the two approaches are shown in Figures 18 and 19.Figure 18
The anglesϕ,θ,andψ stabilize to zero after 35 s; the ϕ and θ states experience overshoots before stabilizing to zero.
(a)(b)Figure 19
The position variablesx, y, and z stabilize to zero after 45 s; the x and y states experience overshoots before stabilizing to zero.
(a)(b)


## 5.2. Trajectory Tracking

The results of the two control strategies implemented for trajectory tracking were previously discussed. Better performance is observed in the feedback linearized system with a PD control. The quadcopter is able to lock on to the desired trajectory and follow the path with accuracy in a lesser time period. Figures20–22 show the desired and actual trajectory plot after 15, 30, and 45 seconds, respectively, for both the PD control and the FBL with a PD control.Figure 20
The trajectory for PD (a) and FBL with PD control (b) after 15 seconds.
(a)(b)Figure 21
The trajectory for PD (a) and FBL with PD control (b) after 30 seconds.
(a)(b)Figure 22
The trajectory for PD (a) and FBL with PD control (b) after 45 seconds.
(a)(b)The quadcopter system simulated with the FBL and PD control is able to lock on to the desired trajectory after the first 15 seconds, while the system with only the PD control is unable to do that properly even after 30 seconds. This shows that the feedback linearization technique with the PD control is more effective in trajectory tracking than the regular PD control.


## 6. Conclusion

The mathematical modeling of the quadcopter system dynamics has been developed based on a few standard assumptions. This was achieved by adequately understanding the different coordinate systems involved, Euler angles, and the Newtonian equation to express the various extraneous forces acting on the quadcopter body. The state-space model of the system dynamics was obtained using Newton’s and Euler’s laws. The model was tested by simulating the quadcopter flight on MATLAB with already established parameters.Attitude stabilization and control of the quadcopter system have been achieved by using a PD controller. The simulation showed that the PD control algorithm stabilized the system states to the desired states. However, the PD controller could not stabilize thex and y positions to the desired state because the state variables are not observable. Another mapping approach was considered to drive the x and y positions to the desired zero state, but this was achieved after a more extended period of time.Two nonlinear control strategies were implemented for trajectory tracking of the quadcopter system. The simulation showed that the PD control was inferior to the FBL with PD control. The hybridization of FBI and PD approach presented accurate tracking of the desired trajectory in a lesser period of time as reflected in Figure22.


### 6.1. Recommendation to Improve the New Technique

The NDI technique used in feedback linearization of the system is implemented based on the assumption that all the states are available and measurable. Since this does not always seem to be the case, a further step in this control strategy can be the design of a suitable estimator or observer which can generate the possible outcomes of a full state from partial state variables. Another step forward in this research is the design of a controller to address the failure of one or more rotors. The quadcopter must be configured to apply the contingency algorithm once failure is detected based on inputs of the controller. Also, the proposed model and control techniques were evaluated using simulations only. Further studies can include the construction of the actual prototypes of a quadcopter to obtain more practical and accurate results.


## 6.1. Recommendation to Improve the New Technique

The NDI technique used in feedback linearization of the system is implemented based on the assumption that all the states are available and measurable. Since this does not always seem to be the case, a further step in this control strategy can be the design of a suitable estimator or observer which can generate the possible outcomes of a full state from partial state variables. Another step forward in this research is the design of a controller to address the failure of one or more rotors. The quadcopter must be configured to apply the contingency algorithm once failure is detected based on inputs of the controller. Also, the proposed model and control techniques were evaluated using simulations only. Further studies can include the construction of the actual prototypes of a quadcopter to obtain more practical and accurate results.



---

*Source: 2449901-2022-10-10.xml*