基于MEMS陀螺的矿用新型钻机开孔定向仪研制

基于MEMS陀螺的矿用新型钻机开孔定向仪研制

燕斌;程建远;蔡远利;代晨昱;李萍;冯宏

【摘 要】针对目前煤矿井下采用人工测量方法不准确和基于三轴光纤陀螺的钻孔

定位成本较高等问题,设计了一种新型钻机开孔定向仪,介绍了仪器的总体方案及工

作原理,搭建了基于MSG7100D-300陀螺和KXR94加速度计,以STM32单片机为

CPU核心的硬件平台;系统采用静态四位置方法寻北方式及卡尔曼滤波的跟踪算法,

有效地、快速地实现系统的测量.通过对原型样机的实验测试,结果表明,基于

MEMS陀螺的钻机开孔定向仪其寻北时间、寻北精度以及动态测量精度,均满足了

钻机开孔定向的指标要求及用户的实际需求.

【期刊名称】《煤田地质与勘探》

【年(卷),期】2018(046)006

【总页数】6页(P187-192)

【关键词】光纤陀螺仪;MEMS陀螺;钻机开孔;寻北仪

【作 者】燕斌;程建远;蔡远利;代晨昱;李萍;冯宏

【作者单位】西安交通大学电子与信息工程学院,陕西西安710049;中煤科工集团

西安研究院有限公司,陕西西安710077;中煤科工集团西安研究院有限公司,陕西西

安710077;西安交通大学电子与信息工程学院,陕西西安710049;中煤科工集团西

安研究院有限公司,陕西西安710077;中煤科工集团西安研究院有限公司,陕西西安

710077;中煤科工集团西安研究院有限公司,陕西西安710077

【正文语种】中 文

【中图分类】TD163

钻探作为探查和治理煤炭开采中隐蔽致灾因素的一种最直接的探查技术，它具有直

观性强和探测精度高等特点，可广泛应用于构造探测、老空区探测、探放水、瓦斯

卸压以及其他隐蔽致灾因素的探查中[1-2]。煤矿井下瓦斯抽采、探放水、注浆加

固的效果，直接受钻孔成孔质量的影响，而钻孔成孔的质量又受钻孔开孔精度的影

响。因此，钻探作为煤矿瓦斯、水害等灾害预防的主要手段之一，其开孔精度就显

得尤为重要。

目前，大多数煤矿都采用传统的人工丈量方法标定开孔方位角和倾斜角，其操作程

序相当繁琐，稳钻的时间长，精度低，给井下防治水、瓦斯抽采等工程的施工和验

收带来诸多不便。2012年，南京捷嘉科技有限公司研制了矿用ZJSD-1钻孔激光

数字化定位仪，减少了测量时间，提高了测量精度，但测量过程涉及大量的人工操

作，准确性仍然较大程度依附于人工操作的精准程度等。2014年中煤科工集团西

安研究院有限公司与北京航空航天大学联合研制了基于三轴光纤陀螺的YHKD2钻

孔定位仪，该仪器的寻北精度为1.5°，寻北时间为3 min[3]。该仪器的精度和自

动化程度较高，符合煤矿井下的测量需求，但其高昂的成本限制了该产品在煤矿井

下的应用推广。因此，寻求一种满足精度要求、低成本的煤矿井下钻孔定向装备迫

在眉睫。基于上述生产中的实际问题，研制了一款基于MEMS Micro-Electro

Mechanical System陀螺的矿用新型钻机开孔定向仪(下文简称“定向仪”)，并

进行了样机试验，检验各项指标，以期早日达到实际生产的需求。

开孔定向仪主要由寻北系统和跟踪系统2部分组成，构成框图如图1所示。寻北

系统主要是用于测量该仪器在静态情况下与地理北极(真北方向)之间的姿态数据

(倾角和方位角)，即确定与真北方向的姿态基准。跟踪系统主要用于测量该仪器寻

北后的测量数据在动态情况下随时间变化而变化的姿态测量数据。将该测量仪器放

置于与钻杆平行的钻机导轨处，依据预设值调整钻机的姿态(倾角和方位角)，达到

预先设定的值即可完成开孔定向操作。

如图2所示，地球自转角速率ωe (ωe=15.041(°)/h)，其方向平行于地球自转轴。

在地球表面纬度为L处，地球自转角速度ωe可以分解为沿子午线方向的水平分量

ωN和沿地垂线Zn方向的垂直分量ωU，其表达式分别为式(1)和式(2)。

陀螺是一种测量围绕其敏感轴方向角速率的传感器；理想情况下，将陀螺垂直安装

在水平面上，使其敏感轴Yg的方向在水平面内与地理坐标系中的北向(真北方向)

平行，则陀螺敏感到的角速度ωo即为ωe在水平方向的分量ωN。当陀螺的敏感

轴Yg在水平面内逆时针方向与真北方向存在一个α的夹角时，如图3所示，则此

时陀螺的输出为的水平分量在陀螺敏感轴上的投影，即

只要测得陀螺在相应方位的输出，通过式(1)和式(3)，便可以计算出在该位置陀螺

敏感轴与真北方向的夹角：

综上可知：理论上，陀螺在单个位置的输出就可以得到寻北结果，但是单位置的结

果与陀螺自身的零偏、零偏稳定性、标度因数、随机噪声等参数有直接关系，而这

些参数无法测得确定的结果，因此，采用单位置的寻北方法精度很低。

微型惯性测量单元(Micro Inertial Measurement Unit，简称MIMU)，主要包括

三轴MEMS陀螺仪和三轴MEMS加速度，数据采集及其专业的导航软件，可以

提供运动载体的位置、速度和姿态信息。2种惯性传感器按照载体坐标系的3个正

方向进行安装，三轴MEMS陀螺用来测量相对于惯性坐标系的载体运动在载体坐

标系下的3个轴向上的角速度分量；三轴MEMS加速度用来测量载体坐标系下3

个轴向上的线加速度分量。系统主要是利用捷联姿态矩阵完成坐标变换，把沿载体

坐标系下的3个轴向上的线加速度转化为沿导航坐标系下的3个轴向上的线加速

度，经过解算得到载体的速度和位置参数；把沿载体坐标系下的3个轴向上的角

速度分量转化为沿导航坐标系下3个轴向上的角速度分量，经过解算得到载体的

姿态参数。

3.1.1 MEMS陀螺选型

随着微机电技术的出现，MEMS陀螺在体积、成本、功耗及抗冲击等方面存在巨

大的优势。随着微电子加工技术的发展，MEMS惯性传感器特别是MEMS陀螺的

精度得到很大的提高。CRM100陀螺仪是SILICON SENSING公司生产的一种新

型的高精度角速度陀螺，既可以输出与角速度呈线性关系的模拟量，也可以通过

SPI协议输出数字量。该陀螺仪采用第5代VSG陀螺技术，具有低功耗、体积小、

输出稳定等特点，尺寸大小为：5.7 mm×4.8 mm× 1.2 mm，零位稳定性24

(°)/h，角度随机游走0.28 (°)/h，采用3.3 V供电，功耗4 mA。MSG7100D-

300 MEMS 微陀螺采用美泰科技公司生产的高精度陀螺，采用SPI协议数字输出，

尺寸大小为：11.43 mm× 11.43 mm×3.8 mm，零位稳定性10 (°)/h，角度随机

游走0.15 (°)/h，其外围电路如图4所示。

3.1.2 加速度采集电路

系统采用的KXR94加速度计芯片是Kionix公司生产的三轴加速度计。该加速度

计内部已经对温度和电压波动引起的偏差进行了设计补偿，因此，由于电压和温度

引起的偏差较小。该器件测量范围为±2g(g为重力加速度)，灵敏度系数为(560

mV)/g，非线性度为0.1％，零加速度漂移为(±150×10-3)g；2.8~3.3 V均可工

作；功耗很低，静态电流约1.1 mA，具体电路如图5所示。

3.2.1 寻北系统算法设计

基于采集位置的不同，目前可分为多位置方案、连续转动方案、双位置方案、四位

置方案等。尽管方案形式上有不同，但基本原理上相似，且各有特点。基于静态四

位置寻北系统算法解算简便及在水平位置时不需要纬度信息等突出优点，本文采用

四位置的方案进行测量，软件流程图如下：

3.2.2 动态跟踪系统算法设计

为了更好地进行动态姿态的测量，本文采用卡尔曼滤波技术实现。卡尔曼滤波

( Kalman Filter，简称KF) 技术通常用于系统状态估计和估计补偿惯性传感器的测

量误差，从而实现多种测量信息融合[4]。图7所示为动态跟踪系统算法设计原理，

主要包括3个部分：①利用系统寻北结果建立初始捷联姿态矩阵，利用MIMU测

量数据进行捷联姿态解算；②利用三轴加速度计测量信息估计水平姿态角；③设计

扩展卡尔曼滤波算法，实现姿态角误差的最优估计，从而得到实时定向仪姿态信息，

即方位角、俯仰角和工具面角。

捷联姿态解算算法[5]

设计捷联姿态解算算法前，首先需要建立MEMS陀螺的测量误差模型。由于确定

性误差可以通过对传感器的测试进行补偿，因此本文只针对随机误差进行滤波处理。

本文采用一种典型的随机误差模型描述MEMS陀螺输入和输出的关系：

式中为陀螺输出角速率，为输入真实角速率，为陀螺漂移，由陀螺速率随机游走白

噪声驱动，为测量白噪声，即角度随机游走白噪声。

本文采用转动四元数表示定向仪的姿态信息，完成捷联姿态矩阵的解算[6]。定义

姿态四元数为，则姿态矩阵可以表示为：

姿态四元数满足微分方程

其中，为机体坐标系相对于导航坐标系即地理坐标系的转动角速率。利用当前时刻

MIMU测量得到的角速率信息，求解姿态四元数微分方程式(6)，便可以获得当前

时刻的姿态四元数和姿态矩阵，利用姿态角和姿态矩阵的关系，求解定向仪的姿态

角如下：

b. 利用重力矢量估计水平姿态

在静态或匀速运动条件下，甚至在动态特性不大的条件下，重力矢量通常可用作参

考矢量，用来估计载体的水平姿态角信息。在算法设计时，利用加速度计测量信息

估计定向仪的水平姿态信息，并将其与MIMU解算的信息进行KF滤波融合，抑

制了MIMU误差积累发散，提高了系统测量精度。设分别为三轴加速度计测量输

出的比力信息，则定向仪水平姿态角解算如下：

c. 扩展卡尔曼滤波算法设计

本文设计的扩展卡尔曼滤波器的状态向量为姿态误差角和陀螺漂移[7-8]。滤波器

的测量值取为系统姿态角误差，姿态角误差是由MIMU解算输出的姿态角与利用

加速度计信息估计得到的姿态角之间的差值，作为系统测量信息。因此，定义系统

状态估计向量为，则系统状态方程为。

其中状态矩阵F(t)的系数参照文献[9-10]，设由MIMU测量信息解算建立的姿态

矩阵为，而真实的姿态矩阵为。将MIMU输出的水平姿态角和利用加速度计信息

求解的水平姿态角表示为真实值δ, γ和误差值δθ, δγ的之和分别为：

卡尔曼滤波器测量方程为

即

测量矩阵为

对于建立的扩展卡尔曼滤波器式(9)和式(11)，通过对连续方程的离散化，进而利

用离散卡尔曼滤波方程进行迭代估计，可以得到姿态误差角的最优估计，利用对

MIMU获得的姿态矩阵进行校正，便可以得到最优估计的姿态矩阵：

利用和式(7)求解获得最优估计的定向仪姿态角信息。系统姿态估计卡尔曼滤波器

的基本原理和迭代估计流程如图8所示。

从表1可以看出在不同倾角下，四位置静态寻北方案后，方位角和倾角与真值的

误差结果。寻北重复性试验结果表明：定向仪寻北测量的时间小于5 min，方位角

寻北的精度较高，最大偏差为 0. 3°。同时，倾角的最大误差的精度为0.1°。

由于倾角参数采用加速度传感器静态测量，随着时间漂移，倾角的大小不会产生变

化。因此，本文中仅对方位角进行测试。表2给出惯性导航跟踪的精度的测量结

果。从表2可以看出，随着时间的漂移方位角精度的误差增大。在间隔60 s后，

方位角最大偏差0.3°左右；间隔300 s后，方位角最大偏差0.7°；在间隔600 s

后，方位角最大偏差1.6°。

a. 本文设计了一种基于高精度MEMS陀螺的新型开孔定向仪，采用寻北和跟踪2

个部分构成，降低了原有采用光纤陀螺传感器的成本。

b. 详细叙述了测量原理，硬件参数和软件设计测试不同倾角下，和时间变化情况

下方位角精度，测量结果能够满足实际钻机开孔的设计需要。

YAN Bin，CHENG Jianyuan，CAI Yuanli，et al. Development of a new type

of drilling orientation device bad on MEMS gyroscope[J]. Coal Geology

& Exploration，2018，46(6)：187–192.

【相关文献】

[1] 石智军，姚宁平，叶根飞. 煤矿井下瓦斯抽采钻孔施工技术与装备[J]. 煤炭科学技术，2009，

37(7)：1–4. SHI Zhijun，YAO Ningping，YE Genfei. Construction technology and

equipment for gas drainage borehole drilling in underground coal mine[J]. Coal Science

and Technology，2009，37(7)：1–4.

[2] 田宏亮. 瓦斯抽采履带式坑道钻机的研制与应用[J]. 煤田地质与勘探，2008，36(5)：77–80.

TIAN Hongliang. Manufacture and application on track-mounted rig for gas drainage[J].

Coal Geology & Exploration，2008，36(5)：77–80.

[3] 林铁，林恒，高爽，等. 光纤陀螺测斜仪数据采集及传输单元设计与实现[J]. 测井技术，

2009，33(4)：402–405. LIN Tie，LIN Heng，GAO Shuang，et al. Design and

implementation of data acquisition and transmission for fiber optic gyroscopes

inclinometer[J]. Well Logging Technology，2009，33(4)：402–405.

[4] 吕振，刘丹，李春光. 基于捷联惯性导航的井下人员精确定位系统[J]. 煤炭学报，2009，

34(8)：1149– Zhen，LIU Dan，LI Chunguang. The preci positioning system of

the mine personnel in coal pit bad on strapdown inertial navigation[J]. Journal of China

Coal Society，2009，34(8)：1149–1152.

[5] 王巍. 光纤陀螺惯性系统[M]. 北京：中国宇航出版社，2010.

[6] PAUL G S. Strapdown inertial navigation integration algorithm design part 1：

Attitude algorithms[J]. Journal of Guidance，Control and Dynamics，1998，21(1)：19–28.

[7] ROBERT M R. Applied mathematics in integrated navigation systems(Third

Edition)[M]. Virginia：American Institute of Aeronautics and Astronautics，Inc，2007.

[8] QI Nie，WEI Gao，LIN Zhao，et al. High accuracy ins bad on fiber optical

gyroscope for AUV application[C]//2th IEEE Conference on Industrial Electronics and

Applications. 2007：2617– 2622.

[9] LI Jie，TAO Ruiping. Initial alignment technology of strapdown inertial navigation

system bad-on stationary ba[C]//Interna­tional Conference on Intelligent Control and

Information Processing. 2010：561–564.

[10] 陈哲. 捷联惯导系统原理[M]. 北京：宇航出版社，1986.