旋转齐次坐标变换#

1 什么是旋转齐次坐标变换#

旋转齐次坐标变换是用 4×4 齐次矩阵表示 3 维空间旋转操作的工具。其核心是将 3 维直角坐标中的“旋转”融入 4 维齐次空间，保持变换形式为线性的矩阵乘法，从而与平移变换统一，方便组合运算。

1.1 核心特征与数学表达#

  1. 变换对象：针对 3 维点或方向向量的齐次坐标。

    • 点的齐次坐标：\( \widetilde{\mathbf{p}} = \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} \)，末尾为 1，会受平移影响。

    • 方向向量的齐次坐标：\( \widetilde{\mathbf{v}} = \begin{bmatrix} v_x \\ v_y \\ v_z \\ 0 \end{bmatrix} \)，末尾为 0，仅受旋转影响，不受平移影响。

  2. 核心矩阵：旋转齐次矩阵的左上角 3×3 为 3 维旋转矩阵（决定旋转轴和角度），右上角 3×1 为平移分量（纯旋转时为 0），最后一行为固定的 [0,0,0,1]。

    • 绕 x 轴旋转（固定轴，右手定则）：\( T_x(\theta) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta & 0 \\ 0 & \sin\theta & \cos\theta & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

    • 绕 y 轴旋转（固定轴，右手定则）：\( T_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta & 0 \\ 0 & 1 & 0 & 0 \\ -\sin\theta & 0 & \cos\theta & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

    • 绕 z 轴旋转（固定轴，右手定则）：\( T_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 & 0 \\ \sin\theta & \cos\theta & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

  3. 变换过程：通过矩阵乘法完成，即 \( \widetilde{\mathbf{p}}' = T \cdot \widetilde{\mathbf{p}} \)，展开后提取前 3 个分量，即为 3 维直角坐标的旋转结果。

2 旋转齐次坐标变换具体数值案例#

以“3 维点 \( \mathbf{p} = (1, 0, 0) \) 绕 z 轴旋转 \( \theta=90^\circ \)”为例（右手定则：四指沿旋转方向，拇指指向 z 轴正方向）。

2.1 步骤 1：转换为齐次坐标#

点的齐次坐标末尾固定为 1，因此：\( \widetilde{\mathbf{p}} = \begin{bmatrix} 1 \\ 0 \\ 0 \\ 1 \end{bmatrix} \)

2.2 步骤 2：构建旋转齐次矩阵#

代入 \( \theta=90^\circ \) 的三角函数值：\( \cos90^\circ = 0 \)，\( \sin90^\circ = 1 \)。
根据绕 z 轴旋转矩阵公式，得：\( T_z(90^\circ) = \begin{bmatrix} 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

2.3 步骤 3：执行齐次变换（矩阵乘法）#

按“4×4 矩阵 × 4×1 向量”规则计算：
\( \widetilde{\mathbf{p}}' = T_z(90^\circ) \cdot \widetilde{\mathbf{p}} = \begin{bmatrix} 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} 1 \\ 0 \\ 0 \\ 1 \end{bmatrix} \)

  • 第 1 行：\( 0×1 + (-1)×0 + 0×0 + 0×1 = 0 \)

  • 第 2 行：\( 1×1 + 0×0 + 0×0 + 0×1 = 1 \)

  • 第 3 行：\( 0×1 + 0×0 + 1×0 + 0×1 = 0 \)

  • 第 4 行：\( 0×1 + 0×0 + 0×0 + 1×1 = 1 \)
    最终得到：\( \widetilde{\mathbf{p}}' = \begin{bmatrix} 0 \\ 1 \\ 0 \\ 1 \end{bmatrix} \)

2.4 步骤 4：转回直角坐标#

忽略齐次坐标末尾的 1，提取前 3 个分量，得旋转后的直角坐标：\( \mathbf{p}' = (0, 1, 0) \)

2.5 结果验证#

原始点 \( (1,0,0) \) 是 x 轴正方向上的点，绕 z 轴旋转 \( 90^\circ \) 后应指向 y 轴正方向，结果 \( (0,1,0) \) 符合几何直觉，变换正确。

3 绕非坐标轴（以 \( x=y=z \) 方向为例）的旋转齐次坐标变换案例#

问题：将三维点 \( \mathbf{p} = (1, 0, 0) \) 绕过原点、方向向量为 \( \mathbf{n}=(1,1,1) \) 的直线旋转 \( \theta = 90^\circ \)。

3.1 核心原理：绕任意轴旋转的 “坐标对齐法”#

当旋转轴不过坐标轴时，通过一系列变换将其与某个坐标轴（通常是 z 轴）对齐，执行简单的绕坐标轴旋转，再变换回去。本例旋转轴过原点，故无需平移步骤。具体分为 4 步：

  1. 姿态调整 1 (\( T_1 \))：绕 z 轴旋转，将旋转轴 \( \mathbf{n} \) 投影到 x-z 平面。

  2. 姿态调整 2 (\( T_2 \))：绕 y 轴旋转，将已位于 x-z 平面的旋转轴与 z 轴对齐。

  3. 目标旋转 (\( T_{\text{旋转}} \))：绕对齐后的 z 轴旋转 \( 90^\circ \)。

  4. 姿态还原 (\( T_{\text{还原}} \))：反向执行两次姿态调整，恢复旋转轴原方向。由于旋转矩阵是正交矩阵，其逆矩阵等于其转置矩阵，故 \( T_{\text{还原}} = (T_2 T_1)^\top = T_1^\top T_2^\top \)。

最终，总的齐次变换矩阵为：

\( T_{\text{总}} = T_{\text{还原}} \cdot T_{\text{还原}} \cdot T_{\text{调整}} = (T_1^\top T_2^\top) \cdot T_{\text{旋转}} \cdot (T_2 T_1) \)

3.2 具体计算步骤#

3.2.1 步骤 1：转换为齐次坐标#

点 \( \mathbf{p} = (1, 0, 0) \) 的齐次坐标为：

\( \widetilde{\mathbf{p}} = \begin{bmatrix} 1 \\ 0 \\ 0 \\ 1 \end{bmatrix} \)

3.2.2 步骤 2：构建姿态调整矩阵 \( T_{\text{调整}} = T_2 T_1 \)#

a) 计算旋转轴单位向量

旋转轴方向向量 \( \mathbf{n} = (1,1,1) \)，其模长为 \( |\mathbf{n}| = \sqrt{1^2 + 1^2 + 1^2} = \sqrt{3} \)。单位化后得到：

\( \mathbf{u} = \frac{\mathbf{n}}{|\mathbf{n}|} = \left( \frac{1}{\sqrt{3}}, \frac{1}{\sqrt{3}}, \frac{1}{\sqrt{3}} \right) \)

b) 第一次姿态调整矩阵 \( T_1 \) (绕 z 轴旋转)

目标：将单位向量 \( \mathbf{u} \) 投影到 xOy 平面，其投影为 \( (1/\sqrt{3}, 1/\sqrt{3}, 0) \)。需要绕 z 轴旋转的角度 \( \phi \)，满足 \( \tan \phi = \frac{u_y}{u_x} = 1 \)，故 \( \phi = 45^\circ \)。为了将投影与 x 轴对齐，需旋转 \( -45^\circ \)（顺时针 45°）。

\( \cos(-45^\circ) = \frac{\sqrt{2}}{2}, \quad \sin(-45^\circ) = -\frac{\sqrt{2}}{2} \)

绕 z 轴的旋转矩阵为：

\( T_1 = \begin{bmatrix} \cos(-45^\circ) & -\sin(-45^\circ) & 0 & 0 \\ \sin(-45^\circ) & \cos(-45^\circ) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & 0 & 0 \\ -\frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

c) 第二次姿态调整矩阵 \( T_2 \) (绕 y 轴旋转)

经过 \( T_1 \) 变换后，旋转轴单位向量 \( \mathbf{u} \) 变为 \( \mathbf{u}' = T_1 \mathbf{u} \)。由于只关心与 y 轴的夹角，可直接计算 \( \mathbf{u} \) 与 z 轴的夹角 \( \alpha \)。单位向量 \( \mathbf{u} \) 与 z 轴单位向量 \( \mathbf{e}_z = (0,0,1) \) 的点积为：

\( \mathbf{u} \cdot \mathbf{e}_z = \frac{1}{\sqrt{3}} \)

根据点积公式 \( \mathbf{u} \cdot \mathbf{e}_z = |\mathbf{u}| |\mathbf{e}_z| \cos\alpha = \cos\alpha \)，故 \( \cos\alpha = \frac{1}{\sqrt{3}} \)。需要将 \( \mathbf{u} \) 旋转到与 z 轴重合，因此绕 y 轴旋转 \( -\alpha \)（根据右手定则，仰角为负）。

\( \cos(-\alpha) = \cos\alpha = \frac{1}{\sqrt{3}}, \quad \sin(-\alpha) = -\sin\alpha = -\sqrt{1 - \cos^2\alpha} = -\sqrt{1 - \frac{1}{3}} = -\frac{\sqrt{2}}{\sqrt{3}} \)

绕 y 轴的旋转矩阵为：

\( T_2 = \begin{bmatrix} \cos(-\alpha) & 0 & \sin(-\alpha) & 0 \\ 0 & 1 & 0 & 0 \\ -\sin(-\alpha) & 0 & \cos(-\alpha) & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} \frac{1}{\sqrt{3}} & 0 & -\frac{\sqrt{2}}{\sqrt{3}} & 0 \\ 0 & 1 & 0 & 0 \\ \frac{\sqrt{2}}{\sqrt{3}} & 0 & \frac{1}{\sqrt{3}} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

d) 计算组合姿态调整矩阵 \( T_{\text{调整}} = T_2 T_1 \)

\( T_{\text{调整}} = \begin{bmatrix} \frac{1}{\sqrt{3}} & 0 & -\frac{\sqrt{2}}{\sqrt{3}} & 0 \\ 0 & 1 & 0 & 0 \\ \frac{\sqrt{2}}{\sqrt{3}} & 0 & \frac{1}{\sqrt{3}} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & 0 & 0 \\ -\frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

逐元素计算左上角 3x3 部分（旋转部分）：

  • 第一行:

    • \( \left(\frac{1}{\sqrt{3}} \cdot \frac{\sqrt{2}}{2} + 0 \cdot \left(-\frac{\sqrt{2}}{2}\right) + \left(-\frac{\sqrt{2}}{\sqrt{3}}\right) \cdot 0\right) = \frac{1}{\sqrt{6}} \)

    • \( \left(\frac{1}{\sqrt{3}} \cdot \frac{\sqrt{2}}{2} + 0 \cdot \frac{\sqrt{2}}{2} + \left(-\frac{\sqrt{2}}{\sqrt{3}}\right) \cdot 0\right) = \frac{1}{\sqrt{6}} \)

    • \( \left(\frac{1}{\sqrt{3}} \cdot 0 + 0 \cdot 0 + \left(-\frac{\sqrt{2}}{\sqrt{3}}\right) \cdot 1\right) = -\frac{\sqrt{2}}{\sqrt{3}} \)

  • 第二行:

    • \( \left(0 \cdot \frac{\sqrt{2}}{2} + 1 \cdot \left(-\frac{\sqrt{2}}{2}\right) + 0 \cdot 0\right) = -\frac{\sqrt{2}}{2} \)

    • \( \left(0 \cdot \frac{\sqrt{2}}{2} + 1 \cdot \frac{\sqrt{2}}{2} + 0 \cdot 0\right) = \frac{\sqrt{2}}{2} \)

    • \( \left(0 \cdot 0 + 1 \cdot 0 + 0 \cdot 1\right) = 0 \)

  • 第三行:

    • \( \left(\frac{\sqrt{2}}{\sqrt{3}} \cdot \frac{\sqrt{2}}{2} + 0 \cdot \left(-\frac{\sqrt{2}}{2}\right) + \frac{1}{\sqrt{3}} \cdot 0\right) = \frac{2}{2\sqrt{3}} = \frac{1}{\sqrt{3}} \)

    • \( \left(\frac{\sqrt{2}}{\sqrt{3}} \cdot \frac{\sqrt{2}}{2} + 0 \cdot \frac{\sqrt{2}}{2} + \frac{1}{\sqrt{3}} \cdot 0\right) = \frac{1}{\sqrt{3}} \)

    • \( \left(\frac{\sqrt{2}}{\sqrt{3}} \cdot 0 + 0 \cdot 0 + \frac{1}{\sqrt{3}} \cdot 1\right) = \frac{1}{\sqrt{3}} \)

所以，姿态调整矩阵为：

\( T_{\text{调整}} = \begin{bmatrix} \frac{1}{\sqrt{6}} & \frac{1}{\sqrt{6}} & -\frac{\sqrt{2}}{\sqrt{3}} & 0 \\ -\frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & 0 & 0 \\ \frac{1}{\sqrt{3}} & \frac{1}{\sqrt{3}} & \frac{1}{\sqrt{3}} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

3.2.3 步骤 3：构建目标旋转矩阵 \( T_{\text{旋转}} \)#

绕对齐后的 z 轴旋转 \( 90^\circ \)：

\( \cos 90^\circ = 0, \quad \sin 90^\circ = 1 \)

绕 z 轴旋转 \( 90^\circ \) 的矩阵为：

\( T_{\text{旋转}} = \begin{bmatrix} 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

3.2.4 步骤 4：构建姿态还原矩阵 \( T_{\text{调整}} = T_1^\top T_2^\top \)#

旋转矩阵是正交矩阵，故还原矩阵为调整矩阵的逆，即其转置。

a) \( T_1 \) 的转置矩阵：

\( T_1^\top = \begin{bmatrix} \frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} & 0 & 0 \\ \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

b) \( T_2 \) 的转置矩阵：

\( T_2^\top = \begin{bmatrix} \frac{1}{\sqrt{3}} & 0 & \frac{\sqrt{2}}{\sqrt{3}} & 0 \\ 0 & 1 & 0 & 0 \\ -\frac{\sqrt{2}}{\sqrt{3}} & 0 & \frac{1}{\sqrt{3}} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

c) 计算组合姿态还原矩阵 \( T_{\text{还原}} = T_1^\top T_2^\top \)

\( T_{\text{还原}} = \begin{bmatrix} \frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} & 0 & 0 \\ \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \frac{1}{\sqrt{3}} & 0 & \frac{\sqrt{2}}{\sqrt{3}} & 0 \\ 0 & 1 & 0 & 0 \\ -\frac{\sqrt{2}}{\sqrt{3}} & 0 & \frac{1}{\sqrt{3}} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

逐元素计算左上角 3x3 部分：

  • 第一行:

    • \( \left(\frac{\sqrt{2}}{2} \cdot \frac{1}{\sqrt{3}} + \left(-\frac{\sqrt{2}}{2}\right) \cdot 0 + 0 \cdot \left(-\frac{\sqrt{2}}{\sqrt{3}}\right)\right) = \frac{1}{\sqrt{6}} \)

    • \( \left(\frac{\sqrt{2}}{2} \cdot 0 + \left(-\frac{\sqrt{2}}{2}\right) \cdot 1 + 0 \cdot 0\right) = -\frac{\sqrt{2}}{2} \)

    • \( \left(\frac{\sqrt{2}}{2} \cdot \frac{\sqrt{2}}{\sqrt{3}} + \left(-\frac{\sqrt{2}}{2}\right) \cdot 0 + 0 \cdot \frac{1}{\sqrt{3}}\right) = \frac{2}{2\sqrt{3}} = \frac{1}{\sqrt{3}} \)

  • 第二行:

    • \( \left(\frac{\sqrt{2}}{2} \cdot \frac{1}{\sqrt{3}} + \frac{\sqrt{2}}{2} \cdot 0 + 0 \cdot \left(-\frac{\sqrt{2}}{\sqrt{3}}\right)\right) = \frac{1}{\sqrt{6}} \)

    • \( \left(\frac{\sqrt{2}}{2} \cdot 0 + \frac{\sqrt{2}}{2} \cdot 1 + 0 \cdot 0\right) = \frac{\sqrt{2}}{2} \)

    • \( \left(\frac{\sqrt{2}}{2} \cdot \frac{\sqrt{2}}{\sqrt{3}} + \frac{\sqrt{2}}{2} \cdot 0 + 0 \cdot \frac{1}{\sqrt{3}}\right) = \frac{1}{\sqrt{3}} \)

  • 第三行:

    • \( \left(0 \cdot \frac{1}{\sqrt{3}} + 0 \cdot 0 + 1 \cdot \left(-\frac{\sqrt{2}}{\sqrt{3}}\right)\right) = -\frac{\sqrt{2}}{\sqrt{3}} \)

    • \( \left(0 \cdot 0 + 0 \cdot 1 + 1 \cdot 0\right) = 0 \)

    • \( \left(0 \cdot \frac{\sqrt{2}}{\sqrt{3}} + 0 \cdot 0 + 1 \cdot \frac{1}{\sqrt{3}}\right) = \frac{1}{\sqrt{3}} \)

所以，姿态还原矩阵为：

\( T_{\text{还原}} = \begin{bmatrix} \frac{1}{\sqrt{6}} & -\frac{\sqrt{2}}{2} & \frac{1}{\sqrt{3}} & 0 \\ \frac{1}{\sqrt{6}} & \frac{\sqrt{2}}{2} & \frac{1}{\sqrt{3}} & 0 \\ -\frac{\sqrt{2}}{\sqrt{3}} & 0 & \frac{1}{\sqrt{3}} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

3.2.5 步骤 5：执行完整旋转齐次变换 \( T_{\text{总}} = T_{\text{还原}} \cdot T_{\text{旋转}} \cdot T_{\text{调整}} \)#

此步骤计算量较大，分两步进行。

第一步：计算中间结果 \( M = T_{\text{旋转}} \cdot T_{\text{调整}} \)

\( M = \begin{bmatrix} 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \frac{1}{\sqrt{6}} & \frac{1}{\sqrt{6}} & -\frac{\sqrt{2}}{\sqrt{3}} & 0 \\ -\frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} & 0 & 0 \\ \frac{1}{\sqrt{3}} & \frac{1}{\sqrt{3}} & \frac{1}{\sqrt{3}} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

逐行计算（行向量点乘列向量）：

  • M 第一行:

    • \( \left(0\cdot \frac{1}{\sqrt{6}} + (-1)\cdot \left(-\frac{\sqrt{2}}{2}\right) + 0\cdot \frac{1}{\sqrt{3}}\right) = \frac{\sqrt{2}}{2} \)

    • \( \left(0\cdot \frac{1}{\sqrt{6}} + (-1)\cdot \frac{\sqrt{2}}{2} + 0\cdot \frac{1}{\sqrt{3}}\right) = -\frac{\sqrt{2}}{2} \)

    • \( \left(0\cdot \left(-\frac{\sqrt{2}}{\sqrt{3}}\right) + (-1)\cdot 0 + 0\cdot \frac{1}{\sqrt{3}}\right) = 0 \)

  • M 第二行:

    • \( \left(1\cdot \frac{1}{\sqrt{6}} + 0\cdot \left(-\frac{\sqrt{2}}{2}\right) + 0\cdot \frac{1}{\sqrt{3}}\right) = \frac{1}{\sqrt{6}} \)

    • \( \left(1\cdot \frac{1}{\sqrt{6}} + 0\cdot \frac{\sqrt{2}}{2} + 0\cdot \frac{1}{\sqrt{3}}\right) = \frac{1}{\sqrt{6}} \)

    • \( \left(1\cdot \left(-\frac{\sqrt{2}}{\sqrt{3}}\right) + 0\cdot 0 + 0\cdot \frac{1}{\sqrt{3}}\right) = -\frac{\sqrt{2}}{\sqrt{3}} \)

  • M 第三行与 \( T_{\text{调整}} \) 的第三行相同。

  • M 第四行与 \( T_{\text{调整}} \) 的第四行相同。

所以，中间结果矩阵为：

\( M = \begin{bmatrix} \frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} & 0 & 0 \\ \frac{1}{\sqrt{6}} & \frac{1}{\sqrt{6}} & -\frac{\sqrt{2}}{\sqrt{3}} & 0 \\ \frac{1}{\sqrt{3}} & \frac{1}{\sqrt{3}} & \frac{1}{\sqrt{3}} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

第二步：计算总变换矩阵 \( T_{\text{总}} = T_{\text{还原}} \cdot M \)

这是最易出错的一步，需要极其仔细。

\( T_{\text{总}} = \begin{bmatrix} \frac{1}{\sqrt{6}} & -\frac{\sqrt{2}}{2} & \frac{1}{\sqrt{3}} & 0 \\ \frac{1}{\sqrt{6}} & \frac{\sqrt{2}}{2} & \frac{1}{\sqrt{3}} & 0 \\ -\frac{\sqrt{2}}{\sqrt{3}} & 0 & \frac{1}{\sqrt{3}} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} & 0 & 0 \\ \frac{1}{\sqrt{6}} & \frac{1}{\sqrt{6}} & -\frac{\sqrt{2}}{\sqrt{3}} & 0 \\ \frac{1}{\sqrt{3}} & \frac{1}{\sqrt{3}} & \frac{1}{\sqrt{3}} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

我们精确计算 \( T_{\text{总}} \) 的旋转部分（3x3）。为简洁，令 \( a = \frac{1}{\sqrt{3}} \)。注意到 \( a^2 = \frac{1}{3} \)。

  • 计算 \( T_{\text{总}}(1,1) \):

\( \frac{1}{\sqrt{6}} \cdot \frac{\sqrt{2}}{2} + \left(-\frac{\sqrt{2}}{2}\right) \cdot \frac{1}{\sqrt{6}} + \frac{1}{\sqrt{3}} \cdot \frac{1}{\sqrt{3}} = \frac{1}{2\sqrt{3}} - \frac{1}{2\sqrt{3}} + \frac{1}{3} = \frac{1}{3} = a^2 \)

  • 计算 \( T_{\text{总}}(1,2) \):

\( \frac{1}{\sqrt{6}} \cdot \left(-\frac{\sqrt{2}}{2}\right) + \left(-\frac{\sqrt{2}}{2}\right) \cdot \frac{1}{\sqrt{6}} + \frac{1}{\sqrt{3}} \cdot \frac{1}{\sqrt{3}} = -\frac{1}{2\sqrt{3}} - \frac{1}{2\sqrt{3}} + \frac{1}{3} = -\frac{1}{\sqrt{3}} + \frac{1}{3} = -a + a^2 \)

  • 计算 \( T_{\text{总}}(1,3) \):

\( \frac{1}{\sqrt{6}} \cdot 0 + \left(-\frac{\sqrt{2}}{2}\right) \cdot \left(-\frac{\sqrt{2}}{\sqrt{3}}\right) + \frac{1}{\sqrt{3}} \cdot \frac{1}{\sqrt{3}} = 0 + \frac{2}{2\sqrt{3}} + \frac{1}{3} = \frac{1}{\sqrt{3}} + \frac{1}{3} = a + a^2 \)

  • 计算 \( T_{\text{总}}(2,1) \):

\( \frac{1}{\sqrt{6}} \cdot \frac{\sqrt{2}}{2} + \frac{\sqrt{2}}{2} \cdot \frac{1}{\sqrt{6}} + \frac{1}{\sqrt{3}} \cdot \frac{1}{\sqrt{3}} = \frac{1}{2\sqrt{3}} + \frac{1}{2\sqrt{3}} + \frac{1}{3} = \frac{1}{\sqrt{3}} + \frac{1}{3} = a + a^2 \)

  • 计算 \( T_{\text{总}}(2,2) \):

\( \frac{1}{\sqrt{6}} \cdot \left(-\frac{\sqrt{2}}{2}\right) + \frac{\sqrt{2}}{2} \cdot \frac{1}{\sqrt{6}} + \frac{1}{\sqrt{3}} \cdot \frac{1}{\sqrt{3}} = -\frac{1}{2\sqrt{3}} + \frac{1}{2\sqrt{3}} + \frac{1}{3} = \frac{1}{3} = a^2 \)

  • 计算 \( T_{\text{总}}(2,3) \):

\( \frac{1}{\sqrt{6}} \cdot 0 + \frac{\sqrt{2}}{2} \cdot \left(-\frac{\sqrt{2}}{\sqrt{3}}\right) + \frac{1}{\sqrt{3}} \cdot \frac{1}{\sqrt{3}} = 0 - \frac{2}{2\sqrt{3}} + \frac{1}{3} = -\frac{1}{\sqrt{3}} + \frac{1}{3} = -a + a^2 \)

  • 计算 \( T_{\text{总}}(3,1) \):

\( \left(-\frac{\sqrt{2}}{\sqrt{3}}\right) \cdot \frac{\sqrt{2}}{2} + 0 \cdot \frac{1}{\sqrt{6}} + \frac{1}{\sqrt{3}} \cdot \frac{1}{\sqrt{3}} = -\frac{2}{2\sqrt{3}} + 0 + \frac{1}{3} = -\frac{1}{\sqrt{3}} + \frac{1}{3} = -a + a^2 \)

  • 计算 \( T_{\text{总}}(3,2) \):

\( \left(-\frac{\sqrt{2}}{\sqrt{3}}\right) \cdot \left(-\frac{\sqrt{2}}{2}\right) + 0 \cdot \frac{1}{\sqrt{6}} + \frac{1}{\sqrt{3}} \cdot \frac{1}{\sqrt{3}} = \frac{2}{2\sqrt{3}} + 0 + \frac{1}{3} = \frac{1}{\sqrt{3}} + \frac{1}{3} = a + a^2 \)

  • 计算 \( T_{\text{总}}(3,3) \):

\( \left(-\frac{\sqrt{2}}{\sqrt{3}}\right) \cdot 0 + 0 \cdot \left(-\frac{\sqrt{2}}{\sqrt{3}}\right) + \frac{1}{\sqrt{3}} \cdot \frac{1}{\sqrt{3}} = 0 + 0 + \frac{1}{3} = a^2 \)

将 \( a = 1/\sqrt{3} \) 代回，得到总变换矩阵的旋转部分：

\( T_{\text{总(3x3)}} = \begin{bmatrix} a^2 & -a + a^2 & a + a^2 \\ a + a^2 & a^2 & -a + a^2 \\ -a + a^2 & a + a^2 & a^2 \end{bmatrix} = \frac{1}{3} \begin{bmatrix} 1 & 1 - \sqrt{3} & 1 + \sqrt{3} \\ 1 + \sqrt{3} & 1 & 1 - \sqrt{3} \\ 1 - \sqrt{3} & 1 + \sqrt{3} & 1 \end{bmatrix} \)

因此，完整的 4x4 总变换齐次矩阵为：

\( T_{\text{总}} = \begin{bmatrix} \frac{1}{3} & \frac{1-\sqrt{3}}{3} & \frac{1+\sqrt{3}}{3} & 0 \\ \frac{1+\sqrt{3}}{3} & \frac{1}{3} & \frac{1-\sqrt{3}}{3} & 0 \\ \frac{1-\sqrt{3}}{3} & \frac{1+\sqrt{3}}{3} & \frac{1}{3} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

第三步：计算旋转后的齐次坐标 \( \widetilde{\mathbf{p}}' = T_{\text{总}} \cdot \widetilde{\mathbf{p}} \)

\( \widetilde{\mathbf{p}}' = \begin{bmatrix} \frac{1}{3} & \frac{1-\sqrt{3}}{3} & \frac{1+\sqrt{3}}{3} & 0 \\ \frac{1+\sqrt{3}}{3} & \frac{1}{3} & \frac{1-\sqrt{3}}{3} & 0 \\ \frac{1-\sqrt{3}}{3} & \frac{1+\sqrt{3}}{3} & \frac{1}{3} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 \\ 0 \\ 0 \\ 1 \end{bmatrix} = \begin{bmatrix} \frac{1}{3} \\ \frac{1+\sqrt{3}}{3} \\ \frac{1-\sqrt{3}}{3} \\ 1 \end{bmatrix} \)

3.2.6 步骤 6：转回直角坐标#

忽略齐次坐标最后的 1，旋转后的点为：

\( \mathbf{p}' = \left( \frac{1}{3}, \frac{1+\sqrt{3}}{3}, \frac{1-\sqrt{3}}{3} \right) \approx (0.333, 0.911, -0.244) \)

\( \boxed{\mathbf{p}' = \left( \frac{1}{3}, \frac{1+\sqrt{3}}{3}, \frac{1-\sqrt{3}}{3} \right)} \)

3.3 结果验证#

3.3.1 几何验证：保距性#

旋转操作是刚体变换，点到旋转轴的距离应保持不变。

3.3.1.1 旋转前：点 \( \mathbf{p} = (1,0,0) \) 到直线 \( x=y=z \) 的距离#

点到直线的距离公式为 \( d = \frac{|\mathbf{p} \times \mathbf{u}|}{|\mathbf{u}|} \)，其中 \( \mathbf{u} \) 是轴的方向向量。

\( \mathbf{p} \times \mathbf{u} = \left(0\cdot \frac{1}{\sqrt{3}} - 0\cdot \frac{1}{\sqrt{3}},\ 0\cdot \frac{1}{\sqrt{3}} - 1\cdot \frac{1}{\sqrt{3}},\ 1\cdot \frac{1}{\sqrt{3}} - 0\cdot \frac{1}{\sqrt{3}}\right) = \left(0, -\frac{1}{\sqrt{3}}, \frac{1}{\sqrt{3}}\right) \)

\( |\mathbf{p} \times \mathbf{u}| = \sqrt{0^2 + \left(-\frac{1}{\sqrt{3}}\right)^2 + \left(\frac{1}{\sqrt{3}}\right)^2} = \sqrt{\frac{2}{3}} = \frac{\sqrt{2}}{\sqrt{3}} \)

\( |\mathbf{u}| = 1 \)，所以距离 \( d = \frac{\sqrt{2}}{\sqrt{3}} = \frac{\sqrt{6}}{3} \)。

3.3.1.2 旋转后：点 \( \mathbf{p}' = \left(\frac{1}{3}, \frac{1+\sqrt{3}}{3}, \frac{1-\sqrt{3}}{3}\right) \) 到直线 \( x=y=z \) 的距离#

向量 \( \mathbf{p}' - \text{proj}_{\mathbf{u}}(\mathbf{p}') \)

先计算 \( \mathbf{p}' \) 在 \( \mathbf{u} \) 上的投影：

\( \mathbf{p}' \cdot \mathbf{u} = \frac{1}{3}\cdot\frac{1}{\sqrt{3}} + \frac{1+\sqrt{3}}{3}\cdot\frac{1}{\sqrt{3}} + \frac{1-\sqrt{3}}{3}\cdot\frac{1}{\sqrt{3}} = \frac{1 + (1+\sqrt{3}) + (1-\sqrt{3})}{3\sqrt{3}} = \frac{3}{3\sqrt{3}} = \frac{1}{\sqrt{3}} \)

投影向量为 \( \text{proj} = \left(\frac{1}{\sqrt{3}}\right)\mathbf{u} = \left(\frac{1}{3}, \frac{1}{3}, \frac{1}{3}\right) \)。

垂足向量为 \( \mathbf{v} = \mathbf{p}' - \text{proj} = \left(\frac{1}{3}-\frac{1}{3}, \frac{1+\sqrt{3}}{3}-\frac{1}{3}, \frac{1-\sqrt{3}}{3}-\frac{1}{3}\right) = \left(0, \frac{\sqrt{3}}{3}, -\frac{\sqrt{3}}{3}\right) \)。

距离 \( d' = |\mathbf{v}| = \sqrt{0^2 + \left(\frac{\sqrt{3}}{3}\right)^2 + \left(-\frac{\sqrt{3}}{3}\right)^2} = \sqrt{\frac{2}{3}} = \frac{\sqrt{6}}{3} \)。

因为 \( d = d' \)，验证了旋转的保距性，结果正确。

3.3.2 Rodrigues 旋转公式验证#

绕任意轴旋转可直接用 Rodrigues 公式计算 3×3 旋转矩阵，无需坐标对齐，公式为：
\( R = \cos\theta \cdot I + (1-\cos\theta)\mathbf{u}\mathbf{u}^T + \sin\theta \cdot [\mathbf{u}]_\times \)
其中 \( [\mathbf{u}]_\times \) 是 \( \mathbf{u} \) 的反对称矩阵。

python代码如下

方法1：

import numpy as np
def rodrigues(v, k, theta):
    k = k / np.linalg.norm(k)
    return v*np.cos(theta) + np.cross(k,v)*np.sin(theta) + k*np.dot(k,v)*(1-np.cos(theta))

p_new = rodrigues(v=[1,0,0], k=[1,1,1], theta=np.pi/2)
print(p_new)		# [ 0.33333333  0.9106836  -0.24401694]

方法2

# 安装pip包，pip install spatialmath-python
from spatialmath import UnitQuaternion, SO3
v = [1, 0, 0]
axis = [1, 1, 1]
theta = 90  # deg
R = SO3.AngleAxis(theta, axis, unit='deg')   # 旋转矩阵
v_rot = R * v
print(v_rot)          # [0.333, 0.911, -0.244]

4 旋转次序对 3D 变换结果的影响#

旋转次序对 3 维齐次变换结果有决定性影响，核心原因是矩阵乘法不满足交换律——不同旋转次序对应不同的矩阵乘法顺序，最终输出完全不同的空间姿态。

4.1 核心原理：矩阵乘法的“不可交换性”#

3 维旋转的本质是多个 3×3 旋转矩阵（融入 4×4 齐次矩阵）的乘积，矩阵乘法满足结合律但不满足交换律：

  • 先绕 x 轴转 \( \theta_1 \)（矩阵 \( T_x \)）、再绕 z 轴转 \( \theta_2 \)（矩阵 \( T_z \)），总旋转矩阵为 \( T_{\text{总1}} = T_z \cdot T_x \)；

  • 先绕 z 轴转 \( \theta_2 \)、再绕 x 轴转 \( \theta_1 \)，总旋转矩阵为 \( T_{\text{总2}} = T_x \cdot T_z \)；

  • 由于 \( T_z \cdot T_x \neq T_x \cdot T_z \)，两种次序的总旋转矩阵不同，作用于同一点后结果必然不同。

4.2 具体案例：直观对比不同旋转次序#

以“点 \( \mathbf{p}=(1,0,0) \) 先绕 x 轴转 \( 90^\circ \)、再绕 z 轴转 \( 90^\circ \)”与“先绕 z 轴转 \( 90^\circ \)、再绕 x 轴转 \( 90^\circ \)”为例。

4.2.1 基础准备：明确旋转齐次矩阵#

  • 绕 x 轴转 \( 90^\circ \) 的齐次矩阵：\( T_x = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 0 & -1 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

  • 绕 z 轴转 \( 90^\circ \) 的齐次矩阵：\( T_z = \begin{bmatrix} 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \)

  • 点 \( \mathbf{p}=(1,0,0) \) 的齐次坐标：\( \widetilde{\mathbf{p}} = \begin{bmatrix} 1 \\ 0 \\ 0 \\ 1 \end{bmatrix} \)

4.2.2 次序 1：先绕 x 轴转 \( 90^\circ \) → 再绕 z 轴转 \( 90^\circ \)#

  1. 绕 x 轴旋转：\( \widetilde{\mathbf{p}}_1 = T_x \cdot \widetilde{\mathbf{p}} = \begin{bmatrix} 1 \\ 0 \\ 0 \\ 1 \end{bmatrix} \)，转回直角坐标 \( \mathbf{p}_1=(1,0,0) \)（x 轴上的点绕 x 轴旋转位置不变）；

  2. 绕 z 轴旋转：\( \widetilde{\mathbf{p}}_{\text{结果1}} = T_z \cdot \widetilde{\mathbf{p}}_1 = \begin{bmatrix} 0 \\ 1 \\ 0 \\ 1 \end{bmatrix} \)，转回直角坐标 \( \mathbf{p}_{\text{结果1}}=(0,1,0) \)。

4.2.3 次序 2：先绕 z 轴转 \( 90^\circ \) → 再绕 x 轴转 \( 90^\circ \)#

  1. 绕 z 轴旋转：\( \widetilde{\mathbf{p}}_2 = T_z \cdot \widetilde{\mathbf{p}} = \begin{bmatrix} 0 \\ 1 \\ 0 \\ 1 \end{bmatrix} \)，转回直角坐标 \( \mathbf{p}_2=(0,1,0) \)；

  2. 绕 x 轴旋转：\( \widetilde{\mathbf{p}}_{\text{结果2}} = T_x \cdot \widetilde{\mathbf{p}}_2 = \begin{bmatrix} 0 \\ 0 \\ 1 \\ 1 \end{bmatrix} \)，转回直角坐标 \( \mathbf{p}_{\text{结果2}}=(0,0,1) \)。

4.3 工程意义#

在机器人运动学、3D 建模等领域，必须明确“旋转次序”（如常见的“Z-Y-X”“X-Y-Z”次序）：

  • 工业机器人手腕旋转需按预设次序运动，才能精准到达目标位置；

  • 3D 模型动画中，角色关节旋转次序错误会导致肢体姿态扭曲。