机器人软件与具身智能 · 专栏四：定位、建图与导航 · 第 4 篇
本专栏第 3 篇讨论了位姿与地图怎样随观测修正，本篇转向地图保存什么、能够查询什么。从八界绕过玩具、接近桌面和携物通行的不同需求出发，理解不同表示保留的空间证据。配套程序使用合成射线、平面和障碍栅格，全部在普通电脑上运行。
八界准备穿过客厅，地上有玩具，旁边有桌子，桌面下方看起来还有一段空隙。相机可以给出点云，建图系统可以给出栅格，三维重建还可以生成漂亮的表面。规划器究竟该相信哪一张地图？
先把问题问具体一点：我们想知道某个位置有没有障碍、某个表面在哪里，还是机器人离碰撞还有多远？这三个问题很接近，却需要不同的数据语义。点云里没有点，可能只是没看见；TSDF 数值为零，可能是表面，也可能是尚未初始化；栅格里的一条白色通道，也不直接等于整台机器人能通过。
地图表示的选择，要从下游需要的查询出发。八界的底盘通行、机械臂接近和持物移动可以共享观测，但未必适合共享同一种压缩后的地图。
图 1｜同一处环境可以形成不同地图表达，各自保留的信息和支持的查询不同。
1. 地图首先是一套坐标、时间和未知状态的约定
不论保存的是点、格子还是距离，最先要明确的都是参考系。本专栏第 3 篇已经说明，回环可能改变地图中的历史位姿。如果建图数据跟着关键帧修正，而缓存的导航目标没有更新，界面上看起来接近的两个点，可能已经不属于同一个地图版本。
八界公开 SDK 的点位导航请求使用地图中的 x、y 和 yaw，并提供地图相关能力。1 应用保存一个"桌边等待点"时，应同时绑定它所属的地图身份与坐标约定。只有三个浮点数，没有地图来源，无法判断机器人换图后是否还能使用。
二维栅格还需要分辨率、宽高、原点平移和原点旋转。ROS 2 的 MapMetaData 包含这些信息；OccupancyGrid 的数据按行优先排列，x 方向先增长。2 如果宽度为 W，单元 (i,j) 的数组位置为 i+jW，它与图像显示时的上下方向是两个需要分别处理的问题。
消息定义中的占据值由应用约定。不能看到 int8[] data，就认定所有生产者都用同一套概率编码。接入时要检查实际发布方如何表达空闲、占据和未知，以及代价地图是否又把这些值转换成了规划代价。
时间也不能省略。昨天固定不动的墙可以放进长期地图；刚被放到地上的玩具需要及时进入局部障碍层。如果数据结构没有保存更新来源、观测时间或衰减机制，地图可能把已经移开的物体永久留下，也可能过早清除仍然存在的障碍。
2. 世界坐标转栅格，为什么不能只除以分辨率
设地图原点平移为 o，原点朝向为 φ，分辨率为 r。世界点 p 先变换到地图网格自身的坐标系，再求所在单元：
text
p_local = R(φ)ᵀ (p_world − o)
[i, j]ᵀ = floor(p_local / r)
p_world_center = o + R(φ) ([i, j]ᵀ + 0.5) r+0.5 表示返回单元中心。若需要角点，就不能继续使用这个公式。floor 表示向负无穷取整；直接转整数会把负数向零截断，从而把边界外的点错误地塞进第一个单元。
示例代码 设置原点 (1,−2) m、旋转 90°、分辨率 0.1 m。单元 (2,3) 的局部中心为 (0.25,0.35) m，经旋转和平移得到世界点 (0.65,−1.75) m。再做逆变换，恢复的索引仍是 (2,3)。
另一个更容易暴露错误的输入是 x=−0.001 m：原点为零、分辨率 0.1 m 时，正确 x 索引为 −1，而非 0。完成转换后还要检查 0≤i<W、0≤j<H，负索引不能直接交给 Python 数组，因为它会被解释为从末尾倒着访问。
图 2｜坐标先逆旋转、再除以分辨率；格子中心与格子角点相差半个单元。负坐标必须使用 floor。
把八界的地图目标投到可视化页面上时，偏移半格通常只表现为几厘米；忽略原点朝向则会让整条路线旋转。两种问题不能都归到"定位不准"里，应该先用已知单元中心做往返转换检查。
3. 占据栅格保存的是证据，不是一张黑白照片
对某个单元 m，令占据概率为 p。用对数几率表示：
text
l = log(p / (1 − p))
p = 1 / (1 + exp(−l))在常见的条件独立近似下，新的逆传感器模型证据可以相加：
text
l_t = l_(t−1) + logit(p(m | z_t, x_t)) − logit(p_prior)减去先验项，是因为当前逆传感器模型的概率已经包含先验解释。如果先验为 0.5，这一项恰好为零，代码容易把它藏起来；换成别的先验以后仍然省略，就会重复累计先验。
本例从 0.5 出发，连续三次获得占据概率 0.7 的独立证据，每次增加 log(0.7/0.3)≈0.847298。最终对数几率约 2.541894，占据概率约 0.927027。这里的"连续三次"必须有信息来源上的解释：同一张深度图复制三遍，不能当成三次新的独立观察。
程序把对数几率截断在 [−4,4]。这样连续 100 次命中后，概率约为 0.982014，而不是无限接近 1。截断有助于控制数值并保留后续反向证据的影响，但它也是建图策略，不能把截断后的数值直接称作经过实机校准的置信度。
射线提供的证据也有层次。假设传感器位于第 0 格，真实回波落在第 6 格：第 0--5 格沿射线可视为空闲证据，第 6 格提供命中证据，第 7--11 格被遮挡，仍然未知。
图 3｜命中位置后方没有获得空闲证据。右侧曲线按本例独立证据与截断规则计算，重复帧不满足这里的独立假设。
程序用独立的 observed 掩码保存"是否观察过"。因为 p=0.5 可能来自完全没观测，也可能来自相反证据抵消，两者的业务含义并不相同。
最大量程返回还需要查传感器定义：如果它明确表示有效射线在量程内没有回波，可以更新这一段空闲空间；如果只是无效深度、饱和或异常值，就不能照此清空障碍。示例中的 hit=False 只模拟前一种明确语义，不把所有缺失测量都解释为可通行。
八界面对地面玩具时，如果相机视线被桌腿挡住，桌腿后面的区域不会因为点云稀疏而自动变成空闲。这个区别会直接影响本专栏第 5 篇中的规划器是否允许路线穿过这些区域。
4. 点云、二维投影与体素，分别丢掉了什么
点云保存了采样到的表面位置，适合几何匹配、局部结构分析和表面重建。它本身通常不完整记录传感器到每个点之间的空闲射线，所以"这一片没有点"无法区分未观测、被遮挡和确实空闲。若下游需要清除障碍，必须保留或恢复观测原点与射线语义。
把三维点投到二维地图，可以显著降低导航计算量，但必须指定高度范围。脚下薄玩具、桌边横梁和高处灯具，可能有相同的 x、y，却影响不同高度的机器人部件。
教学实验在同一平面位置放置高度为 0.05、0.8、1.4 m 的三个障碍。若只考虑 0--0.3 m 的低矮运动包络，有一个障碍落入范围；若运动包络上升到 0--1.1 m，就有两个。这些高度只是演示参数，不能作为八界的实际尺寸。它说明的是：地图投影应该对应被规划的空间包络，而不是随意删掉"看起来太高"的点。
对八界的持物移动，底盘、升降结构、机械臂和所持物体共同决定扫过的体积。如果沿用空载收臂状态的二维 footprint，底盘可以通过，也不能推出整个运动过程不会碰到桌边。
体素把三维空间离散成小立方体，每个单元可以存占据概率、代表点或距离等属性。体素是一种空间组织方式，并不自动规定里面保存的物理量。称为"体素地图"的两个系统，查询语义可能完全不同。
分辨率还有直接的内存代价。一个 10×10×3 m 的稠密体积，使用 2 cm 单元需要 3750 万个体素；改成 1 cm，需要 3 亿个，增长 8 倍。若每格只有一个 float32 字段，就要 12 亿字节；再存一个同类型权重，总计 24 亿字节，尚未计算索引、颜色和容器开销。
稀疏块、哈希结构和八叉树可以避免给大量无用空间预分配全部内存，但需要处理邻接查询、块管理和更新成本。分辨率也不能突破传感器噪声、定位误差与标定误差提供的信息上限。
5. TSDF 为什么适合融合表面，却不能直接当全局避障距离
TSDF 是截断有符号距离场。在常见深度图融合中，先沿投影方向比较测得表面与体素的位置。以本篇一维正对平面为例，深度为 z，查询位置为 x，截断距离为 μ：
text
s = z − x
d = clip(s / μ, −1, 1)
D_new = (W_old D_old + w d) / (W_old + w)
W_new = W_old + w这里 D 使用归一化距离，W 是累积权重；本例每次有效观测 w=1，并跳过表面后方超过 μ 的位置。实际三维实现还要处理相机投影、可见性、噪声权重、权重上限及更新范围。Curless 与 Levoy 的体积融合工作提供了这类加权距离融合的重要基础。3
程序将两次深度 2.0 m、2.1 m 融合，取 μ=0.2 m。在两次都有效且未进入截断饱和的表面附近，零交叉位于 2.05 m。这可以解释多次测量怎样共同估计表面，但并不意味着两次互相矛盾的观测总应取平均：动态物体、错位姿和不同表面混合，也会产生一个现实中不存在的中间表面。
最容易误用的是远离表面的值。在 x=1 m 处，本例 TSDF 已截断为 1，乘回 μ 只有 0.2 m；到融合平面的真实欧式距离却是 1.05 m。它没有承诺保存整个自由空间到最近障碍的距离。
图 4｜一维正对平面中，融合零交叉位于 2.05 m；截断后的 0.2 m 不能替代远处查询点的 1.05 m 欧式距离。未观测位置的零值也不是表面。
在一般三维场景里，还存在投影距离与最近表面欧式距离的差别：沿相机视线测到的表面，不一定就是离查询点最近的表面。本例用正对平面消除了这项差别，专门把截断机制单独算清楚。
另一个反例是 x=3 m。它在两次观测的有效更新范围之外，初始化数组中的 D 虽然仍为零，但 W 也为零。这意味着没有证据，不能据此提取一个新表面。查看 TSDF 时必须把值和有效权重放在一起解释。
6. ESDF 提供最近障碍距离，仍然要说明边界和有效范围
在自由空间中，欧式距离场可以写为：
text
d(x) = min_(y∈障碍集合) ||x − y||有符号版本还定义障碍内部的符号，符号约定必须显式说明。规划和轨迹优化可以用距离判断间隙，用梯度寻找远离障碍的方向。Voxblox 的论文讨论了从 TSDF 维护 ESDF、面向规划查询的做法。4 这不等于把 TSDF 每格乘回截断距离就完成转换。
距离场也有有效域。若某处未观测，系统需要明确采用阻挡、惩罚或允许探索的策略。把未知初始化成很大的正距离，会让规划器误以为那里拥有大量净空；把所有未知都当占据，则可能让尚未探索的通道无法被规划。选择取决于任务，但不能让数组默认值替你做这个决定。
栅格离散边界还会影响净空。本例占据的是整个 0.1 m 方格，而不是方格中心的一个数学点。如果查询点到最近占据中心的距离为 d_center，可以构造保守下界：
text
d_lower = max(0, d_center − √2 r / 2)因为占据格里的任意点离格心最多半条对角线。这个下界便于说明离散误差，却不是精确的方格边界距离；真正使用时要与碰撞模型和距离场实现一致。
教学机器人圆形半径为 0.25 m。某个查询点距障碍格心 0.3 m，直接相减似乎还剩 0.05 m；扣除半对角线后，净空下界仅约 0.229289 m，不能接受。距离 0.4 m 的点，下界约 0.329289 m，才通过本例检查。
图 5｜左侧只计算稠密 float32 距离与权重字段；右侧使用整个方格的保守距离下界。教学圆形半径不代表八界真实 footprint。
当多个障碍距离相同时，最近障碍的身份可能切换，距离函数在这些位置不一定可微。轨迹优化器还需要处理插值、梯度稳定性和采样间碰撞，不能把"有距离、有梯度"当成所有轨迹都可执行的证明。
7. 把地图选择放回八界的一次移动操作
八界从客厅一端前往桌边，长期地图用于确定房间、固定墙体和目标参考；局部观测负责反映新出现的玩具与移动物体。规划底盘路线时，需要能查询通行代价和碰撞的表达；接近桌面时，需要保留高度与表面几何；机械臂规划还要结合关节构型、工具与物体包络。
这些表示可以并存，但更新链路要能追溯。一个障碍进入局部地图时，应知道来自哪一帧、使用哪次位姿、转换到哪个参考系。回环或重新定位之后，应知道哪些缓存需要重投影，哪些短时障碍可以从新的传感器观测重建。
因此，结合八界开源仓库中实际开放的接口与配置5，接入地图接口时先验证四件事：一是坐标往返能否对齐已知点；二是空闲、占据和未知有没有明确编码；三是高度与机器人运动包络是否匹配；四是距离和代价的单位是否与规划器预期一致。地图显示漂亮，只能说明渲染链路工作，无法替代这些语义检查。
本专栏第 5 篇《路径规划入门》将使用一个明确的二维占据模型，先求栅格最短路，再把朝向和运动约束放进状态。这样每次算法升级，都能看清到底增加了哪个约束。
8. 在电脑上复算八组实验
完整程序见 22_maps.py，依赖和输出说明见 examples/README.md。在配套仓库根目录执行：
bash
python3 -m venv .venv-nav
.venv-nav/bin/python -m pip install -r "04_定位建图与导航/22_地图表示怎么选/examples/requirements.txt"
.venv-nav/bin/python "04_定位建图与导航/22_地图表示怎么选/examples/22_maps.py"程序检查八组结果：旋转网格坐标、占据证据累积、射线与未知语义、稠密体素内存、TSDF 融合、TSDF 与欧式距离差异、障碍膨胀以及高度投影。默认只打印报告，不连接机器人、不读写地图文件；--json 输出便于机器读取。
这里的 TSDF 只是一维正对平面，距离变换只对小栅格中的占据中心做穷举，目的是把语义和数值算清楚。它不包含三维重建性能优化、动态物体处理或真实 footprint 验证。