基于MEMS陀螺的矿用新型钻机开孔定向仪研制_燕斌.pdf

第 46 卷 第 6 期煤田地质与勘探Vol. 46 No.6 2018 年 12 月COALGEOLOGY 2. Xi’an Research Institute Co. Ltd., China Coal Technology and Engineering Group Corp., Xi’an 710077, China Abstract At present, the hole-opening orientation can’t satisfy the users’ actual needs, the manual measuring is inaccurate and the hole-opening orientation instrument based on the three axis optical fiber gyro is too expensive in underground coal mine. In order to solve the above problems, a new type instrument for hole-opening orientation is designed. The paper introduced the overall scheme and working principle of the system. The author set up a hardware plat based on MSG7100D-300 gyroscope and KXR94 accelerometer with STM32 microcontroller as the CPU. The system adopts the static four-position for north seeking and Kalman filtering tracking algorithm which could realize system measurement effectively and quickly. The correctness of the scheme is verified by the prototype test. The test results show that the north seeking time, the north finding precision and dynamic measuring precision of the hole orientation instrument based on MEMS gyroscope all meet the requirements of the drilling machine’s opening orientation and the actual needs of the users. Keywords fiber optic gyroscope; MEMS gyroscope; drilling hole; north-seeker 钻探作为探查和治理煤炭开采中隐蔽致灾因素 的一种最直接的探查技术，它具有直观性强和探测 精度高等特点，可广泛应用于构造探测、老空区探 测、探放水、瓦斯卸压以及其他隐蔽致灾因素的探 查中[1-2]。煤矿井下瓦斯抽采、探放水、注浆加固的 效果，直接受钻孔成孔质量的影响，而钻孔成孔的 质量又受钻孔开孔精度的影响。因此，钻探作为煤 矿瓦斯、水害等灾害预防的主要手段之一，其开孔 精度就显得尤为重要。 目前， 大多数煤矿都采用传统的人工丈量方法标 定开孔方位角和倾斜角，其操作程序相当繁琐， 稳钻 的时间长，精度低，给井下防治水、瓦斯抽采等工程 的施工和验收带来诸多不便。2012 年，南京捷嘉科 技有限公司研制了矿用 ZJSD-1 钻孔激光数字化定位 仪，减少了测量时间，提高了测量精度，但测量过程 涉及大量的人工操作， 准确性仍然较大程度依附于人 ChaoXing 188煤田地质与勘探第 46 卷 工操作的精准程度等。2014 年中煤科工集团西安研 究院有限公司与北京航空航天大学联合研制了基于 三轴光纤陀螺的 YHKD2 钻孔定位仪， 该仪器的寻北 精度为 1.5，寻北时间为 3 min[3]。该仪器的精度和 自动化程度较高， 符合煤矿井下的测量需求， 但其高 昂的成本限制了该产品在煤矿井下的应用推广。因 此， 寻求一种满足精度要求、 低成本的煤矿井下钻孔 定向装备迫在眉睫。 基于上述生产中的实际问题， 研 制 了 一 款 基 于 MEMS Micro-Electro Mechanical System 陀螺的矿用新型钻机开孔定向仪下文简称 “定向仪”，并进行了样机试验，检验各项指标，以 期早日达到实际生产的需求。 1定向仪系统构成 开孔定向仪主要由寻北系统和跟踪系统 2 部分 组成，构成框图如图 1 所示。寻北系统主要是用于 测量该仪器在静态情况下与地理北极真北方向之 间的姿态数据倾角和方位角，即确定与真北方向 的姿态基准。跟踪系统主要用于测量该仪器寻北后 的测量数据在动态情况下随时间变化而变化的姿态 测量数据。将该测量仪器放置于与钻杆平行的钻机 导轨处，依据预设值调整钻机的姿态倾角和方位 角，达到预先设定的值即可完成开孔定向操作。 图 1定向仪总体框图 Fig.1Block diagram of the orientation device 2定向仪工作原理 2.1寻北系统的原理 如图 2 所示， 地球自转角速率ωeωe15.041/h， 其方向平行于地球自转轴。 在地球表面纬度为 L 处， 地球自转角速度ωe可以分解为沿子午线 n Y 方向的 水平分量ωN和沿地垂线 Zn方向的垂直分量ωU， 其表 达式分别为式1和式2。 ecosN L1 esinU L2 陀螺是一种测量围绕其敏感轴方向角速率的传 感器；理想情况下，将陀螺垂直安装在水平面上， 使其敏感轴Yg的方向在水平面内与地理坐标系中的 北向真北方向平行，则陀螺敏感到的角速度ωo即 为ωe在水平方向的分量ωN。当陀螺的敏感轴 Yg在 图 2陀螺寻北原理示意图 Fig.2Schematic diagram of gyro north-seeking 水平面内逆时针方向与真北方向存在一个α的夹角 时， 如图 3 所示， 则此时陀螺的输出 o 为 e 的水平 分量在陀螺敏感轴上的投影 g ，即 o cos N 3 图 3陀螺敏感轴与真北向夹角的关系 Fig.3Relationship between gyro sensing axis and true north 只要测得陀螺在相应方位的输出，通过式1和 式3，便可以计算出在该位置陀螺敏感轴与真北方 向的夹角 oo e arccosarccos cos N L     可知理论上，陀螺在单个位置的输出就 可以得到寻北结果，但是单位置的结果与陀螺自身 的零偏、零偏稳定性、标度因数、随机噪声等参数 有直接关系， 而这些参数无法测得确定的结果， 因此， 采用单位置的寻北方法精度很低。 2.2基于 MIMU 惯性导航跟踪系统 微型惯性测量单元Micro Inertial Measurement Unit，简称 MIMU，主要包括三轴 MEMS 陀螺仪和 三轴 MEMS 加速度， 数据采集及其专业的导航软件， 可以提供运动载体的位置、速度和姿态信息。2 种惯 性传感器按照载体坐标系的 3 个正方向进行安装， 三 轴 MEMS 陀螺用来测量相对于惯性坐标系的载体运 动在载体坐标系下的 3 个轴向上的角速度分量； 三轴 MEMS 加速度用来测量载体坐标系下 3 个轴向上的 线加速度分量。 系统主要是利用捷联姿态矩阵完成坐 标变换， 把沿载体坐标系下的 3 个轴向上的线加速度 综上 ChaoXing 第 6 期燕斌等 基于 MEMS 陀螺的矿用新型钻机开孔定向仪研制189 转化为沿导航坐标系下的 3 个轴向上的线加速度， 经 过解算得到载体的速度和位置参数； 把沿载体坐标系 下的 3 个轴向上的角速度分量转化为沿导航坐标系 下 3 个轴向上的角速度分量， 经过解算得到载体的姿 态参数。 3定向仪仪器硬件及软件 3.1定向仪仪器硬件 3.1.1MEMS 陀螺选型 随着微机电技术的出现，MEMS 陀螺在体积、 成本、功耗及抗冲击等方面存在巨大的优势。随着 微电子加工技术的发展，MEMS 惯性传感器特别是 MEMS 陀螺的精度得到很大的提高。CRM100 陀螺 仪是 SILICON SENSING 公司生产的一种新型的高 精度角速度陀螺，既可以输出与角速度呈线性关系 的模拟量，也可以通过 SPI 协议输出数字量。该陀 螺仪采用第 5 代 VSG 陀螺技术，具有低功耗、体积 小、 输出稳定等特点， 尺寸大小为 5.7 mm4.8 mm 1.2 mm，零位稳定性 24 /h，角度随机游走 0.28 /h， 采用 3.3 V 供电， 功耗 4 mA。 MSG7100D- 300 MEMS 微陀螺采用美泰科技公司生产的高精度陀 螺，采用 SPI 协议数字输出，尺寸大小为11.43 mm 11.43 mm3.8 mm，零位稳定性 10 /h，角度随机 游走 0.15 /h，其外围电路如图 4 所示。 图 4MSG7100D-300 陀螺仪外围电路 Fig.4Peripheral circuits of MSG7100D-300 gyro 3.1.2加速度采集电路 系统采用的KXR94加速度计芯片是Kionix公司生 产的三轴加速度计。该加速度计内部已经对温度和电压 波动引起的偏差进行了设计补偿，因此，由于电压和温 度引起的偏差较小。该器件测量范围为2gg 为重力加 速度，灵敏度系数为560 mV/g，非线性度为 0.1％， 零加速度漂移为15010-3g；2.83.3V 均可工作；功 耗很低，静态电流约 1.1 mA，具体电路如图 5 所示。 a 三轴加速度发生电路b 三轴加速度采集电路 图 5加速度数据采集电路 Fig.5Circuit for acceleration data acquisition ChaoXing 190煤田地质与勘探第 46 卷 3.2定向仪系统软件 3.2.1寻北系统算法设计 基于采集位置的不同， 目前可分为多位置方案、 连续转动方案、双位置方案、四位置方案等。尽管 方案形式上有不同，但基本原理上相似，且各有特 点。基于静态四位置寻北系统算法解算简便及在水 平位置时不需要纬度信息等突出优点，本文采用四 位置的方案进行测量，软件流程图如下 图 6寻北系统算法流程图 Fig.6Flow chart of north seeking algorithm 3.2.2动态跟踪系统算法设计 为了更好地进行动态姿态的测量，本文采用卡 尔曼滤波技术实现。卡尔曼滤波 Kalman Filter，简 称 KF 技术通常用于系统状态估计和估计补偿惯 性传感器的测量误差，从而实现多种测量信息融 合[4]。图 7 所示为动态跟踪系统算法设计原理，主 要包括 3 个部分①利用系统寻北结果建立初始捷 联姿态矩阵，利用 MIMU 测量数据进行捷联姿态解 算； ②利用三轴加速度计测量信息估计水平姿态角； ③设计扩展卡尔曼滤波算法，实现姿态角误差的最 优估计，从而得到实时定向仪姿态信息，即方位角、 俯仰角和工具面角。 图 7动态跟踪系统的算法设计原理 Fig.7Design principle of algorithm of dynamic tracking system a. MIMU 捷联姿态解算算法[5] 设计捷联姿态解算算法前，首先需要建立 MEMS 陀螺的测量误差模型。由于确定性误差可以 通过对传感器的测试进行补偿，因此本文只针对随 机误差进行滤波处理。本文采用一种典型的随机误 差模型描述 MEMS 陀螺输入和输出的关系 b y ttb tn tb tw t  ，4 式中 y t为陀螺输出角速率， t为输入真实角 速率， b t为陀螺漂移，由陀螺速率随机游走白噪 声 b w t驱动， n t为测量白噪声，即角度随机游走 白噪声。 本文采用转动四元数表示定向仪的姿态信息， 完成捷联姿态矩阵的解算[6]。定义姿态四元数为 T I0123 , , , qq qq[]Q，则姿态矩阵可以表示为 2222 01231 20 31 30 2 2222 1 20 301232 30 1 1 30 22 30 12 30 1 22 22 222 n b qqqqq qq qq qq q q qq qqqqqq qq q q qq qq qq qq qq q         C 5 姿态四元数 I Q满足微分方程 00 11 22 33 0 0 1 2 0 0 bbb nbxnbynbz bbb nbxnbznby bbb nbynbznbx bbb nbznbynbx qq qq qq qq                           6 其中，,, bbb nbxnbynbz 为机体坐标系相对于导航坐标 系即地理坐标系的转动角速率。利用当前时刻 MIMU测量得到的角速率信息，求解姿态四元数微 分方程式6，便可以获得当前时刻的姿态四元数和 姿态矩阵，利用姿态角和姿态矩阵的关系，求解定 向仪的姿态角如下 arctan1,2/2,2 arcsin3,2 arctan3,1/3,3 nn Ibb n Ib nn Ibb CC C CC              7 b.利用重力矢量估计水平姿态 在静态或匀速运动条件下，甚至在动态特性不 大的条件下，重力矢量通常可用作参考矢量，用来 估计载体的水平姿态角信息。在算法设计时，利用 加速度计测量信息估计定向仪的水平姿态信息，并 将其与MIMU解算的信息进行KF滤波融合，抑制 了MIMU误差积累发散，提高了系统测量精度。设 bbb xyz ggg、、分别为三轴加速度计测量输出的比力信 息，则定向仪水平姿态角解算如下 bb bb s ss tan/ tancos/ xz yz agg agg          8 c. 扩展卡尔曼滤波算法设计 本文设计的扩展卡尔曼滤波器的状态向量为姿 态误差角和陀螺漂移[7-8]。滤波器的测量值取为系统 ChaoXing 第6期燕斌等基于MEMS陀螺的矿用新型钻机开孔定向仪研制191 姿态角误差，姿态角误差是由MIMU解算输出的姿 态角与利用加速度计信息估计得到的姿态角之间的 差值，作为系统测量信息。因此，定义系统状态估 计向量为T,,,,, ENU bx by bzX，则系统状态方 程为。 tttw t  XFX9 其中状态矩阵Ft的系数参照文献[9-10]，设由 MIMU测量信息解算建立的姿态矩阵为 n b  C，而真 实的姿态矩阵为 n b C。将MIMU输出的水平姿态角 和利用加速度计信息求解的水平姿态角表示为真实 值δ, γ和误差值δθ, δγ的之和分别为 111213 212223 313233 n b TTT TTT TTT         C， I I I I          ， S S S S          卡尔曼滤波器测量方程为 1 2 ISI ISI IS IS V t V             Z10 即 ttttZHXV11 测量矩阵为 22 22 3212 32 2212 21 3323 3113 3111 33 2222 31333133 0000 22 0000 T TT T TT t T TT TT TT T TTTT         H 对于建立的扩展卡尔曼滤波器式9和式11， 通过 对连续方程的离散化，进而利用离散卡尔曼滤波方程 进行迭代估计，可以得到姿态误差角的最优估计 ˆˆˆ ,, ENU ，利用 ˆˆˆ ,, ENU 对MIMU获得的姿态矩阵 n b  C进行校正，便可以得到最优估计的姿态矩阵 ˆn b C ˆ nnnn bnbb I   CCCC ， 0 0 0 UN UE NE           12 利用 ˆn b C和式7求解获得最优估计的定向仪姿 态角信息。系统姿态估计卡尔曼滤波器的基本原理 和迭代估计流程如图8所示。 图 8系统姿态估计卡尔曼滤波器原理图 Fig.8Schematic diagram of Kalman filter of system attitude estimation 4试验结果及分析 4.1寻北重复性 从表1可以看出在不同倾角下，四位置静态寻 北方案后，方位角和倾角与真值的误差结果。寻北 重复性试验结果表明定向仪寻北测量的时间小于 5 min，方位角寻北的精度较高，最大偏差为0. 3。 同时，倾角的最大误差的精度为0.1。 表 1不同位置、倾角情况下的寻北精度 Table 1North-finding accuracy in different position and inclination单位 位置方位角真值 倾角–10 倾角0 倾角10 倾角误差方位误差倾角误差方位误差倾角误差方位误差 100.100.200.050.15–0.020.30 2600.050.180.010.050.030.20 31200.020.220.08–0.050.050.16 41800.02–0.240.050.080.060.18 52400.00–0.15–0.020.06–0.060.25 63000.030.05–0.060.020.040.15 4.2动态跟踪试验 由于倾角参数采用加速度传感器静态测量， 随着时间漂移，倾角的大小不会产生变化。因此， 本文中仅对方位角进行测试。表2给出惯性导航 跟踪的精度的测量结果。从表2可以看出，随着 时间的漂移方位角精度的误差增大。在间隔60 s 后，方位角最大偏差0.3左右；间隔300 s后，方 位角最大偏差0.7；在间隔600 s后，方位角最大 偏差1.6。 5结 论 a. 本文设计了一种基于高精度MEMS陀螺的 新型开孔定向仪，采用寻北和跟踪2个部分构成， 降低了原有采用光纤陀螺传感器的成本。 ChaoXing 192煤田地质与勘探第46卷 表 2方位角在不同时间间隔漂移精度测试 Table 2Accuracy test of azimuth drift at different time intervals 次数 不同时间间隔后方位角偏差/ 60 s300 s600 s 10.20.71.5 20.20.61.6 30.20.61.4 40.30.71.5 50.30.51.3 60.20.51.2 b. 详细叙述了测量原理，硬件参数和软件设计 测试不同倾角下，和时间变化情况下方位角精度， 测量结果能够满足实际钻机开孔的设计需要。 参考文献 [1] 石智军， 姚宁平， 叶根飞. 煤矿井下瓦斯抽采钻孔施工技术与 装备[J]. 煤炭科学技术，2009，3771–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， 3771–4. [2] 田宏亮. 瓦斯抽采履带式坑道钻机的研制与应用[J]. 煤田地 质与勘探，2008，36577–80. TIAN Hongliang. Manufacture and application on track-mounted rig for gas drainage[J]. Coal Geology Exploration，2008， 36577–80. [3] 林铁，林恒，高爽，等. 光纤陀螺测斜仪数据采集及传输单元 设计与实现[J]. 测井技术，2009，334402–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，334402–405. [4] 吕振， 刘丹， 李春光. 基于捷联惯性导航的井下人员精确定位 系统[J]. 煤炭学报，2009，3481149–1152. LYUZhen， LIU Dan， LIChunguang. Theprecisepositioningsystem of the mine personnel in coal pit based on strapdown inertial navigation[J]. Journal of China Coal Society ， 2009 ， 348 1149–1152. [5] 王巍. 光纤陀螺惯性系统[M]. 北京中国宇航出版社，2010. [6] PAUL G S. Strapdown inertial navigation integration algorithm design part 1Attitude algorithms[J]. Journal of Guidance， Control and Dynamics，1998，21119–28. [7] ROBERT M R. Applied mathematics in integrated navigation systemsThird Edition[M]. Virginia American Institute of Aeronautics and Astronautics，Inc，2007. [8] QI Nie，WEI Gao，LIN Zhao，et al. High accuracy ins based 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 inertialnavigationsystembased-onstationary base[C]//International Conference on Intelligent Control and Ination Processing. 2010561–564. [10] 陈哲. 捷联惯导系统原理[M]. 北京宇航出版社，1986. 责任编辑 聂爱兰 ChaoXing