Dynamic SLAM The Need For Speed 论文笔记

Dynamic SLAM: The Need For Speed 论文笔记

摘要

提出了一种新的基于特征的、无模型的、物体感知的动态SLAM算法，该算法利用语义分割来估计场景中刚性物体的运动，而不需要估计物体姿态或具有它们的3D模型的任何先验知识。该算法生成动态和静态结构图，并能够提取场景中刚性运动对象的速度。

SLAM中动态对象的说明

所考虑的问题是，在进行SLAM估计的机器人的感测范围内有相对较大的刚性物体移动。SLAM前端能够在不同的时间步长识别和关联来自同一潜在移动对象的点。这些点共享潜在的运动约束，可以利用该运动约束来提高SLAM估计的质量。

A. 问题公式化

使用因子图[27]对具有动态对象估计问题的SLAM进行建模，目标是获得静态和动态3D结构以及最大限度地满足一组测量和姿态变化约束的机器人姿态。

假设存在高斯噪声，这个问题变成一组变量的非线性最小二乘(NLS)优化[28]:

变量名称	数学表达
机器人位姿x	x={x0…xnx}，x∈SE(3)
时间步长nx	k∈0…nx
动态和静态地标集合l	（IR3是3D射影空间）

界标集l = ls∪ld包含一组静态界标ls和一组运动物体界标ld。移动物体上的同一点在每个时间步使用不同的变量表示，即lik 1和lik分别是在时间k1和k看到的相同的物理I点。

B. 刚体上一点的运动模型

变量名称	数学表达
刚体对于参考坐标系的位姿
物体上观察到的特征点在物体坐标系中的坐标
同一点在时间k时在参考坐标系{0}中表示的坐标

注意，对于运动中的刚体，特征点Lli对于所有对象实例都是常量，而0Lk和0lik都是时变的。点坐标由表达式关联:

其中横线表示齐次坐标。

变量名称	数学表达
物体L从k-1到k的相对运动（刚体变换，称为体固定帧姿态变化）
物体上观察到的特征点在物体坐标系中的坐标
同一点在时间k时在参考坐标系{0}中表示的坐标

物体L从k−1到k的相对运动由刚体变换Lk−1 k−1Hk∈SE(3)表示，称为体固定架姿态变化。索引表示变换将基本位姿0Lk−1(左下索引)映射到目标位姿0Lk(右下索引)，用坐标系0Lk−1(左上索引)表示:

图 2a 显示了三个连续对象姿势的这种转换。

图 2. (a)：运动中刚体的坐标。在每一步，点 Lli 相对于刚体姿态 {L} 表示。 (b)：机器人姿势、移动地标位置（黑色）和三个不同时间步长的测量值（红色）

新的刚体坐标由增量位姿变换给出：

考虑目标框架 {L} 中的一个点 Lli。将对象在时间 k - 1 和 k 的两个连续姿势写成表达式（1），并使用（3）中对象的相对运动，该点的运动可以写为：

我们观察到通过变换
将运动刚体上的同一点联系起来

根据[29]，该方程表示了一个位姿变换的坐标系变化，表示了(2)中固定体坐标系的位姿变化与参考坐标系的位姿变化的关系。在参照系中的点运动为:

这个公式是提出的方法的关键，因为它消除了估计物体姿态0Lk的需要，并允许我们直接使用参考坐标系中的点0¯lik。

线速度提取：其他车辆的速度是自动驾驶应用中的一个关键信息。给定车辆刚体在惯性坐标系0k−1Hk中的姿态变化，其线速度矢量为
可以计算:

这个矢量的大小是速度。

其中，
分别是车辆在惯性系0k−1Hk中位姿变化的旋转平移分量，I3为单位矩阵，ck−1为k−1时刻物体质心位置。由于我们的算法是稀疏的，我们不能访问目标的质心，而是通过在目标上检测到的特征的三维质心近似它。(6)的推导在附录中有详细的说明，请读者参考[29]。

C. 动态SLAM中的运动因子

该方法估计了摄像机的位姿、静态和动态结构以及动态结构的运动。为了实现这一点，运动因子与机器人本体感觉传感器获得的里程计，以及路标观测被联合优化:

其中：

变量名称	数学表达
Huber函数（鲁棒函数？）	ρh
3D点测量模型
测量协方差矩阵	Σwk
所有时间步长的所有 mk 个 3D 点测量值的集合
里程计模型，具有σvk里程计协方差矩阵
一组mi个里程计测量集合	o = {o1…omi}
动态物体上点的运动模型，运动协方差矩阵为Σq

被检测刚体j上任意一点的运动，由给出的相同的位姿变换
来表征，对应的系数为：

其中
是服从正态分布的零均值高斯噪声。 (8)中的因子是一个三元因子，我们称之为刚体上一点的运动模型(图3中的橙色因子)。所有的变量按θ = x∪l∪H分组，其中H是描述物体运动的所有变量的集合。

图3 后端(a):同一对象具有多个姿态变化顶点的问题的因子图表示。(b):同一对象具有唯一姿态变化顶点的问题的因子图表示。

D. 因子图

我们将动态SLAM问题建模为一个因子图。因子图公式是非常直观的，它的优点是可以有效地实现批量[27][30]和增量[31]，[32]，[33]求解器。研究表明，在动态SLAM中，了解环境中物体的运动类型是非常有价值的。在这项工作中，我们评估了没有和有恒定运动模型的两种情况:

• 在城市场景中，物体的运动受到变化(加速、减速等)的影响，对运动的建模是具有挑战性的。因此，我们考虑了在每一个时间步长的新姿态变化的估计。图3a显示了这种场景的因子图表示，在这种场景中，同一个物体的运动是使用两个运动顶点对两个不同的时间过渡进行估计的。一个可能的限制是使这些运动估计之间的变化最小化。
• 高速公路场景，每辆车都保持恒定的运动。图3b显示了因子图表示，其中每个对象估计一个单一的运动。

进一步，我们证明了如果体固定架姿态变化是常数，那么参考帧姿态变化也是常数。

对于任何k-1，k‘-1时间指数，体固定姿势变化中的恒定运动为:

我们重新调整(3)并使用(9)得到:

将其替换为
来得到：

由此可见，对于任何k，k0时间指数，特定物体的参考帧姿态变化
成立，并且(8)中的因子相应地改变。恒定运动假设可用于通过保持先前检测到的对象的假设并基于被遮挡对象的重新观测来恢复那些对象来处理遮挡。

系统总览

图4 系统概述：

输入图像被用于实例级对象分割算法中，以提供对象遮罩。该算法然后检测和跟踪潜在移动对象的特征。潜在的动态特征连同被跟踪的静态特征被用于构建该图，该图然后被馈送到后端优化中。

图4所示系统pipeline假设机器人配备有RGB-D摄像机和体感受传感器(例如里程表、IMU)

我们基于特征的物体感知动态SLAM后端估计场景中每个检测到的物体的机器人姿态、静态和动态结构以及姿态变换。为了确保运动物体上的特征被检测到，我们采用实例级的物体分割算法来产生物体掩模。对象分割是静态/动态对象分类和动态对象跟踪中的重要先验知识。然后，前端利用对象遮罩来检测潜在移动对象和静态背景上的特征。特征跟踪是我们方法成功的关键模块。通过对象分割和特征跟踪，SLAM前端能够在不同的时间步长识别和关联同一刚体对象上的点。这些点共享一个潜在的运动模型，我们利用它来实现同步定位、映射和移动对象跟踪。

该算法不需要前端估计物体的姿态或使用物体的任何几何模型。静态和动态3D测量以及来自本体感受传感器的测量被集成到后端，以同时估计相机运动、静态和动态结构以及场景中检测到的对象的SE(3)姿态变换。

一个潜在的运动模型，我们利用它来实现同步定位、映射和移动对象跟踪。

该算法不需要前端估计物体的姿态或使用物体的任何几何模型。静态和动态3D测量以及来自本体感受传感器的测量被集成到后端，以同时估计相机运动、静态和动态结构以及场景中检测到的对象的SE(3)姿态变换。

附录