[发明专利]姿态角度解算方法、装置、处理设备和存储介质

权利要求书

1.一种姿态角度解算方法，其特征在于，包括以下步骤：

获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度；

对所述当前时刻三轴角速度进行积分处理，得到下一时刻姿态预测值；

根据所述当前时刻三轴加速度和所述当前时刻三轴磁场强度，得到当前时刻姿态值；

根据所述下一时刻姿态预测值的方差、所述当前时刻姿态值的方差和所述当前时刻姿态值的误差值，得到滤波融合修正系数；

基于所述滤波融合修正系数，对所述下一时刻姿态预测值进行修正，得到下一时刻姿态修正值；所述下一时刻姿态修正值为由所述滤波融合修正系数乘以所述当前时刻姿态值的误差值获得；

将所述下一时刻姿态修正值转换成姿态角度。

2.根据权利要求1所述的姿态角度解算方法，其特征在于，对所述当前时刻三轴角速度进行积分处理，得到下一时刻姿态预测值的步骤中，包括步骤：

在所述当前时刻三轴角速度上叠加所述陀螺仪的当前时刻零偏误差，以对所述当前时刻三轴角速度进行修正；

对修正后的所述当前时刻三轴角速度进行积分处理，得到所述下一时刻姿态预测值。

3.根据权利要求2所述的姿态角度解算方法，其特征在于，所述当前时刻零偏误差为初始时刻的零偏误差时，所述当前时刻零偏误差为所述陀螺仪在预设时间段内处于静止状态下的三轴角速度均值。

4.根据权利要求2所述的姿态角度解算方法，其特征在于，根据所述下一时刻姿态预测值的方差、所述当前时刻姿态值的方差和所述当前时刻姿态值的误差值，得到滤波融合修正系数的步骤中，包括步骤：

根据所述下一时刻姿态预测值的方差、所述当前时刻姿态值的方差和所述当前时刻姿态值的误差值，获取下一时刻零偏误差。

5.根据权利要求1所述的姿态角度解算方法，其特征在于，根据所述当前时刻三轴加速度和所述当前时刻三轴磁场强度，得到当前时刻姿态值的步骤中，包括步骤：

根据所述当前时刻三轴加速度，获取在预设空间坐标系下的X轴向角度和Y轴向角度；

所述当前时刻三轴磁场强度在水平面上的投影，获取在所述预设空间坐标系下的Z轴向角度；

将所述X轴向角度、所述Y轴向角度和所述Z轴向角度进行四元数转换，获取所述当前时刻姿态值。

6.根据权利要求1所述的姿态角度解算方法，其特征在于，获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度步骤：

在预设周期到来时，获取所述陀螺仪采集的所述当前时刻三轴角速度、所述加速度传感器采集的所述当前时刻三轴加速度以及所述磁场传感器采集的所述当前时刻三轴磁场强度。

7.根据权利要求1至6任意一项所述的姿态角度解算方法，其特征在于，获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度的步骤之前，还包括步骤：

执行初始化程序。

8.一种姿态角度解算装置，其特征在于，包括：

数据获取模块，用于获取陀螺仪采集的当前时刻三轴角速度、加速度传感器采集的当前时刻三轴加速度以及磁场传感器采集的当前时刻三轴磁场强度；

姿态预测模块，用于对所述当前时刻三轴角速度进行积分处理，得到下一时刻姿态预测值；

当前姿态获取模块，用于根据所述当前时刻三轴加速度和所述当前时刻三轴磁场强度，得到当前时刻姿态值；

修正系数获取模块，用于根据所述下一时刻姿态预测值的方差、所述当前时刻姿态值的方差和所述当前时刻姿态值的误差值，得到滤波融合修正系数；

修正模块，用于基于所述滤波融合修正系数，对所述下一时刻姿态预测值进行修正，得到下一时刻姿态修正值；所述下一时刻姿态修正值为由所述滤波融合修正系数乘以所述当前时刻姿态值的误差值获得；

转换模块，用于将所述下一时刻姿态修正值转换成姿态角度。