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

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

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

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

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

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

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

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

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

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

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

2.1 从原理到代码：碰撞代价函数详解

理解了“推力”是怎么来的，我们再看优化器里如何利用它。在EGO-Planner的代价函数中，碰撞项 J_c 的惩罚逻辑是这样的：

它定义了一个安全距离阈值 s_f。对于一个控制点 Qi 到其对应障碍物表面点 p_ij 的有向距离 d_ij（沿 v_ij 方向），根据 d_ij 与 s_f 的关系，施加不同强度的惩罚，形成一个连续可导的分段函数：

• 如果 d_ij >= s_f：说明控制点已经在安全距离之外，不施加任何惩罚（代价为0）。
• 如果 0 <= d_ij < s_f：说明控制点在安全距离内，但还未碰撞。此时惩罚值与 (s_f - d_ij)^3 成正比，是一个温和的三次函数惩罚，鼓励点远离。
• 如果 d_ij < 0：说明控制点已经穿透障碍物。此时惩罚是一个更强的二次函数 a*(s_f-d_ij)^2 + b*(s_f-d_ij) + c，系数 a, b, c 的设计保证了函数在 d_ij=0 处的数值和一阶、二阶连续性，使得优化过程更平滑。

整个碰撞代价是所有控制点、所有对应推力对的惩罚值之和。代码实现上，在 calcDistanceCostRebound 函数中，我们能看到清晰的对应逻辑：

for (auto i = order_; i < end_idx; ++i) // 遍历所有控制点
{
  for (size_t j = 0; j < cps_.direction[i].size(); ++j) // 遍历该控制点的所有{p,v}对
  {
    double dist = (cps_.points.col(i) - cps_.base_point[i][j]).dot(cps_.direction[i][j]); // 计算有向距离 d_ij
    double dist_err = cps_.clearance - dist; // 计算 c_ij = s_f - d_ij
    Eigen::Vector3d dist_grad = cps_.direction[i][j]; // 距离对控制点的梯度就是 v_ij 方向

    if (dist_err < 0) {
        // d_ij > s_f，安全，无惩罚
    } else if (dist_err < demarcation) { // demarcation 通常等于 s_f
        // 0 <= d_ij < s_f，三次惩罚
        cost += pow(dist_err, 3);
        gradient.col(i) += -3.0 * dist_err * dist_err * dist_grad; // 梯度 = -3 * (s_f-d_ij)^2 * v_ij
    } else {
        // d_ij < 0，二次惩罚
        cost += a * dist_err * dist_err + b * dist_err + c;
        gradient.col(i) += -(2.0 * a * dist_err + b) * dist_grad;
    }
  }
}

通过这种设计，当控制点陷入障碍物时，它会受到一个强烈的“推力”（体现在梯度上），将其推回安全区域；当它在安全区域内但离障碍物太近时，也会受到一个温和的“排斥力”，使其保持安全距离。整个过程完全不需要ESDF的参与。

3. 轻量级前端：为什么不需要复杂的Hybrid A*？

在Fast-Planner这类框架中，前端通常使用Hybrid A*等搜索算法，目的是生成一条初步无碰撞的路径，作为后端优化的初始值。这相当于给优化器一个不错的起点，避免它一开始就“误入歧途”。

但EGO-Planner换了一个思路。它的后端优化（我们刚才讲的碰撞推力法）足够强大和鲁棒，即使初始轨迹穿过了障碍物，它也能通过迭代优化把轨迹“拉”出来。因此，它对前端的要求大大降低了：前端只需要提供一条满足起点和目标点状态（位置、速度、加速度）的轨迹即可，完全不用考虑障碍物。

这带来了两个巨大好处：一是前端变得极其简单快速，计算开销极小；二是避免了Hybrid A*可能陷入复杂地形或产生不光滑路径的问题。EGO-Planner的前端主要任务就是生成一条符合动力学约束的、光滑的初始轨迹。

在代码中，这体现在 plan() 函数的前端部分。它主要分为两种情况：

情况一：首次规划或强制使用多项式轨迹。 此时，它会在起点和目标点之间，用Minimum Snap（最小加加速度）轨迹生成一条时间最优的多项式轨迹。这里有个小技巧：它会估算一个总时间 time。如果距离短，按匀加速运动估算；如果距离长，按“加速-匀速-减速”模型估算。然后，在这条多项式轨迹上，以一定时间间隔 ts 采样，得到一系列路径点 (point_set)。采样时还会检查点与点之间的距离，如果太远说明轨迹不够平滑，会缩小 ts 重新采样，直到获得足够多且分布均匀的点。

情况二：基于上一次的轨迹进行“热启动”。 这是更常见的情况。当无人机正在执行轨迹时收到新目标点，它会从当前时刻开始，在上一条轨迹的剩余部分上采样。如果剩余轨迹的终点离新目标点还有一段距离，它会再用Minimum Snap生成一段衔接轨迹。最后，通过一个基于弧长的插值算法，从这些采样点中，等距离地重新采出一组新的路径点，作为本次优化的初始点集。

无论哪种情况，最后都会调用 UniformBspline::parameterizeToBspline 函数，将这些路径点连同起点/终点的速度加速度约束，一起转化为一条均匀B样条曲线的控制点。这个过程本质上是解一个线性方程组 Ap = b，其中 A 是由B样条基函数构成的矩阵，b 是路径点和边界条件，解出的 p 就是控制点。这个轻量级的前端，为后端强大的优化提供了合格的“原材料”。

4. 动力学可行性：如何确保轨迹能被机器人执行？

生成了一条无碰撞的轨迹，还不够。我们必须确保这条轨迹是动力学可行的，即机器人的电机、舵机等执行机构能够实际跟踪这条轨迹。对于无人机来说，这通常意味着轨迹的速度、加速度甚至加加速度（Jerk）不能超过其物理极限。

EGO-Planner在这部分借鉴并改进了Fast-Planner的思路。由于B样条曲线具有凸包性质和导数递归性质，一个非常棒的特性是：我们只需要约束控制点上的速度、加速度等导数，就能保证整条轨迹的导数都不会超过这个范围。这大大简化了约束问题。

具体来说，对于一条均匀B样条，其速度控制点 V_i、加速度控制点 A_i、加加速度控制点 J_i 可以通过控制点 Q_i 直接计算：

• V_i = (Q_{i+1} - Q_i) / Δt
• A_i = (Q_{i+2} - 2Q_{i+1} + Q_i) / Δt²
• J_i = (Q_{i+3} - 3Q_{i+2} + 3Q_{i+1} - Q_i) / Δt³

因此，动力学可行性代价函数 J_d 就是对所有违反约束的导数控制点进行惩罚。EGO-Planner提供了两种惩罚函数：

1. 简单二次惩罚：这是默认方式。例如，对于速度约束，如果某个速度控制点分量 v 超过了最大速度 max_vel，则代价增加 (v - max_vel)²，并对相关的控制点 Q_i 和 Q_{i+1} 计算梯度。这种方式计算简单，但在约束边界处不可导（虽然实践中影响不大）。
2. 复杂连续惩罚：通过定义分段函数（在约束边界附近使用三次函数，远离时使用二次函数），保证了代价函数在约束边界处的二阶连续性。这使得优化过程在接近极限时更加平滑稳定。你可以在代码中通过定义 SECOND_DERIVATIVE_CONTINOUS 宏来启用它。

在 calcFeasibilityCost 函数中，你会看到对每一对控制点计算速度，对每三个控制点计算加速度，并逐一检查其XYZ三个分量是否超限，然后累加惩罚值和梯度。这种方式将动力学约束自然地融入了优化框架，与平滑项、碰撞项一起被最小化。

5. 时间重分配与各向异性曲线拟合：EGO-Planner的“神来之笔”

这是EGO-Planner论文中最精彩、也最体现其创新性的部分。在Fast-Planner中，如果优化后的轨迹在某个段落的动力学约束被违反（比如加速度太大），它会直接拉长该段轨迹的时间跨度（Δt）。但由于B样条的控制点位置与时间跨度Δt是耦合的，改变Δt会导致控制点位置发生变化，从而可能严重扭曲已经优化好的、无碰撞的轨迹形状。这就像拉长一根弹簧，它的形状会改变，可能又碰到障碍物了。

EGO-Planner提出了一个更优雅的解决方案，我称之为 “两步走”策略：

第一步：整体延长时间。 假设我们优化得到了一条无碰撞的轨迹 Φ_s（下标s代表shape，即形状已定）。检查其速度、加速度，找到超出限制最严重的比例，计算一个时间缩放因子 r_e。然后将整条轨迹的持续时间均匀地延长 r_e 倍，得到一个新的时间向量。根据新的时间向量，我们需要重新计算一组控制点 Q'，使得新轨迹 Φ_f（下标f代表feasible，即动力学可行）在对应的时间点上与旧轨迹 Φ_s 的位置尽可能相同，同时起点和终点的状态（速度、加速度）保持一致。这可以通过求解一个线性方程组 AQ' = b 来实现，其中 b 由 Φ_s 在对应时间点的位置和边界状态构成。

第二步：各向异性曲线拟合优化。 第一步得到的新控制点 Q‘ 只是初步解，它保证了形状大致不变，但动力学约束可能还没完全满足。现在，我们以 Q’ 为初始值，进行第二次优化。这次优化的目标函数包含三项：

• 平滑项 J_s：和之前一样，保证轨迹光滑。
• 动力学可行项 J_d：和之前一样，惩罚违反速度加速度约束的部分。
• 曲线拟合项 J_f：这是关键！它要求新轨迹 Φ_f 在形状上要紧贴旧轨迹 Φ_s，但采用了各向异性的惩罚。

什么叫“各向异性”？想象一下，旧轨迹 Φ_s 是一条在空间中蜿蜒的无碰撞路径。我们希望新轨迹不要偏离它。但偏离可以分为两种：垂直于轨迹切线方向的偏离（径向偏离）和沿着轨迹切线方向的偏离（轴向偏离）。径向偏离是危险的，可能导致碰撞；轴向偏离则相对安全，只是让无人机飞得快一点或慢一点。

因此，EGO-Planner在拟合项中设计了椭圆形的惩罚权重。设 v 是旧轨迹在某点的切向方向（单位向量），x 是新旧轨迹在该点的位置差。那么：

• 轴向误差（沿v方向）为 d_a = |x · v|
• 径向误差（垂直v方向）为 d_r = |x × v|
• 拟合代价为 J_f = Σ ( d_a² / a² + d_r² / b² )

这里 a 和 b 是两个权重系数，且 a > b。这意味着，我们对径向偏离（d_r）施加了更重的惩罚（因为分母b小），而对轴向偏离（d_a）惩罚较轻（因为分母a大）。这样，优化过程在调整轨迹以满足动力学约束时，会优先在切线方向上“拉伸”或“压缩”轨迹，而尽量避免在垂直方向上移动，从而最大程度地保持了原轨迹的无碰撞特性。

在代码的 calcFitnessCost 函数中，你可以清晰地看到这个计算过程：

double a2 = 25, b2 = 1; // a=5, b=1，径向惩罚权重是轴向的25倍！
Eigen::Vector3d x = (q.col(i-1)+4*q.col(i)+q.col(i+1))/6.0 - ref_pts_[i-1]; // 新轨迹位置 - 旧轨迹参考位置
Eigen::Vector3d v = (ref_pts_[i] - ref_pts_[i-2]).normalized(); // 旧轨迹的切向方向
double xdotv = x.dot(v); // d_a
Eigen::Vector3d xcrossv = x.cross(v); // d_r 的向量，其模长即为 d_r
double f = pow((xdotv), 2) / a2 + pow(xcrossv.norm(), 2) / b2; // 各向异性拟合代价
cost += f;
// ... 后续计算梯度并累加

通过这种巧妙的“先整体延长时间，再各向异性拟合优化”的两步策略，EGO-Planner成功地将**轨迹形状优化（避障）和时间分配优化（动力学可行）**这两个通常耦合的难题解耦了。它先得到一条好的形状，再通过拟合的方式去调整时间，既保证了安全性，又满足了动态性能，堪称轨迹优化领域一个非常漂亮的解法。

6. 实战踩坑与调参经验

纸上得来终觉浅，绝知此事要躬行。在机器人上实际部署EGO-Planner时，有几个参数和细节需要特别注意，这些坑我都亲自踩过。

1. 安全距离 clearance 与推力对生成： s_f（代码中的 clearance）这个参数至关重要。它决定了控制点多近开始被惩罚。设置太小，无人机可能贴障碍物太近，不安全；设置太大，轨迹可能会被过度推开，在狭窄通道中可能无法通过。我的经验是，这个值至少设为机器人半径的1.5倍。另外，生成推力对 {p, v} 时，那个“与障碍物表面交点” p_ij 的计算精度会影响推力方向。在代码实现中，通常用射线与障碍物包围盒（如八叉树节点）求交来近似，在障碍物形状复杂时，这个近似可能不够准，可以考虑用更精细的碰撞检测库。

2. 优化器权重与收敛： 代价函数 J = λ_s * J_s + λ_c * J_c + λ_d * J_d 中的权重系数需要仔细调节。

• λ_s（平滑权重）：太大会让轨迹过于平滑，牺牲避障性能；太小会让轨迹抖动。一般从1.0开始调。
• λ_c（碰撞权重）：这是最重要的。必须足够大，才能确保将轨迹从障碍物中强力推出。我通常设得比平滑权重大一个数量级（如10.0或更高）。
• λ_d（动力学权重）：根据你的机器人物理极限设定。如果发现优化后速度/加速度仍超限，就加大这个权重。 优化器通常使用非线性优化库（如NLopt、Ceres）。记得设置合理的迭代次数和函数评估次数上限，并监控优化是否收敛。有时优化会陷入局部极小，可以尝试加入少量随机扰动或使用不同的初始解。

3. 时间重分配的比例因子 r_e 计算： 公式 r_e = max( ... ) 计算的是超限最严重的那个比例。在实际中，如果只有某个瞬间的某个维度（比如Z轴加速度）轻微超限，就拉长整条轨迹的时间，可能得不偿失。一种改进策略是分段时间重分配，只拉长那些超限的轨迹段，但这会引入非均匀B样条，增加复杂度。EGO-Planner选择均匀拉伸，简单有效，但在极端动态场景下可能略显保守。

4. 各向异性拟合的权重 a 和 b： 代码中写死了 a2=25, b2=1，即 a=5, b=1。这意味着径向误差的惩罚是轴向的25倍。这个比例在实际中非常有效，它强烈约束了轨迹的侧向移动。如果你发现优化后的轨迹为了满足动力学约束而产生了不希望的弯曲，可以适当增大 a（减小轴向惩罚），但务必谨慎，以免降低避障能力。

5. 与感知模块的衔接： EGO-Planner不依赖ESDF，但它仍然需要感知模块提供障碍物信息。通常是以点云或障碍物包围盒列表的形式输入。规划器内部的碰撞检测需要快速查询某个点是否在障碍物内，以及计算推力向量 {p, v}。这里通常使用增量式欧几里得距离场或**轴向包围盒树（AABB Tree）**来加速查询。确保你的感知数据更新频率和规划频率匹配，并且有适当的时间戳同步，避免使用“过时”的障碍物信息。

从我自己的项目经验来看，EGO-Planner这套框架在计算效率和实际效果之间取得了很好的平衡。它抛弃了沉重的ESDF，用更直接的几何推理解决问题，特别适合在算力有限的机载计算机上运行。其代码结构清晰，模块化好，非常适合作为学习轨迹规划原理和进行二次开发的蓝本。理解它如何将碰撞检测、优化理论、B样条性质和时间分配巧妙结合，对你设计自己的规划算法会有很大启发。