cartographer 论文

欢迎访问我的博客首页。

---

4. 局部二维 SLAM

---

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

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

4.1 扫描

---

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

$$T_{\xi }=\left(\begin{array}{cc}cos{\xi }_{\theta }& -sin{\xi }_{\theta }\\ sin{\xi }_{\theta }& cos{\xi }_{\theta }\end{array}\right)p+\left(\begin{array}{c}{\xi }_x\\ {\xi }_y\end{array}\right).$$

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

4.2 子地图

---

  子地图由雷达连续采集的若干扫描帧构建而成，以概率栅格 $M$ 的形式存在。概率栅格 $M$ 是从分辨率为 $r$ 的栅格点到一个值的映射，即 $M:r\mathbb{Z}\times r\mathbb{Z}\to [p_{min},p_{max}]$。该值代表栅格点有障碍物的概率。栅格点的像素值包含所有离该栅格点最近的雷达点。

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

  把一个扫描帧插入概率栅格前，需要计算出哪些栅格点属于 hits 集，哪些栅格点属于 misses 集：与扫描线终点重合的栅格点属于 hits 集，被扫描线穿过但又不与扫描线终点重合的栅格点属于 misses 集。hits 集中的每个栅格点都有一个概率值 $p_{hit}$，misses 集中的每个栅格点都有一个概率值 $p_{miss}$。

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

  如果一个栅格像素点再次(不是第一次)被观测到，使用下面的公式如下更新它的概率值 $p_{hit}$ 或 $p_{miss}$。

$$\mathrm{odds}(p)=\frac{p}{1-p}$$

这是一个单调递增函数，即，观测到的次数越多，它是 hit 点或 miss 点的可能就越大。

$$\begin{array}{rl}\mathrm{odds}(p)& =\frac{p}{1-p},\\ M_{\mathrm{new}}(x)& =\mathrm{clamp}(\mathrm{odd}{\mathrm{s}}^{-1}(\mathrm{odds}(M_{\mathrm{old}}(x))\cdot \mathrm{odds}(p_{hit}))).\end{array}$$

  $odds$ 是概率 $p$ 到几率（又称比值、比数）的映射。$M$ 是像素值 $x$ 到概率 $p$ 的映射。像素值是 1 到 32767 的整数，是用整数代替小数概率，便于计算。

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

5. 回环闭合

---

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

5.1 优化问题

---

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

$$\arg \underset{{\Xi }^m,{\Xi }^s}{\min }\frac{1}{2}\sum _{ij}\rho (E^2({\xi }_i^m,{\xi }_j^s;\sum _{ij},{\xi }_{ij}))\text{\hspace{1em}(SPA)}$$

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

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

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

$$\begin{array}{rl}{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}-\left(\begin{array}{c}{R}_{{\xi }_i^m}^{-1}(t_{{\xi }_i^m}-t_{{\xi }_j^s})\\ {\xi }_{i;\theta }^m-{\xi }_{j;\theta }^s\end{array}\right)\end{array}$$

5.2 搜索空间

---

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

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

$${\xi }^{\star }=\arg \underset{\xi \in \mathcal{W}}{\min }\sum _{k=1}^K{M}_{\mathrm{nearest}}(T_{\xi }{h}_k)\text{\hspace{1em}(BBS)}$$

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

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

$$\begin{array}{rl}{d}_{max}& =\underset{k=1,...,K}{\max }||h_k||,\\ {\delta }_{\theta }& =\arccos (1-\frac{{\mathrm{r}}^2}{2{\mathrm{d}}_{\max }^2}).\end{array}$$

公式的实现在文件 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。

5.3 分支定界扫描匹配

---

  对于大搜索窗口，我们使用算法 2 更高效地求解 ${\xi }^{\star }$，该算法最初用于混合整数线性规划。算法主要思想是把所有可能组织成一棵树，根结点代表所有可能的解。在 Cartographer 中，所有的可能解是 $\mathcal{W}$。子结点是对其父结点的划分。每个叶结点代表一个可行解。

6. 参考

---

1. cartographer 论文，sci-hub，2016。
2. cartographer 论文翻译，知乎专栏，2021。