运动学基础：刚体运动、旋转矩阵、四元数、齐次变换、正逆运动学
各位同学，欢迎来到第二章。这一章的内容，说白了就是具身智能的「数学语言」。你想想看，机器人要抓杯子、要走路，它得先知道自己胳膊在哪儿、杯子在哪儿吧？这些位置和姿态的描述，就是运动学要解决的问题。
我个人习惯把运动学看作「几何约束的数学表达」。你不需要一开始就搞懂所有公式的推导，但必须理解每个概念背后的物理直觉。好，我们开始。
2.1 刚体运动与位姿描述
什么叫刚体？就是不会变形的物体。机械臂的连杆、无人机的机身，在建模时我们都假设它们是刚体。刚体在空间中的状态，用「位置 + 姿态」来描述，合起来叫位姿。
- 位置：用三维坐标 (x, y, z) 表示，就是物体在空间中的那个点。
- 姿态：描述物体「朝哪看」「怎么转」，这是本章的难点。
我记得刚入行时，总把位置和姿态混为一谈。后来带我的老工程师说了一句话，我记到现在：「位置是平移，姿态是旋转，它们是两码事，但必须一起算。」
核心观点：刚体位姿 = 平移 + 旋转。平移用向量，旋转用矩阵或四元数。两者通过齐次变换统一到一个框架里。
2.2 旋转矩阵
旋转矩阵是描述姿态最直观的方式。它是一个 3×3 的正交矩阵，行列式为 +1。什么意思？就是它只负责「转」，不负责「拉长」或「压缩」。
举个例子，绕 Z 轴旋转 θ 角，旋转矩阵长这样：
R_z(θ) = [cosθ -sinθ 0]
[sinθ cosθ 0]
[0 0 1]绕 X 轴和 Y 轴的类似，我就不全写了。你想想看，一个点 P 在空间中，左乘一个旋转矩阵，它就转到了新位置。这就是旋转的本质——线性变换。
我在项目中遇到过一个问题：连续旋转时，旋转矩阵的乘法顺序特别容易搞反。比如先绕 X 轴转 30°，再绕 Y 轴转 45°，结果是 R_y * R_x * P，不是 R_x * R_y * P。这个顺序我吃过亏，后来每次写代码前都会在纸上画一遍。
注意：旋转矩阵乘法不满足交换律。R1 * R2 ≠ R2 * R1。顺序错了，姿态就完全不对。
2.3 四元数
旋转矩阵有 9 个参数，但自由度只有 3。这会导致数值计算时出现冗余和奇异。四元数就是来解决这个问题的。
四元数可以看作「带约束的复数扩展」。它用 4 个参数表示旋转：一个实部 + 三个虚部。写成 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部，且满足 w² + x² + y² + z² = 1。
为什么用四元数？三个理由：
- 无万向锁：欧拉角在俯仰角 ±90° 时会丢失一个自由度，四元数不会。
- 插值平滑：两个姿态之间做球面线性插值（SLERP），四元数天然支持。
- 计算高效：比旋转矩阵少 5 个参数，组合旋转时只需一次乘法。
我曾经在调试一个机械臂的轨迹规划时，发现末端执行器在某个点突然「抽搐」了一下。查了半天，原来是欧拉角导致的万向锁。换成四元数后，问题立刻消失。嗯，从那以后，我所有涉及姿态插值的代码，一律用四元数。
实用技巧：ROS 和 Eigen 库都内置了四元数类。你只需要记住：四元数乘法对应旋转组合，共轭对应逆旋转。别自己去手写四元数乘法，容易出错。
2.4 齐次变换
现在问题来了：我们既有平移，又有旋转。能不能用一个统一的数学工具把它们放在一起？
能。齐次变换矩阵就是干这个的。它是一个 4×4 的矩阵：
T = [R t]
[0 1]其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量，最后一行是 [0 0 0 1]。
为什么叫「齐次」？因为我们在三维坐标后面加了一个 1，变成齐次坐标 (x, y, z, 1)。这样，平移和旋转就可以用一次矩阵乘法搞定。
举个例子，一个点 P 从坐标系 A 变换到坐标系 B：
P_B = T_A_to_B * P_A这个 T 矩阵，就是两个坐标系之间的「桥梁」。我在做多传感器融合时，经常需要把激光雷达的点云变换到相机坐标系下。每次都是构造一个齐次变换矩阵，然后批量左乘所有点。效率很高，代码也很干净。
关键点：齐次变换矩阵的逆矩阵，可以直接从 T 算出：逆矩阵的旋转部分是 Rᵀ，平移部分是 -Rᵀ * t。不需要重新求逆，省计算量。
2.5 正运动学
正运动学，就是已知关节角度，求末端执行器的位姿。说白了，你告诉机器人「每个关节转多少度」，它告诉你「手在哪儿、朝哪看」。
对于串联机械臂，正运动学的标准方法是 DH 参数法（Denavit-Hartenberg）。每个关节用一个 4×4 的齐次变换矩阵表示，然后把这些矩阵乘起来：
T_0_to_n = T_0_to_1 * T_1_to_2 * ... * T_{n-1}_to_n结果 T_0_to_n 就是基座到末端执行器的总变换。
我记得第一次手算六轴机械臂的正运动学时，算了整整两页纸。后来发现，用 Python 的 SymPy 库做符号推导，几分钟就搞定了。但手动推导的过程让我深刻理解了每个参数的含义——关节角、连杆长度、连杆偏距、扭转角，这四个参数缺一不可。
建议：初学阶段，至少手算一次 2-DOF 或 3-DOF 机械臂的正运动学。用纸笔推导，再用代码验证。这个过程能帮你建立直觉。
2.6 逆运动学
逆运动学是正运动学的逆问题：已知末端执行器的目标位姿，求各个关节的角度。这比正运动学难得多，原因有三：
- 多解性：同一个末端位姿，可能对应多组关节角度（比如「肘部朝上」和「肘部朝下」）。
- 无解性：目标位姿超出了机械臂的工作空间，怎么算都没解。
- 非线性：方程里全是三角函数，没有通用的解析解。
求解逆运动学的方法主要有两类：
- 解析法：针对特定构型的机械臂（比如有球形腕的六轴臂），可以推导出封闭解。速度快，精度高。
- 数值法：用迭代优化（比如牛顿法、雅可比伪逆法）逼近解。通用性强，但可能陷入局部最优。
我在项目中遇到过一个问题：用数值法求解逆运动学时，迭代不收敛。后来发现是初始值选得太差，离真实解太远。解决办法是先用解析法给一个粗略的初始值，再用数值法精调。嗯，混合策略往往比单一方法更可靠。
避坑指南：我曾经在逆运动学代码里忘记处理关节限位，结果算法算出了一个「理论上正确但实际撞机」的角度。从那以后，我每次求解后都会加一步关节限位检查。你也要养成这个习惯。
2.7 知识体系总览
这一章的内容比较多，我画了一张图帮你理清脉络。你看，从刚体运动出发，分出了位置和姿态两条线。姿态用旋转矩阵或四元数描述，位置用平移向量。两者通过齐次变换合并，然后应用到正运动学和逆运动学中。
2.8 本章小结
这一章我们覆盖了运动学的核心概念。你不需要一次记住所有公式，但一定要理解这几条主线：
- 刚体位姿 = 位置 + 姿态，两者独立但统一。
- 旋转矩阵直观但冗余，四元数紧凑且无奇异。
- 齐次变换矩阵是连接不同坐标系的「万能钥匙」。
- 正运动学有唯一解，逆运动学需要小心处理多解和无解。