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 domains3、Replanning with RRTs
原理相比于最原始的 RRT 算法的一些缺点，提出的一种改进的 RRT 算法 为了加快随机树到达目标点的速度，简单的改进方法是：在随机树每次的生长过程中，根据随机概率（0.0 到 1.0 的随机值 p）来选择生长方向是目标点还是随机点。2001 年，LaValle在采样策略方面引入 RRT GoalBias 与 RRT GoalZoom，RRT GoalBias 方法中，规划器随机采样的同时,以一定概率向最终目标运动；RRTGoalZoom 方法中，规划器分别在整个空间和目标点周围的空间进行采样。 和普通RRT的区别仅在于随机撒点的时候有区别，这个p越大，算法越快，但对于复杂地形，可能会陷入局部极小处，反而变慢。一般取p=0.1程序示例参考Rapidly-Exploring Random Trees: A New Tool for Path Planning
2.1 原理双树RRT是在原本RRT的基础上多加了⼀颗随机探索树，各自从起点和终点向外探索拓展，直到两棵树相遇时规划算法收敛。这种改进过的探索策略可以⼤⼤提⾼RRT的运⾏效率。 双树RRT中存在两颗随机树，我们将其命名为A和B，A以起点为根节点，B以终点为根节点。两颗随机树的拓展方式和单树RRT的别无二致，同样都需要经历随机采样+步⻓限制+碰撞检测这三个步骤，但是不同的地⽅在于双树RRT的随机树是交替生长的，⽐⽅说第⼀轮迭代中A树向外⽣⻓，第⼆轮便切换为B树⽣⻓，如此循环。 在每轮迭代中，随机树除了向外拓展之外，还会多出⼀个步骤，就是遍历另一颗随机树中的所有节点，找出离NewNode最近的节点，用于判断两颗随机树是否相遇。 假设算法经历了N次迭代以后，已经拓展出如下图所示的两颗随机树。并且在下⼀轮迭代中，轮到A树进⾏拓展，A树在图中⽤绿⾊线条表示，B树⽤黄色线条表示。当进⼊本轮迭代后，算法成功拓展出A树的新节点NewNode_A，此时算法将遍历B树中的所有节点，找出B树中离NewNode_A最近的节点ClosestNode_B。并判断⼆者是否满⾜步⻓限制以及是否可以通过步⻓检测。如下图所示，这种情况明显⽆法通过碰撞检测。那么A树和B树在这⼀轮迭代中⽆法相遇，需要接着下⼀轮迭代。进⼊下⼀轮迭代，这次便切换为B树进⾏拓展，假设算法拓展出的NewNode_B以及遍历A树后得到的ClosestNode_A如下图所示，经过判断发现⼆者满⾜步⻓限制并且通过了碰撞检测，那么这时A树和B树就成功得相遇了，规划算法收敛当算法收敛以后，只需在两棵树的相遇处分别沿着⽗节点回溯便可以找出从起点到终点的有效路径。注意：双向RRT和RRT的区别不仅仅是在于双向生长，双向RRT比RRT更“贪心”，相比于RRT在生长RRT树的时候，是每产生一个随机点，如果能通过碰撞检测，就往该随机点的方向生长一次，然后该随机点就被废弃了，下一步想继续生长RRT树的话，就只能继续生成新的随机点，每个随机点最多利用一次。而双向RRT在生长RRT树时，先先生成一个随机点，然后，该树往该随机点的方向生长，直到碰到障碍物或则生长到该随机点，这样，一个随机点就被多次利用，加快了速度。2.2 伪码2.3 程序示例2.4 收敛性分析双向RRT的收敛性分析可以应用RRT的收敛性分析2.5 参考1、RRT-Connect: An Efficient Approach to Single-Query Path Planning2、https://www.guyuehome.com/9405
1.1 RRT算法思路我们有两个节点，一个绿色的起点，一个黄色的终点对于RRT，我们做的第一件事就是将起点设置为随机树的根，那么我们就拥有了一颗只有根节点的树这棵树光秃秃的，只有根节点的话不但难看，而且还没用。那么我们这时候就需要从这个根节点出发，向外拓展出新的叶⼦。拓展的方式很简单，就是随机采样+步⻓限制+碰撞检测。 RRT在每轮迭代中会⽣成⼀个随机采样点NewNode，如果NewNode位于自由区域，那么我们就可以遍历随机树中已有的全部节点，找出距离NewNode最近的节点ClosestNode。利⽤距离函数dist(NewNode, ClosestNode)得到二者之间的距离，如果满足步长限制的话，我们将接着对这两个节点进⾏碰撞检测，如果不满足步长限制的话，我们需要沿着NewNode和ClosestNode的连线⽅向，找出⼀个符合步长限制的中间点，用来替代NewNode。最后如果NewNode和ClosestNode通过了碰撞检测，就意味着二者之间存在边（edge），我们便可以将NewNode添加进随机树中。首先以第一轮迭代为例，因为刚开始我们的随机树中只有根节点，所以⽆论NewNode位于何处，遍历出的最近节点ClosestNode必然是根节点。 假设我们遇到下图这种情况，虽然采样点NewNode位于步⻓限制之内，但是却很不巧没有落在自由区域，即采样点落在障碍物的位置时，这个采样点会被算法舍弃。假设我们的步⻓限制为R，也就是说对于每个ClosestNode节点来说，只有当NewNode落在其半径为R的圆的范围内时，这个随机采样点NewNode才有可能被直接采纳。如下图所示，该红⾊随机采样点虽然位于⾃由区域，但是明显在根节点的步⻓限制之外。不过这个节点并不会被简单粗暴地舍弃。⽽是会沿着ClosestNode和NewNode的连线，找出符合步⻓限制的中间点，将这个中间点作为新的采样点。如下图所示，蓝点就可以替代红点作为新的采样点。那么假设我们已经通过第⼀轮迭代拓展出第⼀个叶⼦节点A，毫⽆疑问地A的⽗节点就是根节点，假设我们第⼆轮迭代的随机采样点NewNode为图中的点B，B落在A的步⻓限制范围内，但是A，B之间由于障碍物的阻挡，⽆法通过碰撞检测，于是B就会被算法舍弃。假设我们的随机采样点是下图中的B’，明显B’位于⾃由区域，满⾜步⻓条件，并且可以通过与点A的碰撞检测，那么我们就在B’和A之间添加⼀条边，并且将A设置为B’的⽗节点。学过数据结构的⼩伙伴⼀定知道，在树结构中每个节点最多只有⼀个⽗节点，⽗节点可以拥有多个⼦节点。在经历了N轮迭代后，我们已经获得了⼀颗如下图所示的随机树，这时我们发现此时的随机采样点竟然幸运地落在了终点的步⻓限制范围内，并且⼆者之间不存在障碍物。这时我们便可以认为，该采样点和终点之间存在⼀条边，于是将该节点设为终点的⽗节点，并把终点添加进随机树。此时算法就可以结束迭代了，即规划算法收敛。当规划算法收敛以后，只需要从终点开始，沿着其⽗节点进⾏回溯，就可以找 到起点-终点之间的有效路径。那么总结⼀下，RRT⽣成的每轮迭代中都包含以下这些流程：1. ⽣成⼀个随机采样点NewNode，并判断采样点是否位于⾃由区域2. 遍历随机树，找出距离NewNode最近的节点ClosestNode3. 判断NewNode是否在ClosestNode的步⻓限制范围内，否则寻找中间点替代NewNode4. 判断NewNode和ClosestNode之间是否存在障碍物，即碰撞检测。5. 如果NewNode满⾜以上所有约束条件，则将NewNode添加进随机树，设置ClosestNode为NewNode的⽗节点。6. 判断NewNode是否在终点的步⻓限制范围内，并对其⼆者做碰撞检测。如果满⾜条件则将该NewNode设为终点的⽗节点，并将终点加⼊随机树，即可结束迭代。否则继续迭代。1.2 伪码1.3 RRT的收敛性分析本节主要介绍了该规划⽅法的理论特性。Theorem 1 如果存在长度为k的连接序列，则将 x_init 连接到 x_goal 所预期的迭代次数不超过k/p Pf. 参考S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400. 第392页Theorem 2 Pf. 参考S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400. 第393页Theorem 3 当顶点数趋于无穷时，在 x_init 处初始化的RRT包含 x_goal 的概率将趋近于1 Pf. 参考S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400. 第394页定理1和定理2表示了规划器的收敛率，定理3建⽴了规划器是概率完备的（即，当迭代数趋于⽆穷时，找到解的概率趋于1我们可以看出，由于算法在未达到收敛条件之前是在不断进⾏迭代的，所以只要在规划的起点和终点之间是存在有效的路径，那么只要迭代的次数够多，那么采样点就够多，随机树就长得越茂密，能探索到的区域就⾜够⼴，就必然可以找到有效的路径。所以RRT是概率完备的。但是由于采样点每次都是随机的，所以算法并不能保证找到的路径是最优的路径。因此RRT是⾮最优的。1.4 程序示例1.5 参考1、S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400.2、Rapidly-Exploring Random Trees: A New Tool for Path Planning3、https://www.guyuehome.com/9405
什么是路径规划？ 路径规划（也叫运动学规划），任务是确定控制输入，以驱动机器人从初始配置和速度到目标配置和速度，同时服从基于物理的动力学模型，且能确保机器人在环境中避开障碍。说白了，就是给你一张地图，且已知障碍物分布，以及起始点和目标点的坐标，希望你根据这些信息，找到一条从起点到终点的能绕开障碍物的有效路径，如果可以，还希望这条有效路径尽可能最优（最短），并且希望找到这条有效路径的时间尽可能短（算法足够高效） 目前流行的路径规划分为两大类：基于采样的路径规划和基于搜索的路径规划。运动规划的状态空间是应用于机器人变换的集合，称为位姿空间（configuration space），引入了 C-空间、C-空间障碍物、自由空间等一系列概念，下面介绍一些概念：位姿（configuration）机器人一个位姿指的是一组相互独立的参数集，它能完全确定机器人上所有的点在工作空间 W 中的位置，这些参数用来完整描述机器人在工作空间 W 中的状态。一个位姿通常表示为带有位置和方向参数的一个向量(vector)，用 q 表示。自由度（degrees of freedom）机器人的自由度定义为机器人运动过程中决定其运动状态的所有独立参数的数目，即 q 的维数。位姿空间（configuration space）位姿空间是机器人所有可能位姿组成的集合，代表了机器人所有可能的运动结果，称为 C-空间，也可简记为 C。距离函数（distance function）C-空间中的距离函数定义为该空间中的一个映射
公众号