做了几年机器人导航我发现自己反复回到一个最基础的问题程序里的环境到底长什么样。有人用几何特征有人用拓扑图但真正到了避障、路径规划、多传感器融合落地的时候绝大多数项目最终的落脚点都是一张栅格占用地图。它把一个连续世界切成一格一格的判断每一格回答一个问题——“这里能不能过”。听起来简单想把它画准、存得住、实时更新里面藏着非常多容易低估的坑。这篇文章就把我在实际项目里对栅格占用地图的理解、踩过的坑和可复用的做法一次性讲透。1. 栅格占用地图把连续世界拆成一格一格的判断题1.1 为什么几乎所有导航系统都绕不开栅格先聊一个最根本的问题我们明明有厘米级精度的激光雷达为什么不用连续坐标去描述环境反而要把它离散成网格我的理解是连续坐标对“定位精确”很友好但对“推理决策”非常不友好。机器人看到一个桌子它不需要记住桌角的精确坐标它真正关心的是桌子占住的这一块区域我的轮子能不能碾过去我的机械臂能不能伸进去。这是一个区域性的问题而不是一个点的问题。栅格占用地图正好把环境表达成“区域属性”。每一格代表地面上的一小块面积存储的是这一小块被占据的概率。或者说是这块区域对机器人而言的可通行程度。这种离散化带来三个实打实的好处传感器噪声被天然容忍。激光打在同一个位置不可能每次都返回完全相同的坐标但打在同一个栅格里结果就稳定得多。更新逻辑统一。不管传感器是激光、超声波还是毫米波都能变成“某个网格状态变化”的形式融合起来非常简单。路径规划算法可以直接作用。A*、Dijkstra、DWA这类算法都在离散网格上工作它们不需要理解“墙在几何上是什么形状”只需要知道“哪些栅格是占据的哪些是空闲的”。当然栅格地图也有缺点最明显的就是“固定分辨率”和“内存增长”。如果你把分辨率定成2厘米那么一个100米乘100米的区域就是5000乘5000个格两千万五百万个格这还是在二维。如果做成三维体素地图内存问题会更突出。所以栅格占用地图从来不是一个“拍了脑袋就选”的方案而是一个在表达能力和计算资源之间做权衡的方案。1.2 一张栅格地图里到底存了什么栅格地图的核心数据就是一个二维矩阵矩阵的每个元素对应一块物理区域。这个元素不只是0和1通常是一个概率值。常见的约定是0表示完全空闲1表示完全占据0.5表示未知。但实际存储很少直接用0到1的浮点数而是用对数赔率log-odds来保存后面会细说。我自己做项目时第一版最容易犯的错就是“用0和1画地图”。激光打在墙上就把对应格子设成1没打到的设成0。看起来没毛病但实际一跑就出问题激光打在玻璃上会偶尔穿透返回导致同一面墙这一帧是占据、下一帧是空闲人走过来的时候一部分点落在人身上另一部分穿过人腿之间的缝隙造成一个区域被反复翻转。最终地图上一片麻子。所以栅格地图的工程实现必须带“置信度”。一个网格被观测到一次就判定为墙和被观测到30次之后判定为墙显然可信度不一样。于是占据栅格地图的底层逻辑变成我要记录的是“根据到目前为止的所有观测这个格子被占据的概率有多大”而不是“这一帧激光说了什么”。1.3 分辨率的直觉从地面一块砖说起理解分辨率最简单的方式是把它当成地面铺砖。你把机器人活动区域铺成瓷砖每块瓷砖记录“站人了吗”。瓷砖越大整张图越省内存但也有两个代价一是小障碍物直接消失比如2厘米的线缆在10厘米分辨率下基本不存在二是边界模糊机器人靠近墙的时候它不知道自己能不能再往前蹭3厘米因为那一格已经标记为占据。那到底怎么选分辨率我个人的经验公式是分辨率至少要达到机器人底盘直径的十分之一到五分之一。比如一个直径40厘米的扫地机器人2到4厘米是一个合理的起步值。室内环境0.05米足够用室外无人车一般用0.1到0.2米因为室外范围太大0.05米会让计算量和存储直接爆掉而且室外激光噪声远大于室内高分辨率反而容易把噪声也一块块画进去。关键经验分辨率不是越小越好是“让地图能表达出你关心的最小障碍物”就好。2. 栅格背后的概率更新机制为什么不是“碰到就标记”2.1 一次激光测量到底携带了哪些信息激光雷达一帧扫描有几百甚至几千个点每个点不只是“有一个点打到东西了”。如果我们朴素地理解每个点会告诉我们一个占据的位置。但如果只看这些点地图会非常稀疏因为激光束经过的空旷区域没有被表达出来。这也是很多新人在第一次自己写栅格地图更新时都会漏掉的东西一条激光束从传感器出发在命中一个障碍物之前它所有经过的格子都应该被标记为“空闲”。只有最后那一段被击中的位置才应该标记为“占据”。这个过程在SLAM里叫做射线投射ray casting。想象你手电筒照到一面墙你能说“只有墙上那个亮点是存在的”吗当然不能。光束经过的空间是空的所以机器人能看见墙这本身就是信息。如果把这些途经点忽略地图会变成一堆互不相连的“亮点”路径规划算法只能看见稀疏的几个障碍点其他地方全是未知。实际结果就是机器人规划出一条“穿墙”路线。所以一次有效的激光测量应该拆成两部分中间的空闲栅格 终点的占据栅格。而终点那个栅格也不应该直接设成1而是把它的占据概率往上抬一点。每多观测到一次就往“占据”方向再移动一点。这就是概率更新的雏形。2.2 贝叶斯更新与log-odds一次讲透既然每个网格维护的是一个概率那么新的观测来了之后怎么把这个概率和旧的概率合并这里直接用贝叶斯公式是最自然的思路。假设某个网格被占据的概率是P(occ)我们想根据新的传感器测量z更新它。那么更新后的概率可以写成P(occ | z) P(z | occ) * P(occ) / P(z)其中P(z | occ)是传感器模型——也就是“如果这个格子真的被占据传感器返回当前这个测量的概率”。这个模型不需要特别精确通常只是一个常量。但真正的工程问题在于如果直接用概率值做多次乘法数值会快速逼近0或1而且浮点运算是先乘后归一化长时间运行后精度容易出问题。所以实际代码里几乎都用对数赔率log-odds也叫logit表示。赔率定义为L(x) log( P(x) / (1 - P(x)) )好处是新的测量可以用加法更新L_new L_old L_measurement其中L_measurement 0 表示“这一帧告诉我这个格子被占据了”L_measurement 0 表示“这一帧告诉我这个格子是空闲的”0表示未知、无信息这个加法更新推导很简单本质是把贝叶斯公式里的乘积通过取对数变成加法。工程上还有一个额外好处就是不会因为连续累乘导致下溢出。我常用的初始化是 L_old 0对应P0.5代表未知。一次“占据”观测的增量大概设为0.5到0.9一次“空闲”观测的增量设为-0.4到-0.7。数值大小取决于传感器噪声的严重程度噪声大增量小一点噪声小增量可以大一点。这里必须注意一个问题不考虑“阻尼”会让概率饱和。如果某格已经被占据观测了100次L值越来越大这时候有一帧因为传感器误检而标记它为空闲这一个-0.7根本拉不动它。所以更新公式后面通常会夹一个上下限L_new clamp(L_old L_measurement, L_min, L_max)我一般取L_max10L_min-10。换算成概率大约是 P0.99995和P0.000045。这样既保证了对噪声的鲁棒性又不会让状态彻底“焊死”。2.3 多传感器融合超声波宽波束怎么处理很多人一开始只用激光雷达后来想加入超声波或毫米波。这时候要注意不同传感器的“射线模型”不一样。激光可以近似成一条极细的射线而超声波是一个锥形波束它告诉你“这个方向上某个距离内有物体”但它无法精确到具体是光束中心还是边缘返回的。处理宽波束有个常用技巧不要把整个锥形区域都认为空闲也不要把返回点所在那一格直接标为占据。而是按角度分成多个子射线中间角度视为真实的射线边缘角度降低置信度。简单做法是对同一个网格如果被多个传感器同时认为是占据那么占据概率叠加得更高如果激光认为是占据而超声波认为是空闲则要设置传感器可信度权重。比如激光权重0.8超声波权重0.3最终更新量就是加权和。我自己实际项目里超声波主要是用来补盲区的所以很少让它直接参与栅格地图的更新更多是用它做紧急刹车。真正要融合时记住一个原则无论如何用一个统一的log-odds增量表别把各家传感器的输出直接叠加成一个概率值再存。栅格地图内部永远是log-odds概率值只是给人看的。3. 真实建图流程从一帧激光到一张完整地图3.1 坐标系与传感器模型先对齐再谈建图建图的输入通常不只是激光数据还包括机器人当前位姿。位姿可能来自里程计、IMU、或者SLAM估计出来的定位结果。最关键的预处理是将激光点从激光坐标系转换到机器人坐标系再转换到世界系。这个环节如果外参标定不准地图会出现“重影”——比如一面墙激光在前面几帧看到它在x1.0的位置后来机器人转了一圈再回来因为外参偏了0.1米墙画在x1.1的位置。这几乎是新手翻车率最高的点。我见过很多项目建图算法没选错、参数没调错地图就是糊最后发现是激光雷达安装支架歪了两毫米在5米外被放大成几厘米的重影。所以我自己在建图之前必做三件事静态外参标定确认激光相对机器人中心的平移和旋转。时间同步检查如果里程计和激光时间戳差超过几十毫秒在机器人高速移动时就会产生偏移。地面点过滤对于三维激光或雷达建二维栅格地图前要把高于机器人底盘高度的点滤掉只保留“能挡住机器人”的障碍物层面的点。3.2 我自己常用的栅格更新伪代码做了这么多概念铺垫上一段最直观的代码。下面这个流程可以用在任何语言里核心逻辑完全一样def update_grid_map(grid_values, grid_logodds, robot_pose, laser_points, params): # 把激光点转到世界坐标 for point in laser_points: world_point robot_to_world(robot_pose, point) # 射线投射从机器人位置到激光点末端先清空沿途格子 cells bresenham(robot_pose.xy, world_point) for cell in cells[:-1]: idx grid_index(cell, params.resolution, params.map_origin) grid_logodds[idx] params.free_increment # 终点是占据但也需要做范围检查 last cells[-1] idx grid_index(last, params.resolution, params.map_origin) grid_logodds[idx] params.hit_increment # 可选将logodds转成概率用于可视化/发布 for idx in updated_indices: grid_values[idx] sigmoid(grid_logodds[idx])这个代码里有几个细节值得提bresenham用画线算法找出激光穿越了哪些栅格。注意这个遍历顺序是从传感器位置出发到命中点结束不包括命中点本身。free_increment和hit_increment的值不一定要选成绝对值相等的相反数。因为一帧激光里空闲观测的数量远多于占据观测如果两者等幅最后地图会整体偏“空闲”。所以通常hit_increment绝对值比free_increment大一些比如hit0.8free-0.4。每次更新只修改“本帧激光穿过的格子”不需要把整张地图重算一遍这对性能非常重要。对一个400个点的一帧激光用上面的逻辑更新一次在普通CPU上大约需要几十微秒到几百微秒完全来得及实时跑。3.3 从“局部图”到“全局图”的拼接思路实际建图不是单帧更新而是边移动边更新。最简单的全局建图方法就是把每一帧激光点通过当前位姿变换到全局坐标系然后执行上面的栅格更新。这里的核心问题是当前位姿从哪来如果你的机器人只靠轮式里程计那么在平整地面上短距离还是可以的但一打滑就会出现地图漂移。所以工业级做法是叠加SLAM实时估计位姿并回环检测。地图本身和SLAM是相互依赖的关系SLAM用地图来校正位姿地图又依赖位姿来更新。这也是为什么Cartographer这类系统会把栅格地图作为核心后端。我在实际项目中很少自己单独写完整的SLAM大部分时候是用现成的SLAM框架构建栅格地图然后导出地图文件给导航用。构建完成之后的关键工作是“地图后期处理”清理噪声散点、填充小空洞、把地图转成导航用的代价地图。4. 地图数据结构与内存优化从大数组到稀疏存储4.1 一个朴素二维数组何时会撑爆内存先算一笔账。假设环境是200米乘200米分辨率0.05米那就是4000乘4000总共1600万个格子。如果每个格子存一个float4字节这张图就要64MB存一个double8字节就要128MB。看起来还能接受但真实项目里不会只有一层地图。导航系统通常需要多个图层静态占据层、代价层、膨胀层、障碍物层、机器人足迹层。单是5层地图叠在一起64MB就变成了320MB这还没算地图以多分辨率保存的占用。对于载具、无人机、室外机器人来说连续建图几小时地图范围可能超过1公里这时候直接使用二维数组存储会直接爆掉。4.2 实际项目里我用的几种存储方案我在实践中慢慢形成了这样一个优先级稀疏哈希表优先只存储被更新过的格子没有观测到的格子不占用空间。一个房间建图完成后真正被更新过的格子可能只占理论总格数的20%到40%走廊场景甚至会低于5%。用unordered_map或Python的dict存储时key是栅格坐标value是log-odds值。缺点是查询速度比连续数组慢但实际跑下来性能差别在毫秒级别完全可以接受。四叉树/八叉树四叉树适合二维栅格八叉树适合三维体素栅格。核心思路是如果一个区域内所有子格子的状态都相同那么在父节点直接存一个统一值不再往下细分。这在地图有大片空闲区域时非常高效。缺点是更新局部格子时需要从根节点向下遍历代码复杂度高一些调试起来也麻烦。分层金字塔地图对同一张地图保存0.05米、0.1米、0.2米等多层级。路径规划时全局规划用粗分辨率快速估算局部规划用精分辨率细致搜索。这种方案更偏应用层但不少商业导航产品就是这么干的。4.3 数据类型选择也能省一半内存很多人在栅格地图里存储概率值都用float甚至double其实完全没有必要。log-odds值的范围通常限制在[-10, 10]这个精度用int8_t就足够表达。把log-odds乘以一个缩放系数比如10范围[-100, 100]用单字节表示。整张1600万格子的地图内存直接从64MB降到16MB。我曾经在一个项目里为了优化把所有栅格从float改成了int16_t内存降了一半更新速度反而略快因为缓存命中率更高了。这里有一个工程折中的点如果你在嵌入式设备上跑这个优化几乎是必选项。提醒一句别只盯着“地图本身”的内存还要考虑后续的膨胀层、发布用ROS消息时的拷贝、可视化图像编码。这些中间副本也会吃不少内存处理不好会导致系统卡顿。5. 栅格地图使用中的高频翻车点与对应避坑方案5.1 分辨率选错不是细就好也不是粗就好前面提过网格分辨率的选择公式但实际项目里还有两个隐藏问题。太细0.02米分辨率下激光束经过的格子数量暴增射线投射计算量跟着涨同时每一个细小噪声都会被独立画成一个“障碍物”。很多清扫机器人在0.02米分辨率下建图地图上全是飞点。太粗0.2米分辨率下一个直径30厘米的障碍物只占一两格机器人路径规划时哪怕距离膨胀层做得好也容易贴得太近。我调试时的习惯是先用0.1米分辨率快速验证整个建图链路是否通畅确认没有问题后再降到0.05米。这样可以隔离参数问题链路没通你调到0.01米也没用。5.2 动态障碍物在静态地图里留下的“幽灵”栅格占用地图默认假设环境是静态的。但在真实世界里人、车辆、开门、移动的箱子都会出现。如果这些动态物体被写进长期更新地图最后地图上会出现“鬼影”明明人已经走了那面区域还是显示占据。解决这个问题有三个层次前端过滤检测动态点不和静态地图更新。判断方法可以很简单比如连续多帧同一个位置被观测为占据但位姿一致性偏差很大就判定为动态点。后端衰减对长期地图里的占据概率做时间衰减每隔一段时间把log-odds往0方向拉一点动态物体的“占据”特征会慢慢消失。语义分离把动态障碍物单独放到一个实时层静态地图层只保留过滤后的静态点。这也是自动驾驶里costmap的标准做法。我用得最多的还是第三种。建图阶段关闭一些动态层导航阶段实时维护障碍物层栅格地图本身只保存环境结构。5.3 阈值设置占据判定为何总是误触栅格地图最终要从概率或log-odds转换成一个“能否通行”的布尔值这就需要两个阈值占据阈值和空闲阈值。比如一个格子log-odds大于0.5判定为占据小于-0.5判定为空闲介于两者之间判定为未知。如果阈值设得太高动态噪声不会触发占据但是真实的细小障碍物也会被漏掉设得太低一片区域会被误判为墙。我在实际项目里一般把占据阈值设为0.7对应的log-odds约0.85空闲阈值设为-0.4对应的log-odds约-0.4。这组参数跑起来误判比较少。但是注意这些值不是万能的。传感器噪声低时可以把阈值调低让地图更敏感噪声高时必须调高阈值否则会满地都是障碍。5.4 传感器外参标定误差地图“重影”的根子前面提到了外参这里再深入一点。如果激光雷达相对机器人中心的安装角度偏了0.5度那么在5米外的障碍物位置就会偏移约4.4厘米。在一张0.05米分辨率的栅格地图里这几乎等于让同一堵墙被画到两个相邻格子上。我建议在建图之前做一个简单验证让机器人原地旋转观察同一面墙在地图上的投影是否稳定。如果墙的轮廓在旋转过程中来回摆动那很大概率是外参问题而不是建图算法问题。很多人在算法参数里折腾了一天最后发现是支架歪了。6. 从“画地图”到“用地图”栅格地图如何真正支撑导航决策6.1 占用栅格地图到代价地图的转化栅格地图本身不直接参与规划。路径规划器需要的是一张“代价地图”每个网格除了已知是否占据外还要知道“离障碍物多近算危险”。这个危险程度就是膨胀。膨胀的过程很简单对每个占据栅格以它为中心按机器人半径加上安全距离把周围一圈栅格的代价值从0平滑上升到一个最大值。路径规划搜索时代价高的栅格会被A*算法主动绕开。我在项目里一直强调不要直接在原始占用栅格地图上做路径规划。因为原始地图只有“0/1/未知”没有考虑机器人半径和姿态。就算机器人中心在一格空闲区域它的机身在旋转时也可能扫到旁边的障碍物。只有膨胀后的代价地图才能安全指导导航。6.2 地图的存储与传输一张地图最后到底存成什么建图完成后地图通常要打包保存下来。工业界最常见的格式是PGM图像加YAML配置。PGM里面每个像素对应一个栅格灰度值代表状态0表示占据黑色。254表示空闲白色。205表示未知灰色。YAML配置里记录分辨率、原点坐标、栅格长度等元数据。加载地图时解析图像矩阵恢复成log-odds或代价地图。这里推荐一个保存前的处理对地图做一次形态学闭运算把小空洞和空洞填补掉同时清理孤立噪声点。否则把建图结果直接扔给导航会出现很多莫名其妙的“小岛障碍物”路径规划器为了绕一个只有一两格的噪声点绕路绕大半天。6.3 多机器人共建一张地图需要注意什么最后聊一个常在多机器人协同、工厂物流场景出现的问题多个机器人各建各的图怎么合并成一张统一地图。理论上只要知道两个地图原点之间的相对位姿变换就可以直接叠加。但实际上不同机器人建出的地图可能存在尺度漂移、角度漂移更不用说各自局部地图的误差累积。我做的做法是先提取地图特征点做粗匹配再用栅格对齐做精配准。坐标变换算出后对重叠区域做加权平均——概率值取权重平均权重取决于每个机器人局部地图的置信度。多机器人地图合并没有银弹框架只是辅助真正麻烦的是各机器人定位误差的统计规律不一样这一点必须在合并前就估计出来。7. 我个人调试栅格地图的几条心得写到这里分享几个在项目里反复验证过的经验排序分先后第一先画一张“够糙”的地图再慢工出细活。我第一版建图永远用0.2米分辨率、低阈值先看整体结构对不对。一旦整体结构都是歪的调什么0.05米、调什么概率增量都是白费功夫。第二把传感器外参查三遍再动算法参数。地图重影、模糊、错位八成是外参或时间同步问题不是栅格更新公式问题。拿一个固定障碍物做原地旋转测试是效率最高的排查手段。第三地图更新里别贪心。free和hit增量不要设得太大。我见过有人为了让地图快速收敛把hit增量设成5结果机器人面前十厘米的纸箱被当成墙导航完全卡死。宁可让地图收敛慢一点也要保证传感器误检时不会造成灾难性后果。第四动态物体永远别和静态结构混在一个图层。即使你现在只做静态巡检你的工作场地里也一定会有门、车辆、人。不要等到地图被“幽灵”弄乱了才后悔一开始就把图层分开。栅格占用地图是一个看起来特别基础、实际又特别深的主题。它跨越了传感器、概率估计、数据结构、数据存储、路径规划好几个领域。把上面这套逻辑吃透不管以后是用现成导航框架还是自己写核心模块都会非常顺畅。希望这篇经验总结能帮你少走一些我当年走过的弯路。