自动驾驶 11-2: 激光雷达传感器模型和点云 LIDAR Sensor Models and Point Clouds

在上一个视频中，我们谈到了激光雷达的基本工作原理， 自动驾驶汽车最受欢迎的传感器选择之一。 在接下来的两个视频中，我们将学习如何使用 LIDAR 生成的点云 传感器为我们的自动驾驶汽车进行状态估计。 在本视频结束时，您将能够描述基本的点云数据 用于存储 LIDAR 扫描的结构。

描述点云上的常见空间操作，例如旋转和缩放。 并使用最小二乘法将平面拟合到点云中，以便 检测道路或其他表面。

首先，回想一下 3D LIDAR 传感器返回距离测量值， 它扫描的每个点的仰角和方位角。 我们知道如何将这些球坐标转换成笛卡尔坐标 x, y, 使用逆传感器模型得到 zed 坐标，所以 我们可以使用 LIDAR 扫描的所有测量结果构建一个大型点云。

对于某些 LIDAR 设置，这并不少见 这些点云包含超过一百万个点或更多。

那么我们能做什么这些巨大的点云呢？

让我们考虑一个我们可能在现实世界中遇到的点云的例子。

假设我们的 LIDAR 扫描路边的一棵树，然后 产生一个看起来像这样的点云。

我们只看到面向我们的树部分的点，因为树和 叶子反射红外线。

您可能会问的第一个问题是我们如何跟踪所有这些点？ 我们应该使用什么样的数据结构来处理它们？

一种常见的解决方案是为每个点分配一个索引， 说点1到点n，并存储x，y， 每个点的 zed 坐标为 3 x 1 列向量。 从那里，您可以考虑将这些向量中的每一个存储在一个列表中，或者 你可以将它们并排堆叠成一个矩阵，我们称之为大 P。 这样做可以更轻松地使用标准线性代数 库，比如 Python NumPy 库，它可以让我们利用快速 矩阵运算而不是迭代列表和 独立处理每个向量。

那么我们在谈论什么样的操作呢？ 三种基本的空间操作对于 用点云进行状态估计。

平移、旋转和缩放。 我们将依次讨论这些。

当我们考虑点云上的空间操作时， 我们的直觉可能是从物理上操纵点云的角度来思考 而我们的参考系保持固定。

但是对于状态估计， 换个角度思考问题更有用。

世界上的物体大多停留在参考系 附着在车辆上的人从不同的角度移动和观察世界。

所以让我们考虑一下如何转换我们的参考系，比如说，通过驱动 十米会影响我们对点云中单个点的感知。

我们可以从传感器框架的原点开始绘制矢量， S，在某种程度上，P。

现在，考虑第二个框架，S-prime， 由于车辆的运动，其原点已相对于 S 平移。

请注意，帧 S-prime 的基向量与基向量相同 帧 S。 只有原点移动了。

我们可以从 S-prime 的原点到 P 点绘制另一个向量。

我们立即注意到结果向量， 这里指出，只是其他两个向量的尖端到尾部的总和。

这些向量只是几何对象，直到我们将它们表达为 一个坐标系。 我们所追求的是 S-prime 帧中点 P 的坐标。 我们可以通过减去帧到帧来轻松获得这些 来自 S 帧中 P 坐标的平移向量。 通过简单的平铺，这很容易扩展到全点云上的批处理操作 大矩阵 R 中的帧到帧平移，以及 从点云矩阵中减去它。

根据您使用的语言或线性代数库， 您可能不需要明确构建这个 R 矩阵。 例如，在 Python 中，NumPy 库足够智能，可以重复 使用广播语义隐式地进行帧到帧转换。

现在，让我们考虑一下如果旋转我们的参考系而不是旋转我们的参考系会发生什么 翻译它。

再次记住，我们不会改变物理点 P， 只是我们的看法而已。 所以在这种情况下， 我们只需要考虑从帧 S 的原点到 P 的一个向量。

在这种情况下发生的变化实际上是我们使用的一组基向量 表示向量 S 到 P 的坐标。

请记住，旋转矩阵 C 告诉我们如何找到向量的坐标 在从原始帧中向量的坐标旋转的帧中。

所以如果我们知道从 S 帧到 S-prime 帧的旋转矩阵， 我们所要做的就是将它与 S 帧中 P 的坐标相乘 得到帧 S-prime 中 P 的坐标。 要确定整个旋转点云的坐标，操作是 完全一样，这要归功于矩阵乘法的特性。

最后要考虑的空间操作是缩放， 它的工作原理与旋转非常相似。 但不是改变我们的基向量的方向 坐标系，我们正在改变它们的长度。

在数学上，这只是意味着预先乘以坐标 每个点由一个对角矩阵 S 的非零元素 只是沿每个维度所需的缩放因子。

通常但不总是这些比例因子是相同的， 矩阵乘法相当于乘以一个定标器。

在这些情况下，我们说缩放在每个方向上都是各向同性的或相等的。

我们可以对单个点或对于 整个点云，就像我们为旋转所做的那样。

通常，我们感兴趣的转换是 平移和旋转，有时缩放。

例如，我们经常对估计翻译和 旋转最适合点云，所以 我们可以估计我们的自动驾驶汽车的运动。

我们将在下一个视频中讨论如何做到这一点。

对我们来说幸运的是，很容易将所有三个操作组合成一个 equation 通过首先平移每个向量， 然后旋转到新框架中，最后应用任何缩放。

当然，此操作也扩展到批处理案例。

所以我们已经看到了如何将基本的空间操作应用于点云。 我们将在下一个视频中看到我们如何使用这些概念进行状态估计 用于自动驾驶汽车。

但在我们到达那里之前，还有一个更重要的操作需要讨论，并且 那是平面拟合。 平面拟合最常见和最重要的应用之一 自动驾驶汽车正在弄清楚路面在哪里，并且 随着汽车继续行驶，预测它会在哪里。

如果你回想起你的高中几何课， 您可能还记得 3D 平面的方程。 Zed 等于 a 加 bx 加 cy。 这个方程告诉你平面的高度 z 当您在 x 和 y 方向上移动时会发生变化。 它取决于三个参数 a、b 和 c，它们告诉您斜率 每个方向上的平面以及 zed 轴与平面相交的位置。

所以在我们的例子中，我们有一堆 x、y 和 从我们的 LIDAR 点云中获取 zed，我们想要找到参数 a 的值， b 和 c 为我们提供了通过这些点的最佳拟合平面。

为了做到这一点，我们将回到我们选择的工具的模块一， 最小二乘估计。

我们将首先为点云中的每个点定义一个测量误差 e。 而 e 只是预测值之间的差异 我们的因变量 zed-hat 和 zed 的实际观察值。

我们只需插入我们对参数 a-hat 的当前猜测，就可以得到 zed-hat， b-hat 和 c-hat，以及 x 和 y 的实际值。

在这种情况下，我们正在考虑的误差 e 是针对崎岖路面的， 例如。 即，不完全是平面的表面。

目前，我们忽略了 LIDAR 测量中的实际误差 自己，这也有影响。

我们可以将所有这些误差项叠加成矩阵形式 我们有一个很大的系数矩阵，称为 a。

乘以我们的参数向量 x，减去我们的堆栈测量值 b。

你可以自己计算矩阵乘法，看看我们得到 回到我们开始时使用的相同测量误差方程。

现在，我们要做的就是最小化这个误差的平方，然后 我们会有我们的解决方案。

这与我们解决的电阻问题完全相同 模块一。

我们可以从乘以平方开始得到矩阵多项式 参数向量 x。

从那里，我们取平方误差函数的偏导数 到参数向量 x 并将其设置为 0 以找到最小值。 这给了我们需要解决的线性系统以获得最终的 最小二乘估计。

我们可以使用有效的数值求解器来求解这个线性系统，例如 Python NumPy 的求解函数。 或者只是使用伪逆来获得我们对平面参数的最终答案。

这里要注意的一件重要事情是我们没有考虑 我们的 x、y、zed 测量中的传感器噪声。

我们所做的就是通过一组点找到最适合的平面。

当然可以用更复杂的方式来设置这个问题 这确实考虑了传感器噪声。

您可以使用类似于我们刚刚讨论的批处理方法，或者 您甚至可以考虑在列过滤器中包含道路参数以 当传感器数据进来时，对它们进行动态估计。

最佳解决方案 您的自动驾驶应用程序将取决于您对 LIDAR 数据的信任程度以及 您想对路面的不确定性给予多少思考。

现在，虽然我们在这里描述的所有操作都可以轻松实现 使用 NumPy 或任何其他线性代数库， 有一个很棒的开源工具叫做点云库，或 PCL， 它提供了各种有用的功能来处理点云。 事实上，它是如此有用，以至于您在工业中随处可见。 核心库是用 C++ 构建的，但是 也有非官方的 Python 绑定可用。

如果您想详细了解 PCL 的功能， 我强烈建议访问 pointclouds.org 并环顾四周。

回顾一下，我们已经看到点云是一种捕获所有 来自 LIDAR 扫描的测量结果。 它们通常存储为一个大矩阵。

我们看到了如何使用线性代数对点云进行有用的操作， 比如平移、旋转和缩放。

我们还看到了如何使用最小二乘算法 将 3D 平面拟合到点云以找到路面。

点云库或 PCL 实现了一系列有用的工具 在 C++ 中使用点云。 PCL 中最有用的算法之一称为迭代最近点 算法或ICP，这是一种常用的方法 使用两个 LIDAR 点云估计自动驾驶汽车的运动。 我们将在下一个视频中讨论 ICP 的工作原理。

激光雷达传感器模型和点云

要了解有关 LIDAR 传感器模型和点云的更多信息，请查看以下资源：

• 阅读Timothy D. Barfoot, State Estimation for Robotics (2016)（免费提供）的第 6 章第 1 节和第 2 节。

• 在http://pointclouds.org/探索点云库 (PCL) 中可用的功能。

参考

https://www.coursera.org/learn/state-estimation-localization-self-driving-cars/lecture/Huunu/lesson-2-lidar-sensor-models-and-point-clouds