CN109990776B

CN109990776B - 一种姿态测量方法及装置

Info

Publication number: CN109990776B
Application number: CN201910293397.0A
Authority: CN
Inventors: 庞学亮; 殷超; 向前
Original assignee: Wuhan Shenhailan Technology Co ltd
Current assignee: Wuhan Shenhailan Technology Co ltd
Priority date: 2019-04-12
Filing date: 2019-04-12
Publication date: 2021-09-24
Anticipated expiration: 2039-04-12
Also published as: CN109990776A

Abstract

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

Description

一种姿态测量方法及装置

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

进一步地，还包括：

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

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

进一步地，还包括：

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

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

本发明中提供的一个或多个技术方案，至少具有如下技术效果或优点：

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

附图说明

图1为本发明实施例提供的姿态测量方法的流程图；

图2为本发明实施例提供的姿态测量方法的原理图；

图3为本发明实施例提供的姿态测量装置的模块图；

图4为基于本发明实施例提供的姿态测量装置构建的电子罗盘的结构框图。

具体实施方式

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

本发明实施例中的技术方案为解决上述问题，总体思路如下：

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

为了更好地理解上述技术方案，下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。

参见图1和图2，本发明实施例提供的姿态测量方法，包括：

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

具体地，通过三轴加速度传感器获取俯仰角和横滚角，通过三轴磁传感器获取地磁三分量，通过MEMS角度传感器获取角度变化量。

在本实施例中，三轴加速度传感器选用ADXL355，为高精度三轴加速度传感器。三轴磁传感器选用SEN-XY三个探头和PNI12927驱动芯片。MEMS角度传感器为三轴惯性角加速度传感器，选用MPU6050，其精度在0.01度。

步骤S120：对俯仰角、横滚角、地磁三分量解算出姿态四元数；

具体地，采用高斯法等解算出姿态四元数，作为Kalman滤波(Kalman filtering，卡尔曼滤波)的观测数据。

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

为了使最终得到的姿态结果更加准确，在通过姿态四元数和角度变化量进行滤波之前，还包括：

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

具体地，将三轴加速度传感器和三轴磁传感器的采集数据进行互补算法滤波对MEMS惯导的输出数据进行校正，以提高MEMS角度传感器的解算精度。

对步骤S130进行具体说明，通过姿态四元数和角度变化量进行滤波，包括：

通过姿态四元数和校正后的角度变化量进行滤波，即采用四阶龙格法解算出四元数，将该数据作为Kalman滤波的状态值，通过Kalman滤波，实现四元数输出估计。通过该滤波处理，可以进行降噪，同时还可以避免外部磁场的干扰。

步骤S140：对四元数输出数据进行角度变换，得到姿态结果。

为了进一步提高最终得到的姿态结果的精度，在得到四元数输出数据之后，还包括：

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

具体地，将校正后的由MEMS角度传感器输出的数据通过四阶龙格法解算出的四元数作为二次Kalman滤波的状态值，将第一次Kalman滤波输出的四元数作为二次Kalman滤波的观测值，再进行一次Kalman滤波，以得到更高精度的四元数。

在这种情况下，对四元数输出数据进行角度变换，得到姿态结果，包括：

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

具体地，将四元数转换为方位角、俯仰角和横滚角，实现高精度姿态测量。

参见图3和图4，本发明实施例提供的姿态测量装置，包括：

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

具体地，数据采集模块100包括：三轴加速度传感器、三轴磁传感器和MEMS角度传感器。通过三轴加速度传感器获取俯仰角和横滚角，通过三轴磁传感器获取地磁三分量，通过MEMS角度传感器获取角度变化量。

解算模块200，用于对俯仰角、横滚角、地磁三分量解算出姿态四元数；具体地，采用高斯法等解算出姿态四元数，作为Kalman滤波的观测数据。

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

为了使最终得到的姿态结果更加准确，还包括：

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

滤波模块300，具体用于通过姿态四元数和校正后的角度变化量进行滤波，即采用四阶龙格法解算出四元数，将该数据作为Kalman滤波的状态值，通过Kalman滤波，实现四元数输出估计。通过该滤波处理，可以进行降噪，同时还可以避免外部磁场的干扰。

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

为了进一步提高最终得到的姿态结果的精度，还包括：

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

在这种情况下，数据变换模块400，具体用于对二次滤波后的四元数进行角度变换，得到姿态结果。

在本实施例中，解算模块200、滤波模块300和数据变换模块400在单片机中，并通过稳压芯片对单片机供电，得到的姿态结果通过串口芯片输出。其中，单片机选用STM32L152，其功耗低、稳定性高；稳压芯片采用LP2980，提供3.3V电源；串口芯片采用SP3220芯片，为RS232输出。

【技术效果】

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

2、在通过姿态四元数和角度变化量进行滤波之前，还俯仰角、横滚角、地磁三分量进行互补滤波，对角度变化量进行校正，使得最终得到的姿态结果更加准确。

3、本发明实施例通过二次滤波，进一步提高了最终得到的姿态结果的精度。

本发明实施例通过提供一种姿态测量方法及装置，对平台的俯仰角、横滚角、地磁三分量和角度变化量进行了测量，并将测量到的俯仰角、横滚角、地磁三分量和角度变化量进行了数据的融合处理，解决了现有技术中存在磁场干扰和晃动的问题，其惯导单元采用MEMS集成芯片，其价格低廉、使用方便。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims

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

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

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

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

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

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

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

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

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

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

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

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

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

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

还包括：

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

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

还包括：

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

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