用于精确导航和场景重建的 3D 配准方法（ICRA 2021）

Differential Information Aided 3-D Registration for Accurate Navigation and Scene Reconstruction

作者：Jin Wu, Shuyang Zhang, Yilong Zhu, Ruoyu Geng, Zhongtao Fu, Fulong Ma and Ming Liu（香港科技大学 机器人与多感知实验室(RAM-LAB)

论文地址：https://zhuanlan.zhihu.com/p/378591025?utm_medium=social&utm_oi=985951691249852416

本文核心是研究基于时间差分信息的点云配准问题，提出了在时间差分基础上的点云匹配数学模型，时间差分信息通常来自于点云变化量测量和外部差分信息，如IMU、光流（Optical Flow）、场景流（Scene Flow）等。利用了kD树对提出的新点云配准模型进行迭代最近点（ICP）估计，可以有效的利用时间差分信息对点云对应性带来精确度上的提升。

摘要： 提出了一种新的用于点云配准的3-D 对齐方法，其中采用了测量点的时间差分信息。新问题被证明是一种新颖的多维优化。然后获得此优化的解析解，这为使用 k-D 树的进一步对应匹配奠定了基础。最后，通过许多例子，我们表明新方法在实际实验中具有更好的配准精度。

I 引言

A. Background

3-D配准是当前生物医学成像、多维重建、机器人感知等前沿领域的热门技术[1]、[2]。它找出两个测量点云之间的仿射、刚性或非刚性变换，从而可以有效地合并多个 3-D 视图。这些点云测量通常来自激光扫描仪、RGB-D 相机、结构光等。它也成为使用视觉测量的姿态确定、定位和建图的关键技术[3]-[5]。潜在地，配准也有利于手眼校准问题[6]、[7]。两个点间坐标系{B}和{R}之间的经典 3-D 配准问题可以指定为：

此问题使用最小二乘公式对齐{bi}和{ri} 的两个点集。目标是找到最佳旋转和 R 以及平移 T 以获得最佳点云对齐。

B. Related Work

(1) 中的问题已经通过奇异值分解 (SVD, [8]) 和特征分解 (EIG, [9]) 得到广泛解决。(1) 在点云匹配算法中也起着重要作用，包括迭代最近点（ICP，[10]）和几何特征匹配[11]。问题（1）对于具有大量点的场景非常有效。然而，在工程中，匹配两个点云是具有挑战性的，因为迭代搜索是非凸的。目前，已经付出了大量努力来寻求全局最优 ICP 解决方案，例如 Go-ICP [12] 和 BnB [13]。然而，这些全局最优变体在计算上效率低。随着点数的大幅减少，搜索的复杂性急剧增加。因此，配准性能相应下降，甚至可能导致工程使用失败。这些应用程序中存在的一个关键特性是它们是实时实现的，因此激光扫描仪是连续移动的。可以注意到，具有挑战性的案例通常是动态的。因此，应添加除点云测量之外的更多实质性信息以进行改进。连续测量点云的时间微分测量包含非常有用的 3-D 配准信息。此外， IMU等许多其他传感器可以通过惯性积分 [14]、[15] 提供位姿的差分信息。本文正是基于这样的想法，并提出了一种新的 3-D 配准方法，借助测量的 3-D 点的时间差分。所研究的问题之所以有效，有以下几个原因：

以前的方法通常通过 IMU 测量 [16]、[17] 来近似适当的旋转 R 来解决（1）。但是，如果 bi 和 ri 有偏差，则对应匹配可能会失败。

考虑差分信息时，可以消除这些偏差，从而提高配准精度。也就是说，先前方法的测量是松耦合的，而所提出的解决方案的测量是紧耦合的。

在新问题中考虑了更多的时间差分信息。因此，将给所研究的问题带来更多的实质性信息，从而产生更准确的结果。

C. Contributions

主要贡献是

1) 差分 3-D 配准首次被提出并在数学上进行了表述。这允许一组新的改进配准方程。这些方程在存在多种条件时非常有用。

2) 根据提出的新方程导出解析解的结果，这允许实时计算高效计算。

3）我们建立多个k-D树对不同时刻的点进行同时对应匹配。通过使用匹配良好的点对改进配准结果。

II 差分3D配准方法

A. Rigid Registration

其中 ∆ 表示增量项。这里增量测量 ∆b 和 ∆r 可以直接由连续输出的差分给出。在本文中，旋转和平移增量是使用加窗递归方法（WRA，[19]）估计的。WRA 可以用很小的窗口大小以及很少的历史数据来估计和预测变量，并且被证明比传统的内插或外推方法更有效。建立 (3) 的另一个核心任务是获得 ∆R 和 ∆T 的准确估计，以便可以适当地改进配准。将超低成本 IMU 集成到 LIDAR 中已成为一种普遍做法。与 IMU 结合时，可以直接使用惯性导航机制计算 ∆R 和 ∆T：

平移增量ΔT可以通过IMU预积分获得[14]：

然而，由于 IMU 中的偏差，(4) 和 (5) 中的积分将受到长期漂移的影响。有几种方法可以补偿这种漂移：

1）加速度计和磁力计的辅助：可以通过融合重力加速度和当地地磁场来消除旋转漂移[21]。

2) 零更新辅助：如果激光扫描仪并不总是处于高动态的运动中，它有时会停止。在这种时候，可以调用零角速率更新 (ZARU) 和零速度更新 (ZUPT) 来补偿旋转和平移偏差 [22]。

3) 借助 3-D 激光扫描的内部信息：就像从图像序列中估计 2-D 速度的光流一样，场景流可用于从 3-D 测量中提取 3-D 运动。此外，在城市环境中，有许多具有丰富线和平面特征的建筑可以进一步处理以估计旋转和平移 [23]。

4) 视觉-激光里程计辅助：使用相机和激光扫描仪的组合，可以同时测量2-D和3-D场景。然后它能够进行视觉-激光里程计以进行连续的自我运动估计。

将这种时间差分信息添加到 3-D 配准中可以使测量点数增加一倍，从而在一定程度上提高成功率。配准问题（1）转化为

由于（8）中点云的两个子类别具有不同的尺度，我们需要通过引入点分散和归一化来平衡这种不一致。最终的优化将是

最终，可以通过计算以下矩阵的最大特征值 λmax 及其关联的特征向量 q 来解决新的优化（8）

B. Correspondence Matching

从连续的点云测量中，很难直接获得 ∆bi 和 ∆ri。原因是bi,k和ri,k之间没有对应关系，找到bi,k-1和bi,k也需要对应匹配，其中k表示时刻。为了解决这个问题，我们建立了以下匹配机制：

(13) 形成一个新的ICP形式，与来自连续时间点集的对应匹配。为了有效地获得对应关系，我们使用 k-D 树来加速匹配。用于比较的经典 ICP 公式是

从（14）中，我们可以看到ICP的匹配过程比提出的要简单得多，这表明匹配可能在某些动力学条件下是不适定的。

III 实验结果

A. Overview

为了验证所开发算法的优越性，我们使用图 2 中的无人机 (UAV) 平台，配备机载刚性安装 IMU (Honywell) 和 3-D 激光扫描仪 (Velodyne VLP-16)。IMU 和激光扫描仪之间的时间同步是通过实时运动学 (RTK) 全球导航卫星系统 (GNSS) 接收器的每秒脉冲 (PPS) 进行的。IMU 的采样率为 100Hz，激光扫描仪的频率为 20Hz。该系统还包含一个高精度的姿态和航向参考系统（AHRS）和一个精确的集成导航系统（INS），提供精确的姿态、速度和位置估计。GNSS提供真值。用于对应匹配的 k-D 树的典型实现可以在 [26] 中找到。需要注意的是，由于使用了更多的 k-D 树，因此顺序 k-D 树的计算负载高于经典 ICP。但是，请注意，所有 k-D 树都可以通过并行化实现。因此，所提出的现代多核处理计算机方法的计算成本不会比经典 ICP 高多少。相反，由于考虑了更多的动态测量，因此提出的方法比传统的 ICP 收敛得更快。出于比较的目的，我们使用 ICP 及其 IMUaided 变体进行比较。IMU辅助ICP使用IMU测量的旋转作为预测。惯性姿态/速度/位置传播的机制如（4）和（5）所示。

B. Results

使用开发的无人机平台进行了 20 分钟的试飞。无人机已在城市场景中远程控制以进行 3D 重建。使用我们提出的方法的结果拼接原始点云测量。重建场景如图 1 所示，某一地点的放大视图如图 3 所示。使用不同算法的轨迹如图 4 所示。我们还获得了无人机姿态确定和定位的统计结果，其中 显示在表 I 中。我们还测试了所提出的方法和经典 ICP 的计算负载，其结果在表 II 中提供。在表II中，显示了最大绝对误差(MAE)、均方根误差(RMSE)。

C. Discussions

从上面给出的结果，我们可以观察到所提出方法的优越性。从图 3 可以看出，所提出的方法在重建场景中合并了更丰富的点云扫描。原因是所提出的方法使用多个k-D树进行点云点和差分点的对应匹配。因此，新设计的匹配与松耦合的 IMU 辅助 ICP 相比更准确，并且将匹配更多点进行点云合并。从图 4 所示使用不同算法的轨迹，我们可以看出，与 RTK GNSS 接收机的参考轨迹相比，纯惯性算法的精度最差，而所提出的方法具有最佳精度。IMU 辅助的 ICP 仅因其松耦合的性质而排名第二。所提出的紧耦合差分方法能够消除原始数据中的点偏差，因此具有更好的鲁棒性和准确性。因此，同时提高了姿态确定精度和定位精度，如表 I 所示。如第 III-A 节所示，有更多的 k-D 树用于对应匹配。但是，由于所有 k-D 树都是并行实现的，因此整体计算效率并不比经典 ICP 高多少（见表二）。然而，由于使用了更多的 k-D 树，因此不可避免地必须占用更多的随机存取存储器 (RAM) 空间。

IV结论

在本文中，引入了一种包含时间差分信息的新点云配准公式，以实现混合配准性能。我们表明可以轻松获取时间差分信息。因此，所设计的方案将是实用且易于实施的。通过实验研究，已经发现新的公式可以导致更有效的对应匹配。然而，随着测量维度的增加，我们不可避免地需要更多的空间进行在线处理。