机器人学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}重合。对于坐标系{

N

}(最后一个坐标系),其原点和

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参数的变换矩阵

^{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

结合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)(有点小错误,注意看评论区)



声明

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