做全覆盖路径规划的人多半都是先被 A* 算法领进门的。搜“路径规划算法”A* 永远是出场率最高的那个网上资料多、Matlab 代码也好找。但如果你直接把 A* 拿去解决“覆盖”问题——比如扫地机器人要把房间完整扫一遍、植保无人机要把一块田完全飞一遍——很快就会发现问题A* 擅长的是从 A 点到 B 点的最短路径而全覆盖要的是把整个可达空间走一遍本质上完全是两类问题。这篇我把自己完整走通的一套方案拆给你看网格环境下用往返式也就是牛耕法/弓字形做主覆盖用 A* 做段与段之间的转移衔接全程 Matlab 代码从地图生成跑到覆盖率统计。适合正在做全覆盖路径规划、里面要用 A* 又不知道如何结合的同学参考。整套代码结构我文末也做了扩展建议方便你迁移到自己的项目里。1. 为什么全覆盖路径规划不能直接用 A*问题拆解1.1 A* 解决的是“单次最短”不是“全图覆盖”很多人包括早期阶段的我想法很直接既然 A* 能求两点最短路径那我从起点出发反复 A* 到最近的未覆盖点不就能把整张图覆盖了吗听起来合理实际跑起来会出现三个问题。第一个问题是路径重复。点与点之间来回转移不可避免会走过已经覆盖的区域重复率会非常高。比如一张 40x40 的栅格地图随机分布十几个小障碍这种“最近未覆盖点贪心”的做法重复率轻松超过 30%实际清扫效果很难看。第二个问题是死角。当你贪心选择一个看起来很近的未覆盖点跑过去之后可能发现一条很窄的通道或凹形区域导致当前位置离剩下的未覆盖区非常远只能原路退回。更极端的场景是当前区域与剩余未覆盖区域之间被已覆盖栅格隔开A* 强行找路时会在已覆盖区里反复穿来穿去。第三个问题是目标不可达。如果地图存在多个孤立连通区域贪心选择的目标根本不在当前连通域内A* 会直接无解或在障碍边缘空转。所以纯粹“A* 贪心未覆盖点”的思路工程落地时问题很多路径图看上去像一团乱麻覆盖率还不一定高。1.2 往返式全覆盖的基本思想往返式Boustrophedon其实是我们日常生活中到处都能见到的覆盖方式——扫地机器人走的“弓字形”、农机翻地走的“来回趟”都是这个思路选定一个主方向沿这个方向一行一行扫过去遇到障碍物就跳过到了边界再折返。对于没有障碍物的凸区域往返式基本是最优的一条 S 形轨迹把所有区域串完几乎不重复。真正的难点在障碍物——每块障碍都会把一条完整的扫描行切断成好几段覆盖完一段之后怎么去下一段段与段之间的转移路径如果乱走效率会迅速下降。这就是 A* 应该出场的地方。1.3 层次化拆分调度层 局部规划层我的做法是把问题拆成两层让每个模块只干自己擅长的事顶层调度层决定下一步去覆盖哪个扫描段从段的哪个端点进入底层局部规划层用 A* 把当前位置到目标端点之间的转移路径求出来要求避障且最短。为什么这样分层因为调度层处理的是“段”之间的连接顺序候选数量少、决策直观局部层处理的是“栅格”之间的避障最短路径正好发挥 A* 的长处。两者一衔接就是一套完整的全覆盖路径。这个拆分的另一个好处是各个部分可以独立替换。以后想用 D* 或 RRT 做局部规划只需要改底层接口想把贪心调度换成遗传算法、粒子群那类优化方法顶层换掉就行。后面 7.5 节我会细说这个扩展思路。2. 网格地图建模与可达区域预处理Matlab 实现2.1 栅格矩阵与坐标约定Matlab 里最自然的表达就是二维矩阵0 表示可通行1 表示障碍物。矩阵的行对应地图的 y 向列对应 x 向这个约定后面所有代码都保持一致避免绕晕。简单生成一块测试地图map zeros(40, 40); map(1,:) 1; map(end,:) 1; map(:,1) 1; map(:,end) 1; rng(1); for i 1:18 rr randi([2, 6]); cc randi([2, 6]); r0 randi([2, 38-rr]); c0 randi([2, 38-cc]); map(r0:r0rr-1, c0:c0cc-1) 1; end figure; imagesc(map); colormap(gray); axis image;地图规模 40x40对实验和讲解都够用。你还可以把rng(1)的种子改掉多跑几组看看算法在不同障碍分布下的表现。2.2 障碍物膨胀为什么不能直接拿原始栅格跑如果机器人/车辆有实际尺寸路径就不能贴着障碍物边缘走。我通常的做法是对障碍栅格做膨胀dilation也就是把障碍物周围一定半径内的栅格也标记为不可通行。Matlab 里一句imdilate就解决了% 假设机器人半径对应 radius 个栅格 radius 1; se strel(square, 2*radius1); costmap double(imdilate(logical(map), se)); costmap(map1) 1; % 原始障碍物本身这里有个细节膨胀半径要取“机器人半径对应的栅格数”一般向上取整。半径取 1 就够演示实际工程里要根据机器人尺寸和栅格分辨率算。膨胀之后路径规划器就永远不会把机器人引导到距障碍物不足 1 个栅格的位置。提示如果你用的是 8 邻域 A*膨胀时也要考虑对角穿越。对角方向穿越时机器人实际会斜切过一个单元格很多场景下膨胀半径至少取 1 才安全。这个问题第 4 章还会重点讲。2.3 BFS 提取可达区域让 A* 只搜索有意义的地方在全覆盖算法启动之前先做一次连通域分析。从起点开始 BFS把当前连通域内的所有自由栅格找出来。后面的扫描段生成、覆盖率统计都只针对这个连通域。start [2, 2]; reachable false(size(map)); queue start; reachable(start(1), start(2)) true; while ~isempty(queue) p queue(1,:); queue(1,:) []; for d [-1 0; 1 0; 0 -1; 0 1] np p d; if np(1)1 np(1)size(map,1) np(2)1 np(2)size(map,2) if map(np(1), np(2)) 0 ~reachable(np(1), np(2)) reachable(np(1), np(2)) true; queue(end1,:) np; %#okAGROW end end end end这一步很关键如果地图里有多个互不相通的区域A* 找目标时会无解白白消耗时间。预处理阶段就把“不可能到达”的目标排除掉既省时间又能避免运行期崩溃。3. 往返式扫描段的生成把“面覆盖”变成“段覆盖”3.1 逐行扫描生成连续段固定按行扫描也可以选列后面会对比。对每一行从左到右扫描找出所有连续的可行栅格区间每个区间记为一个扫描段function segments extractSegments(map, reachable) [nr, nc] size(map); segments struct(row, {}, cStart, {}, cEnd, {}); idx 0; for r 2:nr-1 c 2; while c nc-1 if map(r,c) 0 reachable(r,c) cStart c; while c nc-1 map(r,c) 0 reachable(r,c) c c 1; end cEnd c - 1; idx idx 1; segments(idx).row r; segments(idx).cStart cStart; segments(idx).cEnd cEnd; else c c 1; end end end end每个段的两个端点就是“入口/出口”的候选位置。段本身是单行上的一个连续区间进入后可以从左往右扫也可以从右往左扫具体方向由入端点决定。3.2 为什么用“逐行分割”而不是直接生成弓字形路径理想的往返式覆盖应该是连续的 S 形轨迹第一行从左到右到行尾折返到下一行右端继续扫……但障碍物一来S 形轨迹就断了。比如第一行中间有障碍物你从左端扫过去扫到障碍物左侧就得停下但障碍物右侧还有一段没扫。此时你就得选择是先跨到下一行继续还是先绕过障碍物把右侧扫完。“逐行分割成段”的好处是把复杂的几何问题简化成“段生成 段调度”两个简单问题。段生成是纯粹的几何扫描不涉及路径搜索段调度再用 A* 处理。这个分层思路和第一章讲的设计是打通的。3.3 为什么从段的端点进出我从段的端点进入而不是从段中间进入有两个原因。第一段是单行上的连续区间只有两个端点调度状态天然好定义。第二从中间进入会打断扫描轨迹造成段内重复和额外折返。除非极端情况下段的两端都被障碍物封死否则优先选端点进入。每个段在运行时要维护状态未覆盖、覆盖中、已覆盖。主循环每次都从“未覆盖”的段里挑一个去覆盖覆盖完立刻更新状态。这个状态机是第 5 章主循环的核心数据结构。3.4 扫描方向其实是个自由度不是固定死的回到 3.2 节的问题当前行扫完如何无缝衔接下一行用我们这套段调度其实不用纠结暂态的无缝衔接因为调度层会自己选。代价是覆盖轨迹可能不是严格的“弓字形”而是“弓字形 跳段”——但跳段路径由 A* 保证最短且避障总效果通常优于纯贪心逐点覆盖。如果你希望视觉上更接近“弓字形”可以在调度时给“同一行或相邻行的段”加一个小权值偏好让算法优先选相邻的段。怎么做贪心代价从“纯 A* 路径长度”改成“A* 路径长度 0.1 x 段间行距”。这个小改动能把轨迹拉直不少而且基本不影响最优性。第 5 章我会给公式。4. A* 在段间转移路径中的正确用法4.1 为什么转移路径必须用 A* 而不是直线距离有了段接下来就是从当前点到目标段端点的转移。地图里有障碍物直线连过去可能直接穿墙。A* 在网格上的作用正是给定起点和目标在避开障碍的前提下找到最短路径。但也不建议过度使用。我的流程里A* 只在“需要转移”的时候调用而不是对每个栅格点都调用。40x40 的地图大约有几百到上千个可达栅格但扫描段通常只有几十个A* 调用次数也就几十次Matlab 完全扛得住。4.2 A* 的关键实现细节A* 的核心是f(n) g(n) h(n)。g是从起点到当前节点的实际代价h是从当前节点到目标点的估计代价。只要h不大于真实代价值可采纳性A* 找到的就是最优路径。这个数学性质意味着只要启发函数选对算法不会为了省时间而牺牲最优性。Matlab 里没有现成的优先队列我用开放列表加按f值排序的朴素实现对几十个段这种规模完全够用function [path, cost] astar(grid, start, goal, use8) [nr, nc] size(grid); g inf(nr, nc); from zeros(nr, nc, 2); closed false(nr, nc); openList [start(1), start(2), heuristic(start, goal)]; g(start(1), start(2)) 0; while ~isempty(openList) [~, idx] min(openList(:,3)); cur openList(idx, 1:2); openList(idx,:) []; if closed(cur(1), cur(2)) continue; end closed(cur(1), cur(2)) true; if cur(1) goal(1) cur(2) goal(2) break; end for d neighbors(cur, use8, grid) ncost simpleCost(cur, d); ng g(cur(1), cur(2)) ncost; if ng g(d(1), d(2)) g(d(1), d(2)) ng; from(d(1), d(2), :) cur; openList(end1, :) [d(1), d(2), ng heuristic(d, goal)]; %#okAGROW end end end % 路径回溯 path [goal]; while path(1,1) ~ start(1) || path(1,2) ~ start(2) path [from(path(1,1), path(1,2), :); path]; %#okAGROW end cost g(goal(1), goal(2)); end代码里有两个容易踩的坑openList里同一个节点可能出现多次所以弹出来时要检查 closed 标记再跳过从 goal 回溯时如果 goal 的g还是 inf说明目标不可达要提前做无解处理否则回溯循环会死循环。4.3 启发函数选择4 邻域和 8 邻域必须匹配启发函数直接决定搜索效率。4 邻域下移动方向只有上下左右步长恒为 1曼哈顿距离abs(dr) abs(dc)是最合适的启发。8 邻域下斜向移动步长约 1.414此时切比雪夫距离max(abs(dr), abs(dc))更贴合真实代价。欧氏距离虽然也可采纳但偏小会让 A* 多扩展不少节点搜索变慢。我实测过同一张地图8 邻域 切比雪夫启发相比 4 邻域 曼哈顿启发转移路径平均能短 20% 左右。代价是斜向移动需要额外做对角穿越判断见 4.4。所以选择哪种邻域本质上是在路径长度和实现复杂度之间做权衡。工程上我倾向于 8 邻域因为覆盖路径的重复率对长度很敏感省 20% 的绕行很有意义。4.4 8 邻域下的对角穿越问题这是 A* 网格实现里最高频的老大难问题从(r, c)斜着走到(r1, c1)如果(r1, c)和(r, c1)都是障碍物路径就会穿墙角。物理上机器人是不可能直接斜穿一个对角缝的。解决办法是在扩展邻居时加一个判断if abs(dr) 1 abs(dc) 1 if grid(cur(1)dr, cur(2)) 1 || grid(cur(1), cur(2)dc) 1 continue; % 禁止对角穿越 end end这样既保留斜向走的效率又保证路径是可执行的真实轨迹。漏掉这个检查跑出来的路径看着挺短实际根本走不了这在仿真里叫“几何最短”和“可行最短”的区别。第 7 章的踩坑记录里我会再强调一次。5. 贪心调度把扫描段串成一条完整路径5.1 调度策略每次找“看起来最近”的未覆盖段主循环的逻辑一句话总结在剩余未覆盖的段里选一个离当前点最近的段用 A* 过去覆盖这个段然后重复。“最近”的定义有两种做法。第一种是把所有剩余段都算一遍 A*选真实代价最小的优点是结果最优缺点是每次迭代都要跑几十次 A*。第二种是先粗选再精算先用曼哈顿距离给所有段排个序取前 3 到 5 个候选段然后只对候选段跑 A*选真实代价最小的。对小地图用第一种完全没问题对大地图我强烈建议用粗选——预排序选错段的概率很小离得近的段大概率转移路径也短这样能把调度耗时降一个量级。5.2 为什么不用全局最优组合优化的坑看到这里你可能会想段与段之间的选择次序像不像 TSP能不能用遗传算法、粒子群、动态规划求一个全局最优访问顺序思路没错但我要泼一盆冷水段的转移代价不是固定的欧氏距离而是 A* 算出来的路径长度每两个段之间的代价都依赖地图。在这个基础上跑群智能优化计算量很大而且问题本身是 NP-hard。在段数只有几十个的小型全覆盖场景贪心 A* 的质量已经足够好真到了上百个段的大型场景你更应该优化的不是调度算法而是地图预处理和扫描方向——第 6 章的对比实验会说明原因。贪心最大的价值是简单、稳定、可解释。调试时你能明确知道“下一步是去第 3 行的段”而不是面对一个黑箱优化器这对工程项目来说非常重要。5.3 主循环完整代码主循环我完整写一遍关键细节都有注释route []; current start; covered false(size(map)); numSeg numel(segments); remaining 1:numSeg; while ~isempty(remaining) % 粗选候选段按曼哈顿距离预排序 dists arrayfun((k) abs(segments(k).row - current(1)) ... min(abs(segments(k).cStart - current(2)), ... abs(segments(k).cEnd - current(2))), remaining); [~, order] sort(dists); candIdx remaining(order(1:min(3, numel(order)))); % 精算对每个候选段A* 求到两个端点的路径代价 bestCost inf; for k candIdx seg segments(k); [cost1, path1] astar(costmap, current, [seg.row, seg.cStart]); [cost2, path2] astar(costmap, current, [seg.row, seg.cEnd]); if cost1 bestCost bestCost cost1; chosen k; entry left; transPath path1; end if cost2 bestCost bestCost cost2; chosen k; entry right; transPath path2; end end % 执行转移路径路径经过的自由栅格也算覆盖 for i 1:size(transPath, 1) rc transPath(i,:); if map(rc(1), rc(2)) 0 covered(rc(1), rc(2)) true; end end route [route; transPath]; %#okAGROW % 进入段并沿段扫描 seg segments(chosen); if strcmp(entry, left) scanSeq [seg.row * ones(seg.cEnd - seg.cStart 1, 1), (seg.cStart:seg.cEnd)]; else scanSeq [seg.row * ones(seg.cEnd - seg.cStart 1, 1), (seg.cEnd:-1:seg.cStart)]; end for i 1:size(scanSeq, 1) bc scanSeq(i, :); covered(bc(1), bc(2)) true; end route [route; scanSeq]; %#okAGROW current scanSeq(end, :); % 更新剩余段检查是否有段已被转移路径顺路覆盖完 remaining setdiff(remaining, chosen); end这段代码跑完route里存的就是一条从起点出发、覆盖所有可达栅格的完整路径。5.4 边界情况转移路径顺带覆盖了其他段怎么办主循环里有个容易漏掉的细节转移路径经过的自由栅格也在更新covered。也就是说转移路径很可能“顺路把某个未覆盖段的一部分甚至全部扫掉”。如果运气好转移路过的段已经全部覆盖就应该直接从remaining里移除避免后面重复扫。我在完整版本里是这样处理的每次更新covered后把所有剩余段重新检查一遍只要段内所有栅格都已被覆盖就从remaining中剔除。听起来像优化实际上是避免重复覆盖覆盖段的关键步骤。还有一种极特殊情况某个段只剩下中间一个栅格没覆盖但它的两个端点要么被覆盖、要么被障碍包围A* 无法直接到达段变成“死段”。这种情况在贪心流程里很少出现但我在多障碍地图上确实碰到过。我用的处理办法有两个一是干脆舍弃这个残余栅格覆盖率差一点点但路径干净二是把“段覆盖”降级为“残余点覆盖”用 A* 尽量接近它绕行覆盖。实际落地我建议先用前者项目对覆盖率有硬性要求时再上后者。6. 覆盖率、重复率与可视化结果不靠“看着像”说话6.1 三个指标怎么定义才算靠谱做研究也好做工程验收也好不能只看路径图“好像挺满”。我常用的三个指标覆盖率已覆盖栅格数 / 可达栅格数。理想是 100%但实际可能因死角、孤立小区域等原因略低。重复率(路径总长度 - 可达栅格数) / 可达栅格数。路径总长度按栅格移动步数累计8 邻域下斜向一步算sqrt(2)。重复率越低越好0 表示完全没走回头路。转弯次数相邻两步方向发生变化的次数。对扫地机器人这类实际平台转弯次数少意味着能耗低、执行快。这三个指标分别从覆盖完整性、时间效率、可执行性三个维度评价路径比单看一张路径图客观得多。我一般会在算法输出后自动打印这三项跑对比实验时直接看数字挑配置。6.2 静态/动态可视化静态可视化用plot把route画出来即可我习惯把障碍物画成深色块、路径用折线figure; imagesc(map); colormap(gray); hold on; plot(route(:,2), route(:,1), b-, LineWidth, 1.5); plot(start(2), start(1), go, MarkerSize, 8, MarkerFaceColor, g); axis image;动态可视化更有说服力。我用的是循环里对route分段画每画 5 个栅格点刷新一次drawnow。调试时还可以把covered画成渐变色一眼能看出覆盖推进过程。这里有一件事很重要axis image必须加上否则图形会被拉伸栅格看起来就不是正方形路径方向会误导你。6.3 几组对比实验启发函数、邻域选择、扫描方向我用同一张带 18 个随机障碍的地图跑了几组配置结果一起放在表里配置覆盖率重复率总路径长度栅格数行扫描 4邻域 曼哈顿100%24.8%1231行扫描 8邻域 切比雪夫100%19.6%1124列扫描 8邻域 切比雪夫100%17.2%1058随机段序 8邻域 切比雪夫100%32.1%1409注具体数值随地图变化这里只展示趋势。能看出三件事。第一8 邻域 切比雪夫明显优于 4 邻域 曼哈顿因为转移路径允许斜向走绕行的锯齿少。第二扫描方向的影响比大多数初学者以为的大得多——这张地图上障碍分布偏“竖长条”列扫描避免了大量被切断的短段重复率直接降到 17%。所以工程上别把行扫描写死最好行、列都跑一遍取总路径短的那个。第三组“随机段序”是我故意加的对照组提醒自己调度层的优化空间比 A* 本身的邻域选择更大。扫描顺序从贪心改成随机重复率从 17% 跳到 32%。这组数据说明整套流程里最值得挖的是段调度策略而不是在 A* 的邻域参数上抠细节。6.4 扫描方向选择的低成本预判行扫描和列扫描全跑一遍计算量翻倍。有个低成本的预判方法统计地图中所有自由栅格外接矩形的宽高比。如果地图横向更宽说明横向连续段普遍偏长切割少选行扫描反过来纵向更宽选列扫描。这是我调试多张地图后总结的粗规则不保证每个案例最优但能省下不少调参时间。7. 踩坑记录与调参经验7.1 8 邻域对角穿墙角问题再强调一次做代码评审时我抓得最多的一个 bug就是 8 邻域 A* 漏了对角碰撞检测。跑出来的路径比 4 邻域漂亮得多但很多路径在实际场地里根本走不了。极端情况是机器人从(1,1)斜走到(2,2)如果(1,2)和(2,1)都是障碍物机器人就卡在对角缝里。解决办法我已经在 4.4 节给了扩展邻居时对斜向移动做一次两侧栅格检查有障碍就禁止穿越。这个判断不复杂而且对路径质量的影响是决定性的无论如何不能省。7.2 贪心调度钻进死胡同有一次调试稀疏障碍地图贪心选的段离当前点直线距离很近但 A* 求出的转移路径却穿过了一大片已覆盖区域重复率暴涨。后来我发现问题不在 A*而在粗选阶段粗选用的是曼哈顿距离它完全忽略了障碍分布两个点直线距离近不代表转移路径短。我把粗选策略改成两步先把所有段按曼哈顿距离排序取前 5 个候选段再对这些候选段全部执行 A* 求真实代价。如果段总数不超过 30干脆不做粗选直接全量精算。每次迭代多花几秒钟对单张地图的全覆盖任务完全可接受贪心决策质量却稳了很多。7.3 Matlab 性能细节开放列表别用 cellfun小规模实验无所谓但地图到 100x100 以上时A* 开放列表的维护方式直接决定你能不能跑完。我的经验是openList一定用二维数值数组列是[r, c, f]不要用 cell 数组更不要在循环里eval每次取最小f用min(openList(:,3))加索引不要用arrayfun或循环去逐行比较地图的g、covered这些矩阵用inf/logical预分配不要动态增长路径回溯用from矩阵不要在循环里反复拼接path那会让整体复杂度变成 O(n²)。我试过用containers.Map做开放列表结果比朴素数组慢一倍以上。Matlab 的强项是矩阵运算A* 这种逐点循环的算法本身就是它的弱项所以每个操作都尽量保持向量化思维否则地图一大就跑不动。7.4 膨胀半径与地图分辨率的关系膨胀半径是全覆盖路径规划里最容易被低估的参数。半径太大可行区域变小覆盖率下降路径被迫绕大圈半径太小机器人可能剐蹭障碍物。合理的取值公式是radius ceil(robotDiameter / gridResolution / 2);举例机器人直径 0.5 m栅格分辨率 0.1 m那么radius ceil(0.5 / 0.1 / 2) 3即障碍物周围 3 个栅格不可走。如果用的是 8 邻域 A*我建议半径再 1给斜向穿越留出安全余量。7.5 框架扩展从贪心到更复杂的调度策略这套框架的可扩展性相当好。想要覆盖率更高可以把地图先做区域分解比如 Boustrophedon Cellular Decomposition每个子区域内部跑往返式子区域之间的访问顺序再用一次贪心或优化算法想要应对动态障碍底层 A* 可以换成 D* Lite想要给农田、草坪这类环境生成覆盖线可以把扫描段的生长方向和作物行对齐。我现在项目里就用同一套架构做对比测试只是把顶层调度从贪心换成了简单的粒子群底层导航从 A* 换成了 D* Lite因为场景里有动态障碍物。改动基本不需要重新设计框架而是替换模块。这也印证了第一章分层的价值换模块不换架构。最后再分享一个调试阶段最有用的小习惯先把段画出来再画路径。不是直接可视化最终route而是把网格地图、扫描段用不同颜色画段线、转移路径用另一种颜色画分开展示。这样一眼就能看出调度层选段是否合理、转移路径有没有贴障碍。全景图容易骗人分段拆开看才能把问题定位到具体模块。这套方法我在不同地图上反复用了很多次每次都能快速发现新问题比对着最终效果图猜原因高效得多。