六个惯性传感器和1个手机实现人体动作捕捉、定位与环境重建

本文则试图打开惯性动作捕捉的「眼睛」。通过额外佩戴一个手机相机，我们的算法便有了「视觉」。它可以在捕获人体运动的同时感知环境信息，进而实现对人体的精确定位。该项研究来自清华大学徐枫团队，已被计算机图形学领域国际顶级会议SIGGRAPH2023接收。

• 论文地址：https://arxiv.org/abs/2305.01599
• 项目主页：https://xinyu-yi.github.io/EgoLocate/
• 开源代码：https://github.com/Xinyu-Yi/EgoLocate

简介

随着计算机技术的发展，人体感知和环境感知已经成为现代智能应用中不可或缺的两部分。人体感知技术通过捕捉人体运动和动作，可以实现人机交互、智能医疗、游戏等应用。而环境感知技术则通过重建场景模型，可以实现三维重建、场景分析和智能导航等应用。两个任务相互依赖，然而国内外现有技术大多独立地处理它们。研究团队认为，人体运动和环境的组合感知对于人类与环境互动的场景非常重要。首先，人体和环境同时感知可以提高人类与环境互动的效率和安全性。例如，在自动驾驶汽车中，同时感知驾驶员的行为和周围环境可以更好地保证驾驶的安全性和顺畅性。其次，人体和环境同时感知可以实现更高级别的人机交互，例如，在虚拟现实和增强现实中，同时感知用户的动作和周围环境可以更好地实现沉浸式的体验。因此，人体和环境同时感知可以为我们带来更高效、更安全、更智能的人机交互和环境应用体验。

基于此，清华大学徐枫团队提出了仅使用6个惯性传感器（IMU）和1个单目彩色相机的同时实时人体动作捕捉、定位和环境建图技术（如图1所示）。惯性动作捕捉（mocap）技术探索人体运动信号等「内部」信息，而同时定位与建图（SLAM）技术主要依赖「外部」信息，即相机捕捉的环境。前者具有良好的稳定性，但由于没有外部正确的参考，全局位置漂移在长时间运动中会累积；后者可以高精度地估计场景中的全局位置，但当环境信息不可靠时（例如没有纹理或存在遮挡），就容易出现跟踪丢失。

因此，本文有效将这两种互补的技术（mocap和SLAM）结合起来。通过在多个关键算法上进行人体运动先验和视觉跟踪的融合，实现了鲁棒和精确的人体定位和地图重建。

图1 本文提出同时人体动作捕捉与环境建图技术

具体地，本研究将6个IMU穿戴在人的四肢、头和后背上，单目彩色相机固定在头部并向外拍摄。这种设计受到真实人类行为的启发：当人类处于新环境中时，他们通过眼睛观察环境并确定自己的位置，从而在场景中计划他们的运动。

在我们的系统中，单目相机充当人类的眼睛，为本技术提供实时场景重建和自我定位的视觉信号，而IMU则测量人体四肢和头部的运动。这套设置兼容现有VR设备，可利用VR头显中的相机和额外佩戴的IMU进行稳定无漂移的全身动捕和环境感知。整个系统首次实现了仅基于6个IMU和1个相机的同时人体动作捕捉和环境稀疏点重建，运行速度在CPU上达到60fps，并在精度上同时超过了两个领域最先进的技术。该系统的实时示例如图2和图3所示。

图2 在70米的复杂运动中，本系统精确跟踪人体位置并捕捉人体动作，无明显位置漂移。

图3 本系统同时重建人体运动和场景稀疏点的实时示例。

方法介绍

图4 方法总体流程

系统的任务是从6个IMU传感器的朝向和加速度测量值和相机拍摄的彩色图片中实时重建出人体运动、三维场景稀疏点云、并定位人在场景中的位置。我们设计了一个深度耦合的框架，以充分利用稀疏惯性动作捕捉和SLAM技术的互补优势。在这个框架中，人体运动先验与SLAM的多个关键组件相结合，SLAM的定位结果也回馈给人体运动捕捉。如图4所示，根据功能，我们将系统划分为四个模块：惯性动作捕捉模块（Inertial Motion Capture）、相机跟踪模块（Camera Tracking）、建图和闭环检测模块（Mapping & Loop Closing）和人体运动更新模块（Body Translation Updater）。以下分别介绍各个模块。

惯性动作捕捉

惯性动作捕捉模块从6个IMU测量值中估计人体姿态和运动。本模块的设计基于我们之前的PIP[1]工作，但本工作中不再假设场景是一个平坦的地面，而是考虑在3D空间中捕捉自由人体运动。为此，本文对PIP的优化算法进行了适应性的修改。

具体地，本模块首先通过多阶段循环神经网络从IMU测量值中预测人体关节旋转、速度、脚和地面接触概率。利用PIP提出的双重PD控制器，求解人体关节最优控制角加速度和线加速度。随后，本模块优化人体的位姿加速度，使其在满足接触约束条件C下实现PD控制器给出的加速度：

其中J为关节雅可比矩阵，为与地面接触的脚的线速度，约束C要求接触地面的脚的速度要小（不发生滑动）。该二次规划问题求解可以参考PIP[1]。通过姿态加速度积分从而获得人体姿态和运动后，可获得绑定在人体上的相机的位姿，用于后续模块。

相机跟踪

相机跟踪模块以惯性动作捕捉模块给出的初始相机位姿和相机拍摄的彩色图像作为输入，利用图像信息优化相机位姿，消除位置漂移。具体地，本模块基于ORB-SLAM3[2]设计，首先提取图像的ORB特征点，并和已经重建的稀疏地图点（下文介绍）利用特征相似度进行特征匹配，得到匹配的2D-3D点对，进而通过优化重投影误差的方法优化相机位姿。值得注意的是，仅优化重投影误差可能会受错误匹配影响，导致相机位姿优化结果较差。因此，本文在相机跟踪优化中融合人体运动先验信息，以惯性动捕结果作为约束，限制重投影误差的优化过程，以及时发现并排除错误的特征点-地图点匹配。

记地图点的世界坐标为，与之匹配的2D图像特征点的像素坐标为表示所有的匹配关系。用

表示优化前的初始相机位姿，则本模块优化相机位姿R,t：

其中，为鲁棒Huber核函数，将三维旋转映射到三维向量空间，为透视投影运算，动作捕捉旋转和平移项的控制系数。该优化进行3次，每次都根据重投影误差将2D-3D匹配分类成正确或错误，在下一次优化中只有正确匹配被使用，错误匹配则被删除。通过动作捕捉约束提供的强有力的先验知识，本算法可以更好地区分正确和错误匹配，从而提高相机跟踪精度。解出相机位姿后，本模块提取出正确匹配的地图点对数量，并将其作为相机位姿的可信度。

建图和闭环检测

建图和闭环检测模块利用关键帧重建稀疏地图点并检测人体是否到达曾去过的位置以修正累计误差。在建图过程中，我们使用动作捕捉约束的光束平差法（Bundle Adjustment，BA）同时优化稀疏地图点位置和关键帧相机位姿，并引入地图点置信度以动态平衡动作捕捉约束项和重投影误差项的相对强弱关系，从而提高结果精度。当人体运动发生闭环时，进行动作捕捉辅助的位姿图优化（Pose Graph Optimization）以修正闭环误差。最终得到优化后的稀疏地图点位置和关键帧位姿 ，用于下一帧算法运行。

具体地，本模块首先根据地图点的观测情况计算其置信度，用于后续BA优化。如下图5所示，根据观测到地图点的关键帧的位置，本模块计算关键帧基线长度bi和观测视角θi以确定地图点i的置信度，其中k为控制系数。

图5 （a）地图点置信度计算。（b）相同的基线长度b1=b2，更大的观测视角（蓝色）可以更好地抵抗相机位姿的扰动，导致更小的地图点位置误差（e1<e2）。

随后，同时优化最近20个关键帧相机位姿和它们观测到的地图点。其他看见这些地图点的关键帧位姿则在优化中固定不变。记所有可优化的关键帧集合为K0，所有固定的关键帧集合为Kf，关键帧j测到的地图点的集合记为Xj。记

表示需要优化的关键帧朝向和三维位置，

表示地图点位置。则该动作捕捉约束的光束平差法优化定义为：

其中，

表示关键帧j的上一个关键帧，

为动作捕捉约束项的系数。该优化要求地图点的重投影误差要小，且每个关键帧的旋转、相对位置要和动作捕捉的结果相近，其中地图点置信度ci动态决定了动作捕捉约束项和地图点重投影项之间的相对权重关系：对于未充分重建的区域，系统更倾向于相信动作捕捉的结果；反之，若一个区域被反复观测到，系统则更相信视觉跟踪。该优化的因子图表示如下图6所示。

图6 动作捕捉约束的光束平差法优化因子图表示。

当轨迹闭环被检测到后，系统进行闭环优化。基于ORB-SLAM3[2]，记位姿图中的顶点集合为F，边的集合为C。则动作捕捉约束的位姿图优化定义为：

其中，为关键帧j的位姿，为位姿图优化之前关键帧i和j之间的相对位姿，为动作捕捉获得的相机位姿初值，将位姿映射到六维向量空间，为动作捕捉约束项的相对系数。该优化以动作捕捉先验为指导，将闭环误差分散到各个关键帧上。

人体运动更新

人体运动更新模块利用相机跟踪模块优化后的相机位姿及可信度，更新动作捕捉模块给出的人体全局位置。该模块使用Kalman滤波器的预测-校正算法实现。其中，动作捕捉模块提供恒定方差的人体运动加速度，可用于人体全局位置的预测（先验分布）；而相机跟踪模块给出相机位置观测和置信度，用于人体全局位置的校正（后验分布）。其中，相机位置观测的协方差矩阵近似通过匹配的地图点数量计算为如下的对角阵：

其中为小数避免除数为0。即相机跟踪中成功匹配的地图点数量越多，相机位姿观测的方差越小。利用Kalman滤波算法，最终预测人体全局位置。

更详细的方法介绍和公式推导请参考论文原文及附录部分。

实验

对比Mocap

本文方法主要解决了稀疏惯性动作捕捉（Mocap）中全局位置漂移的问题，因此选取主要测试指标为人体全局位置误差。在TotalCapture和HPS两个公开数据集上和SOTA mocap方法TransPose[3]、TIP[4]和PIP[1]的定量测试结果对比如下表1所示，定性测试结果对比如下图7和图8所示。可以看到本文方法在全局定位精度上大幅超过前人惯性动作捕捉方法（在TotalCapture和HPS上分别提升41%和38%），轨迹与真值的相似度最高。

表1 和惯性动作捕捉工作的全局位置误差定量对比（单位：米）。TotalCapture数据集以动作进行分类，HPS数据集以场景进行分类。针对我们的工作，我们测试9次并汇报中位数和标准差。

图7 和惯性动作捕捉工作的全局位置误差定性对比。真值用绿色表示，不同方法预测结果用蓝色表示。每个图片的角落中展示了人体的运动轨迹和当前位置（橙色圆点）。

图8 和惯性动作捕捉工作的全局位置误差定性对比（视频）。真值用绿色表示，本文方法为白色，前人工作的方法使用其他不同颜色（见图例）。

对比SLAM

本文从定位精度和地图重建精度两个角度分别和SOTA SLAM工作ORB-SLAM3[2]的单目和单目惯性版本进行了对比。定位精度的定量对比结果如表2所示。地图重建精度的定量对比结果如表3所示，定性对比结果如图9所示。可以看到，本文方法相比SLAM大幅提高了系统鲁棒性、定位精度和地图重建精度。

表2 和SLAM工作的定位误差定量对比（误差单位：米）。M/MI分别表示ORB-SLAM3的单目/单目惯性版本，On/Off表示SLAM的实时和离线结果。由于SLAM经常跟踪丢失，针对SLAM我们分别汇报了完整序列（Full）和成功跟踪的帧（Tracked）上的平均定位误差；本文方法不存在跟踪丢失情况，因此我们汇报完整序列的结果。每个方法测试9次并汇报中位数和标准差。对于成功跟踪的帧上的误差，我们额外汇报了成功的百分比。如果一个方法失败过多次，我们标记它为失败（用“-”表示）。

表3 和SLAM工作的地图重建误差定量对比（误差单位：米）。M/MI分别表示ORB-SLAM3的单目/单目惯性版本。针对三个不同场景（办公室、室外、工厂），我们测试所有重建的3D地图点距离场景表面几何的平均误差。每个方法测试9次并汇报中位数和标准差。如果一个方法失败过多次，我们标记它为失败（用“-”表示）。

图9 和SLAM工作的地图重建误差定性对比。我们展示了不同方法重建的场景点，颜色表示每个点的误差。

除此之外，本系统通过引入人体运动先验，针对视觉跟踪丢失的鲁棒性大幅提高。在视觉特征较差时，本系统可以利用人体运动先验持续跟踪，而不会像其他SLAM系统一样跟踪丢失并重置或创建新地图。如下图10所示。

图10 和SLAM工作的遮挡鲁棒性比较。右上角展示了真值轨迹参考。由于SLAM初始化的随机性，全局坐标系和时间戳没有完全对齐。

关于更多的实验结果，请参考论文原文、项目主页及论文视频。

总结

本文提出了第一个将惯性动作捕捉 (inertial mocap) 与 SLAM 相结合，实现实时同时进行人体动作捕捉、定位和建图的工作。该系统足够轻量，只需要人体穿戴稀疏的传感器，包括 6 个惯性测量单元和一个手机相机。对于在线跟踪，mocap 和 SLAM 通过约束优化和 Kalman 滤波技术进行融合，从而实现更准确的人体定位。对于后端优化，通过将人体运动先验融入SLAM 中的光束平差优化和闭环优化，进一步减少定位和建图误差。

本研究旨在将人体感知与环境的感知融合在一起。尽管本工作主要关注定位方面，但我们相信，这项工作迈出了联合运动捕捉和精细环境感知与重建的第一步。