CN110864684A

CN110864684A - 用户姿态测算方法

Info

Publication number: CN110864684A
Application number: CN201911210177.3A
Authority: CN
Inventors: 蔡浩原; 赵晟霖; 崔松叶; 李文宽; 刘春秀
Original assignee: Shenzhen Qianhai Weisheng Intelligent Technology Co ltd; Institute of Electronics of CAS
Current assignee: Shenzhen Qianhai Weisheng Intelligent Technology Co ltd; Institute of Electronics of CAS
Priority date: 2019-11-29
Filing date: 2019-11-29
Publication date: 2020-03-06

Abstract

一种用户姿态测算方法，包括：获取加速度计采集的用户的加速度向量数据，监测加速度向量数据在预设时间段的模值的变化量；根据模值的变化量，判断陀螺仪是否处于静止或匀速状态，若是，采集当前时刻陀螺仪的角速度向量数据，记录为静止漂移向量数据；消除陀螺仪采集的用户的角速度向量数据中的静止漂移向量数据；采用消除静止漂移向量数据后的角速度向量数据对磁力计进行辅助校准；设置信赖参数，信赖参数用于度量加速度计及所述磁力计的准确度；根据信赖参数，对用户的加速度向量数据、用户的角速度向量数据及磁力计测量的磁场向量数据进行融合，得到当前时刻用户姿态对应的四元数。该方法具有校准便捷、实时，航向角漂移小的优点。

Description

用户姿态测算方法

技术领域

本发明涉及惯性导航领域，尤其涉及一种用户姿态测算方法。

背景技术

获取设备的位姿信息是消费电子领域的关键技术之一。利用惯性传感器(如加速度计、陀螺仪、磁力计等)可以获取设备的姿态角。现有的基于惯性传感器的姿态测算方法存在随时间延长，航向角漂移增大的问题，即使是使用椭球拟合对磁力计进行校准的算法也有不能实时校准、不能应对环境变化、对用户动作要求高等缺点。

相关技术中，通过卡尔曼滤波或互补滤波对陀螺仪、加速度计和磁力计进行融合，然而，该方法对磁力计的校准采取平面校准法或八字校准法，如果在执行一次校准动作后直接使用，就不能实时应对环境的变化，从而导致误差的积累。

发明内容

(一)要解决的技术问题

针对于现有的技术问题，本发明提出一种用户姿态测算方法，用于至少部分解决上述技术问题之一。

(二)技术方案

本发明一方面提供一种用户姿态测算方法，包括：获取加速度计采集的用户的加速度向量数据，监测上述加速度向量数据在预设时间段的模值的变化量；根据上述模值的变化量，判断上述陀螺仪是否处于静止或匀速状态，若是，采集当前时刻陀螺仪的角速度向量数据，记录为静止漂移向量数据；消除陀螺仪采集的用户的角速度向量数据中的静止漂移向量数据；采用消除静止漂移向量数据后的角速度向量数据对磁力计进行辅助校准；设置信赖参数，上述信赖参数用于度量上述加速度计及上述磁力计的准确度；根据上述信赖参数，对上述用户的加速度向量数据、上述用户的角速度向量数据及上述磁力计测量的磁场向量数据进行融合，得到当前时刻用户姿态对应的四元数。

可选地，上述采用消除静止漂移向量数据后的角速度向量数据对磁力计进行辅助校准包括：基于上述消除静止漂移向量数据后的角速度向量数据，采用扩展卡尔曼滤波算法对上述磁力计进行辅助校准。

可选地，上述根据上述信赖参数，对上述用户的加速度向量数据、上述用户的角速度向量数据及上述磁力计测量的磁场向量数据进行融合包括：将地球坐标系下的重力向量数据及磁场向量数据转换到传感器坐标系下，得到参考加速度向量数据及参考磁场向量数据；将上述参考加速度向量数据与上述用户的加速度向量数据叉乘，得到第一控制误差；将上述参考磁场向量数据上述磁力计测量的磁场向量数据叉乘，得到第二控制误差；设置上述信赖参数的大小，调节第一控制误差及第二控制误差的权重；根据上述权重，结合历史时刻用户姿态对应的四元数，对上述用户的加速度向量数据、上述用户的角速度向量数据及上述磁力计测量的磁场向量数据进行融合，得到当前用户姿态对应的四元数。

可选地，上述根据上述模值的变化量判断上述陀螺仪是否处于静止或匀速状态包括：判断上述变化量是否小于预设阈值，若是，则使用静止计数器计数一次，若否，将静止计数器计数次数清零，重新判断；重复上述操作，直至上述静止计数器计数次数达到预设次数，此时判定上述陀螺仪处于静止或匀速状态。

可选地，上述采用消除静止漂移向量数据后的角速度向量数据对磁力计进行辅助校准还包括：设置控制参数，上述控制参数用于在对磁力计进行校准过程中，确定利用上述消除静止漂移向量数据后的角速度向量数据进行辅助校准所占的权重。

可选地，在上述对上述用户的加速度向量数据、上述用户的角速度向量数据及上述磁力计测量的磁场向量数据进行融合之前，还包括：判断上述磁力计是否校准完成，采用校准完成后的磁力计测量的磁场向量数据与上述用户的加速度向量数据及上述用户的角速度向量数据进行融合。

可选地，上述判断上述磁力计是否校准完成包括：获取磁力计测得的一段磁场数据，以该段磁场数据最后一点的数据为真实值数据，计算其它磁场数据与该真实值数据的均方根误差；判断上述均方根误差是否小于预设阈值，若是，则上述磁力计校准完成。

可选地，当上述用户为静止或或匀速状态时，将上述信赖参数设置为恒定值；当上述用户的加速度逐渐增大时，逐渐减小上述信赖参数的值。

可选地，上述结合历史时刻用户姿态对应的四元数，对上述用户的加速度向量数据、上述用户的角速度向量数据及上述磁力计测量的磁场向量数据进行融合，得到当前用户姿态对应的四元数包括：采用公式

进行迭代运算后积分，得到当前用户姿态对应的四元数；其中，

表示上一时刻迭代的用户姿态对应四元数，^Sω_t表示当前时刻陀螺仪测量的用户的角速度向量数据，

表示当前时刻用户姿态对应的四元数的微分，t表示时刻。

可选地，采用互补滤波算法对上述用户的加速度向量数据、上述用户的角速度向量数据及上述磁力计测量的磁场向量数据进行融合；上述方法还包括：对上述扩展卡尔曼滤波算法及上述互补滤波算法进行初始化。

(三)有益效果

本发明提出的一种用户姿态测算方法，至少具有如下有益效果：

1、基于陀螺仪采集的消除静止漂移向量数据后的角速度向量数据，采用扩展卡尔曼滤波对磁力计进行辅助校准，实现了磁场的动态校准，兼备速度与精度，无需执行复杂的校准动作，具有校准便捷、实时的优点。

2、通过设置信赖参数，完成九轴数据融合过程中，对加速度向量数据及磁场向量数据的筛选，减小了用户所处环境突变带来的误差，具备，航向角漂移小的有点。

3、基于陀螺仪辅助校准磁力计的方法，可以应用到更多的场景，如VR设备、可穿戴设备、汽车导航等，相比于其它基于动态或静态的磁场校准方法更便捷，可以突破一些使用上的限制。

附图说明

图1示意性示出了本发明实施例提供的用户姿态测算方法的原理图；

图2示意性示出了本发明实施例提供的用户姿态测算方法的流程图；

图3示意性示出了本发明实施例提供的陀螺仪静止或匀速状态的判断方法流程图；

图4示意性示出了本发明实施例提供的模拟信赖参数的指数函数曲线图；

图5示意性示出了本发明实施例提供的九轴数据融合的原理图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。

本发明实施例提供一种用户姿态测算方法，该方法的原理如图1所示，在磁力计校准过程中，基于陀螺仪采集的消除静止漂移向量数据后的角速度向量数据进行辅助校准，在九轴数据融合过程中，通过设置信赖参数，对加速度向量数据及磁场向量数据的筛选，从而得到用户姿态对应的四元数。

图2示意性示出了本发明实施例提供的用户姿态测算方法的流程图。如图2所示，该方法例如可以包括操作S201～S206。

S201，对扩展卡尔曼滤波和互补滤波算法进行初始化。

在后续的磁力计校准及九轴数据融合时，分别可以用到扩展卡尔曼滤波和互补滤波算法。由于扩展卡尔曼滤波和互补滤波算法都是迭代的，因此要首先初始化算法中各种变量的迭代初值，包括：硬磁干扰矩阵，软磁干扰矩阵，旋转矩阵，四元数向量，陀螺仪漂移误差向量等。

S202，获取加速度计采集的用户的加速度向量数据，监测加速度向量数据在预设时间段的模值的变化量，根据模值的变化量，判断陀螺仪是否处于静止或匀速状态，若是，采集当前时刻陀螺仪的角速度向量数据，记录为静止漂移向量数据。

陀螺仪的主要误差为静止漂移误差，它是一个随时间变化的值。为了消除陀螺仪采集的三轴角速度向量数据(单位rad/s，记为g＝[g_x g_y g_z]^T)中的静止漂移误差，需要获取陀螺仪处于静止或匀速状态下陀螺仪角速度向量数据，该数据记为陀螺仪的偏移量(记为ε＝[ε_x ε_y ε_z]^T)。具体方法流程如图3所示。

首先，需要动态的获取加速度计测量的三轴加速度向量数据(单位m/s²)，监测三轴加速度向量数据在预设时间段内的模值变化量(记为Δ|g|)，即计算当前时刻模值与上一时刻模值的差值。

其次，根据模值的变化量，判断陀螺仪是否处于静止或匀速状态。具体地，判断一时刻模值的变化量是否小于预设阈值(该阈值可以为一个远小于G(9.81m/s2)的值(0.001))，若是，则使用静止计数器计数一次，静止计数器加1，若否，将静止计数器计数次数清零，重新判断下一时刻模值的变化量是否小于预设阈值。重复上述操作，直至静止计数器计数次数达到预设次数(计数器计数次数已满)，此时判定陀螺仪处于静止或匀速状态。

其中，上述步骤中，记录加速度模值变化和陀螺仪漂移时都需要进行低通滤波处理，设各自的低通滤波参数为α和β，则迭代公式分别为

Δ|g|_k＝(1-α)Δ|g|_k-1+α(Δ|g|_k-Δ|g|_k-1)

ε_k＝(1-β)ε_k-1+βε_k

其中，k表示某时刻，k-1表示k时刻的上一时刻。

S203，消除陀螺仪采集的用户的角速度向量数据中的静止漂移向量数据。

获取到陀螺仪静止或匀速状态下的陀螺仪的偏移量ε后，从陀螺仪测量的角速度向量数据中减去ε，即可得到陀螺仪的校准值。

S204，采用消除静止漂移向量数据后的角速度向量数据对磁力计进行辅助校准。

前述操作过程中，对陀螺仪最大的误差来源进行了消除，此时，就可以使用校准好的陀螺仪来辅助校准磁力计。

具体地，通过陀螺仪的角速度向量数据对应的角速度矩阵与旋转矩阵I的关系，可以计算得到磁力计的理论值递推公式：

B_k+1＝(I+[ω_k]_×Δt)B_k

其中，[ω_k]_×为角速度矩阵，Δt为采样间隔，B_k为k时刻的磁力计的磁场向量数据对应矩阵，B_k+1为k+1时刻的磁力计的磁场向量数据对应矩阵。

将磁场向量数据对应矩阵B、硬磁干扰矩阵V和软磁干扰矩阵W中独立的6个分量(W₁₁ W₂₂ W₃₃ W₁₂ W₁₃ W₂₃)组合成卡尔曼滤波的状态量x，从而得到状态转移函数

的具体形式：

B_cal，k＝(I+[ω_cal，k-1]_×Δt)B_cal，k-1-[ε_g]_×ΔtB_cal，k-1

W_element，k＝W_{elemment，k-1}

V_k＝V_k-1

其中，[ε_g]_×为由陀螺仪的静止漂移构成的3×3矩阵，W_element为W中的6个独立分量构成的列向量，V为硬磁干扰矩阵，它本身是个3×1的列向量，k表示时刻，B_cal为校准的磁场向量数据对应矩阵，ω_cal为校准的角速度向量数据对应矩阵。

在对磁力计进行校准过程中，为确定利用消除静止漂移向量数据后的角速度向量数据进行辅助校准所占的权重，可以设置一控制参数

则上述状态方程的方差矩阵Q_k-1可以为

其中，

用于控制对陀螺仪的利用程度。当陀螺仪采样率较低，或者漂移现象比较严重时，将其取较大值，减少滤波过程中状态方程计算得到的值所占的比重，使得滤波结果较多依靠磁力计的测量值。σ_g为陀螺仪测量白噪声，可根据经验预设。0_x*y为零矩阵。

磁力计的测量方程为

B_p，k＝W_k B_cal，k+V_k+ε_m

其中，B_p为磁力计的直接测量值，B_cal为磁力计的校准值，W和V分别为软磁、硬磁干扰矩阵，ε_m为测量噪声，由于状态量是

所以测量方程B_p，k＝h(X_k)+V_k是非线性的，所以需要求其雅可比矩阵使其线性化。状态量为x_k＝[B_x B_y B_z W₁₁ W₂₂ W₃₃W₁₂ W₁₃ W₂₃ V₁ V₂ V₃]^T，对测量方程B_p，k＝W_k B_cal，k+V_k+ε_m求一阶偏导，得到：

H_k＝[W_k|k-1，h_Wk，I_3×3]

其中，H_k是测量方程的雅可比矩阵，W_k|k-1仍然是软磁干扰矩阵，I_3×3为三阶单位阵。

基于上述磁力计校准过程，可以执行任意动作来校准，不需要严格的绕“8”字运动进行校准。

S205，设置信赖参数，该信赖参数用于度量所述加速度计及磁力计的准确度。

对于加速度计，影响算法的主要误差来源于非重力加速度，对于磁力计，校准后的磁场值也有可能在附近存在很大干扰的情况下出现误差。所以，为了减小不利情况的影响，本发明实施例可以采用信赖参数来度量各自的准确程度，当用户的加速度从零逐渐增大时，逐渐减小信赖参数的值。

对于加速度，分三种情况来考虑：

1.静止或匀速状态，此时加速度计只有重力加速度作用，信赖参数为恒定值，且为最大值。

2.低加速度状态，此时出现了非重力加速度的其他加速度干扰，但干扰并不大，对算法影响也不大，应该使算法适当减少对加速度计的依赖，使信赖参数略微减小。

3.高加速状态，这是最大的误差来源，应该使对加速度计的依赖无限减小，当非重力加速度极大时甚应使其趋近于0。

本发明实施例使用一个指数函数来模拟信赖参数与实际情况的关系，记归一化后的加速度向量为a，γ为一系数，K_p0为互补滤波中PI控制器的比例环节(常数)，则指数函数K_p可以表示为：

K_p＝K_p0 exp(-γΔ|a|)，Δa＝|a|-1

取K_p0＝1，γ＝2，则指数函数如图4所示。

对于磁场校准值则同理，因为校准后的磁场强度是一定的，对公式中的γ和1进行调整即可得到校准磁信息的信赖参数函数。

S206，根据信赖参数，对用户的加速度向量数据、用户的角速度向量数据及磁力计测量的磁场向量数据进行融合，得到当前时刻用户姿态对应的四元数。

由于磁场校准需要短暂的时间，在启动算法进行数据融合时，如果直接使用EKF输出的磁场信息，有时会导致算法失准，所以开启时应该暂时屏蔽EKF的输出，但保持EKF的运行。同时对EKF输出的磁场校准值进行判断。如果可用，就把EKF输出的磁场校准值输入到后面的互补滤波中，如不可用，就截断磁力计向互补滤波的输入。

动态判断的方法为：获取磁力计测得的一段磁场数据(可以取一段固定长度的窗口在每次迭代计算出的磁场强度上向前滑动)，以该段磁场数据最后一点的数据为真实值数据，计算其它磁场数据与该真实值数据的均方根误差；判断均方根误差是否小于预设阈值，若是，则磁力计校准完成。窗口的长度越长，判定的条件就越严格；窗口的长度越短，RMSE对实际运动的跟随越快，同时也会损失一部分精度。RMSE阈值、窗口长度可以根据经验设定。

判定校准完成后，进行数据融合，首先将地球坐标系下的重力向量数据及磁场向量数据转换到传感器坐标系下，得到参考加速度向量数据及参考磁场向量数据；然后，将参考加速度向量数据与用户的加速度向量数据叉乘，得到第一控制误差；将参考磁场向量数据磁力计测量的磁场向量数据叉乘，得到第二控制误差；最后，设置信赖参数的大小，调节第一控制误差及第二控制误差的权重，根据权重，结合历史时刻用户姿态对应的四元数，对用户的加速度向量数据、用户的角速度向量数据及磁力计测量的磁场向量数据进行融合，得到当前用户姿态对应的四元数。

姿态角可以通过四元数迭代得到，设陀螺仪测量的角速度向量为^Sω＝[0 ω_x ω_yω_z]，那么离散形式的四元数导数为

其中，

表示上一时刻迭代的用户姿态对应四元数，它表示从地球系到载体系的旋转，上标^表示它是后验的“真实值”，^Sω_t表示当前时刻陀螺仪测量的用户的角速度向量数据，

表示当前时刻用户姿态对应的四元数的微分，t表示时刻。如此，当Δt足够小时，当前时刻的四元数就可以通过对它的微分进行积分得到：

下面结合具体的原理图进行说明，传感器的融合原理如图5所示，其中，K_p在数值上等于上述信赖参数。参见图5，其中，^Sω_t、^Sa_t、^Sm_t分别表示t时刻在传感器坐标系下的角速度向量数据、加速度向量数据和磁场向量数据，

表示修正后的角速度向量数据，

和

表示把参考向量数据从地球坐标系通过旋转矩阵

旋转到传感器坐标系的理论预测值，它们与^Sa_t、^Sm_t做叉乘得到控制误差，分别通过PI控制器，其中，K_p在数值上等于上述信赖参数。^Eg＝[0 0 1]^T为地球坐标系下的重力向量(参考向量)，^Eb＝[0 b_y b_z]^T为地球坐标系下的磁场向量(参考向量)，它是把磁力计测量值先旋转到地球系下，再把x轴分量和y轴合并到y轴，并假定x轴为0得到的。q/||q||表示对四元数做归一化。

本发明实施例对上述用户姿态测算方法进行验证，在桌面上原地旋转100圈，时长为10min，航向角仅漂移0.08°；穿戴于手臂，在标准400米操场上慢跑，航向角漂移速度仅为0.09°/min。两者航向角漂移角均很小。

综上所述，本发明实施例提出一种用户姿态测算方法，基于陀螺仪采集的消除静止漂移向量数据后的角速度向量数据，采用扩展卡尔曼滤波对磁力计进行辅助校准，实现了磁场的动态校准，兼备速度与精度，无需执行复杂的校准动作。通过设置信赖参数，完成九轴数据融合过程中，对加速度向量数据及磁场向量数据的筛选，减小了用户所处环境突变带来的误差。基于陀螺仪辅助校准磁力计的方法，可以应用到更多的场景，如VR设备、可穿戴设备、汽车导航等，相比于其它基于动态或静态的磁场校准方法更便捷，可以突破一些使用上的限制。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims

1.一种用户姿态测算方法，其特征在于，包括：

获取加速度计采集的用户的加速度向量数据，监测所述加速度向量数据在预设时间段的模值的变化量；

根据所述模值的变化量，判断所述陀螺仪是否处于静止或匀速状态，若是，采集当前时刻陀螺仪的角速度向量数据，记录为静止漂移向量数据；

消除陀螺仪采集的用户的角速度向量数据中的静止漂移向量数据；

采用消除静止漂移向量数据后的角速度向量数据对磁力计进行辅助校准；

设置信赖参数，所述信赖参数用于度量所述加速度计及所述磁力计的准确度；

根据所述信赖参数，对所述用户的加速度向量数据、所述用户的角速度向量数据及所述磁力计测量的磁场向量数据进行融合，得到当前时刻用户姿态对应的四元数。

2.根据权利要求1所述的方法，其特征在于，所述采用消除静止漂移向量数据后的角速度向量数据对磁力计进行辅助校准包括：

基于所述消除静止漂移向量数据后的角速度向量数据，采用扩展卡尔曼滤波算法对所述磁力计进行辅助校准。

3.根据权利要求1所述的方法，其特征在于，所述根据所述信赖参数，对所述用户的加速度向量数据、所述用户的角速度向量数据及所述磁力计测量的磁场向量数据进行融合包括：

将地球坐标系下的重力向量数据及磁场向量数据转换到传感器坐标系下，得到参考加速度向量数据及参考磁场向量数据；

将所述参考加速度向量数据与所述用户的加速度向量数据叉乘，得到第一控制误差；将所述参考磁场向量数据所述磁力计测量的磁场向量数据叉乘，得到第二控制误差；

设置所述信赖参数的大小，调节第一控制误差及第二控制误差的权重；

根据所述权重，结合历史时刻用户姿态对应的四元数，对所述用户的加速度向量数据、所述用户的角速度向量数据及所述磁力计测量的磁场向量数据进行融合，得到当前用户姿态对应的四元数。

4.根据权利要求1所述的方法，其特征在于，所述根据所述模值的变化量判断所述陀螺仪是否处于静止或匀速状态包括：

判断所述变化量是否小于预设阈值，若是，则使用静止计数器计数一次，若否，将静止计数器计数次数清零，重新判断；

重复上述操作，直至所述静止计数器计数次数达到预设次数，此时判定所述陀螺仪处于静止或匀速状态。

5.根据权利要求1或2所述的方法，其特征在于，所述采用消除静止漂移向量数据后的角速度向量数据对磁力计进行辅助校准还包括：

设置控制参数，所述控制参数用于在对磁力计进行校准过程中，确定利用所述消除静止漂移向量数据后的角速度向量数据进行辅助校准所占的权重。

6.根据权利要求1所述的方法，其特征在于，在所述对所述用户的加速度向量数据、所述用户的角速度向量数据及所述磁力计测量的磁场向量数据进行融合之前，还包括：

判断所述磁力计是否校准完成，采用校准完成后的磁力计测量的磁场向量数据与所述用户的加速度向量数据及所述用户的角速度向量数据进行融合。

7.根据权利要求6所述的方法，其特征在于，所述判断所述磁力计是否校准完成包括：

获取磁力计测得的一段磁场数据，以该段磁场数据最后一点的数据为真实值数据，计算其它磁场数据与该真实值数据的均方根误差；

判断所述均方根误差是否小于预设阈值，若是，则所述磁力计校准完成。

8.根据权利要求3所述的方法，其特征在于，当所述用户为静止或或匀速状态时，将所述信赖参数设置为恒定值；

当用户的加速度逐渐增大时，逐渐减小所述信赖参数的值。

9.根据权利要求3所述的方法，其特征在于，所述结合历史时刻用户姿态对应的四元数，对所述用户的加速度向量数据、所述用户的角速度向量数据及所述磁力计测量的磁场向量数据进行融合，得到当前用户姿态对应的四元数包括：

采用公式

表示当前时刻用户姿态对应的四元数的微分，t表示时刻。

10.根据权利要求2所述的方法，其特征在于，采用互补滤波算法对所述用户的加速度向量数据、所述用户的角速度向量数据及所述磁力计测量的磁场向量数据进行融合；

所述方法还包括：对所述扩展卡尔曼滤波算法及所述互补滤波算法进行初始化。