CN115523919A - 一种基于陀螺漂移优化的九轴姿态解算方法 - Google Patents

Abstract

本发明公开了一种基于陀螺漂移优化的九轴姿态解算方法。首先，基于计算量较低的要求，采用九轴传感器的Madgwick滤波器进行姿态解算。其次提出一种全新的陀螺漂移优化方法，通过智能阈值判断的方法，当陀螺仪没有运动时不额外计算旋转角度。将方法处理后输出角速度作为Madgwick滤波器输入，以实现姿态的优化估计。针对于传统九轴姿态解算方法，该方法能有效地解决陀螺仪漂移带来的航向角解算难题，可以在不同的环境下完成姿态解算。

Description

一种基于陀螺漂移优化的九轴姿态解算方法

技术领域

本发明属于惯性导航领域，具体涉及一种基于陀螺漂移优化的九轴姿态解算方法。

背景技术

姿态解算算法是机器人技术的关键技术之一。微惯性传感器具有尺寸小、功耗低和成本低的优点，已成为工业机器人姿态角度解算和方向感知解决方案的重要传感器之一。通常，微惯性传感器安装在机器人的手臂和手腕处，以检测俯仰角(上下弯曲)和航向角(左右移动)。

加速度计通过检测重力向量，与陀螺仪结合做传感器融合可以解算出精确的俯仰角。航向角计算需要对磁力计预先进行高精度的校准，以保证正确的解算角度。然而，磁力计前期校准的复杂计算会导致计算机能耗的上升，并且磁力计很容易受到环境磁场的影响，例如在工业厂房内或是机器人操作间内。卫星导航***，如GPS(全球定位***)和GNSS(全球导航卫星***)不适用于室内环境，因此姿态解算中的航向角解算将成为了机器人行业的困难之一。

陀螺仪可以提供角速率以计算航向角，而使用陀螺仪作为航向角解算的唯一数据来源，会受到积分运算中的累积误差影响，并且陀螺仪读数会随时间发生漂移，将导致姿态解算结果偏离实际角度估计的范围。在各种类型的传感器融合中，如卡尔曼滤波、互补滤波、Madgwick滤波都是常用的滤波器，Madgwick滤波可以结合加速度计、陀螺仪和磁力计计算航向角。然而在没有磁传感器的情况下，其航向角的解算将成为挑战。

发明内容

为解决上述问题，本发明公开了一种基于陀螺漂移优化的九轴姿态解算方法，解决微惯性传感器陀螺仪漂移带来的姿态解算航向角计算难题，提升姿态解算算法航向角解算的鲁棒性和精度。

为达到上述目的，本发明的技术方案如下：

一种基于陀螺漂移优化的九轴姿态解算方法，包括下列步骤：

步骤1)，通过微惯性传感器中的陀螺仪模块测量载体坐标系下的三轴角速度，角速度的四元数形式定义如下：

其中，s是四元数，w是角速度，x,y,z分别代表载体坐标系下的三个轴；

步骤2)，地球坐标系到载体坐标系的旋转矩阵四元数形式定义如下：

其中，S为载体坐标系，E为地球坐标系，q为四元数，

步骤3)，对四元数进行微分，得到地球坐标系相对于载体坐标系的旋转速率：

步骤4)，引入时间t，构造四元数的递推方程：

t为时刻标志，△t为时间间隔；

步骤5)，通过微惯性传感器中的加速度计模块测量载体坐标系下的三轴加速度和磁力计测量的三轴磁强度四元数形式定义如下：

其中，a为加速度计，m为磁力计，mx,my,mz为磁向量的三个分量；

步骤6)，Madgwick滤波器的主要计算步骤被分为以下四个步骤：

步骤6.1)，第一步，通过角速度测量值计算四元数变化：

步骤6.2)，第二步，通过梯度下降算法，将重力和磁力的两个参考向量应用于矫正角速度计算的四元数，加速度参考向量定义如下：

E_a＝[0,0,0,g] (9)

通过计算雅可比矩阵，计算旋转矫正量F：

其中，E是参考向量，g是地球重力加速度，F是矫正量，I是单位向量，J是雅可比矩阵；

步骤6.3)，将求得的四元数矫正量乘以一个可调整的相关系数后加到四元数变量上：

δq′＝δq-βδs (12)

q_t＝q_t-1+δq′△t (14)

步骤6.4)，最后计算航向角：

步骤7)，在陀螺仪上电启动后，在稳定姿态下执行陀螺漂移优化算法。该算法的主要计算步骤被分为以下三个步骤：

步骤7.1)，第一步，计算零偏补偿变量，并用陀螺仪原始输出减去该变量得到补偿后的角速度输出：

其中，TO表示上电零偏，k表示k时刻的值；

步骤7.2)，第二步，计算运动检测的阈值，读取定量i个数据并求其中的绝对值最大值作为阈值：

Z_th＝m_iax{|Z_gy[1]|,|Z_gy[2]|,...,|Z_gy[i]|} (18)

计算得出阈值后，将补偿后的角速度输出与阈值进行比较，大于阈值即为运动状态，原数据输入到后续滤波中，小于等于阈值即判定为静止状态下的噪声。

其中，th表示阈值，gy表示陀螺仪输出，Z表示Z轴；

步骤7.3)，第三步，确定数据存储的向量长度，根据所需的向量长度来推算所需要的静止时间：

A≥W+N (19)

其中，A是向量长度，W是零偏计算长度，N是阈值计算长度；

最后将该算法得到的优化的角速度输入到Madgwick滤波器中，进行姿态计算。

本发明的有益效果是：

(1)本发明解决了无磁辅助下的航向角解算难题，该算法融合具有较强的稳定性。并且在动态和静态情况下既保证了***的鲁棒性，又维持了原算法的精确度。在受外部环境干扰而无法使用磁力计的工业4.0应用中具有较好的应用前景。

(2)本发明采用针对微惯性传感器陀螺漂移的优化算法，为没有磁力计和GNSS的情况下的航向角解算开辟了一种新方法。使用这种技术作为Madgwick滤波器的预处理输入，而不依赖于使用磁力计等外部传感器输入。

(3)本发明采用Madgwick姿态解算算法，使用传感器测量的加速度和磁场来校正陀螺漂移。相较于复杂的线性滤波算法，大幅提高了处理器的计算效率，算法整体运算量小，适用于需要多点部署微惯性传感器模组的工业机器人。

附图说明

图1是本发明基于陀螺漂移优化的九轴姿态解算方法流程图。

图2是本发明陀螺漂移优化算法框架图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

如图所示，本发明所述的一种基于陀螺漂移优化的九轴姿态解算方法，包括下列步骤：

如图1所示，步骤1)，通过微惯性传感器中的陀螺仪模块测量载体坐标系下的三轴角速度，角速度的四元数形式定义如下：

其中，s是四元数，w是角速度，x,y,z分别代表载体坐标系下的三个轴；

步骤2)，地球坐标系到载体坐标系的旋转矩阵四元数形式定义如下：

其中，S为载体坐标系，E为地球坐标系，q为四元数，t为时刻标志，△t为时间间隔；

步骤3)，对四元数进行微分，得到地球坐标系相对于载体坐标系的旋转速率：

步骤4)，引入时间t，构造四元数的递推方程：

步骤5)，通过微惯性传感器中的加速度计模块测量载体坐标系下的三轴加速度和磁力计测量的三轴磁强度四元数形式定义如下：

其中，a为加速度计，m为磁力计，mx,my,mz为磁向量的三个分量；

步骤6)，Madgwick滤波器的主要计算步骤被分为以下四个步骤：

步骤6.1)，第一步，通过角速度测量值计算四元数变化：

步骤6.2)，第二步，通过梯度下降算法，将重力和磁力的两个参考向量应用于矫正角速度计算的四元数，加速度参考向量定义如下：

E_a＝[0,0,0,g] (9)

通过计算雅可比矩阵，计算旋转矫正量F：

其中，E是参考向量，g是地球重力加速度，F是矫正量，I是单位向量，J是雅可比矩阵；

步骤6.3)，将求得的四元数矫正量乘以一个可调整的相关系数后加到四元数变量上：

δq′＝δq-βδs (12)

q_t＝q_t-1+δq′△t (14)

步骤6.4)，最后计算航向角：

如图2所示，步骤7)，在陀螺仪上电启动后，在稳定姿态下执行陀螺漂移优化方法。该方法的主要计算步骤被分为以下三个步骤：

步骤7.1)，第一步，计算零偏补偿变量，并用陀螺仪原始输出减去该变量得到补偿后的角速度输出：

其中，TO表示上电零偏，k表示k时刻的值；

步骤7.2)，第二步，计算运动检测的阈值，读取定量i个数据并求其中的绝对值最大值作为阈值：

计算得出阈值后，将补偿后的角速度输出与阈值进行比较，大于阈值即为运动状态，原数据输入到后续滤波中，小于等于阈值即判定为静止状态下的噪声。

其中，th表示阈值，gy表示陀螺仪输出，Z表示Z轴；

步骤7.3)，第三步，确定数据存储的向量长度，根据所需的向量长度来推算所需要的静止时间：

A≥W+N (19)

其中，A是向量长度，W是零偏计算长度，N是阈值计算长度；

最后将该算法得到的优化的角速度输入到Madgwick滤波器中，进行姿态计算。

需要说明的是，以上内容仅仅说明了本发明的技术思想，不能以此限定本发明的保护范围，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰均落入本发明权利要求书的保护范围之内。

Claims (6)

1.一种基于陀螺漂移优化的九轴姿态解算方法，其特征在于：包括以下步骤：

步骤1)，建立微惯性传感器测量值四元数模型；

步骤2)，推导旋转四元数微分积分方程；

步骤3)，建立重力参考向量、磁参考向量四元数模型；

步骤4)，将Madgwick滤波器作为姿态解算核心算法，该滤波器通过磁传感器读数校正旋转矢量；

步骤5)，构建陀螺漂移优化算法；

步骤6)，构建整体姿态解算算法，将陀螺漂移优化的角速度值输入Madgwick滤波器解算姿态角。

2.根据权利要求1所述的一种基于陀螺漂移优化的九轴姿态解算方法，其特征在于：步骤1)所述建立微惯性传感器测量值的四元数模型，通过微惯性传感器中的陀螺仪模块测量载体坐标系下的三轴角速度，角速度的四元数形式定义如下：

其中，s为四元数，w为角速度，x,y,z分别代表载体坐标系下的三个轴，w_x为x轴下的角速度，w_y为y轴下的角速度，w_z为y轴下的角速度。

3.根据权利要求1所述的一种基于陀螺漂移优化的九轴姿态解算方法，其特征在于：步骤2)所述解算四元数微分方程，得到地球坐标系相对于载体坐标系的旋转速率：

引入时间t，构造四元数的递推方程：

其中，S为载体坐标系，E为地球坐标系，q为四元数，

为地球坐标系到载体坐标系的旋转四元数，t为时刻标志，△t为时间间隔。

4.根据权利要求1所述的一种基于陀螺漂移优化的九轴姿态解算方法，其特征在于：步骤3)通过微惯性传感器加速度计模块和磁力计模块，测量载体坐标系下的三轴加速度和三轴磁强度的四元数形式定义如下：

其中，a为加速度计，m为磁力计，mx,my,mz为磁向量的三个分量。

5.根据权利要求1所述的一种基于陀螺漂移优化的九轴姿态解算方法，其特征在于：步骤4)所述Madgwick滤波器，

步骤51：通过角速度测量值计算四元数变化量：

步骤52：通过梯度下降算法，将重力和磁力的两个参考向量应用于矫正角速度计算的四元数，加速度参考向量定义如下：

E_a＝[0,0,0,g] (8)

通过计算雅可比矩阵，计算旋转矫正量F：

其中，E是参考向量，g是地球重力加速度，F是矫正量，I是单位向量，J是雅可比矩阵；

步骤53：将求得的四元数矫正量乘以一个可调整的相关系数后加到四元数变量上：

δq′＝δq-βδs (11)

q_t＝q_t-1+δq′△t (13)

步骤54：最后计算航向角：

其中，β是滤波器系数，θ是姿态角度。

6.根据权利要求1所述的一种基于陀螺漂移优化的九轴姿态解算方法，其特征在于：步骤5)所述陀螺漂移优化算法，在陀螺仪上电启动后，在稳定姿态下执行该算法；

步骤61：计算零偏补偿变量，并用陀螺仪原始输出减去该变量得到补偿后的角速度输出：

步骤62：计算运动检测的阈值，读取定量i个数据并求其中的绝对值最大值作为阈值：

计算得出阈值后，将补偿后的角速度输出与阈值进行比较，大于阈值即为运动状态，原数据输入到后续滤波中，小于等于阈值即判定为静止状态下的噪声；

其中，th表示阈值，gy表示陀螺仪输出，Z表示Z轴；

步骤63：定数据存储的向量长度，根据所需的向量长度来推算所需要的静止时间：

A≥W+N (18)

其中，A是向量长度，W是零偏计算长度，N是阈值计算长度；

最后将该算法得到的优化的角速度输入到Madgwick滤波器中，进行姿态计算。

Images