面向无人机的高效无欧几里得符号距离场梯度局部规划器：EGO-Planner
一、 研究作者、机构与发表情况
本研究的主要作者为Xin Zhou, Zhepei Wang, Hongkai Ye, Chao Xu 和 Fei Gao。所有作者均来自浙江大学工业控制技术国家重点实验室与网络传感与控制研究所。该研究成果以论文形式发表在IEEE Robotics and Automation Letters期刊上，并于2020年10月被接收（预印本版本）。论文标题为“EGO-Planner: An ESDF-Free Gradient-Based Local Planner for Quadrotors”。
二、 学术背景与研究目标
本研究属于机器人学与自动化领域，具体聚焦于无人机（四旋翼）的在线运动规划子领域。近年来，无人机在线规划方法的发展极大地推动了空中自主能力的边界，使其能够走出实验室，应用于众多现实场景。其中，基于梯度的规划方法通过平滑轨迹并利用梯度信息来优化其与障碍物的距离，显示出巨大潜力并日益流行。
传统的基于梯度的规划器严重依赖于预先构建的欧几里得符号距离场（Euclidean Signed Distance Field， ESDF）来评估梯度的大小和方向，并通过数值优化生成局部最优解。尽管优化过程收敛速度快，但预先构建所需的ESDF却带来了巨大的计算负担。根据现有研究（如EWOK）的统计数据，ESDF计算占据了执行局部规划总处理时间的约70%，已成为此类方法的性能瓶颈，阻碍了其在资源受限平台上的应用。
虽然ESDF被广泛使用，但很少有工作深入分析其必要性。通常，构建ESDF的方法分为全局增量更新和局部批量计算两种。然而，这两种方法都没有紧密围绕轨迹本身进行。如图1所示，在一个典型的自主导航场景中，无人机仅需在局部空间内避障，其优化过程中的轨迹仅覆盖ESDF更新范围的非常有限的一部分。因此，当前基于ESDF的方法并没有专一且直接地为轨迹优化服务，导致大量计算被浪费在计算对规划无贡献的ESDF值上。
基于此背景，本研究的目标是设计一种无需ESDF的、基于梯度的局部规划框架，旨在克服ESDF带来的计算瓶颈，同时保持甚至提升规划性能。该方法旨在通过更轻量级和直接的方式获取避障所需的梯度信息，从而显著减少计算时间，实现高效、鲁棒的无人机局部轨迹规划。
三、 研究方法与详细工作流程
EGO-Planner的核心是一个基于梯度优化的样条优化器和一个后处理精炼程序。其工作流程可概括为：首先，对轨迹进行包含平滑性、碰撞和动态可行性惩罚项的优化。与传统方法不同，其碰撞惩罚并非通过查询预计算的ESDF获得，而是通过一种新颖的碰撞力估计机制生成。若优化后的轨迹违反动力学极限，则启动精炼过程，重新分配时间并调整高阶导数。
1. 碰撞规避力估计 (Collision Avoidance Force Estimation) 这是本研究的核心创新点，旨在替代ESDF提供避障梯度信息。决策变量为B样条曲线的控制点 q。 * 过程：算法从一条忽略碰撞的初始B样条曲线开始迭代优化。在每次迭代中，对于检测到的每个碰撞轨迹段，算法会生成一条该段附近的无碰撞引导路径γ（通常使用A*算法）。然后，对于该碰撞段内的每个控制点 q_i，算法会为其分配一个或多个 {p， v} 对。其中，p 是位于障碍物表面的锚点（从引导路径γ上选取），v 是从控制点 q_i 指向 p 的单位向量，代表排斥方向。如图3所示，这一过程通过寻找垂直于轨迹切向量的平面与引导路径γ的交线来确定 {p, v} 对。 * 距离定义：基于 {p， v} 对，控制点 q_i 到第 j 个障碍物的距离被巧妙地定义为点积形式：*d_ij = (q_i - p_ij) · v_ij*。这实际上定义了一个具有方向性的距离场（如图3c所示），其梯度方向恒为 v_ij。 * 优势：这种方法仅当轨迹碰撞到新障碍物时才计算并存储该障碍物的信息（即 {p， v} 对），使得规划器仅提取必要的障碍物信息，避免了全局ESDF构建的冗余计算。同时，这种显式设计的排斥力比ESDF提供的梯度更直接有效，且不要求初始轨迹无碰撞。
2. 基于梯度的轨迹优化 (Gradient-Based Trajectory Optimization) 轨迹采用均匀B样条曲线参数化，利用其凸包性质和高阶导数易于计算的特性。优化问题被形式化为一个无约束非线性问题，目标函数 J 是平滑惩罚 *J_s*、碰撞惩罚 J_c 和可行性惩罚 J_d 的加权和。 * 平滑性惩罚：通过最小化控制点对应的加速度和加加速度（Jerk）的平方和来实现，确保轨迹的光滑性。 * 碰撞惩罚：基于第1步中定义的障碍物距离 *d_ij*。当 d_ij 小于安全距离 s_f 时，施加惩罚。惩罚函数被设计为一个二次连续可微的分段函数，以确保优化过程的稳定性。其梯度可直接解析计算为 *∂J_c/∂q_i = Σ v_ij * g(c_ij)*，其中 g 是惩罚函数对距离的导数。这直接提供了将控制点（从而整个轨迹）推离障碍物的梯度方向。 * 可行性惩罚：为确保轨迹满足无人机动力学限制（最大速度、加速度、加加速度），利用B样条的凸包性质，直接约束控制点的高阶导数。惩罚函数同样被设计为二次连续可微的分段函数，当导数接近限制时会施加立方惩罚。 * 数值优化：鉴于目标函数会根据新发现的障碍物自适应变化（需快速重启），且主要由二次项构成，研究比较了三种拟牛顿法：Barzilai-Borwein (BB) 方法、截断牛顿法 (Truncated Newton) 和L-BFGS方法。实验比较（见表I）表明，L-BFGS方法在适当选择内存大小的情况下表现最佳，它在重启损失和逆海森矩阵估计精度之间取得了良好平衡，因此被选作优化器。
3. 时间重分配与轨迹精炼 (Time Re-allocation and Trajectory Refinement) 由于优化前无法预知最终轨迹的形态，初始的时间分配可能不合理，导致优化后的轨迹违反动力学极限。本研究提出了一种精炼策略。 * 过程：首先，根据公式 r_e = max( |v_i / v_m|, sqrt(|a_j / a_m|), cbrt(|j_k / j_m|) ) 计算超出限制的比例 *r_e*，其中 v_m， a_m， j_m 为各阶导数的上限。然后，将原轨迹 φ_s 的时间间隔 Δt 按比例延长至 *Δt‘ = r_e Δt*，得到新的时间分配。 * 各向异性曲线拟合：基于新的时间分配，生成一条新的均匀B样条轨迹 *φ_f*。为了在满足动力学约束的同时尽可能保持原轨迹 φ_s 的形状（从而保持其无碰撞性），提出了一个创新的各向异性曲线拟合优化问题。其拟合惩罚项 J_f 不是简单地最小化两点间的欧氏距离，而是分解为轴向位移 *d_a*（沿原轨迹切线方向）和径向位移 *d_r*（垂直于切线方向）。如图5所示，惩罚函数定义为 *J_f = ∫ [ (d_a/a)^2 + (d_r/b)^2 ] dα*，其中 a 和 b 是椭圆的半长轴和半短轴。通过设置 *b << a*，可以对径向位移（可能导致碰撞）施加更大的惩罚，而对轴向位移（主要影响平滑性和动态可行性）施加较小的惩罚，从而在调整轨迹以满足动力学约束时，能最大程度地保持其原有的无碰撞路径形状。此优化问题同样使用L-BFGS求解。
四、 主要实验结果
研究通过仿真基准测试和真实世界实验，全面验证了EGO-Planner的性能、鲁棒性和高效性。
1. 优化算法对比实验 在随机地图中进行100次独立测试，对比BB、截断牛顿和L-BFGS三种算法。结果（表I）显示，L-BFGS的成功率最高（0.89），平均优化时间最短（0.37ms），且所需的目标函数评估次数最少（平均79.04次），证明了其对于本问题的高效性。
2. 有/无ESDF的轨迹生成对比 在相同设置下，将EGO-Planner与两种ESDF方法对比：使用无碰撞初始化的ESDF方法(EI)和使用直线初始化的ESDF方法(ENI)。结果（表II）表明： * 成功率：EGO（0.89）与EI（0.89）相当，远高于ENI（0.69）。 * 轨迹能量（平滑性）：EGO产生的轨迹能量略高于EI，这是因为EGO中与多个障碍物交互的控制点会产生更强的轨迹形变力。 * 计算时间：这是最关键的改进。EGO的总规划时间仅为0.37毫秒。而EI和ENI在优化时间（~0.45毫秒）之外，还需要额外的约5毫秒用于ESDF更新，总时间约为EGO的15倍。这清晰地证明了消除ESDF计算带来的巨大速度优势。
3. 与先进规划器的对比 将EGO-Planner与两种先进的基于ESDF的规划器Fast-Planner和EWOK进行对比。在不同障碍物密度下进行测试。结果（表III， 图7）显示： * 性能：EGO-Planner实现了更短的飞行时间（24.38秒）和轨迹长度（42.24米），但轨迹能量（196.64）高于Fast-Planner（135.21）。作者分析这是因为Fast-Planner使用了前端动力学路径搜索，而EGO直接优化初始直线。EWOK由于目标函数包含指数项，在密集环境中易产生扭曲轨迹且收敛不稳定。 * 计算效率：EGO-Planner的平均规划时间仅为0.81毫秒，显著低于Fast-Planner（3.29毫秒）和EWOK（1.39毫秒）。更重要的是，EGO完全节省了ESDF更新所需的时间（Fast-Planner为4.01毫秒，EWOK为6.43毫秒），整体计算开销降低了一个数量级以上。
4. 真实世界实验 研究在室内外多种复杂、未知环境中进行了实物飞行测试（图10，11）。 * 室内狭小空间：无人机在办公室与杂乱房间之间穿行，通过了宽度不足1米的门洞，并在复杂室内环境中达到了3.56米/秒的速度。 * 动态目标追踪：在飞行中任意指定新目标，验证了规划器在有限传感器视野（FOV）下的快速重规划能力。 * 室外森林环境：无人机在充满树木和灌木的森林中飞行，成功处理了风吹动树枝导致的动态地图噪声，速度超过3米/秒。 这些实验充分证明了EGO-Planner在静态和慢速动态环境中的鲁棒性和高性能。
五、 研究结论与价值
本研究得出结论：EGO-Planner首次实现了无需ESDF的基于梯度局部规划。它通过创新的碰撞力估计方法直接从未知环境中提取必要的避障梯度信息，并辅以各向异性曲线拟合进行轨迹精炼。与现有的先进ESDF规划器相比，该方法能够生成安全性、平滑性和激进性相当的轨迹，同时通过省去ESDF维护，将计算时间降低了一个数量级以上。
该研究的科学价值在于：1) 对ESDF在梯度规划中的必要性进行了批判性思考，并提出了一种更高效的替代范式；2) 提出了一种轻量级且有效的环境信息表示和利用方法（{p， v}对）；3) 提出了各向异性曲线拟合方法，为轨迹形状保持与动力学约束满足的权衡提供了新思路。 其应用价值非常突出：极大地降低了在线运动规划的计算负担，使得高性能的梯度规划算法能够部署在计算资源有限的嵌入式平台（如小型无人机）上，推动了无人机在复杂、动态环境中实时自主导航的实际应用。
六、 研究亮点
七、 其他有价值内容
论文也指出了方法的当前局限性：1) 由于依赖A*搜索引导路径，可能陷入局部最小值；2) 统一的时间重分配策略可能产生保守的轨迹。作者展望了未来的改进方向，包括采用拓扑规划来逃离局部最小值，以及重新形式化问题以生成更优轨迹。此外，当前方法针对静态环境设计，但能处理低速移动障碍物（<0.5米/秒）。未来将通过移动物体检测和拓扑规划进一步拓展至动态环境导航。这些为后续研究指明了方向。