刚体变换与位姿表示：旋转矩阵、欧拉角、四元数、齐次变换矩阵

各位同学，欢迎来到第三章。这一章我们要啃的，是机器人视觉里最基础也最绕不开的一块硬骨头——刚体变换与位姿表示。

说白了，就是回答一个问题：一个物体在三维空间里，到底在哪儿？脸朝哪边？

我刚开始做手眼标定时，被这几种表示方法绕得晕头转向。旋转矩阵、欧拉角、四元数、齐次变换矩阵……每个都能描述位姿，但各有各的脾气。今天我就把这几兄弟的底细，连同Python实现，一次性给你讲透。

3.1 旋转矩阵：最正统，但最啰嗦

旋转矩阵，说白了就是一个3×3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的旋转关系。

数学上长这样：

R = [[r11, r12, r13],
     [r21, r22, r23],
     [r31, r32, r33]]

约束条件有两个：R^T R = I（正交性），det(R) = 1（右手系）。

优点： 组合方便，两个旋转矩阵相乘就是复合旋转。没有奇点，不会突然"卡住"。

缺点： 9个参数，冗余。你想想看，明明3个自由度就够了，它非要9个数。而且对人来说，看着一堆数字完全想象不出物体朝哪转。

3.2 欧拉角：直观，但有坑

欧拉角用三个角度表示旋转：绕X轴（滚转Roll）、绕Y轴（俯仰Pitch）、绕Z轴（偏航Yaw）。

顺序很重要！常见的顺序有ZYX、XYZ等。不同顺序，结果完全不同。

Python里用scipy的Rotation模块可以轻松转换：

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

# 欧拉角 → 旋转矩阵
euler = [30, 45, 60]  # 单位：度
r = R.from_euler('zyx', euler, degrees=True)
matrix = r.as_matrix()
print("旋转矩阵:\n", matrix)

# 旋转矩阵 → 欧拉角
euler_back = r.as_euler('zyx', degrees=True)
print("欧拉角:", euler_back)

3.3 四元数：工业界的宠儿

四元数，很多人一听就头大。其实没那么玄乎。

一个四元数 q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部。它描述的是绕一个单位向量 (x, y, z) 旋转 2*arccos(w) 角度。

为什么工业界爱用四元数？

• 没有万向锁
• 插值平滑（SLERP）
• 计算效率高
• 存储只有4个浮点数

我个人的习惯是：在IMU数据融合、机械臂轨迹规划中，一律用四元数。 只有在最后给用户看或者做可视化时，才转成欧拉角。

# 四元数 → 旋转矩阵
q = [0.707, 0.0, 0.707, 0.0]  # w, x, y, z
r = R.from_quat(q)
matrix = r.as_matrix()
print("四元数对应的旋转矩阵:\n", matrix)

# 旋转矩阵 → 四元数
q_back = r.as_quat()
print("四元数:", q_back)

# 四元数插值（SLERP）
from scipy.spatial.transform import Slerp
import numpy as np

key_rots = R.from_quat([
    [1, 0, 0, 0],
    [0.707, 0, 0.707, 0]
])
key_times = [0, 1]
slerp = Slerp(key_times, key_rots)
interp_times = [0, 0.25, 0.5, 0.75, 1.0]
interp_rots = slerp(interp_times)
print("插值后的四元数:\n", interp_rots.as_quat())

3.4 齐次变换矩阵：把旋转和平移装进一个矩阵

齐次变换矩阵是一个4×4的矩阵：

T = [[R, t],
     [0, 1]]

其中 R 是3×3旋转矩阵，t 是3×1平移向量，最后一行是 [0, 0, 0, 1]。

它的威力在于：一次矩阵乘法，同时完成旋转和平移。

比如，把点 p 从坐标系A变换到坐标系B：

p_b = T_a_to_b @ p_a

这里的 p_a 和 p_b 都是齐次坐标 [x, y, z, 1]^T。

# 构建齐次变换矩阵
def build_transform(R, t):
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

# 示例
R_mat = R.from_euler('zyx', [30, 45, 60], degrees=True).as_matrix()
t_vec = np.array([0.1, 0.2, 0.3])
T = build_transform(R_mat, t_vec)
print("齐次变换矩阵:\n", T)

# 变换一个点
p = np.array([1.0, 0.0, 0.0, 1.0])
p_transformed = T @ p
print("变换后的点:", p_transformed[:3])

3.5 四种表示方法的对比与选择

我个人建议的实践原则：

• 存储和传输： 用四元数+平移向量（7个浮点数）
• 计算和变换： 用齐次变换矩阵（4×4）
• 显示和调试： 用欧拉角+平移向量（6个值）
• 理论分析： 用旋转矩阵