我前阵子调一台自动巡检小车跑了两圈构建地图之后发现导航规划路径总喜欢往角落里钻有些路径明明旁边全是墙它非要绕个大圈从墙缝里穿过去。后来把代价地图打开仔细一看问题出在地图上那些灰色未知区域SLAM 构建的栅格地图里不光有障碍物和可通行区域还有一大片一大片没有探测过的灰色地带。这些灰色区域有的在地图边缘属于“还没探索完”的地方保留没问题但有一些被障碍物结结实实包在里面机器人一辈子也进不去它们在导航栈里却照样参与代价计算把路径规划搅得乱七八糟。这篇文章就是我处理这个问题的完整记录核心思路是借助 OpenCV 对 ROS 栅格地图做连通域分析把那些被围死的“无意义未知区域”识别出来并清理掉。如果你也在做机器人导航或者手头有一张看起来“坑坑洼洼”的二维栅格地图这篇内容应该能帮你省不少事。1. 为什么地图里会出现“无意义”的未知区域1.1 未知区域是怎么产生的先明确一个概念ROS 导航里常用的地图是二维栅格地图每个栅格有三种状态——可通行free一般是 0、占据occupied一般是 100、未知unknown一般是 -1。未知状态不是你主动标出来的而是 SLAM 算法在构图时没有传感器数据覆盖的地方。比如激光雷达扫描范围有限墙角背后、障碍物遮挡的阴影区、机器人根本没有开过去的空地这些位置在算法眼里就是“不知道那里有什么”。这些未知区域大体分成两类。第一类紧贴地图边界机器人在探索过程中只需要再往前走几步把激光打过去未知就变成已知了这类是有意义的未知必须保留否则会误导后续的探索规划。第二类藏在已建图区域的内部四周被占据栅格或不可达区域包围看起来就像地图中一块块灰色的“空洞”。这种空洞外层有墙挡住内层没有传感器入口哪怕你让机器人绕一圈它也无法进入这个区域因为路径规划永远找不到一条穿墙而过的路。我在实际项目里遇到的典型情况是这样一块占地几十平米的室内场地地图建完以后地图中间出现一块几平米大小的灰色区域。从传感器视角看周围一圈都是墙壁或障碍物没有任何通道。导航栈里的全局规划器会把它当作未知区域处理按照参数配置有的 planner 会认为未知可以通行于是规划出来的路径居然试图从墙之间挤过去有的 planner 会认为未知是禁区路径又绕着它走了一个巨额弧线。不管哪种结果都不理想。1.2 未清理的未知区域对导航的实际影响真正让我下定决心写这个工具的是导航过程中的三个具体问题。第一个问题出现在代价地图。ROS 的 move_base 里代价地图有一个参数叫track_unknown_space如果开启未知栅格会被当作障碍物的“未知道路”来膨胀代价极高如果关闭未知区域又会被当作可通行区域参与规划。无论开还是关地图内部这些永远进不去的灰色空洞都只会帮倒忙——它们要么让规划器认为那里能走实则根本过不去要么让规划器觉得那里有潜在障碍白白绕路。第二个问题出在局部规划器。DWA 或者 TEB 这类局部规划器在计算代价时会把代价地图里的未知区域纳入评分。当机器人靠近这些空洞时局部代价地图会突然出现大片高代价区域机器人经常会出现急停、抖动、甚至原地转圈找不到方向的情况。我排查了好久才发现不是规划算法调参有问题而是地图本身就带着一堆“坑”。第三个问题更隐蔽地图会越用越“脏”。如果把一张带大量内部未知区域的地图直接丢给长期运行的机器人每次重启导航代价地图都要重复计算这些区域。整个过程浪费 CPU还会在 RViz 里显示出一堆灰块干扰人工判断。地图的处理应该在工作流里占据一个固定环节就像构图之后做坐标对齐一样而不是等到导航出了问题再回头找原因。1.3 “无意义”的判断标准不是所有灰色都该删处理之前必须把标准定义清楚。我这里说的“删除无意义的未知区域”指的不是把地图里所有灰色都抹掉而是只删除那些与地图边界不相连、被已知区域完全包围的未知区域。判断依据是一句话如果从某个未知栅格出发沿着未知栅格四向或八向移动永远走不到地图边界那它就属于内部孤岛没有保留价值。反过来如果未知区域与地图边缘相连哪怕这个区域很大覆盖了半张地图那也是有意义的——它告诉机器人“外面还有没探索的空间”。探索任务还需要依赖这些区域判断下一步往哪走不应该动。这也是为什么不能直接做“把未知都设成可通行”这种粗暴操作的原因那样等于告诉机器人整个地图已经完整探索完了后续一旦接上自主探索模块很可能会停止探索行为。基于这个标准剩下的事情就变成了一个经典的图像处理问题给我一张灰度图图中值为 255 的像素表示未知区域找出所有不与边界连通的白色连通块把它们变成别的值。2. 格式转换OccupancyGrid 与 OpenCV 之间的数据桥接2.1 先说清 OccupancyGrid 的数据结构ROS 里地图消息的标准类型是nav_msgs/OccupancyGrid它有两个关键字段info和data。info里包含分辨率resolution米/像素、地图宽度width、高度height、原点坐标origin地图左下角的 Map 系坐标data是一个一维数组长度等于width*height按行优先顺序排列也就是第 0 个元素是地图左上角第一个像素接着往右走一行结束后从下一行最左边继续。data数组里每个元素的含义是0 表示可通行100 表示占据-1 表示未知。但也别死板理解这个值实际中有些建图算法会用 0~255 范围表示占据概率发布前再转换到 0~100。我们处理地图的时候最好在拿到消息后先打印一下data的最大值、最小值和几个典型值确认数据范围再动手我就遇到过某台设备发布的地图里占据值写成 254 的情况直接按 100 判断会漏掉障碍物。由于data是 int8 类型-1 在 C 里直接对应-1没什么问题但在 Python 里如果直接转成numpy.uint8-1 会变成 255这个其实正好方便。不过要注意千万不要一开始就把整个数组当成 uint8 来处理。我习惯的做法是先用np.int8读一遍原始数据生成未知掩码再转成uint8交给 OpenCV这样逻辑最清晰后面踩坑的几率也会小很多。2.2 灰度映射的约定OpenCV 里图像通常是 8 位单通道0 是黑255 是白。我用的映射关系是语义ROS 原始值转换后灰度值未知区域-1255白可通行区域00黑占据区域1000黑其他未知数值其他255白为什么把占据也映射成黑色因为清理未知区域的算法只关心“不知道”的区域长什么样不需要区分“可通行”和“占据”它们都是已知状态统一为 0 反而更容易做连通域分析。如果后面要做可视化可以再单独生成一张三色图黑色表示已知可通行白色表示未知灰色表示占据。但核心处理流程里不需要三色一张二值掩码足够了。2.3 转换代码模板下面是我实际用的转换函数直接放在节点文件里。输入是nav_msgs/OccupancyGrid输出是两个 numpy 数组一个用于 OpenCV 处理的灰度图map_gray一个是原始data的 numpy 形式data_np后面要用它来还原地图。import numpy as np import cv2 from nav_msgs.msg import OccupancyGrid def occupancy_grid_to_cv_binary(msg): w, h msg.info.width, msg.info.height data_np np.asarray(msg.data, dtypenp.int8).reshape(h, w).copy() # 未知区域掩码原始值为 -1 的位置置 255其余置 0 unknown_mask np.where(data_np -1, 255, 0).astype(np.uint8) # 所有已知区域包含占据和可通行统一为 0方便后续处理 map_gray unknown_mask.copy() return map_gray, data_np, msg.info反向转换就是把处理后的掩码再还原成data数组。我的做法是拿到一个cleaned_unknown_mask这个掩码在需要清理的位置是 0其余位置保留原值。所以还原时直接把original_data中掩码为 0 的位置改成 0可通行其他位置保留原样。占据的 100 保持不动原有未知但保留下来的 -1 也保持不动。def cv_binary_to_occupancy_grid(original_data_np, cleaned_unknown_mask, info): h, w info.height, info.width new_data original_data_np.copy() # cleaned_unknown_mask 中值为 0 的像素表示被判定为无意义的未知区域 new_data[cleaned_unknown_mask 0] 0 # 置为可通行 msg OccupancyGrid() msg.info info msg.data new_data.reshape(-1).tolist() return msg这一步看起来简单但有一个容易忽略的细节info里的origin和分辨率必须原封不动带回给新地图否则地图的坐标系就偏掉了。我之前手滑把origin输错发布的地图虽然在 RViz 里看着一样但机器人定位直接跳了好几米排查了半小时才发现是地图源的坐标原点被覆盖了。所以反向转换里我唯一允许被修改的只有datainfo一概不动。3. 核心思路怎么用连通域区分“有意义”和“无意义”3.1 为什么选 floodFill 而不是直接做形态学滤波拿到灰度图之后第一个想法通常是做闭运算把小白洞填上。但闭运算本质是基于结构元素的局部操作你很难控制它只会填充“被障碍物包围的未知空洞”而不会破坏“和边界相连的大片未探索区域”。拿我家楼下走廊的地图举例地图左边一整块都是没探索过的未知区域宽度可能横跨几十个像素闭运算用一个 3x3 或 5x5 的核根本起不到任何填充效果反而可能把一些细长的可通行窄道误伤。我采用的方案是基于连通域的 floodFill漫水填充。思路非常直观把整张地图看作一张图片未知区域是白色前景已知区域是黑色背景。如果我用一个填充算法从图像左上角开始沿着所有与图像边界相通的白色像素一直“灌水”那么最终那些被灌到水的白色像素就是与地图边界连通的未知区域——这些是有意义的。剩下没被灌到水的白色像素就像海洋里的孤岛周围都是黑色它们构成的就是“无意义未知区域”。OpenCV 的cv2.floodFill函数就是干这个的。它对图像中某个种子点开始将与种子像素颜色相近并且连通的区域替换为指定颜色。我只需要从左上角(0, 0)开始填充但前提是图像四周必须先扩大一圈把边界外也变成和未知区域一样的白色这样填充才能从边界外“灌”进所有与边界相连的白色像素。3.2 具体步骤拆解第一步给灰度图加边。使用cv2.copyMakeBorder上下左右各加 1 像素填充值取 255也就是白色。这样原本接触图像边缘的未知区域现在和外部白色区域连成一片了。第二步准备掩码。OpenCV 的 floodFill 有一个可选参数是掩码图用于限制填充范围。我准备一张和加边后图像同样尺寸的uint8掩码全部填 0表示不限制填充区域。注意掩码的尺寸必须比原图大两行两列因为 floodFill 内部会用掩码来标记已经访问过的像素。第三步执行填充。种子点选择加边后图像的左上角(1, 1)因为加了一圈边原始图像的(0, 0)现在对应到(1, 1)。要填充的目标颜色值设为 128灰色容差设为 0。填充之后原来连通的白色像素会变成 128。为了避免和其他灰度值冲突我用 128 这个在二值图里不存在的中间值来标记“与边界连通的未知区域”。第四步回到原始尺寸。把填充完的图像去掉加边的那一圈然后对比处理前后的图像如果某个像素原本是 255未知现在变成 128说明它与边界连通保留为未知如果原本是 255现在还是 255说明它是不连通的孤岛这就是要清理的目标。3.3 孤岛清理之后置为可通行还是占据清理出来的内部未知区域应该变成什么值这是一个值得掰扯一下的问题。我最初直接把它们置为 0可通行结果在地图可视化里看到原本灰色的区域变成了黑色导航也正常了。但后来有一次做全局路径规划机器人试图穿越一个内部充满障碍的孤岛区域因为那块地方被标成了可通行全局规划器觉得能走结果路径直接穿墙。原因就是那个孤岛内部其实还有一些占据栅格只是被未知包裹着我把整个未知孤岛置为可通行之后等于把里面的障碍也“放行”了。所以后面我改成了两步策略对于每个被判定为无意义的未知孤岛统计它内部是否存在占据栅格。如果存在占据栅格整个孤岛置为占据100表示这是一个机器人不能进入的区域如果内部几乎全是未知没有占据栅格置为可通行0。实际效果比我预期的好得多带墙的孤岛变成障碍物后全局规划器会很自然地绕开不需要额外调参。要识别每个孤岛内部有没有占据栅格可以用cv2.connectedComponentsWithStats对“无意义未知区域掩码”做连通域标记返回的每个连通域都有一个 bounding box在这个 box 范围里统计原始data_np里大于 0 的像素数即可。统计完之后逐个连通域决定置 0 还是 100。4. 完整代码地图清理节点实现4.1 节点工作流程我最终写的节点是一个 Python 的 ROS 节点订阅/map消息在回调里完成处理然后用 latched publisher 发布到/map_clean。为什么用一次性发布而不是持续处理因为地图在 SLAM 完成后基本不再变化处理一次就够了如果地图有更新可以再触发一次清理没必要在每个回调里重复计算。节点整体流程如下订阅/map。回调函数里把OccupancyGrid转成灰度图。执行 floodFill 得到“与边界连通的未知区域”掩码。用原始数据判断每个无意义孤岛内部是否含占据栅格。构建清理后的地图并发布到/map_clean。在 RViz 里订阅/map_clean即可检查效果。4.2 核心代码#!/usr/bin/env python3 import rospy import numpy as np import cv2 from nav_msgs.msg import OccupancyGrid class MapCleaner: def __init__(self): rospy.init_node(map_cleaner, anonymousTrue) self.map_pub rospy.Publisher(/map_clean, OccupancyGrid, latchTrue, queue_size1) rospy.Subscriber(/map, OccupancyGrid, self.map_callback, queue_size1) rospy.loginfo(Map cleaner started.) rospy.spin() def map_callback(self, msg): w, h msg.info.width, msg.info.height data_np np.asarray(msg.data, dtypenp.int8).reshape(h, w).copy() # 未知掩码未知为255其余为0 unknown_mask np.where(data_np -1, 255, 0).astype(np.uint8) # 加一圈白边让与边界相连的未知区域连到图外 bordered cv2.copyMakeBorder(unknown_mask, 1, 1, 1, 1, cv2.BORDER_CONSTANT, value255) flood_mask np.zeros((h 2, w 2), dtypenp.uint8) # 从左上角原始图的(0,0)即加边后的(1,1)开始填充 cv2.floodFill(bordered, flood_mask, (1, 1), 128, flags0) # 去掉边 flooded bordered[1:-1, 1:-1] # 原来未知、flood后变成了128的是与边界连通的未知保留 keep_unknown_mask np.where((unknown_mask 255) (flooded 128), 255, 0).astype(np.uint8) # 无意义未知原本未知、flood后仍是255 bad_unknown_mask np.where((unknown_mask 255) (flooded 255), 255, 0).astype(np.uint8) # 针对每个无意义孤岛判断内部是否有占据 cleaned_mask unknown_mask.copy() num_labels, labels, stats, _ cv2.connectedComponentsWithStats(bad_unknown_mask, connectivity8) for i in range(1, num_labels): x, y, bw, bh, area stats[i] # 取两个像素余量避免越界 x0, y0 max(0, x - 1), max(0, y - 1) x1, y1 min(w, x bw 1), min(h, y bh 1) region_orig data_np[y0:y1, x0:x1] # 统计该区域内占据栅格的个数 occupied_count np.count_nonzero(region_orig 50) # ROS中占据一般是100大于50就当障碍 if occupied_count 0: # 内部有障碍整个孤岛置为占据 cleaned_mask[labels i] 0 # 先置0随后在最终数组里覆盖 data_np[(labels i) np.isin(data_np, [0, 100, -1])] 100 else: data_np[labels i] 0 # 内部无占据置可通行 # 将被判定为无意义的未知区域统一处理 # data_np 中原来为 -1且不在 keep_unknown_mask 里的置为0或100 cleaned_data np.where( (data_np -1) (keep_unknown_mask 0), 0, data_np ) # 如果孤岛内部有占据需要重新把对应位置置100这一步在循环里已单独处理 # 但要注意循环里处理过的位置此时可能被上一步覆盖因此需再次强制 for i in range(1, num_labels): region labels i if np.count_nonzero((data_np 100) region) 0: cleaned_data[region] 100 # 构造并发布新地图 new_msg OccupancyGrid() new_msg.header msg.header new_msg.info msg.info new_msg.data cleaned_data.astype(np.int8).reshape(-1).tolist() self.map_pub.publish(new_msg) rospy.loginfo(Published cleaned map.) if __name__ __main__: try: MapCleaner() except rospy.ROSInterruptException: pass4.3 代码里几个需要你特别注意的点我先说代码里最容易被坑的地方cv2.floodFill的flags0其实表示使用 4 连通还是 8 连通OpenCV 默认 floodFill 是 4 连通如果你希望 8 连通需要把flags设置成8 | (255 8) | cv2.FLOODFILL_MASK_ONLY之类的组合。我在上面的代码里直接用flags0是 4 连通。对地图来说 4 连通和 8 连接的区别在于斜向是否算连通地图对角线上的栅格其实并不物理相邻所以 4 连通更符合真实逻辑。如果你更习惯 8 连通或者你的地图来自特殊传感器斜向确实可能通过可以改成 8这一步自己决定。另一个点是connectedComponentsWithStats返回的labels数组类型是int32我在循环里用labels i做布尔索引性能没问题但要注意大尺寸地图处理起来会比较慢。我实测 2000x2000 分辨率的地图单次处理大约耗时 0.6 秒如果地图更大或者 CPU 相对较弱建议先降采样再处理最后再映射回去。还有new_msg.header我直接沿用了原地图的 header。但是由于这个节点是订阅后立即发布header里的时间戳是旧的RViz 可能会显示“地图较旧”。我试过把时间戳刷新为rospy.Time.now()也没什么坏处新地图既然是新生成的时间戳更新到当前更合理。上面代码里因为只写了new_msg.header msg.header如果你希望看到时间戳更新可以改成new_msg.header.stamp rospy.Time.now()。4.4 测试方式与运行效果检查写完了不要急着丢到实际机器人上先在 bag 或已有地图文件上测试。我的测试流程是准备好一张已有的.pgm地图和对应的.yaml文件。用map_server发布/map。运行上面的map_cleaner节点。在 RViz 里同时订阅/map和/map_clean用Map显示项加载两个话题Color Scheme分别设置成不同的方案。检查/map_clean里原来灰块的位置是否变成黑色或灰色以及地图边缘的大片未知区域是否保持不变。如果处理完发现原本的地图边缘都被“啃掉”了多半是 floodFill 的种子点或者边界连接设置有问题。我在第一版代码里加边宽度写成了 0导致贴边未知区域没有被判定为连通整个地图边缘全被清掉了。后来加边改回 1 像素问题消失。5. 实测效果与排坑记录5.1 真实地图上的清理结果我拿一张实际巡检场地地图测试尺寸大概是 1800x1200分辨率 0.05 m/px。原图里能看到走廊尽头、房间死角有不少大片灰色未知区域其中一部分在地图边缘保留了下来房间墙角、两个柜子夹缝中出现的几块孤立灰色区域被判定为无意义。清理后的地图中那些夹在障碍物之间的灰色空洞全部消失。最明显的变化是全局规划路径不再试图从柜子之间的狭窄缝隙硬穿DWA 局部规划器在相关区域也变得平滑了。代价地图里大量的高代价“未知膨胀区”被移除RViz 显示清爽了很多。耗时方面整个处理流程只有一帧0.6 秒左右完全可以在 SLAM 完成之后作为一次性的地图后处理步骤对现场运行没有影响。5.2 我踩过的几个关键坑第一个坑是 int8 和 uint8 混用导致的误判。最初我直接把msg.data转成uint8-1 自动变成了 255我用 255判断未知区域看起来没问题。但在“统计孤岛内部是否有占据栅格”的时候我用region_orig 0把 255 也当成了占据结果很多纯未知孤岛被误判成“内部有障碍”然后整块变成了占据地图上一片一片全黑了。后来我把原始数据保持为int8用 50只看占据问题消失。第二个坑是 floodFill 的掩码参数。OpenCV 的 floodFill 如果传入掩码掩码的尺寸必须是原图尺寸加 2也就是(h2, w2)否则会直接报错。我一开始理解成掩码和原图一样大就行结果程序报错说掩码尺寸不匹配。把掩码增大两行两列之后马上正常。顺带一提掩码里非零像素会被 floodFill 视为“墙”不能填充所以初始化为全零是必要的。第三个坑是发布地图的 latch 属性。我用rospy.Publisher(/map_clean, OccupancyGrid, latchTrue, queue_size1)是因为清理后的地图只需要发布一次后续订阅者加入时也能立刻收到最新数据。最初我忘记加latchTrue每次重启 RViz 后/map_clean都是空的必须重新触发一次订阅才显示。加了 latched 后这个问题解决。5.3 几个可以继续扩展的方向这套思路不止能清理地图还能接上其他图像处理技巧产生更多玩法。比如如果你想顺便把地图裁剪到实际探索范围可以利用 floodFill 的结果找到“已知区域”的最小外接矩形把地图裁剪以后调整info.origin这样整张地图文件就会小很多导航加载更快。不过这个操作涉及原点偏移计算需要额外谨慎。再比如地图处理之后如果发现某些孤岛区域内部其实有大量占据栅格说明这些地方可能是一堵墙或者是结构柱可以进一步做膨胀处理让这些区域在地图上占的“安全距离”更合理从而减少机器人贴墙行驶的风险。这个需求一般是在室内巡检、仓库盘点这类场景里比较常见膨胀半径要根据机器人尺寸调整。还有一个方向是把清理逻辑集成进建图流程在 SLAM 停止建图、地图保存之前自动执行一次清理让保存下来的.pgm文件本身就干净省得每次启动地图加载后还要再跑一遍后处理。这个集成非常容易只要在保存地图的代码前面插入相同的处理步骤即可。我在实际项目中已经把这段逻辑做成了一个独立的 ROS 节点放在建图后的后处理流程里配合脚本一键生成最终地图。如果你们也是用 Cartographer 或者 Gmapping 出图拿到的地图里有大量灰块不妨试试这个思路应该能让导航省心不少。最后分享一个处理习惯。我建议清理完地图后一定在 RViz 里把/map_clean的透明度调到 50% 左右叠加在原始/map上肉眼比对一遍。这个过程能帮你发现很多算法判断不合理的细节比如“这个孤岛其实是空的房间只是门被传感器漏掉了”“这个区域虽然被包围但入口很窄机器人可能真的能挤进去”这类边缘情况。算法是死的你的判断才是最后一道防线。通过 OpenCV 和连通域分析地图清理这件事可以从一次手动头疼变成一段标准化脚本省下来的时间足以让你多跑两轮实验或者提前下班吃顿好的。