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

 

Ⅰ.RRT Algorithm

RRT全名叫做Rapidly-exploring random tree，


RRT三维路径规划代码 python rrt路径规划算法_RRT三维路径规划代码 python

1.


2.

3.

伪代码如下：

1 Algorithm BuildRRT
 2     Input: Initial configuration qinit, number of vertices in RRT K, incremental distance Δq)
 3     Output: RRT graph G
 4 
 5     G.init(qinit)
 6     for k = 1 to K do
 7         qrand ← RAND_CONF()
 8         qnear ← NEAREST_VERTEX(qrand, G)
 9         qnew ← NEW_CONF(qnear, qrand, Δq)
10         G.add_vertex(qnew)
11         G.add_edge(qnear, qnew)
12     return G
13 "←" denotes assignment. For instance, "largest ← item" means that the value of largest changes to the value of item.
14 "return" terminates the algorithm and outputs the following value.

 

 Ⅱ. RRT* Algorithm

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


RRT三维路径规划代码 python rrt路径规划算法_RRT三维路径规划代码 python_02

 

1.Connect along a minimum-cost和一个2.Rewire the tree

1.

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

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

 

在知乎上看到一个很好描述上述两个过程的图


RRT三维路径规划代码 python rrt路径规划算法_Path planning_03

 

 

RRT三维路径规划代码 python rrt路径规划算法_RRT三维路径规划代码 python_04