基于采样的路径规划
- 基于采样的方法会通过探索解空间的连续性来获得可行或最优解
- 通过在离散的C-space中采样来构建树或者图，从而表示解空间的连续性，而不是显式地构造它
- 其性能通常取决于：采样策略、碰撞检测、邻点搜索
术语
-
Probabilistic Completeness 概率完备
A feasible solution will be found, if one exists, with probability one as the number of samples approaches infinity.
-
Astmptotical (Near) Optimality 渐进最优
The cost of the returned solution converges almost surely to the optimum as the number of samples approaches infinity.
-
Anytime 即时性
Quickly find a feasible but not necessarily optimal solution, then incrementally improve it over time.
Feasible Path Planning Methods
Probabilistic Road Map （PRM）
Learning phase 学习阶段
在C-space中随机采样数个点，并且删除有碰撞的点。
Query phase 查询阶段
将每个点与邻点相连，并且删除有碰撞的部分；得到图之后用基于图搜素的算法来求解
lazy collision-checking
在找到路径后再检查路径中是否有碰撞
Rapidly-exploring Random Trees
算法流程
-
初始化：图， x i n i t x_{init} xinit， x g o a l x_{goal} xgoal
-
LOOP:
-
在图中随机采样得到一个点 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的连线上以固定步长生成新点 x n e w x_{new} xnew
-
构成 x n e a r x_{near} xnear到 x r a n d x_{rand} xrand的边
-
IF 边无碰撞：
- 在树中加入节点 x n e w x_{new} xnew和边
-
IF x n e w = x g o a l x_{new}=x_{goal} xnew=xgoal：
- 成功
-
-
END LOOP
Optimal Path Planning Methods
Rapidly-exploring Random Tree* (RRT*)
算法流程
-
初始化：图， x i n i t x_{init} xinit， x g o a l x_{goal} xgoal
-
初始化树 T T T
-
LOOP:
-
在图中随机采样得到一个点 x r a n d x_{rand} xrand
-
在已经生成的树 T T T中找到与 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的连线上以固定步长生成新点 x n e w x_{new} xnew
-
构成 x n e a r x_{near} xnear到 x r a n d x_{rand} xrand的边
-
IF 边无碰撞：
- 在 x n e w x_{new} xnew的邻域内找树 T T T中与之相近的数个点 X n e a r X_{near} Xnear
- 在 X n e a r X_{near} Xnear中选择 x n e w x_{new} xnew的父节点，其准则是使得到父节点连接到 x n e w x_{new} xnew之后，路径长度最短（局部最优）
- 在树 T T T中新增 x m i n x_{min} xmin到 x n e w x_{new} xnew的连接
- Rewire: 重新调整树的结构，利用新加入点 x n e w x_{new} xnew的信息来保证上述邻域中其他节点的局部最优性
-
IF x n e w = x g o a l x_{new}=x_{goal} xnew=xgoal：
- 成功
-
-
END LOOP
范围查询策略
查询范围：
range
=
min
{
γ
(
log
(
card
(
V
)
)
card
(
V
)
)
1
d
,
η
}
\text{range} = \min\{\gamma(\frac{\log(\text{card}(V))}{\text{card}(V)})^{\frac{1}{d}},\eta\}
range=min{γ(card(V)log(card(V)))d1,η}
渐进最优性保证：
γ
>
(
2
(
1
+
1
d
)
)
1
d
(
μ
(
X
f
r
e
e
)
ξ
d
)
1
d
\gamma>(2(1+\frac{1}{d}))^{\frac{1}{d}}(\frac{\mu(X_{free})}{\xi_d})^{\frac{1}{d}}
γ>(2(1+d1))d1(ξdμ(Xfree))d1
式中，
d
d
d是空间的维数，
card
(
⋅
)
\text{card}(\cdot)
card(⋅)表示集合元素个数，
η
\eta
η为步长，
ξ
d
\xi_d
ξd为
d
d
d维空间下单位球体积，
μ
(
X
f
r
e
e
)
\mu(X_{free})
μ(Xfree)为规划空间的大小
经验做法是，设置略大于RRT步长（steer distance）的常数范围大小
程序优化技巧
-
Bias Sampling
Sample biasing toward the goal
-
Sample Rejection
Reject samples with g + h > c*
-
Branch-and-bound
Prune the non-promising sub trees to reduce neighbor query cost
-
Graph Sparsify
Reject samples by resolution. Introduce near optimality.
-
Neighbor Query
k-nearest or range near; Approximate nearest neighbor; Range tree for different dimension.
-
Delay Collision Check
Sort the neighbours by potential cost-to-come values.
Check collisions in order and stop once a collision-free edge is found. -
Bi-directional search
-
Conditional Rewire
Rewire Until the first solution is found.
-
Data Re-use
Store the collision-checking results for ChooseParent and Rewire (only valid for symmetric cost).
Convergence Acceleration
RRT算法是对自由空间进行均匀采样，搜索树上会生成很多冗余的分支，所以RRT算法的收敛速度很慢。我们可以对RRT*算法的采样过程进行改进。
RRT*的缺陷：
-
Over-exploitation
不需要对不能使路径更优的节点做Rewire
-
Under-exploitation
Rewire的范围仅作用于新加入节点的相邻节点；并不能保证在每一步中的临时路径均最优
RRT#
RRT#在exploitation方面对RRT*做出了改进，保证在每一步中，已经获得的信息被最大程度利用（exploitation）。
- 保证在每一时刻现有采样点的情况下返回的都是最短路径
- 能对树中定点进行分类，分辨出哪些有可能成为最优路径的一部分（promising points）
Informed Sampling
Informed Sampling 是在采样策略（exploration）方面对RRT类算法进行改进。引入了Informed sets的概念，将采样的空间变为Informed sets而不是整个解空间。通过不断迭代，缩小informed set的大小，使得结果趋向于最优解。
Informed set
omniscient set（全知集）是一个在其中采样不会使得结果更差的集合，而Informed sets是对omniscient set的估计，具体有以下三种选择：
- Bounding boxes
- Path heuristics
- L2 heuristic
考虑precision和recall rate，选择L2 heuristic，其形状为椭圆形
其长轴长度为当前搜索到的最短路径长。在迭代过程中，每次找到更短的路径，就对椭圆参数进行更新，生成更小的Informed set，从而使结果趋向最优。
采样算法
先在以原点为中心的单位圆中进行采样，然后利用椭圆参数，经过拉伸、旋转、平移得到最终的采样点坐标。
GuILD (Guided Incremental Local Densification)
Informed Sampling 的缺陷：当起点到目标点之间出现可使路径更优的窄缝时，由于采样到窄缝的概率很低，路径将很难在迭代过程中被优化
GuILD：由beacon node 划分出local subsets，在两个集合的并集中采样，能更快的收敛到最优解
Local subsets分为两个集合，一个为搜索树起点到beancon node g ( b ) g(b) g(b)，一个为 c ( ξ ) − g ( b ) c(\xi)-g(b) c(ξ)−g(b)。在迭代过程中，前者不断收缩而后者不断扩大，从而使得算法能更容易采样到窄缝中的点。
Kd树
Kd树是什么
Kd树如何构建
基于Kd树的kNN算法
声明：
本学习笔记基于深蓝学院的《机器人运动规划》课程，如有侵权请联系我删除
扫一扫
231
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
