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

来源：浙大高飞团队论文
核心思想：将复杂的"机器人形状"问题从在线计算中剥离，通过离线预计算实现极致的碰撞检测速度（2.4μs 级查询）
大佬的开源地址：https://github.com/JackJu-HIT/RC-ESDF-2D

---

目录

• 1. 什么是 ESDF？
• 2. 什么是 RC-ESDF？
• 3. RC-ESDF 的核心原理
• 4. RC-ESDF 与传统 ESDF 的区别
• 5. RC-ESDF 如何与局部规划器结合？
• 6. 碰撞惩罚计算方法
• 7. 常见疑问解答
• 附录：梯度如何用于轨迹优化

---

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 是在机器人本体坐标系下离线预构建的 ESDF 地图，准确描述机器人的形状（包括非凸形状）：

• 机器人内部：ESDF 值为负（表示穿透深度）
• 机器人表面：ESDF 值为零
• 机器人外部：ESDF 值为正（表示到最近表面的距离）

这张地图记录了机器人身上每个点到其自身"皮肤"（外轮廓）的有符号距离和梯度。它不包含任何环境信息，是一张纯静态的、与外界无关的自我描述地图。

关键在于：RC-ESDF 只与机器人自身形状有关，与环境无关。一旦机器人形状确定，RC-ESDF 就固定了，不需要实时更新。

---

3. RC-ESDF 的核心原理

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

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

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

3.2 查询过程（在线查表）

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

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

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$：

1. 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})$$

2. y 方向线性插值：在 $R_1$ 和 $R_2$ 之间沿 y 方向插值得到 $P$：
   $$f(P)\approx \frac{y_2-y}{y_2-y_1}f(R_1)+\frac{y-y_1}{y_2-y_1}f(R_2)$$

合并写成对称形式：
$$f(P)=\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.2 碰撞检测如何用 RC-ESDF

论文中的碰撞检测流程（参见 Fig.3 和 Fig.4）：

核心思想：只需要 RC-ESDF 一张地图，将环境中的障碍物点变换到机器人本体坐标系下，查询这些点相对于机器人"皮肤"的距离。

步骤1：全局系下的碰撞点筛选

机器人位姿为 $({\mathbf{p}}_k,{\psi }_k)$，使用 AABB（轴对齐包围盒）算法在全局坐标系下划出一个矩形区域，区域内的障碍物点被定义为碰撞点 ${\mathbf{q}}_{w,i}$。

说明：障碍物点直接来自传感器数据（如激光雷达、深度相机）或占据栅格地图，不需要构建环境 ESDF。

步骤2：变换到机器人本体坐标系

将所有障碍物点从世界坐标系变换到机器人本体坐标系下：

$${\mathbf{q}}_{b,i}=R_k^{-1}({\mathbf{q}}_{w,i}-{\mathbf{p}}_k)$$

其中 $R_k$ 是机器人从本体坐标系到世界坐标系的旋转矩阵。

步骤3：查 RC-ESDF 得到距离值

在 RC-ESDF 中查询每个障碍物点 ${\mathbf{q}}_{b,i}$ 对应的距离值 $H_{i,k}$：

• $H_{i,k}<0$：该障碍物点在机器人内部 → 发生碰撞
• $H_{i,k}=0$：该障碍物点恰好在机器人表面 → 接触
• $H_{i,k}>0$：该障碍物点在机器人外部 → 安全

步骤4：计算总碰撞惩罚

将所有障碍物点的 RC-ESDF 值求和，得到该位姿下的总碰撞惩罚值：

$$d_{p_k,{\psi }_k}=\sum _{i=1}^{M_d}{H}_{i,k}$$

其中 $H_{i,k}$ 是第 $i$ 个障碍物点在 RC-ESDF 中查询到的有符号距离值。

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

RC-ESDF 集成到 MPPI 中，本质上是替换原有的 ObstaclesCritic 模块，用精确的机器人形状碰撞检测替代粗糙的代价地图：

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

// RC-ESDF 方式（精确距离）
for (轨迹上每个候选位姿) {
    cost = 0;
    
    // 1. 在全局坐标系下用 AABB 筛选障碍物点
    obstacle_points = getObstaclesInAABB(pose, robot_bbox);
    
    // 2. 对每个障碍物点进行碰撞检测
    for (每个障碍物点 q_w) {
        // 3. 坐标变换：全局坐标 → 机器人本体坐标系
        q_b = transform_to_body_frame(q_w, pose);
        
        // 4. 查 RC-ESDF：获取该障碍物点相对于机器人皮肤的距离
        H = rcesdf->lookup(q_b);
        
        // 5. 累加碰撞惩罚（H < 0 表示碰撞）
        if (H < 0) {
            cost += abs(H);  // 穿透深度作为惩罚
        }
    }
}

关键区别：

• 不需要构建环境 ESDF 地图
• 直接使用传感器感知到的障碍物点
• 通过 RC-ESDF 精确判断障碍物点是否在机器人形状内部

5.4 与不同规划器的适配

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

5.5 不同规划器类型与 RC-ESDF 输出的对应关系

RC-ESDF 提供两种核心输出：距离值和梯度。不同类型的规划器对这两者的需求不同，取决于其底层优化范式。

RC-ESDF 的输出内容

输出	含义	坐标系	用途
距离值 $d_{self}$	查询点到机器人自身皮肤的距离	本体坐标系	碰撞检测、代价评估
梯度 $\nabla {\varphi }_{self}$	指向机器人外部的最快逃离方向	本体坐标系	解析导数计算、优化方向指导

规划器类型分类

规划器类型	代表算法	优化范式	需要 RC-ESDF 的什么？	为什么？
采样-based 规划器	MPPI、RRT*	零阶优化：大量采样轨迹，评估每条轨迹的标量代价，加权平均得到最优解	仅距离值	不需要梯度信息，通过标量代价函数隐式探索避碰轨迹
梯度-based 优化器	Fast-Planner (B-spline)、EGO-Planner	一阶优化：沿代价函数的负梯度方向迭代更新控制变量	距离值 + 梯度	需要解析地计算代价函数对控制点的导数，高效指导优化方向
非线性优化器	TEB、MPC	基于梯度的数值优化：构建非线性约束问题，使用雅可比/海森矩阵求解	距离值 + 梯度	需要梯度构建雅可比矩阵，加速非线性求解器收敛

为什么 MPPI 不使用梯度？

MPPI（Model Predictive Path Integral）的工作流程：

// MPPI 伪代码
for (每条采样轨迹) {
    cost = 0;
    for (轨迹上每个时间点) {
        // 状态前向传播
        x = dynamics(x, u);
        
        // 代价评估（包括碰撞代价）
        cost += collision_cost(x);  // 只需要标量代价
    }
    // 根据总代价加权平均，更新控制序列
    optimal_control = weighted_average(cost, control_samples);
}

原因	详细解释
算法范式不匹配	MPPI 是零阶优化方法，通过采样和加权平均寻找最优解，而非沿梯度方向迭代下降
坐标系转换开销	RC-ESDF 的梯度在本体坐标系下，而 MPPI 的采样轨迹在全局坐标系下，用于优化需要复杂的雅可比变换
梯度的局部性	RC-ESDF 的梯度只描述"当前位姿下某点被侵入时的逃离方向"，而 MPPI 需要评估整条轨迹的全局代价
采样的隐式探索	MPPI 通过大量采样已经隐式探索了不同方向的轨迹，不需要显式梯度来引导搜索

正确的用法对应

┌─────────────────────────────────────────────────────────┐
│                    RC-ESDF  outputs                      │
├──────────────────┬──────────────────────────────────────┤
│ 距离值 H         │ → MPPI / 采样规划器：用于标量代价评估  │
│ 梯度 ∇H          │ → 梯度优化器：用于解析导数计算         │
└──────────────────┴──────────────────────────────────────┘

• MPPI 中的 RC-ESDF：

  double evaluateCollisionCost(Pose pose) {
      double cost = 0;
      
      // 1. 获取当前位姿周围的障碍物点（来自传感器）
      auto obstacles = getObstaclePoints(pose);
      
      // 2. 对每个障碍物点进行碰撞检测
      for (auto& q_w : obstacles) {
          // 3. 变换到本体坐标系
          auto q_b = transformToBodyFrame(q_w, pose);
          
          // 4. 查询 RC-ESDF
          double H = rcesdf->lookup(q_b);
          
          // 5. 累加碰撞代价（H < 0 表示障碍物在机器人内部）
          if (H < 0) {
              cost += -H;  // 穿透深度作为惩罚
          }
      }
      
      return cost;
  }
  

  MPPI 拿到标量代价后，自动在采样过程中隐式探索避碰轨迹。

• 梯度优化器中的 RC-ESDF：

  void optimize_trajectory() {
      for (迭代) {
          // 1. 计算碰撞代价
          cost = collision_cost(traj);
          
          // 2. 计算代价对控制点的梯度（使用 RC-ESDF 梯度）
          grad = ∂cost/∂control_points;
          
          // 3. 沿负梯度方向更新
          control_points -= learning_rate * grad;
      }
  }
  

  RC-ESDF 的梯度用于解析地计算代价函数的导数，高效指导优化方向。

---

附录：梯度如何用于轨迹优化

RC-ESDF 提供解析梯度。由于 RC-ESDF 是以网格地图形式存储的，通过双线性插值可以同时获得 ESDF 值和梯度信息。

A.1 链式求导过程

论文推导了碰撞代价 $F_c({\mathbf{p}}_k,{\psi }_k)$ 对位置和偏航的梯度。首先定义碰撞代价：

$$F_c({\mathbf{p}}_k,{\psi }_k)=d_{p_k,{\psi }_k}^2={\left(\sum _{i=1}^{M_d}{H}_{i,k}\right)}^2$$

其中 $H_{i,k}$ 是第 $i$ 个碰撞点在 RC-ESDF 中查询到的有符号距离值。

A.2 对位置 ${\mathbf{p}}_k$ 的梯度

$$\frac{\partial H_{i,k}}{\partial {\mathbf{p}}_k}=-R_k\frac{\partial H_{i,k}}{\partial {\mathbf{q}}_{b,i}}$$

A.3 对偏航角 ${\psi }_k$ 的梯度

$$\frac{\partial H_{i,k}}{\partial {\psi }_k}=({\mathbf{q}}_{w,i}-{\mathbf{p}}_k{)}^T\left[\begin{array}{cc}-\sin {\psi }_k& \cos {\psi }_k\\ -\cos {\psi }_k& -\sin {\psi }_k\end{array}\right]\frac{\partial H_{i,k}}{\partial {\mathbf{q}}_{b,i}}$$

其中 $\frac{\partial H_{i,k}}{\partial {\mathbf{q}}_{b,i}}$ 是 RC-ESDF 在 ${\mathbf{q}}_{b,i}$ 处的空间梯度，通过双线性插值直接得到。

A.4 碰撞代价的最终梯度

最终，碰撞代价 $F_c$ 对位置和偏航的梯度为：

$$\frac{\partial F_c}{\partial {\mathbf{p}}_k}=2d_{p_k,{\psi }_k}\sum _{i=1}^{M_d}\frac{\partial H_{i,k}}{\partial {\mathbf{p}}_k}$$

$$\frac{\partial F_c}{\partial {\psi }_k}=2d_{p_k,{\psi }_k}\sum _{i=1}^{M_d}\frac{\partial H_{i,k}}{\partial {\psi }_k}$$

A.5 轨迹优化问题的完整形式

轨迹表示

论文使用均匀 B 样条曲线参数化轨迹，同时优化位置控制点 ${\mathbf{Q}}_k$ 和偏航控制点 ${\Psi }_k$。

约束点的选取

根据三阶 B 样条的性质，约束点 ${\mathbf{p}}_k$ 和 ${\psi }_k$ 由相邻控制点的加权平均得到：

$${\mathbf{p}}_k=\frac{1}{6}({\mathbf{Q}}_k+4{\mathbf{Q}}_{k+1}+{\mathbf{Q}}_{k+2})$$

$${\psi }_k=\frac{1}{6}({\Psi }_k+4{\Psi }_{k+1}+{\Psi }_{k+2})$$

完整的目标函数

$$\underset{\mathbf{Q},\Psi }{\min }J={\lambda }_{p_s}{J}_{p_s}+{\lambda }_{p_f}{J}_{p_f}+{\lambda }_{{\psi }_s}{J}_{{\psi }_s}+{\lambda }_{{\psi }_f}{J}_{{\psi }_f}+{\lambda }_c{J}_c$$

其中：

• $J_{p_s},J_{p_f}$：位置平滑性和可行性惩罚
• $J_{{\psi }_s},J_{{\psi }_f}$：偏航角平滑性和可行性惩罚
• $J_c$：碰撞惩罚项（即上文计算的 $F_c$）
• $\lambda$ 为各项的权重系数

数值优化方法

论文采用 L-BFGS 求解数值优化问题。由于 ESDF 值通过线性插值获取，其梯度相对机器人自身运动并不光滑，因此使用了 Lewis-Overton 线搜索来支持非光滑函数。

---

6. 碰撞惩罚计算方法

6.1 基本原理

RC-ESDF 的碰撞检测直接查询障碍物点在本体坐标系下的有符号距离：

$$d_{p_k,{\psi }_k}=\sum _{i=1}^{M_d}{H}_{i,k}$$

其中 $H_{i,k}$ 是第 $i$ 个障碍物点在 RC-ESDF 中查询到的有符号距离值。

物理意义：

条件	含义
$H_{i,k}>0$	障碍物点在机器人外部 → 安全
$H_{i,k}=0$	障碍物点恰好在机器人表面 → 接触
$H_{i,k}<0$	障碍物点在机器人内部，穿透深度为 $

6.2 碰撞惩罚函数

对于每个障碍物点，碰撞惩罚可以定义为：

$$c_i=\{\begin{array}{ll}0,& H_{i,k}\ge 0\\ |H_{i,k}|,& H_{i,k}<0\end{array}$$

总碰撞代价为所有障碍物点惩罚的累加：

$$J_{collision}=\sum _{i=1}^{M_d}{c}_i$$

6.3 带安全裕度的软惩罚（可选）

如果需要考虑安全裕度（而不仅仅是碰撞检测），可以引入阈值 $d_{safe}$：

$$c_i=\{\begin{array}{ll}0,& H_{i,k}\ge d_{safe}\\ (d_{safe}-H_{i,k}{)}^2,& 0\le H_{i,k}<d_{safe}\\ d_{safe}^2+|H_{i,k}|,& H_{i,k}<0\end{array}$$

这样可以在接近机器人表面时就产生惩罚，保持安全距离。

6.4 RC-ESDF 碰撞检测的优势

• 精确形状支持：天然支持任意复杂形状（包括非凸形状），不需要用圆形/矩形近似
• 计算速度快：2.4μs 级查询速度，与地图大小无关
• 解析梯度：通过双线性插值可同时获得距离值和梯度，便于梯度-based 优化
• 全身碰撞评估：精确判断障碍物点是否在机器人形状内部，实现全身碰撞检测

---

7. 常见疑问解答

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

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

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

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

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

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

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

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

传感器实时检测。RC-ESDF 本身不需要任何改动，当传感器（如激光雷达、深度相机）检测到新的障碍物点时，规划器将这些新的障碍物点变换到机器人本体坐标系下，重新查询 RC-ESDF 评估候选轨迹即可。

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

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

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

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

---

总结

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

RC-ESDF 的设计哲学：用离线预计算换取在线极致速度，用分而治之换取任意形状支持。