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

说明书

本发明公开了一种姿态测量方法及装置。先获取俯仰角、横滚角、地磁三分量和角度变化量，再对俯仰角、横滚角、地磁三分量解算出姿态四元数，接着通过姿态四元数和角度变化量进行滤波，得到四元数输出数据；最后对四元数输出数据进行角度变换，得到姿态结果。本发明通过获得的角度变化量对获得的俯仰角、横滚角、地磁三分量进行补偿，避免了磁性干扰和高频晃动，提高了测量精度和实现了动态测量。

技术领域

本发明涉及姿态测量技术领域，尤其涉及一种姿态测量方法及装置。

背景技术

电子罗盘，又称数字罗盘，在现代技术条件中，电子罗盘作为导航仪器或姿态传感器已被广泛应用。电子罗盘与传统指针式和平衡架结构罗盘相比，其能耗低、体积小、重量轻、精度高、可微型化，其输出信号通过处理可以实现数码显示，不仅可以用来指向，其数字信号还可直接送到自动舵，控制船舶的操纵。

由于电子罗盘是利用地磁场测量来测量平台方位的，因而其会受到外界干扰磁场的影响，从而影响其的测量精度。另外，需要通过倾角仪来测量方位角和俯仰角，进行非水平状态补偿，而倾角仪选用加速度传感器，这就决定了这种电子罗盘在自生晃动情况下，其干扰输出很大，因而只能用于静态测量。

发明内容

本发明通过提供一种姿态测量方法及装置，解决了现有技术中测量精度低和只能用于静态测量的技术问题，实现了提高测量精度和实现动态测量的技术效果。

本发明提供了一种姿态测量方法，包括：

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

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

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

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

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

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

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

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

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

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

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

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

本发明还提供了一种姿态测量装置，包括：

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

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

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

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

进一步地，还包括：

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

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

进一步地，还包括：