UKF Two-stage Estimation Algorithm for Heading and Attitude of Linear Plant Protection UAV

SHEN Yue, SUN Zhiwei, SHEN Yayun, ZHANG Dahai, QIAN Peng, LIU Hui

Abstract

Aiming at the problems of serious magnetic field interference, poor dynamic performance of magnetometer calibration and low accuracy of UAV attitude estimation, a two-stage heading and attitude estimation method based on real-time magnetometer calibration was proposed. According to the characteristics of small variation of geomagnetic field vector, the real-time calibration model of magnetometer was established by using Levenberg-Marquardt (LM) algorithm and magnetometer error model, and the error parameters of magnetometer were calculated in real time. Considering the disturbance of motion acceleration, motor magnetic field and environmental magnetic field, the unscented Kalman filter (UKF) was used to fuse gyroscope and accelerometer to realize the first-stage attitude estimation, and the attitude information of roll angle and pitch angle was accurately analyzed through quaternion. The second-stage attitude estimation combined the real time calibration data of the magnetometer and the gyroscope to correct the heading angle, and finally realized the accurate estimation of the UAV attitude and heading. The test results showed that when the external magnetic field interference was up to 30.97μT, the real-time calibration algorithm can still quickly calculate the calibration parameters of the magnetometer, and the mode length root mean square error was 0.59μT, which reduced the noise of heading observation information. The root mean square error of the attitude angle of the attitude measurement system was no more than 0.75°, and the root mean square error of the heading angle was 1.40°. Compared with that of complementary filtering algorithm, the attitude angle accuracy was increased by 0.6°, and the heading angle estimation accuracy was improved by 1.38°. In the dynamic flight test, the attitude estimation algorithm greatly reduced the influence of magnetic interference, the attitude tracking was accurate, the heading angle converged quickly, and the steady-state accuracy was higher.

 

Keywords: linear plant protection UAV, attitude estimation, hierarchical fusion, magnetometer real-time calibration, unscented Kalman filter

 

 


Download Full Text:

PDF


References


ZHENG Jiaqiang, XU Youlin. Development and prospect in environment-friendly pesticide sprayers [J]. Transactions of the Chinese Society for Agricultural Machinery, 2021, 52(3):1 -16.

ZHOU Zhiyan, ZANG Ying, LUO Xiwen, et al. Technology innovation development strategy on agricultural aviation industry for plant protection in China [J]. Transactions of the CSAE, 2013, 29(24):1 - 10. (in Chinese)

CAO Guangqiao, LI Yibai,NAN Feng, et al. Development and analysis of plant protection UAV flight control system and route planning research [J]. Transactions of the Chinese Society for Agricultural Machinery, 2020, 51(8):1 -16. (in Chinese)

TEDALDI D, PRETTO A, MENEGATTI E. A robust and easy to implement method for IMU calibration without external-equipments [C] //2014 IEEE International Conference on Robotics and Automation, 2014:3042 - 3049.

SUN Hongwei, FANG Jiancheng. Error modeling and calibration method of AMR compass [J]. Journal of Beijing University of Aeronautics and Astronautics, 2009, 35(1):104 - 107. (in Chinese)

WU H L, PEI X B, LI J H, et al. An improved magnetometer calibration and compensation method based on Levenberg – Marquardt algorithm for multi-rotor unmanned aerial vehicle [J]. Measurement & Control, 2020, 53(3):276 - 286.

CAI Haoyuan, ZHAO Shenglin, CUII Songye, et al. Nine-axis inertial fusion method based on dynamic magnetic field calibration [J]. Optics and Precision Engineering, 2020, 28(9):2007 - 2016. (in Chinese)

SILIC M, MOHSENI K. Correcting current-induced magnetometer errors on UAVs: an online model-based approach [J]. IEEE Sensors Journal, 2020, 20(2):1067 - 1076.

EUSTON M, COOTE P, MAHONY R, et al. A complementary filter for attitude estimation of a fixed-wing UAV [C] // 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2008:340 - 345.

MAHONY R, HAMEL T, PFLFLIMLIN J M. Nonlinear complementary filters on the special orthogonal group [J]. IEEE Transactions on Automatic Control, 2008, 53(5):1203 - 1218.

MADGWICK S O H, HARRISON A J L, VAIDYANATHAN R. Estimation of IMU and MARG orientation using a gradient descent algorithm [C] // 2011 IEEE Internationa1 Conference on Rehabilitation Robotics, 2011:1 - 7.

SEBESTA K D, BOIZOT N. A real-time adaptive high-gain EKF, applied to a quadcopter inertial navigation system [J]. IEEE Transactions on Industrial Electronics, 2014, 61(1):495 -503.

WU Helong, BAI Yue, PEI Xinbiao, et al. Attitude calculation method based on CPF - EKF for large load plant protection UAV [J]. Transactions of the Chinese Society for Agricultural Machinery, 2018, 49(6):24 - 31, 77.

[ 14] HUANG Peikui, ZHANG Zhigang, LUO Xiwen, et al. Development of external acceleration identification and attitude estimation system of field working vehicle [J]. Transactions of the CSAE, 2019, 35 (3):9 - 15. (in Chinese)

ZHANG Yonggang, ZHANG Yunhao, LI Ning. MEMS/GPS/geomagnetic integrated navigation system based on complementary filter [J]. Systems Engineering and Electronics, 2014, 36(11):2272 - 2279. (in Chinese)

PENG Xiaodong, ZHANG Tiemin, LI Jiyu, et al. Attitude estimation algorithm of agricultural small-UAV based on sensors fusion and calibration [J]. Acta Automatica Sinica, 2015, 41(4):854 - 860. (in Chinese)

CAI Anjiang, LIU Kaifeng, GUO Shihong, et al. Quaternion derivative unscented Kalman filter-based two-step attitude estimation algorithm for multi-rotor unmanned aerial vehicle [J]. Control Theory & Applications, 2020, 37(2):365 - 373.

LU Yanjun, CHEN Yudi, ZHANG Xiaodong, et al. Attitude information fusion method based on extended Kalman filter [J]. Chinese Journal of Scientific Instrument, 2020, 41(9):281 - 288. (in Chinese)

JULIER S J, UHLMANN J K. Unscented filtering and nonlinear estimation [J]. Proceedings of the IEEE, 2004, 92(3):401 - 422.

ZHOU Jianjun, LIN Chunsheng, ZHAO Jianyang. Research on learning flight for unmanned aircraft in background magnetic field [J]. Acta Armamentarii, 2014, 35(1):76 - 82. (in Chinese)

HE C, LI Z, WANG Y J. Attitude algorithm system design of quad-rotor aircraft based on STM32 [C] // Fifth Asia International Symposium on Mechatronics, 2015:1 - 4.


Refbacks

  • There are currently no refbacks.