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

2023-05-16

fastplanner2

摘要
相关工作
- 基于梯度的路径优化
- 拓扑路径规划
路径制导轨迹优化
- A. 优化失效分析
- B. 问题公式化
拓扑路径搜索
- A. 拓扑等价关系
- B. 拓扑路径图
- C. 路径缩短和修剪
实时拓扑路径规划

这篇论文是港科大开源的无人机运动规划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 f_s fs是平滑度的约束，具体在第一版的fast planner里有描述：

f g f_g fg是引导路径和B样条路径之间的距离的惩罚函数：
在这里插入图片描述
这里的 Q i Q_i Qi是B样条的控制点， G i G_i Gi是几何引导路径（就是上面图蓝色线）上对应的点，通过均匀采样得到。这个时候的路径周围的梯度变化都比较平滑，就可以用标准的GTO算法来优化轨迹了。
第二阶段是用B样条曲线对warmup的路径进一步优化，来得到一个安全、平滑、动态可行的路径，这个阶段的目标函数是：在这里插入图片描述
f c f_c fc是碰撞约束，和障碍物近的时候就变得很大。 f a f_a fa和 f v f_v fv分别是对加速度和速度的惩罚，细节都在第一版fast planner的paper里。

拓扑路径搜索

上面讲了用一个几何引导路径得到了一条相对最优的路径，但是这条路径可能并不是最好的。本文提出了一个采样的方法来寻找几条不同的路径来引导上面PGO。

A. 拓扑等价关系

这部分主要是关于路径的等效性，以前的VD算法计算复杂度太大，文章提出了一个均一可视变形方法（UVD）来有效地捕获大量有用的路径，对于路径等效性的检查非常高效。UVD算法流程：
在这里插入图片描述
这里路径等效性的定义就是：对于两条路径，如果起点和终点相同，而且两条路径上对应的采样点，譬如采样点对 Q i Q_i Qi和 G i G_i Gi，每个采样点对之间的连线都没有碰撞，那么这两条路径就等价。譬如下图：
在这里插入图片描述

B. 拓扑路径图

这节是引入两种点来构造一个UVD地图，分别叫做guard和connector，guard用来探索新的区域。开始的时候，在起点 s s s和终点 g g g分别构建两个guard点。connector点用来连接两个guard点形成一条路径，或者取代原来的connector点来形成一条更短的路径。整个算法被限制在 t m a x t_{max} tmax的执行时间内和 N m a x N_{max} Nmax采样次数内。在生成UVD地图后，采用深度优先算法来寻找 s s s和 g g g之间的路径。具体如下图所示：
在这里插入图片描述
任何两个guard节点之间是看不到的，也就是它们的连线是碰撞的。每次在地图上采样一个点，如果这个点另外的任何一个guard都看不到，那么这个点就记作一个新的guard点。然后继续采样，如果一个采样点刚好可以被两个guard点看到，那么就把这个点记作connector，然后把这个connector和这两个guard点连起来。连接好以后做两个事：如果这是一条全新的拓扑路径，那么就保留，否则判断两条拓扑路径的长度，把长的那条去掉。

C. 路径缩短和修剪

像下面这个图所示：
在这里插入图片描述
通过算法1找到的部分路径很曲折，譬如橙色的那条路径，这节的内容就是让路径更平滑，也就是为上面Alg.1算法找到的所有拓扑路径 P r P_r Pr找到一条拓扑相等的捷径 P s P_s Ps来替代。具体如下图：
在这里插入图片描述
整个流程是：先把Alg.1找到的路径 P r P_r Pr拆分成几个离散的的点 P d P_d Pd，每次循环中，如果 P d P_d Pd里的点 p d p_d pd和 P s P_s Ps里的最后一个点之间看不到，然后就找到那个阻挡视线的占据体素，把它往外推，然后加到 P s P_s Ps点集合里去。这个循环一直进行直到最后一个点。这个算法具体如下所示：
在这里插入图片描述

实时拓扑路径规划

第4节中的算法输出了一组有效的路径，可以指导轨迹优化。我们将它们与PGO进行适当的集成，以便实时重新规划。如果检查到碰撞就调用一次拓扑路径地图重建的算法，之后就是缩短路径和剪枝，再之后每条拓扑路径调用PGO算法优化一遍。
值得注意的是，UVD类路径的等效路径会随着障碍物的增加呈指数增长。所以我们每次只选几条路径，譬如 K m a x K_{max} Kmax条，然后对于长度大于最短路径 r m a x r_{max} rmax倍的路径我们也会丢弃掉。

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)