自动驾驶多车协同3D建图

自动驾驶在线协同3D建图

摘要

自动驾驶需要环境的3D表示作为高精度地图。在许多情况下，单个车辆对整个大范围环境进行建图是低效的。因此，车辆组可以协作构建地图。本文提出了一种多车协同在线三维建图方法，各车辆利用3D激光雷达传感器和局部建图算法构建局部地图，并通过一致的方式融合所有局部地图以获得全局地图。协同建图的挑战在于精度和效率。我们证明所提出的协同建图方法不仅能缩短建图时间，还能减少单车建图算法常面临的累积误差。同时，真实世界实验结果表明，我们的建图算法可在线实现，且对每辆车的通信信道和计算资源带来的负担最小。

关键词 —自动驾驶，3D地图构建，激光雷达，协同建图

一、引言

自动驾驶需要构建驾驶环境的3D表示，以用于高精度地图的自主定位、轨迹估计、路径规划和车辆控制。在未知环境中进行自主定位与建图的问题是移动机器人和智能车辆领域中最基本的问题之一，通常被称为SLAM（同步定位与地图构建）问题。在自动驾驶中，使用3D激光雷达的SLAM非常普遍，因为激光雷达能够以相对较低的误差和高频率提供直接的距离测量和点云扫描。

典型的3D地图构建任务可以通过在车辆上安装长时间多通道（从16到128通道）激光雷达（如Velodyne激光雷达），并结合其他必要设备（例如IMU/GNSS、轮速编码器和/或计算机视觉系统，具体取决于解决方案），然后驾驶车辆沿预设路线覆盖建图区域以采集数据来完成。这些数据通过在线或离线建图算法进行处理，以创建地图。该过程可能耗时较长，并且随着建图距离的增加，许多建图算法会受到累积漂移的限制。

为了在对大范围区域（如整个城市的行驶区域）进行建图时提升效率和精度，通常需要两辆或多辆车辆同时参与该建图任务。车辆可以各自独立地构建地图，并在条件允许时将它们的地图融合成一个全局一致的地图。如果假设车辆初始位姿之间的相对坐标变换未知，则地图融合问题将变得具有挑战性。在这种情况下，协作车辆之间不存在公共参考坐标系，必须依据地图数据本身的特征信息来估计局部地图之间的变换关系。

本文提出了一种在线协作式3D地图构建系统，该系统可用于多车协作创建一致的全局地图。配备激光雷达扫描仪的车辆采用基于扫描匹配的建图算法来构建局部3D点云地图。当协作车辆相遇时，它们将交换具有重叠区域的激光雷达扫描数据，以确定各自局部地图参考坐标系之间的相对变换。本文提出了一种由粗到精的点云特征提取与匹配算法，用于对齐重叠区域的地图，从而完成地图融合。

本文组织如下：第二节介绍3D建图和地图融合的相关工作。第三节系统地描述了所提出的协同建图系统的工作流程及其详细组件。第四节通过多个真实世界实验验证了所提出的方法，第五节对全文进行了总结。

II. 相关工作

协同建图问题主要与SLAM问题和地图融合问题相关，这两者在文献中已有大量讨论。SLAM专注于为每个建图车辆自主构建环境地图，而地图融合则有助于将局部地图组合成全局一致的地图。大多数SLAM技术基于概率方法 [1], where the vehicles have probabilistic motion and uncertain perception

模型。此类方法的示例包括基于扩展卡尔曼滤波器（EKF）的方法[2], 、基于Rao‐Blackwellized 粒子滤波器（ RBPF）的方法[3]以及基于稀疏扩展信息滤波器（SEIF）的方法[4]。这些方法在解决SLAM问题方面取得了巨大成功。然而，其所需的计算量使得它们难以用于实时三维建图。基于扫描匹配的SLAM技术[5][6][7]被证明在基于三维点云的地图构建任务中高效且准确。在[8],中，作者引入了惯性测量单元（IMU），并利用闭环来创建大范围地图。本质上，该方法需要批量处理与优化以校正IMU偏差并生成精确地图，因此不适用于需要实时获取地图的应用场景。相反， LOAM（激光里程计与建图）[9]系统无需额外的惯性测量单元（IMU）作为里程计，且3D地图构建任务可在车辆在环境中移动时在线完成。我们采用[9]中描述的方法作为单车的主要3D地图构建算法。

通过扫描匹配算法（如[7][8][9]）获得的地图由注册在局部地图参考坐标系中的点云组成。根据车辆的相对参考坐标系是初始已知、在建图过程中基于某种会合策略 [15][16],获取，还是完全未知[10]，地图融合算法可以分为不同的类别。如果车辆的初始相对位置已知，则地图融合问题将被简化，因为它等价于所有车辆在公共参考坐标系中开始建图的问题[11][12]。我们在本研究中不采用这一假设，因为在实际应用中该假设过于严格。相反，如果协作车辆之间的相对位置完全未知，则地图融合通常需要对所获取的整个地图进行特征提取与匹配，以寻找地图之间的结构/几何相似性[13][14]。此类任务消耗更多的计算能力，因此往往更难实现实时处理。相比之下，我们提出的方法利用车辆会合来识别在建图过程中的相对位姿。该算法可以高效实现，从而能够在线完成地图融合。

III. 三维协同建图系统

A. 系统概述

多车协同三维建图的工作流程如图1所示。假设车辆配备了计算机、局域无线通信接口和高精度RTK GPS接收机。每辆车从各自的初始位置开始探索环境，且未知其他车辆的相对位姿。当未检测到车辆相遇时，各车辆仅使用第三节-B中描述的基于扫描匹配的算法进行单车建图。同时，为了检测对等会合，车辆通过专用的无线通信信道主动发送并监听预定义的信标信号，尝试与处于通信范围内的其他车辆进行通信

范围。信标信号包含每个发送车辆当前的RTK GPS位姿(longitude, latitude, heading)。车辆将利用各自的GPS位姿信息，通过比较彼此之间的物理距离与预设阈值，来判断是否已经“相遇”。需要注意的是，在我们提出的协同建图框架中，会合检测模块也可以采用其他技术实现，例如计算机视觉。

一旦一对车辆确定它们在某个会合点“相遇”，它们将发送最近的激光雷达扫描以相互匹配和融合。地图融合技术将在第三节-D中更详细地描述。地图融合后，车辆将继续进行单体建图，并在可能的情况下相互共享新获取的地图。

B. 单车建图

当团队中只有一辆车辆，或车辆之间尚未“相遇”时，每辆车将独立工作，基于三维激光雷达点云扫描匹配算法构建其局部地图。这里的“扫描”指的是旋转式激光雷达 （例如 Velodyne VLP16）每旋转360度所获取的点云数据。为了创建驾驶区域的正确地图，需要将连续的扫描数据配准到局部坐标系中。在扫描匹配过程中，考虑了重叠的三维扫描的几何结构，以提供准确且一致的地图。

特别是我们采用了“激光里程计与建图（LOAM）”[9]中提出的建图算法作为我们的局部建图方法，因为它不需要后端优化。

优化，并能更好地保证整个框架的实时特性。此外，该算法精度高，且不需要额外的高成本惯性测量单元作为里程计。

如图2所示，局部建图算法包含激光雷达里程计和激光雷达标图模块。点云扫描通过增量式注册来构建局部地图。文献[9]中提出的原始算法适用于安装在云台上的二维激光扫描仪。我们已将其改进，以适应多通道旋转式激光雷达，例如在智能车辆领域广泛应用的Velodyne VPL16。

从下一节开始，我们将详细讨论如何进行多车协同建图。

C. 通信模型和会合对等体检测

假设所有车辆均配备有局域无线通信接口（例如 Wi-Fi、专用短程通信等），以便在车辆处于相互通信范围内时能够以点对点方式相互传输数据。在此设定下，每辆车辆可根据自身的位置信息计算与其他车辆的欧氏距离，从而检测是否存在会合对等车辆。如果该距离低于某一阈值，则认为这两辆车辆为会合对等车辆。随后，它们将启动第三节-D小节中所述的地图融合过程。

直观上，我们希望指示“会合”的对等节点的距离阈值尽可能大（上限为通信范围），以增加检测到会合的可能性，从而触发协同建图过程。然而，该阈值也不宜设置过大，因为在会合点处两辆车辆的激光雷达扫描需要有足够的重叠区域，以获得更好的匹配结果。这一权衡原理将在第四节-A小节中进一步解释，并给出实证结果。

D. 鲁棒地图对齐与融合

一旦建图车辆相遇，意味着它们的局部地图应包含一些需要对齐的重叠区域，以确定其局部参考坐标系之间的相对变换。随后，局部地图可进一步融合为全局地图。这是协同建图的关键步骤之一。

由于本文采用基于三维点云的地图，地图配准基于点云匹配和配准算法 [17][18] 进行。给定两个作为输入的三维点云，匹配算法旨在找出最优的

4x4相对旋转和平移矩阵（RT矩阵，见公式1），用于点云，使得两个点云中对应点之间的距离最小化。

$$
T_{RT} = \begin{bmatrix} R_{3\times3} & T_{3\times1} \ 0_{1\times3} & 1 \end{bmatrix}
\quad (1)
$$

大多数匹配算法的一个特性是，点云中的点越多，收敛到最优结果所需的时间和迭代次数就越多。在我们的场景中，需要高效地匹配来自不同车辆的局部点云地图。因此，我们不是匹配获取的完整局部地图，而仅在车辆会合时匹配最新的激光雷达扫描。原因是，如果车辆在相遇之前各自建图的区域没有重叠，则它们的局部地图中公共部分非常少，难以支持有效匹配。此外，仅使用最近的扫描数据进行交换所需的带宽要低得多。

如图3所示，在交换了最新注册的激光雷达扫描帧后，每辆车辆将对端车辆的激光雷达扫描与相应的局部扫描通过点云匹配算法进行匹配。实际上，由于两辆车的局部参考坐标系之间存在任意差异，待匹配的两个点云可能具有较大的平移和旋转值。常用的ICP [17]或NDT [18]匹配算法可能需要很长时间才能收敛，甚至无法收敛到全局最优。因此，我们提出采用由粗到精的匹配方案来匹配点云。

在粗匹配阶段，我们尝试计算来自不同车辆的两组激光雷达扫描的初始对齐。该初始对齐通过从点云中提取关键点和描述子，并在其中寻找对应关系来实现。为此，我们采用快速点特征直方图（FPFH）[19]算法。FPFH描述子存储了落在关键点球形邻域内的点对之间的法向量相对方向和距离。可以使用RANSAC方法[21]估计关键点之间的点对点对应关系。经过粗匹配阶段后，两组激光雷达扫描应大致对齐。此阶段的输出是一个初始RT矩阵，用于描述车辆中两个局部参考坐标系之间的粗略相对变换。我们将该RT矩阵作为下一精细匹配阶段输入的初始估计值。

在精细匹配阶段，我们使用ICP等迭代配准算法[17]来进一步优化匹配结果并提高精度。该流程具有鲁棒性和高效性，我们将在第四节展示实验结果。

精细匹配阶段的最终输出将是一个RT矩阵，用于准确描述车辆局部地图中使用的两个局部参考坐标系之间的相对变换。因此，我们可以利用4x4 RT矩阵对每个点的坐标进行变换，从而合并已构建的地图并将点云叠加到全局地图中，如公式2所示，其中M2和M1为点坐标

各自的参考坐标系。总之，我们已成功将两个局部地图融合为一个全局地图。

$$
M_2 = T_{RT} \ast M_1
\quad (2)
$$

当任意两辆车辆相遇时，它们可以使用此过程来融合各自的局部地图。此外，当它们离开会合点后，只要处于通信范围内，就可以通过无线通信信道持续相互发送新注册的局部点云扫描。这些新的扫描可以通过直接应用匹配步骤中获得的RT变换矩阵，逐步增量式地融合到全局地图中。

四、实验结果

本文提出的在线协作3D建图方法已通过真实世界实验得到验证。

实验中，我们使用两辆电动巡逻车（见图4）作为平台，每辆车均安装有一个Velodyne VLP‐16激光雷达扫描仪和一个优尼强UGM512 RTK GPS移动站。激光雷达与 GPS之间的参考坐标系已进行标定。我们在校园内实验区域的一栋建筑物顶部设置了优尼强RTK基站。计算任务方面，每辆车辆配备一台联想笔记本电脑，用于运行建图与通信软件。两台笔记本电脑的配置分别为：i7‐6500中央处理器、8GB内存，以及AMD A10‐5700中央处理器、4GB内存。这两台笔记本电脑均能够实时完成本文所提出的各项任务。通信方面，我们采用网件R8000 WIFI路由器，并将其设置为自组网络模式，以实现两辆车辆之间的通信。最大传输速率为3.2 Gbps，通信范围约为50米，具体受建筑物遮挡影响。

软件基于Ubuntu 16.04 LTS操作系统和 ROS‐Kinetic [20]实现。图2展示了用于协同建图的软件流程和模块的框图。

我们验证了所提出的协作建图方法，并在以下三个场景中展示了部分实验结果。在场景A中，我们详细分析了地图配准与融合步骤的结果，这是整个协同建图过程中最关键的步骤之一。在场景B中，我们描述了一个在校园区 域执行的完整协同建图任务。在场景C中，我们展示了协作建图相较于单车建图在时间消耗和建图精度方面的优势。

场景A

在本实验中，我们首先解释选择距离阈值的原理，以确定两辆车辆是否在会合点“相遇”。显然，较大的阈值可以增加车辆会合的可能性。

我们使用第三节-D中提出的由粗到细的扫描匹配方法，在不同距离阈值（如10米、5米、3米和1.5米）下对车辆间的激光雷达扫描进行匹配。定性结果如图5所示。显然，10米距离的匹配结果不理想，而3米和1.5米的结果均可以接受，其中1.5米的对齐更加精确和整齐。表1定量地展示了根据[21],定义的匹配分数，分数越低表示匹配结果越好。为了保证更好的地图融合效果，我们在实验中选择采用1.5米的距离阈值。当然，如果建图任务的设置需要，将阈值放宽至3米也是可行的。

距离阈值	匹配分数
10m	8.45119
5m	4.29151
3m	1.57628
1.5m	0.79729

表 I 不同距离阈值下的匹配分数

当两辆车辆之间的距离在阈值范围内时，它们会发送最新注册的激光雷达扫描数据，通过由粗到精匹配流程进行匹配。由于这些扫描数据已分别在其本地参考系中完成注册，而这些参考系具有任意不同的初始位置和方向，因此扫描之间的旋转和平移也可能任意。这使得难以为ICP等细粒度迭代扫描匹配算法提供合适的初始猜测。

10米。(b) 3米。(c) 1.5米。（5米的结果与 10米相似，因此省略以节省空间）)

如图6(b)所示，直接对扫描应用ICP会导致较大的匹配误差。我们提出的由粗到精匹配方法有助于解决这一问题。在图6(c)中可以看到，粗匹配算法能够为扫描提供相当好的初始对齐，随后通过精细匹配进一步优化。

激光雷达扫描使用叶大小为20厘米的体素滤波器进行降采样，以降低传输带宽和计算强度。在会合期间，车辆之间传输的扫描数据每帧典型大小为100千字节，最高传输速率为10Hz，这对无线信道来说负担不大。我们在具有不同环境结构（如树木、建筑物和道路）的不同会合点进行了测试，匹配结果稳定且能够实时获得。每个车辆中的匹配线程完成一次成功的由粗到精扫描匹配所需时间通常为1至3秒。

是待匹配的原始源扫描和目标扫描。(b) 是没有适当初始猜测的直接ICP匹配结果。(c) 是粗匹配结果，(d) 是精匹配结果)

B. 场景B

在此场景中，任务是通过协作建图对校园内一条超过 500米长的环形路线进行建图。我们使用两辆车辆来完成该任务。需要注意的是，由于车辆以成对方式进行协作，因此可以轻松增加协作车辆的数量。如图7所示，车辆A 和B的建图路径的起点和终点在图中标出。两辆车辆从不同的初始位置开始，各自独立执行建图任务，彼此 unaware 对方的位置。路线设计使得车辆A和B在其路径中途相遇。由于每辆车仅行驶待建图完整路线的一部分，因此建图是协作完成的。当两辆车相遇时，进行地图融合。根据扫描匹配结果揭示的两个局部地图参考框架之间的相对变换，可将每辆车在相遇前后本地获取的地图进行融合。

在图8中，蓝色点云是车辆A构建的地图，红色是车辆B构建的地图。图中左侧红蓝重叠的区域是两辆车首次会合的会合点。我们提出的地图融合技术决定了整体地图的构建质量。如图所示，触发两辆车进行扫描匹配的会合点对齐得非常好。即使在没有的情况下，两条路径终点处的环路也闭合得非常一致

为车辆A的建图路径，(b)为车辆B的建图路径)

我们方法中涉及的显式闭环算法。总体而言，协同建图方法按预期成功运行。

C. 场景C

在此场景中，任务是建图一条比场景B更具挑战性的路线。主要目的是展示使用协同建图的优势。该场景更具挑战性，因为环路沿线的环境更加单调且存在重复模式，这使得扫描配准更加困难。基于扫描匹配的建图算法在这种环境中容易累积误差。协同建图可按比例减少每辆单车的建图距离，从而降低累积误差。

如图9所示，我们同时使用单车（左）和协同（右）建图来完成该任务。整个路线长度约为400米。建图结果如图10所示，左侧为单车建图结果，显然地图构建不正确，因为在真实值中，红色圆圈内的道路是一条没有隔离带的道路。该误差是由于我们所选的单车建图算法中的累积漂移所致。当然，带有闭环的算法可能缓解此问题。然而，在本工作中，我们关注协同建图的简洁性和实时性要求。我们将在未来工作中讨论协同建图中的闭环问题。

所消耗的时间和相对于真实值的总建图长度偏差见表 II。我们可以看到，对于所示的两个场景，协同建图可以节省单车建图所需时间的三分之一到一半。通过在一个任务中使用更多车辆，这种时间节省还可以进一步增加。对于建图质量，除了

为单车建图路径，(b)为协同建图路径)

单车建图结果 (b) 协同建图结果)

视觉验证外，我们使用RTK GPS获得的轨迹总长度作为真实值，并将建图算法得到的轨迹与之进行比较。在两个场景中，协同建图的偏差均小于单车建图，因为更多的车辆共享并减少了总的累积漂移。

场景	RTK轨迹(米)	建图偏差(%)	时间(秒)	轨迹(米)
B单个	583.429	584.859	0.25	170.4
协作	638.422	639.426	0.157	120.02
C单个	394.7	384.433	2.6	121.05
协作	383.207	378.542	1.21	62.31

表II 时间消耗与地图长度偏差

V. 结论

本文提出了一种可用于自动驾驶应用或其他机器人建图场景的在线三维协同建图方法。该方法在地图融合过程中无需预先知道车辆的初始位姿。相反，车辆在会合期间尝试交换局部地图并执行地图融合。本文提出了一种高效的由粗到细的扫描匹配算法，以实现在线且鲁棒的扫描匹配。通过真实世界实验验证了所提方法的有效性。我们已公开发布数据集，以便于未来评估 1。在未来工作中，我们计划改进协同建图方法，使得当两辆车辆在不同时间经过公共区域时也能完成地图融合。时间。并将利用多次会合进行闭环，以优化所构建的地图。