【论文笔记】ego_planner

文章名：EGO-Planner: An ESDF-free Gradient-based Local Planner for Quadrotors

【单位】浙大fastlab

【期刊】IRAL2020

摘要：基于梯度的规划器广泛应用于四旋翼局部规划，其中欧几里德符号距离场（ESDF）是评估梯度幅值和方向的关键。然而，由于轨迹优化程序仅涵盖ESDF更新范围的一个非常有限的子空间，因此计算出此类场具有很大的冗余。本文提出了一种无ESDF的基于梯度规划框架，大大减少了计算时间。主要改进是，通过比较碰撞轨迹和无碰撞引导路径，建立了罚函数中的碰撞项。只有当轨迹碰到新的障碍物时，才会存储生成的障碍物信息，使planner仅提取必要的障碍物信息。然后，如果违反了动力学可行性，我们将拉长分配时间。引入各向异性曲线拟合算法在保持原始形状时调整轨迹的高阶导数（其实是一种曲线适应算法，调整轨迹的高阶导以保证原始形状）。基准比较和实际实验验证了其鲁棒性和高性能。源代码以ros包的形式发布。

目录

1.引言

2.相关工作

A.基于梯度的运动规划

B.欧氏距离场ESDF

3.避障力估计

 4.基于梯度的轨迹优化

5.时间重分配和轨迹细化

6.实验结果

7.总结和未来工作

---

1.引言

        近年来，四旋翼机在线规划方法的出现极大地推动了空中自主的边界，使无人机飞出实验室并出现在许多实际应用中。在这些方法中，基于梯度的方法显示出巨大的潜力，并获得了越来越广泛的应用[1]，它可以平滑轨迹并利用梯度信息改善其间隙。

        传统上，基于梯度的规划器依靠预先构建的 ESDF 图来评估梯度大小和方向，并使用数值优化来生成局部最优解。尽管优化程序具有快速收敛性，但它们在事先构建所需的 ESDF 时会受到很大影响。 正如统计数据（来自 EWOK [2] 的表 II）所述，ESDF 计算占用了执行本地规划的总处理时间的 70% 左右。 因此，我们可以有把握地说，构建 ESDF 已成为基于梯度的规划器的瓶颈，阻碍了该方法应用于资源有限的平台。

        虽然ESDF被广泛应用，但很少有工作分析其必要性。通常，有两种方法来构建一个ESDF。详见第2节。方法可分为增量式全局更新[3]和批量局部更新[4]。然而，他们都没有关注到运动轨迹本身。因此，太多的计算被花费在计算对规划没有贡献的ESDF值上。换句话说，目前基于ESDF的方法并不仅仅和直接地用于轨迹优化。如图1所示。对于一般的自动导航场景，无人机只期望避免局部碰撞，轨迹只覆盖ESDF更新范围的有限空间。在实践中，虽然一些人工制定的规则可以决定一个较小的ESDF范围，但它们缺乏理论合理性，仍然会导致不必要的计算。

         在本文中，我们设计了一个无 ESDF 的基于梯度的局部规划框架，称为 EGO(ESDF-free Gradient-based lOcal），并且我们结合了仔细的工程考虑，使其轻量级和健壮。 所提出的算法由基于梯度的样条优化器和后细化程序(post-refinement procedure)组成。 首先，我们使用平滑度、避障和动力学可行项优化轨迹。 与查询预先计算的 ESDF 的传统方法不同，我们通过将障碍物内的轨迹与引导的无碰撞路径进行比较来对碰撞成本进行建模。 然后我们将力投射到碰撞轨迹上并生成估计的梯度以将轨迹盘绕在障碍物之外。 在优化过程中，轨迹会在附近的障碍物之间反弹几次，最终终止于安全区域。 这样，我们只在必要时计算梯度，避免在与局部轨迹无关的区域计算 ESDF。 如果得到的轨迹违反动力学限制，这通常是由不合理的时间分配造成，那么细化过程被激活。在细化过程中，当超出限制时，将重新分配轨迹时间。随着时间分配的扩大，在平衡可行性和拟合精度的同时，生成了一个新的B样条函数，该样条函数拟合了以前的动力学不可行B样条函数。为了提高鲁棒性，在轴向和径向各向异性上对拟合精度进行了建模。

        据我们所知，这种方法是第一种在没有ESDF的情况下实现基于梯度的局部规划的方法。与现有的最新研究成果相比，该方法生成的安全轨迹具有相当的平滑度和敏捷性，但由于省略了ESDF维护，计算时间缩短了一个数量级以上。为了验证我们的方法，我们在仿真和现实世界中进行了全面的测试。本文的贡献如下：

1):我们提出了一种新的、鲁棒的基于梯度的四旋翼局部规划方法，该方法直接评估和投影障碍物的梯度信息，而不是预先构建的ESDF。

2): 我们提出了一种轻量级但有效的轨迹优化算法，该算法通过描述具有各向异性误差惩罚的轨迹拟合问题来生成更平滑的轨迹.

3):我们将提出的方法集成到一个完全自治的四旋翼系统中，并发布我们的软件供社区参考.

2.相关工作

A.基于梯度的运动规划

        基于梯度的运动规划是无人机局部轨迹生成的主流，它将问题表述为无约束的非线性优化问题。ESDF首先由Ratliff等人在机器人运动规划中引入 [5]。利用其丰富的梯度信息，许多规划框架直接优化配置空间中的轨迹。尽管如此，在离散时间 [5,6] 中优化轨迹并不适合无人机，因为它对动力学约束更加敏感。因此，[7] 提出了一种用于无人机规划的连续时间多项式轨迹优化方法。但是，势函数的积分会导致沉重的计算负担。此外，即使随机重启，该方法的成功率也只在70% 左右。由于这些缺陷，文献[2]介绍了一种基于B样条的轨迹参数化方法，利用了其凸包性质。在[8]中，通过寻找无碰撞的初始路径作为前端，成功率显著提高。 此外，当初始无碰撞路径的生成考虑到动力学约束时，性能会进一步提高 [9, 10]。 周等人[11] 结合感知意识，使系统更加健壮。 在上述方法中，ESDF 在评估与附近障碍物的梯度大小和方向的距离方面起着至关重要的作用。

B.欧氏距离场ESDF

           20多年来，ESDF一直被用于从噪声传感器数据中构建对象，并从[5]以来恢复了人们对机器人运动规划的兴趣。Felzenszwalb等人[4]提出了一种包络算法，将ESDF构造的时间复杂度降低到O(n)，n表示为体素数。该算法不适用于ESDF的增量构建，而在四旋翼飞行中往往需要动态更新。为了解决这个问题，Oleynikova[13]和Han[3]提出了增量ESDF生成方法，即Voxblox和FIESTA。尽管这些方法在动态更新情况下非常高效，但生成的ESDF几乎总是包含冗余信息，这些信息可能根本不会用于规划过程，如图1所示。这个轨迹只扫描整个ESDF更新范围的一个非常有限的子空间。因此，设计一种更智能、更轻量级的方法，而不是维护整个ESDF场是有价值的。

3.避障力估计

        在本文中，决策变量是 B 样条曲线的控制点 Q。 每个 Q 都独立地拥有自己的环境信息。 最初，给出了满足边界约束的朴素 B 样条曲线 Φ，忽略碰撞。 然后，优化过程开始。 对于在迭代中检测到的每个碰撞段，生成无碰撞路径Γ。之后碰撞段的每个控制点 Qi，将会被分配一个在障碍物表面的锚点和对应的斥力方向向量，如图3a所示。i是控制点的索引，j是对的索引。注意每个对专属于某个特定的控制点。 对生成的详细过程总结在算法1和图3b中。然后从Qi到第j块障碍物的距离定义为:

 

 图3：(a)从控制点产生对的过程，p是障碍物表面的点，而v是控制点指向p的单位向量；b）切平面；c) 对产生的距离场。

        为了避免在前几次迭代中轨迹从当前障碍物逃逸之前重复生成 {p, v} 对，我们采用一个标准，将控制点 Qi 所在的障碍物视为新发现的障碍物，前提是当前 Qi 满足 dij  > 0 对于所有有效的 j。 此外，该标准仅允许将有助于最终轨迹的必要障碍纳入优化。 因此，操作时间显著减少。             

        为了将必要的环境感知纳入局部规划器，我们需要明确构造一个目标函数，使轨迹远离障碍。ESDF提供了重要的碰撞信息，但代价是沉重的计算负担。此外，如图2所示，由于来自ESDF的信息不足甚至错误，基于ESDF的planner很容易陷入局部最小值并且无法摆脱障碍。为了避免这种情况，总是需要额外的前端来提供无碰撞的初始轨迹。上述方法在提供避免碰撞的重要信息方面优于ESDF，因为明确设计的排斥力对于各种任务和环境都是相当有效的。此外，该方法对无碰撞初始化没有要求。

 4.基于梯度的轨迹优化

A.问题建模

        在本文中，轨迹由标准B样条曲线参数化，它由阶数,个控制点，节点向量knot vector,。 为了轨迹评估的简单性和效率，我们方法中使用的 Bspline 是统一的，这意味着每个节点与其前任节点之间的时间间隔相同。 本文中的问题表述基于当前最先进的四旋翼局部规划框架 Fast-Planner [14]。 

        B样条享有凸包属性。该属性表明B样条曲线的single span仅由+1个连续控制点控制，并且位于这些点的凸包内。 例如

另一个性质是B样条的K阶导数仍旧是B样条，阶数为。控制点的速度，加速度，jerk可以曲线可以由下式获得：

         遵循[15]的工作，在微分平坦空间中规划控制点，优化问题如下式：

        其中 Js 是平滑惩罚，Jc 是碰撞，Jd 表示可行性。  λs、λc、λd 是每个惩罚项的权重。

         1）平滑度惩罚：在[2]中，平滑度惩罚项被公式化为轨迹平方导数（加速度、加加速度等）的时间积分。 在[10]中，无论时间分配如何，只获取轨迹的几何信息。 在本文中，我们结合这两种方法来惩罚平方加速度和加加速度，而无需时间积分。 受益于凸包特性，最小化 B样条轨迹的二阶和三阶导数的控制点足以减少沿整个曲线的这些导数。 因此，平滑惩罚函数被表述为:

这使高阶导数最小化，使整个轨迹平滑。

        2) 碰撞惩罚: 碰撞惩罚将控制点推离障碍物。这是通过采用安全间隙Sf并以惩罚控制点来实现的。为了进一步促进优化，我们构造了一个两次连续可微的罚函数jc，并随着dij的减小抑制其斜率，从而得到分段函数: 

 

 其中是 Qi 上对产生的成本值。 每个 Qi 的成本是独立评估的，并从所有对应的对中累积。 因此，如果控制点发现更多的障碍物，它会获得更高的轨迹变形权重。 具体来说，添加到第 i 个控制点的代价值为，Np 是属于 Qi 的对的数量。 结合所有 Qi 的成本得出总成本 Jc，即

         与传统的基于 ESDF 的方法 [2, 10] 不同，后者通过场上的三段插值计算梯度，我们通过直接计算 Jc 关于 Qi 的导数来获得梯度，这给出

3） 可行性惩罚项：通过在每个维度上限制轨迹的高阶导数确保可行性。由于凸包特性，约束控制点的导数足以约束整个 B 样条。 因此，惩罚函数表示为：

其中 wv、wa、wj 是每个项的权重，F(·) 是控制点的高阶导数的两次连续可微的度量函数。 

 其中, a1, b1, c1, a2, b2, c2 选择满足二阶连续性， cm 是导数极限， cj 是二次区间的分离点， 三次区间。  λ < 1−是一个弹性系数，具有 以使最终结果满足约束条件，因为成本函数是所有加权项的权衡。

B.数值优化

        本文提出的问题有两个方面的特点。 首先，目标函数 J 根据新发现的障碍物自适应地改变。 它要求求解器能够快速重启。 其次，二次项主导了目标函数的主要形式，使 J 近似二次。 这意味着利用 Hessian 信息可以显著加速收敛。 然而，在实时应用中获得精确的 Hessian 逆是令人望而却步的，因为它消耗了不可忽略的大量计算。 为了避免这种情况，采用了从梯度信息中近似逆 Hessian 的准牛顿方法。

        由于求解器的性能取决于问题，因此我们比较了属于拟牛顿方法的三种算法。它们是Barzilai-Borwein方法 [16]，它能够通过大多数粗略的Hessian估计快速重启，截断的牛顿方法 [17]，它通过向给定状态添加多个微小扰动来估计Hessian，L-BFGS方法 [18]，该方法从先前的目标函数评估中近似于Hessian，但需要一系列迭代才能达到相对准确的估计。Sec中的比较。vi-b指出，L-BFGS在适当选择内存大小的情况下优于其他两种算法，从而平衡了重新启动的损失和Hessian逆估计的准确性。该算法简要解释如下：对一个无约束问题而言，x的更新通过一个近似牛顿步骤:

5.时间重分配和轨迹细化

        在优化之前分配一个准确的时间轮廓是不合理的，因为planner不知道关于最终轨迹的信息。因此，一个额外的时间重新分配程序是至关重要的，以确保动态的可行性。先前的工作将轨迹参数化为一个非均匀的[10,19]样条，并在某些线段超过导数极限时迭代地延长节点跨度的子集。

        然而，一个节点跨度影响多个控制点，反之亦然，当调整节点跨度附近时，导致前一个轨迹的高阶不连续。在本节中，根据IV中的安全轨迹Φs，通过合理地重新分配时间，重新生成一个均匀的B样条轨迹Φf。然后，提出了一种各向异性曲线拟合方法，使Φf能够自由优化其控制点，以满足高阶导数约束，同时保持与Φs几乎相同的导数形状。

        首先，像fast-planner做的，我们计算

        拟合惩罚函数 Jf 表示为从点 Φf(αT) 到相应的 Φs(αT) 的各向异性位移的积分，其中 T 和 T 是 Φs 和 Φf 的轨迹持续时间，α ∈ [0, 1]。 由于拟合曲线 Φs 已经是无碰撞的，我们分配两条惩罚权重较低的曲线的轴向位移以放宽平滑度调整限制，并分配具有高惩罚权重的径向位移以避免碰撞。 为了实现这一点，我们使用球体度量，如图 5 所示，这样在同一球体表面的位移会产生相同的惩罚。 我们用于 Φf(αT) 的球体是通过围绕其主轴之一，即切线 Φ˙s(αT) 旋转以 Φs(αT) 为中心的椭圆获得的。 所以轴向位移da和径向位移dr可以由下式计算：

其中a和b分别是椭圆的半长轴和半短轴，这个问题由L-BFGS解决。

6.实验结果

A.实验细节

        算法2总结了规划框架。 我们将B样条阶数设置为pb = 3。控制点Nc的数量在25左右变化，这是由规划地平线（约7m）和相邻点的初始距离间隔（约0.3m）决定的。 这些是平衡问题复杂性和自由度的经验参数。 时间复杂度为 O(Nc)，因为根据 Bspline 的局部支持属性，一个控制点只影响附近的线段。  L-BFGS 的复杂性在相同的相对容差上也是线性的。 对于无碰撞路径搜索，我们采用A*，它有一个很好的优点是路径Γ总是趋向于

 B.优化算法比较

        在本节中，讨论了三种不同的优化算法，包括 Barzilai-Borwein (BB) 方法、有限内存 BFGS (L-BFGS) 和截断牛顿 (T-NEWTON) 方法 [17]。 具体来说，每个算法在随机映射中独立运行 100 次。 所有相关参数，包括边界约束、时间分配、决策变量初始化和随机种子，对于不同的算法都设置相同。 记录有关成功率、计算时间和目标函数评估次数的数据。 由于失败案例中的数据没有意义，因此只计算成功案例。 相关结果显示在 Tab.I 中，表明 L-BFGS 显着优于其他两种算法。  L-BFGS 通过二阶泰勒展开表征了一种近似，适用于优化第四节-B 中描述的目标函数。 截断牛顿法也近似于二阶优化方向。 但是，过多的目标函数评估会增加优化时间。  BB 方法估计 Hessian为单位矩阵乘以一个标量。尽管如此，Hessian 估计不足仍然导致收敛速度低。

C.有无ESDF的轨迹生成

        我们使用与 Sec.VI-B 相同的设置来执行此比较。 由于 [14] 中解释的在基于 ESDF 的轨迹生成器使用直线初始化时成功率低，我们采用无碰撞初始化。 比较结果见表二。 为清楚起见，具有和不具有无碰撞初始化的基于 ESDF 的方法缩写为 EI 和 ENI。 这种比较表明，所提出的 EGO 算法实现了与基于 ESDF 的无碰撞初始化方法相当的成功率。 然而，EGO 产生的轨迹能量（jerk 积分）略高。 发生这种情况是因为包含多于一对 {p, v} 对的 EGO 控制点产生比 EI 更强的轨迹变形力，如 Sec.IV-A2 所述。 另一方面，更强的力会加速收敛过程，从而缩短优化时间。  ENI 的一些统计数据（以灰色显示）可能不太令人信服，因为与 EI 和 EGO 相比，ENI 测试只能在较少的挑战案例中取得成功，在这些案例中，所产生的轨迹自然更平滑，能量成本更低，速度更低。 值得注意的是，虽然 ESDF 更新大小减小到 10 × 4 × 2，分辨率为 0.1m，轨迹为 9m，但 ESDF 更新仍然占用了大部分计算时间。

 D.多种规划器比较

        我们将提出的规划器与两种最先进的方法，Fast-Planner [14] 和 EWOK [2] 进行比较，它们利用 ESDF 来评估障碍物距离和梯度。 每个规划器从相同的开始到结束运行十倍不同的障碍物密度。 平均性能统计和 ESDF 计算时间显示在 表III 和图7 中。 在 0.5 个障碍物/m2 的地图上由三种方法生成的轨迹如图 8 所示。 从 表III我们得出结论，与 Fast-Planner 相比，所提出的方法实现了更短的飞行时间和轨迹长度，但最终的能源成本更高。 这主要是由于[14]中前端的动力学路径搜索导致。EWOK 在密集环境中会出现曲折的轨迹，因为目标函数包含指数项，导致优化收敛不稳定。 此外，我们得出结论，所提出的方法节省了大量没有 ESDF 更新的计算时间。

E.真实世界实验

        我们在相机 FOV 有限的杂乱未知环境中进行了几项实验。 一项实验是通过预先给定的航路点飞行。 在这个实验中，无人机从一个小办公室开始，穿过门，在一个大杂乱的房间里飞来飞去，然后返回办公室，如图10a和图11所示。 室内实验的最窄通道小于一米，如图 6 所示。 相比之下，无人机在如此杂乱的环境中达到了 3.56m/s。 

        另一个室内实验是在飞行过程中，任意且随机的改变目标点，如图10c所示。在本次测试中，有限 FOV 提出了更大的挑战，即一旦收到新目标或检测到碰撞威胁，必须立即生成可行轨迹。 因此，该实验验证了所提出的planner能够在可行性前提下进行激进的飞行。         

        在户外实验中，无人机飞过一片茂密的树木和低矮的灌木丛，如图 10b 和图 9 所示。 尽管无人机周围的狂野气流导致树枝和树叶摆动，使地图的可靠性降低，但无人机仍然达到了 3m/s 以上的速度。 因此，提出的规划器可以处理实验和现场环境。 我们推荐读者观看视频3以获取更多信息。

7.总结和未来工作

        在本文中，我们研究了 ESDF 对基于梯度的轨迹规划的必要性，并提出了一种无 ESDF 的局部规划器。 它实现了与一些最先进的基于 ESDF 的规划器相当的性能，但将计算时间减少了一个数量级以上。 基准比较和实际实验证明它是稳健且高效的。

       提出的方法仍然存在一些缺陷，即 A* 搜索引入的局部最小值和统一时间重新分配引入的保守轨迹。 因此，我们将致力于执行拓扑规划以逃避局部最小值并重新制定问题以生成接近最优的轨迹。 该规划器专为静态环境而设计，无需任何修改即可应对缓慢移动的障碍物（低于 0.5m/s）。 未来我们将通过移动目标检测和拓扑规划来研究动态环境中的导航。