4. 局部二维 SLAM

二维 SLAM 的局部算法和全局算法是分开的，但它们都优化雷达观测点的位姿 $\xi = (\xi_x, \xi_y, \xi_\theta)$ 。如果传感器位于背包这种不稳定的平台上，我们会使用惯导估计重力方向，再把水平安置的雷达的扫描结果投影到二维世界。

在局部算法中，每一段连续的扫描结果都与世界的一小块匹配。这样的小块称为子地图，匹配的方法是使用非线性优化算法使扫描结果与子地图对齐。该过程称为扫描匹配。扫描匹配会随着时间积累误差，全局算法就是用于去除积累误差的。

4.1 扫描

子地图的构造过程是，不断进行扫描与子地图的坐标帧对齐。扫描帧包含二维空间中的 $K$ 个二维点 $H = \{h_k\}, k=1,...,K$ ，它在子地图中的位姿称为 $T_\xi$ ，该位姿是把扫描帧中的点变换到子地图帧的刚体变换，定义如下

$T_\xi = \begin{pmatrix} cos\xi_\theta & -sin\xi_\theta \\ sin\xi_\theta & cos\xi_\theta \end{pmatrix} p + \begin{pmatrix} \xi_x \\ \xi_y \end{pmatrix}.$

其中，第一个大括号表示旋转矩阵 $R_\xi$ ，第二个大括号表示平移向量 $t_\xi$ 。

4.2 子地图

子地图由雷达连续采集的若干扫描帧构建而成，以概率栅格的形式存在。每个格子的像素值代表该格子中有障碍物的概率。栅格点的像素值包含所有离该栅格点最近的雷达点。
在这里插入图片描述

图 4.1 分辨率为 r 的栅格地图

一个扫描帧被插入栅格地图前，需要计算出哪些栅格点是该帧发现的 hits 点，哪些栅格点是该帧发现的 misses 点。对于每个 hit，与它最近的那个栅格点将被加入 hit 集合；对于每个 misses，与从扫描原点到扫描点这条射线相交且不在 hit 集合中的栅格点将被加入 mis 集合。
在这里插入图片描述

图 4.2 一个扫描帧及其对应的 hits(有叉号的阴影格子) 和 misses(无叉号的阴影格子)

之前没有被扫描过的栅格点出现在这两个集合中，它将被赋予一个概率值 $p_{hit}$ 或 $p_{miss}$ 。之前已经被扫描过的栅格点出现在这两个集合中，其概率值按下面的公式更新

$\begin{aligned} {\rm odds}(p) &= \frac{p}{1-p}, \\ M_{\rm new}(x) &= {\rm clamp}( {\rm odds^{-1}} ( {\rm odds}(M_{\rm old}(x)) \cdot {\rm odds}(p_{hit}) ) ). \end{aligned}$

第一个公式实现在文件 cartographer/mapping/probability_values.h 的函数 Odds。第二个公式的实现在文件 cartographer/mapping/probability_values.cc 的函数 ComputeLookupTableToApplyCorrespondenceCostOdds。

5. 回环闭合

每个扫描只与包含最近几个扫描的子地图匹配，所以上面的 realtime CSM 匹配算法会积累误差。cartographer 使用稀疏位姿调整算法(Sparse Pose Adjustmen) 优化所有扫描和子地图的位姿。当一个扫描被插入子地图时，它的相对位姿会被保存以用于回环闭合。此外，如果一个子地图不再改变，它和与它匹配的扫描都会被用于回环闭合。运行与后台的扫描匹配算法一旦发现一个满足条件的匹配，对应的相对位姿就会被添加到优化问题中。

5.1 优化问题

回环闭合和扫描匹配一样是个非线性优化问题，每隔几秒，就会使用 Ceres 求解

$\arg \mathop {\min} \limits_{\Xi^m, \Xi^s} \frac{1}{2} \sum_{ij} \rho (E^2 (\xi_i^m, \xi_j^s; \sum_{ij}, \xi_{ij}) ) \tag{SPA}$

$\Xi^m = \{\xi_i^m\}, i=1,...,m$ 是 $m$ 个子地图在世界坐标系中的位姿， $\Xi^s = \{\xi_j^s\}, j=1,...,s$ 是 $s$ 个扫描在世界坐标系中的位姿，这些位姿是需要被优化的对象。

每个扫描 $j$ 都有一个与之匹配的子地图 $i$ ，优化过程中会使用它们的相对位姿 $\xi_{ij}$ 。公式中的 $\sum_{ij}$ 是一个协方差矩阵，可以按参考文献 15 计算，或按照公式 (CS) 计算Ceres 的协方差估计特征。

$\rho$ 是损失函数，可以使用 Huber 函数，用于降低外点的影响。残差 $E$ 的计算公式如下：

$\begin{aligned} E^2(\xi_i^m, \xi_j^s; \sum_{ij}, \xi_{ij}) &= e(\xi_i^m, \xi_j^s; \xi_{ij})^T \sum_{ij}{}^{-1} e(\xi_i^m, \xi_j^s; \xi_{ij}) \\ e(\xi_i^m, \xi_j^s; \xi_{ij}) &= \xi_{ij} - \begin{pmatrix} R_{\xi_i^m}^{-1}(t_{\xi_i^m} - t_{\xi_j^s}) \\ \xi_{i;\theta}^m - \xi_{j;\theta}^s \end{pmatrix} \end{aligned}$

5.2 搜索空间

前端实时 CSN 算法和后端快速 CSM 算法，通过扰动旋转和扰动平移产生候选空间。

对每个搜索空间打分非常耗时，因此只有前端实时 CSM 算法会遍历所有搜索空间。后端回环检测的快速 CSM 算法需要与所有子地图匹配，因此使用分支定界算法实现更高效的匹配。

$\xi^\star = \arg \mathop {\min} \limits_{\xi \in \mathcal W} \sum_{k=1}^{K} M_{\rm nearest} (T_\xi h_k) \tag{BBS}$

$\mathcal W$ 是搜索窗口， $M_{\rm nearest}$ 是 $M$ 经过四舍五入得到的网格点。

旋转步长 $\delta_\theta$ 是每次旋转的角度，确定旋转步长的原则是：以激光发射点为中心旋转一个步长时，最远的那个扫描点刚好旋转一个分辨率 $r$ 。

$\begin{aligned} d_{max} &= \max\limits_{k=1,...,K}||h_k||, \\ \delta_\theta &= \rm{arccos}(1 - \frac{r^2}{2d_{max}^2}). \end{aligned}$

公式的实现在文件 correlative_scan_matcher_2d.cc 中的函数 SearchParameters::SearchParameters。

假设水平搜索范围、竖直搜索范围和旋转范围分别是 $W_x$ , $W_y$ , $\theta$ ，则搜索空间的数量等于下面三者的乘积。

$w_x = \lceil \frac{W_x}{r} \rceil, w_y = \lceil \frac{W_y}{r} \rceil, w_\theta = \lceil \frac{W_\theta}{\delta_\theta} \rceil.$

对应的算法如算法 1，这是一个三重循环。

在这里插入图片描述

前端实时 CSM 算法划分搜索空间的函数是 RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates，配置平移搜索范围和旋转搜索范围的参数在文件 cartographer/configuration_files/trajectory_builder_2d.lua。后端快速 CSM 算法划分搜索空间的函数是 FastCorrelativeScanMatcher2D::GenerateLowestResolutionCandidates，配置平移搜索范围和旋转搜索范围的参数在文件cartographer/configuration_files/pose_graph.lua。