在这里分享了运动规划方面的一些基本的算法原理和伪代码实现，其中内容可能存在不完善和错误之处，如有读者发现，欢迎批评指正。

基于采样

这一部分是基于采样的路径规划算法，依次介绍RRT、RRT-connect、RRT*。另外，有关RRT*的其他衍生的算法，日后更新…

RRT

1、RRT (Rapidly-exploring Random Trees)，经典的RRT是单树RRT，算法示意图如下：


2、算法伪代码解释如下：


3、RRT的不足
①收敛慢，目标性不强，这一点相似于BFS，为了整体上向目标点方向生长，一种思路是根据随机的概率，采样点随机地选择通过随机采样还是直接使用目标点作为x_rand，即 x_rand = chooseTarget(sample(map),x_goal,p)，另一种思路是双树RRT。
②没有最优解，→RRT*渐进最优解
③采样范围为整个空间

RRT-connect

1、为提高计算效率，可以同时从起点和终点“生长树枝”。就像这样：

2、基本的过程，如图所示：

3、伪代码，这个还比较详细，基本过程如上图，不再细标

RRT*

1、RRT*对RRT的改进
RRT存在一个问题，虽然能找到一条可行路径，但不保证最优。RRT*在RRT的基础上，进行改进，使路径渐进最优，方法是x_near和x_new不会直接连接起来，而是做一个优化处理，令x_new在一定范围内重新选择父节点，同时rewire（随机树重布线）。
2、RRT*示意与伪代码
①示意
重选父节点&随机树重布线的图示点这里，描述的还比较清晰。
②伪代码



（虽然说是伪代码，可能没有很伪代码）

③可参考的更简洁的伪代码：

下面是用QT写的一个简单实现：

参考文献

1、https://blog.csdn.net/weixin_43465857/article/details/96451631 RRT算法原理图解
2、https://blog.csdn.net/gophae/article/details/103231053 全局路径规划：图搜索算法介绍4(RRT/RRT*)
3、https://zhuanlan.zhihu.com/p/30333530 RRT算法是如何实现的？有哪些优缺点？
4、https://zhuanlan.zhihu.com/p/133224593 基于采样的运动规划算法-RRT(Rapidly-exploring Random Trees)
5、https://www.cnblogs.com/21207-iHome/p/7210543.html RRT路径规划算法
6、https://blog.csdn.net/a735148617/article/details/103647804 从零开始的OMPL库算法学习（2）RRT-connect算法
7、https://zhuanlan.zhihu.com/p/51087819 路径规划——改进RRT算法
8、https://www.jianshu.com/p/489b0b39f3f2 运动规划笔记——邱强