4. 捷联惯导解算（INS）：姿态更新算法（四元数微分方程）、速度更新算法（比力方程）、位置更新算法、圆锥效应与划船效应补偿

各位同学，欢迎来到实战案例的第四讲。今天我们要啃的这块骨头，是INS解算的核心——姿态、速度、位置的更新。说白了，就是怎么用IMU测出来的角速度和加速度，算出载体每一刻到底在哪儿、面朝哪边、跑得多快。

我个人习惯把INS解算比作「积分游戏」。你想想看，陀螺给角速度，积分得角度；加速度计给比力，积分得速度，再积分得位置。听起来简单？嗯，这里要注意，实际工程里全是坑。我当年第一次调车规级IMU，就因为在积分算法上偷了个懒，结果车在模拟器上原地转圈，定位误差直接飞了。

4.1 姿态更新算法：四元数微分方程

姿态更新是INS的「脊梁骨」。姿态算错了，速度和位置全完蛋。目前工程界最常用的姿态表达方式就是四元数。为什么不用欧拉角？因为欧拉角有万向锁问题，而且三角函数计算量大。四元数呢？紧凑、无奇点、适合计算机迭代。

四元数微分方程长这样：

dq/dt = 0.5 * q ⊗ ω

其中ω是载体坐标系下的角速度，q是当前姿态四元数。⊗表示四元数乘法。

实际工程中，我们没法做连续积分，只能用离散化方法。最常用的就是毕卡逼近法（一阶或二阶）。我建议初学者先掌握一阶毕卡法，代码简单，理解起来也直观：

// 一阶毕卡法姿态更新
void attitudeUpdate(Quaternion *q, const Vector3 *gyro, float dt) {
    float wx = gyro->x * dt * 0.5f;
    float wy = gyro->y * dt * 0.5f;
    float wz = gyro->z * dt * 0.5f;
    
    Quaternion dq = {1.0f, wx, wy, wz};
    *q = quaternionMultiply(*q, dq);
    quaternionNormalize(q);  // 归一化，防止误差累积
}

4.2 速度更新算法：比力方程

速度更新依赖的是比力方程。加速度计测的是「比力」，不是真正的加速度。比力包含了重力加速度和运动加速度。所以我们要把重力分量去掉，才能得到真实的运动加速度。

比力方程在导航坐标系下的形式：

dV/dt = C_b^n * f_b - (2ω_ie + ω_en) × V + g_n

看着复杂？我拆开给你看：

• C_b^n * f_b：把加速度计测的比力从载体坐标系转到导航坐标系
• -(2ω_ie + ω_en) × V：哥里奥利力和向心力的补偿项
• g_n：重力加速度（已知的模型值）

实际代码实现时，我习惯用中值法做积分。也就是取当前时刻和上一时刻的加速度平均值来更新速度：

void velocityUpdate(Vector3 *vel, const Vector3 *acc_n, float dt) {
    // acc_n 已经是导航坐标系下的比力（已扣除重力）
    vel->x += (acc_n->x + acc_n_prev->x) * 0.5f * dt;
    vel->y += (acc_n->y + acc_n_prev->y) * 0.5f * dt;
    vel->z += (acc_n->z + acc_n_prev->z) * 0.5f * dt;
    
    // 保存当前值供下一帧使用
    *acc_n_prev = *acc_n;
}

4.3 位置更新算法

位置更新就相对简单了——对速度做积分。但这里有个细节：地球是椭球体，经纬度更新时需要考虑曲率半径。

位置更新的离散形式：

// 经纬高更新（简化版）
void positionUpdate(Position *pos, const Vector3 *vel_n, float dt) {
    float lat = pos->latitude;
    float alt = pos->altitude;
    
    // 计算曲率半径
    float Rn = 6378137.0f / sqrt(1.0f - (0.006694379f * sin(lat) * sin(lat)));
    float Re = Rn * (1.0f - 0.006694379f) / (1.0f - 0.006694379f * sin(lat) * sin(lat));
    
    // 位置更新
    pos->latitude  += vel_n->x * dt / (Rn + alt);
    pos->longitude += vel_n->y * dt / ((Re + alt) * cos(lat));
    pos->altitude  += vel_n->z * dt;
}

嗯，这里要注意，纬度更新后会影响经度更新的分母，所以最好先更新纬度，再用新纬度算经度。

4.4 圆锥效应与划船效应补偿

这两个效应是INS解算里的「隐形杀手」。很多初学者不知道它们的存在，直到定位精度怎么都提不上去。

圆锥效应：当载体做圆锥运动时（比如直升机旋翼的振动），陀螺输出的角速度积分后会产生虚假的姿态漂移。说白了，就是角速度的「整流」效应。

划船效应：类似地，当载体同时有平动和转动时，加速度计积分会产生虚假的速度增量。

补偿方法有很多，我推荐工程中最实用的三子样算法：

// 圆锥效应补偿（三子样算法）
Vector3 coningCompensation(const Vector3 *gyro, float dt) {
    float h = dt / 3.0f;  // 子样间隔
    
    // 三个子样
    Vector3 w1 = gyro[0];
    Vector3 w2 = gyro[1];
    Vector3 w3 = gyro[2];
    
    // 圆锥补偿项
    Vector3 alpha = vectorAdd(w1, w2);
    alpha = vectorAdd(alpha, w3);
    alpha = vectorScale(alpha, h);
    
    // 补偿量
    Vector3 delta = vectorCross(w1, w2);
    delta = vectorAdd(delta, vectorScale(vectorCross(w2, w3), 3.0f));
    delta = vectorAdd(delta, vectorScale(vectorCross(w1, w3), 0.45f));
    delta = vectorScale(delta, h * h);
    
    return vectorAdd(alpha, delta);
}

4.5 知识体系总览

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

这张图把整个流程串起来了。你从左边看，陀螺数据经过圆锥补偿后进入姿态更新，姿态更新结果用于坐标转换。右边加速度计数据经过划船补偿后，结合坐标转换结果，进入速度更新，最后得到位置。每一步都环环相扣。

好了，这一章的内容就到这里。INS解算的这三个核心算法——姿态、速度、位置更新，加上两个补偿算法，是车载定位系统的基石。我建议你动手把代码写一遍，跑几个仿真数据看看效果。只有亲手调过，才能真正理解这些算法背后的物理意义。

可可精品资料库