# Modeling and Design of an Aircraft-Mode Controller for a Fixed-Wing VTOL UAV

Authors: Zhao Deng; Liaoni Wu; Yancheng You
Journal: Mathematical Problems in Engineering
 (2021)

---

## Abstract

Vertical takeoff and landing (VTOL) is an essential feature of unmanned aerial vehicles (UAVs). On the one hand, VTOL can expand and enhance the applications of UAVs; yet, on the other hand, it makes the design of control systems for UAVs more complicated. The most challenging demand in designing the control system is to achieve satisfactory response sharpness of fixed-wing UAVs to control commands and ensure that the aircraft mode channels are effectively decoupled. In this work, a six-degree-of-freedom (6-DoF) model with forces and moments is established based on the aerodynamic analysis, which is carried out through computational fluid dynamics (CFD) numerical simulation. The improved proportional derivative (PD) controller based on the extended state observer (ESO) is proposed to design the inner-loop attitude control, which increases the anti-interference ability for internal and external uncertainty of the UAV system. The motion equations of the UAV are established and divided into independent components of longitudinal and lateral motion to design the outer loop control law under minor disturbance conditions. A total energy control system (TECS) for the longitudinal height channel is proposed, which separates speed control and track control. L1 nonlinear path tracking guidance algorithm is used for lateral trajectory tracking so as to improve curve tracking ability and wind resistance. Effectiveness of this approach is proved by actual flight experiment data. Finally, a controller based on angular velocity control is designed to prevent the attitude and head reference system (AHRS) from malfunctioning. Its effectiveness is verified by the response test of the control system.


---

## Body

## 1. Introduction

The VTOL functions keep the fixed-wing UAVs from relying on runways to takeoff or land and also significantly reduce the risk of accidents during takeoff and landing. Besides, the VTOL function supplies the fixed-wing UAVs, the flexibility to make it easier to perform tasks in mountainous areas, sea, and other scenarios, thereby broadening their application [1]. Furthermore, UAVs with VTOL can quickly implement one-key autonomous takeoff and landing functions, which improves the reliability and safety in takeoff and landing for potential nonprofessional and unskilled users [2, 3]. Hence, the UAVs with VTOL functions have always been the focus of research in the aviation field of Western Europe and the United States [4, 5]. Typical representatives are tail-mounted VD200, tiltrotor “Osprey” V-22, tilt-wing NASA “Greased Lightning,” and fixed-wing HQ-60 from American Latitude Engineering LLC.The design of controllers for VTOL UAVs is widely concerned in both the industry and the research field. In the recent years, multiple novel methods have been proposed to improve the performance of VTOL UAVs. For example, Özgür Dündar proposed to employ an aerodynamic design steps and sizing of both wing and control surfaces to improve static stability and endurance [6]. Wu et al. employed multiple sensors to design a UAV system for emergency response [7]. Oca et al. presented a longitudinal aircraft dynamics to model the takeoff and landing considering the rolling resistance forces during ground roll through a friction model [8]. Govdeli et al. developed a detailed aerodynamic modeling technique along with a fuzzy switching multimodel guidance and control strategy for a UAVs and successfully controlled the aircraft for a full flight envelope from hover to landing [9]. However, to date, the control method and algorithms have not been fully developed to fulfill the needs for various UAVs. More researches are still needed to improve the performance of VTOL UAVs in specific circumstances.Based on novel control techniques and algorithms, this paper takes SD-40 UVA as an example and presents the modeling and design of an aircraft mode controller for fixed-wing UAVs with VTOL functions. The SD-40 UVA has a fixed wing with eight horizontal rotors installed on the double-tail brace, as shown in Figure1.Figure 1
SD-40 UVA.In this work, a six-degree-of-freedom (6-DoF) model with forces and moments is established. A improved proportional derivative (PD) controller based on the extended state observer (ESO) and a total energy control system (TECS) for the longitudinal height channel are proposed. The motion equations of the UAV are established. L1 nonlinear path tracking guidance algorithm is used to improve curve tracking ability and wind resistance. Finally, a controller based on angular velocity control is designed to prevent the attitude and head reference system (AHRS) from malfunctioning. Effectiveness of these novel approaches is investigated by analyzing actual flight experiment data.


## 2. SD-40 Motion Mathematical Model

The nonlinear model obtained by the Newton–Euler formulation involves two main right-hand reference coordinate systems [10], as shown in Figure 2. For the geodetic coordinate frame, which is expressed as Ogxgygzg. A North-East-Down (NED) orthogonal coordinate frame is established with its origin at the takeoff point.Figure 2
Reference coordinate systems for mathematical modeling.Obxbybzb is the body-fixed frame (BFF) attached to the center of gravity (CoG) of SD-40, where Obxb points out the nose of the SD-40, Obyb points out the right wing, and Obzb points out the belly.Bbg represents the transformation matrix from the NED frame to the BFF.(1)Bbg=cθcψcθsψ−sθsθsϕcψ−cϕsψsθsϕsψ+cϕcψcθsϕsθcϕcψ+sϕsψsθcϕsψ−sϕcψcθcϕ,where ϕ,θ,andψ represent the Euler angles and sandc are the shorthand notation of the sine and cosine functions, respectively.


### 2.1. Linear Motion Equation

According to the momentum theorem, the kinetic equation of mass center can be obtained:(2)U˙V˙W˙=1mFxFyFz−PQR×UVW+Bbg00g,where UVWT is the velocity vector in the BFF, m is the mass of the SD-40, FxFyFzT the total force vectors expressed in the BFF, and PQRT is the vector of the attitude angular rates.(3)FxFyFz=Sba−D−FDY−L+T0−FT,Sba=cαcβsβsαcβ−cαsβcβ−sαsβ−sα0cα,where Sba is the transformation matrix from the airflow coordinate frame (AFF) to the BFF, α is the angle of attack, β is the sideslip angle, DYLT is the total force vectors expressed in AFF, T is the horizontal thrust of the engine, FT is the force in BFF produced by eight motors with rotors in multirotor mode, and FD is the drag force in AFF produced by eight rotors in aircraft mode.The components of the force vector and the drag force yields are expanded as(4)D=qs∗Cd,L=qs∗Cl,Y=qs∗Cy,FD=KdVt2,where Vt is the airspeed,Kd is the drag coefficient of rotors, qs is the dynamic pressure, and CdClCyT is the vector of the drag coefficient, the lift coefficient, and the side force coefficient of the fixed wing.Windu Windv WindwT is the wind disturbance. The three-axis velocity in the BFF can be updated as(5)UwVwWw=UVW−BbgWinduWindvWindw.Vt, α, and β are equally updated as(6)Vt=Uw2+Vw2+Ww2,α=atanWwUw,β=asinVwVm.According to the relationship between the NED frame and the BFF, the kinematics equation of the mass center is obtained as(7)X˙Y˙−Z˙=Bbg−1UwVwWw,where XYZT represents the position vector of the UAV in the NED.


### 2.2. Angular Motion Equation

According to the moment of momentum theorem, the dynamic equation of the rotation of the mass center can be derived with respect to the inertia, the total moment of the fixed wing, and the angular rates as follows:(8)P˙Q˙R˙=−I−1PQR×IPQR+I−1L¯+MCLM+MCMN+MCNwhere I is the inertia matrix of the rigid body and I−1 is the inverse matrix of I.(9)I=Ix0−Ixz0Iy0−Ixz0Iz.L¯MNT is the total moment vectors of fix-wing expressed in the BFF, which can be expressed as(10)L¯=qs∗b∗CrM=qs∗c∗CmN=qs∗b∗Cn,where b is the wingspan and c is the length of the average aerodynamic chord. CrCmCnT is a vector representing the roll moment coefficient, the pitch moment coefficient, and the yaw moment coefficient of the fixed wing, respectively. MCLMCMMCNT is the total moment vectors of the multirotor system expressed in the BFF as(11)MCL=−22∗l∗T1−T2−T3+T4+T5−T6−T7+T8,MCM=−22∗l∗−T1+T2+T3−T4−T5+T6+T7−T8,MCN=N1−N2+N3−N4−N5+N6−N7+N8,MNi=Kn∗Ni2i=1,2,3,4,5,6,7,8,where l is the multirotor wheelbase, Kn is the rotational damping moment coefficient, Ni is the revolutions per minute (RPM) of the rotor, Ti is the force in the BFF produced by eight motors with rotors in multirotor mode, and MNi is the counter-torque of each rotor [11].Based on the Euler Angle relation, the relationship between the attitude angle differential and the angular rate can be obtained as(12)ϕ˙θ˙ψ˙=1sϕtanθcϕtanθ0cϕ−sϕ0sϕcθcϕcθPQR.The 6-DoF nonlinear dynamics equations can be expressed by equations (2), (7), (8), and (12).


## 2.1. Linear Motion Equation

According to the momentum theorem, the kinetic equation of mass center can be obtained:(2)U˙V˙W˙=1mFxFyFz−PQR×UVW+Bbg00g,where UVWT is the velocity vector in the BFF, m is the mass of the SD-40, FxFyFzT the total force vectors expressed in the BFF, and PQRT is the vector of the attitude angular rates.(3)FxFyFz=Sba−D−FDY−L+T0−FT,Sba=cαcβsβsαcβ−cαsβcβ−sαsβ−sα0cα,where Sba is the transformation matrix from the airflow coordinate frame (AFF) to the BFF, α is the angle of attack, β is the sideslip angle, DYLT is the total force vectors expressed in AFF, T is the horizontal thrust of the engine, FT is the force in BFF produced by eight motors with rotors in multirotor mode, and FD is the drag force in AFF produced by eight rotors in aircraft mode.The components of the force vector and the drag force yields are expanded as(4)D=qs∗Cd,L=qs∗Cl,Y=qs∗Cy,FD=KdVt2,where Vt is the airspeed,Kd is the drag coefficient of rotors, qs is the dynamic pressure, and CdClCyT is the vector of the drag coefficient, the lift coefficient, and the side force coefficient of the fixed wing.Windu Windv WindwT is the wind disturbance. The three-axis velocity in the BFF can be updated as(5)UwVwWw=UVW−BbgWinduWindvWindw.Vt, α, and β are equally updated as(6)Vt=Uw2+Vw2+Ww2,α=atanWwUw,β=asinVwVm.According to the relationship between the NED frame and the BFF, the kinematics equation of the mass center is obtained as(7)X˙Y˙−Z˙=Bbg−1UwVwWw,where XYZT represents the position vector of the UAV in the NED.


## 2.2. Angular Motion Equation

According to the moment of momentum theorem, the dynamic equation of the rotation of the mass center can be derived with respect to the inertia, the total moment of the fixed wing, and the angular rates as follows:(8)P˙Q˙R˙=−I−1PQR×IPQR+I−1L¯+MCLM+MCMN+MCNwhere I is the inertia matrix of the rigid body and I−1 is the inverse matrix of I.(9)I=Ix0−Ixz0Iy0−Ixz0Iz.L¯MNT is the total moment vectors of fix-wing expressed in the BFF, which can be expressed as(10)L¯=qs∗b∗CrM=qs∗c∗CmN=qs∗b∗Cn,where b is the wingspan and c is the length of the average aerodynamic chord. CrCmCnT is a vector representing the roll moment coefficient, the pitch moment coefficient, and the yaw moment coefficient of the fixed wing, respectively. MCLMCMMCNT is the total moment vectors of the multirotor system expressed in the BFF as(11)MCL=−22∗l∗T1−T2−T3+T4+T5−T6−T7+T8,MCM=−22∗l∗−T1+T2+T3−T4−T5+T6+T7−T8,MCN=N1−N2+N3−N4−N5+N6−N7+N8,MNi=Kn∗Ni2i=1,2,3,4,5,6,7,8,where l is the multirotor wheelbase, Kn is the rotational damping moment coefficient, Ni is the revolutions per minute (RPM) of the rotor, Ti is the force in the BFF produced by eight motors with rotors in multirotor mode, and MNi is the counter-torque of each rotor [11].Based on the Euler Angle relation, the relationship between the attitude angle differential and the angular rate can be obtained as(12)ϕ˙θ˙ψ˙=1sϕtanθcϕtanθ0cϕ−sϕ0sϕcθcϕcθPQR.The 6-DoF nonlinear dynamics equations can be expressed by equations (2), (7), (8), and (12).


## 3. Flight Controller Design

The flight controller of SD-40 consists of an inner loop for attitude control and an outer loop for trajectory control. The attitude angle error is used as the input signal for the attitude control. In contrast, the external loop controller uses the trajectory error and its rate of change as input signals.


### 3.1. Attitude Control

The attitude control design of the UAV can be carried out into extended state observer, pitch channel control, and roll channel control.


#### 3.1.1. Extended State Observer

Equation12 shows that the roll channel, pitch channel, and yaw channel are interrelated, which poses a challenge in the design of a conventional controller. However, Active Disturbance Rejection Control (ADRC) can solve this coupled problem. First, the interaction between different channels inside the system, together with the external disturbance caused by the environment, is treated as the total disturbance of the channel. Then, ESO is applied to each angle channel independently to estimate the total disturbance in real time. Finally, a total disturbance is used to dynamically compensate and linearize the UAV control system [12, 13].Equation system (12) can be updated as(13)ϕ¨=f1ϕ,ϕ˙,θ,θ˙,ψ,ψ˙+ω1+b1L¯,θ¨=f2ϕ,ϕ˙,θ,θ˙,ψ,ψ˙+ω2+b2M,b1=L¯δa=qs∗BL∗ClδaIx,b2=Mδe=qs∗BA∗CmδeIy,where fiϕ,ϕ˙,θ,θ˙,ψ,ψ˙ is a modeled nonlinear dynamic system and ωi is the unmodeled dynamic system including external interference, modeling error, and other factors [14, 15]. b1 is the control input gain from the deflection angle of the aileron angle to roll angular acceleration and b2 is the control input gain from the deflection angle of the elevator angle to pitch angular acceleration.Clδa is the coefficient of the rolling moment, while Cmδe is the coefficient of pitching moment [16].The second-order system can be expanded into a third-order system as(14)x˙1=x2x˙2=x3+bux˙3=f˙x1,x2,ωty=x1.The linear extended state observer can be unfolded into the following equation:(15)e1=z1−yz˙1=z2−β1e1z˙2=z3−β2e1+buz˙3=−β3e1,where z1z2z3T is the output of the ESO, tracking the state variables of the original system x1x2x3T[17, 18]. All poles of the ESO characteristic equation can be located at −ω0 (on the left half-plane of the complex plane) as per the following equation:(16)β1=3ω0β2=3ω02β3=ω03,where ω0 is denoted as the bandwidth of the ESO.The control signal can be expressed as(17)u=u0−β⋅z3b,where u0 is the output of PD controller andβ is the compensation coefficient of total disturbance [19].


#### 3.1.2. Pitch Channel Control Loop

The pitch channel controller needs the signals from the angular rate sensor and attitude angle sensor for feedback, increasing the damping of the UAV system and improving the dynamic response of the open-loop system.The longitudinal control system structure of the SD-40 is shown in Figure3, which is composed of the pitch attitude control loop based on ESO and the height control loop based on the total energy control system (TECS).Figure 3
The longitudinal control structure of the SD-40.Pitch angle control based on ESO is an adaptive PD controller [20] with double loop feedback of the pitch angular rate Q and pitch angle θ. According to equations (14) and (15), x1 is θ and x2 is Q, z1 is the estimation of the θ signal, z2 is the estimation of the Q signal, and z3 is the estimation of the uncertain parts.The system block diagram is shown in Figure4, and the mathematical expression is shown in the following equation:where δe is the deflection angle of the elevator, θg is the pitch angle command, and Q is the pitch angular rate feedback value.Figure 4
Pitch angle controller block diagram of the SD-40.(18)δe=Keθθ−θg+KeQQ−βb2z3,b2 can be calculated with equation (13) as b2=qs∗BA∗Cmδe/Iy=−0.4∼−0.45.The bandwidthω0 of the pitch channel ESO can adjust the tracking speed of the observed state variable. According to reference [21], ω0≈5−10ω¯c , where ω¯c is the equivalent bandwidth of the transient profile, which is used with a settling time of 1 s, ω¯c=4, so ω0≈20−40.A higher bandwidth corresponds to a better command of the disturbance rejection and sensitivity to parameter variations. However, achievable bandwidth is limited by the hardware and software limitations, including sensor noises and the given sampling rate of the actual flight control hardware system. The comparison of responses at different valuesω0 is shown in Figure 5.Figure 5
Comparison of response at different values ofω0.Considering the simulation results and the actual limitations,ω0 is selected as 25. The system response versus the step input is shown in Figure 6. The pitch angle can quickly track instructions and can be well estimated by zl. Figure 7 is the observed value of total disturbance of pitch control loop. The disturbance can be suppressed in a short period.Figure 6
Pitch angle step response (ω0=25).Figure 7
Total disturbance observation.


#### 3.1.3. Roll Channel Control Loop

Similar to the pitch channel, the roll angle control is also developed based on the PD philosophy. The control block diagram of the roll channel is shown in Figure8.Figure 8
Roll angle controller block diagram of the SD-40.The control law is as follows:(19)δa=Kaϕϕ−ϕg+KaPP−βb1z3,where δa is the deflection angle of the aileron, ϕg is the roll angle command, and P is the roll angular rate feedback value, improving the short period damping.b1 can be calculated with equation (13) [12]: b1=qs∗BL∗Clδa/Ix=−0.4∼−0.49.The comparison of response at different valuesω0 is shown in Figure 9. As done previously for the pitch channel, the bandwidth of the roll channel ESO ω0 is selected as equal to 25.Figure 9
Comparison of response at different values ofω0.The roll angle step response is shown in Figure10, which has good tracking and observation effects. Figure 11 is the observed value of the total disturbance of the Roll control loop. The disturbance can be suppressed in a short period and the system can quickly reach a stable state.Figure 10
Roll angle step response (ω0=25).Figure 11
Total disturbance observation.The pitch angle and the roll angle controller based on ESO for the inner loop can respond to and track the command effectively and achieve high precision positioning of the UAV.


### 3.2. Trajectory Control

#### 3.2.1. Longitudinal Height Control

The control of the longitudinal height of the SD-40 is performed through an outer loop based on the TECS. The throttle controls the change of total energy, and the deflection angle of the elevator distributes the kinetic energy and the gravitational potential energy. As such, the speed control and the height control are decoupled [22–24].The error between the kinetic energy and the specified kinetic energy at a certain moment can be expressed as(20)Kerr=Kref−K=12mVtc2−12mVt2,where K is the kinetic energy of the particle, Vt is the true airspeed, and Vtc is the specified airspeed.The error between the gravitational potential energy and the specified gravitational potential energy at a certain moment can be expressed as(21)Uerr=mghc−h.The error of the total energyE and the allocation L of the total energy error can be expressed as per the following equation:(22)E=Uerr+Kerr,L=Uerr−Kerr.The allocation is updated as(23)L=2−kUerr−kKerr.By differentiating both sides of the total energy equation and assuming that the trajectory angleγ is small, the rate of change of the total energy is approximately expressed as(24)E˙=E˙TmgVt=V˙tg+h˙Vt=V˙tg+sinγ≈V˙tg+γ.Meanwhile, the longitudinal dynamics equation of the UAV is expressed as per the following equation:(25)T−D=mV˙t+mgsinγ≈mgV˙tg+γ,where T and D are, respectively, the thrust and drag force of UAV during cruise state. In this balanced state, the thrust increment is expressed as(26)ΔT=mgV˙tg+γ=mgΔE˙V.In summary, the change of total energy can be controlled by the change in thrust [25].The deflection angle of the elevator changes the track angle, which leads to the mutual conversion between the potential energy and the kinetic energy [26, 27]. Assuming that there is no significant energy loss during the conversion process, the change rate of the energy distribution can be defined as(27)L˙=γ−V˙tg.Total energy control block diagram is shown in Figures12 and 13.Figure 12
Total energy change control.Figure 13
Total energy distribution control.Hg is the height command, Vg is the speed command. Esp is the specified energy, Lsp is the specified energy distribution, and E˙sp and L˙sp are the corresponding rates of change. E is the total energy, L is the energy distribution, and E˙ and L˙ are the corresponding rates of change, respectively. KE˙⟶ΔT is the gain from energy change rate to throttle andKL˙⟶Δθ is the gain from energy distribution change rate to pitch angle.The complete TECS control law, as well as the total energy change control channel, can be expressed as(28)E˙c=kpE×E+dpEE˙+IpE∫Edt,Tg=E˙c×KE˙⟶ΔT,KE˙⟶ΔT=δTmax−δTminE˙max−E˙min,where E˙max and E˙min are the max and min rate of change of the total energy in climbing mode and descending mode, respectively. δTmax and δTmin are the max and min opening of the throttle, respectively.The total energy distribution control channel could then be expressed as(29)L˙c=keL×L+deLL˙+IeL∫Ldt.ControllingL˙c with the elevator requires a pitch rate/pitch attitude feedback inner-loop control law, where θg will be transformed into elevator command to stabilize the UAVs short period mode. Thus, L˙c develops θg, serves as the input of the pitch attitude control loop, which acts indirectly on the elevator [28–30].(30)L˙gVt=gh˙−VtV˙tgVt≈γ−V˙tg,θg=L˙cKL˙⟶Δθ=1gVt×L˙c,where gVt is the conversion coefficient from the climb angle to energy distribution change rate, which adapts to a range of speed changes.(1) Simulation Data. Case 1: Given the initial state of UAV, height is 100 m and speed is 30 m/s. Whent = 1 s, change the height command to 105 m, then the height step response is shown in Figure 14.From the graph in Figure15, the height can track the command well with some overshoot. Meanwhile, the speed is basically unchanged, and the fluctuation is controlled within ±0.5 m/s.As the height increases, the kinetic energy is converted into potential energy, which requires a larger throttle energy value. Even though the value of the elevator energy is negative, the UAV performs an upward movement. When the new balance is reached, the value comes back to zero.Case 2: Given the initial state of UAV, the height is set equal to 100 m and the speed to 30 m/s. When timet = 1 s, change the speed command to 32 m/s, then speed step response is shown in Figure 16.As shown in Figure17, the speed can track the command well, with some likely overshoot. Meanwhile, the height is unchanged, and the fluctuation is controlled within a distance of ±0.3 m.As the speed increases, the kinetic energy increases, requiring a larger throttle energy value. The elevator's value is positive to accelerate the UAV, and when the new balance is reached, the value keeps at 2°, as shown in Figure15.Figure 14
Height step response for a given speed.Figure 15
Elevator and throttle command for height step response simulation.Figure 16
Speed step response for a given height.Figure 17
Elevator and throttle command for speed step response simulation.(2) Flight Data. The UAV's initial height is 1470 m and speed is 30 m/s. When t = 1 s, the change in height command and the speed command are Hg=1170m,Vg=32m/s, then step response is shown in Figure 18.Figure 18
Height and speed response of step input test.As shown in Figures18 and 19, the altitude drops slowly with the command, and the airspeed is kept at 35 ± 1 m. Since the UAV flyes at a constant speed, the throttle remains unchanged to keep the kinetic energy intact. The elevator command is essentially negative, raising the UAV's head and increasing the drag as the gravitational potential energy slowly converts to internal power to work done against friction.Figure 19
Elevator and throttle command for height and speed step response test.


#### 3.2.2. Lateral Trajectory Control

The lateral trajectory consists of the roll and yaw motions, which have been considered in the controller design. L1 guidance is well known for its simplicity in tracking circular and linear motions typical for fixed-wing vehicles’ flight plans. However, the law has a specific ADRC effect in curve tracking, which is introduced for circular trajectory tracking, as shown in Figure20. Under this section, the controller design is presented along with simulated results and the profile of the flight data.Figure 20
Principle diagram of circular trajectory tracking.When the UAV moves laterally, the roll angle is not zero, resulting in lift tilt and lateral acceleration as expressed by the following equation:(31)ascmd=V2R=V2L1/2sinη=2V2L1sinη,(32)sinη=sinη1+η2+η3≈η1cosη3+η2cosη3+sinη3,(33)ascmd=2V2c2L12d+2VcL1d˙+V2R.where ascmd is the acceleration command, L1 is the distance between the reference point and the UAV, which is a fixed value, and η is the angle from the ground speed to the line L1. Meanwhile, the velocity direction change due to the acceleration can be expressed as(34)Δψ=asVΔt,(35)ascmd≈Vψ˙=Vθ˙−η˙2=V2R−d¨.Based on the equations (33) and (35), we can get(36)d¨+2ζωnd˙+ωn2d≈0,whereζ=2/2,ωn=2Vc/L1,andc=1−L1/2R2.The undamped natural frequency is determined byV and L1:(37)L1=VTc2π.The period and damping ratio are taken as the control parameters of the control law andL1 as a variable proportional to the flight speed [31].The lateral acceleration is provided by the lateral component of the lift force during the rolling of the UAV as(38)ascmd=g⋅tanϕ.The roll angle command can be expressed as(39)ϕcmd=tan−1ascmdg.L1 control structure diagram shown in Figure 21 is designed based on the following equation:(40)ascmd=Kxd+Kvd˙+V2R,where Kx=ωn2=4π2/T2,Kv=2ζωn=4πζ/T, and R is the radius of the tracked circular arc.Figure 21
L1 block diagram. Here,η2 is the yaw angle from the target heading to the speed heading and d is the lateral deviation distance, that is, the distance between the UAV real-time position and the center of the tracked circular arc minus the radius of the tracked circle.(1) Tracking Simulation. A nonlinear model in Matlab/Simulink environment has been adopted to carry out the circular trajectory tracking simulation. Given the L1 guidance law parameters period damping ζ=0.707, the simulation data at different periods T, as shown in Figures 22 and 23.Figure 22
Yaw angle error at different periods.Figure 23
Lateral deviation distance at different periods.It can be seen from Figures22 and 23, with the decrease of the period T, the response time of the lateral deviation distance and yaw angle error, that the error rapidly narrows, and the steady-state error of the lateral deviation distance decreases accordingly. In contrast, the yaw angle error fluctuates significantly. Reducing the period T will make the system response faster and the control effect more practical. A smaller value of the period T will lead to more aggressive navigation or sharper corners, while a more considerable value will lead to gentler navigation. Meanwhile, considering the safety and control rapidity in the UAV operation, the value of T is selected here as equal to 20 s.Settingζ=0.707 and T=20s simulation initial state Vt=30m/s, constant wind disturbances in the north direction and east direction are added U=5m/sandV=5m/s. Track a circular trajectory with a radius of 600 m (R=600m).The velocity profile with wind disturbances is shown in Figure24. The tracking effect is shown in Figures 25 and 26.Figure 24
Velocity profile with wind disturbances.Figure 25
Circular trajectory tracking under wind disturbance.Figure 26
Angle error and deviation distance under wind disturbance.As shown in Figure24, the crosswind with 5 m/s has been simulated.Simulation results show that the lateral deviation distance can be controlled at ±3 m precision, while the yaw angle error is ±0.5°, which offers a good effect of the circular trajectory tracking.Figures27 and 28 show the simulation of circular track tracking in a windless environment. Setting ζ=0.707 and T=20s, initial state Vt=30m/s, track a circular trajectory with a radius of 600 m. The lateral deviation distance is controlled at −1 m, while the yaw angle error is 0.4°, which shows a good effect of circular trajectory tracking without wind disturbance.Figure 27
Circular trajectory tracking.Figure 28
Angle error and deviation distance.(2) Flight Data Profile. The UAV tracks a circle with a radius of R = 450 m, and the center of the circle is located at longitude: 100.23°, latitude: 41.01°, the tracking effect is shown in Figure 29.Figure 29
Circular trajectory tracking for the flight test.Under the constant wind disturbance of 7 m/s (direction 184), airspeed values vary from 30 m/s to 37 m/s, as shown in Figure30.Figure 30
Wind speed and airspeed for the flight test.The value of the lateral deviation distance is between 0 and −4 m (shown in Figure31), which means the UAV keeps on the left of the desired circular path. The yaw angle error can be controlled at ±3° (shown in Figure 32), larger than the value obtained by simulation. It is affected by crosswind to some extent, but the overall tracking effect is relatively good.Figure 31
Lateral deviation distance for the flight test.Figure 32
Yaw angle error for the flight test.For the SD-40,L1 guidance law enhances the circular trajectory tracking control ability and the ability to resist wind, using the insensitive period and damping parameters. This law can calculate different expected accelerations according to different speeds V and offers good adaptability to different usual flight speeds and interference of ambient wind in actual flight. Furthermore, L1 guidance law can track irregular curves and be extended to more advanced trajectory algorithms, including obstacle avoidance flight and formation flight.


### 3.3. Emergency Control

Due to the limitations of cost, payload weight, and payload size, most small civil UAVs adopt the Attitude and Heading Reference System (AHRS) based on a microelectromechanical system (MEMS). AHRS cannot provide accurate and stable attitude data under the following conditions:(a)
When the UAV is under large continuous overload(b)
When the algorithm of attitude solving unit has some problem(c)
When the calculated data of attitude solving unit are divergent(d)
When the magnetometer is interfered with and provides false heading informationIn all these four cases, a triaxial attitude angle information and geomagnetic course information are invalid. The control method is the same in all the four cases because it is designed to deal with this information invalidation. In this study, we intend to propose a general solution for this information invalidation. The traditional attitude angle control technology is no longer applicable, and the control law based on the angular rate is needed [32]. Sound signals for flight controllers are UAV’s position information, track angle, and ground speed provided by GPS; airspeed, atmospheric altitude, and rate of change information supplied by atmospheric pressure sensor; triaxial acceleration and triaxial angular rate provided by AHRS. The emergency controller design looked at the longitudinal and lateral control of the UAV.


#### 3.3.1. Longitudinal Control Law

TECS can decouple the height control loop and the speed control loop of the longitudinal motion, while the latter has no attitude control loop but directly controls the engine's thrust. Therefore, the speed control loop of TECS law can continue to be used. On the other hand, the height control law needs to be updated.The pitch control loop is used to stabilize the pitch attitude and serves as the inner loop of the height controller. Since there is no pitch angle feedback information, height control based on angular pitch rate is proposed. The reduced-order ESO model is used for the inner-loop angular rate control. The longitudinal controller structure is shown in Figure33:(41)δe=KeIq∫Q−Qgdt+KeQQ,Qg=KQH˙H˙−KeHH−Hg,where Qg is the given pitch angular rate and H˙ is the vertical speed of the UAV.Figure 33
Longitudinal controller structure of the SD-40.The outer loop height control output serves as the input to the inner-loop attitude angular rate control loop, which ultimately acts on the elevator.The UAV's initial state of the simulation: height is 100 m and speed is 30 m/s. Whent = 1 s, the height command is changed toHg=105m.The vertical speed is obtained according to a specific proportional relation of the error between height and its command. Whent = 1 s, there is a sudden change in the vertical speed command due to the considerable height error, but the vertical speed can track the command well and respond quickly (Figure 34).Figure 34
Vertical speed response to height changes.As shown in Figure35, the value of the elevator energy is negative to enable the UAV to move upward, and when the new balance is reached, the energy value comes back to zero.Figure 35
Elevator deflection angle response to height changes.The height step command is tracked without overshoot, and the response is quick (see Figure36). In addition, by putting an integral part into the pitch angular rate control loop, the influence of external interference on the system can be easily neglected.Figure 36
Height step response of the SD-40.


#### 3.3.2. Lateral Control Law

The structure of lateral roll angular rate control is similar to that of longitudinal pitch angular rate control. The turning control of UAV is realized by controlling the yaw angular rate.The outer track control loop outputs the roll angular rate command, which will be used as input to the inner loop of the roll to control the aileron deflection angle (see Figure37 and equations (42) and (43)):(42)δa=KaIp∫P−Pgdt+KaPP,(43)Pg=KaRR−KaPsiPsi−Psig,where Pg is the roll angular rate command, R is the feedback of yaw angular rate, and Psi is the feedback of the track azimuth, which can be provided by GPS.Figure 37
Lateral controller structure of the SD-40.As shown in Figure38, the track azimuth tracked the command well without overshoot.Figure 38
Track azimuth step response to command tracking.The yaw angular rate command needs to be optimized into a smooth transient profile (Figure39), as the desired trajectory to allow the output to follow more closely.Figure 39
Yaw angular rate response to command tracking.It can be seen from Figures40 and 41 that the proportion term plays a significant role in the roll angular rate control loop. In addition, simulation data show that when AHRS fails, the emergency control law without Euler Angle feedback can track the control instructions well and enable the UAV to make a crash landing or continue to perform the mission, improving flight safety.Figure 40
Roll angular rate response to command tracking.Figure 41
Aileron deflection angle response to command tracking.


## 3.1. Attitude Control

The attitude control design of the UAV can be carried out into extended state observer, pitch channel control, and roll channel control.


### 3.1.1. Extended State Observer

Equation12 shows that the roll channel, pitch channel, and yaw channel are interrelated, which poses a challenge in the design of a conventional controller. However, Active Disturbance Rejection Control (ADRC) can solve this coupled problem. First, the interaction between different channels inside the system, together with the external disturbance caused by the environment, is treated as the total disturbance of the channel. Then, ESO is applied to each angle channel independently to estimate the total disturbance in real time. Finally, a total disturbance is used to dynamically compensate and linearize the UAV control system [12, 13].Equation system (12) can be updated as(13)ϕ¨=f1ϕ,ϕ˙,θ,θ˙,ψ,ψ˙+ω1+b1L¯,θ¨=f2ϕ,ϕ˙,θ,θ˙,ψ,ψ˙+ω2+b2M,b1=L¯δa=qs∗BL∗ClδaIx,b2=Mδe=qs∗BA∗CmδeIy,where fiϕ,ϕ˙,θ,θ˙,ψ,ψ˙ is a modeled nonlinear dynamic system and ωi is the unmodeled dynamic system including external interference, modeling error, and other factors [14, 15]. b1 is the control input gain from the deflection angle of the aileron angle to roll angular acceleration and b2 is the control input gain from the deflection angle of the elevator angle to pitch angular acceleration.Clδa is the coefficient of the rolling moment, while Cmδe is the coefficient of pitching moment [16].The second-order system can be expanded into a third-order system as(14)x˙1=x2x˙2=x3+bux˙3=f˙x1,x2,ωty=x1.The linear extended state observer can be unfolded into the following equation:(15)e1=z1−yz˙1=z2−β1e1z˙2=z3−β2e1+buz˙3=−β3e1,where z1z2z3T is the output of the ESO, tracking the state variables of the original system x1x2x3T[17, 18]. All poles of the ESO characteristic equation can be located at −ω0 (on the left half-plane of the complex plane) as per the following equation:(16)β1=3ω0β2=3ω02β3=ω03,where ω0 is denoted as the bandwidth of the ESO.The control signal can be expressed as(17)u=u0−β⋅z3b,where u0 is the output of PD controller andβ is the compensation coefficient of total disturbance [19].


### 3.1.2. Pitch Channel Control Loop

The pitch channel controller needs the signals from the angular rate sensor and attitude angle sensor for feedback, increasing the damping of the UAV system and improving the dynamic response of the open-loop system.The longitudinal control system structure of the SD-40 is shown in Figure3, which is composed of the pitch attitude control loop based on ESO and the height control loop based on the total energy control system (TECS).Figure 3
The longitudinal control structure of the SD-40.Pitch angle control based on ESO is an adaptive PD controller [20] with double loop feedback of the pitch angular rate Q and pitch angle θ. According to equations (14) and (15), x1 is θ and x2 is Q, z1 is the estimation of the θ signal, z2 is the estimation of the Q signal, and z3 is the estimation of the uncertain parts.The system block diagram is shown in Figure4, and the mathematical expression is shown in the following equation:where δe is the deflection angle of the elevator, θg is the pitch angle command, and Q is the pitch angular rate feedback value.Figure 4
Pitch angle controller block diagram of the SD-40.(18)δe=Keθθ−θg+KeQQ−βb2z3,b2 can be calculated with equation (13) as b2=qs∗BA∗Cmδe/Iy=−0.4∼−0.45.The bandwidthω0 of the pitch channel ESO can adjust the tracking speed of the observed state variable. According to reference [21], ω0≈5−10ω¯c , where ω¯c is the equivalent bandwidth of the transient profile, which is used with a settling time of 1 s, ω¯c=4, so ω0≈20−40.A higher bandwidth corresponds to a better command of the disturbance rejection and sensitivity to parameter variations. However, achievable bandwidth is limited by the hardware and software limitations, including sensor noises and the given sampling rate of the actual flight control hardware system. The comparison of responses at different valuesω0 is shown in Figure 5.Figure 5
Comparison of response at different values ofω0.Considering the simulation results and the actual limitations,ω0 is selected as 25. The system response versus the step input is shown in Figure 6. The pitch angle can quickly track instructions and can be well estimated by zl. Figure 7 is the observed value of total disturbance of pitch control loop. The disturbance can be suppressed in a short period.Figure 6
Pitch angle step response (ω0=25).Figure 7
Total disturbance observation.


### 3.1.3. Roll Channel Control Loop

Similar to the pitch channel, the roll angle control is also developed based on the PD philosophy. The control block diagram of the roll channel is shown in Figure8.Figure 8
Roll angle controller block diagram of the SD-40.The control law is as follows:(19)δa=Kaϕϕ−ϕg+KaPP−βb1z3,where δa is the deflection angle of the aileron, ϕg is the roll angle command, and P is the roll angular rate feedback value, improving the short period damping.b1 can be calculated with equation (13) [12]: b1=qs∗BL∗Clδa/Ix=−0.4∼−0.49.The comparison of response at different valuesω0 is shown in Figure 9. As done previously for the pitch channel, the bandwidth of the roll channel ESO ω0 is selected as equal to 25.Figure 9
Comparison of response at different values ofω0.The roll angle step response is shown in Figure10, which has good tracking and observation effects. Figure 11 is the observed value of the total disturbance of the Roll control loop. The disturbance can be suppressed in a short period and the system can quickly reach a stable state.Figure 10
Roll angle step response (ω0=25).Figure 11
Total disturbance observation.The pitch angle and the roll angle controller based on ESO for the inner loop can respond to and track the command effectively and achieve high precision positioning of the UAV.


## 3.1.1. Extended State Observer

Equation12 shows that the roll channel, pitch channel, and yaw channel are interrelated, which poses a challenge in the design of a conventional controller. However, Active Disturbance Rejection Control (ADRC) can solve this coupled problem. First, the interaction between different channels inside the system, together with the external disturbance caused by the environment, is treated as the total disturbance of the channel. Then, ESO is applied to each angle channel independently to estimate the total disturbance in real time. Finally, a total disturbance is used to dynamically compensate and linearize the UAV control system [12, 13].Equation system (12) can be updated as(13)ϕ¨=f1ϕ,ϕ˙,θ,θ˙,ψ,ψ˙+ω1+b1L¯,θ¨=f2ϕ,ϕ˙,θ,θ˙,ψ,ψ˙+ω2+b2M,b1=L¯δa=qs∗BL∗ClδaIx,b2=Mδe=qs∗BA∗CmδeIy,where fiϕ,ϕ˙,θ,θ˙,ψ,ψ˙ is a modeled nonlinear dynamic system and ωi is the unmodeled dynamic system including external interference, modeling error, and other factors [14, 15]. b1 is the control input gain from the deflection angle of the aileron angle to roll angular acceleration and b2 is the control input gain from the deflection angle of the elevator angle to pitch angular acceleration.Clδa is the coefficient of the rolling moment, while Cmδe is the coefficient of pitching moment [16].The second-order system can be expanded into a third-order system as(14)x˙1=x2x˙2=x3+bux˙3=f˙x1,x2,ωty=x1.The linear extended state observer can be unfolded into the following equation:(15)e1=z1−yz˙1=z2−β1e1z˙2=z3−β2e1+buz˙3=−β3e1,where z1z2z3T is the output of the ESO, tracking the state variables of the original system x1x2x3T[17, 18]. All poles of the ESO characteristic equation can be located at −ω0 (on the left half-plane of the complex plane) as per the following equation:(16)β1=3ω0β2=3ω02β3=ω03,where ω0 is denoted as the bandwidth of the ESO.The control signal can be expressed as(17)u=u0−β⋅z3b,where u0 is the output of PD controller andβ is the compensation coefficient of total disturbance [19].


## 3.1.2. Pitch Channel Control Loop

The pitch channel controller needs the signals from the angular rate sensor and attitude angle sensor for feedback, increasing the damping of the UAV system and improving the dynamic response of the open-loop system.The longitudinal control system structure of the SD-40 is shown in Figure3, which is composed of the pitch attitude control loop based on ESO and the height control loop based on the total energy control system (TECS).Figure 3
The longitudinal control structure of the SD-40.Pitch angle control based on ESO is an adaptive PD controller [20] with double loop feedback of the pitch angular rate Q and pitch angle θ. According to equations (14) and (15), x1 is θ and x2 is Q, z1 is the estimation of the θ signal, z2 is the estimation of the Q signal, and z3 is the estimation of the uncertain parts.The system block diagram is shown in Figure4, and the mathematical expression is shown in the following equation:where δe is the deflection angle of the elevator, θg is the pitch angle command, and Q is the pitch angular rate feedback value.Figure 4
Pitch angle controller block diagram of the SD-40.(18)δe=Keθθ−θg+KeQQ−βb2z3,b2 can be calculated with equation (13) as b2=qs∗BA∗Cmδe/Iy=−0.4∼−0.45.The bandwidthω0 of the pitch channel ESO can adjust the tracking speed of the observed state variable. According to reference [21], ω0≈5−10ω¯c , where ω¯c is the equivalent bandwidth of the transient profile, which is used with a settling time of 1 s, ω¯c=4, so ω0≈20−40.A higher bandwidth corresponds to a better command of the disturbance rejection and sensitivity to parameter variations. However, achievable bandwidth is limited by the hardware and software limitations, including sensor noises and the given sampling rate of the actual flight control hardware system. The comparison of responses at different valuesω0 is shown in Figure 5.Figure 5
Comparison of response at different values ofω0.Considering the simulation results and the actual limitations,ω0 is selected as 25. The system response versus the step input is shown in Figure 6. The pitch angle can quickly track instructions and can be well estimated by zl. Figure 7 is the observed value of total disturbance of pitch control loop. The disturbance can be suppressed in a short period.Figure 6
Pitch angle step response (ω0=25).Figure 7
Total disturbance observation.


## 3.1.3. Roll Channel Control Loop

Similar to the pitch channel, the roll angle control is also developed based on the PD philosophy. The control block diagram of the roll channel is shown in Figure8.Figure 8
Roll angle controller block diagram of the SD-40.The control law is as follows:(19)δa=Kaϕϕ−ϕg+KaPP−βb1z3,where δa is the deflection angle of the aileron, ϕg is the roll angle command, and P is the roll angular rate feedback value, improving the short period damping.b1 can be calculated with equation (13) [12]: b1=qs∗BL∗Clδa/Ix=−0.4∼−0.49.The comparison of response at different valuesω0 is shown in Figure 9. As done previously for the pitch channel, the bandwidth of the roll channel ESO ω0 is selected as equal to 25.Figure 9
Comparison of response at different values ofω0.The roll angle step response is shown in Figure10, which has good tracking and observation effects. Figure 11 is the observed value of the total disturbance of the Roll control loop. The disturbance can be suppressed in a short period and the system can quickly reach a stable state.Figure 10
Roll angle step response (ω0=25).Figure 11
Total disturbance observation.The pitch angle and the roll angle controller based on ESO for the inner loop can respond to and track the command effectively and achieve high precision positioning of the UAV.


## 3.2. Trajectory Control

### 3.2.1. Longitudinal Height Control

The control of the longitudinal height of the SD-40 is performed through an outer loop based on the TECS. The throttle controls the change of total energy, and the deflection angle of the elevator distributes the kinetic energy and the gravitational potential energy. As such, the speed control and the height control are decoupled [22–24].The error between the kinetic energy and the specified kinetic energy at a certain moment can be expressed as(20)Kerr=Kref−K=12mVtc2−12mVt2,where K is the kinetic energy of the particle, Vt is the true airspeed, and Vtc is the specified airspeed.The error between the gravitational potential energy and the specified gravitational potential energy at a certain moment can be expressed as(21)Uerr=mghc−h.The error of the total energyE and the allocation L of the total energy error can be expressed as per the following equation:(22)E=Uerr+Kerr,L=Uerr−Kerr.The allocation is updated as(23)L=2−kUerr−kKerr.By differentiating both sides of the total energy equation and assuming that the trajectory angleγ is small, the rate of change of the total energy is approximately expressed as(24)E˙=E˙TmgVt=V˙tg+h˙Vt=V˙tg+sinγ≈V˙tg+γ.Meanwhile, the longitudinal dynamics equation of the UAV is expressed as per the following equation:(25)T−D=mV˙t+mgsinγ≈mgV˙tg+γ,where T and D are, respectively, the thrust and drag force of UAV during cruise state. In this balanced state, the thrust increment is expressed as(26)ΔT=mgV˙tg+γ=mgΔE˙V.In summary, the change of total energy can be controlled by the change in thrust [25].The deflection angle of the elevator changes the track angle, which leads to the mutual conversion between the potential energy and the kinetic energy [26, 27]. Assuming that there is no significant energy loss during the conversion process, the change rate of the energy distribution can be defined as(27)L˙=γ−V˙tg.Total energy control block diagram is shown in Figures12 and 13.Figure 12
Total energy change control.Figure 13
Total energy distribution control.Hg is the height command, Vg is the speed command. Esp is the specified energy, Lsp is the specified energy distribution, and E˙sp and L˙sp are the corresponding rates of change. E is the total energy, L is the energy distribution, and E˙ and L˙ are the corresponding rates of change, respectively. KE˙⟶ΔT is the gain from energy change rate to throttle andKL˙⟶Δθ is the gain from energy distribution change rate to pitch angle.The complete TECS control law, as well as the total energy change control channel, can be expressed as(28)E˙c=kpE×E+dpEE˙+IpE∫Edt,Tg=E˙c×KE˙⟶ΔT,KE˙⟶ΔT=δTmax−δTminE˙max−E˙min,where E˙max and E˙min are the max and min rate of change of the total energy in climbing mode and descending mode, respectively. δTmax and δTmin are the max and min opening of the throttle, respectively.The total energy distribution control channel could then be expressed as(29)L˙c=keL×L+deLL˙+IeL∫Ldt.ControllingL˙c with the elevator requires a pitch rate/pitch attitude feedback inner-loop control law, where θg will be transformed into elevator command to stabilize the UAVs short period mode. Thus, L˙c develops θg, serves as the input of the pitch attitude control loop, which acts indirectly on the elevator [28–30].(30)L˙gVt=gh˙−VtV˙tgVt≈γ−V˙tg,θg=L˙cKL˙⟶Δθ=1gVt×L˙c,where gVt is the conversion coefficient from the climb angle to energy distribution change rate, which adapts to a range of speed changes.(1) Simulation Data. Case 1: Given the initial state of UAV, height is 100 m and speed is 30 m/s. Whent = 1 s, change the height command to 105 m, then the height step response is shown in Figure 14.From the graph in Figure15, the height can track the command well with some overshoot. Meanwhile, the speed is basically unchanged, and the fluctuation is controlled within ±0.5 m/s.As the height increases, the kinetic energy is converted into potential energy, which requires a larger throttle energy value. Even though the value of the elevator energy is negative, the UAV performs an upward movement. When the new balance is reached, the value comes back to zero.Case 2: Given the initial state of UAV, the height is set equal to 100 m and the speed to 30 m/s. When timet = 1 s, change the speed command to 32 m/s, then speed step response is shown in Figure 16.As shown in Figure17, the speed can track the command well, with some likely overshoot. Meanwhile, the height is unchanged, and the fluctuation is controlled within a distance of ±0.3 m.As the speed increases, the kinetic energy increases, requiring a larger throttle energy value. The elevator's value is positive to accelerate the UAV, and when the new balance is reached, the value keeps at 2°, as shown in Figure15.Figure 14
Height step response for a given speed.Figure 15
Elevator and throttle command for height step response simulation.Figure 16
Speed step response for a given height.Figure 17
Elevator and throttle command for speed step response simulation.(2) Flight Data. The UAV's initial height is 1470 m and speed is 30 m/s. When t = 1 s, the change in height command and the speed command are Hg=1170m,Vg=32m/s, then step response is shown in Figure 18.Figure 18
Height and speed response of step input test.As shown in Figures18 and 19, the altitude drops slowly with the command, and the airspeed is kept at 35 ± 1 m. Since the UAV flyes at a constant speed, the throttle remains unchanged to keep the kinetic energy intact. The elevator command is essentially negative, raising the UAV's head and increasing the drag as the gravitational potential energy slowly converts to internal power to work done against friction.Figure 19
Elevator and throttle command for height and speed step response test.


### 3.2.2. Lateral Trajectory Control

The lateral trajectory consists of the roll and yaw motions, which have been considered in the controller design. L1 guidance is well known for its simplicity in tracking circular and linear motions typical for fixed-wing vehicles’ flight plans. However, the law has a specific ADRC effect in curve tracking, which is introduced for circular trajectory tracking, as shown in Figure20. Under this section, the controller design is presented along with simulated results and the profile of the flight data.Figure 20
Principle diagram of circular trajectory tracking.When the UAV moves laterally, the roll angle is not zero, resulting in lift tilt and lateral acceleration as expressed by the following equation:(31)ascmd=V2R=V2L1/2sinη=2V2L1sinη,(32)sinη=sinη1+η2+η3≈η1cosη3+η2cosη3+sinη3,(33)ascmd=2V2c2L12d+2VcL1d˙+V2R.where ascmd is the acceleration command, L1 is the distance between the reference point and the UAV, which is a fixed value, and η is the angle from the ground speed to the line L1. Meanwhile, the velocity direction change due to the acceleration can be expressed as(34)Δψ=asVΔt,(35)ascmd≈Vψ˙=Vθ˙−η˙2=V2R−d¨.Based on the equations (33) and (35), we can get(36)d¨+2ζωnd˙+ωn2d≈0,whereζ=2/2,ωn=2Vc/L1,andc=1−L1/2R2.The undamped natural frequency is determined byV and L1:(37)L1=VTc2π.The period and damping ratio are taken as the control parameters of the control law andL1 as a variable proportional to the flight speed [31].The lateral acceleration is provided by the lateral component of the lift force during the rolling of the UAV as(38)ascmd=g⋅tanϕ.The roll angle command can be expressed as(39)ϕcmd=tan−1ascmdg.L1 control structure diagram shown in Figure 21 is designed based on the following equation:(40)ascmd=Kxd+Kvd˙+V2R,where Kx=ωn2=4π2/T2,Kv=2ζωn=4πζ/T, and R is the radius of the tracked circular arc.Figure 21
L1 block diagram. Here,η2 is the yaw angle from the target heading to the speed heading and d is the lateral deviation distance, that is, the distance between the UAV real-time position and the center of the tracked circular arc minus the radius of the tracked circle.(1) Tracking Simulation. A nonlinear model in Matlab/Simulink environment has been adopted to carry out the circular trajectory tracking simulation. Given the L1 guidance law parameters period damping ζ=0.707, the simulation data at different periods T, as shown in Figures 22 and 23.Figure 22
Yaw angle error at different periods.Figure 23
Lateral deviation distance at different periods.It can be seen from Figures22 and 23, with the decrease of the period T, the response time of the lateral deviation distance and yaw angle error, that the error rapidly narrows, and the steady-state error of the lateral deviation distance decreases accordingly. In contrast, the yaw angle error fluctuates significantly. Reducing the period T will make the system response faster and the control effect more practical. A smaller value of the period T will lead to more aggressive navigation or sharper corners, while a more considerable value will lead to gentler navigation. Meanwhile, considering the safety and control rapidity in the UAV operation, the value of T is selected here as equal to 20 s.Settingζ=0.707 and T=20s simulation initial state Vt=30m/s, constant wind disturbances in the north direction and east direction are added U=5m/sandV=5m/s. Track a circular trajectory with a radius of 600 m (R=600m).The velocity profile with wind disturbances is shown in Figure24. The tracking effect is shown in Figures 25 and 26.Figure 24
Velocity profile with wind disturbances.Figure 25
Circular trajectory tracking under wind disturbance.Figure 26
Angle error and deviation distance under wind disturbance.As shown in Figure24, the crosswind with 5 m/s has been simulated.Simulation results show that the lateral deviation distance can be controlled at ±3 m precision, while the yaw angle error is ±0.5°, which offers a good effect of the circular trajectory tracking.Figures27 and 28 show the simulation of circular track tracking in a windless environment. Setting ζ=0.707 and T=20s, initial state Vt=30m/s, track a circular trajectory with a radius of 600 m. The lateral deviation distance is controlled at −1 m, while the yaw angle error is 0.4°, which shows a good effect of circular trajectory tracking without wind disturbance.Figure 27
Circular trajectory tracking.Figure 28
Angle error and deviation distance.(2) Flight Data Profile. The UAV tracks a circle with a radius of R = 450 m, and the center of the circle is located at longitude: 100.23°, latitude: 41.01°, the tracking effect is shown in Figure 29.Figure 29
Circular trajectory tracking for the flight test.Under the constant wind disturbance of 7 m/s (direction 184), airspeed values vary from 30 m/s to 37 m/s, as shown in Figure30.Figure 30
Wind speed and airspeed for the flight test.The value of the lateral deviation distance is between 0 and −4 m (shown in Figure31), which means the UAV keeps on the left of the desired circular path. The yaw angle error can be controlled at ±3° (shown in Figure 32), larger than the value obtained by simulation. It is affected by crosswind to some extent, but the overall tracking effect is relatively good.Figure 31
Lateral deviation distance for the flight test.Figure 32
Yaw angle error for the flight test.For the SD-40,L1 guidance law enhances the circular trajectory tracking control ability and the ability to resist wind, using the insensitive period and damping parameters. This law can calculate different expected accelerations according to different speeds V and offers good adaptability to different usual flight speeds and interference of ambient wind in actual flight. Furthermore, L1 guidance law can track irregular curves and be extended to more advanced trajectory algorithms, including obstacle avoidance flight and formation flight.


## 3.2.1. Longitudinal Height Control

The control of the longitudinal height of the SD-40 is performed through an outer loop based on the TECS. The throttle controls the change of total energy, and the deflection angle of the elevator distributes the kinetic energy and the gravitational potential energy. As such, the speed control and the height control are decoupled [22–24].The error between the kinetic energy and the specified kinetic energy at a certain moment can be expressed as(20)Kerr=Kref−K=12mVtc2−12mVt2,where K is the kinetic energy of the particle, Vt is the true airspeed, and Vtc is the specified airspeed.The error between the gravitational potential energy and the specified gravitational potential energy at a certain moment can be expressed as(21)Uerr=mghc−h.The error of the total energyE and the allocation L of the total energy error can be expressed as per the following equation:(22)E=Uerr+Kerr,L=Uerr−Kerr.The allocation is updated as(23)L=2−kUerr−kKerr.By differentiating both sides of the total energy equation and assuming that the trajectory angleγ is small, the rate of change of the total energy is approximately expressed as(24)E˙=E˙TmgVt=V˙tg+h˙Vt=V˙tg+sinγ≈V˙tg+γ.Meanwhile, the longitudinal dynamics equation of the UAV is expressed as per the following equation:(25)T−D=mV˙t+mgsinγ≈mgV˙tg+γ,where T and D are, respectively, the thrust and drag force of UAV during cruise state. In this balanced state, the thrust increment is expressed as(26)ΔT=mgV˙tg+γ=mgΔE˙V.In summary, the change of total energy can be controlled by the change in thrust [25].The deflection angle of the elevator changes the track angle, which leads to the mutual conversion between the potential energy and the kinetic energy [26, 27]. Assuming that there is no significant energy loss during the conversion process, the change rate of the energy distribution can be defined as(27)L˙=γ−V˙tg.Total energy control block diagram is shown in Figures12 and 13.Figure 12
Total energy change control.Figure 13
Total energy distribution control.Hg is the height command, Vg is the speed command. Esp is the specified energy, Lsp is the specified energy distribution, and E˙sp and L˙sp are the corresponding rates of change. E is the total energy, L is the energy distribution, and E˙ and L˙ are the corresponding rates of change, respectively. KE˙⟶ΔT is the gain from energy change rate to throttle andKL˙⟶Δθ is the gain from energy distribution change rate to pitch angle.The complete TECS control law, as well as the total energy change control channel, can be expressed as(28)E˙c=kpE×E+dpEE˙+IpE∫Edt,Tg=E˙c×KE˙⟶ΔT,KE˙⟶ΔT=δTmax−δTminE˙max−E˙min,where E˙max and E˙min are the max and min rate of change of the total energy in climbing mode and descending mode, respectively. δTmax and δTmin are the max and min opening of the throttle, respectively.The total energy distribution control channel could then be expressed as(29)L˙c=keL×L+deLL˙+IeL∫Ldt.ControllingL˙c with the elevator requires a pitch rate/pitch attitude feedback inner-loop control law, where θg will be transformed into elevator command to stabilize the UAVs short period mode. Thus, L˙c develops θg, serves as the input of the pitch attitude control loop, which acts indirectly on the elevator [28–30].(30)L˙gVt=gh˙−VtV˙tgVt≈γ−V˙tg,θg=L˙cKL˙⟶Δθ=1gVt×L˙c,where gVt is the conversion coefficient from the climb angle to energy distribution change rate, which adapts to a range of speed changes.(1) Simulation Data. Case 1: Given the initial state of UAV, height is 100 m and speed is 30 m/s. Whent = 1 s, change the height command to 105 m, then the height step response is shown in Figure 14.From the graph in Figure15, the height can track the command well with some overshoot. Meanwhile, the speed is basically unchanged, and the fluctuation is controlled within ±0.5 m/s.As the height increases, the kinetic energy is converted into potential energy, which requires a larger throttle energy value. Even though the value of the elevator energy is negative, the UAV performs an upward movement. When the new balance is reached, the value comes back to zero.Case 2: Given the initial state of UAV, the height is set equal to 100 m and the speed to 30 m/s. When timet = 1 s, change the speed command to 32 m/s, then speed step response is shown in Figure 16.As shown in Figure17, the speed can track the command well, with some likely overshoot. Meanwhile, the height is unchanged, and the fluctuation is controlled within a distance of ±0.3 m.As the speed increases, the kinetic energy increases, requiring a larger throttle energy value. The elevator's value is positive to accelerate the UAV, and when the new balance is reached, the value keeps at 2°, as shown in Figure15.Figure 14
Height step response for a given speed.Figure 15
Elevator and throttle command for height step response simulation.Figure 16
Speed step response for a given height.Figure 17
Elevator and throttle command for speed step response simulation.(2) Flight Data. The UAV's initial height is 1470 m and speed is 30 m/s. When t = 1 s, the change in height command and the speed command are Hg=1170m,Vg=32m/s, then step response is shown in Figure 18.Figure 18
Height and speed response of step input test.As shown in Figures18 and 19, the altitude drops slowly with the command, and the airspeed is kept at 35 ± 1 m. Since the UAV flyes at a constant speed, the throttle remains unchanged to keep the kinetic energy intact. The elevator command is essentially negative, raising the UAV's head and increasing the drag as the gravitational potential energy slowly converts to internal power to work done against friction.Figure 19
Elevator and throttle command for height and speed step response test.


## 3.2.2. Lateral Trajectory Control

The lateral trajectory consists of the roll and yaw motions, which have been considered in the controller design. L1 guidance is well known for its simplicity in tracking circular and linear motions typical for fixed-wing vehicles’ flight plans. However, the law has a specific ADRC effect in curve tracking, which is introduced for circular trajectory tracking, as shown in Figure20. Under this section, the controller design is presented along with simulated results and the profile of the flight data.Figure 20
Principle diagram of circular trajectory tracking.When the UAV moves laterally, the roll angle is not zero, resulting in lift tilt and lateral acceleration as expressed by the following equation:(31)ascmd=V2R=V2L1/2sinη=2V2L1sinη,(32)sinη=sinη1+η2+η3≈η1cosη3+η2cosη3+sinη3,(33)ascmd=2V2c2L12d+2VcL1d˙+V2R.where ascmd is the acceleration command, L1 is the distance between the reference point and the UAV, which is a fixed value, and η is the angle from the ground speed to the line L1. Meanwhile, the velocity direction change due to the acceleration can be expressed as(34)Δψ=asVΔt,(35)ascmd≈Vψ˙=Vθ˙−η˙2=V2R−d¨.Based on the equations (33) and (35), we can get(36)d¨+2ζωnd˙+ωn2d≈0,whereζ=2/2,ωn=2Vc/L1,andc=1−L1/2R2.The undamped natural frequency is determined byV and L1:(37)L1=VTc2π.The period and damping ratio are taken as the control parameters of the control law andL1 as a variable proportional to the flight speed [31].The lateral acceleration is provided by the lateral component of the lift force during the rolling of the UAV as(38)ascmd=g⋅tanϕ.The roll angle command can be expressed as(39)ϕcmd=tan−1ascmdg.L1 control structure diagram shown in Figure 21 is designed based on the following equation:(40)ascmd=Kxd+Kvd˙+V2R,where Kx=ωn2=4π2/T2,Kv=2ζωn=4πζ/T, and R is the radius of the tracked circular arc.Figure 21
L1 block diagram. Here,η2 is the yaw angle from the target heading to the speed heading and d is the lateral deviation distance, that is, the distance between the UAV real-time position and the center of the tracked circular arc minus the radius of the tracked circle.(1) Tracking Simulation. A nonlinear model in Matlab/Simulink environment has been adopted to carry out the circular trajectory tracking simulation. Given the L1 guidance law parameters period damping ζ=0.707, the simulation data at different periods T, as shown in Figures 22 and 23.Figure 22
Yaw angle error at different periods.Figure 23
Lateral deviation distance at different periods.It can be seen from Figures22 and 23, with the decrease of the period T, the response time of the lateral deviation distance and yaw angle error, that the error rapidly narrows, and the steady-state error of the lateral deviation distance decreases accordingly. In contrast, the yaw angle error fluctuates significantly. Reducing the period T will make the system response faster and the control effect more practical. A smaller value of the period T will lead to more aggressive navigation or sharper corners, while a more considerable value will lead to gentler navigation. Meanwhile, considering the safety and control rapidity in the UAV operation, the value of T is selected here as equal to 20 s.Settingζ=0.707 and T=20s simulation initial state Vt=30m/s, constant wind disturbances in the north direction and east direction are added U=5m/sandV=5m/s. Track a circular trajectory with a radius of 600 m (R=600m).The velocity profile with wind disturbances is shown in Figure24. The tracking effect is shown in Figures 25 and 26.Figure 24
Velocity profile with wind disturbances.Figure 25
Circular trajectory tracking under wind disturbance.Figure 26
Angle error and deviation distance under wind disturbance.As shown in Figure24, the crosswind with 5 m/s has been simulated.Simulation results show that the lateral deviation distance can be controlled at ±3 m precision, while the yaw angle error is ±0.5°, which offers a good effect of the circular trajectory tracking.Figures27 and 28 show the simulation of circular track tracking in a windless environment. Setting ζ=0.707 and T=20s, initial state Vt=30m/s, track a circular trajectory with a radius of 600 m. The lateral deviation distance is controlled at −1 m, while the yaw angle error is 0.4°, which shows a good effect of circular trajectory tracking without wind disturbance.Figure 27
Circular trajectory tracking.Figure 28
Angle error and deviation distance.(2) Flight Data Profile. The UAV tracks a circle with a radius of R = 450 m, and the center of the circle is located at longitude: 100.23°, latitude: 41.01°, the tracking effect is shown in Figure 29.Figure 29
Circular trajectory tracking for the flight test.Under the constant wind disturbance of 7 m/s (direction 184), airspeed values vary from 30 m/s to 37 m/s, as shown in Figure30.Figure 30
Wind speed and airspeed for the flight test.The value of the lateral deviation distance is between 0 and −4 m (shown in Figure31), which means the UAV keeps on the left of the desired circular path. The yaw angle error can be controlled at ±3° (shown in Figure 32), larger than the value obtained by simulation. It is affected by crosswind to some extent, but the overall tracking effect is relatively good.Figure 31
Lateral deviation distance for the flight test.Figure 32
Yaw angle error for the flight test.For the SD-40,L1 guidance law enhances the circular trajectory tracking control ability and the ability to resist wind, using the insensitive period and damping parameters. This law can calculate different expected accelerations according to different speeds V and offers good adaptability to different usual flight speeds and interference of ambient wind in actual flight. Furthermore, L1 guidance law can track irregular curves and be extended to more advanced trajectory algorithms, including obstacle avoidance flight and formation flight.


## 3.3. Emergency Control

Due to the limitations of cost, payload weight, and payload size, most small civil UAVs adopt the Attitude and Heading Reference System (AHRS) based on a microelectromechanical system (MEMS). AHRS cannot provide accurate and stable attitude data under the following conditions:(a)
When the UAV is under large continuous overload(b)
When the algorithm of attitude solving unit has some problem(c)
When the calculated data of attitude solving unit are divergent(d)
When the magnetometer is interfered with and provides false heading informationIn all these four cases, a triaxial attitude angle information and geomagnetic course information are invalid. The control method is the same in all the four cases because it is designed to deal with this information invalidation. In this study, we intend to propose a general solution for this information invalidation. The traditional attitude angle control technology is no longer applicable, and the control law based on the angular rate is needed [32]. Sound signals for flight controllers are UAV’s position information, track angle, and ground speed provided by GPS; airspeed, atmospheric altitude, and rate of change information supplied by atmospheric pressure sensor; triaxial acceleration and triaxial angular rate provided by AHRS. The emergency controller design looked at the longitudinal and lateral control of the UAV.


### 3.3.1. Longitudinal Control Law

TECS can decouple the height control loop and the speed control loop of the longitudinal motion, while the latter has no attitude control loop but directly controls the engine's thrust. Therefore, the speed control loop of TECS law can continue to be used. On the other hand, the height control law needs to be updated.The pitch control loop is used to stabilize the pitch attitude and serves as the inner loop of the height controller. Since there is no pitch angle feedback information, height control based on angular pitch rate is proposed. The reduced-order ESO model is used for the inner-loop angular rate control. The longitudinal controller structure is shown in Figure33:(41)δe=KeIq∫Q−Qgdt+KeQQ,Qg=KQH˙H˙−KeHH−Hg,where Qg is the given pitch angular rate and H˙ is the vertical speed of the UAV.Figure 33
Longitudinal controller structure of the SD-40.The outer loop height control output serves as the input to the inner-loop attitude angular rate control loop, which ultimately acts on the elevator.The UAV's initial state of the simulation: height is 100 m and speed is 30 m/s. Whent = 1 s, the height command is changed toHg=105m.The vertical speed is obtained according to a specific proportional relation of the error between height and its command. Whent = 1 s, there is a sudden change in the vertical speed command due to the considerable height error, but the vertical speed can track the command well and respond quickly (Figure 34).Figure 34
Vertical speed response to height changes.As shown in Figure35, the value of the elevator energy is negative to enable the UAV to move upward, and when the new balance is reached, the energy value comes back to zero.Figure 35
Elevator deflection angle response to height changes.The height step command is tracked without overshoot, and the response is quick (see Figure36). In addition, by putting an integral part into the pitch angular rate control loop, the influence of external interference on the system can be easily neglected.Figure 36
Height step response of the SD-40.


### 3.3.2. Lateral Control Law

The structure of lateral roll angular rate control is similar to that of longitudinal pitch angular rate control. The turning control of UAV is realized by controlling the yaw angular rate.The outer track control loop outputs the roll angular rate command, which will be used as input to the inner loop of the roll to control the aileron deflection angle (see Figure37 and equations (42) and (43)):(42)δa=KaIp∫P−Pgdt+KaPP,(43)Pg=KaRR−KaPsiPsi−Psig,where Pg is the roll angular rate command, R is the feedback of yaw angular rate, and Psi is the feedback of the track azimuth, which can be provided by GPS.Figure 37
Lateral controller structure of the SD-40.As shown in Figure38, the track azimuth tracked the command well without overshoot.Figure 38
Track azimuth step response to command tracking.The yaw angular rate command needs to be optimized into a smooth transient profile (Figure39), as the desired trajectory to allow the output to follow more closely.Figure 39
Yaw angular rate response to command tracking.It can be seen from Figures40 and 41 that the proportion term plays a significant role in the roll angular rate control loop. In addition, simulation data show that when AHRS fails, the emergency control law without Euler Angle feedback can track the control instructions well and enable the UAV to make a crash landing or continue to perform the mission, improving flight safety.Figure 40
Roll angular rate response to command tracking.Figure 41
Aileron deflection angle response to command tracking.


## 3.3.1. Longitudinal Control Law

TECS can decouple the height control loop and the speed control loop of the longitudinal motion, while the latter has no attitude control loop but directly controls the engine's thrust. Therefore, the speed control loop of TECS law can continue to be used. On the other hand, the height control law needs to be updated.The pitch control loop is used to stabilize the pitch attitude and serves as the inner loop of the height controller. Since there is no pitch angle feedback information, height control based on angular pitch rate is proposed. The reduced-order ESO model is used for the inner-loop angular rate control. The longitudinal controller structure is shown in Figure33:(41)δe=KeIq∫Q−Qgdt+KeQQ,Qg=KQH˙H˙−KeHH−Hg,where Qg is the given pitch angular rate and H˙ is the vertical speed of the UAV.Figure 33
Longitudinal controller structure of the SD-40.The outer loop height control output serves as the input to the inner-loop attitude angular rate control loop, which ultimately acts on the elevator.The UAV's initial state of the simulation: height is 100 m and speed is 30 m/s. Whent = 1 s, the height command is changed toHg=105m.The vertical speed is obtained according to a specific proportional relation of the error between height and its command. Whent = 1 s, there is a sudden change in the vertical speed command due to the considerable height error, but the vertical speed can track the command well and respond quickly (Figure 34).Figure 34
Vertical speed response to height changes.As shown in Figure35, the value of the elevator energy is negative to enable the UAV to move upward, and when the new balance is reached, the energy value comes back to zero.Figure 35
Elevator deflection angle response to height changes.The height step command is tracked without overshoot, and the response is quick (see Figure36). In addition, by putting an integral part into the pitch angular rate control loop, the influence of external interference on the system can be easily neglected.Figure 36
Height step response of the SD-40.


## 3.3.2. Lateral Control Law

The structure of lateral roll angular rate control is similar to that of longitudinal pitch angular rate control. The turning control of UAV is realized by controlling the yaw angular rate.The outer track control loop outputs the roll angular rate command, which will be used as input to the inner loop of the roll to control the aileron deflection angle (see Figure37 and equations (42) and (43)):(42)δa=KaIp∫P−Pgdt+KaPP,(43)Pg=KaRR−KaPsiPsi−Psig,where Pg is the roll angular rate command, R is the feedback of yaw angular rate, and Psi is the feedback of the track azimuth, which can be provided by GPS.Figure 37
Lateral controller structure of the SD-40.As shown in Figure38, the track azimuth tracked the command well without overshoot.Figure 38
Track azimuth step response to command tracking.The yaw angular rate command needs to be optimized into a smooth transient profile (Figure39), as the desired trajectory to allow the output to follow more closely.Figure 39
Yaw angular rate response to command tracking.It can be seen from Figures40 and 41 that the proportion term plays a significant role in the roll angular rate control loop. In addition, simulation data show that when AHRS fails, the emergency control law without Euler Angle feedback can track the control instructions well and enable the UAV to make a crash landing or continue to perform the mission, improving flight safety.Figure 40
Roll angular rate response to command tracking.Figure 41
Aileron deflection angle response to command tracking.


## 4. Conclusion

In this paper, an aircraft flight mode controller for fixed-wing UAVs with VTOL functions has been successfully designed where the SD-40 served as a reference. The controller design process starts by establishing linear and angular perspective, then proceeds with the design of flight controller. Controllers for the attitude, the trajectory, and emergency condition flight have been developed and tested through simulations in the MATLAB and SIMULINK environment. First, pitch and roll channel controllers have been designed for attitude control. Next, the total energy control system, the extended state observer, and L1 guidance have been proposed for longitudinal and lateral control of fixed-wing UAV SD-40. An attitude controller based on the ESO has been proved to be effective from its anti-interference ability. The TECS is again proposed for the longitudinal height channel, while the L1 guidance law is used for lateral trajectory tracking. The controller and simulation data show that the controller effectively responds to the command and decouples the speed control and the track control while improving the curve tracking ability and the wind resistance ability. Finally, a controller based on angular rate has been proposed in emergency and simulation data to verify its effectiveness.



---

*Source: 7902134-2021-09-29.xml*