fast planner中拓扑路径搜索

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

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

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

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

fast planner中拓扑路径搜索_第1张图片

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

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

fast planner中拓扑路径搜索_第2张图片

 拓扑线路图的生成

算法:

fast planner中拓扑路径搜索_第3张图片

引入两种不同的图节点,即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)以终止循环。 

图示:

fast planner中拓扑路径搜索_第4张图片

具体分析:

整体流程

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

路径的缩短

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

fast planner中拓扑路径搜索_第5张图片

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

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

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

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

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

效果:

fast planner中拓扑路径搜索_第6张图片

路径的修剪

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

fast planner中拓扑路径搜索_第7张图片

 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略多,但等价性检查的成本要低得多。

你可能感兴趣的:(经验分享)