基于传感器校正与融合的农用小型无人机姿态估计算法

Attitude Estimation Algorithm of Agricultural Small-UAV Based on Sensors Fusion and Calibration

实时姿态信息获取是运用农用小型无人机(Unmanned aerial vehicle,UAV)进行变量作业控制的重要环节,本文采用STM32单片机、微机电系统(Micro-electro mechanical system,MEMS)加速度计、陀螺仪、磁强计和无线收发模块设计出农用小型无人机姿态实时解算系统,文中对三轴数字传感器的校正方法以及基于四元数和梯度下降法的多传感器融合姿态估计做了详细地介绍与推导.结果表明,在72 MHz时钟频率下模拟集成电路总线(Inter-integrated circuit,IIC)传感器数据采集及滤波消耗6.2 ms,迭代步长β取0.8,一次姿态解算消耗约0.96 ms,数据更新频率可达100 Hz,能满足实时性要求.经测试在静态时俯仰角和翻滚角输出的平均绝对误差小于1.5,偏航角平均绝对误差小于2.9,小幅震动条件下的俯仰角、翻滚角和偏航角平均绝对误差增加1～2左右.这表明该传感器校正方法与姿态融合算法实用有效,能为农用小型无人机的飞行控制与变量作业实施提供准确的姿态数据.

The real-time attitude information of agricultural small unmanned aerial vehicle （UAV） is a key factor to the de-cision and operation for variable rate program in precision agri-culture. 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 sen-sors, and wireless transceiver module nRF24L01a. Detailed de-scription 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 sen-sor data acquisition and filtering consumes about 6.2 ms, and the algorithm consumes about 0.96 ms with the step size β =0.8 in the 72 MHz clock frequency and soft IIC （Inter-integrated cir-cuit）. The update frequency of attitude data up to 100 Hz 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 cali-bration method are practical and effective which could provide accurate attitude data for precision flight control and variable operations implementation of agricultural small UAV.