4. 全局路径规划：Dijkstra算法、A*算法原理与实现、栅格地图构建

说到全局路径规划，我脑子里第一个蹦出来的场景，就是几年前做仓储机器人项目时的那个下午。机器人明明知道目标在哪，却总在货架中间绕远路。说白了，全局路径规划就是给机器人画一张“大地图”，然后告诉它：从A到B，哪条路最划算。

这一章，我们重点啃两个经典算法——Dijkstra和A*。它们就像路径规划界的“倚天剑”和“屠龙刀”。我会结合栅格地图，把原理、实现、还有我踩过的坑，一次性讲清楚。

4.1 栅格地图：让机器人看懂世界

机器人不是人，它看不懂“前面第三个路口左转”。它需要一种结构化的表达方式。栅格地图，就是最直观的一种。

简单说，我们把环境切成一个个小格子。每个格子要么是“空地”（可通行），要么是“障碍物”（不可通行）。就像围棋棋盘，黑子是障碍，白子是路。

我习惯用Python的numpy来构建栅格地图。举个例子：

import numpy as np

# 创建一个10x10的栅格地图，全部初始化为0（空闲）
grid_map = np.zeros((10, 10), dtype=int)

# 手动添加一些障碍物（1表示障碍）
grid_map[2, 3:7] = 1   # 第3行，第4到7列是墙
grid_map[5, 1:9] = 1   # 第6行，第2到9列是墙
grid_map[7, 4] = 1     # 一个孤立的障碍物

print("栅格地图：")
print(grid_map)

嗯，这里要注意：栅格的分辨率是个双刃剑。分辨率太高，计算量爆炸；分辨率太低，机器人可能钻不过窄通道。我在项目中一般取0.05米到0.1米，具体看机器人尺寸。

4.2 Dijkstra算法：稳扎稳打的“地毯式搜索”

Dijkstra算法，说白了就是“从起点开始，一层一层往外扩”。它保证找到的路径是全局最短的，但代价是——慢。

你想想看，它就像个强迫症患者，必须把所有可能的路都走一遍，才敢告诉你哪条最近。

4.2.1 算法原理

核心思想就一句话：维护一个“未访问节点”的集合，每次从中选出距离起点最近的那个，然后更新它邻居的距离。

具体步骤：

1. 初始化：起点距离为0，其他节点距离为无穷大。
2. 循环：从未访问节点中，选出距离最小的节点u。
3. 更新：对于u的所有邻居v，如果通过u到v的距离更短，就更新v的距离。
4. 标记u为已访问。
5. 重复，直到所有节点都被访问，或者到达目标点。

4.2.2 代码实现

我直接给你一个在栅格地图上跑Dijkstra的简化版代码。核心逻辑都在里面了。

import heapq
import numpy as np

def dijkstra(grid, start, goal):
    """
    grid: 二维栅格地图，0可通行，1障碍
    start: 起点坐标 (x, y)
    goal: 终点坐标 (x, y)
    返回: 路径列表，从起点到终点
    """
    rows, cols = grid.shape
    # 距离数组，初始化为无穷大
    dist = np.full((rows, cols), np.inf)
    dist[start] = 0
    
    # 前驱节点，用于回溯路径
    parent = {}
    
    # 优先队列：(距离, 坐标)
    pq = [(0, start)]
    
    # 四个方向移动：上、下、左、右
    directions = [(-1,0), (1,0), (0,-1), (0,1)]
    
    while pq:
        current_dist, current = heapq.heappop(pq)
        
        # 如果当前距离大于记录的距离，跳过（过时数据）
        if current_dist > dist[current]:
            continue
        
        # 到达目标，结束
        if current == goal:
            break
        
        # 遍历四个邻居
        for dx, dy in directions:
            nx, ny = current[0] + dx, current[1] + dy
            
            # 边界检查
            if 0 <= nx < rows and 0 <= ny < cols:
                # 障碍物检查
                if grid[nx, ny] == 1:
                    continue
                
                # 新距离 = 当前距离 + 1（每一步代价为1）
                new_dist = current_dist + 1
                
                # 如果新距离更短，更新
                if new_dist < dist[nx, ny]:
                    dist[nx, ny] = new_dist
                    parent[(nx, ny)] = current
                    heapq.heappush(pq, (new_dist, (nx, ny)))
    
    # 回溯路径
    path = []
    if goal in parent or goal == start:
        node = goal
        while node != start:
            path.append(node)
            node = parent[node]
        path.append(start)
        path.reverse()
    
    return path

# 使用示例
grid = np.zeros((5, 5), dtype=int)
grid[1, 1:4] = 1  # 障碍物
start = (0, 0)
goal = (4, 4)
path = dijkstra(grid, start, goal)
print("Dijkstra找到的路径:", path)

这段代码我用了heapq，也就是优先队列。每次取最小距离节点的时间复杂度是O(log n)，比暴力遍历快得多。

4.3 A*算法：带“指南针”的搜索

Dijkstra太老实了，它不管目标在哪，都一圈一圈往外扩。A*就不一样了，它有个“指南针”——启发式函数。它会朝着目标方向优先搜索，所以通常比Dijkstra快得多。

我个人觉得，A*是工程中最实用的全局路径规划算法。没有之一。

4.3.1 算法原理

A*的核心公式就一个：

说白了，g(n)是“已经走过的路”，h(n)是“预估还要走的路”。两者相加，就是总代价的估计。

启发式函数h(n)的选择很关键。最常用的是曼哈顿距离和欧几里得距离。

4.3.2 代码实现

A*的代码和Dijkstra很像，区别就在于多了个启发式函数。你看：

import heapq
import numpy as np

def heuristic(a, b):
    """曼哈顿距离作为启发式函数"""
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def a_star(grid, start, goal):
    rows, cols = grid.shape
    
    # g(n) 和 f(n) 数组
    g_score = np.full((rows, cols), np.inf)
    g_score[start] = 0
    f_score = np.full((rows, cols), np.inf)
    f_score[start] = heuristic(start, goal)
    
    parent = {}
    
    # 优先队列：(f(n), 坐标)
    pq = [(f_score[start], start)]
    
    directions = [(-1,0), (1,0), (0,-1), (0,1)]
    
    while pq:
        _, current = heapq.heappop(pq)
        
        if current == goal:
            break
        
        for dx, dy in directions:
            nx, ny = current[0] + dx, current[1] + dy
            
            if 0 <= nx < rows and 0 <= ny < cols and grid[nx, ny] == 0:
                tentative_g = g_score[current] + 1
                
                if tentative_g < g_score[nx, ny]:
                    parent[(nx, ny)] = current
                    g_score[nx, ny] = tentative_g
                    f_score[nx, ny] = tentative_g + heuristic((nx, ny), goal)
                    heapq.heappush(pq, (f_score[nx, ny], (nx, ny)))
    
    # 回溯路径
    path = []
    if goal in parent or goal == start:
        node = goal
        while node != start:
            path.append(node)
            node = parent[node]
        path.append(start)
        path.reverse()
    
    return path

# 使用示例
grid = np.zeros((5, 5), dtype=int)
grid[1, 1:4] = 1
start = (0, 0)
goal = (4, 4)
path = a_star(grid, start, goal)
print("A*找到的路径:", path)

你看，A*和Dijkstra的代码结构几乎一样。唯一的区别就是：Dijkstra用dist（即g(n)）作为优先级，而A*用f(n) = g(n) + h(n)作为优先级。

4.4 Dijkstra vs A*：到底选哪个？

很多新手会问：既然A*更快，为什么还要学Dijkstra？

我的回答是：没有银弹。两种算法各有适用场景。

我个人的经验是：如果地图小于100x100，用Dijkstra也无妨。如果地图更大，或者需要实时规划，果断上A*。

4.5 知识体系总览

为了让你更直观地理解这一章的知识结构，我画了一张流程图。它展示了从栅格地图到路径规划的核心逻辑。

这张图把本章的核心逻辑串起来了。从栅格地图出发，根据实际需求选择Dijkstra或A*，最终输出一条最优路径。

4.6 一些掏心窝的话

最后，我想说几句实在的。

算法是死的，场景是活的。我在项目中见过太多人死磕A*的启发式函数，结果忽略了地图本身的问题。比如，栅格地图的障碍物膨胀没做好，导致路径绕远路。这时候你优化算法代码，不如回头检查一下地图。

另外，不要迷信“最优路径”。有时候，次优路径但更平滑、更安全，反而更好。我习惯在A*找到路径后，加一个路径平滑的后处理步骤。这个我们后面会讲到。

好了，这一章就到这里。代码都给你了，建议你亲手跑一跑，改一改参数，看看效果。纸上得来终觉浅，绝知此事要躬行。