CN107036589B - 一种用于mems陀螺仪的角度测量系统及其方法 - Google Patents

Abstract

本发明公开了一种用于MEMS陀螺仪的角度测量系统及其方法，其涉及陀螺仪角度测量技术领域。该系统包括功率电源模块、基准电压模块、MEMS陀螺仪芯片、高精度AD转换模块和单片机模块，其中功率电源模块为电路中各芯片和期间提供电源；基准电源模块为MEMS陀螺仪芯片和AD转换模块提供高精度的基准电压；MEMS陀螺仪芯片实现对角速度测量并输出模拟信号；高精度AD转换模块负责将陀螺仪芯片的原始模拟信号转换成数字量，并传输给单片机模块；该方法通过卡尔曼滤波抑制静态情况下的零点漂移，动态旋转时则通过二阶低通滤波器降噪并保证角度跟踪的动态性能，将两种滤波器的优势有机结合，最终实现高精度稳定的角度测量。

Description

一种用于MEMS陀螺仪的角度测量系统及其方法

技术领域

本发明涉及陀螺仪角度测量技术领域，特指一种用于MEMS陀螺仪的角度测量系统及其方法。

背景技术

MEMS陀螺仪是一种微型化、低成本的角速度传感器，它被广泛运用在各种场合，如扫地机器人的航向检测、飞行器的姿态解算等。通常人们通过AD采样实时获取陀螺仪输出的角速度然后对其积分得到实时的角度数据。因此为了确保角度的精确，首先要保证角速度数据是准确可靠的。然而MEMS陀螺仪原始角速度输出中耦合了大量的噪声成分,使角速度输出存在较大的误差。MEMS陀螺仪由于自身漂移以及噪声的问题，很容易使得基于原始数据积分得到的角度数据不可靠，从而影响通过MEMS陀螺仪获取角度的精度，而其中陀螺仪漂移是主要的误差来源之一。因此对陀螺仪的漂移进行补偿在工程应用中是极为必要的。

陀螺仪漂移分为确定性漂移和随机性漂移。确定性漂移通过陀螺测试和标定试验加以补偿，而针对MEMS陀螺仪的随机漂移，经检索发现，现有的方法通常是对陀螺仪的误差数据进行建模然后采用滤波，插值等算法对误差进行估计和补偿以减小误差。但该类技术存在一定的技术缺陷，比如模型参数和噪声统计特性存在误差，会对滤波效果带来影响。

经检索发现，张德明等通过构建MEMS陀螺仪的随机漂移误差的一阶AR模型后利用Kalman滤波对陀螺仪输出进行处理以消除误差。但这类技术存在一定缺陷，例如模型参数和噪声统计特性存在误差会对滤波效果产生影响。而在动态旋转情况下，使用Kalman滤波等手段会使陀螺仪在角速度跟踪上产生较大的滞后性，这会导致用于积分的角速度数据始终滞后于真实的角速度，从而导致最终得到的角度滞后和不准确。

发明内容

本发明的目的在于提出一种用于MEMS陀螺仪的角度测量系统及其方法，能够有效实现基于MEMS陀螺仪的角度测量，同时能够动态调整零点实现对陀螺仪漂移进行有效的抑制，在动态测角时亦保证测角的弱滞后性，具有十分重要的工程意义。

为实现本发明的目的，本发明的具体技术方案如下：

一种用于MEMS陀螺仪的角度测量系统，所述角度测量系统包括功率电源模块、基准电压模块、MEMS陀螺仪芯片、高精度AD转换模块和单片机模块；

所述基准电压模块分别与MEMS陀螺仪芯片和高精度AD转换模块连接；所述功率电源模块分别与MEMS陀螺仪芯片、高精度AD转换模块和单片机模块连接；所述高精度AD转换模块还分别与MEMS陀螺仪芯片和单片机模块连接；所述单片机模块通过串行通信接口与外部设备连接以实现角度数据的对外传送。

所述的功率电源模块包括TPS5430芯片、REG113芯片和TPS7A8001芯片；，其中TPS5430芯片将24V转换6V，REG113芯片将6V转换为提供给单片机和AD模块的3.3V电源，TPS7A8001芯片将6V转换为提供给MEMS陀螺仪芯片的5V电源。

作为本发明的优选技术方案，所述的基准电压模块采用ADR425芯片，输出的5V电压作为AD转换模块和MEMS陀螺仪芯片的基准参考电平。

作为本发明的优选技术方案，所述的MEMS陀螺仪芯片选用ADI生产的ADIRS620，其具有低成本、宽频响、±300°/sec的宽角速度测量范围、抗冲击性能好等优势，实用于低成本的旋转测量场合。

作为本发明的优选技术方案，所述的高精度AD转换模块选用TI生产的16位高精度A/D芯片ADS8327，并通过SPI总线将AD转换后的角速度数据传入单片机模块中。

作为本发明的优选技术方案，所述的单片机模块选用ST的stm32f405RGT6，并通过串行通信外设将算法和程序运行得到的角度数据对外发送。

为实现本发明目的，本发明的另一技术方案具体如下：

一种用于MEMS陀螺仪的角度测量方法，其包括前面所述的一种用于MEMS陀螺仪的角度测量系统，该角度测量方法具体包括以下步骤：

步骤1)，采样MEMS陀螺仪原始输出信号，并经AD转换后得到角速度数据；

步骤2)，对角速度数据进行滑动平均滤波。剔除队列中的极值后再进行均值运算，避免局部异常数据的干扰；

步骤3)，对步骤2)得到的数据进行卡尔曼滤波，滤波结果与陀螺仪静态零点进行窗口阈值比较；

若在阈值内，则判断为静态数据，送入队列用于更新零点，抑制零点漂移；

若在阈值外，则判断为动态旋转数据，则转步骤4)继续处理；

步骤4)，对步骤2)得到的数据进行二阶低通滤波，滤波结果与陀螺仪静态零点作差，并与旋转系数、时间系数相乘转化为角度增量；

步骤5)，利用定时器中断机制对角度增量进行高精度实时积分，得到角度数据并通过串行接口发出；

步骤6)，转至步骤1)，循环执行算法，实现角度值实时更新。

与现有技术相比，本发明的有益效果是：

1.本发明采用基于卡尔曼滤波器与二阶低通器相结合的方法，并采用与零点数据进行窗口比较甄别数据属于静态漂移还是动态数据，利用卡尔曼滤波抑制静态漂移，利用二阶低通滤波保证动态性能，实现了MEMS陀螺仪的高精度角度测量，为基于MEMS角速率陀螺仪的角度测量提供一种新的低成本实现方法。

2.本发明方法对去噪后的角速度数据进行积分，得到长时间稳定可靠，且动态响应良好，滞后性弱的角度数据，静态漂移性能和动态测角的精度和时延是MEMS陀螺仪的重要参数指标。

附图说明

图1为本发明角度测量系统原理框图；

图2为本发明角度测量系统PCB设计图；

图3为静态数据卡尔曼滤波前后效果图；

图4为静态数据二阶低通滤波前后效果图；

图5为基于门限的静态动态数据判别示意图；

图6为卡尔曼滤波在动态情况下的滞后性示意图；

图7为二阶低通滤波在动态情况下的滞后性示意图；

图8为角度更新总流程图。

具体实施方式

下面结合附图对本发明作详细的介绍。

实施例1

本实施例1为一种用于MEMS陀螺仪的角度测量系统，该角度测量系统的原理框图如图1所示，其包括功率电源模块、基准电压模块、MEMS陀螺仪芯片、高精度AD转换模块和单片机模块；所述基准电压模块分别与MEMS陀螺仪芯片和高精度AD转换模块连接；所述功率电源模块分别与MEMS陀螺仪芯片、高精度AD转换模块和单片机模块连接；所述高精度AD转换模块还分别与MEMS陀螺仪芯片和单片机模块连接；所述单片机模块通过串行通信接口与外部设备连接以实现角度数据的对外传送。

其中，功率电源模块为电路中各芯片和期间提供电源；基准电源模块为MEMS陀螺仪芯片和AD转换模块提供高精度的基准电压，保证MEMS陀螺仪芯片输出的原始模拟信号的准确性以及AD转换的精度；MEMS陀螺仪芯片实现对角速度测量并输出模拟信号；高精度AD转换模块负责将陀螺仪芯片的原始模拟信号转换成数字量，并传输给单片机模块；单片机模块是算法和程序运行的核心部分，并通过串行通信接口实现角度数据的对外传送。

所述的功率电源模块实现24V输入电压转换为6V，5V、3.3V等电压。其中TPS5430芯片将24V转换6V，REG113芯片将6V转换为提供给单片机和AD模块的3.3V电源，TPS7A8001芯片将6V转换为提供给MEMS陀螺仪芯片的5V电源。设计6V的中间过渡电压是为了通过逐级降低电压避免大压降导致的功率损耗。陀螺仪上电后首先静置一段时间预热。之后将陀螺仪的原始输出信号输入到高精度AD转换芯片中，AD模块的采样和转换频率为10KHz，同时数字信号与模拟信号分开布线并进行地隔离，减少模拟信号中的外部干扰。

实施例2

本实施例2为一种用于MEMS陀螺仪的角度测量方法，其包括前面所述的用于MEMS陀螺仪的角度测量系统，具体包括以下步骤：

步骤1)，采样MEMS陀螺仪原始输出信号，并经AD转换后得到角速度数据；

步骤2)，对角速度数据进行滑动平均滤波，剔除队列中的极值后再进行均值运算，避免局部异常数据的干扰；

AD转换得到的陀螺仪输出值包含大量的噪声，本发明通过10KHz频率采样获得16位原始数字化数据后，首先将数据送入滑动均值滤波器中。具体做法是采用一个长度为10的队列A[]，连续采样10个数据放入其中。每次采样得到新的数组就放入队尾，并丢弃队头数据，随后将队列中的最大数据和最小数据剔除，余下8个数据取平均值该值即作为滑动均值滤波的输出值。利用这种方法可有效减弱随机游走，提高零点的稳定性。

步骤3)，对步骤2)得到的数据进行卡尔曼滤波，滤波结果与陀螺仪静态零点进行窗口阈值比较；若在阈值内，则判断为静态数据，送入队列用于更新零点，抑制零点漂移；若在阈值外，则判断为动态旋转数据，则转步骤4)继续处理；

初始静态零点的更新步骤：

对滑动均值滤波的输出值进行卡尔曼滤波，能够有效降低白噪声成分，初始零点更新阶段务必保证陀螺仪静止，避免零点中包含运动成分导致错误更新。

卡尔曼滤波的状态方程：

X_k＝ΦX_k-1+HW_k

Z_k＝CX_k+V_k

其中：

X_k是k时刻的系统状态，W_k是k时刻对系统的控制量，Φ和H是系统参数，对于多模型系统，Φ和H是矩阵；

Z_k是k时刻的测量值，C是测量系统的参数，对于多测量系统，C为矩阵，V(k)表示测量的噪声，被假设成高斯白噪声(White Gaussian Noise).

时间更新，预测下一时刻的输出值和误差协方差：

P_k/k-1＝ΦP_k-1Φ^T+HQH^T

其中：

是利用前一个状态的预测结果，是利用前一个状态计算的最优结果。

P_k/k-1是对应的协方差(covariance)，P_k-1是对应的协方差(covariance),Φ^T是Φ的转置矩阵，Q表示系统过程的协方差(covariance).

测量更新，计算卡尔曼增益，并由新获得的数据更新估计：

K_k＝P_k/k-1C^T(CP_k/k-1C^T+R)^-1

P_k＝(1-K_kC)P_k/k-1

其中：

K_k为卡尔曼增益。

经过卡尔曼滤波后，噪声得到有效的抑制，静态数据卡尔曼滤波前后效果具体如图3所示。

卡尔曼滤波器输出值送入长度为2000的队列B[]中，将2000个数据取平均作为初始静态零点

角度更新过程：

零点zero得到初始化之后，每次从步骤2)得到数据后即使用下列方法更新角度。

数据首先进行卡尔曼滤波，步骤与所述初始零点的更新步骤相同,滤波后的数据进行类型甄别，判断其为静态零点漂移，还是动态下的角速度数据，具体做法是为当前零点zero设置一个门限值±allow，基于门限的静态动态数据判别具体如图5所示。

若滤波后数据位于门限之内，则认为该数据为静态的零点漂移，送入所述队列B[]中，并对队列数据再求平均更新zero，采用这种机制可有效通过卡尔曼滤波去噪并抑制零点漂移，保证静态情况下零点实时更新。

步骤4)，对步骤2)得到的数据进行二阶低通滤波，滤波结果与陀螺仪静态零点作差，并与旋转系数、时间系数相乘转化为角度增量；

若滤波后数据位于门限之外，则认为该数据为动态测量数据，应与比例系数相乘并积分得到角度。然而卡尔曼滤波器中要求量测噪声和过程噪声均为白噪声，陀螺仪动态旋转时，滤波器输出将产生较大的滞后，卡尔曼滤波在动态情况下的滞后性具体如图6所示，因此会造成积分得到的角度具有较大的滞后性。

本发明中采用的做法是，若卡尔曼滤波后的数据位于门限之外，则对步骤2)得到的数据进行二阶低通滤波后再积分算角。

二阶低通滤波器与卡尔曼滤波器相比动态性能更优良，二阶低通滤波在动态情况下的滞后性如图7所示,滤波后的数据滞后性明显更小。但是二阶低通滤波器在静态情况下的噪声抑制能力弱于卡尔曼滤波，因此不适合用于稳定零点，静态数据二阶低通滤波前后效果具体如图4所示。

二阶低通滤波器传递函数为：

式中：A为增益，一般取1，τ为时间常数，截止频率s为拉氏变换。

将频域的传递函数转换到离散时域，便于程序计算滤波器输出值。

式中：n为步长。

拉氏变换：

L[y']＝sY(s)

L[y”]＝s²Y(s)

Y(s)+3sτY(s)+(sτ)²Y(s)＝X(s)

综合各式得到离散的二阶低通滤波器递推表达式：

其中为采样频率，T为采样周期。

在实际使用中x[n]为滤波器输入，y[n-1]和y[n-2]分别为前一次和前两次滤波获得的数据，初始值均设为0，最后均会随着滤波的进行收敛，这样就可以递推地计算每次通过二阶低通滤波后的数据y[n]。

步骤5)，利用定时器中断机制对角度增量进行高精度实时积分，得到角度数据并通过串行接口发出；

二阶低通滤波器输出值进行积分算角：

angle_new＝angle_old+y[n]×C×T

C为固定的比例系数，将y[n]转换为真实的角速度。

步骤6)，转至步骤1)，循环执行算法，实现角度值实时更新。

本发明通过卡尔曼滤波抑制静态情况下的零点漂移，动态旋转时则通过二阶低通滤波器降噪并保证角度跟踪的动态性能，将两种滤波器的优势有机结合，最终实现了高精度稳定的角度测量，并在机器人航向角测量等实际场合得到应用检验。

本领域技术人员将清楚本发明的范围不限制于以上讨论的示例，有可能对其进行若干改变和修改，而不脱离所附权利要求书限定的本发明的范围。尽管己经在附图和说明书中详细图示和描述了本发明，但这样的说明和描述仅是说明或示意性的，而非限制性的。本发明并不限于所公开的实施例。

Claims (1)

1.一种用于MEMS陀螺仪的角度测量方法，其包括一种用于MEMS陀螺仪的角度测量系统，所述角度测量系统包括功率电源模块、基准电压模块、MEMS陀螺仪芯片、高精度AD转换模块和单片机模块；

所述基准电压模块分别与MEMS陀螺仪芯片和高精度AD转换模块连接；所述功率电源模块分别与MEMS陀螺仪芯片、高精度AD转换模块和单片机模块连接；所述高精度AD转换模块还分别与MEMS陀螺仪芯片和单片机模块连接；所述单片机模块通过串行通信接口与外部设备连接以实现角度数据的对外传送；

所述功率电源模块包括TPS5430芯片、REG113芯片和TPS7A8001芯片；其中，TPS5430芯片将24V转换6V，REG113芯片将6V转换为提供给单片机和AD模块的3.3V电源；TPS7A8001芯片将6V转换为提供给MEMS陀螺仪芯片的5V电源；

所述的基准电压模块采用ADR425芯片，输出的5V电压作为AD转换模块和MEMS陀螺仪芯片的基准参考电平；

所述的MEMS陀螺仪芯片选用ADI生产的ADIRS620；

所述的高精度AD转换模块选用TI生产的16位高精度A/D芯片ADS8327，并通过SPI总线将AD转换后的角速度数据传入单片机模块中；

所述的单片机模块选用ST的stm32f405RGT6，并通过串行通信外设将算法和程序运行得到的角度数据对外发送；

所述角度测量方法具体包括以下步骤：

步骤1)，采样MEMS陀螺仪原始输出信号，并经AD转换后得到角速度数据；

步骤2)，对角速度数据进行滑动平均滤波，剔除队列中的极值后再进行均值运算，避免局部异常数据的干扰；

步骤3)，对步骤2)得到的数据进行卡尔曼滤波，滤波结果与陀螺仪静态零点进行窗口阈值比较；

若在阈值内，则判断为静态数据，送入队列用于更新零点，抑制零点漂移；

若在阈值外，则判断为动态旋转数据，则转步骤4)继续处理；

步骤4)，对步骤2)得到的数据进行二阶低通滤波，滤波结果与陀螺仪静态零点作差，并与旋转系数、时间系数相乘转化为角度增量；

步骤5)，利用定时器中断机制对角度增量进行高精度实时积分，得到角度数据并通过串行接口发出；

步骤6)，转至步骤1)，循环执行算法，实现角度值实时更新。