基于占据栅格地图的 A* 路径规划

机器人的路径规划器回答的问题，和避障控制器回答的问题并不相同。规划器问的是：“哪一条从起点连到终点的空闲格子路径代价最低？”控制器问的是：“此刻什么动作是安全的？”A* 是一种图搜索算法，它把已经付出的代价和剩余代价的估计结合起来，因此是二维占据栅格上最实用的第一种规划器。

路径有多安全，完全取决于你交给它的栅格和车体轮廓。沿空闲格中心画出的数学最短直线，如果不膨胀障碍，照样可能蹭到墙。

把地图变成图

占据栅格把空间划分成大小相同的格子。每格可以是空闲、占据或未知。对一个基础规划器来说，每个可通行的格子都是图中的一个节点，并与它的邻居相连。

四方向移动时，邻居是北、东、南、西，每个代价为 1。八方向移动则再加上对角线，代价为 √2。之后乘以地图分辨率，就能把栅格距离换算成米。

未知格怎么处理，需要你明确决定。谨慎的室内机器人可以把它们当作占据；探索型机器人也可以放行，但加上额外代价。让这件事成为一项策略输入，而不是某次不小心跟错误的数值做了比较。

理解 A* 的三个分数

对每个节点 n，A* 记录三项：

• g(n)：从起点到 n 的已知代价；
• h(n)：从 n 到终点的启发式估计；
• f(n) = g(n) + h(n)：经过 n 的整条路径的估计总代价。

开放集保存已发现、还可能被展开的节点。每一步取出 f 最小的那个节点。如果它就是终点，就沿父节点指针一路回溯到起点，把路径重建出来。否则对每个有效邻居做松弛：如果新算出的 g 比已记录的代价更小，就更新它的代价和父节点。

启发式只负责引导搜索，并不改变已经付出的代价。四方向移动且代价均匀时，曼哈顿距离是合适的：

h = |goalX - x| + |goalY - y|

八方向移动、对角代价为 √2 时，改用八分距离。在开阔空间里，欧氏距离同样不会超过真实路径代价。估计偏高可以让搜索更快，但会牺牲最优路径的保证。

用优先队列实现搜索

Python 标准库就够写一个紧凑的参考实现：

from heapq import heappop, heappush

def astar(start, goal, neighbors, heuristic):
    frontier = [(heuristic(start, goal), 0, start)]
    came_from = {start: None}
    cost = {start: 0}

    while frontier:
        _, current_cost, current = heappop(frontier)
        if current_cost != cost.get(current):
            continue  # stale queue entry
        if current == goal:
            break

        for next_cell, step_cost in neighbors(current):
            new_cost = current_cost + step_cost
            if new_cost < cost.get(next_cell, float("inf")):
                cost[next_cell] = new_cost
                came_from[next_cell] = current
                heappush(frontier, (
                    new_cost + heuristic(next_cell, goal),
                    new_cost,
                    next_cell,
                ))

    if goal not in came_from:
        return None

    path, cell = [], goal
    while cell is not None:
        path.append(cell)
        cell = came_from[cell]
    return list(reversed(path))

与其费力去修改堆里已有的条目，不如把改进后的新值直接压进去，弹出时再把过期条目丢掉。这样实现既小又正确。在内存吃紧的控制器上，用固定尺寸的栅格加索引数组，可以避开动态映射表。

防止对角切角

八方向移动时，如果对角邻居两侧的任一正交格被占据，这个对角邻居就应该被拒绝。否则规划器会从两个在角上相接的障碍中间穿过去——对宽度不为零的机器人来说，这个动作根本做不到。

对于从 (x, y) 到 (x+1, y+1) 的一步移动，要检查 (x+1, y) 和 (x, y+1) 是否都可通行。然后把车体轮廓膨胀作为独立的一层安全措施来施加。

为真实机器人膨胀障碍

让机器人中心直接穿过原始占据格，等于把它当成一个没有尺寸的点。每个障碍至少要按机器人的内切半径加上安全余量来膨胀。等价地说，就是先把障碍撑开，再让一个点状中心在剩下的空闲空间里规划。

硬膨胀半径会把附近的格子直接标成占据。分级的代价场则是在靠墙处加大代价，鼓励机器人留出间隙，而不必把每个附近的格子都判为不可通行。选定的半径必须把车体外伸量、定位不确定度和跟踪误差都算进去。

地图分辨率会改变换算出来的格子数：inflationCells = ceil(inflationMetres / resolution)。这个换算要单独测一遍。0.20 m 的半径在 0.05 m/格 下是 4 格，而不是 0.20 格。

加上有用的通行代价

均匀代价的 A* 返回的是栅格意义上最短的路径。但机器人往往需要的是代价最低的路径。给靠近障碍的格子、倒车动作、急转弯和崎岖地形都加上代价。保持所有边的代价非负；如果最优性对你很重要，就让启发式在该模型下不超过剩余代价。

如果不把朝向纳入状态，就不要加转弯惩罚。若同一个格子朝东到达和朝北到达会带来不同的后续代价，那么只用 (x, y) 就不足以描述这个节点；要么改用 (x, y, heading)，要么接受这个惩罚只是后处理阶段的偏好。

系统性地验证规划器

从答案显而易见的小地图开始：

1. 空栅格：路径应当接近直线最优。
2. 一道墙加一个缺口：路径必须从缺口穿过去。
3. 终点被占据：不陷入死循环，直接返回无路径。
4. 起点等于终点：返回那一个起点格。
5. 对角相接的障碍：不得切角穿过。
6. 窄走廊比膨胀后的车体轮廓还窄：无路径。

对每条返回的路径，断言它的起点和终点正确、每一步都相邻、没有格子被占据，总代价等于各步代价之和。把展开过的格子也画出来，和最终路径一起对照；展开量过大往往暴露出启发式写坏了，或者单位对不上。

常见规划器故障的诊断

• **路径穿墙：**占据索引、行/列顺序或对角合法性检查写错了。
• **路径切角：**障碍没有膨胀，或者允许了对角切角。
• **A* 返回的路径比预期长：**启发式与单步代价的单位不一致，或者过期代价被接受了。
• **搜索吃掉了太多内存：**栅格分辨率细得没有必要，或者重复的队列条目从不被丢弃。
• **机器人跟不动一条有效路径：**规划忽略了车体轮廓、转弯半径、控制器极限或定位误差。
• **每次传感器更新之后路径都在抖：**小改动就重启全局规划，既没有代价滞回，也没有重规划策略。

迷宫求解模拟器演示了在小型未知迷宫上一种相关的距离场方法。避障模拟器说明了为什么一条全局路径仍然需要一层局部安全控制器。而当任务不是抵达某一个目标、而是走遍每个地方时，上面这一套都不适用——覆盖路径规划既没有终点格也没有启发式，它的衡量指标是每平方米要走多少距离。无论如何，路径最终还得被执行，那是另一个问题，有它自己的控制器——见纯跟踪。