第五讲：李群与旋转 — SO(3) / SE(3)

机器人的每一次运动都是一次李群上的行走：关节旋转 $\in SO(3)$ ，末端轨迹 $\in SE(3)$ 。正运动学 FK 是 SE(3) 元素的链式乘积，物理流形 $\mathcal{M}_{phy}$ 的几何结构由此而来。

符号	含义
$SO(3)$	特殊正交群：所有旋转矩阵
$[\omega]_\times$	反对称矩阵（ $\mathfrak{so}(3)$ 代数元素）
$\exp(\theta[\omega]_\times)$	Rodrigues 公式：轴角 → 旋转矩阵
$SE(3)$	特殊欧氏群：旋转 + 平移
$T_{end}$	末端执行器 SE(3) 位姿

前言：理论发展沿革

1840年，法国数学家罗德里格斯（Rodrigues）发表了用轴-角参数化旋转的公式——即今天机器人学中无处不在的 Rodrigues 公式。那时距离李群理论诞生还有40年，罗德里格斯只是在寻找旋转复合的简便计算方法，却无意中写下了李群指数映射在 $SO(3)$ 上的具体实例。同年代，哈密顿（Hamilton）在1843年发现四元数，苦苦寻思了整整15年——答案是在都柏林运河边散步时突然降临，他随手将公式刻在了桥墩上。今天的机器人 SLERP 插值，正是四元数的工程遗产。

1872年，克莱因（Felix Klein）在埃尔朗根提出著名的”埃尔朗根纲领”：每一种几何都对应一个变换群，几何学的本质是研究在特定变换群下不变的性质。同年，挪威数学家索甫斯·李（Sophus Lie）开始系统研究连续变换群，历时二十年建立了完整理论。李原本的动机是寻找微分方程的对称性——如同伽罗瓦用离散群解释多项式方程，李试图用连续群理解微分方程的可积性。

20世纪，嘉当（Élie Cartan）深化了李群结构理论，引入李代数、Killing 型与嘉当分类，完成了单李代数的完全分类。诺特（Emmy Noether）1915年则证明：物理系统的每一个连续对称性都对应一个守恒量——能量守恒来自时间平移不变性（平移群），动量守恒来自空间平移不变性，角动量守恒来自旋转不变性（ $SO(3)$ ）。李群的物理意义由此凸显。

1994年，默里（Murray）、李（Li）和萨斯特里（Sastry）出版《机器人操作的数学导论》，将 $SE(3)$ 李群系统引入机器人正/逆运动学。此后 $SE(3)$ 成为机器人学的标准数学工具。EICPS 中物理流形 $\mathcal{M}_{phy}$ 的切空间结构直接来源于 $\mathfrak{se}(3)$ 李代数——理解这段历史，就理解了为什么关节空间的”直线运动”必须用指数映射 $\exp(\theta[\omega]_\times)$ 而非简单向量加法来表达。

1 SO(3)：旋转群与指数映射

特殊正交群 $SO(3)$ 是所有 $3\times3$ 旋转矩阵的集合：

$SO(3) = \{R \in \mathbb{R}^{3\times3} \mid R^\top R = I,\; \det(R) = 1\}$

其 Lie 代数 $\mathfrak{so}(3)$ 是所有反对称矩阵，对应旋转轴-角表示。指数映射（Rodrigues 公式）将代数元素映射到群：

$\exp(\theta\,[\omega]_\times) = I + \sin\theta\,[\omega]_\times + (1-\cos\theta)\,[\omega]_\times^2$

其中 $\omega \in \mathbb{R}^3$ 为单位旋转轴， $\theta$ 为旋转角， $[\omega]_\times$ 为对应的反对称矩阵。

图1：SO(3) 指数映射 — 四种轴角旋转可视化。虚线=原始坐标轴，实线=旋转后坐标轴，彩色粗箭头=旋转轴 ω，弧线=旋转角 θ。Rodrigues 公式直接在 R³×³ 中完成计算，无需四元数或欧拉角。

2 旋转插值：SLERP vs 欧拉角

在 $SO(3)$ 上的**球面线性插值（SLERP）**沿测地线匀速旋转：

$R(t) = R_0 \cdot \exp\bigl(t \cdot \log(R_0^\top R_1)\bigr), \quad t \in [0,1]$

欧拉角线性插值会产生非测地路径与速度不均匀，在极端情况下出现万向锁（Gimbal Lock）。

插值方法	路径	角速度	奇异性
SLERP	测地线（最短）	匀速	无
欧拉角线性	非测地，迂回	不均匀	万向锁
四元数 SLERP	等价于 SO(3) SLERP	匀速	无（双覆盖）

图2：SLERP vs 欧拉角线性插值对比。左：x 轴方向在单位球 S² 上的旋转轨迹，SLERP（蓝绿）走测地线，欧拉线性（琥珀）绕远路；中：欧拉角随插值参数的变化，欧拉线性在某段出现非线性跳变；右：每步旋转角度（角速度），SLERP 完全匀速，欧拉线性速度波动大。

3 SE(3)：刚体变换群

特殊欧氏群 $SE(3)$ 描述三维刚体变换（旋转 + 平移）：

$SE(3) = \left\{ T = \begin{pmatrix} R & p \\ 0 & 1 \end{pmatrix} \mid R \in SO(3),\; p \in \mathbb{R}^3 \right\}$

变换作用于齐次坐标： $T \cdot \begin{pmatrix} x \\ 1 \end{pmatrix} = \begin{pmatrix} Rx + p \\ 1 \end{pmatrix}$

Lie 代数 $\mathfrak{se}(3)$ 的元素称为twist（旋量），包含线速度 $v$ 和角速度 $\omega$ ：

$\xi = \begin{pmatrix} [\omega]_\times & v \\ 0 & 0 \end{pmatrix} \in \mathbb{R}^{4\times4}$

图3：SE(3) 刚体变换合成。左：纯旋转（SO(3) 子集），坐标系原点不动；中：旋转+平移，灰色虚线为平移向量 p；右：链式变换 T₂ = T₁·T₁₂，机器人 FK 的基本操作单元。

4 机器人正运动学：FK 几何

正运动学（FK） = 各关节 SE(3) 变换的链式乘积：

$T_{end} = T_0 \cdot T_1(q_1) \cdot T_2(q_2) \cdots T_n(q_n)$

其中 $T_k(q_k) = \begin{pmatrix} R_k(q_k) & d_k \\ 0 & 1 \end{pmatrix}$ ， $q_k$ 为第 $k$ 关节角度。

这正是 $\mathcal{M}_{phy}$ （物理流形）的几何结构——关节空间坐标 $q = (q_1,\ldots,q_n)$ 通过 FK 映射到笛卡尔工作空间，工作空间形状完全由 SO(3) 旋转群的代数性质决定。

图4：3-DOF 机器人正运动学。左：5 种关节构型的 FK 骨架（XY 平面），星号=末端执行器，方块=底座；右：随机采样 400 组关节角，末端执行器分布即 M_phy 在笛卡尔空间的投影，颜色表示关节1角度——环状结构由 SO(3) 旋转几何决定。

5 李群在 EICPS 中的定位

层次	李群的使用方式
VLA 大脑层	规划 SE(3) 目标位姿，Jump 切换到新构型
ESP 脊髓层	实时 FK/IK 求解，李代数速度控制
M_phy 物理层	关节空间 $q$ 配备李群几何，黎曼度量来自惯性张量 $M(q)$

图5：李群在 EICPS 中的完整定位。上行工具链：SO(3) → SLERP → SE(3) → FK Chain；下行架构：VLA 规划 SE(3) 位姿，ESP 实时 FK/IK，M_phy 配备李群几何度量。

配套 Notebook

Notebook 包含完整可运行代码：Rodrigues 公式、SLERP 对比、SE(3) 链式合成、3-DOF 机器人 FK 与工作空间采样。

参考文献

Rodrigues, O. (1840). Des lois géométriques qui régissent les déplacements d’un système solide dans l’espace. Journal de Mathématiques Pures et Appliquées, 5, 380–440. Rodrigues 公式的原始出处：用轴-角参数化旋转，比矩阵乘法早40年，是 $\exp(\theta[\omega]_\times)$ 的历史先驱。
Murray, R. M., Li, Z., & Sastry, S. S. (1994). A Mathematical Introduction to Robotic Manipulation. CRC Press. 将 $SE(3)$ 李群系统引入机器人学的权威教材，本讲 FK 链式乘积、指数坐标、旋量理论均参照此书，网络免费获取。
Lynch, K. M., & Park, F. C. (2017). Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press. 现代机器人学教材，全面采用旋量与李群方法，配套 Coursera 课程，是学习 $SE(3)$ 在控制规划中应用的最佳工程入口。
Chirikjian, G. S. (2009). Stochastic Models, Information Theory, and Lie Groups, Vol. 1. Birkhäuser. 李群在概率、信息论与机器人不确定性估计中的高级应用，EST 将不确定性传播形式化为 $SE(3)$ 上的分布演化的理论来源。
Sola, J., Deray, J., & Atchuthan, D. (2018). A micro Lie theory for state estimation in robotics. arXiv:1812.01537. 为机器人状态估计（EKF on Lie groups）提供的实用微型李群教程，是 Rodrigues 公式在滤波器工程中的现代化延伸。

总结

本讲建立了机器人运动学的李群几何基础：

SO(3)：旋转群，Rodrigues 公式 $\exp(\theta[\omega]_\times)$ 直接计算旋转矩阵
SLERP：SO(3) 上的测地线插值，匀速、无奇异，优于欧拉角线性插值
SE(3)：刚体变换群，齐次矩阵编码旋转+平移，链式乘积 = FK
M_phy：关节空间配备李群几何，黎曼度量 $M(q)$ 来自机器人惯性张量

下一讲：具身变换 ET——谱诊断、行为指纹与 ESP 信号的几何结构。