定向鉆進連續取心鉆具姿態監測系統設計

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

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

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

針對以上礦產資源勘探技術問題，國內外學者和研究機構開展了廣泛的研究工作。其中，定向鉆進連續取心作為一項新興技術[4]，廣泛應用于礦產資源勘探、隧道超前探測、非常規能源勘探等領域[5]。該技術要求在鉆進過程中對鉆具進行精確定位和控制，針對此問題，本文對定向取心鉆具的隨鉆姿態監測問題展開了研究。

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

綜上，筆者通過整體框架構建、電路設計、控制與通信編程和數據處理等方面開展了定向鉆進連續取心姿態監測系統的設計，并結合模擬實驗進行了驗證分析，以期研發出具有自主知識產權、質量穩定可靠的定向鉆進連續取心姿態監測系統，為定向鉆進連續取心提供可靠的技術支撐。

1 隨鉆姿態監測技術

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

為了攻克定向連續取心的技術難題，實現鉆具姿態實時監測，推進取心鉆具智能化，本文結合了鉆探技術、傳感器技術與嵌入式系統，尋求鉆具姿態監測的最優解。

在實際應用中，采用單片機作為核心控制組件，單片機接收傳感器回傳的數據并進行計算、控制輸出以及與其他設備進行通信，實現實時高精度數據處理，從而提高系統的穩定性和精度。對硬件和軟件進行聯合設計，最終實現姿態監測系統的完整功能，角度測量誤差0.5°以內。

2 系統總體設計

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

圖1 鉆具姿態監測系統Fig.1 Block diagram of attitude monitoring system for drilling tool

3 系統硬件設計

3.1 主控芯片

為滿足姿態監測系統的要求，選擇基于ARM Cortex-M 內核的STM32F103C8T6 芯片作為主控制器。該芯片具備處理速度快和功耗低等優勢，能夠滿足多參數測量、高實時性和多種擴展功能等要求，能夠滿足鉆具姿態監測系統在定向鉆進連續取心鉆具上的應用。主控模塊如圖2 所示。

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

3.2 姿態傳感器模塊

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

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

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

3.3 無線通信模塊

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

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

3.4 鋰電池與電源模塊

為了實現隨鉆姿態監測，系統需設計獨立供電。系統搭載了一塊容量為400 mAh 鋰電池作為供電電源，其標稱電壓為3.70 V，放電截止電壓為2.75 V，而STM32 模塊、MPU9250 模塊和LoRa 模塊的額定工作電壓均為3.30 V，因此選用了TI 公司生產的TPS61025DRCR 電源芯片，對鋰電池電源進行穩壓。穩壓芯片及其外圍電路如圖5 所示。

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

3.5 小型化系統電路設計

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

圖6 系統電路實物Fig.6 Physical diagram of system circuit

4 系統軟件設計

本系統軟件設計部分基于Keil5、MATLAB、Lab-VIEW 等軟件和STM32 單片機硬件平臺，主要實現功能為MPU9250 九軸數據的獲取與處理、STM32 主控數據解算與處理、LoRa 數據傳輸和上位機姿態展示。

4.1 軟件流程

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

圖7 軟件流程Fig.7 Software flowchart

4.2 姿態解算

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

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

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

表1 姿態解算方法對照Table 1 Comparison of attitude estimation methods

4.3 四元數理論

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

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

為了直觀地表示旋轉狀態，以θ1、θ2、θ3的形式表示鉆具的姿態，轉換公式為：

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

由此可見，四元數Q包含了所有的姿態信息，姿態更新實質上是單位四元數Q的計算過程。

4.4 零點漂移的校準

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

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

表2 陀螺儀和加速度計的零點漂移Table 2 Zero drift of gyroscope and acceleromet er

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

類似的也有：

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

4.5 互補濾波

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

圖9 互補濾波Fig.9 Schematic diagram of complementary filtering

將系統置于靜止狀態，采集陀螺儀源數據，采集時長900 s，并對得到的數據進行積分解算得到角度值，解算出的角度信息如圖10 所示。互補濾波融合后的數據表明，角度值一直在0°附近，效果較好。校準前的數據在前200 s，陀螺儀產生的累積誤差較小，在200～900 s，陀螺儀的累計誤差較為明顯，900 s 誤差約為0.8°。

圖10 陀螺儀積分誤差與校準Fig.10 Gyroscope integration error and calibration

4.6 卡爾曼濾波

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

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

4.6.1 卡爾曼濾波理論

卡爾曼濾波算法包括預測過程和更新過程，通過比較觀測值和預測值之間的差異，實現更準確的狀態估計。

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

卡爾曼濾波預測方程如下：

根據上述公式及主要參數，可以完成卡爾曼濾波的預測和更新過程，實現對實測值的修正。

4.6.2 實測驗證

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

圖11 卡爾曼濾波實測Fig.11 Kalman filtering measurement

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

4.7 上位機設計

為了實時、直觀地觀測到準確的鉆具姿態信息，設計了基于LabVIEW 的鉆具姿態監測上位機系統。所設計的系統具有界面簡潔友好，功能全面的優點，能夠滿足本項目對于鉆具姿態監測的需要。

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

5 實驗驗證

為了驗證本文研究內容和研發儀器的有效性，模擬地下的干擾和鉆具鉆進時的位置和環境特征，在吉林大學朝陽校區地質宮博物館進行了系統測試。

5.1 區域地質概況

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

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

5.2 測試結果

測試結果表明，在校園和教學樓高電磁干擾的環境下，設計的系統仍能穩定工作。將上位機和接收模塊設置于地質宮頂層，多次調整監測系統的姿態，均可實現系統功能，展示準確的姿態信息。采用BWIMU100C 高精度慣性測量單元測得的數據作為參考值，系統的誤差達到0.5°以內。實測結果如圖13 所示。

圖13 測試實驗模型及測試結果Fig.13 Experimental models and test results

6 結論

a.在姿態監測技術和嵌入式系統的基礎上完成了定向鉆進連續取心姿態監測系統設計，構建了以MPU9250 傳感器為核心，STM32 為主控的定向鉆進連續取心姿態監測系統，用四元數法對采集的源數據進行解算，后續采用互補濾波和卡爾曼濾波，有效地抑制了系統的噪聲，提高了系統的監測精度，姿態角監測誤差小于0.5°。

b.設計的姿態監測系統電路長70 mm，寬25 mm，高7 mm，相比其他姿態監測裝備具有較小的體積，并且設計有長距離、低功耗的無線傳輸模塊和獨立的電源與供電模塊，能夠實現對鉆具各個組件進行姿態監測，無需外源供電。

c.設計的姿態監測系統為定向鉆進連續取心技術的實現提供了一種新思路，研制的系統裝備、形成的技術參數體系需要進一步工程檢驗，以驗證其姿態監測的可靠性和穩定性。

d.鉆具姿態監測是構成定向鉆進連續取心技術的關鍵技術之一，雖取得了一些實用性成果，但仍有部分工作需要進一步完善，如設計恒溫倉以適應寬溫地質環境并減小溫度漂移，設計緩震結構以進一步改善實測信號信噪比，拓展姿態監測技術對于物探領域的普適性。