A low-cost integrated navigation system of quadrotor aerial vehicle: design, development and performance Zebo Zhou1, Jianfeng Zhang1, 2, Yong Zeng1, Jinping Bai1, 2, Ling Yang3 School of Aeronautics and Astronautics, University of Electronic Science Technology of China, Chengdu, P.R. China (e-mail:
[email protected]) 2 Laboratory of Aerial Robotics, University of Electronic Science Technology of China, Chengdu, P.R. China 3 Surveying and Geospatial Engineering, School of Civil and Environmental Engineering, University of New South Wales, Sydney 2052, Australia 1
received her Bachelor (2008) and Master (2010) Degrees in Geodesy and Surveying Engineering at Tongji University, China. Her research topic is focused on reliability and separability analysis of fault detection and isolation theory and its application for GPS/Locata/INS integrated navigation system.
BIOGRAPHY Dr. Zebo Zhou is currently a lecturer in the School of Aeronautics and Astronautics, University of Electronic Science and Technology of China (UESTC). He received his B.Sc. and M.Sc. degrees in the School of Geodesy and Geomatics, Wuhan University, China in 2004 and 2006. He obtained the Ph.D. in the School of Surveying and Geo-informatics Engineering, Tongji University, China in 2009. Zebo has received several awards from academic organizations in China and his research interests include GNSS navigation and positioning, GNSS/INS integration, multi-sensor fusion, and surveying data processing theory.
ABSTRACT This paper proposes a low-cost multi-sensor integrated navigation system of QAV, emphasis on the design, development and its performance. Firstly, the overview of QAV and its platform of integrated navigation system is established and detailedly discussed. It consists of heterogeneous sensors including GPS receiver unit, 3-axis gyroscope/accelerometer, 3-axis digital magnetic compass (MC), digital barometric altimeter, navigation computer unit (NCU) and the STM32 micro-processer. The data from different sensor modules has been well timesynchronized by pulse-per-second (PPS) signal from GPS. An efficient integration algorithm is proposed and developed with dual filters architecture to fuse the navigation data, which involves three stages: (i) to weaken the noises of sensors, the position/velocity (PV) from GPS are pre-processed with 1st Kalman filtering (KF) and the outputs of gyroscope/accelerometer are prefiltered with a five-order finite impulse response low-pass filter. (ii) A 15-state error state model is established in the navigation frame. In order to improve fusion accuracy, both the heading and height information from magnetic compass and barometric altimeter are combined with the PV solutions of 1st KF to generate the observation model of 2nd centralized KF. (iii) The final integration solution is reversely used as the feedback for position, velocity and attitude (PVA) outputs from gyroscope/accelerometer. Finally, the real flight experiment is carried out to evaluate the efficiency and performance of our proposed low-cost integrated navigation system of QAV. The result indicates that the navigation system performs well in real
Jianfeng Zhang is currently a postgraduate in the School of Aeronautic & Astronautics and Laboratory of Aerial Robotics, UESTC. He received his B.Sc. degree in the School of Automation, UESTC, in 2011. His research interests include low cost GPS/SINS integration and digital signal processing. Yong Zeng is currently an algorithm engineer in DJIInnovations Technology Co., Ltd.. He received his B.Sc. in the School of Automation, UESTC in 2010 and M.Sc. in the School of Aeronautic & Astronautics, UESTC in 2013. His interests include development of high performance, reliable, and easy-to-use unmanned aerial systems. Jinping Bai is currently an associate professor in the School of Aeronautic & Astronautics, UESTC. He is also a senior software engineer and an expert of Chinese Association for Quality. His current research interests include assessment of systems dependability, integration of intelligent system, and pattern recognition. Ling Yang is currently a PhD candidate at the School of Civil and Environmental Engineering, the University of New South Wales (UNSW), Sydney, Australia. She
!"
#$%!& !"
2114
when establish a well-designed integrated navigation system.
time and significantly improves the flight accuracy and reliability. With the aid of our low-cost navigation platform, the navigation results coincides with the real flight very well and can be implemented in many potential application fields.
Kalman filtering (KF) is extensively used for carrying out the solutions in navigation calculations. The operation of KF relies on the proper definition of a stochastic model besides a functional model (Brown and Hwang 1997; Zhou et al. 2010). Unfortunately, this is difficult in integrated navigation environment, e.g. insufficient observation, time-variant noise, maneuverings, thus the un-modeled errors may cause the suboptimum even divergence of filter. A great of researches have been investigated to solve the problems with adaptive and robust KF techniques (Yang et al. 2001; Yang and Xu 2003; Geng and Wang 2008), which are developed to estimate the optimal adaptive factors by innovations estimation to make the filter residuals covariance consistent with their theoretical covariance estimated with present information and residuals. However, these methods are complicated and time-consuming in computation for the low-cost and compact integrated navigation system.
1 INTRODUCTION The requirement of small aerial vehicles (SAV) for civilian and military purposes is growing in the past few decades (Beard et al. 2005; Harmon et al. 2005, 2006; Farid et al. 2009). Recently, as a certain kind of SAV, the quadrotor aerial vehicles (QAV) have been widely applied in a variety of fields, such as reconnaissance, surveillance and rescue etc. It can provide high degrees of manoeuvrability and is capable of efficiently performing tasks that would be dangerous for humans. Integrated navigation system is essential to a QAV. Its basic task is to provide the precise and timely state information for ensuring the security and reliability of flight. As the most important part of a integrated navigation system, Global positioning system (GPS) provides real-time precise position and velocity suitable for most land, marine, and aircraft navigation applications (Leick 1995). However, it still faces challenges in ensuring continuity of service in urban areas due to, e.g., blockages of GPS signals, low carrier-to-noise density and multipath which would limit GPS’s ability to deliver the required level of availability, accuracy and reliability (Kuusniemi et al. 2004). As a perfect complementary navigation system of GPS, inertial navigation systems (INS) have been extensively implemented with GPS for aerial vehicles navigation (Grewal and Andrews et al. 2001) to overcome the limitations of each navigation sensor thus improve the overall system performance. The most attractive advantage of integrated GPS/INS is the complementation with respect to error characteristics, i.e. INS has a small error in short term but its error grows significantly with time, while GPS has a bounded error (Titterton and Weston 2004). In the early periods, INS is expensive and bulky, which is the major barrier of applying in SAV. Recently, the emergence of the micro electro mechanical systems (MEMS) technology brings the development of compact, light weight and inexpensive MEMS-INS (El-Sheimy et al. 2008; Zhou et al. 2009). It is believed that MEMS-INS would eventually replace the conventional INS sensors in low-cost applications. To assure the self-alignment, attitude observability as well as vertical quantity accuracy, other auxiliary sensors such as magnetic compass, barometric altimeter can be utilized to aid GPS/INS navigation (Hou and El-Sheimy 2003; Godha et al. 2005). However, the critical issue of integrating these MEMS sensors is to ensure the security and stability of QAV and to meet the requirement of application fields with the limited sensor performance
!"
#$%!& !"
This paper addresses the design and development of an integrated navigation system for a QAV based on GPS receiver unit, three-axis gyroscope and three-axis accelerometer, digital magnetic compass, digital barometric altimeter, navigation computer unit (NCU) and the STM32 micro-processer. The basic task of integrated navigation system is to provide the control system with the accurate position, velocity and attitude (PVA) state. The challenge is the poor quality of the very low-cost navigation system (price in total about $60). With the implementations of Kalman filtering and lowpass filtering, an efficient integration algorithm is proposed to improve the accuracy and reliability of the navigation system. The paper is organized as follows: an overview of QAV and its integrated navigation system is presented in Section 2. In Section 3, the hardware architecture of navigation system is presented and discussed in details. In Section 4, an efficient integration algorithm is proposed and developed in three stages for multi-sensor fusion. Two scenarios of real flight experiments are carried out to evaluate the performance of integrated navigation system in Section 5. Finally, concluding remarks are given in Section 6. 2 OVERVIEW OF THE DESIGNED QUADROTOR AERIAL VEHICLE This paper focuses on the QAV as presented in Fig. 1. The symmetry and rigidity structure basically comprises of four rotors attached at the ends of a symmetric cross. It is a multivariate, strongly coupled and under-actuated system with six degree-of-freedom, high nonlinearity. It has a take-off weight below 1.6 kg including the payload of 200 g. The flight time limit is about 25 min. The
2115
communication distance does not exceed 1 km. The movements of QAV are driven and controlled by the four rotors. All rotors are designed with fixed-pitch mechanics. To avoid the unstable flight, the center of its gravity should be kept as close to the middle of QAV.
3 THE ARCHITECTURE NAVIGATION SYSTEM
OF
INTEGRATED
The hardware system consists of several electronics module (see Fig. 3), i.e. GPS receiver, three-axis gyroscope, three-axis linear accelerometer, barometric altimeter, magnetic compass, and navigation computation unit (NCU).
Fig.1 The mechanical physic structure of QAV
The rotating directions of these rotors are shown in Fig. 2. Two opposite rotors are rotating clockwise, while the remaining two rotors are rotating counter-clockwise. Therefore, the torques acting on the b-frame is cancelled, if the rotors are running at identical speeds. In order to start a vertical movement, the torques of each opposite couple rotors are zeros to keep balance, then the lift force driven by changing speed of rotors can make the QAV move in vertical direction. Particularly, the lift force is equal to the weight of QAV, hovering can be realized. In order to start a translational movement in forward direction (X), the speed of the front rotors (Motors 1 and 2) is decreased and the speed of the rear rotors (Motors 3 and 4) is increased, meanwhile keep the toques of the two opposite coupled rotors balanced, which produces a positive pitch angle and leads to thrust vector in forward direction, vice versa. Similarly, the translational movement in Y direction can be achieved by producing a positive or negative roll angle. As a result, the horizontal movement (X-Y) of QAV can be manipulated. For attitude control, the pitch and roll angles are varying with horizontal movement, while the yaw angle is produced when the torques of each opposite couple rotors are not cancelled, i.e. increasing the speed of the rotors rotating clockwise while decreasing the speed of rotors rotating counter-clockwise, or vice versa.
Fig. 3 Top view of integrated navigation system hardware
The schematic diagrams of STM32 and DSP circuits are shown in Fig. 4. The STM32 uses external clock with 8 MHz to generate 72 MHz through inner frequency division. BOOT0 of STM32 is set as low level to make the program start from flash memory. The universal synchronous/asynchronous receiver/transmitter (USART) ports in STM32 are designed to communicate with flight control system (FCS) and DSP respectively. 10 degree-offreedom (DOF) of autonomous navigation sensors including gyroscope, accelerometer, magnetic compass and barometric altimeter are mounted through interintegrated circuit (IIC) bus. The key navigation data is stored with a secure digital SD memory card through serial peripheral interface (SPI) bus. DSP works at the frequency of 150 MHz by dividing frequency from 30 MHz of external clock. When initialize the DSP, the pins of general purpose input output (GPIO) 84~87/XA12~15 are configured as 1 and then the system starts from flash. Meanwhile, GPIO84~87/XA12~15 is controlled by a pull-down resistor to access the full address of external memory. The serial communication interfaces is set as USART to communicate with GPS and STM32. The pulse-per-second (PPS) signal from GPS connects to the DSP external interruption for timesynchronization.
Fig. 2 Free-body diagram of QAV
!"
#$%!& !"
2116
5V
STM32F103VET6 LT1763-3.3
GPS
USART3
DSP TMS320F28335
BOOT0
3.3V GND
10K
PPS
GND 3.3V
JTAG ANS ADXL345 L3G4200D BMP085 HMC5883L
SDA SCL
JNTRST JTDI JTMS JTCK NRST JTDO
IIC-SDA IIC-SCL
Baudrate USART1: 115200bps USART2: 115200bps USART3: 9600bps
USART1
3.3V GND
TRST TDI TMS TCLK RESET TDO
USART2
STM32
Flight Control Board
20pF SD Card Memory
OSC_IN
Sample Rate Gyro: 20Hz Acce: 20Hz Magn: 20Hz Baro: 5Hz
8MHz 1
OSC_OUT
20pF GND
1
X2
GND LM1117-3.3
VDDIO GND
TRST TMS TDI GND TDO TCLK EMU0 EMU1 IS61LV25616AL
LM1117-1.8
1.8V GND
GND
NTRST TMS TDI TDO TCK EMU0 EMU1
33pF
3.3V GND
DATAOUT DATAIN PPS JTAG
MAG
ACC
GYRO
Fig. 6 Hardware architecture of integrated navigation system GND
GPS receiver module
3.3V
GPIO84 GPIO85 GPIO86 GPIO87
XD XA XWE0 XRD XZCS7
DATA ADDRESS WE RD CS
As a low-cost, low-power consuming GPS receiver apparatus, the ublox neo-6M integrating with active antenna is used in this development of hardware system. This module with the size of 28mm× 28mm × 10mm is representative of mass-market receivers and is of considerable benefit in various navigation campaigns especially in navigation environment and designed for meter level (3D) positioning accuracies in accord with the GPS standard positioning service.
GND GND
3.3K 3.3K 3.3K 3.3K
GND
Fig. 4 The schematic diagrams of STM32 and DSP
The schematic diagram of main modules in QAV is shown in Fig. 5. STM32 communicates with both FCS and DSP through USART. Fly Control System
BARO GND
3.3V
VSS GND
1K 1K 1K 1K
IIC
2
SCIB-RX SCIB-TX XINT
30MHz
IIC
3.3V
X1
5V
BUS
3.3V
Ublox NEO-6M
TMS320F28335
IIC
33pF
IIC
SPI-NSS SPI-MISO SPI-MOSI SPI-SCLK
2
/CS DO CMD SCLK
Simulated IIC
GND
Accelerometer module
TMS320F28335
STM32F103VET6
USART-TX
USART2-RX
USART3-RX
SCIC-TX
USART-RX
USART2-TX
USART3-TX
SCIC-RX
The ADXL345 with the size of 3 mm × 5 mm ×1 mm is a small, low power, three-axis accelerometer with high resolution measurement up to ±16 g. Digital output data is formatted as 16-bit twos complement and is accessible through IIC digital interface. The ADXL345 measures the static acceleration of gravity in tilt-sensing applications, as well as dynamic acceleration resulting from motion or shock.
Fig. 5 The schematic diagram of main modules in QAV
3.1 Hardware and communication In this subsection, the navigation hardware platform of integrated navigation system is established and detailedly discussed. It consists of heterogeneous sensors including GPS receiver unit, three-axis gyroscope and three-axis accelerometer, digital magnetic compass, digital barometric altimeter, NCU and STM32 micro-processer which is a family of microcontroller integrated circuits based on the 32-bit reduced instruction set computing (RISC) ARM Cortex cores. With the GPIO simulated IIC bus, the STM32 periodically samples data and sends it to the NCU through serial ports. NCU uses TMS320F28335 digital signal controller, which is able to operating at 150 MHz and has 32-bit floating point unit on chip. Three serial port modules are used to receive the upload data from IMU, GPS and FCS respectively. Figure 6 shows the hardware modules and the implementation of data communication. It should be announced that the STM32 is used to sample data and the DSP is mainly used for Kalman filtering computation.
!"
#$%!& !"
Gyroscope module The gyroscope module L3G4200D with the size of 4 mm × 5 mm × 1.1 mm is a low-power three-axis sensor able to provide a good stability and sensitivity of angular rate. It includes a sensing element and an integrated circuit interface capable of providing the angular rate through IIC interface. Aimed at temperature drift phenomenon of gyroscope, it contains a self-complementary module automatically compensating angular rate measurements. Magnetic compass module The HMC5883L with the size of 3 mm × 3 mm × 0.9 mm includes magneto-resistive sensors plus ASIC containing amplification, automatic degaussing strap drivers, offset cancellation, 12-bit ADC that enables 1° to 2° compass heading accuracy. It is designed for low field magnetic
2117
sensing with an IIC serial bus and provides heading information by measuring the intensity of a local external magnetic field for navigation applications.
4.1 Kalman filter and low-pass filter As the extensively applied methods in various navigation fields, both Kalman filtering and low-pass filtering methods are briefly presented as follows. Kalman filtering (KF) is recursively implemented with following equations, xk Φk , k 1 xˆ k 1 (1)
Barometric altimeter module The BMP085 digital pressure sensor with the size of 5 mm × 5 mm × 1.2 mm is used to measure barometric pressure and translate it into an output of voltage. It relies on atmospheric pressure readings, provides an indirect measure of height above a nominal sea level, typically to the accuracy of much less than 0.1 m and the long term stability can be achieved as well.
Σ xk Φk , k 1 Σ xˆ k 1 ΦkT, k 1 ΣWk
K k Σ xk H kT H k Σ xk H kT Σ k
(2)
1
(3)
xˆ k xk K k zk H k xk
(4)
Σ xˆ k I K k H k Σ xk
(5)
where xk and Σ xk denote the predicted state and its
3.2 Time-synchronization strategy
covariance respectively at epoch k; K k is the gain matrix; xˆ k and Σ xˆ k are the KF estimator and its covariance
Time synchronization is a primary prerequisite in the development of integrated navigation system (Knight, 1992). In this implementation of time-synchronization, the PPS signal output from GPS receiver unit is used as the time reference input to INS, magnetic compass, and barometric altimeter (see Fig. 7). The GPS receiver unit is connected to an interrupt port on the DSP module. When it is on the positive-edge of PPS, the inner clock of DSP will be reset. The navigation data from other sensors units sampled by STM32 module are continuously sent to DSP via RS232. When the next PPS arrives, the inner clock is reset to zero and immediately auto-runs again. Thus the PPS is directly utilized to synchronize the inner clock, which controls A/D sampling of other sensors units. As a result, the converted digital data are time-synchronized with the GPS data.
matrix. A five-order finite impulse response (FIR) filter passes low-frequency signals but attenuates (reduces the amplitude of) signals with frequencies higher than the cutoff frequency.
m(n)
m(n) Fig 8. The architecture of five-order FIR filter
GYRO ACC
RS232
As shown in Fig. 8, the Hamming-window based, linearphrase FIR filter can be expressed as a transfer function in z-domain,
PPS DSP
GPS
MAG
5
H ( z ) = å hk z - k
where hk is the impulse response and is determined according to the cutoff frequency; m denotes the time series. Then the n-th filtered data m¢(n) can be computed by,
∆t time- tag t2=t1+∆t
(6)
k =0
BARO
5
t1
m¢(n) = å hk m(n - k )
4.2 Computation strategy In order to improve the accuracy and enhance the reliability of navigation solutions, an efficient integrated navigation computation strategy is proposed as shown in Fig. 9, in which three stages, i.e. pre-filtering, fusion and correction are mainly involved and presented in the following content.
4 INTEGRATED NAVIGATION ESTIMATION With the implementations of Kalman filtering and lowpass filtering, an efficient integration algorithm is proposed in this section. Its main principle is, firstly use pre-filters to enhance the accuracy and reliability of outputs from GPS, magnetic compass and barometric altimeter sensors, and then provides the real-time corrections of the gyroscope/accelerometer sensor errors as well as the PVA navigation parameters.
!"
#$%!& !"
(7)
k =0
Fig. 7 Time-synchronization method of integrated navigation system
2118
, L, h, VE , VN , VU , , , w ,w ,w b X
b Y
b Z
Σ 1 Φ1 ( ) Σω1 Φ1 ( ) d tk
k
INS , LINS , hINS , VEINS , VNINS , VUINS , INS , INS , INS
f Xb , f Yb , f Zb
hINS
hBAR , VUBAR
hINS
MAG
VEINS VN INS
, L, h
VU INS
, L, h
VE , VN , VU
VE , VN , VU
where z1 GPS
hGPS VEGPS VNGPS VUGPS
T
;
Σ k1 Σ GPS . Then the filtered solutions xˆ 1k and its covariance Σ xˆ1 are trivially estimated in the 1st KF with
Fig. 9 The computation flowchart of integrated navigation system
k
equations (1) ~ (5).
Stage 1: pre-filtering
(ii) Low-pass filter for gyroscope/accelerometer Here first we use the discrete Fourier transform (DFT) to obtain the spectrum amplitude of frequency, GYRO f DFT mGYRO n 2 N 1 j fn N m ( n ) e , f 0, 1, 2, ..., N -1 GYRO n 0 (12) ACC f DFT mACC n 2 N 1 j fn mACC ( n)e N , f 0, 1, 2, ..., N -1 n0 where M denotes the spectrum amplitude of frequency f. Secondly, the empirical frequency of fT,GYRO and fT,ACC for gyroscope/accelerometer are chosen as the truncated frequency of low-pass filter based on the results transformed from (12).
(i) Kalman filtering for GPS PV outputs If number of satellites in view is small or less than four, the reliability of GPS positioning will decrease, even fail to obtain solutions. Moreover, poor GPS positioning solution which results in the large deviations from real trajectory will greatly degrade the fusion solutions. To overcome these problems, the dynamic model is employed to enhance the GPS model strength through Kalman filtering. The dynamic model which describes the low dynamics of QAV is constructed as, x 1 F 1 x1 1 (8) where the superscript “1” denotes the 1st Kalman filtering
x1
LGPS
H k1 is a six-dimensional identity matrix; ε1k ~ 0, Σ k1 and
, L, h, VE , VN , VU
index;
tk 1
t 3 Σv SS t 2 Σv S (10) 3 2 2 t Σv S tΣv 2 The GPS observation model of epoch k is formed as, z1k H k1 x1k ε1k (11)
L h VE VN VU
INS VUINS INS LINS
T
L h VE VN VU
T
;
S 0 with F 1 33 0 0 33 33 1 0 0 ( R h) cos L E 1 S 0 0 where RE and RN are RN h 0 0 1 the sagittal and meridional radii of curvature; h is the height above the ellipsoid; the variance of continuous 033 033 noise Σω1 and Σ v is the variance of GPS 033 Σ v velocity. (8) can be written in discrete-time form as x1k Φk1 , k 1 x1k 1 k1 (9)
Stage 2: centralized Kalman filtering Error states vector with fifteen dimensions are involved in the centralized Kalman filter, i.e. position, velocity and attitude (PVA), gyro drift and accelerator bias in three components respectively. For convenience in the latter calculation, the navigation-frame (n-frame) is chosen and symbolized as ‘n’ for the superscript in the following content. Then the state error differential equations are established as15-dimension state parameters are involved in Kalman filter, x 2 F 2 x 2 G 2 ω2 (13) where x L h VE VN VU rT θT vT
where Φk1, k 1 I F 1t and Δt denotes GPS sampling interval; k1 has the zero-mean with its variance,
including three-dimensional position, velocity, attitude, gyroscope drift and accelerometer zero-bias respectively;
!"
#$%!& !"
2119
T
gX g Y g Z aX a Y a Z gT aT
Frr Fvr Fθr F2 033 033
Frv
033
033
n
033
Fvv
f
Fθv
Ω
033
033
033
033
n in
0 VE sin L Frr 2 RE h cos L 0
C 033 ; 033 1 I 33 Ta 033
033 033 G C bn I 33 0 33
n b
C 1 I 33 Tg n b
033 VN
RE h VE ; 0 2 RE h cos L 0 0
0
RE h VE ; 0 2 RE h VE tan L 0 2 RE h VN
2
where the transition matrix Φk2, k 1 I1515 F 2 t ; the variance of process noise is tk
k
T
tk 1
(15)
nd
The observation model in the 2 Kalman filter can be expressed as, (16) zk2 H k2 xk2 εk2
!"
#$%!& !"
0 0
VU VN tan L VE 2 RE h
VE RE h V N ; RN h 0
2 we cos L
0 1 Fθv RE h tan L RE h
1 RN h 0 0
0 0 ; 0
zGPS-INS where the measurement vector zk zBAR-INS consists z MAG-INS k h hINS of, zBAR-INS BAR ; zMAG-INS MAG INS ; VUBAR VU INS
The discrete-time state model of (13) can be written with first-order linearization, xk2 Φk2, k 1 xk21 k2 (14)
Σ 2 Φ 2 ( )GΣ ω2 G T Φ 2 ( ) d
0 0 ; 1
1 RN h
VUVN VNVE tan L 0 2 2 ; RN h RE h 2 g0 VN2 VE2 0 2 2 RE h RN h R h
0
VN tan L VU V tan L 2 we sin L E R h RE h E 2VE tan L VU 2 we sin L Fvv RN h RE h 2V 2VN E 2 we cos L RN h RE h 0
ω ω2 g ; ωa
0 1 Frv ( RE h) cos L 0
2
VEVN 2VU we sin L 2VN cos L RE h cos 2 L VE2 Fvr 2VE cos L RE h cos2 L 2VE we sin L
0 we sin L Fθr VE we cos L RE h cos 2 L
033 C bn 033 ; 033 I 33
2120
zGPS-INS
LGPS LINS GPS INS h h GPS INS ; VE GPS VE INS V VN INS N GPS VU GPS VU INS
comparison purpose. Two scenarios are designed to evaluate the performances of pre-filtering and centralized fusion filtering respectively.
Experiment setup Before take-off, the power on self-test is executed to initialize the navigation hardware system, i.e. STM32 and DSP including USART, IIC, and SPI etc. After initialization, all data from navigation sensors are wellsynchronized and 200 groups of data from gyroscope/accelerometer are continuously collected to estimate the zero-biases online. With the zero-bias correction, the static initial alignment has been calculated with the aid of data from accelerometer and magnetic compass till the strapdown matrix converges. Once the alignment completed, FCS starts waiting the navigation system signals. The attitude and throttle remote control biases signals from FCS are also calculated before flight. The stochastic models of these sensors are provided by manufactures as mentioned in Section 3. One thing should be noted that the magnetic declination angle in Chengdu is empirically set as e 116 ' . Since all sensors are tightly installed in the QAV and the centers of GPS, magnetic compass and barometric altimeter are very close to the center of gyroscope/accelerometer, the level arm effects can be completely neglected here.
069 I 6 6 vec s3 0112 Accordingly, the design matrix H k2 vec s6 019 vec s9 016
with vec si 0 0 ... 1
ith
.
Neglecting the cross-
correlation of these sensors, the variance of observation noise ε 2 is 011 Σ xˆ1 012 (17) Σ 021 Σ BAR 021 0 0 Σ 11 MAG 11 where Σ BAR and Σ MAG are the variances of barometric altimeter and magnetic compass respectively.
Stage 3: Navigation correction and feedback With the error state estimated by the 2nd KF, the navigation solution can be corrected by, xk xk3 Bxk2 (18) where xk3
r
T
k
INS
vk INS θk INS T
T
Scenario 1: The pre-filtering validation A simple test is carried out to demonstrate the efficiency of 1st KF for GPS module at the squared near the library building of UESTC. Satellites were well tracked and observed in the whole test. Since the reference trajectory cannot be obtained with other sensors, the real circle route is designed as a smoothed trajectory approximate as a circle on purpose. Measured points with GPS are expressed as triangles in Fig. 10. The filtered trajectory solved with the 1st KF, is drawn with red line in Fig. 10. Evidently, some abrupt position measurements exist in this test thus lead to significant deviations from the nearcircular trajectory. On the contrary, due to the contribution of the GPS velocity contained dynamic model, the filtered solutions are smoother and more reliable than measured positions.
T
are computed
according to gyroscope/accelerometer mechanization in 096 I n-frame; B 99 and xk is the final PVA 0 0 66 69 solution at epoch k. Here the raw measurements of gyroscope/accelerometer are not corrected in real time. Because other sensors are low-cost and is probable to wrongly estimate the inertial sensor errors, thus conversely degrades the accuracy of raw measurements. In addition, the PV solutions of final navigation output will be used as the feedback of prior PV of the dynamic model in the 1st KF.
5 EXPERIMENT AND ANALYSIS In this section, to demonstrate the efficiency of our developed integrated navigation system, the real flight experiment is carried out on a square near the library on campus of UESTC. The weather is good and wind is moderate, which is very suitable for the flight since QAV is sensitive to wind condition. The experimental data is collected and stored in the SD memory on integrated navigation system module. All the navigation results are computed in the post-processing mode for latter
!"
#$%!& !"
Fig. 10 The GPS measured and filtered 2D-trajectory
2121
very low-cost QAV, the ‘true’ values cannot be obtained. Therefore, we evaluate the attitude accuracy indirectly by trajectory and variance statistics. The horizontal and the vertical trajectories are shown in Fig. 13. Evidently, the integrated navigation method (red line) performs quite well and the navigation solutions are consistent with that of GPS (circle points). In vertical direction, the starting and ending points are also highlighted in this figure, from which It can be seen that in the whole fight test, due to the aid of barometric altimeter, the height accuracy of integrated navigation system is significantly improved and generally superior to the GPS height. More persuasive results can be found in the beginning and ending periods (see the ellipse areas in Fig 13 on the right side), the integrated navigation system achieve the more stable and reliable height and coincides with the real flight very well.
We also develop the low-pass filters for gyroscope/accelerometer. The accuracy of accelerometer output is mainly influenced by random walk and highfrequency noises driven by vibration of QAV. This can be well-solved by a low-pass filter. Different from the case of accelerometer, dealing with the gyroscope faces a paradox. To weaken the low-frequency drift error of gyroscope, the high-pass filter has been commonly used in complementary filtering. On the other hand, the highfrequency noise driven by rotor wings of QAV should be processed with a low-pass filter. According to vibration test on gyroscope/accelerometer, it is indicated that the vibration greatly degrades the accuracy and reliability of accelerometer outputs, instead, has no significant effect on gyroscope performance. Therefore the five-order FIR filter is employed in the gyroscope/accelerometer modules with the fT,GYRO = 5 Hz and fT,ACC = 0.5 Hz respectively. The filtered results are clearly shown in Figs 11 ~ 12. In comparison with the un-filtered outputs, the pre-filer significantly reduces the high-frequency noise produced by vibration and flux, thus improves the accuracy of gyroscope/accelerometer outputs.
Fig. 11 The outputs of gyroscope (blue dash line – before filtered; red line - after filtered)
Fig. 12 The outputs of accelerometer (blue dash line – before filtered; red line - after filtered)
Scenario 2: performances of integrated navigation system In order to comprehensively test and evaluate our developed navigation system, the typical flight statuses are intentionally experienced during the test, including take-off, hovering, vertical and lateral flying, landing. Since the accurate attitude and heading reference system (AHRS) is not affordable for SAV, especially for this
!"
#$%!& !"
Fig. 13 Comparisons of GPS and integrated navigation results (top: Latitude; middle: Longitude; bottom: height)
2122
The variance statistics of attitude angels, i.e. roll, pitch and heading are shown in Fig. 14. With the aid of magnetic compass, the heading angle converges fastest. All the variances of the estimated angles rapidly converge after a short period and keep stable during the flight. It is worthy to point out that the integrated navigation
solutions without pre-filtering stage are rapidly divergent due to the intensive of noise contained measurements from low-cost sensors and the vibrations produced by rotor-wings. For this reason, here we do not present these figures and results.
Fig. 14 The variance statistics of attitude angles (pitch: left; roll: middle; yaw: heading)
Brown R. and Hwang P. Y. C., Introduction to random signals and applied Kalman filtering, New York: John Wiley and Sons, 1997. Titterton D. H. and Weston J. L., Strapdown inertial navigation technology (Progress in Astronautics and Aeronautics Series), Second edition, American Institute of Aeronautics & Ast, 2004. El-Sheimy N., Hou H. and Niu X., Analysis and modeling of inertial sensors using Allan variance, IEEE Transactions on Instrumentation and Measurement, 2008, 57(1): 140-149. Farid K., Isabelle F. and Kenzo N., Optic flow-based vision system for autonomous 3D localization and control of small aerial vehicles, Robotics and Autonomous Systems, 2009, 57:(6-7): 591-602. Geng Y. and Wang J., Adaptive estimation of multiple fading factors in Kalman filter for navigation applications, GPS Solutions, 2008, 12(4): 273-279. Godha S., Petovello M. G., and Lachapelle G., Performance analysis of MEMS IMU/HSGPS/ magnetic sensor integrated system in urban canyons, In: Proceedings of ION GNSS 2005, Long Beach, CA, September 13-16, 2005, 1021-1029. Grewal M. S. and Andrews A. P., Kalman Filtering: Theory and Practice using Matlab, Second Edition, Wiley & Sons, 2001. Harmon F. G., Frank A. A. and Joshi S. S., The control of a parallel hybrid-electric propulsion system for a small unmanned aerial vehicle using a CMAC neural network, Neural Networks, 2005, 18(5-6): 772-780. Harmon F. G., Frank A. A. and Chattot J. J., Conceptual design and simulation of a small hybrid-electric unmanned aerial vehicle, Journal of Aircraft, 2006, 43(5): 1490-1498. Hou H and El-Sheimy N., Inertial sensors errors modeling using Allan variance, In: Proceedings of ION
6 CONCLUDING REMARKS In summary, a low-cost integrated navigation system of quadrotor aerial vehicle is designed and developed in details. For navigation estimation, a dedicated architecture with dual filters is proposed to fuse the data from navigation sensors with three steps: (i) Pre-filtering for GPS and gyroscope/accelerometer. (ii) 15-state based centralized KF. (iii) Navigation correction and feedback. Finally, the real flight experiment is carried out to evaluate the efficiency and performance of our proposed low-cost integrated navigation system of QAV. The result indicates that the developed navigation system performs well in real time and significantly improves the flight accuracy and reliability. With the aid of our low-cost navigation platform, the solved trajectory coincides with the real flight very well and can be implemented in many potential application fields.
ACKNOWLEDGMENTS This work is supported by the Fundamental Research Funds for the Central Universities (ZYGX2010J114) and partially sponsored by Natural Science Foundations of China (41304018).
REFERENCES Beard R. W., Kingston D., Quigley M., Snyder D., Christiansen R., Johnson W., McLain T. and Goodrich M., Autonomous vehicle technologies for small fixedwing UAVs, Journal of Aerospace Computing, Information, and Communication, 2005, 2(1): 92-108.
!"
#$%!& !"
2123
GPS/GNSS 2003. Portland: The US Institute of Navigation, 2003, 2860-2867. Knight D., Achieving modularity with tightly coupled GPS/INS, In: Proceedings of the IEEE PLANS 92, March 23-27, 1992, Monterey, CA, 426-432. Yang Y., He H. and Xu G., A new adaptively robust filtering for kinematic geodetic positioning, Journal of Geodesy, 2001, 75(2): 109-116. Yang Y. and Xu T., An adaptive Kalman Filter based on sage windowing weights and variance components, The Journal of Navigation, 2003, 56(2): 231-240. Zhou Z., Li Y., Rizos C. and Shen Y., A robust integration of GPS and MEMS-INS through trajectoryconstrained adaptive Kalman filtering, In: Proceedings of ION GNSS 2009, September 22-25, Savannah, GA, 2009, 995-1003. Zhou Z., Shen Y. and Li B., A windowing-recursive approach for GPS real-time kinematic positioning, GPS solutions, 2010, 14(4): 365-373. Zhou Z., Shen, Y. and Li B., Moving time-window based real-time estimation algorithm for the stochastic model of GPS/Doppler navigation, Acta Geodaetica et Cartographica Sinica, 2011, 40(2): 220-225.
!"
#$%!& !"
2124
