EGO-Planner技术解析（一）——无ESDF的轨迹优化与动力学可行性

最新推荐文章于 2026-05-15 06:26:37 发布

原创

最新推荐文章于 2026-05-15 06:26:37 发布 · 523 阅读

1. 为什么EGO-Planner要抛弃ESDF？

在无人机或者移动机器人的轨迹规划领域，ESDF（欧几里得符号距离场）曾经是许多主流规划器（比如大名鼎鼎的Fast-Planner）的“标配”。它就像一个三维空间里的“先知地图”，能告诉你地图上任意一点到最近障碍物的精确距离和方向。规划器利用这个信息，就能轻松地让轨迹“远离”障碍物，听起来很完美，对吧？

但我在实际项目里用久了，发现ESDF有个挺头疼的问题：太“重”了。每次环境感知数据（比如激光雷达点云）一更新，哪怕只是角落里多了一个小箱子，整个地图的ESDF都需要重新计算一遍。这就像为了挪动客厅里的一把椅子，你需要把整个房子的三维模型都重新测量一遍，效率非常低。更重要的是，轨迹优化其实只关心轨迹本身经过的那一小片空间，对远处障碍物的距离信息根本不感兴趣。这种“全图更新”造成了巨大的计算冗余。

更关键的是，ESDF在建图时只能“看到”障碍物的表面，不知道障碍物内部是实心还是空心。这会导致一个经典的“陷阱”问题：想象一条轨迹从两个并排的障碍物中间穿过。ESDF会从两个障碍物表面分别产生一个“推力”，试图把轨迹推开。如果这两个推力大小和方向不合适，轨迹反而可能被“卡”在障碍物中间，甚至被推进我们以为的“空腔”（实则是障碍物内部）里，导致规划失败。这种基于全局场的、间接的避障方式，有时候会显得有点“笨拙”。

EGO-Planner的论文作者正是看到了这些痛点，提出了一个大胆的想法：我们能不能不依赖这个笨重的“先知地图”，只关注轨迹真正撞上的那些障碍物，并直接针对它们生成推力呢？ 这个思路的核心转变是，从“基于全局场的、被动的梯度下降”变为“基于局部碰撞检测的、主动的推力修正”。这就像从依靠一张详细的天气预报图来决定是否带伞，变成了出门感觉雨滴落到身上了再立刻打开伞——反应更直接，计算也更轻量。

2. 核心思想：如何不用ESDF也能“推”开障碍物？

EGO-Planner的整个后端优化，其决策变量依然是B样条曲线的控制点。它的核心避障逻辑非常直观，我把它总结为“碰撞即反馈，推离即优化”。

当一条初始轨迹（可能来自一个非常简单的前端，甚至不考虑障碍物）生成后，规划器会去检查这条轨迹是否与障碍物发生了碰撞。如果发生了碰撞，它不会去查一个全局的ESDF地图来获取梯度，而是会基于这个具体的碰撞信息，直接计算一个方向，把轨迹从障碍物里“推”出来。

具体是怎么“推”的呢？我们来看算法步骤：

定位“肇事”控制点：首先，检查轨迹的每一个B样条控制点。找到那些位于障碍物内部的“坏”控制点，记为 Qi。
寻找“安全”参照点：对于每一个“坏”点 Qi，找到它左右相邻的两个“好”控制点（即不在障碍物内部的点）。利用这两个点，可以生成一条指向障碍物外的短路径，这个方向大致就是我们希望Qi逃离的方向。
构建推力向量 {p, v}：以上一步得到的方向作为法向量，构造一个平面。这个平面会与障碍物表面相交，得到一个交点 p_ij（i代表第i个控制点，j代表第j个障碍物）。那么，从控制点 Qi 指向表面交点 p_ij 的向量 v_ij，就是我们想要的推力方向。这个 (p_ij, v_ij) 对，就是这个控制点针对这个障碍物的“逃生指南”。
避免重复劳动：为了防止轨迹从一个障碍物里逃出来之前，又对同一个障碍物生成重复的推力对，算法设定了一个规则：只有当控制点 Qi 对于某个障碍物 j 的距离 d_ij > 0（即已经不在该障碍物内了），才认为该障碍物是“新发现”的，并为其生成新的推力对。这保证了优化过程的高效性。

这个过程妙在哪里？它按需索取。只有那些真正与轨迹发生碰撞的障碍物，才会被纳入优化项进行计算。这相比计算和维护整个空间的ESDF，计算量大大减少。而且，这个推力是基于实际几何关系计算的，方向更加明确直接，避免了ESDF可能产生的局部极小值或错误梯度问题。