CN106885566A - 一种可穿戴式运动传感器及其抗磁场干扰的方法 - Google Patents

Abstract

本发明公开了一种可穿戴式运动传感器及其抗磁场干扰的方法，属于可穿戴式传感器的研究领域，可以在有磁场干扰的情况下实时精确地估算运动传感器当前的姿态角。本发明利用传感器模块采集的加速度、角速度和磁场的实时数据，根据这些信息判断传感器当前的运动状态和外部磁场干扰情况，然后采用自适应的策略进行多传感器信息融合，计算并输出运动传感器的姿态角。本发明使用方便，不受场地限制，成本低廉，可以在有磁场干扰的情况下实时、高精度地测量人体部位的姿态角，拥有较高的可靠性以及较好的推广前景。

Description

一种可穿戴式运动传感器及其抗磁场干扰的方法

技术领域

本发明属于可穿戴传感器领域，具体涉及一种可穿戴式运动传感器及其抗磁场干扰的方法。

背景技术

随着微机电系统(MEMS)技术的发展，基于MEMS的惯性传感器和磁力计以成本低、体积小、重量轻的优点被广泛地应用在人体运动分析领域，如手势识别，关节运动学分析，日常活动监测。一般地，一个典型的运动传感器包括三轴加速度计、三轴陀螺仪和三轴磁力计，对于人体运动分析来说，准确地测量身体部位的姿态非常关键。为了准确地获取身体部位的姿态，通过融合算法进行多传感器信息融合是常用的方法，比较常用的融合算法有扩展卡尔曼滤波法、梯度下降法、互补滤波法等。通过这些算法，可以提高估算姿态的精确度。然而，估算的精度仍然容易受到外部环境的干扰，特别是环境中的磁场干扰。因为地磁场非常微弱，日常环境中的建筑物，电磁设备，电脑，手机等都会产生较大的磁场干扰。由磁场干扰引起的偏航角的均方根误差会达到15.4°(Yadav et al.在磁场干扰条件下通过AHRS精确估计姿态Sensors 14 2014)。为了解决磁场干扰对估算精度的影响，有些方法通过设定了一个磁场强度阈值，超过阈值的磁场被认为是无效磁场，但是阈值的调整是一个非常繁琐的过程，而且很难找到一个非常合适的阈值。又如申请号为CN201310431846.6的发明专利中公开了一种运动惯性追踪系统，该系统通过移除磁力计模块来回避磁场干扰对精度的影响的问题。然而，减少磁力计模块后，无法获得绝对偏航角，且偏航角会存在漂移误差。而申请号为CN201510666248.6的发明专利中公开了一种基于微惯性传感器的室内定位方法，该方法具有静动态测量数据辨识模块，能够针对静态和动态分别采取不同的方法估算姿态角，但是该方法没有针对磁场干扰进行特别的优化，在环境中有磁场干扰时，仍然会引入较大的估算误差。在有磁场干扰条件下精确估算姿态角对人体运动分析具有重要的意义，有必要提出一种针对外界磁干扰进行特别优化的方法，使得运动传感器的姿态估计具有较强的抗磁场干扰能力。

发明内容

本发明的目的在于解决现有技术中抗磁场干扰能力弱的问题，并提供一种可穿戴式运动传感器及其抗磁场干扰的方法，解决了运动传感器在外部有磁场干扰时精度降低的技术问题。本发明为解决技术问题，所采用的具体技术方案如下：一种可穿戴式运动传感器，包括锂电池、电源管理模块、MCU模块、传感器模块和状态指示模块，所述锂电池与电源管理模块相连，所述电源管理模块、传感器模块以及状态指示模块均与MCU模块相连。

进一步的，所述MCU模块上集成有WIFI。

进一步的，所述传感器模块由加速度计、陀螺仪以及磁力计组成。

本发明的另一目的是提供一种利用可穿戴式运动传感器的抗磁场干扰的方法，包括以下步骤：实时测量传感器模块的加速度、角速度和磁场信息，根据加速度和角速度信息进行静态判断，如果为静态，则保持当前姿态不变，如果为非静态，则进行外部磁场干扰程度计算，根据磁场干扰程度计算出融合加速度和角速度的6轴算法以及融合加速度、角速度和磁场的9轴算法的权重，加权后得到传感器的当前姿态，

式中：为估算的传感器在t时刻的姿态(四元数形式)，为6轴算法得到的姿态，为9轴算法得到的姿态，λ和1-λ分别为6轴算法和9轴算法的权重。

进一步的，所述静态判断具体步骤如下：进行静态检测时，设置了加速度静态判断条件和角速度判断条件，只有同时满足这两个条件时才判定当前传感器处于静态。

加速度的静态判断条件可描述为在一定的时间段内，3轴方向上的加速度变化幅度小于设定的阈值，可以表示为：

式中：表示传感器在t时刻X轴方向的加速度，t₀表示一个可调的时间间隔，是t-t₀时刻X轴方向的加速度，th_a是设定的静态判断的阈值，Y轴，Z轴方向的加速度静态判断条件与X轴的条件相同，X、Y、Z轴的加速度静止判断条件是“与”的关系。

角速度数据判断的条件可描述为3轴的角速度必须分别小于一个设定的阈值，可表示为：

式中：ω_xω_yω_z分别是3轴的角速度，th_gyro是设定的角速度静止判断阈值，X,Y,Z轴的角速度静止判断条件是“与”的关系。

进一步的，所述6轴算法的权重λ的求解过程如下：

将测得的磁场强度和磁倾角与地磁场进行比较，从而确定干扰的程度，其计算公式可表示为：

λ₁＝|||mag||-m₀|/m₀ifλ₁＞1，λ₁＝1

λ₂＝|θ_dip-θ₀|/th_dip ifλ₂＞1，λ₂＝1

λ＝(λ₁+λ₂)/2

式中：||mag||为当前测得的磁场大小，θ_dip为当前测得的磁倾角，m₀和θ₀分别是地磁场大小和磁倾角，th_dip为设定的最大磁倾角误差，λ₁是通过磁场大小计算的磁场干扰程度，λ₂是根据磁倾角计算的干扰程度，最后的权重λ取λ₁、λ₂的平均值。

本发明相对于现有技术而言，其有益效果是：

1、使用本发明计算姿态角，在静态情况下，可以使传感器的角度估算精度对任意强度和时长的磁场干扰免疫。

2、在动态情况下，能够明显降低外部磁场干扰对偏航角估算精度的影响。

3、本发明不依赖于具体的融合算法，通用性好，可以附加在常用的姿态估算算法上，帮助其增强抗磁场干扰能力。

4、本发明硬件结构简单，使用元器件少，能够明显地减小电路板体积和降低成本。

附图说明

图1为本发明中运动传感器结构示意图；

图2为本发明中抗磁场干扰方法的结构图；

图3为本发明中加速度静态判断参数确定示意图；

图4为本发明中静态抗磁场干扰验证方法示意图；

图5为本发明中静态抗磁场干扰验证结果图；

图6为本发明中动态抗磁场干扰验证方法示意图；

图7为本发明中动态抗磁场干扰验证时外部磁场强度图；

图8为本发明中动态抗磁场干扰验证相对欧拉角误差图；

图9为本发明中动态抗磁场干扰验证相对欧拉角均方根误差统计图。

具体实施方式

下面结合附图对本发明进行进一步说明，因便于更好地理解。本发明中的技术特征在不相互冲突的前提下，均可进行相互组合，不构成限制。

本发明中所涉及的部分名词含义如下：

姿态角也称为欧拉角，是一种直观的姿态表示方法，本发明涉及的欧拉角为ZYX旋转顺序的欧拉角，其中绕Z轴旋转为偏航角，绕Y轴旋转为俯仰角，绕X轴旋转为滚转角。

四元数是姿态表示的另一种方法，可以理解为绕一个单位向量旋转一个角度，四元数表示法可以避免欧拉角表示法存在的奇异性问题，一个四元数可以表示为：

其中，e＝[e_xe_ye_z]表示旋转轴，θ表示矢量绕旋转轴旋转的角度。

6轴算法是指仅通过融合三轴加速度、三轴角速度信息来估算姿态角的多传感器信息融合算法。

9轴算法是指通过融合三轴加速度、三轴角速度和三轴磁场信息来估算姿态角的多传感器信息融合算法。

本发明使用一个带有加速度计、陀螺仪和磁力计的运动传感器以及用于该设备的抗磁场干扰的方法，实时估算传感器当前的姿态。本发明具体的实施过程如下：

1)准备工作：

图1为本发明中运动传感器系统模块结构图，包括锂电池、电源管理模块、MCU模块、传感器模块和状态指示模块，所述锂电池与电源管理模块相连，所述电源管理模块、传感器模块以及状态指示模块均与MCU模块相连。本发明的抗磁场干扰的方法通过编程的方式实现在运动传感器中，进行姿态角的实时估算，所述的MCU模块上集成有WiFi模块，本实施例选择为TI公司的CC3200芯片，但不限于此；所述所述传感器模块由加速度计、陀螺仪以及磁力计组成，本实施例选择InvenSense公司的9轴传感器一体化传感器MPU9250，但不限于此。所述运动传感器在使用之前，需进行磁力计校准，以去掉固定的磁场干扰。运动传感器在使用过程中，采样频率为200Hz。

2)静态判断与磁场干扰程度计算

图2所示为本发明中抗磁场干扰方法结构框图，运动传感器实时采集当前的加速度，角速度和磁场信息，首先根据加速度和角速度进行静止状态检测，如果判断为静态的，则保持当前姿态不变，如果非静态，则进行外部磁场干扰程度计算。进行静态检测时，设置了加速度静态判断条件和角速度判断条件，只有同时满足这两个条件时才判定当前传感器处于静态。

加速度的静态判断条件可描述为在一定的时间段内，3轴方向上的加速度变化幅度小于设定的阈值，可以表示为：

式中：表示传感器在t时刻X轴方向的加速度，t₀表示一个可调的时间间隔，是t-t₀时刻X轴方向的加速度，th_a是设定的静态判断的阈值，Y轴，Z轴方向的加速度静态判断条件与X轴的条件相同，X、Y、Z轴的加速度静止判断条件是“与”的关系。图3所示为加速度静态判断阈值选择示意图，选取的th_a应该比加速度计在静态时的峰峰值稍大，t₀决定了实际静态到判定静态的延迟，与在本实施例中，t₀选取为0.5s，th_a选取为0.04g。

角速度数据判断的条件可描述为3轴的角速度必须分别小于一个设定的阈值，可表示为：

式中：ω_xω_yω_z分别是3轴的角速度，th_gyro是设定的角速度静止判断阈值，X，Y，Z轴的角速度静止判断条件是“与”的关系。

同样地，设定的th_gyro应该比陀螺仪在静态时的峰峰值稍大，在本实施例中，th_gyro选取为3°/s。

在计算磁场干扰程度时，将测得的磁场强度和磁倾角与地磁场进行比较，从而确定干扰的程度，其计算公式可表示为：

λ₁＝|||mag||-m₀|/m₀ifλ₁＞1，λ₁＝1

λ₂＝|θ_dip-θ₀|/th_dip ifλ₂＞1，λ₂＝1

λ＝(λ₁+λ₂)/2

式中：||mag||为当前测得的磁场大小，θ_dip为当前测得的磁倾角，m₀和θ₀分别是地磁场大小和磁倾角，他们的值在磁力计校准的过程中确定。th_dip为设定的最大磁倾角误差，λ₁是通过磁场大小计算磁场干扰程度，λ₂是根据磁倾角计算的干扰程度，最后的权重λ取λ₁、λ₂的平均值。

在本实施例中，磁场干扰程度计算的参数确定为：m₀＝0.46Gs，θ₀＝40.6°，th_dip＝20°，实时的磁倾角通过公式θ_dip＝arccos(A(q)g·h/||h||)计算，式中，A(q)为当前姿态的旋转矩阵形式，g为重力加速度，h为测得的磁场。

3)传感器姿态角的计算

如图2所示，在本发明所述方法中，同时进行融合加速度、角速度信息的6轴算法和融合加速度、角速度磁场信息的9轴算法，本实施例的6轴算法和9轴算法均为基于的梯度下降法的融合算法。在传感器姿态求解过程中，当传感器处于静态时，直接以上一次估算的姿态作为本次姿态。当传感器处于动态时，根据计算得到的干扰程度权重λ和1-λ分别应用于6轴算法和9轴算法，得到本次的姿态，可表示为：

式中：为估算的传感器在t时刻的姿态(四元数形式)，为6轴算法得到的姿态，为9轴算法得到的姿态。

4)静态抗磁场干扰验证

图4为本发明中静态抗磁场干扰验证方法示意图，在本实验中，将传感器静止放置在没有磁场干扰的平板上，将一个圆形的永磁铁来回靠近运动传感器，以此方式来模拟外界的磁场干扰，采集这段时间的加速度、角速度和磁力计信息，同时用本发明所述的方法和原始的9轴算法估算传感器当前的姿态角，通过比较两种方法得到的姿态角来确定本发明的静态抗磁场干扰效果。

5)动态抗磁场干扰验证

动态抗磁场干扰验证实验在一个3轴仪器转台上进行，该3轴转台具有XYZ三个旋转自由度，与欧拉角姿态表示法的XYZ轴一一对应，转台的每个旋转轴配有电机和编码器，电机用于提供旋转动力，编码器用于测量每个轴的旋转角度，因编码器的精度高，于是将测得的3轴角度作为标准值与估算的姿态角进行比较，从而可得到估算的姿态角的精度。

图6为本实施例的动态抗磁场干扰验证方法示意图，左下角为仪器转台的坐标系，将运动传感器固定在X轴框架上。X轴框架上安装有磁场干扰模拟装置，包括两端封闭的方形塑料管和一个圆形永磁铁。实验进行时，XYZ轴同时转动，圆形永磁铁会在重力作用下来回滑动，永磁铁与运动传感器的距离也随之变化，从而产生变化的磁场干扰，以此方式模拟外界的磁场干扰。在此条件下，同时使用本发明的方法、原始的9轴算法、原始的6轴算法分别估算传感器的姿态角，与转台提供的标准姿态角进行比较，得到相对欧拉角误差，通过比较3种算法的估算误差，确定本发明的动态抗磁场干扰效果。

6)抗磁场干扰的效果

在本实施例中进行了静态抗磁场干扰实验和动态抗干扰实验，图5为静态抗干扰实验结果，可以看到，本发明估算的姿态角完全不受外界磁场干扰影响，在强磁场干扰下，仍然保持不变，符合实际情况，而原始的9轴算法的偏航角偏差则达到了50°。本实施例中，动态抗干扰实验共进行了10次，图7为其中一次动态抗磁场干扰实验时外部磁场强度图，由图可见，磁场干扰周期性地出现。图8为本次实验三种对比方法估算的相对欧拉角误差图，图9为全部的10次实验得到的相对欧拉角均方根误差图，由图8和图9可见，本发明所述的方法得到的相对欧拉角误差明显小于原始的6轴算法和9轴算法。

由此可见，本发明无论在静态还是在动态条件下，均提高了估算姿态角的精度。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，有关技术领域的普通技术人员，在不脱离本发明的精神和范围的情况下，还可以做出各种变化和变型。如选择的6轴算法和9轴算法也可以使用基于扩展卡尔曼滤波或互补滤波的融合算法。由此可见，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种可穿戴式运动传感器，其特征在于，包括锂电池、电源管理模块、MCU模块、传感器模块和状态指示模块等，所述锂电池与电源管理模块相连，所述电源管理模块、传感器模块以及状态指示模块均与MCU模块相连。

2.根据权利要求1所述的可穿戴式运动传感器，其特征在于，所述MCU模块上集成有WIFI模块。

3.根据权利要求1所述的可穿戴式运动传感器，其特征在于，所述传感器模块由加速度计、陀螺仪以及磁力计组成。

4.一种利用权利要求1所述的可穿戴式运动传感器的抗磁场干扰的方法，其特征在于，所述方法具体如下：实时测量传感器模块的加速度、角速度和磁场信息，根据加速度和角速度信息进行静态判断，如果为静态，则保持当前姿态不变，如果为非静态，则进行外部磁场干扰程度计算，根据磁场干扰程度计算出融合加速度和角速度的6轴算法以及融合加速度、角速度和磁场的9轴算法的权重，加权后得到传感器的当前姿态，

$\begin{array}{cc}{q_E^S}_t=\mathrm{\λ}{q_E^S}_{IMU,t}+(1-\mathrm{\λ}){q_E^S}_{MIMU,t}& 0\≤\mathrm{\λ}\≤1\end{array}$

式中：为估算的传感器在t时刻的姿态(四元数形式)，为6轴算法得到的姿态，为9轴算法得到的姿态，λ和1-λ分别为6轴算法和9轴算法的权重。

5.根据权利要求4所述的可穿戴式运动传感器的抗磁场干扰的方法，其特征在于，所述静态判断具体步骤如下：进行静态检测时，设置了加速度静态判断条件和角速度判断条件，只有同时满足这两个条件时才判定当前传感器处于静态。

加速度的静态判断条件可描述为在一定的时间段内，3轴方向上的加速度变化幅度小于设定的阈值，可以表示为：

$\left\{\begin{array}{c}|a_{x_t}-a_{x_{t-t_0}}|<t{h}_a\\ |a_{y_t}-a_{y_{t-t_0}}|<{\mathrm{th}}_a\\ |a_{z_t}-a_{z_{t-t_0}}|<{\mathrm{th}}_a\end{array}\right.$

式中：表示传感器在t时刻X轴方向的加速度，t₀表示一个可调的时间间隔，是t-t₀时刻X轴方向的加速度，th_a是设定的静态判断的阈值，Y轴，Z轴方向的加速度静态判断条件与X轴的条件相同，X、Y、Z轴的加速度静止判断条件是“与”的关系。

角速度数据判断的条件可描述为3轴的角速度必须分别小于一个设定的阈值，可表示为：

$\left\{\begin{array}{c}\left|{\mathrm{\&omega;}}_x\right|<t{h}_{gyro}\\ \left|{\mathrm{\&omega;}}_y\right|<{\mathrm{th}}_{gyro}\\ \left|{\mathrm{\&omega;}}_z\right|<{\mathrm{th}}_{gyro}\end{array}\right.$

式中：ω_xω_yω_z分别是3轴的角速度，th_gyro是设定的角速度静止判断阈值，X，Y，Z轴的角速度静止判断条件是“与”的关系。

6.根据权利要求4所述的用于可穿戴式运动传感器的抗磁场干扰的方法，其特征在于，所述6轴算法的权重λ的求解过程如下：

将测得的磁场强度和磁倾角与地磁场进行比较，从而确定干扰的程度，其计算公式可表示为：

$\begin{array}{ccc}{\mathrm{\&lambda;}}_1=\frac{\left|\right|\left|mag\right||-m_0|}{m_0}& if& {\mathrm{\&lambda;}}_1>1,{\mathrm{\&lambda;}}_1=1\end{array}$

$\begin{array}{ccc}{\mathrm{\&lambda;}}_2=\frac{|{\mathrm{\&theta;}}_{dip}-{\mathrm{\&theta;}}_0|}{{\mathrm{th}}_{dip}}& if& {\mathrm{\&lambda;}}_2>1,{\mathrm{\&lambda;}}_2=1\end{array}$

λ＝(λ₁+λ₂)/2

式中：||mag||为当前测得的磁场大小，θ_dip为当前测得的磁倾角，m₀和θ₀分别是地磁场大小和磁倾角，th_dip为设定的最大磁倾角误差，λ₁是通过磁场大小计算的磁场干扰程度，λ₂是根据磁倾角计算的干扰程度，最后的权重λ取λ₁、λ₂的平均值。