Modern Robotics 3.2：旋转矩阵、角速度与 SO(3) 直觉理解

读《Modern Robotics》第三章 3.2 时：

公式不会推导没关系，只要抓住三件事：姿态怎么表示、转动怎么表示、二者怎么互相转换。

主线如下：

1. 用旋转矩阵 $R\in SO(3)$ 描述三维姿态，也就是“现在朝哪儿”。
2. 用角速度 $\omega$ 描述姿态的瞬时变化，也就是“正在绕哪根轴、以多快转”。
3. 用反对称矩阵 $[\omega ]\in so(3)$ 把叉乘写成矩阵乘法，这样才能从“姿态怎么变”读出“正在绕哪根轴、转多快”。
4. 用矩阵指数 $R=e^{[\hat{\omega }]\theta }$ 把“持续转动一段角度”变成最终姿态。
5. 用矩阵对数 $\log R$ 从一个姿态反推出等效的旋转轴和旋转角。

一、姿态不是位置：先分清“在哪里”和“朝哪里”

图 1：原书 Figure 3.6。一个刚体的位形由位置 $p$ 和姿态 $R$ 一起描述。

一个三维刚体的状态可以拆成两部分：

• 它在哪里：位置 $p$；
• 它朝哪里：姿态 $R$。

位置比较直观。选一个固定参考系 { s } \{s\} {s}，从 { s } \{s\} {s} 的原点指向物体参考系 { b } \{b\} {b} 的原点，这个向量就是位置。

姿态更麻烦。三维里只说“转了多少度”不够，因为还必须说明“绕哪根轴转”。“绕某根轴转某个角度”是一种很直观的说法，但书里不会只用这两个量来记录姿态；Modern Robotics 选择用旋转矩阵 $R$ 作为统一表示，因为矩阵更方便做参考系变换、连续复合和后续求导。

如果 { b } \{b\} {b} 是物体参考系， { s } \{s\} {s} 是固定参考系，那么：

$$R_{sb}=\left[\begin{array}{ccc}|& |& |\\ {\hat{x}}_b& {\hat{y}}_b& {\hat{z}}_b\\ |& |& |\end{array}\right]$$

这里的每一列都是物体参考系的一根单位轴，但这些单位轴的坐标是用固定参考系 { s } \{s\} {s} 表达的。

所以 $R_{sb}$ 可以读成：

把 { b } \{b\} {b} 的姿态，用 { s } \{s\} {s} 的坐标写出来。

下标也可以这样记：

• 第一个下标 $s$：用哪个参考系表达；
• 第二个下标 $b$：正在描述哪个参考系。

因此 $R_{sb}$ 的三列分别是 ${\hat{x}}_b,{\hat{y}}_b,{\hat{z}}_b$ 在 { s } \{s\} {s} 中的坐标。

二、什么样的矩阵才是旋转矩阵？

$R$ 不是任意 $3\times 3$ 矩阵。由于它的三列是一个右手正交单位坐标系，所以必须满足：

$$R^{T}R=I$$

$$\det R=1$$

第一条说明三根轴都是单位长度，并且两两垂直。第二条说明它保持右手系，排除了镜像翻转。

满足这些条件的所有矩阵组成集合：

S O ( 3 ) = { R ∈ R 3 × 3 ∣ R T R = I , det ⁡ R = 1 } SO(3)=\{R\in\mathbb{R}^{3\times3}\mid R^TR=I,\det R=1\} SO(3)={R∈R3×3∣RTR=I,detR=1}

这里的 $SO(3)$ 可以先不想得太神秘。它就是：

所有合法三维姿态的集合。

旋转矩阵有 9 个元素，但三维姿态只有 3 个自由度。真正把自由度从 9 压到 3 的，是三列向量必须单位长度、两两垂直，也就是 $R^{T}R=I$ 给出的 6 个独立约束。右手系条件对应 $\det R=1$，它不是再减少一个连续自由度，而是在正交矩阵中排除镜像翻转，只保留真正的旋转。

几个性质要记住，后面会一直用：

$$R^{-1}=R^T$$

$$R_1,R_2\in SO(3)\Rightarrow R_1{R}_2\in SO(3)$$

$$\Vert Rx\Vert =\Vert x\Vert$$

也就是说，旋转矩阵的逆就是转置；两个旋转连续做，结果仍然是旋转；旋转不会改变向量长度。

常见的绕固定坐标轴旋转矩阵也可以放在这里统一看：

$$\text{Rot}(\hat{x},\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]$$

$$\text{Rot}(\hat{y},\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$$

$$\text{Rot}(\hat{z},\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$

它们都符合右手定则，也都属于 $SO(3)$。

三、下标消去规则：别让 $R_{ab}$、$R_{bc}$ 把你绕晕

图 2：原书 Figure 3.7。同一个空间点 $p$，在不同方向的参考系中会有不同坐标。

假设有三个参考系 { a } , { b } , { c } \{a\},\{b\},\{c\} {a},{b},{c}：

• $R_{ab}$ 表示 { b } \{b\} {b} 在 { a } \{a\} {a} 中的姿态；
• $R_{bc}$ 表示 { c } \{c\} {c} 在 { b } \{b\} {b} 中的姿态；
• $R_{ac}$ 表示 { c } \{c\} {c} 在 { a } \{a\} {a} 中的姿态。

那么：

$$R_{ac}=R_{ab}{R}_{bc}$$

这就是下标消去规则：$R_{ab}$ 的第二个下标 $b$，和 $R_{bc}$ 的第一个下标 $b$ 接上了，所以中间的 $b$ 消掉，剩下 $a$ 和 $c$。这不是形式游戏，而是在说：如果 { c } \{c\} {c} 的坐标先用 { b } \{b\} {b} 表达，再把 { b } \{b\} {b} 表达到 { a } \{a\} {a}，最终就得到 { c } \{c\} {c} 在 { a } \{a\} {a} 中的表达。

反过来：

$$R_{ba}=R_{ab}^{-1}=R_{ab}^T$$

所以如果你看到 $R_{ab}$，要立刻想到两件事：

• 作为姿态，它的三列是 { b } \{b\} {b} 的三根单位轴在 { a } \{a\} {a} 中的坐标；
• 作为坐标变换，它把同一个向量的坐标从 { b } \{b\} {b} 表达改写成 { a } \{a\} {a} 表达。

反过来，$R_{ba}=R_{ab}^T$，它把同一个向量的坐标从 { a } \{a\} {a} 表达改写成 { b } \{b\} {b} 表达。

例如同一个几何向量 $\mathbf{p}$，在 { b } \{b\} {b} 中的坐标是 $p_b$。要把它改写成 { a } \{a\} {a} 中的坐标：

$$p_a=R_{ab}{p}_b$$

这里向量本身没有动，动的是“用哪套坐标数字描述它”。

四、同一个 $R$ 有三种用法

1. 表示姿态

$$R_{sb}$$

表示 { b } \{b\} {b} 相对于 { s } \{s\} {s} 的姿态。它的列向量就是 { b } \{b\} {b} 的三根轴在 { s } \{s\} {s} 中的坐标。

这是最直接的用法：描述一个参考系朝哪儿。

2. 改变坐标表示

$$p_s=R_{sb}{p}_b$$

这里 $\mathbf{p}$ 是同一个几何向量。$R_{sb}$ 的作用不是把向量真的转动了，而是把同一个向量从 { b } \{b\} {b} 坐标表示改成 { s } \{s\} {s} 坐标表示。

这是第二种用法：改变参考系，而不改变几何对象本身。

3. 主动旋转向量或参考系

图 3：原书 Figure 3.9。同一个旋转矩阵，左乘和右乘对应不同的旋转语义。

$R$ 也可以表示一个主动旋转操作：对象真的被转过去，但仍然用原来的参考系表达。

如果当前姿态是 $R_{sb}$，再施加一个旋转 $R$：

• 如果旋转轴用固定参考系 { s } \{s\} {s} 表达，新的姿态是

$$R_{s{b}^{\prime }}=R{R}_{sb}$$

• 如果旋转轴用当前物体参考系 { b } \{b\} {b} 表达，新的姿态是

$$R_{s{b}^{\prime \prime }}=R_{sb}R$$

三维旋转通常不可交换：

$$R_1{R}_2\ne R_2{R}_1$$

所以“先绕世界 $z$ 轴转，再绕自己当前 $x$ 轴转”，和“先绕自己当前 $x$ 轴转，再绕世界 $z$ 轴转”，结果一般不是同一个姿态。

看到一个旋转矩阵时，先问一句：

它是在表示姿态、改变坐标表示，还是主动旋转对象？

五、角速度：姿态正在怎样变化

旋转矩阵描述的是“现在朝哪儿”。机器人运动时，还要描述“朝向正在怎么变”。最直接的想法是看 $\dot{R}$，也就是旋转矩阵对时间的导数。但 $\dot{R}$ 有 9 个元素，而且它本身不是旋转矩阵；我们希望找到一个只有 3 个独立分量、又有清楚物理意义的量。这就是角速度。

图 4：原书 Figure 3.10。角速度向量的方向是瞬时旋转轴，大小是旋转快慢。

角速度向量可以写成：

$$\omega =\hat{\omega }\dot{\theta }$$

其中：

• $\hat{\omega }$ 是单位旋转轴方向；
• $\dot{\theta }$ 是绕这根轴旋转的角速度大小，单位通常是 rad/s；
• $\omega$ 的方向表示绕哪根轴转，长度表示转得多快。

如果你听过 roll、pitch、yaw，可以顺手排一个坑：不要先把角速度理解成这三个角的导数。欧拉角或 roll-pitch-yaw 只是姿态的某种参数化方式，会依赖旋转顺序，也可能遇到奇异性。角速度 $\omega$ 是刚体瞬时旋转的物理量，它的核心含义是“瞬时绕哪根轴、转得多快”。

六、角速度如何驱动姿态变化

这一节的目的不是发明新的物理量，而是把“叉乘”改写成矩阵乘法。这样角速度就能和旋转矩阵 $R$、旋转矩阵导数 $\dot{R}$ 放在同一套矩阵语言里。

设

$$R(t)=\left[\begin{array}{ccc}|& |& |\\ r_1(t)& r_2(t)& r_3(t)\\ |& |& |\end{array}\right]$$

其中 $r_1,r_2,r_3$ 分别是物体坐标系三根轴 ${\hat{x}}_b,{\hat{y}}_b,{\hat{z}}_b$ 在固定参考系 { s } \{s\} {s} 中的坐标。它们是方向向量的坐标，不是物体上三个点的位置。若角速度也用固定参考系表达，记为 ${\omega }_s$，那么原书 Figure 3.10 对应的几何关系是：

$${\dot{r}}_i={\omega }_s\times r_i,i=1,2,3$$

也就是说，如果把 $r_i$ 看成从原点出发的一根单位箭头，那么刚体旋转时，这根箭头的端点会在单位球面上运动；它的瞬时速度由 ${\omega }_s\times r_i$ 给出。

把三列合在一起：

$$\dot{R}=\left[\begin{array}{ccc}|& |& |\\ {\dot{r}}_1& {\dot{r}}_2& {\dot{r}}_3\\ |& |& |\end{array}\right]=\left[\begin{array}{ccc}|& |& |\\ {\omega }_s\times r_1& {\omega }_s\times r_2& {\omega }_s\times r_3\\ |& |& |\end{array}\right]$$

这个式子已经说明了 $R$ 的变化由角速度决定。但右边还写着三个叉乘，不方便继续做矩阵运算。于是书里引入 bracket notation。

对任意三维向量

$$x=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]$$

定义

$$[x]=\left[\begin{array}{ccc}0& -x_3& x_2\\ x_3& 0& -x_1\\ -x_2& x_1& 0\end{array}\right]$$

这里的方括号是 Modern Robotics 的一个专门记号：它把三维向量 $x$ 变成一个 $3\times 3$ 矩阵。这个矩阵满足：

$$[x{]}^T=-[x]$$

主对角线对称位置的元素互为相反数，所以它叫反对称矩阵。它最重要的作用是：

$$[x]y=x\times y$$

也就是说，$[x]$ 是“拿 $x$ 去叉乘别的向量”的矩阵版本。把这个规则用到 ${\omega }_s$ 上，就有：

$$[{\omega }_s]R=\left[\begin{array}{ccc}|& |& |\\ [{\omega }_s]r_1& [{\omega }_s]r_2& [{\omega }_s]r_3\\ |& |& |\end{array}\right]=\left[\begin{array}{ccc}|& |& |\\ {\omega }_s\times r_1& {\omega }_s\times r_2& {\omega }_s\times r_3\\ |& |& |\end{array}\right]$$

因此：

$$\dot{R}=[{\omega }_s]R$$

这条公式是本节的核心。它说：

如果角速度用固定参考系表达，那么旋转矩阵的导数等于 $[{\omega }_s]$ 左乘 $R$。表示角速度如何驱动姿态变化

顺序不能随便换成 $R[{\omega }_s]$。这里左乘是因为 ${\omega }_s$ 和 $R$ 的列向量 $r_1,r_2,r_3$ 都是在固定参考系 { s } \{s\} {s} 中表达的。

因为 $R^{-1}=R^T$，所以可以把这条公式右乘 $R^T$：

$$\dot{R}{R}^T=[{\omega }_s]R{R}^T$$

又因为 $R{R}^T=I$，所以：

$$[{\omega }_s]=\dot{R}{R}^T$$

这就是从 $\dot{R}$ 中提取空间角速度的方式。到这里，主线已经够了：$\dot{R}$ 本身有 9 个元素，不是角速度；但 $\dot{R}{R}^T$ 会变成一个反对称矩阵，也就是 $[{\omega }_s]$。

因此 $\dot{R}{R}^T$ 只有 3 个独立量，正好对应角速度 ${\omega }_s$。

举个实际例子：机械臂末端装了一个相机，系统每一小段时间都能估计出相机的姿态 $R(t)$。如果只看 $R(t)$，你知道相机“现在朝哪儿”；算出 $\dot{R}{R}^T$ 后，你就知道它“此刻正在绕哪根轴转、转多快”。例如结果算出来是 ${\omega }_s=(1,2,2)$

表示瞬时旋转轴沿固定系方向 $(1,2,2)$

角速度大小是：$\Vert {\omega }_s\Vert =\sqrt{1^2+2^2+2^2}=3\text{rad/s}$

单位旋转轴是：${\hat{\omega }}_s=\frac{1}{3}(1,2,2)$

这就是这个公式的作用：把“姿态随时间的变化”翻译成更直观的“角速度”。

所有这样的 $3\times 3$ 反对称矩阵组成集合 $so(3)$。下一节再专门解释它和 $SO(3)$ 的关系。

七、$SO(3)$、$so(3)$ 和 $[\omega ]$：姿态、速度与矩阵表示

第一，$SO(3)$ 放的是姿态。

一个旋转矩阵

$$R\in SO(3)$$

表示一个已经存在的姿态，也就是“现在朝哪儿”。它不是任意矩阵，而必须满足：

$$R^{T}R=I,\det R=1$$

所以可以先把 $SO(3)$ 理解成：

所有合法三维姿态组成的集合。

第二，$[\omega ]$ 是把角速度写成矩阵。

$[\omega ]$ 不是新的姿态，也不是新的物理量。它只是同一个角速度向量 $\omega$ 的矩阵写法。这样写的好处是可以把叉乘写成矩阵乘法：

$$[\omega ]v=\omega \times v$$

第三，$so(3)$ 放的是所有这样的反对称矩阵。

原书紧接着说，所有 $3\times 3$ 实反对称矩阵组成 $so(3)$：

s o ( 3 ) = { Ω ∈ R 3 × 3 ∣ Ω T = − Ω } so(3)=\{\Omega\in\mathbb{R}^{3\times3}\mid \Omega^T=-\Omega\} so(3)={Ω∈R3×3∣ΩT=−Ω}

任意三维向量 $\omega$ 都能写成一个 $[\omega ]$，任意 $3\times 3$ 反对称矩阵也都能反过来读成某个三维向量。因此可以把它们看成一一对应：

$$\omega \in {\mathbb{R}}^3⟷[\omega ]\in so(3)$$

第四，$so(3)$ 是单位姿态处的速度空间

$$\dot{R}=[{\omega }_s]R$$

这个公式的意思是：当前姿态是 $R$，角速度是 ${\omega }_s$，那么姿态的瞬时变化 $\dot{R}$ 由 $[{\omega }_s]$ 左乘 $R$ 得到。

如果当前姿态刚好是单位姿态：

$$R=I$$

那么上面的公式变成：

$$\dot{R}=[{\omega }_s]I=[{\omega }_s]$$

它说明：当姿态在 $I$ 这个点时，合法的瞬时姿态变化 $\dot{R}$ 正好就是一个反对称矩阵 $[{\omega }_s]$。

反过来，任意一个反对称矩阵 $[\omega ]$，都可以看成“从单位姿态 $I$ 出发，以角速度 $\omega$ 开始转”的瞬时变化。

这就是原书脚注那句话的意思：

$so(3)$ 由 $R=I$ 时所有可能的 $\dot{R}$ 组成。

所以：

• $SO(3)$ 里的点是姿态；
• $R(t)$ 是一条在 $SO(3)$ 里运动的姿态轨迹；
• $\dot{R}(t)$ 是这条轨迹在某一刻的瞬时变化；
• 当这个点是 $I$ 时，所有可能的瞬时变化正好组成 $so(3)$。

这里的“速度”不是平移速度，不是说物体往三维空间哪个方向移动。它说的是姿态变化的速度：绕哪根轴开始转、转得多快。$\omega$ 的方向给出瞬时旋转轴，$\Vert \omega \Vert$ 给出角速度大小。

第五，如果当前姿态不是 $I$，怎么办？

$$[{\omega }_s]=\dot{R}{R}^T$$

如果把角速度改用物体参考系 { b } \{b\} {b} 表达：

$${\omega }_s=R_{sb}{\omega }_b$$

$${\omega }_b=R_{sb}^T{\omega }_s$$

原书 Proposition 3.8 给了一个和这个关系配套的恒等式：

$$R[\omega ]R^T=[R\omega ]$$

代入可得：
$$[{\omega }_b]=R^T\dot{R}=R^{-1}\dot{R}$$

把空间表达和物体表达放在一起：

$$\dot{R}{R}^{-1}=[{\omega }_s]\in so(3)$$

$$R^{-1}\dot{R}=[{\omega }_b]\in so(3)$$

这两条式子都在做同一件事：从姿态变化 $\dot{R}$ 中提取角速度的反对称矩阵表示。区别只是参考系不同：

• ${\omega }_s$：同一个角速度，用固定参考系 { s } \{s\} {s} 表达；
• ${\omega }_b$：同一个角速度，用物体参考系 { b } \{b\} {b} 表达。

这一节最后可以压缩成一句话：

$SO(3)$ 放姿态，$so(3)$ 放单位姿态处的速度，$[\omega ]$ 是把角速度向量放进 $so(3)$ 的矩阵写法。

八、有限旋转：绕某根轴，转某个角度

角速度讲的是瞬时变化。那一个已经完成的姿态 $R$，能不能理解成“绕某根轴转了某个角度”的结果？

可以。

图 5：原书 Figure 3.8。三维中的一个姿态可以看成绕某个单位轴 $\hat{\omega }$ 旋转角度 $\theta$。

三维空间中，任意一个旋转矩阵 $R\in SO(3)$，都可以看作：

$$R=\text{Rot}(\hat{\omega },\theta )$$

也就是绕单位轴 $\hat{\omega }$ 旋转角度 $\theta$。

这叫轴角表示。Modern Robotics 进一步把它写成矩阵指数：

$$R=e^{[\hat{\omega }]\theta }$$

为什么会出现指数？看微分方程：

$$\dot{p}(t)=\hat{\omega }\times p(t)=[\hat{\omega }]p(t)$$

这和普通线性微分方程 $\dot{x}=Ax$ 的形式一样。解就是：

$$p(t)=e^{[\hat{\omega }]t}p(0)$$

如果以大小为 $1$ rad/s 的角速度绕 $\hat{\omega }$ 转动，转过角度 $\theta$ 后，最终旋转矩阵就是：

$$R=e^{[\hat{\omega }]\theta }$$

所以矩阵指数不是凭空来的。它是在说：

把常角速度积分一段旋转角度，得到最终姿态。

对于 $3\times 3$ 反对称矩阵，矩阵指数有闭式结果，也就是 Rodrigues 公式：

$$e^{[\hat{\omega }]\theta }=I+\sin \theta [\hat{\omega }]+(1-\cos \theta )[\hat{\omega }{]}^2$$

这条公式非常实用：不用真的去算无限级数，就能从旋转轴和旋转角得到旋转矩阵。

九、指数坐标：用一个三维向量表示一次旋转

图 6：原书 Figure 3.11。绕轴连续旋转可以用矩阵指数来描述。

轴角表示有两个东西：单位轴 $\hat{\omega }$ 和角度 $\theta$。Modern Robotics 把它们合成一个三维向量：

$$r=\hat{\omega }\theta$$

这个 $r$ 就是旋转的指数坐标。

它的方向表示旋转轴，长度表示旋转角：

$$\Vert r\Vert =\theta$$

如果 $r=0$，表示没有旋转。如果 $r$ 指向某个方向，表示绕这个方向旋转；如果它更长，表示旋转角更大。

于是：

$$R=e^{[r]}$$

如果直接用 $r$ 而不是单位轴 $\hat{\omega }$，令 $\theta =\Vert r\Vert$，当 $\theta \ne 0$ 时：

$$e^{[r]}=I+\frac{\sin \theta }{\theta }[r]+\frac{1-\cos \theta }{{\theta }^2}[r{]}^2$$

这和前面的 Rodrigues 公式是同一件事，只是把 $r=\hat{\omega }\theta$ 代进去了。

图 7：原书 Figure 3.13。指数坐标可以想成一个半径为 $\pi$ 的实心球。

可以把指数坐标 $r=\hat{\omega }\theta$ 想成三维空间中的一个点。点的方向是旋转轴，点到原点的距离是旋转角。

为什么通常只画半径为 $\pi$ 的球？因为旋转角可以选在 $[0,\pi ]$ 之间。超过 $\pi$ 的旋转，可以换成沿相反方向的轴转一个更小的角度。例如绕某轴转 $210^{\circ }$，等价于绕反方向轴转 $150^{\circ }$。

所以指数坐标很直观，但不是完全唯一。尤其在 $\theta =\pi$ 附近，轴的正负会出现等价表示。这不是小问题，写程序计算矩阵对数时必须处理。

十、矩阵对数：从 $R$ 找回旋转轴和旋转角

矩阵指数解决的是：

$$[\hat{\omega }]\theta ⟶R$$

矩阵对数解决反问题：

$$R⟶[\hat{\omega }]\theta$$

也就是：

$$[\hat{\omega }]\theta =\log R$$

对一般情况，也就是 $0<\theta <\pi$，先用迹求角度：

$$\theta ={\cos }^{-1}\left(\frac{\text{tr}(R)-1}{2}\right)$$

再求反对称矩阵：

$$[\hat{\omega }]=\frac{1}{2\sin \theta }(R-R^T)$$

因此：

$$\log R=\frac{\theta }{2\sin \theta }(R-R^T)$$

这就是从一个旋转矩阵中取回轴角表示的核心公式。

但有两个边界情况要单独看。

第一，如果 $R=I$，说明没有旋转：

$$\theta =0,\log R=0$$

这时旋转轴没有唯一意义，因为你没转，绕哪根轴都可以说得通。

第二，如果 $\theta =\pi$，上面的公式会除以 $\sin \theta =0$，不能直接用。此时旋转轴仍然存在，但正负方向有等价性。设 $R=[r_{ij}]$，实际计算时要从 $R$ 的元素里挑一个数值稳定的轴。例如可以按书里的思路，根据 $1+r_{33}$、$1+r_{22}$、$1+r_{11}$ 哪个不为零来选：

$$\hat{\omega }=\frac{1}{\sqrt{2(1+r_{33})}}\left[\begin{array}{c}{r}_{13}\\ r_{23}\\ 1+r_{33}\end{array}\right]$$

或

$$\hat{\omega }=\frac{1}{\sqrt{2(1+r_{22})}}\left[\begin{array}{c}{r}_{12}\\ 1+r_{22}\\ r_{32}\end{array}\right]$$

或

$$\hat{\omega }=\frac{1}{\sqrt{2(1+r_{11})}}\left[\begin{array}{c}1+r_{11}\\ r_{21}\\ r_{31}\end{array}\right]$$

然后：

$$\log R=[\hat{\omega }]\pi$$

第一次读 3.2 时，不必把这个边界算法背下来。更重要的是知道：矩阵对数不是一个简单公式打天下，$\theta =0$ 和 $\theta =\pi$ 必须单独处理。

小结

3.2 的核心不是背几个绕轴旋转矩阵，而是建立一条描述旋转的语言链。

第一，三维姿态用旋转矩阵 $R\in SO(3)$ 表示。它不是任意矩阵，而是列向量正交、单位长度、保持右手系的矩阵。

第二，旋转矩阵有三种语义：表示姿态、改变坐标表示、主动旋转对象。公式长得相似时，要靠语义判断左乘还是右乘。

第三，角速度 $\omega$ 描述姿态的瞬时变化，也就是“正在绕哪根轴、以多快转”。它不是欧拉角导数，也不是 $\dot{R}$ 本身。

第四，$[\omega ]\in so(3)$ 的作用是把叉乘矩阵化。这样可以把

$${\dot{r}}_i={\omega }_s\times r_i$$

合并成矩阵公式：

$$\dot{R}=[{\omega }_s]R=R[{\omega }_b]$$

这一步把角速度和旋转矩阵导数连了起来。

第五，矩阵指数和矩阵对数把 $so(3)$ 与 $SO(3)$ 连起来：

$$[\hat{\omega }]\theta \in so(3)⟶R=e^{[\hat{\omega }]\theta }\in SO(3)$$

反过来，$\log R$ 可以从一个姿态中取回等效的旋转轴和旋转角。

通俗地说：$\mathbf{SO}(3)$ 管“已经朝哪儿”， $\mathbf{\omega }$ 和 $[\mathbf{\omega }]$ 管“正在怎样转”，矩阵指数管“转一段后到哪儿”，矩阵对数管“这个姿态可以看成怎么转过来的”。

参考资料

1. Kevin M. Lynch and Frank C. Park, Modern Robotics: Mechanics, Planning, and Control, Chapter 3: Rigid-Body Motions, Cambridge University Press.
2. Modern Robotics video supplement page: Chapter 3: Rigid-Body Motions.
3. Modern Robotics video: Rotation Matrices (Part 1 of 2).
4. Modern Robotics video: Rotation Matrices (Part 2 of 2).
5. Modern Robotics video: Angular Velocities.
6. Modern Robotics video: Exponential Coordinates of Rotation (Part 1 of 2).
7. Modern Robotics video: Exponential Coordinates of Rotation (Part 2 of 2).
8. Lynch and Park, Modern Robotics 2019 preprint PDF. 文中图片为该 PDF 中 3.2 节相关原书图的局部截图。