第四章 传感器与状态估计：IMU、轮速传感器、GPS/INS组合导航、卡尔曼滤波基础
各位同学，欢迎来到第四章。这一章我们聊聊底盘控制的眼睛和大脑——传感器与状态估计。
说实话，我在刚入行那会儿，总觉得算法是核心，传感器嘛，买好的就行了。后来被现实狠狠教育了一顿——再牛的算法，喂进去的是垃圾数据，吐出来的只能是更垃圾的结果。嗯，传感器融合这件事，说白了就是让车知道自己到底在哪儿、跑得多快、姿态怎么样。
4.1 IMU：惯性测量单元
IMU，全称Inertial Measurement Unit，一般包含三轴加速度计和三轴陀螺仪。有些高级货还会带磁力计，但咱们做底盘控制的，主要用前两个。
加速度计测量的是比力，也就是物体受到的惯性力与重力的合力。你想想看，车停在平地上，加速度计读出来的是(0, 0, -9.8) m/s²——它其实测的是重力在Z轴的分量。
陀螺仪测量的是角速度，单位是rad/s或者°/s。它能告诉你车在横摆、俯仰、侧倾三个方向上的转动快慢。
关键点：IMU的原始数据不能直接用。加速度计有零偏，陀螺仪有漂移。我见过一个项目，陀螺仪零偏没校准，积分10秒后横摆角误差就超过了5度——这要是做ESP，车早就转圈了。
IMU的坐标系定义，行业内一般遵循右手定则：
- X轴：车辆前进方向
- Y轴：车辆左侧方向
- Z轴：车辆上方方向
绕X轴的转动叫侧倾角(Roll)，绕Y轴的叫俯仰角(Pitch)，绕Z轴的叫横摆角(Yaw)。
4.2 轮速传感器
轮速传感器，底盘控制的老朋友了。ABS、TCS、ESC，哪个离得开它？
目前主流的是霍尔式和磁阻式。霍尔式便宜但低速性能差，磁阻式精度高但贵一点。我个人习惯在量产项目中用磁阻式，尤其是做低速泊车辅助的时候，霍尔式在3km/h以下基本没法看。
轮速传感器能直接给出的是每个车轮的转速，单位一般是rpm或者脉冲数/秒。但我们要的是车速和轮加速度，这就涉及到一些处理技巧：
- 车速估计：非驱动轮的速度更接近真实车速。前驱车就看后轮，后驱车就看前轮。当然，制动时所有轮子都会滑移，这时候需要结合IMU来修正。
- 轮加速度：直接对轮速差分会放大噪声。我建议用一阶低通滤波，截止频率设在10-20Hz左右，既能保留动态响应，又能滤掉齿圈振动带来的毛刺。
避坑指南：我曾经在一个项目中，轮速传感器信号线上有共模干扰，导致ABS在干燥路面上误触发。排查了三天，最后发现是线束屏蔽层接地不良。所以，传感器信号处理，硬件和软件都要看，别只盯着算法。
4.3 GPS/INS组合导航
GPS精度高但更新慢（10Hz左右），而且容易丢星。INS（惯性导航）更新快（100Hz以上）但误差会累积。两者组合，取长补短，这就是组合导航的基本思路。
组合导航的模式主要有三种：
| 模式 | 原理 | 适用场景 |
|---|---|---|
| 松耦合 | GPS和INS各自独立解算，然后用GPS结果修正INS | 城市道路，GPS信号较好 |
| 紧耦合 | GPS的原始观测值（伪距、载波相位）直接参与INS滤波 | 隧道、高架桥下等弱信号场景 |
| 深耦合 | INS辅助GPS跟踪环路，抗干扰能力最强 | 军事、特种车辆 |
做底盘控制，松耦合基本够用。但如果你要做L3以上的自动驾驶，紧耦合是底线。为什么？你想想看，车在隧道里丢了GPS，松耦合的INS误差会迅速发散，等出隧道再重新收敛，黄花菜都凉了。
4.4 卡尔曼滤波基础
终于到了卡尔曼滤波。说实话，这是状态估计的基石，也是很多同学觉得最难啃的骨头。别怕，咱们一步步来。
卡尔曼滤波解决的是这样一个问题：我有两个信息来源，一个是模型预测（比如根据上一时刻车速和加速度，预测这一时刻车速），另一个是传感器测量（比如轮速传感器测到的车速）。两个都有误差，怎么融合才能得到最准的结果？
核心公式就五个，我习惯分成两步：
预测步（时间更新）：
x_pred = A * x_prev + B * u // 状态预测
P_pred = A * P_prev * A^T + Q // 误差协方差预测更新步（测量更新）：
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1) // 卡尔曼增益
x_est = x_pred + K * (z - H * x_pred) // 状态估计
P_est = (I - K * H) * P_pred // 误差协方差更新看着公式多，其实逻辑很简单：
- 先根据模型猜一个值（预测）
- 再根据传感器测一个值（测量）
- 然后算一个权重K，谁更准就信谁多一点
- 最后得到融合后的最优估计
调参经验：Q是过程噪声协方差，R是测量噪声协方差。Q越大，说明模型越不可信，滤波会更依赖测量值；R越大，说明传感器越不可信，滤波会更依赖模型预测。我刚开始调的时候，总想把Q和R调得特别精确，后来发现，工程上大致合理就行，关键是看滤波后的残差是否白噪声化。
举个实际例子，用卡尔曼滤波估计车速：
// 状态量：车速 v
// 观测量：轮速传感器测得的车速 v_wheel
// 模型：匀加速模型，加速度由IMU提供
// 初始化
float x = 0.0; // 初始车速
float P = 1.0; // 初始误差协方差
float Q = 0.1; // 过程噪声
float R = 0.5; // 测量噪声
// 每个时间步执行
void KalmanFilter(float acc_imu, float v_wheel, float dt) {
// 预测
float x_pred = x + acc_imu * dt;
float P_pred = P + Q;
// 更新
float K = P_pred / (P_pred + R);
x = x_pred + K * (v_wheel - x_pred);
P = (1 - K) * P_pred;
}这段代码虽然简单，但已经能应付大部分车速估计场景了。当然，实际项目中还要考虑加速度零偏、轮速滑移等因素，需要扩展状态量。
注意：卡尔曼滤波假设噪声是高斯白噪声。如果实际噪声不满足这个条件（比如GPS多路径效应导致的非高斯噪声），滤波效果会变差。这时候可以考虑扩展卡尔曼滤波(EKF)或无迹卡尔曼滤波(UKF)。
4.5 传感器融合实战要点
最后，分享几个我在项目中踩过的坑：
- 时间同步：IMU、轮速、GPS的数据频率不同，到达时间也不同。不做时间同步，融合出来的结果就是错的。我一般用硬件触发或者软件插值来解决。
- 坐标系对齐：IMU的安装角度和车辆坐标系不一定完全重合。需要做标定，把IMU数据旋转到车体坐标系下。差个几度，长期积分下来误差就大了。
- 传感器失效检测：任何传感器都可能坏。卡尔曼滤波里要加一个检测逻辑，当残差超过3倍标准差时，认为传感器失效，暂时不用它的数据。
- 初始化：GPS刚启动时精度很差，IMU需要预热。我习惯等GPS收敛到10米以内，IMU零偏稳定后，再开始融合。
好了，这一章的内容就到这里。传感器与状态估计，说白了就是让车知道自己在哪里、状态怎么样。下一章我们会聊执行器与控制分配，看看怎么把估计出来的状态变成实实在在的刹车和转向指令。
记住一句话：好的状态估计，是底盘控制的地基。地基不稳，楼盖得再高也是白搭。