3. 数学基础回顾（二）：旋转矩阵、欧拉角、四元数、相互转换与插值
好，我们继续。上一章聊完了位置和速度，这一章咱们得把「姿态」这块硬骨头啃下来。
说实话，我刚入行那会儿，觉得旋转不就是转个角度嘛，有啥难的？结果第一次做机械臂的轨迹规划，手臂在奇异点附近直接飞了——嗯，从那以后我再也不敢小看姿态表示了。
这一章，我会把旋转矩阵、欧拉角、四元数这三个东西讲透。它们各自有什么脾气，怎么互相转换，以及做插值时该选谁。你想想看，如果连姿态都描述不清楚，轨迹规划就是空中楼阁。
3.1 旋转矩阵：最直观，但也最啰嗦
旋转矩阵，说白了就是一个 3x3 的矩阵。它把一个向量从坐标系 A 映射到坐标系 B。
它的好处是啥？直观。你一眼就能看出三个轴分别指向哪。而且组合多个旋转时，直接矩阵乘法就行，非常规整。
核心性质：
- 正交矩阵：
R^T * R = I，逆矩阵就是转置。 - 行列式为 +1（纯旋转，不含反射）。
- 9 个元素，但只有 3 个自由度——说白了，冗余信息太多。
我在项目中遇到过一个问题：用旋转矩阵做插值。比如从姿态 A 平滑过渡到姿态 B，直接对矩阵的 9 个元素做线性插值？结果得到的矩阵根本不是正交矩阵，姿态全变形了。所以，旋转矩阵适合做「计算」，不适合做「插值」。
3.2 欧拉角：人类友好，但小心万向锁
欧拉角用三个角度来描述旋转：绕 X 轴转（滚转 Roll）、绕 Y 轴转（俯仰 Pitch）、绕 Z 轴转（偏航 Yaw）。
听起来很符合直觉，对吧？我刚开始做无人机飞控时，用的就是欧拉角。调试时看着角度数值变化，心里特别踏实。
避坑指南：万向锁（Gimbal Lock）
我曾经在一次机械臂的路径规划中，遇到了俯仰角接近 ±90° 的情况。结果滚转和偏航突然「耦合」了，丢失了一个自由度。飞机在空中乱转，机械臂在奇异点附近抖动——这就是万向锁。
为什么会这样？因为欧拉角的旋转顺序是固定的（比如 ZYX），当第二个旋转角达到 ±90° 时，第一个和第三个旋转轴会重合，自由度从 3 降为 2。
欧拉角的另一个问题是：同样的姿态，可以用多组不同的欧拉角表示。这就导致插值时路径不唯一，甚至会出现「绕远路」的情况。所以，我个人习惯：只在人机交互界面用欧拉角，算法内部尽量不用。
3.3 四元数：计算界的瑞士军刀
四元数，听起来很玄乎，其实就是一个四维向量：q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部。
它没有万向锁，插值平滑，计算效率高。现代机器人、游戏引擎、航天器姿态控制，几乎都在用四元数。
我的经验：
刚开始学四元数时，我也觉得抽象。后来我把它理解为「绕一个单位轴 u 旋转 θ 角度」：
q = [cos(θ/2), u*sin(θ/2)]
这么一想，是不是就清晰多了？
四元数的乘法不满足交换律，这点要注意。两个四元数 q1 和 q2 的乘积，表示先应用 q2 再应用 q1 的复合旋转。
3.4 三种表示的对比
| 特性 | 旋转矩阵 | 欧拉角 | 四元数 |
|---|---|---|---|
| 自由度 | 9（冗余） | 3 | 4（单位约束） |
| 万向锁 | 无 | 有 | 无 |
| 插值平滑性 | 差 | 差 | 好（球面插值） |
| 计算效率 | 中等 | 高 | 高 |
| 人类可读性 | 差 | 好 | 差 |
| 组合旋转 | 矩阵乘法 | 角度加减（小心） | 四元数乘法 |
3.5 相互转换：代码实现
下面给出最常用的转换函数。我建议你直接收藏这段代码，项目里经常要用。
import numpy as np
def rotation_matrix_to_euler(R):
"""旋转矩阵 -> 欧拉角 (ZYX顺序)"""
sy = np.sqrt(R[0,0]**2 + R[1,0]**2)
singular = sy < 1e-6
if not singular:
x = np.arctan2(R[2,1], R[2,2])
y = np.arctan2(-R[2,0], sy)
z = np.arctan2(R[1,0], R[0,0])
else:
x = np.arctan2(-R[1,2], R[1,1])
y = np.arctan2(-R[2,0], sy)
z = 0
return np.array([x, y, z])
def euler_to_quaternion(roll, pitch, yaw):
"""欧拉角 -> 四元数 (ZYX顺序)"""
cr = np.cos(roll * 0.5)
sr = np.sin(roll * 0.5)
cp = np.cos(pitch * 0.5)
sp = np.sin(pitch * 0.5)
cy = np.cos(yaw * 0.5)
sy = np.sin(yaw * 0.5)
w = cr * cp * cy + sr * sp * sy
x = sr * cp * cy - cr * sp * sy
y = cr * sp * cy + sr * cp * sy
z = cr * cp * sy - sr * sp * cy
return np.array([w, x, y, z])
def quaternion_to_rotation_matrix(q):
"""四元数 -> 旋转矩阵"""
w, x, y, z = q
R = np.array([
[1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y],
[2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x],
[2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y]
])
return R
注意：欧拉角的转换结果依赖于旋转顺序。上面代码用的是 ZYX 顺序（先绕 Z，再绕 Y，最后绕 X）。如果你的系统用的是其他顺序（比如 XYZ），公式需要相应调整。我曾经因为没注意这个细节，调试了整整两天。
3.6 姿态插值：为什么四元数是王者
做轨迹规划时，经常需要让机器人从姿态 A 平滑运动到姿态 B。这时候，插值方法就很重要了。
- 对欧拉角做线性插值：简单，但路径可能不自然，而且会触发万向锁。
- 对旋转矩阵做线性插值：结果矩阵不正交，姿态变形。
- 对四元数做球面线性插值（Slerp）：路径最短、最平滑，而且保证单位四元数。
Slerp 的公式如下：
def slerp(q1, q2, t):
"""四元数球面线性插值, t in [0, 1]"""
dot = np.dot(q1, q2)
# 如果点积为负，取反以保证最短路径
if dot < 0:
q2 = -q2
dot = -dot
# 防止除零
dot = np.clip(dot, -1, 1)
theta = np.arccos(dot)
if theta < 1e-6:
return q1 # 角度太小，直接返回
sin_theta = np.sin(theta)
return (np.sin((1-t)*theta) / sin_theta) * q1 + (np.sin(t*theta) / sin_theta) * q2
小技巧：插值前检查两个四元数的点积。如果点积为负，说明它们在四维球面上走的是「远路」。把其中一个取反，就能保证走最短路径。这个坑我踩过，现在每次写 Slerp 都会加上这个判断。
3.7 知识体系总览
下面这张图，把三种姿态表示的关系、转换路径和应用场景串起来了。你可以把它当作本章的「导航图」。
嗯，这一章的内容就到这里。旋转矩阵、欧拉角、四元数，这三兄弟各有各的脾气。你只要记住：计算用四元数，显示用欧拉角，组合用旋转矩阵，就能避开大部分坑。