代价地图与全局/局部规划：一张会呼吸的城市活地图

可视化演示

展示栅格代价地图分层结构、A*全局路径搜索过程、以及DWA局部规划器实时避障轨迹，直观呈现Nav2导航栈核心机制。

代价地图基础

网约车司机最怕的不是堵车，而是导航给了一条“地图上不存在”的路。你手机里那张活地图，是市政部门把城市切成无数小方块，一格一格标注出来的：绿色格子可以走，红色格子是施工围挡，灰色格子是数据盲区。机器人世界里的“活地图”就是代价地图（costmap），它把环境切成一格一格的栅格（grid cell），每个格子只存一个数字，代表“能不能走、有多大风险”。

机器人怎么给格子涂色？靠传感器。激光雷达像扫街车一样转一圈，扫到障碍物就把对应格子涂红；深度相机像你的眼睛，把看到的东西投影到地图上；里程计则告诉机器人“你现在站在地图的哪个位置”。这些数据源源不断地写进代价地图，地图也就跟着“呼吸”起来——每次传感器刷新，颜色就变一次。

代价地图的数值不是随便定的，它遵循一套约定俗成的编码：自由空间记 0，代表放心走；占据空间记 254，代表绝对不可通行；未知空间记 255，代表“没探过、按最坏情况处理”。为什么未知比占据还高？因为对机器人来说，看不见的危险比看得见的危险更可怕。

这张会呼吸的活地图，就是司机判断“哪里敢去”的唯一依据。导航 App 敢不敢让你走一条街，先看地图上那串数字答不答应。

分层代价地图

如果所有信息都画在同一张纸上，问题就来了：道路底图半年才更新一次，实时路况每秒都在变，施工点的安全缓冲还要单独描一圈。底图一覆盖，实时信息就没了；实时信息一更新，底图又看不清。聪明的做法是把它们画在不同的透明胶片上，叠在一起看。

代价地图也采用了同样的“胶片叠加”思路，这就是分层代价地图。Nav2 里最常见的三层各司其职：静态图层（static layer）对应那张半年更新一次的底图，来自 SLAM 建好的地图，刷新频率最低；障碍图层（obstacle layer）对应实时路况，激光雷达扫到什么立刻写进去，刷新频率最高；膨胀图层（inflation layer）则在障碍物基础上向外“晕染”一圈代价，对应施工点外的安全缓冲。

这三层最终要合成一张主图（master costmap），规划器只认主图。合并规则很简单粗暴——同一个格子，哪一层给出的代价值最高，就采用哪个，因为安全问题上永远要按最坏情况打算。

分层的好处，就像把底图、路况、缓冲圈分开放：哪块变了就换哪张胶片，既不会互相覆盖，也方便单独调试——这也正是 Nav2 插件化图层设计的初衷。

膨胀与代价设置

老司机开车有个习惯：路过施工围挡，绝不贴着警示锥走，而是主动留出半条车道。围挡本身只占一小块地，但危险区域是一圈“看不见的力场”——越靠近，越心虚。代价地图里的膨胀层，就是这个力场。

膨胀层要做的事，是把每个障碍物往外“吹”出一圈代价渐变带。这里面有两个关键半径：内切半径（inscribed radius）和膨胀半径（inflation radius）。内切半径对应机器人的体型——如果机器人中心离障碍物比这还近，那就铁定撞上了，代价值直接拉满到 254；膨胀半径则是安全缓冲的外边界，超过这个距离，代价归零。中间的过渡带，代价按指数衰减，离障碍越近越红，越远越淡。

实际调试时还有个“缩放因子”（cost scaling factor），它决定衰减曲线是陡峭还是平缓：调大了，安全圈薄而锐利，机器人贴着障碍走；调小了，安全圈厚而绵长，机器人绕得远远的。膨胀半径设多大，要看机器人尺寸和通道宽度——圈画太大，狭窄通道会被堵死；圈画太小，转弯刮蹭的风险又回来了。老司机的那把“尺度感”，就藏在膨胀参数里。

全局路径规划

出发前，导航 App 会先干一件事：在整张城市地图上算出一条从乘客上车点到目的地的大路线。它考虑的是宏观问题——走哪几条主干道、在哪里左转、要不要上高架，完全不理会眼前那辆加塞的车。机器人的全局规划（global planning）干的正是这件事。

全局规划器在全局代价地图上进行搜索，目标是从起点到终点找一条总代价最低的路径。最经典的算法是 A*：它像一位精明的导航员，每到一个路口都知道“已经走了多远”和“离终点还差多远”，把两者加起来作为评判标准，优先探索最有希望的方向，而不是无头苍蝇一样乱转。

全局规划有一个重要前提：它假设环境是静态的。也就是说，这条路线不会每秒钟都重新算，而是规划一次、走一阵子。手机导航也是这个思路——它规划大路线，但路上突发的新状况，得靠司机现场发挥。

局部轨迹规划

真正握着方向盘的是司机本人。你不可能盯着整张城市地图开车，你只看眼前三五米：前车刹车灯亮了，你松油门；右边有人探头，你往左偏一点。这种“短视但高频”的决策，对应机器人的局部规划（local planning）。

Nav2 里最常用的局部规划器之一是 DWA（Dynamic Window Approach，动态窗口法）。它的工作方式像司机在脑子里快速过一遍备选动作：每 100 毫秒左右，在“油门和方向盘”的速度空间里采样出一批候选组合，每个组合往前模拟一小段轨迹，然后给这些轨迹打分，选出最合适的一条执行。打分标准有三项：朝向——这条轨迹是不是指向目标方向；安全——轨迹离障碍物够不够远；速度——能不能保持较高效率。

局部规划只盯着局部代价地图——机器人周围一个小窗口，高频刷新。这就是为什么它能应对“突然加塞的外卖小哥”：障碍物一进窗口，DWA 下一帧就能算出一条绕行轨迹。司机的“手感”，就来自这套三个目标的加权评分。

全局与局部协调

导航 App 和司机从来不是各干各的。App 负责大方向，司机负责脚下；但一旦 App 的路线过时了——比如前方修路封死了——司机不会傻等，而是提醒 App“此路不通，重新算！”机器人的两个规划器，也是这样一对搭档。

配合流程是这样的：全局规划器先在全局代价地图上算出一条路径，由一串路径点组成；局部规划器拿到这些路径点作为“参考线”，每个控制周期做两件事：一是沿参考线往前跟，二是根据局部代价地图实时避障。两者一高一低、一慢一快，像接力赛一样把机器人送到终点。

那什么时候需要重规划？当机器人因为避障偏离全局路径太远，或者局部代价地图显示前方的路径点已经被新障碍堵死时，继续死跟只会越走越偏。此时局部规划器向全局规划器发出“重来”信号，全局规划器以当前位置为起点重新搜索一条新路径。这个过程的数学判据，就是跟踪偏差超过阈值。

这套配合还离不开坐标系变换（TF）这个“翻译官”：激光雷达的数据在传感器坐标系里，机器人的位置在底盘坐标系里，规划器要用的路线在地图坐标系里，全靠 TF 把数据统一起来，两个规划器才能在同一张图上对话。好搭档的标准不过三条：大方向对、脚下稳、该重算时绝不犹豫——代价地图和两级规划器，正是机器人的这三位一体。

AI 应用场景

Python 演示

# -*- coding: utf-8 -*-import mathimport heapq  class Costmap:    """栅格代价地图: 0 自由, 254 障碍, 中间为膨胀代价"""     def __init__(self, w, h, inflation=3):        self.w = w        self.h = h        self.grid = [[0] * w for _ in range(h)]        self.inflation = inflation     def add_obstacle(self, x, y):        self.grid[y][x] = 254     def inflate(self):        # 对每个障碍栅格, 给周围栅格赋指数衰减代价        for y in range(self.h):            for x in range(self.w):                if self.grid[y][x] >= 254:                    for dy in range(-self.inflation, self.inflation + 1):                        for dx in range(-self.inflation, self.inflation + 1):                            nx, ny = x + dx, y + dy                            if 0 <= nx < self.w and 0 <= ny < self.h:                                d = math.hypot(dx, dy)                                if d <= self.inflation:                                    cost = int(253 * math.exp(-d))                                    if self.grid[ny][nx] < cost:                                        self.grid[ny][nx] = cost  def astar(costmap, start, goal):    """A* 全局规划: f(n)=g(n)+h(n)"""    w, h = costmap.w, costmap.h    open_heap = [(0, start)]    came = {start: None}    g = {start: 0}    while open_heap:        _, cur = heapq.heappop(open_heap)        if cur == goal:            path = []            while cur is not None:                path.append(cur)                cur = came[cur]            return path[::-1]        for dx, dy in ((1, 0), (-1, 0), (0, 1), (0, -1)):            nxt = (cur[0] + dx, cur[1] + dy)            if 0 <= nxt[0] < w and 0 <= nxt[1] < h and costmap.grid[nxt[1]][nxt[0]] < 100:                ng = g[cur] + 1 + costmap.grid[nxt[1]][nxt[0]] / 100.0                if nxt not in g or ng < g[nxt]:                    g[nxt] = ng                    h_score = math.hypot(goal[0] - nxt[0], goal[1] - nxt[1])                    heapq.heappush(open_heap, (ng + h_score, nxt))                    came[nxt] = cur    return []  def local_check(costmap, path, idx, lookahead=3):    """局部检测: 往前看几步, 发现致命代价就要求重规划"""    for k in range(1, lookahead + 1):        p = path[min(idx + k, len(path) - 1)]        if costmap.grid[p[1]][p[0]] >= 200:            return False    return True  def main():    cm = Costmap(30, 20, inflation=3)    for x in range(8, 12):        cm.add_obstacle(x, 5)    for x in range(18, 22):        cm.add_obstacle(x, 14)    cm.inflate()     start, goal = (1, 10), (28, 10)    path = astar(cm, start, goal)    assert len(path) > 5, "全局规划失败"    print("全局规划成功, 路径格数:", len(path))     # 运行中突然出现新障碍, 模拟真实世界的动态变化    cm.add_obstacle(14, 10)    cm.inflate()    idx = 0    replan_count = 0    while idx < len(path) - 1:        if not local_check(cm, path, idx):            replan_count += 1            print("局部检测到前方障碍, 执行重规划 %d 次" % replan_count)            path = astar(cm, path[idx], goal)            if not path:                print("无法到达目标")                return            idx = 0            continue        idx += 1    print("到达目标, 全程重规划次数:", replan_count)  if __name__ == "__main__":    main()

1. self.grid = [[0] * w for _ in range(h)]初始化一张全为 0（自由）的栅格图，0 代表畅通。
2. def inflate(self):模拟膨胀层：让障碍物“长胖”，周围栅格获得由近到远的衰减代价。
3. cost = int(253 * math.exp(-d))用指数衰减计算膨胀代价，越靠近障碍数值越高。
4. open_heap = [(0, start)]优先队列存放待探索格子，A* 每次都先取当前总代价最小的格子扩展。
5. ng = g[cur] + 1 + costmap.grid[nxt[1]][nxt[0]] / 100.0扩展邻居时代价 = 步数 + 膨胀代价，让路径主动躲开危险带。
6. h_score = math.hypot(goal[0] - nxt[0], goal[1] - nxt[1])启发式用欧氏距离估计“到终点还有多远”。
7. def local_check(...)局部规划“往前看三步”，发现前方被新障碍堵死就报告。
8. path = astar(cm, path[idx], goal)从当前位置重新跑全局搜索，对应 Nav2 的全局重规划机制。

常见误区

面试题