3. 环境感知与传感器

移动机器人要在仓库、车间、医院等非结构化环境中完成自主取放与搬运，第一步不是规划路径，而是“看清自己在哪里、周围有什么”。环境感知系统由多类传感器共同构成：激光雷达提供高精度二维/三维几何轮廓，深度相机提供稠密彩色点云与近距离细节，IMU 感知姿态与角速度变化，轮式里程计提供连续但会漂移的位移估计。它们各自有盲区与误差特性，只有通过标定与融合，才能形成稳定、连续、可用的环境模型与位姿估计。

一、四类传感器的角色与互补关系

激光雷达（LiDAR）通过飞行时间测距，输出扫描平面或三维点云，测距精度可达厘米级，适合建图、定位与障碍物检测，但在玻璃、镜面、黑色吸光物体前容易丢点，且价格与体积随线数上升。深度相机（RGB-D 或 ToF）以结构光或飞行时间获取深度图，近距离（0.3～5 m）纹理丰富，适合识别托盘孔位、货架层板和抓取目标，但受强光、反光与远距离限制明显。IMU 输出三轴加速度与角速度，高频（100～1000 Hz）响应姿态变化，可短时抑制剧烈运动下的位姿跳变，但积分会累积漂移。轮式里程计由编码器推算位移，低频段平滑连续，却因轮径误差、打滑和地面不平产生长期漂移。

四者的关系可以概括为：激光雷达与深度相机负责“外部几何”，IMU 与里程计负责“自身运动”。前者提供绝对或半绝对观测，后者提供高频相对增量。融合的本质，是用高频但漂移的运动信息，去平滑低频但准确的几何观测。

二、标定：让多传感器“说同一种语言”

标定分为内参、外参与时间同步三类。内参描述单个传感器自身特性，如相机焦距与畸变系数、LiDAR 各线束角度偏差、IMU 零偏与尺度因子。外参描述传感器之间的相对位姿，例如相机光心到 LiDAR 坐标系的旋转平移矩阵、IMU 到车体中心的安装角。时间同步则保证不同频率的数据在同一时刻对齐，否则高速运动时融合结果会出现“重影”。

常用标定流程如下：

1. 相机内参：使用棋盘格或圆点靶标，采集 15～25 张不同角度图像，通过张正友标定法求解。
2. LiDAR-相机外参：在共同视野内放置标定板，提取 LiDAR 平面与图像角点，最小化重投影误差。
3. IMU-底盘外参：静止采集零偏，再通过旋转台或已知轨迹激励，估计安装角与尺度。
4. 时间同步：使用 PTP 或硬件触发，将各传感器时间戳统一到同一时钟源，残差控制在 1 ms 以内。

# 相机内参标定示例（OpenCV）
python calibrate_camera.py \
  --images ./calib_imgs \
  --pattern 9x6 \
  --square_size 0.025 \
  --output camera_intrinsics.yaml

# 标定结果关键字段
camera_matrix: [615.3, 0, 320.1;
                0, 614.8, 240.5;
                0, 0, 1]
dist_coeffs: [-0.281, 0.092, 0.001, -0.002, 0.0]

三、数据融合：从预测到更新的闭环

工程上最常用的融合框架是扩展卡尔曼滤波（EKF）。它以 IMU 和里程计作为预测输入，以 LiDAR 和深度相机的观测作为更新输入，输出连续位姿与速度。对于更高精度的建图与定位，可采用因子图优化（如 LIO-SAM、FAST-LIO），把 IMU 预积分、LiDAR 特征匹配、回环检测统一到一个优化问题中。

融合的关键参数包括：预测与观测的噪声协方差矩阵、外参初值、时间戳对齐阈值、点云降采样体素大小。噪声设置过小会导致滤波器过度信任某一传感器，出现抖动或发散；设置过大则响应迟钝。通常先用静止与匀速场景整定，再在转弯、爬坡、打滑场景中验证。

四、典型场景与常见误区

以仓储托盘搬运为例：机器人驶入货架通道时，LiDAR 扫描两侧货架立柱完成定位；接近托盘时，深度相机识别托盘孔位并输出相对位姿；IMU 在叉臂升降引起的车体俯仰变化中保持姿态连续；里程计在低速直线段提供平滑增量。四者融合后，机器人才能在托盘被部分遮挡、地面有反光的情况下仍稳定完成插入与举升。

复盘时建议记录三类指标：各传感器数据丢帧率与时间戳偏差、标定残差（重投影误差、点面距离）、融合后位姿在静止与回环处的漂移量。只有这些指标稳定，环境感知才能为后续的取放与搬运提供可靠基础。