4、姿态表示方法（下）：四元数的数学定义、四元数乘法、四元数与旋转矩阵的转换、四元数插值
4.1 四元数的数学定义
四元数是一种扩展了复数系统的超复数，由一个实部和三个虚部组成。在姿态描述中，它提供了一种无奇异性、计算效率高的旋转表示方法。
标准形式：
一个四元数 \( q \) 可以表示为：
\[ q = w + xi + yj + zk \]其中 \( w, x, y, z \) 是实数，\( i, j, k \) 是虚数单位，满足以下乘法规则：
\[ i^2 = j^2 = k^2 = ijk = -1 \] \[ ij = k, \quad ji = -k \] \[ jk = i, \quad kj = -i \] \[ ki = j, \quad ik = -j \]向量表示法：
在工程应用中，通常将四元数写为标量部分和向量部分的组合：
\[ q = \begin{bmatrix} w \\ \vec{v} \end{bmatrix}, \quad \vec{v} = \begin{bmatrix} x \\ y \\ z \end{bmatrix} \]单位四元数（用于旋转）：
描述姿态的四元数必须是单位四元数，满足：
\[ \|q\| = \sqrt{w^2 + x^2 + y^2 + z^2} = 1 \]4.2 四元数乘法
四元数乘法（又称哈密顿积）是组合两个旋转的核心运算。给定两个四元数 \( p = [p_w, \vec{p}_v]^T \) 和 \( q = [q_w, \vec{q}_v]^T \)，其乘积 \( r = p \otimes q \) 定义为：
代数形式：
\[ r = p \otimes q = \begin{bmatrix} p_w q_w - p_x q_x - p_y q_y - p_z q_z \\ p_w q_x + p_x q_w + p_y q_z - p_z q_y \\ p_w q_y - p_x q_z + p_y q_w + p_z q_x \\ p_w q_z + p_x q_y - p_y q_x + p_z q_w \end{bmatrix} \]向量形式（更简洁）：
\[ r = \begin{bmatrix} p_w q_w - \vec{p}_v \cdot \vec{q}_v \\ p_w \vec{q}_v + q_w \vec{p}_v + \vec{p}_v \times \vec{q}_v \end{bmatrix} \]重要性质：
- 非交换性： \( p \otimes q \neq q \otimes p \)（除非旋转轴相同）
- 结合律： \( (p \otimes q) \otimes r = p \otimes (q \otimes r) \)
- 单位元： \( q \otimes [1,0,0,0]^T = [1,0,0,0]^T \otimes q = q \)
- 逆元： \( q^{-1} = \frac{q^*}{\|q\|^2} \)，对于单位四元数 \( q^{-1} = q^* = [w, -x, -y, -z]^T \)
旋转组合应用：
若先用四元数 \( q_1 \) 旋转，再用 \( q_2 \) 旋转，等效旋转为：
\[ q_{total} = q_2 \otimes q_1 \]4.3 四元数与旋转矩阵的转换
从四元数到旋转矩阵：
给定单位四元数 \( q = [w, x, y, z]^T \)，对应的旋转矩阵 \( R \) 为：
\[ R = \begin{bmatrix} 1 - 2(y^2 + z^2) & 2(xy - wz) & 2(xz + wy) \\ 2(xy + wz) & 1 - 2(x^2 + z^2) & 2(yz - wx) \\ 2(xz - wy) & 2(yz + wx) & 1 - 2(x^2 + y^2) \end{bmatrix} \]从旋转矩阵到四元数：
给定旋转矩阵 \( R = [r_{ij}] \)，提取四元数的方法如下（需处理数值稳定性）：
| 条件 | 计算方式 |
|---|---|
| \( w \) 最大 | \( w = \frac{1}{2}\sqrt{1 + r_{11} + r_{22} + r_{33}} \) \( x = \frac{r_{32} - r_{23}}{4w} \) \( y = \frac{r_{13} - r_{31}}{4w} \) \( z = \frac{r_{21} - r_{12}}{4w} \) |
| \( x \) 最大 | \( x = \frac{1}{2}\sqrt{1 + r_{11} - r_{22} - r_{33}} \) \( w = \frac{r_{32} - r_{23}}{4x} \) \( y = \frac{r_{12} + r_{21}}{4x} \) \( z = \frac{r_{13} + r_{31}}{4x} \) |
| \( y \) 最大 | \( y = \frac{1}{2}\sqrt{1 - r_{11} + r_{22} - r_{33}} \) \( w = \frac{r_{13} - r_{31}}{4y} \) \( x = \frac{r_{12} + r_{21}}{4y} \) \( z = \frac{r_{23} + r_{32}}{4y} \) |
| \( z \) 最大 | \( z = \frac{1}{2}\sqrt{1 - r_{11} - r_{22} + r_{33}} \) \( w = \frac{r_{21} - r_{12}}{4z} \) \( x = \frac{r_{13} + r_{31}}{4z} \) \( y = \frac{r_{23} + r_{32}}{4z} \) |
注意： 选择 \( w, x, y, z \) 中绝对值最大的作为分母，可避免除零和数值不稳定问题。
4.4 四元数插值
在飞控中，经常需要在两个姿态之间平滑过渡（如目标姿态跟踪、轨迹规划）。四元数插值提供了比欧拉角更平滑、无万向锁的过渡方式。
4.4.1 线性插值（LERP）
最简单的插值方法，但不会保持恒定的角速度，且插值后的四元数需要归一化：
\[ q(t) = \frac{(1-t)q_1 + t q_2}{\|(1-t)q_1 + t q_2\|}, \quad t \in [0,1] \]4.4.2 球面线性插值（SLERP）
SLERP 在单位四元数球面上沿最短弧进行插值，保持恒定的角速度，是飞控中最常用的插值方法。
计算步骤：
- 计算点积： \( \Omega = q_1 \cdot q_2 = w_1 w_2 + x_1 x_2 + y_1 y_2 + z_1 z_2 \)
- 处理负点积： 若 \( \Omega < 0 \)，取 \( q_2 = -q_2 \)，\( \Omega = -\Omega \)（确保走最短路径）
- 计算角度： \( \theta = \arccos(\Omega) \)
- 插值公式： \[ q(t) = \frac{\sin((1-t)\theta)}{\sin\theta} q_1 + \frac{\sin(t\theta)}{\sin\theta} q_2 \]
数值稳定性处理：
当 \( \theta \) 非常小（接近0）时，为避免除零，退化为 LERP：
\[ \text{if } \sin\theta < \epsilon: \quad q(t) = (1-t)q_1 + t q_2 \]4.4.3 飞控中的实际应用建议
- 姿态设定值生成： 使用 SLERP 从当前姿态平滑过渡到目标姿态，插值时间通常为 0.5~2 秒
- 多段插值： 对于复杂轨迹，可将路径分段，每段使用 SLERP，并在连接点处保证一阶连续性
- 性能优化： 在嵌入式飞控中，可使用查表法或近似公式计算 \( \sin \) 和 \( \arccos \) 以降低计算开销
代码示例（伪代码）：
function slerp(q1, q2, t):
dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z
// 处理负点积，走最短路径
if dot < 0:
q2 = -q2
dot = -dot
// 阈值判断，防止除零
if dot > 0.9995:
// 角度很小，使用线性插值
result = (1-t) * q1 + t * q2
return normalize(result)
theta = acos(dot)
sinTheta = sin(theta)
scale1 = sin((1-t) * theta) / sinTheta
scale2 = sin(t * theta) / sinTheta
result = scale1 * q1 + scale2 * q2
return result