RH-Map: Online Map Construction Framework of Dynamic Object Removal Based on 3D Region-wise Hash Map Structure
摘要
移动机器人在室外环境中导航经常遇到动态对象留下的不良痕迹问题，表现为地图上的障碍，阻碍了机器人实现精确定位和有效的导航。
为了解决这个问题，提出了一种新的基于三维区域哈希映射结构(RH-Map)的地图构建框架，该框架由前端扫描刷新器和后端去除模块组成，实现了实时地图构建和在线动态对象去除(dynamic object removal, DOR)。
首先，提出了一种用于有效在线DOR的地图管理的两层3D区域哈希映射结构。然后，在前端扫描刷新器的情况下，采用区域地平面估计(R-GPE)对地面信息进行估计和保留，提出了扫描到地图去除算法(S2M-R)来区分和去除动态区域。此外，为了进一步DOR，提出了保持关键帧的轻量级后端移除模块。
正如SemanticKITTI 的实验验证的那样，与最先进的方法相比，我们提出的框架在地图构建的在线 DOR 上取得了可喜的性能。我们还在现实世界的环境中验证了所提出的框架。源代码发布给社区（https://github.com/YZH-bot/RH-Map）
介绍
清洁可靠的地图在室外环境中无人地面车辆(UGV)和无人机(UAV)等移动机器人平台的导航和探索中发挥着重要作用。同时定位和映射(SLAM)技术通常用于提供周围环境的鲁棒定位和映射[3][4]。
然而，大多数SLAM方法假设环境相对是静态的;因此，环境中的动态对象可能会在地图上留下残余痕迹，如图1(a)所示，称为幽灵轨迹效应[5]。
这些通常会导致环境表示混乱和路径规划的次优结果。SLAM 研究旨在通过使用在线动态对象移除 (DOR) [8]、[9]、[19] 或在优化过程中利用动态对象的状态 [10] 来解决这一假设，以确保在动态环境中更可靠的定位。然而，他们更多地关注提高定位精度，而不是提供密集和干净的地图。
在这项工作中，我们专注于使用 3D LiDAR 在大规模环境中在线密集地图构建的实时动态去除。一些在地图构建中处理DOR任务的研究通常需要长序列历史信息、子图或预先构建的全局地图2、15、16，不适合实时性要求较高的场景。
此外，在线DOR方法依赖于LiDAR信息的短期相邻帧，可以过滤掉当前扫描中的大部分动态对象。然而，由于激光雷达在远距离区域的稀疏性和入射角模糊特性，一些动态对象可能会无限期地持续并保持在地图上，而一些静态点被错误分类，从而降低地图质量。
我们总结了以下挑战：
- 在同时过滤掉动态对象的同时保持地图构建的及时性的困难，
- (2）由于激光雷达数据的特点，在线DOR的弱性能，对地图的质量产生负面影响。
这项工作提出了一种新的动态环境下的DOR在线地图构建框架，称为RH-Map。首先，我们提出了一种两层地图结构，用3D区域和最小分辨率立方体来管理地图，带来的好处是：利用区域结构和哈希映射实现了高效的O(1)复杂度的在线DOR和实时地图更新。
基于区域结构，引入了一种两阶段在线建图框架，用于构建静态地图，包括前端和后端模块。
- 前端模块估计地面信息进行保存，并利用当前扫描去除地图上潜在的动态区域。
- 后端模块维护历史信息的关键帧队列，以有效去除残差动态对象，
如图1所示。我们将贡献总结如下:
- 提出了一种高效的地图管理的区域哈希地图结构，以便在线DOR，保证实时地图更新，在SemanticKITTI上实现了超过10Hz的实时性能
- RH-Map，提出了一种新的DOR在线地图构建框架，该框架由实时前端扫描新鲜和轻量级后端移除模块组成。前端实现了鲁棒的地面估计和实时在线DOR，后端模块实现了进一步去除动态对象以提高地图质量
- 我们在SemanticKITTI上彻底验证了所提出的方法，并展示了现实场景中的在线性能。我们的方法在 DOR 中实现了最先进的性能，可以应用于现实世界中的机器人导航。
相关工作
（略）
Occupancy map-based methods
Visibility-based methods
Segmentation-based methods
Scan pre-segmentation methods
Ground segmentation methods
具体实现
RH-Map原理图如图2所示。基于所提出的两层哈希地图结构，RHMap利用3D区域来表示DOR局部区域的信息，每个区域存储空间中的立方体状态(最小分辨率)。RH-Map 由两部分组成：前端扫描刷新器和基于所提出的数据结构的后端移除模块。
问题描述
与之前的工作15、16、13不同，我们专注于连续激光雷达扫描构建的地图的在线动态去除。在这项工作中，我们使用最先进的基于激光雷达的里程计或SLAM方法，如[3][4]，在世界坐标系\(W\)和LiDAR坐标系\(B\)之间提供SE(3)位姿\(T_B^W\)，并接收实时扫描\(P^B\)作为输入。
基于Region的hash地图结构
在本节中，我们介绍了一种两层哈希映射结构，如图2所示，与ERASOR16的2D自我中心区域伪占用描述符(RPOD)不同，全局3D占用网格图被划分为大小均匀的区域(在我们的实验中为绿色框，0.8m)，每个区域进一步划分为最小分辨率占用网格(Cubes, 0.1m)。
地图结构如图3所示，二进制掩码\(m\)和最小分辨率立方体大小\(s_c\)的值决定了区域的大小\(s_r=(m+1) * s_c\)，举例：如果二进制掩码\(m=00000111, s_c=0.1 m\), 则有\(s_r=0.8 m\)，可以理解为，低位每多一个1，那么在一个region里面的xyz维度可以分别增加1个小立方体。
对于地图构建，首先将扫描的点云转换为世界坐标系，并转换为全局索引\(I\)，然后在每个维度上使用二进制掩码\(m\)进行二进制操作，得到相应的3D区域索引\(I_r\)和3D立方体索引\(I_c\)，它们在RH-Map中组成一个唯一的索引:
其中，
- \(\left[p_x, p_y, p_z\right] \in \mathbb{R}^3\)是全局坐标系下点的坐标
- 显然，\(I=I_r \| I_c\)，也就是说，二进制掩码m的高位表明了region的编码部分，低位表明了一个region内部的小立方体的编码
- 请注意，16位掩码（在我们的实验中）可以处理 6553.5\(m^3\) 的地图大小，扩展掩码的位大小足以容纳更大的范围。（会不会小了一点? [EpsilonJohn]）
在得到\(I_r\)和\(I_c\)后，我们使用[34]这样的3D哈希函数将索引映射到地图中，并更新占用概率\(p\)如下:
其中，
- \(I(\cdot)\)表示获取索引\(I\)中（x,y,z）其中一个维度的值
- \(n_x, n_y, n_z\)是大的素数
- \(N\)是地图的size
- \(L(\cdot)\)表示log odds.
为了简化，我们使用 如下记号：
- \(M_I=M_{I_r, I_c}\) 来表示索引 \(I=I_r \| I_c\) 的地图上的唯一立方体，
- \(M_{I_r}\)表示地图上索引为\(I_r\)的区域
- \(O=(I(x), I(y))\)表示索引为\(I\)的立方体在xy平面上的2D索引
- \(O_r=(I_r(x),I_r(y))\)表示索引为\(I_r\)的区域在xy平面上的2D索引
RH-Map记录和更新的数据如下：
- RH-Map 记录索引为 \(O_r\) 2D区域最高点和最低点在H{max, min} 中
- 以及计算地图上xy平面索引为\(O\)的立方体的地面点平均高度
- 以及每个区域的最小-最大-平均 高度和平面参数
- 它还更新每个立方体的占用概率和地面
我们的方法支持在线操作，哈希地图结构只更新占用位置的状态，不受传统占用网格图或八叉数地图[12]等预设建图范围的限制，而R-POD16由于数据组织的不规则性，只能离线应用，随着全局地图变大，消耗的时间将增加。
前端扫描刷新器
前端扫描刷新器包含基于区域的地平面估计(R-GPE)和scan-to-map去除(S2M-R)。我们的扫描刷新器在 S2M-R 中实现在线动态去除，并应用 R-GPE 根据我们上述提到的基于区域的地图结构来保持地面的完整性。
Region-wise Ground Plane Estimation (R-GPE)
首先，实时扫描数据\(P^{B}\)被转换为世界坐标系\(P^W =T^W_B P^B\)，并插入到我们的地图中，更新区域的H(最低点-最高点)和区域的最大-最小-平均 高度属性，用于后续进行检测。
接下来，我们进行候选地面区域选举，以确定需要执行地平面拟合的范围。此外，扫描中有许多噪声点对地面估计有很大的影响。因此，如果一个区域在其八个相邻区域内没有地面，我们引入了一个异常值过滤过程，将该区域视为噪声。这是建立在地面通常是连续且噪声点通常离散出现的假设下建立的。然后，得到候选地面区域集\({}^{cgr}\mathbb{I}_r\)如下:
其中，
- \(\mathbb{I}^{P^W}\)表示通过公式(1)转换后的点云索引
- \(M_{I_{r}^{i}}\)表示第i个点对应的\(M_{I_{r}^{i}}\)地图区域
- \(M_{I_{r}^{i}}^{N}\)表示地图区域\(M_{I_{r}^{i}}\)的八邻域
- \(O^{i}=(I^{i}(x),I^{i}(y))\)
上面的意思是，候选地面区域的条件是：
- 立方体\(I^{i}\)的高度要小于全局地图中xy平面上对应的高度，即\(I^i(z)<G(O^i)\)
- 立方体\(I^{i}\)所在的区域的八邻域不能没有地面
然后，与16中的区域地平面拟合(R-GPF)类似，我们在每个区域\(M_{I_{r}^{i}}\) (\(I_{r}^{i} \in {}^{cgr}\mathbb{I}_r\))上执行R-GPF。对于\(M_{I_{r}^{i}}\)区域中，z轴上高度小于\(M_{I_{r}^{i}}\)平均高度的小立方体，我们提取其全局索引\(I_{r}^{i}\)，添加到初始地面点集合 \(\mathbb{I}^{I_{r}^{i}}\)中。然后我们使用主成分分析 (PCA) 来拟合地平面并更新区域\(M_{I_{r}^{i}}\)的地平面参数、法向量 \(n_i\) 和平面系数\(d_i=n_i\overline{I^i}\)，其中\(\overline{I^i}\)表示初始地面点集合\(\mathbb{I}^{I_{r}^{i}}\)的平均索引。然后，\(M_{I_{r}^{i}}\)区域的地面全局索引集合\({}^{gro}\mathbb{I}^{I_r^i}\)可以表示如下：
其中，\(r_{gro}\)表示到平面的距离（应该是一个阈值?）
上面的条件大概是，利用立方体的索引计算到区域地平面的距离，如果小于阈值，那么这个立方体就是地面。
最后，我们用提取到的地面立方体集合\({}^{gro}\mathbb{I}^{I_{r}^{i}}\)来更新立方体的地面属性。请注意，只有当检测到新的候选地面区域并且 R-GPE 的效果如图 4(b) 所示时，才会执行 R-GPE
Scan-to-Map Removal
一般来说，与地图相比，单次扫描的信息相对稀疏且缺乏信息。而如果没有事先对LiDAR数据进行预处理，那么基于区域的去除方法通常会导致误报(假阳性(False Positive)，错误地将静态点分类为动态点)，例如停放的汽车、墙壁或树木，如图5(b)所示的红色矩形。
而像ERASOR的R-POD16这样的2D描述符可能会导致假阴性(False Negatives，无法检测到动态点)，例如位于最高点和最低点之间的动态物体。
为了解决这些问题，我们引入了兴趣区域(ROI)提取和二维和三维扫描上下文，以检测不同提取点的动态目标。
首先，与地图相比，LiDAR在LiDAR附近的视场相对有限，导致LiDAR观测与地图存在差异(见图4(a)中红色矩形)；因此，从原始LiDAR点\(P^{B}\)中提取的最大线束ID \(P^B_{max}\)中的点来表示LiDAR的最大FOV：
其中，\(P_{res}^{B}\)（红色点）表示剩余的LiDAR点，那么转换到世界坐标系的ROI点为：\(P_{roi}^W=T_{B}^{W}P_{roi}^{B}=\{P_{res}^{W},P_{max}^{W}\}\)
为了检测扫描和地图在动态目标检测中的差异，我们引入二维区域扫描上下文\({}^{2d}S^{W}\)定义如下:
其中，
- \(\mathbb{O}_{r}^{P^{W}_{roi}}\)表示转换到世界坐标系的LiDAR ROI点\(P_{roi}^W\)对应的2D区域集合
- \(Z_{max}\mathrm{~and~}Z_{min}\)表示在当前帧扫描中2D区域\(O_{r}^{i}\)的最高和最低点的高度信息
然后，我们计算\({}^{2d}S^{W}\)与地图M之间关联区域的比率\(ratio1\)如下：
其中，
- \(\Delta H={}^{2d}S_{O_{r}^{i}}^{W}.Z_{max}-{}^{2d}S_{O_{r}^{i}}^{W}.Z_{min}\)
- \(H(O_r^i).max\)和\(H(O_r^i).min\)分别表示全局地图中的2D区域\(O_{r}^{i}\)的最高和最低点的高度信息
- \(O_{r}^{p_{k}}\)表示当前帧扫描的最大线束ID的激光点（已经转换到世界坐标系）\(p_{k}(p_{k}\in P_{max}^{W})\)的索引所在的2D区域
简单来说，这个比率1的计算就是用当前扫描的区域高度差 / 地图区域的高度差， 如果是最大线束ID的激光击中的区域，分母需要调整一下。
如果比率\(ratio_1^{O_r^i}\)小于去除阈值\(\delta_1\)，我们认为在LiDAR-FOV中的2D区域\(O_{r}^{i}\)是动态的。 （？？？）
如上所述二维描述子的缺点，我们仍然不能完全消除FPs和FNs，如图5(c)所示。这是因为二维描述符无法准确描述“凸起”点的动态和静态状态，如图4(a)中绿色和橙色部分所示。