Path Planning 论文精读（一）EGO-Planner: An ESDF-free Gradient-based Local Planner for Quadrotors

这篇笔记用来描述 2020年 发表在arxiv上的一篇有关 Path Planning 领域的论文，作者是国内无人机集群与规划的知名团队 浙江大学 高飞。

---

写在最前面

为了方便你的阅读，以下几点的注意事项请务必了解：

• 该系列文章每个字都是我理解后自行翻译并写上去的，可能会存在笔误与理解错误，如果发现了希望读者能够在评论区指正，我会在第一时间修正错误。
• 阅读这个系列需要你有基础的 Path planning 相关基础知识，有时候我会直接使用英文名词，因为这些词汇实在不容易找到符合语境的翻译。
• 原文可能因为版面限制存在图像表格与段落不同步的问题，为了更方便阅读，我会在博文中重新对图像表格进行排版，并做到引用图表的文字下方就能看到被引用的图表。因此可能会出现一张图片在博客中多处位置重复出现的情况。
• 对于原文中的图像，我会在必要时对图像描述进行翻译并附上我自己的理解，但如果图像描述不值得翻译我也不会强行写上去。

---

Basic Information

• 论文标题：EGO-Planner: An ESDF-free Gradient-based Local Planner for Quadrotors
• 原文链接: https://arxiv.org/abs/2008.08835
• 发表时间：2020年12月07日
• 发表平台：arxiv
• 预印版本号：[v2] Mon, 7 Dec 2020 04:57:42 UTC (2,439 KB)
• 作者团队：Xin Zhou, Zhepei Wang, Hongkai Ye, Chao Xu and Fei Gao
• 院校机构：
  • Zhejiang University;
• GitHub仓库: https://github.com/ZJU-FAST-Lab/ego-planner

---

Abstract

基于梯度的规划器广泛用于四旋翼局部规划，其中欧几里德有符号距离场 (Euclidean Signed Dis- tance Field, ESDF) 对于评估梯度幅度和方向至关重要。然而，计算这样的场具有很大的冗余度，因为轨迹优化过程仅覆盖 ESDF 更新范围的非常有限的子空间。本文提出了一种基于梯度的无 ESDF 规划框架，大大减少了计算时间。主要的改进是通过将碰撞轨迹与无碰撞引导路径进行比较来制定惩罚函数中的碰撞项。只有当轨迹碰到新障碍物时，才会存储由此产生的障碍物信息，使规划器仅提取必要的障碍物信息。然后，如果违反了动态可行性，则会延长时间分配。引入了各向异性曲线拟合算法来调整轨迹的高阶导数，同时保持原始形状。基准比较和实际实验验证了它的鲁棒性和高性能。源代码以 ROS 包的形式发布。

---

1. Introduction

近年来，四旋翼飞行器在线规划方法的出现极大地推动了飞行自主性的边界，使得无人机飞出实验室，出现在众多实际应用中。在这些方法中，基于梯度的方法显示出巨大的潜力，可以平滑轨迹并利用梯度信息来提高其净空，并且越来越受欢迎。

传统基于梯度的规划器依赖于预先构建的ESDF图来评估梯度幅值和方向，并使用数值优化来生成局部最优解。尽管优化程序收敛速度很快，但由于需要预先构建所需的ESDF，它们会遇到很大的困难。统计数据（ EWOK 中的Table.2）表明，ESDF 计算占用了进行局部规划总处理时间的约 70%。因此，构建 ESDF 已成为基于梯度的规划器的瓶颈，阻碍了该方法应用于资源有限的平台。

尽管 ESDF 被广泛使用，但很少有研究分析其必要性。通常有两种方法可以构建 ESDF：增量全局更新 和 批量局部计算。然而，两种方法都没有关注轨迹本身。因此，在计算对规划没有贡献的 ESDF 值上花费了太多计算。换句话说，当前基于 ESDF 的方法不能单独且直接地用于轨迹优化。Fig.1 所示，对于一般的自主导航场景，无人机预计会局部避免碰撞，轨迹仅覆盖 ESDF 更新范围的有限空间。在实践中，虽然一些手工制定的规则可以决定一个很小的 ​​ESDF 范围，但它们缺乏理论合理性并且仍然会导致不必要的计算。

在本文中，作者设计了一个基于梯度的无 ESDF 局部规划框架 EGO，并结合了工程考虑使其轻量且鲁棒。算法由 基于梯度的样条优化器 和 后细化程序 组成。首先使用平滑度、碰撞和动态可行性优化轨迹。与查询事先计算的 ESDF 的传统方法不同，通过将障碍物内的轨迹与无碰撞的引导路径进行比较来模拟碰撞成本；然后将力投射到碰撞轨迹上并生成估计梯度以将轨迹包裹在障碍物之外。优化过程中，轨迹将在附近的障碍物之间反弹几次，最后终止于安全区域。这样只在必要时计算梯度，并避免在局部轨迹无关的区域计算 ESDF。如果生成的轨迹违反了动态限制（通常是由于时间分配不合理造成的），则激活细化过程。在细化过程中，当超出限制时将重新分配轨迹时间。通过扩大时间分配生成一条新的 B 样条，该样条拟合先前的动态不可行性，同时平衡可行性和拟合精度。为了提高鲁棒性，拟合精度采用各向异性建模，在轴向和径向上采用不同的惩罚。

该方法是第一个无需 ESDF 即可实现基于梯度的局部规划的方法。与SOTA相比，该方法生成了具有相当平滑度和激进的安全轨迹，但省去了 ESDF 维护，计算时间降低了一个数量级以上。在模拟和实际环境中进行了全面的测试。本文的贡献如下：

1. 提出了一种新颖且稳健的基于梯度的四旋翼飞行器局部规划方法，该方法直接从障碍物而非预先构建的ESDF（环境空间分布函数）评估和投射梯度信息；
2. 提出了一种轻量级且有效的轨迹细化算法，该算法通过用各向异性误差惩罚来构建轨迹拟合问题，从而生成更平滑的轨迹；
3. 将所提方法集成到一个完全自主的四旋翼飞行器系统中，并公开代码供社区参考；

---

2. Related Work

A. Gradient-based Motion Planning

基于梯度的运动规划是无人机局部轨迹生成的主流，其将问题表述为无约束非线性优化。ESDF 首次由 Ratliff 等人引入到机器人运动规划中。利用其丰富的梯度信息，许多规划框架直接优化空间中轨迹。然而，在离散时间中优化轨迹并不适合无人机，因为其对动态约束更为敏感。进而有人提出了一种用于无人机规划的连续时间多项式轨迹优化方法，然而这类方法涉及的势函数积分导致了繁重的计算负担。此外，即使在随机重启的情况下，该方法的成功率也仅在 70% 左右。针对这些缺点引入了轨迹的 B 样条参数化，充分利用了凸包特性，通过找到无碰撞的初始路径作为前端，成功率显著提高。当初始无碰撞路径的生成考虑到运动动力学约束时，性能会进一步提高。结合感知意识还能使系统更加稳健。在上述方法中，ESDF 在评估与附近障碍物的梯度幅度和方向的距离方面起着至关重要的作用。

B. Euclidean Signed Distance Field (ESDF)

近二十多年来，ESDF 一直被用于从嘈杂的传感器数据中构建物体。Felzenszwalb 等人提出了一种包络算法，将 ESDF 构建的时间复杂度降低到 O(n)，其中 n 表示体素数。该算法不适用于增量构建 ESDF，而在四旋翼飞行器飞行过程中通常需要动态更新场。为了解决这个问题，Oleynikova 和 Han 提出了增量 ESDF 生成方法，即 Voxblox 和 FIESTA。虽然这些方法在动态更新情况下效率很高，但生成的 ESDF 几乎总是包含冗余信息，这些信息可能根本不会在规划过程中使用。如Fig.1 所示，该轨迹仅扫过整个 ESDF 更新范围中非常有限的子空间。因此，设计一种更智能、更轻量级的方法而不是维护整个场是很有价值的。

---

3. Collision Avoidance Force Estimation

本文的核心变量是B样条曲线上的控制点 $Q$。每个 $Q$ 都独立拥有自身的环境信息，整体流程如下：

1. 给出一条不考虑碰撞满足终点约束的朴素B样条曲线 $\Phi$ ；
2. 开始优化过程，对于迭代中检测到的每个碰撞段，生成一条无碰撞路径 $\Gamma$；
3. 碰撞段的每个控制点 $Q_i$将在障碍物表面分配一个锚点 $p_{ij}$，并对应一个排斥方向向量 $v_{ij}$，如Fig.3(a)所示。
   

$i\in N_{+}$ 表示控制点的索引，$j\in N$ 表示 { p , v } \{p,v\} {p,v} 对的索引。每个 { p , v } \{p,v\} {p,v} 对只属于一个特定的控制点。为了简洁起见省略下标 $ij$ 以免引起歧义。本文中详细的 { p , v } \{p,v\} {p,v}对生成过程在Algorithm. 1中进行了总结，并在Fig.3(b)中进行了说明。

$Q_i$到第 $j$个障碍物的距离定义为：

$$\begin{array}{c}{d}_{ij}=(Q_i-p_{ij})\cdot v_{ij}\end{array}$$

为了避免在前几次迭代中，轨迹在逃离当前障碍物之前重复生成 { p , v } \{p,v\} {p,v}对，作者采用了一个准则： 只有当前 $Q_i$ 对所有有效 $j$ 满足 $d_{ij}>0$ 时，控制点 $Q_i$ 所在的障碍物才被视为新发现的障碍物，该准则仅允许对最终轨迹有贡献的必要障碍物进行优化。因此，运算时间显著缩短。

为了将必要的环境意识融入局部规划器，需要明确构建一个目标函数使轨迹远离障碍物。ESDF 提供了这一重要的碰撞信息，但计算负担沉重。如Fig.2 所示，由于 ESDF 提供的信息不足甚至有误，基于 ESDF 的规划器很容易陷入局部最小值而无法避开障碍物。为了避免这种情况，总是需要一个额外的前端来提供无碰撞的初始轨迹。上述方法在提供避免碰撞的重要信息方面优于 ESDF，因为明确设计的排斥力对于各种任务和环境都相当有效。此外，所提出的方法不需要无碰撞初始化。

---

4. Gradient-Based Trajectory Optimization

A. Problem Formulation

本文采用均匀B样条曲线 $\Phi$ 作为轨迹的参数，该曲线由其度数 $p_b$、$N_c$个控制点 { Q 1 , Q 2 , … , Q N c } \{Q_{1}, Q_{2}, \dots, Q_{N_{c}}\} {Q1​,Q2​,…,QNc​​}和节点向量 { t 1 , t 2 , … , t M } \{t_{1}, t_{2}, \dots, t_{M}\} {t1​,t2​,…,tM​} 唯一确定，其中 $Q_i\in R^3,t_m\in R,M=N_c+p_b$。为了简化和提高轨迹评估的效率，使用的B样条曲线是均匀的，这意味着每个节点与其前一个节点之间的时间间隔$\Delta t=t_{m+1}-t_m$相同。本文中的问题公式基于当前最先进的四旋翼飞行器局部规划框架Fast-Planner。

B 样条曲线具有凸包性质。该性质表明 B 样条曲线的单个跨度仅由 $p_b+1$ 个连续控制点控制，并且位于这些点的凸包内。例如 $(t_i,t_{i+1})$ 内的跨度位于由 $Q_{i-p_b,Q_{i-p_b+1},\dots ,Q_i}$ 形成的凸包内。另一个性质是 B 样条曲线的 $k$ 阶导数仍然是阶数为 $p_{b.k}=p_b-k$ 的 B 样条曲线。由于 $\Delta t$ 与 $\Phi$ 相同，速度 $V_i$、加速度 $A_i$ 和加加速度 $J_i$ 曲线的控制点可由下式获得：

$$\begin{array}{c}{V}_i=\frac{Q_{i+1}-Q_i}{\Delta t};A_i=\frac{V_{i+1}-V_i}{\Delta t};J_i=\frac{A_{i+1}-A_i}{\Delta t}\end{array}$$

在差分平坦输出 (differentially flat outputs) 的约化空间中规划控制点 $Q\in R^3$ 优化问题可表示如下：

$$\begin{array}{c}\underset{Q}{\min }J={\lambda }_s{J}_s+{\lambda }_c{J}_c+{\lambda }_d{J}_d\end{array}$$

其中 $J_s$ 表示平滑度惩罚 Smoothness penalt ；$J_c$ 表示碰撞惩罚 Collision penalty ；$J_d$ 表示可行性惩罚 Feasibility penalty。${\lambda }_s,{\lambda }_c,{\lambda }_d$ 是每个惩罚项的权重。

Smoothness penalty

平滑度惩罚通常被公式化为两种方案 轨迹（加速度、加加速度等）平方导数的时间积分、只考虑轨迹的几何信息，不考虑时间分配。在本文中，作者将上面两种方案进行结合，对加速度和加加速度的平方进行惩罚，而无需时间积分。

得益于凸包特性，最小化B样条轨迹二阶和三阶导数的控制点足以降低整条曲线上的导数。因此，平滑度惩罚函数公式为，最小化高阶导数，使整个轨迹平滑：

$$\begin{array}{c}{J}_s=\sum _{i=1}^{N_c-1}\Vert A_i{\Vert }_2^2+\sum _{i=1}^{N_c-2}\Vert J_i{\Vert }_2^2\end{array}$$

Collision penalty

碰撞惩罚将控制点推离障碍物。通过采用安全间隙 $s_f$ 并惩罚 $d_{ij}<s_s$ 的控制点来实现。构建一个两次连续可微的惩罚函数 $j_c$ 以对其进一步优化，并在 $d_{ij}$ 减小时抑制其斜率，从而得到分段函数：

$$\begin{array}{rl}{j}_c(ij)& =\{\begin{array}{l}0,(c_{ij}<0)\\ c_{ij}^3,(0<c_{ij}\le s_f)\\ 3s_f{c}_{ij}^2-3s_f^2{c}_{ij}+s_f^3,(c_{ij}>s_f)\end{array}\\ c_{ij}& =s_f-d_{ij}\end{array}$$

其中$j_c(i,j)$ 是 $Q_i$ 上 { p , v } j \{p,v\}_{j} {p,v}j​对产生的代价值。每个 $Q_i$ 上的代价都是独立评估的，并从所有相应的 { p , v } j \{p,v\}_{j} {p,v}j​ 对中累积而成。因此如果控制点发现更多障碍物，则它会获得更高的轨迹变形权重。具体而言，添加到第 $i^{th}$ 个控制点的代价值为$j_c(Q_i)={\sum }_{j=1}^{N_p}{j}_c(i,j)$，$N_p$ 是属于 $Q_i$ 的 { p , v } j \{p,v\}_{j} {p,v}j​ 对总数。将所有 $Q_i$ 上的代价合并可得出总代价 $J_c$，即：

$$\begin{array}{c}{J}_c=\sum _{i=1}^{N_c}{j}_c(Q_I)\end{array}$$

与传统的基于 ESDF 的方法通过在场上进行三线性插值来计算梯度不同，作者通过直接计算 $J_c$ 对 $Q_i$ 的导数来获得梯度，从而得到：

$$\begin{array}{c}\frac{\partial J_c}{\partial Q_i}=\sum _{i=1}^{N_c}\sum _{j=1}^{N_p}{v}_{ij}\{\begin{array}{l}0,(c_{ij}<0)\\ -3c_{ij}^2,(0<c_{ij}\le s_f)\\ -6s_f{c}_{ij}+3s_f^2,(c_{ij}>s_f)\end{array}\end{array}$$

Feasibility penalty

可行性保证在于限制轨迹在每一维上的高阶导数，即对所有 $t$ 应用 $|{\Phi }_r^{(k)}(t)|<{\Phi }_{r,max}^{(k)}$，其中 r ∈ { x , y , z } r\in \{x, y, z\} r∈{x,y,z} 表示每个维度。由于凸包特性，约束控制点的导数足以约束整个B样条曲线。因此惩罚函数公式为：

$$\begin{array}{c}{J}_d=\sum _{i=1}^{N_c}{w}_{v}F(V_i)+\sum _{i=1}^{N_c-1}{w}_{a}F(A_i)+\sum _{i=1}^{N_c-2}{w}_{j}F(J_I)\end{array}$$

其中 $w_v,w_a,w_j$是每个项的权重，$F(\cdot )$ 是控制点高阶导数的二次连续可微度量函数。

$$\begin{array}{c}F(C)=\sum _{r=x,y,z}f(c_r)\end{array}$$

$$\begin{array}{c}f(c_r)=\{\begin{array}{l}{a}_1{c}_r^2+b_1{c}_r+c_1,(c_r<-c_j)\\ (-\lambda c_m-c_{}r{)}^3,(-c_j<c_r<-\lambda c_m)\\ 0,(-\lambda c_m\le c_r\le \lambda c_m)\\ (c_r-\lambda c_m{)}^3,(\lambda c_m<c_r<c_j)\\ a_2{c}_r^2+b_2{c}_r+c_2,(c_r\ge c_j)\end{array}\end{array}$$

其中 c r ∈ C ∈ { V i , A i , J i } c_{r}\in C \in \{V_{i}, A_{i}, J_{i}\} cr​∈C∈{Vi​,Ai​,Ji​}；$a_1,b_1,c_1,a_2,b_2,c_2$满足二阶连续性；$c_m$ 为导数极限；$c_j$ 为二次区间和三次区间的分裂点；$\lambda <1-ϵ$ 为弹性系数，$ϵ\ll 1$，使最终结果满足约束条件，因为代价函数是所有加权项的权衡。

B. Numerical Optimization

本文所提出的公式化问题有两个特点：

1. 目标函数 $J$ 会根据新发现的障碍物自适应地改变。这要求求解器能够快速重启；
2. 二次项在目标函数的公式中占主导地位，使得 $J$ 近似于二次函数，这意味着利用 Hessian 信息可以显著加快收敛速度​​。

然而，获取精确的 Hessian 逆矩阵在实时应用中是难以实现的，因为它会消耗不可忽略的大量计算资源。为了解决这个问题，作者采用了拟牛顿法（quasi-Newton），利用梯度信息来近似 Hessian逆矩阵。

由于求解器的性能取决于具体问题，作者比较了三种拟牛顿法：

• Bar​​zilai-Borwein ：能够快速重启，且 Hessian 估计最为粗略；
• Truncated Newton：通过对给定状态添加多个微小扰动来估计 Hessian；
• L-BFGS ：通过先前的目标函数评估来近似 Hessian，但需要一系列迭代才能达到相对准确的估计。

在适当选择内存大小的情况下，L-BFGS 优于其他两种算法，在重启损失和 Hessian 逆估计的准确性之间取得了平衡。该算法对于无约束优化问题 ${\min }_{x\in R^n}f(x)$ ，$x$ 的更新遵循近似的牛顿法步骤：

$$\begin{array}{c}{x}_{k+1}=x_k-{\alpha }_k{H}_k\nabla f_k\end{array}$$

其中 ${\alpha }_k$ 是步长；$H_k$ 会在每次迭代中更新：

$$\begin{array}{c}{H}_{k+1}=V_k^T{H}_k{V}_k+{\rho }_k{s}_k{s}_k^T\end{array}$$

其中 ${\rho }_k=(y_k^T{s}_k{)}^{-1},V_k=I-{\rho }_k{y}_k{s}_k^T,s_k=x_{k+1}-x_k,y_k=\nabla f_{k+1}-\nabla f_k$。

这里 $H_k$ 无需显式计算，算法直接将 $\nabla f_k$ 乘以Equ.12，并递归展开 $m$ 步，从而得到高效的双循环递归更新方法，最终实现线性时间/空间复杂度。Barzilai-Borwein 步骤的权重用作 L-BFGS 更新的初始 Hessian 逆 $H_k^0$，其公式为：

$$\begin{array}{c}{H}_k^0=\frac{s_{k-1}^T{y}_{k-1}}{y_{k-1}^T{y}_{k-1}}Ior\frac{s_{k-1}^T{s}_{k-1}}{s_{k-1}^T{y}_{k-1}}I\end{array}$$

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

---

5. Time Re-Allocation And Trajectory Refinement

在优化之前分配准确的时间曲线是不合理的，因为规划器不知道最终轨迹的信息。因此，额外的时间重新分配程序对于确保动态可行性至关重要。先前的研究将轨迹参数化为非均匀 B 样条，当某些段超出导数极限时，迭代延长节点跨度的子集。

然而，一个节点跨度 $\Delta t_n$ 会影响多个控制点，反之亦然，这导致在起始状态附近调整节点跨度时，与先前的轨迹存在高阶不连续性。本节基于第4章节中的安全轨迹 ${\Phi }_s$，通过合理的时间重新分配，重新生成均匀B样条轨迹 ${\Phi }_f$。提出一种各向异性曲线拟合方法，使${\Phi }_f$ 能够自由地优化其控制点以满足高阶导数约束，同时保持与${\Phi }_s$形状几乎相同的状态。

首先，像 Fast-Planner 一样计算超出比例的极限：

r e = m a x { ∣ V i , r / v m ∣ , ∣ A j , r / a m ∣ , ∣ J k , r / j m ∣ 3 , 1 } \begin{equation} r_{e}=max\{|V_{i,r}/v_{m}|, \sqrt{|A_{j,r}/a_{m}|},\sqrt{|J_{k,r}/j_{m}|}^{3},1\} \end{equation} re​=max{∣Vi,r​/vm​∣,∣Aj,r​/am​∣ ​,∣Jk,r​/jm​∣ ​3,1}​​

其中 i ∈ { 1 , … , N c − 1 } , j ∈ { 1 , … , N c − 2 } , k ∈ { 1 , … , N c − 3 } , r ∈ { x , y , z } i\in\{1,\dots, N_{c}-1\}, j\in\{1, \dots, N_{c}-2\}, k\in \{1,\dots, N_{c}-3\}, r\in\{x,y,z\} i∈{1,…,Nc​−1},j∈{1,…,Nc​−2},k∈{1,…,Nc​−3},r∈{x,y,z} 。下标$m$ 表示导数的极限。$r_e$表示 $\Phi f$ 的时间分配相对于${\Phi }_s$应该延长多少。根据Equ.2，$V_i,A_j,J_k$分别与$\Delta t$、$(\Delta t{)}^2$、$(\Delta t{)}^3$成反比。这样得到${\Phi }_f$的新时间跨度：

$$\begin{array}{c}\Delta t^{{}^{\prime }}=r_e\Delta t\end{array}$$

时间跨度 $\Delta t^{{}^{\prime }}$ 的 ${\Phi }_f$ 最初是在边界约束下生成的，同时保持与 ${\Phi }_s$ 相同的形状和控制点数，通过求解闭式最小二乘问题来实现。然后通过优化来完善其平滑度和可行性。惩罚函数 $J^{{}^{\prime }}$ 由平滑度（第四章 A1 节）、可行性（第四章 A3 节）和曲线拟合（稍后介绍）的线性组合构成，其形式为，其中 ${\lambda }_f$ 是适应度项的权重。：

$$\begin{array}{c}\underset{Q}{\min }{J}^{{}^{\prime }}={\lambda }_s{J}_s+{\lambda }_d{J}_d+{\lambda }_f{J}_f\end{array}$$

拟合惩罚函数 $J_f$ 表示为从点 ${\Phi }_f(\alpha T^{{}^{\prime }})$ 到相应 ${\Phi }_s(\alpha T)$ 的各向异性位移的积分，其中 $T$ 和 $T^{{}^{\prime }}$ 分别是 ${\Phi }_s$ 和 ${\Phi }_f$的轨迹持续时间，$\alpha \in [0,1]$ 。由于拟合曲线 ${\Phi }_s$ 已经无碰撞，为两条曲线的轴向位移分配较低的惩罚权重以放松平滑度调整限制，为径向位移分配较高的惩罚权重以避免碰撞。使用如Fig.5 所示的椭球度量，使得同一椭球表面的位移产生相同的惩罚。

用于${\Phi }_f(\alpha T^{{}^{\prime }})$ 的椭球是通过以${\Phi }_s(\alpha T)$ 为中心的椭圆绕其主轴之一（切线 ${\Phi }_s(\alpha T)$）旋转得到的。因此，轴向位移 $d_a$ 和径向位移 $d_r$ 可以通过以下公式计算：

$$\begin{array}{rl}{d}_a& =({\Phi }_f-{\Phi }_s)\cdot \frac{{\dot{\Phi }}_s}{\Vert {\dot{\Phi }}_s\Vert }\\ d_r& =\Vert ({\Phi }_f-{\Phi }_s)\times \frac{{\dot{\Phi }}_s}{\Vert {\dot{\Phi }}_s\Vert }\Vert \end{array}$$

拟合的乘法函数为：

$$\begin{array}{c}{J}_f={\int }_0^1\left[\frac{d_a(\alpha T^{{}^{\prime }}{)}^2}{a^2}+\frac{d_r(\alpha T^{{}^{\prime }})}{b^2}\right]d\alpha \end{array}$$

其中 $a$ 和 $b$ 分别是椭圆的长半轴和短半轴。该问题采用L-BFGS算法求解。

---

6. Experiment Results

A. Implementation Details

规划框架总结在Algorithm.2 中。将 B 样条函数阶数设置为 $p_b=3$。控制点数 $N_c$ 大约为 25，由规划范围（约 7m）和相邻点的初始距离间隔（约 0.3m）决定。这些是平衡问题复杂性和自由度的经验参数。时间复杂度为$O(N_c)$，因为根据 B 样条函数的局部支持特性，一个控制点仅影响附近的段。在相同的相对容差下，L-BFGS 的复杂度也是线性的。对于无碰撞路径搜索，采用 A*，它的优点是路径 $\Gamma$ 总是自然地趋向于靠近障碍物表面。因此可以直接在 $\Gamma$ 处选择 $p$，而无需搜索障碍物表面。

对于Fig.3(b) 中定义的向量 $R_i$，可以通过均匀 B 样条函数参数化的性质推断，$R_i$ 满足：

$$\begin{array}{c}{R}_i=\frac{Q_{i+1}-Q_{i-1}}{2\Delta t}\end{array}$$

将Equ.18 离散化为有限个点 ${\Phi }_f(k\Delta t^{{}^{\prime }})$ 和 ${\Phi }_s(k\Delta t)$，其中 $k\in N$，$0\le k\le [T/\Delta t]$。为了进一步加强安全性，对最终轨迹周围具有固定半径的圆形管道进行碰撞检查，以提供足够的障碍物间隙。当未检测到碰撞时优化器停止。在飞行平台上进行了真实世界实验，深度由英特尔 RealSense D435 获取，修改了英特尔 RealSense 的 ROS 驱动程序，以每隔一帧启用激光发射器频闪，这使设备可以借助发射器输出高质量的深度图像，以及不受激光干扰的双目图像。修改后的驱动程序也是开源的。

B. Optimization Algorithms Comparison

本节讨论了三种不同的优化算法，包括Barzilai-Borwein (BB)、有限内存BFGS (L-BFGS)和 截断牛顿 (T-NEWTON)。每种算法在随机映射中独立运行100次，所有相关参数，包括边界约束、时间分配、决策变量初始化和随机种子，对于不同的算法都是相同的。记录了成功率、计算时间和目标函数评估次数的数据。由于失败案例的数据毫无意义，因此只统计成功的案例。相关结果如Table.1所示，表明 L-BFGS 明显优于其他两种算法。L-BFGS通过二阶泰勒展开式刻画了一种近似类型，适用于优化第四节B中描述的目标函数；截断牛顿 也近似二阶优化方向$H^{-1}\nabla f_k$，然而过多的目标函数评估会增加优化时间；BB 法将 Hessian 估计为标量 $\lambda$ 乘以 $I$。尽管如此，Hessian 估计不足仍然导致收敛速度较低。

C. Trajectory Generation With & Without ESDF

作者使用与第六节B相同的设置进行比较，鉴于基于ESDF的轨迹生成器使用直线初始化时成功率较低，这里采用无碰撞初始化。比较结果见Table.2。

为了方便表述，这里将基于 ESDF 方法有/无碰撞的初始化缩写为 EI 和 ENI。通过比较可以看出，作者所提出的 EGO 算法实现了与基于 ESDF 无碰撞初始化的相近的成功率。然而，EGO 产生的轨迹能量（加加速度积分）略高。这是因为包含多个 { p , v } \{p,v\} {p,v} 对的 EGO 的控制点产生比 EI 更强的轨迹变形力，如第 IV-A节所述。另一方面，更强的力会加速收敛过程从而缩短优化时间。ENI的一些统计数据可能不太令人信服，因为与 EI 和 EGO 相比，ENI 测试只能在较少的挑战案例中成功，在这些案例中得到的轨迹自然更平滑，能量成本更低，速度更低。尽管对于 9 米轨迹，ESDF 更新尺寸减小到 10×4×2 m^3s，分辨率为 0.1 米，但 ESDF 更新仍然占用大部分计算时间。

D. Multiple Planners Comparison

将规划器与两种SOTA方法 Fast-Planner 和 EWOK 进行了比较，这两种方法利用 ESDF 来评估障碍物距离和坡度。每个规划器从相同的起点到终点，运行 10 次不同障碍物密度的路径。平均性能统计数据和 ESDF 计算时间如Table.3和Fig.7所示。Fig.8 展示了三种方法在 0.5 个障碍物/平方米的地图上生成的轨迹。

从Table.3中可以发现，与 Fast-Planner 相比，作者的方法实现了更短的飞行时间和轨迹长度，但最终导致更高的能量成本。这主要是因为 EWOK 中的前端运动动力学路径搜索在密集环境中会遭遇曲折轨迹，因为目标函数包含指数项，这导致优化中的收敛不稳定。此外，作者的方法无需更新 ESDF 即可节省大量计算时间。

E. Real-World Experiments

作者在像机FOV有限的复杂未知环境中进行了多项实验。其中一个实验是按照预先给定的航点飞行。在该实验中，无人机从一间小办公室出发，穿过门并在一个杂乱的大房间里飞行一圈，然后返回办公室，如Fig.10(a)和Fig.11所示。室内实验中最窄的通道不足一米，如Fig.6所示。相比之下，无人机在如此复杂的环境中飞行速度达到了3.56米/秒。

另一个室内实验是在飞行过程中追逐任意突然给出的目标，如Fig.10(c) 所示。在该测试中，有限的视野带来了更大的挑战，因为一旦接收到新的目标或检测到碰撞威胁，就必须立即重新生成可行轨迹。因此，该实验验证了所提出的规划器能够在可行性的前提下进行积极的飞行。

在户外实验中，无人机飞过一片由茂密的树木和低矮的灌木丛组成的森林，如Fig.10(b)和Fig.9 所示。尽管无人机周围的强气流导致树枝和树叶摆动，降低了地图的可靠性，但无人机的速度仍然超过了 3 米/秒。因此，所提出的规划器可以同时应对实验和现场环境，更详细的演示可以访问 YouTube 中的视频链接：https://youtu.be/UKoaGW7t7Dk；

---

7. Conclusion And Future Work

本文探讨了基于梯度的轨迹规划中 ESDF 的必要性，并提出了一个无 ESDF 的局部规划器。它实现了与一些最先进的基于 ESDF 的规划器相当的性能，但计算时间却减少了一个数量级以上。基准测试和实际实验验证了它的鲁棒性和高效性。

所提出的方法仍然存在一些缺陷，例如 A* 搜索引入的局部最小值以及统一时间重新分配导致的轨迹保守。因此，未来将致力于进行拓扑规划以摆脱局部最小值，并重新表述问题以生成近似最优轨迹。该规划器专为静态环境设计，无需任何修改即可处理缓慢移动的障碍物（低于 0.5 米/秒）。未来，作者计划将通过移动物体检测和拓扑规划研究动态环境导航。