03 指数坐标与 RodriguesExponential Coordinates — 矩阵指数 · 轴 + 角

1 旋转矩阵的三个尴尬

用 9 个数描述 3 个自由度：其中 6 个是约束（三列正交、单位长、$\det = 1$），信息高度冗余。可看一个 $R$，你说不出“绕哪根轴、转了多少”——而这恰恰是人描述转动时最先想到的东西。

更麻烦的是两件事：不能直接插值（两个旋转矩阵取平均，结果一般不满足 $R^{\mathsf{T}}R = I$）；不能直接求导（速度、角速度都要对时间求导，而 $R$ 的导数不再是旋转矩阵）。

三条指向同一个结论：$R$ 记的是“转完之后的坐标”，不是“转动本身”。

2 换一套参数：一根轴加一个角

欧拉定理：任何姿态都可以由“绕某一根轴转一个角”一次得到。于是姿态可以记成 $(\omega,\theta)$——$\omega$ 是单位向量（2 个自由度），$\theta$ 是转角（1 个），合起来仍是 3 个。

这套参数的好处正好抵消上面的三个尴尬：直观（轴和角都画得出来）、能插值（轴不动、角线性变，中间结果仍然是旋转）、能求导（$\dot\theta\,\omega$ 就是角速度）。

3 为什么叫“指数”

因为 $R$ 会写成 $e^{\hat\omega\theta}$——矩阵指数，字面意义上的幂级数求和，不是记号游戏。这个级数会精确坍缩成三项，所以手算可行，这是 03.1 的主线。

数学上这叫李群与李代数：$\hat\omega$ 属于 $so(3)$（一个三维向量空间），指数映射把它送到 $SO(3)$。这门课不需要这套语言，但看到 $e^{\hat\omega\theta}$ 时要知道两件事：它一定能算出来，算出来一定是旋转。

4 本课做两件事

正向：给定 $\omega$ 与 $\theta$，写出 $R$。从 $\dot p = \hat\omega p$ 出发，用 $\hat\omega^{3} = -\hat\omega$ 把级数压成三项，得到 Rodrigues 公式（03.1）。

逆向：给定数值 $R$，取回 $(\omega,\theta)$。迹定角、反对称部分定轴，$\theta = \pi$ 时改走对称路线（03.2，2024 Q1(b)）。

正向考“会算”，逆向考“会解”，卷面上两种都出。逆向那两个边界（$\theta\to0$ 与 $\theta=\pi$）才是分水岭。

1 先把“转起来”写成方程

刚体绕单位轴 $\omega$ 转动，角速度取 1（把时间直接当角度用）。刚体上一点 $p$：它到轴的距离恒定，所以速度只能沿圆周切向——垂直于 $\omega$，也垂直于 $p$。速度大小是 $|p|\sin\varphi$（$\varphi$ 是 $p$ 与轴的夹角，$|p|\sin\varphi$ 正是点到轴的距离），而 $\omega\times p$ 的大小正好是它。于是

叉积对 $p$ 是线性的，可以写成一个矩阵乘向量。定义

$\hat\omega$ 叫 $\omega$ 的反对称矩阵（读作 “hat”）。三条性质：$\hat\omega^{\mathsf{T}} = -\hat\omega$，对角全零，9 个数里只有 3 个独立——正好是 $\omega$ 的三个分量。叉积这个几何操作，被整形成了一次矩阵乘法。

2 解方程，引出矩阵指数

$\dot p = \hat\omega p$ 与标量方程 $\dot x = ax$ 形状完全一样，解也一样：$p(t) = e^{\hat\omega t}p_0$。区别只在指数是矩阵，所以得先定义“矩阵的 $e$”：

这个级数对任何方阵都收敛（按矩阵范数），每一项都是同尺寸矩阵，加法有意义。所以 $p(t)$ 的解不是猜的，是逐项加出来的。令 $t = \theta$（单位角速度下时间就是角度），得到

顺带得到一个后面要用的结论：任何旋转矩阵都是某个反对称矩阵的指数。03.2 反过来从 $R$ 取轴和角，合法性就靠这一条。

3 关键一步：$\hat\omega^{3} = -\hat\omega$

按定义硬算，级数有无穷多项，手算不了。救星是一个恒等式。先证中间结果：$|\omega| = 1$ 时

最快的验法取 $\omega = (0,0,1)$，两侧都算一遍：

一般情形由坐标旋转不变性同样成立。有了它就能算三次方：$\hat\omega^{3} = \hat\omega(\omega\omega^{\mathsf{T}} - I) = (\hat\omega\omega)\omega^{\mathsf{T}} - \hat\omega$。而 $\hat\omega\omega = \omega\times\omega = 0$，所以

这是本节的发动机。它意味着 $\hat\omega$ 的幂只有一个四步循环：$I,\ \hat\omega,\ \hat\omega^{2},\ -\hat\omega,\ -\hat\omega^{2},\ \hat\omega$，如此往复——三次以上全部回到前三档。

4 无穷级数坍缩成三项

把 $e^{\hat\omega\theta}$ 按 $\theta$ 展开，用上面的循环把 $\theta^{3}$ 之后的项全部拉回前三档：奇次项只剩 $\hat\omega$，偶次项只剩 $I$ 与 $\hat\omega^{2}$。

两个括号分别是 $\sin\theta$ 与 $1-\cos\theta$。于是

这就是 Rodrigues 公式。值得亲手推一遍：它说明“无穷级数”和“三项式”不是两个公式，是同一个东西的两种写法——前者告诉你它从哪来，后者让你能手算。

5 写成元素形式（考试要写的那一版）

设 $\omega = (\omega_1,\omega_2,\omega_3)$，记 $c = \cos\theta$、$s = \sin\theta$、$v = 1-\cos\theta$：

不用背。对角元一律 $c+\omega_i^{2}v$；非对角元是 $\omega_i\omega_j v$ 再加一项 $\pm\omega_k s$，其中 $k$ 是剩下的那个下标——按 $1\to2\to3\to1$ 的顺序取负号，反序取正号。照这条规则现推，三十秒写完，比回忆公式可靠。

6 三条体检，加一条使用前提

$\theta = 0$：$R = I$，三项式退化正确。$\omega = \hat{z}$：左上 $2\times2$ 块正好是 02 课的平面旋转，第 3 行第 3 列是 $c+v = 1$，与二维情形完全兼容（本演示就取这个特例，让旋转画在纸面上）。正交性：任意 $\omega$、$\theta$ 代入，$R^{\mathsf{T}}R = I$ 且 $\det R = 1$。

这三条都能当验算：写完 $R$，先查 $\det$，再查 $\theta = 0$ 的退化。小角度时还有 $R \approx I + \theta\hat\omega$——这正是角速度进入运动学的入口。

使用前提：$\omega$ 必须是单位向量。公式里的 $\theta$ 是“绕轴转过的角”。若代进去的是任意向量 $u$（$|u|\neq1$），$e^{\hat u\theta}$ 的实际转角是 $|u|\theta$。要么先单位化，要么把 $|u|$ 并进角度里——混用会让答案整体差一个倍数，而且 $\det$ 检查看不出来。

7 为什么顺序不能交换

$e^{A}e^{B} = e^{A+B}$ 一般只在 $A$、$B$ 可交换时成立。两个 hat 矩阵一般不交换，它们的交换子满足 $[\hat\omega_1,\hat\omega_2] = \widehat{\omega_1\times\omega_2}$，只有两轴平行时才为零。

所以两次旋转的顺序必须原样保留——这是 02.1“下标接龙”的深层原因，也是机器人姿态里“先绕哪个轴转”必须交代清楚的原因。二维是特例：所有平面旋转同轴，交换子恒为零，所以总能交换。

8 面板：看级数怎么收敛

右侧 $S_1$ 到 $S_5$ 是幂级数的部分和，误差列是 $\max|S_k - R|$。$\theta = 60^\circ$ 时 $S_1$ 差得远、$S_3$ 已经很近、$S_5$ 肉眼无差；把 $\theta$ 拖到 $150^\circ$，收敛明显变慢——级数仍然收敛，只是前几项不够用了。

这正好说明三项式的价值：它给的是精确和，不是“项数够多”的近似。下面那块“三项式闭合”把 $I$、$\sin\theta\,\hat\omega$、$(1-\cos\theta)\hat\omega^{2}$ 分开算再相加，结果与精确 $R$ 逐位相同。

9 三维实例：任意转动 = 一条轴 + 一个角

欧拉定理保证：任何旋转矩阵都有特征值为 1 的特征向量——那条不动的轴；转角由迹给出。所以“姿态变了”总可以改写成“绕某条轴转了某个角”。下面这块把改写做成一件事：拖动一个点，图里自动画出转轴与转角。

两个约定。其一，这里只处理纯转动（轴过原点）；带平移的位姿变换要等螺旋运动那一节，那时轴可以不过原点。其二，一个点只给两个约束：把 $p_0$ 拖到新方向 $\hat p$ 的转动有无穷多个（还能绕 $p_0$ 自转任意角）。演示采用“最小转动”约定：轴同时垂直于 $p_0$ 与 $\hat p$，$\hat\omega = \dfrac{p_0\times\hat p}{|p_0\times\hat p|}$，$\theta = \operatorname{atan2}\!\left(|p_0\times\hat p|,\; p_0\cdot\hat p\right)$。把点推到目的方向，不做多余扭转，这与直觉一致。

点拖到位后，页面走两条路：先用 $(\hat\omega,\theta)$ 造 $R$（本节的正向三项式），再用 03.2 的逆推把它拆回来（$\operatorname{tr}\to\theta$，$(R-R^{\mathsf T})/(2\sin\theta)\to\hat\omega$）。两条路必须给出同一组数，右侧面板实时对照。拖动时留意：红线是解出的轴；θ 弧随拖动同步伸缩；轴和角只随点走，不随视角变。

位置与姿态分开调。拖点只定方向（两个自由度）；三轴顶端的圆环手柄让刚体绕 $\hat p$ 原地自转（第三个自由度）。自转 $\phi\neq0$ 之后，复合转动的轴就不垂直于两点——它偏离最小转动的轴（灰色虚线弧），只能靠迹与 $R-R^{\mathsf T}$ 反解。把柄转半圈再看右侧「定轴」那一行，轴会跟着摆动。

边界情形先说清。$\hat p\to p_0$ 时 $\theta\to0$，轴无意义（$R=I$）；$\hat p\to-p_0$ 时 $\theta\to180^\circ$，轴不唯一——任何垂直于 $p_0$ 的轴都行，此时 $(R-R^{\mathsf T})/(2\sin\theta)$ 除零，与 03.2 的 π 陷阱是同一件事，面板会亮红提示。

1 问题，以及两个边界

考题给一个具体的数值 $R$（$3\times3$ 全是数），要求取回 $\omega$ 与 $\theta$。这是 2024 Q1(b)。公式本身只有两行，真正的难点在两个边界：$\theta$ 接近 $0$（轴无定义）与 $\theta = \pi$（通用式除零）。

2 迹定角

出发点是一条只用到对角线的恒等式：

两条推导，记一条就够。

特征值路线：绕轴方向在旋转中不动，特征值是 $1$；垂直于轴的平面内是转角为 $\theta$ 的二维旋转，特征值是 $e^{\pm i\theta}$。迹等于特征值之和，得 $1 + 2\cos\theta$。

直接取迹路线（考场更实用）：对 Rodrigues 逐项取迹。$\operatorname{tr}I = 3$；$\operatorname{tr}\hat\omega = 0$（对角全零）；$\operatorname{tr}\hat\omega^{2} = \operatorname{tr}(\omega\omega^{\mathsf{T}} - I) = 1 - 3 = -2$。于是 $\operatorname{tr}R = 3 - 2(1-\cos\theta) = 1 + 2\cos\theta$。三十秒，不依赖任何定理。

3 于是 $\theta$

$\arccos$ 的值域是 $[0,\pi]$，所以这一步天然给出主值（短方式转角），答题写主值即可。数值实现时要先把它 clamp 到 $[-1,1]$：浮点误差可能让 $(\operatorname{tr}R-1)/2$ 变成 $1.0000000002$，$\arccos$ 直接返回 NaN。

4 反对称部分定轴

把 $R$ 减去它的转置，对称的那部分（$I$ 与 $\hat\omega^{2}$）全部抵消，只剩含 $\sin\theta$ 的一项：

所以

从反对称矩阵的上三角读出三个分量。写成单个分量的形式，符号最容易错：

记忆：每个分量都是“下三角减上三角”，下标按 $1\to2\to3\to1$ 循环。使用条件：$\sin\theta \neq 0$。

5 $\theta\to0$：不是除零 bug，是无解

$\theta\to0$ 时 $R\to I$，$\sin\theta\to0$，公式爆炸。但此时“轴”在物理上根本没有定义——转 $0$ 度，绕哪根轴都一样。所以这个边界没有唯一答案，题目不会这么出。

工程实现里给一个容差就够：$|\sin\theta| < 10^{-6}$ 时直接返回 $\theta = 0$、轴任取（比如 $(0,0,1)$）。

6 $\theta = \pi$：改走对称路线

$\theta = \pi$ 时 $\sin\theta = 0$，通用式除零。但此时 Rodrigues 反而变简单了：$\sin\pi = 0$、$1-\cos\pi = 2$，所以 $R = I + 2\hat\omega^{2}$。代入 $\hat\omega^{2} = \omega\omega^{\mathsf{T}} - I$：

右边是秩 1 的对称矩阵，可以直接读出 $\omega$：对角元给绝对值，$(R_{ii}+1)/2 = \omega_i^{2}$；非对角元给符号，$(R_{ij}+R_{ji})/2 = \omega_i\omega_j$。

操作四步：① 在 $R+I$ 的对角元里找最大的那个 $i$，避免除以接近 $0$ 的量；② 取 $\omega_i = \sqrt{(R_{ii}+1)/2}$；③ 其余分量用 $\omega_j = \dfrac{R_{ij}+R_{ji}}{2\omega_i}$；④ 检查 $|\omega| = 1$。

符号定不下来，这是约定，不是推导。$\omega$ 与 $-\omega$ 配同一个 $\theta=\pi$ 给出完全相同的 $R$，从 $R$ 本身无法判断哪一个“对”。常见约定是让绝对值最大的那个分量取正（考卷上多半直接写 $\omega_1>0$，前提是 $\omega_1\neq0$）；定下它，其余分量按公式跟着算，一致性自动成立。

这与 03.2 第 3 小节 $\arccos$ 取 $[0,\pi]$ 是同一类约定：答案写成 $\omega$ 或 $-\omega$ 都算对，答题时注明一句“取 $\omega_1>0$”即可。所以第 ④ 步只查 $|\omega|=1$——符号不归它管。

面板把两条路线并排显示：把 $\theta$ 拖到 $180^\circ$ 附近，通用式那一行变灰打叉，备用式接管，标签上会写明按哪个对角元取的轴。

进阶一句：$\theta$ 只是接近 $\pi$ 而不等于 $\pi$ 时，通用式在数值上除以很小的 $\sin\theta$，误差被放大；这时改用 $R+I$ 的对角元读出 $\omega$ 再单位化，比通用式稳得多。生产代码通常干脆转四元数再取轴角。考试只考 $\theta$ 恰好等于 $\pi$，走上面的四步就够。

7 数值例子：$\operatorname{tr}R = 0$

先只用迹：$\cos\theta = (0-1)/2 = -1/2$，$\theta = 120^\circ$。这就是 03.3 第一题的答案。

反过来的方向更值得走一遍。取 $\omega = (1,1,1)/\sqrt{3}$、$\theta = 120^\circ$，代元素形式：对角元 $c+\omega_i^{2}v = -\tfrac{1}{2} + \tfrac{1}{3}\cdot\tfrac{3}{2} = 0$；非对角元按 $1\to2\to3$ 的顺序取 $\omega_i\omega_j v - \omega_k s = \tfrac{1}{2} - \tfrac{1}{\sqrt{3}}\cdot\tfrac{\sqrt{3}}{2} = 0$，反序对取正号得 $1$。于是

这是一个循环置换矩阵：三行正交，$\det R = 1$，$\operatorname{tr}R = 0$。逆推反过来验：$R - R^{\mathsf{T}}$ 的第 1 行是 $\sqrt{3}\,(0,-\omega_3,\omega_2) = (0,-1,1)$，读出 $\omega_2 = \omega_3 = 1/\sqrt{3}$，与 $\omega_1$ 对称。

8 不唯一性：多对一

$(\omega,\theta)$ 到 $R$ 是多对一：$\theta$ 加 $2\pi k$ 后 $e^{\hat\omega\theta}$ 不变；$(-\omega,-\theta)$ 给出同一个 $R$；$\theta = 0$ 时轴任意；$\theta = \pi$ 时 $\pm\omega$ 都行。

$\arccos$ 的 $[0,\pi]$ 只是人为选定的主值约定，不是数学上的唯一。这不算缺陷：姿态本身就不记录“转了几圈”，参数化必须把这份信息丢掉。

考试里题目没指定时，写主值，并在旁边注明“其余解为 $\theta + 2\pi k$ 与 $(-\omega,-\theta)$”，通常能补回半步分。

9 考场四步与面板操作

四步：① $\operatorname{tr}R \to \cos\theta \to \theta$；② 算 $\sin\theta$，判断是否接近 $0$；③ 不为零走反对称式，为零走对称式；④ 代回 Rodrigues 验算，看能否复原 $R$。

第 ④ 步只要三十秒，却能抓住几乎所有的符号错和下标错，比反复盯着公式检查有效得多。面板：按钮切换转轴（含“随机斜轴”，用来验证公式对任意 $\omega$ 成立，而不只是坐标轴）；左右拖动扫 $\theta$；点 $\theta = 180^\circ$ 看通用式熄灭、对称式接管。

规则。三道题，覆盖本课三个考点：迹定角、$\pi$ 边界、多解性。做完即时反馈，可随时回到上方面板核对。