扫地机器人从客厅一角出发很多人第一反应是让它用最短路径算法冲到卧室但真实需求远没有这么简单。如果机器人只会找一条最优路线那它扫过的区域就是一条细线整块地面基本没被动过。真正要解决的是在一个有障碍物的网格环境里让机器人遍历所有可通行栅格这就是全覆盖路径规划Complete Coverage Path Planning, CCPP。我这次想聊的是 A算法 和 往返式覆盖 在 Matlab 里的组合实现A负责点对点的最优转移往返式负责区域内的逐行扫描两者拼起来做成一条完整的全覆盖路径。这篇文章适合正在学路径规划的学生、做移动机器人工程样机的开发者以及想用 Matlab 快速验证算法效果的研究者。我会先把原理拆开讲清楚再给出可以直接上手复现的核心代码排查部分也会把我在调试中踩过的坑一并整理出来。只要你的 Matlab 能正常跑脚本就行版本不用太高下面的代码也不依赖额外工具箱一张二值地图加几个函数就能把整条路径画出来。1. 全覆盖路径规划从“点到点”到“面覆盖”1.1 为什么普通A*路线解决不了扫地问题传统路径规划的任务很明确给定起点和终点找一条最短无碰撞路径。A*是这类问题里的经典算法它通过评估函数 f(n)g(n)h(n) 不断扩展节点最终找到最优路径。但在全覆盖场景下问题描述完全变了不再有一个明确终点目标变成“经过所有可通行栅格至少一次”。这是一个典型的NP难度问题你很难在一个大规模网格环境里用多项式时间算出严格最优的全覆盖路线。所以工程和研究中通常不会追求那种理论上完美的整体最优而是把问题拆开先保证覆盖率再尽量降低重复率和路径长度。这个拆解思路非常关键我拿“打扫房间”来类比。有经验的保洁不会在房间里随机乱走而是先靠墙然后一行一行推过去扫完一个区域再换另一个区域。整个过程可以拆成两部分区域内部怎么走区域之间怎么切换。“区域内部怎么走”适合用规则化的往返路径转弯少、好控制“区域之间怎么切换”适合用A*去算绕开障碍物、距离最短。这个项目本质上就是把这两件事拼起来。1.2 这个项目里 A* 往返式各自负责什么A在这个项目里不是覆盖的主体它更像一个“区域之间的连接器”。我们先把地图沿扫描方向切成若干子区域在每个子区域里用往返路径实现覆盖然后在子区域的连接点之间用A计算一条最短转移路径。这样做的优势很明显A*只需要在小范围的点对点问题上运行计算量可控而且能够自动绕开障碍物不需要人工设计复杂的跨区连接规则。如果不用A*跨区连接就得靠手工规则比如“遇到障碍就顺着墙边走”。这种规则在简单地图里勉强能用一旦障碍物多起来路径很容易绕出奇怪的形状甚至卡死。A则保证转移路径在给定代价模型下是最优的所以这套方案具备很强的通用性。你可以任意更换地图只要自由空间是连通的A都能给出一条可行的转移路径。1.3 网格环境建模二值地图和代价地图网格环境把连续空间离散成一个个格子通常用二值矩阵表示0表示可通行1表示障碍物。移动方式一般用四邻域也就是上下左右四个方向。很多初学者会问八邻域能斜着走路径不是更短吗但从全覆盖角度看往返式扫描本身是按行来走的相邻行之间垂直移动一步就到四邻域更贴合“蛇形覆盖”的运动模式。另外四邻域下A*的启发函数用曼哈顿距离就能保证最优性调试起来简单清晰。代价地图则是在二值地图之上的扩展每个格子可以有自己的通行代价比如地毯区域代价更高、墙边区域代价更低。在 Matlab 里二值矩阵就可以直接当代价地图用后续想加权就乘以一个系数。真正需要留心的是地图分辨率网格越小路径越精细但搜索和覆盖计算量也会成倍增加。演示代码里用 20×20 的地图足够说明问题实际应用时网格大小要根据机器人尺寸和定位精度来定一般一个格子对应机器人车体宽度的一半到三分之一比较合适。坐标体系上也要统一Matlab 矩阵索引是(row, col)对应地图上的(y, x)和绘图时常见的坐标方向是反的。我在实现里统一约定路径点第一列是 row第二列是 col用 imagesc 显示地图后plot 的 X 参数用 colY 参数用 row。2. A*算法核心原理评估函数与启发式搜索2.1 从两个点说起评估函数怎么引导搜索A可以理解成一种“带方向感的 Dijkstra”。Dijkstra 从起点向四周均匀扩展哪个近先扩展哪个完全没有目标方向的概念所以搜索范围会像水波一样铺开。A在扩展每个节点时用评估函数 f(n)g(n)h(n) 来排序g(n) 是从起点走到当前节点实际花的代价h(n) 是当前节点到目标节点的估计剩余代价。f 越小说明这个节点既积累了较短路径又有希望更快接近目标所以优先扩展。这里有个关键点h(n) 的估计越准搜索节点越少但如果 h(n) 高估了真实最小代价A就可能错过最短路径。数学上要求 h(n) 可采纳也就是说它永远不能高估到目标的真实代价。在四邻域网格里曼哈顿距离就是最经典的可采纳启发函数这也是我选择它的原因。实际运行中你会看到A的搜索范围会明显偏向目标方向而不是像 Dijkstra 那样铺满整张图。2.2 启发函数为什么选曼哈顿距离在四邻域网格里两个格子之间的最短路径至少要经过 |dx||dy| 步这正好是曼哈顿距离。把它作为 h(n)它永远不会高估真实剩余代价因此是可采纳的A* 返回的路径一定是最短路径。如果换成八邻域情况就变了斜着走一步代价约为 1.414此时曼哈顿距离会高估真实代价。比如目标在右上方对角线方向曼哈顿距离是 2但走斜对角其实只需要 1.414 步这个 h 值比真实代价还大A* 就可能返回次优路径。八邻域下更适合用欧氏距离或切比雪夫距离 max(|dx|, |dy|)。移动方式推荐启发函数能否保证最优说明四邻域曼哈顿距离能与往返扫描的移动模型一致八邻域切比雪夫距离或欧氏距离能搜索范围随地图不同有差异四邻域带权重加权曼哈顿距离不能严格保证搜索更快但路径可能略长上面表格里最后一行是扩展玩法如果觉得搜索太慢可以把曼哈顿距离乘上一个大于1的权重比如 h 1.2 * manhattan(...)这样就变成加权 A*搜索速度更快但路径会略长。这个技巧在研究对比中经常出现不过初学阶段建议先保持 h 严格可采纳把基线做对。2.3 open表、closed表与父节点回溯A* 经典实现需要两张表open 表存放待扩展节点closed 表存放已经扩展过的节点。算法每次从 open 表里取出 f 值最小的节点把它加入 closed 表再遍历它四周可通行的邻居。如果邻居不在 closed 表并且新算出的 g 值更小就更新邻居的 g、f同时记录父节点指向当前节点。当目标节点被移出 open 表时就可以沿父节点一路回溯到起点得到完整路径。父节点回溯是整个实现里最容易写错的地方。常见错误是只在第一次发现邻居时记录父节点后面发现更短路线时不更新结果画出来的路径是“最短的 g 值加上旧父节点”的混合体弯弯绕绕。正确做法是每次 g 更新后必须同步更新父节点。父节点建议用二维索引数组存储也就是把子节点的父节点坐标转成一维索引存进 parent 矩阵里。这样内存省、访问快回溯时用 ind2sub 还原坐标即可。2.4 Matlab实现A*时的几个数据结构选择Matlab 不是写算法最优雅的语言但做验证和可视化是真的舒服。网格尺寸不大时比如 200×200 以内我建议直接用矩阵存 g 值、f 值和父节点open 表用一个 N×3 数组维护每轮用 min 函数找 f 最小值。这样代码最好懂也方便给别人讲。缺点就是每次找最小值和删除节点都是 O(N) 操作网格非常大时会明显变慢。如果后续要扩大地图规模可以考虑用二叉堆优化 open 表或者借用 Java 的优先级队列。但 Matlab 里调 Java 自定义比较器比较麻烦转换数据类型还会踩坑。对绝大多数演示和论文验证场景矩阵加线性搜索已经够用跑一块 50×50 的地图基本是毫秒级。我后面给的代码就是这种简单直接的实现重点是把算法逻辑讲清楚而不是把时间耗在堆结构上。3. 往返式全覆盖路径生成思路3.1 往返式牛耕式路径的特点往返式覆盖也叫牛耕式覆盖名称来自农田耕地拖拉机从地头开到地尾掉头再开回来一条一条把整块田耕完。映射到网格环境里就是把地图按行划分成多条扫描带在每条带里从左到右或从右到左直行然后换行继续反向走形成“弓字形”路径。这种路径最大的优点是简单、稳定、转弯次数少非常适合大多数地面机器人大部分时间直线行驶只在换行处转弯磨损小、控制难度低。但纯往返式路径只适合无障碍或障碍极少的“干净”环境。地图一旦出现孤立障碍物一整行可能被切成好几段如果机械地从左到右走就会撞上障碍。这就是为什么要对地图做区域分解把自由空间切成若干子区域在每个子区域内执行往返覆盖再在子区域之间用 A* 搭桥。3.2 扫描线分解把复杂环境切成子区域扫描线分解的思路是用一个水平扫描线从上往下扫过地图记录每一行自由空间的连续区间。如果相邻两行的区间有重叠说明它们属于同一个自由空间区域可以合并到同一个子区域里如果区间断了说明中间出现了障碍物边界就把当前子区域切一刀形成新的子区域。听起来抽象但实现可以简化。演示项目里我采用了一种“逐行提取连续自由区间然后用 A* 桥接段间连接”的策略每一行会得到若干 [起点列, 终点列] 的区间这些区间就是基本覆盖单元。相比严格的 Boustrophedon 分解这种简化方式牺牲了一些区域合并的规整性但代码非常直观也更容易保证覆盖率。更精细的合并算法可以作为后续扩展方向比如把相邻行之间列区间完全相同的段合并成一个矩形 cell再在 cell 内部生成蛇形路径。3.3 子区域内部怎么生成蛇形路径在得到一个连续的横向区间后生成蛇形路径就很机械了。以行为单位逐行推进偶数行从左到右奇数行从右到左走完当前行后垂直向下移动一行继续反向走。对网格来说一行内的覆盖路径就是这一行从左端点到右端点所有格子的顺序排列换行时再自然连接下一行的端点。这里有一个细节容易踩坑每行往返的两个端点必须取该行实际可通行区间的端点而不是简单套用整个矩形区域的左右边界。否则路径会把障碍物当成可通行格子划进去。实现时每一行的连续区间要从地图中逐格扫描得到不要直接硬编码起点和终点。这样即使在边界不规则的复杂地图里也不会漏覆盖或穿墙。3.4 子区域之间的连接交给A*去收尾逐行扫描后会得到一串覆盖段。我们需要把这些段的首尾接起来组成一条完整覆盖路径。由于段之间可能被障碍物隔开不能简单地从上一段终点直线走到下一段起点这时候就轮到 A* 出场把上一段的终点作为起点下一段的起点作为终点用 A* 算一条绕开障碍物的转移路径拼到完整路径里。访问顺序可以用一个简单的规则确定偶数行从左往右奇数行从右往左。这样整体看就是一个完整的往返覆盖中间断点由 A* 补上。转移路径会重复经过部分已覆盖栅格所以它们不计入覆盖率但会贡献路径总长度和重复率。在评价算法效果时覆盖路径和转移路径要分开看才能衡量 A* 对整体路径的优化程度。4. Matlab代码实现核心函数与主流程4.1 地图构建与可视化先把地图和起点摆出来。下面这段代码构建一张 20×20 的网格地图四周设置边界障碍内部放几个矩形障碍块这样地图既有覆盖区域又能触发 A* 连接逻辑% ------------------------------- % 1. 构建网格地图 % ------------------------------- map zeros(20, 20); map(5:8, 12:15) 1; % 障碍块1 map(12:14, 4:7) 1; % 障碍块2 map(16:18, 10:12) 1; % 障碍块3 map(1, :) 1; % 上边界 map(20, :) 1; % 下边界 map(:, 1) 1; % 左边界 map(:, 20) 1; % 右边界 start [2, 2]; % 机器人起点格式为[row, col] % 显示地图 figure; imagesc(map); colormap(gray); axis equal; axis tight; hold on; plot(start(2), start(1), go, MarkerSize, 10, LineWidth, 2);地图显示后你应该能看到黑色障碍块和白色自由区域。这里的灰色背景表示0黑色表示1。记住绘图坐标和矩阵索引的对应关系plot 的第一个参数对应 col第二个参数对应 row后面画路径时千万别搞反。4.2 A*路径搜索函数A* 是整个方案的“连接器”我先把核心函数完整贴出来然后逐段解释% ------------------------------- % A* 路径搜索四邻域 % 输入map 二值地图start 起点[row,col]goal 终点[row,col] % 输出pathN行2列的坐标序列找不到路径时返回空数组 % ------------------------------- function path astar_path(map, start, goal) [rows, cols] size(map); % 起终点合法性检查 if map(start(1), start(2)) 1 || map(goal(1), goal(2)) 1 error(起点或终点不能位于障碍物上); end % 代价矩阵和父节点矩阵 g inf(rows, cols); f inf(rows, cols); parent zeros(rows, cols); % 父节点用一维索引存储 g(start(1), start(2)) 0; f(start(1), start(2)) manhattan(start, goal); % open表每行是 [row, col, f] openList [start(1), start(2), f(start(1), start(2))]; closed false(rows, cols); % 四邻域方向 dirs [0 1; 0 -1; 1 0; -1 0]; while ~isempty(openList) % 取f值最小的节点 [~, idx] min(openList(:, 3)); cur openList(idx, 1:2); openList(idx, :) []; % 到达目标 if isequal(cur, goal) path reconstruct_path(parent, start, goal); return; end closed(cur(1), cur(2)) true; % 扩展四个方向 for k 1:size(dirs, 1) nr cur(1) dirs(k, 1); nc cur(2) dirs(k, 2); % 边界检查 if nr 1 || nr rows || nc 1 || nc cols continue; end % 障碍物或已扩展 if map(nr, nc) 1 || closed(nr, nc) continue; end tentative_g g(cur(1), cur(2)) 1; if tentative_g g(nr, nc) g(nr, nc) tentative_g; f(nr, nc) tentative_g manhattan([nr, nc], goal); parent(nr, nc) sub2ind([rows, cols], cur(1), cur(2)); openList [openList; nr, nc, f(nr, nc)]; end end end path []; % open表耗尽找不到路径 end % 曼哈顿距离 function h manhattan(node, goal) h abs(node(1) - goal(1)) abs(node(2) - goal(2)); end % 回溯路径 function path reconstruct_path(parent, start, goal) [rows, cols] size(parent); path []; cur goal; while ~(cur(1) start(1) cur(2) start(2)) path [cur; path]; idx parent(cur(1), cur(2)); if idx 0 break; end [cur(1), cur(2)] ind2sub([rows, cols], idx); end path [start; path]; end这段代码有几点需要专门说。第一g 和 f 矩阵初始化为 inf这样第一次发现邻居时肯定满足 tentative_g g(nr, nc)能顺利写入初始值。第二openList 采用简单追加方式同一个节点可能会被加入多次但因为有 closed 判断和 g 值更新逻辑最终结果不会出错只是会多几次无效扩展。对 20×20 的地图完全够用。第三曼哈顿距离在四邻域模型下保证最优性这一点前面已经解释过。如果你试跑后想在更大的地图上测试建议先优化 open 表去重再考虑堆结构。我给的这个版本属于“教学优先”正确性和可读性都很好但性能还有提升空间。4.3 逐行往返覆盖路径生成接下来是核心的覆盖路径生成函数。思路很直接逐行提取自由区间按蛇形顺序生成覆盖路径段与段之间用 A* 连接。下面先给出行区间提取辅助函数% ------------------------------- % 提取第r行的连续自由区间 % 返回 segs [cL, cR] 的矩阵按列坐标升序排列 % ------------------------------- function segs get_row_segments(map, r) [~, cols] size(map); segs []; c 2; % 跳过左边界 while c cols - 1 % 跳过右边界 if map(r, c) 0 cL c; while c cols - 1 map(r, c) 0 c c 1; end cR c - 1; segs [segs; cL, cR]; else c c 1; end end end有了每个行的自由区间再用一个主函数把覆盖路径拼出来% ------------------------------- % 生成完整覆盖路径 % 偶数行从左到右奇数行从右到左段间用A*连接 % ------------------------------- function fullPath generate_cover_path(map, start) [rows, ~] size(map); fullPath []; prevEnd start; direction 1; % 1表示从左到右-1表示从右到左 for r 2:rows-1 segments get_row_segments(map, r); if isempty(segments) continue; end % 根据当前方向排序段 if direction 1 segments sortrows(segments); % 左端点升序 else segments flipud(sortrows(segments)); % 左端点降序 end for i 1:size(segments, 1) cL segments(i, 1); cR segments(i, 2); % 当前段的实际覆盖路径逐格排列 if direction 1 segPath [r * ones(cR - cL 1, 1), (cL:cR)]; else segPath [r * ones(cR - cL 1, 1), (cR:-1:cL)]; end % 段起点和终点 segStart segPath(1, :); segEnd segPath(end, :); % 用A*连接上一段终点到当前段起点 if ~isequal(prevEnd, segStart) conn astar_path(map, prevEnd, segStart); if isempty(conn) error(无法从[%d,%d]连接到[%d,%d], ... prevEnd(1), prevEnd(2), segStart(1), segStart(2)); end fullPath [fullPath; conn(2:end, :)]; end % 拼接当前段覆盖路径 fullPath [fullPath; segPath(2:end, :)]; prevEnd segEnd; end % 换行后反向 direction -direction; end end这个函数运行后fullPath 就是一条连续路径所有自由栅格都会被覆盖。如果地图存在不可达孤岛A* 会返回空数组函数会报错并提示具体位置。这种“宁可报错也不静默跳过”的做法在调试阶段能帮你快速定位问题。4.4 主流程与完整路径拼接主流程代码很简单先生成覆盖路径再可视化结果。% ------------------------------- % 主流程 % ------------------------------- clear; close all; clc; % 构建地图和起点 map zeros(20, 20); map(5:8, 12:15) 1; map(12:14, 4:7) 1; map(16:18, 10:12) 1; map(1, :) 1; map(20, :) 1; map(:, 1) 1; map(:, 20) 1; start [2, 2]; % 生成完整覆盖路径 fullPath generate_cover_path(map, start); % 可视化 figure; imagesc(map); colormap(gray); axis equal; axis tight; hold on; plot(fullPath(:, 2), fullPath(:, 1), b-, LineWidth, 1.5); plot(start(2), start(1), go, MarkerSize, 10, LineWidth, 2); title(A*辅助的往返式全覆盖路径);跑完这段代码你应该能看到一条从起点出发、按蛇形逐行扫描、遇到障碍自动绕行的蓝色路径。路径遇到障碍块时会绕个小弯然后继续回到原来的覆盖顺序上这就是 A* 在起作用。4.5 结果统计覆盖率、重复率与路径长度全覆盖规划一般看三个指标覆盖率、路径长度、重复率。统计方式很简单把最终路径经过的格子标记到 visited 矩阵里% ------------------------------- % 统计指标 % ------------------------------- visited false(size(map)); for i 1:size(fullPath, 1) r fullPath(i, 1); c fullPath(i, 2); if map(r, c) 0 visited(r, c) true; end end freeCount sum(map(:) 0); % 自由栅格总数 coverCount sum(visited(:)); % 实际覆盖的自由栅格数 coverRate coverCount / freeCount * 100; % 覆盖率 pathLen size(fullPath, 1) - 1; % 路径长度步数 repeatRate (pathLen - coverCount) / coverCount * 100; % 重复率 fprintf(覆盖率: %.2f%%\n, coverRate); fprintf(路径长度: %d 步\n, pathLen); fprintf(重复率: %.2f%%\n, repeatRate); % 画已覆盖栅格绿色半透明 [rIdx, cIdx] find(visited (map 0)); plot(cIdx, rIdx, g., MarkerSize, 4);如果地图自由空间完全连通这个方案的覆盖率理论上能到 100%如果存在被障碍完全包围的独立区域覆盖率就会低于 100%同时 A* 会在拼接时报错。这就是引导你去处理不可达区域的一个重要信号。重复率则主要来自 A* 连接段因为转移路径会踩到已经覆盖过的格子。5. 常见问题与调试经验5.1 open表反复加入节点导致搜索变慢我前面说过演示版 A* 的 openList 用追加方式同一个节点可能被加入多次。地图小的时候没感觉地图一变大openList 里会出现大量重复记录导致每轮 min 搜索都要扫描更多行搜索效率明显下降。解决办法是在加入 openList 前检查该节点是否已经在 openList 中如果已在且新 f 更小就更新已有记录的 f而不是追加新行如果不在再追加。从调试经验看地图尺寸在 40×40 以下时线性扫描 openList 基本感觉不到卡顿到了 100×100 且障碍复杂时一次 A* 可能要处理几万个节点线性扫描会明显变慢。这时候优先检查 openList 去重是否做好往往比直接上堆结构提升更明显因为重复节点常常会占掉大量无意义的扫描次数。5.2 明明有通道却搜不到路径遇到 A* 返回空路径先检查三件事。第一起点或终点是否落在障碍格上这是最常见的低级错误建议在函数入口加 error 判断一秒钟就能发现问题。第二四邻域下无法斜穿对角缝如果地图里只有一条对角线宽度的缝隙而机器人又必须走四邻域那它会认为这条路不可行这种情况不算 bug是运动模型限制。第三启发函数是否可采纳如果 h 高估A* 可能提前终止并返回次优路径现象就是路径明显绕远但并不是不可达。现象可能原因快速排查路径为空起终点在障碍物上输出 start、goal 对应 map 值路径为空存在不可达孤岛用连通性检测检查自由区域路径绕远启发函数不可采纳换回曼哈顿距离搜索卡顿open表重复节点多加去重逻辑覆盖漏格行区间提取有误把每行 segments 打印出来5.3 覆盖路径有漏格或重复过多漏格主要出现在 get_row_segments 对边缘区间的处理上。我最开始实现时循环边界写成了 1:cols结果把左边界那一列也当成自由空间路径会穿过墙。后来统一用 2:cols-1 跳过边界问题就消失了。如果你自定义地图时把障碍物放在内部任意位置也要注意区间提取要完整覆盖整行不能因为某个列是障碍就把后面的自由区间丢掉。重复路径过多一般来自 A* 连接段太长。减少重复有两条路一是段访问顺序改成贪心最近邻每次从当前段终点出发找离它最近的未访问段可以明显缩短转移距离二是在生成蛇形路径时考虑出口方向让上一段终点尽量靠近下一段起点。前者改动小、提升快推荐先做。5.4 大网格地图性能优化建议如果要在 200×200 甚至更大的地图上做全覆盖验证建议做两件事。第一把“覆盖路径生成”和“A连接路径”分开看只在段间确实需要绕行时才调用 A不要在整个全覆盖过程中反复对大网格跑搜索。第二正式使用前把地图做一次膨胀处理也就是把障碍物周边 N 格也标记为障碍。这样能模拟机器人的实际物理尺寸避免规划出的路径贴着墙脚走后续控制会安全很多。Matlab 里可以手写两层循环完成膨胀也可以借助图像处理工具箱的 imdilate看自己环境决定。5.5 路径后处理去冗余拐点A* 按栅格搜出来的转移路径经常有“明明能直线走却非要拐直角”的感觉原因是栅格步进导致路径里有大量中间点。可以做一个很简单的拉直处理从路径起点开始每次尝试连接更远的点如果连接线段经过的所有栅格都是自由的就把中间点删掉。这个操作在网格地图上就是逐格遍历逻辑不复杂。但要注意拉直只建议用在 A* 转移路径上不能对覆盖路径做。往返覆盖路径本身就是“直行加掉头”的结构如果强行拉直可能把掉头处的关键拐点删掉覆盖顺序就乱了。我一般会让覆盖路径保持原样只把转移路径拉直这样机器人实际走起来会顺很多。这个项目的价值不仅在于跑通一条覆盖路径更重要的是让你理解点到点规划和区域覆盖规划之间的层次关系。A* 不是被全覆盖替代而是成为全覆盖系统里的一个基础组件。接下来你可以试着把段访问顺序改成贪心最近邻或者给地图加上代价权重看看重复率和路径形态会怎么变化。我就是在这些小的改动里慢慢建立起对路径规划算法调优的直觉的。