GNSS/INS松组合：从误差模型到卡尔曼滤波的工程实践

1. GNSS/INS松组合：为什么它是车载导航的黄金搭档？

第一次接触GNSS/INS松组合时，我完全被各种术语搞晕了。直到有一次在山区测试车载导航系统，GPS信号时断时续，但车辆位置显示却异常稳定，这才真正体会到这个技术的价值。简单来说，GNSS（全球导航卫星系统）和INS（惯性导航系统）就像一对互补的好搭档——一个擅长绝对定位但容易受环境影响，另一个自主性强却会累积误差。

在实际工程中，松组合方案特别受欢迎。相比紧组合，它实现起来更简单，两个系统相对独立，一个出问题不会拖累另一个。我经手过的车载项目里，90%都采用松组合方案。最典型的应用场景就是城市峡谷或隧道环境，GPS信号丢失后，依靠INS还能维持几十秒的高精度定位，这对自动驾驶系统来说简直是救命稻草。

2. 误差模型：从理论到实践的三个关键点

2.1 SINS误差模型构建实战

记得第一次推导误差模型时，我在白板上写满了微分方程。其实核心就是处理两类误差：IMU器件误差（陀螺仪和加速度计的零偏）和导航解算误差（位置、速度、姿态）。以陀螺仪为例，它的零偏不能简单当作常数，我用一阶马尔科夫过程建模更接近实际情况：

# 陀螺零偏模型示例 def gyro_bias_model(bias_prev, tau, dt, noise): return bias_prev * np.exp(-dt/tau) + noise

这里tau是相关时间常数，需要根据IMU型号实测确定。有个坑我踩过：不同批次的IMU器件参数可能差异很大，必须逐个校准。

2.2 GNSS误差特性处理技巧

GNSS的误差特性完全不同。在开阔环境下，它的位置误差可以看作是白噪声；但在多路径效应严重的城区，误差会呈现明显的时间相关性。我的经验是：在松组合中给GNSS测量噪声矩阵加个自适应因子，当卫星数少于5颗或HDOP大于2时，自动增大噪声方差。

2.3 状态变量选择的工程考量

状态变量的选择直接影响滤波效果。经典15状态模型包含：

• 3维位置误差
• 3维速度误差
• 3维姿态误差
• 3维陀螺零偏
• 3维加速度计零偏

但在车载场景下，我通常会简化：去掉垂直方向的速度和位置误差（车辆基本在平面运动），把省下的计算量用来增加陀螺零偏的状态维度，这样在急转弯时姿态更稳定。

3. 卡尔曼滤波实现：时间更新与量测更新的细节把控

3.1 模型离散化的三个陷阱

直接从连续模型离散化会引入误差。我常用的二阶龙格-库塔法比欧拉法精度高，计算量增加不多。特别注意状态转移矩阵Φ的计算，当角速度较大时（比如车辆快速过弯），必须考虑高阶项：

# 状态转移矩阵计算示例 def compute_phi(F, dt): # 二阶近似 return np.eye(F.shape[0]) + F*dt + 0.5*F@F*dt**2

有个容易忽略的点：离散化后的过程噪声Q矩阵需要同步调整，我见过好几个项目因为Q矩阵计算不当导致滤波器发散。

3.2 时间更新的五个优化点

1. 姿态误差处理：小角度假设在车辆动态下容易失效，我改用四元数误差模型后，姿态估计稳定性提升明显
2. 速度更新策略：在急加减速时，我会临时减小时间更新间隔
3. 零偏预测：对于低成本IMU，零偏预测模型要简化
4. 矩阵运算优化：利用对称性减少40%计算量
5. 数值稳定性：定期重置协方差矩阵防止负定

3.3 量测更新的四个实战技巧

当GNSS信号可用时：

1. 新息检测：设置合理的门限值，我常用5倍标准差
2. 自适应滤波：根据GNSS信号质量动态调整量测噪声
3. 杆臂补偿：实测发现10cm的杆臂误差在急转弯时会引入0.5度航向误差
4. 量测延迟处理：GNSS数据通常有100-200ms延迟，需要状态预测补偿

4. 状态量补偿：最容易被忽视的关键步骤

很多开源代码到这里就结束了，但真正的魔法在补偿环节。记得有次调试，滤波器看起来工作正常，但定位误差却越来越大，花了三天才发现是忘记状态量置零了。

具体操作分三步：

1. 将估计出的位置/速度/姿态误差补偿到SINS解算值
2. 立即将状态向量中的对应误差项清零
3. 调整协方差矩阵保持一致性

// 状态补偿示例（嵌入式C代码片段） void state_compensation(float *x, float *P) { sins.position -= x[0:2]; // 位置补偿 x[0:2] = 0.0; // 状态置零 P[0:2,0:2] -= P[0:2,0:2] * 0.1; // 协方差调整 }

在资源受限的嵌入式平台，我还会做定点数优化。有个经验值：状态量补偿的频率应该至少是GNSS更新率的2倍，这样能避免补偿滞后带来的抖动。

5. 调试与验证：从仿真到实车的完整流程

5.1 仿真环境搭建

我习惯用MATLAB/Python先做算法验证。必备工具链：

• IMU数据生成器（模拟各种运动场景）
• GNSS信号模拟器（加入多路径、遮挡等效应）
• 可视化工具（误差曲线、轨迹对比）

建议从简单场景开始：先验证静止状态，再试匀速直线，最后加转向运动。有个技巧：保存中间结果时带上时间戳，这样能回放定位误差的变化过程。

5.2 实车测试注意事项

第一次路测时我犯了个错误：没同步记录原始传感器数据。现在我的检查清单包括：

1. 硬件同步：PPS信号+数据时间戳双重校验
2. 数据备份：至少两个独立存储设备
3. 参考基准：RTK-GNSS+高精度惯导组合
4. 场景标记：通过CAN总线记录转向灯、刹车等车辆信号

5.3 性能评估指标

不要只看定位误差，我关注的五个维度：

1. 收敛速度：从大误差到稳定的时间
2. GNSS拒止表现：信号丢失后的误差增长斜率
3. 计算负载：单次滤波耗时占处理器资源的比例
4. 内存占用：特别是在ARM Cortex-M系列上的表现
5. 鲁棒性：对IMU初始对准误差的容忍度

6. 工程实践中的七个典型问题与解决方案

1. 滤波器发散：八成是过程噪声矩阵Q设置不当，先用仿真确定合理范围
2. GNSS更新时的跳跃：检查杆臂补偿是否正确，我常用CAD模型辅助测量
3. Z轴误差大：车载导航可以适当放松垂直通道约束
4. 初始化时间长：采用静态检测+粗对准组合策略
5. 急转弯时姿态漂移：增加陀螺零偏的状态维度
6. 计算资源不足：改用平方根滤波或降维处理
7. 冷启动问题：融合轮速计等辅助传感器

最近一个项目就遇到第6个问题：原算法在i.MX6上跑满CPU，通过改用UD分解滤波和固定点运算，最终将耗时从15ms降到了4ms。关键是要在精度和实时性之间找到平衡点。