基于MATLAB机器人工具箱的KUKA youBot机械臂运动学建模——DH法_matlab机械臂建模

作者：从前慢现在也慢 | 2024-02-16 21:12:05

踩

matlab机械臂建模

文章开头有机器人工具箱链接，有需要的同学可以自行下载，robot-9.8_2013_2_12.zip_机械臂DH法建模-数据集文档类资源-CSDN下载。官网：Robotics Toolbox - Peter Corke。

以实验室的KUKA youBot五自由度机械臂为切入点，记得当时和实验室的同学在这上面花费了好长时间，最后也没搞定，而这又算是基础中的基础，不能忽视。

DH一般分为标准DH和改进的DH，以John J.Craig的《机器人学导论》来说，它的是Modified DH，另一本孙富春翻译的《机器人学导论——分析、控制及应用》则是用的标准DH。通过Peter Corke的那本《Robotics，Vision and Control》（已有中文版）我才知道有这两个。区别很明显DH参数表的下标如果都是一样的则是标准DH，如果有一半不一样则是Modified DH。

这样的和

机械臂的构型可以看做是连杆构成的，两个连杆（Link）之间才有一个关节（Joint），而常常将大地和底座也当做连杆，因此有个最直接的问题的就是：关节坐标系到底是建在前一个连杆上还是后一个连杆上？我想这也就是为什么会有标准DH和改进的DH的原因了。

这条臂比较特别的地方在于关节0和关节1之间有个偏置，特别是在于改进DH中的坐标系0和坐标系1的设置很特殊。

1. 标准DH

标准DH的介绍我是参考的孙富春老师翻译的《机器人学导论——分析、控制及应用》，后来接触到《Robot Modeling and Control》（中文版名为，机器人建模和控制，机械工业出版社，贾振中译）也是标准DH。

一个刚体的自由度是6，而为什么标准DH只用四个参数呢？这里其实暗含了一个建立坐标系的原则：垂直和相交，刚好六个限制条件。这在《Robot Modeling and Control》这本书中有详细介绍。垂直相交的意思是说，建立坐标系的时候，后一个坐标系的X要与前一个坐标系的Z垂直且相交。如建立X1的时候需要与Z0垂直相交。

在孙老师的那本书的P62页，关于DH参数的解释如下图：

我举个例子，例如：

我记的时候就是前一个Z，和后一个的X。如右边括号里的，而这里正是与后文的改进的DH不一样的地方。

KUKA youBot机械臂的DH


%kuka youBot 
clear;
clc;
close all;
d1=0.147;
a1=0.033;
a2=0.155;
a3=0.135;
d5=0.113;
tool = 0.105;
 
% Joint angle limit.
qlim1=[-169,169]*pi/180;
qlim2=[-65,90]*pi/180;
qlim3=[-151,146]*pi/180;
qlim4=[-102.5,102.5]*pi/180;
qlim5=[-167.5,167.5]*pi/180;
 
 L(1) = Link('d', d1, 'a', a1, 'alpha', pi/2, 'qlim',qlim1);
 L(2) = Link('d', 0, 'a', a2, 'alpha', 0, 'qlim', qlim2, 'offset', 1.57);
%  L(2) = Link('d', 0, 'a', a2, 'alpha', 0, 'qlim', qlim2);
 L(3) = Link('d', 0, 'a', a3, 'alpha', 0,'qlim',qlim3);
 L(4) = Link('d', 0, 'a', 0, 'alpha', -pi/2,'qlim',qlim4, 'offset', -1.57);
%  L(4) = Link('d', 0, 'a', 0, 'alpha', -pi/2,'qlim',qlim4);
 L(5) = Link('d', d5, 'a', 0, 'alpha', 0,'qlim',qlim5);
 
 bot = SerialLink(L,'name','KUKA youBot');
 bot.display();
 
%  bot.plot([0 0 0 0 0]);
  bot.teach;
 
 
bot.tool= transl(0, 0, tool);% EEF
 
%% forward kinematics(default input is rad)
bot.fkine([0 0 0 0 0]*pi/180);
 
bot.fkine([0 0 -90 0 0]*pi/180)

这里发现的问题就是，我用铅笔在纸上用DH建立坐标系的时候的初始姿态(左图)与工具箱的初始姿态（右图）不一致，加了offset才相同。

编好以后可以输入一些角度验证一下

如上图中的这个姿态，第三关节转动90°，X为33 + （655 - 302）= 386mm，Z坐标为底座到第三关节长度为302mm与Figure2中显示的一致，说明DH建立没有问题。接下来求解正运动学矩阵。


% xiaobing http://blog.sina.com.cn/s/blog_a16714bf0102vae4.html
clear;
clc;
syms theta1 alpha1 a1 d1 theta2 alpha2 a2 d2 theta3 alpha3 a3 d3 ...
    theta4 alpha4 a4 d4 theta5 alpha5 a5 d5 theta6 alpha6 a6 d6;
A1=[cos(theta1),-sin(theta1)*cos(alpha1),sin(theta1)*sin(alpha1),a1*cos(theta1);...
    sin(theta1),cos(theta1)*cos(alpha1),-cos(theta1)*sin(alpha1),a1*sin(theta1);...
    0,sin(alpha1),cos(alpha1),d1;...
    0,0,0,1];
A2=[cos(theta2),-sin(theta2)*cos(alpha2),sin(theta2)*sin(alpha2),a2*cos(theta2);...
    sin(theta2),cos(theta2)*cos(alpha2),-cos(theta2)*sin(alpha2),a2*sin(theta2);...
    0,sin(alpha2),cos(alpha2),d2;...
    0,0,0,1];
A3=[cos(theta3),-sin(theta3)*cos(alpha3),sin(theta3)*sin(alpha3),a3*cos(theta3);...
    sin(theta3),cos(theta3)*cos(alpha3),-cos(theta3)*sin(alpha3),a3*sin(theta3);...
    0,sin(alpha3),cos(alpha3),d3;...
    0,0,0,1];
A4=[cos(theta4),-sin(theta4)*cos(alpha4),sin(theta4)*sin(alpha4),a4*cos(theta4);...
    sin(theta4),cos(theta4)*cos(alpha4),-cos(theta4)*sin(alpha4),a4*sin(theta4);...
    0,sin(alpha4),cos(alpha4),d4;...
    0,0,0,1];
A5=[cos(theta5),-sin(theta5)*cos(alpha5),sin(theta5)*sin(alpha5),a5*cos(theta5);...
    sin(theta5),cos(theta5)*cos(alpha5),-cos(theta5)*sin(alpha5),a5*sin(theta5);...
    0,sin(alpha5),cos(alpha5),d5;...
    0,0,0,1];
 
%+---+-----------+-----------+-----------+-----------+
%| j |     theta |         d |         a |     alpha |
%+---+-----------+-----------+-----------+-----------+
%|  1|         q1|      0.147|      0.033|      1.571|
%|  2|         q2|          0|      0.155|          0|
%|  3|         q3|          0|      0.135|          0|
%|  4|         q4|          0|          0|     -1.571|
%|  5|         q5|      0.113|          0|          0|
%+---+-----------+-----------+-----------+-----------+
% 将含有pi/2和0的项转为符号量sym，避免含有超大值的数据。（感谢文末的网友留言）
 
alpha1=sym(pi/2);
 
d2=sym(0);
alpha2=sym(0);
 
d3=sym(0);
alpha3=sym(0);
 
d4=sym(0);
a4=sym(0);
alpha4=sym(-pi/2);
 
a5=sym(0);
alpha5=sym(0);
 
d1=0.147;
a1=0.033;
a2=0.155;
a3=0.135;
d5=0.113;
 
T=simplify(eval(A1*A2*A3*A4*A5))
 
x=T(1,4);
y=T(2,4);
z=T(3,4);
FK =[x; y; z ];
 
theta1=0;
theta2=0;
theta3=-1.57;
theta4=0;
theta5=0;
 
x_math= (cos(theta1)*(135*cos(theta2 + theta3) - 113*sin(theta2 + theta3 + theta4) + 155*cos(theta2) + 33))/1000
y_math=  (sin(theta1)*(135*cos(theta2 + theta3) - 113*sin(theta2 + theta3 + theta4) + 155*cos(theta2) + 33))/1000
z_math= (113*cos(theta2 + theta3 + theta4))/1000 + (27*sin(theta2 + theta3))/200 + (31*sin(theta2))/200 + 147/1000
 
%     0.3011    0       0.0121 与上一个建模程序中的最后一行相同（注意切换成Link2和Link4没有offset的那两句）

正运动学矩阵为

T =|m11 m12 m13 m14|

|m21 m22 m23 m24|

|m31 m32 m33 m34|

|m41 m42 m43 m44|

= [ cos(theta2 + theta3 + theta4)*cos(theta1)*cos(theta5) - sin(theta1)*sin(theta5), - cos(theta5)*sin(theta1) - cos(theta2 + theta3 + theta4)*cos(theta1)*sin(theta5), -sin(theta2 + theta3 + theta4)*cos(theta1),   (cos(theta1)*(135*cos(theta2 + theta3) - 113*sin(theta2 + theta3 + theta4) + 155*cos(theta2) + 33))/1000]

[ cos(theta1)*sin(theta5) + cos(theta2 + theta3 + theta4)*cos(theta5)*sin(theta1),   cos(theta1)*cos(theta5) - cos(theta2 + theta3 + theta4)*sin(theta1)*sin(theta5), -sin(theta2 + theta3 + theta4)*sin(theta1),   (sin(theta1)*(135*cos(theta2 + theta3) - 113*sin(theta2 + theta3 + theta4) + 155*cos(theta2) + 33))/1000]

[                                       sin(theta2 + theta3 + theta4)*cos(theta5),                                        -sin(theta2 + theta3 + theta4)*sin(theta5),              cos(theta2 + theta3 + theta4), (113*cos(theta2 + theta3 + theta4))/1000 + (27*sin(theta2 + theta3))/200 + (31*sin(theta2))/200 + 147/1000]

[                                                                               0,                                                                                 0,                                          0,                                                                                                          1]

m11 = cos(theta2 + theta3 + theta4)*cos(theta1)*cos(theta5) - sin(theta1)*sin(theta5)

m12 = - cos(theta5)*sin(theta1) - cos(theta2 + theta3 + theta4)*cos(theta1)*sin(theta5)

m13= -sin(theta2 + theta3 + theta4)*cos(theta1)

m14 = (cos(theta1)*(135*cos(theta2 + theta3) - 113*sin(theta2 + theta3 + theta4) + 155*cos(theta2) + 33))/1000

m21= cos(theta1)*sin(theta5) + cos(theta2 + theta3 + theta4)*cos(theta5)*sin(theta1)

m22= cos(theta1)*cos(theta5) - cos(theta2 + theta3 + theta4)*sin(theta1)*sin(theta5)

m23 = -sin(theta2 + theta3 + theta4)*sin(theta1)

m24 = (sin(theta1)*(135*cos(theta2 + theta3) - 113*sin(theta2 + theta3 + theta4) + 155*cos(theta2) + 33))/1000

m31 = sin(theta2 + theta3 + theta4)*cos(theta5)

m32 = -sin(theta2 + theta3 + theta4)*sin(theta5)

m33 =  cos(theta2 + theta3 + theta4)

m34 = (113*cos(theta2 + theta3 + theta4))/1000 + (27*sin(theta2 + theta3))/200 + (31*sin(theta2))/200 + 147/1000

最后一行为齐次矩阵的0,0,0,1

2. Modified DH

改进的DH那就是如John J.Craig的《机器人学导论》上的，它与标准DH的区别在于：标准DH的Link0（大地）和Link1之间为J0。而Modified DH则是Link0上有J0，Link1上有J1，基座为坐标系{0}，一般让{0}和{1}重合。标准DH和改进DH的差别，通俗点就是说坐标系到底是建在前一个关节上还是后一个关节上。

Craig的书中文版P52页

P54页建立坐标系的步骤：

举个例子：

通常都是下标有一半是i -1，类似这样

这条臂比较特别的地方在于关节0和关节1之间有个偏置，特别是在于改进DH中的坐标系0和坐标系1的设置很特殊。

clc;
clear;
close all;
L(1) = Link([ 0  0.147  0  0 ],  'modified' );
L(2) = Link([ 0  0  0.033 pi/2  ],  'modified'); 
L(3) = Link([ 0 0 0.155  0  ],  'modified');
L(4) = Link([ 0  0 0.135 0],  'modified'); 
L(5) = Link([ 0  0.218 0 -pi/2],  'modified') ;


youbot = SerialLink(L, 'name', 'youbot');
youbot.display()
youbot.plot([0 0 0 0 0]);

youbot.teach;

% Forward kinematics matrix
syms theta1 theta2 theta3 theta4 theta5;
forward_kinematics=simplify(youbot.fkine([theta1 theta2 theta3 theta4 theta5]))

 % Set all q to zero, where the end-effector is.
 youbot.fkine([0 0 0 0 0]*pi/180)