The drift of inertial navigation system (INS) will lead to large navigation error when a lowcost INS is used in microaerial vehicles (MAV). To overcome the above problem, an INS/optical flow/magnetometer integrated navigation scheme is proposed for GPSdenied environment in this paper. The scheme, which is based on extended Kalman filter, combines INS and optical flow information to estimate the velocity and position of MAV. The gyro, accelerator, and magnetometer information are fused together to estimate the MAV attitude when the MAV is at static state or uniformly moving state; and the gyro only is used to estimate the MAV attitude when the MAV is accelerating or decelerating. The MAV flight data is used to verify the proposed integrated navigation scheme, and the verification results show that the proposed scheme can effectively reduce the errors of navigation parameters and improve navigation precision.
Recently, the autonomous operating unmanned aerial vehicle (MAV) is becoming popular in military and civilian applications [
Due to the drawbacks of GPS and INS, lots of novel navigation devices have been studied. For example, optical flow sensor has been proved which can provide reliable velocity and position information when applied for robot localization or navigation [
In this paper, an optical flow sensor/INS/magnetometer integrated navigation system is proposed for MAV, where optical flow sensor is used to measure the velocity and position of the MAV, and the magnetometer is used to measure the attitude of the MAV; then, these navigation parameters are applied to calibrate the drift of INS by using an extend Kalman filter. This paper is organized as follows: Section
Optical flow is the projection of 3D relative motion onto a 2D image plane. In order to calculate the optical flow, the researchers have proposed many solutions, such as LucasKanade algorithm, HornSchunck algorithm, image interpolation algorithm, block matching algorithm, and feature matching algorithm. In our study, considering the complexity of the software calculation and hardware platform, the block matching algorithm (BMA) based on minimum mean absolute error (MMAE) and the sum of absolute differences (SAD) are selected to calculate the optical flow, which is shown in Figure
Block matching algorithm based on sum of absolute differences.
As shown in Figure
In the initial state, a target is selected at the original point of the imaging plane; the target block will move when the MAV moves. In the searching area of the current frame, the optical flow vector of the target block can be obtained by calculating the minimum SAD between the current block and the previous block.
In the experiment, the images which are perpendicular to the camera are collected. In the whole collecting process, a data block which is 8 × 8 pixel is used as the block matching object, and the searching area contains ±4 pixels. So there are 64 pixel points and 81 candidate vector directions in each frame image. After obtaining each frame image, the mean absolute errors of each candidate vector are calculated and the minimum value is selected as the optical flow vector.
The motion model of optical flow is projecting the threedimensional motion onto the twodimensional image plane of the camera. There are two common optical flow estimation models: one is pinhole image plane approach which is derived from the principle of insect and vertebrate visual system; the other is spherical imaging surface approach which is derived from insect compound eyes. In our study, the pinhole image plane approach is used to estimate the motion of MAV under geographic coordinate system.
The pinhole image plane model is shown as in Figure
The model of pinhole image plane.
Considering any point
Equation (
After derivation calculus to (
Substituting (
In (
By measuring the gravitational field, the accelerometer can determine the roll and pitch of the MAV under the condition of no own acceleration; by measuring the geomagnetic field, the magnetometer can determine the heading of MAV based on the vehicle’s attitude information provided by accelerometer. Then, the whole attitude information without accumulated error can be obtained by integrating accelerometer/magnetometer.
The component of gravity vector under the geographic coordinate system is
The roll and pitch can be calculated:
This method uses the projection information of the earth’s gravitational acceleration in carrier coordinate system to reflect the attitude information of the vehicle, so the above equations are correct only under the condition that there is no acceleration of the vehicle. Actually, the vehicles are not always of static or uniform motion, and the measured values of accelerometer are not equal to the component of gravitational acceleration under MAV coordinate system anymore once the carrier is under accelerated motion. Therefore, this method only can be used for measurement in static state, and it is necessary to find another attitude measuring method for dynamic conditions.
The component of earth’s magnetic field intensity under geographic coordinate system is
Supposing that the magnetic field intensity keeps on being constant during the flight of MAV, the heading of MAV under geographic coordinate system can be calculated by (
In our study, the EKF filter is employed to fuse the accelerometer and optical flow sensor. The velocity and position calculated by accelerometer under navigation coordinates are selected as the state value, and the velocity and position calculated by optical flow sensor are selected as the observer value. The estimation process is shown in Figure
Optical flow sensor/accelerometer integrated system.
Considering the nonlinear system state equation and observed equation,
Substituting the state and observed equation into EKF, the time renewal equation is
Measurement renewal equation is
From the process of EKF mentioned above, the data for position and velocity of MAV can be obtained in geographic coordinate system.
The attitude of MAV can be obtained through the integration of the angular rate from gyroscope output signal, however, the performance of MEMS gyroscope would be influenced by drift. The integrated accelerometer/magnetometer system can provide attitude information without drift; therefore, it is necessary to fuse the data of multisensors by using EKF. The filtering process is shown as Figure
Gyroscope/accelerometer/magnetometer integrated system.
The system observation vector can be expressed as
The effective estimation of MAV velocity and position can be obtained by using optical flow sensor/INS integrated navigation system based on EKF no matter under stationary status or motion status. According to the characteristics of attitude measurements by using gyroscope and accelerometer/magnetometer, accelerometer and magnetometer are integrated to calibrate the gyroscope during stationary or uniform motion state; when the motion state is detected as accelerating or decelerating, the standalone gyroscopes are used to obtain the attitude by strapdown calculating. The motion state is detected by optic flow sensor. The overall navigation scheme is shown in Figure
The flow chart of optical flow sensor/INS/magnetometer integrated navigation system.
In order to verify the proposed optical flow sensor/INS/magnetometer integrated navigation system, a MAV flight experiment was carried out. The experiment location was in the stadium of North University of China, Taiyuan. As shown in Figure
The experiment trajectory.
As shown in Figures
The hardware experiment platform.
The INS/OFS/magnetometer integrated navigation system.
Figures
Comparison results between INS only and OFS/INS/magnetometer at the end position.
Ground truth position at the end  INS only  OFS/INS/magnetometer  

Directions  Ground truth value  Measurements  Error  Estimation  Error 

0  −1.87  —  0.25  — 

50  57.8  15.6%  50.9  1.8% 

0  0.78  —  0.14  — 
The velocity estimation results.
The position estimation results.
The attitude estimation results.
Figure
Trajectory comparison results between ground truth, INS only, and EKF.
In this paper, an optical flow sensor/INS/magnetometer integrated navigation system is proposed for MAV. The proposed method, which is based on EKF, combines optical flow sensor, INS, and magnetometer information to estimate the attitude, velocity, and position of MAV. Specifically, the gyro, accelerator, and magnetometer information are fused together to estimate the MAV attitude when the MAV is static or uniformly moving; and the gyro only is used to estimate the MAV attitude when the MAV is accelerating or decelerating. Experiment results show that the proposed method which can significantly reduce the errors for navigation position, velocity, and attitude, compared with the INS only navigation system, can effectively improve the navigation performance of MAV with a significant value of engineering application.
The authors declare that there is no conflict of interests regarding the publication of this paper.
This work was supported in part by the China National Funds for Distinguished Young Scientists (51225504), the National 973 Program (2012CB723404), the National Natural Science Foundation of China (91123016, 61171056), the Research Project Supported by Shanxi Scholarship Council of China (2015082), and the College Funding of North University of China (110246).