Improved Design and Implementation of Automatic Flight Control System (AFCS) for a Fixed Wing Small UAV

,


Introduction
The great researches in unmanned aerial vehicles (UAVs) manufacturing are to stimulate new ideas for advanced low cost small UAVs (SUAVs).In recent years the focus has shifted from special qualified components to commercial off the shelf (COTS) hardware which can be slightly adapted or modified to provide adequate reliability in flight.
In general terms, automatic flight control system (AFCS) is a system used to guide an aircraft without the assistance of a pilot.A full complete complicated autopilot that controls the position (altitude, latitude, longitude) and attitude (roll, pitch, yaw) during the various phases of flight (e.g., take-off, climbing, level flight, descent, landing, and loiter) [1], [2].
During designing of AFCS many issues must be analyzed; beginning from model in loop (MIL), software in loop (SIL), and processor in loop (PIL) stages.During these stages the environment disturbances and sensors noise are considered in the design architecture of test platform [3], [4].The whole autopilot is tested with many scenarios.The AFCS is composed of Arduino Mega 2560 for flight computer, MPU6050 for IMU, digital compass HMC5883L for magnetometers, UBLOX LEA 6h for GPS, and finally 3DR 915 MHz radio module is used for communication subsystem.
Using low cost micro electro mechanical systems (MEMS) technologies provides us the possibility to shrink the size and weight of the SUAV but its raw output is not accurate due to its internal random noise added with the output.Hence, state estimation techniques such as KF or complementary filter for attitudes and navigation states estimation [5], [6] must be considered in the design of SUAV autopilot to reduce these various types of noise.
All the previous issues are integrated together to construct a high precision autopilot with its two motion controllers: the longitudinal and lateral motion controller [7], [8].Next PIL simulation step which depends on replacing the emulated hardware under test or the controller strategy in the simulation model with real hardware that interacts with the models designed with the computer simulation programs [9], [10].The autopilot designed procedure (lateral and longitudinal) of an Ultrastick-25e SUAV is depending on state space linear models (lateral and longitu-dinal) and analytic linear model of a coordinated turn derivation with trimmed values of a straight and leveling scenario [11].
The remaining of the paper is divided in the following sections.Section 2 presents the review of a complete design and test of SUAV autopilot.The third section introduces both implementation of the autopilot and the selection of avionics and sensors of SUAV.In Sec. 4, the state estimation technique is presented.Section 5 introduces the whole system implementation of the AFCS and the communication between autopilot and the GCS.The sixth section will be devoted to the PIL simulation and autopilot evaluation.Finally, some concluding remarks are provided in Sec. 7.

UAV System Structure
As stated before, the main objective of this paper is to design an autonomous flight control system of SUAV.It allows Ultrastick-25e to be flown autonomously and to be remotely controlled.This system will be composed by an autopilot, GCS, and all the required equipment for data transmission between the autopilot and the GCS.
The design of longitudinal motion controller is designed as follows: Beginning with the inner loop pitch rate (q) (pitch damper), and then pitch attitude hold controller (pitch tracker) which is designed with Proportional Integral (PI) controller, linearization of the nonlinear equation of motion of altitude dynamics is a vital step to get a linear relation between the altitude and pitch angle at assumption of cruise speed of 17 m/s.The outer loop which is the Altitude hold controller was designed also.The response of the velocity loop is simulated in the same manner.
The design of lateral motion controller was introduced also as follows: The most inner loop was designed with feedback gain (roll damper), and then roll attitude hold controller was designed with PI-controller, linearization algorithm to get a linear relation between heading angle and roll attitude, the Yaw angle controller is the outer loop which is used for controlling the direction of aircraft with P-controller only which satisfies the design requirements.Yaw damper is designed with washout filter.

Whole Autopilot Flight Scenarios, Evaluation and Simulation Tests
Ascending scenario is tested in the non-linear model to check the all over behavior of the aircraft and then executed the scenario of straight and leveling flight.The rectangular motion command is applied in the non-linear model to check the behavior of the aircraft.The last test is to check the whole performance of the autopilot in the Climbing Turn scenario.The command is the altitude from 100 to 600 meter and the heading in rectangular motion.
-Simulation time 120 s -Turn in 4 steps from 0 o to 360 o .
-Alt command 500 m direct.Figure 1 shows the trajectory of airplane due to the climbing turn scenario.

Autopilot Hardware Block Diagram
Due to the small size of the Ultrastick-25e, many tasks must be handled and managed by a powerful processor to be capable of the management between the different subsystems of the aircraft.Figure 2 illustrates nearly all of the AFCS electronic components.These electronic components are grouped into seven main areas: -The flight computer.
-Components specific to the scientific payload.
-Communications devices with a ground station.
-Components provide fundamental electrical support.
-Components specific to the aircraft power subsystem.
-Propulsion and control subsystem components.

Selection of Avionics and Sensors
Electronic sets operated on missiles, aircrafts and spacecrafts are usually summarized as "Avionics", which is composed of digital processing modules and communication buses, supporting more avionic applications such as flight control, flight scenario management, flight telemetry, etc.There are direct basic components wanted for autopilot installation, such as navigation set, IMU and supported components.There are other avionics that are not directly the contents of the autopilot, but interact with it as communication, power systems, and engine monitoring.
Avionics have to ensure an enormous variety of important requirements such as safety, robustness to equipment failures and real-time.So aircraft manufacturers have proposed several design metrics.The next generation of avionic architectures involves reconfiguration capabilities and integrating COTS processing equipment such as multicore processors.These two challenges will be central to this next generation of avionics architectures (called altitude Fig. 2. AFCS electronic components. Integrated Modular Avionic 2G) (IMA-2G or IMA 2nd generation) [12].

Flight Computer
Nearly all UAVs have at least one flight computer devoted to performing all processing on-board UAV.This credit card sized single-board-computer can be chosen from commercial products and is built around a CISC or RISC microprocessors [13].Nearly all of the hardware subsystems onboard UAV interface to the flight computer and must be controlled by it.There are many open source boards that can be adapted to perform this flight computer as ARM based boards, AVR based boards, and INTEL based boards.The chosen embedded system for our autopilot implementation is the Arduino MEGA 2560 due to its simplicity, low cost, high efficiency, and historical use in open source autopilots (Ardupilot APM2.x series).

SUAV Sensors
For automatic machines as unmanned aircrafts or other autonomous vehicles, navigation is very important.The navigation states of the aircraft are obtained and estimated according to many techniques such as Attitude and Heading Reference System (AHRS), INS, and the full complete state estimation by GPS/INS technique [14].Our interest lies in integrating IMU consisting of a triad accelerometer and gyros, and magnetometers to estimate the attitudes of the aircraft.With these tools the construction of AHRS is implemented.GPS is used to provide the best possible aircraft position in terms of the latitude, longitude and height above the surface of the earth, also it provides ground heading and velocity.
Gyroscopes and accelerometers chips can be integrated on a small board and can effectively give the attitude of the concerned vehicle.With the advent of MEMS technology, all this can be done at extremely low cost, but with errors.These errors can be considered a negligible problem because of the estimation techniques which will be explained in the following sections AHRS can be developed and implemented in a microcontroller or a digital signal processor.The state estimator algorithm would return the estimated attitude of the aircraft.
The sensors used for guidance, navigation and control of the aircraft will be considered in details.These sensors are listed below: -IMU (Accelerometers, and rate gyros). -Magnetometers. -GPS.

A. Accelerometer
Accelerometer sensors measure the difference between any linear acceleration in the accelerometer's reference frame and the Earth's gravitational field vector.In the absence of linear acceleration, the accelerometer output is a measurement of the gravitational field vector and can be used to determine the pitch and roll orientation angles.Due to the sequence of frames rotation from the inertial frame (F i ) to the body frame (F b ), the attitude angles are appearing.The most common order is the aerospace sequence of yaw then pitch and finally a roll rotation [15].
-The x-axis is aligned along the i b of body frame (F b ) of the aircraft.
-The z-axis points downwards as k b of body frame (F b ) so that; it is aligned with gravity when the aircraft is in a straight and leveling flight.
-The y-axis is aligned at right angles and is aligned along j b of body frame (F b ).
The three accelerometers are mounted near the center of mass of the aircraft, with the sensitive axis of one accelerometer aligned with each of the body axes.Accelerometers measure the difference between the acceleration of the aircraft and the gravitational acceleration.The output of the accelerometers (G p ) is given in (1).
where G p is the output of the accelerometers in the 3-axes (G px , G py , G pz ), R is the rotation matrix (DCM), g is the gravity acceleration, and a r is the linear acceleration measured in the Earth's reference frame (assumed to be equal zero) Direct Cosine Matrix (DCM) can be called from [15] to express the R matrix, which is the rotation matrix de-scribing the orientation of the aircraft body frame (F b ) relative to the earth coordinate frame F i .The aircraft accelerometer output G p (measured in the native accelerometer units of g) is: The accelerometer output has three components, but the vector magnitude must always equal 1g in the absence of linear acceleration.The above rotation sequence in (3) depends only on the roll () and pitch () angles and can be solved, hence the above procedures are done to extract roll and pitch attitudes from accelerometer to make fusion between accelerometers and gyroscopes sensors to estimate the accurate values of roll and pitch attitudes.All accelerometers are completely insensitive to rotations about the gravitational field vector and cannot be used to determine heading, so it's conventional to select either the rotation sequence R xyz or the sequence R yxz to eliminate the yaw rotation and allow solution for the roll and pitch angles.The unknown yaw angle represents the aircraft rotation from north, but its determination requires the addition of a magnetometer sensor to create a compass.Equation ( 2) can be rewritten in the form of (5) relating the roll and pitch angles to the normalized accelerometer reading G p [16]: Solving ( 5) for the roll and pitch angles gives ( 6) and ( 7)

B. Rate Gyroscopes
Rate gyroscopes can be used to determine attitude rates of aircraft, MEMS rate gyros typically operate based on the principle of the Coriolis acceleration.There some types of gyroscopes as following [17].
-Ring laser gyros (very accurate but very expensive).
These MEMS gyros have many kinds of errors.It can be briefed in the fixed bias or offset, bias drift (which is usually modeled as random walk) and bias vibration from one turns to another (thermal effect).It is common to measure the angular rates about each of the body axes using three gyros by aligning the sensitive axis of a gyro along each of the i b , j b , and k b axes of the SUAV.These rate gyros measure the angular body rates p, q, and r.
The symbols in Fig. 3 are stated as following: -x, y, z Body frame axes.-X, Y, Z Forces acting on the body frame.
-L, M, N The moments acting on body frame.
-p, q, r Attitude rates measured on body frame.
-u, v, w Velocity components of the airframe projected onto body frame.

Magnetometer
The compass is a device that measures the direction of the magnetic field locally and provides an indication of heading relative to magnetic north, ψ m .The Earth's magnetic field is three-dimensional, with north, east, and down components that vary with location along the Earth's surface.
Modern digital compasses use three axes magnetometers to measure the strength of the magnetic field along three orthogonal axes.In UAV applications, these axes of measurement are usually aligned with the body axes of the

HMC5883L Digital Compass
The Honeywell HMC5883L is a low-cost digital compass that has many benefits to be used in autopilot implementation; offset cancellation, a 12-bit ADC that enables 1° to 2° compass heading accuracy.

Global Positioning Systems (GPS)
The Global Positioning System (GPS) is a satellitebased navigation system that provides 3-D position information and velocity for objects on or near the Earth's surface [6], [18].The key component of the GPS system is the constellation of 24 satellites that continuously orbit the Earth at an altitude of 20, 180 km.GPS measurement has some kinds of errors that can be briefed in Dilution Of Precision (DOP) factor [15], Ephemeris Data errors in the range of 1 to 5 m, Satellite Clock errors is introducing an error of maximum 3.5 m, Ionosphere errors are typically between 2 and 5 m, Troposphere introduces range errors of about 1 m, Receiver measurements error is about is 0.5 m for maximum due to increasing of the technology.Table 1 illustrates the total device errors briefly.The cumulative effect of each of these error sources on the pseudorange measurement is called the user-equivalent range error (UERE).
Differential GPS [19] can be used to reduce the bias error components of GPS position measurements to much smaller values.GPS measurements are commonly available at 1 Hz.New systems suitable for SUAV implementations provide GPS measurements at 5 Hz updates.

U-BLOX 6H GPS Module
The LEA-6h module series is a high performance GPS module.It can be upgraded using U-center program, ready for GLONASS (Russian) and GALILEO (European) satellites.U-Blox LEA 6H has been designed with low power consumption and lower cost.These reasons make it popular for a wide range of industrial applications, especially SUAV autopilot applications as paparazzi and APM 2.8 autopilots.

Communication Components
The next step of the autopilot implementation is designing of the communication subsystem to construct the down link telemetry data and uplink commands; there are two groups of communications connected between UAV and ground (man pilot, payload and telemetry data transmitted from the UAV to ground station).3DR radio telemetry module V 1 with 915 MHz is used for telemetry communication purposes due to its small size and light weight.It's designed as an open source radio set, offering a lower price, long range (about 500 m) and can be extended to several kilometers with a small omni antenna, The system provides a full-duplex link.

SUAV State Estimator
The availability of using new sensor technologies gives the aerospace engineers the opportunity to design SUAV sensors depending on MEMS which are smaller and lighter than the old mechanical sensor devices.It provides the possibility to shrink the size and weight of the UAV to a new milestone.The solution for defeat the noise and obtain precise results for attitudes and navigation states is to design: -A good controller.
-Make a good analysis.
-Using state estimator depends on good estimators.
KF is one of the best state estimators.It's involved with most of the navigation techniques [5], [6].In this section we will discuss the state estimator techniques by complementary filter and KF.The better estimator is the chosen one in the estimation of SUAV attitudes.

Complementary Filter
The attitude angles from the gyros, accelerometers, and magnetometers are estimated in this subsection by a complementary filter.It's used to combine them in a way that the resulted data are stable and accurate.The gyro raw data are very good and stable for the short term, but they can quickly drift and become inaccurate, the accelerometer gives a good raw data over a longer period of time, but in the short run they can be noisy and also it has many ripples.So the data output from gyros and accelerometers can be combined in the following manner to give us the concept of a complementary filter.
Generally, there are cases where you have two different measurement sources for estimating one variable and the noise properties of the two measurements are such that one source gives good information only in the first time region (gyro) while the other is good only in the extended measurement time region (accelerometer) but with some ripples, so you can use a complementary filter [20].The math calculations of a complementary filter are much simpler, because it only works in one step.The optimal key (8) for implementing the filter is: where Angle is the estimated attitude, AG is the gyro measured factor (for our case = 0.93), gyro is the data measured from the gyroscope, AA is the accelerometer measured factor (for our case = 0.07), and Acc is the data extracted from accelerometer according to (6) for roll angle (φ) or according to (7) for pitch angle (θ).

Kalman Filter (KF)
The modern filter theory began with N. Wiener's work in the 1949.His work was based on minimizing the mean-square error, so this branch of filter theory is sometimes referred to as least-squares filtering [21].In 1960 R. E. Kalman considered the same problem that Wiener had dealt with earlier, but in his paper in 1960 [22], he considered the noisy measurement to be a discrete sequence in time in contrast to a continuous-time signal.
Engineers, especially in the field of navigation, are quick to see the Kalman technique as a practical solution to some applied filtering problems.Also, the rapid advances in computer technology that occurred in the 1960s certainly contributed to popularizing KF as a practical means of separating signal from noise.
It is an optimal combination, in terms of minimization of variance, between the prediction of parameters from a previous time instant and external observations at a present time instant (recursive concept).
KF is an extremely effective and versatile procedure for combining noisy sensor outputs (gyros and accelerometers) to estimate the state of the aircraft with uncertain dynamics.Noisy sensor outputs include output from the GPS and INS.State of the system may include the position (latitude, longitude, height), velocity, attitude (φ, θ, ψ) and attitude rates (P, q, and r) of a vehicle.And uncertain dynamics include unpredictable disturbances in the sensor parameters or disturbances caused by a human operator or a medium (like wind).
Briefly we can say that KF has been just a computer algorithm for processing discrete measurements (the input) into optimal estimates (the output).KF here is applied to estimate good values of attitudes of the aircraft by eliminating the errors introduced by the gyros, accelerometers, and magnetometers.
According to discrete KF the state space equations are: with an output (10) .
The variables W k and V k represent the process and measurement noise respectively.They are assumed to be independent of each other, white, and with normal probability distributions.
The key parameters in KF design are the noise covariance (Q) and measurement covariance (R).We must evaluate the covariance matrices (R, Q); R should typically be the square of the standard deviation of the gyro.It should be there in the data sheet, or you could just keep things stationary and log the data for about 5 minutes, Calculate the mean and standard deviation.

The Implementation Procedures of the Autopilot
From the beginning, we will power our flight computer and connect IMU, and then testing of IMU is implemented to check the successfulness of interfacing it with the flight computer.The proceeding is continued to Read raw data from sensors, Converting to accelerations from (accelerometers) and angular rates from gyroscopes, converting data output from accelerometers to roll and pitch attitudes according to ( 6) and ( 7) respectively, reading data (pitch and roll) from complementary filter algorithm and showing the results, Reading data (pitch and roll) from KF algorithm and showing the results.As discussed earlier, yaw angle is so drifted and all the time from many tests beginning from zero and drift, so we combined another device with MPU6050.Magnetometer can be combined to measure the heading of the aircraft if the value is not accurate, we will combine the two results of the gyroscope and magnetometer with each other.After connecting Honeywell compass HMC5883L, I2C ports are scanned to assure that the compass is right connected.
That's well, we have two devices connected to our flight computer from I2C port 0x68 for MPU6050 and 0x1E for magnetometer, the next step is to calibrate the magnetometer.After calibration of magnetometer, the results are approximately stable and accurate but with some ripples, so heading output from magnetometer is combined with the output of gyroscope to estimate the optimal value.Till now we can stop for a while to analyze the previous work, then decide which state estimator will be chosen in the autopilot design, from the analysis we have three resulted figures that determine the comparison between the attitudes with and without filtering as will be seen in Fig. 4 for pitch estimation comparison, Fig. 5 for roll, and Fig. 6 for yaw.
The previous figures are done to aid us to decide which state estimator can be used to give us the best performance.So KF is so efficient for our requirements for assigning roll and pitch, and Complementary filter data will be used in the assigning of heading.After that, the communication links between autopilot and ground control station wireless are established by connecting our telemetry module 3DR robotics.Then checking the communication between the ground station and autopilot as in Fig. 7.
Till that moment we finished from attitudes state estimator and tested it wired and wireless with excellent performance.The next step is to implement the roll tracker and pitch tracker.

System Analysis 6.1 Processor in Loop (PIL) Simulation for SUAV
Depending on software simulation alone is not sufficient in the evaluation of the real system behavior, but is a basic step in the design.So the need for evaluation of the autopilot is completed by adding PIL simulation.One way to underestimate the gap between simulated and real systems which is the PIL simulation, it is used in many industries, e.g. automotive industry, in marine and aircraft industry to test autopilots, so it will be used to complete the design of the autopilot.This step increases the realism of the simulation, and provides access to the hardware features currently not available in software-only simulation models.It reduces the risks of discovering an error in the very last stage of on-the-field testing [23].PIL tests and simulation for various components and control systems of SUAV can facilitate the design of an efficient autopilot passing on all manufacturing stages.It can greatly increase safety, enhance quality, save time and save money [24].
The I/O signals between PC and hardware controller can be conditioned and adapted to match the connectivity between them.PIL testing step is the last step of the SUAV autopilot, it focuses on one component (controller) rather than the entire system.The rest of the system is simulated by computers (airframe model) which use real time data acquisition systems to read inputs and respond like the real system (real SUAV model).
The controller under test is implemented in the controller hardware (SUAV Flight Computer) and the simulator has to run in real time, the simulation time develops as real time.This real time simulation is obtained by setting the simulation algorithm cycle time equal to the simulation time step (sampling time in Ultrastick-25e autopilot is chosen to be 0.02 sec).
As shown in Fig. 8

PID Controller Implementation Results
The inner loops and outer loops of the autopilot with its longitudinal and lateral motion controllers were designed by using various control strategies and through many schedule procedures beginning from root locus technique to get the feedback gain during design of the dampers (most inner loops).Then designing a PID controller depending on the theory of Routh -Hurwitz method, then the final stage is the optimal tuning to make the rise time, settling time, steady state error, and maximum overshoot smaller than the compared one [25].The proposed autopilot evaluated in linear and nonlinear models.We can summarize the controllers' parameters of the whole autopilot in Tab. 2.
PID algorithm is implemented to make the inner loops (pitch, roll), and outer loops (yaw, height).For example, of results; Figure 9 shows the results +5 o response of the pitch attitude hold a controller after implementation in flight computer, the comparison between the proposed controller [3], [4] and the Minnesota [25].As seen the proposed one continued to be better than the Minnesota as we expected.

Conclusion
The objective of this paper is to implement an AFCS for SUAV.During the sections, we construct the block diagram of the autopilot.Flight computer (ArduMega 2560) is chosen because of its historic successfulness in autopilot design and its simplicity, this paper concludes an advantage by using MEMS sensors such as IMU (MPU6050), Magnetometer (HMC 5883L), GPS (UBLOX lea-6h) for implementing AHRS and finally AFCS.With combining two state estimator techniques for measuring attitude of Ultrastick-25e such as complementary and KFs; the problem of low cost MEMS sensors are finished and solved, also the high speed processing and accurate results are achieved.But we have a disadvantage of our AFCS which is the case relevance to Small UAVs only, due to their low speeds (low speed means low Reynolds number which leads to neglecting of its effectiveness during mathematical modeling stage) and some features belong to Small UAVs only.
The communication link between autopilot and the ground control station was achieved by using 3DR radio telemetry module.PIL simulation is implemented to evaluate the pitch attitude controller by converting the continuous system to a discrete one and implementing the communication adaptation between MATLAB and flight computer.By finishing the PIL the complete design of AFCS is finished, so we can consider this stage of design as a preflight test, nothing more for flying Ultrastick-25e.The results show us that the proposed controller strategy design is very good and convenient to our requirements.
The future work is to adapt our AFCS to be compatible with many types of UAVs (high speed, variable mass, heavy loads, etc.).

Fig. 3 .
Fig. 3. Orientation of axes descriptions of the device and mathematical calculations.

Fig. 4 .
Fig. 4. Pitch angle comparison according to various state estimator techniques.

Fig. 5 .
Fig. 5. Roll angle comparison according to various state estimator techniques.

Fig. 8 .
Fig. 8. PIL design structure.(a) Components of a PIL simulation.(b) Block diagram of a PIL simulation.