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

说明书

本发明公开了一种用于非开挖地下钻头姿态融合的四元数梯度下降方法，包括如下步骤：1）根据采集的钻头内部的陀螺仪数据求解出四元数；2）利用采集的加速度计的数据建立加速度计的误差函数；3）对磁强计进行地磁补偿；4）利用补偿后的磁强计的数据，建立磁强计的误差函数；5）根据加速度计、磁强计两组误差函数求解出四元数；6）根据梯度下降算法融合策略，将步骤1）求解得出的四元数和步骤5）求解出的四元数进行融合，解算出准确的姿态四元数；7）根据步骤6）得到的姿态四元数，求解出偏航角、横滚角和俯仰角。该方法简化了算法的复杂度，解决了陀螺仪数据漂移问题，适用于低成本姿态解算的嵌入式系统。

技术领域

本发明是涉及非开挖导向仪地下探头姿态数据融合技术领域，具体是一种基 于四元数梯度下降的非开挖地下钻头姿态融合方法。

技术背景

随着全球经济的持续发展、人口的迅速增长以及城市化进程的不断加快，地 下管线的需求正变得越来越大，在地下管线工程建设中传统的开挖埋设方法正带 来日益严重的问题，在这种情况下，非开挖技术得到飞速发展。

在非开挖导向施工过程中，整个导向仪技术难点就在于探棒内部安装传感器 的姿态解算，导向系统的探棒安装在钻头内。探棒随钻头一同在地层中移动，探 棒实时测量钻头的角度信息并将该信号发送给地面接收装置。地面操作人员根据 地下探棒测量的钻头空间位置可以判断出钻头实际运动轨迹与设计轨迹的偏差， 控制钻头前进的方向。

对于地下钻头的姿态角度的测量目前主要是应用了三种方法，欧拉角度法由 于存在奇点现象，方向余弦法方程的计算量大，工作效率低。我们通常在姿态角 度中都是使用的四元数的姿态解算方法，该方法计算比较简单，可以进行实时的 解算姿态角度。但是四元数法存在陀螺仪传感器数据漂移问题，使得所解算角度 不准确。因此需要通过多传感器数据融合进行误差补偿提高解算角度精度。

在姿态数据融合的算法中，卡尔曼滤波算法是一种传统的姿态融合算法。该 算法通过选取陀螺仪数据为状态量来进行卡尔曼滤波过程的时间更新，以加速度 计和磁强计数据为观测量建立滤波器的量测更新，该算法在一定程度上能解决因 陀螺仪造成的数据漂移导致角度不精确，但是其计算过程复杂，且其数学运算量 很大，并且扩展卡尔曼滤波算法在用泰勒公式进行非线性方程的线性展开所带入 线性误差。

发明内容

本发明的目的在于克服现有技术的不足，而提供一种基于四元数梯度下降的 非开挖地下钻头姿态融合方法，该方法简化了算法的复杂度，解决了陀螺仪数据 漂移问题，适用于低成本姿态解算的嵌入式系统。

实现本发明目的的技术方案是：

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

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

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

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

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

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

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

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

步骤1)中，所述的陀螺仪数据，是利用三轴陀螺仪，将测量到的传感器x 轴、y轴和z轴的角速度数据ω_x、ω_y和ω_z排列成如下(1)式，如下(2)式描 述地球坐标系相对于传感器坐标系的方向变化速率的四元数导数；