1. 全覆盖路径规划的本质不是“找一条路”而是“铺满一张图”做移动机器人路径规划的人最先接触到的算法大概率是A*给定一个起点和一个终点A*能在地图上找到一条避开障碍物的最短路径。但如果你做的是扫地机器人、割草机、洗地机、农业植保无人机这类设备问题就完全变了一个样它们不是从A点到B点而是要把一整片工作区域完整地覆盖一遍不能有大面积漏扫也不能绕太多冤枉路。这就是全覆盖路径规划Complete Coverage Path Planning简称CCPP。我前阵子接了一个模拟割草机作业的小项目场景是栅格化后的二维地块草地图里散布着几块障碍物区域比如树、花坛、石头。任务很简单让机器人在不重复覆盖的前提下把整个可行走区域“扫”一遍。拿到题目第一反应就是用A找路径但真动手才发现普通A是解决不了这个问题的——它本质上是个“点到点”的优化算法而全覆盖是一个“面到面”的规划问题。这篇文章把我当时的完整思路、Matlab代码框架以及调试过程中踩过的坑都写出来了。核心方案是用“往返式扫描”作为主线覆盖策略只在遇到障碍需要跨区域转移时调用A寻路。这样搭配的好处是往返式保证覆盖的连续性和低重复率A负责用最短路径把机器人从当前死胡同引导到最近尚未覆盖的区域兼顾效率与完整度。无论你是课程作业、竞赛还是实际项目这套思路都能直接迁移。1.1 普通A*为什么解决不了全覆盖问题先说一个很反直觉的结论如果你直接套用A*让机器人“从起点遍历所有未覆盖点”计算量会爆炸而且路径质量很差。因为全覆盖路径规划本质上是一个类似“旅行商问题”的组合优化问题地图上可能有成千上万个栅格点直接在这些点之间做全局路径搜索状态空间太大了。更重要的是A搜索的是“最短路径”而不是“覆盖轨迹”。机器人每次移动一步它不仅是在走路径同时也是在生产覆盖效果。如果只优化行走距离很可能会出现反复横跳、局部密集扫描但其他区域漏掉的情况。实际做项目时我们通常把全覆盖问题拆成两层第一层决定覆盖的“宏观顺序”比如先扫哪块、后扫哪块、遇到障碍后跳到哪里第二层才是在具体转移点之间用A这类算法找到一像素级的最短可行路线。所以这篇文章的做法是用往返式算法解决宏观顺序问题用A解决微观跳转问题。主循环负责“接下来往哪走”A负责“如何安全地走过去”。这样既控制了复杂度又保证了结果接近最优。1.2 “往返式”策略为什么是工程上的常用解“往返式”这个名词听着陌生但你看一眼扫地机器人的工作方式就明白了它沿着一个方向直线清扫到边界后旋转180度在相邻行反过来扫一次次来回就像老式打字机或者牛在耕地。这种方案最早就是模拟农业耕作英文叫Boustrophedon意思是“牛走的路径”。为什么这个策略在工程上这么流行因为它大幅降低了“决策维度”。如果机器人沿直线走只需要决定什么时候换行、换到哪一行而不用在每个栅格点都重新规划方向。对于大多数近似矩形的连续区域先按行扫、到边界换行天然就能覆盖完整而且路径转向次数少重复覆盖率也低。但真实地图不会永远那么规整。当一行中间出现障碍物时机器人沿着当前方向被挡住它不能直接穿过去更麻烦的是有时候前方整段都已经是覆盖过的区域机器人必须决定是原路返回、翻越到下一行还是跳到地图上另一块孤立的未覆盖区域。这时候才轮到A出场。在往返式覆盖的框架里A不是用来逐格寻路的而是充当“区域转移导航模块”。这也是我最后选择这种混合方案的核心理由。2. 网格地图建模与A*子导航模块先把底层轮子搭好在Matlab里写全覆盖路径规划第一步不是写主循环而是处理地图。因为后面所有的覆盖逻辑、A*搜索、覆盖率统计都基于一个干净的栅格地图表示。2.1 栅格地图与障碍物表达我用的地图是一个二维矩阵大小为Nrow x Ncol。矩阵元素取0表示自由栅格取1表示障碍物栅格。比如下面这个代码块就定义了一个带障碍物的12x12地图% 栅格地图0自由1障碍 map zeros(12,12); map(3,3:6) 1; map(5,8:9) 1; map(7:9,4:5) 1; map(10,2:3) 1; map(:,1) 1; % 地图左侧外墙 map(:,12) 1; % 地图右侧外墙 map(1,:) 1; % 地图上侧外墙 map(12,:) 1; % 地图下侧外墙建议把地图边界也设成障碍物这样路径搜索和覆盖循环中不需要额外写“越界判断”逻辑统一些。如果你用的是真实栅格地图数据通常已经包含了边界这步可以跳过。还有一点需要注意全覆盖任务里机器人本身是有尺寸的。如果机器人半径是0.5米栅格尺寸是0.3米那贴着障碍物走的栅格实际上会碰撞。工程上常用的办法是把障碍物做“膨胀”也就是把障碍物周围一定半径内的栅格也标记为1。Matlab里可以用bwdist算距离变换然后用阈值转成膨胀后的障碍物区域。下面这行代码可以生成一个膨胀半径2栅格的地图obsDist bwdist(map 1); inflatedMap obsDist 2; % 障碍物附近2格以内都视为不可通行 inflatedMap double(inflatedMap);膨胀之后机器人中心点只要落在自由栅格里就不会和障碍物碰撞。这个细节很多人写A时可以跑通但一放到全覆盖主循环就出事故原因大多就是没做膨胀处理。2.2 A*算法的核心逻辑与Matlab实现要点虽然这篇文章的主角看起来是“往返式”但A仍然是核心关键词所以我先把A子函数写出来。这里我特意选了四邻域扩展而不是八邻域。原因是全覆盖机器人通常在清扫时做直线运动斜向移动容易造成覆盖重叠或漏扫而且在“跨区转移”阶段斜着走也不是刚需。四邻域A*的代码如下function path astar4(map, start, goal) % 输入map 二值栅格地图0自由1障碍 % start, goal 均为[x, y]坐标 % 输出path 从start到goal的栅格序列不含起点含终点 [Nrow, Ncol] size(map); % gScore存储起点到当前点的实际代价 % parentMap用于回溯路径 gScore inf(Nrow, Ncol); fScore inf(Nrow, Ncol); parent zeros(Nrow, Ncol, 2); gScore(start(2), start(1)) 0; fScore(start(2), start(1)) heuristic(start, goal); openList start; closedMap false(Nrow, Ncol); dirs [1,0; -1,0; 0,1; 0,-1]; while ~isempty(openList) % 在openList中找到f值最小的节点 fVals arrayfun((k) fScore(openList(k,2), openList(k,1)), 1:size(openList,1)); [~, idx] min(fVals); cur openList(idx, :); if isequal(cur, goal) path []; while ~isequal(cur, start) path [cur; path]; cur [parent(cur(2), cur(1), 1), parent(cur(2), cur(1), 2)]; end return; end openList(idx, :) []; closedMap(cur(2), cur(1)) true; for i 1:size(dirs, 1) nxt cur dirs(i, :); if nxt(1) 1 || nxt(1) Ncol || nxt(2) 1 || nxt(2) Nrow continue; end if map(nxt(2), nxt(1)) 1 || closedMap(nxt(2), nxt(1)) continue; end tentativeG gScore(cur(2), cur(1)) 1; if tentativeG gScore(nxt(2), nxt(1)) gScore(nxt(2), nxt(1)) tentativeG; fScore(nxt(2), nxt(1)) tentativeG heuristic(nxt, goal); parent(nxt(2), nxt(1), :) cur; if ~any(openList(:,1) nxt(1) openList(:,2) nxt(2)) openList [openList; nxt]; end end end end path []; % 找不到路径 end function h heuristic(pos, goal) % 四邻域用曼哈顿距离代价比八邻域更直观 h abs(pos(1) - goal(1)) abs(pos(2) - goal(2)); end这个实现没有用containers.Map或结构体而是直接用二维数组存 gScore、fScore和parent效率完全够覆盖任务使用。要注意Matlab矩阵索引是(行,列)而栅格坐标习惯用(列,行)也就是(x,y)在调用时要统一转换我代码里的pos(1)是x列pos(2)是y行。我特别说明一下为什么这里用曼哈顿距离作为启发函数。A要求启发函数不超过真实代价才能保证最优性四邻域移动时相邻两格的真实代价就是1曼哈顿距离是最自然且可行的选择。有人喜欢用欧式距离但在四邻域网格中欧式距离会低估真实代价虽然仍然能找到路径却容易让A扩展更多节点。如果你改成八邻域那启发函数也应该换成切比雪夫距离这是很多人优化时容易忽略的点。2.3 为什么只在区域转移时调用A*有的读者可能会问既然A这么好为什么不在全覆盖主循环里让每一步都调用A找下一个目标那样覆盖率肯定高但计算开销受不了。可以做个简单估算一张100x100的栅格图有约一万个自由栅格如果在每个未覆盖栅格处都做一次A搜索相当于要做几千次点到点的路径规划。每次A在最坏情况下要扩展整个地图的节点总计算量会到千万级别。即使Matlab能在几秒内跑完对于需要实时决策的机器人也算不上优雅。更关键的是往返式扫描本身就有一个很强的先验信息机器人应该尽量沿着当前行方向走直到碰到障碍或已覆盖区域才转向。如果每一步都贪婪地找“全局最近未覆盖点”反而会破坏这种条理性导致路径频繁跨越、转向次数暴增。所以我把A限制在“当前扫描行走不下去”的情况。此时才需要跳出局部视野寻找一个远处的未覆盖区域并用A生成一条最短的可行转移路径。这样既不会频繁调用A*又保证不会死循环。3. 往返式全覆盖主体算法从“牛耕”到“智能跳转”全覆盖策略的难点不在“覆盖当前行”而在“如何从一段扫完的区域平顺切到下一段区域”。这部分我把算法拆成三个层次行内贪心、换行切换、跨区跳转。3.1 行内扫描规则先定方向再尽量往前走我的主循环一开始不是给机器人规划完整路径而是让机器人按照当前方向一步步走。在栅格地图里初始方向设为向右也就是x递增方向。机器人每走一步就把当前栅格标记为已覆盖然后尝试继续沿这个方向走。如果右侧是自由且未覆盖就直接走过去。如果右侧是障碍物或已经覆盖过意味着“这一行到头了”需要进入换行逻辑。此时不要立刻掉头因为有可能左边还有一个狭长自由区域可以补扫。比如地图行中间有一块障碍物机器人从右边扫过来被障碍物挡住时左边可能还有一个未被扫过的空隙。所以我的优先顺序是先继续向前无法向前则回头看一看有没有未覆盖点最后才考虑换行。这个细节能明显降低漏扫率。行内扫描的伪代码逻辑如下当前方向 dircur表示机器人位置 循环 标记 cur 为已覆盖 如果 dir 方向下一格合法且未覆盖 移动到该格 否则 尝试反方向下一个格如果合法且未覆盖 翻转dir移动到该格 否则 进入换行逻辑这么设计的好处是机器人不会在障碍物前盲目转向而是先把当前能走通的“枝杈”扫干净符合“能覆盖就覆盖”的优先原则。3.2 换行切换默认向上偶尔向下在真正的牛耕式路径里横向扫描一般是固定方向换行例如从下往上逐行扫描。如果地图中没有障碍物这个规则非常简单。但我的地图里障碍物分布复杂有时候当前行的下一行整段都被障碍物挡住了这时候继续“向上换行”是走不通的。所以我把换行逻辑做成了“优先尝试向上其次尝试向下”。对于当前列的竖直方向相邻栅格如果它是自由且未覆盖就移动到它并切换行进方向。这样实现起来很简单但会带来一个问题如果地图中恰好有一个类似“U型”的障碍结构机器人可能向上走一下、向下走一下发生所谓的“振荡”。避免振荡的办法是在代码里加一个“最近转向记录”。我实现时用一个变量记录上一次换行是在哪个位置发生的如果连续三次换行都发生在同一个或相邻的栅格里就认为这里已经扫干净了强制交给跨区跳转逻辑处理。这个细节后面我会在避坑章节展开。3.3 跨区跳转用A*寻找最近未覆盖点当机器人连换行都走不动的时候说明当前连通区域基本上扫完了。此时地图上可能还散落着其他未覆盖区域它们被障碍物隔开。传统Boustrophedon会做区域分解把地图按障碍物边界分成多个子区域再规划子区域间的顺序。我这里为了代码实现简单没有显式做区域分解而是直接找“距离机器人当前位置最近的未覆盖栅格”然后用A*把路径算出来。最近未覆盖点的筛选有一个小技巧如果直接遍历所有未覆盖栅格然后逐一算欧氏距离也够快但要过滤掉那些虽然距离近但需要绕很远路的点。更稳妥的办法是维护一个“未覆盖栅格列表”每覆盖一个栅格就把它从列表里移除然后在线性时间内找到最小曼哈顿距离的候选点。在覆盖任务刚开始、地图中未覆盖区域特别多的时候这个简单策略也能快速收敛。找到目标点后调用A导航。需要注意的是在跨区转移过程中A走的路径本身也会覆盖经过的自由栅格。所以主循环里应该让A*返回的路径上的每一个栅格都被标记为已覆盖。这样避免了重复扫描转移路径也不会在后续扫描时再对这些区域做无意义的工作。4. 完整Matlab代码实现与逐段解析原理说再多不如直接放一套能跑的代码。下面的实现是我简化后的版本保留核心逻辑去掉了可视化装饰适合你自己改着玩。4.1 构建地图并初始化覆盖表% MainCoverage.m clc; clear; close all; % 1. 定义地图 map zeros(20, 20); map(:,1) 1; map(:,20) 1; map(1,:) 1; map(20,:) 1; map(5,6:10) 1; map(8:12,5) 1; map(14:16, 14:16) 1; map(3, 13:15) 1; % 2. 初始化覆盖标记 [Nrow, Ncol] size(map); covered false(Nrow, Ncol); % 3. 机器人起点设置为地图左下角自由栅格附近 cur [2, 19]; % [x, y] if map(cur(2), cur(1)) 1 error(起点位于障碍物上请重新设置); end % 4. 初始扫描方向向右 dir 1;这里有一个坐标转换注意点Matlab矩阵的行号y向下递增但人类习惯的栅格直角坐标是x向右、y向上。我代码里cur [2, 19]表示x2y19在矩阵中就是map(19,2)。你的地图如果按不同方式读入要统一检查。我后面的所有代码都按这个约定写。4.2 主循环往返式扫描 A*跳转% 记录完整路径方便可视化 path cur; maxIter 50000; iter 0; while iter maxIter iter iter 1; % 标记当前位置为已覆盖 covered(cur(2), cur(1)) true; % 1. 尝试沿当前方向前进 next cur [dir, 0]; if insideMap(next, Nrow, Ncol) map(next(2), next(1)) 0 ~covered(next(2), next(1)) cur next; path [path; cur]; continue; end % 2. 尝试回头扫描反方向 back cur [-dir, 0]; if insideMap(back, Nrow, Ncol) map(back(2), back(1)) 0 ~covered(back(2), back(1)) dir -dir; cur back; path [path; cur]; continue; end % 3. 尝试向上换行列不变行减1因为显示时y向上 up cur [0, -1]; if insideMap(up, Nrow, Ncol) map(up(2), up(1)) 0 ~covered(up(2), up(1)) cur up; dir -dir; path [path; cur]; continue; end % 4. 尝试向下换行 down cur [0, 1]; if insideMap(down, Nrow, Ncol) map(down(2), down(1)) 0 ~covered(down(2), down(1)) cur down; dir -dir; path [path; cur]; continue; end % 5. 走到死胡同搜索最近的未覆盖点并调用A* uncoveredList findUncovered(map, covered); if isempty(uncoveredList) break; end target nearestPoint(cur, uncoveredList); subPath astar4(map, cur, target); if isempty(subPath) % A*找不到路径说明目标点实际上不在同一个连通域换个候选点试试 target2 secondNearestPoint(cur, uncoveredList); if isempty(target2) break; end subPath astar4(map, cur, target2); if isempty(subPath) break; end end % 沿着A*路径移动并标记经过的栅格 for i 1:size(subPath, 1) stepPos subPath(i, :); if map(stepPos(2), stepPos(1)) 0 covered(stepPos(2), stepPos(1)) true; cur stepPos; path [path; cur]; end end dir 1; % 重置扫描方向 end % 输出结果 coverageRate sum(covered(:)) / sum(map(:) 0) * 100; fprintf(覆盖率%.2f%%\n, coverageRate);4.3 需要用到的子函数function flag insideMap(pos, Nrow, Ncol) flag pos(1) 1 pos(1) Ncol pos(2) 1 pos(2) Nrow; end function list findUncovered(map, covered) [rows, cols] find(map 0 ~covered); list [cols, rows]; % 存成[x, y] end function target nearestPoint(cur, pointList) dist abs(pointList(:,1) - cur(1)) abs(pointList(:,2) - cur(2)); [~, idx] min(dist); target pointList(idx, :); end function target secondNearestPoint(cur, pointList) dist abs(pointList(:,1) - cur(1)) abs(pointList(:,2) - cur(2)); [~, idx] sort(dist); % 跳过第一个不可达点寻找第二个 for i 2:length(idx) target pointList(idx(i), :); return; end target []; end我把findUncovered实现成直接返回所有未覆盖点没有做列表维护因为示例地图很小性能可以接受。如果你要跑1000x1000的大地图建议用队列或者增量式维护未覆盖点集否则每次主循环调用findUncovered都会扫描整个地图效率会直线下降。4.4 可视化与覆盖率统计覆盖率是一刀切的评价指标。我算的是“已覆盖自由栅格 / 总自由栅格”。在往返式框架下如果主循环正常结束覆盖率应该是100%。但实际中会因为地图过于破碎、A*找不到可达点而提前退出所以必须打印覆盖率来监控。可视化部分比较简单你可以用plot画出path的行进轨迹也可以用imagesc动态显示覆盖状态。我常用的是imagesc(map covered)然后把障碍物画成黑色已覆盖区域画成白色未覆盖区域画成灰色这样一眼能看出漏扫在哪。下面是基础可视化代码figure; hold on; % 绘制障碍物 [rowObs, colObs] find(map 1); plot(colObs, -rowObs, ks, MarkerFaceColor, k, MarkerSize, 8); % 绘制覆盖轨迹 if size(path,1) 1 plot(path(:,1), -path(:,2), b-, LineWidth, 1.5); plot(path(1,1), -path(1,2), go, MarkerFaceColor, g, MarkerSize, 10); plot(path(end,1), -path(end,2), ro, MarkerFaceColor, r, MarkerSize, 10); end axis equal; grid on; box on; title(往返式全覆盖路径规划结果); xlabel(X列); ylabel(Y行向下);注意这里我把y坐标取负是因为Matlab的imagesc默认显示矩阵时行号向下递增我想让地图看起来更像真实直角坐标系。如果你不习惯直接在画图时乘负号就行。5. 我在调试这版代码时踩过的坑以及几个有用的扩展思路看完代码再劝一句全覆盖路径规划里最容易出问题的从来不是算法复杂度而是状态机转换的条件写得不完整。我调试这版代码时至少遇到三次死循环和两次覆盖率卡在99%的情况下面把教训写清楚。5.1 振荡和死循环的根源优先级设计不合理最典型的现象是机器人在一个两行区域之间来回换行反复横跳但就是覆盖不到新区域。原因出在换行逻辑上。我一开始写的顺序是向上换行优先向下换行其次然后才考虑回头。结果地图里有个“V型”障碍物机器人走到V型底部先向上走一格然后又因为下一格是障碍物回到向下循环往复。解决的办法有三个层次第一在尝试换行前加“最少前进步数”限制比如必须连续向前走了至少2格才能换行否则说明这个区域很窄不值得在上下行之间折腾第二记录最近换行位置如果连续3次换行距离小于3个栅格就强制切换到A*跳转第三也是最彻底的显式维护一个“已覆盖区域连通域”的概念只有当当前连通域扫完才允许跨区跳转。我的示例代码里没有加步数限制因为地图比较整但你在自己地图上跑的时候强烈建议加一个简单计数器代码大约三行收益非常大。5.2 覆盖率卡在99%A*返回的路径可能穿过未覆盖区但不“完全覆盖”有一次程序跑完显示覆盖率99.2%我看生成路径原来是一个窄长条区域没有被覆盖。原因是跨区跳转时A*计算的路径只经过了一些关键节点但机器人沿着路径走的时候路径两侧各有一行栅格没有被覆盖。全覆盖任务里机器人本身是有宽度的一次移动可能覆盖不只一个栅格。所以正确做法不是简单标记A路径经过的栅格而是要在主循环的“沿A路径移动”阶段对路径经过的每个起点终点之间的中间栅格做左邻右舍扩展覆盖。具体说就是判断机器人前方两侧相邻的可通行栅格只要没被覆盖就一并标记为已覆盖。但这样就与机器人的真实物理清扫宽度有关。如果你做的是模拟研究可以简单认为一次覆盖一个栅格如果做实际机器人建议在仿真里就把清扫宽度建模为3x3或5x5的区域。5.3 地图分辨率与膨胀半径不是越细越好网格分辨率直接决定计算量和覆盖率效果。有些人喜欢把地图离散得很细觉得更精确。但我在20x20的地图上跑得很欢换到200x200的地图后A*的寻路速度会明显下降而且往返式扫描的换行策略容易出现“有些行漏扫有些行反复扫”的问题。我的经验是栅格尺寸应该和机器人清扫宽度在一个量级最好一个栅格等于机器人一次清扫宽度的1/3到1/2。这样既能平滑转行也不会产生太多冗余路径。另外膨胀参数也不要设太大尤其不要在窄通道场景里盲目膨胀否则有些刚好能过的通道会被堵死A*就只能宣告无解覆盖率被迫止步于一个很低的数值。5.4 扩展思路显式区域分解与分层规划我目前这套方法是“扫描为主A*为辅”的最简版本适合快速实现、快速看效果。如果你想让路径更接近最优建议用改进型Boustrophedon先用bwlabel对自由区域做连通域标记把每个连通域当成一个子区域然后在每个子区域内用往返式扫描最后把子区域之间的转移顺序规划问题转化成一个“最小移动代价”的排序问题。这里的转移顺序依然可以用A来评估具体做法是对每两个子区域的边界点做A搜索得到代价矩阵然后用贪心算法或TSP求解器决定访问顺序。这一套下来代码量虽然有提升但覆盖率和重复率指标都会有明显改善而且能处理更复杂的地形。我个人在实际项目中的体会是全覆盖路径规划的重点不是“用更聪明的算法替代往返式”而是把问题分解成覆盖、转移、调度三个子问题并对每个子问题选择合适的工具。A*在最底层解决转移问题已经在绝大多数场景里足够好用。如果你正在做类似的方向建议先把我这套代码跑起来用它当作基线版本然后逐步加入区域分解、换行优化、动态障碍物避开等改进这样每一步都能看到明确的效果差异。