4. 全局路径规划基础

4.1 为什么我们需要“图”来思考？

做机器人控制这些年，我越来越觉得，路径规划本质上就是个“找路”的问题。你想想看，机器人从A点到B点，中间有障碍物、有地形限制、有运动约束——这不就是我们小时候玩的迷宫游戏吗？

但现实世界比迷宫复杂得多。我们得用一种数学工具来描述这个“世界”。这个工具就是——图（Graph）。

图由两部分组成：节点（Node）和边（Edge）。节点代表位置，边代表连接关系。比如，一个房间的四个角落是节点，门和走廊就是边。每条边还可以带一个权重，代表距离、时间或者能耗。

我个人习惯把图想象成一张“交通网”。节点是路口，边是道路，权重就是路长。路径规划，说白了就是在这张网上找到一条从起点到终点的最优路径。

4.2 图搜索算法家族概览

图搜索算法有很多，但万变不离其宗。我根据自己多年的项目经验，把它们分成两大类：

• 盲目搜索（Uninformed Search）： 不知道目标在哪，只能一步步试探。比如BFS（广度优先）、DFS（深度优先）。
• 启发式搜索（Informed Search）： 知道目标的大致方向，利用“启发函数”来引导搜索。比如A*算法、Dijkstra算法（其实Dijkstra是盲目搜索，但它是A*的基础）。

嗯，这里要注意：Dijkstra算法虽然被归类为盲目搜索，但它利用了“代价”信息，所以比BFS聪明得多。它知道每条路有多长，但不知道目标在哪边。

下面这张图，是我自己画的，能帮你快速理解这些算法的关系：

4.3 Dijkstra算法：最经典的“最短路径”算法

Dijkstra算法，由荷兰计算机科学家Edsger Dijkstra在1956年提出。说实话，这个算法已经快70岁了，但至今仍是很多路径规划系统的核心。为什么？因为它保证能找到最短路径，只要所有边的权重都是非负数。

我在项目中遇到过很多次，明明有更快的A*算法，但最后还是用Dijkstra。为什么？因为有些场景下，我们无法准确估计“启发函数”，A*可能会找到次优解。而Dijkstra虽然慢一点，但结果绝对可靠。

4.3.1 算法核心思想

Dijkstra算法的思路其实很简单，就一句话：每次从“未访问”的节点中，选一个距离起点最近的，然后更新它邻居的距离。

你可以想象成：你手里有一张地图，上面标着每个城市到起点的距离。一开始，除了起点是0，其他都是无穷大。然后你从起点出发，每到一个新城市，就看看能不能通过这个城市，让去其他城市的路更短。如果能，就更新地图上的距离。

这个过程，我称之为“贪心式的松弛”。贪心，是因为每次都选最近的节点；松弛，是因为不断尝试缩短距离。

4.3.2 算法步骤详解

我们一步步来。假设有一个图，节点编号0到N-1，起点是0。

1. 初始化：
   • 创建一个数组 dist[]，dist[0]=0，其他为无穷大。
   • 创建一个数组 visited[]，全部为false。
   • 创建一个优先队列 pq，初始放入 (0, 0) 表示 (距离, 节点)。
2. 循环： 当优先队列不为空时：
   • 从 pq 中取出距离最小的节点 u。
   • 如果 visited[u] 为true，跳过（说明已经处理过）。
   • 标记 visited[u] = true。
   • 遍历 u 的所有邻居 v：
   • 计算新距离 newDist = dist[u] + weight(u, v)。
   • 如果 newDist < dist[v]，更新 dist[v] = newDist，并将 (newDist, v) 放入 pq。
3. 结束： 当所有节点都被访问过，或者优先队列为空时，dist[] 中存储的就是从起点到每个节点的最短距离。

4.3.3 代码实现（Python）

下面是我常用的一个实现。我习惯用 heapq 来实现优先队列，简洁高效。

import heapq

def dijkstra(graph, start):
    """
    graph: 邻接表，graph[u] = [(v, weight), ...]
    start: 起点节点
    返回: dist 数组，dist[i] 表示从 start 到 i 的最短距离
    """
    n = len(graph)
    INF = float('inf')
    dist = [INF] * n
    dist[start] = 0
    visited = [False] * n
    
    # 优先队列，元素为 (距离, 节点)
    pq = [(0, start)]
    
    while pq:
        d, u = heapq.heappop(pq)
        
        # 如果已经访问过，跳过
        if visited[u]:
            continue
        
        visited[u] = True
        
        # 遍历邻居
        for v, w in graph[u]:
            if not visited[v]:
                new_dist = dist[u] + w
                if new_dist < dist[v]:
                    dist[v] = new_dist
                    heapq.heappush(pq, (new_dist, v))
    
    return dist

# 示例用法
if __name__ == "__main__":
    # 一个简单的图：0-1(4), 0-2(2), 1-2(1), 1-3(5), 2-3(8)
    graph = [
        [(1, 4), (2, 2)],   # 节点0
        [(0, 4), (2, 1), (3, 5)],  # 节点1
        [(0, 2), (1, 1), (3, 8)],  # 节点2
        [(1, 5), (2, 8)]    # 节点3
    ]
    
    distances = dijkstra(graph, 0)
    print(f"从节点0到各节点的最短距离: {distances}")
    # 输出: [0, 3, 2, 8]
    # 解释: 0->2->1 是3，0->2 是2，0->2->1->3 是8

4.3.4 算法复杂度分析

这个实现的时间复杂度是 O((V+E)logV)。其中V是节点数，E是边数。为什么？因为每个节点最多入队一次（被visited标记后不再入队），每条边最多被检查一次。每次入队和出队操作都是O(logV)。

空间复杂度是 O(V+E)，主要存储邻接表和dist数组。

4.4 从Dijkstra到实际应用

你可能会问：Dijkstra算法在机器人上怎么用？我举个例子。

几年前，我做AGV（自动导引车）的调度系统。工厂地面画了很多二维码，每个二维码就是一个节点。AGV从一个二维码移动到另一个二维码，需要时间。这个时间就是边的权重。

我们直接用Dijkstra计算最短时间路径。但有个问题：地图很大，有上千个节点。每次重新规划路径，Dijkstra都要跑一遍，太慢了。

后来我们做了优化：分层Dijkstra。先把地图分成几个区域，区域内部用Dijkstra，区域之间用另一层Dijkstra。这样，每次规划只需要在局部区域跑Dijkstra，速度提升了好几倍。

4.5 本章小结

这一章，我们聊了图搜索算法的基础，重点讲了Dijkstra算法。说白了，Dijkstra就是“每次选最近的，然后更新邻居”。它简单、可靠，是很多高级路径规划算法的基石。

我个人建议，如果你刚开始学路径规划，先把Dijkstra吃透。把代码写熟，把原理搞懂。后面学A*、D*、RRT等算法时，你会发现它们都有Dijkstra的影子。

嗯，最后提醒一句：永远不要在生产环境中用没有经过测试的路径规划算法。我曾经因为一个边界条件没处理好，导致AGV在工厂里原地打转，被老板骂了一下午。从那以后，我写路径规划代码，一定会先写单元测试，把各种边界情况都覆盖到。

可可精品资料库