CN104792301A - 一种钻孔电视探头受铁磁干扰时的方位矫正方法及装置 - Google Patents

Abstract

本发明公开了一种钻孔电视探头受铁磁干扰时的方位矫正方法，包括：(1)如果三维电子罗盘测量的三轴旋转角度超过设定正向阈值，则将三维电子罗盘测量的角度作为方位角，如果超过，则转步骤(2)；(2)获得三轴陀螺仪测量的三轴旋转角速率，并利用测得的三轴旋转角速率进行积分计算得到三轴旋转角的变化值；(3)利用三轴陀螺仪的三轴旋转角变化值及三轴陀螺仪三轴旋转角度计算初值求得当前时刻的在三维电子罗盘坐标系下的矫正方位角；(4)继续判断，如果三维电子罗盘测量的三轴旋转角度不低于设定的负向阈值，则继续步骤(3)将矫正方位角作为使用的方位角，如果低于负向阈值，则将三维电子罗盘测量三轴旋转角度作为使用的方位角。

Description

一种钻孔电视探头受铁磁干扰时的方位矫正方法及装置

技术领域

本发明属于钻孔成像技术领域，更具体地，涉及一种钻孔电视探头受铁磁干扰时的方位矫正方法及装置，能够矫正钻孔电视探头在铁磁物质中的方位测量。

背景技术

钻孔电视成像法是一种新兴的地质检测方法，相对于传统的钻孔取芯、超声波检测等检测方式，具有直观、效率高、功能强大、便捷等优点，已得到广泛的应用，然而在房屋桥梁等大型建筑物的基桩检测、管桩检测等领域并未得到大范围的应用，造成该情况的一个重要原因是：在实际工程中，尤其是在钢筋混凝土结构建筑的地基勘测过程中，钻孔成像设备的探头在靠近铁磁性物质(如地下钢筋)的区域时，铁磁性物质产生的被探头下放过程激发的反向磁场会干扰探头内部的三维电子罗盘，使其无法向系统报告正确的方位信息，导致图像处理质量降低，这成为了长期困扰业界的一大技术难题。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种钻孔电视探头受铁磁干扰时的方位矫正方法，其目的在于利用三轴陀螺仪对三维电子罗盘测得的方位角进行修正，由此解决钻孔电视探头在靠近铁磁性物质时的测得的方位角错误的技术问题。

为实现上述目的，按照本发明的一个方面，提供了一种钻孔电视探头受铁磁干扰时的方位矫正方法，包括如下步骤：

(1)利用三维电子罗盘测量三轴旋转角度，判断三轴旋转角度是否超过设定的正向阈值，如果未超过，则将三维电子罗盘测量的角度作为方位角，如果超过阈值，则转步骤(2)；

(2)获得三轴陀螺仪测量的三轴旋转角速率，并利用测得的三轴旋转角速率进行积分计算得到三轴旋转角的变化值；

(3)根据三维电子罗盘与三轴陀螺仪之间的坐标转换关系，利用三轴陀螺仪的三轴旋转角变化值及三轴陀螺仪三轴旋转角度计算初值求得当前时刻的在三维电子罗盘坐标系下的矫正方位角，其中三轴陀螺仪三轴旋转角度计算初值是三维电子罗盘方位角超过设定阈值之前一段设定时间的三轴角度值；

(4)继续判断，利用三维电子罗盘测量三轴旋转角度，判断三轴旋转角度是否低于设定的负向阈值，判断方法同步骤(1)的判断方法，如果未低于，说明三维电子罗盘还未脱离铁磁物质的干扰，则继续步骤(3)，将矫正方位角作为使用的方位角，如果低于负向阈值，说明三维电子罗盘已经脱离铁磁物质的干扰，则将三维电子罗盘测量三轴旋转角度作为使用的方位角。

在本发明的一个实施例中，所述步骤(1)中判断三轴旋转角度是否超过设定的正向阈值，具体为：

对三维电子罗盘的三轴旋转角速率进行判断，三维电子罗盘的三轴旋转角速率ω_x>ω_xmax，ω_y>ω_ymax，ω_z>ω_zmax时，认为三轴旋转角度超过设定的正向阈值；其中：

ω_xmax、ω_ymax、ω_zmax为设定的三轴旋转角速率的正向阈值；

${\mathrm{\ω}}_x=\frac{\mathrm{\Δ\α}}{\mathrm{\Δt}}$

${\mathrm{\ω}}_y=\frac{\mathrm{\Δ\β}}{\mathrm{\Δt}}$

${\mathrm{\ω}}_z=\frac{\mathrm{\Δ\γ}}{\mathrm{\Δt}}$

Δα、Δβ、Δγ分别表示在三维电子罗盘在自身坐标系下，第二姿态相对于第一姿态的三轴旋转角度，Δt为三维电子罗盘从第一姿态变化到第二姿态的时间间隔；

Δα＝α₂-α₁

Δβ＝β₂-β₁

Δγ＝γ₂-γ₁

其中α₂、β₂、γ₂为三维电子罗盘第二姿态时的三轴旋转角度值，α₁、β₁、γ₁为三维电子罗盘第一姿态时的三轴旋转角度值。

在本发明的一个实施例中，所述三轴旋转角的变化值计算方法如下：

在三轴陀螺仪自身坐标系下面，第二姿态相对于第一姿态的三轴旋转角变化值公式如下：

$\mathrm{\Δ}{\mathrm{\α}}^{\′}={\∫}_0^{\mathrm{\Δt}}f\left({w_x}^{\′}\right)\mathrm{dt}$

$\mathrm{\Δ}{\mathrm{\β}}^{\′}={\∫}_0^{\mathrm{\Δt}}f\left({w_y}^{\′}\right)\mathrm{dt}$

$\mathrm{\Δ}{\mathrm{\γ}}^{\′}={\∫}_0^{\mathrm{\Δt}}f\left({w_z}^{\′}\right)\mathrm{dt}$

Δt为三轴陀螺仪从第一姿态变化到第二姿态的时间间隔，w_x′、w_y′、w_z′分别是三轴陀螺仪的三轴旋转角速率。

在本发明的一个实施例中，所述三轴陀螺仪三轴旋转角度计算初值的计算方法如下：

设当前时刻为t，取三维电子罗盘在时刻t-t₁到t-t₂这段时间内(t₁>t₂>0)的三轴旋转角α,β,γ，采用最小二乘法建立时间和三轴旋转角之间的方程，该方程可用如下方式求出：

$\stackrel{\‾}{t}=\frac{{\mathrm{\Σ}}_{i=1}^n{t}_i}{n}$

$\stackrel{\‾}{\mathrm{\α}}=\frac{{\mathrm{\Σ}}_{i=1}^n{\mathrm{\α}}_i}{n}$

$\stackrel{\‾}{\mathrm{\β}}=\frac{{\mathrm{\Σ}}_{i=1}^n{\mathrm{\β}}_i}{n}$

$\stackrel{\‾}{\mathrm{\γ}}=\frac{{\mathrm{\Σ}}_{i=1}^n{\mathrm{\γ}}_i}{n}$

$k_{\mathrm{\α}}=\frac{{\mathrm{\Σ}}_{i=1}^n(t_i-\stackrel{\‾}{t})({\mathrm{\α}}_i-\stackrel{\‾}{\mathrm{\α}})}{{\mathrm{\Σ}}_{i=1}^n{(t_i-\stackrel{\‾}{t})}^2}$

$b_{\mathrm{\α}}=\stackrel{\‾}{\mathrm{\α}}-k_{\mathrm{\α}}*\stackrel{\‾}{t}$

$k_{\mathrm{\β}}=\frac{{\mathrm{\Σ}}_{i=1}^n(t_i-\stackrel{\‾}{t})({\mathrm{\β}}_i-\stackrel{\‾}{\mathrm{\β}})}{{\mathrm{\Σ}}_{i=1}^n{(t_i-\stackrel{\‾}{t})}^2}$

$b_{\mathrm{\β}}=\stackrel{\‾}{\mathrm{\β}}-k_{\mathrm{\β}}*\stackrel{\‾}{t}$

$k_{\mathrm{\γ}}=\frac{{\mathrm{\Σ}}_{i=1}^n(t_i-\stackrel{\‾}{t})({\mathrm{\γ}}_i-\stackrel{\‾}{\mathrm{\γ}})}{{\mathrm{\Σ}}_{i=1}^n{(t_i-\stackrel{\‾}{t})}^2}$

$b_{\mathrm{\γ}}=\stackrel{\‾}{\mathrm{\γ}}-k_{\mathrm{\γ}}*\stackrel{\‾}{t}$

α与t的关系式如下：

α＝k_α*t+b_α

β与t的关系式如下：

β＝k_β*t+b_β

γ与t的关系式如下：

γ＝k_γ*t+b_γ

进而利用该方程，求出当前时刻t的三轴旋转角度，作为三轴陀螺仪三轴旋转角度计算初值α₀，β₀，γ₀

α₀＝k_α*t+b_α

β₀＝k_β*t+b_β

γ₀＝k_γ*t+b_γ。

在本发明的一个实施例中，所述当前时刻的在三维电子罗盘坐标系下的矫正方位角的计算公式如下：

α＝α_b+α₀+Δα′

β＝β_b+β₀+Δβ′

γ＝γ_b+γ₀+Δγ′。

其中，三轴旋转角α,β,γ为当前时刻的在三维电子罗盘坐标系下的矫正方位角，α₀，β₀，γ₀为三轴陀螺仪三轴旋转角度计算初值，Δα′、Δβ′、Δγ′分别为三轴旋转角的变化值，α_b，β_b，γ_b为三轴陀螺仪和三维电子罗盘两个坐标系之间的旋转参数。

按照本发明的另一方面，还提供了一种钻孔电视探头受铁磁干扰时的方位矫正装置，所述方位矫正装置包括三维电子罗盘、三轴陀螺仪、通用异步收发传输器模块、集成电路总线模块、单片机以及上位机，其中：

所述三维电子罗盘，用于测量三轴旋转角度；

所述三轴陀螺仪，用于测量三轴旋转角速率；

所述通用异步收发传输器模块，用于实现三维电子罗盘和单片机之间的异步通信；

所述集成电路总线模块，用于实现三轴陀螺仪和单片机之间的实时数据传输；

所述单片机，用于与三维电子罗盘和三轴陀螺仪之间通信，并将数据通过无线局域网传输给上位机进行处理；

所述上位机，用于执行下述操作：

(1)利用三维电子罗盘测量三轴旋转角度，判断三轴旋转角度是否超过设定的正向阈值，如果未超过，则将三维电子罗盘测量的角度作为方位角，如果超过阈值，则转步骤(2)；

(2)获得三轴陀螺仪测量的三轴旋转角速率，并利用测得的三轴旋转角速率进行积分计算得到三轴旋转角的变化值；

(3)根据三维电子罗盘与三轴陀螺仪之间的坐标转换关系，利用三轴陀螺仪的三轴旋转角变化值及三轴陀螺仪三轴旋转角度计算初值求得当前时刻的在三维电子罗盘坐标系下的矫正方位角，其中三轴陀螺仪三轴旋转角度计算初值是三维电子罗盘方位角超过设定阈值之前一段设定时间的三轴角度值；

(4)继续判断，利用三维电子罗盘测量三轴旋转角度，判断三轴旋转角度是否低于设定的负向阈值，判断方法同步骤(1)的判断方法，如果未低于，说明三维电子罗盘还未脱离铁磁物质的干扰，则继续步骤(3)，将矫正方位角作为使用的方位角，如果低于负向阈值，说明三维电子罗盘已经脱离铁磁物质的干扰，则将三维电子罗盘测量三轴旋转角度作为使用的方位角。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，具有以下有益效果：

(1)在三维电子罗盘遇到铁磁性物质干扰时，利用三轴陀螺仪对三维电子罗盘测得的方位角进行修正，有效钻孔成像系统在遇到地下铁磁性物质较为集中的区域时三维电子罗盘失灵的问题；

(2)将钻孔电视的应用范围从水文等工程地质检测和石油勘探扩展到混凝土结构建筑物基桩检测、管桩检测等领域。

附图说明

图1是本发明钻孔电视探头受铁磁干扰时的方位矫正方法流程图；

图2是本发明中三维电子罗盘和三轴陀螺仪的空间直角坐标系示意图；

图3是本发明实施例中钻孔电视探头的结构示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，本发明提供了一种钻孔电视探头受铁磁干扰时的方位矫正方法，包括如下步骤：

(1)利用三维电子罗盘测量三轴旋转角度，判断三轴旋转角度是否超过设定的正向阈值，如果未超过，则将三维电子罗盘测量的角度作为方位角，如果超过阈值，则转步骤(2)；

具体地，判断方法是否超过正向阈值的方法如下：

设三维电子罗盘从第一姿态变化到第二姿态的时间间隔为Δt，并且计算出Δt这段时间内三轴旋转角度变化值Δα、Δβ、Δγ

Δα＝α₂-α₁

Δβ＝β₂-β₁

Δγ＝γ₂-γ₁

分别表示在三维电子罗盘在自身坐标系下，第二姿态相对于第一姿态的三轴旋转角度，其中α₂、β₂、γ₂为三维电子罗盘第二姿态时的三轴旋转角度值，α₁、β₁、γ₁为三维电子罗盘第一姿态时的三轴旋转角度值。于是，当Δt足够小的时候，可以近似计算得到

${\mathrm{\ω}}_x=\frac{\mathrm{\Δ\α}}{\mathrm{\Δt}}$

${\mathrm{\ω}}_y=\frac{\mathrm{\Δ\β}}{\mathrm{\Δt}}$

${\mathrm{\ω}}_z=\frac{\mathrm{\Δ\γ}}{\mathrm{\Δt}}$

当三维电子罗盘受到铁磁物质影响时，此时铁磁物质对三维电子罗盘的影响远大于地磁对它的影响，所以它所测量的三轴角度值会产生较大的变化，对三维电子罗盘的三轴旋转角速率进行判断，当三维电子罗盘的三轴旋转角速率ω_x>ω_xmax，ω_y>ω_ymax，ω_z>ω_zmax时，认为三维电子罗盘已经受到了铁磁物质的影响，其中ω_xmax、ω_ymax、ω_zmax为已经设定好的三轴旋转角速率的正向阈值。

(2)获得三轴陀螺仪测量的三轴旋转角速率，并利用测得的三轴旋转角速率进行积分计算得到三轴旋转角的变化值；

具体地，三轴旋转角的变化值计算方法如下：

设三轴陀螺仪从第一姿态变化到第二姿态的时间间隔为Δt，则在三轴陀螺自身坐标系下面，第二姿态相对于第一姿态的三轴旋转角变化值公式如下：

$\mathrm{\Δ}{\mathrm{\α}}^{\′}={\∫}_0^{\mathrm{\Δt}}f\left(w_x\right)\mathrm{dt}$

$\mathrm{\Δ}{\mathrm{\β}}^{\′}={\∫}_0^{\mathrm{\Δt}}f\left(w_y\right)\mathrm{dt}$

$\mathrm{\Δ}{\mathrm{\γ}}^{\′}={\∫}_0^{\mathrm{\Δt}}f\left(w_z\right)\mathrm{dt}$

(3)根据三维电子罗盘与三轴陀螺仪之间的坐标转换关系，利用三轴陀螺仪的三轴旋转角变化值及三轴陀螺仪三轴旋转角度计算初值求得当前时刻的在三维电子罗盘坐标系下的矫正方位角，其中三轴陀螺仪三轴旋转角度计算初值是三维电子罗盘方位角超过设定阈值之前一段设定时间的三轴角度值。

具体地，三维电子罗盘与三轴陀螺仪之间的坐标转换关系推导方法如下：

如图2所示，设三维电子罗盘和三轴陀螺仪所在的空间直角坐标系为O-XYZ和O′-X′Y′Z′，其坐标系原点不一致，存在三个平移参数，ΔX，ΔY，ΔZ；它们的坐标轴也相互不平行，存在三个旋转参数ε_X，ε_Y，ε_Z，同一个点在这两个空间坐标系中的坐标为(X，Y，Z)与(X′，Y′，Z′)。

显然，这两个坐标系通过坐标轴的平移和旋转变换可取得一致，坐标间的转换关系如下：

$\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\mathrm{\λR}\left[\begin{array}{c}{X}^{\′}\\ Y^{\′}\\ Z^{\′}\end{array}\right]+\left[\begin{array}{c}\mathrm{\ΔX}\\ \mathrm{\ΔY}\\ \mathrm{\ΔZ}\end{array}\right]---\left(1\right)$

其中，λ为两个坐标系间的尺度比例因子，[ΔX，ΔY，ΔY]^T为平移矩阵，R为旋转矩阵，且

$\begin{array}{c}R=R\left({\mathrm{\ϵ}}_Y\right)R\left({\mathrm{\ϵ}}_X\right)R\left({\mathrm{\ϵ}}_Z\right)\\ =\left[\begin{array}{ccc}\cos {\mathrm{\ϵ}}_Y\cos {\mathrm{\ϵ}}_Z-\sin {\mathrm{\ϵ}}_Y\sin {\mathrm{\ϵ}}_X\sin {\mathrm{\ϵ}}_Z& -\cos {\mathrm{\ϵ}}_Y\sin {\mathrm{\ϵ}}_Y-\sin {\mathrm{\ϵ}}_Y\sin {\mathrm{\ϵ}}_X\cos {\mathrm{\ϵ}}_Z& -\sin {\mathrm{\ϵ}}_Y\cos {\mathrm{\ϵ}}_Y\\ \cos {\mathrm{\ϵ}}_X\sin {\mathrm{\ϵ}}_Z& \cos {\mathrm{\ϵ}}_X\cos {\mathrm{\ϵ}}_Z& -\sin {\mathrm{\ϵ}}_X\\ \sin {\mathrm{\ϵ}}_Y\cos {\mathrm{\ϵ}}_Z+\cos {\mathrm{\ϵ}}_Y\sin {\mathrm{\ϵ}}_X\sin {\mathrm{\ϵ}}_Z& -\sin {\mathrm{\ϵ}}_Y\sin {\mathrm{\ϵ}}_Z+\cos {\mathrm{\ϵ}}_Y\sin {\mathrm{\ϵ}}_X\cos {\mathrm{\ϵ}}_Z& \cos {\mathrm{\ϵ}}_Y\cos {\mathrm{\ϵ}}_X\end{array}\right]\end{array}$

可见，只要求出ΔX，ΔY，ΔZ，ε_x，ε_y，ε_z，λ这七个转换参数，或者直接求出旋转矩阵和平移矩阵，就可以实现两个坐标系间的转换。

为了求解坐标系间的转换参数，需要有一定数量的具有两套坐标的公共点(X_i，Y_i，Z_i)和(X_i′，Y_i′，Z_i′)，i＝1,2,3…，n(≥3)，这两套坐标的公共点可以通过精密仪器测量获得，从而根据一定的转换模型来求解转换参数，另外，为了计算方便，对所用到的公共点坐标进行重心化处理，将两个坐标系的公共点的坐标均化算为以重心为原点的重心化坐标，重心分别记为和两个坐标系的重心坐标分别记为(X_g，Y_g，Z_g)和(X_g′，Y_g′，Z_g′)

$\left\{\begin{array}{c}{X}_g=\frac{{\mathrm{\Σ}}_{i=1}^n{X}_i}{n},Y_g=\frac{{\mathrm{\Σ}}_{i=1}^n{Y}_i}{n},Z_g=\frac{{\mathrm{\Σ}}_{i=1}^n{Z}_i}{n}\\ X_g^{\′}=\frac{{\mathrm{\Σ}}_{i=1}^n{X}_i^{\′}}{n},Y_g^{\′}=\frac{{\mathrm{\Σ}}_{i=1}^n{Y}_i^{\′}}{n},Z_g^{\′}=\frac{{\mathrm{\Σ}}_{i=1}^n{Z}_i^{\′}}{n}\\ {\stackrel{\‾}{X}}_i=X_i-X_g,{\stackrel{\‾}{Y}}_i=Y_i-Y_g,{\stackrel{\‾}{Z}}_i=Z_i-Z_g\\ {\stackrel{\‾}{X}}_i^{\′}=X_i^{\′}-X_g^{\′},{\stackrel{\‾}{Y}}_i^{\′}=Y_i^{\′}-Y_g^{\′},{\stackrel{\‾}{Z}}_i^{\′}=Z_i^{\′}-Z_g^{\′}\end{array}\right.---\left(2\right)$

将公共点的重心化坐标代入式(1)，可得：

$\left[\begin{array}{c}\stackrel{\‾}{X}\\ \stackrel{\‾}{Y}\\ \stackrel{\‾}{Z}\end{array}\right]=\mathrm{\λR}\left[\begin{array}{c}{\stackrel{\‾}{X}}^{\′}\\ {\stackrel{\‾}{Y}}^{\′}\\ {\stackrel{\‾}{Z}}^{\′}\end{array}\right]---\left(3\right)$

$\left[\begin{array}{c}\mathrm{\ΔX}\\ \mathrm{\ΔY}\\ \mathrm{\ΔZ}\end{array}\right]=\left[\begin{array}{c}{X}_g\\ Y_g\\ Z_g\end{array}\right]-\mathrm{\λR}\left[\begin{array}{c}{X}_g^{\′}\\ Y_g^{\′}\\ Z_g^{\′}\end{array}\right]---\left(4\right)$

因而，转换参数可分为两步来求解，即可先用式(3)求出旋转参数和比例因子，再用式(4)求出平移参数。

式(3)为非线性函数，为了求解其中的转换参数，需要对其进行线性化，按多元函数泰勒级数展开：

$F=F_0+\frac{\∂F}{\∂\mathrm{\λ}}\mathrm{d\λ}+\frac{\∂F}{\∂{\mathrm{\ϵ}}_X}d{\mathrm{\ϵ}}_X+\frac{\∂F}{\∂{\mathrm{\ϵ}}_Y}d{\mathrm{\ϵ}}_Y+\frac{\∂F}{\∂{\mathrm{\ϵ}}_Z}d{\mathrm{\ϵ}}_Z---\left(5\right)$

对式(3)按泰勒级数在λ＝1，ε_x＝ε_y＝ε_z＝0处展开，并去一次项得

$\begin{array}{c}\left[\begin{array}{c}\stackrel{\‾}{X}\\ \stackrel{\‾}{Y}\\ \stackrel{\‾}{Z}\end{array}\right]={\mathrm{\λ}}_0{R}_0\left[\begin{array}{c}{\stackrel{\‾}{X}}^{\′}\\ {\stackrel{\‾}{Y}}^{\′}\\ {\stackrel{\‾}{Z}}^{\′}\end{array}\right]+\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{\stackrel{\‾}{X}}^{\′}\\ {\stackrel{\‾}{Y}}^{\′}\\ {\stackrel{\‾}{Z}}^{\′}\end{array}\right]\mathrm{d\λ}+\left[\begin{array}{ccc}0& 0& -1\\ 0& 0& 0\\ 1& 0& 0\end{array}\right]\left[\begin{array}{c}{\stackrel{\‾}{X}}^{\′}\\ {\stackrel{\‾}{Y}}^{\′}\\ {\stackrel{\‾}{Z}}^{\′}\end{array}\right]d{\mathrm{\ϵ}}_Y\\ +\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\stackrel{\‾}{X}}^{\′}\\ {\stackrel{\‾}{Y}}^{\′}\\ {\stackrel{\‾}{Z}}^{\′}\end{array}\right]d{\mathrm{\ϵ}}_Z+\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& -1\\ 0& 1& 0\end{array}\right]\left[\begin{array}{c}{\stackrel{\‾}{X}}^{\′}\\ {\stackrel{\‾}{Y}}^{\′}\\ {\stackrel{\‾}{Z}}^{\′}\end{array}\right]d{\mathrm{\ϵ}}_X\end{array}---\left(6\right)$

由式(6)可得如下的关于坐标误差V_X、V_Y、V_Z的误差方程：

$\begin{array}{c}\left[\begin{array}{c}{V}_X\\ V_Y\\ V_Z\end{array}\right]=\left[\begin{array}{cccc}{\stackrel{\‾}{X}}^{\′}& -{\stackrel{\‾}{Z}}^{\′}& 0& -{\stackrel{\‾}{Y}}^{\′}\\ {\stackrel{\‾}{Y}}^{\′}& 0& -{\stackrel{\‾}{Z}}^{\′}& {\stackrel{\‾}{X}}^{\′}\\ {\stackrel{\‾}{Z}}^{\′}& {\stackrel{\‾}{X}}^{\′}& {\stackrel{\‾}{Y}}^{\′}& 0\end{array}\right]\left[\begin{array}{c}\mathrm{d\λ}\\ d{\mathrm{\ϵ}}_Y\\ d{\mathrm{\ϵ}}_X\\ d{\mathrm{\ϵ}}_Z\end{array}\right]-\left[\begin{array}{c}{l}_X\\ l_Y\\ l_Z\end{array}\right]\\ \left[\begin{array}{c}{l}_X\\ l_Y\\ l_Z\end{array}\right]=\left[\begin{array}{c}\stackrel{\‾}{X}\\ \stackrel{\‾}{Y}\\ \stackrel{\‾}{Z}\end{array}\right]-{\mathrm{\λ}}_0{R}_0\left[\begin{array}{c}{\stackrel{\‾}{X}}^{\′}\\ {\stackrel{\‾}{Y}}^{\′}\\ {\stackrel{\‾}{Z}}^{\′}\end{array}\right]\end{array}---\left(7\right)$

式中，参数的初始值分别为：λ₀＝1，R₀取单位矩阵，三个旋转参数的初始值都取零。

因此，对于n个公共点，可根据式(7)列出如下的总体误差方程：

V_3n×1＝A_3n×4X_4×1-L_3n×1    (8)

式中，

X_4×1＝[dλ dε_Y dε_X dε_Z]^T

$A_{3n\×4}=\left[\begin{array}{cccc}{\stackrel{\‾}{X}}_1^{\′}& -{\stackrel{\‾}{Z}}_1^{\′}& 0& -{\stackrel{\‾}{Y}}_1^{\′}\\ {\stackrel{\‾}{Y}}_1^{\′}& 0& -{\stackrel{\‾}{Z}}_1^{\′}& {\stackrel{\‾}{X}}_1^{\′}\\ {\stackrel{\‾}{X}}_1^{\′}& {\stackrel{\‾}{X}}_1^{\′}& {\stackrel{\‾}{Y}}_1^{\′}& 0\\ .& .& .& .\\ .& .& .& .\\ .& .& .& .\\ {\stackrel{\‾}{X}}_n^{\′}& -{\stackrel{\‾}{Z}}_n^{\′}& 0& -{\stackrel{\‾}{Y}}_n^{\′}\\ {\stackrel{\‾}{Y}}_n^{\′}& 0& -{\stackrel{\‾}{Z}}_n^{\′}& {\stackrel{\‾}{X}}_n^{\′}\\ {\stackrel{\‾}{X}}_n^{\′}& {\stackrel{\‾}{X}}_n^{\′}& {\stackrel{\‾}{Y}}_n^{\′}& 0\end{array}\right]$

$L_{3n\×1}=\left[\begin{array}{c}{L}_{X1}\\ L_{Y1}\\ L_{Z1}\\ .\\ .\\ .\\ L_{\mathrm{Xn}}\\ L_{\mathrm{Yn}}\\ L_{\mathrm{Zn}}\end{array}\right]$

利用式(8)，根据最小二乘原理，可得比例因子和旋转参数的改正值：

X_4×1＝[dλ dε_Y dε_X dε_Z]^T＝(A_3n×4 ^TA_3n×4)^-1A_3n×4 ^TL_3n×1    (9)

由于式(8)只能求出旋转参数的改正值，因此需要迭代运算才能求出旋转参数，在迭代的趋近过程中，常数项L是变值，其中的λ₀，R₀需要用每一次迭代求解出的新的参数来重新计算，直到解算的改正值小于某一限差时，停止迭代运算。此时求出来的ε_X，ε_Y，ε_Z即为三轴陀螺仪和三维电子罗盘两个坐标系之间的旋转参数，记为标定值α_b，β_b，γ_b。

三轴陀螺仪三轴旋转角度计算初值的计算方法如下：

设当前时刻为t，取三维电子罗盘在时刻t-t₁到t-t₂这段时间内(t₁>t₂>0)的三轴旋转角α,β,γ，采用最小二乘法建立时间和三轴旋转角之间的方程，该方程可用如下方式求出：

$\stackrel{\‾}{t}=\frac{{\mathrm{\Σ}}_{i=1}^n{t}_i}{n}$

$\stackrel{\‾}{\mathrm{\α}}=\frac{{\mathrm{\Σ}}_{i=1}^n{\mathrm{\α}}_i}{n}$

$\stackrel{\‾}{\mathrm{\β}}=\frac{{\mathrm{\Σ}}_{i=1}^n{\mathrm{\β}}_i}{n}$

$\stackrel{\‾}{\mathrm{\γ}}=\frac{{\mathrm{\Σ}}_{i=1}^n{\mathrm{\γ}}_i}{n}$

$k_{\mathrm{\α}}=\frac{{\mathrm{\Σ}}_{i=1}^n(t_i-\stackrel{\‾}{t})({\mathrm{\α}}_i-\stackrel{\‾}{\mathrm{\α}})}{{\mathrm{\Σ}}_{i=1}^n{(t_i-\stackrel{\‾}{t})}^2}$

$b_{\mathrm{\α}}=\stackrel{\‾}{\mathrm{\α}}-k_{\mathrm{\α}}*\stackrel{\‾}{t}$

$k_{\mathrm{\β}}=\frac{{\mathrm{\Σ}}_{i=1}^n(t_i-\stackrel{\‾}{t})({\mathrm{\β}}_i-\stackrel{\‾}{\mathrm{\β}})}{{\mathrm{\Σ}}_{i=1}^n{(t_i-\stackrel{\‾}{t})}^2}$

$b_{\mathrm{\β}}=\stackrel{\‾}{\mathrm{\β}}-k_{\mathrm{\β}}*\stackrel{\‾}{t}$

$k_{\mathrm{\γ}}=\frac{{\mathrm{\Σ}}_{i=1}^n(t_i-\stackrel{\‾}{t})({\mathrm{\γ}}_i-\stackrel{\‾}{\mathrm{\γ}})}{{\mathrm{\Σ}}_{i=1}^n{(t_i-\stackrel{\‾}{t})}^2}$

$b_{\mathrm{\γ}}=\stackrel{\‾}{\mathrm{\γ}}-k_{\mathrm{\γ}}*\stackrel{\‾}{t}$

α与t的关系式如下：

α＝k_α*t+b_α

β与t的关系式如下：

β＝k_β*t+b_β

γ与t的关系式如下：

γ＝k_γ*t+b_γ

进而利用该方程，以可以求出当前时刻t的三轴旋转角度，作为三轴陀螺仪三轴旋转角度计算初值α₀，β₀，γ₀

α₀＝k_α*t+b_α

β₀＝k_β*t+b_β

γ₀＝k_γ*t+b_γ

当前时刻的在三维电子罗盘坐标系下的矫正方位角的计算公式如下：

α＝α_b+α₀+Δα′

β＝β_b+β₀+Δβ′

γ＝γ_b+γ₀+Δγ′

(4)继续判断，利用三维电子罗盘测量三轴旋转角度，判断三轴旋转角度是否低于设定的负向阈值，判断方法同步骤(1)的判断方法，如果未低于，说明三维电子罗盘还未脱离铁磁物质的干扰，则继续步骤(3)，将矫正方位角作为使用的方位角，如果低于负向阈值，说明三维电子罗盘已经脱离铁磁物质的干扰，则将三维电子罗盘测量三轴旋转角度作为使用的方位角。

如图3所示，为本发明实施例中钻孔电视探头受铁磁干扰时的方位矫正装置的结构示意图，具体地，所述方位矫正装置包括三维电子罗盘、三轴陀螺仪、通用异步收发传输器(Universal AsynchronousReceiver/Transmitter,UART)模块、集成电路总线(Inter-IntegratedCircuit，IIC)模块、单片机以及上位机，其中：

所述三维电子罗盘，用于测量三轴旋转角度；

所述三轴陀螺仪，用于测量三轴旋转角速率；

所述通用异步收发传输器模块，用于实现三维电子罗盘和单片机之间的异步通信；

所述集成电路总线模块，用于实现三轴陀螺仪和单片机之间的实时数据传输；

所述单片机，用于与三维电子罗盘和三轴陀螺仪之间通信，并将数据通过无线局域网传输给上位机进行处理；

所述上位机，用于执行下述操作：

(1)利用三维电子罗盘测量三轴旋转角度，判断三轴旋转角度是否超过设定的正向阈值，如果未超过，则将三维电子罗盘测量的角度作为方位角，如果超过阈值，则转步骤(2)；

(2)获得三轴陀螺仪测量的三轴旋转角速率，并利用测得的三轴旋转角速率进行积分计算得到三轴旋转角的变化值；

(3)根据三维电子罗盘与三轴陀螺仪之间的坐标转换关系，利用三轴陀螺仪的三轴旋转角变化值及三轴陀螺仪三轴旋转角度计算初值求得当前时刻的在三维电子罗盘坐标系下的矫正方位角，其中三轴陀螺仪三轴旋转角度计算初值是三维电子罗盘方位角超过设定阈值之前一段设定时间的三轴角度值；

(4)继续判断，利用三维电子罗盘测量三轴旋转角度，判断三轴旋转角度是否低于设定的负向阈值，判断方法同步骤(1)的判断方法，如果未低于，说明三维电子罗盘还未脱离铁磁物质的干扰，则继续步骤(3)，将矫正方位角作为使用的方位角，如果低于负向阈值，说明三维电子罗盘已经脱离铁磁物质的干扰，则将三维电子罗盘测量三轴旋转角度作为使用的方位角。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种钻孔电视探头受铁磁干扰时的方位矫正方法，其特征在于，所述方法包括如下步骤：

(1)利用三维电子罗盘测量三轴旋转角度，判断三轴旋转角度是否超过设定的正向阈值，如果未超过，则将三维电子罗盘测量的角度作为方位角，如果超过阈值，则转步骤(2)；

(2)获得三轴陀螺仪测量的三轴旋转角速率，并利用测得的三轴旋转角速率进行积分计算得到三轴旋转角的变化值；

(3)根据三维电子罗盘与三轴陀螺仪之间的坐标转换关系，利用三轴陀螺仪的三轴旋转角变化值及三轴陀螺仪三轴旋转角度计算初值求得当前时刻的在三维电子罗盘坐标系下的矫正方位角，其中三轴陀螺仪三轴旋转角度计算初值是三维电子罗盘方位角超过设定阈值之前一段设定时间的三轴角度值；

(4)继续判断，利用三维电子罗盘测量三轴旋转角度，判断三轴旋转角度是否低于设定的负向阈值，判断方法同步骤(1)的判断方法，如果未低于，说明三维电子罗盘还未脱离铁磁物质的干扰，则继续步骤(3)，将矫正方位角作为使用的方位角，如果低于负向阈值，说明三维电子罗盘已经脱离铁磁物质的干扰，则将三维电子罗盘测量三轴旋转角度作为使用的方位角。

2.如权利要求1所述的方法，其特征在于，所述步骤(1)中判断三轴旋转角度是否超过设定的正向阈值，具体为：

对三维电子罗盘的三轴旋转角速率进行判断，三维电子罗盘的三轴旋转角速率ω_x>ω_xmax，ω_y>ω_ymax，ω_z>ω_zmax时，认为三轴旋转角度超过设定的正向阈值；其中：

ω_xmax、ω_ymax、ω_zmax为设定的三轴旋转角速率的正向阈值；

${\mathrm{\ω}}_x=\frac{\mathrm{\Δ\α}}{\mathrm{\Δt}}$

${\mathrm{\ω}}_y=\frac{\mathrm{\Δ\β}}{\mathrm{\Δt}}$

${\mathrm{\ω}}_z=\frac{\mathrm{\Δ\γ}}{\mathrm{\Δt}}$

Δα、Δβ、Δγ分别表示在三维电子罗盘在自身坐标系下，第二姿态相对于第一姿态的三轴旋转角度，Δt为三维电子罗盘从第一姿态变化到第二姿态的时间间隔；

Δα＝α₂-α₁

Δβ＝β₂-β₁

Δγ＝γ₂-γ₁

其中α₂、β₂、γ₂为三维电子罗盘第二姿态时的三轴旋转角度值，α₁、β₁、γ₁为三维电子罗盘第一姿态时的三轴旋转角度值。

3.如权利要求1或2所述的方法，其特征在于，所述三轴旋转角的变化值计算方法如下：

在三轴陀螺仪自身坐标系下面，第二姿态相对于第一姿态的三轴旋转角变化值公式如下：

${\mathrm{\Δ\α}}^{\′}={\∫}_0^{\mathrm{\Δt}}f\left({w_x}^{\′}\right)\mathrm{dt}$

${\mathrm{\Δ\β}}^{\′}={\∫}_0^{\mathrm{\Δt}}f\left({w_y}^{\′}\right)\mathrm{dt}$

${\mathrm{\Δ\γ}}^{\′}={\∫}_0^{\mathrm{\Δt}}f\left({w_z}^{\′}\right)\mathrm{dt}$

Δt为三轴陀螺仪从第一姿态变化到第二姿态的时间间隔，w_x′、w_y′、w_z′分别是三轴陀螺仪的三轴旋转角速率。

4.如权利要求1或2所述的方法，其特征在于，所述三轴陀螺仪三轴旋转角度计算初值的计算方法如下：

设当前时刻为t，取三维电子罗盘在时刻t-t₁到t-t₂这段时间内(t₁>t₂>0)的三轴旋转角α,β,γ，采用最小二乘法建立时间和三轴旋转角之间的方程，该方程可用如下方式求出：

$\stackrel{\‾}{t}=\frac{{\mathrm{\Σ}}_{i=1}^n{t}_i}{n}$

$\stackrel{\‾}{\mathrm{\α}}=\frac{{\mathrm{\Σ}}_{i=1}^n{\mathrm{\α}}_i}{n}$

$\stackrel{\‾}{\mathrm{\β}}=\frac{{\mathrm{\Σ}}_{i=1}^n{\mathrm{\β}}_i}{n}$

$\stackrel{\‾}{\mathrm{\γ}}=\frac{{\mathrm{\Σ}}_{i=1}^n{\mathrm{\γ}}_i}{n}$

$k_{\mathrm{\α}}=\frac{{\mathrm{\Σ}}_{i=1}^n(t_i-\stackrel{\‾}{t})({\mathrm{\α}}_i-\stackrel{\‾}{\mathrm{\α}})}{{\mathrm{\Σ}}_{i=1}^n{(t_i-\stackrel{\‾}{t})}^2}$

$b_{\mathrm{\α}}=\stackrel{\‾}{\mathrm{\α}}-k_{\mathrm{\α}}*\stackrel{\‾}{t}$

$k_{\mathrm{\β}}=\frac{{\mathrm{\Σ}}_{i=1}^n(t_i-\stackrel{\‾}{t})({\mathrm{\β}}_i-\stackrel{\‾}{\mathrm{\β}})}{{\mathrm{\Σ}}_{i=1}^n{(t_i-\stackrel{\‾}{t})}^2}$

$b_{\mathrm{\β}}=\stackrel{\‾}{\mathrm{\β}}-k_{\mathrm{\β}}*\stackrel{\‾}{t}$

$k_{\mathrm{\γ}}=\frac{{\mathrm{\Σ}}_{i=1}^n(t_i-\stackrel{\‾}{t})({\mathrm{\γ}}_i-\stackrel{\‾}{\mathrm{\γ}})}{{\mathrm{\Σ}}_{i=1}^n{(t_i-\stackrel{\‾}{t})}^2}$

$b_{\mathrm{\γ}}=\stackrel{\‾}{\mathrm{\γ}}-k_{\mathrm{\γ}}*\stackrel{\‾}{t}$

α与t的关系式如下：

α＝k_α*t+b_α

β与t的关系式如下：

β＝k_β*t+b_β

γ与t的关系式如下：

γ＝k_γ*t+b_γ

进而利用该方程，求出当前时刻t的三轴旋转角度，作为三轴陀螺仪三轴旋转角度计算初值α₀，β₀，γ₀

α₀＝k_α*t+b_α

β₀＝k_β*t+b_β

γ₀＝k_γ*t+b_γ。

5.如权利要求1或2所述的方法，其特征在于，所述当前时刻的在三维电子罗盘坐标系下的矫正方位角的计算公式如下：

α＝α_b+α₀+Δα′

β＝β_b+β₀+Δβ′

γ＝γ_b+γ₀+Δγ′。

其中，三轴旋转角α,β,γ为当前时刻的在三维电子罗盘坐标系下的矫正方位角，α₀，β₀，γ₀为三轴陀螺仪三轴旋转角度计算初值，Δα′、Δβ′、Δγ′分别为三轴旋转角的变化值，α_b，β_b，γ_b为三轴陀螺仪和三维电子罗盘两个坐标系之间的旋转参数。

6.一种钻孔电视探头受铁磁干扰时的方位矫正装置，其特征在于，所述方位矫正装置包括三维电子罗盘、三轴陀螺仪、通用异步收发传输器模块、集成电路总线模块、单片机以及上位机，其中：

所述三维电子罗盘，用于测量三轴旋转角度；

所述三轴陀螺仪，用于测量三轴旋转角速率；

所述通用异步收发传输器模块，用于实现三维电子罗盘和单片机之间的异步通信；

所述集成电路总线模块，用于实现三轴陀螺仪和单片机之间的实时数据传输；

所述单片机，用于与三维电子罗盘和三轴陀螺仪之间通信，并将数据通过无线局域网传输给上位机进行处理；

所述上位机，用于执行下述操作：

(1)利用三维电子罗盘测量三轴旋转角度，判断三轴旋转角度是否超过设定的正向阈值，如果未超过，则将三维电子罗盘测量的角度作为方位角，如果超过阈值，则转步骤(2)；

(2)获得三轴陀螺仪测量的三轴旋转角速率，并利用测得的三轴旋转角速率进行积分计算得到三轴旋转角的变化值；

(3)根据三维电子罗盘与三轴陀螺仪之间的坐标转换关系，利用三轴陀螺仪的三轴旋转角变化值及三轴陀螺仪三轴旋转角度计算初值求得当前时刻的在三维电子罗盘坐标系下的矫正方位角，其中三轴陀螺仪三轴旋转角度计算初值是三维电子罗盘方位角超过设定阈值之前一段设定时间的三轴角度值；

(4)继续判断，利用三维电子罗盘测量三轴旋转角度，判断三轴旋转角度是否低于设定的负向阈值，判断方法同步骤(1)的判断方法，如果未低于，说明三维电子罗盘还未脱离铁磁物质的干扰，则继续步骤(3)，将矫正方位角作为使用的方位角，如果低于负向阈值，说明三维电子罗盘已经脱离铁磁物质的干扰，则将三维电子罗盘测量三轴旋转角度作为使用的方位角。