基于多传感器融合的姿态估计方法及系统

1.一种基于多传感器融合的姿态估计方法，其特征在于，包括：

2.根据权利要求1所述的姿态估计方法，其特征在于，所述基于所述序列和对应的数值与所述第一预设阈值和/或所述二预设阈值的大小关系，利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计或利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计或维持上一时刻的融合模式的方法，包括：若所述序列和对应的数值大于或等于所述第一预设阈值，利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计；若所述序列和对应的数值小于或等于所述第二预设阈值，利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计；若所述序列和对应的数值在所述第二预设阈值及所述第一预设阈值之间，维持上一时刻的融合模式进行姿态估计。

3.根据权利要求1-2任一项所述的姿态估计方法，其特征在于，所述利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计的方法，包括：获取角速度矢量测量值sωm在k时刻对应的第一四元数姿态估计值的第一变化率所述加速度矢量测量值sam在k时刻对应的第二四元数姿态估计值的第二变化率所述k时刻的上一k-1时刻对应的四元数姿态最优估计值所述k时刻与所述k-1时刻之间的时间增量δk；对所述第一变化率及所述第二变化率做差后，乘以所述时间增量δk，得到所述时间增量δk内对应的变化率差值；对所述时间增量δk内对应的变化率差值与所述四元数姿态最优估计值进行求和，得到所述k时刻对应的姿态；和/或，

4.根据权利要求3所述的姿态估计方法，其特征在于，所述角速度矢量测量值sωm在k时刻对应的第一四元数姿态估计值的第一变化率确定方法，包括：获取第一变化率预设计算公式及其相应的第一组计算参数，将所述第一组计算参数带入所述第一变化率预设计算公式，确定所述角速度矢量测量值sωm在k时刻对应的第一四元数姿态估计值的第一变化率其中，所述第一组计算参数，包括：所述k时刻的上一k-1时刻对应的四元数姿态最优估计值陀螺仪在传感器坐标系s中的偏对称矩阵、所述角速度矢量测量值sωm；和/或，

5.根据权利要求3-4任一项所述的姿态估计方法，其特征在于，所述加速度矢量测量值sam在k时刻对应的第二四元数姿态估计值的第二变化率的确定方法，包括：获取求解所述k时刻四元数的第一预设迭代公式及对应的第二组计算参数；将所述第二组计算参数带入所述第一预设迭代公式，得到求解所述k时刻四元数其中，所述第二组计算参数，包括：调节参数λ、设定阶数的单位矩阵i、目标函数及其对应的雅可比矩阵所述k-1时刻对应的四元数姿态最优估计值其中，所述目标函数描述通过所述k-1时刻对应的四元数姿态最优估计值旋转地球坐标系e下的理论场向量使其匹配到所述k时刻的加速度计在传感器坐标系s中的测量向量ss对应的加速度矢量测量值其中，所述调节参数λ与所述设定阶数的单位矩阵i的乘积λi用于实现步长动态调节；其中，所述理论场向量配置为加速度计在地球坐标系e下测量的重力向量基于所述求解所述k时刻四元数及所述k时刻上一k-1时刻四元数确定所述加速度矢量测量值sam在k时刻对应的第二四元数姿态估计值的第二变化率和/或，

6.根据权利要求1-5所述的姿态估计方法，其特征在于，所述根据所述磁力计测量值确定指示磁场变化方向的序列的方法，包括：根据所述磁力计测量值确定磁场强度的瞬时变化趋势；利用符号函数对所述磁场强度的瞬时变化趋势进行符号化，得到指示磁场变化方向的序列；和/或，

7.一种姿态估计系统，其特征在于，包括：

8.一种姿态估计系统，其特征在于，包括：

9.一种姿态估计系统，其特征在于，包括：

10.一种姿态估计系统，其特征在于，包括：