3.1 随机性采样: PRM

3.1.1 PRM算法两个步骤

1. 学习阶段（预处理阶段）：对状态空间内的安全区域均匀随机采样n个点，每个采样点分别与一定距离内的临近采样点连接，并丢弃掉与障碍物发生碰撞的轨迹，最终得到一个连通图。

2. 查询阶段：对于给定的一对初始状态和目标状态，分别将其连接到上述连通图中，运用图搜索的方法（Dijkstra，A*等）找出一条可行路径。

3.1.2 PRM算法伪代码

1. Line1~Line2: 初始化两个集，其中V表示随机点集，E表示路径集。

2. Line3~Line8: 每次随机采样一个无碰撞的点，直到V中的点集个数达到n。

3. Line9~Line16: 生成概率路图。
1. Line10: 对V中的每个点q，根据一定距离选择最近的k个临域点
2. Line11~Line15: 对每个临域点q’进行判断，若q和q’之间尚无路径，则将其连接形成路径，随后进行碰撞检测，若无碰撞，留下该路径。

3.1.3 PRM算法可视化（Fig.1 ~ Fig.8）

3.1.4 PRM算法优缺点

• 原理简单，容易实现。

• 概率完备性，在处理高维空间时非常灵活高效。

• 构建的Road Map覆盖了整个状态空间，对于确定的start point和goal point是比较浪费资源的。

• 参数的设置对结果影响很大，采样点的数量、领域的大小设置不合理均可能导致路径规划失败。

• 存在狭窄通路问题：空白区域采样点密集，障碍物区域采样点较少，可能无法得到最优路径。

• 最终的路径不满足动力学约束。

3.1.5 PRM算法关键点

1. 如何选择随机采样？
1. 在整个空间内（包含feasible region和infeasible region）均匀随机采样。

2. 最近的k个邻域点如何选择？
1. k-近邻搜索（KNN）：寻找距离当前节点最近的k个邻域点。
2. 半径邻域搜索（Radius Nearest Neighbor）: 找到某个半径距离范围内所有的邻居节点。
3. 可变半径PRM： 把r为半径的圆作为采样节点个数n的函数，采样点较少情况下，r可以取大一点，采样点足够多的时候，r取小一点，即PRM*算法.
4. 高效数据结构：采用KD树（kd tree）等。

3. 点与点之间的局部path如何生成？
1. 直线插值：节点之间通过直线连接

4. 局部路径的碰撞检测
1. 可以增量式取点或者二分法取点，判断点是否在障碍物区域内（如下图所示）。
2. 碰撞检测部分可参考1.2节。