如何用旋转矩阵实现直角坐标系的点旋转？

Question

如何用旋转矩阵实现直角坐标系中点的绕任意轴旋转？在二维情况下，点绕原点旋转可通过2×2旋转矩阵轻松实现，但在三维空间中，若需将一个点绕x、y或z轴之一旋转，应如何构建对应的3×3旋转矩阵？更进一步，当旋转轴不是坐标轴而是任意方向向量时，又该如何推导罗德里格斯（Rodrigues）旋转公式，并正确应用旋转矩阵进行坐标变换？常见问题包括：矩阵乘法顺序错误、旋转方向混淆（顺时针 vs 逆时针）、未归一化旋转轴向量等，这些都会导致旋转结果不准确。

Answer 1

如何用旋转矩阵实现直角坐标系中点的绕任意轴旋转

1. 二维空间中的旋转：从基础理解开始

在二维直角坐标系中，一个点 P = (x, y) 绕原点逆时针旋转角度 θ 后的新坐标可通过如下 2×2 旋转矩阵 计算：

\[ \begin{bmatrix} x' \\ y' \end{bmatrix} = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} x \\ y \end{bmatrix} \]

该公式基于极坐标变换推导而来。关键点在于：旋转方向遵循右手定则（数学惯例为逆时针为正方向），且旋转中心必须是原点。若需绕非原点点旋转，需先平移至原点，旋转后再平移回去。

2. 三维空间中的基本旋转矩阵

进入三维空间后，绕坐标轴（x、y、z）的旋转可分别用以下 3×3 正交矩阵 表示。假设旋转角度为 θ，各轴旋转矩阵如下表所示：

旋转轴	旋转矩阵
X 轴	\( R_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{bmatrix} \)
Y 轴	\( R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix} \)
Z 轴	\( R_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \)

这些矩阵构成了三维旋转的基础构件。实际应用中，复合旋转通过矩阵乘法组合实现，但需注意：矩阵乘法不满足交换律，因此旋转顺序至关重要。例如，先绕X再绕Y与先绕Y再绕X结果不同。

3. 复合旋转与欧拉角的陷阱

当需要进行多轴连续旋转时，通常采用欧拉角（如 yaw-pitch-roll）表示姿态变化。设总旋转矩阵为：

R_{total} = R_z(\psi) \cdot R_y(\theta) \cdot R_x(\phi)

其中 ψ、θ、φ 分别代表偏航、俯仰、滚转角。然而，这种表示存在万向节锁（Gimbal Lock）问题——当某个中间角度为90°时，自由度丢失，导致奇异状态。此外，不同系统对旋转顺序定义不同（如航空航天常用ZYX，而机器人可能用XYZ），极易引发混淆。

4. 任意轴旋转：罗德里格斯（Rodrigues）公式的推导

当旋转轴为任意单位向量 u = (u_x, u_y, u_z) 时，绕该轴旋转角度 θ 的变换无法直接使用上述基本矩阵。此时应使用罗德里格斯旋转公式：

\[ \mathbf{v}' = \mathbf{v} \cos\theta + (\mathbf{u} \times \mathbf{v}) \sin\theta + \mathbf{u} (\mathbf{u} \cdot \mathbf{v}) (1 - \cos\theta) \]

该公式将向量分解为平行于和垂直于旋转轴的两个分量，并分别处理其旋转行为。由此可推导出对应的 3×3 旋转矩阵形式：

R = I + \sin\theta K + (1 - \cos\theta) K^2

其中 I 是单位矩阵，K 是由单位向量 u 构造的反对称矩阵：

K = \begin{bmatrix} 0 & -u_z & u_y \\ u_z & 0 & -u_x \\ -u_y & u_x & 0 \end{bmatrix}

5. 罗德里格斯公式的完整矩阵表达式

展开后的 Rodrigues 旋转矩阵如下：

R(\mathbf{u}, \theta) = \begin{bmatrix} \cos\theta + u_x^2(1{-}\cos\theta) & u_xu_y(1{-}\cos\theta) - u_z\sin\theta & u_xu_z(1{-}\cos\theta) + u_y\sin\theta \\ u_yu_x(1{-}\cos\theta) + u_z\sin\theta & \cos\theta + u_y^2(1{-}\cos\theta) & u_yu_z(1{-}\cos\theta) - u_x\sin\theta \\ u_zu_x(1{-}\cos\theta) - u_y\sin\theta & u_zu_y(1{-}\cos\theta) + u_x\sin\theta & \cos\theta + u_z^2(1{-}\cos\theta) \end{bmatrix}

此矩阵可用于直接对任意向量进行坐标变换。实现时务必确保旋转轴向量已归一化，否则会导致缩放失真。

6. 常见错误与调试建议

• 未归一化旋转轴：若输入向量未单位化，则 Rodrigues 公式失效，应始终执行 u = u / ||u||。
• 旋转方向混淆：默认使用右手法则，若出现反向旋转，检查 sinθ 符号或坐标系手性（左手系 vs 右手系）。
• 矩阵乘法顺序错误：在链式变换中（如 TRT⁻¹），顺序决定语义，建议使用函数封装并添加注释说明。
• 角度单位误用：多数数学库要求弧度制，传入角度前需转换（rad = deg * π / 180）。
• 浮点精度累积误差：长期迭代旋转可能导致矩阵偏离正交性，建议定期重正交化或改用四元数。

7. 实际代码实现示例（Python）

import numpy as np

def rodrigues_rotation(v, axis, theta):
    """绕任意单位轴旋转向量 v"""
    axis = axis / np.linalg.norm(axis)  # 归一化
    cos_t, sin_t = np.cos(theta), np.sin(theta)
    ux, uy, uz = axis
    
    K = np.array([[  0, -uz,  uy],
                  [ uz,   0, -ux],
                  [-uy,  ux,   0]])
    
    R = np.eye(3) + sin_t * K + (1 - cos_t) * K @ K
    return R @ v

# 示例：将点 (1,0,0) 绕 (1,1,1) 方向旋转 90°
v = np.array([1.0, 0.0, 0.0])
axis = np.array([1.0, 1.0, 1.0])
rotated = rodrigues_rotation(v, axis, np.pi/2)
print("旋转后坐标:", rotated)

8. 替代方案比较：四元数与旋转矩阵

虽然旋转矩阵直观易用，但在连续旋转插值或避免万向节锁方面存在局限。相比之下，单位四元数具有以下优势：

1. 内存占用更小（4个数 vs 9个数）
2. 插值更稳定（SLERP）
3. 天然避免奇异点
4. 易于组合多个旋转

但旋转矩阵在图形渲染管线中仍是标准输入格式，因此常需在四元数与矩阵间相互转换。现代引擎（如Unity、Unreal）内部多用四元数管理姿态，输出时转为矩阵。

9. 可视化流程：绕任意轴旋转的步骤分解

graph TD A[原始点 P] --> B{是否绕原点?} B -- 否 --> C[平移使旋转轴过原点] B -- 是 --> D[继续] C --> D D --> E[归一化旋转轴 u] E --> F[构建 Rodrigues 矩阵 R] F --> G[计算 P' = R × P] G --> H{是否需还原平移?} H -- 是 --> I[反向平移] H -- 否 --> J[输出结果] I --> J

10. 高级应用场景与扩展思考

在计算机图形学、机器人运动学、AR/VR姿态跟踪等领域，精确的坐标旋转是核心操作。进一步可结合：

• 齐次坐标与4×4变换矩阵：统一处理旋转与平移
• 李群 SO(3) 与李代数 so(3)：为优化算法提供微分流形视角
• 旋转向量表示：用 ||ω||=θ, direction=axis 编码旋转
• 指数映射：exp: so(3) → SO(3)，连接微小旋转与整体姿态

对于五年以上经验的工程师，深入理解这些数学结构有助于设计更鲁棒的姿态估计与控制算法。