前阵子整理扫地机全覆盖路径的Demo正好把手里这套基于A*A星算法的网格环境下的往返式全覆盖路径规划方案重新捋了一遍。这个题目看着长拆开其实就三层网格地图建模、往返式扫描覆盖、A*做段间转移。这篇文章我打算把从地图建模到Matlab代码落地的整套思路讲清楚包含我自己实测时踩过的坑和参数调整经验适合正在做机器人路径规划课设、竞赛或者准备入门全覆盖路径规划CPP的同学参考。先给一个整体认知A在这个项目里的角色不是用来规划覆盖路径的主体而是解决覆盖到一半怎么绕障碍跳到下一行的转移问题。往返式覆盖负责基础形态A负责灵活绕障两者配合才能得到一条覆盖率足够高、重复率尽量低的完整覆盖轨迹。1. 这个课题到底在解决什么问题全覆盖路径规划Coverage Path PlanningCPP的核心诉求很简单让机器人走遍工作区域内所有需要覆盖的位置同时尽可能少走冤枉路。日常能见到的例子太多了——扫地机器人要跑遍每个房间角落植保无人机要扫过整块农田仓储AGV要巡检到每一排货架的通道洗地车要清洗整个商场地面。只要任务是覆盖一个面而不是从A点走到B点就是CPP问题的范畴。1.1 全覆盖的三个评价指标评估一条覆盖路径好不好主要看三个数覆盖率、重复率、转弯能耗。覆盖率是已覆盖网格数占全部可达网格数的百分比这是硬指标覆盖率不达标说啥都白搭。重复率是重复覆盖的网格数占总覆盖网格数的比例重复率越高说明机器人越在做无用功整体效率越低。第三个指标经常被忽略——转弯次数和转弯角度。轮式机器人原地调头、减速转向都是很耗能量的动作尤其对室内清扫来说转弯越多边刷和吸尘结构磨损越大。这三个指标在实际工程里其实是互相冲突的。一味追求低重复率可能让转移路径变得很长一味追求少转弯又可能漏掉某些狭窄区域。所以设计算法时要在三者之间找一个平衡点而不是只盯着单一指标。1.2 全覆盖主流策略对比常见的基础覆盖策略有随机法、往返式牛耕式、内螺旋式、基于势场法、基于区域分解法几种。我做了个简单对比表格策略覆盖形态优点缺点随机法任意游走实现简单无需完整地图覆盖率低重复率高往返式平行直线扫描覆盖率稳定、路径整齐、转弯规律障碍多时需频繁处理间断区段内螺旋式从外向内环形收缩适合近似方形的大空间对长条形或凹凸地形不友好区域分解法把空间切成若干子区域分别覆盖对复杂地形适应性好分解逻辑复杂计算开销大往返式A*段间转移平行扫描绕障转移兼顾覆盖率和绕障能力工程落地性好需要完整地图环境变化时鲁棒性下降往返式之所以成为工程首选是因为它的路径形态简单、控制方便同时可预测性强。你只要确定了扫描方向和行间距机器人基本上就是老老实实地走平行线不容易漏掉大块区域。但在有障碍物的环境里障碍物会把一行扫面路径拦断成好几个区段标准的往返式直接连续走会撞墙所以需要在区段切换之间引入绕障转移能力——这正是A*的用武之地。1.3 A*在往返式方案中的真正位置很多初学者拿到这个问题第一反应是用A规划一条全覆盖路径。这个思路其实是错的。A本质上是点到点的最短路径搜索算法它解决的问题是给定起点和终点找到一条代价最小的可行路径而不是如何把整个面覆盖完。所以正确的架构是往返式扫描负责覆盖主区域每当机器人扫完一个连续区段需要转移到下一个区段起点时调用A*搜索出一条安全、较短的转移路径。这条转移路径经过的网格也要标记为已覆盖或单独记录否则会重复扫或留下空白。明白了这个分工后面读代码就不容易被绕晕。2. 网格地图建模从真实环境到栅格矩阵网格环境是整套方案的工作基础。在Matlab里网格地图本质上就是一个二维矩阵每个格子对应实际环境中的一个矩形区域矩阵的值表示该区域是否可通行。2.1 栅格矩阵的坐标约定假设我要规划一个5米×5米的房间把每个格子设成0.1米×0.1米那么地图矩阵就是50×50。矩阵里我用0表示可通行1表示障碍物数值越大表示代价越高的区域可以后面扩展。这里有一个非常容易踩的坑矩阵的行列索引和平面坐标的对应关系。Matlab的矩阵索引是行列行从上到下递增列从左到右递增。如果直接拿行列当坐标用y轴方向是反的。我习惯在代码里保持自然坐标约定即x表示列方向y表示行方向但注意y轴朝下。如果涉及显示和计算需要统一约定别一会儿用行列一会儿用坐标很容易出bug。2.2 4邻域还是8邻域A*搜索和往返式覆盖都涉及从当前格子可以走到哪些格子的问题。4邻域允许上下左右四个方向移动8邻域额外加上四个对角方向。两者的区别很实际4邻域路径更保守、更平滑适合不能斜着走的机器人比如差速驱动8邻域路径更短但需要机器人支持斜向运动而且对角穿越时可能存在蹭角风险。我做实验时通常会根据实机能力选择。如果模拟仿真用8邻域更高效如果要对接到实物机器人先看运动控制支不支持斜向直线走。支持的话用8邻域没问题不支持就老老实实4邻域。2.3 障碍膨胀处理现实中的机器人有体积不是质点。即使规划路径擦着障碍物边缘走实际车体也会撞上去。所以在路径规划前必须对障碍物做膨胀inflation处理。膨胀半径取机器人最大外接圆半径再加上一个安全距离。膨胀的计算在栅格图上很简单找到所有障碍格子然后把这些格子周围半径r范围内的所有格子都标记为障碍。r的单位是格子数等于机器人半径除以格子边长。比如机器人半径0.2米格子边长0.1米那膨胀半径就是2个格子。这一步必须在任何路径搜索之前做否则后面A*搜出来的路径再短也没有工程意义。3. 往返式覆盖的主循环设计往返式覆盖也叫牛耕式boustrophedon覆盖名字来源于古希腊人用牛耕地时沿垄沟来回犁地的场景。放在网格环境里核心思想就是从地图的一端开始沿一条直线扫到另一端然后切换到下一行反向扫如此反复像蛇形一样覆盖整个区域。3.1 纵向扫描还是横向扫描第一步要决定扫描方向。这个选择很影响覆盖效率和转移路径长度。通常看地图的长宽比地图明显是横向宽、纵向窄的就沿纵向逐行扫每个来回的直线段长转弯次数少反过来就沿横向逐列扫。更精细的做法是画出地图的矩形包围盒比较宽和高让扫描方向沿着较长的那个维度这样可以减少总转弯次数。不过在有障碍物的情况下还要考虑障碍物的分布。如果障碍物大多是横向长条最好让扫描线沿横向这样障碍物对每行的拦断次数少如果障碍物是竖向长条就沿竖向扫。简单说就是让扫描方向尽量不对着障碍物的长边。3.2 单行扫描逻辑选定扫描方向后主循环其实不难写。我从地图的左上角起始设定一个起点行索引然后对这一行从左到右遍历所有可通行格子。对每个格子执行逗留模拟清扫动作并标记为已覆盖。当这行走到头或者前方被障碍物或地图边界挡住时当前连续区段的覆盖就结束了。这里有个细节要注意一行里可能有多个可通行区段被障碍物隔开。比如地图中间有个柜子一行扫描线会被柜子打断成左右两段。完整的覆盖逻辑应该先把当前行的所有不连续区段都扫完再考虑跳到下一行的区段还是扫完一段就立刻跳工程上通常的做法是先把当前能连续覆盖的区段扫完然后跳转到最近的一个未覆盖区段继续扫不管它在当前行还是相邻行——这样可以避免在同一行过多来回跳。3.3 行间切换与区段转移当一行扫完就要处理跳到下一行的动作。如果相邻行的起始位置是可达的直接走一格过去就行。但当两行之间有障碍物隔开时比如扫描线被墙或者障碍截断下一行对应位置的格子可能是障碍无法直接下移就需要调用A*规划一条从当前位置到下一个目标区段起点的绕障路径。这也就是前面说的A的职责所在。在完整覆盖过程中A可能被调用很多次每一次的起点是当前覆盖位置终点是下一个未覆盖区段的起始点。转移完成后机器人从终点继续执行往返式扫描。4. A*算法在段间转移中的实现细节A*算法的原理很多资料里都有这里不重复基础知识只讲在网格转移场景下真正影响效果的几个关键细节。4.1 启发式函数的选择A的核心是评估函数f(n)g(n)h(n)。g是从起点到当前节点的实际代价h是从当前节点到目标的估计代价。h的选择决定了搜索效率和最优性。如果h始终不大于实际最小代价A保证能找到最优路径这个性质叫admissible可采纳性。h估计越接近真实代价搜索扩展的节点越少速度越快。在这个场景里三个常见选项启发式适用邻域特点曼哈顿距离4邻域h 实际代价可采纳搜索效率中等欧几里得距离8邻域h 实际代价可采纳但往往过于乐观扩展节点偏多对角距离8邻域更接近现实代价搜索更快且不影响最优性符合可采纳条件我在代码里用的是对角距离。公式是max(dx, dy) (sqrt(2) - 1) * min(dx, dy)其中dx是横向格子差dy是纵向格子差。这个公式同时考虑了直线移动和对角移动的代价差异。4.2 邻域扩展与代价设置在8邻域下从当前格子扩展邻居时水平垂直移动代价设为1对角移动代价设为sqrt(2)约1.414。这样搜索出来的路径更贴近真实运动距离不会出现宁可对角线也不走直线这类偏爱式问题。有一个常见错误是对角移动代价设成1和水平移动一样。这样会导致A*倾向于使用对角线路径因为用对角的格子距离更短但代价却一样等于鼓励走斜线最终路径看起来会很怪。4.3 路径重构与输出A*搜索过程中不断更新每个节点的父节点终点找到后从终点沿着父节点链回退到起点得到完整的格点序列。这段序列就是转移路径。在返回Matlab主程序后我会把这串格点逐点标记为已覆盖状态同时对每个格点做一次覆盖动作的通告。实际可能会遇到一个问题A*转移路径经过的某些格子在后续往返式扫描中本来应该被覆盖现在提前被转移路径覆盖了。这没问题只要标记状态统一后边扫描遇到这些格子时直接跳过即可。但如果忽略这步就会在重复率统计里看到一个莫名其妙的偏高重复率。5. Matlab代码实现与关键模块解析下面讲代码层面的落地。我采用的工程结构包含四个文件主脚本、地图生成函数、A*搜索函数、往返式覆盖主循环函数。代码结构清晰方便调试和扩展。5.1 主程序结构主脚本做四件事定义地图参数、生成网格地图、调用覆盖算法、绘制结果。我一般把参数都集中在脚本开头方便反复实验。% 主参数设置 mapSize [50, 50]; % 地图尺寸行 x 列 cellSize 0.1; % 每个格子的实际尺寸单位米 robotRadius 0.2; % 机器人最大外接圆半径单位米 startPos [1, 1]; % 起始网格位置行列 scanDirection col; % 扫描方向col表示沿列方向来回扫 inflationR ceil(robotRadius / cellSize); % 膨胀半径单位格子数 % 生成地图1 表示障碍0 表示可通行 map generateMap(mapSize, inflationR); % 执行覆盖路径规划 [coverPath, coverGrid, metrics] boustrophedonCover(map, startPos, scanDirection); % 可视化结果 plotCoverage(map, coverPath, coverGrid, metrics);这里的地图我预置了一些障碍方便验证绕障效果。5.2 往返式覆盖主循环往返式覆盖主循环是整个程序的核心。思路是维护一个待覆盖格子列表每次从列表里取出一个起始格子然后沿指定方向连续覆盖直到撞到障碍或边界然后寻找下一个最近的未覆盖格子调用A*转移过去继续重复。function [coverPath, coverGrid, metrics] boustrophedonCover(map, startPos, scanDirection) [rows, cols] size(map); coverGrid map; % 0可通行, 1障碍, 2已覆盖, 3转移路径 coverGrid(startPos(1), startPos(2)) 2; coverPath startPos; currentPos startPos; % 用于标记扫描方向是否调转 forward 1; while true % 沿当前方向覆盖可行的一段 [currentPos, coverGrid, coverPath] ... scanLine(currentPos, forward, scanDirection, coverGrid, coverPath); % 找出下一个未覆盖的目标格子 nextTarget findNextTarget(currentPos, coverGrid, scanDirection); if isempty(nextTarget) break; end % 用A*规划转移路径 if ~isequal(currentPos, nextTarget) transferPath astarSearch(coverGrid, currentPos, nextTarget); if isempty(transferPath) % 如果A*找不到路径说明目标不可达标记为障碍可选? 这里做一个保守处理 coverGrid(nextTarget(1), nextTarget(2)) 1; continue; end % 把转移路径加入总路径并标记为已覆盖 for i 2:length(transferPath) p transferPath{i}; if coverGrid(p(1), p(2)) 0 coverGrid(p(1), p(2)) 3; coverPath(end1, :) p; end end currentPos nextTarget; end % 切换扫描方向 forward -forward; end metrics computeMetrics(coverGrid, map); endscanLine函数负责从当前点沿某方向连续行走遇到障碍或边界就停。实现比较直白就是一个while循环不断尝试沿扫描方向前进。5.3 A*搜索函数A*函数的输入是当前地图状态、起点和终点输出是格点序列路径。我用了一个简单的优先队列实现标准Matlab没有内置堆结构所以我用一个cell数组维护open list每次寻找代价最小的节点。地图规模在100×100以内性能完全够用。如果地图更大可以用MATLAB内置的containers.Map配合排序优化或者直接写成C mex函数这里不展开。function path astarSearch(map, startPos, targetPos) [rows, cols] size(map); openList struct(pos, {startPos}, g, 0, h, 0, f, 0, parent, []); closedList false(rows, cols); neighbors [1,0; -1,0; 0,1; 0,-1; 1,1; 1,-1; -1,1; -1,-1]; costs [1, 1, 1, 1, sqrt(2), sqrt(2), sqrt(2), sqrt(2)]; while ~isempty(openList) % 取出f最小的节点 [~, minIdx] min([openList.f]); currentNode openList(minIdx); openList(minIdx) []; % 到达终点重构路径 if isequal(currentNode.pos, targetPos) path reconstructPath(currentNode); return; end closedList(currentNode.pos(1), currentNode.pos(2)) true; % 扩展邻居 for k 1:8 np currentNode.pos neighbors(k, :); if np(1) 1 || np(1) rows || np(2) 1 || np(2) cols continue; end if map(np(1), np(2)) 1 || closedList(np(1), np(2)) continue; end g currentNode.g costs(k); h max(abs(np(1)-targetPos(1)), abs(np(2)-targetPos(2))) ... (sqrt(2)-1)*min(abs(np(1)-targetPos(1)), abs(np(2)-targetPos(2))); f g h; % 检查openList中是否已有该节点如果已有且g更小则跳过 idx findNodeInOpenList(openList, np); if isempty(idx) newNode struct(pos, np, g, g, h, h, f, f, parent, currentNode); openList(end1) newNode; else if g openList(idx).g openList(idx).g g; openList(idx).f g openList(idx).h; openList(idx).parent currentNode; end end end end path {}; % 无路可达 end这里有个值得注意的点openList用cell数组不断插入删除在节点数量放大后会变慢。如果地图是200×200以上建议换成带优先级的二叉堆。我实测100×50的地图A*单次搜索时间在几十毫秒量级跑完整覆盖过程大概是几秒对研究验证完全够用。5.4 可视化与结果统计规划完成后我通常把路径画在地图上用颜色区分覆盖格子、障碍格子和转移路径这样一眼能看出覆盖是否均匀、转移路径是否绕远。function plotCoverage(map, coverPath, coverGrid, metrics) figure; imagesc(coverGrid); colormap([1 1 1; 0 0 0; 0.8 0.8 0.8; 0.2 0.6 1]); hold on; plot(coverPath(:,2), coverPath(:,1), r, LineWidth, 1.2); title(sprintf(覆盖率: %.2f%% 重复率: %.2f%%, ... metrics.coverRate, metrics.repeatRate)); xlabel(列); ylabel(行); axis equal; grid on; end这里有个小细节imagesc显示时需要注意矩阵转置否则图的方向和实际坐标方向不一致。我第一次跑通时就因为这个方向不对一度以为是路径计算错了调试了半天。5.5 覆盖率的统计口径统计覆盖率时要区分已覆盖格子数和可覆盖格子总数。我把转移路径经过的格子也计入已覆盖因为机器人确实走过了那里从清扫效果上说也是覆盖了。重复率的统计同理把转移路径重复经过的格子也算入重复次数。这样做出来的数据更贴近实际使用效果。6. 实测结果、边缘情况与调参心得代码跑通之后我做了两组实验一组是空旷地图均匀扫描另一组是带多个障碍物的复杂地图。动态调整几个参数后得到了一些很有价值的观察。6.1 两种场景的覆盖率统计空旷地图上往返式覆盖几乎完美覆盖率接近100%重复率约0%路径呈标准蛇形。这也是往返式的底气所在——没有障碍的时候它本身就是最优解。带障碍物地图上覆盖率通常能达到95%以上重复率在5%左右。重复的来源主要有两个一是A*转移路径可能会穿过某些区段二是当目标点选择策略不当时会出现绕路到另一行起点、结果又在中途回到已覆盖区域的情况。合理设计findNextTarget策略可以显著降低这部分重复。6.2 踩坑记录我做这个方案时遇到过几个实际问题写出来给大家参考。第一个坑目标点选择策略不能只找最近的未覆盖格子。如果每扫完一个区段就找最近的未覆盖格子可能导致机器人在小范围内反复横跳形成大量短距离转移整体路径变得非常碎。我的改进是加入方向偏好优先寻找当前行进方向附近的未覆盖格子让整体扫描保持蛇形的大趋势。第二个坑A*在8邻域下穿角问题。当路径沿着障碍物边缘走时8邻域扩展可能让路径贴着一个角过去实际机器人转弯半径不够就会撞到角。如果你的机器人转向半径有限可以考虑把邻域降为4邻域或者在障碍物周围额外多膨胀一格。第三个坑扫描方向切换时的起点位置计算。很多往返式实现默认下一行的起点就是当前行起点的正下方但这只有在没有障碍遮挡时成立。我的实现里每次都重新搜索下一行的第一个未覆盖格子然后A走转移路径逻辑上更稳代价是多几次不必要的A调用。我后来加了判断如果当前点正下方可通行就直接下移走不了再调A*效率高很多。第四个坑地图边界处理。Matlab索引从1开始边界判断稍不注意就数组越界。我习惯在邻域扩展里另外做一次bounds检查不要想着依赖try-catch兜底因为一旦越界就可能导致整个仿真中断。6.3 扩展方向这套框架本身很灵活稍微改一改就能应对更多场景。比如动态障碍时可以每走几步更新一次coverGrid障碍变了对A重新搜索多机协同覆盖时可以给每个机器人分配一个子区域各自跑往返式覆盖区域间用A做连通高分辨率地图下可以把网格金字塔分层先在粗网格上规划全覆盖再在细网格上做局部优化。我个人更推荐往分区阈值的自适应选择这个方向深化扫描行距不一定固定可以根据地图局部复杂度动态调整再配合转向代价的加权A*把转弯对能耗的影响也放到代价函数里整套系统就更接近工程应用了。这套代码我维护了一年多前前后后用在不同场景的地图上跑了几十遍。整体感受是往返式A*这个方法虽然听起来不算炫技但胜在结构清晰、可控性强出了问题知道往哪个方向调。对刚开始接触全覆盖路径规划的朋友来说是一个非常好的起点——把基础方法吃透后面再去接触基于深度学习的分割覆盖或者多机器人协作覆盖时很多思路都是相通的。