简介覆盖路径规划CPP是机器人自主作业中的关键问题要求在包含障碍物的环境中规划出能完全遍历作业区域的路径。这份资源将相关算法实现与实验数据整合成一个小型工具包面向机器人、自动化领域的学生与开发者。其内容以MATLAB环境为主涵盖环境建模、路径生成、覆盖策略对比等核心环节包含多个.m脚本、.mat数据文件、.png/.fig可视化结果以及测试说明文档。全部27个文件压缩后仅218KB轻量便携便于按需修改和二次开发。目前已有165人浏览学习。借助包内的图像处理脚本、栅格地图样例和示例结果读者可以快速复现并理解经典覆盖路径规划方法的运行流程同时包内提供的结果文件与测试文本也为评估覆盖率、路径长度等指标提供了依据在此基础之上可进一步扩展多机器人协作覆盖、动态重规划等研究。1. 别把全覆盖路径规划只当成“扫地算法”看到 CPP.zip 这个压缩包名有人会以为是某个工具库的压缩文件但真正值得关注的是它背后的缩写Coverage Path Planning全覆盖路径规划。做施肥机器人、植保无人机、仓储盘点、光伏板巡检的开发者日常都会被同一个问题卡住——给一块不规则地块怎么让设备走完全程、覆盖所有需要作业的区域同时尽量少绕路、少重复、少转弯。这个问题和扫地机路径不是一回事扫地机可以在房间里随意兜圈而作业类机器人要的是“确定的覆盖轨迹”每一条路径都得有可回溯的规划依据。反直觉的地方在于覆盖率从来不是唯一的考核指标。很多团队第一版跑起来覆盖率做到了 99%但重复率飙到 30% 以上作业时长翻倍、能耗上涨用户根本不买账。覆盖问题本质是“在栅格/几何空间里找一条经过所有目标区域的路径并优化长度、重复度、转弯代价”的组合优化问题这也是为什么标题里同时出现了 coverage problem、path planning、coverage planning 这几个词。这篇笔记按我自己的落地习惯展开先建模再选算法再写最小代码最后聊几个仿真里很难暴露的坑。2. 把覆盖问题拆成“要去哪”和“怎么走”地图建模与参数设定2.1 从连续地形到栅格地图全覆盖路径计算的第一步做 CPP 规划前必须先把连续地形转成可计算的地图表示。常见做法是栅格化把作业区域按固定分辨率切分成网格每个格子标记为可通行、障碍、边界外。地图表示的精度直接决定了后续路径的最短长度和转弯分布。栅格地图的优势是直观牛耕线Boustrophedon路径在栅格上可以直接按行扫描劣势是分辨率越高计算量越大所以分辨率不是越高越好。地图的来源一般有三条路一是从 SLAM 构建的二维占据栅格地图直接拿二是从 CAD 图纸或卫星影像手动标注边界和障碍后栅格化三是用外接多边形加语义障碍物离线生成。对纯 CPP 算法验证来说第三种最省事可以自己在脚本里定义多边形结束这是 4~5 个顶点组成的简单多边形即可不用一开始就上真实地图。下面是一段地图初始化的示意代码重点在数据结构的定义不依赖特定框架。用 Python 的二维数组存储每个格子的状态0 表示自由区域1 表示障碍或边界外。为了方便后续访问我还额外存了一个 occupancy 数组用 2 代表“不可到达”。import numpy as np def make_grid_map(width, height, resolution, obstacles): 创建栅格地图 width, height: 地图物理尺寸单位米 resolution: 每格长度单位米/格 obstacles: 障碍物矩形列表 [(x_min, y_min, x_max, y_max), ...] cols int(width / resolution) rows int(height / resolution) grid np.zeros((rows, cols), dtypeint) # 0自由, 1障碍/边界外 for x_min, y_min, x_max, y_max in obstacles: # 将物理坐标转成栅格坐标 c_min int(x_min / resolution) c_max int(x_max / resolution) r_min int(y_min / resolution) r_max int(y_max / resolution) grid[r_min:r_max, c_min:c_max] 1 return grid这段代码把真实世界的尺寸切分成了行列固定、位置可索引的栅格数组。逻辑很简单关键是参数 分辨率。选分辨率时不能只看地图大小更要看机器人的尺寸。比如机器人宽 0.8 米分辨率取 0.05 米地图精细但路径点数量爆炸分辨率取 0.1 米格子和机器人宽度的比例适中规划耗时显著下降。我一般按“机器人宽度的 1/5~1/10”来取小于 1/10 的栅格对全覆盖没有收益只会增加旋转和平滑的负担。2.2 膨胀半径与覆盖起点两个容易被低估的全局参数栅格建好后如果直接把障碍物边缘当边界那么沿障碍物边缘规划出来的路径会让机器人因为自身尺寸撞上障碍。所以要给障碍物做膨胀inflation把障碍栅格沿着四周扩展出一圈不可通行区域膨胀半径等于机器人最大外接半径加安全余量。def inflate_obstacles(grid, inflation_cells): 障碍膨胀处理 inflation_cells: 膨胀半径对应的栅格数 from scipy.ndimage import binary_dilation obstacle_mask grid 1 # 按结构化元素膨胀障碍区域 structure np.ones((inflation_cells * 2 1, inflation_cells * 2 1)) inflated binary_dilation(obstacle_mask, structurestructure) grid[inflated] 1 return grid这里的 scipy 只是做膨胀运算实际工程里也可以用卷积或距离变换代替。膨胀半径设得太小规划路径贴着墙遇拐弯容易刮擦设得太大窄通道被完全堵死覆盖率反而下降。经验值是机器人半宽加 5~10 厘米的安全余量然后除以分辨率换算成栅格数。这个参数在后面的覆盖路径里会直接影响牛耕线的间距和转弯空间务必单独记录到配置文件中方便回溯。起点选择也常被忽略。全覆盖路径的起点决定了第一条牛耕线的位置和方向也决定整个轨迹的能耗分布。常见做法是把起点放在充电桩/停放点投影到地图上的最近可通行栅格仿真时则直接指定为地图左下角最近可通行栅格。起点选在死角会导致一开始就出现大量无效转场选在开阔区域第一条线就能铺开。下表是我常用的一组栅格地图参数适用于轮式机器人在平地上的 2D 覆盖规划参数推荐值说明地图分辨率0.05~0.1 m/格按机器人宽度的 1/5~1/10膨胀半径机器人半径 5~10 cm换算成栅格数后取整数起始栅格最近可通行栅格从停靠点做 A* 得到牛耕线间距作业宽度 × 0.8~0.9保证横向重叠地图外边界外接矩形加 1 格防止越界这里的“牛耕线间距”在下一章会展开但这里已经能看出CPP 不只是路径计算它是“地图参数 覆盖策略 转场策略”三者耦合的事。参数之间互相影响比如膨胀变大之后可通行区域变少牛耕线的转向点数量也随之变化所以一定要在处理流程的每一步都把中间结果可视化存图方便后面排查。3. 用对算法三种覆盖策略的选型逻辑与适用边界3.1 牛耕往返Boustrophedon覆盖规划的基础扫描策略牛耕往返是最经典的全覆盖路径把区域按长边方向分成多条平行带机器人先从一边走到另一边折返回来走相邻的带形似牛耕地。它的好处是路径结构简单、覆盖证明容易、工程实现成本低适合形状接近矩形、障碍物少的地块。牛耕路径的核心参数有三个扫描方向、带间距、转弯方式。扫描方向选得不好会产生大量短行和尖锐转弯。我一般这样确定扫描方向把区域的最大内接矩形的最长边方向作为扫描方向或直接遍历 0~180 度每隔 5 度模拟一次选总路径长度最短的角度。后者的计算量不大但对不规则多边形特别有效。def generate_boustrophedon(grid, start, direction_deg, line_spacing_cells): 生成牛耕式覆盖路径 direction_deg: 扫描角度0 表示沿 x 轴 line_spacing_cells: 牛耕条带间距单位栅格 # 将角度转为栅格步进向量 rad np.deg2rad(direction_deg) step_x np.cos(rad) * line_spacing_cells step_y np.sin(rad) * line_spacing_cells # 生成等距的平行线参数方程: 以 line_off 为偏移 rows, cols grid.shape lines [] offset 0 max_offset cols rows # 覆盖整幅图的偏移范围 while offset max_offset: # 以角度 a 和偏移量 offset 截取直线上的可通行栅格 line_points [] for c in range(cols): # 这里仅示意 0 度方向的情况 if grid[0, 0] is None: # 占位逻辑实际用射线采样 pass # 实际应使用射线与栅格轮廓求交并采样 lines.append(line_pts) offset line_spacing_cells return lines这段代码只是骨架实际生成牛耕线要遍历偏移量在每个偏移上沿扫描方向求与自由栅格的交点再把交点串成线段。注意牛耕线的方向一定是扫描方向的平行线来回交替走相反方向各条线之间用 180 度折返连接。这个策略的关键在于带间距必须小于机器人的作业宽度否则两条相邻带中间会出现覆盖间隙。这里很容易出现“看起来 100% 覆盖实际没覆盖到”的问题所以带间距一定要留重叠工程实现里我会把间距设为作业宽度的 0.85。3.2 区域分解遇到凹多边形和障碍岛时的常用解法牛耕直接套用到凹多边形和内部障碍地块上会出现整条线被障碍打断、产生大量碎片路径的情况。所以先对区域做分解把不规则的连通域切成多个近似矩形的子区域每个子区域内部独立做牛耕子区域之间用转场路径连接。这是目前工程实践里最主流的 CPP 路线分解 分区牛耕。分解方式分为精确分解和近似分解。精确分解以障碍顶点为参考做垂直方向的切片把区域切成若干多边形单元近似分解更简单直接在栅格地图上用形态学方法或者连通域分析把连通可通行区域切成矩形格子块。from collections import deque def split_connected_regions(grid): 将可通行栅格按连通域拆分为多个子区域 visited np.zeros_like(grid, dtypebool) regions [] rows, cols grid.shape for r in range(rows): for c in range(cols): if grid[r, c] ! 0 or visited[r, c]: continue # BFS 找出该连通域 q deque([(r, c)]) visited[r, c] True region_cells [] while q: cr, cc q.popleft() region_cells.append((cr, cc)) for dr, dc in [(-1,0),(1,0),(0,-1),(0,1)]: nr, nc crdr, ccdc if 0 nr rows and 0 nc cols: if grid[nr, nc] 0 and not visited[nr, nc]: visited[nr, nc] True q.append((nr, nc)) regions.append(region_cells) return regions这个连通域分解算法很适合在栅格地图上做预处理能把地图切成互不相连的子区域。但连通域不等价于“适合牛耕的子区域”比如一个 L 形区域连通内部的弯折却会打断牛耕线。所以工程上常见的做法是先做连通域分析把距离过近的碎块合并再做主方向分析把每个连通域用旋转包围盒拟合拟合出的矩形可覆盖区域再进一步划分。这一步会产生大量参数经验是不要把分解目标定成“完全切满”宁可少切几块、留一点重叠也不要切出几十个碎块导致转场路径占比过高。3.3 螺旋覆盖与启发式顺序让覆盖路径更“像人开的”牛耕适合大地块但在地块边缘窄条形区域和障碍物周围螺旋覆盖内螺旋/外螺旋往往效率更高。螺旋覆盖从区域边界开始沿着等距偏移不断向中心收缩或者反过来从中心逐渐外扩。它的优点是转弯平缓、没有频繁 180 度折返适合履带式或差速转向底盘缺点是遇到凹区域需要先做凹坑填充否则螺旋会被打断。分完子区域后还有一个常被忽视的问题多个子区域的访问顺序怎么定。不同访问顺序会导致转场路径长度差异巨大这就是一个典型的“旅行商问题”变种。我一般用最近邻启发式把上一子区域的出口作为当前节点从剩余子区域里找最近的入口子区域数量少于 20 个时直接跑动态规划可以拿到全局最优。下表是两种顺序策略的对比策略计算量效果适用场景最近邻贪婪O(n^2)良好跳点少子区域少于 10 个DP 全排列搜索O(n^2 * 2^n)全局最优子区域少于 20 个按扫描方向排序O(n log n)一般规则田块到了这个层面CPP 问题的复杂度就从“生成一条线”变成了“在多个区域之间做路线组合优化”。很多现成的 CPP 方案看起来慢不是因为牛耕算法写得差而是区域访问顺序没优化转场距离占了总路径的 20% 以上。后续有经验的团队一般会把路径分为“覆盖段”和“转场段”分开记录统计时长时分开看这样问题一眼就能定位。4. 从零搭一套 CPP 最小实现地图、牛耕与 A* 转场4.1 数据模型与接口定义前面讲的是理论这一章给一套可以直接跑的最小代码不含外部 GUI 依赖只依赖 numpy。我的实现思路是把整个 CPP 分成三个模块地图模块、覆盖模块、转场模块。覆盖模块负责子区域的牛耕路径转场模块负责从当前点走到下一段覆盖起点的最短路径。先把地图数据结构定义出来。class CoverageMap: def __init__(self, grid, inflation_cells0): self.grid grid self.inflation_cells inflation_cells self.free_cells set() for r in range(grid.shape[0]): for c in range(grid.shape[1]): if grid[r, c] 0: self.free_cells.add((r, c)) def is_free(self, r, c): return 0 r self.grid.shape[0] and 0 c self.grid.shape[1] and (r, c) in self.free_cells这段代码把自由栅格提前索引成集合后续判断某个栅格能否通行时不需要遍历整幅图。注意在真实地图里障碍物膨胀可以在外部完成这里设计 inflation_cells 参数位是为了保存参数留痕。地图初始化后要随手打印栅格总数、自由栅格占比这两个数字是后续判断覆盖率的上限依据。4.2 在栅格地图上生成牛耕路径牛耕路径的实现可以简化成两步按扫描方向生成一组平行条带对每条条带提取处在自由区域内的线段端点。下面代码先处理最简单的情况——扫描方向为水平0 度即按行扫描。每行内找连续的自由栅格段再把这些段内的点按牛耕顺序排列。def boustrophedon_row(grid): 按行扫描的牛耕路径生成 返回按牛耕顺序访问的栅格坐标列表 rows, cols grid.shape path [] # 奇数行左到右偶数行右到左 for r in range(rows): free_in_row [c for c in range(cols) if grid[r, c] 0] if not free_in_row: continue if r % 2 0: path.extend((r, c) for c in free_in_row) else: path.extend((r, c) for c in reversed(free_in_row)) return path注意这里没有处理障碍间断这只是最简单的情形但已经能跑通流程。逻辑说明即使一行内存在障碍free_in_row 也只取可通行栅格会把障碍区域跳过去下面一行再从障碍的另一侧继续。真正的工程代码会比这个复杂因为障碍导致同一行的“连续段”不连续牛耕折返会跳过整段形成跨障碍跳跃。所以上面的函数用于验证框架可以用于真实地块会出现覆盖缺口。4.3 区域间转场用 A* 连接覆盖段全覆盖的关键不只是覆盖而是覆盖完一块之后怎么高效到下一块。这里我用 A* 做最短路径搜索代价函数用欧氏距离启发函数选曼哈顿距离。A* 是在栅格地图上做转场的标配简单稳定不用上 RRT 这类采样算法。import heapq def astar(grid, start, goal): 栅格地图上的 A* 最短路径 返回从 start 到 goal 的栅格坐标列表 rows, cols grid.shape open_heap [] heapq.heappush(open_heap, (0, start)) came_from {start: None} cost_so_far {start: 0} direction_4 [(-1,0),(1,0),(0,-1),(0,1)] while open_heap: _, current heapq.heappop(open_heap) if current goal: break for dr, dc in direction_4: nr, nc current[0]dr, current[1]dc if not (0 nr rows and 0 nc cols): continue if grid[nr, nc] ! 0: continue new_cost cost_so_far[current] 1 if (nr, nc) not in cost_so_far or new_cost cost_so_far[(nr, nc)]: cost_so_far[(nr, nc)] new_cost priority new_cost abs(nr - goal[0]) abs(nc - goal[1]) heapq.heappush(open_heap, (priority, (nr, nc))) came_from[(nr, nc)] current # 回溯路径 if goal not in came_from: return [] path [] node goal while node is not None: path.append(node) node came_from[node] return path[::-1]实现上用了 4 邻域因为牛耕的折返逻辑本身就是 4 邻域运动转场路径用 8 邻域更短但要额外处理对角穿障碍的问题。工程里转场段我会用 8 邻域然后做路径平滑在仿真验证阶段用 4 邻域就够了避免路径贴角产生碰撞风险。这个 A* 每跑一次的时间在几百毫秒以内百乘百的地图但如果覆盖段数量多到上百个每次调用还要拷贝地图状态性能瓶颈就会出现在这里后面可以通过缓存覆盖段出入口来优化。把上面三个模块串起来一个最小 CPP 的伪主流程如下def run_coverage_planning(grid, start_cell): regions split_connected_regions(grid) plan [] current start_cell for region in regions: # 牛耕生成覆盖段 cover_path boustrophedon_region(grid, region, direction0) # 从当前位置转场到覆盖段起点 transit astar(grid, current, cover_path[0]) plan.extend(transit[:-1]) # 去掉起点重复 plan.extend(cover_path) current plan[-1] return plan这段代码最接近我实际使用的框架顺序每个覆盖段内部还可以做螺旋/牛耕切换当前版本没做先把链路打通。跑出来的 path 是一串栅格坐标要落地成机器人轨迹还需要转成世界坐标、速度插值、平滑处理但这已经不在“覆盖规划”范围内属于运动控制的事。建议按这个链路先在仿真地图里跑通一次再往导航栈上接。5. CPP 落地避坑覆盖率虚高、重复路径与窄通道翻车实录5.1 覆盖率虚高边界栅格与“不可达栅格”没分清现象仿真报告显示覆盖率 100%但把机器人实际走过的脚印画出来发现地图边缘有一圈 1 到 2 格宽的细条从未经过。原因统计覆盖率时把网格空闲区域的每个栅格都列入目标集合但牛耕路径生成时因为膨胀处理边缘一圈被标记为不可通行机器人根本走不到。代码里统计分母没排除不可达区域。解决在指标统计里定义两个集合可达自由栅格和实际访问栅格。覆盖率分母只算可达自由栅格。同时生成牛耕线时要把膨胀后的地图作为输入并单独输出一份“覆盖目标区”的掩码图人工叠加路径图上无偏差确认。这个掩码图在真机阶段也要保存用来排除定位漂移导致的误报。5.2 重复率爆表重叠比例设置不当与折返策略过于死板现象同一片区域被扫了三次路径总长度接近最短可能长度的两倍电池续航严重不足。原因牛耕条带间距设得太密加上转场时每次都用 A* 从当前点走回上一条带起点导致覆盖与转场段反复重叠。很多刚接触 CPP 的团队习惯把条带间距设成作业宽度的 0.5 倍以为覆盖更保险但“密”不等于“好”。解决先把重叠率从 50% 降到 15% 左右保留 5~10 厘米的重叠余量即可。其次转场时不要总是回到本条的起点而是从当前条的终点走出直接找下一条的最近端点作为入口。这一步改完很多场景的重复率能下降 10 到 15 个百分点。最后用输出日志里的重复路径占比作为硬指标低于 10% 才算合格。5.3 窄通道被膨胀堵死区域被切成碎片现象地图里有条实际可以通行的小路膨胀后变成一列障碍栅格把原本一个连通区域切成两块覆盖两条子区域的转场路径被迫绕行路径长度暴涨。原因膨胀半径是针对机器人最大外接半径设计的但窄通道的宽度正好只比机器人宽 5 厘米。物理上能过膨胀后栅格地图上过不去。解决对这种窄通道单独设一个“窄通道豁免”集合在膨胀掩码里把它排除同时在控制层降低该通道的行驶速度、增加安全保障。更工程化的做法是在做连通域分析时对比膨胀前后的连通性变化一旦发现连通域被断开自动标记窄通道位置复核可通行性。这一步很费人力但值得做因为窄通道往往是整个覆盖任务中最容易触发碰撞的地方。5.4 锯齿状路径让覆盖看着全覆盖实际转弯次数爆炸现象地图中心有密集障碍物牛耕路径来回穿插产生大量短线段每次折返都是 180 度原地转向仿真路径图看起来像一片锯齿。原因扫描方向设成了地图外接矩形 x 轴方向而实际可通行区域的长轴方向偏了 30 度。短航线非常多因为每碰到障碍就要折返。解决把扫描方向改成可通行区域的主轴方向。做法是先对自由栅格做 PCA 主成分分析取最大特征值对应的方向作为扫描方向。这个改动对复杂地块的转弯次数影响极大通常能减少百分之三十以上的转向动作。同时对过短的覆盖段做合并小于三四条栅格宽的碎片区域直接并入邻近覆盖段避免单独规划。5.5 仿真完美真机偏差大地图原点没对齐现象仿真路径覆盖率极高上真机之后偏了一整排牛耕线实际覆盖率和仿真差出 20%。原因地图栅格起点与机器人定位系统起点之间存在偏移仿真里两者都是 (0,0)真机上地图原点定义不一致。覆盖路径转成世界坐标时整体平移了一小段距离靠近障碍物的地方直接撞上。解决把地图坐标系、定位坐标系、车身坐标系的变换关系列一个显式的转换矩阵并在运行时输出第一帧坐标校验是否与仿真一致。建议在规划代码里加一个 debug 模式把路径坐标的前 10 个点打印出来和地图原点做差值检查确保坐标系没问题再跑覆盖。这一步属于血泪经验早年做某图像处理 Demo 时因为少对齐一个偏航角整个项目多耗了一个多月。6. 十分钟跑一个覆盖质量评估指标脚本与归档习惯全覆盖路径规划做完之后判断“好不好”不能只看路径图。我习惯写一个独立的评估脚本用四个指标量化覆盖段和转场段覆盖率、重复访问率、平均转向角度、总路径长度。这套脚本在仿真阶段和真机阶段用同一份代码保证数字可对比。def evaluate_coverage(path, free_cells, direction_changesFalse): 评估覆盖质量 path: 覆盖路径点列表 free_cells: 可达自由栅格集合 accessed set(path) coverage len(accessed free_cells) / len(free_cells) # 重复访问率 总访问次数去重后的差值比例 total_visits len(path) unique_visits len(accessed) repeat_ratio (total_visits - unique_visits) / total_visits # 转向次数连续三点夹角变化超过阈值 turn_count 0 for i in range(2, len(path)): d1 (path[i-1][0]-path[i-2][0], path[i-1][1]-path[i-2][1]) d2 (path[i][0]-path[i-1][0], path[i][1]-path[i-1][1]) if d1 ! d2: turn_count 1 return { coverage: round(coverage, 4), repeat_ratio: round(repeat_ratio, 4), turn_count: turn_count, total_path_len: len(path), }评估脚本每次跑完都把结果与地图参数、算法参数一起写入 JSON 存档作为实机测试前的验收依据。我习惯在生成路径的同时导出三张图覆盖目标掩码、实际覆盖栅格热力图、转向点分布图。没有这三张图光看数字很容易漏掉边缘细条和障碍物附近的覆盖空洞这类“只看总量”的评估方式在项目评审里翻车概率极高。再到真机验证的阶段评估脚本要加上时间戳和坐标序列输出。每次跑完覆盖任务保存一份 csv包含时间、x、y、yaw、覆盖状态枚举。后续做误差分析时把这份 csv 和仿真输出的路径叠加能直观看出每一段牛耕线的偏差方向偏差方向一致的回去查地图原点或标定参数方向不一致的查控制跟随误差。这是我最常用的一套闭环排查习惯。如果你刚开始上手 CPP先别追求算法复杂度用牛耕加 A* 跑通一条最简单的闭环路径然后把评估指标脚本跑顺再逐步加区域分解、主轴扫描方向优化、区域访问排序。这个顺序能少走很多弯路每次只引入一个变量覆盖率和重复率的变化都有据可查。希望帮到你。本文还有配套的精品资源点击获取