一种改进误差四元数卡尔曼滤波机器人姿态解算方法

摘要

本发明是一种改进误差四元数卡尔曼滤波机器人姿态解算方法，主要作用于移动机器人姿态解算系统中，使之获得准确的姿态角便于进行后续姿态控制。该方法采用陀螺仪加速度计获得姿态数据，构建系统误差四元数卡尔曼滤波状态方程及量测方程，并根据非重力加速度判定公式判断是否存在非重力加速度，并自适应补偿非重力加速度对姿态解算带来的影响，然后利用误差四元数卡尔曼滤波器，获得误差四元数更新值，计算得到真实的姿态四元数，根据四元数法进行姿态解算得到姿态角。本发明能够实现全姿态解算，而且能明显抑制噪声、漂移误差和非重力加速度对姿态解算的影响。