Robust Real-time UAV Replanning Using Guided Gradient-based Optimization and Topological Paths

这篇论文是港科大开源的无人机运动规划fast planner的第二版，这里写下我对这篇工作的理解。

摘要

 基于梯度的方法（GTO）容易陷入局部最优，本文就是提出一个新的基于GTO的方法来解决这个问题。

相关工作

基于梯度的路径优化

 GTO是一种主要的路径生成算法，把路径生成看作一个最小化目标函数的非线性优化问题。

拓扑路径规划

 用拓扑路径来进行规划

路径制导轨迹优化

A. 优化失效分析

 GTO规划的失败和不好的初始路径有关。如下图所示，在欧式符号距离场（ESDF）中，梯度会拉动路径让它离开障碍物，但是如果路径穿越“山脊”或者“山谷”，梯度会是两个完全相反的方向，让路径规划出现问题。

 对于这种情况，仅仅靠ESDF的梯度并不够，需要额外的信息。

B. 问题公式化

 文中提出的PGO方法是对上面GTO的改进，它把路径用B样条来表示。对于PGO方法，分为两步，第一个阶段产生一个过渡的预热轨迹（warmup trajectory），然后对这个warmup trajectory的平滑度和净空度再进行优化。两个阶段如下图：

 a图的绿色是初始B样条轨迹，橙色的是几何引导路径，几何引导路径把初始轨迹拉到没有碰撞的地方形成warmup 路径（蓝色），然后b图中，对warmup路径再进一步进行平滑度和净空度的优化，得到红色最终轨迹。这个几何引导路径通过A或者RRT等传统方法就可以得到，本文用的是采样的方法得到这条引导路径。
 第一阶段的目标函数是：

这里$f_s$是平滑度的约束，具体在第一版的fast planner里有描述：

$f_g$是引导路径和B样条路径之间的距离的惩罚函数：

这里的$Q_i$是B样条的控制点，$G_i$