RRT算法的基本程序模块（2）

RRT 效果图

承接上一节，在接下来的几篇文章中，作者将详细解剖机器人的避障路径规划算法。

快速随机搜索树（RRT）算法是基于随机采样的路径规划算法，它相比于其他算法的一个优势在于可以有效地将非完整约束考虑在算法内部，从而避免了复杂的运动学约束的考虑，使得路径规划问题简单化。

RRT算法中包含了一些基本的函数模块，用于完成整个路径搜索过程。

采样：对应函数Sampling

Sampling函数中，首先在构型空间X中随机取一个点，这些采样点需要服从相互独立，恒等分布，为了简化，采样点需要满足均匀分布，同理，SampleFree就是在自由空间X_{free}中随机采样，并且满足与Sampling相同的要求。

最近节点 ： 对应函数Nearest

给定一个图G(V,E)，V属于X，E表示将节点集V中两个相连的边， 定义函数Nearest: （G,x）-> v \in V ，定义距离的衡量函数

距离函数

附近节点： 对应函数Near

给定图G=(V,E)，点x \in X, 正实数r \in R. 定义Near: (G,x,r)->V'

附近节点函数，球域

前进一个步长 ： 对应函数Stearing

给定x,y \in X，正实数step \in R， 定义函数Stearing如下：

Stearing(x,y,z):={z:z=x+step(y-x)/|y-x|}

其中，是以x为起点。

碰撞检测： 对应函数 CollisionFree

给定点x,x' \in X, {Y:y \in Y，y=x+t(x‘-x),t \in [0,1]}. 如果Y属于X_{free}，则函数CollisionFree(x,x’)返回值True, 即无碰撞，否则返回False,即有碰撞。如果，x属于X_{free}, 则函数CollisionFree(x,x’)返回True, 否则返回False.

到达目标构型

给定目标构型x_{goal},任一构型x \in X， error \in R， 如果||x_goal-x||<error, 则构型x 落在了目标区域内。这个条件也适用于判断两个树形结构的图是否相遇。

RRT 的伪代码