3、正运动学求解：D-H参数与末端位姿推导

正运动学，说白了就是：给定关节角度，算出末端在哪儿、朝哪个方向。这是机器人学里最基础、也最绕不开的一块。我刚入行时觉得这玩意儿太理论，直到第一次在产线上调机器人——嗯，不懂正运动学，连示教器上的坐标都看不懂。

这一章，我们就把D-H参数法彻底讲透。我会带着你一步步推导，再上代码验证，最后看看几种常见工业机器人的模型长什么样。

3.1 D-H参数：四个数字描述一个关节

D-H参数法，是Denavit和Hartenberg两位老哥在1955年提出的。核心思想很简单：用四个参数，描述相邻两个坐标系之间的变换。

这四个参数分别是：

3.2 从参数到变换矩阵

有了四个参数，相邻坐标系间的变换矩阵长这样：

T = Rot_x(α) · Trans_x(a) · Rot_z(θ) · Trans_z(d)

展开成4×4矩阵：

| cosθ    -sinθ    0     a |
| sinθ·cosα  cosθ·cosα  -sinα  -d·sinα |
| sinθ·sinα  cosθ·sinα   cosα   d·cosα |
| 0        0        0     1 |

这个矩阵，就是相邻连杆坐标系之间的齐次变换矩阵。我建议你亲手推导一遍，别只看书。当年我在学校手推了三遍才真正理解每个元素的意义。

3.3 末端位姿：连乘就对了

对于n轴机器人，末端坐标系相对于基坐标系的变换矩阵，就是所有相邻变换矩阵的乘积：

T_0n = T_01 · T_12 · T_23 · ... · T_(n-1)n

这个T_0n矩阵，包含了末端的位置和姿态信息：

• 位置：矩阵右上角的3×1向量 (p_x, p_y, p_z)^T
• 姿态：矩阵左上角的3×3旋转矩阵，或者转换成欧拉角/四元数

3.4 常见工业机器人D-H参数模型

我们来看三种最常见的构型。这些模型我在项目里都亲手调过，每个都有坑。

3.4.1 六轴串联机器人（如KUKA、ABB）

标准6R构型，D-H参数表（以某型号为例）：

3.4.2 SCARA机器人

SCARA的D-H参数更简单，因为所有关节轴都平行：

SCARA的末端位置计算比六轴简单得多，因为所有旋转轴都平行于Z轴。但要注意：关节3是移动关节，d₃是变量。

3.4.3 协作机器人（如UR5）

UR5的D-H参数比较特殊，它的坐标系定义方式跟传统六轴不太一样：

3.5 MATLAB/Python仿真验证

理论推导完了，得上手验证。我习惯先用MATLAB的Robotics Toolbox快速验证，再用Python写一遍，确保理解透彻。

3.5.1 MATLAB实现

% 定义D-H参数（以六轴机器人为例）
% [a, alpha, d, theta]
dh_params = [
    0,      0,      400,    0;
    300,    -pi/2,  0,      0;
    700,    0,      0,      0;
    150,    -pi/2,  700,    0;
    0,      pi/2,   0,      0;
    0,      -pi/2,  100,    0
];

% 创建机器人模型
robot = SerialLink(dh_params, 'name', '六轴机器人');

% 设定关节角度（单位：弧度）
q = [0, -pi/4, pi/4, 0, pi/6, 0];

% 计算末端位姿
T = robot.fkine(q);

% 显示结果
disp('末端位姿矩阵：');
disp(T);
disp('末端位置：');
disp(T.t);
disp('末端姿态（旋转矩阵）：');
disp(T.R);

3.5.2 Python实现

import numpy as np
import roboticstoolbox as rtb
from spatialmath import SE3

# 定义D-H参数
# 使用标准D-H约定
robot = rtb.DHRobot([
    rtb.RevoluteDH(a=0, alpha=0, d=400, offset=0),
    rtb.RevoluteDH(a=300, alpha=-np.pi/2, d=0, offset=0),
    rtb.RevoluteDH(a=700, alpha=0, d=0, offset=0),
    rtb.RevoluteDH(a=150, alpha=-np.pi/2, d=700, offset=0),
    rtb.RevoluteDH(a=0, alpha=np.pi/2, d=0, offset=0),
    rtb.RevoluteDH(a=0, alpha=-np.pi/2, d=100, offset=0)
], name='六轴机器人')

# 设定关节角度
q = np.array([0, -np.pi/4, np.pi/4, 0, np.pi/6, 0])

# 计算正运动学
T = robot.fkine(q)

print("末端位姿矩阵：")
print(T)
print("\n末端位置：")
print(T.t)
print("\n末端姿态（旋转矩阵）：")
print(T.R)

3.6 正运动学知识体系

下面这张图，把正运动学的核心逻辑串起来了：

3.7 常见问题与避坑指南

• 坐标系定义混乱：不同厂家、不同文献用的D-H约定可能不一样。我建议统一用标准D-H（Craig版），别混着用。
• α的正负号：这是最容易出错的地方。画个坐标系草图，用右手定则确认一下。
• d值的符号：移动关节的d值可正可负，取决于坐标系方向。我曾经在SCARA项目里把d₃的符号搞反，末端上下运动完全反了。
• 零位校准：仿真和实物对不上时，先检查零位。把机器人调到机械零位，看仿真结果是否一致。

正运动学是机器人学的基石。你想想看，如果连末端在哪儿都算不准，后面的轨迹规划、力控算法全是空中楼阁。这一章的内容，我建议你亲手推导一遍，再用代码跑一遍，最后拿实物验证一遍。三遍下来，基本就吃透了。