CN114061571B - 一种自适应梯度下降惯性测量单元的姿态解算方法及系统 - Google Patents

Abstract

本发明涉及一种自适应梯度下降惯性测量单元的姿态解算方法，包括：步骤S1、获取惯性测量单元的输入数据并进行预处理，得到归一化后的角速度四元数更新值以及归一化后的加速度；步骤S2、利用归一化后的角速度四元数更新值求解预测角速度四元数qPre；步骤S3、利用预测角速度四元数qPre对加速度进行修正及归一化处理，得到归一化后的修正加速度求解梯度方向后归一化处理得到归一化后的梯度方向步骤S4、基于遗传算法对模糊系统进行调参；根据前N个时刻归一化后的修正加速度平均值和标准差，采用模糊系统求得自适应系数w；步骤S5、互补滤波法求解下一时刻的角速度四元数精确值Q_exact。与现有技术相比，本发明具有精确度高的优点。

Description

一种自适应梯度下降惯性测量单元的姿态解算方法及系统

技术领域

本发明涉及海洋自主无人系统领域，尤其是涉及一种自适应梯度下降惯性测量单元的姿态解算方法及系统。

背景技术

精确获取水下自主无人系统的姿态数据，是控制复杂结构水下自主无人系统的前提。惯性测量单元具有体积小、性价比高等优点，是获取姿态数据的常用传感器。六轴惯性测量单元由一个三轴陀螺仪和一个三轴加速度计组成，陀螺仪在快速变化的航行状态下测量结果较准确，但是在长时间积分后会有明显的累积误差，需要用加速度计来校准，而加速度计在快速变化的航行状态下测量结果容易受到运动加速度的干扰。因此，姿态解算的关键在于如何融合两者测量的数据以获得较精确的结果。

现有的惯性测量单元姿态解算方法中通常直接使用初始状态下的加速度计数据消除重力加速度，而没有考虑到运动加速度的影响，或者只是基于单次的运动加速度进行修正，导致算法的动态性能较差。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供了一种精度高的自适应梯度下降惯性测量单元的姿态解算方法及系统。

本发明的目的可以通过以下技术方案来实现：

根据本发明的第一方面，提供了一种自适应梯度下降惯性测量单元的姿态解算方法，该方法包括以下步骤：

步骤S1、获取惯性测量单元的输入数据并进行预处理，得到归一化后的角速度四元数更新值Gq_norm以及归一化后的加速度

步骤S2、利用归一化后的角速度四元数更新值Gq_norm求解预测角速度四元数qPre；

步骤S3、利用预测角速度四元数qPre对归一化后的加速度进行修正以及归一化处理，得到归一化后的修正加速度/>并对其求解梯度方向/>归一化处理后得到归一化后的加速度梯度方向/>

步骤S4、基于遗传算法对模糊系统进行调参；根据前N个时刻归一化后的修正加速度的平均值和标准差，采用调参后的模糊系统求得自适应系数w；

步骤S5、使用互补滤波法求解下一时刻的角速度四元数精确值Q_exact。

优选地，所述步骤S1具体为：

所述角速度的预处理为：将陀螺仪测得的角速度转换为四元数向量的形式，结合采样周期，得到角速度四元数更新值Gq，对其进行归一化处理，得到归一化后的角速度四元数更新值Gq_norm；

所述加速度的预处理为：对加速度计测得的加速度进行归一化处理，得到归一化后的加速度

优选地，所述角速度四元数更新值Gq表达式为：

其中，q_exact表示上一时刻的角速度四元数精确值，表示四元数的微分，Δt表示采样周期；Ω为三轴陀螺仪的角速度四元数，表达式为Ω＝[0 ω_x ω_y ω_z]。

优选地，所述步骤2包括以下子步骤：

步骤S21、将归一化后的角速度四元数更新值Gq_norm转换成估算轴角aa_Est；

步骤S22、获取前N个时刻的角速度四元数精确值，使用最小二乘法进行拟合，得到拟合轴角aa_Fit；

步骤S23、结合上一时刻模糊系统调参得到的自适应系数w，求得预测轴角aa_Pre；

步骤S24、将预测轴角aa_Pre转换成预测角速度四元数qPre。

优选地，所述步骤S23中的预测轴角aa_Pre表达式为：

aa_Pre＝w×aa_Fit+(1-w)×aa_Est

其中，aa_Fit为拟合轴角，aa_Est为估算轴角，w为自适应系数。

优选地，所述步骤S3具体包括以下子步骤：

步骤S31、根据预测角速度四元数qPre求解得到预测重力加速度g^b；

步骤S32、将预测重力加速度g^b与归一化后的加速度相减，得到修正加速度a^non-g，并对其进行归一化处理，得到归一化后的修正加速度/>

步骤S33、对归一化后的修正加速度求解梯度方向/>经归一化处理后得到归一化后的加速度梯度方向/>

优选地，所述步骤S33中梯度方向表达式为：

其中，qPre₁、qPre₂、qPre₃、qPre₄为分别为预测角速度四元数qPre的四个分量，为归一化后的修正加速度。

优选地，所述步骤4中基于遗传算法对模糊系统进行调参，具体包括以下子步骤：

步骤S41、设计并初始化模糊系统；

步骤S42、确定遗传算法的输入参数并对其进行编码；

步骤S43、构建基于三轴姿态角的适应度函数fitnes；

步骤S44、使用遗传算法对模糊系统调参；

步骤S45、对每个调参结果进行判断，计算其适应度值，若适应度值满足输出条件则结束，否则继续执行步骤S44。

优选地，所述步骤S5中下一时刻的角速度四元数精确值Q_exact表达式为：

其中，Gq_norm为归一化后的角速度四元数更新值，w为自适应系数，β为梯度下降步长，为归一化后的加速度梯度方向，Δt为时间间隔。

根据本发明的第二方面，提供了一种基于上述自适应梯度下降惯性测量单元的姿态解算方法的系统，该系统包括：

惯性测量单元数据预处理模块(1)，用于对实时获取的惯性测量单元中的加速度数据和角速度数据进行预处理；

预测角速度四元数求解模块(2)，用于对惯性测量单元数据预处理模块(1)得到的预处理后的角速度四元数进行解算，得到预测角速度四元数；

梯度方向求解模块(3)，用于根据预测角速度四元数求解模块(2)得到的预测角速度四元数，结合惯性测量单元数据预处理模块(1)中的加速度数据求解梯度方向；

模糊系统生成及模糊规则优化模块(4)，分别与梯度方向求解模块(3)以及预测角速度四元数求解模块(2)连接，用于构建模糊系统并对系统进行优化调参，从而得到自适应系数；

姿态解算模块(5)，用于根据惯性测量单元数据预处理模块(1)、梯度方向求解模块(3)、模糊系统生成及模糊规则优化模块(4)中的数据，结合互补滤波法实现数据融合以及姿态解算。

与现有技术相比，本发明具有以下优点：

本发明所提出的方法兼顾了精度和动态性能，其中自适应系数可在动态情况下提高算法精度，预测角速度四元数在动静态情况下都能提高算法的精度；相对传统互补滤波法、梯度下降法和扩展卡尔曼滤波法，本发明所提出的算法有着更高的精度，能够满足水下自主无人系统姿态解算的需求。

附图说明

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

图2为本发明惯性测量单元数据预处理的流程示意图；

图3为本发明求解预测角速度四元数的流程示意图；

图4为本发明求解梯度方向的流程示意图；

图5为本发明模糊系统生成及模糊规则优化的流程示意图；

图6为实施例中系统结构图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。

本发明提供了一种自适应梯度下降惯性测量单元姿态解算方法，在梯度下降法的基础上使用模糊系统求解自适应步长。首先根据当前时刻的陀螺仪数据、上一时刻四元数以及前序若干个时刻的轴角数据求得预测角速度四元数；其次，使用预测角速度四元数求解预测重力加速度，并校准当前时刻的加速度计数据，求得梯度方向，同时使用前序若干个时刻经过校准的加速度计数据结合模糊系统获得自适应系数；最后根据陀螺仪四元数、梯度方向以及自适应系数，使用互补滤波发求得当前时刻的角速度四元数精确值。

接下来对本发明的方法实施例进行详细介绍，该方法包括以下步骤：

步骤S1、获取惯性测量单元的输入数据，包括三轴加速度计测得的原始加速度与三轴陀螺仪测得的角速度，并对数据进行预处理；

所述原始加速度预处理为：对获取到的原始加速度进行归一化处理，得到归一化后的加速度

所述角速度预处理为：将三轴陀螺仪的角速度输入转换为四元数向量的形式，结合采样周期，更新角速度四元数，并对角速度四元数更新值Gq进行归一化处理，得到归一化后的角速度四元数更新值Gq_norm，具体过程为：

1)定义四元数表达式为：

q＝q₁+q₂i+q₃j+q₄k＝[q₁ q₂ q₃ q₄]^T，i²＝j²＝k²＝-1

2)对于获取到的三轴陀螺仪的角速度，其四元数表达式为：

Ω＝[0 ω_x ω_y ω_z]

3)计算角速度四元数更新值Gq：

其中，q_exact表示上一时刻的角速度四元数精确值，表示四元数的微分，Δt表示采样周期，Ω为三轴陀螺仪的角速度四元数；

4)对角速度四元数更新值Gq进行归一化处理，得到归一化后的角速度四元数更新值Gq_norm。

步骤S2、利用预处理后得到的归一化后的角速度四元数更新值Gq_norm求解预测角速度四元数qPre，具体包括以下子步骤：

步骤S21、将归一化后的角速度四元数更新值Gq_norm转换成估算轴角aa_Est，具体为：

1)定义轴角表达式为：

axial angle＝[angle axial₁ axial₂ axial₃]

2)将归一化后的角速度四元数转换为轴角形式，得到估算轴角aa_Est：

其中，q₁、q₂、q₃、q₄为四元数符号；

步骤S22、获取前N个时刻的角速度四元数精确值，使用最小二乘法进行拟合，得到拟合轴角aa_Fit；

步骤S23、结合上一时刻模糊系统调参得到的自适应系数w，求得预测轴角aa_Pre，其表达式为：

aa_Pre＝w×aa_Fit+(1-w)×aa_Est

其中，aa_Fit为拟合轴角，aa_Est为估算轴角；

步骤S24、将预测轴角aa_Pre转换成预测角速度四元数qPre，其表达式为：

步骤S3、利用预测角速度四元数qPre对归一化后的加速度进行修正并归一化后，得到归一化后的修正加速度/>基于归一化后的修正加速度/>以及预测角速度四元数qPre，求解梯度方向，并进行归一化处理，得到归一化后的梯度方向/>具体过程包括以下子步骤：

步骤S31、根据预测角速度四元数qPre求解得到预测重力加速度g^b，其表达式为：

其中，qPre₁，qPre₂，qPre₃，qPre₄为预测角速度四元数qPre的四个分量；

步骤S32、将预测重力加速度g^b与归一化后的加速度相减，得到修正加速度a^non-g，并对其进行归一化处理，得到归一化后的修正加速度/>

步骤S33、基于归一化后的修正加速度和预测角速度四元数qPre，求解加速度梯度方向后进行归一化处理，得到归一化后的加速度梯度方向/>

所述梯度方向表达式为：

其中，qPre₁，qPre₂，qPre₃，qPre₄为预测角速度四元数qPre的四个分量，表示归一化后的修正加速度，

步骤S4、使用遗传算法实现模糊系统生成及模糊规则优化；根据前N个时刻归一化后的修正加速度平均值和标准差，采用调参后的模糊系统求得自适应系数w；

所述使用遗传算法实现模糊系统生成及模糊规则优化，具体为：

步骤S41、设计并初始化模糊系统；

模糊系统的输入值为前N个时刻归一化后的修正加速度的平均值和标准差，输出值为权重；根据经验设定隶属度函数，并初始化模糊规则，如表1所示：

表1初始化模糊规则表

步骤S42、确定遗传算法编码；

首先，对隶属度函数及模糊规则表中的参数变量化(见表2-表3)；

其次，设置一定的约束(见表4)，实现遗传算法的输入参数编码：

表2隶属度函数参数变量化

表3模糊规则表参数变量化

表4变量约束

表中，var指代对应的所有变量名；

所述遗传算法的编码表达式为：

[s1s2s3s4s5s6 a1a2a3a4a5a6 w1w2w3w4w5 r1r2r3r4r5r6r7r8r9]

步骤S43、构建适应度函数；适应度函数用于判断种群个体的优劣程度，是遗传算法的关键；所述适应度函数表达式为：

fitnes＝10-(三轴姿态角的RMSE+三轴姿态角的STD)

其中，RMSE为均方误差，用来衡量观测值同真值之间的偏差；STD为标准差，用来衡量一组数自身的离散程度。

根据公式可知，种群个体越优质，适应度函数越大。

步骤S44、使用遗传算法调参；首先对隶属度函数及模糊规则表中的参数变量化，并设置给定的约束，实现遗传算法的输入参数编码；构建适应度函数，确定模糊规则后完成调参；

具体调参结果见表5：

表5遗传算法调参模糊规则表

步骤S45、对每个调参结果进行判断，计算其适应度值，若适应度值满足输出条件则结束，否则继续执行步骤S44。

最终效果对比见表6。

表6两种调参方式效果对比

	手动调参	遗传算法调参
			适应度值	5.537463	7.155482

适应度值越高说明调参效果越好，从表6可知，遗传算法调参效果远好于手动调参。

步骤S5、根据前N个时刻归一化后的修正加速度的平均值和标准差，结合模糊系统求得自适应系数w，并使用互补滤波法求解下一时刻的四元数精确值q_exact，其表达式为：

其中，Gq_norm为归一化后的角速度四元数更新值Gq_norm，w为自适应系数，β为梯度下降步长，为归一化后的梯度方向，Δt为时间间隔。

下面给出本发明的系统实施例，一种基于上述的自适应梯度下降惯性测量单元的姿态解算方法的系统，该系统包括：

惯性测量单元数据预处理模块(1)，用于实时获取惯性测量单元中加速度输入和陀螺仪输入，并实现对姿态数据的预处理；

预测角速度四元数求解模块(2)，用于对惯性测量单元数据预处理模块(1)得到的预处理后的陀螺仪数据进行解算，得到预测角速度数据；

梯度方向求解模块(3)，用于根据预测角速度四元数求解模块(2)中得到的预测角速度数据，结合加速度数据求解梯度方向；

模糊系统生成及模糊规则优化模块(4)，用于结合梯度方向求解模块(3)中的加速度数据，进行模糊系统的构建并完成机器调参，得到自适应系数；

姿态解算模块(5)，用于根据惯性测量单元数据预处理模块(1)、梯度方向求解模块(3)、模糊系统生成及模糊规则优化模块(4)中的数据，结合互补滤波实现数据融合以及姿态解算。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (8)

1.一种自适应梯度下降惯性测量单元的姿态解算方法，其特征在于，该方法包括以下步骤：

步骤S1、获取惯性测量单元的输入数据并进行预处理，得到归一化后的角速度四元数更新值Gq_norm以及归一化后的加速度

步骤S2、利用归一化后的角速度四元数更新值Gq_norm求解预测角速度四元数qPre；

步骤S3、利用预测角速度四元数qPre对归一化后的加速度进行修正以及归一化处理，得到归一化后的修正加速度/>并对其求解梯度方向/>归一化处理后得到归一化后的加速度梯度方向/>具体包括以下子步骤：

步骤S31、根据预测角速度四元数qPre求解得到预测重力加速度g^b；

步骤S32、将预测重力加速度g^b与归一化后的加速度相减，得到修正加速度a^non-g，并对其进行归一化处理，得到归一化后的修正加速度/>

步骤S33、对归一化后的修正加速度求解梯度方向/>经归一化处理后得到归一化后的加速度梯度方向/>表达式为：

其中，qPre₁、qPre₂、qPre₃、qPre₄为分别为预测角速度四元数qPre的四个分量，为归一化后的修正加速度；

步骤S4、基于遗传算法对模糊系统进行调参；根据前N个时刻归一化后的修正加速度的平均值和标准差，采用调参后的模糊系统求得自适应系数w；

步骤S5、使用互补滤波法求解下一时刻的角速度四元数精确值Q_exact。

2.根据权利要求1所述的一种自适应梯度下降惯性测量单元的姿态解算方法，其特征在于，所述步骤S1具体为：

所述角速度的预处理为：将陀螺仪测得的角速度转换为四元数向量的形式，结合采样周期，得到角速度四元数更新值Gq，对其进行归一化处理，得到归一化后的角速度四元数更新值Gq_norm；

所述加速度的预处理为：对加速度计测得的加速度进行归一化处理，得到归一化后的加速度

3.根据权利要求2所述的一种自适应梯度下降惯性测量单元的姿态解算方法，其特征在于，所述角速度四元数更新值Gq表达式为：

其中，q_exact表示上一时刻的角速度四元数精确值，表示四元数的微分，Δt表示采样周期；Ω为三轴陀螺仪的角速度四元数，表达式为Ω＝[0 ω_x ω_y ω_z]。

4.根据权利要求1所述的一种自适应梯度下降惯性测量单元的姿态解算方法，其特征在于，所述步骤2包括以下子步骤：

步骤S21、将归一化后的角速度四元数更新值Gq_norm转换成估算轴角aa_Est；

步骤S22、获取前N个时刻的角速度四元数精确值，使用最小二乘法进行拟合，得到拟合轴角aa_Fit；

步骤S23、结合上一时刻模糊系统调参得到的自适应系数w，求得预测轴角aa_Pre；

步骤S24、将预测轴角aa_Pre转换成预测角速度四元数qPre。

5.根据权利要求4所述的一种自适应梯度下降惯性测量单元的姿态解算方法，其特征在于，所述步骤S23中的预测轴角aa_Pre表达式为：

aa_Pre＝w×aa_Fit+(1-w)×aa_Est

其中，aa_Fit为拟合轴角，aa_Est为估算轴角，w为自适应系数。

6.根据权利要求1所述的一种自适应梯度下降惯性测量单元的姿态解算方法，其特征在于，所述步骤4中基于遗传算法对模糊系统进行调参，具体包括以下子步骤：

步骤S41、设计并初始化模糊系统；

步骤S42、确定遗传算法的输入参数并对其进行编码；

步骤S43、构建基于三轴姿态角的适应度函数fitnes；

步骤S44、使用遗传算法对模糊系统调参；

步骤S45、对每个调参结果进行判断，计算其适应度值，若适应度值满足输出条件则结束，否则继续执行步骤S44。

7.根据权利要求1所述的一种自适应梯度下降惯性测量单元的姿态解算方法，其特征在于，所述步骤S5中下一时刻的角速度四元数精确值Q_exact表达式为：

其中，Gq_norm为归一化后的角速度四元数更新值，w为自适应系数，β为梯度下降步长，为归一化后的加速度梯度方向，Δt为时间间隔。

8.一种基于权利要求1所述的一种自适应梯度下降惯性测量单元的姿态解算方法的系统，其特征在于，该系统包括：

惯性测量单元数据预处理模块(1)，用于对实时获取的惯性测量单元中的加速度数据和角速度数据进行预处理；

预测角速度四元数求解模块(2)，用于对惯性测量单元数据预处理模块(1)得到的预处理后的角速度四元数进行解算，得到预测角速度四元数；

梯度方向求解模块(3)，用于根据预测角速度四元数求解模块(2)得到的预测角速度四元数，结合惯性测量单元数据预处理模块(1)中的加速度数据求解梯度方向；

模糊系统生成及模糊规则优化模块(4)，分别与梯度方向求解模块(3)以及预测角速度四元数求解模块(2)连接，用于构建模糊系统并对系统进行优化调参，从而得到自适应系数；

姿态解算模块(5)，用于根据惯性测量单元数据预处理模块(1)、梯度方向求解模块(3)、模糊系统生成及模糊规则优化模块(4)中的数据，结合互补滤波法实现数据融合以及姿态解算。