第2章正运动学

第2章：正运动学——D-H参数法建立连杆坐标系、正运动学方程推导、MATLAB/Simulink仿真验证

各位同学，欢迎来到正运动学这一章。

说实话，正运动学是机器人学里最基础、也最绕不开的一块内容。你想想看，我们想让机器人动起来，首先得知道：给定每个关节的角度，末端到底在哪儿？ 这就是正运动学要解决的问题。

我个人习惯把正运动学比作「搭积木」。每个关节、每根连杆，都是一块积木。我们只需要按照固定的规则，把积木一块块拼起来，末端的位置和姿态自然就出来了。这个规则，就是今天的主角——D-H参数法。

核心思想： 用四个参数（a, α, d, θ）唯一确定相邻两个连杆之间的坐标变换关系。然后通过矩阵连乘，得到从基座到末端的完整变换矩阵。

2.1 D-H参数法：建立连杆坐标系

D-H参数法，说白了就是一套「标准化」的建系流程。我刚开始学的时候，总觉得它很死板，后来在项目中才发现，正是这种死板，才让代码变得通用、可靠。

我们以最常见的六轴工业机器人为例。每个关节 i 对应一个连杆 i-1。建立坐标系时，遵循以下规则：

Z轴：沿关节 i 的轴线方向。
X轴：沿相邻两Z轴的公垂线，方向从 Z_i-1 指向 Z_i。
Y轴：由右手定则确定。

嗯，这里要注意：如果相邻两Z轴相交，X轴方向取两Z轴的叉积方向。

建好坐标系后，四个D-H参数的定义如下：

参数	符号	物理含义	我的理解
连杆长度	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 的转角	这就是我们实际控制的关节角度

我的小技巧： 在纸上画坐标系时，先用粗线画出Z轴，再用细线画出X轴。这样视觉上更清晰，不容易搞混。我曾经在项目里因为Z轴方向画反，导致仿真结果完全不对，排查了整整一个下午。

2.2 正运动学方程推导

坐标系建好了，参数也定了，接下来就是数学推导。其实没那么可怕，就是矩阵乘法。

相邻两个坐标系 {i-1} 和 {i} 之间的变换矩阵，可以写成：

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

展开成4x4矩阵形式：

       | 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} |
T_i =  | sinθ_i·sinα_{i-1}  cosθ_i·sinα_{i-1}   cosα_{i-1}   d_i·cosα_{i-1} |
       | 0         0         0       1        |

看着有点复杂对吧？别急，我们只需要记住这个矩阵的结构就行。实际编程时，直接套公式。

对于n轴机器人，从基座到末端的变换矩阵就是：

T_0n = T_1 · T_2 · T_3 · ... · T_n

这个矩阵的左上角3x3部分，就是末端的姿态（旋转矩阵）；右上角3x1部分，就是末端的位置（平移向量）。

避坑指南： 我曾经在推导六轴机器人时，把T_1和T_2的顺序搞反了。记住：矩阵乘法不满足交换律！顺序一定是T_1乘T_2，不是T_2乘T_1。

2.3 知识体系与核心逻辑

为了让大家更直观地理解本章的知识脉络，我画了一张流程图：

这张图把整个流程串起来了：从关节角度出发，查D-H参数表，做矩阵连乘，最后得到末端位姿。每一步都很清晰。

2.4 MATLAB/Simulink仿真验证

理论推导完了，不上手验证一下，总觉得不踏实。我个人习惯先用MATLAB写脚本，再搭Simulink模型。

下面是一个简单的MATLAB示例，以2轴平面机器人为例：

% 正运动学计算 - 2轴平面机器人
% D-H参数：a1=1, a2=0.8, 均为旋转关节

function T = fk_2r(theta1, theta2)
    % 定义D-H参数
    a1 = 1.0;   % 连杆1长度
    a2 = 0.8;   % 连杆2长度
    
    % 关节角度（弧度）
    th1 = theta1;
    th2 = theta2;
    
    % 变换矩阵 T1（基座到关节1）
    T1 = [cos(th1), -sin(th1), 0, a1*cos(th1);
          sin(th1),  cos(th1), 0, a1*sin(th1);
          0,         0,        1, 0;
          0,         0,        0, 1];
    
    % 变换矩阵 T2（关节1到关节2）
    T2 = [cos(th2), -sin(th2), 0, a2*cos(th2);
          sin(th2),  cos(th2), 0, a2*sin(th2);
          0,         0,        1, 0;
          0,         0,        0, 1];
    
    % 总变换矩阵
    T = T1 * T2;
end

% 测试：theta1=30°, theta2=45°
T_end = fk_2r(deg2rad(30), deg2rad(45));
disp('末端位置：');
disp(T_end(1:3, 4)');

运行这段代码，你会得到末端的位置坐标。你可以手动算一下，验证结果对不对。

我的经验： 在Simulink中做仿真时，我习惯用MATLAB Function模块封装正运动学函数。这样既保留了MATLAB的灵活性，又能利用Simulink的可视化优势。你可以在Scope里实时观察末端轨迹，非常直观。

Simulink的搭建思路很简单：输入两个关节角度（用Sine Wave或Constant模块），接入MATLAB Function，输出末端位置，再用Scope显示。整个过程不超过10分钟。

嗯，说到这里，我想起一个细节。有一次我在Simulink里仿真，发现末端位置一直在跳变。排查了半天，原来是角度单位搞错了——MATLAB Function里用的是弧度，但输入给的是角度。这种低级错误，犯过一次就再也不会忘了。

2.5 本章小结

正运动学，说白了就是「给角度，求位姿」。D-H参数法给了我们一套标准化的工具，矩阵连乘给了我们数学上的解法，MATLAB/Simulink给了我们验证的手段。

你想想看，掌握了这些，任何串联机器人的正运动学问题，都只是查表和算矩阵而已。是不是感觉也没那么难？

下一章，我们会聊逆运动学——也就是「给位姿，求角度」。那才是真正考验算法功底的地方。不过别担心，先把正运动学吃透，逆运动学就是水到渠成的事。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录