rrt算法流程图_基于采样的路径搜索算法代码实现（RRT和PRM）

1、采样法

利用网格法可以得到对特定离散化后C-space的最优解。但网格法的缺点是计算复杂度高，适用于自由度较低的机器人。本文简要介绍另一种基于采样的规划器，它涉及以下函数，通过这些函数构建一个图(graph)或树(tree)，用于表达机器人的可行运动：

一个随机或者确定函数从C-space或state space中选择样本点

一个用于评估样本点或者运动是否在

空间的函数

一个选择之前自由空间里邻近样本点的函数

一个局部规划器，用于连接新的样本点

采样法放弃了网格法得到的分辨率最优解(resolution-optimal solutions)，由此换来了在处理高自由度机器人规划问题的满意解(satisficing solution)，且求解速度快速。所选择的样本点构成了一个路图或搜索树。相比网格法，采样法可通过更少的样本点来逼近

，因为随着搜索空间自由度的增加，网格法的样本点数目是以指数形式增长的。大部分采样法是概率完备的，也就是说，在有解的情况下，当样本点数目趋于无穷大，找到解的概率趋于100%。

目前有两类算法基于采样法：快速搜索随机树(RRT)和 概率路图(PRM)。RRT采用了树结构在C-space或state space中进行单次规划(single-query planning)，PRM主要是在C-space中创建路图，可实现多次规划(multiple-query planning)。下面分别介绍这两种算法的原理和Python代码实现。

2、RRT算法

RRT算法搜索了一条无碰撞运动，用于从初始状态

到达目标集合

。当用于运动学问题时，

代表了位型

，当用于动力学问题时，

则同时包含了速度。RRT算法从

开始生成一个树，以下是RRT算法的伪代码：

1: initialize search tree T with xstart

2: while T is less than the maximum tree size do

3: xsamp sample from X

4: xnearest nearest node in T to xsamp

5: employ a local planner to find a motion from xnearest to xnew in the direction of xsamp

6: if the motion is collision-free then

7: add xnew to T with an edge from xnearest to xnew

8: if xnew is in Xgoal then

9: return SUCCESS and the motion to xnew

10: end if

11: end if

12: end while

13: return FAILURE

以一个典型的运动学问题为例(x代表q)，第3行将从均匀分布的

中随机地(略倾向于

)选取一个

。第4行中的

是搜索树

中距离

最近的节点。第5行中的

可以选择[

,

]线段上的一点，距离

有一小段距离

。由于

很小，可以用一个简单的局部规划器返回一条直线运动，连接

到

，如果运动是无碰撞的，

将被加入到搜索树

中。

均匀分布的样本点会让生成的树快速探索

空间，下图是由RRT生成的一个树：

RRT算法生成树

从上面的伪代码可以看出在编程时我们有较多的选择，包括如何选择样本点(第3行)，如何定义最近点(第4行)，如何规划向的运动(第5行)。即便对采样法做很小的改变，都可能对规划时间产生很大的影响。下面分别对这三行做进一步描述。

2.1、第3行：采样器(Sampler)

最常见的方法是随机地从均匀分布的

空间采样。比如，对于欧几里德C-space

，以及

关节机器人的C-space

，我们可以在每个关节上均匀采样，对于平面移动机器人的C-space

，可以分别在

和

上均匀采样。

对于动态系统，则是在状态空间中均匀采样，分为两部分：在C-space中均匀采样和在有界速度集上均匀采样(即

)。

实际上，样本点的也不一定需要随机选择。比如可以采用一个确定的采样方案，在

空间生成逐渐细化的网格，最终会在状态空间产生浓密的样本点，当样本点无穷多时，样本将与

空间无限接近。

2.2、第4行：定义最近节点(Nearest Node)

寻找最近节点依赖于在

空间中如何定义距离。对于在

空间上无运动约束的机器人来说，一个很自然的选择就是采用欧几里德距离。对于某些空间，距离的定义就不是这么明显了。

比如平面上的小车机器人，它的C-space表达为

，下图中哪一个与

更近呢？

哪一个位型和灰色的最近？

由于运动约束的存在，小车无法原地转动或侧向移动，为了朝着移动，那么在它后面的那个位型是最佳的位置。因此应该被定义为去往最快速的节点，然而要计算出来是很困难的。一种简单的选择是在每个维度上设置权重，然后计算的加权和。权重描述了不同维度的重要性 。

2.3、第5行：局部规划器(Local Planner)

局部规划器的作用是寻找到一条路径，从