第3章 全局路径规划基础：全局规划器的作用、规划流程、代价地图概念、A*算法原理

大家好，欢迎来到第三章。今天我们来聊聊全局路径规划的基础内容。说实话，这部分是导航系统的「大脑」——没有它，机器人就是个无头苍蝇。我在做第一个机器人项目时，就吃过没搞懂全局规划的亏，后来花了整整一周才把问题定位清楚。嗯，咱们今天就把这块彻底讲透。

3.1 全局规划器到底在干什么？

全局规划器，说白了就是给机器人画一条从起点到终点的「大路线」。它不关心路上的小障碍物怎么躲，只关心大方向对不对。

我个人习惯把全局规划器比作「地图导航」——你输入目的地，它给你一条推荐路线。至于路上哪个井盖没盖好、哪个路口在施工，那是局部规划器的事。

具体来说，全局规划器要干三件事：

• 接收目标点——用户或上层任务层告诉它「去这里」
• 查询代价地图——看看哪些地方能走，哪些不能走
• 执行搜索算法——在可通行区域里找最优路径

你可能会问：「那局部规划器不也能规划路径吗？」没错，但两者分工不同。全局规划器负责「宏观」，局部规划器负责「微观」。全局规划器给出的路径可能很粗糙，但方向是对的；局部规划器则在这个大方向下，实时避障、平滑轨迹。

3.2 规划流程：从起点到终点的完整链路

我在Nav2里调试过无数次规划流程，总结下来就是下面这张图。你先看看，我再细讲。

流程其实不复杂，我拆开来说：

1. 接收目标点——用户通过RViz点击或API调用，告诉机器人「去那里」。
2. 查询代价地图——全局规划器会拉取一份代价地图。注意，这里用的是全局代价地图，不是局部的那份。
3. 执行A*搜索——在代价地图上跑A*算法，找到一条路径。
4. 路径平滑处理——原始路径可能有很多折角，需要平滑一下。我习惯用插值或样条曲线处理。
5. 发布导航路径——把最终路径发布到/plan话题上，局部规划器会订阅它。
6. 路径有效性检查——如果路径被阻塞（比如新出现障碍物），全局规划器会重新规划。

3.3 代价地图概念：机器人眼中的「世界」

代价地图，就是机器人对环境的「理解」。它把真实世界映射成一个网格，每个格子都有一个代价值。代价值越高，说明这个格子越「危险」或「不可通行」。

代价地图通常分三层：

层级	来源	代价值范围	说明
静态层	SLAM建图结果	0（自由）~ 100（障碍物）	墙壁、家具等固定物体
膨胀层	根据机器人半径计算	0 ~ 255	防止机器人撞到障碍物
障碍物层	激光雷达/深度相机实时数据	0 ~ 255	动态障碍物（行人、移动物体）

你想想看，如果没有膨胀层，机器人规划出来的路径可能紧贴着墙壁走。但机器人有体积啊！一转弯就撞上了。所以膨胀层就是给障碍物「加一圈安全距离」。

3.4 A*算法原理：全局规划的核心引擎

A*算法，说白了就是「有脑子的广度优先搜索」。它不像Dijkstra那样盲目地往四周扩散，而是用启发函数引导搜索方向。

核心公式就一个：

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

• g(n)：从起点到当前节点n的实际代价
• h(n)：从当前节点n到终点的估计代价（启发函数）
• f(n)：总代价，A*每次选f(n)最小的节点扩展

启发函数h(n)怎么选？我常用的有两种：

启发函数	公式	适用场景
曼哈顿距离	|x1-x2| + |y1-y2|	四方向移动（上下左右）
欧几里得距离	√((x1-x2)² + (y1-y2)²)	八方向移动或自由移动

我个人习惯用欧几里得距离，因为机器人可以任意方向移动，曼哈顿距离会低估实际路径长度，导致搜索效率下降。

A*的伪代码其实不复杂，我写个简化版：

function AStar(start, goal, costmap):
    openSet = {start}
    cameFrom = {}
    gScore[start] = 0
    fScore[start] = h(start, goal)
    
    while openSet is not empty:
        current = node in openSet with lowest fScore
        
        if current == goal:
            return reconstructPath(cameFrom, current)
        
        openSet.remove(current)
        for neighbor in getNeighbors(current, costmap):
            tentative_g = gScore[current] + cost(current, neighbor)
            if tentative_g < gScore[neighbor]:
                cameFrom[neighbor] = current
                gScore[neighbor] = tentative_g
                fScore[neighbor] = gScore[neighbor] + h(neighbor, goal)
                if neighbor not in openSet:
                    openSet.add(neighbor)
    
    return failure  # 无路径可达

这段代码的核心逻辑就两点：

• openSet：待探索的节点集合，每次选f值最小的
• gScore：记录从起点到每个节点的最短距离

为什么A*比Dijkstra快？因为Dijkstra没有方向感，它会往所有方向均匀扩散。而A*有启发函数「指路」，它会优先往目标方向搜索。你可以想象成：Dijkstra是「地毯式搜索」，A*是「定向搜索」。

3.5 实战中的避坑指南

讲了这么多理论，最后分享几个我在项目中踩过的坑：

• 代价地图分辨率太粗——我用过5cm/像素的地图，结果机器人钻不过去的地方，地图上显示能走。建议室内场景用2-3cm/像素。
• A*搜索超时——地图太大时，A*可能跑几秒都出不来结果。我习惯设置一个超时阈值（比如500ms），超时了就降级用Dijkstra。
• 路径抖动——A*找出来的路径可能有锯齿状，尤其是网格地图。一定要做平滑处理，否则机器人走起来一卡一卡的。
• 启发函数选错——如果机器人只能四方向移动，你却用欧几里得距离，A*会多搜很多节点。反过来，如果机器人能八方向移动，你用曼哈顿距离，效率会低。

嗯，今天就先讲到这里。全局路径规划是导航系统的基石，搞懂了它，后面学局部规划、行为树都会轻松很多。记住一句话：全局规划定方向，局部规划保安全。