[发明专利]一种基于四元数梯度下降的非开挖地下钻头姿态融合方法

权利要求书

1.一种用于非开挖地下钻头姿态融合的四元数梯度下降方法，其特征在于，包括如下步骤：

1)根据采集的钻头内部的陀螺仪数据求解出四元数；

2)利用采集的加速度计的数据建立加速度计的误差函数；

3)对磁强计进行地磁补偿；

4)利用补偿后的磁强计的数据，建立磁强计的误差函数；

5)根据加速度计、磁强计两组误差函数求解出四元数；

6)根据梯度下降算法融合策略，将步骤1)求解得出的四元数和步骤5)求解出的四元数进行融合，解算出准确的姿态四元数；

7)根据步骤6)得到的姿态四元数，求解出偏航角、横滚角和俯仰角。

2.根据权利要求1所述的一种用于非开挖地下钻头姿态融合的四元数梯度下降方法，其特征在于，步骤1)中，所述的陀螺仪数据，是利用三轴陀螺仪，将测量到的传感器x轴、y轴和z轴的角速度数据ω_x、ω_y和ω_z排列成如下(1)式，如下(2)式描述地球坐标系相对于传感器坐标系的方向变化速率的四元数导数；

ω_n＝[ω_x ω_y ω]_z (1)

式(1)中ω_x,ω_y,ω_z表示陀螺仪在x、y、z轴测量的角速度值，式(2)中为机体坐标系B相对于地面参考坐标系E的四元数；

通过对上述(1)式和(2)式进行上一时刻微分方程的展开计算，得到上一时刻的四元数表达式(3)，以及通过计算得到陀螺仪解算的四元数式(4)：

公式(1)和公式(2)中，为上一时刻估算得到的四元数；表示陀螺仪经过微分方程迭代得到的四元数；T_s为离散周期。

3.根据权利要求1所述的一种用于非开挖地下钻头姿态融合的四元数梯度下降方法，其特征在于，步骤2)中，所述的加速度计的误差函数，

首先假设重力方向定义为垂直于z轴，如下(5)式所示：

并根据加速度计测得的加速度值为：

上述(6)式中α_x,α_y,α_z表示加速度计在x，y，z轴的测量值；

加速度的误差方程函数，通过旋转矩阵计算所转换参考坐标系中的加速度到它的载体的坐标系中，减去其所测得加速度的值就是所求加速度计误差函数，误差函数表达式为：

上述(7)式和(8)式中q₁、q₂、q₃、q₄表示四个四元数的值，表示对加速度计三轴测量值进行向量单位化；

对上式进行求导得到下面的雅克比矩阵：

4.根据权利要求1所述的一种用于非开挖地下钻头姿态融合的四元数梯度下降方法，其特征在于，步骤3)中，所述的地磁补偿，是由于地球磁场的测量值会因磁强计附近铁磁性元素的存在而出现失真，地球坐标系中的干扰源(软铁)在地球磁场的测量方向上产生误差，倾角误差，在垂直平面相对于地球表面的误差，可以进行补偿，

如下(10)式所示，时间t时地球磁场在地球坐标系中的测量的三个矢量值分别为h_x，h_y，h_z，通过计算所得的传感器的估计方向旋转值为计算所得归一化磁强计的测量值强度为将其进行旋转到地面磁场得到相对与地面参考坐标系的磁场为