第7章 3D SLAM

07 3D SLAM

7.1 多线激光雷达的工作原理

7.1.1 机械旋转式激光雷达

7.1.2 固态激光雷达

固态激光雷达的水平视野远小于机械旋转式激光雷达，通常在 120° 以内，垂直视野则和机械旋转式激光雷达相当。

7.2 多线激光雷达的扫描匹配

多线激光雷达的扫描匹配效果通常好于单线激光雷达，所以后端处理更简单。大多数多线激光雷达的SLAM系统都不使用子地图这样的概念，而是直接管理点云本身。点云配准主要以 Scan to Scan 和 Scan to Map 为主。

7.2.1 点到点 ICP

设需要配准的两个点云 S 1 = { p 1 , p 2 , . . . p m } S_1=\{\boldsymbol{p}_1, \boldsymbol{p}_2,...\boldsymbol{p}_m\} S1​={p1​,p2​,...pm​} 和 S 2 = { q 1 , q 2 , . . . q m } S_2=\{\boldsymbol{q}_1, \boldsymbol{q}_2,...\boldsymbol{q}_m\} S2​={q1​,q2​,...qm​}。若二者能够正确匹配，则对一组匹配点 ${\mathbf{p}}_i\in S_1，{\mathbf{p}}_j\in S_2$，应该满足

$${\mathbf{p}}_i=\mathbf{R}{\mathbf{q}}_j+\mathbf{t}$$

ICP 算法的总体思路如下：

① 设初始位姿估计为 ${\mathbf{R}}_0，{\mathbf{t}}_0$；

② 从初始位姿开始迭代，设第 $k$ 次迭代时位姿估计为 ${\mathbf{R}}_k，{\mathbf{t}}_k$；

③ 在位姿 ${\mathbf{R}}_k，{\mathbf{t}}_k$ 下，按最近邻方式寻找匹配点，记匹配后的点对为 $({\mathbf{p}}_i，{\mathbf{p}}_j)$；

④ 计算本次迭代结果，求出最优 ${\mathbf{R}}_{k+1}，{\mathbf{t}}_{k+1}$；

$${\mathbf{R}}_{k+1},{\mathbf{t}}_{k+1}=\arg \underset{\mathbf{R},\mathbf{t}}{\min }\Vert {\mathbf{p}}_i-(\mathbf{R}{\mathbf{q}}_j+\mathbf{t}){\Vert }_2^2$$

⑤ 判断解是否收敛（误差值是否足够小），若不收敛，则返回第三步，否则结束。

7.2.2 点到线、点到面 ICP

和前面二维点到线 ICP 类似。在对某个 ${\mathbf{q}}_i$ 进行变换后，并不是寻找一个最近邻，而是寻找一些最近邻，拟合成一条线或一个平面。

先考虑平面的情况，假设平面参数为 $(\mathbf{n},d)\in {\mathbb{R}}^4$，其中 $\mathbf{n}$ 是单位法线向量，$d$ 为截距。对于平面上某点 $\mathbf{p}$，满足

$${\mathbf{n}}^{\top }\mathbf{p}+d=0$$

而对于平面外的点，其与平面的垂直距离可表示为 ${\mathbf{n}}^{\top }\mathbf{p}+d$，于是建立误差函数

$$e_i={\mathbf{n}}^{\top }(\mathbf{R}{\mathbf{q}}_i+\mathbf{t})+d$$

（$S_1$ 中的点 ${\mathbf{q}}_i$ 变换到 $S_2$ 中，寻找最近邻点并拟合平面，得到误差函数）

误差对 $\mathbf{R}，\mathbf{t}$ 的导数为

$$\frac{\partial e_i}{\partial R}=-n^{\top }R{\mathbf{q}}_i^{\wedge },\frac{\partial e}{\partial \mathbf{t}}=\mathbf{n}$$

采用高斯-牛顿等方法不断优化迭代求出 $\mathbf{R}，\mathbf{t}$ 即可。

如果是直线，则可设直线方程（点向式）为

$$\mathbf{p}=\mathbf{d}\tau +{\mathbf{p}}_0$$

其中，$\mathbf{d}$ 是单位长度的方向矢量，${\mathbf{p}}_0$ 是直线上一点，$\tau$ 是参数。 对于不在直线上的某个点 ${\mathbf{q}}_i$，利用叉乘定义，将它到直线的垂直矢量作为误差函数，垂直矢量的长度即为垂直距离，

$${\mathbf{e}}_i=\mathbf{d}\times (\mathbf{R}{\mathbf{q}}_i+\mathbf{t}-{\mathbf{p}}_0)$$

误差对 $\mathbf{R}，\mathbf{t}$ 的导数为

$$\frac{\partial {\mathbf{e}}_i}{\partial \mathbf{R}}=-{\mathbf{d}}^{\wedge }\mathbf{R}{\mathbf{q}}_i^{\wedge },\frac{\partial {\mathbf{e}}_i}{\partial \mathbf{t}}={\mathbf{d}}^{\wedge }$$

高斯-牛顿法迭代求解。

7.2.3 NDT 方法

点到面 ICP 是对局部点进行平面拟合，点到线 ICP 是直线拟合。但是我们为什么要事先假定平面或直线呢？为什么要精确知道面和线参数呢？只需要得到这堆点云的局部统计信息（均值和方差），就可以进行匹配了。NDT的思路如下：

① 把目标点云（$S_2$）按一定分辨率分成若干体素；

② 计算每个体素中的点云高斯分布。设第 $k$ 个体素中的均值为 ${\mathbf{\mu }}_k$，方差为 ${\mathbf{\Sigma }}_k$；

③ 配准时，先计算每个点（$S_1$）落在哪个体素（$S_2$）中，然后建立该点与该体素的 ${\mathbf{\mu }}_k，{\mathbf{\Sigma }}_k$ 构成的残差；

④ 用高斯-牛顿法或 L-M 法对估计位姿进行迭代，然后更新位姿。

关键在于第③步，记被配准点云中的某点 ${\mathbf{q}}_i$，经过 $\mathbf{R}、\mathbf{t}$ 变换后，落在某个体素 $i$ 中，记这个栅格中的残差为

$${\mathbf{e}}_i=\mathbf{R}{\mathbf{q}}_i+\mathbf{t}-{\mathbf{\mu }}_i$$

最后的 $\mathbf{R}、\mathbf{t}$ 由一个加权的最小二乘问题决定：

$$(\mathbf{R},\mathbf{t}{)}^{\ast }=\arg \underset{\mathbf{R},\mathbf{t}}{\min }\sum _i({\mathbf{e}}_i^{\top }{\mathbf{\Sigma }}_i^{-1}{\mathbf{e}}_i).$$

相当于最大化每个点落在所在栅格的概率分布，因此这是一个最大似然估计：

$$(\mathbf{R},\mathbf{t}{)}^{\ast }=\arg \underset{R,\mathbf{t}}{\max }\prod _{i}P(R{\mathbf{q}}_i+\mathbf{t})$$

也就是说，既然目标栅格的点云符合某个统计形状，那么在正确的位姿估计下，落在其中的点也应当符合这个分布。

一个加权最小二乘问题的高斯牛顿解法如下：

$$\sum _i({\mathbf{J}}_i^{\top }{\mathbf{\Sigma }}_i^{-1}{\mathbf{J}}_i)\Delta x=-\sum _i{\mathbf{J}}_i^{\top }{\mathbf{\Sigma }}_i^{-1}{\mathbf{e}}_i$$

雅可比矩阵为

$$\frac{\partial {\mathbf{e}}_i}{\partial \mathbf{R}}=-\mathbf{R}{\mathbf{q}}_i^{\wedge },\frac{\partial {\mathbf{e}}_i}{\partial \mathbf{t}}=\mathbf{I}$$

注：$\mathbf{\Sigma }$ 是三维点的协方差矩阵，等于

$$\mathbf{\Sigma }=\left[\begin{array}{ccc}\sigma (\mathbf{x},\mathbf{x})& \sigma (\mathbf{x},\mathbf{y})& \sigma \left(\mathbf{x},\mathbf{z}\right)\\ \sigma (\mathbf{y},\mathbf{x})& \sigma (\mathbf{y},\mathbf{y})& \sigma \left(\mathbf{y},\mathbf{z}\right)\\ \sigma \left(\mathbf{z},\mathbf{x}\right)& \sigma (\mathbf{z},\mathbf{y})& \sigma (\mathbf{z},\mathbf{z})\end{array}\right]$$

7.3 直接法激光雷达里程计

将多个点云配准、拼接在一起，形成局部地图，最终构建一个激光雷达里程计模块。最简单的配准方式是连续使用 Scan to Scan 方法，将下一帧数据与上一帧数据进行匹配。这种不需要提取特征点的激光雷达里程计被称为直接法激光雷达里程计。

7.3.1 使用 NDT 构建激光雷达里程计

7.3.2 增量式 NDT 里程计

不必把过去的点云拼接在一起形成局部地图，而是把配准好的点云更新到NDT 的每个体素内部，更新他们的高斯分布状态，再做下一步的配准。称这种思路为增量式的 NDT。这种情况下，不必重新构建 NDT 内部状态，也不必重新构建 Kd 树等数据结构，是一种相当快速的实现方式。（在第一帧基础上划分体素，然后不断更新、增加体素，故称为增量式）

两个关键：一是如何维护不断增加的体素，二是每个体素内的高斯参数应该如何改变。

• 高斯分布的增量更新

设某体素之前有 $m$ 个历史点云，他们构成的高斯分布为 ${\mathbf{\mu }}_{\mathrm{H}}，{\mathbf{\Sigma }}_{\mathrm{H}}$，然后新增 $n$ 个点，这 $n$ 个点的均值和方差为 ${\mathbf{\mu }}_{\mathrm{A}}，{\mathbf{\Sigma }}_{\mathrm{A}}$。设合并后的估计为 $\mathbf{\mu }，\mathbf{\Sigma }$。记历史点云中的点为 ${\mathbf{x}}_1,...{\mathbf{x}}_m$，新增点云为 ${\mathbf{y}}_1,...{\mathbf{y}}_m$。则有更新后的均值和协方差矩阵

$$\mathbf{\mu }=\frac{({\mathbf{x}}_1+...+{\mathbf{x}}_m)+({\mathbf{y}}_1+...+{\mathbf{y}}_n)}{m+n}=\frac{m{\mathbf{\mu }}_{\mathrm{H}}+n{\mathbf{\mu }}_{\mathrm{A}}}{m+n}$$
$$\mathbf{\Sigma }=\frac{m({\mathbf{\Sigma }}_H+({\mathbf{\mu }}_H-\mathbf{\mu })({\mathbf{\mu }}_H-\mathbf{\mu }{)}^{\top })+n({\mathbf{\Sigma }}_A+({\mathbf{\mu }}_A-\mathbf{\mu })({\mathbf{\mu }}_A-\mathbf{\mu }{)}^{\top })}{m+n}$$

• 体素的增量维护

除了体素内部的高斯分布应该随点云更新以外，由于车辆往往从某个地点出发一直前行，体素本身也应该随着车辆运动而增加。然而，如果考虑长时间使用这个里程计算法，我们应该将体素数量限制在一定范围内。例如，我们希望体素总量保持在十万个左右，那些很多之前建立的体素应该定期删除。这需要我们维护一个近期使用的缓存机制）。我们会设置一个队列模型，把最近更新的体素放到最前面。当整个队列超出预期容量时，就删除最旧的那部分体素。

7.4 特征法激光雷达里程计

7.4.1 特征的提取

先提取特征点，再 对特征点做配准 的激光雷达里程计，称为特征法激光雷达里程计。

7.4.2 基于激光雷达线束的特征提取

从激光雷达中得到的点云，除了有 $x，y，z$ 这样的位置，还有包括额外的信息：① 某个激光点来自哪一条扫描线，② 同一条线上的激光点的先后顺序关系。③ 有些驱动也会输出各点的极坐标角度、扫描时间等精确信息。

雷达是旋转扫描的，各点会先后形成一条扫描线。计算该扫描线在各点的 曲率，曲率小于某个值则认为是 平面点，否则认为是 角点。

7.4.4 特征法激光雷达里程计的实现

整个里程计的计算流程与之前的 NDT 类似，为角点和平面点分别构建两个局部地图，两者的 ICP 会放入同一个优化问题中。

需要注意的是，特征法里只是对原始点云进行简单特征提取，至于为什么要提取这些特征，为什么角点和平面点要应用不同的 ICP，仅仅是根据经验来设计的，

7.5 松耦合 LIO 系统

松耦合 LIO 系统中，IMU 和 轮速计为状态估计器提供惯性和速度方面的观测源，而雷达点云匹配结果（计算好的位姿）为系统提供位姿数据的观测源。

7.5.1 坐标系说明

由于引进了 IMU，先在有三个坐标系：世界坐标系（W）、IMU 坐标系（I）、雷达坐标系（L），记雷达到 IMU 的转换关系为 ${\mathbf{T}}_{\mathrm{IL}}$。由于 IMU 的运动模型都是在IMU坐标系下推导的，所以把点云都变换到 IMU 坐标系下，以 IMU 为中心来实现定位和建图效果。假设雷达扫描到某点 ${\mathbf{p}}_{\mathrm{L}}$，那么在 IMU 坐标系下，

$${\mathbf{p}}_{\mathrm{I}}={\mathbf{T}}_{\mathrm{IL}}{\mathbf{p}}_{\mathrm{L}}$$

7.5.2 松耦合 LIO 系统的运动与观测方程

7.5.3 松耦合 LIO 系统的数据准备

7.5.4 松耦合 LIO 系统的主要流程