4、全局路径规划：A*算法原理、Dijkstra算法、Nav2中的全局规划器配置
各位同学，今天我们来聊聊全局路径规划。说白了，就是给机器人找一条从A到B的「最优路」。你想想看，机器人在地图里跑，总不能瞎撞吧？它得知道往哪走最省时间、最安全。
我个人习惯把全局规划比作「导航的大脑」。它不关心你脚下有没有小石子，它只关心大方向对不对。嗯，这里要注意：全局规划算出来的路径，通常是一系列离散的坐标点，后续还要靠局部规划器去细化执行。
4.1 核心算法：Dijkstra 与 A*
这两个算法，是全局路径规划的「老大哥」和「小老弟」。我当年刚入行时，第一个手写的路径规划代码就是Dijkstra，那会儿还傻乎乎地以为它是最快的……后来才知道，A*才是工程界的宠儿。
4.1.1 Dijkstra 算法：稳，但慢
Dijkstra的思路特别朴素：从起点开始，一层一层往外「波」开，直到碰到终点。它保证找到的路径是全局最短的，代价就是——它得把整个地图都扫一遍。
核心公式： g(n) = 从起点到当前节点 n 的实际代价
每次从待选节点中，挑 g(n) 最小的那个去扩展。
我在项目中遇到过一个问题：用Dijkstra规划一个100x100的栅格地图，跑了将近2秒才出结果。对于静态地图还行，但机器人一旦需要动态避障，这速度就捉襟见肘了。
避坑指南：我曾经在室内仓库项目里，直接用Dijkstra做全局规划，结果机器人每次启动都要等3秒才能动。后来换成A*，启动延迟降到了0.5秒以内。所以，如果你的地图很大，或者对实时性有要求，Dijkstra可能不是最佳选择。
4.1.2 A* 算法：快，还准
A* 在Dijkstra的基础上加了一个「启发式函数」h(n)。说白了，就是给算法一个「直觉」：往终点方向走，比往反方向走更有可能找到近路。
核心公式： f(n) = g(n) + h(n)
- g(n)：从起点到当前节点的实际代价（和Dijkstra一样）
- h(n)：从当前节点到终点的估计代价（启发式）
- f(n)：总代价，算法每次挑 f(n) 最小的节点去扩展
为什么会快？因为有了h(n)的引导，A*不会像Dijkstra那样「无头苍蝇」一样乱扫。它优先朝着终点方向探索，所以搜索空间小得多。
常用的启发式函数有两种：
- 曼哈顿距离：适合四方向移动（上下左右）
- 欧几里得距离：适合八方向或任意角度移动
我的经验：在Nav2中，默认的全局规划器用的是A*，而且h(n)默认是欧几里得距离。如果你用的是差速轮机器人（可以原地旋转），这个配置没问题。但如果你用的是阿克曼底盘（不能原地转），我建议把h(n)改成曼哈顿距离，路径会更平滑一些。
4.2 算法对比：一张表说清楚
| 特性 | Dijkstra | A* |
|---|---|---|
| 保证最优解 | 是 | 是（当h(n)可采纳时） |
| 搜索速度 | 慢（全图扫描） | 快（有方向引导） |
| 内存占用 | 高 | 低 |
| 适用场景 | 小地图、静态环境 | 大地图、实时性要求高 |
| 启发式函数 | 无 | 需要设计h(n) |
你看，A*在大多数场景下都是更优的选择。但Dijkstra也有它的价值——当你的地图特别小（比如10x10），或者你压根不知道终点在哪时，Dijkstra反而更简单可靠。
4.3 Nav2中的全局规划器配置
好了，理论讲完，咱们来点实际的。Nav2里怎么配全局规划器？
Nav2默认用的是 nav2_navfn_planner 插件，它内部实现的就是A*算法。但如果你想换别的，比如用Dijkstra，或者用更高级的 nav2_smac_planner（支持Hybrid-A*），也是可以的。
4.3.1 默认配置（navfn_planner）
在 planner_params.yaml 里，你通常能看到这样的配置：
planner_server:
ros__parameters:
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_navfn_planner/NavfnPlanner"
tolerance: 0.5
use_astar: true
allow_unknown: false
这里 use_astar: true 就是启用A*。如果你把它改成 false，就会回退到Dijkstra。嗯，就是这么简单。
避坑指南：我曾经把 tolerance 设成0.1米，结果机器人经常因为路径太精确而卡在墙角。后来改成0.5米，路径规划的成功率从70%提升到了95%。记住：全局路径不需要太精细，留点余量给局部规划器去调整。
4.3.2 高级配置（smac_planner）
如果你用的是阿克曼底盘，或者需要更平滑的路径，我建议换成 nav2_smac_planner。它支持Hybrid-A*，能生成符合车辆运动学的路径。
planner_server:
ros__parameters:
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_smac_planner/SmacPlannerHybrid"
tolerance: 0.5
downsample_costmap: false
minimum_turning_radius: 0.4
max_planning_time: 2.0
注意 minimum_turning_radius 这个参数。它决定了机器人能转多急的弯。我见过有人把这个设成0.1米，结果机器人原地打转出不去。对于差速轮，这个值可以设小一点（比如0.2米）；对于阿克曼，得根据实际底盘参数来。
4.4 知识体系总览
为了让你更直观地理解这一章的内容，我画了一张流程图：
从这张图你可以看到，全局路径规划的核心就是选对算法，然后在Nav2里配好参数。我个人建议：
- 新手入门：直接用默认的navfn_planner，把
use_astar设为 true - 阿克曼底盘：换成smac_planner，调好
minimum_turning_radius - 大地图（超过500x500）：考虑用
nav2_smac_planner/SmacPlanner2D，它比navfn更快
最后提醒一句：全局规划不是万能的。它算出来的路径，可能因为地图更新不及时而撞上动态障碍物。所以，一定要配合局部规划器（比如DWA、TEB）一起用。我见过有人只配了全局规划，结果机器人直接撞墙——嗯，那场面挺尴尬的。
好了，这一章就到这里。下一章我们会深入局部路径规划，聊聊怎么让机器人「走得更稳」。