4. 正运动学建模：D-H参数法、连杆坐标系建立、变换矩阵推导
正运动学，说白了就是：你告诉我每个关节转了多少角度，我告诉你末端执行器在哪儿、朝哪个方向。这是机器人学里最基础、也最绕不开的一块。
我刚开始做人形机器人那会儿，总觉得D-H参数法太死板，不就是四个参数来回倒腾嘛。后来在搭建一个双足平衡仿真时，坐标系没对齐，结果机器人走路直接劈叉——嗯，从那以后我再也不敢小看这步了。
4.1 为什么需要D-H参数法？
人形机器人有几十个自由度。每个关节都是一个旋转或平移副。如果每个连杆都单独建坐标系，那变换矩阵会乱成一锅粥。
D-H参数法（Denavit-Hartenberg）提供了一套标准化流程：
- 用四个参数描述相邻连杆的相对位姿
- 参数只与连杆几何和关节类型有关
- 推导出的变换矩阵形式统一，便于编程
核心思想：把复杂的空间运动链，拆解成一系列「绕轴转、沿轴移」的基本操作。每个操作对应一个4×4齐次变换矩阵。
4.2 四个D-H参数的定义
先记住这四个参数的名字和含义。我习惯把它们分成两组：
| 参数 | 符号 | 物理含义 | 关节变量？ |
|---|---|---|---|
| 连杆长度 | ai | 沿Xi轴，从Zi-1到Zi的距离 | 否（固定） |
| 连杆扭角 | αi | 绕Xi轴，从Zi-1到Zi的转角 | 否（固定） |
| 连杆偏距 | di | 沿Zi-1轴，从Xi-1到Xi的距离 | 旋转关节：固定；移动关节：变量 |
| 关节转角 | θi | 绕Zi-1轴，从Xi-1到Xi的转角 | 旋转关节：变量；移动关节：固定 |
我的记忆口诀：「先绕Z转θ，再沿Z移d，然后沿X移a，最后绕X转α」。每次写变换矩阵前，我都在心里默念一遍这个顺序。
4.3 连杆坐标系的建立规则
坐标系怎么摆？有标准流程。你按这个来，基本不会出错：
- 找Z轴：Zi-1轴沿着关节i的轴线方向。旋转关节就是旋转轴，移动关节就是移动方向。
- 找原点：原点Oi-1在Zi-1与Zi的公垂线与Zi-1的交点上。
- 找X轴：Xi-1沿着公垂线方向，从Zi-1指向Zi。
- Y轴：右手定则补齐，Yi-1 = Zi-1 × Xi-1。
你想想看，如果两个Z轴平行，公垂线就不唯一了。这时候怎么选？我一般选经过前一个坐标系原点的那个公垂线，这样di就是0，计算能省一步。
我曾经踩过的坑：在建立人形机器人髋关节坐标系时，忘了区分左右腿的扭角符号。结果左腿正转、右腿反转，仿真里机器人直接原地转圈。后来我养成了习惯——画完坐标系后，用右手法则验证一遍所有Z轴和X轴的方向。
4.4 相邻连杆的变换矩阵
有了四个参数，变换矩阵就是机械式的代入。标准形式如下：
i-1T_i = Rot(z, θ_i) · Trans(z, d_i) · Trans(x, a_i) · Rot(x, α_i)
写成4×4矩阵：
[ cosθ_i -sinθ_i·cosα_i sinθ_i·sinα_i a_i·cosθ_i ]
[ sinθ_i cosθ_i·cosα_i -cosθ_i·sinα_i a_i·sinθ_i ]
[ 0 sinα_i cosα_i d_i ]
[ 0 0 0 1 ]
这个矩阵，我建议你手推一遍。别只看书。我自己推过三遍，每次都有新理解。第一遍是上学时，第二遍是写第一个机器人控制器时，第三遍是教徒弟时——每次都能发现之前忽略的细节。
4.5 正运动学链式计算
从基座到末端，把每个相邻变换矩阵乘起来：
0T_n = 0T_1 · 1T_2 · 2T_3 · ... · n-1T_n结果是一个4×4齐次变换矩阵：
- 左上3×3：末端坐标系相对于基座的旋转矩阵
- 右上3×1：末端坐标系原点在基座坐标系中的位置
- 最后一行：[0 0 0 1]
实战要点：人形机器人通常有两条腿、两条胳膊、一个躯干。正运动学要分别计算每条运动链。我一般把躯干作为公共基座，左腿、右腿、左臂、右臂各算各的。这样模块化，调试起来方便。
4.6 知识体系总览
下面这张图，是我做课程时画的。它把D-H参数法的整个流程串起来了：
4.7 实战：人形机器人腿部D-H参数表
拿人形机器人的左腿举个例子。从髋关节到踝关节，一般有3个自由度：髋关节俯仰、髋关节滚转、膝关节俯仰。我简化一下，只考虑矢状面内的运动：
| 连杆 i | ai (mm) | αi (rad) | di (mm) | θi (变量) | 关节说明 |
|---|---|---|---|---|---|
| 1 | 0 | π/2 | 0 | θ1 | 髋关节俯仰 |
| 2 | Lthigh | 0 | 0 | θ2 | 膝关节俯仰 |
| 3 | Lshank | 0 | 0 | θ3 | 踝关节俯仰 |
把参数代入标准矩阵，然后链式相乘。你就能得到脚掌相对于髋关节的位姿。这个结果，是后面做步态规划的基础。
一个小技巧：写代码实现时，别一次性把整个矩阵乘出来。我习惯先算位置向量，再算旋转矩阵。这样中间结果可以打印出来检查，哪里错了一目了然。
4.8 验证你的正运动学
怎么知道算对了？我一般用两个方法：
- 零位验证：所有关节角度为0时，末端应该在初始位置。比如腿伸直时，脚掌应该在髋关节正下方。
- 单关节验证：只动一个关节，看末端轨迹是不是一个圆弧（旋转关节）或直线（移动关节）。
这两个验证通过了，你的正运动学基本就稳了。剩下的就是调参数、优化性能。
嗯，正运动学这块，说白了就是「给角度，求位姿」。公式是死的，但坐标系怎么摆、参数怎么取，里面有不少经验。希望上面这些内容，能帮你少走我当年走过的弯路。