机器人学DH建模PUMA560机械臂运动学正解Matlab程序

前言

第一次写博客记录学习有关机械臂内容，最近学习林沛群教授的《机器人学》，和看了《机器人学导论》中有关机械臂运动学正解的内容，并通过Matlab程序实现书中的例子，在此记录学习内容和学习过程中遇到的问题。

正文

1.DH参数

DH参数分为分为标准DH参数和改进DH参数两种

坐标系建立步骤：

1，找出各类关节轴，标出（或画出）这些轴线的延长线。在下面的步骤2至步骤5中，仅考虑前后两个相邻的轴线（关节轴和关节轴）

2,找出关节轴和关节轴之间的公垂线或关节轴和关节轴的交点，以关节轴和的交点或公垂线与关节轴的交点作为连杆坐标系i的原点。

3，规定轴沿着关节轴的方向

4，规定轴沿着公垂线方向，如果关节轴和关节轴相交，则规定轴垂直于关节轴和关节轴所在平面。

5，按照右手定则确定轴

6，当第一个关节变量为0（=0）时，规定坐标系{0}和{1}重合。对于坐标系{}（最后一个坐标系），其原点和的方向可以任意选取，选取时尽量使连杆参数为0.

（1）标准DH参数（STD-DH）

1,标准DH参数中，轴方向由当前z轴和前一个坐标系中的z轴确定(叉乘)。遵循右手定则，即由指向，大拇指的方向即为x轴方向。或以和的公垂线作为x轴。

2，建模步骤

（1）将绕轴旋转,使得和共线

（2）将沿着轴平移,使得和重合

（3）将绕轴旋转,使得和共线

（4）将沿着轴方向平移，使得和重合

3,STD-DH四个参数变化顺序:

4,每个步骤得到的变化矩阵（齐次矩阵）

两个相邻坐标系间变换矩阵

得到：

（2）改进DH参数（MOD-DH）

1,改进DH参数中，轴方向由当前z轴和后一个坐标系中的z轴确定(叉乘)。遵循右手定则，即由指向，大拇指的方向即为轴方向。或以和的公垂线作为x轴。

2，建模步骤

（1）将绕轴旋转,使得和共线

（2）将沿着轴方向平移，使得和重合

（3）将绕轴旋转,使得和共线

（4）将沿着轴平移,使得和重合

3,MOD-DH四个参数变化顺序:

两个相邻坐标系间变换矩阵

得到：

2.以改进型DH参数建立机械臂运动学模型

机器人学中求PUMA560正解过程中使用的DH参数是改进型DH参数，因此在这里我也使用改进型DH参数来建立机械臂运动学模型

1.根据机械臂关节轴建立机械臂连杆坐标系如下：

其中{0}坐标系和{1}坐标系重合，并且坐标系{4}，{5}，{6}相交于一点，并且交点与三个坐标系原点重合，关节轴4，5，6互相垂直。

2，机械臂MOD-DH参数

1，以坐标系{0}和{1}为例，如图所示：

由于坐标系{0}和{1}重合，因此，，为0，因为坐标系0是可以旋转的，所以为可变参数，因此可以得到第一组DH参数为：=0，=0，=0，

2.求坐标系{1}和{2}DH参数

将坐标系{2}用红色标出如图所示：

将绕轴顺时针旋转90°使得和共线。因为和相交于同个原点，所以可得,=0。如下图所示：

以前面建立的z轴为基础，将沿着旋转，使得与共线，再平移使得和重合。由于为可变参数，并且和重合，因此得到第二组DH参数为：，=0，=0，

3.求坐标轴{2}和{3}DH参数

如图所示：和平行，和相距，和相距,因此可得到DH参数为：=0，，，

同理可以求得{4}，{5}，{6}坐标轴各自对应的DH参数，最终可得到以下DH参数表

PUMA560机械臂DH参数表

1	0	0	0
2		0	0
3	0
4
5		0	0
6		0	0

3.根据DH参数表，求运动学方程

根据前面所求得改进型DH参数的变换矩阵

结合DH参数表，可求得相邻坐标系的变换矩阵如下：

将各个连杆矩阵连乘就可以得到基坐标系{0}和末端坐标系{6}的变换矩阵

Matlab程序：

format long
syms d1 d2 d3 d4 d5 d6 
syms theta1 theta2 theta3 theta4 theta5 theta6 
syms theta_i theta_i_1 alpha_i_1 alpha_i d_i a_i_1
syms a1 a2 a3 a4 a5 a6
syms alpha1 alpha2 alpha3 alpha4 alpha5 alpha6
syms r11 r12 r13 r21 r22 r23 r31 r32 r33 Px Py Pz
syms T_i
%机器人学PUMA560机械臂参数
d1 = 0;
d2 = 0;
d3 = 0.149;
d4 = 0.433;
d5 = 0;
d6 = 0;
%连杆长度
a0 = 0;
a1 = 0;
a2 = 0.432;
a3 = 0.02;
a4 = 0;
a5 = 0;
%连杆扭角
alpha0 = 0;
alpha1 = -pi/2;
alpha2 = 0;
alpha3 = -pi/2;
alpha4 = pi/2;
alpha5 = -pi/2;
%----------------------------%
%对θ进行赋值，注意赋值一定要在运算之前
%theta1 = pi/3;
%theta2 = pi/4;
%theta3 = pi/5;
%theta4 = pi/3;
%theta5 = pi/4;
%theta6 = pi/5;
T_i=[cos(theta_i),-sin(theta_i),0,a_i_1;...
    sin(theta_i)*cos(alpha_i_1),cos(theta_i)*cos(alpha_i_1),-sin(alpha_i_1),-sin(alpha_i_1)*d_i;...
    sin(theta_i)*sin(alpha_i_1),cos(theta_i)*sin(alpha_i_1),cos(alpha_i_1),cos(alpha_i_1)*d_i;
    0,0,0,1;
    ];
T0_1=[cos(theta1),-sin(theta1),0,0;
      sin(theta1),cos(theta1),0,0;
      0,0,1,0;
      0,0,0,1];
T1_2=[cos(theta2),-sin(theta2),0,0;
      0,0,1,0;
      -sin(theta2),-cos(theta2),0,0;
      0,0,0,1;];
T2_3=[cos(theta3),-sin(theta3),0,a2;
      sin(theta3),cos(theta3),0,0;
      0,0,1,d3;
      0,0,0,1];
T3_4=[cos(theta4),-sin(theta4),0,a3;
     0,0,1,d4;
     -sin(theta4),-cos(theta4),0,0;
     0,0,0,1];
T4_5=[cos(theta5),-sin(theta5),0,0;
      0,0,-1,0;
      sin(theta5),cos(theta5),0,0;
      0,0,0,1];
T5_6=[cos(theta6),-sin(theta6),0,0;
      0,0,1,0;
      -sin(theta6),-cos(theta6),0,0;
      0,0,0,1];
T0_6=T0_1*T1_2*T2_3*T3_4*T4_5*T5_6 %解得运动学正解

4.使用Matlab机器人工具箱验证是否正确

Matlab机器人工具箱代码：

format long
syms d1 d2 d3 d4 d5 d6 
syms theta1 theta2 theta3 theta4 theta5 theta6 
syms theta_i theta_i_1 alpha_i_1 alpha_i d_i a_i_1
syms a1 a2 a3 a4 a5 a6
syms alpha1 alpha2 alpha3 alpha4 alpha5 alpha6
syms r11 r12 r13 r21 r22 r23 r31 r32 r33 Px Py Pz
syms T_i
%机器人学PUMA560机械臂参数
d1 = 0;
d2 = 0;
d3 = 0.149;
d4 = 0.433;
d5 = 0;
d6 = 0;
%连杆长度
a0 = 0;
a1 = 0;
a2 = 0.432;
a3 = 0.02;
a4 = 0;
a5 = 0;
%连杆扭角
alpha0 = 0;
alpha1 = -pi/2;
alpha2 = 0;
alpha3 = -pi/2;
alpha4 = pi/2;
alpha5 = -pi/2;
L1=Link([0     d1       a0       alpha0     ],'modified');%设置连杆参数，注：使用的是改进型DH参数，所以最后参数为'modified'
L2=Link([0     d2       a1       alpha1     ],'modified');
L3=Link([0     d3       a2        alpha2     ],'modified');
L4=Link([0     d4       a3       alpha3     ],'modified');
L5=Link([0     d5       a4       alpha4     ],'modified');
L6=Link([0     d6       a5       alpha5     ],'modified');
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','RobotArm');%将不同连杆链接起来
robot.plot([pi/3,pi/4,pi/5,pi/3,pi/4,pi/5]);%输出机器人模型，后面的六个角为输出时的theta姿态,用弧度制
robot.display();%打印出机器人D-H参数表
T=robot.fkine([pi/3,pi/4,pi/5,pi/3,pi/4,pi/5])%求机械臂正解
robot.teach%机械臂示教
%使用改进型DH法，根据机器人学中对PUMA560机械臂建模方法实现运动学正解

可以通过代入不同的关节角度，使用机械臂工具箱和自己写的求正解的程序，对比验证是否有误。

参考资料

【Matlab 六自由度机器人】定义标准型及改进型D-H参数建立机器人模型（附MATLAB建模代码）_知道机器人标准的d-h参数如何利用matlab进行六轴机器人仿真模型建立-CSDN博客

《机器人学导论》

台大机器人学之运动学——林沛群（含课件+书籍）_哔哩哔哩_bilibili

机器人工程师进阶之路（二）6轴机械臂D-H法建模 - 知乎 (zhihu.com)（有点小错误，注意看评论区）