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

楠楠楠难 2024-07-17 10:35:09 阅读 60

前言

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

正文

1.DH参数

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

坐标系建立步骤：

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

$i$

和关节轴

$z_{i-1}$

）

2,找出关节轴

$i$

和关节轴

i+1

之间的公垂线或关节轴

$i$

和关节轴

i+1

的交点，以关节轴

$i$

和

i+1

的交点或公垂线与关节轴

$i$

的交点作为连杆坐标系i的原点。

3，规定

z_i

轴沿着关节轴

$i$

的方向

4，规定

x_i

轴沿着公垂线方向，如果关节轴

$i$

和关节轴

i+1

相交，则规定

x_i

轴垂直于关节轴

$i$

和关节轴

i+1

所在平面。

5，按照右手定则确定

$y_i$

轴

6，当第一个关节变量为0（

$i$

=0）时，规定坐标系{0}和{1}重合。对于坐标系{

}（最后一个坐标系），其原点和

$x_N$

的方向可以任意选取，选取时尽量使连杆参数为0.

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

1,标准DH参数中，

$x$

轴方向由当前z轴

z_i

和前一个坐标系中的z轴

$z_{i-1}$

确定(叉乘)。遵循右手定则，即由

$z_{i-1}$

指向

z_i

，大拇指的方向即为x轴方向。或以

z_i

和

$z_{i-1}$

的公垂线作为x轴。

2，建模步骤

（1）将

$x_{i-1}$

绕

$z_{i-1}$

轴旋转

$\theta_i$

,使得

$x_{i-1}$

和

$x_{i}$

共线

（2）将

$x_{i-1}$

沿着

$z_{i-1}$

轴平移

$d_i$

,使得

$x_{i-1}$

和

$x_{i}$

重合

（3）将

$z_{i-1}$

绕

$x_{i-1}$

轴旋转

$\alpha _{i-1}$

,使得

$z_{i-1}$

和

z_i

共线

（4）将

$z_{i-1}$

沿着

$x_{i-1}$

轴方向平移

$a_{i-1}$

，使得

$z_{i-1}$

和

z_i

重合

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

$\theta_{i} \rightarrow d_{i}\rightarrow \alpha_{i-1} \rightarrow a_{i-1}$

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

$Rot(z_{i-1},\theta _i)=\begin{bmatrix} cos(\theta _i) & -sin(\theta _i)&0&0 \\ sin(\theta _i)& cos(\theta _i)&0&0\\0&0&1&0\\0&0&0&1 \end{bmatrix} \quad$

$Trans(z_{i-1},d _i)=\begin{bmatrix} 1 & 0&0&0 \\0&1&0&0\\0&0&1&d_i\\0&0&0&1 \end{bmatrix} \quad$

$Rot(x_{i-1},\alpha _{i-1})=\begin{bmatrix} 1 & 0&0&0 \\0&cos(\alpha_{i-1})&-sin(\alpha_{i-1})&0\\0&sin(\alpha_{i-1})&cos(\alpha_{i-1})&0\\0&0&0&1 \end{bmatrix} \quad$

$Trans(x_{i-1},a _{i-1})=\begin{bmatrix} 1 & 0&0&a_{i-1}\\0&1&0&0\\0&0&1&0\\0&0&0&1 \end{bmatrix} \quad$

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

$^{i-1}_iT=Rot(z_{i-1},\theta_i)Trans(z_{i-1},d_i)Rot(x_{i-1},\alpha_{i-1})Trans(x_{i-1},a_{i-1}))$

得到：

$^{i-1}_iT=\begin{bmatrix} cos(\theta_i)& -sin(\theta_i)cos(\alpha_{i-1})&sin(\theta_i)sin(\alpha_{i-1})&a_{i-1}cos(\theta_i)\\sin(\theta_i)&cos(\theta_i)cos(\alpha_{i-1})&-cos(\theta_i)sin(\alpha_{i-1})&a_{i-1}sin(\theta_i)\\0&sin(\alpha_{i-1})&cos(\alpha_{i-1})&d_i\\0&0&0&1 \end{bmatrix} \quad$

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

1,改进DH参数中，

$x$

轴方向由当前z轴

$z_{i-1}$

和后一个坐标系中的z轴

$z_{i}$

确定(叉乘)。遵循右手定则，即由

$z_{i-1}$

指向

$z_{i}$

，大拇指的方向即为

$x_{i-1}$

轴方向。或以

$z_{i-1}$

和

$z_{i}$

的公垂线作为x轴。

2，建模步骤

（1）将

$z_{i-1}$

绕

$x_{i-1}$

轴旋转

$\alpha _{i-1}$

,使得

$z_{i-1}$

和

z_i

共线

（2）将

$z_{i-1}$

沿着

$x_{i-1}$

轴方向平移

$a_{i-1}$

，使得

$z_{i-1}$

和

z_i

重合

（3）将

$x_{i-1}$

绕

$z_{i-1}$

轴旋转

$\theta_i$

,使得

$x_{i-1}$

和

$x_{i}$

共线

（4）将

$x_{i-1}$

沿着

$z_{i-1}$

轴平移

$d_i$

,使得

$x_{i-1}$

和

$x_{i}$

重合

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

$\alpha_{i-1} \rightarrow a_{i-1}\rightarrow \theta_{i} \rightarrow d_{i}$

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

$^{i-1}_iT=Rot(x_{i-1},\alpha_{i-1})Trans(x_{i-1},a_{i-1}))Rot(z_{i-1},\theta_i)Trans(z_{i-1},d_i)$

得到：

$^{i-1}_iT=\begin{bmatrix} 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 \end{bmatrix} \quad$

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

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

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

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

2，机械臂MOD-DH参数

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

由于坐标系{0}和{1}重合，因此

$\alpha_0$

，

$a_0$

，

d_1

为0，因为坐标系0是可以旋转的，所以

$\theta_1$

为可变参数，因此可以得到第一组DH参数为：

$\alpha_0$

=0，

$a_0$

=0，

d_1

=0，

$\theta_1$

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

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

将

$z_1$

绕

$x_1$

轴顺时针旋转90°使得

$z_1$

和

$z_2$

共线。因为

$z_1$

和

$z_2$

相交于同个原点，所以可得

$\alpha_1=-\pi /2$

$a_1$

=0。如下图所示：

以前面建立的z轴为基础，将

$x_1$

沿着

$z_1$

旋转

$\theta_2$

，使得

$x_1$

与

$x_2$

共线，再平移

d_2

使得

$x_1$

和

$x_2$

重合。由于

$\theta_2$

为可变参数，并且

$x_1$

和

$x_2$

重合，因此得到第二组DH参数为：

$\alpha_1=-\pi /2$

，

$a_1$

=0，

d_2

=0，

$\theta_2$

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

如图所示：

$z_2$

和

$z_3$

平行，

$z_2$

和

$z_3$

相距

$a_2$

，

$x_2$

和

$x_3$

相距

$d_3$

,因此可得到DH参数为：

$\alpha_2$

=0，

$a_2$

，

$d_3$

，

$\theta_3$

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

PUMA560机械臂DH参数表

$i$	$\alpha_{i-1}$	$a_{i-1}$	$d_i$	$\theta_i$
1	0	0	0	$\theta_1$
2	$-\frac{\pi }{2}$	0	0	$\theta_2$
3	0	$\alpha_2$	$d_3$	$\theta_3$
4	$-\frac{\pi }{2}$	$\alpha_3$	$d_4$	$\theta_4$
5	$\frac{\pi }{2}$	0	0	$\theta_5$
6	$-\frac{\pi }{2}$	0	0	$\theta_6$

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

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

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

$^{0}_1T=\begin{bmatrix} cos(\theta_1)& -sin(\theta_1)&0&0\\sin(\theta_1)&cos(\theta_1)&0&0\\0&0&1&0\\0&0&0&1 \end{bmatrix} \quad$

$^{1}_2T=\begin{bmatrix} cos(\theta_2)& -sin(\theta_2)&0&0\\0&0&1&0\\-sin(\theta_2)&-cos(\theta_2)&0&0\\0&0&0&1 \end{bmatrix} \quad$

$^{2}_3T=\begin{bmatrix} cos(\theta_3)& -sin(\theta_3)&0&a_2\\sin(\theta_3)&cos(\theta_3)&0&0\\0&0&1&d_3\\0&0&0&1 \end{bmatrix} \quad$

$^{3}_4T=\begin{bmatrix} cos(\theta_4)& -sin(\theta_4)&0&a_3\\0&0&1&d_4\\-sin(\theta_4)&-cos(\theta_4)&0&0\\0&0&0&1 \end{bmatrix} \quad$

$^{4}_5T=\begin{bmatrix} cos(\theta_5)& -sin(\theta_5)&0&0\\0&0&-1&0\\sin(\theta_5)&cos(\theta_5)&0&0\\0&0&0&1 \end{bmatrix} \quad$

$^{5}_6T=\begin{bmatrix} cos(\theta_6)& -sin(\theta_6)&0&0\\0&0&1&0\\-sin(\theta_6)&-cos(\theta_6)&0&0\\0&0&0&1 \end{bmatrix} \quad$

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

$^0_6T$

$^0_6T=^0_1T^1_2T^2_3T^3_4T^4_5T^5_6T$

$^{0}_6T=\begin{bmatrix} r_{11}& r_{12}&r_{13}&P_x\\r_{21}&r_{22}&r_{23}&P_y\\r_{31}&r_{32}&r_{33}&P_z\\0&0&0&1 \end{bmatrix} \quad$

Matlab程序：

<code>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)（有点小错误，注意看评论区）

上一篇：深入理解Linux环境配置文件：.bashrc、.bash_profile和.profile

下一篇：【C++初阶】第九站：vector的介绍及使用

本文标签

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

声明

本文内容仅代表作者观点，或转载于其他网站，本站不以此文作为商业用途
如有涉及侵权，请联系本站进行删除
转载本站原创文章，请注明来源及作者。