扫地机器人从客厅一角出发,很多人第一反应是让它用最短路径算法冲到卧室,但真实需求远没有这么简单。如果机器人只会找一条最优路线,那它扫过的区域就是一条细线,整块地面基本没被动过。真正要解决的是在一个有障碍物的网格环境里,让机器人遍历所有可通行栅格,这就是全覆盖路径规划(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 参数用 col,Y 参数用 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] % 输出:path,N行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* 不是被全覆盖替代,而是成为全覆盖系统里的一个基础组件。接下来你可以试着把段访问顺序改成贪心最近邻,或者给地图加上代价权重,看看重复率和路径形态会怎么变化。我就是在这些小的改动里,慢慢建立起对路径规划算法调优的直觉的。