一种基于加表耦合的MEMS陀螺漂移误差的补偿方法

技术领域

本发明属于惯性导航技术领域，更具体地，涉及一种基于加表耦合的 MEMS陀螺漂移误差的补偿方法。

背景技术

在惯性导航系统中，陀螺和加速度计(加表)的信息的处理直接关系 到载体姿态的解算精度，而姿态是反映载体运动的一个重要参数，其精度 对载体的速度、位置计算、识别跟踪、运动轨迹、性能分析等有重要影响。 通常姿态解算是根据陀螺的数据和载体的速度、位置信息实时计算载体姿 态矩阵，实现导航坐标系对地理坐标系的实时跟随以及载体姿态的解算。 但是由于陀螺和加速度计的精度有限，并且随着时间的增加漂移误差会不 断的积累增大，尤其是低成本的MEMS陀螺漂移误差更大，使系统的姿态 解算精度越来越低，不能够满足工程技术要求。

发明内容

针对现有技术的缺陷，本发明的目的在于提供一种基于加表耦合的 MEMS陀螺漂移误差的补偿方法，旨在解决现有技术中由于MEMS陀螺精 度低、漂移误差大导致姿态解算精度较低的技术问题。

本发明提供的基于加表耦合的MEMS陀螺漂移误差的补偿方法，包括 下述步骤：

(1)根据加表实时测得的载体加速度信息获得陀螺漂移误差量Δω(t)；

(2)建立误差调节模型ω_C(t)＝Δω_P+Δω_I；

根据公式Δω_P＝K_P*Δω(t)和Δω_I＝Δω_I+K_I*dt*Δω(t)以及 所述陀螺漂移误差量Δω(t)获得第一误差调节控制量Δω_P和第二误差调节控 制量Δω_I；

将所述第一误差调节控制量Δω_P和第二误差调节控制量Δω_I代入所述 误差调节模型中获得陀螺漂移误差补偿量ω_C(t)；其中，K_P、K_I分别为第 一、二误差调节系数；dt为采样周期；Δω(t)为陀螺漂移误差量；ω_C(t)为 陀螺漂移误差补偿量；

(3)根据所述陀螺漂移误差补偿量对陀螺数据进行误差补偿并获得补 偿后的陀螺信息。

其中，步骤(1)具体包括：

(1.1)选择东北天地理坐标系作为导航坐标系并获得载体加速度信息 $\overrightarrow{f_i^b}={\left(\begin{array}{ccc}{a}_x& a_y& a_z\end{array}\right)}^T;$其中a_x a_y a_z分别为东北天地理坐标系下载体在X 轴、Y轴、Z轴的加速度；

(1.2)根据公式获得所述载体的三维加速度的模值；

(1.3)根据所述模值和公式将所述载体的三维加速度信息转 换为单位向量；

(1.4)获得东北天地理坐标系下所述载体加速度信息中三个方向的重 力投影$f_g=\left(\begin{array}{c}0\\ 0\\ g\end{array}\right);$

(1.5)根据公式$f_g^b=C_n^b\left(\begin{array}{c}0\\ 0\\ g\end{array}\right)$将重力在东北天地理坐标系下的投影转换为 载体坐标系下的重力投影；其中，为初始化得到的姿态矩阵；g为当地 重力加速度；

(1.6)将步骤(1.3)中的单位向量与步骤(1.5)中的重力投影进行叉 乘后，获得陀螺漂移误差量其中Δω(t)为根据加速度 计求取的陀螺漂移误差量；为单位化后的载体加速度参考量；为重 力加速度在载体坐标系下的投影。

其中，步骤(3)具体包括：

(3.1)在载体坐标系下获得陀螺信息ω_gyro(t)＝[ω_x ω_y ω_z]^T；其中 ω_x ω_y ω_z分别为三个陀螺的输出信息；

(3.2)建立陀螺漂移误差调节模型ω(t)＝ω_gyro(t)+ω_C(t)；其中 ω(t)为陀螺补偿以后的输出；ω_gyro(t)为陀螺补偿前的输出；ω_C(t)为陀螺漂 移误差补偿量；

(3.3)将所述陀螺漂移误差补偿量ω_C(t)以及载体坐标系下的陀螺信息 代入所述陀螺漂移误差调节模型中，获得补偿后的陀螺信息。

本发明提供的基于加表耦合的MEMS陀螺漂移误差的补偿方法，以陀 螺和加速度计为基本数据传感器，并用加速度计的信息对陀螺的漂移进行 有效补偿，降低陀螺的漂移误差，从而提高惯导系统的姿态解算精度，对 惯性导航领域的姿态解算有较大贡献，本发明简单巧妙，符合实际需求进 步显著且实用性强，能够大规模推广使用。

附图说明

图1是本发明实施例提供的基于加表耦合的MEMS陀螺漂移误差的补 偿方法实现流程图。

图2是补偿前俯仰角随时间的变化示意图。

图3是补偿后俯仰角随时间的变化示意图。

图4是补偿前横滚角随时间的变化示意图。

图5是补偿后横滚角随时间的变化示意图。

具体实施方式

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

本发明提供的基于加表耦合的MEMS陀螺漂移误差的补偿方法，解决 了由MEMS陀螺精度低、漂移误差大所导致的姿态解算精度较低，不能满 足工程化应用的问题。

如图1所示，本发明实施例提供的基于加表耦合的MEMS陀螺漂移误 差的补偿方法，包括以下几个步骤：

步骤1：根据加表测得的载体加速度信息计算陀螺漂移误差量；

步骤2：根据陀螺漂移误差量，建立误差调节控制器，求取陀螺漂移误 差补偿量；

步骤3：根据陀螺漂移误差补偿量对陀螺数据进行误差补偿。

作为本发明的一个实施例，步骤1中根据读取的三维加速度信息计算 陀螺漂移误差补偿量，具体通过以下步骤实现：

(1a)设系统选取东北天地理坐标系为导航坐标系，则读取的载体加速 度信息为$\overrightarrow{f_i^b}={\left(\begin{array}{ccc}{a}_x& a_y& a_z\end{array}\right)}^T;$其中，a_x a_y a_z分别为载体的加速度信息；

(1b)求取三维加速度的模值

(1c)根据公式将三维加速度信息转为单位向量；

(1d)在地理坐标系下，东北天三个方向的重力投影为$f_g=\left(\begin{array}{c}0\\ 0\\ g\end{array}\right);$

(1e)根据公式$f_g^b=C_n^b\left(\begin{array}{c}0\\ 0\\ g\end{array}\right)$将重力在地理坐标系下的投影转到载体坐标系 下；其中，为初始化得到的姿态矩阵；g为当地重力加速度；

(1f)将单位化后的载体加速度参考量和重力加速度在载体坐标系下的 投影做叉乘就可以得到加速度计求取的陀螺漂移误差量，即 其中，Δω(t)为根据加速度计求取的陀螺漂移误差量； 为单位化后的载体加速度参考量；为重力加速度在载体坐标系下的 投影。

作为本发明的另一个实施例，步骤2中根据陀螺漂移误差量，建立误 差调节控制器，求取陀螺漂移误差补偿量，具体通过以下步骤实现：

(2a)建立误差调节模型ω_C(t)＝Δω_P+Δω_I；其中，ω_C(t)为陀螺漂 移误差补偿量；Δω_P、Δω_I分别为第一误差调节控制量、第二误差调节 控制量；

(2b)求取误差调节控制量，则Δω_P＝K_P*Δω(t)； Δω_I＝Δω_I+K_I*dt*Δω(t)；其中，K_P、K_I分别为第一、二误差调节系 数；该系数与器件精度有关，可通过试验得到；dt为系统采样周期；Δω(t) 为根据加速度计求取的陀螺漂移误差量。

作为本发明的一个实施例，步骤3中根据陀螺漂移误差补偿量对陀螺 数据进行误差补偿，具体通过以下步骤实现：

(3a)设在载体坐标系下，读取的陀螺信息为： ω_gyro(t)＝[ω_x ω_y ω_z]^T；其中，ω_x ω_y ω_z分别为三个陀螺的输出信息；

(3b)建立陀螺漂移误差调节模型ω(t)＝ω_gyro(t)+ω_C(t)；其中，ω(t) 为陀螺补偿以后的输出；ω_gyro(t)为陀螺补偿前的输出；ω_C(t)为陀螺漂移误 差补偿量；

其中，q_k为k时刻的四元数；q_k-1为k-1时刻的四 元数；M为陀螺角增量的反对称矩阵； 其中， 为载体坐标系到导航坐标系的姿态转换矩阵；q₀，q₁，q₂，q₃为四元数； θ，r分别表示载体的方位角、俯仰角与横滚角；θ＝arcsin(T₃₂)； $\gamma =\arctan \frac{T_{31}}{T_{33}};$

通过上述公式就可以得到通过加速度计补偿以后的陀螺数据，利用补 偿后的陀螺数据对四元数和姿态矩阵进行更新，通过更新后的姿态矩阵根 据公式求解载体的实时姿态信息。

本发明所实现的一种基于加表耦合的MEMS陀螺漂移误差的补偿方 法，以陀螺和加速度计为基本数据传感器，并用加速度计的信息对陀螺的 漂移进行有效补偿，降低陀螺的漂移误差，从而提高惯导系统的姿态解算 精度，对惯性导航领域的姿态解算有较大贡献，本发明简单巧妙，符合实 际需求进步显著且实用性强，能够大规模推广使用。

针对本实施例，进行一次简单的实验验证，实验步骤如下：

(a)将MEMS惯性导航系统固定在三轴隔振转台上，确认无误后通电， 静止不动，采集数据10min；

(b)然后将航向顺时针转一周，再逆时针转动一周回到起始位置；

(c)将俯仰顺时针转动20°，再逆时针转动20°回到起始位置；

(d)将横滚顺时针转动40°，再逆时针转动40°回到起始位置后，再静 止测试10min，断电停止实验。实验结果如下：

从图2～图5可以看出，MEMS惯导的姿态误差变化较快，使用该补 偿算法后，MEMS惯导不仅能够准确敏感俯仰角和横滚角的变化，而且能 够有效地抑制俯仰角和横滚角漂移带来的误差。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已， 并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等 同替换和改进等，均应包含在本发明的保护范围之内。