定向钻进连续取心钻具姿态监测系统设计

林婷婷，刘泽楷，马银龙，朱金宝

(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.钻具姿态监测是构成定向钻进连续取心技术的关键技术之一，虽取得了一些实用性成果，但仍有部分工作需要进一步完善，如设计恒温仓以适应宽温地质环境并减小温度漂移，设计缓震结构以进一步改善实测信号信噪比，拓展姿态监测技术对于物探领域的普适性。