CN111521179A - 基于行进速度自检测的钻探用定位定向仪孔内定位方法 - Google Patents

Abstract

本发明属于地质勘探领域，公开了基于行进速度自检测的钻探用定位定向仪孔内定位方法。本发明结合长距离水平取芯钻机的工作特点，合理规划取芯器的工作模式，充分利用定位定向仪保存的全部测量数据，构建逆向测量数据序列，同时对钻探管道接缝进行运动检测，实现行进速度自检测，并基于完整约束卡尔曼滤波器进行正向、逆向自主导航定位，结合零速修正对定位误差进行抑制校正，并利用正向、逆向自主导航定位误差特性的差异互补性，以加权定位输出作为钻进路径轨迹，提高定位精度。本发明能够满足高原高寒地区地质勘探的需求，为全面、准确的掌握高原高寒地区铁路沿线的地质信息提供有力支撑。

Description

基于行进速度自检测的钻探用定位定向仪孔内定位方法

技术领域

本发明属于地质勘探领域，涉及勘探过程中取芯钻机的孔内定位方法，特别涉及基于行进速度自检的钻探用定位定向仪孔内定位方法。

背景技术

高原高寒地区铁路隧道施工过程中，需要全面、准确的掌握沿线的地质信息，然而受高原高寒地区恶劣地面条件的限制，一般的竖向探测无法实施，不能满足高原高寒地区地质探测需求。长距离水平取芯钻机是一种适用于水平方向快速地质钻探、长距离水平取芯的装备，其能够满足铁路、公路、水利、矿山等领域各种岩石隧道施工的长距离地质取芯钻探，对于高原高寒地区铁路的建设具有十分重要的意义。为了精准的控制钻进路径，满足大埋深、长距离地质取芯钻探的需求，需要在孔内对水平取芯钻机进行精准定位定向，为此，长距离水平取芯钻机需要同时配置定位定向仪。定位定向仪包含惯性测量单元，由惯性测量单元计算输出水平取芯钻机的位置信息，进而对钻进路径进行调整和控制。然而，受惯性器件自身误差的影响，长距离水平钻探情况下，惯性测量单元的定位误差随着工作时间的延长将不断增大，影响钻进路径的控制精度。因此，需要借助辅助手段或者辅助信息对惯性测量单元的定位误差进行抑制或校正，进而实现钻进路径的精确控制。

实际作业环境下，受工况条件以及各型号水平取芯钻机配套设备的限制，定位定向仪能用的辅助定位手段或辅助信息十分有限，并且不尽相同，如何充分挖掘实际工况环境中的可用信息进而提高定位定向精度，成为地质勘探技术人员十分关注的问题。本发明针对高原高寒地区地质勘探的需求，研究基于行进速度自检测的钻探用定位定向仪孔内定位方法，针对长距离水平取芯钻机的工作特点，当定位定向仪在钻探管道内推进时，利用定位定向仪加装的前端加速度计、尾端加速度计分别对钻探管道接缝进行运动检测，根据定位定向仪的长度信息及两只加速度计检测到接缝的时间差，实现行进速度自检测，最后基于完整约束卡尔曼滤波器实现长距离水平取芯钻机的孔内精确定位，进而提高钻进路径的控制精度。

发明内容

考虑到水平取芯钻机在钻进过程中，每钻进一定长度将会布设钻探管道，钻探管道之间相互连接形成整段的钻进路径，钻探管道之间存在接缝，接缝可以作为检测标志使用。通过在定位定向仪的前端、尾端加装加速度计，分别对钻探管道接缝进行运动检测，根据定位定向仪的长度信息及两只加速度计检测到接缝的时间差，实现行进速度自检测。以此为基础，本发明要解决的技术问题就在于：结合长距离水平取芯钻机的工作特点，合理规划取芯器的工作模式，充分利用定位定向仪保存的全部测量数据，同时利用定位定向仪加装的前端加速度计、尾端加速度计分别对钻探管道接缝进行运动检测，根据定位定向仪的长度信息及两只加速度计检测到接缝的时间差，实现行进速度自检测，最后基于完整约束卡尔曼滤波器实现长距离水平取芯钻机的孔内精确定位，进而提高钻进路径的控制精度，满足高原高寒地区地质勘探的需求。

为解决上述技术问题，本发明提出的解决方案为：

基于行进速度自检测的钻探用定位定向仪孔内定位方法，包括以下步骤：

(1)在定位定向仪的前端及尾端分别加装加速度计，并精确测量前端加速度计、尾端加速度计之间的长度距离L，进而将定位定向仪安装到取芯器上，并放置于取芯钻机载车的基座上，向定位定向仪装订初始位置信息和初始速度信息，装订完成后定位定向仪静止15～25分钟，并进行初始自对准，获得初始姿态信息，定位定向仪要在线保存惯性测量单元测量得到的角增量信息和速度增量信息；定位定向仪完成初始自对准后，基于完整约束卡尔曼滤波器进行正向自主导航定位；其中，完整约束卡尔曼滤波器按照如下步骤设计：

(1.1)以姿态误差φⁿ、速度误差δvⁿ、位置误差δpⁿ、陀螺漂移ε^b、加速度计零偏

定位定向仪惯性测量单元的安装误差η为系统状态x(t)，分别确定姿态误差、速度误差、陀螺漂移、加速度计零偏、安装误差的微分方程如下：

其中，

表示导航坐标系n相对于惯性坐标系i的旋转角速度，

表示载体坐标系b与导航坐标系n之间的姿态矩阵，fⁿ表示导航坐标系下表示的比力，

表示地球自转角速度，

表示转移角速度，vⁿ表示速度，

分别表示导航坐标系旋转角速度误差量、地球自转角速度误差量、转移角速度误差量，

表示陀螺组件测量误差，

表示加速度计组件测量误差，w_g、w_a分别表示陀螺组件测量噪声、加速度计组件测量噪声，η＝[η_θ η_Ψ]^T由俯仰角安装误差η_θ及航向角安装误差η_Ψ构成；

(1.2)根据步骤(1.1)中确定的姿态误差、速度误差、位置误差、陀螺漂移、加速度计零偏、安装误差微分方程，构建系统状态方程如下：

其中，

表示系统状态矩阵；

式中，v_E、v_N、v_U分别表示东向、北向、垂向速度，L表示当地纬度，h表示当地高度，R_E、R_N分别表示卯酉圈半径、子午圈半径，ω_ie表示地球自转角速度模值；

表示系统噪声矩阵；

w(t)＝[w_g w_a]^T表示系统噪声；

(1.3)取芯器沿钻进路径推进时，其侧向速度及垂向速度为零，并且当取芯器推进至钻探管道接缝处时，接缝会导致振动加速度，分别记录保存前向加速度计检测到的振动时刻t₁及尾端加速度计检测到的振动时刻t₂，则取芯器的前向速度v_f为L/(t₂-t₁)，进而以前向速度误差δv_y、侧向速度误差δv_x及垂向速度误差δv_z构建完整约束观测量z(t)＝[δv_x δv_yδv_z]^T，并确定观测方程，其中，观测方程的确定通过以下步骤实现：

(1.3.1)将定位定向仪速度输出投影到惯性测量单元坐标系m，按照如下方式：

其中，

表示定位定向仪速度输出在惯性测量单元坐标系m下的投影，v^m表示真实的m坐标系下表示的定位定向仪速度，

表示载体坐标系b与惯性测量单元坐标系m之间的安装关系矩阵，ζ＝[η_θ η_γ η_Ψ]^T表示安装误差角，

表示导航坐标系n与载体坐标系b之间的姿态矩阵；由于横滚角安装误差η_γ不会影响前向速度投影，将其赋值为0，即η_γ＝0；

(1.3.2)以

作为完整约束观测量z(t)，构建观测方程如下：

z(t)＝H(t)x(t)+υ(t) (4)

其中，

且

υ(t)表示观测噪声；

(2)由取芯钻机载车将取芯器推送至钻孔处并驻留，取芯器在钻孔处驻留10～20秒钟，并完成定位定向仪的第一次零速修正，然后采用高压气推的方式将取芯器沿着钻进路径推进，定位定向仪在线保存惯性测量单元测量得到的角增量信息和速度增量信息，其中，零速修正按照如下步骤进行：

(2.1)定位定向仪根据角速度、速度信息自主检测到其零速状态，零速状态下的观测量为

(2.2)以

的x、y、z分量作为零速状态下的观测量z_ZUPT(t)，构建观测方程如下：

z_ZUPT(t)＝H_ZUPT(t)x(t)+μ(t) (5)

其中，

μ(t)表示观测噪声；

(2.3)根据步骤(2.1)、(2.2)所述对零速状态下的定位定向仪完成量测更新；

(3)当取芯器推进至钻进路径末端的钻机处，由取芯器完成取芯作业，然后取芯器在钻进路径末端再次驻留10～20秒钟，并完成定位定向仪的第二次零速修正；此外，定位定向仪要在线保存惯性测量单元测量得到的角增量信息和速度增量信息；

(4)第二次零速修正完成后，通过卷扬机将取芯器从孔内拖出，当取芯器被拖至钻孔处时再次驻留10～20秒钟，并完成定位定向仪的第三次零速修正；第三次零速修正完成后，将取芯器拖动至取芯钻机载车的基座上，再次静止15～25分钟；此外，定位定向仪要在线保存惯性测量单元测量得到的角增量信息和速度增量信息；

(5)将陀螺组件测量角速度

加速度计组件测量比力f^b及地球自转角速度

取反，并按照从后至前的时间顺序反转步骤(1)-(4)中保存的角增量信息、速度增量信息，构成逆向测量数据序列，前向速度信息v_f只进行时间上的逆序处理、不取反，最后基于完整约束卡尔曼滤波器对逆向测量数据序列进行逆向自主导航定位，其中，逆向导航解算如下式所述：

其中，

分别表示逆向时序d、d-1时刻的姿态矩阵，

分别表示逆向时序d、d-1时刻的速度，

分别表示逆向时序d、d-1时刻的位置，gⁿ、

分别表示正向解算、逆向解算当地重力加速度，I₃表示三阶单位矩阵，ΔT表示采样间隔；

逆向自主导航定位包括如下步骤：

(5.1)利用逆向测量数据的第一个15～25分钟的静态阶段完成初始对准，获得初始姿态信息；

(5.2)按照步骤(1.1)-(1.3)所述的基于完整约束卡尔曼滤波器进行逆向滤波，姿态误差方程、速度误差方程、位置误差方程、陀螺漂移、加速度零偏及定位定向仪惯性测量单元的安装误差的微分方程保持不变，观测方程也保持不变；

(5.3)逆向自主导航定位过程中，当取芯器处于驻留状态时，按照步骤(2.1)-(2.3)所述零速修正方式进行误差校正；

(6)以时间对齐后的正向自主导航定位结果pⁿ与逆向自主导航定位结果

的加权结果作为定位输出，加权方式采用下式所述：

其中，p^s为加权结果，P_s、P_f、P_b分别为平滑方差、正向方差、逆向方差；

最后将取芯器推进段的定位结果作为钻进路径轨迹，并根据钻进路径轨迹计算其相对于设计路径的偏差，进而对钻进路径进行调整。

进一步的，所述步骤(1)、(4)中取芯器在取芯钻机载车基座上的静止时长分别为15分钟。

进一步的，所述步骤(1)、(4)中取芯器在取芯钻机载车基座上的静止时长分别为25分钟。

进一步的，所述步骤(2)、(3)、(4)中取芯器在钻孔处及钻进路径末端的驻留时长分别为10秒。

进一步的，所述步骤(2)、(3)、(4)中取芯器在钻孔处及钻进路径末端的驻留时长分别为20秒。

进一步的，所述步骤(2)中采用高压液推的方式将取芯器沿着钻进路径推进。

进一步的，所述步骤(2.3)中采用序贯更新的方式完成量测更新。

进一步的，所述步骤(1.1)、(1.2)中的陀螺漂移、加速度计零偏状态采用反馈校正。

进一步的，所述步骤(1.1)、(1.2)中的安装误差状态采用开环校正。

进一步的，所述步骤(1)中惯性测量单元测量角增量信息和速度增量信息时其采样间隔不大于0.01s。

与现有技术相比，本发明的优点在于：

(1)本发明结合长距离水平取芯钻机的工作特点，合理规划取芯器的工作模式，充分利用定位定向仪保存的全部测量数据，并对钻探管道接缝进行运动检测，以此实现行进速度的检测，提高数据利用率，基于完整约束卡尔曼滤波器对长距离水平取芯钻机进行孔内精确定位；

(2)本发明充分利用正向、逆向自主导航定位误差特性的差异互补性，以加权定位输出结果作为钻进路径轨迹，提高定位精度。

附图说明

图1为本发明方法的流程示意图；

图2为本发明实施过程示意图；

图3为本发明中行进速度自检测示意图；

图4为本发明正逆向解算示意图；

图5为本发明正逆向自主导航定位示意图。

具体实施方式

以下将结合说明书附图和具体实施例对本发明作进一步详细说明。

如图1所示，结合长距离水平取芯钻机的工作特点，合理规划取芯器的工作模式，充分利用定位定向仪保存的全部测量数据，同时利用定位定向仪加装的前端加速度计、尾端加速度计分别对钻探管道接缝进行运动检测，根据定位定向仪的长度信息及两只加速度计检测到接缝的时间差，实现行进速度自检测，最后基于完整约束卡尔曼滤波器实现长距离水平取芯钻机的孔内精确定位，进而提高钻进路径的控制精度，满足高原高寒地区地质勘探的需求。

如图2所示，结合具体应用实例，对本发明的具体流程进行说明：

基于行进速度自检测的钻探用定位定向仪孔内定位方法，包括以下步骤：

(1)在定位定向仪的前端及尾端分别加装加速度计，并精确测量前端加速度计、尾端加速度计之间的长度距离L，进而将定位定向仪安装到取芯器上，并放置于取芯钻机载车的基座上，向定位定向仪装订初始位置信息和初始速度信息，装订完成后定位定向仪静止15～25分钟，并进行初始自对准，获得初始姿态信息，定位定向仪要在线保存惯性测量单元测量得到的角增量信息和速度增量信息；定位定向仪完成初始自对准后，基于完整约束卡尔曼滤波器进行正向自主导航定位；其中，完整约束卡尔曼滤波器按照如下步骤设计：

(1.1)以姿态误差φⁿ、速度误差δvⁿ、位置误差δpⁿ、陀螺漂移ε^b、加速度计零偏

定位定向仪惯性测量单元的安装误差η为系统状态x(t)，分别确定姿态误差、速度误差、陀螺漂移、加速度计零偏、安装误差的微分方程如下：

其中，

表示导航坐标系n相对于惯性坐标系i的旋转角速度，

表示载体坐标系b与导航坐标系n之间的姿态矩阵，fⁿ表示导航坐标系下表示的比力，

表示地球自转角速度，

表示转移角速度，vⁿ表示速度，

分别表示导航坐标系旋转角速度误差量、地球自转角速度误差量、转移角速度误差量，

表示陀螺组件测量误差，

表示加速度计组件测量误差，w_g、w_a分别表示陀螺组件测量噪声、加速度计组件测量噪声，η＝[η_θ η_Ψ]^T由俯仰角安装误差η_θ及航向角安装误差η_Ψ构成；

(1.2)根据步骤(1.1)中确定的姿态误差、速度误差、位置误差、陀螺漂移、加速度计零偏、安装误差微分方程，构建系统状态方程如下：

其中，

表示系统状态矩阵；

式中，v_E、v_N、v_U分别表示东向、北向、垂向速度，L表示当地纬度，h表示当地高度，R_E、R_N分别表示卯酉圈半径、子午圈半径，ω_ie表示地球自转角速度模值；

表示系统噪声矩阵；

w(t)＝[w_g w_a]^T表示系统噪声；

(1.3)取芯器沿钻进路径推进时，其侧向速度及垂向速度为零，并且当取芯器推进至钻探管道接缝处时，接缝会导致振动加速度，分别记录保存前向加速度计检测到的振动时刻t₁及尾端加速度计检测到的振动时刻t₂，则取芯器的前向速度v_f为L/(t₂-t₁)，如图3所示，进而以前向速度误差δv_y、侧向速度误差δv_x及垂向速度误差δv_z构建完整约束观测量z(t)＝[δv_x δv_y δv_z]^T，并确定观测方程，其中，观测方程的确定通过以下步骤实现：

(1.3.1)将定位定向仪速度输出投影到惯性测量单元坐标系m，按照如下方式：

其中，

表示定位定向仪速度输出在惯性测量单元坐标系m下的投影，v^m表示真实的m坐标系下表示的定位定向仪速度，

表示载体坐标系b与惯性测量单元坐标系m之间的安装关系矩阵，ζ＝[η_θ η_γ η_Ψ]^T表示安装误差角，

表示导航坐标系n与载体坐标系b之间的姿态矩阵；由于横滚角安装误差η_γ不会影响前向速度投影，将其赋值为0，即η_γ＝0；

(1.3.2)以

作为完整约束观测量z(t)，构建观测方程如下：

z(t)＝H(t)x(t)+υ(t) (4)

其中，

且

υ(t)表示观测噪声；

(2)由取芯钻机载车将取芯器推送至钻孔处并驻留，取芯器在钻孔处驻留10～20秒钟，并完成定位定向仪的第一次零速修正，然后采用高压气推的方式将取芯器沿着钻进路径推进，定位定向仪在线保存惯性测量单元测量得到的角增量信息和速度增量信息，其中，零速修正按照如下步骤进行：

(2.1)定位定向仪根据角速度、速度信息自主检测到其零速状态，零速状态下的观测量为

(2.2)以

的x、y、z分量作为零速状态下的观测量z_ZUPT(t)，构建观测方程如下：

z_ZUPT(t)＝H_ZUPT(t)x(t)+μ(t) (5)

其中，

μ(t)表示观测噪声；

(2.3)根据步骤(2.1)、(2.2)所述对零速状态下的定位定向仪完成量测更新；

(3)当取芯器推进至钻进路径末端的钻机处，由取芯器完成取芯作业，然后取芯器在钻进路径末端再次驻留10～20秒钟，并完成定位定向仪的第二次零速修正；此外，定位定向仪要在线保存惯性测量单元测量得到的角增量信息和速度增量信息；

(4)第二次零速修正完成后，通过卷扬机将取芯器从孔内拖出，当取芯器被拖至钻孔处时再次驻留10～20秒钟，并完成定位定向仪的第三次零速修正；第三次零速修正完成后，将取芯器拖动至取芯钻机载车的基座上，再次静止15～25分钟；此外，定位定向仪要在线保存惯性测量单元测量得到的角增量信息和速度增量信息；

(5)如图4、图5所示，将陀螺组件测量角速度

加速度计组件测量比力f^b及地球自转角速度

取反，并按照从后至前的时间顺序反转步骤(1)-(4)中保存的角增量信息、速度增量信息，构成逆向测量数据序列，前向速度信息v_f只进行时间上的逆序处理、不取反，最后基于完整约束卡尔曼滤波器对逆向测量数据序列进行逆向自主导航定位，其中，逆向导航解算如下式所述：

其中，

分别表示逆向时序d、d-1时刻的姿态矩阵，

分别表示逆向时序d、d-1时刻的速度，

分别表示逆向时序d、d-1时刻的位置，gⁿ、

分别表示正向解算、逆向解算当地重力加速度，I₃表示三阶单位矩阵，ΔT表示采样间隔；

逆向自主导航定位包括如下步骤：

(5.1)利用逆向测量数据的第一个15～25分钟的静态阶段完成初始对准，获得初始姿态信息；

(5.2)按照步骤(1.1)-(1.3)所述的基于完整约束卡尔曼滤波器进行逆向滤波，姿态误差方程、速度误差方程、位置误差方程、陀螺漂移、加速度零偏及定位定向仪惯性测量单元的安装误差的微分方程保持不变，观测方程也保持不变；

(5.3)逆向自主导航定位过程中，当取芯器处于驻留状态时，按照步骤(2.1)-(2.3)所述零速修正方式进行误差校正；

(6)以时间对齐后的正向自主导航定位结果pⁿ与逆向自主导航定位结果

的加权结果作为定位输出，加权方式采用下式所述：

其中，p^s为加权结果，P_s、P_f、P_b分别为平滑方差、正向方差、逆向方差；

最后将取芯器推进段的定位结果作为钻进路径轨迹，并根据钻进路径轨迹计算其相对于设计路径的偏差，进而对钻进路径进行调整。

进一步的，所述步骤(1)、(4)中取芯器在取芯钻机载车基座上的静止时长分别为15分钟。

进一步的，所述步骤(1)、(4)中取芯器在取芯钻机载车基座上的静止时长分别为25分钟。

进一步的，所述步骤(2)、(3)、(4)中取芯器在钻孔处及钻进路径末端的驻留时长分别为10秒。

进一步的，所述步骤(2)、(3)、(4)中取芯器在钻孔处及钻进路径末端的驻留时长分别为20秒。

进一步的，所述步骤(2)中采用高压液推的方式将取芯器沿着钻进路径推进。

进一步的，所述步骤(2.3)中采用序贯更新的方式完成量测更新。

进一步的，所述步骤(1.1)、(1.2)中的陀螺漂移、加速度计零偏状态采用反馈校正。

进一步的，所述步骤(1.1)、(1.2)中的安装误差状态采用开环校正。

进一步的，所述步骤(1)中惯性测量单元测量角增量信息和速度增量信息时其采样间隔不大于0.01s。

以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.基于行进速度自检测的钻探用定位定向仪孔内定位方法，其特征在于，包括以下步骤：

(1)在定位定向仪的前端及尾端分别加装加速度计，并精确测量前端加速度计、尾端加速度计之间的长度距离L，进而将定位定向仪安装到取芯器上，并放置于取芯钻机载车的基座上，向定位定向仪装订初始位置信息和初始速度信息，装订完成后定位定向仪静止15～25分钟，并进行初始自对准，获得初始姿态信息，定位定向仪要在线保存惯性测量单元测量得到的角增量信息和速度增量信息；定位定向仪完成初始自对准后，基于完整约束卡尔曼滤波器进行正向自主导航定位；其中，完整约束卡尔曼滤波器按照如下步骤设计：

(1.1)以姿态误差φⁿ、速度误差δvⁿ、位置误差δpⁿ、陀螺漂移ε^b、加速度计零偏

定位定向仪惯性测量单元的安装误差η为系统状态x(t)，分别确定姿态误差、速度误差、陀螺漂移、加速度计零偏、安装误差的微分方程如下：

其中，

表示导航坐标系n相对于惯性坐标系i的旋转角速度，

表示载体坐标系b与导航坐标系n之间的姿态矩阵，fⁿ表示导航坐标系下表示的比力，

表示地球自转角速度，

表示转移角速度，vⁿ表示速度，

分别表示导航坐标系旋转角速度误差量、地球自转角速度误差量、转移角速度误差量，

表示陀螺组件测量误差，

表示加速度计组件测量误差，w_g、w_a分别表示陀螺组件测量噪声、加速度计组件测量噪声，η＝[η_θ η_Ψ]^T由俯仰角安装误差η_θ及航向角安装误差η_Ψ构成；

(1.2)根据步骤(1.1)中确定的姿态误差、速度误差、位置误差、陀螺漂移、加速度计零偏、安装误差微分方程，构建系统状态方程如下：

其中，

表示系统状态矩阵；

F₅＝[fⁿ×]，

式中，v_E、v_N、v_U分别表示东向、北向、垂向速度，L表示当地纬度，h表示当地高度，R_E、R_N分别表示卯酉圈半径、子午圈半径，ω_ie表示地球自转角速度模值；

表示系统噪声矩阵；

w(t)＝[w_g w_a]^T表示系统噪声；

(1.3)取芯器沿钻进路径推进时，其侧向速度及垂向速度为零，并且当取芯器推进至钻探管道接缝处时，接缝会导致振动加速度，分别记录保存前向加速度计检测到的振动时刻t₁及尾端加速度计检测到的振动时刻t₂，则取芯器的前向速度v_f为L/(t₂-t₁)，进而以前向速度误差δv_y、侧向速度误差δv_x及垂向速度误差δv_z构建完整约束观测量z(t)＝[δv_x δv_y δv_z]^T，并确定观测方程，其中，观测方程的确定通过以下步骤实现：

(1.3.1)将定位定向仪速度输出投影到惯性测量单元坐标系m，按照如下方式：

其中，

表示定位定向仪速度输出在惯性测量单元坐标系m下的投影，v^m表示真实的m坐标系下表示的定位定向仪速度，

表示载体坐标系b与惯性测量单元坐标系m之间的安装关系矩阵，ζ＝[η_θ η_γ η_Ψ]^T表示安装误差角，

表示导航坐标系n与载体坐标系b之间的姿态矩阵；由于横滚角安装误差η_γ不会影响前向速度投影，将其赋值为0，即η_γ＝0；

(1.3.2)以

作为完整约束观测量z(t)，构建观测方程如下：

z(t)＝H(t)x(t)+υ(t) (4)

其中，

且

υ(t)表示观测噪声；

(2)由取芯钻机载车将取芯器推送至钻孔处并驻留，取芯器在钻孔处驻留10～20秒钟，并完成定位定向仪的第一次零速修正，然后采用高压气推的方式将取芯器沿着钻进路径推进，定位定向仪在线保存惯性测量单元测量得到的角增量信息和速度增量信息，其中，零速修正按照如下步骤进行：

(2.1)定位定向仪根据角速度、速度信息自主检测到其零速状态，零速状态下的观测量为

(2.2)以

的x、y、z分量作为零速状态下的观测量z_ZUPT(t)，构建观测方程如下：

z_ZUPT(t)＝H_ZUPT(t)x(t)+μ(t) (5)

其中，

μ(t)表示观测噪声；

(2.3)根据步骤(2.1)、(2.2)所述对零速状态下的定位定向仪完成量测更新；

(3)当取芯器推进至钻进路径末端的钻机处，由取芯器完成取芯作业，然后取芯器在钻进路径末端再次驻留10～20秒钟，并完成定位定向仪的第二次零速修正；此外，定位定向仪要在线保存惯性测量单元测量得到的角增量信息和速度增量信息；

(4)第二次零速修正完成后，通过卷扬机将取芯器从孔内拖出，当取芯器被拖至钻孔处时再次驻留10～20秒钟，并完成定位定向仪的第三次零速修正；第三次零速修正完成后，将取芯器拖动至取芯钻机载车的基座上，再次静止15～25分钟；此外，定位定向仪要在线保存惯性测量单元测量得到的角增量信息和速度增量信息；

(5)将陀螺组件测量角速度

加速度计组件测量比力f^b及地球自转角速度

取反，并按照从后至前的时间顺序反转步骤(1)-(4)中保存的角增量信息、速度增量信息，构成逆向测量数据序列，前向速度信息v_f只进行时间上的逆序处理、不取反，最后基于完整约束卡尔曼滤波器对逆向测量数据序列进行逆向自主导航定位，其中，逆向导航解算如下式所述：

其中，

分别表示逆向时序d、d-1时刻的姿态矩阵，

分别表示逆向时序d、d-1时刻的速度，

分别表示逆向时序d、d-1时刻的位置，gⁿ、

分别表示正向解算、逆向解算当地重力加速度，I₃表示三阶单位矩阵，ΔT表示采样间隔；

逆向自主导航定位包括如下步骤：

(5.1)利用逆向测量数据的第一个15～25分钟的静态阶段完成初始对准，获得初始姿态信息；

(5.2)按照步骤(1.1)-(1.3)所述的基于完整约束卡尔曼滤波器进行逆向滤波，姿态误差方程、速度误差方程、位置误差方程、陀螺漂移、加速度零偏及定位定向仪惯性测量单元的安装误差的微分方程保持不变，观测方程也保持不变；

(5.3)逆向自主导航定位过程中，当取芯器处于驻留状态时，按照步骤(2.1)-(2.3)所述零速修正方式进行误差校正；

(6)以时间对齐后的正向自主导航定位结果pⁿ与逆向自主导航定位结果

的加权结果作为定位输出，加权方式采用下式所述：

其中，p^s为加权结果，P_s、P_f、P_b分别为平滑方差、正向方差、逆向方差；

最后将取芯器推进段的定位结果作为钻进路径轨迹，并根据钻进路径轨迹计算其相对于设计路径的偏差，进而对钻进路径进行调整。

2.如权利要求1所述的基于行进速度自检测的钻探用定位定向仪孔内定位方法，其特征在于，所述步骤(1)、(4)中取芯器在取芯钻机载车基座上的静止时长分别为15分钟。

3.如权利要求1所述的基于行进速度自检测的钻探用定位定向仪孔内定位方法，其特征在于，所述步骤(1)、(4)中取芯器在取芯钻机载车基座上的静止时长分别为25分钟。

4.如权利要求1所述的基于行进速度自检测的钻探用定位定向仪孔内定位方法，其特征在于，所述步骤(2)、(3)、(4)中取芯器在钻孔处及钻进路径末端的驻留时长分别为10秒。

5.如权利要求1所述的基于行进速度自检测的钻探用定位定向仪孔内定位方法，其特征在于，所述步骤(2)、(3)、(4)中取芯器在钻孔处及钻进路径末端的驻留时长分别为20秒。

6.如权利要求1所述的基于行进速度自检测的钻探用定位定向仪孔内定位方法，其特征在于，所述步骤(2)中采用高压液推的方式将取芯器沿着钻进路径推进。

7.如权利要求1所述的基于行进速度自检测的钻探用定位定向仪孔内定位方法，其特征在于，所述步骤(2.3)中采用序贯更新的方式完成量测更新。

8.如权利要求1所述的基于行进速度自检测的钻探用定位定向仪孔内定位方法，其特征在于，所述步骤(1.1)、(1.2)中的陀螺漂移、加速度计零偏状态采用反馈校正。

9.如权利要求1所述的基于行进速度自检测的钻探用定位定向仪孔内定位方法，其特征在于，所述步骤(1.1)、(1.2)中的安装误差状态采用开环校正。

10.如权利要求1所述的基于行进速度自检测的钻探用定位定向仪孔内定位方法，其特征在于，所述步骤(1)中惯性测量单元测量角增量信息和速度增量信息时其采样间隔不大于0.01s。

Images