【路径规划】(3) RRT 算法求解最短路，附python完整代码

大家好，今天和各位分享一下机器人路径规划中的 RRT 算法，感兴趣的点个关注，文末有 python 代码，那我们开始吧。

---

1. 算法介绍

RRT 算法是由学者 S.M.LaValle 提出来的路径规划算法。该算法是将空间中的一个起始点当作根节点，在空间中利用增量方式进行随机采样，然后向外延伸扩展增加叶节点。重复该步骤直到获得一个从起点到目标点的树，最后回溯找到从根节点到目标点的无碰撞的路径。 

快速搜索随机树（RRT）算法是一种增量式采样的搜索方法，将它应用于路径规划时不需要任何参数整定，也不需要事先对环境信息进行处理和存储，具有良好的适应性，适合解决复杂环境以及多种约束条件的路径规划问题。 

算法优点：

（1）RRT 算法不需要启发式函数，可以在存在复杂障碍物的环境里快速规划出一条安全路径，且更适合实际应用，目前常被用于无人机的运动规划中。

（2）RRT 的快速迭代生长方式决定了其在高维非凸空间的搜索优势。

算法缺点：

（1）稳定性不佳，且内存消耗较大。扩展树生长方向随机，重复规划结果却有很大差异。

（2）随机性采样导致相对效率较低。由于快速扩展随机树有随机采样性，没有目标启发导向，因此在采样过程中有很多无效采样节点，致使效率相对变低。

（3）路径曲折冗余点较多，光滑性不佳。在 RRT 生成的路径中有许多无用的点，这些冗余点使得路径长度增多。

（4）无法在动态障碍物中有效检测路径，不适合于动态环境规划与避障。

---

2. 算法原理

传统 RRT 算法计算流程可分为两个阶段：

第一个阶段。在任务环境中，以算法初始点作为根节点，通过随机采样、逐步迭代地方式，向任务环境中进行扩展，形成一棵随机树，通过判定采样点是否包含目标点或在目标范围内，来判断是否完成随机树的构造，结束采样；

第二阶段。在随机树中从目标点依次往回遍历，直到寻回根节点，即可得到一条从初始点到目标点的有效路径。RRT的具体扩展方式如下图所示。 

首先，以初始点 Pinit 作为随机树的