CN112781588B - 一种随钻陀螺定位定向仪导航解算方法 - Google Patents

一种随钻陀螺定位定向仪导航解算方法 Download PDF

Info

Publication number
CN112781588B
CN112781588B CN202011633460.XA CN202011633460A CN112781588B CN 112781588 B CN112781588 B CN 112781588B CN 202011633460 A CN202011633460 A CN 202011633460A CN 112781588 B CN112781588 B CN 112781588B
Authority
CN
China
Prior art keywords
speed
drilling
navigation
attitude
drill rod
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011633460.XA
Other languages
English (en)
Other versions
CN112781588A (zh
Inventor
杨长浩
闻泽阳
张江源
陈天宇
刘宇尘
张文龙
邢金龙
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xiamen Huayuan Jiahang Technology Co ltd
Original Assignee
Xiamen Huayuan Jiahang Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Xiamen Huayuan Jiahang Technology Co ltd filed Critical Xiamen Huayuan Jiahang Technology Co ltd
Priority to CN202011633460.XA priority Critical patent/CN112781588B/zh
Publication of CN112781588A publication Critical patent/CN112781588A/zh
Application granted granted Critical
Publication of CN112781588B publication Critical patent/CN112781588B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)
  • Excavating Of Shafts Or Tunnels (AREA)

Abstract

本发明公开了一种随钻陀螺定位定向仪导航解算方法，属于导航应用技术领域。本发明包括以下步骤：1、初始对准获取钻杆钻进起始时刻的姿态角；2、钻进过程中进行姿态、速度、位置解算获取实时的导航信息；3、利用运动学约束的方式进行钻进过程中的修正；4、当一根钻杆钻进完成时，获取当前的姿态角；5、利用最小曲率法求解此时位置；6、以最小曲率法获取的位置信息作为滤波观测量进行滤波修正。本发明解决了地下GPS拒止情况下，纯惯性导航应用于随钻设计中误差发散的问题，在不需要额外传感器的前提下，提高了导航算法的定位精度。

Description

一种随钻陀螺定位定向仪导航解算方法
技术领域
本发明属于导航技术领域，具体涉及一种随钻陀螺定位定向仪导航解算方法。
背景技术
随着社会的进步和现代工业的发展，钻探领域如石油开采，地下钻井，地下管道铺设等越来越受到社会的重视。在钻探领域内受到广泛使用的是基于磁通门和三轴加表的定位定向仪来实现地下钻探的定位，但容易受到磁场的干扰，定位精度得到限制。需要一种新的方式来实现地下的定位导航。
捷联惯性导航技术的不断成熟，为钻探地下定位导航提供了新的解决方法。基于陀螺的定位定向仪相比与基于磁通门的定位定向仪，具有更高的精度和更加稳定的性能，能够很好的指导钻探工作。
然而纯惯性导航解算的误差会随着时间不断积累而发散，往往都需要使用其他方法来实现对误差的抑制。但钻探过程中，地下存在GPS拒止问题，无法利用卫导对导航误差进行修正。
发明内容
鉴于现有技术的不足，本发明的目的在于提供一种随钻陀螺定位定向仪导航解算方法，实现在钻进过程中导航信息的计算和误差的抑制以及在钻进停止后对误差的进一步修正和补偿，在GPS拒止条件下提高地下定位精度。
为实现上述目的，本发明采用如下技术方案：一种随钻陀螺定位定向仪导航解算方法，包括以下步骤：
步骤1、初始对准获取钻杆钻进起始时刻的姿态角；
步骤2、钻进过程中进行姿态、速度、位置更新获取实时的导航信息；
步骤3、利用运动学约束的方式进行钻进过程中的修正；
步骤4、当一根钻杆钻进完成时，获取当前的姿态角；
步骤5、利用最小曲率法求解此时位置；
步骤6、以最小曲率法获取的位置信息作为滤波观测量进行滤波修正。
优选地，所述标定方法采用的坐标系定义如下：
b系：钻杆载体坐标系oxbybzb，y轴指钻杆钻进方向，z轴沿钻杆横截面半径向外，x轴与y轴、z轴构成满足右手定则的笛卡尔直角坐标系；
n系：导航坐标系oxbybzb，采用“东-北-天”地理坐标系，x轴指东，y轴指北，z轴指天。
优选地，所述步骤2中，钻进过程中进行姿态、速度、位置更新获取实时的导航信息的方法如下：
在钻进过程中进行姿态、速度、位置解算的方法是采用捷联更新算法实现的；姿态更新微分方程为：
Figure BDA00028806311100000219
其中，
Figure BDA0002880631110000021
为钻杆载体坐标系(b系)相对于导航坐标系(n系)的姿态阵，
Figure BDA0002880631110000022
为导航坐标系(n系)相对钻杆载体坐标系(b系)的旋转角速度；
类比与姿态更新微分方程可得：
Figure BDA0002880631110000023
Figure BDA0002880631110000024
根据
Figure BDA0002880631110000025
其中，矩阵
Figure BDA0002880631110000026
表示以i系作为参考基准，b系从tm-1时刻到tm时刻的旋转变化，
Figure BDA0002880631110000027
由陀螺角速度
Figure BDA0002880631110000028
确定；
Figure BDA0002880631110000029
表示以i系作为参考基准，n系从tm时刻到tm-1时刻的旋转变化，
Figure BDA00028806311100000210
由计算角速度
Figure BDA00028806311100000211
确定，
Figure BDA00028806311100000212
可得：
Figure BDA00028806311100000213
通过初始姿态矩阵，结合陀螺采集的角速度信息，根据上式即可解算下一时刻的姿态信息；
速度更新微分方程为：
Figure BDA00028806311100000214
其中，
Figure BDA00028806311100000215
为载体的速度、
Figure BDA00028806311100000216
为加表测量值、
Figure BDA00028806311100000217
为地球转速、
Figure BDA00028806311100000218
为载体运动引起的旋转角速度、gn为重力；
速度更新通过比力方程的一次积分来实现；加表测量的加速度信息中包含了载体自身运动的加速度信息、重力加速度信息、载体运动所导致的哥式加速度信息和对地向心加速度信息；根据比力方程，加表测量的加速度信息在考虑了包含重力加速度、哥式加速度、对地向心加速度在内的有害加速度信息后，积分即可得到载体运动的速度变化量：
Figure BDA0002880631110000031
Figure BDA0002880631110000032
其中，上标m-1表示上一时刻，m表示下一时刻，T为采样周期；
位置更新微分方程为：
Figure BDA0002880631110000033
其中，L为纬度、λ为经度、h为高度、RM、RN为地球参数；
对速度的一次积分，即可得到位置变化量，可求解位置：
Figure BDA0002880631110000034
Figure BDA0002880631110000035
Figure BDA0002880631110000036
优选地，所述步骤3中，利用运动学约束的方式进行钻进过程中的修正的方法如下：
根据惯性导航的误差模型：
Figure BDA0002880631110000037
Figure BDA0002880631110000038
Figure BDA0002880631110000039
Figure BDA00028806311100000310
Figure BDA00028806311100000311
可知，惯性导航的姿态速度及位置误差是各误差相互耦合的结果，误差方程具有一定规律；考虑到钻进过程比较缓慢，近似为一种静基座的情况；惯导的误差传播方程为一个线性定常系统，根据误差模型建立卡尔曼滤波方程进行修正；钻杆钻进过程中，利用钻杆载体坐标系中x和z轴方向速度为0，做速度约束；
卡尔曼滤波采用15维状态量：
Figure BDA00028806311100000312
其中，φn为失准角、δVn为速度误差、δPn为位置误差、ε为陀螺零偏、
Figure BDA00028806311100000313
为加表零偏；滤波器的状态方程为：
Figure BDA00028806311100000314
其中，u为系统噪声，F矩阵为：
Figure BDA0002880631110000041
F矩阵中有：
Figure BDA0002880631110000042
Figure BDA0002880631110000043
Figure BDA0002880631110000044
Figure BDA0002880631110000045
Figure BDA0002880631110000046
设惯导系统输出的导航坐标系下速度为Vn，载体坐标系下速度为Vb，则：
Figure BDA0002880631110000051
利用钻杆载体坐标系x轴与z轴速度为0作为约束条件，则量测值为：
Figure BDA0002880631110000052
量测方程为：
Z＝HX+V
其中，V为量测噪声，H矩阵为：
Figure BDA0002880631110000053
其中：
Figure BDA0002880631110000054
H15＝Vb×(1，3)
Figure BDA0002880631110000055
H24＝Vb×(3，1)。
优选地，所述步骤5中，利用最小曲率法求解此时位置的方法如下：
最小曲率法基本假设在测段为空间斜平面上的一段圆弧曲线，它用包含作为测点的圆弧井段两个端点的井斜角、位角以及圆弧弯曲角在内的角度参数来计算测段的坐标增量；
钻进工作过程中，每一个钻杆的长度是精确已知的，最小曲率法利用姿态信息和两个测点之间的轨迹长度来计算坐标的增量；最小曲率法的计算公式为：
doleg＝arccos(cos(I1-I2))-sinI1sinI2(1-cos(A2-A1))
RF＝2tan(doleg/2)/doleg
ΔTVD＝1/2·ΔMD(cosI1+cosI2)RF
ΔN＝1/2·ΔMD(sinI1cosA1+sinI2cosA2)RF
ΔE＝1/2·ΔMD(sinI1sinA1+sinI2sinA2)RF
其中，doleg称狗腿度，即全角变化率，其反应了井斜角度的变化和方位角度的变化；RF为比率因数；ΔMD为钻杆长度；I1,I2为两测点的倾角，倾角由姿态角中的俯仰角计算求解，即倾角＝俯仰角+90°；A1,A2为两测点的方位角；ΔTVD,ΔE,ΔN分别为高度变化量，东向变化量，北向变化量；
由此可得：
doleg＝arccos(cos(θ1-θ2))-cosθ1cosθ2(1-cos(ψ2-ψ1))
RF＝2tan(doleg/2)/doleg
h2≈h1+1/2·ΔMD(sinθ1+sinθ2)RF
L2≈L1-1/2·ΔMD(cosθ1cosψ1+cosθ2cosψ2)RF/Re
λ2≈λ1-1/2·ΔMD(cosθ1sinψ1+cosθ2sinψ2)RF/Re/cosL
其中，θ为俯仰姿态角，ψ为方位姿态角，Re为地球半径，角标2表示下一位置，角标1表示上一位置；当已知第一个测点的位置和姿态时，通过钻杆长度和下一测点的姿态便可求解出坐标变化量，从而获取当前的位置信息。
优选地，所述步骤6中，以最小曲率法获取的位置信息作为滤波观测量进行滤波修正的方法如下：
通过捷联惯性导航解算方法获取位置信息：
Figure BDA0002880631110000061
Figure BDA0002880631110000062
Figure BDA0002880631110000063
其中：L为纬度，λ为经度，h为高度，Re为地球半径，f为地球椭圆扁率，vn为速度；通过捷联导航算法中的位置更新，获得位置信息；利用最小曲率法获得的位置信息对捷联导航算法中的姿态、速度、位置进行反馈修正；设计的滤波器如下：
卡尔曼滤波采用15维状态量：
Figure BDA0002880631110000064
滤波器的状态方程为：
Figure BDA0002880631110000065
其中，u为系统噪声，F矩阵为：
Figure BDA0002880631110000066
量测方程为：
Z＝HX+V
其中，V为量测噪声，量测值与量测矩阵如下：
Z＝[Pins-Pmincrv]
H＝[O3×6 I3×3 O3×6]
其中，Pins为捷联更新算法解算出的位置，Pmincrv为最小曲率法求解出的位置。
与现有技术相比，本发明具有以下有益效果：在地下GPS拒止情况下，利用运动学约束和最小曲率法实现钻探过程中和钻探停止后的导航误差的抑制，解决了地下GPS拒止条件下无法对导航误差进行抑制的问题；在钻探过程中进行导航解算和误差抑制、钻探停止后对导航误差进行修正，提高了地下钻探的定位定向的精度。
附图说明
图1为本发明实施例的随钻陀螺定位定向仪导航解算方法的总体流程图。
具体实施方式
为了让本发明的上述特征和优点更明显易懂，下面特举实施例，并配合附图，作详细说明如下。
如图1所示，本实施例提供了一种随钻陀螺定位定向仪导航解算方法，包括以下步骤：
步骤1、初始对准获取钻杆钻进起始时刻的姿态角；
步骤2、钻进过程中进行姿态、速度、位置更新获取实时的导航信息；
步骤3、利用运动学约束的方式进行钻进过程中的修正；
步骤4、当一根钻杆钻进完成时，获取当前的姿态角；
步骤5、利用最小曲率法求解此时位置；
步骤6、以最小曲率法获取的位置信息作为滤波观测量进行滤波修正。
在本实施例中，所述标定方法采用的坐标系定义如下：
b系：钻杆载体坐标系oxbybzb，y轴指钻杆钻进方向，z轴沿钻杆横截面半径向外，x轴与y轴、z轴构成满足右手定则的笛卡尔直角坐标系；
n系：导航坐标系oxbybzb，采用“东-北-天”地理坐标系，x轴指东，y轴指北，z轴指天。
在本实施例中，所述步骤2中，钻进过程中进行姿态、速度、位置更新获取实时的导航信息的方法如下：
在钻进过程中进行姿态、速度、位置解算的方法是采用捷联更新算法实现的；姿态更新微分方程为：
Figure BDA0002880631110000071
其中，
Figure BDA0002880631110000081
为钻杆载体坐标系(b系)相对于导航坐标系(n系)的姿态阵，
Figure BDA0002880631110000082
为导航坐标系(n系)相对钻杆载体坐标系(b系)的旋转角速度；
类比与姿态更新微分方程可得：
Figure BDA0002880631110000083
Figure BDA0002880631110000084
根据
Figure BDA0002880631110000085
其中，矩阵
Figure BDA0002880631110000086
表示以i系作为参考基准，b系从tm-1时刻到tm时刻的旋转变化，
Figure BDA0002880631110000087
可由陀螺角速度
Figure BDA0002880631110000088
确定；
Figure BDA0002880631110000089
表示以i系作为参考基准，n系从tm时刻到tm-1时刻的旋转变化，
Figure BDA00028806311100000810
可由计算角速度
Figure BDA00028806311100000811
确定，
Figure BDA00028806311100000812
可得：
Figure BDA00028806311100000813
通过初始姿态矩阵，结合陀螺采集的角速度信息，根据上式即可解算下一时刻的姿态信息；
速度更新微分方程为：
Figure BDA00028806311100000814
其中，
Figure BDA00028806311100000815
为载体的速度、
Figure BDA00028806311100000816
为加表测量值、
Figure BDA00028806311100000817
为地球转速、
Figure BDA00028806311100000818
为载体运动引起的旋转角速度、gn为重力；
速度更新通过比力方程的一次积分来实现；加表测量的加速度信息中包含了载体自身运动的加速度信息、重力加速度信息、载体运动所导致的哥式加速度信息和对地向心加速度信息等；根据比力方程，加表测量的加速度信息在考虑了包含重力加速度、哥式加速度、对地向心加速度在内的有害加速度信息后，积分即可得到载体运动的速度变化量：
Figure BDA00028806311100000819
Figure BDA00028806311100000820
其中，上标m-1表示上一时刻，m表示下一时刻，T为采样周期；
位置更新微分方程为：
Figure BDA00028806311100000821
其中，L为纬度、λ为经度、h为高度、RM、RN为地球参数；
对速度的一次积分，即可得到位置变化量，可求解位置：
Figure BDA0002880631110000091
Figure BDA0002880631110000092
Figure BDA0002880631110000093
在本实施例中，所述步骤3中，利用运动学约束的方式进行钻进过程中的修正的方法如下：
根据惯性导航的误差模型：
Figure BDA0002880631110000094
Figure BDA0002880631110000095
Figure BDA0002880631110000096
Figure BDA0002880631110000097
Figure BDA0002880631110000098
可知，惯性导航的姿态速度及位置误差是各误差相互耦合的结果，误差方程具有一定规律；考虑到钻进过程比较缓慢，可以近似为一种静基座的情况；惯导的误差传播方程为一个线性定常系统，可根据误差模型建立卡尔曼滤波方程进行修正；钻杆钻进过程中，利用钻杆载体坐标系中x和z轴方向速度为0，做速度约束；
卡尔曼滤波采用15维状态量：
Figure BDA0002880631110000099
其中，φn为失准角、δVn为速度误差、δPn为位置误差、ε为陀螺零偏、
Figure BDA00028806311100000910
为加表零偏；滤波器的状态方程为：
Figure BDA00028806311100000911
其中，u为系统噪声，F矩阵为：
Figure BDA00028806311100000912
F矩阵中有：
Figure BDA0002880631110000101
Figure BDA0002880631110000102
Figure BDA0002880631110000103
Figure BDA0002880631110000104
Figure BDA0002880631110000105
设惯导系统输出的导航坐标系下速度为Vn，载体坐标系下速度为Vb，则：
Figure BDA0002880631110000106
利用钻杆载体坐标系x轴与z轴速度为0作为约束条件，则量测值为：
Figure BDA0002880631110000111
量测方程为：
Z＝HX+V
其中，V为量测噪声，H矩阵为：
Figure BDA0002880631110000112
其中：
Figure BDA0002880631110000113
H15＝Vb×(1，3)
Figure BDA0002880631110000114
H24＝Vb×(3，1)。
钻进过程中利用惯性导航方法进行姿态、速度、位置解算；其误差会随着时间的积累不断发散，利用速度约束方法，在钻进过程中，实时地用钻杆载体坐标系x轴与z轴方向的速度为0作为滤波量测值，进行卡尔曼滤波反馈修正，实现对导航解算误差抑制。
在本实施例中，所述步骤5中，利用最小曲率法求解此时位置的方法如下：
最小曲率法基本假设在测段为空间斜平面上的一段圆弧曲线，它用包含作为测点的圆弧井段两个端点的井斜角、位角以及圆弧弯曲角在内的角度参数来计算测段的坐标增量；
钻进工作过程中，一根钻杆的长度大概在10m左右，由于钻进的特殊性，每当一根钻杆钻入地下后，需要停止钻进，更换钻杆进行下一次的钻进；这种特殊的工作模式无形之中将整个钻进轨迹分段，这为最小曲率法的设计和实现提供了基础；每一个钻杆的长度是精确已知的，最小曲率法利用姿态信息和两个测点之间的轨迹长度来计算坐标的增量；最小曲率法的计算公式为：
doleg＝arccos(cos(I1-I2))-sinI1sinI2(1-cos(A2-A1))
RF＝2tan(doleg/2)/doleg
ΔTVD＝1/2·ΔMD(cosI1+cosI2)RF
ΔN＝1/2·ΔMD(sinI1cosA1+sinI2cosA2)RF
ΔE＝1/2·ΔMD(sinI1sinA1+sinI2sinA2)RF
其中，doleg称狗腿度，即全角变化率，其反应了井斜角度的变化和方位角度的变化；RF为比率因数；ΔMD为钻杆长度；I1,I2为两测点的倾角，倾角由姿态角中的俯仰角计算求解，即倾角＝俯仰角+90°；A1,A2为两测点的方位角；ΔTVD,ΔE,ΔN分别为高度变化量，东向变化量，北向变化量；
由此可得：
doleg＝arccos(cos(θ1-θ2))-cosθ1cosθ2(1-cos(ψ2-ψ1))
RF＝2tan(doleg/2)/doleg
h2≈h1+1/2·ΔMD(sinθ1+sinθ2)RF
L2≈L1-1/2·ΔMD(cosθ1cosψ1+cosθ2cosψ2)RF/Re
λ2≈λ1-1/2·ΔMD(cosθ1sinψ1+cosθ2sinψ2)RF/Re/cosL
其中，θ为俯仰姿态角，ψ为方位姿态角，Re为地球半径，角标2表示下一位置，角标1表示上一位置；当已知第一个测点的位置和姿态时，通过钻杆长度和下一测点的姿态便可求解出坐标变化量，从而获取当前的位置信息。
在本实施例中，所述步骤6中，以最小曲率法获取的位置信息作为滤波观测量进行滤波修正的方法如下：
通过捷联惯性导航解算方法获取位置信息：
Figure BDA0002880631110000121
Figure BDA0002880631110000122
Figure BDA0002880631110000123
其中：L为纬度，λ为经度，h为高度，Re为地球半径，f为地球椭圆扁率，vn为速度；通过捷联导航算法中的位置更新，获得位置信息；利用最小曲率法获得的位置信息对捷联导航算法中的姿态、速度、位置进行反馈修正；设计的滤波器如下：
与步骤3中类似，卡尔曼滤波采用15维状态量：
Figure BDA0002880631110000124
滤波器的状态方程为：
Figure BDA0002880631110000125
其中，u为系统噪声，F矩阵为：
Figure BDA0002880631110000126
量测方程为：
Z＝HX+V
其中，V为量测噪声，量测值与量测矩阵如下：
Z＝[Pins-Pmincrv]
H＝[O3×6 I3×3 O3×6]
其中，Pins为捷联更新算法解算出的位置，Pmincrv为最小曲率法求解出的位置。
一般钻杆长度都是精确已知的，在一根钻杆钻进完成后，可以利用钻杆的长度信息来推算当前的位置。同时利用捷联更新算法求解位置信息，通过卡尔曼滤波的方式，对捷联更新算法求解的位置信息进行修正，可以实现定位精度的进一步提高。另外，若能以较高频率获取钻杆钻进的长度，则可以提高修正的频率。
以上所述，仅为本发明的较佳实施例，并非对本发明做任何形式上的限制，任何熟悉本领域的技术人员但凡未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做任何简单的修改、均等变化与修饰，皆应属本发明的涵盖范围。

Claims (4)

1.一种随钻陀螺定位定向仪导航解算方法，其特征在于：包括以下步骤：
步骤1、初始对准获取钻杆钻进起始时刻的姿态角；
步骤2、钻进过程中进行姿态、速度、位置更新获取实时的导航信息；
步骤3、利用运动学约束的方式进行钻进过程中的修正；
步骤4、当一根钻杆钻进完成时，获取当前的姿态角；
步骤5、利用最小曲率法求解此时位置；
步骤6、以最小曲率法获取的位置信息作为滤波观测量进行滤波修正；
所述解算方法采用的坐标系定义如下：
b系：钻杆载体坐标系oxbybzb，y轴指钻杆钻进方向，z轴沿钻杆横截面半径向外，x轴与y轴、z轴构成满足右手定则的笛卡尔直角坐标系；
n系：导航坐标系oxnynzn，采用“东-北-天”地理坐标系，x轴指东，y轴指北，z轴指天；
所述步骤2中，钻进过程中进行姿态、速度、位置更新获取实时的导航信息的方法如下：
在钻进过程中进行姿态、速度、位置解算的方法是采用捷联更新算法实现的；姿态更新微分方程为：
Figure FDA0003684952460000011
其中，
Figure FDA0003684952460000012
为b系相对于n系的姿态阵，
Figure FDA0003684952460000013
为n系相对钻杆载体坐标系b系的旋转角速度；
类比与姿态更新微分方程可得：
Figure FDA0003684952460000014
Figure FDA0003684952460000015
根据
Figure FDA0003684952460000016
其中，矩阵
Figure FDA0003684952460000017
表示以i系作为参考基准，b系从tm-1时刻到tm时刻的旋转变化，
Figure FDA0003684952460000018
由陀螺角速度
Figure FDA0003684952460000019
确定；
Figure FDA00036849524600000110
表示以i系作为参考基准，n系从tm时刻到tm-1时刻的旋转变化，
Figure FDA0003684952460000021
由计算角速度
Figure FDA0003684952460000022
确定，
Figure FDA0003684952460000023
可得：
Figure FDA0003684952460000024
通过初始姿态矩阵，结合陀螺采集的角速度信息，根据上式即可解算下一时刻的姿态信息；
速度更新微分方程为：
Figure FDA0003684952460000025
其中，
Figure FDA0003684952460000026
为载体的速度、
Figure FDA0003684952460000027
为加表测量值、
Figure FDA0003684952460000028
为地球转速、
Figure FDA0003684952460000029
为载体运动引起的旋转角速度、gn为重力；
速度更新通过比力方程的一次积分来实现；加表测量的加速度信息中包含了载体自身运动的加速度信息、重力加速度信息、载体运动所导致的哥式加速度信息和对地向心加速度信息；根据比力方程，加表测量的加速度信息在考虑了包含重力加速度、哥式加速度、对地向心加速度在内的有害加速度信息后，积分即可得到载体运动的速度变化量：
Figure FDA00036849524600000210
Figure FDA00036849524600000211
其中，上标m-1表示上一时刻，m表示下一时刻，T为采样周期；
位置更新微分方程为：
Figure FDA00036849524600000212
其中，L为纬度， λ为经度， h为高度， RM、RN为地球参数；
对速度的一次积分，即可得到位置变化量，可求解位置：
Figure FDA00036849524600000213
2.根据权利要求1所述的一种随钻陀螺定位定向仪导航解算方法，其特征在于：所述步骤3中，利用运动学约束的方式进行钻进过程中的修正的方法如下：
根据惯性导航的误差模型：
Figure FDA0003684952460000031
Figure FDA0003684952460000032
Figure FDA0003684952460000033
Figure FDA0003684952460000034
Figure FDA0003684952460000035
可知，惯性导航的姿态速度及位置误差是各误差相互耦合的结果，误差方程具有一定规律；考虑到钻进过程比较缓慢，近似为一种静基座的情况；惯导的误差传播方程为一个线性定常系统，根据误差模型建立卡尔曼滤波方程进行修正；钻杆钻进过程中，利用钻杆载体坐标系中x和z轴方向速度为0，做速度约束；
卡尔曼滤波采用15维状态量：
Figure FDA0003684952460000036
其中，φn为失准角、δVn为速度误差、δPn为位置误差、ε为陀螺零偏、
Figure FDA0003684952460000037
为加表零偏；
滤波器的状态方程为：
Figure DEST_PATH_IMAGE002
其中，u为系统噪声，F矩阵为：
Figure FDA0003684952460000039
F矩阵中有：
Figure FDA00036849524600000310
Figure FDA0003684952460000041
Figure FDA0003684952460000042
Figure FDA0003684952460000043
Figure FDA0003684952460000044
设惯导系统输出的导航坐标系下速度为Vn，钻杆载体坐标系下速度为Vb，则：
Figure FDA0003684952460000045
利用钻杆载体坐标系x轴与z轴速度为0作为约束条件，则量测值为：
Figure FDA0003684952460000046
量测方程为：
Z＝HX+V
其中，V为量测噪声，H矩阵为：
Figure FDA0003684952460000051
其中：
Figure FDA0003684952460000052
H15＝Vb×(1,3)
Figure FDA0003684952460000053
H24＝Vb×(3,1)。
3.根据权利要求1所述的一种随钻陀螺定位定向仪导航解算方法，其特征在于：所述步骤5中，利用最小曲率法求解此时位置的方法如下：
最小曲率法基本假设在测段为空间斜平面上的一段圆弧曲线，它用包含作为测点的圆弧井段两个端点的井斜角、位角以及圆弧弯曲角在内的角度参数来计算测段的坐标增量；
钻进工作过程中，每一个钻杆的长度是精确已知的，最小曲率法利用姿态信息和两个测点之间的轨迹长度来计算坐标的增量；最小曲率法的计算公式为：
doleg＝arccos(cos(I1-I2))-sinI1sinI2(1-cos(A2-A1))
RF＝2tan(doleg/2)/doleg
ΔTVD＝1/2·ΔMD(cosI1+cosI2)RF
ΔN＝1/2·ΔMD(sinI1cosA1+sinI2cosA2)RF
ΔE＝1/2·ΔMD(sinI1sinA1+sinI2sinA2)RF
其中，doleg称狗腿度，即全角变化率，其反应了井斜角度的变化和方位角度的变化；RF为比率因数；ΔMD为钻杆长度；I1,I2为两测点的倾角，倾角由姿态角中的俯仰角计算求解，即倾角＝俯仰角+90°；A1,A2为两测点的方位角；ΔTVD,ΔE,ΔN分别为高度变化量，东向变化量，北向变化量；
由此可得：
doleg＝arccos(cos(θ1-θ2))-cosθ1cosθ2(1-cos(ψ2-ψ1))
RF＝2tan(doleg/2)/doleg
h2≈h1+1/2·ΔMD(sinθ1+sinθ2)RF
L2≈L1-1/2·ΔMD(cosθ1cosψ1+cosθ2cosψ2)RF/Re
λ2≈λ1-1/2·ΔMD(cosθ1sinψ1+cosθ2sinψ2)RF/Re/cosL
其中，θ为俯仰姿态角，ψ为方位姿态角，Re为地球半径，角标2表示下一位置，角标1表示上一位置；当已知第一个测点的位置和姿态时，通过钻杆长度和下一测点的姿态便可求解出坐标变化量，从而获取当前的位置信息。
4.根据权利要求1所述的一种随钻陀螺定位定向仪导航解算方法，其特征在于：所述步骤6中，以最小曲率法获取的位置信息作为滤波观测量进行滤波修正的方法如下：
通过捷联惯性导航解算方法获取位置信息：
Figure FDA0003684952460000061
Figure FDA0003684952460000062
Figure FDA0003684952460000063
其中：L为纬度，λ为经度，h为高度，Re为地球半径，f为地球椭圆扁率，vn为速度；通过捷联导航算法中的位置更新，获得位置信息；利用最小曲率法获得的位置信息对捷联导航算法中的姿态、速度、位置进行反馈修正；设计的滤波器如下：
卡尔曼滤波采用15维状态量：
Figure FDA0003684952460000064
滤波器的状态方程为：
Figure 909970DEST_PATH_IMAGE002
其中，u为系统噪声，F矩阵为：
Figure FDA0003684952460000066
量测方程为：
Z＝HX+V
其中，V为量测噪声，量测值与量测矩阵如下：
Z＝[Pins-Pmincrv]
H＝[O3×6 I3×3 O3×6]
其中，Pins为捷联更新算法解算出的位置，Pmincrv为最小曲率法求解出的位置。
CN202011633460.XA 2020-12-31 2020-12-31 一种随钻陀螺定位定向仪导航解算方法 Active CN112781588B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011633460.XA CN112781588B (zh) 2020-12-31 2020-12-31 一种随钻陀螺定位定向仪导航解算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011633460.XA CN112781588B (zh) 2020-12-31 2020-12-31 一种随钻陀螺定位定向仪导航解算方法

Publications (2)

Publication Number Publication Date
CN112781588A CN112781588A (zh) 2021-05-11
CN112781588B true CN112781588B (zh) 2022-08-12

Family

ID=75754788

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011633460.XA Active CN112781588B (zh) 2020-12-31 2020-12-31 一种随钻陀螺定位定向仪导航解算方法

Country Status (1)

Country Link
CN (1) CN112781588B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113777645B (zh) * 2021-09-10 2024-01-09 北京航空航天大学 一种gps拒止环境下高精度位姿估计算法
CN115574815B (zh) * 2022-11-18 2023-05-23 西安玉衡导航科技有限公司 一种非可视环境导航系统、方法、计算机设备及存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5978739A (en) * 1997-10-14 1999-11-02 Stockton; Thomas R. Disconnect information and monitoring system for dynamically positioned offshore drilling rigs
CN110275438A (zh) * 2019-06-17 2019-09-24 中国地质大学（武汉） 一种钻具姿态补偿控制方法及系统
CN110513043A (zh) * 2019-07-15 2019-11-29 中国地质大学（武汉） 一种基于自动导向钻具的垂直钻进过程纠斜控制方法
CN110792430A (zh) * 2019-11-20 2020-02-14 中国地质大学（北京） 一种基于多传感器数据融合的随钻测斜方法及装置
CN111521179A (zh) * 2020-04-28 2020-08-11 中国人民解放军国防科技大学 基于行进速度自检测的钻探用定位定向仪孔内定位方法
CN111878056A (zh) * 2020-05-11 2020-11-03 中国科学院地质与地球物理研究所 一种陀螺随钻测量系统及方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108603405A (zh) * 2015-09-28 2018-09-28 赫尔实验室有限公司 利用多站分析进行的实时轨迹估计
CN111878064B (zh) * 2020-05-11 2024-04-05 中国科学院地质与地球物理研究所 一种姿态测量方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5978739A (en) * 1997-10-14 1999-11-02 Stockton; Thomas R. Disconnect information and monitoring system for dynamically positioned offshore drilling rigs
CN110275438A (zh) * 2019-06-17 2019-09-24 中国地质大学（武汉） 一种钻具姿态补偿控制方法及系统
CN110513043A (zh) * 2019-07-15 2019-11-29 中国地质大学（武汉） 一种基于自动导向钻具的垂直钻进过程纠斜控制方法
CN110792430A (zh) * 2019-11-20 2020-02-14 中国地质大学（北京） 一种基于多传感器数据融合的随钻测斜方法及装置
CN111521179A (zh) * 2020-04-28 2020-08-11 中国人民解放军国防科技大学 基于行进速度自检测的钻探用定位定向仪孔内定位方法
CN111878056A (zh) * 2020-05-11 2020-11-03 中国科学院地质与地球物理研究所 一种陀螺随钻测量系统及方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"A Reduced Inertial Sensor System Based on MEMS for Wellbore Continuous Surveying While Horizontal Drilling";Wang, L 等;《IEEE sensors journal》;20181231;第18卷(第14期);正文第5662-5670页 *
基于陀螺仪的随钻姿态测量新算法;苏毅等;《传感器与微系统》;20130120(第01期);正文第137-140页 *

Also Published As

Publication number Publication date
CN112781588A (zh) 2021-05-11

Similar Documents

Publication Publication Date Title
CN110886606B (zh) 一种随钻特征量辅助的惯性测斜方法及装置
WO2021227012A1 (zh) 一种姿态测量方法
CN110792430B (zh) 一种基于多传感器数据融合的随钻测斜方法及装置
CN107270893B (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
EP3933166A1 (en) Attitude measurement method
US6668465B2 (en) Continuous measurement-while-drilling surveying
US20210199438A1 (en) Heading initialization method for tilt rtk
CN110553646B (zh) 基于惯性和磁航向及零速修正的行人导航方法
CN112781588B (zh) 一种随钻陀螺定位定向仪导航解算方法
US10550686B2 (en) Tumble gyro surveyor
CN111878056A (zh) 一种陀螺随钻测量系统及方法
US6823602B2 (en) Continuous measurement-while-drilling surveying
CN112378399B (zh) 基于捷联惯导和数字全站仪的煤矿巷道掘进机器人精确定位定向方法
CN110440746A (zh) 一种基于四元数梯度下降的非开挖地下钻头姿态融合方法
CN110207698B (zh) 一种极区格网惯导/超短基线紧组合导航方法
CN111189446B (zh) 一种基于无线电的组合导航方法
US9976408B2 (en) Navigation device and method for surveying and directing a borehole under drilling conditions
CN112798014A (zh) 一种基于重力场球谐模型补偿垂线偏差的惯导自对准方法
JP2014041117A (ja) 地中掘削位置を計測する方法、地中掘削位置計測装置及び非開削工法用掘削システム
CN114061574B (zh) 一种基于位置不变约束及零速校正的采煤机定姿定向方法
CN115326064A (zh) 一种采煤机机身与摇臂协同高精度定位装置及定位方法
CN114001757A (zh) 一种磁力计零偏校准方法
CN112033401B (zh) 基于捷联惯导和油缸的智能掘进机器人定位定向方法
AU2012318276B8 (en) Navigation device and method for surveying and directing a borehole under drilling conditions
Gus’ kov et al. Effect of the gyroplatform control error on the accuracy of the initial azimuth alignment of the gyro inclinometer

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant