基于采样的运动规划算法-RRT 作者:半杯茶的小酒杯出处:RR

作者:半杯茶的小酒杯
【基于采样的运动规划算法-RRT】出处:
RRT是Steven M. LaValle和James J. Kuffner Jr.提出的一种通过随机构建Space Filling Tree实现对非凸高维空间快速搜索的算法。该算法可以很容易的处理包含障碍物和差分运动约束的场景，因而广泛的被应用在各种机器人的运动规划场景中。
文章插图
图片来源:
1、 Basic RRT算法原始的RRT算法中将搜索的起点位置作为根节点，然后通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树的叶子节点进入目标区域，就得到了从起点位置到目标位置的路径。
伪代码如下：
文章插图
图片来源:
上述伪代码中， M是地图环境， xinitxinit是起始位置， xgoalxgoal是目标位置。路径空间搜索的过程从起点开始，先随机撒点xrandxrand；然后查找距离xrandxrand最近的节点xnearxnear；然后沿着xnearxnear到xrandxrand方向前进stepsize的距离得到xnewxnew；CollisionFree(M ， EiEi)方法检测Edge(xnewxnew ， xnearxnear)是否与地图环境中的障碍物有碰撞，如果没有碰撞，则将成功完成一次空间搜索拓展。重复上述过程，直至达到目标位置。
文章插图
图片来源::398553223581697@1472033901892/Basic-RRT-algorithm.png
2、基于概率的RRT算法为了加快随机树收敛到目标位置的速度，基于概率的RRT算法在随机树的扩展的步骤中引入一个概率概率p ，根据概率p的值来选择树的生长方向是随机生长(xrandxrand)还是朝向目标位置xgoalxgoal生长。引入向目标生长的机制可以加速路径搜索的收敛速度。
文章插图
基于概率的RRT算法
3、RRT Connect算法RRT Connect算法从初始状态点和目标状态点同时扩展随机树从而实现对状态空间的快速搜索。
文章插图
图片来源:~motionplanning/lecture/lec20.pdf
4、RRT*算法RRT*算法的目标在于解决RRT算法难以求解最优的可行路径的问题，它在路径查找的过程中持续的优化路径，随着迭代次数和采样点的增加，得到的路径越来越优化。迭代的时间越久，就越可以得到相对满意的规划路径。
文章插图
图片来源：
RRT*算法与RRT算法的区别主要在于两点：
1) rewrite的过程。即为xnewxnew重新选择父节点的过程；
2) 随机树重布线的过程；
4.1 Rewrite下面我们看看Rewrite是如何实现的。 RRT*在找到距离xrandxrand最近的节点xnearestxnearest并通过CollisionFree检测之后，并不立即将Edge(xnearestxnearest ， xrandxrand)加入扩展树中。
文章插图
图片来源：
而是以xrandxrand为中心， r为半径，找到所有潜在的父节点集合，并与xnearestxnearest父节点的Cost对比，看是否存在更优Cost的父节点。
文章插图
图片来源：
如下图所示，我们会计算路径xinitxinit->xparentxparent->xchildxchild的Cost=cost1 ，再计算xinitxinit->xpotential_parentxpotential_parent->xchildxchild的Cost=cost2 ，比较cost1和cost2的大小。此处由于xpotential_parentxpotential_parent与xchildxchild之间存在障碍物导致二者的直接连线不可达，所以cost > cost1 ，不需改变xchildxchild的父节点。
文章插图
图片来源：
如下图所示，当路径{xinitxinit->xparentxparent->xchildxchild}的Cost大于{xinitxinit->xpotential_parentxpotential_parent->xchildxchild}的Cost时， RRT*算法会将Edge{xparentxparent->xchildxchild}剔除，并新增Edge{xpotential_parentxpotential_parent->xchildxchild}
文章插图
图片来源：
至此我们就完成了一次Rewrite的过程，新生成的随机树如下。
文章插图
图片来源：
4.2 随机树重布线的过程
文章插图