矿用陀螺测斜仪捷联惯导系统姿态解算及零速校正方法与流程

本发明涉及一种矿用定向钻机随钻测量系统轨迹的校正方法，尤其是以姿态传感器的测量数据作为输入信息，通过不同阶次的龙格库塔法求解捷联惯导系统的机械编排方程，实时输出当前钻头姿态角信息和位置信息，并通过零速校正算法消除捷联惯导系统的累积误差的技术方案。

背景技术：

随钻测量技术是受控定向钻进的关键技术之一。随着我国煤矿综合机械化采煤技术的发展，常采用定向钻机钻探出水平定向长钻孔对煤层进行瓦斯预排及探放水活动。随钻测斜仪是定向钻机进行定向钻进的关键设备，它的主要作用是完成方位角、倾角、工具面角的测量，进而绘制出钻孔轨迹。目前，矿用随钻测斜仪主要使用三轴加速度计和三轴磁阻计作为测斜传感器，依据欧拉角法确定钻头姿态。虽然这种方法稳定可靠，但由于欧拉角法本身的局限性，势必存在测量的奇异点，不能实现全角度测量；此外，磁阻计易受外部磁场干扰，导致钻孔的方位角产生测量误差；该方法只能在更换钻杆后的静止状态下测量，不能在钻进的过程中实时测量。为此，陀螺测斜仪应运而生。陀螺仪与加速度计构成捷联惯导系统的测量单元，通过逐次的积分运算可以实时获得钻头的姿态、速度以及位置信息。但是，直接进行积分运算计算量较大,不易在单片机中实现，目前仍缺乏快速有效的数值解法来代替其中的积分运算。

陀螺仪存在漂移现象，即输出误差随时间逐渐累积。惯导系统长时间运行必将导致累积误差，因此在不断探索如何提高传感器本身的精度外，还需要寻求外部信息对捷联惯导系统进行校正，形成组合导航系统。通常将惯导系统作为主导航系统，其他误差不随时间累积的导航系统如gps、无线电导航、地磁导航等作为外部辅助测量系统。由于煤壁对无线电吸收能力较强，且井下没有gps信号，因此无线电导航技术与gps导航技术不适用于煤矿井下随钻测量。地磁导航技术需要已知当地的地磁数据库，在实际生产中较难实现。因此急需一种稳定可靠的校正手段消除捷联惯导系统的累积误差。

技术实现要素：

本发明的任务是提供一种矿用陀螺测斜仪捷联惯导系统姿态解算及零速校正方法。

具体技术方案如下。

一种矿用陀螺测斜仪捷联惯导系统姿态解算及零速校正方法，所述方法采用的测斜仪包含有三种姿态传感器：陀螺仪、加速度计和磁阻计；所述方法是以陀螺仪和加速度计作为捷联惯导系统的测量单元，以姿态传感器的测量数据作为输入信息，通过不同阶次的龙格库塔法求解捷联惯导系统的机械编排方程，实时输出当前钻头姿态角信息和位置信息，并通过零速校正算法消除捷联惯导系统的累积误差。

附件技术方案一，所述捷联惯导系统姿态解算的方法是以陀螺仪和加速度计作为捷联惯导系统的测量单元，利用四阶龙格库塔法求解姿态矩阵微分方程，利用二阶龙格库塔法求解速度微分方程，一阶龙格库塔法求解位置微分方程。

附件技术方案二，所述零速校正方法的卡尔曼滤波过程需满足两个条件：零速条件和地磁条件；

所述零速条件是根据加速度计输出的幅值进行判断，其公式如下：

其中：为加速度计测量噪声方差；n为采样点数；g为重力加速度模值；

所述地磁条件是根据磁力计输出的幅值进行判断，公式如下：

其中：h为地磁场矢量模值。

本发明中的捷联惯导姿态解算方法的核心思想是通过在积分区间内插值得到更新后的结果，与现有的直接积分运算方法相比，运算量小，一般的嵌入式单片机便可以完成。

本发明中的零速校正方法的卡尔曼滤波过程需满足两个条件：零速条件和地磁条件；其零速条件是根据加速度计输出的幅值进行判断，其地磁条件是根

据磁力计输出的幅值进行判断，零速校正方法与现有的gps组合导航、无线电组合导航等方法相比，充分结合了钻机钻进的工艺，即利用加装钻杆的间隙完成姿态角的校正。此外，磁强计体积小，抗振能力强，更适合于钻杆钻具的狭小空间。

附图说明

图1是本发明捷联惯导解算初始参数表。

图2是本发明捷联惯导系统与磁强计组合的卡尔曼滤波算法示意图。

图3是本发明测斜仪的硬件电路结构图。

图4是本发明陀螺测斜仪中使用的三种姿态传感器坐标系示意图，满足右手螺旋法则。

图5是本发明经卡尔曼滤波后的倾角误差。

图6是本发明经卡尔曼滤波后的方位角误差。

图7是本发明经卡尔曼滤波后的工具面角误差。

图8是本发明经卡尔曼滤波后的北向误差。

图9是本发明经卡尔曼滤波后的东向误差。

图10是本发明经卡尔曼滤波后的地向误差。

具体实施方式

下面作对本发明的具体方式出进一步的说明。

实施本发明上述所提供的一种适用于煤矿井下定向钻机陀螺测斜仪姿态解算及零速校正方法，采用的测斜仪包含三种姿态传感器、陀螺仪、加速度计和磁阻计，以陀螺仪和加速度计作为捷联惯导系统的测量单元，通过不同阶次的龙格库塔法求解捷联惯导系统的机械编排方程，实时输出当前钻头姿态角信息和位置信息，同时为消除捷联惯导系统长时工作所产生的累积误差，以磁强计作为外部辅助测量装置，通过零速校正算法实现多传感器的组合导航，提高钻孔轨迹测量的精确度。

本发明基于矿用陀螺测斜仪的捷联惯导数值解算及零速校正方法是以陀螺仪和加速度计作为捷联惯导系统的测量单元，磁强计作为外部辅助测量单元，其中加速度计测量值，磁强计测量值，陀螺仪测量值，采样间隔时间为。

本发明以不同阶次的龙格库塔法求解捷联惯导系统的机械编排方程，是指利用二阶龙格库塔法求解速度微分方程，得到钻头在地理坐标系下的速度，一阶龙格库塔法求解位置微分方程，得到钻头的位置信息，四阶龙格库塔法求解姿态矩阵微分方程，得到更新之后的四元数矩阵。

本发明通过零速校正算法消除捷联惯导系统的累积误差，是指满足零速条件和地磁测量条件下，构建卡尔曼滤波器，以姿态角误差和速度误差作为观测量，对状态变量作出最优估计，并以输出校正的方式对姿态矩阵进行修正。

本发明捷联惯导系统姿态解算方法为以下具体步骤：

步骤一：采集加速度计、磁强计、陀螺仪的数据。其中加速度计测量值，磁强计测量值，陀螺仪测量值，采样间隔时间为。

步骤二：利用二阶龙格库塔法求解速度微分方程，速度微分方程如下所示：

进一步写成矩阵形式为：

具体求解过程如下：假设初始速度，

其中：

如此递推，得到更新后的地速。

步骤三：利用一阶龙格库塔法求解位置微分方程，其表示如下所示：

进一步写成矩阵形式为：

求解过程如下：

可得更新后的位置矩阵，从而可求得经度表达式：，纬度表达式：以及深度表达式：

步骤四：利用四阶龙格库塔法求解四元数微分方程，该方程表示如下：

根据四元数乘法写成相应矩阵形式：

具体求解过程如下：

设

可得到更新后的表示地理坐标系到载体坐标系的转动四元数，之后经过归一化，便完成了姿态矩阵的更新，同时可求出对应的姿态角：

本发明中的零速校正算法为以下具体步骤：

步骤一：判断测斜仪是否满足零速条件和地磁测量条件。所述的零速条件是根据加速度计输出的幅值进行判断，其公式如下：

其中：为加速度计测量噪声方差；n为采样点数；g为重力加速度模值。

所述的地磁条件是根据磁力计输出的幅值进行判断，公式如下：

其中：h为地磁场矢量模值。

步骤二：当检测到系统满足零速条件和地磁场测量条件后，根据捷联惯导姿态解算算法步骤二中的姿态角计算公式求出当前的姿态角。捷联惯导算法解算出的姿态角减去该姿态角作为卡尔曼滤波器的输入，同时将速度误差置零。如附图2所示。

进一步，卡尔曼滤波器模型为：

选取的状态变量为：

选取速度误差和姿态角误差作为观测量：

状态转移矩阵为：

观测矩阵为：

步骤三：通过输出校正的方式获得最优姿态角，根据最优姿态角重新计算姿态矩阵，完成姿态矩阵的校正。

实施例1

本发明提供的捷联惯导解算及零速校正算法适用于各种类型的陀螺仪所构成的捷联惯导系统，以mems陀螺仪与mems加速度计构成的捷联惯导系统为实施例。

采用本发明的陀螺测斜仪的硬件电路结构图如图3所示，包括传感器数据采集模块、单片机最小系统、温度检测模块、can通讯模块、存储模块。其中，姿态传感器均设定为每隔0.1s采集一次数据，其敏感轴的排列方式如图4所示。将陀螺测斜仪固定于三轴转台上，完成转台姿态角设定后，系统上电运行并预热五分钟，五分钟之后开始采集姿态传感器数据，并通过can通讯模块发送上传的传感器数据。根据图1中的初始参数，在matlab中利用导出的传感器数据和构建的卡尔曼滤波器进行姿态角的解算。

由图5至图10可以看出，将磁强计作为外部辅助测量装置后，捷联式惯导系统的导航精度大幅提高，漂移现象得到抑制。在15分钟的仿真时间内，姿态角误差基本都在2°以内，北向和东向速度误差控制在2m/s以内，地向速度误差控制在4m/s以内。仿真结果表明，采用imu/磁强计组合的测量系统并结合本文建立的卡尔曼滤波模型，可以有效抑制惯导系统的累积误差，系统导航精度大幅改善，从而证明了该算法的正确性和有效性。