Past, Present, and Future of Simultaneous Localization And Mapping 论文阅读（二）

ANATOMY OF A MODERN SLAM SYSTEM

SLAM SYSTEM 包含

• 前端：提取传感器数据
• 后端： 通过前端提取的数据，执行判断。

SLAM 的后端目前被认为是一个MAP(最大后验概率）问题，并且通常以因子图表示。假设我们评估未知变量 $X$ ,典型的该变量代表了机器人的轨迹（离散的位姿）和环境的路标点，我们给了测量变量 $Z=\{ Z_k = 1...m \}$ ,则问题可以表达为
$Z_k = h_k(X_k) +\epsilon \qquad X_k \in X \tag1$
在MAP评估中我们通过计算 $X^*$，即 $p(X|Z)$的最大后验，取值评估 $X$.依据贝叶斯理论得此等式，相当于给定测量值，我们估计最有可能的位姿。式中p(X)是先验，因无先验将其置为常量。
$X^* = \underset{X}{\mathrm{argmax}}\,p(X|Z)=\underset{X}{\mathrm{argmax}}\,p(Z|X)p(X)\tag2$
在这种情况下MAP问题变成了最大似然估计问题。假设评估 $Z$之间彼此独立，噪声项的影响是不相关的则
$X^*=\underset{X}{\mathrm{argmax}}\,p(X)\prod_{k=1}^{m}p(Z_k|X)= p(x)\prod_{k=1}^{m}p(Z_k|X_k)\tag3$
我们注意到通过观察右面的式子我们发现 $Z_k$只与 $X_k$的子集有关。式3可以用因子图表示，用因子图有两个优点，一是直观，二是复杂问题更容易表达。为了去用更明确的表示，假设噪声 $\epsilon_k$为零均值高斯噪声协方差为 $\Omega_k$， $||e||_\Omega^2 = e^T\Omega\, e$,，则（3）式变为
$p(Z_k|X_k)\propto\,exp(-\frac12\,||h_k(X_k) - Z_k||^{2}_{\Omega_k})\tag4$
先验可以表示为
$p(X)\propto exp(-\frac12||h_0(X) - Z_0||^2_{\Omega_0}\tag5$
因为最大后验概率等价于其最小付对数，所以MAP最终变为
$X^* = \underset{X}{\mathrm{argmin}}\,-log ( p(X)\prod_{k=1}^{m}p(Z_k|X_k))=\underset{X} {\mathrm{argmin}}\sum_{k=0}^m||h_k(X_k)-Z_k||^2_{\Omega_k}\tag6$
因此变成了最小二乘问题，更多情况下 $h_k(.)$ 是个非线性函数，在公式（6）中我们假设为噪声为正态分布，其他的分布将会导致不同的还加函数，如是拉普拉斯分布将是 $l_1 norm$,为了增加鲁棒性，通常使用 $l_2 norm$.(Huber or Tukey loss).

CV专家可能会注意到，公式(6)和SFM中BA有点像，的确两者都源于最大后验公式。然而有两个关键点是的SLAM独特。公式（6）不像BA被投影几何限制，而实包含了许多的传感器模型，例如惯性、轮式等。例如在雷达建图通常是限制相对位姿对于不同视点的相应，而对于直接法视觉SLAM则是对同一位置不同视角的像素强度差异进行约束。不同于BA的第二点是（6）是增量式求解(solved incrementally)的,对于机器人每个时间步的移动的测量都是可用的。

公式（6） 的最小化通常是连续线性化解决，如高斯牛顿(Gauss Newton)、列文伯格-马夸尔特(Levenberg-Marquardt)（更现代的方法是通过凸优化或拉格朗日对偶求解）高斯牛顿法通过给定初始值 $\hat{\chi}$处理迭代问题，每次迭代GN近似最小化公式（6）为
$\delta_\chi^* = \underset{\delta_\chi}{\mathrm{\arg\min}}\,\sum_{k = 0}^m\, || A_k \delta_\chi -b_k ||_{\Omega_k}^2 = \underset{\delta_\chi}{\mathrm{\arg\min}}\,|| A \delta_\chi -b ||_{\Omega}^2 \tag7$
这里 $\delta_\chi$是在线性化点 $\hat{\chi}$一个修正, 这里通过不断的改变 $\delta$, 直到找到使误差最小的 $\delta^*$, $A_k = \frac {\partial h_k(\chi)}{\partial \chi}$ 是评估函数 $h(.)$对于 $\chi$ 的雅可比,其中 $b_k = z_k - h_k(\hat{\chi_k})$是在 $\hat{\chi}$的残差。
优化矫正 $\delta_\chi^*$ 的最小化可以通过公式（7）的近似形式计算
$\delta_\chi^* = (A^T\Omega A)^{-1}A^T\Omega b\tag8$
在每次迭代线性化点通过 $\hat{\chi}\leftarrow\hat{\chi} + \delta_\chi^*$进行更新。这里（ $A^T \Omega A$)是指代高斯的近似。
到目前为止，我们假设 $\chi$属于向量空间，主这样方便 $\hat{\chi} + \delta_\chi^*$的定义。当 $\chi$包含的变量属于光滑的流形，GN方法的结构是保持不变的。usual sum 被更合适的映射替代 例如，但是 $\hat{\chi} + \delta_\chi^*$被 $\hat{\chi}\bigoplus \delta^*_\chi$替代。 $\delta_\chi^*$被定义为流形在 $\hat{\chi}$点的正切空间。

现代SLAM求解器关键点是（8）中的雅可比矩阵A是稀疏的，使得可以用快速线性求解器 $\delta_\chi^*$,并且可以增量求解器或在线求解器。当下的SLAM库包括（GTSAM, g2o, Ceres, iSAM 和SLAM++）。都可以在很短的时间求解成千上万的变量。

SLAM公式描述到现在基本上指的是最大后验概率评估，因子图优化，图-SLAM，全平滑或SAM。一个流行的变体是位姿图优化。被评估的变量是机器人轨迹采样点，并且每个因子约束一对位姿。

MAP评估方法已经被证明更准确和有效超过原始的基于滤波的方法。但目前也有一些基于滤波的SLAM方法获得了state-of-the-art的效果。当线性化点更准确时，基于EKF的方法和优化的方法差距也会变的更小。从接下来的讨论，可以发现基于MAP的评估，通常需要经过预处理，因此，该部分所讨论的一般被认为是后端。