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

说明书

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

步骤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包括以下子步骤：