0. 基于采样的运动规划算法 - RRT (Rapidly-exploring Random Trees)

RRT 是 Steven M. LaValle 和 James J. Kuffner Jr. 提出的一种通过随机构建 Space Filling Tree 实现对非凸高维空间快速搜索的算法。该算法可以很容易的处理包含障碍物和差分运动约束的场景，因而广泛的被应用在各种机器人的运动规划场景中。

BP Network

1、 Basic RRT 算法

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

伪代码如下：

BP Network

上述伪代码中，M 是地图环境，$x_{init}$是起始位置，$x_{goal}$是目标位置。路径空间搜索的过程从起点开始，先随机撒点$x_rand$; 然后查找距离 $x_rand$ 最近的节点 $x_{near}$; 然后沿着 $x_{near}$ 到 $x_{rand}$方向前进 stepsize 的距离得到$x_{new}$; CollisionFree(M, $E_i$) 方法检测 Edge $(x_{new},x_{near})$ 是否与地图环境中的障碍物有碰撞，如果没有碰撞，则将成功完成一次空间搜索拓展。重复上述过程，直至达到目标位置。

BP Network

图片来源:https://www.researchgate.net/profile/Burak_Boyacioglu/publication/306404973/figure/fig1/AS:398553223581697@1472033901892/Basic-RRT-algorithm.png

2、基于概率的 RRT 算法

为了加快随机树收敛到目标位置的速度，基于概率的 RRT 算法在随机树的扩展的步骤中引入一个概率 $p$，根据概率 $p$ 的值来选择树的生长方向是随机生长 ($x_{rand}$) 还是朝向目标位置 $x_{goal}$ 生长。引入向目标生长的机制可以加速路径搜索的收敛速度。

基于概率的 RRT 算法

3、RRT Connect 算法

RRT Connect 算法从初始状态点和目标状态点同时扩展随机树从而实现对状态空间的快速搜索。

BP Network

图片来源:https://www.cs.cmu.edu/~motionplanning/lecture/lec20.pdf

4、RRT * 算法

RRT * 算法的目标在于解决 RRT 算法难以求解最优的可行路径的问题，它在路径查找的过程中持续的优化路径，随着迭代次数和采样点的增加，得到的路径越来越优化。迭代的时间越久，就越可以得到相对满意的规划路径。

BP Network

图片来源：https://blog.csdn.net/gophae/article/details/103231053

RRT * 算法与 RRT 算法的区别主要在于两点：

1. rewrite 的过程。即为 $x_{new}$ 重新选择父节点的过程；
2. 随机树重布线的过程；

4.1 Rewrite

下面我们看看 Rewrite 是如何实现的。RRT * 在找到距离 $x_{rand}$ 最近的节点 $x_{nearest}$ 并通过 CollisionFree 检测之后，并不立即将 Edge (x_{nearest},x_{rand}) 加入扩展树中。

BP Network

图片来源：https://blog.csdn.net/weixin_43795921/article/details/88557317

而是以 $x_{rand}$ 为中心，$r$ 为半径，找到所有潜在的父节点集合，并与 $x_{nearest}$ 父节点的 Cost 对比，看是否存在更优 Cost 的父节点。

BP Network

图片来源：https://blog.csdn.net/weixin_43795921/article/details/88557317

如下图所示，我们会计算路径 $x_{init} \rightarrow x_{parent} \rightarrow x_{child}$ 的 Cost=$cost1$，再计算 $x_{init} \rightarrow x_{potential_parent} \rightarrow x_{child}$ 的 Cost=$cost2$，比较 $cost1$ 和 $cost2$ 的大小。此处由于 $x_{potential_parent}$ 与 ${x_child}$ 之间存在障碍物导致二者的直接连线不可达，所以 $cost > cost1$ ，不需改变 $x_{child}$ 的父节点。

BP Network

图片来源：https://blog.csdn.net/weixin_43795921/article/details/88557317

如下图所示，当路径 ${x_{init} \rightarrow x_{parent}->x_{child}}$的 Cost 大于 ${x_{init} \rightarrow x_{potential_parent} \rightarrow x_{child}}$的 Cost 时，RRT^* 算法会将 Edge${ x_{parent} \rightarrow x_{child}}$剔 除，并 新 增 Edge${ x_{potential_parent} \rightarrow x_{child}}$。

BP Network

图片来源：https://blog.csdn.net/weixin_43795921/article/details/88557317

至此我们就完成了一次 Rewrite 的过程，新生成的随机树如下。

BP Network

图片来源：https://blog.csdn.net/weixin_43795921/article/details/88557317

4.2 随机树重布线的过程

BP Network

图片来源：https://blog.csdn.net/weixin_43795921/article/details/88557317

在为 $x_{new}$ 重新选择父节点之后，重布线使得生成新节点后的随机树减少冗余通路，减小路径代价。

如上图所示，$x_{new}$ 为新生成的节点，4、6、8 是 $x_{new}$ 的近邻节点，0、 4、 5 分别为近邻节点的父节点。

路径 {0->4} 的 Cost 为: 10

路径 {0->4->6} 的 Cost 为： 10 + 5 = 15

路径 {0->1->5->8} 的 Cost 为: 3 + 5 + 1 = 9

先尝试将节点 4 的父节点改为 $x_{new}$，到达节点 4 的路径变为 {0->1->5->9->4}，新路径的 Cost=3+5+3+4=15，新路径的 Cost 大于原路径 Cost，所以不改变节点 4 的父节点。

再尝试改变节点 8 的父节点为 $x_{new}$，到达节点 8 的路径变为 {0->1->5->9->8}, 新路径的 Cost=3+5+3+3=14，新路径的 Cost 大于原路径 Cost，随意不改变节点 8 的父节点。

再尝试改变节点 6 的父节点为 $x_{new}$，到达路径 6 的路径变为 {0->1->5->9->6}, 新的 Cost=3+5+3+1=12, 新路径的 Cost 小于原路径 Cost，因此将节点 6 的父节点更新为节点 9。

重布线后的随机树如上右图所示。

4.3 RRT * 算法效果

从 RRT 与 RRT 的效果可以看出，RRT 的路径规划的结果优于 RRT 算法。

BP Network

RRT^* VS RRT。图片来源：https://www.cc.gatech.edu/~dellaert/11S-AI/Topics/Entries/2011/2/21_PRM,_RRT,_and_RRT__files/06-RRT.pdf

BP Network

RRT^* VS RRT With Obstacles。图片来源：https://www.cc.gatech.edu/~dellaert/11S-AI/Topics/Entries/2011/2/21_PRM,_RRT,_and_RRT__files/06-RRT.pdf

RRT^* 算法 + 赛车动力学实现车辆 180 度转弯。图片来源：https://www.youtube.com/watch?v=KSB_9KE6fWI

参考链接

1、基于 RRT 的运动规划算法综述 (https://wenku.baidu.com/view/8de40fafbdeb19e8b8f67c1cfad6195f312be80a.html)

2、RRT 维基百科 (https://en.wikipedia.org/wiki/Rapidly-exploring_random_tree)

3、PRM, RRT, and RRT*，Frank Dellaer (https://www.cc.gatech.edu/~dellaert/11S-AI/Topics/Entries/2011/2/21_PRM,_RRT,_and_RRT__files/06-RRT.pdf)

4、路径规划 —— 改进 RRT 算法 (https://zhuanlan.zhihu.com/p/51087819)

5、运动规划 RRT * 算法图解 (https://blog.csdn.net/weixin_43795921/article/details/88557317)

6、全局路径规划：图搜索算法介绍 4 (RRT/RRT*)(https://blog.csdn.net/gophae/article/details/103231053)

ref: [1]. https://zhuanlan.zhihu.com/p/133224593 [2]. https://blog.csdn.net/gophae/article/details/103231053 [3]. https://xwlu.github.io/wiki/path-planning/rrt/ [4]. ※ https://dlonng.com/posts/rrt