解耦时空: GNSS+IMU融合定位算法核心逻辑
前言
高精度、高频率、全天候的定位是自动化系统的基础。单一传感器无法满足要求。GNSS+IMU组合是当前的主流技术方案。本文旨在梳理该方案的内部工作逻辑。
核心部件拆解
GNSS (+RTK): 全局坐标锚点
GNSS的核心功能是提供全局坐标系下的绝对位置，本质是一个”锚点”。
RTK (实时动态差分) 是一种增强技术，而非独立硬件。它通过地面基准站修正大气误差等，将GNSS定位精度从米级提升至厘米级。
GNSS的输出包括位置（经纬高）和位置不确定性（协方差）。RTK固定解时，不确定性极小；单点解则较大。
但它也有明显的局限：
低更新率是首要问题。GNSS通常为1-10Hz，无法描述连续运动。
环境敏感是另一个痛点。信号在城市峡谷、隧道、林下等环境会中断或产生多路径效应，导致定位失败或精度骤降。我在实际项目中见过不少因此导致的定位跳变。
IMU: 高频运动推算单元
IMU通过内部的加速度计和陀螺仪，以极高频率感知自身运动。
工作原理：航位推算 (Dead Reckoning)
陀螺仪测量角速度，积分后得到姿态。加速度计测量比力（运动加速度+重力加速度），积分两次后得到位移。
输出包括三轴角速度和三轴比力加速度。
然而IMU的局限同样明显：
相对定位意味着它只能推算”相对于上一刻的变化”，没有全局概念。
误差累积是最致命的问题。传感器自身的噪声和偏置（Bias）在积分过程中会被持续放大，导致结果随时间快速漂移。十几分钟不用GNSS校正，误差可能就漂出几十米。
融合算法的核心 - 预测与校正
融合算法的本质是一个预测-校正的循环，经典实现是卡尔曼滤波器 (Kalman Filter, KF) 及其变种（EKF, UKF）。
预测阶段 (IMU主导): 高频航位推算
在两次GNSS更新的间隙（例如0.1秒），IMU以高频（如100Hz）进行状态预测。
系统状态向量 x 通常包含：位置 p、速度 v、姿态 q（常用四元数表示）。
从 k-1 时刻到 k 时刻，时间间隔 Δt：
姿态更新 (Attitude Update)
输入是陀螺仪测量的角速度 ω_m。逻辑是将上一时刻的姿态，根据当前角速度旋转一小步。
公式: q_k = q_{k-1} ⊗ Δq(ω_m * Δt)，其中 ⊗ 为四元数乘法。
速度与位置更新 (Velocity & Position Update)
输入是加速度计测量的比力 a_m。
逻辑分为三步：
- 将设备坐标系下的加速度
a_m，利用刚更新的姿态q_k转换到全局导航坐标系a_nav。 - 在导航坐标系下减去重力加速度
g，得到物体真实的运动加速度a_true。 - 对
a_true进行积分，更新速度和位置。
公式:
a_nav = R(q_k) * a_m
a_true = a_nav - g
v_k = v_{k-1} + a_true * Δt
p_k = p_{k-1} + v_{k-1} * Δt + 0.5 * a_true * Δt²
此阶段保证了输出的高频率和平滑性，但误差会随时间累积。
校正阶段 (GNSS主导): 卡尔曼滤波更新
当GNSS数据到达时，执行校正。
输入包括GNSS测量值 z（即GNSS提供的位置 p_gnss）和测量不确定性 R（GNSS对其输出结果的置信度）。
核心步骤如下：
计算残差 y: y = p_gnss - p_predicted
即GNSS的”答案”与IMU预测的”答案”之间的差距。
计算卡尔曼增益 K
K 是一个动态计算的权重，用于决定在多大程度上相信GNSS的修正。它由IMU预测的不确定性 P 和GNSS测量的不确定性 R 共同决定。
逻辑很简单：若GNSS准（R小），IMU已漂移（P大），则 K 值大，大力度相信GNSS；反之则 K 值小，小幅度修正。
更新状态 x: x_updated = x_predicted + K * y
用残差和权重来修正IMU的预测值。这个修正不仅作用于位置，还会通过协方差矩阵的传递，间接修正速度、姿态甚至IMU的偏置误差。这是GNSS能”校准”IMU的关键。
更新不确定性 P: 经过一次准确的外部校正，系统整体的不确定性降低。
现代融合方案的演进
松耦合 vs. 紧耦合
松耦合 (Loosely Coupled) 方案中，GNSS模块独立解算出位置，再与IMU的推算结果在滤波器层面融合。这种方式简单，但GNSS可用卫星数不足（<4颗）时，整个系统会失效。
紧耦合 (Tightly Coupled) 方案则将GNSS原始观测量（伪距、载波相位）直接送入融合滤波器，与IMU数据共同解算。即使可用卫星数不足以独立定位，这些原始信息仍能有效约束IMU的漂移。这是当前高性能系统的主流方案，鲁棒性更强。我在几个自动驾驶项目中都采用了这种架构，效果确实比松耦合稳定很多。
滤波 vs. 优化
滤波 (Filtering) 以卡尔曼滤波为代表。它只利用到当前时刻为止的测量值来估计当前状态，是”向前”的算法，无法利用未来的信息修正过去。优点是计算量小，实时性好。
优化 (Optimization) 以因子图优化 (Factor Graph Optimization) 为代表。它将一段时间内的所有状态和测量（IMU、GNSS）构建成一个图模型，通过非线性优化的方法，寻找一条最优的、最符合所有测量数据的状态轨迹。
优化的优势在于精度更高，因为它能利用全局信息进行平滑。目前在自动驾驶的SLAM（同步定位与建图）和高精度后处理测绘中广泛应用。
AI/ML的介入
IMU误差建模方面，传统方法使用简单的随机游走模型描述IMU误差。研究表明，利用深度学习网络（如LSTM）可以学习更复杂的IMU误差模型，从而在GNSS中断期间实现更精准的推算。这块我还在跟进，感觉落地还需要时间。
GNSS信号质量甄别是另一个应用方向。利用机器学习分类器，可以识别和剔除由多路径效应等引起的劣质GNSS测量值，避免其”污染”融合结果，提升城市环境下的定位鲁棒性。
结论
GNSS-RTK-IMU融合定位是一个系统工程。它利用IMU的高频特性弥补GNSS的低频缺陷，利用GNSS的绝对精度锚点来校正IMU的累积误差。其算法核心是预测-校正框架。
现代方案通过紧耦合、图优化和AI/ML等技术，正不断将该系统的精度、可用性和鲁棒性推向新的高度。这套技术栈我已经用了好几年，虽然细节一直在演进，但核心的预测-校正逻辑始终没变——经典之所以经典，就是因为它抓住了问题的本质。