做移动机器人项目这几年我打交道最多的一个词就是“全局路径规划”。不管是扫地机器人在客厅里绕开拖鞋还是AGV小车在仓库货架间穿梭又或者游戏里的NPC从A点走到B点背后站着的核心算法基本都是A* Search Algorithm。可以说A是路径规划领域最经典、最耐用的一把刀。这篇文章不打算念教科书定义而是从工程落地的角度把A的来龙去脉、代码实现、优化手段和实际踩坑经验摊开讲清楚适合刚接触路径规划的学生也适合正在项目里落地导航功能的工程师。1. 全局路径规划到底在解决什么问题1.1 路径规划在机器人系统里的位置机器人要从房间这头走到那头靠的不是单个算法而是一整套“感知-建图-定位-规划-控制”的流水线。传感器获取环境信息后建图模块生成一张可供查询的地图定位模块告诉你当前在哪里然后全局路径规划才上场在地图上找到一条从当前点到目标点的、可通行的路径。这条路径通常是宏观层面的比如“从卧室穿过走廊到客厅”不会精细到每个电机的转速。真正负责躲避动态障碍、输出平滑速度指令的是后面的局部规划器和运动控制模块。这个分层逻辑特别重要。很多人一开始搞混了全局规划和局部规划的边界结果在动态环境里把全局A*强行高频重跑既浪费算力又得不到平滑轨迹。全局路径规划承担的是“战略层”任务它输出一条离散的路径点序列这些点可能是一个个栅格坐标也可能是一系列位姿。局部规划器拿到这些点之后再结合实时的传感器数据做“战术层”的避障和速度规划。理解了这层关系你就明白为什么全局规划不要求极高频率——地图变化不剧烈时每隔几秒甚至几十秒重新规划一次都够用。1.2 A*算法在规划算法家族中的坐标提到路径规划绕不开几个基础算法Dijkstra、BFS、贪心最佳优先搜索以及A*。把它们放在一起对比事情会清楚很多算法是否考虑路径代价能否保证最优搜索效率典型场景Dijkstra是是慢向四周均匀扩张小规模图、等权图退化为BFSBFS否是无权图中等无权图最短路径贪心最佳优先只看启发值否快但容易被局部障碍骗追求速度、不要求最优A*是是启发函数可采纳时快绝大多数二维/三维路径规划对比之后A的优势就很直观了它同时利用了“已经走过的代价”和“还没走的估计代价”相当于既知道自己在哪又盯着目标方向。Dijkstra最大的问题是无差别扩张哪怕目标就在旁边它依然要铺满周围一圈才甘心贪心搜索则目标方向感很强但一旦遇到一个长条形的障碍物就会一头扎进死胡同里绕不出来等撞了墙才发现自己走错方向。A把两者的优点缝合在一起代价函数里的g项保证搜索路径不偏离现实太远h项保证搜索朝着目标方向推进。只要h设计得合理A*既快又能保证最优。这里可以给一点直觉假设地图有N个格子Dijkstra可能要遍历掉以起点为圆心、半径等于路径长度的整个圆形区域A*则因为有了h的引导只会沿着通往目标点的一个“窄带”区域搜索。这个窄带在开阔场地里尤其明显效率差距可以达到一个数量级。2. A*算法的核心原理与数学基础2.1 从Dijkstra到A*启发式的引入A*的底层是一个很朴素的思路。你在城市里开车去一个目的地如果完全没有导航提示你只能一圈一圈地向外试探这就是Dijkstra如果城市是棋盘式布局你至少知道目的地方向于是会朝那个方向多走一点再看路这就是贪心搜索。而老司机的做法是既要看眼前已经开过的距离也要大致估算剩余路程的直线距离两者加起来最小的路线优先尝试。这里的“估算剩余路程”就是启发式函数h。A*对每个待探索的节点计算一个综合代价值f(n) g(n) h(n)g(n)表示从起点走到节点n已经付出的实际代价h(n)表示从n到终点的估计代价。每次循环从待探索列表里取出f值最小的节点进行处理直到到达终点。这套框架约束了A*的核心行为它不会像贪心搜索那样只顾着往目标的方向冲因为如果那条路实际很难走g值会迅速上升让f值变差它也不会像Dijkstra那样漫无目的地铺开因为h的引导会把搜索能量集中到有希望的方向。2.2 三个关键函数与它们的关系为了体会f、g、h三者的关系举个简单的一维例子。假设你在第0格目标在第10格你正在评估第3格和第7格。第3格的g值是3h值是7f10第7格的g值是7h值是3f也等于10。f相同时算法会两边平均推进这是合理的。如果第7格前面有一面长墙导致绕行代价变大g值上升f就会和第3格的那条路拉开差距算法自然会重新回到第3格的方向。这个权衡过程就是A*的灵魂g是脚踩在地上的现实h是望向远方的直觉两者缺一不可。实际工程里我见过不少新手把f、g、h完全搞混尤其是在修改代码时不小心把h当成g来累加导致搜索乱跑。一个稳妥的习惯是每次扩展节点时先单独打印出该节点的g、h、f三个值把路径搜索过程可视化出来很快就能看出哪里不对劲。2.3 启发式函数的选择与可采纳性启发式函数是A*里最需要动脑筋的部分。不同移动模型要配不同的距离度量曼哈顿距离h |dx| |dy|适用于只允许上下左右四方向移动的地图。欧氏距离h sqrt(dx² dy²)适用于八方向或任意角度移动。切比雪夫距离h max(|dx|, |dy|)适用于允许八个方向且斜走代价与直走相同的模型。这里最核心的概念是可采纳性h(n)的估计值必须永远不大于从n到终点的真实最小代价。只要h满足可采纳性A*就能保证找到全局最优解。原因是当算法最终到达终点时任何其他未完成路径的最小f值都不可能小于这个终点的真实代价否则就会因为h被高估而把那条更短的路径“看起来”比当前终点更差从而错过最优解。以四方向移动为例曼哈顿距离给出的值恰好等于无障碍时从一格到另一格的最短步数它永远小于等于真实代价因此绝对安全。欧氏距离在四方向模型下也始终不大于真实代价。真正危险的情况是你在八方向移动里用了曼哈顿距离但斜走代价小于2这个时候h就可能高估真实代价导致A*找不到最优路径。所以选择启发式函数前先想清楚你的移动模型到底允许哪些方向、每条边的代价是多少。2.4 一致性为什么重要比可采纳性更强一点的属性是一致性也叫单调性对于任意相邻节点n和m都满足 h(n) ≤ cost(n, m) h(m)。意思是从n出发先走到m再估算到终点的距离这个结果不应该比直接从n估算更乐观。一致性带来的好处是每个节点只需要被处理一次因为这个节点一旦被扩展它的g值就不会再被更新成更小的值。如果不一致同一个节点可能被反复加入待探索队列导致运行时间暴涨。绝大多数情况下用曼哈顿距离或欧氏距离配合标准四方向/八方向移动都能天然满足一致性你不用刻意去凑。但理解它有助于排查一些诡异的“死循环”现象——我调试A*时曾遇到过启发式函数返回了负数破坏了单调性结果open list里节点反复进出程序窗口卡到像宕机一样。后来加了h值的合法性检查才定位到问题。3. A*算法的代码实现与实操要点3.1 栅格地图与环境建模实际项目里A*跑的地图通常不是简单的0/1矩阵。以激光雷达构建的栅格地图为例每个格子存储的是被占据的概率常见的做法是设定一个阈值比如占据概率超过50%视为障碍物然后为了照顾机器人自身体积还要对障碍物区域做膨胀处理——也就是把障碍物边界往外扩一定格数保证规划出来的路径不会让机器人蹭着墙边走过。这一步看起来不起眼但对A的效果影响非常大。很多新手直接把原始占据栅格丢给A规划出来的路径贴着墙边走机器人实际开过去就蹭墙了。原因很简单路径规划把机器人当成一个质点而真实机器人是有半径的。膨胀就是把“质点机器人”的假设修正成“有体积的机器人”的过程。膨胀半径通常取机器人半径除以栅格分辨率再加上一到两格的安全余量。地图里的代价值也不一定只有0和1。在带代价的栅格地图上可以给不同区域附加不同的通行代价比如地毯区域移动更慢、靠近障碍物有惩罚、坡度大要避开等。A*的g值累加时把这些代价值算进去规划出来的路径就会主动避开高代价区域这才是工程中真正实用的“全局路径规划”。3.2 完整的A*实现Python我直接给一份可以跑通的Python实现。它用二维列表表示地图0是空地1是障碍物支持四方向和八方向切换同时做了对角线穿墙检查。这份代码的结构很适合作为后续工程改造的起点模板。import heapq import math def heuristic(a, b, h_typemanhattan): dx abs(a[0] - b[0]) dy abs(a[1] - b[1]) if h_type manhattan: return dx dy elif h_type euclidean: return math.hypot(dx, dy) elif h_type chebyshev: return max(dx, dy) return dx dy def astar(grid, start, goal, allow_diagonalTrue, h_typemanhattan): rows, cols len(grid), len(grid[0]) # 起点和终点合法性检查 if not (0 start[0] rows and 0 start[1] cols): raise ValueError(start out of range) if not (0 goal[0] rows and 0 goal[1] cols): raise ValueError(goal out of range) if grid[start[0]][start[1]] 1 or grid[goal[0]][goal[1]] 1: return None # 移动方向与代价定义 if allow_diagonal: directions [(-1,0),(1,0),(0,-1),(0,1), (-1,-1),(-1,1),(1,-1),(1,1)] move_cost { (0,1): 1.0, (0,-1): 1.0, (1,0): 1.0, (-1,0): 1.0, (1,1): 1.414, (1,-1): 1.414, (-1,1): 1.414, (-1,-1): 1.414, } else: directions [(-1,0),(1,0),(0,-1),(0,1)] move_cost {(0,1):1.0,(0,-1):1.0,(1,0):1.0,(-1,0):1.0} open_heap [] heapq.heappush(open_heap, (0.0, start)) g_score {start: 0.0} came_from {} while open_heap: current_f, current heapq.heappop(open_heap) # 跳过 g 值更新后残留的旧堆条目 if current in g_score and current_f g_score[current] heuristic(current, goal, h_type): continue if current goal: path [] while current in came_from: path.append(current) current came_from[current] path.append(start) return path[::-1] for d in directions: nx, ny current[0] d[0], current[1] d[1] if not (0 nx rows and 0 ny cols): continue if grid[nx][ny] 1: continue # 对角线移动不允许穿过墙角 if d[0] ! 0 and d[1] ! 0: if grid[current[0] d[0]][current[1]] 1: continue if grid[current[0]][current[1] d[1]] 1: continue neighbor (nx, ny) tentative_g g_score[current] move_cost[d] if neighbor not in g_score or tentative_g g_score[neighbor]: came_from[neighbor] current g_score[neighbor] tentative_g h heuristic(neighbor, goal, h_type) heapq.heappush(open_heap, (tentative_g h, neighbor)) return None这份代码没有维护独立的closed set而是通过g_score字典来跳过更差的重复路径。每次发现更优的g值才把节点重新压入堆所以同一个节点可能被压入多次但堆顶取到的永远是当前最小的f值条目。优点是逻辑简单、代码量小缺点是内存占用稍高。如果地图很大可以再单独维护一个closed set来加速判断。3.3 关键代码段逐行解读重点说几个容易踩坑的地方。第一对角线穿墙检查。从(0,0)斜走到(1,1)时如果(1,0)或者(0,1)是障碍物这条斜线就穿过了墙角必须禁止。代码里用grid[current[0] d[0]][current[1]]和grid[current[0]][current[1] d[1]]分别检查当前格右侧和下方两个直角邻接格。这个检查不做路径就像机器人穿模一样直接斜穿障碍物的尖角。第二移动代价的设置。斜对角移动代价取1.414而不是1。如果斜走和直走代价一样切比雪夫距离就是可采纳的但如果你用了欧氏距离而地图实际是八方向栅格欧氏距离对斜走的估计1.414和实际斜走代价1不匹配严格来说会高估。我的经验是直走1.0、斜走1.414、启发式用欧氏距离这组搭配在八方向栅格上表现最自然。如果只想用整数运算可以改成直走10、斜走14避免浮点数误差。第三堆里存的是元组(f, node)。Python元组比较会先比较f再比较节点坐标。如果两个节点f相等就会去比较坐标元组这本身没有问题。要注意的是节点类型统一用tuple不要混用列表和元组否则比较时会直接抛类型错误。第四起点终点合法性检查。代码里在搜索前检查了起点和终点是否越界、是否落在障碍物上。这个检查非常重要因为真实系统中定位误差可能把机器人坐标推到障碍物边缘不提前判断会让搜索空转甚至返回错误路径。3.4 八方向移动与地图分辨率的配合八方向移动能让路径更短但随之而来的是平滑需求和撞墙风险。如果你在高分辨率地图上用八方向A*路径会出现大量锯齿状折线这在仿真里无所谓但真机执行时会让底盘频繁变速严重影响控制效果。工程上常见的做法是全局规划用四方向或较粗分辨率的网格先跑通局部规划再用高分辨率网格处理细节。找不到最优路径不是最可怕的最可怕的是规划出一条看起来很短、但实际无法跟踪的路径。4. 工程实战中的优化方案4.1 加权A*在最优性和速度之间取舍标准A追求最优解但很多场景并不需要严格最短路径只要路径差不多短、规划速度够快就行。加权A的做法是把f值计算改成f(n) g(n) w * h(n)w是大于1的权重比如1.5或2.0。h的权重被抬高后算法会更激进地朝目标方向冲搜索节点数显著减少代价是得到的路径可能比最优解长几个百分点。这个取舍在工程上非常划算尤其是地图大、规划频率高的场景。我自己做项目时的经验是w取1.2到1.5之间路径长度损失一般不超过3%但搜索耗时能减少30%到50%。w太高会让算法表现得更像贪心搜索遇到凹形障碍物时容易一头扎进去绕不出来。提示加权A*如果用于实时导航最好加上一个“次优路径接收”的逻辑——只有当目标点确实到达不了时才考虑放宽权重返回一条可通行路径而不是一上来就给很大权重。4.2 JPS跳点搜索网格地图上的加速利器JPSJump Point Search是专门针对二维均匀栅格设计的加速算法。它的核心洞察是在开阔空地上很多节点根本不需要逐个搜索。假设当前正在向北扩展如果前方直到很远都没有障碍物那么这一整条直线上的节点都没有产生“分岔路”的可能直接跳到关键的转折点跳点就行。这样可以把大量中间节点压缩掉搜索速度提升一到两个数量级。不过JPS有前提地图必须是均匀网格、移动代价一致、并且没有非均匀的cost值。一旦地图里引入了“经过某些区域更贵”的代价层比如地毯区域更慢、靠近墙有惩罚JPS就不能直接用了。所以JPS更适合二维静态栅格场景比如游戏寻路机器人导航里如果用了代价地图反而用标准的加权A*更合适。我自己实现过一版JPS最大的感受是它比A难写得多。要找跳点需要写专门的规则函数而且要处理搜索方向的对称性和边界情况很容易出现边界bug。如果你只是需要在某个局部地图上加速可以先试试双向A性价比更高。4.3 双向A*与分层规划双向A*的思路很好理解从起点和终点同时向前搜索直到两个搜索前沿相遇。它的效果在起点和终点相距很远时特别明显因为两个搜索波前加起来扫过的区域远小于单个波前从起点出发扫过的区域。实现时比较棘手的是终止条件和相遇判断——不能简单地在一个方向扩展节点时检查对方是否已访问要处理好两个方向的f值总和这个下界否则可能提前终止得到次优解。分层规划是另一种思路先在一张高层的抽象地图上规划一条大致路线再在每一段内部用A精算。最典型的例子是游戏里的分层寻路地图分成多个区块区块之间通过路点连接NPC先选区块路径再走区块内部的细路径。这种设计把搜索维度大幅压缩内存占用和实时性都会好很多。全局路径规划在超大场景下如果不做分层就算A再优化也很难达到实时指标。4.4 路径平滑与运动约束融合A*输出的是离散栅格路径直接给底层控制单元会导致机器人一顿一顿地走尤其在斜向移动和转弯处会频繁加减速。常用的处理手法是先把路径简化用直线段把那些可以“一眼看过去”的中间点裁掉比如Douglas-Peucker算法或者简单的射线投射法再用B样条或贝塞尔曲线做平滑。更讲究的做法是把运动约束塞进A本身。在代价值里加入转弯惩罚给“当前前进方向和下一步方向不一致”的移动增加额外代价这样A自己就会尽量走出顺畅的大弧线而不是锯齿形的折线。扫地机器人和AGV导航中这招特别实用因为这类底盘转向成本高频繁原地转向不仅费电还伤电机。我在调试导航时发现八成以上“路径看起来很奇怪”的问题都和没做平滑或者没加转弯约束有关。5. 常见问题与排查技巧实录5.1 路径穿墙问题穿墙几乎是每个初写A*的人都会遇到的现象路径从障碍物的尖角斜穿过去。原因基本就两个没有做对角线相邻格子的障碍物检查。地图分辨率太粗障碍物边界只有一个栅格宽斜穿在视觉上非常夸张。对策就是前面代码里提到的穿墙检查斜向移动前检查两个直角邻接格子是否都为空地。这个检查我建议直接写死进移动代价计算里不要在调用方再单独判断否则很容易漏。5.2 路径贴墙、不平滑如果规划出的路径贴着障碍物边界走离障碍物特别近优先检查膨胀半径。在栅格地图上膨胀半径至少要让路径与障碍物保持一个机器人半径的距离。如果机器人传感器噪声大、控制延迟高建议再加一到两格冗余。路径不平滑的问题则用路径简化加样条平滑处理不要指望A*直接输出光滑轨迹。5.3 明明有路却找不到路径地图上看起来明显有通路但A*返回None。这种情况我遇到的频率不低排查顺序很重要起点或终点是否被膨胀层覆盖或被地图标记为障碍。这是最常发生的情况定位误差会把起点坐标推到障碍物格子里。地图转换成行列表后坐标有没有反了。栅格地图的读取顺序、图像坐标和机器人坐标经常不一致检查一下x、y的对应关系。搜索边界是否写对。比如用了cols却当成rowsPython里这种错位通常直接抛异常但如果你写了模糊的保护条件可能只是静默地搜不到。检查启发式函数返回的h值是否出现负数或NaN这会导致堆排序错乱。排查这类问题时我的习惯是先跑代码后看地图先用一张20x20的小地图手工确认搜索逻辑正确再上真实地图。直接在大地图上调bug很难分清是地图问题还是算法问题。5.4 性能触底时怎么排查如果A*在大型地图上跑得很慢先别急着换算法按这几个步骤拿数据说话统计扩展的节点数。如果扩展了全图的绝大部分说明启发式引导太弱了h值太小或者恒为0那A*实际上退化成了Dijkstra。此时应该增大h权重或者改用更强的启发式。检查地图分辨率。很多时候需求根本不需要厘米级分辨率把地图降采样后跑全局规划效率提升立竿见影。全局规划用低分辨率地图、局部规划用高分辨率地图是工程里的经典组合。检查数据结构。有没有用min(open_list)来找最小f值那会O(n)扫描地图一大就卡。换成heapq优先队列后复杂度降到O(log n)差距是天壤之别。5.5 动态场景怎么办需要说清楚一件事A本质上是静态规划算法它假设地图在搜索期间不变。如果环境里有人走来走去、有门忽然关上A规划出的路径可能瞬间失效。工程上处理动态场景通常不是让A去适配而是加一个局部规划器全局路径提供方向局部规划器实时避开动态障碍物。如果场景变化很频繁比如仓库里大量机器人同时作业那就要考虑DLite这类增量式重规划算法它能在环境部分变化时复用之前的搜索结果而不是从头搜一遍。写在最后我在实际运行A过程中的一个体会是算法本身一点都不神秘几十行代码就能跑通但真正决定一个路径规划模块好不好用的几乎全是算法之外的工程细节——地图怎么建、障碍物怎么膨胀、代价怎么设计、路径怎么平滑。A之所以值得你花时间彻底搞懂是因为它是所有启发式搜索的基础。理解了f g h这个框架后面看D*、D* Lite、Hybrid A*、RRT*这些变种都会事半功倍。如果你正在写自己的第一个全局路径规划模块我的建议很简单先不要上任何优化技巧把最朴素的A*用最清晰的数据结构写出来跑通、画图、观察然后再一步一步往工程化方向加码。这个过程走过一遍之后你会发现自己对“路径规划”这四个字的理解比看再多论文都实在。
