4、运动学基础（二）：标准D-H参数法、建立机械臂连杆坐标系、正运动学求解
好，咱们接着聊。上一章我们把刚体变换和齐次矩阵讲透了，那都是基本功。这一章要动真格的了——怎么用一套标准的方法，把机械臂的“骨架”描述清楚，然后算出它的末端到底在哪儿。
说白了，D-H参数法就是给机械臂的每个关节和连杆“上户口”。你给它四个参数，它就能告诉你，这根杆子相对于上一根杆子是怎么摆的。我当年第一次接触这个，觉得不就是四个数嘛，有啥了不起？后来真上手做项目才发现，这四个数要是定错了，后面所有计算全白搭。
4.1 标准D-H参数法：四个参数定乾坤
标准D-H参数法，核心就是四个参数。记住，是四个，不是五个，也不是三个。它们分别是：
- 连杆长度 ai-1：从 Zi-1 轴到 Zi 轴，沿 Xi-1 轴方向测量的距离。说白了，就是两根关节轴之间的“最短距离”。
- 连杆扭角 αi-1：从 Zi-1 轴到 Zi 轴，绕 Xi-1 轴旋转的角度。这个角度决定了两个关节轴是平行、垂直还是歪着。
- 连杆偏距 di：从 Xi-1 轴到 Xi 轴，沿 Zi 轴方向测量的距离。对于移动关节，这个值就是变量。
- 关节角 θi：从 Xi-1 轴到 Xi 轴，绕 Zi 轴旋转的角度。对于旋转关节，这个值就是变量。
你可能会问，为什么是这四个？其实这是经过无数前辈验证的，最少参数就能完整描述两个相邻连杆之间的相对位置和姿态。我在做六轴机器人标定时，曾经试过用六个参数，结果冗余得一塌糊涂，最后还是老老实实回到D-H。
核心口诀：先沿 X 轴平移 a，再绕 X 轴转 α，然后沿 Z 轴平移 d，最后绕 Z 轴转 θ。顺序不能乱，乱了坐标系就全错了。
4.2 建立连杆坐标系：从图纸到数学
有了参数，下一步就是给每个连杆建坐标系。这一步特别容易出错，我建议你按下面的步骤来，别跳步。
- 找关节轴：先画出所有关节的旋转轴（Z轴）。记住，Zi 轴沿着关节 i+1 的运动方向。
- 定原点：Zi 轴和 Zi-1 轴的公垂线与 Zi 轴的交点，就是坐标系 {i} 的原点。
- 定 X 轴：Xi 轴沿着公垂线方向，从 Zi-1 指向 Zi。
- 定 Y 轴：右手定则，Yi = Zi × Xi。
嗯，这里要注意一个特殊情况：如果两个 Z 轴平行，那公垂线就不唯一了。我个人的习惯是，取经过前一个坐标系原点的公垂线，这样计算最方便。
下面这张图，是我用 SVG 画的，展示了标准D-H参数法中相邻连杆坐标系的定义关系。你看一眼就能明白这四个参数是怎么“量”出来的。
4.3 正运动学求解：从关节空间到操作空间
坐标系建好了，D-H参数也填好了，接下来就是机械臂的“终极一问”：给定六个关节角度，末端到底在哪儿？
正运动学求解，说白了就是连乘。把每个相邻连杆的变换矩阵乘起来，就得到了从基座到末端的总变换矩阵。
相邻连杆的变换矩阵公式长这样：
i-1T_i = Rot_x(α_{i-1}) · Trans_x(a_{i-1}) · Rot_z(θ_i) · Trans_z(d_i)展开成矩阵形式就是：
i-1T_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 |看着有点吓人，对吧？其实你仔细看，左上角3x3是旋转矩阵，右上角3x1是平移向量。我当年学的时候，硬是把这个矩阵抄了十遍，抄到闭着眼睛都能写出来。
我的小技巧：别死记硬背。你只要记住变换顺序是“先转α，再移a，再转θ，再移d”，然后按这个顺序去乘对应的基本变换矩阵，结果自然就出来了。我在项目里从来都是现场推导，从不背公式。
4.4 实战：六轴机械臂的D-H参数表
光说不练假把式。咱们拿一个典型的六轴工业机器人来举例。假设它的结构是：RRRRRR（六个旋转关节）。
建好坐标系后，填出来的D-H参数表大概长这样：
| 连杆 i | ai-1 | αi-1 | di | θi |
|---|---|---|---|---|
| 1 | 0 | 0° | d1 | θ1 |
| 2 | a1 | -90° | 0 | θ2 |
| 3 | a2 | 0° | 0 | θ3 |
| 4 | a3 | -90° | d4 | θ4 |
| 5 | 0 | 90° | 0 | θ5 |
| 6 | 0 | -90° | d6 | θ6 |
有了这张表，正运动学就是机械的矩阵乘法。把每个 i-1T_i 算出来，然后：
0T_6 = 0T_1 · 1T_2 · 2T_3 · 3T_4 · 4T_5 · 5T_6结果矩阵的左上角3x3就是末端的姿态，右上角3x1就是末端的位置。搞定。
避坑指南：我曾经在做一个协作机器人项目时，因为把 α 的符号搞反了，导致仿真里机器人末端飞到了天花板外面。查了两天才发现是正负号的问题。所以，填参数表时，α 的方向一定要和右手定则一致，别想当然。
4.5 验证你的正运动学
算完了怎么知道对不对？我教你三个土办法：
- 零位验证：把所有关节角设成0，看看末端位置是不是在机械臂的“伸直”状态。如果不在，那肯定错了。
- 单关节运动：只动一个关节，看末端轨迹是不是一个圆。如果是，说明这个关节的变换矩阵对了。
- 与实物对照：拿尺子量一下机械臂在某个姿态下的实际位置，和计算结果对比。误差应该在毫米级。
嗯，其实说白了，正运动学就是一套“按图索骥”的功夫。只要坐标系建对了，参数填准了，剩下的就是耐心地做矩阵乘法。别怕麻烦，我刚开始做的时候，每个矩阵都手算验证，算到第三遍才敢用。
这一章的内容就到这儿。记住，D-H参数法是机械臂运动学的基石，后面所有的轨迹规划、动力学、控制，都建立在这四个参数之上。把基础打牢，后面才能走得远。
可可精品资料库