📚 课程目录第3章刚体变换与姿态表示

3. 刚体变换与姿态表示：旋转矩阵、欧拉角、四元数、齐次坐标与变换矩阵

各位，欢迎来到第三章。这一章，我们聊聊空间里一个物体到底是怎么「转」的。

做传感器融合，你天天都得跟姿态打交道。IMU 告诉你角速度，视觉告诉你特征点在哪，激光雷达告诉你点云坐标。但这些数据来自不同的坐标系——你得把它们对齐。怎么对齐？靠刚体变换。

说白了，刚体变换就是描述一个物体在三维空间里「挪了位置、转了方向」。它不改变物体的形状和大小，只改变它的位姿。我刚开始做机器人时，觉得这东西不就是个矩阵乘法嘛，有啥难的？后来发现，选错表示方式，调试能调到你怀疑人生。

3.1 旋转矩阵：最直观，但最啰嗦

旋转矩阵，本质是一个 3×3 的正交矩阵，行列式为 +1。它把一个向量从一个坐标系映射到另一个坐标系。

假设你在世界坐标系下看到一个点 p，它在相机坐标系下的坐标是 p'，那么：

p' = R * p

其中 R 就是旋转矩阵。它的每一列，分别是原坐标系各轴在目标坐标系中的投影。

优点：无歧义，可组合，可求逆。你随便拿两个旋转矩阵一乘，结果还是旋转矩阵。

缺点：9 个参数，但只有 3 个自由度。冗余。而且你没法直观地看出它转了多大角度、绕哪个轴转的。

我的经验：旋转矩阵适合做中间计算，不适合做人机交互。你让我看一个旋转矩阵，我根本猜不出它对应什么姿态。但你要让我做连续旋转的插值，我肯定用矩阵链乘——稳定、可靠。

3.2 欧拉角：直观，但小心万向锁

欧拉角用三个角度表示旋转：绕 X 轴转（滚转 Roll）、绕 Y 轴转（俯仰 Pitch）、绕 Z 轴转（偏航 Yaw）。顺序很重要，常见的有 ZYX、ZYZ 等。

我习惯用 ZYX 顺序，也就是先偏航、再俯仰、最后滚转。这个顺序在无人机和机器人里很常见。

角度	符号	范围	说明
偏航 (Yaw)	ψ	[-π, π]	绕 Z 轴旋转，相当于左右转头
俯仰 (Pitch)	θ	[-π/2, π/2]	绕 Y 轴旋转，相当于点头
滚转 (Roll)	φ	[-π, π]	绕 X 轴旋转，相当于歪头

优点：直观，三个数就能描述姿态，人脑容易理解。

缺点：有万向锁问题。当俯仰角接近 ±90° 时，偏航和滚转变得不可区分，丢失一个自由度。另外，欧拉角插值不平滑，直接线性插值会导致姿态抖动。

我曾经踩过的坑：有一次做云台控制，直接用欧拉角做 PID 反馈。结果云台转到接近天顶时，突然疯狂抖动。查了两天才发现是万向锁——俯仰角到了 90°，偏航和滚转耦合了。后来果断换成四元数，问题解决。

3.3 四元数：计算界的瑞士军刀

四元数，很多人一听就头大。其实它没那么可怕。四元数是一个超复数，形式为：

q = w + xi + yj + zk

其中 w 是实部，x、y、z 是虚部。它满足 i² = j² = k² = ijk = -1。

单位四元数可以用来表示旋转。给定一个旋转轴 u（单位向量）和旋转角 θ，对应的四元数是：

q = [cos(θ/2), u_x * sin(θ/2), u_y * sin(θ/2), u_z * sin(θ/2)]

用四元数旋转一个向量 v：

v' = q * v * q⁻¹

其中 v 被当作纯虚四元数 [0, v_x, v_y, v_z]。

优点：无万向锁，可平滑插值（球面线性插值 SLERP），计算效率高，存储只需 4 个浮点数。

缺点：不够直观。你拿一个四元数给我，我完全想象不出它对应的姿态。另外，四元数必须归一化，否则会引入畸变。

  关键对比：旋转矩阵适合链式变换，欧拉角适合人机交互，四元数适合插值和滤波。在卡尔曼滤波里，我几乎只用四元数表示姿态——因为它没有奇点，而且归一化约束天然适合做状态估计。

3.4 齐次坐标与变换矩阵：把旋转和平移统一起来

旋转矩阵只能处理旋转。要同时处理旋转和平移，我们需要齐次坐标和变换矩阵。

齐次坐标把一个三维点 [x, y, z]ᵀ 扩展成四维 [x, y, z, 1]ᵀ。这样，一个刚体变换就可以用一个 4×4 的变换矩阵表示：

T = [ R   t ]
    [ 0ᵀ  1 ]

其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量。

变换矩阵的逆也很简单：

T⁻¹ = [ Rᵀ   -Rᵀ * t ]
       [ 0ᵀ      1     ]

你想想看，如果没有齐次坐标，你要先旋转再平移，得写两个式子。有了变换矩阵，一个矩阵乘法搞定一切。

我个人的习惯是：所有坐标系之间的变换，统一用 4×4 变换矩阵表示。这样代码结构清晰，调试也方便。

3.5 知识结构总览

下面这张图，把四种姿态表示的关系和适用场景画清楚了。

3.6 实际项目中的选择建议

说了这么多，到底该用哪个？我根据实际项目经验，给个参考：

IMU 姿态解算：用四元数。Mahony 滤波、Madgwick 滤波都是基于四元数的。欧拉角在俯仰接近 90° 时会炸。
机械臂运动学：用齐次变换矩阵。DH 参数法直接生成 4×4 矩阵，正逆运动学都方便。
人机界面显示：用欧拉角。操作员需要直观看到偏航、俯仰、滚转数值。
视觉 SLAM：内部用四元数或旋转矩阵，输出时转成欧拉角给用户看。
多传感器标定：用齐次变换矩阵。把相机、激光雷达、IMU 之间的外参统一成 4×4 矩阵，方便链式变换。

一个小技巧：如果你不确定该用哪种表示，先问自己三个问题——需要插值吗？需要求导吗？需要给人看吗？插值选四元数，求导选旋转矩阵，给人看选欧拉角。如果都要，那就都实现，内部转来转去。

3.7 代码示例：四种表示互转

下面是一个 Python 示例，演示旋转矩阵、欧拉角、四元数、齐次变换矩阵之间的转换。我用的是 scipy 和 numpy，实际项目中你也可以用 Eigen（C++）或 ROS 的 tf 库。

import numpy as np
from scipy.spatial.transform import Rotation as R

# 1. 欧拉角 → 旋转矩阵
euler = [0.1, 0.2, 0.3]  # [roll, pitch, yaw] 弧度
rot_mat = R.from_euler('xyz', euler).as_matrix()
print("旋转矩阵:\n", rot_mat)

# 2. 旋转矩阵 → 四元数
quat = R.from_matrix(rot_mat).as_quat()  # [x, y, z, w]
print("四元数 (x, y, z, w):", quat)

# 3. 四元数 → 欧拉角
euler_back = R.from_quat(quat).as_euler('xyz')
print("欧拉角 (弧度):", euler_back)

# 4. 构建齐次变换矩阵
t = np.array([1.0, 2.0, 3.0])  # 平移向量
T = np.eye(4)
T[:3, :3] = rot_mat
T[:3, 3] = t
print("齐次变换矩阵:\n", T)

# 5. 用齐次矩阵变换一个点
p = np.array([0.5, 0.0, -0.3, 1.0])  # 齐次坐标
p_transformed = T @ p
print("变换后的点:", p_transformed[:3])

嗯，代码很简单。但要注意：scipy 的四元数顺序是 [x, y, z, w]，而有些库（比如 Eigen）是 [w, x, y, z]。这个坑我踩过，对接不同库时一定要确认顺序。

3.8 避坑指南

最后，分享几个我亲身踩过的坑：

欧拉角顺序：不同领域默认顺序不同。航空常用 ZYX，机器人常用 ZYX 或 XYZ。一定要在文档里写清楚。
四元数归一化：每次更新四元数后，记得归一化。数值误差会累积，导致非单位四元数，旋转会变形。
变换矩阵的逆：不要直接用 np.linalg.inv(T)。用上面给的公式算逆，又快又准。
万向锁检测：如果俯仰角接近 ±90°，欧拉角的偏航和滚转会突变。做控制时，建议加一个保护逻辑，或者直接切换到四元数。