第一章 捷联惯导系统概述

各位同学好，我是你们这门课的主讲。今天咱们聊聊捷联惯导系统最基础的东西——基本原理、坐标系定义，还有姿态表示方法。这些东西看着简单，但我在项目里吃过不少亏，所以想从一开始就把这些坑给你们指出来。

1.1 捷联惯导的基本原理

捷联惯导，说白了就是把惯性传感器（陀螺仪和加速度计）直接“绑”在载体上。跟平台式惯导不一样，它没有物理上的稳定平台。那怎么导航呢？靠数学！

陀螺仪测角速度，加速度计测比力。然后通过计算机实时解算，算出姿态、速度和位置。我刚开始接触这个的时候，总觉得“这不就是积分嘛，有什么难的？”后来发现，积分误差积累起来，能把你的导航结果带到沟里去。

嗯，这里要注意：捷联惯导没有活动部件，可靠性高，成本低。但代价是计算量大，对算法要求高。咱们这门课，就是专门讲怎么在嵌入式平台上把这些算法跑得又快又准。

1.2 坐标系定义

搞惯导，坐标系是绕不开的。我见过不少新手，坐标系搞混了，结果整个程序推倒重来。所以咱们先把坐标系理清楚。

常用的坐标系有这么几个：

• 地心惯性坐标系（i系）：原点在地心，轴指向恒星。这是牛顿定律成立的坐标系。说白了，就是绝对空间的一个参考。
• 地球坐标系（e系）：原点在地心，z轴指向北极，x轴指向本初子午线。跟着地球转的。
• 导航坐标系（n系）：通常选“东北天”或“北东地”。我习惯用东北天，因为直观。原点在载体所在位置。
• 载体坐标系（b系）：原点在载体质心，x轴朝前，y轴朝右，z轴朝下（右前上也有，看具体定义）。

坐标系	原点	特点
i系	地心	不旋转，惯性定律适用
e系	地心	随地球自转
n系	载体位置	东北天，导航计算用
b系	载体质心	传感器直接测量

1.3 姿态表示方法

姿态表示，是捷联惯导的核心。说白了，就是描述b系相对于n系怎么转的。常用的有三种：欧拉角、四元数、方向余弦矩阵。咱们一个一个说。

1.3.1 欧拉角

欧拉角最直观。就是三个角度：航向角（ψ）、俯仰角（θ）、横滚角（φ）。你想想看，飞机怎么转的？先转航向，再抬头，最后侧倾。这就是欧拉角的物理意义。

但是！欧拉角有个致命问题——万向锁。当俯仰角接近±90°时，航向和横滚就分不清了。我在做无人机项目时，有一次飞机垂直爬升，姿态解算直接炸了。后来查原因，就是万向锁。

1.3.2 方向余弦矩阵（DCM）

DCM是一个3×3的矩阵，表示两个坐标系之间的旋转关系。它没有万向锁问题，但计算量大。每次更新要算9个元素，而且还要做正交化处理。

我记得有一次做嵌入式移植，CPU资源紧张，用DCM做姿态更新，一次迭代要好几毫秒。后来换成四元数，时间降了一个数量级。

// DCM更新示例（简化版）
// C_b^n 是方向余弦矩阵
// omega 是角速度向量
void dcm_update(float C[3][3], float omega[3], float dt) {
    // 构造反对称矩阵
    float Omega[3][3] = {
        {0, -omega[2], omega[1]},
        {omega[2], 0, -omega[0]},
        {-omega[1], omega[0], 0}
    };
    // 一阶近似更新
    for(int i=0; i<3; i++)
        for(int j=0; j<3; j++)
            C[i][j] += dt * (C[i][0]*Omega[0][j] + 
                             C[i][1]*Omega[1][j] + 
                             C[i][2]*Omega[2][j]);
    // 别忘了正交化！
    orthogonalize(C);
}

1.3.3 四元数

四元数是我个人最喜欢的姿态表示方法。它用四个数表示旋转：q = [q0, q1, q2, q3]。没有万向锁，计算量小，而且容易做插值。

四元数跟欧拉角、DCM之间可以互相转换。我一般用四元数做姿态更新，只在需要输出时转成欧拉角。

// 四元数更新（毕卡算法）
// q 是四元数，omega 是角速度，dt 是时间步长
void quaternion_update(float q[4], float omega[3], float dt) {
    float wx = omega[0], wy = omega[1], wz = omega[2];
    float norm = sqrt(wx*wx + wy*wy + wz*wz);
    float half_theta = 0.5f * norm * dt;
    
    if(norm > 1e-10f) {
        float sin_half = sinf(half_theta) / norm;
        float cos_half = cosf(half_theta);
        
        float dq[4] = {
            cos_half,
            wx * sin_half,
            wy * sin_half,
            wz * sin_half
        };
        // 四元数乘法
        float q_new[4];
        q_new[0] = q[0]*dq[0] - q[1]*dq[1] - q[2]*dq[2] - q[3]*dq[3];
        q_new[1] = q[0]*dq[1] + q[1]*dq[0] + q[2]*dq[3] - q[3]*dq[2];
        q_new[2] = q[0]*dq[2] - q[1]*dq[3] + q[2]*dq[0] + q[3]*dq[1];
        q_new[3] = q[0]*dq[3] + q[1]*dq[2] - q[2]*dq[1] + q[3]*dq[0];
        
        for(int i=0; i<4; i++) q[i] = q_new[i];
        // 归一化
        float norm_q = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
        for(int i=0; i<4; i++) q[i] /= norm_q;
    }
}

1.4 三种方法的对比

方法	优点	缺点	适用场景
欧拉角	直观，物理意义明确	万向锁，计算有奇点	显示、小角度运动
DCM	无奇点，线性	计算量大，需正交化	全姿态，但算力充足时
四元数	无奇点，计算量小	不够直观	嵌入式实时系统首选

我个人在嵌入式项目里，90%的情况都用四元数。只有在需要跟用户交互时，才转成欧拉角。DCM我基本不用，除非是教学演示。

1.5 本章知识体系

下面这张图，是我画的本章知识结构。你看一眼，就能明白各个概念之间的关系。

这张图把本章内容串起来了。你记住：捷联惯导的核心就是用数学平台替代物理平台，而数学平台的基础就是姿态更新。姿态表示方法里，四元数是嵌入式平台的首选。

好了，第一章就讲这么多。内容不多，但都是基础。坐标系搞不清，后面全白搭。姿态表示选不对，代码写再多也没用。希望你们能把这些概念吃透。

---

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