开始
本人很懒，习惯了只看不写。废话少说，直奔主题：原始RRT算法原理图文简介（图都是我自己按照步骤一幅幅画的——闲的蛋疼，但应该比较直观易懂，能被借鉴参考也算我的功德）。 RRT是一种通用方法，原理也较为简单，不受机器人类型和自由度的约束。下面按结合一组过程实例化的图片来讲解一下RRT实现路径规划的原理。
第一步：如图所示：在地图中添加了机器人的起点（蓝色）和终点（黄色），黑色部分表示障碍物。将起点初始化为生长树的根节点（此时的树只包含起点这一个节点，就像还只是一颗种子）；
第二步：向地图中的自由空间（非障碍区）随机位置生成一个随机点（图中三角形），也随便举个栗子画一下；
第三步：以刚刚生成的随机点为目标，遍历生长树上的现存节点，计算每个节点到该随机点的距离，筛选出距离最小的节点作为最近点。此时树上仅存在起点（一颗没发芽的种子），所以直接选取起点为最近点。以最近点和随机点的连线（图中我牵的红线）为生长方向（随机点仅起确定生长方向的作用）； 第四步：从最近点向目标点生长，生长的长度为步长，（每一次生长的步长是固定的，步长看情况设定：太短导致算法搜索速度变慢，太长导致生长会跨过障碍物），从此时的最近点也就是起点沿着生长方向生长一个步长得到一个生长点（空心圆圈）； 种子终于要发芽了！ 第五步：判断新生成的生长点是否与障碍物有碰撞，若没有碰撞则将生长点添加到树上（发芽成功），若碰撞了就剔除该生长点，生长作废（发芽失败，等待重新发芽），图中很明显是没有碰到障碍物，发芽成功！此时的生长树上就存在了两个节点（蓝色）； 第六步：再次生成随机点（我们根据图慢慢来讲解啊） 第七步：以刚刚生成的随机点为目标，遍历生长树上的现存节点，计算每个节点到该随机点的距离，筛选出距离最小的节点作为最近点。和上面第三步相同，计算筛选出此时生长树上离随机节点最近的最近点依然是起点，然后以最近点和随机点的连线为生长方向（继续牵线）； 第八步：重复了第四步：从最近点向目标点生长，得到了新的生长点； 第九步：判断是否成功，又重复了第五步。。。很明显又成功了，你说气不气人？把新的生长点纳入生长树中（现在树上已经有三个节点了） 第十步：再来！重复操作，加随机点！看图！（争取把多种情况实例化说一遍，针对小白） 第十一步：同第三步，这次筛选出来的最近点终于不是起点了，而是第九步最新纳入生长树上的节点，直接牵线； 第十二步：如图，最近点向随机节点靠近一个步长，生成了一个生长点（长到了障碍物里面）。 第十三步：生长点都长障碍物里面去了当然发生了碰撞啊，还撞得不轻，生长失败！（红色警告） 剔除该生长节点，此次生长作废，不合格，树不接受。 第十四步：此处省去好多步（受不了了，终于写出这句话，憋了好久），重复上述操作：加随机点，找最近点，向点生长产生生长点，碰撞检测，更新生长树，直到有树节点进入了终点的设定邻域（她终于进入了他的视线），表示路径规划划成功！很多步后效果大致是这个样纸： 第十五步：从终点开始依次回溯父节点（终点找它爸爸，他爸爸找他爷爷，一直找到起点为止）把这些节点按（辈分）顺序提取出来就构成了路径。图中红色线段就是本次实例的路径结果。 END~
