2、全局路径规划基础：Dijkstra算法原理与实现、A*算法核心思想、启发式函数的设计与调优、栅格地图与拓扑地图的构建

全局路径规划，说白了就是给机器人找一条从起点到终点的路。你想想看，如果连地图都没有，机器人就是个瞎子。所以这一章，我们先打好地基。

我个人习惯把全局路径规划比作「导航软件里的路线规划」。你输入目的地，它给你算出一条路。但机器人比人麻烦——它得自己建地图，自己算路径，还得实时避障。嗯，这里我们先聊静态地图下的规划。

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

Dijkstra算法，我当年在数据结构课上第一次接触。当时觉得这玩意儿好笨——从起点开始，一圈一圈往外扩，直到碰到终点。但后来做项目才发现，笨有笨的好处：它保证找到最短路径。

核心思想其实很简单：

• 维护一个「未访问节点」的集合
• 每次从集合中挑出距离起点最近的节点
• 更新它邻居的距离（松弛操作）
• 重复直到终点被访问

我在项目中遇到过一个问题：如果地图很大（比如1000x1000的栅格），Dijkstra会慢得让人抓狂。因为它要遍历几乎所有的节点。但如果你只需要保证最优解，它依然是首选。

来看一个简单的Python实现：

import heapq

def dijkstra(graph, start, goal):
    # graph: 邻接表 {node: [(neighbor, cost), ...]}
    # 返回最短路径和距离
    
    pq = [(0, start)]  # (累计代价, 节点)
    visited = set()
    dist = {start: 0}
    parent = {start: None}
    
    while pq:
        cost, node = heapq.heappop(pq)
        
        if node in visited:
            continue
        visited.add(node)
        
        if node == goal:
            # 回溯路径
            path = []
            while node is not None:
                path.append(node)
                node = parent[node]
            return path[::-1], cost
        
        for neighbor, edge_cost in graph.get(node, []):
            new_cost = cost + edge_cost
            if neighbor not in dist or new_cost < dist[neighbor]:
                dist[neighbor] = new_cost
                parent[neighbor] = node
                heapq.heappush(pq, (new_cost, neighbor))
    
    return None, float('inf')  # 无路径

2.2 A*算法：给搜索装上「导航仪」

A*算法，说白了就是Dijkstra的升级版。它加了一个启发式函数，告诉搜索「往终点方向走更有可能找到路」。我刚开始学的时候觉得这玩意儿很玄乎，后来自己实现了一遍才发现，其实就是多算了一个估计值。

核心公式就一个：

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

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

你想想看，Dijkstra只用了g(n)，所以它像无头苍蝇一样乱撞。A*用了g(n)+h(n)，就像有了指南针，直奔终点方向。但这里有个坑：如果h(n)估计得太离谱，A*可能找不到最优解。

A*的代码和Dijkstra几乎一样，只是多了启发式函数：

import heapq

def heuristic(a, b):
    # 曼哈顿距离（适用于四方向移动）
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def astar(grid, start, goal):
    # grid: 0=可通行, 1=障碍物
    rows, cols = len(grid), len(grid[0])
    pq = [(0 + heuristic(start, goal), 0, start)]  # (f, g, node)
    visited = set()
    g_cost = {start: 0}
    parent = {start: None}
    
    while pq:
        f, g, node = heapq.heappop(pq)
        
        if node in visited:
            continue
        visited.add(node)
        
        if node == goal:
            path = []
            while node is not None:
                path.append(node)
                node = parent[node]
            return path[::-1], g
        
        for dx, dy in [(0,1), (0,-1), (1,0), (-1,0)]:
            nx, ny = node[0] + dx, node[1] + dy
            if 0 <= nx < rows and 0 <= ny < cols and grid[nx][ny] == 0:
                neighbor = (nx, ny)
                new_g = g + 1  # 假设每步代价为1
                if neighbor not in g_cost or new_g < g_cost[neighbor]:
                    g_cost[neighbor] = new_g
                    f = new_g + heuristic(neighbor, goal)
                    parent[neighbor] = node
                    heapq.heappush(pq, (f, new_g, neighbor))
    
    return None, float('inf')

2.3 启发式函数的设计与调优

启发式函数是A*的灵魂。调得好，搜索飞快；调得差，比Dijkstra还慢。我曾经在一个仓储机器人项目里，就因为启发式函数没调好，导致机器人绕了远路，差点撞到货架。

常用的启发式函数有三种：

类型	公式	适用场景	特点
曼哈顿距离	|x1-x2| + |y1-y2|	四方向移动（上下左右）	简单、可采纳、常用
欧几里得距离	√((x1-x2)² + (y1-y2)²)	八方向或自由移动	可采纳、但计算稍慢
对角距离	max(|dx|, |dy|) + (√2-1)*min(|dx|, |dy|)	八方向移动（允许斜走）	最精确、可采纳

举个例子，如果你想让机器人更快找到路径（不要求最优），可以把f(n)改成：

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

w=1时是标准A*，w>1时是「加权A*」。我做过实验，w=1.5时搜索节点数能减少40%，路径长度只增加5%。对于大多数机器人应用，这个trade-off是值得的。

2.4 栅格地图与拓扑地图的构建

地图是路径规划的基础。没有地图，算法再牛也没用。我见过不少新手，一上来就调A*参数，结果发现地图建错了——障碍物标反了，或者分辨率不对。嗯，这里我们先搞清楚两种主流地图。

栅格地图（Grid Map）

说白了就是把环境切成一个个小格子。每个格子要么是「可通行」（0），要么是「障碍物」（1）。ROS里常用的costmap就是栅格地图的一种。

• 优点：直观、容易实现、适合激光雷达数据
• 缺点：格子多了内存爆炸（比如1000x1000就有100万个格子）
• 分辨率选择：室内一般0.05m/格，室外可以0.1m/格

构建栅格地图时，我习惯用ROS的nav_msgs/OccupancyGrid消息。它用0-100表示占用概率，0是空闲，100是障碍物，-1是未知。这样比简单的0/1更灵活。

拓扑地图（Topological Map）

拓扑地图不关心具体坐标，只关心「节点」和「边」。节点代表关键位置（比如房间门口、走廊拐角），边代表连通关系。说白了就是一张「关系图」。

• 优点：内存小、搜索快、适合大范围导航
• 缺点：构建麻烦、丢失细节信息
• 典型应用：扫地机器人按房间清扫、仓库机器人按货架导航

我在项目中常用的是「混合地图」：先用栅格地图做局部避障，再用拓扑地图做全局路径规划。这样既保证了精度，又提高了效率。

好了，这一章的内容就到这里。Dijkstra和A*是全局路径规划的基石，启发式函数是调参的关键，地图构建是前提。把这些搞懂了，后面的动态避障和局部规划才能玩得转。