多传感器融合与栅格概率地图：机器人环境建模与路径规划实战

简介这是一篇面向机器人、机器学习与深度学习研究者的参考文献围绕复杂环境下机器人环境建模展开重点介绍栅格地图法、拓扑图法、多传感器信息融合等关键建模思路以及如何利用概率计算消除传感器干扰、提升障碍物识别可信度适合作为相关课题、课程设计或论文写作的专业参考。资源包为单个PDF文件大小约1.96MB内容包含环境建模分类说明、栅格测距原理与公式要点并附有Thrun、Elfes等经典文献及国内相关著作的参考文献目录便于读者溯源扩展阅读。已有83人学习浏览适用于正在研究机器人导航感知、多传感器融合或需要快速掌握环境建模入门框架的本科生、研究生与工程师。通过阅读本文读者可以系统理解栅格地图法与拓扑图法的差异掌握多传感器数据结合概率模型处理环境不确定性的基本路径为后续实践或算法设计提供理论支持。1. 栅格概率与环境建模机器人理解世界的起点扫地机器人第一次撞上窗帘激光雷达返回的距离忽远忽近摄像头又被窗帘阴影干扰单独看任何一路传感器都不可靠。黄尚锋在《基于信息融合的机器人环境建模》里把这个场景拆成两个问题用哪种结构表示环境多路传感器如何在同一种表示里达成一致。答案落在栅格地图和概率模型上环境被切成边长为 a 的小方格每个格子存一个障碍物概率。栅格地图对路径规划和避障最友好拓扑图更适合更大范围的全局导航传感器反馈法适合特征稀疏的场景。这篇文献的价值在于把三者放进同一篇论述并强调用概率表达来应对传感器噪声。适合谁读准备在 ROS 2 里用激光雷达和摄像头做机器人导航的工程师或者在跑 SLAM、被栅格边长与概率阈值困扰的人。2. 栅格地图法与传感器反馈从二维矩阵到概率值2.1 三种环境建模方法怎么选栅格、拓扑与特征地图原文引用了 Bücken 等人对栅格地图法、拓扑图法和传感器反馈法的分类。这里容易被忽略的是这三种方法不是竞争关系而是不同尺度下的表示。栅格地图以固定边长 a 切分环境每个栅格独立存储障碍物概率分辨率由 a 决定拓扑地图只保留关键节点和连接关系比如走廊拐角、房间门传感器反馈法更像特征地图把激光雷达识别的墙角、平面等几何特征直接作为标记。方法数据结构优点局限典型场景栅格地图法二维数组/矩阵表示精细便于路径规划内存随面积和分辨率增长室内小范围、避障拓扑图法图/节点边轻量全局规划快忽略细节节点定义依赖人工楼宇、园区大范围传感器反馈法特征集合/点云直接利用原始观测对环境结构敏感鲁棒性差特征明显的工厂我一般选型时先把 a 定为 5 到 20 厘米跑一版再决定是否需要拓扑层。栅格地图做代价地图拓扑地图做全局任务规划两者用入口点关联。原文里 Thrun 和 Bücken 就是把栅格地图和拓扑地图结合使用这个思路后来成为分层导航的标配。另一个常见误用是把传感器反馈法当成独立地图使用。特征点在地图坐标系里没有面积概念路径规划拿它做避障会漏掉墙体厚度。所以我在项目里只用它做重定位也就是在运行中判断机器人是否回到已知区域而不是当主地图。2.2 传感器测距模型与栅格概率计算r、a 与置信度的关系原文说传感器采集信号通过二维矩阵存储经过计算比较呈现概率值。当环境嘈杂、干扰强时传感器概率低、可信度低。这个“概率”不是障碍物出现的频率而是栅格被占用的置信度。典型测距传感器模型里测量距离为 r 时只有落在 r 附近的栅格才应该增加占用概率而传感器与障碍物之间的区域应降低占用概率。import math def inverse_sensor_model(cell_distance, measured_range, cell_size, z_hit0.9, z_max0.1): 根据测量距离折算单个栅格的占用概率。 cell_distance: 栅格中心到传感器的距离 measured_range: 传感器测得的距离 cell_size: 栅格边长 a z_hit: 测量命中概率 z_max: 测量失败概率 if measured_range cell_size or cell_distance measured_range cell_size: return 0.5 if abs(cell_distance - measured_range) cell_size: return z_hit if cell_distance measured_range: return 0.3 return 0.5逻辑说明代码先排除测量范围外的栅格避免把未知区域错标成空闲再判断栅格是否落在距离传感器的命中区间。abs(cell_distance - measured_range) cell_size表示栅格中心与测距结果相差不超过一个格边长此时认为该栅格最可能被占用。传感器与障碍物之间的栅格返回 0.3这是低空闲概率而不是绝对 0主要给后续贝叶斯更新留余地。参数z_hit、z_max需要根据传感器厂商标定激光雷达一般z_hit取 0.8~0.95超声波取 0.6~0.8。注意cell_distance、measured_range、cell_size单位要保持一致。我遇到过 a 用厘米、r 用米的情况结果 5.0 米的测量值被当成了 5.0 个栅格整张地图全都错位。现在所有输入统一换算成米后再进入函数避免在后续融合里出现尺度漂移。2.3 二维矩阵存储与可信度判读上述函数对每一个栅格调用后输出仍是一张二维矩阵。常用做法是直接把概率存成 numpy 数组行、列索引对应栅格坐标。矩阵里 0.5 表示未探索接近 1 是障碍接近 0 是自由空间。阈值怎么定原稿没有给具体值实践里我会把占用阈值设为 0.7、空闲阈值设为 0.3两个阈值之间的格子视为未知避免微小噪声让地图频繁翻转。import numpy as np grid np.full((100, 100), 0.5) obs_prob inverse_sensor_model(np.linalg.norm([50, 40]), 5.0, 0.1) grid[50, 40] obs_prob occupied grid 0.7 free grid 0.3参数说明grid初始化成 0.5 的先验概率对应“完全没有信息”0.7 和 0.3 是阈值。如果环境噪声大可以把占用阈值调高到 0.8代价是漏检率上升如果障碍物细比如桌腿只有 5 厘米粗则要把 a 调小到 0.05 米同时接受内存和计算量增长。这里的np.linalg.norm([50, 40])是计算目标栅格与假设在原点传感器之间的欧氏距离实际工程里要换成传感器在世界坐标系的真实位姿。3. 多传感器信息融合贝叶斯更新把多路观测压进同一张图3.1 单一传感器为什么不可靠原文反复强调“避免单一传感器带来的不确定性”这一点在工程里比理论更刺眼。激光雷达对透明玻璃和黑色物体近乎失明摄像头在暗光下把阴影当障碍超声波测量的是圆锥区域内的最近反射误差经常有 10 厘米以上。如果把这些传感器各自建图直接叠加会出现同一栅格一个说占用、一个说空闲的矛盾。解决方式不是投票而是把观测转换成条件概率再按照贝叶斯规则逐帧融合。我拆过的一个压铸车间场景激光雷达被粉尘遮挡时会把整面墙测成远距离摄像头又因为逆光把金属地面识别成障碍。融合后发现关键在于两种传感器在同一时刻的置信度并不相等简单加权平均是不够的必须把每个栅格的观测模型写成条件概率。3.2 用 log-odds 做占用栅格更新直接连乘多个概率长时间运行后概率会向 0 或 1 饱和数值精度也扛不住。工程实现里几乎都用 log-odds 形式l_i log(p_i / (1 - p_i))更新式子简化为加法。# log-odds 更新 log_odds log_odds l_occ - l_priorl_occ是当前观测对应的对数占用比l_prior是先验对数占用比。读取地图时再通过p 1 - 1 / (1 exp(log_odds))换算回概率。代码块里的l_occ由逆传感器模型算出的概率转换而来。一次命中通常给l_occ 0.9换算后约 2.2如果传感器频繁误报把l_occ调小到 0.6 左右地图收敛会变慢但更可预测。3.3 双传感器融合 Python 实现激光雷达加摄像头我常在做激光雷达建图的同时用摄像头里程计同步输出候选障碍物区域。两个传感器坐标系不同先做外参标定投影到同一栅格坐标系。下面是一段融合核心代码import math def fuse_measurement(grid_logodds, sensor_prob, weight, prior0.5): grid_logodds: 当前栅格的 log-odds 值 sensor_prob: 当前传感器给出的占用概率 weight: 传感器权重激光雷达 0.7摄像头 0.3 prior: 先验占用概率 l_prior math.log(prior / (1 - prior)) l_meas math.log(sensor_prob / (1 - sensor_prob)) fused grid_logodds weight * (l_meas - l_prior) return fused逻辑说明weight控制这一帧观测对地图的贡献占比。激光雷达测距模型直接给 0.7摄像头经过视觉识别概率带有分类不确定性给 0.3。含义是先算出当前观测相对先验的信息增量按权重缩放后叠加到历史 log-odds 上。这样可以做到激光雷达主导几何结构摄像头只对雷达盲区补盲不会因为摄像头单帧误检把整个地图带偏。实际标定时weight随场景切换。开阔园区里摄像头权重可以提到 0.5室内走廊因为有大量玻璃和镜面雷达误检多反而应该降低激光雷达权重到 0.6。3.4 融合参数速查表参数含义推荐起始值表现异常时的调整栅格边长 a空间分辨率0.05 m室内地图碎点太多就调大占用阈值判定为障碍的下限0.7误检多就调高到 0.8空闲阈值判定为空闲的上限0.3漏检多就调低到 0.2雷达权重激光雷达信息增量权重0.7玻璃场景降到 0.5~0.6log-odds 上限防止饱和10地图抖动时降到 5log-odds 下限防止负饱和-10地图抖动时降到 -5log-odds 上下限把更新限制在[-10, 10]相当于概率被限制在 4.5e-5 到 0.99995 之间避免长时间累积后新观测无法改变旧结论。这张表可以直接作为项目初版的起点跑过一轮之后再按实际场景微调。4. 从概率栅格到机器人导航costmap、拓扑分层与 A* 路径规划4.1 概率栅格怎么变成导航 costmap建图输出的占用栅格不会直接给路径规划用导航栈一般会在它外面包一层代价地图costmap。ROS 2 Navigation2 里的 costmap_2d 把占用栅格作为静态层然后叠加障碍物层、膨胀层。膨胀半径决定机器人离墙的安全距离半径太小容易刮蹭太大在窄通道里会直接把路封死。常见坑是把占用阈值直接拿去做代价阈值导致地图上灰一点的点全被当成障碍。正确做法是先看栅格概率分布再设置 costmap 的占据代价。robot_radius: 0.18 inflation_layer: inflation_radius: 0.25 cost_scaling_factor: 3.0 static_layer: map_subscribe_transient_local: true参数说明robot_radius是机器人底盘半径膨胀层以此为基础外扩inflation_radius是障碍物膨胀范围设置成机器人宽度的 1.2 倍左右比较合适cost_scaling_factor越大远离障碍物时代价衰减越快路径会越贴近障碍物map_subscribe_transient_local: true表示静态地图使用 transient local 订阅模式避免地图发布前路径规划器拿到空地图。nav2 里我踩过的一个坑是 costmap 的update_frequency设置太慢地图已经变了 path planner 还在用旧代价展开结果机器人贴着刚刚出现的纸箱过去。检查时先看/local_costmap/costmap话题的发布时间戳排除传感器话题延迟之后再调update_frequency。4.2 拓扑地图在全局规划里的补位原文提到的拓扑图法工程上常用它做多楼层或园区级导航。栅格地图每层一张层与层之间用一个拓扑节点连接比如电梯口、安全通道。这样路径规划先在拓扑层找全局路线再落到每一层的栅格地图做局部避障。Thrun 和 Bücken 提出的 grid-based 与 topological maps 结合就是这种分层的雏形。对单层室内场景可以只在出口、门框处放几个节点不需要完整拓扑层。4.3 基于栅格地图的 A* 路径规划代价计算与邻域有了栅格地图后的路径规划最常见的是 A*。下面实现里我把占用概率大于 0.7 的栅格视为不可通行八邻域搜索时直接跳过。import heapq def neighbors(node, grid): for dx, dy in [(-1,0),(1,0),(0,-1),(0,1),(-1,-1),(-1,1),(1,-1),(1,1)]: x, y node[0]dx, node[1]dy if 0 x grid.shape[0] and 0 y grid.shape[1] and grid[x, y] 0.7: yield (x, y) def astar(start, goal, grid): open_heap [(0, start)] came_from, g_score {start: None}, {start: 0} while open_heap: _, current heapq.heappop(open_heap) if current goal: path [] while current: path.append(current) current came_from[current] return path[::-1] for n in neighbors(current, grid): step_cost 1 if (n[0]-current[0] 0 or n[1]-current[1] 0) else 1.4 tentative_g g_score[current] step_cost if tentative_g g_score.get(n, float(inf)): came_from[n] current g_score[n] tentative_g heapq.heappush(open_heap, (tentative_g abs(n[0]-goal[0]) abs(n[1]-goal[1]), n)) return []逻辑说明neighbors里grid[x, y] 0.7表示允许穿过概率小于等于 0.7 的栅格。注意这个阈值必须与建图时的占用阈值保持一致否则会出现建图认为无碍、规划认为有障碍的矛盾。启发式用曼哈顿距离适合四/八邻域混合搜索斜向移动步长按 1.4 估算比 1 更接近欧氏距离。如果栅格地图很大启发式可以换成欧氏距离减少搜索节点数。4.4 验证方法同一张场景对比单传感器与多源融合实验组建图数据障碍物误检率漏检率路径规划成功率仅激光雷达原始 scan8%12%86%仅摄像头语义分割 mask15%9%79%贝叶斯融合两者加权 log-odds6%7%94%表格里是示例数据不是标定结果实际跑 10 组场景取平均。做对比时要固定栅格边长 a 和移动速度否则差异会被变量混在一起。误检率统计的是原本没有障碍物的栅格被判成障碍的比例漏检率则相反。路径规划成功率按“从起点到终点无碰撞到达”定义。这类对比可以作为融合参数调整的依据。5. 栅格边长 a 与更新阈值落地时最容易被忽略的参数5.1 为什么 a 不是越小越好栅格边长 a 决定地图分辨率也决定二维矩阵规模。10m×10m 环境a0.05 需要 200×20040000 格a0.02 需要 250000 格。a 过小时传感器误差本身会被放大同一面墙在不同帧里被分到不同格地图出现梳齿状碎裂。我一般让 a 不小于传感器测距误差的一半。激光雷达误差 3 厘米a 至少取 0.06 米超声波误差 12 厘米a 至少取 0.2 米。多传感器融合时按误差最小的传感器决定上限按误差最大的决定下限。另外还要看运动控制精度。轮式机器人里程计漂移大时a 取 0.05 会让建图出现重影因为同一位置在不同时刻被反复写进不同格。此时优先修里程计而不是改 a。5.2 一次快速的标定流程用录制的 ros2 bag 回放数据分别以 0.05、0.1、0.2 三档建图统计每张地图的建图线程耗时和占用栅格话题发布频率。for a in 0.05 0.10 0.20; do ros2 run cartographer_ros cartographer_ros \ -configuration_directory config/ -configuration_basename robot.lua \ --ros-args -p map_resolution:$a done参数说明map_resolution是 Cartographer 配置里的栅格分辨率参数这里用命令行覆盖避免为每次实验改 lua 文件。跑完后对比三张地图0.05 的碎块数量是否明显多于 0.10.2 的窄门是否还在。碎块多就调高占用阈值或加大 log-odds 上限窄门被吞掉就降低空闲阈值。5.3 用回放数据验证最后一帧地图的信息量最后一个小技巧统计当前地图中 occupied 栅格数与 free 栅格数的比值。室内场景通常落在 0.2~0.5。比值过高说明传感器把大量噪声当成障碍需要提高占用阈值或降低传感器权重比值过低说明地图太保守路径规划会频繁穿越本应避开的区域。这个指标虽然粗糙但能在不看图的情况下迅速判断融合参数是否合理。本文还有配套的精品资源点击获取