一种基于四元数梯度下降的非开挖地下钻头姿态融合方法与流程

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

技术背景

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

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

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

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

技术实现要素：

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

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

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

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

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

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

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

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

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

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

步骤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)中，为上一时刻估算得到的四元数；表示陀螺仪经过微分方程迭代得到的四元数；ts为离散周期。

步骤2)中，所述的加速度计的误差函数，

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

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

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

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

上述(7)式和(8)式中q1、q2、q3、q4表示四个四元数的值，表示对加速度计三轴测量值进行向量单位化；

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

步骤3)中，所述的地磁补偿，是由于地球磁场的测量值会因磁强计附近铁磁性元素的存在而出现失真，地球坐标系中的干扰源(软铁)在地球磁场的测量方向上产生误差，倾角误差，在垂直平面相对于地球表面的误差，可以进行补偿，

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

步骤4)中，所述的建立磁强计的误差函数，是将地球磁场看作由水平轴和垂直轴上的分量组成的，将其水平轴和垂直轴上的分量分别表示为式(12)和式(13)如下所示：

(12)式和(13)式中bx、bz分别为地磁场标准化后的地面参考坐标系中x、z轴上磁场强度，mx、my、mz分别为磁强计在x、y、z轴所测量的磁场强度值，机体的坐标系磁强计所测得的磁场强度的数据值；

磁强计的误差函数也是为参考坐标系中所测得的磁场强度减去载体本身的坐标系所测得的磁场的强度的值，式子如下：

对该误差函数进行求导，得到下式的雅克比矩阵：

步骤5)中，所述的根据加速度计、磁强计两组误差函数求解四元数，根据之前所求解的加速度计和磁强计测量数据的误差方程，具体是建立如下(17)式所示的加速度计和地磁传感器的误差函数，并求得的雅可比矩阵，如(18)式所示：

根据(17)式和(18)式计算出误差函数的梯度值，如下(19)式所示；根据梯度下降的迭代求出四元数的值，如下(20)式所示：

(20)式中，μ(x)为梯度下降法的步长。

步骤6)中，所述的姿态四元数融合策略，是步骤1)求通过陀螺仪数据解算出的四元数和步骤5)通过加速度计磁强计测出加速度和磁场强度解算的四元数进行融合，解算出更加准确的姿态四元数，通过传感器的坐标系相对于地球的估计方向所得到的通过融合方位计算得到的和将两个四元数进行融合，融合式如下(21)式所示：

(21)式中，γ和(1-γ)分别为加速度计磁强计数据的四元数和陀螺仪数据解算的四元数的权重，当γ为0时，表示只使用陀螺仪测量，不需要使用加速度计/磁力计所测得的相关数据来测量四元数；另一方面,当γ的值接近1时，(1-γ)趋于0时，表明陀螺仪测量解算的四元数不准确误差大，测量的加速度计和磁力计更可靠，最优解需要发散速度和收敛速度相等，要求收敛速度μ(k)/ts和发散速度β相等,因此得下述(22)式：

梯度下降法的收敛速度与载体的运动速度有关，因此必须要求收敛速度要大于运动速度，γ需要比较大，那么由(23)式得μ(k)/ts很大，β非常小，因此得到式(24)：

根据公式(10)、(11)、(21)、(24)可得最终得姿态数据融合公式(25)：

式中表示为前一次融合解算的四元数的值。

步骤7)中，所述的求解出偏航角、横滚角和俯仰角，求解三个角度方法如下所示：

θ＝-arcsin2(q1q3-q0q2)(27)

上述式中，ψ表示偏航角，θ表示俯仰角，φ表示横滚角。

有益效果：本发明提供的一种用于非开挖地下钻头姿态融合的四元数梯度下降方法，该方法避免了扩展卡尔曼滤波算法在用泰勒公式进行非线性方程的线性展开所带入线性误差，简化了算法的复杂度，解决了陀螺仪数据漂移问题，非常适合应用于低成本姿态解算的嵌入式系统，可以在经济上很大程度减少非开挖钻头碳棒的成本。

附图说明

图1为本发明方法的流程图；

图2为本发明实施的结构图。

具体实施方式

下面结合附图和实施例对本发明内容做进一步阐述，但不是对本发明的先限定。

实施例：

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

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)中，为上一时刻估算得到的四元数；表示陀螺仪经过微分方程迭代得到的四元数；ts为离散周期。

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

所述的加速度计的误差函数，

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

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

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

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

上述(7)式和(8)式中q1、q2、q3、q4表示四个四元数的值，表示对加速度计三轴测量值进行向量单位化；

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

3)对磁强计进行地磁补偿，是由于地球磁场的测量值会因磁强计附近铁磁性元素的存在而出现失真，地球坐标系中的干扰源(软铁)在地球磁场的测量方向上产生误差，倾角误差，在垂直平面相对于地球表面的误差，可以进行补偿，

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

4)利用补偿后的磁强计的数据，建立磁强计的误差函数，是将地球磁场看作由水平轴和垂直轴上的分量组成的，将其水平轴和垂直轴上的分量分别表示为式(12)和式(13)如下所示：

(12)式和(13)式中bx、bz分别为地磁场标准化后的地面参考坐标系中x、z轴上磁场强度，mx、my、mz分别为磁强计在x、y、z轴所测量的磁场强度值，机体的坐标系磁强计所测得的磁场强度的数据值；

磁强计的误差函数也是为参考坐标系中所测得的磁场强度减去载体本身的坐标系所测得的磁场的强度的值，式子如下：

对该误差函数进行求导，得到下式的雅克比矩阵：

5)根据加速度计、磁强计两组误差函数求解出四元数，由于仅仅测量重力或地球磁场并不能提供传感器的唯一方向，为了做到这一点，两个领域的测量值和参考方向可以用方程来描述，根据之前所求解的加速度计和磁强计测量数据的误差方程，具体是建立如下(17)式所示的加速度计和地磁传感器的误差函数，并求得的雅可比矩阵，如(18)式所示：

根据(17)式和(18)式计算出误差函数的梯度值，如下(19)式所示；根据梯度下降的迭代求出四元数的值，如下(20)式所示：

(20)式中，μ(x)为梯度下降法的步长。

6)根据梯度下降算法融合策略，是将步骤1)求通过陀螺仪数据解算出的四元数和步骤5)通过加速度计磁强计测出加速度和磁场强度解算的四元数进行融合，解算出更加准确的姿态四元数，通过传感器的坐标系相对于地球的估计方向所得到的通过融合方位计算得到的和将两个四元数进行融合，融合式如下(21)式所示：

(21)式中，γ和(1-γ)分别为加速度计磁强计数据的四元数和陀螺仪数据解算的四元数的权重，当γ为0时，表示只使用陀螺仪测量，不需要使用加速度计/磁力计所测得的相关数据来测量四元数，另一方面,如果γ的值接近1时，(1-γ)趋于0时，这种情况表明对于陀螺的测量解算的四元数不准确误差大，测量的加速度计和磁力计更可靠，最优解需要发散速度和收敛速度相等，要求收敛速度μ(k)/ts和发散速度β相等,因此得下述(22)式：

梯度下降法的收敛速度与载体的运动速度有关，因此必须要求收敛速度要大于运动速度，γ需要比较大，那么由(23)式得μ(k)/ts很大，β非常小，因此得到式(24)：

根据公式(10)、(11)、(21)、(24)可得最终得姿态数据融合公式(25)：

式中表示为前一次融合解算的四元数的值。

7)根据步骤6)得到的姿态四元数，求解出偏航角、横滚角和俯仰角，求解三个角度方法如下所示：

θ＝-arcsin2(q1q3-q0q2)(27)

上述式中，ψ表示偏航角，θ表示俯仰角，φ表示横滚角。