第三章 路径规划策略：全局与局部路径规划
各位同学，今天我们来聊聊路径规划策略。说实话，这是整个多轴插补系统里最烧脑的部分之一。我做了这么多年运动控制，见过太多因为路径规划不当导致撞机、抖动、甚至电机烧毁的案例。嗯，咱们今天就把这块硬骨头啃下来。
3.1 全局路径规划 vs 局部路径规划
先问大家一个问题：你从北京开车去上海，需要做哪些决策？
首先，你得看地图，决定走京沪高速还是沿海高速。这是全局路径规划——在已知的静态环境中，找出一条从起点到终点的可行路径。
但上了高速之后呢？前面有辆大货车开得慢，你得变道超车；前方施工，你得绕行。这就是局部路径规划——根据传感器实时反馈，动态调整当前的运动轨迹。
说白了，全局规划是「战略层面」，局部规划是「战术层面」。两者缺一不可。
核心区别：
- 全局规划：已知环境、离线计算、追求最优解
- 局部规划：未知障碍、在线计算、追求实时性
我在一个自动化仓储项目中遇到过这样的问题：全局路径规划得很好，但机器人一运行就撞到临时堆放的货物。为什么？因为全局规划不知道现场有变化。后来我加了一层局部路径规划，用激光雷达实时检测障碍物，问题就解决了。
3.2 A*算法：最常用的全局路径规划算法
A*算法，说白了就是「带方向的Dijkstra」。它用了一个启发式函数来引导搜索方向，避免像Dijkstra那样漫无目的地扩散。
公式很简单：f(n) = g(n) + h(n)
g(n)：从起点到当前节点的实际代价h(n)：从当前节点到终点的估计代价（启发式函数）f(n)：总代价
我个人习惯用欧几里得距离作为启发式函数，效果比较稳定。但要注意，如果h(n)估计得太大，A*会退化成贪心算法；如果太小，又退化成Dijkstra。这个平衡点需要根据实际场景调整。
避坑指南：我曾经在一个AGV项目中，因为启发式函数设置不当，导致A*算法在狭窄通道里来回震荡，路径规划耗时增加了3倍。后来改用曼哈顿距离，效果好了很多。
3.3 Dijkstra算法：最可靠的「笨办法」
Dijkstra算法是路径规划里的「老黄牛」。它不依赖任何启发式信息，老老实实地从起点开始，一层一层往外扩展，直到找到终点。
优点：一定能找到最短路径。
缺点：搜索范围太大，效率低。
你想想看，如果地图有10000个节点，Dijkstra可能要遍历其中8000个才能找到路径。而A*可能只需要遍历2000个。这就是为什么在实际工程中，Dijkstra用得越来越少。
但话说回来，Dijkstra在某些场景下还是不可替代的。比如在拓扑地图中，节点数量不多，但需要严格保证最优性时，我依然会用Dijkstra。
3.4 RRT算法：高维空间的「探险家」
RRT（Rapidly-exploring Random Tree）算法，是我个人觉得最「聪明」的路径规划算法。它不追求全局最优，而是用随机采样的方式快速探索空间。
核心思想：在自由空间中随机撒点，然后从已有的树中找最近的节点，向新点生长一段距离。重复这个过程，直到树生长到目标区域。
// RRT算法伪代码
function RRT(start, goal, maxIterations):
tree = {start}
for i = 1 to maxIterations:
randomPoint = sampleRandomPoint()
nearestNode = findNearest(tree, randomPoint)
newNode = extend(nearestNode, randomPoint, stepSize)
if collisionFree(nearestNode, newNode):
tree.add(newNode)
if distance(newNode, goal) < threshold:
return extractPath(tree, newNode)
return failureRRT最大的优势是：在高维空间（比如6轴机器人）中，它比A*和Dijkstra快得多。为什么？因为它不需要遍历整个空间，而是用随机采样「碰运气」。
注意：RRT算法不保证找到最优路径，甚至不保证能找到路径（虽然概率趋近于1）。而且生成的路径往往很「粗糙」，需要后续做平滑处理。
我记得有一次做7自由度机械臂的避障规划，A*跑了5分钟还没出结果，换成RRT后，3秒就找到了一条可行路径。虽然路径弯弯曲曲的，但至少能用了。
3.5 算法对比与选型建议
| 算法 | 最优性 | 效率 | 适用场景 |
|---|---|---|---|
| Dijkstra | 保证最优 | 低 | 小规模、静态环境 |
| A* | 保证最优（合理启发式） | 中 | 中等规模、静态环境 |
| RRT | 不保证最优 | 高 | 高维空间、动态环境 |
选型建议：
- 如果地图小、要求最优解 → 用Dijkstra
- 如果地图中等、要求最优解 → 用A*
- 如果维度高、实时性要求高 → 用RRT
3.6 知识体系总览
下面这张图是我自己整理的路径规划知识体系，大家可以对照着理解各个算法之间的关系。
嗯，这张图把今天讲的内容串起来了。大家可以看到，全局规划和局部规划是两条腿走路，缺一不可。而A*、Dijkstra、RRT这三个算法，各有各的脾气，选对了事半功倍，选错了...嗯，你懂的。
我的建议：初学者先从A*入手，它最「中庸」，也最实用。等把A*吃透了，再去看Dijkstra和RRT。我在带团队的时候，都是让新人先拿A*练手，一般两周就能上手。
好了，今天的内容就到这里。路径规划这块，光看理论是不够的，建议大家找几个开源库（比如ROS的navigation stack）跑一跑，看看实际效果。有什么问题，咱们下节课再聊。
公众号:蓝海资料掘金营，微信deep3321