Attitude Estimation Algorithm of Agricultural Small-UAV Based on Sensors Fusion and Calibration
-
摘要: 实时姿态信息获取是运用农用小型无人机(Unmanned aerial vehicle, UAV)进行变量作业控制的重要环节,本文采用STM32单片机、微机电系统(Micro-electro mechanical system, MEMS)加速度计、陀螺仪、磁强计和无线收发模块设计出农用小型无人机姿态实时解算系统,文中对三轴数字传感器的校正方法以及基于四元数和梯度下降法的多传感器融合姿态估计做了详细地介绍与推导.结果表明,在72MHz时钟频率下模拟集成电路总线(Inter-integrated circuit, ⅡC)传感器数据采集及滤波消耗6.2ms,迭代步长取0.8,一次姿态解算消耗约0.96ms,数据更新频率可达100Hz,能满足实时性要求.经测试在静态时俯仰角和翻滚角输出的平均绝对误差小于1.5,偏航角平均绝对误差小于2.9,小幅震动条件下的俯仰角、翻滚角和偏航角平均绝对误差增加1~2左右.这表明该传感器校正方法与姿态融合算法实用有效,能为农用小型无人机的飞行控制与变量作业实施提供准确的姿态数据.Abstract: The real-time attitude information of agricultural small unmanned aerial vehicle(UAV) is a key factor to the decision and operation for variable rate program in precision agriculture. A real-time attitude estimation system of agricultural small UAV is designed here which consists of a microprocessor STM32, micro-electro mechanial system(MEMS) inertial sensors, and wireless transceiver module nRF24L01a. Detailed description and derivation of sensor calibration method and the multi-sensor fusion algorithm of attitude estimation based on quaternion derivation and the gradient descent algorithm are presented in the paper. Experimental results show that the sensor data acquisition and filtering consumes about 6.2ms, and the algorithm consumes about 0.96ms with the step size =0.8 in the 72MHz clock frequency and soft IIC(Inter-integrated circuit). The update frequency of attitude data up to 100Hz can meet real-time requirements. Statistics shows that the static mean absolute errors of pitch and roll are below 1.5 and the mean absolute errors of yaw are below 2.9. The mean absolute error of pitch, roll and yaw will be increased by 1~2 under the condition of micro-vibration of low frequency. It is indicated that the attitude estimation fusion algorithm and sensors calibration method are practical and effective which could provide accurate attitude data for precision flight control and variable operations implementation of agricultural small UAV.
-
[1] Sugiura R, Noguchi N, Ishii K, Shibata Y, Toriyama K. Field information system using an agricultural helicopter towards precision farming. Biosystems Engineering, 2005, 90(4):229-240 [2] [2] Primicerio J, Gennaro S F D, Fiorillo E, Genesio L, Lugato E, Matese A, Vaccari F P. A flexible unmanned aerial vehicle for precision agriculture. Precision Agriculture, 2012, 13(4):517-523 [3] Li Yao-Ming, Xu Li-Zhang, Xiang Zhong-Ping, Deng Ling-Li. Research advances of rice planting mechanization in Japan. Transactions of the Chinese Society of Agricultural Engineering, 2008, 21(11):182-185(李耀明, 徐立章, 向忠平, 邓玲黎. 日本水稻种植机械化技术的最新研究进展. 农业工程学报, 2005, 21(11):182-185) [4] Li Ji-Yu, Zhang Tie-Min, Peng Xiao-Dong, Ma Xue-Dong, Ke Zhou-Zhi, Zeng Xu-Sheng. Collection platform of field location information based on four-rotor aircraft. Transactions of the Chinese Society for Agricultural Machinery, 2013, 44(5):202-206(李继宇, 张铁民, 彭孝东, 马学东, 柯宙志, 曾旭升. 四旋翼飞行器农田位置信息采集平台设计与实验. 农业机械学报, 2013, 44(5):202-206) [5] Xue Xin-Yu, Lan Yu-Bin. Agricultural aviation applications in USA. Transactions of the Chinese Society for Agricultural Machinery, 2013, 44(5):194-201(薛新宇, 兰玉彬. 美国农业航空技术现状和发展趋势分析. 农业机械学报, 2013, 44(5):194-201) [6] [6] Long D, Tobias F, Chen Y Q. A data fusion system for attitude estimation of low-cost miniature UAVs. Journal of Intelligent and Robotic Systems, 2012, 65(1):621-635 [7] [7] Madgwick S O H, Harrison A J L, Vaidyanathan R H. Estimation of IMU and MARG orientation using a gradient descent algorithm. In:Proceedings of the 2011 IEEE International Conference on Rehabilitation Robotics(ICORR). Switzerland:IEEE, 2011. 1-7 [8] [8] Mahony R, Hamel T, Pflimin J M. Nonlinear complementary filters on the special orthogonal group. IEEE Transactions on Automatic Control, 2008, 53(5):1203-1218 [9] Luo-Bin, Wang Yong-Tian, Liu Yue. Multi-sensor data fusion for optical tracking of head pose. Acta Automatica Sinica, 2010, 36(9):1239-1249(罗斌, 王涌天, 刘越. 光学头部姿态跟踪的多传感器数据融合研究. 自动化学报, 2010, 36(9):1239-1249) [10] Peng Xiao-Dong, Zhang Tie-Min, Li Ji-Yu, Yan Guo-Qi. Research on field calibration of three-axis MEMS digital acceleration. Journal of Vibration, Measurement Diagnosis, 2014, 34(3):544-548(彭孝东, 张铁民, 李继宇, 闫国琦. 三轴数字MEMS加速度计现场标定方法. 振动、测试与诊断, 2014, 34(3):544-548) [11] Li Rong-Bing, Liu Jian-Ye, Sun Yong-Rong. MEMS-IMU configuration and its inertial sensors' calibration for installation errors. Journal of Chinese Inertial Technology, 2007, 15(5):526-530(李荣冰, 刘建业, 孙永荣. MEMS-IMU构型设计及惯性器件安装误差标定方法. 中国惯性技术学报, 2007, 15(5):526-530) [12] Li Jie, Hong Hui-Hui, Zhang Wen-Dong. Research on calibration techniques for MEMS-micro inertial measurement unit. Chinese Journal of Sensors and Actuators, 2008, 21(7):1170-1173(李杰, 洪惠惠, 张文栋. MEMS微惯性测量组合标定技术研究. 传感技术学报, 2008, 21(7):1170-1173 [13] Guo Peng-Fei, Ren Zhang, Qiu Hai-Tao, Ding Xin-Chun. Twelve-position calibrating method without north reference for magnetic compass. Journal of Chinese Inertial Technology, 2007, 15(5):598-601(郭鹏飞, 任章, 邱海韬, 丁新春. 一种十二位置不对北的磁罗盘标定方法. 中国惯性技术学报, 2007, 15(5):598-601) [14] Peng Xiao-Dong, Chen Yu, Li Ji-Yu, Yan Guo-Qi, Zhang Tie-Min. Study on calibration method of MEMS 3-axis digital gyroscope. Transducer and Microsystem Technologies, 2013, 32(6):63-65(彭孝东, 陈瑜, 李继宇, 闫国琦, 张铁民. MEMS三轴数字陀螺仪标定方法研究. 传感器与微系统, 2013, 32(6):63-65) [15] Chao Min, Jiang Dong-Fang, Wen Cai-Hong. Error analysis and calibration of magnetic compass. Chinese Journal of Sensors and Actuators, 2010, 23(4):525-532(晁敏, 蒋东方, 文彩虹. 磁罗盘误差分析与校准. 传感技术学报, 2010, 23(4):525-532) [16] Wu Jie, Yan Jian-Guo. Research on attitude estimation algorithm based on Kalman filter. Computer Simulation, 2012, 29(2):54-57(吴杰, 闫建国. 基于修正的卡尔曼滤波的姿态估计算法研究. 计算机仿真, 2012, 29(2):54-57) [17] Zhang Rong-Hui, Jia Hong-Guang, Chen Tao, Zhang Yue. Attitude solution for strapdown inertial navigation system based on quaternion algorithm. Optics and Precision Engineering, 2008, 16(10):1964-1970(张荣辉, 贾宏光, 陈涛, 张跃. 基于四元数法的捷联式惯性导航系统的姿态解算. 光学精密工程, 2008, 16(10):1964-1970) [18] Qin Yong-Yuan. Inertial Navigation. BeiJing:Science Press, 2006.(秦永元. 惯性导航. 北京:科学出版社, 2006.) [19] Huang Xu, Wang Chang-Hong. The design of an extended Kalman filter for magnetometer and MEMS gyroscope/accelerometer integrated attitude determination. Journal of Natural Science of Heilongjiang University, 2005, 22(4):454-458(黄旭, 王常虹. 磁强计和微机械陀螺/加速度计组合定姿的扩展卡尔曼滤波器设计. 黑龙江大学自然科学学报, 2005, 22(4):454-458) [20] Ge Quan-Bo, Li Wen-Bin, Sun Ruo-Yu, Xu Zi. Centralized fusion algorithms based on EKF for multisensor non-linear systems. Acta Automatica Sinica, 2012, 39(6):816-825(葛泉波, 李文斌, 孙若愚, 徐姿. 基于EKF的集中式融合估计研究. 自动化学报, 2012, 39(6):816-825) [21] Guo Xiao-Hong, Yang Zhong, Chen Zhe, Yang Cheng-Shun, Gong Hua-Jun. Application of extended Kalman filter and complementary filter in attitude estimation. Transducer and Microsystem Technologies, 2011, 30(11):149-152(郭晓鸿, 杨忠, 陈喆, 杨成顺, 龚华军. EKF和互补滤波器在飞行姿态确定中的应用. 传感器与微系统, 2011, 30(11):149-152) [22] Xin Qi, Shi Zhong-Ke. Flight attitude determination base on multiple measurements. Flight Dynamics, 2012, 30(6):527-531(辛琪, 史忠科. 基于多源信息的飞行姿态估计方法. 飞行力学, 2012, 30(6):527-531) [23] Liang Yan-De, Cheng Min, He Fu-Ben, Li Hang. Flight attitude determination base on multiple measurements. Flight Dynamics, 2011, 30(11):56-58(梁延德, 程敏, 何福本, 李航. 基于多源信息的飞行姿态估计方法. 飞行力学, 2011, 30(11):56-58) [24] Zhang Hao, Ren Qian. Data fusion method of quad-rotor aircraft attitude measurement system. Ordnance Industry Automation, 2013, 32(1):28-31(张浩, 仁芊. 四旋翼飞行器航姿测量系统的数据融合方法. 兵工自动化, 2013, 32(1):28-31) [25] Wu Qi-Zong, Hou Fu-Jun. Operations Research and Optimization Techniques. BeiJing:China Machine Press, 2013.(吴祈宗, 侯福均. 运筹学与最优化方法. 北京:机械工业出版社, 2013.) 期刊类型引用(51)
1. 张欣,韩志,陈春雷,罗哉,江文松. 基于多传感器融合的轨检车构架位姿测量方法. 计量学报. 2024(01): 18-23 . 百度学术
2. 陈汉泉,陶杰,鲁仁全. 多传感器数据融合下的无人机高度扩张状态观测器设计. 控制理论与应用. 2024(12): 2365-2373 . 百度学术
3. 孙文涵,鲜斌. 基于方位信息的无人机编队控制设计与验证. 控制理论与应用. 2023(09): 1537-1546 . 百度学术
4. 沈跃,孙志伟,沈亚运,张大海,钱鹏,刘慧. 直线型植保无人机航姿UKF两级估计算法. 农业机械学报. 2022(09): 151-159 . 百度学术
5. 刘少兵,纪新春,魏东岩,李雯,刘宇欣,巩应奎,袁洪. 基于磁干扰补偿的智能手机车载AHRS算法. 传感技术学报. 2022(12): 1643-1649 . 百度学术
6. 张子容. 基于单片机控制的农用无人机导航研究. 农机化研究. 2020(03): 240-244+248 . 百度学术
7. 曾成,李文霄. 组合导航中磁强计干扰估计与补偿方法. 燕山大学学报. 2020(01): 49-54 . 百度学术
8. 郭威,吴允平,王廷银. MEMS四元数卡尔曼滤波算法的电梯姿态估计. 计算机系统应用. 2020(03): 246-252 . 百度学术
9. 施奇杰,曹大平. 基于扩展卡尔曼滤波的姿态估计算法. 信息技术. 2020(06): 78-83 . 百度学术
10. 叶坤涛,殷超,吉俄木沙,朱宝仪,吴焱森. IIC总线IP核的改进研究. 电子器件. 2020(03): 628-634 . 百度学术
11. 徐利锋,杨中柱,黄祖胜,丁维龙. 结合混合粒子群算法的植保无人机航线设计方法. 小型微型计算机系统. 2020(09): 1826-1832 . 百度学术
12. 李斌飞,崔世钢,施国英,祖林禄. 基于无迹卡尔曼滤波的农用无人机定位研究. 中国农机化学报. 2020(09): 156-161 . 百度学术
13. 徐鑫,赵鹤鸣. 基于改进型卡尔曼滤波的运动载体姿态估计. 传感技术学报. 2020(09): 1279-1284 . 百度学术
14. 陆欣,刘忠,谢宇,何爱林. 附加运动约束的MEMS惯性传感器融合定姿算法. 海军工程大学学报. 2019(01): 100-106 . 百度学术
15. 陈卓,任久春,朱谦. 基于梯度下降的自适应姿态融合算法. 传感器与微系统. 2019(03): 124-126 . 百度学术
16. 王鑫,张丽杰. 基于自适应CKF的姿态数据融合算法. 电子测量技术. 2019(03): 11-15 . 百度学术
17. 张煌军,徐雪松,张文清,刘瑞. 基于SCKF的4旋翼无人机的姿态估计. 江西师范大学学报(自然科学版). 2019(02): 154-159 . 百度学术
18. 陈卓,任久春,朱谦. 基于倾斜测流的浮标海流计原型设计与实现. 自动化仪表. 2019(04): 85-88 . 百度学术
19. 张煌军,徐雪松,罗伟,刘瑞. 基于平方根容积卡尔曼滤波的四旋翼无人机的姿态解算. 科学技术与工程. 2019(12): 248-253 . 百度学术
20. 韩晓微,岳高峰,崔建江,汤浩泽. 四旋翼无人机陀螺阵列数据融合算法. 仪器仪表学报. 2019(08): 213-221 . 百度学术
21. 李乃星,曹培培,刘圣起,于欢. 载体机动和磁干扰环境下的航姿算法研究. 制导与引信. 2019(03): 51-56+60 . 百度学术
22. 陆欣,刘忠,张宏欣,张宏伟. 三轴磁强计及其非对准误差校正算法. 华中科技大学学报(自然科学版). 2018(08): 83-88 . 百度学术
23. 张小俊,徐子寒,杨士鹏. 机器人姿态解算算法研究. 机械设计与制造. 2018(06): 246-249 . 百度学术
24. 吴开华,孙学超,张竞成,陈丰农. 基于高度融合的植保无人机仿地飞行方法研究. 农业机械学报. 2018(06): 17-23 . 百度学术
25. 李成勇. STM32嵌入式平台下四旋翼飞行器模型研究. 无线电工程. 2018(06): 453-456 . 百度学术
26. 李众民. 民族体育教学中的特殊姿态校对视觉监控平台设计. 现代电子技术. 2018(01): 178-181+186 . 百度学术
27. 王翔,白茹,崔晓阳,吴涛,钱正洪. 姿态检测系统中的数据融合算法综述. 电子科技. 2018(10): 39-43 . 百度学术
28. 黄艳国,樊云东,陈自如. 基于信息融合的四旋翼飞行器的姿态解算. 计算机仿真. 2018(09): 59-64 . 百度学术
29. 王诗章,鲜斌,杨森. 无人机吊挂飞行系统的减摆控制设计. 自动化学报. 2018(10): 1771-1780 . 本站查看
30. 曾聪,章政,王龙. 基于共轭梯度的EKF姿态估计算法. 计算机工程与设计. 2018(10): 3118-3124+3161 . 百度学术
31. 张宏欣,周穗华,张伽伟. 三轴磁强计实时自校正算法. 电子学报. 2017(07): 1750-1757 . 百度学术
32. 李增彦,李小民,刘秋生,周兆英. 巡飞弹空中自适应快速初始姿态估计. 光学精密工程. 2017(02): 493-501 . 百度学术
33. 张新星. 农用无人机智能植保系统设计. 时代农机. 2017(03): 85-86 . 百度学术
34. 罗君,刚煜,唐立军. 基于混合滤波的四轴飞行器的姿态解算. 测控技术. 2017(07): 5-8+12 . 百度学术
35. 王龙,章政,王立. 改进扩展卡尔曼滤波的四旋翼姿态估计算法. 计算机应用. 2017(04): 1122-1128 . 百度学术
36. 王守华,邓桂辉,纪元法,孙希延. 基于组合滤波的低成本飞行器姿态解算算法. 计算机应用. 2017(05): 1507-1511 . 百度学术
37. 何磊,罗兵,吴文启. 基于DGPS航迹偏差的多旋翼无人机磁干扰检测技术研究. 计算机测量与控制. 2017(10): 39-42+47 . 百度学术
38. 孟凡琨,巨永锋,文常保. 随机网格回归Monte Carlo双UAVs最优目标协调跟踪. 计算机科学与探索. 2017(03): 450-458 . 百度学术
39. 陈阳,宋昌盛,吴智铧. 基于STM32便携式航向姿态参考系统设计. 龙岩学院学报. 2017(02): 58-62+79 . 百度学术
40. 张宏欣,周穗华,张伽伟. 磁偶极子跟踪的渐进贝叶斯滤波方法. 自动化学报. 2017(05): 822-834 . 本站查看
41. 章政,王龙,曾聪. 运动加速度抑制的动态步长梯度下降姿态解算算法. 信息与控制. 2017(02): 136-143 . 百度学术
42. 陈莹超,熊继军,周兆英,谭秋林,李增彦. 基于短期数据融合的自主三维导航系统. 仪表技术与传感器. 2017(01): 130-133+138 . 百度学术
43. 南余荣,宛冬晴,潘帅,姚亚楠. 二阶互补滤波姿态求解器的设计. 浙江工业大学学报. 2017(04): 416-420 . 百度学术
44. 陈建翔,万子敬,王向军. 基于四元数的低成本姿态测量系统设计. 传感技术学报. 2016(05): 706-710 . 百度学术
45. 万幼川,郑顺义,柯涛. 新型航空传感器快速处理方法探讨. 地理空间信息. 2016(12): 1-3+10+7 . 百度学术
46. 左国玉,于双悦,龚道雄. 遥操作护理机器人系统的操作者姿态解算方法研究. 自动化学报. 2016(12): 1839-1848 . 本站查看
47. 尚剑宇,邓志红,付梦印,汪顺亭. 制导炮弹转速测量技术研究进展与展望. 自动化学报. 2016(11): 1620-1629 . 本站查看
48. 张昆,张铁民,廖贻泳,兰玉彬. 基于Labview的无人机飞行状态实时监测评估系统设计. 农业工程学报. 2016(18): 183-189 . 百度学术
49. 王真,高凤岐,高敏,高伟伟. 旋转矢量多迭代捷联姿态解算误差补偿算法. 中国测试. 2016(08): 113-117 . 百度学术
50. 张春草,张剑波,朱国力. 基于多传感器融合的TBM姿态角测量方法. 华中科技大学学报(自然科学版). 2015(12): 48-51+81 . 百度学术
51. 张昆,张铁民,廖贻泳,庄晓霖,李晟华,兰玉彬. 基于Labview的遥控无人机操控测试系统(英文). 农业工程学报. 2015(S2): 11-16 . 百度学术
其他类型引用(58)
-
计量
- 文章访问数: 2133
- HTML全文浏览量: 106
- PDF下载量: 2040
- 被引次数: 109