[发明专利]基于速度信息辅助的钻探用定位定向仪孔内定位方法

权利要求书

1.基于速度信息辅助的钻探用定位定向仪孔内定位方法，其特征在于，包括以下步骤：

(1)将定位定向仪安装到取芯器上，并放置于取芯钻机载车的基座上，并在卷扬机上加装光电编码器，进一步向定位定向仪装订初始位置信息和初始速度信息，装订完成后定位定向仪静止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)取芯器沿钻进路径推进时，其侧向速度及垂向速度为零，以前向速度误差δ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)

其中，且v_f的数值等于光电编码器输出的位置增量信息除以采样时间，且υ(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)中保存的角增量信息、速度增量信息、光电编码器的位置增量信息，构成逆向测量数据序列，光电编码器的位置增量信息只进行时间上的逆序处理、不取反，最后基于速度信息辅助的完整约束卡尔曼滤波器对逆向测量数据序列进行逆向自主导航定位，其中，逆向导航解算如下式所述：

其中，

分别表示逆向时序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分别为平滑方差、正向方差、逆向方差；

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