3、姿态解算基础：四元数、欧拉角、旋转矩阵的数学关系与转换

做无人机调参，绕不开姿态解算。说白了，就是搞清楚飞机当前“脑袋朝哪、机身歪没歪”。

我刚开始接触飞控时，觉得这东西就是背公式。后来踩了几次坑才明白——选错表达方式，轻则调参调到手抽筋，重则飞机直接翻跟头。

这一章，咱们把四元数、欧拉角、旋转矩阵这三兄弟的关系彻底捋清楚。

3.1 三种姿态表达方式，各自什么脾气？

先给个总览表格，方便你快速对比：

表达方式 核心思想 优点 缺点
欧拉角 绕三个轴依次旋转（Yaw-Pitch-Roll） 直观，人脑好理解 有万向锁，插值不平滑
旋转矩阵 3x3 正交矩阵描述旋转 运算方便，可叠加 9个参数，冗余，有约束
四元数 4个参数的超复数（w + xi + yj + zk） 无万向锁，插值平滑，计算快 不直观，调试时难脑补

我的建议：飞控内部运算，一律用四元数。人机交互显示，转成欧拉角。矩阵？嗯，它更多是理论桥梁，实际代码里用得少。

3.2 欧拉角：最直观，但最危险

欧拉角就是咱们常说的偏航（Yaw）、俯仰（Pitch）、横滚（Roll）。你拿着遥控器打杆，本质上就是在调这三个角度。

但这里有个大坑——万向锁（Gimbal Lock）。

我记得第一次做固定翼飞控时，俯仰拉到90度，偏航和横滚突然“锁死”了。飞机直接失控，幸亏是在仿真里。为什么会这样？因为欧拉角在 Pitch = ±90° 时，Yaw 和 Roll 的旋转轴重合了，丢失了一个自由度。

避坑指南：千万不要在飞控核心姿态解算里直接用欧拉角做积分或插值。我曾经见过一个开源项目这么干，结果飞机做大机动时姿态直接跳变。嗯，那代码后来被重写了。

3.3 旋转矩阵：数学上的瑞士军刀

旋转矩阵 R 是一个 3x3 的正交矩阵，满足 RTR = I，行列式为 +1。

它可以把一个向量从机体坐标系转到世界坐标系：

v_world = R * v_body

旋转矩阵的好处是可以连续相乘，组合多次旋转。但坏处也很明显——9个参数，每次更新后还得做正交化，不然矩阵会“漂移”。

我个人习惯只在坐标变换推导时用矩阵，写代码时直接转四元数。

3.4 四元数：飞控的“内功心法”

四元数长这样：

q = w + x*i + y*j + z*k
其中 w² + x² + y² + z² = 1

它表示绕一个单位向量 u 旋转 θ 角度：

q = cos(θ/2) + u*sin(θ/2)

为什么飞控都用它？三个字：稳、快、准。

  • 无万向锁——随便你怎么转，不会丢自由度
  • 计算量小——乘法比矩阵少一半
  • 插值平滑——做姿态平滑时，球面线性插值（SLERP）非常自然

小技巧：调试时如果发现四元数数值异常，先检查它是不是单位四元数。我遇到过好几次，都是因为积分步长太大导致模长偏离1，结果姿态越算越偏。

3.5 三者互转：你需要的就这几行代码

下面给出最常用的转换公式和代码。你直接复制到项目里就能用。

3.5.1 四元数 → 欧拉角

// 假设四元数 q = [w, x, y, z]
// 输出：roll, pitch, yaw (单位：弧度)

float roll  = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y));
float pitch = asin(2*(w*y - z*x));  // 注意：这里可能遇到万向锁
float yaw   = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z));

注意：当 pitch 接近 ±90° 时，asin 的输入会接近 ±1，此时 roll 和 yaw 不稳定。实际代码里要做保护，比如限制 pitch 范围在 ±89°。

3.5.2 欧拉角 → 四元数

// 输入：roll, pitch, yaw (弧度)
// 输出：四元数 q = [w, x, y, z]

float cy = cos(yaw * 0.5);
float sy = sin(yaw * 0.5);
float cp = cos(pitch * 0.5);
float sp = sin(pitch * 0.5);
float cr = cos(roll * 0.5);
float sr = sin(roll * 0.5);

q.w = cy * cp * cr + sy * sp * sr;
q.x = cy * cp * sr - sy * sp * cr;
q.y = sy * cp * sr + cy * sp * cr;
q.z = sy * cp * cr - cy * sp * sr;

3.5.3 四元数 → 旋转矩阵

// 输入：四元数 q = [w, x, y, z]
// 输出：3x3 旋转矩阵 R

R[0][0] = 1 - 2*(y*y + z*z);
R[0][1] = 2*(x*y - w*z);
R[0][2] = 2*(x*z + w*y);

R[1][0] = 2*(x*y + w*z);
R[1][1] = 1 - 2*(x*x + z*z);
R[1][2] = 2*(y*z - w*x);

R[2][0] = 2*(x*z - w*y);
R[2][1] = 2*(y*z + w*x);
R[2][2] = 1 - 2*(x*x + y*y);

3.6 一张图看懂三者的关系

下面这张 SVG 图，把三者的转换路径和核心公式画清楚了。你保存下来，调参时看一眼，思路会清晰很多。

四元数、欧拉角、旋转矩阵关系图 姿态表达方式转换关系图 四元数 q = w + xi + yj + zk 欧拉角 (Yaw, Pitch, Roll) 旋转矩阵 3x3 正交矩阵 atan2 / asin 半角公式 直接代入 特征值法 三次旋转矩阵相乘 元素提取 核心原则 • 飞控内部运算：优先使用四元数（无万向锁、计算快） • 人机交互显示：转成欧拉角（直观、易理解） • 坐标变换推导：使用旋转矩阵（数学上最严谨）

3.7 实战中的选择建议

说了这么多，到底怎么选？我直接给结论：

  • IMU 数据融合（Mahony/Madgwick 滤波）：用四元数。代码里全是四元数乘法，效率最高。
  • PID 控制器输入：用欧拉角。因为你要的是“当前角度 vs 目标角度”的误差，欧拉角最直观。
  • 坐标系转换（比如把加速度计读数转到世界系）：用旋转矩阵。虽然代码里可以转成四元数再算，但矩阵形式更容易 debug。

我的经验：刚开始做飞控时，我总想着“一步到位”，所有地方都用四元数。结果调试时看数据一脸懵——谁能一眼看出 q=[0.707, 0.707, 0, 0] 对应什么姿态？后来我学乖了：内部运算用四元数，日志记录和显示用欧拉角。这样既保证了性能，又保住了头发。

3.8 避坑清单

最后，把我在项目中踩过的坑列出来，你直接避着走：

  1. 欧拉角做插值——别干这事。两个角度差 350° 和 -10°，插值结果天差地别。用四元数的 SLERP。
  2. 忘记归一化——四元数用久了模长会漂。每次更新后记得归一化，不然姿态会慢慢“歪掉”。
  3. 万向锁不处理——如果你非要用欧拉角做控制，Pitch 接近 ±90° 时要做保护。我一般限制在 ±85°。
  4. 矩阵不正交——旋转矩阵用久了会变成“近似正交”，导致坐标变换有误差。定期做 Gram-Schmidt 正交化。
← 上一章 📖 返回目录 下一章 →