Abstract
The drift of inertial navigation system (INS) will lead to large navigation error when a low-cost INS is used in microaerial vehicles (MAV). To overcome the above problem, an INS/optical flow/magnetometer integrated navigation scheme is proposed for GPS-denied 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.
1. Introduction
Recently, the autonomous operating unmanned aerial vehicle (MAV) is becoming popular in military and civilian applications [1–3], such as aerial surveillance, cargo delivery, search and rescue, and remote sensing and mapping. A reliable navigation system is very important for safely operating the MAV. Currently, global positioning system (GPS) [4] and inertial navigation system (INS) [5] are the most widely used navigation techniques for the MAV. GPS is based on satellite which can provide relatively consistent accuracy navigation; however, the satellite signals would be lost in city canyon or indoor environments [6, 7]. INS is a self-contained device which can offer a complete set of navigation parameters with high frequency, including attitude, velocity, and position; however, the rapid growth of systematic errors with time is a main drawback of stand-alone INS [8, 9].
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 [10, 11]. Optical flow techniques are motivated by bird and insect flights which are used to solve the navigation problem. Optical flow can be thought to be the 2D projection of the 3D perceived motion of the objects, which has been widely utilized for motion estimation [12, 13]. When facing the ground, the optical flow sensors can be used for accurate velocity calculation, and the position can be obtained by integration. For example, computer mouse sensor is a successful application of optical flow theory. Benefiting from previous study of optical flow algorithm and hardware design, robotics researchers have employed optical flow sensors for the MAV navigation. Ding et al. [14] added optical flow into the GPS/INS integration for UAV navigation where optical flow rate is used as backup velocity aiding in case of GPS signal outages, and the verification results showed that, by using optical flow, the height estimation was still within a meter range within 20 seconds after GPS dropped out. Mercado et al. [15] proposed a GPS/INS/optical flow data fusion method for position and velocity estimation; experiment results showed that a good velocity measurement could be obtained by optical flow under rough GPS conditions. Rhudy et al. [16] aided wide-field optical flow to INS for UAV, where the wide-field optical flow was used to regulate INS drift for the purpose of GPS-denied navigation. Gageik et al. [17] applied an optical flow sensor for 2D positioning of UAV, and position hold result showed that the standard deviation of the position error was just 10 cm and the position error was just about 30 cm after landing. It can be clearly seen that the optical flow is an effective solution for velocity and position measurement. However, traditional optical flow is detected by video camera, which is not suitable for MAV because of the weight, size, and power. In 2013, an optical flow sensor based on a machine vision CMOS image sensor for MAV was proposed and named as PX4FLOW [18], and the indoor verification results showed that the error was just 0.5 m with the overall trajectory being 28.44 m. Compared to traditional optical flow sensors, PX4FLOW is of low power, low latency, small size, and low cost; therefore, it is very suitable for MAV applications.
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 2 is the theory of velocity and position estimation by optical flow. The data fusion model is given in Section 3. Section 4 is the experiment and verification. Section 5 is the conclusion.
2. Estimation of Velocity and Position by Optical Flow Sensor
2.1. The Calculation of Optical Flow
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 Lucas-Kanade algorithm, Horn-Schunck 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 1.
As shown in Figure 1, is set as the gray value of target block selected from the previous frame, and is the gray value of the target block which is to be matched in the searching area of current frame, where , . The principle of BMA based on MMAE and the SAD is to search and which satisfy (1); then, the optical flow vector can be obtained, where the unit of is pixel/sec; is the sample frequency of the camera and the unit is fram:e/sec; is the minimum mean absolute error:
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.
2.2. The Model of Optical Flow
The motion model of optical flow is projecting the three-dimensional motion onto the two-dimensional image plane of the camera. There are two common optical flow estimation models: one is pin-hole 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 pin-hole image plane approach is used to estimate the motion of MAV under geographic coordinate system.
The pin-hole image plane model is shown as in Figure 2. is set as a point under camera coordinate system; stands for the focal length, so can be expressed as :
Considering any point on the ground, point has the following relationship relative to the MAV under the camera coordinate system:
Equation (7) is expanded in three dimensions and (8) can be obtained: where is angular velocity of MAV; is average velocity of MAV under camera coordinate system.
After derivation calculus to (4), the relationship between the velocity of under the camera coordinate system and the velocity of under the image plane can be obtained:where , and (10)–(12) can obtained by expanding (9):
Substituting (8) into (10), (11), (13) can be obtained:
In (13), and are the optical flow components on directions and , which can be calculated by SAD and BMA. can be obtained by ultrasonic which was integrated in the optical flow sensor. Angular velocity , , and can be obtained by gyroscope; and can be substituted by (2) and (3). Therefore, the translational velocity of the MAV under the camera coordinate system can be estimated, and the MAV velocity under geographic coordinate system can be calculated through coordinate transformational matrix . After integration of velocity, the data for position of MAV can be obtained at last.
2.3. Attitude Estimation Based on Accelerometer/Magnetometer
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.
2.3.1. The Roll and Pitch Obtained by Accelerometer
The component of gravity vector under the geographic coordinate system is . When the vehicle is static (no relative acceleration to the navigation coordinate system), the measured value of accelerometer which is fixed in the carrier coordinate system is . The heading of the vehicle has no influence on the output of accelerometers of directions and due to the gravitational acceleration perpendicular to horizontal plane. Therefore, it can be concluded that
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.
2.3.2. The Heading Obtained by Magnetometer
The component of earth’s magnetic field intensity under geographic coordinate system is . The magnetometers are fixed on the carrier and the coordinate of the magnetometers is the same as the carrier coordinate system , and the component of magnetic field intensity under the carrier coordinate system is . The projection of magnetic field intensity to the geographic and camera coordinate systems can be presented aswhere can be referred to by [19]. In Taiyuan area (northern latitude 37.8°, east longitude 112.5°), is shown as (17), and the data of can be obtained from the magnetometer:
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 (16) and (17) based on the roll and pitch provided by accelerometer.
3. Data Fusion Based on EKF
3.1. Optical Flow Sensor/Accelerometer Integrated System for Velocity and Position Measurements
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 3.
Considering the nonlinear system state equation and observed equation,where is state vector which includes the velocity information and position information of MAV; is observer vector which includes the optical flow data of directions and from optical flow sensor and the data obtained from ultrasonic sensors. is the control vector of system that can be obtained by the accelerometer data after coordinate matrix transformation; is matrix of control allocation; is matrix of noise allocation; is matrix of process noise; is observer noise; represents the system state function; represents the observation function.
Substituting the state and observed equation into EKF, the time renewal equation is
Measurement renewal equation iswhere and .
From the process of EKF mentioned above, the data for position and velocity of MAV can be obtained in geographic coordinate system.
3.2. Gyroscope/Accelerometer/Magnetometer Integrated System Based on EKF
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 4; the system state vector can be expressed as , where is quaternion of the system state; it can be calculated by fourth-order Runge-Kutta method. is the output of gyroscope.
The system observation vector can be expressed as , which can be calculated by Gauss-Newton method based on the data of accelerometer and magnetometer.
3.3. Optical Flow Sensor/INS/Magnetometer Integrated Navigation Scheme
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 strap-down calculating. The motion state is detected by optic flow sensor. The overall navigation scheme is shown in Figure 5.
4. Experiments and Verification
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 6, we can see that, during the flight procedure, the MAV kept flying to the north. The flying distance was 50 meters, and the flight height is 1.5 meters.
4.1. Experiments Platform
As shown in Figures 7 and 8, the autonomous MAV is used as flight platform (Figure 7(a)); STM32F103Z is selected as the flight control processor (Figure 7(b)); the optical flow sensor is produced by 3D Robtics Corporation which is named as PX4FLOW (Figure 7(c)); the IMU is MPU6050 (Figure 7(d)); the magnetometer is HMC3883L which is produced by Honeywell Limited (Figure 7(d)); DJ1 2312 which is produced by DJI is selected as the MAV motor (Figure 7(e)); and HC-12 wireless serial interface module (Figure 7(f)) is used to transmit the sensor’s data from MAV to PC.
4.2. Verification Results
Figures 9–11 are the velocity estimation results, position estimation results, and attitude estimation results, respectively. Table 1 is the end position comparison results between INS only and OFS/INS/magnetometer. It can be concluded that (1) in the process of velocity estimation and position estimation, the OFS/INS/magnetometer based on EKF can decrease the drift of INS effectively. For example, from Figure 9 we can see that the position drift is about 8 meters after 50 meters trajectory without the correction of OFS and magnetometer, and the position drift is decreased to less than 1 meter after using the integrated algorithm and (2) in the process of attitude estimation, the roll angle, pitch angle, and the yaw angle can be effectively tracked by OFS/INS/magnetometer based on EKF during static and uniform velocity status. And after nonuniform velocity motion, the attitude can be corrected immediately.
Figure 12 is the trajectory comparison results between ground truth, INS only, and EKF. It can be concluded that the proposed optical flow sensor/INS/magnetometer integrated navigation system can provide a higher accurate navigation result compared to INS only navigation system.
5. Conclusion
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.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
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 (2015-082), and the College Funding of North University of China (110246).