正交矩阵与旋转Orthogonal Matrices & Rotations

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

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

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

A quadrotor's flight controller reads the gyroscope (part of the IMU) hundreds to thousands of times a second. Every time, it multiplies "the small turn since the last reading" into the current orientation: \( R \leftarrow R\,\Delta R \). Every sensor on a robot works the same way: how the camera sits relative to the body, how the lidar sits relative to the body, each is stored as a rotation matrix.

Rotation matrices have two properties ordinary matrices lack. First, they only move things, they never stretch them: turn a cup around and its size and shape stay the same. Second, the promise Lesson 1.3 made: the inverse is the transpose, \( R^{-1} = R^\top \). Going from "camera frame to world frame" to "world frame to camera frame" needs no inversion, just flip the matrix: R_cam_world = R_world_cam.T.

The price is that rotation matrices form a very picky set. Of nine numbers only three are free; the other six are pinned down by six conditions ("each column has length 1, every pair of columns is perpendicular"). Every update adds a bit of rounding error, and after a few hundred of them the matrix has quietly stopped being a rotation. It starts to stretch and shear, and the program raises no error. This lesson shows exactly where the pickiness comes from, and how to repair it.

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

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

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

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

Pin a sheet of graph paper at the origin and turn it. The squares are still squares, the sides have the same length, the right angles are still right angles, and the distance between any two points on the sheet is unchanged. This is a rigid motion (here only about the origin).

In the language of Lesson 1.3: the shape of the grid is fully determined by where î and ĵ land (the two columns of the matrix). For the squares to survive, those two landing spots must satisfy two conditions:

• each column still has length 1 (the sides did not change);
• the two columns are still perpendicular (the right angles did not change).

Columns that satisfy both are called orthonormal, and a square matrix with orthonormal columns is an orthogonal matrix. Switch between four transformations below. The F is the shape being transformed (its color shows the class) and the circle is the unit circle. Only maps that keep "the circle a circle and the F undistorted" are rigid. A mirror does not stretch either, but it turns the F into a mirror-image F; that is a separate class we will sort out shortly.

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

\(Q^\top Q\) is a table of dot products between columns (Lesson 1.7): entry (i, j) is \(\mathbf q_i\cdot\mathbf q_j\). Orthonormal columns mean: \(1\) on the diagonal (a column dotted with itself is its squared length) and \(0\) off the diagonal (different columns are perpendicular). So the table is the identity:

换个角度理解这件事。\(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\)。点击"播放"，或者拖动阶段滑块。

Here is another way to see it. The \(i\)-th row of \(Q^\top\) is \(\mathbf q_i\), so the \(i\)-th entry of \(Q^\top\mathbf x\) is \(\mathbf q_i\cdot\mathbf x\), the projection length of \(\mathbf x\) along \(\mathbf q_i\) (Lesson 1.9). Conversely, \(Q\) uses those numbers as a recipe: \(Q\mathbf c = c_1\mathbf q_1 + c_2\mathbf q_2\). So \(Q^\top\) "reads off coordinates" and \(Q\) "rebuilds the vector from coordinates"; read, then rebuild, and the vector is back. This only works when the \(\mathbf q_i\) are perpendicular and of length 1; otherwise \(\mathbf q_i\cdot\mathbf x\) is not a clean coordinate.

Below, try it on a rotation, a mirror and a non-orthogonal matrix: apply \(Q\), then apply \(Q^\top\). Press play, or drag the stage slider.

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

The columns of a rotation matrix \(R\) are the new axes written in old coordinates (the passive view of Lesson 1.3). Because \(R^{-1} = R^\top\), its rows have an equally clear meaning: row \(i\) is the old \(i\)-th axis written in new coordinates. Drag the red arrow tip to turn the new frame, and watch the amber old axis drop perpendiculars onto the new axes.

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

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

A 3D \(R\) has three columns: the three axes x′, y′, z′ of the body frame, written in world coordinates. Of nine numbers only three are free: three angles (yaw, pitch, roll, the Euler angles) generate it. Here we use the yaw → pitch → roll (ZYX) order; the many troubles with Euler angles (gimbal lock, competing conventions) are for Lesson 5.2.

Drag the sliders and watch \(R\), \(R^\top R\) and the determinant: however you turn, \(R^\top R\) is the identity and the determinant is \(+1\). Then switch on "mirror" to negate one axis: \(R^\top R\) is still \(I\), but the determinant becomes \(-1\) and the frame turns left-handed. It is still an "orthogonal matrix", but no longer a rotation.

\(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。

The minus sign in \(R_y\) sits in the lower left, not the upper right: this keeps the frame right-handed (the "positive" sense is \(z\to x\)). Products of 3D rotations are rotations: \((AB)^\top(AB) = B^\top A^\top A B = B^\top B = I\), and the determinants multiply to \(+1\). So \(SO(3)\) is closed under multiplication, has the identity \(I\), and every element has an inverse (its transpose): that is a group (Lesson 5.4).

But 3D rotations do not commute. Take two 90° turns: \(R_x(90^\circ)R_z(90^\circ) = \begin{bmatrix}0&-1&0\\0&0&-1\\1&0&0\end{bmatrix}\), whereas \(R_z(90^\circ)R_x(90^\circ) = \begin{bmatrix}0&0&1\\1&0&0\\0&1&0\end{bmatrix}\). They differ (Lesson 1.4: order matters).

One last preview: every 3D rotation has an axis, a direction it leaves exactly in place, i.e. an eigenvector with eigenvalue 1 (Lesson 1.11). The reason in a line: in odd dimension the determinant is 1 and complex eigenvalues come in conjugate pairs, so one real eigenvalue must equal 1.

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

A gyroscope measures the body's angular velocity \(\boldsymbol\omega\). Every \(\Delta t\) seconds the orientation must be updated. The exact update is \(R \leftarrow R\,\Exp(\boldsymbol\omega\Delta t)\) (Lesson 5.6), which keeps \(R\) exactly inside the rotations. But for convenience people often use the first-order approximation:

其中 \(\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\|\)（对数坐标）随步数的变化。当偏差越过红色虚线，就应当修复。

where \(\Omega = \skew{\boldsymbol\omega\Delta t}\) is the skew-symmetric matrix of Lesson 1.8 and \(\mathbf u\) the rotation axis. This approximate matrix is not a rotation: each step stretches the directions perpendicular to the axis by about \(\sqrt{1+\|\boldsymbol\omega\Delta t\|^2}\). A single step is off by a few parts in ten thousand; a few hundred steps add up to visible deformation. Another common source of error is rounding: store \(R\) in low precision (or fixed point) and every step injects a tiny error that accumulates like a random walk.

The experiment below simulates this: the body turns at a constant rate and \(R\) is updated every step (100 Hz). On the left are the three columns of \(R\) (thick arrows) and the image of the unit sphere (wireframe), with the ideal orientation as thin arrows; on the right is \(\|R^\top R - I\|\) (log scale) against step count. When the error crosses the red dashed line, it is time to repair.