基于IMU与磁力计的手势提取手套-原理及其实现

手势提取依据所采用传感器的不同，可以分为基于视觉，基于惯性传感器，基于FSR，基于EMG传感器的方法。其中基于视觉的方法使用场景有限，且无法获取精确的手指关节角度；基于FSR的方法难以布置传感器且难以获取手指基关节的横向转动角位置；基于EMG的传感器目前来看仅仅纯在于医疗和实验场景中，日常功能性使用受限于EMG采集的方式。故目前市场上已经商用的高精度的手势手套大部分为基于惯性传感器与磁力计的方法，如VR手套。

本文来源于作者的硕士毕设课题，需要用到实时的手势提取作为控制器的输入量，由此开始了这个低成本高精度的手势提取手套的研发，目前原理性问题已经解决。本文将从以下部分说明：

1. 硬件及传感器选择

2. 传感器数据的处理

3. 姿态的融合，关节角度提取

4. 优化方案

由于硕士论文还没开始写，更没有发表，本人也还没毕业，所以本文只说明实现原理与步骤, 暂不提供源码: https://github.com/ShieldQiQi/MRH-HardwareCode

---

一、硬件及传感器选择

硬件MCU考虑到成本，使用的是stm32f103c8t6，然而由于姿态解算需要大量的矩阵及浮点运算，所以实际运行起来(每个MCU处理3个传感器单元)还是有点吃力的。IMU采用经典的MPU6050，磁力计使用国产芯片QMC5883L。

二、传感器数据处理

1.总概

摆在我们面前的有两种传感器，MPU6050集成了三轴的加速度和三轴的角速度，QMC5883L集成了三轴的磁场强度。由于我们使用磁力计仅仅用于姿态解算，所以并不关心磁场的幅值，采样后直接归一化即可。而加速度可用于位置积分，角速度可用于姿态矩阵或者四元数的积分，但在用于提取姿态矩阵时也需要归一化。

我没有采用MPU6050内置的BMP解算姿态，我需要自己融合以获得更加灵活的效果。具体到怎么使用I2C协议提取QMC和MPU我这里不做详细说明，毕竟Github上开源代码一大堆。我将数据手套传感器网络模块化处理，每根手指由一套传感器网络来采集关节角度，具体拓扑图如图2-1所示：

图2-1 传感器网络拓扑

在上图2-1中，每根手指由3片传感器单元覆盖，其中手指基座处的传感器固定于手背，其它两片分别位于近指节和中指节。

2.传感器采样的原始数据处理

单根手指上分布有三个传感器单元，除了中指节上的传感器外，其它两个需要同时用到MPU6050和QMC5883L。

1.第三个传感器不需要QMC是因为我们近指节和中指节之间只有一个自由度，如果近指节上的姿态已知，我们仅仅通过中指节上的加速度计就可以得到近指节和中指节之间的关节角度(注意奇点问题)，然后由这个角度和前一个连杆的姿态获得中指节的姿态。

2.MPU6050集成了陀螺仪和加速度计，实际实验发现该传感器在静止状态下角速度输出平稳，角速度和加速度存在一定偏移，如果使用积分求姿态，一定要对角速度的偏移进行修正，如：在静止状态下采取各轴角速度的平均值，然后把该数据作为后期实际应用的偏置即可。

3.QMC5883L由于各种环境因素和磁场干扰环境的存在，波动较大(最好用队列做个平滑滤波)，且偏移较大而且每颗芯片偏移都不一致，所以对每片都需要进行详细的椭圆模型校准：当我们把磁力计的轴做一个球面旋转，那么最终在该轴上的磁场强度理论上为一个球体，然而由于传感器制造和环境的不同，实际上每个轴在不同的方向测得的最大磁场强度都不一样(偏置)，而且对于XYZ三个轴，每个轴的最大变化范围也不一样(说明为椭球的原因)，所以需要我们对每个轴的磁场强度先进行偏置，然后磁场强度归一化，只有这样得到的数据才是相对准确的，不然用未修正的数据得到的关节角度或者连杆姿态误差很大。

三、姿态的融合，关节角度提取

1.姿态提取

当我们已经能够获得单根手指三个传感器的原始数据后，以单个传感器为例，我们就可以通过三轴加速度和三轴磁场强度获取静止状态下的传感器姿态矩阵，即所在的指节(连杆矩阵)：

$$ Matrix=\left[ \begin{matrix} eastX & eastY & eastZ \\ magnetHorizX & magnetHorizY & magnetHorizZ \\ accelX & accelY & accelZ \end{matrix} \right] \tag{1} $$

在上式(1)中,

1.第三行为加速度获得的三轴加速度归一化后的结果，静止时，该向量为重力加速度的反向同模向量(参考加速度计原理)，所以可以理解为全局坐标系的天轴在传感器(MPU6050)坐标系的各轴上的投影，反过来就是传感器坐标系在全局坐标系天轴(Z轴)上的投影，所以归一化后我们可以直接作为传感器姿态矩阵的第三行。

2.第二行同理可由磁力计(磁场指向北，所以可以作为北轴(Y轴))获得(需要提前变换到MPU6050的矩阵里表示)，但值得注意的是，由于磁场强度在我们北半球并不是平行于地面，而是斜插向地面，所以需要将其分解为一个指向北平行于地面的向量和一个与重力同向的向量，然后取水平平行向量归一化作为第二行即可，至于如何分解，这种小问题就留给读者吧。

3.第一行(东轴)我们直接根据第二行(北轴)和第三行(天轴)向量做叉乘得到。

2.关节角度提取

获得基座和近指节的姿态矩阵后，通过第三个传感器的加速度计获得第三个传感器姿态矩阵的第三行，第二个传感器姿态矩阵与第三传感器姿态矩阵是一个绕自身X轴旋转Theta角度的关系(具体绕哪个轴看安装的位置)，然后就可以解出该角度，利用该角度可继续获得第三传感器姿态矩阵的另外两行(其实得到角度后就没必要再算该矩阵了)(注意奇点问题)。

同理，我们通过第一和第二传感器的姿态矩阵之间的运算，可以直接得到基关节出两个正交的自由度(使用D-H法建模，逆解得出关节角度解析式)。

四、优化方案

静止时我们可以通过加速度计和磁力计获得姿态，进而得到关节角度，但是由于运动过程中，加速度计测得的不再仅仅包含重力加速度，而是存在一些其它随机方向的加速度分量，所以此时得到的姿态不再准确，需要我们利用角速度积分得到新的姿态矩阵，关于如何运用，请看我的上一篇博文:

https://www.cnblogs.com/QiQi-Robotics/p/14562475.html

最简单的，我们可以采取静止时使用加速度计法，运动时采用积分法，一旦静止下来，就会通过加速度法对姿态进行修正以防止积分的累计误差。更为人们做研究的是一些卡尔曼滤波方法，利用积分做预测，利用加速度法直接解算做修正，但是由于二者的预测和测量的误差协方差是不定的，所以理论上是得不到最佳的最小误差效果，但应该可以通过工程实验大致确定一个参数范围以获得一个比较好的效果。我正在实验中。

discre kalman fliter

2021-04-11更新，分别测试了两段式互补滤波，连续式互补滤波以及卡尔曼滤波，效果如下图：

从左到右依次为：连续互补滤波；两段式互补滤波；卡尔曼滤波(红线为滤波后，绿线为滤波前)

五、目前效果