0. 简介

现有很多LiDAR里程计都依赖于某种形式的ICP估计帧间位姿，例如CT-ICP, LOAM等。现有的系统设计需要对机器人的运动(CT-ICP)和环境的结构(LeGO-LOAM)有一些特定的假设。而且几乎没有系统能够不需要调参（例如特征提取、面特征拟合、法向量估计、畸变矫正）就能用于不同的场景、不同的LiDAR、不同的运动模式、以及不同种类的机器人。为此《KISS-ICP: In Defense of Point-to-Point ICP – Simple, Accurate, and Robust Registration If Done the Right Way》通过去除大部分部件并专注于核心元素，得到了一个简单且非常高效的系统，并且可以使用不同的LiDAR传感器在各种环境条件下运行（无人车、无人机、两轮车Segway、手持固态LiDAR）。相关的代码已经开源了，我们可以在Github上找到代码。

1. 主要贡献

本文的主要贡献是提出了一种简单而高效的方法，用于构建激光雷达里程计系统，可以在机器人在环境中导航时在线准确计算机器人的姿态。

1. 文中“保持小而简单”的方法利用点对点ICP与最先进的里程计系统不相上下
2. 该方法可以在大量不同的环境和运动配置下准确计算机器人的里程计，并且使用相同的系统配置
3. 提供了一种有效的解决运动扭曲问题的方法，而不依赖于IMUs或轮式里程计。

图1：我们提出的里程计管道在不同数据集上使用相同的参数生成的点云地图（蓝色）。我们在图中用黄色表示最新的扫描。这些扫描使用不同的传感器记录，具有不同的点密度、不同的方向和不同的拍摄模式。汽车示例来自MulRan数据集[15]。Voxgraph数据集[23]中的无人机和NCLT数据集[5]中使用的Segway机器人展示了高加速度运动轨迹。手持Livox激光雷达[17]与通常使用的旋转机械激光雷达具有完全不同的拍摄模式。

2. KISS-ICP方法综述

本研究旨在通过逐步注册激光雷达扫描仪记录的点云，增量计算移动激光雷达传感器的轨迹。我们将组件减少到最小集合，以构建一个有效、准确、稳健且仍然相对简单的激光雷达里程计系统。

对于以局部、自我中心点云P={p_i|p_i∈\mathbb{R}^3}形式的每个三维扫描，我们执行以下四个步骤以在时间t获得全局姿态估计T_t∈SE(3)。

1. 首先，我们应用传感器运动预测和运动补偿，通常称为去畸变，以消除传感器在扫描期间运动引起的3D数据畸变。
2. 其次，我们对当前扫描进行子采样。
3. 第三，我们估计输入点云与参考点云之间的对应关系，我们称之为局部地图。我们使用自适应阈值方案进行对应关系估计，限制可能的数据关联并过滤潜在的离群值。
4. 第四，我们使用鲁棒的点对点ICP算法将输入点云注册到局部地图。最后，我们使用已注册扫描的下采样版本更新局部地图。以下详细描述这些组件。

3. 步骤1：运动预测和扫描去畸变

我们主张在移动机器人记录数据的情况下重新考虑点云配准。人们不应该将其视为配准任意一对3D点云。相反，应该将其表述为通过配准连续扫描来估计机器人的实际运动偏差与预期运动之间的差异。

在考虑LiDAR数据之前，可以使用不同的方法来计算机器人的预期运动。三种最流行的选择是恒定速度模型、通过编码器获得的轮式编码器和基于IMU的运动估计。恒定速度[29]模型假设机器人的平移和旋转速度与上一个时间步长相同。它不需要额外的传感器（没有轮式编码器，没有IMU），因此是最广泛适用的选择。

我们的方法使用恒定速度模型有两个原因：首先，它通常适用，不需要额外的传感器，并避免了传感器之间的时间同步需求。其次，正如我们在实验评估中所展示的那样，它足够好，可以在搜索数据关联和去畸变3D扫描时提供一个可靠的初始估计。这是因为机器人LiDAR传感器通常以10 Hz到20 Hz的速率记录和流式传输点云，即每0.05秒到0.1秒。在大多数情况下，加速或减速，即在这样短的时间间隔内偏离恒定速度模型的偏差，相当小。如果机器人加速或减速，机器人姿态的恒定速度估计将略有偏差，因此我们需要通过配准来纠正这个估计。这些加速度决定了（静态）3D点的可能位移。

恒定速度模型通过使用旋转矩阵R_t∈SO(3)和位移矢量t_t∈\mathbb{R}^3表示的上一个姿态估计T_{t−1} = (R_{t−1}, t_{t−1})和T_{t−2} = (R_{t−2}, t_{t−2})来近似时间t处的平移和角速度，分别表示为v_t和ω_t。我们首先计算相对姿态T_{pred,t}，作为运动预测：

速度和角速度为：

其中∆t是一个LiDAR扫描的采集时间，通常为0.05秒或0.1秒，Log: SO(3) → \mathbb{R}^3 提取轴角表示。 请注意，也可以使用轮式编码器或基于IMU的运动预测方法来计算每个时间步长的v_t和ω_t。这不会改变我们方法的其余部分。例如，如果有好的轮式编码器测量可用，也可以使用它。但是，我们使用恒定速度作为通用方法。 在一个LiDAR扫描的采集时间∆t内，扫描仪会测量多个3D点。每个点p_i∈ P的相对时间戳s_i∈ [0，∆t]描述了相对于扫描的第一个测量的记录时间。这个相对时间戳允许我们计算运动补偿，导致经过校正的扫描P^∗ 的去畸变点p^∗_i∈ P^∗。(这个思路和Fast-LIO2中去畸变的思路类似)

其中Exp: \mathbb{R}^3 →SO(3)从轴角表示计算旋转矩阵。请注意，Exp(s_i,ω_t)等效于在轴角域中执行SLERP插值算法。

这种扫描去畸变的形式，特别是使用恒定速度模型，易于实现，通用，并且不需要额外的传感器，传感器之间的高精度时间同步，或者需要估计IMU偏差。这种方法通常比更复杂的补偿系统[10]表现得更好，至少在扫描开始和结束之间的运动很小，这对大多数机器人应用程序来说是成立的。

4. 步骤2：点云子采样

在点云中识别一组关键点是扫描配准的常见方法[14]，[24]，[35]。通常这样做是为了实现更快的收敛和/或更高的数据关联鲁棒性。然而，点云的复杂过滤通常带有额外的复杂度和需要经常调整的参数。 我们提出的方法不是提取3D关键点，而是仅计算一个空间下采样版本\hat{P}^∗的去畸变扫描P^∗。使用体素网格进行下采样。正如我们将在下面一节中详细解释的那样，我们使用体素网格作为我们的本地地图，其中每个体素网格的大小为v×v×v，并且每个单元格仅存储一定数量的点。每次处理进入的扫描时，我们首先将扫描的点云下采样到一个中间点云P^∗_{merge}，然后在确定机器人的相对运动后用于更新地图。为了获得P^∗_{merge}中的点，我们使用α∈(0.0,1.0]的体素大小αv，并且每个体素仅保留一个点。

对于ICP配准，较低分辨率的扫描是有益的。因此，我们再次使用β∈[1.0,2.0]的体素大小βv对P^∗_{merge}进行下采样，仅保留每个体素中的一个点，从而计算出进一步降低的点云\hat{P}^∗。这进一步减少了配准期间处理的点数，并允许快速和高效的对齐。这种“双重下采样”的想法源于CT-ICP [10]，这是迄今为止在KITTI上表现最佳的开源LiDAR里程计系统。

然而，大多数体素化方法选择每个占用体素的中心来下采样点云[25]，[36]。不一定位于原点云上，本文实验发现保留原点云的点效果要好一点。这意味着降低的点云是去畸变点云的子集，即\hat{P}^∗⊆P^∗。在我们的实现中，我们仅保留第一个插入voxel里的点。

5. 步骤3：scan到local map对应关系估计

与之前的工作[1]，[10]，[19]，[35]一致，我们将去畸变和下采样的扫描\hat{P}^∗与到目前为止构建的点云即本地地图进行配准，以计算增量姿态估计∆T_{icp}。我们使用帧到地图的配准，因为它比帧到帧的对齐更可靠和鲁棒[1]，[19]。为了有效地实现这一点，我们必须定义一个表示先前注册的扫描的数据结构。

现代方法使用非常不同类型的表示来表示本地地图。流行的方法包括体素网格[35]，三角形网格[31]，surfel表示[1]或隐式表示[11]。如上一节所述，我们利用一个体素网格来存储3D点的子集。我们使用一个体素大小为v×v×v的网格，并存储每个体素最多N_{max}个点。注册后，**我们通过使用全局姿态估计T_t将来自新扫描的点{T_t p | p ∈ P^∗_{merge}}添加到体素网格中来更新体素网格。**已经包含N_{max}个点的体素不会被更新。此外，鉴于当前的姿态估计，我们移除超出最大范围r_{max}的体素。因此，地图的大小将保持有限。

我们使用哈希表而不是3D数组来存储体素，从而实现内存有效表示和快速最近邻搜索[10]，[20]。但是，使用的数据结构可以轻松替换为VDBs [18]，[32]，Octrees [30]，[34]或KD-Trees [2]。

6. 自适应阈值数据关联(比较重点的工作)

ICP通常执行最近邻数据关联，以在两个点云之间找到对应点[3]。在搜索关联点时，通常会强制规定相应点之间的最大距离，通常使用1m或2m的值[1]，[31]，[35]。这个最大距离阈值可以看作是一种异常值拒绝方案，因为所有距离大于该阈值的对应关系都被视为异常值并被忽略。

这个阈值τ的要求取决于预期的初始姿态误差、场景中动态物体的数量和类型，以及某种程度上的传感器噪声。它通常是启发式选择的。基于第3节中关于恒定速度运动预测的考虑，我们可以通过分析里程计可能随时间偏离运动预测的程度，从数据中估计一个可能的极限。姿态中的这种偏差∆T恰好对应于要应用于预测姿态的局部ICP校正（但事先不知道）。直观地说，我们可以观察机器人的加速度在∆T的大小上。如果机器人没有加速度，则∆T的大小通常在零左右，这意味着恒定速度假设成立，ICP不需要进行任何校正。 我们通过利用到目前为止成功的ICP执行，将这些信息集成到我们的数据关联搜索中。我们可以估计在存在潜在加速度的情况下，相邻扫描之间相应点之间可能的点位移，通过∆T表达如下：

其中∆R ∈ SO(3)和∆t ∈ \mathbb{R}^3指偏差的旋转和平移分量，由以下公式给出：

δ_{rot}(∆R)这个术语代表当旋转∆R时，在最大测距范围r_{max}内进行测距所造成的位移。请参见图2。需要注意的是，公式（5）构成了点位移的上界。

图2：旋转和平移偏差（∆R，∆t）从预测运动中引起的最大点位移δ（∆T）的计算示例

上式由三角不等式可得。

为了获得δ_{rot}，还可以考虑其他方法，例如考虑自适应阈值计算中的单个测距范围[4]。在我们的测试中，我们没有看到任何结果上的差异，但总运行时间增加了3倍；因此，我们使用r_{max}而不是r来计算δ_{rot}。

为了计算时间t的阈值τ_t，我们考虑使用Gaussian分布覆盖δ，使用到目前为止计算的轨迹上的公式（5）的值，每当偏差大于最小距离δ_{min}时，即机器人运动偏离恒定速度模型的情况。其标准差是

M_t是一个索引集，表示直到t时刻的偏差。

这样做可以避免在机器人不移动或长时间以恒定速度移动时过度降低σ_t标准差的值。在我们的实验中，我们将此阈值δ_{min}设置为0.1米。然后，我们将三倍标准差边界τ_t = 3σ_t计算为阈值τ_t，用于下一节中的数据关联搜索。

7. 第4步：通过鲁棒优化进行对齐

我们基于经典的点对点ICP [3]进行配准。选择这种方法的优点是我们不需要计算依赖于扫描仪或环境的数据相关特征，如法线、曲率或其他描述符。此外，在噪声或稀疏的LiDAR扫描仪中，例如法线这样的特征通常不是非常可靠的。因此，在对齐过程中忽略法线等数量是明确的设计决策，使我们的系统能够很好地适用于不同的传感器分辨率。

为了获得机器人姿态的全局估计T_t，我们首先将预测模型T_{pred,t}应用于局部坐标系中的扫描\hat{P}^∗。然后，我们使用先前的姿态估计T_{t−1}将其转换为全局坐标系，得到源点。

对于ICP的每个迭代j，我们通过在体素网格（第5节）上进行最近邻搜索，仅考虑点对点距离低于τ_t的对应关系，从而获得点云S和局部地图Q = {q_i | q_i ∈ \mathbb{R}^3}之间的一组对应关系。为了计算当前的姿态校正∆T_{est,j}，我们执行鲁棒优化，最小化点对点残差的总和。

其中C(τ_t)是距离小于τ_t的最近邻对应关系集合，ρ是Geman-McClure鲁棒核函数，即具有强离群值拒绝特性的M-估计函数，由以下公式给出：

其中核函数的比例参数κ_t会根据σ_t进行在线调整。最后，我们会更新点s_i，即

并重复此过程，直到达到收敛准则。通过这个过程，我们得到了变换T_t = ∆T_{icp,t}T_{t−1}T_{pred,t}，其中∆T_{icp,t} = \Pi_j∆T_{est,j}。虽然我们将预测模型T_{pred,t}（即常速度预测）应用于扫描的局部坐标系，但我们在机器人的全局参考系中执行ICP校正∆T_{icp,t}，这是出于效率原因这样做，因为它允许我们每个ICP迭代只对源点S进行一次变换。因此，在时间t使用的局部姿态偏差∆T_t可以用公式（5）表示。

ICP算法的标准终止准则是控制迭代次数。此外，大多数方法还有一个基于解的最小变化的进一步准则。相反，我们发现控制迭代次数并不总是能够找到一个好的解决方案。因此，我们只采用基于应用的校正小于γ的终止准则，而不强制规定最大迭代次数。最后，ICP校正被应用于点云P^*_{merge}，并且将点集集成到局部地图中。

7. 参考链接

https://zhuanlan.zhihu.com/p/624628401