3. 路径规划基础：图搜索算法（Dijkstra、A*）、RRT、PRM
各位同学，今天我们来聊聊路径规划里最基础、也最核心的一块——图搜索算法。说实话，我当年刚入行时，觉得路径规划不就是找条路嘛，有啥难的？直到我在一个仓储机器人项目里，看着机器人在货架间原地打转，我才意识到：选对搜索算法，比多装两个激光雷达管用多了。
这一章，我会把图搜索里的几个经典算法掰开揉碎讲清楚。Dijkstra、A*、RRT、PRM，它们各有脾气，也各有适用场景。你想想看，一个扫地机器人和一个手术机器人，用的规划算法能一样吗？肯定不能。
3.1 图搜索的核心思想
说白了，路径规划就是把环境抽象成一张图。图里有节点（位置点）和边（可通行路径）。我们要做的，就是在这张图上找到一条从起点到终点的最优路径。
嗯，这里要注意：“最优”的定义是变化的。有时候是距离最短，有时候是时间最少，有时候是能耗最低。我在做AGV调度时，就遇到过要求“转弯次数最少”的奇葩需求——因为每次转弯都会磨损轮子。
- 节点（Node）：空间中的离散位置点
- 边（Edge）：节点之间的连接关系，通常带有代价（Cost）
- 搜索策略：如何遍历这些节点，找到目标路径
下面这张图，是我自己画的一个知识框架，帮你理清这几种算法的关系：
3.2 Dijkstra算法：稳扎稳打的“老实人”
Dijkstra算法，我习惯叫它“老实人算法”。为什么？因为它不偷懒，不取巧，从起点开始，一层一层往外扩张，直到找到终点。
它的核心逻辑很简单：维护一个“未访问节点”的集合，每次从中选出距离起点最近的节点，然后更新它邻居的距离。说白了，就是贪心 + 动态规划。
3.2.1 算法伪代码
function Dijkstra(Graph, start, goal):
dist[start] = 0
prev[start] = null
unvisited = all nodes in Graph
while unvisited is not empty:
u = node in unvisited with smallest dist
if u == goal:
break
remove u from unvisited
for each neighbor v of u:
alt = dist[u] + cost(u, v)
if alt < dist[v]:
dist[v] = alt
prev[v] = u
return reconstruct_path(prev, goal)
你看，代码就这么几行。但实际跑起来，它会把整个地图都探索一遍。我记得有一次调试，地图里有个死胡同，Dijkstra愣是把它也搜了个遍——这就是它“老实”的地方。
3.3 A*算法：带“指南针”的搜索
A*算法，说白了就是在Dijkstra的基础上加了个“指南针”。这个指南针就是启发式函数（Heuristic），它告诉算法：“嘿，终点在那边，你先往那个方向搜！”
A*的代价函数是：f(n) = g(n) + h(n)
g(n)：从起点到当前节点n的实际代价h(n)：从当前节点n到终点的估计代价（启发式）
我个人习惯用欧几里得距离作为启发式函数。为什么？因为它计算简单，而且满足“可采纳性”——也就是说，它永远不会高估到终点的距离，保证了A*能找到最优解。
3.3.1 A* vs Dijkstra 对比
| 特性 | Dijkstra | A* |
|---|---|---|
| 搜索方式 | 无信息搜索（盲目） | 启发式搜索（有方向） |
| 最优性 | 保证最优 | 保证最优（h可采纳时） |
| 搜索空间 | 大（全图探索） | 小（指向终点） |
| 适用场景 | 小规模、静态地图 | 中大规模、静态地图 |
| 效率 | 较低 | 较高 |
你想想看，如果地图很大，Dijkstra要搜一大片区域，而A*直接朝着终点方向搜，效率自然高出一大截。但A*也有弱点——如果启发式函数设计得不好，它可能比Dijkstra还慢。
3.4 RRT：快速探索随机树
好，接下来我们聊聊RRT。这个算法跟前面两个完全不同。Dijkstra和A*都是确定性搜索，而RRT是随机采样的。
RRT的全称是Rapidly-exploring Random Tree。它的思路很暴力：在地图上随机撒点，然后从已有的树中找最近的节点，向这个随机点生长一段距离。重复这个过程，直到树长到终点附近。
说实话，我第一次看到这个算法时，觉得它太“野”了——随机撒点能靠谱吗？后来在一个高维机械臂的规划项目里，Dijkstra和A*都因为状态空间太大而跑不动，RRT反而轻松搞定了。
- 适合高维空间：比如7自由度的机械臂，状态空间是7维的，图搜索根本没法建图
- 不需要显式建图：只需要一个碰撞检测函数就行
- 速度快：因为只采样部分空间，不是全空间搜索
3.4.1 RRT的改进版：RRT*
RRT有个问题——它找到的路径通常不是最优的，甚至可能很绕。后来有人提出了RRT*，在生长过程中加入了重新连接（Rewire）的步骤，能逐步优化路径。
我在一个无人机避障项目里用过RRT*。说实话，它比RRT慢一些，但路径质量好很多。如果你对路径质量有要求，建议直接用RRT*。
3.5 PRM：概率路图
PRM（Probabilistic Roadmap）跟RRT有点像，也是基于随机采样的。但它的思路不同：先建图，再查路径。
PRM分两步走：
- 学习阶段：在地图上随机撒点，只保留在自由空间里的点，然后连接相邻的点，形成一个路图（Roadmap）
- 查询阶段：把起点和终点连接到路图上，然后用Dijkstra或A*在路图上找路径
你想想看，这就像你先画好一张城市地图，然后每次导航时直接查地图就行了，不用每次都重新探索。所以PRM特别适合多查询（Multi-Query）的场景——比如同一个地图上，要反复规划不同起点终点的路径。
3.6 算法选型指南
讲了这么多，到底该用哪个？我根据自己的项目经验，给你一个简单的选型建议：
| 场景 | 推荐算法 | 原因 |
|---|---|---|
| 小地图（<1000节点），静态 | Dijkstra 或 A* | 保证最优，实现简单 |
| 大地图（>10000节点），静态 | A* | 启发式加速，效率高 |
| 高维空间（机械臂、无人机） | RRT / RRT* | 避免维度灾难 |
| 同一地图多次查询 | PRM | 预处理后查询极快 |
| 动态环境（障碍物移动） | RRT 或 D* Lite | 能快速重规划 |
嗯，这里要提醒一句：没有银弹。我见过有人非要用A*做高维机械臂规划，结果状态空间爆炸，内存直接溢出。也见过有人用RRT做二维小地图规划，路径歪歪扭扭，看着就难受。选算法，一定要看场景。
3.7 本章小结
这一章我们聊了四种经典的路径规划算法：
- Dijkstra：稳扎稳打，保证最优，但效率一般
- A*：带指南针的Dijkstra，效率高，是实际项目中最常用的
- RRT：随机采样，适合高维空间，但不保证最优
- PRM：先建图再查询，适合多查询场景
我个人觉得，作为机器人算法工程师，A*和RRT是必须掌握的。这两个算法覆盖了90%以上的实际场景。至于Dijkstra，理解它的原理就好，因为A*就是它的升级版。PRM嘛，遇到多查询场景时再拿出来用。
下一章，我们会把这些算法放到实际系统中，看看怎么在ROS里实现它们。到时候我会分享一些代码层面的坑，比如怎么处理动态障碍物、怎么优化碰撞检测——这些都是我在项目里踩过的雷。