Motion Plan之基于采样的路径规划算法笔记

最新推荐文章于 2024-11-26 19:52:30 发布

原创

最新推荐文章于 2024-11-26 19:52:30 发布 · 1.5k 阅读

10 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #笔记 #人工智能 #科技 #agi

本文介绍基于采样的路径规划算法，阐述其优缺点、与图搜索路径规划的差异，围绕采样撒点、建图时机和范围衍生出多种算法。还介绍PRM、RRT等算法的原理、流程及差异，共同处理流程包括采样、构图、碰撞检测和路径搜索，具体算法依需求和环境选择。

Motion Plan之搜索算法笔记

背景：

基于采样算法是一种在路径规划中广泛应用的有效方法。它通过在图中随机选择点来生成一个简化的搜索图，从而加速搜索过程。这种方法的主要优点包括减少内存使用，避免计算错误，具有动态障碍物对抗性，以及易于应用于高维C空间。
基于采样路径规划优缺点如下：
优点：
a.即时生成搜索图不需要存全部图信息、减少内存
b.不试图明确构造C空间及其边界
c.即时生图有一定动态障碍物对抗性
d.易于应用于高维C空间
e.如果进行了足够的预处理，支持快速查询
不足：
a.需要解决2点边界值问题。
b.对某些任务效果不佳。
b.1.不太可能在狭窄通道中放置样本节点。
b.2.在约束表面上采样/连接节点困难。
c.既不是最优的也不是完整的。

1.有了图搜索路径规划为什么还需要随机采样路径规划"

Sample base plan"和"搜索路径规划"是两种不同的策略，它们在处理问题和优化解决方案时有所不同。
** a.Sample base plan**：这是一种基于样本的规划方法，它通过生成大量的样本点来寻找最优解。这种方法的优点是可以处理高维度和复杂的问题，因为它不需要对整个问题空间进行详细的搜索。然而，这种方法的缺点是可能会产生次优的解决方案，因为它并不总是能找到全局最优解⁴。此外，如果样本点的生成和选择方法不合适，可能会导致效率低下⁵。
** b.搜索路径规划**：这是一种基于搜索的规划方法，它通过在问题空间中搜索最优路径来找到解决方案。这种方法的优点是在给定足够的计算资源和时间的情况下，可以找到全局最优解。然而，这种方法的缺点是在处理高维度和复杂的问题时，可能会遇到"维度灾难"的问题，因为问题空间的大小随着维度的增加而指数级增长，这使得搜索变得非常困难和耗时。
这两种方法各有优缺点，适用于不同的问题和场景。在实际应用中，可能需要根据具体的问题特性和需求，选择最合适的规划方法。对于一些特别复杂或者高维度的问题，可能需要结合使用这两种方法，或者使用其他的优化策略和技术。

2.基于采样路径规划算法主要围绕以下几点，衍生出各种随机采样路径规划算法

a.如何采样撒点(采样撒点方式）：均匀采样、高斯采样、随机采样
采样方法是选择节点的具体方式，常见的采样方法包括均匀采样、随机采样、高斯采样等。均匀采样是从图中均匀分布的点中随机选择点，随机采样是从图中随机选择点。
b.何时建图（何时做采样点是否可行点判断）
建图时机是指确定在何时生成搜索图。通常情况下，当需要搜索解决方案时，可以生成搜索图。但是，在问题规模较大时，生成搜索图可能会消耗大量时间和内存。因此，可以根据问题的具体情况来确定建图的时机。
c.建图范围（需要遍历的采样点）
建图范围是指需要遍历的采样点。通常情况下，建图范围包括图中所有节点。但是，在问题规模较大时，遍历所有节点可能会消耗大量时间和内存。因此，可以根据问题的具体情况来确定建图范围。
可行的路径规划方法：概率路线图 (PRM)，快速探索随机树 (RRT)；最优路径规划方法：RRT*；加速收敛：RRT#，Informed-RRT*，和 GuILD。

3.基于采样的路径规划算法的共同处理流程：

尽管这些算法在实现细节上有所不同，但它们都遵循了基于采样的路径规划算法的基本框架，即采样、构建图或树、碰撞检测和路径搜索。这些共同的处理流程使得这些算法能够在各种复杂的环境中进行有效的路径规划。具体选择哪种算法，需要根据实际应用的需求和环境条件来决定。
基于采样的路径规划算法的共同处理流程主要包括以下几个步骤
** a.采样**：在配置空间中随机撒点，生成一组采样点。
** b.构建图或树**：根据采样点之间的关系（例如距离、连通性等），构建一个图或树来表示整个配置空间。
** c.碰撞检测**：检查采样点和采样点之间的连线是否与障碍物发生碰撞，如果有碰撞，则舍弃这些采样点或连线。
** d.路径搜索**：在构建的图或树上搜索一条从起点到终点的路径。
这个流程是基于采样的路径规划算法的基本框架，不同的算法可能会在这个基础上添加一些额外的步骤，例如概率路图算法（PRM）的学习阶段和查询阶段，快速随机扩展树算法（RRT）的树扩展过程，RRT*算法的路径优化过程等。这些算法的具体实现和优化方法可能会有所不同，但它们的核心思想都是通过采样和搜索来寻找一条满足特定条件的路径。

4.prm、rrt、rrt、rrt# 、Informed-RRT 、GuILD差异

基于采样的路径规划算法，如概率路线图（PRM）、快速搜索随机树（RRT）、RRT_、RRT#、Informed-RRT_和GuILD，虽然在实现细节上有所不同，但它们都遵循了一些共同的处理流程，包括采样、构建图或树、碰撞检测和路径搜索。这些共同的处理流程构成了这类算法的基本框架。
然而，这些算法在具体实现这些处理流程时，会有一些重要的差异。主要差异:

PRM属于多查询算法,可用于离线全局规划;RRT属于单查询算法,适用于在线规划。
PRM采用批量全局采样;RRT采用增量式采样。PRM覆盖整个空间,RRT增量扩展。
PRM构建无向图(可能有环),RRT构建有向树。
PRM全局在图上搜索路径,RRT从根节点增量搜索。
RRT*、Informed-RRT*、GuILD在RRT基础上引入了贪心策略、启发式、预构建路线图等机制进行加速和优化。

更详细的介绍他们之间差异:

采样范围:全局vs增量
- PRM:预处理阶段进行全局批量采样
- RRT/RRT_/_:增量迭代采样
- GuILD:在增量迭代采样的基础上结合全局Roadmap
数据表示:图vs树
- PRM:构建路线图(图结构)
- RRT/RRT#:构建树结构
- RRG/GuILD:既有树结构,也结合路线图
搜索策略:全局vs增量
- PRM:全局搜索路径
- RRT/RRT*/RRT#:增量构建搜索树
- GuILD:在RRT的搜索树上结合全局路线图搜索
是否利用额外信息进行加速和优化
- RRT*/RRT#:在RRT上增加贪心策略
- Informed-RRT*:集成启发式采样
- GuILD:集成局部子集采样
时间性能

PRM:两阶段分离,预处理时间长
RRT系列算法:适合在线规划

下面是这些算法的原理和流程的详细描述：
PRM(Probabilistic Roadmap Method):

原理:通过在自由空间中随机采样节点,然后使用本地规划方法连接这些节点来构建路线图(roadmap)。
流程:分学习阶段和查询阶段。学习阶段中,随机采样自由空间中的配置,并使用简单的运动规划器测试这些配置之间的连接;通过重复这个过程,逐步构建路线图。查询阶段中,将起点和终点连接到路线图,然后在路线图上搜索路径。
- 采样：在整个配置空间中进行均匀采样，生成一些节点。
- 连接：对于每个节点，找到距离它最近的几个节点，并检查它们之间是否可以直接连接。如果可以，就在这两个节点之间添加一条边。
- 搜索：在这个图中，使用图搜索算法（如Dijkstra或A*）来找到从起始点到目标点的最短路径。

缺点是环境变化后要重新构建地图,计算代价大，难以处理窄通道。

RRT(Rapidly Exploring Random Tree):

原理:一种增量搜索算法,通过扩展一棵树来探索空间。从起点开始,选择最靠近树的随机配置并尝试扩展树向该配置移动来增长树。
流程:重复采样、找到nearest节点、尝试连接新样本和nearest节点、如果连接成功则添加新节点和边到树中,直到树达到目标区域或指定迭代次数。
- 采样：在每次迭代中，随机采样一个点。
- 连接：找到距离这个点最近的树中的节点，然后向这个点的方向扩展一条边。
- 搜索：重复上述过程，直到找到一条从起始点到目标点的路径。

缺点是扩展方向随机,扩展不均匀,易进入死胡同。

RRT*(Rapidly Exploring Random Tree*):

原理:RRT的优化版本。与RRT类似,但在扩展树的同时,还会改进树上已经存在的路径,通过重新连接节点来优化路径质量。
流程:与RRT基本相同,增加了考虑近邻节点代价的ChooseParent过程和Rewire过程。
- 采样：与RRT算法相同，每次迭代中随机采样一个点。
- 连接：不仅连接到距离采样点最近的节点，还会尝试连接到周围的其他节点，以寻找可能的更优路径。
- 搜索：与RRT算法相同，通过不断迭代来搜索路径。

缺点是重新连接代价较大，收敛到最优解较慢。

RRT# (Rapidly-exploring Random Tree Sharp)：

原理:这是RRT*的一个变种，它在每次迭代中都会尝试优化整个树，以更快地找到更优的路径⁴。

Informed-RRT:*

原理:利用先验信息来加速RRT*的搜索过程。会向更有可能包含最优解的区域扩展树。在找到初始解决方案后，它会在一个椭圆区域内进行采样，该区域包含了所有可能改进当前解决方案的状态。
流程:与RRT*基本相同,改进了采样策略,偏向先验设置的有利区域采样。

GuILD:

原理:利用prebuilt roadmap来加速RRT的搜索过程。首先构建全局roadmap,然后使用RRT在roadmap上执行局部搜索以查找解决方案路径。它依赖于增量密集化来发现逐渐缩短的路径。在计算出从起始点到目标点的可行路径后，GuILD利用这些信息从Informed Set的局部子集进行采样。
流程:分为Global Planning和Local Planning两个阶段。Global Planning构建roadmap覆盖整个空间;Local Planning基于roadmap使用RRT搜索路径。

正文：

概率路线图（PRM）规划器

概率路线图规划器是一种运动规划算法，确定从机器人的起始配置到目标配置的路径，同时避免碰撞。PRM背后的基本思想是从机器人的配置空间中随机取样，测试它们是否在自由空间中，并使用本地规划器尝试将这些配置连接到其他附近的配置。
概率路线图规划器包括两个阶段：构造阶段和查询阶段。

在构造阶段，构建一个路线图（图）。创建一个随机配置。它连接到邻居，例如，最近的k个。
在查询阶段，将起始配置和目标配置连接到图上，然后通过Dijkstra的最短路径查询获得路径。

概念和符号定义：

路线图是一个图G(V,E)
Q-Free中的机器人配置q是一个顶点
边(q1, q2)意味着这些机器人配置之间的无碰撞路径 - 这里需要本地规划器
需要一个度量来衡量配置之间的距离：dist(q1,q2)（例如，欧几里得距离）
使用节点的粗采样和边的细采样
无碰撞的顶点，边在Q-Free中形成一个路线图

PRM路线图构建

最初的空路线图图G
随机选择一个机器人配置q
如果q→Q-Free（无碰撞配置）则添加到G
重复直到选择N个顶点
对于每个顶点q，选择k个最近的邻居
本地规划器尝试将q连接到邻居q’
如果连接成功（即，无碰撞的本地路径），添加边(q, q’)

带有障碍物的二维平面环境

在C-空间中随机采样N个无碰撞的配置

将Q-Free中的每个顶点与K个最近的邻居连接

#roadmap construction algorithm
input:
  n: number of nodes in the roadmap
  k: number of closest neighbors to examine for each configuration
output:
  A roadmap g = (v, e)
--------------------------------------------------------------------------
v <-- 0
e <-- 0
while |v| < n do
    repeat
        q<--random configuration in Q
    until q is collision-free
    v <--v U v{
   
   g}
end while
for all q in V do
    Nq <--the k closest neighbors of q chosen from V according to dist
    for all q' in Nq do
        if (q,q') not in E and delta(q,q') npt null then
            E <--E U{
   
   (q,q')}
        end of
    end for 
end for

PRM搜索路径

给定q_init和q_goal，需要将每个连接到路线图
在路线图中找到q_init和q_goal的k个最近邻居，规划本地路径
问题：路线图图可能有断开的组件…
需要找到从q_init，q_goal到同一组件的连接
一旦在路线图上，使用Dijkstra算法

将起点和终点连接到路线图中的最近节点

图搜索最短路径

处理多个查询-一旦在路线图上，就找到一条路径

#solve query algorithm
input:
    qinit:the initial configuration
    qgoal:the goal configuration
    k:the number of closest neighbors to examne for each configuration
    g=(v,e):the road map computed by roadmap construction algorithm
output:
    a path from qinit to qgoal or failure
------------------------------------------------------<