4. 运动学基础：刚体运动描述，旋转矩阵与四元数，正运动学与逆运动学，雅可比矩阵
好，咱们今天聊聊运动学。说实话，这是整个具身智能控制里最“硬核”的部分之一。你想想看，机器人要动起来，首先得知道“怎么描述它动了”。我刚开始做机械臂项目时，就栽在这上面——明明指令发下去了，末端执行器就是跑不到目标点。后来才发现，是运动学模型没建对。
运动学不关心力，不关心质量，只关心“位置、速度、加速度”这些几何关系。说白了，就是研究机器人怎么动，而不是为什么动。这一块搞明白了，后面的动力学、控制学才有根基。
4.1 刚体运动描述
先问个问题：一个刚体在三维空间里，有几个自由度？
答案是6个。3个平移自由度（沿着x、y、z轴移动），3个旋转自由度（绕着x、y、z轴转动）。这6个自由度组合起来，就能描述刚体在空间中的任意位姿。
我个人习惯用“位姿”这个词，它包含两层意思：
- 位置：刚体上某个参考点的坐标，通常用三维向量 p = [x, y, z]^T 表示。
- 姿态：刚体相对于参考坐标系的朝向，可以用旋转矩阵、四元数、欧拉角等方式表示。
嗯，这里要注意：位置和姿态是绑在一起的。你不能只描述位置不管姿态，否则机械臂抓东西时，手爪方向不对，照样抓不起来。
核心概念：刚体运动 = 平移 + 旋转。平移用向量，旋转用矩阵或四元数。
4.2 旋转矩阵与四元数
旋转描述是运动学里最容易出bug的地方。我见过太多人因为欧拉角万向锁问题，导致机器人突然“翻跟头”。
4.2.1 旋转矩阵
旋转矩阵是一个3x3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的旋转关系。
比如，一个点p在坐标系A中的坐标是p_A，在坐标系B中的坐标是p_B，那么：
p_B = R_AB * p_A其中R_AB就是旋转矩阵，表示从A到B的旋转。
旋转矩阵有几个性质：
- R^T = R^{-1}（正交性）
- det(R) = 1（右手系）
- 三个列向量两两正交，且为单位向量
我在项目中遇到过一个问题：连续旋转时，旋转矩阵的累积误差会越来越大。因为浮点数运算不是精确的，多次乘法后矩阵会失去正交性。所以每隔一段时间，需要对旋转矩阵做一次“正交化”处理。
小技巧：用 Rodrigues 公式可以将旋转向量转换为旋转矩阵。旋转向量 = 旋转轴方向单位向量 × 旋转角度。
4.2.2 四元数
四元数是我个人比较偏爱的旋转表示方式。为什么？因为它没有万向锁问题，而且插值平滑。
四元数可以写成：q = w + xi + yj + zk，其中w是实部，[x, y, z]是虚部。或者更常见地表示为 q = [w, x, y, z]^T。
单位四元数（模长为1）可以用来表示旋转。给定一个旋转轴 u（单位向量）和旋转角度 θ，对应的四元数为：
q = [cos(θ/2), u_x * sin(θ/2), u_y * sin(θ/2), u_z * sin(θ/2)]我曾经踩过一个坑：四元数的乘法顺序。两个四元数 q1 和 q2 的复合旋转，顺序是 q = q1 * q2，表示先做 q2 旋转，再做 q1 旋转。这个顺序搞反了，结果就是天差地别。
避坑指南：四元数乘法不满足交换律！q1 * q2 ≠ q2 * q1。我刚开始写代码时，就因为搞混了顺序，让机械臂在空中画了个“8”字，差点撞到旁边的设备。
四元数转旋转矩阵的公式：
R = [[1-2(y²+z²), 2(xy-wz), 2(xz+wy)],
[2(xy+wz), 1-2(x²+z²), 2(yz-wx)],
[2(xz-wy), 2(yz+wx), 1-2(x²+y²)]]嗯，这个公式看着复杂，但写代码时直接套用就行。我建议把四元数运算封装成一个类，避免每次手写。
4.3 正运动学与逆运动学
这两个概念是运动学的核心。说白了：
- 正运动学：已知关节角度，求末端位姿。
- 逆运动学：已知末端位姿，求关节角度。
正运动学是“确定”的——给定一组关节角度，末端位姿是唯一确定的。逆运动学则麻烦得多，可能有多个解，甚至无解。
4.3.1 正运动学
正运动学通常用 Denavit-Hartenberg（D-H）参数法建立。每个关节用4个参数描述：
| 参数 | 含义 |
|---|---|
| a_i | 连杆长度 |
| α_i | 连杆扭转角 |
| d_i | 连杆偏距 |
| θ_i | 关节角 |
然后通过齐次变换矩阵连乘，得到末端相对于基座的位姿：
T_0n = T_01 * T_12 * ... * T_(n-1)n每个T矩阵是4x4的，包含旋转和位移信息。
我记得第一次手算6轴机械臂的正运动学时，整整算了一下午。后来发现用符号计算工具（比如SymPy）可以自动推导，省了不少时间。
4.3.2 逆运动学
逆运动学才是真正的挑战。对于6轴机械臂，逆运动学可能有：
- 0个解（目标点不可达）
- 1个解（边界情况）
- 2个解（肘部向上/向下）
- 4个解（肩部、肘部组合）
- 8个解（全组合）
我一般用解析法求解逆运动学，因为速度快、精度高。但对于冗余自由度机器人（比如7轴），就得用数值法了。
经验之谈：逆运动学求解时，一定要考虑关节限位。我曾经遇到过求解出的关节角度在数学上完全正确，但实际关节根本转不到那个位置。所以，加约束条件很重要。
4.4 雅可比矩阵
雅可比矩阵 J 描述了关节速度与末端速度之间的关系：
v = J * q_dot其中 v 是末端速度（包含线速度和角速度），q_dot 是关节速度。
雅可比矩阵的维度是 6×n，n是关节数。对于6轴机械臂，J是6×6的方阵。
雅可比矩阵有什么用？
- 速度控制：给定末端速度，反算关节速度。
- 奇异性分析：当J的行列式为0时，机器人处于奇异位形，某些方向会失去自由度。
- 力控制：通过力的对偶关系，关节力矩 τ = J^T * F。
我做过一个项目，需要在焊接过程中保持末端工具与焊缝的恒定速度。当时就是用雅可比矩阵的伪逆来求解关节速度，实现了实时轨迹跟踪。
避坑指南：雅可比矩阵在奇异点附近会变得病态，导致关节速度过大。我曾经在调试时没注意，结果机械臂在奇异点附近突然“抽搐”，关节电机差点过流保护。后来加了阻尼最小二乘法（DLS）才解决。
知识体系总览
下面这张图总结了本章的核心内容，你可以把它当作一个思维导图来看：
这张图把本章的四个核心模块串起来了。从刚体运动描述开始，到旋转表示方法，再到正逆运动学求解，最后用雅可比矩阵连接速度与力。每一步都是下一步的基础。
我个人觉得，运动学是具身智能里最“优雅”的部分——它用纯粹的数学描述了物理世界的运动规律。你把这些公式吃透了，后面做控制、做规划，都会顺手很多。
实践建议：找一款开源机器人（比如UR5、KUKA LBR iiwa），下载它的URDF文件，然后用Python的robotics库（比如Pinocchio、pybullet）搭建运动学模型。亲手算一遍正运动学和逆运动学，比看十遍书都管用。
公众号:蓝海资料掘金营，微信deep3321