[发明专利]一种基于九轴IMU的姿态解算方法、装置及存储介质

权利要求书

1.一种基于九轴IMU的姿态解算方法，其特征在于，所述姿态解算方法包括：

基于获取到的九轴IMU的陀螺仪、加速度计和磁力计的原始数据，分别得到经过标定后的陀螺仪标定数据、加速度计标定数据和磁力计标定数据；

对得到的陀螺仪标定数据和加速度计标定数据进行数据融合，得到初步姿态数据；

确定一个表征磁场时间统计和空间统计的磁场矢量-权重值集合；

其中，通过以下步骤确定所述磁场矢量-权重值集合：

基于每个初始磁场矢量以及该初始磁场矢量对应的权重值，得到多个初始集合元素；

当新增磁场矢量与任意一个初始磁场矢量的差值在预设圆锥范围内时，确定与该新增磁场矢量对应的初始磁场矢量的累计权重值；

当新增磁场矢量与任意一个初始磁场矢量的差值不在预设圆锥范围内时，确定该新增磁场矢量以及该新增磁场矢量对应的权重值为新增集合元素；

基于与该新增磁场矢量对应的初始磁场矢量的累计权重值，或所述新增集合元素，更新所述磁场矢量-权重值集合；

将加速度计标定数据和磁力计标定数据转换成用于矢量描述的加速度矢量和磁场矢量；

当到达预设的磁力修正时刻时，根据所述初步姿态数据和当前磁力修正时刻获得的磁场矢量对所述磁场矢量-权重值集合进行修正，得到修正后的磁场矢量-权重值集合；

其中，所述当到达预设的磁力修正时刻时，根据所述初步姿态数据和当前磁力修正时刻获得的磁场矢量对所述磁场矢量-权重值集合进行修正，得到修正后的磁场矢量-权重值集合，包括：

在九轴IMU处于静止状态下，确定所述磁场矢量-权重值集合中最大的权重值对应的磁场矢量；

基于当前磁力修正时刻下所述磁场矢量以及所述初步姿态数据中的陀螺仪的姿态四元数，确定IMU坐标系下的磁场矢量；

将所述IMU坐标系下的磁场矢量与所述磁场矢量-权重值集合中最大的权重值对应的磁场矢量进行比较，得到当前磁力修正时刻所述磁力计检测到的磁场方向与稳定磁场方向之间的角度差值；

若所述角度差值在预设角度差范围内时，将当前磁力修正时刻IMU坐标系下的磁场矢量添加到所述磁场矢量-权重值集合中；

若所述角度差值不在预设角度差范围内时，则不将当前磁力修正时刻IMU坐标系下的磁场矢量添加到所述磁场矢量-权重值集合中；

当到达预设的姿态修正时刻时，根据修正后的磁场矢量-权重值集合中权重值最高的磁场矢量以及当前处于静止状态的加速度计的加速度矢量，对所述初步姿态数据进行修正，得到最终姿态数据，其中，所述姿态修正时刻对应周期是所述磁力修正时刻对应周期的整数倍；

其中，所述当到达预设的姿态修正时刻时，根据修正后的磁场矢量-权重值集合中权重值最高的磁场矢量以及当前处于静止状态的加速度计的加速度矢量，对所述初步姿态数据进行修正，得到最终姿态数据，包括：

在九轴IMU处于静止状态下，确定所述磁场矢量-权重值集合中最大的权重值对应的磁场矢量；

基于最大的权重值对应的磁场矢量和当前姿态修正时刻由所述加速度计确定出的加速度矢量，更新所述九轴IMU的姿态四元数；

确定更新后的九轴IMU的姿态四元数为最终姿态数据。

2.根据权利要求1所述的姿态解算方法，其特征在于，所述对得到的陀螺仪标定数据和加速度计标定数据进行数据融合，得到初步姿态数据，包括：

根据上一采样时刻已计算得到的初步姿态数据，结合获得的当前采样时刻陀螺仪标定数据，计算得到当前采样时刻的候选初步姿态数据；

若所述九轴IMU处于非静止状态，则基于所述候选初步姿态数据对所述初步姿态数据进行更新，得到更新后的初步姿态数据；

若所述九轴IMU处于静止状态，由当前采样时刻的加速度计标定数据确定在IMU初始坐标系下的当前采样时刻重力矢量；

根据在所述IMU初始坐标系下预先确定出的初始重力矢量，确定当前采样时刻重力矢量和所述初始重力矢量之间的误差矢量；

根据所述候选初步姿态数据以及所述误差矢量，采用数据融合算法，计算得到一个融合后初步姿态数据；基于所述融合后初步姿态数据对所述初步姿态数据进行更新，得到更新后的初步姿态数据；其中，所述IMU初始坐标系为九轴IMU的陀螺仪、加速度计和磁力计经过标定后的初始时刻确定出的坐标系。