Part I · 第 1 章 线性代数 · 1.10

正交矩阵与旋转

旋转只搬动东西，不拉伸、不歪斜：方格还是方格，直角还是直角。翻译成矩阵语言，就是两列长度为 1 且互相垂直。这一个条件白送你三样东西：逆就是转置，长度和角度都不变，行列式只能是 ±1。它也是姿态估计里最容易悄悄坏掉的一个条件。

约 30 分钟 前置：1.4 · 1.5 · 1.7 · 1.9 7 个交互

    1为什么机器人需要它

    四旋翼的飞控每秒要读几百到上千次陀螺仪（IMU 的一部分）。每一次都把"这一小段时间转过的角度"乘进当前姿态：\( R \leftarrow R\,\Delta R \)。机器人身上的每个传感器也一样：相机相对机身朝哪、激光雷达相对机身朝哪，都存成一个旋转矩阵。

    旋转矩阵有两个普通矩阵没有的好性质。第一，它只搬动东西，不拉伸：杯子转个方向，大小和形状都不变。第二，是 1.3 节留下的承诺：它的逆就是它的转置，\( R^{-1} = R^\top \)。"相机系到世界系"换成"世界系到相机系"，不需要求逆，把矩阵转过来就行：R_cam_world = R_world_cam.T。

    代价是，旋转矩阵是一个非常"挑剔"的集合。九个数里只有三个是自由的，其余六个被"列长为 1、列列垂直"这六个条件锁死。每次更新都带一点舍入误差，几百次之后，这个矩阵就悄悄地不再是旋转，开始拉伸、剪切，而程序不会报任何错。这一节要看清它到底挑剔在哪里，以及怎么修。

    2直觉：不拉不扯的动作

    把一张方格纸钉在原点，转一下它。方格还是方格，边长没变，直角还是直角，纸上任意两点的距离也没变。这种动作叫刚体运动（rigid motion，这里先只看绕原点的）。

    用 1.3 节的话翻译：网格的形状完全由 î 和 ĵ 的落点（矩阵的两列）决定。要让方格保持原样，这两个落点必须满足两条：

    • 每一列的长度仍是 1（边长没变）；
    • 两列仍然互相垂直（直角没变）。

    满足这两条的列叫标准正交（orthonormal），列标准正交的方阵叫正交矩阵（orthogonal matrix）。下面切换四种变换：F 是被变换的图形（颜色表示分类），圆圈是单位圆。只有保持"圆还是圆、F 没有变形"的才是刚体运动。镜像也不拉伸，但它把 F 翻成了镜像字，这是下面要分清的另一类。

    FIG 1哪些变换是刚体运动？
    虚线是变换前的 F 和单位圆，实心是变换后的。旋转和镜像之后，圆仍然是半径 1 的圆；拉伸和剪切把它压成了椭圆，F 也变了形。右侧读出两列的长度和夹角：刚体运动的两列总是 1、1、90°。
    核心洞见

    "保持长度和角度"和"列标准正交"是同一件事的两种说法。几何说法：网格只被刚性地搬动。代数说法：q₁、q₂ 长度为 1 且垂直。下一节你自己拖动这两列，亲手感受这条边界在哪里。

    3动手玩：正交性实验室

    拖动红色 Qî 和绿色 Qĵ 的箭头尖（或直接改矩阵里的数），F 和网格跟着变。右侧的"体检表"实时算出两列的长度、夹角，以及 \(Q^\top Q\)：它的每个元素是绿色，说明与单位矩阵 \(I\) 吻合。F 的颜色告诉你分类：绿色是旋转，紫色是镜像，红色是不正交。

    FIG 2正交性实验室
    矩阵 Q（拖动或输入）
    \(Q^\top Q\) 的第 (i, j) 格 = \(\mathbf q_i\cdot\mathbf q_j\)，即两列的点积。
    预设
    虚线圆和虚线 F 是原图；实线椭圆是单位圆的像，浅色平行四边形是单位正方形的像。两列垂直时，原点处会出现一个小直角标记。
    试一试
    • 只拖箭头，做出一个逆时针 60° 的旋转。提示：cos 60° = 0.5，sin 60° ≈ 0.87，两列的位置由它们决定。
    • 做出关于直线 y = x 的镜像。提示：镜像把 (1, 0) 送到哪里？(0, 1) 呢？F 会变成什么样？
    • 做一个两列长度都是 1，但 \(Q^\top Q \neq I\) 的矩阵。F 发生了什么？是哪一格告诉了你？
    • 做一个两列互相垂直，但长度不是 1 的矩阵。这次 F 又发生了什么？\(Q^\top Q\) 的哪些格变红了？

    4第二种视角：为什么转置就是逆

    \(Q^\top Q\) 是一张列与列的点积表（1.7 节）：第 (i, j) 格是 \(\mathbf q_i\cdot\mathbf q_j\)。列标准正交意味着：对角线上是 \(1\)（每列自己的点积是长度的平方），对角线外是 \(0\)（不同列垂直）。于是这张表就是单位矩阵：

    $$ Q^\top Q = I \quad\Longrightarrow\quad Q^{-1} = \hl{T}{Q^\top}. $$

    换个角度理解这件事。\(Q^\top\) 的第 \(i\) 行就是 \(\mathbf q_i\)，所以 \(Q^\top\mathbf x\) 的第 \(i\) 个分量是 \(\mathbf q_i\cdot\mathbf x\)，即 \(\mathbf x\) 沿 \(\mathbf q_i\) 方向的投影长度（1.9 节）。反过来，\(Q\) 把这些分量当作配方：\(Q\mathbf c = c_1\mathbf q_1 + c_2\mathbf q_2\)。所以 \(Q^\top\) 负责"读出坐标"，\(Q\) 负责"按坐标搭回向量"，先读后搭，向量就回来了。这个"先读后搭"只在 \(\mathbf q_i\) 互相垂直、长度为 1 时才成立：否则 \(\mathbf q_i\cdot\mathbf x\) 就不是干净的坐标。

    下面先对一个旋转、一个镜像、一个非正交矩阵各做一遍：先作用 \(Q\)，再作用 \(Q^\top\)。点击"播放"，或者拖动阶段滑块。

    FIG 3先 Q，再 Qᵀ
    虚线 F 是起点。旋转和镜像：作用 \(Q^\top\) 后 F 精确回到起点。"非正交"：\(A^\top\) 不是 \(A\) 的逆，F 回不去（真正的 \(A^{-1}\) 存在，但长得和 \(A^\top\) 不一样，见右侧两个矩阵）。镜像是靠"压扁再翻开"的插值动画演示的：它没法靠连续转动得到，这正是镜像和旋转的区别。

    行是什么？旧坐标轴在新坐标系里的样子

    旋转矩阵 \(R\) 的列是新坐标轴在旧坐标系里的坐标（1.3 节的被动视角）。因为 \(R^{-1} = R^\top\)，它的行也有清晰的含义：第 \(i\) 行是旧的第 \(i\) 根坐标轴，写在新坐标系里的坐标。拖动红色箭头尖转动新坐标系，看琥珀色的旧坐标轴向新轴上作垂线。

    FIG 4行 = 旧坐标轴的新读数
    红、绿线段的长度（带符号）是旧轴在新轴 x′、y′ 上的投影，它们正是 \(R\) 的那一行。投影之所以能直接当坐标读，是因为新轴互相垂直、长度为 1。

    5第三种视角：三维里的旋转

    三维的 \(R\) 有三列，也就是机体坐标系的三根轴 x′、y′、z′ 在世界坐标系里的样子。九个数里只有三个自由：用偏航、俯仰、横滚三个角（欧拉角）就能生成它。这里用 yaw → pitch → roll（ZYX）的顺序，欧拉角的各种麻烦（万向锁、多种约定）留给 5.2 节。

    拖动滑块，看 \(R\)、\(R^\top R\) 和行列式怎么变：不管怎么转，\(R^\top R\) 都是单位矩阵，行列式都是 \(+1\)。然后打开"镜像"，把一根轴取反：\(R^\top R\) 仍然是 \(I\)，行列式却变成 \(-1\)，坐标系变成左手系。它还是"正交矩阵"，但不再是旋转。

    FIG 5三维姿态与检查
    基本旋转预设
    细灰线是世界坐标轴，红绿蓝粗箭头是机体轴（\(R\) 的三列），半透明的盒子是随机体转动的车身，琥珀色小柱是装在车身上的一根天线。拖动画面可以转动视角。
    • 用滑块让蓝色 z′ 轴指向世界 +x 方向（\(R\) 的第 3 列变成 (1, 0, 0)）。提示：试试 \(R_y(90^\circ)\) 预设。
    • 打开"镜像"，让行列式变成 \(-1\)。注意右侧的 \(R^\top R\) 依然是 \(I\)。

    6再看数学

    提示：把鼠标悬停在公式中带底色的符号上，FIG 2 会加粗对应的箭头、夹角或椭圆；悬停 \(Q^\top\) 则加粗 FIG 3 里的 F。

    定义

    一个 \(n\times n\) 实矩阵 \(Q\) 是正交矩阵，当且仅当它的列 \(\mathbf q_1,\dots,\mathbf q_n\) 标准正交，即

    $$ \hl{qtq}{Q^\top Q} = \begin{bmatrix} \hl{n1}{\ci{\mathbf q_1}\!\cdot\!\ci{\mathbf q_1}} & \hl{dot}{\ci{\mathbf q_1}\!\cdot\!\cj{\mathbf q_2}} \\ \hl{dot}{\cj{\mathbf q_2}\!\cdot\!\ci{\mathbf q_1}} & \hl{n2}{\cj{\mathbf q_2}\!\cdot\!\cj{\mathbf q_2}} \end{bmatrix} = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} = I \qquad (n=2). $$

    这里为了好读写成了 \(2\times2\)，一般情形里 \(Q^\top Q\) 的 \((i,j)\) 格是 \(\mathbf q_i\cdot\mathbf q_j = \delta_{ij}\)（\(i=j\) 为 1，否则为 0）。"正交"这个名字有历史包袱：它要求的是列标准正交，只"垂直"并不够，还要长度为 1（见误区 5）。

    它带来的性质

    $$ (Q\mathbf x)\cdot(Q\mathbf y) = \mathbf x^\top \hl{qtq}{Q^\top Q}\,\mathbf y = \mathbf x\cdot\mathbf y, \qquad |Q\mathbf x| = |\mathbf x|, \qquad \angle(Q\mathbf x, Q\mathbf y) = \angle(\mathbf x,\mathbf y). $$

    点积不变，所以长度（自己和自己的点积再开方）和夹角（1.7 节：由点积算出来）也不变。反过来，保持所有长度的线性变换一定是正交的。

    $$ Q^\top Q = I \;\Longrightarrow\; Q^{-1} = Q^\top, \quad\text{and then also}\quad QQ^\top = I. $$

    \(Q^\top\) 是 \(Q\) 的左逆；方阵的左逆必是逆，所以 \(QQ^\top = I\) 也成立：行也标准正交。这就是 FIG 4 里"行读出旧坐标"的原因。

    $$ \det(Q^\top Q) = \det(Q^\top)\det(Q) = \det(Q)^2 = \det I = 1 \;\Longrightarrow\; \det Q = \pm 1. $$

    用 1.5 节的话：正交矩阵不改变面积（体积），只可能翻不翻面。\(\det Q = +1\) 是旋转，\(\det Q = -1\) 则带一次镜像：一个反射（reflection），或旋转再加一次反射（三维里的 \(-I\) 就是这样）。全体 \(n\times n\) 正交矩阵记作 \(O(n)\)，其中行列式为 \(+1\) 的那一半记作 \(SO(n)\)（special orthogonal group）。注意方向：正交 ⇒ \(\det=\pm1\)，反过来不成立（剪切矩阵 \(\begin{bmatrix}1&1\\0&1\end{bmatrix}\) 的行列式是 1，却不正交）。

    二维：SO(2)

    第一列是单位向量，可以写成 \((\cos\theta,\sin\theta)\)。第二列要垂直于它且长度为 1，只剩 \(\pm(-\sin\theta,\cos\theta)\) 两个选择；行列式 \(+1\) 选中了加号。所以每个二维旋转都长这样，由一个角度决定：

    $$ R(\theta) = \begin{bmatrix} \ci{\cos\theta} & \cj{-\sin\theta} \\ \ci{\sin\theta} & \cj{\cos\theta} \end{bmatrix}, \qquad R(\alpha)\,R(\beta) = R(\alpha+\beta), \qquad R(\theta)^{-1} = R(\theta)^\top = R(-\theta). $$

    二维旋转"先转 \(\beta\) 再转 \(\alpha\)"等于一次转 \(\alpha+\beta\)，而且顺序可以交换。（三维就不行了，见下。）

    三维：SO(3) 与基本旋转

    绕三根坐标轴的转动是 3D 旋转的"积木"：

    $$ R_x(\theta)=\begin{bmatrix}\ci{1}&\cj{0}&\ck{0}\\ \ci{0}&\cj{\cos\theta}&\ck{-\sin\theta}\\ \ci{0}&\cj{\sin\theta}&\ck{\cos\theta}\end{bmatrix},\;\; R_y(\theta)=\begin{bmatrix}\ci{\cos\theta}&\cj{0}&\ck{\sin\theta}\\ \ci{0}&\cj{1}&\ck{0}\\ \ci{-\sin\theta}&\cj{0}&\ck{\cos\theta}\end{bmatrix},\;\; R_z(\theta)=\begin{bmatrix}\ci{\cos\theta}&\cj{-\sin\theta}&\ck{0}\\ \ci{\sin\theta}&\cj{\cos\theta}&\ck{0}\\ \ci{0}&\cj{0}&\ck{1}\end{bmatrix}. $$

    \(R_y\) 里的负号在左下角，不是右上角：这是为了保持右手系（\(z\to x\) 的方向是"正方向"）。三维旋转的乘积还是旋转：\((AB)^\top(AB) = B^\top A^\top A B = B^\top B = I\)，行列式相乘仍是 \(+1\)。所以 \(SO(3)\) 对乘法封闭，有单位元 \(I\)，每个元素有逆（转置），这就是一个群（5.4 节）。

    但三维旋转不能交换顺序。取 90° 的两个：\(R_x(90^\circ)R_z(90^\circ) = \begin{bmatrix}0&-1&0\\0&0&-1\\1&0&0\end{bmatrix}\)，而 \(R_z(90^\circ)R_x(90^\circ) = \begin{bmatrix}0&0&1\\1&0&0\\0&1&0\end{bmatrix}\)，两者不同（1.4 节：顺序很重要）。

    最后一个预告：每个三维旋转都有一根转轴，即被它原样留下的方向，也就是特征值为 1 的特征向量（1.11 节）。直观原因：奇数维的旋转，行列式为 1，特征值又成对出现，必有一个特征值是 1。

    7机器人中的它：姿态漂移与修复

    陀螺仪测的是机体角速度 \(\boldsymbol\omega\)。每隔 \(\Delta t\) 秒，姿态要更新一次。严格的更新是 \(R \leftarrow R\,\Exp(\boldsymbol\omega\Delta t)\)（5.6 节），它会把 \(R\) 精确地留在旋转里。但图省事的人常用一阶近似：

    $$ R_{k+1} = R_k\,(I + \skew{\boldsymbol\omega\Delta t}), \qquad (I+\Omega)^\top(I+\Omega) = I - \Omega^2 = I + \|\boldsymbol\omega\Delta t\|^2\,(I - \mathbf u\mathbf u^\top) \neq I, $$

    其中 \(\Omega = \skew{\boldsymbol\omega\Delta t}\) 是 1.8 节的反对称矩阵，\(\mathbf u\) 是转轴方向。这个近似矩阵不是旋转：每一步，垂直于转轴的方向被多拉长约 \(\sqrt{1+\|\boldsymbol\omega\Delta t\|^2}\) 倍。单步只差万分之几，几百步以后就积累成肉眼可见的变形。另一种常见的偏差来源是舍入：把 \(R\) 存成低精度（或定点数）时，每一步都有微小误差，它们像随机游走一样慢慢累积。

    下面的实验模拟这个过程：机体以恒定角速度转动，每步更新 \(R\)（100 Hz）。左边是 \(R\) 的三列（粗箭头）和单位球的像（线框），细箭头是理想姿态，右边是 \(\|R^\top R - I\|\)（对数坐标）随步数的变化。当偏差越过红色虚线，就应当修复。

    FIG 6姿态漂移实验
    R 的三列与单位球的像
    ‖RᵀR − I‖ 随步数（对数轴）
    "一阶更新"每步都把垂直于转轴的方向拉长一点，球变成椭球，列向量越来越长且不再垂直。"舍入到 3 位小数"的误差小得多，但永远不会消失。修复之后曲线垂直掉到 10⁻⁶ 以下，但只修形状，不能修好已经积累的朝向误差。两种修法的差别看读数：格拉姆–施密特保持第 1 列的方向不变，把全部修正压在后两列上，所以朝向会被额外带偏（在 0.05 的容许线上约 0.5°，在偏差 0.4 时约 3.5°，见"与理想的夹角"）；SVD 取"最近的旋转"，修正量最小（"修正量"），朝向几乎不动。
    • 运行实验，让 \(\|R^\top R - I\|\) 越过红色虚线（0.05）。此时的 \(R\) 还是旋转吗？单位球变成了什么？
    • 越线之后，点"立即修复"，让偏差掉到 10⁻⁶ 以下。再试试另一种修复方法，比较"修正量"和"与理想的夹角"。
    怎么修：两种重新正交化

    格拉姆–施密特（1.9 节）：\(\mathbf q_1 = \mathbf c_1/|\mathbf c_1|\)，\(\mathbf q_2\) 是 \(\mathbf c_2\) 去掉沿 \(\mathbf q_1\) 的分量再归一化，\(\mathbf q_3 = \mathbf q_1\times\mathbf q_2\)。便宜，但有偏向：第 1 列原样保留，误差全部压在后面的列上，而且结果依赖列的顺序。

    SVD 最近旋转（1.12 节）：若 \(R = U\Sigma V^\top\)，则离 \(R\) 最近（弗罗贝尼乌斯范数）的旋转是 \(U V^\top\)，即把所有"拉伸" \(\Sigma\) 直接换成 1。为了保证行列式为 \(+1\)，写成 \(U\,\mathrm{diag}(1,1,\det(UV^\top))\,V^\top\)。一视同仁、修正量最小，这也是点云配准里 Kabsch / Umeyama 算法求最优旋转的方法。本页的实现用等价的牛顿迭代 \(Q\leftarrow\tfrac12(Q+Q^{-\top})\) 求 \(UV^\top\)。

    真实系统里

    姿态估计器几乎都带"防漂移"这一步。以旋转矩阵存姿态的（比如经典的 DCM 算法）每次更新后都做一次重新正交化；以单位四元数存姿态的（5.2 节）只需要把 4 个数除以长度，一步就够，这也是它在嵌入式里受欢迎的原因之一。测试里也常加一条断言：‖RᵀR − I‖ < 1e-6 且 det R > 0。

    8常见误区

    误区 1：对旋转矩阵的元素直接线性插值

    想在两个姿态之间平滑过渡，最直观的做法是 \((1-t)R_0 + tR_1\)。这不是旋转：它既不保持长度，也不保持垂直。极端例子：\(R(0^\circ) = I\)，\(R(180^\circ) = -I\)，它们的中点是零矩阵，整个平面塌成一个点。下面把终点角度拉到 180°，把 \(t\) 拉到 0.5 看看。绿色是"按角度插值" \(R((1-t)\theta_0 + t\theta_1)\)，红色是"逐元素插值"。正确的做法在 5.9 节展开。

    FIG 7逐元素插值 vs 按角度插值
    紫色虚线是终点姿态。红色 F 的缩放比为 \(\sqrt{1-2t(1-t)(1-\cos\theta_1)}\)，只有 \(t=0\) 或 \(1\) 时才是 1；它转动的速度也不均匀。
    • 让红色 F 缩到几乎一个点（缩放比 < 0.08）。
    误区 2：连续乘了很多次，却从不重新正交化

    即使每一步用的是严格的旋转，浮点舍入也会让 \(R^\top R\) 一点点偏离 \(I\)；用一阶近似更是每步都多拉长一点（FIG 6）。变形后的 \(R\) 再乘向量，长度就不对了，而且 \(R^\top\) 也不再是 \(R^{-1}\)，坐标变换来回走会走不回原处。给长时间运行的积分器加上周期性的修复，并监控 \(\|R^\top R - I\|\)。

    误区 3：行列式为 −1 的"旋转"，一个镜像的坐标系

    标定、手眼标定、点云配准算出来的 \(R\) 有时满足 \(R^\top R = I\)，行列式却是 \(-1\)：某根轴被取了反，坐标系变成左手系（FIG 5 的"镜像"）。它通过了"正交"检查，但没有任何真实的刚体能转成这样，用它做位姿会让物体整个翻成镜像。Kabsch / Umeyama 的 SVD 解就可能给出这种结果，标准修正是把最后一个奇异向量取反（即上面的 \(\mathrm{diag}(1,1,\det(UV^\top))\)）。所以检查旋转时，一定同时看 \(R^\top R = I\) 和 \(\det R = +1\)。

    误区 4：R 和 Rᵀ 用反了

    \(R\) 到底是把哪个坐标系的坐标换成哪个？约定（本课程，与 5.1 节一致）：\(R_{AB}\) 把 \(B\) 系的坐标换成 \(A\) 系的坐标，\(R\) 的列是 \(B\) 的轴写在 \(A\) 里。所以 R_world_cam @ p_cam 得到 p_world，而 R_world_cam.T 才是 R_cam_world。变量名里的下标像分数一样相消：R_world_cam @ R_cam_tool 得到 R_world_tool。再加上 1.3 节的主动与被动：把点转 30°，和把坐标系转 30°，读数互为转置。动手前，先问"这是在搬点，还是在换系？"

    误区 5："正交矩阵"只要求列互相垂直

    名字起得不好：正交矩阵的列要标准正交，即垂直且长度为 1。列互相垂直但长度各异的矩阵只是缩放了各个方向（FIG 2 的第四个任务），\(Q^\top Q\) 是对角矩阵，不是 \(I\)，\(Q^\top\) 也不是它的逆。反过来，行列式等于 1 也不够（剪切）。唯一靠得住的检验是 \(Q^\top Q = I\)。

    9小练习

    1. 下面哪个矩阵是正交矩阵？
    逐个算 \(Q^\top Q\)。A 是 \(\begin{bmatrix}1&1\\1&2\end{bmatrix}\)（行列式为 1 的剪切，不正交）；B 是 \(2I\)（列垂直但长度是 \(\sqrt2\)）；D 是 \(\mathrm{diag}(4,\,0.25)\)（行列式也是 1，却在缩放）；只有 C 得到 \(I\)：\(0.6^2+0.8^2=1\)，\(0.6\cdot(-0.8)+0.8\cdot0.6=0\)。它是旋转约 53.1°。
    2. 相机的朝向是 \(R_{\text{world,cam}} = R_z(90^\circ) = \begin{bmatrix}0&-1&0\\1&0&0\\0&0&1\end{bmatrix}\)（只看旋转，忽略平移）。世界坐标里 \((1,0,0)\) 这个方向，在相机坐标系中的读数是？
    要从世界系换到相机系，用 \(R_{\text{cam,world}} = R_{\text{world,cam}}^\top\)（逆就是转置）：\(\begin{bmatrix}0&1&0\\-1&0&0\\0&0&1\end{bmatrix}\begin{bmatrix}1\\0\\0\end{bmatrix} = \begin{bmatrix}0\\-1\\0\end{bmatrix}\)。一个常见的错误是直接乘 \(R\)，得到 \((0,1,0)\)。直观检查：相机的 x 轴指向世界的 y 方向，所以世界的 x 方向在相机看来是它的"负 y"（右手边）。
    3. 一个姿态积分器跑了很久，现在 \(\|R^\top R - I\| \approx 0.2\)。该怎么办？
    0.2 已经不可忽略：列长偏离了百分之几，直角偏了约 3°，而且误差还在随步数继续增长；\(R^\top\) 也不再是 \(R^{-1}\)。只有投回旋转集合才能修好。\(R^\top\) 对一个已经变形的 \(R\) 毫无帮助；除以行列式只能修整体缩放，而一阶漂移是各向异性的拉伸，修不好。
    4. 一次标定返回的矩阵 \(R\) 满足 \(R^\top R = I\)，但 \(\det R = -1\)。最合理的判断是？
    \(R^\top R = I\) 说明它是正交的，\(R^\top = R^{-1}\) 仍然成立（选项 D 不对）；行列式 \(-1\) 说明有镜像。任何转角的旋转行列式都是 \(+1\)。在三维里，除以 \(-1\) 只会把所有轴一起取反（得到 \(-R\)，行列式是 \(+1\)，但那是另一个姿态，不是原来想要的）。应当检查是哪根轴被标反了。
    5. 二维姿态 \(R(0^\circ)\) 与 \(R(180^\circ)\) 的逐元素平均 \(\tfrac12(R_0+R_1)\) 是什么？
    \(R(0^\circ) = I\)，\(R(180^\circ) = -I\)，平均是 \(\tfrac12(I - I) = 0\)。正确的中点是 \(R(90^\circ)\)，要对角度（或 5.9 节的李代数）做插值，而不是对矩阵元素。

    10延伸阅读

    • G. Strang, Introduction to Linear Algebra, §4.4 — 标准正交基、正交矩阵与格拉姆–施密特
    • K. Lynch & F. Park, Modern Robotics, §3.2 — 旋转与角速度（SO(3) 的定义与性质）
    • 3Blue1Brown, Essence of Linear Algebra — 第 5 章（三维空间中的线性变换）
    • J. Solà, J. Deray & D. Atchuthan, “A micro Lie theory for state estimation in robotics”, 2018 — 以后讲 SO(3) 的指数映射与插值时会用到
    • W. Kabsch, “A solution for the best rotation to relate two sets of vectors”, Acta Cryst. A32, 1976; S. Umeyama, “Least-squares estimation of transformation parameters between two point patterns”, IEEE TPAMI 13(4), 1991 — 用 SVD 求最近 / 最优旋转的原始论文