于向怀，向 政，孟祥涛

(北京航天时代光电科技有限公司，北京 100094)

0 引言

近年来，随着“科技助力体育”口号的提出，越来越多的技术应用到了人体运动测量领域[1-2]。目前，常见的姿态测量方法有基于光学标记点的定位测姿方法、基于图像分析的姿态识别方法以及惯性技术姿态测量。基于光学标记点的定位测姿方法和基于图像分析的姿态识别方法均为基于光学的姿态测量方法，在水面之上进行姿态测量时因为水的折射、水花和人体对光线的遮挡等问题，无法实现较好的姿态测量效果；水下光学测量则需要在狭窄环境下对设备进行布置，需要较多摄像头才能够针对全泳段进行姿态测量，成本较高且实现难度较大。目前，在国家体育总局训练局已经安装布置了一套15m泳段的光学姿态测量设备，但其仅能提供侧向图像的展示，不能给出定量姿态指标，同时价格也极为昂贵。随着惯性技术中具有轻量化和成本低两个特性的MEMS惯性器件的发展，惯性技术作为一种不依赖外部信息、多场景应用的技术，也逐渐引入人体运动测量中。

惯性姿态测量需要通过初始对准技术提供初值从而不断积分计算得到连续的运动姿态信息。最传统的初始对准方法是抗晃动粗对准[3]，其算法核心是假设载体处于静止状态，测量得到的角速度和加速度信息等于地球自转角速度和重力加速度，两者相比从而计算得到初始姿态角，但该算法忽略了晃动和惯性器件测量误差的影响。另外，因为游泳运动佩戴传感器轻质化、小型化的要求，必须使用MEMS器件进行测量，而目前的MEMS器件仍然不能很好地感应地速，所以抗晃动粗对准方法无法应用于游泳运动中。目前，惯性领域中常用的晃动基座下初始对准方法[4]则需要较长的时间才能完成相关工作，对运动员的使用体验影响很大，所以一个快速且精度满足要求的初始对准方法成为了亟待解决的问题。

本文提出了一种新的基于Mahony互补滤波算法的初始对准方法，基于MEMS-SINS初始对准原理，使用加速度计和磁力计对陀螺估计的角度进行修正，修正过程采用Mahony算法，很好地结合了陀螺的低频性能和加速度计的高频性能，可以快速提供一个精度符合游泳运动需求且具有一定抗干扰能力的初始对准结果。

1 游泳运动姿态特性分析

游泳运动是运动幅度较大、速度较快、全身肢体协调性要求较高的一项运动。对于竞技游泳运动选手来说，提供最为全面和准确的反馈定量分析十分重要，游泳运动测量设备的设计需要与游泳运动过程的运动特性进行结合。

游泳运动具有以下特性：

1)游泳运动过程中的姿态变化具有一定的周期性；

2)游泳运动过程中的速度维持在1m／s～2m／s；

3)游泳运动过程中的横滚角变化范围为±60°；

4)游泳运动过程中的转动角速度范围为±720(°)／s；

5)游泳运动过程中不存在较长的静止时间；

6)游泳运动需要尽可能地降低水的阻力。

综上，游泳运动过程极为复杂，且对姿态测量设备的体积、质量以及使用体验等有极高的要求，所以采用MEMS惯性器件组合作为姿态测量设备，并研究快速初始对准算法。因为惯性技术测量的是固定点位的姿态信息，所以选择一个合适的测量点对反馈游泳运动过程极为重要。研究表明，髋部在游泳过程中可以集中体现运动员躯干扭转幅度、扭转频率等关键信息，所以以其作为惯性姿态测量点反映运动员整体姿态运动较为合适，设备佩戴示意图如图1所示。

图1 设备佩戴背部示意图Fig.1 Schematic diagram of equipment wearing back

2 MEMS-SINS初始对准原理

静基座初始对准时，其输出与东北天导航坐标系中比力的关系为

式(2)中，fb＝[fxfyfz]T为加速度计的输出，gn＝[0 0g]T为当地重力矢量，g为当地水平重力加速度。由式(1)和式(2)可以得到

从而计算得到两个水平姿态角θ和γ，但无法获取偏航角ψ， 故引入磁强计提供偏航角的参考信息。

用校准后的三轴磁强计载体坐标系的磁场强度[5-7]来确定导航坐标系与载体坐标系的关系

式(4)中，Mb为载体坐标系下的三维磁场强度，Mn为导航坐标系下的三维磁场强度，H0为当地地磁场水平强度，α为当地磁偏角，β为当地磁倾角。

由于

所以可以确定偏航角ψ。

综上，由式(3)和式(8)可以从加速度信息和磁场强度信息计算得到姿态角，可以实现利用MEMS设备的游泳运动姿态测量初始姿态角的计算。

3 Mahony互补滤波方法

MEMS捷联惯性导航系统计算得到的初始姿态角需要进行快速的修正从而获得较高精度的结果。Mahony于2008年提出了一种非线性互补滤波进行姿态估计的算法适合应用于此过程[8-10]，其主要思想是利用地理系下的重力进行三轴角速度的修正，计算量小，且有着良好的跟随能力和鲁棒性，可以快速地得到姿态信息，但受限于重力矢量，只能够对两个水平姿态角进行估计。本文中引入了地磁场进行补充，从而能够在偏航角方向提供参考，并进行估计，具体如图2所示。

图2 Mahony互补滤波算法流程图Fig.2 Flowchart of Mahony complementary filtering algorithm

将误差姿态角进行比例和积分处理，最终将PI运算结果加到当前陀螺所测得的三轴角速度上，Kp和Ki参数大小关联修正陀螺角速度的速度，Kp和Ki越大，姿态收敛的速度就越快。

最后，将已经被加速度修正后的角速度代入式(11)中进行四元数的更新

更新完毕后的四元数会因为各个步骤中的计算误差等因素逐渐失去单位化特性，再进行单位化处理，通过四元数与姿态角对应关系计算得到姿态角

由此可以快速地对初始姿态角度进行修正，从而获得一个精度较高的初始姿态角，为游泳运动的姿态测量过程提供一个精度较高的初始值。

4 实验与实测结果及分析

为了验证算法的可行性，采用实验室自研的MEMS惯性姿态测量系统对算法进行测试，设计框架如图3所示。这是一款使用CC3200作为主控制器、MTI-3作为主传感器、使用WIFI作为数据传输手段、使用FLASH作为数据存储单元的惯性姿态测量系统，实物图如图4所示。按照游泳运动中传感器对人体干扰尽可能小的要求，其质量仅为16g，尺寸仅为50mm×26mm×14mm，尽可能满足了游泳运动佩戴的要求。

图3 惯导系统设计框架Fig.3 Diagram of inertial navigation system design framework

图4 MTI-3实物图Fig.4 Physicaldrawing of MTI-3

MTI-3的选型是因为其体积小、质量小且角速度测量范围大，MTI-3的最大测量可以达到2000(°)／s。游泳运动不同于航空常见的姿态测量应用中飞机的运动场景，游泳运动相比于飞机运动来讲，姿态变化更加快速，转动角速度极大，实测场景下存在720(°)／s级别的角速度，这个是在MTI-3的2000(°)／s包络内，所以采用了此IMU作为数据采集单元。

IMU设备通过电池供电运行，数据临时存储在板载FLASH中，整体结构采用了IP68等级的防水设计，数据采集完毕后通过WIFI与上位机软件进行数据通讯，将数据上传并进行分析，系统整体运行框架如图5所示。

图5 系统整体展示Fig.5 Overalldisplay of the system

4.1 实验结果与分析

为了验证快速初始对准算法的可行性，在实验室的大理石平板上进行精度评估测试，该平板每年一次校准，要求水平误差不大于10″，北向基准误差不大于1′，实际测试结果如图6所示。

图6 水平基准下的实际测试数据Fig.6 Diagram of actualtest data under horizontalreference

由图6可知，x轴、y轴的加速度计存在0.15m／s2的零偏误差。z轴为重力加速度测量轴，测量所在地为北京，北京的重力加速度值认定为9.8015m／s2，所以其也存在零偏误差。因为地球自转角速度为7.292×10－5rad／s，而图示数据的稳定性都在10－3量级，故陀螺无法感应到地球自转角速度。

图7展示了在水平基准上进行测试的过程。因为测试时是按照东北天的方位角进行放置的，所以水平角对准结果的真实值为0°，若计算初始值也为0°，则无法展示收敛过程，故将初始值设置为一个偏差较大的值从而展示收敛过程。可以看出，该算法在0.2s时进入了收敛环节，即5%误差带；在0.4s时收敛完毕，达到了快速性的要求。采用3s的数据作为对准结果，发现其水平角度对准结果为-0.042°和0.077°，精度为角分级别；偏航角精度受磁力计以及磁场建模精度限制仅为度级别，但也符合游泳运动的需求。

图7 水平基准下的对准结果Fig.7 Diagram of alignment results under horizontalreference

4.2 实测结果与分析

为了验证快速初始对准算法的实际应用性能，在浙江省水上运动管理中心请国家游泳队运动员进行了实际的测试。设备按照图8的方式进行了佩戴，其x轴朝右，y轴朝前，z轴朝天，传感器y轴与人体脊骨对齐， 并经过严格调整，因为绑带的限制，此处的安装误差仅对横滚角造成小角度的固定偏差影响，本文关注于对准方法以及对准误差，不再对此处进行探讨。实际测试请专业运动员按照实际运动过程做了准备工作，其中包含了跳前的跳台预备跳水的静止过程。

图8 运动员佩戴设备示意图Fig.8 Schematic diagram of equipment wearing by swimmers

实测数据如图9所示。可以看出，在跳水前运动员在12s～20s保持了8s的静止状态，提供了初始对准的时间窗口期，在20s～45s跳水并进行了前进过程，在45s～50s进行了翻身动作，在50s～85s进行了返回，其余时间的数据为运动数据之外的佩戴与取下设备的时间。

图9 运动员游泳实际采集数据Fig.9 Diagram of realdata collected by swimmers

对准窗口期的对准结果如图10所示，因为无法保证运动员何时跳水，所以算法设定为与实验测试一致的3s对准时间。可以看出，实测数据上算法在0.2s时进入了收敛环节，由于运动员本身不是绝对的静止状态，水平姿态角存在一些波动，无法衡量是否收敛完成，但0.3s后围绕定值上下波动。偏航角因为磁力计的精度仅为度级别，所以近乎不波动。同时也可以看出，运动员对于自身静止的保持极为优秀。

图10 实测数据算法对准结果Fig.10 Alignment results of real measured data

图11是图9中数据以图10的初始对准结果作为初始值进行姿态测量的结果。其中，为了方便运动员与教练员理解，特意将偏航角设置为0°，并让角度范围从-180°～180°变成了0°～360°。26.85s前运动员从起点游动到终点，26.85s～28.15s为运动员终点转身动作，28.15s后游回起点。可以看出，运动员各个运动阶段的时间与上边根据原始数据进行推算的时间保持了一致，很好地还原了整个运动过程。

图11 实际姿态测量结果图Fig.11 Diagram of realattitude measurement results

5 结论

本文针对游泳运动场景下惯性姿态测量初始对准时间短、精度高的要求，提出了采用Mahony算法进行快速对准的方法。通过角速度信息计算姿态角，加速度信息修正水平姿态角，磁场强度信息修正偏航角，使用互补滤波方法对多源信息进行融合，从而快速得到惯性姿态测量的初值。在实验室场景和实际游泳运动场景对该算法进行了验证，本算法可以在3s内达到姿态水平角角分级别、偏航角度级别的对准精度，满足了游泳运动姿态测量的要求。