Sampling Based Planners
基于采样的路径规划方法是无需遍历起点到终点的所有栅格的,这就是与A Star及Dijkstra、JPS算法的不同。
作用原理
根据在空间中进行撒点,撒点后线段进行连接的方式构建路线图/树,来形象地表示整个空间。
PRM:基于采样,简化由栅格为主的A*算法与JPS算法,生成图的结构去找就会快速很多
- 学习:learning phase:在环境中随机撒点。(均匀采样)
- 查询:query phase
可以看出部分点落在了障碍物上,因为到时候要使用线段进行连接,所以就会舍去这些落在了障碍物上的点。
线段连接的限制
- 远点不要
- 过障碍物的点不要
Learning Phase
- 距离准则
- 称那些不在障碍物当中的点为Collision-Free(障碍物准则)
PRM影响效率的原因
就是因为要找所有在障碍物当中的点,所以导致采样搜索构建树的时间很长。
解决方法,先不进行舍弃,先进行线段的连线。此时的LP只遵循距离准则,即距离远的点舍弃。
在所有连好的线段上进行路径的规划。如下图所示。
此时看到,线段穿过了障碍物的点,但是我们的最优化路径是不能包含障碍物的,所以这条最优路径不采用,把经过这个点的两条线进行删除。就得到下图的轨迹。
删除后继续寻找别的最优轨迹。
这种算法也称为Lazy collision-checking
去除不可行的线段之后,就再继续寻找其他可行的路径,就会进行重新构建。
快速搜索随机树RRT
伪代码
-
采样,可以根据实际情况进行采样,与PRM一样,先采样一个蓝色的小点,之后根据大红点的距离,在大红点前面delta距离处(此时的delta距离应该是与蓝点最近的一个点)得到小红点,绿色的为终点的位置,大红点与小红点之间没有障碍物,进行连线,这就完成了树的拓展。
-
继续进行随机采样,按照1中的标准继续重复。
3.回顾之前所说的collision-free,第三步找到了xnew与xrand,xnew是距离xrand最近的点,此时中间无间隔,就可以将xnew加入到树当中。
4.假如此时碰到点在障碍物当中,那这个点就不能加入到树当中。
最终肯定会扩散到终点的周围,我们把距离终点周围的一小圈点也认为是终点,无非是添加了一些偏移量