第3章旋转矩阵

旋转矩阵：绕单轴旋转、性质、合成与逆

各位同学，今天我们聊聊旋转矩阵。说实话，这东西在机器人运动规划里太常用了。我刚开始做机械臂路径规划时，天天跟它打交道。你想想看，要让机械臂末端执行器从A点转到B点，中间怎么转？靠的就是旋转矩阵。

先看最简单的——绕单个坐标轴旋转。我习惯把这三个基本旋转叫做“三板斧”。

绕X轴旋转角度θ：

R_x(θ) = [1      0       0    ]
         [0   cosθ   -sinθ   ]
         [0   sinθ    cosθ   ]

绕Y轴旋转角度θ：

R_y(θ) = [ cosθ   0    sinθ  ]
         [ 0      1    0     ]
         [-sinθ   0    cosθ  ]

绕Z轴旋转角度θ：

R_z(θ) = [cosθ  -sinθ   0   ]
         [sinθ   cosθ   0   ]
         [ 0      0     1   ]

嗯，这里要注意符号。我当年第一次写代码时，把sinθ的符号搞反了，结果机械臂在仿真里转得乱七八糟。后来养成了习惯：每次写完都用手动算一个简单角度验证一下，比如θ=90°。

小技巧：验证旋转矩阵对不对，可以看它的行列式是不是1，以及每一列是不是单位向量。这两个条件同时满足，基本就没问题。

旋转矩阵有几个重要性质，我建议你记牢了。为什么？因为后面做运动学正解、逆解时，这些性质能帮你省不少事。

说白了，旋转矩阵不会拉伸或压缩物体，只是让它转个角度。我在做视觉伺服时，经常用这个性质来验证标定结果——如果旋转矩阵不正交，那标定肯定有问题。

  核心要点：旋转矩阵的逆就是它的转置。这个性质太方便了，计算逆矩阵时直接转置就行，不用去算复杂的逆矩阵公式。

实际应用中，很少只绕一个轴转。比如让机械臂末端先绕Z轴转30°，再绕Y轴转45°，最后绕X轴转60°。这就涉及到旋转矩阵的合成。

合成规则很简单：后乘原则。如果先转R₁，再转R₂，最后转R₃，那么总的旋转矩阵是：

R = R_3 · R_2 · R_1

注意顺序！矩阵乘法不满足交换律。我见过太多新手在这里栽跟头。举个例子：

// 先绕Z转30°，再绕Y转45°
R = R_y(45°) * R_z(30°)

// 如果顺序搞反了
R_wrong = R_z(30°) * R_y(45°)  // 结果完全不同！

为什么会这样？你想想看，第二次旋转是在第一次旋转后的坐标系里进行的。顺序一变，结果天差地别。

避坑指南：我曾经在调试一个六轴机械臂时，发现末端姿态总是不对。查了两天，最后发现是旋转矩阵的乘法顺序写反了。从那以后，我每次写旋转合成代码，都会在注释里明确标注旋转顺序。

旋转矩阵的逆，说白了就是反向旋转。如果R表示绕某个轴转θ角，那么R^-1就表示绕同一个轴转-θ角。

利用正交性，我们有：

R^(-1) = R^T

这个性质太实用了。比如你有一个旋转矩阵R，想求它的逆，直接转置就行：

// 假设R是3x3旋转矩阵
R_inv = R.transpose()  // 在Eigen库中
// 或者
R_inv = np.linalg.inv(R)  // 在NumPy中，但用转置更快

我个人习惯用转置而不是求逆函数。为什么？因为转置是O(n²)的复杂度，而求逆是O(n³)。对于3x3矩阵差别不大，但养成好习惯总没错。

  验证方法：算完逆矩阵后，检查R·R-1是否等于单位矩阵。如果结果不是单位矩阵，说明你的旋转矩阵不正交，或者计算有误。

下面这张图展示了旋转矩阵的核心知识结构，我建议你把它记在脑子里：

这张图把旋转矩阵的四个核心知识点串起来了。绕单轴旋转是基础，性质是工具，合成是应用，逆是反向操作。四者缺一不可。

我的建议：刚开始学的时候，别急着背公式。先理解旋转矩阵的几何意义——它描述的是坐标系之间的旋转关系。理解了这一点，后面的东西就顺理成章了。

好了，旋转矩阵的内容就讲到这里。记住：多动手算，多写代码验证。理论懂了，实践跟上，才能真正掌握。