3.2 随机性采样: RRT

3.2.1 RRT算法概述

• 定义与提出者：RRT算法（Rapidly-exploring Random Tree，快速探索随机树）由Steven M. LaValle和James J. Kuffner Jr.提出，通过随机构建Space Filling Tree实现对非凸高维空间的快速搜索。

• 应用场景：广泛应用于各种机器人的运动规划场景，特别是包含障碍物和差分运动约束的环境。

• 算法变种：包括Basic RRT、基于概率的RRT、RRT Connect和RRT*算法，其中前三种属于单查询方法，而RRT*属于渐近最优算法

3.2.2 RRT算法详解

3.2.2.1 Basic RRT算法

• 基本思想
将搜索的起点位置作为根节点，然后通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树的叶子节点进入目标区域时，就得到了从起点位置到目标位置的路径。

• 基本步骤

1. 第3行：将起点 初始化为搜索树的根节点。

2. 第5-8行：在构型空间内随机（一般使用均匀分布）生成一个节点，并在搜索树中取出距离采样点最近的节点，然后沿着到方向前进得到，继而得到边。

3. 第9-11行：利用方法检测该边是否与地图边界和障碍物碰撞。
1. 若无碰撞，则成功完成一次空间搜索拓展，并将节点和边加到搜索树中
2. 若有碰撞，重新构建

4. 第12-13行：若到达目标点，则搜索树构建完成。

• 优缺点
• 优点
• 适用范围广，参数简单，高维空间规划性能优秀
• 相比较于PRM算法，具有更高的效率
• 缺点
• 得到的并非最优路径，且每次搜索的路径具有随机性
• 在扩展节点时从无障碍区域内随机选择节点，会导致产生部分无用节点，节点利用率低，增加算法随机性的同时也降低了算法的收敛速度

• 参数设置
RRT算法中非常重要的参数即步长，其极大影响搜索树的形状
• 步长太大，可能无法成功绕过障碍物，且在终点附近来回跳动
• 步长太小，搜索树的构建耗时增加，且采样点非常密集

3.2.2.2 基于概率的RRT算法

• 基本思想
为了加快随机树收敛到目标位置的速度，基于概率的RRT算法在随机树的扩展的步骤中引入一个概率，以一定的概率来选择目标点作为随机点。引入向目标生长的机制可以加速路径搜索的收敛速度。

• 基本步骤
整体步骤和基本RRT相同，只需在选择随机点的时候稍作改变，如下：

• 优缺点
• 优点：加速路径搜索的收敛速度
• 缺点：如果目标概率的不断加大，当障碍物遮挡目标时容易陷入局部搜索无法跳出（随机产生的分支太少了）

• 参数设置 引入的概率对随机树的扩展有一定的影响：
• 概率越小()，树的分支越多（过度随机采样，原始 RRT），会导致生长缺乏方向性
• 概率越小()，树的分支越少（过度趋向目标采样，类似最佳优先的贪心搜索），会导致很难绕过障碍物找到目标点

3.2.2.3 RRT Connect

• 基本思想
RRT-Connect分别以起点和目标点为根节点生成两棵树进行双向扩展，当两棵树建立连接时可认为路径规划成功。通过一次采样得到一个采样点，然后两棵搜索树同时向采样点方向进行扩展，加快两棵树建立连接的速度。

• 基本步骤
大概流程和Basic RRT类似，主要不同点在于：
1. 双向扩展
1. 在每次迭代中，随机选择一个采样点。
2. 从随机树和随机树中分别找到距离采样点最近的节点和。
3. 分别从和向方向扩展一定步长，得到新的候选节点和。
4. 检查新节点是否与障碍物碰撞，若无碰撞，则将其添加到对应的树中。
2. 连接检查
1. 在每次迭代后，检查两棵树是否“连接”，即检查随机树中的任意节点是否与随机树中的任意节点足够接近（通常是在某个设定的阈值内）。
2. 如果找到这样的节点对，则通过它们可以构建出一条从起点到目标的路径。

• 优缺点
• 优点：具有良好的搜索特性，相较于Basic RRT的搜索速度和搜索效率均有显著提高
• 缺点：单查询算法，最终路径不是最优的

3.2.2.4 RRT*算法

• 基本思想
Basic RRT，基于概率的RRT和RRT Connect算法虽然能快速的找到路径，但却不是最优路径。而RRT*算法的目标就在于解决RRT算法难以求解最优的可行路径问题，其在路径查找的过程中持续优化路径，随着迭代次数和采样点的增加，得到的路径越来越接近最优。

• 基本步骤



1. 第5-8行：和Basic RRT算法相同，包括随机采样、寻找最近节点、确定新节点和碰撞检测
2. 第9-10行：重新选择父节点，选择标准为以为圆心，为半径，找到所有潜在的父节点集合，并与父节点的cost对比（cost为初始节点到父节点，再到新节点的总cost），若有更优cost的节点，则将其设为新节点的父节点。如下图：当路径的cost小于路径的cost时，算法会将节点设为节点的父节点，并将边删除，新增边。

3. 第11行：将新增的节点和边加入到随机树中。
4. 第12行：重新对随机树进行布线，如果近邻节点的父节点修改为新增的节点，可以减少路径代价，则将新节点设为近邻节点的父节点。如下图：若路径的cost大于路径的cost，则将设置为的父节点，并将边删除，新增边。

• 优缺点
• 优点：通过引入重新选择父节点和重新对随机树进行布线步骤，RRT*算法能够不断优化路径，提高路径的质量和长度。这使得最终生成的路径更加平滑，减少了路径中的冗余和不必要的转折。
• 缺点：与RRT算法相比，RRT算法需要更多的计算资源和时间来进行路径的优化和重新连接。这可能导致在实时性要求较高的场景中，算法的性能受到一定影响。

• 参数设置
半径对算法有一定的影响：
• 太小（）：每次优化的临近点很少，优化力度太小，路径类似与 RRT算法。
• 太大（）：每次优化的邻居点非常多(比如包括起点)，增加计算量，降低搜索效率，而且会引入一些不必要的路径，进而影响路径质量。

3.2.2.4 RRT*的拓展算法（不做详细介绍）

1. Kinodynamic-RRT*: 在传统的RRT*算法中，节点和节点之间直接使用直线连接，其不满足智能体的动力学约束，因此可以将直线更换为其他曲线（例如贝塞尔曲线等）或者使用优化的方式求路径。 Kinodynamic RRT*: Optimal Motion Planning for Systems with Linear Differential Constraints https://www.youtube.com/watch?v=RB3g_GP0-dU

2. Anytime-RRT*: Anytime-RRT*算法能够在机器人执行当前轨迹的同时，不断地更新和优化路径树，这意味着算法能够在机器人运动过程中实时地适应环境的变化，找到更优的路径。 由于算法具有实时优化的能力，因此它特别适合在动态环境中使用。在动态环境中，障碍物的位置和形状可能会发生变化，Anytime-RRT*算法能够及时地调整路径规划，确保机器人能够安全地到达目的地。 Anytime-RRT * 算法路径规划演示 

3. Informed RRT*: RRT*在地图空间中采样进行均匀撒点，采样点会布及整个地图从而导致很多不必要的采样。Informed RRT*把采样的范围限制在一个椭圆里面，以起始点和终点作为椭圆的焦点，以 RRT*生成的路径长度 L 作为椭圆上点到焦点的距离之和，在椭圆内进行采样，随着生成的路径越来越优化，长度越来越短，椭圆也会越来越扁，从而集中采样点进行了有效的路径优化。 Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic

4. Cross-entropy motion planning(交叉熵运动规划): 利用交叉熵方法（Cross-Entropy Method, CEM）来估计稀有事件概率，并将其与采样运动规划方法（如RRT*）相结合。CEM通过迭代地更新采样分布来指导采样过程，使其更加关注于生成低成本的路径或轨迹。这种自适应采样策略有助于在复杂环境中找到高质量的解决方案。 Cross-Entropy Randomized Motion Planning