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

权利要求书

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为自适应系数。