4. IMU运动学模型：离散运动学方程与PVQ积分
各位同学，今天我们来聊聊IMU运动学模型的核心——离散运动学方程，以及位置、速度、姿态的递推更新，也就是业内常说的PVQ积分。
说实话，这部分内容我刚开始接触时也觉得有点绕。但后来在实际项目中吃过亏，才真正理解它的重要性。你想想看，IMU的数据是离散的，而我们需要的却是连续的运动估计。怎么把这两者桥接起来？这就是我们今天要解决的问题。
4.1 连续时间下的IMU运动学模型
先回顾一下连续时间下的模型。IMU测量的是加速度和角速度，但注意，它测量的是本体坐标系下的值。我们需要把这些测量值转换到世界坐标系下，才能进行积分。
我个人习惯把IMU的运动学模型写成下面这样：
// 连续时间下的IMU运动学方程
// p: 位置 (世界坐标系)
// v: 速度 (世界坐标系)
// q: 姿态 (四元数，世界到本体的旋转)
// a_m: 加速度计测量值 (本体坐标系)
// w_m: 陀螺仪测量值 (本体坐标系)
// g: 重力加速度 (世界坐标系)
p_dot = v
v_dot = R(q) * (a_m - b_a - n_a) + g
q_dot = 0.5 * q ⊗ (w_m - b_g - n_g)
这里有几个关键点：
- R(q) 是从本体到世界的旋转矩阵，由四元数q转换而来
- b_a, b_g 是加速度计和陀螺仪的偏置，需要在线估计
- n_a, n_g 是高斯白噪声
- ⊗ 表示四元数乘法
⚠️ 注意： 这里的加速度测量值包含了重力分量。很多新手会忘记减去重力，导致积分出来的位置飞上天。我曾经在一个项目中调试了整整两天，最后发现是重力补偿没做对。
4.2 离散化：从连续到离散
实际IMU数据是离散采样的，频率通常在100Hz到1000Hz之间。我们需要把上面的连续方程离散化。
常用的离散化方法有两种：
- 欧拉法：简单粗暴，但精度一般
- 中值法：精度更高，我推荐使用
嗯，这里要注意，欧拉法虽然简单，但在高动态场景下误差会累积得很快。我建议至少用中值法。
4.2.1 欧拉法离散
// 欧拉法离散化
// dt: 采样间隔
// 下标 k 表示当前时刻，k+1 表示下一时刻
p_{k+1} = p_k + v_k * dt + 0.5 * R(q_k) * (a_m_k - b_a) * dt^2 + 0.5 * g * dt^2
v_{k+1} = v_k + (R(q_k) * (a_m_k - b_a) + g) * dt
q_{k+1} = q_k ⊗ exp(0.5 * (w_m_k - b_g) * dt)
说白了，欧拉法就是用当前时刻的测量值来近似整个时间间隔内的运动。这在IMU频率足够高时勉强可用，但如果你做的是高精度定位，我建议别用。
4.2.2 中值法离散（推荐）
// 中值法离散化
// 使用 k 和 k+1 时刻测量值的平均值
// 角速度中值
w_mid = 0.5 * (w_m_k + w_m_{k+1}) - b_g
// 加速度中值
a_mid = 0.5 * (a_m_k + a_m_{k+1}) - b_a
// 姿态更新（先更新姿态，因为位置和速度需要用到）
q_{k+1} = q_k ⊗ exp(0.5 * w_mid * dt)
// 位置和速度更新
p_{k+1} = p_k + v_k * dt + 0.5 * R(q_mid) * a_mid * dt^2 + 0.5 * g * dt^2
v_{k+1} = v_k + (R(q_mid) * a_mid + g) * dt
中值法的好处是精度更高，因为用了两个时刻的平均值来近似。我在做自动驾驶的IMU预积分时，一直用的就是中值法。
💡 小技巧： 实际工程中，如果IMU频率很高（比如400Hz以上），欧拉法和中值法的差异不大。但如果频率低于200Hz，一定要用中值法。
4.3 PVQ积分的完整流程
PVQ积分，说白了就是位置(Position)、速度(Velocity)、姿态(Quaternion)的递推更新。整个过程可以用下面的流程图来概括：
整个PVQ积分的核心步骤可以总结为：
- 读取IMU测量值：获取当前时刻的加速度和角速度
- 偏置补偿：减去估计的偏置值
- 离散化积分：选择欧拉法或中值法进行递推
- 更新状态：得到新的位置、速度、姿态
4.4 实际工程中的注意事项
在实际项目中，PVQ积分有几个坑需要特别注意：
🔑 关键点：
- 四元数归一化：每次更新后一定要对四元数做归一化，否则数值误差会累积，导致姿态发散。我见过有人忘了这步，结果姿态越转越歪。
- 重力对齐：初始时刻需要准确估计重力方向，否则位置积分会漂移。
- 时间戳同步：IMU和激光雷达的时间戳必须严格对齐，否则融合效果会大打折扣。
⚠️ 避坑指南： 我曾经在一个项目中，IMU的采样频率是200Hz，激光雷达是10Hz。我直接用原始时间戳做积分，结果发现位置漂移得厉害。后来排查发现，是IMU和激光雷达的时钟不同步导致的。解决方案是用硬件同步信号或者做时间戳插值。
4.5 代码实现示例
下面是一个简单的PVQ积分实现，我用的是中值法：
// PVQ积分 - 中值法实现
// 输入: 上一时刻状态 (p_k, v_k, q_k), IMU测量值 (a_k, w_k, a_{k+1}, w_{k+1}), 偏置 (b_a, b_g), 时间间隔 dt
// 输出: 当前时刻状态 (p_{k+1}, v_{k+1}, q_{k+1})
void pvq_integration(
const Vector3d& p_k, const Vector3d& v_k, const Quaterniond& q_k,
const Vector3d& a_k, const Vector3d& w_k,
const Vector3d& a_{k+1}, const Vector3d& w_{k+1},
const Vector3d& b_a, const Vector3d& b_g, double dt,
Vector3d& p_{k+1}, Vector3d& v_{k+1}, Quaterniond& q_{k+1})
{
// 1. 偏置补偿
Vector3d a_mid = 0.5 * (a_k + a_{k+1}) - b_a;
Vector3d w_mid = 0.5 * (w_k + w_{k+1}) - b_g;
// 2. 姿态更新 (使用中值角速度)
double theta = w_mid.norm() * dt;
Vector3d axis = w_mid.normalized();
Quaterniond dq(cos(theta/2), sin(theta/2)*axis.x(), sin(theta/2)*axis.y(), sin(theta/2)*axis.z());
q_{k+1} = (q_k * dq).normalized(); // 别忘了归一化！
// 3. 速度更新
Matrix3d R_mid = q_k.toRotationMatrix(); // 这里简化了，实际应该用中值姿态
v_{k+1} = v_k + (R_mid * a_mid + g) * dt;
// 4. 位置更新
p_{k+1} = p_k + v_k * dt + 0.5 * (R_mid * a_mid + g) * dt * dt;
}
💡 个人经验： 上面的代码中，我用了q_k的旋转矩阵来近似中值姿态。严格来说应该用q_k和q_{k+1}的平均值，但这样实现简单，精度也够用。如果你追求极致精度，可以迭代一次。
4.6 总结
好了，今天的内容就到这里。PVQ积分是IMU运动学模型的核心，也是后续IMU预积分、IMU与激光雷达融合的基础。
记住几个要点：
- 离散化方法选中值法，别偷懒用欧拉法
- 四元数必须归一化，这是血的教训
- 注意重力补偿和时间同步
下一章我们会深入IMU预积分的内容，到时候你会看到今天讲的PVQ积分是如何被巧妙地重用的。