CN110567483B - 一种mems传感器的数据处理方法 - Google Patents

Abstract

本发明提供了一种MEMS传感器的数据处理方法，包括以下步骤：1）初始化时，将k次的六轴数据作为测量值存入存储区；2）计算第k次与第k‑1、k‑2...2、1次数据的差的平均值，以及卡尔曼系数的过程噪声协方差值和测量噪声协方差的值；3）假定第k次数据的预测值等于第k‑1次数据的估计值，将预测值代入卡尔曼滤波算法，得到第k次的数据估计值，以及卡尔曼状态矩阵和增益矩阵；4）用步骤3得到的数据估计值，通过卡尔曼算法计算第k次的角度估计值；5）传感器采集第k+1次的数据后，重复进行步骤2‑4。本发明动态调节卡尔曼滤波预测参数，数据准确、实时性高、波动小，避免了车轮在转向时左右轮产生拖滑现象，保障了汽车转向的稳定性，提升了用户的体验。

Description

一种MEMS传感器的数据处理方法

技术领域

本发明涉及新能源汽车领域,具体涉及一种MEMS传感器的数据处理方法。

背景技术

随着电动汽车的普及，多轮独立电驱动车辆由电动机直接驱动车轮或者采用轮毂电机形式将电动机和车轮集成为一体，省掉了离合器、变速器、传动桥以及差速器等传统传动环节。对于多轮独立电驱动的电动汽车来说，各个车轮由单独的驱动电机驱动，驱动转矩可以独立控制，需要通过计算电子差速的方法解决汽车转向时的差速问题，避免车轮在转向时左右轮产生拖滑现象，在一定程度上保障汽车转向的稳定性。

电子差速需要车辆方向盘转角、车辆的侧倾角、车辆的俯仰角、车辆的三轴加速度、车辆的Z轴角速度变量，其中车辆方向盘转角可以直接获得，其他参数是基于车辆运动条件下的参数，现有的技术中是利用MEMS传感器进行数据采集处理和角度估计。由于从数据采集到计算出电子差速的时间需要在汽车拖滑现象发生前的很短时间内完成，所以要求数据采集以及计算尽可能的快，MEMS传感器相对于其他传感器采集数据更快速、准确。

现有的MEMS传感器的数据处理普遍是采用先用平均值滤波对采样数据进行初步处理，再采用三轴加速度计与三轴陀螺仪的数据进行卡尔曼滤波方法融合计算X、Y、Z的角度值的方法。这种方法应用在车辆角度估计中时，在平均值滤波取滤波数据过多的情况下，会造成有效数据延迟；在平均值滤波取滤波数据较少的情况下，会造成有滤波数据波动大和角度数据波动大的问题；所以这种情况下都会造成有效数据延迟、数据波动大、峰值数据不准确。同样如果使用常规的卡尔曼滤波进行六轴数据的单轴滤波时，也会出现数据在某个范围内数据波动较大或者较小的问题。在高速运动的车辆中，有效数据的延迟、角度数据大幅波动以及峰值数据不准确都会造成车辆功能达不到预期。

发明内容

为解决上述问题，本发明提供了一种MEMS传感器的数据处理方法,该方案计算速度更快，数据更准确，其动态峰值性能更好。

本发明解决其技术问题所采用的技术方案是：一种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)+…+2+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为测量噪声协方差的线性方程斜率。

更进一步地，步骤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次数据的卡尔曼状态矩阵。

更进一步地，步骤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次的角度预测值，wx(k)为第k次的角速度的估计值；步骤4d：针对Y轴方向，计算角度预测值：

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

其中,w_θy(k)为第k次的角度预测值，wy(k)为第k次的角速度的估计值；步骤4e：针对X轴，计算第k次的角度测量值a_θx(k)：

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

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

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

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

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

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

其中：

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₂为二次卡尔曼滤波的测量噪声协方差，是初始化的一个定值。

本发明的有益效果在于：六轴MEMS传感器的单轴数据滤波采用的是动态卡尔曼滤波方法，通过动态调节卡尔曼滤波预测参数，数据准确、实时性高、波动小，更好地避免了车轮在转向时左右轮产生拖滑现象，保障了汽车转向的稳定性，提升了用户的体验。

具体实施方式

为了使本发明的目的及优点更加清楚明白，以下结合实施例对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，不用于限定本发明。

一种MEMS传感器的数据处理方法，所述的数据为六轴数据，包括角速度数据和加速度数据，所述的角速度数据包括X、Y、Z三个方向的角速度，所述的加速度数据包括X、Y、Z三个方向的加速度。本发明所述的数据处理方法包括以下步骤：

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

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

dz＝(k*Z(k)-(∑(Z(k-1)+Z(k-2)+…+2+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次采集数据得到的测量值，Z包括in_wx,in_wy,in_wz,in_ax,in_ay,in_az六个变量；

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

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

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

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

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

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

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

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

值，X包括wx,wy,wz,ax,ay,az六个变量；

步骤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次数据的卡尔曼状态矩阵。

步骤2和步骤3的算法是针对六轴数据分别进行计算的。

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

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

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轴方向，计算第k次的角速度的偏移量：

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轴方向，计算第k次的角度预测值：

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

其中，w_θx(k)为第k次的角度预测值，wx(k)为第k次的角速度的估计值；步骤4d：针对Y轴方向，计算第k次的角度预测值：

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

其中，w_θy(k)为第k次的角度预测值，wy(k)为第k次的角速度的估计值；步骤4e：针对X轴，计算第k次的角度测量值a_θx(k)：

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

将上述的角度预测值和角度测量值进行二次卡尔曼滤波，计算出第k次的角

度估计值：

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

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

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

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

其中：

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₂为二次卡尔曼滤波的测量噪声协方差，是初始化的一个定值。

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

本发明提供一种MEMS传感器的数据处理方法，该方法中针对六轴MEMS传感器的单轴数据滤波采用的是动态卡尔曼滤波方法，通过动态调节卡尔曼滤波预测参数，得到实时的角度估计值。该方法得到的数据准确、实时性高、波动小，更好地避免了车轮在转向时左右轮产生拖滑现象，保障了汽车转向的稳定性，提升了用户的体验。

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

Claims (3)

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₂为二次卡尔曼滤波的测量噪声协方差，是初始化的一个定值。