📚 课程目录第2章姿态表示基础

2. 姿态表示基础

做传感器融合这些年，我最大的体会是：不懂传感器特性，融合就是空中楼阁。你想想看，如果连传感器怎么工作的、误差从哪来的都不清楚，后面那些卡尔曼滤波、粒子滤波再漂亮，也是白搭。

这一章，咱们就把常见的几种传感器——IMU、GPS、摄像头、激光雷达、毫米波雷达——挨个过一遍。我会重点讲它们的原理、特性，以及最关键的：误差从哪来，怎么建模。

核心观点：传感器融合的本质，是用多个有缺陷的观测，拼出一个更接近真实的状态。所以，理解每个传感器的"缺陷"（误差模型），比理解它的"优点"更重要。

2.1 IMU：惯性测量单元

IMU 这东西，说白了就是加速度计加陀螺仪。加速度计测比力，陀螺仪测角速度。我刚开始做无人机飞控的时候，觉得 IMU 数据挺准的，后来才发现——IMU 的误差会随时间累积，而且累积得很快。

2.1.1 工作原理

加速度计：测量物体受到的比力（重力 + 运动加速度）。MEMS 加速度计用梳齿电容结构，质量块位移导致电容变化。
陀螺仪：测量角速度。MEMS 陀螺仪基于科里奥利效应，振动质量块在旋转时产生横向位移。
磁力计（可选）：测量地磁场方向，辅助航向估计。

2.1.2 误差来源与建模

IMU 的误差模型，我习惯分成两大类：确定性误差和随机误差。

误差类型	来源	建模方式
零偏（Bias）	制造工艺、温度变化	常数 + 温度多项式 + 随机游走
标度因子误差	灵敏度不一致	比例系数 + 非线性项
交轴耦合	轴对齐偏差	3×3 耦合矩阵
白噪声	电子热噪声	高斯白噪声，功率谱密度已知
随机游走	零偏随时间漂移	一阶马尔可夫过程

我的经验：实际项目中，零偏和随机游走是最头疼的。我曾经在一个车载导航项目里，IMU 刚上电时零偏稳定，跑了半小时后漂了 0.5°/s——如果不建模，航向角直接废了。所以，零偏在线估计是 IMU 融合的必修课。

2.1.3 常用误差模型公式

IMU 的误差模型，我一般写成这样：

加速度计测量值 = 真实值 + 零偏 + 标度因子误差 + 交轴耦合 + 白噪声
陀螺仪测量值 = 真实值 + 零偏 + 标度因子误差 + 交轴耦合 + 白噪声

其中：
- 零偏 Bias 建模为：Bias(t) = Bias_const + Bias_drift(t)
- Bias_drift 用随机游走描述：Bias_drift(t+dt) = Bias_drift(t) + w * sqrt(dt)
- w 是驱动噪声，方差由 Allan 方差分析得到

注意：Allan 方差是分析 IMU 噪声特性的标准工具。它能帮你区分白噪声、随机游走、量化噪声等。我建议每个做融合的工程师，都学会看 Allan 方差曲线——它能告诉你 IMU 的"真实水平"。

2.2 GPS：全球定位系统

GPS 的优点很明显：误差不随时间累积。但它的缺点也很致命：更新频率低（通常 10Hz），容易受遮挡，城市峡谷里经常丢星。

2.2.1 工作原理

GPS 定位的原理，说白了就是三边测量。卫星发射信号，接收机测量信号传播时间，乘以光速得到距离。至少 4 颗卫星就能解出三维位置和时间。

2.2.2 误差来源

卫星钟差：卫星时钟与 GPS 时间不同步。广播星历会给出钟差参数。
电离层延迟：信号穿过电离层时速度变化。双频接收机可以消除大部分。
对流层延迟：大气折射导致。模型可以部分补偿。
多路径效应：信号反射导致测距误差。城市环境最严重。
接收机噪声：热噪声、量化噪声等。

2.2.3 误差建模

GPS 的误差模型，我通常用高斯分布近似：

位置测量值 = 真实位置 + 高斯噪声

其中：
- 水平方向：σ ≈ 3-5 米（单点定位），σ ≈ 0.1-0.5 米（RTK）
- 垂直方向：σ 约为水平的 1.5-2 倍
- 噪声协方差矩阵：R = diag(σ_x², σ_y², σ_z²)

关键点：GPS 的误差是有界的，不会像 IMU 那样发散。所以 GPS 常用来修正 IMU 的累积误差。但要注意——GPS 在遮挡环境下误差会突然变大，这时候需要自适应协方差调整。

2.3 摄像头：视觉传感器

摄像头是传感器融合里最"丰富"的传感器——它提供的信息量最大，但也是最难处理的。我做过一个视觉 SLAM 项目，摄像头在纹理丰富的场景里表现完美，一到白墙走廊就崩了。

2.3.1 工作原理

摄像头通过镜头将三维世界投影到二维图像平面。每个像素记录了光强（灰度）或颜色（RGB）。

2.3.2 误差来源

镜头畸变：径向畸变（桶形/枕形）和切向畸变。需要标定。
像素量化噪声：模拟信号转数字时的量化误差。
运动模糊：曝光时间内相机运动导致图像模糊。
光照变化：过曝、欠曝、阴影等。
特征提取误差：角点、特征点定位精度有限。

2.3.3 误差建模

摄像头误差模型，我主要关注投影误差：

像素坐标 = 投影(三维点) + 畸变 + 量化噪声

其中：
- 投影模型：针孔模型或鱼眼模型
- 畸变模型：Brown 模型或 Kannala-Brandt 模型
- 量化噪声：均匀分布，范围 ±0.5 像素

我的建议：摄像头标定是基本功。我见过太多项目因为标定不准导致融合失败。记住：标定板要打印平整，拍摄角度要覆盖全视场。我曾经用 A4 纸打印标定板，结果纸是弯的，标定出来的畸变参数全是错的——后来换了亚克力板才搞定。

2.4 激光雷达：LiDAR

激光雷达是自动驾驶的"宠儿"。它直接输出三维点云，精度高，不受光照影响。但价格贵，而且在下雨、雾天性能会下降。

2.4.1 工作原理

激光雷达发射激光脉冲，测量反射回来的时间（ToF）或频率变化（FMCW），从而计算距离。通过旋转或扫描，得到周围环境的点云。

2.4.2 误差来源

测距噪声：时间测量精度有限。通常 ±1-3 cm。
角度噪声：扫描机构精度有限。通常 ±0.1°。
多回波干扰：激光打到玻璃、水面等产生多个回波。
运动畸变：扫描过程中传感器运动导致点云变形。
混叠：远距离目标回波延迟超过脉冲周期。

2.4.3 误差建模

点云测量值 = 真实位置 + 测距噪声 + 角度噪声

其中：
- 测距噪声：高斯分布，σ_r ≈ 0.01-0.03 m
- 角度噪声：高斯分布，σ_θ ≈ 0.1°-0.5°
- 在极坐标系下建模更方便：
  - 距离：r_meas = r_true + n_r
  - 方位角：θ_meas = θ_true + n_θ
  - 俯仰角：φ_meas = φ_true + n_φ

注意：激光雷达在雨雾天气下，测距噪声会显著增大。我做过一个测试，大雨天 16 线激光雷达的有效探测距离从 100 米降到了 30 米。这时候如果还用原来的噪声模型，融合结果会严重偏离真实值。

2.5 毫米波雷达

毫米波雷达是"低调的实力派"。它不如激光雷达精度高，但全天候能力极强——雨、雾、雪、灰尘都不怕。而且能直接测速度（多普勒效应）。

2.5.1 工作原理

毫米波雷达发射毫米波段的电磁波（24GHz、77GHz 等），通过测量回波的时延（距离）、频率变化（速度）和相位差（角度）来感知目标。

2.5.2 误差来源

距离分辨率：受带宽限制。77GHz 雷达带宽 4GHz，距离分辨率约 3.75 cm。
多普勒模糊：速度测量存在模糊，需要解模糊。
角度分辨率：受天线孔径限制。通常 10°-20°。
杂波：地面、建筑物等产生的虚假回波。
多径：信号反射导致虚假目标。

2.5.3 误差建模

雷达测量值 = [距离, 速度, 角度] + 噪声

其中：
- 距离噪声：σ_r ≈ 0.1-0.5 m（取决于信噪比）
- 速度噪声：σ_v ≈ 0.1-0.5 m/s
- 角度噪声：σ_θ ≈ 1°-5°（取决于天线数量）
- 检测概率：P_d = f(SNR)，低 SNR 时容易漏检

融合中的角色：毫米波雷达在传感器融合里，通常负责目标检测和跟踪。它的速度测量很准，可以用来辅助 IMU 的速度估计。但角度分辨率低，所以常和摄像头配合——摄像头提供角度精度，雷达提供距离和速度精度。

2.6 传感器特性对比

做融合的时候，我习惯先列个表，看看各个传感器的优缺点：

传感器	优点	缺点	典型更新率	典型误差
IMU	高频、不受环境干扰	误差累积、需要初始化	100-1000 Hz	零漂 0.01-1°/s
GPS	误差有界、全球覆盖	低频、易遮挡	1-20 Hz	3-5 m（单点）
摄像头	信息丰富、成本低	受光照影响、计算量大	10-60 Hz	1-3 像素
激光雷达	精度高、3D 点云	成本高、受天气影响	5-20 Hz	1-3 cm
毫米波雷达	全天候、直接测速	角度分辨率低	10-50 Hz	0.1-0.5 m

我的融合策略：IMU 做高频预测，GPS 做低频修正，摄像头做语义理解，激光雷达做精确建图，毫米波雷达做全天候目标检测。每个传感器都有自己的"主场"，融合就是让它们取长补短。

2.7 误差建模的通用框架

最后，我总结一个通用的误差建模思路。不管什么传感器，都可以用这个框架：

确定测量模型：测量值 = f(真实状态) + 误差
分解误差来源：系统误差（可标定） + 随机误差（需建模）
标定系统误差：通过实验确定偏差、标度因子等
建模随机误差：用高斯分布、随机游走、马尔可夫过程等
验证模型：用实际数据检验模型是否合理

嗯，这一章的内容就到这里。传感器基础打牢了，后面讲融合算法的时候，你才能理解为什么有些方法有效，有些方法会失效。记住：误差模型是融合算法的"灵魂"。