Robust Real-time UAV Replanning Using Guided Gradient-based Optimization and Topological Paths（论文解析）

论文涉及无人机路径规划领域，提出一种可行的局部最优求解问题，描述了一种用于自主四旋翼导航的 鲁棒轨迹重规划方法。 通过 路径引导优化、 拓扑路径搜索和 并行轨迹优化克服了局部极小。

主要创新点

1. 一种用于实时轨迹重新规划的鲁棒优化方法，path-guided optimization 称为PGO，以提高重新规划的成功率。
2. 一种有效的拓扑路径搜索算法，并将其与所提出的PGO相结合，以更彻底地搜索解空间并产生更好的重新规划。
   拓扑路径搜索在概念上与 “Path deformation roadmaps: Compact graphs with useful cycles for motion planning,” 最接近，但具有重新设计的实时性能算法。

导向路径优化（PGO）

GTO：gradient-based trajectory optimization 基于梯度的轨迹优化，将轨迹重规划公式化为一个非线性优化问题，该问题权衡了平滑性、安全性和动态可行性。但受到局部极小值问题的困扰。

典型的GTO将欧几里德符号距离场（ESDF）的梯度结合到碰撞成本中，以将轨迹推出障碍物。然而，ESDF中存在一些“山谷”或“山脊”，导致一定情况下的轨迹优化失败，如下优化失败的典型示例：

如果原始产生的待优化的轨迹穿过欧几里得符号距离场的“谷”（左）或“脊”（右）（用橙色虚线表示），在实际中表现为从障碍物的位置穿过，或者沿正交于梯度距离场的方向运动，则轨迹会在临界点的相邻部分被推向相反方向。红色箭头表示ESDF的梯度，黄色箭头表示目标函数的梯度。因此，如果轨迹发生碰撞并穿过这些区域，ESDF的梯度将在某些点突然改变。为了解决这个问题，必须引入额外的信息，这些信息可以产生一个目标函数，该目标函数的梯度始终使轨迹变形到自由空间。

PGO：一种新的基于GTO的重规划方法， path-guided optimization 路径引导优化，利用几何路径有效地指导优化 ，重规划的成功率得到保证。调用PGO以更彻底地探索解决方案空间

PGO由两个不同的阶段组成：

1. 生成中间预热轨迹
2. 采用B样条优化方法，将预热轨迹进一步细化为平滑、安全、动态可行的轨迹

1阶段 使用几何引导路径来将初始轨迹吸引到自由空间如下图，无碰撞路径很容易从传统的方法（如A* 和RRT* ）中获得。首先将B样条轨迹向引导路径上变形（左图），完成后大部分的轨迹会处于安全区域，即使还有部分碰撞障碍物，这时继续采用GTO的方法完成最后的轨迹质量优化（右图）。

第一阶段目标函数为：

fs是轨迹的平滑度成本，而fg是惩罚引导路径和B样条轨迹之间的距离的成本。
B样条曲线中每个控制点Qi被分配有引导路径上的关联点Gi ，沿着指导路径被均匀地采样。

2阶段 B样条优化方法，进一步将预热轨迹细化为平滑、安全且动态可行的轨迹，其目标函数为：

fc是ESDF上评估的碰撞成本，如果弹道接近障碍物，碰撞成本会迅速增加。fv和fa惩罚不可行的速度和加速度。

虽然与以前的方法相比，PGO多了一步优化，但它可以在更短的时间内生成更好的轨迹。第一阶段只需要微不足道的时间，但会生成更容易进一步细化的预热轨迹，从而提高整体效率。

拓扑路径搜索

基于采样的拓扑路径搜索，以找到一组不同的路径来指导PGO
拓扑等价关系：

左图中四个轨迹在同源的定义下是等价的，但表示四旋翼的基本不同的运动。右图中蓝色轨迹与绿色轨迹不同，但与紫色轨迹等价。看两条轨迹在引导变形时是否碰到障碍物。

均匀能见度变形 UVD 是 可见度变形VD特殊的子集：

UVD（左）和VD（右）之间的比较。一条路径上的每个红色点都转换为另一条路径中的一个点（绿色）。任何两个相关点对应于UVD中的相同参数，但VD中参数不相同。
s表示在轨迹的起点到终点由0到1变化的过程，在轨迹中按照点到起点的距离均匀分配。取拓扑等价的导向路径作为引导，在变形过程中就不会触及障碍物。用线段 s1s2检查碰撞。

拓扑路线图的生成 ：

红色和绿色节点分别表示防护装置和连接器。（a） -（c）：增加防护装置以占据不同的空间区域，并增加连接器以在防护装置之间形成新的连接。（d） ：新连接器取代旧连接器，使连接更短。（e） ：深度优先搜索找到的一些路径。红色和橙色路径都属于同一个UVD类，即等效，而粉色路径是其UVD类的唯一成员。
产生拓扑路线图的算法伪代码：

等效路径表示属于同一个UVD类，伪代码中若有理解错误的地方欢迎理论区讨论。

长路径缩短方法：深度优先搜索找到的每个Pr找到拓扑上等价的快捷路径Ps。

迂回而长的路径（绿线）被缩短。对于原始路径上的每个离散点（绿色点），检查其对缩短路径最后一个航路点（红色点）的可见性。块体素的中心点（虚线）将被推送并附加为新的路点。
将原有的绿色路径离散为许多点，起点作为新路径上的点，从起点开始遍历每一个点，与新路径的最后一个点用线段连接，如果线段被障碍物阻拦，这时为新路径添加路径点，路径点由碰撞的障碍物栅格位置沿线段的正交方向延伸一小段距离，添加路径点后继续遍历原有的路径离散点，与新产生的路径点连接成线段继续判断障碍物产生新的节点，直到遍历到终点后将终点加入新路径中。

实时轨迹重规划 一旦检测到冲突，就会在立方体内触发拓扑路线图构建，