SLAM 与位姿图

把一台扫地机器人放进一套从没见过的公寓。它要规划清扫路线，就需要一张地图；它要把每一帧激光扫描拼进地图，又必须知道这一帧是在哪里扫的。4.8 节的 EKF 定位假设地图已知，12.1 节的 ICP 和 15.1 节的视觉惯性里程计只估计相邻两帧之间的相对运动。前者需要地图，后者没有地图也能工作，可是误差会一直累积：每一步的小误差叠加起来，走得越远偏得越多，而且永远不会自己变小。

SLAM（Simultaneous Localization And Mapping，同时定位与建图）的做法是把机器人的整条轨迹和地图当作同一组未知数一起估计。关键在于回环：机器人回到以前到过的地方并认出来时，就得到一条连接“现在”和“很久以前”的测量。这一条测量足以把一路积累的漂移拉回来，并把修正分摊到整条轨迹上。

现代 SLAM 几乎都写成6.9 节的因子图最小二乘：变量是一串机器人位姿，因子是里程计和回环。6.9 节的 FIG 3 已经在 24 个位姿的圆上演示过回环怎样修正漂移。本节再往前走四步：(1) 一个完整的系统，前端从真实的激光扫描里自己找回环（地点识别加 ICP 验证），后端在 \(\mathrm{SE}(2)\) 上优化位姿图；(2) 两种更早的方法 EKF-SLAM 和 FastSLAM，以及它们各自卡在哪里；(3) 错误回环和鲁棒核；(4) 增量求解：机器人每走一步，求解器到底要重算多少。

下面是一个 24 m × 14 m 的室内环境：外墙、两个大的方块障碍，以及墙边一些小箱子。机器人沿“8”字形路线走一圈，共约 77 m，每走 0.8 m 记一帧 360° 激光扫描（180 束，测距噪声 2 cm），一共 92 帧。扫描本身很准，问题全出在“把扫描放到哪里”。如果用真实位姿拼，得到的地图和真实墙壁严丝合缝；如果只能用轮式里程计算出的位姿拼，每一帧都放在一个略有偏差的位置上。

这里的里程计有两类误差：随机噪声（每一步都不同），以及航向偏差：每走一米，航向多算一个固定的小角度，比如两个轮子的半径差了一点点就会这样（9.1 节）。航向误差最致命，因为它会被距离放大：航向偏了 \(\delta\theta\)，之后每走一米就多出约 \(\delta\theta\) 米的横向误差。

几个值得试的设置：把航向偏差和随机噪声都调成 0，地图和真实一模一样；只去掉噪声、保留 0.2°/m 的偏差，终点误差仍有 1.4 m：系统误差和随机误差一样有害。偏差调成负值，地图向另一侧弯。还要注意：误差并不是单调增长的，在“8”字路线上，往一个方向转弯造成的误差，有一部分会被反方向的转弯抵消，所以中途的最大误差比终点大。

下图用的是 FIG 1 的同一个世界、同一条路线、同样的扫描，跑的是一个完整的、简化但不作弊的位姿图 SLAM。系统分两部分：

前端（从传感器数据里造出约束）：每 0.8 m 建一个关键帧，即一个位姿变量 \(X_k\)，并附上这一帧的扫描。相邻关键帧之间加一条里程计边，测量值是轮式里程计给出的相对位姿。然后做地点识别：在当前估计里，找出离新关键帧不到“搜索半径”、且至少早 20 帧的旧关键帧（最近的 2 个），把两帧扫描用 ICP 对齐。初值来自当前估计，并且再从 ±10° 的航向各试一次。对齐之后，用点到线距离检查：至少 75% 的点落在对方扫描 10 cm 以内、且这些点的 RMS 距离小于 5 cm，才接受为一条回环边，测量值就是 ICP 的结果。

后端（把所有约束一起满足）：每加入一条回环，就从当前估计出发做 Gauss-Newton（最多 10 次迭代，代价上升时加一点 LM 阻尼），把第一个位姿固定为规范（6.9 节）。新关键帧的初值，是把里程计接在上一个关键帧的当前估计后面得到的。地图不单独存储：每次都由“各关键帧的扫描放在各自的估计位姿上”重新画出来，所以位姿一被修正，地图也跟着修正。

位姿图是 2000 年代后期才成为主流的。在那之前，SLAM 主要按 4.7 节的贝叶斯滤波来做：只估计当前位姿和地图，旧位姿一到就“边缘化”掉。下面两个实验用同一个场景：18 个点状路标，机器人沿 14 m × 9 m 的椭圆以 1 m/s 逆时针走两圈（约 76 s）。传感器测量前方 ±90°、4.5 m 以内路标的距离和方位（噪声 0.1 m 和 2°），并且已知每次看到的是哪个路标（数据关联已知，和 4.8 节一样）；里程计速度噪声 0.1 m/s，角速度噪声 0.05 rad/s。

EKF-SLAM 把 4.8 节的 EKF 定位直接推广：状态不只是机器人的 \((x,y,\theta)\)，而是把所有已看到的路标坐标都拼进去，\(\mathbf y=[\x;\mathbf m_1;\dots;\mathbf m_n]\in\R^{3+2n}\)，用一个均值和一个 \((3+2n)\times(3+2n)\) 的协方差 \(P\) 表示全部信念。第一次看到一个路标时，用测量反推它的位置，把状态扩维；以后每次再看到它，就做一次普通的 EKF 更新。精髓在协方差的非对角块 \(\hl{pxm}{P_{xm}}\)：路标是从一个有误差的位姿看到的，所以它的误差和机器人的误差相关，路标之间也因此相关。正因为有这些相关，机器人重新看到很早以前的路标时，修正会顺着 \(P\) 传给所有路标，包括此刻根本看不见的那些。

保留相关性时，两圈后的路标 RMS 误差约 0.07 m，NEES 约 3：略高于理想的 2，这是 EKF-SLAM 已知的线性化不一致（见第 8 节）。代价是：每次更新都要改写整个 \(P\)，状态维数 \(N=3+2n\)，每次更新约 \(2N^2\) 次乘加，内存也是 \(N^2\)。18 个路标时 \(N=39\)，毫无压力；一张一万个路标的地图，每次更新就是 \(8\times10^8\) 次运算。这就是 EKF-SLAM 只能用于小地图的原因。

FastSLAM（Montemerlo 等，2002）注意到一件事：如果整条路径已知，各路标就彼此独立了，因为它们之间的相关全是通过机器人位姿传过来的。于是用 4.10 节的粒子滤波表示路径，每个粒子是一条路径假设 \(\hl{path}{\x^{[i]}_{1:t}}\)，并且带着 \(n\) 个各自独立的 2×2 小 EKF，每个路标一个。每一步：按运动模型给每个粒子采样一个新位姿，用它自己的地图更新它看到的路标，并按测量和它的地图吻合的程度更新权重；有效粒子数 \(N_\text{eff}\) 低于 \(M/2\) 时做系统重采样。每次更新只碰被看到的那个路标，代价约为 \(M\) 乘以每步测量数，与地图大小无关（用树形结构共享地图时是 \(M\log n\)）。

地点识别会认错：两段一样的走廊、一排相同的货架、换了季节的街道。一条错误回环说“这两个位姿在同一个地方”，在平方损失下它的拉力与偏差成正比，而且它的信息矩阵和真回环一样大，于是整张图被硬拽到一起。补救办法是 6.9 节、11.5 节讲过的鲁棒核：把回环边的代价换成增长更慢的 \(\hl{kern}{\rho}\)，用 IRLS 求解，每条边按当前的归一化误差 \(\chi=\sqrt{\mathbf e^\top\Omega\mathbf e}\) 得到一个权重 \(w(\chi)\)。下面是一个 120 个位姿的“曼哈顿世界”位姿图（机器人在 1 m 网格上随机游走，回到同一个网格点就有一定概率产生真回环，共 47 条），再随机加入若干条错误回环；所有优化都从同一个里程计初值出发。鲁棒核只加在回环边上。

再试几种组合（默认图、6 条错误回环）：Huber（尺度 3）把误差降到约 0.42 m，比平方损失好得多，但做不到参照水平：它的影响函数有界却不为零，错误回环还在用常数大小的力拉。错误回环加到 20 条时，Huber 的误差升到约 1.5 m，Cauchy 和 DCS 仍是 0.08 m。尺度太小也不行：把 DCS 的尺度降到 1，47 条真回环里有 40 条被一起关掉，误差 0.67 m；降到 0.5，47 条真回环有 46 条被关掉，结果基本停在里程计初值。原因是从里程计初值看，真回环的误差也很大（漂移有 1 m，而回环的 σ 是 5 cm，\(\chi\) 可达 20），一个太“狠”的核分不清谁真谁假。点“换一张图”可以看到，DCS 在某些图上尺度 3 也会误杀真回环，而 Cauchy 更宽容。

机器人每秒都在加新位姿。每来一个关键帧就从头解一遍整张图，代价随地图增大而增大，迟早跟不上。增量平滑（iSAM，Kaess 等 2008；iSAM2，2012）的想法是：保留上一步的分解结果，只改动受新因子影响的那一部分。用 6.9 节的语言：正规方程矩阵 \(H=LL^\top\)，Cholesky 因子 \(\hl{fill}{L}\) 的第 \(c\) 列只依赖排在它前面的变量。加入一个新因子，只会改写消元树上从这个因子最早的变量到根的那一条路径上的列，其余的列一个数也不变。下图用 FIG 2 那条“8”字路线的位姿图（按真实几何建：里程计链，加上相距不到 1 m、相隔至少 20 帧的回环，共 92 个位姿、19 条回环），逐步计算 \(L\) 的非零块（每块 3×3），并标出每一步必须重算的块。

记位姿 \(\x_{0:t}\)、地图 \(\mathbf m\)、控制（里程计）\(\mathbf u_{1:t}\)、测量 \(\mathbf z_{1:t}\)。在马尔可夫假设下（4.7 节），整个后验分解成一串小因子的乘积：

估计整个左边叫完整 SLAM（平滑），就是位姿图和 BA 做的事。只要当前状态 \(p(\x_t,\mathbf m\mid\cdot)=\int p(\x_{0:t},\mathbf m\mid\cdot)\,d\x_{0:t-1}\) 叫在线 SLAM（滤波），就是 EKF-SLAM 做的事。在高斯近似下，“积分掉”一个变量就是对信息矩阵做 Schur 补（11.6 节）：被消去的位姿的所有邻居两两连起来。每个路标都是从某个位姿上看到的，消掉一串位姿之后，所有路标都互相连上，这就是 FIG 3 那个越来越满的矩阵的来历。不做边缘化，信息矩阵保持稀疏：每个块只对应一个因子。

状态 \(\mathbf y=[\x;\mathbf m_1;\dots;\mathbf m_n]\)，协方差按块写成下式。对路标 \(j\) 的距离-方位测量 \(\mathbf z=(r,\phi)\)，记 \(\boldsymbol\delta=\mathbf m_j-\mathbf t\)，\(q=\|\boldsymbol\delta\|^2\)，预测为 \(\hat r=\sqrt q\)，\(\hat\phi=\operatorname{atan2}(\delta_y,\delta_x)-\theta\)。雅可比 \(H\) 只在机器人的 3 列和路标 \(j\) 的 2 列上非零：

虽然 \(H\) 很稀疏，\(PH^\top\) 却是一整列稠密的块：只要 \(P_{xm}\) 和 \(P_{mm}\) 非零，增益 \(K\) 在每个路标上都有分量，\(KSK^\top\) 改写 \(P\) 的全部 \((3+2n)^2\) 个元素。这就是 \(O(n^2)\) 的来源，也是回环时“看不见的路标也被修正”的来源。新路标 \(\mathbf m=\mathbf t+r\,(\cos(\theta+\phi),\sin(\theta+\phi))\) 的扩维要用 \(G_x=\partial\mathbf m/\partial\x\) 和 \(G_z=\partial\mathbf m/\partial\mathbf z\)：新的协方差块是 \(G_xP_{xx}G_x^\top+G_zRG_z^\top\)，与已有状态的交叉块是 \(G_xP_{x\,\cdot}\)。FIG 3 的“全部丢掉”就是每一步之后把 \(P_{xm}\) 和 \(P_{mm}\) 的非对角块置零。

这一步是精确的（不是近似），因为给定整条路径，各路标的测量互不相干：这叫 Rao-Blackwell 化，能解析处理的部分（路标）用高斯解析处理，只对剩下的部分（路径）采样。权重按 4.10 节的重要性采样推出：提议分布是运动模型，所以权重就是测量似然，\(H_m\) 是测量对路标的 2×2 雅可比。FastSLAM 2.0 把最新的测量也放进提议分布，粒子更集中，所需粒子数少得多。

位姿 \(X_i=(\mathbf t_i,\theta_i)\)。一条边 \((i,j)\) 带一个相对位姿测量 \(Z_{ij}=(\Delta\mathbf t,\Delta\theta)\) 和信息矩阵 \(\Omega_{ij}\)（协方差的逆）。误差是“测量的逆乘以预测”这个变换 \(E_{ij}=Z_{ij}^{-1}X_i^{-1}X_j\) 写成向量（预测与测量完全一致时 \(E_{ij}=I\)，误差为零）：

这和 6.9 节的残差几乎一样，只是平移误差再转到测量 \(Z_{ij}\) 的坐标系里：这是 g2o 的约定；GTSAM 用 \(\Log(E_{ij})\)，两者零点相同、在零附近一阶相同（6.10 节：\(V(\varphi)\approx I\)）。在二维里，旋转只是一个角度，\(\theta\leftarrow\operatorname{wrap}(\theta+\delta\theta)\) 本身就是 SO(2) 上的指数映射更新，平移在世界坐标里直接相加，所以 FIG 2 直接用 \((\mathbf t,\theta)\) 加增量；三维时必须用 6.10 节的 \(\Exp\)。令 \(\mathbf a=R_i^\top(\mathbf t_j-\mathbf t_i)\)，\(R=R(\Delta\theta)^\top R(\theta_i)^\top\)，两个 3×3 雅可比块是

上面这行是页面加载时现算的：把解析雅可比和中心差分逐元素比较。\(H\) 的块 \((i,j)\) 非零当且仅当 \(i,j\) 之间有边，所以它是“对角带 + 回环处的几块”。全图在整体平移和旋转下不变，\(H\) 有 3 维零空间，固定 \(X_0\)（规范，6.9 节）后才可逆。\(w_{ij}\) 是 IRLS 权重：里程计边取 1，回环边按下面的核函数取值。

Huber 对大误差只给 \(k/\chi\) 的权重，力的大小 \(w\chi=k\) 有界但不为零。Cauchy 和 DCS 是“回落”型：\(\chi\) 很大时权重趋于零，错误回环几乎被关掉，但代价不再是凸的，结果依赖初值。DCS（dynamic covariance scaling，Agarwal 等 2013）是从 switchable constraints（Sünderhauf 与 Protzel 2012）推出来的：后者给每条回环配一个要和位姿一起优化的“开关” \(s\in[0,1]\)，回环的信息矩阵乘以 \(s^2\)，再加一项先验 \(\Phi(1-s)^2\)（\(\Phi\) 是开关先验方差的倒数）。于是这条边的代价是 \(s^2\chi^2+\Phi(1-s)^2\)，完全关掉（\(s=0\)）时等于 \(\Phi\)。Agarwal 等分析收敛点时要求这个代价不超过 \(\Phi\)，得到 \(0\le s\le2\Phi/(\Phi+\chi^2)\)，取其中不超过 1 的最大值，就是上面的闭式 \(s\)：开关不再是要优化的变量，信息矩阵直接按 \(s^2\) 缩放。注意 \(\Phi\) 是和 \(\chi^2\) 比较的，不是和 \(\chi\)：\(\chi^2\le\Phi\) 时权重为 1。

在最优点附近，位姿的协方差约为 \(H^{-1}\)（和 11.6 节 BA 的协方差同理）。FIG 2 的椭圆用的是它的 3×3 对角块：先分解 \(H=LL^\top\)，再对每个位姿解两次三角方程，不需要求出整个 \(H^{-1}\)。增量更新也建立在这个分解上：加入一个新因子相当于 \(H\leftarrow H+J^\top\Omega J\)，其中 \(J\) 只在几个变量上非零；Cholesky 因子 \(\hl{fill}{L}\) 中改变的列，恰好是消元树上从这几个变量中最早的那个到根的路径（Davis 与 Hager 的秩更新理论）。所以按时间排序时，一条里程计边只改动最后两列，一条连回很早位姿的回环会改动从那一位姿以后的几乎所有列。

真实系统的结构和 FIG 2 一样分成前端和后端，差别在于每一块都做得更结实。前端：连续帧之间不只靠轮式里程计，而是用扫描匹配或视觉/视觉惯性里程计（12.1、15.1 节）给出相对位姿；地点识别不依赖当前估计的位置（那正是漂移之后最不可信的东西），而是比较外观或几何描述子；候选回环要经过几何验证。后端：稀疏求解器、好的消元顺序、鲁棒核，以及增量更新。SLAM 给出的是位姿和挂在位姿上的测量；把它们做成哪一种地图（占据栅格、代价地图、距离场、拓扑图……），取决于谁来读地图，这是 15.3 节的内容。

还有一个 12.1 节留下的问题：位姿图里每条边的信息矩阵 \(\Omega\) 从哪来？对 ICP 边，常用的是高斯牛顿近似 \(\Omega\approx A/\sigma^2\)（\(A=\sum J_i^\top J_i\)），但 12.1 节已经指出它只是下界，没有算上错误对应和采样间距，真实的不确定度更大；直接用它，回环边会显得过于自信。FIG 2 采用的是固定的保守值（里程计边 σ = 5 cm、0.015 rad，回环边 3 cm、0.008 rad）。退化场景（长走廊）里 \(A\) 在沿走廊方向接近奇异，这时把它当信息矩阵反而是对的：它告诉后端“这个方向我不知道”。