做网站步骤,学平面设计好找工作吗,在哪里能找到做网站的人,国家企业年报申报入口官网Motion Plan之搜索算法笔记
背景#xff1a;
基于采样算法是一种在路径规划中广泛应用的有效方法。它通过在图中随机选择点来生成一个简化的搜索图#xff0c;从而加速搜索过程。这种方法的主要优点包括减少内存使用#xff0c;避免计算错误#xff0c;具有动态障碍物对抗…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 roadmapk: number of closest neighbors to examine for each configuration
output:A roadmap g (v, e)
--------------------------------------------------------------------------
v -- 0
e -- 0
while |v| n dorepeatq--random configuration in Quntil q is collision-freev --v U v{g}
end while
for all q in V doNq --the k closest neighbors of q chosen from V according to distfor all q in Nq doif (q,q) not in E and delta(q,q) npt null thenE --E U{(q,q)}end ofend for
end for PRM搜索路径
给定q_init和q_goal需要将每个连接到路线图在路线图中找到q_init和q_goal的k个最近邻居规划本地路径问题路线图图可能有断开的组件…需要找到从q_initq_goal到同一组件的连接一旦在路线图上使用Dijkstra算法
将起点和终点连接到路线图中的最近节点图搜索最短路径 处理多个查询-一旦在路线图上就找到一条路径
#solve query algorithm
input:qinit:the initial configurationqgoal:the goal configurationk:the number of closest neighbors to examne for each configurationg(v,e):the road map computed by roadmap construction algorithm
output:a path from qinit to qgoal or failure
--------------------------------------------------------------------------
Nqinit--the k closest neighbors of qinit from V according to dist
Ngoal--the k closest neghbors of qgoal from V according to dist
V--{qinit} U{qgoal} U V
set q to be the closest neighbor of qinit in Nqinit
repeatif delta(qinit,q) not null henE--(qinit,q) U Eelseset q to be the next closest neighbor of qinit in Nqinitend if
until a connection was succesful or the set Nqinit is empty
set q to be the closest neighbor of qgoal in Nqgoal
repeatif delta(qgoal,q) not null henE--(qgoal,q) U Eelseset q to be the next closest neighbor of qgoal in Ngoalend if
until a connection was succesful or the set Ngoal is empty
P--shortest path(qinit,qgoal,G)
if P is not empty thenreturn p
elsereturn failure
end if
懒惰的碰撞检查
在不考虑碰撞的情况下采样点并生成线段懒惰。在未进行碰撞检查的路线图上找到一条路径。如果路径不是无碰撞的则删除相应的边和节点。重新开始寻找路径。
RRT快速探索随机树
单查询规划器用于从配置A到配置B随机采样Q-Free以从q_start到q_goal的路径向目标生长一棵树可以使用2棵树分别以q_start和q_goal为根。随着树的生长它们最终会共享一个公共节点并合并成一条路径
RRT构建树算法
通过执行随机控制在树中生成“下一个状态”从起点到终点构建一棵树
起始节点是树的根生成新的随机配置q_rand找到最近的树节点q沿着路径(q, q_rand)移动距离step_size如果无碰撞将q_new添加为新的树节点重复以上步骤… 从起始节点开始一次扩展一节点的树每次随机生成新的样本配置尝试将样本连接到树中的最近节点如果无碰撞向样本方向创建新节点的小距离步长 - 这里需要调用本地规划器 一旦树达到目标我们就有了一条路径路径在任何意义上都不是最优的每次路径可能都会不同 - 随机的可以扩展到更高的维度
#build rrt algorithm
input:q0:the confguration where the tree is rootedn:the number of attempts to expand the tree
output:A tree T(V,E)that is rooted at q0 and hasn confgurations
----------------------------------------------------------------------
V-{q0}
E--0
for i1 to n doqrand--a randomly chosen free confgurationextend RRT(T,qrand)
end for
return T#extend rrt algorith
input:T(V,E):an RRTq:a configuration toward which the treet T is grown
output:a new confguration qnew toward q,or nil in case of failure
----------------------------------------------------------------------
qnear--closest neighbor q of T
qnew--progress qnew by step size along the straght line in Q between qnew
qrand
if qnew is collision free thenV--V U (qnew)E--E U {(qnear,qnew){return qnew
end if
return nil优点 l 易于实现 l 旨在找到从起点到终点的路径 l 比PRM更具目标导向性缺点 l 不是最优解 l 不高效留有改进空间 l 在整个空间中取样
最优RRTRRT∗
RRT快速探索随机树是一种增量式的、基于采样的运动规划算法它构建了一棵树这棵树以起始位置为根不断向未探索的区域扩展。然而RRT生成的路径并不一定是最优的。为了解决这个问题研究人员提出了RRT算法。RRT 在RRT基础上做了改进主要是进行了重新选择父节点和重布线的操作。试想在RRT中我们的采样点最终与整棵树上和它最近的点连了起来但这未必是最好的选择我们的最终目的是让这个点与起点的距离尽可能近。RRT* 便对此做了改进它在采样点加入路径树以后以其为圆心画了一个小圈考虑是否有更好的父节点连到那些节点上能使起点到该点的距离更短尽管那些节点并不是离采样点最近的点。在介绍RRT之前我们先介绍RRG算法之所以介绍RRG是因为RRG引入了RRT中非常重要的技术点父节点重连技术。所以我们先来看看RRG然后在切入RRT会更容易理解RRT算法。
快速探索随机图RRG
快速探索随机图算法被引入为一种增量而非批处理算法用于构建可能包含循环的连接路线图。RRG算法与RRT类似首先尝试将最近的节点连接到新的样本。如果连接尝试成功新节点将被添加到顶点集。然而RRG有以下不同之处。每次向顶点集V添加新点xnew时都会尝试从V中所有其他在半径r(card (V )) min{γRRG(log(card (V ))/ card (V ))1/d, η}的球内的顶点进行连接其中η是出现在局部转向函数定义中的常数γRRG γ∗ RRG 2 (1 1/d) 1/d (µ(Xfree)/ζd) 1/d。对于每个成功的连接都会向边集E添加一个新的边。因此很明显对于相同的采样序列RRT图一个有向树是RRG图一个无向图可能包含循环的子图。特别是这两个图共享相同的顶点集RRT图的边集是RRG图的边集的子集。还可以考虑另一个版本的算法称为k-nearest RRG其中寻找k个最近邻的连接k k(card (V )) : kRRG log(card (V ))其中kRRG k ∗ RRG e (1 1/d)并且Xnear ← kNearest(G (V, E), xnew, kRRG log(card (V )))在算法5的第7行。注意k ∗ RRG是一个只依赖于d的常数不依赖于问题实例不像γ ∗ RRG。此外kRRG 2e对所有问题实例都是有效的选择。
Algorithm : RRG
V ← {xinit}; E ← ∅;
for i 1, . . . , n doxrand ← SampleFreei;xnearest ← Nearest(G (V, E), xrand);xnew ← Steer(xnearest, xrand) ;if ObtacleFree(xnearest, xnew) thenXnear ← Near(G (V, E), xnew, min{γRRG(log(card (V ))/ card (V ))1/d, η}) ;V ← V ∪ {xnew}; E ← E ∪ {(xnearest, xnew),(xnew, xnearest)} ;foreach xnear ∈ Xnear doif CollisionFree(xnear, xnew) then E ← E ∪ {(xnear, xnew),(xnew, xnear)}
return G (V, E);最优RRTRRT∗
维护树结构而不是图结构不仅在内存需求方面更经济而且在某些应用中可能更有优势例如相对容易地扩展到具有微分约束的运动规划问题或应对建模错误。RRT∗算法通过修改RRG避免形成循环通过移除“冗余”边即不是从树的根即初始状态到顶点的最短路径的部分从而获得。由于RRT和RRT∗图是具有相同根和顶点集的有向树边集是RRG的子集这相当于对RRT树进行“重连”确保顶点通过最小成本路径到达。在讨论算法之前需要引入一些新的函数。给定两点x1, x2 ∈ R^d让Line(x1, x2) : [0, s] → X表示从x1到x2的直线路径。给定一棵树G (V, E)让Parent : V → V是一个函数它将一个顶点v ∈ V映射到唯一的顶点u ∈ V使得(u, v) ∈ E。按照惯例如果v0 ∈ V是G的根顶点那么Parent(v0) v0。最后让Cost : V → R≥0是一个函数它将一个顶点v ∈ V映射到从树的根到v的唯一路径的成本。为了简化在陈述算法时我们将假设一个加法成本函数即Cost(v) Cost(Parent(v)) c(Line(Parent(v), v))尽管在下一节的分析中这并不是必需的。按照惯例如果v0 ∈ V是G的根顶点那么Cost(v0) 0。RRT∗算法如算法伪代码所示以与RRT和RRG相同的方式将点添加到顶点集V。它还考虑了从新顶点xnew到Xnear的连接即其他在距离r(card (V )) min{γRRT∗ (log(card (V ))/ card (V ))1/d, η}内的顶点。然而并非所有可行的连接都会导致新的边被插入到边集E。特别是i从Xnear中可以连接到xnew的顶点创建一条边并且ii如果通过xnew的路径的成本低于通过当前父节点的路径的成本则从xnew到Xnear的顶点创建新的边在这种情况下将删除连接顶点和其当前父节点的边以保持树结构。还可以考虑另一个版本的算法称为k-nearest RRT∗其中寻找k个最近邻的连接k(card (V )) kRRG log(card (V ))并且Xnear ← kNearest(G (V, E), xnew, kRRG log(i))在算法6的第7行。考虑历史成本而不仅仅是局部信息 重连以提高局部最优性
Algorithm : RRT∗
V ← {xinit}; E ← ∅;
for i 1, . . . , n doxrand ← SampleFreei;xnearest ← Nearest(G (V, E), xrand);xnew ← Steer(xnearest, xrand) ;if ObtacleFree(xnearest, xnew) thenXnear ← Near(G (V, E), xnew, min{γRRT∗ (log(card (V ))/ card (V ))1/d, η}) ;V ← V ∪ {xnew};xmin ← xnearest; cmin ← Cost(xnearest) c(Line(xnearest, xnew));foreach xnear ∈ Xnear do // Connect along a minimum-cost pathif CollisionFree(xnear, xnew) ∧ Cost(xnear) c(Line(xnear, xnew)) cmin thenxmin ← xnear; cmin ← Cost(xnear) c(Line(xnear, xnew))E ← E ∪ {(xmin, xnew)};foreach xnear ∈ Xnear do // Rewire the treeCollisionFree(xnew, xnear) ∧ Cost(xnew) c(Line(xnew, xnear)) Cost(xnear)then xparent ← Parent(xnear);E ← (E \ {(xparent, xnear)}) ∪ {(xnew, xnear)}
return G (V, E);从经验上看对于低维规划我们可以将常数范围设置得稍大于转向距离。RRT*常用提效优化方法
偏向采样 向目标偏向的采样样本拒绝 拒绝 g h c* 的样本分支定界树剪枝 剪除非有希望的子树以减少邻居查询成本。图稀疏化 通过分辨率拒绝样本。引入近似最优性。邻居查询 k-最近或范围内最近近似最近邻不同维度的范围树。延迟碰撞检查 按潜在的成本值对邻居进行排序。 按顺序检查碰撞并在找到无碰撞的边时停止。双向搜索有条件的重连 在找到第一个解决方案之前进行重连。数据重用 存储 ChooseParent 和 Rewire 的碰撞检查结果仅对对称成本有效。
RRT#
RRT*的利用存在一些缺陷。过度利用的情况下没有必要对非有前途的顶点进行“重连”。在利用不足的情况下“重连”只在新节点添加到生成树后发生并且只对其“附近”的节点起作用。它并不能保证在任何中间迭代中的临时路径是最优的。一言概之RRT#引入了代价函数、边成本、代价成本、启发函数来记录历史选择路径的代价状态所以在做新节点选择时候可以基于这些代价函数快速选出有价值的节点扩展。同时把RRG图探索和利用两阶段做法解决了RRT算法探索、利用耦合导致的只能求解局部最优问题。既保证了算法渐进最优有保证计算的效率。
摘要
RRT#(RRT “sharp”)它不仅保证了渐进最优性而且还确保了以初始状态为根的生成树包含了有可能成为最优解的顶点的最低成本路径信息。这意味着如果当前图中已经有一些顶点在目标区域内那么最好的可能解决方案就可以立即计算出来。为了克服与搜索高维空间相关的计算复杂性大多数运动规划算法的设计方式相似并执行两个基本任务探索和利用。探索获取搜索空间拓扑的信息即空间的子集如何连接而利用则通过处理探索任务计算的可用信息逐步改进解决方案。因此如果可用信息不足探索可能无法找到解决方案。然而探索最大程度地利用了当前可用的信息。一般来说这两个任务的结合方式产生了在次优性、渐进最优性、收敛速率等方面具有不同特性的规划器。为了设计最优的运动规划算法必须确保探索覆盖整个搜索空间至少是渐进的并且利用在不陷入局部解的情况下找到最好的可能解决方案。概率方法已被证明对于解决具有动态约束的高维搜索空间的运动规划问题非常有效。其中快速探索随机树(RRTs)是最受欢迎的。然而最近的研究表明当算法收敛时RRTs返回的最佳路径几乎总是即以概率1远离最优解。这重新激发了开发具有最优性保证的增量采样算法来解决运动规划问题的兴趣。在中作者提出了快速探索随机图(RRG)算法该算法是渐进最优的即它确保当样本数量趋于无穷大时将找到最优路径。基于RRG算法同样的作者后来提出了一种新的算法即RRT*该算法从RRG算法构造的图中提取一棵树。RRT_算法遵循与RRG算法中的探索过程相同的过程然而此外新的可用信息会立即被利用。这是通过在新采样点的局部区域重新布线树来改进邻居顶点的到达成本值来实现的。因此探索和利用任务在RRT_算法中是通过设计耦合的即当前树的任何顶点的到达成本值都不能在不将新顶点包含到树中的情况下得到改进。由于这两个任务在本质上是不同的因此一种直观的提高性能的方法例如提高收敛速率是将这两者分开并将它们作为不同的过程实现以便尽可能地并行运行。RRT#算法将探索和利用作为两个独立的过程实现。为了进一步提高效率我们在利用步骤中也使用了松弛法。采用了RRG算法的探索算法然后使用了高斯-赛德尔类型的松弛方法进行利用。RRT#算法保证了渐进最优性但是此外它还确保在每一步到那一步为止的可用信息都被充分地利用到了最大程度。为底层图构造了一个以初始点为根的生成树该树包含了从初始点到目标集的最佳可能路径的信息以及一组顶点的到达成本值包括有可能成为最优解的目标集。这有两个好处。首先它确保在每个时间点算法返回给定当前可用采样点的最短路径。其次它允许我们根据顶点可能成为最优路径的潜力对顶点进行分类。因此人们可以快速地确定最有可能找到最优解的区域。这些信息可以随后用来提高算法的收敛速度以及更有效地探索无障碍空间。
II. 问题的形式化
A. 符号和定义 让X表示状态空间我们假设它是Rd的一个开子集其中d ∈ N且d ≥ 2。让障碍区域和目标区域分别由Xobs和Xgoal表示。无障碍空间由Xfree X \Xobs定义。让初始状态由xinit ∈ Xfree表示。给定两点x, x’ ∈ Rd之间的直线段由Line(x, x’) {θ ∈ R, 0 ≤ θ ≤ 1 : θx (1 − θ)x’}表示。让G (V, E)表示一个图其中V和E ⊆ V ×V分别是顶点和边的有限集。在后续我们将使用图来表示从Xfree随机选择的一组有限点之间的连接。后继顶点给定有向图G (V, E)中的一个顶点v ∈ V集值函数succ : (G, v) 7 → V ′ ⊆ V返回可以从顶点v到达的V中的顶点。前驱顶点给定有向图G (V, E)中的一个顶点v ∈ V集值函数pred : (G, v) 7 → V ′ ⊆ V返回是进入v的边的尾部的V中的顶点。父顶点给定有向图G (V, E)和一个顶点v ∈ V函数parent : v 7 → u返回一个唯一的顶点u ∈ V使得(u, v) ∈ E且u ∈ pred(G, v)。生成树给定有向图G (V, E)G的生成树可以定义为T (Vs, Es)其中Vs V和Es {(u, v) : (u, v) ∈ E且parent(v) u}。边成本值给定边e (u, v) ∈ E函数c : e 7 → r返回一个非负实数。然后c(u, v)其中v ∈ succ(G, u)是从u到v移动的成本。到达成本值给定顶点v ∈ V函数g : v 7 → r返回一个非负实数r这是从给定的初始状态xinit ∈ Xfree到v的路径的成本。我们将使用g∗(v)来表示顶点v在Xfree中可以达到的最优到达成本值。启发式值给定顶点v ∈ V和目标区域Xgoal函数h : (v, Xgoal) 7 → r返回一个估计值r这是从v到Xgoal的最优成本如果v ∈ Xgoal我们设h(v) 0。如果它从不高估到达Xgoal的实际成本那么它就是一个可接受的启发式。在本文中我们总是假设h是一个可接受的启发式。众所周知不可接受的启发式可以用来加速搜索但它们可能导致次优路径。我们希望解决以下运动规划问题给定一个有界且连通的开集X ⊂ Rd集合Xfree和Xobs X \Xfree一个初始点xinit ∈ Xfree和一个目标区域Xgoal ⊂ Xfree找到连接xinit到目标区域Xgoal的最小成本路径。如果不存在这样的路径那么报告无法找到解决方案。在下一节中我们将提出一种迭代算法该算法找到连接从Xfree随机采样的一系列点的最优路径。该算法基于与RRT_算法中发现的类似的思想但有一个重要的区别。虽然RRT_算法是基于在添加新顶点后对树进行局部重连但是提出的算法在每次迭代中都包含了一个重规划步骤类似于在LPA_和D_算法中实现的以有效地传播由于包含新顶点而在图的相关部分中发生的变化。因此提出的算法确保在每次迭代中都计算出当前图中的最优路径。相反RRT*算法并不能保证在任何中间迭代的临时路径是最优的。由于任何算法都将在有限次迭代后终止因此重要的是要确保在终止时返回的路径是最优的给定到那一点为止的所有可用数据。此外重要的是要确保在每次迭代中都有效地计算最优路径。这意味着在下一次重新规划步骤中需要考虑来自前一次迭代的任何先前信息。在我们的实现中这是通过跟踪图中最“有前途”的顶点这些是可以成为最优路径的顶点并在新信息变得可用时更新这些顶点的到达成本值来完成的。已经证明这相当于在每次迭代后对变量进行适当的重新排序以编码更新的信息类似于在高斯-赛德尔松弛方法中解决固定点问题时所做的事情。
III. RRT#算法 - 概述
给定一个图G (V, E)一个初始顶点xinit和一个目标顶点集Xgoal以下的Bellman类型方程可以用来计算所有顶点的最优到达成本值 g∗(vi) min vj ∈pred(G,vi) (g∗(vj ) c(vj , vi)), (1) 其中边界条件是g∗(vi) 0如果vi xinit。这个方程可以使用Bellman-Ford算法有效地解决。人们可以通过松弛方程1来引入Bellman-Ford算法的Gauss-Seidel版本以便根据到达成本值。为此让nk是算法的第k次迭代中的顶点即状态的数量让gk, ∈ Rnk是nk维向量其组件是在第k次迭代的算法中更新了th顺序顶点的到达成本值后的顶点的到达成本值也就是说gk, i gk1 i 如果vi vo()和gk, i gk i如果vo() ≺ vi。初始条件设置为g0,0 i 0对于vi xinit和g0,0 i ∞ 对于所有vi ∈ V \ {xinit}。然后Bellman-Ford算法的Gauss-Seidel迭代可以简洁地写成gk,0 fG(gk−1,0)其中 fGo() (gk−1,−1) min vj ∈pred(G,vo()) (gk−1,−1 j c(vj , vo())), (2) 和边界条件fGo() (gk−1,−1) 0如果vo() xinit。在每次迭代中gk−1,0的组件被一次更新为gk−1, o() fGo() (gk−1,−1)其中 gk,0 gk−1,nk 。 在算法的第k次迭代的th步计算的值fGo()是顶点vo()的一步预测到达成本估计称为顶点的局部最小到达成本估计或简称为顶点的lmc值在中也称为rhs值。因此阶段(k, )的顶点vi的lmc值定义为lmck,(vi) fGi (gk,)对于Gauss-Seidel类型的迭代如果顶点vi被称为静止的那么gk,(vi) lmck,(vi)这意味着gk, i fGi (gk,)。否则它被称为非静止的。 RRT#算法在每次迭代中执行两个任务即探索和利用。探索任务实现了RRG算法的扩展过程然后由利用任务跟进该任务实现了Bellman-Ford算法的Gauss-Seidel版本如方程2所示。下面给出了每个步骤的简要描述。探索这一步在Xfree中随机采样一个点然后通过将其作为当前图中的新顶点通过连接缺失的边将基础图向采样点扩展。以下过程是探索步骤的一部分。采样Sample : N → Xfree从Xfree返回独立的同分布的i.i.d样本。最近邻Nearest返回给定有限集V中根据给定的距离函数最接近给定点x的点。附近的顶点Near返回给定有限集V中根据给定的距离函数最接近给定点x的n个点。转向Steer返回以给定状态x为中心的球中的点该点根据给定的距离函数最接近另一个给定点xnew。碰撞检查给定两点x1, x2 ∈ Xfree布尔函数ObstacleFree(x1, x2)检查连接这两点的线段是否属于Xfree。如果线段是Xfree的子集即Line(x1, x2) ⊂ Xfree它返回True否则返回False。图扩展Extend是一个函数它将图G向随机采样点xrand扩展最近的顶点。利用这一步实现了改进当前顶点的到达成本值的任务因为新的信息变得可用。它还将对有前途的顶点见下面的方程3和v∗ goal目标集中具有最低到达成本值的目标顶点的最低成本路径信息编码为以初始顶点为根的生成树。在每次迭代中非静止顶点的到达成本值按照它们的f值即从初始顶点到通过感兴趣的顶点的目标集的最优路径的成本的低估的顺序更新每次迭代中具有较小g值的顶点优先。注意该算法的工作方式是只需要一部分顶点而不是所有顶点的静止性就足以计算从xinit到Xgoal的最优路径。利用步骤中使用的过程的详细信息如下。排序给定一个顶点v ∈ V函数Key : v 7 → k返回一个实数向量k ∈ R2其组件是k1(v) lmc(v) h(v)和k2(v) lmc(v)。键的组件分别对应于A*算法中的f-和g值。键之间的优先关系由字典排序确定。给定两个键k1, k2 ∈ R2布尔函数4 : (k1, k2) 7 → {False, True}当且仅当k11 k21或k11 k21且k12 ≤ k22时返回True否则返回False。有前途的顶点给定一个图G (V, E)其中xinit ∈ V让g∗(v)是在给定的图G上可以达到的顶点v的最优到达成本值让v∗ goal argminv∈V ∩Xgoal g∗(v)。有前途的顶点集Vprom ⊂ V定义为 Vprom {v : [f(v), g∗(v)] ≺ [f(v∗ goal), g∗(v∗ goal)]}, (3) 其中f(v) g∗(v)h(v)。只有有前途的顶点有可能成为从xinit到Xgoal的最优路径的一部分。因此所有有前途的顶点在每次迭代结束时都必须是静止的。相关区域让x∗ goal ∈ Xgoal是目标区域中具有Xgoal中最低最优到达成本值的点即x∗ goal argminx∈Xgoal g∗(x)。Xfree的相关区域是一组点x其中x的最优到达成本值加上从x到Xgoal移动的最优成本的估计小于x∗ goal的最优到达成本值即 Xrel {x ∈ Xfree : g∗(x) h(x) g∗(x∗ goal)}。 位于Xrel中的点有可能成为从xinit到Xgoal的最优路径的一部分。重新规划给定第k次迭代的图Gk (V k, Ek)目标区域Xgoal ⊂ Xfree和一个任意向量gk−1,0 ∈ Rnk它是所有v ∈ V k的到达成本值其中gk−1,0 i 0对于v xinit函数Replan :(Gk, Xgoal, gk−1,0) 7 → (Gk, Xgoal, gk,0)在所有有前途的顶点都变为静止之前迭代地对非静止顶点进行操作。Replan函数用于传播图的拓扑变化的影响因为每次迭代都会添加新的顶点。顶点的优先级顶点的优先级与它们关联的键的优先级相同优先队列用于根据它们各自的键值对图的所有非静止顶点进行排序。以下函数被定义为管理优先队列。更新队列函数UpdateQueue根据顶点v的g-和lmc-值改变队列。如果顶点v是非静止的那么它要么被插入到队列中要么根据它的最新键值更新它在队列中的优先级如果它已经在队列中。否则如果它是一个静止的顶点那么它将从队列中移除。每一步扩展的顶点的顺序是通过在每一步选择队列中键值最小的顶点来确定的。查找最小值函数findmin()返回队列中所有顶点中优先级最高的顶点。这是键值最小的顶点。删除顶点函数remove()从队列中删除顶点v。更新优先级函数update()通过重新分配顶点v的键值为新的给定键值改变队列中顶点v的优先级。插入顶点给定一个顶点v ∈ V和一个键值函数insert()将顶点v和给定的键值一起添加到队列中。
IV. RRT#算法 - 详细信息
RRT#算法的主体在算法1中给出它与其他RRT变体RRTRRGRRT_等相似但有一个显著的例外它使用所有当前顶点在图中的键值来跟踪顶点的静止性。RRT_和RRT#算法的一个重要区别是RRT算法计算的树中的所有顶点都有一个基于它们的有限到达成本值的统一类型而在RRT#算法中顶点根据它们的一步预测到达成本值的估计有四种不同的类型。在RRT#算法中每个顶点v都根据其(g(v), lmc(v))对的值被分类为以下四个类别之一具有有限键值的静态顶点g(v) ∞, lmc(v) ∞且g(v) lmc(v)具有无限键值的静态顶点g(v) ∞, lmc(v) ∞具有有限键值的非静态顶点g(v) ∞ , lmc(v) ∞且g(v) 6 lmc(v)和具有无限g-值和有限lmc-值的非静态顶点g(v) ∞ , lmc(v) ∞。具有无限键值的静态顶点总是非有前途的而对于其余的情况顶点可以是有前途的或非有前途的。算法首先将初始点xinit添加到基础图的顶点集中。然后它通过从Xfree中随机采样一个点xrand并向xrand扩展图逐步增长Xfree中的图。然后Replan过程在算法3中提供将新信息传播到整个图中以改进图中有前途的顶点的到达成本值。所有由采样和扩展步骤产生的计算后面跟着利用算法1的第5-7行形成了算法的单次迭代。这个过程重复给定的固定次数的迭代。最后返回的是以初始顶点为根包含有前途顶点和v∗ goal的最低成本路径信息的生成树。这个生成树算法1中的第7行包含了每个有前途的顶点和v∗ goal的最低成本路径的信息这些路径可以在当前图上实现。这是RRT#算法和其他RRT变体包括RRT算法之间的关键区别之一。此外在RRT#算法中有前途的顶点的g值等于可以通过图的边实现的相应最优到达成本值。这使我们能够在扩展过程中如果新顶点有任何有前途的邻居顶点就用一个较小的估计值初始化新顶点的g值。每当图的任何部分出现新的信息时这个估计都会改进到最好可能的值。因此图中每个有前途的顶点的g值都会很快收敛到其最优到达成本值。RRT#算法使用的Extend过程在算法2中给出。在每次迭代中Extend过程试图将图向随机采样的点xrand ∈ Xfree扩展。首先在第3行找到图中最近的顶点xnearest然后在下一行将xnearest向随机采样的点xrand转向。如果连接转向点xnew和xnearest的线段是可行的那么新点xnew就准备加入到图的顶点集中。然后在xnew的一些邻域即Near过程返回的顶点集中进行局部搜索以找到局部最小的到达成本估计值和相应的父顶点。这在算法2的第8-13行中完成。新顶点xnew和所有导致可行路径的扩展都被添加到图的顶点和边集中这在第14-15行中完成。最后根据其静止性决定是否将新顶点插入到优先队列中这在UpdateQueue过程中完成。新插入的顶点可能是非静止的如果它有一个有限的lmc值。因此需要检查生成树并在必要时执行适当的操作以更新最低成本路径信息。Replan过程在算法3中提供被调用来通过迭代地对图的非静止和有前途的顶点进行操作更新生成树。它只是从优先队列中弹出最有前途的非静止顶点如果有的话并通过将其lmc值赋给其g值使这个非静止顶点变为静止。然后它的新g值在其邻居中传播以便在算法3的第6-10行中改进它们的lmc值。然而这种信息传播也可能导致一些顶点变得非静止因此所有产生的非静止顶点也被插入到优先队列中。这个过程继续直到优先队列中没有非静止的有前途的顶点为止。注意算法3中第2行的终止条件确保了当Replan过程终止时所有有前途的非静止顶点都被扩展。如果Replan(Gk, Xgoal)过程被允许对图的所有非静止顶点进行操作那么这显然是正确的也就是说如果算法3中第2行的终止条件被替换为“队列不为空”的条件。在这种情况下Replan过程将扩展所有的顶点直到它们都变为静止然后过程才会终止。然而算法3中第2行的终止条件实际上确保了更多的内容即所有有前途的顶点只有这些顶点都变为静止所以没有必要扩展非有前途的顶点。这对于效率很重要因为一个非有前途的顶点不能成为最优路径的一部分。因此没有必要扩展非有前途的顶点从而加快了整个算法的速度。
Algorithm 1: Body of the RRT# Algorithm
RRT#(xinit, Xgoal, X )V ← {xinit}; E ← ∅;G ← (V , E);for k 1 to N doxrand ← Sample(k);G ← Extend(G, xrand);Replan(G, Xgoal);(V , E) ← G; E′ ← ∅;foreach x ∈ V doE′ ← E′ ∪ {(parent(x), x)}return T (V , E′)Algorithm 2: Extend Procedure
Extend(G,x)(V , E) ← G; E′ ← ∅;xnearest ← Nearest(G, x);xnew ← Steer(xnearest, x);if ObstacleFree(xnearest, xnew) thenInitialize(xnew, xnearest );Xnear ← Near(G, xnew, |V |);foreach xnear ∈ Xnear doif ObstacleFree(xnear, xnew) thenif lmc(xnew) g(xnear) c(xnear, xnew)thenlmc(xnew) g(xnear) c(xnear, xnew);parent(xnew) xnear;E′ ← E′ ∪ {(xnear, xnew), (xnew, xnear)} ;V ← V ∪ {xnew};E ← E ∪ E′;UpdateQueue(xnew);return G′ ← (V , E)Algorithm 3: Replan Procedure
Replan(G,Xgoal)while q.f indmin() ≺ Key(v∗goal) dox q.f indmin();g(x) lmc(x);q.delete(x);foreach s ∈ succ(G, x) doif lmc(s) g(x) c(x, s) thenlmc(s) g(x) c(x, s);parent(s) x;UpdateQueue(s);Algorithm 4: Auxiliary Procedures
Initialize(x, x′)g(x) ← ∞;if x′ ∅ thenlmc(x) ← ∞;parent(x) ← ∅;elselmc(x) g(x′) c(x′, x);parent(x) x′;
UpdateQueue(x)if g(x) 6 lmc(x) and x ∈ q thenq.update(x, Key(x));else if g(x) 6 lmc(x) and x /∈ q thenq.insert(x, Key(x));else if g(x) lmc(x) and x ∈ q thenq.delete(x);
Key(x)return k (lmc(x) h(x), lmc(x));RRT*和RRT#算法计算的树的演变分别在(a)-(d)和(e)-(h)中显示。树的配置(a)(e)在250次迭代时(b)(f)在500次迭代时©(g)在2500次迭代时。和(d)(h)在10000次迭代时。
Informed RRT*
RRT_算法改进解决方案的必要条件是添加一个来自规划问题的子集Xf ⊆ X的状态。对于寻求在R中最小化路径长度的问题 n这个子集可以通过一个长轴椭球体一种特殊类型的超椭球体来估计Xfb ⊇ Xf这个长轴椭球体以初始状态和目标状态为焦点。通过拒绝采样一个更大的集合来添加这个子集中的新状态的概率当问题的维度增加采样集合的大小增加或者解决方案接近理论最小值时这个概率变得任意小。提出了一种简单的方法来直接采样Xfb这允许创建有信息采样的规划器如Informed RRT_。Informed RRT_使用启发式来缩小规划问题到原始域的子集。这使得它本质上依赖于当前的解决方案成本因为当与规划问题本身相关的长轴椭球体更大时它不能集中搜索。同样它只能将子集缩小到由最优解决方案定义的下界。_Informed RRT_能够在比RRT_少得多的迭代次数内找到接近最优的解决方案。有信息子集的直接采样比全局采样更快地增加了围绕最优解的密度从而增加了改进解决方案和进一步集中搜索的概率。相比之下RRT在整个规划域上具有均匀的密度改进解决方案实际上降低了找到进一步改进的概率。Informed RRT是对RRT_的一个简单修改展示了明显的改进。在模拟中它在简单配置上的表现与现有的RRT_算法一样好并且在配置变得更困难时显示出数量级的改进。由于其专注的搜索该算法对规划问题的维度和域的依赖性较小以及更早找到更好的拓扑不同路径的能力。它还能够在等价计算的情况下找到比RRT_更紧密的最优解并且在没有障碍的情况下可以在有限时间内找到最优解。它也可以与其他算法如路径平滑结合使用以进一步减少搜索空间。Informed RRT算法明确计算Xfb并直接从中采样。与路径偏置不同它对最优解的同伦类没有做任何假设与启发式偏置不同它不探索不能改进解决方案的状态。由于它基于RRT所以它能够在搜索的整个过程中保留在Xfb中找到的所有状态与Anytime RRTs不同。通过直接采样Xfb它总是采样潜在的改进无论Xfb相对于X的大小如何。这使得它能够有效地工作无论规划问题的大小或当前解决方案的相对成本接近理论最小值与样本拒绝和图剪枝方法不同。在问题中启发式没有提供任何额外的信息它的表现与RRT完全相同。直接采样方法允许创建有信息的采样规划器。这样的规划器被称为Informed RRT被提出来展示有信息的增量搜索的优点。Informed RRT_的行为与RRT_相同直到找到第一个解决方案之后它只从由可接受的启发式定义的状态子集中采样以可能改进解决方案。这个子集隐含地平衡了利用与探索并且不需要额外的调整即没有额外的参数或假设即搜索所有相关的同伦类。虽然启发式可能并不总是改进搜索但它们在实际规划中的突出表现证明了它们的实用性。在它们没有提供额外信息的情况下例如当被告知的子集包括整个规划问题时Informed RRT_等同于RRT。RRT_和Informed RRT_在随机世界上找到的等价成本解决方案。在找到初始解决方案后Informed RRT_将搜索集中在状态空间的椭球形有信息子集Xfb ⊆ X上该子集包含了所有可以改进当前解决方案的状态无论同伦类如何。这使得Informed RRT_能够比RRT*更快地找到更好的解决方案而不需要任何额外的用户调整参数。随机世界问题上RRT_和Informed RRT_的解决方案成本与计算时间。两个规划器都运行到找到相同成本的解决方案。图a, c显示了最终结果而图b显示了解决方案成本与计算时间。从图a可以看出RRT_在探索规划问题的区域时花费了大量的计算资源这些区域无法改进当前的解决方案而图c展示了Informed RRT_是如何集中搜索的。
在没有障碍物的情况下Informed RRT*收敛到机器零的最优。起始和目标状态分别以绿色和红色显示它们之间的距离为100个单位。当前的解决方案以洋红色突出显示用于说明的椭球形采样域Xfb以灰色虚线显示。改进解决方案会减小采样域的大小创建一个反馈效应使其收敛到理论最小值的机器零。图a显示了59次迭代后的第一个解决方案b在175次迭代后和c在1142次迭代后的最终解决方案此时椭圆已经退化为一条从开始到目标的线。
算法伪代码 II. 减少RRT树增长的一些方法
A. 以前的工作 以前的工作是通过样本偏置、基于启发式的样本拒绝、基于启发式的图剪枝和/或迭代搜索来专注RRT和RRT*。1样本偏置样本偏置试图通过偏置从X中抽取的样本的分布增加从Xf中抽取状态的频率。这继续添加来自Xf之外的状态这些状态不能改进解决方案。它还导致在被搜索的问题上的非均匀密度违反了一个关键的RRT假设。a) 启发式偏置采样启发式偏置采样试图通过用启发式估计对每个状态进行加权增加采样Xf的概率。它被用来改进常规的RRT由Urmson和Simmons [5]在启发式引导的RRT (hRRT)中通过选择状态的概率与它们的启发式成本成反比。hRRT被证明找到比RRT更好的解决方案然而使用RRTs意味着解决方案几乎肯定是次优的[7]。Kiesel等人[11]使用两阶段过程来创建他们的f-biasing技术中的RRT_启发式。首先解决规划问题的粗略抽象以提供每个离散状态的启发式成本。然后RRT_通过随机选择一个离散状态并在其中使用连续均匀分布进行采样来采样新状态。离散采样被偏置使得属于抽象解决方案的状态具有最高的选择概率。这种技术为RRT算法的整个持续时间提供了启发式偏置然而为了考虑离散抽象它保持选择每个状态的非零概率。因此仍然会采样到不能改进当前解决方案的状态。b) 路径偏置路径偏置采样试图通过在当前解决方案路径周围采样增加采样Xf的频率。这假设当前解决方案要么与最优解同伦要么只被小障碍物分隔。由于这个假设通常不成立路径偏置算法必须继续全局采样以避免局部最优。这两种采样方法的比例通常是用户调整的参数。Alterovitz等人[12]使用路径偏置开发了快速探索路线图(RRM)。一旦找到初始解决方案RRM的每次迭代要么采样一个新状态要么从当前解决方案中选择一个现有状态并对其进行精化。路径精化通过将选定的状态连接到其邻居从而产生一个图而不是一棵树。Akgun和Stilman[13]在他们的双树版本的RRT中使用路径偏置。一旦找到初始解决方案该算法将用户指定的百分比的迭代用于精化当前解决方案。它通过随机选择解决方案路径中的一个状态然后显式地从其Voronoi区域中采样来实现这一点。这增加了改进当前路径的概率但以牺牲探索其他同伦类为代价。他们的算法也在探索状态空间时使用样本拒绝第II-B.2节。Nasir等人[14]在他们的RRT-Smart算法中结合了路径偏置和平滑。当找到解决方案时RRT*-Smart首先将路径平滑并减少到最小的状态数量然后使用这些状态作为进一步采样的偏置。这将路径平滑算法的复杂性添加到规划器中同时仍然需要全局采样以避免局部最优。虽然路径平滑快速降低了当前解决方案的成本但它也可能通过减少绘制样本的偏置点的数量进一步违反了RRT关于均匀密度的假设。Kim等人[15]在他们的Cloud RRT算法中使用可见性分析生成初始偏置。当找到解决方案时这个偏置被更新以进一步集中路径附近的采样。2基于启发式的样本拒绝基于启发式的样本拒绝试图通过在X上使用拒绝采样来采样Xfb从而增加实时采样Xf的速率。从较大的分布中抽取的样本要么被保留要么根据它们的启发式值被拒绝。Akgun和Stilman[13]在他们的算法中使用了这样的技术。虽然这对于单次迭代来说计算成本很低但是找到Xfb中的单个状态所需的迭代次数与其相对于采样域的大小成正比。当解决方案接近理论最小值或规划域增长时这变得不平凡。Otte和Correll[8]在他们的并行化C-FOREST算法中从规划域的子集中抽取样本。这个子集被定义为包围椭球形有信息子集的超矩形。虽然这改善了样本拒绝的性能但随着问题维度的增加其效用会减少注2。3图剪枝图剪枝试图通过使用启发式函数将图限制在Xfb从而增加实时探索Xf。规划图中的启发式成本大于当前解决方案的状态被周期性地移除同时继续全局采样。RRTs的空间填充性质使剪枝图的扩展偏向于Xfb的边界。在子集被填充后只有来自Xfb本身的样本才能向图中添加新状态。因此图剪枝在贪婪地填充目标子集后就变成了一种拒绝采样方法。由于向RRT添加新状态需要调用最近邻算法图剪枝将比简单的样本拒绝更加计算昂贵同时仍然受到相同的概率限制。4随时RRTsFerguson和Stentz[6]认识到解决方案从上面限制了可以进一步改进的状态子集。他们的迭代RRT方法Anytime RRTs解决了一系列独立的规划问题这些问题的域由前一个解决方案定义。他们将这些域表示为椭圆但没有讨论如何生成样本。限制规划域鼓励每个RRT找到比前一个更好的解决方案然而为了做到这一点他们必须丢弃已经在Xfb中找到的状态。 对于在R中寻求最小化路径长度的问题 n有信息的采样域Xfb是一个以初始状态xstart和目标状态xgoal为焦点的椭圆。椭圆的形状取决于初始状态和目标状态两者之间的理论最小成本cmin以及迄今为止找到的最佳解决方案的成本cbest。椭圆的偏心率由cmin/cbest给出。 III. Informed RRT*中椭球形有信息子集的分析
给定一个正的成本函数从xstart到xgoal的最优路径的成本约束通过x ∈ Xf (x)等于从xstart到x的最优路径的成本g (x)加上从x到xgoal的最优路径的成本h (x)。由于基于RRT的算法渐进地从上面接近每个状态的最优路径一个可接受的启发式估计fb(·)必须估计这两个术语。可接受性的充分条件是组件gb(·)和bh (·)分别是g (·)和h (·)的可接受启发式。对于寻求在R中最小化路径长度的问题 n欧几里得距离是两个术语的可接受启发式即使有运动约束。那么可能改进当前解决方案的状态的有信息子集Xfb ⊇ Xf可以用当前解决方案的成本cbest的封闭形式来表示 Xfb x ∈ X ||xstart − x||2 ||x − xgoal||2 ≤ cbest , 这是n维长轴椭球即特殊的超椭球的一般方程。焦点是xstart和xgoal横向直径是cbest共轭直径是p c 2 best - c 2 min (图4)。fb(·)的可接受性使得添加一个状态在Xfb是改进解决方案的必要条件。由于RRT的空间填充性质添加这样一个状态的概率很快就变成了采样这样一个状态的概率1。 因此通过均匀采样一个更大的子集x i1 ∼ U (Xs)Xs ⊇ Xfb在任何迭代中改进解决方案的概率小于或等于集合度量λ (·)的比率 P注2矩形拒绝采样设Xs是一个紧密包围有信息子集的超矩形即每个边的宽度对应于长轴椭球体的直径[8]。从3中从Xs均匀抽取的样本在Xfb中的概率为ζn 2n随着n的增加这个概率迅速减小。例如当n 6时这给出了每次迭代通过拒绝采样改进解决方案的最大8%的概率无论具体的解决方案、问题或算法参数如何。定理1无障碍线性收敛对有信息子集的均匀采样x ∼ U Xfb 最佳解决方案的成本cbest在没有障碍的情况下线性收敛到理论最小值cmin。证明状态的启发式值等于通过状态并且焦点在xstart和xgoal的长轴椭球体的横向直径。对于均匀采样期望值是[20] E h fb(x) inc2 bestc 2 min (n1)cbest . (4) 我们假设RRT_的重连参数大于有信息子集的直径就像RRT_的渐进最优性的证明假设η大于规划问题的直径一样[7]。解决方案成本的期望值c i best然后是从直径为c i−1 best的长轴椭球体中抽取的样本的启发式成本的期望值即E c i best E h fb V. INFORMED RRT 我们在Algs. 1和2中提出了一个使用直接有信息采样的示例算法Informed RRT*。它与[7]中提出的RRT_完全相同只是增加了第3行、第6行、第7行、第30行和第31行。像RRT_一样它通过在状态空间中逐步构建树T (V, E)由一组顶点V ⊆ Xfree和边E ⊆ Xfree × Xfree组成来搜索规划问题的最优路径σ ∗ 。新的顶点是通过在自由空间中向随机选择的状态增长图来添加的。每个新顶点都会对图进行重连使得附近顶点的成本最小化。该算法与RRT_的不同之处在于一旦找到解决方案它就会将搜索集中在可以改进解决方案的规划问题的部分。它通过直接采样椭球形启发式来实现这一点。当找到解决方案时第30行Informed RRT_将它们添加到可能的解决方案列表中第31行。它使用这个列表的最小值第6行来计算和直接采样Xfb第7行。按照惯例我们认为一个空集的最小值是无穷大。新的子函数在下面描述而与RRT*共有的子函数的描述可以在[7]中找到Sample给定两个姿态xfrom, xto ∈ Xfree和一个最大启发式值cmax ∈ R函数Sample (xfrom, xto, cmax)返回从状态空间中独立且同分布i.i.d.抽取的样本xnew ∈ X使得从xstart到xgoal的最优路径的成本约束通过xnew小于cmax如第III节和Alg. 2所述。在大多数规划问题中xfrom ≡ xstart, xto ≡ xgoalAlg. 2的第2行到第4行可以在问题开始时计算一次。InGoalRegion给定一个姿态x ∈ Xfree函数InGoalRegion (x)如果状态在规划问题定义的目标区域Xgoal中返回True否则返回False。一个常见的目标区域是一个以目标为中心的半径为rgoal的球即 Xgoal x ∈ Xfree ||x − xgoal||2 ≤ rgoal 。RotationToWorldFrame给定两个姿态作为超椭球体的焦点xfrom, xto ∈ X函数RotationToWorldFrame (xfrom, xto)返回从超椭球体对齐的框架到世界框架的旋转矩阵C ∈ SO (n)如6所示。如前所述在大多数规划问题中这个旋转矩阵只需要在问题开始时计算一次。SampleUnitNBall函数SampleUnitNBall返回从单位半径的n球体的体积中均匀抽取的样本即xball ∼ U (Xball)。这个n球体以原点为中心。A. 计算重连半径 在每次迭代中重连半径rRRT∗必须足够大以保证几乎肯定的渐进收敛同时又要足够小以只生成可处理的数量的重连候选者。Karaman和Frazzoli[7]提出了这个重连半径的下界这个下界是以问题空间的度量和图中顶点的数量为基础的。他们的表达式假设样本的分布是单位平方的均匀分布。由于Informed RRT_均匀地采样可以改进解决方案的规划问题的子集所以可以从这个有信息子集的度量和其中的相关顶点来计算重连半径。这个更新的半径减少了必要的重连数量进一步改善了Informed RRT_的性能。正在进行的工作集中在找到这个表达式的确切形式但是[7]提供的半径似乎是合适的。这个表达式也存在一个k-最近邻版本。在第VI节中使用的两个规划问题。障碍物的宽度w和间隙的位置yg对于每次实验运行都是随机选择的。在5秒内对(a)的一个例子对于一个最优解决方案成本为112.01的问题。注意到障碍物的存在为椭球形子集提供了一个下界但是Informed RRT_仍然比RRT_搜索了一个显著减小的域从而增加了收敛速率和最终解决方案的质量。对于一个3%偏离中心的间隙的图5(b)的一个例子。通过将搜索空间集中在包含最优解的规划问题的区域的子集上Informed RRT_能够在4.00秒内找到通过狭窄开口的路径而RRT_需要12.32秒。
综合全局和增量信息的GuILD
简单来说,GuILD利用搜索树信息分解原始问题,定义局部采样子集,从而使采样更有针对性和高效,加速了基于采样的最优运动规划的收敛。Guided Incremental Local Densification (GuILD)它利用搜索树中顶点的到达成本的改进来指导采样策略从而更快地发现更短的路径。GuILD可以与任何基于采样的运动规划器结合使用例如RRT_Informed RRT_等。这是一种基于采样的运动规划器利用搜索树中的顶点到达成本的改善来指导采样策略从而更快地发现更短的路径。该算法的原理是当发现到搜索树中的某个顶点的更短路径时就可以缩小从该顶点到目标的Informed Set从而减少无效的采样。
提出了GuILD框架,可以有效利用搜索树信息来聚焦样本,从而指导增量密化。将原始的问题分解成两个子问题,引入了灯塔的概念,灯塔是一个搜索树中的顶点。定义了由灯塔诱导的局部子集,并比较了它们与已知集的理论属性。提出了几种BEACONSELECTOR策略,包括一种敌对多臂老虎机算法。在各种规划域中进行了实验比较,结果表明无论BEACONSELECTOR的选择如何,GuILD都优于已知集密化基线。GuILD在高维规划问题以及包含难以采样同伦类的规划域中表现出色。
算法核心原理GuILD的核心思想是利用搜索树中顶点的到达成本的改进来缩小采样空间从而更有可能采样到更优的点。具体来说GuILD维护了一个Informed Set (IS)它是一个包含所有可能产生更短路径的点的集合。IS的大小取决于当前最优路径的长度和欧几里得启发式。GuILD还维护了一个Local Subset (LS)它是一个包含所有到达成本有所改进的点的集合。LS的大小取决于到达成本的改进量和一个阈值参数。GuILD的采样策略是从IS和LS中按一定的概率进行采样其中LS的采样概率随着到达成本的改进量的增加而增加。这样GuILD可以在全局和局部之间进行平衡从而更快地发现更短的路径。关键步骤GuILD的关键步骤如下
初始化从配置空间中随机采样一个点作为搜索树的根节点将其加入到IS和LS中。迭代重复以下步骤直到找到一条从起点到目标的路径或者达到最大迭代次数。 采样从IS和LS中按一定的概率进行采样得到一个新的点。扩展在搜索树中找到与新点最近的节点如果两者之间的路径是可行的就将新点作为搜索树的一个新节点并连接到最近的节点。更新更新新节点的到达成本如果新节点到目标的距离小于当前最优路径的长度就更新最优路径。如果新节点的到达成本有所改进就将其加入到LS中。如果新节点到目标的距离小于IS的边界就将其加入到IS中。
GuILD算法
初始化搜索树T将起点xs加入T设置初始路径ξ为xs到xt的直线设置初始Informed Set IS为整个配置空间X。重复以下步骤直到找到一条满足条件的路径或达到最大迭代次数 从IS中采样一个点x如果x与T中的任何顶点相连则将x加入T并更新T中的边和顶点成本。如果x与xt相连则更新路径ξ为T中的最短路径并更新IS为{x|x ∈ X , h(xs, x) h(x, xt) ≤ c(ξ)}。对于T中的每个顶点xv计算其到目标的Informed Set ISv并从ISv中采样一个点xv’如果xv’与xv相连则将xv’加入T并更新T中的边和顶点成本。如果xv’与xt相连则更新路径ξ为T中的最短路径并更新IS为{x|x ∈ X , h(xs, x) h(x, xt) ≤ c(ξ)}。
输入配置空间X起点xs目标xt最大迭代次数N
输出一条从xs到xt的路径ξ或者空初始化搜索树T将xs加入T
设置初始路径ξ为xs到xt的直线
设置初始Informed Set IS为整个配置空间X
设置迭代次数i为0当i N时重复以下步骤从IS中采样一个点x如果x与T中的任何顶点相连则将x加入T并更新T中的边和顶点成本如果x与xt相连则更新路径ξ为T中的最短路径更新IS为{x|x ∈ X , h(xs, x) h(x, xt) ≤ c(ξ)}对于T中的每个顶点xv计算其到目标的Informed Set ISv并从ISv中采样一个点xv如果xv与xv相连则将xv加入T并更新T中的边和顶点成本如果xv与xt相连则更新路径ξ为T中的最短路径更新IS为{x|x ∈ X , h(xs, x) h(x, xt) ≤ c(ξ)}i i 1如果路径ξ存在则返回路径ξ
否则返回空
#其中h(x, y)表示x和y之间的欧几里得距离c(ξ)表示路径ξ的长度。GuILD框架主要优势
更高的性能无论信标选择器如何GuILD在一系列规划领域中都优于现有的最先进的Informed Set密集化基线。特别是在简单的规划领域GuILD能够带来适度的改进并在处理困难领域时表现出色[1]。更高的采样效率在实验中无论信标选择器如何GuILD的采样效率都比从Informed Set采样更高。这支持了GuILD的假设即所有三个GuILD选择器都能达到相当的采样效率这表明他们的关键在于如何选择信标而不是信标的具体选择[3]。自动聚焦采样在低成本同源性规划问题中GuILD能够自动聚焦采样以通过这些狭窄的通道提供路径。在Trap环境中GuILD实例能够在大约1100-1200个样本中识别出狭窄的通道并快速提供路径[2]。灵活性GuILD框架可以轻松并且高效地融入任何基于采样的最优运动规划算法[5]。利用搜索树的信息GuILD框架的一个核心观点是即使规划器未能发现更短的路径搜索树中仍然包含有价值的信息可以用来指导后续的路径搜索和优化[3]。
总的来说GuILD框架的优势主要体现在其高效的性能、采样效率、自动聚焦采样的能力以及其灵活性和对搜索树信息的有效利用。(a) Informed Set和(b) GuILD引导的Local Subsets的比较。为了进一步缩小Informed Set的范围GuILD选择了一个信标绿色并利用搜索树中的信息将原始问题分解为两个较小的问题。(a) 初始解。 (b) GuILD选择一个信标并引导Local Subsets绿色它们没有覆盖狭窄的通道。 © 规划器没有找到一条更短的路径到达目标所以Informed Set保持不变。但是GuILD利用搜索树中到达成本的改善来更新Local Subsets。起点-信标集缩小以进一步聚焦采样而信标-目标集之间的剩余余量用于扩展信标-目标集。Local Subsets现在覆盖了狭窄的通道聚焦采样在Informed Set内以快速收敛到(d)最优解。Local Subsets ELS绿色由信标b定义 以及搜索树上的到达成本g(b)和当前最优解的成本c(ξ)。
小结
本文介绍了“基于采样的路径规划常用算法”介绍的算法有很多种但其实都是围绕PRM图搜索、RRT树搜索做的优化、组合的变种。Sample base plan这是一种基于样本的规划方法它通过生成大量的样本点来寻找最优解。这种方法的优点是可以处理高维度和复杂的问题因为它不需要对整个问题空间进行详细的搜索。然而这种方法的缺点是可能会产生次优的解决方案因为它并不总是能找到全局最优解⁴。此外如果样本点的生成和选择方法不合适可能会导致效率低下⁵。 尽管这些算法在实现细节上有所不同但它们都遵循了基于采样的路径规划算法的基本框架即采样、构建图或树、碰撞检测和路径搜索。这些共同的处理流程使得这些算法能够在各种复杂的环境中进行有效的路径规划。具体选择哪种算法需要根据实际应用的需求和环境条件来决定。基于采样的路径规划算法的共同处理流程主要包括以下几个步骤** a.采样**在配置空间中随机撒点生成一组采样点。 ** b.构建图或树**根据采样点之间的关系例如距离、连通性等构建一个图或树来表示整个配置空间。 ** c.碰撞检测**检查采样点和采样点之间的连线是否与障碍物发生碰撞如果有碰撞则舍弃这些采样点或连线。 ** d.路径搜索**在构建的图或树上搜索一条从起点到终点的路径。 这个流程是基于采样的路径规划算法的基本框架不同的算法可能会在这个基础上添加一些额外的步骤例如概率路图算法PRM的学习阶段和查询阶段快速随机扩展树算法RRT的树扩展过程RRT*算法的路径优化过程等。这些算法的具体实现和优化方法可能会有所不同但它们的核心思想都是通过采样和搜索来寻找一条满足特定条件的路径。基于采样的路径规划算法如概率路线图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系列算法:适合在线规划
