算法分类
- 基于图结构：Dijkstra、A*、D*
- 基于采样：PRM、RRT
Dijkstra算法
算法原理：假如P(A,F)是A到F的最短路径，D是该最短路径上的某个点，那么P(A,D)必定也是A到D的最短路径。按照这个结论，只要不断搜索从出发点到其他点的最短路径，就能得到所需的求解路径。
算法流程：
- 根据与当前节点有直接关系的节点，计算起点与各节点的最近路径
- 访问未访问节点中离起点距离最短的节点，回到步骤1
- 直至所有节点均被访问
A*算法
算法原理：在代价函数中加入该节点到目标节点的估计代价（启发函数），降低搜索节点的范围。
$ f_{代价函数}(x)=g_{实际代价}(x)+h_{估计代价(启发函数)}(x) \tag{1} $
$ h(x)<cost^*(x,target) \tag{2} $
算法流程同Dijkstra，仅在计算距离（代价）时有差别。估计代价h(x)越小，搜索节点的范围越大。当估计代价h(x)为0时，A*算法退化为Dijkstra算法。但估计代价h(x)也不能过大，必须满足式（2），才能保证A*算法的完备性。
h(x)的常见形式有曼哈顿距离、对角线距离、欧氏距离等。当栅格地图中只允许向4个方向（上、下、左、右）移动时，选用曼哈顿距离；当栅格地图中允许向8个方向（上、下、左、右、左上、左下、右上、右下）移动时，选用对角线距离；当栅格地图中允许向任意方向移动时，选用欧氏距离。
Hybrid A*（混合A*）
Hybrid A*算法是一种图搜索算法，改进于A*算法。与普通的A*算法区别在于，Hybrid A*规划的路径考虑了车辆的运动学约束，即满足了车辆的最大曲率约束。
由于车辆运动学模型的限制，车辆无法按照 A* 所生成的路径行走。例如下图所示的情况，车辆最初的朝向是向下的，而A*的路径则是从障碍物的上方绕过去的。由于转弯半径的限制，这样的路径无法跟随，而Hybrid A*则会考虑到车辆初始方向和终点方向，以及每一次节点拓展都符合车辆的运动学，因此生成的路径一定是车辆可以跟随的。
Hybrid A*规划的路径由两部分组成，第一部分是考虑了车辆运动学的探索结点连接而成的路径；第二部分是使用ReedSheeps曲线连接中间点位姿与目标位姿的路径。
PRM算法
算法原理：全名Probabilistic Road Map，也称概率路线图算法，将机器人所处的连续空间用随机采样离散化，再在离散采样点上进行路径搜索。
算法流程：
- 随机采样：在地图上抛撒一些随机点，利用这些随机点对地图中的连续空间进行离散化
- 移除无效采样点：将落在障碍物上的采样点删除
- 连接：按照最近邻规则将采样点与周围相邻点进行连接
- 移除无效连接：将横穿障碍物的连接删除，这样就构建出了所谓的PRM路线图
- 添加导航任务的源节点和目标节点：将源节点和目标节点与PRM路线图相连
- 搜索路径：在构建出来的PRM路线图上利用A*算法搜索源节点到目标节点之间的路径
RRT算法
算法原理：全名Rapidly-Exploring Random Tree，快速搜索随机树，一边通过采样方式构建树结构，一边进行路径搜索。
算法流程：
- 初始化：设置起始点作为树的根节点。
- 随机采样：随机生成一个点，并将其作为新的目标点。
- 扩展树：从树中找到距离新目标点最近的节点，然后在该节点和新目标点之间生成一条新的路径段。确保路径段不会穿越障碍物或违反运动约束。
- 添加节点：将生成的新节点添加到树中。
- 判断终止条件：检查新节点是否接近目标点，如果满足终止条件，则生成了一条可行路径。
- 重复步骤2至步骤5，直到达到指定的迭代次数或找到了可行路径。
参考
- 《机器人SLAM导航：核心技术与实践》张虎，机械工业出版社
- 知乎：路径规划该如何入门?
- 各种算法demo：github/zhm-real/PathPlanning
- 仿真平台：PathFinding（如果演示过程中卡住，可以先点一下别的标签页，再点一回原标签页。或者先将浏览器最小化，再复原）
- Introduction to the A* Algorithm
- csdn：路径规划算法学习参考资料
- 知乎：混合A*算法详解
- 博客园：Hybrid A*算法原理
- 知乎：Hybrid A*原理与代码
- Hybrid A*论文：Practical Search Techniques in Path Planning for Autonomous Driving
- 知乎：从零开始学图形学：10分钟看懂贝塞尔曲线
