四元数基础：定义、运算与旋转矩阵转换

说到姿态解算，四元数是个绕不开的话题。我记得刚入行那会儿，看到四元数这四个分量就头疼——这玩意儿到底是个啥？后来做项目做多了，才慢慢体会到它的妙处。今天我就把自己这些年对四元数的理解，掰开了揉碎了讲给你听。

一、四元数的定义

四元数，说白了就是一个超复数。我们熟悉的复数是 a+bi，四元数就是 q = w + xi + yj + zk。其中 w 是实部，x、y、z 是虚部，i、j、k 是三个虚数单位。

这里有个关键规则：

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

嗯，注意最后一条——乘法不满足交换律。这一点在运算时特别容易出错，我刚开始写代码时就吃过这个亏。

二、四元数的基本运算

实际项目中，我们最常用的就是下面这几种运算。我建议你把这些函数封装好，以后直接调用就行。

2.1 四元数乘法

两个四元数相乘，结果还是一个四元数。公式看起来有点复杂，但其实就是多项式展开再合并同类项。

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

2.2 共轭与模长

四元数的共轭就是把虚部取反：q* = w - xi - yj - zk。模长就是 sqrt(w² + x² + y² + z²)。

为什么要关心共轭？因为单位四元数的逆就是它的共轭。这个性质在姿态解算中太有用了——想撤销一个旋转，直接乘共轭就行。

2.3 向量旋转

用四元数旋转一个三维向量 v，操作是这样的：

// 用四元数 q 旋转向量 v
void quat_rotate(float q[4], float v[3], float v_out[3]) {
    // 构造纯四元数 p = (0, vx, vy, vz)
    float p[4] = {0, v[0], v[1], v[2]};
    // 计算 q * p * q*
    float temp[4], result[4];
    quat_multiply(q, p, temp);
    quat_multiply(temp, q_conj(q), result);
    // 提取虚部
    v_out[0] = result[1];
    v_out[1] = result[2];
    v_out[2] = result[3];
}

三、四元数与旋转矩阵的转换

四元数和旋转矩阵可以互相转换。为什么需要这个？因为有些场合用矩阵更方便（比如坐标变换），有些场合用四元数更合适（比如插值）。

3.1 四元数 → 旋转矩阵

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

1-2(y²+z²)	2(xy-wz)	2(xz+wy)
2(xy+wz)	1-2(x²+z²)	2(yz-wx)
2(xz-wy)	2(yz+wx)	1-2(x²+y²)

这个公式我建议你直接背下来，或者存成代码片段。我在做IMU标定时，经常需要在这两种表示之间来回切换。

3.2 旋转矩阵 → 四元数

反过来转换稍微麻烦一点。假设旋转矩阵是 R = [m00, m01, m02; m10, m11, m12; m20, m21, m22]：

// 从旋转矩阵提取四元数
void matrix_to_quat(float R[3][3], float q[4]) {
    float tr = R[0][0] + R[1][1] + R[2][2];
    if (tr > 0) {
        float 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 {
        // 处理特殊情况，这里省略
    }
}

四、四元数的优势

做了这么多年惯性导航，我个人觉得四元数有三大优势：

1. 无万向节锁——这是最核心的优势。欧拉角在俯仰角接近 ±90° 时会丢失一个自由度，四元数完全没有这个问题。你想想看，飞机做特技飞行时俯仰角肯定会超过 90°，这时候用欧拉角就完蛋了。
2. 计算效率高——四元数乘法只需要 16 次浮点乘法和 12 次加法，而 3×3 矩阵乘法需要 27 次乘法和 18 次加法。在嵌入式平台上，这个差距很明显。
3. 插值平滑——四元数的球面线性插值（SLERP）可以生成平滑的旋转过渡。我在做云台控制时就用这个特性，让相机转动特别顺滑，没有卡顿感。

五、知识体系总览

下面这张图是我自己整理的，把四元数相关的知识点串在了一起。你可以把它当作一个快速索引。

这张图把四元数的定义、运算、转换、优势和应用串在了一起。你可以看到，所有知识点都围绕着「四元数基础」这个核心展开。我个人习惯在学新东西时先画这样的图，能帮自己建立全局视角。

---

公众号:蓝海资料掘金营，微信deep3321