简介这是一份面向SLAM与机器人自主导航研究者的优质项目实战资源以ROS作为系统框架将ORB-SLAM视觉里程计与PSPNet101语义分割网络结合实现了从几何地图构建到环境语义理解的一体化方案。资源以zip压缩包形式提供共351个文件压缩包约56.84MB。文件类型涵盖C核心算法源码、Python辅助脚本、ROS消息与参数配置以及构建所需的CMakeLists与数据集运行示例目录清晰便于按模块二次开发。项目完整覆盖ORB-SLAM的特征提取、跟踪、局部建图与回环检测等关键模块并在基础上接入PSPNet101语义分割使机器人能识别墙壁、地面、家具等类别将几何地图升级为带语义标签的语义地图支撑更智能的路径规划与自主导航决策。对于希望掌握语义SLAM工程落地、理解多传感器融合与深度学习协同的开发者具有较高参考价值。目前已有255人学习下载适合具备一定ROS基础、正在进阶SLAM与场景理解的研究者或工程师动手实践。1. 为什么自主机器人需要语义SLAM而不是普通SLAM普通视觉SLAM给出一串定位坐标和稀疏点云但导航用的不是同一个数据。机器人走到门前几何SLAM知道这里有个平面却不明白“这是门”于是停在一米外反复规划路线。ORB-SLAM的定位稳定PSPNet101的语义分割精确但两者默认不通信这是大多数视觉slam项目落地到自主导航时的真实断层。把分割结果贴回SLAM生成的地图和代价地图机器人才会把“可以推开或绕过的门”“不能撞的墙”“要避让的人”区分开。这条链路在ROS里的工程做法已经成熟ORB-SLAM出位姿和深度PSPNet101出逐像素类别一个语义融合节点把两者合成带标签的地图再交给导航层做语义导航。适合已经跑过基础视觉slam、准备往自主导航方向深入的工程师新手也能按数据集先跑通再移植到真机。2. 语义SLAM的骨架ORB-SLAM定位与PSPNet101分割怎么协作2.1 ORB-SLAM能输出的东西位姿、关键帧和深度常见做法是让ORB-SLAM工作在纯定位线程、只输出相机位姿话题不要让它直接驱动导航。原因有两个其一ORB-SLAM回环修正时位姿会跳变move_base的局部代价地图不处理这类跳变其二建图阶段需要的是关键帧对应的位姿和深度而不是最终轨迹。ROS侧一般把位姿话题发布为/orb_slam3/camera_pose消息类型是geometry_msgs/PoseStamped频率跟随图像帧率常见30 Hz。模块输入输出话题与数据类型ORB-SLAM 视觉里程计左目或RGB-D图像相机位姿、路标点、关键帧/orb_slam3/camera_posePoseStampedPSPNet101 语义分割同一相机的RGB帧逐像素类别索引/semantic/seg_labelsensor_msgs/Image mono16语义融合节点位姿、深度、分割标签带语义标签的点云/semantic/points_outPointCloud2ORB特征点在弱纹理和动态物体上容易漂移机械臂或人员走动的场景会出现地图点跳变所以我在做这条链路时把定位频率和分割频率拆开前端跟踪跑全速分割只处理关键帧不每帧都跑一次PSPNet推理。关键帧由ORB-SLAM内部决定融合节点需要监听/orb_slam3/keyframe_pose而不是每一帧位姿每个关键帧对应的RGB图、深度图和分割结果合成一个batch写入语义地图。这种“低频分割加高频位姿”的方式会让地图更干净。实践参数上PSPNet推理步长设为5到10帧融合节点用环形缓冲队列把最近的分割结果按时间戳缓存队列长度设300帧足够。内存占用约等于一个300乘640乘480的uint8数组加一个同样尺寸的标签图不会爆内存。但这里引出一个关键问题分割结果和位姿不在同一时刻到达时间同步怎么处理。2.2 PSPNet101的语义理解金字塔池化给了什么PSPNet101网络本体不负责定位它解决“这个像素属于哪一类”的分类问题。与U-Net这种编码解码结构比PSPNet在编码器尾部插入金字塔池化模块用不同尺寸的池化核提取全局上下文特别适合处理室内走廊、空旷路面这类需要全局信息才能判别的目标。主干ResNet101的下采样倍率是1/32所以送入网络的输入尺寸一般保持640乘480输出上采样回原图后得到每个像素的类别索引。类别索引是整数矩阵不是概率热力图要得到概率需要额外做softmax。导航层只关心argmax结果不需要概率所以工程里直接发布索引图。部署方式建议用ONNX或TorchScript固化成静态图避免真机上装完整PyTorch。官方训练好的模型常用在Cityscapes或ADE20K上这两个数据集的类别id和机械场景的类别定义对不上比如Cityscapes把“道路”“人行道”“墙”分得很细而移动机器人导航只需要“可通行”“不可通行”“动态”三类。因此类别id必须经过映射表变成机器人自己的定义例如0背景、1地面、2障碍、3动态。融合节点只认机器人自己的id不认数据集原始id稍后章节给出映射表实现。2.3 融合节点的时间戳同步与坐标变换在ROS里跑这条语义SLAM链路三种同步方式中我用得最多的是approximate time sync。它要求两个话题时间戳相差在容差内就触发一次回调例如0.05秒以内适合RGB和深度相机频率相同的情况。exact time sync要求完全相等实际中几乎连续对不上除非用硬件触发。手动缓冲队列适合分割或异步来源当前RGB帧到了去队列里找时间戳最近的标签。一个很实际的坑sensor_msgs/Image的帧率不一致会让queue很快塞满。默认queue_size设10但如果分割步长为5帧输入队列必须放大到50以上否则老帧被丢弃融合节点经常拿不到与当前关键帧对应的分割结果。坐标变换上ORB-SLAM输出的是相机到世界的变换T_cw而把点云投影回世界需要世界到相机的逆变换T_wc所以取逆是必须的。另一个坐标系问题是内参PSPNet在去畸变图上做分割ORB-SLAM的深度也是去畸变后的图两者像素坐标才一一对应。为了保险我把进系统前的图先统一做一次cv2.undistort再分别送进SLAM和分割入口。融合节点收到一帧同步数据后做三件事核心伪代码# semantic_fusion_node.py 关键帧粒度的核心逻辑 def on_sync(rgb_msg, depth_msg, label_msg, pose_msg): # 1. ORB-SLAM 输出的 pose 是 T_cw转成相机到世界 T_wc T_cw pose_msg T_wc inverse(T_cw) # 2. 取相机内参把深度图转成规范化坐标 fx, fy, cx, cy get_camera_intrinsics() rows, cols depth_msg.height, depth_msg.width for u in range(rows): for v in range(cols): z depth_msg[u, v] # 深度值单位米 if z 0 or z max_range: # 剔除无效和超远距离 continue x (v - cx) * z / fx y (u - cy) * z / fy p_cam [x, y, z, 1] p_world T_wc p_cam # 转到地图坐标系 semantic_id label_msg[u, v] # PSPNet 出的类别索引 append_point(p_world, semantic_id) publish_pointcloud(points, frame_idworld)这段伪代码逐像素循环真机上必须用矩阵运算重写否则跑不到可用帧率。我一般拆成三步先用meshgrid生成坐标网格再用深度掩码过滤最后用批量矩阵乘法变换到世界系。参数上max_range通常设8到12米超出相机有效深度范围的点会引入大量噪声地图中表现为“飞点”。深度图NaN区域超过40%时建议直接把该帧扔掉不送入后端避免ORB-SLAM的位姿被带偏。普通点云每个点只带xyz语义点云每个点还带一个uint16的语义id。后面做导航时就根据这个id做过滤和膨胀。3. 用ROS搭出可复现的语义SLAM最小链路3.1 ubuntu 20.04 安装 ROS用鱼香ROS一键安装验收环境目前语义SLAM相关的ROS包如orb_slam2_ros和pspnet的ROS封装主要支持ROS1。所以在工程上我选择Ubuntu 20.04加ROS Noetic加OpenCV 4.2。手工装ros-noetic-desktop-full的依赖项太多最省时间的是用鱼香ROS一键安装脚本wget http://fishros.com/install -O fishros . fishros # 安装过程中选择 ROS Noetic对应 Ubuntu 20.04 # 脚本结束并自动 source source /opt/ros/noetic/setup.bash用. fishros而不是bash fishros脚本才会把环境变量导入当前shell。安装完成后用echo $ROS_DISTRO验证输出是否为noetic。接着启动USB相机驱动roslaunch usb_cam usb_cam-test.launch videodevice:/dev/video0 rostopic hz /usb_cam/image_rawrostopic hz看到30左右说明图像话题正常。如果做纯验证不上实体相机更稳直接用rosbag离线回放TUM或KITTI数据集能屏蔽相机驱动的时间戳抖动ORB-SLAM不至于因为时间戳乱序而频繁输出Tracker lost。3.2 工程工作区布局orb_slam3_ros、pspnet_ros 与 semantic_mapping我习惯把链路按包职责拆成四个独立目录放进同一个工作区src/ ├── orb_slam3_ros/ # 视觉定位发布位姿与路标点 ├── pspnet_ros/ # 语义分割订阅RGB发布label图 ├── semantic_mapping/ # 融合节点产出语义点云和八叉树地图 └── nav_semantic/ # 把语义层转成导航层的costmap输入不需要把PSPNet集成进ORB-SLAM源码里修改特征提取逻辑分割结果只作用于建图与导航。编译orb_slam2_ros或orb_slam3_ros时最常遇到OpenCV版本适配问题Noetic自带OpenCV 4.2旧代码里3.x的API会报编译错误需要把宏改成OpenCV 4的调用写法。依赖方面Ubuntu 20.04通过sudo apt install libeigen3-dev libboost-system-dev补齐ORB-SLAM3还需要Pangolin。无显示器环境编译Pangolin容易卡在GUI依赖上先装libgl1-mesa-dev libglew-dev再编。pspnet_ros节点建议用ONNX Runtime的C封装嵌入省去部署时装PyTorch和CUDA runtime。若想减少编译量用Python的rospy订阅RGB跑推理也可以缺点是ResNet101在640乘480输入下每帧约40到60毫秒看GPU型号。这个延迟在关键帧粒度下可以接受但CPU推理基本不可用。3.3 启动顺序先定位、再分割、最后建图启动顺序有讲究。先让ORB-SLAM跑起来并稳定跟踪几十帧轨迹不再闪烁后再启动语义分割节点最后启动融合建图节点。反过来时PSPNet推理结果没有相机位姿可匹配融合节点等待位姿队列越积越多最终内存溢出或产生整片重复点云。三段式启动命令# 终端1定位需要RGB-D话题 roslaunch orb_slam3_ros rgbd_tum.launch \ camera_topic:/camera/rgb/image_raw \ depth_topic:/camera/depth/image_raw # 终端2语义分割 roslaunch pspnet_ros pspnet.launch \ model_path:./models/pspnet101.onnx \ image_topic:/camera/rgb/image_raw # 终端3语义建图 roslaunch semantic_mapping semantic_map.launch \ point_topic:/camera/depth/points这里depth_topic如果是/camera/depth/image_raw需要在launch里加一个image_proc节点把它转成点云。三个阶段对应三个launch中每个node的respawn属性若ORB-SLAM死掉整个系统应当停止而不是继续空转建图。4. 语义地图生成从标签图到带语义的八叉树4.1 把PSPNet101固化成ONNX再挂在ROS上第一步把训练好的PSPNet101导出成ONNX。导出时的输入尺寸必须和实际相机分辨率一致。常见的错误是训练时用512乘512方形输入部署时直接resize成640乘480物体比例变形分割结果里地面和墙黏在一起。若相机是16比9训练和导出时都用640乘480或1280乘480不要用正方形。部署推理节点时用Python接口import cv2 import numpy as np import onnxruntime as ort # 使用ONNX Runtime加载固定模型不依赖PyTorch运行时 sess ort.InferenceSession( pspnet101_640x480.onnx, providers[CUDAExecutionProvider] ) def segment(rgb_bgr): # 输入是OpenCV格式BGR img cv2.resize(rgb_bgr, (640, 480)) img cv2.cvtColor(img, cv2.COLOR_BGR2RGB) img img.astype(np.float32) / 255.0 img (img - np.array([0.485, 0.456, 0.406])) / \ np.array([0.229, 0.224, 0.225]) x img.transpose(2, 0, 1)[None].astype(np.float32) out sess.run(None, {sess.get_inputs()[0].name: x})[0] # 取最大概率类别转成uint16作为语义索引图 label np.argmax(out[0], axis0).astype(np.uint16) return label注意三个参数归一化的均值和方差必须与训练一致预训练权重来自ImageNet就用ImageNet的统计量输入张量名和维度用NETRON或onnxruntime的get_inputs()确认是NCHW输出argmax后得到的是类别索引图发布为ROS的mono16图像话题比把120个类别通道全发出去省带宽。mono16在RViz里不能用默认颜色映射显示时先用cv_bridge转成RGB再可视化。4.2 语义融合节点把标签贴回三维点融合节点订阅四个话题RGB、深度、位姿、分割图把标签投影到点云。建图阶段建议先生成带“语义id”字段的PointCloud2不要直接生成八叉树因为后续导航要对不同类别做不同过滤策略。点云结构里加一个额外字段存语义id比把id编码进RGB字段更干净。体素滤波参数对着实际场景调!-- voxel_filter 参数控制语义点云密度 -- param nameleaf_size value0.03 / param namefilter_field valuez / param namemin_value value-0.5 / param namemax_value value3.0 /leaf_size是体素边长室内取0.03米走廊或空旷场地取0.05就够。filter_fieldz表示只保留世界坐标系z轴在-0.5到3.0范围内的点直接把天花板和地板以下噪点滤掉。这里滤掉的是点云里的离群点但ORB-SLAM的位姿漂移造成的整帧错位滤不掉所以融合时要注意定期检查轨迹闭合情况。八叉树地图用octomap_server参数按场景设!-- octomap_server 关键参数 -- param nameresolution value0.05 / param namesensor_model/max_range value8.0 / param namesensor_model/hit value0.97 / param namesensor_model/miss value0.4 /分辨率0.05和地图范围直接决定内存量。一个20米乘20米乘3米的房间用0.05米分辨率建图八叉树节点量很大保守估计几百MB内存建议先对语义点云做体素降采样再喂给八叉树点的数量直接决定建图速度和内存占用。hit0.97和miss0.4是贝叶斯更新的概率参数语义建图通常保持默认重点调max_range和resolution。4.3 类别id映射表与误检处理语义融合里最容易被忽略的参数类别映射。PSPNet原始数据集类别和机器人自定义id之间做重映射配置写在yaml里而不是硬编码原始数据集类别机器人语义id导航行为0 void/背景0忽略1 地面/道路1静态可通行面2 墙壁/结构物2禁行静障碍3 人/车/动物3动态层单独跟踪融合节点初始化时加载label_map.yaml遇到映射表里没有的原始类别统一归为背景。误检方面分割网络对远景小物体会把“门”分成“墙”如果导航需要识别可开关的门就得额外用几何特征校验门的轮廓宽度通常在0.8到1.2米高度在1.8到2.2米通过点云聚类后判断是否符合门的尺寸阈值再决定是否覆盖分割结果。5. 语义导航从语义地图到 move_base 能用的代价地图5.1 语义地图怎么切给导航层把可通行信息告诉 costmap语义地图不能直接给move_base。move_base管理的是2D代价地图需要的是每个栅格的可通行性。做法是从语义点云里按类别过滤出静态障碍物的点投影成2D栅格地图同时把地面、墙壁、动态实体分开处理。我用的配置是导航代价地图分两层静态层用语义障碍点云生成动态层用RGB-D实时检测的动态类别生成不跟建图耦合。静态语义建图可以容忍1到2秒延迟局部导航的障碍检测必须实时。costmap_common_params.yaml里给语义障碍层新建一个observation sourceobstacle_layer: observation_sources: semantic_obstacle semantic_obstacle: topic: /semantic/obstacle_cloud data_type: PointCloud2 clearing: true marking: true obstacle_range: 5.0 raytrace_range: 6.0 track_unknown_space: true语义层要求/semantic/obstacle_cloud只发布障碍类别的点不含地面。融合节点在建图结束后单独发布过滤后的点云只保留语义id为2的墙壁障碍体素地面类别1不进obstacle_layer。obstacle_range: 5.0表示5米以内进入局部代价地图室内小场景下调到3.0更稳避免把房间另一头的墙膨胀得过于夸张。clearing和marking必须同时为true机器人移动时对原先的障碍点做raytracing清除否则机器人的历史轨迹会残留在地图里污染路径。实际调参时观察RViz中局部代价地图的膨胀层如果路径规划来回抖动优先降低obstacle_range和膨胀半径。5.2 语义目标点替代坐标目标点自主导航如果只是避开障碍到达坐标那不算语义导航。语义导航的关键是把目标从坐标变成“带语义的目标点”。在语义栅格地图里按类别找目标比如找门def find_target(map_label, category_id1, x_rangeNone, y_rangeNone): # map_label 是二维单通道语义栅格值是语义id mask (map_label category_id) ys, xs np.where(mask) if len(xs) 0: return None # 返回可通行区域质心作为move_base目标点 return np.mean(xs), np.mean(ys)找到目标后用move_base发送rostopic pub /move_base/goal move_base_msgs/MoveBaseActionGoal \ {goal: {target_pose: {header: {frame_id: map, stamp: now}, \ pose: {position: {x: 1.5, y: 2.0, z: 0.0}, orientation: {w: 1.0}}}}}参数frame_id必须和全局代价地图的固定坐标系一致通常是map或odom不一致时move_base会报“No matching frame”。x和y是目标点坐标语义定位出的候选点还要做一次代价查询检查该位姿在语义代价地图上确实可通行并且周围膨胀层的代价值低于阈值否则会导致路径规划失败但导航一直尝试。5.3 动态语义类别要不要进静态地图常见错误是把语义分割的每一帧都并入静态八叉树。人走过去后地图里留了人形障碍后续规划认为那里永远不可通行。规范做法是静态层只保留静态类别即墙壁、门、楼梯、地面行人、车辆等动态类别只写进全局语义层或独立动态层不并入静态八叉树。如果PSPNet输出的类别映射表里动态类只有“人”和“车”就把这两个类别从建图类别里剔除导航层单独开一个costmap动态层订阅这些点。更细的做法是用一个带时间戳的环形缓冲动态类别的点连续N帧出现在同一位置才认为它转为静态例如停下的车。N设10到20帧太小会把短暂停顿的人错误固化太大则车停稳后导航还绕道很久。这一步决定了语义导航在走廊里碰到临时停靠的障碍物时是绕行还是卡住。6. Gazebo仿真中验证语义SLAM的四个关键检查6.1 仿真环境里先验证时间戳与坐标系用Gazebo搭一个含墙壁和门的室内场景给机器人模型加RGB-D相机后录制一段bagrosbag record -O semantic_test.bag \ /camera/rgb/image_raw /camera/depth/image_raw \ /orb_slam3/camera_pose /semantic/seg_label录20秒用rostopic hz检查四个话题帧率再rostopic echo看时间戳是否递增。时间戳间断跳跃先修相机驱动驱动仿真环境中把相机update_rate设为30并关闭sim_time对queue的影响。6.2 检查语义标签与几何轮廓是否对齐相机内参和分割输入分辨率不一致会导致标签与点云错位。把分割结果发布成/semantic/overlay话题在RViz里叠加到彩色图像上观察门的边缘是否与彩色图边缘对齐。1到2个像素的偏移在3米外会造成10厘米级的地图误差对代价地图影响不大但会把门的语义定位目标点拉歪导致机器人朝门框撞。6.3 用真实bag验证建图质量的三个指标回放bag观察语义点云三个指标深度无效点比例投影空洞面积以及障碍类别是否出现在地图上本不存在的区域。点云厚度超过5厘米时把八叉树分辨率改为0.1并打开体素滤波节点。6.4 导航层面验证目标点可达性验证语义导航的下发指令结果不要只盯着RViz看直接订阅move_base反馈rostopic echo /move_base/result | grep status_text返回Goal reached说明语义导航链路通了。若一直返回No valid plan回到语义栅格地图检查目标点的标签是不是被误判成障碍类别。用这个顺序能快速定位是建图阶段的问题还是导航配置的问题把语义SLAM的“建图-理解-导航”整条链路在一天内验干净。本文还有配套的精品资源点击获取 语义SLAM实战：ORB-SLAM与PSPNet融合实现自主机器人导航
简介这是一份面向SLAM与机器人自主导航研究者的优质项目实战资源以ROS作为系统框架将ORB-SLAM视觉里程计与PSPNet101语义分割网络结合实现了从几何地图构建到环境语义理解的一体化方案。资源以zip压缩包形式提供共351个文件压缩包约56.84MB。文件类型涵盖C核心算法源码、Python辅助脚本、ROS消息与参数配置以及构建所需的CMakeLists与数据集运行示例目录清晰便于按模块二次开发。项目完整覆盖ORB-SLAM的特征提取、跟踪、局部建图与回环检测等关键模块并在基础上接入PSPNet101语义分割使机器人能识别墙壁、地面、家具等类别将几何地图升级为带语义标签的语义地图支撑更智能的路径规划与自主导航决策。对于希望掌握语义SLAM工程落地、理解多传感器融合与深度学习协同的开发者具有较高参考价值。目前已有255人学习下载适合具备一定ROS基础、正在进阶SLAM与场景理解的研究者或工程师动手实践。1. 为什么自主机器人需要语义SLAM而不是普通SLAM普通视觉SLAM给出一串定位坐标和稀疏点云但导航用的不是同一个数据。机器人走到门前几何SLAM知道这里有个平面却不明白“这是门”于是停在一米外反复规划路线。ORB-SLAM的定位稳定PSPNet101的语义分割精确但两者默认不通信这是大多数视觉slam项目落地到自主导航时的真实断层。把分割结果贴回SLAM生成的地图和代价地图机器人才会把“可以推开或绕过的门”“不能撞的墙”“要避让的人”区分开。这条链路在ROS里的工程做法已经成熟ORB-SLAM出位姿和深度PSPNet101出逐像素类别一个语义融合节点把两者合成带标签的地图再交给导航层做语义导航。适合已经跑过基础视觉slam、准备往自主导航方向深入的工程师新手也能按数据集先跑通再移植到真机。2. 语义SLAM的骨架ORB-SLAM定位与PSPNet101分割怎么协作2.1 ORB-SLAM能输出的东西位姿、关键帧和深度常见做法是让ORB-SLAM工作在纯定位线程、只输出相机位姿话题不要让它直接驱动导航。原因有两个其一ORB-SLAM回环修正时位姿会跳变move_base的局部代价地图不处理这类跳变其二建图阶段需要的是关键帧对应的位姿和深度而不是最终轨迹。ROS侧一般把位姿话题发布为/orb_slam3/camera_pose消息类型是geometry_msgs/PoseStamped频率跟随图像帧率常见30 Hz。模块输入输出话题与数据类型ORB-SLAM 视觉里程计左目或RGB-D图像相机位姿、路标点、关键帧/orb_slam3/camera_posePoseStampedPSPNet101 语义分割同一相机的RGB帧逐像素类别索引/semantic/seg_labelsensor_msgs/Image mono16语义融合节点位姿、深度、分割标签带语义标签的点云/semantic/points_outPointCloud2ORB特征点在弱纹理和动态物体上容易漂移机械臂或人员走动的场景会出现地图点跳变所以我在做这条链路时把定位频率和分割频率拆开前端跟踪跑全速分割只处理关键帧不每帧都跑一次PSPNet推理。关键帧由ORB-SLAM内部决定融合节点需要监听/orb_slam3/keyframe_pose而不是每一帧位姿每个关键帧对应的RGB图、深度图和分割结果合成一个batch写入语义地图。这种“低频分割加高频位姿”的方式会让地图更干净。实践参数上PSPNet推理步长设为5到10帧融合节点用环形缓冲队列把最近的分割结果按时间戳缓存队列长度设300帧足够。内存占用约等于一个300乘640乘480的uint8数组加一个同样尺寸的标签图不会爆内存。但这里引出一个关键问题分割结果和位姿不在同一时刻到达时间同步怎么处理。2.2 PSPNet101的语义理解金字塔池化给了什么PSPNet101网络本体不负责定位它解决“这个像素属于哪一类”的分类问题。与U-Net这种编码解码结构比PSPNet在编码器尾部插入金字塔池化模块用不同尺寸的池化核提取全局上下文特别适合处理室内走廊、空旷路面这类需要全局信息才能判别的目标。主干ResNet101的下采样倍率是1/32所以送入网络的输入尺寸一般保持640乘480输出上采样回原图后得到每个像素的类别索引。类别索引是整数矩阵不是概率热力图要得到概率需要额外做softmax。导航层只关心argmax结果不需要概率所以工程里直接发布索引图。部署方式建议用ONNX或TorchScript固化成静态图避免真机上装完整PyTorch。官方训练好的模型常用在Cityscapes或ADE20K上这两个数据集的类别id和机械场景的类别定义对不上比如Cityscapes把“道路”“人行道”“墙”分得很细而移动机器人导航只需要“可通行”“不可通行”“动态”三类。因此类别id必须经过映射表变成机器人自己的定义例如0背景、1地面、2障碍、3动态。融合节点只认机器人自己的id不认数据集原始id稍后章节给出映射表实现。2.3 融合节点的时间戳同步与坐标变换在ROS里跑这条语义SLAM链路三种同步方式中我用得最多的是approximate time sync。它要求两个话题时间戳相差在容差内就触发一次回调例如0.05秒以内适合RGB和深度相机频率相同的情况。exact time sync要求完全相等实际中几乎连续对不上除非用硬件触发。手动缓冲队列适合分割或异步来源当前RGB帧到了去队列里找时间戳最近的标签。一个很实际的坑sensor_msgs/Image的帧率不一致会让queue很快塞满。默认queue_size设10但如果分割步长为5帧输入队列必须放大到50以上否则老帧被丢弃融合节点经常拿不到与当前关键帧对应的分割结果。坐标变换上ORB-SLAM输出的是相机到世界的变换T_cw而把点云投影回世界需要世界到相机的逆变换T_wc所以取逆是必须的。另一个坐标系问题是内参PSPNet在去畸变图上做分割ORB-SLAM的深度也是去畸变后的图两者像素坐标才一一对应。为了保险我把进系统前的图先统一做一次cv2.undistort再分别送进SLAM和分割入口。融合节点收到一帧同步数据后做三件事核心伪代码# semantic_fusion_node.py 关键帧粒度的核心逻辑 def on_sync(rgb_msg, depth_msg, label_msg, pose_msg): # 1. ORB-SLAM 输出的 pose 是 T_cw转成相机到世界 T_wc T_cw pose_msg T_wc inverse(T_cw) # 2. 取相机内参把深度图转成规范化坐标 fx, fy, cx, cy get_camera_intrinsics() rows, cols depth_msg.height, depth_msg.width for u in range(rows): for v in range(cols): z depth_msg[u, v] # 深度值单位米 if z 0 or z max_range: # 剔除无效和超远距离 continue x (v - cx) * z / fx y (u - cy) * z / fy p_cam [x, y, z, 1] p_world T_wc p_cam # 转到地图坐标系 semantic_id label_msg[u, v] # PSPNet 出的类别索引 append_point(p_world, semantic_id) publish_pointcloud(points, frame_idworld)这段伪代码逐像素循环真机上必须用矩阵运算重写否则跑不到可用帧率。我一般拆成三步先用meshgrid生成坐标网格再用深度掩码过滤最后用批量矩阵乘法变换到世界系。参数上max_range通常设8到12米超出相机有效深度范围的点会引入大量噪声地图中表现为“飞点”。深度图NaN区域超过40%时建议直接把该帧扔掉不送入后端避免ORB-SLAM的位姿被带偏。普通点云每个点只带xyz语义点云每个点还带一个uint16的语义id。后面做导航时就根据这个id做过滤和膨胀。3. 用ROS搭出可复现的语义SLAM最小链路3.1 ubuntu 20.04 安装 ROS用鱼香ROS一键安装验收环境目前语义SLAM相关的ROS包如orb_slam2_ros和pspnet的ROS封装主要支持ROS1。所以在工程上我选择Ubuntu 20.04加ROS Noetic加OpenCV 4.2。手工装ros-noetic-desktop-full的依赖项太多最省时间的是用鱼香ROS一键安装脚本wget http://fishros.com/install -O fishros . fishros # 安装过程中选择 ROS Noetic对应 Ubuntu 20.04 # 脚本结束并自动 source source /opt/ros/noetic/setup.bash用. fishros而不是bash fishros脚本才会把环境变量导入当前shell。安装完成后用echo $ROS_DISTRO验证输出是否为noetic。接着启动USB相机驱动roslaunch usb_cam usb_cam-test.launch videodevice:/dev/video0 rostopic hz /usb_cam/image_rawrostopic hz看到30左右说明图像话题正常。如果做纯验证不上实体相机更稳直接用rosbag离线回放TUM或KITTI数据集能屏蔽相机驱动的时间戳抖动ORB-SLAM不至于因为时间戳乱序而频繁输出Tracker lost。3.2 工程工作区布局orb_slam3_ros、pspnet_ros 与 semantic_mapping我习惯把链路按包职责拆成四个独立目录放进同一个工作区src/ ├── orb_slam3_ros/ # 视觉定位发布位姿与路标点 ├── pspnet_ros/ # 语义分割订阅RGB发布label图 ├── semantic_mapping/ # 融合节点产出语义点云和八叉树地图 └── nav_semantic/ # 把语义层转成导航层的costmap输入不需要把PSPNet集成进ORB-SLAM源码里修改特征提取逻辑分割结果只作用于建图与导航。编译orb_slam2_ros或orb_slam3_ros时最常遇到OpenCV版本适配问题Noetic自带OpenCV 4.2旧代码里3.x的API会报编译错误需要把宏改成OpenCV 4的调用写法。依赖方面Ubuntu 20.04通过sudo apt install libeigen3-dev libboost-system-dev补齐ORB-SLAM3还需要Pangolin。无显示器环境编译Pangolin容易卡在GUI依赖上先装libgl1-mesa-dev libglew-dev再编。pspnet_ros节点建议用ONNX Runtime的C封装嵌入省去部署时装PyTorch和CUDA runtime。若想减少编译量用Python的rospy订阅RGB跑推理也可以缺点是ResNet101在640乘480输入下每帧约40到60毫秒看GPU型号。这个延迟在关键帧粒度下可以接受但CPU推理基本不可用。3.3 启动顺序先定位、再分割、最后建图启动顺序有讲究。先让ORB-SLAM跑起来并稳定跟踪几十帧轨迹不再闪烁后再启动语义分割节点最后启动融合建图节点。反过来时PSPNet推理结果没有相机位姿可匹配融合节点等待位姿队列越积越多最终内存溢出或产生整片重复点云。三段式启动命令# 终端1定位需要RGB-D话题 roslaunch orb_slam3_ros rgbd_tum.launch \ camera_topic:/camera/rgb/image_raw \ depth_topic:/camera/depth/image_raw # 终端2语义分割 roslaunch pspnet_ros pspnet.launch \ model_path:./models/pspnet101.onnx \ image_topic:/camera/rgb/image_raw # 终端3语义建图 roslaunch semantic_mapping semantic_map.launch \ point_topic:/camera/depth/points这里depth_topic如果是/camera/depth/image_raw需要在launch里加一个image_proc节点把它转成点云。三个阶段对应三个launch中每个node的respawn属性若ORB-SLAM死掉整个系统应当停止而不是继续空转建图。4. 语义地图生成从标签图到带语义的八叉树4.1 把PSPNet101固化成ONNX再挂在ROS上第一步把训练好的PSPNet101导出成ONNX。导出时的输入尺寸必须和实际相机分辨率一致。常见的错误是训练时用512乘512方形输入部署时直接resize成640乘480物体比例变形分割结果里地面和墙黏在一起。若相机是16比9训练和导出时都用640乘480或1280乘480不要用正方形。部署推理节点时用Python接口import cv2 import numpy as np import onnxruntime as ort # 使用ONNX Runtime加载固定模型不依赖PyTorch运行时 sess ort.InferenceSession( pspnet101_640x480.onnx, providers[CUDAExecutionProvider] ) def segment(rgb_bgr): # 输入是OpenCV格式BGR img cv2.resize(rgb_bgr, (640, 480)) img cv2.cvtColor(img, cv2.COLOR_BGR2RGB) img img.astype(np.float32) / 255.0 img (img - np.array([0.485, 0.456, 0.406])) / \ np.array([0.229, 0.224, 0.225]) x img.transpose(2, 0, 1)[None].astype(np.float32) out sess.run(None, {sess.get_inputs()[0].name: x})[0] # 取最大概率类别转成uint16作为语义索引图 label np.argmax(out[0], axis0).astype(np.uint16) return label注意三个参数归一化的均值和方差必须与训练一致预训练权重来自ImageNet就用ImageNet的统计量输入张量名和维度用NETRON或onnxruntime的get_inputs()确认是NCHW输出argmax后得到的是类别索引图发布为ROS的mono16图像话题比把120个类别通道全发出去省带宽。mono16在RViz里不能用默认颜色映射显示时先用cv_bridge转成RGB再可视化。4.2 语义融合节点把标签贴回三维点融合节点订阅四个话题RGB、深度、位姿、分割图把标签投影到点云。建图阶段建议先生成带“语义id”字段的PointCloud2不要直接生成八叉树因为后续导航要对不同类别做不同过滤策略。点云结构里加一个额外字段存语义id比把id编码进RGB字段更干净。体素滤波参数对着实际场景调!-- voxel_filter 参数控制语义点云密度 -- param nameleaf_size value0.03 / param namefilter_field valuez / param namemin_value value-0.5 / param namemax_value value3.0 /leaf_size是体素边长室内取0.03米走廊或空旷场地取0.05就够。filter_fieldz表示只保留世界坐标系z轴在-0.5到3.0范围内的点直接把天花板和地板以下噪点滤掉。这里滤掉的是点云里的离群点但ORB-SLAM的位姿漂移造成的整帧错位滤不掉所以融合时要注意定期检查轨迹闭合情况。八叉树地图用octomap_server参数按场景设!-- octomap_server 关键参数 -- param nameresolution value0.05 / param namesensor_model/max_range value8.0 / param namesensor_model/hit value0.97 / param namesensor_model/miss value0.4 /分辨率0.05和地图范围直接决定内存量。一个20米乘20米乘3米的房间用0.05米分辨率建图八叉树节点量很大保守估计几百MB内存建议先对语义点云做体素降采样再喂给八叉树点的数量直接决定建图速度和内存占用。hit0.97和miss0.4是贝叶斯更新的概率参数语义建图通常保持默认重点调max_range和resolution。4.3 类别id映射表与误检处理语义融合里最容易被忽略的参数类别映射。PSPNet原始数据集类别和机器人自定义id之间做重映射配置写在yaml里而不是硬编码原始数据集类别机器人语义id导航行为0 void/背景0忽略1 地面/道路1静态可通行面2 墙壁/结构物2禁行静障碍3 人/车/动物3动态层单独跟踪融合节点初始化时加载label_map.yaml遇到映射表里没有的原始类别统一归为背景。误检方面分割网络对远景小物体会把“门”分成“墙”如果导航需要识别可开关的门就得额外用几何特征校验门的轮廓宽度通常在0.8到1.2米高度在1.8到2.2米通过点云聚类后判断是否符合门的尺寸阈值再决定是否覆盖分割结果。5. 语义导航从语义地图到 move_base 能用的代价地图5.1 语义地图怎么切给导航层把可通行信息告诉 costmap语义地图不能直接给move_base。move_base管理的是2D代价地图需要的是每个栅格的可通行性。做法是从语义点云里按类别过滤出静态障碍物的点投影成2D栅格地图同时把地面、墙壁、动态实体分开处理。我用的配置是导航代价地图分两层静态层用语义障碍点云生成动态层用RGB-D实时检测的动态类别生成不跟建图耦合。静态语义建图可以容忍1到2秒延迟局部导航的障碍检测必须实时。costmap_common_params.yaml里给语义障碍层新建一个observation sourceobstacle_layer: observation_sources: semantic_obstacle semantic_obstacle: topic: /semantic/obstacle_cloud data_type: PointCloud2 clearing: true marking: true obstacle_range: 5.0 raytrace_range: 6.0 track_unknown_space: true语义层要求/semantic/obstacle_cloud只发布障碍类别的点不含地面。融合节点在建图结束后单独发布过滤后的点云只保留语义id为2的墙壁障碍体素地面类别1不进obstacle_layer。obstacle_range: 5.0表示5米以内进入局部代价地图室内小场景下调到3.0更稳避免把房间另一头的墙膨胀得过于夸张。clearing和marking必须同时为true机器人移动时对原先的障碍点做raytracing清除否则机器人的历史轨迹会残留在地图里污染路径。实际调参时观察RViz中局部代价地图的膨胀层如果路径规划来回抖动优先降低obstacle_range和膨胀半径。5.2 语义目标点替代坐标目标点自主导航如果只是避开障碍到达坐标那不算语义导航。语义导航的关键是把目标从坐标变成“带语义的目标点”。在语义栅格地图里按类别找目标比如找门def find_target(map_label, category_id1, x_rangeNone, y_rangeNone): # map_label 是二维单通道语义栅格值是语义id mask (map_label category_id) ys, xs np.where(mask) if len(xs) 0: return None # 返回可通行区域质心作为move_base目标点 return np.mean(xs), np.mean(ys)找到目标后用move_base发送rostopic pub /move_base/goal move_base_msgs/MoveBaseActionGoal \ {goal: {target_pose: {header: {frame_id: map, stamp: now}, \ pose: {position: {x: 1.5, y: 2.0, z: 0.0}, orientation: {w: 1.0}}}}}参数frame_id必须和全局代价地图的固定坐标系一致通常是map或odom不一致时move_base会报“No matching frame”。x和y是目标点坐标语义定位出的候选点还要做一次代价查询检查该位姿在语义代价地图上确实可通行并且周围膨胀层的代价值低于阈值否则会导致路径规划失败但导航一直尝试。5.3 动态语义类别要不要进静态地图常见错误是把语义分割的每一帧都并入静态八叉树。人走过去后地图里留了人形障碍后续规划认为那里永远不可通行。规范做法是静态层只保留静态类别即墙壁、门、楼梯、地面行人、车辆等动态类别只写进全局语义层或独立动态层不并入静态八叉树。如果PSPNet输出的类别映射表里动态类只有“人”和“车”就把这两个类别从建图类别里剔除导航层单独开一个costmap动态层订阅这些点。更细的做法是用一个带时间戳的环形缓冲动态类别的点连续N帧出现在同一位置才认为它转为静态例如停下的车。N设10到20帧太小会把短暂停顿的人错误固化太大则车停稳后导航还绕道很久。这一步决定了语义导航在走廊里碰到临时停靠的障碍物时是绕行还是卡住。6. Gazebo仿真中验证语义SLAM的四个关键检查6.1 仿真环境里先验证时间戳与坐标系用Gazebo搭一个含墙壁和门的室内场景给机器人模型加RGB-D相机后录制一段bagrosbag record -O semantic_test.bag \ /camera/rgb/image_raw /camera/depth/image_raw \ /orb_slam3/camera_pose /semantic/seg_label录20秒用rostopic hz检查四个话题帧率再rostopic echo看时间戳是否递增。时间戳间断跳跃先修相机驱动驱动仿真环境中把相机update_rate设为30并关闭sim_time对queue的影响。6.2 检查语义标签与几何轮廓是否对齐相机内参和分割输入分辨率不一致会导致标签与点云错位。把分割结果发布成/semantic/overlay话题在RViz里叠加到彩色图像上观察门的边缘是否与彩色图边缘对齐。1到2个像素的偏移在3米外会造成10厘米级的地图误差对代价地图影响不大但会把门的语义定位目标点拉歪导致机器人朝门框撞。6.3 用真实bag验证建图质量的三个指标回放bag观察语义点云三个指标深度无效点比例投影空洞面积以及障碍类别是否出现在地图上本不存在的区域。点云厚度超过5厘米时把八叉树分辨率改为0.1并打开体素滤波节点。6.4 导航层面验证目标点可达性验证语义导航的下发指令结果不要只盯着RViz看直接订阅move_base反馈rostopic echo /move_base/result | grep status_text返回Goal reached说明语义导航链路通了。若一直返回No valid plan回到语义栅格地图检查目标点的标签是不是被误判成障碍类别。用这个顺序能快速定位是建图阶段的问题还是导航配置的问题把语义SLAM的“建图-理解-导航”整条链路在一天内验干净。本文还有配套的精品资源点击获取