前阵子整理工程归档的时候，翻到一份标注着20200412的机器人走迷宫项目代码。当时做这个项目主要是为了应付课程设计兼竞赛，但从需求梳理、算法选型、代码实现到实体机器人调试，整套流程跑下来，收获远超过一个“迷宫最短路径”Demo本身。这篇博文把这个项目从零到一的完整过程拆开来讲，包括我当时为什么选A*而不是DFS、栅格地图怎么建模、路径平滑怎么处理、实体机器人转弯为什么会飘，以及踩过的几个典型坑。如果你是刚接触路径规划的初学者，或者正在做类似课程设计、竞赛项目，这篇内容可以直接当参考线。
1. 项目全貌与需求拆解
1.1 机器人走迷宫到底在解决什么问题
走迷宫听起来像个小游戏，但放到机器人身上，本质上是三个子问题：
第一是“我在哪”。机器人需要知道自己在地图中的位置，这涉及定位，实体机器人可以用编码器、惯性导航或者视觉标记完成。纯仿真环境下简单很多，坐标可以直接读。
第二是“路怎么走”。已知地图和起终点，要算出一条从起点到终点、能避开障碍物的路径。这个问题的核心是“搜”还是“算”：DFS这类属于搜，一个格子一个格子试探；A*这类属于启发式计算，目标明确地朝终点推进。
第三是“怎么走过去”。路径算出来是一堆离散坐标点，但机器人有物理尺寸、有转弯半径、有加速度限制，直接把点扔给电机跑，大概率会撞墙。所以路径平滑和运动控制是工程落地的关键。
这个项目最终的验收条件是：一辆两轮差速小车，从迷宫入口出发，自动走到出口，过程不碰墙，路径尽量短，时间尽量快。注意是“尽量短”和“尽量快”——这就是最短路和时间最优的博弈，后面调参的时候会反复遇到。
1.2 方案选型：为什么先做仿真再做实体
我当时的思路很明确：先在电脑上把地图、算法、路径画出来，确认逻辑没问题，再往实体小车上传代码。很多初学者犯的错误是一上来就焊板子、接电机，结果算法没写好，车已经撞坏两三次。
仿真的好处是可控性强：
- 地图可以随意改，障碍物摆放位置、迷宫复杂度一键生成
- 调试时可以暂停——A*某一步算错了，画出来一眼就能看出来
- 路径效果可以量化对比：路径长度、转折次数、计算耗时都有记录
用Python写了一个简单的2D栅格仿真界面，把每一步扩展的节点都可视化出来。这个仿真器虽然简陋，但帮我发现了至少三个算法层面的问题：启发函数权重没调好时路径会绕远、地图边界处理不严谨导致越界、死胡同场景下DFS栈溢出。
这里特意强调一点：先仿真后实体，能省下的不是一两天，而是一两周的调试时间。实体机器人的调试成本太高了，换电池、充电、搬迷宫、录日志、看轨迹，一次来回就十几分钟。
2. 核心算法原理与选型思路
路径规划算法在迷宫场景下有多个候选方案，这里把主流的四种放在一起对比，并给出这个项目选型的实际考量。这部分内容不局限于题目本身，而是给出一套可复用的思考方式。
2.1 四种经典路线对比：DFS、BFS、Dijkstra、A*
| 算法 | 原理 | 完备性 | 最优性 | 时间复杂度 | 适用场景 |
|---|---|---|---|---|---|
| DFS（深度优先） | 一条路走到黑，不行就回溯 | 完备，但可能极慢 | 不保证最优 | O(分支数^深度) | 迷宫规模小、无需最优路径 |
| BFS（广度优先） | 层层扩散，逐层搜索 | 完备 | 无权图下最优 | O(V+E) | 等权重栅格、确保最短路径 |
| Dijkstra | 带权重的优先扩散 | 完备 | 带权图最优 | O((V+E)logV) | 地图含不同地形代价 |
| A* | 带启发函数的优先扩散 | 完备 | 启发函数合理时最优 | 介于BFS和最优之间 | 静态地图、单次查询、需要快速逼近最优路径 |
这四种算法我在项目中全部实现了一遍，目的就是为了对比实验数据。实测在一个30×40的栅格迷宫里，BFS扩展节点数是A的约3倍，Dijkstra约4倍。也就是说，同样一张地图，A能在更短的时间内算出一条与BFS几乎相同长度的路径。
你可能会问，迷宫路径有没有“唯一解”的情况？如果迷宫是不带环的树形迷宫，走法路径往往被墙壁结构限定死，那DFS反而可以直接找到出口——但那条路长度是不是全局最短？不一定。比如树形迷宫可能出现“一路走到深处再折返很远”的情况，DFS走出来的往往是“最先碰到的那个出口”，而不是“所有出口里最近的那个”。所以题目同时强调“走迷宫”和“最短路径”，就基本排除了纯DFS的方案，优先考虑BFS或A*。
2.2 我最后选择：A*算法为核心，BFS兜底
A*算法从“当前格子”走向“目标格子”的过程，用一张图来看：每个待扩展节点根据f(n) = g(n) + h(n)排序。
g(n)：从起点到当前节点实际走过的距离h(n)：当前节点到终点的预估距离，曼哈顿距离或者欧氏距离f(n)：两者相加 —— 决定了下一轮优先处理哪个节点
A的“聪明”之处在于，它不用像Dijkstra那样四面八方均匀扩散，而是优先沿着“离终点更近”的方向走。但代价是：如果h(n)估值不准确（尤其是低估了实际代价），A就退化为Dijkstra，扩展节点数飙升。如果h(n)严重高估，算法会过于“贪心”，可能放弃最优路径。
先在某高校模拟项目里用30×40栅格对比过BFS和A*。BFS扩展节点数约为A*的3倍，耗时约为2.5倍。在真实迷宫场景里，虽然视觉上两条路线“看起来差不多”，但算力消耗直接影响了后续实体机器人的决策频率。
加一个兜底策略：如果在运行过程中，A*因为地图数据异常（比如障碍物图层脏数据导致不可达），在设定时间内没计算出路径，就自动切换为BFS重新搜索。BFS虽然慢，但胜在稳，不容易因为启发函数“带偏”。
2.3 环境建模：栅格地图与坐标换算
地图是把物理迷宫转换成算法能处理的数据结构。我做的是栅格地图，也就是把迷宫切成一个个小格子。栅格尺寸的选择直接影响算法复杂度和路径质量。
假设迷宫物理尺寸是1.5米×2米，机器人底盘直径是20厘米，考虑每边留3厘米安全余量，栅格大小设为13厘米×13厘米。为什么不是20厘米？因为20厘米意味着机器人中心贴着障碍物走，稍有偏差就会擦墙。13厘米的网格，机器人在通道里走起来有7厘米左右的横向余量，实测比较稳。
地图存储用二维数组，0表示空地，1表示障碍物，2表示起点，3表示终点：
import numpy as np # 0=空，1=障碍，2=起点，3=终点 maze = np.array([ [1, 1, 1, 1, 1, 1, 1, 1, 1, 1], [1, 2, 0, 0, 1, 0, 0, 0, 0, 1], [1, 0, 1, 0, 1, 0, 1, 1, 0, 1], [1, 0, 1, 0, 0, 0, 1, 0, 0, 1], [1, 0, 1, 1, 1, 1, 1, 0, 1, 1], [1, 0, 0, 0, 1, 0, 0, 0, 0, 1], [1, 0, 1, 0, 1, 0, 1, 1, 1, 1], [1, 1, 1, 0, 0, 0, 0, 0, 0, 1], [1, 0, 0, 0, 1, 1, 0, 1, 3, 1], [1, 1, 1, 1, 1, 1, 1, 1, 1, 1], ])坐标转换这部分，看代码不如看公式直观：
- 地图坐标到物理坐标：物理X = 栅格X × 栅格尺寸 + 栅格尺寸/2
- 物理坐标到地图坐标：栅格X = int(物理X / 栅格尺寸)
- 机器人占据面积在栅格地图上近似为一个半径2格的圆，也就是扩展障碍物1~2格，作为膨胀半径
这个“膨胀半径”是实体实测后反馈到仿真里的一个关键参数。一开始我设的是1格，路径贴障碍物太近，实体车一转向就擦墙；后来改成2格，路径明显靠中心线走，稳定很多。
3. 路径规划实现详解（附代码）
3.1 A*核心逻辑逐段拆解
A*的实现代码网上非常多，但多数是“能用”级别，缺少细节处理。我挑几个关键点说明。
这里直接展示核心部分：使用优先队列（heapq），保证每次取出的都是f值最小的节点：
import heapq import math def heuristic(a, b): """曼哈顿距离，4方向运动时适用""" return abs(a[0] - b[0]) + abs(a[1] - b[1]) def astar(maze, start, goal): rows, cols = maze.shape open_set = [] heapq.heappush(open_set, (0, start)) g_score = {start: 0} came_from = {} while open_set: _, current = heapq.heappop(open_set) if current == goal: path = [] while current in came_from: path.append(current) current = came_from[current] path.append(start) path.reverse() return path # 四方向邻居 for dx, dy in [(1,0), (-1,0), (0,1), (0,-1)]: neighbor = (current[0] + dx, current[1] + dy) if not (0 <= neighbor[0] < rows and 0 <= neighbor[1] < cols): continue if maze[neighbor] == 1: continue tentative_g = g_score[current] + 1 if neighbor not in g_score or tentative_g < g_score[neighbor]: came_from[neighbor] = current g_score[neighbor] = tentative_g f_score = tentative_g + heuristic(neighbor, goal) heapq.heappush(open_set, (f_score, neighbor)) return None # 无路径几个容易被忽略的细节：
- 为什么用
g_score字典而不把代价直接存在节点里？因为同一个节点可能被不同路径访问多次，字典存的是“当前已知最小代价”，这是Dijkstra/A*能保证最优性的关键 - 为什么邻居顺序不影响结果？A*的扩展顺序由
f值排序决定，邻居的遍历顺序只影响同一优先级时的取用顺序 - 边界检查放在障碍物检查之前，避免数组越界导致程序崩溃
模拟场景里跑这段代码，输出是路径坐标列表，但注意这个路径是“8字形贴边”的，转弯处全是直角，直接给机器人跑会一顿一挫。
3.2 路径平滑与转角优化
栅格地图出来的路径有两个工程问题：
- 路径由相邻格子的中心点连接，线是锯齿状
- 存在不必要的转弯，比如“之”字形绕行
在实体车上，转弯越多，时间越长，误差累积越严重。所以加了两个后处理步骤：
第一步，拐点简化。连续三点共线时，删掉中间点。这个操作能用一段直线替代两三段短折线。
第二步，平滑处理。用简单的移动平均把路径“捋直”：
def smooth_path(path, weight_smooth=0.3, weight_data=0.7): # 保持起点和终点不变，中间点渐进修正 new_path = path.copy() for _ in range(100): for i in range(1, len(path) - 1): for j in range(2): new_path[i][j] += weight_data * (path[i][j] - new_path[i][j]) new_path[i][j] += weight_smooth * (new_path[i-1][j] + new_path[i+1][j] - 2 * new_path[i][j]) return new_path注意权重分配。weight_data太大，路径贴近原始折线，平滑效果弱；weight_smooth太大，路径可能“抄近道”穿过障碍物角落。工程上常用的比例是0.7：0.3，且平滑前必须保证障碍物已经做了膨胀处理，否则平滑后路径被“拉”到障碍物上。
3.3 参数计算的实操记录
那次实验里，我最得意的是把“路径质量”量化了。规划出一条路径后，自动统计：
- 路径长度：所有相邻点距离之和
- 转折次数：相邻三个点夹角小于等于45度视为一次转折
- 最大曲率半径：局部路径的曲率，判断小车能不能转过来
以30×40栅格迷宫为例，调参前后的数据：
| 参数 | 未平滑 | 平滑后 | 实际效果 |
|---|---|---|---|
| 路径长度 | 52格 | 48格 | 缩短7.7% |
| 转折次数 | 19次 | 11次 | 减少42% |
| 最大单段转角 | 90度 | 约30度 | 高速转弯更安全 |
| 预计行驶时间 | 26秒 | 21秒 | 提速19% |
这里“路径长度”之所以会变短，是因为平滑去掉了多余的“贴边绕行”；“转折次数”下降42%，对实体车的舵机压力直接减半。
4. 从仿真到实体：机器人侧适配
4.1 仿真环境验证怎么做
仿真不只是拿来画路径的，我总结了一套“三阶段验证法”，每一步都有明确通过标准：
第一阶段，单地图验证。随便手画两三张特殊地图——一张大片空地、一张死胡同多、一张螺旋形通道。A*必须全部走出有效路径。
第二阶段，随机地图压测。写一个迷宫生成器，随机摆放障碍物或生成完美迷宫，跑500张地图，统计成功率和平均耗时。当时测出来的平均规划耗时是12毫秒，最大不超过50毫秒，说明算法完全能满足实时调用。
第三阶段，动态扰动测试。模拟机器人在半路偏离路径的情况，把起点改成偏离点重新规划。这个场景对应实体车“打滑后回到正确航向”的需求。
顺便说一句，随机压测时发现BFS比A更“抗造”，因为BFS没有启发函数偏差，只要图能连通，必然能找到解。所以最终定的是“A优先，BFS兜底”，这个设计在答辩演示时很加分，展现了对异常场景的考虑。
4.2 路径点怎么转成运动指令
实体机器人底层是通过串口发速度指令的，比如向左转30度，前进50厘米。路径规划出来的点是全局坐标，而底层的控制是相对动作，中间要做坐标转换。
这里用的是一个很经典的增量式追踪方式：
def generate_commands(path, robot_state): commands = [] for i in range(len(path) - 1): delta_x = path[i+1][0] - path[i][0] delta_y = path[i+1][1] - path[i][1] target_angle = math.degrees(math.atan2(delta_y, delta_x)) angle_diff = target_angle - robot_state['yaw'] # 角度归一化到 -180~180 angle_diff = (angle_diff + 180) % 360 - 180 distance = math.hypot(delta_x, delta_y) commands.append({ 'rotate': angle_diff, 'move': distance }) robot_state['yaw'] = target_angle return commands这段代码有个隐蔽问题：差分驱动小车旋转时，左右轮速度相反，如果原地转90度，会绕着一个“瞬时旋转中心”原地掉头。实际测试中发现，原地转90度后位置会有3~5厘米的偏移，所以后续改了“边转边前进”的圆弧过渡方式，让车在运动中完成转向。
4.3 调参实录：膨胀半径、速度与转弯
实体调试是整个项目最花时间的部分，也是收获最大的部分。三个关键参数反复调了几轮：
膨胀半径：从1格调到2格后，“擦墙”问题基本消失，但代价是路经在一些狭窄通道里会绕远。比如迷宫有一条宽4格的通道，膨胀半径设为2格后，通道标记为全障碍，A*认为过不去，直接绕大圈。后来改成“膨胀半径只膨胀一个方向，另一侧贴近墙走”的非对称膨胀方案，才把这个问题解决。非对称膨胀简单说就是在转弯方向留出更多余量，直行方向靠墙贴着走。
速度：直行速度设0.3m/s时路径追踪误差在2厘米内，放到0.5m/s误差到了6厘米。原因是转向控制和速度控制共用同一套PID参数，速度上来后PID跟不上。后来给速度环和转向环分了两套参数，直行速度才提上去。
转弯策略：不要原地转，更不要“先急停再转弯”。正确的做法是预测性转向：提前2~3个路径点判断前进趋势，提前把前轮（或差速轮）的角度打过去。效果是转弯过程丝滑了很多，行驶总时间下降了25%。
5. 常见问题与排查技巧实录
5.1 问题速查表
| 现象 | 可能原因 | 排查方法 |
|---|---|---|
| 路径规划超时 | 障碍物把终点“包围”了，A*无解 | 检查地图连通性，加BFS兜底 |
| 路径贴墙太近 | 膨胀半径太小或未膨胀 | 增大膨胀半径，观察路径是否居中 |
| 规划出来的路径明显绕远 | 启发函数h值过大（高估） | 改为曼哈顿距离或欧氏距离，不要用距离的平方 |
| 路径不平滑，锯齿明显 | 缺少拐点简化和平滑后处理 | 加平滑算法，权重调到0.7：0.3 |
| 实体车直行时左右晃 | PID转向环参数太激进 | 减小转向环Kp，先稳定再求快 |
| 实体车转弯时漂移 | 转弯时速度过高或原地转 | 降低转弯速度，改为圆弧过渡 |
| 终点前卡住但不报警 | 终点坐标是路径点中心，车体中心到达才算 | 将终点判定改为“车体覆盖终点区域”即可 |
5.2 几个值得留意的细节坑
代码层面的坑大家都容易踩，我额外说三个“经验性”的坑。
第一，地图数据规格要统一。项目从仿真转到实体时，地图变了两次。仿真用的栅格是13厘米，实体迷宫变了，栅格尺寸也要变，但坐标转换代码里的栅格尺寸是写死的，导致机器人第一次实体测试时直接冲出迷宫边界。排查了一个多小时才定位到，其实是地图实际物理尺寸和程序里不一致。后面给坐标转换函数加了个参数校验，一旦传入的坐标超出地图范围，立刻打印警告。
第二，A的启发函数不一定要用曼哈顿距离。如果机器人允许斜着走（8方向），曼哈顿距离是严重高估的，会导致A贪心走入死角。这时候要用欧氏距离或切比雪夫距离。真实世界里的机器人几乎都可以斜着走，所以实际项目里建议直接用欧氏距离，能让A*的行为更贴近真实运动。
第三，实体调试时间不要全花在“调PID”上。PID参数不顺先怀疑机械和传感器——轮子是否打滑、编码器是否松动、地板是否太光滑。某次项目里小车走直线一直偏右，查了一圈是万向轮的螺丝松了，而不是PID有问题。这属于“先看硬件，再调软件”的典型教训。
最后再分享一个实践中的高效工作习惯：仿真和实体的代码库尽量共用，只把底层驱动接口替换掉。路径规划、地图处理、平滑算法这些纯算法模块，用一套代码就能跑在两个环境里。这样调试时，算法问题在仿真里改，硬件问题在实体上查，互不干扰，极大减少“环境不同导致结果不一样”的困惑。
机器人走迷宫这个项目做下来，最大的感受不是“我学会了几种算法”，而是你永远无法在仿真里预知实体世界的所有破事。但反过来，仿真给了你一个足够安全的训练场，让你可以把算法本身的问题全部暴露掉。等到实体调试的时候，你面对的只有一个问题源：物理世界的不确定性。这个分层调试的思路，后续做导航、避障、SLAM这些更复杂的方向时，都是同一个套路。希望这篇拆解能帮正在做路径规划的朋友少走几个弯路。
