4、传感器模型：针孔相机模型、LiDAR测距原理、IMU测量模型、各传感器误差来源
做多传感器融合，第一步不是调算法，而是搞懂每个传感器到底是怎么“看”世界的。说白了，你得知道它给你的数据是怎么来的，又带着什么“毛病”。
我见过不少新手，一上来就怼着卡尔曼滤波调参数，结果发现相机和LiDAR对同一个物体的观测差了半米——这根本不是算法能解决的，是模型没对齐。所以这一章，咱们把三个核心传感器的模型和误差来源彻底捋一遍。
4.1 针孔相机模型：从3D到2D的投影
相机成像，本质上是一个降维过程。3D世界里的一个点，怎么变成图像上的一个像素？
我个人习惯用针孔模型来理解，它简单、直观，而且90%的视觉SLAM问题都能用它搞定。
4.1.1 投影公式
假设世界坐标系下有一个点 P = [X, Y, Z]ᵀ。它先通过外参（旋转R和平移t）变换到相机坐标系，再通过内参投影到像素平面。
数学上就三步：
- 世界→相机：Pcam = R·P + t
- 相机→归一化平面：x = Xcam/Zcam, y = Ycam/Zcam
- 归一化→像素：u = fx·x + cx, v = fy·y + cy
这里fx, fy是焦距（单位是像素），cx, cy是光心偏移。嗯，这就是最经典的针孔模型。
关键点：Zcam在分母上。这意味着深度越远，同样的物理位移在图像上变化越小。这也是为什么远距离的定位精度会下降——你想想看，一个点从10米移到11米，在图像上可能只移动了几个像素。
4.1.2 畸变模型
现实中的镜头不是完美的针孔。尤其是广角镜头，会有明显的径向畸变和切向畸变。
我在项目中遇到过最坑的一次：用鱼眼相机做室内定位，没做畸变校正就直接跑特征匹配，结果地图点全歪了。后来花了整整两天排查，才发现是畸变参数没加载对。
畸变校正的公式长这样：
// 径向畸变
x_distorted = x * (1 + k1*r^2 + k2*r^4 + k3*r^6)
y_distorted = y * (1 + k1*r^2 + k2*r^4 + k3*r^6)
// 切向畸变
x_distorted += 2*p1*x*y + p2*(r^2 + 2*x^2)
y_distorted += p1*(r^2 + 2*y^2) + 2*p2*x*y其中r² = x² + y²，k1,k2,k3是径向系数，p1,p2是切向系数。一般标定板跑一遍OpenCV就能拿到这些参数。
我的习惯：拿到新相机，第一件事就是做内参标定。别偷懒，哪怕厂家给了参数，也自己跑一遍。因为镜头在运输、安装过程中可能已经发生了微小的形变。
4.2 LiDAR测距原理：TOF与三角法
LiDAR的核心任务就一个：测距。目前主流的有两种原理：TOF（飞行时间）和三角法。
4.2.1 TOF LiDAR
TOF的原理很简单：发射一束激光，打到物体上反射回来，记录时间差Δt。距离d = c·Δt/2，c是光速。
但实际工程中，没这么理想。我做过一个项目，用16线机械式LiDAR扫描室外场景，发现远距离的点云有周期性抖动。后来查资料才知道，那是多路径效应——激光在玻璃幕墙之间来回反射，导致测距值偏大。
TOF LiDAR的误差来源主要有：
- 时间测量精度：电子器件的计时分辨率有限，一般误差在±2cm左右
- 温度漂移：激光二极管的发射功率随温度变化，影响回波检测
- 多路径效应：激光在多个表面反射，导致测距值偏大
- 混叠效应：当目标距离超过最大测距范围时，会出现距离模糊
4.2.2 三角法LiDAR
三角法常用于低成本方案。它利用激光发射点和接收器之间的基线，通过三角几何计算距离。
说白了，就是已知发射点和接收点的位置，以及激光在接收器上的成像位置，反推目标距离。精度跟基线长度成正比，但体积也会变大。
三角法的误差主要来自：
- 基线标定误差：发射器和接收器的相对位置必须精确
- 像素分辨率：接收器上的成像点定位精度有限
- 表面反射特性：不同材质的反射率不同，影响成像点检测
避坑指南：我曾经在户外用三角法LiDAR做AGV导航，结果大太阳底下直接失效了。原因是环境光太强，淹没了激光信号。所以选型时一定要考虑使用场景的光照条件。
4.3 IMU测量模型：加速度计与陀螺仪
IMU是唯一一个不依赖外部信号的传感器。它自己就能测角速度和加速度。但代价是——它漂移得厉害。
4.3.1 加速度计模型
加速度计测量的是比力（specific force），也就是物体受到的合力减去重力。数学上：
a_measured = a_true - g + b_a + n_a其中：
- a_true：真实加速度
- g：重力加速度（约9.81 m/s²）
- b_a：零偏（bias），随时间缓慢变化
- n_a：白噪声
注意，IMU静止时，它测到的不是0，而是重力加速度的反方向。所以初始化时，我们通常用加速度计来估计重力方向，从而得到俯仰角和横滚角。
4.3.2 陀螺仪模型
陀螺仪测量角速度。模型类似：
ω_measured = ω_true + b_g + n_g陀螺仪的零偏b_g是IMU误差的主要来源。我做过一个实验：把IMU放在桌面上静止1小时，积分出来的角度漂移了30多度。这就是为什么纯IMU无法长时间独立定位。
4.3.3 IMU的误差来源
| 误差类型 | 来源 | 影响 |
|---|---|---|
| 零偏（Bias） | 制造工艺、温度变化 | 积分后位置漂移随时间平方增长 |
| 白噪声 | 电子热噪声 | 高频抖动，积分后变为随机游走 |
| 尺度因子误差 | 标定不准确 | 测量值与真实值成比例偏差 |
| 交轴耦合 | 三轴不完全正交 | 一个轴的运动会串扰到其他轴 |
| 温度漂移 | 芯片发热、环境温度变化 | 零偏和尺度因子随温度变化 |
我的经验：IMU的零偏是时变的，不能用一个固定值去补偿。我习惯在系统启动后做5-10秒的静态初始化，估计初始零偏，然后在运行过程中用滤波器在线估计零偏的变化。
4.4 各传感器误差来源总结
做外参标定之前，你得先知道每个传感器自己的“毛病”在哪。不然你标出来的外参，可能只是在补偿传感器自身的误差。
我画了一张图，帮你理清思路：
从这张图可以看得很清楚：每个传感器都有自己的“内伤”。相机有畸变，LiDAR有多路径，IMU有零偏。外参标定解决的是传感器之间的相对位姿问题，而不是传感器自身的标定问题。
我曾经踩过的坑：有一次做相机-LiDAR融合，发现投影误差始终降不下去。折腾了三天，最后发现是相机的内参标定板打印尺寸错了——内参本身就不准，外参怎么标都没用。所以我的建议是：先单独标定每个传感器，再做外参标定。顺序不能乱。
好了，这一章的内容就到这里。传感器模型是外参标定的地基，地基不稳，上层建筑再漂亮也是白搭。下一章我们会深入具体的标定方法，到时候这些误差模型都会用上。
一句话总结：搞懂传感器怎么“犯错”，才能知道外参标定该“纠正”什么。