4. 经典全局路径规划：Dijkstra算法原理与实现，A*算法原理与启发式函数设计

说到全局路径规划，Dijkstra和A*是绕不开的两个经典算法。我个人习惯把它们比作「地图导航的鼻祖」——一个稳扎稳打，一个聪明高效。今天我们就来拆解这两个算法，看看它们到底是怎么工作的，以及在实际项目中该怎么用。

4.1 Dijkstra算法：最朴素的「地毯式搜索」

Dijkstra算法的核心思想其实很简单：从起点出发，一层层向外扩散，直到找到终点。它保证找到的路径是全局最短的，但代价是搜索范围很大。

算法原理

• 维护一个距离表，记录从起点到每个节点的最短距离
• 每次从未访问的节点中，选距离最小的那个进行扩展
• 更新邻居节点的距离，如果发现更短路径就替换
• 重复直到所有节点都被访问，或者到达终点

我在项目中遇到过一个问题：用Dijkstra在100x100的栅格地图上做路径规划，结果跑了快2秒才出结果。你想想看，机器人要是等2秒才动，黄花菜都凉了。所以Dijkstra更适合静态环境、小规模地图，或者作为其他算法的基准对比。

代码实现（Python伪代码）

import heapq

def dijkstra(graph, start, goal):
    # graph: 邻接表，格式 {node: [(neighbor, cost), ...]}
    distances = {node: float('inf') for node in graph}
    distances[start] = 0
    pq = [(0, start)]  # (累计代价, 当前节点)
    came_from = {start: None}
    
    while pq:
        current_dist, current = heapq.heappop(pq)
        
        if current == goal:
            break
            
        if current_dist > distances[current]:
            continue  # 跳过过时的记录
            
        for neighbor, weight in graph[current]:
            new_dist = current_dist + weight
            if new_dist < distances[neighbor]:
                distances[neighbor] = new_dist
                came_from[neighbor] = current
                heapq.heappush(pq, (new_dist, neighbor))
    
    # 回溯路径
    path = []
    node = goal
    while node is not None:
        path.append(node)
        node = came_from[node]
    return path[::-1]

4.2 A*算法：带「方向感」的搜索

A*算法在Dijkstra的基础上加了一个启发式函数，说白了就是给搜索过程装了个「指南针」。它不再盲目地向四周扩散，而是优先朝着目标方向探索。

核心公式

A*的代价函数是：f(n) = g(n) + h(n)

• g(n)：从起点到当前节点n的实际代价
• h(n)：从当前节点n到终点的估计代价（启发式函数）
• f(n)：总估计代价，优先队列按f值排序

启发式函数设计

启发式函数的设计直接影响A*的性能和正确性。我个人常用的几种：

启发式函数	适用场景	特点
曼哈顿距离	四方向移动（上下左右）	计算快，但可能高估代价
欧几里得距离	八方向或自由移动	更准确，但计算稍慢
对角线距离	八方向移动（含对角线）	兼顾准确性和效率

A*算法实现

import heapq
import math

def heuristic(a, b):
    # 使用欧几里得距离
    return math.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2)

def a_star(grid, start, goal):
    # grid: 二维栅格，0可通行，1障碍物
    rows, cols = len(grid), len(grid[0])
    open_set = [(0, start)]  # (f值, 节点)
    g_score = {start: 0}
    f_score = {start: heuristic(start, goal)}
    came_from = {start: None}
    
    while open_set:
        _, current = heapq.heappop(open_set)
        
        if current == goal:
            path = []
            while current is not None:
                path.append(current)
                current = came_from[current]
            return path[::-1]
        
        # 四方向邻居
        for dx, dy in [(0,1), (0,-1), (1,0), (-1,0)]:
            neighbor = (current[0]+dx, current[1]+dy)
            
            if not (0 <= neighbor[0] < rows and 0 <= neighbor[1] < cols):
                continue
            if grid[neighbor[0]][neighbor[1]] == 1:
                continue  # 障碍物
                
            tentative_g = g_score[current] + 1  # 假设每步代价为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)
                heapq.heappush(open_set, (f_score[neighbor], neighbor))
    
    return None  # 无路径

4.3 Dijkstra vs A*：怎么选？

说实话，这两个算法没有绝对的优劣，关键看场景。我总结了一个简单的选择指南：

• 地图小（< 50x50）：用Dijkstra，实现简单，结果可靠
• 地图大（> 100x100）：用A*，启发式函数能大幅减少搜索节点
• 动态环境：两个都不太适合，考虑D* Lite或RRT*
• 需要精确最短路径：Dijkstra更稳妥，A*的启发式可能引入误差

4.4 知识体系总览

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

好了，Dijkstra和A*的核心内容就这些。记住：算法是死的，场景是活的。多在实际项目中调试，你自然能找到最适合的那一个。