[发明专利]一种姿态测量方法及装置

权利要求书

1.一种姿态测量方法，其特征在于，包括：

获取俯仰角、横滚角、地磁三分量和角度变化量；

对所述俯仰角、横滚角、地磁三分量解算出姿态四元数；

通过所述姿态四元数和所述角度变化量进行滤波，得到四元数输出数据；

对所述四元数输出数据进行角度变换，得到姿态结果；

通过获得的角度变化量对获得的俯仰角、横滚角、地磁三分量进行补偿，避免了磁性干扰和高频晃动；

在所述通过所述姿态四元数和所述角度变化量进行滤波之前，还包括：

将所述俯仰角、横滚角、地磁三分量进行互补滤波，对所述角度变化量进行校正；

所述通过所述姿态四元数和所述角度变化量进行滤波，包括：

通过所述姿态四元数和所述校正后的角度变化量进行滤波；

在所述得到四元数输出数据之后，还包括：

将所述四元数输出数据和所述校正后的角度变化量进行二次滤波，得到二次滤波后的四元数；

所述对所述四元数输出数据进行角度变换，得到姿态结果，包括：

对所述二次滤波后的四元数进行角度变换，得到姿态结果；

通过二次滤波，进一步提高了最终得到的姿态结果的精度。

2.一种姿态测量装置，其特征在于，包括：

数据采集模块，用于获取俯仰角、横滚角、地磁三分量和角度变化量；

解算模块，用于对所述俯仰角、横滚角、地磁三分量解算出姿态四元数；

滤波模块，用于通过所述姿态四元数和所述角度变化量进行滤波，得到四元数输出数据；

数据变换模块，用于对所述四元数输出数据进行角度变换，得到姿态结果；

通过获得的角度变化量对获得的俯仰角、横滚角、地磁三分量进行补偿，避免了磁性干扰和高频晃动；

还包括：

校正模块，用于将所述俯仰角、横滚角、地磁三分量进行互补滤波，对所述角度变化量进行校正；

所述滤波模块，具体用于通过所述姿态四元数和所述校正后的角度变化量进行滤波；

还包括：

二次滤波模块，用于将所述四元数输出数据和所述校正后的角度变化量进行二次滤波，得到二次滤波后的四元数；

所述数据变换模块，具体用于对所述二次滤波后的四元数进行角度变换，得到姿态结果；

通过二次滤波，进一步提高了最终得到的姿态结果的精度。