CN108827291B - 运动载体下的mems陀螺仪输出的零偏补偿方法及装置 - Google Patents

Abstract

本发明实施例提供的一种运动载体下的MEMS陀螺仪输出的零偏补偿方法及装置，该方法包括：在第一采集时刻和第二采集时刻分别获取地磁计的第一信息和加速度计的第二信息，根据所述第一信息和所述第二信息分别建立所述地磁计的地磁四元数、所述陀螺仪的姿态四元数，根据所述地磁四元数获得共轭四元数，并根据所述共轭四元数和所述姿态四元数获得误差四元数，根据所述误差四元数获得对应的平台误差角；根据所述平台误差角和预设的陀螺零偏反馈系数分别获得对应于所述第一采集时刻和所述第二采集时刻的所述陀螺仪的零偏值，根据所述陀螺仪的零偏值获得对应于第二采集时刻的所述陀螺仪的输出值，实现快速补偿MEMS陀螺零偏，确保陀螺仪稳定输出。

Description

运动载体下的MEMS陀螺仪输出的零偏补偿方法及装置

技术领域

本发明涉及导航技术领域，尤其涉及一种运动载体下的MEMS陀螺仪输出的零偏补偿方法及装置。

背景技术

陀螺零偏补偿是为提高惯性导航精度服务的，对于高精度机电陀螺或光学陀螺，陀螺参数具有较高的逐次加电一致性，在使用前进行标定即可满足运动载体的导航精度需求，但对于MEMS陀螺，特别是消费级MEMS陀螺，零偏逐次加电重复性较差，变化幅度可达到1000°/h小时以上，若不进行补偿则无法实现在运动载体上导航的目的。

发明内容

本发明提供一种运动载体下的MEMS陀螺仪输出的零偏补偿方法及装置，用于解决现有技术中对于MEMS陀螺不进行补偿则无法实现在运动载体上导航的问题。

第一方面，本发明实施例提供一种运动载体下的MEMS陀螺仪输出的零偏补偿方法，包括：

在第一采集时刻和第二采集时刻分别获取地磁计的第一信息和加速度计的第二信息，根据所述第一信息和所述第二信息分别建立所述地磁计的地磁四元数、所述陀螺仪的姿态四元数；

根据所述地磁四元数获得共轭四元数，并根据所述共轭四元数和所述姿态四元数获得误差四元数；

根据所述误差四元数获得对应的平台误差角；

根据所述平台误差角和预设的陀螺零偏反馈系数分别获得对应于所述第一采集时刻和所述第二采集时刻的所述陀螺仪的零偏值；

根据所述陀螺仪的零偏值获得对应于第二采集时刻的所述陀螺仪的输出值；

其中，所述第二采集时刻晚于所述第一采集时刻。

可选地，在第一采集时刻和第二采集时刻分别获取地磁计的第一信息和加速度计的第二信息，根据所述第一信息和所述第二信息分别建立所述地磁计的地磁四元数、所述陀螺仪的姿态四元数，包括：

获取所述地磁计的标定参数和输出值，所述标定参数和输出值作为所述第一信息；

根据所述地磁计的标定参数和输出值获得所述陀螺仪的航向；

获取所述加速度计的俯仰角、横滚角，所述俯仰角和横滚角作为所述第二信息；

根据所述航向、俯仰角和横滚角获得所述地磁四元数和所述姿态四元数。

可选地，根据所述地磁四元数获得共轭四元数，并根据所述共轭四元数和所述姿态四元数获得误差四元数，包括：

根据所述地磁四元数采用第一计算公式获得共轭四元数，所述第一计算公式为：

其中，

为共轭四元数，q_M0，q_Mx，q_My，q_Mz均为地磁四元数的四个数值；

根据共轭四元数、姿态四元数采用第二计算公式获得误差四元数，所述第二计算公式为：

其中，dq为误差四元数，

为共轭四元数，q_G为姿态四元数。

可选地，根据所述误差四元数获得对应的平台误差角，包括：

根据所述误差四元数采用第三计算公式获得对应的平台误差角，所述第三计算公式为：

δλ_x＝0.5·dq₀·dq₁

δλ_y＝0.5·dq₀·dq₂

δλ_z＝0.5·dq₀·dq₃

其中，设δλ(x，y，z)为平台误差角，δλ_x、δλ_y、δλ_z为平台误差角的三个数值，dq₀、dq₁、dq₂、dq₃为误差四元数的四个数值。

可选地，根据所述陀螺仪的零偏值获得对应于第二采集时刻的所述陀螺仪的输出值，包括：

将对应于所述第一采集时刻的零偏值和对应于所述第二采集时刻的零偏值进行差值计算，获得变化幅度值；

将变化幅度值与预设的零偏收敛阈值进行比较，当变化幅度值小于零偏收敛阈值，则将对应于第二采集时刻的陀螺仪的输出值减去对应的零偏值作为所述陀螺仪的输出值。

第二方面，本发明实施例提供一种运动载体下的MEMS陀螺仪输出的零偏补偿装置，包括：

建立模块，用于在第一采集时刻和第二采集时刻分别获取地磁计的第一信息和加速度计的第二信息，根据所述第一信息和所述第二信息分别建立所述地磁计的地磁四元数、所述陀螺仪的姿态四元数；

第一计算模块，用于根据所述地磁四元数获得共轭四元数，并根据所述共轭四元数和所述姿态四元数获得误差四元数；

第二计算模块，用于根据所述误差四元数获得对应的平台误差角；

第三计算模块，用于根据所述平台误差角和预设的陀螺零偏反馈系数分别获得对应于所述第一采集时刻和所述第二采集时刻的所述陀螺仪的零偏值；

输出模块，用于根据所述陀螺仪的零偏值获得对应于第二采集时刻的所述陀螺仪的输出值；

其中，所述第二采集时刻晚于所述第一采集时刻。

可选地，所述建立模块具体用于：

获取所述地磁计的标定参数和输出值，所述标定参数和输出值作为所述第一信息；

根据所述地磁计的标定参数和输出值获得所述陀螺仪的航向；

获取所述加速度计的俯仰角、横滚角，所述俯仰角和横滚角作为所述第二信息；

根据所述航向、俯仰角和横滚角获得所述地磁四元数和所述姿态四元数。

可选地，所述第一计算模块具体用于：

根据所述地磁四元数采用第一计算公式获得共轭四元数，所述第一计算公式为：

其中，

为共轭四元数，q_M0，q_Mx，q_My，q_Mz均为地磁四元数的四个数值；

根据共轭四元数、姿态四元数采用第二计算公式获得误差四元数，所述第二计算公式为：

其中，dq为误差四元数，

为共轭四元数，q_G为姿态四元数。

可选地，所述第二计算模块具体用于：

根据所述误差四元数采用第三计算公式获得对应的平台误差角，所述第三计算公式为：

δλ_x＝0.5·dq₀·dq₁

δλ_y＝0.5·dq₀·dq₂

δλ_z＝0.5·dq₀·dq₃

其中，设δλ(x，y，z)为平台误差角，δλ_x、δλ_y、δλ_z为平台误差角的三个数值，dq₀、dq₁、dq₂、dq₃为误差四元数的四个数值。

可选地，所述输出模块具体用于：

将对应于所述第一采集时刻的零偏值和对应于所述第二采集时刻的零偏值进行差值计算，获得变化幅度值；

将变化幅度值与预设的零偏收敛阈值进行比较，当变化幅度值小于零偏收敛阈值，则将对应于第二采集时刻的陀螺仪的输出值减去对应的零偏值作为所述陀螺仪的输出值。

由上述技术方案可知，本发明实施例提供的一种运动载体下的MEMS陀螺仪输出的零偏补偿方法及装置，通过在第一采集时刻和第二采集时刻分别获取地磁计的第一信息和加速度计的第二信息，根据所述第一信息和所述第二信息分别建立所述地磁计的地磁四元数、所述陀螺仪的姿态四元数，根据所述地磁四元数获得共轭四元数，并根据所述共轭四元数和所述姿态四元数获得误差四元数，根据所述误差四元数获得对应的平台误差角；根据所述平台误差角和预设的陀螺零偏反馈系数分别获得对应于所述第一采集时刻和所述第二采集时刻的所述陀螺仪的零偏值，根据所述陀螺仪的零偏值获得对应于第二采集时刻的所述陀螺仪的输出值，实现快速补偿MEMS陀螺零偏，确保陀螺仪稳定输出。

附图说明

图1为本发明一实施例提供的运动载体下的MEMS陀螺仪输出的零偏补偿方法的流程示意图；

图2为本发明一实施例提供的运动载体下的MEMS陀螺仪输出的零偏补偿装置的结构示意图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

图1示出了本发明一实施例提供一种运动载体下的MEMS陀螺仪输出的零偏补偿方法，包括：

S11、在第一采集时刻和第二采集时刻分别获取地磁计的第一信息和加速度计的第二信息，根据所述第一信息和所述第二信息分别建立所述地磁计的地磁四元数、所述陀螺仪的姿态四元数；

S12、根据所述地磁四元数获得共轭四元数，并根据所述共轭四元数和所述姿态四元数获得误差四元数；

S13、根据所述误差四元数获得对应的平台误差角；

S14、根据所述平台误差角和预设的陀螺零偏反馈系数分别获得对应于所述第一采集时刻和所述第二采集时刻的所述陀螺仪的零偏值；

S15、根据所述陀螺仪的零偏值获得对应于第二采集时刻的所述陀螺仪的输出值；

其中，所述第二采集时刻晚于所述第一采集时刻。

针对步骤S11-步骤S15，需要说明的是，在本发明实施例中，第一采集时刻和第二采集时刻为相邻的两个采集时刻，所述第二采集时刻晚于所述第一采集时刻。

在本发明实施例中，获取地磁计的信息之前，首先要对地磁计的参数进行标定。地磁计的误差主要归纳为三类，分别是当量误差、零偏以及非正交误差，列写磁力计输出椭圆方程：

分别为磁力计在水平方向的输出值，其与地磁真值

之间的关系可以表示为：

对于归一化标定结果，

可以表示为

的形式，将(2)代入归一化表达式后即可表示为(1)式的形式。对a,b,ρ,x₀,y₀展开后可得到各参数与参数A-F的变换关系，在此不再赘述。

a,b,ρ,x₀,y₀分别为地磁X轴半径、地磁Y轴半径、椭圆倾斜角、X轴向零偏、Y轴向零偏。

将相关误差项与地磁计输出关联，为了实现对参数A-F的最优估计，可以对上式进行适当改造，以矩阵形式表示从而适应最小二乘计算，如式(3)所示：

经过多次迭代计算后，所有的地磁计测试数据可合成为一个大矩阵，矩阵可表示为如下形式：

X×P＝W (4)

其中：

通过最小二乘计算得到最优估计，计算公式可表示如下：

P_est＝(X^TX)^-1X (8)

经过最小二乘解算后可以得到地磁计A-F的计算结果，并变换得到a,b,ρ,x₀,y₀等误差参数。

在本发明实施例中，所述第一信息包括地磁计的标定参数和输出值。所述第二信息包括加速度计的俯仰角和横滚角。

载体运动状态下，利用经过标定后的参数及地磁计输出得到初始航向ψ。

初始不水平角度(包括俯仰角θ、横滚角γ)可利用加速度计获得，同时利用ψ、θ、γ建立地磁四元数初值q0_M和姿态四元数初值q0_G。

在本发明实施例中，经过多个时刻点的信息获取，能够对地磁四元数和姿态四元数进行更新。

在本发明实施例中，根据所述地磁四元数采用第一计算公式获得共轭四元数，所述第一计算公式为：

其中，

为共轭四元数，q_M0，q_Mx，q_My，q_Mz均为地磁四元数的四个数值；

根据共轭四元数、姿态四元数采用第二计算公式获得误差四元数，所述第二计算公式为：

其中，dq为误差四元数，

为共轭四元数，q_G为姿态四元数。

根据所述误差四元数采用第三计算公式获得对应的平台误差角，所述第三计算公式为：

δλ_x＝0.5·dq₀·dq₁

δλ_y＝0.5·dq₀·dq₂

δλ_z＝0.5·dq₀·dq₃

其中，设δλ(x，y，z)为平台误差角，δλ_x、δλ_y、δλ_z为平台误差角的三个数值，dq₀、dq₁、dq₂、dq₃为误差四元数的四个数值。

为了更好对上述内容进行解释说明，在本发明实施例中，k+1时刻为第二采集时刻，k时刻为第一采集时刻。

计算

的共轭四元数

将

和

四元数相乘得到误差四元数

在本发明实施例中，

根据所述误差四元数采用第三计算公式获得对应的平台误差角，所述第三计算公式为：

δλ_x＝0.5·dq₀·dq₁

δλ_y＝0.5·dq₀·dq₂

δλ_z＝0.5·dq₀·dq₃

其中，设δλ(x，y，z)为平台误差角，δλ_x、δλ_y、δλ_z为平台误差角的三个数值，dq₀、dq₁、dq₂、dq₃为误差四元数的四个数值。

进一步的解释说明计算得到平台误差角，记为

记为：

选取一个小于1的浮点数作为陀螺零偏反馈系数k，将平台误差角与反馈系数相乘即为本次迭代计算所得零偏值，记为

表示为：

设定零偏收敛阈值，

与

三个轴向变化幅度小于0.05°/s即认为互补滤波迭代收敛；在陀螺输出中扣除

后可得到经过零偏补偿的陀螺输出。另外，当变化幅度值不小于零偏收敛阈值，则直接陀螺输出。

本发明实施例提供的一种运动载体下的MEMS陀螺仪输出的零偏补偿方法，通过在第一采集时刻和第二采集时刻分别获取地磁计的第一信息和加速度计的第二信息，根据所述第一信息和所述第二信息分别建立所述地磁计的地磁四元数、所述陀螺仪的姿态四元数，根据所述地磁四元数获得共轭四元数，并根据所述共轭四元数和所述姿态四元数获得误差四元数，根据所述误差四元数获得对应的平台误差角；根据所述平台误差角和预设的陀螺零偏反馈系数分别获得对应于所述第一采集时刻和所述第二采集时刻的所述陀螺仪的零偏值，根据所述陀螺仪的零偏值获得对应于第二采集时刻的所述陀螺仪的输出值，实现快速补偿MEMS陀螺零偏，确保陀螺仪稳定输出。

图2示出了本发明一实施例提供的一种运动载体下的MEMS陀螺仪输出的零偏补偿装置，包括建立模块21、第一计算模块22、第二计算模块23、第三计算模块24、输出模块25，其中：

建立模块21，用于在第一采集时刻和第二采集时刻分别获取地磁计的第一信息和加速度计的第二信息，根据所述第一信息和所述第二信息分别建立所述地磁计的地磁四元数、所述陀螺仪的姿态四元数；

第一计算模块22，用于根据所述地磁四元数获得共轭四元数，并根据所述共轭四元数和所述姿态四元数获得误差四元数；

第二计算模块23，用于根据所述误差四元数获得对应的平台误差角；

第三计算模块24，用于根据所述平台误差角和预设的陀螺零偏反馈系数分别获得对应于所述第一采集时刻和所述第二采集时刻的所述陀螺仪的零偏值；

输出模块25，用于根据所述陀螺仪的零偏值获得对应于第二采集时刻的所述陀螺仪的输出值；

其中，所述第二采集时刻晚于所述第一采集时刻。

由于本发明实施例所述装置与上述实施例所述方法的原理相同，对于更加详细的解释内容在此不再赘述。

需要说明的是，本发明实施例中可以通过硬件处理器(hardware processor)来实现相关功能模块。

本发明实施例提供的一种运动载体下的MEMS陀螺仪输出的零偏补偿装置，通过在第一采集时刻和第二采集时刻分别获取地磁计的第一信息和加速度计的第二信息，根据所述第一信息和所述第二信息分别建立所述地磁计的地磁四元数、所述陀螺仪的姿态四元数，根据所述地磁四元数获得共轭四元数，并根据所述共轭四元数和所述姿态四元数获得误差四元数，根据所述误差四元数获得对应的平台误差角；根据所述平台误差角和预设的陀螺零偏反馈系数分别获得对应于所述第一采集时刻和所述第二采集时刻的所述陀螺仪的零偏值，根据所述陀螺仪的零偏值获得对应于第二采集时刻的所述陀螺仪的输出值，实现快速补偿MEMS陀螺零偏，确保陀螺仪稳定输出。

此外，本领域的技术人员能够理解，尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如，在下面的权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。

应该注意的是上述实施例对本发明进行说明而不是对本发明进行限制，并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施例。在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本发明可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。

本领域普通技术人员可以理解：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明权利要求所限定的范围。

Claims (8)

1.一种运动载体下的MEMS陀螺仪输出的零偏补偿方法，其特征在于，包括：

在第一采集时刻和第二采集时刻分别获取地磁计的第一信息和加速度计的第二信息，根据所述第一信息和所述第二信息分别建立所述地磁计的地磁四元数、所述陀螺仪的姿态四元数；

根据所述地磁四元数获得共轭四元数，并根据所述共轭四元数和所述姿态四元数获得误差四元数；

根据所述误差四元数获得对应的平台误差角；

根据所述平台误差角和预设的陀螺零偏反馈系数分别获得对应于所述第一采集时刻和所述第二采集时刻的所述陀螺仪的零偏值；

根据所述陀螺仪的零偏值获得对应于第二采集时刻的所述陀螺仪的输出值；

其中，所述第二采集时刻晚于所述第一采集时刻；

其中，根据所述误差四元数获得对应的平台误差角，包括：

根据所述误差四元数采用第三计算公式获得对应的平台误差角，所述第三计算公式为：

δλ_x＝0.5·dq₀·dq₁

δλ_y＝0.5·dq₀·dq₂

δλ_z＝0.5·dq₀·dq₃

其中，设δλ(x，y，z)为平台误差角，δλ_x、δλ_y、δλ_z为平台误差角的三个数值，dq₀、dq₁、dq₂、dq₃为误差四元数的四个数值。

2.根据权利要求1所述的方法，其特征在于，在第一采集时刻和第二采集时刻分别获取地磁计的第一信息和加速度计的第二信息，根据所述第一信息和所述第二信息分别建立所述地磁计的地磁四元数、所述陀螺仪的姿态四元数，包括：

获取所述地磁计的标定参数和输出值，所述标定参数和输出值作为所述第一信息；

根据所述地磁计的标定参数和输出值获得所述陀螺仪的航向；

获取所述加速度计的俯仰角、横滚角，所述俯仰角和横滚角作为所述第二信息；

根据所述航向、俯仰角和横滚角获得所述地磁四元数和所述姿态四元数。

3.根据权利要求2所述的方法，其特征在于，根据所述地磁四元数获得共轭四元数，并根据所述共轭四元数和所述姿态四元数获得误差四元数，包括：

根据所述地磁四元数采用第一计算公式获得共轭四元数，所述第一计算公式为：

其中，

为共轭四元数，q_M0，q_Mx，q_My，q_Mz为地磁四元数的四个数值；

根据共轭四元数、姿态四元数采用第二计算公式获得误差四元数，所述第二计算公式为：

其中，dq为误差四元数，

为共轭四元数，q_G为姿态四元数。

4.根据权利要求1所述的方法，其特征在于，根据所述陀螺仪的零偏值获得对应于第二采集时刻的所述陀螺仪的输出值，包括：

将对应于所述第一采集时刻的零偏值和对应于所述第二采集时刻的零偏值进行差值计算，获得变化幅度值；

将变化幅度值与预设的零偏收敛阈值进行比较，当变化幅度值小于零偏收敛阈值，则将对应于第二采集时刻的陀螺仪的输出值减去对应的零偏值作为所述陀螺仪的输出值。

5.一种运动载体下的MEMS陀螺仪输出的零偏补偿装置，包括：

建立模块，用于在第一采集时刻和第二采集时刻分别获取地磁计的第一信息和加速度计的第二信息，根据所述第一信息和所述第二信息分别建立所述地磁计的地磁四元数、所述陀螺仪的姿态四元数；

第一计算模块，用于根据所述地磁四元数获得共轭四元数，并根据所述共轭四元数和所述姿态四元数获得误差四元数；

第二计算模块，用于根据所述误差四元数获得对应的平台误差角；

第三计算模块，用于根据所述平台误差角和预设的陀螺零偏反馈系数分别获得对应于所述第一采集时刻和所述第二采集时刻的所述陀螺仪的零偏值；

输出模块，用于根据所述陀螺仪的零偏值获得对应于第二采集时刻的所述陀螺仪的输出值；

其中，所述第二采集时刻晚于所述第一采集时刻；

所述第二计算模块具体用于：

根据所述误差四元数采用第三计算公式获得对应的平台误差角，所述第三计算公式为：

δλ_x＝0.5·dq₀·dq₁

δλ_y＝0.5·dq₀·dq₂

δλ_z＝0.5·dq₀·dq₃

其中，设δλ(x，y，z)为平台误差角，δλ_x、δλ_y、δλ_z为平台误差角的三个数值，dq₀、dq₁、dq₂、dq₃为误差四元数的四个数值。

6.根据权利要求5所述的装置，其特征在于，所述建立模块具体用于：

获取所述地磁计的标定参数和输出值，所述标定参数和输出值作为所述第一信息；

根据所述地磁计的标定参数和输出值获得所述陀螺仪的航向；

获取所述加速度计的俯仰角、横滚角，所述俯仰角和横滚角作为所述第二信息；

根据所述航向、俯仰角和横滚角获得所述地磁四元数和所述姿态四元数。

7.根据权利要求6所述的装置，其特征在于，所述第一计算模块具体用于：

根据所述地磁四元数采用第一计算公式获得共轭四元数，所述第一计算公式为：

其中，

为共轭四元数，q_M0，q_Mx，q_My，q_Mz为地磁四元数的四个数值；

根据共轭四元数、姿态四元数采用第二计算公式获得误差四元数，所述第二计算公式为：

其中，dq为误差四元数，

为共轭四元数，q_G为姿态四元数。

8.根据权利要求5所述的装置，其特征在于，所述输出模块具体用于：

将对应于所述第一采集时刻的零偏值和对应于所述第二采集时刻的零偏值进行差值计算，获得变化幅度值；

将变化幅度值与预设的零偏收敛阈值进行比较，当变化幅度值小于零偏收敛阈值，则将对应于第二采集时刻的陀螺仪的输出值减去对应的零偏值作为所述陀螺仪的输出值。

Images