机器人正运动学实例——PUMA560机械臂（附Matlab机器人工具箱建模代码）
前言
前面文章讲到了机器人正运动学的基本方法，这篇文章就是以实际案例来记录如何进行运动学正解、运动空间分析等（包括利用Matlab机器人工具箱来实现），本文案例是PUMA560六自由度机械臂。
一、方法回顾
①进行正运动学的基础是分析机器人的机构原理，第一步是建立连杆坐标系（使用右手法则，并尽可能让更多参数为0，简化运动方程）；
②描述连杆与关节参数，进行连杆变换建立D-H参数表（是改进的D-H法）；
③建立正运动学方程
至此，列写正运动学方程即完成了正运动学的描述，也就是通过不同已知的关节变量最终得到的是机器人末端相对于基坐标系的运动。
二、PUMA560机械臂正运动学分析
1. 绘制机构原理图并建立坐标系
PUMA560机械臂是6R的机械臂，也就是只有六个转动，最后的三转自由度集中在了末端，三轴相交。可以对其构建机构原理图如下：
坐标系的Z轴指向旋转关节轴向，坐标系建立符合右手法则。
连杆{1}系为了方便描述与{0}系重合，因此{1}系相对于{0}系就只有绕Z轴的转动；其他坐标系建立如上图所示。
2. 建立D-H参数表
建立了坐标系，再根据已知的长度等参数，可以根据上面提到的描述连杆参数的方法，列写D-H参数表如下：
注：括号中代表了上图位姿下，该变量的数值（注意正负号）。
开始我不太明白为何运动学分析前一定要列写这个参数表，在写齐次变换矩阵时候，我一边写一边分析不就好了吗？
这里引用哈工大一个师兄的知乎回答，大概意思就是D-H参数作用主要不是为了下面齐次变换矩阵，而是建立关节物理模型和空间位置姿态之间的关系。（可能有些抽象，感兴趣可以深入学一学）
个人认为D-H参数表可以很清楚的说明连杆重要的四类参数（而欧式变换需要至少6个参数），对于建立正运动学模型有重要意义。
3. 正运动学方程推导+Matlab求解
① 推导的理论基础
这里我们需要建立{6}系对于{0}系的变换矩阵，也就是进行下面的推导：
同时，连杆变换中讲到了通用的子变换情况，齐次变换矩阵为：
注意：这里的矩阵变换通式是用的改进型的D-H法，因此下面的mtlab都是用的改进型。
② 具体过程
让我们来列写各个子变换，结合上面通式与D-H表，也可以很容易列写
~
：
当然我写的下面matlab程序可以由D-H参数表与变换通式，直接输出各个子变换的值~
对上述矩阵相乘可以用Matlab运算（比较快）：
clc; clear; %以下为正运动学求解过程 %声明变量 syms theata1 theata2 theata3 theata4 theata5 theata6 a2 a3 d2 d4; % %代入参数值 % theata1=0; theata2=0; theata3=-pi/2; theata4=0; theata5=0; theata6=0; % a2=400; a3=10; d2=120; d4=400; %D-H参数表 DH=[0 0 0 theata1; -pi/2 0 d2 theata2; 0 a2 0 theata3; -pi/2 a3 d4 theata4; pi/2 0 0 theata5; -pi/2 0 0 theata6]; t=1; %齐次变换 for i=1:6 T0 = [cos(DH(i,4)) -sin(DH(i,4)) 0 DH(i,2); sin(DH(i,4))*cos(DH(i,1)) cos(DH(i,4))*cos(DH(i,1)) -sin(DH(i,1)) -DH(i,3)*sin(DH(i,1)); sin(DH(i,4))*sin(DH(i,1)) cos(DH(i,4))*sin(DH(i,1)) cos(DH(i,1)) DH(i,3)*cos(DH(i,1)); 0 0 0 1 ] t = t*T0 end 上述代码实际上就是利用了一个for循环，来计算
~
矩阵并输出到
中；之后为了计算矩阵相乘，就引入了T，每计算一个子变换就乘进去一个，最终得到的T结果就是
。
如果已知了theata1~theata6的关节变量值，可以在DH参数表下面写明对应数值，这样就可以计算出对应的齐次变换矩阵真值了~
比如，上面代码代入参数值取消注释后运行代码，可以输出这个参数值下的齐次变换矩阵，可以看到末端相对基座标的位置信息：
% theata1=0; theata2=0; theata3=-pi/2; theata4=0; theata5=0; theata6=0; % a2=400; a3=10; d2=120; d4=400;
也就是代表了机械臂末端在基坐标系下的位置是：x=800，y=120，z=10。
三、Matlab正运动仿真——机器人工具箱
接下来是利用matlab里的工具箱进行机械臂的建模，这一步骤就是可以很便捷的对各个关节进行参数设置，就能够直观看出末端的位置角度信息。
下面是整体的建模代码：
clc; clear; %定义连杆的D-H参数 %连杆偏移 d1 = 0; d2 = 120; d3 = 0; d4 = 400; d5 = 0; d6 = 0; %连杆长度 a1 = 0; a2 = 0; a3 = 400; a4 = 10; a5 = 0; a6 = 0; %连杆扭角 alpha1 = 0; alpha2 = -pi/2; alpha3 = 0; alpha4 = -pi/2; alpha5 = pi/2; alpha6 = -pi/2; %建立机器人模型 % theta d a alpha L1=Link([0 d1 a1 alpha1 ],'modified'); L2=Link([0 d2 a2 alpha2 ],'modified'); L3=Link([0 d3 a3 alpha3 ],'modified'); L4=Link([0 d4 a4 alpha4 ],'modified'); L5=Link([0 d5 a5 alpha5 ],'modified'); L6=Link([0 d6 a6 alpha6 ],'modified'); %限制机器人的关节空间 L1.qlim = [(-165/180)*pi,(165/180)*pi]; L2.qlim = [(-150/180)*pi, (60/180)*pi]; L3.qlim = [(-150/180)*pi, (90/180)*pi]; L4.qlim = [(-180/180)*pi,(180/180)*pi]; L5.qlim = [(-115/180)*pi,(115/180)*pi]; L6.qlim = [(-360/180)*pi,(360/180)*pi]; %连接连杆，机器人取名为myrobot robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','6Rrobot'); robot.base = transl(0 ,0 ,660.4); %基坐标系进行平移 robot.display(); %打印出机器人D-H参数表 robot.teach(); %展示机器人模型 %调整坐标轴长度 ax = gca; ax.XLim = [-1000, 1000]; ax.YLim = [-1000, 1000]; ax.ZLim = [-1000, 1500]; hold on; 之后可以输出下面的结果，图中我修改了转轴3的角度：
可以和上面旋转矩阵直接求解的结果（x=800，y=120，z=10）对比，发现机器人的z位置为什么是670.4呢？这是因为建系时为了方便列写后续参数，没有将底座的高度考虑进来。我在上面代码中写了这样一句：
robot.base = transl(0 ,0 ,660.4); %基坐标系进行平移就是将基坐标系平移，这句命令将基坐标系向Z方向平移了660.4个单位，也就是PUMA560机器人图中的基座高度，因此这两个解出来实际是一样的。
四、PUMA560工作空间可视化
上面讲到了如何进行这个机器人的建模和正运动学解算，下面我们来具体求一下这个机器人的工作空间到底是多大呢，而工作空间算出来就可以知道，我们设计的机器人能不能满足工作环境的需要了~
只需要在上面正解的代码后面，加下面的代码即可：
% 计算工作空间 num = 10000; % 采样点数，点数越多越精确但计算时间越长 q = zeros(num, 3); % 提前声明这样的位置矩阵 % 计算每个关节构型对应的末端执行器位置 for i = 1:num q1=L1.qlim(1)+ rand * (L1.qlim(2)-L1.qlim(1)) ; q2=L2.qlim(1)+ rand * (L1.qlim(2)-L1.qlim(1)) ; q3=L3.qlim(1)+ rand * (L1.qlim(2)-L1.qlim(1)) ; q4=L4.qlim(1)+ rand * (L1.qlim(2)-L1.qlim(1)) ; q5=L5.qlim(1)+ rand * (L1.qlim(2)-L1.qlim(1)) ; q6=L6.qlim(1)+ rand * (L1.qlim(2)-L1.qlim(1)) ; q= [q1 q2 q3 q4 q5 q6]; T = robot.fkine(q); p(i,:) = transl(T); end % 可视化工作空间 scatter3(p(:,1), p(:,2), p(:,3), 10, 'b', '.');这是最终的工作空间结果图：
总结
本文主要针对PUMA560这种六自由度机械臂，进行了正运动学分析，其中为了方便计算旋转矩阵利用到了matlab工具；之后利用matlab机器人工具箱，进行了机械臂的建模，这一结果也可以用于正运动学的解算；最后对其工作空间进行了可视化分析~ 以上过程的基础是机构自由度、连杆参数、齐次变换等，在我专栏中都有介绍，如有需要可以查阅~
本人也在不断学习过程中，如有不足之处，还望批评指正~
