路径规划——SAMPLE SEARCH方法（PRM 、RRT、RRT*）

SAMPLE SEARCH

参考资料来源：高飞老师的《移动机器人规划》课程、Steven M.LaValle的《planning algorithms》、csdn和知乎上的笔记摘要。

注：这里指的路径规划，指的是不包括时间信息的路径（并非轨迹），大部分内容参考高飞老师的《移动机器人规划》课程，如果之后会用到SAMPLE SEARCH方法，在写关于论文解读的博客，下述内容主要是对SAMPLE SEARCH的流程的初步描述。

0.SAMPLE SEARCH 的引入

基于图搜索的方法在处理高维问题时效率低下，而基于采样的路径规划方法可以有效解决该问题。SAMPLE SEARCH是一种基于样本的规划方法，它通过生成大量的样本点来寻找最优解。这种方法的优点是可以处理高维度和复杂的问题，因为它不需要对整个问题空间进行详细的搜索。然而，这种方法的缺点是可能会产生次优的解决方案，因为它并不总是能找到全局最优解。此外，如果样本点的生成和选择方法不合适，可能会导致效率低下。

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

• 采样：在配置空间中随机撒点，生成一组采样点。
• 构建图或树：根据采样点之间的关系（例如距离、连通性等），构建一个图或树来表示整个配置空间。
• 碰撞检测：检查采样点和采样点之间的连线是否与障碍物发生碰撞，如果有碰撞，则舍弃这些采样点或连线。
• 路径搜索：在构建的图或树上搜索一条从起点到终点的路径。

2.常见的SAMPLE SEARCH 算法

2.1 PRM (Probabilistic Roadmap Method)

• 原理：随机散点，将这些散点构建成为一个图结构

• 伪代码流程：

  1.采样：在C-space 中随机采样N个点
  2.碰撞检测：删除在障碍物上的点
  3.构建图结构：对于每个节点，找到距离最近的几个节点，并在两个节点之间添加一条边
  4.路径搜索：使用图搜索方法找到一条可行path
  

• 图示：
  

• 改进策略：Lazy collision-checking
  在不考虑碰撞的情况下采样点并生成线段；在未碰撞的路线图上找到一条路径；如果路径发生碰撞，则删除相应的边和节点；重新开始寻找路径。
  

2.2 RRT (Rapidly-exploring Random Tree)

• 原理：一种增量搜索算法，通过扩展一棵树来探索空间。
• 伪代码流程：
  
  循环体解读：

Xrand <- Sample(M) //在地图上随机采样一个点Xrand
Xnear <- Near(Xrand,T) // 查找距离随机点Xrand最近的节点Xnear
Xnew <- Steer(Xrand,Xnear,StepSize) // 沿着Xnear到Xrand方向前进stepsize的距离得到Xnew
Ei <- Edge(Xnew,Xnear) // 记Xnew 和 Xnear 作为一条边
    if CollisionFree(M,Ei) then
        T.addNode(Xnew)
        T.addEdge(Ei) // 如果与障碍物没有碰撞，则记作完成一次空间搜索拓展
    if Xnew = Xgoal then
        Success(); // 找到目标点即可找到路径

• 图示：
  随机采样+寻找Xnear+根据步长确定Xnew：
  

  碰撞检测：
  
  路径搜索：
  
  ROS版本：
  

2.3 RRT*

• 原理：与RRT类似，单在扩展树的同时，还会改进树上已经存在的路径，通过重新连接节点来优化路径质量

• 伪代码流程：
  
  注：表头为RRT*

  解读

  if CollisionFree(Xnew) then
    DXnear <- NearC(T,Xnew);//尝试连接周围的其他节点
    xmin <- ChooseParent(DXnear,Xnear,Xnew);//选取代价最小的节点xmin
    T.addNodEdge(Xmin,Xnew);//在Xmin和Xnew之间连接边
    T.rewire();//范围内节点重新连接
  

• 图示：
  尝试连接其他节点：
  

  选取cost最小的节点：
  

  在Xmin和Xnew之间添加边：
  

  范围内的节点重新连接：
  
  ROS版本：