Cartographer三部曲（一）：Real-Time Correlative Scan Matching

前言：本文是和Cartographer相关的三篇论文之一的《Real-Time Correlative Scan Matching》的阅读总结，主要介绍了激光slam的前端部分。

创新点：

• 以往的方法总是希望强调较小的计算量而更少的关注计算结果的质量，由于计算能力的提高这已经不那么重要了，加上文中论证误差函数往往是存在多个局部最优值的使得其他算法结果质量不高，因此本文提出的方法能够在提高计算复杂度的条件下求解出全局最优值。
• 以往只是单纯的求解最优值，而本文可以计算当前估计的概率分布为后续优化工作提供更多信息。

回顾相关工作（后面需要继续学习）：

• ICP
• ICL

算法实现

问题描述

已知：参考帧机器人的位置$x_{i-1}$、里程计信息$u$、参考帧位置地图$m$和当前位置下机器人传感器的测量$z$。

待求：当前机器人的位置分布$x_i$。

采用数学概率语言表达:$p(x_i|x_{i-1},u,m,z)$

使用概率公式简化

首先使用贝叶斯准则化简
$$p\left(x_i|x_{i-1},u,m,z\right)\propto p\left(z|x_i,m\right)p\left(x_i|x_{i-1},u\right)$$

明显可以看出，前面一部分$p\left(z|x_i,m\right)$含义在于已知参考帧地图和当前位置，得到实际的测量的概率，可以认为是后验概率。后面一部分$p\left(x_i|x_{i-1},u\right)$含义是根据上一次的位置和里程计信息对当前状态的估计，称为先验概率，通常根据机器人模型求解认为这样一个分布是高斯的。

后面一部分由于是高斯的，很容易求解，主要侧重点在前面部分求解往往是非高斯、多峰分布的。由于每个测量是没有相关性的，因此认为
$$p\left(z|x_i,m\right)=\prod _{j}p(z_j|x_j,m)$$
最终转换成为求每一个点被测量的概率。可以认为当地图中的障碍物离当前点最近的距离越小，这个点会被激光雷达测量到的概率越大，反之则越不可能得到这样的测量结果。

查找表栅格化

对于$p(z|x_i,m)$概率可以通过建立一个2D查找表来加速。首先对地图进行栅格化，参考帧的激光雷达信息会占据栅格地图中的部分点称为$m_i$。遍历这些点，假设传感器模型符合类似香蕉型的分布，计算周围的点$p_i$被测量到的概率。由于不同的$m_i$会访问到同样的$p_i$，取两者较大值即可。

方法概述

目的：
$$p(x_i|x_{i-1},u,m,z)\to p\left(z|x_i,m\right)$$
转换为计算$p(z|x_i,m)$,下面讲描述两种方法快速的计算相应的概率。

多级分辨率的实现

暴力搜索方法

机器人的状态维度为三维,($\Delta x\Delta y\Delta \theta )$,将这三维离散成离散状态。使用三重for循环遍历，最后再采用一层for循环遍历所有的激光点，最通过查找表来计算每个激光点的概率。算法效率较低。

计算2D薄片

查找过程分为：旋转激光点、平移激光点，再根据激光点的位置查找表中对应的概率。而平移过程计算是很简单的，加上一个数即可，因此将for循环中的$\theta$放在最外层，每次先计算旋转而$\Delta x\Delta y)$仅需要平移一下激光点位置便可求解。相较于暴力搜索，旋转的计算次数从三重for变成了一重，其他时间仅仅是在平移，因此大大的降低了计算时间。

多尺度的求解

1. 在低分辨率下搜索全搜索空间中的概率$p\left(z|x_i,m\right)$。
2. 找到没有被访问过的低分辨率下的最优位置记为$L_i$,如果$L_i<H_{best}$结束，否则进行第三步。
3. 在高分辨率下遍历该最优位置，计算最优的位置记为$H_i$，如果$H_i>H_{best}$则$H_{best}=H_i$，反之则回到第二步。

GPU的实现

略，和主要算法无关

计算分布协方差

寻找到一个高斯分布去描述当前的匹配得到的位置

$$\begin{array}{rl}K& =\sum _j{x}_i^{(j)}{x}_i^{(j)T}p\left(x_i^{(j)}|x_{i-1},u,m,z\right)\\ u& =\sum _j{x}_i^{(j)}p\left(x_i^{(j)}|x_{i-1},u,m,z\right)\\ s& =\sum _{j}p\left(x_i^{(j)}|x_{i-1},u,m,z\right)\\ {\Sigma }_{x_i}& =\frac{1}{s}K-\frac{1}{s^2}u{u}^T\end{array}$$
（原文就直接给出四个公式一点解释都没有，这也太敷衍了吧）

首先看到第三个公式，求得所有的概率和，若为归一化后的概率$s\equiv 1$,因此这里的概率是没有归一化的。这里可以定义归一化概率为
$$p^{{}^{\prime }}\left(x_i^{(j)}|x_{i-1},u,m,z\right)=\frac{p\left(x_i^{(j)}|x_{i-1},u,m,z\right)}{s}$$
计算均值为
$$\begin{array}{rlr}\mu & =\sum _j{x}_i^{(j)}{p}^{{}^{\prime }}\left(x_i^{(j)}|x_{i-1},u,m,z\right)& \\ & =\sum _j{x}_i^{(j)}\frac{p\left(x_i^{(j)}|x_{i-1},u,m,z\right)}{s}& =\frac{u}{s}\end{array}$$

计算协方差为：
$$\begin{array}{rlr}{\Sigma }_{x_i}& =E[x^2]-E[x{]}^2& \\ & =\sum _j{x}_i^{(j)}{x}_i^{(j)T}{p}^{{}^{\prime }}\left(x_i^{(j)}|x_{i-1},u,m,z\right)-{\mu }^2& =\frac{1}{s}K-\frac{1}{s^2}{u}^{T}u\end{array}$$