2、数学基础：坐标系定义（惯性系、地球系、导航系、载体系）、欧拉角、方向余弦矩阵、四元数基本运算

捷联惯导姿态解算的物理本质，是描述载体（如无人机、导弹、机器人）相对于某个参考坐标系的角运动关系。在工程实现中，我们并不直接测量姿态角，而是通过陀螺仪输出的角速度积分获得姿态更新。因此，必须首先建立一套严格的坐标系定义和数学表达工具，才能将传感器数据与空间姿态一一对应。本章将系统梳理四种核心坐标系、欧拉角旋转顺序、方向余弦矩阵（DCM）以及四元数的基本运算规则，为后续的姿态更新算法打下坚实的数学地基。

2.1 四种坐标系的定义与工程含义

核心原理：坐标系是描述姿态的参照基准。在捷联惯导中，我们关心的是载体系（b系）相对于导航系（n系）的角位置关系。惯性系（i系）是牛顿力学成立的惯性空间，地球系（e系）随地球自转，导航系通常取为当地水平坐标系（东北天或北东地）。理解各坐标系之间的转换关系，是姿态解算的第一步。

判断方法：工程中需要明确区分：陀螺仪和加速度计测量的是相对于惯性空间的角速度和比力，但我们需要的是相对于导航系的姿态。因此，必须考虑地球自转和载体运动带来的影响。对于中低精度应用（如消费级无人机），地球自转速率（15°/h）通常可以忽略；但对于高精度惯导，必须补偿这一项。

2.2 欧拉角：直观但存在奇异性

核心原理：欧拉角用三个角度描述刚体相对于参考系的姿态，通常定义为横滚角（φ）、俯仰角（θ）、偏航角（ψ）。其本质是三次基本旋转的组合，旋转顺序至关重要。在航空航天领域，常用Z-Y-X顺序（先偏航、再俯仰、最后横滚）。

操作流程：从导航系到载体系的旋转矩阵可分解为三次基本旋转的连乘：

C_b^n = R_x(φ) · R_y(θ) · R_z(ψ)

其中R_x、R_y、R_z分别为绕X、Y、Z轴的基本旋转矩阵。需要注意的是，矩阵乘法不满足交换律，因此旋转顺序必须严格固定。

案例拆解：某固定翼无人机在爬升过程中，俯仰角达到85°，此时横滚角从10°变化到15°。若采用欧拉角直接微分方程更新，会出现姿态跳变和数值发散。实际工程中，我们改用四元数进行姿态递推，仅在输出控制指令时通过转换公式提取欧拉角。这样既保证了数值稳定性，又保留了欧拉角的直观性。

2.3 方向余弦矩阵：精确但冗余

核心原理：方向余弦矩阵（DCM）是一个3×3正交矩阵，其元素为两个坐标系各轴之间夹角的余弦值。DCM可以无奇异性地描述任意姿态，且可以直接用于向量坐标变换。但9个元素之间存在6个约束条件（正交性和单位行列式），导致计算冗余。

判断方法：在工程中，DCM主要用于坐标变换和姿态提取。例如，将加速度计测量的比力从载体系转换到导航系，或从DCM中提取欧拉角用于显示。DCM的更新方程涉及矩阵微分，计算量较大，且需要定期正交化修正。

2.4 四元数基本运算：工程首选

核心原理：四元数是一个四维超复数，表示为q = q0 + q1·i + q2·j + q3·k，其中i、j、k满足i²=j²=k²=ijk=-1。单位四元数（模长为1）可以无奇异性地描述三维旋转，且运算规则简洁高效。四元数与旋转轴和旋转角的关系为：q = [cos(θ/2), sin(θ/2)·u]，其中u为旋转轴单位向量。

操作流程：四元数的基本运算包括乘法、共轭、归一化和旋转操作。两个四元数相乘表示旋转的复合，共轭表示逆旋转。将向量v从载体系转换到导航系的公式为：

v_n = q ⊗ v_b ⊗ q*

其中q*为q的共轭四元数，⊗表示四元数乘法。在代码实现中，四元数乘法可展开为矩阵形式，避免循环开销。

案例拆解：某四旋翼无人机采用MPU6050 IMU，陀螺仪量程±2000°/s，输出频率1kHz。姿态解算流程为：读取角速度 → 四元数微分方程更新 → 归一化 → 加速度计修正（互补滤波）。其中四元数微分方程为：

dq/dt = 0.5 · q ⊗ ω

其中ω为角速度四元数（实部为0）。采用一阶龙格-库塔法离散化后，每个周期仅需约20次浮点运算，在STM32F405上运行耗时约2μs，完全满足实时性要求。经过100秒飞行测试，姿态漂移小于2°，验证了四元数算法的工程有效性。