第一次拿到AGV全覆盖避障路径规划这个课题的MATLAB代码需求时我差点走偏——本能地翻开A*和Dijkstra的旧笔记。后来真正把扫地机器人放到一个塞满办公桌椅的房间里跑了一遍才彻底明白点到点导航和全覆盖路径规划根本是两码事前者关心怎么走到后者关心怎么走全。从家庭到商场覆盖的完整度才是清洁机器人最核心的KPI没有人愿意花两小时看机器人在屋里反复绕圈结果客厅角落还是漏的。标题里的第一类算法在业内通常指不依赖随机搜索的确定性全覆盖方法也叫牛耕式/单元分解类算法这正是商业扫地机弓字形清扫的底层逻辑。这篇文章我把栅格建模、单元分解、牛耕遍历、遇障回退到跳转补扫的整个链路拆开讲并提供MATLAB可运行的代码骨架适合做毕设、参加机器人比赛或刚接触AGV全覆盖规划的朋友作为起点。1. 全覆盖路径规划的本质为什么扫地和导航是两回事1.1 扫地机器人真正要解决的问题不是怎么走而是怎么走全在学术定义里全覆盖路径规划Complete Coverage Path PlanningCPP指的是在给定自由空间后求一条连续轨迹使机器人携带的作业工具清扫盘、割草刀、喷头走过的覆盖区域能覆盖整个目标空间同时全程满足无碰撞约束。这句话初看平平无奇真正写代码时才感受到分量。A规划的是一条从起点到终点的线难点在搜索效率CPP规划的是一整片区域的访问顺序难点在无遗漏。想象一下一个100×100的栅格地图A只需找到一条几百步的路径CPP则要决定上万个格子的访问顺序。这个顺序编排问题本质上非常接近哈密顿路径问题的变种——在栅格图上找到一条尽量不重复访问所有空闲格的路径。众所周知这类问题在大规模地图上想求全局最优计算量会爆炸所以工程上不会去做全局搜索而是用分解模板的贪心策略来逼近。另一个容易忽略的点是KPI设置。企业验收清洁机器人第一眼看的指标是覆盖率一般要求达到98%以上低于95%基本就不合格接着才是重复率、路径长度和耗时。很多新手在博客里晒出来的路径图看着很漂亮一圈圈螺旋但覆盖率只有90%这种方案在实际项目中是过不了验收的。所以做这个课题第一件事就是把目标刻在脑子里覆盖率是硬指标路径漂亮是次要的。1.2 第一类算法的准确定位确定性的全覆盖方法关于标题里第一类算法全覆盖智能算法这个说法不同资料里的归类口径不太一样但业内比较常见的分类方式是按照是否依赖随机搜索或迭代优化分成三类。算法类别代表方法核心思想适用场景第一类确定性全覆盖方法牛耕式/弓字形、内螺旋全覆盖、梯形分解、牛耕分解空间分解 固定扫描模板路径可复现地图已知或局部已知对实时性要求高的商用扫地机第二类智能优化全覆盖方法遗传算法、蚁群优化、粒子群算法优化的覆盖顺序用迭代搜索手段优化覆盖序列和方向离线规划、复杂不规则区域、追求更低重复率第三类随机/在线覆盖方法随机漫游、基于传感器前沿的在线覆盖没有完整地图也能覆盖不保证覆盖率未知环境、灾难救援机器人、验证性质的原型很多论文也把第一类方法称为启发式智能算法因为它内部的单元分解、方向选择、回溯规则本身就属于启发式决策。不必在这种命名上过度纠结重要的是认清第一类算法的特性它不需要大量的随机迭代计算量小实时性强路径稳定可复现所以商业产品的实时清扫几乎都跑在这类算法上。第二类算法更多用在离线场景或者在第一类结果之上做路径优化。这篇文章聚焦第一类算法把它的每一个决策规则都讲透。1.3 全覆盖搜索空间和A*完全不在一个量级我把同一张地图分别用A和CPP在MATLAB里跑过一次感受非常直观。A的搜索节点数是几千级别在100×100栅格上毫秒级就出结果而CPP的路径是一条访问了上万个格子的序列任何一个格子的访问时机都影响后续决策。更关键的是覆盖顺序选得差重复率会从5%飙到30%能耗跟着翻倍。这意味着直接套用图搜索启发函数的思路是行不通的。CPP必须把问题拆成两层第一层是区域拆分把自由空间切成若干个不包含障碍物的子区域第二层是子区域内部扫描在每个子区域里用弓字形模板跑满再把子区域之间的跳转路径接起来。这就是第一类算法的整体框架。接下来的内容就是沿着这个框架一步步落地。2. 环境建模栅格地图构建与参数选择的门道2.1 从真实世界到二进制栅格地图无论算法多高端第一步都必须把环境变成机器能读的数据。在MATLAB里做全覆盖实验最常用的环境模型是占据栅格地图Occupancy Grid Map把二维空间切成均匀网格每个格子标记为空闲或占据。地图来源通常有三种。第一种最省事直接手工构造矩阵做算法验证1表示墙/障碍物0表示可通行区域。第二种是用MATLAB的occupancyMap类加载激光SLAM建好的地图文件适合有真实机器人平台的人。第三种是针对工厂AGV项目把CAD图纸转成图片后栅格化再手工修正。对于刚起步的初学者我强烈建议从手工矩阵开始先把算法跑通再换真实地图否则你会分不清bug是出在地图处理上还是算法上。一个合格的手工测试地图至少包含这些元素外墙、一个矩形障碍、一个L形障碍、一个窄通道。代码骨架如下mapRow 60; mapCol 100; gridMap zeros(mapRow, mapCol); % 外墙 gridMap(1, :) 1; gridMap(mapRow, :) 1; gridMap(:, 1) 1; gridMap(:, mapCol) 1; % 矩形障碍 gridMap(20:30, 40:50) 1; % L形障碍 gridMap(40:46, 60:80) 1; gridMap(40:50, 60:66) 1;这里只有一个陷阱值得提醒gridMap中的占据格代表的是障碍物本体占据的格子而机器人是有体积的它的中心不能贴着障碍物边界走所以第一步必须做障碍物膨胀。2.2 障碍物膨胀用一个圆盘结构元素解决问题机器人中心点模型在工程上非常常用。做法是假设机器人是一个点然后把所有障碍物按照机器人半径向外膨胀一圈膨胀后的地图表示机器人中心不能进入的区域。这样碰撞检测就从复杂的几何求交问题退化成了查表问题。MATLAB里用形态学膨胀一行就搞定se strel(disk, R, 0); inflatedMap imdilate(gridMap, se);关键在于R的计算。假设机器人清扫盘直径0.4m网格尺寸0.1m安全余量0.05m那么R floor((0.4 / 2) / 0.1) floor(0.05 / 0.1) 2 0 2也就是对障碍物做半径为2格的圆形膨胀。膨胀之后原来贴障碍物边缘的可通格会被标记为障碍机器人中心路径自然就和障碍物保持了一段安全距离。这里要特别说明一个常见误解膨胀后的地图禁止的是机器人中心点不代表机器人整体不能穿过某些区域。比如一条宽度为0.8m的走廊机器人直径0.4m膨胀后中心剩余通道宽度尚有0.4m机器人能过但如果走廊宽度只有0.5m膨胀后中心通道完全消失机器人就过不去了。2.3 栅格分辨率太粗漏角落太细卡性能栅格尺寸的选择直接影响覆盖质量。我试过三组参数差异非常明显分辨率0.05m时边角覆盖精细但地图栅格数量变成原来的4倍规划时间暴涨而且重复路径多了不少分辨率0.2m时规划飞快但墙角、桌腿缝这些位置经常会因为整格被视为风险区而漏扫。网格尺寸优点缺点0.05m边角覆盖精确状态空间大重复率高实时规划吃力0.1m商用扫地机常用精度和性能均衡窄间隙偶尔被合并0.2m规划快适合演示边角漏扫明显覆盖率虚高经验值是在物理网格尺寸上取清扫盘宽度的一半到清扫盘宽度之间。比如清扫盘宽0.35m网格取0.1~0.18m比较合理。网格再细对覆盖率没有实质提升反而让路径步数变多执行时间和能耗都上去了。3. 第一类算法核心实现思路牛耕式、单元分解与回溯3.1 牛耕式遍历为什么所有全覆盖算法都绕不开它牛耕式Boustrophedon全覆盖小名弓字形或回字形名字来自古希腊人耕地的方式牛从田地一边犁到另一边掉头再犁回来来回反复直到整块地犁完。对矩形自由空间来说这个策略极其稳定从起点沿主方向走到边界转90度到相邻行反方向再走如此往复。由于行与行之间没有缝隙覆盖完整度天然接近100%又因为相邻行反向衔接机器人的转弯次数被压缩到最小。这里有一个方向选择问题值得展开到底沿长边扫描还是沿短边扫描答案是尽量沿较长边扫描。假设房间20m×10m清扫宽度0.5m沿长边扫描需要掉头10/0.5-119次沿短边扫描需要掉头20/0.5-139次。转弯不仅耗时间还伴随额外的电机电流冲击和地面标记轨迹偏移所以优化掉头次数是全覆盖路径规划一个永远的主题。矩形房间的代码实现很直白pos [startRow, startCol]; direction 1; % 1向右-1向左 while coveredCount targetCount % 标记当前格 if ~covered(pos(1), pos(2)) covered(pos(1), pos(2)) true; coveredCount coveredCount 1; end % 沿主方向前进 nextPos pos [0, direction]; if canMove(nextPos) pos nextPos; continue; end % 走到边界换行 nextRow pos(1) 1; if nextRow mapRow canMove([nextRow, pos(2)]) pos [nextRow, pos(2)]; direction -direction; continue; end break; % 整块区域扫完 end这套骨架虽然简单但它把全覆盖算法最核心的直行优先、边界换行、反向回扫逻辑体现得很清楚。复杂场景的所有变体都是在这个骨架上加障碍判断和跳转逻辑。3.2 单元分解把塞满障碍物的房间变成一串可扫的伪矩形现实房间不可能都是空旷矩形茶几、沙发、柱子把空间割得七零八落。如果直接在整张地图上做简单弓字形扫到障碍物面前就得停一停就可能漏掉障碍物另一侧的区间路径会碎得非常难看。解决办法是把自由空间拆成多个互不重叠的子区域每个子区域尽量接近矩形子区域内部用弓字形扫满子区域之间用跳转路径连接。经典的分解方法有两种。梯形分解的做法是从每个障碍物顶点向地图边界做竖直延伸线把自由空间切成一串梯形。梯形虽然不是矩形但只要上下底平行牛耕式路径依然能覆盖。牛耕分解则是只在凹顶点处做竖直切割生成的分区数量比梯形分解更少跳转次数也相应减少因此论文里更常见。对于MATLAB快速实验我推荐一种更简单的近似实现基于行区间的分解逐行扫描地图把每行的可通行格子切成若干个连续区间段。比如第10行 [1:35] 和 [50:88] 是两个区间第11行 [1:35] 和 [50:88] 还是两个区间那么这两行同序号区间就可以拼成同一个子区域的局部。当障碍物导致区间段数量变化时就在变化的那一行切一条分界线上一个子区域结束新的子区域开始。这是最直观、最容易调试的分解版本。3.3 回溯跳转扫到死路之后的救场逻辑牛耕式单元分解能覆盖大多数情况但总有些尴尬局面当前行的下一行对应位置是墙无法直接换行或者子区域边界到了剩下的未覆盖区域在远处。这时机器人需要主动放弃当前行走到最近的未覆盖栅格继续扫。这个动作在工程上叫回溯Backtracking。我把逻辑拆成三步在当前格周围搜索最近的未覆盖空闲格。搜索范围先限制在附近5行以内找不到再扩大到全图。用广度优先搜索BFS计算从当前位置到目标格的最短可行路径。沿路径移动移动过程中路径上的格子会被标记为已覆盖——这会产生重复覆盖代价但比漏掉一整块区域划算得多。用耕地来类比牛耕式是犁地回溯就是犁到田尽头发现隔壁垄还没犁于是提着犁跨过去。这段跨过去的路虽然被重复踩了但总比那块地荒着强。回溯实现里必须注意搜索范围限制。如果每次回溯都全局搜索机器人可能会反复横穿整张地图重复率爆炸。我在实验中踩过这个坑不加范围限制时覆盖率98%重复率却高达43%。限制为局部搜索后重复率降到15%左右。4. 避障决策的细节静态障碍提前进地图动态障碍分情况处理4.1 静态避障的真正含义规划即避障很多新手一听到避障两个字就以为是在机器人运动过程中实时绕开障碍物。其实对已知环境里的静态障碍物正确的处理方式早在第2章就完成了——障碍物膨胀之后栅格地图里已经没有贴着障碍物边缘的格子了全覆盖规划器只需要把占据格视为不可进入路径自然就不会撞上墙。这也是为什么我把避障拆成两部分来讲。静态障碍的规避写在地图预处理阶段动态障碍的规避写在实时决策阶段。前者是离线计算后者是在线反应。如果两者混在一起做代码会又慢又乱。4.2 动态避障临时出现的障碍物要怎么应对商用扫地机器人在实际环境中会遇到人、宠物、掉落的袜子、临时搬来的椅子。这些问题没法预知只能靠传感器实时发现。对全覆盖规划器来说处理动态障碍有三种策略。场景推荐策略理由行人短暂经过减速等待或局部绕行几秒钟的事情全局重规划不划算长时间停留的新障碍物屏蔽该区域标记为占据格避免反复撞同一个位置障碍物离开后恢复该区域为空闲并标记未覆盖下次补扫保证覆盖率不降核心原则是不要一遇到动态障碍就全局重新规划。重新规划意味着抛弃当前覆盖进度状态重新计算计算开销大而且已经覆盖的区域还得重复覆盖一遍。正确做法是把动态障碍物当作临时占据格只影响局部决策当前格被堵就尝试绕过它绕不过去就先做回溯跳转等障碍物消失后再回来补扫。4.3 运动方向优先级的贪心规则全覆盖算法在主循环内部是一条决策链我心里一直维护一个优先级表当前方向直行成本最低同侧换行反向进入相邻行局部回溯限制搜索范围全局回溯最后手段这个优先级表不是随便定的它背后是运动学代价。对差速驱动机器人直行几乎不消耗额外转向时间换行需要一次90度转弯回溯跳转则可能要走一段S形路径。如果贪心规则写反了比如遇到一堵不长的墙就立刻回溯机器人会在两个区域之间来回弹跳路径丑重复率也高。我调试时习惯把贪心优先级打印出来看发现大多数覆盖率异常都出在回溯触发得太早上。正确的做法是在同一个子区域内就算当前方向被堵也要先尝试上下相邻行能不能走实在走不了才允许跳出子区域。5. 从原理到MATLAB代码骨架数据结构、主循环与可视化5.1 数据结构把地图、覆盖状态、当前位置分开存写全覆盖算法最容易踩的坑是把地图和覆盖状态混在同一个矩阵里。一旦混在一起回溯搜索时会连障碍物和已覆盖区域都分不清调试非常痛苦。我的习惯是维护三份独立数据% freeMap: 逻辑矩阵true表示机器人可以进入已做膨胀处理 % covered: 逻辑矩阵true表示已经被覆盖过 % pos: [row, col]机器人当前所在格判断下一步能不能走只看freeMap判断这格有没有扫过只看covered。两个矩阵交叉就能得到需要补扫的区域——这是回溯搜索的候选集合。5.2 主循环直行、换行、回溯三件套完整的主循环代码骨架如下。为了篇幅不失控我这里给出核心逻辑辅助函数用调用形式标注covered false(size(freeMap)); targetCount nnz(freeMap); coveredCount 0; pos startPos; mainDir 1; maxRetryStuck 0; while coveredCount targetCount % 1. 标记当前格已经覆盖 if freeMap(pos(1), pos(2)) ~covered(pos(1), pos(2)) covered(pos(1), pos(2)) true; coveredCount coveredCount 1; end % 2. 沿主方向尝试直行 nextPos pos [0, mainDir]; if canMove(nextPos, freeMap) pos nextPos; continue; end % 3. 尝试换行反向进入相邻行 if trySwitchRow() pos newPos; mainDir -mainDir; continue; end % 4. 局部回溯找最近未覆盖格 targetPos searchNearestUncovered(pos, covered, freeMap, searchRange); if isempty(targetPos) % 扩大搜索范围再找一次 targetPos searchNearestUncovered(pos, covered, freeMap, inf); end if isempty(targetPos) break; % 已全部覆盖 end path bfsPath(pos, targetPos, freeMap); pos followPath(path, covered, coveredCount); end这个逻辑有一个显著特点它不依赖任何全局优化器每个决策都局部、贪心、可解释。可解释性在工程调试里非常重要——覆盖率不对时你能精确看出来是换行规则写错了还是回溯范围太小而不是对着黑盒遗传算法一头雾水。5.3 可视化必须全程看着机器人扫再看数字调参全覆盖算法和A不同A输出一条路径你看一眼端点就知道对不对全覆盖输出一整片轨迹密密麻麻的线条单看数字根本判断不了哪里出了问题。所以可视化不是可选项是必需品。function plotProgress(freeMap, covered, pos, pathHistory) clf; % 0:未知/墙(黑) 1:空闲(白) 2:已覆盖(灰) displayMap freeMap 2 * covered; imagesc(displayMap); colormap(gray); hold on; plot(pathHistory(:, 2), pathHistory(:, 1), r.-); plot(pos(2), pos(1), go, MarkerSize, 10, MarkerFaceColor, g); axis equal; xlim([1, size(freeMap, 2)]); ylim([1, size(freeMap, 1)]); drawnow; pause(0.02); end调试时把pause(0.02)去掉改成每步暂停0.2秒慢速播放你会看到机器人是怎么决策的。很多覆盖率问题慢放一眼就能看出来比如机器人在某个凹角反复进出比如回溯路径横穿了半张地图。这些现象在数据统计里根本看不出来。6. 常见坑位与实测调优覆盖率上不去多半是这几种情况6.1 U型陷阱三面墙封住去路之后怎么出来U型区域是全覆盖算法最经典的陷阱。机器人沿着行扫进U型区域底部前方是墙上方是墙下方也是墙退回去又等于回头扫。如果搜索回复里没有回退到入口的设计机器人会在底部无限循环或者直接把U型区域排除在覆盖区之外。我的解决方法是加一条卡死检测统计最近连续N步我取40步新增覆盖格的数量如果新增覆盖格数量为0说明机器人被困在当前子区域强制触发全局回溯先回到已知的未覆盖区域入口再重新进入。实测中这个规则比单纯的搜索最近未覆盖格更稳因为它主动打断了局部死循环。6.2 窄通道与膨胀半径的冲突重复率爆炸的元凶膨胀半径设定太大时两个相距很近的障碍物之间的缝隙会被膨胀操作完全填平原本连通的区域被切成两个独立连通分量。全覆盖算法被迫在连通分量之间频繁跳转每跳一次都要走一段重复路径重复率直接翻倍。我在一个模拟厂房地图上做过对比膨胀半径从2格加到4格覆盖率不变重复率从11%涨到27%。解决思路有两个第一是减少安全余量但会牺牲碰撞安全性第二是采用双层地图——精细地图用于覆盖判定粗糙膨胀地图用于宏观路径搜索两者结合兼顾安全性和重复率。后者实现稍微复杂但效果明显。6.3 覆盖率、重复率与转弯次数的权衡不存在绝对最优全覆盖问题本质上是一个多目标问题。覆盖率和重复率之间存在天然矛盾为了提高覆盖率机器人必须进入每一个边角这通常会带来更多重复路径为了降低重复率而跳过一些复杂角落覆盖率又会掉下来。转弯次数更是直接影响清洁效率。这三个指标单看任何一个都是片面的。我用同一张地图跑了三组参数结果如下网格尺寸覆盖率重复率转弯次数执行时间0.1m99.2%12%8626s0.15m97.8%9%5817s0.2m94.5%8%3911s网格越细覆盖率越高但重复率和转弯次数也随之上升。实际项目中没有最优只有符合验收标准且成本可控。如果验收要求覆盖率98%那0.1m是底线如果只是演示demo0.15m的视觉效果好很多。这个权衡一定是在做算法之前就想清楚的。6.4 离线自测方法至少跑五张地图再下结论我接触过不少同学拿一张矩形地图跑出漂亮轨迹就认为算法做完了。这种结论害了自己。全覆盖算法的好坏必须靠多地图交叉验证我至少会用五张测试地图纯矩形、带矩形障碍、带L形障碍、带U型凹槽、多房间连通。每张地图都统计覆盖率、重复率、路径长度和转弯次数四项指标横向对比。还有一个容易被忽视的检查点对比coverage和freeMap的差集。用一条命令找出所有空闲但未被覆盖的格子然后人工逐个确认这些格子是不是被膨胀误吞的边角。很多覆盖率虚高的问题其实都出在这个差集检查上比如某个角落格被膨胀后标记为不可达算法统计时把它从目标里剔除了看起来覆盖率99%实际上真实物理空间根本没扫到。最后分享一下我的实操经验。做全覆盖路径规划demo上手顺序千万不要乱来先写无障碍矩形房间牛耕式跑通了再塞障碍物再写单元分解和回溯。一开始就上复杂算法95%的时间都在调bug而不是调参数。另外一定要把可视化开启慢速运行这个算法是看着它怎么扫才能真正理解的东西不是跑完看统计数字就能调出来的。等第一类算法吃透了你可以拿第二类遗传算法/蚁群优化来做对比实验用重复率作为优化目标去改进覆盖顺序——那是另一个值得花时间深挖的方向但地基一定是这篇里的第一类算法。