第2章 · 传感器模型与数据处理
做SLAM这么多年，我最大的体会就是：传感器数据质量决定了系统上限。算法再花哨，数据一塌糊涂，最后也是一团浆糊。这一章咱们就把激光雷达、IMU、轮式里程计这三个核心传感器的模型和数据处理讲透。嗯，都是我在项目里踩过坑、填过土的经验。
1. 激光雷达工作原理与数学模型
激光雷达，说白了就是通过发射激光束并测量往返时间来测距。我最早接触的是2D单线雷达，后来做3D SLAM才跟多线雷达打交道。它的核心模型其实不复杂：
- TOF 原理：\( d = c \cdot \Delta t / 2 \)，光速乘时间差除以2。精度受时钟分辨率限制。
- 三角测距：利用激光发射点、目标、接收器之间的几何关系。近距离精度高，远了就飘。
- 波束模型：实际测量值 = 真实距离 + 高斯噪声 + 偶然误差（比如边缘反射）。
我个人的习惯：拿到一个新雷达，先做静态噪声分析。把雷达对着白墙采100帧，算一下标准差。如果超过2cm，就要小心了。我曾经在某个项目里被廉价雷达的跳变坑惨了，后来加了离群点滤波器才稳住。
ROS2里常用 sensor_msgs::msg::LaserScan 或 PointCloud2。对于2D雷达，我一般直接转成极坐标数组，方便做scan matching。
2. IMU预积分理论
IMU预积分，这名字听着唬人，其实核心就一句话：在帧间对加速度和角速度积分，避免重复积分。你想想看，如果每次优化都要从零开始积分，计算量就炸了。
预积分把两个关键帧之间的IMU测量值打包成一个相对运动约束。公式长这样：
ΔR_ij = ∏ exp((ω_k - b_g) · Δt)
Δv_ij = Σ R_k · (a_k - b_a) · Δt
Δp_ij = Σ [Δv_k · Δt + 0.5·R_k·(a_k - b_a)·Δt²]这里 \( b_g, b_a \) 是零偏，需要在线估计。我在实际项目里发现，预积分对低端IMU尤其友好——只要零偏跟踪得好，即使加速度计噪声大，位姿估计依然稳。
我曾经在AGV上用过一颗工业级IMU，预积分后的相对位姿误差能控制在0.5%以内。但要注意，预积分对时间戳精度很敏感，ROS2里最好用 rclcpp::Time 做差值，别用系统时钟。
3. 轮式里程计模型
轮式里程计是SLAM里最朴素的传感器，但千万别小看它。两轮差速模型是基础：
- 差分驱动：\( v = (v_r + v_l)/2 \)，\( \omega = (v_r - v_l)/L \)（L为轮距）。
- 阿克曼模型：前轮转向，后轮驱动，需要转向角约束。
- 全向轮/麦克纳姆轮：三个自由度，解算稍微复杂些。
我习惯在ROS2里把轮式里程计封装成 nav_msgs::msg::Odometry，协方差矩阵根据轮子打滑程度动态调整。你想想看，如果地面是瓷砖，打滑严重，协方差就得放大。
注意：轮式里程计有累积误差，长时间运行必须融合其他传感器。我曾经见过一个纯轮式里程计的AGV，跑了200米后偏了3米……嗯，后来加了激光雷达修正才解决。
4. 传感器标定与时间同步
标定和时间同步，是SLAM工程里最容易被忽视、但最致命的部分。我见过太多团队算法调得飞起，最后发现是标定板没放平。
标定主要分三类：
| 标定类型 | 方法 | 我的经验 |
|---|---|---|
| 激光雷达内参 | 工厂标定 / 自标定（如VLP-16） | 建议每半年重标一次，温度变化会影响 |
| 激光-IMU外参 | 手眼标定 (hand-eye calibration) | 用Kalibr工具，注意运动激励要足够 |
| 轮式里程计标定 | 直线+旋转实验 | 我一般先测轮距，再微调左右轮比例 |
时间同步：ROS2里用 TimeSynchronizer 或 message_filters 做近似同步。但硬件触发才是王道。我曾在工控机上用PTP（精确时间协议）把激光雷达和IMU的时钟同步到微秒级，效果立竿见影。
一句话总结：标定误差1°，SLAM轨迹可能偏5%。时间不同步10ms，融合直接崩。所以，别偷懒。
好了，这一章的内容就是这些。传感器模型是SLAM的基石，理解透了后面算法才扎实。