路径规划（五）Extended_RRT

5.1 原理

        在现实世界的场景中，通常会出现这样的情况:有关环境的初始可用信息是不完整的，或者环境本身是动态的。在这些情况下，当接收到新信息时，初始解决方案可能会失效，例如通过机载传感器。当这种情况发生时，通常会放弃当前的 RRT，并从零开始生长新的 RRT。这可能是一项非常耗时的操作，尤其是在规划问题很复杂的情况下。另一方面，在确定性规划界存在重规划算法，当这种变化发生时，它们能够有效地修复之前的解决方案，而不需要从头重新规划。这就是通过连续域规划路径的问题，如果每次更新地图，都用RRT重新规划，效率相当低下。Extended_RRT则专门用于解决这种动态路径规划问题。

        Extended_RRT的思路是这样的：在老地图中，由RRT算法得出的路径，在障碍物动态变化不太大的前提下，在新地图中，大概率也是能通过的，就算障碍物变化很大，之前的路径也或多或少包含了当前障碍物区域的信息，所以，在新障碍物区域中，在重新规划路径时，原有的初始RRT路径的信息可以利用上，而没有必要完完全全重零开始用RRT来规划。

        那么如何利用上初始RRT路径的信息呢？

思路如下：

        在老地图上先用RRT算法的处一条初始路径，将这条初始路径上的节点存储在集合waypoints中，当环境更新后，希望在新地图上得出一条路径，在随机撒点的步骤，新的随机点有概率p落在目标节点处，此外，还有r的概率落在waypoints的节点中，剩余1-p-r的概率在目标区域内随即撒点。这样在重规划时，就可以把初始路径的信息利用进来。完成随机点选取后，剩余的碰撞检测、树的生长、路径回溯步骤与RRT一致。

总结：

Extended_RRT适用于需要反复路径重规划的场景中，效率比直接重新进行RRT要高得多，和RRT的主要区别在于，在选取新的随机点时，利用上了初始路径的信息，而不是完全随机撒点。

5.2伪码

5.3 程序示例

5.4 参考

1、Real-Time Randomized Path Planning for Robot Navigation* 

2、Extended RRT algorithm with dynamic N-dimensional cuboid domains

3、Replanning with RRTs