移动机器人规划 - 基于采样的路径搜索-编程知识

0 预备知识

基于采样的规划器：
（1）不要试图显示地构造C空间及其边界
（2）只需要简单的机器人配置是否发生碰撞
（3）利用简单的碰撞测试，充分了解空间
（4）碰撞检测是一个单独的模块-可根据应用进行定制
（5）随着碰撞检测的改进，这些算法也在改进
（6）单点查询和多点查询的请求方法不同
规划的完备性概念
（1）完备规划器：在有限的时间内正确查找一条规划的路径
（2）概率完备性：如果解存在，规划器最终会找到它，使用随机采样
（3）解决方案完备性：与上述一样，但基于确定性采样（例如，在固定网格上采样）

内容概要：

（1）PRM
（2）RRT
（3）基于优化的采样方法
（4）先进的路径规划方法
（5）实现

1 PRM

论文链接：https://ieeexplore.ieee.org/abstract/document/508439/

在这里插入图片描述
PRM:
（1）图结构
（2）分解为两个阶段：①学习 ②查询
①：在C-space上采样N个点；删除那些在障碍物里的点；连接到最近的点，并获得无碰撞段；删除与障碍物冲突的线段。

②：在路线图上搜索从起点到目标的路径（使用Dijkstra算法或A* 算法）；路线图现在类似于栅格地图（或简化栅格地图）
在这里插入图片描述

（3）可以有效地检查采样配置和样本之间的连接是否存在碰撞
（4）相对少的步骤和局部路径就足以捕获自由空间的连通性

PRM的优缺点
在这里插入图片描述

优点：
简单，只需要调整参数即可实现不同场景下的路径规划，且不需要对环境中的障碍物进行精确建模，在高维空间和动态环境中的路径规划有很大优势。
缺点：
①需要处理两点边界值的问题
②在状态空间上构建图，但不特别关注生成路径（不关注启发式的添加）
③该算法存在狭窄通路问题，当规划的路径需要通过密集的障碍物或者需要经过狭窄的通道时，PRM方法的效率变的低下

为了提高效率
Lazy collision-checking
①碰撞检测过程非常耗时，特别是在复杂或高维的环境中。
②采样点并生成线段，而不考虑碰撞(Lazy)

③必要时进行碰撞检查：在未进行碰撞检查的情况下生成的道路图上查找路径
如果路径不是无碰撞的，则删除相应的边和节点

④重新启动路径查找

PRM算法伪代码：
在这里插入图片描述
$V$ 表示随机点集， $E$ 表示路径集
①随机采样一个无碰撞的点，将这个无碰撞的点加入 $V$ 中，重复 $n$ 次
②生成概率路图

对 $V$ 中的每个点 $q$ ，根据一定的距离范围选择 $k$ 个邻域点
对每个邻域点 $q^{'}$ 进行判断，如果 $q$ 和 $q^{'}$ 尚未形成路径，则将其连接形成路径，随后进行碰撞检测，若无碰撞，则保留该路径。
按上述步骤建完图，使用图搜索算法搜索出一条最短路径。

2 RRT

①算法通常将起点作为根节点 $x_{init}$ ，加入到随机树的节点集合中
②从可行区域内随机选取一个节点 $x_{rand}$ ，并在已生成的树中利用欧氏距离判断距离 $x_{rand}$ 最近的点 $x_{near}$
③从 $x_{near}$ 与 $x_{rand}$ 的连线方向上扩展固定步长 $u$ ，得到新节点 $x_{new}$ （如果 $x_{near}$ 与 $x_{rand}$ 间的距离小于步长，则直接将 $x_{rand}$ 作为新节点 $x_{new}$ ）。
④若 $x_{new}$ 与 $x_{near}$ 之间无障碍物，将 $x_{new}$ 加入到随机树的节点集合中，同时将 $x_{near}$ 作为 $x_{new}$ 的父节点，将边 $x_{near},x_{new})$ 加入到随机树的边集中
⑤若这两个节点间有障碍物，则重新选择 $x_{near}$ 并进行扩展。
循环执行以上步骤，直到随机树的叶节点包含了目标点，并从中找出一条各节点连接成的从起点至终点的无碰撞路径。
在这里插入图片描述

采样过程是完全随机的，但是我们可以在采样时以一定的概率直接采样终点作为 $x_{rand}$ ，加快搜索速度。

RRT概述：
单查询算法，目标是尽可能快的找到一条从起点到终点的可行路径；模拟树木生长时树根不断向四周扩散的过程
在这里插入图片描述

RRT的优缺点：
优点：
（1）旨在找到从开始到目标的路径，若能够获得全局环境并进行建模，可进行全局路径规划。若无法获得全局环境，如自动驾驶汽车路径规划问题，能够在动态规划中对局部地图进行规划以生成局部路径
（2）比PRM更有针对性
缺点：
（1）非最优解决方案；当路径中包含障碍物之间形成的狭窄通道时，使用RRT算法规划路径有一定几率无法规划出最优路径
（2）效率不高，留有改进空间
（3）在整个空间采样

为了提高效率：
（1）引入Kd-tree

参考博客：https://blog.csdn.net/junshen1314/article/details/51121582

在这里插入图片描述

（2）双向RRT / RRT连接
①RRT-Connect算法在RRT的基础上引入了双树扩展环节，分别以起点和目标点为根节点同时扩展随机树从而②实现对状态空间的快速搜索。
当两棵树建立连接时可认为路径规划成功。
③通过一次采样得到一个采样点 $q_{rand}$ ，然后两棵搜索树同时向采样点方向进行扩展，加快两棵树建立连接的速度。相较于单树扩展的RRT算法，RRT-Connect加入了启发式步骤，加快了搜索速度，对于狭窄通道也具有较好的效果。
在这里插入图片描述

特点：
（1）Connect算法较之前的算法在扩展的步长上更长，使得树的生长更快
（2）两棵树不断朝向对方交替扩展，而不是采用随机扩展的方式，特别当起始位姿和目标位姿处于约束区域时，两棵树可以通过朝向对方快速扩展而逃离各自的约束区域
（3）带有启发性的扩展使得树的扩展更加贪婪和明确，使得双树RRT算法较之单树RRT算法更加有效

缺点：都是单查询算法，最终路径并不是最优的

总结RRT：
（1）逐步构建
（2）快速搜索
（3）关键功能：Sampling, Nearest, Collision-checking

3 基于优化的采样方法

RRT*是渐进最优算法
在这里插入图片描述

$S am pl e F ree$ ：在可行的区域内随机采样
$Nearest(G,x_{rand})$ ：在已生成的树中利用欧氏距离判断距离 $x_{rand}$ 最近的点 $x_{nearest}$
$Steer(x_{nearest},x_{rand})$ : 从 $x_{nearest}$ 与 $x_{rand}$ 的连线方向上扩展固定步长得到新节点 $x_{new}$
$Line(x_nearest, x_new)$ ：两个点之间的直线距离（欧氏距离）
CollisionFree和ObstacleFree：都是判断两个节点之间是否有障碍物