Xu Dongfu (徐東甫), Pei Xinbiao, Bai Yue, Peng Cheng, Wu Ziyi, Xu Zhijun

(*Changchun Institute of Optics, Fine Mechanics and Physics, Chinese Academy of Sciences, Changchun 130033, P.R.China) (**University of Chinese Academy of Sciences, Beijing 100039, P.R.China)

Altitude information fusion method and experiment for UAV①

Xu Dongfu (徐東甫)***, Pei Xinbiao***, Bai Yue②*, Peng Cheng*, Wu Ziyi***, Xu Zhijun*

(*Changchun Institute of Optics, Fine Mechanics and Physics, Chinese Academy of Sciences, Changchun 130033, P.R.China) (**University of Chinese Academy of Sciences, Beijing 100039, P.R.China)

Altitude regulation is a fundamental problem in UAV (unmanned aerial vehicles) control to ensure hovering and autonomous navigation performance. However, data from altitude sensors may be unstable by interference. A digital-filter-based improved adaptive Kalman method is proposed to improve accuracy and reliability of the altitude measurement information.A unique sensor data fusion structure is designed to make different sensors switch automatically in different environment. Simulation and experimental results show that an improved Sage-Husa adaptive extended Kalman filter (SHAEKF) is adopted in altitude data fusion which means that altitude error is limited to 1.5m in high altitude and 1.2m near the ground. This method is proved feasible and effective through hovering flight test and three-dimensional track flight experiment.

unmanned aerial vehicles (UAV), altitude information fusion, multi-sensor, adaptive Kalman filter

0 Introduction

In the last two decades, great efforts have been made for the research and development of multiple rotor UAVs driven by the significant progress of sensing and mechatronics technology[1-3]. This sort of aircrafts has shown its promising applications in both civic and military aspects. Compared with the traditional helicopters, they have the advantages of simplicity in mechanical construction, ease in modeling, control and maintenance[4-7].

The accuracy and uniformity are very important for UAVs. However, due to the small structure and low cost, UAVs commonly use low cost sensors, such as MEM’s based acceleration, barometer and GPS (global positioning system). Suffered from severe drift and noise, the obtained estimation is unfeasible in practice. The attempt on this regard has been made by many researchers, by means of applying Kalman filter based techniques.

Normally, Kalman filter is the acknowledged rule of technology for data fusion, which requires that measure noise and system noise are known[8]. However, when the Kalman filter is used in UAVs, measure noises and system noises are unknown and time-varied. The Kalman filter is no longer optimal and its convergence cannot be guaranteed.

The SHAKF (Sage-Husa adaptive Kalman filter) can solve the above problems, which estimates system noises and measure noises in theory adaptively[9,10]. When estimating the altitude in UAVs, the SHAKF could not separate the system noises and the measure noises accurately, let alone the high accuracy estimation variance values. There is a constant error between estimations of the system noises and the measure noises[11]. The variance of the system noises is deviated, which result in wrong judgment of estimation, such as, increasing weight of barometer in the altitude estimation, decreasing precision of altitude and causing misconvergence. It cannot meet the need of UAVs autonomous flight.

An improved Sage-Husa adaptive extended Kalman filtering (SHAEKF) is designed, aiming at the development of estimation for altitude. The improved SHAEKF uses the noise variance of MEMS accelerometer by real-time estimating the system noises variance of altitude[11], which not only reduces the estimation error of the system noises, but also guarantees the convergence. At the same time, united with the enhanced Kalman filter[12,13], the improved SHAEKF can inhibit the divergence of filter. Combined with the estimation structure based on the feature of different sensors, the sensor defects can be made up and accurate status information of the altitude can be obtained. At last, experiments were carried out to verify the feasibility and effectiveness of the method.

1 Altitude system structure

1.1 Brief introduction of UAV platform

The UAV used in this paper, shown in Fig.1, is an electric-powered multi-rotor UAV, which is named Hex-rotor[14].

Fig.1 UAV experimental platform

The UAV’s structure is shown in Fig.2. Six equal-length, long light rods are placed evenly around the center of the UAV. On the tip of each rod there are two coaxial rotors with driving units. In clockwise direction the upper six rotors are numbered 1～6, while the lower six are numbered 7～12. Among them, rotor No.1, 3, 5, 8, 10, 12 rotate clockwise, while rotor No.2, 4, 6, 7, 9, 11 rotate counterclockwise. The angle between the rotor’s shaft and the body plane isγ(0<γ<90°),andtwoadjacentrotor’sshaftpointsoppositedirection.ThegeographiccoordinatesystemandthebodycoordinatesystemarealsoshowninFig.2.

1.2 Altitude measurement unit

Altitude measurement unit consists of MEMS-based accelerometers, barometer, GPS module and laser module.

1.2.1 Accelerometer

Fig.2 Diagram of Hex-Rotor aircraft structure

(1)

where:

Positioninformationcanbesolvedbyintegrationofspeedva.

(2)

Fig.3 Altitude of MEMS ACC

1.2.2 Barometric altimeter

The air pressure decreases with the increasing of altitude. Based on this principle, altitude can be determined by measuring pressure with barometer. However, the decrease of altitude is not uniform. Under the standard atmospheric conditions, altimeter formula is expressed as

(3)

whereRisthegasconstant,gnistheaccelerationoffreefall.βistheverticalchangerateoftemperature,Ta,Paandhaaretheenvironment’satmospherictemperaturelowerlimit,atmosphericpressureandgeopotentialheightrespectively.Phistheatmosphericstaticpressuremeasuredundercurrentaltitude.Settingthebarometermeasuredaltitudeattake-offpositionasareference,andcomparingwithitscurrentvalue,thealtitudeofUAVwithrespecttothealtitudeofthetakingoffplanecanbeobtained,whichisthealtitudeinformationdesiredinnavigation.

Inactualmeasurement,theactualatmosphericconditionscannotmeettherequiredstandard;therefore,measurementerrormayoccur.Thismeasurementerrorincreaseswithreducedaltitude.WhenUAVisinflight,barometeroutputaccuracyismainlyaffectedbythehigh-frequencynoiseandconstanterrors.Theerrorcanbeexpressedas

hb=h0+εb+ωb

(4)

where,hbisthemeasuredvalueofthebarometer,h0istheidealaltitude.Constanterrorismainlyrelatedtotemperatureandpressure,andcanbecompensatedbyinitialcalibration.ThereadingsofaltimeterwhentheUAVishoveringareshowninFig.4,intheeffectoftherotor’sdownwash,winddisturbanceandbarometererror.Thereisalsoadriftsothisaltitudesensitivetooutsideinterferencecannotbeusedalone.

Fig.4 Altitude of barometer

1.2.3 GPS altitude measure

The GPS module receives data from three or more satellites whose coordinates are already known, then calculates the coordinates of the measuring points. The altitude obtained from GPS positioning by calculation and conversion can be expressed as

hg=h0+ωg

(5)

wherehgisthemeasuredGPSvalue,h0istheidealaltitude,ωgisthemeasuringnoise(whitenoise).

Fig.5 Altitude of GPS

Fig.5 is the GPS altitude of the UAV during hovering. The GPS module output is overall smooth with small volatile. However, the GPS output frequency is 1-10Hz, much smaller than the control frequency of the UAV which reaches 50Hz. So the GPS data cannot meet the requirements of the aircraft’s dynamic response. It can be also seen from Fig.5 that sometimes there is no output because the GPS module is blocked. Therefore, the GPS module cannot be used alone either.

1.2.4 Laser ranging module

The laser ranging module of the UAV uses 905nm, near-infrared laser with phase method. The measurement range is 0.1m～25m, while measurement accuracy reaches ±5mm at the frequency of 100Hz. The error of laser ranging module can be expressed as

hl=h0+ωl

(6)

wherehgisthelasermeasuringvalue，h0istheidealaltitude,ωgisthemeasuringnoise(whitenoise).Fig.6isthelasermoduleheightoftheUAVduringhovering.

AsseeninFig.6,thealtitudedataprovidedbylaserrangingmodulehashighprecisionwithtransition.Consideringitsmeasurementrangeof1m～25m,laserrangingmodulecanonlybeusedatlowaltitudes.

Fig.6 Altitude of laser module

2 Altitude data fusions

As seen from the data above，the error of altitude obtained from accelerometer accumulates over time because of the two integration; The barometer output gave a much wide measuring range due to its sensing mechanism with larger white noise. The GPS module not only has rather big fluctuation and noise with a constant bias, but also has no output when blocked; The laser module reading is rather accurate detection but only in a limited range of 0 to 30m above ground level. Therefore, based on the characteristics of each sensor, a process of SHAEKF is designed as shown in Fig.7.

Fig.7 Process of SHAEKF

The process of SHAEKF is divided into two parts: data analysis module and improved SHAEKF which improves the accuracy of altitude in different environment.

2.1 State and observation models of altitude

UAVs vertical movement can be described by linear mathematical model:

(7)

It can be said by equation of state：

(8)

(9)

Xk=ΦXk-1+Buk-1+ΓWk-1

(10)

where：

(11)

Xkis the state vector,Tsisthefusiontimecycle,Wk∈R1×1isthesystemnoise,itsvariancestatisticalpropertiesarecompletelydecidedbythestatisticalfeaturesofmodelinput,therefore,thesystemnoisedrivearray: Γ=B.

The measurement equation of discrete time state model is

Zk=HXk+Vk

(12)

where Zk∈R3×1is the measurement provided from the GPS module, the MEMS accelerometer, the laser module and the barometer. Vk∈R3×1is the measurement noise. H is the quantity measurement matrix.

Above all, the discrete time linear state model (state equation and measurement equation) of UAVS integration system is

(13)

The laser module is in a limited range of 0 to 25m above ground level, the laser module is used below 25m and the barometer is used above 25m in the process of SHAEKF, as shown in Eq.(14):

(14)

2.2 Data analysis module

There are many errors on the altitude status from different sensors before the Kalman filter. Fuzzy Kalman filter is used to calculate theoretical and actual variance of innovation. A data analysis is added to the process of data fusion to identify the outputs of sensors and determines weight coefficient.

The actual variance of innovation is calculated byNsamplingdata:

(15)

Thefusionaltitudeistreatedaspredictivevalue,andtheoutputistreatedasmeasurementvalue.vkisthedifferencevaluebetweenpredictivevalueandmeasurementvalue,namelyinnovation.Nisdecidedbythecharacteristicsofeachsensor.Thetheoreticalvarianceofinnovationisdefinedas

(16)

UsingfuzzyKalmantomatchvariance,avariantisdefendedtoinspectthedifferenceofpredictivevalueandmeasurementvalue:

(17)

Iftheoutputsarestable,theratiooftheoreticalandtheactualvarianceαCxiscloseto1,otherwiseαCxwillbecomebiggerwhentheoutputsmalfunction.InspectingthechangeofαCx,thefuzzyruleisagain:

(18)

Measuredbyexperiment,β=[βg,βb,βm,βl]isusedtoadjustweightcoefficient,whichcanadaptivelyadjusttheweightofeachsensor,ensuringtheaccuracyofestimationinsituationofhighdynamicindifferentenvironmentofUAVs.

2.3ImprovedSHAEKF

TheimprovedSHAEKFisgivenbythefollowingrecursiveequations:

①Firststepofprediction：

(19)

②Updateinnovation：

(20)

③Updatethepredictionsquareerrormatrix：

(21)

④Noiseofmeasurement：

(22)

⑤ Filter convergence criterion：

(23)

If Eq.(23) is satisfied, the filter is in convergence and keep the Pk/k-1in ③; Otherwise, update Pk/k-1by strong Kalman filter：

(24)

⑥ Update the filter gain：

(25)

State altitude estimation：

(26)

whereαistheweightcoefficient.

⑦Updatestateestimationsquareerrormatrix：

(27)

⑧Systemnoiseestimation：

The improved SHAEKF has advantages as follows:

(1) Estimates system noise and measurement noise

The top priority of the improved SHAEKF is separating the system noise and the measurement noise while their statistical properties are neither unknown. For altitude fusion system of UAVs, system noise is mainly derived from the integral of MEMS accelerometer error, noise parameters are relatively stable. Therefore, the variance of estimation system noise can be calculated in real-time by the altitude of the accelerometer. The improvement not only can solve the problem of big error in estimation of the system noise, but also keep the system stable.

(2) Exponential fading factor in Eq.(28)：

(28)

When the UAV is in a state of flight, system in the process of dynamic changes, noise parameter has a weak non-stationary. The fading memory factor is used to raise the innovation in state estimation.

(3) Combined with the strong tracking Kalman filter, based on the filter convergence criterion, the improved SHAEKF is combined with the strong tracking Kalman filter. If the criterion is established, the filter convergence keeps updating Pk/k-1to calculate filter gain, otherwise , the actual error of filter exceeds expected value in theory and Pk/k-1is updated by the strong tracking Kalman. Meanwhile the weight of each sensor is adaptively adjusted according to the weight coefficient, ensuring the accuracy of estimation in situation of high dynamic in different environment.

3 Simulation and experimental results

To validate the estimation of altitude fusion method, simulation and actual experimental were presented. Since UAV usually flies in short time, experiments are carried within 15 minutes. And the low and high altitudes are distinguished by the range of the laser ranging module -25m.

3.1 Simulation experiment

The two simulations are as follows: one simulation is in the height of 15m in the case of sudden failure of the GPS, the other simulation is in the height of 35m, simulation of more than 25m in which the fusion algorithm could adjust the weights of the sensors. The output data of each sensor is shown in Fig.8 and Fig.9.

Fig.8 Altitude data of sensors at 15m

Fig.9 Altitude data of sensors at 35m

Simulation results are shown in Fig.10 and Fig.11.

Fig.10 Result of Information Fusion at 15m

Fig.11 Result of information fusion at 35m

As shown in Fig.10, the lines denote the altitude of improved algorithm and the altitude of Kalman filter improved before. When the GPS broke down, the accuracy of Kalman filter reduced while the improved algorithm keeps the accuracy of fusion altitude. In Fig.11 the output of laser ranging module is 0, the fusion effect of the improved algorithm is better than the improvement before.

3.2 Multipoint hovering flight experiment

In order to further verify the effectiveness of the altitude in different altitudes and different environments, this section presents experimental results from the multipoint hovering flight. The hovering altitudes are 10m, 22m and 37m. The real time altitudes with no wind recorded within 1000s are shown in Fig.12. Fig.13 is the altitude with winds grade 3. The attitude and altitude control is satisfactory in the sense that the UAV is stable around the desired altitude.

Fig.12 Spot hovering test with no wind

Fig.13 Spot hovering test with wind grade 3

The error at low altitudeel,andtheerrorathighaltitudeehwithnowind:

Theerroratlowaltitudeel,andtheerrorathighaltitudeehwiththewindgrade3:

3.3 3-Dtrajectoriesexperiment

Forfurtherverifyingthereliabilityandhighdynamicofthealtitude,experimentalresultsarepresentedfromtherotorcraftUAVsautonomousflightinJilin,China.Accordingtotherequirementsoftask，flyinginthegroundspeedof4m/s,theUAVtracksthetrajectoryofatriangleatthealtitudefrom5mto25m.TheActualflighttrajectoryisrecordedasFig.14.

Fig.14 3D trajectory of the UAV

As can be learned from Fig.14, the UAV can accomplish the trajectory with the altitude error less than 1.5m. The experiment shows that the fusion method could meet the need of high dynamic.

4 Conclusions

The paper presents a data fusion approach to the problem of low altitude accuracy in UAVs.

Adopting a number of different altitude information, a unique data fusion structure is designed according to the characteristics of each sensor.

Based on the use of rotor aircraft environment, an improved SHAKF-Kalman filter data fusion method is presented. The algorithm can adaptively adjust using the weight of each sensor for ensuring the accuracy of estimation in situation of high dynamic in UAVs.

As mentioned above, the multi-sensor data fusion based on the improved SHAKF- Kalman filter is positive. The estimation of altitude reaches a precision of 1.5 meter. The algorithm is stable, and much more adaptable to engineering.

Reference

[ 1] Zoto V, Gao X G. Intermediate carriers for UAV swarms: problem of fleet composition.JournalofSystemsEngineering&Electronics, 2013, 24(1):101-107

[ 2] Qian M S, Jiang B, Xu D Z, et al. Robust dynamics surface fault tolerant control design for attitude control systems of UAV.SystemsEngineering&Electronics, 2014, 36(9):1798-1803

[ 3] Qu Y, Zhang Y. Cooperative localization against GPS signal loss in multiple UAVs flight.JournalofSystemsEngineeringandElectronics, 2011, 22(1): 103-112

[ 4] Grzonka S, Grisetti G, Burgard W. A fully autonomous indoor quadrotor.IEEETransactionsonRobotics, 2012, 28(1):90-100

[ 5] Sebesta K D, Boizot N. A real-time adaptive high-gain EKF, applied to a quadcopter inertial navigation system.IEEETransactionsonIndustrialElectronics, 2014, 61(1): 495-503

[ 6] Wei G, Li J. Adaptive Kalman filtering for the integrated SINS/DVL system.JournalofComputationalInformationSystems, 2013, 16(9):6443-6450

[ 7] Chingiz H, Halil E S. Robust adaptive kalman filter for estimation of UAV dynamics in the presence of sensor/actuator faults.AerospaceScienceandTechnology, 2013, 28(1): 376-383

[ 8] Sage A P, Husa G W. Adaptive filtering with unknown prior statist. In: Proceedings of the Joint Automatic Control Conference, Tokyo, Japan, 1969. 760-769

[ 9] Kownackl C. Optimization approach to adapt Kalman filters for the real-time application of accelerometer and gyroscope signals’ filtering.DigitalSignalProcessing, 2011, 21(1): 31-140

[10] Zhang C Y. Approach to adaptive filtering algorithm.ChineseJournalofAeronautics, 1998, 19(75): 596-599 (In Chinese)

[11] Rigatos G. Nonlinear Kalman filters and particle filters for integrated navigation of unmanned aerial vehicles.RoboticsandAutonomousSystems, 2012, 60(7): 978-995

[12] Hajiyev C, Soken H E. Robust adaptive Kalman filter for estimation of UAV dynamics in the presence of sensor/actuator faults.AerospaceScienceandTechnology, 2013, 28(1): 376-383

[13] Duan Z S, Han C A. A strong tracking adaptive state estimator and simulation.JournalofSystemSimulation, 2004, 16(5):1020-1023 (In Chinese)

[14] Liu R H, Wang H. All attitude magnetic deviation compensation for digital magnetic compass.OpticsandPrecisionEngineering, 2011, 19(8): 1867-1873 (In Chinese)

[15] Gao X X, Jiang R, Gao M M. Control scheme based on the inverse system method online learning BP neural network adaptive compensate. In: Proceedigns of the 2010 IEEE International Conference on Intelligent Computing and Intelligent Systems (ICIS), Xiamen, China, 2010. 874-878

Xu Dongfu， male， born in 1987. He is a Ph.D candidate of the University of Chinese Academy of Sciences, and he received his B.S. degree in Jilin University in 2011. His main research direction is unmanned aerial vehicles integrated navigation and control.

10.3772/j.issn.1006-6748.2017.02.007

①Supported by the National Natural Science Foundation of China (No. 61304017, 11372309), Key Technology Development Project of Jilin Province (No. 20150204074GX), the Project Development Plan of Science and Technology (No. 20150520111zh) and the Provincial Special Funds Project of Science and Technology Cooperation (No. 2014SYHZ0004).

②To whom correspondence should be addressed. E-mail: baiyueciomp@163.com

on Mar. 4， 2016