机器人规划与控制研究所 ——机器人/自动驾驶规划与控制方向综合、全面、专业的平台。4万人订阅的微信大号。点击标题下蓝字“机器人规划与控制研究所”关注，我们将为您提供有价值、有深度的延伸阅读。
今天是农历腊月二十九，正值除夕。在这个万家灯火辞旧迎新的时刻，祝屏幕前的每一位开发者朋友：
新年快乐，马到成功，心想事成！
非常感谢大家一年多来的陪伴与支持。在新的一年里，我将继续再接再厉，坚持深耕机器人算法领域，持续为大家带来更有价值的技术分享与优质开源项目。
今天为大家奉上一份“除夕技术礼包”：以机器人为中心（Robo-centric）的欧式距离场——RC-ESDF 的 C++ 工程实现。
关于该算法的数学原理与理论背景，大家可以回顾我之前的文章： 👉 【机器人轨迹优化算法】RC-ESDF：考虑任意机器人轮廓的欧式距离场快速计算
🚀 GitHub 开源地址
本项目已在 GitHub 同步开源，欢迎大家 Star 与 Fork： 🔗 https://github.com/JackJu-HIT/RC-ESDF-2D
项目构建非常简单，仅依赖 Eigen 核心库，OpenCV 为可选依赖（仅用于结果的可视化诊断）。
💡 为什么要做这个工作？
关注本公众号的老粉丝应该了解，我始终坚持“边学习、边撸码、边分享”的节奏，核心宗旨只有一个：
以工程落地为唯一目标。
去年我基于浙大高飞老师 FAST-Lab 团队开源的 Ego-Planner 和 Fast-Planner 进行了二次开发。原始算法在轨迹优化性能上确实表现强悍，不仅实时性极高，而且支持在低算力嵌入式平台上流畅运行。
但在实际场景中，注意到：原始算法在优化时并未充分考虑机器人自身的复杂轮廓。
对于户外无人车或复杂环境下的移动机器人，考虑车体轮廓（Footprint）去做轨迹优化是确保安全性的必要条件。因此，才有了这次 RC-ESDF 的工程实现：
- 适配 Fast-Planner：RC-ESDF 可以无缝集成，因为其障碍物约束本质上就是基于 ESDF 场的。
- 增强 Ego-Planner：Ego-Planner 原生不需要梯度信息（仅需碰撞检测）。我们可以调用 RC-ESDF 的最近障碍物接口，快速判断车体轮廓是否发生碰撞，提升碰撞检查的精度。
- 优化 TEB 算法：对于目前主流的 TEB 算法，RC-ESDF 能有效解决“随着环境障碍物数量增加，g2o 优化耗时呈线性剧增”的痛点，加速优化收敛。
以上这些集成工作将是我下一步升级的目标，后续我会继续在 GitHub 持续更新，并同步公众号的技术心得。
⚠️ 避坑指南
需要特别指出的是，由于 RC-ESDF 是在机器人中心坐标系（Body Frame）下构建的，因此在使用时：
- 必须将全局坐标下的障碍物转换到 Body Frame。
- 这就要求在优化每一个轨迹点时，必须具备该点的 坐标以及航向角 。
本次开源的代码专注于机器人坐标系下的 ESDF 核心算法实现，暂不涉及复杂的坐标变换逻辑。
📊 可视化效果演示
首先来看一下 RC-ESDF 的可视化运行结果：
通过内置的 visualizeEsdf() 函数，您可以清晰地观察：
- 🟢 绿色区域: 机器人外部安全区 (dist>0)。
- ⚪ 白色箭头: 解析梯度向量 ∇D（始终指向最短路径脱离碰撞的方向）。
最后再次祝大家除夕快乐！希望这份开源代码能为你的春节假期增添一份“极客”的乐趣。新的一年，我们代码见！
因个人水平有限，错误在所难免，如有错误，请帮忙指出。
2026年2月16日 9:00:01
Jack Ju 于丹东
核心代码接口如下：
* @file rc_esdf.h * @brief RC-ESDF: Robo-Centric 2D Signed Distance Field. * @author juchunyu * @date 2026-02-15 09:00:01 * @copyright Copyright (c) 2025-2026 Institute of Robotics Planning and Control (IRPC). * All rights reserved. * This library provides real-time distance and gradient queries for * robot-centric navigation and collision avoidance. */#ifndef RC_ESDF_H#define RC_ESDF_H#include #include #include #include #include #include * @brief Robo-Centric ESDF (2D Version for Ground Robots) * 存储方式：一维数组模拟二维网格 * 坐标系：Robot Body Frame (原点通常在机器人中心) * 约定：内部距离为负，外部距离为 0 (Paper setting) */class RcEsdfMap {public: RcEsdfMap() = default; * @brief 初始化地图参数 * @param width_m 地图物理宽度 (米) * @param height_m 地图物理高度 (米) * @param resolution 分辨率 (米/像素) */ void initialize(double width_m, double height_m, double resolution); * @brief (离线阶段) 根据多边形生成 ESDF * 简单的暴力生成法，实际使用时只在程序启动时跑一次 * @param polygon 机器人的顶点列表 (按顺序，Body Frame) */ void generateFromPolygon(const std::vector<:vector2d>& polygon); * @brief (在线阶段) 查询距离和梯度 * 使用双线性插值 (Bilinear Interpolation) * * @param pos_body [输入] 障碍物点在 Body Frame 下的坐标 * @param dist [输出] 距离 (内部为负，外部为0) * @param grad [输出] 梯度 (指向距离增加的方向，即指向体外) * @return true 如果点在地图范围内, false 如果点在地图范围外 */ bool query
(const Eigen::Vector2d& pos_body, double& dist, Eigen::Vector2d& grad) const; void visualizeEsdf(const std::vector<:vector2d>& footprint);private: inline void posToGrid(const Eigen::Vector2d& pos, double& gx, double& gy) const { gx = (pos.x() - origin_x_) / resolution_; gy = (pos.y() - origin_y_) / resolution_; } inline float getRaw(int x, int y) const { if (x 0 || x >= grid_size_x_ || y 0 || y >= grid_size_y_) return 0.0f; return data_[y * grid_size_x_ + x]; } double pointToSegmentDistSq(const Eigen::Vector2d& p, const Eigen::Vector2d& v, const Eigen::Vector2d& w); bool isPointInPolygon(const Eigen::Vector2d& p, const std::vector<:vector2d>& poly); double resolution_; double width_m_, height_m_; double origin_x_, origin_y_; int grid_size_x_, grid_size_y_;
std::vector<float> data_; };#endif
一、 核心设计思想
- 地图的坐标原点 始终在机器人的中心。这意味着无论机器人在全局地图中移动到哪里，避障计算都在局部坐标系下进行，解耦了全局复杂的地图更新。
符号距离场 (Signed Distance Field)：- 内部为负值：表示障碍物侵入了机器人轮廓，数值代表侵入深度。
- 外部为正值：表示安全区域，数值代表离轮廓的最短距离。
解析梯度 (Analytic Gradient)：- 不仅仅提供距离，还通过数学求导提供“最快离开碰撞的方向”。这对于基于梯度的优化器（如 g2o, Ceres）至关重要。
二、 核心功能模块解析
1. 初始化 (initialize)
- 网格化：根据物理宽高和分辨率计算网格数量
grid_size。 - 中心对齐：计算
origin_x/y，将左下角坐标设为 ，确保物理原点 位于网格阵列的正中心。 - 内存分配：
data_.assign 初始化一维数组来模拟二维连续空间，保证缓存友好。
2. ESDF 生成 (generateFromPolygon)
这是算法的“预计算”阶段：
- 距离计算：利用
pointToSegmentDistSq 计算该点到多边形每一条边的最短距离，取最小值。 - 判定内外：调用
isPointInPolygon（射线法）。 - 时间复杂度：。因为是局部地图，这个开销在启动时或轮廓改变时是可以接受的。
3. 在线查询 (query) —— 最核心部分
这是算法的“运行时刻”部分，要求极高的速度：
- 双线性插值 (Bilinear Interpolation)：
- 由于 ESDF 值只存储在离散的格点中心，为了获得平滑的距离值，查询点会根据周围 4 个格点的值进行加权平均。
gx - 0.5 的修正：这是为了将物理坐标对齐到格点中心（因为格子中心才是存储数据的地方）。
- 通过对双线性插值公式分别对 (x方向) 和 (y方向) 求偏导。
grad.x() = d_alpha / resolution_。- 这个梯度向量始终指向距离增加最快的方向（即指向机器人外部）。
4. 可视化诊断 (visualizeEsdf)
- 色彩映射：利用
max_inner 和 max_outer 独立归一化。 -
精度补偿：使用了
std::round 处理像素坐标转换，避免了浮点数转整数时直接截断导致的“差一个像素”的问题。 - 对齐检查：在图像上画出物理轮廓（黄色），开发者可以通过黄色框是否压在红绿交界线上来判断算法精度。
三、 数学辅助函数解析
- 点到线段距离 (
pointToSegmentDistSq)：
- 限制 ，确保投影点在线段内，处理了点到顶点还是到边的逻辑。
点在多边形内判定 (isPointInPolygon)：- 采用了经典的射线法 (Even-Odd Rule)。
- 通过统计从点发出的射线穿过多边形边的次数来判定：奇数为内，偶数为外。
在 RC-ESDF 代码中，最核心的算法实现集中在 query 函数中。它通过双线性插值（Bilinear Interpolation）将离散的网格数据转换为连续的物理场，并推导出解析梯度（Analytic Gradient）。以下是关于距离和梯度计算的具体数学逻辑解析：
1. 坐标映射：从物理空间到网格空间
首先，查询点 是在机器人 Body 系下的物理坐标。
- 中心对齐补偿： 由于网格数据（ 等）是存储在格子的中心点上的，为了进行插值，我们将坐标系向左下偏移 个单位：
- 确定邻域： 找到 左下角的整数索引 ，以及相对于该格子的偏移量 ：
2. 距离计算：双线性插值
为了得到平滑的距离场 ，我们获取查询点周围四个格点的值：
双线性插值公式：
为什么这样做？
- 如果直接取最近邻（Nearest Neighbor），距离值会在格子边缘发生跳变。
- 插值保证了距离函数在空间上是连续的，这对于轨迹优化算法的稳定性至关重要。
3. 梯度计算：解析求导
在机器人避障中，我们不仅需要知道“撞没撞”（距离），还需要知道“往哪儿推”（梯度）。梯度 是距离场对物理坐标的导数。
根据链式法则：
(1) 对 方向求导 (X轴方向距离变化率)：
固定 ，对插值公式求 的偏导：
这个公式的物理意义是：在底部和顶部边缘分别计算 X 方向的差分，再根据 Y 方向的偏移 进行线性加权。
(2) 对 方向求导 (Y轴方向距离变化率)：
固定 ，对插值公式求 的偏导：
同理，这是在左侧和右侧边缘计算 Y 方向差分后的加权结果。
(3) 最终梯度向量：
4. 算法实现的工程特性
为什么解析梯度比数值梯度好？
- 数值梯度（差分法）：需要查询两次距离（如 ），计算量翻倍。
- 解析梯度（代码中采用的方法）：在计算距离的过程中，利用已经获取的 直接通过乘加运算得出导数，几乎没有额外开销。
梯度的物理含义：
在 RC-ESDF 中：
- 如果点在机器人内部（），梯度向量 指向离碰撞最远（向外）的方向。
- 所以在避障优化中，给机器人的施加的力通常是 （如果障碍物进入了机器人内部，就把机器人往梯度增加的方向推）。
边界处理：
代码中有一行判断：
if (v00 == 0 && v10 == 0 && v01 == 0 && v11 == 0) { ... }
这是因为在 ESDF 生成逻辑中，远离机器人的地方值为正（或 0）。如果四周都是 0，说明该点处于完全安全区且不在场的作用范围内，此时强制梯度为 0，避免优化器在安全区域产生无用的随机推力。
总结
该算法模块通过双线性插值实现了从离散网格到连续空间的平滑映射，其导出的解析梯度为基于二阶或一阶导数的优化算法（如 TEB 中的 g2o 边）提供了高质量的雅可比矩阵（Jacobian）输入。
* @file main.cpp * @brief Testing RC-ESDF: Robo-Centric 2D Signed Distance Field. * @author juchunyu @qq.com> * @date 2026-02-15 10:00:01 * @copyright Copyright (c) 2025-2026 Institute of Robotics Planning and Control (IRPC). * All rights reserved.*/int main() { std::vector<Eigen::Vector2d> footprint; footprint.push_back(Eigen::Vector2d(0.5, 0.4)); footprint.push_back(Eigen::Vector2d(-0.5, 0.4)); footprint.push_back(Eigen::Vector2d(-0.5, -0.4)); footprint.push_back(Eigen::Vector2d(0.5, -0.4));
RcEsdfMap rc_map; rc_map.initialize(10.0, 10.0, 0.1); rc_map.generateFromPolygon(footprint); std::vector<Eigen::Vector2d> obs_points_body; for(int i = 0; i 100;i++) { obs_points_body.push_back(Eigen::Vector2d(0.0, 0.0)); obs_points_body.push_back(Eigen::Vector2d(0.4, 0.2)); obs_points_body.push_back(Eigen::Vector2d(0.6, 0.6)); obs_points_body.push_back(Eigen::Vector2d(1, 1)); } std::cout << obs_points_body.size() << std::endl; std::cout << std::endl;
auto start_time = std::chrono::high_resolution_clock::now(); for (const auto& p : obs_points_body) { double dist; Eigen::Vector2d grad;
bool in_box = rc_map.query(p, dist, grad); std::cout << p.x() y() if (!in_box) { std::cout Out of Box (Safe)" << std::endl; } else { std::cout Dist: " << dist << grad.x() y() endl;
if (dist 0) { std::cout << -grad.x() << -grad.y() endl; } } } auto end_time = std::chrono::high_resolution_clock::now();
std::chrono::duration<double, std::milli> elapsed_ms = end_time - start_time; std::cout << obs_points_body.size() << std::endl; std::cout << std::endl; std::cout << elapsed_ms.count() endl; std::cout << elapsed_ms.count() / obs_points_body.size() endl; rc_map.visualizeEsdf(footprint); return 0;}
这段 main.cpp是 RC-ESDF（以机器人为中心的二维符号距离场）库的测试程序。它模拟了在一个 10m x 10m 的局部环境中，如何根据机器人形状生成距离场，并高频查询障碍物点的距离和梯度。
以下是代码各个部分的详细解析：
1. 机器人形状定义 (Robot Footprint)
std::vector<:vector2d> footprint;
footprint.push_back(Eigen::Vector2d(0.5, 0.4));
footprint.push_back(Eigen::Vector2d(-0.5, 0.4));
footprint.push_back(Eigen::Vector2d(-0.5, -0.4));
footprint.push_back(Eigen::Vector2d(0.5, -0.4));
- 功能：定义机器人在其局部坐标系（Body Frame）下的物理轮廓。
- 数学意义：这里定义了一个中心在 ，长为 （方向），宽为 （方向）的矩形。
- 应用：ESDF 将计算空间中任意点到这个多边形边界的最短距离。
2. 初始化与场生成 (Initialization & Generation)
RcEsdfMap rc_map;
rc_map.initialize(10.0, 10.0, 0.1);
rc_map.generateFromPolygon(footprint);
initialize：创建一个 的网格，分辨率为 。这意味着内部维护了一个 的数据矩阵。generateFromPolygon：这是预计算阶段。程序遍历网格中的每一个像素，计算它在多边形内还是外，并计算到边缘的最短距离。
3. 测试数据构造
std
::vector<:vector2d> obs_points_body;
for(int i = 0; i 100;i++) {
obs_points_body.push_back(Eigen::Vector2d(0.0, 0.0)); // 深度碰撞点
obs_points_body.push_back(Eigen::Vector2d(0.4, 0.2)); // 边缘内碰撞点
obs_points_body.push_back(Eigen::Vector2d(0.6, 0.6)); // 外部安全点
obs_points_body.push_back(Eigen::Vector2d(1, 1)); // 外部安全点
}
- 目的：模拟真实场景中传感器（如激光雷达）扫到的障碍物点。通过循环 100 次（总计 400 个点），可以更准确地测量算法的平均查询耗时。
4. 核心查询循环与性能计时
auto start_time = std::chrono::high_resolution_clock::now();
for (constauto& p : obs_points_body) {
double dist;
Eigen::Vector2d grad;
bool in_box = rc_map.query(p, dist, grad);
// ... 后续逻辑
}
auto end_time = std::chrono::high_resolution_clock::now();
query(p, dist, grad)：这是本库最核心的函数。-
- 原理：它不在网格上做暴力搜索，而是直接通过双线性插值读取预计算好的 ESDF 值，因此时间复杂度是 。
- 计时逻辑：利用
std::chrono 计算总查询耗时。在机器人避障算法（如 TEB）中，每秒可能需要进行成千上万次查询，因此单次查询必须控制在微秒级。
5. 碰撞逻辑处理
if (dist 0) {
std::cout <" [COLLISION] Push robot direction: ("
<< -grad.x() <", " << -grad.y() <")" <std::endl;
}
dist < 0：表示障碍物点位于机器人多边形内部，即发生了碰撞。- 梯度
grad：梯度向量 指向距离增加最快的方向（即离开碰撞的方向）。 grad：如果障碍物点在机器人坐标系下，我们需要将机器人“推离”障碍物，推力的方向就是梯度的反方向（针对障碍物点而言）。
6. 统计输出与可视化
std::cout <"Avg Time per Point: " << elapsed_ms.count() / obs_points_body.size() <" ms" <std::endl;
rc_map.visualizeEsdf(footprint);
- 性能指标：输出单点查询的平均耗时。根据之前的测试，这个值通常在 0.002ms (2μs) 左右，极度高效。
visualizeEsdf：调用 OpenCV 窗口，将数学上的距离场转换成直观的图像：
总结
这个 main.cpp 展示了一个完整的
“建模 -> 生成 -> 查询 -> 评价”的闭环。它证明了该算法在处理任意形状（多边形）机器人时的准确性和高效性，是后续集成到自动驾驶导航系统（如轨迹优化器）的基础。
我建立了个轨迹优化与运动控制方向交流群，欢迎各位同行专家加入交流群，不收费哈，欢迎添加作者的微信，加入交流群。
最后再次祝大家除夕快乐！希望这份开源代码能为你的春节假期增添一份“极客”的乐趣。新的一年，我们代码见！
