融合卡尔曼改进小波的姿态传感器滤波方法

王静静，秦 刚，李腾

（西安工业大学 电子信息工程学院，西安 710021）

XGZT-XXX 型姿态传感器为西安工业大学自主研发，由MEMS 加速度计、MEMS 陀螺仪与MEMS倾角传感器组成，该传感器具有体积小、成本低、功耗低等优点。但由于MEMS 陀螺仪自身输出噪声大，且在实验过程中受不可避免的各种复杂的振动、干扰激励（如实验过程中人员的走动、安放不稳引起抖动等），为了提高系统测量精度，需要对其进行降噪处理。

对惯性器件滤波处理，有经典滤波器和现代滤波器，惯性器件的噪声与信号通常是相互重叠，不适合使用经典滤波器（IIR、FIR），可根据信号和噪声的统计特性，利用数理统计方法进行估计，即现代滤波器[1]。现代滤波器中常用的方法有卡尔曼滤波、经验模态分解（empirical mode decomposition，EMD）、小波阈值去噪等[2]。文献[3]建立了时间序列模型引入卡尔曼进行滤波处理，但缺乏自适应性，长时间会导致滤波发散。文献[4]中将卡尔曼与小波进行了融合，并且对卡尔曼进行了抗野值的优化，但是计算量较大，过于复杂，对主控芯片有一定的要求。文献[5]提出新的Garrote 型阈值函数，克服了软硬阈值函数的不足，但是函数需调节的参数较多，难以掌控。文献[6]提出将卡尔曼与小波模糊阈值进行了融合，但却忽略了陀螺仪的低频噪声。针对以上问题，本文采用卡尔曼融合改进小波去噪方法，首先对原始数据进行预处理，建立MEMS 陀螺仪的时间序列模型，输入卡尔曼滤波器进行首次滤波，然后使用改进的小波阈值去噪方法进行二次滤波，从而完成对陀螺仪信号的去噪处理。

1 基于ARMA 的卡尔曼滤波

1.1 数据预处理

为满足时间序列模型平稳、正态、零均值的建模要求，要对陀螺仪的原始数据进行预处理[7]。采用拉伊达准则法和最小二乘法进行异常值处理与去趋势项处理。陀螺仪原始数据预处理后如图1 所示。

图1 预处理前后数据对比图Fig.1 Data comparison before and after preprocessing

1.2 平稳性及正态性检验

（1）平稳性检验

利用采用ADF（augmented dickey-fuller test）准则检验序列的平稳性[8]。使用Matlab 中的adftest 函数返回值为1，结果显示该序列具有平稳性。

（2）正态性检验

本文采用偏态系数ξ 和峰态v 系数来检测样本数据的正态性，公式如下：

当ξ≈0、v≈3 时，可以认为x（t）为正态序列，求得ξ=2.8281，v=0.0018，故该序列满足建模要求。

1.3 ARMA 模型定阶和参数估计

MEMS 陀螺仪的ARMA 模型阶数一般不超过3阶，采用赤池信息准则（akaike information criterion，AIC）和贝叶斯信息准则（bayesian information criterion，BIC）确定具体的阶数[9]。利用自回归逼近方法求得AIC 准则与BIC 准则检验结果如表1 所示。通过对比发现ARMA（1，1）的AIC 和BIC 值小于其他模型，故采用ARMA（1，1），其表达式为

表1 AIC 准则与BIC 准则检验结果Tab.1 Test results of AIC criteria and BIC criteria

采用多项式拟合得陀螺仪X 轴的输出模型：

1.4 卡尔曼滤波器设计

离散型线性随机系统的方程为

式中：Xk为系统状态向量；Zk为测量序列；Φk，k-1为系统转移矩阵；Γk，k-1为输入矩阵；Hk为观测矩阵；Wk和Vk为系统过程噪声序列和观测噪声序列，为均值为零且相互独立的高斯白噪声，它们的协方差矩阵分别为Qk和Rk，满足以下特性：

式中：δkj为克罗内克尔（kronecker-δ）函数，其特性是：

则离散型卡尔曼滤波方程为

状态一步预测方程：

状态估计方程：

滤波增益矩阵：

预测误差方差阵：

估计误差方差阵：

通过以上公式可知，给定X0、P0，根据观测值Zk即可随时间循环更新。

2 小波阈值去噪

小波阈值去噪方法具有快速、简单、可操作性高等优良特性[10]。基本原理：选择合适的小波基函数对原始信号进行分解得到一系列小波系数，通过阈值和阈值函数，将高于该阈值的小波系数进行保留或适当收缩后保留，低于该阈值的小波系数归零，最后选择不为零的小波系数重构得到去噪后的信号。

2.1 小波阈值函数

（1）硬阈值

式中：λ 为阈值；ωj，k为去噪前小波系数；为去噪后小波系数。

（2）软阈值

式中：sgn（）为符号函数。

硬阈值函数存在断连、不连续等问题，导致滤波后信号出现震荡。而软阈值函数虽然连续，却存在固定的偏差，会造成滤波不彻底的问题。

2.2 小波阈值

目前，常用的阈值估计方法有固定阈值、极大极小准则阈值、Stein 无偏似然阈值等。

（1）固定阈值

固定阈值λ 由Donoho 和Johnstone 提出[11]，表达式为

式中：σ 为噪声标准差；N 为信号长度。

（2）Stein 无偏似然阈值

该方法原理是首先求得给定阈值的似然估计，然后将非似然的值进行最小化，对应的值就是所需要的阈值。

（3）极大极小准则阈值。

极大极小准则阈值是一种固定阈值，原理是产生一个最小均方误差[11]。表达式如下：

固定阈值较为绝对，缺乏灵活性，存在“过扼杀”的问题；极大极小准则阈值过于保守，存在“过保留”的问题；无偏似然阈值对于低信噪比信号不适合[12]。

2.3 小波阈值去噪的改进方案

2.3.1 基于模糊阈值函数改进的小波阈值去噪

基于以上对常用的阈值函数和阈值的不足，本文提出通过在2 种经典阈值和模糊隶属度函数的基础上设计阈值函数，对小于极大极小准则阈值的小波系数进行去除，大于固定值阈值的小波系数进行保留，在极大极小准则阈值与固定阈值之间的小波系数采用模糊隶属度函数处理。本文采用升半岭型模糊隶属函数对信号进行去噪[13-14]，函数表达式为

式中：ωj，k为去噪前小波系数；为去噪后小波系数；λa为极大极小准则阈值；λb为固定阈值；μA（ωj，k）为模糊隶属度函数。

2.3.2 基于自适应小波模糊阈值去噪

由于不同频带的噪声分量的不同，故本文在模糊阈值函数的基础上引入动态调节系数r，提出了一种改进的阈值计算公式：

式中：λa为极大极小准则阈值；λb为固定阈值。

在小波分析中，信号相关度较大的小波系数相对比较大，跟噪声相关度较小的小波系数较小[15]。故根据不同的分解尺度，每一层都选择相应的阈值，实现分层阈值动态变化。

式中：r 为动态调节系数；j 为分解层数。

2.3.3 基于LS 改进的小波阈值去噪

对于传统小波阈值去噪过程中只处理高频分量部分，从而忽略了陀螺仪的低频噪声的不足。本文将小波分解后的低频部分采用最小二乘对陀螺仪低频部分进行平滑滤波，保留原有的信号趋势。

3 实验验证与结果分析

3.1 仿真实验

在仿真实验中，利用Matlab 软件生成原始的测试正弦信号：x（t）＝2sin（2π f1t+cos（2π f2t））；加入不同的白噪声序列n（t）[16]，得到不同信噪比的含噪信号：y（t）=x（t）+n（t）。选取4 dB 小波经过3 尺度分解，不同的滤波方法对其进行处理，引入信噪比和均方差来反映滤波效果。对于含噪的信号，信噪比越大、均方根越小证明去噪效果越好。

如表2 所示为不同方案的信噪比与均方根，图2 所示为信噪比为4 dB 的信号滤波结果，从实验结果可以看出，在不同信噪比下，本文方案的信噪比均大于其他方法，均方误差均小于其他方法。在信噪比为4 dB 下，改进的小波模糊阈值比小波模糊阈值去噪方法信噪比提高了5.143 dB，均方根减少了0.142。本文方案滤波后的信号更加还原原始信号。

表2 不同方案的信噪比与均方根Tab.2 SNR and RMSE of different schemes

图2 信噪比为4 dB 的信号滤波结果Fig.2 Signal filtering results with 4 dB SNR

3.2 实测数据

将MEMS 陀螺仪静置于经纬仪控制实验台上保持水平，预热30 min 后，进行数据采集，采样频率为200 Hz，选取连续的2000 个采样点作为样本数据，对其进行不同去噪方法实验验证。实验采集平台如图3 所示。

图3 经纬仪实验采集设备Fig.3 Theodolite experimental acquisition equipment

不同滤波方法的前后对比图如图4 所示，可知本方案处理后的信号减小了毛刺，振荡更小，更加平滑。查阅文献可知，Allan 方差双对数图可以在时间频域上根据不同噪声在双对数Allan 方差曲线上的斜率不同来将各个噪声进行区分，因此常用来识别陀螺仪的随机噪声分量[15]。故引入Allan 方差对滤波前后MEMS 陀螺仪噪声进行辨识，结果如表3所示。

表3 Allan 方差辨识滤波前后随机噪声系数Tab.3 Allan variance identification random noise figure before and after filtering

图4 不同滤波方法的前后对比图Fig.4 Before and after comparison of different filtering methods

实验结果表明，利用本方案滤波后的量化噪声、角度随机游走、零偏不稳定性、角速率随机游走及速率斜坡分别减少了63.17%、81.57%、87.59%、81.64%、77.80%，相比于卡尔曼滤波融合小波模糊阈值方法提高了3.82%、6.42%、22.14%、16.39%、10.59%。本文方案的滤波效果明显减少陀螺仪的随机噪声，相比于传统方法效果更佳。

4 结语

针对XGZT-XXX 型姿态传感器输出噪声大、易受复杂环境干扰的问题，本文采用卡尔曼联合改进的小波阈值方法进行去噪处理，引入Allan 方差、信噪比、均方根对陀螺仪的噪声系数进行估计，通过仿真实验与实测数据实验结果表明，本文所改进的方法可以有效减小陀螺仪的噪声，滤波效果优于卡尔曼滤波和小波阈值方法，对提高XGZT-XXX 型姿态传感器的稳定性和抗干扰能力具有实践意义。