本发明涉及一种旋转导向稳定平台的重力工具面角动态测量方法及装置，采用双加速度计、三轴磁通门传感器和陀螺仪测量的稳定平台运动时的相应加速度、磁场和角速度数据，进行前置滤波后，在不同工况和不同传感器故障时自动判断传感器故障并切换至相应解算方法解算重力工具面角，实现对重力工具面角的测量，提高了重力工具面角测量的可靠性；并通过互补滤波融合双加速度计或单加速度计或磁通门传感器测量并解算的重力工具面角和陀螺仪测量的对地速率，削弱横向振动与扭转振动对重力工具面角测量值的影响，得到的重力工具面角测量值更加精确。
1.一种旋转导向稳定平台重力工具面角动态测量方法，其特征在于，含有以下步骤：(一)在稳定平台上安装主加速度计、副加速度计、三轴磁通门传感器和陀螺仪，所述主加速度计的XZ轴平面与钻井导向工具的装置面重合，所述副加速度计的XZ轴平面与所述主加速度计的XZ轴平面呈夹角，所述三轴磁通门传感器位于所述主加速度计的一端，且所述三轴磁通门传感器的XZ轴平面与钻井导向工具的装置面重合，所述陀螺仪的敏感轴与所述稳定平台轴线平行；
(二)稳定平台运动时，主加速度计实时测量所述主加速度计所在处稳定平台的三轴加速度分量，副加速度计实时测量所述副加速度计所在处稳定平台的三轴加速度分量，三轴磁通门传感器测量所述三轴磁通门传感器所在处稳定平台的三轴磁场分量；陀螺仪实时测量所述陀螺仪所在点处围绕稳定平台轴线转动的角速率；
(三)对主加速度计测量的Y轴测量值 和Z轴测量值 副加速度计测量的Y轴测量值和Z轴测量值 三轴磁通门传感器测量的Y轴测量值by和Z轴测量值bz、陀螺仪测量的测量值ωg进行前置滤波处理，滤除噪声后得到估计值(四)判定主加速度计和副加速度计的工作状态以及井下工况的振动情况；若主加速度计和副加速度计均正常工作且振动强度处于正常范围时，采用双加速度计重力工具面角解算方法解算得到重力工具面角βa；若主加速度计和副加速度计中某一个故障且振动强度处于正常范围时，采用单加速度计重力工具面角解算方法解算得到重力工具面角βg；若主加速度计和副加速度计均故障或振动强度超过正常范围时，判断三轴磁通门传感器是否正常工作，若三轴磁通门传感器正常工作，采用磁通门重力工具面角解算方法解算得到重力工具面角βb，若三轴磁通门传感器故障，则工具面角无法正常解算，需起钻维修；
(五)判定陀螺仪是否工作正常，若陀螺仪工作正常，通过互补滤波融合步骤(四)解算的重力工具面角βa或βg或βb和陀螺仪测量值ωg的估计值 得到互补滤波器重力工具面角βc，将互补滤波器重力工具面角βc作为最终重力工具面角测量值，若陀螺仪故障，则直接采用步骤(四)解算的重力工具面角βa或βg或βb作为最终重力工具面角测量值β。
2.如权利要求1所述的旋转导向稳定平台重力工具面角动态测量方法，其特征在于，双加速度计重力工具面角解算方法解算得到重力工具面角的具体步骤如下：记稳定平台半径为R，主加速度计Y轴和Z轴的测量误差项为 副加速度计Y轴和Z轴的测量误差项为 则主加速度计Y轴和Z轴的测量值 以及副加速度计Y轴和Z轴的测量值 满足如下测量方程：式中，g为重力加速度，βa为重力工具面角， 为稳定平台转动角加速度，ω为稳定平台转动角速率，γ为副加速度计的XZ轴平面与主加速度计的XZ轴平面的夹角，0°<γ≤360°；
3.如权利要求2所述的旋转导向稳定平台重力工具面角动态测量方法，其特征在于，当γ＝90°时，经过前置滤波处理后的主加速度计Y轴和Z轴的估计值 以及副加速度计Y轴和Z轴的估计值 之间存在如下冗余关系：则测量方程表示为：
式中，ayf表示经过主加速度计和副加速度计数据融合后的Y轴测量值，azf表示经过主加速度计和副加速度计数据融合后的Z轴测量值；
当γ＝180°时，经过前置滤波处理后的主加速度计Y轴和Z轴的估计值 以及副加速度计Y轴和Z轴的估计值 之间存在如下冗余关系：则测量方程表示为：
当γ＝270°时，经过前置滤波处理后的主加速度计Y轴和Z轴的估计值 以及副加速度计Y轴和Z轴的估计值 之间存在如下冗余关系：则测量方程表示为：
4.如权利要求2所述的旋转导向稳定平台重力工具面角动态测量方法，其特征在于，单加速度计重力工具面角解算方法解算得到重力工具面角的具体步骤如下：在副加速度计故障的情况下，采用主加速度计估计值解算重力工具面角
在主加速度计故障的情况下，采用副加速度计估计值解算重力工具面角
5.如权利要求1所述的旋转导向稳定平台重力工具面角动态测量方法，其特征在于，磁通门重力工具面角解算方法解算得到重力工具面角的具体步骤如下：记稳定平台所在位置地磁场总强度为B，静态测量的井斜角为α、井斜方位角为 和磁倾角为D；
定义钻井导向工具的当前姿态 是从初始位置按照沿三轴磁通门传感器的Z轴改变井斜方位角 然后绕三轴磁通门传感器的Y轴改变井斜角α，最后绕三轴磁通门传感器的X轴改变重力工具面角βb得到；三次转动对应的姿态变换矩阵Mz，My和Mx如下:记： 则有：
当by2≠0或bz2≠0时公式(10)成立，式中，bx1,by1,bz1表示初始位置按照沿三轴磁通门传感器的Z轴改变井斜方位角 后的三轴磁场分量，bx2,by2,bz2表示绕三轴磁通门传感器的Y轴改变井斜角α的三轴磁场分量；
6.如权利要求1所述的旋转导向稳定平台重力工具面角动态测量方法，其特征在于，步骤(五)中，通过互补滤波融合重力工具面角βa或βg或βb和陀螺仪测量值ωg的估计值 得到的互补滤波器重力工具面角βc，采用Laplace形式表示为：式中，Kp和Ki为互补滤波器PI控制器参数，βi为重力工具面角βa或βg或βb， 为陀螺仪测量值ωg的估计值。
7.如权利要求6所述的旋转导向稳定平台重力工具面角动态测量方法，其特征在于，步骤(五)中，在互补滤波融合重力工具面角βa或βg或βb和陀螺仪测量值ωg的估计值 之前，通过重力工具面角预处理消除重力工具面角的跳变，其具体步骤为：记：
式中，βi为重力工具面角βa或βg或βb，Δe和β1为预处理的中间变量，e为预处理输出的偏差值，互补滤波器重力工具面角βc输出前需经过重力工具面角值域处理，使得βc位于[0,
8.一种旋转导向稳定平台重力工具面角动态测量装置，其特征在于，包括：
主加速度计，安装于稳定平台上，用于测量所述主加速度计所在处稳定平台的三轴加速度分量，所述主加速度计的XZ轴平面与钻井导向工具的装置面重合；
副加速度计，安装于稳定平台上，用于测量所述副加速度计所在处稳定平台的三轴加速度分量，所述副加速度计的XZ轴平面与所述主加速度计的XZ轴平面呈夹角；
三轴磁通门传感器，安装于稳定平台上，用于测量所述三轴磁通门传感器所在处稳定平台的三轴磁场分量，所述三轴磁通门传感器位于主加速度计的一端，且所述三轴磁通门传感器的XZ轴平面与钻井导向工具的装置面重合；
陀螺仪，安装于稳定平台上，用于测量所述陀螺仪所在点处围绕稳定平台轴线转动的角速率，所述陀螺仪的敏感轴与所述稳定平台轴线平行；
前置滤波器模块，分别与所述主加速度计、所述副加速度计、所述三轴磁通门传感器、所述陀螺仪连接，用于消除测量噪声，得到陀螺仪测量值的估计值；
与所述前置滤波器模块连接的振动强度判定单元，用于判断稳定平台当前的振动强度；
与所述振动强度判定单元连接的故障判断单元，用于判断所述主加速度计、所述副加速度计、所述三轴磁通门传感器和所述陀螺仪是否故障；
与所述故障判断单元连接的加速度数据融合单元，所述加速度数据融合单元包括Y轴数据融合模块和Z轴数据融合模块；
与所述加速度数据融合单元连接的加速度重力工具面角解算单元，所述加速度重力工具面角解算单元包括双加速度重力工具面角解算模块和单加速度重力工具面角解算模块，用于解算重力工具面角；
与所述故障判断单元连接的磁通门重力工具面角解算单元，用于解算重力工具面角；
分别与所述加速度重力工具面角解算单元和所述磁通门重力工具面角解算单元连接的互补滤波数据融合单元，用于融合所述加速度重力工具面角解算单元或所述磁通门重力工具面角解算单元解算得到的重力工具面角与陀螺仪测量值的估计值得到重力工具面角。
9.如权利要求8所述的旋转导向稳定平台重力工具面角动态测量装置，其特征在于，所述互补滤波数据融合单元包括重力工具面角预处理模块和与重力工具面角预处理模块连接的互补滤波器，所述重力工具面角预处理模块分别与所述加速度重力工具面角解算单元和所述磁通门重力工具面角解算单元连接，用于消除重力工具面角的跳变。
10.如权利要求8或9所述的旋转导向稳定平台重力工具面角动态测量装置，其特征在于，所述稳定平台上设有主加速度计安装槽、副加速度计安装槽、三轴磁通门传感器安装槽和陀螺仪安装槽，所述副加速度计安装槽设有多个，用于改变所述副加速度计与主加速度计之间夹角的角度。
旋转导向稳定平台重力工具面角动态测量方法及装置
技术领域
[0001] 本发明属于钻井工具姿态动态测量技术领域，涉及单轴惯性平台姿态测量技术，具体地说，涉及了一种旋转导向稳定平台的重力工具面角动态测量方法及装置。
背景技术
[0002] 石油定向钻井技术中，重力工具面是非常重要的工艺参数。与钻柱呈一定弯角的导向钻井工具下到井底后，弯曲面与井底圆平面所成交线以井底圆面高边为始边，顺时针转过的角度称为重力工具面，导向钻井工具的弯曲面称为装置面。常规导向钻井工具为弯接头或带弯角的螺杆钻具，旋转导向钻井工具是在常规导向工具基础上发展起来的一种高性能导向钻进工具。旋转导向钻井工具按照导向方式可以分为推靠式和指向式两种，而每一种导向方式按照钻铤是否旋转又可以分为静态式和动态式两种，由此组合出四种旋转导向钻井方式，其中动态指向式旋转导向工具是目前最先进的旋转导向钻井工具。与其他旋转导向钻井工具不同，为保证旋转导向工具在钻铤连续旋转的情况下重力工具面角稳定，动态指向式旋转导向工具内部设计有电控稳定平台，用以隔离钻铤的连续旋转，保证钻头的稳定指向，即重力工具面角稳定。由此可见，重力工具面角是否稳定直接决定了钻井工具能否导向。
[0003] 常规MWD(随钻测量)系统在复合钻进过程中可以实现重力工具面角动态测量，但是，由于重力工具面角更新速率较慢，不能满足旋转导向稳定平台的测量要求。实际钻井过程中，纵向振动、横向振动和扭矩振动产生的运动加速度会对重力加速度测量结果产生严重干扰，在高速动态测量时影响更加严重，导致解算出的重力工具面角动态测量结果误差较大。
[0004] 公开号为CN 105180889 A的在中国专利文献公开了一种用于钻井的动态旋转姿态测量装置及方法，并具体公开了一种动态旋转姿态测量方法，该方法首先通过磁性传感器、加速度计和陀螺仪分别测量地球磁场分量、地球重力分量和整个装置的转速，将各个传感器采集的物理输入转换成电压输出；然后根据安装误差系数和陀螺仪得到的转速，在加速度计的输出项中去除有害加速度项，实现误差补偿；然后根据加速度计和磁性传感器采集的地球磁场和地球重力数据计算钻井的动态旋转姿态。该方法采用陀螺仪转速微分进行加速度误差补偿，但该方法容易受到噪声的影响，补偿效果差。
[0005] 公开号为CN 104727807 A的中国专利文献公开了一种角度位置测量方法及系统，具体公开了一种钻具角度位置测量方法及系统。该方法包括以下步骤：实时采集井下工况信息；基于泥浆流量信息以及地磁场信息或重力场信息所确定的旋转速度来判断钻具是否处于静止的状态；当钻具静止时，计算井斜、钻具的静态重力工具面和静态磁工具面，并将基准工具面确定为静态重力工具面或者静态磁工具面；当钻具运动时，计算磁工具面的变化量；将基准工具面与磁工具面的变化量叠加计算得到钻具的动态重力工具面从而确定钻具的角度位置。该发明利用旋转磁场的相对变化来实时测量钻具的重力工具面，同时监测磁场干扰情况，可避免磁干扰对测量结果的影响，实现钻具角度位置的准确测量。但在动态情况下，完全依靠磁通门测量重力工具面角时，磁通门容易受到井下铁磁物质干扰，且该发明假设某一段动态测量时间段内井斜、方位、磁倾角不变或变化较小，该假设会导致重力工具面角测量偏差较大。
[0006] 杨全进等提出的一种用于井下钻具旋转中动态方位测量的新方法(参考文献：杨全进，蒋海旭，左信.一种用于井下钻具旋转中动态方位测量的新方法[J].石油钻井工艺，2014,36(1)：40-43)分析了利用矢量旋转的方法，从理论上推导计算磁工具面与重力工具面差角的过程，并给出了实验结果。但该方法并未给出详细通过磁通门原始测量数据结算重力工具面角的公式。
[0007] 由上可知，直接将传感器安装于旋转导向稳定平台上，设计专用的姿态测量方法及装置用以提高测量结果的精确度是十分有必要的。
发明内容
[0008] 本发明针对现有技术动态测量重力工具面角时存在的测量结果误差大的不足，提供一种测量误差小、可靠性高的旋转导向稳定平台的重力工具面角动态测量方法及装置。
[0009] 为了达到上述目的，本发明提供了一种旋转导向稳定平台的重力工具面角动态测量方法，含有以下步骤：
[0010] (一)在稳定平台上安装主加速度计、副加速度计、三轴磁通门传感器和陀螺仪，所述主加速度计的XZ轴平面与钻井导向工具的装置面重合，所述副加速度计的XZ轴平面与所述主加速度计的XZ轴平面呈夹角，所述三轴磁通门传感器位于所述主加速度计的一端，且所述三轴磁通门传感器的XZ轴平面与钻井导向工具的装置面重合，所述陀螺仪的敏感轴与所述稳定平台轴线平行；
[0011] (二)稳定平台运动时，主加速度计实时测量所述主加速度计所在处稳定平台的三轴加速度分量，副加速度计实时测量所述副加速度计所在处稳定平台的三轴加速度分量，三轴磁通门传感器测量所述三轴磁通门传感器所在处稳定平台的三轴磁场分量；陀螺仪实时测量所述陀螺仪所在点处围绕稳定平台轴线转动的角速率；
[0012] (三)对主加速度计测量的Y轴测量值 和Z轴测量值 副加速度计测量的Y轴测量值 和Z轴测量值 三轴磁通门传感器测量的Y轴测量值by和Z轴测量值bz、陀螺仪测量的测量值ωg进行前置滤波处理，滤除噪声后得到估计值
[0013] (四)判定主加速度计和副加速度计的工作状态以及井下工况的振动情况；若主加速度计和副加速度计均正常工作且振动强度处于正常范围时，采用双加速度计重力工具面角解算方法解算得到重力工具面角βa；若主加速度计和副加速度计中某一个故障且振动强度处于正常范围时，采用单加速度计重力工具面角解算方法解算得到重力工具面角βg；若主加速度计和副加速度计均故障或振动强度超过正常范围时，判断三轴磁通门传感器是否正常工作，若三轴磁通门传感器正常工作，采用磁通门重力工具面角解算方法解算得到重力工具面角βb，若三轴磁通门传感器故障，则工具面角无法正常解算，需起钻维修；
[0014] (五)判定陀螺仪是否工作正常，若陀螺仪工作正常，通过互补滤波融合步骤(四)解算的重力工具面角βa或βg或βb和陀螺仪测量值ωg的估计值 得到互补滤波器重力工具面角βc，互补滤波器重力工具面角βc作为最终工具面角测量值β，若陀螺仪故障，则直接采用步骤(四)解算的重力工具面角βa或βg或βb作为最终重力工具面角测量值β。
[0015] 进一步的，双加速度计重力工具面角解算方法解算得到重力工具面角的具体步骤如下：
[0016] 记稳定平台半径为R，主加速度计Y轴和Z轴的测量误差项为 副加速度计Y轴和Z轴的测量误差项为 则主加速度计Y轴和Z轴的测量值 以及副加速度计Y轴和Z轴的测量值 满足如下测量方程：
[0017]
[0018] 式中，g为重力加速度，βa为重力工具面角， 为稳定平台转动角加速度，ω为稳定平台转动角速率，γ为副加速度计的XZ轴平面与主加速度计的XZ轴平面的夹角，0°<γ≤360°；
[0019] 忽略测量的误差项，整理公式(1)得到：
[0020]
[0021] 通过解算公式(2)得到重力工具面角βa。
[0022] 进一步的，当γ＝90°时，经过前置滤波处理后的主加速度计Y轴和Z轴的估计值以及副加速度计Y轴和Z轴的估计值 之间存在如下冗余关系：
[0023]
[0024] 则测量方程表示为：
[0025]
[0026] 式中，ayf表示经过主加速度计和副加速度计数据融合后的Y轴测量值，azf表示经过主加速度计和副加速度计数据融合后的Z轴测量值；
[0027] 通过解算公式(4)得到重力工具面角βa；
[0028] 当γ＝180°时，经过前置滤波处理后的主加速度计Y轴和Z轴的估计值 以及副加速度计Y轴和Z轴的估计值 之间存在如下冗余关系：
[0029]
[0030] 则测量方程表示为：
[0031]
[0032] 通过解算公式(6)得到重力工具面角βa；
[0033] 当γ＝270°时，经过前置滤波处理后的主加速度计Y轴和Z轴的估计值 以及副加速度计Y轴和Z轴的估计值 之间存在如下冗余关系：
[0034]
[0035] 则测量方程表示为：
[0036]
[0037] 通过解算公式(8)得到重力工具面角βa。
[0038] 进一步的，单加速度计重力工具面角解算方法解算得到重力工具面角的具体步骤如下：
[0039] 在副加速度计故障的情况下，采用主加速度计估计值解算重力工具面角[0040] 在主加速度计故障的情况下，采用副加速度计估计值解算重力工具面角[0041] 进一步的，磁通门重力工具面角解算方法解算得到重力工具面角的具体步骤如下：
[0042] 记稳定平台所在位置地磁场总强度为B，静态测量的井斜角为α、井斜方位角为和磁倾角为D；
[0043] 定义钻井导向工具的当前姿态 是从初始位置按照沿三轴磁通门传感器的Z轴改变井斜方位角 然后绕三轴磁通门传感器的Y轴改变井斜角α，最后绕三轴磁通门传感器的X轴改变重力工具面角βb得到；三次转动对应的姿态变换矩阵Mz，My和Mx如下:
[0044]
[0045] 记： 则有：
[0046]
[0047] 当by2≠0或bz2≠0时式(10)成立，式中，bx1,by1,bz1表示初始位置按照沿三轴磁通门传感器的Z轴改变井斜方位角 后的三轴磁场分量，bx2,by2,bz2表示绕三轴磁通门传感器的Y轴改变井斜角α的三轴磁场分量；
[0048] 通过解算公式(10)得到重力工具面角βb。
[0049] 进一步的，步骤(五)中，通过互补滤波融合重力工具面角βa或βg或βb和陀螺仪测量值ωg的估计值 得到的互补滤波器重力工具面角βc，采用Laplace形式表示为：
[0050]
[0051] 式中，Kp和Ki为互补滤波器PI控制器参数，βi为重力工具面角βa或βg或βb， 为陀螺仪测量值ωg的估计值。
[0052] 优选的，步骤(五)中，在互补滤波融合重力工具面角βa或βg或βb和陀螺仪测量值ωg的测量值ωg之前，通过重力工具面角预处理消除重力工具面角的跳变，其具体步骤为：：
[0053] 记：
[0054]
[0055] 式中，βi为重力工具面角βa或βg或βb，Δe和β1为预处理的中间变量，e为预处理输出的偏差值，互补滤波器重力工具面角βc输出前需经过重力工具面角值域处理，使得βc位于[0,360]值域范围内。
[0056] 为了达到上述目的，本发明还提供了一种旋转导向稳定平台重力工具面角动态测量装置，包括：
[0057] 主加速度计，安装于稳定平台上，用于测量所述主加速度计所在处稳定平台的三轴加速度分量，所述主加速度计的XZ轴平面与钻井导向工具的装置面重合；
[0058] 副加速度计，安装于稳定平台上，用于测量所述副加速度计所在处稳定平台的三轴加速度分量，所述副加速度计的XZ轴平面与所述主加速度计的XZ轴平面呈夹角；
[0059] 三轴磁通门传感器，安装于稳定平台上，用于测量所述三轴磁通门传感器所在处稳定平台的三轴磁场分量，所述三轴磁通门传感器位于主加速度计的一端，且所述三轴磁通门传感器的XZ轴平面与钻井导向工具的装置面重合；
[0060] 陀螺仪，安装于稳定平台上，用于测量所述陀螺仪所在点处围绕稳定平台轴线转动的角速率，所述陀螺仪的敏感轴与所述稳定平台轴线平行；
[0061] 前置滤波器模块，分别与所述主加速度计、所述副加速度计、所述三轴磁通门传感器、所述陀螺仪连接，用于消除测量噪声，得到陀螺仪测量值的估计值；
[0062] 与所述前置滤波器模块连接的振动强度判定单元，用于判断稳定平台当前的振动强度；
[0063] 与所述振动强度判定单元连接的故障判断单元，用于判断所述主加速度计、所述副加速度计、所述三轴磁通门传感器和所述陀螺仪是否故障；
[0064] 与所述故障判断单元连接的加速度数据融合单元，所述加速度数据融合单元包括Y轴数据融合模块和Z轴数据融合模块；
[0065] 与所述加速度数据融合单元连接的加速度重力工具面角解算单元，所述加速度重力工具面角解算单元包括双加速度重力工具面角解算模块和单加速度重力工具面角解算模块，用于解算重力工具面角；
[0066] 与所述故障判断单元连接的磁通门重力工具面角解算单元，用于解算重力工具面角；
[0067] 分别与所述加速度重力工具面角解算单元和所述磁通门重力工具面角解算单元连接的互补滤波数据融合单元，用于融合所述加速度重力工具面角解算单元或所述磁通门重力工具面角解算单元解算得到的重力工具面角与陀螺仪测量值的估计值得到重力工具面角。
[0068] 优选的，所述互补滤波数据融合单元包括重力工具面角预处理模块和与重力工具面角预处理模块连接的互补滤波器，所述重力工具面角预处理模块分别与所述加速度重力工具面角解算单元和所述磁通门重力工具面角解算单元连接，用于消除重力工具面角的跳变。
[0069] 优选的，所述稳定平台上设有主加速度计安装槽、副加速度计安装槽、三轴磁通门传感器安装槽和陀螺仪安装槽，所述副加速度计安装槽设有多个，用于改变所述副加速度计与主加速度计之间夹角的角度。
[0070] 优选的，所述主加速度计安装槽和所述三轴磁通门传感器安装槽布置在钻井导向工具的装置面与稳定平台侧面的交线上，所述陀螺仪安装槽布置在所述副加速度计安装槽的正上方。
[0071] 优选的，所述主加速度计的X轴与所述稳定平台轴线平行，所述主加速度计的Z轴指向所述稳定平台的自转轴。
[0072] 优选的，所述三轴磁通门传感器的X轴与所述稳定平台轴线平行，所述三轴磁通门传感器的Z轴指向所述稳定平台的自转轴。
[0073] 与现有技术相比，本发明的有益效果在于：
[0074] (1)本发明采用双加速度计、三轴磁通门传感器和陀螺仪测量的数据，在不同工况和不同传感器故障时自动判断传感器故障并切换至备用方案进行测量，提高了重力工具面角测量的可靠性。
[0075] (2)本发明通过互补滤波融合双加速度计或单加速度计或磁通门传感器测量并解算的重力工具面角和陀螺仪测量的对地速率，削弱横向振动与扭转振动对重力工具面角测量值的影响，得到的重力工具面角测量值更加精确，提高了重力工具面角的动态测量精度。
[0076] (3)本发明对重力工具面角进行预处理，消除重力工具面角在360°位置处的不连续跳变，使得互补滤波器精度不受工具面测量值跳变的影响，实现重力工具面角在[0,360]范围内的高精度动态测量。
附图说明
[0077] 图1为本发明实施例旋转导向稳定平台的重力工具面角动态测量方法的流程图。
[0078] 图2为本发明实施例稳定平台上不同传感器的安装示意图。
[0079] 图3为本发明图2的A-A剖面图。
[0080] 图4a-4b为本发明实施例双加速度计重力工具面角解算方法解算重力工具面角的实测图。
[0081] 图5为本发明实施例陀螺仪测量的数据融合互补滤波器结构图。
[0082] 图6为本发明实施例重力工具面角预处理后效果实测图。
[0083] 图中，1、主加速度计，2、副加速度计，3、三轴磁通门传感器，4、陀螺仪，5、主加速度计安装槽，6、副加速度计安装槽，7、三轴磁通门传感器安装槽，8、陀螺仪安装槽，9、稳定平台轴线，10、副加速度计备选安装槽，11、钻井导向工具的装置面交线,12、稳定平台。
具体实施方式
[0084] 下面，通过示例性的实施方式对本发明进行具体描述。然而应当理解，在没有进一步叙述的情况下，一个实施方式中的元件、结构和特征也可以有益地结合到其他实施方式中。
[0085] 参见图1、图2，本发明提供了一种旋转导向稳定平台的重力工具面角动态测量方法，含有以下步骤：
[0086] 步骤一：在稳定平台12上安装主加速度计1、副加速度计2、三轴磁通门传感器3和陀螺仪4，所述主加速度计1的XZ轴平面与钻井导向工具的装置面重合，所述副加速度计2的XZ轴平面与所述主加速度计1的XZ轴平面呈夹角，所述三轴磁通门传感器3位于所述主加速度计1的一端，且所述三轴磁通门传感器3的XZ轴平面与钻井导向工具的装置面重合，所述陀螺仪4的敏感轴与所述稳定平台轴线9平行；
[0087] 步骤二：稳定平台运动时，主加速度计实时测量所述主加速度计所在处稳定平台的三轴加速度分量，副加速度计实时测量所述副加速度计所在处稳定平台的三轴加速度分量，三轴磁通门传感器测量所述三轴磁通门传感器所在处稳定平台的三轴磁场分量；陀螺仪实时测量所述陀螺仪所在点处围绕稳定平台轴线转动的角速率；
[0088] 步骤三：对主加速度计测量的Y轴测量值 和Z轴测量值 副加速度计测量的Y轴测量值 和Z轴测量值 三轴磁通门传感器测量的Y轴测量值by和Z轴测量值bz、陀螺仪测量的测量值ωg进行前置滤波处理，滤除噪声后得到估计值
[0089] 步骤四：判定主加速度计和副加速度计的工作状态以及井下工况的振动情况；若主加速度计和副加速度计均正常工作且振动强度处于正常范围时，采用双加速度计重力工具面角解算方法解算得到重力工具面角βa；若主加速度计和副加速度计中某一个故障且振动强度处于正常范围时，采用单加速度计重力工具面角解算方法解算得到重力工具面角βg；若主加速度计和副加速度计均故障或振动强度超过正常范围时，判断三轴磁通门传感器是否正常工作，若三轴磁通门传感器正常工作，采用磁通门重力工具面角解算方法解算得到重力工具面角βb，若三轴磁通门传感器故障，则工具面角无法正常解算，需起钻维修；
[0090] 步骤五：判定陀螺仪是否工作正常，若陀螺仪工作正常，通过互补滤波融合步骤四解算的重力工具面角βa或βg或βb和陀螺仪测量值ωg的估计值 得到互补滤波器重力工具面角βc，将互补滤波器重力工具面角βc作为最终重力工具面角测量值β，若陀螺仪故障，则直接采用步骤S4解算的重力工具面角βa或βg或βb作为最终重力工具面角测量值β。
[0091] 本发明上述测量方法，采用双加速度计、三轴磁通门传感器和陀螺仪测量的数据，在不同工况和不同传感器故障时自动判断传感器故障并切换至备用方案进行测量，提高了重力工具面角测量的可靠性；通过互补滤波器融合重力工具面角和陀螺仪对地速率，提高重力工具面角的动态测量精度。
[0092] 上述测量方法的步骤四中，双加速度计重力工具面角解算方法解算得到重力工具面角的具体步骤如下：
[0093] 记稳定平台半径为R，主加速度计Y轴和Z轴的测量误差项为 副加速度计Y轴和Z轴的测量误差项为 则主加速度计Y轴和Z轴的测量值 以及副加速度计Y轴和Z轴的测量值 满足如下测量方程：
[0094]
[0095] 式中，g为重力加速度，βa为重力工具面角，为稳定平台转动角加速度，ω为稳定平台转动角速率，γ为副加速度计的XZ轴平面与主加速度计的XZ轴平面的夹角，0°<γ≤360°；
[0096] 忽略测量的误差项，整理公式(1)得到：
[0097]
[0098] 通过解算公式(2)得到重力工具面角βa。
[0099] 由于γ值需要满足便于计算和易于实现敏感轴冗余的要求，对γ值选择做一定限制。当γ＝90°,180°,270°时，主加速度计Y轴和Z轴的测量值 以及副加速度计Y轴和Z轴的测量值 之间存在固定的数值关系，当γ≠90°,180°,270°时，主加速度计Y轴和Z轴的测量值 以及副加速度计Y轴和Z轴的测量值 之间的关系将受到重力工具面角影响，不能实现敏感轴冗余。
[0100] 当γ＝90°时，经过前置滤波处理后的主加速度计Y轴和Z轴的估计值 以及副加速度计Y轴和Z轴的估计值 之间存在如下冗余关系：
[0101]
[0102] 则测量方程表示为：
[0103]
[0104] 式中，ayf表示经过主加速度计和副加速度计数据融合后的Y轴测量值，azf表示经过主加速度计和副加速度计数据融合后的Z轴测量值；
[0105] 通过解算公式(4)得到重力工具面角βa；
[0106] 当γ＝180°时，经过前置滤波处理后的主加速度计Y轴和Z轴的估计值 以及副加速度计Y轴和Z轴的估计值 之间存在如下冗余关系：
[0107]
[0108] 则测量方程表示为：
[0109]
[0110] 通过解算公式(6)得到重力工具面角βa；
[0111] 当γ＝270°时，经过前置滤波处理后的主加速度计Y轴和Z轴的估计值 以及副加速度计Y轴和Z轴的估计值 之间存在如下冗余关系：
[0112]
[0113] 则测量方程表示为：
[0114]
[0115] 通过解算公式(8)得到重力工具面角βa。
[0116] 上述测量方法的步骤四中，单加速度计重力工具面角解算方法解算得到重力工具面角的具体步骤如下：
[0117] 在副加速度计故障的情况下，采用主加速度计估计值解算重力工具面角[0118] 在主加速度计故障的情况下，采用副加速度计估计值解算重力工具面角[0119] 上述测量方法的步骤四中，磁通门重力工具面角解算方法解算得到重力工具面角的具体步骤如下：
[0120] 记稳定平台所在位置地磁场总强度为B，静态测量的井斜角为α、井斜方位角为和磁倾角为D；
[0121] 定义钻井导向工具的当前姿态 是从初始位置按照沿三轴磁通门传感器的Z轴改变井斜方位角 然后绕三轴磁通门传感器的Y轴改变井斜角α，最后绕三轴磁通门传感器的X轴改变重力工具面角βb得到；三次转动对应的姿态变换矩阵Mz，My和Mx如下:
[0122]
[0123] 记： 则有：
[0124]
[0125] 当by2≠0或bz2≠0时式(10)成立，式中，bx1,by1,bz1表示初始位置按照沿三轴磁通门传感器的Z轴改变井斜方位角 后的三轴磁场分量，bx2,by2,bz2表示绕三轴磁通门传感器的Y轴改变井斜角α的三轴磁场分量；
[0126] 通过解算公式(10)得到重力工具面角βb。
[0127] 参见图5，图5为陀螺仪测量的数据融合互补滤波器的结构图。加速度传感器或磁通门传感器噪声集中在高频段，陀螺仪噪声集中在低频段，因此，加速度传感器或磁通门传感器与陀螺仪之间具有互补的测量特性。本发明上述测量方法采用非线性互补滤波器，引入PI控制器减小测量值的误差和陀螺漂移，作为优选设计，上述测量方法的步骤五中，通过互补滤波融合重力工具面角βa或βg或βb和陀螺仪测量值ωg的估计值 得到的互补滤波器重力工具面角βc，采用Laplace形式表示为：
[0128]
[0129] 式中，Kp和Ki为互补滤波器PI控制器参数，βi为重力工具面角βa或βg或βb， 为陀螺仪测量值ωg的估计值。融合后得到的重力工具面角，消弱了横向振动与扭转振动对重力工具面角的影响，使重力工具面角的测量更精确。
[0130] 上述测量方法的步骤五中，在互补滤波融合重力工具面角βa或βg或βb和陀螺仪测量值ωg的估计值 之前，通过重力工具面角预处理消除重力工具面角的跳变，其具体步骤为：
[0131] 记：
[0132]
[0133] 式中，βi为重力工具面角βa或βg或βb，Δe和β1为预处理的中间变量，e为预处理输出的偏差值，互补滤波器重力工具面角βc输出前需经过重力工具面角值域处理，使得βc位于[0,360]值域范围内。
[0134] 参见图2，本发明还提供了一种旋转导向稳定平台重力工具面角动态测量装置，包括：
[0135] 主加速度计1，安装于稳定平台12上，用于测量所述主加速度计1所在处稳定平台12的三轴加速度分量，所述主加速度计1的XZ轴平面与钻井导向工具的装置面重合；
[0136] 副加速度计2，安装于稳定平台12上，用于测量所述副加速度计2所在处稳定平台12的三轴加速度分量，所述副加速度计2的XZ轴平面与所述主加速度计1的XZ轴平面呈夹角；
[0137] 三轴磁通门传感器3，安装于稳定平台12上，用于测量所述三轴磁通门传感器3所在处稳定平台12的三轴磁场分量，所述三轴磁通门传感器3位于主加速度计1的一端，且所述三轴磁通门传感器3的XZ轴平面与钻井导向工具的装置面重合；
[0138] 陀螺仪4，安装于稳定平台12上，用于测量所述陀螺仪4所在点处围绕稳定平台轴线9转动的角速率，所述陀螺仪4的敏感轴与所述稳定平台轴线9平行；
[0139] 前置滤波器模块，分别与所述主加速度计、所述副加速度计、所述三轴磁通门传感器、所述陀螺仪连接，用于消除测量噪声，得到陀螺仪测量值的估计值；
[0140] 与所述前置滤波器模块连接的振动强度判定单元，用于判断稳定平台当前的振动强度；
[0141] 与所述振动强度判定单元连接的故障判断单元，用于判断所述主加速度计、所述副加速度计、所述三轴磁通门传感器和所述陀螺仪是否故障；
[0142] 与所述故障判断单元连接的加速度数据融合单元，所述加速度数据融合单元包括Y轴数据融合模块和Z轴数据融合模块；
[0143] 与所述加速度数据融合单元连接的加速度重力工具面角解算单元，所述加速度重力工具面角解算单元包括双加速度重力工具面角解算模块和单加速度重力工具面角解算模块，用于解算重力工具面角；
[0144] 与所述故障判断单元连接的磁通门重力工具面角解算单元，用于解算重力工具面角；
[0145] 分别与所述加速度重力工具面角解算单元和所述磁通门重力工具面角解算单元连接的互补滤波数据融合单元，用于融合所述加速度重力工具面角解算单元或所述磁通门重力工具面角解算单元解算得到的重力工具面角与陀螺仪测量值的估计值得到重力工具面角。
[0146] 对上述测量装置进行优选设计，所述互补滤波数据融合单元包括重力工具面角预处理模块和与重力工具面角预处理模块连接的互补滤波器，所述重力工具面角预处理模块分别与所述加速度重力工具面角解算单元和所述磁通门重力工具面角解算单元连接，用于消除重力工具面角的跳变。
[0147] 为了便于不同传感器的安装，对上述测量装置进行优选设计，参见图2和图3，所述稳定平台上设有主加速度计安装槽5、副加速度计安装槽6、三轴磁通门传感器安装槽7和陀螺仪安装槽8，所述副加速度计安装槽设有多个，用于改变所述副加速度计与主加速度计之间夹角的角度。根据实际布线和结构设计的要求改变副加速度计的安装位置，可以更改夹角的角度。参见图3，作为优选，所述副加速度计安装槽设置有三个，分别是一个副加速度计安装槽6和两个副加速度计备选安装槽10。
[0148] 继续参见图2，所述主加速度计安装槽5和所述三轴磁通门传感器安装槽6布置在钻井导向工具的装置面与稳定平台侧面的交线上，所述陀螺仪安装槽8布置在所述副加速度计安装槽6的正上方。
[0149] 继续参见图2，所述主加速度计1的X轴与所述稳定平台轴线9平行，所述主加速度计1的Z轴指向所述稳定平台的自转轴。
[0150] 继续参见图2，所述三轴磁通门传感器3的X轴与所述稳定平台轴线9平行，所述三轴磁通门传感器3的Z轴指向所述稳定平台的自转轴。
[0151] 实施例1：在本实施例中，主加速度计和副加速度计之间的夹角选择γ＝180°的布置方式，稳定平台水平安装在固定架上。参见图4，稳定平台转速在±2.5rps间变化。当稳定平台转速由+2.5rps变化到-2.5rps或由-2.5rps变化到+2.5rps时，运动加速度对Y轴和Z轴的测量值影响较大。Y轴数据处理前后加速度测量值的差值约为2.8g，Y轴测量值波动明显减小，经过双加速度数据融合方法处理后，有效抑制了运动加速度对测量值的影响。Z轴加速度测量值在数据融合前后的均值相差约为0.63g，根据稳定平台转速计算得到离心加速度约为0.628g，双加速度数据融合方法较好的削弱了离心加速度对Z轴测量值的影响。
[0152] 实施例2：在本实施例中，参见图6，稳定平台以1rps匀速转动，采用常规处理方法测量重力工具面角时得到的误差e不连续。但是稳定平台自身姿态变化是连续的，经过本发明测量方法测量的重力工具面角经过重力工具面角预处理，保留了有用的偏差，消除了不连续跳变。
[0153] 以上所举实施例仅用为方便举例说明本发明，并非对本发明保护范围的限制，在本发明所述技术方案范畴，所属技术领域的技术人员所作各种简单变形与修饰，均应包含在以上申请专利范围中。
