---

摘要

代码：github
原文：原文

摘要—多激光雷达（LiDAR）传感器在机器人领域，特别是在自动驾驶汽车的定位和感知任务中，得到了越来越广泛的应用，这些任务都依赖于构建精确环境地图的能力。为此，我们提出了一种新的实时激光雷达单独里程计方法，称为CT-ICP（连续时间ICP），并通过一种新型的回环检测程序将其完整地实现为一个SLAM系统。该方法的核心是引入了扫描匹配中的连续性和扫描之间的非连续性相结合。这不仅允许在配准过程中进行扫描的弹性变形，从而提高精度，还增强了对高频运动的鲁棒性，尤其是来自非连续性的影响。我们在此里程计基础上构建了一个完整的SLAM系统，采用基于高程图像2D匹配的快速纯激光雷达回环检测，提供具有回环约束的姿态图。为了展示该方法的鲁棒性，我们在七个数据集上进行了测试：KITTI、KITTI-raw、KITTI-360、KITTI-CARLA、ParisLuco、Newer College和NCLT，这些测试场景包括驾驶和高频运动场景。CT-ICP里程计和回环检测代码现已在线公开。CT-ICP目前在KITTI里程计排行榜中名列第一，公开代码中，平均相对平移误差（RTE）为0.59%，每次扫描的平均时间为60毫秒，使用的是单线程CPU。

图1：上方为一幅彩色的激光雷达扫描图；颜色取决于每个点的时间戳（从最早的蓝色到最新的红色）。通过对扫描起始和结束位置的两个姿态进行联合优化，并根据时间戳插值，将扫描弹性变形以与地图（白色点）对齐，从而创建了连续时间的扫描到地图里程计。下方展示了我们轨迹的表达式，具有扫描内部姿态的连续性和扫描之间的非连续性。

一、介绍

许多激光雷达基于多个激光光纤连续从旋转单元发射的原理。我们将“扫描”定义为在有限时间范围内聚合的点，这些点覆盖了足够的视场（如用于自动驾驶车辆的激光雷达中的360度视场）。为了考虑连续采集，诸如[3][4]中的激光雷达里程计方法在假设恒定速度运动的情况下对当前扫描进行变形。然而，这一假设未考虑到大的方向或速度变化。另一方面，最近的一些研究[5-6]提出了具有连续时间轨迹的实时激光雷达里程计方法。可以根据每个激光雷达点的时间戳从控制姿态（直接姿态或使用样条曲线）计算其姿态。然而，方法[5]需要使用IMU来整合高频运动，而[6]中的连续性约束则会平滑这些运动，我们认为这会以精度为代价。

• 我们提出了一种具有扫描内部姿态连续性和相邻扫描之间非连续性的新的弹性轨迹表达式。实际上，这通过解决一个弹性的扫描到地图配准问题来定义，该问题由每个扫描的两个姿态（扫描的开始和结束）进行参数化，并在前一扫描的结束姿态与当前扫描的开始姿态之间设置了接近约束。图1展示了扫描（彩色点）与地图（白色点）的配准。颜色从蓝色到红色，表示我们CT-ICP方法中每个点的相对时间戳 𝑖。我们的里程计弹性地将新的扫描调整到建筑物上。
  作为我们的主要贡献，我们提出：
• 一种基于扫描内姿态连续性和扫描间不连续性的新的弹性激光雷达里程计方法。
  我们还作为次要贡献提出：
• 基于稠密点云并存储在稀疏体素结构中的局部地图，以实现实时处理速度。在驾驶和高频运动场景中，对7个数据集进行的大规模实验活动，所有实验均可使用公开和宽松的开源代码进行复现[2]。
• 一种快速的回环检测方法，与姿态图后端结合，构建完整的SLAM，并集成到pyLiDAR-SLAM中[2]。

二、相关工作

许多激光雷达里程计方法基于迭代最近点（ICP）方法【7】及其更高效的点到平面变体【8】【9】。KinectFusion【10】对于RGB-D传感器在从帧到帧的配准转变到帧到模型配准方面取得了显著进展，类似地，激光雷达里程计方法采用扫描到地图的配准方式。
SuMa【11】和SuMa++【12】将扫描表示为图像（距离图像），地图则以一组surfels的形式呈现。配准通过将当前扫描与通过将surfels地图投影到GPU上渲染得到的图像进行对齐来实现。LOAM【13】在距离图像中检测不同类别的关键点（边缘、平面），并将检测到的关键点注册到体素网格中，但邻域搜索对每类关键点使用不同的kd树。由于关键点地图足够稀疏，因此搜索可以实时进行。LeGO-LOAM【14】通过将地面上的关键点与其他关键点分离，改进了前述方法。F-LOAM【3】优化了配准过程，使其更加快速，且以超过20Hz的频率运行。最近，MULLS【4】通过在扫描中检测3D关键点，并使用多种类型的关键点（地面、立面、屋顶、柱子、梁和顶点），进一步改进了这一方法。不同于上述方法，IMLS-SLAM【15】将地图表示为稠密点云，【16】采用TSDF（有符号距离场）表示地图，PUMA【17】则使用网格表示地图，但这三种方法都无法实时运行。为了考虑扫描过程中的传感器运动，大多数先前的方法【3】【4】【15】首先通过假设恒定速度运动模型，基于先前姿态对当前扫描进行变形。然后，在ICP迭代过程中保持扫描变形不变。虽然这种方法在大多数驾驶场景中表现良好，但对于扫描之间的方向或加速度的突变缺乏足够的鲁棒性。
另一种方法通过定义一个连续时间轨迹，并使用控制姿态（通过线性插值或B样条）来考虑扫描过程中运动的影响。CT-SLAM【18】通过每个扫描使用多个姿态定义轨迹；【19】也使用连续时间轨迹，并对每个扫描使用六个姿态（使用B样条基函数表示轨迹）。然而，这两种方法均不能实时运行。最近，Elastic LiDAR Fusion【5】和MARS LiDAR里程计【6】提出了实时操作的连续时间轨迹表示方法。方法【5】通过线性插值姿态，并使用稀疏surfels地图进行里程计计算，以及使用稠密的2D盘形surfels进行地图构建；而方法【6】则利用B样条对连续时间轨迹进行建模，并通过多分辨率surfels地图实现实时速度。这些方法倾向于平滑轨迹，但在许多实际环境中，传感器的运动可能会因地形不规则而产生抖动，并产生高频运动（相对于控制点的频率），这些运动未能被这些方法考虑。
相比之下，CT-ICP在扫描期间定义了连续时间轨迹，且在扫描之间存在不连续性。在一次扫描过程中，轨迹由两个姿态（扫描的开始和结束）进行参数化。然而，与【6】等方法不同，CT-ICP的最终轨迹是非连续的——扫描开始时的姿态与前一扫描结束时的姿态不相等。我们认为，这种方法能够弥补插值无法处理的运动不规则性。
尽管激光雷达里程计的精度较高，但在开阔环境中仍然会积累误差，从而导致轨迹漂移。回环闭合过程可以全局修正轨迹，但回环检测仍然是激光雷达SLAM中的一个开放问题。目前，大多数SLAM解决方案主要依赖配准方法直接关闭回环【20】【11】【13】，这种方法仅适用于较小的轨迹和低漂移情况。不同的地点识别方法已经被提出，这些方法在单个扫描上操作【21】【22】【23】【24】，但它们对环境变化较为敏感，更适用于驾驶场景。方法【4】在ICP优化前使用了全局配准程序【25】，但每次配准的计算成本非常高，从而限制了发现更多回环的可能性。最近，深度学习方法被提出【23】【24】，但由于训练要求，这些方法并不适应新的环境。相比之下，我们提出了一种新的回环闭合过程，该过程在投影到高程图像上的聚合点云上运行。该过程要求传感器的运动主要是2D的，并且能够估算重力向量，因此可以集成到任何满足这些条件的激光雷达里程计中。与我们的工作最接近的是【22】，该方法构建了高程图像，但仅在扫描上操作，而我们的过程则在局部地图上运行，因此无需将每个扫描与先前的已观测位置进行匹配，使其在在线SLAM场景中更加高效。

图2：NCLT数据集（左上）、KITTI-CARLA（右上）、Newer College数据集（左下）和ParisLuco（右下）聚合点云，展示了使用CT-ICP获得的地图质量。

三. CT-ICP 里程计

A. 里程计公式

我们的CT-ICP里程计为当前扫描参数化了两个姿态：扫描开始时的姿态 ${\Omega }_b^n=\left(R_b^n,t_b^n\right)$（R代表旋转，t代表平移，b代表开始）和扫描结束时的姿态 ${\Omega }_e^n=\left(R_e^n,t_e^n\right)$（e代表结束）。为了简化符号，在接下来的内容中，我们省略了表示当前扫描姿态的n。对于在扫描的第一个时间戳 ${\tau }_b$ 和最后一个 ${\tau }_e$ 之间捕获的每个传感器测量值 $\tau \in \left[{\tau }_b,{\tau }_e\right]$，通过在两个扫描姿态之间进行插值来估计传感器的姿态。这些姿态将点从LiDAR框架 $L(\tau )$ 转换到世界框架 $W=L(0)$。与其他连续时间轨迹方法不同，里程计的姿态 ${\Omega }_b$，新的扫描与前一次扫描的结束姿态 ${\Omega }_e^{n-1}$ 不匹配。我们在优化中添加了一个邻近性约束，以迫使这两个姿态保持接近。我们的公式使我们的里程计对传感器的高频运动更加鲁棒。
对于每个新的扫描 $S^n$，我们首先从 $S^n$ 中提取一组关键点样本，这些关键点由 $I^n$ 索引： { p i ∈ S n ∣ i ∈ I n } \left\{p_{i}\in S^{n}\mid i\in I^{n}\right\} {pi​∈Sn∣i∈In}（使用扫描中点的简单网格采样），我们将这些点注册到局部地图中。这个地图是一个由所有先前注册扫描构建的密集点云 $M^n=\left\{q_i^W\right\}$，并存储在一个稀疏体素网格中。其构建细节在第三节-B中详细说明。然后，我们的扫描匹配估计两个最优姿态 ${\Omega }_b^{\ast }$ 和 ${\Omega }_e^{\ast }$，从而在优化过程中处理扫描的失真，并将点转换到世界框架中，然后添加到局部地图中。这些最优姿态由解决以下问题给出，参数 $X=\left({\Omega }_b,{\Omega }_e\right)\in SE(3{)}^2$ 用粗体表示：

$$\underset{X\in SE(3{)}^2}{\mathrm{argmin}}{F}_{ICP}(X)+{\beta }_l{C}_{loc}(X)+{\beta }_v{C}_{vel}(X)(1)$$

其中 $F_{ICP}$ 是扫描到地图的连续时间ICP：

$$F_{ICP}(X)=\frac{1}{\left|I^n\right|}\sum _{i\in I^n}\rho \left(r_i^2[X]\right)(2)$$

对于每个 $i\in I^n$，

$$r_i[X]=a_i\left(p_i^W[X]-q_i^W\right)\cdot n_i(3)$$

$$p_i^W[X]=R^{{\alpha }_i}[X]\ast p_i^L+t^{{\alpha }_i}[X](4)$$

$$R^{{\alpha }_i}[X]=\mathrm{slerp}\left(R_b,R_e,{\alpha }_i\right)(5)$$

$$t^{{\alpha }_i}[X]=\left(1-{\alpha }_i\right)t_b+{\alpha }_i{t}_e(6)$$
$\rho (s)$ 是一个鲁棒损失函数，用于最小化异常值的影响，$r_i$ 是样本点 $p_i^W$ 与地图中最近邻点之间的点到平面距离的残差（如ICP经典公式所定义）。$p_i^W[X]$ 是在世界框架中表示的点 $p_i$，$n_i$ 是局部地图中 $p_i^W$ 邻域的法线，而 $p_i^L$ 是传感器测量值（在LiDAR框架中）。${\Omega }^{{\alpha }_i}[X]=\left(R^{{\alpha }_i},t^{{\alpha }_i}\right)\in$ SE(3) 是从时间 ${\tau }_i,L\left({\tau }_i\right)$ 的LiDAR框架到世界W的转换。它通过定义 ${\alpha }_i=\left({\tau }_i-{\tau }_b\right)/\left({\tau }_e-{\tau }_b\right)$ 在 ${\Omega }_b$ 和 ${\Omega }_e$ 之间进行插值来估计。对于旋转插值，我们使用标准的球面线性插值（slerp）。我们还引入了权重来优先考虑平面邻域：$a_i=a_{2D}=\left({\sigma }_2-{\sigma }_3\right)/{\sigma }_1$ 如[15]中所定义（其中 ${\sigma }_i$ 是邻域协方差的特征值的平方根），是 $p_i^W$ 邻域的平面度。注意与[15]和大多数LiDAR里程计方法不同，我们使用局部地图的密集点云而不是当前扫描来计算法线 $n_i$ 和平面度权重 $a_i$，从而得到更丰富的邻域。这一计算在ICP的每次迭代中对每个样本点 $p_i$ 进行，因为它们位置的细化导致更精确的邻域。此外，在方程中我们引入了两个约束：$C_{loc}$（位置一致性约束）和 $C_{vel}$（恒定速度约束），以及各自的权重 ${\beta }_i$ 和 ${\beta }_v$，定义如下：

$$C_{loc}(t_b)={\Vert t_b-t_{a-1}\Vert }_1(7)$$
$$C_{vel}(t_b,t_e)={\Vert (t_e-t_b)-(t_{o-1}-t_{r-1})\Vert }_2(8)$$

$C_{loc}$ 强制传感器的起始和结束位置保持一致（限制不连续性），而 $C_{vel}$ 限制了过快的加速度。这些约束足以迫使CT-ICP的弹性与由权重 ${\beta }_i$ 和 $\beta$（在所有实验中均设为0.001）隐式定义的运动模型保持一致。CT-ICP执行迭代，直到参数步长的范数达到阈值（通常在平移上为0.1厘米，旋转上为0.01°）或达到迭代次数（5次以确保实时性）。默认情况下，CT-ICP在单线程上运行，但可以利用并行处理（用于构建邻域，或在解决线性系统时）来应对需要更多迭代和样本点的更具挑战性的场景。

B. 局部地图和鲁棒轮廓

作为局部地图，我们使用来自先前扫描的点云（类似于IMLS-SLAM[15]），但与之不同的是，世界框架(W)中的点存储在一个稀疏的体素数据结构中，以便比kd树更快地访问邻域（恒定时间访问而不是对数时间）。地图的体素大小控制了邻域搜索的半径和存储点云的细节层次。在驾驶场景中，它被设置为1.0米，而在高频运动场景中设置为0.80米，这在这两个方面提供了令人满意的平衡。定义地图网格的体素大小很重要，因为它定义了邻域搜索半径以及我们局部地图的细节层次。每个体素最多存储20个点，使得任意两点之间的距离不小于10厘米，以限制由于扫描线沿线测量密度导致的冗余。一旦体素已满，就不再向其中插入更多点。 为了构建点 $p_i^W$ 的邻域（计算 $n_i$ 和 $a_i$ 所需），我们从当前点的27个体素邻域中选取$k=20$个最近邻点。在对当前扫描n运行CT-ICP之后，这些点被添加到局部地图中。请注意，与$pyLiDAR$的$F2M$不同，局部地图不是基于最后扫描的滑动窗口，而是仅根据它们与最后插入扫描中心的距离来从地图中移除体素。
使用这类地图的里程计对错误的注册非常敏感，并且无法从错误扫描插入的地图污染中恢复。这在数据集中快速方向变化尤为成问题。对于这些类型的数据集，我们引入了一个鲁棒轮廓，它检测困难案例（快速方向变化）和注册失败（位置不一致或大量新关键点落在空体素中），并尝试使用更保守的参数集（尤其是更多的采样关键点和更大的邻域搜索）重新注册当前扫描；对于重要的方向修改（≥5°），我们不将新扫描插入地图，这有更高的概率被错位。鲁棒性的提高是以增加运行时间为代价的。

图4：KITTI-360序列00（11501个扫描）的回环闭合定性结果。左上为通过投影局部地图构建的高程图像。右上展示了CT-ICP里程计的轨迹以及通过计算的回环闭合约束（CT-ICP+LC）修正后的轨迹。下方显示了为同一个转弯（与左上局部地图相同）找到的不同回环闭合约束（绿色）。

四. 回环闭合和后端

我们的回环闭合算法在其内存中维护一个由里程计注册的最后扫描的窗口。当窗口达到$N_{map}$个扫描大小时，这些点被聚合成一个点云，放置在窗口中心的坐标框架中。然后，这个地图的每个点被插入到一个2D高程网格中，对于每个像素，保留在最高海拔的点。从这个2D网格中，通过在 $Z_{min}$和$Z_{max}$之间剪辑每个像素的z坐标，获得高程图像。然后提取旋转不变的2D特征[26]。保存在内存中的高程网格。除了最后的$N_{Noverlap}$个扫描外，其他所有扫描都从窗口中丢弃。

每次构建新的高程图像（每$N_{map}$-$N_{Noverlap}$个扫描），它都会与内存中保存的高程图像进行匹配。通过RANSAC稳健地拟合两个特征集之间的2D刚性变换，使用内点数的阈值来验证对应关系。当匹配被验证后，对高程网格的点云执行初始2D变换的ICP细化（使用Open3D的ICP[27]），产生精确的6-DoF回环闭合约束。为了减少候选数量，初始过滤器选择了最接近当前网格的$n_{candidates}=10$个候选。

作为后端，我们的SLAM使用了一个标准的位姿图（PG），使用g2o[28]实现，类似于[20]。PG在添加新的里程计约束时定期添加新的位姿，但只有当检测到新的回环约束时才全局优化轨迹，在这种情况下，回环闭合模块的轨迹也会更新。图4总结了我们的回环闭合程序。

目前，我们的程序要求传感器的运动主要是在地面上的平面运动，并且需要外部校准以使z轴与地面平面的法线对齐。这限制了支持的传感器设置以及授权的运动。请注意，如果已知重力向量或当地地面平面，可以正确投影高程图像。然而，我们在☑节中展示了，对于传感器主要指向上方的户外场景，我们的回环闭合检测成功地检测到许多回环，使我们的后端能够成功地纠正里程计的漂移误差。

五. 实验

我们通过在多种数据集上进行实验来展示我们方法的效率和多样性：KITTI、KITTI-raw、KITTI-360、KITTI-CARLA、ParisLuco、Newer College Dataset（NCD）和NCLT。我们的方法只需要每个点的几何（xyz字段）和时间戳来弹性地扭曲扫描（它们基于方位角线性估计，对于KITTI-raw和KITTI-360）。

表1：在KITTI-corrected、KITTI-raw、KITTI-360、KITTI-CARLA和ParisLuco数据集上的驾驶模式下，以及在NCD和NCLT数据集上的高频运动模式下的相对平移误差（RTE）[%]。AVG为所有序列各段的RTE平均值，T为每个扫描的平均运行时间。KITTI-corrected是唯一经过运动校正的扫描数据集（）；所有其他数据集的扫描均为未经校正的原始点云。

表2：每个数据集一个序列的回环闭合（LC）指标。ATE=刚体变换拟合到真实轨迹和估计轨迹后，平均绝对轨迹误差[m]，Nloop为检测到的回环数量，LO=激光雷达里程计。