PX4姿态控制数学基础：欧拉角变化率与角速度的本质关系深度解析

摘要

在多旋翼飞控系统的姿态控制中,欧拉角变化率与角速度是两个容易混淆但本质不同的概念。本文从李群微分几何、刚体动力学和工程实现三个层面,系统阐述两者的数学物理关系,揭示PX4采用四元数+角速度架构的深层原因,为飞控系统设计提供理论支撑。

关键词: 姿态控制、李群SO(3)、欧拉角、角速度、四元数、PX4

---

1. 引言:核心问题与常见误区

1.1 三个关键概念的辨析

在姿态控制系统中,我们经常遇到三个相关但不同的概念:

概念	数学符号	类型	物理意义	测量方式
欧拉角变化率	$[\dot{\varphi },\dot{\theta },\dot{\psi }]$	矢量	欧拉角参数的时间导数	间接计算
角速度	$[{\omega }_x,{\omega }_y,{\omega }_z]$	矢量	机体相对惯性系的瞬时旋转速度	陀螺仪直接测量
角速率	$\omega =|\mathbf{\omega }|$	标量	角速度矢量的模长	计算得出

1.2 常见误区澄清

误区1: 俯仰角变化率等于俯仰角速度,即 $\dot{\theta }={\omega }_y$

真相: 一般情况下 ${\omega }_y\ne \dot{\theta }$,两者通过复杂的非线性变换相关:
$${\omega }_y=\cos \varphi \cdot \dot{\theta }+\sin \varphi \cos \theta \cdot \dot{\psi }$$

误区2: 角速率等于姿态角变化率的模长

真相:
$$\Vert \mathbf{\omega }\Vert \ne \Vert [\dot{\varphi },\dot{\theta },\dot{\psi }]\Vert$$
两者之间的关系依赖于当前姿态,不存在简单的代数关系。

误区3: 三个姿态通道可以独立控制

真相: 由于欧拉角序贯旋转的本质,三个通道高度耦合,无法独立处理。

---

2. 数学理论基础:李群SO(3)的微分结构

2.1 旋转群SO(3)的几何本质

特殊正交群SO(3)定义为保持欧几里得内积不变的旋转变换集合:

SO(3)={R∈R3×3∣RTR=I,det⁡(R)=1}\text{SO}(3) = \{R \in \mathbb{R}^{3\times 3} \mid R^T R = I, \det(R) = 1\}SO(3)={R∈R3×3∣RTR=I,det(R)=1}

这个定义蕴含深刻的几何意义:

• 保内积: 旋转不改变向量长度和夹角
• 行列式为1: 排除反射变换,保证方向性
• 流形结构: SO(3)是三维光滑流形,同胚于实射影空间${\mathbb{RP}}^3$

SO(3)的拓扑性质直接导致了姿态表示的固有困难:

• 基本群 ${\pi }_1(\text{SO}(3))={\mathbb{Z}}_2$ 暗示了双覆盖性质
• 不存在全局坐标系: 任何参数化(如欧拉角)都必然存在奇异点
• 旋转非交换性: $R_1{R}_2\ne R_2{R}_1$ (一般情况)

2.2 李代数so(3)与角速度的同构

李代数so(3)是SO(3)在单位元处的切空间,由所有3×3反对称矩阵构成:

so(3)={Ω∈R3×3∣ΩT=−Ω}\text{so}(3) = \{\Omega \in \mathbb{R}^{3\times 3} \mid \Omega^T = -\Omega\}so(3)={Ω∈R3×3∣ΩT=−Ω}

帽子映射(hat map)建立了角速度向量与李代数的自然同构:

$$\hat{}:{\mathbb{R}}^3\to \text{so}(3),\mathbf{\omega }=\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]\mapsto [\mathbf{\omega }{]}_{\times }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$

这个映射的物理意义在于:

• $[\mathbf{\omega }{]}_{\times }\mathbf{v}=\mathbf{\omega }\times \mathbf{v}$ (叉积的矩阵表示)
• 李括号对应角速度叉积: $[[{\mathbf{\omega }}_1{]}_{\times },[{\mathbf{\omega }}_2{]}_{\times }]=[{\mathbf{\omega }}_1\times {\mathbf{\omega }}_2{]}_{\times }$
• 编码了角速度合成的非交换性

2.3 切映射与参数化

欧拉角 $(\varphi ,\theta ,\psi )$ 提供了SO(3)的局部坐标卡:

$$R(\varphi ,\theta ,\psi )=R_z(\psi )R_y(\theta )R_x(\varphi )$$

设参数空间曲线 $\gamma (t)=(\varphi (t),\theta (t),\psi (t))$,对应的旋转矩阵曲线为 $R(t)=R(\gamma (t))$。通过链式法则:

$$\dot{R}=\frac{\partial R}{\partial \varphi }\dot{\varphi }+\frac{\partial R}{\partial \theta }\dot{\theta }+\frac{\partial R}{\partial \psi }\dot{\psi }$$

角速度定义为:
$$[\mathbf{\omega }{]}_{\times }=R^T\dot{R}$$

这就是切映射(tangent map),将参数空间的速度映射到李代数。经过复杂的矩阵运算,可以得到:

$$\mathbf{\omega }=W(\varphi ,\theta )\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]$$

其中转换矩阵:

$$\boxed{W(\varphi ,\theta )=\left[\begin{array}{ccc}1& 0& -\sin \theta \\ 0& \cos \varphi & \sin \varphi \cos \theta \\ 0& -\sin \varphi & \cos \varphi \cos \theta \end{array}\right]}$$

---

3. 转换矩阵的物理推导

3.1 序贯旋转的几何意义

ZYX欧拉角(Roll-Pitch-Yaw)的旋转序列:

1. 绕世界系Z轴旋转 $\psi$ (yaw)
2. 绕新Y轴旋转 $\theta$ (pitch)
3. 绕新X轴旋转 $\varphi$ (roll)

关键洞察:每次旋转都在已旋转的坐标系中进行,这是耦合产生的根源。

3.2 角速度的分量合成

总角速度由三部分贡献:

$${\mathbf{\omega }}_{\text{total}}={\mathbf{\omega }}_{\varphi }+{\mathbf{\omega }}_{\theta }+{\mathbf{\omega }}_{\psi }$$

Roll贡献(绕机体X轴):
$${\mathbf{\omega }}_{\varphi }=\dot{\varphi }{\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]}_{\text{body}}$$

Pitch贡献(绕已roll的Y’轴):

Pitch旋转轴在机体系的表示:
$${\mathbf{e}}_{y^{\prime }}=R_x(-\varphi )\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]={\left[\begin{array}{c}0\\ \cos \varphi \\ -\sin \varphi \end{array}\right]}_{\text{body}}$$

因此:
$${\mathbf{\omega }}_{\theta }=\dot{\theta }{\left[\begin{array}{c}0\\ \cos \varphi \\ -\sin \varphi \end{array}\right]}_{\text{body}}$$

Yaw贡献(绕世界系Z轴):

世界Z轴在机体系的表示:
$${\mathbf{e}}_z^{\text{body}}=R_x(-\varphi )R_y(-\theta )\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}-\sin \theta \\ \sin \varphi \cos \theta \\ \cos \varphi \cos \theta \end{array}\right]$$

因此:
$${\mathbf{\omega }}_{\psi }=\dot{\psi }{\left[\begin{array}{c}-\sin \theta \\ \sin \varphi \cos \theta \\ \cos \varphi \cos \theta \end{array}\right]}_{\text{body}}$$

3.3 完整转换关系

合并三个分量,得到分通道展开形式:

$$\boxed{\{\begin{array}{l}{\omega }_x=\dot{\varphi }-\sin \theta \cdot \dot{\psi }\\ {\omega }_y=\cos \varphi \cdot \dot{\theta }+\sin \varphi \cos \theta \cdot \dot{\psi }\\ {\omega }_z=-\sin \varphi \cdot \dot{\theta }+\cos \varphi \cos \theta \cdot \dot{\psi }\end{array}}$$

关键观察:

• 每个角速度分量都受多个欧拉角变化率影响
• 转换系数依赖于当前姿态角 $(\varphi ,\theta )$
• $\theta \to \pm 90°$ 时,$W$ 变为奇异(万向锁)

3.4 逆向转换

求解 $W^{-1}$ 得到:

$$\boxed{\{\begin{array}{l}\dot{\varphi }={\omega }_x+\sin \varphi \tan \theta \cdot {\omega }_y+\cos \varphi \tan \theta \cdot {\omega }_z\\ \dot{\theta }=\cos \varphi \cdot {\omega }_y-\sin \varphi \cdot {\omega }_z\\ \dot{\psi }=\frac{\sin \varphi }{\cos \theta }\cdot {\omega }_y+\frac{\cos \varphi }{\cos \theta }\cdot {\omega }_z\end{array}}$$

警告: 包含 $\tan \theta$ 和 $\sec \theta =1/\cos \theta$,在 $\theta =\pm 90°$ 时发散!

---

4. 动力学方程的表示形式

4.1 欧拉方程:角速度表示

刚体的欧拉方程在机体坐标系下表示为:

$$\boxed{\mathbf{I}\dot{\mathbf{\omega }}+\mathbf{\omega }\times (\mathbf{I}\mathbf{\omega })=\mathbf{\tau }}$$

其中:

• $\mathbf{I}=\text{diag}(I_{xx},I_{yy},I_{zz})$ 是惯性张量
• $\mathbf{\omega }\times (\mathbf{I}\mathbf{\omega })$ 是陀螺力矩项
• $\mathbf{\tau }$ 是外加控制力矩

展开为三个耦合方程:
$$\{\begin{array}{l}{I}_{xx}{\dot{\omega }}_x=(I_{yy}-I_{zz}){\omega }_y{\omega }_z+{\tau }_x\\ I_{yy}{\dot{\omega }}_y=(I_{zz}-I_{xx}){\omega }_z{\omega }_x+{\tau }_y\\ I_{zz}{\dot{\omega }}_z=(I_{xx}-I_{yy}){\omega }_x{\omega }_y+{\tau }_z\end{array}$$

优势:

• 形式简洁,物理意义清晰
• 直接反映角动量守恒
• 非线性耦合项明确,便于控制补偿

4.2 欧拉-拉格朗日方程:欧拉角表示

用欧拉角参数化时,拉格朗日方法给出:

动能: $T=\frac{1}{2}{\mathbf{\omega }}^T\mathbf{I}\mathbf{\omega }=\frac{1}{2}{\dot{\mathbf{\eta }}}^{T}M(\mathbf{\eta })\dot{\mathbf{\eta }}$

其中广义惯性矩阵:
$$M(\mathbf{\eta })=W^T(\varphi ,\theta )\mathbf{I}W(\varphi ,\theta )$$

欧拉-拉格朗日方程:
$$M(\mathbf{\eta })\ddot{\mathbf{\eta }}+C(\mathbf{\eta },\dot{\mathbf{\eta }})\dot{\mathbf{\eta }}=W^T(\varphi ,\theta ){\mathbf{\tau }}_B$$

其中 $C$ 包含科里奥利力和离心力项。

劣势:

• $M$ 和 $C$ 高度非线性,依赖姿态
• 奇异点处 $M$ 不可逆
• 计算复杂度高
• 物理直观性差

这解释了为什么现代飞控采用欧拉方程而非欧拉-拉格朗日方程。

---

5. 控制理论视角

5.1 线性化与小角度近似

在悬停点附近 $(|\varphi |,|\theta |,|\psi |\ll 1)$:

$$\sin \varphi \approx \varphi ,\cos \varphi \approx 1,\sin \theta \approx \theta ,\cos \theta \approx 1$$

转换矩阵简化为:
$$W\approx \left[\begin{array}{ccc}1& 0& -\theta \\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\approx I$$

此时:
$${\omega }_x\approx \dot{\varphi }-\theta \dot{\psi },{\omega }_y\approx \dot{\theta },{\omega }_z\approx \dot{\psi }$$

这就是为什么简单PID在小角度范围有效。但随着角度增大:

• 耦合项 $-\theta \dot{\psi }$ 不可忽略
• 三角函数非线性凸显
• 简单PID性能退化

5.2 四元数表示的几何优势

单位四元数 S3={q∈H∣∣q∣=1}\mathbb{S}^3 = \{q \in \mathbb{H} \mid |q| = 1\}S3={q∈H∣∣q∣=1} 提供SO(3)的双覆盖。

核心优势:

1. 线性微分关系:
   $$\dot{q}=\frac{1}{2}q\otimes {\omega }_q$$
   其中 ${\omega }_q=[0,{\omega }_x,{\omega }_y,{\omega }_z{]}^T$,无需复杂转换矩阵。

2. 无奇异性:
   整个SO(3)都被平滑覆盖,无万向锁问题。

3. 姿态误差的几何意义:
   $$q_e=q^{\ast }\otimes q_d$$
   其虚部 $2[q_{e1},q_{e2},q_{e3}{]}^T$ 近似为轴角误差。

4. 控制律简洁性:
   $$\mathbf{\tau }=-K_p\text{sgn}(q_{e0})\left[\begin{array}{c}{q}_{e1}\\ q_{e2}\\ q_{e3}\end{array}\right]-K_d\mathbf{\omega }$$

5. 数值稳定性:
   只需归一化约束 $|q|=1$,而旋转矩阵需满足9个正交约束。

---

6. 传感器与状态估计

6.1 陀螺仪测量原理

MEMS陀螺仪基于科里奥利效应,直接测量角速度:

$${\mathbf{\omega }}_m={\mathbf{\omega }}_{\text{true}}+{\mathbf{b}}_g+{\mathbf{n}}_g$$

其中:

• ${\mathbf{b}}_g$: 缓变偏置(bias)
• ${\mathbf{n}}_g$: 高斯白噪声

关键特性:

• 带宽: 几百Hz到几kHz
• 直接输出角速度,无需转换
• 高更新率支持快速角速率控制环(1-8kHz)

这是级联控制结构(姿态外环+角速率内环)的硬件基础。

6.2 姿态估计的信息融合

传感器互补:

• 陀螺仪: 高频角速度,短期精确,长期漂移
• 加速度计: 静态/匀速时测量重力方向 ${\mathbf{g}}_B=R^T{\mathbf{g}}_I$,提供roll/pitch
• 磁力计: 地磁场方向 ${\mathbf{m}}_B=R^T{\mathbf{m}}_I$,倾斜补偿后提供yaw

扩展卡尔曼滤波(EKF)框架:

状态预测(四元数):
$$q_{k+1}=q_k\otimes \exp \left(\frac{1}{2}{\mathbf{\omega }}_k\Delta t\right)$$

若用欧拉角:
$$[\varphi ,\theta ,\psi {]}_{k+1}=[\varphi ,\theta ,\psi {]}_k+W^{-1}({\varphi }_k,{\theta }_k){\mathbf{\omega }}_k\Delta t$$

问题: 需计算 $W^{-1}$,在奇异点数值不稳定。

误差状态卡尔曼滤波(ESKF)混合方案:

• 名义状态用四元数(全局无奇异)
• 误差状态用小角度 $\delta \mathbf{\theta }$ (线性化便利)
• 结合两者优势,成为主流方案

---

7. 非线性控制策略

7.1 滑模控制

设计滑模面:
$$\mathbf{s}=\mathbf{\omega }-{\mathbf{\omega }}_d+\lambda ({\mathbf{q}}_v-{\mathbf{q}}_{vd})$$

控制律:
$$\mathbf{\tau }=-\mathbf{I}(\mathbf{\omega }\times (\mathbf{I}\mathbf{\omega }))+\mathbf{I}({\dot{\mathbf{\omega }}}_d-\lambda {\mathbf{\omega }}_e)-K\text{sgn}(\mathbf{s})$$

优势: 对参数不确定性和扰动鲁棒

挑战: 符号函数引起抖振,需饱和函数或高阶滑模(如Super-twisting)缓解

7.2 反步法(Backstepping)

步骤1: 姿态运动学层,设计虚拟控制

Lyapunov函数: $V_1=\frac{1}{2}\Vert {\mathbf{q}}_v{\Vert }^2$

虚拟控制: ${\mathbf{\omega }}_d=-K_1{\mathbf{q}}_v$ 使 ${\dot{V}}_1\le 0$

步骤2: 角速度动力学层,设计实际力矩

增广Lyapunov: $V_2=V_1+\frac{1}{2}{\mathbf{z}}^T\mathbf{Iz}$,其中 $\mathbf{z}=\mathbf{\omega }-{\mathbf{\omega }}_d$

控制律: $\mathbf{\tau }=\mathbf{I}({\dot{\mathbf{\omega }}}_d-\mathbf{\omega }\times ({\mathbf{I}}^{-1}\mathbf{\tau }))-K_2\mathbf{z}-{\mathbf{q}}_v$

保证: ${\dot{V}}_2\le -K_1\Vert {\mathbf{q}}_v{\Vert }^2-K_2\Vert \mathbf{z}{\Vert }^2<0$ (全局渐近稳定)

扩展: 自适应反步法可在线估计惯性矩阵不确定性。

---

9. PX4的架构选择

9.1 为什么不用欧拉角变化率?

维度	欧拉角变化率方案	角速度方案(PX4)
物理定律	不符合欧拉方程	直接对应欧拉方程
传感器	间接计算,噪声大	陀螺仪直接测量
奇异性	万向锁 $\theta =\pm 90°$	四元数无奇异
耦合	三通道强耦合	机体系独立
转换	需计算$W(\varphi ,\theta )$	四元数线性微分
数值	三角函数密集	仅代数运算
控制设计	复杂非线性	几何直观

9.2 PX4的实际实现

// 姿态控制器(外环) - 基于四元数
Quatf q_error = q_current.inversed() * q_desired;
Vector3f attitude_error = 2.0f * q_error.canonical().imag();
Vector3f rate_setpoint = attitude_error.emult(_proportional_gain);

// 角速率控制器(内环) - 直接使用角速度
Vector3f angular_velocity = imu.gyro;  // 陀螺仪直接输出
Vector3f rate_error = rate_setpoint - angular_velocity;
Vector3f torque = rate_error.emult(_rate_gain) 
                  + feedforward_compensation;

核心理念: 控制物理量(角速度),而非数学表示(欧拉角变化率)。

---

10. 代码实现参考

10.1 前向转换:欧拉角变化率→角速度

#include <matrix/math.hpp>
#include <cmath>

using namespace matrix;

Vector3f eulerRateToAngularVelocity(
    float phi, float theta, const Vector3f& euler_rate)
{
    float sphi = sinf(phi);
    float cphi = cosf(phi);
    float stheta = sinf(theta);
    float ctheta = cosf(theta);
    
    Vector3f omega;
    omega(0) = euler_rate(0) - stheta * euler_rate(2);
    omega(1) = cphi * euler_rate(1) + sphi * ctheta * euler_rate(2);
    omega(2) = -sphi * euler_rate(1) + cphi * ctheta * euler_rate(2);
    
    return omega;
}

10.2 逆向转换:角速度→欧拉角变化率

Vector3f angularVelocityToEulerRate(
    float phi, float theta, const Vector3f& omega)
{
    float sphi = sinf(phi);
    float cphi = cosf(phi);
    float ttheta = tanf(theta);
    float sectheta = 1.0f / cosf(theta);  // 警告:奇异性!
    
    Vector3f euler_rate;
    euler_rate(0) = omega(0) + sphi * ttheta * omega(1) 
                    + cphi * ttheta * omega(2);
    euler_rate(1) = cphi * omega(1) - sphi * omega(2);
    euler_rate(2) = sphi * sectheta * omega(1) 
                    + cphi * sectheta * omega(2);
    
    return euler_rate;
}

注意: 当 $|\theta |\to 90°$ 时,sectheta 发散,必须检查奇异性!

10.3 使用建议

// ✅ 推荐:直接使用角速度
Vector3f angular_velocity = imu.gyro;
Vector3f rate_sp = attitude_control.update(q);
Vector3f torque = rate_control.update(angular_velocity, rate_sp, dt);

// ❌ 不推荐:绕道欧拉角变化率
Eulerf euler(q);
Vector3f euler_rate = compute_euler_rate(euler, euler_prev, dt);
Vector3f angular_velocity = eulerRateToAngularVelocity(
    euler.phi(), euler.theta(), euler_rate);  // 多此一举

---

11. 总结与展望

11.1 核心要点回顾

1. 本质区别:

   • 欧拉角变化率是参数空间的速度
   • 角速度是流形切空间的速度
   • 通过依赖姿态的转换矩阵 $W(\varphi ,\theta )$ 非线性关联

2. 数学根源:

   • SO(3)的非欧几何结构
   • 李群-李代数对应
   • 欧拉角序贯旋转的非交换性

3. 工程实践:

   • 四元数+角速度架构避免奇异性
   • 直接对应物理定律和传感器测量
   • 计算高效,数值稳定

4. 控制设计:

   • 小角度线性化适用PID
   • 大机动需非线性方法(滑模/反步)
   • 四元数提供几何直观的控制律

附录:完整转换公式速查

前向转换(欧拉角变化率→角速度)

$$\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]=\left[\begin{array}{ccc}1& 0& -\sin \theta \\ 0& \cos \varphi & \sin \varphi \cos \theta \\ 0& -\sin \varphi & \cos \varphi \cos \theta \end{array}\right]\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]$$

逆向转换(角速度→欧拉角变化率)

$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0& \cos \varphi & -\sin \varphi \\ 0& \sin \varphi \sec \theta & \cos \varphi \sec \theta \end{array}\right]\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]$$

角速率计算

$$\omega =\Vert \left[\begin{array}{ccc}1& 0& -\sin \theta \\ 0& \cos \varphi & \sin \varphi \cos \theta \\ 0& -\sin \varphi & \cos \varphi \cos \theta \end{array}\right]\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]\Vert$$

欧拉角变化率到角速度的分量展开

根据矩阵乘法规则，将转换矩阵与欧拉角变化率向量相乘：

完整展开过程

$$\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]=\left[\begin{array}{ccc}1& 0& -\sin \theta \\ 0& \cos \varphi & \sin \varphi \cos \theta \\ 0& -\sin \varphi & \cos \varphi \cos \theta \end{array}\right]\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]$$

分量展开

${\omega }_x$ (Roll轴角速度)

取转换矩阵第一行与欧拉角变化率向量做内积：

$${\omega }_x=1\cdot \dot{\varphi }+0\cdot \dot{\theta }+(-\sin \theta )\cdot \dot{\psi }$$

$$\boxed{{\omega }_x=\dot{\varphi }-\sin \theta \cdot \dot{\psi }}$$

---

${\omega }_y$ (Pitch轴角速度)

取转换矩阵第二行与欧拉角变化率向量做内积：

$${\omega }_y=0\cdot \dot{\varphi }+\cos \varphi \cdot \dot{\theta }+(\sin \varphi \cos \theta )\cdot \dot{\psi }$$

$$\boxed{{\omega }_y=\cos \varphi \cdot \dot{\theta }+\sin \varphi \cos \theta \cdot \dot{\psi }}$$

---

${\omega }_z$ (Yaw轴角速度)

取转换矩阵第三行与欧拉角变化率向量做内积：

$${\omega }_z=0\cdot \dot{\varphi }+(-\sin \varphi )\cdot \dot{\theta }+(\cos \varphi \cos \theta )\cdot \dot{\psi }$$

$$\boxed{{\omega }_z=-\sin \varphi \cdot \dot{\theta }+\cos \varphi \cos \theta \cdot \dot{\psi }}$$

---

最终结果（三个分量）

$$\boxed{\{\begin{array}{l}{\omega }_x=\dot{\varphi }-\sin \theta \cdot \dot{\psi }\\ {\omega }_y=\cos \varphi \cdot \dot{\theta }+\sin \varphi \cos \theta \cdot \dot{\psi }\\ {\omega }_z=-\sin \varphi \cdot \dot{\theta }+\cos \varphi \cos \theta \cdot \dot{\psi }\end{array}}$$

---

物理意义解读

角速度分量	主要贡献	次要贡献	耦合特性
${\omega }_x$	Roll变化率 $\dot{\varphi }$	Yaw变化率 $\dot{\psi }$（受俯仰角调制）	Pitch角度影响Yaw贡献
${\omega }_y$	Pitch变化率 $\dot{\theta }$	Yaw变化率 $\dot{\psi }$（受Roll和Pitch调制）	两个姿态角共同调制
${\omega }_z$	Yaw变化率 $\dot{\psi }$	Pitch变化率 $\dot{\theta }$（受Roll调制）	Roll角度分配Pitch和Yaw贡献

关键观察：

• 每个角速度分量都受多个欧拉角变化率影响
• 耦合系数依赖于当前姿态角 $(\varphi ,\theta )$
• 这就是为什么三个通道不能独立控制

---

C++代码实现

// 方法1：直接计算
void computeAngularVelocity(
    float phi, float theta,           // 当前姿态角
    float dphi, float dtheta, float dpsi,  // 欧拉角变化率
    float& omega_x, float& omega_y, float& omega_z)  // 输出角速度
{
    float sin_phi = sinf(phi);
    float cos_phi = cosf(phi);
    float sin_theta = sinf(theta);
    float cos_theta = cosf(theta);
    
    // Roll轴
    omega_x = dphi - sin_theta * dpsi;
    
    // Pitch轴
    omega_y = cos_phi * dtheta + sin_phi * cos_theta * dpsi;
    
    // Yaw轴
    omega_z = -sin_phi * dtheta + cos_phi * cos_theta * dpsi;
}

// 方法2：使用Matrix库
#include <matrix/math.hpp>

matrix::Vector3f eulerRateToAngularVelocity(
    float phi, float theta, 
    const matrix::Vector3f& euler_rate)
{
    float sphi = sinf(phi);
    float cphi = cosf(phi);
    float stheta = sinf(theta);
    float ctheta = cosf(theta);
    
    matrix::Vector3f omega;
    omega(0) = euler_rate(0) - stheta * euler_rate(2);
    omega(1) = cphi * euler_rate(1) + sphi * ctheta * euler_rate(2);
    omega(2) = -sphi * euler_rate(1) + cphi * ctheta * euler_rate(2);
    
    return omega;
}

---

Python验证代码

import numpy as np

def euler_rate_to_angular_velocity(phi, theta, dphi, dtheta, dpsi):
    """
    将欧拉角变化率转换为角速度
    
    Args:
        phi: Roll角 (rad)
        theta: Pitch角 (rad)
        dphi: Roll变化率 (rad/s)
        dtheta: Pitch变化率 (rad/s)
        dpsi: Yaw变化率 (rad/s)
    
    Returns:
        omega_x, omega_y, omega_z: 角速度分量 (rad/s)
    """
    sin_phi = np.sin(phi)
    cos_phi = np.cos(phi)
    sin_theta = np.sin(theta)
    cos_theta = np.cos(theta)
    
    # 按公式计算
    omega_x = dphi - sin_theta * dpsi
    omega_y = cos_phi * dtheta + sin_phi * cos_theta * dpsi
    omega_z = -sin_phi * dtheta + cos_phi * cos_theta * dpsi
    
    return omega_x, omega_y, omega_z

# 测试
if __name__ == "__main__":
    # 水平姿态
    phi, theta = 0, 0
    dphi, dtheta, dpsi = 1.0, 1.0, 0.0
    
    wx, wy, wz = euler_rate_to_angular_velocity(phi, theta, dphi, dtheta, dpsi)
    print(f"水平姿态: ωx={wx:.3f}, ωy={wy:.3f}, ωz={wz:.3f}")
    # 预期: ωx=1.0, ωy=1.0, ωz=0.0
    
    # 有俯仰角
    phi, theta = 0, np.radians(45)
    dphi, dtheta, dpsi = 1.0, 0.0, 1.0
    
    wx, wy, wz = euler_rate_to_angular_velocity(phi, theta, dphi, dtheta, dpsi)
    print(f"俯仰45°: ωx={wx:.3f}, ωy={wy:.3f}, ωz={wz:.3f}")
    # ωx受θ影响, ωz受θ影响