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样条。因此，惩罚函数被制定为公式

其中w_v，w_a，w_j是每个项的权重，F(·)是控制点的高阶导数的两次连续可微度量函数。公式（9）中的f(c_r)是用于惩罚的函数，公式（10）中的参数a_1，b_1，c_1，a_2，b_2，c_2是为了满足二阶连续性而选择的。
c_m是导数限制，c_j是二次区间和三次区间的分割点。λ＜1-c是一种弹性系数，其中c<<1，以使最终结果满足约束条件，因为成本函数是所有加权项的权衡。

3.2 数值优化(重点内容)

本文中所制定的问题具有两个方面的特点。首先，目标函数J会根据新发现的障碍物自适应地改变。这要求求解器能够快速重新启动。其次，二次项占据了目标函数的制定，使得J近似为二次函数。这意味着利用海森矩阵信息可以显著加速收敛。然而，在实时应用中获得精确的反海森矩阵是不可行的，因为它需要消耗大量的计算。为了规避这个问题，采用了从梯度信息中近似反海森矩阵的拟牛顿方法。

由于求解器的性能取决于问题本身，我们比较了三种属于拟牛顿方法的算法。它们是Barzilai-Borwein方法[16]，它能够通过最简单的海森矩阵估计进行快速重启；截断牛顿方法[17]，它通过向给定状态添加多个微小扰动来估计海森矩阵；L-BFGS方法[18]，它从先前的目标函数评估中近似海森矩阵，但需要进行一系列迭代才能达到相对准确的估计。在第VI-B节中的比较表明，选择适当的记忆大小，L-BFGS算法优于其他两种算法，平衡了重启的损失和反海森矩阵估计的准确性。该算法简要解释如下。对于一个无约束优化问题min_{x∈\mathbb{R}_n} f(x)，更新x遵循近似牛顿步骤。

然后海森矩阵被表示为一个递归的公式，其中α_k是步长，H_k在每次迭代中通过公式

进行更新。这里的H_k并不是显式地计算出来的。该算法利用Barzilai-Borwein步骤的权重作为L-BFGS更新的初始反海森矩阵H^0_k，并使用强Wolfe条件下的单调线搜索来强制收敛。该算法具有线性时间/空间复杂度，并采用高效的两层循环递归更新方法[16]。

4. 参考链接

https://zhuanlan.zhihu.com/p/530922644

https://blog.csdn.net/weixin_42284263/article/details/122283727

https://blog.csdn.net/weixin_44673253/article/details/126985079