做全覆盖路径规划这件事很多人最开始想的都是“怎么让机器人把每个格子都扫一遍”但真正在网格地图上把结果跑出来以后你会发现最花时间、最考功夫的往往不是扫地本身而是怎么从一个覆盖终点快速移动到下一个覆盖起点。这个移动过程就是转移路径规划也就是 A* 算法真正发挥作用的地方。这篇内容我围绕“A* 网格环境 往返式全覆盖 Matlab 实现”这条主线把整个方案的原理、断点提取、衔接顺序优化、工程实现细节和实测数据完整梳理一遍适合正在做移动机器人覆盖任务、搞栅格路径规划或者需要交一份 Matlab 仿真代码的读者参考。内容偏工程实践会有大量可以直接复用的代码思路和参数经验。1. 全覆盖任务为什么绕不开A*往返式方案的真实难点1.1 A*在覆盖任务中的角色定位往返式全覆盖路径规划也常被叫做牛耕式覆盖或割草式覆盖核心思路非常直白把地图按照一定方向切成一排一排的带状区域机器人沿着某一行从头扫到尾然后抬升到下一行的起点再反方向扫回来像犁地一样一层一层推进。这种规划方式在很多背景下都成立小区扫地机器人、农田植保、仓储巡检、船体除锈本质上都是这个逻辑的变体。但有个问题很少有人在一开始点明这个方法默认的前提是地图里没有障碍物或者障碍物可以被行人沿着行方向“跨过去”。一旦地图里出现墙体、柱子、设备区这些占地面积比较大的障碍物一整行的覆盖就会被切断变成一段一段的碎片。每一段都相当于一个独立的线性覆盖任务机器人完成这一段之后需要从这一段的末端移动到另一段的某个端头继续覆盖。这时候你面临的就是一个典型的“点对点最短路径”问题。这个点对点移动的最短路径就是 A* 的用武之地。换句话说A在这个项目里的角色不是“主路径生成器”而是“覆盖段之间的转移路径优化器”*。主覆盖骨架由往返扫描规则给出A* 负责把碎片重新缝起来。很多人做完整个仿真之后会觉得A* 在这里好像只是跑了个基础寻路没什么技术含量。但实际调过之后就知道转移路径的质量直接决定覆盖率、重复率、总航程这三个核心指标。转移走直线看着近如果中间穿墙A* 给出的绕行结果比直线长出一截新手往往想不通为什么规划出来的路径这么“弯”。后面我会详细展开为什么“看着近”和“走得通”在网格地图里是两回事。1.2 为什么不用BFS或直线连接既然只是点对点寻路为什么不用广度优先搜索 BFS或者干脆两点之间连一条直线先说直线连接。在无障碍的空旷地图里直线确实是最优的。但网格地图的障碍物通常是不规则多边形直线一旦撞上障碍栅格这条路线就完全不可用。要在直线被挡住时规划绕行路径本质上已经进入了寻路算法的领域绕不过去的。再说 BFS。BFS 确实能找到最短路径而且实现比 A* 还简单。但它的致命问题是探索范围太大BFS 会从起点出发一圈一圈地往外扩完全不考虑终点在哪一侧导致在连通性较好的开阔区域里它会浪费大量时间扩展与目标无关的节点。A* 多了一个启发式函数 h(n)等于在搜索时给每个节点一个“离终点还有多远”的估计优先扩展那些“已经走得短且预测还能更短”的节点搜索范围被大幅压缩。在小地图上可能感受不到差别一旦地图到 200×200 以上差距会非常明显。1.3 容易先踩的坑把A*当成“全覆盖主生成器”我刚上手这个方向时也犯过一个认知错误以为 A* 可以用来直接生成覆盖路径比如把“已覆盖状态”塞进状态空间里让算法自己去摸索怎么覆盖整张图。理论上不是完全不可行——把每个节点的状态定义成 (当前位置, 已覆盖位图) 之后确实可以套用 A* 框架来做全覆盖搜索。但实际跑一个 50×50 的地图就明白了已覆盖位图的组合数量是天文数字状态空间直接爆炸内存被撑满还是轻的大多数情况是连一次收敛都等不到。所以工程上几乎没有人在做静态地图全覆盖时用纯 A* 硬刚。更合理的分工是行扫描规则负责生成“覆盖段”断点选择策略负责确定“下一段是谁”A* 负责“怎么走过去”。这个三层结构是本文整个方案的主干也是最容易被复用到实际工程里的模式。2. 网格地图与A*实现细节邻域、启发式与代价设计2.1 栅格地图的数据结构与坐标约定Matlab 里做网格路径规划地图我一般用逻辑矩阵map(row, col)表示true表示障碍栅格false表示自由栅格。注意 Matlab 的矩阵索引是先行后列行坐标表示向下列坐标表示向右左上角是(1,1)。这和常见图像坐标系一致但和画图时用的plot(x, y)坐标系不同——plot里第一个坐标是横轴对应列第二个坐标是纵轴对应行。如果你直接把map拿来画图不做坐标转换会发现整张图是上下颠倒的。我常用的约定是这样的% 地图尺寸 [rows, cols] size(map); % 逻辑坐标 - 物理坐标(r, c) 对应 plot 中的 (c, rows - r 1) % 物理坐标 - 逻辑坐标令 x c, y rows - r 1则 r rows - y 1, c x除了基础地图还要准备两个等尺寸矩阵covMap记录哪些自由栅格已经被覆盖0 未覆盖1 已覆盖visitedMap记录 A* 搜索过程中节点是否已经扩展过避免重复访问。这几个矩阵在代码里维度都一样完全可以用逻辑索引做批量操作比写双层循环快得多。比如统计覆盖率freeCnt sum(~map(:)); coveredCnt sum(covMap(:) ~map(:)); coverage coveredCnt / freeCnt * 100;2.2 八邻域与对角线代价网格环境下机器人每一步能朝哪些方向移动直接改变路径长度和灵活性。全覆盖任务里我几乎总是用八邻域而不是四邻域原因很简单四邻域下机器人从一个栅格只能走上下左右路径转向角度只有 90°走起来很僵硬八邻域允许斜向移动转移路径更平滑覆盖段之间的衔接也更自然。代价设置上水平/垂直移动代价 1对角移动代价 sqrt(2)也就是 1.414。这样做的好处是路径长度直接对应真实的物理距离g(n) 计算出来的数值跟地图上的实际距离是同一个量纲后面统计总航程、重复率都比较直观。有一点容易被忽略八邻域下的“斜穿角”问题。在栅格地图里如果当前格子和对角目标格子都是自由栅格但中间的拐角栅格是障碍直接斜穿过去在视觉上会“擦着墙走”很多实际机器人并不允许这种运动。处理方式通常有两种严格模式判断斜向移动时要求相邻的两个正交栅格也必须是自由的否则禁止该方向移动宽松模式不做额外判断只要目标栅格自由就走。全覆盖任务的机器人一般体积比栅格小或者希望尽可能提高灵活性我建议用严格模式。这个判断在isValidMove函数里加几行就能实现不会增加多少计算量但能避免很多后续路径合法性上的争论。注意四邻域没有斜穿角问题如果实现的是四邻域版本则无需考虑。2.3 启发式函数的选择A* 的搜索效率很大程度上依赖启发式函数 h(n)。它衡量的是“从节点 n 到终点的估计代价”。如果 h(n) 始终小于真实最小代价A* 保证找到最优解h(n) 越接近真实值搜索扩展的节点越少如果 h(n) 在某些情况下大于真实代价算法会倾向于走一条看似很近但并非最短的路径就失去最优性保证。对于八邻域地图最合适的启发式是切比雪夫距离h(n) max(|r - goalR|, |c - goalC|) * 1但要注意八邻域中最小单步代价是 1而切比雪夫距离计算出来的是“以最小单步代价移动需要的步数”两者相乘得到的才是符合代价定义的启发值。如果是对角线移动代价统一设为 sqrt(2)情况会复杂一些此时更稳妥的是使用欧几里得距离h(n) sqrt((r - goalR)^2 (c - goalC)^2)欧氏距离在任何邻域定义下都小于等于真实最短路径代价是最安全的选择。缺点是它比真实代价偏低搜索扩展量略多一些。如果地图规模不大直接用欧氏距离写代码最简单、不容易出隐性问题。我实测过的一种组合是八邻域、对角代价 sqrt(2)、启发式用切比雪夫距离乘以 sqrt(2)。在多数障碍地图上效果都不错扩展节点数比欧氏距离少 20%~40%。但如果地图里存在大量长走廊、死胡同切比雪夫距离可能对某些区域高估偶尔导致次优路线。所以在工程项目里我一般默认用欧氏距离追求效率时才切换成切比雪夫。启发式函数选择对比邻域类型常用启发式是否保证最优搜索范围四邻域曼哈顿距离是中等八邻域正交1、对角1.414欧氏距离是偏大八邻域正交1、对角1.414切比雪夫×1.414多数时候较小八邻域统一步数代价切比雪夫×单步代价是小2.4 权重系数与应用扩展A* 还有一个很常用的变体加权 A*。把评价函数改成f(n) g(n) w * h(n)当 w 1 时算法会更激进地向终点方向搜索扩展节点数量大幅下降代价是路径可能比最优解长 5%~10%。在覆盖任务里转移路径多绕几步通常无关紧要而搜索速度却非常重要尤其在栅格地图规模大、断点数量多的时候。我的调参经验是地图小于 100×100直接把 w 设为 1求精确最优地图到了 200×200 以上或者 A* 需要在线频繁调用时w 取 1.1~1.3速度提升明显路径质量损失可以接受。顺便提一个扩展方向如果机器人不是质点而是有一定几何尺寸可以在代价函数里增加“靠近障碍物的额外代价值”让 A* 自动生成偏向安全区域的路径。公式可以写成cost baseMoveCost obstaclePenalty coverageBias其中obstaclePenalty根据当前栅格周围障碍密度计算coverageBias后面会讲到用于控制转移路线尽量走在已覆盖区域降低整体重复率。3. 往返式覆盖路径的分段生成与断点提取3.1 逐行扫描生成覆盖段往返式全覆盖的第一步是把地图扫描成覆盖段。我使用的方法是从上到下逐行遍历地图在每一行内从左到右扫描。每行内连续的自由栅格组成一个覆盖段。遇到障碍栅格则终止当前段开始记录下一段。最终得到一组带状覆盖段每个段包含所在行号起始列结束列左端点坐标和右端点坐标。伪代码逻辑如下function segs generateScanSegments(map) [rows, cols] size(map); segs []; for r 1:rows c 1; while c cols if map(r, c) c c 1; continue; end % 找到连续自由栅格段 cEnd c; while cEnd cols ~map(r, cEnd) cEnd cEnd 1; end cEnd cEnd - 1; % 记录覆盖段 segs [segs; struct(... row, r, startCol, c, endCol, cEnd, ... left, [r, c], right, [r, cEnd], ... covered, false)]; c cEnd 1; end end end这个步骤看起来简单但它其实把“二维全覆盖问题”转换成了“一维线性单元的组合调度问题”复杂度降低了一个量级后面的一切衔接优化都建立在这些线段上。覆盖段左右端点是机器人进入该段的入口候选位置。为什么是两端而不是中间因为覆盖段是连续直线带状区域机器人最优的覆盖方式是沿行方向横扫过去。从中间进入意味着得先走到某一端掉头反而增加路程所以两端是自然的入口集合。3.2 断点列表与离线最短路径矩阵所有覆盖段的端点合在一起就构成了断点列表。每个断点有两个身份它既是一个覆盖段的入口也是上一个覆盖段的出口。设计者的核心任务是在所有断点之间找到一条覆盖顺序让机器人在执行完所有段后总路程最短。这本质上是一个旅行商问题TSP不同的是机器人访问的不是任意点而是“成对的端点”访问一个端点后覆盖段本身必须随之覆盖覆盖段的两个端点互相之间关系强绑定。直接求解 TSP 的精确算法复杂度极高覆盖段数量一旦超过 20 个就非常吃力。所以工程上用的是近似解法——贪心最近邻 局部优化。而贪心最近邻要评估“从当前断点到候选断点的路径代价”这一步就需要先算出所有断点对之间的最短路径。断点对之间的最短路径是通过 A* 批量预计算的。算法流程是提取所有覆盖段的左右端点组成端点列表对每两个端点运行一次 A*得到最短路径长度把结果填入代价矩阵D(i, j)若某对端点间不存在可行路径D(i, j)记为 Inf后续选择时自动跳过该组合。这一步是整个方案中离线计算量最大的部分。端点数 N 个A* 要跑 N×(N-1)/2 次。地图越大、断点越多耗时越长。我通常在正式跑覆盖顺序之前先把代价矩阵算好缓存起来避免在顺序优化过程中反复调用 A*。如果觉得全量预计算太耗时可以只计算“当前断点出发到剩余所有未覆盖段端点”的行动态扩展但要注意维护已计算部分。对于 100×100 的地图全量预计算大约需要几百毫秒到几秒完全在可接受范围内。3.3 可达性预判与孤立区域处理有一个很隐蔽的问题某些覆盖段被障碍物完全包围或半包围从某些起点出发可能到达不了或者要绕非常远的距离才能到。如果不做可达性预判覆盖顺序优化时会把这种段排到很靠后的位置导致机器人最后走了大量重复路却依然覆盖不到。我在项目里用了一个前置步骤用 BFS 或 A* 判断每个覆盖段是否与起点所在连通区域相连通。具体做法从机器人的初始位置出发做一次区域生长BFS 遍历自由栅格标记所有可达的自由栅格检查每个覆盖段的两个端点在不在可达集合内。如果某个覆盖段完全不可达就直接从覆盖列表中剔除并记录进不可达段。这样覆盖率和“理论上可覆盖率”就能分开统计后面分析实验结果时才不会混淆算法问题与地图连通性问题。对可达但代价很高的覆盖段我建议把它的优先级提高而不是降低因为它是全图最“难啃”的部分越到后面越容易被困在局部区域。实测下来把孤立岛状区提前插入覆盖序列总航程能减少 10% 以上。4. 断点连接顺序优化双端扩展与最近邻策略4.1 贪心最近邻思路假设现在机器人在某个断点 A剩余 N 个未覆盖段。贪心最近邻的策略是从 A 出发选择离 A 路径代价最小的那个未覆盖段的任意一个端点作为下一个目标走过去然后更新当前位置为该段的对侧端点再重复这个过程。这个思路非常直觉实现也快但有个明显缺陷它只考虑“走过去这段路”不考虑“进入这个段后段本身要走多长、出来后在哪个位置”。两个距离很近但方向相反的覆盖段走完一个后发现出口离另一个很远总路程反而长。所以我在项目里实际用的是双端扩展策略比纯最近邻效果好很多。4.2 双端进入优化选择最佳进入端双端扩展核心思路是对每个未覆盖段分别计算“从当前断点 A 到达该段左端点的最短路径代价”和“从当前断点 A 到达该段右端点的最短路径代价”。然后取两者中较小值再叠加该段的自身长度作为加入该段的“实际代价”。这样做的理由很朴素同一段可以从左端进也可以从右端进进法不同走出该段后落点不同直接影响下一跳的起点位置。所以“去一段的距离”和“段自身长度”必须绑定在一起评估。具体选择公式for each segment s in remaining: costLeft D(A, s.left) s.length costRight D(A, s.right) s.length cost(s) min(costLeft, costRight) choose s* argmin cost(s)进入段后机器人沿段方向覆盖从对侧端点出来。如果选择左端进入那么覆盖完后的当前位置是右端反之则是左端。这个出口端点同时是下一轮迭代的“当前断点”。当前断点 A ↓ A* 转移到 s* 的左端/右端 ↓ 沿段覆盖 当前断点 s* 的对侧端点用这个策略跑 50×50 地图、30 个覆盖段时总航程比纯单端最近邻平均能低 8%~15%。尤其在障碍物分布不均匀的地图里收益更明显。双端的额外开销只是每个候选段多查一次代价矩阵计算量完全可忽略。4.3 转移路径上的重复覆盖控制全覆盖任务里覆盖率不是唯一指标重复率同样重要。机器人沿着 A* 规划出的转移路径前进时有可能路过尚未覆盖的自由区域——这条路径走完之后那些区域就算被顺路覆盖了。对指标的影响是机器人实际执行路线中有一部分路程和主扫描路线重叠总量变成重复覆盖。工程处理上有两种思路顺路覆盖思路不主动规避未覆盖区域把 MACRO 覆盖理所当然地当作转移过程的一部分路径更短总耗时也更低但重复率指标会相对不好看严格分离思路A* 路径尽量贴着已覆盖区域边界走避免触碰未覆盖自由栅格。我实测的结论是全覆盖任务模型里通常取前者。因为“重复率”在实际任务中的定义本来就存在争议——从概率覆盖的角度机器人只要走过某个栅格就算覆盖过一次真正有害的重复是“同一栅格被扫三遍以上”或者“转移路径在已覆盖区域内部来回绕”。顺路覆盖恰好属于“低效率但不高害”的行为。如果确实需要降低重复率一个可行技巧是在 A* 代价函数里加上一个覆盖状态偏置项cost cost coverageBias * (1 - covMap(r, c));这个偏置让路径计算时更倾向于走已覆盖栅格。coverageBias 我一般设为 0.1~0.3太高会影响路径长度太低没效果。只跑一遍覆盖的话建议设置为 0.1 或干脆不加需要严格压低重复率时再加。4.4 失效链与兜底策略双端扩展也存在退化场景。当一个覆盖段被选入路径后如果它实际上是死胡同——只有一端能进出另一端被障碍物完全堵住——那么进得去、出不来整个流程就断了。典型例子是“L”形走廊尽头的覆盖段。兜底策略需要实现两种情形若某个段从当前断点出发只有一个端点可达另一个端点 A* 返回 Inf则强制从可达端点进入覆盖完后返回进入端点再原路折返若两个端点都不可达则跳过该段保留到下一轮外围循环尝试一次。这套机制在普通矩形地图里触发概率不高但一旦触发就是“要不要卡死”的区别。加了它之后整个规划流程的鲁棒性会明显提升。5. Matlab实现要点与性能优化5.1 工程结构设计我推荐将整个工程拆成几个独立的函数模块这样后期替换算法、调整参数、复现实验结果都很方便main.m % 主入口初始化、调用各模块、输出统计 buildMap.m % 地图生成随机障碍物或手工绘制 generateScanSegments.m % 扫描段生成 computeCostMatrix.m % 断点间 A* 离线代价矩阵 planCoverageOrder.m % 双端贪心顺序规划 astarPath.m % 单次 A* 寻路 drawResult.m % 可视化地图、覆盖轨迹、统计信息这样分层的另一个好处是你后面想换掉贪心策略改成动态规划或遗传算法来优化覆盖顺序只需要替换planCoverageOrder.m一个文件其他模块完全不用动。5.2 open list 的两种实现方式A* 的性能瓶颈主要在 open list 的节点排序上。Matlab 没有现成的优先队列数据结构常见做法有两种直接用结构体数组每次弹出最小 f 值时用sortrows排序手写一个最小二叉堆。我实测过节点数少于 2000 时sortrows完全够用代码也短但地图超过 100×100、A* 要高频调用时排序开销占比会非常高此时二叉堆优势明显。关于二叉堆的实现我建议直接封装成MinHeap类里面有push、pop、isEmpty三个方法就够了不需要写太复杂。小技巧为了减少频繁的堆操作可以在扩展邻居前先查visitedMap再查当前节点是否已在 open list 里避免重复入堆。5.3 矢量化与矩阵预分配Matlab 最忌讳的是在循环里动态扩充数组路径规划代码尤其容易踩这个坑。A* 的 open list 虽然看似必须动态增长但可以用逻辑矩阵 节点 ID 的方式来规避形态问题给每个栅格一个唯一 IDid sub2ind([rows, cols], r, c)用数组gScore和fScore记录每个栅格对应的代价值Inf 表示未计算用逻辑矩阵visitedMap记录已扩展节点。这样每一步更新代价都是 O(1) 的矩阵赋值不需要动态增长。唯一的缺点是每次要从所有未访问节点里挑 f 最小的这在密集地图上会比较慢。折中方案是维护一个节点 ID 数组只在扩展时截断不逐格删除结合fScore数组批量找出最小值。这部分属于优化细节有性能需求的读者可以尝试。map、covMap、gScore、fScore这些矩阵全部在进入循环前预分配好用Inf填充fScore比在循环里每次判断ismember快得多。5.4 可视化技巧调试全覆盖算法最直观的方式是画出完整路线。我的画法分三层用imagesc(map)显示地形障碍栅格深色自由栅格浅色用hold onplot画出覆盖轨迹线宽设为 1.2在覆盖段端点上用小圆圈标注断点位置次序高亮。如果想实时观察覆盖进度可以在主循环里每隔几帧调用一次drawnow配合pause(0.05)。注意不要每次都重绘地图底图用handle imshow()获取图像句柄后循环里只更新轨迹句柄的XData和YData刷新效率会高很多。刚开始调试时很容易把整个地图画一遍路径一长图形卡顿明显这个细节对体验影响很大。5.5 实测结果与参数参考我用 40×30 的地图做了几组实验障碍物随机生成占比约 8% 和 30% 两档每次随机种子不同各跑 10 次取平均。数据结构为八邻域、对角代价 sqrt(2)、欧氏距离启发式、w1硬件为普通笔记本电脑Matlab 版本 R2023b。典型结果大致如下地图规模障碍占比覆盖段数总航程/栅格转移路程占比覆盖率解算时间40×308%12~181820~210018%~25%99.4%0.15s40×3030%20~282150~260035%~42%97.8%0.25s100×8010%55~80约950022%~30%98.9%2.8s障碍率升高后覆盖段被切得更碎转移路程占比明显上升这正好印证了前面的判断碎片的衔接是整个任务的真实瓶颈。覆盖率达不到 100% 的原因是角落存在单栅格宽的死角A* 和行扫描的栅格粒度决定了这类区域无法处理。如果项目里要求严格 100%需要额外引入局部螺旋覆盖或转弯半径补偿逻辑这属于扩展内容主流场景下不是必选项。数值给出来供参考换地图、换障碍率后结果会有浮动但趋势是一样的。抵达率 100% 完全是可行的只要地图不存在孤立不可达区域。写在最后的工程体会这套“A* 转移 往返式覆盖段 双端贪心”的结构我在好几个项目里都用过。最初也是最容易掉进去的坑就是急着写 A* 寻路忽略覆盖段的抽象建模。实际上 A* 本身的代码半天就能调通后面真正的工程量在“段的生成”“可达性预判”“顺序优化”这三块它们的代码量加起来往往是 A* 主体的好几倍。如果你也在做类似方向建议把算法调试和实验验证的重心后移先把覆盖段和断点数据结构设计好后面的所有优化都是在这张 “骨架”上长肉会很顺。