数学基础：向量、矩阵、坐标系定义与旋转矩阵
姿态解算的本质，是回答“一个刚体在三维空间中相对于某个参考系，绕了多大角度、朝哪个方向转”的问题。而这一切的数学语言，都建立在向量、矩阵和坐标系之上。无论是加速度计、陀螺仪还是磁力计，它们输出的原始数据都是相对于自身“机体坐标系”的向量；而我们要得到的俯仰角、横滚角、航向角，则是相对于“地理坐标系”的投影关系。因此，理解坐标系的定义、掌握旋转矩阵的性质，是进入姿态解算世界的第一道门槛。本章将系统梳理这些基础数学工具，并重点剖析旋转矩阵的几何意义与运算规则，为后续的四元数、欧拉角、卡尔曼滤波等内容打下坚实的根基。
2.1 向量与矩阵：姿态描述的原子语言
核心原理：在三维空间中，一个向量可以表示为 v = [vx, vy, vz]ᵀ，它既有大小又有方向。在姿态解算中，向量通常用来表示某个物理量（如重力加速度、磁场强度、角速度）在特定坐标系下的投影。矩阵则是对向量进行线性变换的工具。一个 3×3 矩阵作用于一个三维向量，可以改变其方向（旋转）和长度（缩放）。姿态解算中使用的旋转矩阵，是一种特殊的正交矩阵，它只改变向量的方向，不改变其长度。
判断方法或操作流程：当我们从传感器读取数据时，需要明确该数据是在哪个坐标系下表达的。例如，加速度计输出的 [ax, ay, az] 是机体坐标系下的比力向量；而我们要计算俯仰角，需要知道重力向量在地理坐标系下的表达是 [0, 0, g]。因此，操作流程是：明确传感器坐标系 → 明确参考坐标系 → 建立旋转矩阵 → 进行向量变换。
| 数学对象 | 维度 | 在姿态解算中的角色 | 关键性质 |
|---|---|---|---|
| 向量 | 3×1 | 表示加速度、角速度、磁场等物理量 | 有方向、有大小，可进行加法、数乘、点积、叉积 |
| 旋转矩阵 | 3×3 | 描述两个坐标系之间的旋转变换 | 正交矩阵，行列式为+1，逆矩阵等于转置 |
| 单位矩阵 I | 3×3 | 零旋转（坐标系重合） | 任何向量乘以 I 保持不变 |
2.2 两大核心坐标系：地理系与机体系
核心原理：地理坐标系（n系，Navigation Frame）是姿态解算的参考基准。通常采用“北-东-地”（NED）或“东-北-天”（ENU）的右手定则。NED 坐标系中，X 轴指向北，Y 轴指向东，Z 轴指向地心。机体坐标系（b系，Body Frame）则固定在飞行器或运动载体上，原点位于载体质心，X 轴沿机头方向，Y 轴沿右翼方向，Z 轴垂直于机体向下。姿态解算的目标，就是实时求出 b 系相对于 n 系的旋转关系。
案例拆解或实战场景：假设一架四旋翼无人机悬停在空中。此时，加速度计在机体坐标系下测得的比力约为 [0, 0, -g]（假设机体水平）。如果无人机向前倾斜 30°，那么加速度计测得的 X 轴分量将不再是 0，而是 g·sin(30°) ≈ 4.9 m/s²。通过这个简单的向量投影关系，我们可以初步估计俯仰角。但要注意，加速度计无法区分重力加速度和运动加速度，因此仅适用于静态或准静态场景。
2.3 旋转矩阵的基本性质与运算规则
核心原理：旋转矩阵 R 是一个 3×3 的正交矩阵，满足 RᵀR = I 且 det(R) = +1。这意味着旋转矩阵的逆矩阵就是它的转置矩阵，即 R⁻¹ = Rᵀ。这一性质极大地简化了坐标变换的逆运算。旋转矩阵的每一列（或每一行）都是单位向量，且两两正交。从几何上看，旋转矩阵的列向量表示的是原坐标系各轴在新坐标系中的方向余弦。
判断方法或操作流程：旋转矩阵的乘法不满足交换律，即 R₁·R₂ ≠ R₂·R₁。这对应着三维旋转的顺序依赖性。在实际应用中，我们通常采用“Z-Y-X”欧拉角顺序（先偏航，再俯仰，最后横滚）来构建旋转矩阵：
R = Rz(ψ) · Ry(θ) · Rx(φ)
其中：
Rz(ψ) = [cosψ, -sinψ, 0; sinψ, cosψ, 0; 0, 0, 1]
Ry(θ) = [cosθ, 0, sinθ; 0, 1, 0; -sinθ, 0, cosθ]
Rx(φ) = [1, 0, 0; 0, cosφ, -sinφ; 0, sinφ, cosφ]案例拆解或实战场景：假设某载体先绕 Z 轴旋转 30°（偏航），再绕新的 Y 轴旋转 45°（俯仰）。我们想将一个在机体坐标系下的向量 v_b = [1, 0, 0]（机头方向单位向量）转换到地理坐标系。计算过程如下：
- 构建 Rz(30°) 和 Ry(45°) 矩阵；
- 计算 R = Rz(30°) · Ry(45°)；
- 计算 v_n = R · v_b。
最终得到的 v_n 就是机头方向在地理坐标系下的投影。这个投影向量可以用来计算航向角或进行导航解算。
2.4 常见误区与总结复盘
常见误区：
- 坐标系混淆：将传感器数据直接当作地理坐标系下的数据使用，忽略了坐标变换。
- 旋转顺序错误：在构建旋转矩阵时，没有按照约定的顺序（如 Z-Y-X）进行矩阵乘法，导致结果错误。
- 矩阵转置误用：将 R_b→n 与 R_n→b 混淆。实际上，R_b→n = (R_n→b)ᵀ。
- 忽略正交性校验：在数值计算中，由于浮点误差累积，旋转矩阵可能逐渐失去正交性。需要定期进行正交化修正（如 Gram-Schmidt 正交化）。
总结复盘：本章的核心是建立“坐标系-向量-矩阵”三位一体的思维框架。在后续章节中，无论是推导四元数更新方程，还是设计互补滤波器，都需要频繁使用这些基础概念。建议读者在纸上手动推导一遍旋转矩阵的乘法，并编写一个简单的 Python 脚本验证 RᵀR = I 的性质。只有亲手算过，才能真正理解旋转矩阵的几何意义。