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

发布时间：2024年01月20日

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 V V表示随机点集， E E E表示路径集
①随机采样一个无碰撞的点，将这个无碰撞的点加入 V V V中，重复 n n n次
②生成概率路图

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

2 RRT

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

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

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 r a n d q_{rand} qrand?，然后两棵搜索树同时向采样点?方向进行扩展，加快两棵树建立连接的速度。相较于单树扩展的RRT算法，RRT-Connect加入了启发式步骤，加快了搜索速度，对于狭窄通道也具有较好的效果。
在这里插入图片描述

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

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

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

3 基于优化的采样方法

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

  • S a m p l e F r e e SampleFree SampleFree：在可行的区域内随机采样
  • N e a r e s t ( G , x r a n d ) Nearest(G,x_{rand}) Nearest(G,xrand?)：在已生成的树中利用欧氏距离判断距离 x r a n d x_{rand} xrand?最近的点 x n e a r e s t x_{nearest} xnearest?
  • S t e e r ( x n e a r e s t , x r a n d ) Steer(x_{nearest},x_{rand}) Steer(xnearest?,xrand?): 从 x n e a r e s t x_{nearest} xnearest?与 x r a n d x_{rand} xrand?的连线方向上扩展固定步长得到新节点 x n e w x_{new} xnew?
  • L i n e ( x n e a r e s t , x n e w ) Line(x_nearest, x_new) Line(xn?earest,xn?ew)：两个点之间的直线距离（欧氏距离）
  • CollisionFree和ObstacleFree：都是判断两个节点之间是否有障碍物
    在这里插入图片描述
    在这里插入图片描述

Kinodynamic-RRT*
在这里插入图片描述
更改Steer()函数以适应机器人导航中的运动或其他约束

Anytime-RRT*
当机器人执行当前轨迹时，继续优化RRT树，讲究一个实时！！
在这里插入图片描述

4 基于采样的先进方法

informed RRT*（启发式）
在这里插入图片描述
在这里插入图片描述
Cross-entropy motion planning
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

5 实现

OMPL
Moveit with ROS
Tutorials
在这里插入图片描述
在这里插入图片描述

加油😀

文章来源:https://blog.csdn.net/BigDavid123/article/details/135716300
本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。
最新文章