基于MEMS陀螺的礦用新型鉆機(jī)開(kāi)孔定向儀研制

基于MEMS陀螺的礦用新型鉆機(jī)開(kāi)孔定向儀研制

燕斌;程建遠(yuǎn);蔡遠(yuǎn)利;代晨昱;李萍;馮宏

【摘 要】針對(duì)目前煤礦井下采用人工測(cè)量方法不準(zhǔn)確和基于三軸光纖陀螺的鉆孔

定位成本較高等問(wèn)題,設(shè)計(jì)了一種新型鉆機(jī)開(kāi)孔定向儀,介紹了儀器的總體方案及工

作原理,搭建了基于MSG7100D-300陀螺和KXR94加速度計(jì),以STM32單片機(jī)為

CPU核心的硬件平臺(tái);系統(tǒng)采用靜態(tài)四位置方法尋北方式及卡爾曼濾波的跟蹤算法,

有效地、快速地實(shí)現(xiàn)系統(tǒng)的測(cè)量.通過(guò)對(duì)原型樣機(jī)的實(shí)驗(yàn)測(cè)試,結(jié)果表明,基于

MEMS陀螺的鉆機(jī)開(kāi)孔定向儀其尋北時(shí)間、尋北精度以及動(dòng)態(tài)測(cè)量精度,均滿足了

鉆機(jī)開(kāi)孔定向的指標(biāo)要求及用戶的實(shí)際需求.

【期刊名稱(chēng)】《煤田地質(zhì)與勘探》

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

【總頁(yè)數(shù)】6頁(yè)(P187-192)

【關(guān)鍵詞】光纖陀螺儀;MEMS陀螺;鉆機(jī)開(kāi)孔;尋北儀

【作 者】燕斌;程建遠(yuǎn);蔡遠(yuǎn)利;代晨昱;李萍;馮宏

【作者單位】西安交通大學(xué)電子與信息工程學(xué)院,陜西西安710049;中煤科工集團(tuán)

西安研究院有限公司,陜西西安710077;中煤科工集團(tuán)西安研究院有限公司,陜西西

安710077;西安交通大學(xué)電子與信息工程學(xué)院,陜西西安710049;中煤科工集團(tuán)西

安研究院有限公司,陜西西安710077;中煤科工集團(tuán)西安研究院有限公司,陜西西安

710077;中煤科工集團(tuán)西安研究院有限公司,陜西西安710077

【正文語(yǔ)種】中 文

【中圖分類(lèi)】TD163

鉆探作為探查和治理煤炭開(kāi)采中隱蔽致災(zāi)因素的一種最直接的探查技術(shù)，它具有直

觀性強(qiáng)和探測(cè)精度高等特點(diǎn)，可廣泛應(yīng)用于構(gòu)造探測(cè)、老空區(qū)探測(cè)、探放水、瓦斯

卸壓以及其他隱蔽致災(zāi)因素的探查中[1-2]。煤礦井下瓦斯抽采、探放水、注漿加

固的效果，直接受鉆孔成孔質(zhì)量的影響，而鉆孔成孔的質(zhì)量又受鉆孔開(kāi)孔精度的影

響。因此，鉆探作為煤礦瓦斯、水害等災(zāi)害預(yù)防的主要手段之一，其開(kāi)孔精度就顯

得尤為重要。

目前，大多數(shù)煤礦都采用傳統(tǒng)的人工丈量方法標(biāo)定開(kāi)孔方位角和傾斜角，其操作程

序相當(dāng)繁瑣，穩(wěn)鉆的時(shí)間長(zhǎng)，精度低，給井下防治水、瓦斯抽采等工程的施工和驗(yàn)

收帶來(lái)諸多不便。2012年，南京捷嘉科技有限公司研制了礦用ZJSD-1鉆孔激光

數(shù)字化定位儀，減少了測(cè)量時(shí)間，提高了測(cè)量精度，但測(cè)量過(guò)程涉及大量的人工操

作，準(zhǔn)確性仍然較大程度依附于人工操作的精準(zhǔn)程度等。2014年中煤科工集團(tuán)西

安研究院有限公司與北京航空航天大學(xué)聯(lián)合研制了基于三軸光纖陀螺的YHKD2鉆

孔定位儀，該儀器的尋北精度為1.5°，尋北時(shí)間為3 min[3]。該儀器的精度和自

動(dòng)化程度較高，符合煤礦井下的測(cè)量需求，但其高昂的成本限制了該產(chǎn)品在煤礦井

下的應(yīng)用推廣。因此，尋求一種滿足精度要求、低成本的煤礦井下鉆孔定向裝備迫

在眉睫。基于上述生產(chǎn)中的實(shí)際問(wèn)題，研制了一款基于MEMS Micro-Electro

Mechanical System陀螺的礦用新型鉆機(jī)開(kāi)孔定向儀(下文簡(jiǎn)稱(chēng)“定向儀”)，并

進(jìn)行了樣機(jī)試驗(yàn)，檢驗(yàn)各項(xiàng)指標(biāo)，以期早日達(dá)到實(shí)際生產(chǎn)的需求。

開(kāi)孔定向儀主要由尋北系統(tǒng)和跟蹤系統(tǒng)2部分組成，構(gòu)成框圖如圖1所示。尋北

系統(tǒng)主要是用于測(cè)量該儀器在靜態(tài)情況下與地理北極(真北方向)之間的姿態(tài)數(shù)據(jù)

(傾角和方位角)，即確定與真北方向的姿態(tài)基準(zhǔn)。跟蹤系統(tǒng)主要用于測(cè)量該儀器尋

北后的測(cè)量數(shù)據(jù)在動(dòng)態(tài)情況下隨時(shí)間變化而變化的姿態(tài)測(cè)量數(shù)據(jù)。將該測(cè)量?jī)x器放

置于與鉆桿平行的鉆機(jī)導(dǎo)軌處，依據(jù)預(yù)設(shè)值調(diào)整鉆機(jī)的姿態(tài)(傾角和方位角)，達(dá)到

預(yù)先設(shè)定的值即可完成開(kāi)孔定向操作。

如圖2所示，地球自轉(zhuǎn)角速率ωe (ωe=15.041(°)/h)，其方向平行于地球自轉(zhuǎn)軸。

在地球表面緯度為L處，地球自轉(zhuǎn)角速度ωe可以分解為沿子午線方向的水平分量

ωN和沿地垂線Zn方向的垂直分量ωU，其表達(dá)式分別為式(1)和式(2)。

陀螺是一種測(cè)量圍繞其敏感軸方向角速率的傳感器；理想情況下，將陀螺垂直安裝

在水平面上，使其敏感軸Yg的方向在水平面內(nèi)與地理坐標(biāo)系中的北向(真北方向)

平行，則陀螺敏感到的角速度ωo即為ωe在水平方向的分量ωN。當(dāng)陀螺的敏感

軸Yg在水平面內(nèi)逆時(shí)針?lè)较蚺c真北方向存在一個(gè)α的夾角時(shí)，如圖3所示，則此

時(shí)陀螺的輸出為的水平分量在陀螺敏感軸上的投影，即

只要測(cè)得陀螺在相應(yīng)方位的輸出，通過(guò)式(1)和式(3)，便可以計(jì)算出在該位置陀螺

敏感軸與真北方向的夾角：

綜上可知：理論上，陀螺在單個(gè)位置的輸出就可以得到尋北結(jié)果，但是單位置的結(jié)

果與陀螺自身的零偏、零偏穩(wěn)定性、標(biāo)度因數(shù)、隨機(jī)噪聲等參數(shù)有直接關(guān)系，而這

些參數(shù)無(wú)法測(cè)得確定的結(jié)果，因此，采用單位置的尋北方法精度很低。

微型慣性測(cè)量單元(Micro Inertial Measurement Unit，簡(jiǎn)稱(chēng)MIMU)，主要包括

三軸MEMS陀螺儀和三軸MEMS加速度，數(shù)據(jù)采集及其專(zhuān)業(yè)的導(dǎo)航軟件，可以

提供運(yùn)動(dòng)載體的位置、速度和姿態(tài)信息。2種慣性傳感器按照載體坐標(biāo)系的3個(gè)正

方向進(jìn)行安裝，三軸MEMS陀螺用來(lái)測(cè)量相對(duì)于慣性坐標(biāo)系的載體運(yùn)動(dòng)在載體坐

標(biāo)系下的3個(gè)軸向上的角速度分量；三軸MEMS加速度用來(lái)測(cè)量載體坐標(biāo)系下3

個(gè)軸向上的線加速度分量。系統(tǒng)主要是利用捷聯(lián)姿態(tài)矩陣完成坐標(biāo)變換，把沿載體

坐標(biāo)系下的3個(gè)軸向上的線加速度轉(zhuǎn)化為沿導(dǎo)航坐標(biāo)系下的3個(gè)軸向上的線加速

度，經(jīng)過(guò)解算得到載體的速度和位置參數(shù)；把沿載體坐標(biāo)系下的3個(gè)軸向上的角

速度分量轉(zhuǎn)化為沿導(dǎo)航坐標(biāo)系下3個(gè)軸向上的角速度分量，經(jīng)過(guò)解算得到載體的

姿態(tài)參數(shù)。

3.1.1 MEMS陀螺選型

隨著微機(jī)電技術(shù)的出現(xiàn)，MEMS陀螺在體積、成本、功耗及抗沖擊等方面存在巨

大的優(yōu)勢(shì)。隨著微電子加工技術(shù)的發(fā)展，MEMS慣性傳感器特別是MEMS陀螺的

精度得到很大的提高。CRM100陀螺儀是SILICON SENSING公司生產(chǎn)的一種新

型的高精度角速度陀螺，既可以輸出與角速度呈線性關(guān)系的模擬量，也可以通過(guò)

SPI協(xié)議輸出數(shù)字量。該陀螺儀采用第5代VSG陀螺技術(shù)，具有低功耗、體積小、

輸出穩(wěn)定等特點(diǎn)，尺寸大小為：5.7 mm×4.8 mm× 1.2 mm，零位穩(wěn)定性24

(°)/h，角度隨機(jī)游走0.28 (°)/h，采用3.3 V供電，功耗4 mA。MSG7100D-

300 MEMS 微陀螺采用美泰科技公司生產(chǎn)的高精度陀螺，采用SPI協(xié)議數(shù)字輸出，

尺寸大小為：11.43 mm× 11.43 mm×3.8 mm，零位穩(wěn)定性10 (°)/h，角度隨機(jī)

游走0.15 (°)/h，其外圍電路如圖4所示。

3.1.2 加速度采集電路

系統(tǒng)采用的KXR94加速度計(jì)芯片是Kionix公司生產(chǎn)的三軸加速度計(jì)。該加速度

計(jì)內(nèi)部已經(jīng)對(duì)溫度和電壓波動(dòng)引起的偏差進(jìn)行了設(shè)計(jì)補(bǔ)償，因此，由于電壓和溫度

引起的偏差較小。該器件測(cè)量范圍為±2g(g為重力加速度)，靈敏度系數(shù)為(560

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

作；功耗很低，靜態(tài)電流約1.1 mA，具體電路如圖5所示。

3.2.1 尋北系統(tǒng)算法設(shè)計(jì)

基于采集位置的不同，目前可分為多位置方案、連續(xù)轉(zhuǎn)動(dòng)方案、雙位置方案、四位

置方案等。盡管方案形式上有不同，但基本原理上相似，且各有特點(diǎn)。基于靜態(tài)四

位置尋北系統(tǒng)算法解算簡(jiǎn)便及在水平位置時(shí)不需要緯度信息等突出優(yōu)點(diǎn)，本文采用

四位置的方案進(jìn)行測(cè)量，軟件流程圖如下：

3.2.2 動(dòng)態(tài)跟蹤系統(tǒng)算法設(shè)計(jì)

為了更好地進(jìn)行動(dòng)態(tài)姿態(tài)的測(cè)量，本文采用卡爾曼濾波技術(shù)實(shí)現(xiàn)。卡爾曼濾波

( Kalman Filter，簡(jiǎn)稱(chēng)KF) 技術(shù)通常用于系統(tǒng)狀態(tài)估計(jì)和估計(jì)補(bǔ)償慣性傳感器的測(cè)

量誤差，從而實(shí)現(xiàn)多種測(cè)量信息融合[4]。圖7所示為動(dòng)態(tài)跟蹤系統(tǒng)算法設(shè)計(jì)原理，

主要包括3個(gè)部分：①利用系統(tǒng)尋北結(jié)果建立初始捷聯(lián)姿態(tài)矩陣，利用MIMU測(cè)

量數(shù)據(jù)進(jìn)行捷聯(lián)姿態(tài)解算；②利用三軸加速度計(jì)測(cè)量信息估計(jì)水平姿態(tài)角；③設(shè)計(jì)

擴(kuò)展卡爾曼濾波算法，實(shí)現(xiàn)姿態(tài)角誤差的最優(yōu)估計(jì)，從而得到實(shí)時(shí)定向儀姿態(tài)信息，

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

捷聯(lián)姿態(tài)解算算法[5]

設(shè)計(jì)捷聯(lián)姿態(tài)解算算法前，首先需要建立MEMS陀螺的測(cè)量誤差模型。由于確定

性誤差可以通過(guò)對(duì)傳感器的測(cè)試進(jìn)行補(bǔ)償，因此本文只針對(duì)隨機(jī)誤差進(jìn)行濾波處理。

本文采用一種典型的隨機(jī)誤差模型描述MEMS陀螺輸入和輸出的關(guān)系：

式中為陀螺輸出角速率，為輸入真實(shí)角速率，為陀螺漂移，由陀螺速率隨機(jī)游走白

噪聲驅(qū)動(dòng)，為測(cè)量白噪聲，即角度隨機(jī)游走白噪聲。

本文采用轉(zhuǎn)動(dòng)四元數(shù)表示定向儀的姿態(tài)信息，完成捷聯(lián)姿態(tài)矩陣的解算[6]。定義

姿態(tài)四元數(shù)為，則姿態(tài)矩陣可以表示為：

姿態(tài)四元數(shù)滿足微分方程

其中，為機(jī)體坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系即地理坐標(biāo)系的轉(zhuǎn)動(dòng)角速率。利用當(dāng)前時(shí)刻

MIMU測(cè)量得到的角速率信息，求解姿態(tài)四元數(shù)微分方程式(6)，便可以獲得當(dāng)前

時(shí)刻的姿態(tài)四元數(shù)和姿態(tài)矩陣，利用姿態(tài)角和姿態(tài)矩陣的關(guān)系，求解定向儀的姿態(tài)

角如下：

b. 利用重力矢量估計(jì)水平姿態(tài)

在靜態(tài)或勻速運(yùn)動(dòng)條件下，甚至在動(dòng)態(tài)特性不大的條件下，重力矢量通常可用作參

考矢量，用來(lái)估計(jì)載體的水平姿態(tài)角信息。在算法設(shè)計(jì)時(shí)，利用加速度計(jì)測(cè)量信息

估計(jì)定向儀的水平姿態(tài)信息，并將其與MIMU解算的信息進(jìn)行KF濾波融合，抑

制了MIMU誤差積累發(fā)散，提高了系統(tǒng)測(cè)量精度。設(shè)分別為三軸加速度計(jì)測(cè)量輸

出的比力信息，則定向儀水平姿態(tài)角解算如下：

c. 擴(kuò)展卡爾曼濾波算法設(shè)計(jì)

本文設(shè)計(jì)的擴(kuò)展卡爾曼濾波器的狀態(tài)向量為姿態(tài)誤差角和陀螺漂移[7-8]。濾波器

的測(cè)量值取為系統(tǒng)姿態(tài)角誤差，姿態(tài)角誤差是由MIMU解算輸出的姿態(tài)角與利用

加速度計(jì)信息估計(jì)得到的姿態(tài)角之間的差值，作為系統(tǒng)測(cè)量信息。因此，定義系統(tǒng)

狀態(tài)估計(jì)向量為，則系統(tǒng)狀態(tài)方程為。

其中狀態(tài)矩陣F(t)的系數(shù)參照文獻(xiàn)[9-10]，設(shè)由MIMU測(cè)量信息解算建立的姿態(tài)

矩陣為，而真實(shí)的姿態(tài)矩陣為。將MIMU輸出的水平姿態(tài)角和利用加速度計(jì)信息

求解的水平姿態(tài)角表示為真實(shí)值δ, γ和誤差值δθ, δγ的之和分別為：

卡爾曼濾波器測(cè)量方程為

即

測(cè)量矩陣為

對(duì)于建立的擴(kuò)展卡爾曼濾波器式(9)和式(11)，通過(guò)對(duì)連續(xù)方程的離散化，進(jìn)而利

用離散卡爾曼濾波方程進(jìn)行迭代估計(jì)，可以得到姿態(tài)誤差角的最優(yōu)估計(jì)，利用對(duì)

MIMU獲得的姿態(tài)矩陣進(jìn)行校正，便可以得到最優(yōu)估計(jì)的姿態(tài)矩陣：

利用和式(7)求解獲得最優(yōu)估計(jì)的定向儀姿態(tài)角信息。系統(tǒng)姿態(tài)估計(jì)卡爾曼濾波器

的基本原理和迭代估計(jì)流程如圖8所示。

從表1可以看出在不同傾角下，四位置靜態(tài)尋北方案后，方位角和傾角與真值的

誤差結(jié)果。尋北重復(fù)性試驗(yàn)結(jié)果表明：定向儀尋北測(cè)量的時(shí)間小于5 min，方位角

尋北的精度較高，最大偏差為 0. 3°。同時(shí)，傾角的最大誤差的精度為0.1°。

由于傾角參數(shù)采用加速度傳感器靜態(tài)測(cè)量，隨著時(shí)間漂移，傾角的大小不會(huì)產(chǎn)生變

化。因此，本文中僅對(duì)方位角進(jìn)行測(cè)試。表2給出慣性導(dǎo)航跟蹤的精度的測(cè)量結(jié)

果。從表2可以看出，隨著時(shí)間的漂移方位角精度的誤差增大。在間隔60 s后，

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

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

a. 本文設(shè)計(jì)了一種基于高精度MEMS陀螺的新型開(kāi)孔定向儀，采用尋北和跟蹤2

個(gè)部分構(gòu)成，降低了原有采用光纖陀螺傳感器的成本。

b. 詳細(xì)敘述了測(cè)量原理，硬件參數(shù)和軟件設(shè)計(jì)測(cè)試不同傾角下，和時(shí)間變化情況

下方位角精度，測(cè)量結(jié)果能夠滿足實(shí)際鉆機(jī)開(kāi)孔的設(shè)計(jì)需要。

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.

【相關(guān)文獻(xiàn)】

[1] 石智軍，姚寧平，葉根飛. 煤礦井下瓦斯抽采鉆孔施工技術(shù)與裝備[J]. 煤炭科學(xué)技術(shù)，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ī)的研制與應(yīng)用[J]. 煤田地質(zhì)與勘探，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] 林鐵，林恒，高爽，等. 光纖陀螺測(cè)斜儀數(shù)據(jù)采集及傳輸單元設(shè)計(jì)與實(shí)現(xiàn)[J]. 測(cè)井技術(shù)，

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] 呂振，劉丹，李春光. 基于捷聯(lián)慣性導(dǎo)航的井下人員精確定位系統(tǒng)[J]. 煤炭學(xué)報(bào)，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] 王巍. 光纖陀螺慣性系統(tǒng)[M]. 北京：中國(guó)宇航出版社，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] 陳哲. 捷聯(lián)慣導(dǎo)系統(tǒng)原理[M]. 北京：宇航出版社，1986.