CN109827545B - 一种基于双mems加速度计的在线倾角测量方法 - Google Patents

Abstract

本发明公开了一种基于双MEMS加速度计的在线倾角测量方法，建立第一和第二MEMS加速度计的三轴零偏误差寄存器；然后，获取运动物体一个时刻的双MEMS加速度计三轴输出；判断这一时刻双MEMS加速度计的三轴输出是否同时都处于静止状态，如果同时都是处于静止状态，则认为物体处于静止状态，否则认为物体是处于运动状态；本发明采用双MEMS加速度计零偏在线估计补偿算法，实现了MEMS加速度计零偏的在线估计与补偿，有效地解决了由于MEMS加速度计零偏变化导致的倾角测量精度下降的问题，提升了倾角测量精度。

Description

一种基于双MEMS加速度计的在线倾角测量方法

技术领域

本发明涉及测量仪器技术领域，具体涉及一种基于双MEMS加速度计的在线倾角测量方法。

背景技术

倾角测量技术经常用于系统的水平、垂直测量。通过对系统偏离水平位置的角度测量反映出被测量对象偏离平衡位置的程度，进而给被测对象的制动控制提供控制信息，作为一种检测方法，它已成为桥梁架设、铁路铺设、土木工程、石油钻井、航空航海、工业自动化、智能平台、机械加工等领域不可缺少的重要测量技术。而随着时代的发展，传统的倾角仪已经不能够满足一些设备对测量精度和量程的要求，缺点也逐渐暴露，如体积大、只能在小范围角度内实现高精度测量、提高精度会造成体积和质量更大等，人们迫切需求一款新型的倾角仪来解决这些问题。随着微机电系统(MEMS,Micro-Electro-MechanicalSystem)技术的出现，采用MEMS传感器集成的倾角仪在同等精度要求的测量条件下，比传统的倾角仪成本低、体积小、可靠性高，被广泛应用于航空航天、建筑行业、工业自动化等需要倾角测量的领域。由MEMS传感器集成的倾角仪系统一般采用MEMS加速度计来组成系统，主要采用三角函数来完成角度的测量。但是MEMS传感器由于内部结构十分微小，因此非常容易受到外界环境因素的干扰，所以相对于传统的倾角仪，由MEMS传感器组成的倾角仪需要较为复杂的误差补偿技术对其输出的数据进行滤波处理，以达到高精度的测量。

发明内容

鉴于技术背景，本发明提出一种基于双MEMS加速度计的在线倾角测量方法，以实现对系统倾角的准确测量，该倾角测量方法中存在两套独立的MEMS加速度计，这两套加速度计的作用有如下两点：1.用于冗余备份，提高系统的稳健性，当其中任意一套加速度计出现故障时，另外一套仍可以确保系统正常运行；2.当两套加速度计设备均可正常工作时，可利用本专利中设计的双MEMS加速度计倾角估计方法，将双MEMS加速度计的输出进行有效融合，同时完成对MEMS加速度计零偏的在线估计与补偿，达到提升倾角测量精度的目的。

为了实现上述目的，本发明的技术方案是：

一种基于双MEMS加速度计的在线倾角测量方法，是对运动物体采用双MEMS加速度计确定横滚角和俯仰角的方法，双MEMS加速度计分别称为第一MEMS加速度计和第二MEMS加速度计，其步骤包括：

步骤一：建立第一MEMS加速度计的三轴零偏误差寄存器，建立第二MEMS加速度计的三轴零偏误差寄存器，将第一和第二MEMS加速度计出厂零偏参数作为第一MEMS加速度计的三轴零偏误差寄存器和第二MEMS加速度计的三轴零偏误差寄存器的初始值；

步骤二：获取运动物体一个时刻的第一MEMS加速度计和第二的MEMS加速度计的三轴输出；

步骤三：判断这一时刻第一MEMS加速度计和第二的MEMS加速度计的三轴输出是否同时都处于静止状态，如果第一MEMS加速度计和第二的MEMS加速度计同时都是处于静止状态，则认为物体处于静止状态，执行物体静止状态的横滚角和俯仰角的计算方法，在物体静止状态的横滚角和俯仰角的计算方法中重新测算第一MEMS加速度计的三轴零偏和第二MEMS加速度计的三轴零偏误差，并对第一MEMS加速度计的三轴零偏误差寄存器和第二MEMS加速度计的三轴零偏误差寄存器的数据进行更新，输出这一时刻物体静止状态横滚角和俯仰角；否则认为物体是处于运动状态，执行物体运动状态的横滚角和俯仰角的计算方法，并输出这一时刻物体运动状态的横滚角和俯仰角；

步骤四：继续获取物体下一个时刻的第一MEMS加速度计和第二的MEMS加速度计的三轴输出，返回步骤三直至测量结束。

方案进一步是：所述判断这一时刻第一MEMS加速度计和第二的MEMS加速度计的三轴输出是否同时都处于静止状态是：判断这一时刻k第一MEMS加速度计k时刻的三轴输出偏离误差，以及第二MEMS加速度计k时刻的三轴输出偏离误差是否同时小于一个设定阈值，如果同时小于一个设定阈值，则认为处于静止状态，否则认为处于运动状态；

第一MEMS加速度计k时刻的三轴输出偏离误差的判断公式是：

第二MEMS加速度计k时刻的三轴输出偏离误差的判断公式是：

其中：

分别代表这一时刻k的第一MEMS加速度计X，Y，Z轴输出；

分别代表这一时刻k的第二MEMS加速度计X，Y，Z轴输出；

分别代表第一MEMS加速度计三轴零偏误差寄存器的X，Y，Z三轴零偏误差值；

分别代表第二MEMS加速度计三轴零偏误差寄存器的X，Y，Z三轴零偏误差值；

g代表当地重力加速度；

thre为静态检测阈值。

方案进一步是：所述静态检测阈值thre为0.4m/s²。

方案进一步是：所述物体运动状态的横滚角和俯仰角的计算方法是：提取第一MEMS加速度计三轴零偏误差寄存器的零偏误差数值对第一MEMS加速度计的三轴输出进行零偏补偿，提取第二MEMS加速度计三轴零偏误差寄存器的零偏误差数值对第二MEMS加速度计的三轴输出进行零偏补偿，然后计算得到第一MEMS加速度计和第二的MEMS加速度计三轴输出零偏补偿后的三轴输出平均值，根据三轴输出平均值计算获得这一时刻的物体运动状态的横滚角和俯仰角。

方案进一步是：所述物体静止状态的横滚角和俯仰角的计算方法是：

构建并定义第一MEMS加速度计和第二的MEMS加速度计零偏与姿态角误差关系模型，以及获得误差关系模型中的误差测量向量；

用kalman滤波方法对零偏与姿态角误差关系模型滤波，将误差测量向量作为滤波输入，从滤波收敛处理获得第一MEMS加速度计k时刻三轴输出横滚角误差、俯仰角误差与三轴零偏误差，以及第二MEMS加速度计k时刻三轴输出横滚角误差、俯仰角误差与三轴零偏误差；

用第一MEMS加速度计k时刻横滚角误差与俯仰角误差数值对第一MEMS加速度计估计得到的横滚角与俯仰角进行误差补偿；

用第二MEMS加速度计k时刻横滚角误差与俯仰角误差数值对第二MEMS加速度计估计得到的横滚角与俯仰角进行误差补偿；

将第一MEMS加速度计与第二MEMS加速度计获得横滚角与俯仰角进行平均，获得这一时刻的物体静止状态的横滚角和俯仰角；

将第一MEMS加速度计和第二的MEMS加速度计k时刻三轴输出零偏误差数值分别对第一MEMS加速度计的三轴零偏误差寄存器和第二MEMS加速度计的三轴零偏误差寄存器的数据进行更新；

其中：

所述构建并定义第一MEMS加速度计和第二的MEMS加速度计零偏与姿态角误差关系模型是：

第一步：由获得静止状态这一时刻k的第一MEMS加速度计和第二的MEMS加速度计的三轴输出计算得到静止状态的第一MEMS加速度计的横滚角与俯仰角初始值和第二MEMS加速度计的横滚角与俯仰角初始值；

当载体坐标系的XYZ轴分别对应于载体的右前上，导航坐标系的XYZ轴为当地的东北天时，其计算公式分别为：

其中：

分别代表

在载体系X，Y，Z轴上的投影；

分别代表

在载体系X，Y，Z轴上的投影；

和

分别是由第一MEMS加速度计三轴输出

计算得到的横滚角与俯仰角值；

和

分别是由第二MEMS加速度计三轴输出

计算得到的横滚角与俯仰角值；

a tan代表反正切函数，a sin代表反正弦函数；

g代表当地重力加速度；

第二步：通过定义k时刻横滚角真值

和俯仰角真值

获得第一MEMS加速度计k时刻三轴输出横滚角误差

与俯仰角误差

以及第二MEMS加速度计k时刻三轴输出横滚角误差

与俯仰角误差

第三步：通过设定k时刻的航向角真值为ψ_k＝0，利用

和ψ_k构建利用第一MEMS加速度计输出获得的姿态转换矩阵

以及利用

和ψ_k构建利用第二MEMS加速度计输出获得的姿态转换矩阵

第四步：利用姿态转换矩阵定义第一MEMS加速度计和第二的MEMS加速度计零偏与姿态角误差关系模型：

x(k+1)＝A_kx(k)+ω

Z(k)＝H_kx(k)+υ

其中：x(k)为在k时刻的误差状态向量，

其中：

是第一MEMS加速度计姿态角误差；

是第二MEMS加速度计姿态角误差；

a¹＝[a¹(x) a¹(y) a¹(z)]^T是第一MEMS加速度计三轴零偏误差；

a²＝[a²(x) a²(y) a²(z)]^T是第二MEMS加速度计三轴零偏误差；

Z(k)为在k时刻的测量向量；

ω和υ分别为对应维度的过程噪声和测量噪声，均为零均值的高斯白噪声，且满足ω～N(0,Q)，υ～N(0,R)，其中Q,R分别代表对应噪声的协方差矩阵；

A_k为k时刻的两个加速度计状态转移矩阵，并且A_k满足

其中：

其中：

代表k时刻的测量矩阵，

其中：

0_2×2代表行数为2，列数为2的零矩阵,0_3×2代表行数为3，列数为2的零矩阵,0_2×3代表行数为2，列数为3的零矩阵,0_3×3代表行数为3，列数为3的零矩阵；I_3×3代表行数为3，列数为3的单位矩阵；

所述获得误差关系模型中的误差测量向量是：

根据计算得到的

和

建立第一MEMS加速度计误差模型和第二MEMS加速度计误差模型；

第一MEMS加速度计误差模型是：

第二MEMS加速度计误差模型是：

由第一MEMS加速度计误差模型和第二MEMS加速度计误差模型获得误差模型的测量向量：

用Ob_k作为kalman滤波输入进行滤波计算。

与现有技术相比，本发明的有益效果：

(1)本发明的倾角测量方案由于具备了双MEMS加速度计设备，当其中任意一套加速度计出现故障时，另外一套仍可以确保系统正常运行；双MEMS加速度计的配置有效提升了系统对于MEMS加速度计的故障异常的鲁棒性，增加了系统的稳健度；

(2)本发明的MEMS加速度计零偏在线估计补偿算法，实现了MEMS加速度计零偏的在线估计与补偿，有效地解决了由于MEMS加速度计零偏变化导致的倾角测量精度下降的问题，提升了倾角测量精度。

下面结合附图和实施例对本发明进行详细描述。

附图说明

图1为基于双MEMS加速度计的倾角测量方法流程框图；

图2为静止状态下的双MEMS加速度计倾角测量方法流程图。

具体实施方式

本实施例提供了一种基于双MEMS加速度计的在线倾角测量方法，是对运动物体采用双MEMS加速度计确定横滚角和俯仰角的方法，双MEMS加速度计分别称为第一MEMS加速度计和第二MEMS加速度计，该方法以实现对系统倾角的准确测量。该倾角测量方法中存在两套独立的MEMS加速度计，这两套加速度计的作用有如下两点：1.用于冗余备份，提高系统的稳健性，当其中任意一套加速度计出现故障时，另外一套仍可以确保系统正常运行；2.当两套加速度计设备均可正常工作时，可利用本专利中设计的双MEMS加速度计倾角估计方法，将双MEMS加速度计的输出进行有效融合，同时完成对MEMS加速度计零偏的在线估计与补偿，达到提升倾角测量精度的目的。

因此，本方法的倾角测量流程可如图1所示：首先双MEMS加速度计开机自检，判断加速度计是否存在故障；如果仅一套加速度计出现故障，则舍弃该加速度计数据，仅使用正常工作的MEMS加速度计数据结合单MEMS加速度计倾角测量方法进行倾角计算；如果双MEMS加速度计均处于正常状态，则进行静止检测，判断系统是否处于静止状态，如果处于运动状态，则利用运动状态下的双MEMS加速度计倾角测量方法进行运算；如果处于静止状态，则利用静止状态下的双MEMS加速度计倾角测量方法进行运算。与传统方案相比，该方法具有更高的系统冗余度，可以有效减少因为器件故障带来的不利影响。

当双MEMS加速度计均处于正常状态，所执行的步骤包括：

步骤一：建立第一MEMS加速度计的三轴零偏误差寄存器，建立第二MEMS加速度计的三轴零偏误差寄存器，将第一和第二MEMS加速度计出厂零偏参数作为第一MEMS加速度计的三轴零偏误差寄存器和第二MEMS加速度计的三轴零偏误差寄存器的初始值；

步骤二：获取运动物体一个时刻的第一MEMS加速度计和第二的MEMS加速度计的三轴输出；

步骤三：判断这一时刻第一MEMS加速度计和第二的MEMS加速度计的三轴输出是否同时都处于静止状态，如果第一MEMS加速度计和第二的MEMS加速度计同时都是处于静止状态，则认为物体处于静止状态，执行物体静止状态的横滚角和俯仰角的计算方法，在物体静止状态的横滚角和俯仰角的计算方法中重新测算第一MEMS加速度计的三轴零偏和第二MEMS加速度计的三轴零偏误差，并对第一MEMS加速度计的三轴零偏误差寄存器和第二MEMS加速度计的三轴零偏误差寄存器的数据进行更新，输出这一时刻物体静止状态横滚角和俯仰角；否则认为物体是处于运动状态，执行物体运动状态的横滚角和俯仰角的计算方法，并输出这一时刻物体运动状态的横滚角和俯仰角；

步骤四：继续获取物体下一个时刻的第一MEMS加速度计和第二的MEMS加速度计的三轴输出，返回步骤三直至测量结束。

其中：所述判断这一时刻第一MEMS加速度计和第二的MEMS加速度计的三轴输出是否同时都处于静止状态是：判断这一时刻k第一MEMS加速度计k时刻的三轴输出偏离误差，以及第二MEMS加速度计k时刻的三轴输出偏离误差是否同时小于一个设定阈值，如果同时小于一个设定阈值，则认为处于静止状态，否则认为处于运动状态；

第一MEMS加速度计k时刻的三轴输出偏离误差的判断公式是：

第二MEMS加速度计k时刻的三轴输出偏离误差的判断公式是：

其中：

分别代表这一时刻k的第一MEMS加速度计X，Y，Z轴输出；

分别代表这一时刻k的第二MEMS加速度计X，Y，Z轴输出；

分别代表第一MEMS加速度计三轴零偏误差寄存器的X，Y，Z三轴零偏误差值；

分别代表第二MEMS加速度计三轴零偏误差寄存器的X，Y，Z三轴零偏误差值；

g代表当地重力加速度；

thre为静态检测阈值。

实施例中：所述静态检测阈值thre取值为0.4m/s²。

实施例中：所述物体运动状态的横滚角和俯仰角的计算方法是：提取第一MEMS加速度计三轴零偏误差寄存器的零偏误差数值对第一MEMS加速度计的三轴输出进行零偏补偿，提取第二MEMS加速度计三轴零偏误差寄存器的零偏误差数值对第二MEMS加速度计的三轴输出进行零偏补偿，然后计算得到第一MEMS加速度计和第二的MEMS加速度计三轴输出零偏补偿后的三轴输出平均值，根据三轴输出平均值计算获得这一时刻的物体运动状态的横滚角和俯仰角。

具体为：

获取第一MEMS加速度计在k时刻的三轴输出输出：

其中：

分别代表

在载体系X，Y，Z轴上的投影；

获取第二MEMS加速度计在k时刻的原始三轴输出：

其中：

分别代表

在载体系X，Y，Z轴上的投影；获取第一MEMS加速度计的三轴零偏：

a¹＝[a¹(x) a¹(y) a¹(z)]^T；

其中：

a¹(x) a¹(y) a¹(z)分别代表a¹在载体系X，Y，Z轴上的投影；

获取第二MEMS加速度计的三轴零偏：

a²＝[a²(x) a²(y) a²(z)]^T；

其中：

a²(x) a²(y) a²(z)分别代表a²在载体系X，Y，Z轴上的投影；

第一MEMS加速度计的三轴零偏的计算存储结果：

其中：

分别代表

在载体系X，Y，Z轴上的投影；

第二MEMS加速度计的三轴零偏的计算存储结果：

其中

分别代表

在载体系X，Y，Z轴上的投影；其中，

为加速度计零偏a¹,a²的估计结果。

对k时刻两套加速度计的输出进行零偏补偿后平均：

根据平均结果计算得到k时刻的横滚角γ_k和俯仰角θ_k，并将横滚角和俯仰角结果进行输出：

如果两套MEMS加速度计开机，并进行开机自检，如果MEMS加速度计均处于异常状态，则该系统无法正常工作，提示异常信息并退出；如果只有一个MEMS加速度计处于异常状态，则将异常的MEMS加速度计信息数据舍弃，仅利用正常工作的MEMS加速度计结合单MEMS加速度计倾角测量方法进行倾角测量，同时输出倾角测量结果。由于此时系统只有一个MEMS加速度计处于正常工作状态，则系统退化为单MEMS加速度计的倾角测量，则利用单MEMS加速度计倾角测量方法进行倾角测量，测量方法介绍如下：

假定加速度计1可以正常工作，加速度计2出现故障。则仅利用加速度计1的原始输出和加速度计零偏计算存储结果计算得到k时刻的横滚角γ_k和俯仰角θ_k，并将横滚角和俯仰角结果进行输出。

其中，atan代表反正切函数，asin代表反正弦函数，g代表当地重力加速度。

实施例中：静止状态下的双MEMS加速度计倾角测量方法流程图如图2所示。其所述物体静止状态的横滚角和俯仰角的计算方法是：

构建并定义第一MEMS加速度计和第二的MEMS加速度计零偏与姿态角误差关系模型，以及获得误差关系模型中的误差测量向量；

用kalman滤波方法对零偏与姿态角误差关系模型滤波，将误差测量向量作为滤波输入，从滤波收敛处理获得第一MEMS加速度计k时刻三轴输出横滚角误差、俯仰角误差与三轴零偏误差，以及第二MEMS加速度计k时刻三轴输出横滚角误差、俯仰角误差与三轴零偏误差；

用第一MEMS加速度计k时刻横滚角误差与俯仰角误差数值对第一MEMS加速度计估计得到的横滚角与俯仰角进行误差补偿；

用第二MEMS加速度计k时刻横滚角误差与俯仰角误差数值对第二MEMS加速度计估计得到的横滚角与俯仰角进行误差补偿；

将第一MEMS加速度计与第二MEMS加速度计获得横滚角与俯仰角进行平均，获得这一时刻的物体静止状态的横滚角和俯仰角；

将第一MEMS加速度计和第二的MEMS加速度计k时刻三轴输出零偏误差数值分别对第一MEMS加速度计的三轴零偏误差寄存器和第二MEMS加速度计的三轴零偏误差寄存器的数据进行更新；

其中：

所述构建并定义第一MEMS加速度计和第二的MEMS加速度计零偏与姿态角误差关系模型是：

第一步：由获得静止状态这一时刻k的第一MEMS加速度计和第二的MEMS加速度计的三轴输出计算得到静止状态的第一MEMS加速度计的横滚角与俯仰角初始值和第二MEMS加速度计的横滚角与俯仰角初始值；

当载体坐标系的XYZ轴分别对应于载体的右前上，导航坐标系的XYZ轴为当地的东北天时，其计算公式分别为：

其中：

分别代表

在载体系X，Y，Z轴上的投影；

分别代表

在载体系X，Y，Z轴上的投影；

和

分别是由第一MEMS加速度计三轴输出

计算得到的横滚角与俯仰角值；

和

分别是由第二MEMS加速度计三轴输出

计算得到的横滚角与俯仰角值；

a tan代表反正切函数，a sin代表反正弦函数；

g代表当地重力加速度；

第二步：通过定义k时刻横滚角真值

和俯仰角真值

获得第一MEMS加速度计k时刻三轴输出横滚角误差

与俯仰角误差

以及第二MEMS加速度计k时刻三轴输出横滚角误差

与俯仰角误差

第三步：通过设定k时刻的航向角真值为ψ_k＝0，利用

和ψ_k构建利用第一MEMS加速度计输出获得的姿态转换矩阵

以及利用

和ψ_k构建利用第二MEMS加速度计输出获得的姿态转换矩阵

第四步：利用姿态转换矩阵定义第一MEMS加速度计和第二的MEMS加速度计零偏与姿态角误差关系模型：

x(k+1)＝A_kx(k)+ω

Z(k)＝H_kx(k)+υ

其中：x(k)为在k时刻的误差状态向量，

其中：

是第一MEMS加速度计姿态角误差；

是第二MEMS加速度计姿态角误差；

a¹＝[a¹(x) a¹(y) a¹(z)]^T是第一MEMS加速度计三轴零偏误差；

a²＝[a²(x) a²(y) a²(z)]^T是第二MEMS加速度计三轴零偏误差；

Z(k)为在k时刻的测量向量；

ω和υ分别为对应维度的过程噪声和测量噪声，均为零均值的高斯白噪声，且满足ω～N(0,Q)，υ～N(0,R)，其中Q,R分别代表对应噪声的协方差矩阵；

A_k为k时刻的两个加速度计状态转移矩阵，并且A_k满足

其中：

其中：

代表k时刻的测量矩阵，

其中：

0_2×2代表行数为2，列数为2的零矩阵,0_3×2代表行数为3，列数为2的零矩阵,0_2×3代表行数为2，列数为3的零矩阵,0_3×3代表行数为3，列数为3的零矩阵；I_3×3代表行数为3，列数为3的单位矩阵；

所述获得误差关系模型中的误差测量向量是：

根据计算得到的

和

建立第一MEMS加速度计误差模型和第二MEMS加速度计误差模型；

第一MEMS加速度计误差模型是：

第二MEMS加速度计误差模型是：

由第一MEMS加速度计误差模型和第二MEMS加速度计误差模型获得误差模型的测量向量：

用Ob_k作为kalman滤波输入进行滤波计算。

在零偏与姿态角误差关系模型作为滤波模型，与误差测量向量Ob_k作为滤波输入的基础上，利用kalman滤波方法进行滤波处理，得到

与

的估计结果，利用该估计结果对横滚角及俯仰角估计结果进行修正补偿。kalman滤波是一种已知技术，本实施例不再详细介绍，基于kalman滤波的倾角在线估计算法过程如下，为避免引起不必要的混淆，定义该在线估计算法的起始时刻为j，即下述滤波的初始计算值为：

与P_j，分别对应于倾角在线估计算法的状态初始估计结果与初始估计误差协方差，必须指出的是：

其中

与

为加速度计零偏的已有存储结果。则在k时刻的倾角在线估计算法过程如下：

P_k,k-1＝A_kP_k-1A_k ^T+Q

K_k＝P_k,k-1H_k ^T[H_kP_k,k-1H_k ^T+R]^-1

P_k＝[I_10×10-K_kH_k]P_k,k-1

其中，

代表k-1步滤波结果(对x(k-1)的估计结果)，

代表k步滤波结果(对x(k)的估计结果)，同时为第k步的最终估计输出结果，

代表k-1步的状态一步预测，P_k,k-1代表k-1步的预测误差方差，K_k代表k步的滤波增益，P_k代表k步滤波误差方差，P_k-1代表k-1步的滤波误差方差，I_10×10代表行数为10，列数为10的单位矩阵。

其中：

与

分别代表a¹与a²在k时刻的估计值，

其中：

为

的在k时刻的估计值，

为

的在k时刻的估计值；

为

的在k时刻的估计值，

为

的在k时刻的估计值。

经过一段时间的滤波处理，等待滤波结果收敛后，最终得到

的稳定收敛值，定义在第L步实现了滤波的稳定收敛，则提取出

中的

与

即为最终估计得到的角度误差，将估计出的角度误差进行反馈修正，并将加速度计1的结果与加速度计2的结果进行平均，得到最终的估计结果如下：

其中，

为最终估计得到的横滚角结果，

为最终估计得到的俯仰角结果，并将其输出。而

中的

与

则作为新的加速度计零偏估计结果而对原有加速度计零偏估计结果进行替换，即

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于双MEMS加速度计的在线倾角测量方法，是对运动物体采用双MEMS加速度计确定横滚角和俯仰角的方法，双MEMS加速度计分别称为第一MEMS加速度计和第二MEMS加速度计，其特征在于：

步骤一：建立第一MEMS加速度计的三轴零偏误差寄存器，建立第二MEMS加速度计的三轴零偏误差寄存器，将第一和第二MEMS加速度计出厂零偏参数作为第一MEMS加速度计的三轴零偏误差寄存器和第二MEMS加速度计的三轴零偏误差寄存器的初始值；

步骤二：获取运动物体一个时刻的第一MEMS加速度计和第二的MEMS加速度计的三轴输出；

步骤三：判断这一时刻第一MEMS加速度计和第二的MEMS加速度计的三轴输出是否同时都处于静止状态，如果第一MEMS加速度计和第二的MEMS加速度计同时都是处于静止状态，则认为物体处于静止状态，执行物体静止状态的横滚角和俯仰角的计算方法，在物体静止状态的横滚角和俯仰角的计算方法中重新测算第一MEMS加速度计的三轴零偏和第二MEMS加速度计的三轴零偏误差，并对第一MEMS加速度计的三轴零偏误差寄存器和第二MEMS加速度计的三轴零偏误差寄存器的数据进行更新，输出这一时刻物体静止状态横滚角和俯仰角；否则认为物体是处于运动状态，执行物体运动状态的横滚角和俯仰角的计算方法，并输出这一时刻物体运动状态的横滚角和俯仰角；

步骤四：继续获取物体下一个时刻的第一MEMS加速度计和第二的MEMS加速度计的三轴输出，返回步骤三直至测量结束；

所述物体运动状态的横滚角和俯仰角的计算方法是：提取第一MEMS加速度计三轴零偏误差寄存器的零偏误差数值对第一MEMS加速度计的三轴输出进行零偏补偿，提取第二MEMS加速度计三轴零偏误差寄存器的零偏误差数值对第二MEMS加速度计的三轴输出进行零偏补偿，然后计算得到第一MEMS加速度计和第二的MEMS加速度计三轴输出零偏补偿后的三轴输出平均值，根据三轴输出平均值计算获得这一时刻的物体运动状态的横滚角和俯仰角；

所述物体静止状态的横滚角和俯仰角的计算方法是：

构建并定义第一MEMS加速度计和第二的MEMS加速度计零偏与姿态角误差关系模型，以及获得误差关系模型中的误差测量向量；

用kalman滤波方法对零偏与姿态角误差关系模型滤波，将误差测量向量作为滤波输入，从滤波收敛处理获得第一MEMS加速度计k时刻三轴输出横滚角误差、俯仰角误差与三轴零偏误差，以及第二MEMS加速度计k时刻三轴输出横滚角误差、俯仰角误差与三轴零偏误差；

用第一MEMS加速度计k时刻横滚角误差与俯仰角误差数值对第一MEMS加速度计估计得到的横滚角与俯仰角进行误差补偿；

用第二MEMS加速度计k时刻横滚角误差与俯仰角误差数值对第二MEMS加速度计估计得到的横滚角与俯仰角进行误差补偿；

将第一MEMS加速度计与第二MEMS加速度计获得横滚角与俯仰角进行平均，获得这一时刻的物体静止状态的横滚角和俯仰角；

将第一MEMS加速度计和第二的MEMS加速度计k时刻三轴输出零偏误差数值分别对第一MEMS加速度计的三轴零偏误差寄存器和第二MEMS加速度计的三轴零偏误差寄存器的数据进行更新；

其中：

所述构建并定义第一MEMS加速度计和第二的MEMS加速度计零偏与姿态角误差关系模型是：

第一步：由获得静止状态这一时刻k的第一MEMS加速度计和第二的MEMS加速度计的三轴输出计算得到静止状态的第一MEMS加速度计的横滚角与俯仰角初始值和第二MEMS加速度计的横滚角与俯仰角初始值；

当载体坐标系的XYZ轴分别对应于载体的右前上，导航坐标系的XYZ轴为当地的东北天时，其计算公式分别为：

其中：

分别代表

在载体系X，Y，Z轴上的投影；

分别代表

在载体系X，Y，Z轴上的投影；

和

分别是由第一MEMS加速度计三轴输出

计算得到的横滚角与俯仰角值；

和

分别是由第二MEMS加速度计三轴输出

计算得到的横滚角与俯仰角值；

atan代表反正切函数，asin代表反正弦函数；

g代表当地重力加速度；

第二步：通过定义k时刻横滚角真值

和俯仰角真值

获得第一MEMS加速度计k时刻三轴输出横滚角误差

与俯仰角误差

以及第二MEMS加速度计k时刻三轴输出横滚角误差

与俯仰角误差

第三步：通过设定k时刻的航向角真值为ψ_k＝0，利用

和ψ_k构建利用第一MEMS加速度计输出获得的姿态转换矩阵

以及利用

和ψ_k构建利用第二MEMS加速度计输出获得的姿态转换矩阵

第四步：利用姿态转换矩阵定义第一MEMS加速度计和第二的MEMS加速度计零偏与姿态角误差关系模型：

x(k+1)＝A_kx(k)+ω

Z(k)＝H_kx(k)+υ

其中：x(k)为在k时刻的误差状态向量，

其中：

是第一MEMS加速度计姿态角误差；

是第二MEMS加速度计姿态角误差；

a¹＝[a¹(x) a¹(y) a¹(z)]^T是第一MEMS加速度计三轴零偏误差；

a²＝[a²(x) a²(y) a²(z)]^T是第二MEMS加速度计三轴零偏误差；

Z(k)为在k时刻的测量向量；

ω和υ分别为对应维度的过程噪声和测量噪声，均为零均值的高斯白噪声，且满足ω～N(0,Q)，υ～N(0,R)，其中Q,R分别代表对应噪声的协方差矩阵；

A_k为k时刻的两个加速度计状态转移矩阵，并且A_k满足

其中：

其中：

代表k时刻的测量矩阵，

其中：

0_2×2代表行数为2，列数为2的零矩阵,0_3×2代表行数为3，列数为2的零矩阵,0_2×3代表行数为2，列数为3的零矩阵,0_3×3代表行数为3，列数为3的零矩阵；I_3×3代表行数为3，列数为3的单位矩阵；

所述获得误差关系模型中的误差测量向量是：

根据计算得到的

和

建立第一MEMS加速度计误差模型和第二MEMS加速度计误差模型；

第一MEMS加速度计误差模型是：

第二MEMS加速度计误差模型是：

由第一MEMS加速度计误差模型和第二MEMS加速度计误差模型获得误差模型的测量向量：

用Ob_k作为kalman滤波输入进行滤波计算。

2.根据权利要求1所述的基于双MEMS加速度计的在线倾角测量方法，其特征在于，所述判断这一时刻第一MEMS加速度计和第二的MEMS加速度计的三轴输出是否同时都处于静止状态是：判断这一时刻k第一MEMS加速度计k时刻的三轴输出偏离误差，以及第二MEMS加速度计k时刻的三轴输出偏离误差是否同时小于一个设定阈值，如果同时小于一个设定阈值，则认为处于静止状态，否则认为处于运动状态；

第一MEMS加速度计k时刻的三轴输出偏离误差的判断公式是：

第二MEMS加速度计k时刻的三轴输出偏离误差的判断公式是：

其中：

分别代表这一时刻k的第一MEMS加速度计X，Y，Z轴输出；

分别代表这一时刻k的第二MEMS加速度计X，Y，Z轴输出；

分别代表第一MEMS加速度计三轴零偏误差寄存器的X，Y，Z三轴零偏误差值；

分别代表第二MEMS加速度计三轴零偏误差寄存器的X，Y，Z三轴零偏误差值；

g代表当地重力加速度；

thre为静态检测阈值。

3.根据权利要求2所述的基于双MEMS加速度计的在线倾角测量方法，其特征在于，所述静态检测阈值thre为0.4m/s²。

Images