基于IMU和Mahony算法的人体手臂关节角度姿态测量系统研究

杨光耀 夏振华 肖一帆 杨泽瑞 吕元

关键词：IMU；手臂关节测量；Mahony算法；跟踪模拟

1 绪论

1.1 研究背景和意义

人体关节角度检测在康复医疗、体育运动、实时监护等领域都非常有应用价值。在康复医疗领域，对关节极限角度进行检测，可以为医生的临床诊断、病人的康复效果评估提供重要参数；在体育运动领域，对关节运动规律捕捉分析，可以为运动员的训练方法提供科学指导;在实时监护领域，获取关节运动信息可以为老年人、康复期患者的跌倒风险评估、步态检测等提供重要依据[1]。

目前关节姿态角测量方法主要有光学传感器测量、三维影像测试分析系统、MEMS惯性传感器测量等。光学传感器测量结果精度较高，然而在动作捕获系统中较少采用这类传感器进行测量，且成本较高，低成本设备难以达到测量精度[2]。三维影像测量需要利用摄像机搭建摄像测量系统，利用OpenCV和嵌入式设备进行分析，数据处理复杂，硬件成本较高。而MEMS技术近几年得到了快速发展，且成本低廉，性价比较高。但是MEMS器件，精度相对较差，同时陀螺仪、加速度计、地磁计单个传感器无法得到满意的姿态角信息，所以需要一些融合算法，进行姿态估计。

本文提出了一种基于MEMS 九轴传感器，采用Mahony姿态解算算法作为数据滤波器的人体手臂关节角度姿态测量系统，算法中以陀螺仪输出角速度为主，加速度计和磁力计的即时输出值对陀螺仪进行修正，最终获得三个姿态角，并以姿态角作为手臂关节运动判断标准和三维模型跟踪可视化，最后通过实验证明了该系统的可行性和准确性。

2 手臂关节测量系统

本文设计的系统主要分为两个部分：1） 两个分别安装在人体上臂和下臂的基于MEMS惯性传感器和MCU的手臂运动检测模块，用于获取人体手臂运动过程中的姿态数据；2） 基于PC的上位机软件，用于接收测姿态角数据和跟踪显示手臂的运动状态。

2.1 MCU 硬件设计

整体硬件框图如图1所示。本系统硬件上由两块相同的测量节点构成，节点原理图、PCB 使用同一Al?tium工程，MCU主控芯片使用STM32F401，六轴陀螺仪、加速度计选用ICM20602，磁力计选用AK8975。MEMS 惯性传感器作为外围芯片与主控芯片STM32F4通过SPI协议进行数据读写，从节点姿态数据通过串口1发送汇总至主节点。

2.2 MCU 软件设计

如图2所示，首先进行外设及惯性传感器初始化，主从节点时序同步后main.c 建立任务调度器并进入循环执行，任务调度中主要实现了传感器数据的获取、预处理及姿态解算，从机以一定频率发送姿态数据，主机对pitch、roll和yaw姿态角汇总并计算关节角度，通过串口转Wi-Fi发送至上位机进行展示。

2.3 UDP 上位机

上位机使用Qt 开发。上位机使用UDP和串口读取接收到的数据，测量模块解算得到最终姿态角。QCustomPlot 绘制的“Pitch 波形”展示了主从节点的姿态角原始数据和滤波数据对比图。上位机的图形界面见图3。

2.4 手臂模型载入与控制

模型的开发基于OpenGL和obj三维模型载入和重绘方式。obj文件通过文本行的形式指定顶点（v）、法线（vn）、纹理坐标（vt）和面（f）等信息，因此本文通过筛选关键字来获得模型的各种数据。最终三维手臂模型导入效果如图4所示。

OpenGL进行模型和视图变换，主要涉及三种变化：translate（），模型平移；rotate（），模型逆时针旋转；scale（num） ，模型缩放。

3 Mahony 姿态解算算法

本文不直接使用加速度和磁场强度信息进行姿态解算，通过设计Mahony 滤波器，使用加速度和磁场强度信息修正机体角速度，算法流程如图5所示。

Mahony 算法主要实现数据融合，算法用PI 控制器对重力加速度、磁力偏向的误差量对陀螺仪数据进行校正[3]。

3.1 加速度和角速度误差

惯性系下的重力加速度在地球表面上任一点方向指向竖直向下，大小为g（≈ 9.81）。MEMS惯性传感器测量得到的物理量是相对于惯性系的，因此需要将惯性系下坐标GR 转换到载体坐标系。只需要左乘惯性系到载体坐标系坐标变换矩阵Gb R，得到理论重力加速度v?：

另外，还可以通过加速度计测量出实际重力加速度向量vˉ。考虑到向量外积模的大小与向量夹角呈正相关，故通过计算外积来得到向量方向差值θ：

在进行叉乘运算前，应先将理论向量v?和实际向量vˉ做单位化处理，故有：

角速度误差的求取本质和加速度误差一致，只是需要注意的是地磁的大小与方向并非固定不变，且磁场向量还需通过GbR变换到地理坐标系中，经过一定处理后，再通过GbR变换回机体坐标系，从而得到理论地磁向量。最终计算得到误差向量em[4] 。

3.2 误差补偿

根据四元数计算欧拉角可得到测量节点姿态。至此，主从机都实现了姿态解算的过程。

3.3 算法验证

开始实验前，模块需要上电后在测量节点初始化过程中尽量将模块置于水平面，这个过程包括两块测量节点进行偏移量校正和主从机的时间同步。并且由于OpenGL控制模型旋转角度的范围是0°～360°，因此上位机需要将得到姿态角数值增加180°。

3.3.1 静态实验

将测量模块静止放置在水平平台，则俯仰角、横滚角和偏航角均为180°。由图6可见yaw角误差会随着时间累积，静态特性较差，而pitch和roll角原始波形较为平稳，静态特性良好。在经过Mahony算法互补滤波之后，各个姿态角精度很高，误差在0. 5°之内，并且没有发生角度偏移现象。

3.3.2 动态实验

在室外进行时长30s的手臂动态实验，动作包括单一姿态角变化和混合姿态角变化。如图7所示，Mahony算法解算的姿态角输出平滑，滤波效果更好，动态误差小。手臂變换运动轨迹时姿态角快速变化时，姿态角曲线光滑，不存在“尖峰”问题，可快速响应姿态的变化。

3.3.3 手臂跟踪动态实验

将姿态测量单元固定于靠近腕关节和肘关节3cm 处，保持身体其他部位不动，肩关节位置可近似看成固定，作为位置原点，此时IMU测量单元的测量精确程度是最高的[5]。图8为手臂姿态跟踪展示。

测量时发现测量节点pitch 接近90°时，测量误差逐渐增大，如表1所示。测量误差源于三方面，一是静态测量需要先将测量节点安装在手臂上，再进行关节尺测量，测量角度很容易出现误差。二是手臂的皮肤不平整也可能造成载体坐标系出现偏移从而影响测量结果。三是pitch测量值在接近90°时会达到极值。

由于yaw姿态角和pitch姿态角本质上都是肘关节和肩关节的屈伸运动，并且由于yaw不存在极值问题，因此在动态测量的表现上要优于pitch角。根据图8 手臂三维模型的运动看出基本和手臂姿态保持一致

4 总结

本课题设计了一种基于IMU和Mahony的双节点手臂姿态测量系统，通过实验对算法进行验证，结果表明本文Mahony融合算法能够有效融合多个传感器数据，可以满足手臂关节姿态高精度解算的任务需求。而且，系统还可以进一步优化：可将传感器替换成集成九轴MEMS惯性传感器可在一定程度减小因焊接过程中可能存在的安装移位造成的坐标系轴的偏移。

MEMS惯性传感器进行偏移量的计算与校正的算法仍然可优化，通过得到精确的偏移量减小偏移误差可使测量结果更精确。如果增加气压计则可基于本课题设计的关节姿态测量模块进行高度测定，本模块作为一个节点进而建立一套可实现全身动作捕捉的测量系统。