[发明专利]一种MEMS传感器的数据处理方法

权利要求书

1.一种MEMS传感器的数据处理方法，所述的数据为六轴数据，包括角速度数据和加速度数据，所述的角速度数据包括X、Y、Z三个方向的角速度，所述的加速度数据包括X、Y、Z三个方向的加速度，其特征在于，所述的数据处理方法包括以下步骤：

步骤1：初始化时，将MEMS传感器采集的k次的六轴数据作为测量值存入存储区，k≥1；

步骤2：计算第k次数据与第k-1、k-2....2、1次数据的差的平均值，并计算卡尔曼系数的过程噪声协方差值Q和测量噪声协方差的值R；

步骤3：假定第k次数据的预测值等于第k-1次数据的估计值，将所述的预测值代入卡尔曼滤波算法，得到第k次的MEMS传感器的数据估计值，以及卡尔曼状态矩阵和增益矩阵；

步骤4：用步骤3得到的数据估计值，通过卡尔曼算法计算第k次的角度估计值；

步骤5：传感器采集第k+1次的数据后，重复进行步骤2-4；

所述步骤1至步骤5中的数据处理方法针对六轴数据的六个数据变量分别计算，

步骤2通过以下公式分别计算第k次数据的Q(k)和R(k)：

dz＝(k*Z(k)-(∑(Z(k-1)+Z(k-2)+…+Z(2)+Z(1))))/(k-1)

Q(k)＝C+D*|dz|

R(k)＝|A+B*Q(k)|

其中，

dz为第k个数据与第k-1、k-2....2、1个数据的差的平均值；

k为采集数据的次数；

Z(k)为第k次采集数据得到的测量值；

C为过程噪声协方差的线性方程常数；

D为过程噪声协方差的线性方程斜率；

A为测量噪声协方差的线性方程常数；

B为测量噪声协方差的线性方程斜率。

2.如权利要求1所述的一种MEMS传感器的数据处理方法，其特征在于，步骤3包括以下子步骤：

步骤3a：假定第k次数据的预测值等同于第k-1次数据的估计值：

X(k)^-＝X(k-1)

其中，X(k)^-为第k次数据的预测值，X(k-1)为第k-1次数据的估计值；

步骤3b：将所述的第k次数据的预测值X(k)^-及第k次数据的测量值Z(k)代入卡尔曼算法，计算出第k次的数据估计值X(k)、卡尔曼增益矩阵K(k)和卡尔曼状态矩阵P(k)：

P(k)^-＝P(k-1)+Q(k)

X(k)＝X(k)^-+K(k)(Z(k)-X(k)^-)

K(k)＝P(k)^-/(P(k)^-+R(k))

P(k)＝(1-K(k))P(k)^-

其中：

P(k)^-为第k次的状态矩阵计算的中间值；

P(k-1)为k-1时刻的状态矩阵；

Q(k)为步骤2计算出来的第k次的过程噪声协方差；

R(k)为步骤2计算出来的第k次的测量噪声协方差；

K(k)为第k次数据的卡尔曼增益矩阵；

P(k)为第k次数据的卡尔曼状态矩阵。

3.如权利要求1所述的一种MEMS传感器的数据处理方法，其特征在于，步骤4包括如下子步骤：

步骤4a：针对X轴方向，计算角速度的偏移量：

wxoff(k)＝1/dt*(θx(k-1)-w_θx(k-1))

其中：

wxoff(k)为第k次X轴方向角速度的偏移量：

θx(k-1)为第k-1次角度计算得到的X轴方向的角度估计值；

w_θx(k-1)为第k-1次用测量数据得到的X轴方向的角度估计值；

步骤4b：针对Y轴方向，计算角速度的偏移量：

wyoff(k)＝1/dt*(θy(k-1)-w_θy(k-1))

其中：

wyoff(k)为第k次Y轴方向角速度的偏移量；

θy(k-1)为第k-1次角度计算得到的Y轴方向的角度估计值；

w_θy(k-1)为第k-1用测量数据得到的Y轴方向的角度估计值；

步骤4c：针对X轴方向，计算角度预测值：

w_θx(k)＝θx(k-1)+(wx(k)-wxoff(k))*dt

其中，w_θx(k)为第k次的x方向角度预测值，wx(k)为第k次的x方向角速度的估计值；

步骤4d：针对Y轴方向，计算角度预测值：

w_θy(k)＝θy(k-1)+(wy(k)-wyoff(k))*dt

其中，w_θy(k)为第k次的y方向角度预测值，wy(k)为第k次的y方向角速度的估计值；

步骤4e：针对X轴，计算第k次的角度测量值a_θx(k)：

步骤4f：针对Y轴，计算第k次的角度测量值a_θy(k)：

步骤4g：将上述的角度预测值和角度测量值进行二次卡尔曼滤波，计算第k次的角度估计值：

P₂(k)^-＝P₂(k-1)+Q₂ (15)

X₂(k)＝X₂(k)^-+K₂(k)(Z₂(k)-X₂(k)^-) (16)

K₂(k)＝P₂(k)^-/(P₂(k)^-+R₂) (17)

P₂(k)＝(1-K₂(k))P₂(k)^- (18)

其中：

X₂(k)为第k次的角度估计值，包括θx(k)和θy(k)两个变量；

X₂(k)^-为第k次的角度预测值，包括w_θy(k)和w_θx(k)两个变量；

Z₂(k)为第k次的角度测量值，包括a_θx(k)和a_θy(k)两个变量；

K₂(k)为二次卡尔曼滤波中第k次的卡尔曼增益矩阵；

P₂(k)为二次卡尔曼滤波中第k次的卡尔曼状态矩阵；

Q₂为二次卡尔曼滤波的过程噪声协方差，是初始化的一个定值；

R₂为二次卡尔曼滤波的测量噪声协方差，是初始化的一个定值。