全向AGV的优化互补滤波姿态解算

Attitude Algorithm of Optimal Complementary Filter for Omnidirectional AGV

针对单一的MEMS陀螺仪无法解决本身的易发散和磁力计易受磁场干扰导致引入新的噪声,而带来的姿态估计不精确的问题,提出了一种基于全向AGV的优化互补滤波的姿态解算方法。偏航角不参与四元数解算,在水平姿态角四元数解算的基础上,利用共轭梯度法减小陀螺仪的漂移误差。再引进经过椭球修正后的磁力计数据作为观测量,与偏航角进行一阶互补滤波融合,并确定加权因子。搭建了基于STM32和MEMS传感器为核心的全向AGV实验平台,实验结果表明,该方法能有效抑制陀螺仪易发散和磁力计易受干扰的问题,提高姿态解算的精确性,使姿态解算具有良好的动态和静态性能,同时保证了系统的稳定性。

As a single MEMS gyroscope can not solve the problems that it is easy to diverge and the magnetometer is easy to be interfered by the magnetic field,which lead to the introduction of new noise and the inaccurate attitude estimation,a new attitude estimation method based on omnidirectional AGV is proposed in this paper.According to this method,the yaw angle does not participate in the quaternion solution;on the basis of the quaternion algorithm of the horizontal attitude angle,the conjugate gradient method is used to reduce the drift error of the gyroscope.Then,the data of magnetometer modified by ellipsoid is introduced as the observation data,which is fused with yaw angle by first-order complementary filtering,and the weighting factor is determined.An omnidirectional AGV experiment platform based on STM32 and MEMS sensor is built up.The experimental results show that this method can effectively suppress the divergence of gyroscope and the interference of magnetometer,improve the accuracy of attitude algorithm,make the attitude algorithm have good dynamic and static performances,and ensure the stability of the system.