PX4姿态解算技术详解（二）：姿态的表示方法

第2章 姿态估计的数学基础

在深入分析 PX4 的姿态估计算法之前，有必要先建立姿态的数学描述体系。正如《PX4姿态解算技术详解（一）》中所述，姿态的本质是坐标系之间的旋转变换。为了精确描述这种变换，数学上发展出了多种等价的表示方法。

PX4 的 matrix 数学库（源码位于 src/lib/matrix/matrix/）系统地封装了这些表示。Dcm、Quaternion、Euler、AxisAngle 四个模板类共享以下核心设计特征：

• 统一的坐标变换语义：均遵循 "frame 2 to frame 1" 的约定，即表示将向量从坐标系 2（通常是机体坐标系 body）变换到坐标系 1（通常是导航坐标系 navigation）；
• 模板化与类型安全 ：基于 C++ 模板实现，支持 float（带 f 后缀，如 Dcmf、Quatf、Eulerf、AxisAnglef）和 double（带 d 后缀）两种精度；
• 无缝的隐式转换 ：任意两种表示之间均支持直接的隐式构造和赋值，例如 Quatf q = Dcmf(...)、Eulerf e = q、AxisAnglef aa = e 等，极大地简化了多模块间的姿态数据流转。

本章将依次介绍这四种姿态表示。理解它们的数学定义、库实现以及在源码中的映射关系，是后续分析姿态解算与控制算法的前提。

---

2.1 姿态的表示方法：方向余弦矩阵（DCM）

方向余弦矩阵（Direction Cosine Matrix，DCM）是最基本、最直观的旋转表示。它以 3×33 \times 33×3 实矩阵的形式，直接揭示了旋转的线性代数本质，也是理解后续四元数、欧拉角表示的基础。在 PX4 中，DCM 主要用于理论推导、初始化构造以及部分坐标变换场景。

2.1.1 数学定义

设存在两个右手直角坐标系：参考坐标系（导航坐标系，记为 nnn）和刚体坐标系（机体坐标系，记为 bbb）。从机体坐标系到导航坐标系的旋转关系可以用一个 3×33 \times 33×3 的矩阵 Cbn\mathbf{C}_b^nCbn 表示：

Cbn=[ibnjbnkbn] \mathbf{C}_b^n = \begin{bmatrix} \mathbf{i}_b^n & \mathbf{j}_b^n & \mathbf{k}_b^n \end{bmatrix} Cbn=[ibnjbnkbn]

其中，矩阵的每一列都是机体坐标系某一基轴在导航坐标系中的单位投影向量。例如，第一列 ibn\mathbf{i}_b^nibn 表示机体 X 轴（前向）在导航系中的方向，第二列 jbn\mathbf{j}_b^njbn 表示机体 Y 轴（右向）在导航系中的方向，第三列 kbn\mathbf{k}_b^nkbn 表示机体 Z 轴（向下）在导航系中的方向。

DCM 的名称来源于其元素的物理意义：矩阵的第 iii 行第 jjj 列元素 cijc_{ij}cij 等于导航系第 iii 轴与机体系第 jjj 轴夹角的余弦值 ，即 cij=cos⁡(θij)c_{ij} = \cos(\theta_{ij})cij=cos(θij)。因此，Cbn\mathbf{C}_b^nCbn 的第一行记录了机体系三个轴与导航系北向轴（X轴）的夹角余弦；第二行记录了与东向轴（Y轴）的夹角余弦；第三行记录了与地向轴（Z轴）的夹角余弦。

从线性代数的角度看，Cbn\mathbf{C}_b^nCbn 也是一个坐标变换矩阵。若一个向量 v\mathbf{v}v 在机体坐标系下的坐标为 vb\mathbf{v}^bvb，则其在导航坐标系下的坐标 vn\mathbf{v}^nvn 可通过矩阵乘法得到：

vn=Cbn vb \mathbf{v}^n = \mathbf{C}_b^n \, \mathbf{v}^b vn=Cbnvb

反之，若要将导航系下的向量转换到机体系，则利用旋转矩阵的逆（由于旋转矩阵是正交矩阵，其逆等于转置）：

vb=(Cbn)Tvn=Cnb vn \mathbf{v}^b = \left( \mathbf{C}_b^n \right)^T \mathbf{v}^n = \mathbf{C}_n^b \, \mathbf{v}^n vb=(Cbn)Tvn=Cnbvn

2.1.2 基本性质与 SO(3)\mathrm{SO}(3)SO(3) 群结构

作为合法的旋转矩阵，DCM 必须满足以下数学约束：

正交性： 矩阵的列向量（以及行向量）两两正交且均为单位向量。用矩阵形式表示为：

(Cbn)TCbn=Cbn(Cbn)T=I \left( \mathbf{C}_b^n \right)^T \mathbf{C}_b^n = \mathbf{C}_b^n \left( \mathbf{C}_b^n \right)^T = \mathbf{I} (Cbn)TCbn=Cbn(Cbn)T=I

行列式为 +1： det⁡(Cbn)=+1\det(\mathbf{C}_b^n) = +1det(Cbn)=+1。这一条件保证了坐标变换是旋转变换（而非包含镜像反射的变换），维持了右手坐标系的定向。

保范性： 旋转变换不改变向量的长度，即对于任意向量 v\mathbf{v}v，都有 ∥Cbnv∥=∥v∥\| \mathbf{C}_b^n \mathbf{v} \| = \| \mathbf{v} \|∥Cbnv∥=∥v∥。

这些性质使得 DCM 属于三维特殊正交群 SO(3)\mathrm{SO}(3)SO(3)。在微分几何的视角下，所有 3×33 \times 33×3 的合法旋转矩阵构成了一个三维光滑流形，而 DCM 就是这个流形在 R3×3\mathbb{R}^{3 \times 3}R3×3 中的自然嵌入。后续介绍的四元数、欧拉角和轴角，本质上都是对这个同一流形的不同局部参数化（坐标图）。

2.1.3 PX4 数学库中的 Dcm 类

在 PX4 的 matrix 数学库中，方向余弦矩阵由模板类 matrix::Dcm 实现（常用别名 matrix::Dcmf）。该类继承自 SquareMatrix<Type, 3>，封装了 DCM 的构造、转换与正交化操作。

Dcm 类的核心设计遵循了严格的坐标变换语义约定。根据源码注释（src/lib/matrix/matrix/Dcm.hpp）：

"This library uses the convention that premultiplying a three dimensional vector represented in coordinate system 1 will apply a rotation from coordinate system 1 to coordinate system 2 to the vector. Likewise, a matrix instance of this class also represents a coordinate transformation from frame 2 to frame 1."

换言之，若 Dcmf 实例 R 的语义是从 body 到 navigation 的坐标变换（即 Cbn\mathbf{C}_b^nCbn），则执行 R * v_body 的结果即为 vnavigation\mathbf{v}^{navigation}vnavigation。

Dcm 类提供了多种构造方式，便于在不同姿态表示之间转换：

• 默认构造 ：Dcmf R; 初始化为 3×33 \times 33×3 单位矩阵，代表无旋转的恒等变换。
• 从四元数构造 ：Dcm(const Quaternion<Type> &q) 根据四元数元素直接计算出对应的 9 个矩阵元素，避免了三角函数运算，效率较高。
• 从欧拉角构造 ：Dcm(const Euler<Type> &euler) 基于 3-2-1（Z-Y-X）内旋顺序（Tait-Bryan 角）计算旋转矩阵。
• 从轴角构造 ：Dcm(const AxisAngle<Type> &aa) 先将轴角转为四元数，再转为 DCM。

此外，Dcm 还提供了 renormalize() 方法，用于对矩阵的每一行进行归一化。由于数值积分或传感器噪声可能导致 DCM 逐渐丧失正交性，定期执行正交化是工程实践中的常用手段。

2.1.4 在 PX4 代码中的典型使用

（1）基于加速度与磁力计的初始姿态构造

在 attitude_estimator_q 模块的初始化阶段，系统利用重力方向与地磁场方向在导航系中已知这一事实，通过 Gram-Schmidt 正交化过程直接构造出从导航系到机体系的旋转矩阵：

// 'k' 是地球 Z 轴（向下）在机体坐标系中的单位向量
Vector3f k = -_accel;
k.normalize();

// 'i' 是地球 X 轴（北向）在机体坐标系中的单位向量
// 通过将磁力计投影去除与 k 平行的分量，获得水平面分量
Vector3f i = (_mag - k * (_mag * k));
i.normalize();

// 'j' 是地球 Y 轴（东向）在机体坐标系中的单位向量，由叉乘保证正交
Vector3f j = k % i;

// 按行填充旋转矩阵：R 的每一行分别是 i, j, k（即导航系基轴在机体系中的投影）
Dcmf R;
R.row(0) = i;
R.row(1) = j;
R.row(2) = k;

// 将 DCM 转换为四元数，作为后续递推的初始值
_q = R;

（2）外部航向测量的坐标变换

当系统接入外部视觉或动作捕捉（Mocap）数据时，模块需要将外部系统给出的机体航向向量转换到机体坐标系下。此时利用了 DCM 的转置（即逆旋转）特性：

Dcmf Rmoc = Quatf(mocap.q);      // 从外部四元数构造 DCM
Vector3f v(1.0f, 0.0f, 0.4f);    // 世界坐标系下的参考向量

// Rmoc 表示从机体系到世界系的旋转（R_wr）
// 通过转置获得从世界系到机体系的变换
_mocap_hdg = Rmoc.transpose() * v;

2.1.5 本节小结

方向余弦矩阵是姿态表示的基石。它以 3×33 \times 33×3 实矩阵的形式，直观地刻画了两个坐标系之间的方向关系，并通过矩阵乘法直接实现向量的坐标变换。在 PX4 中，matrix::Dcm 类完整地封装了 DCM 的数学语义与工程操作。

然而，DCM 的 9 个元素中只有 3 个自由度，存在严重的信息冗余；且长时间的数值积分可能破坏正交约束，需要额外的正交化处理。为了克服这些缺陷，工程实践中广泛采用四元数作为更紧凑、更高效的旋转表示。下一节将详细介绍四元数的数学语言及其在 PX4 中的核心应用。

---

2.2 姿态的表示方法：四元数

为了解决 DCM 的冗余和正交维护问题，工程实践中广泛采用四元数（Quaternion）作为姿态的紧凑表示。四元数只需 4 个参数，无奇异性，便于插值与递推，是 PX4 飞行控制中首选的姿态数学工具。在 PX4 的架构中，四元数是姿态估计模块的输出标准、控制模块的输入标准，也是日志与地面站记录的标准形式。

2.2.1 数学定义

四元数由爱尔兰数学家哈密顿（Hamilton）于 1843 年提出，是复数向三维空间的自然推广。一个四元数 q\mathbf{q}q 包含一个实部（标量）和三个虚部（向量）：

q=q0+q1i+q2j+q3k=[q0qv] \mathbf{q} = q_0 + q_1 \mathbf{i} + q_2 \mathbf{j} + q_3 \mathbf{k} = \begin{bmatrix} q_0 \\ \mathbf{q}_v \end{bmatrix} q=q0+q1i+q2j+q3k=[q0qv]

其中 qv=[q1,q2,q3]T\mathbf{q}_v = [q_1, q_2, q_3]^Tqv=[q1,q2,q3]T 为虚部向量。PX4 的 matrix 库将四元数存储为四维列向量，约定第一个元素为实部，即：

q=[q0q1q2q3]T \mathbf{q} = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}^T q=[q0q1q2q3]T

表示零旋转的单位四元数为 q=[1,0,0,0]T\mathbf{q} = [1, 0, 0, 0]^Tq=[1,0,0,0]T。

Hamilton 积

两个四元数的乘积 p⊗q\mathbf{p} \otimes \mathbf{q}p⊗q（在代码中直接用 * 运算符）遵循 Hamilton 规则：

p⊗q=[p0q0−pv⋅qvp0qv+q0pv+pv×qv] \mathbf{p} \otimes \mathbf{q} = \begin{bmatrix} p_0 q_0 - \mathbf{p}_v \cdot \mathbf{q}_v \\ p_0 \mathbf{q}_v + q_0 \mathbf{p}_v + \mathbf{p}_v \times \mathbf{q}_v \end{bmatrix} p⊗q=[p0q0−pv⋅qvp0qv+q0pv+pv×qv]

在 PX4 的源码注释（Quaternion.hpp）中特别强调了这一约定：

"The product zzz of two quaternions z=q2∗q1z = q_2 * q_1z=q2∗q1 represents an intrinsic rotation in the order of first q1q_1q1 followed by q2q_2q2."

这意味着，若先执行旋转 q1\mathbf{q}_1q1 再执行 q2\mathbf{q}_2q2，则复合旋转为 q=q2⊗q1\mathbf{q} = \mathbf{q}_2 \otimes \mathbf{q}1q=q2⊗q1。这与 DCM 的复合顺序 Ctotal=C2C1C{total} = C_2 C_1Ctotal=C2C1 完全一致。

共轭与逆

四元数的共轭定义为实部不变、虚部取反：

q∗=[q0−qv] \mathbf{q}^* = \begin{bmatrix} q_0 \\ -\mathbf{q}_v \end{bmatrix} q∗=[q0−qv]

对于单位四元数（姿态表示中总是单位化的），其逆等于共轭：

q−1=q∗∥q∥2 \mathbf{q}^{-1} = \frac{\mathbf{q}^*}{\|\mathbf{q}\|^2} q−1=∥q∥2q∗

当 ∥q∥=1\|\mathbf{q}\| = 1∥q∥=1 时，简化为 q−1=q∗\mathbf{q}^{-1} = \mathbf{q}^*q−1=q∗。这一性质使得求逆运算极其高效，只需改变三个虚部符号即可。

2.2.2 与三维旋转及 DCM 的关系

四元数与单位四元数之间存在一一对应关系。设单位四元数 qnb\mathbf{q}_{nb}qnb 表示从机体系 bbb 到导航系 nnn 的旋转，则机体系下的向量 vb\mathbf{v}^bvb 可通过三明治乘积变换到导航系：

vn=qnb⊗[0vb]⊗qnb−1 \mathbf{v}^n = \mathbf{q}{nb} \otimes \begin{bmatrix} 0 \\ \mathbf{v}^b \end{bmatrix} \otimes \mathbf{q}{nb}^{-1} vn=qnb⊗[0vb]⊗qnb−1

这与 DCM 的坐标变换 vn=Cbnvb\mathbf{v}^n = \mathbf{C}_b^n \mathbf{v}^bvn=Cbnvb 在数学上完全等价。将四元数展开为矩阵形式，即可得到对应的 DCM 元素（如 Dcm.hpp 中的 Dcm(const Quaternion<Type> &q) 构造器所示）。

从几何上看，单位四元数还可以表示为转轴-转角 形式：若绕单位轴 e\mathbf{e}e 旋转角度 θ\thetaθ，则：

q=[cos⁡(θ/2)esin⁡(θ/2)] \mathbf{q} = \begin{bmatrix} \cos(\theta/2) \\ \mathbf{e} \sin(\theta/2) \end{bmatrix} q=[cos(θ/2)esin(θ/2)]

这种表示没有欧拉角的万向锁奇异性，且绕任意轴的旋转都可以简洁表达。

2.2.3 PX4 数学库中的 Quaternion 类

PX4 的 matrix::Quaternion 模板类（常用别名 matrix::Quatf）继承自 Vector4<Type>，将四元数的数学运算高度工程化。与 Dcm 类一样，它遵循 "frame 2 to frame 1" 的坐标变换语义。

存储约定

• 标量在前 ：q(0) 为实部 www，q(1)、q(2)、q(3) 为虚部 x,y,zx, y, zx,y,z。
• 零旋转 ：Quaternion() 默认构造为 [1,0,0,0][1, 0, 0, 0][1,0,0,0]。

构造方式

Quaternion 类提供了丰富的构造方式，使其成为 PX4 中姿态表示的统一入口：

构造方式	代码示例	说明
默认构造	Quatf q;	单位四元数 [1,0,0,0][1,0,0,0][1,0,0,0]
从数组	Quatf q(data);	data[4] 直接拷贝
从 DCM	Quatf q(R);	基于矩阵迹的数值稳定转换算法
从欧拉角	Quatf q(Eulerf(phi, theta, psi));	Z-Y-X 内旋顺序（Tait-Bryan）
从轴角	Quatf q(AxisAnglef(axis, angle));	转轴-转角直接映射
从双向量	Quatf q(src, dst);	构造从 src 到 dst 的最短旋转弧
从分量	Quatf q(w, x, y, z);	直接指定四个元素

其中，从 DCM 构造四元数的算法采用基于迹的分支判断，有效避免了开方中出现负数的数值问题，是工程实现中的经典方法。

核心方法

• operator* ：Hamilton 四元数乘法，满足内旋复合顺序 q2∗q1q_2 * q_1q2∗q1。
• inversed() / invert()：求逆，对单位四元数等价于取共轭。
• canonical() / canonicalize() ：规范化，确保实部非负（避免 qqq 与 −q-q−q 表示同一旋转导致的符号歧义）。
• rotateVector(v) ：将向量从机体系旋转到导航系（三明治乘积 qvq−1q v q^{-1}qvq−1）。
• rotateVectorInverse(v) ：将向量从导航系旋转到机体系（q−1vqq^{-1} v qq−1vq）。
• dcm_z()：直接提取等效旋转矩阵的第三列（机体 Z 轴在导航系中的方向），无需完整构造 DCM，计算量极小。
• derivative1(w) / derivative2(w)：计算机体系/导航系角速度对应的四元数导数，用于姿态积分。
• expq(u)：四元数指数映射，用于更高精度的姿态积分。

2.2.4 在 PX4 代码中的典型使用

（1）姿态初始化：从 DCM 到四元数

在 attitude_estimator_q 模块的初始化阶段，系统先用加速度计和磁力计构造出 DCM R，然后通过隐式转换得到初始四元数：

Dcmf R;
R.row(0) = i;
R.row(1) = j;
R.row(2) = k;

_q = R;          // 调用 Quaternion(const Dcm<Type>&) 构造器
_q.normalize();  // 确保单位化

这体现了四元数作为姿态表示统一入口的作用：无论数据来自 DCM、欧拉角还是外部传感器，最终都统一转换为四元数形式存储和传递。

（2）坐标变换：rotateVector 与 rotateVectorInverse

四元数在 PX4 中最频繁的使用场景是向量在不同坐标系间的旋转：

// 机体系 -> 导航系
Vector3f mag_earth = _q.rotateVector(_mag);
Vector3f vision_hdg_earth = _q.rotateVector(_vision_hdg);

// 导航系 -> 机体系
Vector3f k = _q.rotateVectorInverse(Vector3f(0.0f, 0.0f, 1.0f));

rotateVectorInverse 利用了单位四元数逆等于共轭的特性，以 O(1)O(1)O(1) 的符号代价完成了逆旋转，无需任何矩阵求逆或转置运算。

（3）旋转的复合：磁偏角修正

当需要叠加一个额外的偏航旋转时，PX4 构造一个绕 Z 轴的欧拉角四元数，与当前姿态相乘：

Quatf decl_rotation = Eulerf(0.0f, 0.0f, _mag_decl);
_q = _q * decl_rotation;

四元数乘法的非交换性与旋转的内旋顺序天然吻合，使得复合运算在代码中非常自然。

（4）从四元数快速提取机体 Z 轴

在多旋翼姿态控制模块 mc_att_control 中，dcm_z() 用于快速获取机体 Z 轴方向（推力方向），而无需构造完整的 DCM：

const Vector3f e_z = q.dcm_z();        // 当前姿态的机体 Z 轴在 NED 中的投影
const Vector3f e_z_d = qd.dcm_z();     // 期望姿态的机体 Z 轴投影

2.2.5 本节小结

四元数以紧凑的 4 元结构完整描述了 3 维旋转，避免了 DCM 的冗余正交约束问题，也避免了欧拉角的万向锁奇异性。PX4 的 matrix::Quaternion 类严格遵循 Hamilton 约定，提供了从构造、乘法、求逆到向量旋转的一整套高效工具。

在 PX4 的工程实现中，四元数是姿态的标准表示形式：姿态估计模块输出四元数，控制模块读取四元数，地面站与日志系统也以四元数记录姿态。

然而，四元数的四个元素对人类来说几乎没有直接的可读性。对于飞行员、调试工程师以及地面站操作人员而言，最直观的姿态描述仍然是横滚角、俯仰角和偏航角。下一节将介绍的欧拉角，正是填补这一人机交互空缺的工具。

---

2.3 姿态的表示方法：欧拉角

欧拉角（Euler Angles）以横滚角（Roll, ϕ\phiϕ） 、俯仰角（Pitch, θ\thetaθ）和 偏航角（Yaw, ψ\psiψ）三个角度，为人类提供了最直观的旋转可读性。它是无人机姿态在人机交互层面的主要表示形式。然而，欧拉角在数学上存在固有的 万向锁（Gimbal Lock）奇异性，且三轴旋转的非交换性导致复合运算复杂。因此，在 PX4 的设计哲学中，欧拉角不直接参与核心的状态估计与控制运算，而是作为四元数/DCM 的"可解释输出"。

2.3.1 数学定义

在三维空间中，任意两个右手直角坐标系之间的旋转可以通过连续三次绕坐标轴的旋转 来实现。由于旋转顺序的不同，欧拉角存在多种定义方式。PX4 的 matrix::Euler 类严格采用航空领域最常用的 3-2-1 内旋顺序（Z-Y-X，Tait-Bryan 角）：

1. 绕初始 Z 轴旋转角度 ψ\psiψ（Yaw / 偏航角）
2. 绕新产生的 Y' 轴旋转角度 θ\thetaθ（Pitch / 俯仰角）
3. 绕新产生的 X'' 轴旋转角度 ϕ\phiϕ（Roll / 横滚角）

用旋转矩阵表示，这一序列的复合旋转为：

Cbn=Cx(ϕ) Cy(θ) Cz(ψ) \mathbf{C}_b^n = \mathbf{C}_x(\phi) \, \mathbf{C}_y(\theta) \, \mathbf{C}_z(\psi) Cbn=Cx(ϕ)Cy(θ)Cz(ψ)

按上述顺序相乘后，得到完整的 DCM 表达式：

Cbn=[cθcψ−cϕsψ+sϕsθcψsϕsψ+cϕsθcψcθsψcϕcψ+sϕsθsψ−sϕcψ+cϕsθsψ−sθsϕcθcϕcθ] \mathbf{C}_b^n = \begin{bmatrix} c\theta c\psi & -c\phi s\psi + s\phi s\theta c\psi & s\phi s\psi + c\phi s\theta c\psi \\ c\theta s\psi & c\phi c\psi + s\phi s\theta s\psi & -s\phi c\psi + c\phi s\theta s\psi \\ -s\theta & s\phi c\theta & c\phi c\theta \end{bmatrix} Cbn= cθcψcθsψ−sθ−cϕsψ+sϕsθcψcϕcψ+sϕsθsψsϕcθsϕsψ+cϕsθcψ−sϕcψ+cϕsθsψcϕcθ

（式中 c⋅c\cdotc⋅ 表示 cos⁡\coscos，s⋅s\cdots⋅ 表示 sin⁡\sinsin。）

2.3.2 万向锁与奇异性

欧拉角表示存在一个根本性的数学缺陷：当俯仰角 θ=±90°\theta = \pm 90°θ=±90° 时，第一次绕 Z 轴的旋转和第三次绕 X 轴的旋转将发生在同一平面内，导致横滚角和偏航角无法独立区分 。这一现象称为万向锁（Gimbal Lock）。

从 DCM 的表达式可以直观看出：当 θ=±π/2\theta = \pm \pi/2θ=±π/2 时，cos⁡θ=0\cos\theta = 0cosθ=0，矩阵第一行和第二行将退化为只含有 (ϕ±ψ)(\phi \pm \psi)(ϕ±ψ) 的组合项，无法单独反解出 ϕ\phiϕ 和 ψ\psiψ。此时，系统丢失了一个自由度，从三维旋转流形 SO(3)\mathrm{SO}(3)SO(3) 的局部参数化退化为一个二维子空间。

对于固定翼或垂直起降飞行器，大角度俯仰机动会直接触发万向锁区域。因此，PX4 的控制核心不使用欧拉角作为反馈状态，而仅将其用于：

• 地面站显示与日志记录
• 人机遥控接口的摇杆映射
• 特定模式中的简化航向提取
• 测试验证与参数标定

2.3.3 PX4 数学库中的 Euler 类

PX4 的 matrix::Euler 模板类（常用别名 matrix::Eulerf）继承自 Vector<Type, 3>，将欧拉角的存储与转换操作进行了轻量级封装。

存储约定

三个角度按顺序存储在三维向量中：

• phi() = (*this)(0)：横滚角 ϕ\phiϕ，绕 X 轴旋转
• theta() = (*this)(1)：俯仰角 θ\thetaθ，绕 Y 轴旋转
• psi() = (*this)(2)：偏航角 ψ\psiψ，绕 Z 轴旋转

所有角度均以**弧度（rad）**为单位。

构造方式

构造方式	代码示例	说明
默认构造	Eulerf e;	未初始化
从三个角度	Eulerf e(phi, theta, psi);	直接指定横滚、俯仰、偏航
从 DCM	Eulerf e(R);	从方向余弦矩阵反解欧拉角，带万向锁分支保护
从四元数	Eulerf e(q);	先隐式转为 DCM，再反解欧拉角

从 DCM 反解欧拉角的安全实现

Euler(const Dcm<Type> &dcm) 是工程实现中最需要注意的构造器。其源码清晰地处理了万向锁情况：

theta() = std::asin(-dcm(2, 0));

if ((std::fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) {
    phi() = 0;
    psi() = std::atan2(dcm(1, 2), dcm(0, 2));

} else if ((std::fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) {
    phi() = 0;
    psi() = std::atan2(-dcm(1, 2), -dcm(0, 2));

} else {
    phi() = std::atan2(dcm(2, 1), dcm(2, 2));
    psi() = std::atan2(dcm(1, 0), dcm(0, 0));
}

逻辑解读：通过 θ=arcsin⁡(−R31)\theta = \arcsin(-R_{31})θ=arcsin(−R31) 提取俯仰角；当 θ\thetaθ 接近 ±90°\pm 90°±90° 时（阈值 1.0e-3 rad），将横滚角强制置零，偏航角解为等效组合角；非奇异区域则通过 atan2 分别提取 ϕ\phiϕ 和 ψ\psiψ。

2.3.4 在 PX4 代码中的典型使用

（1）构造简单旋转四元数

欧拉角最常见的用途是作为构造绕单轴旋转四元数的便捷中间表示。例如，磁偏角修正只需要一个绕导航系 Z 轴的纯偏航旋转：

Quatf decl_rotation = Eulerf(0.0f, 0.0f, _mag_decl);
_q = _q * decl_rotation;

相比直接手写四元数公式，这种方式代码可读性更高，物理意义也更明确。

（2）从姿态中提取偏航角

虽然控制律不基于欧拉角，但上层逻辑常需单独获取航向。例如，多旋翼姿态控制中手动飞行模式需要当前航向：

const float yaw = Eulerf(q).psi();

以及姿态估计器发生状态重置时，提取重置前后的偏航差异：

const float delta_psi = Eulerf(delta_q_reset).psi();

这些场景的共同特点是：只关心偏航角，不依赖完整的欧拉角控制结构 。由于俯仰角通常不会接近 90°90°90°，万向锁风险可以忽略。

（3）日志输出与调试显示

在 EKF2 的输出预测器中，四元数姿态经常被转换为欧拉角用于打印：

const matrix::Eulerf euler = q_att;
printf("roll: %.4f, pitch: %.4f, yaw: %.4f\n",
       (double)euler.phi(), (double)euler.theta(), (double)euler.psi());

（4）地磁场模型的坐标旋转

在 EKF2 的磁力计融合中，地磁场模型给出的参数是磁偏角和磁倾角。为了构造参考地磁场向量，代码中使用欧拉角构造 DCM：

_wmm_earth_field_gauss = Dcmf(Eulerf(0, -inclination_rad, declination_rad))
                         * Vector3f(strength_gauss, 0, 0);

2.3.5 本节小结

欧拉角以人类最直观的方式描述了三维旋转，是地面站显示、日志记录和简单旋转构造不可或缺的工具。然而，其固有的万向锁奇异性和非交换性决定了它无法胜任高速实时控制与复杂姿态估计的核心计算任务。

在 PX4 中，matrix::Eulerf 类提供了从 DCM 和四元数安全反解欧拉角的工程实现，特别是在万向锁边界附近通过分支判断保证了数值稳定性。开发者应遵循 PX4 的设计哲学：用四元数做运算，用欧拉角做展示。

除了 DCM、四元数和欧拉角之外，三维旋转还有一种在几何上极为直观、同时作为连接物理测量与数学状态桥梁的表示方式------轴角。下一节将对其进行详细介绍。

---

2.4 姿态的表示方法：轴角

轴角（Axis-Angle）表示的核心思想是：三维空间中的任何一个旋转，都可以等效为绕某一单位向量（轴）旋转一个特定角度 。这种表示与人类对"转动了多少、绕着什么转"的直觉完全吻合，同时也是连接四元数与旋转矩阵的一座重要桥梁。在 PX4 的工程实践中，轴角是连接物理传感器测量 （陀螺仪角增量）与数学姿态状态（四元数）的关键中间表示。

2.4.1 数学定义

设单位向量 e=[ex,ey,ez]T\mathbf{e} = [e_x, e_y, e_z]^Te=[ex,ey,ez]T（满足 ∥e∥=1\|\mathbf{e}\| = 1∥e∥=1）为旋转轴，θ\thetaθ 为绕该轴的旋转角度（右手定则），则这一对 (e,θ)(\mathbf{e}, \theta)(e,θ) 就构成了一个轴角表示。

为了便于计算和存储，工程上通常将轴角编码为一个三维向量：

Φ=θ e=[θexθeyθez] \mathbf{\Phi} = \theta \, \mathbf{e} = \begin{bmatrix} \theta e_x \\ \theta e_y \\ \theta e_z \end{bmatrix} Φ=θe= θexθeyθez

这个向量的方向 即为旋转轴，模长即为旋转角度：

∥Φ∥=θ,Φ∥Φ∥=e \|\mathbf{\Phi}\| = \theta, \quad \frac{\mathbf{\Phi}}{\|\mathbf{\Phi}\|} = \mathbf{e} ∥Φ∥=θ,∥Φ∥Φ=e

当 θ=0\theta = 0θ=0 时，表示没有旋转，此时 Φ\mathbf{\Phi}Φ 为零向量，旋转轴无定义。

2.4.2 与四元数和 DCM 的关系

轴角与单位四元数之间存在直接且简洁的映射关系：

q=[cos⁡(∥Φ∥/2)Φ∥Φ∥sin⁡(∥Φ∥/2)] \mathbf{q} = \begin{bmatrix} \cos(\|\mathbf{\Phi}\|/2) \\ \frac{\mathbf{\Phi}}{\|\mathbf{\Phi}\|} \sin(\|\mathbf{\Phi}\|/2) \end{bmatrix} q=[cos(∥Φ∥/2)∥Φ∥Φsin(∥Φ∥/2)]

反过来，从单位四元数到轴角的反解为：

θ=2arctan⁡(∥qv∥q0),e=qv∥qv∥ \theta = 2 \arctan\left( \frac{\|\mathbf{q}_v\|}{q_0} \right), \quad \mathbf{e} = \frac{\mathbf{q}_v}{\|\mathbf{q}_v\|} θ=2arctan(q0∥qv∥),e=∥qv∥qv

当旋转角度很小时（θ≪1\theta \ll 1θ≪1），上述映射可以线性近似为：

q≈[1Φ/2] \mathbf{q} \approx \begin{bmatrix} 1 \\ \mathbf{\Phi}/2 \end{bmatrix} q≈[1Φ/2]

这一近似在 PX4 的 IMU 积分、误差状态更新等场景中有着极其重要的应用------它意味着小角度旋转可以直接用轴角的一半来近似四元数的虚部。

轴角到 DCM 的转换由 Rodrigues 旋转公式 给出。设 Φ∧\mathbf{\Phi}^\wedgeΦ∧ 为向量 Φ\mathbf{\Phi}Φ 的反对称矩阵（hat 算子），则：

C=I+sin⁡θθΦ∧+1−cos⁡θθ2(Φ∧)2 \mathbf{C} = \mathbf{I} + \frac{\sin\theta}{\theta} \mathbf{\Phi}^\wedge + \frac{1 - \cos\theta}{\theta^2} (\mathbf{\Phi}^\wedge)^2 C=I+θsinθΦ∧+θ21−cosθ(Φ∧)2

当 θ\thetaθ 很小时，C≈I+Φ∧\mathbf{C} \approx \mathbf{I} + \mathbf{\Phi}^\wedgeC≈I+Φ∧。这再次印证了轴角在小角度下的简洁性。

2.4.3 PX4 数学库中的 AxisAngle 类

PX4 的 matrix::AxisAngle 模板类（常用别名 matrix::AxisAnglef）继承自 Vector3<Type>，本质上就是一个三维向量，其方向为旋转轴，模长为旋转角度。

存储约定

• 继承 Vector3f 的存储结构：v(0)、v(1)、v(2) 分别对应 Φx\Phi_xΦx、Φy\Phi_yΦy、Φz\Phi_zΦz。
• 模长 = 角度 ：v.norm() 返回旋转角 θ\thetaθ（弧度）。
• 方向 = 转轴 ：v.unit() 返回单位旋转轴 e\mathbf{e}e。

构造方式

构造方式	代码示例	说明
默认构造	AxisAnglef aa;	未初始化的三维向量
从数组/向量	AxisAnglef aa(data);	直接拷贝三维向量
从分量	AxisAnglef aa(x, y, z);	直接指定轴角向量的三个元素
从轴+角	AxisAnglef aa(axis, angle);	axis 会被自动归一化，再乘以 angle
从四元数	AxisAnglef aa(q);	反解四元数得到轴角，小角度有数值保护
从 DCM	AxisAnglef aa(R);	先转四元数，再反解轴角
从欧拉角	AxisAnglef aa(euler);	先转四元数，再反解轴角

其中，从四元数反解轴角的算法实现特别处理了小角度情况：

Type mag = q.imag().norm();
if (std::fabs(mag) >= Type(1e-10)) {
    v = q.imag() * Type(Type(2) * std::atan2(mag, q(0)) / mag);
} else {
    v = q.imag() * Type(Type(2) * Type(sign(q(0))));
}

这段代码的数学含义正是 Φ=2arctan⁡(∥qv∥/q0)⋅qv∥qv∥\mathbf{\Phi} = 2 \arctan(\|\mathbf{q}_v\|/q_0) \cdot \frac{\mathbf{q}_v}{\|\mathbf{q}_v\|}Φ=2arctan(∥qv∥/q0)⋅∥qv∥qv。当虚部极小时，用 sign(q(0)) 保证符号正确，避免除以零。

核心方法

• axis() ：返回单位旋转轴。若当前向量为零向量，则默认返回 X 轴 [1, 0, 0]，避免无定义问题。
• angle() ：返回旋转角的绝对值，即向量的 L2 范数 norm()。

2.4.4 在 PX4 代码中的典型使用

（1）IMU 角增量到四元数：EKF2 的核心运算

IMU 的陀螺仪在每个采样周期内测量的是机体角速度 ω\boldsymbol{\omega}ω。若采样周期为 Δt\Delta tΔt，则角增量（即小角度轴角）为 Δθ=ω Δt\Delta \boldsymbol{\theta} = \boldsymbol{\omega} \, \Delta tΔθ=ωΔt。

在 EKF2 的姿态更新中，这一角增量被直接视为轴角，并转换为四元数的微分旋转：

// EKF2 output_predictor.cpp
const Quatf dq(AxisAnglef{delta_angle_corrected});
_q_att = _q_att * dq;

以及：

// EKF2 ekf.cpp
const Quatf dq(AxisAnglef{corrected_delta_ang});
q_new = q_old * dq;

这段代码是 PX4 状态估计中最频繁执行的运算之一。其数学本质是：将陀螺仪在一个周期内的积分结果 Δθ\Delta \boldsymbol{\theta}Δθ 视为轴角，通过 AxisAnglef 到 Quatf 的隐式转换，得到对应的四元数微分旋转 dq，再与上一时刻的姿态四元数复合。

（2）固定翼与多旋翼的姿态指令构造

在固定翼姿态控制中，根据机体横滚和俯仰指令构造姿态四元数：

// fw_att_control
const Quatf q_sp_rp = AxisAnglef(roll_body, pitch_body, 0.f);

这里 AxisAnglef(roll_body, pitch_body, 0.f) 构造了一个轴角向量，代表绕机体系各轴分别旋转对应角度，随后隐式转为四元数。

多旋翼姿态控制中从角速度设定值构造姿态变化：

// mc_att_control
_quat_state = _quat_state * Quatf(AxisAnglef(rate_setpoint_new));

（3）倾转分离与最短旋转角度量

在多旋翼的几何控制中，经常需要将期望姿态分解为倾斜（Tilt）和偏航（Yaw） 。AxisAnglef 提供了一种优雅的分析工具：

const Quatf q_sp_yaw = AxisAnglef(z_unit, -1.23f);

以及通过轴角反解来比较两个四元数之间的倾斜角差异：

EXPECT_FLOAT_EQ(AxisAnglef(q_tilt_sp_ne_before).angle(),
                AxisAnglef(q_tilt_sp_ne).angle());

AxisAnglef(q).angle() 直接得到两个姿态之间的最小旋转角 （geodesic distance on SO(3)），而 axis() 给出了实现这一最短旋转的转轴。

（4）传感器校准中的角度误差度量

在加速度计校准和磁力计校准模块中，轴角被用来度量两组向量之间的偏离程度：

// accelerometer_calibration.cpp
const float angle = AxisAnglef(Quatf(accel_avg, accel_ref)).angle();

// VehicleMagnetometer.cpp
float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle();

Quatf(src, dst) 构造的是从 src 到 dst 的最短旋转四元数，再用 AxisAnglef 反解即可得到整体角度误差。

（5）垂起固定翼（VTOL）的过渡旋转

在 tailsitter 的过渡模式中，轴角用于构造过渡期间的期望旋转：

// tailsitter.cpp
_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis, _trans_roll_min));

2.4.5 本节小结

轴角表示以"绕什么轴、转多少度"这一最直观的方式刻画了三维旋转，同时与四元数和旋转矩阵之间存在简洁明确的数学映射。在 PX4 的工程实现中，matrix::AxisAnglef 是连接物理传感器测量 与数学姿态状态的关键中间表示，支撑了 EKF2 的姿态积分、控制器的姿态指令构造、传感器校准等大量核心功能。

---

2.5 四种姿态表示的对比与 PX4 的选择

DCM、四元数、欧拉角和轴角并非相互替代，而是在 PX4 的不同环节中协同工作。下表从数学与工程两个维度对它们进行了系统对比：

特性	方向余弦矩阵（DCM）	四元数（Quaternion）	欧拉角（Euler）	轴角（Axis-Angle）
参数个数	9	4	3	3
自由度冗余	高（仅 3 个独立自由度）	低（1 个约束）	无冗余	无冗余
奇异性	无	无	有（万向锁）	无（零旋转除外）
约束维护	需显式正交化	只需归一化	无需维护	无需维护
复合运算	矩阵乘法（27 次乘加）	Hamilton 积（16 次乘加）	无封闭形式	无封闭形式
向量旋转	矩阵-向量乘（9 次乘加）	三明治乘积/优化后约 18 次	不直接支持	需先转矩阵或四元数
直观可读性	差	差	好	好
小角度近似	不适用	q≈[1,Φ/2]T\mathbf{q} \approx [1, \mathbf{\Phi}/2]^Tq≈[1,Φ/2]T	不适用	C≈I+Φ∧\mathbf{C} \approx \mathbf{I} + \mathbf{\Phi}^\wedgeC≈I+Φ∧
PX4 核心用途	坐标变换、初始化	状态估计与控制的核心表示	显示、日志、简单旋转构造	IMU 积分、姿态指令、误差度量

PX4 的设计哲学

基于上述分析，PX4 在工程实现中形成了清晰的分工策略：

• DCM：旋转的"完整矩阵形式"，是理论推导和初始化的基础；
• 四元数：旋转的"工程标准形式"，是状态估计与控制运算的核心；
• 欧拉角：旋转的"人机交互形式"，是显示、记录和简单参数化的工具；
• 轴角：旋转的"物理直观形式"，是连接传感器测量与四元数状态的桥梁。

在 PX4 的源码中，这四种表示通过 matrix 库的无缝隐式转换形成了高度统一的姿态处理 pipeline。例如，一个典型的数据流可能是：

传感器数据（加速度、磁力计）
    ↓
构造 DCM（初始化）
    ↓
隐式转为 Quatf（标准状态）
    ↓
用 AxisAnglef 积分 IMU 角增量（预测更新）
    ↓
用 Eulerf 反解出 roll/pitch/yaw（日志显示）

掌握了这四种表示方法的数学定义、相互转换关系以及在 PX4 源码中的具体实现，读者就已经具备了深入理解后续姿态解算与控制算法的完整数学基础。

联系我们：wx（LingZhiLab）

关于我们：灵智实验室（LingzhiLab）成立于2020年，核心团队源自西北工业大学，由一群深耕无人系统、自动控制与机器人技术的青年工程师与科研人员组成。我们始终秉持"开放、协同、智能、可靠"的理念，致力于推动无人智能体在复杂环境下的自主感知、决策与控制能力。

实验室聚焦于基于开源飞控（如PX4）与ROS 2的深度融合，构建高可靠、模块化、可扩展的无人系统软件架构。依托扎实的工程实践与学术背景，灵智实验室积极参与开源社区建设，助力科研教育与产业落地。