Part I · 第 5 章刚体运动与李群 · 5.2

旋转的表示

同一个朝向可以写成 9 个数（旋转矩阵）、3 个角（欧拉角）、一根轴加一个角（轴角），或者 4 个数（四元数）。它们描述的是同一件事，却各有各的长处和陷阱：欧拉角直观，但会"万向节死锁"；四元数紧凑、插值方便，却把每个旋转写了两遍。

约 25 分钟前置：1.10 正交矩阵与旋转 · 5.1 坐标系与位姿 5 个交互

1为什么机器人需要它

一架无人机的飞控、一个机械臂的末端姿态、一台手机里的陀螺仪，都要存"现在朝哪个方向"。可是软件里到处是不同的写法：遥控器上的横滚、俯仰、偏航是欧拉角；ROS 的消息里是四元数；运动学里是 3×3 的旋转矩阵；视觉里的 Rodrigues 向量是轴角。你每天都要在它们之间来回转换，所以必须知道每一种在什么时候好用，什么时候会坑你。

旋转只有三个自由度（1.10 节：9 个数，6 个约束），但没有任何一种只用 3 个数、没有奇点、又能连续描述所有旋转的写法。这是拓扑上的事实（$\SO$ 不同胚于任何三维欧氏空间的一块区域）。所以每种表示都是一种取舍。这一节把四种表示放在一起，让你亲手感受这些取舍。

2直觉：同一个朝向，四种写法

约定：右手系，$z$ 向上，旋转都是主动的、右手定则为正（拇指指向转轴，四指的方向是正方向）。$R$ 的三列是机体的 $x, y, z$ 轴在世界系里的方向（1.10 节、5.1 节），所以 $R$ 把机体系的读数变成世界系的读数。

旋转矩阵 $R$：9 个数，满足 $R\T R = I$、$\det R = 1$。能直接乘向量、直接连乘，不会有奇点，但冗余。
欧拉角 $(\psi, \theta, \varphi)$：三个角，依次绕 $z$（偏航 yaw）、$y$（俯仰 pitch）、$x$（横滚 roll）转。人最容易读懂，但有奇点。
轴角 $(\hat{\mathbf u}, \theta)$：一根单位转轴加一个转角。几何上最直接；它的"长度为 $\theta$ 的向量" $\boldsymbol\omega = \theta\hat{\mathbf u}$ 就是 5.6 节的旋转向量。
单位四元数 $q = (w, x, y, z)$：$w = \cos\tfrac\theta2$，$(x,y,z) = \sin\tfrac\theta2\,\hat{\mathbf u}$。4 个数、1 个约束 $\|q\| = 1$。

下面这个"罗塞塔石碑"里，用三种输入方式中的任何一种调整朝向，其余的读数会实时跟着算出来。

FIG 1罗塞塔石碑：四种表示实时互转

拖动画面旋转视角

淡色是世界坐标系，亮色是机体（带一个竖直的尾翼，方便看出横滚）。紫色虚线是转轴。欧拉角为 ZYX 顺序，即 $R = R_z(\psi)R_y(\theta)R_x(\varphi)$。拖动任何一行的滑块，另外两行的滑块和右边的读数都会跟着变。四元数那一行会自动归一化。

试一试

用任意一种输入，做出绕 z 轴转 90°。看看另外三种表示各自是什么。（四元数应该是 $(0.71, 0, 0, 0.71)$，为什么是 0.71？）
做出绕 x 轴转 180°。四元数的 $w$ 是多少？为什么？
点四元数那一行的"取负 $-q$"按钮。物体动了吗？旋转矩阵变了吗？

核心洞见

四种写法只是同一个对象的不同坐标。真正的对象是 $\SO$ 里的一个点，这个点没有"天然"的坐标。每一种参数化在某些地方好用，在另一些地方失效。区别只在于：它是否连续、有没有奇点、能不能直接连乘或插值。

3动手玩：万向节死锁

欧拉角相当于三个嵌套的"万向节环"：最外面的环绕世界 $z$ 轴转（偏航），中间的环绕被带动的 $y$ 轴转（俯仰），最里面的环绕被带动的 $x$ 轴转（横滚），物体装在最里层。每个环的转轴都由外面的环决定。

当俯仰角到 $\pm 90°$，最里面的转轴 $x$ 被折到了和最外面的转轴 $z$ 重合（红色箭头压在蓝色箭头上）。这时转偏航和转横滚，物体做的是同一种运动，一个自由度丢了。这叫万向节死锁。在它附近，要产生某个角速度，欧拉角需要变化得非常快。

FIG 2万向节与死锁

拖动画面旋转视角

蓝环 = 偏航，绿环 = 俯仰，红环 = 横滚。三根彩色箭头是三个环各自的转轴（蓝 $\hat z$、绿 $\hat y'$、红 $\hat x''$）。右边的 $\det J$ 是"欧拉角速度 $\to$ 世界角速度"的 3×3 矩阵的行列式，它等于 $-\cos\theta$：到 $\pm 90°$ 变成 0，矩阵不可逆。"微调测试"把偏航和横滚各加 10°（俯仰为负时横滚取反），看物体净转了多少度。

试一试

把俯仰调到 $\pm 90°$（点预设），看红色转轴和蓝色转轴怎样重合。
在俯仰 $90°$ 处点"记下当前朝向"，再同时改变偏航和横滚（保持 $\psi - \varphi$ 不变），让它们都和记下时相差 20° 以上，而朝向差保持在 1° 以内。两组完全不同的欧拉角，同一个朝向。
先在俯仰 $0°$ 附近读一下"微调测试"的净转角（约 14°），再把俯仰设到约 $88°$ 再读一次（只剩约 0.35°）：在死锁附近，偏航和横滚各改 10° 几乎完全抵消，欧拉角的大变化只对应很小的转动，数值上很不稳。

另一个麻烦：双覆盖

四元数没有奇点，代价是每个旋转有两个四元数：$q$ 和 $-q$ 表示同一个旋转。原因是半角：$q = (\cos\tfrac\theta2, \sin\tfrac\theta2\,\hat{\mathbf u})$，转角 $\theta$ 转一整圈（360°），$q$ 只走了半圈，变成了 $-q$；要转 720° 才回到 $q$。用下面的滑块把 $\theta$ 拉到 360°：物体回到了原样，右边圆上的点却在 $(-1, 0)$。

FIG 3双覆盖：转 720° 才回到同一个四元数

机体：绕固定轴转 θ

四元数 q 在单位圆上

右图：横轴是 $w = \cos\tfrac\theta2$，纵轴是 $\sin\tfrac\theta2$（四元数的向量部分沿 $\hat{\mathbf u}$ 的分量，可正可负）。点随 $\theta$ 在单位圆上以一半的角速度转动。左图的机体转 360° 就回到原样，右图的点却只走了半圈。

试一试

把 $\theta$ 设到 360°：机体回到原样，$q$ 却是 $(-1, 0, 0, 0)$。
再继续到 720°，$q$ 才回到 $(1, 0, 0, 0)$。

插值：欧拉角、slerp、nlerp

把一个朝向平滑地转到另一个朝向，各种表示给出不同的路径。欧拉角线性插值：三个角各自匀速变化，路径可能绕远路，角速度忽大忽小。slerp（球面线性插值）：沿四元数单位球面上的大圆弧匀速走，等价于绕一根固定轴匀速转，是最短、最匀的路径。nlerp：先对四元数做线性插值，再归一化，路径和 slerp 相同，只是速度略不均匀，但算起来便宜。

注意"最短"依赖于符号：如果 $q_0 \cdot q_1 < 0$，应该用 $-q_1$ 替换 $q_1$，否则会走一条超过 180° 的长路。

FIG 4三种插值，同一对起点和终点

拖动画面旋转视角

角速度（相对 slerp 的均匀速度）

从左到右（沿 $y$ 轴排开）：紫 = 欧拉角线性插值，琥珀 = slerp，青 = nlerp。每个物体的机头轨迹用同色线画出（粗线是已走过的部分），灰点是起点机头，绿点是终点机头。右图：三种方法的角速度随 $s$ 的变化；slerp 是一条水平线。"最短路径"关掉后，当 $q_0\cdot q_1<0$，slerp 和 nlerp 会走长的那条路。

试一试

用滑块或预设，让欧拉角插值的峰值角速度超过 slerp 的 2 倍。
关掉"最短路径"，找一对起点终点，使 slerp 实际转过的总角度超过 180°。再打开，它就变回 ≤180°。

4再看数学

提示：把鼠标悬停在公式中带底色的符号上，FIG 3 中对应的量会被高亮。

欧拉角（ZYX，偏航-俯仰-横滚）

$R = R_z(\psi)\,R_y(\theta)\,R_x(\varphi)$：先绕 $z$ 转 $\psi$（偏航），再绕转过之后的新 $y'$ 轴转 $\theta$（俯仰），最后绕新的 $x''$ 轴转 $\varphi$（横滚）（这是"内旋"的读法，和 FIG 2 的万向节一致；等价的"外旋"读法是先绕世界 $x$ 转 $\varphi$，再绕世界 $y$ 转 $\theta$，最后绕世界 $z$ 转 $\psi$，顺序倒过来）。展开是

$$ R = \begin{bmatrix} c_\psi c_\theta & c_\psi s_\theta s_\varphi - s_\psi c_\varphi & c_\psi s_\theta c_\varphi + s_\psi s_\varphi \\ s_\psi c_\theta & s_\psi s_\theta s_\varphi + c_\psi c_\varphi & s_\psi s_\theta c_\varphi - c_\psi s_\varphi \\ -s_\theta & c_\theta s_\varphi & c_\theta c_\varphi \end{bmatrix}, \qquad \theta = -\arcsin R_{31},\ \ \psi = \operatorname{atan2}(R_{21}, R_{11}),\ \ \varphi = \operatorname{atan2}(R_{32}, R_{33}). $$

当 $\theta = \pm 90°$ 时 $c_\theta = 0$，矩阵第一列和最后一行只剩常数，其余元素只依赖 $\varphi - \psi$（$\theta = +90°$）或 $\varphi + \psi$（$\theta = -90°$），$\psi$ 和 $\varphi$ 无法分开。这就是死锁；上面的公式也在那里失效（$R_{11} = R_{21} = 0$），代码必须单独处理。同一个朝向的"三轴顺序"有 12 种（ZYX、ZYZ、XYZ……），"内旋 / 外旋"再翻一倍，所以脱离约定谈欧拉角没有意义。

欧拉角速度和角速度的关系也在 $\theta = \pm 90°$ 处退化：$\boldsymbol\omega_{\text{world}} = \dot\psi\,\hat z + \dot\theta\,\hat y' + \dot\varphi\,\hat x''$，三根轴 $\hat z$、$\hat y'$、$\hat x''$ 组成的矩阵 $J$ 的行列式 $\det J = -\cos\theta$（FIG 2）。

轴角与 Rodrigues 公式

转轴 $\hat{\mathbf u}$、转角 $\theta$ 给出旋转矩阵（5.6 节会从指数映射推出它）：

$$ R = I + \sin\theta\,\skew{\hat{\mathbf u}} + (1-\cos\theta)\,\skew{\hat{\mathbf u}}^2, \qquad \theta = \arccos\tfrac{\operatorname{tr}R - 1}{2}, \quad \skew{\hat{\mathbf u}} = \tfrac{R - R\T}{2\sin\theta}. $$

这里的 $\skew{\hat{\mathbf u}}$ 是 1.8 节的叉积矩阵。轴角也不唯一：$(\hat{\mathbf u}, \theta)$ 与 $(-\hat{\mathbf u}, -\theta)$ 相同，$\theta \to \theta + 2\pi$ 也相同，$\theta = 0$ 时轴无定义，$\theta = \pi$ 时 $\pm\hat{\mathbf u}$ 都对。

单位四元数

四元数 $q = w + x\,\mathbf i + y\,\mathbf j + z\,\mathbf k$，写成 $q = (w, \mathbf v)$。本课程用 Hamilton 约定、$(w,x,y,z)$ 顺序。乘法是

$$ q_1 \otimes q_2 = \big(w_1 w_2 - \mathbf v_1\cdot\mathbf v_2,\; w_1\mathbf v_2 + w_2\mathbf v_1 + \mathbf v_1\times\mathbf v_2\big). $$

单位四元数 $\|q\| = 1$ 与旋转的对应是

$$ q = \Big(\hl{w}{\cos\tfrac{\theta}{2}},\; \hl{v}{\sin\tfrac{\theta}{2}\,\cw{\hat{\mathbf u}}}\Big), \qquad (0, \mathbf p') = q \otimes (0, \mathbf p) \otimes q^{*}, \qquad R(q)\,\mathbf p = \mathbf p'. $$

为什么是半角：旋转 $\mathbf p \mapsto q\mathbf p q^*$ 在左右各乘了一次 $q$，每次只出一半的转角。于是 $q$ 和 $-q$ 给出同样的变换（两个负号抵消），这就是双覆盖：单位四元数构成三维球面 $S^3$，它 2 对 1 地覆盖 $\SO$。连乘对应：$R(q_1\otimes q_2) = R(q_1)R(q_2)$。矩阵形式为

$$ R(q) = \begin{bmatrix} 1-2(y^2+z^2) & 2(xy - wz) & 2(xz + wy) \\ 2(xy + wz) & 1-2(x^2+z^2) & 2(yz - wx) \\ 2(xz - wy) & 2(yz + wx) & 1-2(x^2+y^2) \end{bmatrix}. $$

每步乘完后把 $q$ 除以 $\|q\|$ 就能纠正数值漂移（比重新正交化 $R$ 便宜得多）。插值 slerp 为 $q(s) = \dfrac{\sin((1-s)\Omega)}{\sin\Omega}\,q_0 + \dfrac{\sin(s\Omega)}{\sin\Omega}\,q_1$，其中 $\cos\Omega = q_0\cdot q_1$。

放在一起比较

	矩阵	欧拉角	轴角	四元数
数字个数	9 (−6)	3	3 or 4	4 (−1)
奇点	无	有（死锁）	$\theta=0$ 轴不定	无，但 $q\equiv -q$
连乘 / 作用于向量	直接	要转成矩阵	要转成矩阵	较直接
插值	不能直接插	不均匀	好（5.6 节）	slerp / nlerp
人读起来	一般	最好	好	差

5机器人中的它：姿态控制与"取最短路径"

四旋翼的姿态控制器每个周期都做同一件事：算出当前姿态和目标姿态之间的误差旋转，再用它去驱动机体转向目标。用四元数写：$q_e = q_{\text{cur}}^{*}\otimes q_{\text{des}}$（机体系里的误差），期望角速度 $\boldsymbol\omega_{\text{cmd}} = 2k\,\mathbf v_e$，其中 $\mathbf v_e$ 是 $q_e$ 的向量部分（小角度时 $2\mathbf v_e \approx$ 误差转角向量）。

陷阱在双覆盖：目标姿态 $q_{\text{des}}$ 和 $-q_{\text{des}}$ 是同一个朝向，但算出的 $\mathbf v_e$ 方向相反。若 $w_e < 0$，$q_e$ 在"远的那一半球面"上，控制器会朝错误的方向转大半圈。PX4 等飞控的做法是：如果 $w_e < 0$，先把 $q_e$ 换成 $-q_e$，保证转角不超过 180°。

下面让机体从单位姿态出发，目标是绕 $z$ 轴转 270°（$q_{\text{des}} = (-0.71, 0, 0, 0.71)$，$w<0$）。分别开关"符号修正"，看机体怎么转。

FIG 5姿态误差控制：有无符号修正

拖动画面旋转视角

淡淡的绿色机体是目标姿态，青色机体是当前姿态，琥珀色曲线是机头走过的轨迹。控制律：$\boldsymbol\omega = 2k\,\sigma\,\mathbf v_e$，$\sigma = \operatorname{sign}(w_e)$（开启修正时）或恒为 1（关闭时）；积分用 $q \leftarrow q\otimes\Exp(\boldsymbol\omega\,dt)$（把旋转 $\Exp(\boldsymbol\omega\,dt)$ 写成四元数）。"累计转角"是 $\int\|\boldsymbol\omega\|dt$。

试一试

关闭符号修正并运行：机体沿正方向转了 270°，走了大半圈。
开启符号修正再运行：机体反方向只转 90° 就到了。

真实系统里

ROS 的 geometry_msgs/Quaternion 字段顺序是 (x, y, z, w)，而 Eigen 的构造函数 Quaternion(w, x, y, z) 却是 w 在前（它内部的 coeffs() 又是 x, y, z, w）。Hamilton 与 JPL 两种四元数约定不同（乘法顺序、旋转方向都可能相反），ROS、Eigen、Ceres 用 Hamilton，不少航天与早期 VIO 文献用 JPL。UI 和遥控用欧拉角；运动学、点云变换用矩阵；状态估计里的状态用四元数或旋转矩阵，增量用轴角（旋转向量）（5.6、5.7 节）；相机标定输出的 Rodrigues 向量就是轴角。

6常见误区

误区 1：谈欧拉角不说约定

"roll=30°、pitch=20°、yaw=10°"本身不是一个旋转：顺序是 ZYX 还是 XYZ？绕机体轴还是世界轴？角度单位是度还是弧度？这几个问题答案不同，得到的矩阵就不同。接口文档和变量名里要写清楚（例如 euler_zyx_rad），并且不要用欧拉角做内部状态、插值或求导。

误区 2：以为死锁只在正好 90° 才发生

离 $\pm 90°$ 还有几度时，$\psi$ 和 $\varphi$ 虽然在数学上可分，数值上已经病态：$\psi$ 的一点点噪声会被 $\varphi$ 的反向大变化抵消。滤波器若把欧拉角当状态，在这里会出现巨大的、没有物理意义的跳变（FIG 2 的第三个任务）。

误区 3：把四元数当普通 4 维向量比较、平均

$q$ 和 $-q$ 是同一个旋转，但 $\|q - (-q)\| = 2$ 最大。比较两个姿态要看 $|q_1\cdot q_2|$（等于 $\cos$ 的半角），对一组四元数求平均前要先把它们统一到同一个半球（$q_i\cdot q_1 \ge 0$），否则平均值可能接近零。严格的平均要在流形上做（5.9 节）。

误区 4：四元数的顺序、约定和半角

(w,x,y,z) 还是 (x,y,z,w)？Hamilton 还是 JPL？把 $\theta$ 当成 $w$ 的角度而忘了 $\theta/2$？这些错误都不会让程序崩溃，只会让机器人转错方向、转错角度。接收外部四元数时，先转一个已知的旋转（例如绕 $z$ 转 90°）验证约定。另外，四元数每次连乘后要归一化，否则会慢慢漂离单位球面。

7小练习

1. 绕 $z$ 轴转 90° 的单位四元数 $(w,x,y,z)$ 是？

$w = \cos\tfrac\theta2 = \cos45° = \tfrac{\sqrt2}{2}$，向量部分是 $\sin45°\cdot(0,0,1)$。$(0,0,0,1)$ 是转 180°，最后一个选项忘了半角。

2. 一个机体绕固定轴连续转了 360°。它的四元数是？旋转矩阵是？

$w = \cos180° = -1$，向量部分 $\sin180° = 0$。矩阵 $R(q) = R(-q)$ 是单位阵（双覆盖）。$-I$ 的行列式是 $-1$，根本不是旋转。

3. ZYX 欧拉角在 $\theta = 90°$ 时，$\psi$ 和 $\varphi$ 出现了什么问题？

矩阵仍然正交，朝向也仍然存在，只是这组角度不再唯一（有一整条曲线上的 $(\psi,\varphi)$ 给出同一个朝向）。四元数没有这个问题，但问题出在参数化，不在旋转本身。

4. 起点 $q_0$、终点 $q_1$，$q_0\cdot q_1 = -0.4$。要走最短路径做 slerp，应该？

$q_1$ 与 $-q_1$ 是同一个旋转，但它们到 $q_0$ 的球面距离一个大于 90°，一个小于 90°（对应转角 $2\Omega$ 大于或小于 180°）。点积为负时换成 $-q_1$，路径就是短的那条。两个都取反，点积不变，没有用。

8延伸阅读

Grant Sanderson (3Blue1Brown) & Ben Eater, "Visualizing quaternions" — 可交互地看清四元数与 $S^3$ 的关系
J. Solà, "Quaternion kinematics for the error-state Kalman filter", arXiv:1711.02508 — Hamilton 与 JPL 约定、四元数的各种公式，写得非常清楚
K. Lynch & F. Park, Modern Robotics, Ch. 3.2 — 旋转矩阵、指数坐标与欧拉角
T. Barfoot, State Estimation for Robotics, Ch. 6 — 旋转的各种参数化及其优缺点
K. Shoemake, "Animating rotation with quaternion curves", SIGGRAPH 1985 — slerp 的出处