经典文献阅读之--EGO-Planner(无ESDF的四旋翼局部规划器)

0. 简介

作为局部规划器而言，当机器人或无人机想要避开动态障碍物时。局部规划器就显得尤为重要了。其中基于梯度的规划器被广泛用于四旋翼无人机的局部规划，其中欧几里得符号距离场（ESDF）对于评估梯度幅度和方向至关重要。然而，由于轨迹优化过程仅覆盖了ESDF更新范围的一个非常有限的子空间，因此计算这样一个场具有很大的冗余性。《EGO-Planner: An ESDF-free Gradient-based Local Planner for Quadrotors》一文提出了一种无ESDF的基于梯度的规划框架，它显著地减少了计算时间。其中主要改进是，惩罚函数中的碰撞项是通过比较碰撞轨迹和无碰撞引导路径来制定的。所得的障碍物信息只有在轨迹遇到新的障碍物时才会被存储，使规划器只提取必要的障碍物信息。然后，如果轨迹违反了动态可行性，我们将延长时间分配。引入了一个各向异性曲线拟合算法来调整轨迹的高阶导数，同时保持原始形状。基准比较和现实世界的实验验证了其稳健性和高性能。源码我们可以在Github上找到。

1. 主要贡献

这种方法是第一个实现基于梯度的本地规划而不需要ESDF的。与现有的最先进算法相比，所提出的方法通过省略ESDF维护，生成具有可比较平滑性和侵略性的安全轨迹，但计算时间比现有的方法要低一个数量级以上。我们在仿真和实际环境中进行了全面测试，以验证我们的方法。这篇文章的主要贡献包括：
1）我们提出了一种新颖且强大的基于梯度的四旋翼本地规划方法，它直接从障碍物中评估和投影梯度信息，而不是使用预先构建的ESDF。
2）我们提出了一种轻量级但有效的轨迹细化算法，通过使用各向异性误差惩罚来制定轨迹拟合问题，生成更平滑的轨迹。
3）我们将所提出的方法集成到一个完全自主的四旋翼系统中，并发布我们的软件供社区参考。
在这里插入图片描述

图1：仅在很小的ESDF更新范围内进行优化的轨迹

2. 避障力估计(衡量指标，主要的贡献点)

本文中，决策变量是B样条曲线的控制点 $Q$ ，每个 $Q$ 都独立地具有其自身的环境信息。最初，给出一个满足终端约束的朴素B样条曲线 $Φ$ ，而不考虑碰撞。然后，优化过程开始。在每次迭代中检测到碰撞段后，将生成一条无碰撞路径 $Γ$ 。然后，对于每个碰撞段的控制点 $Q_i$ ，将分配一个锚点 $p_{ij}$ 在障碍物表面上，带有相应的斥力方向向量 $v_{ij}$ ，如图3a所示。记 $i∈\mathbb{N}_+$ 为控制点的索引， $j∈\mathbb{N}$ 为 ${p，v\}$ 对的索引。注意，每个 ${p，v\}$ 对仅属于一个特定的控制点。为简洁起见，我们省略了 $ij$ 的下标，而不会引起歧义。本文中详细的 ${p，v\}$ 对生成过程总结在算法1中，并在图3b中说明。然后，将 $Q_i$ 到第 $j$ 个障碍物的距离定义为：
在这里插入图片描述
为了避免在前几次迭代中轨迹逃离当前障碍物之前重复生成 ${p，v\}$ 对，我们采用一个标准，只有当当前 $Q_i$ 满足所有有效 $j$ 的 $d_{ij} > 0$ 时，才将其所在的障碍物视为新发现的。此外，该标准只允许将对最终轨迹有贡献的必要障碍物纳入优化中。因此，操作时间显著减少。为了将必要的环境意识融入本地规划器中，我们需要明确构建一个目标函数，使轨迹远离障碍物。ESDF提供了这个重要的碰撞信息，但代价是沉重的计算负担。

此外，如图2所示，基于ESDF的规划器很容易陷入局部最小值，并由于ESDF提供的信息不足或错误而无法避开障碍物。为避免这种情况，始终需要另外一个前端提供无碰撞的初始轨迹。上述方法在提供避免碰撞的重要信息方面优于ESDF，因为明确设计的斥力可以在各种任务和环境中相当有效。此外，所提出的方法对无碰撞初始化没有要求。

在这里插入图片描述

图2：轨迹陷入局部最小值，这是非常常见的情况，因为相机没有看到障碍物的后面。

在这里插入图片描述

3：a）穿过障碍物的轨迹Φ为控制点生成了几个 ${p，v\}$ 对。 $p$ 是障碍物表面上的点， $v$ 是从控制点指向 $p$ 的单位向量。b）与切向量 $R_i$ 垂直的平面 $Ψ$ 与 $Γ$ 相交形成一条线 $l$ ，从中确定 ${p，v\}$ 对。c）距离场定义的切片可视化， $d_{ij} = (Q_i - p_{ij})·v_{ij}$ 。颜色表示距离，箭头是相同的梯度，等于 $v$ 。 $p$ 在零距离平面上。

3. 基于梯度的轨迹优化

3.1 问题形式化

在本文中，轨迹由一个均匀的B样条曲线 $Φ$ 参数化，它由其次数 $p_b$ ， $N_c$ 个控制点 ${Q_1，Q_2，…，Q_{N_c}\}$ 和一个结点向量 ${t_1，t_2，…，t_M\}$ 唯一确定，其中 $Q_i∈\mathbb{R}^3$ ， $t_m∈\mathbb{R}$ 且 $M = N_c + p_b$ 。为了轨迹评估的简便和高效，我们方法中使用的B样条是均匀的，这意味着每个结点与其前身之间的时间间隔为 $\Delta t= t_{m+1} - t_m$ 。本文中的问题形式化基于当前最先进的四旋翼局部规划框架Fast-Planner [14]。

B样条具有凸包性质。该性质表明，B样条曲线的单个跨度仅由 $p_b + 1$ 个相邻控制点控制，并位于这些点的凸包内。例如，跨度在 $t_i，t_{i+1})$ 内的点位于由 ${Q_{i - p_b}，Q_{i - p_{b + 1}}，…，Q_i\}$ 形成的凸包内。另一个性质是，B样条的第 $k$ 导数仍然是一个次数为pb-k的B样条。由于 $\Delta t$ 仅与 $Φ$ 相关，因此速度 $V_i$ ，加速度 $A_i$ 和Jerk $J_i$ 曲线的控制点由以下公式获得：
在这里插入图片描述
我们遵循文献[15]的工作，计划控制点 $Q∈\mathbb{R}^3$ 在不同平滑输出的降维空间中。然后，优化问题的表述如下：

其中 $J_s$ 是平滑度惩罚项， $J_c$ 是碰撞检测， $J_d$ 表示可行性。 $λ_s$ ， $λ_c$ ， $λ_d$ 是每个惩罚项的权重。

3.1.1 平滑性惩罚项

在[2]中，平滑性惩罚项被定义为轨迹的二次导数（加速度、跳跃度等）的平方在时间积分上的结果。在[10]中，只考虑轨迹的几何信息，而不考虑时间分配。在本文中，将这两种方法结合起来，惩罚平方加速度和跳跃度，而不进行时间积分。由于凸包属性的好处，将B样条轨迹的二阶和三阶导数的控制点最小化就足以减少整个曲线上的这些导数。因此，平滑性惩罚函数被定义为最小化高阶导数，使整个轨迹平滑。

3.1.2 碰撞惩罚项

碰撞惩罚项将控制点推离障碍物，采用安全间隔 $s_f$ ，并惩罚 $d_{ij} < s_f$ 的控制点。为了进一步方便优化，我们构造了一个二次连续可微的惩罚函数jc，并在 $d_{ij}$ 减小时抑制其斜率，得到分段函数。

$j_c(i，j)$ 是由 $Q_i$ 上的 ${p，v\}$ j对产生的成本值。对每个 $Q_i$ 的成本进行独立评估，并从所有相应的 ${p，v\}_j$ 对中累积。因此，如果发现了更多的障碍物，则控制点获得更高的轨迹变形权重。具体来说，添加到第 $i$ 个控制点的成本值是 $j_c(Q_i)=∑^{N_p}_{j=1}j_c(i，j)$ ，其中 $N_p$ 是属于 $Q_i$ 的 ${p，v\}_j$ 对的数量。将所有 $Q_i$ 的成本组合起来得到总成本 $J_c$ ，即公式（6）。
在这里插入图片描述

与传统的基于ESDF的方法[2，10]不同，后者通过对场进行三线性插值来计算梯度，我们通过直接计算 $J_c$ 相对于 $Q_i$ 的导数来获得梯度
在这里插入图片描述

3.1.3 可行性惩罚项

通过限制轨迹在每个维度上的高阶导数，即对所有 $t$ 应用 $|Φ^{(k)}_r(t)| <Φ^{(k)}_{r,max}$ ，其中 $r∈{x,y,z}$ 表示每个维度。由于凸包特性，限制控制点的导数就足以限制整个B样条。因此，惩罚函数被制定为公式