用C++构建激光雷达障碍物检测器

在完成Udacity的传感器融合纳米学位后，我用C++实现了一个基于激光雷达的障碍物检测管道。为了使其运行，我首先将原始代码库适配到现代库和工具链中——更新了构建系统，解决了依赖问题，并确保与最新版本的PCL和C++编译器兼容。

这个项目专注于处理城市驾驶场景中的激光雷达点云数据。但在深入实现之前，我会先介绍激光雷达的工作原理及其重要性，以及它如何解决相机和雷达等其他传感器的局限性。（提示：传感器冗余不仅是一种很好的补充，而且对安全性至关重要。）

1、为什么激光雷达很重要

激光雷达（Light Detection and Ranging）是一种基于激光的传感技术，能够捕捉环境的高度精确三维测量值。与依赖环境光的相机不同，激光雷达主动发射数千——甚至每秒可达数百万——个红外激光脉冲，覆盖一个宽广的视场，通常是360°，并计算每个脉冲返回所需的时间。这使得即使在恶劣的照明条件或高反光环境中，也能进行精确的距离测量，而这是相机通常难以应对的情况。

除了深度信息，激光雷达还能捕获强度值，从而提供表面材料的洞察力。结果是生成了一个密集且高分辨率的三维环境地图，使它成为自动驾驶系统感知的强大工具。尽管目前成本较高，但激光雷达在鲁棒性和可靠性方面的关键优势对于实时决策和碰撞避免至关重要。

2、检测器的工作原理

我们的激光雷达点云数据（以.pcd文件形式存储）由三维数据点（x, y, z + 强度）组成，捕捉高速公路驾驶环境的空间快照。这些数据以大约10Hz的频率记录（约每0.1秒一次）。在这个项目中，原始点云通过一系列过滤、分割和聚类技术进行处理——更具体地说：

• 体素网格过滤：降低点云密度以加速处理，同时保留空间结构。
• RANSAC平面分割：通过拟合平面模型识别路面，将地面点与物体分离。
• KD树欧几里得聚类：将附近的点分组，以识别不同的障碍物如车辆或行人。

这些步骤将未结构化的激光雷达数据转换为具有边界框的结构化障碍物检测，这对于路径规划、态势感知和安全导航至关重要。这基本上就是障碍物检测项目的主体部分，但它不仅仅是一个编码练习。

基于激光雷达的障碍物检测是自动驾驶安全的基础组件。

与相机不同，激光雷达提供可靠的深度数据，无论光照条件如何。以高空间精度检测和跟踪障碍物对于路径规划、碰撞避免和安全车辆操作至关重要。这就是为什么这个项目如此重要的原因——它是迈向更智能和负责任的移动系统的重要一步。

你可以在GitHub上找到逐步说明。在这篇文章中，我将带你了解激光雷达数据是如何被过滤、分割和聚类以检测3D环境中的障碍物的——并分享一些从头开始使用C++构建此项目的关键经验。

3、激光雷达模型和模拟点云

为了模拟点云生成过程，我们使用了一个简化的激光雷达模型。该模型接收最大和最小射线距离、角度分辨率以及周围车辆列表等参数。基于这些输入，它生成一组代表激光雷达射线的射线。

然后激光雷达的扫描功能执行光线投射——一种3D渲染技术，从摄像机视角模拟光线路径。每条射线都会检查是否与附近的车辆或地面平面发生碰撞。结果是一个点云，捕捉场景，并添加少量的高斯噪声来模仿真实传感器的不完美。下图展示了一个简单的高速公路场景，说明了这一过程。

4、点云数据预处理

接下来，我们将处理来自实际激光雷达的真实世界点云数据。这些数据通常存储在.pcd文件中（用于该项目的数据的github链接）。相同的处理技术也可以扩展到处理连续流式传入的点云。

上面的图像显示了一个覆盖大面积的高分辨率点云。为了帮助处理管道更有效地处理数据，需要对点云进行过滤。主要有两种技术用于此目的：体素网格过滤和感兴趣区域（ROI）裁剪。

4.1 体素网格过滤

体素网格过滤是一种点云降采样方法。该技术利用体素化网格方法减少数据集中的点数量。它首先创建一个3D体素网格（可以将体素视为空间中的小3D盒子，比单个点云占用更少的内存），其分辨率由输入参数控制。分辨率应足够低以提高处理速度，但不能低到完全丢失物体定义。然后，每个体素中的所有点将被其质心近似表示。

为什么降采样很重要？

由于生成的数据量巨大，在车辆内部网络上传输原始激光雷达点云是低效的。为了解决这个问题，数据被降采样并转换为更紧凑的表示形式，称为stixels——“stick”和“pixel”的组合。stixels最初是为立体相机处理开发的，它们是垂直矩形段（像火柴棍一样），用来近似场景中的物体表面。

4.2 区域兴趣（ROI）裁剪

激光雷达扫描范围从自车向外延伸很远。可以通过裁剪来保留有用的信息，从而减少处理时间。一个适当的感兴趣区域应该包括自车前方足够的空间，以便能够及时对靠近的障碍物做出反应。对于侧面，至少应覆盖道路宽度以检测附近的物体或车辆。此外，还可以移除击中自车车顶的点。

过滤和裁剪后的点云数据如下所示。

4.3 点云预处理指南

为了从你的点云处理管道中获得最佳效果，请考虑以下调整：

1. 体素网格降采样：使用足够大的体素大小以提高性能，但又足够小以保留物体形状。可以从0.2米或0.3米的值开始，并根据场景和处理速度进行调整。
2. 感兴趣区域（ROI）：定义一个包含足够前方空间以检测和响应接近障碍物的区域，两侧覆盖道路宽度以检测附近物体或车辆，并确保所有关键障碍物都在ROI内以实现有效感知。
3. 相机视图（可选）：调整[environment.cpp](https://github.com/moorissa/lidar-obstacle-detector/blob/main/src/environment.cpp)中的相机角度，以帮助选择和可视化ROI。使用俯视图以获得布局上下文。使用侧视图检查垂直过滤和边界框。
4. 移除自车车顶点：使用pcl::CropBox隔离自车车顶点索引。同样将这些索引传递给pcl::ExtractIndices以将其移除，就像在点云分割中一样。这有助于防止自车结构产生误检。
5. 调试边界框：使用renderBox函数可视化场景中边界框的大小和位置。有助于检查ROI、体素大小和过滤逻辑是否按预期工作。

5、点云分割

激光雷达点云处理的一个关键目标是从潜在障碍物中分离出道路平面。如果道路平坦，区分道路点和非道路点相对简单。为此，基于随机抽样一致性（RANSAC）算法的平面分割被使用。

RANSAC（随机抽样一致性）平面分割

RANSAC代表随机抽样一致性，是一种用于检测数据中异常值的方法。该算法运行一定次数迭代，并返回最符合数据的模型。每次迭代随机选择子集数据并拟合模型如直线或平面。具有最多内点或最低噪声的迭代则被用作最佳模型。具体来说，以下是通过分离道路表面（这不是障碍物）来检测点云场景中障碍物的步骤。

1. 假设：使用基于RANSAC的平面分割来找到道路，假设它是平坦的。最大的平面表面被认为是道路。
2. 实现概述——使用ProcessPointClouds类在[processPointClouds.cpp](https://github.com/moorissa/lidar-obstacle-detector/blob/main/src/processPointClouds.cpp)中进行过滤、分割（使用RANSAC）、聚类以及加载/保存PCD文件。
3. 步骤：

• 实现SegmentPlane函数：(1) 设置RANSAC的最大迭代次数和距离阈值，(2) 提取内点（位于道路平面上的点）
• 实现SeparateClouds函数：使用PCL的ExtractIndices将道路云（内点）与障碍物云（非内点）分开

输出（分割点云的可视化）将是道路作为一个云（例如绿色），障碍物如汽车或树木作为另一个云（例如红色）。

RANSAC算法有多种变体。一种类型选择最小可能的子集来拟合模型。对于一条直线，那将是两个点；对于一个平面则是三个点。然后通过迭代遍历剩余的每个点并计算其到模型的距离来计数内点。那些距离模型在某个特定距离内的点被视为内点。拥有最多内点的迭代即为最佳模型。

RANSAC的其他方法可能会采样一部分模型点，例如总点数的20%，然后拟合一条直线到那部分。然后计算该直线的误差，误差最小的迭代即为最佳模型。这种方法可能有一些优点，因为并非每次迭代都需要考虑每个点。建议尝试不同的方法并测试时间结果，看看哪种方法最适合。下面的图形展示了二维RANSAC算法。

平面分割过程的输出是一对点云——一个表示道路，另一个表示障碍物。分割后的PCD数据如下所示。

6、欧几里得聚类

一旦障碍物和道路点被分割，下一步是对代表不同障碍物的点进行聚类。聚类类似于在点群之间绘制边界并将其分组为对象，例如“汽车”或“行人”，本质上不同于分割。以下是聚类的一个示例。

一种执行聚类的方法是使用欧几里得聚类算法。其想法是根据点之间的接近程度创建关联。这涉及执行最近邻搜索，为了高效地做到这一点，需要一个如KD树之类的数据结构。

6.1 KD树实现

KD树是一种K维二叉搜索树，通过在交替维度上分割点来组织空间数据。通过这种方式，KD树能够以O(log(n))的时间复杂度实现高效的最近邻搜索，而不是O(n)。这是因为通过在KD树中将点分组到区域中，搜索空间被极大地缩小，从而避免了对成千上万个点进行昂贵的距离计算。这里解释了构建KD树的算法这里。下图显示了二维点在空间分割前后的对比。蓝色线条表示X维度分割，红色线条表示Y维度分割。

一旦点能够插入树中，下一步就是在树中相对于给定点搜索附近的点。在距离容差范围内的点被认为是附近的。

寻找附近邻居的朴素方法是遍历树中的每个点并与目标点比较距离，选择落在目标点距离容差范围内的点索引。

相反，使用KD树时，会使用一个边长为2 X 距离容差、以目标点为中心的方框。如果当前节点点在该框内，则仅在此时计算欧几里得距离，并根据此决定该点是否应添加到附近点列表中。进一步地，如果该框不跨越节点分割区域，则完全跳过另一侧的分支。如果它跨越，则递归探索该侧。下图展示了这一点。红色十字表示完全跳过的区域。

一旦实现了KD树方法来搜索附近的点，下一步是实现欧几里得聚类方法，该方法根据它们的接近程度将单独的聚类索引分组。在[cluster.cpp](https://github.com/moorissa/lidar-obstacle-detector/blob/main/src/quiz/cluster/cluster.cpp)中有名为euclideanCluster的函数，它返回一个向量的向量整数，这是聚类索引的列表。

1. 为了执行聚类，遍历云中的每个点并跟踪哪些点已经被处理。
2. 对于每个点，将其添加到定义为聚类的点列表中，然后使用上一练习中的搜索函数获取所有靠近该点的点列表。
3. 对于每个尚未处理的靠近点，将其添加到聚类中并重复调用靠近点的过程。
4. 当第一个聚类的递归停止时，创建一个新的聚类并遍历点列表，对新聚类重复上述过程。
5. 一旦所有点都被处理，就会发现一定数量的聚类，将其作为聚类列表返回。

下图显示了聚类后的二维空间。

下图显示了真实的聚类PCD数据。

6.2 如何改进树

为了优化k-d树中的搜索性能，保持树平衡并通过在插入时均匀划分点空间非常重要。这是通过在x轴和y轴之间交替分裂并在当前轴上始终插入中位数点来实现的。

例如，给定以下四个二维点：

(-6.3, 8.4), (-6.2, 7), (-5.2, 7.1), (-5.7, 6.3)

我们会按照以下顺序插入这些点：1. (-5.2, 7.1)——x值的中位数（第一次x分割）2. (-6.2, 7)——按y排序的剩余点的中位数（y分割）3. (-5.7, 6.3)——x分割（剩余两个点的较低中位数）4. (-6.3, 8.4)——最后一个点

在每一级选择中位数确保区域尽可能均匀地分割，从而生成一个平衡良好的树，显著加快最近邻或范围查询的速度。

7、边界框

作为最后的润色，可以在聚类周围添加边界框。边界框的体积也可以看作是汽车不允许进入的空间，否则会导致碰撞。

在这种生成边界框的方法中，框总是沿着X轴和Y轴定向。如果聚类的大多数点沿这些轴分布，这是可以接受的。然而，如果聚类有一个非常长的矩形物体，与X轴呈45度角，则生成的边界框会过大，限制了自车可用的移动空间。

在上图中，右侧的边界框更高效，考虑到围绕Z轴的旋转，并包含所有点所需的最小面积。本项目不包括生成此类框的代码，但可以使用主成分分析（PCA）技术来识别点的主要轴，并使用四元数成员进行旋转。

8、结束语

我们已经涵盖了从了解激光雷达传感器的工作原理，到模拟点云数据，再到实现过滤、分割和聚类技术。从头开始用C++构建激光雷达障碍物检测器不仅仅是点云处理的练习——它是一次深入了解使自动驾驶汽车理解和安全导航世界的感知系统的深入探讨。这也提醒了机器人和自主性的一个基本原理，那就是感知就是力量。

在动态环境中准确检测和分类障碍物构成了碰撞避免、路径规划以及最终信任自动驾驶系统的基础。

随着自动驾驶技术的发展，激光雷达仍然是传感器套件中的关键部分——在复杂的驾驶场景中提供深度、精确性和冗余。通过在C++中实现这些技术，我对看似简单的驾驶决策背后的复杂性有了更深的理解。

我们也可以将这次练习视为适用于任何依赖视觉感知的安全关键系统的框架——无人机、监控、机器人，甚至是国防应用。请随意查看我的Github仓库中的完整代码、文档和逐步说明，并亲自尝试一下。无论是学生、研究人员还是工程师进入机器人领域，我希望这个项目能让你更好地理解激光雷达，并激励你构建智能移动的未来。

原文链接：Building a LiDAR Obstacle Detector in C++

汇智网翻译整理，转载请标明出处