Robust Rank Deficient SLAM

摘要

• 自主移动机器人需要地图来进行有效、安全的导航，而 SLAM 总体上仍然是一个未解决的问题。尽管如此，环境特征和传感器的某些组合允许易于处理的解决方案。特别是，线段 (2D) 或平面 (3D) 等线性特征的检测和跟踪已在许多人造环境中被证明是稳健的。然而，这些类型的特征会产生秩不足的约束，这给基于图的 SLAM 优化器带来了挑战。我们通过分析表示轨迹的因子图中每个节点的约束的近似零空间，提出了更稳健地使用秩亏特征和约束的技术。我们还扩展了对应计算和地图更新例程的辅助方法，它们的组合为等级不足的 SLAM 系统产生了最先进的性能。我们展示了比较内存使用、计算负载、准确性和鲁棒性的定量实验结果，这些实验针对真实数据和模拟数据进行了多项消融测试

1. introduction

• RD-SLAM 专为包含线性特征的环境而设计，并解决了密集迭代最近点 (ICP) [3]、[4] 和相关扫描匹配 (CSM) [17] 中固有的两个弱点。 首先，基于 ICP 的方法对异常值不具有鲁棒性，而 CSM 由于离散化无法计算精确的最大似然变换。 其次，这两种算法的在线内存效率都不高，通常需要存储原始点云或占用网格。 随着机器人应用程序的规模不断扩大并在更轻的硬件上运行，这些表示变得难以处理。 为了解决这些问题，RD-SLAM 提取线段或平面小平面，并通过计算这些较大特征的对应关系来计算相对变换。
• 鉴于线段或平面之间缺乏距离度量，我们提供了一组算法和相似度函数来进行稳健的对应计算。
  虽然线性特征更容易检测和跟踪，但在非线性最小二乘优化问题中使用它们会导致不稳定，因为对应关系可能无法完全约束机器人的运动。 我们提出了一种基于秩亏约束集的近似零空间向优化问题添加正则化项的算法，其效果如图 1 所示。这些项还降低了优化器对不同传感器不确定性模型的敏感性 ，与硬约束相反，可能允许优化器逃避局部最小值。 我们还扩展了一种用于构建和维护高度压缩的最大似然几何地图的现有方法，以允许这些地图随着机器人轨迹估计的变化而在线更新。
  

2. Relative work

• 尽管许多方法使用潜在的秩不足特征，但只有少数方法使用共线性约束进行了研究。一些方法将它们合并到已经受约束的因子图中 [15]，而大多数方法在线检测退化 [10]、[26]、[6]、[23]。 RD-SLAM 采用后一种方法，但不同之处在于它没有明确地沿退化维度投影惯性测量，也没有在传感模式之间进行任何硬切换。相反，我们检测退化并将正则化项添加到现有的优化问题中。构建长期地图时的另一个挑战是如何组合描述相同物理对象的基元。 RD-SLAM 遵循长期向量映射 [14] 的方法，它使用形状协方差矩阵分解。 [22] 中在递归最小二乘法下提出了一个类似的技巧。本文将这些方法扩展到在线工作，以防在优化过程中基础位姿估计发生变化时可能需要转换基元。

3. RANK - 不足的满贯

• 选择正确的抽象级别为特征检测和计算对应特征§3.A
• 稳健地使用秩不足约束 §3.B
• 在线轨迹优化轨迹 §3.C

3.1 A 特征对应

我们提出了用于稳健计算线段和平面小平面之间对应关系的伪度量：

2D 中的线段：

• 我们在以下假设下推导出线段相似性 (LSS) 的度量。首先，从连续扫描中提取的相应线段应该具有相似的位置和方向。其次，相应的线段不需要共存；它们可能只是共线的。只要至少有 2 个非平行段，共线性不仅可以提供足够的信息，而且在不满足此条件的某些场景中，例如沿着直线走廊行​​驶时，它也比共置更稳健，或者当机器人由于遮挡或范围和视野限制而只能检测到部分特征时。已经提出了许多 LSS 公式 [24]，但没有一个满足上述假设的所有标准。因此，我们提出了对各向异性段的相对位置敏感的 LSS 的定义。给定线段 a 和 b，我们将 LSS(a, b) 定义为：$$LSS(a,b)=(d_{\theta }/{\tau }_{\theta }{)}^2+(d_{\perp }/{\tau }_{\perp }{)}^2+(d_{\parallel ||}/{\tau }_{\parallel }{)}^2$$
  其中 $d_{\ast }$ 是 SE(2) 子空间上的不同度量，并且${\tau }_{\ast }$ 是基于传感器特性的比例因子。
• 令线段 a 和 b 具有端点 $p_1^i$和$p_2^i$，长度$l_i$，质心${\bar{p}}_i$ ，以及 $i=a,b$ 时的方向 θ i，如图 2 所示。我们定义 LSS 的 $d_{\ast }$ 项如下:
  
  $d_{\theta }=|sin({\theta }_b-{\theta }_a)|and{d}_{\perp }=|({\bar{p}}_b-{\bar{p}}_a)\cdot {\hat{n}}_a|$
  其中 ${\hat{n}}_a$ 是线段 $a$ 的法向量。
  定义段$a$以使$l_a\ge l_b$允许$d_{\perp }$对称。如果$p_1^b$和$p_2^b$在线段$a$定义的直线上的投影均位于a段边界之外，则平行方向上的距离为非零。就是 $d_{\parallel }=min(d_{\parallel }^1,d_{\parallel }^2)$。这儿，$d_{\parallel }^j=\{\begin{array}{cc}{t}_j-l_a& t_j⩾l_a\\ 0& 0⩾t_j⩽l_a\\ |t_j|& t_j<0\end{array}$ 。 这儿，$t_j=(p_j^b-p_1^a)\cdot \frac{(p_2^a-p_1^a)}{\Vert p_2^a-p_1^a\Vert }$

3D 平面

• 可以以类似的方式稳健地计算平面相似度 (PFS)。 给定两个平面 a 和 b，PFS 由相似的项组成。$$PFS(a,b)=(d_{\theta }/{\tau }_{\theta }{)}^2+(d_{\perp }/{\tau }_{\perp }{)}^2+(d_{\parallel ||}/{\tau }_{\parallel }{)}^2$$
  这儿，$d_{\theta }$和$d_{\perp }$代表 法向量和点到平面的距离，一个关键的区别是$d_{\parallel }$。
• 在明确考虑每个小平面的函数时，确定一对平面小平面是否重叠是很昂贵的。 因此，我们用一个椭圆来表示每个面，该椭圆是从形状协方差矩阵的特征向量和特征值推导出来的，该矩阵由与平面面相对应的点云的子集构造而成，如图 3 所示。给定椭圆 $a$ 和 $b$，其中 ${\delta }_{\theta }(a,b)<{\tau }_{\theta }$ , 由中心 ${\bar{p}}_a,{\bar{p}}_b$ , 特征向量 $e_1^a,e_2^a$和$e_1^b,e_2^b$，以及特征值${\lambda }_1^a,{\lambda }_2^a$和${\lambda }_1^b,{\lambda }_2^b$定义，我们投影 $b$ 到 $a$ 上，产生共面的特征向量集。 设这些新的特征向量和特征值分别为 $e_1^{b^{\prime }}、e_2^{b^{\prime }}$ 和${\lambda }_1^{b^{\prime }}、{\lambda }_2^{b^{\prime }}$ 。 我们将 $d_{\parallel }$定义为:
  
  $$d_{\parallel }=max(0,\Vert {\bar{p}}_a-{\bar{p}}_b\Vert -\Vert {\bar{\tau }}_a-{\bar{\tau }}_b\Vert )$$
• 下面给出的${\tau }_{\ast }$ 方程,假设椭圆$\ast$已被变换，因此 ${\bar{p}}_{\ast }$位于原点，且 $e_1^{\ast }\cdot \hat{y}=0$。
  ${\tau }_{\ast }=<t{\lambda }_1^{\ast }{\lambda }_2^{\ast }cos(\theta ),t{\lambda }_1^{\ast }{\lambda }_2^{\ast }sin(\theta )>$
  这儿 $t=(\sqrt{({\lambda }_1^{\ast })co{s}^2(\theta )+{\lambda }_2^{\ast })si{n}^2(\theta )}{)}^{-1}$ 和 $\theta =arctan({\bar{p}}_y^{\ast },{\bar{p}}_x^{\ast })$
• 除了 LSS 和 PFS 伪度量之外，我们还发现大致基于大小丢弃特征会增加额外的鲁棒性。 我们丢弃短于 20cm 的线段并丢弃面积小于 0.16m 2 的平面小平面。

3.2 处理秩亏约束

• 给定一组对应$C$，其中$C_{a,b}\in C$，将特征 $a$ 与特征 $b$ 分别在 $t_i$ 和 $t_j$ 时刻可见，我们通常可以使用共线或共面约束将视觉特征的优化问题写为: $$x^{\ast }=\underset{x}{\mathrm{argmin}}\sum _{C_{a,b}\in C}{d}_{\theta }(a,A_{i,j}b)+d_{\perp }(a,A_{i,j}b)$$
• 其中$X^{\ast }$是MLE轨迹，$A_{ij}$ 在观察到的特征 $x_j$ 转换为$x_i$ 的姿势。 这种约束的明显缺陷是沿一个或多个轴可能缺乏信息。 当传感器采样密度、视野或范围减小时，这种情况很常见，从而限制了观察到的特征的数量和信息多样性。 迫使更小的优化窗口的计算约束具有类似的效果，因为它们降低了进行信息数据关联的可能性。
• 考虑 位姿 $x_t$ 和对应集合 $C_t$ ，其中 $\forall c_{a,b}\in C_t$ ，在时间 $t$ 观察到 $a$ 或 $b$。 令 $F$ 是所有特征 $f$的集合，使得 $c_{f,\ast }\in C_t$ 。 然后是二阶矩矩阵的尺度：$M={\sum }_{f\in F}{\hat{n}}_f{\hat{n}}_f^T$。
• 其中${\hat{n}}_f$ 是特征 $f$ 的单位法线。从理论上讲，简并度可以通过对M进行高斯消去来检测。 然而，深度数据中的噪声导致特征正态估计，几乎总是产生技术上满秩的矩阵。 这会导致优化器找到关于 $d_{\perp }$ 的全局最小值，这是不准确的，因为在零方向的信噪比低。
• 为了解决这个问题，我们近似 M 的零空间并沿所有零方向应用约束。 与其他使用“硬”约束来阻止优化器完全沿空方向移动的方法相比，我们将这些约束强制为正则化项或“软”约束，本质上限制优化器仅通过移动来找到解决方案 条件良好的方向在一定的公差范围内。
• 算法 1 显示了逼近 null(M) 的方法。我们分析了 $M^{,}s$ 的条件数 $\kappa$，因为 M 是正态的，所以它是从它的特征值计算出来的。 大条件数表示退化维度，在我们的实验中，我们使用 ${\tau }_{\kappa }=10.0$。 在实践中，${\tau }_{\kappa }$ 很容易调整，因为大多数退化轴的条件数比条件良好的轴高几个数量级。 对于优化窗口内的每个姿势，算法 1 生成一个集合 $N_t$ ，表示姿势 $x_t$ 上约束的零空间的基础。 形式的正则化项:
  $J(X)={\sum }_{t=1}^{t_f}{\sum }_{\hat{\eta }\in N_t}(x_t-x_{t-1}-(u_t-x_{t-1}))\cdot \hat{\eta }$
• 然后将其添加到每个姿势的成本函数中。 这里，η̂ 是描述视觉约束零空间的基向量，u 是惯性测量值，x 是姿态变量。 将等式 11 和 13 结合在一起，以及惯性测量的成本函数，我们得到了一个类似于:
  $X^{\ast }=\underset{x}{\mathrm{argmin}}{\sum }_{t=1}^{t_f}||(x_t-x_{t-1}-(u_t-x_{t-1})||{\sum }_{C_{a,b}\in C}{d}_{\theta }(a,A_{i,j}b)+d_{\perp }(a,A_{i,j}b)+{\sum }_{t=1}^{t_f}{\sum }_{\hat{\eta }\in N_t}(x_t-x_{t-1}-(u_t-x_{t-1}))\cdot \hat{\eta }$

3.3 地图更新

• 为了存储环境的长期表示，我们采用长期矢量地图 (LTVM) [14] 来在线工作。 在线 LTVM 根据现有的特征图检查在全局框架中注册的新检测到的特征，该图在首次部署机器人时为空。 每一帧，都会执行统计测试，以估计给定特征对应于地图中已表示的物理实体的可能性。 如果新要素表示未观察到的对象，则将其添加到地图中。 如果它表示对已映射对象的观察，则将地图特征更新为新特征和地图特征的加权和，其中权重是支持每个特征的原始观察的数量。 我们发现不同的统计测试适用于不同的特征。 对于线段，我们使用 [14] 中的卡方检验，对于平面小平面，我们使用基于 §3.A.2 中介绍的椭圆表示的保守投影阈值。
• 如果它们的边界一起增长，也可以在地图中已有的两个要素之间执行合并。这个过程是昂贵的，因为它缩放为 $O(n^2)$，其中 $n$是地图中的特征数量，因为必须检查每个映射的特征以查看它是否可以与任何其他特征合并。然而，实际上 $n$ 很小，因为特征是增量合并的。即使对于建筑比例尺的地图，$n$ 通常也是数百或数千。此外，空间分区数据结构（例如 kd-trees）可以消除大多数比较，从而提供进一步的加速。 LTVM 的主要优势之一是保持最大似然特征位置估计以及高压缩比。这可以通过以解耦方式存储每个线段或平面小平面的支持观测的形状协方差矩阵表示。然而，特征是在机器人框架中提取的，而地图更新是在全局框架中完成的。因此，解耦的表示必须旋转到全局框架。我们使用由 $N$ 个深度观测 $p$ 构造的解耦形状协方差矩阵表示特征： $S={\sum }_{i=1}^N{p}_i{p}_i^T-N\bar{p}{\bar{p}}^T=S_o-N\bar{S}$
• 其中 $S_o$ 表示特征的方向，$\bar{S}$ 是特征质心, $\bar{p}$ 的外积。 给定由旋转 $R$ 和平移 $T$ 定义的仿射变换，我们可以计算变换矩阵 ${\bar{S}}^{\prime }$ 和 ${\bar{S}}_o^{\prime \prime }$ 为:
  ${\bar{S}}^{\prime }=(R\bar{p}+T)(R\bar{p}+T{)}^T$
  ${\bar{S}}^{\prime \prime }=N(R\frac{1}{N}{S}_o{R}^T-R\bar{S}{R}^T+{\bar{S}}^{\prime })$

结论

计算效率和内存
精度
鲁棒性

• 鉴于包含显着噪声和视觉伪影的低质量传感，我们主要关注计算和内存效率以及准确性和鲁棒性。我们假设在这些约束下，RD-SLAM 系列的算法，特别是本文中提出的改进，与密集 ICP 方法以及通过对因子实施硬约束来处理秩不足的方法相比，提供了有利的权衡.为了验证这一假设，我们使用在马萨诸塞大学阿默斯特分校收集的模拟 2D 激光雷达和 3D 点云数据以及 2D 和 3D 数据进行了多项实验。我们分别使用 Hokuyo UST-10LX 和 Asus Xtion PRO 来获取激光雷达和点云数据。配备这些传感器的机器人在大学建筑物周围进行远程操作，其中包括许多功能有限的笔直走廊以及一些宽度超出传感器范围的开放区域。重要的是，这些数据集代表典型的人类环境，具有仅包含部分信息的大型、易于观察的特征。此外，轨迹中的许多点不能基于视觉特征完全约束。机器人通常会收到仅约束一个位置轴的信息，这种情况可以持续比用于优化的滑动窗口更长的时间，通常为 1 到 2 秒。对于每个传感器，从 5 个部署中收集数据，每个部署在同一环境中采用不同的路径。所有时序实验均在 3.70GHz 四核处理器上完成。

• A. 计算和内存效率为了测试计算效率，我们比较了 RD-SLAM 与密集 ICP 的对应计算和姿态优化所需的时间。我们将提取特征所需的时间包括在 RDSLAM 的时序结果中，并且我们使用带有 kd-tree 的密集 ICP 实现来有效地修剪非对应关系。我们发现 RD-SLAM 平均需要 7ms 来产生 3D 响应，而 ICP 实现平均需要 22ms。优化过程中节省的时间也很可观。在优化窗口为 20 个姿势的情况下，RD-SLAM 使用平均 61 毫秒收敛，而点对点对应平均花费 177 毫秒。为了测试内存效率，我们使用模拟环境比较了存储几种不同可能的地图表示所需的空间。以 3D 形式存储原始点云每秒需要大约 10MB。这在部署中无限增长，显然是不可行的。创建分辨率为 2cm 的 3D 占用网格需要近 100MB 来探索我们大约 10m x 10m 的环境。将地图存储在八叉树中可以减少内存，但仍然比平面表示更昂贵。存储部署期间提取的所有平面面也可以无限增长，但在我们的模拟中，每 10,000 个姿势只需要大约 1MB。最后，逐步合并平面面允许机器人将整个地图存储在大约 10KB 中。计算和内存节省对于 3D 数据更为明显，但对于 2D 数据仍然很重要。