CN104501832A - 一种改进型实用惯性传感器降噪装置 - Google Patents

Abstract

本发明属于惯性传感器的标定测试和捷联惯性系统技术领域，具体涉及一种结合模拟滤波和数字滤波的改进型实用惯性传感器降噪装置。其中，本发明提供的HMM-RLS滤波的第一次迭代，直接以量测值的原始数据作为滤波输出，实现了降噪方法真正意义上的零延迟。本发明以单次数据处理耗时来反映滤波器的计算量，HMM-RLS的方法在不考虑器件实现平台的条件下，纯算法复杂度下降90％以上。本发明由FPGA实现HMM-RLS，相比于DSP实现，进一步降低算法所需的运算时间。

Description

一种改进型实用惯性传感器降噪装置

技术领域

本发明属于惯性传感器的标定测试和捷联惯性系统技术领域，具体涉及一种结合模拟滤波和数字滤波的改进型实用惯性传感器降噪装置。

背景技术

MEMS惯性传感器和光纤惯性传感器在国内得到快速发展，受制于现有器件硬件条件和加工工艺的制约，惯性组件(IMU)的精度一直与国外有较大差距。IMU的精度严重影响捷联惯性导航系统的精度。在现有器件的基础上，提高IMU测量精度的方案包括：通过标定消除IMU的确定性误差(安装误差、常值漂移等)，通过降噪技术消除随机性误差。

降噪技术分为模拟降噪技术和数字降噪技术。仅考虑数字降噪技术是不够的，高频干扰信号混叠到低频段，这些信号仅依靠数字滤波器是不够的。而目前的IMU数据采集电路中，对于传感器的模拟信号通常不考虑抗混叠滤波，或仅采用RC电路进行抗混叠滤波；或通过运算放大器搭建抗混叠滤波，这种方案的不足在于截止频率固定。

目前较为常用的惯性器件数字降噪技术主要有FIR数字滤波器、ARMA模型Kalman滤波等，但都在降噪效果、滤波时延等方面难以令人满意。

隐马尔科夫模型(HMM)是一种双重随机过程，其中状态链需要通过观测链才能表现出来，因而称为隐式马尔科夫模型，该模型更适于复杂随机信号处理问题。

从抗混叠滤波和HMM的应用角度，目前迫切需要一种兼顾实用性、降噪效果和滤波延迟的惯性器件降噪方案。

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是：如何解决目前的惯性传感器测量信号包含大量的噪声和高频扰动信号的问题，如何提供改进型的实用惯性传感器降噪装置。

(二)技术方案

为解决上述技术问题，本发明提供一种改进型实用惯性传感器降噪装置，其包括：模拟信号可编程抗混叠滤波模块、AD转换电路、放大电路、FPGA模块、信息处理单元、参数更新下载模块；所述信息处理单元为IMU采集数据的接受单元；

所述FPGA模块包括：零延迟HMM-RLS数字滤波器、IIC单元、串口单元、AD控制单元；其中IIC单元和串口单元用于实现参数更新下载接口；

所述模拟信号可编程抗混叠滤波模块包括：加速度计、模拟电流信号的精密电阻采样单元、FPGA分频输出单元、低通滤波电路；

其中，加速度计为提供模拟电流输出信号的石英加速度计；由采样电阻实现的模拟电流信号的精密电阻采样单元用于加速度计信号的I/V；低通滤波电路用于可编程抗混叠滤波，经过抗混叠滤波处理后的弱电压信号，经过放大电路，达到所述AD转换电路所能采样的幅值范围，AD转换得到的数字信号下行传输至FPGA模块；所述FPGA模块还用于通过AD控制单元完成对AD转换电路和低通滤波电路的控制；

其中，利用下述公式(1)确定抗混叠滤波的阶数，A_s为幅度衰减值，f为阻带频率，f_c为截止频率；具体应用中，结合f＝n_sam×f_samp-f_c，n_sam为过采样率，f_samp为AD的转换频率；

$n\≈\frac{A_s}{20{\log }_{10}\frac{\mathrm{\ω}}{{\mathrm{\ω}}_c}}\≈\frac{A_s}{20{\log }_{10}\frac{f}{f_c}}---\left(1\right)$

低通滤波电路的截止频率设置为可调，其截止频率和CLK引脚的输入频率成比例；

经过抗混叠滤波处理之后的模拟信号，获得信号的预滤波，然后送入下一级的放大电路以及AD转换电路；

其中，所述零延迟HMM-RLS滤波器，其基于惯性器件的HMM模型，并在HMM/KF方法的基础上，推演而来：

(1)马尔科夫模型建立过程，依据惯性器件的输出特性，设计二维隐马尔科夫离散模型，下述公式(2)：

$\left[\begin{array}{c}{x}_k\\ x_{k-1}\end{array}\right]=\left[\begin{array}{cc}{a}_{11}& a_{12}\\ a_{21}& a_{22}\end{array}\right]\left[\begin{array}{c}{x}_{k-1}\\ x_{k-2}\end{array}\right]+w_k---\left(2\right)$

二维隐马尔科夫模型观测模型则如公式(3)可表示为：

$\left[\begin{array}{c}{y}_k\\ y_{k-1}\end{array}\right]=\left[\begin{array}{cc}{c}_{11}& c_{12}\\ c_{21}& c_{22}\end{array}\right]\left[\begin{array}{c}{x}_k\\ x_{k-1}\end{array}\right]+v_k---\left(3\right)$

式中，状态变量为惯性器件的滤波估值，观测量y表示为惯性器件的原始输出；其中，本发明所涉及的带下标的变量均带时效性，涉及的变量下标k-2，k-1，k表征不同的离散时刻；系统噪声w_k和量测噪声v_k皆为零均值的白噪声，满足下述公式(4)：

$\left\{\begin{array}{c}E\left[w_k\right]=0,\mathrm{Cov}[w_k,w_j]=E\left[w_k{w}_j^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[v_k\right]=0,\mathrm{Cov}[v_k,v_j]=E\left[v_k{v}_j^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kj}}\\ \mathrm{Cov}[w_k,v_j]=E\left[w_k{v}_j^T\right]=0\end{array}\right.---\left(4\right)$

Q_k，R_k表示噪声阵系数，δ_kj表示对角阵的元素，E[]，Cov[]为期望和协方差运算符号；

隐马尔科夫和卡尔曼滤波降噪方法可表示为如下述公式(5)及公式(6)：

${\hat{X}}_{k/k-1}=F_{k,k-1}{\hat{X}}_{k-1}---\left(5\right)$

${\hat{X}}_k={\hat{X}}_{k/k-1}+K(Z_k-H_k{\hat{X}}_{k/k-1})--\left(6\right)$

其中，为k-1时刻的状态值，为k时刻相对于k-1时刻

的状态预测值，F_k：k-1为k-1时刻的状态矩阵；为k时刻滤波估值，Z_k为k时刻观测值，H_k为k时刻的量测矩阵；K为卡尔曼滤波增益；根据下述公式(7)、公式(8)及公式(9)计算如下：

$P_{k-1}^{-1}=P_{k-1/k-2}^{-1}+H_{k-1}^T{R}_{k-1}^{-1}{H}_{k-1}---\left(7\right)$

$P_{k/k-1}=F_{k,k-1}{P}_{k-1}{F}_{k,k-1}^T+G_{k,k-1}{Q}_{k-1}{G}_{k,k-1}^T---\left(8\right)$

$K=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(9\right)$

令k时刻观测量为 ${\hat{Z}}_k=\left[\begin{array}{c}{Z}_{k+1}\\ Z_k\end{array}\right],$ 状态量为 ${\hat{X}}_k=\left[\begin{array}{c}{X}_{k+1}\\ X_k\end{array}\right],$ 同时取 $F_{k,k-1}=\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right],H_k=\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right],$ 获得状态的滤波值如公式(10)：

X_k+1＝(1-K)X_k+K Z_k+1    (10)

该公式(10)表明对于HMM/KF方法的描述等价于低通数字递归滤波器，其截至频率由滤波增益系数K确定；

为保证随机信号处理的灵活性，选择遗忘因子优化RLS方法，具体算法根据如下公式组(11)所示：

$\hat{d}\left(n\right)={\mathrm{\ω}}^T(n-1)y\left(n\right)$

$e\left(n\right)=x\left(n\right)-\hat{d}\left(n\right)$

$K\left(n\right)=R_{\mathrm{yy}}^{-1}\left(n\right)y\left(n\right)---\left(11\right)$

$R_{\mathrm{yy}}^{-1}(n+1)=\frac{1}{{\mathrm{\λ}}_R}[R_{\mathrm{yy}}^{-1}\left(n\right)+\frac{K\left(n\right)K^T\left(n\right)}{{\mathrm{\λ}}_R+y^T\left(n\right)K\left(n\right)}]$

ω(n)＝ω(n-1)+k(n)e(n)

式中，y(n)为输入矢量，ω(n)为权值，x(n)为期望输出，e(n)为先验估计误差，R(n)为输入矢量的相关矩阵，K(n)为Kalman滤波增益；上述计算过程的核心为计算Kalman滤波增益；

具体而言，此处所涉及的HMM-RLS数字滤波器的具体算法实施步骤如下：

选择ω(n)为状态向量，初值为零；以k-1时刻的陀螺输出为x(n)，令y(n)取常值1，R_yy初值取为1；λ_R的取值与Kalman滤波增益值一一对应；加速度计和陀螺仪共用一组参数；

HMM-RLS降噪方法具体过程如下：

步骤1：确定滤波参数x(n)、y(n)、R_yy、ω(n)和λ_R的初值；

步骤2：运算Kalman滤波增益，具体根据如下公式(12)及公式(13)进行：

$K\left(n\right)=R_{\mathrm{yy}}^{-1}\left(n\right)y\left(n\right)---\left(12\right)$

$R_{\mathrm{yy}}^{-1}(n+1)=\frac{1}{{\mathrm{\λ}}_R}[R_{\mathrm{yy}}^{-1}\left(n\right)+\frac{K\left(n\right)K^T\left(n\right)}{{\mathrm{\λ}}_R+y^T\left(n\right)K\left(n\right)}]---\left(13\right)$

(3)根据Kalman滤波增益，通过下述公式(14)至公式(16)计算惯性器件滤波输出：

$\hat{d}\left(n\right)={\mathrm{\ω}}^T(n-1)y\left(n\right)---\left(14\right)$

$e\left(n\right)=x\left(n\right)-\hat{d}\left(n\right)---\left(15\right)$

ω(n)＝ω(n-1)+k(n)e(n)    (16)。(三)有益效果

与现有技术相比较，本发明具备如下有益效果：

(1)本发明提供的HMM-RLS滤波的第一次迭代，直接以量测值的原始数据作为滤波输出，实现了降噪方法真正意义上的零延迟。

(2)本发明以单次数据处理耗时来反映滤波器的计算量，HMM-RLS的方法在不考虑器件实现平台的条件下，纯算法复杂度下降90％以上。

(3)本发明由FPGA实现HMM-RLS，相比于DSP实现，进一步降低算法所需的运算时间。在IMU数据采集解算直接进行IMU数据降噪处理，完善了IMU数据采集部分的功能，减轻了导航信息处理单元的运算负担。通过调整可以实现降噪效果的调整，且陀螺仪数据和加速度计数据HMM-RLS滤波过程可共用滤波参数。

(4)本发明针对模拟信号，增加了模拟信号可编程抗混叠滤波电路，通过FPGA提供的分频时钟信号，可以实现抗混叠滤波电路截止频率的在线调整，弥补了目前很多设计仅提供数字滤波器，而无法处理采样过程中的信号混叠问题的不足。

(5)本发明提供了降噪技术中截止频率、降噪参数的可在线调整的灵活性，并设计了参数更新电路。针对参数调整，提供了完善的在线参数更新电路，参数实时更新可通过串口通信和读写EEPROM实现。

附图说明

图1为惯性器件降噪技术模块示意图。

图2为模拟信号可编程抗混叠滤波硬件电路示意图。

图3为HMM/KF滤波增益K调整曲线。

图4为FPGA实现HMM-RLS过程的状态机示意图。

图5为EEPROM接口电路原理图。

图6为FPGA扩展串口接口电路示意图。

图7为对应K＝0.0083加计滤波延迟对比示意图。

图8为对应K＝0.005加计滤波延迟对比示意图。

图9为对应K＝0.003加计滤波延迟对比示意图。

具体实施方式

为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

为解决现有技术的问题，本发明提供一种改进型实用惯性传感器降噪装置，如图1所示，其包括：模拟信号可编程抗混叠滤波模块、AD转换电路、放大电路、FPGA模块、信息处理单元、参数更新下载模块；所述信息处理单元为IMU采集数据的接受单元，该部分本发明不做详细说明；

所述FPGA模块包括：零延迟HMM-RLS数字滤波器、IIC单元、串口单元、AD控制单元；其中IIC单元和串口单元用于实现参数更新下载接口；

所述模拟信号可编程抗混叠滤波模块包括：加速度计、模拟电流信号的精密电阻采样单元、FPGA分频输出单元、LTC 1068的低通滤波电路；

图2所示为模拟信号可编程抗混叠滤波硬件电路示意图，其中，加速度计为提供模拟电流输出信号的石英加速度计；由采样电阻实现的模拟电流信号的精密电阻采样单元用于加速度计信号的I/V；低通滤波电路LTC1068用于可编程抗混叠滤波，经过抗混叠滤波处理后的弱电压信号，经过放大电路，达到所述AD转换电路所能采样的幅值范围，AD转换得到的数字信号下行传输至FPGA模块；所述FPGA模块还用于通过AD控制单元完成对AD转换电路和低通滤波电路LTC1068的控制；

其中，利用下述公式(1)确定抗混叠滤波的阶数，A_s为幅度衰减值，f为阻带频率，f_c为截止频率；具体应用中，需要结合f＝n_sam×f_samp-f_c，n_sam为过采样率，f_samp为AD的转换频率；

$n\≈\frac{A_s}{20{\log }_{10}\frac{\mathrm{\ω}}{{\mathrm{\ω}}_c}}\≈\frac{A_s}{20{\log }_{10}\frac{f}{f_c}}---\left(1\right)$

低通滤波电路LTC1068的截止频率可调，其截止频率和CLK引脚的输入频率成比例，针对LTC1068该比值为1∶100；

利用FilterCAD工具，输入A_s、f、f_s，可以进行抗混叠滤波电路的推荐设计，包括芯片和基本应用电路。对基本滤波电路，需要增加输入输出信号端增加的一阶RC滤波电路，电源的滤波电容。

经过抗混叠滤波处理之后的模拟信号，获得信号的预滤波，然后可以送入下一级的放大电路以及AD转换电路；

其中，所述零延迟HMM-RLS滤波器，其基于惯性器件的HMM模型，并在HMM/KF方法的基础上，推演而来：

(1)马尔科夫模型建立过程，依据惯性器件的输出特性，设计二维隐马尔科夫离散模型，下述公式(2)：

$\left[\begin{array}{c}{x}_k\\ x_{k-1}\end{array}\right]=\left[\begin{array}{cc}{a}_{11}& a_{12}\\ a_{21}& a_{22}\end{array}\right]\left[\begin{array}{c}{x}_{k-1}\\ x_{k-2}\end{array}\right]+w_k---\left(2\right)$

二维隐马尔科夫模型观测模型则如公式(3)可表示为：

$\left[\begin{array}{c}{y}_k\\ y_{k-1}\end{array}\right]=\left[\begin{array}{cc}{c}_{11}& c_{12}\\ c_{21}& c_{22}\end{array}\right]\left[\begin{array}{c}{x}_k\\ x_{k-1}\end{array}\right]+v_k---\left(3\right)$

式中，状态变量为惯性器件的滤波估值，观测量y表示为惯性器件的原始输出；其中，本发明所涉及的带下标的变量均带时效性，涉及的变量下标k-2，k-1，k表征不同的离散时刻；系统噪声w_k和量测噪声v_k皆为零均值的白噪声，满足下述公式(4)：

$\left\{\begin{array}{c}E\left[w_k\right]=0,\mathrm{Cov}[w_k,w_j]=E\left[w_k{w}_j^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[v_k\right]=0,\mathrm{Cov}[v_k,v_j]=E\left[v_k{v}_j^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kj}}\\ \mathrm{Cov}[w_k,v_j]=E\left[w_k{v}_j^T\right]=0\end{array}\right.---\left(4\right)$

Q_k，R_k表示噪声阵系数，δ_kj表示对角阵的元素，E[]，Cov[]为期望和协方差运算符号；

隐马尔科夫和卡尔曼滤波(HMM/KF)降噪方法可表示为如下述公式(5)及公式(6)：

${\hat{X}}_{k/k-1}=F_{k,k-1}{\hat{X}}_{k-1}---\left(5\right)$

${\hat{X}}_k={\hat{X}}_{k/k-1}+K(Z_k-H_k{\hat{X}}_{k/k-1})--\left(6\right)$

其中，为k-1时刻的状态值，为k时刻相对于k-1时刻的状态预测值，F_k，k-1为k-1时刻的状态矩阵；为k时刻滤波估值，Z_k为k时刻观测值，H_k为k时刻的量测矩阵；K为卡尔曼滤波增益；根据下述公式(7)、公式(8)及公式(9)计算如下：

$P_{k-1}^{-1}=P_{k-1/k-2}^{-1}+H_{k-1}^T{R}_{k-1}^{-1}{H}_{k-1}---\left(7\right)$

$P_{k/k-1}=F_{k,k-1}{P}_{k-1}{F}_{k,k-1}^T+G_{k,k-1}{Q}_{k-1}{G}_{k,k-1}^T---\left(8\right)$

$K=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(9\right)$

令k时刻观测量为 ${\hat{Z}}_k=\left[\begin{array}{c}{Z}_{k+1}\\ Z_k\end{array}\right],$ 状态量为 ${\hat{X}}_k=\left[\begin{array}{c}{X}_{k+1}\\ X_k\end{array}\right],$ 同时取 $F_{k,k-1}=\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right],H_k=\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right],$ 可获得状态的滤波值如公式(10)：

X_k+1＝(1-K)X_k+K Z_K+1    (10)

该公式(10)表明对于HMM/KF方法的描述等价于低通数字递归滤波器，其截至频率由滤波增益系数K确定；

常规的数字滤波器，其增益系数K为常值，无法自适应调整。利用HMM/KF方法，滤波增益K值得到自适应调节，逐渐收敛至一个较小的稳态值，过程如图3所示，滤波增益K逐渐收敛至一个较小的稳态值。

RLS方法和Kalman滤波均基于方差最小原则推导得出，因此考虑借助RLS方法获得Kalman滤波增益。

为保证随机信号处理的灵活性，可选择遗忘因子优化RLS方法，具体算法根据如下公式组(11)所示：

$\hat{d}\left(n\right)={\mathrm{\ω}}^T(n-1)y\left(n\right)$

$e\left(n\right)=x\left(n\right)-\hat{d}\left(n\right)$

$K\left(n\right)=R_{\mathrm{yy}}^{-1}\left(n\right)y\left(n\right)---\left(11\right)$

$R_{\mathrm{yy}}^{-1}(n+1)=\frac{1}{{\mathrm{\λ}}_R}[R_{\mathrm{yy}}^{-1}\left(n\right)+\frac{K\left(n\right)K^T\left(n\right)}{{\mathrm{\λ}}_R+y^T\left(n\right)K\left(n\right)}]$

ω(n)＝ω(n-1)+k(n)e(n)

式中，y(n)为输入矢量，ω(n)为权值，x(n)为期望输出，e(n)为先验估计误差，R(n)为输入矢量的相关矩阵，K(n)为Kalman滤波增益；上述计算过程的核心为计算Kalman滤波增益；

具体而言，此处所涉及的HMM-RLS数字滤波器的具体算法实施步骤如下：

选择ω(n)为状态向量，初值为零。以k-1时刻的陀螺输出为x(n)，令y(n)取常值1，R_yy初值取为1。λ_R的取值与Kalman滤波增益值一一对应，建议取值0.9512。加速度计和陀螺仪共用一组参数。

HMM-RLS降噪方法具体过程如下：

步骤1：确定滤波参数x(n)、y(n)、R_yy、ω(n)和λ_R的初值；

步骤2：运算Kalman滤波增益，具体根据如下公式(12)及公式(13)进行：

$K\left(n\right)=R_{\mathrm{yy}}^{-1}\left(n\right)y\left(n\right)---\left(12\right)$

$R_{\mathrm{yy}}^{-1}(n+1)=\frac{1}{{\mathrm{\λ}}_R}[R_{\mathrm{yy}}^{-1}\left(n\right)+\frac{K\left(n\right)K^T\left(n\right)}{{\mathrm{\λ}}_R+y^T\left(n\right)K\left(n\right)}]---\left(13\right)$

(3)根据Kalman滤波增益，通过下述公式(14)至公式(16)计算惯性器件滤波输出：

$\hat{d}\left(n\right)={\mathrm{\ω}}^T(n-1)y\left(n\right)---\left(14\right)$

$e\left(n\right)=x\left(n\right)-\hat{d}\left(n\right)---\left(15\right)$

ω(n)＝ω(n-1)+k(n)e(n)    (16)。

FPGA模块采用并行计算，基于FPGA实现本发明中设计的HMM-RLS降噪方法，能够解决DSP软件实现存在的并行性和速度问题。进一步解放DSP，达到DSP专注导航解算等复杂的数学计算过程。

FPGA实现的HMM-RLS降噪方法，具体实施过程，需要考虑算法顺序执行的状态机应用和算法涉及的乘法器等硬件资源的复用。

图4所示为FPGA实现的HMM-RLS算法顺序执行所需的状态机，HMM-RLS滤波程序共分为三个顺序执行过程：参数初始化状态S0包括S0 initial和S0 step1，其中S0 initial为参数的初始化，S0 step1为的数据更新；S1实现Kalman滤波增益计算，计算完成，置位step2_finish，由状态S1过渡到状态S2，！step2_finish标志计算未完成；S2实现滤波器输出，并置位finsh完成向状态S0的过渡。

HMM-RLS算法涉及标量(选择32位二进制，运算过程所涉及的数据格式，本文不做详细说明)乘除法等运算，必然涉及乘法器等硬件资源的复用。状态S1和S2中涉及的运算模块的复用可参考以下方法，乘法器的输入端口在不同的时刻可以有不同的赋值：

assign mulql_ina＝(state_next＝＝S0)？S0_ina((state_next_2＝＝S1)？S1_ina：ina)；

assign mulql_inb＝(state_next＝＝S0)？S0_inb((state_next_2＝＝S1)？S1inb：inb)；

本发明所涉及的参数更新下载电路：包括FPGA、IIC读写的EEPROM和XR16C2850和MAX3485ESA组成的串口电路。

针对不同应用平台的惯性系统，本发明涉及的参数更新电路，进行滤波参数的更行有两个目的：其一，通过改变参数，改变抗混叠滤波的截止频率和零延迟HMM-RLS滤波的滤波效果；其二改变采样频率，可以适当提高的导航参数解算频率，以其获得更高的精度，与之对应的需要改变滤波参数。

本发明通过IIC扩展了一片EEPROM，并增加串口芯片XR16C2850、MAX3485ESA。图5和6为所涉及的EEPROM和串口电路示意图，图5中T2为三芯跳线，WP为写保护引脚，SCL0、SDA0为IIC信号线。将惯性器件的标定结果和本发明所设计的可编程截止频率参数，通过串口发送到FPGA，FPGA将接收到的参数通过IIC接口写入到EEPROM AT24C02中，当再次上电时，FPGA可直接从AT24C02读取参数。

综上，结合以下信号跟踪能力和计算复杂度，对本发明的优异效果做进一步说明：

(1)HMM-RLS随机漂移处理方法描述及随机信号跟踪能力分析；

根据本发明所涉及的HMM-RLS数字滤波器的具体实施步骤，结合公式(12)、R_yy的初值和常值y(n)，计算得到滤波增益K初值为1。

根据发明中涉及的数字递归滤波实施步骤，即X_k+1＝(1-K)X_k+K Z_k+1。分析得到，发明中提供的HMM-RLS的第一个降噪输出为X_k+1＝K Z_k+1＝Z_k+1。滤波的第一次迭代，直接以量测值的原始数据作为滤波输出，实现了降噪方法真正意义上的零延迟。

本发明针对上述分析，进行如下验证。设HMM/KF方法参数Q和R_a分别为：

Q＝diag{q×10^-3g q×10^-3g}²

R_a＝diag{r×10^-4g r×10^-4g}²

式中，g为重力加速度，q，r的为噪声方差阵参数，具体设置如

表1所示。

表1.三组对比试验的滤波参数设置

表2三种方法的滤波延迟对比

图7、图8、图9为低通数字滤波、HMM/KF滤波和扩展RLS方法(即HMM-RLS)三种方法在表1所提供的参数条件下，所取得的降噪效果对比图。在图7、图8、图9中，最左侧的为HMM-RLS的降噪效果，中间的为HMM/KF的降噪效果，最右侧的低通数字滤波的降噪效果；坐标横轴表示时间，单位为s(秒)，纵轴为加速度单位m/s²，图7、图8、图9依次为X、Y、Z轴对应的加速度。表2给出了对图7、图8、图9结果的数字对比结果。本发明所涉及的降噪方法实现了真正意义上的零延迟滤波输出。

(2)HMM-RLS和HMM/KF计算复杂度的对比；

本发明涉及的HMM-RLS方法将2维HMM/KF方法转化为一维随机信号处理问题，避免了矩阵运算。通过分析两种算法的浮点运算操作数目(floating-point operations，flops)来近似对比两种方法的复杂度。

如下所示，为常用的基本运算所需的flops次数：

1、加、减运算

A∈R^n×m，B∈R^n×m，A±B运算需nm次flops

2、乘运算

A∈R^n×m，B∈R^m×l，AB运算需2mnl-nl次flops。

3、矩阵求逆

A∈R^n×n，A^-1需n³次flops。

本发明中HMM/KF方法涉及n(n＝2)矩阵，所需的flops次数为19n³-n(n＝2，19n³-n＝150)。HMM-RLS仅涉及一维标量计算，运算中所需的flops次数为11。可知HMM-RLS方法所需的flops数仅为HMM/KF方法的7.33％。

本发明以单次数据处理耗时来反映滤波器的计算量，以此来验证flops分析得出的结论。结果如表3所示，总结了HMM/KF方法和HMM-RLS方法的复杂度分别从flops和半实物仿真滤波耗时。flops分析方法得到的结果为7.33％，半实物仿真滤波耗时的6％，两者接近，进而算法复杂度的分析结果得到验证。HMM-RLS的方法在不考虑器件实现平台的条件下，纯算法复杂度下降90％以上。

表3 HMM/KF和HMM-RLS计算复杂度对比

(3)FPGA实现HMM-RLS，相比于DSP实现，进一步降低算法所需的运算时间。在IMU数据采集解算直接进行IMU数据降噪处理，完善了IMU数据采集部分的功能，减轻了导航信息处理单元的运算负担。通过调整可以实现降噪效果的调整，且陀螺仪数据和加速度计数据HMM-RLS滤波过程可共用滤波参数。

(4)本发明针对模拟信号，增加了模拟信号可编程抗混叠滤波电路，通过FPGA提供的分频时钟信号，可以实现抗混叠滤波电路截止频率的在线调整，弥补了目前很多设计仅提供数字滤波器，而无法处理采样过程中的信号混叠问题的不足。

(5)本发明提供了降噪技术中截止频率、降噪参数的可在线调整的灵活性，并设计了参数更新电路。针对参数调整，提供了完善的在线参数更新电路，参数实时更新可通过串口通信和读写EEPROM实现。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (1)

1.一种改进型实用惯性传感器降噪装置，其特征在于，其包括：模拟信号可编程抗混叠滤波模块、AD转换电路、放大电路、FPGA模块、信息处理单元、参数更新下载模块；所述信息处理单元为IMU采集数据的接受单元；

所述FPGA模块包括：零延迟HMM-RLS数字滤波器、IIC单元、串口单元、AD控制单元；其中IIC单元和串口单元用于实现参数更新下载接口；

所述模拟信号可编程抗混叠滤波模块包括：加速度计、模拟电流信号的精密电阻采样单元、FPGA分频输出单元、低通滤波电路；

其中，加速度计为提供模拟电流输出信号的石英加速度计；由采样电阻实现的模拟电流信号的精密电阻采样单元用于加速度计信号的I/V；低通滤波电路用于可编程抗混叠滤波，经过抗混叠滤波处理后的弱电压信号，经过放大电路，达到所述AD转换电路所能采样的幅值范围，AD转换得到的数字信号下行传输至FPGA模块；所述FPGA模块还用于通过AD控制单元完成对AD转换电路和低通滤波电路的控制；

其中，利用下述公式(1)确定抗混叠滤波的阶数，A_s为幅度衰减值，f为阻带频率，f_c为截止频率；具体应用中，结合f＝n_sam×f_samp-f_c，n_sam为过采样率，f_samp为AD的转换频率；

$n\≈\frac{A_s}{20{\log }_{10}\frac{\mathrm{\ω}}{{\mathrm{\ω}}_c}}\≈\frac{A_s}{20{\log }_{10}\frac{f}{f_c}}---\left(1\right)$

低通滤波电路的截止频率设置为可调，其截止频率和CLK引脚的输入频率成比例；

经过抗混叠滤波处理之后的模拟信号，获得信号的预滤波，然后送入下一级的放大电路以及AD转换电路；

其中，所述零延迟HMM-RLS滤波器，其基于惯性器件的HMM模型，并在HMM/KF方法的基础上，推演而来：

(1)马尔科夫模型建立过程，依据惯性器件的输出特性，设计二维隐马尔科夫离散模型，下述公式(2)：

$\left[\begin{array}{c}{x}_k\\ x_{k-1}\end{array}\right]=\left[\begin{array}{cc}{a}_{11}& a_{12}\\ a_{21}& a_{22}\end{array}\right]\left[\begin{array}{c}{x}_{k-1}\\ x_{k-2}\end{array}\right]+w_k---\left(2\right)$

二维隐马尔科夫模型观测模型则如公式(3)可表示为：

$\left[\begin{array}{c}{y}_k\\ y_{k-1}\end{array}\right]=\left[\begin{array}{cc}{c}_{11}& c_{12}\\ c_{21}& c_{22}\end{array}\right]\left[\begin{array}{c}{x}_k\\ x_{k-1}\end{array}\right]+v_k---\left(3\right)$

式中，状态变量为惯性器件的滤波估值，观测量y表示为惯性器件的原始输出；其中，本发明所涉及的带下标的变量均带时效性，涉及的变量下标k-2，k-1，k表征不同的离散时刻；系统噪声w_k和量测噪声v_k皆为零均值的白噪声，满足下述公式(4)：

$\left\{\begin{array}{c}E\left[w_k\right]=0,\mathrm{Cov}[w_k,w_j]=E\left[w_k{w}_j^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[v_k\right]=0,\mathrm{Cov}[v_k,v_j]=E\left[v_k{v}_j^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kj}}\\ \mathrm{Cov}[w_k,v_j]=E\left[w_k{v}_j^T\right]=0\end{array}\right.---\left(4\right)$

Q_k，R_k表示噪声阵系数，δ_kj表示对角阵的元素，E[]，Cov[]为期望和协方差运算符号；

隐马尔科夫和卡尔曼滤波降噪方法可表示为如下述公式(5)及公式(6)：

${\hat{X}}_{k/k-1}=F_{k,k-1}{\hat{X}}_{k-1}---\left(5\right)$

${\hat{X}}_k={\hat{X}}_{k/k-1}+K(Z_k-H_k{\hat{X}}_{k/k-1})---\left(6\right)$

其中，为k-1时刻的状态值，为k时刻相对于k-1时刻的状态预测值，F_k，k-1为k-1时刻的状态矩阵；为k时刻滤波估值，Z_k为k时刻观测值，H_k为k时刻的量测矩阵；K为卡尔曼滤波增益；根据下述公式(7)、公式(8)及公式(9)计算如下：

$P_{k-1}^{-1}=P_{k-1/k-2}^{-1}+H_{k-1}^T{R}_{k-1}^{-1}{H}_{k-1}---\left(7\right)$

$P_{k/k-1}=F_{k,k-1}{P}_{k-1}{F}_{k,k-1}^T+G_{k,k-1}{Q}_{k-1}{G}_{k,k-1}^T---\left(8\right)$

$K=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(9\right)$

令k时刻观测量为 ${\hat{Z}}_k=\left[\begin{array}{c}{Z}_{k+1}\\ Z_k\end{array}\right],$ 状态量为 ${\hat{X}}_k=\left[\begin{array}{c}{X}_{k+1}\\ X_k\end{array}\right],$ 同时取 $F_{k,k-1}=\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right],H_k=\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right],$ 获得状态的滤波值如公式(10)：

X_k+1＝(1-K)X_k+K Z_k+1                  (10)

该公式(10)表明对于HMM/KF方法的描述等价于低通数字递归滤波器，其截至频率由滤波增益系数K确定；

为保证随机信号处理的灵活性，选择遗忘因子优化RLS方法，具体算法根据如下公式组(11)所示：

$\begin{array}{c}\hat{d}\left(n\right)={\mathrm{\ω}}^T(n-1)y\left(n\right)\\ e\left(n\right)=x\left(n\right)-\hat{d}\left(n\right)\\ K\left(n\right)=R_{\mathrm{yy}}^{-1}\left(n\right)y\left(n\right)\\ R_{\mathrm{yy}}^{-1}(n+1)=\frac{1}{{\mathrm{\λ}}_R}[R_{\mathrm{yy}}^{-1}\left(n\right)+\frac{K\left(n\right)K^T\left(n\right)}{{\mathrm{\λ}}_R+y^T\left(n\right)K\left(n\right)}]\\ \mathrm{\ω}\left(n\right)=\mathrm{\ω}(n-1)+k\left(n\right)e\left(n\right)\end{array}---\left(11\right)$

式中，y(n)为输入矢量，ω(n)为权值，x(n)为期望输出，e(n)为先验估计误差，R(n)为输入矢量的相关矩阵，K(n)为Kalman滤波增益；上述计算过程的核心为计算Kalman滤波增益；

具体而言，此处所涉及的HMM-RLS数字滤波器的具体算法实施步骤如下：

选择ω(n)为状态向量，初值为零；以k-1时刻的陀螺输出为x(n)，令y(n)取常值1，R_yy初值取为1；λ_R的取值与Kalman滤波增益值一一对应；加速度计和陀螺仪共用一组参数；

HMM-RLS降噪方法具体过程如下：

步骤1：确定滤波参数x(n)、y(n)、R_yy、ω(n)和λ_R的初值；

步骤2：运算Kalman滤波增益，具体根据如下公式(12)及公式(13)进行：

$K\left(n\right)=R_{\mathrm{yy}}^{-1}\left(n\right)y\left(n\right)---\left(12\right)$

$R_{\mathrm{yy}}^{-1}(n+1)=\frac{1}{{\mathrm{\λ}}_R}[R_{\mathrm{yy}}^{-1}\left(n\right)+\frac{K\left(n\right)K^T\left(n\right)}{{\mathrm{\λ}}_R+y^T\left(n\right)K\left(n\right)}]---\left(13\right)$

(3)根据Kalman滤波增益，通过下述公式(14)至公式(16)计算惯性器件滤波输出：

$\hat{d}\left(n\right)={\mathrm{\ω}}^T(n-1)y\left(n\right)---\left(14\right)$

$e\left(n\right)=x\left(n\right)-\hat{d}\left(n\right)---\left(15\right)$

ω(n)＝ω(n-1)+k(n)e(n)              (16)。