EGO-Planner: An ESDF-free Gradient-based Local Planner for Quadrotors

一、简介

EGO-Planner是一个无ESDF（欧几里得符号距离场）的基于梯度的局部路径规划框架

EGO-Planner由基于梯度的样条优化器和后细化两部分组成

二、创新点

最主要的创新点应该是无ESDF，显著减少了计算时间。ESDF在基于梯度的规划器中，对于评估梯度幅度和方向至关重要。然而，计算这样的场有很多冗余，因为轨迹优化过程仅覆盖ESDF更新范围的非常有限的子空间。本文提出了一种不需要ESDF的基于梯度的规划框架。主要改进是通过将惩罚函数中的碰撞项与无碰撞的引导路径进行比较来制定碰撞项。如果轨迹遇到新的障碍物，将只存储结果障碍物信息，使规划器只提取必要的障碍物信息。然后，如果违反动态可行性，我们会延长时间分配。介绍了一种各向异性（轴向和径向）曲线拟合算法，可以调整轨迹的高阶导数而保持原始形状。

 三、规划算法流程

1. 首先使用平滑度，碰撞以及动态可行性优化轨迹
2. 与预先计算ESDF的传统方法不同，通过将障碍物内的轨迹与引导的无碰撞轨迹进行比较对比，来对碰撞成本进行建模
3. 将力投射到碰撞轨迹上并生成估计的梯度以将轨迹包裹在障碍物之外，在优化过程中，轨迹会在附近的障碍物之间反弹几次，最终终止于安全区域。只在必要时计算梯度，避免在与局部轨迹无关的区域计算ESDF。

4. 如果生成的轨迹违反动态限制，这通常是由不合理的时间分配引起的，则激活细化过程。在细化期间，当超出限制时重新分配轨迹时间。随着时间分配的扩大，生成了一种新的B样条，它在平衡可行性和拟合精度的同时，拟合了之前的动态不可行。
5. 为了提高鲁棒性，拟合精度采用各向异性建模，在轴向和径向上有不同的惩罚。

 四、代码流程

1. FindInit(Q,G)：生成一条满足终端约束但不考虑障碍物的B样条曲线
2. IsCollisionFree(E,Q)：判断控制点是否在环境中是无碰撞的，有碰撞时输出false，无碰撞时输出true
3. CheckAndAddObstacleInfo(E,Q)：检测控制点所在障碍物，并添加障碍物信息({p,v}对以及距离场)
4. EvaluatePenalty(Q)：根据问题建模构造控制点相应惩罚项J以及对应的梯度（光滑，碰撞，可行性）
5. OneStepOptimize(J,G)：求解惩罚项的最小化问题，即最优化求解，从而得到满足惩罚项的最小化的控制点位置，即完成第一步的轨迹优化
6. IsFeasible(Q)：判断由控制点Q决定的轨迹是否可行(主要是速度以及其多阶导数否超过限制最大值)
7. ReAllocateTime(Q)：通过重新分配由控制点Q 决定的轨迹的时间降低速度以及其多阶导数，使其满足各类速度约束
8. CurveFittingOptimize(Q)：将之前的惩罚中碰撞项替换为曲线拟合项，求解惩罚项最优化问题。使其在满足新时间间隔的前提下，拟合由旧控制点构成的轨迹得到新轨迹，在继承旧轨迹的无碰撞特性的前提下实现约束下可行性

五、未来优化方向

提出的方法仍然存在一些缺陷，其中一个是A*搜索引入的局部最小值，另一个是统一时间重新分配引入的保守轨迹。因此，作者提到将致力于执行拓扑规划以逃脱局部最小值，并重新制定问题以生成接近最优的轨迹。另外该规划器设计用于静态环境，并且可以处理缓慢移动的障碍物（低于0.5m/s）而不需要任何修改。未来将致力于通过移动物体检测和拓扑规划来处理动态环境导航。