地图表示

15.2 节的 SLAM 解决了“我在哪里”，并顺手留下一份地图。但“地图”这个词掩盖了一个设计决定：要存什么。同一栋楼，扫地机器人存一张 5 cm 分辨率的占据栅格，自动驾驶车存车道线和交通灯的位置，视觉 SLAM 存几十万个带描述子的三维点，而一个听得懂“去厨房拿杯子”的机器人还要知道哪片区域叫厨房、杯子在哪个柜子上。

原因是地图有不同的读者，每个读者问不同的问题。定位问“从这里看出去，传感器会看到什么”，它需要独特、可重复识别的东西（角点、柱子、特征点）。运动规划问“机器人的身体能不能放在这里、离障碍多远”，它需要空闲空间与净空（14.1 节的 A* 在栅格上搜索，14.3 节的 CHOMP 要距离场的梯度）。任务规划问“东西在哪、房间怎么连”，它需要物体和地点。没有一种表示能把三个问题都答得又快又省，所以真实系统总是同时维护几种地图，各自为一个读者服务。

12.2 节已经介绍了体素、OctoMap、TSDF，以及对数几率形式的占据栅格（那里的 FIG 2），本节不再重复，而是往前走：激光雷达的反向传感器模型怎样定、最大量程读数怎么办、位姿不准时地图会变成什么样；导航栈在占据栅格之上又搭了什么（代价地图、距离场、拓扑图）；稀疏的路标地图与语义地图各自擅长什么；以及世界在变的时候，地图怎样“改口”和“遗忘”。

下面是一套 12 m × 8 m 的公寓：左边客厅（一张桌子、一根柱子），右下办公室（书架、角落里一张深色沙发），右上厨房（靠墙的橱柜），两扇 1 m 宽的门。同一个世界画成六种地图。拖动琥珀色探针，读数会同时列出六张地图对“这里”分别能回答什么，以及每张地图占多少内存。

12.2 节已经说明了占据栅格的核心：每格存“被占据”的对数几率 \(\ell\)，每次测量往上加一个数（4.2 节的“证据相加”）。真正动手时，你得决定两件那里没细讲的事。第一是反向传感器模型（inverse sensor model）：一束测得距离 \(z\) 的激光，对它经过的每个格子到底意味着什么？最常用的写法（Thrun 等《Probabilistic Robotics》第 9 章）只有一个参数：障碍的“厚度” \(\hl{alpha}{\alpha}\)。格心到传感器的距离 \(r\) 小于 \(z-\alpha/2\) 的格子加 \(\hl{lfree}{\ell_{\rm free}}\)（激光穿过了它，多半是空的）；\(|r-z|\le\alpha/2\) 的格子加 \(\hl{locc}{\ell_{\rm occ}}\)（激光停在这里）；更远的格子什么也不加（激光没到过那里）。第二是没有回波的射线：可能前方 5 m 内真的什么都没有，也可能打在了深色或透明的表面上没反射回来。你可以“忽略它”，也可以“把整条射线到最大量程都当作空闲”。

还有一件事是地图之外的：画地图用的是估计的位姿。下面的机器人沿一条 57 m 的环线走过三个房间，每 0.2 s 扫描一次（360°、180 束、量程 5 m）。“里程计航向漂移”给估计的航向每走一米加一点偏差，地图就按这个有偏差的位姿去画，正如没有回环校正的里程计（15.1、15.2 节）。

占据栅格说的是“这一格里有没有东西”，可规划器关心的是“我的机器人放在这一格中心会不会撞”。对一个半径为 \(\rho\) 的圆形机器人，答案是：只要离最近障碍的距离小于 \(\rho\) 就会撞。这正是 7.2 节的构型空间障碍：把障碍向外“膨胀” \(\rho\)，机器人就能当成一个点来规划。ROS 的 costmap_2d（以及 ROS 2 的 Nav2）把这件事做成了一个分层结构（Lu 等 2014 的 layered costmap）：静态层来自 SLAM 建好的地图，障碍层来自实时传感器（这里是一辆可以拖动的推车），膨胀层按到最近障碍的距离 \(d\) 给每格一个 0 到 254 的代价，各层逐格取最大。代价的形状只有三个参数：内切半径 \(\hl{rins}{r_{\rm ins}}\)（以内一律 253，“必撞”）、膨胀半径 \(r_{\rm inf}\)（以外为 0）、以及中间指数衰减的速度 \(\hl{kscale}{k}\)。

规划器（这里是 14.1 节的 A*，8 连通）把代价 ≥ 253 的格子当成墙，其他代价为 \(c\) 的格子步长乘以 \(1+3c/252\)：离墙近的格子能走，但贵。所以 \(r_{\rm ins}\) 决定“能不能过”，\(r_{\rm inf}\) 和 \(k\) 决定“愿意离墙多远”。

膨胀层的核心是“每格到最近障碍格的距离”，也就是 距离变换。12.2 节讲了 SDF 是什么、它的梯度为什么能把轨迹推离障碍（CHOMP、TrajOpt 正是这样用它的，见 14.3 节）；这里关心的是从一张栅格怎样算出它。最直接的办法叫“野火”（brushfire）：从所有障碍格同时出发做一次 Dijkstra（14.1 节），像火从障碍向外烧。问题在于步长：只走上下左右（4 连通），量出来的是曼哈顿距离；8 个方向都算 1 步，量出来的是切比雪夫距离；斜步算 \(\sqrt2\) 的“倒角”（chamfer）距离好得多，但仍不精确。精确的欧氏距离变换（Felzenszwalb 与 Huttenlocher）则把二维问题拆成“先每列、再每行”的一维问题，每个一维问题用抛物线下包络在线性时间里解完。

下图左边是一个小栅格（每格看作 10 cm），点击格子可以添加或删除障碍。选一种算法，看等距带（交替的青色条纹）的形状：一个孤立障碍点周围，曼哈顿是菱形，切比雪夫是正方形，倒角是八边形，只有精确 EDT 是圆。

距离场还藏着一个结构：它的“山脊”，即到两个（或更多）障碍等距的点，连成一张网，叫广义 Voronoi 图（GVD，12.2 节称之为中轴）。沿着它走的机器人离两侧障碍一样远，是最安全的路线；它把二维的空闲空间压缩成一维的骨架。Thrun（1998）进一步观察到：骨架上净空出现局部极小的地方，就是门、走廊口这样的“瓶颈”。把每个这样的关键点（\(\hl{crit}{\mathbf c}\)）和它两侧最近的障碍点（“基点”）连起来，就是横穿瓶颈的关键线；沿关键线把空闲空间切开，剩下的每一块就是一个“地点”（房间）。地点做节点、关键线做连接它们的边，就得到一张拓扑地图：图很小（这里 3 个地点、2 扇门），规划在上面几乎不花时间，而且“经过办公室去厨房”这样的结果能直接说给人听。

下图把这条流水线完整跑了一遍（按 Thrun 的思路做了简化，细节见第 5 节）：距离场 → 骨架 → 净空小于 0.7 m 的窄段里的局部极小 → 关键线 → 区域 → 图。拖动客厅里的桌子、改变客厅门的宽度，看拓扑怎样变。

到目前为止的地图都是稠密的：每一格都有值，不管那里有没有东西。另一类地图只存少数有用的东西。路标地图（landmark / feature map）只存可重复识别的点：EKF-SLAM 与 FastSLAM 估计的就是路标的坐标（15.2 节），视觉 SLAM（如 ORB-SLAM）存的是三维地图点，每个点带一个 32 字节的 ORB 描述子。它极其省内存、天生适合定位（用 11.4 节的 PnP 或 12.1 节的配准），但它不知道两个路标之间是不是空的，所以不能直接拿来规划。语义地图再往上一层，存的是“东西”和“地方”：物体的类别、位置和大小，房间的名字和连通关系。三维场景图（3D scene graph）把它们组织成层级：建筑 → 楼层 → 房间 → 地点 → 物体，可以回答“厨房里有没有杯子”这样的问题，也是语言驱动的机器人（Part VII）读取世界的接口。

12.2 节推导过：在各格独立、测量条件独立的假设下，格子 \(i\) 的对数几率按 \(\ell_{t,i}=\ell_{t-1,i}+\operatorname{logit}p(m_i\mid z_t,x_t)-\ell_0\) 更新（\(\ell_0=0\)，即先验 \(p=0.5\)）。中间那一项 \(p(m_i\mid z_t,x_t)\) 叫反向传感器模型，因为它把因果倒过来写：传感器模型本来是“世界 → 测量”的 \(p(z\mid m,x)\)，这里要的却是“测量 → 这一格”。FIG 2 用的是最简单的分段形式（\(r_i\) 是格 \(i\) 的格心到传感器的距离，\(z\) 是这一束的读数；没有回波的射线在“忽略”策略下对所有格子都是 0）：

\(\alpha\) 怎么选。占据带的宽度 \(\alpha\) 要盖住测距噪声（几个 \(\sigma\)）和栅格的离散化（终点所在格的格心离真实终点最远可达半个对角线 \(h/\sqrt2\)），所以 \(\alpha\) 通常取一到两个格宽。取得太大，表面前方（朝传感器一侧）\(\alpha/2\) 内本来空闲的格子每次都被加 \(\ell_{\rm occ}\)，墙就朝机器人“长胖”：这是 FIG 2 里 \(\alpha=0.4\) m 时约 400 个误占据格的来源。表面后方没有这个问题，因为射线停在表面上，不会再往后加。

独立性假设的代价。每一束都被当成对每一格的独立证据，但同一次扫描里相邻的几束其实经过同一批格子、受同一个位姿误差影响；把它们当独立证据相加会让地图过度自信（4.2 节的“同一票数了两遍”）。Thrun（2003）用前向模型 \(p(z\mid m,x)\) 对整张地图做优化，能消除这种矛盾，但代价高得多；实际系统用钳位 \(\ell\in[\ell_{\min},\ell_{\max}]\) 来给自信设上限。

位姿误差变成地图误差。一个航向误差 \(\delta\theta\) 会把距离 \(r\) 处的终点横向移动约 \(r\,\delta\theta\)。FIG 2 的漂移是每米 \(\kappa\)，走完长 \(L\) 的环线后航向误差为 \(\kappa L\)：\(\kappa=0.05\) °/m 时 \(57\times0.05\approx2.9^\circ\)，即 0.050 弧度，5 m 外的墙就偏了 \(5\times0.050\approx0.25\) m，两格半。同一面墙被两个不同的位姿各画一遍，就是“双墙”。这就是建图离不开 15.2 节的回环与位姿图：先把位姿估对，再画地图（或在回环后用新位姿把地图重画一遍）。

costmap_2d 的膨胀层用的就是下面这个函数（\(d\) 是格心到最近致命格的距离；它把结果截断成整数，FIG 3 四舍五入，最多差 1）。只要中心距离 \(d\le r_{\rm ins}\)，圆形机器人就一定撞；\(r_{\rm ins}\) 外代价从 252 开始按 \(e^{-k(d-r_{\rm ins})}\) 衰减：\(k\) 大，代价只在墙边一窄条里有，路径就贴墙；\(k\) 小，代价铺得更开，路径就被推向房间中间。

（254 是致命格，253 是内切区；\(s\) 是 A* 走进代价为 \(c\) 的格子的步长代价，\(\ell_{\rm step}\) 是 1 格或 \(\sqrt2\) 格，\(c\ge253\) 的格子禁止进入；\(\lfloor\cdot\rceil\) 是四舍五入（FIG 3 的做法），\(w=3\) 是 FIG 3 里代价的权重。）注意 \(r_{\rm ins}\) 是机器人轮廓的内切圆半径。非圆形机器人（比如长方形底盘）还有一个外接圆半径：中心距离在两者之间时，撞不撞取决于朝向，代价地图只能把它标成“可能撞”（这里的 252 及以下），真正的碰撞检查要交给知道轮廓的局部控制器。

野火在无障碍的平面上走出的是格点路径，所以在方向 \((a,b)\)（\(a\ge b\ge0\)，单位为格）上它量到的是：4 连通 \(a+b\)，8 连通单位步长 \(a\)，倒角 \(a+(\sqrt2-1)b\)；真值是 \(\sqrt{a^2+b^2}\)。令 \(t=b/a\in[0,1]\)，倒角与真值之比为 \(f(t)=\big(1+(\sqrt2-1)t\big)/\sqrt{1+t^2}\)，在 \(t=\sqrt2-1\)（约 22.5°）处取最大值 \(\sqrt{4-2\sqrt2}\approx1.0824\)。于是 4 连通、8 连通（单位步长）和倒角三种野火的相对误差 \(e_4,e_8,e_{\rm ch}\) 的范围分别是：

精确算法（Felzenszwalb & Huttenlocher 2012）利用平方距离可分离：\(\hl{dt}{D}(x,y)^2=\min_{(x',y')\in\mathcal O}\big[(x-x')^2+(y-y')^2\big]\)。先对每一列算 \(g(x,y)=\min_{y'}(y-y')^2\)（只看这一列的障碍），再对每一行算

每个 \(x'\) 贡献一条开口向上、顶点在 \((x',g(x',y))\) 的抛物线，答案是这些抛物线的下包络。抛物线两两只交一次，所以从左到右扫一遍、用一个栈维护“当前还在包络上的抛物线”，就能在 \(O(n)\) 内求出整行的下包络，整张图 \(O(N)\)。FIG 4 与本页所有其他距离场（FIG 1、3、5）都用这个算法，结果和逐点暴力搜索一致到浮点精度；算法同时记下每格最近的那个障碍格（“特征变换”），FIG 5 的骨架就是用它找出来的：相邻两格的最近障碍突然相隔很远，说明它们跨过了一道山脊。

GVD 是自由空间中“最近障碍点不唯一”的点集。在 FIG 5 中，一个格子属于骨架的条件是：它的净空 \(D\ge0.3\) m，且某个 4 邻居的最近障碍格与它的相距超过两格；然后用 Zhang–Suen 细化把它削成一格宽。沿着骨架上一段净空小于 \(c_{\max}=0.7\) m 的窄段（它的两端都要接到宽阔处，否则只是伸进墙角的“毛刺”），净空序列的局部极小（要求两侧至少回升 4 cm）就是关键点 \(\hl{crit}{\mathbf c}\)；过 \(\mathbf c\)、垂直于骨架方向一直延伸到两侧障碍的线段是关键线。这是对 Thrun（1998）的简化：他的关键点是 Voronoi 图上净空的局部极小，关键线是把关键点和它的两个基点连起来；在门口这样的直瓶颈里，两种画法几乎重合。\(c_{\max}\)、4 cm 的回升要求、“毛刺”规则和 40 格的区域下限都是本页为了让分割稳定而自己加的阈值。所以在这里，“是不是门”取决于 \(c_{\max}\)：门宽超过 \(2c_{\max}=1.4\) m，它在算法眼里就只是房间里的一段宽敞地带，这就是 FIG 5 第一个任务的答案。

一个格子被观测为占据很多次之后，\(\ell\) 可能非常大；东西挪走以后，要清掉它需要 \(\ell/|\ell_{\rm free}|\) 次“穿过”。钳位把这个次数封顶：从 \(\ell_{\max}\) 降到判为占据的门槛 \(\ell_{\rm thr}\) 以下需要

钳位只对还能再看到的地方有效：要有射线重新穿过那一格。看不到的地方（传感器的盲区、被挡住的区域、量程以外）既不会被清掉，也不会被确认。衰减（每一步把 \(\ell\) 乘以 \(2^{-\Delta t/\tau}\)，向“未知”回落）不依赖新的观测，所以能清掉看不到的鬼影，代价是同样会忘掉看不到的真墙：半衰期为 \(\tau\) 时，一个被钳在 \(\ell_{\max}\) 的格子在约 \(3.13\tau\) 后就不再被当作占据。FIG 6 把三种策略并排放在一起。

占据栅格假设世界是静止的，可真实世界里人会走过、椅子会被挪走。下图里机器人停在一个房间的左侧，开机时原地转一圈，把四周（包括身后的墙）都看了一遍，之后用 270° 激光雷达（身后 90° 是盲区）每 0.1 s 扫描一次。第 2 到 15 s 有人走进来、站了 5 s、再从右边的门出去；第 20 s 椅子被搬走。三张地图用同一串扫描并行更新：不钳位、钳位 \([-2,3.5]\)、钳位 + 衰减（半衰期 \(\tau\)）。这里没有回波的射线按“空闲到最大量程”处理，否则从门口出去的射线永远清不掉门边的格子。