第四章 路径规划基础：从问题定义到图搜索实战

各位同学，欢迎来到路径规划的世界。说实话，我刚开始接触自动驾驶时，觉得路径规划不就是找条路嘛，有什么难的？直到第一次在实车上调试，看着规划出来的路径让车一头扎进绿化带……嗯，那感觉，记忆犹新。

这一章，咱们把路径规划的基础打牢。我会从问题定义讲起，然后深入图搜索的两大经典算法：Dijkstra 和 A*。别怕，代码我会带着你一行行看明白。

4.1 路径规划问题到底在解决什么？

说白了，路径规划就是回答三个问题：

  • 我在哪？ —— 车辆的当前位姿
  • 要去哪？ —— 目标位置
  • 怎么去？ —— 一条无碰撞、可执行的路径

我个人习惯把路径规划拆成两个层次：全局路径规划和局部路径规划。全局规划负责从起点到终点的宏观路线，像地图导航；局部规划则处理动态障碍物、交通规则这些实时变化的东西。这一章我们只聊全局规划，而且是基于图搜索的方法。

核心定义：路径规划 = 在状态空间中，找到一条从初始状态到目标状态的无碰撞路径，同时满足某种最优性准则（如最短距离、最小时间、最低能耗）。

你想想看，如果把环境离散成一个个节点，节点之间有边连接，边上有权重（代表距离或代价），那问题就变成了：在带权图中找最短路径。这就是图搜索算法的用武之地。

4.2 图搜索基础：把世界变成节点和边

我在项目中遇到过一种情况：用栅格地图做规划，每个格子是一个节点，相邻格子之间有边。但如果你直接用8邻域连接，车可能会斜着穿过一个实际上过不去的缝隙。这就是图建模的细节问题。

图搜索的核心要素就三个：

  1. 节点（Node）：状态空间中的离散点，比如栅格地图的每个格子
  2. 边（Edge）：节点之间的连接关系，通常带有代价（cost）
  3. 搜索策略：如何遍历节点，找到最优路径

下面这张图展示了从栅格地图到图模型的转换逻辑：

栅格地图 S G 图模型（节点+边） S 1 2 3 X 4 5 6 G 1 1 1 1 1 1 1 1 1 1 图例 起点 终点 障碍物 可通行边 不可通行

你看，左边的栅格地图里，红色格子是障碍物，绿色S是起点，橙色G是终点。映射到右边的图模型后，障碍物对应的节点被标记为X，不可通行。剩下的节点通过边连接，每条边上的数字代表移动代价（这里假设相邻格子代价为1）。

4.3 Dijkstra算法：稳扎稳打的经典

Dijkstra算法，说白了就是「贪心 + 动态规划」的结合体。它保证能找到从起点到所有节点的最短路径，前提是边的权重非负。

我记得刚入行时，用Dijkstra做路径规划，地图稍微大一点就慢得让人抓狂。后来才明白，它之所以慢，是因为它「一视同仁」——不管目标在哪，它都往所有方向均匀扩展。

4.3.1 算法核心思想

Dijkstra维护一个「未访问节点集」，每次从中选出距离起点最近的节点，然后更新它的邻居。就这么简单。但背后的道理很深刻：一旦某个节点被访问过，它的最短距离就确定了，不会再变。

关键数据结构：

  • dist[]：记录从起点到每个节点的当前最短距离
  • visited[]：标记节点是否已确定最短路径
  • prev[]：记录路径中每个节点的前驱节点，用于回溯路径

4.3.2 代码实现

下面是我在实际项目中用过的Dijkstra实现，加了详细的注释。你注意看优先队列的用法，这是性能的关键。

import heapq

def dijkstra(graph, start, goal):
    """
    graph: 邻接表表示的图，格式 {node: [(neighbor, cost), ...]}
    start: 起点
    goal:  终点
    返回: (最短距离, 路径列表)
    """
    # 初始化
    dist = {node: float('inf') for node in graph}
    dist[start] = 0
    prev = {node: None for node in graph}
    
    # 优先队列：(当前距离, 节点)
    pq = [(0, start)]
    visited = set()
    
    while pq:
        current_dist, current = heapq.heappop(pq)
        
        # 如果当前节点已访问，跳过
        if current in visited:
            continue
        
        visited.add(current)
        
        # 到达终点，提前退出
        if current == goal:
            break
        
        # 遍历邻居
        for neighbor, cost in graph[current]:
            if neighbor in visited:
                continue
            
            new_dist = current_dist + cost
            if new_dist < dist[neighbor]:
                dist[neighbor] = new_dist
                prev[neighbor] = current
                heapq.heappush(pq, (new_dist, neighbor))
    
    # 回溯路径
    path = []
    node = goal
    while node is not None:
        path.append(node)
        node = prev[node]
    path.reverse()
    
    return dist[goal], path

避坑指南：我曾经在项目中直接用列表来维护未访问节点，每次找最小值都遍历一遍。地图有几千个节点时，速度慢到无法接受。后来换成优先队列（堆），性能提升了两个数量级。记住：图搜索的性能瓶颈往往在「找最小值」这一步。

4.4 A*算法：带方向感的搜索

A*算法是Dijkstra的「升级版」。它引入了一个启发式函数，告诉搜索「往哪个方向走更有希望」。说白了，Dijkstra是盲人摸象，A*是带着指南针找路。

4.4.1 启发式函数

A*的核心公式就一个：

f(n) = g(n) + h(n)

  • g(n)：从起点到节点n的实际代价（和Dijkstra一样）
  • h(n)：从节点n到终点的估计代价（启发式函数）
  • f(n)：节点n的总估计代价

启发式函数的选择直接影响A*的性能和最优性。常用的有：

启发式函数 公式 适用场景 特点
曼哈顿距离 |x1-x2| + |y1-y2| 四方向移动（上下左右） 计算快，可采纳
欧几里得距离 √((x1-x2)² + (y1-y2)²) 八方向或任意方向移动 更精确，但计算稍慢
切比雪夫距离 max(|x1-x2|, |y1-y2|) 八方向移动（对角代价相同） 适合网格地图

重要概念：可采纳性（Admissibility）

如果h(n) 永远不会高估到终点的实际代价，那么A*保证能找到最短路径。曼哈顿距离和欧几里得距离都是可采纳的，但如果你用了一个高估的启发式，A*会退化成贪心算法，可能找不到最优解。

4.4.2 A*代码实现

你看，A*的代码和Dijkstra几乎一样，唯一的区别就是优先队列里存的是f(n)而不是g(n)。

import heapq
import math

def heuristic(a, b):
    """欧几里得距离作为启发式函数"""
    return math.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)

def astar(graph, start, goal, positions):
    """
    graph: 邻接表
    start, goal: 节点ID
    positions: 每个节点的坐标 {node: (x, y)}
    """
    dist = {node: float('inf') for node in graph}
    dist[start] = 0
    prev = {node: None for node in graph}
    
    # 优先队列存 (f值, 节点)
    f_start = heuristic(positions[start], positions[goal])
    pq = [(f_start, start)]
    visited = set()
    
    while pq:
        current_f, current = heapq.heappop(pq)
        
        if current in visited:
            continue
        
        visited.add(current)
        
        if current == goal:
            break
        
        for neighbor, cost in graph[current]:
            if neighbor in visited:
                continue
            
            new_g = dist[current] + cost
            if new_g < dist[neighbor]:
                dist[neighbor] = new_g
                prev[neighbor] = current
                # f = g + h
                h = heuristic(positions[neighbor], positions[goal])
                f = new_g + h
                heapq.heappush(pq, (f, neighbor))
    
    # 回溯路径
    path = []
    node = goal
    while node is not None:
        path.append(node)
        node = prev[node]
    path.reverse()
    
    return dist[goal], path

个人经验：我在做园区低速自动驾驶时，发现A*的搜索效率比Dijkstra高出3-5倍。但要注意，如果启发式函数设计得不好（比如h(n)远小于实际代价），A*会退化成Dijkstra，优势就没了。反过来，如果h(n)太大，虽然搜索快，但路径可能不是最优的。这中间的平衡，需要根据实际场景调参。

4.5 Dijkstra vs A*：怎么选？

你可能会问：既然A*更快，那为什么还要学Dijkstra？

原因很简单：Dijkstra保证最优，A*的最优性依赖于启发式函数。在有些场景下，你找不到好的启发式函数，或者环境变化太快，A*的优势就不明显了。

我个人的选择原则是这样的：

  • 地图小（几百个节点以内）：用Dijkstra，简单可靠，不用操心启发式
  • 地图大，且目标明确：用A*，配合曼哈顿或欧几里得距离，效率高
  • 动态环境，需要反复规划：考虑D* Lite或RRT*等更高级的算法，但那是后话了

一句话总结：Dijkstra是「广度优先的加权版」，A*是「带方向感的Dijkstra」。两者都是图搜索的基石，后面的Hybrid A*、D*、RRT*等算法，或多或少都借鉴了它们的思想。

好了，这一章的内容就到这里。路径规划的基础打牢了，后面学更高级的算法才会得心应手。记住，代码一定要自己动手敲一遍，光看是学不会的。我在调试A*时，曾经因为启发式函数写错了符号，导致车在原地打转……嗯，这种坑，踩过一次就记住了。

可可精品资料库

← 上一章 📖 返回目录 下一章 →