4. 姿态运动学方程：从欧拉到四元数

各位同学，今天我们聊一个绕不开的话题——姿态运动学方程。

说白了，就是描述飞行器怎么转的数学工具。你给它一个角速度，它怎么从当前姿态变到下一个姿态？这就是运动学方程要干的事。

我个人习惯把姿态运动学分成两派：欧拉角派和四元数派。两派各有千秋，也各有坑。咱们一个一个说。

4.1 基于欧拉角的运动学方程

欧拉角，大家应该不陌生。滚转φ、俯仰θ、偏航ψ，三个角度，直观又好理解。

但问题来了：角速度ω和欧拉角的变化率之间，不是简单的相等关系。

为什么？因为欧拉角的旋转顺序是有讲究的。你想想看，先绕x轴转，再绕y轴转，最后绕z轴转——每次旋转的参考坐标系都在变。

我推导一下这个关系。假设旋转顺序是Z-Y-X（偏航-俯仰-滚转），这是航空界最常用的顺序。那么角速度向量ω = [p, q, r]ᵀ 和欧拉角速率 [φ̇, θ̇, ψ̇]ᵀ 的关系是：

⎡ p ⎤   ⎡ 1   0       -sinθ    ⎤ ⎡ φ̇ ⎤
⎢ q ⎥ = ⎢ 0   cosφ    sinφ·cosθ ⎥ ⎢ θ̇ ⎥
⎣ r ⎦   ⎣ 0  -sinφ    cosφ·cosθ ⎦ ⎣ ψ̇ ⎦

反过来，我们更常用的是从角速度求欧拉角速率：

⎡ φ̇ ⎤   ⎡ 1   sinφ·tanθ   cosφ·tanθ  ⎤ ⎡ p ⎤
⎢ θ̇ ⎥ = ⎢ 0   cosφ        -sinφ       ⎥ ⎢ q ⎥
⎣ ψ̇ ⎦   ⎣ 0   sinφ·secθ   cosφ·secθ  ⎦ ⎣ r ⎦
⚠️ 注意：万向锁！
当θ = ±90°时，tanθ和secθ趋于无穷大。这就是著名的万向锁问题。
我曾经在调试一个固定翼的垂直爬升段时，俯仰角接近90°，结果欧拉角速率直接炸了。嗯，那次之后我就老老实实加了四元数备份。

所以，欧拉角运动学方程虽然直观，但有两个硬伤：

  • 万向锁：俯仰±90°时方程奇异
  • 计算量大：每次都要算三角函数

但在小角度假设下（比如平飞状态，θ很小），可以简化成：

φ̇ ≈ p + r·tanθ·sinφ + r·cosφ·tanθ   → 简化后 φ̇ ≈ p
θ̇ ≈ q·cosφ - r·sinφ                   → 简化后 θ̇ ≈ q
ψ̇ ≈ q·sinφ·secθ + r·cosφ·secθ         → 简化后 ψ̇ ≈ r

说白了，小角度时，欧拉角速率≈机体角速度。很多初学飞控的同学直接这么用，其实在大多数固定翼平飞场景下，误差是可以接受的。

4.2 基于四元数的运动学方程

四元数，很多人一听就头大。但我要说，四元数才是工程界的真爱。

为什么？因为它没有奇点，计算效率高，还方便插值。

四元数q = [q₀, q₁, q₂, q₃]ᵀ，其中q₀是标量部分，[q₁, q₂, q₃]是矢量部分。它满足归一化条件：q₀² + q₁² + q₂² + q₃² = 1。

四元数的运动学方程长这样：

⎡ q̇₀ ⎤   ⎡ 0   -p  -q  -r ⎤ ⎡ q₀ ⎤
⎢ q̇₁ ⎥ = ½ ⎢ p    0   r  -q ⎥ ⎢ q₁ ⎥
⎢ q̇₂ ⎥     ⎢ q   -r   0   p ⎥ ⎢ q₂ ⎥
⎣ q̇₃ ⎦     ⎣ r    q  -p   0 ⎦ ⎣ q₃ ⎦

写成紧凑形式：

q̇ = ½ · Ω(ω) · q

其中Ω(ω)是角速度的反对称矩阵形式。

💡 核心要点：
四元数运动学方程是线性的！没有三角函数，没有奇点。这是它最大的优势。

我在实际项目中，通常用四元数做核心姿态解算，只在输出给用户或记录日志时才转成欧拉角。这样既避免了万向锁，又保留了直观性。

4.3 方程推导与简化

咱们来推导一下四元数运动学方程。别怕，其实思路很清晰。

推导思路：

  1. 四元数描述的是旋转，旋转可以用旋转轴和旋转角表示
  2. 角速度是旋转角对时间的导数
  3. 对四元数求导，就得到了上面的方程

具体推导过程（我简化一下）：

设旋转轴为单位向量u，旋转角为θ。那么四元数可以写成：

q = [cos(θ/2), u·sin(θ/2)]ᵀ

对时间求导：

q̇ = [-θ̇/2 · sin(θ/2), u̇·sin(θ/2) + u·θ̇/2 · cos(θ/2)]ᵀ

而角速度ω = θ̇·u（假设旋转轴不变）。代入整理，就得到了上面的矩阵形式。

🔧 工程简化技巧：
在嵌入式平台上，我建议用一阶龙格-库塔法（欧拉法）来离散化四元数运动学方程：
q(t+Δt) = q(t) + q̇(t) · Δt
         = q(t) + ½ · Ω(ω(t)) · q(t) · Δt
然后别忘了归一化：
q = q / ||q||
这一步很重要！数值误差会让四元数偏离单位长度，导致姿态失真。

4.4 两种方法的对比

特性 欧拉角法 四元数法
直观性 ⭐⭐⭐⭐⭐ ⭐⭐
无奇点 ❌（万向锁） ✅
计算效率 低（三角函数） 高（仅乘加）
插值平滑 差 好（球面线性插值）
工程推荐度 仅用于小角度场景 通用首选
📌 我的建议：
- 如果你在做固定翼的平飞控制，欧拉角简化版够用
- 如果你要做全姿态飞行（筋斗、倒飞、垂直爬升），请务必用四元数
- 我个人的飞控代码里，核心姿态更新只用四元数，只在最后输出时转成欧拉角给地面站看

4.5 避坑指南

最后，分享几个我踩过的坑：

  • 四元数归一化：每次更新后必须归一化，否则姿态会漂。我曾经因为忘了归一化，飞了3分钟后姿态误差达到20°。
  • 欧拉角转四元数：注意旋转顺序。不同顺序得到的四元数不同，一定要和你的飞控约定一致。
  • 角速度单位：rad/s vs deg/s。我见过有人把deg/s当成rad/s用，结果姿态疯狂震荡。
  • 离散化步长：Δt太大时，一阶欧拉法误差会累积。建议用二阶龙格-库塔法，或者干脆用四元数乘法来更新。

好了，姿态运动学方程就讲到这里。下一节我们聊动力学方程——飞行器是怎么在力和力矩作用下运动的。到时候你会发现，运动学只是开胃菜，动力学才是硬骨头。

← 上一章 📖 返回目录 下一章 →