3. 环境感知与建图

移动操作机器人要完成“走过去、看清楚、抓得稳”，第一步不是规划，而是知道自己在哪里、周围有什么。环境感知与建图正是这条链路的起点：激光雷达、深度相机、IMU 提供原始观测，SLAM 把这些观测对齐成一致的位姿轨迹，最终输出栅格地图、点云地图或语义地图，供导航与抓取模块消费。本节按“传感器原理 → SLAM 基础 → 三类地图构建”的顺序展开。

一、三类传感器的原理与互补性

激光雷达（LiDAR）通过测量激光往返时间或相位差获得距离。机械旋转式雷达在水平面扫描，输出一圈圈 2D 点；固态雷达用光学相控阵或 MEMS 扫描，体积小、寿命长。它的核心优势是测距精度高（厘米级）、不受光照影响、视场宽，缺点是点云稀疏、对玻璃和镜面易失效、成本较高。

深度相机分结构光与飞行时间（ToF）两类。结构光投射已知红外图案，通过图案形变解算深度；ToF 直接测量调制光的相位延迟。深度相机输出稠密深度图，近距离（0.3～3 m）细节丰富，非常适合抓取前的物体分割与位姿估计，但受强光干扰、对黑色吸光材质和透明物体效果差，视场角也有限。

IMU（惯性测量单元）由三轴加速度计与三轴陀螺仪组成，部分含磁力计。它测量线加速度与角速度，通过积分得到姿态与位移。IMU 频率高（100～1000 Hz）、短时响应快，但积分会累积漂移，单独使用几分钟就会发散。因此它通常作为“高频补充”，与低频但绝对准确的 LiDAR/相机融合。

二、SLAM 基础：前端、后端与回环

SLAM（Simultaneous Localization and Mapping）要同时估计机器人轨迹和地图。典型架构分四部分：

• 前端（里程计）：用相邻帧点云配准（ICP、NDT）或视觉特征匹配，估计帧间相对运动，输出粗略轨迹。
• 后端（优化）：把位姿作为节点、观测作为边，构建因子图，用最小二乘或 iSAM 求解全局一致轨迹。
• 回环检测：识别“回到走过的地方”，加入回环约束，消除累积漂移。常用 Scan Context、DBoW 等方法。
• 地图构建：按优化后的位姿把观测点投影到全局坐标系，形成地图。

按传感器组合，常见方案有：2D 激光 SLAM（Gmapping、Cartographer）、3D 激光 SLAM（LOAM、LIO-SAM）、视觉 SLAM（ORB-SLAM3）、视觉惯性 SLAM（VINS-Fusion）以及激光惯性 SLAM（FAST-LIO2）。移动操作机器人常选激光惯性方案，因为它在室内外都能稳定工作，且能直接支撑导航代价地图。

三、三类地图的构建方法与适用场景

栅格地图把空间离散成固定分辨率的格子，每格用概率表示被占据的程度。构建时用射线投射更新：从传感器原点沿光束方向，末端格子标记为占据，中间格子标记为空闲。常用分辨率 0.05 m，概率更新用对数几率（log-odds）避免数值下溢。它直接服务于路径规划与代价地图，是导航的“标准输入”。

点云地图保留三维点的原始坐标与强度，不做离散化。构建时把每帧点云按优化位姿变换到世界坐标系并拼接，常用体素滤波降采样控制规模。它几何保真度高，适合避障、抓取位姿估计和三维重建，但数据量大、不含语义，直接用于规划效率低。

语义地图在几何地图基础上附加类别标签，如“桌子”“椅子”“门”“可抓取物体”。构建流程通常是：先建几何地图，再用深度相机或 RGB 图像做目标检测与分割，把检测结果反投影到三维空间并关联到地图格子或点云簇。它让机器人能理解“这是什么”，从而支持语义导航和任务级抓取。

四、实操案例：室内抓取场景的建图配置

以一台搭载 16 线激光雷达、RGB-D 相机和 IMU 的移动操作机器人为例，建图流程如下：

# 1. 启动激光惯性 SLAM
roslaunch fast_lio mapping_mid360.launch

# 2. 启动深度相机与语义分割节点
roslaunch realsense2_camera rs_camera.launch
rosrun semantic_seg segmentation_node

# 3. 保存点云地图
rosrun pcl_ros pointcloud_to_pcd input:=/cloud_registered

# 4. 生成栅格地图（投影 + 膨胀）
rosrun nav2_map_server map_saver_cli -f ./maps/room

# 5. 语义标注：把检测框反投影到地图
rosrun semantic_mapping label_projector

五、常见误区与复盘

• 误区一：只靠深度相机建图。深度相机视场窄、易受光照影响，长走廊容易丢失。应把 LiDAR 作为主传感器，深度相机做近场补充。
• 误区二：忽略时间同步。LiDAR、相机、IMU 时间戳未对齐会导致运动畸变和地图错层。务必使用硬件触发或 PTP 同步。
• 误区三：外参标定草率。雷达与 IMU 之间的旋转平移若不准，回环后会出现明显弯曲。建议用 Kalibr 或手动标定后验证。
• 误区四：地图越密越好。点云过密会拖慢规划与抓取。应按用途分层：导航用栅格，抓取用局部点云，语义用轻量标签。

复盘时可用一条判断链：传感器数据是否同步 → 外参是否准确 → 前端配准是否收敛 → 回环是否触发 → 地图是否满足下游需求。逐级排查，比盲目调参更高效。掌握这条链路后，后续的导航与抓取才有可靠的空间基础。