机器人路径规划其五六 RRT RRT*

RRT算法和RRT*算法是一种基于随机采样的路径规划算法，其中RRT*是众多RRT变种中比较出名的算法，RRT*解决了RRT无法得出最优路径的问题，只要RRT*算法迭代的次数足够多，就一定能找出最优的路径，但是随之而来的就是规划需要的时间变长。笔者在做本科毕设的时候在为SLAM移动机器人规划路径时用的就是OMPL中的RRT*，如果要得到最佳的路径的话，时间得几秒，但是如果想降低迭代的时间，得出的路径就不是最优的，如起始点和目标点没有障碍物但是规划的路径仍然不是一条直线。但是无论如何，RRT都是一种有效的机器人路径规划算法。

 

---

 Ⅰ.RRT Algorithm

RRT全名叫做 Rapidly-exploring random tree，

1. 起始时树的根节点就是起始点，然后在图的有效空间内随机采样，采样点可能落在搜索空间内的任一个位置。

2. 一旦一个随机点被选定，算法就会尝试将该随机点和离RRT树最近的一个元素连接，如果该连接在限制条件以内，如连接长度小于设定的步长且两点之间无障碍物，则将该随机点纳入RRT树，成为其一个新的元素，该元素的父节点为离它最近的那个点；如果该点到树中某点的最近的距离比设定的步长要大，则可以选择以两点相连的直线为方向，以设定的最大步长为长度，重新选择这个直线的端点作为树的新元素，而不再选用生成的随机点。

3. 迭代的过程中会对每个点判断点和目标点是否能够直接相连，如果直接相连，则可以提前退出搜索。

伪代码如下：

Algorithm BuildRRT
    Input: Initial configuration qinit, number of vertices in RRT K, incremental distance Δq)
    Output: RRT graph G

    G.init(qinit)
    for k = 1 to K do
        qrand ← RAND_CONF()
        qnear ← NEAREST_VERTEX(qrand, G)
        qnew ← NEW_CONF(qnear, qrand, Δq)
        G.add_vertex(qnew)
        G.add_edge(qnear, qnew)
    return G
"←" denotes assignment. For instance, "largest ← item" means that the value of largest changes to the value of item.
"return" terminates the algorithm and outputs the following value.

 

---

  Ⅱ. RRT* Algorithm

先来看一段RRT*的伪代码：

 

 RRT*大体步骤和RRT相同，只不过多了一个1.Connect along a minimum-cost和一个2.Rewire the tree 的步骤，就是这两个步骤使得RRT*可以得到最优的路径，

1. 步骤1的就是为随机选中的点重新选择父节点，使得该点到起点的cost能够最小，至于cost的定义，就是路径的长度，看具体情况选什么类型，其中父节点的选择可以是该节点附近相连的所有点

2. 步骤2就是在重新选完父节点后，为该节点附近区域的所有树中的节点重新布线，即rewire，布线的原则是使所有节点到起始点的cost最小

经过以上两个步骤，随着迭代次数的增加，就一定能够找到最佳的路径。

 

在知乎上看到一个很好描述上述两个过程的图，分享如下：原文链接：https://zhuanlan.zhihu.com/p/51087819

 

 

 

 Reference:

[1] https://en.wikipedia.org/wiki/Rapidly-exploring_random_tree#cite_note-incremental-8

[2] https://zhuanlan.zhihu.com/p/51087819