去年我们小组调试某智能车迷宫赛项时最崩溃的一幕是机器人明明离出口只剩两格却总在死胡同里反复调头。后来我们才意识到“机器人走迷宫”听起来像是一道题实际上至少是两个问题地图建模问题与最短路径算法问题。这篇文章就把我们试过的DFS、BFS、Dijkstra、A*四种方案从头到尾复盘一遍直接给出可复现的代码和调试中的教训。适合正准备走迷宫/最短路径算法相关比赛或者想快速把仿真路径搬到真机上的同学参考。我先说一个关键结论不要一开始就上高级算法。迷宫在20×20以内时BFS已经够用一旦加入转向代价或者迷宫规模超过100×100Dijkstra和A*才真正拉开差距。而大部分真机翻车不是算法不够强而是坐标约定、路径回溯、已知地图与未知地图这三件事没做好。1. 迷宫问题先拆成两层地图建模与路径搜索1.1 从赛场拿到的不只是一张图我们最初犯的错是把迷宫赛题当成“给定一张图找一条路”。实际上机器人面临的输入往往很朴素像素图、激光数据、灰度传感器信号甚至是一段二值数组。你需要先把这些原始信息转成机器人能识别的数字地图然后才能谈“最短路径”三个字。一个典型的迷宫赛题是黑白网格图黑线代表可行区域白色代表禁止区域更常见的是给一张栅格图障碍物是黑色块通道是白色。如果直接拿这种图跑路径搜索第一件事就是把图片压缩成二维数组而不该把像素当节点。否则一张500×500的图片会产生25万个节点BFS和A*都能跑但嵌入式控制器上会明显卡顿。我们把“迷宫问题”拆成两层底层是地图建模负责把视觉/传感器数据变成0、1、2组成的网格上层是路径搜索负责在网格上计算起点到终点的最短路径。这两层必须独立否则换一次地图分辨率就要重写整个路径模块。1.2 数字地图结构0/1/2网格的坐标约定我建议用以下约定0代表可以走的空地1代表障碍物2代表起点或终点。[ [2, 0, 1, 0, 0], [1, 0, 1, 0, 0], [0, 0, 0, 1, 0], [0, 1, 0, 0, 0], [0, 0, 0, 1, 0] ]这个5×5的地图中起点是左上角(0,0)终点可以放在右下角(4,4)。注意这里我用“行、列”的顺序写坐标grid[row][col]row对应世界坐标的Y方向col对应X方向。很多第一次写寻路的人会把(x, y)直接当成(row, col)结果路径整体转置小车沿着墙壁走。在真机项目里我们统一在入口处做一次坐标转换函数把所有传感器坐标、里程计坐标、地图坐标全部规定成“row向下为正、col向右为正”。后面接惯性导航或轮式里程计都很方便至少不会出现“地图路径是反的机器人倒退着走”这种低级事故。1.3 访问状态别藏在函数参数里网格地图通常会作为参数传给BFS、Dijkstra等函数但我们踩过一个坑当地图是动态更新时如果函数内部直接修改grid会在后续重规划时留下脏数据。比如在线路径规划时机器人刚把某个格子标记为“临时障碍”结果下一次寻路却把这个格子当作永久墙。更稳妥的做法是定义一个MapConfig结构体把地图数据、宽度、高度、起点、终点、分辨率、动态障碍表放在一起路径搜索函数只读地图数据更新地图单独走一个接口。这样既方便单元测试也方便后面换A*或RRT时不用改外部调用。2. 先跑通第一版DFS探测与BFS最短路2.1 DFS适合现场试探不适合静态最优深度优先搜索看起来和迷宫最配沿着一条路走到黑撞墙就回退一格换方向。在真实机器人上这种“试探式”走法确实常见因为局部传感器只知道岔路口不知道全局地图。用DFS做现场探索代码简单对内存消耗也小。但DFS有一个致命问题它不保证最短。小学见到的“手贴墙走迷宫”和DFS本质一样能走出来但路径可能绕成蛇形。如果赛题要求的是“最短路径”或者“最快到达”那DFS只能用来做未知迷宫的地图构建阶段不能用它输出最终路径。另一个隐患是递归栈。DFS天然可以用递归写def dfs(grid, x, y, visited, path): if not (0 x len(grid) and 0 y len(grid[0])): return False if grid[x][y] 1 or (x, y) in visited: return False visited.add((x, y)) path.append((x, y)) if (x, y) goal: return True for dx, dy in ((1,0),(-1,0),(0,1),(0,-1)): if dfs(grid, xdx, ydy, visited, path): return True path.pop() return False这种写法在10×10的小迷宫里没事但一旦到100×100Python递归深度会直接超出限制嵌入式C语言里递归栈也可能爆。所以我建议即使做DFS也用显式栈实现避免依赖调用栈。2.2 BFS求最短路径的完整实现BFS是全篇的重头戏因为它简单、直观、可解释等权网格上的最短路径用它最稳妥。BFS的步骤是从起点开始先访问所有距离为1的格子再访问距离为2的格子直到终点出现。因为每层都是逐层扩散第一次到达终点的路径一定是最短路。真正容易写错的是路径还原。只算“到终点需要多少步”很简单但机器人要的是一条完整轨迹。所以BFS需要记录每个格子的父节点最后从终点回溯到起点。下面这段是我在实际调试中用的版本from collections import deque def bfs_path(grid, start, goal): rows, cols len(grid), len(grid[0]) parent {start: None} q deque([start]) while q: cur q.popleft() if cur goal: break for dr, dc in ((1, 0), (-1, 0), (0, 1), (0, -1)): nxt (cur[0] dr, cur[1] dc) if not (0 nxt[0] rows and 0 nxt[1] cols): continue if grid[nxt[0]][nxt[1]] 1: continue if nxt in parent: continue parent[nxt] cur q.append(nxt) if goal not in parent: return None path [] node goal while node is not None: path.append(node) node parent[node] path.reverse() return path几个值得注意的地方我用parent字典同时扮演“访问标记”和“来源记录”两个角色。因为字典的key存在说明该节点已经被访问过不需要单独写一个visited集合。方向顺序上、下、左、右不影响最短路径长度但会影响多条最短路径中最终选出的那条。如果希望路径更“直线优先”可以把方向顺序调成右、下、左、上。如果goal not in parent说明终点不可达此时不要强行回溯直接返回None。2.3 BFS解决不了“转向代价”BFS有一个隐含假设每个格子到相邻格子的代价都是1。可真实的机器人不是这样。走直线时轮子转速稳定转弯时要么减速、要么原地旋转代价明显更大。当迷宫设计者在通道里布了连续弯道时BFS找出来的“格子数最短”路径可能充满锯齿走起来比绕远路还慢。这时就需要把“代价”这个维度补上。20×20以下的小迷宫还可以用BFS硬跑一旦要求机器人快速通过复杂路径就必须看下一节的Dijkstra和A*。3. 加权重之后Dijkstra与A*的选择题3.1 把转向代价写进状态空间带权迷宫的本质是给边加权重直行一格代价1.0转弯90度代价1.5或2.0。问题是如果只用(x, y)表示状态根本不知道机器人进入这个格子时朝哪个方向也就无法判断下一步是直行还是转弯。解决办法是把状态从(x, y)扩展成(x, y, heading)heading记录进入该格子时的朝向。这样从(x, y, heading)走向邻居(nx, ny, new_heading)时如果new_heading heading就按直行给代价否则按转弯给代价。注意同样的物理位置如果从不同方向进入代价和后续路径都不一样所以它其实是一个“扩展状态图”。这也是为什么很多同学直接改BFS代码加权重后总得到错误路径——BFS只认格子不认方向。3.2 Dijkstra的实现骨架Dijkstra和BFS最大的区别是把普通队列换成优先队列每次弹出当前代价最小的状态。网格图中它的复杂度大约是O(E log V)在等权图上比BFS慢但能正确求解带权图。import heapq # 0右 1下 2左 3上 DIRS [(0, 1), (1, 0), (0, -1), (-1, 0)] def dijkstra_turn(grid, start, goal, turn_cost1.5, straight_cost1.0): rows, cols len(grid), len(grid[0]) # 初始朝向假设朝右实际项目中从起点姿态读取 dist {(start, 0): 0} parent {(start, 0): None} pq [(0, start, 0)] while pq: d, cur, heading heapq.heappop(pq) if d ! dist.get((cur, heading), float(inf)): continue if cur goal: break for new_heading, (dr, dc) in enumerate(DIRS): nxt (cur[0] dr, cur[1] dc) if not (0 nxt[0] rows and 0 nxt[1] cols): continue if grid[nxt[0]][nxt[1]] 1: continue step_cost straight_cost if new_heading heading else turn_cost nd d step_cost key (nxt, new_heading) if nd dist.get(key, float(inf)): dist[key] nd parent[key] (cur, heading) heapq.heappush(pq, (nd, nxt, new_heading)) if not any(k[0] goal for k in dist): return None # 找到终点时朝向代价最小的那个状态 best min(((k, v) for k, v in dist.items() if k[0] goal), keylambda x: x[1]) state best[0] path [] while state is not None: path.append(state[0]) state parent[state] path.reverse() return path这个版本里我简化了回溯逻辑先选出到达终点且总代价最小的状态再顺着parent往前找。如果你只想验证算法可以先用小迷宫跑一遍把turn_cost分别设成1.0和1.5看路径是否发生变化。设成1.0时它应该等价于BFS给出的最短路。3.3 A*启发式与最优性边界A是在Dijkstra基础上加一个启发式函数h(n)用于估计当前点到终点的剩余代价。搜索时每次弹出g(n) h(n)最小的状态这样能明显减少搜索范围。常见启发式是曼哈顿距离abs(x - goal_x) abs(y - goal_y)要求相邻格子移动代价是1。如果加入了转弯代价曼哈顿距离不再准确A有可能得到的不是全局最优。这一点很容易被忽略。我看到不少人直接把带转向代价的迷宫丢给A*用曼哈顿距离算出来路径确实短了但偶尔会多几个多余拐弯。原因是启发式低估了转弯代价导致搜索提前收敛到一条看似“格子数少、实际转弯多”的路径。解决方案有两个要么把转弯代价做大让启发式更接近真实代价要么放弃A*的最优性要求接受“近似最优但搜索很快”的路径。对于比赛来说后者常常可以接受因为路径少绕几个格子远不如少几次原地转向实在。3.4 三个算法直接对比算法适用场景最优性时间/空间注意点BFS等权网格、小地图严格最短O(NM)队列友好无法处理转向代价Dijkstra带权网格、规模中等严格最优O(E log V)较慢状态要带方向A*已知地图、单目标、地图大启发式合理时最优搜索范围小启发式要尽量贴近实际代价真机调试时我们优先用BFS做正确性验证再用A做性能优化最后用一次Dijkstra验证带权结果是否和A显著不同。如果差别不大比赛就固定用A*版本。4. 真机调试中三次翻车复盘4.1 坐标系原点不一致路径上下颠倒第一次把BFS路径接到真机上机器人直接往反方向跑。查了很久发现仿真器里Y轴朝上而地图数组的Y轴朝下。我在地图里算出的(row0, col0)是左上角真机却把(0,0)当成了屏幕左下角。两者在Y方向上完全相反生成的路径自然上下翻转。解决方式不是“减一下高度就行”而是建立一个统一坐标转换函数。所有外部输入必须先过一次坐标转换转成数组坐标后再进入路径算法所有算法输出的轨迹也必须通过逆变换才能发给底盘控制器。不要在某些模块里偷偷处理坐标那样一定会漏。4.2 路径回溯时起点消失有一次路径轨迹打印出来起点莫名其妙没了机器人不知道该先走到哪。后来定位到代码问题我在回溯时用node parent[node]做循环但循环结束条件是node is not None。问题出在初始化时我直接写node parent[goal]导致起点没有被加入path并且先移动到了起点之前的None状态。正确做法是让parent[start] None回溯时从goal开始一直追到node is None最后把path整体反转。另外很多带权版本里parent的状态是(x, y, heading)而不是(x, y)回溯时也得跟着处理状态否则会出现方向信息丢失生成的轨迹在拐弯处对不上。4.3 把“已知地图”当成“在线地图”用这是我们最隐蔽的一次翻车离线仿真时A*跑得飞快路径也漂亮但一上车就跑偏甚至往新出现的障碍物里钻。原因是仿真用的是赛题原始布局而真机启动时看到的迷宫会因为传感器误差、机械定位偏差产生细微差异原来标记为通道的地方可能被识别成障碍。后来我们改成“在线两步走”第一步用DFS或激光建图先扫一遍实时地图第二步把实时地图发给A重规划。如果地图更新频率不高每次只在发现新障碍时重跑一次A只有地图变化非常频繁时才考虑D* Lite这类动态重规划算法。对大多数赛题来说先在起点完成一次地图构建再离线规划已经够用。5. 四类迷宫实测数据与最终选型建议5.1 测试数据规模、障碍率、耗时为了做选型我在开发板上用Python跑了一组对比测试。迷宫类型分四种小型迷宫、窄通道迷宫、带权重迷宫、大规模开阔迷宫。测试结果只能说明趋势不同硬件和语言差异很大但相对关系值得参考。迷宫规模特征BFS耗时Dijkstra耗时A*耗时20×20空旷0.6ms0.9ms0.5ms50×50多死胡同11ms15ms4ms100×100窄通道66ms88ms21ms200×200大量开放区域410ms560ms45ms可见规模越大A的搜索范围优势越明显。在100×100以上的迷宫里BFS和Dijkstra的差距并没有想象中大因为Dijkstra在等权图上会访问大量无意义节点A因为有了方向性引导能更早找到终点。5.2 不同场景的终极选择如果只做一个课程作业或Demo我建议直接用BFS把路径还原逻辑吃透这比什么都宝贵。如果比赛要求“最短时间到达”并且转弯有真实代价建议用带转向代价的A*同时手动验证路径“锯齿少、拐弯少”。如果地图未知别急着上高级算法先用简单探测刷新地图再用A*做局部重规划。我个人的倾向是不要迷信“A一定比Dijkstra快”。A的优势完全依赖启发式和实际代价的匹配程度。当启发式严重偏离真实代价时A*可能退化成Dijkstra甚至更差。在带权迷宫里真正的收益来自你为转向单独建立状态空间而不是算法本身。5.3 一个提高真机成功率的细节路径算法输出的是一长串网格点。如果把这些点逐一下发到控制器机器人会在每个网格点都尝试对准坐标出现“行走路线歪歪扭扭、频繁原地修正”的情况。我们在真机上做了一个拐点提取遍历整条路径把夹角接近180°的中间点去掉只保留真正需要转弯的关键点。这样做的效果立竿见影机器人沿直线段匀速走到关键点再转向平均完成任务时间缩短了三分之一。这个优化不改变搜索算法但把“算法找到的几何路径”变成了“机器人能执行的轨迹”。如果你也在做机器人走迷宫建议先完成这一步再回去调A*的启发式权重。