机器人运动学及轨迹规划— (2)DH建模与正运动学方程

Horo321

已于 2025-01-11 20:28:06 修改

阅读量4.9k

点赞数 18

文章标签：机器人 matlab

于 2024-06-30 22:03:35 首次发布

本文链接：https://blog.csdn.net/mao3332606/article/details/140077341

版权

1、DH参数法建模

可以将机器人各轴看成由一系列关节连接起来的连杆组成，将坐标系固连在每一个连杆关节上，则可以通过齐次变换来描述这些坐标系之间的相对位置和方向。这种通过齐次变换建立机构的运动学模型被称为D-H参数法，在DH参数模型下机器人各连杆的关系可以用a、alpha、d、theta 四个参数表示，其中theta为关节角度，d为相邻关节距离，a为连杆长度，alpha 为连杆扭角。

目前DH参数法一般分为标准SDH（standard DH）与改进DH（modified DH），标准DH将固连坐标系定义在连杆的末端，而改进DH则是将坐标系定义在连杆的前端。MDH适用范围更广，更容易理解和推导，本文采用MDH对ABB的IRB2600-1.65型号机器人进行建模，由于后三轴坐标系建立在腕部，到法兰的距离在matlab机器人工具箱中难以体现，因此对MDH参数建模的验证在ABB的离线编程仿真软件RobotStudio中进行。

（1）SDH

SDH的变换顺序为θ、d、a、α

因此转换矩阵为：

（2）MDH

MDH的变换顺序为α、a、θ、d：

则转换矩阵为：

对于IRB2600型号机器人，首先确定各关节轴 J1-J6：

此时Z轴方向已知确定X和Y轴方向：

X轴方向为由(Z)关节轴线i指向i+1。

Y轴方向由右手法则确定（右手握拳，螺旋方向与Z轴转向X轴方向一致，拇指指向为Y轴正方向）。

求得的DH参数为：

2、建立正运动学方程

正运动学是给定机器人关节空间下的各关节角度，来求解机器人末端位姿，即将关节空间的关节变量转换为笛卡尔空间的位置和姿态。

将MDH参数代入转换矩阵 $_{i}^{i-1}\textrm{T}$ 得：

将矩阵输入matlab计算给定关节角度下的位姿：

%改进DH参数建模  正运动学
theta=[60*pi/180,-25*pi/180,40*pi/180,45*pi/180,-20*pi/180,60*pi/180];%给出关节角度
theta1 = theta(1);
theta2 = theta(2);
theta3 = theta(3);
theta4 = theta(4);
theta5 = theta(5);
theta6 = theta(6);
%    theta          d         a         alpha
MDH=[theta1         0.445     0              0;
     theta2-pi/2    0         0.150      -pi/2;
     theta3         0         0.700          0;
     theta4         0.795     0.115      -pi/2;
     theta5         0         0           pi/2;
     theta6         0         0         -pi/2];
 
 T01=[cos(MDH(1,1))         -sin(MDH(1,1))           0                    0;
      sin(MDH(1,1))         cos(MDH(1,1))            0                    0;
      0                     0                        1             MDH(1,2);
      0                     0                        0                   1];
 T12=[cos(MDH(2,1))         -sin(MDH(2,1))           0             MDH(2,3);
      0                     0                        1                    0;
      -sin(MDH(2,1))        -cos(MDH(2,1))           0                    0;
      0                     0                        0                   1];
 T23=[cos(MDH(3,1))         -sin(MDH(3,1))           0             MDH(3,3);
      sin(MDH(3,1))         cos(MDH(3,1))            0                    0;
      0                     0                        1                    0;
      0                     0                        0                   1];
 T34=[cos(MDH(4,1))         -sin(MDH(4,1))           0             MDH(4,3);
      0                     0                        1             MDH(4,2);
      -sin(MDH(4,1))        -cos(MDH(4,1))           0                    0
      0                     0                        0                   1];
 T45=[cos(MDH(5,1))         -sin(MDH(5,1))           0                    0;
      0                     0                        -1                   0;
      sin(MDH(5,1))         cos(MDH(5,1))            0                    0;
      0                     0                        0                   1];
 T56=[cos(MDH(6,1))         -sin(MDH(6,1))           0                    0;
      0                     0                        1                    0;
      -sin(MDH(6,1))        -cos(MDH(6,1))           0                    0;
      0                     0                        0                   1];
% 腕部到法兰
 T6TCP=[-1                  0                        0                    0;
        0                   -1                       0                    0;
        0                   0                        1                0.085;
        0                   0                        0                   1];
    
  T06=T01*T12*T23*T34*T45*T56*T6TCP;

输入运行以上代码，然后命令行窗口输出注释行即可得到以下结果：

由T06可以得到机器人末端的位置为（385，625.7，983.9），而姿态为以上旋转矩阵。在RobotStudio6.08中添加IRB2600型号机器人，各关节设置为（60°，-25°，40°，45°，-20°，60°），示教目标点后在机器人控制器中查看机器人当前位姿：

控制器中机器人姿态采用欧拉角表示，而我们得到的结果为旋转矩阵，因此需要进行转换，可以自己写旋转矩阵转欧拉角公式：

R=[ -0.6992     0.1693    0.6946
     0.6781    -0.1505    0.7194    
     0.2263     0.9740   -0.0096 ];

r11=R(1,1);r12=R(1,2);r13=R(1,3);
r21=R(2,1);r22=R(2,2);r23=R(2,3);
r31=R(3,1);r32=R(3,2);r33=R(3,3);

Euler= [atan2(r21, r11); atan2(-r31, sqrt(r32 ^ 2 + r33 ^ 2)); atan2(r32, r33); ]*57.3

也可以直接用matlab的转换函数rotm2eul(R)：

R=[-0.6992    0.1693    0.6946    
    0.6781   -0.1505    0.7194    
    0.2263    0.9740   -0.0096];
Euler=rotm2eul(R)*57.3

得到结果为：

可以看到在相同的关节角度下，求得的机器人末端位姿与官方数据一致，证明建立的MDH参数与正运动学方程准确无误。