机械臂标准DH建模及正运动学分析（以IRB4600型工业机械臂为例）

1. 前言

对于工业机械臂而言，运动学是不考虑力学特性的情况下对机械臂的几何参数与其位置、速度、加速度等运动特性的关系研究。DH建模是运动学的基础，全称为Denavit-Hartenberg建模方法，是一种广泛应用于机器人运动学中的建模技术。该方法通过在每个连杆上建立坐标系，并利用齐次坐标变换来实现连杆之间的坐标转换。通过依次变换，可以推导出末端执行器相对于基坐标系的位姿，从而建立机器人的运动学方程。
一般DH建模分为标准DH法（SDH）和改进DH法（MDH），这两种方法对于串联开环的机械臂结构来说没有什么影响，只是建模方式略有不同，SDH将坐标系建立在机械臂连杆末端，而MDH则将坐标系建立在连杆首段。两者之间的异同点另开篇阐述，这里将以IRB4600型六自由度工业机械臂为例，选择标准DH建模方法，建立其正运动学模型。

2. 标准DH建模

2.1 连杆坐标系

标准DH模型坐标系建立规则可以概括如下几个步骤。
（1）所有关节，无一例外用z轴表示。如果是关节是旋转的，z轴位于按右手定则选装的方向，如果关节是滑动的，z轴为沿实现运动的方向。在每一种情况下，关节i处的z轴（以及该关节的本地参考坐标系）的下标为i-1。
（2）通常关节不一定平行或相交。因此，通常z轴是斜线，但是总有一条距离最短的公垂线，它正交于任意两条斜线。通常在公垂线方向上定义x轴。所以如果 a_i表示 z_i−1 与 z_i 之间的公垂线，则 x_i的方向沿 a_i。
（3）如果两个关节的z轴平行，选取与前一关节的公垂线共线的一条公垂线；如果两个相邻关节的z轴是相交的，那么它们之间没有公垂线，可选取两条z轴的叉积方向作为x轴。
（4）根据右手坐标系原则确定y轴方向
结合IRB4600-20/2.50型6轴机器人的机械结构，建立关于IRB4600-20/2.50型机器人的连杆坐标系。如图1所示。
在这里插入图片描述

图1.IRB400-20/2.50机器人连杆坐标系

2.2 DH参数表

连杆坐标系确立后，就可以根据连杆坐标系的位置相对关系来确定用于指示坐标系{i-1} 和坐标系{i}间相对位置和方位的4个几何参数。四个参数的定义如下：
a_i：在连杆坐标系中，沿着x_i轴，从z_i轴到z_i+!轴平移的距离；
α_i：在连杆坐标系中，绕着x_i轴，从z_i轴到z_i+!轴转过的角度；
d_i：在连杆坐标系中，沿着z_i轴，从x_i轴到x_i+1轴平移的距离；
θ_i：在连杆坐标系中，绕着z_i轴，从x_i轴到x_i+1轴转过的角度。
根据前面建立的连杆坐标系和建模规则，可分析IRB4600-20/2.50型六自由度工业机器人的运动学几何参数，结合工业机器人各个部件的出厂参数得出具体数据，如表1所示。

表1 连杆参数表

连杆	a_i(mm)	d_i(mm)	α_i(°)	θ_i(°)	关节运动范围
1	170	495	-90	0	-180-180
2	1095	0	0	-90	-90-180
3	175	0	-90	0	-180-75
4	0	1230.5	90	0	-400-400
5	0	0	90	-180	-125-125
6	0	85	0	0	-400-400

3. 正运动学分析

根据空间坐标转换的原理，坐标里两个坐标系的变换可以用齐次变换矩阵的乘积形式来描述：
在这里插入图片描述

基于IRB4600型机器人连杆坐标系和几何参数，可得连杆坐标变换的通用公式：
在这里插入图片描述

其中，矩阵的前3*3部分表示的是两个相邻坐标系之间的旋转变换，最后一列表示的两个相邻坐标系之间的平移变换，对于n个自由度的机器人，可通过上述规则建立各个关节的变换矩阵，这样就可实现工业机器人从基坐标系到任意一个连杆坐标系变换。
根据给出的关节参数和各个关节角度，然后计算机器人末端连杆在基坐标系中所处的位置和姿态，这个过程就叫做运动学正解过程。依照标准D-H建模原则针对IRB4600型机器人建立连杆坐标系之间的坐标变换模型后，再将表1中的连杆参数代入机器人齐次变换矩阵的通式中，可得 6 个相邻坐标系之间的齐次变换矩阵：
在这里插入图片描述

机器人末端法兰盘中心相对于机器人基坐标系的位姿可以由式1.3中6个齐次变换矩阵依次相乘得到如下矩阵：
在这里插入图片描述

其中：在这里插入图片描述
矩阵₆⁰T表示机器人末端连杆坐标系相对于机器人基坐标系的位置和姿态，其中子阵N表示末端连杆坐标系相对于机器人基坐标系的姿态信息，子阵P表示末端连杆坐标系相对于机器人基坐标系的位置信息，该矩阵即是机器人运动学的正解。

4. matlab运动学仿真

六自由度工业机器人运动学分析的正逆解算是在矩阵运算的基础上进行数据处理计算，利用matlab分别通过公式推导和robotic toolbox工具箱建立机器人模型，关键代码如下。
工具箱代码：

clc;
clear;
%          thetai       di      ai-1     alphai-1
L1 = Link([0            495     175      -pi/2  ],'standard');
L2 = Link([0            0       1095      0     ],'standard');L2.offset=-pi/2;
L3 = Link([0            0       175      -pi/2  ],'standard');
L4 = Link([0            1230.5  0        pi/2   ],'standard');
L5 = Link([0            0       0        pi/2   ],'standard');L5.offset=-pi;
L6 = Link([0            85      0        0      ],'standard');L1.qlim =[-180*pi/180, 180*pi/180];
L2.qlim =[-90*pi/180, 150*pi/180];
L3.qlim =[-180*pi/180, 75*pi/180];
L4.qlim =[-400*pi/180, 400*pi/180];
L5.qlim =[-125*pi/180, 125*pi/180];
L6.qlim =[-400*pi/180, 400*pi/180];IRB4600=SerialLink([L1 L2 L3 L4 L5 L6], 'name', 'IRB4600');
IRB4600.display();
IRB4600.teach;

代码中使用Link函数设置机器人连杆，qlim函数限制关节变量的最大极限值和最小极限值，形成每个关节的运动范围，SerialLink函数连接连杆构成机械臂，'standard’表示使用的是标准DH建模方法。在上述代码中，IRB4600是机械臂的命名；IRB4600.display()是将机器人的标准DH参数表打印到控制台上。IRB4600.teach则是在界面上展示IRB4600机器人模型，在Tech界面，机器人模型可以根据输入的关节角变化而实时变化。L2.offset=-pi/2和L5.offset=-pi表示关节2和关节5处，机器人的关节角分别有-90度和-180度的偏移。整体由机器人工具箱建立的模型如图所示：
在这里插入图片描述

图2. IRB4600机器人可视化界面

公式推导代码：

clc;
clear;
% 正解输 入关节角
q_input = [10,20,30,40,50,60];
%% 公式推导正解
% 连杆参数(没有的默认为零）
a1 = 175;
a2 = 1095;
a3 = 175;
d1 = 495;
d4 = 1230.5;
d6 = 85;
Q1 = q_input(1)*pi/180;
Q2 = (q_input(2)-90)*pi/180;
Q3 = q_input(3)*pi/180;
Q4 = q_input(4)*pi/180;
Q5 = (q_input(5)- 180)*pi/180;
Q6 = q_input(6)*pi/180;% DH模型
L1=[cos(Q1),0,-sin(Q1),a1*cos(Q1);sin(Q1),0,cos(Q1),a1*sin(Q1);0,-1,0,d1;0,0,0,1];
L2=[cos(Q2),-sin(Q2),0,a2*cos(Q2);sin(Q2),cos(Q2),0,a2*sin(Q2);0,0,1,0;0,0,0,1];
L3=[cos(Q3),0,-sin(Q3),a3*cos(Q3);sin(Q3),0,cos(Q3),a3*sin(Q3);0,-1,0,0;0,0,0,1];
L4=[cos(Q4),0,sin(Q4),0;sin(Q4),0,-cos(Q4),0;0,1,0,d4;0,0,0,1];
L5=[cos(Q5),0,sin(Q5),0;sin(Q5),0,-cos(Q5),0;0,1,0,0;0,0,0,1];
L6=[cos(Q6),-sin(Q6),0,0;sin(Q6),cos(Q6),0,0;0,0,1,d6;0,0,0,1];
% 求解末端位置与矩阵
T = L1*L2*L3*L4*L5*L6;
% 打印结果
disp("正解结果：")
disp(T);