3、四元数基础：四元数的定义与运算法则、四元数与旋转矩阵的转换、四元数在姿态更新中的优势

说到姿态解算，很多新手第一反应就是欧拉角。嗯，俯仰、横滚、偏航，多直观啊。但等你真正在嵌入式平台上跑起来，就会发现欧拉角有个让人头疼的问题——万向锁。我记得第一次在四轴飞行器上做姿态控制时，就因为万向锁导致飞机在特定角度下疯狂抖动，差点炸机。

后来我转向了四元数。说实话，刚接触时觉得这东西很抽象，四个数怎么就能表示旋转了？但用久了你会发现，四元数在嵌入式飞控里简直就是神器。今天我就带你彻底搞懂它。

3.1 四元数的定义

四元数本质上是一个超复数。普通复数有一个实部和一个虚部，四元数则有一个实部和三个虚部。我习惯把它写成这样：

q = w + xi + yj + zk

其中 w 是实部，x、y、z 是虚部。i、j、k 是三个虚数单位，它们满足一个特殊的乘法规则：

• i² = j² = k² = -1
• ij = k, ji = -k
• jk = i, kj = -i
• ki = j, ik = -j

这个规则说白了就是：任意两个不同的虚数单位相乘，结果等于第三个，但顺序不同符号就不同。你想想看，这其实和三维空间中的叉积很像，对吧？

在飞控里，我们通常用单位四元数来表示旋转。所谓单位四元数，就是模长为1的四元数：

||q|| = sqrt(w² + x² + y² + z²) = 1

3.2 四元数的运算法则

四元数的运算其实不复杂，掌握几个核心就够了。

3.2.1 加法与减法

对应分量相加减，没什么特别的：

q1 + q2 = (w1+w2) + (x1+x2)i + (y1+y2)j + (z1+z2)k

3.2.2 乘法（最关键）

四元数乘法不满足交换律，这点一定要注意。两个四元数相乘：

q1 * q2 = (w1*w2 - x1*x2 - y1*y2 - z1*z2) +
          (w1*x2 + x1*w2 + y1*z2 - z1*y2)i +
          (w1*y2 - x1*z2 + y1*w2 + z1*x2)j +
          (w1*z2 + x1*y2 - y1*x2 + z1*w2)k

看着复杂，但写成代码其实就几行。我个人习惯用这个公式：

// 四元数乘法
void quat_mult(float q1[4], float q2[4], float q_out[4]) {
    q_out[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];
    q_out[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2];
    q_out[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1];
    q_out[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0];
}

3.2.3 共轭与逆

四元数的共轭就是把虚部取反：

q* = w - xi - yj - zk

对于单位四元数，逆就等于共轭：

q⁻¹ = q*

这个性质在姿态更新中非常有用，后面会用到。

3.3 四元数与旋转矩阵的转换

在实际飞控代码中，我们经常需要在四元数和旋转矩阵之间来回切换。比如，你要把加速度计的数据从机体坐标系转到世界坐标系，用旋转矩阵就比用四元数直观。

3.3.1 四元数转旋转矩阵

给定单位四元数 q = (w, x, y, z)，对应的旋转矩阵 R 为：

R₁₁ = 1 - 2(y² + z²)	R₁₂ = 2(xy - wz)	R₁₃ = 2(xz + wy)
R₂₁ = 2(xy + wz)	R₂₂ = 1 - 2(x² + z²)	R₂₃ = 2(yz - wx)
R₃₁ = 2(xz - wy)	R₃₂ = 2(yz + wx)	R₃₃ = 1 - 2(x² + y²)

代码实现也很直接：

void quat_to_rotmat(float q[4], float R[3][3]) {
    float w = q[0], x = q[1], y = q[2], z = q[3];
    float x2 = x*x, y2 = y*y, z2 = z*z;
    
    R[0][0] = 1 - 2*(y2 + z2);
    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*(x2 + z2);
    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*(x2 + y2);
}

3.3.2 旋转矩阵转四元数

反过来，从旋转矩阵提取四元数稍微麻烦一点。我常用的方法是：

void rotmat_to_quat(float R[3][3], float q[4]) {
    float tr = R[0][0] + R[1][1] + R[2][2];
    float S;
    
    if (tr > 0) {
        S = sqrt(tr + 1.0) * 2;
        q[0] = 0.25 * S;
        q[1] = (R[2][1] - R[1][2]) / S;
        q[2] = (R[0][2] - R[2][0]) / S;
        q[3] = (R[1][0] - R[0][1]) / S;
    } else if (R[0][0] > R[1][1] && R[0][0] > R[2][2]) {
        // ... 其他分支处理
    }
}

3.4 四元数在姿态更新中的优势

为什么飞控圈几乎都用四元数做姿态更新？说白了，三个字：稳、快、准。

3.4.1 没有万向锁

欧拉角在俯仰角接近±90度时，横滚和偏航会耦合在一起，这就是万向锁。用四元数完全没这个问题。你想想看，四元数是在四维空间里做旋转，根本不存在奇点。

3.4.2 计算效率高

姿态更新本质上就是不断积分角速度。用四元数更新只需要做四元数乘法：

q_new = q_old * q_delta

其中 q_delta 是由角速度计算出来的增量四元数。整个过程只需要16次乘法和12次加法，比旋转矩阵的9个元素更新快得多。

3.4.3 易于插值

在飞控中，我们经常需要在两个姿态之间平滑过渡。四元数的球面线性插值（SLERP）非常优雅：

q(t) = (sin((1-t)*θ) * q1 + sin(t*θ) * q2) / sin(θ)

其中 θ 是两个四元数之间的夹角。这种插值能保证旋转路径最短，而且角速度恒定。用欧拉角做插值？嗯，那画面太美我不敢看。

3.5 实战建议

最后，给你几个我在项目中积累的经验：

• 始终归一化： 每次更新完四元数，记得归一化。数值误差会慢慢累积，不归一化的话，姿态会漂移。
• 注意乘法顺序： 四元数乘法不满足交换律。我习惯用 q_new = q_delta * q_old 表示在世界坐标系下旋转，用 q_new = q_old * q_delta 表示在机体坐标系下旋转。搞反了，姿态就反了。
• 调试时转欧拉角： 虽然计算用四元数，但调试时我建议转成欧拉角来看。人脑对欧拉角更直观，方便排查问题。

四元数这东西，刚接触时觉得绕，用多了就离不开了。等你真正在飞控里跑起来，就会发现它有多香。