第二章：运动学基础——正运动学与逆运动学、D-H参数法、齐次变换矩阵

各位同学，欢迎来到运动学基础这一章。说实话，运动学是整个机器人控制的基石。你想想看，如果连机器人末端在哪儿、关节该怎么转都算不清楚，那后面的轨迹跟踪、力控制根本无从谈起。我在做第一个双足机器人项目时，就吃过这个亏——当时急着调步态，结果运动学模型里一个符号搞反了，机器人直接原地劈叉。嗯，从那以后，我再也不敢轻视运动学建模了。

2.1 从空间描述说起：位置与姿态

要描述一个刚体在空间中的状态，我们需要两样东西：位置和姿态。位置好理解，就是三维坐标 (x, y, z)。但姿态呢？说白了就是“它朝哪个方向”。

我个人习惯用旋转矩阵来描述姿态。一个 3x3 的旋转矩阵 R，它的每一列代表刚体坐标系在参考坐标系下的单位方向向量。举个例子，如果刚体坐标系和参考坐标系完全对齐，那 R 就是单位矩阵。

核心概念：一个刚体的完整位姿 = 位置向量 p + 旋转矩阵 R。合在一起，就是一个 4x4 的齐次变换矩阵。

2.2 齐次变换矩阵：把旋转和平移打包

为什么要用齐次变换矩阵？因为方便。你想想看，如果我们要把点从一个坐标系变换到另一个坐标系，先旋转再平移，写出来是：

p' = R * p + t

这式子本身没问题，但如果你要连续变换多次，比如从基座到肩关节、再到肘关节、再到腕关节……每次都要写一个旋转加平移，非常啰嗦。齐次变换矩阵把旋转和平移合并成一个 4x4 矩阵：

T = | R   t |
    | 0   1 |

这样，连续变换就变成了矩阵乘法：T_total = T1 * T2 * T3 * ...。我在项目中遇到过，用齐次矩阵做链式变换，代码写起来特别清爽，而且不容易出错。

小技巧：写代码时，我习惯用 Eigen 或 NumPy 的矩阵类来管理齐次变换。手动算矩阵乘法太容易出错了，尤其是符号。

2.3 D-H参数法：机器人建模的标准语言

说到运动学建模，就绕不开 D-H 参数法。Denavit 和 Hartenberg 这两位前辈在 1955 年提出了一套标准流程，用四个参数来描述相邻两个连杆之间的相对位姿：

参数	符号	含义
连杆长度	a_i-1	沿 x_i-1 轴，从 z_i-1 到 z_i 的距离
连杆扭转角	α_i-1	绕 x_i-1 轴，从 z_i-1 到 z_i 的转角
连杆偏距	d_i	沿 z_i 轴，从 x_i-1 到 x_i 的距离
关节角	θ_i	绕 z_i 轴，从 x_i-1 到 x_i 的转角

有了这四个参数，相邻连杆的变换矩阵就可以写成标准形式。我建议你把这个矩阵背下来，或者至少能快速推导出来：

T_i = Rot_x(α_{i-1}) * Trans_x(a_{i-1}) * Rot_z(θ_i) * Trans_z(d_i)

展开后就是：

T_i = | cosθ_i   -sinθ_i   0    a_{i-1} |
      | sinθ_i*cosα_{i-1}  cosθ_i*cosα_{i-1}  -sinα_{i-1}  -d_i*sinα_{i-1} |
      | sinθ_i*sinα_{i-1}  cosθ_i*sinα_{i-1}   cosα_{i-1}   d_i*cosα_{i-1} |
      | 0          0          0    1 |

注意：D-H 参数有两种约定——标准 D-H 和改进 D-H。区别在于坐标系附着的位置不同。我个人习惯用改进 D-H（也叫 Craig 约定），因为它在处理树状结构时更直观。但无论用哪种，一定要在项目里统一，否则会乱套。

2.4 正运动学：从关节空间到工作空间

正运动学很简单：给定一组关节角度，求末端位姿。说白了就是“把 D-H 矩阵乘起来”。

对于一个人形机器人，我们通常把每条腿和每条手臂都看作一个运动链。比如右腿：髋关节（3个自由度）→ 膝关节（1个自由度）→ 踝关节（2个自由度）。总共 6 个自由度，刚好决定脚掌的位姿。

正运动学的计算流程：

建立各连杆的 D-H 参数表
写出每个关节的变换矩阵 T_i
从基座开始，依次左乘：T_0_n = T_1 * T_2 * ... * T_n
提取结果矩阵中的旋转部分 R 和平移部分 p

我在项目中遇到过，正运动学算出来之后，一定要做一次“合理性检查”。比如把关节角设为零位，看看末端是不是在预期的初始位置。如果不对，八成是 D-H 参数表里的符号搞反了。

2.5 逆运动学：从工作空间到关节空间

逆运动学就麻烦多了。给定末端位姿，求关节角度。说白了就是“解方程”。

为什么难？因为：

解可能不存在（超出工作空间）
解可能不唯一（多解）
解可能不连续（奇异点）

对于人形机器人，逆运动学通常用数值方法求解。我最常用的是雅可比伪逆法：

Δq = J⁺ * Δx

其中 J⁺ 是雅可比矩阵的伪逆，Δx 是末端位姿误差，Δq 是关节角修正量。迭代直到误差小于阈值。

避坑指南：我曾经在逆运动学迭代时没加阻尼项，结果在奇异点附近关节速度直接飞了。后来加了 Levenberg-Marquardt 阻尼，问题就解决了。代码里加一行 lambda 的事，但能救你一命。

2.6 知识体系总览

下面这张图是我自己整理的本章知识结构。你可以把它当作一个“地图”，学完本章后对照着检查自己是否都掌握了。

2.7 本章小结

这一章我们讲了三个核心内容：

齐次变换矩阵：把旋转和平移统一成一个 4x4 矩阵，方便链式运算
D-H 参数法：用四个参数标准化描述连杆关系，是机器人建模的通用语言
正/逆运动学：正运动学是矩阵乘法，逆运动学是解方程（通常用数值法）

说实话，运动学本身并不难，难的是不出错。我建议你每建一个模型，都先用正运动学算几个特殊位姿验证一下。等确认 D-H 参数和矩阵都对了，再去做逆运动学和轨迹规划。否则，后面所有的调试都是在浪费时间。

一句话总结：运动学是机器人控制的“坐标系”，没有它，你连机器人在哪儿都不知道。

可可精品资料库

📚 课程目录

第二章：运动学基础——正运动学与逆运动学、D-H参数法、齐次变换矩阵

2.1 从空间描述说起：位置与姿态

2.2 齐次变换矩阵：把旋转和平移打包

2.3 D-H参数法：机器人建模的标准语言

2.4 正运动学：从关节空间到工作空间

2.5 逆运动学：从工作空间到关节空间

2.6 知识体系总览

2.7 本章小结