基于CPF-EKF算法的大载荷植保无人机姿态解算方法

Attitude Calculation Method Based on CPF-EKF for Large Load Plant Protection UAV

为了解决传统人工喷洒农药的不足,更高效地进行病虫害的防治,设计了基于八轴十六旋翼无人机的农药喷洒系统,实现了农药的机载喷洒功能。使用共轴双桨和旋翼模块的倾斜配置,对八轴多旋翼无人机进行结构改进,提高了系统的安全性与可靠性。整个系统满载10 kg,喷洒飞行速度可到达5 m/s,飞行时间超过10 min。针对传统扩展卡尔曼滤波(Extended Kalman filter,EKF)姿态解算方法无法满足大载荷无人机强振动条件下的工作要求,导致姿态角解算精度不高,并且容易导致姿态角发散的问题,提出了基于20维状态量的CPF-EKF算法,额外引入了陀螺仪、加速度计和磁力计偏置误差作为状态量,使三轴姿态角的最优估计值更加准确,并且引入互补滤波(Complement filter,CPF)检测模块,当检测到EKF有发散趋势时,对EKF进行复位,从而简单高效地避免了EKF发散。采用实际飞行数据对算法进行验证,静态试验表明,该算法滚转角和俯仰角精度为±0.05°,偏航角精度为±0.2°。动态试验中以MTi传感器输出为参考,CPF-EKF在姿态解算过程中出现复位,三轴姿态角准确跟踪并未发散,并且动态精度与MTi相当,滚转角、俯仰角精度为±0.1°,偏航角精度为±0.5°,并且算法具有良好的实时性,证明了该算法的有效性。

In order to solve the shortcomings of traditional artificial spraying pesticides and more efficient prevention and treatment of diseases and pests, a pesticide spraying system based on sixteen-rotor unmanned aerial vehicle （UAV） was designed. The sixteen-rotor UAV＇ s basic structure and attitude calculation method were explained. The whole system was full of 10 kg, cruising speed can reach 5 m/s, and the flight time was more than 10 min. The traditional extended Kalman filter （EKF） attitude calculation method cannot meet the work requirements under the strong vibration condition of the large load UAV. The attitude angle calculation accuracy was not high and the attitude angle divergence was easily caused. A CPF - EKF algorithm based on 20 dimensional state quantity was proposed. The bias error of gyroscopes, aceelerometers and magnetometers were added as the state quantity, which made the optimal estimation of the attitude angle of the three axes more accurate. Complementary filtering （CPF） was treated as an EKF detection module. When the EKF had the divergence trend, the EKF was reset, thus the EKF divergence was avoided simply and efficiently. Using actual flight data to verify the algorithm, the static experiment showed that the precision of the roll angle and the pitch angle of the algorithm were ± 0.05°, the precision of the yaw angle was± 0.2°. The dynamic experiment showed that the precision of the roll angle and the pitch angle of the algorithm were ± 0. 1°, the precision of the yaw angle was ± 0.5 ° , and the algorithm had good real-time performance.