4. 姿态解算数学基础

姿态解算的本质，是将IMU输出的角速度与加速度原始数据，通过一系列数学变换，转换为直观、稳定的姿态描述（如欧拉角、旋转矩阵或四元数）。这一过程高度依赖五个数学工具：三角函数用于角度与方向余弦的换算，向量运算用于描述重力与加速度的投影关系，矩阵乘法用于坐标系旋转与姿态更新，微分方程用于描述姿态随时间的连续变化，而数值积分则是将连续微分方程离散化到MCU上可执行的迭代算法。本章将逐一拆解这五块基石，并重点说明它们如何在卡尔曼滤波与互补滤波的框架中协同工作。

上图展示了本章内容在姿态解算系统中的位置：IMU原始数据（角速度与加速度）首先经过三角函数、向量运算、矩阵乘法等数学工具进行预处理与坐标变换，再通过微分方程建立姿态运动模型，最后利用数值积分在离散时间域内递推求解。卡尔曼滤波与互补滤波则负责融合不同传感器的优势，抑制单一传感器的漂移或噪声。

4.1 三角函数：角度与方向余弦的桥梁

核心原理：三角函数在姿态解算中承担着两个基本任务：一是将角度（欧拉角）转换为方向余弦值，用于构建旋转矩阵；二是从旋转矩阵或向量投影中反解出角度。例如，当加速度计测量到重力向量在机体坐标系下的投影 (a_x, a_y, a_z) 时，横滚角 φ 和俯仰角 θ 可以通过反正切函数直接求得：

φ = atan2(a_y, a_z)
θ = atan2(-a_x, sqrt(a_y² + a_z²))

这里使用 atan2 而非 atan，是为了正确处理四个象限的角度范围，避免因分母符号导致的歧义。此外，三角函数还用于将角速度积分得到的角度增量转换为旋转矩阵的增量形式。

判断方法：在实际工程中，需要根据姿态表示方式选择合适的三角函数组合。若使用欧拉角，则需注意万向锁问题（当俯仰角接近 ±90° 时，横滚与偏航角退化）；若使用四元数，则三角函数主要隐含在四元数到旋转矩阵的转换公式中。下表总结了常见场景下的三角函数应用：

4.2 向量运算：重力投影与叉积校正

核心原理：向量运算是连接加速度计与陀螺仪的纽带。加速度计测量的是比力向量（包含重力与运动加速度），在静止或匀速运动时，其测量值即为重力向量在机体坐标系下的投影。通过将估计的姿态矩阵（或四元数）与已知的重力参考向量 [0, 0, 1] 相乘，可以得到预测的重力投影向量。将预测向量与实际加速度计测量向量做叉积，得到的误差向量正比于姿态误差的角度，这一误差可用于修正陀螺仪积分带来的漂移。

操作流程：互补滤波与卡尔曼滤波中均使用这一思想。以互补滤波为例，其核心步骤为：

1. 用当前姿态四元数计算重力方向预测向量 v = R(q) * [0,0,1]。
2. 将加速度计测量值归一化得到 a_meas。
3. 计算叉积 e = a_meas × v，该误差向量包含了姿态偏差信息。
4. 将误差 e 经过比例-积分（PI）控制器后，叠加到陀螺仪角速度上，修正四元数微分方程。

案例拆解：假设四轴飞行器悬停时，加速度计输出为 [0.02, 0.01, 0.99]（已归一化），而当前姿态估计的重力投影为 [0.05, -0.02, 0.99]。叉积结果为 [0.01*0.99 - 0.99*(-0.02), 0.99*0.05 - 0.02*0.99, 0.02*(-0.02) - 0.01*0.05] ≈ [0.0297, 0.0297, -0.0009]。该误差向量的 x、y 分量较大，表明俯仰与横滚方向存在约 1.7° 的偏差，PI 控制器将据此产生修正角速度，逐步消除该偏差。

4.3 矩阵乘法：坐标系旋转与链式变换

核心原理：姿态解算的核心是描述机体坐标系（b系）相对于导航坐标系（n系，通常为东北天）的旋转关系。这一关系可以用旋转矩阵 C_bⁿ 表示，它是一个 3×3 的正交矩阵，满足 C^T = C^-1。矩阵乘法用于实现向量的坐标系变换：v_n = C_b^n * v_b。当姿态连续变化时，旋转矩阵通过链式乘法不断更新：C(k+1) = C(k) * ΔC，其中 ΔC 是由角速度增量构造的旋转矩阵。

判断方法：选择旋转矩阵还是四元数进行姿态更新，需要权衡计算量与稳定性。旋转矩阵不存在奇异问题，但需要正交化修正（9个参数）；四元数仅有4个参数，计算量小，但需要归一化处理。下表对比了两种表示方式：

实战场景：在MEMS惯性导航中，通常使用四元数进行姿态更新，仅在输出欧拉角或需要与外部系统交互时，才将四元数转换为旋转矩阵。例如，将四元数 q = [q0, q1, q2, q3] 转换为旋转矩阵的公式为：

C = [[1-2(q2²+q3²), 2(q1q2-q0q3), 2(q1q3+q0q2)],
     [2(q1q2+q0q3), 1-2(q1²+q3²), 2(q2q3-q0q1)],
     [2(q1q3-q0q2), 2(q2q3+q0q1), 1-2(q1²+q2²)]]

该矩阵可用于将加速度计或磁力计向量从机体坐标系转换到导航坐标系，便于后续的姿态误差计算。

4.4 微分方程与数值积分：从连续到离散

核心原理：姿态运动学微分方程描述了姿态参数随时间的变化率。对于四元数，其微分方程为：

dq/dt = 0.5 * q ⊗ ω

其中 ω 为陀螺仪测量的角速度四元数（实部为0），⊗ 表示四元数乘法。该方程是连续时间域的，而MCU只能以离散时间步长 Δt 进行处理，因此需要数值积分方法。最常用的一阶欧拉法为：

q(k+1) = q(k) + dq/dt * Δt

但欧拉法在较大 Δt 或高动态场景下会引入显著累积误差。更精确的龙格-库塔法（如RK4）通过多次函数评估提高精度，但计算量约为欧拉法的4倍。

操作流程：选择积分方法需根据系统动态特性与MCU算力权衡。对于无人机飞控（通常 1kHz 更新率），一阶欧拉法配合四元数归一化已足够；对于高精度INS或低更新率场景，建议使用RK4或二阶龙格库塔。此外，陀螺仪零偏会导致积分后的角度持续漂移，因此必须结合加速度计/磁力计进行修正。

案例拆解：某机器人平台使用 200Hz 姿态更新率，陀螺仪零偏为 0.01°/s。若仅使用纯积分，1小时后姿态漂移可达 36°。引入互补滤波后，加速度计修正将漂移抑制在 2° 以内。具体实现时，将陀螺仪积分得到的姿态与加速度计估计的姿态进行加权融合，权重系数根据传感器噪声特性确定（通常陀螺仪权重为 0.98，加速度计为 0.02）。