2. 姿态描述与坐标系基础

AHRS（Attitude and Heading Reference System，姿态航向参考系统）输出的核心量是姿态，而姿态本身并不是一个“绝对量”，它必须依附于两个坐标系之间的相对关系来定义。因此，在写任何一行姿态解算代码之前，先把坐标系约定和姿态表示方式固定下来，是整门课程中最不能跳过的一步。本节围绕机体坐标系、导航坐标系、欧拉角、方向余弦矩阵和四元数这五个对象，说明它们各自表示什么、彼此如何转换，以及在AHRS中应当如何选用。

一、两个坐标系：姿态的“参照物”

机体坐标系（Body Frame，记作 b 系）固连在飞行器上，随飞行器一起运动。工程上通常取：原点位于飞行器质心，x_b 轴指向机头前方，y_b 轴指向右翼，z_b 轴指向机腹下方，构成右手系。这个“前—右—下”约定在航空领域最常用，它与“北—东—地”的导航坐标系配合时，俯仰、滚转、偏航的正负号最直观。

导航坐标系（Navigation Frame，记作 n 系）是姿态的参考基准。对于近地、短时、小范围的AHRS，通常取当地水平坐标系：原点位于载体所在位置，x_n 轴指北，y_n 轴指东，z_n 轴沿当地垂线指向地心，即“北—东—地”（NED）右手系。若采用“东—北—天”（ENU）约定，则所有角度符号都会相应改变，因此全文必须统一约定，中途不得混用。本节后续内容一律采用 NED 约定。

姿态的本质，就是 b 系相对于 n 系的旋转关系。把这个旋转关系用不同数学对象表达出来，就得到欧拉角、方向余弦矩阵和四元数三种等价形式。

二、欧拉角：最直观但最脆弱

欧拉角把任意旋转分解为三次绕坐标轴的连续转动。在航空中采用 Z-Y-X 顺序（即先偏航、再俯仰、最后滚转），得到三个角：

• 偏航角 ψ（yaw）：绕 z_n 轴旋转，机头相对正北的夹角，右偏为正，范围通常取 (−180°, 180°]；
• 俯仰角 θ（pitch）：绕旋转后的 y 轴转动，机头相对水平面的夹角，抬头为正，范围 (−90°, 90°)；
• 滚转角 φ（roll）：绕旋转后的 x 轴转动，右翼下沉为正，范围 (−180°, 180°]。

欧拉角的最大优点是物理意义明确，飞行员和地面站都直接读它。但它有两个致命问题：一是万向节死锁，当俯仰角接近 ±90° 时，偏航与滚转的旋转轴重合，二者无法区分，解算会出现奇异；二是三角函数运算量大，且姿态更新时若直接对欧拉角做积分，误差会随姿态变化非线性累积。因此欧拉角适合作为输入输出接口，不适合作为AHRS内部递推的状态量。

三、方向余弦矩阵：旋转的完整代数表达

方向余弦矩阵（DCM，又称旋转矩阵）C_bⁿ 是一个 3×3 正交矩阵，其元素是 b 系各轴与 n 系各轴之间夹角的余弦。它把机体坐标下的矢量 v^b 变换到导航系：vⁿ = C_bⁿ v^b。按 Z-Y-X 顺序，C_bⁿ 可写成三个基本旋转矩阵的乘积：

C_b^n = Rz(ψ) · Ry(θ) · Rx(φ)

        [ cψcθ   cψsθsφ−sψcφ   cψsθcφ+sψsφ ]
      = [ sψcθ   sψsθsφ+cψcφ   sψsθcφ−cψsφ ]
        [ −sθ    cθsφ          cθcφ          ]

其中 c 表示 cos，s 表示 sin。

DCM 没有奇异点，姿态更新时可直接用角速度的反对称矩阵左乘：Ċ_bⁿ = C_bⁿ · [ω^b×]，其中 [ω^b×] 是机体角速度构成的反对称矩阵。代价是需要维护 9 个元素，且数值积分会破坏正交性，必须周期性做正交化（如 Gram-Schmidt 或 SVD 修正）。在AHRS中，DCM 常用于把加速度计、磁力计的观测矢量从 b 系转到 n 系做比对。

四、四元数：AHRS 内部递推的首选

四元数用一个标量和三个矢量分量表示旋转，记作 q = [q₀, q₁, q₂, q₃]^T，满足 q₀²+q₁²+q₂²+q₃²=1。它只有 4 个参数，无奇异，更新方程是双线性形式，计算量远小于 DCM，因此成为绝大多数AHRS内部姿态递推的状态量。其微分方程为：

q̇ = 0.5 · q ⊗ ω_b

其中 ω_b = [0, ωx, ωy, ωz] 为机体角速度四元数，
⊗ 表示四元数乘法。

离散实现时，常用一阶或二阶龙格-库塔积分，例如采样周期 T 下：q_k+1 = q_k + 0.5·T·(q_k⊗ω_b)，随后归一化。四元数本身不直观，但可以随时转成 DCM 或欧拉角供显示与判断。

五、三种表示的转换关系

三者是同一旋转的不同“语言”，转换关系必须记牢。由四元数到 DCM：

C_b^n =
[ 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²) ]

由 DCM 到欧拉角（NED、Z-Y-X）：θ = −arcsin(C₃₁)，φ = arctan2(C₃₂, C₃₃)，ψ = arctan2(C₂₁, C₁₁)。由欧拉角到四元数则按 Z-Y-X 顺序构造。实际工程中，AHRS 内部用四元数递推，输出前转成欧拉角，观测更新时转成 DCM，三者各司其职。

六、在AHRS中的实际选用与转换流程

把上述关系落到一个具体实现上，典型流程是：初始化时由加速度计和磁力计计算初始姿态，得到初始四元数 q₀；进入循环后，每个采样周期读取陀螺仪角速度 ω^b，用 q̇ = 0.5·q⊗ω_b 递推并归一化；随后把 q 转成 C_bⁿ，将加速度计测得的重力方向、磁力计测得的磁北方向从 b 系转到 n 系，与理论重力矢量 [0,0,1]^T 和理论磁矢量比对，得到误差；用该误差修正陀螺零偏或直接修正四元数；最后把修正后的 q 转成欧拉角输出。

理解这五种对象的分工，是后续姿态解算、传感器融合与误差分析的基础。欧拉角负责“给人看”，DCM 负责“做变换”，四元数负责“做递推”，三者通过固定的转换关系互相支撑，而坐标系约定则是这一切能够成立的前提。