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

本公开涉及姿态估计，尤其涉及一种基于多传感器融合的姿态估计方法及系统。

背景技术：

1、动作捕捉技术的发展，穿戴式设备在体育训练、康复医疗、虚拟现实等领域得到广泛应用。这些设备通常集成有惯性测量单元(imu)，包括加速度计、陀螺仪和磁力计，用于获取人体运动数据。然而，在复杂的室内环境中，由于电磁设备、金属结构等因素导致的磁场干扰会对imu中的磁力计读数产生影响，进而影响姿态估计的准确性和稳定性。

2、目前，虽然有多种姿态估计算法，如扩展卡尔曼滤波(ekf)、互补滤波器(cf)等，但这些算法在磁场干扰环境下表现不佳。

技术实现思路

1、本公开提出了一种基于多传感器融合的姿态估计方法及系统技术方案。

2、根据本公开的一方面，提供了一种基于多传感器融合的姿态估计方法，包括：

3、获取磁力计测量值smm、第一预设阈值及小于所述第一预设阈值的二预设阈值；

4、根据所述磁力计测量值确定指示磁场变化方向的序列，基于所述指示磁场变化方向的序列确定设定范围对应的序列和；

5、基于所述序列和对应的数值与所述第一预设阈值和/或所述二预设阈值的大小关系，利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计或利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计或维持上一时刻的传感器融合模式进行姿态估计。

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

7、优选地，所述利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计的方法，包括：获取角速度矢量测量值sωm在k时刻对应的第一四元数姿态估计值的第一变化率所述加速度矢量测量值sam在k时刻对应的第二四元数姿态估计值的第二变化率所述k时刻的上一k-1时刻对应的四元数姿态最优估计值所述k时刻与所述k-1时刻之间的时间增量δk；对所述第一变化率及所述第二变化率做差后，乘以所述时间增量δk，得到所述时间增量δk内对应的变化率差值；对所述时间增量δk内对应的变化率差值与所述四元数姿态最优估计值进行求和，得到所述k时刻对应的姿态。

8、优选地，所述利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计的方法，包括：获取角速度矢量测量值sωm在k时刻对应的第一四元数姿态估计值的第一变化率所述加速度矢量测量值sam及所磁力计测量值smm在k时刻对应的第二四元数姿态估计值的第二变化率所述k时刻的上一k-1时刻对应的四元数姿态最优估计值所述k时刻与所述k-1时刻之间的时间增量δk；对所述第一变化率及所述第二变化率做差后，乘以所述时间增量δk，得到所述时间增量δk内对应的变化率差值；对所述时间增量δk内对应的变化率差值与所述四元数姿态最优估计值进行求和，得到所述k时刻对应的姿态。

9、优选地，在所述对所述第一变化率及所述第二变化率做差之前，对所述第二变化率进行归一化处理，得到归一化的第二变化率对所述第一变化率及所述归一化的第二变化率做差。

10、优选地，所述对所述第一变化率及所述第二变化率做差后，乘以所述时间增量δk，得到所述时间增量δk内对应的变化率差值之前，还包括：获取陀螺仪对应的发散率β；利用所述发散率β对所述第二变化率进行调整，得到调整后的第二变化率；对所述第一变化率及所述调整后的第二变化率做差后，乘以所述时间增量δk，得到所述时间增量δk内对应的变化率差值。

11、优选地，所述利用所述发散率β对所述第二变化率进行调整，得到调整后的第二变化率的方法，包括：所述发散率β乘以所述第二变化率得到调整后的第二变化率

12、优选地，所述角速度矢量测量值sωm在k时刻对应的第一四元数姿态估计值的第一变化率确定方法，包括：获取第一变化率预设计算公式及其相应的第一组计算参数，将所述第一组计算参数带入所述第一变化率预设计算公式，确定所述角速度矢量测量值sωm在k时刻对应的第一四元数姿态估计值的第一变化率其中，所述第一组计算参数，包括：所述k时刻的上一k-1时刻对应的四元数姿态最优估计值陀螺仪在传感器坐标系s中的偏对称矩阵、所述角速度矢量测量值sωm。

13、优选地，在所述将所述第一组计算参数带入所述第一变化率预设计算公式，确定所述角速度矢量测量值sωm在k时刻对应的第一四元数姿态估计值的第一变化率之前，对所述第一组计算参数中的所述角速度矢量测量值sωm进行归一化处理。

14、优选地，确定所述第一变化率预设计算公式的方法，包括：在计算k+1时刻对应的四元数q(k+δk)及k时刻对应的四元数q(k)的差值后，除以所述k+1时刻与所述k时刻之间的时间增量对应的时间增量δk，并在δk→0的条件下求取极限。

15、优选地，所述计算k+1时刻对应的四元数及k时刻对应的四元数的差值的方法，包括：获取在时间区间[k,k+δk]内对应的四元数变化量δq，利用所述k时刻对应的四元数q(k)及所述四元数变化量δq确定所述k+1时刻对应的四元数q(k+δk)；计算所述k+1时刻对应的四元数q(k+δk)与所述k时刻对应的四元数q(k)的差值。

16、优选地，所述利用所述k时刻对应的四元数q(k)及所述四元数变化量δq确定所述k+1时刻对应的四元数q(k+δk)的方法，包括：按照设定阶数对所述四元数变化量δq进行泰勒展开，得到泰勒展开多项式；忽略所述泰勒展开多项式中所述设定阶数对应的高阶项，得到设定阶数多项式；利用所述设定阶数多项式乘以所述k时刻对应的四元数q(k)，确定所述k+1时刻对应的四元数q(k+δk)。

17、优选地，所述在时间区间[k,k+δk]内对应的四元数变化量δq的确定方法，包括：获取陀螺仪的单位旋转轴向量及时间增量δk下陀螺仪的旋转角度；分别对所述旋转角度机械能余弦计算及正弦计算，得到对应的余弦分量及正弦分量；所述单位旋转轴向量乘以所述正弦分量后，加上所述余弦分量，得到在时间区间[k,k+δk]内对应的四元数变化量δq。

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

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

20、优选地，所述重力向量配置为[0 0 0gz]；其中，gz表示所述加速度计在地球坐标系e下测量的垂直分量。

21、优选地，所述地磁向量配置为其中，hz、hx及hy分别表示地磁z方向分量、地磁x方向分量和地磁y方向分量。

22、优选地，所述根据所述磁力计测量值确定指示磁场变化方向的序列的方法，包括：根据所述磁力计测量值确定磁场强度的瞬时变化趋势；利用符号函数对所述磁场强度的瞬时变化趋势进行符号化，得到指示磁场变化方向的序列。

23、优选地，所述基于所述指示磁场变化方向的序列确定设定范围对应的序列和的方法，包括：利用设定滑动窗口大小的滑动窗口确定所述指示磁场变化方向的序列对应的序列和。

24、优选地，其中，所述上一时刻的传感器融合模式，包括：利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计或利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计。

25、优选地，在所述利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计或利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计之前，获取角速度矢量测量值sωm对应的补偿角速度值sωc,k，利用加速度矢量测量值sam及补偿角速度值sωc,k进行姿态估计或利用加速度矢量测量值sam、补偿角速度值sωc,k及磁力计测量值smm进行姿态估计。

26、优选地，所述补偿角速度值sωc,k的确定方法，包括：获取偏置漂移sωb,k；所述角速度矢量测量值sωm减去所述偏置漂移sωb,k，得到所述补偿角速度值sωc,k。

27、优选地，所述偏置漂移sωb,k的确定方法，包括：基于k-1时刻对应的四元数姿态最优估计值的共轭及k时刻对应的第二四元数姿态估计值的第二变化率确定所述k时刻对应的角速度误差sωe,k；所述k时刻对应的角速度误差sωe,k乘以所述k+1时刻与所述k时刻之间的时间增量对应的时间增量δk后，在设定时刻范围进行求和或积分；得到累计角速度误差；所述累计角速度误差乘以设定的比例因子ζ，得到偏置漂移sωb,k。

28、根据本公开的一方面，提供了一种基于多传感器融合的姿态估计系统，包括：

29、获取单元，用于获取磁力计测量值smm、第一预设阈值及小于所述第一预设阈值的二预设阈值；

30、确定单元；用于根据所述磁力计测量值确定指示磁场变化方向的序列，基于所述指示磁场变化方向的序列确定设定范围对应的序列和；

31、估计单元，用于基于所述序列和对应的数值与所述第一预设阈值和/或所述二预设阈值的大小关系，利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计或利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计或维持上一时刻的传感器融合模式进行姿态估计；其中，所述上一时刻的传感器融合模式，包括：利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计或利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计。

32、根据本公开的一方面，提供了一种基于多传感器融合的姿态估计系统，包括：处理器；用于存储处理器可执行指令的存储器；其中，所述处理器被配置为：执行上述基于多传感器融合的姿态估计方法。

33、根据本公开的一方面，提供了一种基于多传感器融合的姿态估计系统，包括：电子设备；其中，所述电子设备进一步包括：处理器；用于存储处理器可执行指令的存储器；其中，所述处理器被配置为：执行上述基于多传感器融合的姿态估计方法。

34、根据本公开的一方面，提供了一种基于多传感器融合的姿态估计系统，包括：计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现上述基于多传感器融合的姿态估计方法。

35、根据本公开的一方面，提供了一种基于多传感器融合的姿态估计系统，包括：计算机程序产品，包括计算机程序/指令，该计算机程序/指令被处理器执行时实现上述的姿态估计方法。

36、根据本公开的一方面，提供了一种基于多传感器融合的姿态估计系统，包括：获取单元，用于获取磁力计测量值smm、第一预设阈值及小于所述第一预设阈值的二预设阈值；确定单元；用于根据所述磁力计测量值确定指示磁场变化方向的序列，基于所述指示磁场变化方向的序列确定设定范围对应的序列和；其中，所述根据所述磁力计测量值确定指示磁场变化方向的序列，包括：根据所述磁力计测量值确定磁场强度的瞬时变化趋势；利用符号函数对所述磁场强度的瞬时变化趋势进行符号化，得到指示磁场变化方向的序列；估计单元，用于基于所述序列和对应的数值与所述第一预设阈值和/或所述二预设阈值的大小关系，利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计或利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计或维持上一时刻的传感器融合模式进行姿态估计。

37、根据本公开的一方面，提供了一种基于多传感器融合的姿态估计系统，包括：、获取单元，用于获取磁力计测量值smm、第一预设阈值及小于所述第一预设阈值的二预设阈值；

38、确定单元；用于根据所述磁力计测量值确定指示磁场变化方向的序列，基于所述指示磁场变化方向的序列确定设定范围对应的序列和；

39、估计单元，用于基于所述序列和对应的数值与所述第一预设阈值和/或所述二预设阈值的大小关系，利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计或利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计或维持上一时刻的传感器融合模式进行姿态估计；其中，所述基于所述序列和对应的数值与所述第一预设阈值和/或所述二预设阈值的大小关系，利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计或利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计或维持上一时刻的融合模式，包括：若所述序列和对应的数值大于或等于所述第一预设阈值，利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计；若所述序列和对应的数值小于或等于所述第二预设阈值，利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计；若所述序列和对应的数值在所述第二预设阈值及所述第一预设阈值之间，维持上一时刻的融合模式进行姿态估计。

40、根据本公开的一方面，提供了一种基于多传感器融合的姿态估计系统，包括：

41、获取单元，用于获取磁力计测量值smm、第一预设阈值及小于所述第一预设阈值的二预设阈值；

42、确定单元；用于根据所述磁力计测量值确定指示磁场变化方向的序列，基于所述指示磁场变化方向的序列确定设定范围对应的序列和；

43、估计单元，用于基于所述序列和对应的数值与所述第一预设阈值和/或所述二预设阈值的大小关系，利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计或利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计或维持上一时刻的传感器融合模式进行姿态估计；其中，所述利用加速度矢量测量值sam及角速度矢量测量值sωm进行姿态估计，包括：获取角速度矢量测量值sωm在k时刻对应的第一四元数姿态估计值的第一变化率所述加速度矢量测量值sam在k时刻对应的第二四元数姿态估计值的第二变化率所述k时刻的上一k-1时刻对应的四元数姿态最优估计值所述k时刻与所述k-1时刻之间的时间增量δk；对所述第一变化率及所述第二变化率做差后，乘以所述时间增量δk，得到所述时间增量δk内对应的变化率差值；对所述时间增量δk内对应的变化率差值与所述四元数姿态最优估计值进行求和，得到所述k时刻对应的姿态；和/或，所述利用加速度矢量测量值sam、角速度矢量测量值sωm及磁力计测量值smm进行姿态估计，包括：获取角速度矢量测量值sωm在k时刻对应的第一四元数姿态估计值的第一变化率所述加速度矢量测量值sam及所磁力计测量值smm在k时刻对应的第二四元数姿态估计值的第二变化率所述k时刻的上一k-1时刻对应的四元数姿态最优估计值所述k时刻与所述k-1时刻之间的时间增量δk；对所述第一变化率及所述第二变化率做差后，乘以所述时间增量δk，得到所述时间增量δk内对应的变化率差值；对所述时间增量δk内对应的变化率差值与所述四元数姿态最优估计值进行求和，得到所述k时刻对应的姿态。

44、在本公开实施例中，提出的一种基于多传感器融合的姿态估计方法及系统技术方案，以解决目前姿态估计算法在磁场干扰环境下表现不佳的技术问题。

45、应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，而非限制本公开。

46、根据下面参考附图对示例性实施例的详细说明，本公开的其它特征及方面将变得清楚。