数学基础：坐标系定义与姿态描述方法
姿态解算与稳定控制的核心，是回答“飞行器/载体现在相对于参考系转了多少、朝向哪里”这一基本问题。无论是四旋翼无人机、机械臂末端，还是惯性测量单元（IMU）驱动的云台，其控制律设计都建立在精确的姿态描述之上。本章将系统梳理机体坐标系与惯性坐标系的定义、欧拉角、旋转矩阵、四元数、方向余弦矩阵（DCM）的数学本质，并给出工程中最常用的相互转换方法与注意事项。这些内容不仅是后续卡尔曼滤波、互补滤波的基础，也是理解“姿态”这一抽象概念从物理世界到数学表达的关键桥梁。
2.1 坐标系定义：惯性系与机体系
核心原理：姿态描述必须建立在明确的坐标系基准之上。惯性坐标系（i系）是描述载体运动与姿态的参考基准，通常选取地球固连坐标系（NED：北东地）或地心惯性系（ECI）。在无人机、机器人等近地面应用中，NED坐标系最为常用：X轴指向北、Y轴指向东、Z轴指向地心。机体坐标系（b系）则固连于载体本身，原点位于载体质心，通常定义X轴沿机体纵轴向前（机头方向）、Y轴沿机体右翼、Z轴沿机体竖轴向下（右手定则）。
判断方法：工程中需明确区分“载体姿态”与“载体位置”。姿态描述的是b系相对于i系的角位置关系，而位置描述的是b系原点在i系中的坐标。两者独立但耦合——姿态影响加速度计测量值的分解，位置变化又通过旋转矩阵影响导航解算。
2.2 欧拉角：直观但存在万向锁
核心原理：欧拉角通过三次绕不同轴的旋转来描述姿态，常用顺序为Z-Y-X（偏航ψ→俯仰θ→横滚φ）。其物理意义直观：横滚角φ为绕机体X轴旋转，俯仰角θ为绕机体Y轴旋转，偏航角ψ为绕机体Z轴旋转。旋转矩阵由三次基本旋转矩阵连乘得到：C_b^n = Rz(ψ)·Ry(θ)·Rx(φ)。
操作流程：从旋转矩阵提取欧拉角时，需注意象限判断。标准公式为：φ = atan2(R21, R11)，θ = -asin(R31)，ψ = atan2(R32, R33)。但工程中更推荐使用atan2函数而非asin/acos，以避免角度范围模糊。
| 欧拉角 | 旋转轴 | 范围 | 工程含义 |
|---|---|---|---|
| 横滚φ | X轴 | ±180° | 左右倾斜 |
| 俯仰θ | Y轴 | ±90° | 抬头低头 |
| 偏航ψ | Z轴 | ±180° | 机头朝向 |
2.3 旋转矩阵与方向余弦矩阵（DCM）
核心原理：旋转矩阵（又称方向余弦矩阵）是一个3×3正交矩阵，其元素为两个坐标系基向量之间的方向余弦。DCM将b系向量映射到i系：v_i = C_b^n · v_b。其逆矩阵等于转置矩阵（正交性），即C_n^b = (C_b^n)^T。DCM的9个元素存在6个约束条件（正交性），因此实际自由度为3。
判断方法：工程中判断DCM是否正确，可检查其行列式是否等于1（允许微小误差），以及是否满足C·C^T = I。若误差累积导致正交性破坏，需进行正交化修正（如Gram-Schmidt过程）。
2.4 四元数：高效无奇异
核心原理：四元数由1个实部和3个虚部组成：q = q0 + q1·i + q2·j + q3·k，满足i²=j²=k²=ijk=-1。单位四元数（模长为1）可表示三维旋转，其优点是无万向锁问题、计算量小（仅4个参数）、插值平滑。旋转矩阵可由四元数导出：
R = [
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²)
]操作流程：四元数更新采用一阶龙格-库塔积分：q_dot = 0.5 · q ⊗ ω，其中ω为陀螺仪角速度四元数（实部为0）。每次更新后必须归一化，防止模长漂移。
q_total = q_yaw ⊗ q_pitch ⊗ q_roll（从右到左）。在代码中务必保持一致的乘法顺序，否则姿态会完全错误。
2.5 相互转换与工程选型
核心原理：三种描述方法本质等价，但适用场景不同。欧拉角适合人机交互显示；DCM适合需要明确物理意义的场合；四元数适合实时解算与滤波。转换关系如下：
- 欧拉角→四元数：分别计算三个基本旋转四元数，再按Z-Y-X顺序相乘。
- 四元数→欧拉角：利用四元数元素直接计算：
φ = atan2(2(q0q1+q2q3), 1-2(q1²+q2²))，θ = asin(2(q0q2-q3q1))，ψ = atan2(2(q0q3+q1q2), 1-2(q2²+q3²))。 - DCM↔四元数：通过矩阵迹与元素关系转换，注意当q0接近0时需采用其他元素计算。
| 描述方法 | 参数数量 | 万向锁 | 计算量 | 适用场景 |
|---|---|---|---|---|
| 欧拉角 | 3 | 有 | 小 | 显示、简单控制 |
| DCM | 9 | 无 | 中 | 需要明确物理意义 |
| 四元数 | 4 | 无 | 小 | 滤波、实时解算 |
2.6 实战场景：IMU姿态初始化与更新
案例背景：某四旋翼飞控使用BMI088惯性传感器，需在起飞前完成姿态初始化，并在飞行中以1kHz频率更新姿态。初始姿态由加速度计（测横滚俯仰）和磁力计（测偏航）确定，飞行中由陀螺仪积分。
操作步骤：
- 静态初始化：采集100组加速度计数据求平均，计算初始横滚俯仰角：
φ = atan2(ay, az)，θ = atan2(-ax, sqrt(ay²+az²))。 - 磁力计校准：旋转飞行器采集磁力计数据，拟合椭球参数消除硬磁/软磁干扰，计算偏航角。
- 四元数初始化：将欧拉角转换为四元数，作为滤波初值。
- 实时更新：读取陀螺仪角速度，执行四元数积分并归一化，再转换为欧拉角供控制律使用。
结果对比：采用四元数更新后，10分钟悬停测试姿态漂移小于2°，而直接使用欧拉角积分漂移超过15°。复盘发现，四元数避免了万向锁问题，且归一化操作有效抑制了积分误差累积。