CN109067381A - 一种mems陀螺仪随机噪声的实时滤波系统及方法 - Google Patents

Abstract

本发明公开了一种MEMS陀螺仪随机噪声的实时滤波系统，包括MEMS陀螺仪和微处理器MCU，陀螺仪与MCU通过信号线连接。算法模块包括AMA模块、以及FAKF滤波器；所述FAKF滤波器包括RLS模块、FLC模块以及AKF模块。本发明的实时滤波方法采用改进的RLS拟合ARMA时间序列模型各项系数，对获得的陀螺仪的噪声序列实时更新模型中的系数a₁、a₂、c₁和σ²，有效解决离线模型中的噪声模型与长时间运动后的噪声特性不一致的问题。本发明通过采用AMA方法检测陀螺仪运动过程中的突变点，对稳态运动状态采用较小Q_i值，以提高其精度，而对剧烈变加速运动状态采用较大的Q_i值，以提高其对运动的跟随能力，解决动态运动精度和跟随能力的问题。

Description

一种MEMS陀螺仪随机噪声的实时滤波系统及方法

技术领域

本发明涉及惯性导航技术领域，特别是涉及一种对实际应用场景中的MEMS陀螺仪随机噪声的实时滤波的系统及方法。

背景技术

MEMS陀螺仪是一类使用MEMS(Micro Electro-Mechanical System)技术制成的测量物体运动时角速率的惯性测量单元，是航姿参考系统(AHRS)与微惯性导航系统(Micro-INS)的核心器件。因其具有体积小、重量轻、成本低以及可靠性好等优点，中低端性能、单体MEMS器件构建的姿态传感器已经广泛应用于山体边坡、建筑桥梁、工程设施、飞行器、车辆以及人体姿态的监测，具有良好的经济效益和社会效益，而高精度的MEMS陀螺仪则应用于无人机以及精确制导武器等国防领域。随着MEMS陀螺仪在消费级、工业级、军事国防领域和航空航天领域的深入应用，国内外很多科研单位和相关部门越来越重视MEMS陀螺仪的研究。但是MEMS陀螺仪受自身材料、制造工艺和设计水平的限制，以及具有输出信号随机噪声大、稳定性差、易受工作环境等外界因素的影响等不足，导致其测量精度普遍不高，这也成为限制MEMS惯性器件广泛应用以及航姿参考系统和惯性导航系统精度的一大瓶颈。因此，为了提高系统精度，需要研究MEMS陀螺仪的误差特性分析、噪声建模和信号处理，以及研究对实时信号处理的硬件。

MEMS陀螺仪的输出模型表示为：

W_i＝w_i0+K_ij·w_i+w_n

其中，W_i为陀螺仪输出；w_i0为常值漂移；K_ij为系数矩阵，包括标度因数和轴与轴间的耦合系数；w_i为陀螺仪角速度输入量；w_n为随机噪声，即围绕陀螺仪输出均值的上下波动。

输出模型可以用于分析陀螺仪的误差特性，其中常值漂移属于MEMS陀螺仪的确定性误差，常值漂移和系数矩阵可以通过标定技术进行有效补偿，但随机噪声w_n属于随机误差，作为一种高频噪声，叠加在输入量一起构成陀螺仪的输出，无法通过标定技术补偿消除，只能通过滤波的方法进行降噪，进而减小随机噪声对MEMS陀螺仪输出的影响。

目前，对MEMS陀螺仪随机噪声降噪的方法主要有基于时间序列的卡尔曼滤波、小波降噪和基于神经网络或支持向量机建模的滤波，但是小波技术和神经网络技术具有大量矩阵运算，且动态运动时很难有效区分真值输入和随机噪声等问题，而基于噪声时间序列模型的卡尔曼滤波技术具有精度高、实时性好等优点，因此被广泛研究和应用。当前，基于时间序列的卡尔曼滤波简要流程如下：

1、离线采集陀螺仪静态时的输出：通过差分处理获取随机噪声序列，并对噪声进行白噪声检验和时间序列建模，对整个离线采集的样本点建模可得到ARMA(n_a,n_c)的系数n_a,n_c和白噪声方差σ²，AMRA是自回归滑动平均模型。以ARMA(2,1)为例，噪声模型可表示为：

w_n(t)-a₁·w_n(t-1)-a₂·w_n(t-2)＝ε(t)+c₁·ε(t-1)

其中，残差ε(t)为方差为σ²的白噪声。

2、陀螺仪输出模型：

ω_k＝ω_ik+ω_nk

噪声模型可表示为：

w_n(t)-a₁·w_n(t-1)-a₂·w_n(t-2)＝ε(t)+c₁·ε(t-1)

Var(ε(t))＝σ²

真值输入模型：

ω_i(t)＝ω_i(t-1)+ξ(t)

Var(ξ(t))＝Q_i

因此可选取x_k为状态量，w_i(t)为真值的估计值，x(t)为随机噪声的估计值；

状态转移矩阵F_k，噪声转移矩阵Γ_k，根据状态量之间的关系：

系统噪声ω_k的方差矩阵为

陀螺仪的输出值，即y_k＝X(t)+x(t)，所以量测转移矩阵H_k为：

H_k＝[1 0 1]

量测噪声v_k的方差矩阵为

R＝[r²]

根据建立的系统方程和量测方程，可以进行卡尔曼滤波，卡尔曼滤波是一种无偏最小方差估计，相当于一个低通滤波器，该方法可以取得很好的降噪效果。但是，目前MEMS陀螺仪降噪方法还存在三个主要缺点：

1、MEMS陀螺仪随机噪声具有时变、弱平稳特性，离线建立的噪声模型不再能准确描述重新启动或长时间运动后的噪声特性，即之前的状态转移矩阵F_k和噪声转移矩阵Γ_k不再能准确描述当前噪声特性，这将直接影响卡尔曼滤波器系统方程的准确性。

2、目前，现有技术和研究主要集中在稳态运动场景，包括静态和恒定转速运动模式。但是在实际应用场景中，陀螺仪往往处于复杂运动模式下，但是在系统方程中，真值的估计值X(t)＝X(t-1)，所以真值的估计值与实际运动状态始终会延迟一个时间单位。由于陀螺仪芯片采样频率较高，在稳态和缓慢变速运动时，该延迟对估计值X(t)几乎没有影响，但是，当陀螺仪的测量环境发生剧烈变速运动时，由于真值发生突变，估计值X(t)与真值差异很大，导致这种延迟效果会很明显，而现有技术中对于复杂运动模式的解决方案无法兼顾精度和跟随能力，导致复杂运动模式下，陀螺仪的测量精度很差。

3、量测噪声R是影响卡尔曼滤波器性能的重要参数，不恰当的取值会影响滤波器效果甚至导致滤波发散，但量测噪声的先验统计特性很难或无法得到，针对这一问题，很多自适应滤波器被提出和采用，如Sage-Husa自适应滤波算法，粒子滤波算法，虽然它们可以取得很好效果，但具有大量矩阵运算，计算量大、实时性和稳定性差。

发明内容

本发明所采用的技术方案如下：

一种MEMS陀螺仪随机噪声的实时滤波系统，包括MEMS陀螺仪和微处理器MCU，陀螺仪与MCU通过信号线连接，陀螺仪的原始信号经MCU内部的算法模块滤波处理后，输出滤波后的信号。

作为上述技术方案的进一步改进：

所述算法模块包括AMA模块、以及FAKF滤波器；所述FAKF滤波器包括RLS模块、FLC模块以及AKF模块。

一种使用MEMS陀螺仪随机噪声的实时滤波系统的实时滤波方法，包括以下步骤：

第一步：通过RLS模块计算，获得ARMA时间序列模型的系数；

第二步：通过AMA模块检测样本序列的突变点，实时判断陀螺仪运动模式是稳态还是动态；

第三步，通过AMA模块检测样本序列的突变点，实时判断陀螺仪运动模式是稳态还是动态，判断第一步的RLS模块是否持续计算：如果判断陀螺仪为动态模式，此时陀螺仪输出为运动输入和噪声的叠加，无法有效区分出噪声，则不需要RLS连续计算来更新参数。

作为上述技术方案的进一步改进：

在第一步之前还包括预热和判断步骤：

预热步骤：启动MEMS陀螺仪并静置预热；

判断步骤：AMA模块检测陀螺仪预热后的噪声特性，利用自适应移动平均算法自动判断样本序列中是否有突变点，如果连续两个样本序列没有出现突变点，预热完成，进入权利要求3的步骤；如果有突变点，重复预热和判断步骤。

所述第一步中ARMA的系数a₁、a₂、c₁和σ²分成两段计算，拟合方法如下：

首先，通过RLS模块拟合得到白噪声σ²估计值：

然后，通过RLS模块拟合得到系数a₁、a₂、c₁：

y(t)＝Ψ^T(t)θ+ε(t)

s(t+1)＝1/[1+ψ^T(t+1)Q(t)ψ(t+1)]

Q(t+1)＝Q(t)-s(t+1)Q(t)ψ(t+1)ψ^T(t+1)Q(t)^T

θ^T(t)＝[a₁,a₂,c₁]

通过实时采集陀螺仪的测量值，计算并更新噪声模型中的系数a₁、a₂、c₁和σ²。

所述检测样本序列的突变点的方法为，取样N个样本点，以2q+1为滑窗宽度，进行普通滑动平均：

对y_t再进行自适应移动平均

将输出Y_t作为输入重复进行自适应移动平均，各个滑窗内的均方差为：

通过与阈值λ对比，来获得突变点：

t＝2q+1、2q+2、…、N-2q

如果序列平稳没有突变点，则判断陀螺仪处于稳态模式，采取较小值的系统噪声进行FAKF滤波降噪；如果有突变点，则判断陀螺仪处于动态模式，采取较大的系统噪声进行FAKF滤波降噪。

所述FAKF滤波降噪方法中，将卡尔曼滤波器中状态量更新公式为：

其中定义为新息，滤波器在稳定状态下时，新息应为零均值白噪声，即方差恒定；

其理论值为

实际值(一段滑窗内样本点均方差)为

通过检测新息理论值和实际值的匹配情况，对量测噪声R_k进行补偿；

构建的模糊逻辑输入量为

模糊逻辑输出量为补偿因子S_k，用来调整量测噪声R_k

R_k＝S_kR_k-1

FAKF滤波降噪实时地通过R_k＝S_kR_k-1补偿量测噪声，得到相对准确的量测噪声R。

本发明的有益效果如下：

本发明的MEMS陀螺仪随机噪声的实时滤波方法采用改进的RLS(递推最小二乘法)拟合ARMA(自回归滑动平均)时间序列模型各项系数，对获得的陀螺仪的噪声序列实时更新模型中的系数a₁、a₂、c₁和σ²，有效解决离线模型中的噪声模型与长时间运动后的噪声特性不一致的问题。

本发明采用AMA(自适应移动平均算法)检测陀螺仪运动过程中的突变点，即陀螺仪角速度发生突然变化的数据点，并在不同运动状态下采用不同的系统噪声方差矩阵Q中Q_i值，即对稳态运动状态采用较小Q_i值，以提高其精度，而对剧烈变加速运动状态采用较大的Q_i值，以提高其对运动的跟随能力，解决动态运动精度和跟随能力的问题。

本发明采用基于协方差匹配技术的模糊逻辑进行实时调整，首先，计算卡尔曼滤波过程中新息r_i的理论值P_rk和实际值C_rk，构建其比值q_k和量测噪声的补偿因数S_k分别作为模糊逻辑的输入与输出，然后，在隶属函数和模糊逻辑规则的控制下，实时地通过R_k＝S_kR_k-1补偿量测噪声，最终得到相对准确的量测噪声R，避免不适当的R而影响卡尔曼滤波器性能。

附图说明

图1为本发明的硬件结构示意图。

图2为本发明的算法模块示意图。

图3为本发明的滤波方法流程图。

图4为未经滤波处理的陀螺仪的原始信号曲线。

图5为本发明对图4中信号滤波处理后的输出信号曲线。

具体实施方式

下面结合附图，说明本发明的具体实施方式。

如图1所示为本发明的硬件结构示意图，电源模块与MEMS陀螺仪和微处理器MCU电连接，MEMS陀螺仪包括测量X/Y轴信号的陀螺仪以及测量Z轴信号的陀螺仪，陀螺仪与MCU通过信号线连接，陀螺仪测量的原始信号输入MCU并经MCU内部的算法模块滤波处理后，通过串口实时输出滤波后的信号，MCU还连接有程序烧写器接口，用于向MCU写入滤波所需的算法程序。

如图2所示为本发明的算法模块示意图，本发明的算法模块集成在MCU中，包括AMA模块、以及FAKF滤波器(模糊逻辑自适应滤波器)。其中FAKF滤波器包括RLS模块、FLC模块(Fuzzy Logic Control，模糊逻辑控制模块)以及AKF模块(自适应滤波模块)。

如图3所示，本发明系统工作时的实时滤波方法包括以下步骤。

第一步，启动MEMS陀螺仪并静置预热。由于MEMS陀螺仪容易受到外部环境，例如温度的影响，因此，系统启动后不能直接采集MEMS陀螺仪的噪声特性，否则将直接影响后续滤波效果。系统通电启动到预热后系统稳定，通常不会超过三分钟，因此，预热时间大于三分钟即可。

第二步，MCU中集成的AMA模块检测MEMS陀螺仪预热后的噪声特性，判断系统预热后是否稳定。AMA模块采集陀螺仪的输出值，样本点取4096个点，利用自适应移动平均算法自动判断样本序列中是否有突变点。如果系统预热后稳定，那么MEMS陀螺仪的输出值应该是稳定的，当连续采集两个样本序列没有出现突变点，说明MEMS陀螺仪状态稳定，预热完成，进入第三步。如果有突变点，说明MEMS陀螺仪状态不稳定，重复第一步和第二步。

第三步，使用RLS模块在线计算拟合获得ARMA时间序列模型的系数。本发明将a₁、a₂、c₁和σ²分成两段计算，可以避免系数与白噪声之间的耦合，进而提高系统的精度和拟合速度。

拟合方法如下：

噪声的ARMA(n_a,n_c)模型重新写为：

A(q^-1)y(t)＝C(q^-1)ε(t)

本发明通过采取RLS方法拟合ARMA的系数，a₁、a₂、c₁和σ²。首先，通过RLS模块拟合得到白噪声σ²估计值。

然后，通过RLS模块拟合得到系数a₁、a₂、c₁。

y(t)＝ψ^T(t)θ+ε(t)

s(t+1)＝1/[1+Ψ^T(t+1)Q(t)ψ(t+1)]

Q(t+1)＝Q(t)-s(t+1)Q(t)ψ(t+1)ψ^T(t+1)Q(t)^T

θ^T(t)＝[a₁,a₂,c₁]

本发明通过采用改进的RLS(递推最小二乘法)，通过以上的算法拟合得到ARMA时间序列模型各项系数，而且可以实时采集陀螺仪的测量值，实时计算并更新噪声模型中的系数a₁、a₂、c₁和σ²，可以有效解决离线模型中的噪声模型与长时间运动后的噪声特性不一致的问题。

第四步，通过AMA模块检测样本序列的突变点，实时判断陀螺仪运动模式是稳态还是动态。

AMA检测方法为：取样N个样本点，以2q+1为滑窗宽度，进行普通滑动平均：

对y_t再进行自适应移动平均

其中

D(t)＝|y(t+q)-y(t-q)|

D′(t)＝D′(t+1)-D′(t)

将输出Y_t作为输入重复进行自适应移动平均，各个滑窗内的均方差为

通过与阈值λ对比，来获得突变点

t＝2q+1、2q+2、…、N-2q

经过AMA计算，如果序列平稳没有突变点，则判断陀螺仪处于稳态模式，此时采取较小值的系统噪声Q_i1进行FAKF滤波降噪，可以获得更高精度的滤波效果。如果判断有突变点，则判断陀螺仪处于动态模式，动态模式下主要考虑系统数据处理的跟随能力，因此在动态模式下采取较大的系统噪声Q_i2进行FAKF滤波降噪，一方面可以保证系统的跟随能力，同时可以具有比较好的降噪能力，在动态模式下可以兼顾跟随能力和降噪能力。

其中，本发明的FAKF(模糊自适应滤波)的方法如下：

卡尔曼滤波器中状态量更新公式为：

其中定义为新息，滤波器在稳定状态下时，新息应为零均值白噪声，即方差恒定。

其理论值为

实际值(一段滑窗内样本点均方差)为

由于量测噪声R_k先验不可知或随时间、环境变化，错误的取值会使滤波效果不好甚至滤波发散，所以，本发明通过检测新息理论值和实际值的匹配情况，对量测噪声R_k进行补偿。

构建的模糊逻辑输入量为

模糊逻辑输出量为补偿因子S_k，用来调整量测噪声R_k

R_k＝S_kR_k-1

本发明的FAKF在隶属函数和模糊逻辑规则的控制下，实时地通过R_k＝S_kR_k-1补偿量测噪声，最终得到相对准确的量测噪声R，避免不适当的R而影响卡尔曼滤波器性能。

第五步，通过AMA模块检测样本序列的突变点，实时判断陀螺仪运动模式是稳态还是动态，判断第三步的RLS模块是否持续计算。

本发明中的滤波方法中，经过第二步中AMA判断MEMS陀螺仪稳定后，必须通过RLS模块拟合得到ARMA各项系数的初值。如果在陀螺仪运行过程中，始终利用RLS模块拟合ARMA的系数，会大大增加计算量。为此，本发明当第四步中AMA判断为稳态模式时，持续进行RLS更新噪声模型参数，因为在稳态模式下，对于足够多的样本点，通过插值法就能获得准确的噪声数据；当判断为动态模式时，将不再进行RLS计算更新参数，因为动态模式下，陀螺仪输出为运动输入和噪声的叠加，无法有效区分出噪声，而且动态模式下需要较高的跟随能力，因此不再进行RLS计算，节约运算，减少延迟。

通过本发明的实时滤波方法，尤其是对于复杂运动模式下的陀螺仪的数据，既可以得到很好的滤波精度，而且动态模式下本系统的跟随能力很好。图4和图5所示为本发明对加速运动过程中陀螺仪输出数据的滤波前后的对比图，如图4所示，为未经滤波处理的陀螺仪输出信号的原始曲线图，即陀螺仪直接测量值的曲线，可知陀螺仪的测量值有三次变大的过程，每一次数据变化的阶段代表陀螺仪处于加速运动过程，而曲线的平直段代表了匀速运动，不管是加速运动还是匀速运动，原始数据曲线上有明显的波动，该波动即为MEMS陀螺仪的噪声。图5是对图4中原始信号滤波处理后输出的信号曲线，即经过本发明的实时滤波系统滤波后的输出值。对比原始曲线和FAKF曲线，明显可以看出滤波后的FAKF曲线与原始曲线的趋势一样，特别是在加速运动过程中，两条曲线整体趋势基本一致，没有延迟，证明本发明的实时滤波方法有很好的跟随能力，保证系统的输出结果代表了MEMS实时测量数据，避免数据的延迟。同时，FAKF曲线明显比原始曲线光滑，波动小，即经过本系统滤波后，很好的消除了陀螺仪测量值中的噪声，显著提高陀螺仪的测量精度。根据对图4和图5中信噪比计算，原始数据的信噪比为30.5dB，而经过滤波处理后的信噪比为39.9dB，证明经过本方法滤波后的信噪比比原始数据提高10倍左右，很好的消除了陀螺仪测量值中的噪声。

以上描述是对本发明的解释，不是对发明的限定，在不违背本发明精神的情况下，本发明可以作任何形式的修改。因此，举凡所属技术领域中具有通常知识者在未脱离本发明的精神与技术思想下完成的一切等效修饰或改变，仍应由本发明的权利要求所覆盖。

Claims (7)

1.一种MEMS陀螺仪随机噪声的实时滤波系统，其特征在于：包括MEMS陀螺仪和微处理器MCU，陀螺仪与MCU通过信号线连接，陀螺仪的原始信号经MCU内部的算法模块滤波处理后，输出滤波后的信号。

2.根据权利要求1所述的MEMS陀螺仪随机噪声的实时滤波系统，其特征在于：所述算法模块包括AMA模块、以及FAKF滤波器；所述FAKF滤波器包括RLS模块、FLC模块以及AKF模块。

3.一种使用如权利要求1所述的MEMS陀螺仪随机噪声的实时滤波系统的实时滤波方法，其特征在于：包括以下步骤：

第一步：通过RLS模块计算，获得ARMA时间序列模型的系数；

第二步：通过AMA模块检测样本序列的突变点，实时判断陀螺仪运动模式是稳态还是动态；

第三步，通过AMA模块检测样本序列的突变点，实时判断陀螺仪运动模式是稳态还是动态，判断第一步的RLS模块是否持续计算：如果判断陀螺仪为动态模式，此时陀螺仪输出为运动输入和噪声的叠加，无法有效区分出噪声，则不需要RLS连续计算来更新参数。

4.根据权利要求3所述的实时滤波方法，其特征在于：所述第一步之前还包括预热和判断步骤：

预热步骤：启动MEMS陀螺仪并静置预热；

判断步骤：AMA模块检测陀螺仪预热后的噪声特性，利用自适应移动平均算法自动判断样本序列中是否有突变点，如果连续两个样本序列没有出现突变点，预热完成，进入权利要求3的步骤；如果有突变点，重复预热和判断步骤。

5.根据权利要求3所述的实时滤波方法，其特征在于：所述第一步中ARMA的系数a₁、a₂、c₁和σ²分成两段计算，拟合方法如下：

首先，通过RLS模块拟合得到白噪声σ²估计值：

然后，通过RLS模块拟合得到系数a₁、a₂、c₁：

y(t)＝ψ^T(t)θ+ε(t)

s(t+1)＝1/[1+ψ^T(t+1)Q(t)ψ(t+1)]

Q(t+1)＝Q(t)-s(t+1)Q(t)ψ(t+1)ψ^T(t+1)Q(t)^T

θ^T(t)＝[a₁,a₂,c₁]

通过实时采集陀螺仪的测量值，计算并更新噪声模型中的系数a₁、a₂、c₁和σ²。

6.根据权利要求3所述的实时滤波方法，其特征在于：所述检测样本序列的突变点的方法为，取样N个样本点，以2q+1为滑窗宽度，进行普通滑动平均：

对y_t再进行自适应移动平均

将输出Y_t作为输入重复进行自适应移动平均，各个滑窗内的均方差为：

通过与阈值λ对比，来获得突变点：

t＝2q+1、2q+2、…、N-2q

如果序列平稳没有突变点，则判断陀螺仪处于稳态模式，采取较小值的系统噪声进行FAKF滤波降噪；如果有突变点，则判断陀螺仪处于动态模式，采取较大的系统噪声进行FAKF滤波降噪。

7.根据权利要求6所述的实时滤波方法，其特征在于：所述FAKF滤波降噪方法中，将卡尔曼滤波器中状态量更新公式为：

其中定义为新息，滤波器在稳定状态下时，新息应为零均值白噪声，即方差恒定；

其理论值为

实际值(一段滑窗内样本点均方差)为

通过检测新息理论值和实际值的匹配情况，对量测噪声R_k进行补偿；

构建的模糊逻辑输入量为

模糊逻辑输出量为补偿因子S_k，用来调整量测噪声R_k

R_k＝S_kR_k-1

FAKF滤波降噪实时地通过R_k＝S_kR_k-1补偿量测噪声，得到相对准确的量测噪声R。