路径规划（九）快速行进树（FMT *）

9.1 原理

        FMT*算法专门针对解决高维构型空间中的复杂运动规划问题，它是为高密度障碍物的环境构建的算法。该算法被证明是渐近最优的，并且比同类型算法（RRT*）更快收敛到最优解。FMT*算法在预先确定的概率绘制的样本数量上执行“惰性”动态规划递归，以生长路径树，该路径树在成本到达空间中稳定地向外移动。

        FMT*的最终产物是一棵树，它在连续空间中获取一批样本，然后它能在图中使用惰性的动态编程搜索该样本集合，并以此找到路径，这也是一个渐进最优的解决方案，FMT*相比于RRT*的加速效果优势在高维和碰撞检查很昂贵的情况下尤其突出。很棒的一点是，FMT*是从起始位置开始构建，而不是像RRT*是在空间的任意位置采点，因为这可能会得到非常远的点或非常近的点，这有什么好处呢？这意味着你不必在树中回溯以进行重布线，因为这在计算上效率低下。FMT*比RRT*更好，因为它创建的连接接近最佳，没有重布线。

        FMT*算法在预先确定的采样点数量上执行前向动态规划递归，并相应地通过在代价到达空间中稳步向外移动生成路径树。FMT*执行动态规划递归，其特点有三个关键特征:

·它是为磁盘连接图量身定制的，其中两个样本的距离低于给定的界限（称为连接半径）则这两个样本被认为是邻居，因此是可连接的。

·它同时执行图构造和图搜索。

·为了评估动态规划递归中的即时成本，算法“懒惰地”忽略了障碍物的存在，每当与新样本的局部最优(假设没有障碍物)连接与障碍物相交时，该样本就会简单地跳过并留待以后，而不是在邻域中寻找其他连接。

注意：

FMT*的样本点是提前生成好的，然后把这些点固定，再利用这些固定好的点来生成行进树，注意区别于RRT*那一套，RRT*是生成随机点的同时，生成行进树。

9.2 算法思路

上图（a）是RRT*添加新节点的某一步，上图（b）（c）是FMT*添加新节点的某一步。

        先看RRT*，节点9是新考虑的节点，它在第一次重布线时，需要从它的所有邻居节点中找出一个父节点，使得节点9到达起始节点的cost最小，因此节点9需要计算它到4、5、6、8号节点的距离并同时进行碰撞检测，因此，第一次重布线过程就要求待加入的节点对它的所有邻居节点进行一次碰撞检测，第二次重布线过程也需要计算距离和碰撞检测，但这在第一次重布线过程中做过了，可以记录先来直接利用，因此第二次重布线过程碰撞检测这一步可以不用重复进行，因此，总的来说，RRT*每新加入一个节点，该节点需要对它的所有邻居节点进行一次碰撞检测。

        再看FMT*，上图（b）（c）中的x就是新考虑的节点，在图（b）中，x需计算它到集合V_open中所有的邻居节点的cost，但不需要进行碰撞检测，从中选择一个能使它到达初始节点总cost最小的节点作为它的父节点，然后，对它和这个父节点的连线进行碰撞检测，如果能通过碰撞检测，则加入x，若不能，则下一个x，因此，总的来说，FMT*每新加入一个节点，永远只需要进行一次碰撞检测。

FMT*比RRT*每新加入一个节点需要进行的碰撞检测次数少得多，而且FMT*也是渐进最优的，这就是FMT*相比于RRT*的优势所在。

9.3 伪码

9.4 程序示例

下图中的 圆圈代表提前采好的随机点

9.5 参考

Fast Marching Tree: a Fast Marching Sampling-Based Method for Optimal Motion Planning in Many Dimensions∗