设想机械臂正在伸向货架。运动学模型能告诉我们：在当前关节角度下，手腕位于哪里；如果某个关节再转动一点，手腕会向哪里移动。要让手腕避开货架，还需要另一类信息：手腕离货架有多远，以及向哪个方向移动会让这个距离增大。
距离场把这类环境几何信息整理成一种便于查询的形式。它给空间中的位置赋予一个距离值，规划器再利用这个值及其变化方向，调整机器人运动。
本文重点讨论的两种方法： Chamfer 加权邻域距离变换 和 可分离欧式距离变换 。
1. 机器人怎样把看到的环境变成地图
1.1 点云记录物体表面的位置
深度相机或激光雷达可以测量传感器到物体表面的距离。结合每次测量的方向，就能得到一个三维点的位置。许多测量点放在一起，形成点云。
例如，传感器面对货架时，点云中可能出现立柱、层板边缘和药盒外表面的采样点。随着机器人移动，传感器从新的位置观察，原先被遮挡的表面也可能逐渐进入点云。
为了把多次观测放进同一张地图，需要知道传感器在地图中的位姿。假设一个测量点在传感器坐标系中的位置为 pₛ，传感器到地图坐标系的旋转为 R、平移为 t，则该点在地图坐标系中的位置 pₘ 为：
p m = R p s + t p_m=R p_s+t pm=Rps+t
这一步与机器人运动学中的坐标变换相同。环境感知接下来要解决的是：怎样把大量观测点组织成可查询的空间信息。
图 1：测量光束沿途提供自由空间证据，终点提供占据证据，遮挡后方保留为未知区域。右图用小格子把这些信息组织起来。
1.2 占据栅格为每一小块空间保存状态
一种直观的方法，是用大小一致的小立方体划分空间。每个小立方体称为 体素 ；二维示意中则称为格子。
每个体素可以保存三类状态：
| 状态 | 来自观测的含义 | 一个直观例子 |
|---|---|---|
| 自由 | 有证据表明该空间可以穿过 | 测量光束从传感器到返回点之间经过的空间 |
| 占据 | 有证据表明该空间存在物体 | 深度测量的返回点所在体素 |
| 未知 | 当前观测尚不足以判断 | 货架层板后方被遮挡的区域 |
实际传感器存在测量噪声。同一个体素可能先后收到不同证据，因此建图通常会累积观测置信度。例如，多次测到某处表面，会提高该处的占据概率；多次光束穿过某处，会提高它是自由空间的可信度。
规划系统还会为未知区域选择处理策略。保守规划可以把未知格作为障碍处理；探索任务则可以允许机器人逐步进入未知区域并补充观测。这个选择会影响哪些格子成为距离变换的障碍种子。
占据地图形成后，距离变换就有了输入：哪些格子属于障碍，哪些格子属于自由空间。后文的小网格示例使用已经确定这两类状态的区域，便于观察距离如何计算。
1.3 分辨率把物理坐标与格子下标联系起来
体素的边长称为地图分辨率，记为 r，单位为米。比如 r = 0.10 m，表示每个体素的三个边长都是 10 cm。
设地图原点的 x 坐标为 oₓ，一个点的 x 坐标为 pₓ。它所在体素的 x 下标 iₓ 为：
i x = ⌊ p x − o x r ⌋ i_x=\left\lfloor\frac{p_x-o_x}{r}\right\rfloor ix=⌊rpx−ox⌋
符号 ⌊·⌋ 表示向下取整。y、z 两个方向使用相同的规则。知道下标后，该体素中心的 x 坐标 cₓ 为：
c x = o x + ( i x + 1 2 ) r c_x=o_x+\left(i_x+\frac{1}{2}\right)r cx=ox+(ix+21)r
图 2：地图原点为 (0,0)、分辨率为 0.10 m 时，点 (0.27,0.16) m 落在下标为 (2,1) 的格子里，该格子的中心位于 (0.25,0.15) m。
分辨率决定地图描述环境的细致程度。较小的体素能够表达更细的结构，也会增加需要存储和处理的格子数量。对于边长固定的三维区域，分辨率减半，体素数会增加到原来的八倍。
2. 距离场：给空间中的每个位置写上距离
2.1 从"这里有障碍"到"这里离障碍多远"
占据地图回答一个离散问题：某个格子是否被占据。避障优化还需要知道附近有多少活动余地。
可以想象在货架表面外画出一层层等距面：第一层离货架 5 cm，第二层离货架 10 cm，第三层离货架 15 cm。机器人球心落在哪一层附近，就有多大的空间余量。把这种距离信息存储在地图中，就得到距离场。
设 Ω 表示障碍物占据的实体区域，∂Ω 表示它的表面。对一个查询位置 p，选择表面上与它最近的位置 b。p 到表面的欧式距离 d§ 为：
d ( p ) = min b ∈ ∂ Ω ∥ p − b ∥ 2 d(p)=\min_{b\in\partial\Omega}\|p-b\|_2 d(p)=b∈∂Ωmin∥p−b∥2
其中 ‖p−b‖₂ 就是三维直线距离。下标 2 表示欧式范数，也就是熟悉的平方和开根号。
2.2 用正负号表示内外关系
如果再用正负号记录位置在障碍物外部还是内部，就得到带符号距离场：
- 障碍物外部：距离为正。
- 障碍物表面：距离为零。
- 障碍物内部：距离为负，绝对值表示到表面的距离。
通常用 φ§ 表示带符号距离。 ESDF 是 Euclidean Signed Distance Field 的缩写，中文为"欧式带符号距离场"。其中，Euclidean 描述距离的度量方式，Signed 描述内外的符号约定，Field 表示整个空间上的数值分布。
图 3：左图的每条曲线连接距离相同的位置。右图沿圆心取一条截线，距离函数在障碍内部为负，在两侧边界经过零，再向外增加。
以半径为 0.45 m 的球形障碍为例：球心外 0.80 m 的位置，到表面的距离是 0.35 m；距球心 0.20 m 的内部位置，其带符号距离为 −0.25 m。
2.3 数值地图把这个连续函数采样下来
空间中的位置有无穷多个，计算机采用有限数量的体素中心来存储距离。于是可以把一张距离地图想成一张三维表格：每个体素中心保存一个数字，其他位置通过邻近数字估算。
整个使用过程因此分成两个阶段：
- 构建距离场： 根据占据地图，计算各个体素中心的距离。
- 查询距离场： 给定机器人球心等任意位置，计算该位置的距离与梯度。
构建阶段面对整张地图，查询阶段面对一个个具体位置。接下来介绍的 Chamfer 和可分离欧式距离变换，主要解决第一个阶段。
3. 距离变换究竟在计算什么
3.1 让占据格成为距离传播的起点
从占据地图出发，可以先把每个占据体素的中心作为一个 障碍种子 。种子处的距离设为 0，其他格子的距离暂时设为无穷大，含义是"尚未找到附近障碍"。
随后为每个格子寻找最近的障碍种子，并记录距离。完成这项计算的过程称为 距离变换 ，英文为 Distance Transform，简称 DT。采用欧式度量时称为 EDT。
这里有一个自然的离散化过程：环境表面先被划分为体素，再由体素中心代表其位置。地图分辨率描述了这一步的空间尺度。构建带符号场时，还会结合自由格和占据格之间的边界位置来确定零面。
3.2 最直接的计算方法
假设地图中有 N 个格子，其中有 M 个障碍种子。对每个格子，遍历所有障碍种子，计算距离，再选最小值，就能得到答案。
这种方法容易理解，却需要大约 N×M 次距离比较。大地图中，许多格子之间的距离关系存在重复信息。高效距离变换会利用这些结构，使计算量主要随地图格子数 N 增长。
Chamfer 利用的是 邻居之间可以传递距离 ；可分离欧式距离变换利用的是 欧式距离平方可以沿坐标轴分解 。
4. 方法一：Chamfer 用邻居传递距离
4.1 先规定向邻居走一步的代价
先看二维网格。一个格子周围有八个邻居：上下左右四个，四个斜对角。
如果体素间距暂时取为 1，那么横向或纵向移动一步，几何长度是 1；斜向移动一步，长度是 √2。一组直观的 Chamfer 权重就是：横竖一步记 1，斜向一步记 √2。
图 4：从障碍种子 (0,0) 到查询格 (3,2)，邻域路径可以走两次斜向、一次横向，总代价为 2√2+1；两点间的直线距离为 √13。
把这种"沿允许的邻接方向行走、把每一步权重累加"的结果作为距离，就是 Chamfer 距离的直观含义。它使用少量固定方向，近似空间中的任意方向。
上例中：
- 欧式距离：√13 ≈ 3.606 格。
- Chamfer 距离：1+2√2 ≈ 3.828 格。
若 r = 0.10 m，对应的距离分别约为 0.361 m 和 0.383 m。
4.2 当前格怎样从邻居得到答案
假设某个邻居已经知道自己离最近障碍有多远。那么当前格可以先走到这个邻居，再接上它通向障碍的距离。
用 C(i) 表示下标为 i 的格子保存的距离，单位为格；j 表示它的一个邻居；w(i−j) 表示从 j 移动到 i 的步长权重。通过这个邻居得到的候选距离为 C(j)+w(i−j)。如果候选更小，就更新当前格：
C ( i ) ← min C ( i ) , C ( j ) + w ( i − j ) C(i)\leftarrow\min\leftC(i),\\ C(j)+w(i-j)\\right C(i)←minC(i), C(j)+w(i−j)
对多个邻居重复这个操作，当前格就能选到其中最小的候选。每次更新只需要读取一个距离、加一个固定权重，再比较大小。
例如，一个格子的左邻居保存 3.0，上邻居保存 2.5，左下斜邻居保存 1.8。通过这三个邻居得到的候选分别为 4.0、3.5、3.214，因此当前格可以更新为约 3.214。
4.3 一次正向扫描，一次反向扫描
为了让信息有次序地传播，可以先从左下角开始，逐行向右上角扫描。每处理一个格子，就使用本轮已经处理过的邻居。这样，障碍的信息会沿扫描方向逐渐传开。
然后从右上角开始反向扫描，让来自另一侧的距离信息补充进来。对于图中的标准八邻域，这两次扫描就能得到加权网格距离。
图 5：中心格是障碍种子。第一次扫描后，一部分格子仍保持 ∞；反向扫描把距离传到另一侧。表中数值以格为单位，显示到一位小数。
当有多个障碍种子时，每个种子都从 0 开始传播。同一格收到多条候选后取最小值，于是自动归属给距离更小的障碍。
4.4 为什么近似误差和方向有关
若查询格相对单个种子的水平、竖直偏移绝对值排序为 a ≥ b ≥ 0，那么最短的八邻域路径可以由 b 次斜向移动和 a−b 次横向移动组成：
d C = r ( a − b ) + b 2 , d E = r a 2 + b 2 d_C=r\left(a-b)+b\\sqrt{2}\\right,\qquad d_E=r\sqrt{a^2+b^2} dC=r(a−b)+b2 ,dE=ra2+b2
d_C 是 Chamfer 距离，d_E 是欧式距离。沿水平、竖直和 45° 方向，允许的步长恰好能连成直线；其他方向需要把几种步长拼起来，折线路径会稍长。
图 6：采用 1、√2 权重时，Chamfer 的等距轮廓呈八边形；与坐标轴夹角为 22.5° 左右的方向，距离高估约 8.24%。这些数字可以直接由上面的两个公式计算。
权重可以进一步调整，使不同方向上的误差相互平衡。降低某些步长权重，能够补偿折线路径偏长的部分代价，同时会使另一些方向略微低估。
在三维中，一个体素周围有 26 个邻居，可以分为三类：
| 邻居关系 | 数量 | 几何步长 | 一组调整后的权重示例 |
|---|---|---|---|
| 共用一个面 | 6 | 1 | 0.92644 |
| 共用一条棱 | 12 | √2 | 1.34065 |
| 共用一个角 | 8 | √3 | 1.65849 |
三维 Chamfer 仍然使用邻域传递和正反扫描，只是邻域从二维的八个方向扩展到三维的二十六个方向。邻域形状与权重共同决定距离近似的特性。
5. 方法二：把欧式距离拆成几个一维问题
5.1 平方距离给了我们一个分解机会
欧式距离的平方由各个坐标方向的平方差相加得到。二维中，查询格 (x,y) 与障碍种子 (u,v) 的平方距离为：
( x − u ) 2 + ( y − v ) 2 (x-u)^2+(y-v)^2 (x−u)2+(y−v)2
如果寻找所有障碍中的最近者，就需要取这个表达式的最小值。可分离欧式距离变换利用的正是"各轴平方差相加"这一结构。
设 f(u,v) 在障碍种子处为 0，其余格子为无穷大。整个二维平方距离场 D(x,y) 可以写为：
D ( x , y ) = min u , v f ( u , v ) + ( x − u ) 2 + ( y − v ) 2 D(x,y)=\min_{u,v}\leftf(u,v)+(x-u)\^2+(y-v)\^2\\right D(x,y)=u,vminf(u,v)+(x−u)2+(y−v)2
x、y、u、v 都是格子下标，D 的单位是格²。引入 f 后，障碍格能够给出有限候选，其他格子的初始候选保持无穷大。
5.2 先处理竖向，再处理横向
第一轮，固定一列，只沿 y 方向寻找最近种子，保存竖向平方距离 A(x,y)：
A ( x , y ) = min v f ( x , v ) + ( y − v ) 2 A(x,y)=\min_v\leftf(x,v)+(y-v)\^2\\right A(x,y)=vminf(x,v)+(y−v)2
第二轮，固定一行，对其他列 u 提供的候选进行比较。每个候选已经带着第一轮算好的竖向代价 A(u,y)，再加上横向平方差：
D ( x , y ) = min u A ( u , y ) + ( x − u ) 2 D(x,y)=\min_u\leftA(u,y)+(x-u)\^2\\right D(x,y)=uminA(u,y)+(x−u)2
把第一式代入第二式，可以恢复前面的二维最小值表达式。这个代数关系保证了分两轮计算仍然得到同一个最小平方距离。
图 7：两个障碍种子位于 (1,1) 和 (5,4)。先沿 y 计算，有障碍的两列得到有限值；再沿 x 组合这些候选，整张图都得到平方距离。
以查询格 (3,2) 为例：
- 列 1 提供的竖向代价是 (2−1)² = 1，再加横向代价 (3−1)² = 4，总计 5。
- 列 5 提供的竖向代价是 (2−4)² = 4，再加横向代价 (3−5)² = 4，总计 8。
选较小的 5，再开平方，得到 √5 格。若分辨率为 0.10 m，距离就是约 0.224 m。
三维中再增加一轮坐标轴处理即可。可以依次沿 z、y、x 计算；中间始终保存平方距离，最后开平方并乘以分辨率 r。
5.3 一维问题怎样算得更快
现在把注意力放到一条长度为 n 的格子线上。用 j 表示查询下标，用 q 表示候选下标，f(q) 表示前一轮坐标轴已经累计的平方距离代价。
候选 q 对查询位置 j 提供的总平方距离是：
P q ( j ) = f ( q ) + ( j − q ) 2 P_q(j)=f(q)+(j-q)^2 Pq(j)=f(q)+(j−q)2
把 j 当作横坐标，这个函数是一条开口向上的抛物线，顶点横坐标为 q，高度为 f(q)。每个候选对应一条这样的抛物线。
对任意查询位置，选所有抛物线中最低的那个值，就得到最终的一维变换结果 T(j)：
T ( j ) = min q P q ( j ) T(j)=\min_q P_q(j) T(j)=qminPq(j)
所有抛物线最低部分连起来形成的曲线，称为 下包络 。
图 8：候选 1、4、7 的已有代价分别为 0、16、0。查询 j=4 时，它们给出的值分别是 9、16、9。蓝色下包络选择 9；灰色抛物线在整条查询线上都处于更高的位置，可以从候选表中移除。
这提供了一种整理候选的办法：只保留会在某一段查询区间成为最小值的候选，并记录这些区间在哪里交接。
若两个候选下标为 p、q，交接位置 s 满足两条抛物线相等。展开平方项后可得到：
s = f ( q ) + q 2 − f ( p ) − p 2 2 ( q − p ) s=\frac{f(q)+q^2-f(p)-p^2}{2(q-p)} s=2(q−p)f(q)+q2−f(p)−p2
当 p=1、q=7，且二者的已有代价都为 0 时，交点就在 s=4。交点左侧由候选 1 提供较小值，右侧由候选 7 提供较小值。
5.4 为什么整个变换可以做到线性复杂度
候选可以按下标从小到大加入。新候选出现后，如果某个旧候选已经失去所有最优区间，就把它从候选表末尾移除。每个候选至多加入一次、移除一次。
候选表整理好后，再从左到右扫描查询位置。最优候选随着查询位置向前推进，指针也沿候选表向前移动。
因此，一条长度为 n 的线，其候选整理与查询合起来只需要与 n 成比例的工作量。全图每个坐标方向的线段长度总和，都等于格子总数 N，所以二维和三维欧式距离变换都可以做到 O(N) 。
这就是可分离欧式距离变换的关键：用坐标轴分解减少问题维度，再用下包络组织每条线上的最小值计算。
6. 怎样把外部距离扩展成带符号距离
6.1 再以自由格为种子算一遍
前面讨论的距离变换以占据格为种子。在自由格中，它给出到最近占据中心的距离；在占据格中，它的值为 0。
为了描述内部深度，可以交换自由与占据的角色，再算一次距离变换。这样会得到两个字段：
- dO：到最近占据体素中心的距离。
- dF：到最近自由体素中心的距离。
自由格的 dF 为 0，占据格的 dO 为 0。一个常见的离散带符号组合是：
ϕ g = d O − d F \phi_g=d_O-d_F ϕg=dO−dF
下标 g 表示这是从离散格点建立的带符号字段。自由区域得到正值，占据区域得到负值。
图 9：体素中心 4～8 构成一个占据块。中间体素 6 到最近自由中心的距离是 3 格，因此离散带符号值为 −3。向边缘移动时，内部距离的绝对值逐渐减小。
6.2 体素中心与几何边界怎样对应
考虑一面恰好位于相邻格子之间的平面墙。一侧自由体素中心到另一侧占据体素中心的距离是 r，而各自到墙面的距离是 r/2。
这说明格点中心距离与表面距离之间，还存在一个边界定位步骤。对于这个平面例子，自由侧的正距离减去 r/2，占据侧的负距离加上 r/2，就能对应到墙面。三维曲面则会结合体素采样、零面约定和插值来近似。
从学习顺序看，可以把"到最近种子中心的距离"作为距离变换的基本结果，再理解体素代表的实体边界在哪里。这样就能同时把握算法计算的对象，以及它与真实物体表面之间的几何关系。
6.3 带符号场为什么有利于局部调整
当一个候选机器人位置进入障碍区域后，负距离可以表达进入的深度。比如从 −0.08 m 调整到 −0.03 m，说明该位置正在接近自由空间；继续调整到正值，就来到障碍外部。
Chamfer 与欧式距离变换都可以分别计算 dO 和 dF，再合成带符号字段。选择哪种距离度量，以及是否保存内部负距离，是地图设计中的两个独立选择。
7. 查询点落在格子之间时，距离怎样算
机器人球心通常位于连续空间中的任意位置。距离地图保存的是有限个体素中心值，因此查询时需要根据附近样本计算距离。查询接口可以返回外部距离，也可以返回带符号距离；下面统一把查询结果记作 d。
最直接的连续化方式是 插值 ：根据查询点与周围体素中心的相对位置，给附近样本分配权重。
7.1 一维：两个端点之间按比例分配
设左侧样本位于 x₀，保存距离 d₀；右侧样本位于 x₀+r，保存距离 d₁。查询位置为 x，定义归一化位置 t：
t = x − x 0 r , d ( x ) = ( 1 − t ) d 0 + t d 1 t=\frac{x-x_0}{r},\qquad d(x)=(1-t)d_0+t d_1 t=rx−x0,d(x)=(1−t)d0+td1
t=0 时取左端值，t=1 时取右端值，t=0.5 时取平均值。
如果两个距离分别为 0.20 m 和 0.26 m，查询点走过四分之一间隔，那么得到的距离为 0.75×0.20+0.25×0.26 = 0.215 m。
图 10：左图是一维线性插值；右图中，一个二维查询点由四个角点共同确定。图中的方框由相邻体素中心围成，四个中心之间的间距是地图分辨率。
7.2 二维：先沿 x 插值，再沿 y 插值
设四个角点距离分别为 d₀₀、d₁₀、d₀₁、d₁₁。第一个下标表示左右位置，第二个下标表示上下位置。查询点的归一化横向、纵向位置分别为 tₓ、tᵧ。
先在上下两行各做一次线性插值，再按查询点的高度混合两行结果，就得到 双线性插值 ：
d = ( 1 − t x ) ( 1 − t y ) d 00 + t x ( 1 − t y ) d 10 + ( 1 − t x ) t y d 01 + t x t y d 11 d=(1-t_x)(1-t_y)d_{00}+t_x(1-t_y)d_{10}+(1-t_x)t_y d_{01}+t_x t_y d_{11} d=(1−tx)(1−ty)d00+tx(1−ty)d10+(1−tx)tyd01+txtyd11
这个式子可以理解为"四个角点距离乘以各自的权重，再相加"。四个权重的和为 1。
图 10 的右图采用下面这组数据：
| 角点 | 距离 | 对应权重 | 加权贡献 |
|---|---|---|---|
| 左下 d₀₀ | 0.20 m | 0.75×0.40 = 0.30 | 0.060 m |
| 右下 d₁₀ | 0.26 m | 0.25×0.40 = 0.10 | 0.026 m |
| 左上 d₀₁ | 0.28 m | 0.75×0.60 = 0.45 | 0.126 m |
| 右上 d₁₁ | 0.34 m | 0.25×0.60 = 0.15 | 0.051 m |
所以查询距离为 0.263 m 。这四个数可以来自一面倾斜平面墙，其距离沿两个坐标方向平稳增长。
7.3 三维：八个角点分两层插值
三维中，查询点由附近八个体素中心包围。可以把它们看作上下两层，每层四个角点：先在两层内分别做双线性插值，得到 D₀ 和 D₁，再根据归一化高度 t_z 做线性插值：
d = ( 1 − t z ) D 0 + t z D 1 d=(1-t_z)D_0+t_zD_1 d=(1−tz)D0+tzD1
图 11：每个角点保存一个距离。下层与上层分别形成一个二维插值结果，最后沿高度方向混合。
这种方式称为 三线性插值 。为了紧凑表达八个角点的权重，定义 B₀(t)=1−t、B₁(t)=t。令 d_abc 表示下标 a、b、c 各取 0 或 1 时的角点距离，则：
d ( p ) = ∑ a = 0 1 ∑ b = 0 1 ∑ c = 0 1 B a ( t x ) B b ( t y ) B c ( t z ) d a b c d(p)=\sum_{a=0}^{1}\sum_{b=0}^{1}\sum_{c=0}^{1}B_a(t_x)B_b(t_y)B_c(t_z)d_{abc} d(p)=a=0∑1b=0∑1c=0∑1Ba(tx)Bb(ty)Bc(tz)dabc
Chamfer 和欧式距离变换生成的格点距离，都可以通过这套查询方式使用。两者在建图时采用不同计算方法，查询时则可以共用同一种连续化方法。
实现查询时，要先找到包围查询点的两个相邻中心。以 x 方向为例，中心位置从 oₓ+r/2 开始，间隔为 r。因此，位于查询点左侧的中心下标 i₀ₓ 为：
i 0 x = ⌊ p x − o x r − 1 2 ⌋ i_{0x}=\left\lfloor\frac{p_x-o_x}{r}-\frac{1}{2}\right\rfloor i0x=⌊rpx−ox−21⌋
右侧中心的下标就是 i₀ₓ+1。在 y、z 方向分别找到前后两个中心，组合起来就是需要的八个角点。相邻体素中心构成了连续位置查询所使用的插值网格。
8. 梯度怎样告诉机器人往哪里移动
8.1 梯度是距离对空间位置的敏感程度
距离值回答"现在离障碍多远"。梯度回答"位置变化一点，距离将怎样变化"。
对于三维距离函数 d(x,y,z)，它的梯度是三个偏导数组成的向量：
∇ d = ( ∂ d ∂ x , ∂ d ∂ y , ∂ d ∂ z ) T \nabla d=\left(\frac{\partial d}{\partial x},\frac{\partial d}{\partial y},\frac{\partial d}{\partial z}\right)^{\mathsf{T}} ∇d=(∂x∂d,∂y∂d,∂z∂d)T
例如，∂d/∂x = 0.6，表示沿 x 正方向移动 1 mm，距离在局部近似增加 0.6 mm。位置发生一个很小的变化 Δp 时，距离变化满足一阶近似：
Δ d ≈ ∇ d T Δ p \Delta d\approx\nabla d^{\mathsf{T}}\Delta p Δd≈∇dTΔp
这个式子与运动控制中使用雅可比做小位移线性化的思路一致。给定同样大小的空间位移，沿梯度方向移动，能够在局部获得最大的距离增长。
图 12：单个光滑障碍附近，梯度通常沿远离表面的法向。两个障碍之间存在最近表面的切换区域，距离场会出现折痕，梯度方向随最近障碍改变。
在表面光滑、最近表面点唯一的位置，理想欧式距离的梯度长度为 1。离散化和插值会改变局部斜率，因此栅格距离场的梯度长度也会随采样位置变化。
8.2 从插值公式直接求导
查询时已经取得周围的距离值，因此可以对插值公式直接求导。
一维线性插值的斜率为：
∂ d ∂ x = d 1 − d 0 r \frac{\partial d}{\partial x}=\frac{d_1-d_0}{r} ∂x∂d=rd1−d0
含义很直接：两端距离相差多少，除以两端位置相隔多少。这里必须保留分母 r，才能把"每格的变化"换算为"每米的变化"。
二维双线性插值的 x 方向导数为：
∂ d ∂ x = ( 1 − t y ) ( d 10 − d 00 ) + t y ( d 11 − d 01 ) r \frac{\partial d}{\partial x}=\frac{(1-t_y)(d_{10}-d_{00})+t_y(d_{11}-d_{01})}{r} ∂x∂d=r(1−ty)(d10−d00)+ty(d11−d01)
可以先看分子里的两个差值：d₁₀−d₀₀ 是下排沿 x 的距离变化，d₁₁−d₀₁ 是上排沿 x 的距离变化。查询点靠近哪一排，就给那一排的变化更大权重。
同理，y 方向导数为：
∂ d ∂ y = ( 1 − t x ) ( d 01 − d 00 ) + t x ( d 11 − d 10 ) r \frac{\partial d}{\partial y}=\frac{(1-t_x)(d_{01}-d_{00})+t_x(d_{11}-d_{10})}{r} ∂y∂d=r(1−tx)(d01−d00)+tx(d11−d10)
8.3 把上一节的查询例子继续算完
仍然使用图 10 的四个距离，取 r=0.10 m、tₓ=0.25、tᵧ=0.60。
x 方向的距离变化：下排为 0.26−0.20=0.06 m，上排为 0.34−0.28=0.06 m。两排相同，所以无论查询点在哪个高度，x 方向导数都是 0.06/0.10=0.6。
y 方向同理：左列和右列的距离差都是 0.08 m，除以 0.10 m，得到 0.8。
因此，这次查询同时得到：
- 距离 d = 0.263 m。
- 梯度 ∇d = (0.6,0.8)。
若沿梯度方向移动 2 cm，对应位移为 Δp=(0.012,0.016) m。代入一阶关系，距离增加约 0.6×0.012+0.8×0.016 = 0.020 m 。
8.4 三维梯度沿用相同思路
三维中，x 方向的导数由四组左右对应角点的距离差构成。继续使用上一节定义的 B₀、B₁：
∂ d ∂ x = 1 r ∑ b = 0 1 ∑ c = 0 1 B b ( t y ) B c ( t z ) ( d 1 b c − d 0 b c ) \frac{\partial d}{\partial x}=\frac{1}{r}\sum_{b=0}^{1}\sum_{c=0}^{1}B_b(t_y)B_c(t_z)(d_{1bc}-d_{0bc}) ∂x∂d=r1b=0∑1c=0∑1Bb(ty)Bc(tz)(d1bc−d0bc)
这里括号中的差值表示一组沿 x 相隔一个体素间距的角点距离差；y、z 方向的权重决定这组差值对查询点的影响。
将坐标角色轮换，就得到另外两个方向：
∂ d ∂ y = 1 r ∑ a = 0 1 ∑ c = 0 1 B a ( t x ) B c ( t z ) ( d a 1 c − d a 0 c ) \frac{\partial d}{\partial y}=\frac{1}{r}\sum_{a=0}^{1}\sum_{c=0}^{1}B_a(t_x)B_c(t_z)(d_{a1c}-d_{a0c}) ∂y∂d=r1a=0∑1c=0∑1Ba(tx)Bc(tz)(da1c−da0c)
∂ d ∂ z = 1 r ∑ a = 0 1 ∑ b = 0 1 B a ( t x ) B b ( t y ) ( d a b 1 − d a b 0 ) \frac{\partial d}{\partial z}=\frac{1}{r}\sum_{a=0}^{1}\sum_{b=0}^{1}B_a(t_x)B_b(t_y)(d_{ab1}-d_{ab0}) ∂z∂d=r1a=0∑1b=0∑1Ba(tx)Bb(ty)(dab1−dab0)
因此，一次取得八个角点距离后，就能同时算出查询距离与三个梯度分量。
8.5 另一种常见方法：有限差分
如果已有一个能返回距离的查询函数，也可以在查询点两侧各移动一小段距离 h，通过中心差分近似导数。令 eₓ 表示 x 方向的单位向量：
∂ d ∂ x ≈ d ( p + h e x ) − d ( p − h e x ) 2 h \frac{\partial d}{\partial x}\approx\frac{d(p+h e_x)-d(p-h e_x)}{2h} ∂x∂d≈2hd(p+hex)−d(p−hex)
y、z 方向同样处理。完整三维梯度需要查询六个偏移位置。
直接对插值公式求导，得到的是当前插值函数在该位置的导数；有限差分则用两个采样位置之间的平均斜率来近似。差分步长 h 决定了这次估计观察多大的空间范围。对于提供插值距离的地图，复用角点数据求解析导数通常很方便。
8.6 两种距离场的梯度形状
还可以用一个单独的障碍种子，直观看到两种距离度量对梯度的影响。设种子位于二维原点，查询位置为 p=(x,y)。欧式距离的连续表达式是 √(x²+y²)，它的梯度为：
∇ d E = ( x , y ) T x 2 + y 2 \nabla d_E=\frac{(x,y)^{\mathsf{T}}}{\sqrt{x^2+y^2}} ∇dE=x2+y2 (x,y)T
这个向量沿从种子指向查询点的方向，并随查询点的位置平滑转动。
对于横竖权重为 1、斜向权重为 √2 的 Chamfer 场，在 x>y>0 的方向区域内，距离表达式为 d_C=x+(√2−1)y。对应的梯度为：
∇ d C = ( 1 , 2 − 1 ) T \nabla d_C=(1,\sqrt{2}-1)^{\mathsf{T}} ∇dC=(1,2 −1)T
它在这个方向区域内保持固定，进入另一个方向区域后再改变。有限邻接方向形成的折线等距轮廓，也就对应着分区域变化的梯度方向。
图 13：左图的欧式梯度沿径向变化；右图采用 1、√2 权重，Chamfer 梯度在同一方向区域内相同。这里展示单种子度量的连续表达式；栅格查询还会根据角点采样执行插值。
例如，在 p=(0.4,0.2) m 处，欧式表达式给出的梯度约为 (0.894,0.447)，Chamfer 表达式给出的梯度约为 (1,0.414)。两者都倾向于远离这个种子，但局部移动方向和斜率各有特点。
9. 从空间梯度走到关节运动
9.1 用球体描述机器人与环境的间隙
机械臂可以用若干碰撞球近似。设其中一个球的球心为 c，半径为 R；地图查询到的球心距离为 d©。球体表面相对环境的净距 g 为：
g = d ( c ) − R g=d(c)-R g=d(c)−R
例如，球心距障碍表面 0.12 m，球半径为 0.04 m，则球体与环境之间还有 0.08 m 的净距。若希望保留安全余量 m，就要求 g≥m。
图 14：环境距离场给出球心的空间梯度，机器人运动学给出关节到球心位置的雅可比。两者相乘，就把空间避障方向传递到关节空间。
9.2 与运动学雅可比相乘
令 q 为活动关节向量，球心位置由正运动学 c(q) 给出，球心雅可比为 Jc=∂c/∂q。若有 n 个活动关节，Jc 是一个 3×n 矩阵。
把球心位置、地图梯度和雅可比都表达在同一坐标系中，利用链式法则得到：
∇ q g = J c ( q ) T ∇ d ( c ( q ) ) \nabla_q g=J_c(q)^{\mathsf{T}}\nabla d(c(q)) ∇qg=Jc(q)T∇d(c(q))
右边的 ∇d 是一个三维空间向量；左边的 ∇qg 有 n 个分量，分别表示各个关节变化时，环境净距怎样变化。
这个式子把两个领域连接起来：环境地图负责提供空间几何信息，机器人模型负责描述可实现的运动方向。
9.3 一个简单的避障代价
设 v 表示安全余量的不足量：v=max(0,m−g)。当净距已经达到 m 时，v 为 0；净距不足时，v 为正。
给这个不足量一个二次惩罚，令 w 为权重，就得到：
E ( q ) = 1 2 w max ( 0 , m − g ( q ) ) 2 E(q)=\frac{1}{2}w\left\\max(0,m-g(q))\\right^2 E(q)=21wmax(0,m−g(q))2
在 v>0 的区域，代价对关节的梯度为：
∇ q E = − w v J c ( q ) T ∇ d ( c ( q ) ) \nabla_q E=-w v J_c(q)^{\mathsf{T}}\nabla d(c(q)) ∇qE=−wvJc(q)T∇d(c(q))
优化器沿负代价梯度调整关节，就会倾向于增加球体净距。实际轨迹优化还会同时考虑目标跟踪、平滑性和关节运动限值，避障梯度作为其中一项共同参与求解。
这也解释了为什么距离场不仅需要可靠的距离值，还需要有用的局部变化信息：前者决定是否接近障碍，后者引导候选运动向更宽裕的空间调整。
10. Chamfer 的速度优势来自哪里
10.1 固定邻域让每个格子的工作很简单
Chamfer 的基本操作是"邻居距离加一个固定权重，然后取较小值"。邻居数量固定，地图扩大后，每个格子的计算方式仍然相同。
此外，它可以按规则顺序扫描连续内存。相邻格子的数据位置接近，通常能够较好地利用处理器缓存。权重也可以预先准备好。
这些特征使 Chamfer 成为一种结构简单、开销容易控制的距离变换方法。
10.2 可分离 EDT 同样具有线性复杂度
可分离欧式距离变换也只需要与格子数成比例的工作量。它额外进行的操作主要来自候选管理：计算抛物线交点、维护候选区间、选择当前最优候选，以及最后的开平方。
使用 N 表示整张地图的格子数，M 表示障碍种子数，可以这样比较：
| 方法 | 计算怎样展开 | 计算量随规模变化 |
|---|---|---|
| 逐格枚举全部种子 | 每个格子比较 M 个障碍 | O(NM) |
| Chamfer | 每格处理固定数量的邻居，进行规则扫描 | O(N) |
| 可分离欧式距离变换 | 沿各坐标轴处理一维线段，用下包络组织候选 | O(N) |
O(N) 的含义是：在其他条件相近时，格子数翻倍，核心工作量也大致翻倍。两种线性算法仍然可以有不同的每格开销。
10.3 输出需求和数据组织也会影响速度
构建带符号场时，通常要分别计算外部与内部距离，比只保存外部距离增加一轮变换。数据类型、缓存访问、工作区复用和并行方式，也都会影响运行速度。
可分离 EDT 在同一轴上的不同线段可以分别处理，适合按线段并行；Chamfer 在一次扫描中会利用刚刚更新过的邻居，需要保持对应的传播顺序。
因此，更完整的性能判断需要同时考虑距离精度、内外距离需求、网格规模以及处理器上的实现方式。Chamfer 的优势在于局部运算简单，可分离 EDT 的优势在于以线性计算量保留欧式度量。
11. 把两种方法放在同一张学习地图里
两种方法的前端都可以使用占据网格，后端都可以提供插值距离和梯度。它们的核心差异集中在 如何给体素中心计算距离 。
| 理解角度 | Chamfer 距离变换 | 可分离欧式距离变换 |
|---|---|---|
| 一个直观图像 | 在少量允许方向上走格子，累加步长 | 把直线距离平方拆成各轴贡献 |
| 信息怎样组织 | 邻居把当前最小距离传给下一格 | 一维候选各贡献一条抛物线，选下包络 |
| 每格数值的度量 | 由邻域和权重定义的近似距离 | 到离散障碍种子中心的欧式距离 |
| 方向特性 | 误差随方向变化，权重可调节误差分布 | 保留欧式几何中的方向关系 |
| 三维扩展 | 使用面、棱、角对应的邻居 | 增加一轮坐标轴的一维变换 |
| 内部负距离 | 可以交换种子类别，再计算并合成 | 可以交换种子类别，再计算并合成 |
| 连续位置查询 | 可采用三线性插值 | 可采用三线性插值 |
| 空间梯度 | 对插值函数求导或使用有限差分 | 对插值函数求导或使用有限差分 |
选择地图方案时，可以顺着三个问题思考：需要怎样的几何精度；机器人是否需要利用障碍内部的距离信息来调整候选；地图以怎样的大小和频率更新。
从一条完整的机器人避障链路来看，各环节的作用可以连成一句话： 传感器测到表面，占据地图组织空间，距离变换填入距离，插值提供连续查询，梯度与雅可比把环境信息转成关节优化方向。
12. 参考文献
- 占据地图与观测融合： Sebastian Thrun、Wolfram Burgard、Dieter Fox，Probabilistic Robotics，MIT Press，2005。可重点阅读 Occupancy Grid Mapping 相关章节。
- Chamfer 距离变换： Gunilla Borgefors，Distance Transformations in Digital Images，1986。讨论数字网格上的邻域距离与权重设计。
- 可分离欧式距离变换： Pedro F. Felzenszwalb、Daniel P. Huttenlocher，Distance Transforms of Sampled Functions，2012。本文的一维抛物线下包络和逐轴分解，可以沿这篇论文继续学习。
- 三维距离场综述： Mark W. Jones、J. Andreas Bærentzen、Miloš Šrámek，3D Distance Fields: A Survey of Techniques and Applications，2006。适合在理解基础概念后，了解更多距离场表示与构建方法。