ego-planner论文阅读

EGO-Planner: An ESDF-Free Gradient-Based Local Planner for Quadrotors

本文是将高飞老师等人的这篇论文翻译了一遍，顺便跟着读一下，只是记录一下自己读的论文，无其他见解，原文链接如下：
https://ieeexplore.ieee.org/document/9309347

Ⅰ 前言

在无人机的路径规划方法中，基于梯度的方法能够平滑轨迹并利用梯度信息改善其间隙。传统的基于梯度的规划器依赖于预先构建的ESDF图来评估梯度的大小和方向，并使用数值优化来生成局部最优解。尽管优化程序具有快速的收敛性，但它们在事先构造所需的ESDF时却遭受了很大的困难。根据表1的统计，ESDF的计算占据了进行局部规划总处理时间的70 %左右。因此可以说构建ESDF已成为基于梯度的规划器的瓶颈，阻碍了该方法应用于资源受限的平台。

表1：来自参考文献【2】，由表可知ESDF的计算占据了进行局部规划总处理时间的70 %左右
通常，构建ESDF有两种方式。如第二节所述，方法可分为增量式全局更新方法和批量式局部计算方法。然而，两者都没有关注轨迹本身。因此，过多的计算花费在计算对规划没有贡献的ESDF值上。也就是说，目前基于ESDF的方法并不能单独、直接地服务于轨迹优化。如图1所示，对于无人机期望在局部避免碰撞的一般自主导航场景，轨迹只覆盖ESDF更新范围的有限空间。在实践中，一些手工制定的规则虽然可以确定一个较小的ESDF范围，但缺乏理论上的合理性，仍然会导致不必要的计算。

图1
ego-planner设计了一个无ESDF的基于梯度的局部规划框架EGO（EGO取自ESDF-free Gradient-based lOcal），并结合了仔细的工程考虑，使其具有轻量级和鲁棒性。该算法由基于梯度的样条优化器和后处理过程组成。首先，该方法优化了带有平滑性、碰撞性和动力学可行性项的轨迹。与传统方法查询预计算的ESDF不同，ego通过比较障碍物内的轨迹和一条引导的无碰撞路径来建模碰撞成本。然后将力投影到碰撞轨迹上，并生成估计的梯度，将轨迹从障碍物中包裹起来。在优化过程中，轨迹会在附近障碍物之间反弹几次，最终终止在安全区域内。用这种方法只在必要时计算梯度，避免在与局部轨迹无关的区域计算ESDF。如果得到的轨迹违反动力学限制，这通常是由于时间分配不合理造成的，则激活求精过程。在精化过程中，当超出限制时，轨迹时间被重新分配。随着时间分配的扩大，产生了一种新的B样条，在平衡可行性和拟合精度的同时，拟合了之前的动态不可行的B样条。为了提高鲁棒性，对拟合精度进行各向异性建模，对轴向和径向施加不同的惩罚。
ego是第一个在没有ESDF的情况下实现基于梯度的局部规划的方法。与现有最先进的工作相比，所提方法生成的安全轨迹具有相当的平滑性和激进性，但通过省略ESDF维护，计算时间降低了一个数量级以上。ego的贡献是：
（1）提出了一种新颖且鲁棒的基于梯度的四旋翼飞行器局部规划方法，该方法直接利用障碍物评估和投影梯度信息，而不是预先构建的ESDF。
（2）提出了一种轻量级但有效的轨迹修正算法，该算法通过建立带有各向异性误差惩罚的轨迹拟合问题来生成更平滑的轨迹。

Ⅱ 避撞力估计

在本文中，决策变量是B样条曲线的控制顶点Q。每个Q独立地拥有自己的环境信息。首先，给定一个满足终端约束的朴素B样条曲线Φ，且不考虑碰撞。然后开始优化过程，对于迭代中检测到的每个碰撞段，生成无碰撞路径 Г。之后，碰撞段的每个控制点Qi将在障碍物表面被分配一个锚点p_ij，并具有相应的斥力方向矢量v_ij，如图2(a)所示。i ∈ N+ 表示控制点的索引，j ∈ N 表示{p, v}对的索引，每一个{p, v}对仅属于一个特定控制点。为了简洁起见，省略下标 ij 不会引起歧义。文中详细的{p, v}对生成过程总结在算法1中，并如图2(b) 所示。从Qi到第j个障碍物的障碍物距离定义为：

图二：(a) 穿过障碍物的轨迹 Φ 生成多个控制点 {p, v} 对。 p 是障碍物表面的点，v 是从控制点指向 p 的单位向量。 (b)垂直于切向量Ri的平面Ψ与Γ相交形成线l，由此确定{p，v}对。 ©距离场定义的切片可视化 d_ij =(Qi − p_ij) · v_ij。颜色表示距离，箭头是等于 v 的相同梯度。p 位于零距离平面。

在前几次迭代中，轨迹避开当前障碍物之前，为了避免重复的{ p，v }对生成，采用了一个准则，认为仅当当前Qi对所有有效的j满足d_ij > 0时，控制点Qi所在的障碍物是新发现的。此外，该准则仅允许对最终轨迹有贡献的必要障碍物被纳入优化。因此减少了操作时间。
为了将必要的环境意识纳入到局部规划器中，我们需要显式地构造一个使轨迹远离障碍物的目标函数。ESDF提供了这种重要的碰撞信息，但代价是沉重的计算负担。此外，如图3所示，由于ESDF的信息不足甚至错误，基于ESDF的规划很容易陷入局部最小值而无法逃离障碍。为了避免这种情况，总是需要一个额外的前端来提供无碰撞的初始轨迹。上述方法在提供碰撞避免的重要信息方面优于ESDF，因为显式设计的斥力对于不同的任务和环境可以相当有效。此外，所提出的方法对无碰撞初始化没有要求。

图三：轨迹陷入局部最小值，因为相机看不到障碍物的背部。

Ⅲ 基于梯度的轨迹优化

A 问题建模

在ego中，轨迹被均匀B样条曲线Φ参数化，该B样条曲线由阶数pb，Nc个控制点{Q1，Q2，…，Q_Nc}和节点向量{t1，t2，…，t_M}唯一确定，其中Qi∈R3，tm∈R，M = Nc + pb。为了轨迹估计的简单性和效率，ego的方法中使用的B样条是均匀的，即每个节点与它的前一个节点以相同的时间间隔delta_t = tm + 1 - tm分离。ego的问题描述基于当前最先进的四旋翼飞行器局部规划框架Fast - Planner。(一环套一环啊，后面再看fast-planner)
B样条具有凸包性质，如图4所示。这一性质表明一条B样条曲线的单个跨距仅由pb + 1个连续的控制顶点控制，且位于这些点的凸包内。例如，( ti , ti + 1)内的跨度位于{ Qi-pb，Qi-pb + 1，…，Qi }构成的凸包内。另一个性质是B样条的k阶导数仍然是pb阶B样条，k = pb - k。由于delta_t关于Φ恒成立，得到速度Vi、加速度Ai、加加速度Ji曲线的控制点为


图4：B样条曲线的凸包性质。灰色点代表控制点。只要所有的控制点都在可行性包围盒(黑色虚线框)中，整个曲线就会停留在包围盒内部。不失一般性，每个凸包由四个顶点组成。
ego沿用"D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors”"的工作，在差分平坦输出的约化空间中规划控制点Q∈R3 .然后，将优化问题表述如下:

其中Js是平滑惩罚，Jc是碰撞惩罚，Jd表示可行性。λs，λc，λ d是每个惩罚项的权重。
（1）平滑惩罚：在Usenko等人的“Real-time trajectory replanning for mavs using uniform b-splines and a 3d circular buffer,”中，平滑惩罚被公式化为轨迹(加速度、加加速度等)的平方导数的时间积分。在fast-planner中，不考虑时间分配，只取轨迹的几何信息。ego结合这两种方法来惩罚加速度和加加速度的平方，不需要时间积分。
受益于凸包性质，最小化B样条轨迹的二阶和三阶导数的控制点足以降低这些导数在整个曲线上的分布。因此，平滑惩罚函数的表达式为：

这可以使高阶导数最小化，从而使得整个轨迹平滑。
（2）碰撞惩罚：碰撞惩罚使得控制点远离障碍物，通过采用安全间隙sf和惩罚控制点之间的距离d_ij < sf来实现。为了更方便优化，ego构造了一个二次连续可微的惩罚函数jc，并且随着d_ij的减小，抑制其斜率，从而得到分段函数

其中，jc( i , j)为{ p，v }_ j对 对Qi产生的代价值。
每个Qi上的成本是独立评估的，并从所有对应的{ p，v }_j对进行累加。因此，如果控制点发现更多的障碍物，那么它将获得更高的轨迹变形权重。具体来说，第i个控制点的成本增加值为jc ( Qi ) =∑(j = 1~Np) jc( i , j)，Np为属于Qi的{ p，v }_j对的个数。对所有Qi进行成本合并，得到总成本Jc，即：

不同于传统的基于ESDF的方法，该方法通过在场上进行三线性插值来计算梯度。通过直接计算Jc关于Qi的导数得到梯度，即：

（3）可行性惩罚：通过在每个单一维度上限制轨迹的高阶导数，即保证可行性。对所有的t，应用|Φk_r (t)| < Φ(k)_r,max，其中r∈{ x，y，z }表示每个维度。得益于凸包性质，控制点的约束导数足以约束整个B样条。因此，惩罚函数的表达式为:

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

式中：cr∈C∈{ Vi，Ai，Ji }，选择a1，b1，c1，a2，b2，c2满足二阶连续性，cm为导数极限，cj为二次区间和三次区间的分裂点，λ < 1 - ε 为弹性系数，使得最终结果满足约束条件，其中0< ε <<1，因为代价函数是所有加权项的权衡。

B.数值优化

本章节表述的问题主要有两个方面，一是目标函数J根据新发现的障碍物自适应变化，这要求求解器能够快速重启。其次，二次项主导了目标函数的制定，使得J近似为二次项。这意味着Hessian信息的利用可以显著加速收敛。然而，在实时应用中很难获得精确的逆Hessian，因为它需要消耗大量计算。为了避免这种情况，采用从梯度信息近似逆Hessian的拟牛顿方法。
由于求解器的性能依赖于问题，比较三种属于拟牛顿方法的算法。分别是Barzilai-Borwein方法，它能够用大多数粗Hessian估计快速重启；truncated Newton方法，它通过向给定状态添加多个微小扰动来估计Hessian；L - BFGS方法，它从先前的目标函数评估中近似Hessian，但需要一系列迭代来达到相对准确的估计。在适当选择内存大小的情况下，L - BFGS优于其他两种算法，平衡了重启损失和逆Hessian估计精度。该算法简要说明如下。对于无约束优化问题minx∈Rn f ( x )，x的更新遵循近似牛顿步

其中αk为步长，Hk在每次迭代时通过下式更新

各变量的计算分别为：

这里不显式计算Hk。算法右乘∇fk到上述计算H_(k+1)的式中，并对m步进行递归扩展，得到高效的双循环递归更新方法，导致线性时间/空间复杂度。将Barzilai-Borwein步的权重作为L - BFGS更新的初始逆Hessian H0_k，即

采用强Wolfe条件下的单调线搜索来实现收敛。

注：Hessian 矩阵是包含一个实值多变量函数所有二阶偏导数的方阵，用于描述函数在某点的局部曲率。它提供了关于函数曲率的重要信息。具体来说，对于一个实值函数 𝑓(𝑥)，其 Hessian 矩阵 𝐻的元素是 𝑓的二阶偏导数：

在一些优化算法中，如Newton方法和拟牛顿方法，Hessian矩阵或其近似值用于加速收敛。精确的Hessian矩阵通常计算成本很高，因此拟牛顿方法通过使用梯度信息来近似Hessian，从而在较低计算成本下实现快速收敛。
拟牛顿方法是一类迭代优化算法，旨在利用一阶导数（梯度）信息来近似二阶导数（Hessian矩阵），从而避免直接计算Hessian矩阵带来的高计算成本。

Ⅳ 时间重分配和轨迹优化

在优化之前分配一个精确的时间剖面是不合理的，因为此时规划器不知道最终轨迹的信息。因此，一个额外的时间重分配过程对于保证动态可行性是至关重要的。fast-planner将轨迹参数化为非均匀 B 样条，并在某些段超出导数限制时迭代地延长结跨度的子集。
然而，一个结跨度Δt_n影响多个控制点，反之亦然，当在初始状态附近调整结跨度时，会导致之前的轨迹的高阶不连续性。在本节中，根据Ⅲ中的安全轨迹Φs，通过合理的时间重新分配重新生成均匀的B样条轨迹Φf。然后，提出了一种各向异性曲线拟合方法，使Φf自由优化其控制点以满足高阶导数约束，同时保持与Φs几乎相同的形状。
首先像Fast-Planner一样计算超出限制的比例：

其中 i ∈{1,…,Nc − 1}，j ∈{1,…,Nc − 2}，k ε {1,…,Nc − 3} 且 r ε{x, y，z} 轴。带有下标 m 的概念表示导数的限制。 re 表示我们应该将 Φf 的时间分配相对于 Φs 延长多少。Vi、Aj 和 Jk 分别与Δt、Δt的平方和Δt的立方成反比。
然后就得到了Φf的新的时间跨度：

通过求解一个封闭的最小二乘问题，时间跨度Δt‘的Φf最初是在边界约束下生成的，同时保持与 Φs 相同的形状和控制点数。然后通过优化来完善平滑性和可行性。由平滑度、可行性和曲线拟合的线性组合制定的惩罚函数 J′ 为：

其中 λf 是适应度项的权重。
适应度惩罚函数 Jf 被表述为从点 Φf (αT ′) 到相应的 Φs(αT ) 的各向异性位移的积分，其中 T 和 T ′ 是 Φs 和 Φf 的轨迹持续时间，α ∈ [0, 1]。由于拟合曲线Φs已经是无碰撞的，因此我们为两条曲线分配低惩罚权重的轴向位移以放宽平滑度调整限制，并以高惩罚​​权重的径向位移来避免碰撞。为了实现这一目标，ego使用球体度量，如图 5 所示，以便同一球体表面的位移产生相同的惩罚。我们用于 Φf (αT ′) 的椭球体是通过围绕其主轴之一（即切线Φs(αT )的导数）旋转以 Φs(αT ) 为中心的椭圆来获得的。（相当于定义一个椭球体，旋转椭球体的中心在曲线Φs(αT) 上的某一点，以该点为中心点定义一个椭圆，这个椭圆的轴之一是曲线在该点的切线Φ˙𝑠(𝛼𝑇)，将该椭圆绕着它的一个主轴旋转一周，生成一个旋转椭球体。这个主轴是椭圆所在平面中的一个轴，而该平面包含切线 Φ˙𝑠(𝛼𝑇)。）因此轴向位移da和径向位移dr可计算为：

适应度惩罚函数为:

其中a和b分别是椭圆的半长轴和半短轴。 L-BFGS 解决了这个问题。（具体的解决方法如下注释）

图5：优化轨迹 Φf 来拟合轨迹 Φs，同时调整平滑度和可行性。黑点和绿点是轨迹上的样本点。 Φf (αT ′) 和 Φs(αT ) 之间的位移沿两个椭圆主轴分解为 da 和 dr。红色椭圆表面上的点产生相同的惩罚。
注：椭圆的长半轴和短半轴作为优化参数，计算目标函数 J 对于路径参数和椭圆参数（如半长轴a 和半短轴 b）的梯度。梯度表示为 ∇J，它是一个向量，包含了目标函数相对于优化变量的偏导数。
选择初始路径参数和椭圆参数计算初始的目标函数值J及初始梯度 ∇J，然后使用L-BFGS进行迭代更新。在每次迭代中，使用当前的梯度信息∇J 和过去的梯度信息来近似Hessian矩阵的逆。然后利用得到的Hession矩阵的逆来计算搜索方向pk

其中，Hk是当前迭代步的Hessian矩阵的逆的近似。
使用线搜索方法确定步长αk，更新优化变量：x_(k+1)=xk+αk*pk，然后更新新的体度∇J_(k+1)和目标函数值J(x_k+1)。
然后判断是否满足收敛条件，如梯度的范数小于某个阈值，或者目标函数的变化小于某个阈值。如果收敛，则结束迭代；否则，继续迭代。最终得到优化后的椭参数。