林婷婷，劉澤楷，馬銀龍，朱金寶

(1.吉林大學(xué) 儀器科學(xué)與電氣工程學(xué)院，吉林 長春 130012；2.吉林大學(xué) 建設(shè)工程學(xué)院，吉林 長春 130012)

經(jīng)濟(jì)社會發(fā)展與礦產(chǎn)資源密不可分，礦產(chǎn)資源在氣候變化、資源危機(jī)、全球治理、可持續(xù)發(fā)展等一系列重大挑戰(zhàn)都有著非常重要的地位，而戰(zhàn)略性關(guān)鍵礦產(chǎn)資源供給安全一直是世界各國必須面對的一大難題[1]。當(dāng)前我國約2/3 的戰(zhàn)略性礦產(chǎn)資源需要進(jìn)口，其中約有1/2 的戰(zhàn)略性礦產(chǎn)對外依存度超出了50%。而我國在礦產(chǎn)資源勘探還面臨著增儲方法有限、新增資源儲量無法滿足國家戰(zhàn)略需求的問題[2]。根據(jù)《中國礦產(chǎn)資源報告2022》，2021 年我國地質(zhì)勘查投資972.87 億元，較上年增長11.6%[3]，為礦產(chǎn)資源勘探提供了有力的支持。但是在鉆探技術(shù)層面，如何快速定向連續(xù)獲得巖(礦)心樣品，為礦產(chǎn)資源儲量評價提供準(zhǔn)確參數(shù)，是目前階段國內(nèi)礦產(chǎn)勘探“卡脖子”技術(shù)難題。

針對以上礦產(chǎn)資源勘探技術(shù)問題，國內(nèi)外學(xué)者和研究機(jī)構(gòu)開展了廣泛的研究工作。其中，定向鉆進(jìn)連續(xù)取心作為一項(xiàng)新興技術(shù)[4]，廣泛應(yīng)用于礦產(chǎn)資源勘探、隧道超前探測、非常規(guī)能源勘探等領(lǐng)域[5]。該技術(shù)要求在鉆進(jìn)過程中對鉆具進(jìn)行精確定位和控制，針對此問題，本文對定向取心鉆具的隨鉆姿態(tài)監(jiān)測問題展開了研究。

在測斜儀器方面，陀螺測斜儀和磁性MWD(Measurement While Drilling)是廣泛使用的姿態(tài)監(jiān)測工具。陀螺測斜儀按信號傳輸方式可分為有線陀螺測斜儀和無線隨鉆陀螺測斜儀(GWD)，有線陀螺測斜儀已在全世界范圍內(nèi)廣泛應(yīng)用，國內(nèi)對GWD 的研究目前正在測試階段，而國外主要由美國陀螺數(shù)據(jù)公司主導(dǎo)。美國的GWD 已廣泛應(yīng)用于鉆井測量，相關(guān)產(chǎn)品有20° GWD、40° GWD 和70° GWD 三種型號，分別適用于最大作業(yè)井斜角度為20°、40°和70°的情況[6]；在進(jìn)行大斜度套管開窗、導(dǎo)斜器定向、海洋平臺叢式井或加密井防碰繞障和救援井施工時，由于磁干擾的存在，無法使用磁性MWD，必須采用不受磁干擾的陀螺測斜儀來有效解決測斜問題。同時，在向東或向西的大斜度井眼中鉆進(jìn)時，應(yīng)用磁性MWD 和某些類型的陀螺測斜儀可能會導(dǎo)致較大的方位測量誤差。王立兵等[7]指出慣性傳感元件存在的零點(diǎn)漂移問題會對測量造成影響，Chen Weicao 等[8]進(jìn)一步分析了零點(diǎn)漂移的影響因素，將兩大主要影響因素總結(jié)為讀數(shù)偏移和角度偏移；胡茂曉、蔣碩碩[9-10]提出，構(gòu)建姿態(tài)監(jiān)測系統(tǒng)的關(guān)鍵問題有兩個：姿態(tài)傳感器數(shù)據(jù)的采集與融合；數(shù)據(jù)的實(shí)時回傳與分析。為解決這2 個關(guān)鍵問題，相關(guān)研究深入到姿態(tài)矩陣[11]、四元數(shù)解算[12-13]和濾波算法等領(lǐng)域。其中，對于姿態(tài)監(jiān)測系統(tǒng)常用的濾波算法有互補(bǔ)濾波和卡爾曼濾波[14-15]。黃鎮(zhèn)等[16]基于互補(bǔ)濾波和卡爾曼濾波設(shè)計(jì)了一種姿態(tài)解算方法，實(shí)現(xiàn)了誤差2°以內(nèi)的歐拉角測量；M.M.R.Masrafee 等[17]將卡爾曼濾波應(yīng)用于姿態(tài)監(jiān)測系統(tǒng)，對卡爾曼濾波算法進(jìn)行了理論分析；J.Sola、Ruiter 等[18-19]深入解析了卡爾曼濾波的更新過程。D.M.Henderson 等[20]將卡爾曼濾波與四元數(shù)解算方法結(jié)合，用于分析航天飛機(jī)的歐拉角和變換矩陣；P.Owczarek[21]基于卡爾曼濾波和互補(bǔ)濾波，實(shí)現(xiàn)了視覺系統(tǒng)中圓形標(biāo)志物的中心點(diǎn)追蹤捕捉。

綜上，筆者通過整體框架構(gòu)建、電路設(shè)計(jì)、控制與通信編程和數(shù)據(jù)處理等方面開展了定向鉆進(jìn)連續(xù)取心姿態(tài)監(jiān)測系統(tǒng)的設(shè)計(jì)，并結(jié)合模擬實(shí)驗(yàn)進(jìn)行了驗(yàn)證分析，以期研發(fā)出具有自主知識產(chǎn)權(quán)、質(zhì)量穩(wěn)定可靠的定向鉆進(jìn)連續(xù)取心姿態(tài)監(jiān)測系統(tǒng)，為定向鉆進(jìn)連續(xù)取心提供可靠的技術(shù)支撐。

1 隨鉆姿態(tài)監(jiān)測技術(shù)

隨鉆姿態(tài)監(jiān)測技術(shù)通過在鉆井過程中依據(jù)傳感器數(shù)據(jù)和算法來實(shí)時監(jiān)測鉆具的姿態(tài)，提供有關(guān)鉆具的傾斜角度、方位角度和軸向旋轉(zhuǎn)角度等信息，協(xié)助鉆井操作控制和避免潛在的問題，涉及的關(guān)鍵組件與技術(shù)主要包括：MEMS(Micro-Electro-Mechanical System)傳感器、慣性導(dǎo)航系統(tǒng)(Inertial Navigation System,INS)、鉆桿振動監(jiān)測技術(shù)、數(shù)據(jù)融合和算法處理技術(shù)等，通過軟硬件協(xié)同，提升姿態(tài)監(jiān)測系統(tǒng)的準(zhǔn)確性和穩(wěn)定性。

為了攻克定向連續(xù)取心的技術(shù)難題，實(shí)現(xiàn)鉆具姿態(tài)實(shí)時監(jiān)測，推進(jìn)取心鉆具智能化，本文結(jié)合了鉆探技術(shù)、傳感器技術(shù)與嵌入式系統(tǒng)，尋求鉆具姿態(tài)監(jiān)測的最優(yōu)解。

在實(shí)際應(yīng)用中，采用單片機(jī)作為核心控制組件，單片機(jī)接收傳感器回傳的數(shù)據(jù)并進(jìn)行計(jì)算、控制輸出以及與其他設(shè)備進(jìn)行通信，實(shí)現(xiàn)實(shí)時高精度數(shù)據(jù)處理，從而提高系統(tǒng)的穩(wěn)定性和精度。對硬件和軟件進(jìn)行聯(lián)合設(shè)計(jì)，最終實(shí)現(xiàn)姿態(tài)監(jiān)測系統(tǒng)的完整功能，角度測量誤差0.5°以內(nèi)。

2 系統(tǒng)總體設(shè)計(jì)

本文設(shè)計(jì)的姿態(tài)監(jiān)測系統(tǒng)以STM32F103C8T6 單片機(jī)為主控，通過姿態(tài)傳感器MPU9250 對運(yùn)動姿態(tài)進(jìn)行監(jiān)測，提供俯仰角、橫滾角和航向角等姿態(tài)信息，進(jìn)一步求取鉆具的方位角、頂角和工具面向角。經(jīng)過校正、消噪、濾波處理后的姿態(tài)角度信息通過LoRa模塊發(fā)送至地面上位機(jī)，上位機(jī)軟件完成角度數(shù)據(jù)顯示、運(yùn)動曲線繪制和3D 姿態(tài)展示。設(shè)計(jì)的系統(tǒng)如圖1 所示。

圖1 鉆具姿態(tài)監(jiān)測系統(tǒng)Fig.1 Block diagram of attitude monitoring system for drilling tool

3 系統(tǒng)硬件設(shè)計(jì)

3.1 主控芯片

為滿足姿態(tài)監(jiān)測系統(tǒng)的要求，選擇基于ARM Cortex-M 內(nèi)核的STM32F103C8T6 芯片作為主控制器。該芯片具備處理速度快和功耗低等優(yōu)勢，能夠滿足多參數(shù)測量、高實(shí)時性和多種擴(kuò)展功能等要求，能夠滿足鉆具姿態(tài)監(jiān)測系統(tǒng)在定向鉆進(jìn)連續(xù)取心鉆具上的應(yīng)用。主控模塊如圖2 所示。

圖2 STM32F103C8T6 芯片連接Fig.2 Schematic diagram of STM32F103C8T6 chip

3.2 姿態(tài)傳感器模塊

為了實(shí)現(xiàn)地下鉆具姿態(tài)實(shí)時監(jiān)測，需要綜合測量精度、模塊尺寸和功耗等因素對傳感器進(jìn)行選型。MPU9250 是首個將9 軸運(yùn)動姿態(tài)監(jiān)測整合為一體的數(shù)字傳感器，在解決傳感器組合中軸間差異問題的同時，實(shí)現(xiàn)了傳感器體積的縮小和系統(tǒng)功耗的降低。其內(nèi)部集成了三軸陀螺儀、三軸加速度計(jì)和三軸磁力計(jì)，并具備3 個16 位加速度AD 輸出，3 個16 位加速度AD 輸出和14 位磁力計(jì)AD 輸出，可編程陀螺儀參數(shù)可選±250、±500、±1 000、±2 000 (°)/s，可編程的加速度參數(shù)可選±2、±4、±8、±16 g，磁力計(jì)最大量程可達(dá)到±4 800 μT。在本文應(yīng)用中，陀螺儀滿量程范圍設(shè)置為±2 000 (°)/s，陀螺儀的ADC 位數(shù)為16 位，由此計(jì)算得到靈敏度為：65 536/4 000=16.4 LSB/((°)·s-1)。加速度計(jì)滿量程范圍設(shè)置為±2 g，靈敏度為：65 536/4=16 384 LSB/g。MPU9250 與單片機(jī)的數(shù)據(jù)交互通過IIC 接口實(shí)現(xiàn)，最高傳輸速率可達(dá)400 kHz/s，其外圍電路如圖3 所示。

圖3 MPU9250 及其外圍電路Fig.3 MPU9250 and its peripheral circuit

MPU9250 姿態(tài)傳感器獲取姿態(tài)信息的基本原理是：陀螺儀獲得角速度源數(shù)據(jù)，通過積分得到當(dāng)前的姿態(tài)，用加速度計(jì)和磁力計(jì)的數(shù)據(jù)分別對陀螺儀進(jìn)行重力校正和地磁校正。重力校正和地磁校正采用的方法為固定矢量觀測法，重力矢量計(jì)算可以得到以重力矢量為法向量的當(dāng)?shù)厮矫?，從而校正了歐拉角中的俯仰角和橫滾角。結(jié)合已知的水平面，從地磁矢量分離出水平面上指向磁極的矢量，從而校正了歐拉角中的航向角。

3.3 無線通信模塊

LoRa (Long Range Radio)是Semtech 公司創(chuàng)建的低功耗局域網(wǎng)無線標(biāo)準(zhǔn)，在相同功耗條件下，LoRa 的傳輸距離相比傳統(tǒng)的無線射頻通信提升3～5 倍，實(shí)現(xiàn)了低功耗和遠(yuǎn)距離的統(tǒng)一。本文選用的LoRa 模塊電路圖如圖4 所示，模塊傳輸距離可達(dá)6 000 m，功耗約0.5 W，尺寸為36 mm×20 mm×3 mm，同時滿足了系統(tǒng)遠(yuǎn)傳輸距離、低功耗、小尺寸的要求。

圖4 LoRa 模塊連接Fig.4 Schematic diagram of LoRa module

3.4 鋰電池與電源模塊

為了實(shí)現(xiàn)隨鉆姿態(tài)監(jiān)測，系統(tǒng)需設(shè)計(jì)獨(dú)立供電。系統(tǒng)搭載了一塊容量為400 mAh 鋰電池作為供電電源，其標(biāo)稱電壓為3.70 V，放電截止電壓為2.75 V，而STM32 模塊、MPU9250 模塊和LoRa 模塊的額定工作電壓均為3.30 V，因此選用了TI 公司生產(chǎn)的TPS61025DRCR 電源芯片，對鋰電池電源進(jìn)行穩(wěn)壓。穩(wěn)壓芯片及其外圍電路如圖5 所示。

圖5 TPS61025DRCR 電源芯片模塊及外圍電路Fig.5 TPS61025DRCR power supply chip module and peripheral circuit

3.5 小型化系統(tǒng)電路設(shè)計(jì)

為了實(shí)現(xiàn)隨鉆測量的目標(biāo)，整個系統(tǒng)必須是低功耗的，且系統(tǒng)電路必須是小型化的。這對監(jiān)測模塊系統(tǒng)電路的尺寸提出了較高的要求，因此，對電路進(jìn)行了設(shè)計(jì)，其尺寸為：長70 mm、寬25 mm、高7 mm，集成了STM32 主控模塊、MPU9250 傳感器模塊、LoRa 無線通信模塊和電源模塊，并由鋰電池供電，可供系統(tǒng)在非休眠狀態(tài)下工作6 h 以上，系統(tǒng)電路實(shí)物如圖6 所示。

圖6 系統(tǒng)電路實(shí)物Fig.6 Physical diagram of system circuit

4 系統(tǒng)軟件設(shè)計(jì)

本系統(tǒng)軟件設(shè)計(jì)部分基于Keil5、MATLAB、Lab-VIEW 等軟件和STM32 單片機(jī)硬件平臺，主要實(shí)現(xiàn)功能為MPU9250 九軸數(shù)據(jù)的獲取與處理、STM32 主控數(shù)據(jù)解算與處理、LoRa 數(shù)據(jù)傳輸和上位機(jī)姿態(tài)展示。

4.1 軟件流程

系統(tǒng)的軟件流程如圖7 所示，首先進(jìn)行系統(tǒng)時鐘的初始化，隨后進(jìn)行串口、LoRa、單片機(jī)I2C 接口和MPU9250 傳感器的初始化。MPU9250 傳感器回傳的數(shù)據(jù)存儲到寄存器中，由STM32 內(nèi)核處理器進(jìn)行校正、四元數(shù)歸一化、互補(bǔ)濾波和卡爾曼濾波，最后上位機(jī)匹配LoRa 模塊。STM32 主控負(fù)責(zé)完成姿態(tài)傳感器源數(shù)據(jù)采集、姿態(tài)解算和姿態(tài)輸出，LoRa 模塊負(fù)責(zé)傳輸STM32 的姿態(tài)輸出數(shù)據(jù)，最終由上位機(jī)實(shí)現(xiàn)角度數(shù)據(jù)與3D 姿態(tài)實(shí)時顯示。

圖7 軟件流程Fig.7 Software flowchart

4.2 姿態(tài)解算

姿態(tài)解算本質(zhì)上是求解鉆具的固聯(lián)坐標(biāo)系和地理坐標(biāo)系間的姿態(tài)矩陣[13]。設(shè)鉆具的固聯(lián)坐標(biāo)系為b系，當(dāng)鉆具轉(zhuǎn)動時，b系也隨之轉(zhuǎn)動。設(shè)地理坐標(biāo)系為R系，X軸指向東，Y軸指向北，Z軸指向天。由b系到R系的坐標(biāo)變換矩陣即為鉆具的姿態(tài)矩陣，如圖8所示。主控模塊根據(jù)傳感器的輸出實(shí)時計(jì)算出姿態(tài)矩陣，從而實(shí)時獲取鉆具當(dāng)前的俯仰角、橫滾角和航向角。陀螺儀和加速度計(jì)分別用來測量鉆具姿態(tài)變化的角速度信息和加速度信息，根據(jù)陀螺儀和加速度計(jì)的輸出和磁力計(jì)的校正，可以對姿態(tài)矩陣進(jìn)行實(shí)時更新。

圖8 鉆具坐標(biāo)系、地理坐標(biāo)系Fig.8 Schematic diagram of drill tool coordinate system and geographic coordinate system

在姿態(tài)解算中，通常采用3 種方法來表示姿態(tài)矩陣，分別是歐拉角、方向余弦和四元數(shù)。三種解算方法見表1，這3 種方法各有優(yōu)劣，在實(shí)際應(yīng)用時的選用情況也有所區(qū)別。四元數(shù)法由于其計(jì)算量小，并且適用于全姿態(tài)解算[12]，可以較好地對鉆具姿態(tài)信息進(jìn)行實(shí)時監(jiān)測，因此，本文選用四元數(shù)法對姿態(tài)數(shù)據(jù)進(jìn)行解算。

表1 姿態(tài)解算方法對照Table 1 Comparison of attitude estimation methods

4.3 四元數(shù)理論

四元數(shù)的基本表示形式為：Q(q0,q1,q2,q3)=q0+q1i+q2j+q3k，其中，q0、q1、q2、q3為實(shí)數(shù)，i、j、k為相互正交的單位向量，也是虛數(shù)單位。

在三維空間中，任何旋轉(zhuǎn)都可以表示為繞某個軸以某個角度的旋轉(zhuǎn)，此方法被稱為Axis-Angle 表示方法。假設(shè) (x,y,z)是 表示軸方向的單位向量，θ表示繞該軸旋轉(zhuǎn)的角度，那么四維向量(θ,x,y,z)可以用來表示三維空間中的任意旋轉(zhuǎn)。如果表征R系至b系的旋轉(zhuǎn)四元數(shù)Q已確定，則可以計(jì)算出姿態(tài)矩陣：

為了直觀地表示旋轉(zhuǎn)狀態(tài)，以θ1、θ2、θ3的形式表示鉆具的姿態(tài)，轉(zhuǎn)換公式為：

式中：θ1為俯仰角；θ2為橫滾角；θ3為航向角，對其進(jìn)行校正后，通過矩陣運(yùn)算，求取鉆具的方位角、頂角和工具面向角。

由此可見，四元數(shù)Q包含了所有的姿態(tài)信息，姿態(tài)更新實(shí)質(zhì)上是單位四元數(shù)Q的計(jì)算過程。

4.4 零點(diǎn)漂移的校準(zhǔn)

MPU9250 傳回的源數(shù)據(jù)中夾雜了較嚴(yán)重的噪聲，在芯片處于靜止?fàn)顟B(tài)時，仍會出現(xiàn)較為明顯的數(shù)據(jù)擺動[7]。除此之外，受到零點(diǎn)漂移的影響，數(shù)據(jù)并非只在靜止工作點(diǎn)附近波動。零點(diǎn)漂移現(xiàn)象的影響因素主要有兩點(diǎn)，一是芯片的測量精度，二是芯片的安裝精度。前者稱為讀數(shù)偏移，后者稱為角度偏移[8]。在使用濾波算法抑制噪聲之前需對數(shù)據(jù)進(jìn)行校準(zhǔn)，以消除偏移的影響。

零點(diǎn)漂移的校準(zhǔn)過程就是找出擺動數(shù)據(jù)圍繞的中心值的過程。以陀螺儀X軸源數(shù)據(jù)Dx和加速度計(jì)X軸源數(shù)據(jù)Ex為例，將系統(tǒng)靜置于水平面，此時Dx和Ex理論值為0，但由于零點(diǎn)漂移現(xiàn)象的存在，往往會存在偏移量。以50 Hz 采樣率采集了一組數(shù)據(jù)見表2。

表2 陀螺儀和加速度計(jì)的零點(diǎn)漂移Table 2 Zero drift of gyroscope and acceleromet er

在實(shí)際應(yīng)用中，為了盡量減小角度偏移，一般將MPU9250 傳感器固定在鉆具上，使二者坐標(biāo)系盡可能地重合，以最大程度地減小的角度偏移；對于讀數(shù)偏移，在每次啟動系統(tǒng)時采用如下方法進(jìn)行校準(zhǔn)，以Dx為例，在校準(zhǔn)陀螺儀角速度數(shù)據(jù)時，以10 個數(shù)據(jù)的均值7.97 LSB 為讀數(shù)的偏移量，將每次的讀數(shù)減去偏移量作為校準(zhǔn)后的讀數(shù)，即：

類似的也有：

式中：Dx0為校準(zhǔn)后的陀螺儀X軸數(shù)據(jù)；Ex0為校準(zhǔn)后的加速度計(jì)X軸數(shù)據(jù)。以此類推，可以獲得陀螺儀X軸角速度讀數(shù)Dx0、陀螺儀Y軸角速度讀數(shù)Dy0，和陀螺儀Z軸角速度讀數(shù)Dz0，以及加速度計(jì)X軸加速度讀數(shù)Ex0和加速度計(jì)Y軸加速度讀數(shù)Ey0。

4.5 互補(bǔ)濾波

陀螺儀在動態(tài)響應(yīng)方面表現(xiàn)良好，但在解算姿態(tài)時會產(chǎn)生積累誤差；磁力計(jì)和加速度計(jì)在測量姿態(tài)時沒有累積誤差，但動態(tài)響應(yīng)較差。因此，它們在頻域上具有互補(bǔ)的特性。為了獲取最優(yōu)的姿態(tài)數(shù)據(jù)，可以采用互補(bǔ)濾波的方法進(jìn)行融合，如圖9 所示。在短時間內(nèi)陀螺儀得到的角度更優(yōu)，分配更高的權(quán)重；隨著測量時間的延長，采用4.4 節(jié)中的校準(zhǔn)方法，使用校準(zhǔn)后的加速度計(jì)數(shù)據(jù)對陀螺儀數(shù)據(jù)進(jìn)行處理，增加加速度計(jì)的權(quán)重，通過不同的濾波器(高通、低通等)融合陀螺儀與加速度計(jì)的數(shù)據(jù)，得到整個頻帶的信號，從而提高測量精度和系統(tǒng)的動態(tài)特性[16]，就是互補(bǔ)濾波的核心思想。

圖9 互補(bǔ)濾波Fig.9 Schematic diagram of complementary filtering

將系統(tǒng)置于靜止?fàn)顟B(tài)，采集陀螺儀源數(shù)據(jù)，采集時長900 s，并對得到的數(shù)據(jù)進(jìn)行積分解算得到角度值，解算出的角度信息如圖10 所示?；パa(bǔ)濾波融合后的數(shù)據(jù)表明，角度值一直在0°附近，效果較好。校準(zhǔn)前的數(shù)據(jù)在前200 s，陀螺儀產(chǎn)生的累積誤差較小，在200～900 s，陀螺儀的累計(jì)誤差較為明顯，900 s 誤差約為0.8°。

圖10 陀螺儀積分誤差與校準(zhǔn)Fig.10 Gyroscope integration error and calibration

4.6 卡爾曼濾波

雖然互補(bǔ)濾波能夠在很大程度上利用陀螺儀、加速度計(jì)和傳感器的頻率特性優(yōu)勢，提高姿態(tài)數(shù)據(jù)精度[14]，但陀螺儀、加速度計(jì)和磁力計(jì)各自都存在誤差，互補(bǔ)濾波的方法難以進(jìn)一步提升傳感器獲取數(shù)據(jù)的精度。

卡爾曼濾波是著名的遞歸算法，最初用于線性問題[15]。在本文研究的姿態(tài)監(jiān)測系統(tǒng)中，卡爾曼濾波融合陀螺儀、加速度計(jì)和磁力計(jì)的輸出數(shù)據(jù)，根據(jù)上一狀態(tài)預(yù)測當(dāng)前狀態(tài)，再與實(shí)際測得的狀態(tài)進(jìn)行比較，將預(yù)估的當(dāng)前狀態(tài)與測得的當(dāng)前狀態(tài)融合[20-21]，對鉆具的姿態(tài)角進(jìn)行估算，得出最優(yōu)的角度和角速度[17]，減小了數(shù)據(jù)的波動，大大確保了數(shù)據(jù)的準(zhǔn)確性，從而糾正實(shí)際測量得到的曲線，實(shí)現(xiàn)了數(shù)據(jù)精度的提升。

4.6.1 卡爾曼濾波理論

卡爾曼濾波算法包括預(yù)測過程和更新過程，通過比較觀測值和預(yù)測值之間的差異，實(shí)現(xiàn)更準(zhǔn)確的狀態(tài)估計(jì)。

預(yù)測過程根據(jù)上一時刻(t-1 時刻)的后驗(yàn)估計(jì)值，估計(jì)當(dāng)前時刻(t時刻)的狀態(tài)，并得到t時刻的先驗(yàn)估計(jì)值；更新過程則利用當(dāng)前時刻的測量值，修正預(yù)測過程中的估計(jì)值，以獲得當(dāng)前時刻的后驗(yàn)估計(jì)值[18]。

卡爾曼濾波預(yù)測方程如下：

根據(jù)上述公式及主要參數(shù)，可以完成卡爾曼濾波的預(yù)測和更新過程，實(shí)現(xiàn)對實(shí)測值的修正。

4.6.2 實(shí)測驗(yàn)證

為了驗(yàn)證卡爾曼濾波的有效性，最大程度上還原姿態(tài)監(jiān)測系統(tǒng)的地下工作環(huán)境，對系統(tǒng)開展了震動數(shù)據(jù)采集實(shí)驗(yàn)。實(shí)驗(yàn)選用了Z軸線性馬達(dá)對監(jiān)測系統(tǒng)施加持續(xù)震動，圖11a 為實(shí)驗(yàn)環(huán)境，為緩沖較小的水泥路面，以方位角讀數(shù)為例，對比卡爾曼濾波前后的姿態(tài)數(shù)據(jù)去噪的效果，初始方位角約為13°，實(shí)測數(shù)據(jù)展示在圖11b 中。

圖11 卡爾曼濾波實(shí)測Fig.11 Kalman filtering measurement

由測試結(jié)果可知，卡爾曼濾波有效地抑制了由系統(tǒng)震動引入的噪聲，具有較好的消噪能力。

4.7 上位機(jī)設(shè)計(jì)

為了實(shí)時、直觀地觀測到準(zhǔn)確的鉆具姿態(tài)信息，設(shè)計(jì)了基于LabVIEW 的鉆具姿態(tài)監(jiān)測上位機(jī)系統(tǒng)。所設(shè)計(jì)的系統(tǒng)具有界面簡潔友好，功能全面的優(yōu)點(diǎn)，能夠滿足本項(xiàng)目對于鉆具姿態(tài)監(jiān)測的需要。

主控模塊解算后的數(shù)據(jù)通過LoRa 模塊無線傳輸至上位機(jī)，上位機(jī)通過矩陣運(yùn)算實(shí)現(xiàn)了方位角、頂角和工具面向角的實(shí)時顯示，并繪制對應(yīng)的圖線。其中方位角顯示范圍為 -180°～ 180°，頂角顯示范圍為-90°～ 90°，工具面向角顯示范圍為 -180°～ 180°。同時，為了直觀地展示鉆具的姿態(tài)，在上位機(jī)中設(shè)置了3D姿態(tài)展示界面，3D 姿態(tài)展示功能具有高準(zhǔn)確性和高實(shí)時性的特點(diǎn)，能夠幫助確定鉆具的地下姿態(tài)。

5 實(shí)驗(yàn)驗(yàn)證

為了驗(yàn)證本文研究內(nèi)容和研發(fā)儀器的有效性，模擬地下的干擾和鉆具鉆進(jìn)時的位置和環(huán)境特征，在吉林大學(xué)朝陽校區(qū)地質(zhì)宮博物館進(jìn)行了系統(tǒng)測試。

5.1 區(qū)域地質(zhì)概況

地質(zhì)宮博物館位置如圖12 所示。由圖可知，實(shí)驗(yàn)場地周邊有多棟建筑(居民樓、教學(xué)樓、學(xué)生公寓等)、信號基站和供電線路等干擾因素。同時地質(zhì)宮博物館內(nèi)有常年運(yùn)轉(zhuǎn)的大功率電氣設(shè)備，對系統(tǒng)的源數(shù)據(jù)獲取、信號傳輸和姿態(tài)展示提出了很高的要求。地質(zhì)宮博物館高度約50 m，上位機(jī)設(shè)置在地質(zhì)宮頂層，監(jiān)測系統(tǒng)設(shè)置在地質(zhì)宮底層。

圖12 長春市地質(zhì)宮博物館位置Fig.12 Location of Changchun Geological Palace Museum

5.2 測試結(jié)果

測試結(jié)果表明，在校園和教學(xué)樓高電磁干擾的環(huán)境下，設(shè)計(jì)的系統(tǒng)仍能穩(wěn)定工作。將上位機(jī)和接收模塊設(shè)置于地質(zhì)宮頂層，多次調(diào)整監(jiān)測系統(tǒng)的姿態(tài)，均可實(shí)現(xiàn)系統(tǒng)功能，展示準(zhǔn)確的姿態(tài)信息。采用BWIMU100C 高精度慣性測量單元測得的數(shù)據(jù)作為參考值，系統(tǒng)的誤差達(dá)到0.5°以內(nèi)。實(shí)測結(jié)果如圖13 所示。

圖13 測試實(shí)驗(yàn)?zāi)Ｐ图皽y試結(jié)果Fig.13 Experimental models and test results

6 結(jié)論

a.在姿態(tài)監(jiān)測技術(shù)和嵌入式系統(tǒng)的基礎(chǔ)上完成了定向鉆進(jìn)連續(xù)取心姿態(tài)監(jiān)測系統(tǒng)設(shè)計(jì)，構(gòu)建了以MPU9250 傳感器為核心，STM32 為主控的定向鉆進(jìn)連續(xù)取心姿態(tài)監(jiān)測系統(tǒng)，用四元數(shù)法對采集的源數(shù)據(jù)進(jìn)行解算，后續(xù)采用互補(bǔ)濾波和卡爾曼濾波，有效地抑制了系統(tǒng)的噪聲，提高了系統(tǒng)的監(jiān)測精度，姿態(tài)角監(jiān)測誤差小于0.5°。

b.設(shè)計(jì)的姿態(tài)監(jiān)測系統(tǒng)電路長70 mm，寬25 mm，高7 mm，相比其他姿態(tài)監(jiān)測裝備具有較小的體積，并且設(shè)計(jì)有長距離、低功耗的無線傳輸模塊和獨(dú)立的電源與供電模塊，能夠?qū)崿F(xiàn)對鉆具各個組件進(jìn)行姿態(tài)監(jiān)測，無需外源供電。

c.設(shè)計(jì)的姿態(tài)監(jiān)測系統(tǒng)為定向鉆進(jìn)連續(xù)取心技術(shù)的實(shí)現(xiàn)提供了一種新思路，研制的系統(tǒng)裝備、形成的技術(shù)參數(shù)體系需要進(jìn)一步工程檢驗(yàn)，以驗(yàn)證其姿態(tài)監(jiān)測的可靠性和穩(wěn)定性。

d.鉆具姿態(tài)監(jiān)測是構(gòu)成定向鉆進(jìn)連續(xù)取心技術(shù)的關(guān)鍵技術(shù)之一，雖取得了一些實(shí)用性成果，但仍有部分工作需要進(jìn)一步完善，如設(shè)計(jì)恒溫倉以適應(yīng)寬溫地質(zhì)環(huán)境并減小溫度漂移，設(shè)計(jì)緩震結(jié)構(gòu)以進(jìn)一步改善實(shí)測信號信噪比，拓展姿態(tài)監(jiān)測技術(shù)對于物探領(lǐng)域的普適性。