4、惯性导航解算(INS)：姿态更新、速度更新、位置更新算法，圆锥/划桨效应补偿

各位同学，今天我们来啃一块硬骨头——惯性导航解算。说实话，我刚入行那会儿，觉得INS不就是积分嘛，有什么难的？直到第一次在车载平台上跑算法，车一颠簸，姿态直接飞了……嗯，从那以后我再也不敢小看这个模块了。

惯性导航解算，说白了就是利用IMU输出的角速度和加速度，实时推算载体的姿态、速度和位置。你想想看，IMU给的是原始数据，我们要把它变成可用的导航信息，中间这步就是INS的核心。

4.1 姿态更新算法

姿态更新是INS里最敏感的一环。为什么？因为姿态算错了，速度和位置全跟着错。我见过不少新手，姿态更新用最简单的欧拉角法，结果在俯仰角接近90度时直接奇异——这就是典型的“万向锁”问题。

我个人习惯用四元数法做姿态更新。四元数没有奇异性，计算量也适中。核心公式是这样的：

// 姿态更新（四元数法）
// q: 当前姿态四元数
// w: 角速度 (rad/s)
// dt: 采样间隔

void attitudeUpdate(Quaternion &q, const Vector3d &w, double dt) {
    double half_dt = 0.5 * dt;
    double wx = w.x * half_dt;
    double wy = w.y * half_dt;
    double wz = w.z * half_dt;
    
    // 构造增量四元数
    Quaternion dq(1.0, wx, wy, wz);
    
    // 归一化
    double norm = sqrt(dq.w*dq.w + dq.x*dq.x + dq.y*dq.y + dq.z*dq.z);
    dq.w /= norm; dq.x /= norm; dq.y /= norm; dq.z /= norm;
    
    // 更新姿态
    q = q * dq;
    
    // 再次归一化，防止误差累积
    norm = sqrt(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
    q.w /= norm; q.x /= norm; q.y /= norm; q.z /= norm;
}

4.2 速度更新算法

速度更新相对直观一些，就是把加速度积分。但这里有个陷阱——你用的加速度是载体坐标系下的，而速度更新需要在导航坐标系下进行。

所以流程是：先把加速度从载体坐标系转到导航坐标系，再扣除重力，然后积分。公式如下：

// 速度更新
// v: 当前速度 (导航系)
// a_body: 加速度计读数 (载体系)
// q: 当前姿态四元数
// g: 重力向量 (导航系)
// dt: 采样间隔

void velocityUpdate(Vector3d &v, const Vector3d &a_body, 
                    const Quaternion &q, const Vector3d &g, double dt) {
    // 1. 将加速度从载体系转到导航系
    Vector3d a_nav = q.rotate(a_body);
    
    // 2. 扣除重力
    a_nav = a_nav - g;
    
    // 3. 积分
    v = v + a_nav * dt;
}

4.3 位置更新算法

位置更新就是速度的积分。但要注意，位置更新对误差的累积效应最明显。你想想看，速度上有个小偏差，积分到位置就是随时间线性增长的误差。

位置更新代码很简单：

// 位置更新
// p: 当前位置
// v: 当前速度
// dt: 采样间隔

void positionUpdate(Vector3d &p, const Vector3d &v, double dt) {
    p = p + v * dt;
}

嗯，代码确实简单。但实际工程中，位置误差是INS最大的痛点。纯惯性导航的位置误差会随时间三次方增长——这就是为什么INS必须配合其他传感器做融合。

4.4 圆锥效应补偿

圆锥效应，这个名字听起来挺玄乎。说白了就是：当载体同时存在两个正交方向的角振动时，四元数更新会产生一个虚假的旋转分量。这个效应在振动环境下特别明显。

我曾经在无人机平台上调试，发现悬停时姿态一直在缓慢漂移。查了半天，最后发现是圆锥效应没补偿。补偿方法其实不复杂，核心思想是用多子样采样来近似角速度的积分：

// 圆锥效应补偿（三子样法）
// w1, w2, w3: 连续三个采样时刻的角速度
// dt: 子样间隔

Vector3d coningCompensation(const Vector3d &w1, const Vector3d &w2, 
                            const Vector3d &w3, double dt) {
    // 计算角增量
    Vector3d dtheta1 = w1 * dt;
    Vector3d dtheta2 = w2 * dt;
    Vector3d dtheta3 = w3 * dt;
    
    // 圆锥补偿项
    Vector3d coning = (1.0/3.0) * cross(dtheta1, dtheta3) 
                    + (2.0/3.0) * cross(dtheta1, dtheta2);
    
    return coning;
}

4.5 划桨效应补偿

划桨效应和圆锥效应是对偶的。圆锥效应针对角运动，划桨效应针对线运动。当载体同时存在线振动和角振动时，速度更新会产生虚假的增量。

补偿方法类似，也是用多子样采样：

// 划桨效应补偿（二子样法）
// a1, a2: 连续两个采样时刻的加速度
// w1, w2: 对应的角速度
// dt: 子样间隔

Vector3d scullingCompensation(const Vector3d &a1, const Vector3d &a2,
                              const Vector3d &w1, const Vector3d &w2, double dt) {
    // 计算角增量和速度增量
    Vector3d dtheta1 = w1 * dt;
    Vector3d dtheta2 = w2 * dt;
    Vector3d dv1 = a1 * dt;
    Vector3d dv2 = a2 * dt;
    
    // 划桨补偿项
    Vector3d sculling = 0.5 * (cross(dtheta1, dv2) + cross(dv1, dtheta2));
    
    return sculling;
}

4.6 知识体系总览

为了让大家更直观地理解INS解算的完整流程，我画了一张流程图：

这张图把INS解算的完整链路展示出来了。从IMU原始数据开始，经过圆锥/划桨补偿，再到姿态、速度、位置的递推更新。每一步都环环相扣，任何一环出问题，后面的结果都会受影响。

4.7 总结与避坑指南

好了，这一章的内容就到这里。我最后给大家总结几个实战中容易踩的坑：

• 四元数忘记归一化——这是最常见的错误，会导致姿态慢慢漂移。我建议每次更新后都做一次归一化，养成习惯。
• 重力扣除不准确——重力向量必须精确已知，否则速度更新会有常值偏差。记得根据纬度计算重力大小，不要用9.8糊弄过去。
• 忽略圆锥/划桨效应——在振动环境下，这两个效应的影响不可忽略。我曾经在车载测试中吃过这个亏，后来老老实实加上了补偿。
• 采样率不够——IMU采样率低于100Hz时，圆锥补偿效果很差。我建议至少200Hz起步。

嗯，这一章就讲到这里。INS解算是多传感器融合的基础，后面的章节我们会陆续引入GPS、视觉、激光雷达等传感器，和INS做融合。到时候你会发现，INS虽然单独用不行，但和其他传感器一结合，效果就非常棒了。

---

公众号:蓝海资料掘金营，微信deep3321