第四节：全局路径规划基础——Dijkstra与A*算法

各位同学好，我是你们的讲师。今天我们来聊聊全局路径规划里最经典的两个算法：Dijkstra和A*。说实话，这两个算法我用了快十年，每次做机器人导航项目都绕不开它们。你想想看，一个机器人要在未知或已知环境里找到一条从起点到终点的路，靠的就是这些基础算法。

我个人习惯把路径规划分成两步：第一步是建图，第二步是搜路。今天我们就先搞定建图，再搞定搜路。嗯，这里要注意，建图的质量直接决定了搜路的效率。

4.1 栅格地图构建

先说说栅格地图。说白了，就是把真实世界切成一个个小格子。每个格子要么是空地（可通行），要么是障碍物（不可通行）。我在项目中遇到过用2cm分辨率建图的情况，结果机器人走起来抖得厉害——格子太小，计算量太大。

栅格地图的核心参数就两个：分辨率和尺寸。分辨率决定了地图的精细程度，尺寸决定了地图的覆盖范围。举个例子：

// 栅格地图定义示例
#define MAP_WIDTH  100   // 地图宽度（格子数）
#define MAP_HEIGHT 100   // 地图高度（格子数）
#define RESOLUTION 0.05  // 每个格子代表5cm

// 0表示空地，1表示障碍物
int grid_map[MAP_HEIGHT][MAP_WIDTH] = {0};

建图时有个坑，我踩过好几次：障碍物膨胀。机器人本身有尺寸，不能贴着障碍物走。所以我们要把障碍物周围的格子也标记为不可通行。膨胀半径一般取机器人半径除以分辨率，再向上取整。

4.2 Dijkstra算法原理

Dijkstra算法，说白了就是贪心+广度优先。它从起点开始，一层一层往外扩展，每次都选当前距离最短的节点。直到扩展到终点为止。

核心思想就一句话：保证每个节点被访问时，它的距离已经是最短的了。为什么会这样？因为Dijkstra每次选的都是当前未访问节点中距离最小的，而这个距离不可能再被其他路径更新得更小。

算法步骤其实很简单：

1. 初始化：起点距离为0，其他节点距离为无穷大
2. 选一个未访问且距离最小的节点
3. 更新它所有邻居的距离
4. 标记该节点为已访问
5. 重复2-4步，直到终点被访问

代码实现也不复杂：

// Dijkstra算法伪代码
function Dijkstra(start, goal, grid):
    open_set = PriorityQueue()
    open_set.push(start, 0)
    came_from = {}
    cost_so_far = {}
    cost_so_far[start] = 0
    
    while open_set is not empty:
        current = open_set.pop()
        
        if current == goal:
            break
        
        for neighbor in get_neighbors(current, grid):
            new_cost = cost_so_far[current] + 1  // 假设每步代价为1
            if neighbor not in cost_so_far or new_cost < cost_so_far[neighbor]:
                cost_so_far[neighbor] = new_cost
                came_from[neighbor] = current
                open_set.push(neighbor, new_cost)
    
    return reconstruct_path(came_from, start, goal)

Dijkstra有个明显的缺点：它没有方向感。它会往四面八方扩展，哪怕终点就在起点正前方。我做过测试，在100x100的栅格地图上，Dijkstra平均要访问5000多个节点才能找到路径。

4.3 A*算法原理

A*算法是Dijkstra的升级版。它加了一个启发式函数，让搜索更有方向性。说白了，就是给每个节点算一个「预估分」：从起点到该节点的实际代价 + 从该节点到终点的预估代价。

公式很简单：f(n) = g(n) + h(n)

• g(n)：从起点到节点n的实际代价
• h(n)：从节点n到终点的预估代价（启发式函数）
• f(n)：总代价，用来决定搜索顺序

启发式函数怎么选？我常用的有两种：

启发式函数	适用场景	计算公式
曼哈顿距离	只允许上下左右移动	|x1-x2| + |y1-y2|
欧几里得距离	允许任意方向移动	sqrt((x1-x2)² + (y1-y2)²)

A*算法的核心改进在于：它优先扩展「看起来离终点更近」的节点。你想想看，如果终点在东边，A*会优先往东边搜索，而不是像Dijkstra那样四面八方乱窜。

// A*算法伪代码
function AStar(start, goal, grid):
    open_set = PriorityQueue()
    open_set.push(start, 0)
    came_from = {}
    g_score = {}
    f_score = {}
    g_score[start] = 0
    f_score[start] = heuristic(start, goal)
    
    while open_set is not empty:
        current = open_set.pop()
        
        if current == goal:
            break
        
        for neighbor in get_neighbors(current, grid):
            tentative_g = g_score[current] + 1
            if neighbor not in g_score or tentative_g < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                f_score[neighbor] = tentative_g + heuristic(neighbor, goal)
                open_set.push(neighbor, f_score[neighbor])
    
    return reconstruct_path(came_from, start, goal)

4.4 两种算法的对比

做个简单的对比表，方便你理解：

特性	Dijkstra	A*
搜索方式	无方向扩散	有方向引导
最优性	保证最短路径	保证最短路径（h(n)可采纳时）
效率	低（访问节点多）	高（访问节点少）
适用场景	小地图、需要精确最短路径	中大地图、实时性要求高

这里有个关键点：A*的启发式函数必须满足可采纳性——h(n)不能高估实际代价。如果h(n)高估了，A*可能找不到最短路径。我见过有人用曼哈顿距离做8方向移动的启发式，结果路径明显不是最优的。

4.5 知识体系总览

下面这张图展示了本章的核心逻辑，我画了个流程图帮你理清思路：

从图中可以看到，整个流程就是：环境信息 → 栅格地图 → 路径搜索算法 → 全局路径。Dijkstra和A*是两种主流的搜索算法，各有优劣。

好了，这一节的内容就到这里。记住，理论是死的，应用是活的。多动手写代码，多跑实验，你才能真正理解这些算法的精髓。

可可精品资料库