Part I · 第 5 章刚体运动与李群 · 5.2Part I · Ch. 5 Rigid-Body Motion & Lie Groups · 5.2

旋转的表示Representing Rotations

同一个朝向可以写成 9 个数（旋转矩阵）、3 个角（欧拉角）、一根轴加一个角（轴角），或者 4 个数（四元数）。它们描述的是同一件事，却各有各的长处和陷阱：欧拉角直观，但会"万向节死锁"；四元数紧凑、插值方便，却把每个旋转写了两遍。 One orientation can be written as nine numbers (a rotation matrix), three angles (Euler angles), an axis plus an angle, or four numbers (a quaternion). They describe the same thing but have different strengths and traps: Euler angles are intuitive but suffer gimbal lock; quaternions are compact and interpolate well, yet write every rotation twice.

约 25 分钟~25 min 前置：1.10 正交矩阵与旋转 · 5.1 坐标系与位姿Prereqs: 1.10 Orthogonal matrices & rotations · 5.1 Frames & poses 5 个交互5 interactives

1为什么机器人需要它Why robots need this

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

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

A drone's flight controller, a robot arm's end-effector orientation and a phone's gyroscope all have to store "which way am I facing". Yet software is full of different spellings: roll, pitch and yaw on a joystick are Euler angles; ROS messages carry quaternions; kinematics uses 3×3 rotation matrices; the Rodrigues vector in vision is axis-angle. You convert among them daily, so you need to know when each one helps and when it bites.

A rotation has only three degrees of freedom (Lesson 1.10: nine numbers, six constraints), but no 3-number description is singularity-free and continuous over all rotations. This is a topological fact ($\SO$ is not homeomorphic to any region of 3D Euclidean space). So every representation is a trade-off. This lesson puts four of them side by side so you can feel these trade-offs by hand.

2直觉：同一个朝向，四种写法Intuition: one orientation, four spellings

约定：右手系，$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$。

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

Conventions: right-handed, $z$ up, rotations are active and positive by the right-hand rule (thumb along the axis, fingers curl in the positive sense). The three columns of $R$ are the body's $x, y, z$ axes in world coordinates (Lessons 1.10 and 5.1), so $R$ converts body readings into world readings.

Rotation matrix $R$: nine numbers with $R\T R = I$ and $\det R = 1$. It multiplies vectors and chains directly and has no singularities, but is redundant.
Euler angles $(\psi, \theta, \varphi)$: three angles about $z$ (yaw), $y$ (pitch), then $x$ (roll). The easiest for people to read, but singular.
Axis-angle $(\hat{\mathbf u}, \theta)$: a unit axis and an angle. The most direct geometrically; the vector $\boldsymbol\omega = \theta\hat{\mathbf u}$ of length $\theta$ is the rotation vector of Lesson 5.6.
Unit quaternion $q = (w, x, y, z)$: $w = \cos\tfrac\theta2$ and $(x,y,z) = \sin\tfrac\theta2\,\hat{\mathbf u}$. Four numbers with one constraint $\|q\| = 1$.

In the "Rosetta stone" below, adjust the orientation with any of the three inputs and the other readouts are computed live.

FIG 1罗塞塔石碑：四种表示实时互转Rosetta stone: four representations, live conversions

拖动画面旋转视角drag to orbit

淡色是世界坐标系，亮色是机体（带一个竖直的尾翼，方便看出横滚）。紫色虚线是转轴。欧拉角为 ZYX 顺序，即 $R = R_z(\psi)R_y(\theta)R_x(\varphi)$。拖动任何一行的滑块，另外两行的滑块和右边的读数都会跟着变。四元数那一行会自动归一化。 The faint frame is the world; the bright one is the body (with a vertical tail fin so roll is visible). The dashed purple line is the rotation axis. Euler angles use ZYX order, $R = R_z(\psi)R_y(\theta)R_x(\varphi)$. Drag any row of sliders and the other two rows and the readouts follow. The quaternion row normalises itself automatically.

试一试Try this

用任意一种输入，做出绕 z 轴转 90°。看看另外三种表示各自是什么。（四元数应该是 $(0.71, 0, 0, 0.71)$，为什么是 0.71？）With any input, make a 90° turn about z. See what the other three representations read. (The quaternion should be $(0.71, 0, 0, 0.71)$. Why 0.71?)
做出绕 x 轴转 180°。四元数的 $w$ 是多少？为什么？Make a 180° turn about x. What is the quaternion's $w$, and why?
点四元数那一行的"取负 $-q$"按钮。物体动了吗？旋转矩阵变了吗？Press the "negate $-q$" button on the quaternion row. Did the body move? Did the matrix change?

核心洞见Key insight

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

The four spellings are just different coordinates for the same object. The real object is a point in $\SO$, and it has no "natural" coordinates. Each parametrisation is good in some places and fails in others. The differences are only: is it continuous, does it have singularities, can you chain or interpolate it directly.

3动手玩：万向节死锁Play: gimbal lock

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

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

Euler angles are like three nested gimbal rings: the outermost ring turns about the world $z$ axis (yaw), the middle one about the $y$ axis it carries along (pitch), and the innermost about the $x$ axis it carries (roll), with the body mounted inside. Each ring's axis is dictated by the rings outside it.

When pitch reaches $\pm 90°$, the innermost axis $x$ is folded onto the outermost axis $z$ (the red arrow lies on the blue one). Now turning yaw or roll makes the body do the same motion: one degree of freedom is gone. This is gimbal lock. Near it, the Euler angles must change extremely fast to produce an ordinary angular velocity.

FIG 2万向节与死锁Gimbals and lock

拖动画面旋转视角drag to orbit

蓝环 = 偏航，绿环 = 俯仰，红环 = 横滚。三根彩色箭头是三个环各自的转轴（蓝 $\hat z$、绿 $\hat y'$、红 $\hat x''$）。右边的 $\det J$ 是"欧拉角速度 $\to$ 世界角速度"的 3×3 矩阵的行列式，它等于 $-\cos\theta$：到 $\pm 90°$ 变成 0，矩阵不可逆。"微调测试"把偏航和横滚各加 10°（俯仰为负时横滚取反），看物体净转了多少度。 Blue ring = yaw, green = pitch, red = roll. The three coloured arrows are the rings' axes (blue $\hat z$, green $\hat y'$, red $\hat x''$). The $\det J$ on the right is the determinant of the 3×3 matrix that maps Euler rates to world angular velocity; it equals $-\cos\theta$ and vanishes at $\pm 90°$, where the matrix is singular. The "nudge test" adds 10° to both yaw and roll (roll is negated when pitch is negative) and reports the net rotation the body undergoes.

试一试Try this

把俯仰调到 $\pm 90°$（点预设），看红色转轴和蓝色转轴怎样重合。Take pitch to $\pm 90°$ (use the preset) and watch the red axis fall onto the blue one.
在俯仰 $90°$ 处点"记下当前朝向"，再同时改变偏航和横滚（保持 $\psi - \varphi$ 不变），让它们都和记下时相差 20° 以上，而朝向差保持在 1° 以内。两组完全不同的欧拉角，同一个朝向。At pitch $90°$ press "snapshot", then change yaw and roll together (keeping $\psi - \varphi$ fixed) until both differ from the snapshot by over 20° while the orientation difference stays under 1°. Two very different sets of Euler angles, one orientation.
先在俯仰 $0°$ 附近读一下"微调测试"的净转角（约 14°），再把俯仰设到约 $88°$ 再读一次（只剩约 0.35°）：在死锁附近，偏航和横滚各改 10° 几乎完全抵消，欧拉角的大变化只对应很小的转动，数值上很不稳。First read the nudge test's net rotation with pitch near $0°$ (about 14°), then set pitch to about $88°$ and read it again (only about 0.35°): near the lock, 10° changes in yaw and roll almost cancel, so large Euler changes map to tiny rotations, which is numerically fragile.

另一个麻烦：双覆盖Another wrinkle: the double cover

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

Quaternions have no singularity, at the price of two quaternions per rotation: $q$ and $-q$ are the same rotation. The reason is the half-angle: $q = (\cos\tfrac\theta2, \sin\tfrac\theta2\,\hat{\mathbf u})$, so when $\theta$ goes once around (360°), $q$ only goes half way and becomes $-q$; it takes 720° to come back to $q$. Drag $\theta$ to 360° with the slider below: the body is back where it started, but the point on the circle is at $(-1, 0)$.

FIG 3双覆盖：转 720° 才回到同一个四元数Double cover: 720° to return to the same quaternion

机体：绕固定轴转 θbody: turned by θ about a fixed axis

四元数 q 在单位圆上q on the unit circle

右图：横轴是 $w = \cos\tfrac\theta2$，纵轴是 $\sin\tfrac\theta2$（四元数的向量部分沿 $\hat{\mathbf u}$ 的分量，可正可负）。点随 $\theta$ 在单位圆上以一半的角速度转动。左图的机体转 360° 就回到原样，右图的点却只走了半圈。 Right: the horizontal axis is $w = \cos\tfrac\theta2$ and the vertical axis is $\sin\tfrac\theta2$ (the vector part of the quaternion along $\hat{\mathbf u}$, signed). The point moves around the unit circle at half the angular rate of $\theta$. The body on the left is back home after 360°, while the point has only travelled halfway.

试一试Try this

把 $\theta$ 设到 360°：机体回到原样，$q$ 却是 $(-1, 0, 0, 0)$。Set $\theta$ to 360°: the body is back, yet $q$ is $(-1, 0, 0, 0)$.
再继续到 720°，$q$ 才回到 $(1, 0, 0, 0)$。Continue to 720° and $q$ finally returns to $(1, 0, 0, 0)$.

插值：欧拉角、slerp、nlerpInterpolation: Euler lerp, slerp, nlerp

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

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

To turn smoothly from one orientation to another, each representation gives a different path. Euler-angle lerp: each angle changes uniformly; the path may detour and the angular speed varies. slerp (spherical linear interpolation) moves along the great-circle arc on the quaternion sphere at constant speed, equivalent to turning about a single fixed axis at constant rate: the shortest and most uniform path. nlerp: linearly interpolate the quaternions and then normalise; the path is the same as slerp's, only the speed is slightly uneven, but it is cheap.

Note that "shortest" depends on the sign: if $q_0 \cdot q_1 < 0$, replace $q_1$ by $-q_1$, or you will take a long way of more than 180°.

FIG 4三种插值，同一对起点和终点Three interpolations, one start and one goal

拖动画面旋转视角drag to orbit

角速度（相对 slerp 的均匀速度）angular speed (vs. slerp's uniform speed)

从左到右（沿 $y$ 轴排开）：紫 = 欧拉角线性插值，琥珀 = slerp，青 = nlerp。每个物体的机头轨迹用同色线画出（粗线是已走过的部分），灰点是起点机头，绿点是终点机头。右图：三种方法的角速度随 $s$ 的变化；slerp 是一条水平线。"最短路径"关掉后，当 $q_0\cdot q_1<0$，slerp 和 nlerp 会走长的那条路。 From left to right (laid out along the $y$ axis): purple = Euler-angle lerp, amber = slerp, cyan = nlerp. Each body's nose path is drawn in its colour (thick for the part already travelled); the grey dot is the start nose and the green dot the goal nose. Right: the angular speed of each method vs. $s$; slerp is a flat line. With "shortest path" off, when $q_0\cdot q_1<0$ slerp and nlerp take the long way round.

试一试Try this

用滑块或预设，让欧拉角插值的峰值角速度超过 slerp 的 2 倍。With the sliders or a preset, make the Euler lerp's peak angular speed exceed twice slerp's.
关掉"最短路径"，找一对起点终点，使 slerp 实际转过的总角度超过 180°。再打开，它就变回 ≤180°。Turn "shortest path" off and find a start/goal pair for which slerp rotates through more than 180° in total. Turn it back on and it drops to at most 180°.

4再看数学The math

提示：把鼠标悬停在公式中带底色的符号上，FIG 3 中对应的量会被高亮。Tip: hover the highlighted symbols in the formulas; the matching quantity in FIG 3 lights up.

欧拉角（ZYX，偏航-俯仰-横滚）Euler angles (ZYX: yaw-pitch-roll)

$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 = R_z(\psi)\,R_y(\theta)\,R_x(\varphi)$. Read as intrinsic rotations: yaw by $\psi$ about $z$, then pitch by $\theta$ about the new $y'$, then roll by $\varphi$ about the new $x''$, exactly the gimbals of FIG 2 (the equivalent extrinsic reading is roll by $\varphi$ about the world $x$, then pitch by $\theta$ about the world $y$, then yaw by $\psi$ about the world $z$, the opposite order). Expanded,

$$ 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……），"内旋 / 外旋"再翻一倍，所以脱离约定谈欧拉角没有意义。

At $\theta = \pm 90°$, $c_\theta = 0$: the first column and last row become constants and every other entry depends only on $\varphi - \psi$ (for $\theta = +90°$) or $\varphi + \psi$ (for $\theta = -90°$); $\psi$ and $\varphi$ cannot be separated. That is the lock, and the extraction formulas above fail there ($R_{11} = R_{21} = 0$), so code must special-case it. There are 12 axis orders (ZYX, ZYZ, XYZ, …) and intrinsic/extrinsic doubles it again, so Euler angles mean nothing without their convention.

欧拉角速度和角速度的关系也在 $\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）。

The relation between Euler rates and angular velocity degenerates at $\theta = \pm 90°$ too: $\boldsymbol\omega_{\text{world}} = \dot\psi\,\hat z + \dot\theta\,\hat y' + \dot\varphi\,\hat x''$, and the matrix $J$ built from the three axes $\hat z$, $\hat y'$, $\hat x''$ has $\det J = -\cos\theta$ (FIG 2).

轴角与 Rodrigues 公式Axis-angle and the Rodrigues formula

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

An axis $\hat{\mathbf u}$ and an angle $\theta$ give the rotation matrix (Lesson 5.6 derives it from the exponential map):

$$ 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}$ 都对。

Here $\skew{\hat{\mathbf u}}$ is the cross-product matrix of Lesson 1.8. Axis-angle is not unique either: $(\hat{\mathbf u}, \theta)$ equals $(-\hat{\mathbf u}, -\theta)$, $\theta \to \theta + 2\pi$ gives the same rotation, the axis is undefined at $\theta = 0$, and at $\theta = \pi$ both $\pm\hat{\mathbf u}$ work.

单位四元数Unit quaternions

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

A quaternion is $q = w + x\,\mathbf i + y\,\mathbf j + z\,\mathbf k$, written $q = (w, \mathbf v)$. This course uses the Hamilton convention with $(w,x,y,z)$ ordering. The product is

$$ 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$ 与旋转的对应是

A unit quaternion $\|q\| = 1$ corresponds to a rotation by

$$ 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)$。矩阵形式为

Why the half angle: the map $\mathbf p \mapsto q\mathbf p q^*$ applies $q$ once on each side, and each side contributes half of the turn. So $q$ and $-q$ give the same map (the two minus signs cancel): the double cover. The unit quaternions form the 3-sphere $S^3$, which covers $\SO$ two-to-one. Composition matches: $R(q_1\otimes q_2) = R(q_1)R(q_2)$. In matrix form,

$$ 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$。

After each multiplication, dividing $q$ by $\|q\|$ corrects numerical drift (much cheaper than re-orthonormalising $R$). Slerp is $q(s) = \dfrac{\sin((1-s)\Omega)}{\sin\Omega}\,q_0 + \dfrac{\sin(s\Omega)}{\sin\Omega}\,q_1$, with $\cos\Omega = q_0\cdot q_1$.

放在一起比较Side by side

	矩阵Matrix	欧拉角Euler	轴角Axis-angle	四元数Quaternion
数字个数Numbers	9 (−6)	3	3 or 4	4 (−1)
奇点Singularity	无none	有（死锁）yes (gimbal lock)	$\theta=0$ 轴不定axis undefined at $\theta=0$	无，但 $q\equiv -q$none, but $q\equiv -q$
连乘 / 作用于向量Compose / act on vectors	直接direct	要转成矩阵via matrix	要转成矩阵via matrix	较直接fairly direct
插值Interpolation	不能直接插not directly	不均匀uneven	好（5.6 节）good (Lesson 5.6)	slerp / nlerpslerp / nlerp
人读起来Human-readable	一般so-so	最好best	好good	差poor

5机器人中的它：姿态控制与"取最短路径"In robotics: attitude control and "take the short way"

四旋翼的姿态控制器每个周期都做同一件事：算出当前姿态和目标姿态之间的误差旋转，再用它去驱动机体转向目标。用四元数写：$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$）。分别开关"符号修正"，看机体怎么转。

A quadrotor's attitude controller does the same thing every cycle: compute the error rotation between the current and the desired attitude and use it to drive the airframe toward the target. With quaternions: $q_e = q_{\text{cur}}^{*}\otimes q_{\text{des}}$ (the error in the body frame) and the commanded rate is $\boldsymbol\omega_{\text{cmd}} = 2k\,\mathbf v_e$, where $\mathbf v_e$ is the vector part of $q_e$ (for small angles $2\mathbf v_e \approx$ the error rotation vector).

The trap is the double cover: $q_{\text{des}}$ and $-q_{\text{des}}$ are the same attitude, but they give opposite $\mathbf v_e$. If $w_e < 0$, $q_e$ sits on the "far hemisphere" and the controller turns most of the way round in the wrong direction. Flight stacks such as PX4 handle it by replacing $q_e$ with $-q_e$ whenever $w_e < 0$, which guarantees a turn of at most 180°.

Below, the airframe starts at the identity and the target is a 270° turn about $z$ ($q_{\text{des}} = (-0.71, 0, 0, 0.71)$, $w<0$). Toggle the sign fix and watch how it turns.

FIG 5姿态误差控制：有无符号修正Attitude error control: with and without the sign fix

拖动画面旋转视角drag to orbit

淡淡的绿色机体是目标姿态，青色机体是当前姿态，琥珀色曲线是机头走过的轨迹。控制律：$\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$。 The faint green body is the target attitude, the cyan one the current attitude, and the amber curve is the path of the nose. Control law: $\boldsymbol\omega = 2k\,\sigma\,\mathbf v_e$ with $\sigma = \operatorname{sign}(w_e)$ (fix on) or always 1 (fix off); integration is $q \leftarrow q\otimes\Exp(\boldsymbol\omega\,dt)$ (the rotation $\Exp(\boldsymbol\omega\,dt)$ written as a quaternion). "Total turned" is $\int\|\boldsymbol\omega\|dt$.

试一试Try this

关闭符号修正并运行：机体沿正方向转了 270°，走了大半圈。Turn the sign fix off and run: the body turns 270° the positive way, most of a full circle.
开启符号修正再运行：机体反方向只转 90° 就到了。Turn the sign fix on and run again: the body reaches the target with just 90° the other way.

真实系统里In real systems

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 向量就是轴角。

ROS's geometry_msgs/Quaternion orders its fields (x, y, z, w), while Eigen's constructor Quaternion(w, x, y, z) takes w first (and its internal coeffs() is x, y, z, w again). Hamilton vs. JPL quaternion conventions differ (product order and even the rotation direction may flip); ROS, Eigen and Ceres use Hamilton, while much aerospace and early VIO literature uses JPL. UIs and RC sticks use Euler angles; kinematics and point-cloud transforms use matrices; state estimators keep the state as a quaternion or matrix and the increments as axis-angle (rotation vectors) (Lessons 5.6 and 5.7); the Rodrigues vector output by camera calibration is axis-angle.

6常见误区Common pitfalls

误区 1：谈欧拉角不说约定Pitfall 1: Euler angles without a convention

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

"roll = 30°, pitch = 20°, yaw = 10°" alone is not a rotation: is the order ZYX or XYZ? About body axes or world axes? Degrees or radians? Different answers give different matrices. Spell it out in the interface docs and in variable names (e.g. euler_zyx_rad), and do not use Euler angles for internal state, interpolation or differentiation.

误区 2：以为死锁只在正好 90° 才发生Pitfall 2: thinking gimbal lock only happens at exactly 90°

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

A few degrees away from $\pm 90°$, $\psi$ and $\varphi$ are mathematically separable but numerically ill-conditioned: a hair of noise in $\psi$ is cancelled by a large opposite change in $\varphi$. A filter that keeps Euler angles as its state will show huge, physically meaningless jumps there (the third task of FIG 2).

误区 3：把四元数当普通 4 维向量比较、平均Pitfall 3: comparing or averaging quaternions as plain 4-vectors

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

$q$ and $-q$ are the same rotation, yet $\|q - (-q)\| = 2$ is the maximum. To compare two attitudes look at $|q_1\cdot q_2|$ (the cosine of the half angle); before averaging a set of quaternions, first move them onto the same hemisphere ($q_i\cdot q_1 \ge 0$), or the mean can be near zero. A principled average lives on the manifold (Lesson 5.9).

误区 4：四元数的顺序、约定和半角Pitfall 4: quaternion order, convention and the half angle

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

(w,x,y,z) or (x,y,z,w)? Hamilton or JPL? Treating $\theta$ as $w$'s angle and forgetting $\theta/2$? None of these crash the program; they just make the robot turn the wrong way or by the wrong amount. When you receive an outside quaternion, feed it a known rotation (say 90° about $z$) to verify the convention. And renormalise after each product, or the quaternion slowly drifts off the unit sphere.

7小练习Check yourself

1. 绕 $z$ 轴转 90° 的单位四元数 $(w,x,y,z)$ 是？1. What is the unit quaternion $(w,x,y,z)$ of a 90° turn about $z$?

$w = \cos\tfrac\theta2 = \cos45° = \tfrac{\sqrt2}{2}$，向量部分是 $\sin45°\cdot(0,0,1)$。$(0,0,0,1)$ 是转 180°，最后一个选项忘了半角。$w = \cos\tfrac\theta2 = \cos45° = \tfrac{\sqrt2}{2}$ and the vector part is $\sin45°\cdot(0,0,1)$. $(0,0,0,1)$ is a 180° turn, and the last option forgets the half angle.

2. 一个机体绕固定轴连续转了 360°。它的四元数是？旋转矩阵是？2. A body turns 360° about a fixed axis. What are its quaternion and rotation matrix?

$w = \cos180° = -1$，向量部分 $\sin180° = 0$。矩阵 $R(q) = R(-q)$ 是单位阵（双覆盖）。$-I$ 的行列式是 $-1$，根本不是旋转。$w = \cos180° = -1$ and the vector part $\sin180° = 0$. The matrix $R(q) = R(-q)$ is the identity (double cover). $-I$ has determinant $-1$ and is not a rotation at all.

3. ZYX 欧拉角在 $\theta = 90°$ 时，$\psi$ 和 $\varphi$ 出现了什么问题？3. For ZYX Euler angles at $\theta = 90°$, what goes wrong with $\psi$ and $\varphi$?

矩阵仍然正交，朝向也仍然存在，只是这组角度不再唯一（有一整条曲线上的 $(\psi,\varphi)$ 给出同一个朝向）。四元数没有这个问题，但问题出在参数化，不在旋转本身。The matrix is still orthogonal and the orientation still exists; it is this parametrisation that stops being unique (a whole curve of $(\psi,\varphi)$ gives the same orientation). Quaternions avoid it, but the problem is in the parametrisation, not in the rotation.

4. 起点 $q_0$、终点 $q_1$，$q_0\cdot q_1 = -0.4$。要走最短路径做 slerp，应该？4. Start $q_0$, goal $q_1$, $q_0\cdot q_1 = -0.4$. To slerp along the shortest path you should

$q_1$ 与 $-q_1$ 是同一个旋转，但它们到 $q_0$ 的球面距离一个大于 90°，一个小于 90°（对应转角 $2\Omega$ 大于或小于 180°）。点积为负时换成 $-q_1$，路径就是短的那条。两个都取反，点积不变，没有用。$q_1$ and $-q_1$ are the same rotation, but one is more than 90° from $q_0$ on the sphere and the other less (the turn $2\Omega$ being over or under 180°). When the dot product is negative, using $-q_1$ picks the short path. Negating both leaves the dot product unchanged and does nothing.

8延伸阅读Further reading

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