fast planner中拓扑路径搜索

首先是对应文章中的拓扑路径搜索原理进行介绍：

为什么要进行拓扑路径搜索？

即为了生成几何引导轨迹(拓扑路径=几何引导轨迹)。why?

原因：GTO仅依赖于ESDF提供的梯度信息时，在通过“谷”(a)或“脊”(b)等类型的复杂障碍物时，ESDF梯度会发生突变，导致整体的目标函数的梯度在轨迹的不同部分产生相反的方向，从而导致轨迹优化失败。

因此：1、引入了额外的信息产生一个几何引导路径即拓扑路径

            2、将碰撞段中的轨迹重规划分为两阶段：第一阶段，通过几何引导路径产生预热轨迹(a)；第二阶段，通过GTO利用ESDF的梯度信息，将预热轨迹进一步的优化为平滑、安全、动态可行的轨迹(b)。

 拓扑线路图的生成

算法：

引入两种不同的图节点，即guards和connectors，类似于PRM [19]的可视性。

guards负责探索自由空间的不同部分，并且任意两个guards ，g1和g2彼此不可见(g1g2连接线处于碰撞中)。

在主循环之前，在起点s和终点g创建两个guards。

每次一个采样点对所有其他guards不可见时，在该点创建一个新的guard(第6-7行)。

为了形成路线图的路径，connectors用于连接不同的guards(第7-19行)。

当一个采样点正好对两个guards可见时，创建一个新的connectors，或者连接guards以形成拓扑上不同的连接(第19-20行)，或者替换现有的connectors以形成更短的路径(第16-17行)。

设置时间限制(tmax)或采样数限制(Nmax)以终止循环。 

图示：

具体分析：

整体流程

1、通过拓扑线路图生成一系列path
2、对冗余的path进行short Prune,并只保留属于不同UVD类前Kmax个最短path
3、首先利用PGO对Kmax个最短path进行优化生成预热轨迹，最后再利用GTO对预热轨迹进行二次优化，最后选取COST最小的path。

路径的缩短

从Alg.1获得的一些路径可能绕道。这样的路径是不利的，因为在PGO的第一阶段它会使轨迹过度变形，使其变得不平滑。因此，Alg.2为通过深度优先搜索找到的每个Pr找到拓扑等价的捷径路径Ps。

该算法将Pr均匀离散为一组点Pd。

在每次迭代中，如果pd中的一个点Pd从Ps中的最后一个点看不到(第3-4行)

则找到阻挡Ps.back()视图的第一个被占用体素的中心(第5行)。

然后，该点在垂直于ld的方向上被推离障碍物，并与pb处的ESDF梯度共面(第6行)

之后，该点被附加到Ps(第7行)，该过程继续，直到到达最后一点。

效果：

路径的修剪

虽然在Alg.1，避免了两个guards之间的冗余连接，在s和g之间可能存在少量冗余路径。为了完全排除重复的路径，我们检查任意两条路径之间的等价性（UVD），并且只保留拓扑上不同的路径。(就是通过UVD，只保留不同UVD的路径，减少路径)

 UVD(左)和VD(右)的比较。一条路径上的每个红点都会转换为另一条路径上的绿点。任意两个关联点对应于UVD中的相同参数s，而VD中不是相同参数s。

它们都定义了两条路径τ1(s)和τ2(s)之间的连续映射，其中τ1(s)上的一个点通过一条直线变换为τ2(s)上的一个点。主要区别在于，对于UVD，点τ1(s1)映射到τ2(s2)，其中s1 = s2，而对于VD，s1不一定等于s2。在概念上，UVD不太普遍，它是VD类型的子集的特征。实际上，它捕获的不同路径的类别比VD略多，但等价性检查的成本要低得多。