坐标系在动态SLAM中究竟有多重要？

01  摘要

大多同时定位于建图（SLAM）系统在传统上假定的都是静态世界，这与现实世界的场景不符。为了使机器人能够在动态环境中安全导航和规划，必须采用能够处理动态目标的表示。动态SLAM是SLAM研究中的一个新兴领域，它不仅提高了整体系统的精度，还提供了目标运动的额外估计。

最新的文献提供了动态SLAM的两种主要表示方法，将动态目标点在世界坐标系或者目标坐标系中表示。尽管在局部参考系中表达目标点可能看上去很直观，但是这可能无法获得最精确、最鲁棒的解决方案。

本文对各种动态SLAM公式进行深入分析，确定解决该问题的最佳方法。基于此目的，本文引入一种使用GTSAM的与前端无关的框架——用于评估各种动态SLAM公式。

02  介绍

同时定位和建图（SLAM）是一个已被研究三十余载的问题。SLAM系统使机器人能够构建环境的表示，同时还能在该环境中定位自身。当下的许多SLAM解决方案是在假设环境大部分由静态元素组成的前提下执行的，很遗憾，这在动态目标丰富的现实世界中可能不成立。

传统上，SLAM系统将与运动目标相关联的传感器数据作为异常值，并且在估计过程中剔除它们，而忽略任何与动态目标相关的有用信息。将目标集成到SLAM框架中的优势是，生成的地图可以直接向导航和任务规划系统通知待估计的目标运动和场景结构，提高了机器人系统在复杂动态环境中的鲁棒性。因此，SLAM中的一项新兴策略是结合对场景中动态目标的观测结果，并且估计其运动。在本文中，我们将这种系统称为动态SLAM系统。

最近，已经探索了多目标视觉里程计和基于图优化的动态SLAM系统，以基于静态点和动态点观测结果来联合定位机器人并且估计静态结构和场景中刚体目标的运动/轨迹。这些系统通常采用局部、滑动窗口或者批量优化技术，文献中给出了这些优化问题中表示变量的各种方式。在设计SLAM系统时，选择合适的表示是非常重要的，因为这决定了系统的鲁棒性、精度和效率。因此，对不同表示进行正规分析是至关重要的，这些表示清楚地描述了此类系统成功执行的情况。

图1 展示了以目标为中心 vs 以世界为中心。图1(a)展示了一种更直观的方法，其中观测到的动态点在其对应目标的局部坐标系中表示，本文称之为以目标为中心。

局部表达的点可以被建模为相对于目标坐标系的静态点，以增强刚体假设，因此可以由因子图中的单个状态变量来表示。然而，目标坐标系的位姿是无法直接观测的，并且当根据目标的部分观测进行估计时可能是不可靠的。

一种替代方法是在已知的参考坐标系中表达动态点，例如相机坐标系或者地图/世界坐标系。我们先前的工作VDO-SLAM证明了SE(3)运动可以在包含世界坐标系的任何参考坐标系中表达。根据这一点并且通过在世界坐标系中表示动态目标点，避免了估计目标位姿，并且产生了精确的结果。本文将这种表示称为以世界为中心，如图1(b)所示。

本文探索了如何在动态SLAM系统中更好地表示目标。为此，我们引入了一个基于因子图的优化框架，用于开发和测试不同的动态SLAM表示。基于最先进的文献，我们实现了以世界和目标为中心的表示，严格分析了由此产生的SLAM系统的精度和鲁棒性。基于这一分析，本文提出了动态SLAM表示，其最精确且最鲁棒地估计相机位姿和目标运动。

本文的贡献如下：

1）本文引入了一组详细的数学公式和图结构，用于在SLAM问题中估计自身运动和跟踪动态目标；

2）本文使用现实世界数据集严格分析、评估和测试各种表示； 

3）本文提供了一个使用GTSAM的动态SLAM优化框架，该框架实现了本文中提出的各种表示。

03  背景

3.1 参考坐标系和符号

3.2 位姿变换和坐标系变化



04  表示

本节引入了若干种表示，在基于因子图的动态SLAM估计框架中定义变量并且建模这些变量之间的关系（因子），类似于最先进的方法。本文将这些表示分为以世界为中心或者以目标为中心。

4.1 SLAM前端



4.2 以世界为中心的表示

4.3 以目标为中心的表示

以目标为中心的方法估计世界坐标系中的相机位姿、静态点、目标运动和位姿。对应的因子图如图3所示，其中我们强调了用于实验的公式的不同变体。

图3 展示了以目标为中心表示的因子



4.4 目标运动学因子



05  总结与未来展望

本文对动态SLAM的多种解决方案进行了全面分析，并且在现有现实世界数据集上评估了所提出的表示。为此，我们使用GTSAM开发了一个与前端无关的优化框架，其可以轻松地实现并且测试不同的配置。根据动态目标及其对应点观测在因子图中的表示方式，这些表示分为以目标为中心和以世界为中心。以目标为中心的表示更为直观，因为基于刚体假设，目标点相对于目标局部坐标系是静态的。然而，本文分析表明，以世界为中心的表示可以产生更精确的目标运动估计，同时在相机位姿估计方面也表现更好，并且在优化过程中展现更好的稳定性。在未来，我们计划对研究成果进行正式描述，也可以提前提供明确的指导方案。

审核编辑：刘清