📚 课程目录第4章传感器模型

4. 传感器模型：针孔相机、IMU、GPS与激光雷达

传感器融合这事儿，说白了就是让不同传感器互相补台。但补台之前，你得先搞清楚每个传感器的“脾气”——也就是它的数学模型。我做了这么多年机器人感知，见过太多融合方案翻车，根源往往不是算法不行，而是传感器模型没建对。

这一章，咱们把四种最常用的传感器模型掰开揉碎讲清楚：针孔相机、IMU、GPS、激光雷达。嗯，每个模型我都会结合项目里踩过的坑来说。

4.1 针孔相机模型

相机模型是视觉SLAM和三维重建的基石。最经典的，就是针孔模型。

核心思想：三维空间点通过光心投影到成像平面上。说白了，就是小孔成像的数学表达。

数学上，一个三维点 P = (X, Y, Z) 投影到像素坐标 p = (u, v) 的过程分三步：

世界坐标系 → 相机坐标系：通过外参矩阵 [R|t] 变换。
相机坐标系 → 归一化平面：除以 Z 得到归一化坐标 (x, y)。
归一化平面 → 像素坐标系：通过内参矩阵 K 映射。

内参矩阵 K 长这样：

K = [fx  0  cx]
    [ 0  fy  cy]
    [ 0   0   1]

其中 fx, fy 是焦距（像素单位），cx, cy 是光心偏移。我当年刚入门时，总搞不清 fx 和 fy 为什么不一样——后来才知道，这是传感器像素不是正方形导致的。

避坑指南：我曾经在一个项目里直接用厂家标称的内参，结果三维重建误差大得离谱。后来重新用棋盘格标定了一遍，才发现出厂数据和实际使用环境差了 3% 左右。记住：内参一定要现场标定，别偷懒。

实际相机还有畸变——主要是径向畸变（桶形/枕形）和切向畸变。畸变模型一般用多项式拟合：

x_distorted = x * (1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p1*x*y + p2*(r^2 + 2*x^2)
y_distorted = y * (1 + k1*r^2 + k2*r^4 + k3*r^6) + p1*(r^2 + 2*y^2) + 2*p2*x*y

k1, k2, k3 是径向畸变系数，p1, p2 是切向畸变系数。你想想看，如果不做畸变校正，直接用原始图像做特征匹配，那误差会累积到后面的位姿估计里，越跑越偏。

4.2 IMU 测量模型

IMU（惯性测量单元）提供加速度和角速度。它不像相机那样“看”外部环境，而是测量自身的运动。但IMU有个毛病——漂移。

IMU的测量模型可以写成：

a_m = a_true + b_a + n_a + g
ω_m = ω_true + b_g + n_g

其中：

a_m, ω_m：测量值
a_true, ω_true：真实值
b_a, b_g：零偏（bias），随时间缓慢变化
n_a, n_g：高斯白噪声
g：重力加速度（这个要特别注意，加速度计测的是“比力”，包含重力）

重要提醒：IMU的零偏不是常数！它会随温度、时间漂移。我做过一个无人机项目，起飞前IMU零偏标定得好好的，飞了10分钟后偏了0.5度/秒——积分出来的姿态直接歪了30度。所以，融合方案里一定要在线估计零偏。

IMU的积分模型（预积分）在VINS和ORB-SLAM3里用得很多。简单说，就是把两帧图像之间的IMU测量值积分起来，作为约束。我个人习惯用中值积分，精度比欧拉积分高，计算量又比四阶龙格-库塔小。

4.3 GPS 定位模型

GPS提供的是位置信息，但精度有限——民用GPS一般在2-5米，差分GPS（DGPS）可以到厘米级。

GPS的测量模型相对简单：

z_gps = p_true + n_gps

其中 p_true 是真实位置（通常是经纬高），n_gps 是测量噪声。但这里有个坑：GPS的噪声不是白噪声，它有时间相关性——也就是所谓的“多路径效应”和“电离层延迟”。

GPS类型	精度	更新频率	典型应用
民用GPS	2-5 m	1-10 Hz	手机导航、车载定位
DGPS	10-50 cm	1-10 Hz	农业机械、测绘
RTK-GPS	1-5 cm	5-20 Hz	自动驾驶、无人机精准起降

在传感器融合中，GPS通常作为“低频校正”。比如在VIO（视觉惯性里程计）里，GPS每1秒给一个位置约束，把IMU积分累积的漂移拉回来。我曾经在一个园区自动驾驶项目里，只用VIO跑了3公里，位置误差到了20米——加上GPS后，误差直接降到1米以内。

经验之谈：GPS的坐标系转换是个容易出错的地方。经纬高（LLA）要转成地心地固（ECEF），再转成局部东北天（ENU）。我建议在融合框架里统一用ENU坐标系，这样位置和姿态的物理意义更直观。

4.4 激光雷达点云模型

激光雷达（LiDAR）通过发射激光束并测量返回时间，得到周围环境的点云。每个点包含三维坐标和反射强度。

激光雷达的测量模型：

p_lidar = R * p_world + t + n_lidar

其中 R 和 t 是激光雷达相对于世界坐标系的位姿，n_lidar 是测量噪声。但实际中，激光雷达的噪声模型比这复杂——它跟距离、入射角、目标材质都有关系。

我整理了几种主流激光雷达的参数对比：

参数	机械式（如HDL-64）	固态（如Livox Horizon）	MEMS（如Valeo SCALA）
线数	16-128	1（非重复扫描）	4-8
视场角	360°×30°	81.7°×25.1°	145°×5°
测距精度	±2 cm	±2 cm	±5 cm
点频	~1.3M pts/s	~240k pts/s	~50k pts/s

点云模型在融合中的关键问题是“数据关联”——也就是怎么把不同时刻、不同视角的点云对应起来。常用的方法有ICP（迭代最近点）和NDT（正态分布变换）。

注意：激光雷达点云存在“运动畸变”。如果雷达在运动过程中扫描，一帧点云里的点其实对应不同时刻的位姿。我做过一个高速移动机器人项目，没做运动补偿，结果建图全是拖影。解决办法是用IMU或轮式里程计给每个点插值位姿。

4.5 四种传感器模型对比

四种传感器各有长短，融合就是取长补短。下面这张表帮你快速对比：

传感器	测量内容	优点	缺点	典型融合角色
针孔相机	图像（纹理/特征）	信息丰富、成本低	受光照影响、无深度	视觉里程计、闭环检测
IMU	加速度/角速度	高频、不受环境干扰	漂移严重	短时间积分、运动预测
GPS	位置	无累积误差	低频、精度有限、有遮挡	全局校正
激光雷达	三维点云	精确深度、全天候	成本高、稀疏纹理	建图、定位、障碍物检测

4.6 知识体系总览

下面这张图把四种传感器模型的核心逻辑串起来了。你一看就明白：每个传感器输出什么、怎么建模、融合时怎么用。