TSDF与ESDF有符号距离场建图原理

可视化演示

展示系统处理流水线、实时性能指标曲线及多方案对比分析，包含数据流转、吞吐延迟监控与方案评估。

有符号距离场

雕刻家每摸到一个点，就相当于给空间的某个位置贴一张便签。便签上只写两件事：离最近表面的距离数值，以及在表面内外的方向——用正负号表示。当整个工作室的地板、墙面、空气中都贴满了这样的便签，就构成了一张“有符号距离场”。它不描述“哪里有东西”，而描述“这里离东西有多远、在东西里面还是外面”，这是它和普通占用地图最大的不同。

在机器人世界里，我们不可能给连续空间贴满便签，而是先把空间切成许多小立方体，这些立方体叫做体素（Voxel），就像把工作室地板铺上方格。每个体素里放一块“小石子”，上面刻着一个数字：它到最近表面的距离。所有体素合起来，就是离散化的距离场。体素越小，方格越密，便能刻画越精细的曲面，但代价也越大——这一点我们留到“工程调优”一节细说。

形式化地写，对于空间中的任意一点 $\mathbf{x}$，设场景中所有表面点的集合为 $\mathcal{S}$，那么该点的有符号距离值是：

就像雕刻工作室的地板被铺上带刻度的方格，机器人想知道自己离墙多远，低头看一眼脚下那块石子上的数字即可；符号则告诉它：此刻是站在粘土外面，还是正身处雕塑肚子里。

TSDF原理

雕刻家很快发现，给整个工作室都贴精确便签是白费力气：离雕塑三米远的墙角，贴“3 米”和“3.1 米”毫无区别。于是他只给表面附近一个“手肘长度”（记为 μ）以内的空气贴精确标签，更远的地方统一写“很远”。这个“只在表面附近精雕细琢”的设定，就是截断距离 μ。由此得到的场，叫作 TSDF——Truncated SDF，截断有符号距离场。

TSDF 把每个体素的距离值除以 μ 归一化，并且把结果截断在 $[-1, +1]$ 之间：表面处值为 0，表面外为正（最大 +1），表面内为负（最小 −1），超出截断带的一律记为 ±1。这样做的好处是：内存只花在表面附近，而表面附近恰恰是重建最需要细节的地方。

光有截断还不够。雕刻家每次触摸同一处，由于手抖、角度不同，读数会有误差。多摸几次后，他该信哪一次？答案是做加权平均：可信度高的触摸（正对着表面、距离近）权重大，可信度低的权重小。设第 $i$ 帧在体素 $\mathbf{x}$ 处测得值为 $f_i$、可信权重为 $w_i$，那么融合公式为：

回到工作室：雕刻家每次触摸只更新近表面一层便签，同一处被摸到多次后，便签上的数字取加权平均——手再抖，误差也会在一次又一次的触摸中被“平均”掉，雕塑的轮廓在黑暗中逐渐变得坚实而清晰。

ESDF原理

雕像完工，要把它搬出狭窄的门洞。这时只有表面附近的便签就不够用了：机械臂旋转时，任何一个凸起的肩部都可能撞上门框。雕刻家需要知道全空间的准确距离——从雕像表面到每一面墙、天花板、地板，每一个位置的直线距离都要心里有数。这种“全场真实距离”的字段，就是 ESDF——Euclidean SDF，欧氏有符号距离场。

ESDF 也建立在体素网格上，但每个体素存的不是“相对表面有多近”，而是“离最近的障碍物有多远”。对自由空间里的点，这个值是正数；障碍物内部的点则为负。由于它保存了全空间的真实距离，查询任意一点到最近障碍的距离都可以在常数时间内完成——这正是路径规划最需要的“体检报告”。假设障碍物区域为 $\mathcal{O}$：

在实际导航中，我们还会把机器人自身的半径考虑进去：查询距离时直接用 $D(\mathbf{x})-r_{\mathrm{robot}}$ 作为有效安全距离，相当于把障碍“膨胀”了一圈。把 ESDF 想象成一张布满“雾”的舒适度地图，越亮的地方离障碍越远；梯度就是雾中的“风向标”，永远指向更空旷的方向。机器人顺着风向标微调身形，就能安然穿过门洞。

建图流程

一次完整的建图，就像雕刻家一次完整的“摸骨”：先确定自己的手在哪里（位姿估计），再沿手指方向感受哪里碰到了表面（深度测量），最后更新对应的便签（体素更新）。传感器换成深度相机或激光雷达，“手指”就是一束束射线。

具体来说，TOF 相机或激光雷达发射一束射线，遇到表面后返回深度值 $z_s$。对于同一条射线上的每个体素，设它沿光轴到传感器的深度为 $z(\mathbf{x})$，那么该体素的符号距离近似为“体素深度减去测量深度”：

整个流程串起来就是：深度帧输入 → 位姿对齐 → 体素更新 → 地图输出。其中位姿对齐由里程计或 SLAM 提供，位姿一旦有偏差，地图就会“重影”；体素更新则用前面讲的加权融合把多帧数据揉在一起。雕刻家每次触摸都是一帧“深度图”，他先确认自己站在哪儿，再记录指尖到表面的深度，最后把新便签与旧便签做加权平均——一轮轮循环，一尊立体的雕像就在黑暗中慢慢成形。

SLAM应用

有了 TSDF 地图，雕刻家终于可以“看见”完整的雕像了。但体素里存的是距离值，不是表面本身——该怎么把表面“取”出来？答案很漂亮：表面恰好穿过那些距离值为零的位置。

在体素网格中，表面一侧的体素 TSDF 为正、另一侧为负。只要找到相邻体素符号发生变化的“临界地带”，再用线性插值求出距离恰好为零的亚体素位置，就能拼出光滑的表面网格。这个操作叫表面提取，三维中常用的 Marching Cubes 算法做的正是这件事：

在 RGB-D SLAM 中，TSDF 是稠密重建的“常客”：KinectFusion 把每一帧深度图实时融合进 TSDF 体素场，再用光线投射直接渲染出表面；Open3D 等库则帮助把多帧点云通过 ICP 配准对齐，再统一融进同一张 TSDF 网格。雕刻家凭着上千次触摸零点的位置，把一张张“指纹”（点云帧）严丝合缝地拼在一起，终于拼出雕像完整的肌肉与轮廓。

路径规划

现在轮到助手出场：一个轮式机器人要把雕像搬出门。它手里有三件法宝——目标方向（门的方向）、ESDF 距离表，以及表上的梯度“风向标”。每走一步，它查一下脚下体素的距离和梯度，就能决定该怎么调整。

ESDF 的梯度永远指向“远离最近障碍”的方向，所以机器人把速度拆成两股：一股朝目标前进，一股沿梯度方向“往外顶”，合成后的速度既靠近门，又被障碍“推开”。这个简单而优雅的思路写出来就是：

工程上更成熟的方案是轨迹优化（如 Chomp）：把 ESDF 的距离和梯度写进代价函数，算法反复迭代，把一条原本会剐蹭的轨迹“顶”离障碍物，同时保持平滑。动态场景中，还可以比较当前 ESDF 与历史 ESDF 的距离突变，快速发现新出现的障碍。就像助手双手端着雕像，身体感受着四面八方“空气的推力”（梯度），每当靠近门框，推力就轻轻把它推回安全区，最终有惊无险地挪出门洞。

工程调优

终于到了落地环节。雕刻家的工作室地板方格该铺多密？格子越小，雕像细节越真，但地板要铺的格子数量按三次方爆炸式增长。机器人建图同样面临这个核心权衡：体素分辨率与内存、算力的取舍。

假设建图区域是边长 $L$ 米的立方体，体素边长是 $r$ 米，每个体素存距离和权重需要 $B$ 个字节，那么总内存为：

工程经验上，截断距离 $\mu$ 通常取 5~10 个体素边长：太小则传感器误差导致空洞、融合不到帧；太大则远处的截断值污染表面细节。内存吃紧时，可以用哈希体素只为表面附近的截断带分配内存（TSDF 天然适合这种“稀疏存储”），或者先粗建图确认轮廓，再在重点区域局部细化。聪明的雕刻家正是这么干的：先粗铺一遍方格确认轮廓，再在雕塑肩膀、脸部换细方格，门框周围加密便签，而不必给整间屋子铺满——该省的地方省，该精细的地方精细，这才是工程的真功夫。

AI 应用场景

Python 演示

import math  class SimpleTSDF:    """极简二维 TSDF 体素地图：截断距离 + 权重融合"""     def __init__(self, size=50, voxel=0.1, trunc=0.5):        self.size = size        self.voxel = voxel        self.trunc = trunc        self.values = [[0.0] * size for _ in range(size)]        self.weights = [[0.0] * size for _ in range(size)]     def update(self, sx, sy, weight=1.0):        """融合一次表面测量点 (sx, sy)，更新其截断带内的体素"""        idx = int(sx / self.voxel)        idy = int(sy / self.voxel)        radius = int(self.trunc / self.voxel) + 1        for di in range(-radius, radius + 1):            for dj in range(-radius, radius + 1):                ni, nj = idx + di, idy + dj                if not (0 <= ni < self.size and 0 <= nj < self.size):                    continue                cx = (ni + 0.5) * self.voxel                cy = (nj + 0.5) * self.voxel                dist = math.hypot(cx - sx, cy - sy)                if dist > self.trunc:                    continue                f = min(dist / self.trunc, 1.0)                old_w = self.weights[ni][nj]                self.values[ni][nj] = (                    self.values[ni][nj] * old_w + f * weight) / (old_w + weight)                self.weights[ni][nj] = old_w + weight     def build_esdf(self):        """从 TSDF 找表面体素，为每个体素计算真实欧氏距离"""        surf = [(i, j) for i in range(self.size) for j in range(self.size)                if 0.0 < self.values[i][j] < 0.4]        esdf = [[0.0] * self.size for _ in range(self.size)]        if not surf:            return esdf        for i in range(self.size):            for j in range(self.size):                esdf[i][j] = min(math.hypot(i - si, j - sj)                                 for si, sj in surf) * self.voxel        return esdf  def main():    tsdf = SimpleTSDF(size=50, voxel=0.1, trunc=0.5)    # 模拟激光雷达从两个视角扫描同一面墙    for x in range(5, 45, 3):        tsdf.update(x * 0.1, 2.0, weight=0.8)        tsdf.update(x * 0.1, 2.05, weight=1.2)    esdf = tsdf.build_esdf()    near = [v for row in tsdf.values for v in row if v > 0]    print("TSDF 表面附近体素数:", len(near),          "平均距离:", round(sum(near) / len(near), 3))    print("ESDF: 体素(10,14)到墙距离:", round(esdf[10][14], 3), "米")  if __name__ == "__main__":    main()

1. self.values = [[0.0] * size for _ in range(size)]为每个体素分配一个距离值，初始全为 0。
2. radius = int(self.trunc / self.voxel) + 1计算截断半径，只更新表面附近的体素，体现 TSDF 的“局部精雕”。
3. if dist > self.trunc: continue截断判断：超出 μ 的体素直接跳过，不参与融合。
4. self.values[ni][nj] = (self.values[ni][nj] * old_w + f * weight) / (old_w + weight)加权平均融合：旧值和新值按权重比例混合，多帧测量越摸越准。
5. if 0.0 < self.values[i][j] < 0.4把距离值较小的表面附近体素当作“障碍种子”，用于生成 ESDF。
6. min(math.hypot(i - si, j - sj) for si, sj in surf) * self.voxel对每个空闲体素求到最近障碍种子的欧氏距离，再乘体素物理尺寸换算成米。
7. tsdf.update(x * 0.1, 2.0, weight=0.8)模拟一次带权重的表面测量：不同角度给不同置信度。

常见误区

面试题