SO(3) & SE(3) 旋转与刚体变换¶

English contract: Foundations overview · Primary references: SO(3) and SE(3)

前置要求: 05-coordinate-transform.md（齐次坐标、3D 变换矩阵） 预计学习时间: 2–3 小时 完成后你能: 在欧拉角、旋转矩阵、轴角、四元数之间互相转换；解释 SO(3) 与 SE(3) 的群性质；理解万向锁成因并知道为何仿真器内部都用四元数。

1. 旋转表示总览¶

同一个三维旋转，有四种常见"语言"描述它。它们描述的是同一件事，但各有优劣：

表示法	自由度	参数数	主要优点	主要缺点
欧拉角 (RPY)	3	3	直观、人好读	万向锁、顺序依赖
旋转矩阵	3	9	直接做坐标变换、无歧义	9 个数冗余、需正交化
轴角 (Axis-Angle)	3	4	直观（绕某轴转某角）	0°/180° 附近不唯一
四元数 (Quaternion)	3	4	无万向锁、插值平滑、紧凑	不直观、需归一化

自由度 vs 参数数：3D 旋转只有 3 个自由度，但旋转矩阵有 9 个数（多出 6 个约束），四元数有 4 个数（多出 1 个约束 ‖q‖=1）。参数数 > 自由度意味着有冗余约束。

2. SO(3)：特殊正交群¶

SO(3)（Special Orthogonal group）就是所有 3D 旋转矩阵的集合：

SO(3) = { R ∈ ℝ³ˣ³ | RᵀR = I,  det(R) = +1 }

它有两个约束条件，对应物理含义：

RᵀR = I（正交）：旋转不改变长度也不改变角度。每一列都是单位向量、两两垂直。
det(R) = +1（特殊）：排除掉镜像反射（det = -1）。镜像不是"旋转能到"的。

旋转矩阵的性质¶

性质	公式	物理含义
逆 = 转置	R⁻¹ = Rᵀ	反向旋转
保长度	‖Rp‖ = ‖p‖	刚体转动不变形
保角度	(Rp)·(Rq) = p·q	刚体转动不改变夹角
行列式为 1	det(R)=1	纯旋转，无翻转
闭合	R₁R₂ ∈ SO(3)	两个旋转复合仍是旋转

直觉：SO(3) 是一个"群"——你可以把任意两个旋转相乘得到另一个合法旋转，也能求逆。这正是机器人连杆旋转能链式复合的数学基础。

3. SE(3)：特殊欧氏群¶

SE(3)（Special Euclidean group）= 旋转 + 平移，即上一节学的 4×4 齐次变换矩阵的集合：

SE(3) = { T = [R t; 0 1] | R ∈ SO(3), t ∈ ℝ³ }

SE(3) 描述刚体变换 (rigid body transformation)：既旋转又平移，但不发生形变。机械臂末端位姿、相机位姿、物体 6D 位姿都属于 SE(3)。

SE(3) 的维数：6 (= 3 旋转 + 3 平移)

记忆：SO(3) 管转（3 自由度），SE(3) 管转 + 移（6 自由度）。机器人里常说的"6D 位姿 (6-DoF pose)"指的就是 SE(3) 中的一个元素。

4. 四元数基础¶

4.1 为什么用四元数？¶

欧拉角在 pitch = ±90° 时会丢失一个自由度（万向锁，见第 5 节），而旋转矩阵有 9 个冗余数。四元数用 4 个数 q = [w, x, y, z] 描述旋转，无万向锁、数值稳定、适合插值，是仿真器和游戏引擎的标配。

4.2 四元数的定义¶

单位四元数 q = [w, x, y, z]，满足 w² + x² + y² + z² = 1，对应"绕单位轴 û = (ux, uy, uz) 转 θ 角"：

q = [cos(θ/2),  ux·sin(θ/2),  uy·sin(θ/2),  uz·sin(θ/2)]
     ↑w           ↑x             ↑y             ↑z

直觉：四元数把"轴 + 角"打包成 4 个数。θ/2 是因为四元数在"转两圈才回起点"——这是它的数学结构决定的，记住即可。

4.3 Hamilton 乘积¶

两个四元数相乘（Hamilton product）等价于两次旋转的复合。顺序与矩阵一样不可交换：

q = q₁ ⊗ q₂   (先 q₂ 后 q₁)

4.4 四元数 → 旋转矩阵¶

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

# ---- 四元数 <-> 旋转矩阵 (用 scipy 标准化) ----
# scipy 用 [x, y, z, w] 顺序，注意与 [w,x,y,z] 区分
quat_xyzw = [0, 0, np.sin(np.pi/4), np.cos(np.pi/4)]   # 绕 z 轴转 90°
rot = R.from_quat(quat_xyzw)

R_mat = rot.as_matrix()           # 四元数 → 旋转矩阵
print("旋转矩阵:\n", R_mat)

euler = rot.as_euler('xyz', degrees=True)   # → 欧拉角
print("欧拉角 (xyz):", euler)               # [0, 0, 90]

def quat_to_rotmat(w, x, y, z):
    """四元数 [w,x,y,z] → 3x3 旋转矩阵（手写版，便于理解结构）"""
    return np.array([
        [1-2*(y*y+z*z), 2*(x*y-w*z),   2*(x*z+w*y)],
        [2*(x*y+w*z),   1-2*(x*x+z*z), 2*(y*z-w*x)],
        [2*(x*z-w*y),   2*(y*z+w*x),   1-2*(x*x+y*y)],
    ])

q = [np.cos(np.pi/4), 0, 0, np.sin(np.pi/4)]   # [w,x,y,z], 绕 z 转 90°
print(quat_to_rotmat(*q))

5. 万向锁（Gimbal Lock）¶

5.1 成因¶

欧拉角按固定顺序分解旋转（如 ZYX = yaw→pitch→roll）。当中间角 pitch = ±90° 时，第一和第三个旋转轴重合，丢失一个自由度——这就是万向锁。

正常情况：三个轴各管一个方向      万向锁 (pitch=90°)：首尾两轴重合
   z                                z
   ↑                                ↑
   │  y                              \  y=x (重合!)
   │ /                                \ /
   ●───→ x                            ●───→

直觉：飞机机头上仰 90° 后，再"偏航"和再"滚转"效果一样——你分不清是动哪个轴了。

5.2 四元数如何解决¶

四元数不分阶段、不依赖顺序，它直接描述"绕一个轴转一个角"，因此不存在轴重合的问题。这就是 MuJoCo、PyBullet、Unity 等引擎内部统一用四元数的原因。

# 演示万向锁：pitch=90° 时，yaw 与 roll 耦合，不同组合得到同一旋转
rot_lock = R.from_euler('ZYX', [30, 90, 10], degrees=True)     # yaw=30,pitch=90,roll=10
rot_same = R.from_euler('ZYX', [40, 90, 20], degrees=True)     # yaw=40,pitch=90,roll=20
print("两个矩阵几乎相同:", np.allclose(rot_lock.as_matrix(),
                                     rot_same.as_matrix()))    # True → 自由度丢失

6. 指数映射与对数映射（简介）¶

旋转还可以用"旋转矢量"ω（3 维，方向=转轴、模长=转角）表示。它与旋转矩阵之间通过指数/对数映射互转：

旋转矩阵:   R = exp(ω̂)        （指数映射，ω̂ 是 ω 的反对称矩阵）
旋转矢量:   ω = log(R)        （对数映射）

这是 SO(3) 上的"李代数 so(3)"。对刚入门者只需记住：指数/对数映射是"旋转矢量 ↔ 旋转矩阵"的桥梁，也是四元数插值（SLERP）与位姿优化的数学基础。scipy 提供了 as_rotvec() 直接用：

rot = R.from_euler('z', 90, degrees=True)
omega = rot.as_rotvec()              # 旋转矢量 = log(R)
print("旋转向量:", omega)            # [0, 0, 1.5708] → 绕 z 轴转 90°
print("还原:", R.from_rotvec(omega).as_euler('xyz', degrees=True))  # [0,0,90]

7. 旋转表示互相转换一览¶

        ┌──────────┐  as_euler   ┌──────────┐
        │ 旋转矩阵  │ ──────────► │  欧拉角   │
        │  (R)     │ ◄────────── │ (RPY)    │
        └────┬─────┘  from_euler └──────────┘
             │ as_quat / from_quat
             ▼
        ┌──────────┐  as_rotvec  ┌──────────┐
        │  四元数   │ ──────────► │  轴角/    │
        │  (q)     │ ◄────────── │ 旋转向量  │
        └──────────┘  from_rotvec└──────────┘

# 完整转换演示：同一旋转走遍四种表示
rot = R.from_euler('XYZ', [30, 45, 60], degrees=True)

print("旋转矩阵:\n", rot.as_matrix().round(3))
print("欧拉角 :", rot.as_euler('XYZ', degrees=True).round(1))
print("四元数 :", rot.as_quat().round(3))      # [x,y,z,w]
print("旋转向量:", rot.as_rotvec().round(3))    # 方向=轴, 模长=角(弧度)

# 关键：无论从哪种表示出发，as_matrix() 得到的旋转矩阵一致
assert np.allclose(R.from_quat(rot.as_quat()).as_matrix(),
                   R.from_rotvec(rot.as_rotvec()).as_matrix())

8. 连接项目¶

关节角 → 末端位姿：examples/fk_ik_demo.py 输入关节角 (theta1, theta2)，输出末端 (x, y)。每段连杆的旋转拼起来就是 SO(3)/SE(3) 的链式复合（2D 下退化为 SO(2)）。
MuJoCo 内部用四元数：MuJoCo 的 qpos 用四元数 [w, x, y, z] 存刚体朝向，而不是欧拉角，正是为了避免万向锁。读取/设置位姿时务必用四元数或 mjuu_quat2mat 转换。
3D 手指链：examples/finger_chain_3d.py 在三维空间里做正运动学，每段连杆的旋转就是 SO(3) 元素，末端位姿属于 SE(3)。

检查理解¶

概念题：SO(3) 的两个约束 RᵀR=I 和 det(R)=+1 分别排除了哪类"不合法"的变换？如果允许 det(R)=-1 会发生什么？
辨析题：3D 旋转只有 3 个自由度，为什么旋转矩阵有 9 个数、四元数有 4 个数？多出来的参数受什么约束？
万向锁题：用代码验证 R.from_euler('ZYX',[30,90,10]) 与 R.from_euler('ZYX',[40,90,20]) 的旋转矩阵相同，并解释为什么。
转换题：给定四元数 q=[0.5, 0.5, 0.5, 0.5]（先判断它是否单位四元数），分别求出对应的旋转矩阵、欧拉角和旋转向量。
项目题：finger_chain_3d.py 中每段连杆的旋转属于 SO(3) 还是 SE(3)？整个手指末端的 6D 位姿属于哪个群？若要把它存入 MuJoCo，应该用哪种旋转表示？

下一篇: 07-fk-jacobian-ik.md —— 把这些变换串成正运动学，并用 Jacobian 反解关节角。