Proceedings of the ASME 2009 International Design Engineering Technical Conferences & Computers and Information in Engineering Conference IDETC/CIE 2009 August 30 - September 2, 2009, San Diego, California, USA Proceedings of the ASME 2009 International Design Engineering Technical Conferences & Computers and Information in Engineering Conference IDETC/CIE 2009 August 30-September 2, 2009, San Diego, USA
DETC2009-87675
DETC2009/MESA-87675
DESIGN AND IMPLEMENTATION OF SENSING AND ESTIMATION SOFTWARE IN AGGIENAV, A SMALL UAV NAVIGATION PLATFORM
Calvin Coopmans, Haiyang Chao and YangQuan Chen Center for Self-Organizing and Intelligent Systems (CSOIS), Electrical and Computer Engineering Department, Utah State University, Logan, UT 84322 − 4120, USA E-mails: {cal.coopmans, haiyang.chao}@aggiemail.usu.edu,
[email protected]
ABSTRACT Small UAV performance is limited by the sensors and software filters used in the navigational systems. Several solutions of various complexity and cost exist, however no ready-made solutions exist for a high-accuracy, low-cost UAV system. Presented is the design (low-level system as well as high-level extended Kalman filter) for a specifically designed small-UAV navigation platform, AggieNav.
The options for low-cost miniature attitude navigation sensing are few; most inexpensive, small fixed-wing UAVs use infrared thermopiles sense the relative heat differential from the black body radiation of the Earth’s surface. While this technique does work, it is not accurate enough for some remote measurement tasks and has problems when flying near large land features such as mountains and through valleys. Combined with a modern civilian GPS receiver, navigation is possible. Inertial measurement units with roll-rates and accelerometers are the standard sensor for attitude determination in flight. Several solutions exist for navigation in small UAVs, such as, all of which use Kalman or other state-estimation filter of some kind to provide a high-accuracy estimation of the current position and attitude of the system at a given instant. While these systems work well for their intended applications, even the academic prices are prohibitive for integration into inexpensive UAVs. Additionally, the source code is not made available, and users of the systems are forced to accept the filter/algorithm performance of the systems as they are given.
NOMENCLATURE UAV Unmanned Aerial Vehicle. IMU Inertial Measurement Unit. Typically a 3-axis roll rate gyro co-axial with a 3-axis accelerometer. INS Inertial Navigation System. Suggests a combination of IMU and other senors such as GPS via a software filter. UAV Unmanned Aerial Vehicle.
Recently, due to the high-availability of low-cost sensing chips, many consumer-level devices such as the Apple iPhone, Nintendo Wii and Sony PlayStation 3 have integrated accelerometers into their hardware for more immersive user-experiences. Several chip manufactures are producing 3-axis accelerometers, however, the availability of integrated 3-axis roll-rate gyro chips is not as broad. Analog Devices, inc. is, at the time of this writing, the only company producing fully-integrated 6-degree-of-
INTRODUCTION To accomplish mission goals or to fly passengers to their destinations, all aircraft rely on sensors for the core of their navigational systems. For small UAVs, the sensor suite determines much about the usefulness and capabilities of such a system, and is mostly limited because of the relatively high cost of highquality navigational sensors and equipment. 1
c 2009 by ASME Copyright
freedom sensor units [1]. In any navigation system, performance and reliability are primary design drivers. In the world of small UAVs, size, weight and price are also of very high importance. AggieNav is a unique design of cutting-edge sensing, power and processing hardware, paired with highly sophisticated data filtering algorithms, giving it a decisive lead in front of other similar systems. AggieNav has a unique feature set not found on other INS solutions:
ital signal processor (DSP) core. Thus, AggieNav can support the future of small UAV navigation and attitude estimation algorithms.
Gumstix Linux Computer TTL Serial
•Full high-accuracy 6-Degree-of-Freedom IMU •Full 3-axis tilt-compensated compass •Universal interface with any GPS unit via a serial port •Dual pressure sensors for static and dynamic air pressures •72 MIPS onboard processing •Onboard hardware mounts for a 600MHz Gumstix Linux computer, and Paparazzi [2] based autopilot system (“AggiePilot”) for a full UAV system •Lowest price for any such system •Open software architecture allows AggieNav to be customized or augmented easily •Weight: 70g, power: 1.0W total with Gumstix and GPS
IMU Compass
AggieNav
SPI Data Bus
GPS, Etc. SPI Bus Master
AggiePilot Autopilot
1 AGGIENAV SYSTEM DESCRIPTION AggieNav (Figures 2 and 3) has a combination of sensors to support high quality flight navigation. Primarily, if course, is the Analog Devices ADIS16354 Inertial Measurement Unit, with 6 full axis of roll-rate and accelerometer data. A Honeywell HMC6343 tilt-compensated magnetic compass provides slower, but accurate, filtered, absolute heading data. A high-rate civilian GPS receiver, in this case a 4Hz uBlox LEA-5H unit, provides lower bandwidth data about the UAV’s global position. Finally, two small yet accurate pressure sensors (SCP-1000 series) from VTI give both static and dynamic pressure, allowing for the calculation of both absolute elevation (with launch time calibration) and windspeed (differential via Pitot tube [3]). The power system is designed for the full voltage range of the batteries in the UAVs, and provides additional power to the Gumstix computer and the onboard host-mode mini-USB port.
Figure 1.
2 FLEXIBLE COMPUTING: GUMSTIX The Gumstix Verdex unit mounted on AggieNav (Figures 1 and 2) is used as a co-processor for the extensive filter computations, as well as control of other aspects of the UAV mission including control of payload and downlinking of mission data. With small redesign efforts, the AggieNav system can support the newer line of Gumstix-the Overo Earth, Wind, Water, and Fire. The Overo line is is capable of up to 1200 Dhrystone MIPS, and the Overo Water even more, with an integrated C64x+ dig-
AGGIENAV INTERNAL DATA CONNECTION DIAGRAM
Gumstix Verdex Linux System
5.0V; 3.3V Switching Regulators
uBlox LEA-5H GPS Receiver
User Interface (LEDs, Buttons)
Atmel AVR32A256B Microcontroller
External Data Interfaces (TTL-Serial; SPI Slave)
Honywell HMC6343 3-axis Magnetic Compass
Analog Devices ADIS1654 6-DoF IMU
VTI SCP-1000 Pressure Sensors (x2)
Figure 2.
Papazazzi TWOG Autopilot Board
AGGIENAV HARDWARE BLOCK DIAGRAM
3 UAV ATTITUDE ESTIMATION AggieNav provids three axis gyro, accelerometer, magnetic heading sensor and GPS outputs. The main application of Ag2
c 2009 by ASME Copyright
Table 1.
AGGIENAV VS OTHER SMALL UAV INS SOLUTIONS
INS Unit
AggieNav
Stock Microstrain 3DM GX2
Crossbow MNAV
Max Gyro Bandwidth
700Hz (unfiltered)
250Hz
25Hz
Max Gyro Dynamic Range
±300deg/sec
±300deg/sec standard
±150deg/sec
Accelerometer Dynamic Range
±10g
±5g
±2g
±10g
Accelerometer Resolution
14bit
16bit
NA
14bit
Accelerometer Bandwidth
700Hz (unfiltered)
250Hz
25Hz
9Hz
Magnetics Rate
5Hz heading solution
250Hz
1-100Hz
NA
Magnetics Accuracy
±3deg
0.001Gauss
NA
NA
GPS Rate
4Hz
No GPS
4Hz
NA
Max GPS Accuracy
2.5m
No GPS
3m
5m
Pressure Rate
1.8Hz
No Pressure
NA
NA
Pressure Accuracy (Abs)
1.5Pa
No Pressure
NA
2.44Pa
Power Draw
600mW
470mW (minimum)
400mW
700mW
3.1
Procerus Kestrel 9Hz ±300deg/sec
ATTITUDE REPRESENTATION
There are several methods to represent the orientation of a 3D rigid body in the inertial frame including Euler angle, directional cosine matrix, and unit quaternion etc. The most commonly used one is Euler angle (roll, φ, pitch, θ, and yaw, ψ). The trajectory control of an unmanned aerial vehicle can be converted to cascaded control of roll, pitch, and yaw. However, Euler angle has some singularity points. Unit quaternion provides another representation without introducing a gimbal lock problem. Unit quaternion could be defined as follows:
Figure 3.
q0 q1 q= q2 , q3
AGGIENAV PROTOTYPE WITH GUMSTIX ANG GPS UNIT
gieNav is for unmanned vehicles and robotics navigation, which requires estimation of the vehicle orientation with respect to the inertial frame [4]. The accurate estimation of the unmanned vehicle orientation could have a big impact on the later image referencing and registration [5]. Therefore, an attitude estimation algorithm is also developed for AggieNav. A lot of researchers have done similar work [6], [7], [8], [9].
where
q20 + q21 + q22 + q23 = 1.
(1)
The conversion from unit quaternion to Euler angles can be written as: 2(q2 q3 + q0 q1 ) , q20 − q21 − q22 + q23 θ = arcsin(−2(q1 q3 − q0 q2 )), 2(q1 q2 + q3 q0 ) . ψ = arctan 2 q0 + q21 − q22 − q23 φ = arctan
To test our filter, we are using MATLAB to process actual flight data collected in tandem with a commercial Microstrain 3DM-GX2 IMU w/ compass. This allows us to test the results of our filter against a known good solution on the same data. 3
(2) (3) (4)
c 2009 by ASME Copyright
3.2
IMPLEMENTATION OF AN EXTENDED KALMAN FILTER (EKF) Given a nonlinear system, an extended Kalman filter could be designed [10]. Assume the simplified nonlinear model is : xk = f (xk−1 , uk ) + wk , yk = g(xk ) + vk .
surement from the yaw axis, the nonlinear system can be modeled by [9]:
0 − pˆ −qˆ −ˆr 0 0 0 pˆ 0 rˆ −qˆ 0 0 0 qˆ −ˆr 0 pˆ 0 0 0 1 rˆ qˆ − pˆ 0 0 0 0 q˙ = q + wk , wk ∼ N(0, Q), (10) 2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 000 2g(q1 q3 − q0 q2 ) (11) yˆ = 2g(q2 q3 + q0 q1 ) + vk , vk ∼ N(0, R), g(q20 − q21 − q22 + q23 )
(5) (6)
where xk is the system state vector, uk is the system input vector, yk is the system measurement vector, wk is the process white noise with the distribution wk ∼ N(0, Q), vk is the observation white noise with the distribution vk ∼ N(0, R). The key idea of Kalman filter is to fuse the estimates of system states from both the system equation and measurement equation. The recursive structure of Kalman filter also makes it robust to the system changes. To start the Kalman filter estimation, the initial values need to set including Q, R and P0|0 . The Kalman filter predict and update steps include:
where bp p pˆ qˆ = q − bq . br r rˆ
(12)
xˆk|k−1 = f (xk−1|k−1 ˆ , uk ), Pk|k−1 = Fk Pk−1|k−1 FkT + Qk , yˆk = yk − g(xk|k−1 ˆ ),
It needs to be pointed out that Eqn. 11 is an approximation close to the static case since only the gravity is considered for accelerometer measurements. It is true when the UAV is not accelerating too much. But a more general dynamic case could be represented as follows [6]:
Kk = Pk|k−1 GTk (Gk Pk|k−1 GTk + Rk )−1 , xˆk|k = xk|k−1 ˆ + Kk yˆk , Pk|k = (I − Kk Gk )Pk|k−1 , (7)
yˆ =
where Fk =
∂f |xˆ ,u , ∂x k−1|k−1 k
Gk =
∂g |xˆ ∂x k|k−1
(8)
ATTITUDE ESTIMATION USING EKF Assume the system state is a vector q, representing the unit quaternion and the gyro bias and the system output is a vector y, ˆ representing the acceleration data [9]:
4 EXPERIMENTAL RESULTS 4.1 RAW SENSOR DATA COMPARISON The raw sensor data from AggieNav is compared with the sensor data from Microstrain Gx2 IMU, which is an expensive IMU with 3-axis gyro, accelerometer and magnetic sensors. The AggieNav data is scaled to the same unit as Gx2 IMU. The gyro data is shown in the unit of degree/s and the acceleration data is shown in the unit of m/s2 .
ax y = ay . az
vk ∼ N(0, R),(13)
where Va is the 3D speed measured by GPS. Due to the time limit, Eqn 11 is used in our experiments. The attitude state estimation can be calculated using the steps described in Sec.3.2.
3.3
q0 q1 q2 q= q3 , bp bq br
qVa sinθ + sinθ g rVa cosθ−pVa sinθ − cosθsinφ + vk , g −qVa cosθ − cosθcosφ g
(9)
Assume p is the gyro measurement from the roll axis, q is the gyro measurement from the pitch axis, and r is the gyro mea4
c 2009 by ASME Copyright
200
10 aggienav gx2
150
aggienav gx2
8 6
100 4 ax (m/s2)
p (deg/s)
50 0
2 0 −2
−50
−4 −100 −6 −150 −200 80
−8 90
100
110
120 time (s)
130
140
150
−10 80
160
90
100
(a) gyro p
130
140
150
160
10 aggienav gx2
200
aggienav gx2
8
150
6
100
4
50
2
ay (m/s2)
q (deg/s)
120 time (s)
(a) acceleration x
250
0
0
−50
−2
−100
−4
−150
−6
−200 −250 80
110
−8 90
100
110
120 time (s)
130
140
150
−10 80
160
90
100
(b) gyro q
110
120 time (s)
130
140
150
160
(b) acceleration y
200
0 aggienav gx2
150
aggienav gx2
−2 −4
100 −6 az (m/s2)
r (deg/s)
50 0
−8 −10 −12
−50
−14 −100 −16 −150 −200 80
−18 90
100
110
120 time (s)
130
140
150
−20 80
160
90
(c) gyro r Figure 4.
100
110
120 time (s)
130
140
150
160
(c) acceleration z Figure 5.
Gyro Sensor Comparison
5
Acceleration Sensor Comparison
c 2009 by ASME Copyright
4.2
PRELIMINARY RESULTS FOR ATTITUDE ESTIMATION USING KALMAN FILTER The preliminary EKF is designed based on the descriptions above with no heading corrections due to the magnetic sensor interpretation problems. The EKF is set to run at 100hz.
Along with a comparison with other lower-cost UAV navigation systems, the theory and design of the EKF is shown, and results on real-world data have been presented. The future of the project involves further development of the dynamic cases, and more flight testing using the hardware as the navigation reference.
ACKNOWLEDGMENT The author would like to thank all of CSOIS, Yiding Han, Austin Jensen, as well as the Utah Water Lab and the Association for Unmanned Vehicle Systems International (AUVSI) for their Student UAS competitions. This work has been supported by Utah Water Research Laboratory MLF funding (2006-2010).
50 40 30
phi (deg.)
20 10 0 −10
REFERENCES [1] A NALOG D EVICES , I NC ., 2008. High Precision Tri-Axis Inertial Sensor. One Technology Way, P.O. Box 9106, Norwood, MA 02062-9106, U.S.A., 01. Datasheet. [2] “Paparazzi UAV Forum”. URL http://www.recherche.enac.fr/paparazzi/. [3] Klopfenstein Jr., R., 1998. “Air velocity and flow measurement using a pitot tube”. ISA Transactions, 37, pp. 257– 263. [4] Chao, H., Cao, Y., and Chen, Y. Q., Under Revision. “Autopilots for small unmanned aerial vehicles: a survey”. International Journal of Control, Automation and Systems. [5] Chao, H., Baumann, M., Jensen, A., Chen, Y. Q., Cao, Y., Ren, W., and McKee, M., 2008. “Band-reconfigurable multi-uav-based cooperative remote sensing for real-time water management and distributed irrigation control”. IFAC World Congress, pp. 11744–11749. [6] Beard, R. W., 2007. State Estimation for Micro Air Vehicles, Vol. 70 of Studies in Computational Intelligence. Springer Berlin / Heidelberg, pp. 173–199. [7] Eldredge, A. M., 2006. “Improved State Estimation for Miniature Air Vehicles”. MS Thesis, Brigham Young University, December. [8] Christiansen, R. S., 2004. “Design of an Autopilot for Small Unmanned Aerial Vehicles”. MS Thesis, Brigham Young University, December. [9] Jang, J. S., and Liccardo, D., 2006. “Automation of small UAVs using a low cost MEMS sensor and embedded computing platform”. In 25th Digital Avionics Systems Conference, 2006 IEEE/AIAA, pp. 1 – 9. [10] Welch, G., and Bishop, G., 2006. An Introduction to the Kalman Filter. Report, University of North Carolina at Chapel Hill, July. See also URL http://www.cs.unc.edu/ welch/kalman/kalmanIntro.html.
−20 −30
gx2 kalman
−40 −50
5
10
15
20
25
30
35
40
35
40
(a) Roll Angle Estimation 20 15 10
the (deg.)
5 0 −5 −10 gx2 kalman
−15 −20
5
10
15
20
25
30
(b) Pitch Angle Estimation Figure 6.
Angle Estimation Using EKF
From the figures above, we can see that the roll and pitch angles can be estimated with the current EKF although there is obvious high frequency noise. The problems left are the prefiltering for the raw sensor data and Kalman filter covariance matrix estimation.
5 CONCLUSION In this paper we have presented a high-performance Extended Kalman Filter, running with AggieNav, a collection of high-quality, low cost sensors for UAV attitude estimation. 6
c 2009 by ASME Copyright
