MATLAB六自由度机器人运动学正解：D-H参数与齐次变换矩阵详解
前言：什么是机器人运动学正解？
本篇详细讲解机器人运动学正解（Forward Kinematics）的核心概念，帮助您理解正向运动学的原理，并掌握利用D-H参数求解末端位姿的方法。文章涵盖运动学正解的定义、齐次变换矩阵的数学推导以及Matlab代码实现，助你系统掌握这一机器人学基础。
下面进入正文，首先介绍正解的求解目标，然后逐步推导齐次变换矩阵，最后在Matlab中完成代码实现。
一、运动学正解（Forward Kinematics）定义与原理
运动学正解（Forward Kinematics）是指：已知机器人各关节的运动参数（例如关节角度或位移），求解末端执行器相对于基座坐标系的位置与姿态。简单来说，就是根据每个关节的运动状态，计算出机器人“手”在空间中的具体坐标和朝向。
求解过程通常分为三个步骤：首先建立各连杆之间的连接关系；然后计算相邻连杆间的齐次变换矩阵；最后将所有变换矩阵依次相乘，得到从末端执行器到基座的总变换矩阵。该矩阵中的变量仅为各关节的旋转角度，因此输入一组关节角度即可求得末端在笛卡尔空间的位置坐标。
1. 齐次变换矩阵（Homogeneous Transformation Matrix）
在上一篇文章建立六自由度机器人模型时，我们已详细讲解了D-H参数的定义与坐标轴设置方法。此处仅简要回顾关键符号：D-H参数包含四个量——θ（关节转角）、d（连杆偏移）、a（连杆长度）、α（连杆扭角）。对于标准型D-H参数，齐次变换矩阵通过四个基本变换依次相乘得到：先绕z轴旋转θ，再沿z轴平移d，接着沿x轴平移a，最后绕x轴旋转α。通用矩阵表达式如下：
^{i-1}T_i = Rot(z,θ_i) × Trans(z,d_i) × Trans(x,a_i) × Rot(x,α_i)
对于改进型D-H参数，变换顺序不同：先绕x轴转αi-1，再沿x轴平移ai-1，然后绕z轴转θi，最后沿z轴平移di。通用矩阵为：
^{i-1}T_i = Rot(x_i,α_{i-1}) × Trans(x_i,a_{i-1}) × Rot(z_i,θ_i) × Trans(z_i,d_i)
两种D-H参数约定的符号对照如下表：
| DH约定参数 | 符号约定 |
|---|---|
| θ (theta) | joint angle 关节转角 |
| d | link offset 连杆偏移 |
| a | link length 连杆长度 |
| α (alpha) | link twist 连杆扭角 |
具体而言，参数a表示沿x1轴方向测量的z0与z1之间的距离；角度α为垂直于x1的平面内z0与z1的夹角，其正方向由右手定则确定；d是从原点o0到轴线x1与z0交点的距离，沿z0轴测量；θ是在垂直于z0的平面内从x0到x1的旋转角度。掌握这些符号的含义后，即可写出每个关节的齐次变换矩阵。
2. 总变换矩阵（End-Effector Pose）
确定D-H参数的建立方式后，将其代入通用齐次变换矩阵公式，得到从基座到末端所有相邻连杆的变换矩阵：0T1、1T2、……、5T6。本文采用改进型D-H参数，各矩阵形式如下（符号含义：θi、αi、ai、di）：
0T1 = left[ begin{matrix} cosθ_1 & -sinθ_1 & 0 & a_1 \\ cosα_1 sinθ_1 & cosα_1 cosθ_1 & -sinα_1 & -d_1 sinα_1 \\ sinα_1 sinθ_1 & sinα_1 cosθ_1 & cosα_1 & d_1 cosα_1 \\ 0 & 0 & 0 & 1 \end{matrix} \right]
1T2 = left[ begin{matrix} cosθ_2 & -sinθ_2 & 0 & a_2 \\ cosα_2 sinθ_2 & cosα_2 cosθ_2 & -sinα_2 & -d_2 sinα_2 \\ sinα_2 sinθ_2 & sinα_2 cosθ_2 & cosα_2 & d_2 cosα_2 \\ 0 & 0 & 0 & 1 \end{matrix} \right]
……（类似地，2T3、3T4、4T5、5T6均按此模式）
将这六个矩阵按顺序依次相乘，即得到六自由度机器人的总变换矩阵：
0T6 = 0T1 × 1T2 × 2T3 × 3T4 × 4T5 × 5T6 = left[ begin{matrix} n_x & o_x & a_x & p_x \\ n_y & o_y & a_y & p_y \\ n_z & o_z & a_z & p_z \\ 0 & 0 & 0 & 1 \end{matrix} \right]
矩阵的前三列表示末端执行器的姿态（方向），最后一列的前三个元素即为末端的位置坐标（px, py, pz）。
二、Matlab代码实现：运动学正解函数
理论部分清晰后，在Matlab中编写代码便水到渠成。以下分两步实现：首先定义各连杆的D-H参数，然后构建齐次变换矩阵并计算总变换矩阵。
1. 定义各连杆D-H参数
定义参数有两种常用方法。第一种是将所有参数均设为符号变量，但这样得到的总变换表达式极其冗长，不便于观察。因此推荐第二种方法：将常数参数直接赋值，仅将关节角度θ保留为符号变量。具体参数值可参考上一篇文章中建立的机器人模型。
% 连杆偏移
d1 = 398;
d2 = -0.299;
d3 = 0;
d4 = 556.925;
d5 = 0;
d6 = 165;
% 连杆长度
a1 = 0;
a2 = 168.3;
a3 = 650.979;
a4 = 156.240;
a5 = 0;
a6 = 0;
% 连杆扭角
alpha1 = 0;
alpha2 = pi/2;
alpha3 = 0;
alpha4 = pi/2;
alpha5 = -pi/2;
alpha6 = pi/2;
% 关节角度保留为符号变量
syms theta1 theta2 theta3 theta4 theta5 theta62. 构建齐次变换矩阵并计算总变换
为方便引用，先将全部参数整合到一个矩阵中。注意：由于机器人在第二个关节处存在一个偏移量，因此theta2需增加pi/2。
% 参数矩阵取名为MDH
MDH = [theta1 d1 a1 alpha1;
theta2+pi/2 d2 a2 alpha2;
theta3 d3 a3 alpha3;
theta4 d4 a4 alpha4;
theta5 d5 a5 alpha5;
theta6 d6 a6 alpha6];然后利用改进型D-H的通用矩阵公式，依次定义六个齐次变换矩阵。代码结构简洁，但需注意矩阵的每一项都要正确引用MDH矩阵的对应列：第1列为θ，第2列为d，第3列为a，第4列为α。
T01 = [cos(MDH(1,1)) -sin(MDH(1,1)) 0 MDH(1,3);
sin(MDH(1,1))*cos(MDH(1,4)) cos(MDH(1,1))*cos(MDH(1,4)) -sin(MDH(1,4)) -sin(MDH(1,4))*MDH(1,2);
sin(MDH(1,1))*sin(MDH(1,4)) cos(MDH(1,1))*sin(MDH(1,4)) cos(MDH(1,4)) cos(MDH(1,4))*MDH(1,2);
0 0 0 1];
T12 = [cos(MDH(2,1)) -sin(MDH(2,1)) 0 MDH(2,3);
sin(MDH(2,1))*cos(MDH(2,4)) cos(MDH(2,1))*cos(MDH(2,4)) -sin(MDH(2,4)) -sin(MDH(2,4))*MDH(2,2);
sin(MDH(2,1))*sin(MDH(2,4)) cos(MDH(2,1))*sin(MDH(2,4)) cos(MDH(2,4)) cos(MDH(2,4))*MDH(2,2);
0 0 0 1];
% T23, T34, T45, T56 结构完全相同，只需更换MDH的行索引
% （此处省略，实际代码中依次写出即可）
最后，总变换矩阵即为六个矩阵的乘积：
T06 = T01 * T12 * T23 * T34 * T45 * T56;3. 代码运行结果与验证
下面给出一组具体的关节角度进行测试验证：
theta1 = pi/3;
theta2 = pi/4;
theta3 = pi/5;
theta4 = pi/3;
theta5 = pi/4;
theta6 = pi/5;运行代码后得到T06矩阵，其数值结果如下图所示（图1）。可利用上一篇建立的机器人模型中的robot.teach()函数进行仿真验证，末端位姿与计算结果完全吻合（图2）。
总结与下一步
运动学正解是机器人学中最基础也是最重要的模块之一，其核心在于通过D-H参数构建齐次变换矩阵，并连乘得到末端执行器的位置与姿态。在代码实现时，需特别注意参数的定义以及数学表达式的准确性，尤其是关节偏移量带来的角度补偿。掌握运动学正解后，后续的运动学逆解（反解）、轨迹规划等内容将更加顺利。
