【杂记四】刚体运动 +SE(3)

一、基础知识

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：三维空间中的特殊欧几里得群

本质是：

描述一个刚体在三维空间中的“位置 + 姿态（方向）”的标准数学工具