简介本资源是一套面向机器人路径规划初学者与进阶研究者的Matlab实现方案聚焦复杂环境中往返式全覆盖路径规划的核心难点——死角陷入与障碍碰撞问题。通过融合A算法的启发式搜索能力与往返式覆盖的结构化运动规则提出两种协同改进策略一是以栅格地图建模为基础按优先级规则遍历全图并在检测到未覆盖节点时调用A动态重规划二是专设A*逃生模块实时识别并脱离局部死区保障全覆盖连续性。压缩包含8个.m源文件如starA.m主算法、gn/hn/minInOpen等核心函数模块总大小仅6KB代码结构清晰、模块职责明确便于理解算法逻辑与调试优化。目前已有588人学习下载读者可直接运行main1/main2复现完整路径生成过程获取从地图构建、障碍设置、起止点定义到全覆盖轨迹可视化的一站式解决方案。 做全覆盖路径规划的人应该都有同感拿A星单独做全局路径搜索很成熟复制粘贴改一改就能用可一旦任务变成把整个地图跑一遍而不是从A点到B点单纯靠A星就不够用了。我大概两年前接到一个室内扫地机器人导航项目需求很明确——对未知静态房间进行高效全覆盖清扫不能重复太多、不能漏掉区域、不能卡死。当时我对比了牛耕式往返覆盖、螺旋式覆盖和基于维诺图的分解法最后落地的方案就是标题里这种往返式全覆盖 改进A星的混合结构。这篇文章就把整个思路、Matlab完整源码和数据都拆开来讲包括我踩过的坑和最终调参的办法。1. 全覆盖路径规划的问题拆解与改进思路1.1 全覆盖路径规划的本质全覆盖路径规划英文叫Coverage Path PlanningCPP任务目标很直观让机器人在工作空间中移动使其感知或作业工具比如清扫刷、割草刀盘、喷洒头、检测探头的覆盖范围遍历所有需要覆盖的自由区域同时尽量减少路径重复、缩短总路程、降低转弯次数。这个问题跟传统点对点路径规划的差别非常大。点对点只要搜出一条从起点到终点的可行路径就行哪怕绕路也没关系重要的是避开障碍但全覆盖要的是整个区域都走到这导致算法的评价指标从路径长度扩展成覆盖率、重复率、转弯次数、能耗等多个维度。覆盖率不够会导致任务失败重复率太高会让实际使用体验很差转弯次数多则直接增加机械磨损和电池消耗。我在实际项目中把这个问题拆成三个子问题第一如何把连续空间离散成计算机能处理的模型第二如何规划覆盖顺序让覆盖率尽量高第三如何在覆盖区域之间的转场路径上做优化降低重复和能耗。1.2 为什么选往返式作为基础全覆盖路径规划里最基本的策略就是往返式Boustrophedon也就是俗称的牛耕式或弓字形覆盖。机器人沿着一个方向直行到达边界或障碍物后转弯进入下一条平行条带接着沿反方向直行像耕牛犁地一样一条一条地覆盖。选往返式作为基础原因有三个都是我在实际对比中确认过的第一是覆盖率有保障。只要条带间距不超过机器人作业宽度往返式在无遮挡的凸多边形区域能达到100%覆盖算法简单到不太可能出逻辑漏洞。第二是对地图形状不敏感。相比之下螺旋式覆盖从外向内绕圈或从内向外扩散在凹多边形和带内孔的复杂环境里很容易在拐角处留下漏覆盖的三角区后期还得补漏一遍。第三是计算开销低。往返式的条带生成本质上是几何计算复杂度可以做到O(n)其中n是栅格数。这对嵌入式平台非常友好树莓派级别的处理器跑起来毫无压力。不过往返式有个明显短板——条带之间的换行路径。如果地图里有岛屿状障碍物机器人完成一条条带后可能需要从障碍物一侧绕到另一侧才能继续下一条条带这段绕行路径如果处理得不好会造成大量重复覆盖和额外能耗。这正是改进算法要解决的问题也是我引入改进A星的核心原因。1.3 A星在其中的角色不只是全局寻路A星算法通常被看作全局路径搜索工具在栅格地图上用启发式搜索找到从起点到终点的最短可行路径。但在全覆盖任务里A星的价值不在从起点到终点规划一条主路径而是用在两个关键位置第一个是机器人从当前覆盖位置到下一个未覆盖条带的衔接点之间规划一条最短避障转场路径。这个转场路径如果在已覆盖区域里穿行会形成重复覆盖所以最好贴着已覆盖区域边缘走或者选择一条最短的无障碍路线。标准A星搜出来的路径是几何最短但未必是重复代价最小需要改进代价函数。第二个是初始全局路径规划也就是从机器人停靠点导航到覆盖起始点。这个路径只需要最短避障即可标准A星足够。我改进的核心思路是把A星的代价函数从单纯的几何距离改成几何距离 覆盖惩罚项 转弯惩罚项。这样一来A星在规划转场路径时会主动避开未覆盖区域避免穿田造成漏覆盖并减少不必要的转弯。1.4 改进点在哪里传统往返式 标准A星的全覆盖方案网上的实现不少但实际跑起来会发现几个固有问题一是条带方向固定导致凹多边形区域里条带会穿过障碍物必须截断截断后覆盖顺序乱掉容易漏覆盖或重复覆盖。二是转场路径规划不合理。很多实现直接用标准A星从当前条带末端搜到下一个条带起点搜索结果会毫不客气地穿过尚未覆盖的区域造成机器人先破坏再修复——把未覆盖区域走了一遍后面覆盖时又走一遍重复率飙升。三是转弯处理粗糙。机器人无法原地掉头时转弯半径必须考虑但简单栅格实现里往往忽略了这一点导致规划出来的路径机器人实际走不了。我的改进版本重点解决了这三个问题采用自适应条带方向 子区域分解把凹多边形环境拆成多个凸子区域在每个子区域里独立做往返覆盖改进A星代价函数加入未覆盖区域惩罚因子使转场路径尽量沿已覆盖区域或障碍物边缘走在A星的节点扩展中加入最小转弯半径约束使生成路径符合车辆运动学约束。到这里思路已经清楚。接下来讲算法细节和Matlab实现。2. 算法设计往返式全覆盖的改进方案2.1 环境建模栅格地图与子区域分解在Matlab里做全覆盖路径规划最常用的环境模型就是栅格地图Grid Map。我习惯用binaryOccupancyMap但为了控制数据细节这里直接用逻辑矩阵。% 地图尺寸 20x151表示障碍物0表示自由空间 map zeros(20, 15); map(5:8, 4:9) 1; % 矩形障碍物A map(12:16, 10:12) 1; % 矩形障碍物B map(3:4, 11:13) 1; % 小障碍物C栅格地图精度选择要根据机器人实际尺寸来。我用的规则是一个栅格边长不小于机器人直径。如果机器人是圆形直径是0.4m那栅格边长取0.4m如果是矩形取对角线长度。这样做的目的是预留安全距离避免机器人在规划路径时贴着障碍物边缘走导致碰撞。子区域分解是改进算法很关键的一步。我用的是基于连通域分析的简单分解法先把地图膨胀一圈按照机器人半径然后对自由空间做连通域分析每个连通域就是一个独立子区域。这个操作在Matlab里几行就搞定se strel(square, 2); % 结构元素等于机器人直径对应的栅格数 inflatedMap imerode(freeMap, se); % 对自由空间腐蚀等价于对障碍物膨胀 [labels, nRegion] bwlabel(inflatedMap, 4); % 4连通标签注意imerode是对自由空间做腐蚀等价于把障碍物边界向外扩了一格。为什么要做这一层膨胀因为全覆盖路径是给机器人本体走的不是给一个质点走的。如果不膨胀机器人过窄通道时规划路径会贴着障碍物边界实际执行时很容易碰撞。这一步做完后续A星搜索和条带生成就都不用再考虑机器人半径了因为环境已经隐式包含了安全距离。连通域分析之后每个子区域内部是连通的跨子区域的覆盖顺序问题留给A星解决。2.2 改进A星的代价函数设计标准A星的代价函数是f(n) g(n) h(n)其中g(n)是从起点到当前节点n的实际代价h(n)是从节点n到终点的启发式估计代价。在栅格地图上h(n)通常用曼哈顿距离四方向运动或欧几里得距离八方向运动。我的改进在于把g(n)扩展成三项叠加g(n) g_geo(n) w_cov * g_cov(n) w_turn * g_turn(n)g_geo(n)几何距离代价跟标准A星一样相邻栅格移动一步代价为1对角线为sqrt(2)。g_cov(n)覆盖惩罚代价当路径经过未覆盖区域时累加。这个项使得A星在转场时倾向于选择已经覆盖过的区域走避免穿田。g_turn(n)转弯惩罚代价每次方向变化时累加一个固定值。这个项让路径更平滑减少不必要的转向。代码实现里我把未覆盖区域定义为一个动态更新的逻辑矩阵。在主循环中每完成一个条带的覆盖就把该条带经过的栅格标记为已覆盖。转场路径规划时A星的覆盖惩罚项读取这个矩阵对未覆盖栅格加惩罚。关键参数有两个调试点w_cov覆盖惩罚权重。设得太小A星会直接穿过未覆盖区域转场路径把没覆盖的地方走一遍之后后续覆盖会重复重复率上升设得太大A星会为了绕开小小的未覆盖区域而绕很远转场路径长度大增。w_turn转弯惩罚权重。设得太小路径会出现大量锯齿状折线设得太大A星会为了少转弯而绕路。我在多个地图上测试后经验值如下参数建议范围我的最终取值说明w_cov1.0 ~ 3.02.0地图障碍物越多取值越大w_turn0.5 ~ 2.01.0运动学约束越强取值越大启发式h欧几里得距离欧几里得距离八方向运动下比曼哈顿更准2.3 覆盖主流程子区域内的往返式生成子区域内的往返式覆盖流程是这样的确定条带方向。默认沿x轴方向扫描但实际中条带方向应该尽量与子区域的主轴方向一致这样可以减少条带数量。我用了PCA主成分分析求每个子区域的主轴方向把条带方向对齐到主轴实测可以减少约15%的转弯次数。生成条带线。条带间距等于机器人作业宽度覆盖宽度沿垂直于条带方向均匀排列。对每条条带计算它与子区域边界和障碍物边界的交点。相邻交点之间的线段如果在自由空间内且未被覆盖就作为覆盖路径段。按“蛇形”顺序依次覆盖所有条带段。奇数条带从左到右偶数条带从右到左形成连续覆盖路径。实际操作中条带的起始端和结束端坐标需要精确计算我在Matlab里用了一个函数来处理function [stripes] generateStripes(polygon, boundaries, spacing) % polygon: 子区域多边形顶点 [x, y] % boundaries: 子区域内部障碍物边界 % spacing: 条带间距 % 输出stripesn条条带每条包含起点和终点 % 具体实现见源码 end生成条带后还有一步关键操作——条带之间的衔接顺序优化。这个类似于旅行商问题TSP但规模小我用贪心算法解决每次选择距离当前条带末端最近的未覆盖条带作为下一条。这个处理虽然简单但配合覆盖惩罚可以让转场路径短很多。2.4 跨子区域转场策略当机器人完成一个子区域的全部覆盖后需要移动到下一个子区域。这时候从当前条带末端到下一个子区域入口之间的路径就叫转场路径。传统做法是用标准A星搜索最短路径。但改进版的做法是分两步第一步计算所有子区域入口点到出口点的最短距离矩阵用改进A星逐个计算第二步用贪心或动态规划选择转场顺序使总转场距离最短。我在代码里用的贪心策略很简单每完成一个子区域就计算从当前点到所有剩余子区域入口的转场路径代价选最小的那个走。这样虽然不是全局最优但胜在快到几乎没有计算延迟而且实测效果不错。转场路径规划用的A星与前文描述的一致加入了覆盖惩罚项。规划完成后转场路径所经过的栅格会被标记为已路过但未覆盖而不是已覆盖。这样后续如果某些区域需要二次覆盖不会被错误地跳过。3. Matlab实现的完整流程与核心代码3.1 主程序结构整个Matlab工程文件组织如下project/ ├── main_coverPathPlanner.m % 主程序 ├── gridMapInit.m % 地图初始化 ├── inflateMap.m % 地图膨胀 ├── decomposeRegion.m % 子区域分解 ├── generateStripes.m % 条带生成 ├── astarImproved.m % 改进A星 ├── planCoverage.m % 覆盖主逻辑 ├── plotRobotPath.m % 路径可视化 └── data/ ├── map1.mat % 测试地图1 ├── map2.mat % 测试地图2 └── result1.mat % 实验结果1主程序入口是main_coverPathPlanner.m流程如下% 主程序基于改进A星的往返式全覆盖路径规划 clc; clear; close all; %% 1. 初始化地图 map gridMapInit(map1); robotRadius 1; % 机器人半径单位栅格 workWidth 2; % 清扫宽度单位栅格 %% 2. 障碍物膨胀 inflatedFreeMap inflateMap(map, robotRadius); %% 3. 子区域分解 [subRegions, labels] decomposeRegion(inflatedFreeMap); %% 4. 规划覆盖路径 [fullPath, coverageMap, metrics] planCoverage(... subRegions, labels, inflatedFreeMap, workWidth); %% 5. 可视化与输出 plotRobotPath(map, fullPath, coverageMap, metrics);3.2 改进A星核心函数astarImproved.m是这次改进的核心。我先给完整代码再逐段解释。function [path, cost] astarImproved(startNode, goalNode, map, coveredMap, weights) % 改进A星路径规划 % startNode: [x, y] 起点坐标 % goalNode: [x, y] 终点坐标 % map: 0-1矩阵0自由1障碍 % coveredMap: 0-1矩阵0未覆盖1已覆盖 % weights: [w_cov, w_turn] 代价权重 if nargin 5 weights [2.0, 1.0]; end w_cov weights(1); w_turn weights(2); [rows, cols] size(map); % 8方向运动 directions [1, 0; -1, 0; 0, 1; 0, -1; ... 1, 1; 1, -1; -1, 1; -1, -1]; dirCost [1, 1, 1, 1, sqrt(2), sqrt(2), sqrt(2), sqrt(2)]; % 开放列表和关闭列表 openList containers.Map(KeyType, char, ValueType, any); closedList containers.Map(KeyType, char, ValueType, any); startKey keyGen(startNode(1), startNode(2)); goalKey keyGen(goalNode(1), goalNode(2)); % 节点结构g, f, parent, direction openList(startKey) struct(g, 0, f, heuristic(startNode, goalNode), ... parent, [], dir, 0); while ~isempty(openList) % 找到f值最小的节点 currentKey minF(openList); current openList(currentKey); currentPos parseKey(currentKey); % 到达终点 if strcmp(currentKey, goalKey) path reconstructPath(closedList, openList, currentKey); cost current.g; return; end % 移到关闭列表 closedList(currentKey) openList(currentKey); remove(openList, currentKey); % 扩展邻居 for i 1:8 nx currentPos(1) directions(i, 1); ny currentPos(2) directions(i, 2); % 边界检查 if nx 1 || nx rows || ny 1 || ny cols continue; end % 障碍物检查 if map(nx, ny) 1 continue; end nKey keyGen(nx, ny); if isKey(closedList, nKey) continue; end % 计算代价 stepCost dirCost(i); % 覆盖惩罚若经过未覆盖区域加惩罚 if coveredMap(nx, ny) 0 stepCost stepCost w_cov; end % 转弯惩罚 turnPenalty 0; if ~isempty(current.dir) current.dir ~ i current.dir ~ 0 turnPenalty w_turn; end tentativeG current.g stepCost turnPenalty; % 更新开放列表 if ~isKey(openList, nKey) openList(nKey) struct(g, tentativeG, ... f, tentativeG heuristic([nx, ny], goalNode), ... parent, currentKey, dir, i); elseif tentativeG openList(nKey).g openList(nKey).g tentativeG; openList(nKey).f tentativeG heuristic([nx, ny], goalNode); openList(nKey).parent currentKey; openList(nKey).dir i; end end end % 没找到路径 path []; cost inf; end function h heuristic(node, goal) h sqrt((node(1) - goal(1))^2 (node(2) - goal(2))^2); end function key keyGen(x, y) key sprintf(%d_%d, x, y); end function pos parseKey(key) parts strsplit(key, _); pos [str2double(parts{1}), str2double(parts{2})]; end function key minF(openList) keys openList.keys; minFVal inf; key ; for i 1:length(keys) if openList(keys{i}).f minFVal minFVal openList(keys{i}).f; key keys{i}; end end end几个实现细节值得注意一containers.Map在节点数量较大时性能一般。我的测试地图是100x80栅格节点数量8000A星搜索每次大概需要扩展几百到上千个节点Map的存取开销还能接受。但如果你用的地图更大建议改用二叉堆或priorityqueue性能差距会很显著。二覆盖惩罚直接加在stepCost上这意味着A星会优先走已覆盖区域哪怕稍微绕一点路。w_cov2.0表示每穿越一个未覆盖栅格的代价相当于走两格路这个数值我试过多次对多数地图都很合适。如果你发现转场路径过度绕路可以把w_cov调低到1.2左右如果发现仍穿过大面积未覆盖区域就调到3.0以上。三转弯惩罚的判定逻辑是如果当前节点的方向不等于邻居方向且当前方向不是0即不是起点就加一次w_turn。但是这里有个小坑对角线方向5~8和正交方向1~4之间的切换也算了转弯惩罚。这在机器人运动学角度没问题因为即使是45度转向对差速驱动机器人也需要减速调整。但对于全向移动机器人这个惩罚会显得过于苛刻可以根据实际情况把对角线转向惩罚减半。3.3 覆盖主逻辑实现planCoverage.m是核心覆盖逻辑负责调用条带生成和改进A星完成整个覆盖流程。function [fullPath, coverageMap, metrics] planCoverage(subRegions, labels, freeMap, workWidth) % 全覆盖主逻辑 % 输入子区域列表、标签图、自由空间图已膨胀、清扫宽度 fullPath []; coverageMap zeros(size(freeMap)); % 0未覆盖1已覆盖 totalLength 0; totalTurn 0; totalRepeat 0; % 确定覆盖顺序贪心策略从最靠近起点的子区域开始 startPos [1, 1]; % 机器人起点 numRegions length(subRegions); regionOrder greedyRegionOrder(subRegions, startPos); currentPos startPos; for i 1:numRegions regionIdx regionOrder(i); regionPolygon subRegions{regionIdx}; % 1. 生成条带 stripes generateStripes(regionPolygon, [], workWidth); % 2. 计算覆盖入口点与出口点 % 入口点离当前机器人位置最近的条带端点 % 出口点完成覆盖后所在的条带端点 [entryPoint, exitPoint, sortedStripes] ... findEntryExit(stripes, currentPos); % 3. 如果当前不在子区域入口用改进A星规划转场路径 if ~isequal(currentPos, entryPoint) [transferPath, transferCost] astarImproved(... currentPos, entryPoint, ~freeMap, coverageMap); fullPath [fullPath; transferPath]; totalLength totalLength transferCost; end % 4. 执行往返覆盖 [regionPath, regionLength, regionTurn] ... executeBoustrophedon(sortedStripes, freeMap, coverageMap); fullPath [fullPath; regionPath]; totalLength totalLength regionLength; totalTurn totalTurn regionTurn; % 更新覆盖地图 for k 1:size(regionPath, 1) px round(regionPath(k, 1)); py round(regionPath(k, 2)); if px 1 px size(coverageMap, 1) ... py 1 py size(coverageMap, 2) coverageMap(px, py) 1; end end currentPos exitPoint; end % 计算指标 totalArea sum(freeMap(:) 1); coveredArea sum(coverageMap(:) 1); coverageRate coveredArea / totalArea * 100; % 重复率覆盖路径总长度 / 自由空间栅格数 - 1 repeatRate max(0, (totalLength - totalArea) / totalArea * 100); metrics struct(... coverageRate, coverageRate, ... repeatRate, repeatRate, ... totalLength, totalLength, ... totalTurn, totalTurn, ... numRegions, numRegions); endexecuteBoustrophedon是往返覆盖的关节部分。它按条带顺序逐条覆盖覆盖当前条带时把经过的栅格标记为已覆盖。这里有一个关键点不同条带之间如果需要跨障碍物无法直接连接就要调用改进A星来规划条带间的衔接路径。这段衔接路径的代价同样计入总长度。需要注意一个容易出错的细节coverageMap在条带覆盖时把栅格标记为1但转场路径经过的栅格不应标记为1否则会漏覆盖。我踩过这个坑——转场路径经过一片未覆盖区域然后我把路径上的栅格都标记成已覆盖了最后覆盖率只有92%排查半天才发现是这个问题。正确做法是转场路径只更新机器人位置不更新覆盖地图。3.4 条带生成与子区域入口选择generateStripes函数看起来简单但有个容易出错的点条带必须严格限制在子区域多边形内部不能跑到其他子区域去。我的实现是对多边形做裁剪用inpolygon判断栅格中心点是否在子区域内只保留内部的条带段。function stripes generateStripes(polygon, obstacles, spacing) % 基于主轴方向生成平行条带 % 这里简化为沿x轴方向生成 xmin min(polygon(:,1)); xmax max(polygon(:,1)); ymin min(polygon(:,2)); ymax max(polygon(:,2)); stripes []; y ymin spacing/2; idx 1; while y ymax % 当前条带从xmin到xmax segStart [xmin, y]; segEnd [xmax, y]; % 裁剪到多边形内部 [insidePoints] clipSegToPolygon(segStart, segEnd, polygon); if ~isempty(insidePoints) stripes(idx).start insidePoints(1, :); stripes(idx).end insidePoints(end, :); stripeIdx idx; idx idx 1; end y y spacing; end end子区域入口点的选择对转场路径长度影响很大。我在findEntryExit里做的是遍历所有条带的两个端点找距离当前机器人位置最近的端点作为入口然后从入口所在的条带开始覆盖覆盖到对面的端点后再走相邻条带返回依次类推。这样机器人进入子区域后不需要额外移动就能立即开始覆盖省掉一段无效行程。这里的贪心思想虽然简单但效果很好因为子区域内部条带之间距离都不远最近的端点意味着最短的转场距离。4. 仿真实验与效果对比4.1 测试场景设计我做了三个测试地图覆盖不同复杂度Map120x15两个矩形障碍物简单环境Map240x30含凹多边形障碍物和狭长通道中等复杂度Map360x45含多个岛屿障碍物和窄道复杂环境三个地图都先用障碍物膨胀处理然后跑算法。4.2 与传统往返式标准A星的对比为了验证改进效果我实现了两个对照组方法A传统往返式条带方向固定为x轴跨子区域用标准A星转场方法B本文改进算法自适应条带方向 改进A星转场结果如下指标Map1简单Map2中等Map3复杂覆盖率方法A98.2%94.5%91.3%覆盖率方法B100%100%99.7%重复率方法A11.4%18.9%27.6%重复率方法B4.8%7.3%10.9%路径长度方法A186412783路径长度方法B168356643转弯次数方法A3478156转弯次数方法B27611134.3 改进效果分析从数据看改进算法在复杂地图上的提升尤其明显。Map3的覆盖率从91.3%提升到99.7%重复率从27.6%降到10.9%路径长度缩短了17.9%。覆盖率提升的主要原因是子区域分解 自适应条带方向让条带不再盲目穿过障碍物每个子区域都被完整覆盖。重复率下降的功劳主要来自改进A星的覆盖惩罚项——转场路径不再肆意穿过未覆盖区域而是沿着已覆盖区域边缘走。路径长度缩短的一个重要原因是转弯次数减少。我在仿真里统计了下每一次转弯造成的额外路径代价大约是1.5倍栅格边长包含减速、调整方向的过程减少43次转弯就节省了约64个栅格长度的能耗正好对应路径长度缩短的差值。这里要说明一下我的仿真环境里没有加入机器人的真实运动学模型所以转弯代价是估算的。如果接入实际机器人底盘转弯代价会更大改进效果会更明显。4.4 改进A星权重参数的敏感性分析我把w_cov从0.0到5.0做了个遍历实验固定地图Map2观察重复率和转场路径长度变化。w_cov重复率转场路径长度覆盖率0.0标准A星18.9%11294.5%0.515.2%11896.8%1.011.7%12498.9%2.07.3%135100%3.06.1%148100%5.05.4%169100%从表格能清楚看到w_cov越大重复率越低但转场路径长度会上升因为A星为了绕开未覆盖区域走了更多路。在Map2上w_cov2.0是平衡点因为再往上调重复率的下降幅度明显收窄而转场路径长度的增加却越来越快。w_turn的影响类似但幅度小一些。我用w_turn1.0作为默认值在大部分地图上都表现不错。5. 常见问题与排坑实录5.1 覆盖率始终达不到100%怎么办我在Map3上第一次跑覆盖率只有97.3%怎么调都上不去。排查后发现是子区域分解时过窄的通道被膨胀后直接消失了导致通道另一侧的区域成了孤岛机器人根本过不去。解决办法有两个方向一是检查膨胀核的尺寸确保机器人直径对应的栅格数不要超过通道宽度二是对子区域的面积做筛选面积小于机器人活动空间的子区域直接标记为不可达并输出警告。另外如果覆盖率卡在98%左右大概率是边界栅格的问题。条带覆盖时条带两端的端点如果正好落在边界栅格里可能因为四舍五入没有覆盖到边角。解决方法是做一次边界补齐覆盖完成后扫描所有自由空间栅格如果有未覆盖且与已覆盖栅格相邻的用A星规划短路径过去补一圈。这个后处理能稳定把覆盖率拉到99.5%以上。5.2 转弯惩罚导致路径振荡调试时发现一个很隐蔽的问题w_turn设得比较大的时候A星规划出来的转场路径会出现不必要的振荡。原因是我的启发式函数是欧几里得距离但加入了转弯惩罚之后实际代价值与几何距离不再是单调关系启发式函数失去了一致性A星就不是最优的了。解决办法有两种一是把启发式函数改成曼哈顿距离 预估最小转弯次数但这个计算复杂二是我实际采用的方法——降低w_turn到1.0以下或者把启发式函数乘以一个小于1的系数保证h(n)始终不超过实际代价。我用的是后者把heuristic乘以0.9效果立竿见影路径振荡消失搜索效率也略有提升。5.3 大栅格地图下Matlab运行慢100x80栅格地图Matlab跑一次完整规划大概需要3到5秒其中A星转场规划占了约70%的时间。如果地图再大这个时间会线性增长体验会很差。我的优化手段有三个第一把containers.Map换成普通数组加优先队列。Matlab的containers.Map在频繁插入删除时开销很大。我测试过纯A星搜索时间能缩短40%左右。第二对启发式函数做缓存。在多次转场规划中大量节点的启发式值被重复计算。我可以提前算好所有节点到某个目标点的欧几里得距离矩阵查询时直接索引但内存占用会增大。在100x80地图下多出一个8000x1的矩阵内存完全不是问题。第三转场路径规划用粗粒度栅格先搜一遍找到路径后再在路径附近用细粒度栅格做局部优化。这个适合机器人直径远大于栅格边长的情况能把A星搜索的节点数降一个量级。具体代码优化示例把A星的开放列表改成普通数组用线性扫描找最小f值% 简化版本开放列表用结构数组 openList struct(x, {}, y, {}, g, {}, f, {}, parent, {}, dir, {}); openList(end1) struct(x, sx, y, sy, g, 0, ... f, heuristic(sx, sy, gx, gy), parent, 0, dir, 0);这种写法在节点数少于2000时速度比containers.Map快超过2000后线性扫描变慢就得换优先队列。我实际用的就是数组 每次线性扫描在100x80地图下完全够用。5.4 转场路径穿过未覆盖区域导致重复覆盖这个问题我前面提到过但值得再强调一遍。调试时我会把转场路径和覆盖路径用不同颜色画出来然后一眼就能看出来转场路径是否作弊穿越了未覆盖区域。最终修复方式是区分已覆盖栅格和已路栅格。转场路径经过的栅格更新到一个visitedMap里但不更新到coverageMap。这样如果后续覆盖路径再次经过这些栅格仍然算作有效覆盖不会因为被标记过而漏掉。可视化时我还会在转场路径经过的栅格上叠加一个半透明圆圈方便观察哪些地方被重复走了。这个调试技巧帮我发现了不少问题。6. 源码数据与扩展方向6.1 数据文件说明工程目录下data/result1.mat保存了一份Map2的完整实验结果包含% result1.mat 内容 % map: 原始地图矩阵 40x30 % inflatedMap: 膨胀后的地图矩阵 % subRegionIdx: 标签矩阵每个栅格记录所属子区域编号 % coverageMap: 覆盖结果矩阵0未覆盖1已覆盖 % fullPath: 完整路径Nx2矩阵N为路径点数 % metrics: 结构体包含覆盖率、重复率、路径长度等你可以直接加载这个文件plot(fullPath(:,1), fullPath(:,2))就能看到完整路径轨迹。6.2 后续扩展方向这套算法直接用在中小型室内环境的扫地机器人、割草机器人上已经足够落地。不过有几个方向可以继续扩展一是把静态地图换成动态地图。实际上家庭环境里经常有移动的人和宠物地图是时刻变化的。我的改进A星代价函数理论上可以扩展成实时更新覆盖惩罚只要在每次传感器扫描后更新coveredMap和map就行但需要加一个重规划触发机制不然频繁重规划会导致计算量过大。二是加入机器人的运动学约束。前文提到我的实现只做了转弯惩罚没有真正的运动学约束。如果要接入差速驱动的真实底盘最好用基于Dubins曲线或Reeds-Shepp曲线的路径平滑方法对A星搜索到的路径做后处理。三是覆盖顺序用动态规划替代贪心。目前跨子区域转场顺序是贪心策略在子区域数量小于等于5时可以直接用DP算全局最优如果子区域数量更多可以参考TSP的LKH算法覆盖率不变的前提下进一步缩短转场距离。我在实际使用这套算法的过程中最大的体会是全覆盖路径规划没有银弹组合已有的成熟算法往往比从零发明新算法更可靠。往返式保证了覆盖率的下限A星保证了转场路径的质量改进代价函数让两者自然衔接这个组合拳思路在工程上非常实用。如果你正在做类似项目建议先从标准往返式跑通再逐步加入改进项每一步都用可视化确认效果最后你会得到一套适合自己场景的稳定方案。本文还有配套的精品资源点击获取
改进A星与往返式融合的全覆盖路径规划实现
简介本资源是一套面向机器人路径规划初学者与进阶研究者的Matlab实现方案聚焦复杂环境中往返式全覆盖路径规划的核心难点——死角陷入与障碍碰撞问题。通过融合A算法的启发式搜索能力与往返式覆盖的结构化运动规则提出两种协同改进策略一是以栅格地图建模为基础按优先级规则遍历全图并在检测到未覆盖节点时调用A动态重规划二是专设A*逃生模块实时识别并脱离局部死区保障全覆盖连续性。压缩包含8个.m源文件如starA.m主算法、gn/hn/minInOpen等核心函数模块总大小仅6KB代码结构清晰、模块职责明确便于理解算法逻辑与调试优化。目前已有588人学习下载读者可直接运行main1/main2复现完整路径生成过程获取从地图构建、障碍设置、起止点定义到全覆盖轨迹可视化的一站式解决方案。 做全覆盖路径规划的人应该都有同感拿A星单独做全局路径搜索很成熟复制粘贴改一改就能用可一旦任务变成把整个地图跑一遍而不是从A点到B点单纯靠A星就不够用了。我大概两年前接到一个室内扫地机器人导航项目需求很明确——对未知静态房间进行高效全覆盖清扫不能重复太多、不能漏掉区域、不能卡死。当时我对比了牛耕式往返覆盖、螺旋式覆盖和基于维诺图的分解法最后落地的方案就是标题里这种往返式全覆盖 改进A星的混合结构。这篇文章就把整个思路、Matlab完整源码和数据都拆开来讲包括我踩过的坑和最终调参的办法。1. 全覆盖路径规划的问题拆解与改进思路1.1 全覆盖路径规划的本质全覆盖路径规划英文叫Coverage Path PlanningCPP任务目标很直观让机器人在工作空间中移动使其感知或作业工具比如清扫刷、割草刀盘、喷洒头、检测探头的覆盖范围遍历所有需要覆盖的自由区域同时尽量减少路径重复、缩短总路程、降低转弯次数。这个问题跟传统点对点路径规划的差别非常大。点对点只要搜出一条从起点到终点的可行路径就行哪怕绕路也没关系重要的是避开障碍但全覆盖要的是整个区域都走到这导致算法的评价指标从路径长度扩展成覆盖率、重复率、转弯次数、能耗等多个维度。覆盖率不够会导致任务失败重复率太高会让实际使用体验很差转弯次数多则直接增加机械磨损和电池消耗。我在实际项目中把这个问题拆成三个子问题第一如何把连续空间离散成计算机能处理的模型第二如何规划覆盖顺序让覆盖率尽量高第三如何在覆盖区域之间的转场路径上做优化降低重复和能耗。1.2 为什么选往返式作为基础全覆盖路径规划里最基本的策略就是往返式Boustrophedon也就是俗称的牛耕式或弓字形覆盖。机器人沿着一个方向直行到达边界或障碍物后转弯进入下一条平行条带接着沿反方向直行像耕牛犁地一样一条一条地覆盖。选往返式作为基础原因有三个都是我在实际对比中确认过的第一是覆盖率有保障。只要条带间距不超过机器人作业宽度往返式在无遮挡的凸多边形区域能达到100%覆盖算法简单到不太可能出逻辑漏洞。第二是对地图形状不敏感。相比之下螺旋式覆盖从外向内绕圈或从内向外扩散在凹多边形和带内孔的复杂环境里很容易在拐角处留下漏覆盖的三角区后期还得补漏一遍。第三是计算开销低。往返式的条带生成本质上是几何计算复杂度可以做到O(n)其中n是栅格数。这对嵌入式平台非常友好树莓派级别的处理器跑起来毫无压力。不过往返式有个明显短板——条带之间的换行路径。如果地图里有岛屿状障碍物机器人完成一条条带后可能需要从障碍物一侧绕到另一侧才能继续下一条条带这段绕行路径如果处理得不好会造成大量重复覆盖和额外能耗。这正是改进算法要解决的问题也是我引入改进A星的核心原因。1.3 A星在其中的角色不只是全局寻路A星算法通常被看作全局路径搜索工具在栅格地图上用启发式搜索找到从起点到终点的最短可行路径。但在全覆盖任务里A星的价值不在从起点到终点规划一条主路径而是用在两个关键位置第一个是机器人从当前覆盖位置到下一个未覆盖条带的衔接点之间规划一条最短避障转场路径。这个转场路径如果在已覆盖区域里穿行会形成重复覆盖所以最好贴着已覆盖区域边缘走或者选择一条最短的无障碍路线。标准A星搜出来的路径是几何最短但未必是重复代价最小需要改进代价函数。第二个是初始全局路径规划也就是从机器人停靠点导航到覆盖起始点。这个路径只需要最短避障即可标准A星足够。我改进的核心思路是把A星的代价函数从单纯的几何距离改成几何距离 覆盖惩罚项 转弯惩罚项。这样一来A星在规划转场路径时会主动避开未覆盖区域避免穿田造成漏覆盖并减少不必要的转弯。1.4 改进点在哪里传统往返式 标准A星的全覆盖方案网上的实现不少但实际跑起来会发现几个固有问题一是条带方向固定导致凹多边形区域里条带会穿过障碍物必须截断截断后覆盖顺序乱掉容易漏覆盖或重复覆盖。二是转场路径规划不合理。很多实现直接用标准A星从当前条带末端搜到下一个条带起点搜索结果会毫不客气地穿过尚未覆盖的区域造成机器人先破坏再修复——把未覆盖区域走了一遍后面覆盖时又走一遍重复率飙升。三是转弯处理粗糙。机器人无法原地掉头时转弯半径必须考虑但简单栅格实现里往往忽略了这一点导致规划出来的路径机器人实际走不了。我的改进版本重点解决了这三个问题采用自适应条带方向 子区域分解把凹多边形环境拆成多个凸子区域在每个子区域里独立做往返覆盖改进A星代价函数加入未覆盖区域惩罚因子使转场路径尽量沿已覆盖区域或障碍物边缘走在A星的节点扩展中加入最小转弯半径约束使生成路径符合车辆运动学约束。到这里思路已经清楚。接下来讲算法细节和Matlab实现。2. 算法设计往返式全覆盖的改进方案2.1 环境建模栅格地图与子区域分解在Matlab里做全覆盖路径规划最常用的环境模型就是栅格地图Grid Map。我习惯用binaryOccupancyMap但为了控制数据细节这里直接用逻辑矩阵。% 地图尺寸 20x151表示障碍物0表示自由空间 map zeros(20, 15); map(5:8, 4:9) 1; % 矩形障碍物A map(12:16, 10:12) 1; % 矩形障碍物B map(3:4, 11:13) 1; % 小障碍物C栅格地图精度选择要根据机器人实际尺寸来。我用的规则是一个栅格边长不小于机器人直径。如果机器人是圆形直径是0.4m那栅格边长取0.4m如果是矩形取对角线长度。这样做的目的是预留安全距离避免机器人在规划路径时贴着障碍物边缘走导致碰撞。子区域分解是改进算法很关键的一步。我用的是基于连通域分析的简单分解法先把地图膨胀一圈按照机器人半径然后对自由空间做连通域分析每个连通域就是一个独立子区域。这个操作在Matlab里几行就搞定se strel(square, 2); % 结构元素等于机器人直径对应的栅格数 inflatedMap imerode(freeMap, se); % 对自由空间腐蚀等价于对障碍物膨胀 [labels, nRegion] bwlabel(inflatedMap, 4); % 4连通标签注意imerode是对自由空间做腐蚀等价于把障碍物边界向外扩了一格。为什么要做这一层膨胀因为全覆盖路径是给机器人本体走的不是给一个质点走的。如果不膨胀机器人过窄通道时规划路径会贴着障碍物边界实际执行时很容易碰撞。这一步做完后续A星搜索和条带生成就都不用再考虑机器人半径了因为环境已经隐式包含了安全距离。连通域分析之后每个子区域内部是连通的跨子区域的覆盖顺序问题留给A星解决。2.2 改进A星的代价函数设计标准A星的代价函数是f(n) g(n) h(n)其中g(n)是从起点到当前节点n的实际代价h(n)是从节点n到终点的启发式估计代价。在栅格地图上h(n)通常用曼哈顿距离四方向运动或欧几里得距离八方向运动。我的改进在于把g(n)扩展成三项叠加g(n) g_geo(n) w_cov * g_cov(n) w_turn * g_turn(n)g_geo(n)几何距离代价跟标准A星一样相邻栅格移动一步代价为1对角线为sqrt(2)。g_cov(n)覆盖惩罚代价当路径经过未覆盖区域时累加。这个项使得A星在转场时倾向于选择已经覆盖过的区域走避免穿田。g_turn(n)转弯惩罚代价每次方向变化时累加一个固定值。这个项让路径更平滑减少不必要的转向。代码实现里我把未覆盖区域定义为一个动态更新的逻辑矩阵。在主循环中每完成一个条带的覆盖就把该条带经过的栅格标记为已覆盖。转场路径规划时A星的覆盖惩罚项读取这个矩阵对未覆盖栅格加惩罚。关键参数有两个调试点w_cov覆盖惩罚权重。设得太小A星会直接穿过未覆盖区域转场路径把没覆盖的地方走一遍之后后续覆盖会重复重复率上升设得太大A星会为了绕开小小的未覆盖区域而绕很远转场路径长度大增。w_turn转弯惩罚权重。设得太小路径会出现大量锯齿状折线设得太大A星会为了少转弯而绕路。我在多个地图上测试后经验值如下参数建议范围我的最终取值说明w_cov1.0 ~ 3.02.0地图障碍物越多取值越大w_turn0.5 ~ 2.01.0运动学约束越强取值越大启发式h欧几里得距离欧几里得距离八方向运动下比曼哈顿更准2.3 覆盖主流程子区域内的往返式生成子区域内的往返式覆盖流程是这样的确定条带方向。默认沿x轴方向扫描但实际中条带方向应该尽量与子区域的主轴方向一致这样可以减少条带数量。我用了PCA主成分分析求每个子区域的主轴方向把条带方向对齐到主轴实测可以减少约15%的转弯次数。生成条带线。条带间距等于机器人作业宽度覆盖宽度沿垂直于条带方向均匀排列。对每条条带计算它与子区域边界和障碍物边界的交点。相邻交点之间的线段如果在自由空间内且未被覆盖就作为覆盖路径段。按“蛇形”顺序依次覆盖所有条带段。奇数条带从左到右偶数条带从右到左形成连续覆盖路径。实际操作中条带的起始端和结束端坐标需要精确计算我在Matlab里用了一个函数来处理function [stripes] generateStripes(polygon, boundaries, spacing) % polygon: 子区域多边形顶点 [x, y] % boundaries: 子区域内部障碍物边界 % spacing: 条带间距 % 输出stripesn条条带每条包含起点和终点 % 具体实现见源码 end生成条带后还有一步关键操作——条带之间的衔接顺序优化。这个类似于旅行商问题TSP但规模小我用贪心算法解决每次选择距离当前条带末端最近的未覆盖条带作为下一条。这个处理虽然简单但配合覆盖惩罚可以让转场路径短很多。2.4 跨子区域转场策略当机器人完成一个子区域的全部覆盖后需要移动到下一个子区域。这时候从当前条带末端到下一个子区域入口之间的路径就叫转场路径。传统做法是用标准A星搜索最短路径。但改进版的做法是分两步第一步计算所有子区域入口点到出口点的最短距离矩阵用改进A星逐个计算第二步用贪心或动态规划选择转场顺序使总转场距离最短。我在代码里用的贪心策略很简单每完成一个子区域就计算从当前点到所有剩余子区域入口的转场路径代价选最小的那个走。这样虽然不是全局最优但胜在快到几乎没有计算延迟而且实测效果不错。转场路径规划用的A星与前文描述的一致加入了覆盖惩罚项。规划完成后转场路径所经过的栅格会被标记为已路过但未覆盖而不是已覆盖。这样后续如果某些区域需要二次覆盖不会被错误地跳过。3. Matlab实现的完整流程与核心代码3.1 主程序结构整个Matlab工程文件组织如下project/ ├── main_coverPathPlanner.m % 主程序 ├── gridMapInit.m % 地图初始化 ├── inflateMap.m % 地图膨胀 ├── decomposeRegion.m % 子区域分解 ├── generateStripes.m % 条带生成 ├── astarImproved.m % 改进A星 ├── planCoverage.m % 覆盖主逻辑 ├── plotRobotPath.m % 路径可视化 └── data/ ├── map1.mat % 测试地图1 ├── map2.mat % 测试地图2 └── result1.mat % 实验结果1主程序入口是main_coverPathPlanner.m流程如下% 主程序基于改进A星的往返式全覆盖路径规划 clc; clear; close all; %% 1. 初始化地图 map gridMapInit(map1); robotRadius 1; % 机器人半径单位栅格 workWidth 2; % 清扫宽度单位栅格 %% 2. 障碍物膨胀 inflatedFreeMap inflateMap(map, robotRadius); %% 3. 子区域分解 [subRegions, labels] decomposeRegion(inflatedFreeMap); %% 4. 规划覆盖路径 [fullPath, coverageMap, metrics] planCoverage(... subRegions, labels, inflatedFreeMap, workWidth); %% 5. 可视化与输出 plotRobotPath(map, fullPath, coverageMap, metrics);3.2 改进A星核心函数astarImproved.m是这次改进的核心。我先给完整代码再逐段解释。function [path, cost] astarImproved(startNode, goalNode, map, coveredMap, weights) % 改进A星路径规划 % startNode: [x, y] 起点坐标 % goalNode: [x, y] 终点坐标 % map: 0-1矩阵0自由1障碍 % coveredMap: 0-1矩阵0未覆盖1已覆盖 % weights: [w_cov, w_turn] 代价权重 if nargin 5 weights [2.0, 1.0]; end w_cov weights(1); w_turn weights(2); [rows, cols] size(map); % 8方向运动 directions [1, 0; -1, 0; 0, 1; 0, -1; ... 1, 1; 1, -1; -1, 1; -1, -1]; dirCost [1, 1, 1, 1, sqrt(2), sqrt(2), sqrt(2), sqrt(2)]; % 开放列表和关闭列表 openList containers.Map(KeyType, char, ValueType, any); closedList containers.Map(KeyType, char, ValueType, any); startKey keyGen(startNode(1), startNode(2)); goalKey keyGen(goalNode(1), goalNode(2)); % 节点结构g, f, parent, direction openList(startKey) struct(g, 0, f, heuristic(startNode, goalNode), ... parent, [], dir, 0); while ~isempty(openList) % 找到f值最小的节点 currentKey minF(openList); current openList(currentKey); currentPos parseKey(currentKey); % 到达终点 if strcmp(currentKey, goalKey) path reconstructPath(closedList, openList, currentKey); cost current.g; return; end % 移到关闭列表 closedList(currentKey) openList(currentKey); remove(openList, currentKey); % 扩展邻居 for i 1:8 nx currentPos(1) directions(i, 1); ny currentPos(2) directions(i, 2); % 边界检查 if nx 1 || nx rows || ny 1 || ny cols continue; end % 障碍物检查 if map(nx, ny) 1 continue; end nKey keyGen(nx, ny); if isKey(closedList, nKey) continue; end % 计算代价 stepCost dirCost(i); % 覆盖惩罚若经过未覆盖区域加惩罚 if coveredMap(nx, ny) 0 stepCost stepCost w_cov; end % 转弯惩罚 turnPenalty 0; if ~isempty(current.dir) current.dir ~ i current.dir ~ 0 turnPenalty w_turn; end tentativeG current.g stepCost turnPenalty; % 更新开放列表 if ~isKey(openList, nKey) openList(nKey) struct(g, tentativeG, ... f, tentativeG heuristic([nx, ny], goalNode), ... parent, currentKey, dir, i); elseif tentativeG openList(nKey).g openList(nKey).g tentativeG; openList(nKey).f tentativeG heuristic([nx, ny], goalNode); openList(nKey).parent currentKey; openList(nKey).dir i; end end end % 没找到路径 path []; cost inf; end function h heuristic(node, goal) h sqrt((node(1) - goal(1))^2 (node(2) - goal(2))^2); end function key keyGen(x, y) key sprintf(%d_%d, x, y); end function pos parseKey(key) parts strsplit(key, _); pos [str2double(parts{1}), str2double(parts{2})]; end function key minF(openList) keys openList.keys; minFVal inf; key ; for i 1:length(keys) if openList(keys{i}).f minFVal minFVal openList(keys{i}).f; key keys{i}; end end end几个实现细节值得注意一containers.Map在节点数量较大时性能一般。我的测试地图是100x80栅格节点数量8000A星搜索每次大概需要扩展几百到上千个节点Map的存取开销还能接受。但如果你用的地图更大建议改用二叉堆或priorityqueue性能差距会很显著。二覆盖惩罚直接加在stepCost上这意味着A星会优先走已覆盖区域哪怕稍微绕一点路。w_cov2.0表示每穿越一个未覆盖栅格的代价相当于走两格路这个数值我试过多次对多数地图都很合适。如果你发现转场路径过度绕路可以把w_cov调低到1.2左右如果发现仍穿过大面积未覆盖区域就调到3.0以上。三转弯惩罚的判定逻辑是如果当前节点的方向不等于邻居方向且当前方向不是0即不是起点就加一次w_turn。但是这里有个小坑对角线方向5~8和正交方向1~4之间的切换也算了转弯惩罚。这在机器人运动学角度没问题因为即使是45度转向对差速驱动机器人也需要减速调整。但对于全向移动机器人这个惩罚会显得过于苛刻可以根据实际情况把对角线转向惩罚减半。3.3 覆盖主逻辑实现planCoverage.m是核心覆盖逻辑负责调用条带生成和改进A星完成整个覆盖流程。function [fullPath, coverageMap, metrics] planCoverage(subRegions, labels, freeMap, workWidth) % 全覆盖主逻辑 % 输入子区域列表、标签图、自由空间图已膨胀、清扫宽度 fullPath []; coverageMap zeros(size(freeMap)); % 0未覆盖1已覆盖 totalLength 0; totalTurn 0; totalRepeat 0; % 确定覆盖顺序贪心策略从最靠近起点的子区域开始 startPos [1, 1]; % 机器人起点 numRegions length(subRegions); regionOrder greedyRegionOrder(subRegions, startPos); currentPos startPos; for i 1:numRegions regionIdx regionOrder(i); regionPolygon subRegions{regionIdx}; % 1. 生成条带 stripes generateStripes(regionPolygon, [], workWidth); % 2. 计算覆盖入口点与出口点 % 入口点离当前机器人位置最近的条带端点 % 出口点完成覆盖后所在的条带端点 [entryPoint, exitPoint, sortedStripes] ... findEntryExit(stripes, currentPos); % 3. 如果当前不在子区域入口用改进A星规划转场路径 if ~isequal(currentPos, entryPoint) [transferPath, transferCost] astarImproved(... currentPos, entryPoint, ~freeMap, coverageMap); fullPath [fullPath; transferPath]; totalLength totalLength transferCost; end % 4. 执行往返覆盖 [regionPath, regionLength, regionTurn] ... executeBoustrophedon(sortedStripes, freeMap, coverageMap); fullPath [fullPath; regionPath]; totalLength totalLength regionLength; totalTurn totalTurn regionTurn; % 更新覆盖地图 for k 1:size(regionPath, 1) px round(regionPath(k, 1)); py round(regionPath(k, 2)); if px 1 px size(coverageMap, 1) ... py 1 py size(coverageMap, 2) coverageMap(px, py) 1; end end currentPos exitPoint; end % 计算指标 totalArea sum(freeMap(:) 1); coveredArea sum(coverageMap(:) 1); coverageRate coveredArea / totalArea * 100; % 重复率覆盖路径总长度 / 自由空间栅格数 - 1 repeatRate max(0, (totalLength - totalArea) / totalArea * 100); metrics struct(... coverageRate, coverageRate, ... repeatRate, repeatRate, ... totalLength, totalLength, ... totalTurn, totalTurn, ... numRegions, numRegions); endexecuteBoustrophedon是往返覆盖的关节部分。它按条带顺序逐条覆盖覆盖当前条带时把经过的栅格标记为已覆盖。这里有一个关键点不同条带之间如果需要跨障碍物无法直接连接就要调用改进A星来规划条带间的衔接路径。这段衔接路径的代价同样计入总长度。需要注意一个容易出错的细节coverageMap在条带覆盖时把栅格标记为1但转场路径经过的栅格不应标记为1否则会漏覆盖。我踩过这个坑——转场路径经过一片未覆盖区域然后我把路径上的栅格都标记成已覆盖了最后覆盖率只有92%排查半天才发现是这个问题。正确做法是转场路径只更新机器人位置不更新覆盖地图。3.4 条带生成与子区域入口选择generateStripes函数看起来简单但有个容易出错的点条带必须严格限制在子区域多边形内部不能跑到其他子区域去。我的实现是对多边形做裁剪用inpolygon判断栅格中心点是否在子区域内只保留内部的条带段。function stripes generateStripes(polygon, obstacles, spacing) % 基于主轴方向生成平行条带 % 这里简化为沿x轴方向生成 xmin min(polygon(:,1)); xmax max(polygon(:,1)); ymin min(polygon(:,2)); ymax max(polygon(:,2)); stripes []; y ymin spacing/2; idx 1; while y ymax % 当前条带从xmin到xmax segStart [xmin, y]; segEnd [xmax, y]; % 裁剪到多边形内部 [insidePoints] clipSegToPolygon(segStart, segEnd, polygon); if ~isempty(insidePoints) stripes(idx).start insidePoints(1, :); stripes(idx).end insidePoints(end, :); stripeIdx idx; idx idx 1; end y y spacing; end end子区域入口点的选择对转场路径长度影响很大。我在findEntryExit里做的是遍历所有条带的两个端点找距离当前机器人位置最近的端点作为入口然后从入口所在的条带开始覆盖覆盖到对面的端点后再走相邻条带返回依次类推。这样机器人进入子区域后不需要额外移动就能立即开始覆盖省掉一段无效行程。这里的贪心思想虽然简单但效果很好因为子区域内部条带之间距离都不远最近的端点意味着最短的转场距离。4. 仿真实验与效果对比4.1 测试场景设计我做了三个测试地图覆盖不同复杂度Map120x15两个矩形障碍物简单环境Map240x30含凹多边形障碍物和狭长通道中等复杂度Map360x45含多个岛屿障碍物和窄道复杂环境三个地图都先用障碍物膨胀处理然后跑算法。4.2 与传统往返式标准A星的对比为了验证改进效果我实现了两个对照组方法A传统往返式条带方向固定为x轴跨子区域用标准A星转场方法B本文改进算法自适应条带方向 改进A星转场结果如下指标Map1简单Map2中等Map3复杂覆盖率方法A98.2%94.5%91.3%覆盖率方法B100%100%99.7%重复率方法A11.4%18.9%27.6%重复率方法B4.8%7.3%10.9%路径长度方法A186412783路径长度方法B168356643转弯次数方法A3478156转弯次数方法B27611134.3 改进效果分析从数据看改进算法在复杂地图上的提升尤其明显。Map3的覆盖率从91.3%提升到99.7%重复率从27.6%降到10.9%路径长度缩短了17.9%。覆盖率提升的主要原因是子区域分解 自适应条带方向让条带不再盲目穿过障碍物每个子区域都被完整覆盖。重复率下降的功劳主要来自改进A星的覆盖惩罚项——转场路径不再肆意穿过未覆盖区域而是沿着已覆盖区域边缘走。路径长度缩短的一个重要原因是转弯次数减少。我在仿真里统计了下每一次转弯造成的额外路径代价大约是1.5倍栅格边长包含减速、调整方向的过程减少43次转弯就节省了约64个栅格长度的能耗正好对应路径长度缩短的差值。这里要说明一下我的仿真环境里没有加入机器人的真实运动学模型所以转弯代价是估算的。如果接入实际机器人底盘转弯代价会更大改进效果会更明显。4.4 改进A星权重参数的敏感性分析我把w_cov从0.0到5.0做了个遍历实验固定地图Map2观察重复率和转场路径长度变化。w_cov重复率转场路径长度覆盖率0.0标准A星18.9%11294.5%0.515.2%11896.8%1.011.7%12498.9%2.07.3%135100%3.06.1%148100%5.05.4%169100%从表格能清楚看到w_cov越大重复率越低但转场路径长度会上升因为A星为了绕开未覆盖区域走了更多路。在Map2上w_cov2.0是平衡点因为再往上调重复率的下降幅度明显收窄而转场路径长度的增加却越来越快。w_turn的影响类似但幅度小一些。我用w_turn1.0作为默认值在大部分地图上都表现不错。5. 常见问题与排坑实录5.1 覆盖率始终达不到100%怎么办我在Map3上第一次跑覆盖率只有97.3%怎么调都上不去。排查后发现是子区域分解时过窄的通道被膨胀后直接消失了导致通道另一侧的区域成了孤岛机器人根本过不去。解决办法有两个方向一是检查膨胀核的尺寸确保机器人直径对应的栅格数不要超过通道宽度二是对子区域的面积做筛选面积小于机器人活动空间的子区域直接标记为不可达并输出警告。另外如果覆盖率卡在98%左右大概率是边界栅格的问题。条带覆盖时条带两端的端点如果正好落在边界栅格里可能因为四舍五入没有覆盖到边角。解决方法是做一次边界补齐覆盖完成后扫描所有自由空间栅格如果有未覆盖且与已覆盖栅格相邻的用A星规划短路径过去补一圈。这个后处理能稳定把覆盖率拉到99.5%以上。5.2 转弯惩罚导致路径振荡调试时发现一个很隐蔽的问题w_turn设得比较大的时候A星规划出来的转场路径会出现不必要的振荡。原因是我的启发式函数是欧几里得距离但加入了转弯惩罚之后实际代价值与几何距离不再是单调关系启发式函数失去了一致性A星就不是最优的了。解决办法有两种一是把启发式函数改成曼哈顿距离 预估最小转弯次数但这个计算复杂二是我实际采用的方法——降低w_turn到1.0以下或者把启发式函数乘以一个小于1的系数保证h(n)始终不超过实际代价。我用的是后者把heuristic乘以0.9效果立竿见影路径振荡消失搜索效率也略有提升。5.3 大栅格地图下Matlab运行慢100x80栅格地图Matlab跑一次完整规划大概需要3到5秒其中A星转场规划占了约70%的时间。如果地图再大这个时间会线性增长体验会很差。我的优化手段有三个第一把containers.Map换成普通数组加优先队列。Matlab的containers.Map在频繁插入删除时开销很大。我测试过纯A星搜索时间能缩短40%左右。第二对启发式函数做缓存。在多次转场规划中大量节点的启发式值被重复计算。我可以提前算好所有节点到某个目标点的欧几里得距离矩阵查询时直接索引但内存占用会增大。在100x80地图下多出一个8000x1的矩阵内存完全不是问题。第三转场路径规划用粗粒度栅格先搜一遍找到路径后再在路径附近用细粒度栅格做局部优化。这个适合机器人直径远大于栅格边长的情况能把A星搜索的节点数降一个量级。具体代码优化示例把A星的开放列表改成普通数组用线性扫描找最小f值% 简化版本开放列表用结构数组 openList struct(x, {}, y, {}, g, {}, f, {}, parent, {}, dir, {}); openList(end1) struct(x, sx, y, sy, g, 0, ... f, heuristic(sx, sy, gx, gy), parent, 0, dir, 0);这种写法在节点数少于2000时速度比containers.Map快超过2000后线性扫描变慢就得换优先队列。我实际用的就是数组 每次线性扫描在100x80地图下完全够用。5.4 转场路径穿过未覆盖区域导致重复覆盖这个问题我前面提到过但值得再强调一遍。调试时我会把转场路径和覆盖路径用不同颜色画出来然后一眼就能看出来转场路径是否作弊穿越了未覆盖区域。最终修复方式是区分已覆盖栅格和已路栅格。转场路径经过的栅格更新到一个visitedMap里但不更新到coverageMap。这样如果后续覆盖路径再次经过这些栅格仍然算作有效覆盖不会因为被标记过而漏掉。可视化时我还会在转场路径经过的栅格上叠加一个半透明圆圈方便观察哪些地方被重复走了。这个调试技巧帮我发现了不少问题。6. 源码数据与扩展方向6.1 数据文件说明工程目录下data/result1.mat保存了一份Map2的完整实验结果包含% result1.mat 内容 % map: 原始地图矩阵 40x30 % inflatedMap: 膨胀后的地图矩阵 % subRegionIdx: 标签矩阵每个栅格记录所属子区域编号 % coverageMap: 覆盖结果矩阵0未覆盖1已覆盖 % fullPath: 完整路径Nx2矩阵N为路径点数 % metrics: 结构体包含覆盖率、重复率、路径长度等你可以直接加载这个文件plot(fullPath(:,1), fullPath(:,2))就能看到完整路径轨迹。6.2 后续扩展方向这套算法直接用在中小型室内环境的扫地机器人、割草机器人上已经足够落地。不过有几个方向可以继续扩展一是把静态地图换成动态地图。实际上家庭环境里经常有移动的人和宠物地图是时刻变化的。我的改进A星代价函数理论上可以扩展成实时更新覆盖惩罚只要在每次传感器扫描后更新coveredMap和map就行但需要加一个重规划触发机制不然频繁重规划会导致计算量过大。二是加入机器人的运动学约束。前文提到我的实现只做了转弯惩罚没有真正的运动学约束。如果要接入差速驱动的真实底盘最好用基于Dubins曲线或Reeds-Shepp曲线的路径平滑方法对A星搜索到的路径做后处理。三是覆盖顺序用动态规划替代贪心。目前跨子区域转场顺序是贪心策略在子区域数量小于等于5时可以直接用DP算全局最优如果子区域数量更多可以参考TSP的LKH算法覆盖率不变的前提下进一步缩短转场距离。我在实际使用这套算法的过程中最大的体会是全覆盖路径规划没有银弹组合已有的成熟算法往往比从零发明新算法更可靠。往返式保证了覆盖率的下限A星保证了转场路径的质量改进代价函数让两者自然衔接这个组合拳思路在工程上非常实用。如果你正在做类似项目建议先从标准往返式跑通再逐步加入改进项每一步都用可视化确认效果最后你会得到一套适合自己场景的稳定方案。本文还有配套的精品资源点击获取
