一种基于互补滤波的钻具重力加速度提取方法与流程

一种基于互补滤波的钻具重力加速度提取方法
【技术领域】
1.本发明涉及一种重力加速度提取方法，具体涉及一种基于互补滤波的钻具重力加速度提取方法。


背景技术：

2.在煤矿井下钻孔施工过程中，由于煤岩层赋存状况变化、施工工艺等原因，会导致实际钻孔偏离原设计方向，造成安全隐患。随钻测量(measurement while drilling，mwd)通过在钻进过程中连续不断检测有关钻头的姿态信息，从而来指导钻头按照设计方向钻进，实现钻具轨迹的实时调整。目前常用的随钻姿态测量系统普遍采用磁力计和加速度计组合方案，这种方法测量原理简单，但钻具振动会带来巨大的测量误差，通常该方法测量前需要先让钻机停止运行。使用这种测量方法不仅影响钻井效率，而且由于相邻测量点距离过大，钻头调整周期变长，从而导致井眼轨迹偏离设计轨迹，每个测量点之间的井眼轨迹也只能通过数学方法计算得出，因此无法获得精确的井眼轨迹。
3.针对传统随钻测量工具不能克服底部钻具在工作中的振动和冲击等问题，目前主流的研究思路是先从加速度计中提取重力加速度信息，然后再进行姿态解算。对于重力加速度信息的提取主要分成两类研究思路，一类是利用重力加速度信息和由振动、冲击等带来的非重力加速度信息在频域、时域上的特征差异将其区分。在这一类方法中，mushfiqul alam等使用小波阈值法重构加速度计中的重力信息，提出一种可变带宽的加速度计自适应滤波方法，利用正弦基的重力加速度信息频带估计方法来不断调整滤波带宽，预先滤除信号频带外振动信号噪声。但该方法要求振动加速度频带和重力加速度频带分离，且小波基函数的选择也比较困难。yang h和luo t等在小波去噪的基础上提出先使用经验模态分解来滤除传感器高频分量，再用小波对其重构。张文秀等利用重力加速度和振动信号在时域上互不相关的特点，提出了一种基于互相关检测的重力加速度信号提取方法，但是该方法需要假设振动和冲击加速度为随机信号，且参考信号选取比较困难，对工作环境适应性不强。
4.另一类是基于多传感器融合的思路，如gabriele ligorio等以基于陀螺仪的重力加速度递推方程和非重力加速度的高斯马尔科夫模型作为预测方程，以加速度计信号作为观测量，在卡尔曼滤波框架中融合得到重力加速度信息；yang y和li f等提出的一种自适应衰减平方根无迹卡尔曼滤波方法，在ukf中引入衰减因子，实时调整滤波器增益，有效将加速度计的振动信号解耦。但是该算法限制条件较多，需要钻具工作时噪声满足高斯分布。yang j等基于科里奥利方程建立了重力加速度的预测方程。杨金显等为进一步提高系统的鲁棒性，选取基于陀螺仪重力加速度递推方程作为主滤波器，基于加速度计、磁力计观测的重力加速度作为子滤波器，在联邦卡尔曼融合框架中得到重力加速度信息。但由于重力加速度提取模块只是随钻测量系统中的一个数据预处理模块，而且由于井下对于电子设备功率的限制，单片机无法提供足够的计算能力，上述基于卡尔曼滤波框架的重力加速度提取算法计算量大，难以在嵌入式设备上运行。


技术实现要素：

5.为了解决上述问题，本发明针对基于信号特征差异的信号处理算法无法有效提取重力加速度，基于卡尔曼滤波的多传感器融合算法难以实时计算的问题，提供一种基于互补滤波的钻具重力加速度提取方法，采用互补滤波器对陀螺仪和加速度计的数据进行融合，以滤除近钻头的振动加速度干扰，同时为了提高对钻具不同运动状态的适应能力，引入模糊算法来动态调整互补滤波器参数，通过模糊算法综合判断钻具所处状态，动态调整互补滤波器的截止频率，利用该方法提取出的重力加速度信号进行姿态解算可有效的提高姿态角的精度和稳定性，其在单片机上运行时间仅为0.8496ms，有利于保证姿态解算的实时性，具有较好的工程应用价值。
6.本发明是通过以下技术方案实现的，提供一种基于互补滤波的钻具重力加速度提取方法，包括以下步骤：
7.s1构建坐标系；
8.s2构建重力加速度递推方程；
9.s3构建互波滤补模型；
10.s4基于模糊算法计算自适应系数；
11.s5根据s3构建互波滤补模型、s4计算的自适应系数，结合s2的重力加速度递推方程，即可获得重力加速度。
12.特别的，所述s1具体按照以下方案实施：
13.选取地理坐标系以北东地为顺序构成的右手直角坐标系作为导航坐标系，用o-x
nynzn
表示，其中xn指向北，yn指向东，zn指向地心；以钻具的三个基本轴线建立钻具坐标系，用o-xbybzb表示，其中xb与钻具轴线重合，yb和zb相互垂直且构成的平面与钻具轴线垂直；所述地理坐标系中，井斜角θ为钻具钻进轴xb与水平面的夹角，向下为正，反之为负，工具面向角γ为钻孔横截面内由钻孔高边到yb的夹角，其中θ∈(-90
°
,90
°
)，γ∈(-180
°
,180
°
)。
14.特别的，所述s2具体按照以下方案实施：
15.s21根据坐标系，设从n系到b系的姿态旋转矩阵为旋转矩阵是随时间变化的，其变化规律用微分方程来描述，所述旋转矩阵的微分方程如下：
[0016][0017]
于公式(1)中，bω
×
为钻具坐标系相对导航坐标系的角速度矢量构成的反对称矩阵，按如下公式计算：
[0018][0019]
s22设[t
k-1
,tk]时间段的陀螺输出角增量为则公式(1)微分方程的齐次线性解如下：
[0020]
[0021]
将公式(3)两边同时乘以导航坐标系下的重力加速度ng＝[0 0
ꢀ‑
g0]
t
，得到载体系下的重力加速度递推方程如下：
[0022][0023]
s23引入单子样 前一周期的等效旋转矢量补偿不可交换误差，得到是重力加速度递推方程如下：
[0024][0025]
于公式(5)中，φk为等效旋转矢量，角增量δθk＝δt
·
[bωk×
]。
[0026]
特别的，所述s3具体按照以下方案实施：
[0027]
s31构建陀螺仪和加速度计的测量模型：
[0028][0029]
公式(6)中，byk是陀螺仪测量值，bω
t,k
是k时刻的陀螺实际角速度，bω
b,k
为陀螺仪的零偏，ηg为陀螺仪的低频噪声；bfk是加速度计测量值，
bak
为载体的重力加速度，ηa为加速度计的高频噪声；
[0030]
s32通过低通和高通滤波器构建互补滤波器{l(s)h(s)},取低通滤波器高通滤波器使用高通滤波器h(s)对加速度计进行滤波，用低通滤波器l(s)对陀螺仪进行滤波，即得到互补滤波器模型
[0031]
特别的，所述互波滤补模型采用比例积分控制器进行调节，取其中k
p
的大小决定互补滤波的截止频率，当k
p
越小，陀螺仪起的作用越大，当k
p
越大，加速度计起的作用越大，ki是积分时间常数的倒数。
[0032]
特别的，所述s4具体按照以下方案实施：
[0033]
s41使用加速度计测量值bfk减去陀螺仪测量值通过式(5)递推出的重力加速度得到非重力加速度如下：
[0034][0035]
定义一个长度为n的非重力加速度滑动窗口，取该滑动窗口的最大值作为非重力加速度vib的幅值如下：
[0036]
vibk＝max
i∈[k,k-n-1]
(f
non_gravity,i
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)；
[0037]
s42将旋转轴上的角速度经过低通滤波器后作为钻具的旋转角速度，所述旋转速度rot按如下公式计算：
[0038]
rotk＝αby
x(k)
(1-α)by
x(k-1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)；
[0039]
s43设计模糊算法。
[0040]
特别的，所述s43具体按照以下方案实施：
[0041]
s431输入量模糊化：
[0042]
设非重力加速度vib和旋转速度rot的模糊量的模糊子集都为{s,m,l}，其中s、m、l分别代表小、中、大，将模糊算法的输出δk
p
的模糊子集设置为{nm,ns,z,ps,pm}，其中nm、ns、z、ps、pm分别代表负大、负小、零、正小、正大，通过实验得到vib、rot、δk
p
对应的模糊论域为：
[0043][0044]
选用隶属函数将输入量模糊化，得到输入的模糊量和该模糊量所属的隶属度；
[0045]
s432建立模糊规则表和模糊推理，所述模糊规则表按以下原则设定：
[0046]
当vib较小时，增大δk
p
以增加对加速度计的信任，当rot和vib都比较大时，减小δk
p
以增加对陀螺仪的信任；
[0047]
所述模糊推理的规则为：
[0048]
if(vib is vib)and(rot is rot)then(δk
p is kp)；
[0049]
s433反模糊化
[0050]
使用重心法解模糊器对模糊推理后的结果进行解模糊，得到互补滤波器参数的补偿值δk
p
，其按如下公式计算：
[0051][0052]
于公式(11)中，δkpi是经过模糊推理得到的模糊量，δkpi∈kp，μ
kp
为隶属函数。
[0053]
本发明提供一种基于互补滤波的钻具重力加速度提取方法，利用钻具旋转速度和非重力加速度幅值作为模糊算法输入，通过模糊算法综合判断钻具所处状态，动态调整互补滤波器的截止频率。解决了井下近钻头动态姿态测量过程中由于高速旋转、剧烈振动而导致加速度计不能测量出真实的钻具重力分量的问题。利用该方法提取出的重力加速度信号进行姿态解算可以有效的提高姿态角的精度和稳定性，且该方法具有良好的实时性，在单片机上运行时间仅为0.8496ms，有利于保证姿态解算的实时性，因此具有较好的工程应用价值。
【附图说明】
[0054]
图1为本发明随钻测量系统传感器和坐标系；
[0055]
图2为本发明三角型模糊隶属函数；
[0056]
图3为本发明流程框图；
[0057]
图4为提取重力前后井斜角/工具面角对比图；
[0058]
图5为振动实验中不同重力提取算法对比图；
[0059]
图6为振动实验中不同算法的重力信号姿态解算图。
【具体实施方式】
[0060]
为了使本发明的目的、技术方案及优点更加清楚明白，以下对本发明进一步详细说明。
[0061]
请参阅图1-图6，本发明提供一种基于互补滤波的钻具重力加速度提取方法，其利用加速度计和陀螺仪在频域上的互补特性，采用互补滤波器对二者数据进行融合以滤除近钻头的振动加速度干扰，同时为了提高对钻具不同运动状态的适应能力，引入模糊算法来动态调整互补滤波器参数。具体包括以下步骤：
[0062]
s1构建坐标系
[0063]
如图1，选取地理坐标系以“北东地”为顺序构成的右手直角坐标系作为导航坐标系(n系)，其用o-x
nynzn
表示，xn指向北，yn指向东，zn指向地心；以钻具的三个基本轴线建立钻具坐标系(b系)，用o-xbybzb表示，xb与钻具轴线重合，yb和zb相互垂直且构成的平面与钻具轴线垂直。
[0064]
随钻测量中三个基本参数量为井斜角、方位角、工具面角，其中井斜角θ(θ∈(-90
°
,90
°
))为钻具钻进轴xb与水平面的夹角，此处定义向下为正，反之为负；工具面向角γ(γ∈(-180
°
,180
°
))为钻孔横截面内由钻孔高边到yb的夹角；方位角φ(φ∈(0
°
,360
°
))为钻具在水平面投影与导航系xn轴的夹角；由于重力加速度并不能直接解算出方位角，因此本发明仅关注井斜角和工具面角。
[0065]
s2重力加速度提取算法
[0066]
在s1坐标系定义下，设从n系到b系的姿态旋转矩阵为旋转矩阵是随时间变化的，其变化规律用如下微分方程来表示：
[0067]
旋转矩阵的微分方程为：
[0068][0069]
其中bω
×
为钻具坐标系相对导航坐标系的角速度矢量构成的反对称矩阵，采用如下公式表示：
[0070][0071]
设[t
k-1
,tk]时间段的陀螺输出角增量为公式(1)微分方程的齐次线性解为：
[0072][0073]
将公式(3)两边同时乘以导航系(n系)下的重力加速度ng＝[0 0
ꢀ‑
g0]
t
，得到载体系下的重力加速度递推方程如下：
[0074][0075]
由于打钻过程中的旋转和振动会使得钻具处在高动态环境中，因此钻具不满足定
轴转动的假设，若直接假设角增量δθk≈δt
·
[bωk×
]，然后对式(4)进行离散化，则会引入不可交换误差降低重力加速度的解算精度，为此，此处引入单子样 前一周期的等效旋转矢量补偿不可交换误差，于是得到重力加速度递推方程为：
[0076][0077]
于公式(5)中，φk为等效旋转矢量，角增量δθk＝δt
·
[bωk×
]。
[0078]
s3构建mahony互补滤波模型
[0079]
将陀螺仪测量值代入公式(5)中，可以递推出钻具的重力加速度，这种方法动态响应特性良好，对外部干扰的适应性强，受振动影响小，但该过程需要不断积分，而积分会放大陀螺仪的低频误差。直接采用加速度计测量的方法静态响应良好，但当载体发生加速或者减速运动时候会引入运动加速度。动态特性对应高频，静态特性对应低频，这两种方法在频率上互补，本发明使用互补滤波算法融合这两种方法的测量优点。
[0080]
s31构建陀螺仪和加速度计的测量模型：
[0081][0082]
公式(6)中，byk是陀螺仪测量值，bω
t,k
是k时刻的陀螺实际角速度，bω
b,k
为陀螺仪的零偏，ηg为陀螺仪的低频噪声；bfk是加速度计测量值，
bak
为载体的重力加速度，ηa为加速度计的高频噪声；
[0083]
2、s32通过低通和高通滤波器构建互补滤波器{l(s) h(s)},取低通滤波器高通滤波器使用高通滤波器h(s)对加速度计进行滤波，用低通滤波器l(s)对陀螺仪进行滤波，即可得到互补滤波器模型。在此模型中，使用比例积分控制器对互补滤波器进行调节，取其中k
p
的大小决定互补滤波的截止频率，当k
p
越小，陀螺仪起的作用越大；当k
p
越大，加速度计起的作用越大；ki是积分时间常数的倒数，积分环节的引入是为了消除陀螺中的零偏。
[0084]
s4基于模糊算法的自适应系数
[0085]
由于钻具的运动状态比较复杂，其运动状态的改变会影响传感器的噪声分布，公式(6)中ηa就包含振动和旋转带来的运动加速度，而钻具的旋转速度变化会改变加速度计中噪声的频率分布，所以单一的预设参数无法使得滤波器的估计结果达到最优。
[0086]
在实际工作中，钻具的运动状态并不会对陀螺仪的零偏bω
b,k
造成太大影响，所以ki预设为常数，通过k
p
的动态调整实现对钻具运动状态的自适应。而钻具的旋转速度会影响非重力加速度的频率，旋转速度越快，非重力加速度频率越大，但当钻头空载时振动加速度影响较小，所以引入传感器检测到的非重力加速度的幅值综合判断钻具运动状态。基于此，为了提高本算法对钻具运动状态的适应性，本发明通过模糊算法不断检测非重力加速度幅值vib和钻具的旋转速度rot，利用模糊规则表在线对互补滤波器的k
p
参数进行调整，定义δk
p
为模糊算法的输出。
[0087]
s41非重力加速度计算
[0088]
使用加速度计测量值bfk减去陀螺仪测量值通过式(5)递推出的重力加速度得到非重力加速度为：
[0089][0090]
定义一个长度为n的非重力加速度滑动窗口，取该滑动窗口的最大值作为非重力加速度的幅值：
[0091]
vibk＝max
i∈[k,k-n-1]
(f
non_gravity,i
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)。
[0092]
s42旋转速度计算
[0093]
将旋转轴上的角速度经过低通滤波器后作为钻具的旋转角速度：
[0094]
rotk＝αby
x(k)
(1-α)by
x(k-1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)。
[0095]
s43模糊算法设计
[0096]
(1)输入量模糊化
[0097]
设非重力加速度vib和旋转速度rot的模糊量的模糊子集都为{s,m,l}，其中s、m、l分别代表小、中、大；将模糊算法的输出δk
p
的模糊子集设置为{nm,ns,z,ps,pm}，其中nm、ns、z、ps、pm分别代表负大、负小、零、正小、正大，通过实验确定vib、rot、δk
p
对应的模糊论域为：
[0098][0099]
选用图2所示的三角型函数作为隶属函数，使用图2(a)和图2(b)的隶属函数将输入量模糊化，即得到输入的模糊量和该模糊量所属的隶属度。
[0100]
(2)建立模糊规则和模糊推理
[0101]
其中规则表设置的基本原则为：当vib较小时，增大δk
p
以增加对加速度计的信任；当rot和vib都比较大时，减小δk
p
以增加对陀螺仪的信任；结合实验分析最终制定的模糊规则如表1所示。
[0102]
表1模糊规则表
[0103][0104]
在模糊推理时认为模糊规则表中的每条规则的权重相等，and计算选用隶属度最小的作为δkpi的隶属度，模糊推理的规则为：
[0105]
if(vib is vib)and(rot is rot)then(δk
p is kp)
[0106]
(3)反模糊化
[0107]
使用重心法解模糊器对模糊推理后的结果进行解模糊，得到互补滤波器参数的补偿值δk
p
，如下公式：
[0108][0109]
公式(11)中，δkpi是经过模糊推理得到的模糊量，δkpi∈kp，μ
kp
对应图2(c)的隶属函数。
[0110]
综上所述，本发明提出的重力加速度提取算法总体框图如图3所示，其中是通过公式(5)得到的递推重力加速度，用来和bfk做叉乘计算误差向量be
g(k)
，是经过校正后的陀螺仪值再次经过公式(5)递推得到的滤波后的重力加速度矢量。
[0111]
为了证实本发明所述方法具有可行性，以下通过旋转实验检验本发明所述方法的准确性，通过振动实验检验本发明所述方法的稳定性。实验过程中分别将传感器模块放置在旋转实验台和振动实验台上，选用xsens公司的mti-3-8a7g6t模块记录传感器模块中加速度计和陀螺仪的原始信号，传感器模块采样频率为100hz。之后再将传感器原始信号导入到matlab进行分析和实验。
[0112]
一、旋转实验
[0113]
旋转实验以传感器模块的y轴做为旋转轴，将传感器平行固定在旋转台上。调整旋转台的转速为163rpm/min，实验时间为1分20秒。记录加速度计和陀螺仪的输出值，运行本发明提供方法算法提取重力加速度信号。将提取后的重力加速度用于井斜角和工具面角的解算。该实验对比使用提取重力加速度和不提取重力加速度分别结合陀螺仪信号进行井斜角和工具面角的解算，姿态解算方法使用基于余弦矩阵的ekf算法，对比结果如图4所示。
[0114]
由图4上方图井斜角可以看出，当以井斜角恒等于0
°
为基准，不提取重力加速度的井斜角误差在3.5
°
左右，提取重力加速度后的井斜角误差在1.0
°
以内，误差明显减小。从下方图工具面向角可以看到，提取重力加速度前后解算出的工具面向角在相位上保持高度重合，并没有出现因为引入重力提取算法而使得数据计算滞后的情况发生。由此可见采用本发明提取重力加速度信号后，计算出的井斜角相比于原始加速度信号解算的井斜角波动范围明显缩小，更接近于实际测试条件下的角度。
[0115]
二、振动实验
[0116]
振动实验中将传感器的井斜角控制为-7.4
°
，工具面向角控制为0
°
。设置振动幅度为5倍重力加速度，将振动台的振动频率设置成从10hz上升到200hz，再从200hz下降到10hz的形式，振动时间为5分32秒。记录加速度计和陀螺仪的原始信号。在振动台上，模块三个轴的重力分量分别为一个常量，对每个轴分别运行本发明以及对比算法(kalman框架的重力加速度提取算法)，对比结果如图5所示，此时振动台的振动频率为13hz。
[0117]
由图5中可以看出，虽然本发明和基于kalman的重力加速度提取算法均能有效从含有大量振动加速度的原始信号中提取出重力加速度信息，相较于基于kalman的重力加速度提取算法，本发明提取出的重力加速度信号要更加的稳定。
[0118]
为了进一步验证重力加速度提取的稳定性，继续保持传感器模块z轴朝上，井斜角
为-7.4
°
，工具面向角为0
°
的姿态，分别利用加速度计原始信号、通过发明提取出的重力加速度信号、通过kalman滤波提取出的重力加速度信号结合陀螺仪原始信号进行井斜角和工具面角的解算。计算出的姿态角对比结果如图6所示。
[0119]
在振动过程中，模块的姿态是没有改变的，所以解算出的模块姿态应该要一直保持井斜角为-7.4
°
，工具面向角为0
°
的姿态。由图6可以看出，在100个采样点(1秒)的周期内，使用加速度计原始信号解算出的井斜角漂移了0.3
°
，使用经过kalman滤波后的重力加速度解算出的井斜角漂移大于0.05
°
，而使用本发明提取的重力加速度解算出的井斜角基本没有漂移，由此可见，本发明较kalman算法解算的井斜角更加稳定。而对于工具面向角，使用加速度计原始信号解算漂移了0.5
°
，不能满足工程使用要求，而本发明和kalman算法表现接近，漂移均控制在0.1
°
之内。
[0120]
为进一步量化三种方法解算出姿态角的稳定性，计算5分32秒内，共31954个采样点，三种方法在井斜角为-7.4
°
的误差均值和标准差。
[0121]
表2三种方法对于井斜角的误差参数
[0122]
方法误差均值误差标准差未提取重力加速度0.82690.6586本发明0.00680.0379kalman滤波算法0.09020.0485
[0123]
由表2可以看出，使用本发明和kalman滤波算法提取出的重力加速度都能提高姿态角解算精度和稳定性。相较于kalman滤波算法，本发明计算出的井斜角的误差均值更小。
[0124]
以pc机的matlab软件为测试平台，分别运行本发明和基于kalman框架的重力提取算法，在matlab中循环运行两种算法1000次，计算两种方法的单次运行时间。以stm32f103zet6为测试平台，晶振的频率为8mhz，分别运行两种算法，计算两种方法的单次运行时间。运行时间对比如表3所示。
[0125]
表3单次运行时间对比
[0126][0127]
由表3可以看出，pc机的测试结果显示：本发明相较于基于kalman滤波框架的重力加速度提取算法单次运算时间减少了31.9％，由此可见，采用本发明提取重力加速度后，姿态估计精度得到明显提升％。在stm32f103zet6单片机的测试环境中本文算法相比于基于kalman滤波框架的重力加速度提取算法单次运算时间减少了80.4％。又由于kalman滤波中有矩阵乘法和矩阵求逆的过程，而stm32f103zet6单片机中没有浮点运算单元，所以在单片机的测试结果中基于kalman框架的重力提取算法相比本发明表现更差。
[0128]
实验结果表明，利用本发明提取出的重力加速度信号进行姿态解算可以有效的提高姿态角的精度和稳定性。同时本发明具有良好的实时性，在单片机上运行时间仅为0.8496ms时，有利于保证姿态解算的实时性，因此具有较好的工程应用价值。
[0129]
综上所述，为解决井下近钻头动态姿态测量过程中由于钻具高速旋转、剧烈振动而导致加速度计不能测量出真实的钻具重力分量的问题，本发明提供的一种基于互补滤波的钻具重力加速度提取方法。首先通过余弦矩阵微分方程得到基于陀螺仪的重力加速度的递推方程，然后利用陀螺仪和加速度计在频率上互补的特性，在互补滤波框架下融合得到钻具重力加速度；同时，为了提高系统鲁棒性，利用非重力加速度和钻具旋转速度作为模糊算法输入，通过模糊算法动态调整互补滤波参数。