RC-ESDF 详解：以机器人为中心的欧几里得有符号距离场

无宜

195人浏览 · 2026-04-13 00:31:30

无宜 · 2026-04-13 00:31:30 发布

RC-ESDF 详解：以机器人为中心的欧几里得有符号距离场

来源：浙大高飞团队论文
核心思想：将复杂的"机器人形状"问题从在线计算中剥离，通过离线预计算实现极致的碰撞检测速度（2.4μs 级查询）

1. 什么是 ESDF？

ESDF（Euclidean Signed Distance Field，欧几里得有符号距离场） 是一种环境地图表示方法：

将空间离散化为网格
每个点存储到最近障碍物的欧几里得距离
正距离 = 在障碍物外部（安全）
负距离 = 在障碍物内部（碰撞）
同时存储梯度方向（最快的逃离方向）

ESDF 使得规划器不仅能判断"能不能走"，还能精确掌握"离障碍物多远"以及"该往哪躲"。

传统 ESDF 的构建流程

传感器数据 → 占据栅格地图 → 距离变换算法 → ESDF 距离场

常用算法包括 BFS 广度优先搜索、FIESTA 增量更新、Voxblox（TSDF 转 ESDF） 等。

2. 什么是 RC-ESDF？

RC-ESDF（Robo-centric ESDF，以机器人为中心的 ESDF） 是高飞团队提出的创新方法。其核心思路是：

将"机器人在环境中的碰撞检测"这个动态问题，转化为"在机器人坐标系下，机器人形状与静态障碍物场的距离查询"这个静态问题。

一句话理解

RC-ESDF 在机器人本体坐标系下，预先构建一张描述机器人自身几何形状的"自我距离场"地图。这张地图记录了机器人身上每个点到其自身"皮肤"（外轮廓）的有符号距离和梯度。

它不包含任何环境信息，是一张纯静态的、与外界无关的自我描述地图。

3. RC-ESDF 的核心原理

3.1 构建过程（离线预计算）

机器人 CAD 模型 → 体素化（本体坐标系） → 距离变换 → RC-ESDF 地图

体素化与边界标记：根据机器人几何模型，在本体坐标系中创建密集网格，"皮肤"表面经过的格子标记为边界（距离=0）
内外区分：内部点标记为负，外部点标记为正
距离变换传播：计算每个点到最近边界的最短欧氏距离，内部为负值
预计算解析梯度：提前算好每个位置的梯度方向，供规划器直接调用

3.2 查询过程（在线查表）

运行时，RC-ESDF 完全不更新，只做 O(1) 复杂度的内存访问：

将机器人候选位姿变换到本体坐标系
在 RC-ESDF 查表，获取"该点到自己皮肤的距离"
同时查全局 ESDF（环境地图），获取"该点到最近障碍物的距离"
比较两者：如果障碍物距离 < 自身皮肤距离 → 碰撞

3.3 关键数值含义

值	含义
正 (+)	该点在机器人外部（安全），数值 = 到最近表面的距离
零 (0)	该点恰好位于机器人轮廓表面
负 (-)	该点在机器人内部，绝对值 = 穿透深度

3.4 在线查询的插值方法

RC-ESDF 在离线阶段构建了一个规则网格，每个网格点存储了机器人坐标系下的距离值和梯度方向。在线查询时，机器人当前位置并不恰好落在网格节点上，因此需要通过插值快速估算中间值。

📐 双线性插值原理

双线性插值是一种在二维网格上估算任意点函数值的常用方法。基本思想是：先在一个方向（x 轴）上做两次线性插值，再在另一个方向（y 轴）上做一次线性插值，从而得到平滑的曲面拟合结果。

假设已知网格四个角点 $Q_{11}=(x_1, y_1)$ 、 $Q_{12}=(x_1, y_2)$ 、 $Q_{21}=(x_2, y_1)$ 、 $Q_{22}=(x_2, y_2)$ 上的函数值。对于任意点 $P = (x, y)$ ，其中 $x_1 \le x \le x_2, y_1 \le y \le y_2$ ：

x 方向线性插值：在 $y = y_1$ 线上，由 $Q_{11}$ 和 $Q_{21}$ 插值得到 $R_1$ ：
$f(R_1) \approx \frac{x_2 - x}{x_2 - x_1} f(Q_{11}) + \frac{x - x_1}{x_2 - x_1} f(Q_{21})$
在 $y = y_2$ 线上，由 $Q_{12}$ 和 $Q_{22}$ 插值得到 $R_2$ ：
$f(R_2) \approx \frac{x_2 - x}{x_2 - x_1} f(Q_{12}) + \frac{x - x_1}{x_2 - x_1} f(Q_{22})$
y 方向线性插值：在 $R_1$ 和 $R_2$ 之间沿 y 方向插值得到 $P$ ：
$\approx \frac{y_2 - y}{y_2 - y_1} f(R_1) + \frac{y - y_1}{y_2 - y_1} f(R_2)$

合并写成对称形式：
$\sum_{i=1}^2 \sum_{j=1}^2 w_{ij} \, f(Q_{ij})$
其中权重 $w_{ij}$ 与 $P$ 到对角点的距离成比例。

🧠 直观理解

特性	说明
线性	在每个方向上，函数值随坐标线性变化
双	在两个独立的方向上各做一次线性插值
效果	得到的是双线性曲面（网格内光滑，但二阶导数不连续）

🔧 在 RC-ESDF 中的应用

使用双线性插值的好处：

计算极快：只需几次乘加运算，时间复杂度 O(1)
连续可微：保证轨迹优化时梯度信息平滑，避免碰撞函数出现台阶突变
精度足够：对于中等分辨率的网格，插值误差远小于机器人控制精度需求

📝 与最近邻插值的对比

插值方法	速度	光滑性	精度	是否可导
最近邻	最快	阶梯状	较低	不可导
双线性	很快	连续	较高	一阶可导

在机器人运动规划这类需要梯度下降的场合，双线性插值是网格距离场查询的首选。

4. RC-ESDF 与传统 ESDF 的区别

对比维度	传统 ESDF（环境为中心）	RC-ESDF（机器人为中心）
坐标系	世界坐标系	机器人本体坐标系
构建基础	依赖环境障碍物信息	依赖机器人自身形状模型
更新方式	在线实时更新，开销大	离线预计算，在线无需更新
存储内容	空间点到最近环境障碍物的距离	机器人本体上点到最近自身表面的距离
运行时任务	在线实时计算距离场	仅坐标变换 + 查表
查询速度	取决于地图大小和算法	2.4μs 级，与地图大小无关
形状支持	凸形状可实时处理，非凸形状计算复杂	天然支持任意复杂形状（包括非凸）
梯度类型	数值梯度	解析梯度（精确指导逃离方向）

核心区别：一表 vs. 一计算器

	传统 ESDF	RC-ESDF
本质	动态计算器	静态速查表
任务	一体化：同时处理"自我形状"+“环境障碍物”	分而治之：离线处理"自我"，在线专注"环境"

5. RC-ESDF 如何与局部规划器结合？

5.1 完整碰撞检测需要两张地图

角色	全称	职责	构建方式
自我模型	RC-ESDF	描述机器人自身的几何轮廓	离线一次性预计算
世界模型	局部 ESDF	描述机器人周围的环境障碍物	在线实时增量更新

5.2 与 MPPI 等采样规划器的结合

RC-ESDF 集成到 MPPI 中，本质上是扩展原有的 ObstaclesCritic 模块：

// 原有方式（基于 Costmap）
for (轨迹上每个采样点) {
    cost = costmap->getCost(x, y);  // 粗糙的膨胀代价
}

// RC-ESDF 方式（精确距离）
for (轨迹上每个采样点) {
    // 1. 坐标变换：全局坐标 → 机器人本体坐标系
    (bx, by) = transform_to_body_frame(x, y, pose);

    // 2. 查 RC-ESDF：获取该点到自身皮肤的距离
    d_self = rcesdf->lookup(bx, by);

    // 3. 查局部 ESDF：获取该点到障碍物的距离
    d_world = local_esdf->lookup(x, y);

    // 4. 代价计算
    if (d_world < d_self) {
        cost = w * (d_self - d_world);  // 碰撞惩罚
    }
}

5.3 与不同规划器的适配

规划器	RC-ESDF 的作用
Fast-Planner	无缝替换碰撞评估模块
Ego-Planner	快速判断车体轮廓是否碰撞，替代原有的"碰撞轨迹 vs 无碰撞引导路径"比较
TEB	解决障碍物增多时优化耗时剧增的问题，加速收敛
MPPI	扩展 ObstaclesCritic，用精确距离替代膨胀代价

6. 三级碰撞检测与指数衰减软惩罚框架

m = φ_self(x_body) - d_obs(x_world)

其中：

$\phi_{self} \ge 0$ ：RC-ESDF 查询得到的机器人外部点到自身皮肤的距离
$d_{obs} \ge 0$ ：全局/局部 ESDF 查询得到的点到最近障碍物的距离

物理意义：

条件	含义
$m > 0$	皮肤离障碍物还有 $m$ 的余量 → 安全
$m = 0$	皮肤刚好碰到障碍物
$m < 0$	皮肤已被穿透，穿透深度为 $

6.2 三级安全区间（基于 $m$ ）

设定两个正阈值 $M_1 > M_2 > 0$ ：

区间	条件	含义
安全区	$\ge M_1$	足够安全，代价为 0
警告区	$M_2 \le m < M_1$	接近障碍物，施加轻度软惩罚
危险区	$\le m < M_2$	非常接近或即将碰撞，施加强指数惩罚
碰撞区	$m < 0$	已穿透，施加极大常数惩罚或截断

6.3 指数衰减软惩罚函数

$J_{collision} = w \cdot \begin{cases} 0, & m \ge M_1 \\ e^{-\lambda (m - M_1)} - e^{-\lambda (M_2 - M_1)}, & M_2 \le m < M_1 \\ e^{-\lambda (m - M_1)} - e^{-\lambda (M_2 - M_1)} + \alpha \cdot (M_2 - m), & 0 \le m < M_2 \\ C_{max}, & m < 0 \end{cases}$

参数说明：

$\lambda > 0$ ：衰减速率，越大则靠近时惩罚上升越快
$\alpha > 0$ ：危险区内的线性附加项，确保进入危险区后惩罚快速抬升
$C_{max}$ ：极大常数，表示绝对不可接受的碰撞状态

6.4 为什么比单纯用 $d_{obs}$ 更好？

利用 RC-ESDF 的精确形状信息：不同部位（如尖角、凸起）的 $\phi_{self}$ 不同，因此同样的环境距离 $d_{obs}$ 在不同部位产生的安全裕度 $m$ 不同，惩罚自然不同 → 真正实现了全身碰撞评估
指数衰减的连续性：从安全区到警告区再到危险区，代价函数连续可导（碰撞区可平滑化），有利于 MPPI 的梯度/加权采样优化
动态适应性：结合预测的障碍物位置，可以提前减小 $M_1, M_2$ 或增大 $\lambda$ ，实现预测性避障

6.5 简历/技术文档描述参考

将 RC-ESDF 预计算的自我距离场与局部 ESDF 环境距离场融合，定义安全裕度 $\phi_{self} - d_{obs}$ ，在此基础上构建三级碰撞检测区间（安全/警告/危险）与指数衰减软惩罚代价函数，并串联动态障碍物预测链路，有效提升了动态干扰场景下全身绕障的连续性与规划鲁棒性。

该设计使得 MPPI 在复杂形状机器人的实时避障中，单次碰撞查询仅需微秒级，且惩罚函数平滑可微，保证了轨迹优化的稳定性。

7. 常见疑问解答

Q1：RC-ESDF 需要提前构建整个环境地图吗？

不需要。RC-ESDF 本身只包含"自我"，与环境完全无关。

真正需要环境信息的是局部 ESDF（或全局 ESDF），它负责实时感知"周围有什么"。

Q2：RC-ESDF 建好之后还需要更新吗？

不需要。RC-ESDF 是纯离线预计算的静态表，只要机器人形状不变，就永远不需要更新。

Q3：为什么有了全局 ESDF 还需要 RC-ESDF？

全局 ESDF 能告诉你"离障碍物多远"，但**不知道机器人自己的皮肤在哪里"。RC-ESDF 通过离线预计算，把"机器人形状"这个问题从在线计算中彻底剥离，换来了 2.4μs 级的极致查询速度。

Q4：突然出现动态障碍物怎么办？

**环境感知系统（局部 ESDF）**会实时增量更新（如 FIESTA 算法）。RC-ESDF 本身不需要任何改动，规划器拿到新的环境信息后，重新评估候选轨迹即可。

Q5：RC-ESDF 对复杂形状的优势在哪里？

对于非圆形机器人（如多旋翼无人机、机械臂），传统方法只能用圆形/矩形近似。RC-ESDF 天然支持任意复杂形状（包括非凸形状），因为它提前用 CAD 模型构建好了完整的自我距离场。

Q6：RC-ESDF 的"排斥力"是怎么工作的？

RC-ESDF 本身不产生排斥力，它提供的是指向机器人外部的解析梯度（最快逃离方向）。这个梯度与全局 ESDF 的梯度联合，指导优化器调整机器人位姿，从而产生"推开"效果。

总结

	传统 ESDF	RC-ESDF
核心思想	一体化构建，环境中心	分而治之，机器人为中心
自我形状	在线近似（如圆形）	离线精确预计算
在线计算	重（实时更新）	轻（仅查表）
查询速度	取决于地图大小	2.4μs 固定
形状支持	简单形状	任意复杂形状
架构	一张图搞定一切	两张图协同：自我离线表 + 环境在线图