Coursera自动驾驶课程第16讲：LIDAR Sensing

在第15讲《Coursera自动驾驶课程第15讲：GNSS and INS Sensing for Pose Estimation》我们学习了自动驾驶定位中常用的两种传感器：IMU（惯性测量单元） 和GNSS（全球导航卫星系统）。

本讲我们将学习自动驾驶汽车上另一种常见的传感器：LiDAR（激光雷达）。激光雷达使汽车具备自动驾驶的能力，因为它可以感知到汽车四周所有方向并且能够提供非常准确的距离信息。事实上，除了少数例外情况下，今天道路上的大多数自动驾驶汽车都配备了某种类型的激光雷达。在本讲中，我们将学习到：

LIDAR 的工作原理，理解激光雷达是如何测量距离信息的；
LIDAR 的测量模型，掌握 3D 或 2D 激光雷达的测量模型；
理解点云数据结构并掌握其三种基本转换操作（平移、旋转、缩放），最终我们将使用点云配准方法来进行汽车定位；

文章目录

1. Light Detection and Ranging Sensors
- 1.1 Overview
- 1.2 Measurement Models for 3D/2D LIDAR Sensors（重点）
- 1.3 Measurement error for LIDAR sensors
2. LIDAR Sensor Models and Point Clouds
- 2.1 Data Structures
- 2.2 Operations on Point Clouds（重点）
- 2.3 Finding the Road with 3D Plane Fitting
3. Pose Estimation from LIDAR Data （重点）
- 3.1 State Estimation via Point Set Registration
- 3.2 Iterative Closest Point (ICP) Algorithm
- 3.3 ICP-Solving for the Optimal Transformation
- 3.4 Outliers
推荐阅读

1. Light Detection and Ranging Sensors

1.1 Overview

如果你见过 Waymo 或 Uber 的自动驾驶汽车（或百度、小马智行的自动驾驶汽车），你可能已经注意到车顶上有东西在旋转。它就是激光雷达，它是用来 提供车辆周围环境的详细3D扫描。事实上，激光雷达是自动驾驶汽车和许多其它类型的移动机器人上最常用的传感器之一。激光雷达有许多不同的形状和尺寸，可以用来测量单个点、进行2D 扫描或完整的 3D 扫描。目前使用的最多的激光雷达有加利福尼亚的 Velodyne、日本的 Hokuyo 和德国的 SICK 所生产的激光雷达（国内则有速腾聚创、大疆等）。在本将中，我们以 Velodyne 激光雷达为例来进行介绍（其工作原理与其它激光雷达基本是相似的）。

在我们深入了解 LIDAR 之前，我们先快速浏览一下其历史。 LIDAR 诞生于二十世纪六十年代，激光技术发明后不久。第一位激光雷达的使用者是美国国家大气研究中心的气象学家，他们部署了激光雷达来测量云层的高度。这些云层测距仪今天仍在被使用，不仅用于测量云层，还可以用于检测火山灰和空气污染。今天，激光雷达的使用越来越广，大量用于测量和绘制地球表面，以及农业、地质、军事等用途。但第一个让激光雷达进入公众视野的应用是阿波罗 15 号，第四次载人登月任务，第一次使用激光高度计绘制月球表面。

所以，我们已经看到 LiDAR 可用于测量距离并创建某种类型的地图，但它们实际上是如何工作的，我们如何在自动驾驶汽车上使用它们？下面我们就首先来介绍激光雷达的工作原理。

如果要在一维空间中构建一个基本的 LIDAR，通常需要三个组件：一个激光器、一个光电探测器和一个非常精确的秒表。

1）激光器首先沿着一些已知的射线方向发射近红外频带内的短脉冲。与此同时，秒表开始计数。激光脉冲以光速从传感器向外行进并击中远距离目标。目标可能是我们前面的一辆车，或者像停车标志或建筑物这样的静止物体；
2）只要目标表面没有抛光或发亮，激光脉冲就会从目标表面向各个方向散射，其中一些反射光会沿着原始光线方向返回。光电探测器捕捉到返回脉冲，秒表会告诉你从脉冲第一次消失到它回来之间经过了多长时间。该时间称为往返时间；
3）现在，我们知道光速，略等于 333 亿米每秒。因此，我们可以将光速乘以往返时间来确定激光脉冲行进的总往返距离。由于光的传播速度比汽车快得多，因此可以将 LiDAR 和目标视为在所有这一切发生所需的几纳秒内看作是静止的。这意味着从 LiDAR 到目标的距离只是我们刚刚计算的往返距离的一半。这种测距技术被称为飞行时间测距。公式如下，其中 ccc 为光速， ttt 为往返时间，rrr 为激光雷达测量的距离；

r=12c⋅t(16.1)r=\frac{1}{2}c \cdot t \tag{16.1} r=21c⋅t(16.1)

虽然这不是构建 LiDAR 的唯一方法，但它是一种非常常见的方法，也可用于其他类型的测距传感器，如雷达和声纳。值得一提的是，光电探测器还会告诉返回脉冲的强度相对于发射脉冲的强度。这种强度信息不太常用于自动驾驶，但它提供了一些关于环境几何形状和光束反射材料的额外信息。

那么，为什么强度数据有用呢？在某种程度上，事实证明可以从 LiDAR 强度数据创建 2D 图像，然后使用计算机视觉相关技术来处理图像数据。由于激光雷达是它自己的光源，它实际上为自动驾驶汽车提供了一种在黑暗中看东西的方式。下图是一个原始的 LiDAR 扫描到的环境信息。中间的黑洞是传感器本身（激光雷达）所在的盲点，从那里向外扩散的同心圆是旋转的 Velodyne 传感器产生的扫描线。扫描中的每个点都根据返回信号的强度着色。 3D 扫描中的整个点集合称为点云（Point Clouds） ，我们将在后面几节学习如何使用点云进行状态估计。

1.2 Measurement Models for 3D/2D LIDAR Sensors（重点）

在我们讨论点云之前，我们首先来考虑点云中的单个点。目前，激光雷达通常使用球坐标 来表示单个点位置，测分别为：

从激光雷达中心原点到 3D 点的距离，用 rrr 表示；
从传感器 XY 平面向上或向下测量的俯仰角度，用 ϵ\epsilonϵ 表示；
以及从传感器 X 轴逆时针测量的方位角来，用 α\alphaα 表示；

这是有道理的，因为方位角和俯仰角告诉你激光脉冲的方向，而距离告诉你目标点在该方向上的位置。方位角和俯仰角是使用告诉你方向的编码器测量的，而距离是使用我们之前介绍的飞行时间测量的。对于 Velodyne 类型的激光雷达，俯仰角对于给定的扫描线是固定的。

现在，假设我们想要确定扫描点的笛卡尔 XYZ 坐标，这是我们在将多个 LiDAR 扫描组合成地图时经常要做的事。 要将球坐标转换为笛卡尔坐标，我们要用到一个逆传感器模型。之所以说这是逆模型，因为我们的实际测量值是在球坐标中给出的，而我们试图在产生它们的点处重建笛卡尔坐标。

反过来，从笛卡尔坐标到球坐标，我们可以计算出这里给出的逆变换。这是我们用于 3D LiDAR 的前向传感器模型，它给定一组笛卡尔坐标，定义了传感器实际报告的内容。

3D LiDAR 的逆传感器模型为：
[xyz]=h−1(r,α,ϵ)=[rcos⁡αcos⁡ϵrsin⁡αcos⁡ϵrsin⁡ϵ](16.2)\left[\begin{array}{l} x \\ y \\ z \end{array}\right]=\mathbf{h}^{-1}(r, \alpha, \epsilon)=\left[\begin{array}{c} r \cos \alpha \cos \epsilon \\ r \sin \alpha \cos \epsilon \\ r \sin \epsilon \end{array}\right] \tag{16.2} ⎣⎡xyz⎦⎤=h−1(r,α,ϵ)=⎣⎡rcosαcosϵrsinαcosϵrsinϵ⎦⎤(16.2)

3D LiDAR 的前向传感器模型为：
[rαϵ]=h(x,y,z)=[x2+y2+z2tan⁡−1(yx)sin⁡−1(zx2+y2+z2)](16.3)\left[\begin{array}{l} r \\ \alpha \\ \epsilon \end{array}\right]=\mathbf{h}(x, y, z)=\left[\begin{array}{c} \sqrt{x^{2}+y^{2}+z^{2}} \\ \tan ^{-1}\left(\frac{y}{x}\right) \\ \sin ^{-1}\left(\frac{z}{\sqrt{x^{2}+y^{2}+z^{2}}}\right) \end{array}\right] \tag{16.3} ⎣⎡rαϵ⎦⎤=h(x,y,z)=⎣⎢⎢⎡x2+y2+z2tan−1(xy)sin−1(x2+y2+z2z)⎦⎥⎥⎤(16.3)

现在，我们正在使用的自动驾驶汽车大部分时间都使用像 Velodyne 这样的 3D LiDAR 传感器，但有时候也会单独使用 2D LiDAR。对于二维激光雷达，我们使用完全相同的传感器模型。但是 3D 点的 zzz 方向值为零。换句话说，我们所有的测量都被限制在传感器的 XY 平面内，我们的球坐标塌陷到熟悉的 2D 极坐标。

2D LiDAR 的逆传感器模型为：
[xy0]=h−1(r,α,0)=[rcos⁡αrsin⁡α0](16.4)\left[\begin{array}{l} x \\ y \\ 0 \end{array}\right]=\mathbf{h}^{-1}(r, \alpha, 0)=\left[\begin{array}{c} r \cos \alpha \\ r \sin \alpha \\ 0 \end{array}\right] \tag{16.4} ⎣⎡xy0⎦⎤=h−1(r,α,0)=⎣⎡rcosαrsinα0⎦⎤(16.4)

2D LiDAR 的前向传感器模型为：

[rα0]=h(x,y,0)=[x2+y2tan⁡−1(yx)0](16.5)\left[\begin{array}{l} r \\ \alpha \\ 0 \end{array}\right]=\mathbf{h}(x, y, 0)=\left[\begin{array}{c} \sqrt{x^{2}+y^{2}} \\ \tan ^{-1}\left(\frac{y}{x}\right) \\ 0 \end{array}\right] \tag{16.5} ⎣⎡rα0⎦⎤=h(x,y,0)=⎣⎡x2+y2tan−1(xy)0⎦⎤(16.5)

1.3 Measurement error for LIDAR sensors

我们现在已经看到了如何在传感器测量的球坐标和我们通常对状态估计感兴趣的笛卡尔坐标之间进行转换，但是测量噪声呢？对于激光雷达传感器，有几个重要的噪声源需要考虑。

首先，反射信号到达的确切时间存在不确定性，这是因为我们用来计算飞行时间的秒表必然具有有限的分辨率；
同样，在 2D 和 3D 激光雷达中，方向的确切方向存在不确定性，因为用于测量的编码器也具有有限的分辨率
另一个重要因素是与目标表面的相互作用，它会减弱返回信号。例如，如果目标表面完全是黑色的，它可能会吸收大部分激光脉冲。或者如果它像镜子一样非常闪亮，则激光脉冲可能会完全偏离原始脉冲方向。在这两种情况下，激光雷达通常会有最大的距离测量误差，这可能意味着沿光束方向存在空白空间，或者脉冲遇到高吸收或高反射表面。换句话说，根本无法判断是否存在某物，如果自动驾驶汽车仅依靠激光雷达来检测和避开障碍物，这可能会成为安全问题。
最后，光速实际上取决于它所穿过的材料。例如，在我们的飞行时间计算中，空气的温度和湿度也会突然影响光速。

这些因素通常通过球坐标上的附加零均值高斯噪声来解释。正如我们之前所见，高斯噪声模型对于状态估计特别方便，即使它在大多数情况下并不完全准确。

含有噪声的前向传感器模型为：
[rαϵ]=h(x,y,z,v)=[x2+y2+z2tan⁡−1(yx)sin⁡−1(zx2+y2+z2)]+v,v∼N(0,R)(16.6)\left[\begin{array}{c} r \\ \alpha \\ \epsilon \end{array}\right]=\mathbf{h}(x, y, z, \mathbf{v})=\left[\begin{array}{c} \sqrt{x^{2}+y^{2}+z^{2}} \\ \tan ^{-1}\left(\frac{y}{x}\right) \\ \sin ^{-1}\left(\frac{z}{\sqrt{x^{2}+y^{2}+z^{2}}}\right) \end{array}\right]+\mathbf{v} ,\mathbf{v} \sim \mathcal{N}(\mathbf{0}, \mathbf{R}) \tag{16.6} ⎣⎡rαϵ⎦⎤=h(x,y,z,v)=⎣⎢⎢⎡x2+y2+z2tan−1(xy)sin−1(x2+y2+z2z)⎦⎥⎥⎤+v,v∼N(0,R)(16.6)

另一个非常重要的错误来源是运动失真，这是因为激光雷达所连接的车辆通常相对于它正在扫描的环境是运动的。现在，虽然汽车不太可能以光速的量级运动，但汽车相对于传感器自身旋转速度是相近的，如当扫描距离为 10−10010-10010−100 米时，扫描频率约为 5−20Hz5-20Hz5−20Hz。这意味着激光雷达扫描中的每个点都是从稍微不同的位置和稍微不同的方向获取的，这可能会导致激光雷达扫描中出现重复目标等伪影。

2. LIDAR Sensor Models and Point Clouds

2.1 Data Structures

开始之前，先回顾上一小节，我们学习了激光雷达的基本工作原理。3D LiDAR 传感器会返回其扫描的各个点的 距离，俯仰角和方位角。我们知道如何使用逆传感器模型将这些球形坐标转换为笛卡尔 X,Y,ZX,Y,ZX,Y,Z 坐标，因此我们可以使用 LiDAR 扫描的所有测量值来构建点云。

那么我们可以怎么处理这些点云呢？让我们考虑一下在现实世界中遇到点云的一个例子。如下图激光雷达传感器在路边扫描了附近的树，并产生了一个看起来像下图的点云。

我们只看到一部分面向我们的树的点，因为树和叶子反射红外光。先思考一个问题，我们应该使用哪些数据结构来处理它们？一个通用解决方案是为每个点分配索引，例如点 111 到点 nnn，并将每个点的 x,y,zx,y,zx,y,z 坐标存储为 3×13\times13×1 的列向量。这些点构成了一个矩阵 Ps\mathbf{P}_sPs，形式可以写为：

Ps=[ps(1)ps(2)⋯ps(n)]=[xs(1)xs(2)⋯xs(n)ys(1)ys(2)⋯ys(n)zs(1)zs(2)⋯zs(n)](16.7)\mathbf{P}_{s}=\left[\begin{array}{llll} \mathbf{p}_{s}^{(1)} & \mathbf{p}_{s}^{(2)} & \cdots & \mathbf{p}_{s}^{(n)} \end{array}\right]=\left[\begin{array}{cccc} x_{s}^{(1)} & x_{s}^{(2)} & \cdots & x_{s}^{(n)} \\ y_{s}^{(1)} & y_{s}^{(2)} & \cdots & y_{s}^{(n)} \\ z_{s}^{(1)} & z_{s}^{(2)} & \cdots & z_{s}^{(n)} \end{array}\right] \tag{16.7} Ps=[ps(1)ps(2)⋯ps(n)]=⎣⎢⎡xs(1)ys(1)zs(1)xs(2)ys(2)zs(2)⋯⋯⋯xs(n)ys(n)zs(n)⎦⎥⎤(16.7)

这样做可以使我们更容易地使用标准的线性代数库，如 Numpy 库，它让我们能够利用矩阵快速运算，而不是遍历列表并独立处理每个向量。

2.2 Operations on Point Clouds（重点）

下面我们来讨论常见的点云操作，关于点云有三个基本的空间操作，对于使用点云进行状态估计很重要： 平移，旋转和缩放。下面我们依次介绍这些中的每一个操作。

当我们考虑点云上的空间操作时，我们的直觉可能是在我们的参考系保持固定不变。但是对于状态估计，反过来考虑事情会更有用。世界上的物体大多保持不动，而车辆上的参考系一直在变并且从不同的角度观察世界。因此，让我们考虑如何平移我们的参考系。

如下图所示，原始坐标系原点为 SSS ，目标点为 PPP。由于车辆的运动，新坐标系原点已经变为 S′S'S′。从图中我们可以得到下面的向量关系：
rps′→=rps→−rs′s→(16.8)\underset{\rightarrow}{r^{ps'}}=\underset{\rightarrow}{r^{ps}}-\underset{\rightarrow}{r^{s's}} \tag{16.8} →rps′=→rps−→rs′s(16.8)

我们想要的是新坐标系中点 PPP 的坐标。根据公式 (16.8) 我们可以得到下面的式子，在线性代数库中我们可以很方面的处理所有点的平移操作。
ps′(j)=ps(j)−rss′sPs′=Ps−Rss′sRss′s=[rss′srss′s⋯rss′s](16.9)\begin{aligned} \mathbf{p}_{s^{\prime}}^{(j)} &=\mathbf{p}_{s}^{(j)}-\mathbf{r}_{s}^{s^{\prime} s} \quad \\ \mathbf{P}_{s^{\prime}} &=\mathbf{P}_{s}-\mathbf{R}_{s}^{s^{\prime} s} \quad \\ \mathbf{R}_{s}^{s^{\prime} s} &=\left[\begin{array}{llll} \mathbf{r}_{s}^{s^{\prime} s} & \mathbf{r}_{s}^{s^{\prime} s} & \cdots & \mathbf{r}_{s}^{s^{\prime} s} \end{array}\right] \end{aligned} \tag{16.9} ps′(j)Ps′Rss′s=ps(j)−rss′s=Ps−Rss′s=[rss′srss′s⋯rss′s](16.9)

现在，让我们考虑旋转我们的参考系而不是平移它会发生什么。再次，请记住，我们没有改变点 PPP，只是改变了我们观察的视角。我们对坐标系 rs\mathbf{r}_srs进行了旋转，得到了新的坐标系 rs′\mathbf{r}_{s'}rs′，二者之间关系为：
rs‘=Cs′srs(16.10)\mathbf{r}_{s‘}=\mathbf{C}_{s's}\mathbf{r}_s \tag{16.10} rs‘=Cs′srs(16.10)

同样地，我们也可以得到新坐标下各个点的坐标：
ps′(j)=Cs′sps(j)Ps′=Cs′sps(16.11)\begin{aligned} \mathbf{p}_{s^{\prime}}^{(j)} &=\mathbf{C}_{s's}\mathbf{p}_{s}^{(j)} \\ \mathbf{P}_{s^{\prime}} &=\mathbf{C}_{s's}\mathbf{p}_{s} \\ \end{aligned} \tag{16.11} ps′(j)Ps′=Cs′sps(j)=Cs′sps(16.11)

最后要考虑的空间操作是缩放，它的工作原理与旋转非常相似。但是，我们不是在我们的坐标系中改变基础向量的方向，而是改变它们的长度。我们对坐标系 rs\mathbf{r}_srs进行了缩放，得到了新的坐标系 rs′\mathbf{r}_{s'}rs′，二者之间关系为：
rs′=[sx000sy000sz]⏟ss′srs(16.12)\mathbf{r}_{s^{\prime}}=\underbrace{\left[\begin{array}{ccc} s_{x} & 0 & 0 \\ 0 & s_{y} & 0 \\ 0 & 0 & s_{z} \end{array}\right]}_{\mathbf{s}_{s^{\prime} s}} \mathbf{r}_{s} \tag{16.12} rs′=ss′s⎣⎡sx000sy000sz⎦⎤rs(16.12)

同样地，我们也可以得到新坐标下各个点的坐标：
ps′(j)=Ss′sps(j)Ps′=Ss′sps(16.13)\begin{aligned} \mathbf{p}_{s^{\prime}}^{(j)} &=\mathbf{S}_{s's}\mathbf{p}_{s}^{(j)} \\ \mathbf{P}_{s^{\prime}} &=\mathbf{S}_{s's}\mathbf{p}_{s} \\ \end{aligned} \tag{16.13} ps′(j)Ps′=Ss′sps(j)=Ss′sps(16.13)

通常，我们感兴趣的转换是平移和旋转的组合，有时是缩放。例如，我们经常要估计与点云最佳对齐的平移和旋转参数，以便我们可以估计我们的自动驾驶汽车的运动。对我们来说幸运的是，很容易将所有三个操作组合成一个方程，首先平移每个向量，然后旋转到新坐标系，最后应用任何缩放。当然，此操作也扩展到矩阵处理。

对于单个点，操作为：
ps′(j)=Ss′sCs′s(ps(j)−rss′s)(16.14)\mathbf{p}_{s^{\prime}}^{(j)}=\mathbf{S}_{s^{\prime} s} \mathbf{C}_{s^{\prime} s}\left(\mathbf{p}_{s}^{(j)}-\mathbf{r}_{s}^{s^{\prime} s}\right) \tag{16.14} ps′(j)=Ss′sCs′s(ps(j)−rss′s)(16.14)

对于所有的点，操作为：
Ps′=Ss′sCs′s(Ps−Rss′s)(16.15)\mathbf{P}_{s^{\prime}}=\mathbf{S}_{s^{\prime} s} \mathbf{C}_{s^{\prime} s}\left(\mathbf{P}_{s}-\mathbf{R}_{s}^{s^{\prime} s}\right) \tag{16.15} Ps′=Ss′sCs′s(Ps−Rss′s)(16.15)

2.3 Finding the Road with 3D Plane Fitting

我们已经看到了如何将基本的空间操作应用于点云。我们将在下一节中介绍如何使用这些操作来做自动驾驶汽车的状态估计。但在此之前，还有一个更重要的操作来讨论，这是平面拟合。自动驾驶汽车的平面拟合的最常见和重要的应用之一确定路面的位置并预测汽车继续行驶时的位置。回顾高中所学过的几何知识，3D平面等式为：
z=a+bx+cy(16.16)z =a+bx+ cy \tag{16.16} z=a+bx+cy(16.16)

平面方程取决于三个参数，a,b,ca,b,ca,b,c，它表示在每个方向上的平面的斜率，并且 zzz 轴与平面相交。假设我们有从 LiDAR 进行测量得到的 x,y,zx,y,zx,y,z 值，我们希望找到参数 a,b,ca,b,ca,b,c ，使我们能够通过这些点得到最佳拟合的平面。这里我们将会使用最小二乘估计法。

我们将首先定义点云中的每个点的测量错误 eee。 eee 是变量 zzz 的预测值与实际观察值之间的差异。其公式为：
ej=z^j−zj=(a^+b^xj+c^yj)−zjj=1…n(16.17)\begin{aligned} e_{j} &=\hat{z}_{j}-z_{j} =\left(\hat{a}+\hat{b} x_{j}+\hat{c} y_{j}\right)-z_{j} \quad j=1 \ldots n \end{aligned} \tag{16.17} ej=z^j−zj=(a^+b^xj+c^yj)−zjj=1…n(16.17)

对于多个点，我们可以将所有这些误差堆叠成矩阵形式：

[e1e2⋮en]⏟e=[1x1y11x2y2⋮⋮⋮1xnyn]⏟A[abc]⏟x−[z1z2⋮zn]⏟b(16.18)\underbrace{\left[\begin{array}{c} e_{1} \\ e_{2} \\ \vdots \\ e_{n} \end{array}\right]}_{\mathbf{e}}=\underbrace{\left[\begin{array}{ccc} 1 & x_{1} & y_{1} \\ 1 & x_{2} & y_{2} \\ \vdots & \vdots & \vdots \\ 1 & x_{n} & y_{n} \end{array}\right]}_{\mathbf{A}} \underbrace{\left[\begin{array}{c} a \\ b \\ c \end{array}\right]}_{\mathbf{x}}-\underbrace{\left[\begin{array}{c} z_{1} \\ z_{2} \\ \vdots \\ z_{n} \end{array}\right]}_{\mathbf{b}} \tag{16.18} e⎣⎢⎢⎢⎡e1e2⋮en⎦⎥⎥⎥⎤=A⎣⎢⎢⎢⎡11⋮1x1x2⋮xny1y2⋮yn⎦⎥⎥⎥⎤x⎣⎡abc⎦⎤−b⎣⎢⎢⎢⎡z1z2⋮zn⎦⎥⎥⎥⎤(16.18)
现在，我们所要做的就是最小化这个误差的平方，即：
x^=arg⁡min⁡xLLS(x)(16.19)\hat{\mathbf{x}} =\arg \min _{\mathbf{x}} \mathscr{L}_{\mathrm{LS}}(\mathbf{x})\tag{16.19} x^=argxminLLS(x)(16.19)
误差平方项为：
LLS(x)=eTe=(Ax−b)T(Ax−b)=xTATAx−xTATb−bTAx+bTb(16.20)\begin{aligned} \mathscr{L}_{\mathrm{LS}}(\mathbf{x}) &=\mathbf{e}^{T} \mathbf{e} \\ &=(\mathbf{A} \mathbf{x}-\mathbf{b})^{T}(\mathbf{A} \mathbf{x}-\mathbf{b}) \\ &=\mathbf{x}^{T} \mathbf{A}^{T} \mathbf{A} \mathbf{x}-\mathbf{x}^{T} \mathbf{A}^{T} \mathbf{b}-\mathbf{b}^{T} \mathbf{A} \mathbf{x}+\mathbf{b}^{T} \mathbf{b} \end{aligned}\tag{16.20} LLS(x)=eTe=(Ax−b)T(Ax−b)=xTATAx−xTATb−bTAx+bTb(16.20)
我们取平方误差函数关于参数向量 xxx 的偏导数，并将其设置为 0 以找到最小值：
∂LLS(x)∂x∣x=x^=2ATAx^−2ATb=0⟶ATAx^=ATb(16.21)\left.\frac{\partial \mathscr{L}_{\mathrm{LS}}(\mathbf{x})}{\partial \mathbf{x}}\right|_{\mathbf{x}=\hat{\mathbf{x}}}=2 \mathbf{A}^{T} \mathbf{A} \hat{\mathbf{x}}-2 \mathbf{A}^{T} \mathbf{b}=\mathbf{0} \quad \longrightarrow \quad \mathbf{A}^{T} \mathbf{A} \hat{\mathbf{x}}=\mathbf{A}^{T} \mathbf{b}\tag{16.21} ∂x∂LLS(x)∣∣∣∣x=x^=2ATAx^−2ATb=0⟶ATAx^=ATb(16.21)
最终所要求的参数为：
x^=(ATA)−1ATb(16.22)\hat{\mathbf{x}}=\left(\mathbf{A}^{T} \mathbf{A}\right)^{-1} \mathbf{A}^{T} \mathbf{b}\tag{16.22} x^=(ATA)−1ATb(16.22)

现虽然我们在这里描述的所有操作都可以使用 NumPy 或任何其他线性代数库轻松实现，但有一个很棒的开源工具，称为点云库或 PCL，它提供了各种有用的函数来处理点云。事实上，它非常有用，在工业中的任何地方都可以找到它。核心库是用 C++ 构建的，但也有非官方的 Python 绑定可用。如果想了解有关 PCL 功能的更多信息，可以访问 point cloud.org 。

3. Pose Estimation from LIDAR Data （重点）

3.1 State Estimation via Point Set Registration

在上一节我们已经学会了如何平移，旋转和缩放点云，现在我们来看看如何在真实的点云上使用这些操作来估计自动驾驶汽车的运动。这类问题通常也被叫做点云匹配问题，这是计算机视觉和模式识别中最重要的问题之一。

回到上一节我们看到的例子，如下图一辆自动驾驶汽车在车顶上安装有激光雷达，在 t1t_1t1 时刻得到点云 PS\mathbf{P}_SPS，在 t2t_2t2 时刻得到另一组点云 PS′\mathbf{P}_{S'}PS′。点云匹配问题是指同一物体在两个不同坐标系下的点云，如何去对齐两个点云并从而确定传感器在两帧之间的运动。

更具体地说，我们想找出两个传感器坐标系之间的最佳平移和最佳旋转关系，使得两组点云之间的距离最小化。现在，假如我们知道第二个点云 PS′\mathbf{P}_{S'}PS′ 中的每个点都对应于第一个点云 PS\mathbf{P}_SPS 中的某个点，我们就可以轻松解决这个问题。我们所要做的就是找到将每个点与其孪生点对齐的平移和旋转，即 C^s′s,r^ss′s\hat{\mathbf{C}}_{s^{\prime} s}, \hat{\mathbf{r}}_{s}^{s^{\prime} s}C^s′s,r^ss′s。在这个例子中，我们理想的旋转矩阵是单位阵，即没有旋转，理想的平移是沿着汽车的前进方向平移。但是现在有一个问题：我们并不知道哪些点是相互对应的？

3.2 Iterative Closest Point (ICP) Algorithm

现在让我们思考如何在不提前知道任何对应关系的情况下解决这个问题。解决这类问题最常用的算法称为迭代最近点算法或简称 ICP。ICP 背后的直观理解是，当我们找到最佳的平移和旋转关系（或者如果我们事先知道它们，并使用它们将一个点云转换到另一个坐标系下时），彼此对应的点应是距离最近的。

ICP 的思想是，我们对平移和旋转设定一个初始值 {Cˇs′s,rˇss′s}\left\{\check{\mathbf{C}}_{s^{\prime} s}, \check{\mathbf{r}}_{s}^{s^{\prime} s}\right\}{Cˇs′s,rˇss′s}，然后对 PS’\mathbf{P}_{S’}PS’ 中的每一个点找到 PS\mathbf{P}_SPS 中与它距离最近的点；然后求解最佳参数 {C^s′s,r^ss′s}\left\{\hat{\mathbf{C}}_{s^{\prime} s}, \hat{\mathbf{r}}_{s}^{s^{\prime} s}\right\}{C^s′s,r^ss′s}。以此类推，不停的迭代，直到最终收敛为止。

3.3 ICP-Solving for the Optimal Transformation

上一节介绍了 ICP 算法的思想，现在我们来讨论具体如何实现，即如何求解最佳转换。也许你已经猜到我们将使用我们最喜欢的工具，最小二乘法。我们的目标是找到最能对齐两个点云的旋转和平移参数。具体来说，我们希望最小化每对匹配点之间的距离平方之和，这是我们在此处定义的损失函数之一，公式为：
{C^s′s,r^ss′s}=argmin⁡{Cs′,,rsss′}LLS(Cs′s,rss′s)(16.23)\left\{\hat{\mathbf{C}}_{s^{\prime} s}, \hat{\mathbf{r}}_{s}^{s^{\prime} s}\right\}=\operatorname{argmin}\left\{\mathbf{C}_{s^{\prime},}, \mathbf{r}_{s}^{s_{s}^{\prime}}\right\} \mathscr{L}_{\mathrm{LS}}\left(\mathbf{C}_{s^{\prime} s}, \mathbf{r}_{s}^{s^{\prime} s}\right) \tag{16.23} {C^s′s,r^ss′s}=argmin{Cs′,,rsss′}LLS(Cs′s,rss′s)(16.23)

其中：
LLS(Cs′s,rss′s)=∑j=1n∥Cs′s(ps(j)−rss′s)−ps′(j)∥22(16.24)\mathscr{L}_{\mathrm{LS}}\left(\mathbf{C}_{s^{\prime} s}, \mathbf{r}_{s}^{s^{\prime} s}\right)=\sum_{j=1}^{n}\left\|\mathbf{C}_{s^{\prime} s}\left(\mathbf{p}_{s}^{(j)}-\mathbf{r}_{s}^{s^{\prime} s}\right)-\mathbf{p}_{s^{\prime}}^{(j)}\right\|_{2}^{2} \tag{16.24}LLS(Cs′s,rss′s)=j=1∑n∥∥∥Cs′s(ps(j)−rss′s)−ps′(j)∥∥∥22(16.24)

这个最小二乘问题比我们迄今为止遇到的问题要复杂一些。那是因为旋转矩阵在损失函数内部。需要注意的是，旋转矩阵和向量的性质并不一样。如果将两个向量相加，则会得到另一个向量。但如果将两个旋转矩阵相加，则结果不一定是有效的旋转矩阵。实际上，3D 旋转属于特殊正交阵即 SO3SO3SO3。在本课程中我们不会深入探讨（这个可以参考李群相关书籍或下文推荐资料）。

现在我们介绍求解最优转换的方法，总共包含四步：

首先，我们来计算每个点云的中心坐标，即所有点坐标求和取平均：

μs=1n∑j=1nps(j)μs′=1n∑j=1nps′(j)(16.25)\boldsymbol{\mu}_{s}=\frac{1}{n} \sum_{j=1}^{n} \mathbf{p}_{s}^{(j)} \quad \boldsymbol{\mu}_{s^{\prime}}=\frac{1}{n} \sum_{j=1}^{n} \mathbf{p}_{s^{\prime}}^{(j)} \tag{16.25} μs=n1j=1∑nps(j)μs′=n1j=1∑nps′(j)(16.25)

其次，我们使用一个 3×33×33×3 的来捕获两个点云的分布。 Ws′s\mathbf{W}_{s^{\prime} s}Ws′s 矩阵是我们用来估计最优旋转矩阵的量。
Ws′s=1n∑j=1n(ps(j)−μs)(ps′(j)−μs′)T(16.26)\mathbf{W}_{s^{\prime} s}=\frac{1}{n} \sum_{j=1}^{n}\left(\mathbf{p}_{s}^{(j)}-\boldsymbol{\mu}_{s}\right)\left(\mathbf{p}_{s^{\prime}}^{(j)}-\boldsymbol{\mu}_{s^{\prime}}\right)^{T} \tag{16.26} Ws′s=n1j=1∑n(ps(j)−μs)(ps′(j)−μs′)T(16.26)
第三步实际上是找到最佳旋转矩阵。这一步是最复杂的，它涉及到 Ws′s\mathbf{W}_{s^{\prime} s}Ws′s 矩阵的奇异值分解(SVD)。SVD 是一种将矩阵分解为两个矩阵 U\mathbf{U}U 和 V\mathbf{V}V 以及对角矩阵 S\mathbf{S}S 的乘积的方法，其非零项称为原始矩阵的奇异值。其中 U\mathbf{U}U 和 V\mathbf{V}V 满足 UTU=1\mathbf{U}^{T} \mathbf{U}=\mathbf{1}UTU=1、VTV=1\mathbf{V}^{T} \mathbf{V}=\mathbf{1}VTV=1。
USVT=Ws′s(16.27)\mathbf{U S V}^{T}=\mathbf{W}_{s^{\prime} s} \tag{16.27} USVT=Ws′s(16.27)
一旦完成了奇异值分解，我们就得到了旋转矩阵：
C^s′s=U[10001000det⁡Udet⁡V]VT(16.28)\hat{\mathbf{C}}_{s^{\prime} s}=\mathbf{U}\left[\begin{array}{llc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \operatorname{det} \mathbf{U} \operatorname{det} \mathbf{V} \end{array}\right] \mathbf{V}^{T} \tag{16.28} C^s′s=U⎣⎡10001000detUdetV⎦⎤VT(16.28)
现在，我们有了旋转估计，最后一步是平移估计。只需要将一个点云的中心旋转到另一个点云的参考系中，然后两个坐标向量的差即为平移向量。
r^ss′s=μs−C^s′sTμs′(16.29)\hat{\mathbf{r}}_{s}^{s^{\prime} s}=\boldsymbol{\mu}_{s}-\hat{\mathbf{C}}_{s^{\prime} s}^{T} \boldsymbol{\mu}_{s^{\prime}}\tag{16.29} r^ss′s=μs−C^s′sTμs′(16.29)

事实上，上面介绍的算法只是 ICP 的一种变体，称为点对点 ICP，它的名字来源于我们的目标函数或损失函数最小化对应点对之间的距离。另一种在城市或室内等结构化环境使用的流行变体称为点到平面 ICP。我们不是最小化点对之间的距离，而是将多个平面拟合到第一个点云中（下图红色点云），然后最小化第二个点云中的每个点与其最近的第一个点云中的平面之间的距离。

3.4 Outliers

现在，到目前为止，我们一直假设我们的激光雷达看到的物体是静止的。如果他们是运动的怎么办？一个常见的例子是，如果我们的汽车在繁忙的高速公路上行驶并扫描前面的车辆。两辆车以相同的速度行驶，而我们的自动驾驶汽车正在不停采集激光雷达数据点。我们再次思考，汽车的哪种运动最能对齐两个点云？答案是我们不需要转换两个点云。但是，当然，我们确实运动了，只是与我们前面的车辆无关。这显然是一个人为的例子。实际上，点云还包括许多静止的物体，如道路、建筑物和树木。但是在存在移动物体的情况下还继续使用 ICP 会使我们的运动估计远离真实运动。因此，我们需要小心排除或减轻违反我们对静止世界假设的异常点的影响。一种方法是将 ICP 运动估计与 GPS 和 INS 估计相结合。

处理此类异常值的一种简单的方法是选择一个不同的损失函数，该损失函数对异常值引起的误差损失比我们的平方误差损失更不敏感。具有此属性的损失函数称为鲁棒损失函数或鲁棒成本函数，有多种选择。

假设误差为：
e(j)=Cs′s(ps(j)−rss′s)−ps′(j)(16.30)\mathbf{e}^{(j)}=\mathbf{C}_{s^{\prime} s}\left(\mathbf{p}_{s}^{(j)}-\mathbf{r}_{s}^{s^{\prime} s}\right)-\mathbf{p}_{s^{\prime}}^{(j)} \tag{16.30} e(j)=Cs′s(ps(j)−rss′s)−ps′(j)(16.30)

下图是常见的四种损失函数：

L1L1L1 损失函数为：
ρ(e)=∥e∥1=∑i∣ei∣(16.31)\rho(\mathbf{e})=\|\mathbf{e}\|_{1}=\sum_{i}\left|e_{i}\right| \tag{16.31} ρ(e)=∥e∥1=i∑∣ei∣(16.31)

Cauchy 损失函数为：
ρ(e)=k22log⁡(1+1k2∥e∥22)(16.32)\rho(\mathbf{e})=\frac{k^{2}}{2} \log \left(1+\frac{1}{k^{2}}\|\mathbf{e}\|_{2}^{2}\right) \tag{16.32} ρ(e)=2k2log(1+k21∥e∥22)(16.32)

Huber 损失函数为：
ρ(e)={12∥e∥22k(∥e∥1−k2)(16.33)\rho(e)=\left\{\begin{array}{r} \frac{1}{2}\|\mathbf{e}\|_{2}^{2} \\ k\left(\|\mathbf{e}\|_{1}-\frac{k}{2}\right) \end{array}\right. \tag{16.33} ρ(e)={21∥e∥22k(∥e∥1−2k)(16.33)

后三种损失函数与平方差损失函数的区别在于：损失函数的斜率不会随着误差的增大而继续增加，而是保持不变或逐渐减小。但是也存在问题，稳健的损失函数使 ICP 问题求解变得更加困难，因为我们无法再为点云对齐步骤推导出一个好的封闭形式解决方案，这意味着我们需要在主循环中添加另一个迭代优化步骤。