4. 全局路径规划-Dijkstra算法：图搜索基础、Dijkstra算法原理与实现、在ROS中调用global_planner
各位同学，欢迎来到第四章。从这一章开始，我们正式进入路径规划的核心地带。
全局路径规划，说白了就是回答一个问题：“在已知地图中，怎么从A点走到B点？” 你想想看，机器人不像人，它没有直觉。它需要一套严格的数学方法，在地图上找出一条最优路径。而Dijkstra算法，就是最经典、最基础的那把钥匙。
我个人习惯把路径规划比作“找路”。你手里有张地图（栅格地图或拓扑地图），地图上每个格子或节点都有邻居。Dijkstra算法就像个严谨的探险家，一步步向外探索，直到找到终点。它保证找到的路径是最短路径——前提是地图信息是静态的。
核心要点：Dijkstra算法是广度优先搜索（BFS）的加权版本。BFS只考虑步数，Dijkstra考虑每条边的代价（距离、时间、能耗等）。
4.1 图搜索基础
在讲算法之前，我们先聊聊“图”。图是什么？图就是由节点（Node）和边（Edge）组成的结构。在机器人导航里，节点可以是地图上的一个栅格，也可以是一个路标点；边就是连接两个节点的路径，边上通常带有一个权重（代价）。
我刚开始做SLAM时，对图的理解很浅。直到有一次在实验室里跑导航，机器人死活绕远路。后来发现，是我把地图的连通性搞错了——两个节点之间明明有墙，我却没设置障碍边。嗯，从那以后，我每次建图都会手动检查一遍邻接关系。
图搜索的核心任务就是：在图中找到一条从起点到终点的路径，使得路径上的总代价最小。 常见的图搜索算法有：
- BFS（广度优先搜索）：无权图，保证找到最短路径（按步数）。
- DFS（深度优先搜索）：不保证最短，但内存占用小。
- Dijkstra：加权图，保证找到最小代价路径。
- A*：Dijkstra的启发式改进版，更快。
我们这章重点讲Dijkstra。为什么先讲它？因为它是A*算法的基础。你搞懂了Dijkstra，A*就是加个启发函数的事。
4.2 Dijkstra算法原理
Dijkstra算法的思想其实很朴素：从起点开始，不断选择当前已知代价最小的节点进行扩展，直到扩展到终点。 它维护一个“代价表”，记录从起点到每个节点的当前最短距离。每次从代价表中取出最小值对应的节点，然后更新它的邻居。
具体步骤我总结成五步：
- 初始化：起点代价设为0，其他节点代价设为无穷大。所有节点标记为“未访问”。
- 选择当前节点：从未访问的节点中，选出代价最小的那个。
- 扩展邻居：遍历当前节点的所有邻居。如果通过当前节点到达邻居的代价更小，就更新邻居的代价，并记录前驱节点。
- 标记已访问：当前节点标记为“已访问”，不再处理。
- 重复：重复步骤2-4，直到终点被标记为“已访问”或所有节点都被访问。
为什么会这样？因为Dijkstra利用了贪心策略：每次选代价最小的节点，这个节点的最短路径就已经确定了。你想想看，如果还有更短的路径能到这个节点，那它早就被选出来了。
个人经验：我在实际项目中，经常把Dijkstra的“代价”定义为欧氏距离加上一个惩罚项。比如，靠近障碍物的边，代价乘以1.5。这样机器人会主动远离墙壁，走得更安全。
4.3 Dijkstra算法实现
光说不练假把式。我们直接上代码。下面是一个用Python实现的Dijkstra算法，适用于栅格地图。代码我尽量精简，方便你理解核心逻辑。
import heapq
import numpy as np
def dijkstra(grid, start, goal):
"""
grid: 2D numpy array, 0=可通行, 1=障碍物
start: (x, y) 起点坐标
goal: (x, y) 终点坐标
返回: 路径列表 [(x1,y1), (x2,y2), ...] 或 None
"""
rows, cols = grid.shape
# 代价表，初始化为无穷大
cost = np.full((rows, cols), np.inf)
cost[start] = 0
# 前驱节点表，用于回溯路径
parent = {}
# 优先队列 (代价, 节点)
pq = [(0, start)]
# 方向：上下左右
dirs = [(-1,0), (1,0), (0,-1), (0,1)]
while pq:
current_cost, current = heapq.heappop(pq)
# 如果当前代价大于记录代价，跳过（旧数据）
if current_cost > cost[current]:
continue
# 到达终点
if current == goal:
break
# 扩展邻居
for dx, dy in dirs:
nx, ny = current[0] + dx, current[1] + dy
if 0 <= nx < rows and 0 <= ny < cols and grid[nx, ny] == 0:
new_cost = current_cost + 1 # 假设每步代价为1
if new_cost < cost[(nx, ny)]:
cost[(nx, ny)] = new_cost
parent[(nx, ny)] = current
heapq.heappush(pq, (new_cost, (nx, ny)))
# 回溯路径
if goal not in parent and start != goal:
return None
path = []
node = goal
while node != start:
path.append(node)
node = parent[node]
path.append(start)
path.reverse()
return path
这段代码有几个关键点：
- 用
heapq实现优先队列，保证每次取出的都是最小代价节点。 - 用
parent字典记录路径，方便回溯。 - 障碍物直接跳过，不扩展。
注意：Dijkstra算法要求所有边的代价为非负数。如果有负权边，算法会失效。不过在我们机器人导航场景中，距离、时间都是非负的，所以放心用。
我曾经在一个项目中，用Dijkstra做全局规划，地图是1000x1000的栅格。结果发现算法跑得很慢，要好几秒。后来一查，是优先队列实现有问题——我用了列表而不是堆，每次取最小值都要遍历。改成heapq后，速度提升了10倍。所以，数据结构选对了，事半功倍。
4.4 在ROS中调用global_planner
ROS里已经帮我们封装好了全局路径规划器。你不需要自己写Dijkstra，直接调用global_planner包就行。这个包默认使用Dijkstra算法，当然你也可以改成A*。
配置步骤如下：
- 安装包：
sudo apt install ros-noetic-global-planner（以Noetic为例）。 - 修改move_base参数：在launch文件中，指定
base_global_planner为global_planner/GlobalPlanner。 - 调整参数：通过
~GlobalPlanner命名空间设置参数，比如use_dijkstra、old_navfn_behavior等。
下面是一个典型的launch文件片段：
<launch>
<node pkg="move_base" type="move_base" name="move_base" output="screen">
<param name="base_global_planner" value="global_planner/GlobalPlanner"/>
<rosparam file="$(find my_robot)/config/global_planner_params.yaml" command="load"/>
</node>
</launch>
对应的YAML参数文件：
GlobalPlanner:
use_dijkstra: true # 使用Dijkstra算法
old_navfn_behavior: false # 使用新的规划器行为
visualize_potential: true # 可视化势场
allow_unknown: false # 不允许未知区域
planner_window_x: 0.0 # 规划窗口x范围，0表示全局
planner_window_y: 0.0 # 规划窗口y范围
我个人建议把visualize_potential设为true。这样你可以在Rviz里看到代价势场，调试起来非常直观。有一次，我发现机器人规划的路径总是贴着障碍物，一看势场图，原来是膨胀半径设小了。调大之后，路径就变得安全了。
避坑指南：我曾经在ROS里直接用了默认的navfn规划器，结果发现它也是Dijkstra，但实现上有些差异。如果你需要更稳定的表现，建议显式指定global_planner/GlobalPlanner，并设置use_dijkstra=true。
4.5 知识体系总览
为了让你对本章内容有个整体把握，我画了一张图。它展示了从图搜索基础到Dijkstra算法，再到ROS调用的完整链路。
这张图把本章的四个核心模块串起来了。你从图搜索基础出发，理解Dijkstra原理，然后动手写代码，最后在ROS里落地。每一步都是下一步的前提。
4.6 小结与思考
Dijkstra算法虽然经典，但也不是万能的。它有几个明显的局限：
- 计算量大：在大型地图上，它需要探索大量节点，速度慢。
- 无启发式：它不知道终点在哪，只能盲目向外扩展。
- 静态环境：如果地图动态变化，Dijkstra需要重新规划。
不过，作为基础算法，它依然是理解更高级算法的基石。你想想看，A*就是在Dijkstra的基础上加了一个启发函数，让搜索更有方向性。所以，把Dijkstra吃透，后面的路就好走了。
一句话总结：Dijkstra算法是全局路径规划的“Hello World”。它简单、可靠、保证最优，是每个机器人工程师必须掌握的技能。