第1章 车载传感器融合：GPS、IMU、轮速传感器、融合定位算法
各位同学，今天我们来聊聊车载传感器融合。说实话，这是整个车载定位系统里最核心、也最让人头疼的部分。我当年刚入行时，以为定位就是GPS加个地图，结果第一次路测就翻车了——车进隧道后定位直接漂到隔壁省去了。嗯，从那以后我才真正开始研究传感器融合。
1.1 为什么需要传感器融合？
先问大家一个问题：单靠GPS能搞定车载定位吗？答案是不能。GPS在城市峡谷、隧道、地下停车场这些场景下，信号会严重衰减甚至完全丢失。你想想看，如果自动驾驶系统在隧道里突然不知道自己在哪了，那后果是什么？
所以我们需要多个传感器互相补充。每个传感器都有自己的短板，但组合起来就能形成完整的定位能力。我个人习惯把传感器融合比作「团队协作」——GPS负责全局定位，IMU负责短时高精度推算，轮速传感器负责里程约束，三者配合才能稳定输出。
核心原则：传感器融合不是简单取平均值，而是利用各传感器的优势互补，通过算法估计出最可靠的位姿。
1.2 三大传感器的特性分析
我们先逐个看看这三个传感器的脾气秉性。了解它们的优缺点，你才能知道融合时该信任谁。
1.2.1 GPS（全球定位系统）
GPS大家都很熟悉了。它提供绝对位置信息，误差通常在2-10米（民用级）。但它的致命问题是更新频率低——一般只有10Hz，而且容易受遮挡影响。
我在项目中遇到过最典型的情况：车辆在高架桥下行驶，GPS信号被桥体遮挡，定位点开始随机跳动。这时候如果你还完全信任GPS，那车辆控制就会跟着乱抖。
| 特性 | 说明 |
|---|---|
| 优点 | 无累积误差，提供绝对坐标 |
| 缺点 | 更新率低（10Hz），易受遮挡，多路径效应 |
| 典型场景 | 开阔道路、高速公路 |
1.2.2 IMU（惯性测量单元）
IMU包含加速度计和陀螺仪。它不依赖外部信号，完全靠自己推算运动。更新频率很高，通常能达到100-200Hz。但它的致命伤是——误差会随时间累积。
说白了，IMU就像一个记性不太好的人。它记得你刚才走了几步，但走着走着就开始记错。你让它走100米，它可能告诉你走了105米。这个误差会越来越大，这就是所谓的「漂移」。
注意：IMU的零偏和温漂是工程中的大坑。我曾经在夏天路测时，IMU因为温度升高，零偏直接翻了一倍，导致定位结果严重偏离。后来我们加上了温度补偿模型才解决。
1.2.3 轮速传感器
轮速传感器测量车轮的转速，可以推算出车辆的速度和行驶距离。它比IMU更稳定，但只能提供一维的速度信息，而且受轮胎打滑影响很大。
我记得有一次在冰雪路面测试，轮速传感器显示车速是30km/h，但实际车速只有20km/h——轮胎一直在打滑。这时候如果你直接用轮速去推算位置，那误差就大了去了。
| 传感器 | 优势 | 劣势 | 典型应用 |
|---|---|---|---|
| GPS | 无累积误差 | 更新慢、易遮挡 | 全局定位修正 |
| IMU | 高频、自主 | 累积漂移 | 短时高精度推算 |
| 轮速 | 稳定、低成本 | 打滑影响、一维 | 里程约束 |
1.3 融合定位算法核心逻辑
好了，了解了每个传感器的特性，我们来看看怎么把它们融合起来。最常用的方法是卡尔曼滤波（Kalman Filter），尤其是扩展卡尔曼滤波（EKF）。
为什么用卡尔曼滤波？因为它能很好地处理「预测+观测」的问题。IMU和轮速负责预测（推算下一时刻的位置），GPS负责观测（修正累积误差）。两者结合，既保证了高频输出，又避免了长期漂移。
小技巧：在实际工程中，我建议把轮速传感器作为速度约束加入状态方程，而不是直接作为观测。这样能更好地处理打滑场景。
下面我画了一张融合定位的流程图，帮你理清整体逻辑：
1.4 代码实战：EKF融合定位
光说不练假把式。下面我给出一个简化版的EKF融合定位代码。这个代码我在实际项目里用过类似的框架，当然真实场景要复杂得多，但核心逻辑是一样的。
// 简化版EKF融合定位（C++伪代码）
class EKFFusion {
private:
// 状态向量: [x, y, yaw, vx, vy]
Eigen::VectorXd state_;
// 协方差矩阵
Eigen::MatrixXd covariance_;
public:
void Predict(const IMUData& imu, const WheelData& wheel, double dt) {
// 1. 使用IMU角速度更新航向
state_[2] += imu.gyro_z * dt; // yaw更新
// 2. 使用轮速更新速度
double speed = (wheel.left_speed + wheel.right_speed) / 2.0;
state_[3] = speed * cos(state_[2]); // vx
state_[4] = speed * sin(state_[2]); // vy
// 3. 更新位置
state_[0] += state_[3] * dt; // x
state_[1] += state_[4] * dt; // y
// 4. 更新协方差（简化版）
covariance_ += Q_; // Q为过程噪声
}
void Update(const GPSData& gps) {
// 1. 计算观测残差
Eigen::Vector2d z; // GPS观测 [x_gps, y_gps]
z << gps.x, gps.y;
Eigen::Vector2d h; // 预测观测
h << state_[0], state_[1];
Eigen::Vector2d y = z - h; // 残差
// 2. 计算卡尔曼增益
Eigen::Matrix2d H; // 观测矩阵
H << 1, 0, 0, 0,
0, 1, 0, 0;
Eigen::Matrix2d S = H * covariance_ * H.transpose() + R_;
Eigen::MatrixXd K = covariance_ * H.transpose() * S.inverse();
// 3. 更新状态
state_ += K * y;
// 4. 更新协方差
covariance_ = (Eigen::MatrixXd::Identity(5,5) - K * H) * covariance_;
}
};经验之谈：代码里的Q和R矩阵（过程噪声和观测噪声）是调参的关键。我建议你先在仿真环境里调好参数，再上实车。实车调试时，重点关注GPS信号质量——当GPS信号差时，适当增大R矩阵，减少对GPS的信任。
1.5 工程中的避坑指南
最后，我分享几个实际项目中踩过的坑，希望能帮你少走弯路。
- 时间同步问题：GPS、IMU、轮速传感器的时间戳必须对齐。我曾经因为IMU和GPS的时间差没处理好，导致融合结果出现周期性抖动。解决办法是用硬件同步信号或者软件插值对齐。
- 坐标系统一：GPS输出的是经纬度（WGS84），IMU输出的是载体坐标系，轮速是车辆坐标系。融合前必须统一到同一个坐标系下。我习惯用UTM坐标系作为中间桥梁。
- 异常值处理：GPS偶尔会出现跳变（比如多路径效应）。一定要加异常检测逻辑，比如卡方检验，把异常观测剔除掉，否则融合结果会被带偏。
- 初始化问题：EKF需要初始状态和协方差。如果初始位置不准，后面很难收敛。我建议车辆静止时先做初始化，利用静止时的IMU数据估算零偏。
特别提醒：千万不要在车辆运动过程中初始化EKF！我见过有人这么干，结果状态估计发散得一塌糊涂。静止初始化虽然慢一点，但稳啊。
好了，关于传感器融合的基础知识就讲到这里。记住一句话：没有完美的传感器，只有完美的融合算法。下一节我们会深入讨论卡尔曼滤波的数学推导，到时候我会用更直观的方式给大家讲明白。
公众号:蓝海资料掘金营，微信deep3321