一、基础知识
1. 什么是刚体
刚体（Rigid Body） 就是一个不会变形的物体。
举例：
一个木块、一只水瓶、一台手机，都可以近似看作刚体。
即使你用力搬动它，它内部形状和尺寸不会变化。
2. 三维空间中的刚体运动是什么？
刚体在三维空间中，运动有 两种基本形式：
平移运动（Translation）
整个物体沿着某个方向移动，但方向不变、不会转动。
💡 举例：
你把水杯从桌子左边滑到右边，杯子没转，只是“平移”
旋转运动（Rotation）
刚体围绕某个轴“旋转”，但中心可能不动。
💡 举例：
你拿着一支笔转圈圈（像旋转陀螺），这就是绕笔心的旋转
3.总结
三维空间中刚体运动 = 平移 + 旋转 的组合！
准确地知道一个刚体的位置和方向，就要描述它的：
位置坐标（平移）
姿态 / 朝向（旋转，通常用欧拉角、四元数、旋转矩阵）
这就是机器人、机械臂、无人机、VR 控制器等领域中最基本的物理模型！
二、旋转描述
1. 欧拉角（Euler Angles）
就是用 3 个角度值，分别表示：绕 X轴、Y轴、Z轴 的转动。
比如：先绕 X 轴转 30°，再绕 Y 轴转 45°，再绕 Z 轴转 90°
优点：
通俗、好理解（类似你“抬头-摇头-歪头”）
缺点：
有个严重问题叫 万向节锁（Gimbal Lock）：两个轴重合，导致一个旋转自由度丢失（会卡住）
如何利用缺点：
当旋转一个角度90度，与另一个角度重合，则会减少一个选择自由度。可以运用，限制不需要的自由度。
2. 旋转矩阵（Rotation Matrix）
一个 3×3 的矩阵，可以用来把一个向量旋转到另一个方向。
实质上是通过线性代数实现旋转。
优点：
数学上非常严谨，适合做组合、多次旋转
可以直接用于变换坐标、绘图、仿真
缺点：
不直观、占内存大（9个数）
3. 四元数（Quaternion）
数学家发明的一个超越三维的数学结构
用 4 个数字（而不是 3 或 9 个）就可以稳定表示任意旋转
优点：
没有万向节锁！（解决了欧拉角的问题）
更节省计算资源（比矩阵小，比欧拉角稳定）
缺点：
不直观，你看不懂那 4 个数具体“转了多少度”
但对机器人/无人机/Unity游戏/AR VR 来说超级常用！
4. 旋转变换
你想把一个物体旋转：
绕 X 轴 90°
绕 Y 轴 0°
绕 Z 轴 0°
用欧拉角表示就是 [90, 0, 0]（单位：度）
# 欧拉角 (degree) → 旋转矩阵
import numpy as np
from scipy.spatial.transform import Rotation as R
# 欧拉角 (degree) → 旋转矩阵
euler_angles = [90, 0, 0] # 绕XYZ轴的旋转
r = R.from_euler('xyz', euler_angles, degrees=True)
rotation_matrix = r.as_matrix()
print("Rotation Matrix:\n", rotation_matrix)
欧拉角 ↔ 四元数
四元数（Quaternion）用的是 4 个值：[w, x, y, z]
继续上面的例子：
# 欧拉角 → 四元数
quat = r.as_quat()
print("Quaternion [x, y, z, w]:", quat)
# 四元数 → 欧拉角
r2 = R.from_quat(quat)
euler_back = r2.as_euler('xyz', degrees=True)
print("Recovered Euler:", euler_back)
四元数 ↔ 旋转矩阵
# 四元数 → 旋转矩阵
rot_mat2 = R.from_quat(quat).as_matrix()
print("Rotation Matrix from Quaternion:\n", rot_mat2)
# 旋转矩阵 → 四元数
quat_back = R.from_matrix(rot_mat2).as_quat()
print("Recovered Quaternion:", quat_back)
应用场景
三、SE(3)
1. 定义
SE(3) 全称是：Special Euclidean group in 3D：三维空间中的特殊欧几里得群
本质是：
描述一个刚体在三维空间中的“位置 + 姿态（方向）”的标准数学工具