第3章 第1节-基于采样的路径规划-PRM&RRT及其优化

基于采样的路径规划(Sampling Based-Planners)

与基于search方法所不同的是,基于采样的路径规划不需要遍历空间所有点,而是通过在空间中随机撒点,通过线段连点构成路图/树(没有障碍的),来抽象的表达这个空间。 这个更高效,更适用于复杂环境、三维/高维空间

1. complete Planner完备规划器:在一定时间内,总可以找到一条路径从出发点到终点,A*就是

2. Probabilistic Complete Planner概率规划器:如果一个解存在(路径规划=找函数的解,如高斯法),使用随机采样的方法,改规划器最终能找到这个解

3. Resolution Complete Planner:和上面一样,但是基于一个确定的采样(如在一个fixed grid中采样

课程安排

1.概率路图

2.快速搜索随机树RRT

3.优化算法

4.Advanced 算法

5.实际应用

Probabilistic Road Map 概率路图(PRM)

第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第1张图片

PRM:是一种图结构

他把规划分为两个阶段:

1.学习Learning phase:学习地图结构:在环境中撒点,连点,从而做出一个图的结构,这是图的简化版,找路径直接在这个上找就会简单很多

1)采样

(1)随机采点 (2)障碍物中的点扔掉

第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第2张图片
|
第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第3张图片

|

2.连接相邻的点,得到无碰撞的有效分割

要求:

(1)起点、终点要被连接到里面,点与点之间的连接要符合距离要求:太远的点不能连(如图不能连1-3,1-4等,避免路径太多,查询效率低)所以红色的线要删掉 (2)有障碍物的点不要,所以删除绿色的线

第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第4张图片
|

第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第5张图片
|

2.查询 Query phase

直接用Dijkstra或者A*找到一条路径

直接A*等就很复杂,要很多网格计算

第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第6张图片

eg

1学习过程 2查询过程
第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第7张图片 第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第8张图片
第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第9张图片 第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第10张图片

优点Pros:概率完备的:若存在解,那么在空间中随机踩点,肯定可以找到解;相对于Dijkstra和A*不需要在整个环境中搜索,只要给出图(简化的环境)就可以找出,效率更高

问题Cons:要解决两点之间的约束,因为两个直线之间是不可导的,不适合机器人行走,要构造一条曲线满足机器人行走; 算法过于冗长,一开始就单纯的简化环境,没有考虑起始点与终点,相对不够有效率

3.PRM改进-Lazy collision-checking

效率影响因素:检查点是否在障碍物内很耗时

Lazy collision-checking:直接不考虑点在不在障碍物内,直接不去点。这样生成的路径可能是经过障碍物的,那么再把这个不可行的点及其相关的路径删了,再重新构图,…,以此类推,直到找到可行路径
第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第11张图片
第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第12张图片|第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第13张图片|

快速搜索随机树(Rapidly-exploring Random Tree)

相对于PRM更有针对性,能快速规划,在复杂/高维环境下搜索效率更高

在起点x init附近采样一些点构成树,相对于PRM是一种增量式的路径(首先采点,找树里面最近的点做一些连接,并且要障碍物检测,然后把新的点添加到树中,这个过程不断迭代,从而把树构建出来。直到把目标点也添加到树中,则可以找到从起始点到终点的路径

相对于之前的PRM,这个Tree方法不需要区分learning phase和query phase,他的目标就是构建一条路径,只要目标点被加入则反方向查询直接找到路径,而且不需要A*等算法。

伪代码:

第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第14张图片

输入:地图、初始点、终点

输出:起点到终点的路径

for i=1 to n do

第一次循环:(比较简单)

得到蓝色采样点,得到蓝色-红色路径,让红色往这条线上走 D e l t a Delta Delta距离,得到小的红色点,若这条段线段上没有障碍物,则原红色点+现在的线段=更新后的树
第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第15张图片

第二次循环

(1) x_rand<-Sample

Sample得到随机点x_rand

第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第16张图片

(2)x_near<-Near(x_rand,\Tau)

再刚刚生成的树上找一点距离x_rand最近的点x_near

(3)x_new<-Steer(x_rand,x_near,StepSize)

(4)E_i<-Edge(x_new,x_near)

(5) if CollisionFree(M,E_i) then \Tau.addNode(x_new) \Tau.addEdge(E_i)

Steer就是x_near到x_rand的连线上移动一定的距离StepSize,得到新的点x_new,和边Ei,如果这个edge Ei和Point x_new是collision-free的,那么这个点和边就加到树里面去,完成一次更新

第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第17张图片

后面的循环:若出现collision,则因为(5)中的判断条件,则边和点不会加到树中

第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第18张图片

最后

(6) if x_new=x_goal then Success(其实这力不应该直接说最后把绿色的点加入树就成功了,而是把绿色周围的点加到树中就成功了,因为绿色的点不一定会被随机采样到。如果绿色周围的点被选中,则也说明路径规划完成,只是有一定的偏移量

|第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第19张图片|第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第20张图片|

所以从终点倒着找父节点,最终找到起始节点,则路径就出来了

缺点:区域特殊,如下。落在狭窄区域的概率比较小,导致可能一直在一侧扩散

|第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第21张图片|第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第22张图片|

PPT的优势与缺点

优势Pros:相对于PRM,更有针对性的去找起点到终点的路径,速度较快

缺点Cons:不是最优路径(不是最短,也不是完全符合运动学约束)

改进:toward improving efficiency

1.Kd-tree

当重新选取一个rand点时,需要找打离其最近的点–笨方法是一一对比;Kd-tree工具:对所以点取横坐标的中位数,4,然后根据4划分成左右两支,再左右两侧分别纵坐标,…这个方法可以快速找到离你最近的点

第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第23张图片

第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第24张图片

2.Bidirectional RRT/RRT Connect

通过一次采样,在起点、终点处分别构建树,直到x_rand同时连接两棵树算法截止

有利于解决狭窄区域的概率比较小,导致可能一直在一侧扩散的问题,因为在终点也生成树了!

第3章 第1节-基于采样的路径规划-PRM&RRT及其优化_第25张图片

Bidirectional RRT 相对于单纯的RRT效率是高的,对于特例:终点在narrow path的问题也很有帮助,但对于narrow path问题他不是解决的唯一方式,更多的人倾向于通过改进采样的方式来解决narrow path问题:通过一些采样函数使得在狭窄区域落的点采样多一点(自行百度了解)

你可能感兴趣的:(深蓝学院,学习轨迹规划,算法,机器学习,人工智能)