构建了基于MEMS陀螺仪、加速度计及磁强计的姿态测量系统。研究了基于四元数的扩展卡尔曼滤波算法。通过四元数微分方程和陀螺噪声误差建立了卡尔曼状态方程, 利用加速度计和磁强计的输出数据, 采用梯度下降法计算出测量四元数。用从实际的惯性测量单元采集的数据对滤波器进行测试, 结果证明航向角在通过卡尔曼滤波器后, 有效的抑制了姿态角的发散, 使得航向角姿态角误差小于3°, 该算法能够实现小型无人机的高精度姿态解算。
Attitude measurement system is constructed based on MEMS gyroscope, accelerometer and magnetometer, and the extended Kalman filtering algorithm is studied based on quaternion.Kalman state equation is established through the quaternion differential equation and the gyro noise error.Using the output data of accelerometer and magnetometer, measurement quaternion is worked out with the gradient descent method.The filter is tested with data collected from actual inertial measurement unit, and the results prove that heading angle through Kalman filter can effectively inhibit the divergence of attitude angle, with the heading angle error less than 3°.The algorithm can achieve high precision attitude algorithm of small drone.
[1]吴杰, 闫建国.基于修正的卡尔曼滤波的姿态估计算法研究[J].计算机仿真, 2012, 29(2):54-57.
[2]刘兴川, 张盛, 李丽哲, 等.基于四元数的MARG传感器姿态测量算法[J].清华大学学报(自然科学版), 2012, 52(5):627-631.
[3]薛亮, 姜澄宇, 常洪龙, 等.基于状态约束的MIMU/磁强计组合姿态估计滤波算法[J].中国惯性技术学报, 2009, 17(3):338-343.
[4]Eugenio Denti, Roberto Galatolo, Francesco Schettini.An AHRS based on a kalman filter for the integration of inertial, magnetometric and GPS data[C].27th Internation Congress of the Aeronautical Sciences, 2010.
[5]Cordova Alarcon J R, Rodriguez Cortes H, Vicente Vivas E.Extended kalman filter tuning in attitude estimation from inertial and magnetic field measurements[C].Electrical Engineering, Computing Science and Automatic Control, CCE, 2009 6th International Conference on, 2009.
[6]Sebastian O H.Madgwick, Andrew J L.Harrison, Ravi Vaidyanathan.Estimation of IMU and MARG orientation using a gradient descent algorithm[C].IEEE International Conference on Rehabilitation Robotics, 2011.
[7]黄旭, 王常虹, 伊国兴, 等.利用磁强计及微机械加速度计和陀螺的姿态估计扩展卡尔曼滤波器[J].中国惯性技术学报, 2005, 13(2):27-34.
[8]杨淑洁, 曾庆双, 伊国兴.低成本无人机姿态测量系统研究[J].传感器与微系统, 2012, 31(2):15-22.
