基于双级互补滤波的姿态测量算法设计

陀螺仪的漂移、载体的线性加速度和磁场的干扰是影响MARG传感器姿态测量精度的主要原因。针对传统姿态测量算法在磁干扰环境下由于航向角误差导致水平角测量精度降低以及载体线性加速度影响水平角精度的问题，提出了一种基于四 元数的双级互补滤波姿态融合算法。该算法利用加速度计和磁力计测量数据分别对估计四元数进行补偿修正，避免了磁干扰环境下航向角误差对水平姿态测量的影响。同时引入线性加速度误差和磁干扰误差自适应补偿方案，以降低线性加速度与磁干扰的影响，为了验证算法的有效性，进行了静态与动态实验。实验结果表明该姿态测量算法能显著提高姿态测量精度和抗干扰能力，与传统的Mahony算法相比，俯仰/滚动角的测量精度完全不受磁干扰的影响，性能得到了明显的提升。