CN106706003A - 一种基于三轴mems陀螺仪的寻北旋转在线校准方法 - Google Patents

Abstract

本发明请求保护一种基于三轴MEMS陀螺仪寻北旋转在线校准方法，本方法通过在旋转控制系统上，对集成三轴MEMS陀螺仪的姿态仪进行‘在线’旋转控制，通过对三轴MEMS陀螺仪进行安装误差标定、滤波处理以及温度补偿，对姿态仪的姿态信息实时测量，当姿态仪的横滚角、俯仰角均为零时输出航向角，运用载体坐标系和导航坐标系之间的转换矩阵进行变换，以及考虑当地磁偏角后，最后得到寻北值。本方法采用MEMS陀螺仪，成本低，寻北速度快，精度高，计算原理简单，实用于高要求的井下寻北。

Description

一种基于三轴MEMS陀螺仪的寻北旋转在线校准方法

技术领域

本发明属于一种三轴MEMS陀螺仪寻北旋转在线校准方法，特别涉及寻北系统中的寻北方法。

背景技术

在人们生产和生活中，方位的确定是必不可少的，且不断开发着定位技术，然而，传统定位技术环境适应性差、精度低，已不能满足快速发展的生活需求。随着现代科技快速发展，测井、勘探、航海航空等领域对于寻北技术的要求也越来越高。惯性技术集经典力学和物理学一体的综合技术，能够控制轨迹和运动姿态，它的出现促进寻北系统自主、高精度方向发展。目前，光纤陀螺的缺陷是价格昂贵，环境适应性差，如果不加装高效保温瓶，很难满足4000m井深的测量需求，且体积随精度增加而增大，很难满足在普通标准套管井和投入钻杆及小井径的应用需求。动力调谐陀螺相比框架陀螺，精度已有很大幅度提高，但抗冲击振动能力差。基于机械陀螺的寻北仪是较为成熟的，具有高精度、低温漂等优点，然而，它的缺点也明显，可靠性低、动态范围有限、环境适应能力差等。

MEMS陀螺仪因为具有小型化，高可靠性，抗冲击振动，低成本，低功率消耗等优点，因此，基于MEMS陀螺仪的寻北系统成为一个很有潜力的长期的发展方向。同时，国内外关于MEMS陀螺寻北技术的研究还较少，尤其MEMS陀螺寻北的研究更少。对现有技术进行检索，得到以下相关技术文献。文献【1】提出了测井的常规方法是用地磁测量技术，该方法容易受到外界磁场的干扰，磁场异常的地质条件乃至特殊天文现象都将导致仪器测量精度下降。文献【2】提出了捷联惯导系统寻北仪，常选择其作为核心陀螺仪，陀螺仪本身精度不高、体积小、低成本，因此，基于动力调谐陀螺的捷联惯导寻北仪结构简单、寻北时间短，但精度低。

[1]Ehmann S,Hordt A,Leven M,et al.Paleomagnetic inclination anddeclination from three-component borehole magnetometer data—New insightsfrom logging in the Louisville seamounts[J].Journal of Geophysical Research,120(1):18-41.

[2]毛玉良,陈家斌,宋春雷,等.捷联惯导姿态误差模型分析[J].中国惯性技术学报,2013,21(2):182-185.

基于以上所述，本发明提出了一种三轴MEMS陀螺仪寻北旋转在线校准方法。

发明内容

本发明旨在解决以上现有技术的问题。提出了一种计算方法简单，速度快的基于三轴MEMS陀螺仪的寻北旋转在线校准方法。本发明的技术方案如下：

一种基于三轴MEMS陀螺仪的寻北旋转在线校准方法，其包括以下步骤：

101、选定一个集成三轴MEMS、陀螺仪及姿态仪的惯性系统；

102、选定旋转控制系统(二自由度及以上的旋转系统)；

103、对步骤102的旋转控制系统的主轴和俯仰轴进行归零操作；

104、将步骤101惯性系统的姿态仪固定在步骤103的旋转控制系统转台面上；

105、加电预热步骤104的姿态仪30分钟至姿态仪输出稳定；

106、通过数据接口将姿态仪和计算机端连接起来进行数据通信，通过姿态仪测得三轴MEMS陀螺仪的角速度原始值；

107、将步骤106测得的三轴MEMS陀螺仪的角速度原始值分别进行滤波处理；

108、将步骤107滤波处理的数据进行温度补偿；

109、将步骤108经过温度补偿后的数据进行三轴MEMS陀螺仪安装误差的标定；

110、调整旋转控制系统的主轴和俯仰轴，保证姿态仪的初始姿态角信息为(0,0,α)，其中α表示航向角；

111、进行井下复杂环境的模拟(如设置各种温度下的不同的俯仰角和横滚角)；

112、在经过步骤111井下复杂环境的模拟后，发出寻北指令，系统底层在收到寻北命令后，立刻对姿态仪的横滚角和俯仰角进行判断，并对旋转控制系统进行控制；

113、计算寻北值，完成校准。

进一步的，所述步骤104：将惯性系统的姿态仪固定在旋转控制系统的转台面上具体为：姿态仪的横轴X轴、纵轴Y轴平行于转台水平面，竖轴Z轴垂直于转台水平面，其中姿态仪分别绕横轴X轴、纵轴X轴、竖轴X轴旋转时，对应角度的变化分别为俯仰角β、横滚角γ、航向角α，且大小相等。

进一步的，步骤107测得的三轴MEMS陀螺仪采集的角速度原始值分别进行滤波处理的方法为卡尔曼滤波。

进一步的，所述步骤108：进行温度补偿采用的方法为灰色模型。

进一步的，步骤109三轴MEMS陀螺仪安装误差的标定模型为：

其中ω_xb，ω_yb，ω_zb分别为输入角速率；ω_xg，ω_yg，ω_zg分别为三轴MEMS陀螺仪测量值；i＝x,y,z；j＝x,y,z；K_ij为当i轴有输入角速率时，j轴检测到的角速率与输入角速率的比值；ω_x0、ω_y0、ω_z0分别为三轴MEMS陀螺仪的零偏。

进一步的，步骤112对姿态仪的横滚角和俯仰角进行判断，并对旋转控制系统进行控制，包括：A1、若横滚角和俯仰角均为0，则旋转控制系统转台保持静止，再执行步骤113；

A2、若横滚角γ不为0，俯仰角为0，则通过旋转控制系统控制姿态仪围绕Y轴旋转-γ角度，再执行步骤113；

A3、若横滚角为0，俯仰角β不为0，则通过旋转控制系统控制姿态仪围绕X轴旋转-β角度，再执行步骤113；

A4、若横滚角γ和俯仰角β均不为0，则通过旋转控制系统控制姿态仪先围绕Y轴旋转-γ角度，再通过旋转控制系统控制姿态仪围绕X轴旋转-β角度，再执行步骤113。

进一步的，步骤113计算寻北值采用的方法如下：

根据得到的三轴MEMS陀螺数据，带入(4)，

W为载体坐标系中姿态仪的角速率四元数；是四元数的乘法符号，矩阵形式表示成：

接着不断带入公式(5)得到实时更新后的四元数，

进而得到实时的姿态仪的姿态：

通过判断横滚角γ和俯仰角β的值，调整旋转控制系统使得横滚角γ′和俯仰角β′均为零，此时输出的α′通过(6)式计算出真北方向，

Φ＝α′+Δδ (7)

式中，α′表示姿态仪方位角，Δδ为当地磁偏角，Φ为姿态仪与真北方向的夹角。

本发明的优点及有益效果如下：

本发明操作简单易于实现，对步骤107三轴MEMS陀螺仪原始测量值进行处理，可以克服由于刻度因子不固定引起而然误差；同时步骤108滤波处理的数据进行温度补偿，可以降低温度对实验数据的影响；且步骤109经过补偿后的数据进行三轴MEMS陀螺仪安装误差的标定，可以避免安装误差带来的实验数据误差；步骤113，可以看出本方法寻北计算方法简单，速度快等特点。

随着现代科技快速发展，测井、勘探、航海航空等领域对于寻北技术的要求也越来越高。目前，光纤陀螺的缺陷是价格昂贵，环境适应性差，如果不加装高效保温瓶，很难满足4000m井深的测量需求，且体积随精度增加而增大，很难满足在普通标准套管井和投入钻杆及小井径的应用需求。MEMS陀螺仪因为具有小型化，高可靠性，抗冲击振动，低成本，低功率消耗等优点，因此，基于MEMS陀螺仪的寻北系统成为一个很有潜力的长期的发展方向。

附图说明

图1是本校准方法流程图

图2是本发明提供优选实施例四元素法原理图；

图3是初始状态；

图4是作业过程；

图5是寻北值确认。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是，

以下结合附图，对本发明作进一步说明：

(1)先选定一个集成三轴MEMS陀螺仪姿态仪的惯性系统；

(2)选定旋转控制系统双轴电动转台，并对电动转台的主轴和俯仰轴进行归零操作，使转台处于水平；

(3)用姿态仪的横轴(X轴)、纵轴(Y轴)平行于转台水平面，竖轴(Z轴)垂直于转台水平面，其中分别绕横轴(X轴)、纵轴(Y轴)、竖轴(Z轴) 旋转时，对应角度的变化分别为横滚角γ、俯仰角β、航向角α，且大小相等。

(4)加电预热姿态仪，约30分钟；

(5)同数据接口将姿态仪和计算机端连接起来进行数据通信，在计算机端打开上位机，即数据数据采集软件，配置好数据接收端口和设置好波特率即可进行通信；

(6)将测得的三轴MEMS陀螺仪采集的角速度原始值分别进行卡尔曼滤波处理，得到三组陀螺仪角速度原值值，单位为Mv,乘上刻度因子即可得到角速度(度每秒)；

(7)将滤波处理的数据进行温度补偿，采用方法为灰色预测模型；

(8)将补偿后的数据进行三轴MEMS陀螺仪安装误差的标定，采用的三轴MEMS陀螺仪安装误差的标定模型为：

其中ω_xb，ω_yb，ω_zb分别为输入角速率；ω_xg，ω_yg，ω_zg分别为三轴MEMS陀螺仪测量值；i＝x,y,z；j＝x,y,z；K_ij为当i轴有输入角速率时，j轴检测到的角速率与输入角速率的比值；ω_x0、ω_y0、ω_z0分别为三轴MEMS陀螺仪的零偏。

(9)通过滤波处理、温度补偿和标定后输出的三轴MEMS陀螺数据作为后续寻北计算的输入值；

(10)通过双轴电动转台调整保证姿态仪的初始姿态角信息为(0,0,α)，即是横滚角γ和俯仰角β均为0时，通过转移矩阵(2)和(3)计算得到初始四元素Q₀；

因此，可得到初始四元素Q₀：

(11)模拟井下复杂的的环境，旋转转台，主轴旋转范围为-360°～+360°，俯仰轴旋转范围为-90°～+90°，并对对姿态仪缓慢均匀加热，不超过70℃。

(12)进一步的，对姿态仪的横滚角和俯仰角进行判定：包括：A1、若横滚角和俯仰角均为0，则旋转控制系统转台保持静止，再执行步骤113；

A2、若横滚角γ不为0，俯仰角为0，则通过旋转控制系统控制姿态仪围绕Y轴旋转-γ角度，再执行步骤113；

A3、若横滚角为0，俯仰角β不为0，则通过旋转控制系统控制姿态仪围绕X轴旋转-β角度，再执行步骤113；

A4、若横滚角γ和俯仰角β均不为0，则通过旋转控制系统控制姿态仪先围绕Y轴旋转-γ角度，再通过旋转控制系统控制姿态仪围绕X轴旋转-β角度，再执行步骤113；

进一步的，步骤113计算寻北值采用的方法如下：

设定东北天坐标为导航坐标系n且依次为X轴、Y轴、Z轴，载体坐标系为b且载体坐标系X轴、Y轴、Z轴分别与姿态仪安装坐标系重合，载体坐标系至导航坐标系的转换矩阵为。导航坐标系与载体坐标系对应依次X轴，Z轴，Y轴坐标轴对应夹角，为姿态仪横滚公角r,俯仰角β,航向角α。

通过MEMS陀螺仪三个轴的角速率并结合常微分方程对姿态矩阵进行求解，对于姿态仪的姿态信息通过四元素法更新，旋转在线校准方案采用四元素法。

四元素的微分方程如下：

式中，Q为姿态四元素，Q＝q₀+q₁i+q₂j+q₃k，q₀、q₁、q₂、q₃均为实数；t₀为姿态仪运动的初始时刻；Q₀为姿态仪初始姿态角对应的四元素，姿态仪的初始姿态角信息为(r,β,α)，由载体坐标系至导航坐标系的转换矩阵：

因此，可得到初始四元素Q₀：

W为载体坐标系中姿态仪的角速率四元数；是四元数的乘法符号，矩阵形式表示成：

式中，ω_i(i＝1,2,3)为三轴MEMS陀螺仪在载体坐标系X、Y、Z轴上的角速率分量，Ω(ω)是4×4阶的反对称矩阵。

通常，在采样时间T_s间隔内假设MEMS陀螺仪的值均不变，离散化的四元数姿态更新公式变为：

式中，k＝0,1,…。在实际解算过程中，和必须按照级数展开取有限项，得四元素的一阶近似算法：

因此，只需知道q₀、q₁、q₂、q₃和MEMS陀螺仪的角速率，就能得到更新后的四元数。由姿态四元素和旋转矩阵的关系，得到姿态矩阵：

由更新后的四元数得到更新后的姿态矩阵和欧拉角的旋转矩阵一一对比，得到更新后的姿态角信息：

由于姿态仪的初始状态为(0,0,α)，基于三轴MEMS陀螺仪的寻北系统在工作过程中，利用三轴MEMS陀螺仪数据由四元素法更新姿态仪的姿态角得到实时的姿态信息(γ′,β′,α′)。

当收到寻北命令时，系统会对姿态仪的横滚角γ′和俯仰角β′大小进行判断，通过旋转控制系统对姿态仪的俯仰角和横滚角进行旋转控制，使姿态仪的横滚角和俯仰角均变为零。此时，姿态仪输出的航向角即可表示为姿态仪与磁北方向的夹角。

由于磁北与真北之间存在磁偏角，因此，姿态仪与真北方向的夹角即可表示为：

Φ＝α′+Δδ (10)

式中，α′表示姿态仪方位角，Δδ为当地磁偏角，Φ为姿态仪与真北方向的夹角。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (7)

1.一种基于三轴MEMS陀螺仪的寻北旋转在线校准方法，其特征在于，包括以下步骤：

101、选定一个集成三轴MEMS陀螺仪的姿态仪的惯性系统；

102、选定二自由度及以上的旋转控制系统；

103、对步骤102的旋转控制系统的主轴和俯仰轴进行归零操作；

104、将步骤101惯性系统的姿态仪固定在步骤103的旋转控制系统的转台面上；

105、加电预热步骤104的姿态仪30分钟至姿态仪输出稳定；

106、通过数据接口将姿态仪和计算机端连接起来进行数据通信，通过姿态仪测得三轴MEMS陀螺仪的角速度原始值；

107、将步骤106测得的三轴MEMS陀螺仪的角速度原始值分别进行滤波处理；

108、将步骤107滤波处理的数据进行温度补偿；

109、将步骤108经过温度补偿后的数据进行三轴MEMS陀螺仪安装误差的标定；

110、调整旋转控制系统的主轴和俯仰轴，保证姿态仪的初始姿态角信息为(0,0,α)，其中α表示航向角；

111、进行井下复杂环境的模拟；

112、在经过步骤111井下复杂环境的模拟后，发出寻北指令，系统底层在收到寻北命令后，立刻对姿态仪的横滚角和俯仰角进行判断，并对旋转控制系统进行控制；

113、计算寻北值，完成校准。

2.根据权利要求1所述的基于三轴MEMS陀螺仪的寻北旋转在线校准方法，其特征在于，所述步骤104：将惯性系统的姿态仪固定在旋转控制系统的转台面上具体为：姿态仪的横轴X轴、纵轴Y轴平行于转台水平面，竖轴Z轴垂直于转台水平面，其中姿态仪分别绕横轴X轴、纵轴X轴、竖轴X轴旋转时，对应角度的变化分别为俯仰角β、横滚角γ、航向角α，且大小相等。

3.根据权利要求1或2所述的基于三轴MEMS陀螺仪的寻北旋转在线校准方法，其特征在于，步骤107测得的三轴MEMS陀螺仪采集的角速度原始值分别进行滤波处理的方法为卡尔曼滤波。

4.根据权利要求3所述的基于三轴MEMS陀螺仪的寻北旋转在线校准方法，其特征在于，所述步骤108：进行温度补偿采用的方法为灰色模型。

5.根据权利要求4所述的基于三轴MEMS陀螺仪的寻北旋转在线校准方法，其特征在于，步骤109三轴MEMS陀螺仪安装误差的标定模型为：

$\left(\begin{array}{c}{\mathrm{\ω}}_{xb}\\ {\mathrm{\ω}}_{yb}\\ {\mathrm{\ω}}_{zb}\end{array}\right)={\left(\begin{array}{ccc}{K}_{xx}& K_{yx}& K_{zx}\\ K_{xy}& K_{yy}& K_{zy}\\ K_{xz}& K_{yz}& K_{zz}\end{array}\right)}^{-1}\left(\begin{array}{c}{\mathrm{\ω}}_{xg}-{\mathrm{\ω}}_{x0}\\ {\mathrm{\ω}}_{yg}-{\mathrm{\ω}}_{y0}\\ {\mathrm{\ω}}_{zg}-{\mathrm{\ω}}_{z0}\end{array}\right)---\left(1\right)$

其中ω_xb，ω_yb，ω_zb分别为输入角速率；ω_xg，ω_yg，ω_zg分别为三轴MEMS陀螺仪测量值；i＝x,y,z；j＝x,y,z；K_ij为当i轴有输入角速率时，j轴检测到的角速率与输入角速率的比值；ω_x0、ω_y0、ω_z0分别为三轴MEMS陀螺仪的零偏。

6.根据权利要求4所述的基于三轴MEMS陀螺仪的寻北旋转在线校准方法，其特征在于，步骤112对姿态仪的横滚角和俯仰角进行判断，并对旋转控制系统进行控制，包括：A1、若横滚角和俯仰角均为0，则旋转控制系统转台保持静止，再执行步骤113；

A2、若横滚角γ不为0，俯仰角为0，则通过旋转控制系统控制姿态仪围绕Y轴旋转-γ角度，再执行步骤113；

A3、若横滚角为0，俯仰角β不为0，则通过旋转控制系统控制姿态仪围绕X轴旋转-β角度，再执行步骤113；

A4、若横滚角γ和俯仰角β均不为0，则通过旋转控制系统控制姿态仪先围绕Y轴旋转-γ角度，再通过旋转控制系统控制姿态仪围绕X轴旋转-β角度，再执行步骤113。

7.根据权利要求6所述的基于三轴MEMS陀螺仪的寻北旋转在线校准方法，其特征在于，步骤113计算寻北值采用的方法如下：

根据得到的三轴MEMS陀螺数据，带入(4)，

$\stackrel{\·}{Q}=\left[\begin{array}{c}{\stackrel{\·}{q}}_0\\ {\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ {\stackrel{\·}{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_1& -{\mathrm{\ω}}_2& -{\mathrm{\ω}}_3\\ {\mathrm{\ω}}_1& 0& {\mathrm{\ω}}_3& -{\mathrm{\ω}}_2\\ {\mathrm{\ω}}_2& -{\mathrm{\ω}}_3& 0& {\mathrm{\ω}}_1\\ {\mathrm{\ω}}_3& {\mathrm{\ω}}_2& -{\mathrm{\ω}}_1& 0\end{array}\right]\·\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\frac{1}{2}\mathrm{\Ω}\left(\mathrm{\ω}\right)Q---\left(4\right)$

W为载体坐标系中姿态仪的角速率四元数；是四元数的乘法符号，矩阵形式表示成：

接着不断带入公式(5)得到实时更新后的四元数，

$Q_{k+1}=\left(I+\frac{\mathrm{\Ω}\left(\mathrm{\ω}\right)T_s}{2}\right)Q_k---\left(5\right)$

进而得到实时的姿态仪的姿态：

$\left\{\begin{array}{c}{r}^{\′}=arctan(-\frac{2(q_1{q}_3-q_0{q}_2)}{q_0^2-q_1^2-q_2^2+q_3^2})\\ {\mathrm{\β}}^{\′}=arcsin\left(2\right(q_2{q}_3+q_0{q}_1))\\ {\mathrm{\α}}^{\′}=\arctan (-\frac{2(q_1{q}_2-q_0{q}_3)}{q_0^2-q_1^2+q_2^2-q_3^2})\end{array}\right.---\left(6\right)$

通过判断横滚角γ和俯仰角β的值，调整旋转控制系统使得横滚角γ′和俯仰角β′均为零，此时输出的α′通过(6)式计算出真北方向，

Φ＝α′+Δδ (7)

式中，α′表示姿态仪方位角，Δδ为当地磁偏角，Φ为姿态仪与真北方向的夹角。