基于MATLAB机器人工具箱的KUKA youBot机械臂运动学建模——DH法
文章开头有机器人工具箱链接，有需要的同学可以自行下载。CSDN下载积分不能设置为0，抱歉~ 没有积分的同学可以去官网下载搞定。
以实验室的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)
相关链接：
- http://blog.csdn.net/pengjc2001/article/details/70156333 PUMA560机器人的DH建立过程
- http://blog.sina.com.cn/s/blog_a16714bf0102vae4.html MATLAB计算机器人运动学正解
- http://www.tekkotsu.org/Kinematics.html DH参数解释
https://ww2.mathworks.cn/academia/courseware/modelling-design-control-robotic-mechanisms.html
二自由度，三自由度和并联机械臂的运动学和动力学分析及m文件