01 se3 pose representation
本文是「位姿与刚体几何」系列第 1 篇。配套代码：
code/se3_demo.py，所有结论都可以用脚本复现，建议边读边跑。
直觉层¶
机器人每时每刻都在回答同一个问题¶
机械臂末端在哪、朝向哪边；无人机在空中是什么姿态；人形机器人的躯干相对地面歪了多少。这些问题全是同一个形式：一个刚体相对另一个刚体的位姿（position + orientation）。
位置好办，三个数 (x, y, z) 就是全部。麻烦的是朝向。
朝向为什么难？因为"转"不是三个独立数字能老实描述的东西。你可以用三个欧拉角，但飞机俯仰 90° 时，偏航和滚转突然变成同一个动作（万向锁）；你可以用四元数，四个数也很优雅，但两个四元数"相加平均"得到的不是中间姿态。
最诚实的出发点是旋转矩阵：一个 3×3 的正交矩阵，9 个数。
9 个数，只有 3 个自由度¶
旋转矩阵 R 满足两个约束：
RᵀR = I（列向量互相垂直且为单位长）—— 6 个独立方程det(R) = +1（不许把空间翻成镜像）—— 1 个方程
9 − 6 − 1 = 2？不对，这些约束不独立，实际留给旋转的自由度是 3。你可以不背这个减法，直接做实验：对旋转矩阵的 9 个元素做数值扰动，看哪些方向"动得了"。配套代码的验证 [1] 做的就是这件事——
[1] 切空间数值雅可比 shape=(9, 3), rank=3
9 维空间里只有 3 个方向能动，其余 6 个方向一推就被正交性弹回来。旋转矩阵的冗余不是浪费，是它在替你守住"这是一个旋转"的承诺：你随手写一个 9 维向量，多半根本不是旋转；但你随手写一个接近正交的矩阵，它仍然是（近似的）旋转。这个性质后面做优化时有大用，先按下不表。
左乘还是右乘：新手第一大坑¶
同样是"再绕 z 轴转 10°"，有两种完全不同的含义：
R ← Rz(10°) · R：在世界坐标系里转。机器人悬在空中，绕世界原点的 z 轴公转，位置会跟着画圆。R ← R · Rz(10°)：在本体坐标系里转。机器人绕自己身上的 z 轴自转，位置不动。
验证 [4] 把这件事量化了：只加旋转扰动时，左乘会顺带改变平移，右乘不会。这不是数学趣味，它直接决定了后面状态估计文章里"为什么 IMU 融合几乎总用右扰动"——因为扰动应该只影响姿态，不该把已经估计好的位置一起带偏。
记忆法：矩阵从右往左读。越靠右的动作，越发生在"更本体的"坐标系里。
进阶层¶
SE(3)：把位姿当成一个群¶
齐次变换矩阵把旋转和平移打包成 4×4：
SE(3) 是一个李群：既是群（可乘、可逆），又是光滑流形（可以求导、可以做优化）。这句话是整个 M1 主线的钥匙，值得拆开：
- 群：两个位姿复合还是位姿（
T₁T₂），逆还是位姿（T⁻¹）。 - 流形：位姿的集合不是一块平地，而是一个弯曲的曲面。你在曲面上做"线性插值""加法更新"，都会滑出曲面——除非你沿着曲面的切空间走。
李代数 se(3) 就是 SE(3) 在单位元处的切空间。exp 映射负责从切空间"走回"曲面，log 反之。工程上你需要记住三条：
1. 指数映射有闭式解（Rodrigues 公式），代码里的 exp_so3 / exp_se3 各只有几行。这就是为什么机器人学库从不存欧拉角——存旋转向量或矩阵，更新时一步 exp 就回曲面。
2. log 在接近 π 时会病态。验证 [2] 显示正常角度往返误差 ~1e-11，而接近 π 时不仅精度下降，还有符号翻转问题（+π 和 −π 是同一个旋转）。如果你的优化器在某个迭代里姿态突然跳变 180°，十有八九是撞上了这里。处理方式：把旋转限制在 (−π, π) 内追踪连续性，或者干脆用增量式更新避免全局 log。
3. 参数残差和位姿残差不是一回事。验证 [3] 中，|φ| 较大时 log(exp(ξ)) 的参数值可能和 ξ 不同，但回代后的位姿完全一致。做优化时，残差应该定义在位姿（或李代数差）上，而不是原始参数向量上——这是很多自研优化代码翻车的地方。
三种表示，三种代价¶
| 表示 | 维数 | 插值 | 万向锁 | 适用场景 |
|---|---|---|---|---|
| 欧拉角 | 3 | 直接线性插值 | 有，俯仰 ±90° 时退化 | 人机交互、日志可读 |
| 四元数 | 4 | slerp/nlerp | 无 | 运行时状态、IMU |
| 旋转矩阵 / 李代数 | 9 / 3 | 流形插值 | 无 | 优化、复合变换 |
图：多轴耦合下，欧拉角线性插值的偏差随转角按 1−cos(θ/2) 增长，90° 处达 10.66°；四元数插值始终为 0。数据来自配套实验，可用 code/se3_demo.py --plot 复现。
单轴旋转时三者都没问题，坑全在多轴耦合。配套代码的验证 [5] 做了一个实验：绕一根倾斜轴从单位阵转到 90°，比较"取一半"时各种插值方法偏离真实半程姿态多少度：
矩阵线性混合 (SVD 投影后) 0.000° *（见下方说明）
欧拉角线性插值 10.660°
四元数归一化插值 0.000°
欧拉角插值在 90° 处偏差超过 10°，且误差随转角按 1−cos(θ/2) 增长——这就是为什么任何涉及"平滑转向"的功能（相机云台、动画、轨迹插值）都必须走四元数或流形插值。
关于矩阵混合的说明：从单位阵出发转 t 角，中点矩阵恰好 SVD 投影回精确的半角旋转，所以误差为 0——这是这个特例的巧合，不是矩阵混合的一般性质。两个任意姿态的矩阵线性混合后偏差会很大，实验脚本改成任意起点即可验证，留给读者。
一点历史：为什么这套路数在机器人界扎了根¶
把位姿当李群处理，在 1980 年代由 Murray、Li、Sastry 等人系统引入机器人学（《A Mathematical Introduction to Robotic Manipulation》是源头之一），此后经 MLS（Modern Robotics 教材）、Sophus/Ceres/GTSAM 等库的落地，已经成为状态估计和优化方向的事实标准。老教材（如 Craig）用的 DH 参数 + 欧拉角体系，在做优化时会处处撞上"更新量滑出流形"的问题——这也是后续第 3 篇（李代数与扰动模型）要正面回答的：优化器为什么必须知道流形的存在。
核心代码逐句精讲¶
整个 se3_demo.py 里真正需要吃透的只有两个函数：exp_so3（Rodrigues 公式）和 exp_se3。下面逐句拆解，建议对照 code/se3_demo.py 原文件阅读。
exp_so3：从旋转向量到旋转矩阵
def exp_so3(w): # (1)
theta = np.linalg.norm(w) # (2)
if theta < 1e-12: # (3)
return np.eye(3) + hat(w) # (3a)
k = w / theta # (4)
K = hat(k) # (5)
return (np.eye(3) # (6)
+ np.sin(theta) * K # (6a)
+ (1 - np.cos(theta)) * (K @ K)) # (6b)
- (1) 输入
w是三维旋转向量：方向 = 旋转轴，长度 = 旋转角（rad），即李代数元素 \([\boldsymbol{\phi}]_\times\) 的向量形式。 - (2) 取模长得到旋转角 \(\theta = \|\boldsymbol{\phi}\|\)。旋转的"大小"和"轴"从这里分离。
- (3) 小角度保护。展开 \(\exp\) 的级数 \(\exp([\phi]_\times) = I + [\phi]_\times + \tfrac{1}{2}[\phi]_\times^2 + \cdots\)，当 \(\theta \to 0\) 时只剩一阶项 (3a)——不保护的话，下面 (4) 除以 \(\theta\) 会除零，且数值噪声会把结果搅乱。1e-12 这个阈值下，一阶近似误差在 \(10^{-24}\) 量级，完全可忽略。
- (4) 单位轴 \(k = w/\theta\)。
- (5)
hat把向量变成反对称矩阵 \([k]_\times\)，作用是让"叉积"变成"矩阵乘法"：\([k]_\times v = k \times v\)。这是全部推导的枢纽。 - (6) Rodrigues 公式本体：\(\exp(\theta[k]_\times) = I + \sin\theta\,[k]_\times + (1-\cos\theta)\,[k]_\times^2\)。注意这不是级数截断，是精确的闭式解——反对称矩阵的幂满足 \(K^3 = -K\)，级数里每一项都能归并到这三项里，所以三行代码就是"精确的指数"，没有近似。这也是机器人学库普遍用它而不用通用矩阵指数（
scipy.linalg.expm）的原因：快一个数量级，且没有特征值分解的数值副作用。
exp_se3：从 twist 到 4×4 位姿
def exp_se3(xi): # (1)
rho, phi = xi[:3], xi[3:] # (2)
theta = np.linalg.norm(phi) # (3)
J = (np.eye(3) # (4)
+ (1 - np.cos(theta)) / theta**2 * K # (4a)
+ (theta - np.sin(theta)) / theta**3 * (K @ K)) # (4b)
T[:3, :3] = exp_so3(phi) # (5)
T[:3, 3] = J @ rho # (6)
return T
- (1) 输入
xi（six-vector，常记作 \(\xi\) 或 \(\mathcal{V}\theta\)）是 twist：前 3 维平移部分 \(\rho\)，后 3 维旋转部分 \(\phi\)。注意顺序约定：不同教材/库平移在前在后不统一（GTSAM 与本代码一致是平移在前；Sophus 是旋转在前），对接外部库时第一件事是核对这件事，错序是常见 bug。 - (3) 旋转部分的模长 \(\theta\) 仍然是"总旋转角"。
- (4) 矩阵 \(J\) 是 SE(3) 的左雅可比 \(J_l(\theta)\)，见 (4a)(4b)：它回答"绕固定轴转 \(\theta\) 角的过程中，平移积累是多少"。系数 \(\frac{1-\cos\theta}{\theta^2}\) 和 \(\frac{\theta-\sin\theta}{\theta^3}\) 来自对 \(\int_0^\theta \exp(s[\omega]_\times)\,ds\) 逐项积分——平移不是简单地把 \(\rho\) 抄进矩阵第四列，因为旋转和平移是同时发生的。
- (5) 旋转块直接复用
exp_so3，没有任何新东西。 - (6) 平移块 \(= J_l(\theta)\,\rho\)。直觉版本：\(\rho\) 是"随旋转发生的直线运动"，\(J\) 把它"扭"成螺旋线。这也是 SE(3) 元素的几何本质——螺旋运动（screw motion），Chasles 定理说任何刚体位移都能表示成一次螺旋。
逐句讲解的完整原则：核心函数逐行精讲（如上）、工具函数逐段讲、验证脚本只讲结论。后续每篇理论篇都会带这样的部分，案例番外的逐句比例更高。
实现什么、为什么这样写¶
这两个函数实现什么：它们是"李代数 ↔ 李群"的桥。整条流水线里的位置是——IMU 给出角速度增量（李代数侧），优化器在切空间里算更新量（李代数侧），而机器人的真实状态必须存成位姿（李群侧）。没有 exp/log，两边数据无法碰面。
为什么这样写：两个约束决定了当前实现——(a) 零重依赖：只靠 numpy，读者 clone 即跑；(b) 教学透明：每一步都能对应到公式，出问题可以逐行查。下面会看到，生产环境中你多半不该自己写它。
多种实现方案的对比分析（Rodrigues 闭式解 vs 泰勒级数 vs
scipy.linalg.expmvsscipy.Rotation，含实测误差/耗时/依赖四列对比表、每种写法的优缺点与最终选型理由）收录在 CSDN 完整版，此处不展开。
工程视角¶
- ROS / tf2 里每一个坐标变换就是一个
SE(3)。你调过的"机械臂到位但姿态歪 10°"，多半是左乘右乘用反了，或者欧拉角顺序（ZYX 还是 XYZ）和对方约定不一致。 - IMU 输出的是四元数，而优化器内部往往用李代数增量。两者的桥就是
exp/log。理解本文，后面读 VIO、手眼标定源码才不会停留在"抄公式"层面。 - 日志排查技巧：打印位姿时永远附上 det(R) 和 RᵀR−I 的范数。如果不是 1 和 0，说明上游某处把矩阵当普通数组操作了，误差会随时间指数放大。
延伸阅读¶
- 教材：Lynch & Park, Modern Robotics（第 3 章），免费 PDF 官方提供，李群视角讲得最干净
- 综述：Sola 等人, A micro Lie theory for state estimation in robotics（arXiv:1812.01537），后续几篇的蓝本
- 下一篇（第 3 篇）：李群与李代数——扰动模型为什么用右扰动
- 配套代码：
se3_demo.py，仅依赖 numpy/scipy，--plot出图
完整内容链接（完整可用代码）：https://blog.csdn.net/2301_81238088/category_13220213.html
完整版每周二更新，订阅后第一时间推送。