CN111121820B - 基于卡尔曼滤波的mems惯性传感器阵列融合方法 - Google Patents

Abstract

本发明公开了一种基于卡尔曼滤波的MEMS惯性传感器阵列融合方法，采用传感器阵列技术与卡尔曼滤波技术结合，对运动信息数据包中的三轴角速率值和三轴加速度值进行误差标定、温度补偿和卡尔曼滤波算法融合，减小了惯性传感器的噪声，提高了惯性传感器对运动物体的测量精度。同时为了提高惯性传感器阵列在动态条件下的性能，在卡尔曼滤波器中引入自适应渐消因子，通过减小以往估计值在现有估计值的比重来提高滤波效果。该方法精度高、适用性强，有助于解决现有低成本惯性传感器精度过低、难以满足许多实际应用的需求等问题。

Description

基于卡尔曼滤波的MEMS惯性传感器阵列融合方法

技术领域

本发明属于惯性传感器领域，具体涉及一种基于卡尔曼滤波的MEMS惯性传感器阵列融合方法。

背景技术

惯性传感器，包含加速度计和陀螺仪，是分别用于检测加速度信息和角速度信息的传感器。由于惯性传感器完全自主、不受干扰、输出信息量大、输出信息实时性强的优点，在军用、航空航天领域获得广泛应用。随着微机电(MEMS，Micro Electro MechanicalSystem)技术的发展，基于MEMS技术的惯性传感器成本与功耗不断降低，性能也逐渐提升，目前在消费领域，微陀螺和微加速度计随处可见。然而，由于工艺、检测电路等条件的限制，目前微陀螺和微加速度计的精度普遍较低，制约了其在更多领域的应用，国内与国外的差距相对较大。因此，研究如何有效提高微陀螺和微加速度计的精度具有十分重要的理论意义和应用价值。

利用数据融合技术可以提升传感器应用时的性能。将来自多个传感器或多源信息进行综合处理，可以得到更为准确、可靠的结果。因此，可同时使用多个硅微机械陀螺组成阵列，对同一信号进行冗余检测并输出多个检测值，然后采用数据融合技术对这些检测值进行分析综合，得到对输入角速率的最优估计，从而提高陀螺的精度，此即为“虚拟陀螺技术”。

目前国外对虚拟陀螺技术方面的成果鲜有公布，国内的研究大多局限于理论分析和仿真验证上。如2009年，东南大学的吉训生等通过阵列信号的数据融合和滤波处理减小了陀螺仪动态信号的噪声，使其零偏不稳定性提高了2.54倍，漂移性能提高了1.52倍。2017年，昆明理工大学的陈书钊等提出了三种用于多传感器数据融合的Kalman滤波器，利用仿真的方法对搭建的陀螺阵列输出的数据进行处理，对三种模型的效果进行了检验。可见，由于实验条件的限制，目前国内在硬件平台搭建方面的经验不足，对阵列系统在真实动态环境下的研究不多，不利于工程化实现。

发明内容

本发明的目的在于提供一种基于卡尔曼滤波的MEMS惯性传感器阵列融合方法，采用传感器阵列技术与卡尔曼滤波技术结合，解决了现有低成本惯性传感器精度过低、难以满足许多实际应用的需求等问题。

实现本发明目的的技术解决方案为：一种基于卡尔曼滤波的MEMS惯性传感器阵列融合方法，所述惯性传感器阵列内置温度传感器，步骤如下：

步骤1、将惯性传感器阵列固定在运动物体上，采集运动物体的运动信息数据包，通过运动信息数据包的帧头及帧尾判断该运动信息数据包的正确性；若不正确，重复步骤1，接收下一包；若正确，进入步骤2；

步骤2、根据惯性传感器阵列的协议，将运动信息数据包从二进制转换为十进制，其中运动信息数据包括三轴角速率、三轴加速度和温度，每个角速率、加速度和温度数据均由高八位数据D₂和低八位数据D₁构成，则运动信息数据包内数据的十进制值u按照如下公式计算：

u＝(D₂*256+D₁)*r/0xFFFF

其中，r为传感器的量程，对运动信息数据包中的数据依次解析，得到三个轴的角速率值并构成的列向量

三个轴的加速度值并构成的列向量

温度值T，进入步骤3；

步骤3、分别对

和

进行误差标定：

其中，

为标定后的三轴角速率值，

为标定后的加速度值，C_g为陀螺仪的标度因数，

为陀螺仪的零偏，C_a为加速度计的标度因数，

为加速度计的零偏，利用转台对传感器进行标定即可获得C_g、

C_a和

进入步骤4；

步骤4、分别对

和

进行温度补偿，采用四阶多项式拟合，拟合公式如下：

其中，T为当前传感器的温度值，a_n为传感器的温度系数，n＝0,1,2……9，

为温度补偿后的角速率值，

为温度补偿后的加速度值，进入步骤5；

步骤5、利用卡尔曼滤波算法融合各传感器的

和

根据状态方程与预测方程并结合滤波参数进行滤波迭代，具体如下：

X_k＝A_k,k-1X_k-1+W_k-1

Z_k＝H_kX_k+V_k

其中X_k为滤波值，时刻k＝1，2，3……；Z_k为测量值原值，带入

的具体值；A_k,k-1为AR模型参数，W_k-1为系统噪声，量测矩阵H_k为由1构成的行向量，V_k为测量噪声，进入步骤6；

步骤6、结合四元数法利用融合后的角速度值和加速度值解算出姿态、速度、位置信息，四元数法用如下公式表示：

其中，四元数

ω_x、ω_y、ω_z为融合后的角速度值；

载体的姿态角：

其中，θ、γ、ψ即为运动物体的俯仰角、滚转角、偏航角；

计算更新后的姿态转换矩阵

令

f_N、f_E、f_D分别代表北向、东向和地向比力，根据融合后的加速度值求得；

进行速度的更新：

式中，V_N、V_E、V_D分别为北向、东向和地向速度，R_M、R_N分别为考虑地球为椭球时的地球子午圈半径和卯酉圈半径，ω_ie为地球自转角速度，L为纬度，g为重力加速度；

速度信息更新完成后，更新位置信息：

式中，λ为经度，h为高度。

本发明与现有技术相比，其显著优点在于：

(1)传统提高惯性传感器精度的方法一般通过提高电路性能及工艺来实现，难度大，精度提高有限，本发明通过算法提高了传感器对运动物体的测量精度，使其在静态环境下的零偏不稳定性提高了5-7倍，动态环境下的零偏不稳定性提高了3-4倍。

(2)精度高，为进一步提高传感器阵列的输出精度，提出了一种改进的卡尔曼滤波算法，在卡尔曼滤波器中引入自适应渐消因子，通过减小以往估计值在现有估计值的比重来提高滤波效果。

(3)适用性强，利用传感器的标度因数和零偏进行了标定，利用温度系数补偿了传感器阵列的输出，降低了温度对传感器精度的影响，提高了传感器阵列融合的性能，保证了输出的可靠性。

附图说明

图1为本发明基于卡尔曼滤波的MEMS惯性传感器阵列融合方法流程图。

图2为本发明中的卡尔曼滤波流程图。

具体实施方式

下面结合附图对本发明作进一步详细描述。

结合图1，本发明所述的基于卡尔曼滤波的MEMS惯性传感器阵列融合方法，所采用的惯性传感器阵列内置温度传感器，具体步骤如下：

步骤1、将惯性传感器阵列固定在运动物体上，采集运动物体的运动信息数据包，通过运动信息数据包的帧头及帧尾判断该运动信息数据包的正确性；若不正确，重复步骤1，接收下一包；若正确，进入步骤2。

步骤2、根据惯性传感器阵列的协议，将运动信息数据包从二进制转换为十进制，其中运动信息数据包括三轴角速率、三轴加速度和温度，每个角速率、加速度和温度数据均由高八位数据D₂和低八位数据D₁构成，则运动信息数据包内数据的十进制值u按照如下公式计算：

u＝(D₂*256+D₁)*r/0xFFFF

其中，r为传感器的量程，对运动信息数据包中的数据依次解析，得到三个轴的角速率值并构成的列向量

三个轴的加速度值并构成的列向量

温度值T，进入步骤3。

步骤3、分别对

和

进行误差标定：

其中，

为标定后的三轴角速率值，

为标定后的加速度值，C_g为陀螺仪的标度因数，

为陀螺仪的零偏，C_a为加速度计的标度因数，

为加速度计的零偏，利用转台对传感器进行标定即可获得C_g、

C_a和

进入步骤4。

步骤4、分别对

和

进行温度补偿，采用四阶多项式拟合，拟合公式如下：

其中，T为当前传感器的温度值，a_n(n＝0,1,2……9)为传感器的温度系数，

为温度补偿后的角速率值，

为温度补偿后的加速度值；利用温箱对传感器进行温度循环实验，用最小二乘法进行拟合，获取到传感器的温度系数a_n，进入步骤5。

步骤5、利用卡尔曼滤波算法融合各传感器的

和

卡尔曼滤波的具体过程如图2所示。根据状态方程与预测方程并结合滤波参数进行滤波迭代，提高了传感器对运动信息的测量精度，由于卡尔曼滤波器具有无限增长的记忆特性，且其滤波值X_k使用k时刻以前的全部观测数据，因此，考虑在渐消卡尔曼滤波器中引入自适应渐消因子，通过减小以往估计值在现有估计值的比重来提高滤波效果，具体如下：

X_k＝A_k,k-1X_k-1+W_k-1

Z_k＝H_kX_k+V_k

其中X_k为滤波值，时刻k＝1，2，3……；Z_k为测量值原值，带入

的具体值；A_k,k-1为AR模型参数，W_k-1为系统噪声，量测矩阵H_k为由1构成的行向量，V_k为测量噪声，进入步骤6。

步骤6、结合四元数法利用融合后的角速度值和加速度值解算出姿态、速度、位置信息，四元数法用如下公式表示：

其中，四元数

ω_x、ω_y、ω_z为融合后的角速度值，利用该微分方程即可获取每一时刻的四元数。

再根据以下方程即可求解出载体的姿态角：

其中，θ、γ、ψ即为运动物体的俯仰角、滚转角、偏航角。

再根据以下方程可以计算出更新后的姿态转换矩阵：

令

代表北向、东向和地向比力，可根据融合后的加速度值求得。利用如下公式即可进行速度的更新：

式中，V_N、V_E、V_D分别为北向、东向和地向速度，R_M、R_N分别为考虑地球为椭球时的地球子午圈半径和卯酉圈半径，ω_ie为地球自转角速度，L为纬度，g为重力加速度。

速度信息更新完成后，利用下式即可更新位置信息：

式中，λ为经度，h为高度。

实施例1

将惯性传感器阵列固定在三轴转台上，该转台可以模拟物体的运动，采集运动物体的运动信息数据包，通过误差标定、温度补偿和卡尔曼滤波算法融合，减小了陀螺仪和加速度的噪声，使其在静态环境下的零偏不稳定性提高了6.28倍，动态环境下的零偏不稳定性提高了3.51倍。

Claims (1)

1.一种基于卡尔曼滤波的MEMS惯性传感器阵列融合方法，所述惯性传感器阵列内置温度传感器，其特征在于，步骤如下：

步骤1、将惯性传感器阵列固定在运动物体上，采集运动物体的运动信息数据包，通过运动信息数据包的帧头及帧尾判断该运动信息数据包的正确性；若不正确，重复步骤1，接收下一包；若正确，进入步骤2；

步骤2、根据惯性传感器阵列的协议，将运动信息数据包从二进制转换为十进制，其中运动信息数据包括三轴角速率、三轴加速度和温度，每个角速率、加速度和温度数据均由高八位数据D₂和低八位数据D₁构成，则运动信息数据包内数据的十进制值u按照如下公式计算：

u＝(D₂*256+D₁)*r/0xFFFF

其中，r为传感器的量程，对运动信息数据包中的数据依次解析，得到三个轴的角速率值并构成的列向量

三个轴的加速度值并构成的列向量

温度值T，进入步骤3；

步骤3、分别对

和

进行误差标定：

其中，

为标定后的三轴角速率值，

为标定后的加速度值，C_g为陀螺仪的标度因数，

为陀螺仪的零偏，C_a为加速度计的标度因数，

为加速度计的零偏，利用转台对传感器进行标定即可获得C_g、

C_a和

进入步骤4；

步骤4、分别对

和

进行温度补偿，采用四阶多项式拟合，拟合公式如下：

其中，T为当前传感器的温度值，a_n为传感器的温度系数，n＝0,1,2……9，

为温度补偿后的角速率值，

为温度补偿后的加速度值，进入步骤5；

步骤5、利用卡尔曼滤波算法融合各传感器的

和

根据状态方程与预测方程并结合滤波参数进行滤波迭代，具体如下：

X_k＝A_k,k-1X_k-1+W_k-1

Z_k＝H_kX_k+V_k

其中X_k为滤波值，时刻k＝1，2，3……；Z_k为测量值原值，带入

的具体值；A_k,k-1为AR模型参数，W_k-1为系统噪声，量测矩阵H_k为由1构成的行向量，V_k为测量噪声，进入步骤6；

步骤6、结合四元数法利用融合后的角速度值和加速度值解算出姿态、速度、位置信息，四元数法用如下公式表示：

其中，四元数

ω_x、ω_y、ω_z为融合后的角速度值；

载体的姿态角：

其中，θ、γ、ψ即为运动物体的俯仰角、滚转角、偏航角；

计算更新后的姿态转换矩阵

令

f_N、f_E、f_D分别代表北向、东向和地向比力，根据融合后的加速度值求得；

进行速度的更新：

式中，V_N、V_E、V_D分别为北向、东向和地向速度，R_M、R_N分别为考虑地球为椭球时的地球子午圈半径和卯酉圈半径，ω_ie为地球自转角速度，L为纬度，g为重力加速度；

速度信息更新完成后，更新位置信息：

式中，λ为经度，h为高度。

Images