基于采样的路径规划算法RRT的优化：RRT＊，Kinodynamic-RRT＊，Anytime-RRT ＊，Informed RRT ＊

基本的基于采样的路径规划算法RRT，在地图中进行采样取点，直到新的节点取到终点的一定阈值范围内，视为查找到路径，（ RRT算法详细）。但是在这个规划过程中，找出的路径总是曲折的。为此对RRT算法进行了一系列优化：

RRT * 算法

RRT*算法针对性地解决RRT算法生成路径不最优的缺陷

左图是RRT算法原本生成的不平滑的路径，右图是RRT * 算法伪代码。
RRT * 算法伪代码中，在新产生的节点 x(new) 画一个规定半径的圆，找到最近的节点为 x1, x2, x(near)，分别求 x(new) 经过几个最近节点到达红色起始点的路径大小，取路径最短的节点与 x(new) 连接起来。

RRT * 算法选择父节点没有直接把最近的节点当父节点，而是搜索了多个邻域节点，从而选定父节点。RRT*突出的特点就展现在伪代码中的 rewire() 重写步骤，帮助修改链接过程，使数据更优化。

rewire() 的过程是重写节点的父节点：在上面例子中将 x(new) 其余的两个点 (x1,x2) 与 x(new) 连接，比较（ x1 原来的路径到红色起始点的路径长度 d1 ）和 （x1 经过 x(new) 再到达红色起始点的路径长度 d2），如果后者的路径更短，则会更换 x1 的父节点为 x(new) 。在上面例子中显然 d1 小于 d2 ，所以其父节点不会改变。而 （ x2 原来的路径到红色起始点的路径长度 d1 ）和 （x2 经过 x(new) 再到达红色起始点的路径长度 d2），后者的路径更短，所以 x2 的父节点重写为 x(new)，如下图：

RRT * 算法会通过不断地修改父节点来优化轨迹，从而能够产生一个较平滑的轨迹，
在这个视频：RRT*算法路径规划演示 中可以较快理解RRT *的路径规划过程，以及轨迹优化的过程。

/

Kinodynamic-RRT*

传统的RRT * 算法 x(new) 和 x(near) 的连接直接用直线连接，在下图左图的连接中会碰到障碍物，但实际上根据机器人的动力学约束，实际路径可能不会碰到障碍物。

Kinodynamic-RRT * 算法中用曲线代替直线，让生成的路径更加满足动力学约束，
在这个视频：Kinodynamic-RRT*算法路径规划演示 中可以直观地发现Kinodynamic-RRT* 的路径规划过程。

Anytime-RRT *

Anytime-RRT * 是在机器人运动的过程中不断地更新路径，在机器人执行当前路径时保持最优的树节点路径，即实时的RRT * 算法，能更好地适应机器人运动过程中环境变化比较大的情况。在这个视频：Anytime-RRT * 算法路径规划演示 中可以容易地理解 Anytime-RRT * 的路径规划过程。

Informed RRT *

RRT * 在地图空间中采样进行均匀撒点，采样点会布及整个地图会进行很多不必要的采样。
Informed RRT *把采样的范围限制在一个椭圆里面：
以起始点和终点作为椭圆的焦点，以 RRT * 生成的路径长度 L 作为椭圆上点到焦点的距离之和，在椭圆内进行采样，随着生成的路径越来越优化，长度越来越短，椭圆也会越来越扁，从而集中采样点进行了有效的路径优化。

关于搜索树按搜索方向生长的计算方法

三角函数的方法

先计算出夹角 𝜃 ，采用三角函数的方法沿生长方向延伸：生长长度为 L
𝜃 = arctan（(Xrand.y - Xnear.y) / (Xrand.x - Xnear.x) ）
得到夹角 𝜃 后，计算 Xnear 沿 x 方向 和 沿 y 方向对应的增长值：
Xadd = L * sin𝜃
Yadd = L * cos𝜃
所以 Xnew：（Xnear.x + Xadd , Xnear.y + Yadd）得到生长后的节点。

向量方法

通过上图中的公式得到长度比例关系：k = ||vw|| / ||vo||。（图中有误，最后写反了）
o.x = v.x + (w.x-v.x) * k
o.y = v.y + (w.y-v.y) * k
得到生长节点 o 的坐标