[发明专利]一种四元数高斯粒子滤波移动机器人姿态解算方法

摘要

本发明提出的一种四元数高斯粒子滤波移动机器人姿态解算方法，属于数字滤波和多传感器数据融合技术领域，主要作用于移动机器人姿态解算系统中，使之获得准确的姿态角便于进行后续姿态控制。该方法采用陀螺仪加速度计获得姿态数据，构建系统四元数状态方程及非线性量测方程，并采用高斯粒子滤波直接处理非线性的量测方程，获得最优四元数估计，根据四元数姿态变换矩阵实时解算出最优姿态角。本发明不仅能够实现全姿态解算，而且能明显抑制噪声和漂移误差，获得精确姿态角，同时降低粒子滤波的计算量，使算法满足系统实时性要求。

主权项

1.一种四元数高斯粒子滤波移动机器人姿态解算方法，其特征在于，具体包括以下步骤：步骤1：选取移动机器人姿态角及相应的参考坐标系，确定姿态变换矩阵；步骤2：选取四元数作为系统状态量，根据初始姿态角获得初始四元数，利用陀螺仪输出数据建立四元数微分方程作为姿态解算系统的线性状态方程；步骤3：根据加速度计三轴输出数据与重力加速度之间的关系建立系统非线性观测方程；步骤4：根据系统的状态方程、观测方程及四元数与姿态角之间的变换关系，利用高斯粒子滤波框架，建立四元数高斯粒子滤波算法流程，获取四元数的估计值；步骤5：对四元数的估计值进行归一化处理，通过归一化四元数的值及姿态变换矩阵解算出滤波后的姿态角。